《视觉slam十四讲》初学小白笔记（8）

 对极约束

#include 
#include 
#include 
#include 
#include 
#include "extra.h"

using namespace std;
using namespace cv;

void find_feature_matches(const Mat& img_1,const Mat& img_2,
  std::vector& keypoints_1,
  std::vector& keypoints_2,
  std::vector& matches)
{
  //初始化
  Mat descriptors_1,descriptors_2;				//创建描述子
  Ptr detector = ORB::create("ORB");		//创建ORB特征点检测
  Ptr descriptor = ORB::create("ORB");	//使用ORB特征来描述特征点
  Ptr 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距离
  vectormatch;
  matcher->match(descriptors_1,descriptors_2,match);
  
  //第四步：匹配点对筛选
  double min_dist=10000,max_dist=0;
  for(int i=0;imax_dist) max_dist=dist;
  }
  
  //显示最大和最小距离
  printf("Max dist =%f\n",max_dist);
  printf("Min dist =%f\n",min_dist);
  
  // 当描述子之间的距离大于两倍的最小距离时,即认为匹配有误
  // 但有时候最小距离会非常小,设置一个经验值30作为下限
  for(int i=0;i keypoints_1,
  std::vector keypoints_2,
  std::vector matches,
  Mat& R,Mat& t)
{
  //相机内参
  double cx = 325.1;  // 像素坐标系与成像平面之间的原点平移
  double cy = 249.7;
  double fx = 520.9;  // 焦距
  double fy = 521.0;
  Mat K=(Mat_(3,3)<形式
  vector points1;
  vector points2;
  
  for(int i=0;i<(int)matches.size();i++){
    points1.push_back(keypoints_1[matches[i].queryIdx].pt);//queryIdx此匹配对应的查询图像的特征描述子索引 
    points2.push_back(keypoints_2[matches[i].trainIdx].pt);//trainIdx此匹配对应的训练(模板)图像的特征描述子索引 
  }
  
  //计算基础矩阵 F=K^(-T)EK^(-1)
  Mat fundamental_matrix;
  fundamental_matrix=findFundamentalMat(points1,points2,CV_FM_8POINT);
  cout<<"基础矩阵 = "<(0,2))/K.at(0,0),	//(x-cx)/fx
    (p.y-K.at(1,2))/K.at(1,1)	//(y-cy)/fy 像素坐标减去平移量后除以旋转值
  );
}

int main(int argc,char** argv){
  if(argc!=3){
    cout<<"usage:pose_estimation_2d2d img_1 img_2"< keypoints_1,keypoints_2;
  vector matches;
  find_feature_matches(img_1,img_2,keypoints_1,keypoints_2,matches);
  cout<<"一共找到了"<(3,3)<<			//t的反对称矩阵
    0,-t.at(2,0),t.at(1,0),
    t.at(2,0),0,-t.at(0,0),
    -t.at(1,0),t.at(0,0),0
  );
  cout<<"t^R="<(3, 3) << fx, 0, cx, 0, fy, cy, 0, 0, 1);

  //计算对极约束
  for(DMatch m:matches){
    Point2d pt1=pixel2cam(keypoints_1[m.queryIdx].pt,K);
    Mat y1=(Mat_(3,1)<(3,1)<

因为使用的是opencv2，所以要另外加上extra.cc 和extra.h文件

尤其实在CMakeLists.txt中，否则会报错

add_executable( pose_estimation_2d2d pose_estimation_2d2d.cc extra.cc )
target_link_libraries( pose_estimation_2d2d ${OpenCV_LIBS} )

运行结果

对极约束方程结果近似为0