BGL "efficient" Dijkstra Solver

descartes_light/solvers/include/descartes_light/solvers/bgl/bgl_dijkstra_solver.h

@@ -0,0 +1,43 @@
+#ifndef DESCARTES_LIGHT_SOLVERS_BGL_BGL_DIJKSTRA_SOLVER_H
+#define DESCARTES_LIGHT_SOLVERS_BGL_BGL_DIJKSTRA_SOLVER_H
+
+#include <descartes_light/solvers/bgl/bgl_solver.h>
+
+namespace descartes_light
+{
+/**
+ * @brief BGL solver implementation that constructs vertices and edges in the build function and uses Dijkstra's
+ * algorithm with a default visitor to search the graph
+ */
+template <typename FloatType>
+class BGLDijkstraSolverVE : public BGLSolverBaseVE<FloatType>
+{
+public:
+ using BGLSolverBaseVE<FloatType>::BGLSolverBaseVE;
+
+ SearchResult<FloatType> search() override;
+};
+
+using BGLDijkstraSolverVEF = BGLDijkstraSolverVE<float>;
+using BGLDijkstraSolverVED = BGLDijkstraSolverVE<double>;
+
+/**
+ * @brief BGL solver implementation that constructs vertices and edges in the build function and uses Dijkstra's
+ * algorithm with a visitor that terminates the search once a vertex in the last rung of the graph is encountered rather
+ * than allowing it to continue until the distance to all nodes in the graph has been calculated
+ */
+template <typename FloatType>
+class BGLEfficientDijkstraSolverVE : public BGLSolverBaseVE<FloatType>
+{
+public:
+ using BGLSolverBaseVE<FloatType>::BGLSolverBaseVE;
+
+ SearchResult<FloatType> search() override;
+};
+
+using BGLEfficientDijkstraSolverVEF = BGLEfficientDijkstraSolverVE<float>;
+using BGLEfficientDijkstraSolverVED = BGLEfficientDijkstraSolverVE<double>;
+
+} // namespace descartes_light
+
+#endif // DESCARTES_LIGHT_SOLVERS_BGL_BGL_DIJKSTRA_SOLVER_H

descartes_light/solvers/include/descartes_light/solvers/bgl/bgl_solver.h

@@ -97,20 +97,4 @@ class BGLSolverBaseVE : public BGLSolverBaseV<FloatType>
  const std::vector<typename StateEvaluator<FloatType>::ConstPtr>& state_eval) override;
 };
 
-/**
- * @brief BGL solver implementation that constructs vertices and edges in the build function and uses Dijkstra's
- * algorithm with a default visitor to search the graph
- */
-template <typename FloatType>
-class BGLDijkstraSolverVE : public BGLSolverBaseVE<FloatType>
-{
-public:
- using BGLSolverBaseVE<FloatType>::BGLSolverBaseVE;
-
- SearchResult<FloatType> search() override;
-};
-
-using BGLDijkstraSolverVEF = BGLDijkstraSolverVE<float>;
-using BGLDijkstraSolverVED = BGLDijkstraSolverVE<double>;
-
 } // namespace descartes_light

descartes_light/solvers/include/descartes_light/solvers/bgl/impl/bgl_dijkstra_solver.hpp

