【嵌入式应用9】三角测量和pnp，以及kdeveop的使用

最新推荐文章于 2022-09-28 21:33:40 发布
最新推荐文章于 2022-09-28 21:33:40 发布
阅读量317 收藏 1
点赞数
分类专栏： 嵌入式应用
版权声明：本文为博主原创文章，遵循 CC 4.0 BY-SA 版权协议，转载请附上原文出处链接和本声明。
本文链接：https://blog.csdn.net/qq_34591921/article/details/106082544
版权

目录

1.三角测量

triangulation.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" // used in opencv2 
using namespace std;
using namespace cv;
 
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 (
    const std::vector<KeyPoint>& keypoints_1,
    const std::vector<KeyPoint>& keypoints_2,
    const std::vector< DMatch >& matches,
    Mat& R, Mat& t );
 
void triangulation (
    const vector<KeyPoint>& keypoint_1,
    const vector<KeyPoint>& keypoint_2,
    const std::vector< DMatch >& matches,
    const Mat& R, const Mat& t,
    vector<Point3d>& points
);
 
// 像素坐标转相机归一化坐标
Point2f pixel2cam( const Point2d& p, const Mat& K );
 
int main ( int argc, char** argv )
{
    if ( argc != 3 )
    {
        cout<<"usage: triangulation img1 img2"<<endl;
        return 1;
    }
    //-- 读取图像
    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 );
 
    //-- 三角化
    vector<Point3d> points;
    triangulation( keypoints_1, keypoints_2, matches, R, t, points );
    
    //-- 验证三角化点与特征点的重投影关系
    Mat K = ( 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++ )
    {
        Point2d pt1_cam = pixel2cam( keypoints_1[ matches[i].queryIdx ].pt, K );
        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;
        
        // 第二个图
        Point2f pt2_cam = pixel2cam( keypoints_2[ matches[i].trainIdx ].pt, K );
        Mat pt2_trans = R*( 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 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] );
        }
    }
}
 
void pose_estimation_2d2d (
    const std::vector<KeyPoint>& keypoints_1,
    const std::vector<KeyPoint>& keypoints_2,
    const 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标定值
    int 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;
 
    //-- 从本质矩阵中恢复旋转和平移信息.
    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< KeyPoint >& keypoint_1, 
    const vector< KeyPoint >& keypoint_2, 
    const std::vector< DMatch >& matches,
    const Mat& R, const Mat& t, 
    vector< Point3d >& points )
{
    Mat T1 = (Mat_<float> (3,4) <<
        1,0,0,0,
        0,1,0,0,
        0,0,1,0);
    Mat T2 = (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)
    );
    
    Mat K = ( Mat_<double> ( 3,3 ) << 520.9, 0, 325.1, 0, 521.0, 249.7, 0, 0, 1 );
    vector<Point2f> pts_1, pts_2;
    for ( 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) );
    }
    
    Mat pts_4d;
    cv::triangulatePoints( T1, T2, pts_1, pts_2, pts_4d );
    
    // 转换成非齐次坐标
    for ( int i=0; i<pts_4d.cols; i++ )
    {
        Mat x = pts_4d.col(i);
        x /= x.at<float>(3,0); // 归一化
        Point3d p (
            x.at<float>(0,0), 
            x.at<float>(1,0), 
            x.at<float>(2,0) 
        );
        points.push_back( p );
    }
}
 
Point2f pixel2cam ( const Point2d& p, const Mat& K )
{
    return Point2f
    (
        ( p.x - K.at<double>(0,2) ) / K.at<double>(0,0), 
        ( p.y - K.at<double>(1,2) ) / K.at<double>(1,1) 
    );
}

cmakelist.cpp
注意opencv路径是安装时那个安装包的路径～

cmake_minimum_required( VERSION 2.8 )
project( imageBasics )
 
# 添加c++ 11标准支持
set( CMAKE_CXX_FLAGS "-std=c++11" )
 
# 寻找OpenCV库
set(OpenCV_DIR /media/ufo/存储/qq下载的/ufoo/opencv-3.4.1/)
find_package( OpenCV 3 REQUIRED )
# 添加头文件
include_directories( ${OpenCV_INCLUDE_DIRS} )
 
add_executable( triangulation triangulation.cpp )
# 链接OpenCV库
target_link_libraries( triangulation ${OpenCV_LIBS} )

编译：

使用develop导入对应的文件夹，build：
下载安装develop直接用apt-get快一些～

