T265鱼眼双目+IMU Kalibr联合标定

ZCONG呀

于 2024-07-18 15:27:23 发布

阅读量450

点赞数 10

文章标签：笔记

本文链接：https://blog.csdn.net/abc20150125/article/details/140514585

版权

一、IMU标定

1、安装cere库

sudo apt-get install liblapack-dev libsuitesparse-dev libcxsparse3.1.2 libgflags-dev 
sudo apt-get install libgoogle-glog-dev libgtest-dev

下载编译

git clone https://github.com/ceres-solver/ceres-solver.git
cd ceres-solver
mkdir build
cd build
cmake ..
make 
sudo make install

2、安装 code_utils

构建工作空间

mkdir -p ~/catkin_ws/src
cd ~/catkin_ws
catkin_make

安装依赖

sudo apt-get install libdw-dev

下载编译code_utils

cd ~/catkin_ws/src
git clone https://github.com/gaowenliang/code_utils.git
cd ..
catkin_make

修改 code_utils 的 CMakeLists.txt 文件，如下：

#include_directories("include")
include_directories("include" "include/code_utils")

3、安装 imu_utils

cd ～/catkin_ws/src
git clone https://github.com/gaowenliang/imu_utils.git
cd ..
catkin_make

4、IMU标定

通过rosbag采集imu静止数据集约两小时

rosbag record -O imsee_imu.bag /imsee/imu --duration 2h

修改imu_test.launch文件：

<launch>

    <node pkg="imu_utils" type="imu_an" name="imu_an" output="screen">
         <param name="imu_topic" type="string" value= "/imu/data"/>   <!--修改imu话题名 -->
        <param name="imu_name" type="string" value= "xsens"/>        <!--自定义imu名称（随意，方便标定后找结果）-->
        <param name="data_save_path" type="string" value= "$(find imu_utils)/data/"/>
        <param name="max_time_min" type="int" value= "120"/>         <!--bag时长，单位分钟-->
        <param name="max_cluster" type="int" value= "100"/>
    </node>

</launch>

回放imu数据并使用 imu_utils 标定IMU

rosbag play -r 200 imsee_imu.bag
roslaunch imu_utils imu_test.launch

注：launch文件中bag时长大于bag实际时长会一直处于等待数据界面

标定结果：

%YAML:1.0
---
type: IMU
name: myimu
Gyr:
   unit: " rad/s"
   avg-axis:
      gyr_n: 8.3822876223020376e-04
      gyr_w: 1.4932932694981262e-04
   x-axis:
      gyr_n: 8.7415010652547710e-04
      gyr_w: 1.6788773554071102e-04
   y-axis:
      gyr_n: 9.9111400239917610e-04
      gyr_w: 1.5530685451284852e-04
   z-axis:
      gyr_n: 6.4942217776595796e-04
      gyr_w: 1.2479339079587832e-04
Acc:
   unit: " m/s^2"
   avg-axis:
      acc_n: 8.5761239087474885e-03
      acc_w: 2.5603628585529436e-03
   x-axis:
      acc_n: 1.3066105610309155e-02
      acc_w: 4.2996582168879612e-03
   y-axis:
      acc_n: 6.5333900321730524e-03
      acc_w: 2.1042936199663497e-03
   z-axis:
      acc_n: 6.1288760837602575e-03
      acc_w: 1.2771367388045192e-03

二、相机IMU联合标定

1、安装Kalibr标定工具

安装依赖：

sudo apt-get install python-setuptools python-rosinstall ipython libeigen3-dev libboost-all-dev doxygen libopencv-dev
sudo apt-get install libopencv-dev ros-kinetic-vision-opencv ros-kinetic-image-transport-plugins ros-kinetic-cmake-modules python-software-properties software-properties-common libpoco-dev python-matplotlib python-scipy python-git python-pip ipython libtbb-dev libblas-dev liblapack-dev python-catkin-tools libv4l-dev

下载kalibr编译

cd ~/kalibr_ws/src
git clone https://github.com/ethz-asl/Kalibr.git
cd ~/kalibr_ws
catkin build -DCMAKE_BUILD_TYPE=Release -j4
source ~/kalibr_ws/devel/setup.bash

2、相机标定

制作标定版，可以在此网址设计自己想要的标定般大小格式，然后根据参数写一个标定版参数文件，我这里用的是aprilgrid标定版，参数文格式如下：