@@ -0,0 +1,146 @@
+#ifndef DESCARTES_LIGHT_SOLVERS_BGL_IMPL_BGL_DIJKSTRA_SOLVER_HPP
+#define DESCARTES_LIGHT_SOLVERS_BGL_IMPL_BGL_DIJKSTRA_SOLVER_HPP
+
+#include <descartes_light/solvers/bgl/bgl_dijkstra_solver.h>
+
+#include <descartes_light/descartes_macros.h>
+DESCARTES_IGNORE_WARNINGS_PUSH
+#include <boost/graph/dijkstra_shortest_paths.hpp>
+DESCARTES_IGNORE_WARNINGS_POP
+
+namespace descartes_light
+{
+template <typename FloatType>
+SearchResult<FloatType> BGLDijkstraSolverVE<FloatType>::search()
+{
+ // Convenience aliases
+ auto& graph_ = BGLSolverBase<FloatType>::graph_;
+ const auto& source_ = BGLSolverBase<FloatType>::source_;
+ auto& predecessor_map_ = BGLSolverBase<FloatType>::predecessor_map_;
+ const auto& ladder_rungs_ = BGLSolverBase<FloatType>::ladder_rungs_;
+
+ // Internal properties
+ auto index_prop_map = boost::get(boost::vertex_index, graph_);
+ auto weight_prop_map = boost::get(boost::edge_weight, graph_);
+ auto color_prop_map = boost::get(&Vertex<FloatType>::color, graph_);
+ auto distance_prop_map = boost::get(&Vertex<FloatType>::distance, graph_);
+
+ predecessor_map_.clear();
+ boost::associative_property_map<std::map<VertexDesc<FloatType>, VertexDesc<FloatType>>> predecessor_prop_map(
+ predecessor_map_);
+
+ // Perform the search
+ boost::dijkstra_shortest_paths(graph_,
+ source_,
+ predecessor_prop_map,
+ distance_prop_map,
+ weight_prop_map,
+ index_prop_map,
+ std::less<>(),
+ std::plus<>(),
+ std::numeric_limits<FloatType>::max(),
+ static_cast<FloatType>(0.0),
+ boost::default_dijkstra_visitor(),
+ color_prop_map);
+
+ // Find lowest cost node in last rung
+ auto target = std::min_element(ladder_rungs_.back().begin(),
+ ladder_rungs_.back().end(),
+ [&](const VertexDesc<FloatType>& a, const VertexDesc<FloatType>& b) {
+ return graph_[a].distance < graph_[b].distance;
+ });
+
+ SearchResult<FloatType> result;
+
+ // Reconstruct the path from the predecesor map; remove the artificial start state
+ const auto vd_path = BGLSolverBase<FloatType>::reconstructPath(source_, *target);
+ result.trajectory = BGLSolverBase<FloatType>::toStates(vd_path);
+ result.trajectory.erase(result.trajectory.begin());
+
+ result.cost = graph_[*target].distance;
+
+ return result;
+}
+
+/**
+ * @brief Visitor for Dijkstra search that terminates the search once a vertex in the last rung has been encountered
+ */
+template <typename FloatType>
+class DijkstraTerminateEarlyVisitor : public boost::default_dijkstra_visitor
+{
+public:
+ DijkstraTerminateEarlyVisitor(long last_rung_idx) : last_rung_idx_(last_rung_idx) {}
+
+ /**
+ * @brief Hook for introspecting the search when a new vertex is opened by the search.
+ * @details Throws the vertex descriptor that is the termination of the path once the first vertex in the last rung of
+ * the graph is encountered
+ */
+ void examine_vertex(VertexDesc<FloatType> u, const BGLGraph<FloatType>& g)
+ {
+ if (g[u].rung_idx == last_rung_idx_)
+ throw u;
+ }
+
+private:
+ const long last_rung_idx_;
+};
+
+template <typename FloatType>
+SearchResult<FloatType> BGLEfficientDijkstraSolverVE<FloatType>::search()
+{
+ // Convenience aliases
+ auto& graph_ = BGLSolverBase<FloatType>::graph_;
+ const auto& source_ = BGLSolverBase<FloatType>::source_;
+ auto& predecessor_map_ = BGLSolverBase<FloatType>::predecessor_map_;
+ const auto& ladder_rungs_ = BGLSolverBase<FloatType>::ladder_rungs_;
+
+ // Internal properties
+ auto index_prop_map = boost::get(boost::vertex_index, graph_);
+ auto weight_prop_map = boost::get(boost::edge_weight, graph_);
+ auto color_prop_map = boost::get(&Vertex<FloatType>::color, graph_);
+ auto distance_prop_map = boost::get(&Vertex<FloatType>::distance, graph_);
+
+ predecessor_map_.clear();
+ boost::associative_property_map<std::map<VertexDesc<FloatType>, VertexDesc<FloatType>>> predecessor_prop_map(
+ predecessor_map_);
+
+ DijkstraTerminateEarlyVisitor<FloatType> visitor(static_cast<long>(ladder_rungs_.size() - 1));
+
+ // Perform the search
+ try
+ {
+ boost::dijkstra_shortest_paths(graph_,
+ source_,
+ predecessor_prop_map,
+ distance_prop_map,
+ weight_prop_map,
+ index_prop_map,
+ std::less<>(),
+ std::plus<>(),
+ std::numeric_limits<FloatType>::max(),
+ static_cast<FloatType>(0.0),
+ visitor,
+ color_prop_map);
+ }
+ catch (const VertexDesc<FloatType>& target)
+ {
+ SearchResult<FloatType> result;
+
+ // Reconstruct the path from the predecesor map; remove the artificial start state
+ const auto vd_path = BGLSolverBase<FloatType>::reconstructPath(source_, target);
+ result.trajectory = BGLSolverBase<FloatType>::toStates(vd_path);
+ result.trajectory.erase(result.trajectory.begin());
+
+ result.cost = graph_[target].distance;
+
+ return result;
+ }
+
+ // If the visitor never threw the vertex descriptor, there was an issue with the search
+ throw std::runtime_error("Search failed to encounter vertex associated with the last waypoint in the trajectory");
+}
+
+} // namespace descartes_light
+
+#endif // DESCARTES_LIGHT_SOLVERS_BGL_IMPL_BGL_DIJKSTRA_SOLVER_HPP

