ORB-SLAM 重投影误差卡方检验

swx1000

已于 2024-06-10 21:28:28 修改

阅读量1.3k

点赞数 12

文章标签：算法人工智能

于 2024-06-10 21:23:29 首次发布

本文链接：https://blog.csdn.net/swx89/article/details/139581615

版权

/**
 * @brief 对给定的homography matrix打分,需要使用到卡方检验的知识
 * 
 * @param[in] H21                       从参考帧到当前帧的单应矩阵
 * @param[in] H12                       从当前帧到参考帧的单应矩阵
 * @param[in] vbMatchesInliers          匹配好的特征点对的Inliers标记
 * @param[in] sigma                     方差，默认为1
 * @return float                        返回得分
 */
float Initializer::CheckHomography(
    const cv::Mat &H21,                 //从参考帧到当前帧的单应矩阵
    const cv::Mat &H12,                 //从当前帧到参考帧的单应矩阵
    vector<bool> &vbMatchesInliers,     //匹配好的特征点对的Inliers标记
    float sigma)                        //估计误差
{
    // 说明：在已值n维观测数据误差服从N(0，sigma）的高斯分布时
    // 其误差加权最小二乘结果为  sum_error = SUM(e(i)^T * Q^(-1) * e(i))
    // 其中：e(i) = [e_x,e_y,...]^T, Q维观测数据协方差矩阵，即sigma * sigma组成的协方差矩阵
    // 误差加权最小二次结果越小，说明观测数据精度越高
    // 那么，score = SUM((th - e(i)^T * Q^(-1) * e(i)))的分数就越高
    // 算法目标： 检查单应变换矩阵
    // 检查方式：通过H矩阵，进行参考帧和当前帧之间的双向投影，并计算起加权最小二乘投影误差

    // 算法流程
    // input: 单应性矩阵 H21, H12, 匹配点集 mvKeys1
    //    do:
    //        for p1(i), p2(i) in mvKeys:
    //           error_i1 = ||p2(i) - H21 * p1(i)||2
    //           error_i2 = ||p1(i) - H12 * p2(i)||2
    //           
    //           w1 = 1 / sigma / sigma
    //           w2 = 1 / sigma / sigma
    // 
    //           if error1 < th
    //              score +=   th - error_i1 * w1
    //           if error2 < th
    //              score +=   th - error_i2 * w2
    // 
    //           if error_1i > th or error_2i > th
    //              p1(i), p2(i) are inner points
    //              vbMatchesInliers(i) = true
    //           else 
    //              p1(i), p2(i) are outliers
    //              vbMatchesInliers(i) = false
    //           end
    //        end
    //   output: score, inliers

	// 特点匹配个数
    const int N = mvMatches12.size();

	// Step 1 获取从参考帧到当前帧的单应矩阵的各个元素
    const float h11 = H21.at<float>(0,0);
    const float h12 = H21.at<float>(0,1);
    const float h13 = H21.at<float>(0,2);
    const float h21 = H21.at<float>(1,0);
    const float h22 = H21.at<float>(1,1);
    const float h23 = H21.at<float>(1,2);
    const float h31 = H21.at<float>(2,0);
    const float h32 = H21.at<float>(2,1);
    const float h33 = H21.at<float>(2,2);

	// 获取从当前帧到参考帧的单应矩阵的各个元素
    const float h11inv = H12.at<float>(0,0);
    const float h12inv = H12.at<float>(0,1);
    const float h13inv = H12.at<float>(0,2);
    const float h21inv = H12.at<float>(1,0);
    const float h22inv = H12.at<float>(1,1);
    const float h23inv = H12.at<float>(1,2);
    const float h31inv = H12.at<float>(2,0);
    const float h32inv = H12.at<float>(2,1);
    const float h33inv = H12.at<float>(2,2);

	// 给特征点对的Inliers标记预分配空间
    vbMatchesInliers.resize(N);

	// 初始化score值
    float score = 0;

    // 基于卡方检验计算出的阈值（假设测量有一个像素的偏差）
	// 自由度为2的卡方分布，显著性水平为0.05，对应的临界阈值
    const float th = 5.991;

    //信息矩阵，方差平方的倒数
    const float invSigmaSquare = 1.0/(sigma * sigma);

    // Step 2 通过H矩阵，进行参考帧和当前帧之间的双向投影，并计算起加权重投影误差
    // H21 表示从img1 到 img2的变换矩阵
    // H12 表示从img2 到 img1的变换矩阵 
    for(int i = 0; i < N; i++)
    {
		// 一开始都默认为Inlier
        bool bIn = true;

		// Step 2.1 提取参考帧和当前帧之间的特征匹配点对
        const cv::KeyPoint &kp1 = mvKeys1[mvMatches12[i].first];
        const cv::KeyPoint &kp2 = mvKeys2[mvMatches12[i].second];
        const float u1 = kp1.pt.x;
        const float v1 = kp1.pt.y;
        const float u2 = kp2.pt.x;
        const float v2 = kp2.pt.y;

        // Step 2.2 计算 img2 到 img1 的重投影误差
        // x1 = H12*x2
        // 将图像2中的特征点通过单应变换投影到图像1中
        // |u1|   |h11inv h12inv h13inv||u2|   |u2in1|
        // |v1| = |h21inv h22inv h23inv||v2| = |v2in1| * w2in1inv
        // |1 |   |h31inv h32inv h33inv||1 |   |  1  |
		// 计算投影归一化坐标
        const float w2in1inv = 1.0/(h31inv * u2 + h32inv * v2 + h33inv);
        const float u2in1 = (h11inv * u2 + h12inv * v2 + h13inv) * w2in1inv;
        const float v2in1 = (h21inv * u2 + h22inv * v2 + h23inv) * w2in1inv;
   
        // 计算重投影误差 = ||p1(i) - H12 * p2(i)||2
        const float squareDist1 = (u1 - u2in1) * (u1 - u2in1) + (v1 - v2in1) * (v1 - v2in1);
        const float chiSquare1 = squareDist1 * invSigmaSquare;

        // Step 2.3 用阈值标记离群点，内点的话累加得分
        if(chiSquare1>th)
            bIn = false;    
        else
            // 误差越大，得分越低
            score += th - chiSquare1;

        // 计算从img1 到 img2 的投影变换误差
        // x1in2 = H21*x1
        // 将图像2中的特征点通过单应变换投影到图像1中
        // |u2|   |h11 h12 h13||u1|   |u1in2|
        // |v2| = |h21 h22 h23||v1| = |v1in2| * w1in2inv
        // |1 |   |h31 h32 h33||1 |   |  1  |
		// 计算投影归一化坐标
        const float w1in2inv = 1.0/(h31*u1+h32*v1+h33);
        const float u1in2 = (h11*u1+h12*v1+h13)*w1in2inv;
        const float v1in2 = (h21*u1+h22*v1+h23)*w1in2inv;

        // 计算重投影误差 
        const float squareDist2 = (u2-u1in2)*(u2-u1in2)+(v2-v1in2)*(v2-v1in2);
        const float chiSquare2 = squareDist2*invSigmaSquare;
 
        // 用阈值标记离群点，内点的话累加得分
        if(chiSquare2>th)
            bIn = false;
        else
            score += th - chiSquare2;   

        // Step 2.4 如果从img2 到 img1 和 从img1 到img2的重投影误差均满足要求，则说明是Inlier point
        if(bIn)
            vbMatchesInliers[i]=true;
        else
            vbMatchesInliers[i]=false;
    }
    return score;
}

