Update interface to leverage eigen and simplify ladder graph data str…

…uctures (#46)

descartes_light/CMakeLists.txt

@@ -68,7 +68,6 @@ add_library(${PROJECT_NAME}
   src/ladder_graph.cpp
   src/ladder_graph_dag_search.cpp
 )
-target_process_export(${PROJECT_NAME})
 target_link_libraries(${PROJECT_NAME} PUBLIC console_bridge::console_bridge OpenMP::OpenMP_CXX)
 target_cxx_version(${PROJECT_NAME} PUBLIC VERSION ${DESCARTES_CXX_VERSION})
 target_compile_options(${PROJECT_NAME} PUBLIC ${DESCARTES_COMPILE_OPTIONS})

descartes_light/cmake/descartes_light_macros.cmake

@@ -23,18 +23,6 @@
 # See the License for the specific language governing permissions and
 # limitations under the License.
 
-macro(target_process_export target)
-  string(TOUPPER "${target}" EXPORT_PREFIX)
-
-  if (NOT DEFINED ${EXPORT_PREFIX}_LIBRARY_SHARED)
-    target_compile_definitions(${target} PRIVATE "${EXPORT_PREFIX}_LIBRARY_SHARED")
-  endif()
-
-  if(NOT BUILD_SHARED_LIBS)
-      target_compile_definitions(${target} PUBLIC "${EXPORT_PREFIX}_STATIC_LIBRARY")
-  endif()
-endmacro()
-
 macro(descartes_variables)
   if (NOT DEFINED BUILD_SHARED_LIBS)
     set(BUILD_SHARED_LIBS ON)

descartes_light/include/descartes_light/descartes_light.h

@@ -25,7 +25,7 @@ DESCARTES_IGNORE_WARNINGS_PUSH
 DESCARTES_IGNORE_WARNINGS_POP
 
 #include <descartes_light/ladder_graph.h>
-#include <descartes_light/interface/position_sampler.h>
+#include <descartes_light/interface/waypoint_sampler.h>
 #include <descartes_light/interface/edge_evaluator.h>
 
 namespace descartes_light
@@ -36,18 +36,18 @@ class Solver
 public:
   Solver(const std::size_t dof);
 
-  bool build(const std::vector<typename PositionSampler<FloatType>::Ptr>& trajectory,
-             const std::vector<typename EdgeEvaluator<FloatType>::Ptr>& edge_eval,
+  void build(const std::vector<typename WaypointSampler<FloatType>::ConstPtr>& trajectory,
+             const std::vector<typename EdgeEvaluator<FloatType>::ConstPtr>& edge_eval,
              int num_threads = getMaxThreads());
 
-  bool build(const std::vector<typename PositionSampler<FloatType>::Ptr>& trajectory,
-             typename EdgeEvaluator<FloatType>::Ptr edge_eval,
+  void build(const std::vector<typename WaypointSampler<FloatType>::ConstPtr>& trajectory,
+             typename EdgeEvaluator<FloatType>::ConstPtr edge_eval,
              int num_threads = getMaxThreads());
 
   const std::vector<std::size_t>& getFailedVertices() const { return failed_vertices_; }
   const std::vector<std::size_t>& getFailedEdges() const { return failed_edges_; }
 
-  bool search(std::vector<FloatType>& solution);
+  std::vector<Eigen::Matrix<FloatType, Eigen::Dynamic, 1>> search();
 
   static int getMaxThreads() { return omp_get_max_threads(); }
 

descartes_light/include/descartes_light/impl/descartes_light.hpp

@@ -23,6 +23,7 @@ DESCARTES_IGNORE_WARNINGS_PUSH
 #include <console_bridge/console.h>
 #include <sstream>
 #include <algorithm>
+#include <Eigen/Geometry>
 DESCARTES_IGNORE_WARNINGS_POP
 
 #include <descartes_light/descartes_light.h>
@@ -68,8 +69,8 @@ Solver<FloatType>::Solver(const std::size_t dof) : graph_{ dof }
 }
 
 template <typename FloatType>
-bool Solver<FloatType>::build(const std::vector<typename PositionSampler<FloatType>::Ptr>& trajectory,
-                              const std::vector<typename EdgeEvaluator<FloatType>::Ptr>& edge_eval,
+void Solver<FloatType>::build(const std::vector<typename WaypointSampler<FloatType>::ConstPtr>& trajectory,
+                              const std::vector<typename EdgeEvaluator<FloatType>::ConstPtr>& edge_eval,
                               int num_threads)
 {
   graph_.resize(trajectory.size());
@@ -82,10 +83,15 @@ bool Solver<FloatType>::build(const std::vector<typename PositionSampler<FloatTy
 #pragma omp parallel for num_threads(num_threads)
   for (long i = 0; i < static_cast<long>(trajectory.size()); ++i)
   {
-    std::vector<FloatType> vertex_data;
-    if (trajectory[static_cast<size_t>(i)]->sample(vertex_data))
+    std::vector<Eigen::Matrix<FloatType, Eigen::Dynamic, 1>> vertex_data = trajectory[static_cast<size_t>(i)]->sample();
+    if (!vertex_data.empty())
     {
-      graph_.getRung(static_cast<size_t>(i)).data = std::move(vertex_data);
+      Rung<FloatType>& r = graph_.getRung(static_cast<size_t>(i));
+      r.nodes.reserve(vertex_data.size());
+      for (const auto& v : vertex_data)
+      {
+        r.nodes.push_back(Node<FloatType>(v));
+      }
     }
     else
     {
@@ -110,11 +116,26 @@ bool Solver<FloatType>::build(const std::vector<typename PositionSampler<FloatTy
 #pragma omp parallel for num_threads(num_threads)
   for (long i = 1; i < static_cast<long>(trajectory.size()); ++i)
   {
-    const auto& from = graph_.getRung(static_cast<size_t>(i) - static_cast<size_t>(1));
+    auto& from = graph_.getRung(static_cast<size_t>(i) - static_cast<size_t>(1));
     const auto& to = graph_.getRung(static_cast<size_t>(i));
 
-    if (!edge_eval[static_cast<size_t>(i - 1)]->evaluate(
-            from, to, graph_.getEdges(static_cast<size_t>(i) - static_cast<size_t>(1))))
+    bool found = false;
+    for (std::size_t i = 0; i < from.nodes.size(); ++i)
+    {
+      for (std::size_t j = 0; j < to.nodes.size(); ++j)
+      {
+        // Consider the edge:
+        std::pair<bool, FloatType> results =
+            edge_eval[static_cast<size_t>(i - 1)]->evaluate(from.nodes[i].state, to.nodes[j].state);
+        if (results.first)
+        {
+          found = true;
+          from.nodes[i].edges.emplace_back(results.second, j);
+        }
+      }
+    }
+
+    if (!found)
     {
 #pragma omp critical
       {
@@ -140,40 +161,38 @@ bool Solver<FloatType>::build(const std::vector<typename PositionSampler<FloatTy
   reportFailedVertices(failed_vertices_);
   reportFailedEdges(failed_edges_);
 
-  return failed_edges_.empty() && failed_vertices_.empty();
+  if (!failed_edges_.empty() || !failed_vertices_.empty())
+    throw std::runtime_error("Failed to build graph.");
 }
 
 template <typename FloatType>
-bool Solver<FloatType>::build(const std::vector<typename PositionSampler<FloatType>::Ptr>& trajectory,
-                              typename EdgeEvaluator<FloatType>::Ptr edge_eval,
+void Solver<FloatType>::build(const std::vector<typename WaypointSampler<FloatType>::ConstPtr>& trajectory,
+                              typename EdgeEvaluator<FloatType>::ConstPtr edge_eval,
                               int num_threads)
 {
-  std::vector<typename EdgeEvaluator<FloatType>::Ptr> evaluators(trajectory.size() - 1, edge_eval);
-  return build(trajectory, evaluators, num_threads);
+  std::vector<typename EdgeEvaluator<FloatType>::ConstPtr> evaluators(trajectory.size() - 1, edge_eval);
+  build(trajectory, evaluators, num_threads);
 }
 
 template <typename FloatType>
-bool Solver<FloatType>::search(std::vector<FloatType>& solution)
+std::vector<Eigen::Matrix<FloatType, Eigen::Dynamic, 1>> Solver<FloatType>::search()
 {
   DAGSearch<FloatType> s(graph_);
   const auto cost = s.run();
 
+  std::vector<Eigen::Matrix<FloatType, Eigen::Dynamic, 1>> solution;
   if (cost == std::numeric_limits<FloatType>::max())
-    return false;
+    return solution;
 
   const auto indices = s.shortestPath();
-
   for (std::size_t i = 0; i < indices.size(); ++i)
-  {
-    const auto* pose = graph_.vertex(i, indices[i]);
-    solution.insert(end(solution), pose, pose + graph_.dof());
-  }
+    solution.push_back(graph_.getRung(i).nodes[indices[i]].state);
 
   std::stringstream ss;
   ss << "Solution found w/ cost = " << cost;
   CONSOLE_BRIDGE_logInform(ss.str().c_str());
 
-  return true;
+  return solution;
 }
 
 }  // namespace descartes_light

descartes_light/include/descartes_light/impl/ladder_graph.hpp

@@ -23,13 +23,13 @@
 namespace descartes_light
 {
 template <typename FloatType>
-LadderGraph<FloatType>::LadderGraph(const std::size_t dof) noexcept : dof_(dof)
+LadderGraph<FloatType>::LadderGraph(std::size_t dof) noexcept : dof_(dof)
 {
   assert(dof != 0);
 }
 
 template <typename FloatType>
-void LadderGraph<FloatType>::resize(const std::size_t n_rungs)
+void LadderGraph<FloatType>::resize(std::size_t n_rungs)
 {
   rungs_.resize(n_rungs);
 }
@@ -47,121 +47,90 @@ std::size_t LadderGraph<FloatType>::dof() const noexcept
 }
 
 template <typename FloatType>
-typename LadderGraph<FloatType>::Rung& LadderGraph<FloatType>::getRung(const std::size_t index) noexcept
+std::vector<Rung<FloatType>>& LadderGraph<FloatType>::getRungs() noexcept
 {
-  return const_cast<Rung&>(static_cast<const LadderGraph&>(*this).getRung(index));
+  return rungs_;
 }
 
 template <typename FloatType>
-const typename LadderGraph<FloatType>::Rung& LadderGraph<FloatType>::getRung(const std::size_t index) const noexcept
+const std::vector<Rung<FloatType>>& LadderGraph<FloatType>::getRungs() const noexcept
 {
-  assert(index < rungs_.size());
-  return rungs_[index];
+  return rungs_;
 }
 
 template <typename FloatType>
-std::vector<typename LadderGraph<FloatType>::EdgeList>&
-LadderGraph<FloatType>::getEdges(const std::size_t index) noexcept  // see p.23 Effective C++ (Scott Meyers)
+Rung<FloatType>& LadderGraph<FloatType>::getRung(std::size_t rung_index) noexcept
 {
-  return const_cast<std::vector<EdgeList>&>(static_cast<const LadderGraph&>(*this).getEdges(index));
+  return const_cast<Rung<FloatType>&>(static_cast<const LadderGraph&>(*this).getRung(rung_index));
 }
 
 template <typename FloatType>
-const std::vector<typename LadderGraph<FloatType>::EdgeList>&
-LadderGraph<FloatType>::getEdges(const std::size_t index) const noexcept
+const Rung<FloatType>& LadderGraph<FloatType>::getRung(std::size_t rung_index) const noexcept
 {
-  assert(index < rungs_.size());
-  return rungs_[index].edges;
+  assert(rung_index < rungs_.size());
+  return rungs_[rung_index];
 }
 
 template <typename FloatType>
-std::size_t LadderGraph<FloatType>::rungSize(const std::size_t index) const noexcept
+std::size_t LadderGraph<FloatType>::rungSize(std::size_t rung_index) const noexcept
 {
-  return getRung(index).data.size() / dof_;
+  return getRung(rung_index).nodes.size();
 }
 
 template <typename FloatType>
 std::size_t LadderGraph<FloatType>::numVertices() const noexcept
 {
   std::size_t count = 0;  // Add the size of each rung d
   for (const auto& rung : rungs_)
-    count += (rung.data.size() / dof_);
+    count += rung.nodes.size();
   return count;
 }
 
 template <typename FloatType>
 std::pair<std::size_t, bool> LadderGraph<FloatType>::indexOf(descartes_core::TrajectoryID id) const noexcept
 {
-  auto it = std::find_if(rungs_.cbegin(), rungs_.cend(), [id](const Rung& r) { return id == r.id; });
+  auto it = std::find_if(rungs_.cbegin(), rungs_.cend(), [id](const Rung<FloatType>& r) { return id == r.id; });
   if (it == rungs_.cend())
     return { 0u, false };
   else
     return { static_cast<std::size_t>(std::distance(rungs_.cbegin(), it)), true };
 }
 
 template <typename FloatType>
-bool LadderGraph<FloatType>::isLast(const std::size_t index) const noexcept
+bool LadderGraph<FloatType>::isLast(std::size_t rung_index) const noexcept
 {
-  return index + 1 == size();
+  return rung_index + 1 == size();
 }
 
 template <typename FloatType>
-bool LadderGraph<FloatType>::isFirst(const std::size_t index) const noexcept
+bool LadderGraph<FloatType>::isFirst(std::size_t rung_index) const noexcept
 {
-  return index == 0;
+  return rung_index == 0;
 }
 
 template <typename FloatType>
-const FloatType* LadderGraph<FloatType>::vertex(const std::size_t rung, const std::size_t index) const
+void LadderGraph<FloatType>::removeRung(std::size_t rung_index)
 {
-  return getRung(rung).data.data() + (dof_ * index);
+  rungs_.erase(std::next(rungs_.begin(), static_cast<long>(rung_index)));
 }
 
 template <typename FloatType>
-void LadderGraph<FloatType>::assignEdges(const std::size_t rung,
-                                         std::vector<EdgeList>&& edges)  // noexcept?
+void LadderGraph<FloatType>::clearNodes(std::size_t rung_index)
 {
-  getEdges(rung) = std::move(edges);
+  rungs_[rung_index].nodes.clear();
 }
 
 template <typename FloatType>
-void LadderGraph<FloatType>::assignRung(const std::size_t index,
-                                        descartes_core::TrajectoryID id,
-                                        const std::vector<std::vector<FloatType>>& sols)
+void LadderGraph<FloatType>::clearEdges(std::size_t rung_index)
 {
-  Rung& r = getRung(index);
-  r.id = id;
-  r.data.reserve(sols.size() * dof_);
-  for (const auto& sol : sols)
-  {
-    r.data.insert(r.data.end(), sol.cbegin(), sol.cend());
-  }
-  // Given this new vertex set, build an edge list for each
-  getEdges(index).resize(r.data.size());
+  for (auto& n : rungs_[rung_index].nodes)
+    n.edges.clear();
 }
 
 template <typename FloatType>
-void LadderGraph<FloatType>::removeRung(const std::size_t index)
+void LadderGraph<FloatType>::insertRung(std::size_t rung_index)
 {
-  rungs_.erase(std::next(rungs_.begin(), static_cast<long>(index)));
-}
-
-template <typename FloatType>
-void LadderGraph<FloatType>::clearVertices(const std::size_t index)
-{
-  rungs_[index].data.clear();
-}
-
-template <typename FloatType>
-void LadderGraph<FloatType>::clearEdges(const std::size_t index)
-{
-  rungs_[index].edges.clear();
-}
-
-template <typename FloatType>
-void LadderGraph<FloatType>::insertRung(const std::size_t index)
-{
-  rungs_.insert(std::next(rungs_.begin(), static_cast<long>(index)), Rung());
+  rungs_.insert(std::next(rungs_.begin(), static_cast<long>(rung_index)), Rung<FloatType>());
 }
 
 template <typename FloatType>

descartes_light/include/descartes_light/impl/ladder_graph_dag_search.hpp

@@ -49,24 +49,24 @@ FloatType DAGSearch<FloatType>::run()
   }
 
   // Now we iterate over the graph in 'topological' order
-  for (size_type rung = 0; rung < solution_.size() - 1; ++rung)
+  for (size_type r = 0; r < solution_.size() - 1; ++r)
   {
-    const auto n_vertices = graph_.rungSize(rung);
-    const auto next_rung = rung + 1;
+    const auto next_r = r + 1;
+    const Rung<FloatType>& rung = graph_.getRung(r);
+
     // For each vertex in the out edge list
-    for (size_t index = 0; index < n_vertices; ++index)
+    for (size_t v = 0; v < rung.nodes.size(); ++v)
     {
-      const auto u_cost = distance(rung, index);
-      const auto& edges = graph_.getEdges(rung)[index];
+      const auto u_cost = distance(r, v);
       // for each out edge
-      for (const auto& edge : edges)
+      for (const auto& edge : rung.nodes[v].edges)
       {
         auto dv = u_cost + edge.cost;  // new cost
-        if (dv < distance(next_rung, edge.idx))
+        if (dv < distance(next_r, edge.idx))
         {
-          distance(next_rung, edge.idx) = dv;
-          predecessor(next_rung, edge.idx) =
-              static_cast<unsigned>(index);  // the predecessor's rung is implied to be the current rung
+          distance(next_r, edge.idx) = dv;
+          // the predecessor's rung is implied to be the current rung
+          predecessor(next_r, edge.idx) = static_cast<unsigned>(v);
         }
       }
     }  // vertex for loop