ompl/geometric/src/PathHybridization.cpp
00001 /********************************************************************* 00002 * Software License Agreement (BSD License) 00003 * 00004 * Copyright (c) 2011, Willow Garage, Inc. 00005 * All rights reserved. 00006 * 00007 * Redistribution and use in source and binary forms, with or without 00008 * modification, are permitted provided that the following conditions 00009 * are met: 00010 * 00011 * * Redistributions of source code must retain the above copyright 00012 * notice, this list of conditions and the following disclaimer. 00013 * * Redistributions in binary form must reproduce the above 00014 * copyright notice, this list of conditions and the following 00015 * disclaimer in the documentation and/or other materials provided 00016 * with the distribution. 00017 * * Neither the name of the Willow Garage nor the names of its 00018 * contributors may be used to endorse or promote products derived 00019 * from this software without specific prior written permission. 00020 * 00021 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 00022 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 00023 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 00024 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 00025 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 00026 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 00027 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 00028 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 00029 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 00030 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 00031 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00032 * POSSIBILITY OF SUCH DAMAGE. 00033 *********************************************************************/ 00034 00035 /* Author: Ioan Sucan */ 00036 00037 #include "ompl/geometric/PathHybridization.h" 00038 #include <boost/graph/dijkstra_shortest_paths.hpp> 00039 00040 namespace ompl 00041 { 00042 namespace magic 00043 { 00045 static const double GAP_COST_FRACTION = 0.05; 00046 } 00047 } 00048 00049 ompl::geometric::PathHybridization::PathHybridization(const base::SpaceInformationPtr &si) : 00050 si_(si), 00051 stateProperty_(boost::get(vertex_state_t(), g_)), 00052 name_("PathHybridization") 00053 { 00054 root_ = boost::add_vertex(g_); 00055 stateProperty_[root_] = NULL; 00056 goal_ = boost::add_vertex(g_); 00057 stateProperty_[goal_] = NULL; 00058 } 00059 00060 ompl::geometric::PathHybridization::~PathHybridization() 00061 { 00062 } 00063 00064 void ompl::geometric::PathHybridization::clear() 00065 { 00066 hpath_.reset(); 00067 paths_.clear(); 00068 00069 g_.clear(); 00070 root_ = boost::add_vertex(g_); 00071 stateProperty_[root_] = NULL; 00072 goal_ = boost::add_vertex(g_); 00073 stateProperty_[goal_] = NULL; 00074 } 00075 00076 void ompl::geometric::PathHybridization::print(std::ostream &out) const 00077 { 00078 out << "Path hybridization is aware of " << paths_.size() << " paths" << std::endl; 00079 int i = 1; 00080 for (std::set<PathInfo>::const_iterator it = paths_.begin() ; it != paths_.end() ; ++it, ++i) 00081 out << " path " << i << " of length " << it->length_ << std::endl; 00082 if (hpath_) 00083 out << "Hybridized path of length " << hpath_->length() << std::endl; 00084 } 00085 00086 const std::string& ompl::geometric::PathHybridization::getName() const 00087 { 00088 return name_; 00089 } 00090 00091 void ompl::geometric::PathHybridization::computeHybridPath() 00092 { 00093 boost::vector_property_map<Vertex> prev(boost::num_vertices(g_)); 00094 boost::dijkstra_shortest_paths(g_, root_, boost::predecessor_map(prev)); 00095 if (prev[goal_] != goal_) 00096 { 00097 PathGeometric *h = new PathGeometric(si_); 00098 for (Vertex pos = prev[goal_]; prev[pos] != pos; pos = prev[pos]) 00099 h->append(stateProperty_[pos]); 00100 h->reverse(); 00101 hpath_.reset(h); 00102 } 00103 } 00104 00105 const ompl::base::PathPtr& ompl::geometric::PathHybridization::getHybridPath() const 00106 { 00107 return hpath_; 00108 } 00109 00110 unsigned int ompl::geometric::PathHybridization::recordPath(const base::PathPtr &pp, bool matchAcrossGaps) 00111 { 00112 PathGeometric *p = dynamic_cast<PathGeometric*>(pp.get()); 00113 if (!p) 00114 { 00115 OMPL_ERROR("Path hybridization only works for geometric paths"); 00116 return 0; 00117 } 00118 00119 if (p->getSpaceInformation() != si_) 00120 { 00121 OMPL_ERROR("Paths for hybridization must be from the same space information"); 00122 return 0; 00123 } 00124 00125 // skip empty paths 00126 if (p->getStateCount() == 0) 00127 return 0; 00128 00129 PathInfo pi(pp); 00130 00131 // if this path was previously included in the hybridization, skip it 00132 if (paths_.find(pi) != paths_.end()) 00133 return 0; 00134 00135 // the number of connection attempts 00136 unsigned int nattempts = 0; 00137 00138 // start from virtual root 00139 Vertex v0 = boost::add_vertex(g_); 00140 stateProperty_[v0] = pi.states_[0]; 00141 pi.vertices_.push_back(v0); 00142 00143 // add all the vertices of the path, and the edges between them, to the HGraph 00144 // also compute the path length for future use (just for computational savings) 00145 const HGraph::edge_property_type prop0(0.0); 00146 boost::add_edge(root_, v0, prop0, g_); 00147 double length = 0.0; 00148 for (std::size_t j = 1 ; j < pi.states_.size() ; ++j) 00149 { 00150 Vertex v1 = boost::add_vertex(g_); 00151 stateProperty_[v1] = pi.states_[j]; 00152 double weight = si_->distance(pi.states_[j-1], pi.states_[j]); 00153 const HGraph::edge_property_type properties(weight); 00154 boost::add_edge(v0, v1, properties, g_); 00155 length += weight; 00156 pi.vertices_.push_back(v1); 00157 v0 = v1; 00158 } 00159 00160 // connect to virtual goal 00161 boost::add_edge(v0, goal_, prop0, g_); 00162 pi.length_ = length; 00163 00164 // find matches with previously added paths 00165 for (std::set<PathInfo>::const_iterator it = paths_.begin() ; it != paths_.end() ; ++it) 00166 { 00167 const PathGeometric *q = static_cast<const PathGeometric*>(it->path_.get()); 00168 std::vector<int> indexP, indexQ; 00169 matchPaths(*p, *q, (pi.length_ + it->length_) / (2.0 / magic::GAP_COST_FRACTION), indexP, indexQ); 00170 00171 if (matchAcrossGaps) 00172 { 00173 int lastP = -1; 00174 int lastQ = -1; 00175 int gapStartP = -1; 00176 int gapStartQ = -1; 00177 bool gapP = false; 00178 bool gapQ = false; 00179 for (std::size_t i = 0 ; i < indexP.size() ; ++i) 00180 { 00181 // a gap is found in p 00182 if (indexP[i] < 0) 00183 { 00184 // remember this as the beginning of the gap, if needed 00185 if (!gapP) 00186 gapStartP = i; 00187 // mark the fact we are now in a gap on p 00188 gapP = true; 00189 } 00190 else 00191 { 00192 // check if a gap just ended; 00193 // if it did, try to match the endpoint with the elements in q 00194 if (gapP) 00195 for (std::size_t j = gapStartP ; j < i ; ++j) 00196 { 00197 attemptNewEdge(pi, *it, indexP[i], indexQ[j]); 00198 ++nattempts; 00199 } 00200 // remember the last non-negative index in p 00201 lastP = i; 00202 gapP = false; 00203 } 00204 if (indexQ[i] < 0) 00205 { 00206 if (!gapQ) 00207 gapStartQ = i; 00208 gapQ = true; 00209 } 00210 else 00211 { 00212 if (gapQ) 00213 for (std::size_t j = gapStartQ ; j < i ; ++j) 00214 { 00215 attemptNewEdge(pi, *it, indexP[j], indexQ[i]); 00216 ++nattempts; 00217 } 00218 lastQ = i; 00219 gapQ = false; 00220 } 00221 00222 // try to match corresponding index values and gep beginnings 00223 if (lastP >= 0 && lastQ >= 0) 00224 { 00225 attemptNewEdge(pi, *it, indexP[lastP], indexQ[lastQ]); 00226 ++nattempts; 00227 } 00228 } 00229 } 00230 else 00231 { 00232 // attempt new edge only when states align 00233 for (std::size_t i = 0 ; i < indexP.size() ; ++i) 00234 if (indexP[i] >= 0 && indexQ[i] >= 0) 00235 { 00236 attemptNewEdge(pi, *it, indexP[i], indexQ[i]); 00237 ++nattempts; 00238 } 00239 } 00240 } 00241 00242 // remember this path is part of the hybridization 00243 paths_.insert(pi); 00244 return nattempts; 00245 } 00246 00247 void ompl::geometric::PathHybridization::attemptNewEdge(const PathInfo &p, const PathInfo &q, int indexP, int indexQ) 00248 { 00249 if (si_->checkMotion(p.states_[indexP], q.states_[indexQ])) 00250 { 00251 double weight = si_->distance(p.states_[indexP], q.states_[indexQ]); 00252 const HGraph::edge_property_type properties(weight); 00253 boost::add_edge(p.vertices_[indexP], q.vertices_[indexQ], properties, g_); 00254 } 00255 } 00256 00257 std::size_t ompl::geometric::PathHybridization::pathCount() const 00258 { 00259 return paths_.size(); 00260 } 00261 00262 void ompl::geometric::PathHybridization::matchPaths(const PathGeometric &p, const PathGeometric &q, double gapCost, 00263 std::vector<int> &indexP, std::vector<int> &indexQ) const 00264 { 00265 std::vector<std::vector<double> > C(p.getStateCount()); 00266 std::vector<std::vector<char> > T(p.getStateCount()); 00267 00268 for (std::size_t i = 0 ; i < p.getStateCount() ; ++i) 00269 { 00270 C[i].resize(q.getStateCount(), 0.0); 00271 T[i].resize(q.getStateCount(), '\0'); 00272 for (std::size_t j = 0 ; j < q.getStateCount() ; ++j) 00273 { 00274 // as far as I can tell, there is a bug in the algorithm as presented in the paper 00275 // so I am doing things slightly differently ... 00276 double match = si_->distance(p.getState(i), q.getState(j)) + ((i > 0 && j > 0) ? C[i - 1][j - 1] : 0.0); 00277 double up = gapCost + (i > 0 ? C[i - 1][j] : 0.0); 00278 double left = gapCost + (j > 0 ? C[i][j - 1] : 0.0); 00279 if (match <= up && match <= left) 00280 { 00281 C[i][j] = match; 00282 T[i][j] = 'm'; 00283 } 00284 else 00285 if (up <= match && up <= left) 00286 { 00287 C[i][j] = up; 00288 T[i][j] = 'u'; 00289 } 00290 else 00291 { 00292 C[i][j] = left; 00293 T[i][j] = 'l'; 00294 } 00295 } 00296 } 00297 // construct the sequences with gaps (only index positions) 00298 int m = p.getStateCount() - 1; 00299 int n = q.getStateCount() - 1; 00300 00301 indexP.clear(); 00302 indexQ.clear(); 00303 indexP.reserve(std::max(n,m)); 00304 indexQ.reserve(indexP.size()); 00305 00306 while (n >= 0 && m >= 0) 00307 { 00308 if (T[m][n] == 'm') 00309 { 00310 indexP.push_back(m); 00311 indexQ.push_back(n); 00312 --m; --n; 00313 } 00314 else 00315 if (T[m][n] == 'u') 00316 { 00317 indexP.push_back(m); 00318 indexQ.push_back(-1); 00319 --m; 00320 } 00321 else 00322 { 00323 indexP.push_back(-1); 00324 indexQ.push_back(n); 00325 --n; 00326 } 00327 } 00328 while (n >= 0) 00329 { 00330 indexP.push_back(-1); 00331 indexQ.push_back(n); 00332 --n; 00333 } 00334 while (m >= 0) 00335 { 00336 indexP.push_back(m); 00337 indexQ.push_back(-1); 00338 --m; 00339 } 00340 }