SLAM各传感器的标定总结：Camera/IMU/LiDAR

ZARD帧心

已于 2023-05-13 21:28:58 修改

阅读量8.1k

点赞数 19

文章标签： matlab linux 计算机视觉自动驾驶 ubuntu

于 2023-04-08 17:25:31 首次发布

本文链接：https://blog.csdn.net/zardforever123/article/details/130030767

版权

文章目录

一、相机（单目）内参的标定
二、IMU内参的标定
三、相机与IMU联合标定
四、相机与LiDAR联合标定
五、LiDAR与IMU联合标定

相机和IMU的内参标定，相机、IMU和LiDAR之间的联合标定方法，其中工具包的安装环境均在Ubuntu20.04环境下，使用的工具包括：MATLAB工具箱、ROS标定工具包、kalibr、OpenCV、imu_utils、calibration_camera_lidar、lidar_imu_calib

一、相机（单目）内参的标定

1.1 方案一：MATLAB工具箱

这个是最方便的，参考（文章），打开matlab，找到APP，下拉三角查看更多选项找到Camera Calibrator（还提供了双目标定的程序，这里演示单目）:

在这里插入图片描述

添加拍摄好的标定板的图片（可以是方案二保存的图像序列），拍摄标定板时最好占满半屏以上，有前后左右的平移，有旋转。

在这里插入图片描述

输入标定板中相邻棋盘格之间实际距离，标定精度很大程度取决于标定板的精度，我这里是35mm，点击确定之后程序自动提取角点并匹配，结果如下：

在这里插入图片描述

选择标定的畸变参数类型，径向畸变以及切向畸变均有，然后点击calibrate计算。

在这里插入图片描述

右侧会显示每帧的平均重投影误差和所有帧的平均重投影误差，以及各帧图像的位姿可视化。可以看到有两帧重投影误差明显偏大，点击查看图像有运动模糊，可以在左侧对应帧的图像上右键删除，然后重新计算，最后结果的平均重投影误差0.16个像素，精度已经十分高了。

在这里插入图片描述

点击输出相机标定参数，查看标定参数变量：

在这里插入图片描述

1.2 方案二：使用ROS标定工具包

参考我这篇文章（链接）的7.1节。

1.3 方案三：使用标定工具kalibr

kalibr内部提供了多IMU标定、相机IMU标定，多相机标定的功能，这里使用多相机标定命令实现对单个相机的标定

1.3.1 安装kalibr

# for Ubuntu18.04
# 安装依赖
sudo apt update
sudo apt-get install python-setuptools python-rosinstall ipython libeigen3-dev libboost-all-dev doxygen libopencv-dev ros-melodic-vision-opencv ros-melodic-image-transport-plugins ros-melodic-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
sudo pip install python-igraph --upgrade
# 创建工作空间并下载源码
mkdir -p kalibr_ws/src
cd kalibr_ws/src
git clone https://github.com/ethz-asl/kalibr.git
# 编译
cd .. 
source /opt/ros/melodic/setup.bash
catkin init
catkin config --extend /opt/ros/melodic
catkin config --merge-devel # Necessary for catkin_tools >= 0.4.
catkin config --cmake-args -DCMAKE_BUILD_TYPE=Release
catkin_make -DCMAKE_BUILD_TYPE=Release -j4

# for Ubuntu20.04
# 安装依赖
sudo apt update
sudo apt-get install python3-setuptools python3-rosinstall ipython3 libeigen3-dev libboost-all-dev doxygen libopencv-dev ros-noetic-vision-opencv ros-noetic-image-transport-plugins ros-noetic-cmake-modules python3-software-properties software-properties-common libpoco-dev python3-matplotlib python3-scipy python3-git python3-pip libtbb-dev libblas-dev liblapack-dev libv4l-dev python3-catkin-tools python3-igraph libsuitesparse-dev libgtk-3-dev
# 下面两个可能没反映或者错误，请等待和多尝试几次
pip3 install attrdict
pip3 install wxPython
# 创建工作空间并下载源码
mkdir -p kalibr_ws/src
cd kalibr_ws/src
git clone https://github.com/ethz-asl/kalibr.git
# 编译
cd .. 
source /opt/ros/noetic/setup.bash
catkin init
catkin config --extend /opt/ros/noetic
catkin config --merge-devel # Necessary for catkin_tools >= 0.4.
catkin config --cmake-args -DCMAKE_BUILD_TYPE=Release
catkin_make -DCMAKE_BUILD_TYPE=Release -j8

