第三讲 特征提取与配准
2016.11 更新
- 把原文的SIFT替换成了ORB，这样你可以在没有nonfree模块下使用本程序了。
- OpenCV可以使用 apt-get install libopencv-dev ，一样能成功。
- 因为换成了ORB，所以调整了good match的阈值，并且匹配时需要使用 Brute Force match。
- 请以现在的github上源码为准。
师兄：同学们大家好，又到我们每周一次的“一起做RGB-D SLAM”时间啦！大家还记得上周我们讲了什么东西吗？
小萝卜：等一下师兄！我们的博客什么时候变成每周一更了？
师兄：这个，这不是因为终于到暑假了，我可以专门搞学术了嘛。
小萝卜：胡说！前两天我还看到你在超市开挖掘机来着！
师兄：这事你就别提了啊……
小萝卜：我还有照片为证呢！
师兄：你能不能别提这个事了啊……我们赶紧开始讲课吧。
小萝卜：师兄，那可是儿童专区啊，您可是个博士，自重啊！
上讲回顾
在上一讲中，我们介绍了如何把2D图像坐标转换为3D空间坐标。然后，利用推导的数学公式，实现了把图像转化为点云的程序。在上一讲的末尾，我们给出了一道作业题，希望读者去把这两件事做成一个函数库，以供将来调用。不知道大家回去之后做了没有呢？
小萝卜：读者这么勤奋肯定已经做好了啦！
师兄：嗯，所以呢，这一讲里面我们就要用到上面两个函数了。在讲解本讲的内容之前，先介绍一下我们是如何封装上节课代码的。在 代码根目录/include 下，我们新建了一个 slamBase.h 文件，存放上一讲以及以后讲到的各种函数：
include/slamBase.h:
1 /************************************************************************* 2 > File Name: rgbd-slam-tutorial-gx/part III/code/include/slamBase.h 3 > Author: xiang gao 4 > Mail: gaoxiang12@mails.tsinghua.edu.cn 5 > Created Time: 2015年07月18日 星期六 15时14分22秒 6 > 说明：rgbd-slam教程所用到的基本函数（C风格） 7 ************************************************************************/ 8 # pragma once 9 10 // 各种头文件 11 // C++标准库 12 #include <fstream> 13 #include <vector> 14 using namespace std; 15 16 // OpenCV 17 #include <opencv2/core/core.hpp> 18 #include <opencv2/highgui/highgui.hpp> 19 20 //PCL 21 #include <pcl/io/pcd_io.h> 22 #include <pcl/point_types.h> 23 24 // 类型定义 25 typedef pcl::PointXYZRGBA PointT; 26 typedef pcl::PointCloud<PointT> PointCloud; 27 28 // 相机内参结构 29 struct CAMERA_INTRINSIC_PARAMETERS 30 { 31 double cx, cy, fx, fy, scale; 32 }; 33 34 // 函数接口 35 // image2PonitCloud 将rgb图转换为点云 36 PointCloud::Ptr image2PointCloud( cv::Mat& rgb, cv::Mat& depth, CAMERA_INTRINSIC_PARAMETERS& camera ); 37 38 // point2dTo3d 将单个点从图像坐标转换为空间坐标 39 // input: 3维点Point3f (u,v,d) 40 cv::Point3f point2dTo3d( cv::Point3f& point, CAMERA_INTRINSIC_PARAMETERS& camera );
可以看到，我们把相机参数封装成了一个结构体，另外还声明了 image2PointCloud 和 point2dTo3d 两个函数。然后，在 src/ 目录下新建 slamBase.cpp 文件：
src/slamBase.cpp
1 /************************************************************************* 2 > File Name: src/slamBase.cpp 3 > Author: xiang gao 4 > Mail: gaoxiang12@mails.tsinghua.edu.cn 5 > Implementation of slamBase.h 6 > Created Time: 2015年07月18日 星期六 15时31分49秒 7 ************************************************************************/ 8 9 #include "slamBase.h" 10 11 PointCloud::Ptr image2PointCloud( cv::Mat& rgb, cv::Mat& depth, CAMERA_INTRINSIC_PARAMETERS& camera ) 12 { 13 PointCloud::Ptr cloud ( new PointCloud ); 14 15 for (int m = 0; m < depth.rows; m++) 16 for (int n=0; n < depth.cols; n++) 17 { 18 // 获取深度图中(m,n)处的值 19 ushort d = depth.ptr<ushort>(m)[n]; 20 // d 可能没有值，若如此，跳过此点 21 if (d == 0) 22 continue; 23 // d 存在值，则向点云增加一个点 24 PointT p; 25 26 // 计算这个点的空间坐标 27 p.z = double(d) / camera.scale; 28 p.x = (n - camera.cx) * p.z / camera.fx; 29 p.y = (m - camera.cy) * p.z / camera.fy; 30 31 // 从rgb图像中获取它的颜色 32 // rgb是三通道的BGR格式图，所以按下面的顺序获取颜色 33 p.b = rgb.ptr<uchar>(m)[n*3]; 34 p.g = rgb.ptr<uchar>(m)[n*3+1]; 35 p.r = rgb.ptr<uchar>(m)[n*3+2]; 36 37 // 把p加入到点云中 38 cloud->points.push_back( p ); 39 } 40 // 设置并保存点云 41 cloud->height = 1; 42 cloud->width = cloud->points.size(); 43 cloud->is_dense = false; 44 45 return cloud; 46 } 47 48 cv::Point3f point2dTo3d( cv::Point3f& point, CAMERA_INTRINSIC_PARAMETERS& camera ) 49 { 50 cv::Point3f p; // 3D 点 51 p.z = double( point.z ) / camera.scale; 52 p.x = ( point.x - camera.cx) * p.z / camera.fx; 53 p.y = ( point.y - camera.cy) * p.z / camera.fy; 54 return p; 55 }
最后，在 src/CMakeLists.txt 中加入以下几行，将 slamBase.cpp 编译成一个库，供将来调用：
ADD_LIBRARY( slambase slamBase.cpp ) TARGET_LINK_LIBRARIES( slambase ${OpenCV_LIBS} ${PCL_LIBRARIES} )
这两句话是说，把slamBase.cpp编译成 slamBase 库，并把该库里调到的OpenCV和PCL的部分，和相应的库链接起来。是不是感觉代码更有条理了呢？今后，我们会在每一讲里介绍新的内容，并把实现的代码封装里这个slamBase库中。
图像配准 数学部分
SLAM是由“定位”（Localization)和“建图”（Mapping)两部分构成的。现在来看定位问题。要求解机器人的运动，首先要解决这样一个问题：给定了两个图像，如何知道图像的运动关系呢？
这个问题可以用基于特征的方法（feature-based）或直接的方法（direct method）来解。虽说直接法已经有了一定的发展，但目前主流的方法还是基于特征点的方式。在后者的方法中，首先你需要知道图像里的“特征”，以及这些特征的一一对应关系。
假设我们有两个帧：$F_1$和$F_2$. 并且，我们获得了两组一一对应的特征点：$$P=\{p_1, p_2, \ldots, p_N \} \in F_1 $$ $$Q=\{ q_1, q_2, \ldots, q_N \} \in F_2 $$. 其中$p$和$q$都是$R^3$中的点。
我们的目的是求出一个旋转矩阵$R$和位移矢量$t$，使得：$$ \forall i, p_i = R q_i + t $$.
然而实际当中由于误差的存在，等号基本是不可能的。所以我们通过最小化一个误差来求解$R,t$:
$$\min\limits_{R,t} \sum\limits_{i=1}^{N} \| p_i - (R q_i + t) \|_2 $$
这个问题可以用经典的ICP算法求解。其核心是奇异值分解(SVD)。我们将调用OpenCV中的函数求解此问题，所以在此就不细讲ICP的具体步骤了。有兴趣的读者可以参阅1987年PAMI上的一篇文章，也是最原始的ICP方法：Least-squares fitting of two 3-D point sets。
那么从这个数学问题上来讲，我们的关键就是要获取一组一一对应的空间点，这可以通过图像的特征匹配来完成。
提示：由于OpenCV中没有提供ICP，我们在实现中使用PnP进行求解。
图像配准 编程实现
本节中，我们要匹配两对图像，并且计算它们的位移关系。它们分别是rgb1,png, rgb2.png, depth1.png, depth2.png. 人眼可以直观地看到第二个图是向左转动了一些。
照例，我们先给出完整的程序代码。读者可以先把代码浏览一遍，我们再加以详细的解释。因为本节的代码比较长，我把它折叠起来，否则排版就太长了。
1 /************************************************************************* 2 > File Name: detectFeatures.cpp 3 > Author: xiang gao 4 > Mail: gaoxiang12@mails.tsinghua.edu.cn 5 > 特征提取与匹配 6 > Created Time: 2015年07月18日 星期六 16时00分21秒 7 ************************************************************************/ 8 9 #include<iostream> 10 #include "slamBase.h" 11 using namespace std; 12 13 // OpenCV 特征检测模块 14 #include <opencv2/features2d/features2d.hpp> 15 #include <opencv2/nonfree/nonfree.hpp> 16 #include <opencv2/calib3d/calib3d.hpp> 17 18 int main( int argc, char** argv ) 19 { 20 // 声明并从data文件夹里读取两个rgb与深度图 21 cv::Mat rgb1 = cv::imread( "./data/rgb1.png"); 22 cv::Mat rgb2 = cv::imread( "./data/rgb2.png"); 23 cv::Mat depth1 = cv::imread( "./data/depth1.png", -1); 24 cv::Mat depth2 = cv::imread( "./data/depth2.png", -1); 25 26 // 声明特征提取器与描述子提取器 27 cv::Ptr<cv::FeatureDetector> _detector; 28 cv::Ptr<cv::DescriptorExtractor> _descriptor; 29 30 // 构建提取器，默认两者都为sift 31 // 构建sift, surf之前要初始化nonfree模块 32 cv::initModule_nonfree(); 33 _detector = cv::FeatureDetector::create( "GridSIFT" ); 34 _descriptor = cv::DescriptorExtractor::create( "SIFT" ); 35 36 vector< cv::KeyPoint > kp1, kp2; //关键点 37 _detector->detect( rgb1, kp1 ); //提取关键点 38 _detector->detect( rgb2, kp2 ); 39 40 cout<<"Key points of two images: "<<kp1.size()<<", "<<kp2.size()<<endl; 41 42 // 可视化， 显示关键点 43 cv::Mat imgShow; 44 cv::drawKeypoints( rgb1, kp1, imgShow, cv::Scalar::all(-1), cv::DrawMatchesFlags::DRAW_RICH_KEYPOINTS ); 45 cv::imshow( "keypoints", imgShow ); 46 cv::imwrite( "./data/keypoints.png", imgShow ); 47 cv::waitKey(0); //暂停等待一个按键 48 49 // 计算描述子 50 cv::Mat desp1, desp2; 51 _descriptor->compute( rgb1, kp1, desp1 ); 52 _descriptor->compute( rgb2, kp2, desp2 ); 53 54 // 匹配描述子 55 vector< cv::DMatch > matches; 56 cv::FlannBasedMatcher matcher; 57 matcher.match( desp1, desp2, matches ); 58 cout<<"Find total "<<matches.size()<<" matches."<<endl; 59 60 // 可视化：显示匹配的特征 61 cv::Mat imgMatches; 62 cv::drawMatches( rgb1, kp1, rgb2, kp2, matches, imgMatches ); 63 cv::imshow( "matches", imgMatches ); 64 cv::imwrite( "./data/matches.png", imgMatches ); 65 cv::waitKey( 0 ); 66 67 // 筛选匹配，把距离太大的去掉 68 // 这里使用的准则是去掉大于四倍最小距离的匹配 69 vector< cv::DMatch > goodMatches; 70 double minDis = 9999; 71 for ( size_t i=0; i<matches.size(); i++ ) 72 { 73 if ( matches[i].distance < minDis ) 74 minDis = matches[i].distance; 75 } 76 77 for ( size_t i=0; i<matches.size(); i++ ) 78 { 79 if (matches[i].distance < 4*minDis) 80 goodMatches.push_back( matches[i] ); 81 } 82 83 // 显示 good matches 84 cout<<"good matches="<<goodMatches.size()<<endl; 85 cv::drawMatches( rgb1, kp1, rgb2, kp2, goodMatches, imgMatches ); 86 cv::imshow( "good matches", imgMatches ); 87 cv::imwrite( "./data/good_matches.png", imgMatches ); 88 cv::waitKey(0); 89 90 // 计算图像间的运动关系 91 // 关键函数：cv::solvePnPRansac() 92 // 为调用此函数准备必要的参数 93 94 // 第一个帧的三维点 95 vector<cv::Point3f> pts_obj; 96 // 第二个帧的图像点 97 vector< cv::Point2f > pts_img; 98 99 // 相机内参 100 CAMERA_INTRINSIC_PARAMETERS C; 101 C.cx = 325.5; 102 C.cy = 253.5; 103 C.fx = 518.0; 104 C.fy = 519.0; 105 C.scale = 1000.0; 106 107 for (size_t i=0; i<goodMatches.size(); i++) 108 { 109 // query 是第一个, train 是第二个 110 cv::Point2f p = kp1[goodMatches[i].queryIdx].pt; 111 // 获取d是要小心！x是向右的，y是向下的，所以y才是行，x是列！ 112 ushort d = depth1.ptr<ushort>( int(p.y) )[ int(p.x) ]; 113 if (d == 0) 114 continue; 115 pts_img.push_back( cv::Point2f( kp2[goodMatches[i].trainIdx].pt ) ); 116 117 // 将(u,v,d)转成(x,y,z) 118 cv::Point3f pt ( p.x, p.y, d ); 119 cv::Point3f pd = point2dTo3d( pt, C ); 120 pts_obj.push_back( pd ); 121 } 122 123 double camera_matrix_data[3][3] = { 124 {C.fx, 0, C.cx}, 125 {0, C.fy, C.cy}, 126 {0, 0, 1} 127 }; 128 129 // 构建相机矩阵 130 cv::Mat cameraMatrix( 3, 3, CV_64F, camera_matrix_data ); 131 cv::Mat rvec, tvec, inliers; 132 // 求解pnp 133 cv::solvePnPRansac( pts_obj, pts_img, cameraMatrix, cv::Mat(), rvec, tvec, false, 100, 1.0, 100, inliers ); 134 135 cout<<"inliers: "<<inliers.rows<<endl; 136 cout<<"R="<<rvec<<endl; 137 cout<<"t="<<tvec<<endl; 138 139 // 画出inliers匹配 140 vector< cv::DMatch > matchesShow; 141 for (size_t i=0; i<inliers.rows; i++) 142 { 143 matchesShow.push_back( goodMatches[inliers.ptr<int>(i)[0]] ); 144 } 145 cv::drawMatches( rgb1, kp1, rgb2, kp2, matchesShow, imgMatches ); 146 cv::imshow( "inlier matches", imgMatches ); 147 cv::imwrite( "./data/inliers.png", imgMatches ); 148 cv::waitKey( 0 ); 149 150 return 0; 151 }