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 }
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Defines