1.3.2 准备标定板

准备aprilgrid标定板，这是kalibr推荐使用的标定板，如下图所示。标定板可以去官网下载作者提供的，但是需要科学上网，因此可以用作者的功能包生成：

rosrun kalibr kalibr_create_target_pdf --type apriltag --nx [NUM_COLS] --ny [NUM_ROWS] --tsize [TAG_WIDTH_M] --tspace [TAG_SPACING_PERCENT]
eg: rosrun kalibr kalibr_create_target_pdf --type apriltag --nx 8 --ny 8 --tsize 0.1 --tspace 0.3
# 作者给的：
eg: rosrun kalibr kalibr_create_target_pdf --type apriltag --nx 6 --ny 6 --tsize 0.088 --tspace 0.3

若报错：No module named ‘pyx’，参考github的issues解决：

sudo apt install python-pyx #18.04
sudo apt install python3-pyx #20.04

就会在当前目录下生成pdf文件：

在这里插入图片描述

1.3.3 标定方法

（1）首先录制数据，驱动自己的相机，录制数据时上下左右前后平移和旋转各三次，最后还有无规则运动加强鲁棒性。

rosbag record /image_raw -O images.bag

kalibr推荐的帧率为4Hz, 这里有一个降帧的命令，可以将原本的rosbag中的old_topic降帧成4hz的new_topic, 然后边rosbag play, 边rosbag record new_topic:

rosrun topic_tools throttle messages old_topic 4.0 new_topic

（2）接下来就可以标定了

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

其中，几个参数文件需要注意一下：

--bag images.bag：录制的数据包路径；
--bag-from-to 5 20 是指从bag的第5s读到第20s，就是把一头一尾截掉一部分，因为在开始和结束可能有一些剧烈的抖动，会导致错误，你要是整个过程都很平稳，不截掉也行；
--target april_6x6_24x24mm.yaml：标定板参数文件，其中tagSize和tagSpacing两个参数作者特别说明了含义，根据自己打印的标定板修改

target_type: 'aprilgrid'   #gridtype
tagCols: 6              #number of apriltags
tagRows: 6             #number of apriltags
tagSize: 0.088          #size of apriltag, edge to edge [m]
tagSpacing: 0.3         #ratio of space between tags to tagSize
codeOffset: 0           #code offset for the first tag in the aprilboard

在这里插入图片描述

--models pinhole-radtan：相机模型，这里是针孔相机模型，pinhole-radtan 是最常用的相机模型，包括了径向畸变和切向畸变，更多可以参考作者给的相机模型说明；
--topics /image_raw：图像话题；
--show-extraction：可以显示图像，可以看到提取的特征点。

标定完成后，自动弹出绘图结果（我用的这个数据不太对，重投影误差有点大，自己多录几个数据）：

在这里插入图片描述

同时生成以下几个结果文件：

相机内参：

cam0:
  cam_overlaps: []
  camera_model: pinhole
  distortion_coeffs: [-0.294719325148729, 0.08601391290708914, -0.0003238494917967103, -0.00041999959826033857]
  distortion_model: radtan
  intrinsics: [461.14152593310064, 459.7917242849438, 363.2138823275693, 235.61324563304655]
  resolution: [752, 480]
  rostopic: /cam0/image_raw

标定结果的txt文档；
标定结果绘图的pdf。

1.4 方案四：编写程序调用OpenCV标定

参考《OpenCV4快速入门》这本书

#include <opencv2/opencv.hpp>
#include <fstream>
#include <iostream>
#include <vector>
#include <cstring>
#include <sstream>
#include <cstdlib>
using namespace std;
using namespace cv;

void initUndistAndRemap(vector<Mat> imgs, Mat cameraMatrix, Mat distCoeffs, Size imageSize, vector<Mat> &undistImgs)
{
    // 计算映射坐标矩阵
    Mat R = Mat::eye(3, 3, CV_32F);
    Mat mapx = Mat(imageSize, CV_32FC1);
    Mat mapy = Mat(imageSize, CV_32FC1);
    // 内参矩阵/畸变系数/...
    initUndistortRectifyMap(cameraMatrix, distCoeffs, R, cameraMatrix, imageSize, CV_32FC1, mapx, mapy);
    for (int i = 0; i < imgs.size(); i++)
    {
        Mat undistImg;
        remap(imgs[i], undistImg, mapx, mapy, INTER_LINEAR);
        undistImgs.push_back(undistImg);
    }
}

void undist(vector<Mat> imgs, Mat cameraMatrix, Mat distCoeffs, vector<Mat> &undistImgs)
{
    for (int i = 0; i < imgs.size(); i++)
    {
        Mat undistImg;
        // 单幅图像去畸变:畸变图像/去畸变后的图像/内参矩阵/畸变系数
        undistort(imgs[i], undistImg, cameraMatrix, distCoeffs);
        undistImgs.push_back(undistImg);
    }
}

bool LoadData(const string &imagePath, const string &imageFilename, vector<Mat> &imgs)
{
    ifstream Left;
    Left.open(imageFilename.c_str());
    while (!Left.eof())
    {
        string s;
        getline(Left, s);
        if (!s.empty())
        {
            stringstream ss;
            ss << s;
            double t;
            string imageName;
            ss >> t;
            ss >> imageName;
            Mat img = imread(imagePath + "/" + imageName);
            imgs.push_back(img);
            if (!img.data)
            {
                cout << "请输入正确的图像文件" << endl;
                return 0;
            }
        }
    }

    return 1;
}

int main(int argc, char **argv)
{
    if (argc != 6)
    {
        cerr << endl
             << "Usage: ./CameraCalibration path_to_CalibrateImage path_to_calibdata.txt board_size_cols board_size_rows corners_of_checkerboard(mm)" << endl
             << "eg: ./CameraCalibration ../CalibrateData ../CalibrateData/calibdata.txt 9 6 10" << endl;

        return 1;
    }

    // -1 读取数据
    vector<Mat> imgs;
    LoadData(argv[1], argv[2], imgs);
    // 棋盘格内角点行列数
    int bcols, brows;
    // 棋盘格每个方格的真实尺寸
    double side_length;
    stringstream ss;
    ss << argv[3];
    ss >> bcols;
    ss.clear();
    ss << argv[4];
    ss >> brows;
    ss.clear();
    ss << argv[5];
    ss >> side_length;
    Size board_size = Size(bcols, brows);

    // -2 提取并计算标定板角点像素坐标
    // 多副图像分别放入vector<vector<Point2f>>
    vector<vector<Point2f>> imgsPoints;
    for (int i = 0; i < imgs.size(); i++)
    {
        Mat img1 = imgs[i], gray1;
        cvtColor(img1, gray1, COLOR_BGR2GRAY);
        vector<Point2f> img1_points;
        // 第一个参数是输入的棋盘格图像
        // 第二个参数是棋盘格内部的角点的行列数（注意：不是棋盘格的行列数，而内部角点-不包括边缘-的行列数）
        // 第三个参数是检测到的棋盘格角点，类型为std::vector<cv::Point2f>
        bool ret = findChessboardCorners(gray1, board_size, img1_points);

        // 细化标定板角点坐标（亚像素）,Size(5, 5)为细化方格坐标领域范围
        find4QuadCornerSubpix(gray1, img1_points, Size(5, 5));

        // 第一个参数是棋盘格图像（8UC3）既是输入也是输出
        // 第二个参数是棋盘格内部角点的行、列
        // 第三个参数是检测到的棋盘格角点
        // 第四个参数是cv::findChessboardCorners()的返回值。
        drawChessboardCorners(img1, board_size, img1_points, ret);
        // string windowNumber = to_string(i);
        // imshow("chessboard corners"+windowNumber, img1);

        imgsPoints.push_back(img1_points);
    }

    // -3 使用棋盘格每个内角点的世界坐标
    vector<vector<Point3f>> objectPoints; // 空间三维坐标（位于一个平面内，以此为xy坐标平面）
    for (int i = 0; i < imgsPoints.size(); i++)
    {
        vector<Point3f> tempPointSet;
        for (int j = 0; j < board_size.height; j++)
        {
            for (int k = 0; k < board_size.width; k++)
            {
                // 假设标定板为世界坐标系的z平面，即z=0
                Point3f realPoint;
                realPoint.x = j * side_length;
                realPoint.y = k * side_length;
                realPoint.z = 0;
                tempPointSet.push_back(realPoint);
            }
        }
        objectPoints.push_back(tempPointSet);
    }

    // -4 内参及畸变标定
    Size imageSize; // 图像尺寸
    imageSize.width = imgs[0].cols;
    imageSize.height = imgs[0].rows;
    // 定义内外参
    Mat cameraMatrix = Mat(3, 3, CV_32FC1, Scalar::all(0)); // 相机内参数矩阵
    Mat distCoeffs = Mat(1, 5, CV_32FC1, Scalar::all(0));   // 相机的5个畸变系数
    vector<Mat> rvecs, tvecs;                               // 每幅图像的旋转向量/平移向量
    // 调用OpenCV标定函数
    calibrateCamera(objectPoints, imgsPoints, imageSize, cameraMatrix, distCoeffs, rvecs, tvecs, 0);
    cout << "相机的内参矩阵K=" << endl
         << cameraMatrix << endl;
    cout << "相机畸变系数:" << endl
         << distCoeffs << endl;

    // -5 保存数据
    ofstream calibrateFile;
    // 写入数据:
    calibrateFile.open("../calibrateCamera.txt"); // 没有自动创建
    if (!calibrateFile.is_open())                 // 文件是否打开
    {
        exit(EXIT_FAILURE); // 终止程序
    }
    calibrateFile << fixed;     // 开始写入,与cout相同
    calibrateFile.precision(5); // 写入小数精度
    calibrateFile << "cameraMatrix K=" << endl
                  << cameraMatrix; // 写入数据（覆盖
    calibrateFile << endl << "distCoeffs=" << endl
                  << distCoeffs;
    calibrateFile.close();

    // -6 图像去畸变
    vector<Mat> undistImgs;
    // 使用initUndistortRectifyMap()函数和remap()函数校正图像
    initUndistAndRemap(imgs, cameraMatrix, distCoeffs, imageSize, undistImgs); // 畸变图像/前文计算得到的内参矩阵/畸变系数/图像尺寸/去畸变后的图像,自定义函数是为了处理多副图像
    // undist(imgs, cameraMatrix, distCoeffs, undistImgs);//用undistort()函数直接计算校正图像,自定义函数是为了处理多副图像
    // 显示校正前后的图像(一张示例)
    for (int i = 0; i < 1; i++)
    {
        string windowNumber = to_string(i);
        imshow("chessboard corners without undistort -- image" + windowNumber, imgs[i]);
        imshow("chessboard corners with undistort -- image" + windowNumber, undistImgs[i]);
        imwrite("../chessboard corners without undistort.png", imgs[i]);
        imwrite("../chessboard corners with undistort.png", undistImgs[i]);
    }
    waitKey(0);

    // -7 单目投影(重投影)：根据成像模型及空间点三位坐标计算图像二维坐标
    vector<vector<Point2f>> imagePoints; // 存放二维坐标
    // 根据三维坐标和相机与世界坐标系时间的关系估计内角点像素坐标
    for (int i = 0; i < imgs.size(); i++)
    {
        Mat rvec = rvecs[i], tvec = tvecs[i];
        vector<Point3f> PointSets = objectPoints[i];
        vector<Point2f> imagePoint;                                                 // 存放二维坐标
        projectPoints(PointSets, rvec, tvec, cameraMatrix, distCoeffs, imagePoint); // 输入三维点/旋转及平移向量/前文计算得到的内参矩阵和畸变矩阵/输出二维点
        imagePoints.push_back(imagePoint);
    }

    // -8 计算重投影误差
    vector<vector<double>> ReProjectionError;
    vector<vector<double>> ReProjectionErrorX;
    vector<vector<double>> ReProjectionErrorY;
    double e = 0.0;
    for (int i = 0; i < imgs.size(); i++)
    {
        vector<Point2f> imagePoint = imagePoints[i]; // 存放二维坐标
        vector<double> er;
        vector<double> erX;
        vector<double> erY;
        for (int j = 0; j < imagePoint.size(); j++)
        {
            double eX = imagePoint[j].x - imgsPoints[i][j].x;
            double eY = imagePoint[j].y - imgsPoints[i][j].y;
            double error = sqrt(pow(eX, 2) + pow(eY, 2));
            erX.push_back(eX);
            erY.push_back(eY);
            er.push_back(error);
            e += error;
        }
        ReProjectionError.push_back(er);
        ReProjectionErrorX.push_back(erX);
        ReProjectionErrorY.push_back(erY);
    }
    // 计算估计值和图像中计算的真实时之间的平均误差
    cout << "平均重投影误差:" << e / (imagePoints[0].size() * imgs.size()) << endl;

    // -9 保存重投影误差数据
    ofstream ReProjectionErrorFile;
    ReProjectionErrorFile.open("../ReProjectionError.txt");
    ofstream ReProjectionErrorFileX;
    ReProjectionErrorFileX.open("../ReProjectionErrorX.txt");
    ofstream ReProjectionErrorFileY;
    ReProjectionErrorFileY.open("../ReProjectionErrorY.txt");
    if (!ReProjectionErrorFile.is_open() || !ReProjectionErrorFileX.is_open() || !ReProjectionErrorFileY.is_open())
    {
        exit(EXIT_FAILURE);
    }
    ReProjectionErrorFile << fixed;
    ReProjectionErrorFile.precision(5);
    ReProjectionErrorFileX << fixed;
    ReProjectionErrorFileX.precision(5);
    ReProjectionErrorFileY << fixed;
    ReProjectionErrorFileY.precision(5);
    for (int i = 0; i < imgs.size(); i++)
    {
        for (int j = 0; j < imagePoints[i].size(); j++)
        {
            ReProjectionErrorFile << ReProjectionError[i][j] << " ";
            ReProjectionErrorFileX << ReProjectionErrorX[i][j] << " ";
            ReProjectionErrorFileY << ReProjectionErrorY[i][j] << " ";
        }
        ReProjectionErrorFile << endl;
        ReProjectionErrorFileX << endl;
        ReProjectionErrorFileY << endl;
    }
    ReProjectionErrorFile.close();
    ReProjectionErrorFileX.close();
    ReProjectionErrorFileY.close();

    /*
    // 圆形标定板角点提取
    Mat img2 = imread("../CalibrateData/circle.png");
    Mat gray2;
    cvtColor(img2, gray2, COLOR_BGR2GRAY);
    Size board_size2 = Size(7, 7);          //圆形标定板圆心数目（行，列）
    vector<Point2f> img2_points;//检测角点，单副图像放入vector<Point2f>
    findCirclesGrid(gray2, board_size2, img2_points);           //计算圆形标定板检点
    find4QuadCornerSubpix(gray2, img2_points, Size(5, 5));      //细化圆形标定板角点坐标
    drawChessboardCorners(img2, board_size2, img2_points, true);
    imshow("圆形标定板角点检测结果", img2);
    waitKey();
    */

    return 0;
}

