/*---------------------------------------------------------------
Destructor
---------------------------------------------------------------*/
CHMHMapArc::~CHMHMapArc()
{
CHMHMapNodePtr node;
// To the nodes:
if ((node = m_parent->getNodeByID(m_nodeFrom)).present())
node->onArcDestruction(this);
if ((node = m_parent->getNodeByID(m_nodeTo)).present())
node->onArcDestruction(this);
// To the graph:
if (m_parent.get())
m_parent->onArcDestruction(this);
}
CHMHMapArcPtr CHMHMapArc::Create(
CHMHMapNodePtr &from,
CHMHMapNodePtr &to,
const THypothesisIDSet &hyps,
CHierarchicalMHMap *parent)
{
CHMHMapArcPtr obj = CHMHMapArcPtr(new CHMHMapArc(from,to,hyps,parent));
if (parent)
{
// To the graph:
parent->onArcAddition(obj);
// To the nodes:
if (from) from->onArcAddition(obj);
if (to) to->onArcAddition(obj);
}
return obj;
}
/*---------------------------------------------------------------
Other constructor
---------------------------------------------------------------*/
CHMHMapArc::CHMHMapArc(
CHMHMapNodePtr &from,
CHMHMapNodePtr &to,
const THypothesisIDSet &hyps,
CHierarchicalMHMap *parent) :
m_hypotheses(hyps),
m_nodeFrom(),
m_nodeTo(),
m_parent(parent),
m_arcType(ARC_TYPES,DEFAULT_ARC_TYPE ),
m_annotations()
{
if (from) m_nodeFrom = from->getID();
if (to) m_nodeTo = to->getID();
// parent will be NULL only inside a ">>" operation, as a temporal
// initialization of an empty object with the default constructor:
}
CHMHMapArcPtr CHMHMapArc::Create(
const CHMHMapNode::TNodeID &from,
const CHMHMapNode::TNodeID &to,
const THypothesisIDSet &hyps,
CHierarchicalMHMap *parent)
{
CHMHMapArcPtr obj = CHMHMapArcPtr(new CHMHMapArc(from,to,hyps,parent));
if (parent)
{
parent->onArcAddition(obj);
CHMHMapNodePtr node;
if ( (node = parent->getNodeByID(from)).present() )
node->onArcAddition(obj);
if ( (node = parent->getNodeByID(to)).present() )
node->onArcAddition(obj);
}
return obj;
}
void hmt_slam_guiFrame::updateLocalMapView()
{
WX_START_TRY
m_glLocalArea->m_openGLScene->clear();
// Get the hypothesis ID:
THypothesisID hypID = (THypothesisID )atoi( cbHypos->GetStringSelection().mb_str() );
if ( m_hmtslam->m_LMHs.find(hypID)==m_hmtslam->m_LMHs.end() )
{
wxMessageBox(_U( format("No LMH has hypothesis ID %i!", (int)hypID).c_str() ), _("Error with topological hypotesis"));
return;
}
// Get the selected area or LMH in the tree view:
wxArrayTreeItemIds lstSelect;
size_t nSel = treeView->GetSelections(lstSelect);
if (!nSel) return;
CItemData *data1 = static_cast<CItemData*>( treeView->GetItemData( lstSelect.Item(0) ) );
if (!data1) return;
if (!data1->m_ptr) return;
CSerializablePtr obj = data1->m_ptr;
if (obj->GetRuntimeClass()==CLASS_ID(CHMHMapNode))
{
// The 3D view:
opengl::CSetOfObjectsPtr objs = opengl::CSetOfObjects::Create();
// -------------------------------------------
// 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
}
// Two passes: 1st draw the map on the ground, then the rest.
for (int nRound=0;nRound<2;nRound++)
{
CHMHMapNodePtr firstArea;
CPose3DPDFGaussian refPoseThisArea;
for (size_t nSelItem = 0; nSelItem<nSel;nSelItem++)
{
CItemData *data1 = static_cast<CItemData*>( treeView->GetItemData( lstSelect.Item(nSelItem) ) );
if (!data1) continue;
if (!data1->m_ptr) continue;
CHMHMapNodePtr area= CHMHMapNodePtr(data1->m_ptr);
if (!area) continue;
// Is this the first rendered area??
if ( !firstArea )
{
firstArea = area;
}
else
{
// Compute the translation btw. ref. and current area:
CPose3DPDFParticles pdf;
m_hmtslam->m_map.computeCoordinatesTransformationBetweenNodes(
firstArea->getID(),
area->getID(),
pdf,
hypID,
200 );
/*0.15f,
DEG2RAD(5.0f) );*/
refPoseThisArea.copyFrom( pdf );
cout << "Pose " << firstArea->getID() << " - " << area->getID() << refPoseThisArea << endl;
}
CMultiMetricMapPtr obj_mmap = area->m_annotations.getAs<CMultiMetricMap>( NODE_ANNOTATION_METRIC_MAPS, hypID, false );
CRobotPosesGraphPtr obj_robposes = area->m_annotations.getAs<CRobotPosesGraph>( NODE_ANNOTATION_POSES_GRAPH, hypID, false );
TPoseID refPoseID;
area->m_annotations.getElemental( NODE_ANNOTATION_REF_POSEID, refPoseID, hypID, true);
// ---------------------------------------------------------
// The metric map:
// ---------------------------------------------------------
if (nRound==0)
{
opengl::CSetOfObjectsPtr objMap= opengl::CSetOfObjects::Create();
obj_mmap->getAs3DObject(objMap);
objMap->setPose( refPoseThisArea.mean );
objs->insert(objMap);
}
if (nRound==1)
{
// ---------------------------------------------------------
// Bounding boxes for grid maps:
// ---------------------------------------------------------
if (obj_mmap->m_gridMaps.size())
{
float x_min = obj_mmap->m_gridMaps[0]->getXMin();
float x_max = obj_mmap->m_gridMaps[0]->getXMax();
float y_min = obj_mmap->m_gridMaps[0]->getYMin();
float y_max = obj_mmap->m_gridMaps[0]->getYMax();
opengl::CSetOfLinesPtr objBB = opengl::CSetOfLines::Create();
objBB->setColor(0,0,1);
objBB->setLineWidth( 4.0f );
objBB->appendLine(x_min,y_min,0, x_max,y_min,0);
objBB->appendLine(x_max,y_min,0, x_max,y_max,0);
objBB->appendLine(x_max,y_max,0, x_min,y_max,0);
objBB->appendLine(x_min,y_max,0, x_min,y_min,0);
objBB->setPose( refPoseThisArea.mean );
objs->insert(objBB);
}
// -----------------------------------------------
// Draw a 3D coordinates corner for the ref. pose
// -----------------------------------------------
{
CPose3D p;
(*obj_robposes)[refPoseID].pdf.getMean(p);
opengl::CSetOfObjectsPtr corner = stock_objects::CornerXYZ();
corner->setPose( refPoseThisArea.mean + p);
corner->setName(format("AREA %i",(int)area->getID() ));
corner->enableShowName();
objs->insert( corner );
}
// -----------------------------------------------
// Draw ellipsoid with uncertainty of pose transformation
// -----------------------------------------------
if (refPoseThisArea.cov(0,0)!=0 || refPoseThisArea.cov(1,1)!=0)
{
opengl::CEllipsoidPtr ellip = opengl::CEllipsoid::Create();
ellip->setPose( refPoseThisArea.mean );
ellip->enableDrawSolid3D(false);
CMatrixDouble C = CMatrixDouble(refPoseThisArea.cov);
if (C(2,2)<1e6)
C.setSize(2,2);
else C.setSize(3,3);
ellip->setCovMatrix(C);
ellip->setQuantiles(3);
ellip->setLocation( ellip->getPoseX(), ellip->getPoseY(), ellip->getPoseZ()+0.5);
ellip->setColor(1,0,0);
ellip->setLineWidth(3);
objs->insert( ellip );
}
// ---------------------------------------------------------
// Draw each of the robot poses as 2D/3D ellipsoids
// ---------------------------------------------------------
for (CRobotPosesGraph::iterator it=obj_robposes->begin();it!=obj_robposes->end();++it)
{
}
}
} // end for nSelItem
} // two pass
// Add to the scene:
m_glLocalArea->m_openGLScene->insert(objs);
}
else
if (obj->GetRuntimeClass()==CLASS_ID( CLocalMetricHypothesis ))
{
//CLocalMetricHypothesis *lmh = static_cast<CLocalMetricHypothesis*>( obj );
}
m_glLocalArea->Refresh();
WX_END_TRY
}
/*---------------------------------------------------------------
CLSLAM_RBPF_2DLASER
Implements a 2D local SLAM method based on a RBPF
over an occupancy grid map. A part of HMT-SLAM.
\param LMH The local metric hypothesis which must be updated by this SLAM algorithm.
\param act The action to process (or NULL).
\param sf The observations to process (or NULL).
WE ALREADY HAVE CONTROL OVER THE CRITICAL SECTION OF THE LMHs!
--------------------------------------------------------------- */
void CLSLAM_RBPF_2DLASER::processOneLMH(
CLocalMetricHypothesis *LMH,
const CActionCollectionPtr &actions,
const CSensoryFramePtr &sf )
{
MRPT_START
// Get the current robot pose estimation:
TPoseID currentPoseID = LMH->m_currentRobotPose;
// If this is the first iteration, just create a new robot pose at the origin:
if (currentPoseID == POSEID_INVALID )
{
currentPoseID = CHMTSLAM::generatePoseID();
LMH->m_currentRobotPose = currentPoseID; // Change it in the LMH
// Create a new robot pose:
CPose3D initPose(0,0,0);
ASSERT_( LMH->m_particles.size()>0 );
for (CLocalMetricHypothesis::CParticleList::iterator it=LMH->m_particles.begin();it!=LMH->m_particles.end();++it)
it->d->robotPoses[ currentPoseID ] = initPose;
ASSERT_( m_parent->m_map.nodeCount()==1 );
m_parent->m_map_cs.enter();
CHMHMapNodePtr firstArea = m_parent->m_map.getFirstNode();
ASSERT_(firstArea);
LMH->m_nodeIDmemberships[currentPoseID] = firstArea->getID();
// Set annotation for the reference pose:
firstArea->m_annotations.setElemental( NODE_ANNOTATION_REF_POSEID, currentPoseID , LMH->m_ID);
m_parent->m_map_cs.leave();
}
bool insertNewRobotPose = false;
if (sf)
{
if ( LMH->m_nodeIDmemberships.size()<2 ) // If there is just 1 node (the current robot pose), then there is no observation in the map yet!
{ // Update map if this is the first observation!
insertNewRobotPose = true;
}
else
{
// Check minimum distance from current robot pose to those in the neighborhood:
map< TPoseID, CPose3D > lstRobotPoses;
LMH->getMeans( lstRobotPoses );
CPose3D *currentRobotPose = & lstRobotPoses[currentPoseID];
float minDistLin = 1e6f;
float minDistAng = 1e6f;
//printf("Poses in graph:\n");
for (map< TPoseID, CPose3D >::iterator it = lstRobotPoses.begin();it!=lstRobotPoses.end();++it)
{
if (it->first != currentPoseID )
{
float linDist = it->second.distanceTo( *currentRobotPose );
float angDist = fabs(math::wrapToPi( it->second.yaw() - currentRobotPose->yaw() ));
minDistLin = min( minDistLin, linDist );
if ( linDist < m_parent->m_options.SLAM_MIN_DIST_BETWEEN_OBS )
minDistAng = min(minDistAng, angDist);
}
}
// time to insert a new node??
insertNewRobotPose = (minDistLin>m_parent->m_options.SLAM_MIN_DIST_BETWEEN_OBS) || ( minDistAng > m_parent->m_options.SLAM_MIN_HEADING_BETWEEN_OBS );
}
} // end if there are SF
// Save data in members so PF callback "prediction_and_update_pfXXXX" have access to them:
m_insertNewRobotPose = insertNewRobotPose;
// ------------------------------------------------
// Execute RBPF method:
// 1) PROCESS ACTION
// 2) PROCESS OBSERVATIONS
// ------------------------------------------------
CParticleFilter pf;
pf.m_options = m_parent->m_options.pf_options;
pf.executeOn( *LMH, actions.pointer(), sf.pointer() );
// 3) The appearance observation: update the log likelihood
// ...
// -----------------------------------------------------------
// 4) UPDATE THE MAP
// -----------------------------------------------------------
if (insertNewRobotPose)
{
m_parent->printf_debug("[CLSLAM_RBPF_2DLASER] Adding new pose...\n");
// Leave the up-to-now "current pose" in the map, insert the SF in it, and...
// ----------------------------------------------------------------------------
TPoseID newCurrentPoseID = CHMTSLAM::generatePoseID();
// ...and create a new "current pose" making a copy of the previous one:
// and insert the observations into the metric maps:
// ----------------------------------------------------------------------------
for (CLocalMetricHypothesis::CParticleList::iterator partIt = LMH->m_particles.begin();
partIt!=LMH->m_particles.end();
partIt++)
{
const CPose3D *curRobotPose = &partIt->d->robotPoses[currentPoseID];
partIt->d->robotPoses[newCurrentPoseID]= *curRobotPose;
sf->insertObservationsInto( &partIt->d->metricMaps, curRobotPose );
}
// Add node membership: for now, a copy of the current pose:
LMH->m_nodeIDmemberships[newCurrentPoseID] = LMH->m_nodeIDmemberships[currentPoseID];
// Now, insert the SF in the just added robot pose:
// -----------------------------------------------------
LMH->m_SFs[ currentPoseID ] = *sf;
// Sets the new poseID as current robot pose:
// ----------------------------------------------------
TPoseID newlyAddedPose = currentPoseID;
currentPoseID = LMH->m_currentRobotPose = newCurrentPoseID;
// Mark the "newlyAddedPose" as pending to reconsider in the graph-cut method
// (Done in the main loop @ LSLAM thread)
// --------------------------------------------------------------------------------
LMH->m_posesPendingAddPartitioner.push_back( newlyAddedPose );
m_parent->printf_debug("[CLSLAM_RBPF_2DLASER] Added pose %i.\n", (int)newlyAddedPose);
// Notice LC detectors:
// ------------------------------
{
synch::CCriticalSectionLocker lock( &m_parent->m_topLCdets_cs );
for (std::deque<CTopLCDetectorBase*>::iterator it=m_parent->m_topLCdets.begin();it!=m_parent->m_topLCdets.end();++it)
(*it)->OnNewPose( newlyAddedPose, sf.pointer() );
}
} // end of insertNewRobotPose
MRPT_END
}
void CHierarchicalMHMap::loadFromXMLfile(std::string fileName)
{
CSimpleDatabase db;
CSimpleDatabaseTablePtr table;
size_t j,numnodes,numarcs;
std::map<size_t,CHMHMapNodePtr> nodemap;
std::map<size_t,CHMHMapNodePtr>::iterator nodemapit;
typedef std::pair<size_t,CHMHMapNodePtr> IDPair;
std::map<size_t,CHMHMapNode::TNodeID> nodeanotmap;
std::map<size_t,CHMHMapNode::TNodeID>::iterator nodeanotmapit;
typedef std::pair<size_t,CHMHMapNode::TNodeID> IDnodeanotPair;
db.loadFromXML(fileName);
table=db.getTable("nodes");
numnodes=table->getRecordCount();
//printf("Loading nodes\n");
std::vector<std::string> node_anots;
for (j=0;j<numnodes;j++)
{
CHMHMapNodePtr node;
node=CHMHMapNode::Create(this);
node->m_label=table->get(j,"nodename");
nodemap.insert(IDPair( atoi(table->get(j,"id").c_str()),node) );
node->m_nodeType.setType(table->get(j,"nodetype"));
node->m_hypotheses.insert( COMMON_TOPOLOG_HYP);
printf("Loaded node %s\n",node->m_label.c_str());
std::deque<std::string> lista;
mrpt::utils::tokenize(table->get(j,"annotation-list")," ",lista);
for (size_t r=0;r<lista.size();r++)
nodeanotmap.insert(IDnodeanotPair((size_t)atoi(lista[r].c_str()),node->getID()));
//A map with key the id of annotations and value the id of nodes;
}
table=db.getTable("arcs");
numarcs=table->getRecordCount();
printf("Loading arcs\n");
for (j=0;j<numarcs;j++)
{
CHMHMapArcPtr arc,arcrev;
size_t from,to;
from=atoi(table->get(j,"from").c_str());
to=atoi(table->get(j,"to").c_str());
CHMHMapNodePtr nodefrom,nodeto;
nodemapit=nodemap.find(from);
nodefrom=nodemapit->second;
std::cout<<"finding nodes"<<std::endl;
nodemapit=nodemap.find(to);
nodeto=nodemapit->second;
std::cout<<"added arc from "<< nodefrom->m_label << " to " <<nodeto->m_label<<std::endl;
arc=CHMHMapArc::Create(nodefrom,nodeto,0,this);
arc->m_arcType.setType(table->get(j,"arctype"));
arc->m_hypotheses.insert( COMMON_TOPOLOG_HYP);
if (atoi(table->get(j,"bidirectional").c_str())==1)
{
printf("Creating bidirectional arc\n");
arcrev=CHMHMapArc::Create(nodeto,nodefrom,0,this);
arcrev->m_arcType.setType(table->get(j,"arctype"));
arcrev->m_hypotheses.insert( COMMON_TOPOLOG_HYP);
}
}
std::cout<<"Graph with ["<<numnodes<<"] nodes and ["<<numarcs<<"] arcs loaded succesfully."<<std::endl;
table=db.getTable("annotations");
size_t numannot=table->getRecordCount();
printf("Loading annotations\n");
for (size_t j=0;j<numannot;j++)
{
string type=table->get(j,"annotation-type");
string value=table->get(j,"annotation-value");
nodeanotmapit =nodeanotmap.find(atoi(table->get(j,"id").c_str()));
if (nodeanotmapit!=nodeanotmap.end())
{
if (type=="placePose")
{
CPoint2DPtr o=CPoint2D::Create();
o->fromString(value);
CHMHMapNodePtr node=getNodeByID(nodeanotmapit->second);
node->m_annotations.set(NODE_ANNOTATION_PLACE_POSE,o,COMMON_TOPOLOG_HYP);
}
}
}
}
/*---------------------------------------------------------------
initializeEmptyMap
---------------------------------------------------------------*/
void CHMTSLAM::initializeEmptyMap()
{
THypothesisIDSet LMH_hyps;
THypothesisID newHypothID = generateHypothesisID();
LMH_hyps.insert( COMMON_TOPOLOG_HYP );
LMH_hyps.insert( newHypothID );
// ------------------------------------
// CLEAR HIERARCHICAL MAP
// ------------------------------------
CHMHMapNode::TNodeID firstAreaID;
{
synch::CCriticalSectionLocker locker( &m_map_cs );
// Initialize hierarchical structures:
// -----------------------------------------------------
m_map.clear();
// Create a single node for the starting area:
CHMHMapNodePtr firstArea = CHMHMapNode::Create( &m_map );
firstAreaID = firstArea->getID();
firstArea->m_hypotheses = LMH_hyps;
CMultiMetricMapPtr emptyMap = CMultiMetricMapPtr(new CMultiMetricMap(&m_options.defaultMapsInitializers) );
firstArea->m_nodeType.setType( "Area" );
firstArea->m_label = generateUniqueAreaLabel();
firstArea->m_annotations.set( NODE_ANNOTATION_METRIC_MAPS, emptyMap, newHypothID );
firstArea->m_annotations.setElemental( NODE_ANNOTATION_REF_POSEID, POSEID_INVALID , newHypothID );
} // end of lock m_map_cs
// ------------------------------------
// CLEAR LIST OF HYPOTHESES
// ------------------------------------
{
synch::CCriticalSectionLocker lock( &m_LMHs_cs );
// Add to the list:
m_LMHs.clear();
CLocalMetricHypothesis &newLMH = m_LMHs[newHypothID];
newLMH.m_parent = this;
newLMH.m_currentRobotPose = POSEID_INVALID ; // Special case: map is empty
newLMH.m_log_w = 0;
newLMH.m_ID = newHypothID;
newLMH.m_neighbors.clear();
newLMH.m_neighbors.insert( firstAreaID );
newLMH.clearRobotPoses();
} // end of cs
// ------------------------------------------
// Create the local SLAM algorithm object
// -----------------------------------------
switch( m_options.SLAM_METHOD )
{
case lsmRBPF_2DLASER:
{
if (m_LSLAM_method) delete m_LSLAM_method;
m_LSLAM_method = new CLSLAM_RBPF_2DLASER(this);
}
break;
default:
THROW_EXCEPTION_CUSTOM_MSG1("Invalid selection for LSLAM method: %i",(int)m_options.SLAM_METHOD );
};
// ------------------------------------
// Topological LC detectors:
// ------------------------------------
{
synch::CCriticalSectionLocker lock( &m_topLCdets_cs );
// Clear old list:
for (std::deque<CTopLCDetectorBase*>::iterator it=m_topLCdets.begin();it!=m_topLCdets.end();++it)
delete *it;
m_topLCdets.clear();
// Create new list:
// 1: Occupancy Grid matching.
// 2: Cummins' image matching.
for (vector_string::const_iterator d=m_options.TLC_detectors.begin();d!=m_options.TLC_detectors.end();++d)
m_topLCdets.push_back( loopClosureDetector_factory(*d) );
}
// ------------------------------------
// Other variables:
// ------------------------------------
m_LSLAM_queue.clear();
// ------------------------------------
// Delete log files:
// ------------------------------------
if (!m_options.LOG_OUTPUT_DIR.empty())
{
mrpt::system::deleteFilesInDirectory( m_options.LOG_OUTPUT_DIR );
mrpt::system::createDirectory( m_options.LOG_OUTPUT_DIR );
mrpt::system::createDirectory( m_options.LOG_OUTPUT_DIR + "/HMAP_txt" );
mrpt::system::createDirectory( m_options.LOG_OUTPUT_DIR + "/HMAP_3D" );
mrpt::system::createDirectory( m_options.LOG_OUTPUT_DIR + "/LSLAM_3D" );
mrpt::system::createDirectory( m_options.LOG_OUTPUT_DIR + "/ASSO" );
mrpt::system::createDirectory( m_options.LOG_OUTPUT_DIR + "/HMTSLAM_state" );
}
}
/*---------------------------------------------------------------
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
}