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

最新推荐文章于 2024-09-06 22:53:27 发布

我叫林克不叫塞尔达

最新推荐文章于 2024-09-06 22:53:27 发布

阅读量696

点赞数 1

分类专栏： SLAM

本文链接：https://blog.csdn.net/Doraemon_Zzn/article/details/89073808

版权

SLAM 专栏收录该内容

2 篇文章 0 订阅

订阅专栏

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 <iostream>
#include <opencv2/core/core.hpp>
#include <opencv2/features2d/features2d.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/calib3d/calib3d.hpp>

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

void pose_estimation_2d2d(
        std::vector<KeyPoint>& KeyPoint_1,
        std::vector<KeyPoint>& KeyPoint_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("/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<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);

    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<<"E=t^R="<<endl<<t_x*R<<endl;

    Mat K=(Mat_<double>(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_<double>(3,1)<<pt1.x,pt1.y,1);
        Point2d pt2=pixel2cam(keypoints_2[m.queryIdx].pt,K);
        Mat y2=(Mat_<double>(3,1)<<pt2.x,pt2.y,1);
        Mat d=y2.t()*t_x*R*y1;
        cout<<"验证这里的对极约束是否为0"<<d<<endl;
    }
    return 0;
}

void find_feature_matches(
        const Mat& img_1,const Mat& img_2,
        std::vector<KeyPoint>& KeyPoint_1,
        std::vector<KeyPoint>& KeyPoint_2,
        std::vector<DMatch>& matches)
{
    Mat descriptors_1,descriptors_2;

    Ptr<FeatureDetector>detector=ORB::create();
    Ptr<DescriptorExtractor>descriptor=ORB::create();
    Ptr<DescriptorMatcher>matcher=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);

    vector<DMatch>match;
    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;
    }

    cout<<"--Max dist : "<<max_dist<<endl;
    cout<<"--Min dist : "<<min_dist<<endl;

    for(int i=0;i<descriptors_1.rows;i++)
     {
        if(match[i].distance<=max(2*min_dist,30.0))
        {
            matches.push_back(match[i]);
        }
     }
}

//VSLAM14讲 P86页
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> &KeyPoint_1, std::vector<KeyPoint> &KeyPoint_2, std::vector<DMatch> &matches, Mat &R, Mat &t)
{
    Mat K=(Mat_<double>(3,3)<<813.8,0,345.0, 0,813.9,248.5,0,0,1);

    vector<Point2f>point1;
    vector<Point2f>point2;

    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是"<<endl<<fundamental_matrix<<endl;

    //E矩阵
    Point2d principal_point(345.0,248.5);
    double focal_length=813.8;
    Mat essential_matrix;
    essential_matrix=findEssentialMat(point1,point2,focal_length,principal_point);
    cout<<"本质矩阵E是"<<endl<<essential_matrix<<endl;

    //H矩阵
    Mat homography_matric;
    homography_matric=findHomography(point1,point2,RANSAC,3);
    cout<<"单应矩阵H是"<<endl<<homography_matric<<endl;

    //从本质矩阵恢复旋转与平移
    recoverPose(essential_matrix,point1,point2,R,t,focal_length,principal_point);
    cout<<"旋转矩阵R是"<<endl<<R<<endl;
    cout<<"平移矩阵t是"<<endl<<t<<endl;
}

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} )

运行结果