这段代码的目的是通过计算双向投影误差，使用卡方检验的方法对给定的单应矩阵（Homography Matrix）进行打分。

函数参数说明

H21: 从参考帧到当前帧的单应矩阵。
H12: 从当前帧到参考帧的单应矩阵。
vbMatchesInliers: 匹配好的特征点对的Inliers标记，表示哪些特征点是内点。
sigma: 估计误差的方差，默认为1。

函数返回值

float: 返回单应矩阵的得分，得分越高表示矩阵越符合数据。

函数步骤

Step 1: 初始化

const int N = mvMatches12.size();

获取匹配点对的数量。

Step 2: 提取单应矩阵的元素

const float h11 = H21.at<float>(0,0);
const float h12 = H21.at<float>(0,1);
const float h13 = H21.at<float>(0,2);
const float h21 = H21.at<float>(1,0);
const float h22 = H21.at<float>(1,1);
const float h23 = H21.at<float>(1,2);
const float h31 = H21.at<float>(2,0);
const float h32 = H21.at<float>(2,1);
const float h33 = H21.at<float>(2,2);

const float h11inv = H12.at<float>(0,0);
const float h12inv = H12.at<float>(0,1);
const float h13inv = H12.at<float>(0,2);
const float h21inv = H12.at<float>(1,0);
const float h22inv = H12.at<float>(1,1);
const float h23inv = H12.at<float>(1,2);
const float h31inv = H12.at<float>(2,0);
const float h32inv = H12.at<float>(2,1);
const float h33inv = H12.at<float>(2,2);

