0. 简介
作为自动驾驶行业最头疼的问题之一,外参标定一直以来受到广泛的关注,尤其是最常使用的激光雷达与相机的外参标定。之前在文章:3D雷达与相机的标定方法详细教程与多传感器融合感知 --传感器外参标定及在线标定学习均有提及,但是作者在调研时候发现IROS 2022年的一篇论文也讲述了一种新型的自动标定方案《Joint Camera Intrinsic and LiDAR-Camera Extrinsic Calibration》。下面我们来详细的分析一下。
1. 常见的联合标定方法
常见的摄像头-激光雷达标定方法主要分为两大类,一种是基于目标的联合标定,还有就是基于无目标的联合标定。
基于目标联合标定:这里所说的目标是指人为设计的参照物,比如棋盘格等,主要的算法是通过检测图像和激光雷达的角点,设计重投影的误差函数来完成外参的获取。
无目标联合标定:一般适用于动态标定中,依据车身周围的环境作为参照,例如匹配车道线等来完成联合标定。
本文作为一种基于联合目标的标定方法,其优点在于精度高,如果需要学习无目标联合标定的同学可以来看一下https://github.com/lovelyyoshino/SensorsCalibration这个项目。
2. 代码详解
文中做了一系列的工作用于实现激光与雷达的外参标定。首先文中提出了一种全新的标定板,用于联合标定任务,在这个标定板下可以完成相机内参,畸变因子,LiDAR-Camera外参的联合标定。设计一套数据采集,标定数据处理方案。注释版请参考链接:https://github.com/lovelyyoshino/JointCalib
首先通过检测标定板,定位角点和圆孔中心点,通过最小化3d-2d点对的重投影误差(点对源自标定板的圆孔中心和棋盘格的角点。)构建摄像头-激光雷达的非线性优化函数
// 输出rvecs :旋转向量,tvecs :位移向量
double re_error =
cv::calibrateCamera(_boards_pts_3d, _imgs_pts, image_size, camera_matrix,
k, rvecsMat, tvecsMat, CV_CALIB_FIX_PRINCIPAL_POINT); // 从make_board_points拿到的点,来完成重投影误差
std::cout << "reprojection is " << re_error << std::endl;
Eigen::Matrix3d camera_intrinsic; // 内参矩阵
camera_intrinsic << camera_matrix.at<double>(0, 0),
camera_matrix.at<double>(1, 0), camera_matrix.at<double>(2, 0),
camera_matrix.at<double>(0, 1), camera_matrix.at<double>(1, 1),
camera_matrix.at<double>(2, 1), camera_matrix.at<double>(0, 2),
camera_matrix.at<double>(1, 2), camera_matrix.at<double>(2, 2);
Eigen::VectorXd distort(2);
distort << k.at<double>(0, 0), k.at<double>(0, 1); //畸变矩阵
std::vector<Eigen::MatrixXd> vec_extrinsics;
for (size_t i = 0; i < rvecsMat.size(); i++)
{
cv::Mat rvec = rvecsMat[i];
cv::Mat tvec = tvecsMat[i];
cv::Mat rot;
cv::Rodrigues(rvec, rot); // 旋转矩阵
std::vector<cv::Point2f> imgpoints_cir;
cv::projectPoints(_boards_pts_cir[i], rvecsMat[i], tvecsMat[i],
camera_matrix, k, imgpoints_cir); // _boards_pts_cir三维点投影到图像平面
size_t y_min = imgpoints_cir[0].y;
size_t y_max = imgpoints_cir[0].y;
size_t x_min = imgpoints_cir[0].x;
size_t x_max = imgpoints_cir[0].x;
for (size_t i = 1; i < 4; i++)
{
if (imgpoints_cir[i].y > y_max) // 找到图像平面上的最大和最小y值
y_max = imgpoints_cir[i].y;
if (imgpoints_cir[i].y < y_min)
y_min = imgpoints_cir[i].y;
if (imgpoints_cir[i].x > x_max) // 找到图像平面上的最大和最小x值
x_max = imgpoints_cir[i].x;
if (imgpoints_cir[i].x < x_min)
x_min = imgpoints_cir[i].x;
}
std::vector<cv::Point2f> imgpoints_cir1; // scl
// 找到对应的四个圆心点
for (size_t i = 0; i < 4; i++)
{
if ((imgpoints_cir[i].x - x_min) <= 50 &&
(imgpoints_cir[i].y - y_min) <= 50)
imgpoints_cir1.push_back(imgpoints_cir[i]);
}
for (size_t i = 0; i < 4; i++)
{
if ((x_max - imgpoints_cir[i].x) <= 50 &&
(imgpoints_cir[i].y - y_min) <= 50)
imgpoints_cir1.push_back(imgpoints_cir[i]);
}
for (size_t i = 0; i < 4; i++)
{
if ((imgpoints_cir[i].x - x_min) <= 50 &&
(y_max - imgpoints_cir[i].y) <= 50)
imgpoints_cir1.push_back(imgpoints_cir[i]);
}
for (size_t i = 0; i < 4; i++)
{
if ((x_max - imgpoints_cir[i].x) <= 50 &&
(y_max - imgpoints_cir[i].y) <= 50)
imgpoints_cir1.push_back(imgpoints_cir[i]);
}
if (imgpoints_cir1.size() != 4)
{
std::cout << "imgpoints_cir1.size() must = 4" << std::endl; // 如果投影点不是四个,则报错
return;
}
_imgs_pts_cir2D_true.push_back(imgpoints_cir1); // 将四个圆心点保存到_imgs_pts_cir2D_true中
Eigen::Vector3d r0, r1, r2, t;
//得到的旋转量以及平移量
r0 << rot.at<double>(0, 0), rot.at<double>(1, 0), rot.at<double>(2, 0);
r1 << rot.at<double>(0, 1), rot.at<double>(1, 1), rot.at<double>(2, 1);
r2 << rot.at<double>(0, 2), rot.at<double>(1, 2), rot.at<double>(2, 2);
t << tvec.at<double>(0, 0), tvec.at<double>(0, 1), tvec.at<double>(0, 2);
Eigen::MatrixXd RT(3, 4);
RT.block<3, 1>(0, 0) = r0;
RT.block<3, 1>(0, 1) = r1;
RT.block<3, 1>(0, 2) = r2;
RT.block<3, 1>(0, 3) = t;
// 将RT外参+矩阵保存到vec_extrinsics中
vec_extrinsics.push_back(RT);
}
对于图像使用opencv函数对标定板进行检测,对于点云,先建立一个标定板的mask,通过 RANSAC拟合mask的大致的标定板区域。然后再运用网格搜索算法精确拟合圆孔四个点。
…详情请参照古月居