以书中的数据为例：

在这里插入图片描述

可以看到畸变校正前后的差别，图像中的直线不再变形

在这里插入图片描述
使用Python绘制重投影误差：

#!/usr/bin/env python
# -*- coding: utf-8 -*-
# 编     写：ZARD帧心
import numpy as np
import matplotlib.pyplot as plt

errorX = np.genfromtxt('ReProjectionErrorX.txt')  # 将文件中数据加载到数组里
errorY = np.genfromtxt('ReProjectionErrorY.txt')
(m, n) = np.shape(errorX)
error = (errorX**2 + errorY**2)**0.5

# 画每帧图像的重投影误差均值
error_image =  np.average(error, axis=1)  # 按行求均值
image = range(m)
plt.bar(image, error_image)
plt.xlabel('Image number',{'Size':30})
plt.ylabel('ReProjection Error (pixel)',{'Size':30})
plt.xticks(fontsize=30) #x轴刻度字体大小
plt.yticks(fontsize=30) #y轴刻度字体大小
plt.show()

# 画所有角点的重投影误差
x = errorX.reshape(m*n,1)
y = errorY.reshape(m*n,1)
r = error.reshape(m*n,1) # 用于映射颜色的数值
# np.set_printoptions(threshold=np.inf)
# print(r)
# print(max(r))
h = plt.scatter(x, y, c=r, marker='*', alpha=0.8, cmap='viridis')
plt.xlabel('X error (pixel)',{'Size':30})
plt.ylabel('Y error (pixel)',{'Size':30})
plt.xticks(fontsize=30) #x轴刻度字体大小
plt.yticks(fontsize=30) #y轴刻度字体大小
cb=plt.colorbar(h) # 显示颜色条
font = {'weight' : 'normal',
        'size'   : 30, }
