PathHybridization.cpp
64 ompl::geometric::PathHybridization::PathHybridization(base::SpaceInformationPtr si, base::OptimizationObjectivePtr obj)
112 g_, root_,
114 .distance_compare([this](base::Cost c1, base::Cost c2) { return obj_->isCostBetterThan(c1, c2); })
132 const ompl::geometric::PathGeometricPtr &ompl::geometric::PathHybridization::getHybridPath() const
137 unsigned int ompl::geometric::PathHybridization::recordPath(const geometric::PathGeometricPtr &p, bool matchAcrossGaps)
190 matchPaths(*p, *q, obj_->combineCosts(pi.cost_, path.cost_).value() / (2.0 / magic::GAP_COST_FRACTION), indexP,
269 void ompl::geometric::PathHybridization::attemptNewEdge(const PathInfo &p, const PathInfo &q, int indexP, int indexQ)
284 void ompl::geometric::PathHybridization::matchPaths(const PathGeometric &p, const PathGeometric &q, double gapValue,
289 CostMatrix C = CostMatrix::Constant(p.getStateCount(), q.getStateCount(), obj_->identityCost());
const std::string & getName() const
Get the name of the algorithm.
Definition: PathHybridization.cpp:103
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...
Definition: PathHybridization.cpp:137
void print(std::ostream &out=std::cout) const
Print information about the computed path.
Definition: PathHybridization.cpp:93
std::size_t pathCount() const
Get the number of paths that are currently considered as part of the hybridization.
Definition: PathHybridization.cpp:279
const geometric::PathGeometricPtr & getHybridPath() const
Get the currently computed hybrid path. computeHybridPath() needs to have been called before.
Definition: PathHybridization.cpp:132
Definition of a cost value. Can represent the cost of a motion or the cost of a state.
Definition: Cost.h:111
static const double GAP_COST_FRACTION
The fraction of the path length to consider as gap cost when aligning paths to be hybridized.
Definition: PathHybridization.cpp:112
void computeHybridPath()
Run Dijkstra's algorithm to find out the lowest-cost path among the mixed ones.
Definition: PathHybridization.cpp:108
std::size_t getStateCount() const
Get the number of states (way-points) that make up this path.
Definition: PathGeometric.h:292
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...
Definition: PathHybridization.cpp:284
PathHybridization(base::SpaceInformationPtr si)
The constructor needs to know about the space information of the paths it will operate on.
Definition: PathHybridization.cpp:52
base::State * getState(unsigned int index)
Get the state located at index along the path.
Definition: PathGeometric.h:280