37 #include "ompl/geometric/PathHybridization.h"
38 #include <boost/graph/dijkstra_shortest_paths.hpp>
50 si_(si), stateProperty_(boost::get(vertex_state_t(), g_))
52 root_ = boost::add_vertex(g_);
53 stateProperty_[root_] = NULL;
54 goal_ = boost::add_vertex(g_);
55 stateProperty_[goal_] = NULL;
58 ompl::geometric::PathHybridization::~PathHybridization(
void)
68 root_ = boost::add_vertex(g_);
69 stateProperty_[root_] = NULL;
70 goal_ = boost::add_vertex(g_);
71 stateProperty_[goal_] = NULL;
76 out <<
"Path hybridization is aware of " << paths_.size() <<
" paths" << std::endl;
78 for (std::set<PathInfo>::const_iterator it = paths_.begin() ; it != paths_.end() ; ++it, ++i)
79 out <<
" path " << i <<
" of length " << it->length_ << std::endl;
81 out <<
"Hybridized path of length " << hpath_->length() << std::endl;
86 boost::vector_property_map<Vertex> prev(boost::num_vertices(g_));
87 boost::dijkstra_shortest_paths(g_, root_, boost::predecessor_map(prev));
88 if (prev[goal_] != goal_)
91 for (Vertex pos = prev[goal_]; prev[pos] != pos; pos = prev[pos])
92 h->
append(stateProperty_[pos]);
108 OMPL_ERROR(
"Path hybridization only works for geometric paths");
114 OMPL_ERROR(
"Paths for hybridization must be from the same space information");
125 if (paths_.find(pi) != paths_.end())
129 unsigned int nattempts = 0;
132 Vertex v0 = boost::add_vertex(g_);
133 stateProperty_[v0] = pi.states_[0];
134 pi.vertices_.push_back(v0);
138 const HGraph::edge_property_type prop0(0.0);
139 boost::add_edge(root_, v0, prop0, g_);
141 for (std::size_t j = 1 ; j < pi.states_.size() ; ++j)
143 Vertex v1 = boost::add_vertex(g_);
144 stateProperty_[v1] = pi.states_[j];
145 double weight = si_->distance(pi.states_[j-1], pi.states_[j]);
146 const HGraph::edge_property_type properties(weight);
147 boost::add_edge(v0, v1, properties, g_);
149 pi.vertices_.push_back(v1);
154 boost::add_edge(v0, goal_, prop0, g_);
158 for (std::set<PathInfo>::const_iterator it = paths_.begin() ; it != paths_.end() ; ++it)
161 std::vector<int> indexP, indexQ;
172 for (std::size_t i = 0 ; i < indexP.size() ; ++i)
188 for (std::size_t j = gapStartP ; j < i ; ++j)
190 attemptNewEdge(pi, *it, indexP[i], indexQ[j]);
206 for (std::size_t j = gapStartQ ; j < i ; ++j)
208 attemptNewEdge(pi, *it, indexP[j], indexQ[i]);
216 if (lastP >= 0 && lastQ >= 0)
218 attemptNewEdge(pi, *it, indexP[lastP], indexQ[lastQ]);
226 for (std::size_t i = 0 ; i < indexP.size() ; ++i)
227 if (indexP[i] >= 0 && indexQ[i] >= 0)
229 attemptNewEdge(pi, *it, indexP[i], indexQ[i]);
240 void ompl::geometric::PathHybridization::attemptNewEdge(
const PathInfo &p,
const PathInfo &q,
int indexP,
int indexQ)
242 if (si_->checkMotion(p.states_[indexP], q.states_[indexQ]))
244 double weight = si_->distance(p.states_[indexP], q.states_[indexQ]);
245 const HGraph::edge_property_type properties(weight);
246 boost::add_edge(p.vertices_[indexP], q.vertices_[indexQ], properties, g_);
252 return paths_.size();
256 std::vector<int> &indexP, std::vector<int> &indexQ)
const
269 double match = si_->distance(p.
getState(i), q.
getState(j)) + ((i > 0 && j > 0) ? C[i - 1][j - 1] : 0.0);
270 double up = gapCost + (i > 0 ? C[i - 1][j] : 0.0);
271 double left = gapCost + (j > 0 ? C[i][j - 1] : 0.0);
272 if (match <= up && match <= left)
278 if (up <= match && up <= left)
296 indexP.reserve(std::max(n,m));
297 indexQ.reserve(indexP.size());
299 while (n >= 0 && m >= 0)
311 indexQ.push_back(-1);
316 indexP.push_back(-1);
323 indexP.push_back(-1);
330 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...
const SpaceInformationPtr & getSpaceInformation(void) const
Get the space information associated to this class.
void clear(void)
Clear all the stored paths.
void append(const base::State *state)
Append state to the end of this path. The memory for state is copied.
base::State * getState(unsigned int index)
Get the state located at index along the path.
void print(std::ostream &out=std::cout) const
Print information about the computed path.
PathHybridization(const base::SpaceInformationPtr &si)
The constructor needs to know about the space information of the paths it will operate on...
std::size_t getStateCount(void) const
Get the number of states (way-points) that make up this path.
const base::PathPtr & getHybridPath(void) const
Get the currently computed hybrid path. computeHybridPath() needs to have been called before...
void computeHybridPath(void)
Run Dijkstra's algorithm to find out the shortest path among the mixed ones.
#define OMPL_ERROR(fmt,...)
Log a formatted error string.
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...
void reverse(void)
Reverse the 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(void) const
Get the number of paths that are currently considered as part of the hybridization.
Definition of a geometric path.
A boost shared pointer wrapper for ompl::base::Path.