视觉SLAM——2D-2D：对极几何

一.实践：对极约束求解相机运动

slambook2/ch7/pose_estimation_2d2d.cpp

#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) {
    
    
  //-- 读取图像
  Mat img_1 = imread("../camera1left.png");
  Mat img_2 = imread("../camera1right.png");
  assert(img_1.data && img_2.data && "Can not 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;

  //-- 估计两张图像间运动
  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;

  //-- 从本质矩阵中恢复旋转和平移信息.
  // 此函数仅在Opencv3中提供
  recoverPose(essential_matrix, points1, points2, R, t, focal_length, principal_point);
  cout << "R is " << endl << R << endl;
  cout << "t is " << endl << t << endl;

}

运行结果:
输入1


输出1

/home/wenjing/slambook2-master/ch7/cmake-build-debug/pose_estimation_2d2d
-- Max dist : 92.000000 
-- Min dist : 7.000000 
一共找到了100组匹配点
fundamental_matrix is 
[-3.367783622325468e-07, -4.540218081033638e-05, 0.01512489128159875;
 4.227517932359297e-05, 1.076307687598131e-05, -0.0008410290624747567;
 -0.01348985397741287, -0.005627504817378393, 0.9999999999999999]
essential_matrix is 
[-0.005299619507464461, 0.4153259505042596, -0.2093379581162782;
 -0.3979155368885018, -0.005374073191384199, 0.5439040734436752;
 0.213049132031667, -0.5328462856313121, -0.01189517227409016]
homography_matrix is 
[1.104533926923398, 0.1337523923035233, -115.1477203655932;
 0.02323104934899613, 1.120849378755429, -45.59557477750322;
 1.652411470409963e-05, 0.0001751508477790184, 1]
R is 
[0.999607659183783, -0.001346216278803991, -0.0279770513608753;
 0.001657110479383161, 0.9999371059026738, 0.01109225963650354;
 0.02796035918899292, -0.01113426875529666, 0.9995470205914812]
t is 
[-0.7531983645262609;
 -0.3026638522567442;
 -0.5840263831472632]
t^R=
[-0.007494793782873279, 0.5873595920086204, -0.2960485794875313;
 -0.5627375489466907, -0.007600087192441429, 0.7691965172940176;
 0.3012969719709997, -0.75355844379993, -0.01682231395678243]
