MRPT EKF SLAM (3D表示) 编程学习笔记

An implementation of EKF-based SLAM with range-bearing sensors, odometry, a full 6D robot pose, and 3D landmarks.

The main method is "processActionObservation" which processes pairs of action/observation.

The following Wiki page describes an front-end application based on this class: http://babel.isa.uma.es/mrpt/index.php/Application:kf-slam

See also:

An implementation for 2D only: CRangeBearingKFSLAM2D

类之间调用关系：以CKalmanFilterCapable<>为基类，关键的方法在于processActionObservation，getCurrentState和getCurrentRobotPose。

类的详细文档：http://babel.isa.uma.es/mrpt/reference/stable/classmrpt_1_1slam_1_1_c_range_bearing_k_f_s_l_a_m.html#_details

附上代码：

// EKF SLAM

void EKFSLAM()

{

	std::string configFile="../EKF-SLAM_test.ini";

	CRangeBearingKFSLAM  mapping;

	CConfigFile			 cfgFile( configFile );

	std::string			 rawlogFileName;



	//mapping.debugOut = &myDebugStream;

	CDisplayWindow3D        win("My window");



	// The rawlog file:

	rawlogFileName = cfgFile.read_string("MappingApplication","rawlog_file",std::string("log.rawlog"));

	unsigned int	rawlog_offset = cfgFile.read_int("MappingApplication","rawlog_offset",0);

	unsigned int SAVE_LOG_FREQUENCY= cfgFile.read_int("MappingApplication","SAVE_LOG_FREQUENCY",1);

	bool  SAVE_3D_SCENES = cfgFile.read_bool("MappingApplication","SAVE_3D_SCENES", true);

	bool  SAVE_MAP_REPRESENTATIONS = cfgFile.read_bool("MappingApplication","SAVE_MAP_REPRESENTATIONS", true);

	string OUT_DIR = cfgFile.read_string("MappingApplication","logOutput_dir","OUT_KF-SLAM");

	string ground_truth_file = cfgFile.read_string("MappingApplication","ground_truth_file","");

	//cout << "RAWLOG FILE:" << endl << rawlogFileName << endl;

	ASSERT_( fileExists( rawlogFileName ) );

	CFileGZInputStream	rawlogFile( rawlogFileName );



	// 创建输出目录

	//deleteFilesInDirectory(OUT_DIR);

	//createDirectory(OUT_DIR);



	// Load the config options for mapping:

	mapping.loadOptions( CConfigFile(configFile) );

	/* mapping.KF_options.dumpToConsole();

	mapping.options.dumpToConsole();

	*/

	//			INITIALIZATION

	// ----------------------------------------

	//mapping.initializeEmptyMap();



	// The main loop:

	CActionCollectionPtr	action;

	CSensoryFramePtr		observations;

	size_t			rawlogEntry = 0, step = 0;

	deque<CPose3D>   meanPath; // The estimated path

	CPose3DPDFGaussian		robotPose;

	std::vector<CPoint3D>	LMs;

	std::map<unsigned int,CLandmark::TLandmarkID> LM_IDs;

	CMatrixDouble	fullCov;

	CVectorDouble   fullState;



	for(;;)

	{

		if (os::kbhit())

		{	// Esc quit the program

			char	pushKey = os::getch();

			if (27 == pushKey)

				break;

		}



		// Load action/observation pair from the rawlog:

		if (! CRawlog::readActionObservationPair( rawlogFile, action, observations, rawlogEntry) )

			break; // file EOF



		if (rawlogEntry>=rawlog_offset)

		{

			// Process the action and observations:

			mapping.processActionObservation(action,observations);



			// Get current state:

			mapping.getCurrentState( robotPose,LMs,LM_IDs,fullState,fullCov );

			//cout << "Mean pose: " << endl << robotPose.mean << endl;

			//cout << "# of landmarks in the map: " << LMs.size() << endl;



			// Build the path:

			meanPath.push_back( robotPose.mean );



			

			// Free rawlog items memory:

			action.clear_unique();

			observations.clear_unique();

		}// (rawlogEntry>=rawlog_offset)



		//cout << format("\nStep %u  - Rawlog entries processed: %i\n", (unsigned int)step, (unsigned int)rawlogEntry);

		step++;



		// Show Mapping

		opengl::COpenGLScenePtr &scene3D = win.get3DSceneAndLock();



		// Modify the scene:

		opengl::CGridPlaneXYPtr grid = opengl::CGridPlaneXY::Create(-1000,1000,-1000,1000,0,5);

		grid->setColor(0.4,0.4,0.4);

		scene3D->insert( grid );



		// Robot path:

		opengl::CSetOfLinesPtr linesPath = opengl::CSetOfLines::Create();

		linesPath->setColor(1,0,0);



		double x0=0,y0=0,z0=0;



		if (!meanPath.empty())

		{

			x0 = meanPath[0].x();

			y0 = meanPath[0].y();

			z0 = meanPath[0].z();

		}



		for (deque<CPose3D>::iterator it=meanPath.begin();it!=meanPath.end();++it)

		{

			linesPath->appendLine(

				x0,y0,z0,

				it->x(), it->y(), it->z() );

			x0=it->x();

			y0=it->y();

			z0=it->z();

		}

		scene3D->insert( linesPath );

		opengl::CSetOfObjectsPtr  objs = opengl::CSetOfObjects::Create();

		mapping.getAs3DObject(objs);

		scene3D->insert( objs );



		// Unlock it, so the window can use it for redraw:

		win.unlockAccess3DScene();

		// Update window, if required

		win.forceRepaint();



	}	// end "while(1)"

}



void CMrptImageBasicTestView::OnSlamEkfslam()

{

	// 调用EKF-SLAM

	HANDLE handle;

	DWORD id;

	handle=CreateThread(NULL,0,(LPTHREAD_START_ROUTINE)EKFSLAM,NULL,0,&id);

	CloseHandle(handle);

}

执行结果 按3D格式显示，可以自主调整视角

 

参考：MRPT Project

ps：有谁也做SLAM方面的研究，可以跟我联系，大家相互交流学习。