视觉slam学习之——ch7 视觉里程计（centos系统）

最新推荐文章于 2023-08-24 17:11:04 发布

ciky奇

最新推荐文章于 2023-08-24 17:11:04 发布

阅读量1.7k

点赞数 3

分类专栏： SLAM 计算机视觉机器人文章标签：特征提取和匹配本质矩阵E 单应性矩阵 centos

本文链接：https://blog.csdn.net/c20081052/article/details/102486486

版权

计算机视觉同时被 3 个专栏收录

46 篇文章 10 订阅

订阅专栏

机器人

8 篇文章 1 订阅

订阅专栏

SLAM

6 篇文章 3 订阅

订阅专栏

这个章节主要讲解：

图像特征提取；

多幅图像匹配特征点；

对极几何；

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）：

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

根据配对点的像素位置求出E或者F；
根据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重构（官方文档） 推荐好好看这篇官方手册，对每个函数都有详细解释。

ciky奇

关注

3
点赞
踩
23

收藏

觉得还不错? 一键收藏
0
评论
复制链接

分享到 QQ

分享到新浪微博

扫一扫

专栏目录