文章目录
一、相机(单目)内参的标定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 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# 新开一个终端执行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 --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的数量,形成约束的数量,再等一下就会出现计算的结果: