0

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

我可以用 Gauss-Newton 方法实现 PnP、ICP 和直接方法求解，因为它们的实现仅在雅可比矩阵和误差函数上有所不同，我想问有没有办法最小化三个误差函数并将它们组合在一起？例如，opencv库在“opencv_contrib/odometry.cpp”中结合了ICP和direct方法，我贴在下面，但是我不太明白“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, levelCameraMatrix_inv, resultRt_inv,
                                srcLevelDepth, srcFrame->pyramidMask[level], dstLevelDepth, dstFrame->pyramidNormalsMask[level],
                                maxDepthDiff, corresps_icp);
            if(corresps_rgbd.rows < minCorrespsCount && corresps_icp.rows < minCorrespsCount)
                break;

            Mat AtA(transformDim, transformDim, CV_64FC1, Scalar(0)), AtB(transformDim, 1, CV_64FC1, 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], resultRt,
                                   dstFrame->pyramidCloud[level], dstFrame->pyramidNormals[level],
                                   corresps_icp, AtA_icp, AtB_icp, icpEquationFuncPtr, transformDim);
                AtA += AtA_icp;
                AtB += AtB_icp;
            }
4

0 回答 0