Point cloud is abnormal with direct method

[CanCanZeng]

Hi, thank you for sharing this great job! I use a velodyne VLP-32C lidar with external imu. For a outdoor data, if I set extract feature to true, the program runs well, but has small drift in z axis. Then I set extract feature to false, the trajectory is still right, but the pointcloud in rviz is abnormal. The points far from ground are lost, I mean, only the base of a building is remain, points about 2 meters high are gone. The first image is with extract feature on, and second is off. Is this a normal phenomenon？

[XW-HKU]

have you ever set the line number?

[CanCanZeng]

Yes, the paramters are

common:
    lid_topic:  "/velodyne_points"
    imu_topic:  "/imu/internal/imu"
    time_sync_en: false         # ONLY turn on when external time synchronization is really not possible

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

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_T: [ -0.02165215, 0.02611613, 0.04081799 ]  # 这里是 T_base_lidar
    extrinsic_R: [ 0.999841, -0.0162439, -0.00735231,
                   0.0162228, 0.999864, -0.00291686,
                   0.0073987, 0.00279712, 0.999969 ]

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

There is something unusual that the lines are not evenly distributed. See https://icave2.cse.buffalo.edu/resources/sensor-modeling/VLP32CManual.pdf page 57

[CanCanZeng]

I set the point ring number myself according to this mannual.

[XW-HKU]

Since I don't have any vlp-32 dataset, I don't know if there is any problem in FAST-LIO when working with it. Therefore could you send me a little piece of data sequence to let me check?

[XW-HKU]

I set the point ring number myself according to this mannual.

how do you set the ring？

[CanCanZeng]

I use an unordered_map to set the ring:

const std::unordered_map<int, uint16_t> angle_to_ringNumber { 
                                                        {-250,0},   {-156, 1},   {-113, 2},   {-88, 3},
                                                        {-73, 4},   {-61,  5},   {-53,  6},   {-47, 7},
                                                        {-40, 8},   {-37,  9},   {-33,  10},  {-30, 11},
                                                        {-27, 12},  {-23,  13},  {-20,  14},  {-17, 15},
                                                        {-13, 16},  {-10,  17},  {-7,   18},  {-3,  19},
                                                        {0,   20},  {3,    21},  {7,    22},  {10,  23},
                                                        {13,  24},  {17,   25},  {23,   26},  {33,  27},
                                                        {47,  28},  {70,   29},  {103,  30},  {150, 31}  // 所有数值是正常角度值乘以10再取整
                                                        };

// 计算竖直方向上的角度（雷达的第几线）
float verticalAngle = atan2(z, sqrt(x * x + y * y)) * 180 / M_PI;
int ang = std::round(verticalAngle * 10);
uint16_t  ring = angle_to_ringNumber.at(ang);

[CanCanZeng]

You can download my dataset from google drive: https://drive.google.com/file/d/1ZRXnHBDMe_P4h2Ixn0HbKfuZgb9p2iXW/view?usp=sharing

[XW-HKU]

Actually, the ring will has no influences in the raw point process if the point timestamps are give (it is given in your dataset). The problem is that the "point_filter_num" parameter in mapping_velodyne.launch file should not be set to 4. I don't know why, but I tried 1 and 2, the point cloud and its size looks normal. I already updated the codes, please update to the newest FAST-LIO and set point_filter_num to 1 and try again. Thanks for your dataset, it helps a lot.

[XW-HKU]

And one more thing, the extrinsic in velodyne.yaml should be the LiDAR's pose based on the IMU (IMU is the origin). You may need to double-check it. If you already have an extrinsic of IMU based on LiDAR (LiDAR is the origin), such as (T, R). The extrinsic defined in FAST-LIO can be computed as:

extrinsic_T = - T; extrinsic_R = R.transpose();

[CanCanZeng]

Thanks for reminding me! I calibrate the rotation between lidar and imu, but not for translation, which should be a small value. I run vins-mono to get the trajectory of imu and then run a-loam to get the trajectory of lidar, then use this function to find the rotation https://github.com/chennuo0125-HIT/lidar_imu_calib/blob/6b578685aeb10de9f0e419444d2cc3e5f420e648/lidar_imu_calib/src/lib/calibExRLidar2Imu.cpp#L167 it's a bit complicated since the device is not at my hand, so I need to find a self-calibration method. How can you judge the result of calibration? Is there an intuitive method to check the quality of calibration? Thank you in advance!

[CanCanZeng]

Actually, the ring will has no influences in the raw point process if the point timestamps are give (it is given in your dataset). The problem is that the "point_filter_num" parameter in mapping_velodyne.launch file should not be set to 4. I don't know why, but I tried 1 and 2, the point cloud and its size looks normal. I already updated the codes, please update to the newest FAST-LIO and set point_filter_num to 1 and try again. Thanks for your dataset, it helps a lot.

The point cloud now is normal. I wonder how do you find the cause so quickly? I have no idea when facing this kind of problem

[XW-HKU]

Actually, the ring will has no influences in the raw point process if the point timestamps are give (it is given in your dataset). The problem is that the "point_filter_num" parameter in mapping_velodyne.launch file should not be set to 4. I don't know why, but I tried 1 and 2, the point cloud and its size looks normal. I already updated the codes, please update to the newest FAST-LIO and set point_filter_num to 1 and try again. Thanks for your dataset, it helps a lot.

The point cloud now is normal. I wonder how do you find the cause so quickly? I have no idea when facing this kind of problem

May be just because i am familier with codes.

[CanCanZeng]

And one more thing, the extrinsic in velodyne.yaml should be the LiDAR's pose based on the IMU (IMU is the origin). You may need to double-check it. If you already have an extrinsic of IMU based on LiDAR (LiDAR is the origin), such as (T, R). The extrinsic defined in FAST-LIO can be computed as:

extrinsic_T = - T; extrinsic_R = R.transpose();

Hi, is this a mistake? it should be extrinsic_T = -R.transpose() * T?