large drift when up and down the stairs

[CanCanZeng]

Hi, I use the latest code (dcc38885ac80adf3eb91028fb7f9834573ec7464), and my device is calibrated by lidar_align, and has well time synchronization. The algorithm run very well in outdoor scene. But when I test it in indoor scene, especially when I go up or down stairs, the odometry may drift suddenly. My config is like below:

preprocess:
    lidar_type: 2                # 1 for Livox serials LiDAR, 2 for Velodyne LiDAR, 3 for ouster LiDAR, 
    scan_line: 32
    blind: 0.5

mapping:
    acc_cov: 0.1
    gyr_cov: 0.1
    b_acc_cov: 0.0001
    b_gyr_cov: 0.0001
    fov_degree:    180
    det_range:     100.0
    extrinsic_est_en:  false      # true: enable the online estimation of IMU-LiDAR extrinsic
    extrinsic_T: [ 0.040121, -0.00958998, -0.0345577 ] 
    extrinsic_R: [ 0.999979, -0.0045052, 0.00468756,
                   0.00449743, 0.999988, 0.00166568,
                   -0.00469502, -0.00164457, 0.999988 ]

publish:
    path_en:  true
    scan_publish_en:  true       # false: close all the point cloud output
    dense_publish_en: true       # false: low down the points number in a global-frame point clouds scan.
    scan_bodyframe_pub_en: true  # true: output the point cloud scans in IMU-body-frame

pcd_save:
    pcd_save_en: false
    interval: -1                 # how many LiDAR frames saved in each pcd file; 
                                 # -1 : all frames will be saved in ONE pcd file, may lead to memory crash when having too much frames.

and I try to increse iteration and decrease filter size:

    <param name="feature_extract_enable" type="bool" value="0"/>
    <param name="point_filter_num" type="int" value="1"/>
    <param name="max_iteration" type="int" value="10" />
    <param name="filter_size_surf" type="double" value="0.1" />
    <param name="filter_size_map" type="double" value="0.1" />
    <param name="cube_side_length" type="double" value="1000" />

this will make the odometry runs longer, but it will still drift at second time or third time when I walk into the stairwell. The stair is only about 1.2 meters wide, so I set blind to 0.5, otherwiese the odometry will drift more quickly. I do not care about the running time, do you have any suggestion on my situation?

below is the picture of the stair

[XW-HKU]

which lidar？

the minimum range of Velodyne is 0.9m，so in this environment，there may not has enough effective lidar points. without enough lidar points, fast-lio cannot work. I suggest you check how many points come from topic "cloud_registered".

[CanCanZeng]

well, the point cloud is normal, as you can see the picture below, the red rectrangular show the affected points, since the points are too near from the wall. But there are still many valid points, above 20k points per frame.

one more weird thing is that, I down stairs at first and then up stairs. If I start from begin, the odometry will drift when I up stairs, but if I start from the middle, i.e., skip the down stairs time, the odometry runs well.

I upload my rosbag data and the log file to google drive: https://drive.google.com/drive/folders/1snXiAWdvae-lKhqfv9D1N1djTsENtHjR?usp=sharing

[CanCanZeng]

I find a bug that in my point cloud, the uint of time channel is second, but it seems that in the code, the uint is us. I will change that and test again.

[CanCanZeng]

I tried three method:

1. change this line https://github.com/hku-mars/FAST_LIO/blob/dcc38885ac80adf3eb91028fb7f9834573ec7464/src/preprocess.cpp#L325 to added_pt.curvature = pl_orig.points[i].time * 1000.0; // units: ms

2. direct set added_pt.curvature to 0

3. use ring to calculate time instead of using the provide time, and change the omega_l to 7.21 since my device runs in 20Hz mode.

the performance is 1 > 2 > 3, but no much difference.

[XW-HKU]

I checked your bag, there are two problems:

1. The point cloud undistortion works not well, this mainly because of Extrinsic not rigth or time unsync or point timestamps not right. It should be noted that the timestamp of every lidar point in your data should be the us (1e-6 second) for velodyne lidar and ns (1e-9 second) for livox. If everything is right, the wall should be very thick, not like this picture:

2. As I have metioned, the point is too few and deneration (only 20 points in this condition) when you walk into the stair room.

[CanCanZeng]

Thank you for help me checking the data, it's very kind of you!

1. how do you get the first image? As I already find that the uint of my data's time is second, I change the code here https://github.com/hku-mars/FAST_LIO/blob/dcc38885ac80adf3eb91028fb7f9834573ec7464/src/preprocess.cpp#L325 and get a slightly better result than original. In this mode, the undistorted point is not that terrible than the picture you showed. And I find that the rviz has perspective projection effect, so when you look closer to the points of wall, and rotate the view, the wall is not very thick. And actually the "wall" is not wall, it's window, so it should not very thin.

But you give a very intuitive method to see if the undistrotion is right, thus judge if extrinsic or something is right, it's very useful!

