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

最近用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(θ)