【Linux配置五】 Ubuntu18.04+kalibr标定工具箱安装编译

一、安装kalibr

1、安装依赖项

所有 Ubuntu 版本的通用要求如下：

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

然后由于不同的Python版本，您将需要安装以下内容：

# 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

本文为ubuntu18.04系统，安装命令如下：

sudo apt-get install -y python3-dev python-pip python-scipy \
    python-matplotlib ipython python-wxgtk4.0 python-tk python-igraph

注：安装python-igraph时，我用pip命令无法安装，故改为apt-get命令，安装得很顺利。

2、创建kalibr的工作空间

Ubuntu18.04 创建kalibr的工作空间

mkdir -p ~/kalibr_workspace/src
cd ~/kalibr_workspace
source /opt/ros/melodic/setup.bash
catkin init
catkin config --extend /opt/ros/melodic
catkin config --merge-devel
catkin config --cmake-args -DCMAKE_BUILD_TYPE=Release

3、源码下载

地址：https://github.com/ethz-asl/kalibr
官网教程是从网站克隆，但克隆版本并不符合安装系统，总是出错，显示kalibr_calibrate_imu_camera命令未找到，多次重新编译也未成功。

所以本文选用kalibr与ubuntu18.04对应的版本（其他ubuntu系统同理）

下载后将kalibr-fix-18.04.zip解压到 ~/kalibr_workspace/src文件夹下：

4、源码编译

cd ~/kalibr_workspace/
catkin build -DCMAKE_BUILD_TYPE=Release -j4

5、安装过程中的报错

如果出现错误，可以多编译几次，好多错误重新编译就没有了catkin build -DCMAKE_BUILD_TYPE=Release -j4

5.1 下载kalibr-master做备用

一般的，如果编译错误，将报错所指路径文件全部替换为kalibr-master对应的文件，简单粗暴，亲测有效

5.2 报错一

将~/kalibr_workspace/src/aslam_cv文件夹下的aslam_cv_python aslam_cv_backend_python 替换为kalibr-master对应的文件

5.3 报错二

将~/kalibr_workspace/src/kalibr/Schweizer-Messer文件夹下的python_module替换为kalibr-master对应的文件

5.4 安装成功

注意：37个包，不是36个包

安装kalibr参考：
http://t.csdn.cn/TzErH
http://t.csdn.cn/yydgf

二、测试kalibr

1、测试相机标定

1） 官网下载相机标定测试数据

2） 在kalibr_workspace文件夹下新建一个文件夹camera0放置下载的数据，camera0文件夹里面存放下载的数据。

3）在kalibr_workspace文件夹下打开终端，输入命令，别忘了设置环境

// 设置环境，否则报错 --- kalibr_calibrate_cameras：未找到命令
source ~/kalibr_workspace/devel/setup.bash 
kalibr_calibrate_cameras \
    --target /YOUR PATH/april_6x6.yaml \
    --bag /YOUR PATH/cam_april.bag --bag-from-to 30 50 \
    --models pinhole-fov \
    --topics /cam0/image_raw

我的路径是/home/lzy/kalibr_workspace/camera0/，因此指令为：

source ~/kalibr_workspace/devel/setup.bash 
kalibr_calibrate_cameras \
    --target /home/lzy/kalibr_workspace/camera0/april_6x6.yaml \
    --bag /home/lzy/kalibr_workspace/camera0/cam_april.bag --bag-from-to 30 50 \
    --models pinhole-fov \
    --topics /cam0/image_raw

2、测试相机与imu联合标定

1） 从https://pan.baidu.com/s/1bWQT7g提取码2g2t下载数据包，将解压后dynamic的放在工作空间kalibr_workspace里

2）在kalibr_workspace文件夹下打开终端，输入命令，别忘了设置环境

source ~/kalibr_workspace/devel/setup.bash
kalibr_calibrate_imu_camera \
  --target /YOUR PATH/april_6x6.yaml \
  --cam /YOUR PATH/camchain.yaml \
  --imu /YOUR PATH/imu_adis16448.yaml \
  --bag /YOUR PATH/dynamic.bag --bag-from-to 5 45

我的路径是/home/lzy/kalibr_workspace/dynamic，因此指令为：

source ~/kalibr_workspace/devel/setup.bash
kalibr_calibrate_imu_camera \
 --target /home/lzy/kalibr_workspace/dynamic/april_6x6.yaml \
 --cam /home/lzy/kalibr_workspace/dynamic/camchain.yaml \
 --imu /home/lzy/kalibr_workspace/dynamic/imu_adis16448.yaml \
 --bag /home/lzy/kalibr_workspace/dynamic/dynamic.bag --bag-from-to 5 45

