深蓝学院视觉SLAM第六次习题（下）

最新推荐文章于 2023-02-21 12:04:48 发布

小白的进修之路

最新推荐文章于 2023-02-21 12:04:48 发布

阅读量1.9k

点赞数

分类专栏：视觉SLAM十四讲

本文链接：https://blog.csdn.net/weixin_44218240/article/details/106059234

版权

这篇博客介绍了视觉SLAM中直接法的应用，包括误差计算、雅可比矩阵和多层图像金字塔。还讨论了直接法与光流的比较，提出光流的inverse和compositional概念的无效性，并分析了直接法的加速策略。此外，博主通过计算光流来估算视差，并与给定图像进行比较，但遇到了不理想的结果，可能是代码错误导致的。

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

2.直接法

误差为
在这里插入图片描述
雅可比矩阵为

首先要计算移动过的点的坐标，之后求解雅可比矩阵

void DirectPoseEstimationSingleLayer(
        const cv::Mat &img1,
        const cv::Mat &img2,
        const VecVector2d &px_ref,
        const vector<double> depth_ref,
        Sophus::SE3 &T21
) {
   

    // parameters
    int half_patch_size = 4;
    int iterations = 100;

    double cost = 0, lastCost = 0;
    int nGood = 0;  // good projections
    VecVector2d goodProjection;

    for (int iter = 0; iter < iterations; iter++) {
   
        nGood = 0;
        goodProjection.clear();

        // Define Hessian and bias
        Matrix6d H = Matrix6d::Zero();  // 6x6 Hessian
        Vector6d b = Vector6d::Zero();  // 6x1 bias

        for (size_t i = 0; i < px_ref.size(); i++) {
   

            // compute the projection in the second image
            // TODO START YOUR CODE HERE
	    double X_ref=depth_ref[i]*(px_ref[i][0]-cx)/fx;
	    double Y_ref=depth_ref[i]*(px_ref[i][1]-cy)/fy;
	    double Z_ref=depth_ref[i];
	    Matrix4d T_21=T21.matrix();
	    double X_cur=T_21(0,0)*X_ref+T_21(0,1)*Y_ref+T_21(0,2)*Z_ref+T_21(0,3);
	    double Y_cur=T_21(1,0)*X_ref+T_21(1,1)*Y_ref+T_21(1,2)*Z_ref+T_21(1,3);
	    double Z_cur=T_21(2,0)*X_ref+T_21(2,1)*Y_ref+T_21(2,2)*Z_ref+T_21(2,3);
	  
            float u =0, v = 0;
	    u=(