激光雷达和相机联合标定仿真实验2.0

激光雷达和相机联合标定仿真实验2.0激光雷达和相机联合标定仿真实验2.0这个星期重新做了⼀下激光雷达和相机的联合标定仿真实验，写篇⽂章记录⼀下。1.原理与模型1.1.物理原理主要原理学习了这篇论⽂：。我们模拟的是⼆维激光雷达和相机的系统，也就是说激光雷达测得的云点图为空间中某⼀条线上所有的点在激光雷达参考系下的坐标值，如下图：相机拍到的图像和雷达测得的点⼤概就是这样。相机的标定基本上与这篇⽂论介绍的相同，在之前的⽂章⾥也介绍过：。设标定板在Z=0平⾯上，其上的某点世界参考系下坐标为Pw，则相机参考系下坐标为P=R⋅P+t，其中R,t分别为旋转矩阵和平c移向量，代表相机在世界参考系下的位姿。该点在雷达参考系下的坐标为Pl=Φ⋅P+Δ，这⾥的Φ,Δ就是我们要标定的量，Pl则相当于我们测得的云点图⾥的点。c在世界参考系下与标定板垂直的向量写成：N=[001]T−[000]Tw故在相机坐标系下，垂直向量为：N=(R⋅[001]T+tc)−(R⋅[000]T+t)=R⋅[001]T=R3我们令R=[R1R2R3]。我们可知∣∣R3∣∣=1，令垂直向量的模长为相机中⼼到标定板平⾯的距离，则有N=R3⋅(R3T⋅t)。c于是，我们推得下关系式：∣N⋅(Φ−1(P−Δ))∣=∣∣N∣∣2clc于是我们需要求解以下最⼩⼆乘问题：∑∑1∣Nc,i⋅(Φ−1(Pl,i,j−Δ))∣∣∣N∣∣c,imin2(−∣∣N∣∣)2c,iji其中：i代表位姿id，j代表某个位姿下激光雷达测得点的id。1.2.数学原理原理部分参考了《Methodsfornon-linearleastsquaresproblems》。我们利⽤ceres求解这个最⼩⼆乘问题，本质是⽤L-M算法求解，原理如下：在这个⾮线性最⼩⼆乘问题中，要求解的量为：TTTN=[Nc,1Nc,2...Nc,n]T,Φ,Δc令：∣Nc,i⋅(Φ−1(Pl,i,j−Δ))∣∣∣Nc,i∣∣fij(N,Φ,Δ)=c−∣∣Nc,i∣∣则：f(N,Φ,Δ)=f(x)=[ff...f11ff...f]T2122nmc121m然后问题就变成了⼀般的⾮线性最⼩⼆乘问题：1minF(x)=min2∣∣f(x)∣∣2将f(x+h)⼀阶泰勒展开，得：f(x+h)=f(x)+J(x)h+o(∣∣h∣∣)≈f(x)+J(x)h则有：11F(x+h)≈L(h)=2f(x)Tf(x)+hTJ(x)Tf(x)+2hTJ(x)TJ(x)hL-M算法是再在F中加⼊⼀个阻尼项：111minminh=argG(h)=arg2f(x)Tf(x)+hTJ(x)Tf(x)+2hTJ(x)TJ(x)h+2μhThhh对h求偏导后，令式⼦为零得：J(x)Tf(x)+J(x)TJ(x)h+μh=0化简后则有：(JTJ+μI)h=−JTf关于μ的选取，在开始时⼀般取：A0=J(x0)TJ(x0)max0{a}iiμ=τ⋅i其中τ为⾃⼰定的⼀个常数。之后的取值根据：F(x)−F(x+h)L(0)−L(h)ρ=算法步骤如下：begin: k:=0;ν:=2;x:=x0T A=J(x)J(x);g:=J(x)Tf(x) found=(∣∣g∣∣∞≤ϵ1);μ:=τ⋅max{aii} while(notfound)and(k<kmax)  k:=k+1;Solve(A+μI)h=−g  if∣∣h∣∣≤ϵ2(∣∣x∣∣+ϵ2)   found:=true  else   x:=x+hnew   ρ:=(F(x)−F(x))/(L(0)−L(h))new   ifρ>0    x:=xnewT    A=J(x)J(x);g:=J(x)Tf(x)    found=(∣∣g∣∣∞≤ϵ1)1    μ:=μ⋅max{,1−(2ρ−1)3};ν:=23   else    μ:=μ⋅ν;ν:=2⋅νend2.MATLAB⽣成模拟数据2.1.MATLAB需注意的地⽅注意：MATLAB的旋转向量θn好像是指绕n顺时针旋转θ见下图MATLAB旋转向量到旋转矩阵的函数：由逆时针旋转设定推得的Rodrigues'rotationformula为：R=cos(θ)I+(1−cos(θ))nnT+sin(θ)nΛ⎡0−10⎤⎣100⎦001，可见，MATLAB⾥的旋转矩阵代表的旋转⽅向为顺时针。π代⼊上⾯r=2[001]T得R=这⾥我研究了好久，最后才发现是MATLAB⾥⾯定义不太⼀样。。2.2.模型参数⎡750320⎤750240⎦0⎣0001相机内参矩阵：激光雷达外参：Φ=[−0.25−0.02−0.01]T，Δ=[0−1.00.1](m)令相机测得的⾓点位置存在⼀个⾼斯分布的误差，平均值为，标准差为0.5(pixel)。激光雷达测得的点云存在⼀个均匀分布的误差，分0布范围为±0.05(m)。初始时，相机的旋转量为零，平移矩阵为[−0.3−0.22]T。标定板⼤⼩为11×11个⽅格，即有10×10个⾓点，每个格⼦⼤⼩为0.076(m)。初始时标定板处于Z=0平⾯上，在实验过程中绕某⼀条轴旋转标定板。模拟的相机拍摄图像如下(红线为激光雷达的扫描线)：激光雷达的云点图即是这些图上红线上所对应的点在激光雷达参考系下的坐标。3.代码部分代码有点长，思路就是⽤ceres求解之前那个最⼩⼆乘问题。//common_include.h⾥⾯就是⼀些常⽤的库⽐如ceresEigen、这些的。#include"common/common_include.h"#include"common/config.h"#include"common/config.h"#include<string>#include<boost/format.hpp>#include<fstream>#include<cmath>#include"tools/rotation.h"#include"tools/projection.h"//第⼀个functorstructCOST_FUNCTION{COST_FUNCTION(Eigen::Vector3dN,Eigen::Vector3dPf):_N(N),_Pf(Pf){}template<typenameT>booloperator()(constT*constlaser,T*residual)const{Tprediction[3];Tpoint[3];point[0]=T(_Pf(0,0));point[1]=T(_Pf(1,0));point[2]=T(_Pf(2,0));//⼀个把激光雷达参考系转化到相机参考系的模板函数//template<classT>//Laser2Camera(constT*laser,constT*point,T*prediction)//这⾥要注意MATLAB⾥⾯的旋转⽅向为顺时针Laser2Camera(laser,point,prediction);TN[3];N[0]=T(_N[0]);N[1]=T(_N[1]);N[2]=T(_N[2]);Tnorm=ceres::sqrt(N[0]*N[0]+N[1]*N[1]+N[2]*N[2]);Tx[3];x[0]=T(_N(0,0));x[1]=T(_N(1,0));x[2]=T(_N(2,0));Ta=x[0]*prediction[0]+x[1]*prediction[1]+x[2]*prediction[2];Tzero=T(0);Tb=a>zero?a:-a;residual[0]=b/norm-normreturntrue;}constEigen::Vector3d_N;constEigen::Vector3d_Pf;};//把N⼀起优化的functorstructCOST_FUNCTION2{COST_FUNCTION2(Eigen::Vector3dPf):_Pf(Pf){}template<typenameT>booloperator()(constT*constlaser,constT*constN,T*residual)const{Tprediction[3];Tpoint[3];point[0]=T(_Pf(0,0));point[1]=T(_Pf(1,0));point[2]=T(_Pf(2,0));Laser2Camera(laser,point,prediction);Tnorm=ceres::sqrt(N[0]*N[0]+N[1]*N[1]+N[2]*N[2]);Ta=N[0]*prediction[0]+N[1]*prediction[1]+N[2]*prediction[2];Tzero=T(0);Tb=a>zero?a:-a;residual[0]=b/norm-norm;returntrue;returntrue;}constEigen::Vector3d_Pf;};intmain(){boost::formatfmt("%s%d.out");//⾃⼰定义的⼀个读取⼀些参数的类，不是必要的Config::setParameterFile("../config/default.yaml");stringdata_path=Config::get<string>("DataPath2");ifstreamfin;//储存3D坐标下⼀张图的⾓点vector<Point3f>objects;//储存⼀张图像的⾓点vector<Point2f>corners;//储存所有图像的⾓点vector<vector<Point2f>>imageCorners;//储存所有图的⾓点对应的空间位置vector<vector<Point3f>>objectCorners;//储存⼀张图对应的云点图vector<Eigen::Vector3d>clouds;//储存所有的云点图vector<vector<Eigen::Vector3d>>allClouds;//⽣成⼀张图⾓点对应的空间位置//这⾥要注意顺序for(inti=1;i<11;i++)for(intj=1;j<11;j++)objects.push_back(Point3f(j*0.076,i*0.076,0.0));//读取各个图像的⾓点和云点图for(inti=0;i<11;i++){//清空原先存储的点corners.clear();clouds.clear();//存储⾓点及其对应的空间坐标fin.open(data_path+(fmt%"corners"%(i)).str());if(!fin){cout<<data_path+(fmt%"corners"%(i)).str()<<"Failed!\n";continue;}else{doubledata[2]={0};for(inti=0;i<100;i++){for(auto&d:data)fin>>d;corners.push_back(Point2f(data[0],data[1]));}imageCorners.push_back(corners);objectCorners.push_back(objects);}fin.close();//存储云点图fin.open(data_path+(fmt%"laserdata"%(i)).str());if(!fin){cout<<data_path+(fmt%"laserdata"%(i)).str()<<"Failed!\n";continue;}else{doubledata[3]={0};for(inti=0;i<100;i++){for(auto&d:data)fin>>d;clouds.push_back(Eigen::Vector3d(data[0],data[1],data[2]));}allClouds.push_back(clouds);}fin.close();fin.close();}//A为相机内参，D为畸变系数，R为每张图⽚的旋转向量，为t每张图⽚的平移向量MatA;MatD;vector<Mat>R;vector<Mat>t;//标定，不考虑畸变系数calibrateCamera(objectCorners,imageCorners,Size(10,10),A,D,R,t,CALIB_ZERO_TANGENT_DIST+CALIB_FIX_K1+CALIB_FIX_K2+CALIB_FIX_K3+CALIB_FIX_K4+CALIB_FIX_K5+CALIB_FIX_K6);cout<<"---------------------------------------------------------\n";cout<<"Theintrinsicmatrixis:\n"<<A<<endl;cout<<"---------------------------------------------------------\n";cout<<"Thedistortionvectoris:\n"<<D<<endl;cout<<"---------------------------------------------------------\n";//每⼀张图对应的Nvector<Eigen::Vector3d>Ns;for(inti=0;i<R.size();i++){Eigen::Vector3drvectorn(R[i].at<double>(0,0),R[i].at<double>(1,0),R[i].at<double>(2,0));Eigen::AngleAxisdrvector(rvectorn.norm(),rvectorn/rvectorn.norm());Eigen::Matrix3drmatrix=rvector.toRotationMatrix();doubleR3t1=(rmatrix(0,2)*t[i].at<double>(0,0)+rmatrix(1,2)*t[i].at<double>(1,0)+rmatrix(2,2)*t[i].at<double>(2,0));doubleR3t=ceres::sqrt(R3t1*R3t1);Eigen::Vector3dR3(rmatrix(0,2),rmatrix(1,2),rmatrix(2,2));Eigen::Vector3dN=-R3*R3t1;Ns.push_back(N);}for(inti=0;i<11;i++){cout<<Ns[i].transpose();cout<<endl;}doublelaser[6]={0};doublelaser_true[6]={-0.25,-0.02,-0.01,0,-1,0.1};//往问题中添加误差项ceres::Problemproblem;for(inti=0;i<R.size();i++){for(intj=0;j<allClouds[i].size();j++){ceres::CostFunction*costfunction=newceres::AutoDiffCostFunction<COST_FUNCTION,1,6>(newCOST_FUNCTION(Ns[i],allClouds[i][j]));problem.AddResidualBlock(costfunction,newceres::CauchyLoss(0.5),laser);}}//开始优化cout<<"Begintooptimize...\n";ceres::Solver::Optionsoptions;options.linear_solver_type=ceres::DENSE_QR;options.minimizer_progress_to_stdout=true;ceres::Solver::Summarysummary;ceres::Solve(options,&problem,&summary);cout<<"Optimizationfinish!\n";cout<<"---------------------------------------------------------\n";cout<<"TheRotationVectoris:"<<"["<<laser[0]<<","<<laser[1]<<","<<laser[2]<<"]\n";cout<<"TheTranslationVectoris:"<<"["<<laser[3]<<","<<laser[4]<<","<<laser[5]<<"]\n";cout<<"---------------------------------------------------------\n";//计算平均误差Eigen::Map<Eigen::Vector3d>r_vect(laser);Eigen::Map<Eigen::Vector3d>t_n(laser+3);Eigen::Map<Eigen::Vector3d>t_n(laser+3);Eigen::AngleAxisdrvector_new(r_vect.norm(),-r_vect/r_vect.norm());Eigen::Matrix3dR_n=rvector_new.toRotationMatrix();doubleerror=0;intindex=0;for(inti=0;i<Ns.size();i++){for(intj=0;j<allClouds[i].size();j++){doublenorm=Ns[i].norm();doublea=(Ns[i].transpose()*(R_n*(allClouds[i][j]-t_n)));a/=norm;a=(a>0)?a:-a;doublee=a-norm;error+=e*e;index++;}}error/=index;cout<<"Theaverageerroris:"<<error<<".\n";cout<<"---------------------------------------------------------\n";doubleN[3*Ns.size()];for(inti=0;i<Ns.size();i++){N[3*i+0]=Ns[i](0,0);N[3*i+1]=Ns[i](1,0);N[3*i+2]=Ns[i](2,0);}//把N放⼊⼀起优化ceres::Problemproblem2;for(inti=0;i<R.size();i++){for(intj=0;j<allClouds[i].size();j++){ceres::CostFunction*costfunction=newceres::AutoDiffCostFunction<COST_FUNCTION2,1,6,3>(newCOST_FUNCTION2(allClouds[i][j]));problem2.AddResidualBlock(costfunction,newceres::CauchyLoss(0.5),laser,N+3*i);}}cout<<"Begintooptimize...\n";ceres::Solver::Optionsoptions2;options2.linear_solver_type=ceres::DENSE_QR;options2.minimizer_progress_to_stdout=false;options2.gradient_tolerance=1e-20;ceres::Solver::Summarysummary2;ceres::Solve(options2,&problem2,&summary2);cout<<"Optimizationfinish!\n";cout<<"---------------------------------------------------------\n";cout<<"updatingCamerapose...\n";Ns.clear();for(inti=0;i<Ns.size();i++){Eigen::Vector3dN_new(N[3*i+0],N[3*i+1],N[3*i+2]);Ns.push_back(N_new);}cout<<"-----------------------------------------------------