描述

求单应矩阵
已知一个平面上的4个点（A、B、C、D），经过两个针孔相机分别成像得到4个点的图像坐标分别为

  • 相机1： (xA1, yA1)，(xB1, yB1), (xC1, yC1), (xD1, yD1)
  • 相机2： (xA2, yA2)，(xB2, yB2), (xC2, yC2), (xD2, yD2),
  • 计算相机1的图像到相机2图像的单应矩阵，并转换相机2图像中的点坐标到相机1图像坐标中

代码

#include <iostream>
#include <stdio.h>
#include <vector>
#include "Eigen/Dense"
#include "Eigen/Core"

// 单应矩阵为 相机2的点 = 单应矩阵 * 相机1的点
void calc_homography(int pts1[4][2], int pts2[4][2], int pts_in[][2], int pts_out[][2])
{
    // 4个点对带入公式求解
    // A * H' = B
    Eigen::MatrixXd A;
    A.resize(8, 8); 
    A<< pts1[0][0], pts1[0][1], 1, 0, 0, 0, -pts1[0][0]*pts2[0][0], -pts2[0][0]*pts1[0][1],
        0, 0, 0, pts1[0][0], pts1[0][1], 1, -pts1[0][0]*pts2[0][1], -pts1[0][1]*pts2[0][1],
        pts1[1][0], pts1[1][1], 1, 0, 0, 0, -pts1[1][0]*pts2[1][0], -pts2[1][0]*pts1[1][1],
        0, 0, 0, pts1[1][0], pts1[1][1], 1, -pts1[1][0]*pts2[1][1], -pts1[1][1]*pts2[1][1],
        pts1[2][0], pts1[2][1], 1, 0, 0, 0, -pts1[2][0]*pts2[2][0], -pts2[2][0]*pts1[2][1],
        0, 0, 0, pts1[2][0], pts1[2][1], 1, -pts1[2][0]*pts2[2][1], -pts1[2][1]*pts2[2][1],
        pts1[3][0], pts1[3][1], 1, 0, 0, 0, -pts1[3][0]*pts2[3][0], -pts2[3][0]*pts1[3][1],
        0, 0, 0, pts1[3][0], pts1[3][1], 1, -pts1[3][0]*pts2[3][1], -pts1[3][1]*pts2[3][1];
    // std::cout<<A<<std::endl;

    Eigen::MatrixXd B;
    B.resize(8, 1); 
    B<< pts2[0][0], pts2[0][1],
        pts2[1][0], pts2[1][1],
        pts2[2][0], pts2[2][1],
        pts2[3][0], pts2[3][1];
    // std::cout<<B<<std::endl;

    Eigen::MatrixXd H;
    H = A.inverse() * B;
    // std::cout<<H<<std::endl;
    // std::cout<<A*H<<std::endl;   

    Eigen::MatrixXd H_result;
    H_result.resize(3,3);
    H_result<< H(0,0), H(1,0), H(2,0),
               H(3,0), H(4,0), H(5,0),
               H(6,0), H(7,0), 1;
    std::cout<<" 单应矩阵H（满足相机2的点 = H * 相机1的点）"<<std::endl;
    std::cout<<" homography = "<<std::endl;
    std::cout<<H_result<<std::endl;

    // 可以用这段话来验证单应矩阵是否正确
    // for (int i = 0 ; i < 4; i++){
    //     Eigen::MatrixXd p1;
    //     p1.resize(3,1);
    //     p1<< pts1[i][0], pts1[i][1], 1;

    //     Eigen::MatrixXd p2;
    //     p2.resize(3,1);
    //     p2<< pts2[i][0], pts2[i][1], 1;

    //     Eigen::MatrixXd p_1_to2;
    //     p_1_to2 = H_result * p1;
    //     std::cout<<"   "<<i<<std::endl;
    //     std::cout<<p_1_to2(0, 0)/ p_1_to2(2, 0)<<std::endl;
    //     std::cout<<p_1_to2(1, 0)/ p_1_to2(2, 0)<<std::endl;
    //     std::cout<<p_1_to2(2, 0)/ p_1_to2(2, 0)<<std::endl;
    //     std::cout<<p2<<std::endl;
    // }

    // 相机2的点转换到相机1下，应该是
    // 单应矩阵的逆 * 相机2的点 ，并且归一化为齐次坐标
    for (int i = 0 ; i < 4; i++){
        Eigen::MatrixXd pp2;
        pp2.resize(3,1);
        pp2<< pts_in[i][0], pts_in[i][1], 1;


        Eigen::MatrixXd output;
        output = H_result.inverse() * pp2;
        pts_out[i][0] = output(0,0)/output(2,0);
        pts_out[i][1] = output(1,0)/output(2,0);


        printf("相机2的点(%d, %d) 转换到相机1的点是 (%d, %d)\n", 
            pts_in[i][0], pts_in[i][1], pts_out[i][0], pts_out[i][1]);
    }

}

int main(int argc, char **argv)
{
    int pts1[4][2] = {
        {20, 45},
        {30, 600},
        {560, 650},
        {550, 30}
    };
    int pts2[4][2] = {
        {10, 15},
        {400, 560},
        {450, 600},
        {560, 20}
    };

    int pts2_in[][2] = {
        {15, 25},
        {35, 67},
        {100, 200},
        {300, 560}
    };
    int pts1_out[4][2];
    calc_homography(pts1, pts2, pts2_in, pts1_out);
    return 0;
}

代码输出

 单应矩阵H（满足相机2的点 = H * 相机1的点）
 homography = 
    1.83051     21.3757    -967.324
   0.805292      29.952    -1317.16
-0.00223071   0.0480823           1
相机2的点(15, 25) 转换到相机1的点是 (14, 46)
相机2的点(35, 67) 转换到相机1的点是 (-14, 52)
相机2的点(100, 200) 转换到相机1的点是 (-163, 84)
相机2的点(300, 560) 转换到相机1的点是 (1371, -310)