3d激光slam建图与定位(1)

节���图

匹配精度 帧率做到100ms左右，纯ndt在2-3cm的位置误差 ，角度误差在0.2度左右

一.代码实现流程

二.ndt算法原理

一.该算法定位有三个进程文件

1.map_loader.cpp用于点云地图的读取，从文件中读取点云后对这个点云地图进行旋转平移后发布点云地图到ros

#include "map_loader.h"MapLoader::MapLoader(ros::NodeHandle &nh){    std::string pcd_file_path, map_topic;    // 点云地图读取路径    nh.param("pcd_path", pcd_file_path, "");    // 点云地图话题名    nh.param("map_topic", map_topic, "point_map");    //初始位置的相对位置坐标变换 参数读取    init_tf_params(nh);    //要发布的map话题    pc_map_pub_ = nh.advertise(map_topic, 10, true);    // 点云地图路径存储    file_list_.push_back(pcd_file_path);    //从文件中读取pcd    auto pc_msg = CreatePcd();    // 根据初始坐标变换进行点云地图的坐标变换    auto out_msg = TransformMap(pc_msg);//发布点云地图话题    if (out_msg.width != 0) {out_msg.header.frame_id = "map";pc_map_pub_.publish(out_msg);}}void MapLoader::init_tf_params(ros::NodeHandle &nh){    // 从参数中加载坐标系变换值    nh.param("x", tf_x_, 0.0);    nh.param("y", tf_y_, 0.0);    nh.param("z", tf_z_, 0.0);    nh.param("roll", tf_roll_, 0.0);    nh.param("pitch", tf_pitch_, 0.0);    nh.param("yaw", tf_yaw_, 0.0);    ROS_INFO_STREAM("x"