移动端实时三维重建KinectFusion-ios(4)——ICP模块

        博主的源码地址：https://github.com/sjy234sjy234/KinectFusion-ios

        原论文："KinectFusion: Real-time dense surface mapping and tracking."

        之前的文章介绍了KinectFusion-ios的算法框架，主要由4个处理模块构成，并介绍了数据准备模块。本文主要介绍第2个处理模块——ICP模块，用于解决实时的相机位姿估计。

一、相机位姿估计

        下面的数学模型描述，来自高翔的视觉slam十四讲

                     

        在我们的场景下，默认第一帧的相机位姿为参考帧，相机位姿估计的作用是对于新到来的每一帧深度数据，估计该帧在拍摄时的相机的位置，即一个空间变换矩阵。其中，每一帧的相机位姿估计都需要用到ICP算法进行求解。

二、基于投影的ICP

        如上面的数学模型所描述，ICP算法，本来是用来求解两组相似形状的点云之间的空间变换的。但是，点云存储的无序性，导致ICP算法要消耗很长的时间去搜索这两组点云之间相互匹配好的点对。对于实时三维重建系统，通常使用基于投影的ICP进行相机位姿的求解。即对于时间上连续的两帧深度图数据，可以分别提取以像素栅格化格式存储的三维顶点数据图。将这些数据当成点云，由于连续两帧之间的相机位姿变换通常较小，这里的ICP在初始迭代的时候不用再进行邻近点匹配，而把存储在相同像素位置的顶点当作邻近点直接进行匹配，不停进行迭代求解。这种基于投影的ICP，运行效率很高，而且也可以利用GPGPU进行优化加速。此外，三维顶点数据图的提取过程请参考博主的上一篇文章。

三、ICP的一轮迭代

        数学的推导过程请参考KinectFusion的原文章，这里不详细展开。基于连续帧小角度变换的假设，以及点到面的误差度量，ICP的每一轮迭代被近似简化成求解一个6元超定的线性方程组，所求的6个未知数是(△x, △y, △z, △α, △β, △γ)，其中的(△x, △y, △z)是偏移，(△α, △β, △γ)是旋转。

        假设该方程组为A x = b，其中x = [△x, △y, △z, △α, △β, △γ]'， A是一个nx6矩阵，b是一个nx1的向量。因此，ICP的一轮迭代可以概括成两步操作：准备方程组、求解方程组。

1、准备方程组

        即将时间上连续的两帧的顶点数据图和法向数据图作为输入，根据点到面的误差度量以及小角度近似，提取出方程组A x = b中的A矩阵和b向量。这部分实现的源码在文件夹FuICPPrepareMatrix中，这是一个简单的metal gpgpu管线，逐像素的转换由gpgpu核函数并行实现。

2、求解方程组

        对于超定方程组A x = b，可以两边同时乘以A的转置矩阵A'，得到(A' A) x = (A' b)，令A* = A' A， b * = A' b，得到新的方程组A* x = b*。其中，A*是6x6的矩阵，b*是6x1的向量，求解新的方程组就可以得到原超定方程组的最小二乘解。这个数值计算的结论在理论上已经得到证明。因此，求解超定方程组可以分成两步操作：简化方程矩阵、求解简化方程。

        首先，简化方程矩阵的源码在FuICPReduceMatrix文件夹中，利用metal自带的MetalPerformanceShaders实现。然后，求解简化方程的操作，由于计算量很小，我们直接使用Eigen库在CPU中实现。值得注意的是，简化方程矩阵的操作中，其实是在做两个矩阵乘法运算A* = A' A和b * = A' b，而使用MetalPerformanceShaders的api实现的矩阵乘法的运行效率较低。这里程序可优化的空间较大。

四、代码的整体调用

         下面的代码在FusionProcessor.mm文件的processFrame方法中

    //icp
    if(fusionFrameIndex<=0)
    {
        //first frame, no icp
        NSLog(@"first frame, fusion reset");
        [self reset];
    }
    else
    {
        //icp iteration
        BOOL isSolvable=YES;
        simd::float3x3 currentF2gRotate;
        simd::float3 currentF2gTranslate;
        simd::float3x3 preF2gRotate;
        simd::float3 preF2gTranslate;
        simd::float3x3 preG2fRotate;
        simd::float3 preG2fTranslate;
        matrix_transform_extract(m_frameToGlobalTransform,currentF2gRotate,currentF2gTranslate);
        matrix_transform_extract(m_frameToGlobalTransform, preF2gRotate, preF2gTranslate);
        matrix_transform_extract(m_globalToFrameTransform, preG2fRotate, preG2fTranslate);
        for(int level=PYRAMID_LEVEL-1;level>=0;--level)
        {
            uint iteratorNumber=ICPIteratorNumber[level];
            for(int it=0;it<iteratorNumber;++it)
            {
                uint occupiedPixelNumber = [_fuICPPrepareMatrix computeCurrentVMap:m_currentVertexMapPyramid[level] andCurrentNMap:m_currentNormalMapPyramid[level] andPreVMap:m_preVertexMapPyramid[level] andPreNMap:m_preNormalMapPyramid[level] intoLMatrix:m_icpLeftMatrixPyramid[level] andRMatrix:m_icpRightMatrixPyramid[level] withCurrentR:currentF2gRotate andCurrentT:currentF2gTranslate andPreF2gR:preF2gRotate andPreF2gT:preF2gTranslate andPreG2fR:preG2fRotate andPreG2fT:preG2fTranslate  andThreshold:m_icpThreshold andIntrinsicXYZ2UVD:m_intrinsicXYZ2UVD[level] withLevel:level];
                if(occupiedPixelNumber==0)
                {
                    isSolvable=NO;
                }
                if(isSolvable)
                {
                    [_fuICPReduceMatrix computeLeftMatrix:m_icpLeftMatrixPyramid[level] andRightmatrix:m_icpRightMatrixPyramid[level] intoLeftReduce:m_icpLeftReduceBuffer andRightReduce:m_icpRightReduceBuffer withLevel:level andOccupiedNumber:occupiedPixelNumber];
                    float result[6];
                    isSolvable=matrix_float6x6_solve((float*)m_icpLeftReduceBuffer.contents, (float*)m_icpRightReduceBuffer.contents, result);
                    if(isSolvable)
                    {
                        simd::float3x3 rotateIncrement=matrix_float3x3_rotation(result[0], result[1], result[2]);
                        simd::float3 translateIncrement={result[3], result[4], result[5]};
                        currentF2gRotate=rotateIncrement*currentF2gRotate;
                        currentF2gTranslate=rotateIncrement*currentF2gTranslate+translateIncrement;
                    }
                }
            }
            if(!isSolvable)
            {
                break;
            }
        }
        if(isSolvable)
        {
            matrix_transform_compose(m_frameToGlobalTransform, currentF2gRotate, currentF2gTranslate);
            m_globalToFrameTransform=simd::inverse(m_frameToGlobalTransform);
        }
        else
        {
            NSLog(@"lost frame");
            return NO;
        }
    }

        第2-7行，当帧序号fusionFrameIndex为0时，把初始帧作为参考帧，不需要进行ICP，调用[self reset]，对所有数据进行初始化，进入全新的一次重建。

        第8-60行，当帧序号fusionFrameIndex大于0时，这是一帧新到来的深度帧，需要进行ICP相机位姿估计。其中，26-43行是单轮的ICP迭代，通常ICP需要通过多次迭代才能收敛。此外，在求解ICP迭代时，还利用了上一篇文章介绍的mipmap降采样的数据，来降低求解的复杂度。