这个章节主要讲解:

图像特征提取

多幅图像匹配特征点;

对极几何;

PNP问题;

ICP问题;

三角化原理;


一. 特征点提取和匹配

工程实践需要你事先安装了opencv3; 由于opencv3中提供了由本质矩阵E 恢复R,t的接口。

opencv2和opencv3提取特征点时有些语法写法不太一样~

CLion打开slambook2-master的ch7工程;

其中旧的slambook的ch7中有如下的文件:

而slambook2中变成如下的了:

 

其中 feature_extraction.cpp 变成了orb_cv.cpp

本文针对orb_cv.cpp做实践;

其代码内容如下:

#include <iostream>
#include <opencv2/core/core.hpp>
#include <opencv2/features2d/features2d.hpp>   //特征提取必须的库
#include <opencv2/highgui/highgui.hpp>
#include <chrono>                               //计时用
 
using namespace std;
using namespace cv;
 
int main(int argc, char **argv) {
  if (argc != 3) {
    cout << "usage: feature_extraction img1 img2" << endl;
    return 1;
  }
  //-- 读取图像
  Mat img_1 = imread(argv[1], CV_LOAD_IMAGE_COLOR);   //加载彩色图像,在终端传参
  Mat img_2 = imread(argv[2], CV_LOAD_IMAGE_COLOR);
  assert(img_1.data != nullptr && img_2.data != nullptr);  //确保图像正确读取
 
  //-- 初始化
  std::vector<KeyPoint> keypoints_1, keypoints_2;     //两张图像的特征点向量
  Mat descriptors_1, descriptors_2;                   //特征描述器
  Ptr<FeatureDetector> detector = ORB::create();         //构建ORB特征检测器
  Ptr<DescriptorExtractor> descriptor = ORB::create();   //构建ORB特征描述器
  Ptr<DescriptorMatcher> matcher = DescriptorMatcher::create("BruteForce-Hamming");  //创建匹配,选择暴力匹配,计算描述器的汉明距离,ORB无法用欧式距离计算
 
  //-- 第一步:检测 Oriented FAST 角点位置
  chrono::steady_clock::time_point t1 = chrono::steady_clock::now();
  detector->detect(img_1, keypoints_1);    //提取图1的关键点
  detector->detect(img_2, keypoints_2);    //提取图2的关键点
 
  //-- 第二步:根据角点位置计算 BRIEF 描述子
  descriptor->compute(img_1, keypoints_1, descriptors_1);  //计算描述子
  descriptor->compute(img_2, keypoints_2, descriptors_2);
  chrono::steady_clock::time_point t2 = chrono::steady_clock::now();
  chrono::duration<double> time_used = chrono::duration_cast<chrono::duration<double>>(t2 - t1);
  cout << "extract ORB cost = " << time_used.count() << " seconds. " << endl;
 
  Mat outimg1;
  drawKeypoints(img_1, keypoints_1, outimg1, Scalar::all(-1), DrawMatchesFlags::DEFAULT);
  imshow("ORB features", outimg1);    //显示图1提取的ORB特征
 
  //-- 第三步:对两幅图像中的BRIEF描述子进行匹配,使用 Hamming 距离
  vector<DMatch> matches;
  t1 = chrono::steady_clock::now();
  matcher->match(descriptors_1, descriptors_2, matches);  //进行匹配,饭后DMatch结构的匹配结果,该结构中包含图1,2的关键点ID,两者的汉明距离
  t2 = chrono::steady_clock::now();
  time_used = chrono::duration_cast<chrono::duration<double>>(t2 - t1);
  cout << "match ORB cost = " << time_used.count() << " seconds. " << endl;
 
  //-- 第四步:匹配点对筛选
  // 计算最小距离和最大距离
  auto min_max = minmax_element(matches.begin(), matches.end(),
                                [](const DMatch &m1, const DMatch &m2) { return m1.distance < m2.distance; });
  double min_dist = min_max.first->distance;
  double max_dist = min_max.second->distance;
 
  printf("-- Max dist : %f \n", max_dist);
  printf("-- Min dist : %f \n", min_dist);
 
  //当描述子之间的距离大于两倍的最小距离时,即认为匹配有误.但有时候最小距离会非常小,设置一个经验值30作为下限.
  std::vector<DMatch> good_matches;
  for (int i = 0; i < descriptors_1.rows; i++) {
    if (matches[i].distance <= max(2 * min_dist, 30.0)) {
      good_matches.push_back(matches[i]);   //选取好的匹配结果
    }
  }
 
  //-- 第五步:绘制匹配结果
  Mat img_match;     //暴力匹配的结果
  Mat img_goodmatch; //刷选后匹配的结果
  drawMatches(img_1, keypoints_1, img_2, keypoints_2, matches, img_match);
  drawMatches(img_1, keypoints_1, img_2, keypoints_2, good_matches, img_goodmatch);
  imshow("all matches", img_match);
  imshow("good matches", img_goodmatch);
  waitKey(0);
 
  return 0;
}

