• 对极几何与三角化求3D空间坐标

    一，使用对极几何约束求R,T

    第一步：特征匹配。提取出有效的匹配点

    1. void find_feature_matches(const Mat &img_1, const Mat &img_2,
    2.                           std::vector &keypoints_1,
    3.                           std::vector &keypoints_2,
    4.                           std::vector &matches) {
    5.   //-- 初始化
    6.   Mat descriptors_1, descriptors_2;
    7.   // used in OpenCV3
    8.   Ptr detector = ORB::create();
    9.   Ptr descriptor = ORB::create();
    10.   // use this if you are in OpenCV2
    11.   // Ptr detector = FeatureDetector::create ( "ORB" );
    12.   // Ptr descriptor = DescriptorExtractor::create ( "ORB" );
    13.   Ptr matcher = DescriptorMatcher::create("BruteForce-Hamming");
    14.   //-- 第一步:检测 Oriented FAST 角点位置
    15.   detector->detect(img_1, keypoints_1);
    16.   detector->detect(img_2, keypoints_2);
    17.  
    18.   //-- 第二步:根据角点位置计算 BRIEF 描述子
    19.   descriptor->compute(img_1, keypoints_1, descriptors_1);
    20.   descriptor->compute(img_2, keypoints_2, descriptors_2);
    21.  
    22.   //-- 第三步:对两幅图像中的BRIEF描述子进行匹配，使用 Hamming 距离
    23.   vector match;
    24.   // BFMatcher matcher ( NORM_HAMMING );
    25.   matcher->match(descriptors_1, descriptors_2, match);
    26.  
    27.   //-- 第四步:匹配点对筛选
    28.   double min_dist = 10000, max_dist = 0;
    29.  
    30.   //找出所有匹配之间的最小距离和最大距离, 即是最相似的和最不相似的两组点之间的距离
    31.   for (int i = 0; i < descriptors_1.rows; i++) {
    32.     double dist = match[i].distance;
    33.     if (dist < min_dist) min_dist = dist;
    34.     if (dist > max_dist) max_dist = dist;
    35.   }
    36.  
    37.   printf("-- Max dist : %f \n", max_dist);
    38.   printf("-- Min dist : %f \n", min_dist);
    39.  
    40.   //当描述子之间的距离大于两倍的最小距离时,即认为匹配有误.但有时候最小距离会非常小,设置一个经验值30作为下限.
    41.   for (int i = 0; i < descriptors_1.rows; i++) {
    42.     if (match[i].distance <= max(2 * min_dist, 30.0)) {
    43.       matches.push_back(match[i]);
    44.     }
    45.   }
    46. }

    二、使用本质矩阵求解R，T

    第二步：根据匹配点对，依据对极几何约束原理，求相机运动的R,t

    1. void pose_estimation_2d2d(
    2.   const std::vector &keypoints_1,
    3.   const std::vector &keypoints_2,
    4.   const std::vector &matches,
    5.   Mat &R, Mat &t) {
    6.   // 相机内参,TUM Freiburg2
    7.   Mat K = (Mat_<double>(3, 3) << 520.9, 0, 325.1, 0, 521.0, 249.7, 0, 0, 1);
    8.  
    9.   //-- 把匹配点转换为vector的形式
    10.   vector points1;
    11.   vector points2;
    12.  
    13.   for (int i = 0; i < (int) matches.size(); i++) {
    14.     points1.push_back(keypoints_1[matches[i].queryIdx].pt);
    15.     points2.push_back(keypoints_2[matches[i].trainIdx].pt);
    16.   }
    17.  
    18.   //-- 计算本质矩阵
    19.   Point2d principal_point(325.1, 249.7);        //相机主点, TUM dataset标定值
    20.   int focal_length = 521;            //相机焦距, TUM dataset标定值
    21.   Mat essential_matrix;
    22.   essential_matrix = findEssentialMat(points1, points2, focal_length, principal_point);
    23.  
    24.   //-- 从本质矩阵中恢复旋转和平移信息.
    25.   recoverPose(essential_matrix, points1, points2, R, t, focal_length, principal_point);
    26. }

    三、由R，T三角化空间坐标

    第三步：根据针孔相机模型的公式，由 R,t估计特征点的空间坐标

    1. //三角化,根据匹配点和求解到的三维点。存储在points中
    2. void triangulation(
    3.   const vector &keypoint_1,
    4.   const vector &keypoint_2,
    5.   const std::vector &matches,
    6.   const Mat &R, const Mat &t,
    7.   vector &points) {
    8.   Mat T1 = (Mat_<float>(3, 4) <<
    9.     1, 0, 0, 0,
    10.     0, 1, 0, 0,
    11.     0, 0, 1, 0);
    12.     //根据求解到的RT构造T2矩阵
    13.   Mat T2 = (Mat_<float>(3, 4) <<
    14.     R.at<double>(0, 0), R.at<double>(0, 1), R.at<double>(0, 2), t.at<double>(0, 0),
    15.     R.at<double>(1, 0), R.at<double>(1, 1), R.at<double>(1, 2), t.at<double>(1, 0),
    16.     R.at<double>(2, 0), R.at<double>(2, 1), R.at<double>(2, 2), t.at<double>(2, 0)
    17.   );
    18.   //相机内参
    19.   Mat K = (Mat_<double>(3, 3) << 520.9, 0, 325.1, 0, 521.0, 249.7, 0, 0, 1);
    20.   vector pts_1, pts_2;
    21.   for (DMatch m:matches) {
    22.     // 将像素坐标转换至相机坐标
    23.     pts_1.push_back(pixel2cam(keypoint_1[m.queryIdx].pt, K));
    24.     pts_2.push_back(pixel2cam(keypoint_2[m.trainIdx].pt, K));
    25.   }
    26.  
    27.   Mat pts_4d;
    28.   cv::triangulatePoints(T1, T2, pts_1, pts_2, pts_4d);
    29.  
    30.   // 转换成非齐次坐标
    31.   for (int i = 0; i < pts_4d.cols; i++) {
    32.     Mat x = pts_4d.col(i);
    33.     x /= x.at<float>(3, 0); // 归一化
    34.     Point3d p(
    35.       x.at<float>(0, 0),
    36.       x.at<float>(1, 0),
    37.       x.at<float>(2, 0)
    38.     );
    39.     points.push_back(p);
    40.   }
    41. }

    其中 triangulatePoints()的具体用法为

    1. triangulatePoints(T1, T2, left, right, points_final) ;
    2.  
    3. Mat T1 = (Mat_<float>(3, 4) <<
    4. 		1, 0, 0, 0,
    5. 		0, 1, 0, 0,
    6. 		0, 0, 1, 0);
    7. Mat T2 = (Mat_<float>(3, 4) <<
    8. 		R.at<double>(0, 0), R.at<double>(0, 1), R.at<double>(0, 2), T.at<double>(0, 0),
    9. 		R.at<double>(1, 0), R.at<double>(1, 1), R.at<double>(1, 2), T.at<double>(1, 0),
    10. 		R.at<double>(2, 0), R.at<double>(2, 1), R.at<double>(2, 2), T.at<double>(2, 0)
    11. 		);`
    12. triangulatePoints(T1, T2, left, right, points_final) ;
    13.  
    14. 其中T2为3x4的[R|T]矩阵，left、right为相机坐标系下的归一化坐标，
    15. 因此不能直接使用提取到的像素坐标。应首先将像素坐标通过相机内参转化到相机坐标系下。

    所以通过函数pixel2cam可将像素坐标转换到归一化相机坐标系下
    归一化坐标：X=(u-u0)/fx

    1. //像素坐标到归一化平面相机坐标的转换
    2. Point2f pixel2cam(const Point2f& p, const Mat& K)
    3. {
    4.     return Point2f
    5.     (
    6.         (p.x - K.at<double>(0, 2)) / K.at<double>(0, 0),
    7.         (p.y - K.at<double>(1, 2)) / K.at<double>(1, 1)
    8.     );
    9. }

    四、代码demo

    总的代码为：

    1. #include 
    2. #include 
    3. // #include "extra.h" // used in opencv2
    4. using namespace std;
    5. using namespace cv;
    6.  
    7. void find_feature_matches(
    8.   const Mat &img_1, const Mat &img_2,
    9.   std::vector &keypoints_1,
    10.   std::vector &keypoints_2,
    11.   std::vector &matches);
    12.  
    13. void pose_estimation_2d2d(
    14.   const std::vector &keypoints_1,
    15.   const std::vector &keypoints_2,
    16.   const std::vector &matches,
    17.   Mat &R, Mat &t);
    18.  
    19. void triangulation(
    20.   const vector &keypoint_1,
    21.   const vector &keypoint_2,
    22.   const std::vector &matches,
    23.   const Mat &R, const Mat &t,
    24.   vector &points
    25. );
    26.  
    27. /// 作图用
    28. inline cv::Scalar get_color(float depth) {
    29.   float up_th = 50, low_th = 10, th_range = up_th - low_th;
    30.   if (depth > up_th) depth = up_th;
    31.   if (depth < low_th) depth = low_th;
    32.   return cv::Scalar(255 * depth / th_range, 0, 255 * (1 - depth / th_range));
    33. }
    34.  
    35. // 像素坐标转相机归一化坐标
    36. Point2f pixel2cam(const Point2d &p, const Mat &K);
    37.  
    38. int main(int argc, char **argv) {
    39.   if (argc != 3) {
    40.     cout << "usage: triangulation img1 img2" << endl;
    41.     return 1;
    42.   }
    43.   //-- 读取图像
    44.   Mat img_1 = imread(argv[1], CV_LOAD_IMAGE_COLOR);
    45.   Mat img_2 = imread(argv[2], CV_LOAD_IMAGE_COLOR);
    46.  
    47.   vector keypoints_1, keypoints_2;
    48.   vector matches;
    49.   find_feature_matches(img_1, img_2, keypoints_1, keypoints_2, matches);
    50.   cout << "一共找到了" << matches.size() << "组匹配点" << endl;
    51.  
    52.   //-- 估计两张图像间运动
    53.   Mat R, t;
    54.   pose_estimation_2d2d(keypoints_1, keypoints_2, matches, R, t);
    55.  
    56.   //-- 三角化
    57.   vector points;
    58.   //tr是三维点
    59.   triangulation(keypoints_1, keypoints_2, matches, R, t, tr);
    60.  
    61.   //-- 验证三角化点与特征点的重投影关系
    62.   Mat K = (Mat_<double>(3, 3) << 520.9, 0, 325.1, 0, 521.0, 249.7, 0, 0, 1);
    63.   Mat img1_plot = img_1.clone();
    64.   Mat img2_plot = img_2.clone();
    65.   for (int i = 0; i < matches.size(); i++) {
    66.     // 第一个图
    67.     float depth1 = points[i].z;
    68.     cout << "depth: " << depth1 << endl;
    69.     Point2d pt1_cam = pixel2cam(keypoints_1[matches[i].queryIdx].pt, K);
    70.     cv::circle(img1_plot, keypoints_1[matches[i].queryIdx].pt, 2, get_color(depth1), 2);
    71.  
    72.     // 第二个图
    73.     Mat pt2_trans = R * (Mat_<double>(3, 1) << points[i].x, points[i].y, points[i].z) + t;
    74.     float depth2 = pt2_trans.at<double>(2, 0);
    75.     cv::circle(img2_plot, keypoints_2[matches[i].trainIdx].pt, 2, get_color(depth2), 2);
    76.   }
    77.   cv::imshow("img 1", img1_plot);
    78.   cv::imshow("img 2", img2_plot);
    79.   cv::waitKey();
    80.  
    81.   return 0;
    82. }
    83.  
    84. void find_feature_matches(const Mat &img_1, const Mat &img_2,
    85.                           std::vector &keypoints_1,
    86.                           std::vector &keypoints_2,
    87.                           std::vector &matches) {
    88.   //-- 初始化
    89.   Mat descriptors_1, descriptors_2;
    90.   // used in OpenCV3
    91.   Ptr detector = ORB::create();
    92.   Ptr descriptor = ORB::create();
    93.   // use this if you are in OpenCV2
    94.   // Ptr detector = FeatureDetector::create ( "ORB" );
    95.   // Ptr descriptor = DescriptorExtractor::create ( "ORB" );
    96.   Ptr matcher = DescriptorMatcher::create("BruteForce-Hamming");
    97.   //-- 第一步:检测 Oriented FAST 角点位置
    98.   detector->detect(img_1, keypoints_1);
    99.   detector->detect(img_2, keypoints_2);
    100.  
    101.   //-- 第二步:根据角点位置计算 BRIEF 描述子
    102.   descriptor->compute(img_1, keypoints_1, descriptors_1);
    103.   descriptor->compute(img_2, keypoints_2, descriptors_2);
    104.  
    105.   //-- 第三步:对两幅图像中的BRIEF描述子进行匹配，使用 Hamming 距离
    106.   vector match;
    107.   // BFMatcher matcher ( NORM_HAMMING );
    108.   matcher->match(descriptors_1, descriptors_2, match);
    109.  
    110.   //-- 第四步:匹配点对筛选
    111.   double min_dist = 10000, max_dist = 0;
    112.  
    113.   //找出所有匹配之间的最小距离和最大距离, 即是最相似的和最不相似的两组点之间的距离
    114.   for (int i = 0; i < descriptors_1.rows; i++) {
    115.     double dist = match[i].distance;
    116.     if (dist < min_dist) min_dist = dist;
    117.     if (dist > max_dist) max_dist = dist;
    118.   }
    119.  
    120.   printf("-- Max dist : %f \n", max_dist);
    121.   printf("-- Min dist : %f \n", min_dist);
    122.  
    123.   //当描述子之间的距离大于两倍的最小距离时,即认为匹配有误.但有时候最小距离会非常小,设置一个经验值30作为下限.
    124.   for (int i = 0; i < descriptors_1.rows; i++) {
    125.     if (match[i].distance <= max(2 * min_dist, 30.0)) {
    126.       matches.push_back(match[i]);
    127.     }
    128.   }
    129. }
    130.  
    131. void pose_estimation_2d2d(
    132.   const std::vector &keypoints_1,
    133.   const std::vector &keypoints_2,
    134.   const std::vector &matches,
    135.   Mat &R, Mat &t) {
    136.   // 相机内参,TUM Freiburg2
    137.   Mat K = (Mat_<double>(3, 3) << 520.9, 0, 325.1, 0, 521.0, 249.7, 0, 0, 1);
    138.  
    139.   //-- 把匹配点转换为vector的形式
    140.   vector points1;
    141.   vector points2;
    142.  
    143.   for (int i = 0; i < (int) matches.size(); i++) {
    144.     points1.push_back(keypoints_1[matches[i].queryIdx].pt);
    145.     points2.push_back(keypoints_2[matches[i].trainIdx].pt);
    146.   }
    147.  
    148.   //-- 计算本质矩阵
    149.   Point2d principal_point(325.1, 249.7);        //相机主点, TUM dataset标定值
    150.   int focal_length = 521;            //相机焦距, TUM dataset标定值
    151.   Mat essential_matrix;
    152.   essential_matrix = findEssentialMat(points1, points2, focal_length, principal_point);
    153.  
    154.   //-- 从本质矩阵中恢复旋转和平移信息.
    155.   recoverPose(essential_matrix, points1, points2, R, t, focal_length, principal_point);
    156. }
    157.  
    158. //三角化,根据匹配点和求解到的三维点。存储在points中
    159. void triangulation(
    160.   const vector &keypoint_1,
    161.   const vector &keypoint_2,
    162.   const std::vector &matches,
    163.   const Mat &R, const Mat &t,
    164.   vector &points) {
    165.   Mat T1 = (Mat_<float>(3, 4) <<
    166.     1, 0, 0, 0,
    167.     0, 1, 0, 0,
    168.     0, 0, 1, 0);
    169.     //根据求解到的RT构造T2矩阵
    170.   Mat T2 = (Mat_<float>(3, 4) <<
    171.     R.at<double>(0, 0), R.at<double>(0, 1), R.at<double>(0, 2), t.at<double>(0, 0),
    172.     R.at<double>(1, 0), R.at<double>(1, 1), R.at<double>(1, 2), t.at<double>(1, 0),
    173.     R.at<double>(2, 0), R.at<double>(2, 1), R.at<double>(2, 2), t.at<double>(2, 0)
    174.   );
    175.   //相机内参
    176.   Mat K = (Mat_<double>(3, 3) << 520.9, 0, 325.1, 0, 521.0, 249.7, 0, 0, 1);
    177.   vector pts_1, pts_2;
    178.   for (DMatch m:matches) {
    179.     // 将像素坐标转换至相机坐标
    180.     pts_1.push_back(pixel2cam(keypoint_1[m.queryIdx].pt, K));
    181.     pts_2.push_back(pixel2cam(keypoint_2[m.trainIdx].pt, K));
    182.   }
    183.  
    184.   Mat pts_4d;
    185.   cv::triangulatePoints(T1, T2, pts_1, pts_2, pts_4d);
    186.  
    187.   // 转换成非齐次坐标
    188.   for (int i = 0; i < pts_4d.cols; i++) {
    189.     Mat x = pts_4d.col(i);
    190.     x /= x.at<float>(3, 0); // 归一化
    191.     Point3d p(
    192.       x.at<float>(0, 0),
    193.       x.at<float>(1, 0),
    194.       x.at<float>(2, 0)
    195.     );
    196.     points.push_back(p);
    197.   }
    198. }
    199.  
    200. Point2f pixel2cam(const Point2d &p, const Mat &K) {
    201.   return Point2f
    202.     (
    203.       (p.x - K.at<double>(0, 2)) / K.at<double>(0, 0),
    204.       (p.y - K.at<double>(1, 2)) / K.at<double>(1, 1)
    205.     );
    206. }

  • 相关阅读:
    Numpy学习
    k8s-实战——kubeadm二进制编译
    第13章 网络安全漏洞防护技术原理与应用
    vue项目中使用antvX6新手教程，附demo案例讲解（可拖拽流程图、网络拓扑图）
    uniApp监听左右滑动事件
    新库上线 | CnOpenData文化、体育和娱乐业工商注册企业基本信息数据
    Python学习之CSDN21天学习挑战赛计划之11
    ModStartBlog v6.1.0 界面显示优化，富文本升级
    Python 页面解析：Beautiful Soup库的使用
    助你成为专业终端人，阿里巴巴第三届终端练习生计划开启报名！
  • 原文地址：https://blog.csdn.net/xiao__run/article/details/132736625