YJZLuckyBoy/liorf

May you check your messages on bilibili? Need some help for running Mid360 with LIORF

LiuYMUNI opened this issue · 11 comments

LiuYMUNI commented

Dear YJZLuckzBoy, I send you a message on bilibili, can you please reply me and help. Thanks a lot.

KimHyung commented

Hello, LiuYMUNI
I am also planning to use liorf slam with livox mid 360.

Did you find the livox mid 360 sensor setting parameter value?

LiuYMUNI commented
KimHyung commented

Hi LiuYMUNI

I figured out how to slam liorf with mid 360.

setp 1. (imageProjection.cpp)
add LivoxPointXTZIRT struct under struct VelodynePointXYZIRT

struct VelodynePointXYZIRT
{
    PCL_ADD_POINT4D
    PCL_ADD_INTENSITY;
    uint16_t ring;
    float time;
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
} EIGEN_ALIGN16;
POINT_CLOUD_REGISTER_POINT_STRUCT (VelodynePointXYZIRT,
    (float, x, x) (float, y, y) (float, z, z) (float, intensity, intensity)
    (uint16_t, ring, ring) (float, time, time)
)

struct LivoxPointXYZIRT
{
    PCL_ADD_POINT4D
    float intensity;
    uint8_t tag;
    uint8_t line;
    float time;
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
} EIGEN_ALIGN16;
POINT_CLOUD_REGISTER_POINT_STRUCT(LivoxPointXYZIRT, 
      (float, x, x)(float, y, y)(float, z, z)(float, intensity, intensity)
      (uint8_t, tag, tag)(uint8_t, line, line)(float, time, time)
)

and add var

