精选SLAM第七讲实践：【前端1】对极约束求解相机运动，三角测量triangulation详细实现（单目2D-2D-估计深度）

老张高手

分类：建模仿真

发布时间 2023.06.02阅读数 1368 评论数 0

22.10.5

1对极约束求解相机运动

1 这里选择重新建一个文件包，新建CMakeLists.txt

里面有src目录，目录下放咱们的cpp文件以及图像1，2文件

cpp代码如下所示

#include <iostream>
using namespace std;
#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/features2d//features2d.hpp>
#include <opencv2/calib3d/calib3d.hpp>
#include <cstring>
// #include "extra.h" // use this if in OpenCV2

/****************************************************
 * 本程序演示了如何使用2D-2D的特征匹配估计相机运动
 * **************************************************/

string first_file = "../src/1.png";
string second_file = "../src/2.png";

void find_feature_matches(const cv::Mat& img_1, const cv::Mat& img_2,
                          std::vector<cv::KeyPoint>& keypoints_1, std::vector<cv::KeyPoint>& keypoints_2,
                          std::vector<cv::DMatch>& matches);

void pose_estimation_2d2d(std::vector<cv::KeyPoint>& keypoints_1, std::vector<cv::KeyPoint>& keypoints_2,
                          std::vector<cv::DMatch>& matches,
                          cv::Mat& R, cv::Mat& t);
// 像素坐标转相机归一化坐标
cv::Point2d pixel2cam (const cv::Point2d& p, const cv::Mat& K);


int main(int argc, char ** argv)
{
    //-- 读取图像
    cout << "读取src文件下的图片" << endl;
    cv::Mat img_1 = cv::imread(first_file,CV_LOAD_IMAGE_COLOR);
    cv::Mat img_2 = cv::imread(second_file, CV_LOAD_IMAGE_COLOR);
    assert(img_1.data != nullptr && img_2.data != nullptr);

    vector<cv::KeyPoint> keypoints_1, keypoints_2;
    vector<cv::DMatch> matches;
    find_feature_matches(img_1,img_2,keypoints_1,keypoints_2,matches);
    cout<<"一共找到了"<<matches.size() <<"组匹配点"<<endl;

    //-- 估计两张图像间运动
    cv::Mat R,t;
    pose_estimation_2d2d(keypoints_1,keypoints_2,matches,R,t);

    //-- 验证E=t^R*scale
    cv::Mat t_x = (cv::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;

    //-- 验证对极约束      // 相机内参
    cv::Mat K = (cv::Mat_<double>(3,3) << 520.9, 0, 325.1, 0, 521.0, 249.7, 0,0,1);
    for(cv::DMatch m:matches)
    {
        cv::Point2d pt1 = pixel2cam(keypoints_1[m.queryIdx].pt,K);
        cv::Mat y1 = (cv::Mat_<double>(3,1) << pt1.x,pt1.y,1);
        cv::Point2d pt2 = pixel2cam ( keypoints_2[ m.trainIdx ].pt, K );
        cv::Mat y2 = ( cv::Mat_<double> ( 3,1 ) << pt2.x, pt2.y, 1 );
        cv::Mat d = y2.t() * t_x * R * y1;
        cout << "epipolar constraint = " << d << endl;
    }
    return 0;
}

void find_feature_matches(const cv::Mat& img_1, const cv::Mat& img_2,
                          std::vector<cv::KeyPoint>& keypoints_1, std::vector<cv::KeyPoint>& keypoints_2,
                          std::vector<cv::DMatch>& matches)
{
    //-- 初始化
    cv::Mat descriptors_1, descriptors_2;
    // used in OpenCV3
    cv::Ptr<cv::FeatureDetector> detector = cv::ORB::create();
    cv::Ptr<cv::DescriptorExtractor> descriptor = cv::ORB::create();

    // use this if you are in OpenCV2
    // Ptr<FeatureDetector> detector = FeatureDetector::create ( "ORB" );
    // Ptr<DescriptorExtractor> descriptor = DescriptorExtractor::create ( "ORB" );

    cv::Ptr<cv::DescriptorMatcher> matcher = cv::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<cv::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;
    }
    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]);
        }
    }
}

