All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator
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), stateProperty_(boost::get(vertex_state_t(), g_))
00051 {
00052     root_ = boost::add_vertex(g_);
00053     stateProperty_[root_] = NULL;
00054     goal_ = boost::add_vertex(g_);
00055     stateProperty_[goal_] = NULL;
00056 }
00057 
00058 ompl::geometric::PathHybridization::~PathHybridization(void)
00059 {
00060 }
00061 
00062 void ompl::geometric::PathHybridization::clear(void)
00063 {
00064     hpath_.reset();
00065     paths_.clear();
00066 
00067     g_.clear();
00068     root_ = boost::add_vertex(g_);
00069     stateProperty_[root_] = NULL;
00070     goal_ = boost::add_vertex(g_);
00071     stateProperty_[goal_] = NULL;
00072 }
00073 
00074 void ompl::geometric::PathHybridization::print(std::ostream &out) const
00075 {
00076     out << "Path hybridization is aware of " << paths_.size() << " paths" << std::endl;
00077     int i = 1;
00078     for (std::set<PathInfo>::const_iterator it = paths_.begin() ; it != paths_.end() ; ++it, ++i)
00079         out << "  path " << i << " of length " << it->length_ << std::endl;
00080     if (hpath_)
00081         out << "Hybridized path of length " << hpath_->length() << std::endl;
00082 }
00083 
00084 void ompl::geometric::PathHybridization::computeHybridPath(void)
00085 {
00086     boost::vector_property_map<Vertex> prev(boost::num_vertices(g_));
00087     boost::dijkstra_shortest_paths(g_, root_, boost::predecessor_map(prev));
00088     if (prev[goal_] != goal_)
00089     {
00090         PathGeometric *h = new PathGeometric(si_);
00091         for (Vertex pos = prev[goal_]; prev[pos] != pos; pos = prev[pos])
00092             h->append(stateProperty_[pos]);
00093         h->reverse();
00094         hpath_.reset(h);
00095     }
00096 }
00097 
00098 const ompl::base::PathPtr& ompl::geometric::PathHybridization::getHybridPath(void) const
00099 {
00100     return hpath_;
00101 }
00102 
00103 unsigned int ompl::geometric::PathHybridization::recordPath(const base::PathPtr &pp, bool matchAcrossGaps)
00104 {
00105     PathGeometric *p = dynamic_cast<PathGeometric*>(pp.get());
00106     if (!p)
00107     {
00108         msg_.error("Path hybridization only works for geometric paths");
00109         return 0;
00110     }
00111 
00112     if (p->getSpaceInformation() != si_)
00113     {
00114         msg_.error("Paths for hybridization must be from the same space information");
00115         return 0;
00116     }
00117 
00118     // skip empty paths
00119     if (p->getStateCount() == 0)
00120         return 0;
00121 
00122     PathInfo pi(pp);
00123 
00124     // if this path was previously included in the hybridization, skip it
00125     if (paths_.find(pi) != paths_.end())
00126         return 0;
00127 
00128     // the number of connection attempts
00129     unsigned int nattempts = 0;
00130 
00131     // start from virtual root
00132     Vertex v0 = boost::add_vertex(g_);
00133     stateProperty_[v0] = pi.states_[0];
00134     pi.vertices_.push_back(v0);
00135 
00136     // add all the vertices of the path, and the edges between them, to the HGraph
00137     // also compute the path length for future use (just for computational savings)
00138     const HGraph::edge_property_type prop0(0.0);
00139     boost::add_edge(root_, v0, prop0, g_);
00140     double length = 0.0;
00141     for (std::size_t j = 1 ; j < pi.states_.size() ; ++j)
00142     {
00143         Vertex v1 = boost::add_vertex(g_);
00144         stateProperty_[v1] = pi.states_[j];
00145         double weight = si_->distance(pi.states_[j-1], pi.states_[j]);
00146         const HGraph::edge_property_type properties(weight);
00147         boost::add_edge(v0, v1, properties, g_);
00148         length += weight;
00149         pi.vertices_.push_back(v1);
00150         v0 = v1;
00151     }
00152 
00153     // connect to virtual goal
00154     boost::add_edge(v0, goal_, prop0, g_);
00155     pi.length_ = length;
00156 
00157     // find matches with previously added paths
00158     for (std::set<PathInfo>::const_iterator it = paths_.begin() ; it != paths_.end() ; ++it)
00159     {
00160         const PathGeometric *q = static_cast<const PathGeometric*>(it->path_.get());
00161         std::vector<int> indexP, indexQ;
00162         matchPaths(*p, *q, (pi.length_ + it->length_) / (2.0 / magic::GAP_COST_FRACTION), indexP, indexQ);
00163 
00164         if (matchAcrossGaps)
00165         {
00166             int lastP = -1;
00167             int lastQ = -1;
00168             int gapStartP = -1;
00169             int gapStartQ = -1;
00170             bool gapP = false;
00171             bool gapQ = false;
00172             for (std::size_t i = 0 ; i < indexP.size() ; ++i)
00173             {
00174                 // a gap is found in p
00175                 if (indexP[i] < 0)
00176                 {
00177                     // remember this as the beginning of the gap, if needed
00178                     if (!gapP)
00179                         gapStartP = i;
00180                     // mark the fact we are now in a gap on p
00181                     gapP = true;
00182                 }
00183                 else
00184                 {
00185                     // check if a gap just ended;
00186                     // if it did, try to match the endpoint with the elements in q
00187                     if (gapP)
00188                         for (std::size_t j = gapStartP ; j < i ; ++j)
00189                         {
00190                             attemptNewEdge(pi, *it, indexP[i], indexQ[j]);
00191                             ++nattempts;
00192                         }
00193                     // remember the last non-negative index in p
00194                     lastP = i;
00195                     gapP = false;
00196                 }
00197                 if (indexQ[i] < 0)
00198                 {
00199                     if (!gapQ)
00200                         gapStartQ = i;
00201                     gapQ = true;
00202                 }
00203                 else
00204                 {
00205                     if (gapQ)
00206                         for (std::size_t j = gapStartQ ; j < i ; ++j)
00207                         {
00208                             attemptNewEdge(pi, *it, indexP[j], indexQ[i]);
00209                             ++nattempts;
00210                         }
00211                     lastQ = i;
00212                     gapQ = false;
00213                 }
00214 
00215                 // try to match corresponding index values and gep beginnings
00216                 if (lastP >= 0 && lastQ >= 0)
00217                 {
00218                     attemptNewEdge(pi, *it, indexP[lastP], indexQ[lastQ]);
00219                     ++nattempts;
00220                 }
00221             }
00222         }
00223         else
00224         {
00225             // attempt new edge only when states align
00226             for (std::size_t i = 0 ; i < indexP.size() ; ++i)
00227                 if (indexP[i] >= 0 && indexQ[i] >= 0)
00228                 {
00229                     attemptNewEdge(pi, *it, indexP[i], indexQ[i]);
00230                     ++nattempts;
00231                 }
00232         }
00233     }
00234 
00235     // remember this path is part of the hybridization
00236     paths_.insert(pi);
00237     return nattempts;
00238 }
00239 
00240 void ompl::geometric::PathHybridization::attemptNewEdge(const PathInfo &p, const PathInfo &q, int indexP, int indexQ)
00241 {
00242     if (si_->checkMotion(p.states_[indexP], q.states_[indexQ]))
00243     {
00244         double weight = si_->distance(p.states_[indexP], q.states_[indexQ]);
00245         const HGraph::edge_property_type properties(weight);
00246         boost::add_edge(p.vertices_[indexP], q.vertices_[indexQ], properties, g_);
00247     }
00248 }
00249 
00250 std::size_t ompl::geometric::PathHybridization::pathCount(void) const
00251 {
00252     return paths_.size();
00253 }
00254 
00255 void ompl::geometric::PathHybridization::matchPaths(const PathGeometric &p, const PathGeometric &q, double gapCost,
00256                                                     std::vector<int> &indexP, std::vector<int> &indexQ) const
00257 {
00258     std::vector<std::vector<double> > C(p.getStateCount());
00259     std::vector<std::vector<char> >   T(p.getStateCount());
00260 
00261     for (std::size_t i = 0 ; i < p.getStateCount() ; ++i)
00262     {
00263         C[i].resize(q.getStateCount(), 0.0);
00264         T[i].resize(q.getStateCount(), '\0');
00265         for (std::size_t j = 0 ; j < q.getStateCount() ; ++j)
00266         {
00267             // as far as I can tell, there is a bug in the algorithm as presented in the paper
00268             // so I am doing things slightly differently ...
00269             double match = si_->distance(p.getState(i), q.getState(j)) + ((i > 0 && j > 0) ? C[i - 1][j - 1] : 0.0);
00270             double up    = gapCost + (i > 0 ? C[i - 1][j] : 0.0);
00271             double left  = gapCost + (j > 0 ? C[i][j - 1] : 0.0);
00272             if (match <= up && match <= left)
00273             {
00274                 C[i][j] = match;
00275                 T[i][j] = 'm';
00276             }
00277             else
00278                 if (up <= match && up <= left)
00279                 {
00280                     C[i][j] = up;
00281                     T[i][j] = 'u';
00282                 }
00283                 else
00284                 {
00285                     C[i][j] = left;
00286                     T[i][j] = 'l';
00287                 }
00288         }
00289     }
00290     // construct the sequences with gaps (only index positions)
00291     int m = p.getStateCount() - 1;
00292     int n = q.getStateCount() - 1;
00293 
00294     indexP.clear();
00295     indexQ.clear();
00296     indexP.reserve(std::max(n,m));
00297     indexQ.reserve(indexP.size());
00298 
00299     while (n >= 0 && m >= 0)
00300     {
00301         if (T[m][n] == 'm')
00302         {
00303             indexP.push_back(m);
00304             indexQ.push_back(n);
00305             --m; --n;
00306         }
00307         else
00308             if (T[m][n] == 'u')
00309             {
00310                 indexP.push_back(m);
00311                 indexQ.push_back(-1);
00312                 --m;
00313             }
00314             else
00315             {
00316                 indexP.push_back(-1);
00317                 indexQ.push_back(n);
00318                 --n;
00319             }
00320     }
00321     while (n >= 0)
00322     {
00323         indexP.push_back(-1);
00324         indexQ.push_back(n);
00325         --n;
00326     }
00327     while (m >= 0)
00328     {
00329         indexP.push_back(m);
00330         indexQ.push_back(-1);
00331         --m;
00332     }
00333 }