毫米波雷达点云帧间匹配的定位

１．目的：通过毫米波雷达的点云scan to scan 匹配，更新雷达的位置

物理模型【参考１】：

-若是绕着静态的世界坐标系旋转，有RA，即左乘旋转矩阵 
- 若是绕着动态的自身坐标系旋转，有A’R’， 即右乘旋转矩阵 
- 若是进行平移，则有TA，即左乘平移矩阵， A’T’为右乘平移矩阵

那么雷达在移动，所以右乘

Radar_pos1 * cloudpoint_radar1=  Radar_pos2 * cloudpoint_radar2　＝cloudpoint的世界坐标（不变）

这些参数均是４＊４的齐次矩阵。

为什么不变呢？因为我们认为帧间的物体几乎一致。

那么Ｒadar_pos2= Radar_pos1 * cloudpoint_radar1* inv(cloudpoint_radar2)      其中ｉｎｖ表示逆运算

cloudpoint_radar1* inv(cloudpoint_radar2) 这就是帧间匹配的结果，利用ＩＣＰ等匹配方法就可以得到。

最后，Ｒadar_pos2= Radar_pos1 * ＩＣＰ变换矩阵

其中ｉｃｐ变换矩阵是从后一帧变换到前一帧的变换矩阵。

具体的代码实现可以参考【参考文献２】

参考资料：

１．矩阵左乘与右乘的区别：https://blog.csdn.net/miaomiaoyuan/article/details/54973363

２．激光雷达匹配更新的ｍａｔｌａｂ代码：https://www.cnblogs.com/21207-iHome/p/7994033.html