// 像素坐标转相机归一化坐标
cv::Point2d pixel2cam (const cv::Point2d& p, const cv::Mat& K)
{
    return cv::Point2d(   //at是内参数矩阵
            (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<cv::KeyPoint>& keypoints_1, std::vector<cv::KeyPoint>& keypoints_2,
                          std::vector<cv::DMatch>& matches,
                          cv::Mat& R, cv::Mat& t)
{
    // 相机内参,TUM Freiburg2
    cv::Mat K = (cv::Mat_<double>(3,3) << 520.9, 0, 325.1, 0, 521.0, 249.7, 0, 0, 1);

    //-- 把匹配点转换为vector<Point2f>的形式
    vector<cv::Point2f> points1;
    vector<cv::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);//匹配点对中第二张图片上的点
    }
    //-- 计算基础矩阵
    cv::Mat fundamental_matrix;
    fundamental_matrix = cv::findFundamentalMat(points1, points2,CV_FM_8POINT);//计算给定一组对应点的基本矩阵 八点法
    cout << "fundamental_matrix is" << endl << fundamental_matrix << endl;

    //-- 计算本质矩阵
    cv::Point2d principal_point (325.1, 249.7); 	//相机光心, TUM dataset标定值
    double focal_length = 521; 	//相机焦距, TUM dataset标定值
    cv::Mat essential_matrix;
    essential_matrix = cv::findEssentialMat(points1,points2,focal_length,principal_point);
    cout<<"essential_matrix is "<<endl<< essential_matrix<<endl;

    //-- 计算单应矩阵
    cv::Mat homography_matrix;
    homography_matrix = cv::findHomography(points1, points2, cv::RANSAC, 3);
    cout<<"homography_matrix is "<<endl<<homography_matrix<<endl;

    //-- 从本质矩阵中恢复旋转和平移信息.
    cv::recoverPose(essential_matrix, points1, points2, R,t,
                    focal_length,principal_point);
    cout<<"R is "<<endl<<R<<endl;
    cout<<"t is "<<endl<<t<<endl;
}

2 外层slambook2中ch7的cmake文件夹(里面有个findg2o的cmake文件）拖到新建的文件包内

cmakelists代码

cmake_minimum_required(VERSION 2.8)
project(pose_estimation_2d2d)

set(CMAKE_BUILD_TYPE "Release")
set( CMAKE_CXX_STANDARD 14)

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

find_package(OpenCV 3.1 REQUIRED)
# find_package( OpenCV REQUIRED ) # use this if in OpenCV2
include_directories("/usr/include/eigen3")
include_directories(${OpenCV_INCLUDE_DIRS})

add_executable(pose_estimation_2d2d src/pose_estimation_2d2d.cpp)

target_link_libraries(pose_estimation_2d2d ${OpenCV_LIBRARIES})

3 组合拳

mkdir build
cd build
cmake ..
make

4`./pose_estimation_2d2d`

实现成功！

2 triangulation三角测量

其实也就是在对极约束的条件上加个三角测量深度
建包和上面类似，就不再细说了

1 triangulation.cpp

#include <iostream>
using namespace std;
#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/features2d//features2d.hpp>
#include <opencv2/calib3d/calib3d.hpp>
#include <cstring>
// #include "extra.h" // use this if in OpenCV2

/****************************************************
 * 本程序演示了如何使用2D-2D的特征匹配估计相机运动
 * **************************************************/

string first_file = "../src/1.png";
string second_file = "../src/2.png";

void find_feature_matches(const cv::Mat& img_1, const cv::Mat& img_2,
                          std::vector<cv::KeyPoint>& keypoints_1, std::vector<cv::KeyPoint>& keypoints_2,
                          std::vector<cv::DMatch>& matches);

void pose_estimation_2d2d(std::vector<cv::KeyPoint>& keypoints_1, std::vector<cv::KeyPoint>& keypoints_2,
                          std::vector<cv::DMatch>& matches,
                          cv::Mat& R, cv::Mat& t);
// 像素坐标转相机归一化坐标
cv::Point2d pixel2cam (const cv::Point2d& p, const cv::Mat& K);

//加入了三角测量部分
void triangulation(const vector<cv::KeyPoint>& keypoint_1, const vector<cv::KeyPoint>& keypoint_2,
                   const std::vector<cv::DMatch>& matches, const cv::Mat& R, const cv::Mat& t,
                   vector<cv::Point3d>& points);


int main(int argc, char ** argv)
{
    //-- 读取图像
    cout << "读取src文件下的图片" << endl;
    cv::Mat img_1 = cv::imread(first_file,CV_LOAD_IMAGE_COLOR);
    cv::Mat img_2 = cv::imread(second_file, CV_LOAD_IMAGE_COLOR);
    assert(img_1.data != nullptr && img_2.data != nullptr);

    vector<cv::KeyPoint> keypoints_1, keypoints_2;
    vector<cv::DMatch> matches;
    find_feature_matches(img_1,img_2,keypoints_1,keypoints_2,matches);
    cout<<"一共找到了"<<matches.size() <<"组匹配点"<<endl;

    //-- 估计两张图像间运动
    cv::Mat R,t;
    pose_estimation_2d2d(keypoints_1,keypoints_2,matches,R,t);

    //-- 三角化
    vector<cv::Point3d> points;
    triangulation(keypoints_1,keypoints_2,matches,R,t,points);

    //-- 验证三角化点与特征点的重投影关系
    cv::Mat K = (cv::Mat_<double>(3,3) << 520.9, 0, 325.1, 0, 521.0, 249.7, 0, 0, 1 );
    for (int i = 0; i < matches.size(); i++)
    {
        cv::Point2d pt1_cam = pixel2cam(keypoints_1[matches[i].queryIdx].pt,K);
        cv::Point2d pt1_cam_3d(points[i].x/points[i].z, points[i].y/points[i].z);

        cout<<"point in the first camera frame: "<<pt1_cam<<endl;
        cout<<"point projected from 3D "<<pt1_cam_3d<<", d="<<points[i].z<<endl;

        // 第二个图
        cv::Point2f pt2_cam = pixel2cam(keypoints_2[matches[i].trainIdx].pt,K);
        cv::Mat pt2_trans = R * (cv::Mat_<double>(3,1) << points[i].x,points[i].y,points[i].z) + t;
        pt2_trans /= pt2_trans.at<double>(2,0);
        cout<<"point in the second camera frame: "<<pt2_cam<<endl;
        cout<<"point reprojected from second frame: "<<pt2_trans.t()<<endl;
        cout<<endl;
    }


    return 0;
}

void find_feature_matches(const cv::Mat& img_1, const cv::Mat& img_2,
                          std::vector<cv::KeyPoint>& keypoints_1, std::vector<cv::KeyPoint>& keypoints_2,
                          std::vector<cv::DMatch>& matches)
{
    //-- 初始化
    cv::Mat descriptors_1, descriptors_2;
    // used in OpenCV3
    cv::Ptr<cv::FeatureDetector> detector = cv::ORB::create();
    cv::Ptr<cv::DescriptorExtractor> descriptor = cv::ORB::create();

    // use this if you are in OpenCV2
    // Ptr<FeatureDetector> detector = FeatureDetector::create ( "ORB" );
    // Ptr<DescriptorExtractor> descriptor = DescriptorExtractor::create ( "ORB" );

    cv::Ptr<cv::DescriptorMatcher> matcher = cv::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<cv::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;
    }
    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]);
        }
    }
}

