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

最近用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(,);
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(, , );
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 ;
}
           

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

已知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(,);
m << ,-,,, ,,,, ,-,,, ,,,, ,-,,, ,,,;
//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();
rhs << , , ,,,;
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 ;
}
           

上面代码输入了带有噪声的三对点进行测试，运行结果与理论值近似。

在实际运用中，如果有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