移動端實時三維重建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降采樣的資料，來降低求解的複雜度。