标定完成后，会生成下面4个文件。其中比较关注的IMU和相机变换矩阵和timeshift都在以camchain命名的.yaml文件中，在results的txt文件中也可以看到

测试kalibr参考：
http://t.csdn.cn/yydgf

三、实践

1、标定imu

本文标定的imu为sbg

注意事项：
1、 imu 录制时必须静置不动，官方建议两个小时以上；录制时话题类型不做要求，标定时类型必须是sensor_msgs/Imu
2、imu 标定时话题类型必须是sensor_msgs/Imu
3、imu 标定时不要求xyz轴必须是右前上之类的

1.1 安装imu_utils

1.1.1 安装ceres

此项为必要依赖。本机在前续使用中已安装了ceres-1.14.0，此处记录对应版本仅供参考，Github上有最新版本应该是2.0.0。ceres-solver同时还需依赖eigen，本机前续使用中已安装了eigen-3.3.7。
1）安装ceres的一些相关依赖（参考http://www.ceres-solver.org/installation.html）[2]

# CMake
sudo apt-get install cmake
# google-glog + gflags
sudo apt-get install libgoogle-glog-dev libgflags-dev
# BLAS & LAPACK
sudo apt-get install libatlas-base-dev
# SuiteSparse and CXSparse (optional)
sudo apt-get install libsuitesparse-dev

2）build, test, and install Ceres

# 已提前下载好的压缩包，没有下载要先去git
tar zxf ~/Documents/ceres-solver-1.14.0.tar.gz
mkdir ceres-bin
cd ceres-bin
cmake ../ceres-solver-1.14.0
make -j4
make test
sudo make install

1.1.2 安装code_utils

sudo apt-get install libdw-dev
mkdir -p ~/kalibr_imu/src
cd kalibr_imu/src
git clone https://github.com/gaowenliang/code_utils.git

这里需要修改 code_utils 的 CMakeLists.txt 文件,将 CMAKE_CXX_FLAGS “-std=c++11” 改为 CMAKE_CXX_STANDARD 14 并添加 include_directories(include/code_utils)。然后在~/kalibr_imu路径下执行catkin_make。这里也是因为后面下载的imu_utils编译时是需要依赖code_utils的，所以这里先单独编译一下，当然也可以在两个包都下好的情况下使用catkin_make -DCATKIN_WHITELIST_PACKAGES=‘code_utils’先指定编译，再catkin_make -DCATKIN_WHITELIST_PACKAGES=’‘全部编译。

1.1.3 安装imu_utils

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

同样，在catkin_make前，需要修改imu_utils 的 CMakeLists.txt 文件,将 CMAKE_CXX_FLAGS “-std=c++11” 改为 CMAKE_CXX_STANDARD 14。

1.2 录制IMU数据及标定

sbg录制的话题为/sbg/imu_data，其类型是自定义类型sbg_driver/SbgImuData
sbg标定的话题为/imu0，其类型是sensor_msgs/Imu

1.2.1 录制imu数据（sbg）

sbg的使用教程：https://github.com/SBG-Systems/sbg_ros_driver
sbg录制的话题为/sbg/imu_data，其类型是自定义类型sbg_driver/SbgImuData，后续标定前需要将其类型转为sensor_msgs/Imu
启动sbg驱动

roslaunch sbg_driver sbg_device.launch

录制imu，话题为/sbg/imu_data

rosbag record /sbg/imu_data -o sbg.bag

查看imu频率

rosrun rqt_topic rqt_topic

1.2.2 标定及结果

1）配置imu_utils/launch路径下的sbg.launch，相应名称和参数应以自己IMU为准。
需要修改的参数：
imu 话题"imu_topic"需要与录制时的话题保持一致，类型为sensor_msgs/Imu
imu 名字"imu_name"自定义，建议与launch名保持一致，决定后续标定结果的文件名
imu 加载时间"max_time_min"，单位为分钟，表示从rosbag中加载数据的时间长度，根据实际录制的时间确定，官方建议两小时以上

<launch>
    <node pkg="imu_utils" type="imu_an" name="imu_an" output="screen">
        <param name="imu_topic" type="string" value= "/imu0"/>
        <param name="imu_name" type="string" value= "sbg"/>
        <param name="data_save_path" type="string" value= "$(find imu_utils)/data/"/>
        <param name="max_time_min" type="int" value= "50"/>
        <param name="max_cluster" type="int" value= "100"/>
    </node>
