参考：https://blog.csdn.net/weixin_44303829/article/details/121524380
https://github.com/AbangLZU/SC-LeGO-LOAM.git
https://github.com/AbangLZU/ndt_localizer.git

将建图和定位分别使用lego-loam和ndt来进行，实车上的效果非常不错，所以在这里好好学习一下原理和代码：

使用sc-lego-loam建图

git clone https://github.com/AbangLZU/SC-LeGO-LOAM.git

以上是git链接，直接编译即可。

catkin_make

修改成自己的雷达和imu的接口：

将原本的这两行注释或者直接修改——utility.h文件

// extern const string pointCloudTopic = "/os1_points";
// extern const string imuTopic = "/imu/data";

修改为自己的雷达和IMU的Topic

extern const string pointCloudTopic = "/velodyne_points";
extern const string imuTopic = "/imu/data";

以及雷达的一些参数：

# 注释
// Ouster OS1-64
//extern const int N_SCAN = 64;
//extern const int Horizon_SCAN = 1024;
//extern const float ang_res_x = 360.0/float(Horizon_SCAN);
//extern const float ang_res_y = 33.2/float(N_SCAN-1);
//extern const float ang_bottom = 16.6+0.1;
//extern const int groundScanInd = 15;
       
# 添加
// VLP-16
extern const int N_SCAN = 16;
extern const int Horizon_SCAN = 1800;
extern const float ang_res_x = 0.2;
extern const float ang_res_y = 2.0;
extern const float ang_bottom = 15.0+0.1;
extern const int groundScanInd = 7;
       
# 对于64线的VLP雷达，添加：
extern const int N_SCAN = 64;
extern const int Horizon_SCAN = 2083;
extern const float ang_res_x = 360.0/float(Horizon_SCAN);
extern const float ang_res_y = 26.8/float(N_SCAN-1);
extern const float ang_bottom = 24.8;
extern const int groundScanInd = 55;

编译然后运行即可：

roslaunch lego-loam run.launch

#播放bag包

rosbag play --clock ***.bag

播放完后ctrl+c结束后就能在map文件夹中自动保存地图的pcd文件。

使用ndt定位

git clone https://github.com/AbangLZU/ndt_localizer.git

同样github上直接git，接下来修改相关的接口：
将之前生成的pcd文件复制到ndt_localizer文件下的map文件夹里，对launch文件进行修改：

map_loader.launch

ndt_localizer.launch

将urdf注释掉：

pointsdownsample.launch

将雷达话题改为/velodyne_points；leaf_size改为1.0（如果采用的是16线的激光雷达,那么降采样的 leaf_size 控制在1 - 2 m 较为合适,当采用的激光雷达为 32 线及以上 , 可以将 leaf size 设置为 2 - 3 m ）

<!-- <arg name="points_topic" default="/apollo/sensor/velodyne32/PointCloud2/fusion" /> -->
<arg name="points_topic" default="/velodyne_points" />
<arg name="output_log" default="false" />
<arg name="leaf_size" default="1.0" />

然后编译后就能运行了：

roslaunch ndt_localizer ndt_localizer.launch

当然这里是需要提供初始位姿的：
再选择rviz里面的 2D Pose Estimate，选定车辆的初始姿态（一切使用预先构建的地图进行配准定位的方法都需要提供初始姿态,在工业界的实践中,这一初始姿态通常是通过 gnss 获得,本文中我们简化这一步,在 Rviz 中手动指定初始姿态, Rviz 中设定的初始姿态通常会被默认发送至 /initialpose topic 上）
所建的地图：（转为栅格地图）