针对上面这个cpp,自己写个CMakeLists.txt (由于提供的CMakeLists.txt是针对ch7下面所有的cpp的),这个代码主要是用到了OpenCV3  内容如下:

cmake_minimum_required(VERSION 2.8)
project(orb_cv)  #我将其重命名了下
 
set(CMAKE_BUILD_TYPE "Release")
add_definitions("-DENABLE_SSE")
set(CMAKE_CXX_FLAGS "-std=c++11 -O2 ${SSE_FLAGS} -msse4")
#list(APPEND CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake)
 
find_package(OpenCV 3 REQUIRED)
#find_package(G2O REQUIRED)
#find_package(Sophus REQUIRED)
 
include_directories(
        ${OpenCV_INCLUDE_DIRS}
)
 
add_executable(orb_cv orb_cv.cpp)
target_link_libraries(orb_cv ${OpenCV_LIBS})

然后就是编译了:

//新建了个文件夹,命名为feature-extracture
//将上面新建的CMakeLists.txt和orb_cv.cpp拷贝进来,切到这个目录下
mkdir build
cd build
cmake ..
make

得到可执行文件orb_cv,我将图像1.png和2.png拷贝到这build下,终端运行:

./orb_cv 1.png 2.png

显示ORB提取特征花费时间2.46495秒;

匹配时间8.00450712秒;

暴力匹配结果如下(每个特征点都会寻找匹配关系):

匹配汉明距离最大95,最小7; 

刷选匹配结果如下:

 特征点之间的匹配关系可为计算相机姿态提供必要信息。


特征匹配后得到了特征点之间的对应关系

如果只有两个单目图像,得到了2D-2D间的关系 ——对极几何

如果匹配的是帧和地图,得到3D-2D间的关系 —— PnP

如果匹配的是RGB-D图,得到3D-3D间的关系 —— ICP

二.对极几何

概念部分请参考博客:视觉里程计之对极几何

引用博主的一幅图

两个图像中的p1和p2可以通过特征匹配得到,P未知,e1和e2未知,T12(相机2到相机1的变换)待求;

对极约束为:

本质矩阵E(Essential Matrix)

基础矩阵F(Fundamental Matrix) 

 相机位姿估计问题变为如下两步:

  1. 根据配对点的像素位置求出E或者F;
  2. 根据E或者F求出R,t

E与F只相差内参,由于内参通常为已知的,所以往往使用更简单的E。用经典的8点法求E;

从E计算R,t :用奇异值分解;

 

2D-2D情况下,只知道图像坐标之间的对应关系

  • 当特征点在平面上时(例如俯视或仰视),使用H恢复R,t;
  • 否则,使用E或者F恢复R,t;

求得R,t后:

  • 利用三角化计算特征点的3D位置(即深度)

 

针对ch7中的pose_estimation_2d2d.cpp 代码进行分析;

在ch7中新建个文件夹叫pose-estimation2d2d,然后将pose_estimation_2d2d.cpp 和 CMakeLists.txt 拷贝进来,其中CMakeLists.txt内容修改为:

cmake_minimum_required(VERSION 2.8)
project(pose_estimation_2d2d)
 
set(CMAKE_BUILD_TYPE "Release")
add_definitions("-DENABLE_SSE")
set(CMAKE_CXX_FLAGS "-std=c++11 -O2 ${SSE_FLAGS} -msse4")
#list(APPEND CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake)
 
find_package(OpenCV 3 REQUIRED)
#find_package(G2O REQUIRED)
#find_package(Sophus REQUIRED)
 