</launch>

2）启动.launch文件，播放录制的IMU数据包。
此时的sbg.bag是已经将话题类型修改为sensor_msgs/Imu之后的bag包

# 启动.launch文件
cd kalibr_imu
source devel/setup.bash
roslaunch imu_utils sbg.launch
# 在对应路径下播放数据包，使用-r 200来倍速播放
rosbag play -r 200 sbg.bag

3）完成后可以在imu_utils/data路径下找到与imu_name对应的xxx_imu_param.yaml文件。
sbg_imu_param.yaml如下。其中_n代表noise，_w代表random walk。

%YAML:1.0
---
type: IMU
name: sbg
Gyr:
   unit: " rad/s"
   avg-axis:
      gyr_n: 4.9437760129591220e-04
      gyr_w: 3.0009989457718918e-05
   x-axis:
      gyr_n: 4.6063631944431345e-04
      gyr_w: 2.6747246508923371e-05
   y-axis:
      gyr_n: 4.8208450909023370e-04
      gyr_w: 2.5466994384681401e-05
   z-axis:
      gyr_n: 5.4041197535318935e-04
      gyr_w: 3.7815727479551984e-05
Acc:
   unit: " m/s^2"
   avg-axis:
      acc_n: 6.2662678044191102e-03
      acc_w: 2.8805164533371702e-04
   x-axis:
      acc_n: 6.5695260918416801e-03
      acc_w: 2.6707017662488822e-04
   y-axis:
      acc_n: 6.7936962756448083e-03
      acc_w: 4.2277805228637218e-04
   z-axis:
      acc_n: 5.4355810457708429e-03
      acc_w: 1.7430670708989062e-04

2、标定camera

本文标定的是深度相机kinectV1
本文录制的话题是 /camera/rgb/image_color，其类型为sensor_msgs/Image
下载标定板时建议下载原尺寸标定板，比如Aprilgrid 6x6 0.8x0.8 m (A0 page)就是A0纸
标定结果，重投影误差在1~2pix之内，比较好。重投影误差较大，建议重新录制bag包

注意：
1、相机录制频率为4Hz
2、相机录制的话题名称无所谓，但是话题类型必须是sensor_msgs/Image
3、标定相机命令里面的模型必须选对，本文选取的是--models pinhole-radtan，否则会报错，这一步非常重要。具体模型参考https://github.com/ethz-asl/kalibr/wiki/supported-models
4、相机标定模型根据要标定的数据选取，比如我需要标定的是(intrinsics vector: [fu fv pu pv])和(distortion_coeffs: [k1 k2 r1 r2])，故选取--models pinhole-radtan

2.1 采集相机数据

启动深度相机kinectV1

source devel/setup.bash
roslaunch freenect_launch freenect.launch

将话题 /camera/rgb/image_color频率修改为20Hz

rosrun topic_tools throttle messages /camera/rgb/image_color 20.0 /my_image_color

录制bag

rosbag record /my_image_color -o my_image_color.bag

打开相机屏幕

rosrun rqt_image_view rqt_image_view 

录取bag个人经验：
1、录取时长最少3分钟。--bag-from-to 25 150 最少120s，图片数量500张左右。
2、录取过程中要缓慢移动，倾斜。标定板位置最好出现在rqt_image_view 视野的各个位置

2.2 下载并打印标定板

标定板下载地址：https://github.com/ethz-asl/kalibr/wiki/downloads
本文选择的是 Aprilgrid 6x6 0.8x0.8 m (A0 page)，创建或修改april_6x6.yaml。以下是A4纸下的尺寸

#example for aprilgrid
  target_type: 'aprilgrid' #gridtype
  tagCols: 6                  #number of apriltags
  tagRows: 6                  #number of apriltags
  tagSize: 0.021              #size of apriltag, edge to edge [m]
  tagSpacing: 0.2857             #ratio of space between tags to tagSize
                              #example: tagSize=2m, spacing=0.5m --> tagSpacing=0.25[-]

上图来源：https://github.com/ethz-asl/kalibr/wiki/calibration-targets

2.3 标定相机

在kalibr_workspace文件夹下新建一个文件夹camera，放置录制好的my_image_color.bag和修改后的april_6x6.yaml

在kalibr_workspace文件夹下打开终端，输入

