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");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();    //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();