定位运行后提示“Please set initialization pose for localization by RViz -> 2D Pose Estimate”，

[gsamful]

建图后的地图保存后，并保存到data目录下。运行localization并播放bag后，提示通过RViz初始化2D pose，但点击后没有任何反应，一直提示需要设置初始化位姿。

[zm0612]

在未修改代码的情况下，地图名称需要固定为map.pcd，如果这里设置正确，启动之后rviz上应该会显示一次全局的地图。然后通过RViz的2D pose功能点击界面中某一个位置，将会加载出来一个local map的点云，其亮度会比全局地图点云更亮。

另外，如果数据包是你自己录的，还需要保证localization/config_xxx.yaml中的激光雷达和IMU的topic名称与bag中一致。

[gsamful]

十分感谢作者百忙之中的回复！代码改了部分（比如下面提到的更改topic名称，如果不改建图就会出错），建图完成后保存并改为map.pcd，但启动后的确没有显示全局地图，不知道提示的“没有匹配到intensity”是导致问题的原因吗？

此外，前面建图中，更改了system.cpp中的cov_acc.norm()判定阈值，使用原来的0.05建图会报错，还有就是config中的标定矩阵我改了，其他地方没做修改。

[zm0612]

"不知道提示的“没有匹配到intensity”是导致问题的原因吗?" —— 这个不会影响使用。

我建议可以通过再代码中增加log输出，查看是不是地图加载的时候没有成功，例如；可在如下代码后增加log，输出点云路径和加载之后点云点个数等排查，是否数据输入有误。 https://github.com/zm0612/funny_lidar_slam/blob/611cc5893933af06716f254fa21728b977d977bf/src/slam/localization.cpp#L177

[gsamful]

请教一个问题，如果要得到100hz的融合位姿信息，需要做出怎样的更改？希望大佬点拨一下

[zm0612]

你好，该SLAM系统定位的Pose输出频率是由激光雷达驱动的，如需改成100hz，只能考虑将SLAM系统输出的Pose与IMU进行融合，来提升频率，有以下思路供你参考：

1. 在我的代码定位函数中（如下），将IMU和激光雷达雷达输出的Pose进行融合。 https://github.com/zm0612/funny_lidar_slam/blob/611cc5893933af06716f254fa21728b977d977bf/src/slam/system.cpp#L562
2. 你自己新建一个ros节点，订阅我的定位程序输出的Pose，然后你再订阅IMU的Topic，然后进行融合。

至于融合方法，你可以使用卡曼尔进行融合，将IMU的数据作为预测，将激光雷达的数据作用观测。也可以使用我写的预积分进行融合，同样也是IMU做预测，激光雷达做观测。均能将定位频率提升至100hz。

[gsamful]

好的，谢谢！

[gsamful]

大佬好，再请教下，工程里是将imu标定到雷达的吗？输出的融合位姿是激光雷达的吧？但配置文件里好像是将激光雷达标定到了imu？所以这个融合定位输出的定位点是哪里的？

[zm0612]

hello，坐标系用的是imu坐标系。会将激光雷达点云数据转换到imu坐标系下，这个的实现是在点云预处理的类中完成的preprocessing.cpp。类似如下位置的畸变矫正函数，实现了点云畸变矫正，并同时转到了imu坐标系下： https://github.com/zm0612/funny_lidar_slam/blob/611cc5893933af06716f254fa21728b977d977bf/src/slam/preprocessing.cpp#L204

https://github.com/zm0612/funny_lidar_slam/blob/611cc5893933af06716f254fa21728b977d977bf/src/lidar/lidar_distortion_corrector.cpp#L56

[gsamful]

好的，谢谢。也就说，融合定位点是imu的位置。现在跑我录的数据，融合定位输出的轨迹跟rtk轨迹值差别比较大😂，我再研究一下。

[Phymin]

hello，坐标系用的是imu坐标系。会将激光雷达点云数据转换到imu坐标系下，这个的实现是在点云预处理的类中完成的preprocessing.cpp。类似如下位置的畸变矫正函数，实现了点云畸变矫正，并同时转到了imu坐标系下：

https://github.com/zm0612/funny_lidar_slam/blob/611cc5893933af06716f254fa21728b977d977bf/src/slam/preprocessing.cpp#L204

https://github.com/zm0612/funny_lidar_slam/blob/611cc5893933af06716f254fa21728b977d977bf/src/lidar/lidar_distortion_corrector.cpp#L56

作者，你好，如果我要将校正后的点云重新转换到雷达坐标系下，是不是只需要加入下面的代码：

Vec3d pt_corrected = q_ref_inv_ * q_curr * pt_imu; 
const auto T_imu_to_lidar = T_lidar_to_imu_.inverse();
Vec3d pt_corrected_lidar =
 T_imu_to_lidar.block<3, 3>(0, 0) * pt_corrected + T_imu_to_lidar.block<3, 1>(0, 3);

还有其他地方需要修改的吗？谢谢

[zm0612]

@Phymin hello，这里函数操作之后将点云进行了畸变矫正，并转到imu坐标系下。后续会将imu坐标系下的点云数据与imu数据进行融合，如果这里又转成了激光雷达坐标系，会给后续的融合带来误差，甚至系统发散。

如果确实有转到激光雷达坐标系下的需求，建议在最终融合输出之后的地方进行转，可以把定位节点输出的位姿根据imu到lidar的外参进行转换，下面这个函数是定位节点最终输出给system的姿态。 https://github.com/zm0612/funny_lidar_slam/blob/611cc5893933af06716f254fa21728b977d977bf/src/slam/localization.cpp#L625

另外还有一些可视化的地方，也需要去修改一下，例如： https://github.com/zm0612/funny_lidar_slam/blob/611cc5893933af06716f254fa21728b977d977bf/src/slam/localization.cpp#L636

[Phymin]

好的，明白了，谢谢