相机、雷达和IMU参数标定

最新推荐文章于 2024-05-13 17:04:23 发布

MGWT

最新推荐文章于 2024-05-13 17:04:23 发布

阅读量576

点赞数 1

文章标签：笔记相机

本文链接：https://blog.csdn.net/weixin_45646976/article/details/134850895

版权

起因

由于项目需要，新买的相机、Lidar和IMU都是自己手动装上去的，需要进行标定。海康的摄像头，速腾16线激光雷达和LPMS-IG1的IMU。

参考1： SLAM各传感器的标定总结：Camera/IMU/LiDAR ，基本上直接参考这个，就能完成所有参数标定，但是对于Lidar和IMU标定比较费劲，参考下面这个。

参考2：激光雷达和IMU联合标定包 lidar_IMU_calib 基于RS-32的扩展以及 ubuntu20.04使用浙大开源包lidar_IMU_calib，这两个是对Lidar和IMU的标定。

内部参数标定计划：

相机内参标定：https://github.com/ethz-asl/kalibr

IMU内参标定 ：https://github.com/gaowenliang/imu_utils

外部参数标定计划：

相机和IMU标定： https://github.com/ethz-asl/kalibr

lidar和IMU标定：https://github.com/hku-mars/LiDAR_IMU_Init 或者 https://github.com/APRIL-ZJU/lidar_IMU_calib

相机和Lidar标定：https://github.com/XidianLemon/calibration_camera_lidar 或者 https://github.com/ankitdhall/lidar_camera_calibration

第三个标定比较麻烦，通过上面两个就可以计算出相机和雷达的外参数，这里就不标定了。

实现过程：

各种传感器的SDK安装：

首先IMU的SDK安装: https://www.alubi.cn/lpms-ig1-series/

相机SDK安装：参考 海康威视在linux下使用笔记——ros驱动相机

Lidar的SDK安装：官方手册或者 速腾聚创16线激光雷达ros驱动安装与rviz点云展示说明

数据录制：

相机内参：（除了Lidar和IMU，其他直接看参考文章1）

对准走廊的标定靶，录制数据时上下左右前后平移和旋转各三次，最后还有无规则运动加强鲁棒性。

rosbag record /image_raw -O images.bag

source devel/setup.bash
rosrun kalibr kalibr_calibrate_cameras --target april_6x6_24x24mm.yaml --bag images.bag --bag-from-to 5 20 --models pinhole-radtan --topics /image_raw --show-extraction

IMU内参标定：（建议看参考1）

录制imu.bag让IMU静止不动30-120分钟，越久越好，录制IMU的bag

rosbag play -r 200 imu_utils/imu.bag  #(这里要写你录制的包的路径，200倍速播放)
source ./devel/setup.bash
roslaunch imu_utils A3.launch #示例数据

注意这里的A3.launch文件包含一些参数，如IMU话题和名称，以及标定时长，按需修改：

相机与IMU联合标定:（建议看参考1）

教程给的推荐频率是相机20Hz，IMU 200Hz。订阅两个消息，图像和imu的topic：

录制的时候尽量运动的平滑一些，速度太快相机得到的图像质量太差，也不可太慢要激励imu各个轴，作者提供了一个数据采集参考的演示视频，总的来说就是前后上下左右平移和旋转各三次，以及一段无规则运动（注意别运动太剧烈）。标定板打印完全，录制数据包时相机距离推荐0.8m

source devel/setup.bash
rosrun kalibr kalibr_calibrate_imu_camera --target dynamic/april_6x6.yaml --bag dynamic/dynamic.bag --cam dynamic/camchain_mono.yaml --imu dynamic/imu_adis16448.yaml --bag-from-to 5 45 --imu-models scale-misalignment --timeoffset-padding 0.1 --show-extraction

-dynamic/dynamic.bag：录制的数据包路径；
-bag-from-to 5 45 从bag的第5s读到第45s；
-target april_6x6.yaml：标定板参数文件；

• --cam dynamic/camchain.yaml：前面标定的相机内参(直接用这个数据标的不太对)，如果不是用kalibr标定的，可能要要按照下面格式修改一个文件：

cam0:
cam_overlaps: [1, 3]
camera_model: pinhole
distortion_coeffs: [-0.0016509958435871643, 0.02437222940989351, -0.03582816956989852,
0.019860839087717054]
distortion_model: equidistant
intrinsics: [461.487246372674, 460.1113992557959, 356.39105303227853, 231.15719697054647]
resolution: [752, 480]
rostopic: /cam0/image_raw

• --imu dynamic/imu_adis16448.yaml：使用前面方法标定的IMU内参，按照格式填写

-imu-models scale-misalignment：IMU的参数模型
-timeoffset-padding 0.1：这个值越大,标定的运行时间会越长

