std::vector<unsigned int> DynamicProgramming<TLabel, TNode>::Optimize()
{
std::cout << "Nodes: " << this->Nodes.size() << " labels: " << this->LabelSet.size() << std::endl;
if(this->Nodes.size() == 0)
{
throw std::runtime_error("DynamicProgramming: There are 0 nodes!");
}
if(this->LabelSet.size() == 0)
{
throw std::runtime_error("DynamicProgramming: There are 0 labels!");
}
ComputeGrids();
std::vector<Index> path = TracePath();
std::vector<unsigned int> labelIds(path.size());
for(unsigned int i = 0; i < path.size(); ++i)
{
labelIds[i] = path[i][1]; // Extract the label from the index (of (nodes,labels) grid)
}
std::reverse (labelIds.begin( ), labelIds.end( ) );
return labelIds;
}
void QTPFS::PathSearch::Finalize(IPath* path) {
TracePath(path);
#ifdef QTPFS_SMOOTH_PATHS
SmoothPath(path);
#endif
path->SetBoundingBox();
// path remains in live-cache until DeletePath is called
pathCache->AddLivePath(path);
}
/**
* @function FindPath
* @brief This is in the whole free space
*/
std::vector<Eigen::Vector3i> LJM2::FindPath( int _n1, int _n2 ) {
std::vector<Eigen::Vector3i> path;
printf( "--> Start FindPath in whole free space \n" );
int nodeStart = _n1;
int nodeTarget = _n2;
//-- Fill start space
Node3D *node;
node = &mNodes[nodeStart];
node->s.costG = 0;
node->s.costH = CostHeuristic( nodeStart, nodeTarget )*( 0 + mAlpha*1 );
node->s.costF = node->s.costG + node->s.costH;
node->s.parent = -1;
node->s.status = IN_NO_SET;
//-- Push it into the open set
PushOpenSet( nodeStart );
//-- Loop
int x;
int count = 0;
while( count < sMaxIter ) {
count++;
//-- Remove top node in OpenSet
try {
x = PopOpenSet();
} catch( int i ) {
printf( "-- (%d) No more nodes to pop out \n", i );
break;
}
//-- Check if it is goal
if( x == nodeTarget ) {
printf( "--> Found a path! : iters: %d Cost: %.3f \n", count, mNodes[x].s.costF ); TracePath( x, path ); break;
}
//-- Add node to closed set
mNodes[x].s.status = IN_CLOSED_SET;
std::vector<int> neighbors = mGeometricNeighbors[x];
//--
for( int i = 0; i < neighbors.size(); i++ ) {
if( mNodes[ neighbors[i] ].s.status == IN_CLOSED_SET ) {
continue;
}
int y = mNodes[ neighbors[i] ].index; // Same as neighbors[i] actually
float tentative_G_score = mNodes[x].s.costG + EdgeCost( x, y, sNominalValue )*( mNodes[y].s.value + mAlpha*1 );
if( mNodes[y].s.status != IN_OPEN_SET ) {
node = &mNodes[y];
node->s.parent = x;
node->s.costG = tentative_G_score;
node->s.costH = CostHeuristic( y, nodeTarget )*( 0 + mAlpha*1 );
node->s.costF = node->s.costG + node->s.costH;
PushOpenSet(y);
}
else {
if( tentative_G_score < mNodes[y].s.costG ) {
node = &mNodes[y];
node->s.parent = x;
node->s.costG = tentative_G_score;
node->s.costH = CostHeuristic( y, nodeTarget )*( 0 + mAlpha*1 );
node->s.costF = node->s.costG + node->s.costH;
//-- Reorder your OpenSet
UpdateLowerOpenSet( y );
}
}
} //-- End for every neighbor
} //-- End of while
node = NULL;
printf("Finished FindPath whole free space \n");
return path;
}
