37 #include "ompl/geometric/PathHybridization.h" 38 #include <boost/graph/dijkstra_shortest_paths.hpp> 51 : si_(std::move(si)), stateProperty_(boost::get(vertex_state_t(), g_)), name_(
"PathHybridization")
53 root_ = boost::add_vertex(g_);
54 stateProperty_[root_] =
nullptr;
55 goal_ = boost::add_vertex(g_);
56 stateProperty_[goal_] =
nullptr;
59 ompl::geometric::PathHybridization::~PathHybridization() =
default;
67 root_ = boost::add_vertex(g_);
68 stateProperty_[root_] =
nullptr;
69 goal_ = boost::add_vertex(g_);
70 stateProperty_[goal_] =
nullptr;
75 out <<
"Path hybridization is aware of " << paths_.size() <<
" paths" << std::endl;
77 for (
auto it = paths_.begin(); it != paths_.end(); ++it, ++i)
78 out <<
" path " << i <<
" of length " << it->length_ << std::endl;
80 out <<
"Hybridized path of length " << hpath_->length() << std::endl;
90 boost::vector_property_map<Vertex> prev(boost::num_vertices(g_));
91 boost::dijkstra_shortest_paths(g_, root_, boost::predecessor_map(prev));
92 if (prev[goal_] != goal_)
94 auto h(std::make_shared<PathGeometric>(si_));
95 for (Vertex pos = prev[goal_]; prev[pos] != pos; pos = prev[pos])
96 h->append(stateProperty_[pos]);
109 auto *p = dynamic_cast<PathGeometric *>(pp.get());
112 OMPL_ERROR(
"Path hybridization only works for geometric paths");
116 if (p->getSpaceInformation() != si_)
118 OMPL_ERROR(
"Paths for hybridization must be from the same space information");
123 if (p->getStateCount() == 0)
129 if (paths_.find(pi) != paths_.end())
133 unsigned int nattempts = 0;
136 Vertex v0 = boost::add_vertex(g_);
137 stateProperty_[v0] = pi.states_[0];
138 pi.vertices_.push_back(v0);
142 const HGraph::edge_property_type prop0(0.0);
143 boost::add_edge(root_, v0, prop0, g_);
145 for (std::size_t j = 1; j < pi.states_.size(); ++j)
147 Vertex v1 = boost::add_vertex(g_);
148 stateProperty_[v1] = pi.states_[j];
149 double weight = si_->distance(pi.states_[j - 1], pi.states_[j]);
150 const HGraph::edge_property_type properties(weight);
151 boost::add_edge(v0, v1, properties, g_);
153 pi.vertices_.push_back(v1);
158 boost::add_edge(v0, goal_, prop0, g_);
162 for (
const auto &path : paths_)
164 const auto *q = static_cast<const PathGeometric *>(path.path_.get());
165 std::vector<int> indexP, indexQ;
176 for (std::size_t i = 0; i < indexP.size(); ++i)
192 for (std::size_t j = gapStartP; j < i; ++j)
194 attemptNewEdge(pi, path, indexP[i], indexQ[j]);
210 for (std::size_t j = gapStartQ; j < i; ++j)
212 attemptNewEdge(pi, path, indexP[j], indexQ[i]);
220 if (lastP >= 0 && lastQ >= 0)
222 attemptNewEdge(pi, path, indexP[lastP], indexQ[lastQ]);
230 for (std::size_t i = 0; i < indexP.size(); ++i)
231 if (indexP[i] >= 0 && indexQ[i] >= 0)
233 attemptNewEdge(pi, path, indexP[i], indexQ[i]);
244 void ompl::geometric::PathHybridization::attemptNewEdge(
const PathInfo &p,
const PathInfo &q,
int indexP,
int indexQ)
246 if (si_->checkMotion(p.states_[indexP], q.states_[indexQ]))
248 double weight = si_->distance(p.states_[indexP], q.states_[indexQ]);
249 const HGraph::edge_property_type properties(weight);
250 boost::add_edge(p.vertices_[indexP], q.vertices_[indexQ], properties, g_);
256 return paths_.size();
260 std::vector<int> &indexP, std::vector<int> &indexQ)
const 273 double match = si_->distance(p.
getState(i), q.
getState(j)) + ((i > 0 && j > 0) ? C[i - 1][j - 1] : 0.0);
274 double up = gapCost + (i > 0 ? C[i - 1][j] : 0.0);
275 double left = gapCost + (j > 0 ? C[i][j - 1] : 0.0);
276 if (match <= up && match <= left)
281 else if (up <= match && up <= left)
299 indexP.reserve(std::max(n, m));
300 indexQ.reserve(indexP.size());
302 while (n >= 0 && m >= 0)
311 else if (T[m][n] ==
'u')
314 indexQ.push_back(-1);
319 indexP.push_back(-1);
326 indexP.push_back(-1);
333 indexQ.push_back(-1);
static const double GAP_COST_FRACTION
The fraction of the path length to consider as gap cost when aligning paths to be hybridized.
void clear()
Clear all the stored paths.
const base::PathPtr & getHybridPath() const
Get the currently computed hybrid path. computeHybridPath() needs to have been called before.
std::size_t getStateCount() const
Get the number of states (way-points) that make up this path.
const std::string & getName() const
Get the name of the algorithm.
void computeHybridPath()
Run Dijkstra's algorithm to find out the shortest path among the mixed ones.
base::State * getState(unsigned int index)
Get the state located at index along the path.
Main namespace. Contains everything in this library.
#define OMPL_ERROR(fmt,...)
Log a formatted error string.
void print(std::ostream &out=std::cout) const
Print information about the computed path.
unsigned int recordPath(const base::PathPtr &pp, bool matchAcrossGaps)
Add a path to the hybridization. If matchAcrossGaps is true, more possible edge connections are evalu...
std::size_t pathCount() const
Get the number of paths that are currently considered as part of the hybridization.
void matchPaths(const geometric::PathGeometric &p, const geometric::PathGeometric &q, double gapCost, std::vector< int > &indexP, std::vector< int > &indexQ) const
Given two geometric paths p and q, compute the alignment of the paths using dynamic programming in an...
Definition of a geometric path.
PathHybridization(base::SpaceInformationPtr si)
The constructor needs to know about the space information of the paths it will operate on.
A shared pointer wrapper for ompl::base::Path.