通过对极几何的原理和矩阵分解技术,能够估计出两幅图像间的相对旋转和平移
在pose_estimation_2d2d.cpp代码中,通过特征匹配找到了两幅图像中对应的特征点,然后通过对极约束和基础矩阵/本质矩阵的计算,得到了相机的旋转矩阵R和平移向量t。
代码原理
1. 特征匹配 (find_feature_matches)
首先,使用ORB算法检测图像中的关键点,并为这些关键点生成描述符。然后,使用Hamming距离通过暴力匹配算法匹配两幅图像的描述符。匹配过程中,通过设置阈值过滤掉一些不好的匹配,以得到更准确的匹配点对。
2. 计算基础矩阵 (findFundamentalMat)
基础矩阵 F F F描述了在无约束相机(即不知道内参)情况下,一对匹配点在两幅图像中的对极约束:
x ′ T F x = 0 x'^{T}Fx = 0 x′TFx=0
其中, x x x和 x ′ x' x′分别是第一幅和第二幅图像中的匹配点的齐次坐标。基础矩阵包含了相机的旋转和平移信息,但它不能直接用于恢复这些信息,因为它还包含了相机内参的影响。
3. 计算本质矩阵 (findEssentialMat)
本质矩阵 E E E也描述了匹配点之间的对极约束,但它是在已知相机内参的情况下使用的:
x ′ T E x = 0 x'^{T}Ex = 0 x′TEx=0
与基础矩阵不同,本质矩阵仅由相机的旋转和平移决定,不包含内参的信息。这使得从本质矩阵中恢复出的旋转和平移更加精确。本质矩阵可以通过SVD分解进一步得到相机的旋转 R R R和平移 t t t。
4. 计算单应矩阵 (findHomography)
单应矩阵 H H H描述了一幅图像到另一幅图像的透视变换,假设场景是平面的或者两幅图像之间仅发生旋转:
x ′ = H x x' = Hx x′=Hx
单应矩阵在处理平面场景或纯旋转的情况下非常有用,但它不能提供场景深度信息或是复杂场景中的三维结构信息。
5. 从本质矩阵恢复旋转和平移 (recoverPose)
最后一步使用函数recoverPose
从本质矩阵中恢复出旋转
R
R
R和平移
t
t
t。这是通过对本质矩阵进行SVD分解并应用一些约束来实现的,以确保找到正确的解。
在提供的代码中,基础矩阵 F F F、本质矩阵 E E E和单应矩阵 H H H的计算是通过OpenCV库中的特定函数完成的。这些函数使用图像中匹配点对的信息来估计相应的矩阵。下面是每个矩阵计算方法的详细说明:
代码解析
1. 计算基础矩阵 F F F
基础矩阵是使用findFundamentalMat
函数计算的,该函数的输入是两组对应的点(在这种情况下是points1
和points2
),这些点是从匹配的关键点中提取出来的像素坐标。cv::FM_8POINT
方法被用作计算方法,它是一种经典的算法,基于八点算法求解基础矩阵。
Mat fundamental_matrix;
fundamental_matrix = findFundamentalMat(points1, points2, cv::FM_8POINT);
2. 计算本质矩阵 E E E
本质矩阵是使用findEssentialMat
函数计算的,该函数需要两组匹配点(同样是points1
和points2
),相机焦距和光心(主点)的位置。这里使用TUM数据集的标定值。findEssentialMat
函数内部使用五点算法来求解本质矩阵。
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);
3. 计算单应矩阵 H H H
单应矩阵是使用findHomography
函数计算的,输入是相同的两组匹配点。此处使用的是RANSAC算法来鲁棒地估计单应矩阵,它可以有效地处理异常值(即错误匹配)。
Mat homography_matrix;
homography_matrix = findHomography(points1, points2, RANSAC, 3);
findHomography
函数中的RANSAC
参数指定了使用RANSAC算法进行鲁棒估计,3
是RANSAC算法的阈值,用于决定何时将一个点视为内点。
总结
这三个矩阵 F , E , H F, E, H F,E,H的计算都依赖于从两幅图像中提取并匹配的特征点。基础矩阵 F F F和本质矩阵 E E E用于估计相机的运动(即旋转 R R R和平移 t t t),而单应矩阵 H H H在处理平面场景或估计纯旋转时特别有用。在这段代码中,通过对这些矩阵的计算和分解,可以从两幅图像中的2D-2D匹配点对恢复出相对于相机位姿的信息。
书中代码
#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 );
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], IMREAD_COLOR );
Mat img_2 = imread ( argv[2], IMREAD_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 );
//-- 验证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;
}
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] );
}
}
}
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 )
{
// 相机内参,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 );
}
//-- 计算基础矩阵
Mat fundamental_matrix;
fundamental_matrix = findFundamentalMat ( points1, points2, cv::FM_8POINT );
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 );
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;
}