epipolar constraint = [-0.0005889047679175918]
epipolar constraint = [-0.0001970548744731071]
epipolar constraint = [-0.000191953128060196]
epipolar constraint = [3.897127214436136e-06]
epipolar constraint = [0.0004093279601083966]
epipolar constraint = [-0.0006044018394668893]
epipolar constraint = [-0.0002026623153718876]
epipolar constraint = [-8.399437281009225e-05]
epipolar constraint = [0.0003634800510686764]
epipolar constraint = [-0.000404504694922786]
epipolar constraint = [-0.0009939583901792254]
epipolar constraint = [0.0004492012433307824]
epipolar constraint = [-0.006607588514972366]
epipolar constraint = [0.002004179734347966]
epipolar constraint = [0.001883375280647748]
epipolar constraint = [-0.007092262782272832]
epipolar constraint = [-0.000310547705915945]
epipolar constraint = [-0.005417377344380008]
epipolar constraint = [3.432840433581497e-06]
epipolar constraint = [0.0001367164365111417]
epipolar constraint = [2.26693762489516e-06]
epipolar constraint = [0.0002036665524684511]
epipolar constraint = [0.0006732495773808522]
epipolar constraint = [0.0003946030911415599]
epipolar constraint = [0.0003854430482014282]
epipolar constraint = [0.0003693687091368691]
epipolar constraint = [0.0003532025706689534]
epipolar constraint = [-0.0001814291758014952]
epipolar constraint = [0.0001848590517768978]
epipolar constraint = [-0.000175842798932388]
epipolar constraint = [-0.00612788616759136]
epipolar constraint = [0.0002699808149908278]
epipolar constraint = [-0.0001433428491391942]
epipolar constraint = [0.0004070663092046178]
epipolar constraint = [0.001405374933404063]
epipolar constraint = [-1.806123111092184e-05]
epipolar constraint = [7.764665836046403e-05]
epipolar constraint = [0.0001330035073515146]
epipolar constraint = [0.0001789420603130198]
epipolar constraint = [0.01965437756392013]
epipolar constraint = [-0.007112332131945856]
epipolar constraint = [-0.0001943535380535544]
epipolar constraint = [-0.0003100004089579833]
epipolar constraint = [-9.081398665922169e-05]
epipolar constraint = [5.934808033966621e-05]
epipolar constraint = [-6.757730780865279e-05]
epipolar constraint = [5.513686263336481e-06]
epipolar constraint = [-0.0005015651034902258]
epipolar constraint = [0.001804032779915948]
epipolar constraint = [0.0004274288828822193]
epipolar constraint = [0.0002448536550603564]
epipolar constraint = [0.0001174706044919926]
epipolar constraint = [-0.0001253413825062222]
epipolar constraint = [-0.0001998051002025936]
epipolar constraint = [-0.007107516603840635]
epipolar constraint = [-0.0001595875298878446]
epipolar constraint = [3.490749177520458e-06]
epipolar constraint = [-0.0007825797147913982]
epipolar constraint = [0.0001252623822376175]
epipolar constraint = [-0.0002118678653436623]
epipolar constraint = [-0.0006014301126874289]
epipolar constraint = [-1.365489435489753e-05]
epipolar constraint = [-5.826909236561484e-06]
epipolar constraint = [0.001815879670416118]
epipolar constraint = [0.0001895411013570214]
epipolar constraint = [-0.0007029537110360695]
epipolar constraint = [-0.0001195570492571324]
epipolar constraint = [-1.73470258817976e-05]
epipolar constraint = [-5.2633303165045e-05]
epipolar constraint = [-0.001051756077362354]
epipolar constraint = [0.002182419518114695]
epipolar constraint = [-3.264669632171979e-05]
epipolar constraint = [-0.0014518129115344]
epipolar constraint = [-0.00161940805450568]
epipolar constraint = [-0.0050858711498038]
epipolar constraint = [0.002878344209746225]
epipolar constraint = [-0.0007576846521278602]
epipolar constraint = [0.0002464823715623843]
epipolar constraint = [0.0005206778268829204]
epipolar constraint = [0.0005030411928010903]
epipolar constraint = [0.0001988030645425076]
epipolar constraint = [-0.0003655110226506442]
epipolar constraint = [0.000347684197897602]
epipolar constraint = [-0.0003235270267764323]
epipolar constraint = [-0.0004655424638512207]
epipolar constraint = [-0.003066618491611994]
epipolar constraint = [-0.0002204640315995876]
epipolar constraint = [0.0006732860008179659]
epipolar constraint = [0.0005937857732712382]
epipolar constraint = [0.003148016951437016]
epipolar constraint = [0.0007650139404548062]
epipolar constraint = [0.0008101766972340718]
epipolar constraint = [0.0001008602218928734]
epipolar constraint = [0.0005673849632492067]
epipolar constraint = [0.0002741003906510386]
epipolar constraint = [0.0002573956456415061]
epipolar constraint = [3.456386660694033e-05]
epipolar constraint = [0.0008040110497321562]
epipolar constraint = [0.002941483682536453]
epipolar constraint = [0.0003038927142818837]

Process finished with exit code 0

输入2

输出2

/home/wenjing/slambook2-master/ch7/cmake-build-debug/pose_estimation_2d2d
-- Max dist : 95.000000 
-- Min dist : 4.000000 
一共找到了79组匹配点
fundamental_matrix is 
[4.844484382466111e-06, 0.0001222601840188731, -0.01786737827487386;
 -0.0001174326832719333, 2.122888800459598e-05, -0.01775877156212593;
 0.01799658210895528, 0.008143605989020664, 1]
essential_matrix is 
[-0.02036185505234771, -0.4007110038118444, -0.033240742498241;
 0.3939270778216368, -0.03506401846698084, 0.5857110303721015;
 -0.006788487241438231, -0.5815434272915687, -0.01438258684486259]
homography_matrix is 
[0.9497129583105288, -0.143556453147626, 31.20121878625771;
 0.04154536627445031, 0.9715568969832015, 5.306887618807696;
 -2.81813676978796e-05, 4.353702039810921e-05, 1]