// 像素坐标转相机归一化坐标
cv::Point2d pixel2cam (const cv::Point2d& p, const cv::Mat& K)
{
    return cv::Point2d(   //at是内参数矩阵
            (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<cv::KeyPoint>& keypoints_1, std::vector<cv::KeyPoint>& keypoints_2,
                          std::vector<cv::DMatch>& matches,
                          cv::Mat& R, cv::Mat& t)
{
    // 相机内参,TUM Freiburg2
    cv::Mat K = (cv::Mat_<double>(3,3) << 520.9, 0, 325.1, 0, 521.0, 249.7, 0, 0, 1);

    //-- 把匹配点转换为vector<Point2f>的形式
    vector<cv::Point2f> points1;
    vector<cv::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);//匹配点对中第二张图片上的点
    }
    //-- 计算基础矩阵
    cv::Mat fundamental_matrix;
    fundamental_matrix = cv::findFundamentalMat(points1, points2,CV_FM_8POINT);//计算给定一组对应点的基本矩阵 八点法
    cout << "fundamental_matrix is" << endl << fundamental_matrix << endl;

    //-- 计算本质矩阵
    cv::Point2d principal_point (325.1, 249.7); 	//相机光心, TUM dataset标定值
    double focal_length = 521; 	//相机焦距, TUM dataset标定值
    cv::Mat essential_matrix;
    essential_matrix = cv::findEssentialMat(points1,points2,focal_length,principal_point);
    cout<<"essential_matrix is "<<endl<< essential_matrix<<endl;

    //-- 计算单应矩阵
    cv::Mat homography_matrix;
    homography_matrix = cv::findHomography(points1, points2, cv::RANSAC, 3);
    cout<<"homography_matrix is "<<endl<<homography_matrix<<endl;

    //-- 从本质矩阵中恢复旋转和平移信息.
    cv::recoverPose(essential_matrix, points1, points2, R,t,
                    focal_length,principal_point);
    cout<<"R is "<<endl<<R<<endl;
    cout<<"t is "<<endl<<t<<endl;
}