提取单应矩阵 H21H_{21}H21 和 H12H_{12}H12 的元素。

Step 3: 初始化Inliers标记和得分

vbMatchesInliers.resize(N);
float score = 0;

为特征点对的Inliers标记预分配空间，并初始化得分为0。

Step 4: 设置卡方检验的阈值和方差的倒数

const float th = 5.991;
const float invSigmaSquare = 1.0/(sigma * sigma);

设置卡方检验的阈值，假设测量有一个像素的偏差。计算方差的倒数。

Step 5: 计算双向投影误差，并打分

for(int i = 0; i < N; i++) {
    bool bIn = true;
    const cv::KeyPoint &kp1 = mvKeys1[mvMatches12[i].first];
    const cv::KeyPoint &kp2 = mvKeys2[mvMatches12[i].second];
    const float u1 = kp1.pt.x;
    const float v1 = kp1.pt.y;
    const float u2 = kp2.pt.x;
    const float v2 = kp2.pt.y;

    // 计算从当前帧投影到参考帧的重投影误差
    const float w2in1inv = 1.0/(h31inv * u2 + h32inv * v2 + h33inv);
    const float u2in1 = (h11inv * u2 + h12inv * v2 + h13inv) * w2in1inv;
    const float v2in1 = (h21inv * u2 + h22inv * v2 + h23inv) * w2in1inv;
    const float squareDist1 = (u1 - u2in1) * (u1 - u2in1) + (v1 - v2in1) * (v1 - v2in1);
    const float chiSquare1 = squareDist1 * invSigmaSquare;

    if(chiSquare1 > th)
        bIn = false;
    else
        score += th - chiSquare1;

    // 计算从参考帧投影到当前帧的重投影误差
    const float w1in2inv = 1.0/(h31 * u1 + h32 * v1 + h33);
    const float u1in2 = (h11 * u1 + h12 * v1 + h13) * w1in2inv;
    const float v1in2 = (h21 * u1 + h22 * v1 + h23) * w1in2inv;
    const float squareDist2 = (u2 - u1in2) * (u2 - u1in2) + (v2 - v1in2) * (v2 - v1in2);
    const float chiSquare2 = squareDist2 * invSigmaSquare;

    if(chiSquare2 > th)
        bIn = false;
    else
        score += th - chiSquare2;

    // 根据误差判断是否为内点
    vbMatchesInliers[i] = bIn;
}

对每个特征点对，计算从当前帧到参考帧以及从参考帧到当前帧的双向重投影误差。通过卡方检验阈值判断是否为内点，累加得分。

详细解释

提取特征点对: 提取参考帧和当前帧之间的特征匹配点对，获取特征点的坐标。
计算从当前帧投影到参考帧的重投影误差: 使用单应矩阵 H12H_{12}H12 将当前帧的特征点投影到参考帧，计算投影点与实际点之间的距离（重投影误差）。
计算从参考帧投影到当前帧的重投影误差: 使用单应矩阵 H21H_{21}H21 将参考帧的特征点投影到当前帧，计算投影点与实际点之间的距离（重投影误差）。
计算卡方检验值: 计算重投影误差的卡方检验值，并与阈值进行比较。
累加得分和标记内点: 如果误差小于阈值，累加得分并将特征点对标记为内点；否则，标记为外点。