pcl::PointCloud<PointXYZIRT>::Ptr laserCloudIn;
pcl::PointCloud<LivoxPointXYZIRT>::Ptr tmpLivoxCloudIn;`

void allocateMemory()
{
    laserCloudIn.reset(new pcl::PointCloud<PointXYZIRT>());
    tmpLivoxCloudIn.reset(new pcl::PointCloud<LivoxPointXYZIRT>());
    tmpOusterCloudIn.reset(new pcl::PointCloud<OusterPointXYZIRT>());
    tmpMulranCloudIn.reset(new pcl::PointCloud<MulranPointXYZIRT>());
    fullCloud.reset(new pcl::PointCloud<PointType>());

    resetParameters();
}

step 2. change cachePointCloud and ringFiled condition (imageProjection.cpp)

if (sensor == SensorType::VELODYNE)
{
    pcl::moveFromROSMsg(currentCloudMsg, *laserCloudIn);
}
else if (sensor == SensorType::LIVOX)
{
    pcl::moveFromROSMsg(currentCloudMsg, *tmpLivoxCloudIn);
    laserCloudIn->points.resize(tmpLivoxCloudIn->size());
    laserCloudIn->is_dense = tmpLivoxCloudIn->is_dense;
    for (size_t i = 0; i < tmpLivoxCloudIn->size(); i++)
    {
        auto &src = tmpLivoxCloudIn->points[i];
        auto &dst = laserCloudIn->points[i];
        dst.x = src.x;
        dst.y = src.y;
        dst.z = src.z;
        dst.intensity = src.intensity;
        dst.ring = src.line;
        dst.time = src.time;
    }
}

ringFlag = -1;
for (int i = 0; i < (int)currentCloudMsg.fields.size(); ++i)
{
    //added for livox
    if (currentCloudMsg.fields[i].name == "ring" || currentCloudMsg.fields[i].name == "line")
    {
        ringFlag = 1;
        break;
    }
}

step 3 change yaml param.(lio_sam_default.yaml)

sensor: livox                            # lidar sensor type, 'velodyne' or 'ouster' or 'livox' or 'robosense'
 N_SCAN: 4                                  # number of lidar channel (i.e., Velodyne/Ouster: 16, 32, 64, 128, Livox Horizon: 6)
Horizon_SCAN: 8000                          # lidar horizontal resolution (Velodyne:1800, Ouster:512,1024,2048, Livox Horizon: 4000)
downsampleRate: 1                           # default: 1. Downsample your data if too many points(line). i.e., 16 = 64 / 4, 16 = 16 / 1
point_filter_num: 1                         # default: 3. Downsample your data if too many points(point). e.g., 16: 1, 32: 5, 64: 8
lidarMinRange: 3.0                          # default: 1.0, minimum lidar range to be used
lidarMaxRange: 80.0                       # default: 1000.0, maximum lidar range to be used

I found out that the N_SCAN parameter of mid-360 is 4. Right now I'm looking for Horizon SCAN values.

When running with the Horizon SCAN = 4000, the drift became worse when rotating more than 70 degrees at a time. I am trying to find the optimal value through parameter tuning.

good
bad

  • ❤️1
LiuYMUNI commented
seajayshore commented

Thanks for these instructions for the Mid-360!

I have implemented them but it doesn't seem to work... Did you have to modify anything related to the frame_id to make things appear in rviz?

I am getting the same error as this: #16
But there is no obvious solution given in that thread...

KimHyung commented

I have not modified anything related to frame_id in the provided lio_sam_livox.yaml file.
I only modified the topics names and some extrinsic values.

liorf:

# Topics
pointCloudTopic: "/livox/lidar_192_168_20_104"               # Point cloud data
# pointCloudTopic: "/top/velodyne_points"               # Point cloud data
imuTopic: "/imu/data"                         # IMU data
odomTopic: "odometry/imu"                   # IMU pre-preintegration odometry, same frequency as IMU
gpsTopic: "/imu/nav_sat_fix"                   # GPS odometry topic from navsat, see module_navsat.launch file

# Frames
lidarFrame: "base_link"
baselinkFrame: "base_link"
odometryFrame: "odom"
mapFrame: "map"

# GPS Settings
useImuHeadingInitialization: true           # if using GPS data, set to "true"
useGpsElevation: true                      # if GPS elevation is bad, set to "false"
gpsCovThreshold: 2.0                        # m^2, threshold for using GPS data
poseCovThreshold: 25.0                      # m^2, threshold for using GPS data

# Export settings
savePCD: false                              # https://github.com/TixiaoShan/LIO-SAM/issues/3
savePCDDirectory: "/Downloads/LOAM/"        # in your home folder, starts and ends with "/". Warning: the code deletes "LOAM" folder then recreates it. See "mapOptimization" for implementation

# Sensor Settings
# sensor: velodyne                            # lidar sensor type, 'velodyne' or 'ouster' or 'livox' or 'robosense'
# N_SCAN: 32                                  # number of lidar channel (i.e., Velodyne/Ouster: 16, 32, 64, 128, Livox Horizon: 6)
# Horizon_SCAN: 1800                          # lidar horizontal resolution (Velodyne:1800, Ouster:512,1024,2048, Livox Horizon: 4000)
# downsampleRate: 2                           # default: 1. Downsample your data if too many points(line). i.e., 16 = 64 / 4, 16 = 16 / 1
# point_filter_num: 1                         # default: 3. Downsample your data if too many points(point). e.g., 16: 1, 32: 5, 64: 8
# lidarMinRange: 1.0                          # default: 1.0, minimum lidar range to be used
# lidarMaxRange: 100.0                       # default: 1000.0, maximum lidar range to be used

sensor: livox                            # lidar sensor type, 'velodyne' or 'ouster' or 'livox' or 'robosense'
N_SCAN: 4                                  # number of lidar channel (i.e., Velodyne/Ouster: 16, 32, 64, 128, Livox Horizon: 6)
Horizon_SCAN: 4000                          # lidar horizontal resolution (Velodyne:1800, Ouster:512,1024,2048, Livox Horizon: 4000)
downsampleRate: 1                           # default: 1. Downsample your data if too many points(line). i.e., 16 = 64 / 4, 16 = 16 / 1
point_filter_num: 1                         # default: 3. Downsample your data if too many points(point). e.g., 16: 1, 32: 5, 64: 8
lidarMinRange: 3.0                          # default: 1.0, minimum lidar range to be used
lidarMaxRange: 80.0                       # default: 1000.0, maximum lidar range to be used

# IMU Settings
imuType: 1                                  # 0: 6-axis  1: 9-axis
imuRate: 50.0
imuAccNoise: 3.9939570888238808e-03
imuGyrNoise: 1.5636343949698187e-03
imuAccBiasN: 6.4356659353532566e-05
imuGyrBiasN: 3.5640318696367613e-05
imuGravity: 9.80511
imuRPYWeight: 0.01

# Extrinsics: T_lb (lidar -> imu)
extrinsicTrans: [0.0, 0.0, 0.0]
extrinsicRot: [-1, 0, 0,
                0, 1, 0,
                0, 0, -1]

# This parameter is set only when the 9-axis IMU is used, but it must be a high-precision IMU. e.g. MTI-680
extrinsicRPY: [0, -1, 0,
                1, 0, 0,
                0, 0, 1]

# voxel filter paprams
mappingSurfLeafSize: 0.4                      # default: 0.4 - outdoor, 0.2 - indoor

# robot motion constraint (in case you are using a 2D robot)
z_tollerance: 1000                            # meters
rotation_tollerance: 1000                     # radians

# CPU Params
numberOfCores: 20                            # number of cores for mapping optimization
mappingProcessInterval: 0.1                  # seconds, regulate mapping frequency

# Surrounding map
surroundingkeyframeAddingDistThreshold: 1.0   # meters, regulate keyframe adding threshold
surroundingkeyframeAddingAngleThreshold: 0.2  # radians, regulate keyframe adding threshold
surroundingKeyframeDensity: 2.0               # meters, downsample surrounding keyframe poses   
surroundingKeyframeSearchRadius: 50.0         # meters, within n meters scan-to-map optimization (when loop closure disabled)
surroundingKeyframeMapLeafSize: 0.5           # downsample local map point cloud

# Loop closure
loopClosureEnableFlag: true
loopClosureFrequency: 1.0                     # Hz, regulate loop closure constraint add frequency
surroundingKeyframeSize: 50                   # submap size (when loop closure enabled)
historyKeyframeSearchRadius: 15.0             # meters, key frame that is within n meters from current pose will be considerd for loop closure
historyKeyframeSearchTimeDiff: 30.0           # seconds, key frame that is n seconds older will be considered for loop closure
historyKeyframeSearchNum: 25                  # number of hostory key frames will be fused into a submap for loop closure
loopClosureICPSurfLeafSize: 0.5               # downsample icp point cloud  
historyKeyframeFitnessScore: 0.3              # icp threshold, the smaller the better alignment

# Visualization
globalMapVisualizationSearchRadius: 1000.0    # meters, global map visualization radius
globalMapVisualizationPoseDensity: 10.0       # meters, global map visualization keyframe density
globalMapVisualizationLeafSize: 1.0           # meters, global map visualization cloud density




# Navsat (convert GPS coordinates to Cartesian)
navsat:
frequency: 50
wait_for_datum: false
delay: 0.0
magnetic_declination_radians: 0
yaw_offset: 0
zero_altitude: false
broadcast_utm_transform: false
broadcast_utm_transform_as_parent_frame: false
publish_filtered_gps: false

# EKF for Navsat
ekf_gps:
publish_tf: false
map_frame: map
odom_frame: odom
base_link_frame: base_link
world_frame: odom

frequency: 50
two_d_mode: false
sensor_timeout: 0.01
# -------------------------------------
# External IMU:
# -------------------------------------
imu0: imu_correct
# make sure the input is aligned with ROS REP105. "imu_correct" is manually transformed by myself. EKF can also transform the data using tf between your imu and base_link
imu0_config: [false, false, false,
              true,  true,  true,
              false, false, false,
              false, false, true,
              true,  true,  true]
imu0_differential: false
imu0_queue_size: 50 
imu0_remove_gravitational_acceleration: true
# -------------------------------------
# Odometry (From Navsat):
# -------------------------------------
odom0: odometry/gps
odom0_config: [true,  true,  true,
                false, false, false,
                false, false, false,
                false, false, false,
                false, false, false]
odom0_differential: false
odom0_queue_size: 10

#                            x     y     z     r     p     y   x_dot  y_dot  z_dot  r_dot p_dot y_dot x_ddot y_ddot z_ddot
process_noise_covariance: [  1.0,  0,    0,    0,    0,    0,    0,     0,     0,     0,    0,    0,    0,    0,      0,
                              0,    1.0,  0,    0,    0,    0,    0,     0,     0,     0,    0,    0,    0,    0,      0,
                              0,    0,    10.0, 0,    0,    0,    0,     0,     0,     0,    0,    0,    0,    0,      0,
                              0,    0,    0,    0.03, 0,    0,    0,     0,     0,     0,    0,    0,    0,    0,      0,
                              0,    0,    0,    0,    0.03, 0,    0,     0,     0,     0,    0,    0,    0,    0,      0,
                              0,    0,    0,    0,    0,    0.1,  0,     0,     0,     0,    0,    0,    0,    0,      0,
                              0,    0,    0,    0,    0,    0,    0.25,  0,     0,     0,    0,    0,    0,    0,      0,
                              0,    0,    0,    0,    0,    0,    0,     0.25,  0,     0,    0,    0,    0,    0,      0,
                              0,    0,    0,    0,    0,    0,    0,     0,     0.04,  0,    0,    0,    0,    0,      0,
                              0,    0,    0,    0,    0,    0,    0,     0,     0,     0.01, 0,    0,    0,    0,      0,
                              0,    0,    0,    0,    0,    0,    0,     0,     0,     0,    0.01, 0,    0,    0,      0,
                              0,    0,    0,    0,    0,    0,    0,     0,     0,     0,    0,    0.5,  0,    0,      0,
                              0,    0,    0,    0,    0,    0,    0,     0,     0,     0,    0,    0,    0.01, 0,      0,
                              0,    0,    0,    0,    0,    0,    0,     0,     0,     0,    0,    0,    0,    0.01,   0,
                              0,    0,    0,    0,    0,    0,    0,     0,     0,     0,    0,    0,    0,    0,      0.015]
  • 👍1
seajayshore commented

Thanks for that! I actually found some other changes that I made were the reason it was not working!

The changes you shared for the Mid-360 seem to work fine! Thanks! :-D

So far I have found the performance of liorf with the Mid-360 to be a bit strange - the start of mapping is very shaky and the loop closure feature seems to keep pulling the odometry backwards even though the lidar is stationary... After ~20seconds it becomes more stable but is still not great. (Not better than Fast-LIO2 for example)

This is only my testing from 2 recordings so nothing conclusive! Maybe I am don't initialise the system properly or something...

Is your experience similar?

  • 👍1
ALazyLearner commented

I follow @KimHyung 's instruction and make it to do liorf relocalization. Thanks a lot for that.
Additionally, I still do not know how to set Horizon_SCAN value. I echo the lidar data in the command line and found there are about 20000 points per frame, maybe I should set Horizon_SCAN=5000 or 6000?

zhangxu0089 commented

livox line is not the same meaning of the mechanical lidar, actually is a block ,how can it work ?

YJZLuckyBoy commented

@ALazyLearner @zhangxu0089 Actually, liorf does not need to know the ring of each point in the point cloud data. 'ring' is only used to sparsify the point cloud. If you do not know how to set the 'N_SCAN' and 'Horizon_SCAN', you only need to ensure that N_SCAN * Horizon_SCAN > N_Points, which refers to the number of point clouds in per frame.

YJZLuckyBoy commented

Thanks for that! I actually found some other changes that I made were the reason it was not working!

The changes you shared for the Mid-360 seem to work fine! Thanks! :-D

So far I have found the performance of liorf with the Mid-360 to be a bit strange - the start of mapping is very shaky and the loop closure feature seems to keep pulling the odometry backwards even though the lidar is stationary... After ~20seconds it becomes more stable but is still not great. (Not better than Fast-LIO2 for example)

This is only my testing from 2 recordings so nothing conclusive! Maybe I am don't initialise the system properly or something...

Is your experience similar?

With mid-360, maybe you can try the following parameters:

mappingSurfLeafSize: 0.15