R is 
[0.9985961798781877, -0.05169917220143662, 0.01152671359827862;
 0.05139607508976053, 0.9983603445075083, 0.02520051547522452;
 -0.01281065954813537, -0.02457271064688494, 0.9996159607036126]
t is 
[-0.8220841067933339;
 -0.0326974270640541;
 0.5684264241053518]
t^R=
[-0.02879601157010514, -0.5666909361828475, -0.0470095088643642;
 0.5570970160413605, -0.04958801046730488, 0.8283204827837457;
 -0.009600370724838811, -0.8224266019846685, -0.02034004937801358]
epipolar constraint = [0.002528128704106514]
epipolar constraint = [-0.001663727901710814]
epipolar constraint = [-0.0008009088410885212]
epipolar constraint = [0.0001705869410470254]
epipolar constraint = [-0.0003338015008984979]
epipolar constraint = [0.0003385525272308065]
epipolar constraint = [0.0001729349818584552]
epipolar constraint = [-9.552408320477601e-06]
epipolar constraint = [-0.0008834408754688165]
epipolar constraint = [-0.000444586092781381]
epipolar constraint = [-0.000156067923593961]
epipolar constraint = [0.001512967129777068]
epipolar constraint = [-0.0002644334828964742]
epipolar constraint = [-3.514414351252562e-06]
epipolar constraint = [-0.0004170632811044614]
epipolar constraint = [-0.0007749589896892117]
epipolar constraint = [0.002091463454860276]
epipolar constraint = [-0.001016195254389909]
epipolar constraint = [0.0005870797511206075]
epipolar constraint = [0.0002701337927295891]
epipolar constraint = [-0.0008153290073634545]
epipolar constraint = [0.005595329855570208]
epipolar constraint = [0.004456949384590653]
epipolar constraint = [-0.00109225844630996]
epipolar constraint = [-0.00122027527435923]
epipolar constraint = [0.0001206258081121597]
epipolar constraint = [-0.0007167735646266809]
epipolar constraint = [0.002034045481378033]
epipolar constraint = [-0.001256283161205782]
epipolar constraint = [-2.841028832301085e-06]
epipolar constraint = [-0.0009452266349239957]
epipolar constraint = [-0.0003143393086872948]
epipolar constraint = [-0.003421410506807192]
epipolar constraint = [0.0006511227496003788]
epipolar constraint = [-0.001310996225762057]
epipolar constraint = [-0.0001590978941137114]
epipolar constraint = [-0.002209031974010213]
epipolar constraint = [-0.0007840664639768846]
epipolar constraint = [-0.00219092181574767]
epipolar constraint = [0.002922765516590181]
epipolar constraint = [-0.0002852488690396338]
epipolar constraint = [0.001288650990044271]
epipolar constraint = [0.002981122529430141]
epipolar constraint = [0.001104024767925333]
epipolar constraint = [0.0005839797440089639]
epipolar constraint = [-0.002811645087152688]
epipolar constraint = [-0.001723388366795087]
epipolar constraint = [0.0001541481260837613]
epipolar constraint = [0.0006004071491191379]
epipolar constraint = [-0.001728591166312573]
epipolar constraint = [-0.0007782250239872224]
epipolar constraint = [-0.001075058873840032]
epipolar constraint = [0.004654782908027483]
epipolar constraint = [-0.00145774661621554]
epipolar constraint = [0.0003259585394422768]
epipolar constraint = [-9.139514634399354e-06]
epipolar constraint = [2.094089762112034e-06]
epipolar constraint = [-0.00122725042555329]
epipolar constraint = [-0.0008551935807612487]
epipolar constraint = [0.001650773210968584]
epipolar constraint = [0.00116044245491314]
epipolar constraint = [0.001879717958470126]
epipolar constraint = [-5.97742462742773e-06]
epipolar constraint = [-0.0003369336336238871]
epipolar constraint = [0.004360922097753794]
epipolar constraint = [-0.005310637569393865]
epipolar constraint = [-0.0006060103678098214]
epipolar constraint = [1.216121374464363e-06]
epipolar constraint = [-0.003401336870289186]
epipolar constraint = [0.002238878760289525]
epipolar constraint = [0.001475291883444502]
epipolar constraint = [-0.003206338609952966]
epipolar constraint = [-0.001462525388296471]
epipolar constraint = [-0.0007503932332671159]
epipolar constraint = [0.00384724837206624]
epipolar constraint = [6.646617176919722e-07]
epipolar constraint = [0.0007123827789497824]
epipolar constraint = [-0.0005911111586385312]
epipolar constraint = [-0.004921591124588801]