include_directories(
        ${OpenCV_INCLUDE_DIRS}
)
 
# add_executable( pose_estimation_2d2d pose_estimation_2d2d.cpp extra.cpp ) # use this if in OpenCV2 
add_executable(pose_estimation_2d2d pose_estimation_2d2d.cpp)
target_link_libraries(pose_estimation_2d2d ${OpenCV_LIBS})

终端切到这个pose-estimation2d2d目录下,编译:

mkdir build
cd build
cmake ..
make -j2

 将图像1.png和2.png拷贝到build中,执行:

./pose_estimation_2d2d 1.png 2.png 

输出结果如下:

匹配汉明距离最小7,最大95;

找到81组匹配点,调用opencv3的接口,计算得到基础矩阵F,本质矩阵E, 单应矩阵H,相机旋转矩阵R,平移向量t(已经长度归一化为1了),以及打印81组匹配点之间的对极约束值(理论上应该是0的)

 

 pose_estimation_2d2d.cpp源码分析如下:

#include <iostream>
#include <opencv2/core/core.hpp>
#include <opencv2/features2d/features2d.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/calib3d/calib3d.hpp>
// #include "extra.h" // use this if in OpenCV2
 
using namespace std;
using namespace cv;
 
/****************************************************
 * 本程序演示了如何使用2D-2D的特征匹配估计相机运动
 * **************************************************/
 
void find_feature_matches(              //特征匹配封装成了一个函数
  const Mat &img_1, const Mat &img_2,   //输入两幅图像
  std::vector<KeyPoint> &keypoints_1,  //特征点向量
  std::vector<KeyPoint> &keypoints_2,
  std::vector<DMatch> &matches);      //匹配向量
 
void pose_estimation_2d2d(             //位姿估计封装成一个函数
  std::vector<KeyPoint> keypoints_1,
  std::vector<KeyPoint> keypoints_2,
  std::vector<DMatch> matches,
  Mat &R, Mat &t);
 
// 像素坐标转相机归一化坐标
Point2d pixel2cam(const Point2d &p, const Mat &K);   //像素坐标转为归一化的相机坐标Zc=1
 
int main(int argc, char **argv) {
  if (argc != 3) {  //需要传入两幅图像
    cout << "usage: pose_estimation_2d2d img1 img2" << endl;
    return 1;
  }
  //-- 读取图像
  Mat img_1 = imread(argv[1], CV_LOAD_IMAGE_COLOR); //读取彩色图
  Mat img_2 = imread(argv[2], CV_LOAD_IMAGE_COLOR);
  assert(img_1.data && img_2.data && "Can not load images!");  //确保读到图像
 
  vector<KeyPoint> keypoints_1, keypoints_2;   
  vector<DMatch> matches;
  find_feature_matches(img_1, img_2, keypoints_1, keypoints_2, matches);  //寻找两幅图像的特征点和特征点匹配
  cout << "一共找到了" << matches.size() << "组匹配点" << endl;   //打印匹配结果
 
  //-- 估计两张图像间运动
  Mat R, t;  //(图像2相对图像1的运动)
  pose_estimation_2d2d(keypoints_1, keypoints_2, matches, R, t);  //估计相机运动,通过2D-2D点恢复相机旋转与平移R,t
 
  //-- 验证E=t^R*scale
  Mat t_x =
    (Mat_<double>(3, 3) << 0, -t.at<double>(2, 0), t.at<double>(1, 0),
      t.at<double>(2, 0), 0, -t.at<double>(0, 0),
      -t.at<double>(1, 0), t.at<double>(0, 0), 0);
 
  cout << "t^R=" << endl << t_x * R << endl;    //输出平移向量和旋转矩阵的外积
 
  //-- 验证对极约束
  Mat K = (Mat_<double>(3, 3) << 520.9, 0, 325.1, 0, 521.0, 249.7, 0, 0, 1);  //相机内参,一般是给定的
  for (DMatch m: matches) {        //遍历所有刷选后的匹配
    Point2d pt1 = pixel2cam(keypoints_1[m.queryIdx].pt, K);  //将匹配的点转变到归一化后的相机坐标,其Zc=1
    Mat y1 = (Mat_<double>(3, 1) << pt1.x, pt1.y, 1);   //点pt1的归一化相机坐标,3x1维度
    Point2d pt2 = pixel2cam(keypoints_2[m.trainIdx].pt, K);
    Mat y2 = (Mat_<double>(3, 1) << pt2.x, pt2.y, 1);
    Mat d = y2.t() * t_x * R * y1;       //这个计算公式是对极约束的公式,判断其是否为0;
    cout << "epipolar constraint = " << d << endl;  //输出对极约束值
  }
  return 0;
}
 
