视觉SLAM十四讲 第七章 对极约束

2D-2D：对极几何

注意：用的是自己标定好的相机

遇到的问题有：

1.CMakeLists.txt中找不到G2O和CSparse

find_package( G2O REQUIRED )
find_package( CSparse REQUIRED )

这是因为用的是第三方库，需要在cmake_modules中添加下面两个

2.运行程序的时候，出现下面这个错误

.

对着高博的程序检查，发现create括号里面写错一个字母

3.c++: error: unrecognized command line option ‘-03’

打开CMakeLists.txt，将“数字0”修改成“大写字母O”即可解决。

 

下面是代码

#include 
#include 
#include 
#include 
#include 

using namespace cv;
using namespace std;
/*************************
 * 2D-2D特征匹配估计相机运动
 * ***********************/
void find_feature_matches(
        const Mat& img_1,const Mat& img_2,
        std::vector& KeyPoint_1,
        std::vector& KeyPoint_2,
        std::vector& matches);

void pose_estimation_2d2d(
        std::vector& KeyPoint_1,
        std::vector& KeyPoint_2,
        std::vector& matches,
        Mat& R,Mat& t);
//像素坐标转相机归一化坐标
Point2d pixel2cam(const Point2d& p,const Mat& K);


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

      Mat img_1=imread("/home/xxx/Projects/slam14/pose_estimation_2d2d/img1.jpg",CV_LOAD_IMAGE_COLOR);
      Mat img_2=imread("/home/xxx/Projects/slam14/pose_estimation_2d2d/img2.jpg",CV_LOAD_IMAGE_COLOR);
//    Mat img_1=imread(argv[1],CV_LOAD_IMAGE_COLOR);
//    Mat img_2=imread(argv[2],CV_LOAD_IMAGE_COLOR);
    vector keypoints_1,keypoints_2;
    vectormatches;
    find_feature_matches(img_1,img_2,keypoints_1,keypoints_2,matches);
    cout<<"一共找到了"<(3,3)<<
                                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<<"E=t^R="<(3,3)<<813.8,0,345.0, 0,813.9,248.5,0,0,1);
    for(DMatch m:matches)
    {
        Point2d pt1=pixel2cam(keypoints_1[m.queryIdx].pt,K);
        Mat y1 =(Mat_(3,1)<(3,1)<& KeyPoint_1,
        std::vector& KeyPoint_2,
        std::vector& matches)
{
    Mat descriptors_1,descriptors_2;

    Ptrdetector=ORB::create();
    Ptrdescriptor=ORB::create();
    Ptrmatcher=DescriptorMatcher::create("BruteForce-Hamming");

    detector->detect(img_1,KeyPoint_1);
    detector->detect(img_2,KeyPoint_2);

    descriptor->compute(img_1,KeyPoint_1,descriptors_1);
    descriptor->compute(img_2,KeyPoint_2,descriptors_2);

    vectormatch;
    matcher->match(descriptors_1,descriptors_2,match);

    double min_dist=10000,max_dist=0;

    for(int i=0;imax_dist)max_dist=dist;
    }

    cout<<"--Max dist : "<(0,2))/K.at(0,0),
                (p.y-K.at(1,2))/K.at(1,1)
                );
}

void pose_estimation_2d2d(std::vector &KeyPoint_1, std::vector &KeyPoint_2, std::vector &matches, Mat &R, Mat &t)
{
    Mat K=(Mat_(3,3)<<813.8,0,345.0, 0,813.9,248.5,0,0,1);

    vectorpoint1;
    vectorpoint2;

    for(int i=0;i<(int)matches.size();i++)
    {
        point1.push_back(KeyPoint_1[matches[i].queryIdx].pt);
        point2.push_back(KeyPoint_2[matches[i].trainIdx].pt);
    }

    //F矩阵
    Mat fundamental_matrix;
    fundamental_matrix=findFundamentalMat(point1,point2,CV_FM_8POINT);
    cout<<"基础矩阵F是"<

CMakeLists.txt

cmake_minimum_required(VERSION 2.8)
project(pose_estimation_2d2d)

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

#添加cmake模块以使用g2o
list( APPEND CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake_modules )

find_package( OpenCV 3.4.3 REQUIRED )
find_package( G2O REQUIRED )
find_package( CSparse REQUIRED )

INCLUDE_DIRECTORIES(
    ${OpenCV_INCLUDE_DIRS}
    ${G2O_INCLUDE_DIRS}
    ${CSPARSE_INCLUDE_DIR}
    "/usr/include/eigen3")

add_executable(${PROJECT_NAME} "main.cpp")
target_link_libraries(${PROJECT_NAME} ${OpenCV_LIBS} )

运行结果

 第一组数据这里的对极约束的验证，精度和书上比差的有点大。后面才发现是选的图像问题，这两张图位移和旋转太大了，所以误差很大。

这一组数据就要好很多。

 

来自视觉SLAM十四讲 第七章