概述

• 特征点的投影模型为 \(p=\frac{1}{Z} KP\)，\(P\)为世界坐标系下某点的坐标（\(Z\)为z方向的坐标），\(p\)为对应图像特征点。\(K\)为内参，在标定好的相机下，\(K\)已知
• 根据对极几何约束，假设\(p_{2}\)为相机位姿运动\(R\)，\(t\)后与前一帧的特征点\(p_{1}\)匹配的特征点，则有
  \[s_1p_1 = KP\]
  \[s_2p_2 = K(RP+t)\]
• 参考视觉slam14讲的推导，这里可以得到对极约束
  \[{p}_2^T{K^{ - T}}{t^ \wedge }RK{H^{ - 1}}{p_1} = 0\]
  可以通过8点法求解本质矩阵进而得到\(R\)，\(t\)
• 每两帧之间的位姿递推误差积累很快，因此直接递推的位姿是不太稳定的。
• \(t\)的缩放尺寸不确定，因此不能获得绝对位置

测试代码

主要基于视觉slam14讲的代码，稍微改动的测试，尽管能够求解姿态但是并不十分准确，后续考虑使用双目相机实现定位功能

#include <opencv2/core.hpp>
#include <opencv2/highgui.hpp>
#include <opencv2/videoio.hpp>
#include <iostream>
#include "opencv2/features2d/features2d.hpp"
#include <vector>
#include <time.h>
#include <opencv2/calib3d/calib3d.hpp>
#include <Windows.h>
//#include "stdafx.h"using namespace cv;
using namespace std;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);int main()
{VideoCapture cap1;//VideoCapture cap2;cap1.open(1);//白色摄像头//cap2.open(2);//黑色摄像头//if (!cap1.isOpened()||!cap2.isOpened())if (!cap1.isOpened()){return -1;}//将摄像头从640*480改成320*240，速度从200ms提升至50ms//cap1.set(CV_CAP_PROP_FRAME_WIDTH, 320);//cap1.set(CV_CAP_PROP_FRAME_HEIGHT, 240);//cap2.set(CV_CAP_PROP_FRAME_WIDTH, 320);//cap2.set(CV_CAP_PROP_FRAME_HEIGHT, 240);//namedWindow("Video", 1);//namedWindow("Video", 2);//namedWindow("pts", 3);//Mat frame;Mat img_1;Mat img_2;while (1){cap1 >> img_1;Sleep(10);cap1 >> img_2;if (!img_1.data || !img_2.data){cout << "error reading images " << endl;return -1;}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);//cout << "R:" << endl << R << endl;//cout << "t:" << endl << t << endl;-- 验证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);//  Mat y1 = (Mat_<double>(3, 1) << pt1.x, pt1.y, 1);//  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;//  cout << "epipolar constraint = " << d << endl;//}waitKey(1);}cap1.release();//cap2.release();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 = match[0].distance, max_dist = match[0].distance;//找出所有匹配之间的最小距离和最大距离, 即是最相似的和最不相似的两组点之间的距离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((p.x - K.at<double>(0, 2)) / K.at<double>(0, 0),(p.y - K.at<double>(1, 2)) / K.at<double>(1, 1));
}void pose_estimation_2d2d(std::vector<KeyPoint> keypoints_1,std::vector<KeyPoint> keypoints_2,std::vector< DMatch > matches,Mat& R, Mat& t)
{// 相机内参,需要标定得到/*1225.22831056496  36.6177252813478    342.7841696131240   1178.20016318321    187.2907550112760   0   1*//*1296.76842892674  46.6256354215592    409.7179331436720   1210.08953016730    69.83892431592290   0   1*///Mat K = (Mat_<double>(3, 3) << 520.9, 0, 325.1, 0, 521.0, 249.7, 0, 0, 1);Mat K = (Mat_<double>(3, 3) << 1296.76842892674, 46.6256354215592, 409.717933143672, 0, 1210.08953016730, 69.8389243159229, 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);}//-- 计算基础矩阵Mat fundamental_matrix;fundamental_matrix = findFundamentalMat(points1, points2, CV_FM_8POINT);//cout << "fundamental_matrix is " << endl << fundamental_matrix << endl;//-- 计算本质矩阵Point2d principal_point(409.717933143672, 69.8389243159229);    //相机光心, 标定值double focal_length = 1296.76842892674;         //相机焦距, 标定值Mat essential_matrix;essential_matrix = findEssentialMat(points1, points2, focal_length, principal_point);//cout << "essential_matrix is " << endl << essential_matrix << endl;//-- 计算单应矩阵Mat homography_matrix;homography_matrix = findHomography(points1, points2, RANSAC, 3);//cout << "homography_matrix is " << endl << homography_matrix << endl;//-- 从本质矩阵中恢复旋转和平移信息.recoverPose(essential_matrix, points1, points2, R, t, focal_length, principal_point);//cout << "R is " << endl << R << endl;//cout << "t is " << endl << t << endl;cout << R << endl;
}