cb.ax.tick_params(labelsize=30)  #设置色标刻度字体大小。
cb.set_label('ReProjection Error',fontdict=font) #设置colorbar的标签字体及其大小
plt.show()

下面是绘制的和方案一MATLAB同样的数据结果，和MATLAB差远了，可能是我代码写的有问题，如果有人看出哪里需要修改请务必告诉我，谢谢
在这里插入图片描述

在这里插入图片描述

后来我又用一个简单的方法剔除粗差，得到了下面的结果，好一点但仍然和MATLAB差远了

在这里插入图片描述

二、IMU内参的标定

用imu_utils标定IMU，参考：文章
（1）安装依赖项

sudo apt-get install libdw-dev

（2）下载imu_utils和code_utils，但是gaowenliang的imu_utils有单位的问题，使用mintar版本的imu_utils
（3）在编译之前先注意几个问题：

code_utils依赖eigen、OpenCV3、ceres，这些是我以前文章的链接，有他们的安装教程；
不要同时编译imu_utils和code_utils，imu_utils 依赖 code_utils，所以先编译code_utils再编译imu_utils。

（4）放到你的工作空间中编译：

catkin_make

编译报错：code_utils-master/src/sumpixel_test.cpp:2:24: fatal error: backward.hpp:No such file or directory，在code_utils下面找到sumpixel_test.cpp，修改