void find_feature_matches(const Mat &img_1, const Mat &img_2,
                          std::vector<KeyPoint> &keypoints_1,
                          std::vector<KeyPoint> &keypoints_2,
                          std::vector<DMatch> &matches) {
  //-- 初始化
  Mat descriptors_1, descriptors_2;
  // used in OpenCV3
  Ptr<FeatureDetector> detector = ORB::create();  //创建特征检测器
  Ptr<DescriptorExtractor> descriptor = ORB::create(); //构建特征描述器
  // use this if you are in OpenCV2
  // Ptr<FeatureDetector> detector = FeatureDetector::create ( "ORB" );
  // Ptr<DescriptorExtractor> descriptor = DescriptorExtractor::create ( "ORB" );
  Ptr<DescriptorMatcher> matcher = DescriptorMatcher::create("BruteForce-Hamming");  //暴力匹配法,汉明距离
  //-- 第一步:检测 Oriented FAST 角点位置
  detector->detect(img_1, keypoints_1);  //计算图1,图2的特征点
  detector->detect(img_2, keypoints_2);
 
  //-- 第二步:根据角点位置计算 BRIEF 描述子
  descriptor->compute(img_1, keypoints_1, descriptors_1);  //计算描述子
  descriptor->compute(img_2, keypoints_2, descriptors_2);
 
  //-- 第三步:对两幅图像中的BRIEF描述子进行匹配,使用 Hamming 距离
  vector<DMatch> match;
  //BFMatcher matcher ( NORM_HAMMING );
  matcher->match(descriptors_1, descriptors_2, match);  //计算;两个描述子之间的匹配
 
  //-- 第四步:匹配点对筛选
  double min_dist = 10000, max_dist = 0;
 
  //找出所有匹配之间的最小距离和最大距离, 即是最相似的和最不相似的两组点之间的距离
  for (int i = 0; i < descriptors_1.rows; i++) {
    double dist = match[i].distance;
    if (dist < min_dist) min_dist = dist;
    if (dist > max_dist) max_dist = dist;
  }
 
  printf("-- Max dist : %f \n", max_dist);  
  printf("-- Min dist : %f \n", min_dist);
 
  //当描述子之间的距离大于两倍的最小距离时,即认为匹配有误.但有时候最小距离会非常小,设置一个经验值30作为下限.
  for (int i = 0; i < descriptors_1.rows; i++) {
    if (match[i].distance <= max(2 * min_dist, 30.0)) {
      matches.push_back(match[i]);  //刷选较好的匹配结果
    }
  }
}
 
Point2d pixel2cam(const Point2d &p, const Mat &K) {
  return Point2d  //返回归一化后的相机坐标(Xc,Yc,1)
    (
      (p.x - K.at<double>(0, 2)) / K.at<double>(0, 0),  //(u-u0)/fx
      (p.y - K.at<double>(1, 2)) / K.at<double>(1, 1)   //(v-v0)/fy
    );
}
 
