2D-2D:E2RT_Triangulation
2D-2D:E2RT_Triangulation
引言
ch7.当相机为单目时,只知道2D的像素坐标,因而问题是根据两组2D点估计运动。该问题用对极几何来解决。
1.2D-2D对极约束
对极约束：
实际求解步骤：
- （1）根据已经匹配的像素点由公式使用八点法求解即求解本质矩阵.
- （2）根据本质矩阵使用奇异值分解(SVD)，求出.
由图可以看出，只有图(1)是真正的解。只需要把任意一点代入四种解中,检测该点在两个相机下的深度,就可以确定哪个解是正确的。对于单目初始化不能只有纯旋转,必须要有一定程度的平移。具体参考教材.
E2Rt code:
#include <Eigen/Core>
#include <Eigen/Dense>
#include <Eigen/Geometry>
using namespace Eigen;
#include <sophus/so3.h>
using namespace Sophus;
#include <iostream>
using namespace std;
int main(int argc, char **argv) {
// 给定Essential矩阵
Matrix3d E;//3*3
E << -0.0203618550523477, -0.4007110038118445, -0.03324074249824097,
0.3939270778216369, -0.03506401846698079, 0.5857110303721015,
-0.006788487241438284, -0.5815434272915686, -0.01438258684486258;
// 待计算的R,t
Matrix3d R;
Vector3d t;
// SVD and fix singular values(奇异值)
// START YOUR CODE HERE
//E = U*Sigma*VT ==> Sigma = U.inv()*E*VT.inv()
JacobiSVD<MatrixXd> svd(E, ComputeThinU | ComputeThinV);
Matrix3d U = svd.matrixU(); // Obtain matrix U
Matrix3d V = svd.matrixV(); // Obtain matrix V
Matrix3d Sigma = U.inverse() * E * V.transpose().inverse(); // Obtain matrix Sigma
cout<<"U:\n"<<U<<"\nV:\n"<<V<<"\nSigma:\n"<<Sigma<<endl;
vector<double> tao = {Sigma(0,0), Sigma(1,1), Sigma(2,2)};//是小写的sigma不是tao
sort(tao.begin(),tao.end());//从大到小排序 Sigma = diag(sigma1,sigma2,sigma3);
double tao_mean = (tao[1]+tao[2])*0.5;
Matrix3d SigmaFix = Matrix3d::Zero(); // SigmaFix: 處理后的Sigma矩陣
SigmaFix(0,0) = tao_mean;
SigmaFix(1,1) = tao_mean;
cout<<"Sigma after fix: \n"<<SigmaFix<<endl;
// END YOUR CODE HERE
// set t1, t2, R1, R2
// START YOUR CODE HERE 两种可能，四种组合 //AngleAxisd第二章中Eigen的旋转矩阵
Matrix3d R_Z1 = AngleAxisd(M_PI/2,Vector3d(0,0,1)).matrix(); // R_Z1: 沿 Z 轴旋转 90 度得到的旋转矩阵
Matrix3d R_Z2 = AngleAxisd(-M_PI/2,Vector3d(0,0,1)).matrix(); // 公式！
Matrix3d t_wedge1 = U * R_Z1 * SigmaFix * U.transpose(); // t1^
Matrix3d t_wedge2 = U * R_Z2 * SigmaFix * U.transpose(); // t2^
Matrix3d R1 = U * R_Z1.transpose() * V.transpose();
Matrix3d R2 = U * R_Z2.transpose() * V.transpose();
// END YOUR CODE HERE
cout << "R1 = " << R1 << endl;
cout << "R2 = " << R2 << endl;
cout << "t1 = " << SO3::vee(t_wedge1).transpose() << endl;
cout << "t2 = " << SO3::vee(t_wedge2).transpose() << endl;
// check t^R=E up to scale
Matrix3d tR1 = t_wedge1 * R1;
cout << "1. t^R = " << tR1 << endl;
Matrix3d tR2 = t_wedge2 * R2;
cout << "2. t^R = " << tR2 << endl;
Matrix3d tR3 = t_wedge1 * R2;
cout << "3. t^R = " << tR3 << endl;
Matrix3d tR4 = t_wedge2 * R1;
cout << "4. t^R = " << tR4 << endl;
return 0;
}
结果分析：
U:
-0.0890846 -0.562354 -0.822084
0.993441 -0.109576 -0.0326974
-0.0716928 -0.819605 0.568426
V:
0.556696 -0.0369821 0.829892
0.0601827 0.998179 0.00411052
0.828533 -0.0476569 -0.557908
Sigma:
0.707107 1.73472e-17 -1.66533e-16
-2.1684e-16 0.707107 2.77556e-17
8.19728e-17 -5.31748e-18 9.10962e-17
Sigma after fix:
0.707107 0 0
0 0.707107 0
0 0 0
R1 = -0.365887 -0.0584576 0.928822
-0.00287462 0.998092 0.0616848
0.930655 -0.0198996 0.365356
R2 = -0.998596 0.0516992 -0.0115267
-0.0513961 -0.99836 -0.0252005
0.0128107 0.0245727 -0.999616
t1 = -0.581301 -0.0231206 0.401938
t2 = 0.581301 0.0231206 -0.401938
1. t^R = -0.0203619 -0.400711 -0.0332407
0.393927 -0.035064 0.585711
-0.00678849 -0.581543 -0.0143826
2. t^R = -0.0203619 -0.400711 -0.0332407
0.393927 -0.035064 0.585711
-0.00678849 -0.581543 -0.0143826
3. t^R = 0.0203619 0.400711 0.0332407
-0.393927 0.035064 -0.585711
0.00678849 0.581543 0.0143826
4. t^R = 0.0203619 0.400711 0.0332407
-0.393927 0.035064 -0.585711
0.00678849 0.581543 0.0143826
四种组合到底是哪一种还需要实际验证。
2.八点法求解E
当点对多余8对且可能存在误匹配的情况时,会更倾向于使用随机采样一致性(Random Sample Concensus,RANSAC)求解，而不是最小二乘。RANSAC是一种通用的做法,适用于很多带错误数据的情况,可以处理带有错误匹配的数据。
RANSAC算法参考：
像点总是在对极线，因此可以选择像点到对极线的距离作为判断该点是内点还是外点的依据，设点到对极的距离的阈值为d。则使用RANSAC的方法估算基础矩阵的步骤：
- 从匹配的点对中选择8个点，使用8点法估算出基础矩阵
- 计算其余的点对到其对应对极线的距离，如果则该点为内点，否则为外点。记下符合该条件的内点的个数为
- 迭代k次，或者某次得到内点的数目占有的比例大于等于95%，则停止。选择最大的基础矩阵作为最终的结果。
RANSAC能够不依赖于任何额外信息的情况下，将数据划分为内点和外点。但也有其相应的缺点，RANSAC并不能保证得到正确的结果，需要提高迭代的次数；另一个是，内点外点的判断需要设置阈值。
3.三角测量求深度
对于单目SLAM的地图点深度估计，依靠两帧匹配的图像实现(但是具有尺度不确定性)。
- 归一化平面：,其中的已经知道。
- 左乘得：,根据右侧求解出，然后一次求出就得到了深度。
- 由于噪声的存在,估得的 R, t,不一定精确使式为0，所以常用的最小二乘解而不是零解。
CODE：从2D图像进行ORB特征提取–>特征匹配–>极线约束八点法求E(F、H)–>R,t–>三角测量求深度
/*
*三角测量法 求解 两组单目相机 图像点深度
* s1 * x1 = s2 * R * x2 + t
* x1 x2 为两帧图像上 两点对 在归一化坐标平面上的坐标
* s1 和 s2为两个特征点的深度 ，由于误差存在， s1 * x1 = s2 * R * x2 + t不精确相等
* 常见的是求解最小二乘解，而不是零解
* s1 * x1叉乘x1 = s2 * x1叉乘* R * x2 + x1叉乘 t=0 可以求得x2
*
*/
#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" // used in opencv2
using namespace std;//标准库 命名空间
using namespace cv; //opencv库命名空间
// ./pose_estimation_2d2d 1.png 2.png
/****************************************************
* 本程序演示了如何使用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 (
const std::vector<KeyPoint>& keypoints_1,
const std::vector<KeyPoint>& keypoints_2,
const std::vector< DMatch >& matches,
Mat& R, Mat& t );
// 三角测量
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
);
// 像素坐标转相机归一化坐标
Point2f pixel2cam( const Point2d& p, const Mat& K );
int main ( int argc, char** argv )
{
if ( argc != 3 )//命令行参数 1.png 2.png
{
cout<<"用法: ./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;
//-- 估计两张图像间运动 R t
Mat R,t;//旋转和平移 第一张图 到第二章图的坐标变换矩阵和平移矩阵
pose_estimation_2d2d ( keypoints_1, keypoints_2, matches, R, t );
//-- 三角化 由运动变换矩阵 R t 以及三角测量法 得到 特征点的深度
// s1 * x1 = s2 * R * x2 + t
// s1 * x1叉乘x1 = s2 * x1叉乘* R * x2 + x1叉乘 t=0 可以求得x2
vector<Point3d> points;
triangulation( keypoints_1, keypoints_2, matches, R, t, points );
//-- 验证三角化点与特征点的重投影关系
Mat K = ( Mat_<double> ( 3,3 ) << 520.9, 0, 325.1, 0, 521.0, 249.7, 0, 0, 1 );// 相机内参,TUM Freiburg2
for ( int i=0; i<matches.size(); i++ )
{
Point2d pt1_cam = pixel2cam( keypoints_1[ matches[i].queryIdx ].pt, K );
Point2d pt1_cam_3d(points[i].x/points[i].z, points[i].y/points[i].z ); //归一化
cout<<"第一帧图像中的特征点: "<<pt1_cam<<endl;
cout<<"从三角计算得到的3D点重投影得到的二维点： "<<pt1_cam_3d<<", d="<<points[i].z<<endl;
// 第二个图
Point2f pt2_cam = pixel2cam( keypoints_2[ matches[i].trainIdx ].pt, K );
Mat pt2_trans = R*( Mat_<double>(3,1) << points[i].x, points[i].y, points[i].z ) + t;
pt2_trans /= pt2_trans.at<double>(2,0);//归一化
cout<<"第二帧图像中的特征点: "<<pt2_cam<<endl;
cout<<"从三角计算得到的3D点重投影得到的二维点： "<<pt2_trans.t()<<endl;
cout<<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 )
{
//--------------------第0步:初始化------------------------------------------------------
Mat descriptors_1, descriptors_2;//描述子
// OpenCV3 特征点检测器 描述子生成器 用法
Ptr<FeatureDetector> detector = ORB::create();
Ptr<DescriptorExtractor> descriptor = ORB::create();
// 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 );
}
//-- 计算基础矩阵 F
Mat fundamental_matrix;
fundamental_matrix = findFundamentalMat ( points1, points2, CV_FM_8POINT );
cout<<"fundamental_matrix is "<<endl<< fundamental_matrix<<endl;
//-- 计算本质矩阵 E
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 );
cout<<"essential_matrix is "<<endl<< essential_matrix<<endl;
//-- 计算单应矩阵 H 采用随机采样一致性求解
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;
}
//三角测量
// s1 * x2 = s2 * x1 * R + t
// s1 和 s2为深度 误差存在，x1 和 x2 可能不同
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 )
{
// 变换矩阵 在第一幅图像下 的变换矩阵 Pc1 = Pw = T1 * Pw
Mat T1 = (Mat_<float> (3,4) <<
1,0,0,0,
0,1,0,0,
0,0,1,0);
// Pc2 = T2 * Pw = [ R t] * Pw
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)
);
// 相机内参,TUM Freiburg2
// [fx 0 cx
// 0 fy cy
// 0 0 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;//相机坐标下点坐标
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 );
// 转换成非齐次坐标
for ( int i=0; i<pts_4d.cols; i++ )
{
Mat x = pts_4d.col(i);
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 );
}
}
// 像素坐标转相机归一化坐标
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)
);
}