通过这种方式，函数能够有效地评估单应矩阵的准确性，并返回一个得分来衡量单应矩阵的质量。

其中

		// 计算投影归一化坐标
        const float w2in1inv = 1.0/(h31inv * u2 + h32inv * v2 + h33inv);
        const float u2in1 = (h11inv * u2 + h12inv * v2 + h13inv) * w2in1inv;
        const float v2in1 = (h21inv * u2 + h22inv * v2 + h23inv) * w2in1inv;

段代码的目的是将图像2中的特征点通过单应矩阵 $eq?H_%7B12%7D$ 投影到图像1中，计算归一化坐标，并最终得到在图像1中的投影坐标。为了更好地理解这段代码，我们可以一步一步地解释这些计算的目的和过程。

单应矩阵的作用

单应矩阵 $eq?H$ 描述的是一个平面到另一个平面的投影变换。对于图像中的点 $eq?%5Cleft%20%28%5Cmu%20%2C%5Cnu%20%5Cright%20%29$ ，在齐次坐标表示下，它可以通过矩阵乘法进行投影变换。具体来说，假设我们有一个点 $eq?%5Cleft%20%28%20%5Cmu%20_%7B2%7D%2C%20%5Cnu%20_%7B2%7D%5Cright%20%29$ 在图像2中，我们希望通过单应矩阵 $eq?H_%7B12%7D$ 将它投影到图像1中，得到投影点 $eq?%5Cleft%20%28%5Cmu%20_%7B2%7D%5E%7Bin1%7D%2C%20%5Cnu%20_%7B2%7D%5E%7Bin1%7D%20%5Cright%20%29$ 。

齐次坐标变换

在齐次坐标中，一个点 $eq?%5Cleft%20%28%5Cmu%20%2C%5Cnu%20%5Cright%20%29$ 可以表示为 $eq?%5Cleft%20%28%20%5Cmu%20%2C%5Cnu%20%2C1%20%5Cright%20%29$ 。单应矩阵 $eq?H_%7B12%7D$ 将这个点变换到新的坐标系中，其变换公式 $eq?H_%7B12%7D$ 如下：

$eq?%5Cbegin%7Bpmatrix%7D%20u%27%20%5C%5C%20v%27%20%5C%5C%20w%27%20%5Cend%7Bpmatrix%7D%20%3D%20%5Cbegin%7Bpmatrix%7D%20h_%7B11%7D%5E%7B%5Ctext%7Binv%7D%7D%20%26%20h_%7B12%7D%5E%7B%5Ctext%7Binv%7D%7D%20%26%20h_%7B13%7D%5E%7B%5Ctext%7Binv%7D%7D%20%5C%5C%20h_%7B21%7D%5E%7B%5Ctext%7Binv%7D%7D%20%26%20h_%7B22%7D%5E%7B%5Ctext%7Binv%7D%7D%20%26%20h_%7B23%7D%5E%7B%5Ctext%7Binv%7D%7D%20%5C%5C%20h_%7B31%7D%5E%7B%5Ctext%7Binv%7D%7D%20%26%20h_%7B32%7D%5E%7B%5Ctext%7Binv%7D%7D%20%26%20h_%7B33%7D%5E%7B%5Ctext%7Binv%7D%7D%20%5C%5C%20%5Cend%7Bpmatrix%7D%20%5Cbegin%7Bpmatrix%7D%20u%20%5C%5C%20v%20%5C%5C%201%20%5Cend%7Bpmatrix%7D$

经过矩阵乘法，我们得到

$eq?u%27%20%3D%20h_%7B11%7D%5E%7B%5Ctext%7Binv%7D%7D%20%5Ccdot%20u_2%20+%20h_%7B12%7D%5E%7B%5Ctext%7Binv%7D%7D%20%5Ccdot%20v_2%20+%20h_%7B13%7D%5E%7B%5Ctext%7Binv%7D%7D$

