【SVO】klt与极限搜索块匹配findEpipolarMatchDirect

Matcher::findEpipolarMatchDirect 函数逻辑与原理分析

核心目标：

在极线上搜索参考帧特征点 ref_ftr 在当前帧 cur_frame 中的最佳匹配点，并通过三角化计算深度。

关键步骤解析：

1. 极线端点计算：

const BearingVector A = T_cur_ref.getRotation().rotate(ref_ftr.f) + T_cur_ref.getPosition() * d_min_inv;
const BearingVector B = T_cur_ref.getRotation().rotate(ref_ftr.f) + T_cur_ref.getPosition() * d_max_inv;
cur_frame.cam()->project3(A, &px_A);  // 投影到当前帧像素坐标
cur_frame.cam()->project3(B, &px_B);
epi_image_ = px_A - px_B;  // 极线方向向量

• 原理：利用深度倒数范围 [d_min_inv, d_max_inv] 计算参考帧特征点对应的3D点在当前帧中的投影范围（px_A, px_B）。
• 作用：确定极线搜索区间。

2. 仿射扭曲矩阵计算：

warp::getWarpMatrixAffine(
    ref_frame.cam_, cur_frame.cam_, ref_ftr.px, ref_ftr.f,
    1.0 / d_estimate_inv, T_cur_ref, ref_ftr.level, &A_cur_ref_);

• 原理：根据两帧间位姿变换 T_cur_ref 和深度估计值，计算参考帧到当前帧的仿射变换矩阵 A_cur_ref_。
• 作用：补偿视角变化导致的图像变形。

3. 边缘特征方向过滤：

