Esempio n. 1
0
/*---------------------------------------------------------------
						changeCoordinatesReference
 ---------------------------------------------------------------*/
void  CPose3DPDFGaussianInf::changeCoordinatesReference( const CPose3D &newReferenceBase )
{
	MRPT_START

	CPose3DPDFGaussian a;
	a.copyFrom(*this);
	a.changeCoordinatesReference(newReferenceBase);
	this->copyFrom(a);

	MRPT_END
}
Esempio n. 2
0
/*---------------------------------------------------------------
					copyFrom2D
  ---------------------------------------------------------------*/
CPose3DQuatPDF* CPose3DQuatPDF::createFrom2D(const CPosePDF &o)
{
	MRPT_START

	CPose3DPDFGaussian q;
	q.copyFrom(o);

	return new CPose3DQuatPDFGaussian(q);

	MRPT_END
}
Esempio n. 3
0
/*---------------------------------------------------------------
					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;
}
Esempio n. 4
0
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
}
Esempio n. 5
0
/*---------------------------------------------------------------
				   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);
}