多传感融合定位（六）—— autoware NDT单独测试
在 多传感融合定位（五）—— autoware NDT单独编译与使用 中，将autoware定位建图有关的包拿出来单独编译，编译成功，接下来将进行包的测试，理清代码逻辑，最终在自己的车上进行实验，查看定位结果。
目录
1. 使用官方提供数据包进行建图与定位
1.1 建图测试
1.2 定位测试
2. 使用自己的传感器进行定位与建图
2.1 数据采集
2.2 离线建图
2.3 实时定位
3. 定位结果分析
1. 使用官方提供数据包进行建图与定位
ROSBAG Demo · Wiki · Autoware Foundation / MovedToGitHub / autoware · GitLabProject has been moved to Github: https://github.com/Autoware-AI/autoware.ai
https://gitlab.com/autowarefoundation/autoware.ai/autoware/-/wikis/ROSBAG-Demo
1.1 建图测试
先按照上面的demo下载解压对应的数据，接下来播放数据包，查看对应的数据包信息。
可见，包内只提供了两种topic，GPS、点云；先来看下GPS信息：
sentence: "QQ02C,INSATT,V,004112.00,8.194,12.416,151.186,@65"
---
header:
seq: 156045
stamp:
secs: 1427157687
nsecs: 185960054
frame_id: "/gps"
sentence: "WW03E,INSDAT,004112.00,1.623,-2.647,-10.231,-2.953,-2.487,1.610,@95"
---
header:
seq: 156046
stamp:
secs: 1427157687
nsecs: 334904909
frame_id: "/gps"
sentence: "$GNRMC,004112.20,A,3514.0978980,N,13700.2997565,E,9.4358,147.001,240315,7.320,E,D*1F"
---
header:
seq: 156047
stamp:
secs: 1427157687
nsecs: 352860927
frame_id: "/gps"
sentence: "$GPGGA,004112.20,3514.0978980,N,13700.2997565,E,4,12,0.82,47.7540,M,38.4589,M,1.2,0556*43"
---
header:
seq: 156048
stamp:
secs: 1427157687
nsecs: 362860918
frame_id: "/gps"
sentence: "$GNVTG,147.001,T,154.321,M,9.4358,N,17.4751,K,D*09"
---
header:
seq: 156049
stamp:
secs: 1427157687
nsecs: 371870994
frame_id: "/gps"
里面有很多种消息类型，接下来运行建图节点：
roslaunch lidar_localizer ndt_mapping.launch 启动nmea转换节点：
roslaunch gnss_localizer nmea2tfpose.launch
播放数据：
rosbag play sample_moriyama_150324.bagrviz可视化建图过程：
查看节点图：
地图保存，在程序运行前，先运行下面的程序：
rosrun pcl_ros pointcloud_to_pcd input:=/ndt_map prefix:=map 地图被不停保存到pcd文件中，该文件位于运行指令的目录下： 
加载查看地图：
roslaunch map_file points_map_loader.launch path_pcd:="/home/sml/0.pcd"
加载地图很慢，考虑降采样来减小地图，还有通过分割，将大地图分割成小地图。
leaf size: 0.3 地图大小：19.7M, 效果：
leaf size: 0.15 地图大小：499.8M, 效果：(加载还是快的，但是，rviz操作很卡)
最终测试结果，leaf_size在0.255左右比较合适。
rosrun map_tools pcd_filter "PointXYZI" "0.255" "/home/sml/0.pcd" 1.2 定位测试
autoware定位测试：
（1）播放数据
（2）设置TF：主要设置雷达到base_link的静态坐标变换
（3）加载地图及TF
roslaunch map_file points_map_loader.launch path_pcd:="/home/sml/0.26_0.pcd"
roslaunch lidar_localizer setup_tf.launch 
TF文件设置了两个静态坐标变换：
（4）设置点云过滤：用于实时输入点云的过滤
roslaunch points_downsampler points_downsample.launch
（5）设置NDT matching（个人GPU好像还是有问题，只能CPU运行）
(6) 播放数据，可视化定位
（7）节点分析
（8）将gnss用于初定位
roslaunch gnss_localizer nmea2tfpose.launch roslaunch runtime_manager runtime_manager.launchroslaunch lidar_localizer ndt_matching.launch
ndt_stat信息：
将上面的工作整理了一下，写了下面的launch文件，可以一次启动多个launch:
上面的工作，运行只需要两个窗口：
roslaunch runtime_manager runtime_manager.launchroslaunch lidar_localizer localization.launch然后播放数据，就能得到定位结果。
2. 使用自己的传感器进行定位与建图
无人驾驶Autoware代码中GNSS和激光雷达定位ndt_matching - 简书
https://www.jianshu.com/p/be87ec155e15
2.1 数据采集
（1）启动驱动
roslaunch driver_launch driver.launch（2）使用autoware来录制数据包（也可以使用下面的指令录制数据包）
# rosbag record /topic1 /topic12 -o out.bag
# rosbag record -a out.bag（3）驱动分析
测试驱动与定位程序一起运行：
有个问题，nmea2tfpose包还是不输出pose信息，那就换一个（fix2tfpose）：
2.2 离线建图
(1) jetson 下构建的地图
（2）notebook下构建的地图
2.3 实时定位
(1) 使用/fix
2.4 关于NDT_matching参数说明
NDT_matching参数在autoware中是通过runtime manager以话题的方式发布的，话题通过界面触发，每切换界面按钮、修改参数并确定就触发一次话题发布。
参数说明：
- init_pos_gnss：1通过GNSS来确定初值，0通过初始位姿确定初值；
- use_predict_pose：1使用预测位姿，0不使用预测位姿；
- error_threshold: （1.0）该参数不知道干嘛的，好像没用；
- resolution: （0.1000000014）网格大小设置；
- step_size: (0.1000000014) 迭代步长；
- trans_epsilon: （0.00999999977648）算法收敛条件；
- max_iterations: （30）最大迭代次数；
这些参数通过话题方式发布，来实时改变程序中的NDT配置参数，类似于动态参数配置，至于为什么不用ros中的动态参数配置，不知道。
接下来的工作，可以将这部分参数写成一般参数，在程序开始时加载。
3. 定位结果分析
evo测评TUM数据集_dididada~的博客-CSDN博客_evo tum
https://blog.csdn.net/qq_43265072/article/details/104715515
使用evo评测VIO算法 - 简书https://www.jianshu.com/p/a7a3f93bfbba（1）安装evo
pip3 install evo --upgrade（2）保存轨迹为TUM格式
ROS轨迹保存为tum格式,并用evo轨迹绘制_just_do_it567的博客-CSDN博客https://blog.csdn.net/just_do_it567/article/details/114672305(3）绘制数据图
数据飞来飞去，很乱，以为是哪里出了问题，使用rviz可视化也一样。
下图是NDT_path:
