Unstable z-direction pose estimation

[YangSiri]

Hi, @TixiaoShan

I have been testing this system in various scenarios such as a daily crowded street. But I found out there may be large offsets mainly along the z-direction of the final pose. I thought it would be surf-optimization problems. So I tried changing the amount of /flat feature points which are on the ground, decreasing the threshold of converging, or increasing the number of iterations during /Odom and /mapping(sometimes it won't converge within the iterations). They didn't work out well. I'm not saying this system has such problems. Maybe it is due to my data and scenes. All I need is some advice on enhancing the stability of pose estimation in the z-direction. How to improve it? Could you please give me some comments on this?

Some examples:

Looking forward to your help THANKS

[TixiaoShan]

Are you using IMU with the lidar? Both LOAM and LeGO-LOAM uses IMU roll and pitch data to correct the final pose. If the IMU is not mounted in ROS standard or not aligned with lidar, it will cause significant drift.

[YangSiri]

No, @TixiaoShan

Laser points are the only input.

[TixiaoShan]

@YangSiri How is your lidar mounted?

If you are not using IMU, A-LOAM might run better than LeGO-LOAM.

[YangSiri]

Actually no, @TixiaoShan

My VLP-16 is normally horizontally mounted above ground at around 1m. For my data, A-LOAM performs not better. So you strongly recommend using the IMU to solve this kind of problem especially running on the terrain with a slightly changing slope?

[TixiaoShan]

Point cloud from lidar is always skewed (or distorted). Since you are not using an IMU, the point cloud deskew is not perfectly done in all the methods. Using skewed point cloud for odometry estimation will eventually cause a large drift. That's why all the methods recommend using an IMU for point cloud deskew and transformation initial guess. Did you try LIO-mapping? It's a more recent and tightly coupled method but it requires an IMU.

[YangSiri]

Yes, @TixiaoShan

Thank you for your help. I will think more about what you said. But why does it only happens at z-direction? I'm still confused. Yes, I think LIO depends even more on the IMU.

[TixiaoShan]

Yes, @TixiaoShan

Thank you for your help. I will think more about what you said. But why does it only happens at z-direction? I'm still confused. Yes, I think LIO depends even more on the IMU.

According to my own experience, the more drift along Z is caused by the sensor itself. It only has 16 channels along Z direction. Compared with the 1800 horizontal measurements, 16 is too small!

[YangSiri]

Yeah, @TixiaoShan

That's an interesting explanation.lol Thank you very much!

[TixiaoShan]

@YangSiri Another "hacking" method just comes to my mind to fix the z drift problem if you use LeGO-LOAM for your own pose estimation tasks. However, this is only suitable if the robot only drives in a relatively flat area.

You can add these lines to the transformUpdate() function in mapOptimization.cpp. These lines will constraint the final system transformation.

// pitch
        if (transformTobeMapped[0] < -rotation_tollerance)
            transformTobeMapped[0] = -rotation_tollerance;
        if (transformTobeMapped[0] > rotation_tollerance)
            transformTobeMapped[0] = rotation_tollerance;
// roll
        if (transformTobeMapped[2] < -rotation_tollerance)
            transformTobeMapped[2] = -rotation_tollerance;
        if (transformTobeMapped[2] > rotation_tollerance)
            transformTobeMapped[2] = rotation_tollerance;
// z
        if (transformTobeMapped[4] < -z_tollerance)
            transformTobeMapped[4] = -z_tollerance;
        if (transformTobeMapped[4] > z_tollerance)
            transformTobeMapped[4] = z_tollerance;

[YangSiri]

Thanks, @TixiaoShan

I'll try on that. But i think thresholding is still not useful though driving on a flat road but with a slightly changing slope? Sometimes it has to be a relatively large deltaZ.

[TixiaoShan]

Thanks, @TixiaoShan

I'll try on that. But i think thresholding is still not useful though driving on a flat road but with a slightly changing slope? Sometimes it has to be a relatively large deltaZ.

You are right. The usage of thresholding is very limited.