最小二乘法实现二维多传感器标定

最近用C++实现了多传感器的标定，实现方法很简单，但是很实用，特来分享一波。

二维多传感器标定问题

简单说明现在需要解决的问题。如图每个坐标系都代表一个传感器的位姿，十字星代表同一时刻两个传感器感知到的环境中的固定点（特征点）。现在已知两个传感器坐标系中个特征点的对应坐标值（带有噪声），需要求得两个传感器间的位姿变换参数（也就是这里的旋转矩阵R和平移矩阵T）。

数学表达如下
已知传感器a观测到的特征点(1,2,3…N)坐标(x,y)为：
(Pa1x,Pa1y),(Pa2x,Pa2y),…,(PaNx,PaNx)
已知传感器b观测到的特征点(1,2,3…N)坐标(w,z)为：
(Pb1w,Pb1z),(Pb2w,Pb2z),…,(PbNw,PbNz)
求xy坐标系变换到wz坐标系的平移参数与旋转参数。
坐标系变换可以表示为：

A∗Pa=Pb

A=⎡⎣⎢cos(θ)sin(θ)0−sin(θ)cos(θ)0xtyt1⎤⎦⎥

其中θ是旋转角度，xt,yt是平移。

利用Eigen库求解最小二乘解

为了在C++里实现带有矩阵运算的最小二乘法，我使用了Eigen库。Eigen是开源库，可以在主页下载：
（将Eigen文件夹放在源文件夹下即可）
http://eigen.tuxfamily.org/index.php?title=Main_Page

Eigen有给出很多最小二乘的解决方案，例如下面就有一个利用SVD做最小二乘的方法：
http://eigen.tuxfamily.org/dox/classEigen_1_1JacobiSVD.html

补全代码，可以得到一个可运行的例子：

#include <iostream>#include "Eigen/SVD"using namespace std;using namespace Eigen;int main(){MatrixXf m = MatrixXf::Random(3,2);cout << "Here is the matrix m:" << endl << m << endl;JacobiSVD<MatrixXf> svd(m, ComputeThinU | ComputeThinV);cout << "Its singular values are:" << endl << svd.singularValues() << endl;cout << "Its left singular vectors are the columns of the thin U matrix:" << endl << svd.matrixU() << endl;cout << "Its right singular vectors are the columns of the thin V matrix:" << endl << svd.matrixV() << endl;Vector3f rhs(1, 0, 0);cout << "Now consider this rhs vector:" << endl << rhs << endl;cout << "A least-squares solution of m*x = rhs is:" << endl << svd.solve(rhs) << endl;return 0;}

简单介绍上面代码。这个例子实现的是：

已知Mx=rhs,已知3* 2的矩阵M，以及3* 1的向量rhs。代码通过最小二乘法求得了最优的x。

步骤大致如下：

1. 先利用MatrixXf m = MatrixXf::Random(3,2);生成了3*2的随机数矩阵m；

2. 对m做SVD分解。这个JacobiSVD默认只会计算singular values。如果需要求得U或者V矩阵,需要另外设定。这里我们要求解最小二乘，只需要求解 thin U 和 thin V，所以用到的函数是：
   JacobiSVD<MatrixXf> svd(m, ComputeThinU | ComputeThinV);

3. 用svd.solve(rhs)求解x。

解决二维多传感器标定问题

前面的例程解出了Mx=rhs中的x。但是回看第一部分，我们需要求解的是：

A∗Pa=Pb

⎡⎣⎢cos(θ)sin(θ)0−sin(θ)cos(θ)0xtyt1⎤⎦⎥⎡⎣⎢PaxPay1⎤⎦⎥=⎡⎣⎢PbwPbz1⎤⎦⎥

其中已知的是右边两个矩阵。所以我们需要先重构矩阵为：

⎡⎣⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢Pa1xPa1yPa2xPa2y...PaNxPaNy−Pa1yPa1x−Pa2yPa2x−PaNyPaNx101010010101⎤⎦⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥∗⎡⎣⎢⎢⎢⎢cos(θ)sin(θ)xtyt⎤⎦⎥⎥⎥⎥=⎡⎣⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢Pb1xPb1yPb2xPb2y...PbNxPbNy⎤⎦⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥

现在就变成了Mx=rhs求解x的形式，可以简单改写例程即可求解：

#include <iostream>#include "Eigen/SVD"using namespace std;using namespace Eigen;int main(){MatrixXf m(6,4);m << 1.97,-2.03,1.1,0, 2,2.21,0,1, 3,-3,1,0, 3,3.1,0,1, 4,-3,1,0, 3,4,0,1;//m(0,0)= 2;m(0,1)=-2;m(0,2)=1;m(0,3)=0;//m(1,0)= 2;m(1,1)=2;m(1,2)=0;m(1,3)=1;//m(2,0)= 3;m(2,1)=-3;m(2,2)=1;m(2,3)=0;//m(3,0)= 3;m(3,1)=-3;m(3,2)=0;m(3,3)=1;//m(4,0)= 4;m(4,1)=-3;m(4,2)=1;m(4,3)=0;//m(5,0)= 3;m(5,1)=4;m(5,2)=0;m(5,3)=1;cout << "Here is the matrix m:" << endl << m << endl;JacobiSVD<MatrixXf> svd(m, ComputeThinU | ComputeThinV);cout << "Its singular values are:" << endl << svd.singularValues() << endl;cout << "Its left singular vectors are the columns of the thin U matrix:" << endl << svd.matrixU() << endl;cout << "Its right singular vectors are the columns of the thin V matrix:" << endl << svd.matrixV() << endl;VectorXf rhs(6);rhs << 2, 2, 3.02,3,4.11,3;cout << "Now consider this rhs vector:" << endl << rhs << endl;cout << "A least-squares solution of m*x = rhs is:" << endl << svd.solve(rhs) << endl;return 0;}

上面代码输入了带有噪声的三对点进行测试，运行结果与理论值近似。
在实际运用中，如果有P对点，需要将MatrixXf m(6,4);修改为MatrixXf m(P,4);。VectorXf rhs(6);修改为VectorXf rhs(P);。
输入矩阵有两种形式，一种是一次性输入，m << 1.97, … , …… ;
另一种是单个输入： m(0,0)= 2;m(0,1)=-2;m(0,2)=1;m(0,3)=0; …
根据输入需求进行修改即可。

参考

数组重构
https://math.stackexchange.com/questions/77462/finding-transformation-matrix-between-two-2d-coordinate-frames-pixel-plane-to

Eigen下载
http://eigen.tuxfamily.org/index.php?title=Main_Page

Eigen—JacobiSVD详解
http://eigen.tuxfamily.org/dox/classEigen_1_1JacobiSVD.html