/*---------------------------------------------------------------
clear
---------------------------------------------------------------*/
void CHierarchicalMHMap::clear()
{
// Remaining arcs and nodes will be deleted now
// A delicate issue; we must:
// 1) .clear() all the smart pointers
// 2) then, empty the list of nodes/arcs, which will only
// contain empty smart pointers, thus will not raise callbacks again.
// ----------------------------------------------------------------
TNodeList nodes = m_nodes;
TArcList arcs = m_arcs;
// 1:
std::for_each(nodes.begin(),nodes.end(), metaprogramming::ObjectClearSecond() );
std::for_each(arcs.begin(),arcs.end(), metaprogramming::ObjectClear2() );
// 2:
m_nodes.clear();
m_arcs.clear();
}
/*---------------------------------------------------------------
perform_TLC
Topological Loop Closure: Performs all the required operations
to close a loop between two areas which have been determined
to be the same.
---------------------------------------------------------------*/
void CHMTSLAM::perform_TLC(
CLocalMetricHypothesis &LMH,
const CHMHMapNode::TNodeID Ai, // areaInLMH,
const CHMHMapNode::TNodeID Ae, // areaLoopClosure,
const mrpt::poses::CPose3DPDFGaussian &pose_i_wrt_e
)
{
MRPT_START
ASSERT_(Ai!=Ae)
synch::CCriticalSectionLocker locker ( &LMH.m_robotPosesGraph.lock );
printf_debug("[perform_TLC] TLC of areas: %u <-> %u \n",(unsigned)Ai, (unsigned)Ae );
// * Verify a1 \in LMH & a2 \notin LMH
// ----------------------------------------------------------------------
ASSERT_( LMH.m_neighbors.count(Ai) == 1);
ASSERT_( LMH.m_neighbors.count(Ae) == 0);
// * Get the old pose reference of the area which will desapear:
// ----------------------------------------------------------------------
TPoseID poseID_Ai_origin;
m_map.getNodeByID(Ai)->m_annotations.getElemental( NODE_ANNOTATION_REF_POSEID,poseID_Ai_origin, LMH.m_ID, true);
// TPoseID poseID_new_origin;
// m_map.getNodeByID(Ae)->m_annotations.getElemental( NODE_ANNOTATION_REF_POSEID,poseID_new_origin, LMH.m_ID, true);
// * Make a list Li of pose IDs in Ai, which need a change in coordinates:
// ----------------------------------------------------------------------------------------------
//TPoseIDSet Li;
//for (map<TPoseID,CHMHMapNode::TNodeID>::const_iterator it=LMH.m_nodeIDmemberships.begin();it!=LMH.m_nodeIDmemberships.end();++it)
// if(it->second==Ai)
// Li.insert(it->first);
// * Change all poses in "Ai" from "p" to " 0 (-) pose1wrt2 (+) (p (-) oldRef )"
// ----------------------------------------------------------------------------------------------
// For all the particles
//for ( CLocalMetricHypothesis::CParticleList::const_iterator it = LMH.m_particles.begin(); it!=LMH.m_particles.end(); ++it)
//{
// // Touch only the affected poseIDs:
// for (TPoseIDSet::const_iterator n=Li.begin();n!=Li.end();++n)
// {
// map<TPoseID,CPose3D>::iterator itPos = it->d->robotPoses.find(*n);
// ASSERT_(itPos!=it->d->robotPoses.end());
// itPos->second = Apose_ei + (itPos->second - oldRef);
// }
//}
// Change coords in incr. partitioning as well:
//CSimpleMap *SFseq = LMH.m_robotPosesGraph.partitioner.getSequenceOfFrames();
//for (std::map<uint32_t,TPoseID>::const_iterator it=LMH.m_robotPosesGraph.idx2pose.begin();it!=LMH.m_robotPosesGraph.idx2pose.end();++it)
//{
// // Only if the pose has been affected:
// if (Li.count(it->second)==0) continue;
// CPose3DPDFPtr pdf;
// CSensoryFramePtr sf;
// SFseq->get( it->first, pdf, sf);
// // Copy from particles:
// CPose3DPDFParticlesPtr pdfParts = CPose3DPDFParticlesPtr(pdf);
// LMH.getPoseParticles( it->second, *pdfParts);
//}
// * Mark all poses in LMH \in Ai as being of "Ae":
// ----------------------------------------------------------------------------------------------
for (map<TPoseID,CHMHMapNode::TNodeID>::iterator it=LMH.m_nodeIDmemberships.begin();it!=LMH.m_nodeIDmemberships.end();++it)
if(it->second==Ai)
it->second=Ae;
// * Replace "Ai" by "Ae" in the list of neighbors areas:
// ----------------------------------------------------------------------------------------------
LMH.m_neighbors.erase(LMH.m_neighbors.find(Ai));
LMH.m_neighbors.insert(Ae);
// * Reflect the change of coordinates in all existing arcs from/to "Ai", which are not part of the LMH (list of neighbors)
// "p" to " 0 (-) pose1wrt2 (+) (p (-) oldRef )"...
// NOTE: Arcs to "Ai" must be replaced by arcs to "Ae"
// ----------------------------------------------------------------------------------------------
{
TArcList lstArcs;
m_map.getNodeByID(Ai)->getArcs(lstArcs,LMH.m_ID);
for(TArcList::iterator arc=lstArcs.begin();arc!=lstArcs.end();++arc)
{
// TODO ...
}
}
// * The node for "area Ai" must be deleted in the HMAP, and all its arcs.
// ----------------------------------------------------------------------------------------------
{
TArcList lstArcs;
m_map.getNodeByID(Ai)->getArcs(lstArcs);
for(TArcList::iterator arc=lstArcs.begin();arc!=lstArcs.end();++arc)
arc->clear(); // The "delete" will automatically remove the entry in "m_map". Other smrtpnts will be cleared too.
m_map.getNodeByID(Ai).clear(); // The "delete" will automatically remove the entry in "m_map"
}
// * Merge Ae into the LMH: This includes the change of coordinates to those used now in "Ai" and the rest of neighbors!
// ----------------------------------------------------------------------------------------------
CPose3DPDFParticles pdfAiRef;
LMH.getPoseParticles(poseID_Ai_origin,pdfAiRef);
CPose3D AiRef;
pdfAiRef.getMean(AiRef);
const CPose3D &Apose_ie = pose_i_wrt_e.mean;
//const CPose3D Apose_ei = -Apose_ie;
const CPose3D AeRefInLMH = AiRef + Apose_ie;
CRobotPosesGraphPtr AePG = m_map.getNodeByID(Ae)->m_annotations.getAs<CRobotPosesGraph>(NODE_ANNOTATION_POSES_GRAPH, LMH.m_ID);
// For each pose in Ae:
for (CRobotPosesGraph::const_iterator it=AePG->begin();it!=AePG->end();++it)
{
const TPoseID poseId = it->first;
const TPoseInfo &pi = it->second;
// Insert the observations into the metric maps:
ASSERT_(LMH.m_particles.size() == pi.pdf.size() );
for (size_t i=0;i<pi.pdf.size();i++)
{
// Transport coordinates:
CPose3D &p = *pi.pdf.m_particles[i].d;
LMH.m_particles[i].d->robotPoses[poseId] = AeRefInLMH + p;
//pi.sf.insertObservationsInto( &LMH.m_particles[i].d->metricMaps, pi.pdf.m_particles[i].d );
}
// Add node membership:
LMH.m_nodeIDmemberships[poseId] = Ae;
// Now, insert the SF
LMH.m_SFs[poseId] = pi.sf;
// Also add to the list of poses pending to be evaluated by the AA:
LMH.m_posesPendingAddPartitioner.push_back(poseId);
}
// * The current pose of the robot should be in the new coordinate system:
// ----------------------------------------------------------------------------------------------
// This is already done since the current pose is just another poseID and it's threaded like the others.
// * Rebuild metric maps for consistency:
// ----------------------------------------------------------------------------------------------
LMH.rebuildMetricMaps();
// * Update all the data in the node "Ae":
// ----------------------------------------------------------------------------------------------
LMH.updateAreaFromLMH(Ae);
MRPT_END
}
/*---------------------------------------------------------------
getAs3DScene
Returns a 3D representation of the current robot pose, all
the poses in the auxiliary graph, and each of the areas
they belong to.
---------------------------------------------------------------*/
void CLocalMetricHypothesis::getAs3DScene( opengl::CSetOfObjectsPtr &objs ) const
{
objs->clear();
// -------------------------------------------
// Draw a grid on the ground:
// -------------------------------------------
{
opengl::CGridPlaneXYPtr obj = opengl::CGridPlaneXY::Create(-100,100,-100,100,0,5);
obj->setColor(0.4,0.4,0.4);
objs->insert(obj); // it will free the memory
}
// ---------------------------------------------------------
// Draw each of the robot poses as 2D/3D ellipsoids
// For the current pose, draw the robot itself as well.
// ---------------------------------------------------------
std::map< TPoseID, CPose3DPDFParticles > lstPoses;
std::map< TPoseID, CPose3DPDFParticles >::iterator it;
getPathParticles( lstPoses );
// -----------------------------------------------
// Draw a 3D coordinates corner for each cluster
// -----------------------------------------------
{
CCriticalSectionLocker locker( &m_parent->m_map_cs );
for ( TNodeIDSet::const_iterator n=m_neighbors.begin();n!=m_neighbors.end();++n)
{
const CHMHMapNodePtr node = m_parent->m_map.getNodeByID( *n );
ASSERT_(node);
TPoseID poseID_origin;
CPose3D originPose;
if (node->m_annotations.getElemental( NODE_ANNOTATION_REF_POSEID,poseID_origin, m_ID))
{
lstPoses[ poseID_origin ].getMean(originPose);
opengl::CSetOfObjectsPtr corner = stock_objects::CornerXYZ();
corner->setPose(originPose);
objs->insert( corner );
}
}
} // end of lock on map_cs
// Add a camera following the robot:
// -----------------------------------------
const CPose3D meanCurPose = lstPoses[ m_currentRobotPose ].getMeanVal();
{
opengl::CCameraPtr cam = opengl::CCamera::Create();
cam->setZoomDistance(85);
cam->setAzimuthDegrees(45 + RAD2DEG(meanCurPose.yaw()));
cam->setElevationDegrees(45);
cam->setPointingAt(meanCurPose);
objs->insert(cam);
}
map<CHMHMapNode::TNodeID,CPoint3D> areas_mean; // Store the mean pose of each area
map<CHMHMapNode::TNodeID,int> areas_howmany;
for (it=lstPoses.begin(); it!=lstPoses.end();it++)
{
opengl::CEllipsoidPtr ellip = opengl::CEllipsoid::Create();
// Color depending on being into the current area:
if ( m_nodeIDmemberships.find(it->first)->second == m_nodeIDmemberships.find(m_currentRobotPose)->second )
ellip->setColor(0,0,1);
else
ellip->setColor(1,0,0);
const CPose3DPDFParticles *pdfParts = &it->second;
CPose3DPDFGaussian pdf;
pdf.copyFrom( *pdfParts );
// Mean:
ellip->setLocation(pdf.mean.x(), pdf.mean.y(), pdf.mean.z()+0.005); // To be above the ground gridmap...
// Cov:
CMatrixDouble C = CMatrixDouble(pdf.cov); // 6x6 cov.
C.setSize(3,3);
// Is a 2D pose??
if ( pdf.mean.pitch() == 0 && pdf.mean.roll() == 0 && pdf.cov(2,2) < 1e-4f )
C.setSize(2,2);
ellip->setCovMatrix(C);
ellip->enableDrawSolid3D(false);
// Name:
ellip->setName( format("P%u", (unsigned int) it->first ) );
ellip->enableShowName();
// Add it:
objs->insert( ellip );
// Add an arrow for the mean direction also:
{
mrpt::opengl::CArrowPtr obj = mrpt::opengl::CArrow::Create(
0,0,0,
0.20,0,0,
0.25f,0.005f,0.02f);
obj->setColor(1,0,0);
obj->setPose(pdf.mean);
objs->insert( obj );
}
// Is this the current robot pose?
if (it->first == m_currentRobotPose )
{
// Draw robot:
opengl::CSetOfObjectsPtr robot = stock_objects::RobotPioneer();
robot->setPose(pdf.mean);
// Add it:
objs->insert( robot );
}
else // The current robot pose does not count
{
// Compute the area means:
std::map<TPoseID,CHMHMapNode::TNodeID>::const_iterator itAreaId = m_nodeIDmemberships.find( it->first );
ASSERT_( itAreaId != m_nodeIDmemberships.end() );
CHMHMapNode::TNodeID areaId = itAreaId->second;
areas_howmany[ areaId ]++;
areas_mean[ areaId ].x_incr( pdf.mean.x() );
areas_mean[ areaId ].y_incr( pdf.mean.y() );
areas_mean[ areaId ].z_incr( pdf.mean.z() );
}
} // end for it
// Average area means:
map<CHMHMapNode::TNodeID,CPoint3D>::iterator itMeans;
map<CHMHMapNode::TNodeID,int>::iterator itHowMany;
ASSERT_( areas_howmany.size() == areas_mean.size() );
for ( itMeans = areas_mean.begin(), itHowMany = areas_howmany.begin(); itMeans!=areas_mean.end(); itMeans++, itHowMany++ )
{
ASSERT_( itHowMany->second >0);
float f = 1.0f / itHowMany->second;
itMeans->second *= f;
}
// -------------------------------------------------------------------
// Draw lines between robot poses & their corresponding area sphere
// -------------------------------------------------------------------
for (it=lstPoses.begin(); it!=lstPoses.end();it++)
{
if (it->first != m_currentRobotPose )
{
CPoint3D areaPnt( areas_mean[ m_nodeIDmemberships.find( it->first )->second ] );
areaPnt.z_incr( m_parent->m_options.VIEW3D_AREA_SPHERES_HEIGHT );
const CPose3DPDFParticles *pdfParts = &it->second;
CPose3DPDFGaussian pdf;
pdf.copyFrom( *pdfParts );
opengl::CSimpleLinePtr line = opengl::CSimpleLine::Create();
line->setColor(0.8,0.8,0.8, 0.3);
line->setLineWidth(2);
line->setLineCoords(
pdf.mean.x(), pdf.mean.y(), pdf.mean.z(),
areaPnt.x(), areaPnt.y(), areaPnt.z() );
objs->insert( line );
}
} // end for it
// -------------------------------------------------------------------
// Draw lines for links between robot poses
// -------------------------------------------------------------------
// for (it=m_robotPoses.begin(); it!=m_robotPoses.end();it++)
/* for (it=lstPoses.begin(); it!=lstPoses.end();it++)
{
const CPose3DPDFParticles *myPdfParts = &it->second;
CPose3DPDFGaussian myPdf;
myPdf.copyFrom( *myPdfParts );
std::map<TPoseID,TInterRobotPosesInfo>::const_iterator itLink;
for (itLink=it->second.m_links.begin();itLink!=it->second.m_links.end();itLink++)
{
if (itLink->second.SSO>0.7)
{
CRobotPosesAuxiliaryGraph::const_iterator hisIt = m_robotPoses.find( itLink->first );
ASSERT_( hisIt !=m_robotPoses.end() );
const CPose3DPDFGaussian *hisPdf = & hisIt->second.m_pose;
opengl::CSimpleLinePtr line = opengl::CSimpleLine::Create();
line->m_color_R = 0.2f;
line->m_color_G = 0.8f;
line->m_color_B = 0.2f;
line->m_color_A = 0.3f;
line->m_lineWidth = 3.0f;
line->m_x0 = myPdf->mean.x;
line->m_y0 = myPdf->mean.y;
line->m_z0 = myPdf->mean.z;
line->m_x1 = hisPdf->mean.x;
line->m_y1 = hisPdf->mean.y;
line->m_z1 = hisPdf->mean.z;
objs->insert( line );
}
}
} // end for it
*/
// ---------------------------------------------------------
// Draw each of the areas in the neighborhood:
// ---------------------------------------------------------
{
CCriticalSectionLocker locker( &m_parent->m_map_cs ); //To access nodes' labels.
for ( itMeans = areas_mean.begin(); itMeans!=areas_mean.end(); itMeans++ )
{
opengl::CSpherePtr sphere = opengl::CSphere::Create();
if (itMeans->first == m_nodeIDmemberships.find( m_currentRobotPose)->second )
{ // Color of current area
sphere->setColor(0.1,0.1,0.7);
}
else
{ // Color of other areas
sphere->setColor(0.7,0,0);
}
sphere->setLocation(itMeans->second.x(), itMeans->second.y(), itMeans->second.z() + m_parent->m_options.VIEW3D_AREA_SPHERES_HEIGHT);
sphere->setRadius( m_parent->m_options.VIEW3D_AREA_SPHERES_RADIUS );
// Add it:
objs->insert( sphere );
// And text label:
opengl::CTextPtr txt = opengl::CText::Create();
txt->setColor(1,1,1);
const CHMHMapNodePtr node = m_parent->m_map.getNodeByID( itMeans->first );
ASSERT_(node);
txt->setLocation( itMeans->second.x(), itMeans->second.y(), itMeans->second.z() + m_parent->m_options.VIEW3D_AREA_SPHERES_HEIGHT );
// txt->m_str = node->m_label;
txt->setString( format("%li",(long int)node->getID()) );
objs->insert( txt );
}
} // end of lock on map_cs
// ---------------------------------------------------------
// Draw links between areas:
// ---------------------------------------------------------
{
CCriticalSectionLocker locker( &m_parent->m_map_cs );
for ( itMeans = areas_mean.begin(); itMeans!=areas_mean.end(); itMeans++ )
{
CHMHMapNode::TNodeID srcAreaID = itMeans->first;
const CHMHMapNodePtr srcArea = m_parent->m_map.getNodeByID( srcAreaID );
ASSERT_(srcArea);
TArcList lstArcs;
srcArea->getArcs(lstArcs);
for (TArcList::const_iterator a=lstArcs.begin();a!=lstArcs.end();++a)
{
// target is in the neighborhood of LMH:
if ( (*a)->getNodeFrom() == srcAreaID )
{
map<CHMHMapNode::TNodeID,CPoint3D>::const_iterator trgAreaPoseIt = areas_mean.find( (*a)->getNodeTo() );
if ( trgAreaPoseIt != areas_mean.end() )
{
// Yes, target node of the arc is in the LMH: Draw it:
opengl::CSimpleLinePtr line = opengl::CSimpleLine::Create();
line->setColor(0.8,0.8,0);
line->setLineWidth(3);
line->setLineCoords(
areas_mean[srcAreaID].x(), areas_mean[srcAreaID].y(), areas_mean[srcAreaID].z() + m_parent->m_options.VIEW3D_AREA_SPHERES_HEIGHT,
trgAreaPoseIt->second.x(), trgAreaPoseIt->second.y(), trgAreaPoseIt->second.z() + m_parent->m_options.VIEW3D_AREA_SPHERES_HEIGHT );
objs->insert( line );
}
}
} // end for each arc
} // end for each area
} // end of lock on map_cs
}