生成几个结果文件：

• 相机内外参以及和imu之间的时间同步差（IMU相对于camera的延时）：

• IMU内外参：

LiDAR与IMU联合标定：（建议结合本文看参考2）

这里使用浙大的标定算法，主要就是需要对不同雷达修改代码，就能完成标定。

对速腾16线激光雷达的修改：https://gitee.com/deep-blue-hero/lidar_-imu_calib_for_-rs-16 ，这里是我的修改。如果不是速腾16线，建议按照下面一步一步修改即可完成标定。

下面是对原版代码的编译过程。

初始化子项目的时候：运行build_submodules.sh文件时候如果出现pangolin出现py….11的问题，可以直接在cmake中取消这部分的编译，然后删除这个功能包，然后再sh文件中注释更新部分，直接编译即可。最好还是直接使用本机安装的。

#!/usr/bin/env bash

MYPWD=$(pwd)

#https://www.cnblogs.com/robinunix/p/11635560.html
set -x
set -e

# https://stackoverflow.com/a/45181694
NUM_CORES=`getconf _NPROCESSORS_ONLN 2>/dev/null || sysctl -n hw.ncpu || echo 1`
NUM_PARALLEL_BUILDS=$NUM_CORES

BUILD_PANGOLIN=thirdparty/build-pangolin

# 注释这两个，就不会更新了
# git submodule sync --recursive
# git submodule update --init --recursive

rm -rf "$BUILD_PANGOLIN"

mkdir -p "$BUILD_PANGOLIN"
pushd "$BUILD_PANGOLIN"
cmake ../Pangolin
make -j$NUM_PARALLEL_BUILDS
popd

然后再pangolin中的cmakelists.txt文件中取消external模块的编译就可以。

add_custom_target(pangolin_uninstall
  "${CMAKE_COMMAND}" -P "${CMAKE_CURRENT_BINARY_DIR}/cmake_uninstall.cmake")

# 注释下面这行
# add_subdirectory("external")
add_subdirectory("src")

if(BUILD_TESTS)
    set(Pangolin_DIR ${Pangolin_BINARY_DIR}/src)
    add_subdirectory("test")
endif()

这里修改cmake问价如下可以解决一小问题

# find_package(Boost REQUIRED COMPONENTS system filesystem thread date_time)
find_package(Boost REQUIRED)

关于录制数据：
1.需要在平面多的房间里录制；
2.xyz轴方向都需要移动
3.不要旋转过/移动过猛

下面是关键内容，对源码进行修改

项目中作者关于其他Lidar使用教程，github上的教程： https://github.com/APRIL-ZJU/lidar_IMU_calib/issues/1

重要的是修改 dataset_reader.h 文件，然后重写 unpack_scan 函数。
在这里插入图片描述

然后找到你的雷达的用户手册，接下来修改主要参考用户手册。然后按照参考2，进行源代码修改。可以下载作者网盘的代码，进行对应的修改，用vscode打开，会有git显示在哪里修改了。参照进行修改即可。

这里解决卡出在第0次迭代不动的问题： https://github.com/APRIL-ZJU/lidar_IMU_calib/issues/55

如果有gui没有显示修改launch文件即可，将show_ui修改为true。

<?xml version="1.0"?>
<launch>
    <arg name="topic_imu"           default="/imu/data" />
    <arg name="path_bag"            default="/home/neu/work/ar_junjie/calibration/lidar_imu_data/2023-12-02-22-25-27.bag" />
    <arg name="bag_start"           default="1" />
    <arg name="bag_durr"            default="110" />
    <arg name="scan4map"            default="15" />
    <arg name="lidar_model"         default="RS_16" />
    <arg name="ndtResolution"       default="0.5" /> <!-- 0.5 for indoor case and 1.0 for outdoor case -->

    <arg name="time_offset_padding" default="0.015" />
    <arg name="show_ui"    default="true" />

    <node pkg="li_calib" type="li_calib_gui" name="li_calib_gui" output="screen">
    <!-- <node pkg="li_calib" type="li_calib_gui" name="li_calib_gui" output="screen" clear_params="true" launch-prefix="gdb -ex run &#45;&#45;args">-->

        <param name="topic_imu"         type="string"   value="$(arg topic_imu)" />
        <param name="topic_lidar"       type="string"   value="/rslidar_points" />
        <param name="LidarModel"        type="string"   value="$(arg lidar_model)" />
        <param name="path_bag"          type="string"   value="$(arg path_bag)" />
        <param name="bag_start"         type="double"   value="$(arg bag_start)" />
        <param name="bag_durr"          type="double"   value="$(arg bag_durr)" /> <!-- for data association -->
        <param name="scan4map"          type="double"   value="$(arg scan4map)" />
        <param name="ndtResolution"     type="double"   value="$(arg ndtResolution)" />

        <param name="time_offset_padding"   type="double"   value="$(arg time_offset_padding)" />
        <param name="show_ui"               type="bool"     value="$(arg show_ui)" />
    </node>

