基于ICP算法计算点集之间的变换矩阵（旋转、平移）

前言

本文主要是计算两个激光雷达之间的变换矩阵，即计算两组点云之间的变换矩阵。其中处理的点云数据主要是由x,y,z,intensity组成的，代表空间位置x,ry,z 和每个点云对应的反射强度intensity；

这里计算点集之间的变换矩阵，用到每个点云的x,y,z信息，可表示为n*3的数组；两组激光雷达点云，可以表示为2和n*3的数组。

首先使用ICP点云匹配算法，计算两组点云之n*3的数组间对应的点；然后基于SVD算法求出两个对应点集合的旋转矩阵R和转移矩阵t。

目录

一、基于ICP匹配对应点

二、计算变换矩阵

2.1 思路

2.2 计算中心点

2.3 将点集合移动到原点

2.4 计算协方差矩阵H

2.5 计算旋转矩阵R

2.6 计算平移向量t

三、实现代码案例

参考文献

---

一、基于ICP匹配对应点

基于点的匹配算法中常用最多的是 ICP（Iterative Closest Point）算法；其实质是基于两个集合中最近点对的搜索，并且估计将它们对齐的“刚性变换”。然后，将刚性变换应用于一个集合的点，并迭代该过程直到收敛。

优点：原理简单、易于理解和实现，同时配准精度较高。

弊端：ICP 算法在迭代初期就必须确定初始位置&