void pose_estimation_2d2d(std::vector<KeyPoint> keypoints_1,  //传入图像1,2的特征点,以及刷选后的匹配
                          std::vector<KeyPoint> keypoints_2,
                          std::vector<DMatch> matches,
                          Mat &R, Mat &t) {
  // 相机内参,TUM Freiburg2
  Mat K = (Mat_<double>(3, 3) << 520.9, 0, 325.1, 0, 521.0, 249.7, 0, 0, 1);
 
  //-- 把匹配点转换为vector<Point2f>的形式
  vector<Point2f> points1;
  vector<Point2f> points2;
 
  for (int i = 0; i < (int) matches.size(); i++) {
    points1.push_back(keypoints_1[matches[i].queryIdx].pt);   //将匹配点对中图1特征点ID对应的点存到向量points1
    points2.push_back(keypoints_2[matches[i].trainIdx].pt);   //将匹配点对中图2特征点ID对应的点存到向量points2
  }
 
  //-- 计算基础矩阵
  Mat fundamental_matrix;
  fundamental_matrix = findFundamentalMat(points1, points2, CV_FM_8POINT);  //调用了opencv中计算基础矩阵的接口,如果用eigen编写估计比opencv的Mat实现快些;传入匹配的2D点坐标,用8点法计算
  cout << "fundamental_matrix is " << endl << fundamental_matrix << endl;
 
  //-- 计算本质矩阵
  Point2d principal_point(325.1, 249.7);  //相机光心, TUM dataset标定值(在图像平面中的像素位置)
  double focal_length = 521;      //相机焦距, TUM dataset标定值
  Mat essential_matrix;           //定义本质矩阵
  essential_matrix = findEssentialMat(points1, points2, focal_length, principal_point);                 //根据匹配点,光轴,光心调用opencv的接口计算本质矩阵 
  cout << "essential_matrix is " << endl << essential_matrix << endl;
 
  //-- 计算单应矩阵,单应矩阵主要处理目标点位于一个平面上              
  //-- 但是本例中场景不是平面,单应矩阵意义不大
  Mat homography_matrix;
  homography_matrix = findHomography(points1, points2, RANSAC, 3);  //当目标点对多于4个点对时,用到RANSAC,由于单应矩阵是8自由度(9个位置量减去一个尺度不变性,一对点构建两个等式,求8个位置参数需要至少4个点对)
  cout << "homography_matrix is " << endl << homography_matrix << endl;
 
  //-- 从本质矩阵中恢复旋转和平移信息.
  // 此函数仅在Opencv3中提供
  recoverPose(essential_matrix, points1, points2, R, t, focal_length, principal_point);  //输出R,t
  cout << "R is " << endl << R << endl;
  cout << "t is " << endl << t << endl;
 
}

代码中相机平移向量t=[Tx,Ty,Tz]T的矩阵表示为S,其表达式为:

其中findHomography() 函数形式和参数说明如下:

Mat cv::findHomography	(	InputArray 	srcPoints,
                                InputArray 	dstPoints,
                                int 	method = 0,
                                double 	ransacReprojThreshold = 3,
                                OutputArray 	mask = noArray(),
                                const int 	maxIters = 2000,
                                const double 	confidence = 0.995 )

参数详解:

其中 recoverPose()函数原型和参数解析如下:


如上,由2D-2D特征点匹配计算E,F等,然后恢复R,t。接下来用三角测量则可以计算点的空间位置了。

三.  三角测量,空间点位置求解

单目SLAM中,无法由一张图像获取像素深度信息,需要用三角测量法估计。假设x1与x2为两个特征点归一化相机坐标,则:

 其中s1和s2是深度信息,两边同时左乘x2的反对称矩阵可得:

其中R和t已经可以求出,则可直接求出s1,然后算出s2, 由于噪声的存在,求出的R和t不一定使上式为0,所以更常见的做法是求出最小二乘解而不是零解。

上上面式子可以写成:

\left [ -Rx_{1} ,x_{2}\right ]\begin{bmatrix} s_{1}\\ s_{2} \end{bmatrix}=t

即求Ax=b形式的最小二乘解问题,则x=(A^{T}A)^{-1}A^{T}b

以下为工程实践:

在slambook2-master/ch7中新建个文件夹叫 triangulation,然后将triangulation.cpp和CMakeLists.txt拷贝进来,修改CMakeLists.txt的内容如下:

cmake_minimum_required(VERSION 2.8)
project(triangulation)
 
set(CMAKE_BUILD_TYPE "Release")
add_definitions("-DENABLE_SSE")
set(CMAKE_CXX_FLAGS "-std=c++11 -O2 ${SSE_FLAGS} -msse4")
#list(APPEND CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake)
 
find_package(OpenCV 3 REQUIRED)
#find_package(G2O REQUIRED)
#find_package(Sophus REQUIRED)
 
include_directories(
        ${OpenCV_INCLUDE_DIRS}
)
 
 
# # add_executable( triangulation triangulation.cpp extra.cpp) # use this if in opencv2
add_executable(triangulation triangulation.cpp)
target_link_libraries(triangulation ${OpenCV_LIBS})

