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 logError(
"Path hybridization only works for geometric paths");
114 logError(
"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);