// #include "backward.hpp"
// 为 
#include “code_utils/backward.hpp”

若遇见一大堆C++标准库的错误，首先想到尝试修改C++标准：

# set(CMAKE_CXX_FLAGS "-std=c++11")
set(CMAKE_CXX_FLAGS "-std=c++14")

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

rosbag record /imu -O imu.bag

（5）标定IMU，这里以imu_utils的示例数据为例：

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

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

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

标定完成后，会在$(find imu_utils)/data/下生成mynteye_imu_param.yaml以及多个txt文档：
在这里插入图片描述

三、相机与IMU联合标定

使用标定工具kalibr，前面已经安装，别的教程给的推荐频率是相机20Hz，IMU 200Hz。由于我无法科学上网，下不了作者给的示例数据，这里的数据是从这篇博客获得：Ubuntu18.04+kalibr标定工具箱安装编译
（1）录制
订阅两个消息，图像和imu的topic：

rosbag record /imu_raw /image_raw

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

在这里插入图片描述

（2）标定

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

与1.3类似，参数：

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

target_type: 'aprilgrid'   #gridtype
tagCols: 6              #number of apriltags
tagRows: 6             #number of apriltags
tagSize: 0.088          #size of apriltag, edge to edge [m]
tagSpacing: 0.3         #ratio of space between tags to tagSize
codeOffset: 0           #code offset for the first tag in the aprilboard

--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内参，按照下面格式填写

rostopic: /imu0
update_rate: 200.0 #Hz

accelerometer_noise_density: 0.01 #continous
accelerometer_random_walk: 0.0002

gyroscope_noise_density: 0.005 #continous
gyroscope_random_walk: 4.0e-06

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

标定完成后自动弹出结果绘图：
在这里插入图片描述
同时在数据文件夹里，生成几个结果文件，包括：

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

cam0:
  T_cam_imu:
  - [0.01676354570202121, 0.9998590327410452, -0.000947724452812317, 0.06843047675330072]
  - [-0.9998594579273542, 0.016763745309816058, 0.0002030674908090371, -0.014939612219976138]
  - [0.00021892627629230532, 0.0009441871264906492, 0.999999530290868, -0.0038646995667698156]
  - [0.0, 0.0, 0.0, 1.0]
  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
  timeshift_cam_imu: 5.9040012306710654e-05

IMU内外参：

imu0:
  T_i_b:
  - [1.0, 0.0, 0.0, 0.0]
  - [0.0, 1.0, 0.0, 0.0]
  - [0.0, 0.0, 1.0, 0.0]
  - [0.0, 0.0, 0.0, 1.0]
  accelerometer_noise_density: 0.01
  accelerometer_random_walk: 0.0002
  gyroscope_noise_density: 0.005
  gyroscope_random_walk: 4.0e-06
  model: calibrated
  rostopic: /imu0
  time_offset: 0.0
  update_rate: 200.0

标定结果的txt文档
标定结果绘图的pdf文档

在这里插入图片描述

四、相机与LiDAR联合标定

标定方法是从Autoware分离出来的标定包calibration_camera_lidar
（1）cmake正常安装nlopt

git clone https://github.com/stevengj/nlopt.git
cd nlopt
mkdir build && cd build
cmake ..
make
sudo make install

注意不要使用sudo apt-get install安装，后面catkin_make会遇到问题
（2）编译calibration_camera_lidar：

catkin_make

