About fastlio bug?

[luameows]

Hi there, I found that in mapping/fastlio/src/preprocess.cpp [velodyne_handler] function, you set the timestamp of each point in a scan to 0? That may be a little different from offical-fastlio. In fastlio, they will use imu to do forward broadcast and undistor every point in a scan. This funcation is located at IMU_Processing.hpp" line 219 sort(xxxx)**. Maybe you forgot?

[w111liang222]

Hi there, I found that in mapping/fastlio/src/preprocess.cpp [velodyne_handler] function, you set the timestamp of each point in a scan to 0? That may be a little different from offical-fastlio. In fastlio, they will use imu to do forward broadcast and undistor every point in a scan. This funcation is located at IMU_Processing.hpp" line 219 sort(xxxx)**. Maybe you forgot?

Hi,

It's not a bug, we don't use the offical fastlio to undistort the points because the offical fastlio estimates the pose at the end time of the lidar scan, but we need the pose at the start time of the scan.

So, we do the points undistortion (only rotation part) at slam/mapping/fastlio/src/fastlio.cpp line 153 before sending to the fastlio mapping. After the fastlio outputs the odometry, we calculate the delta pose between two adjacent frames and undistort the origin pointcloud again at slam/backend/hdl_graph_slam/apps/hdl_graph_slam_nodelet.cpp line 200.

[heroacool]

Hi there, I found that in mapping/fastlio/src/preprocess.cpp [velodyne_handler] function, you set the timestamp of each point in a scan to 0? That may be a little different from offical-fastlio. In fastlio, they will use imu to do forward broadcast and undistor every point in a scan. This funcation is located at IMU_Processing.hpp" line 219 sort(xxxx)**. Maybe you forgot?

Hi,

It's not a bug, we don't use the offical fastlio to undistort the points because the offical fastlio estimates the pose at the end time of the lidar scan, but we need the pose at the start time of the scan.

So, we do the points undistortion (only rotation part) at slam/mapping/fastlio/src/fastlio.cpp line 153 before sending to the fastlio mapping. After the fastlio outputs the odometry, we calculate the delta pose between two adjacent frames and undistort the origin pointcloud again at slam/backend/hdl_graph_slam/apps/hdl_graph_slam_nodelet.cpp line 200.

why you need the pose at the start time of the scan?

[w111liang222]

Hi there, I found that in mapping/fastlio/src/preprocess.cpp [velodyne_handler] function, you set the timestamp of each point in a scan to 0? That may be a little different from offical-fastlio. In fastlio, they will use imu to do forward broadcast and undistor every point in a scan. This funcation is located at IMU_Processing.hpp" line 219 sort(xxxx)**. Maybe you forgot?

Hi, It's not a bug, we don't use the offical fastlio to undistort the points because the offical fastlio estimates the pose at the end time of the lidar scan, but we need the pose at the start time of the scan. So, we do the points undistortion (only rotation part) at slam/mapping/fastlio/src/fastlio.cpp line 153 before sending to the fastlio mapping. After the fastlio outputs the odometry, we calculate the delta pose between two adjacent frames and undistort the origin pointcloud again at slam/backend/hdl_graph_slam/apps/hdl_graph_slam_nodelet.cpp line 200.

why you need the pose at the start time of the scan?

In LSD, the coordinate system is uniformly defined, we use the position of the scan at the start time as the origin of the coordinate system of the points, so the points are forward undistorted and the pose outputed by slam is at the start time of the scan. If you need the pose at the end time of the scan, you can use the imu data to predict

[minghongss]

官方的fastlio代码是通过imu预测得到lidar结束时刻的先验位姿，然后通过反向传播得到基于lidar结束时刻坐标系的去畸变点云，然后通过ieskf进行位姿更新。我看您的代码，您是仍然使用imu预测得到lidar结束时刻的先验位姿，但是因为您不是使用的反向传播，且我看您上述的描述，推测您的去畸变点云是基于lidar起始时刻坐标系的，此时ieskf迭代更新的时候，先验位姿是lidar结束时刻的位姿，但是点云却是在起始时刻下，这样感觉有点怪。（ps：虽然通过ieskf的迭代，最终是能够将在lidar结束时刻的先验位姿更新到lidar起始时刻）。

[w111liang222]

官方的fastlio代码是通过imu预测得到lidar结束时刻的先验位姿，然后通过反向传播得到基于lidar结束时刻坐标系的去畸变点云，然后通过ieskf进行位姿更新。我看您的代码，您是仍然使用imu预测得到lidar结束时刻的先验位姿，但是因为您不是使用的反向传播，且我看您上述的描述，推测您的去畸变点云是基于lidar起始时刻坐标系的，此时ieskf迭代更新的时候，先验位姿是lidar结束时刻的位姿，但是点云却是在起始时刻下，这样感觉有点怪。（ps：虽然通过ieskf的迭代，最终是能够将在lidar结束时刻的先验位姿更新到lidar起始时刻）。

lsd修改了fastlio后，imu预测的是上一帧结束时刻的位姿(其实也就是当前帧起始时刻的位姿)，你可以通过打印imu队列，可以看到每次迭代更新时，一直缓存着大约10个imu(imu为100hz)

[minghongss]

官方的fastlio代码是通过imu预测得到lidar结束时刻的先验位姿，然后通过反向传播得到基于lidar结束时刻坐标系的去畸变点云，然后通过ieskf进行位姿更新。我看您的代码，您是仍然使用imu预测得到lidar结束时刻的先验位姿，但是因为您不是使用的反向传播，且我看您上述的描述，推测您的去畸变点云是基于lidar起始时刻坐标系的，此时ieskf迭代更新的时候，先验位姿是lidar结束时刻的位姿，但是点云却是在起始时刻下，这样感觉有点怪。（ps：虽然通过ieskf的迭代，最终是能够将在lidar结束时刻的先验位姿更新到lidar起始时刻）。

lsd修改了fastlio后，imu预测的是上一帧结束时刻的位姿(其实也就是当前帧起始时刻的位姿)，你可以通过打印imu队列，可以看到每次迭代更新时，一直缓存着大约10个imu(imu为100hz)

如果是按您说的这样，那就没啥问题了。但是我看了代码，在sync_packages()等地方没看到对应的逻辑？您方便指出一下么？

[w111liang222]

官方的fastlio代码是通过imu预测得到lidar结束时刻的先验位姿，然后通过反向传播得到基于lidar结束时刻坐标系的去畸变点云，然后通过ieskf进行位姿更新。我看您的代码，您是仍然使用imu预测得到lidar结束时刻的先验位姿，但是因为您不是使用的反向传播，且我看您上述的描述，推测您的去畸变点云是基于lidar起始时刻坐标系的，此时ieskf迭代更新的时候，先验位姿是lidar结束时刻的位姿，但是点云却是在起始时刻下，这样感觉有点怪。（ps：虽然通过ieskf的迭代，最终是能够将在lidar结束时刻的先验位姿更新到lidar起始时刻）。

lsd修改了fastlio后，imu预测的是上一帧结束时刻的位姿(其实也就是当前帧起始时刻的位姿)，你可以通过打印imu队列，可以看到每次迭代更新时，一直缓存着大约10个imu(imu为100hz)

如果是按您说的这样，那就没啥问题了。但是我看了代码，在sync_packages()等地方没看到对应的逻辑？您方便指出一下么？

sync_packages中的lidar_mean_scantime一直是0 (每个点的时间也就是point.curvature在前面都被置0了)，所以每次调用时lidar_end_time = lidar_start_time

[minghongss]

明白了，谢谢您的解答。