sudo apt-get install kdevelop

在这里插入图片描述

运行：

./build/triangulation 0.png 1.png

效果：

在这里插入图片描述

2.pnp

pose_estimation_3d2d.cpp

#include <iostream>
#include <opencv2/core/core.hpp>
#include <opencv2/features2d/features2d.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/calib3d/calib3d.hpp>
#include <Eigen/Core>
#include <Eigen/Geometry>
#include <g2o/core/base_vertex.h>
#include <g2o/core/base_unary_edge.h>
#include <g2o/core/block_solver.h>
#include <g2o/core/optimization_algorithm_levenberg.h>
#include <g2o/solvers/csparse/linear_solver_csparse.h>
#include <g2o/types/sba/types_six_dof_expmap.h>
#include <chrono>
 
using namespace std;
using namespace cv;
 
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 );
 
// 像素坐标转相机归一化坐标
Point2d pixel2cam ( const Point2d& p, const Mat& K );
 
void bundleAdjustment (
    const vector<Point3f> points_3d,
    const vector<Point2f> points_2d,
    const Mat& K,
    Mat& R, Mat& t
);
 
int main ( int argc, char** argv )
{
    if ( argc != 5 )
    {
        cout<<"usage: pose_estimation_3d2d img1 img2 depth1 depth2"<<endl;
        return 1;
    }
    //-- 读取图像
    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;
 
    // 建立3D点
    Mat d1 = imread ( argv[3], CV_LOAD_IMAGE_UNCHANGED );       // 深度图为16位无符号数，单通道图像
    Mat K = ( Mat_<double> ( 3,3 ) << 520.9, 0, 325.1, 0, 521.0, 249.7, 0, 0, 1 );
    vector<Point3f> pts_3d;
    vector<Point2f> pts_2d;
    for ( DMatch m:matches )
    {
        ushort d = d1.ptr<unsigned short> (int ( keypoints_1[m.queryIdx].pt.y )) [ int ( keypoints_1[m.queryIdx].pt.x ) ];
        if ( d == 0 )   // bad depth
            continue;
        float dd = d/5000.0;
        Point2d p1 = pixel2cam ( keypoints_1[m.queryIdx].pt, K );
        pts_3d.push_back ( Point3f ( p1.x*dd, p1.y*dd, dd ) );
        pts_2d.push_back ( keypoints_2[m.trainIdx].pt );
    }
 
    cout<<"3d-2d pairs: "<<pts_3d.size() <<endl;
 
    Mat r, t;
    solvePnP ( pts_3d, pts_2d, K, Mat(), r, t, false ); // 调用OpenCV 的 PnP 求解，可选择EPNP，DLS等方法
    Mat R;
    cv::Rodrigues ( r, R ); // r为旋转向量形式，用Rodrigues公式转换为矩阵
 
    cout<<"R="<<endl<<R<<endl;
    cout<<"t="<<endl<<t<<endl;
 
    cout<<"calling bundle adjustment"<<endl;
 
    bundleAdjustment ( pts_3d, pts_2d, K, R, t );
}
 
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 bundleAdjustment (
    const vector< Point3f > points_3d,
    const vector< Point2f > points_2d,
    const Mat& K,
    Mat& R, Mat& t )
{
    // 初始化g2o
    typedef g2o::BlockSolver< g2o::BlockSolverTraits<6,3> > Block;  // pose 维度为 6, landmark 维度为 3
    Block::LinearSolverType* linearSolver = new g2o::LinearSolverCSparse<Block::PoseMatrixType>(); // 线性方程求解器
    Block* solver_ptr = new Block ( linearSolver );     // 矩阵块求解器
    g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg ( solver_ptr );
    g2o::SparseOptimizer optimizer;
    optimizer.setAlgorithm ( solver );
 
    // vertex
    g2o::VertexSE3Expmap* pose = new g2o::VertexSE3Expmap(); // camera pose
    Eigen::Matrix3d R_mat;
    R_mat <<
          R.at<double> ( 0,0 ), R.at<double> ( 0,1 ), R.at<double> ( 0,2 ),
               R.at<double> ( 1,0 ), R.at<double> ( 1,1 ), R.at<double> ( 1,2 ),
               R.at<double> ( 2,0 ), R.at<double> ( 2,1 ), R.at<double> ( 2,2 );
    pose->setId ( 0 );
    pose->setEstimate ( g2o::SE3Quat (
                            R_mat,
                            Eigen::Vector3d ( t.at<double> ( 0,0 ), t.at<double> ( 1,0 ), t.at<double> ( 2,0 ) )
                        ) );
    optimizer.addVertex ( pose );
 
    int index = 1;
    for ( const Point3f p:points_3d )   // landmarks
    {
        g2o::VertexSBAPointXYZ* point = new g2o::VertexSBAPointXYZ();
        point->setId ( index++ );
        point->setEstimate ( Eigen::Vector3d ( p.x, p.y, p.z ) );
        point->setMarginalized ( true ); // g2o 中必须设置 marg 参见第十讲内容
        optimizer.addVertex ( point );
    }
 
    // parameter: camera intrinsics
    g2o::CameraParameters* camera = new g2o::CameraParameters (
        K.at<double> ( 0,0 ), Eigen::Vector2d ( K.at<double> ( 0,2 ), K.at<double> ( 1,2 ) ), 0
    );
    camera->setId ( 0 );
    optimizer.addParameter ( camera );
 
    // edges
    index = 1;
    for ( const Point2f p:points_2d )
    {
        g2o::EdgeProjectXYZ2UV* edge = new g2o::EdgeProjectXYZ2UV();
        edge->setId ( index );
        edge->setVertex ( 0, dynamic_cast<g2o::VertexSBAPointXYZ*> ( optimizer.vertex ( index ) ) );
        edge->setVertex ( 1, pose );
        edge->setMeasurement ( Eigen::Vector2d ( p.x, p.y ) );
        edge->setParameterId ( 0,0 );
        edge->setInformation ( Eigen::Matrix2d::Identity() );
        optimizer.addEdge ( edge );
        index++;
    }
 
    chrono::steady_clock::time_point t1 = chrono::steady_clock::now();
    optimizer.setVerbose ( true );
    optimizer.initializeOptimization();
    optimizer.optimize ( 100 );
    chrono::steady_clock::time_point t2 = chrono::steady_clock::now();
    chrono::duration<double> time_used = chrono::duration_cast<chrono::duration<double>> ( t2-t1 );
    cout<<"optimization costs time: "<<time_used.count() <<" seconds."<<endl;
 
    cout<<endl<<"after optimization:"<<endl;
    cout<<"T="<<endl<<Eigen::Isometry3d ( pose->estimate() ).matrix() <<endl;
}