descartes_light/solvers/include/descartes_light/solvers/bgl/impl/bgl_solver.hpp

@@ -19,7 +19,6 @@
 
 #include <descartes_light/descartes_macros.h>
 DESCARTES_IGNORE_WARNINGS_PUSH
-#include <boost/graph/dijkstra_shortest_paths.hpp>
 #include <console_bridge/console.h>
 #include <fstream>
 #include <omp.h>
@@ -299,56 +298,4 @@ BGLSolverBaseVE<FloatType>::buildImpl(const std::vector<typename WaypointSampler
  return status;
 }
 
-template <typename FloatType>
-SearchResult<FloatType> BGLDijkstraSolverVE<FloatType>::search()
-{
- // Convenience aliases
- auto& graph_ = BGLSolverBase<FloatType>::graph_;
- const auto& source_ = BGLSolverBase<FloatType>::source_;
- auto& predecessor_map_ = BGLSolverBase<FloatType>::predecessor_map_;
- const auto& ladder_rungs_ = BGLSolverBase<FloatType>::ladder_rungs_;
-
- // Internal properties
- auto index_prop_map = boost::get(boost::vertex_index, graph_);
- auto weight_prop_map = boost::get(boost::edge_weight, graph_);
- auto color_prop_map = boost::get(&Vertex<FloatType>::color, graph_);
- auto distance_prop_map = boost::get(&Vertex<FloatType>::distance, graph_);
-
- predecessor_map_.clear();
- boost::associative_property_map<std::map<VertexDesc<FloatType>, VertexDesc<FloatType>>> predecessor_prop_map(
- predecessor_map_);
-
- // Perform the search
- boost::dijkstra_shortest_paths(graph_,
- source_,
- predecessor_prop_map,
- distance_prop_map,
- weight_prop_map,
- index_prop_map,
- std::less<>(),
- std::plus<>(),
- std::numeric_limits<FloatType>::max(),
- static_cast<FloatType>(0.0),
- boost::default_dijkstra_visitor(),
- color_prop_map);
-
- // Find lowest cost node in last rung
- auto target = std::min_element(ladder_rungs_.back().begin(),
- ladder_rungs_.back().end(),
- [&](const VertexDesc<FloatType>& a, const VertexDesc<FloatType>& b) {
- return graph_[a].distance < graph_[b].distance;
- });
-
- SearchResult<FloatType> result;
-
- // Reconstruct the path from the predecesor map; remove the artificial start state
- const auto vd_path = BGLSolverBase<FloatType>::reconstructPath(source_, *target);
- result.trajectory = BGLSolverBase<FloatType>::toStates(vd_path);
- result.trajectory.erase(result.trajectory.begin());
-
- result.cost = graph_[*target].distance;
-
- return result;
-}
-
 } // namespace descartes_light

descartes_light/solvers/src/bgl/bgl_solver.cpp

