最小二乘法實作二維多傳感器标定

最近用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