CMakeLists.txt

cmake_minimum_required( VERSION 2.8 )
project( imageBasics )
 list( APPEND CMAKE_MODULE_PATH /home/ufo/嵌入式/week12/g2o/g2o/cmake_modules )
# 添加c++ 11标准支持
set( CMAKE_CXX_FLAGS "-std=c++11" )
 
# 寻找OpenCV库
set(OpenCV_DIR /media/ufo/存储/qq下载的/ufoo/opencv-3.4.1/)
set(CSparse /home/ufo/嵌入式/week12/g2o/g2oo) 
find_package( OpenCV 3 REQUIRED )
find_package( CSparse REQUIRED )
include_directories( ${CSPARSE_INCLUDE_DIR} )


# 添加头文件
include_directories( ${OpenCV_INCLUDE_DIRS}  ${CSPARSE_INCLUDE_DIR})
include_directories("/usr/include/eigen3")
include_directories("/usr/local/include/g2o/EXTERNAL/csparse")
add_executable( pose_estimation_3d2d pose_estimation_3d2d.cpp )
# 链接OpenCV库
target_link_libraries( pose_estimation_3d2d ${OpenCV_LIBS} ${G2O_LIBS} ${CSPARSE_LIBRARY})

遇到错误：
在这里插入图片描述
重装降级G2O～并没什么用～
可能因为cmakelist不熟悉，所以一直报错
于是参考大佬代码，没用上面那个代码，改用下面这种代码：
在cpp同级目录放一些cmake_modules：
在这里插入图片描述
pose_estimation_3d2d.cpp：

#include <iostream>
#include <opencv2/core/core.hpp>
#include <opencv2/features2d/features2d.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/calib3d/calib3d.hpp>
#include <Eigen/Core>
#include <Eigen/Geometry>
#include <g2o/core/base_vertex.h>
#include <g2o/core/base_unary_edge.h>
#include <g2o/core/block_solver.h>
#include <g2o/core/optimization_algorithm_levenberg.h>
#include <g2o/solvers/csparse/linear_solver_csparse.h>
#include <g2o/types/sba/types_six_dof_expmap.h>
#include <chrono>

