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方面的研究，可以跟我联系，大家相互交流学习。