2D-2D:E2RT_Triangulation

最新推荐文章于 2024-05-20 09:43:30 发布

古路

最新推荐文章于 2024-05-20 09:43:30 发布

阅读量771

点赞数 1

分类专栏： slam # 十四讲文章标签：三角测量本质矩阵基础矩阵单应矩阵

本文链接：https://blog.csdn.net/fb_941219/article/details/89761677

版权

slam 同时被 2 个专栏收录

70 篇文章 22 订阅

订阅专栏

十四讲

17 篇文章 2 订阅

订阅专栏

2D-2D:E2RT_Triangulation

引言

ch7.task5_3.当相机为单目时,只知道2D的像素坐标,因而问题是根据两组2D点估计运动。该问题用对极几何来解决。

1.2D-2D对极约束

对极约束：
在这里插入图片描述

实际求解步骤：

（1）根据已经匹配的像素点 $x_1,x_2$ 由公式 $x_2^Tt^{\wedge}Rx_1=0$ 使用八点法求解 $(t^{\wedge}R)_{3*3}$ 即求解本质矩阵 $E$ .
（2）根据本质矩阵 $E$ 使用奇异值分解(SVD)， $E=U\Sigma V^T$ 求出 $R, t$ .

由图可以看出，只有图(1)是真正的解。只需要把任意一点代入四种解中,检测该点在两个相机下的深度,就可以确定哪个解是正确的。对于单目初始化不能只有纯旋转,必须要有一定程度的平移。具体参考教材 $P_{155}$ .

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点法估算出基础矩阵 $F_i$
计算其余的点对到其对应对极线的距离 $d_n$ ，如果 $d_n≤d$ 则该点为内点，否则为外点。记下符合该条件的内点的个数为 $m_i$
迭代k次，或者某次得到内点的数目 $m_i$ 占有的比例大于等于95%，则停止。选择 $m_i$ 最大的基础矩阵作为最终的结果。

RANSAC能够不依赖于任何额外信息的情况下，将数据划分为内点和外点。但也有其相应的缺点，RANSAC并不能保证得到正确的结果，需要提高迭代的次数；另一个是，内点外点的判断需要设置阈值。

3.三角测量求深度

对于单目SLAM的地图点深度估计，依靠两帧匹配的图像实现(但是具有尺度不确定性)。
在这里插入图片描述

归一化平面： $s_1x_1=s_2Rx_2+t$ ,其中的 $R, t$ 已经知道。
左乘 $x_1^{\wedge}$ 得： $s_1x_1^{\wedge}x_1=0=s_2x_1^{\wedge}Rx_2+x_1^{\wedge}t$ ,根据右侧求解出 $s_2$ ，然后一次求出 $s_1$ 就得到了深度。
由于噪声的存在,估得的 R, t,不一定精确使式 $s_1x_1=s_2Rx_2+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) 
    );
}

在这里插入图片描述

古路

关注

1
点赞
踩
5

收藏

觉得还不错? 一键收藏
0
评论
2D-2D:E2RT_Triangulation

E2RT_Triangulation引言1.2D-2D对极约束2.八点法求解E3.三角测量求深度引言ch7.当相机为单目时,只知道2D的像素坐标,因而问题是根据两组2D点估计运动。该问题用对极几何来解决。1.2D-2D对极约束对极约束：实际求解步骤：（1）根据已经匹配的像素点x1,x2x_1,x_2x1,x2根据x2Tt∧Rx1=0x_2^Tt^{\wedge}Rx_1=0x...
复制链接

扫一扫

专栏目录