文章目录

一、相机（单目）内参的标定1.1 方案一：MATLAB工具箱1.2 方案二：使用ROS标定工具包1.3 方案三：使用标定工具kalibr1.3.1 安装kalibr1.3.2 准备标定板1.3.3 标定方法 1.4 方案四：编写程序调用OpenCV标定 二、IMU内参的标定三、相机与IMU联合标定四、相机与LiDAR联合标定五、LiDAR与IMU联合标定5.1 方案一：浙大开源lidar_IMU_calib5.2 方案二：lidar-align5.3 方案三：lidar_imu_calib

相机和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 updatesudo 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-devsudo pip install python-igraph --upgrade# 创建工作空间并下载源码mkdir -p kalibr_ws/srccd kalibr_ws/srcgit clone /ethz-asl/kalibr.git# 编译cd .. source /opt/ros/melodic/setup.bashcatkin initcatkin config --extend /opt/ros/melodiccatkin config --merge-devel # Necessary for catkin_tools >= 0.4.catkin config --cmake-args -DCMAKE_BUILD_TYPE=Releasecatkin_make -DCMAKE_BUILD_TYPE=Release -j4

# for Ubuntu20.04# 安装依赖sudo apt updatesudo 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 attrdictpip3 install wxPython# 创建工作空间并下载源码mkdir -p kalibr_ws/srccd kalibr_ws/srcgit clone /ethz-asl/kalibr.git# 编译cd .. source /opt/ros/noetic/setup.bashcatkin initcatkin config --extend /opt/ros/noeticcatkin config --merge-devel # Necessary for catkin_tools >= 0.4.catkin config --cmake-args -DCMAKE_BUILD_TYPE=Releasecatkin_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.04sudo 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.bashrosrun 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' #gridtypetagCols: 6 #number of apriltagstagRows: 6 #number of apriltagstagSize: 0.088#size of apriltag, edge to edge [m]tagSpacing: 0.3 #ratio of space between tags to tagSizecodeOffset: 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: pinholedistortion_coeffs: [-0.294719325148729, 0.08601391290708914, -0.0003238494917967103, -0.00041999959826033857]distortion_model: radtanintrinsics: [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=0Point3f 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 npimport matplotlib.pyplot as plterrorX = 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.bashroslaunch 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.bashrosrun 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' #gridtypetagCols: 6 #number of apriltagstagRows: 6 #number of apriltagstagSize: 0.088#size of apriltag, edge to edge [m]tagSpacing: 0.3 #ratio of space between tags to tagSizecodeOffset: 0 #code offset for the first tag in the aprilboard

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

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

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

rostopic: /imu0update_rate: 200.0 #Hzaccelerometer_noise_density: 0.01 #continousaccelerometer_random_walk: 0.0002gyroscope_noise_density: 0.005 #continousgyroscope_random_walk: 4.0e-06

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

标定完成后自动弹出结果绘图：

同时在数据文件夹里，生成几个结果文件，包括：

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

cam0:T_cam_imu:- [0.0167635457021, 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: pinholedistortion_coeffs: [-0.0016509958435871643, 0.02437222940989351, -0.03582816956989852, 0.019860839087717054]distortion_model: equidistantintrinsics: [461.487246372674, 460.1113992557959, 356.39105303227853, 231.15719697054647]resolution: [752, 480]rostopic: /cam0/image_rawtimeshift_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.01accelerometer_random_walk: 0.0002gyroscope_noise_density: 0.005gyroscope_random_walk: 4.0e-06model: calibratedrostopic: /imu0time_offset: 0.0update_rate: 200.0

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

四、相机与LiDAR联合标定

标定方法是从Autoware分离出来的标定包calibration_camera_lidar

（1）cmake正常安装nlopt

git clone /stevengj/nlopt.gitcd nloptmkdir build && cd buildcmake ..makesudo 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.cmakejsk_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 functionvoid cv::operator>><cv::Mat>(cv::FileNode const&, cv::Mat&)': /usr/local/include/opencv2/core/persistence.hpp:1261: undefined reference tocv::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# 新开一个终端执行roscorerosrun 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-1006.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

项目地址：/APRIL-ZJU/lidar_IMU_calib

（1）编译准备

mkdir -p ~/catkin_li_calib/srccd ~/catkin_li_calib/src# 下载源码git clone /APRIL-ZJU/lidar_IMU_calib# 安装依赖# ndt_ompwstool initwstool merge lidar_IMU_calib/depend_pack.rosinstallwstool updatecd 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_makesource ./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/-06-08-15-52-45_3.bag"/></node></launch>

执行功能包：

roslaunch lidar_imu_calib calib_exR_lidar2imu.launch

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