@@ -16,6 +16,7 @@
  * limitations under the License.
  */
 #include <descartes_light/solvers/bgl/impl/bgl_solver.hpp>
+#include <descartes_light/solvers/bgl/impl/bgl_dijkstra_solver.hpp>
 #include <descartes_light/solvers/bgl/impl/utils.hpp>
 
 namespace descartes_light
@@ -35,6 +36,9 @@ template class BGLSolverBaseVE<float>;
 template class BGLDijkstraSolverVE<double>;
 template class BGLDijkstraSolverVE<float>;
 
+template class BGLEfficientDijkstraSolverVE<double>;
+template class BGLEfficientDijkstraSolverVE<float>;
+
 // Free functions
 template SubGraph<double> createDecoratedSubGraph(const BGLGraph<double>& g);
 template SubGraph<float> createDecoratedSubGraph(const BGLGraph<float>& g);

descartes_light/test/benchmarks/benchmarks.cpp

@@ -3,7 +3,7 @@
 #include <descartes_light/test/solver_factory.h>
 // Solvers
 #include <descartes_light/solvers/ladder_graph/ladder_graph_solver.h>
-#include <descartes_light/solvers/bgl/bgl_solver.h>
+#include <descartes_light/solvers/bgl/bgl_dijkstra_solver.h>
 
 DESCARTES_IGNORE_WARNINGS_PUSH
 #include <boost/core/demangle.hpp>
@@ -80,9 +80,13 @@ int main(int, char**)
  benchmark(SolverFactory<LadderGraphSolverD>());
  benchmark(SolverFactory<LadderGraphSolverF>());
 
- // BGL ladder graph
+ // BGL full Dijkstra solver
  benchmark(SolverFactory<BGLDijkstraSolverVED>());
  benchmark(SolverFactory<BGLDijkstraSolverVEF>());
 
+ // BGL efficient Dijkstra solver
+ benchmark(SolverFactory<BGLEfficientDijkstraSolverVED>());
+ benchmark(SolverFactory<BGLEfficientDijkstraSolverVEF>());
+
  return 0;
 }

descartes_light/test/include/descartes_light/test/impl/solver_factory.hpp

@@ -2,24 +2,32 @@
 
 #include <descartes_light/test/solver_factory.h>
 #include <descartes_light/solvers/ladder_graph/ladder_graph_solver.h>
-#include <descartes_light/solvers/bgl/bgl_solver.h>
+#include <descartes_light/solvers/bgl/bgl_dijkstra_solver.h>
 
 namespace descartes_light
 {
-// Ladder graph solver Factory
-template <typename FloatT>
-struct SolverFactory<LadderGraphSolver<FloatT>>
+// Ladder graph solver factory
+template <typename FloatType>
+struct SolverFactory<LadderGraphSolver<FloatType>>
 {
- using FloatType = FloatT;
- typename Solver<FloatT>::Ptr create() const { return std::make_unique<LadderGraphSolver<FloatT>>(1); }
+ typename Solver<FloatType>::Ptr create() const { return std::make_shared<LadderGraphSolver<FloatType>>(1); }
 };
 
-// Boost Ladder graph solver Factory
-template <typename FloatT>
-struct SolverFactory<BGLDijkstraSolverVE<FloatT>>
+// Boost full Dijkstra graph solver factory
+template <typename FloatType>
+struct SolverFactory<BGLDijkstraSolverVE<FloatType>>
 {
- using FloatType = FloatT;
- typename Solver<FloatT>::Ptr create() const { return std::make_unique<BGLDijkstraSolverVE<FloatT>>(1); }
+ typename Solver<FloatType>::Ptr create() const { return std::make_shared<BGLDijkstraSolverVE<FloatType>>(1); }
+};
+
+// Boost efficient Dijkstra graph solver factory
+template <typename FloatType>
+struct SolverFactory<BGLEfficientDijkstraSolverVE<FloatType>>
+{
+ typename Solver<FloatType>::Ptr create() const
+ {
+ return std::make_shared<BGLEfficientDijkstraSolverVE<FloatType>>(1);
+ }
 };
 
 } // namespace descartes_light