$eq?v%27%20%3D%20h_%7B21%7D%5E%7B%5Ctext%7Binv%7D%7D%20%5Ccdot%20u_2%20+%20h_%7B22%7D%5E%7B%5Ctext%7Binv%7D%7D%20%5Ccdot%20v_2%20+%20h_%7B23%7D%5E%7B%5Ctext%7Binv%7D%7D$

$eq?w%27%20%3D%20h_%7B31%7D%5E%7B%5Ctext%7Binv%7D%7D%20%5Ccdot%20u_2%20+%20h_%7B32%7D%5E%7B%5Ctext%7Binv%7D%7D%20%5Ccdot%20v_2%20+%20h_%7B33%7D%5E%7B%5Ctext%7Binv%7D%7D$

归一化过程

由于在齐次坐标系中，实际的二维坐标需要通过归一化得到，即用 $eq?%5Comega%20%5E%7B%27%7D$ 除以 $eq?%5C%5Cu%20%5E%7B%27%7D$ 和 $eq?%5C%5C%5Cupsilon%20%5E%7B%27%7D$ ：

$eq?u_%7B2%5Ctext%7Bin1%7D%7D%20%3D%20%5Cfrac%7Bu%27%7D%7Bw%27%7D%20%3D%20%5Cfrac%7Bh_%7B11%7D%5E%7B%5Ctext%7Binv%7D%7D%20%5Ccdot%20u_2%20+%20h_%7B12%7D%5E%7B%5Ctext%7Binv%7D%7D%20%5Ccdot%20v_2%20+%20h_%7B13%7D%5E%7B%5Ctext%7Binv%7D%7D%7D%7Bh_%7B31%7D%5E%7B%5Ctext%7Binv%7D%7D%20%5Ccdot%20u_2%20+%20h_%7B32%7D%5E%7B%5Ctext%7Binv%7D%7D%20%5Ccdot%20v_2%20+%20h_%7B33%7D%5E%7B%5Ctext%7Binv%7D%7D%7D$

$eq?v_%7B2%5Ctext%7Bin1%7D%7D%20%3D%20%5Cfrac%7Bv%27%7D%7Bw%27%7D%20%3D%20%5Cfrac%7Bh_%7B21%7D%5E%7B%5Ctext%7Binv%7D%7D%20%5Ccdot%20u_2%20+%20h_%7B22%7D%5E%7B%5Ctext%7Binv%7D%7D%20%5Ccdot%20v_2%20+%20h_%7B23%7D%5E%7B%5Ctext%7Binv%7D%7D%7D%7Bh_%7B31%7D%5E%7B%5Ctext%7Binv%7D%7D%20%5Ccdot%20u_2%20+%20h_%7B32%7D%5E%7B%5Ctext%7Binv%7D%7D%20%5Ccdot%20v_2%20+%20h_%7B33%7D%5E%7B%5Ctext%7Binv%7D%7D%7D$

为了实现这种归一化，我们首先计算 $eq?%5Comega%20%5E%7B%27%7D$ 的倒数 $eq?%5Comega%20_%7B2%7D%5E%7Binlinv%7D$ ：

$eq?w_%7B2%5Ctext%7Bin1inv%7D%7D%20%3D%20%5Cfrac%7B1%7D%7Bh_%7B31%7D%5E%7B%5Ctext%7Binv%7D%7D%20%5Ccdot%20u_2%20+%20h_%7B32%7D%5E%7B%5Ctext%7Binv%7D%7D%20%5Ccdot%20v_2%20+%20h_%7B33%7D%5E%7B%5Ctext%7Binv%7D%7D%7D$

代码中的计算步骤如下：

const float w2in1inv = 1.0 / (h31inv * u2 + h32inv * v2 + h33inv);
const float u2in1 = (h11inv * u2 + h12inv * v2 + h13inv) * w2in1inv;
const float v2in1 = (h21inv * u2 + h22inv * v2 + h23inv) * w2in1inv;

1.计算投影的第三个坐标 $eq?%5Comega%20%5E%7B%27%7D$ 的倒数：

2.计算归一化后的投影坐标：

这段代码通过计算单应矩阵变换后的齐次坐标，并进行归一化，得到图像2中的点在图像1中的对应点。这样可以实现两个图像平面之间的点映射，用于进一步的误差计算和特征匹配验证。

