前言 前段时间说到小编手里有两个传感器，6个激光雷达与摄像头，还有一个微波摄像头，分别装在一个路口的不同地方，需要将这些传感器进行融合输出目标列表。但是要做到融合首先第一步就是标定，做空间同步，第二是时间同步。 接下来我以激光雷达与摄像头为例，其中激光雷达首先需要做目标检测，多目标追踪输出的为目标结构化数据（此知识算法不在此处延申，大家可查查激光雷达处理的相关算法文章） 群号交流：193369905 1、联合标定原理 在用OpenCV中的函数，通过4对对应的点的坐标计算两个图像之间单应矩阵H，然后调用射影变换函数，将一幅图像变换到另一幅图像的视角中。当时只是知道通过单应矩阵，能够将图像1中的像素坐标（u1,v1）)变换到图像2中对应的位置上(u2,v2)，而没有深究其中的变换关系。 单应(Homography)是射影几何中的概念，又称为射影变换。它把一个射影平面上的点(三维齐次矢量)映射到另一个射影平面上，并且把直线映射为直线，具有保线性质。总的来说，单应是关于三维齐次矢量的一种线性变换，可以用一个3×3的非奇异矩阵H表示这是一个齐次坐标的等式，
x1=Hx2
在这里H是一个3×3的齐次矩阵，具有8个未知量。假设已经取得了两图像之间的单应，则可单应矩阵H可以将两幅图像关联起来。这有了很多实际的应用，例如图像的校正、对齐以及在SLAM中估计两个相机间的运动。此处我用作做为联合标定。 首先我们将是雷达平面假设为一个二维水平面，忽略其坡度。 即可得 雷达平面上点p1(x1,y1),与图像上点p2(x2,y2)是一对匹配得点对，其单应性矩阵为H，则有
接下来我们求解这个矩阵 在真实的应用场景中，我们计算的点对中都会包含噪声。比如点的位置偏差几个像素，甚至出现特征点对误匹配的现象，如果只使用4个点对来计算单应矩阵，那会出现很大的误差。因此，为了使得计算更精确，一般都会使用远大于4个点对来计算单应矩阵。另外上述方程组采用直接线性解法通常很难得到最优解，所以实际使用中一般会用其他优化方法，如奇异值分解、Levenberg-Marquarat（LM）算法等进行求解。 本工程里使用了四种方法，分别为直接解方程组，最小二乘法，ransac方法，基于PROSAC的鲁棒算法，具体解法可自行百度。 2、标定效果 在这里由于是路口，不同于自动驾驶车，我们可以在感兴趣融合区域联合标定十个点，同时得到两个传感器得目标结果值 如下所示：
接下来解出点对的单应性矩阵即可，在这里我写了一个类，调用类方法即可得，其中h为雷达坐标到图像坐标变换值，h_inv为图像到3d世界雷达坐标。
3、标定效果验证 最后我们把标定结果，根据雷达目标计算出图像坐标如图所示
我们可以看到目标点基本重合。 最后我放出部分相关核心代码：
#include "CamCal.h"
#include<string>
#include<stdio.h>
C2dPtSel o2dPtSel;
void on_mouse(int event, int x, int y, int flags, void*) // mouse event
{
if (!o2dPtSel.chkImgLd())
{
std::cout << "Error: on_mouse(): frame image is unloaded" << std::endl;
return;
}
if (event == CV_EVENT_FLAG_LBUTTON)
o2dPtSel.addNd(x, y);
return;
}
CCamCal::CCamCal(void)
{
// list of 3D points for PnP
std::vector<cv::Point2f>().swap(m_vo3dPt);
// list of 2D points for PnP
std::vector<cv::Point2f>().swap(m_vo2dPt);
}
CCamCal::~CCamCal(void)
{
// list of 3D points for PnP
std::vector<cv::Point2f>().swap(m_vo3dPt);
// list of 2D points for PnP
std::vector<cv::Point2f>().swap(m_vo2dPt);
}
void CCamCal::initialize(CCfg oCfg, cv::Mat oImgFrm)
{
// configuration parameters
m_oCfg = oCfg;
// frame image
m_oImgFrm = oImgFrm.clone();
// list of 3D points for PnP
m_vo3dPt = m_oCfg.getCal3dPtLs();
// list of 2D points for PnP
if (!m_oCfg.getCalSel2dPtFlg())
m_vo2dPt = m_oCfg.getCal2dPtLs();
// homography matrix
m_oHomoMat = cv::Mat(3, 3, CV_64F);
// reprojection error
m_fReprojErr = DBL_MAX;
}
void CCamCal::process(void)
{
// select 2D points if they are not provided in the configuration file
if (m_oCfg.getCalSel2dPtFlg())
{
std::vector<cv::Point2f>().swap(m_vo2dPt);
o2dPtSel.initialize(m_oCfg, m_oImgFrm);
std::vector<cv::Point> vo2dPt = o2dPtSel.process();
std::cout << "Selected 2D points on the frame image: " << std::endl;
for (int i = 0; i < vo2dPt.size(); i++)
{
m_vo2dPt.push_back(vo2dPt[i]);
if ((vo2dPt.size() - 1) > i)
std::cout << "[ " << vo2dPt[i].x << ", " << vo2dPt[i].y << " ]," << std::endl;
else
std::cout << "[ " << vo2dPt[i].x << ", " << vo2dPt[i].y << " ]" << std::endl;
}
}
m_fprojErr=calcprojErr(m_oHomoMat, m_oCfg.getCalTyp(), m_oCfg.getCalRansacReprojThld());
// compute homography matrix
if (-1 == m_oCfg.getCalTyp())
// run all calibration types
runAllCalTyp();
else
{
m_oHomoMat = cv::findHomography(m_vo3dPt, m_vo2dPt, m_oCfg.getCalTyp(), m_oCfg.getCalRansacReprojThld());
m_fReprojErr = calcReprojErr(m_oHomoMat, m_oCfg.getCalTyp(), m_oCfg.getCalRansacReprojThld());
}
std::cout << std::endl;
}
void CCamCal::output(void)
{
// output text file of homography matrix
outTxt();
// plot a display grid on the ground plane
pltDispGrd();
}
void CCamCal::runAllCalTyp(void)
{
cv::Mat oHomoMat;
double fReprojErr;
// a regular method using all the points
try
{
oHomoMat = cv::findHomography(m_vo3dPt, m_vo2dPt, 0, 0);
fReprojErr = calcReprojErr(oHomoMat, 0, 0);
if (fReprojErr < m_fReprojErr)
{
m_fReprojErr = fReprojErr;
m_oHomoMat = oHomoMat;
}
}
catch(cv::Exception& e)
{
const char* pcErrMsg = e.what();
std::cout << "Exception caught: " << pcErrMsg << std::endl;
}
// Least-Median robust method
try
{
oHomoMat = cv::findHomography(m_vo3dPt, m_vo2dPt, 4, 0);
fReprojErr = calcReprojErr(oHomoMat, 4, 0);
if (fReprojErr < m_fReprojErr)
{
m_fReprojErr = fReprojErr;
m_oHomoMat = oHomoMat;
}
}
catch(cv::Exception& e)
{
const char* pcErrMsg = e.what();
std::cout << "Exception caught: " << pcErrMsg << std::endl;
}
// RANSAC-based robust method
for (double t = 100; t >= 10; t -= 5)
{
try
{
oHomoMat = cv::findHomography(m_vo3dPt, m_vo2dPt, 8, t);
fReprojErr = calcReprojErr(oHomoMat, 8, t);
if (fReprojErr < m_fReprojErr)
{
m_fReprojErr = fReprojErr;
m_oHomoMat = oHomoMat;
}
}
catch(cv::Exception& e)
{
const char* pcErrMsg = e.what();
std::cout << "Exception caught: " << pcErrMsg << std::endl;
}
}
}
double CCamCal::calcReprojErr(cv::Mat oHomoMat, int nCalTyp, double fCalRansacReprojThld)
{
double fReprojErr = 0;
for (int i = 0; i < m_vo3dPt.size(); i++)
{
cv::Mat o3dPtMat(3, 1, CV_64F);
cv::Mat o2dPtMat(3, 1, CV_64F);
cv::Point2f o2dPt;
o3dPtMat.at<double>(0, 0) = m_vo3dPt[i].x;
o3dPtMat.at<double>(1, 0) = m_vo3dPt[i].y;
o3dPtMat.at<double>(2, 0) = 1;
o2dPtMat = oHomoMat * o3dPtMat;
o2dPt = cv::Point2f((o2dPtMat.at<double>(0, 0) / o2dPtMat.at<double>(2, 0)), (o2dPtMat.at<double>(1, 0) / o2dPtMat.at<double>(2, 0)));
fReprojErr += cv::norm(m_vo2dPt[i] - o2dPt);
}
fReprojErr /= m_vo3dPt.size();
if (8 == nCalTyp)
std::cout << "Average reprojection error of method #" << nCalTyp << " (threshold: " << fCalRansacReprojThld << "): " << fReprojErr << std::endl;
else
std::cout << "Average reprojection error of method #" << nCalTyp << ": " << fReprojErr << std::endl;
return fReprojErr;
}
double CCamCal::calcprojErr(cv::Mat oHomoMat, int nCalTyp, double fCalRansacReprojThld)
{
double fReprojErr = 0;
double s=0;
cv::Mat oHomoMat_inv= cv::findHomography(m_vo2dPt, m_vo3dPt, 0, 0);
for (int i = 0; i < m_vo3dPt.size(); i++)
{
cv::Mat o3dPtMat(3, 1, CV_64F);
cv::Mat o2dPtMat(3, 1, CV_64F);
cv::Point2f o3dPt;
o2dPtMat.at<double>(0, 0) = m_vo2dPt[i].x;
o2dPtMat.at<double>(1, 0) = m_vo2dPt[i].y;
o2dPtMat.at<double>(2, 0) = 1;
o3dPtMat = oHomoMat_inv * o2dPtMat;
o3dPt = cv::Point2f((o3dPtMat.at<double>(0, 0) / o3dPtMat.at<double>(2, 0)), (o3dPtMat.at<double>(1, 0) / o3dPtMat.at<double>(2, 0)));
std::cout<<"point2d["<< m_vo2dPt[i].x<<" "<<m_vo2dPt[i].y<<"]"<<" world cordinate is: "<<o3dPt<<std::endl;
fReprojErr += cv::norm(m_vo3dPt[i] - o3dPt);
}
fReprojErr /= m_vo3dPt.size();
if (8 == nCalTyp)
std::cout << "Average reprojection error of method #" << nCalTyp << " (threshold: " << fCalRansacReprojThld << "): " << fReprojErr << std::endl;
else
std::cout << "Average reprojection error of method #" << nCalTyp << ": " << fReprojErr << std::endl;
return fReprojErr;
}
void CCamCal::outTxt(void)
{
FILE* pfHomoMat = fopen(m_oCfg.getOutCamMatPth(), "w");
fprintf(pfHomoMat, "Homography matrix: %.15lf %.15lf %.15lf;%.15lf %.15lf %.15lf;%.15lf %.15lf %.15lf\n",
m_oHomoMat.at<double>(0, 0), m_oHomoMat.at<double>(0, 1), m_oHomoMat.at<double>(0, 2),
m_oHomoMat.at<double>(1, 0), m_oHomoMat.at<double>(1, 1), m_oHomoMat.at<double>(1, 2),
m_oHomoMat.at<double>(2, 0), m_oHomoMat.at<double>(2, 1), m_oHomoMat.at<double>(2, 2));
printf("Homography matrix: %.15lf %.15lf %.15lf;%.15lf %.15lf %.15lf;%.15lf %.15lf %.15lf\n",
m_oHomoMat.at<double>(0, 0), m_oHomoMat.at<double>(0, 1), m_oHomoMat.at<double>(0, 2),
m_oHomoMat.at<double>(1, 0), m_oHomoMat.at<double>(1, 1), m_oHomoMat.at<double>(1, 2),
m_oHomoMat.at<double>(2, 0), m_oHomoMat.at<double>(2, 1), m_oHomoMat.at<double>(2, 2));
if (m_oCfg.getCalDistFlg())
{
cv::Mat oCalIntMat = m_oCfg.getCalIntMat();
fprintf(pfHomoMat, "Intrinsic parameter matrix: %.15lf %.15lf %.15lf;%.15lf %.15lf %.15lf;%.15lf %.15lf %.15lf\n",
oCalIntMat.at<double>(0, 0), oCalIntMat.at<double>(0, 1), oCalIntMat.at<double>(0, 2),
oCalIntMat.at<double>(1, 0), oCalIntMat.at<double>(1, 1), oCalIntMat.at<double>(1, 2),
oCalIntMat.at<double>(2, 0), oCalIntMat.at<double>(2, 1), oCalIntMat.at<double>(2, 2));
printf("Intrinsic parameter matrix: %.15lf %.15lf %.15lf;%.15lf %.15lf %.15lf;%.15lf %.15lf %.15lf\n",
oCalIntMat.at<double>(0, 0), oCalIntMat.at<double>(0, 1), oCalIntMat.at<double>(0, 2),
oCalIntMat.at<double>(1, 0), oCalIntMat.at<double>(1, 1), oCalIntMat.at<double>(1, 2),
oCalIntMat.at<double>(2, 0), oCalIntMat.at<double>(2, 1), oCalIntMat.at<double>(2, 2));
cv::Mat oCalDistCoeffMat = m_oCfg.getCalDistCoeffMat();
fprintf(pfHomoMat, "Distortion coefficients: %.15lf %.15lf %.15lf %.15lf\n",
oCalDistCoeffMat.at<double>(0), oCalDistCoeffMat.at<double>(1),
oCalDistCoeffMat.at<double>(2), oCalDistCoeffMat.at<double>(3));
printf("Distortion coefficients: %.15lf %.15lf %.15lf %.15lf\n",
oCalDistCoeffMat.at<double>(0), oCalDistCoeffMat.at<double>(1),
oCalDistCoeffMat.at<double>(2), oCalDistCoeffMat.at<double>(3));
}
fprintf(pfHomoMat, "Reprojection error: %.15lf\n", m_fReprojErr);
printf("Reprojection error: %.15lf\n", m_fReprojErr);
fclose(pfHomoMat);
}
void CCamCal::pltDispGrd(void)
{
cv::Mat oImgPlt = m_oImgFrm.clone();
cv::Size oDispGrdDim = m_oCfg.getCalDispGrdDim();
// find the limits of the 3D grid on the ground plane
double fXMin = DBL_MAX;
double fYMin = DBL_MAX;
double fXMax = -DBL_MAX;
double fYMax = -DBL_MAX;
for (int i = 0; i < m_vo3dPt.size(); i++)
{
if (fXMin > m_vo3dPt[i].x)
fXMin = m_vo3dPt[i].x;
if (fYMin > m_vo3dPt[i].y)
fYMin = m_vo3dPt[i].y;
if (fXMax < m_vo3dPt[i].x)
fXMax = m_vo3dPt[i].x;
if (fYMax < m_vo3dPt[i].y)
fYMax = m_vo3dPt[i].y;
}
// compute the endpoints for the 3D grid on the ground plane
std::vector<cv::Point2f> vo3dGrdPtTop, vo3dGrdPtBtm, vo3dGrdPtLft, vo3dGrdPtRgt;
for (int x = 0; x < oDispGrdDim.width; x++)
{
vo3dGrdPtTop.push_back(cv::Point2f((fXMin + (x * ((fXMax - fXMin) / (oDispGrdDim.width - 1)))), fYMin));
vo3dGrdPtBtm.push_back(cv::Point2f((fXMin + (x * ((fXMax - fXMin) / (oDispGrdDim.width - 1)))), fYMax));
}
for (int y = 0; y < oDispGrdDim.height; y++)
{
vo3dGrdPtLft.push_back(cv::Point2f(fXMin, (fYMin + (y * ((fYMax - fYMin) / (oDispGrdDim.height - 1))))));
vo3dGrdPtRgt.push_back(cv::Point2f(fXMax, (fYMin + (y * ((fYMax - fYMin) / (oDispGrdDim.height - 1))))));
}
// compute the endpoints for the projected 2D grid
std::vector<cv::Point2f> vo2dGrdPtTop, vo2dGrdPtBtm, vo2dGrdPtLft, vo2dGrdPtRgt;
for (int i = 0; i < oDispGrdDim.width; i++)
{
cv::Mat o3dPtMat(3, 1, CV_64F);
cv::Mat o2dPtMat(3, 1, CV_64F);
o3dPtMat.at<double>(0, 0) = vo3dGrdPtTop[i].x;
o3dPtMat.at<double>(1, 0) = vo3dGrdPtTop[i].y;
o3dPtMat.at<double>(2, 0) = 1;
o2dPtMat = m_oHomoMat * o3dPtMat;
vo2dGrdPtTop.push_back(cv::Point2f((o2dPtMat.at<double>(0, 0) / o2dPtMat.at<double>(2, 0)), (o2dPtMat.at<double>(1, 0) / o2dPtMat.at<double>(2, 0))));
o3dPtMat.at<double>(0, 0) = vo3dGrdPtBtm[i].x;
o3dPtMat.at<double>(1, 0) = vo3dGrdPtBtm[i].y;
o3dPtMat.at<double>(2, 0) = 1;
o2dPtMat = m_oHomoMat * o3dPtMat;
vo2dGrdPtBtm.push_back(cv::Point2f((o2dPtMat.at<double>(0, 0) / o2dPtMat.at<double>(2, 0)), (o2dPtMat.at<double>(1, 0) / o2dPtMat.at<double>(2, 0))));
}
for (int i = 0; i < oDispGrdDim.height; i++)
{
cv::Mat o3dPtMat(3, 1, CV_64F);
cv::Mat o2dPtMat(3, 1, CV_64F);
o3dPtMat.at<double>(0, 0) = vo3dGrdPtLft[i].x;
o3dPtMat.at<double>(1, 0) = vo3dGrdPtLft[i].y;
o3dPtMat.at<double>(2, 0) = 1;
o2dPtMat = m_oHomoMat * o3dPtMat;
vo2dGrdPtLft.push_back(cv::Point2f((o2dPtMat.at<double>(0, 0) / o2dPtMat.at<double>(2, 0)), (o2dPtMat.at<double>(1, 0) / o2dPtMat.at<double>(2, 0))));
o3dPtMat.at<double>(0, 0) = vo3dGrdPtRgt[i].x;
o3dPtMat.at<double>(1, 0) = vo3dGrdPtRgt[i].y;
o3dPtMat.at<double>(2, 0) = 1;
o2dPtMat = m_oHomoMat * o3dPtMat;
vo2dGrdPtRgt.push_back(cv::Point2f((o2dPtMat.at<double>(0, 0) / o2dPtMat.at<double>(2, 0)), (o2dPtMat.at<double>(1, 0) / o2dPtMat.at<double>(2, 0))));
}
// draw grid lines on the frame image
for (int i = 0; i < oDispGrdDim.width; i++)
cv::line(oImgPlt, vo2dGrdPtTop[i], vo2dGrdPtBtm[i], cv::Scalar(int(255.0 * ((double)i / (double)oDispGrdDim.width)), 127, 127), 2, CV_AA);
for (int i = 0; i < oDispGrdDim.width; i++)
cv::line(oImgPlt, vo2dGrdPtLft[i], vo2dGrdPtRgt[i], cv::Scalar(127, 127, int(255.0 * ((double)i / (double)oDispGrdDim.width))), 2, CV_AA);
// plot the 2D points
for (int i = 0; i < m_vo2dPt.size(); i++)
{
char acPtIdx[32];
cv::circle(oImgPlt, m_vo2dPt[i], 6, cv::Scalar(255, 0, 0), 1, CV_AA); // draw the circle
sprintf(acPtIdx, "%d", i);
cv::putText(oImgPlt, acPtIdx, m_vo2dPt[i], cv::FONT_HERSHEY_SIMPLEX, 1, cv::Scalar(255,0, 255), 2);
}
// plot the projected 2D points
// plot the 2D points
for (int i = 0; i < m_vo2dPt.size(); i++)
{
cv::Mat o3dPtMat(3, 1, CV_64F);
cv::Mat o2dPtMat(3, 1, CV_64F);
o3dPtMat.at<double>(0, 0) = m_vo3dPt[i].x;
o3dPtMat.at<double>(1, 0) = m_vo3dPt[i].y;
o3dPtMat.at<double>(2, 0) = 1;
o2dPtMat = m_oHomoMat * o3dPtMat;
cv::circle(oImgPlt, cv::Point2f((o2dPtMat.at<double>(0, 0) / o2dPtMat.at<double>(2, 0)), (o2dPtMat.at<double>(1, 0) / o2dPtMat.at<double>(2, 0))),
12, cv::Scalar(0, 0, 255), 1, CV_AA); // draw the circle
}
// display plotted image
cv::namedWindow("3D grid on the ground plane", CV_WINDOW_NORMAL);
cv::imshow("3D grid on the ground plane", oImgPlt);
cv::waitKey(0);
cv::destroyAllWindows();
// save plotted image
if (m_oCfg.getOutCalDispFlg())
cv::imwrite(m_oCfg.getOutCalDispPth(), oImgPlt);
}
C2dPtSel::C2dPtSel(void)
{
}
C2dPtSel::~C2dPtSel(void)
{
}
void C2dPtSel::initialize(CCfg oCfg, cv::Mat oImgFrm)
{
// configuration parameters
m_oCfg = oCfg;
// frame image for plotting results
m_oImgFrm = oImgFrm.clone();
}
std::vector<cv::Point> C2dPtSel::process(void)
{
std::vector<cv::Point> voVanPt;
if (m_oCfg.getCalSel2dPtFlg())
{
std::cout << "Hot keys: \n"
<< "\tESC - exit\n"
<< "\tr - re-select a set of 2D points\n"
<< "\to - finish selecting a set of 2D points\n"
<< std::endl;
cv::Mat oImgFrm = m_oImgFrm.clone();
cv::namedWindow("selector of 2D points", CV_WINDOW_NORMAL);
cv::imshow("selector of 2D points", m_oImgFrm);
cv::setMouseCallback("selector of 2D points", on_mouse); // register for mouse event
while (1)
{
int nKey = cv::waitKey(0); // read keyboard event
if (nKey == 27)
break;
if (nKey == 'r') // reset the nodes
{
std::vector<cv::Point>().swap(m_voNd);
m_oImgFrm = oImgFrm.clone();
cv::imshow("selector of 2D points", m_oImgFrm);
cv::waitKey(0);
}
if (nKey == 'o') // finish selection of pairs of test points
{
cv::destroyWindow("selector of 2D points");
std::vector<cv::Point2f> vo3dPt = m_oCfg.getCal3dPtLs();
if (vo3dPt.size() == m_voNd.size())
return m_voNd;
else
{
std::cout << "Error: Mis-match between the number of selected 2D points and the number of 3D points." << std::endl;
break;
}
}
}
}
}
void C2dPtSel::addNd(int nX, int nY)
{
char acNdIdx[32];
cv::Point oCurrNd;
oCurrNd.x = nX;
oCurrNd.y = nY;
m_voNd.push_back(oCurrNd);
// std::cout << "current node(" << oCurrNd.x << "," << oCurrNd.y << ")" << std::endl; // for debug
cv::circle(m_oImgFrm, oCurrNd, 6, cv::Scalar(255, 0, 0), 1, CV_AA); // draw the circle
sprintf(acNdIdx, "%d", (int)(m_voNd.size() - 1));
cv::putText(m_oImgFrm, acNdIdx, oCurrNd, cv::FONT_HERSHEY_SIMPLEX, 1, cv::Scalar(0, 0, 255), 2);
cv::imshow("selector of 2D points", m_oImgFrm);
}
最后总结 好多朋友问我怎么将多个激光雷达与多个摄像头融合，在这里我简单解答下，摄像头与激光雷达首先各自均要做内部融合，例如6个激光雷达首先要进行内部标定拼接，这里我放张图吧。
标红处为激光雷达安装位置，拼接完输出结构化数据再与摄像头进行融合。 后续接下来会讲如何进行目标级别融合。。。。。。有兴趣童鞋可加群193369905交流，也可提交相关简历，加入我们或者实习。。。
开心洋葱 , 版权所有丨如未注明 , 均为原创丨未经授权请勿修改 , 转载请注明摄像头与激光雷达微波雷达的融合算法之二标定！
