激光雷达和相机联合标定

 

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

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