RGBD-SLAM学习3
发布时间
阅读量：
阅读量
持续关注高博的博客内容，并阅读了第三篇文章： http://www.cnblogs.com/gaoxiang12/p/4659805.html
第三篇主要分为两个模块：一个是实现生成点云数据的功能模块；另一个是负责图像匹配到位姿变化的模块
将产生点云功能封装成一个库：
首先说一下，一个程序编译链接之后，并不一定都生成可执行程序，有的是生成了库，以备其他程序调用。所以这两个有点并行的意味。首先，一个库要有头文件和库文件：
头文件slamBase.h，放在include文件夹中：
//
// Created by robin on 18-1-29.
//
//# 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)，注意下这里比较奇葩，这个三维向量并不是坐标，没有什么实际意义，仅仅是一个数列，分别存储了像素坐标uv和深度数据d。用于给函数传递参数。
cv::Point3f point2dTo3d( cv::Point3f& point, CAMERA_INTRINSIC_PARAMETERS& camera );内容较为基础，并主要涉及创建了一个用于存储相机内参信息的数据结构。该结构体中包含了必要的参数定义，并实现了两个核心功能模块。
再看src中的源文件slamBase.cpp：
//
// Created by robin on 18-1-29.
//
#include "slamBase.h"
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;
}
cv::Point3f point2dTo3d( cv::Point3f& point, CAMERA_INTRINSIC_PARAMETERS& camera )
{
cv::Point3f p; // 3D 点
p.z = double( point.z ) / camera.scale;
p.x = ( point.x - camera.cx) * p.z / camera.fx;
p.y = ( point.y - camera.cy) * p.z / camera.fy;
return p;
}更明了，include进来头文件后，写两个功能函数的定义就好了。
首先是最直观的做法：我们创建了一个名为slamBase.cpp的文件，并在其中实现了关键的数据处理逻辑以及算法实现代码。随后，在源代码目录下新增了相应的头文件和必要的包含路径声明。接着，在CMakeLists.txt中添加了构建脚本以实现高效的编译运行机制以及环境配置管理功能
# 增加库文件slamBase
add_library(slambase slamBase.cpp )
target_link_libraries(slambase ${OpenCV_LIBS} ${PCL_LIBS})OK，这就好了～～～我们点击编译一下看看：
没问题，在软件项目管理中，在lib目录下制作了一个命名为libslambase.a的静态共享库文件。需要注意的是：尽管这个静态库文件被命名为libslambase.a（即包含扩展名），但其实际命名遵循的是add_library函数所指定的名字——即为slambase（不带扩展名）。因此，在后续操作中链接该库时，请依据其实际命名规则使用相应的名称：即使用其库名slambase（而不考虑扩展名），这与它的头文件路径slamBase.h、源代码路径为slamBase.cpp以及对应的可执行文件路径libslambase.a均无关紧要。
第二部分，书写detectFeatures.cpp，
高博原始的代码：
#include<iostream>
#include "slamBase.h"
using namespace std;
// OpenCV 特征检测模块
#include <opencv2/features2d/features2d.hpp>
#include <opencv2/nonfree/nonfree.hpp>
#include <opencv2/calib3d/calib3d.hpp>
int main( int argc, char** argv )
{
// 声明并从data文件夹里读取两个rgb与深度图
cv::Mat rgb1 = cv::imread( "./data/rgb1.png");
cv::Mat rgb2 = cv::imread( "./data/rgb2.png");
cv::Mat depth1 = cv::imread( "./data/depth1.png", -1);
cv::Mat depth2 = cv::imread( "./data/depth2.png", -1);
// 声明特征提取器与描述子提取器
cv::Ptr<cv::FeatureDetector> _detector;
cv::Ptr<cv::DescriptorExtractor> _descriptor;
// 构建提取器，默认两者都为sift
// 构建sift, surf之前要初始化nonfree模块
cv::initModule_nonfree();
_detector = cv::FeatureDetector::create( "GridSIFT" );
_descriptor = cv::DescriptorExtractor::create( "SIFT" );
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::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::FlannBasedMatcher 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::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;
}
for ( size_t i=0; i<matches.size(); i++ )
{
if (matches[i].distance < 4*minDis)
goodMatches.push_back( matches[i] );
}
// 显示 good matches
cout<<"good matches="<<goodMatches.size()<<endl;
cv::drawMatches( rgb1, kp1, rgb2, kp2, goodMatches, imgMatches );
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 = 325.5;
C.cy = 253.5;
C.fx = 518.0;
C.fy = 519.0;
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 );
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, 100, inliers );
cout<<"inliers: "<<inliers.rows<<endl;
cout<<"R="<<rvec<<endl;
cout<<"t="<<tvec<<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::imshow( "inlier matches", imgMatches );
cv::imwrite( "./data/inliers.png", imgMatches );
cv::waitKey( 0 );
return 0;
}关于内容就不再多做赘述，条理清晰明确即可。特别提醒：在编写代码时，请确保包含我们自定义的库文件slamBase.h
相应的还有CMakeLists.txt日常基本操作，添加：
# 增加特征点匹配的可执行文件
add_executable(detectfeacures detectFeacures.cpp )
# 链接上我们自己写的库
target_link_libraries(detectfeacures slambase )OK，编译日常不通过。。。
首先第一个报错：
CMakeFiles\untitled1.dir/objects.a(main.cpp.obj): In function `_static_initialization_and_destruction_0':
C:/www/cpp/boost_1_62_0/boost/system/error_code.hpp:221: undefined reference to `boost::system::generic_category()'
C:/www/cpp/boost_1_62_0/boost/system/error_code.hpp:222: undefined reference to `boost::system::generic_category()'
C:/www/cpp/boost_1_62_0/boost/system/error_code.hpp:223: undefined reference to `boost::system::system_category()'经查找相关资料后发现，在解决该问题的过程中发现网上存在相同报错代码实例。然而由于文件名的具体内容并不影响这一现象的存在因此无需过分关注其具体细节部分。关键在于解决这三个undefined reference问题以及后续的具体实现方式。
undefined reference to `boost::system::generic_category()'
undefined reference to `boost::system::generic_category()'
undefined reference to `boost::system::system_category()'关于未定义引用的问题，请访问以下链接查看相关信息：https://segmentfault.com/q/1010000007420187?sort=created。主要原因可能是没有正确地包含boost_system库导致的问题。参考引用答案进行调整：然后，在CMakeLists.txt文件中添加相应的代码段：
# 由于需要链接boost库，所以需要find_package和include_directries进 boost库
find_package(Boost REQUIRED COMPONENTS system)
include_directories(${Boost_INCLUDE_DIRS})之后还需要将编译的文件链接上，将target_link_libraries改为：
# 这里也是将boost库链接进来
target_link_libraries(detectfeacures ${Boost_LIBRARIES} slambase )现在开始编译。旧的问题已经解决完毕，在当前遇到的新问题主要是由于OpenCV每日更新所带来的变化所导致。主要有以下三点：
- 为了实现更好的功能定位与配置管理需求，在项目中采用了ORB算法替代了原来的方案：
// 声明特征提取器与描述子提取器
cv::Ptr<cv::FeatureDetector> _detector = cv::ORB::create();;
cv::Ptr<cv::DescriptorExtractor> _descriptor = cv::ORB::create();2、匹配器matcher的创建要随动ORB，改成Brute Force match：
//cv::FlannBasedMatcher matcher; //这是高博的
auto matcher = cv::DescriptorMatcher::create ( "BruteForce-Hamming" );3、solvePnPRansac函数引入了confidence参数：该算法用于评估有用结果的置信度。主要改动是增加了这样一个参数值（建议将其设置为函数默认值0.99）。
cv::solvePnPRansac( pts_obj, pts_img, cameraMatrix, cv::Mat(), rvec, tvec, false, 100, 1.0, 0.99, inliers );在这三项修改中，在高博博客的开头部分都有所提及。然而，在GitHub上最新的版本尚未找到对应资源。自行进行调整同样能带来帮助，并参考了以下博客文章：
改动完最终测试能跑通的代码：
//
// Created by robin on 18-1-29.
//
#include<iostream>
#include "slamBase.h"
using namespace std;
// OpenCV 特征检测模块
#include <opencv2/core/core.hpp>
#include <opencv2/features2d/features2d.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/calib3d/calib3d.hpp>
int main( int argc, char** argv )
{
// 声明并从data文件夹里读取两个rgb与深度图
cv::Mat rgb1 = cv::imread( "../data/rgb1.png");
cv::Mat rgb2 = cv::imread( "../data/rgb2.png");
cv::Mat depth1 = cv::imread( "../data/depth1.png", -1);
cv::Mat depth2 = cv::imread( "../data/depth2.png", -1);
// 声明特征提取器与描述子提取器
cv::Ptr<cv::FeatureDetector> _detector = cv::ORB::create();;
cv::Ptr<cv::DescriptorExtractor> _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::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::FlannBasedMatcher matcher;
auto matcher = cv::DescriptorMatcher::create ( "BruteForce-Hamming" );
matcher->match( desp1, desp2, matches );
cout<<"Find total "<<matches.size()<<" matches."<<endl;
// 可视化：显示匹配的特征
cv::Mat imgMatches;
cv::drawMatches( rgb1, kp1, rgb2, kp2, matches, imgMatches );
cv::imshow( "matches", imgMatches );
cv::imwrite( "../data/matches.png", imgMatches );
cv::waitKey( 0 );
// 筛选匹配，把距离太大的去掉
// 这里使用的准则是去掉大于四倍最小距离的匹配
vector< cv::DMatch > goodMatches;
//先将最小距离设置为一个很大的值
double minDis = 9999;
//然后遍历整个matches，找到最小的距离。
for ( size_t i=0; i<matches.size(); i++ )
{
if ( matches[i].distance < minDis )
minDis = matches[i].distance;
}
//遍历，小于最小距离四倍的匹配push进goodmatches。
for ( size_t i=0; i<matches.size(); i++ )
{
if (matches[i].distance < 4*minDis)
goodMatches.push_back( matches[i] );
}
// 显示 good matches
cout<<"good matches="<<goodMatches.size()<<endl;
cv::drawMatches( rgb1, kp1, rgb2, kp2, goodMatches, imgMatches );
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 = 325.5;
C.cy = 253.5;
C.fx = 518.0;
C.fy = 519.0;
C.scale = 1000.0;
for (size_t i=0; i<goodMatches.size(); i++)
{
// query 是第一个关键点的索引, train 是第二个关键点的索引
//获取3d点
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;
// 将(u,v,d)转成(x,y,z)
cv::Point3f pt ( p.x, p.y, d );
cv::Point3f pd = point2dTo3d( pt, C );
pts_obj.push_back( pd );
//获取2d点
pts_img.push_back( cv::Point2f( kp2[goodMatches[i].trainIdx].pt ) );
}
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.99, inliers );
cout<<"inliers: "<<inliers.rows<<endl;
cout<<"R="<<rvec<<endl;
cout<<"t="<<tvec<<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::imshow( "inlier matches", imgMatches );
cv::imwrite( "../data/inliers.png", imgMatches );
cv::waitKey( 0 );
return 0;
}全部评论 (0)
还没有任何评论哟~