Process finished with exit code 0

二.从 E 恢复 R, t

CMakeLists.txt

cmake_minimum_required(VERSION 2.8)
project(E2Rt)

set(CMAKE_BUILD_TYPE "Release")
set(CMAKE_CXX_FLAGS "-std=c++11")

find_package(OpenCV 3 REQUIRED)
#find_package(G2O REQUIRED)
find_package(Sophus REQUIRED)

include_directories(
        ${OpenCV_INCLUDE_DIRS}
        ${Sophus_INCLUDE_DIRS}
        "/usr/include/eigen3/"
)

add_executable(E2Rt E2Rt.cpp)
target_link_libraries(E2Rt ${OpenCV_LIBS})

E2Rt.cpp

//
// Created by 高翔 on 2017/12/19.
// 本程序演示如何从Essential矩阵计算R,t
//

#include <Eigen/Core>
#include <Eigen/Dense>
#include <Eigen/Geometry>

using namespace Eigen;

#include <sophus/so3.hpp>

#include <iostream>

using namespace std;

int main(int argc, char **argv) {
    
    

    // 给定Essential矩阵
    Matrix3d E;
    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 sigular values
    // START YOUR CODE HERE
    JacobiSVD<MatrixXd> svd(E,ComputeThinU | ComputeThinV);
    Matrix3d U=svd.matrixU();
    Matrix3d V=svd.matrixV();
    VectorXd sigma_value=svd.singularValues();
    Matrix3d SIGMA=U.inverse()*E*V.transpose().inverse();
    Vector3d sigma_value2={
    
    (sigma_value[0]+sigma_value[1])/2,(sigma_value[0]+sigma_value[1])/2,0};
    Matrix3d SIGMA2=sigma_value2.asDiagonal();
    cout<<"SIGMA=\n"<<SIGMA<<endl;
    cout<<"sigma_value=\n"<<sigma_value<<endl;
    cout<<"SIGMA2=\n"<<SIGMA<<endl;
    cout<<"sigma_value2=\n"<<sigma_value<<endl;
    // END YOUR CODE HERE

    // set t1, t2, R1, R2 
    // START YOUR CODE HERE
    Matrix3d t_wedge1;
    Matrix3d t_wedge2;

    Matrix3d R1;
    Matrix3d R2;
    // END YOUR CODE HERE

    cout << "R1 = " << R1 << endl;
    cout << "R2 = " << R2 << endl;
    cout << "t1 = " << Sophus::SO3d::vee(t_wedge1) << endl;
    cout << "t2 = " << Sophus::SO3d::vee(t_wedge2) << endl;

    // check t^R=E up to scale
    Matrix3d tR = t_wedge1 * R1;
    cout << "t^R = " << tR << endl;

    return 0;
}

运行结果：

/home/wenjing/E2Rt/E2Rt/cmake-build-debug/E2Rt
SIGMA=
    0.707107  2.08167e-17 -1.66533e-16
-2.18575e-16     0.707107  3.46945e-17
 7.28828e-17 -6.03969e-17  9.47393e-17
sigma_value=
   0.707107
   0.707107
1.29353e-16
SIGMA2=
    0.707107  2.08167e-17 -1.66533e-16
-2.18575e-16     0.707107  3.46945e-17
 7.28828e-17 -6.03969e-17  9.47393e-17
sigma_value2=
   0.707107
   0.707107
1.29353e-16
R1 = 6.94218e-310 3.01789e-301 6.94218e-310
2.96439e-323            0 6.94218e-310
3.39519e-313 6.94218e-310 6.94218e-310
R2 = 6.94218e-310 6.94218e-310 6.94218e-310
6.94218e-310 6.94218e-310 6.94218e-310
6.94218e-310 6.94218e-310 6.94218e-310
t1 = 6.94218e-310
6.94218e-310
6.94218e-310
t2 = 6.94218e-310
6.94218e-310
6.94218e-310
t^R = 0 0 0
0 0 0
0 0 0

Process finished with exit code 0