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

发布于:2025-07-07 ⋅ 阅读:(20) ⋅ 点赞:(0)

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点深度。

失败处理

失败原因 返回值 触发条件
边缘特征方向与极线夹角过大 MatchResult::kFailAngle 夹角余弦值小于阈值 epi_search_edgelet_max_angle
图像块扭曲失败 MatchResult::kFailWarp warpAffine 返回失败
匹配得分不足 MatchResult::kFailScore 最佳 ZMSSD 得分超过阈值

关键算法与技巧

  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;
    }