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

这个星期重新做了一下激光雷达和相机的联合标定仿真实验，写篇文章记录一下。

1. 原理与模型

主要原理学习了这篇论文：摄像机-激光雷达静态外参联合标定《Extrinsic calibration of a camera and laser range finder (improves camera calibration)》。

我们模拟的是二维激光雷达和相机的系统，也就是说激光雷达测得的云点图为空间中某一条线上所有的点在激光雷达参考系下的坐标值，如下图：

相机拍到的图像和雷达测得的点大概就是这样。

相机的标定基本上与摄像机内参标定《A Flexible New Technique for Camera Calibration》这篇文论介绍的相同，在之前的文章里也介绍过：相机的标定之手机相机的标定。

设标定板在 $Z=0$Z=0平面上，其上的某点世界参考系下坐标为 $P_w$Pw​，则相机参考系下坐标为 $P_c=R\cdot P+t$Pc​=R⋅P+t，其中 $R,t$R,t 分别为旋转矩阵和平移向量，代表相机在世界参考系下的位姿。

该点在雷达参考系下的坐标为 $P_l=\Phi\cdot P_c+\Delta$Pl​=Φ⋅Pc​+Δ，这里的 $\Phi,\Delta$Φ,Δ 就是我们要标定的量，$P_l$Pl​ 则相当于我们测得的云点图里的点。

在世界参考系下与标定板垂直的向量写成： $N_w=[0\ \ 0\ \ 1]^T-[0\ \ 0\ \ 0]^T$Nw​=[0  0  1]T−[0  0  0]T故在相机坐标系下，垂直向量为： $N_c=(R\cdot[0\ \ 0\ \ 1]^T+t)-(R\cdot[0\ \ 0\ \ 0]^T+t)=R\cdot[0\ \ 0\ \ 1]^T=R_3$Nc​=(R⋅[0  0  1]T+t)−(R⋅[0  0  0]T+t)=R⋅[0  0  1]T=R3​我们令 $R=[R_1\ \ R_2\ \ R_3]$R=[R1​  R2​  R3​]。

我们可知 $||R_3||=1$∣∣R3​∣∣=1，令垂直向量的模长为相机中心到标定板平面的距离，则有 $N_c=R3\cdot(R_3^T\cdot t)$Nc​=R3⋅(R3T​⋅t)。

于是，我们推得下关系式：
$|N_c\cdot(\Phi^{-1}(P_l-\Delta))|=||N_c||^2$∣Nc​⋅(Φ−1(Pl​−Δ))∣=∣∣Nc​∣∣2

于是我们需要求解以下最小二乘问题：
$\min\sum_i\sum_j(\frac{|N_{c,i}\cdot(\Phi^{-1}(P_{l,i,j}-\Delta))|}{||N_{c,i}||}-||N_{c,j}||)^2$mini∑​j∑​(∣∣Nc,i​∣∣∣Nc,i​⋅(Φ−1(Pl,i,j​−Δ))∣​−∣∣Nc,j​∣∣)2

其中：$i$i 代表位姿 $id$id，$j$j 代表某个位姿下激光雷达测得点的 $id$id。

2. MATLAB 生成模拟数据

2.1. MATLAB需注意的地方

注意：MATLAB的旋转向量 $\theta n$θn 好像是指绕 $n$n 顺时针旋转 $\theta$θ 角 ???? ???? ???? 。

见下图MATLAB旋转向量到旋转矩阵的函数：

由 逆时针旋转 设定推得的Rodrigues' rotation formula 为：
$R=cos(\theta)\ I + (1-cos(\theta))\ nn^T+sin(\theta)\ n^{\Lambda}$R=cos(θ) I+(1−cos(θ)) nnT+sin(θ) nΛ

Rodrigues' rotation formula推导。

代入上面 $r=\frac{\pi}{2}[0\ 0\ 1]^T$r=2π​[0 0 1]T 得 $R=\begin{bmatrix}0&-1&0\\1& 0&0\\0&0&1\end{bmatrix}$R=⎣⎡​010​−100​001​⎦⎤​，可见，MATLAB里的旋转矩阵代表的旋转方向为顺时针。

这里我研究了好久，最后才发现是MATLAB里面定义不太一样。。

2.2. 模型参数

相机内参矩阵：$\begin{bmatrix}750&0&320\\0&750&240\\0&0&1\end{bmatrix}$⎣⎡​75000​07500​3202401​⎦⎤​

激光雷达外参：$\Phi=[-0.25\ \ -0.02\ \ -0.01]^T，\Delta = [0\ \ -1.0\ \ 0.1]$Φ=[−0.25  −0.02  −0.01]T，Δ=[0  −1.0  0.1] (m)

令相机测得的角点位置存在一个高斯分布的误差，平均值为 $0$0，标准差为 $0.5$0.5 (pixel) 。激光雷达测得的点云存在一个均匀分布的误差，分布范围为 $\pm 0.05$±0.05 (m) 。