if (isEdgelet(ref_ftr.type) {
    const Eigen::Vector2d grad_cur = (A_cur_ref_ * ref_ftr.grad).normalized();
    const double cosangle = fabs(grad_cur.dot(epi_image_.normalized()));
    if (cosangle < options_.epi_search_edgelet_max_angle) 
        return MatchResult::kFailAngle;  // 边缘方向与极线夹角过大
}

• 原理：边缘特征的梯度方向应与极线方向一致（夹角小）。
• 作用：过滤掉梯度方向与极线方向不一致的边缘特征，提升匹配鲁棒性。

4. 图像金字塔预处理：

search_level_ = warp::getBestSearchLevel(A_cur_ref_, ref_frame.img_pyr_.size() - 1);
epi_length_pyramid_ = epi_image_.norm() / (1 << search_level_);

• 原理：根据仿射矩阵尺度选择最佳金字塔层级 search_level_，并计算该层级的极线长度。
• 作用：在粗分辨率上快速搜索，减少计算量。

5. 参考图像块扭曲：

warp::warpAffine(A_cur_ref_, ref_frame.img_pyr_[ref_ftr.level], ...);
patch_utils::createPatchFromPatchWithBorder(...);  // 提取参考图像块

• 原理：将参考帧中的图像块通过仿射变换 A_cur_ref_ 扭曲到当前帧视角。
• 作用：生成用于匹配的模板图像块 patch_。

6. 极线搜索策略：

• 情况1：极线过短（epi_length_pyramid_ < 2.0）

  px_cur_ = (px_A + px_B) / 2.0;  // 取极线中点
  findLocalMatch(cur_frame, epi_dir_image, search_level_, px_cur_);  // 局部搜索
  

• 情况2：极线较长

  PatchScore patch_score(patch_);  // 预计算参考图像块评分
  scanEpipolarLine(cur_frame, A, B, C, patch_score, ...);  // 沿极线扫描
  

• 原理：
  • 短极线：直接在极线中点附近局部搜索。
  • 长极线：在极线上滑动参考图像块，通过 ZMSSD（零均值平方和差） 计算匹配得分，选择得分最高的位置。

7. 匹配验证与优化：

if (zmssd_best < PatchScore::threshold()) {
    if (options_.subpix_refinement) 
        findLocalMatch(...);  // 亚像素优化
    cur_frame.cam()->backProject3(px_cur_, &f_cur_);  // 反投影到3D
    depthFromTriangulation(T_cur_ref, ref_ftr.f, f_cur_, &depth);  // 三角化深度
}

• 原理：
  • 亚像素优化：在粗匹配位置附近进行高斯牛顿迭代，提升精度。
  • 三角化：利用两帧间位姿和匹配点光线，求解3D点深度。

失败处理：

关键算法与技巧：

1. 仿射光流（Affine Warping）
   • 补偿视角变化，使图像块在不同帧间保持形状一致。
2. 金字塔搜索（Pyramid Search）
   • 在低分辨率图像上快速定位，逐步细化到高分辨率。
3. 极线约束（Epipolar Constraint）
   • 将搜索范围从整幅图像压缩到一条直线，减少计算量。
4. ZMSSD 匹配度量
   • 对光照变化鲁棒：通过减去图像块均值，消除亮度差异影响。
5. 亚像素优化（Subpixel Refinement）
   • 通过二次拟合或高斯牛顿法，使匹配精度达到亚像素级别。

代码流程总结：

此函数实现了高效且鲁棒的直接法特征匹配，适用于视觉里程计（VO）和SLAM系统中的特征跟踪与深度估计。

    Matcher::MatchResult Matcher::findEpipolarMatchDirect(
        const Frame& ref_frame,
        const Frame& cur_frame,
        const Transformation& T_cur_ref,
        const FeatureWrapper& ref_ftr,
        const double d_estimate_inv,
        const double d_min_inv,
        const double d_max_inv,
        double& depth)
    {
        int zmssd_best = PatchScore::threshold();

        // Compute start and end of epipolar line in old_kf for match search, on image plane
        const BearingVector A = T_cur_ref.getRotation().rotate(ref_ftr.f) + T_cur_ref.getPosition() * d_min_inv;
        const BearingVector B = T_cur_ref.getRotation().rotate(ref_ftr.f) + T_cur_ref.getPosition() * d_max_inv;
        Eigen::Vector2d px_A, px_B;
        cur_frame.cam()->project3(A, &px_A);
        cur_frame.cam()->project3(B, &px_B);
        epi_image_ = px_A - px_B;

        //LOG(INFO) << "A:" << A;
        //LOG(INFO) << "B:" << B;
        
        //LOG(INFO) << "px_A:" << px_A;
        //LOG(INFO) << "px_B:" << px_B;

        // Compute affine warp matrix
        warp::getWarpMatrixAffine(
            ref_frame.cam_, cur_frame.cam_, ref_ftr.px, ref_ftr.f,
            1.0 / std::max(0.000001, d_estimate_inv), T_cur_ref, ref_ftr.level, &A_cur_ref_);

        // feature pre-selection
        reject_ = false;
        if (isEdgelet(ref_ftr.type) && options_.epi_search_edgelet_filtering)
        {
            const Eigen::Vector2d grad_cur = (A_cur_ref_ * ref_ftr.grad).normalized();
            const double cosangle = fabs(grad_cur.dot(epi_image_.normalized()));
            if (cosangle < options_.epi_search_edgelet_max_angle)
            {
                reject_ = true;
                return MatchResult::kFailAngle;
            }
        }

        //LOG(INFO) << "A_cur_ref_: " << A_cur_ref_;

        // prepare for match
        //    - find best search level
        //    - warp the reference patch
        search_level_ = warp::getBestSearchLevel(A_cur_ref_, ref_frame.img_pyr_.size() - 1);
        // length and direction on SEARCH LEVEL
        epi_length_pyramid_ = epi_image_.norm() / (1 << search_level_);
        GradientVector epi_dir_image = epi_image_.normalized();

        if (!warp::warpAffine(A_cur_ref_, ref_frame.img_pyr_[ref_ftr.level], ref_ftr.px,
            ref_ftr.level, search_level_, kHalfPatchSize + 1, patch_with_border_))
            return MatchResult::kFailWarp;

        patch_utils::createPatchFromPatchWithBorder(
            patch_with_border_, kPatchSize, patch_);

        // Case 1: direct search locally if the epipolar line is too short
        if (epi_length_pyramid_ < 2.0)
        {
            px_cur_ = (px_A + px_B) / 2.0;
            MatchResult res = findLocalMatch(cur_frame, epi_dir_image, search_level_, px_cur_);
            if (res != MatchResult::kSuccess)
                return res;
            cur_frame.cam()->backProject3(px_cur_, &f_cur_);
            f_cur_.normalize();
            return matcher_utils::depthFromTriangulation(T_cur_ref, ref_ftr.f, f_cur_, &depth);
        }

        // Case 2: search along the epipolar line for the best match
        PatchScore patch_score(patch_); // precompute for reference patch
        BearingVector C = T_cur_ref.getRotation().rotate(ref_ftr.f) + T_cur_ref.getPosition() * d_estimate_inv;

        //LOG(INFO) << "C: " << C;
        //LOG(INFO) << "px_cur_: " << std::setprecision(15) << px_cur_.transpose();

        scanEpipolarLine(cur_frame, A, B, C, patch_score, search_level_, &px_cur_, &zmssd_best);

        //LOG(INFO) << "zmssd_best: " << zmssd_best;

        // check if the best match is good enough
        if (zmssd_best < PatchScore::threshold())
        {
            if (options_.subpix_refinement)
            {
                MatchResult res = findLocalMatch(cur_frame, epi_dir_image, search_level_, px_cur_);
                if (res != MatchResult::kSuccess)
                    return res;
            }

            //LOG(INFO) << "BACK PROJECT";

            cur_frame.cam()->backProject3(px_cur_, &f_cur_);
            f_cur_.normalize();

            //LOG(INFO) << "f_cur_ NORM: " <<std::setprecision(15)<< f_cur_.x() <<" " << f_cur_.y() << " " << f_cur_.z();
            //LOG(INFO) << "T_cur_ref: \n" << std::setprecision(15) << T_cur_ref;
            //LOG(INFO) << "ref_ftr.f: " << std::setprecision(15) << ref_ftr.f.transpose();

            return matcher_utils::depthFromTriangulation(T_cur_ref, ref_ftr.f, f_cur_, &depth);
        }
        else
            return MatchResult::kFailScore;
    }