yaml/aprilgrid.yaml

target_type: 'aprilgrid'    #gridtype
tagCols: 11                  #number of apriltags
tagRows: 10                  #number of apriltags
tagSize: 0.045             #size of apriltag, edge to edge [m]
tagSpacing: 0.3          #ratio of space between tags to tagSize

checkerboard.yaml

target_type: 'checkerboard' #gridtype
targetCols: 8               #number of internal chessboard corners
targetRows: 8               #number of internal chessboard corners
rowSpacingMeters: 0.008      #size of one chessboard square [m]
colSpacingMeters: 0.008      #size of one chessboard square [m]

主要用到的就是这两种标定板格式

降低相机话题发布频率，推荐是4Hz，但是我用的是20Hz

rosrun topic_tools throttle messages /camera/fisheye1/image_raw 20.0 /left
rosrun topic_tools throttle messages /camera/fisheye2/image_raw 20.0 /right

录制相机标定bag包，当然也可以先录制原始话题，再通过播放bag包的形式录制低频率的话题，在这里我遇到了一些问题：当我以4hz进行标定的时候，即使我有1000张图片他还是会提示我数据不足，也可能是我移动不够充分的原因吧，于是我尝试了一下20Hz与原始的频率，我发现最后结果原始频率精度会高一些，但是相对的速度会慢很多。

rosbag record -O reals_camera.bag /camera/fisheye1/image_raw /camera/fisheye2/image_raw

使用kalibr标定

rosrun kalibr kalibr_calibrate_cameras --target yaml/aprilgrid.yaml --bag bag/imu_camera_7.17.bag  --topic /camera/fisheye1/image_raw /camera/fisheye2/image_raw --models pinhole-equi pinhole-equi --bag-from-to 5 255

T265相机在使用 pinhole-equi模型进行标定的时候可能会出现焦距初始化失败的问题，在终端中初始化焦距为1,后续标定时终端设置焦距为400即可

export KALIBR_MANUAL_FOCAL_LENGTH_INIT=1

相机标定结果在0.5个像素以内，标定精度较好，一个像素也能用

3、相机IMU联合标定

根据imu标定结果重新生成一个imu配置文件为联合标定提供参数，格式如下：

rostopic: /camera/imu
update_rate: 10.0 #Hz
 
accelerometer_noise_density: 1.8677079396467611e-02
accelerometer_random_walk: 5.4687746398927092e-04
gyroscope_noise_density: 2.8883517709083844e-03
gyroscope_random_walk: 2.2670359336940637e-05

将相机imu固定之后，通过绕轴运动充分激励imu，然后可以通过画八字的方式进行运动，整个标定过程要保证标定板在画面内。

rosbag record /imu/data_raw /stereo/left/image_raw /stereo/right/image_raw -O images_imu.bag

使用Kalibr进行联合标定

rosrun kalibr kalibr_calibrate_imu_camera --target yaml/aprilgrid.yaml --bag camimu_7.17.14.bag --cams bag/imu_camera_7.17-camchain.yaml --imu releimu_param.yaml

最后输出标定结果：

Calibration results
===================
Normalized Residuals
----------------------------
Reprojection error (cam0):     mean 0.30525551123349065, median 0.2674185657744718, std: 0.19450491173814535
Reprojection error (cam1):     mean 0.2930368921326456, median 0.2534528915494121, std: 0.19319762845660415
Gyroscope error (imu0):        mean 0.43950109462399295, median 0.40427936719841456, std: 0.4332252003723668
Accelerometer error (imu0):    mean 0.5491123118207009, median 0.4341897979957793, std: 0.5740011161726761

Residuals
----------------------------
Reprojection error (cam0) [px]:     mean 0.30525551123349065, median 0.2674185657744718, std: 0.19450491173814535
Reprojection error (cam1) [px]:     mean 0.2930368921326456, median 0.2534528915494121, std: 0.19319762845660415
Gyroscope error (imu0) [rad/s]:     mean 0.004014302036038767, median 0.0036925948688739945, std: 0.003956979459643774
Accelerometer error (imu0) [m/s^2]: mean 0.03243173227523299, median 0.02564416601504755, std: 0.03390171759885015

Transformation (cam0):
-----------------------
T_ci:  (imu0 to cam0): 
[[-0.99998788 -0.00409842  0.00272868  0.01562209]
 [ 0.00409426 -0.99999045 -0.00152633  0.00867264]
 [ 0.0027349  -0.00151514  0.99999511 -0.00188076]
 [ 0.          0.          0.          1.        ]]

