vslam从入门到入土：kalibr标定，以D455为例

1.imu标定

1.1 ceres安装

sudo apt-get install cmake
sudo apt-get install libgoogle-glog-dev
sudo apt-get install libatlas-base-dev
sudo apt-get install libeigen3-dev
sudo apt-get install libsuitesparse-dev

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

1.2 下载编译code_utils

mkdir -p ~/imu_catkin_ws/src
cd ~/imu_catkin_ws/src
git clone https://github.com/gaowenliang/code_utils.git
cd ..
catkin_make

1.2.1 错误解决

#fatal error: elfutils/libdw.h: No such file or directory
sudo apt-get install libdw-dev

#fatal error: backward.hpp: No such file or directory
#cd XXXX/code_utils/src/sumpixel_test.cpp
include "code_utils/backward.hpp"

cv::Mat m = cv::imread(filename, CV_LOAD_IMAGE_GRAYSCALE);
改为
cv::Mat m = cv::imread(filename, cv::IMREAD_GRAYSCALE);

cv::Mat m = cv::imread(filename, CV_LOAD_IMAGE_COLOR);
改为
cv::Mat m = cv::imread(filename, cv::IMREAD_COLOR);

cv::Mat m = cv::imread(filename, CV_LOAD_IMAGE_UNCHANGED);
改为
cv::Mat m = cv::imread(filename, cv::IMREAD_UNCHANGED);

1.3 下载编译imu_utils

cd ~/imu_catkin_ws/src/
git clone https://github.com/gaowenliang/imu_utils.git
cd ..
catkin_make

1.4 ROS附件

sudo sh -c 'echo "deb http://packages.ros.org/ros/ubuntu $(lsb_release -sc) main" > /etc/apt/sources.list.d/ros-latest.list'
sudo apt-key adv --keyserver 'hkp://keyserver.ubuntu.com:80' --recv-key C1CF6E31E6BADE8868B172B4F42ED6FBAB17C654
sudo apt-get update
export ROS1_DISTRO=noetic # kinetic=16.04, melodic=18.04, noetic=20.04
sudo apt-get install ros-$ROS1_DISTRO-desktop-full
sudo apt-get install python-catkin-tools # ubuntu 16.04, 18.04
sudo apt-get install python3-catkin-tools python3-osrf-pycommon # ubuntu 20.04

1.5 下载编译kalibr

#1.Install the build and run dependencies 
#The general requirements common to all version of Ubuntu are the following:
sudo apt-get install -y \
    git wget autoconf automake nano \
    libeigen3-dev libboost-all-dev libsuitesparse-dev \
    doxygen libopencv-dev \
    libpoco-dev libtbb-dev libblas-dev liblapack-dev libv4l-dev


#Then due to different Python versions, you will need to install the following:
# Ubuntu 16.04
sudo apt-get install -y python2.7-dev python-pip python-scipy \
    python-matplotlib ipython python-wxgtk3.0 python-tk python-igraph
# Ubuntu 18.04
sudo apt-get install -y python3-dev python-pip python-scipy \
    python-matplotlib ipython python-wxgtk4.0 python-tk python-igraph
# Ubuntu 20.04
sudo apt-get install -y python3-dev python3-pip python3-scipy \
    python3-matplotlib ipython3 python3-wxgtk4.0 python3-tk python3-igraph

#2.Create a catkin workspace and clone the project
#First we can create a workspace. It is important to configure this to build in release mode otherwise optimization will be slow.
mkdir -p ~/kalibr_workspace/src
cd ~/kalibr_workspace
export ROS1_DISTRO=noetic # kinetic=16.04, melodic=18.04, noetic=20.04
source /opt/ros/$ROS1_DISTRO/setup.bash
catkin init
catkin config --extend /opt/ros/$ROS1_DISTRO
catkin config --merge-devel # Necessary for catkin_tools >= 0.4.
catkin config --cmake-args -DCMAKE_BUILD_TYPE=Release

#We can then clone the project:
cd ~/kalibr_workspace/src
git clone https://github.com/ethz-asl/kalibr.git

#3.Build the code using the Release configuration. Depending on the available memory, you might need to reduce the build threads (e.g. add -j2 to catkin_make) 
cd ~/kalibr_workspace/
catkin build -DCMAKE_BUILD_TYPE=Release -j4

#4.Once the build is finished you have to source the catkin workspace setup to use Kalibr
source ~/kalibr_workspace/devel/setup.bash
rosrun kalibr <command_you_want_to_run_here>

 1.6.标定imu

1.6.1 进入/realsense_ws/src/realsense-ros/realsense2_camera/launch/rs_camera.launch

<arg name="unite_imu_method"          default="linear_interpolation"/>

1.6.2 录制imu.bag

cd realsense_ws
source devel/setup.bash
roslaunch realsense2_camera rs_camera.launch
rosbag record -O imu_calibration /camera/imu

静待1h

1.6.3 创建calibration/d455文件夹（option）

mkdir calibration/d455

1.6.4 打开~/imu_catkin_ws/src/imu_utils/launch

gedit d455_imu_calibration.launch

写入

<launch>

    <node pkg="imu_utils" type="imu_an" name="imu_an" output="screen">
    	<!--TOPIC名称和上面一致-->
        <param name="imu_topic" type="string" value= "/camera/imu"/>
        <!--imu_name 无所谓-->
        <param name="imu_name" type="string" value= "d455"/>
        <!--标定结果存放路径-->
        <param name="data_save_path" type="string" value= "$(find imu_utils)/data/"/>
        <!--数据录制时间-min-->
        <param name="max_time_min" type="int" value= "50"/>
        <!--采样频率，即是IMU频率，采样频率可以使用rostopic hz /camera/imu查看，设置为200，为后面的rosbag play播放频率-->
        <param name="max_cluster" type="int" value= "200"/>
    </node>
    
