KinectFusion 中的相机的位姿估计使用了点到面的ICP算法,因为其假设算法运行的帧率很高,所以相邻两帧相机的位姿变化较小,使用点到面的ICP比点到点的ICP算法计算速度快,收敛速度快。
其使用的公式如下所示
(1)式中,p为当前帧的3d 点, q是TSDF中对应的3d点,求解的位姿是当前帧相对于TSDF坐标系的位姿。
(5)式子中,求解雅克比矩阵使用了链式求导法则,其中李代数的前三个参数是translation部分,后三个参数对应的是rotation。其中(4)可以参考<<视觉SLAM14讲>>
代码如下所示,注意下面的雅克比矩阵先是对rotation求导,在对translation求导。
typedef Matx<float, 6, 7> ABtype;
struct GetAbInvoker : ParallelLoopBody
{
GetAbInvoker(ABtype& _globalAb, Mutex& _mtx,
const Points& _oldPts, const Normals& _oldNrm, const Points& _newPts, const Normals& _newNrm,
Affine3f _pose, Intr::Projector _proj, float _sqDistanceThresh, float _minCos) :
ParallelLoopBody(),
globalSumAb(_globalAb), mtx(_mtx),
oldPts(_oldPts), oldNrm(_oldNrm), newPts(_newPts), newNrm(_newNrm), pose(_pose),
proj(_proj), sqDistanceThresh(_sqDistanceThresh), minCos(_minCos)
{ }
virtual void operator ()(const Range& range) const override
{
#if USE_INTRINSICS
CV_Assert(ptype::channels == 4);
const size_t utBufferSize = 9;
float CV_DECL_ALIGNED(16) upperTriangle[utBufferSize*4];
for(size_t i = 0; i < utBufferSize*4; i++)
upperTriangle[i] = 0;
// how values are kept in upperTriangle
const int NA = 0;
const size_t utPos[] =
{
0, 1, 2, 4, 5, 6, 3,
NA, 9, 10, 12, 13, 14, 11,
NA, NA, 18, 20, 21, 22, 19,
NA, NA, NA, 24, 28, 30, 32,
NA, NA, NA, NA, 25, 29, 33,
NA, NA, NA, NA, NA, 26, 34
};
//
// T = [pos]
//
//
//