T_ic:  (cam0 to imu0): 
[[-0.99998788  0.00409426  0.0027349   0.01559154]
 [-0.00409842 -0.99999045 -0.00151514  0.00873374]
 [ 0.00272868 -0.00152633  0.99999511  0.00185136]
 [ 0.          0.          0.          1.        ]]

timeshift cam0 to imu0: [s] (t_imu = t_cam + shift)
0.00870769756179636


Transformation (cam1):
-----------------------
T_ci:  (imu0 to cam1): 
[[-0.9999966   0.00134679  0.00223245 -0.04746357]
 [-0.00133956 -0.99999387  0.00323464  0.00897478]
 [ 0.0022368   0.00323164  0.99999228 -0.00110675]
 [ 0.          0.          0.          1.        ]]

T_ic:  (cam1 to imu0): 
[[-0.9999966  -0.00133956  0.0022368  -0.04744891]
 [ 0.00134679 -0.99999387  0.00323164  0.00904223]
 [ 0.00223245  0.00323464  0.99999228  0.00118367]
 [ 0.          0.          0.          1.        ]]

timeshift cam1 to imu0: [s] (t_imu = t_cam + shift)
0.008823780689585845

Baselines:
----------
Baseline (cam0 to cam1): 
[[ 0.99998505 -0.00544443 -0.00050449 -0.06303916]
 [ 0.00544676  0.9999739   0.00474609  0.0002262 ]
 [ 0.00047864 -0.00474877  0.99998861  0.00080769]
 [ 0.          0.          0.          1.        ]]
baseline norm:  0.0630447403377865 [m]


Gravity vector in target coords: [m/s^2]
[-0.14821871 -9.67232671 -1.61013977]


Calibration configuration
=========================

cam0
-----
  Camera model: pinhole
  Focal length: [286.8633842473987, 286.71813554346863]
  Principal point: [420.81623183682495, 387.85843447459183]
  Distortion model: equidistant
  Distortion coefficients: [-0.01384914573289277, 0.06332218296231706, -0.06264556905109019, 0.0146035636622523]
  Type: aprilgrid
  Tags: 
    Rows: 10
    Cols: 11
    Size: 0.045 [m]
    Spacing 0.0135 [m]

cam1
-----
  Camera model: pinhole
  Focal length: [286.42854810908597, 285.9129583456371]
  Principal point: [421.2229804648273, 381.797114314822]
  Distortion model: equidistant
  Distortion coefficients: [-0.00590907217788528, 0.039933432296039156, -0.04236336068996756, 0.008934063498334074]
  Type: aprilgrid
  Tags: 
    Rows: 10
    Cols: 11
    Size: 0.045 [m]
    Spacing 0.0135 [m]



IMU configuration
=================

IMU0:
 ----------------------------
  Model: calibrated
  Update rate: 10.0
  Accelerometer:
    Noise density: 0.01867707939646761 
    Noise density (discrete): 0.059062110932640646 
    Random walk: 0.0005468774639892709
  Gyroscope:
    Noise density: 0.0028883517709083844
    Noise density (discrete): 0.00913377027985136 
    Random walk: 2.2670359336940637e-05
  T_ib (imu0 to imu0)
    [[1. 0. 0. 0.]
     [0. 1. 0. 0.]
     [0. 0. 1. 0.]
     [0. 0. 0. 1.]]
  time offset with respect to IMU0: 0.0 [s]

ZCONG呀

关注

10
点赞
踩
6

收藏

觉得还不错? 一键收藏
0
评论
T265鱼眼双目+IMU Kalibr联合标定

T265相机在使用 pinhole-equi模型进行标定的时候可能会出现焦距初始化失败的问题，在终端中初始化焦距为1,后续标定时终端设置焦距为400即可。将相机imu固定之后，通过绕轴运动充分激励imu，然后可以通过画八字的方式进行运动，整个标定过程要保证标定板在画面内。录制相机标定bag包，当然也可以先录制原始话题，再通过播放bag包的形式录制低频率的话题，相机标定结果在0.5个像素以内，标定精度较好，一个像素也能用。回放imu数据并使用 imu_utils 标定IMU。使用kalibr标定。
复制链接

扫一扫