PathHybridization.h
1 /*********************************************************************
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2011, Willow Garage, Inc.
5 * All rights reserved.
6 *
7 * Redistribution and use in source and binary forms, with or without
8 * modification, are permitted provided that the following conditions
9 * are met:
10 *
11 * * Redistributions of source code must retain the above copyright
12 * notice, this list of conditions and the following disclaimer.
13 * * Redistributions in binary form must reproduce the above
14 * copyright notice, this list of conditions and the following
15 * disclaimer in the documentation and/or other materials provided
16 * with the distribution.
17 * * Neither the name of the Willow Garage nor the names of its
18 * contributors may be used to endorse or promote products derived
19 * from this software without specific prior written permission.
20 *
21 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32 * POSSIBILITY OF SUCH DAMAGE.
33 *********************************************************************/
34 
35 /* Author: Ioan Sucan */
36 
37 #ifndef OMPL_GEOMETRIC_PATH_HYBRIDIZATION_
38 #define OMPL_GEOMETRIC_PATH_HYBRIDIZATION_
39 
40 #include "ompl/base/SpaceInformation.h"
41 #include "ompl/base/OptimizationObjective.h"
42 #include "ompl/geometric/PathGeometric.h"
43 #include <boost/graph/graph_traits.hpp>
44 #include <boost/graph/adjacency_list.hpp>
45 #include <iostream>
46 #include <set>
47 
48 namespace ompl
49 {
50  namespace geometric
51  {
53 
54  OMPL_CLASS_FORWARD(PathHybridization);
56 
70  class PathHybridization
71  {
72  public:
74  PathHybridization(base::SpaceInformationPtr si);
75 
78  PathHybridization(base::SpaceInformationPtr si, base::OptimizationObjectivePtr obj);
79 
81 
83  const geometric::PathGeometricPtr &getHybridPath() const;
84 
86  void computeHybridPath();
87 
91  unsigned int recordPath(const geometric::PathGeometricPtr &pp, bool matchAcrossGaps);
92 
94  std::size_t pathCount() const;
95 
101  void matchPaths(const geometric::PathGeometric &p, const geometric::PathGeometric &q, double gapValue,
102  std::vector<int> &indexP, std::vector<int> &indexQ) const;
103 
105  void clear();
106 
108  void print(std::ostream &out = std::cout) const;
109 
111  const std::string &getName() const;
112 
113  private:
115  struct vertex_state_t
116  {
117  using kind = boost::vertex_property_tag;
118  };
119 
120  using HGraph = boost::adjacency_list<
121  boost::vecS, boost::vecS, boost::undirectedS,
122  boost::property<vertex_state_t, base::State *,
123  boost::property<boost::vertex_predecessor_t, unsigned long int,
124  boost::property<boost::vertex_rank_t, base::Cost>>>,
125  boost::property<boost::edge_weight_t, base::Cost>>;
126 
127  using Vertex = boost::graph_traits<HGraph>::vertex_descriptor;
128  using Edge = boost::graph_traits<HGraph>::edge_descriptor;
129 
130  struct PathInfo
131  {
132  PathInfo(const geometric::PathGeometricPtr &path)
133  : path_(path), states_(path->getStates()), cost_(base::Cost())
134  {
135  vertices_.reserve(states_.size());
136  }
137 
138  bool operator==(const PathInfo &other) const
139  {
140  return path_ == other.path_;
141  }
142 
143  bool operator<(const PathInfo &other) const
144  {
145  return path_ < other.path_;
146  }
147 
148  geometric::PathGeometricPtr path_;
149  const std::vector<base::State *> &states_;
150  base::Cost cost_;
151  std::vector<Vertex> vertices_;
152  };
154 
155  void attemptNewEdge(const PathInfo &p, const PathInfo &q, int indexP, int indexQ);
156 
157  base::SpaceInformationPtr si_;
158  base::OptimizationObjectivePtr obj_;
159  HGraph g_;
160  boost::property_map<HGraph, vertex_state_t>::type stateProperty_;
161  Vertex root_;
162  Vertex goal_;
163  std::set<PathInfo> paths_;
164  geometric::PathGeometricPtr hpath_;
165 
167  std::string name_;
168  };
169  }
170 }
171 
172 #endif
const std::string & getName() const
Get the name of the algorithm.
unsigned int recordPath(const geometric::PathGeometricPtr &pp, bool matchAcrossGaps)
Add a path to the hybridization. If matchAcrossGaps is true, more possible edge connections are evalu...
void print(std::ostream &out=std::cout) const
Print information about the computed path.
void clear()
Clear all the stored paths.
std::size_t pathCount() const
Get the number of paths that are currently considered as part of the hybridization.
const geometric::PathGeometricPtr & getHybridPath() const
Get the currently computed hybrid path. computeHybridPath() needs to have been called before.
void computeHybridPath()
Run Dijkstra's algorithm to find out the lowest-cost path among the mixed ones.
void matchPaths(const geometric::PathGeometric &p, const geometric::PathGeometric &q, double gapValue, 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...
PathHybridization(base::SpaceInformationPtr si)
The constructor needs to know about the space information of the paths it will operate on.
Main namespace. Contains everything in this library.
Definition: AppBase.h:21