激光雷达和相机联合标定
实现联合标定的框架主要有Autoware、Apollo、lidar_camera_calibration、but_velodyne，具体请参考：激光雷达和相机的联合标定。虽然这些框架已经帮我们实现了，但是框架对使用的雷达和相机的品牌有要求。
准备工作
需要一个标定板，尺寸可以自己定，但是需要雷达和相机都能捕捉到的，当然，雷达捕捉到的数据越多越好。然后把标定板悬空放置，如下图所示。
为什么要这么做呢？其实就是方便后序在激光雷达捕获的点云中，分离出这个标定板。好了，现在调整好雷达和相机，转到合适位置，在相同位置上进行拍照和点云捕捉。捕捉完成后，需要找到至少三个对应点对，也就是二维点和三维点对，然后利用这三个点对进行PNP求解，计算出相机坐标系和雷达坐标系之间的变换关系。
这个就是变换公式了。
找寻对应点
下面是捕获的图像和点云：
注意：雷达的视角要更广一些，比相机捕捉到更多信息。点云数据中有一个空间中的平面，那个便是标定板。我们在图像中提取标定板中的三个顶点，这样只需要在点云数据中把这三个顶点坐标找寻出来即可，那么如何在这么多的点云数据中搜索对应的三个点呢？
在这里我们借助了处理点云的第三方工具PolyWorks，当然你也可以找寻其它的工具，PolyWorks可以手动去删除多余的点云，并保存需要的点云。在众多点云中分离出标定板点云数据。分离点云之后，PolyWorks可以基于分离出的标定板点云数据，拟合出一个长方形（就是标定板），并且得出这个长方形各个顶点的坐标。除了这种方式，PCL点云库还提供了一系列的函数来拟合直线和平面，Matlab也可以，这两种方式都可以达到想要的效果，但是着实没有这个方便。
这样，二维图像上的三个顶点位置可以确定，三维点云数据中的三个顶点坐标可以确定，接下来就是PNP求解了~
PNP求解
由上面的公式可知，在计算雷达和相机外参的时候，还需要相机的内参，关于相机的内参标定，网上教程非常多，实现方式也不止一种，ROS、Opencv、Matlab都可以~
当对应点和相机内参都得到之后，就可以使用Opencv的solvePnp来求解相机坐标系和雷达坐标系之间的变换关系~
solvePnP(outDim, inDim, cameraMatrix, distCoeff, rvec, tvec);
其中：outDim为对应三维点坐标，inDim为对应二维点坐标，rvec和tvec为输出的变换矩阵。
具体代码如下所示：
#include<iostream>
#include<opencv2/opencv.hpp>
#include<vector>
#include<iostream>
#include <string>
#include <vector>
#include <fstream> //文件流库函数
#include <stdio.h>
#include <stdlib.h>
using namespace std;
using namespace cv;
void SplitString(const string& s, vector<string>& v, const string& c)
{
string::size_type pos1, pos2;
pos2 = s.find(c);
pos1 = 0;
while (string::npos != pos2)
{
v.push_back(s.substr(pos1, pos2 - pos1));
pos1 = pos2 + c.size();
pos2 = s.find(c, pos1);
}
if (pos1 != s.length())
v.push_back(s.substr(pos1));
}
int main()
{
Mat image = imread("C:/Users/18301/Desktop/biaoding.jpg");
//对应的二维点和三维点对
float threeDim[3][3] = { { -699, 3349, 1060 }, { -190, 3560, 565 }, { -1190, 3152, 477 }};
float twoDim[3][2] = { { 771, 385 }, { 1367, 942 }, { 149, 1003 } };
vector<Point3f>outDim;
vector<Point2f>inDim;
vector<float> distCoeff;
distCoeff.push_back(-0.0565);
distCoeff.push_back(0.0643);
distCoeff.push_back(0);
distCoeff.push_back(0);
distCoeff.push_back(0);
for (int i = 0; i < 3; i++)
{
outDim.push_back(Point3f(threeDim[i][0], threeDim[i][1], threeDim[i][2]));
inDim.push_back(Point2f(twoDim[i][0], twoDim[i][1]));
}
Mat cameraMatrix(3, 3, CV_32F);
//float tempMatrix[3][3] = { { 3522.3, 0, 0 }, { 0, 3538.4, 0 }, { 1968.9,1375.4,1.0 } };
float tempMatrix[3][3] = { { 3522.3, 0, 1968.9 }, { 0, 3538.4, 1375.4 }, { 0, 0, 1.0 } };//相机的内参矩阵
for (int i = 0; i < 3; i++)
{
for (int j = 0; j < 3; j++)
{
cameraMatrix.at<float>(i, j) = tempMatrix[i][j];
}
}
Mat rvec, tvec;
solvePnP(outDim, inDim, cameraMatrix, distCoeff, rvec, tvec);
Rodrigues(rvec, rvec);
//读取txt文件
ifstream infile;
infile.open("C:/Users/18301/Desktop/point.txt"); //将文件流对象与文件连接起来
assert(infile.is_open()); //若失败,则输出错误消息,并终止程序运行
string s;
vector<string> v;
vector<double> tmp;
vector<vector<double>> all_data;
// all_data.resize(all_data.max_size() + 1);
while (getline(infile, s))
{
//cout << s << endl;
SplitString(s, v, ","); //可按多个字符来分隔;
for (int i = 0; i < 3; i++)
{
tmp.push_back(atof(v[i].c_str())*1000);
}
all_data.push_back(tmp);
v.clear();
tmp.clear();
}
infile.close(); //关闭文件输入流
/*for (int i = 0; i < all_data.size(); i++)
{
cout << all_data[i][0] << " " << all_data[i][1] << " " << all_data[i][2] << endl;
}*/
cout << all_data.size() << endl;
vector<Point3f> all_points;
for (int i = 0; i < all_data.size(); i++)
{
all_points.push_back(Point3f(all_data[i][0], all_data[i][1], all_data[i][2]));
}
std::vector<cv::Point2f> projectedPoints;
cv::projectPoints(all_points, rvec, tvec, cameraMatrix, distCoeff, projectedPoints);
for (int i = 0; i < projectedPoints.size(); i++)
{
Point2f p = projectedPoints[i];
if (p.y<=1080)
{
circle(image, p, 5, Scalar(255, 255, 0), 1, 8, 0);
}
}
imwrite("C:/Users/18301/Desktop/result.jpg", image);
cout << rvec << endl;
cout << tvec << endl;
return 0;
}
注意：这里的点云是以txt格式保存的，C++如何读取并分割txt文件的数据方法也在代码中，求出外参后，可以通过反映射投影来把三维点映射到二维图像中，opencv中通过cv::projectPoints()函数来实现，映射后的结果如下所示：
这里只是把标定板的三维点云进行了映射，把整个场景点云进行映射的图如下所示：
为什么点云看起来好像是断掉的呢？是不是标定结果错误呢？其实不是，标定结果很正常，第一幅结果图上面显示的是标定板的三维点映射到二维图像，第二幅图像上比第一幅图像上多余的点其实是标定板后面的墙壁上的点云数据，因为标定板挡住其后面的一部分面积，所以墙壁上有很大一部分是空缺的，而且雷达和相机的视角不完全相同，映射到二维图像上会有一部分数据是空缺的，另外一部分数据映射后会和标定板数据产生重合，但是其真实的深度不相同，详细请看前面的捕获的点云数据图。
缺点与不足
由于提取的点比较少，只有三个，精度上可能不是太高，后面可以通过把标定板中间挖出一个正方形，提取内正方形的顶点来增加对应点对。而且我们相机的焦距很大，标定板不能太近，雷达只有16线，过于稀疏，提取的点云也不是太稠密，标定板只有400多个点。
激光雷达-相机外参自动化标定算法及项目实战
通常而言，激光雷达和相机固定安装于无人车上，其相对关系一定，获得准确的齐次变换矩阵或者 (x,y,z,roll,pitch,yaw) 变化量即完成了传感器的外参标定。然而，无人车安装的传感器数量较多，高效且自动化地完成多传感器标定对于自动驾驶研发而言非常关键，本文介绍一种自动化标定相机-激光雷达的方法，并且给出了相对完整的ROS实现以供读者参考。
本文基于论文：Automatic extrinsic calibration between a camera and a 3D Lidar using 3D point and plane correspondences中提到的方法和开源的代码改进了一个鲁棒性相对较强的自动标定方法，并且使用ROS实现。
准备工作
1.标定前你需要有你的相机和Lidar的Ros驱动，能够采集得到type为sensor_msgs/Image 和sensor_msgs/PointCloud2 的 rosmsg；
2.准备一块标定板，木板或者纸板都行，注意为平板，板子的尺寸不限制，但是为了使lidar的反射线束尽可能多，建议做大一些，作为参考，我所采用的是一块110cm×90cm的木板；
3.打印一张和板子尺寸相当的棋盘格，可以在该网站下载棋盘格文件，作为参考，我采用的是8×6的棋盘格；
4.将你的无人车（机器人）摆在一个相对空旷一点的地方，采集Camera和lidar的数据，采集的方法可以参考文末的rosbag，相机的标定bag可以由远及近、各个角度，LiDAR-相机联合标定的bag建议将标定板45度斜立，由近及远9个位置；
也可以直接使用文末提供的rosbag进行实验,下载链接见文末。
数据采集
本方法可以在线标定，但是为了读者能够相对简单地复现，在文末提供了rosbag供大家离线实现，要离线标定需要录制两个rosbag，一个是纯相机采集的棋盘格数据，主要用作相机内参的标定，采集过程：测试者由远到近手持标定板，分别在俯仰角和侧向转动标定板，注意
相机内参标定
相机内参可以使用ROS下的package camera_calibration 完成，指令如下：
rosrun camera_calibration cameracalibrator.py --size 8x6 --square 0.108 image:=/usb_cam0/image_raw
其中–size 后指定标定板中棋盘格内的Corner数量，注意Corner指棋盘格内的黑白对角数，而不是指格子数，座椅一个 9×79×7 格子的棋盘Corner数为8×68×6，通过–square 参数设定每个格子的边长，单位为米，image指定输入的图像的topic，可以在车上做在线的标定，也可以录好rosbag进行离线标定。为了供读者复现，我们采用rosbag离线的方式实验。
相应的bag下载链接见文末rosbag下载链接，该bag主要用于实践使用，如果读者觉得下载麻烦，可以选择跳过相机内参标定这一步，博主已经将本例中准确的相机内参配置于联合标定项目中，跳过此步骤不影响后面的相机-激光雷达联合标定实践。
运行rosbag,camera_calibration 弹出窗口并开始标定，如下图所示：
当sample数足够多的时候，CALIBRATE button 会变亮，点击CALIBRATE完成标定，点击SAVE,标定结果将默认保存至/tmp/calibrationdata.tar.gz,解压得到相机的畸变系数、分辨率、相机矩阵等信息：
[image]
width
1920
height
1080
[narrow_stereo]
camera matrix
1980.419103 0.000000 916.316859
0.000000 1972.701183 596.930682
0.000000 0.000000 1.000000
distortion
-0.546687 0.337506 0.002990 0.006806 0.000000
rectification
1.000000 0.000000 0.000000
0.000000 1.000000 0.000000
0.000000 0.000000 1.000000
projection
1705.022949 0.000000 919.183345 0.000000
0.000000 1884.496948 603.655841 0.000000
0.000000 0.000000 1.000000 0.000000
算法介绍
我采用了该论文中的标定算法，并在其基础上做出了改进以提升算法的鲁棒性。该方法主要分为特征提取和目标函数优化两步。
特征提取
由于棋盘格能够使用OpenCV进行准确的检测，所以camera-lidar标定的特征提取的难点实际是在Lidar的特征提取上，三维点云数据稀疏，对于小的棋盘格实际上很难像图像那样使用Corner进行特征点定位，为了简化问题并加速标定流程，厂商通常会采用一个标定车间或者是空地进行多传感器标定。我们选择了一块相对空旷的场地进行数据采集，划定一个相对确定的矩形区域（lidar）坐标系缩小lidar进行标定板检测的ROI，这样可以极大降低误检测的概率；本项目中，我们在 rqt_reconfigure 中动态调整点云数据的ROI，界面如下：
使用rviz -d rviz/cam_lidar_calibration.rviz 查看ROI，记得把你的frame显示为你激光雷达的frame，如下图所示：
截取ROI以后，对目标的标定板的分割就相对简单了，获取点云中最高的点zmax，已知标定板的长和宽计算对角线长度 diagonal，得到最低点 zmin=zmax−diagonal,以zmax和 zmin作为上下限截取整个点云，基本能够滤除地面点；
此时点云只剩标定板和举标定板的人了，对RANSAC其进行平面拟合,代码如下：
pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients());
pcl::PointIndices::Ptr inliers(new pcl::PointIndices());
int i = 0, nr_points = static_cast<int>(cloud_filtered->points.size());
pcl::SACSegmentation<pcl::PointXYZIR> seg;
seg.setOptimizeCoefficients(true);
seg.setModelType(pcl::SACMODEL_PLANE);
seg.setMethodType(pcl::SAC_RANSAC);
seg.setMaxIterations(1000);
seg.setDistanceThreshold(plane_dist_threshold_);
pcl::ExtractIndices<pcl::PointXYZIR> extract;
seg.setInputCloud(cloud_filtered);
seg.segment(*inliers, *coefficients);
// 标定板法向量的大小
float mag = sqrt(pow(coefficients->values[0], 2) + pow(coefficients->values[1], 2)
+ pow(coefficients->values[2], 2));
我们采用SACMODEL_PLANE模型，这里的距离阈值plane_dist_threshold_可以在launch文件中指定，分割完的点的index赋给了inliersinliers。因为激光雷达存在measurement误差，平面拟合分割出来的点的实际坐标并不严格在一个平面上，这为后面的直线拟合、相交点计算造成了麻烦，因为我们使用pcl内置的方法将分割出来的点投射到拟合得到的平面上（该平面由系数coefficients确定）：
pcl::PointCloud<pcl::PointXYZIR>::Ptr cloud_seg(new pcl::PointCloud<pcl::PointXYZIR>);
extract.setInputCloud(cloud_filtered);
extract.setIndices(inliers);
extract.setNegative(false);
extract.filter(*cloud_seg);
// Project the inliers on the fit plane
pcl::PointCloud<pcl::PointXYZIR>::Ptr cloud_projected(new pcl::PointCloud<pcl::PointXYZIR>);
pcl::ProjectInliers<pcl::PointXYZIR> proj;
proj.setModelType(pcl::SACMODEL_PLANE);
proj.setInputCloud(cloud_seg);
proj.setModelCoefficients(coefficients);
proj.filter(*cloud_projected);
结果如下：
图中白色的点即为我们拟合并分割出来的标定板的反射点。寻找到每一个ring的起点和终点，一般来说激光雷达是正常的平放的话可以简单地选取x或者y的最大最小值的点作为每个线条的端点，我们通过比较每个ring的两端点距离大小确定一个middle ring，以middle ring为分界可以得到四条边（左上，右上、左下和右下）的点，如下图所示：
根据各个边的端点我们可以用RANSAC做直线拟合，获得标定板的四条边，最后使用pcl::lineWithLineIntersection 方法过得四个角的坐标值 oilk,k=1,2,3,4，计算左右两个角的中心点作为标定板的中心点坐标 oil，前面我们使用RANSAC做了平面拟合，得到了标定板的法向量 mag，所以可以得到平面的朝向向量 nil，图中所示蓝色箭头。
以上的符号中，i均表示第i个样本，标定过程中需要采集多组样本用于优化目标函数。
至此我们能够自动地提取出三维点云中标定板的四角 oilk、中心点 oil和朝向向量 nil。接下来是图像的棋盘格特征点提取：
cv::Mat gray;
std::vector<cv::Point2f> corners, corners_undistorted;
std::vector<cv::Point3f> grid3dpoint;
cv::cvtColor(cv_ptr->image, gray, CV_BGR2GRAY);
ROS_INFO_STREAM("cols: " << gray.cols << " rows: " << gray.rows);
// Find checkerboard pattern in the image
bool patternfound = cv::findChessboardCorners(gray, patternNum, corners,CALIB_CB_ADAPTIVE_THRESH + CALIB_CB_NORMALIZE_IMAGE);
cornerSubPix(gray, corners, Size(11, 11), Size(-1, -1),TermCriteria(CV_TERMCRIT_EPS + CV_TERMCRIT_ITER, 30, 0.1));
将图像转为灰度图，使用cv::findChessboardCorners找到内部对角点，然后使用cornerSubPix() 进行亚像素优化，得到每个角点在图像坐标系下的坐标（像素位置）corners，同时，由于我们知道棋盘格每个格子的实际尺寸（边长），我们可以计算出每个角点在3维坐标系下相较于棋盘中心点的坐标grid3dpoint：
double tx, ty; // Translation values
// Location of board frame origin from the bottom left inner corner of the checkerboard
tx = (patternNum.height - 1) * patternSize.height / 2;
ty = (patternNum.width - 1) * patternSize.width / 2;
// Board corners w.r.t board frame
for (int i = 0; i < patternNum.height; i++) {
for (int j = 0; j < patternNum.width; j++) {
cv::Point3f tmpgrid3dpoint;
// Translating origin from bottom left corner to the centre of the checkerboard
tmpgrid3dpoint.x = i * patternSize.height - tx;
tmpgrid3dpoint.y = j * patternSize.width - ty;
tmpgrid3dpoint.z = 0;
grid3dpoint.push_back(tmpgrid3dpoint);
}
}
其中patternNum为8×68×6的棋盘，本实例中的标定板的格子宽高为
patternSize.height=patternSize.width=108cm
此外根据标定板的宽和高可以算出标定板四角相较于棋盘中心点的坐标，根据角点的像素坐标corners和三维坐标grid3dpoint，可以根据透视n点算法（Perspective-n-Point (PnP) algorithm）得到两个坐标系的变换矩阵，在OpenCV中，可以通过函数**cv::solvePnP()**来调用该算法：
cv::solvePnP(grid3dpoint, corners, i_params.cameramat, i_params.distcoeff, rvec, tvec);
其中i_params.cameramat为相机矩阵，i_params.distcoeff为畸变系数，都是通过相机的标定得到的内参。rvec和tvec分别为求得的变换的旋转向量和平移向量。我们将这两个向量用一个齐次变换矩阵来表示:
// chessboardpose is a 3*4 transform matrix that transforms points in board frame to camera frame | R&T
cv::Mat chessboardpose = cv::Mat::eye(4, 4, CV_64F);
cv::Mat tmprmat = cv::Mat(3, 3, CV_64F); // rotation matrix
cv::Rodrigues(rvec, tmprmat); // Euler angles to rotation matrix
for (int j = 0; j < 3; j++) {
for (int k = 0; k < 3; k++) {
chessboardpose.at<double>(j, k) = tmprmat.at<double>(j, k);
}
chessboardpose.at<double>(j, 3) = tvec.at<double>(j);
}
从而得到相机中的标定板的朝向向量（标定板平面的法向量）nic:
chessboard_normal.at<double>(0) = 0;
chessboard_normal.at<double>(1) = 0;
chessboard_normal.at<double>(2) = 1;
chessboard_normal = chessboard_normal * chessboardpose(cv::Rect(0, 0, 3, 3)).t();
同时还可以根据该其次变换矩阵换算出标定板的中心点和四个角点在相机坐标系下的三维坐标 oicoci 和 oick,k=1,2,3,4，我们将相机三维坐标系下的角点和中心点反投射到图像坐标系，得到如下图像：
总结一下，通过自动特征提取算法，我们分别得到了标定板在激光雷达坐标系下的中心点坐标、四个角点坐标和朝向向量，以及在相机坐标系下的中心点坐标、四个角点坐标和朝向向量：
采集若干组特征，在本例中，我们采集8-9组特征，i=9；构造目标函数优化，最终得到传感器外参；
优化目标函数
在该问题中优化的目标主要为两个向量:
camera-lidar外参标定工具编译使用
将cam_lidar_calibration包克隆到你的ros workspace中，推荐使用catkin build编译。打开配置文件配置cam_lidar_calibration/cfg/initial_params.txt相应参数：
/usb_cam0/image_raw
/velodyne/left_front
0
32
8 6
108
1100 900
0 0
1978.259195 0.000000 918.829246
0.000000 1969.753118 595.575540
0.000000 0.000000 1.000000
5
-0.543649 0.322171 0.003254 0.006478 0.000000
1920 1080
配置文件含义：
第1, 2行分别指定Image和PointCloud的topic名称
第3行为0表示普通相机，为1表示鱼眼相机
第4行为激光雷达线数，本文的例子中我们采用Velodyne VLP-32c激光雷达，故线数为32
第5行指定棋盘格内对角数
第6行指定每个格子的实际边长（单位为mm）
第7行指定标定板的宽和高（单位为mm）
第8行指定棋盘格到标定板两边的offset（单位为mm）
第9-11行指定相机矩阵（通过相机内参标定获得，本例的参数已给出）
第12行指定相机的畸变系数个数，如果为普通相机，通常为5个畸变系数，鱼眼相机则为4个;
第13行指定相机的畸变系数，由相机内参标定获得
第14行指定相机宽和高的像素数量
启动标定项目的launch文件：
roslaunch cam_lidar_calibration cam_lidar_calibration.launch
下载文末提供的包含点云和图像的rosbag，由于rosbag较大，我们做了压缩，并且只保留了VelodyneScan数据，首先通过文末链接下载压缩文件camera_lidar.tar.gz，解压缩：
tar -zxvf camera_lidar.tar.gz
克隆博主修改后的velodyne驱动至你的ros workspace内，编译并运行：
cd 你的ros workspace/src/
git clone https://github.com/AbangLZU/velodyne-Apollo
cd .. && catkin build
source devel/setup.bash
roslaunch velodyne_pointcloud VLP-32C_points.launch
接着播放解压后的rosbag：
rosbag play camera_lidar.bag
运行rviz并且指定使用本项目中的rviz配置文件:
rviz -d rviz/cam_lidar_calibration.rviz
在 cam_lidar_calibration.launch 的命令行界面中输入以下指令完成数据的采集：
输入i采集一个样本，输入i以后建议暂停rosbag以观察特征提取效果;
如图，我们可以在Rviz中看到特征检测的效果，确定这一样本可用的话，在命令号中按enter，并继续播放rosbag
采集样本时，确保棋盘格在图像和点云的画面内。否则特征提取失败，采集8-9个合适的样本（注意每次只有按下enter键样本才会被用于后续的目标函数优化），并可以开始优化，在命令行中按下o，程序将进入自动的优化流程；
最终优化得到的平移量和旋转欧拉角会被输出到命令行中，如下所示：
finish Analytical rotation matrix [-1.036126022403965, -0.1638794527757443, 0.008845283766657015;
-0.04286490916356046, 0.1569642781584801, -0.9734963378954911;
-0.05223954254647389, -0.9267763676301324, -0.007819553145415839]
Analytical Euler angles -1.57923 0.0563054 -3.10025
Rotation and Translation after first optimization -1.58894 0.0406462 -3.09517 -0.925397 0.829087 0.41448
Extrinsic Parameters -1.58567 0.00236683 -3.09016 -0.958068 0.876605 0.412671
The problem is optimized in 17.5892 seconds.
那么最终的外参为旋转量（roll,pitch,yaw，单位为弧度）:
-1.58567 0.00236683 -3.09016
平移量（xyz，单位为米）：
-0.958068 0.876605 0.412671
将这些参数填入到project_img.launch文件中：
<node pkg="cam_lidar_calibration" type="projector" name="projector" output="screen">
<param name="roll" value="-1.58567"/>
<param name="pitch" value="0.00236683"/>
<param name="yaw" value="-3.09016"/>
<param name="x" value="-0.958068"/>
<param name="y" value="0.876605"/>
<param name="z" value="0.412671"/>
</node>
运行bag并且运行该launch:
roslaunch cam_lidar_calibration project_img.launch
最后我们就能够在rviz中查看到点云投射到图像的结果（即标定的效果），同时能能够通过rostopic echo /tf以查看camera 到lidar的tf，如下所示：
transforms:
-
header:
seq: 0
stamp:
secs: 1587721262
nsecs: 562099592
frame_id: "left_front"
child_frame_id: "camera"
transform:
translation:
x: -0.958068013191
y: 0.87660497427
z: 0.412670999765
rotation:
x: -0.0174866063579
y: 0.712131134959
z: -0.701574418226
w: 0.0188891744494
项目代码与bag下载链接
源代码: https://github.com/AbangLZU/cam_lidar_calibration
实例rosbag下载链接：https://pan.baidu.com/s/1bcBFOK-WjAe-73Z-UJMOKg 提取码: k88p
参考：
https://blog.csdn.net/AdamShan/article/details/105736726
上一篇： 如何把本地项目上传到Github