旧包需要添加melodic或noetic版本ROS的支持，修改CMakelists.txt 72/94/114行：

# if ("{ROS_VERSION}" MATCHES "(indigo|jade|kinetic)")
if ("{ROS_VERSION}" MATCHES "(indigo|jade|kinetic|melodic|noetic)")

如果报错

jsk_recognition_msgsConfig.cmake
jsk_recognition_msgs-config.cmake

说明编译缺少jsk-recognition-msgs包，安装：

sudo apt-get install ros-<ROS VERSION NAME>-jsk-recognition-msgs

报错：error: ‘CV_RGB’ was not declared in this scope; did you mean ‘CV_YUV2RGB’?根据位置提示把报错位置的CV_RGB改为cvScalar
报错： error: no match for ‘operator=’ (operand types are ‘CvMat’ and ‘cv::Mat’)
这是CvMat与cv::Mat相互转化的问题，将错误的两句改为：

*m_intrinsic =  cvMat(m_intrinsic_opencv2to1);
*m_dist = cvMat(m_dist_opencv2to1);

报错：cv_image->image, 将报错的cv_image->image改为cvIplImage(cv_image->image)
注意在不同版本上，cvIplimage可能写为Iplimage，cvMat可能写为CvMat。

以上这些都是OpenCV版本的问题，代码基于opencv2写的，与opencv3/4不兼容

CMakeFiles/calibration_publisher.dir/nodes/calibration_publisher/calibration_publisher.cpp.o: In function void cv::operator>><cv::Mat>(cv::FileNode const&, cv::Mat&)': /usr/local/include/opencv2/core/persistence.hpp:1261: undefined reference to cv::Mat::Mat()’
在CMakeLists.txt中为calibration_publisher节点链接opencv

target_link_libraries(calibration_publisher
        ${catkin_LIBRARIES}
        ${OpenCV_LIBS})

（3）录包：选择小车固定，移动标定板，标定板移动与相机标定类似，相对小车前方远近左右，倾斜各种姿态均有（可以选择使用Autoware或者rosbasg record TOPIC录制）；
（4）执行功能包：

source devel/setup.bash
# 新开一个终端执行roscore
rosrun calibration_camera_lidar calibration_toolkit

（5）标定方法
1）使用Autoware播放和暂停数据集；
2）启动之后选择图像topic（根据自己发布的话题选择）；
3）选择标定类型：相机到Velodyne；
因为工具基于autoware，所以此处的点云topic需要转换为/points_raw，如果点云话题不对，可通过下面两种方式修改：

直接在雷达驱动的参数配置里更改帧ID为：velodyne，topic为：/points_raw
通过rosbag播放时，进行话题转换
例如：

rosbag play autoware-20221006.bag /rslidar_points:=/points_raw

4）点击OK弹出标定的界面，界面上方文本框要填写的pattern size是标定板棋盘格边长(m)，pattern number是角点尺寸（注意是内部角点数，也就是棋盘格格子长宽数-1）

5）标定前，导入相机内参标定的yml文件（需要是使用本功能包标定的结果，且这个步骤不是必要，因为下面的步骤会同时标定相机内参），点击load导入文件；导入完成后，cameramat /distcoeff /imagesize会显示对应内参数值；

6）调整窗口尺寸，鼠标点击黑色部分按b键调整颜色，按2切换模式，调整视角，显示出点云数据，LiDAR可视化调整方法：
移动点云：上下左右方向键、PgUp、PgDn
旋转点云：a、d、w、s、q、e
切换模式：数字1和数字2
视角缩放：减号缩小、加号放大
点云大小：o键缩小点云、p放大点云（建议放大，较为清晰）
改变点云窗口背景颜色：b（建议背景全部调整成白色，清晰）

播放到合适（标定板在图像和点云中均完整和清晰）的帧之后暂停数据集，点击Grab添加一帧数据到备选标定序列。然后调整右下角激光数据角度，使坐标轴打到标定板中心，点击左键标红中心，添加到标定帧。标红点错的时候右键可以取消标红，如果这个备选帧都不满意可以点击右上角Remove。如下图所示：

在这里插入图片描述

标30帧左右，然后点击calibrate开始计算标定结果，CameraExtrinsicMat /cameramat /distcoeff /imagesize会显示对应参数值。结果计算好之后可以点击project把标定板上的激光点投影到图像上来大致查看标定的正确与否，图中红色点，最后点击左上角Save保存标定结果（仅保存联合标定结果，包括相机的内外参：相机坐标系到雷达坐标系；保存成.yml文件，不保存标定使用的数据，因为数据太大了，如果需要可以保存）

在这里插入图片描述

（7）结果保存后可以通过Autoware可视化查看相机与LiDAR标定的融合结果，Simulation播放数据，在Sensing界面点击Calibration Publisher加载标定结果文件，target_frame标定数据中激光雷达frame ID；camera_frame 标定数据中视觉相机 frame ID；在ref选择栏中，加载yml文件

再点击points image，然后打开rviz。打开菜单栏Panels-Add New Panel，选择ImageViewPlugin插件，选择图像和点云话题，然后继续播放数据集，就可以看到图像与点云的融合情况

五、LiDAR与IMU联合标定

使用标定包lidar_imu_calib。前面两章标定之后，其实可以直接推算LiDAR与IMU的外参，但是标定一下也可以确认前面标定是否正确，或者设备无视觉传感器。

5.1 方案一：浙大开源lidar_IMU_calib

项目地址：https://github.com/APRIL-ZJU/lidar_IMU_calib
（1）编译准备

mkdir -p ~/catkin_li_calib/src
cd ~/catkin_li_calib/src

# 下载源码
git clone https://github.com/APRIL-ZJU/lidar_IMU_calib

# 安装依赖
# ndt_omp
wstool init
wstool merge lidar_IMU_calib/depend_pack.rosinstall
wstool update
cd lidar_imu_calib
./build_submodules.sh

pcl-1.10需要用c++14及以上编译，在ndt_omp的CMakeLists.txt中修改
我们已经在系统安装过Pangolin，将PANGOLIN_DIR的路径注释掉

# set(PANGOLIN_DIR "${CMAKE_CURRENT_SOURCE_DIR}/thirdparty/build-pangolin")
find_package(Pangolin REQUIRED)

（2）编译

cd ../..
catkin_make
source ./devel/setup.bash

报错fatal error: pcap.h: No such file or directory #include <pcap.h>，安装：

sudo apt-get install libpcap-dev

（3）标定
录制标定数据集时，尽量在室内平面较多的地方，也要注意不要在太过狭窄的小房间内。标定之前，需要在licalib_gui.launch文件中更改录制的bag包的地址，和持续时间，以及lidar的类型，话题名：

<?xml version="1.0"?>
<launch>
    <arg name="topic_imu"           default="/imu1/raw/data_withSync" />
    <arg name="path_bag"            default="/home/$(env USER)/rosbag/VLP_16/Garage-01.bag" />
    <arg name="bag_start"           default="1" />
    <arg name="bag_durr"            default="30" />
    <arg name="scan4map"            default="15" />
    <arg name="lidar_model"         default="VLP_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="false" />

    <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="/velodyne_packets" />
        <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>

运行：
1）calib.sh（这种方法还需要在callib.sh中修改相关的参数）

./src/lidar_IMU_calib/calib.sh

2）运行launch文件

roslaunch li_calib licalib_gui.launch

5.2 方案二：lidar-align

5.3 方案三：lidar_imu_calib

这个功能包只校准激光雷达和 imu 之间转换中的姿态分量
（1）从github下载源码，放到自己的工作空间下编译：

catkin_make

如果遇到大量PCL报错，是因为使用的ubuntu 20.04的ROS版本是noetic，其自带的pcl是pcl-1.10，这个版本需要用c++14及以上编译，在CMakeLists.txt中修改
（2）录包：录制的环境尽量在平面与线特征较多的地方，例如结构较丰富的建筑物，不要在退化环境（大量树木灌木或者隧道），小车缓慢运动
（3）标定

修改Launch文件中IMU、LiDAR消息话题名称以及bag路径：

<launch>
    <node pkg="lidar_imu_calib" type="calib_exR_lidar2imu_node" name="calib_exR_lidar2imu_node" output="screen">
        <param name="/lidar_topic" value="/velodyne_points"/>
        <param name="/imu_topic" value="/imu/data"/>
        <param name="/bag_file" value="/media/zard/SLAMData/2017-06-08-15-52-45_3.bag"/>
    </node>
</launch>

执行功能包：

roslaunch lidar_imu_calib calib_exR_lidar2imu.launch

功能包会显示逐步添加lidar消息，最后会显示 lidar的消息数量和imu的数量，形成约束的数量，再等一下就会出现计算的结果：

在这里插入图片描述