【MVS】极线约束

原创于 2024-12-27 14:57:55 发布 · 723 阅读

8 ·

CC 4.0 BY-SA版权

文章标签：

#xr #深度学习 #线性代数 #无人机

SLAM 专栏收录该内容

80 篇文章

订阅专栏

极线约束

- 1. 极线约束

1. 极线约束

int TrackLocalMap::epipolar_check(cv::Point2f p0, cv::Point2f p1)
{
    // undist to normalize coordinate!
    cv::Point2f p0_undist = camera_calib.at(0)->undistort_cv(p0);
    cv::Point2f p1_undist = camera_calib.at(1)->undistort_cv(p1);
    Eigen::Vector3f uv0(p0_undist.x, p0_undist.y, 1);
    Eigen::Vector3f uv1(p1_undist.x, p1_undist.y, 1);
    // Eigen::Vector3f uv0_pixel = K0 * uv0;
    Eigen::Vector3f uv1_pixel = K1 * uv1;

    Eigen::Vector3f P0_min = min_dist * uv0;
    Eigen::Vector3f P0_max = max_dist * uv0;
    Eigen::Vector4f P1_min = T_c1c0 * P0_min.homogeneous();
    Eigen::Vector4f P1_max = T_c1c0 * P0_max.homogeneous();

    Eigen::Vector3f uv1_min = (1.0 / P1_min(2)) * K1 * P1_min.head<3>();  // (xa, ya)
    Eigen::Vector3f uv1_max = (1.0 / P1_max(2)) * K1 * P1_max.head<3>();  // (xb, yb)

    float dx = uv1_min(0) - uv1_max(0);
    float dy = uv1_min(1) - uv1_max(1);
    float point2epline_dist =
        fabs((dy * uv1_pixel(0) - dx * uv1_pixel(1) + uv1_max(1) * uv1_min(0) - uv1_min(1) * uv1_max(0)))
        / sqrt(dx * dx + dy * dy);
    
    float dist_TH = epi_dist;

    //float min_x = (uv1_min(0) < uv1_max(0)) ? uv1_min(0) : uv1_max(0);
    //float max_x = uv1_min(0) + uv1_max(0) - min_x;
    //bool is_good = (point2epline_dist < dist_TH) && (uv1_pixel(0) < max_x) && (uv1_pixel(0) > min_x);
    float min_y = (uv1_min(1) < uv1_max(1)) ? uv1_min(1) : uv1_max(1);
    float max_y = uv1_min(1) + uv1_max(1) - min_y;
    bool is_good = (point2epline_dist < dist_TH) && (uv1_pixel(1) < max_y) && (uv1_pixel(1) > min_y);

    //if (!is_good) {
    //    printf("LRfail: p0: [%.2f,%.2f], p1: [%.2f,%.2f], uv1_pixel: [%.2f,%.2f], min = [%.2f,%.2f], max = [%.2f,%.2f], dist=%.2f\n",
    //        p0.x, p0.y, p1.x, p1.y, uv1_pixel(0), uv1_pixel(1), uv1_min(0), uv1_min(1), uv1_max(0), uv1_max(1), point2epline_dist);
    //}

    return is_good;
}

这段代码实现的是一个用于检查两个点是否满足极线约束的函数。极线约束是立体视觉中的一个基本概念，它描述了两个视图中对应点之间的关系。下面是对这段代码的数学推导和解释。

1. 未畸变坐标

首先，将输入的点 ( p0 ) 和 ( p1 ) 通过相机校准参数进行未畸变处理，得到未畸变的点 ( p0_undist ) 和 ( p1_undist )。

2. 归一化坐标

将未畸变的点转换为归一化坐标系中的点 ( uv0 ) 和 ( uv1 )。归一化坐标系是相机坐标系，其中 ( z = 1 )。

3. 像素坐标

将 ( uv1 ) 通过相机内参 ( K1 ) 转换为像素坐标 ( uv1_pixel )。

4. 极线约束

对于点 ( p0 )，在相机0的坐标系中，它对应的3D点可以表示为 ( P0_min ) 和 ( P0_max )，分别代表最小和最大距离。通过相机之间的变换矩阵 ( T_c1c0 )，将 ( P0_min ) 和 ( P0_max ) 转换到相机1的坐标系中，得到 ( P1_min ) 和 ( P1_max )。

5. 极线方程

将 ( P1_min ) 和 ( P1_max ) 转换为归一化坐标系中的点 ( uv1_min ) 和 ( uv1_max )。这两个点定义了极线在相机1的图像平面上的两个端点。

6. 点到极线的距离

计算点 ( uv1_pixel ) 到极线 ( uv1_min ) 和 ( uv1_max ) 的距离。极线方程可以表示为 ( ax + by + c = 0 )，其中 ( a = dy )，( b = -dx )，( c = uv1_max(1) * uv1_min(0) - uv1_min(1) * uv1_max(0) )。

点到直线的距离公式为：
[
\text{distance} = \frac{|ax_1 + by_1 + c|}{\sqrt{a^2 + b^2}}
]
其中 ( (x_1, y_1) ) 是点 ( uv1_pixel ) 的坐标。