然后终端进到这个目录下,执行如下指令:

mkdir build
cd build
cmake ..
make

 会在build中生成可执行文件triangulation,将1.png和2.png拷贝到build下,终端执行:

./triangulation 1.png 2.png 

终端输出结果如下:打印了81个图1情况下目标点相对相机的距离Zc(即depth)

打印出匹配点 的距离值depth

triangulation.cpp 的源码分析如下:

#include <iostream>
#include <opencv2/opencv.hpp>
// #include "extra.h" // used in opencv2
using namespace std;
using namespace cv;
 
void find_feature_matches(            //寻找特征点,匹配分装成一个函数
  const Mat &img_1, const Mat &img_2,
  std::vector<KeyPoint> &keypoints_1,
  std::vector<KeyPoint> &keypoints_2,
  std::vector<DMatch> &matches);
 
void pose_estimation_2d2d(                  //位姿估计分装成一个函数,得到相机的R,t
  const std::vector<KeyPoint> &keypoints_1,
  const std::vector<KeyPoint> &keypoints_2,
  const std::vector<DMatch> &matches,
  Mat &R, Mat &t);
 
void triangulation(                    //三角测量计算3D坐标封装成函数
  const vector<KeyPoint> &keypoint_1,
  const vector<KeyPoint> &keypoint_2,
  const std::vector<DMatch> &matches,
  const Mat &R, const Mat &t,
  vector<Point3d> &points
);
 
/// 作图用
inline cv::Scalar get_color(float depth) {
  float up_th = 50, low_th = 10, th_range = up_th - low_th;  //距离阈值区间[10,50]
  if (depth > up_th) depth = up_th;     //将实测深度值限制在阈值区间中
  if (depth < low_th) depth = low_th;
  return cv::Scalar(255 * depth / th_range, 0, 255 * (1 - depth / th_range));
}
 
// 像素坐标转相机归一化坐标
Point2f pixel2cam(const Point2d &p, const Mat &K);  //将像素坐标转为归一化后的相机坐标,其Zc=1
 
int main(int argc, char **argv) {
  if (argc != 3) {
    cout << "usage: triangulation img1 img2" << endl;
    return 1;
  }
  //-- 读取图像
  Mat img_1 = imread(argv[1], CV_LOAD_IMAGE_COLOR);  //加载彩色图
  Mat img_2 = imread(argv[2], CV_LOAD_IMAGE_COLOR);
 
  vector<KeyPoint> keypoints_1, keypoints_2;
  vector<DMatch> matches;
  find_feature_matches(img_1, img_2, keypoints_1, keypoints_2, matches);  //寻找特征点和匹配
  cout << "一共找到了" << matches.size() << "组匹配点" << endl;
 
  //-- 估计两张图像间运动
  Mat R, t;
  pose_estimation_2d2d(keypoints_1, keypoints_2, matches, R, t); //计算2D-2D的E,然后恢复得到R,t
 
  //-- 三角化
  vector<Point3d> points;  //3D点向量
  triangulation(keypoints_1, keypoints_2, matches, R, t, points);  //传入图1,2的特征点,刷选后的匹配结果,由本质矩阵恢复得到的R和t,输出图1中目标在其相机姿态中的3D实际相机坐标(Xc,Yc,Zc)
 
  //-- 验证三角化点与特征点的重投影关系
  Mat K = (Mat_<double>(3, 3) << 520.9, 0, 325.1, 0, 521.0, 249.7, 0, 0, 1);  //相机内参
  Mat img1_plot = img_1.clone();
  Mat img2_plot = img_2.clone();
  for (int i = 0; i < matches.size(); i++) {  //遍历刷选后的匹配点对
    // 第一个图
    float depth1 = points[i].z;           //图1中第i和匹配的距离
    cout << "depth: " << depth1 << endl;
    Point2d pt1_cam = pixel2cam(keypoints_1[matches[i].queryIdx].pt, K);
    cv::circle(img1_plot, keypoints_1[matches[i].queryIdx].pt, 2, get_color(depth1), 2);  //在图1中画出刷选后的匹配点(颜色与距离相关)
 
    // 第二个图
    Mat pt2_trans = R * (Mat_<double>(3, 1) << points[i].x, points[i].y, points[i].z) + t;   //根据公式x2=Rx1+t, 其中x1和x2都是相机坐标,这个式子是将图1中计算得到的P的实际坐标投影到图2中相机坐标下;
    float depth2 = pt2_trans.at<double>(2, 0);   //理论计算得到P在图2相机下的相机坐标
    cv::circle(img2_plot, keypoints_2[matches[i].trainIdx].pt, 2, get_color(depth2), 2);  //在图2中画出刷选后的匹配点(颜色与距离相关)
  }
  cv::imshow("img 1", img1_plot);
  cv::imshow("img 2", img2_plot);
  cv::waitKey();
 
  return 0;
}
 
