激光雷达和相机的联合标定的实现（源代码，干货满满）

介绍

最近要做激光雷达和相机的联合标定，我们使用相机进行二维图像上的object detection，激光雷达可以辅助测距，从而帮助我们判断物体相对于相机坐标系的位置。实现联合标定的框架主要有Autoware、Apollo、lidar_camera_calibration、but_velodyne，具体请参考：激光雷达和相机的联合标定。虽然这些框架已经帮我们实现了，但是框架对使用的雷达和相机的品牌有要求，可惜我们买的是不知名的。。。没事，那就自己做吧。

相机和雷达

我们使用的相机是大恒的相机，雷达是镭神的16线激光雷达（没错，是16线的）。设计了一个云台，可以把相机和雷达安装到车上。

准备工作

需要一个标定板，尺寸可以自己定，但是需要雷达和相机都能捕捉到的，当然，雷达捕捉到的数据越多越好。然后把标定板悬空放置，如下图所示。

为什么要这么做呢？其实就是方便后序在激光雷达捕获的点云中，分离出这个标定板。好了，现在调整好雷达和相机，转到合适位置，在相同位置上进行拍照和点云捕捉。捕捉完成后，需要找到至少三个对应点对，也就是二维点和三维点对，然后利用这三个点对进行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
#include
#include
#include
#include 
#include 
#include   //文件流库函数
#include 
#include 

using namespace std;
using namespace cv;

void SplitString(const string& s, vector& 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 } };

	vectoroutDim;
	vectorinDim;
	vector 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(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 v;
	vector tmp;
	vector> 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 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 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多个点。

感悟

刚开始打算使用autoware来进行相机和雷达的标定，搞了几天终于把环境装好了，但是发现相机和雷达传感器中没有对应的品牌，问雷达那边，人家有写好的插件，但是不提供给你用，需要收费，没办法，只能自己写了~