2. how do you get the second picture? The white points are cloud_registered ? I add a line here https://github.com/hku-mars/FAST_LIO/blob/04b99f05e5aafddf6d86f4047e9e773bf96dbebd/src/laserMapping.cpp#L491 std::cout << "cloud_registered:" << laserCloudmsg.width << std::endl; the output is always above 10K if I set blind to 1. Below is a screenshot when I walk in the stair room. The big colored points are cloud_registered

[XW-HKU]

1. if your input is in unit second, then you should use:

added_pt.curvature = pl_orig.points[i].time * 1000.0; // units: ms

2. Maybe most of the points is (0,0,0), you just uncomment the line: https://github.com/hku-mars/FAST_LIO/blob/04b99f05e5aafddf6d86f4047e9e773bf96dbebd/src/laserMapping.cpp#L914 And I mean the point number when the degeneration happends not the normal condition.

3. Update to the latest code and try again, I just update the codes.

[CanCanZeng]

I upload one frame of lidar https://drive.google.com/file/d/1p_uTHU_xiPXPS2UmPnajckM8IarBmTcJ/view?usp=sharing you can open it in CloudCompare. when you drag it in the window of CloudCompare, select Scalar field as ring, then you can see below picture, it shows that the down side ring is 0 and the top is 31. (using Point Picking tool can show the scalar field number)

And then drag the ply file into the window again, this time select Scalar field as time. Then you can see the picture below, this can give you the point's X Y Z coord with time. It seems that the time is right, from begin 0 to end 0.05.

I'm just sharing a method to debug the data, hoping it will be usefull to others :smile:

[XW-HKU]

I upload one frame of lidar https://drive.google.com/file/d/1p_uTHU_xiPXPS2UmPnajckM8IarBmTcJ/view?usp=sharing you can open it in CloudCompare. when you drag it in the window of CloudCompare, select Scalar field as ring, then you can see below picture, it shows that the down side ring is 0 and the top is 31. (using Point Picking tool can show the scalar field number)

And then drag the ply file into the window again, this time select Scalar field as time. Then you can see the picture below, this can give you the point's X Y Z coord with time. It seems that the time is right, from begin 0 to end 0.05.

I'm just sharing a method to debug the data, hoping it will be usefull to others 😄

Great！Thanks!

[XW-HKU]

I upload one frame of lidar https://drive.google.com/file/d/1p_uTHU_xiPXPS2UmPnajckM8IarBmTcJ/view?usp=sharing you can open it in CloudCompare. when you drag it in the window of CloudCompare, select Scalar field as ring, then you can see below picture, it shows that the down side ring is 0 and the top is 31. (using Point Picking tool can show the scalar field number)

And then drag the ply file into the window again, this time select Scalar field as time. Then you can see the picture below, this can give you the point's X Y Z coord with time. It seems that the time is right, from begin 0 to end 0.05.

I'm just sharing a method to debug the data, hoping it will be usefull to others smile

I just check that most of the points are in 2 meter, I think it is hard to deal with this condition in FAST-LIO's current version, we may better support this environment in the future version.

[CanCanZeng]

It's very kind of you to do that! But I don't understand, if I set blind to 0.5 and set filter_size_surf and filter_size_map to 0.1, why won't fast-lio2 handle this environment? With this configuration and the latest code(94973cb309de0cc73dab1e5b4fbe8e7be2cc89bd), the effect number is always very large, but the odometry will still fail when I up stairs (success in down stairs and it seems that the map is very accurate).

[XW-HKU]

It's very kind of you to do that! But I don't understand, if I set blind to 0.5 and set filter_size_surf and filter_size_map to 0.1, why won't fast-lio2 handle this environment? With this configuration and the latest code(94973cb), the effect number is always very large, but the odometry will still fail when I up stairs (success in down stairs and it seems that the map is very accurate).

The main reason is that there are too many mismatch in very small scene. Most of LiDAR's measurements noise in close area will grow. Even with the same noises, the mismatch probability will also grow in close distances. FAST-LIO 3 will discuss these uncertainty, in the future.

[yuancaimaiyi]

@CanCanZeng 激光视觉通吃呀,牛！

[kxch928925819]

视觉还需要等徐博士发布R3Live，我是想试试geoslam的采集方式，运用fast-lio，因为买不到avia，搞个二手的velodyne试试手气！ 有兴趣一起玩！

------------------ Original ------------------ From: 李城（大橙子） @.> Date: Mon,Oct 11,2021 4:03 PM To: hku-mars/FAST_LIO @.> Cc: Subscribed @.***> Subject: Re: [hku-mars/FAST_LIO] large drift when up and down the stairs (#65)

@CanCanZeng 激光视觉通吃呀,牛！

— You are receiving this because you are subscribed to this thread. Reply to this email directly, view it on GitHub, or unsubscribe. Triage notifications on the go with GitHub Mobile for iOS or Android.

[CanCanZeng]

@CanCanZeng 激光视觉通吃呀,牛！

都只懂点皮毛

[stale[bot]]

This issue has been automatically marked as stale because it has not had recent activity. It will be closed if no further activity occurs. Thank you for your contributions.