初始时，相机的旋转量为零，平移矩阵为 $[-0.3\ \ -0.2\ \ 2]^T$[−0.3  −0.2  2]T 。

标定板大小为 $11\times11$11×11个方格，即有 $10\times10$10×10 个角点，每个格子大小为 $0.076$0.076 (m) 。初始时标定板处于 $Z=0$Z=0 平面上，在实验过程中绕某一条轴旋转标定板。

模拟的相机拍摄图像如下 (红线为激光雷达的扫描线)：

激光雷达的云点图即是这些图上红线上所对应的点在激光雷达参考系下的坐标。

3. 代码部分

代码有点长，思路就是用ceres求解之前那个最小二乘问题。

// common_include.h 里面就是一些常用的库比如 ceres、Eigen 这些的。
#include "common/common_include.h"
#include "common/config.h"
#include <string>
#include <boost/format.hpp>
#include <fstream>
#include <cmath>

#include "tools/rotation.h"
#include "tools/projection.h"
// 第一个functor
struct COST_FUNCTION {
	COST_FUNCTION(Eigen::Vector3d N, Eigen::Vector3d Pf): _N(N), _Pf(Pf) {}
	template <typename T>
	bool operator() (const T* const laser, T* residual) const {
		T prediction[3];
		T point[3];
		point[0] = T(_Pf(0,0));
		point[1] = T(_Pf(1,0));
		point[2] = T(_Pf(2,0));
		// 一个把激光雷达参考系转化到相机参考系的模板函数
		// template<class T>
		// Laser2Camera(const T* laser, const T* point, T* prediction)
		// 这里要注意MATLAB里面的旋转方向为顺时针
		Laser2Camera(laser, point, prediction);
		
		T N[3];
		N[0] = T(_N[0]);
		N[1] = T(_N[1]);
		N[2] = T(_N[2]);
		T norm = ceres::sqrt(N[0]*N[0] + N[1]*N[1] + N[2]*N[2]);
		T x[3];
		x[0] = T(_N(0,0));
		x[1] = T(_N(1,0));
		x[2] = T(_N(2,0));
		
		T a = x[0]*prediction[0] + x[1]*prediction[1] + x[2]*prediction[2];
        T zero = T(0);
		T b = a > zero ? a : -a;
		
		residual[0] = b/norm - norm
		return true;
	}
	
	const Eigen::Vector3d _N;
	const Eigen::Vector3d _Pf;
};
// 把 N 一起优化的functor
struct COST_FUNCTION2 {
	COST_FUNCTION2(Eigen::Vector3d Pf):_Pf(Pf) {}
	
	template <typename T>
	bool operator() (const T* const laser, const T* const N, T* residual) const {
		T prediction[3];
		T point[3];
		point[0] = T(_Pf(0,0));
		point[1] = T(_Pf(1,0));
		point[2] = T(_Pf(2,0));
		Laser2Camera(laser, point, prediction);
		
		T norm = ceres::sqrt(N[0]*N[0] + N[1]*N[1] + N[2]*N[2]);
		
		T a = N[0]*prediction[0] + N[1]*prediction[1] + N[2]*prediction[2];
        T zero = T(0);
		T b = a > zero ? a : -a;
		
		residual[0] = b/norm - norm;	
		return true;
	}
	const Eigen::Vector3d _Pf;
};