void find_feature_matches(const Mat &img_1, const Mat &img_2,
                          std::vector<KeyPoint> &keypoints_1,
                          std::vector<KeyPoint> &keypoints_2,
                          std::vector<DMatch> &matches) {
  //-- 初始化
  Mat descriptors_1, descriptors_2;
  // used in OpenCV3
  Ptr<FeatureDetector> detector = ORB::create();
  Ptr<DescriptorExtractor> descriptor = ORB::create();
  // use this if you are in OpenCV2
  // Ptr<FeatureDetector> detector = FeatureDetector::create ( "ORB" );
  // Ptr<DescriptorExtractor> descriptor = DescriptorExtractor::create ( "ORB" );
  Ptr<DescriptorMatcher> matcher = DescriptorMatcher::create("BruteForce-Hamming");
  //-- 第一步:检测 Oriented FAST 角点位置
  detector->detect(img_1, keypoints_1);
  detector->detect(img_2, keypoints_2);
 
  //-- 第二步:根据角点位置计算 BRIEF 描述子
  descriptor->compute(img_1, keypoints_1, descriptors_1);
  descriptor->compute(img_2, keypoints_2, descriptors_2);
 
  //-- 第三步:对两幅图像中的BRIEF描述子进行匹配,使用 Hamming 距离
  vector<DMatch> match;
  // BFMatcher matcher ( NORM_HAMMING );
  matcher->match(descriptors_1, descriptors_2, match);
 
  //-- 第四步:匹配点对筛选
  double min_dist = 10000, max_dist = 0;
 
  //找出所有匹配之间的最小距离和最大距离, 即是最相似的和最不相似的两组点之间的距离
  for (int i = 0; i < descriptors_1.rows; i++) {
    double dist = match[i].distance;
    if (dist < min_dist) min_dist = dist;
    if (dist > max_dist) max_dist = dist;
  }
 
  printf("-- Max dist : %f \n", max_dist);
  printf("-- Min dist : %f \n", min_dist);
 
  //当描述子之间的距离大于两倍的最小距离时,即认为匹配有误.但有时候最小距离会非常小,设置一个经验值30作为下限.
  for (int i = 0; i < descriptors_1.rows; i++) {
    if (match[i].distance <= max(2 * min_dist, 30.0)) {
      matches.push_back(match[i]);
    }
  }
}
 
void pose_estimation_2d2d(
  const std::vector<KeyPoint> &keypoints_1,
  const std::vector<KeyPoint> &keypoints_2,
  const std::vector<DMatch> &matches,
  Mat &R, Mat &t) {
  // 相机内参,TUM Freiburg2
  Mat K = (Mat_<double>(3, 3) << 520.9, 0, 325.1, 0, 521.0, 249.7, 0, 0, 1);
 
  //-- 把匹配点转换为vector<Point2f>的形式
  vector<Point2f> points1;
  vector<Point2f> points2;
 
  for (int i = 0; i < (int) matches.size(); i++) {
    points1.push_back(keypoints_1[matches[i].queryIdx].pt);
    points2.push_back(keypoints_2[matches[i].trainIdx].pt);
  }
 
  //-- 计算本质矩阵
  Point2d principal_point(325.1, 249.7);        //相机主点, TUM dataset标定值
  int focal_length = 521;            //相机焦距, TUM dataset标定值
  Mat essential_matrix;              
  essential_matrix = findEssentialMat(points1, points2, focal_length, principal_point);  //求本质矩阵,需要焦距,光心坐标
 
  //-- 从本质矩阵中恢复旋转和平移信息.
  recoverPose(essential_matrix, points1, points2, R, t, focal_length, principal_point);   //输出R,t,这个R,t计算的是图2相对图1的旋转和平移
}
 