source devel/setup.bash
kalibr_calibrate_cameras \
    --target /home/lzy/kalibr_workspace/camera/april_6x6.yaml \
    --bag /home/lzy/kalibr_workspace/camera/my_image_color.bag --bag-from-to 25 150 \
    --models pinhole-radtan \
    --topics /my_image_color

应用时，注意修改路径

2.4 报错

[ERROR] [1656413643.074323]: initialization of focal length for cam with topic /my_image_color
[ERROR] [1656413643.084323]: calibrateIntrinsics: Optimization failed!
[ERROR] [1656413643.093601]: initialization of intrinsics for cam with topic /my_image_color failed  

解决方法：
1、确定选取的相机模型是否正确，模型不对的话会导致上述问题，具体模型参考https://github.com/ethz-asl/kalibr/wiki/supported-models。基本模型正确的话，会解决大部分问题。
2、模型选取正确依然报错的话，修改aslam_cv/aslam_cameras/include/aslam/cameras/implementation/PinholeProjection.hpp文件的下列部分。位置760行左右，建议直接Ctrl+F搜索if(f_guesses.empty())

#include 
#include 
#include 

using namespace std;
namespace aslam {

namespace cameras {
...
    for (size_t j=0; j<target.rows(); ++j)
    {
      for (size_t k=j+1; k<target.cols(); ++k)
      {
        // find distance between pair of vanishing points which
        // correspond to intersection points of 2 circles
        std::vector < cv::Point2d > ipts;
        ipts = PinholeHelpers::intersectCircles(center[j](0), center[j](1),
                                                radius[j], center[k](0), center[k](1), radius[k]);
        if (ipts.size()<2)
          continue;

         double f_guess = cv::norm(ipts.at(0) - ipts.at(1)) / M_PI;

         std::cout << "f_guess: "<<f_guess<< std::endl;
        if(std::isnan(f_guess)) {
            std::cout << "nan ,not use " << std::endl;
            continue;
         }
         f_guesses.push_back(f_guess);
      }
    }
  }

  //get the median of the guesses
  // if(f_guesses.empty())
  //   return false;
  if(f_guesses.empty()) {
    const char* manual_input = std::getenv("KALIBR_MANUAL_FOCAL_LENGTH_INIT");
    if(manual_input != nullptr) {
      double input_guess;
      std::cout << "Initialization of focal length failed. Provide manual initialization: " << std::endl;
      std::cin >> input_guess;
      SM_ASSERT_GT(std::runtime_error, input_guess, 0.0, 
                "Focal length needs to be positive.");
      std::cout << "Initializing focal length to " << input_guess << std::endl;
      f_guesses.push_back(input_guess);
    } else {
      std::cout << "Initialization of focal length failed. You can enable"
        << " manual input by setting 'KALIBR_MANUAL_FOCAL_LENGTH_INIT'." << std::endl;
      return false;
    }
  }
  double f0 = PinholeHelpers::medianOfVectorElements(f_guesses);
...

修改后的标定命令为

source devel/setup.bash
export KALIBR_MANUAL_FOCAL_LENGTH_INIT=True 
kalibr_calibrate_cameras \
    --target /home/lzy/kalibr_workspace/camera/april_6x6.yaml \
    --bag /home/lzy/kalibr_workspace/camera/my_image_color.bag --bag-from-to 25 150 \
    --models pinhole-radtan \
    --topics /my_image_color

3、camera-imu 联合标定

imu：sbg 、频率100Hz、话题类型sensor_msgs/Imu
camera：kinectV1 、频率4Hz、话题类型sensor_msgs/Image
imu和 camera 的bag可以分开录制，最好整合成一个bag就行了；分开录制要求时间重合。

3.1 录制imu

启动sbg驱动

roslaunch sbg_driver sbg_device.launch

录制imu，话题为/sbg/imu_data。注意此时的话题类型是自定义类型，后续要把类型改为sensor_msgs/Imu

rosbag record /sbg/imu_data -o imu_sbg.bag

查看imu频率

rosrun rqt_topic rqt_topic

3.2 录制camera

启动深度相机kinectV1

source devel/setup.bash
roslaunch freenect_launch freenect.launch

将话题 /camera/rgb/image_color频率修改为4Hz

rosrun topic_tools throttle messages /camera/rgb/image_color 4.0 /my_image_color

录制bag

rosbag record /my_image_color -o my_image_color.bag

打开相机屏幕

rosrun rqt_image_view rqt_image_view 

3.3 bag整合

将imu_sbg.bag里面话题类型改为sensor_msgs/Imu
将my_image_color.bag里面话题类型改为sensor_msgs/Image
将话题修改后的imu_sbg.bag和my_image_color.bag整合成一个bag包，即kalibr.bag，信息如下