</launch>

1.6.5 解算 

cd imu_util
source devel/setup.bash
roslaunch imu_utils d455_imu_calibration.launch

回放数据包,打开新的终端

cd 存放imu_calibration.bag的路径
rosbag play -r 200 imu_calibration.bag

1.6.6 标定结束后在imu_catkin_ws/src/imu_utils/data找到d455_imu_param.yaml，编写按格式imu.yaml并放到calibration/d455文件夹

#Accelerometers
accelerometer_noise_density: 1.1964613285076815e-02  #Noise density (continuous-time)
accelerometer_random_walk:   3.7731070649168168e-04  #Bias random walk

#Gyroscopes
gyroscope_noise_density:     1.8592840863593844e-03   #Noise density (continuous-time)
gyroscope_random_walk:       1.6445652273008798e-05  #Bias random walk

rostopic:                    /imu      #the IMU ROS topic
update_rate:                 200.0      #Hz (for discretization of the values above)

1.6.7 编写april_6x6_A4.yaml，并放到calibration/d455文件夹

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

2.标定D455的stereo_camera:

2.1 关闭结构光

realsense-viewer

stereo Module--Emitter Enabled --- 选择off 

2.2 打开摄像头

cd realsense_ws
source devel/setup.bash
roslaunch realsense2_camera rs_camera.launch

2.3 打开rviz，frame_id选择camera_link, 然后 add-topic， 添加/camera/color/image_raw、/camera/infra1/image_rect_raw、/camera/infra2/image_rect_raw

2.4 修改发布频率

rosrun topic_tools throttle messages /camera/color/image_raw 4.0 /color
rosrun topic_tools throttle messages /camera/infra1/image_rect_raw 4.0 /infra_left
rosrun topic_tools throttle messages /camera/infra2/image_rect_raw 4.0 /infra_right

2.5 录制ROS数据包，录完之后把multicameras_calibration.bag放到calibration/d455文件夹

rosbag record -O multicameras_calibration /infra_left /infra_right /color

2.6 标定

cd kalibr_workspace/
source /devel/setup.bash

rosrun kalibr kalibr_calibrate_cameras --target ~/calibration/d455/april_6x6_A4.yaml --bag  ~/calibration/d455/multicameras_calibration.bag --models pinhole-radtan pinhole-radtan pinhole-radtan --topics /infra_left /infra_right /color --bag-from-to 3 180 --show-extraction --approx-sync 0.04

2.7 calibration/d455/results,并把kalibr_workspace里的三个新文件放入results中

cd calibration/d455
mkdir results

3.联合标定

3.1 进入calibration/d455/results，复制camchain-homecassomulticameras_calibration.yaml，并命名为chain.yaml，打开chain.yaml, 删除cam2所有数据。最后放入calibration/d455

cam0:
  camera_model: pinhole
  distortion_coeffs: [0.0029016480013736887, -0.0021244172332884315, -0.000222443891451069,
    0.0005370296782569457]
  distortion_model: radtan
  intrinsics: [428.3537922602939, 428.3201454661401, 422.67752703029004, 236.60599791251943]
  resolution: [848, 480]
  rostopic: /infra_left
cam1:
  T_cn_cnm1:
  - [0.9999987396489537, 0.00013984992942629745, 0.00158150007803968, -0.09502174553582819]
  - [-0.00013959895607696578, 0.9999999776469461, -0.00015880250050972824, -2.9298551348984124e-05]
  - [-0.0015815222512068161, 0.00015858152460290095, 0.9999987368188344, 6.142646077231596e-05]
  - [0.0, 0.0, 0.0, 1.0]
  camera_model: pinhole
  distortion_coeffs: [0.004099161247227021, -0.0018322021727942453, -0.0002654352006165499,
    -9.23143936505887e-05]
  distortion_model: radtan
  intrinsics: [429.21590850601166, 429.26468902016296, 422.41636668551513, 236.74635707195984]
  resolution: [848, 480]
  rostopic: /infra_right

3.2 进入/catkin_ws/src/realsense-ros/realsense2_camera.launch，复制rs_camera.launch，重命名为rs_imu_stereo.launch

<arg name="enable_sync"               default="true"/>
<arg name="unite_imu_method"          default="linear_interpolation"/>

3.3 启动realsense&rviz，然后 add-topic， 添加/camera/imu、/camera/infra1/image_rect_raw、/camera/infra2/image_rect_raw

roslaunch realsense2_camera rs_imu_stereo.launch
rviz

3.4 修改发布频率

rosrun topic_tools throttle messages /camera/infra1/image_rect_raw 20.0 /infra_left
rosrun topic_tools throttle messages /camera/infra2/image_rect_raw 20.0 /infra_right
rosrun topic_tools throttle messages /camera/imu 200.0 /imu

3.5 录制

rosbag record -O imu_stereo.bag /infra_left /infra_right /imu

3.6 标定

rosrun kalibr kalibr_calibrate_imu_camera --bag ~/calibration/d455/imu_stereo.bag --cam ~/calibration/d455/chain.yaml --imu ~/calibration/d455/imu.yaml --target ~/calibration/d455/april_6x6_A4.yaml --bag-from-to 10 180 --show-extraction

3.6.1 如果以上命令出错，试一下改成绝对路径

3.6.2 好坏判断标准：

results-imucam-homezjimu_stereo.txt中的重投影误差Reprojection error，重投影误差在0.1-0.2