假设有两个图像，分别为图像1和图像2。点 $eq?%5Cleft%20%28%20%5Cmu%20_%7B1%7D%2C%20%5Cnu%20_%7B1%7D%5Cright%20%29$ 是图像1中的一个特征点，点 $eq?%5Cleft%20%28%20%5Cmu%20_%7B2%7D%2C%20%5Cnu%20_%7B2%7D%5Cright%20%29$ 是图像2中的对应特征点。单应矩阵 $eq?H_%7B12%7D$ 可以将图像2中的点投影到图像1中。

在之前的步骤中，已经计算了点 $eq?%5Cleft%20%28%20%5Cmu%20_%7B2%7D%2C%20%5Cnu%20_%7B2%7D%5Cright%20%29$ 在图像1中的投影位置 $eq?%5Cleft%20%28%5Cmu%20_%7B2%7D%5E%7Bin1%7D%2C%20%5Cnu%20_%7B2%7D%5E%7Bin1%7D%20%5Cright%20%29$ .

重投影误差

重投影误差的计算是为了衡量单应矩阵变换的准确性。具体来说，是计算原图像1中的点 $eq?%5Cleft%20%28%20%5Cmu%20_%7B1%7D%2C%20%5Cnu%20_%7B1%7D%5Cright%20%29$ 与投影到图像1中的点 $eq?%5Cleft%20%28%5Cmu%20_%7B2%7D%5E%7Bin1%7D%2C%20%5Cnu%20_%7B2%7D%5E%7Bin1%7D%20%5Cright%20%29$ .之间的欧氏距离。

// 计算重投影误差 = ||p1(i) - H12 * p2(i)||^2
const float squareDist1 = (u1 - u2in1) * (u1 - u2in1) + (v1 - v2in1) * (v1 - v2in1);

这一行代码计算的是图像1中的点 $eq?%5Cleft%20%28%20%5Cmu%20_%7B1%7D%2C%20%5Cnu%20_%7B1%7D%5Cright%20%29$ 与通过单应矩阵 $eq?H_%7B12%7D$ 投影到图像1中的点 $eq?%5Cleft%20%28%5Cmu%20_%7B2%7D%5E%7Bin1%7D%2C%20%5Cnu%20_%7B2%7D%5E%7Bin1%7D%20%5Cright%20%29$ .之间的平方欧氏距离。公式如下：

$eq?%5Ctext%7BsquareDist1%7D%20%3D%20%28u_1%20-%20u_%7B2%5Ctext%7Bin1%7D%7D%29%5E2%20+%20%28v_1%20-%20v_%7B2%5Ctext%7Bin1%7D%7D%29%5E2$

卡方误差

接下来，将这个平方欧氏距离转换为卡方误差：

const float chiSquare1 = squareDist1 * invSigmaSquare;

这里， $eq?%5Ctext%7BinvSigmaSquare%7D$ 是方差 $eq?%5Csigma$ 的平方的倒数：

$eq?%5Ctext%7BinvSigmaSquare%7D%20%3D%20%5Cfrac%7B1%7D%7B%7B%5Csigma%5E2%7D%7D$

因此， $eq?%5Cchi%20%5E%7B2%7D$ 误差的计算公式为：

$eq?%5Cchi%5E2%20%3D%20%5Ctext%7BsquareDist1%7D%20%5Ccdot%20%5Ctext%7BinvSigmaSquare%7D$

将平方欧氏距离 $eq?%5Ctext%7BsquareDist1%7D$ 乘以 $eq?%5Ctext%7BinvSigmaSquare%7D$ 后得到 $eq?%5Cchi%20%5E%7B2%7D$ 的误差衡量的是特征点 $eq?%5Cleft%20%28%20%5Cmu%20_%7B1%7D%2C%20%5Cnu%20_%7B1%7D%5Cright%20%29$ 与投影点 $eq?%5Cleft%20%28%5Cmu%20_%7B2%7D%5E%7Bin1%7D%2C%20%5Cnu%20_%7B2%7D%5E%7Bin1%7D%20%5Cright%20%29$ . 的加权距离。