using namespace std;
using namespace cv;

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

// 像素坐标转相机归一化坐标
Point2d pixel2cam ( const Point2d& p, const Mat& K );

int main ( int argc, char** argv )
{
    if ( argc != 4 )
    {
        cout<<"usage: pose_estimation_3d2d img1 img2 depth1"<<endl;
        return 1;
    }
    //-- 读取图像
    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;

    // 建立3D点
    Mat d1 = imread ( argv[3], CV_LOAD_IMAGE_UNCHANGED );       // 深度图为16位无符号数，单通道图像
    Mat K = ( Mat_<double> ( 3,3 ) << 520.9, 0, 325.1, 0, 521.0, 249.7, 0, 0, 1 );
    vector<Point3f> pts_3d;
    vector<Point2f> pts_2d;
    for ( DMatch m:matches )
    {
        ushort d = d1.ptr<unsigned short> (int ( keypoints_1[m.queryIdx].pt.y )) [ int ( keypoints_1[m.queryIdx].pt.x ) ];
        if ( d == 0 )   // bad depth
            continue;
        float dd = d/5000.0;
        Point2d p1 = pixel2cam ( keypoints_1[m.queryIdx].pt, K );
        pts_3d.push_back ( Point3f ( p1.x*dd, p1.y*dd, dd ) );
        pts_2d.push_back ( keypoints_2[m.trainIdx].pt );
    }

    cout<<"3d-2d pairs: "<<pts_3d.size() <<endl;

    Mat r, t;
    solvePnP ( pts_3d, pts_2d, K, Mat(), r, t, false ); // 调用OpenCV 的 PnP 求解，可选择EPNP，DLS等方法
    Mat R;
    cv::Rodrigues ( r, R ); // r为旋转向量形式，用Rodrigues公式转换为矩阵

    cout<<"R="<<endl<<R<<endl;
    cout<<"t="<<endl<<t<<endl;
}

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

CMakeLists.txt：

cmake_minimum_required(VERSION 2.8)
project(test)
list(APPEND CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake_modules)
set(CMAKE_CXX_STANDARD 11)
SET( G2O_ROOT /usr/local/include/g2o )
find_package( OpenCV REQUIRED )
find_package(Eigen3  REQUIRED)
find_package(G2O REQUIRED)
find_package( CSparse)
include_directories(
${EIGEN3_INCLUDE_DIR}
${OpenCV_INCLUDE_DIRS}
${G2O_INCLUDE_DIRS} 
${CSPARSE_INCLUDE_DIR} 
)
add_executable(pose_estimation_3d2d pose_estimation_3d2d.cpp)
target_link_libraries( pose_estimation_3d2d ${EIGEN3_LIB} ${OpenCV_LIBS} ${G2O_LIB} ${CSPARSE_LIB})
target_link_libraries(pose_estimation_3d2d g2o_core g2o_stuff)

编译

kdevelop build一下：
在这里插入图片描述

运行：

 ./build/pose_estimation_3d2d 0.png 1.png 1.png

效果：

在这里插入图片描述
源代码：https://github.com/2575829997/Ai-and-embedded
参考：
pnp：https://blog.csdn.net/weixin_36448497/article/details/82288818
g2o：https://blog.csdn.net/slzlincent/article/details/86555166

Ufoo360
关注
  • 0
    点赞
  • 踩
  • 1
    收藏
    觉得还不错? 一键收藏
  • 0
    评论
