/*---------------------------------------------------------------
						changeCoordinateOrigin
 ---------------------------------------------------------------*/
void CLocalMetricHypothesis::changeCoordinateOrigin( const TPoseID &newOrigin )
{
	CPose3DPDFParticles		originPDF( m_particles.size() );

	CParticleList::const_iterator it;
	CPose3DPDFParticles::CParticleList::iterator	itOrgPDF;

	for ( it = m_particles.begin(), itOrgPDF=originPDF.m_particles.begin(); it!=m_particles.end(); it++, itOrgPDF++ )
	{
		TMapPoseID2Pose3D::iterator  refPoseIt =  it->d->robotPoses.find( newOrigin );
		ASSERT_( refPoseIt != it->d->robotPoses.end() )
		const CPose3D  &refPose = refPoseIt->second;

		// Save in pdf to compute mean:
		*itOrgPDF->d = refPose;
		itOrgPDF->log_w = it->log_w;

		TMapPoseID2Pose3D::iterator   End = it->d->robotPoses.end();
		// Change all other poses first:
		for (TMapPoseID2Pose3D::iterator  itP=it->d->robotPoses.begin();itP!=End;++itP)
			if (itP!=refPoseIt)
				itP->second = itP->second - refPose;

		// Now set new origin to 0:
		refPoseIt->second.setFromValues(0,0,0);
	}

	// Rebuild metric maps for consistency:
	rebuildMetricMaps();

	// Change coords in incr. partitioning as well:
	{
		synch::CCriticalSectionLocker locker ( &m_robotPosesGraph.lock );

		CSimpleMap *SFseq = m_robotPosesGraph.partitioner.getSequenceOfFrames();
		for (std::map<uint32_t,TPoseID>::const_iterator it=m_robotPosesGraph.idx2pose.begin();it!=m_robotPosesGraph.idx2pose.end();++it)
		{
			CPose3DPDFPtr		pdf;
			CSensoryFramePtr	sf;
			SFseq->get( it->first, pdf, sf);

			// Copy from particles:
			ASSERT_( pdf->GetRuntimeClass() == CLASS_ID(CPose3DPDFParticles) );
			CPose3DPDFParticlesPtr pdfParts = CPose3DPDFParticlesPtr(pdf);
			getPoseParticles( it->second, *pdfParts);
		}
	}
}
Beispiel #2
0
/*---------------------------------------------------------------
				updateSensoryFrameSequence
  ---------------------------------------------------------------*/
void  CMultiMetricMapPDF::updateSensoryFrameSequence()
{
	MRPT_START
	CPose3DPDFParticles	posePartsPDF;
	CPose3DPDFPtr		previousPosePDF;
	CSensoryFramePtr	dummy;

	for (size_t i=0;i<SFs.size();i++)
	{
		// Get last estimation:
		SFs.get(i,previousPosePDF,dummy);

		// Compute the new one:
		getEstimatedPosePDFAtTime(SF2robotPath[i], posePartsPDF);

		// Copy into SFs:
		previousPosePDF->copyFrom( posePartsPDF );
	}

	MRPT_END
}
Beispiel #3
0
/*---------------------------------------------------------------

						TBI_main_method

	Topological Bayesian Inference (TBI) process within HMT-SLAM

  ---------------------------------------------------------------*/
