mrpt tips

1.命名空间及头文件地址

mrpt 1.0	命名空间	头文件地址
COccupancyGridMap2D.h	mrpt::slam	/usr/include/mrpt/maps/include/mrpt/slam
CICP.h	mrpt::slam	/usr/include/mrpt/slam/include/mrpt/slam
CSimplePointsMap.h	mrpt::slam	/usr/include/mrpt/maps/include/mrpt/slam
CObservation2DRangeScan.h	mrpt::slam	/usr/include/mrpt/obs/include/mrpt/slam
CPose2D.h	mrpt::poses	/usr/include/mrpt/base/include/mrpt/poses
CPose3D.h	mrpt::poses	/usr/include/mrpt/base/include/mrpt/poses
CPosePDF.h	mrpt::poses	/usr/include/mrpt/base/include/mrpt/poses

2.图优化

• 函数实现文件为 mrpt/libs/graphslam/include/mrpt/graphslam/levmarq.h

#include <mrpt/random.h>
#include <mrpt/graphslam/levmarq.h>
#include <mrpt/system/threads.h> // sleep()
#include <mrpt/gui.h>
#include <mrpt/opengl/CSetOfObjects.h>
#include <mrpt/opengl/COpenGLScene.h>
#include <mrpt/opengl/graph_tools.h>

using namespace mrpt;
using namespace mrpt::utils;
using namespace mrpt::graphs;
using namespace mrpt::graphslam;
using namespace mrpt::poses;
using namespace mrpt::math;
using namespace mrpt::gui;
using namespace mrpt::opengl;
using namespace mrpt::random;
using namespace std;

static void my_levmarq_feedback(
		const CNetworkOfPoses2DInf &graph,
		const size_t iter,
		const size_t max_iter,
		const double cur_sq_error )
	{
		if ((iter % 100)==0)
			cout << "Progress: " << iter << " / " << max_iter << ", total sq err = " << cur_sq_error << endl;
	}

int main()
{
	CNetworkOfPoses2DInf graph;
	CNetworkOfPoses2DInf::global_poses_t  real_node_poses;

	const size_t N_VERTEX = 50;
	const double DIST_THRES = 7;
	const double NODES_XY_MAX = 20;

	//加点
	for (TNodeID j=0;j<N_VERTEX;j++)
	{
		static double ang = 2*M_PI/N_VERTEX;
		const double R = NODES_XY_MAX + 2 * (j % 2 ? 1:-1);
		CPose2D p(R*cos(ang*j), R*sin(ang*j), ang);
		real_node_poses[j] = p;
		graph.nodes[j] = p;
	}

	//加边
	static const int DIM = CNetworkOfPoses2DInf::edge_t::type_value::static_size;
	CMatrixFixedNumeric<double,DIM,DIM> inf_matrix;
	inf_matrix.unit(CMatrixFixedNumeric<double,DIM,DIM>::RowsAtCompileTime, square(1.0/10));
	for (TNodeID i=0;i<N_VERTEX;i++)
	{
		for (TNodeID j=i+1;j<N_VERTEX;j++)
		{
			if ( real_node_poses[i].distanceTo(real_node_poses[j]) < DIST_THRES )
			{
				CNetworkOfPoses2DInf::edge_t RelativePose(real_node_poses[j] - real_node_poses[i], inf_matrix);
				graph.insertEdge(i, j, RelativePose );
			}
		}
	}

	//图的原点
	graph.root = TNodeID(0);
	//GroundTruth
	const CNetworkOfPoses2DInf  graph_GT = graph;

	//加噪声
	randomGenerator.randomize(123);
	const double STD_NOISE_NODE_XYZ = 0.5;
	const double STD_NOISE_NODE_ANG = DEG2RAD(5);
	const double STD_NOISE_EDGE_XYZ = 0.001;
	const double STD_NOISE_EDGE_ANG = DEG2RAD(0.01);
	for (CNetworkOfPoses2DInf::edges_map_t::iterator itEdge=graph.edges.begin();itEdge!=graph.edges.end();++itEdge)
		{
			const CNetworkOfPoses2DInf::edge_t::type_value delta_noise(CPose3D(
				randomGenerator.drawGaussian1D(0,STD_NOISE_EDGE_XYZ),
				randomGenerator.drawGaussian1D(0,STD_NOISE_EDGE_XYZ),
				randomGenerator.drawGaussian1D(0,STD_NOISE_EDGE_XYZ),
				randomGenerator.drawGaussian1D(0,STD_NOISE_EDGE_ANG),
				randomGenerator.drawGaussian1D(0,STD_NOISE_EDGE_ANG),
				randomGenerator.drawGaussian1D(0,STD_NOISE_EDGE_ANG) ));
			itEdge->second.getPoseMean() += CNetworkOfPoses2DInf::edge_t::type_value(delta_noise);
		}


	for (CNetworkOfPoses2DInf::global_poses_t::iterator itNode=graph.nodes.begin();itNode!=graph.nodes.end();++itNode)
			if (itNode->first!=graph.root)
				itNode->second.getPoseMean() += CNetworkOfPoses2DInf::edge_t::type_value( CPose3D(
					randomGenerator.drawGaussian1D(0,STD_NOISE_NODE_XYZ),
					randomGenerator.drawGaussian1D(0,STD_NOISE_NODE_XYZ),
					randomGenerator.drawGaussian1D(0,STD_NOISE_NODE_XYZ),
					randomGenerator.drawGaussian1D(0,STD_NOISE_NODE_ANG),
					randomGenerator.drawGaussian1D(0,STD_NOISE_NODE_ANG),
					randomGenerator.drawGaussian1D(0,STD_NOISE_NODE_ANG) ) );


	//优化前的图
	const CNetworkOfPoses2DInf  graph_initial = graph;

	TParametersDouble  params;
	//params["verbose"]  = 1;
	params["profiler"] = 1;
	params["max_iterations"] = 500;
	params["scale_hessian"] = 0.1;  
	params["tau"] = 1e-3; 
	graphslam::TResultInfoSpaLevMarq  levmarq_info;
	//图优化
	graphslam::optimize_graph_spa_levmarq(graph, levmarq_info, NULL, params, &my_levmarq_feedback);
	
	//显示
	CDisplayWindow3D  win("graph-slam demo results");
	CDisplayWindow3D  win2("graph-slam demo initial state");

	TParametersDouble  graph_render_params1;
	graph_render_params1["show_edges"] = 1;
	graph_render_params1["edge_width"] = 1;
	graph_render_params1["nodes_corner_scale"] = 1;
	CSetOfObjectsPtr gl_graph1 = mrpt::opengl::graph_tools::graph_visualize(graph, graph_render_params1 );

	TParametersDouble  graph_render_params2;
	graph_render_params2["show_ground_grid"] = 0;
	graph_render_params2["show_edges"] = 0;
	graph_render_params2["show_node_corners"] = 0;
	graph_render_params2["nodes_point_size"]  = 17;
	CSetOfObjectsPtr gl_graph2 = mrpt::opengl::graph_tools::graph_visualize(graph_initial, graph_render_params2 );

	graph_render_params2["nodes_point_size"]  = 10;
	CSetOfObjectsPtr gl_graph5 = mrpt::opengl::graph_tools::graph_visualize(graph, graph_render_params2 );

	TParametersDouble  graph_render_params3;
	graph_render_params3["show_ground_grid"] = 0;
	graph_render_params3["show_ID_labels"] = 1;
	graph_render_params3["show_edges"] = 1;
	graph_render_params3["edge_width"] = 3;
	graph_render_params3["nodes_corner_scale"] = 2;
	CSetOfObjectsPtr gl_graph3 = mrpt::opengl::graph_tools::graph_visualize(graph_GT, graph_render_params3 );
	CSetOfObjectsPtr gl_graph4 = mrpt::opengl::graph_tools::graph_visualize(graph_initial, graph_render_params3 );

	{
		COpenGLScenePtr &scene = win.get3DSceneAndLock();
		scene->insert(gl_graph1);
		scene->insert(gl_graph3);   //GT
		scene->insert(gl_graph2);   //未优化点位
		scene->insert(gl_graph5);   //优化后点位
		win.unlockAccess3DScene();
		win.repaint();
	}

	{
		COpenGLScenePtr &scene = win2.get3DSceneAndLock();
		scene->insert(gl_graph3);
		scene->insert(gl_graph4);
		win2.unlockAccess3DScene();
		win2.repaint();
	}

	cout << "Close any window to end...\n";
	while (win.isOpen() && win2.isOpen() && !mrpt::system::os::kbhit())
	{
		mrpt::system::sleep(10);
	}

	return 0;
}

