特征点匹配,并求旋转矩阵R和位移向量t

Posted 112358nizhipeng

tags:

篇首语:本文由小常识网(cha138.com)小编为大家整理,主要介绍了特征点匹配,并求旋转矩阵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()

 

 

 和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) 去掉。

 

技术分享图片








以上是关于特征点匹配,并求旋转矩阵R和位移向量t的主要内容,如果未能解决你的问题,请参考以下文章

matlab练习程序(对应点集配准的四元数法)

3d激光雷达开发(旋转和位移)

如何求稀疏矩阵的全部特征值和特征向量?

SLAM中的变换(旋转与位移)表示方法

应用于 3D 点的 2D 旋转矩阵

矩阵变换:沿任意轴旋转及其推导