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; }