http://www.cnblogs.com/gaoxiang12/p/4669490.html
得到两帧图像的选择矩阵和位移矩阵,就可以把两张图像的点云给拼接起来,形成更大的点云。
点云的拼接,实质上是对点云做变换的过程。这个变换往往是用变换矩阵(transform matrix)来描述的:
T=[R3×3O1×3t3×11]∈R4×4T=[R3×3t3×1O1×31]∈R4×4
该矩阵的左上部分是一个3×33×3的旋转矩阵,它是一个正交阵。右上部分是3×13×1的位移矢量。左下是3×13×1的缩放矢量,在SLAM中通常取成0,因为环境里的东西不太可能突然变大变小(又没有缩小灯)。右下角是个1.
这样的一个阵可以对点或者其他东西进行齐次变换:
⎡⎣⎢⎢⎢⎢y1y2y31⎤⎦⎥⎥⎥⎥=T⋅⎡⎣⎢⎢⎢⎢x1x2x31⎤⎦⎥⎥⎥⎥[y1y2y31]=T⋅[x1x2x31]
由于变换矩阵结合了旋转和缩放,是一种较为经济实用的表达方式。它在机器人和许多三维空间相关的科学中都有广泛的应用。PCL里提供了点云的变换函数,只要给定了变换矩阵,就能对移动整个点云:
至此,我已经实现了一个只有两帧的SLAM程序。然而,也许你还不知道,这已经是一个视觉里程计(Visual Odometry)啦!只要不断地把进来的数据与上一帧对比,就可以得到完整的运动轨迹以及地图了呢!