mrpt tips

其他 2020-04-13 11:53:17 阅读次数: 0

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();
random_repick
发布了50 篇原创文章 · 获赞 31 · 访问量 3万+
私信 关注

猜你喜欢

转载自blog.csdn.net/random_repick/article/details/78261705
今日推荐
手把手教你用 LangChain 实现大模型 Agent
外星人入侵（python）
超全的免费chatGPT列表【建议收藏】
52.2k star! 自己部署gpt4free, 免费使用各种GPT
2024年 （第十届）全国大学生统计建模大赛优秀论文解析——中国经济发展与碳排放库兹涅茨曲线的验证研究
【自动驾驶技术】自动驾驶汽车AI芯片汇总——NVIDIA篇
7个免费的ChatGPT网站，给大家送上
Angular v18 正式发布！
【VMware】 vCenter Converter standalone 6.6.0正式版下载
开源日报 | Angular v18；大模型价格战下的推理优化；Mistral AI以开源模型瞄准美国市场；硅谷有自己的鲁迅
数学建模Matlab之数据预处理方法
充电桩---ISO15118协议详细介绍
周排行
keepalived实现LB配置
数据库相关中间件收录集
Spring Boot 入门之 Web 篇（二） Spring Boot 入门之 Web 篇（二）
gitee 搭建个人网站
Java校招基础知识总结（横扫BAT,就业经验交流会演讲稿）
工程管理器
Delphi定位TDataSet数据集最后一条记录
cocos2dx笔记1:概述
Java实现 LeetCode 110 平衡二叉树
MacBook IDEA激活码
每日归档
更多
2024-05-30(4)
2024-05-29(65)
2024-05-28(2)
2024-05-27(56)
2024-05-26(6)
2024-05-25(68)
2024-05-24(65)
2024-05-23(9)
2024-05-22(41)
2024-05-21(8)

代码天地 © 2018 codetd.com

境外服务器   最新电视剧2022