void triangulation(
  const vector<KeyPoint> &keypoint_1,
  const vector<KeyPoint> &keypoint_2,
  const std::vector<DMatch> &matches,
  const Mat &R, const Mat &t,
  vector<Point3d> &points) {
  Mat T1 = (Mat_<float>(3, 4) <<
    1, 0, 0, 0,
    0, 1, 0, 0,
    0, 0, 1, 0);   //这块表示图1的相机作为参考,变换矩阵中没有相机旋转和平移
  Mat T2 = (Mat_<float>(3, 4) <<
    R.at<double>(0, 0), R.at<double>(0, 1), R.at<double>(0, 2), t.at<double>(0, 0),
    R.at<double>(1, 0), R.at<double>(1, 1), R.at<double>(1, 2), t.at<double>(1, 0),
    R.at<double>(2, 0), R.at<double>(2, 1), R.at<double>(2, 2), t.at<double>(2, 0)
  );  //T2中传入R,t,即图2相对图1这个参考位置的变换矩阵
 
  Mat K = (Mat_<double>(3, 3) << 520.9, 0, 325.1, 0, 521.0, 249.7, 0, 0, 1);  //相机内参
  vector<Point2f> pts_1, pts_2;  //图1和图1上的2D图像像素点转换后的归一化相机坐标点,对应的Zc=1,所以只要计算Xc和Yc
  for (DMatch m:matches) {
    // 将像素坐标转换至相机坐标
    pts_1.push_back(pixel2cam(keypoint_1[m.queryIdx].pt, K));
    pts_2.push_back(pixel2cam(keypoint_2[m.trainIdx].pt, K));
  }
 
  Mat pts_4d;
  cv::triangulatePoints(T1, T2, pts_1, pts_2, pts_4d); //传入两个图像对应相机的变化矩阵,各自相机坐标系下归一化相机坐标,输出的3D坐标是齐次坐标,共四个维度,因此需要将前三个维度除以第四个维度以得到非齐次坐标xyz
 
  // 转换成非齐次坐标
  for (int i = 0; i < pts_4d.cols; i++) {  //遍历所有的点,列数表述点的数量
    Mat x = pts_4d.col(i);  //x为4x1维度
    x /= x.at<float>(3, 0); // 归一化
    Point3d p(
      x.at<float>(0, 0),
      x.at<float>(1, 0),
      x.at<float>(2, 0)
    );
    points.push_back(p);  //将图1测得的目标相对相机实际位置(Xc,Yc,Zc)存入points
  }
}
 
Point2f pixel2cam(const Point2d &p, const Mat &K) {
  return Point2f
    (
      (p.x - K.at<double>(0, 2)) / K.at<double>(0, 0),
      (p.y - K.at<double>(1, 2)) / K.at<double>(1, 1)
    );
}

其中triangulatePoints 的函数原型为:

void triangulatePoints(InputArray projMatr1, InputArray projMatr2, InputArray projPoints1, InputArray projPoints2, OutputArray points4D)

参数说明:

即需要第一个相机的变换矩阵,第2个相机的变换矩阵,图1中计算得到的点,图2中计算得到的点,返回重构的4xN 维度点集,其中N是点的数量。

通过三角测量及像素上的匹配点,可以求出路标即P点基于第一个相机坐标系的三维坐标点,然后通过对极约束求出的R,t可以求出在第二个相机坐标系上路标P点的三维坐标值。

三角平移是由平移得到的,有平移才会有对极几何约束的三角形。纯旋转是无法使用三角测量的,对极约束将永远满足。

在平移时,三角测量有不确定性,会引出三角测量的矛盾。这个矛盾就是:当平移很小时,像素上的不确定性将导致较大的深度不确定性,平移较大时,在相同的相机分辨率下,三角化测量将更精确。三角化测量的矛盾:平移增大,会导致匹配失效,平移太小,三角化精度不够。

 


参考:

findHomography()函数详解

opencv相机标定与3D重构(官方文档)推荐好好看这篇官方手册,对每个函数都有详细解释


本文转载自:https://blog.csdn.net/c20081052/article/details/102486486