准备工作
需要一个标定板,尺寸可以自己定,但是需要雷达和相机都能捕捉到的,当然,雷达捕捉到的数据越多越好。然后把标定板悬空放置,如下图所示。
为什么要这么做呢?其实就是方便后序在激光雷达捕获的点云中,分离出这个标定板。好了,现在调整好雷达和相机,转到合适位置,在相同位置上进行拍照和点云捕捉。捕捉完成后,需要找到至少三个对应点对,也就是二维点和三维点对,然后利用这三个点对进行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&