/*---------------------------------------------------------------
changeCoordinatesReference
---------------------------------------------------------------*/
void CPose3DPDFGaussianInf::changeCoordinatesReference( const CPose3D &newReferenceBase )
{
MRPT_START
CPose3DPDFGaussian a;
a.copyFrom(*this);
a.changeCoordinatesReference(newReferenceBase);
this->copyFrom(a);
MRPT_END
}
/*---------------------------------------------------------------
copyFrom2D
---------------------------------------------------------------*/
CPose3DQuatPDF* CPose3DQuatPDF::createFrom2D(const CPosePDF &o)
{
MRPT_START
CPose3DPDFGaussian q;
q.copyFrom(o);
return new CPose3DQuatPDFGaussian(q);
MRPT_END
}
/*---------------------------------------------------------------
getFirstMovementEstimation
---------------------------------------------------------------*/
bool CActionCollection::getFirstMovementEstimation( CPose3DPDFGaussian &out_pose_increment ) const
{
CActionRobotMovement3DPtr act3D = getActionByClass<CActionRobotMovement3D>();
if (act3D)
{
out_pose_increment = act3D->poseChange;
return true;
}
CActionRobotMovement2DPtr act2D = getActionByClass<CActionRobotMovement2D>();
if (act2D)
{
out_pose_increment.copyFrom( *act2D->poseChange );
return true;
}
return false;
}
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
}
/*---------------------------------------------------------------
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
}
/*---------------------------------------------------------------
operator =
---------------------------------------------------------------*/
void CPose3DQuatPDFGaussian::copyFrom(const CPosePDF &o)
{
CPose3DPDFGaussian aux;
aux.copyFrom(o);
this->copyFrom(aux);
}