lidar_align联合标定激光雷达和IMU外参
lidar_align联合标定激光雷达和IMU外参
运行LIO-SAM之前需要联合标定激光雷达和IMU的外参，官方推荐的方案是浙大的开源联合标定方案：lidar_IMU_calib
发现采用苏黎世理工的方案更加简单粗暴一点：lidar_align，具体操作步骤如下：
1.下载源码
建议下载修正过后的代码：lidar_align_wwtx
建立工作空间catkin_ws，按照以下方式进行编译：
cd catkin_ws
catkin_make
source devel/setup.bash
roslaunch lidar_align lidar_align.launch
注意，提前录制bag包，录制的包中只保留imu和lidar的话题，并且，在启动程序前，找到launch文件，修改bag所在的路径，不用修改任何话题，程序会自己找到话题。
2.源码编译时遇到的错误
错误一：编译时出现Could not find NLOPTConfig.cmake
解决办法：找到这个文件并将其放入到工程目录下，并在CMakeLists.txt里加上这样一句话：
list(APPEND CMAKE_FIND_ROOT_PATH ${PROJECT_SOURCE_DIR})
set (CMAKE_PREFIX_PATH "/usr/local/lib/cmake/nlopt")
错误二：No odom messages found
因为这里面有odom里程计的数据而没有加入IMU数据，需要自己在loader.cc里进行改写。于是上网搜索了一下，有位博主自己改写了：链接或者下载上述推荐代码，已经改写完成：lidar_align
Active Transformation Vector (x,y,z,rx,ry,rz) from the Pose Sensor Frame to the Lidar Frame:
[-0.0608575, -0.0758112, 0.27089, 0.00371254, 0.00872398, 1.60227]
//接下来的矩阵是需要关注的，前3*3是用于转换矩阵，最后一列是平移矩阵，在lio_sam中的param.yaml中可以找到
Active Transformation Matrix from the Pose Sensor Frame to the Lidar Frame:
-0.0314953 -0.999473 0.0078319 -0.0608575
0.999499 -0.0314702 0.00330021 -0.0758112
-0.003052 0.00793192 0.999964 0.27089
0 0 0 1
Active Translation Vector (x,y,z) from the Pose Sensor Frame to the Lidar Frame:
[-0.0608575, -0.0758112, 0.27089]
Active Hamiltonen Quaternion (w,x,y,z) the Pose Sensor Frame to the Lidar Frame:
[0.69588, 0.00166397, 0.00391012, 0.718145]
Time offset that must be added to lidar timestamps in seconds:
0.00594481
ROS Static TF Publisher: <node pkg="tf" type="static_transform_publisher" name="pose_lidar_broadcaster" args="-0.0608575 -0.0758112 0.27089 0.00166397 0.00391012 0.718145 0.69588 POSE_FRAME LIDAR_FRAME 100" />
3.编译并运行LIO-SAM
安装并编译好LIOSAM。
遇到的错误：
Error: GTSAM was built against a different version of Eigen。
这个问题是GTSAM自带的eigen库和之前安装的eigen库的版本不同导致的。
解决办法：
修改gtsam下的CMakeLists.txt文件中的内容，在if(GTSAM_USE_SYSTEM_EIGEN)前面加上set(GTSAM_USE_SYSTEM_EIGEN ON)，使得gtsam使用自己安装的eigen而不是它自带的eigen。然后重新编译gtsam，之后再编译LIO-SAM，问题解决。
/usr/bin/ld: 找不到 -lBoost::timer
找到CMake.Lists文件，在第27行后添加：
find_package(Boost REQUIRED COMPONENTS timer)
在运行前需要修改一下param.yaml文件：
lio_sam:
# Topics
# 修改话题激光雷达和IMU话题
pointCloudTopic: "points_raw" # Point cloud data
imuTopic: "imu_correct" # IMU data
odomTopic: "odometry/imu" # IMU pre-preintegration odometry, same frequency as IMU
gpsTopic: "odometry/gpsz" # GPS odometry topic from navsat, see module_navsat.launch file
# Frames
lidarFrame: "base_link"
baselinkFrame: "base_link"
odometryFrame: "odom"
mapFrame: "map"
# GPS Settings
useImuHeadingInitialization: false # if using GPS data, set to "true"
useGpsElevation: false # 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, either 'velodyne' or 'ouster'
N_SCAN: 16 # number of lidar channel (i.e., 16, 32, 64, 128)
Horizon_SCAN: 1800 # lidar horizontal resolution (Velodyne:1800, Ouster:512,1024,2048)
downsampleRate: 1 # default: 1. Downsample your data if too many points. i.e., 16 = 64 / 4, 16 = 16 / 1
lidarMinRange: 1.0 # default: 1.0, minimum lidar range to be used
lidarMaxRange: 1000.0 # default: 1000.0, maximum lidar range to be used
# IMU Settings 这个需要改变IMU噪声和重力加速度
imuAccNoise: 3.9939570888238808e-03
imuGyrNoise: 1.5636343949698187e-03
imuAccBiasN: 6.4356659353532566e-05
imuGyrBiasN: 3.5640318696367613e-05
imuGravity: 9.80511
imuRPYWeight: 0.01
# Extrinsics (lidar -> IMU)这个是上述标定的参数
extrinsicTrans: [0.0, 0.0, 0.0] #注意这个是平移向量，标定的4*4矩阵的最后一列
# extrinsicRot: [-1, 0, 0,
# 0, 1, 0,
# 0, 0, -1]
# extrinsicRPY: [0, 1, 0,
# -1, 0, 0,
# 0, 0, 1]
extrinsicRot: [1, 0, 0, #这个是4*4的前面的3*3
0, 1, 0,
0, 0, 1]
extrinsicRPY: [1, 0, 0, #这个是4*4的前面的3*3
0, 1, 0,
0, 0, 1]
# LOAM feature threshold
edgeThreshold: 1.0
surfThreshold: 0.1
edgeFeatureMinValidNum: 10
surfFeatureMinValidNum: 100
# voxel filter paprams
odometrySurfLeafSize: 0.4 # default: 0.4 - outdoor, 0.2 - indoor
mappingCornerLeafSize: 0.2 # default: 0.2 - outdoor, 0.1 - indoor
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: 4 # number of cores for mapping optimization
mappingProcessInterval: 0.15 # 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)
# 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
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: true
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]
参考
激光雷达和IMU联合标定并运行LIOSAM