Kinect Fusion ICP算法极小化目标函数

Kinect Fusion采用的是点到面的距离和作为目标函数：

式子中的R为旋转矩阵，t为平移向量；pi为当前帧在通过上一帧的坐标转换矩阵（或者上一次迭代之后的坐标转换矩阵）转换成全局坐标系的坐标，qi为上一帧在全局坐标系下的坐标（通过光线投影获得的坐标），ni为上一帧的法向量；

由于两位姿变换较小，所以旋转矩阵可以表述为：

所以，

所以，

对上述得到的近似的目标函数进行求偏导，得到

通过求解上述线性方程来得到x。