//加入了三角测量部分
void triangulation(const vector<cv::KeyPoint>& keypoint_1, const vector<cv::KeyPoint>& keypoint_2,
                   const std::vector<cv::DMatch>& matches, const cv::Mat& R, const cv::Mat& t,
                   vector<cv::Point3d>& points)
{
    cv::Mat T1 = (cv::Mat_<float> (3,4) <<
            1,0,0,0,
            0,1,0,0,
            0,0,1,0);
    cv::Mat T2 = (cv::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));

    cv::Mat K = (cv::Mat_<double> (3,3) << 520.9, 0, 325.1, 0, 521.0, 249.7, 0, 0, 1);
    vector<cv::Point2f> pts_1,pts_2;
    for(cv::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));
    }

    cv::Mat pts_4d;
    //第一个相机的3x4投影矩阵。
    //第2个相机的3x4投影矩阵。
    cv::triangulatePoints(T1,T2,pts_1,pts_2,pts_4d);

    // 转换成非齐次坐标
    for(int i = 0; i < pts_4d.cols; i++)
    {
        cv::Mat x = pts_4d.col(i);
        x /= x.at<float>(3,0);// 归一化
        cv::Point3d p(x.at<float>(0,0), x.at<float>(1,0), x.at<float>(2,0));
        points.push_back(p);
    }
}

2 CMakeLists.txt

cmake_minimum_required(VERSION 2.8)
project(triangulation)

set(CMAKE_BUILD_TYPE "Release")
set( CMAKE_CXX_STANDARD 14)

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

find_package(OpenCV 3.1 REQUIRED)
# find_package( OpenCV REQUIRED ) # use this if in OpenCV2
include_directories("/usr/include/eigen3")
include_directories(${OpenCV_INCLUDE_DIRS})

add_executable(triangulation src/triangulation.cpp)

target_link_libraries(triangulation ${OpenCV_LIBRARIES})

3 输出

实现成功！

机器视觉 SLAM 图像处理自动驾驶三角测量

打赏 0

上一篇：SLAM第六讲实践： gaussNewton ，ceres库，g2o库, g2o_viewer可视化优化超详细安装与使用

下一篇：SLAM第九讲实践：【后端1】基于slambook2 的ceres的BA优化以及g2o的BA优化

精选SLAM第七讲实践：【前端1】对极约束求解相机运动，三角测量triangulation详细实现（单目2D-2D-估计深度）

老张高手

1对极约束求解相机运动

1 这里选择重新建一个文件包，新建CMakeLists.txt

2 外层slambook2中ch7的cmake文件夹(里面有个findg2o的cmake文件）拖到新建的文件包内

3 组合拳

4./pose_estimation_2d2d

2 triangulation三角测量

1 triangulation.cpp

2 CMakeLists.txt

3 输出

为你推荐

Ardupilot固件之Flightgear模拟仿真

系统辨识专题（三）——多旋翼飞行器模型参数辨识问题

[1](含源码)通过关节力矩指令控制LBR/iiwa机械臂运动

(13) 期望最大化算法 Expectation Maximum : K-均值聚类，混合高斯 - PRML && CS229

特征法前端（二）

如何获得gazebo仿真中的机器人位姿真值

评论（0）

关于作者

老张高手

19

0

100

2

ROS入门跟着我就够了（五） ROS常用组件

【ORB_SLAM2】 ubuntu18.04下从笔记本调换到usb摄像头

SLAM第三讲实践：相机位姿 VisualizeGeometry的实现（Pangolin安装以及cpp实现）

相关推荐

cartographer探秘第四章之代码解析（七）--- pose_extrapolator.h 及 imu_tracker.h

关于机器人状态估计(3)-卡尔曼滤波

基于车辆运动学横纵向协同的模型预测控制MPC

Simulink系列 —— S-function的使用笔记（1）

matlab：绘制box函数和高斯函数曲线并进行傅里叶变换

Webots 机器人仿真平台(八) 添加GPS传感器

热门泡泡

30积分 失眠，聊聊自己搞ROS的心得体会吧

ros学习路线

30积分 TF_REPEATED_DATA ignoring data错误

各位大佬，有什么ROS定位算法推荐吗

5积分 想买能用ROS2的开发套件。或者开发板

5积分 ros中启动gazebo时报错

给作者打赏

忘记密码

修改头像

添加你感兴趣的标签

举报类型（必选）

举报详情（选填）

4`./pose_estimation_2d2d`

30积分失眠，聊聊自己搞ROS的心得体会吧

5积分想买能用ROS2的开发套件。或者开发板