算法流程
-
寻找关键点,计算描述子,寻找匹配点
-
利用对极几何计算本质矩阵,基础矩阵,单应矩阵
-
8点法求解本质矩阵(基础矩阵)
-
大于8点时可采用最小二乘法、随机采样一致性(RANSAC)
-
存在误匹配时倾向使用RANSAC
-
利用SVD(奇异值分解)求得旋转矩阵和平移矩阵
-
数值法、分析法分解单应矩阵求得R、t
-
验证本质矩阵,对极约束
-
相机内参数矩阵、焦距、光心已知
-
因为对极约束的尺度特性,真实的地图通常与计算出的相差一个非0常数倍
-
因为本质矩阵为平移矩阵叉乘旋转矩阵求得,当相机没有做平移运动时,对极几何无法应用
-
分析对极约束结果可知,算法精度不高
代码
//
// Created by wcm on 2020/6/6.
//
#include <iostream>
#include <opencv2/core/core.hpp>
#include <opencv2/features2d/features2d.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/calib3d/calib3d.hpp>
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);
int main( ){
//--读取图像
Mat img_1=imread("/home/automobile/wcm/slambook2/ch7/1.png", CV_LOAD_IMAGE_COLOR);
Mat img_2=imread("/home/automobile/wcm/slambook2/ch7/2.png", CV_LOAD_IMAGE_COLOR);
assert(img_1.data && img_2.data && "Cannot 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;
//--估计两张图像间运动
//R=R21
Mat R, t;
pose_estimation_2d2d(keypoints_1, keypoints_2, matches, R, t);
//--验证E=t^R*scale
//首先声明一个3*3的空矩阵,然后根据t求取t的反对称矩阵t^
//at<double>(i,j)函数表示像素点的位置坐标
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>