CHMTSLAM::TMessageLSLAMfromTBIPtr CHMTSLAM::TBI_main_method(
	CLocalMetricHypothesis	*LMH,
	const CHMHMapNode::TNodeID &areaID)
{
	MRPT_START

	CHMTSLAM	*obj = (CHMTSLAM*) LMH->m_parent.get();

	const THypothesisID		LMH_ID = LMH->m_ID;

	// Lock the map:
	synch::CCriticalSectionLocker( &obj->m_map_cs );

	TMessageLSLAMfromTBIPtr msg = TMessageLSLAMfromTBIPtr(new TMessageLSLAMfromTBI());

	// Fill out easy data:
	msg->hypothesisID = LMH_ID;
	msg->cur_area = areaID;

	// get a pointer to the current area:
	const CHMHMapNodePtr currentArea = obj->m_map.getNodeByID( areaID );
	ASSERT_(currentArea);

	obj->logFmt(mrpt::utils::LVL_DEBUG, "[TBI] Request for area id=%i\n",(int)areaID);

	// --------------------------------------------------------
	// 1) Use bounding-boxes to get a first list of candidates
	//    The candidates are saved in "msg->loopClosureData"
	// -------------------------------------------------------
	// But first: if the areas are within the LMH, then we have to update the maps in the HMAP!
	if (LMH->m_neighbors.find( areaID ) != LMH->m_neighbors.end() )
	{
		// Update:
		LMH->updateAreaFromLMH( areaID );
	}

	for (CHierarchicalMapMHPartition::iterator a=obj->m_map.begin();a!=obj->m_map.end();++a)
	{
		// Only for other areas!
		if (a->first==areaID) continue;

		// Test hypothesis: LMH_ID
		if (a->second->m_hypotheses.has(LMH_ID))
		{
			// Not neighbors:
			if (a->second->isNeighbor( areaID, LMH_ID) )
				continue;

			// OK, check:
			// But first: if the areas are within the LMH, then we have to update the maps in the HMAP!
			if (LMH->m_neighbors.find( a->first ) != LMH->m_neighbors.end() )
			{
				// Update:
				LMH->updateAreaFromLMH( a->first );
			}

			// Compute it:
			double match = obj->m_map.computeOverlapProbabilityBetweenNodes(
					areaID,		// From
					a->first,	// To
					LMH_ID );

			obj->logFmt(mrpt::utils::LVL_DEBUG, "[TBI] %i-%i -> overlap prob=%f\n",(int)areaID,(int)a->first,match);

			if (match>0.9)
			{
				// Initialize the new entry in "msg->loopClosureData" for the areas:
				//  "areaID" <-> "a->first"
				TMessageLSLAMfromTBI::TBI_info	&tbi_info = msg->loopClosureData[ a->first ];

				tbi_info.log_lik = 0;
				tbi_info.delta_new_cur.clear();
			}
		}
	} // end for each node in the graph.


	// ----------------------------------------------------
	// 2) Use the TBI engines
	// ----------------------------------------------------
	std::set<CHMHMapNode::TNodeID>  lstNodesToErase;
	{
		synch::CCriticalSection lock( obj->m_topLCdets_cs );

		for ( deque<CTopLCDetectorBase*>::const_iterator it=obj->m_topLCdets.begin();it!=obj->m_topLCdets.end();++it)
		{
			for (map< CHMHMapNode::TNodeID, TMessageLSLAMfromTBI::TBI_info >::iterator candidate = msg->loopClosureData.begin();candidate != msg->loopClosureData.end();++candidate)
			{
				// If the current log_lik of this area is reaaaally low, we could skip the computation with other LC detectors...
				// ----------------------------------------------------------------------------------------------------------------
				// TODO: ...

				// Proceed:
				// ----------------------------------------------------------------------------------------------------------------
				const CHMHMapNodePtr refArea = obj->m_map.getNodeByID( candidate->first );
				double this_log_lik;

				// get the output from this LC detector:
				CPose3DPDFPtr pdf = (*it)->computeTopologicalObservationModel(
					LMH->m_ID,
					currentArea,
					refArea,
					this_log_lik );

				// Add to the output:
				candidate->second.log_lik += this_log_lik;

				// This is because not all LC detector MUST return a pose PDF (i.e. image-based detectors)
				if (pdf.present())
				{
					ASSERT_( IS_CLASS(pdf, CPose3DPDFSOG ) );
					CPose3DPDFSOGPtr SOG = CPose3DPDFSOGPtr( pdf );

					// Mix (append) the modes, if any:
					if (SOG->size()>0)
						candidate->second.delta_new_cur.appendFrom( *SOG );
					else
						lstNodesToErase.insert(candidate->first);
				}
			} // end for each candidate area
		} // end for each LC detector

	} // end of m_topLCdets_cs lock

	// Delete candidates which had no PDF when they should.
	for (set<CHMHMapNode::TNodeID>::const_iterator it=lstNodesToErase.begin();it!=lstNodesToErase.end();++it)
		msg->loopClosureData.erase(*it);

	obj->logFmt(mrpt::utils::LVL_DEBUG, "[TBI_main] Done. %u candidates found.\n",(unsigned int)msg->loopClosureData.size() );

	return msg;
	MRPT_END
}
Beispiel #4
0
// ------------------------------------------------------
//				MapBuilding_ICP
// ------------------------------------------------------
void MapBuilding_ICP()
{
	MRPT_TRY_START
	  

	CTicTac								tictac,tictacGlobal,tictac_JH;
	int									step = 0;
	std::string							str;
	CSensFrameProbSequence				finalMap;
	float								t_exec;
	COccupancyGridMap2D::TEntropyInfo	entropy;

	size_t						rawlogEntry = 0;
	CFileGZInputStream					rawlogFile( RAWLOG_FILE.c_str() );

	CFileGZOutputStream sensor_data;

	// ---------------------------------
	//		Constructor
	// ---------------------------------
	CMetricMapBuilderICP mapBuilder(
		&metricMapsOpts,
		insertionLinDistance,
		insertionAngDistance,
		&icpOptions
		);

	mapBuilder.ICP_options.matchAgainstTheGrid = matchAgainstTheGrid;

	// ---------------------------------
	//   CMetricMapBuilder::TOptions
	// ---------------------------------
	mapBuilder.options.verbose					= true;
	mapBuilder.options.enableMapUpdating		= true;
    mapBuilder.options.debugForceInsertion		= false;
	mapBuilder.options.insertImagesAlways		= false;

	// Prepare output directory:
	// --------------------------------
	deleteFilesInDirectory(OUT_DIR);
	createDirectory(OUT_DIR);

	// Open log files:
	// ----------------------------------
	CFileOutputStream  f_log(format("%s/log_times.txt",OUT_DIR));
	CFileOutputStream  f_path(format("%s/log_estimated_path.txt",OUT_DIR));
	CFileOutputStream  f_pathOdo(format("%s/log_odometry_path.txt",OUT_DIR));


	// Create 3D window if requested:
	CDisplayWindow3DPtr	win3D;
#if MRPT_HAS_WXWIDGETS
	if (SHOW_PROGRESS_3D_REAL_TIME)
	{
		win3D = CDisplayWindow3DPtr( new CDisplayWindow3D("ICP-SLAM @ MRPT C++ Library (C) 2004-2008", 600, 500) );
		win3D->setCameraZoom(20);
		win3D->setCameraAzimuthDeg(-45);
	}
#endif

	if(OBS_FROM_FILE == 0){
	  sensor_data.open("sensor_data.rawlog");
	  printf("Receive From Sensor\n");
	  initLaser();
	  printf("OK\n");
	}
	

	// ----------------------------------------------------------
	//						Map Building
	// ----------------------------------------------------------
	CActionCollectionPtr	action;
	CSensoryFramePtr		observations, temp_obs;
	CSensoryFramePtr obs_set;
	CPose2D					odoPose(0,0,0);

	CSimplePointsMap	oldMap, newMap;
	CICP					ICP;

	vector_float		accum_x, accum_y, accum_z;
	
	// ICP Setting
	ICP.options.ICP_algorithm = (TICPAlgorithm)ICP_method;

	ICP.options.maxIterations			= 40;
	ICP.options.thresholdAng =			0.15;
	ICP.options.thresholdDist			= 0.75f;
	ICP.options.ALFA					= 0.30f;
	ICP.options.smallestThresholdDist	= 0.10f;
	ICP.options.doRANSAC = false;
	ICP.options.dumpToConsole();
	//
	CObservationPtr obsSick = CObservationPtr(new CObservation2DRangeScan());
	CObservationPtr obsHokuyo = CObservationPtr(new CObservation2DRangeScan());
	CSimplePointsMap hokuyoMap;
	
	bool isFirstTime = true;
	bool loop = true;

    /*
		  cout << index << "frame, Input the any key to continue" << endl;
		  getc(stdin);
		  fflush(stdin);
    */

	tictacGlobal.Tic();
	while(loop)
	{
      /*
		if(BREAK){
		  cout << index << "frame, Input the any key to continue" << endl;
		  getc(stdin);
		  fflush(stdin);
		}else{
		  if(os::kbhit())
			loop = true;
		}
      */

        if(os::kbhit())
          loop = true;
        

		if(DELAY) {
		  sleep(15);
		}
	  

		// Load action/observation pair from the rawlog:
		// --------------------------------------------------

		if(OBS_FROM_FILE == 1) {
		  if (! CRawlog::readActionObservationPair( rawlogFile, action, temp_obs, rawlogEntry) )
			break; // file EOF

		  obsSick = temp_obs->getObservationByIndex(0);
		  obsHokuyo = temp_obs->getObservationByIndex(1);
		  
		  observations = CSensoryFramePtr(new CSensoryFrame());		  
		  observations->insert((CObservationPtr)obsSick);

		  hokuyoMap.clear();
		  hokuyoMap.insertObservation(obsHokuyo.pointer());
		  
		}else{
		  rawlogEntry = rawlogEntry+2;

		  tictac.Tic();

		  obsSick = CObservationPtr(new CObservation2DRangeScan());
		  obsHokuyo = CObservationPtr(new CObservation2DRangeScan());

		  if(!receiveDataFromSensor((CObservation2DRangeScan*)obsSick.pointer(),SICK)){
			cout << " Error in receive sensor data" << endl;
			return;
		  }

		  if(!receiveDataFromSensor((CObservation2DRangeScan*)obsHokuyo.pointer(),HOKUYO)){
			cout << " Error in receive sensor data" << endl;

			return;
		  }
		  
		  cout << "Time to receive data : " << tictac.Tac()*1000.0f << endl;

		  obsSick->timestamp = mrpt::system::now();
		  obsSick->setSensorPose(CPose3D(0,0,0,DEG2RAD(0),DEG2RAD(0),DEG2RAD(0)));
		  ((CObservation2DRangeScan*)obsSick.pointer())->rightToLeft = true;
		  ((CObservation2DRangeScan*)obsSick.pointer())->stdError = 0.003f;

		  obsHokuyo->timestamp = mrpt::system::now();
		  obsHokuyo->setSensorPose(CPose3D(0,0,0.4,DEG2RAD(0),DEG2RAD(-90),DEG2RAD(0)));
		  ((CObservation2DRangeScan*)obsHokuyo.pointer())->rightToLeft = true;
		  ((CObservation2DRangeScan*)obsHokuyo.pointer())->stdError = 0.003f;
		  
		  cout << "rawlogEntry : " << rawlogEntry << endl;

		  CActionRobotMovement2D myAction;

		  newMap.clear();
		  obsSick.pointer()->insertObservationInto(&newMap);

		  if(!isFirstTime){

			static float					runningTime;
			static CICP::TReturnInfo		info;
			static CPose2D initial(0,0,0);

			CPosePDFPtr ICPPdf = ICP.AlignPDF(&oldMap, &newMap, initial, &runningTime, (void*)&info);
			CPose2D		estMean;
			CMatrixDouble33		estCov;
			ICPPdf->getCovarianceAndMean(estCov, estMean);
			printf("ICP run in %.02fms, %d iterations (%.02fms/iter), %.01f%% goodness\n -> ", runningTime*1000, info.nIterations, runningTime*1000.0f/info.nIterations, info.goodness*100 );
			cout << "ICP Odometry : " << ICPPdf->getMeanVal() << endl;

			myAction.estimationMethod = CActionRobotMovement2D::emScan2DMatching;
			myAction.poseChange = CPosePDFPtr( new CPosePDFGaussian(estMean, estCov));
		  }else{
			isFirstTime = false;
		  }

		  oldMap.clear();
		  oldMap.copyFrom(newMap);

		  observations = CSensoryFramePtr(new CSensoryFrame());
		  action = CActionCollectionPtr(new CActionCollection());		  

		  observations->insert((CObservationPtr)obsSick);

		  obs_set = CSensoryFramePtr(new CSensoryFrame());

		  obs_set->insert((CObservationPtr)obsSick);
		  obs_set->insert((CObservationPtr)obsHokuyo);
		  
		  action->insert(myAction);

		  sensor_data << action << obs_set;

		  hokuyoMap.clear();
		  hokuyoMap.insertObservation(obsHokuyo.pointer());

		}


		if (rawlogEntry>=rawlog_offset)
		{
			// Update odometry:
			{
				CActionRobotMovement2DPtr act= action->getBestMovementEstimation();
				if (act)
					odoPose = odoPose + act->poseChange->getMeanVal();
			}

			// Execute:
			// ----------------------------------------
			tictac.Tic();
				mapBuilder.processActionObservation( *action, *observations );
			t_exec = tictac.Tac();
			printf("Map building executed in %.03fms\n", 1000.0f*t_exec );


			// Info log:
			// -----------
			f_log.printf("%f %i\n",1000.0f*t_exec,mapBuilder.getCurrentlyBuiltMapSize() );

			const CMultiMetricMap* mostLikMap =  mapBuilder.getCurrentlyBuiltMetricMap();

			if (0==(step % LOG_FREQUENCY))
			{
				// Pose log:
				// -------------
				if (SAVE_POSE_LOG)
				{
					printf("Saving pose log information...");
					mapBuilder.getCurrentPoseEstimation()->saveToTextFile( format("%s/mapbuild_posepdf_%03u.txt",OUT_DIR,step) );
					printf("Ok\n");
				}
			}


			// Save a 3D scene view of the mapping process:
			if (0==(step % LOG_FREQUENCY) || (SAVE_3D_SCENE || win3D.present()))
			{
                CPose3D robotPose;
				mapBuilder.getCurrentPoseEstimation()->getMean(robotPose);

				COpenGLScenePtr		scene = COpenGLScene::Create();

                COpenGLViewportPtr view=scene->getViewport("main");
                ASSERT_(view);

                COpenGLViewportPtr view_map = scene->createViewport("mini-map");
                view_map->setBorderSize(2);
                view_map->setViewportPosition(0.01,0.01,0.35,0.35);
                view_map->setTransparent(false);

				{
					mrpt::opengl::CCamera &cam = view_map->getCamera();
					cam.setAzimuthDegrees(-90);
					cam.setElevationDegrees(90);
					cam.setPointingAt(robotPose);
					cam.setZoomDistance(20);
					cam.setOrthogonal();
				}

				// The ground:
				mrpt::opengl::CGridPlaneXYPtr groundPlane = mrpt::opengl::CGridPlaneXY::Create(-200,200,-200,200,0,5);
				groundPlane->setColor(0.4,0.4,0.4);
				view->insert( groundPlane );
				view_map->insert( CRenderizablePtr( groundPlane) ); // A copy

				// The camera pointing to the current robot pose:
				if (CAMERA_3DSCENE_FOLLOWS_ROBOT)
				{
				    scene->enableFollowCamera(true);

					mrpt::opengl::CCamera &cam = view_map->getCamera();
					cam.setAzimuthDegrees(-45);
					cam.setElevationDegrees(45);
					cam.setPointingAt(robotPose);
				}

				// The maps:
				{
					opengl::CSetOfObjectsPtr obj = opengl::CSetOfObjects::Create();
					mostLikMap->getAs3DObject( obj );
					view->insert(obj);

					// Only the point map:
					opengl::CSetOfObjectsPtr ptsMap = opengl::CSetOfObjects::Create();
					if (mostLikMap->m_pointsMaps.size())
					{
                        mostLikMap->m_pointsMaps[0]->getAs3DObject(ptsMap);
                        view_map->insert( ptsMap );
					}
				}

				// Draw the robot path:
				CPose3DPDFPtr posePDF =  mapBuilder.getCurrentPoseEstimation();
				CPose3D  curRobotPose;
				posePDF->getMean(curRobotPose);
				{
					opengl::CSetOfObjectsPtr obj = opengl::stock_objects::RobotPioneer();
					obj->setPose( curRobotPose );
					view->insert(obj);
				}
				{
					opengl::CSetOfObjectsPtr obj = opengl::stock_objects::RobotPioneer();
					obj->setPose( curRobotPose );
					view_map->insert( obj );
				}


				// Draw Hokuyo total data
				{
				  CRenderizablePtr hokuyoRender_t = scene->getByName("hokuyo_total");

				  if(!hokuyoRender_t){
					hokuyoRender_t = CPointCloud::Create();
					hokuyoRender_t->setName("hokuyo_total");
					hokuyoRender_t->setColor(0,0,1);
					hokuyoRender_t->setPose( CPose3D(0,0,0) );
					getAs<CPointCloud>(hokuyoRender_t)->setPointSize(3);
					scene->insert( hokuyoRender_t);					
				  }

				  for(size_t i =0 ; i < accum_x.size(); i++){
					getAs<CPointCloud>(hokuyoRender_t)->insertPoint(accum_x[i], accum_y[i], accum_z[i]);					
				  }
				  cout << "accum_x size : " << accum_x.size() << endl;


				}
				
				// Draw Hokuyo Current data plate
				{
				  CRenderizablePtr hokuyoRender = scene->getByName("hokuyo_cur");
				  hokuyoRender = CPointCloud::Create();
				  hokuyoRender->setName("hokuyo_cur");
				  hokuyoRender->setColor(0,1,0);
				  hokuyoRender->setPose( curRobotPose );
				  getAs<CPointCloud>(hokuyoRender)->setPointSize(0.1);
				  getAs<CPointCloud>(hokuyoRender)->loadFromPointsMap(&hokuyoMap);				  
				  scene->insert( hokuyoRender);

				  vector_float cur_x = getAs<CPointCloud>(hokuyoRender)->getArrayX();
				  vector_float cur_y = getAs<CPointCloud>(hokuyoRender)->getArrayY();
				  vector_float cur_z = getAs<CPointCloud>(hokuyoRender)->getArrayZ();


				  //				  cout << "current pose : " << curRobotPose << endl;				  
				  for(size_t i =0 ; i < cur_x.size(); i++){
					/*
					float rotate_x = cur_x[i]+ curRobotPose.y()*sin(curRobotPose.yaw()*3.14/180.0);
					float rotate_y = cur_y[i]+ curRobotPose.y()*cos(curRobotPose.yaw()*3.14/180.0);
					*/
					float rotate_x = curRobotPose.x() + cur_y[i]*sin(-curRobotPose.yaw());
					float rotate_y = curRobotPose.y() + cur_y[i]*cos(-curRobotPose.yaw());
					//					printf("cur_x, cur_y, cur_z : %f %f %f \n", rotate_x,rotate_y, cur_z[i]);

					accum_x.push_back(rotate_x);
					accum_y.push_back(rotate_y);
					accum_z.push_back(cur_z[i]);
				  }
				}

				// Save as file:
				if (0==(step % LOG_FREQUENCY) && SAVE_3D_SCENE)
				{
					CFileGZOutputStream	f( format( "%s/buildingmap_%05u.3Dscene",OUT_DIR,step ));
					f << *scene;
				}

				// Show 3D?
				if (win3D)
				{
					opengl::COpenGLScenePtr &ptrScene = win3D->get3DSceneAndLock();
					ptrScene = scene;

					win3D->unlockAccess3DScene();

					// Move camera:
					win3D->setCameraPointingToPoint( curRobotPose.x(),curRobotPose.y(),curRobotPose.z() );

					// Update:
					win3D->forceRepaint();

					sleep( SHOW_PROGRESS_3D_REAL_TIME_DELAY_MS );
				}
			}


			// Save the memory usage:
			// ------------------------------------------------------------------
			{
				printf("Saving memory usage...");
				unsigned long	memUsage = getMemoryUsage();
				FILE		*f=os::fopen( format("%s/log_MemoryUsage.txt",OUT_DIR).c_str() ,"at");
				if (f)
				{
					os::fprintf(f,"%u\t%lu\n",step,memUsage);
					os::fclose(f);
				}
				printf("Ok! (%.04fMb)\n", ((float)memUsage)/(1024*1024) );
			}

			// Save the robot estimated pose for each step:
			f_path.printf("%i %f %f %f\n",
				step,
				mapBuilder.getCurrentPoseEstimation()->getMeanVal().x(),
				mapBuilder.getCurrentPoseEstimation()->getMeanVal().y(),
				mapBuilder.getCurrentPoseEstimation()->getMeanVal().yaw() );


			f_pathOdo.printf("%i %f %f %f\n",step,odoPose.x(),odoPose.y(),odoPose.phi());

		} // end of if "rawlog_offset"...

		step++;
		printf("\n---------------- STEP %u | RAWLOG ENTRY %u ----------------\n",step, (unsigned)rawlogEntry);

		// Free memory:
		action.clear_unique();
		observations.clear_unique();
	};

	printf("\n---------------- END!! (total time: %.03f sec) ----------------\n",tictacGlobal.Tac());

	//	hokuyo.turnOff();	
	sick.stop();

	

	// Save map:
	mapBuilder.getCurrentlyBuiltMap(finalMap);

	str = format("%s/_finalmap_.simplemap",OUT_DIR);
	printf("Dumping final map in binary format to: %s\n", str.c_str() );
	mapBuilder.saveCurrentMapToFile(str);

	CMultiMetricMap  *finalPointsMap = mapBuilder.getCurrentlyBuiltMetricMap();
	str = format("%s/_finalmaps_.txt",OUT_DIR);
	printf("Dumping final metric maps to %s_XXX\n", str.c_str() );
	finalPointsMap->saveMetricMapRepresentationToFile( str );

	if (win3D)
		win3D->waitForKey();

	MRPT_TRY_END
}
Beispiel #5
0
void CPoseRandomSampler::setPosePDF( const CPose3DPDFPtr &pdf ) { 
	setPosePDF(pdf.pointer()); 
}
Beispiel #6
0
// ------------------------------------------------------
//				MapBuilding_ICP
//  override_rawlog_file: If not empty, use that rawlog
//  instead of that in the config file.
// ------------------------------------------------------
void MapBuilding_ICP(const string &INI_FILENAME, const string &override_rawlog_file)
{
	MRPT_START

	CConfigFile				iniFile(INI_FILENAME);

	// ------------------------------------------
	//			Load config from file:
	// ------------------------------------------
	const string RAWLOG_FILE			 = !override_rawlog_file.empty() ? override_rawlog_file : iniFile.read_string("MappingApplication","rawlog_file","",  /*Force existence:*/ true);
	const unsigned int rawlog_offset		 = iniFile.read_int("MappingApplication","rawlog_offset",0,  /*Force existence:*/ true);
	const string OUT_DIR_STD			 = iniFile.read_string("MappingApplication","logOutput_dir","log_out",  /*Force existence:*/ true);
	const int LOG_FREQUENCY		 = iniFile.read_int("MappingApplication","LOG_FREQUENCY",5,  /*Force existence:*/ true);
	const bool  SAVE_POSE_LOG		 = iniFile.read_bool("MappingApplication","SAVE_POSE_LOG", false,  /*Force existence:*/ true);
	const bool  SAVE_3D_SCENE        = iniFile.read_bool("MappingApplication","SAVE_3D_SCENE", false,  /*Force existence:*/ true);
	const bool  CAMERA_3DSCENE_FOLLOWS_ROBOT = iniFile.read_bool("MappingApplication","CAMERA_3DSCENE_FOLLOWS_ROBOT", true,  /*Force existence:*/ true);

	bool 	SHOW_PROGRESS_3D_REAL_TIME = false;
	int		SHOW_PROGRESS_3D_REAL_TIME_DELAY_MS = 0;

	MRPT_LOAD_CONFIG_VAR( SHOW_PROGRESS_3D_REAL_TIME, bool,  iniFile, "MappingApplication");
	MRPT_LOAD_CONFIG_VAR( SHOW_PROGRESS_3D_REAL_TIME_DELAY_MS, int, iniFile, "MappingApplication");

	const char* OUT_DIR = OUT_DIR_STD.c_str();

	// ------------------------------------
	//		Constructor of ICP-SLAM object
	// ------------------------------------
	CMetricMapBuilderICP mapBuilder;

	mapBuilder.ICP_options.loadFromConfigFile( iniFile, "MappingApplication");
	mapBuilder.ICP_params.loadFromConfigFile ( iniFile, "ICP");

	// Construct the maps with the loaded configuration.
	mapBuilder.initialize();

	// ---------------------------------
	//   CMetricMapBuilder::TOptions
	// ---------------------------------
	mapBuilder.options.verbose = true;
    mapBuilder.options.alwaysInsertByClass.fromString( iniFile.read_string("MappingApplication","alwaysInsertByClass","") );


	// Print params:
	printf("Running with the following parameters:\n");
	printf(" RAWLOG file:'%s'\n", RAWLOG_FILE.c_str());
	printf(" Output directory:\t\t\t'%s'\n",OUT_DIR);
	printf(" matchAgainstTheGrid:\t\t\t%c\n", mapBuilder.ICP_options.matchAgainstTheGrid ? 'Y':'N');
	printf(" Log record freq:\t\t\t%u\n",LOG_FREQUENCY);
	printf("  SAVE_3D_SCENE:\t\t\t%c\n", SAVE_3D_SCENE ? 'Y':'N');
	printf("  SAVE_POSE_LOG:\t\t\t%c\n", SAVE_POSE_LOG ? 'Y':'N');
	printf("  CAMERA_3DSCENE_FOLLOWS_ROBOT:\t%c\n",CAMERA_3DSCENE_FOLLOWS_ROBOT ? 'Y':'N');

	printf("\n");

	mapBuilder.ICP_params.dumpToConsole();
	mapBuilder.ICP_options.dumpToConsole();


	// Checks:
	ASSERT_(RAWLOG_FILE.size()>0);
	ASSERT_(fileExists(RAWLOG_FILE));

	CTicTac								tictac,tictacGlobal,tictac_JH;
	int									step = 0;
	string							str;
	CSimpleMap				finalMap;
	float								t_exec;
	COccupancyGridMap2D::TEntropyInfo	entropy;

	size_t						rawlogEntry = 0;
	CFileGZInputStream					rawlogFile( RAWLOG_FILE.c_str() );


	// Prepare output directory:
	// --------------------------------
	deleteFilesInDirectory(OUT_DIR);
	createDirectory(OUT_DIR);

	// Open log files:
	// ----------------------------------
	CFileOutputStream  f_log(format("%s/log_times.txt",OUT_DIR));
	CFileOutputStream  f_path(format("%s/log_estimated_path.txt",OUT_DIR));
	CFileOutputStream  f_pathOdo(format("%s/log_odometry_path.txt",OUT_DIR));


	// Create 3D window if requested:
	CDisplayWindow3DPtr	win3D;
#if MRPT_HAS_WXWIDGETS
	if (SHOW_PROGRESS_3D_REAL_TIME)
	{
		win3D = CDisplayWindow3D::Create("ICP-SLAM @ MRPT C++ Library", 600, 500);
		win3D->setCameraZoom(20);
		win3D->setCameraAzimuthDeg(-45);
	}
#endif

	// ----------------------------------------------------------
	//						Map Building
	// ----------------------------------------------------------
	CPose2D					odoPose(0,0,0);

	tictacGlobal.Tic();
	for (;;)
	{
		CActionCollectionPtr	action;
		CSensoryFramePtr		observations;
		CObservationPtr			observation;

		if (os::kbhit())
		{
			char c = os::getch();
			if (c==27)
				break;
		}

		// Load action/observation pair from the rawlog:
		// --------------------------------------------------
		if (! CRawlog::getActionObservationPairOrObservation( rawlogFile, action, observations, observation, rawlogEntry) )
			break; // file EOF

		const bool isObsBasedRawlog = observation.present();

		if (rawlogEntry>=rawlog_offset)
		{
			// Update odometry:
			if (isObsBasedRawlog)
			{
				static CPose2D lastOdo;
				static bool firstOdo = true;
				if (IS_CLASS(observation,CObservationOdometry))
				{
					CObservationOdometryPtr o = CObservationOdometryPtr(observation);
					if (!firstOdo)
						odoPose = odoPose + (o->odometry - lastOdo);

					lastOdo=o->odometry;
					firstOdo=false;
				}
			}
			else
			{
				CActionRobotMovement2DPtr act= action->getBestMovementEstimation();
				if (act)
					odoPose = odoPose + act->poseChange->getMeanVal();
			}

			// Execute:
			// ----------------------------------------
			tictac.Tic();
			if (isObsBasedRawlog)
					mapBuilder.processObservation( observation );
			else	mapBuilder.processActionObservation( *action, *observations );
			t_exec = tictac.Tac();
			printf("Map building executed in %.03fms\n", 1000.0f*t_exec );


			// Info log:
			// -----------
			f_log.printf("%f %i\n",1000.0f*t_exec,mapBuilder.getCurrentlyBuiltMapSize() );

			const CMultiMetricMap* mostLikMap =  mapBuilder.getCurrentlyBuiltMetricMap();

			if (0==(step % LOG_FREQUENCY))
			{
				// Pose log:
				// -------------
				if (SAVE_POSE_LOG)
				{
					printf("Saving pose log information...");
					mapBuilder.getCurrentPoseEstimation()->saveToTextFile( format("%s/mapbuild_posepdf_%03u.txt",OUT_DIR,step) );
					printf("Ok\n");
				}
			}

			// Save a 3D scene view of the mapping process:
			if (0==(step % LOG_FREQUENCY) || (SAVE_3D_SCENE || win3D.present()))
			{
                CPose3D robotPose;
				mapBuilder.getCurrentPoseEstimation()->getMean(robotPose);

				COpenGLScenePtr		scene = COpenGLScene::Create();

                COpenGLViewportPtr view=scene->getViewport("main");
                ASSERT_(view);

                COpenGLViewportPtr view_map = scene->createViewport("mini-map");
                view_map->setBorderSize(2);
                view_map->setViewportPosition(0.01,0.01,0.35,0.35);
                view_map->setTransparent(false);

				{
					mrpt::opengl::CCamera &cam = view_map->getCamera();
					cam.setAzimuthDegrees(-90);
					cam.setElevationDegrees(90);
					cam.setPointingAt(robotPose);
					cam.setZoomDistance(20);
					cam.setOrthogonal();
				}

				// The ground:
				mrpt::opengl::CGridPlaneXYPtr groundPlane = mrpt::opengl::CGridPlaneXY::Create(-200,200,-200,200,0,5);
				groundPlane->setColor(0.4,0.4,0.4);
				view->insert( groundPlane );
				view_map->insert( CRenderizablePtr( groundPlane) ); // A copy

				// The camera pointing to the current robot pose:
				if (CAMERA_3DSCENE_FOLLOWS_ROBOT)
				{
				    scene->enableFollowCamera(true);

					mrpt::opengl::CCamera &cam = view_map->getCamera();
					cam.setAzimuthDegrees(-45);
					cam.setElevationDegrees(45);
					cam.setPointingAt(robotPose);
				}

				// The maps:
				{
					opengl::CSetOfObjectsPtr obj = opengl::CSetOfObjects::Create();
					mostLikMap->getAs3DObject( obj );
					view->insert(obj);

					// Only the point map:
					opengl::CSetOfObjectsPtr ptsMap = opengl::CSetOfObjects::Create();
					if (mostLikMap->m_pointsMaps.size())
					{
                        mostLikMap->m_pointsMaps[0]->getAs3DObject(ptsMap);
                        view_map->insert( ptsMap );
					}
				}

				// Draw the robot path:
				CPose3DPDFPtr posePDF =  mapBuilder.getCurrentPoseEstimation();
				CPose3D  curRobotPose;
				posePDF->getMean(curRobotPose);
				{
					opengl::CSetOfObjectsPtr obj = opengl::stock_objects::RobotPioneer();
					obj->setPose( curRobotPose );
					view->insert(obj);
				}
				{
					opengl::CSetOfObjectsPtr obj = opengl::stock_objects::RobotPioneer();
					obj->setPose( curRobotPose );
					view_map->insert( obj );
				}

				// Save as file:
				if (0==(step % LOG_FREQUENCY) && SAVE_3D_SCENE)
				{
					CFileGZOutputStream	f( format( "%s/buildingmap_%05u.3Dscene",OUT_DIR,step ));
					f << *scene;
				}

				// Show 3D?
				if (win3D)
				{
					opengl::COpenGLScenePtr &ptrScene = win3D->get3DSceneAndLock();
					ptrScene = scene;

					win3D->unlockAccess3DScene();

					// Move camera:
					win3D->setCameraPointingToPoint( curRobotPose.x(),curRobotPose.y(),curRobotPose.z() );

					// Update:
					win3D->forceRepaint();

					sleep( SHOW_PROGRESS_3D_REAL_TIME_DELAY_MS );
				}
			}


			// Save the memory usage:
			// ------------------------------------------------------------------
			{
				printf("Saving memory usage...");
				unsigned long	memUsage = getMemoryUsage();
				FILE		*f=os::fopen( format("%s/log_MemoryUsage.txt",OUT_DIR).c_str() ,"at");
				if (f)
				{
					os::fprintf(f,"%u\t%lu\n",step,memUsage);
					os::fclose(f);
				}
				printf("Ok! (%.04fMb)\n", ((float)memUsage)/(1024*1024) );
			}

			// Save the robot estimated pose for each step:
			f_path.printf("%i %f %f %f\n",
				step,
				mapBuilder.getCurrentPoseEstimation()->getMeanVal().x(),
				mapBuilder.getCurrentPoseEstimation()->getMeanVal().y(),
				mapBuilder.getCurrentPoseEstimation()->getMeanVal().yaw() );


			f_pathOdo.printf("%i %f %f %f\n",step,odoPose.x(),odoPose.y(),odoPose.phi());

		} // end of if "rawlog_offset"...

		step++;
		printf("\n---------------- STEP %u | RAWLOG ENTRY %u ----------------\n",step, (unsigned)rawlogEntry);
	};

	printf("\n---------------- END!! (total time: %.03f sec) ----------------\n",tictacGlobal.Tac());

	// Save map:
	mapBuilder.getCurrentlyBuiltMap(finalMap);

	str = format("%s/_finalmap_.simplemap",OUT_DIR);
	printf("Dumping final map in binary format to: %s\n", str.c_str() );
	mapBuilder.saveCurrentMapToFile(str);

	CMultiMetricMap  *finalPointsMap = mapBuilder.getCurrentlyBuiltMetricMap();
	str = format("%s/_finalmaps_.txt",OUT_DIR);
	printf("Dumping final metric maps to %s_XXX\n", str.c_str() );
	finalPointsMap->saveMetricMapRepresentationToFile( str );

	if (win3D)
		win3D->waitForKey();

	MRPT_END
}