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 = ; m < depth.rows; m++)
for (int n=; n < depth.cols; n++)
{
// 获取深度图中(m,n)处的值
ushort d = depth.ptr<ushort>(m)[n];
// d 可能没有值,若如此,跳过此点
if (d == )
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*];
p.g = rgb.ptr<uchar>(m)[n*+];
p.r = rgb.ptr<uchar>(m)[n*+]; // 把p加入到点云中
cloud->points.push_back( p );
}
// 设置并保存点云
cloud->height = ;
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", -);
cv::Mat depth2 = cv::imread( "./data/depth21.png", -); // 声明特征提取器与描述子提取器
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(-), cv::DrawMatchesFlags::DRAW_RICH_KEYPOINTS );
cv::namedWindow("Keypoints",);
cv::imshow( "keypoints", imgShow );
cv::imwrite( "./data/keypoints.png", imgShow );
cv::waitKey(); //暂停等待一个按键 // 计算描述子
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",);
cv::imshow( "matches", imgMatches );
cv::imwrite( "./data/matches.png", imgMatches );
cv::waitKey( ); // 筛选匹配,把距离太大的去掉
// 这里使用的准则是去掉大于四倍最小距离的匹配
vector< cv::DMatch > goodMatches;
double minDis = ;
for ( size_t i=; i<matches.size(); i++ )
{
if ( matches[i].distance < minDis )
minDis = matches[i].distance;
}
cout<<"min dis = "<<minDis<<endl; for ( size_t i=; i<matches.size(); i++ )
{
if (matches[i].distance < *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",);
cv::imshow( "good matches", imgMatches );
cv::imwrite( "./data/good_matches.png", imgMatches );
cv::waitKey(); // 计算图像间的运动关系
// 关键函数: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=; 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 == )
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[][] = {
{C.fx, , C.cx},
{, C.fy, C.cy},
{, , }
}; // 构建相机矩阵
cv::Mat cameraMatrix( , , CV_64F, camera_matrix_data );
cv::Mat rvec, tvec, inliers;
// 求解pnp
cv::solvePnPRansac( pts_obj, pts_img, cameraMatrix, cv::Mat(), rvec, tvec, false, , 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=; i<inliers.rows; i++)
{
matchesShow.push_back( goodMatches[inliers.ptr<int>(i)[]] );
}
cv::drawMatches( rgb1, kp1, rgb2, kp2, matchesShow, imgMatches );
cv::namedWindow("inlier matches",);
cv::imshow( "inlier matches", imgMatches );
cv::imwrite( "./data/inliers.png", imgMatches );
cv::waitKey( ); return ;
}

使用到结构,和将旋转向量转为旋转矩阵的函数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) 去掉。

(2)特征点匹配,并求旋转矩阵R和位移向量t的更多相关文章

  1. c++ 知道旋转前后矩阵向量值 求旋转矩阵c++/c#代码 知道两个向量求他们的旋转矩阵

    原文作者:aircraft 原文链接:https://www.cnblogs.com/DOMLX/p/12115244.html 知道旋转前后矩阵向量值 如何去求旋转矩阵R 的c++/c#代码??? ...

  2. 第二篇 特征点匹配以及openvslam中的相关实现详解

    配置文件 在进入正题之前先做一些铺垫,在openvslam中,配置文件是必须要正确的以.yaml格式提供,通常需要指明使用的相机模型,ORB特征检测参数,跟踪参数等. #==============# ...

  3. 容斥原理应用(求1~r中有多少个数与n互素)

    问题:求1~r中有多少个数与n互素. 对于这个问题由容斥原理,我们有3种写法,其实效率差不多.分别是:dfs,队列数组,位运算. 先说说位运算吧: 用二进制1,0来表示第几个素因子是否被用到,如m=3 ...

  4. OpenCV使用FLANN进行特征点匹配

    使用FLANN进行特征点匹配 目标 在本教程中我们将涉及以下内容: 使用 FlannBasedMatcher 接口以及函数 FLANN 实现快速高效匹配( 快速最近邻逼近搜索函数库(Fast Appr ...

  5. sift、surf、orb 特征提取及最优特征点匹配

    目录 sift sift特征简介 sift特征提取步骤 surf surf特征简介 surf特征提取步骤 orb orb特征简介 orb特征提取算法 代码实现 特征提取 特征匹配 附录 sift si ...

  6. OpenCvSharp 通过特征点匹配图片

    现在的手游基本都是重复操作,一个动作要等好久,结束之后继续另一个动作.很麻烦,所以动起了自己写一个游戏辅助的心思. 这个辅助本身没什么难度,就是通过不断的截图,然后从这个截图中找出预先截好的能代表相应 ...

  7. opencv 增强现实(二):特征点匹配

    import cv2 as cv import numpy as np # def draw_keypoints(img, keypoints): # for kp in keypoints: # x ...

  8. 机器学习入门-数值特征-数据四分位特征 1.quantile(用于求给定分数位的数值) 2.plt.axvline(用于画出竖线) 3.pd.pcut(对特征进行分位数切分,生成新的特征)

    函数说明: 1.  .quantile(cut_list) 对DataFrame类型直接使用,用于求出给定列表中分数的数值,这里用来求出4分位出的数值 2.  plt.axvline()  # 用于画 ...

  9. (分解质因数模板)求 1~r 内与 n 互素的元素个数

    void Solve(LL n){ ///分解质因数保存结果于p p.clear(); ; i*i<=n; i++) ){ p.push_back(i); ) n/=i; } ) p.push_ ...

随机推荐

  1. mysql对emoji的支持

    步骤: 升级mysql数据库到5.5.3+ 修改database.table和column字符集 alter database DATABASE_NAME character set = utf8mb ...

  2. 06 爬虫框架:scrapy

    爬虫框架:scrapy   一 介绍 Scrapy一个开源和协作的框架,其最初是为了页面抓取 (更确切来说, 网络抓取 )所设计的,使用它可以以快速.简单.可扩展的方式从网站中提取所需的数据.但目前S ...

  3. 为什么大神的UI设计那么高级?答案尽在此文…

    对于每个网页设计师而言,在设计过程中总会碰到需要作出设计决策的时候.也许你的公司并没有全职设计师,而需求上则要求设计出全新的UI:又或者你正在制作一个你自己的个人项目,而你希望它比 Bootstrap ...

  4. C++对象拾遗

    #include <iostream.h> #include <string.h> //using namespace std; class A { public:     A ...

  5. [精彩] 关于DB2的内存分配

    这两天在看DB2的内存管理的内容,看的很是模糊,有以下问题不明白,请教 是不是数据库管理器的共享内存就是DB2能够使用的最大内容呢,然后数据库全局内存从管理器内存那里获得分配的内存,然后应用程序全局内 ...

  6. nginx集群tomcat,session共享问题

    tomcat-redis-session-manager版本: apache-tomcat-8.0.32.nginx-1.13.12.redis_3.2.1 github地址: https://git ...

  7. python 取整itertools

    #coding:utf-8 import sys import itertools def MaxString(n,nums): list1 = nums list2 = [] for i in ra ...

  8. 1) Apache Maven 's README.txt

    Apache Maven What is it? ----------- Maven is a software project management and comprehension tool. ...

  9. 20170908工作日记--Volley源码详解

    Volley没有jar包,需要从官网上下载源码自己编译出来,或者做成相关moudle引入项目中.我们先从最简单的使用方法入手进行分析: //创建一个网络请求队列 RequestQueue reques ...

  10. Before an Exam

    http://acm.hust.edu.cn/vjudge/contest/view.action?cid=93241#problem/B (654123) http://codeforces.com ...