Kinect Fusion采用的是点到面的距离和作为目标函数:
式子中的R为旋转矩阵,t为平移向量;pi为当前帧在通过上一帧的坐标转换矩阵(或者上一次迭代之后的坐标转换矩阵)转换成全局坐标系的坐标,qi为上一帧在全局坐标系下的坐标(通过光线投影获得的坐标),ni为上一帧的法向量;
由于两位姿变换较小,所以旋转矩阵可以表述为:
所以,
所以,
对上述得到的近似的目标函数进行求偏导,得到
通过求解上述线性方程来得到x。