最近用c++實現了多感測器的標定，實現方法很簡單，但是很實用，特來分享一波。簡單說明現在需要解決的問題。如圖每個座標系都代表乙個感測器的位姿，十字星代表同一時刻兩個感測器感知到的環境中的固定點（特徵點）。現在已知兩個感測器座標系中個特徵點的對應座標值（帶有雜訊），需要求得兩個感測器間的位姿變換引數（也就是這裡的旋轉矩陣r和平移矩陣t）。
數學表達如下
已知感測器a觀測到的特徵點(1,2,3…n)座標(x,y)為： (p
a1x,
pa1y
),(p
a2x,
pa2y
),…,
(pan
x,pa
nx)
已知感測器b觀測到的特徵點(1,2,3…n)座標(w,z)為： (p
b1w,
pb1z
),(p
b2w,
pb2z
),…,
(pbn
w,pb
nz)
求xy座標系變換到wz座標系的平移引數與旋轉引數。
座標系變換可以表示為： a∗
pa=p
b a=
⎡⎣⎢c
os(θ
)sin
(θ)0
−sin
(θ)c
os(θ
)0xt
yt1⎤
⎦⎥其中
θ 是旋轉角度，xt
,yt 是平移。
eigen有給出很多最小二乘的解決方案，例如下面就有乙個利用svd做最小二乘的方法：
補全**，可以得到乙個可執行的例子：
#include
#include "eigen/svd"
using
namespace
std;
using
namespace eigen;
int main()
已知mx=rhs,已知3* 2的矩陣m，以及3* 1的向量rhs。**通過最小二乘法求得了最優的x。步驟大致如下：
先利用matrixxf m = matrixxf::random(3,2);生成了3*2的隨機數矩陣m；
對m做svd分解。這個jacobisvd預設只會計算singular values。如果需要求得u或者v矩陣,需要另外設定。這裡我們要求解最小二乘，只需要求解 thin u 和 thin v，所以用到的函式是：
jacobisvdsvd(m, computethinu | computethinv);
用svd.solve(rhs)求解x。
前面的例程解出了mx
=rhs
中的x。但是回看第一部分，我們需要求解的是： a∗
pa=p
b ⎡⎣
⎢cos
(θ)s
in(θ
)0−s
in(θ
)cos
(θ)0
xtyt
1⎤⎦⎥
⎡⎣⎢p
axpa
y1⎤⎦
⎥=⎡⎣
⎢pbw
pbz1
⎤⎦⎥
其中已知的是右邊兩個矩陣。所以我們需要先重構矩陣為： ⎡⎣
⎢⎢⎢⎢
⎢⎢⎢⎢
⎢⎢⎢p
a1xp
a1yp
a2xp
a2y.
..pa
nxpa
ny−p
a1yp
a1x−
pa2y
pa2x
−pan
ypan
x101
0100
1010
1⎤⎦⎥
⎥⎥⎥⎥
⎥⎥⎥⎥
⎥⎥∗⎡
⎣⎢⎢⎢
⎢cos
(θ)s
in(θ
)xty
t⎤⎦⎥
⎥⎥⎥=
⎡⎣⎢⎢
⎢⎢⎢⎢
⎢⎢⎢⎢
⎢pb1
xpb1
ypb2
xpb2
y...
pbnx
pbny
⎤⎦⎥⎥
⎥⎥⎥⎥
⎥⎥⎥⎥
⎥ 現在就變成了mx=rhs求解x的形式，可以簡單改寫例程即可求解：
#include
#include "eigen/svd"
using
namespace
std;
using
namespace eigen;
int main()
在實際運用中，如果有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; …
根據輸入需求進行修改即可。
陣列重構
eigen—jacobisvd詳解
Matlab實現最小二乘法
p polyfit x,y,n 最小二乘法計算擬合多項式係數。x，y為擬合資料向量，要求維度相同，n為擬合多項式次數。返回p向量儲存多項式係數，由最高次向最低次排列。y polyval p,x 計算多項式的函式值。返回在x處多項式的值，p為多項式係數，元素按多項式降冪排序。擬合以下資料 x 12 9...
最小二乘法及其C 實現
監督學習中，如果 的變數是離散的，我們稱其為分類 如決策樹，支援向量機等 如果 的變數是連續的，我們稱其為回歸。回歸分析中，如果只包括乙個自變數和乙個因變數，且二者的關係可用一條直線近似表示，這種回歸分析稱為一元線性回歸分析。如果回歸分析中包括兩個或兩個以上的自變數，且因變數和自變數之間是線性關係，...
線性回歸 最小二乘法實現
目錄 一 線性回歸 給定由n個屬性描述的樣本x x0,x1,x2,xn 線性模型嘗試學習乙個合適的樣本屬性的線性組合來進行 任務，如 f x w1x1 w2x2 wnxn b w.t x b。通過一定方法學得向量w和常數b後，模型便可以確定下來。而對於給定資料集d xm x1,x2,xn 線性回歸則...