            Topics              Types  Message Count     Frequency
0  /cam0/image_raw  sensor_msgs/Image            568      3.758933
1            /imu0    sensor_msgs/Imu          14587  52428.800000

3.4 IMU内参yaml、相机内外参yaml、标定板yaml

1）相机内外参yaml就用在前续2.2.3相机标定结果中获得的以camchain开头命名的.yaml文件即可；注意rostopic

cam0:
  cam_overlaps: []
  camera_model: pinhole
  distortion_coeffs: [0.11954846981816165, -0.1358152244503053, -0.003688000623544225,
    0.0026429797169365]
  distortion_model: radtan
  intrinsics: [426.9314446745908, 427.0007202207628, 320.2282058993642, 266.1083473596716]
  resolution: [640, 480]
  rostopic: /cam0/image_raw

2）标定板yaml使用前续2.2.2中创建的标定板yaml文件即可；

#example for aprilgrid
  target_type: 'aprilgrid' #gridtype
  tagCols: 6                  #number of apriltags
  tagRows: 6                  #number of apriltags
  tagSize: 0.021              #size of apriltag, edge to edge [m]
  tagSpacing: 0.2857             #ratio of space between tags to tagSize
                              #example: tagSize=2m, spacing=0.5m --> tagSpacing=0.25[-]

#example for checkerboard
#  targetType: 'checkerboard' #gridtype
#  targetCols: 6              #number of internal chessboard corners
#  targetRows: 7              #number of internal chessboard corners
#  rowsMetricSize: 0.06       #size of one chessboard square [m]
#  colsMetricSize: 0.06       #size of one chessboard square [m]

3）联合标定中imu内参yaml格式如下，并不能直接拿imu_utils的标定结果yaml文件来用，否则会报错。yaml其中各项可依照前续1.2.2imu标定结果中填入avg-axes的对应值。注意rostopic和频率update_rate

rostopic: /imu0
update_rate: 100.0 #Hz

accelerometer_noise_density: 6.2662678044191102e-03 #continous
accelerometer_random_walk: 2.8805164533371702e-04
gyroscope_noise_density: 4.9437760129591220e-04 #continous
gyroscope_random_walk: 3.0009989457718918e-05

注意：
查看 IMU内参yaml、相机内外参yaml里面的rostopic是否与录制的bag话题一致

3.5 联合标定

在kalibr_workspace文件夹下打开终端，输入如下命令

source ~/kalibr_workspace/devel/setup.bash
kalibr_calibrate_imu_camera \
  --target /home/lzy/kalibr_workspace/camera_imu/april_6x6.yaml \
  --cam /home/lzy/kalibr_workspace/camera_imu/camchain.yaml \
  --imu /home/lzy/kalibr_workspace/camera_imu/imu_sbg.yaml \
  --bag /home/lzy/kalibr_workspace/camera_imu/kalibr.bag --bag-from-to 15 45

–target 后面是标定板yaml的绝对路径；
–cam 后面相机内外参yaml（camchain-xxx.yaml）的绝对路径；
–imu 后面是对应格式的imu内参yaml的绝对路径；
–bag 后面是录制的数据包的绝对路径；
–bag-from-to 后面是想要使用数据时间段的起始时间和结束时间，单位：秒(s)
–show-extraction 是在标定过程中的一个显示界面，可以看到图片提取的过程，可以不要。
注意：实际应用时需要将命令里面的参数改为相应的参数，比如路径和yaml

2）标定完成后，会生成下面4个文件。其中比较关注的IMU和相机变换矩阵和timeshift都在以camchain命名的.yaml文件中，在results的txt文件中也可以看到。

实践kalibr参考：
http://t.csdn.cn/mUfC1 【kalibr标定realsenseD435i（二）–多相机标定】
https://guyuehome.com/34592 【Kalibr使用：imu+camera联合标定过程详解】
http://t.csdn.cn/PfkKl 【VIO测试准备——使用imu_utils和kalibr进行相机与IMU标定】，个人感觉比较好
http://t.csdn.cn/ALqjU 【Kalibr 之 Camera-IMU 标定 (总结)】
http://t.csdn.cn/ine8X【kalibr使用笔记】 bug总结

配套安装包
https://download.csdn.net/download/a_happy_bird/85721502