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