KinectFusion: 位姿估计

最新推荐文章于 2024-06-25 09:42:29 发布

minxuanjun

最新推荐文章于 2024-06-25 09:42:29 发布

阅读量1.1k

点赞数

分类专栏： KinectFusion 文章标签： SLAM 三维重建

本文链接：https://blog.csdn.net/minxuanjun/article/details/86713438

版权

本文介绍了KinectFusion中利用点到面ICP算法进行相机位姿估计的过程。由于算法假设高帧率，位姿变化小，点到面ICP在速度和收敛性上优于点到点ICP。通过公式(1)表示当前帧3d点与TSDF中对应点的关系，利用链式求导法则求解位姿雅克比矩阵，其中李代数参数分别对应平移和旋转。代码实现中，雅克比矩阵首先对旋转求导，然后对平移求导。

摘要由CSDN通过智能技术生成

KinectFusion 中的相机的位姿估计使用了点到面的ICP算法，因为其假设算法运行的帧率很高，所以相邻两帧相机的位姿变化较小，使用点到面的ICP比点到点的ICP算法计算速度快，收敛速度快。

其使用的公式如下所示

$E(\xi ) = \sum \left \|n_{q_i}^T* (exp(\xi ^\Lambda ) p_i - q_i)\right \|_2} (1)$

$\mu = exp(\xi ^\Lambda ) p_i - q_i (2)$

$\frac{\partial E(\xi )}{\partial \mu} = n_{q_i}^T (3)$

$\frac{\partial \mu}{\partial \xi} = [I_{3x3}, - (exp(\xi ^{\Lambda })*p_i)^{\Lambda }] (4)$

$\frac{\partial E(\xi )}{\partial \xi} = \frac{\partial E(\xi )}{\partial \mu}\frac{\partial \mu}{\partial \xi} = n_{q_i}^T [I_{3x3}, - (exp(\xi ^{\Lambda })*p_i)^{\Lambda }] = [ n_{q_i}^T, ((exp(\xi ^{\Lambda })*p_i)^{\Lambda } *n_{q_i})^T] (5)$

（1）式中，p为当前帧的3d 点， q是TSDF中对应的3d点，求解的位姿是当前帧相对于TSDF坐标系的位姿。

（5）式子中，求解雅克比矩阵使用了链式求导法则，其中李代数 $\xi$ 的前三个参数是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] 
        // 
        // 
        //

最低0.47元/天解锁文章

minxuanjun

关注

0
点赞
踩
6

收藏

觉得还不错? 一键收藏
1
评论
KinectFusion: 位姿估计

KinectFusion 中的相机的位姿估计使用了点到面的ICP算法，因为其假设算法运行的帧率很高，所以相邻两帧相机的位姿变化较小，使用点到面的ICP比点到点的ICP算法计算速度快，收敛速度快。其使用的公式如下所示（1）式中，p为当前帧的3d 点， q是TSDF中对应的3d点，求解的位姿是当前帧相对于TSDF坐标系的位姿。（5）式子中，求解雅克比矩阵使用了链式求导...
复制链接

扫一扫

专栏目录