</launch>

下面是运行效果

然后运行launch文件，会出现如下ui界面：
在这里插入图片描述
点击初始化，然后点击数据关联，如下关联点不是0，就表明比较好。

然后按照下面：

优化以此后效果如下：可视化如上

Load dataset from /home/neu/work/ar_junjie/calibration/lidar_imu_data/2023-12-02-22-25-27.bag
/rslidar_points: 1100
/imu/data: 22001
Ceres Solver Report: Iterations: 12, Initial cost: 3.090004e+06, Final cost: 1.512365e+03, Termination: CONVERGENCE
[Initialization] Done. Euler_ItoL initial degree: 179.341 177.042 179.046
[Association] start ....
Plane type  :108  25  88; Plane number: 221
Surfel point number: 41331
[Association] 39578.2 ms

================ Iteration 0 ==================
Ceres Solver Report: Iterations: 16, Initial cost: 4.429489e+06, Final cost: 2.816766e+04, Termination: CONVERGENCE

============== After optimization ================
[Gyro]  Error size, average: 21980; 0.00416329 0.00390573 0.00647236
[Accel] Error size, average: 21980;  0.0309484 0.0355377 0.0189558
[LiDAR] Error size, average: 41331; 0.0173975

P_LinI      :   0.010484 -0.0030227   0.335537
euler_LtoI  :  0.291292 -0.422859 -0.715252
P_IinL      : -0.0129755 0.00115491  -0.335462
euler_ItoL  :  179.703  179.581 -179.283
time offset : 0
gravity     : -0.264985 -0.402293   9.77814
acce bias   : -0.0615988 -0.0641652  0.0537617
gyro bias   :   1.8702e-05 -2.63847e-06  3.13453e-05
[BatchOptimization] 8388.27 ms

细化优化后效果如下：可视化如下，效果好了很多
在这里插入图片描述

================ Iteration 1 ==================
[Association] start ....
Plane type  :1103  220  366; Plane number: 1689
Surfel point number: 137429
[Association] 32562.8 ms
Ceres Solver Report: Iterations: 11, Initial cost: 3.010645e+04, Final cost: 3.002304e+04, Termination: CONVERGENCE

============== After optimization ================
[Gyro]  Error size, average: 21980; 0.00422294 0.00396916 0.00648512
[Accel] Error size, average: 21980;  0.0309524 0.0355387 0.0189847
[LiDAR] Error size, average: 137429; 0.016185

P_LinI      :   0.0115793 -0.00574507     0.33381
euler_LtoI  :  0.198745 -0.420081 -0.661382
P_IinL      : -0.0140785 0.00442492  -0.333734
euler_ItoL  :  179.796  179.582 -179.337
time offset : 0
gravity     : -0.256991 -0.398886   9.77849
acce bias   : -0.0614368 -0.0579201  0.0541251
gyro bias   : 2.83648e-05 9.93374e-06 2.50628e-05
[Refinement] 12699.9 ms

这里尝试在细化一次：可视化如下，基本没有什么变化：（表示优化应该完成了）
在这里插入图片描述

================ Iteration 2 ==================
[Association] start ....
Plane type  :1136  213  364; Plane number: 1713
Surfel point number: 138301
[Association] 34827.8 ms
Ceres Solver Report: Iterations: 11, Initial cost: 3.006577e+04, Final cost: 3.003867e+04, Termination: CONVERGENCE

============== After optimization ================
[Gyro]  Error size, average: 21980; 0.00424599 0.00397834 0.00648505
[Accel] Error size, average: 21980;  0.0309534 0.0355407 0.0190015
[LiDAR] Error size, average: 138301; 0.0161165

P_LinI      :   0.0117197 -0.00671013    0.333046
euler_LtoI  :  0.161992 -0.424985 -0.634221
P_IinL      : -0.0142528 0.00561107  -0.332968
euler_ItoL  :  179.833  179.577 -179.365
time offset : 0
gravity     : -0.255119 -0.395813   9.77867
acce bias   : -0.0613341 -0.0557059  0.0542354
gyro bias   : 1.72131e-05  1.2962e-05 2.78498e-05
[Refinement] 14752.3 ms

最后拟合到的平面可视化如下：还是很不错的。
在这里插入图片描述
实际传感器如下：

欢迎交流！！！

MGWT

关注

1
点赞
踩
9

收藏

觉得还不错? 一键收藏
0
评论
相机、雷达和IMU参数标定

关于相机、Lidar和IMU内外参标定的一些记录，主要是参考前人的文章。
复制链接

扫一扫