如何结合PnP、ICP和直接法的非线性方法？

如何解决如何结合PnP、ICP和直接法的非线性方法？

我目前正在研究一些视觉里程计算法，我发现在解决 PnP 和 ICP 等问题时​​有非线性和线性方法，而直接方法可以最大限度地减少两点之间的强度误差。上述非线性方法也将重投影误差的两点之间的误差最小化。

我可以使用高斯-牛顿方法实现 PnP、ICP 和直接方法求解，因为它们的实现仅在雅可比矩阵和误差函数上有所不同，我想问有没有办法最小化三个误差函数并将它们组合在一起？例如opencv库在“opencv_contrib/odometry.cpp”中结合了ICP和直接方法，我贴在下面，但是我不太明白“calcRgbdLsmMatrices”函数中的原理，有没有其他方法可以将三个结合起来他们使用非线性方法？

提前感谢您的帮助！！

for(int iter = 0; iter < iterCounts[level]; iter ++) 
        {
            Mat resultRt_inv = resultRt.inv(DECOMP_SVD);
            if(method & RGBD_odoMETRY)
                computeCorresps(levelCameraMatrix,levelCameraMatrix_inv,resultRt_inv,srcLevelDepth,srcFrame->pyramidMask[level],dstLevelDepth,dstFrame->pyramidTexturedMask[level],maxDepthDiff,corresps_rgbd);

            if(method & ICP_odoMETRY)
                computeCorresps(levelCameraMatrix,dstFrame->pyramidnormalsMask[level],corresps_icp);
            if(corresps_rgbd.rows < minCorrespsCount && corresps_icp.rows < minCorrespsCount)
                break;

            Mat AtA(transformDim,transformDim,CV_64FC1,Scalar(0)),AtB(transformDim,1,Scalar(0));
            if(corresps_rgbd.rows >= minCorrespsCount) // 120
            {
                calcRgbdLsmMatrices(srcFrame->pyramidImage[level],srcFrame->pyramidCloud[level],resultRt,dstFrame->pyramidImage[level],dstFrame->pyramid_dI_dx[level],dstFrame->pyramid_dI_dy[level],corresps_rgbd,fx,fy,sobelScale,AtA_rgbd,AtB_rgbd,rgbdEquationFuncPtr,transformDim);

                AtA += AtA_rgbd;
                AtB += AtB_rgbd;
            }
            if(corresps_icp.rows >= minCorrespsCount)
            {
                calcICPLsmMatrices(srcFrame->pyramidCloud[level],dstFrame->pyramidCloud[level],dstFrame->pyramidnormals[level],corresps_icp,AtA_icp,AtB_icp,icpEquationFuncPtr,transformDim);
                AtA += AtA_icp;
                AtB += AtB_icp;
            }