专栏目录
【视觉里程计】对极几何，三角测量，PnP，ICP原理
WaitingfoDreams的博客
09-22 2548
对于视觉里程计的一些个人理解。
VSLAM基础（五）————三角化、PnP、ICP
potxxx的博客
02-03 2466
在之前的章节中我们获得了同名像点从而恢复相片的姿态，接下来就该轮到vslam前端中最后一部分————计算三维点坐标（三角化），并通过三维坐标预测姿态（PnP）。 一、三角化 在单目slam中仅通过单张图像无法获得像素的深度信息，所以我们需要通过三角测量（Triangulation）的方法来估计地图的深度。一般情况是当我们得到两个视图的一组匹配点，我们希望能恢复出世界点在三维世界的坐标。 这里...
PnP问题求解
realjc的博客
03-04 6891
PnP算法是指已知3D点及其在相机上的投影，求相机姿态的问题。 投影方程可以表示为： λ[uv1]=K[Rt][xyz1] \lambda \begin{bmatrix} u \\ v \\ 1 \end{bmatrix} = K \begin{bmatrix} R & t \end{bmatrix}\begin{bmatrix} x \\ y \\ z\\1 \end{bmatrix...
计算机视觉——三维视觉II
weixin_43631804的博客
04-01 3160
【本学期选修国科大计算机视觉课程，本人方向也是cv，因此想通过博客方式记录课程中比较重要的点，巩固并且进行一些扩展学习】 【从本章节开始由申抒含老师为我们讲述相关知识】 【本章节内容针对相机标定与稀疏重建进行讲解】 接下来我们将主要针对以下内容进行介绍： • 三角化：已知x、K、R、t，求X • 相机标定：已知x、X，求K、R、t • 姿态估计：已知x、X 、K ，求R、t • 稀疏重建：已知x，求K、R、t、X • 重投影误差最小化 • 通过线性方法求解初始
SLAM基础 ICP PnP 三角化 F和H（ORB例子）
最新发布
XiaoLeiZer的博客
09-28 1970
SLAM基础知识
嵌入式定时器应用之脉宽测量输入捕获实验
07-11
选用通用定时器 TIM5 的 CH1， 就 PA0 这个 GPIO 来测量信号的脉宽。在开发板中 PA0 接的是一个按键，默认接 GND， 当按键按下的时候 IO口会被拉高，这个时候我们可以利用定时器的输入捕获功能来测量按键按下的这段...
基于MEMS 和MR 传感器的嵌入式系统姿态测量
01-19
相比平台式惯导系统而言，其具有 体积相对更小，成本相对更低，易于安装和维护并且可靠性更高的有点，因此，捷联惯导系 统在飞行器导航和姿态测量中得到了广泛的研究和应用。 　然而，传统的姿态测量系统包括捷联式...
基于嵌入式系统的远程参数测量的方法和实现
08-26
嵌入式系统在现代科技领域扮演着至关重要的角色，尤其在远程参数测量方面，它们以其高效能、低成本和灵活性赢得了广泛应用。本文详细介绍了基于三星公司ARM9处理器S3C2410构建的嵌入式测量监控系统，该系统不仅能够...
IMU（加速度计和陀螺仪设备）在嵌入式应用中使用的指南-综合文档
05-22
总的来说，IMU在嵌入式应用中的使用是一门涉及多领域知识的技术，涵盖了传感器原理、数据处理、嵌入式系统设计以及应用层算法优化等内容。深入理解和熟练掌握这些知识点，对于开发出稳定、精确的嵌入式系统具有重要...
IMU（加速度计和陀螺仪设备）在嵌入式应用中使用的指南.zip-综合文档
05-19
了解以上要点后，结合“IMU（加速度计和陀螺仪设备）在嵌入式应用中使用的指南.pdf”这份文档，读者可以深入学习IMU的选型、集成、数据处理以及实际应用案例，进一步提升在嵌入式系统中使用IMU的能力。该文档将涵盖...
视觉SLAM学习【3】-----视觉SLAM通过三角测量和PnP法估计特征点的空间位置(采用make编译方式)
qq_42451251的博客
05-13 1124
ROS学习【20】-----视觉SLAM通过三角测量和PnP法估计特征点的空间位置目录一、G2O的安装1、g2o的下载2、文件上传ubuntu3、安装依赖库4、g2o的编译二、项目创建1、创建项目文件夹2、创建三角测量的cpp文件3、创建PnP法的cpp文件4、创建CMakeLists.txt配置文件三、编译项目1、文件准备2、项目进行编译2、结果运行 嵌入式开发学习也慢慢的进入尾声了，从之前的ros学习，到gazebo再到机械臂的学习，到现在的slam视觉学习，层次逐渐上升，难度逐渐加大，本次博客，林君学
【SLAM】三角测量与PNP
heroybc的博客
05-26 1886
一、三角测量 在深度估计中，双目视觉可以通过极限约束找到对应的三维点信息，在雷达中可以通过TOF找到深度信息，那么在单目视觉中，如何获得深度信息呢？ 这就需要三角测量。 > 三角测量在三角学与几何学上是一借由测量目标点与固定基准线的已知端点的角度，测量目标距离的方法。而不是直接测量特定位置的距离（三边量测法）。当已知一个边长及两个观测角度时，观测目标点可以被标定为一个三角形的第三个点。 三角量测亦可意指为超大三角形系统的精确测量，称作三角量测网络。这源自于威理博·司乃耳于1615-17的作品，他展现
slam第六讲(视觉里程计1_1)
bestrivern的博客
09-23 1630
VO按是否需要提取特征，分为特征点法的前端和不提特征的前端。这一章讲的是基于特征点法的前端，分为以下内容。 特征点法：找到两张2D图像上的匹配点。 对极几何：根据2D-2D特征点对求解R,t。 三角测量：根据2D-2D特征点求深度。 PnP：根据3D点云和匹配的2D图像求R,t。 ICP：求两个点云之间的R,t。 关系是： 特征点法找到2D图像的匹配点对，用于对极几何和pnp 对极...
对极几何，PnP，ICP 和BA之间的关系
zhengbq_seu的博客
08-15 7206
一．基本概念 注意这里的BA 指的就是目标函数中的f函数为普通相机的观测方程的非线性优化解算方法，所以本文中BA与非线性优化同概念． 对极几何，PnP，ICP 一般都只用于前端，为BA提供初值（这里的BA可能是前端也可能是后端），但是PnP和ICP都可能会用到BA的方法． BA在前端后端都会用到，前后端也就是所谓的范围的问题，前端可能只在相邻几帧进行BA操作，后端的范围就要大的多 二．对极...
视觉SLAM中的对极约束、三角测量、PnP、ICP问题
等你下课
06-10 8190
这篇博客是在学习高翔《视觉SLAM十四讲》过程中对计算机视觉的多视图几何相关知识点做的总结。
视觉里程计1 高翔
一个有梦想，有追求，有抱负的人
07-13 335
小白（我）本着学习，提高自我，增加知识的想法，决定认真分析高翔博士slam。在此立下一个flag： 每周进行知识总结（CSDN）； 每周要有一个计划 决定一年时间学好slam。 我也不知道要说什么了，总之就是这个意思吧，“不荒废自己，撸起袖子加油干，做出成绩来”。废话不说了。 视觉里程计1 特征点法 特征点是由关键点和描述子两部分组成，关键点是指该特征点在图像里面的位置，描述子通常是一个向量，描述了该关键点周围像素的信息。到目前为止ORB特征则是目前非常具有代表性的实时图像特征。 ORB特征由关键点和描
把已有的cmake工程导入kdevlop
lcmssd
06-14 5814
注意kdevop会在工程目录创建一个隐藏的目录, 所以想清空目录中所有kdevelop配置信息, 需要删除这个隐藏目录 1.  kdevelop->Project->open/import project      选择CMakeLists.txt, 一路OK 2. kdeveop->Build Selection     如果正常会在build目录下生成可执行程序， 这个后面有用 3. kde
SLAM学习笔记----坐标关系梳理及PnP算法详解
YOULANSHENGMENG的博客
05-23 3832
一，重要的坐标关系的解析 四个坐标系：世界坐标系、相机坐标系、图像坐标系、像素坐标系。 世界坐标系：机器人或相机运动过程中，肯定需要知道它的位置，因此需要设定世界坐标系，认定固定不动，作为 参考坐标系，描述世界中的任何一点P（Xw,Yw,Zw)。 相机坐标系：相机或机器人运动的一个坐标系，通过世界坐标系的变换（旋转R,平移T）计算得到。因此主要是将 世界坐标系描述的点坐标P（Xw,Yw,Zw)转换成相机坐标系下描述P（Xc,Yc,Zc)，方便计算得到在成像坐标系的坐 标。 图像（成像）坐标系：描
【商城项目2】配置git
qq_34591921的博客
06-22 343
【商城项目2】配置git 1.配置git 1.下载git https://git-scm.com 2.配置git 桌面右键进入git bash here 3.配置用户名，我的叫ufo360 git config --global user.name "替换用户名" 4.配置邮箱，我的叫2575829997@qq.com git config --global user.email "替换邮箱" 5.配置ssh免密登录 ssh-keygen -t rsa -C "替换邮箱" 然后三次回车 6.查

“相关推荐”对你有帮助么？

  • 非常没帮助
  • 没帮助
  • 一般
  • 有帮助
  • 非常有帮助
提交
评论
添加红包

请填写红包祝福语或标题

个

红包个数最小为10个

元

红包金额最低5元

当前余额3.43元 前往充值 >
需支付：10.00元
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付元
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明：

1.余额是钱包充值的虚拟货币，按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载，可以购买VIP、付费专栏及课程。

余额充值