int main() {
	boost::format fmt( "%s%d.out" );
	// 自己定义的一个读取一些参数的类，不是必要的
	Config::setParameterFile("../config/default.yaml");
	string data_path = Config::get<string> ("DataPath2");
	ifstream fin;
	
	// 储存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 (int i=1; i<11; i++)
		for (int j=1; j<11; j++)
			objects.push_back( Point3f(j*0.076, i*0.076, 0.0) );
			
	// 读取各个图像的角点和云点图
	for (int i=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 {
			double data[2] = {0};
			for (int i=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 {
			double data[3] = {0};
			for (int i=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();
	}
	// A为相机内参，D为畸变系数，R为每张图片的旋转向量，t为每张图片的平移向量
	Mat A;
	Mat D;
	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 << "The intrinsic matrix is: \n" << A <<endl;
	cout << "---------------------------------------------------------\n";
	cout << "The distortion vector is: \n" << D << endl;
	cout << "---------------------------------------------------------\n";
	
	// 每一张图对应的 N
	vector<Eigen::Vector3d> Ns;
	for (int i=0; i<R.size(); i++) {
		Eigen::Vector3d rvectorn(R[i].at<double>(0,0), R[i].at<double>(1,0), R[i].at<double>(2,0));
		Eigen::AngleAxisd rvector(rvectorn.norm(), rvectorn/rvectorn.norm());
		Eigen::Matrix3d rmatrix = rvector.toRotationMatrix();
		double R3t1 = (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));
		double R3t = ceres::sqrt(R3t1*R3t1);
		Eigen::Vector3d R3(rmatrix(0,2), rmatrix(1,2), rmatrix(2,2));
		Eigen::Vector3d N = -R3*R3t1;
		Ns.push_back(N);
	}
	for (int i=0; i<11; i++) {
	    cout << Ns[i].transpose();
	    cout << endl;
	} 
	
	double laser[6] = {0};
	double laser_true[6] = {-0.25, -0.02, -0.01, 0, -1, 0.1};

	// 往问题中添加误差项
	ceres::Problem problem;
	for (int i=0; i<R.size(); i++) {
		for (int j=0; j<allClouds[i].size(); j++) {
			ceres::CostFunction* costfunction = new ceres::AutoDiffCostFunction<COST_FUNCTION, 1, 6> ( new COST_FUNCTION(Ns[i], allClouds[i][j]) );
			problem.AddResidualBlock(costfunction, new ceres::CauchyLoss(0.5), laser);
		}
	}
	
	// 开始优化
	cout << "Begin to optimize...\n";
	ceres::Solver::Options options;
	options.linear_solver_type = ceres::DENSE_QR;
	options.minimizer_progress_to_stdout = true;
	ceres::Solver::Summary summary;
	ceres::Solve ( options, &problem, &summary );
	cout << "Optimization finish !\n";

	cout << "---------------------------------------------------------\n";
	cout << "The Rotation Vector is: " << "[ " << laser[0] << ", " << laser[1] << ", " << laser[2] << " ]\n";
	cout << "The Translation Vector is: " << "[ " << 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::AngleAxisd rvector_new(r_vect.norm(), -r_vect/r_vect.norm());
	Eigen::Matrix3d R_n = rvector_new.toRotationMatrix();
	double error = 0;
	int index = 0;
	for (int i=0; i<Ns.size(); i++) {
	    for (int j=0; j<allClouds[i].size(); j++) {
	        double norm = Ns[i].norm();
	        double a = (Ns[i].transpose() * (R_n * (allClouds[i][j] - t_n)));
	        a /= norm;
	        a = (a>0) ? a : -a;
	        double e = a -norm;
	        error += e*e;
	        index++;
	    }
	}
	error /= index;
	cout << "The average error is: " << error << ".\n";
	cout << "---------------------------------------------------------\n";

	double N[3*Ns.size()];
	for (int i=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::Problem problem2;
	for (int i=0; i<R.size(); i++) {
		for (int j=0; j<allClouds[i].size(); j++) {
			ceres::CostFunction* costfunction = new ceres::AutoDiffCostFunction<COST_FUNCTION2, 1, 6, 3> ( new COST_FUNCTION2(allClouds[i][j]) );
			problem2.AddResidualBlock(costfunction, new ceres::CauchyLoss(0.5), laser, N+3*i);
		}
	}
	
	cout << "Begin to optimize...\n";
	ceres::Solver::Options options2;
	options2.linear_solver_type = ceres::DENSE_QR;
	options2.minimizer_progress_to_stdout = false;
	options2.gradient_tolerance = 1e-20;
	ceres::Solver::Summary summary2;
	ceres::Solve ( options2, &problem2, &summary2 );
	cout << "Optimization finish !\n";
	
	cout << "---------------------------------------------------------\n";
	cout << "updating Camera pose...\n";
	Ns.clear();
	for (int i=0; i<Ns.size(); i++) {
		Eigen::Vector3d N_new(N[3*i+0], N[3*i+1], N[3*i+2]);
		Ns.push_back(N_new);
	}
	cout << "---------------------------------------------------------\n";
	cout << "The Rotation Vector is: " << "[ " << laser[0] << ", " << laser[1] << ", " << laser[2] << " ]\n";
	cout << "The Translation Vector is: " << "[ " << laser[3] << ", " << laser[4] << ", " << laser[5] << " ]\n";
	cout << "---------------------------------------------------------\n";
		
	Eigen::Map<Eigen::Vector3d> r_vect_new(laser);
	Eigen::Map<Eigen::Vector3d> t_n_new(laser + 3);
	Eigen::AngleAxisd rvector_new_new(r_vect_new.norm(), -r_vect_new/r_vect_new.norm());
	Eigen::Matrix3d R_n_new = rvector_new_new.toRotationMatrix();
	double error_new = 0;
	int index_new = 0;
	for (int i=0; i<Ns.size(); i++) {
	    for (int j=0; j<allClouds[i].size(); j++) {
	        double norm = Ns[i].norm();
	        double a = (Ns[i].transpose() * (R_n * (allClouds[i][j] - t_n)));
	        a /= norm;
	        a = (a>0) ? a : -a;
	        double e = a -norm;
	        error_new += e*e;
	        index_new++;
	    }
	}
	error /= index;
	cout << "The average error is: " << error << ".\n";
	cout << "---------------------------------------------------------\n";
	
	return 0;
}

4. 结果

代码运行结果如下：
第一次优化是直接把相机标定求得的 $N$N 拿来用，得到此时的 $\Phi, \Delta$Φ,Δ ，平均误差为0.13左右；第二次优化是把 $N$N 放进最小二乘问题的那个方程里面一起优化，优化后的平均误差变成了0.00012，极大的减小了平均误差值。