zoukankan      html  css  js  c++  java
  • (2)特征点匹配,并求旋转矩阵R和位移向量t

    include头文件中有slamBase.h

    # pragma once
    // 各种头文件 
    // C++标准库
    #include <fstream>
    #include <vector>
    using namespace std;
    
    // OpenCV
    #include <opencv2/core/core.hpp>
    #include <opencv2/highgui/highgui.hpp>
    
    //PCL
    #include <pcl/io/pcd_io.h>
    #include <pcl/point_types.h>
    
    // 类型定义
    typedef pcl::PointXYZRGBA PointT;
    typedef pcl::PointCloud<PointT> PointCloud;
    
    // 相机内参结构
    struct CAMERA_INTRINSIC_PARAMETERS 
    { 
        double cx, cy, fx, fy, scale;
    };
    
    // 函数接口
    // image2PonitCloud 将rgb图和深度图转换为点云
    PointCloud::Ptr image2PointCloud( cv::Mat& rgb, cv::Mat& depth, CAMERA_INTRINSIC_PARAMETERS& camera );
    
    // point2dTo3d 将单个点从图像坐标转换为空间坐标
    // input: 3维点Point3f (u,v,d)
    cv::Point3f point2dTo3d( cv::Point3f& point, CAMERA_INTRINSIC_PARAMETERS& camera );

    其中有三个部分,相机内参结构,rgb图和深度图转点云,2维像素点转3维空间点坐标(头文件中函数原型)。

    src中源程序slamBase.cpp

    #include "slamBase.h"

    //传入rgb, depth, 和相机内参 PointCloud::Ptr image2PointCloud( cv::Mat& rgb, cv::Mat& depth, CAMERA_INTRINSIC_PARAMETERS& camera ) { PointCloud::Ptr cloud ( new PointCloud ); for (int m = 0; m < depth.rows; m++) for (int n=0; n < depth.cols; n++) { // 获取深度图中(m,n)处的值 ushort d = depth.ptr<ushort>(m)[n]; // d 可能没有值,若如此,跳过此点 if (d == 0) continue; // d 存在值,则向点云增加一个点 PointT p; // 计算这个点的空间坐标 p.z = double(d) / camera.scale; p.x = (n - camera.cx) * p.z / camera.fx; p.y = (m - camera.cy) * p.z / camera.fy; // 从rgb图像中获取它的颜色 // rgb是三通道的BGR格式图,所以按下面的顺序获取颜色 p.b = rgb.ptr<uchar>(m)[n*3]; p.g = rgb.ptr<uchar>(m)[n*3+1]; p.r = rgb.ptr<uchar>(m)[n*3+2]; // 把p加入到点云中 cloud->points.push_back( p ); } // 设置并保存点云 cloud->height = 1; cloud->width = cloud->points.size(); cloud->is_dense = false; return cloud; } //像素坐标转为3维点 cv::Point3f point2dTo3d( cv::Point3f& point, CAMERA_INTRINSIC_PARAMETERS& camera ) { cv::Point3f p; // 3D 点 p.z = double( point.z ) / camera.scale; //point.z d p.x = ( point.x - camera.cx) * p.z / camera.fx; //point.x u p.y = ( point.y - camera.cy) * p.z / camera.fy; //point.y v return p; }

    和实现程序slamBase.cpp在同一文件夹下的CMakeLists.txt

    # 增加PCL库的依赖
    FIND_PACKAGE( PCL REQUIRED COMPONENTS common io )
    
    list(REMOVE_ITEM PCL_LIBRARIES "vtkproj4") # use this in Ubuntu 16.04
    
    # 增加opencv的依赖
    FIND_PACKAGE( OpenCV REQUIRED )
    INCLUDE_DIRECTORIES( ${OpenCV_INCLUDE_DIRS} )
    
    # 添加头文件和库文件
    ADD_DEFINITIONS( ${PCL_DEFINITIONS} )
    INCLUDE_DIRECTORIES( ${PCL_INCLUDE_DIRS}  )
    LINK_LIBRARIES( ${PCL_LIBRARY_DIRS} )
    
    #把slamBase.cpp编译成 slamBase 库,并把该库里调到的OpenCV和PCL的部分,和相应的库链接起来
    ADD_LIBRARY( slambase slamBase.cpp )   # 实现文件  slamBase.cpp
    TARGET_LINK_LIBRARIES( slambase
        ${OpenCV_LIBS} 
        ${PCL_LIBRARIES} )
    
    ADD_EXECUTABLE( detectFeatures detectFeatures.cpp ) #  可执行程序  detectFeatures.cpp
    TARGET_LINK_LIBRARIES( detectFeatures 
        slambase
        ${OpenCV_LIBS} 
        ${PCL_LIBRARIES} )

    库函数:ADD_LIBRARY( slambase slamBase.cpp )        TARGET_LINK_LIBRARIES( slambase ${OpenCV_LIBS} ${PCL_LIBRARIES} )

    可执行程序:ADD_EXECUTABLE( detectFeatures detectFeatures.cpp )     TARGET_LINK_LIBRARIES( detectFeatures slambase ${OpenCV_LIBS} ${PCL_LIBRARIES} ) 使用到上诉的库

    #include<iostream>
    #include "slamBase.h" //
    using namespace std;
    
    // OpenCV 特征检测模块
    #include <opencv2/features2d/features2d.hpp>
    // #include <opencv2/nonfree/nonfree.hpp> // use this if you want to use SIFT or SURF
    #include <opencv2/calib3d/calib3d.hpp>
    
    int main( int argc, char** argv )
    {
        // 声明并从data文件夹里读取两个rgb与深度图
        cv::Mat rgb1 = cv::imread( "./data/rgb20.png");
        cv::Mat rgb2 = cv::imread( "./data/rgb21.png");
        cv::Mat depth1 = cv::imread( "./data/depth20.png", -1);
        cv::Mat depth2 = cv::imread( "./data/depth21.png", -1);
    
        // 声明特征提取器与描述子提取器
        cv::Ptr<cv::FeatureDetector> detector;
        cv::Ptr<cv::DescriptorExtractor> descriptor;
    
        // 构建提取器,默认两者都为 ORB
        
        // 如果使用 sift, surf ,之前要初始化nonfree模块
        // cv::initModule_nonfree();
        // _detector = cv::FeatureDetector::create( "SIFT" );
        // _descriptor = cv::DescriptorExtractor::create( "SIFT" );
        
        detector = cv::ORB::create();
        descriptor = cv::ORB::create();
    
        vector< cv::KeyPoint > kp1, kp2; //关键点
        detector->detect( rgb1, kp1 );  //提取关键点
        detector->detect( rgb2, kp2 );
    
        cout<<"Key points of two images: "<<kp1.size()<<", "<<kp2.size()<<endl;
        
        // 可视化, 显示关键点
        cv::Mat imgShow;
        cv::drawKeypoints( rgb1, kp1, imgShow, cv::Scalar::all(-1), cv::DrawMatchesFlags::DRAW_RICH_KEYPOINTS );
        cv::namedWindow("Keypoints",0);
        cv::imshow( "keypoints", imgShow );
        cv::imwrite( "./data/keypoints.png", imgShow );
        cv::waitKey(0); //暂停等待一个按键
       
        // 计算描述子
        cv::Mat desp1, desp2;
        descriptor->compute( rgb1, kp1, desp1 );
        descriptor->compute( rgb2, kp2, desp2 );
    
        // 匹配描述子
        vector< cv::DMatch > matches; 
        cv::BFMatcher matcher;
        matcher.match( desp1, desp2, matches );
        cout<<"Find total "<<matches.size()<<" matches."<<endl;
    
        // 可视化:显示匹配的特征
        cv::Mat imgMatches;
        cv::drawMatches( rgb1, kp1, rgb2, kp2, matches, imgMatches );
        cv::namedWindow("matches",0);
        cv::imshow( "matches", imgMatches );
        cv::imwrite( "./data/matches.png", imgMatches );
        cv::waitKey( 0 );
    
        // 筛选匹配,把距离太大的去掉
        // 这里使用的准则是去掉大于四倍最小距离的匹配
        vector< cv::DMatch > goodMatches;
        double minDis = 9999;
        for ( size_t i=0; i<matches.size(); i++ )
        {
            if ( matches[i].distance < minDis )
                minDis = matches[i].distance;
        }
        cout<<"min dis = "<<minDis<<endl;
    
        for ( size_t i=0; i<matches.size(); i++ )
        {
            if (matches[i].distance < 10*minDis)
                goodMatches.push_back( matches[i] );
        }
    
        // 显示 good matches
        cout<<"good matches="<<goodMatches.size()<<endl;
        cv::drawMatches( rgb1, kp1, rgb2, kp2, goodMatches, imgMatches );
        cv::namedWindow("good matches",0);
        cv::imshow( "good matches", imgMatches );
        cv::imwrite( "./data/good_matches.png", imgMatches );
        cv::waitKey(0);
    
        // 计算图像间的运动关系
        // 关键函数:cv::solvePnPRansac()
        // 为调用此函数准备必要的参数
        
        // 第一个帧的三维点
        vector<cv::Point3f> pts_obj;
        // 第二个帧的图像点
        vector< cv::Point2f > pts_img;
    
        // 相机内参,使用到结构
        CAMERA_INTRINSIC_PARAMETERS C;
        C.cx = 682.3;
        C.cy = 254.9;
        C.fx = 979.8;
        C.fy = 942.8;
        C.scale = 1000.0;
    
        for (size_t i=0; i<goodMatches.size(); i++)
        {
            // query 是第一个, train 是第二个
            cv::Point2f p = kp1[goodMatches[i].queryIdx].pt;
            // 获取d是要小心!x是向右的,y是向下的,所以y才是行,x是列!
            ushort d = depth1.ptr<ushort>( int(p.y) )[ int(p.x) ];
            if (d == 0)
                continue;
            pts_img.push_back( cv::Point2f( kp2[goodMatches[i].trainIdx].pt ) );
    
            // 将(u,v,d)转成(x,y,z)
            cv::Point3f pt ( p.x, p.y, d ); //   深度值/scale = z
            cv::Point3f pd = point2dTo3d( pt, C );
            pts_obj.push_back( pd );
        }
        //使用到结构
        double camera_matrix_data[3][3] = {
            {C.fx, 0, C.cx},
            {0, C.fy, C.cy},
            {0, 0, 1}
        };
    
        // 构建相机矩阵
        cv::Mat cameraMatrix( 3, 3, CV_64F, camera_matrix_data );
        cv::Mat rvec, tvec, inliers;
        // 求解pnp
        cv::solvePnPRansac( pts_obj, pts_img, cameraMatrix, cv::Mat(), rvec, tvec, false, 100, 1.0, 0.95, inliers ,cv::SOLVEPNP_ITERATIVE);
        // 求旋转向量和平移向量,旋转矩阵
        cout<<"inliers: "<<inliers.rows<<endl;
        cout<<"R="<<rvec<<endl;
        cout<<"t="<<tvec<<endl;
    //旋转矩阵
    cv::Mat R;
    cv::Rodrigues(rvec,R);
    cout<<"R_matrix="<<R<<endl;

    // 画出inliers匹配 vector< cv::DMatch > matchesShow; for (size_t i=0; i<inliers.rows; i++) { matchesShow.push_back( goodMatches[inliers.ptr<int>(i)[0]] ); } cv::drawMatches( rgb1, kp1, rgb2, kp2, matchesShow, imgMatches ); cv::namedWindow("inlier matches",0); cv::imshow( "inlier matches", imgMatches ); cv::imwrite( "./data/inliers.png", imgMatches ); cv::waitKey( 0 ); return 0; }

    使用到结构,和将旋转向量转为旋转矩阵的函数cv::Rodrigues()

     OpenCV会利用一种“随机采样一致性”(Random Sample Consensus)的思路(见https://en.wikipedia.org/wiki/RANSAC

    cv::solvePnPRansac()函数    https://blog.csdn.net/jay463261929/article/details/53818611

     和src,include文件同一文件夹下的CMakeLists.txt

    CMAKE_MINIMUM_REQUIRED( VERSION 2.8 )
    PROJECT( slam )
    
    SET(CMAKE_CXX_COMPILER "g++")
    SET( CMAKE_BUILD_TYPE Debug  )
    SET(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin) #可执行文件输出的文件夹
    SET(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib) #库函数编译输出位置
    
    INCLUDE_DIRECTORIES( ${PROJECT_SOURCE_DIR}/include ) #头文件
    LINK_DIRECTORIES( ${PROJECT_SOURCE_DIR}/lib) #使用编译好的库文件libslamBase.a
    
    ADD_SUBDIRECTORY( ${PROJECT_SOURCE_DIR}/src ) #可执行程序

    注意:当lib 文件下已经有编译好的库库文件libslamBase.a,可以将第一个CMakeLists.txt中ADD_LIBRARY( slambase slamBase.cpp )  TARGET_LINK_LIBRARIES( slambase ${OpenCV_LIBS} ${PCL_LIBRARIES} )去掉,因为slamBase.cpp已经被编译。

    将第二个CMakeLists.txt中SET(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib) 去掉。

  • 相关阅读:
    ThreadLocal的原理和使用场景
    sleep wait yield join方法的区别
    GC如何判断对象可以被回收
    双亲委派
    ConcurrentHashMap原理,jdk7和jdk8版本
    hashCode和equals
    接口和抽象类区别
    为什么局部内部类和匿名内部类只能访问局部final变量
    【GAN】基础GAN代码解析
    TF相关codna常用命令整理
  • 原文地址:https://www.cnblogs.com/112358nizhipeng/p/9218114.html
Copyright © 2011-2022 走看看