• 优化函数为graphslam::optimize_graph_spa_levmarq(graph, levmarq_info, NULL, params, &my_levmarq_feedback);参数依次为需要优化的图也是结果图，优化结果信息，优化点集（NULL表示所有点），优化参数，回调函数。第五个参数回调函数指针默认为NULL，如果没有后续处理，可以这样调用graphslam::optimize_graph_spa_levmarq(graph, levmarq_info, NULL, params);

3.CObservation2DRangeScan存取

• 保存，假设要保存的变量是my_scan

ofstream output_frame("frame.txt");
ofstream output_valid("valid.txt");
vector<float> output_scan = my_scan->scan;
vector<char> valid = my_scan->validRange;
for(int j = 0; j < output_scan.size(); ++j)
{
	//保存inf
	if(output_scan[j] > 60)
    {
	    output_frame << 60.89 << "\t";
    }
    else
    {
	    output_frame << output_scan[j] << "\t";
    }
    //validRange非0即1
    output_valid << (int)valid[j] << "\t";
}
output_valid << endl;
output_frame << endl;
output_frame.close();
output_valid.close();

• 读取

ifstream input_frame("frame.txt");
ifstream input_valid("valid.txt");
if(!input_frame.is_open() || !input_valid.is_open())
{
	return;
}
CObservation2DRangeScan scan;
//根据保存的数据确定
scan.aperture = 6.26573;
scan.rightToLeft = true;
float f;
int v, flag = 0， SCAN_SIZE = 360;
while(1)
{
    scan.scan.clear();        
    scan.validRange.clear();     //会将validRange的大小设为0
    //SCAN_SIZE根据保存的数据确定
    for(int i = 0; i < SCAN_SIZE; ++i)
    {
        //用此方式判断文件尾，防止多读一次
        input_frame >> f;
        if(input_frame.eof()) 
        {
            flag = 1;
            break;
        }
        if(f > 60)
        {
            scan.scan.push_back(1/0.0f);
        }
        else
        {
            scan.scan.push_back(f);
        }
        input_valid >> v;
        scan.validRange.push_back(char(v));
    }
    if(flag == 1) break;
}
input_frame.close();
input_valid.close();