一起做RGB-D SLAM-目录 说明： 介绍如何编程开发自己的RGB-D SLAM 本专栏是并不按照原教程的行文，而是笔记格式，摘取核心内容描述 SLAM： SLAM是Simultaneous Localization and Mapping的缩写，意为“同时定位与建图”。 它是指运动物体根据传感器的信息，一边计算自身位置，一边构建环境地图的过程。 目前，SLAM的应用领域主要有机器人、虚拟现实和增强现实。 其用途包括传感器自身的定位，以及后续的路径规划、场景理解。 作者： 昵称：半闲居士，SLAM专家 博客：http://www.cnblogs.com/gaoxiang12/ 目录： 一起做RGB-D SLAM-目录 一起做RGB-D SLAM (1) 一起做RGB-D SLAM (2) 一起做RGB-D SLAM (3) 一起做RGB-D SLAM (4) 一起做RGB-D SLAM (5) 一起做RGB-D SLAM (6) 一起做RGB-D SLAM (7) 一起做RGB-D SLAM (8) 参考： CMake实践：http://file.ncnynl.com/ros/CMake%20Practice.pdf 教程github库：https://github.com/gaoxiang12/rgbd-slam-tutorial-gx SLAMCN 一起做RGB-D SLAM
一起做RGB-D SLAM (1)-前言及准备 说明： 介绍基本的C程序结构和编译方法 前言： SLAM，即Simultaneous Localization and Mapping，中文译作同时定位与地图创建，是近几十年里机器人领域有重大发展的研究方向。 作为自主机器人的核心技术，SLAM在机器人导航、控制、生产等方面都有着重要的研究意义。尤其在二十一世纪，以视觉传感器为中心的视觉SLAM技术，在理论和方法上都经历了明显的转变与突破，正逐步从实验室研究迈向成熟的市场应用。 在国外研究如火如荼之际，它在国内的研究尚处于起步阶段。有关SLAM的中文资料、书籍更是难以一见。然而，随着机器人技术得到国家的重视，越来越多的青年研究者、学生正逐渐跨入这片领域。 目标： 通过自己写程序，才能真正懂得slam的原理 预备知识与编程环境： 编程环境：ubuntu 12.04。建议读者和我们使用一样或类似的环境。 如果你（出于酷或者其他什么原因）就是要用Arch/Fedora/Mac，请你自己配环境。 编程软件：OpenCV, PCL, g2o OpenCV OpenCV, 计算机视觉库 目前（2015.6）较好的版本是2.4系列，因为3.0系列还不完善 下载OpenCV源代码： http://opencv.org/downloads.html。 安装依赖： sudo apt-get install build-essential libgtk2.0-dev libjpeg-dev libtiff4-dev libjasper-dev libopenexr-dev cmake python-dev python-numpy python-tk libtbb-dev libeigen2-dev yasm libfaac-dev libopencore-amrnb-dev libopencore-amrwb-dev libtheora-dev libvorbis-dev libxvidcore-dev libx264-dev libqt4-dev libqt4-opengl-dev sphinx-common texlive-latex-extra libv4l-dev libdc1394-22-dev libavcodec-dev libavformat-dev libswscale-dev 把OpenCV解压到下载目录中，用cmake编译再安装. mkdir build cd build cmake .. make sudo make install 头文件在/usr/local/include/，里面有opencv和opencv2的头文件，基本只用opencv2。 库文件默认就在/usr/local/lib/ cmake就是linux下的C++管理工具，简单的代码你用g++一条条敲，再多些可以用Makefile来管理，cmake就是自动生成makefile的工具，比makefile集成度更高一些。 二进制安装OpenCV 2.4.9： sudo apt-get install libopencv-dev PCL PCL，Point Cloud Library，点云库 PCL官网(http://pointclouds.org) 在Ubuntu 1404或更低版本，你需要通过私有源来安装PCL库： sudo add-apt-repository ppa:v-launchpad-jochen-sprickerhof-de/pcl sudo apt-get update sudo apt-get install libpcl-all 在 Ubuntu 1604，PCL库已经存在于公共软件源中，直接安装即可： sudo apt-get install libpcl-dev pcl-tools 编写测试程序： 新slam的文件夹，里面再新建四个文件夹： bin 用来放编译好的可执行二进制文件 src 用来放源代码 lib 用来放编译好的库文件 include 用来放头文件 slam的文件夹里，新建文件CMakeLists.txt，用于确定如何编译。 touch CMakeLists.txt 图示： 编辑CMakeLists.txt文件，输入内容如下： CMAKE_MINIMUM_REQUIRED( VERSION 2.8 ) #设定版本 PROJECT( slam ) #设定工程名 SET( CMAKE_CXX_COMPILER "g++") #设定编译器 #设定可执行二进制文件的目录 SET( EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin) #设定存放编译出来的库文件的目录 SET( LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib) #并且把该目录设为连接目录 LINK_DIRECTORIES( ${PROJECT_SOURCE_DIR}/lib) #设定头文件目录 INCLUDE_DIRECTORIES( ${PROJECT_SOURCE_DIR}/include) #增加子文件夹，也就是进入源代码文件夹继续构建 ADD_SUBDIRECTORY( ${PROJECT_SOURCE_DIR}/src) 编写测试程序 在src/目录下，新建main.cpp touch main.cpp vim mian.cpp 内容如下： #include <iostream> int main(int argc, char**argv) { std::cout<<"Hello SLAM!"<<std::endl; return 0; } 在src/目录下，新建一个CMakeLists.txt touch CMakeLists.txt vim CMakeLists.txt 内容如下： ADD_EXECUTABLE( main main.cpp ) 在slam目录下，新建build, 在进入build目录，编译代码： mkdir build cd build cmake .. make cmake就会把这个main.cpp编译成一个叫做main的二进制，放在在bin/目录下， 如果你执行这个二进制，就会输出Hello SLAM的语句，如图： 参考： 《CMake实践》 本教程的github代码库：https://github.com/gaoxiang12/rgbd-slam-tutorial-gx
一起做RGB-D SLAM (2)-从图像到点云 说明： 介绍编写一个将图像转换为点云的程序 从图像到点云 最简单的点云地图即是把不同位置的点云进行拼接得到 使用RGB-D相机时，会从相机里读到两种数据：彩色图像和深度图像。 如果你有Kinect和ros，可以运行： roslaunch openni_launch openni.launch 彩色图像与深度图像就会发布在 /camera/rgb/image_color 和 /camera/depth_registered/image_raw 中。 你可以通过如下命令显示彩色图像： rosrun image_view image_view image:=/camera/rgb/image_color 也可以在Rviz里看到图像与点云的可视化数据 rosrun rviz rviz 效果图： 这两张图来自于nyuv2数据集：http://cs.nyu.edu/~silberman/datasets/ 原图格式是ppm和pgm的，被我转成了png格式 你可以直接另存为这两个图，也可以到我的git里面获取这两个图 实际Kinect里（或其他rgb-d相机里）直接采到的RGB图和深度图可能会有些小问题 有一些时差（约几到十几个毫秒）。这个时差的存在，会产生“RGB图已经向右转了，怎么深度图还没转”的感觉 光圈中心未对齐。因为深度毕竟是靠另一个相机获取的，所以深度传感器和彩色传感器参数可能不一致 深度图里有很多“洞”。因为RGB-D相机不是万能的，它有一个探测距离的限制啦！太远或太近的东西都是看不见的呢。 关于这些“洞”，我们暂时睁一只眼闭一只眼，不去理它。以后我们也可以靠双边bayes滤波器去填这些洞。 但是！这是RGB-D相机本身的局限性。软件算法顶多给它修修补补，并不能完全弥补它的缺陷 在我们给出的这两个图中，都进行了预处理。你可以认为“深度图就是彩色图里每个像素距传感器的距离” 把这两个图转成点云，因为计算每个像素的空间点位置，可是后面配准、拼图等一系列事情的基础. 从2D到3D（数学部分） 上面两个图像给出了机器人外部世界的一个局部的信息. 假设这个世界由一个点云来描述：X={x1,…,xn}.其中每一个点,有r,g,b,x,y,z一共6个分量，表示它们颜色与空间位置。 颜色方面，主要由彩色图像记录； 而空间位置，可以由图像和相机模型、姿态一起计算出来。 对于常规相机，SLAM里使用针孔相机模型: (图来自http://www.comp.nus.edu.sg/~cs4243/lecture/camera.pdf) 简而言之，一个空间点[x,y,z]和它在图像中的像素坐标[u,v,d] (d指深度数据) 的对应关系是这样的： 这个公式是从(x,y,z)推到(u,v,d)的。反之，我们也可以把它写成已知(u,v,d)，推导(x,y,z)的方式.公式如下： 根据这个公式就可以构建点云 如果相机发生了位移和旋转，那么只要对这些点进行位移和旋转操作即可 从2D到3D （编程部分） 代码根目录/src/ 文件夹中新建一个generatePointCloud.cpp文件 touch src/generatePointCloud.cpp 内容如下： // C++ 标准库 #include <iostream> #include <string> 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; // 相机内参 const double camera_factor = 1000; const double camera_cx = 325.5; const double camera_cy = 253.5; const double camera_fx = 518.0; const double camera_fy = 519.0; // 主函数 int main( int argc, char** argv ) { // 读取./data/rgb.png和./data/depth.png，并转化为点云 // 图像矩阵 cv::Mat rgb, depth; // 使用cv::imread()来读取图像 // API: http://docs.opencv.org/modules/highgui/doc/reading_and_writing_images_and_video.html?highlight=imread#cv2.imread rgb = cv::imread( "./data/rgb.png" ); // rgb 图像是8UC3的彩色图像 // depth 是16UC1的单通道图像，注意flags设置-1,表示读取原始数据不做任何修改 depth = cv::imread( "./data/depth.png", -1 ); // 点云变量 // 使用智能指针，创建一个空点云。这种指针用完会自动释放。 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_factor; 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(); cout<<"point cloud size = "<<cloud->points.size()<<endl; cloud->is_dense = false; pcl::io::savePCDFile( "./pointcloud.pcd", *cloud ); // 清除数据并退出 cloud->points.clear(); cout<<"Point cloud saved."<<endl; return 0; } 程序运行需要数据。请把上面的那两个图存放在代码根目录/data下。 OpenCV的imread函数读取图片。在OpenCV2里，图像是以矩阵(cv::MAt)作为基本的数据结构。 Mat结构既可以帮你管理内存、像素信息，还支持一些常见的矩阵运算，是非常方便的结构。 彩色图像含有R,G,B三个通道，每个通道占8个bit（也就是unsigned char），故称为8UC3（8位unsigend char, 3通道）结构。 而深度图则是单通道的图像，每个像素由16个bit组成（也就是C++里的unsigned short），像素的值代表该点离传感器的距离。通常1000的值代表1米，所以我们把camera_factor设置成1000. 这样，深度图里每个像素点的读数除以1000，就是它离你的真实距离了 我们按照“先列后行”的顺序，遍历了整张深度图。在这个双重循环中： for (int m = 0; m < depth.rows; m++) for (int n=0; n < depth.cols; n++) m指图像的行，n是图像的列。它和空间点的坐标系关系是这样的： 深度图第m行，第n行的数据可以使用depth.ptr(m) [n]来获取。其中，cv::Mat的ptr函数会返回指向该图像第m行数据的头指针。然后加上位移n后，这个指针指向的数据就是我们需要读取的数据啦 计算三维点坐标的公式我们已经给出过了，代码里原封不动地实现了一遍。我们根据这个公式，新增了一个空间点，并放入了点云中。最后，把整个点云存储为 ./data/pointcloud.pcd 文件 编译并运行 最后，我们在src/CMakeLists.txt里加入几行代码，告诉编译器我们希望编译这个程序。请在此文件中加入以下几行： # 增加PCL库的依赖 FIND_PACKAGE( PCL REQUIRED COMPONENTS common io ) # 增加opencv的依赖 FIND_PACKAGE( OpenCV REQUIRED ) # 添加头文件和库文件 ADD_DEFINITIONS( ${PCL_DEFINITIONS} ) INCLUDE_DIRECTORIES( ${PCL_INCLUDE_DIRS} ) LINK_LIBRARIES( ${PCL_LIBRARY_DIRS} ) ADD_EXECUTABLE( generate_pointcloud generatePointCloud.cpp ) TARGET_LINK_LIBRARIES( generate_pointcloud ${OpenCV_LIBS} ${PCL_LIBRARIES} ) 编译新的工程： cd build cmake .. make cd .. 如果编译通过，就可在bin目录下找到新写的二进制：generate_pointcloud 运行它： bin/generate_pointcloud 即可在data目录下生成点云文件。 现在，你肯定希望查看一下新生成的点云了。如果已经安装了pcl，就可以通过： pcl_viewer pointcloud.pcd 提示： 如果你打开点云，只看到红绿蓝三个方块，请按R重置视角。刚才你是站在原点盯着坐标轴看呢。 如果点云没有颜色，请按5显示颜色。 cmake过程可能有PCL的警告，如果你编译成功了，无视它即可。这是程序员的本能。
一起做RGB-D SLAM (3)-特征提取与配准 说明： 介绍2D图像坐标转换为3D空间坐标 介绍如何封装基本函数 介绍如何特征提取与配准 封装基本函数 在 代码根目录/include 下，我们新建了一个 slamBase.h 文件 touch include/slamBase.h vim include/slamBase.h 内容如下： /************************************************************************* > File Name: rgbd-slam-tutorial-gx/part III/code/include/slamBase.h > Author: xiang gao > Mail: gaoxiang12@mails.tsinghua.edu.cn > Created Time: 2015年07月18日 星期六 15时14分22秒 > 说明：rgbd-slam教程所用到的基本函数（C风格） ************************************************************************/ # 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 ); 我们把相机参数封装成了一个结构体，另外还声明了 image2PointCloud 和 point2dTo3d 两个函数 在 src/ 目录下新建 slamBase.cpp 文件 touch src/slamBase.cpp vim src/slamBase.cpp 内容如下： /************************************************************************* > File Name: src/slamBase.cpp > Author: xiang gao > Mail: gaoxiang12@mails.tsinghua.edu.cn > Implementation of slamBase.h > Created Time: 2015年07月18日 星期六 15时31分49秒 ************************************************************************/ #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; } 最后，在 src/CMakeLists.txt 中加入以下几行，将 slamBase.cpp 编译成一个库，供将来调用： ADD_LIBRARY( slambase slamBase.cpp ) TARGET_LINK_LIBRARIES( slambase ${OpenCV_LIBS} ${PCL_LIBRARIES} ) 把slamBase.cpp编译成 slamBase 库，并把该库里调到的OpenCV和PCL的部分，和相应的库链接起来 图像配准 数学部分 SLAM是由“定位”（Localization)和“建图”（Mapping)两部分构成的。 现在来看定位问题。 要求解机器人的运动，首先要解决这样一个问题：给定了两个图像，如何知道图像的运动关系呢？ 这个问题可以用基于特征的方法（feature-based）或直接的方法（direct method）来解。虽说直接法已经有了一定的发展，但目前主流的方法还是基于特征点的方式。在后者的方法中，首先你需要知道图像里的“特征”，以及这些特征的一一对应关系。 提示：由于OpenCV中没有提供ICP，我们在实现中使用PnP进行求解。 图像配准 编程实现 我们要匹配两对图像，并且计算它们的位移关系。它们分别是rgb1,png, rgb2.png, depth1.png, depth2.png. 人眼可以直观地看到第二个图是向左转动了一些 新建detectFeatures.cpp文件，实现特征提取 touch src/detectFeatures.cpp vim src/detectFeatures.cpp 实现代码： /************************************************************************* > File Name: detectFeatures.cpp > Author: xiang gao > Mail: gaoxiang12@mails.tsinghua.edu.cn > 特征提取与匹配 > Created Time: 2015年07月18日 星期六 16时00分21秒 ************************************************************************/ #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][4] = { {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; } 代码的解释： 第一部分： 18行至52行，特征的检测与描述子的计算。 要在一个图像里提取特征，第一步是计算“关键点”，然后，针对这些关键点周围的像素，计算其“描述子”。 在OpenCV中，分别由cv::FeatureDetector和cv::DescriptorExtractor来计算。 cv::Ptr<cv::FeatureDetector> _detector = cv::FeatureDetector::create( "SIFT" ); cv::Ptr<cv::DescriptorExtractor> _descriptor = cv:: DescriptorExtractor::create( "SIFT" ); 然后，使用 _detector->detect()函数提取关键点。 值得一提的是，_detector和_descriptor的类型可以由字符串指定。 如果你想构建FAST, SURF等特征，只需改后面的字符串即可。 关键点是一种cv::KeyPoint的类型。 你可以看它的API,可以看到，KeyPoint结构中带有 Point2f pt 这个成员变量，指这个关键点的像素坐标。 此外，有的关键点还有半径、角度等参数，画在图里就会像一个个的圆一样。 有了一组KeyPoint之后，就可以调用： _descriptor->compute( image, keypoint, descriptor ) 在 keypoint 上计算描述子。 描述子是一个cv::Mat的矩阵结构，它的每一行代表一个对应于Keypoint的特征向量。 当两个keypoint的描述子越相似，说明这两个关键点也就越相似。 我们正是通过这种相似性来检测图像之间的运动的。 第二部分：特征匹配（53行至88行） 接下来，我们对上述的描述子进行匹配。 在OpenCV中，你需要选择一个匹配算法，例如粗暴式（bruteforce），近似最近邻（Fast Library for Approximate Nearest Neighbour, FLANN）等等。 这里我们构建一个FLANN的匹配算法： vector< cv::DMatch > matches; cv::FlannBasedMatcher matcher; matcher.match( desp1, desp2, matches ); 匹配完成后，算法会返回一些 DMatch 结构。 该结构含有以下几个成员： queryIdx 源特征描述子的索引（也就是第一张图像）。 trainIdx 目标特征描述子的索引（第二个图像） distance 匹配距离，越大表示匹配越差。 有了匹配后，可以用drawMatch函数画出匹配的结果： 仅靠描述子的匹配似乎是太多了些，把许多不相似的东西也匹配起来了。(由于这两个图像只有水平旋转，所以水平的匹配线才是对的，其他的都是误匹配）。 因此，需要筛选一下这些匹配，例如，把distance太大的给去掉（源文件69到88行）。 筛选的准则是：去掉大于最小距离四倍的匹配。如图： 筛选之后，匹配就少了许多了，图像看起来也比较干净。 第三部分 求解PnP 求解PnP的核心是调用OpenCV里的SolvePnPRansac函数。该函数的接口如下（来自OpenCV在线API）： 这个函数需要输入一组匹配好的三维点: objectPoints 和一组二维图像点: imagePoints. 返回的结果是旋转向量 rvec 和平移向量tvec。其他的都是算法中的参数。因此，我们需要想办法构建这两组输入点，它们实际上就是从goodmatches里抽取来的。 由于我们已经提取了KeyPoint，知道了它们的像素坐标。那么，利用上一节提供的库函数，就可以直接转到三维点啦。代码如下：（因为很重要就又贴了一遍） // 计算图像间的运动关系 // 关键函数：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][11] = { {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 ); 求解完成后，rvec和tvec就含有了位移和旋转的信息，至此，我们已经成功地计算出两个图像的运动啦！ 经过了筛选，我们提供的匹配里还是存在误匹配的情况。 根据误匹配计算运动是不靠谱的。这时该怎么办呢？ OpenCV会利用一种“随机采样一致性”（Random Sample Consensus）的思路（见参考）。意思即为，在现有的匹配中随机取一部分，估计其运动。因为正确的匹配结果肯定是相似的，而误匹配的结果肯定满天乱飞。只要把收敛的结果取出来即可。 ransac适用于数据噪声比较大的场合。这对图像的inlier大概是这样的 仍有一些误差,不过可以通过调节我们之前的筛选过程以及之后的ransac参数，得到更好的结果 课后作业： 请把计算图像运动信息封装成一个函数，放进slamBase库中。 由于我们的程序变复杂了，出现了一些内部的参数，如特征点类型，筛选准则，ransac参数等等。 如果我们把这些参数值定义在源代码里，那么每修改一次，就要重新编译一遍程序。这非常麻烦。 所以，我们希望把参数定义在外部文件中，在程序刚开始时读取此文件。 这样一来，只要更改此文件即可完成参数的调节，不必重新编译程序了。 因此，请你完成一个读取参数的类，放进slamBase中。
一起做RGB-D SLAM (4)-点云拼接 说明： 介绍利用上节得到一个旋转向量与平移向量，把两张图像的点云给拼接起来，形成更大的点云 函数封装 上节函数封装进slamBase库中，编辑slamBase.h vim include/slamBase.h 代码如下： // 帧结构 struct FRAME { cv::Mat rgb, depth; //该帧对应的彩色图与深度图 cv::Mat desp; //特征描述子 vector<cv::KeyPoint> kp; //关键点 }; // PnP 结果 struct RESULT_OF_PNP { cv::Mat rvec, tvec; int inliers; }; // computeKeyPointsAndDesp 同时提取关键点与特征描述子 void computeKeyPointsAndDesp( FRAME& frame, string detector, string descriptor ); // estimateMotion 计算两个帧之间的运动 // 输入：帧1和帧2, 相机内参 RESULT_OF_PNP estimateMotion( FRAME& frame1, FRAME& frame2, CAMERA_INTRINSIC_PARAMETERS& camera ); 我们把关键帧和PnP的结果都封成了结构体，以便将来别的程序调用, 编辑slamBase.cpp vim src/slamBase.cpp 这两个函数的实现如下： // computeKeyPointsAndDesp 同时提取关键点与特征描述子 void computeKeyPointsAndDesp( FRAME& frame, string detector, string descriptor ) { cv::Ptr<cv::FeatureDetector> _detector; cv::Ptr<cv::DescriptorExtractor> _descriptor; cv::initModule_nonfree(); _detector = cv::FeatureDetector::create( detector.c_str() ); _descriptor = cv::DescriptorExtractor::create( descriptor.c_str() ); if (!_detector || !_descriptor) { cerr<<"Unknown detector or discriptor type !"<<detector<<","<<descriptor<<endl; return; } _detector->detect( frame.rgb, frame.kp ); _descriptor->compute( frame.rgb, frame.kp, frame.desp ); return; } // estimateMotion 计算两个帧之间的运动 // 输入：帧1和帧2 // 输出：rvec 和 tvec RESULT_OF_PNP estimateMotion( FRAME& frame1, FRAME& frame2, CAMERA_INTRINSIC_PARAMETERS& camera ) { static ParameterReader pd; vector< cv::DMatch > matches; cv::FlannBasedMatcher matcher; matcher.match( frame1.desp, frame2.desp, matches ); cout<<"find total "<<matches.size()<<" matches."<<endl; vector< cv::DMatch > goodMatches; double minDis = 9999; double good_match_threshold = atof( pd.getData( "good_match_threshold" ).c_str() ); 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 < good_match_threshold*minDis) goodMatches.push_back( matches[i] ); } cout<<"good matches: "<<goodMatches.size()<<endl; // 第一个帧的三维点 vector<cv::Point3f> pts_obj; // 第二个帧的图像点 vector< cv::Point2f > pts_img; // 相机内参 for (size_t i=0; i<goodMatches.size(); i++) { // query 是第一个, train 是第二个 cv::Point2f p = frame1.kp[goodMatches[i].queryIdx].pt; // 获取d是要小心！x是向右的，y是向下的，所以y才是行，x是列！ ushort d = frame1.depth.ptr<ushort>( int(p.y) )[ int(p.x) ]; if (d == 0) continue; pts_img.push_back( cv::Point2f( frame2.kp[goodMatches[i].trainIdx].pt ) ); // 将(u,v,d)转成(x,y,z) cv::Point3f pt ( p.x, p.y, d ); cv::Point3f pd = point2dTo3d( pt, camera ); pts_obj.push_back( pd ); } double camera_matrix_data[3][3] = { {camera.fx, 0, camera.cx}, {0, camera.fy, camera.cy}, {0, 0, 1} }; cout<<"solving pnp"<<endl; // 构建相机矩阵 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 ); RESULT_OF_PNP result; result.rvec = rvec; result.tvec = tvec; result.inliers = inliers.rows; return result; } 我们还实现了一个简单的参数读取类。 这个类读取一个参数的文本文件，能够以关键字的形式提供文本文件中的变量： vim include/slamBase.h 代码如下： // 参数读取类 class ParameterReader { public: ParameterReader( string filename="./parameters.txt" ) { ifstream fin( filename.c_str() ); if (!fin) { cerr<<"parameter file does not exist."<<endl; return; } while(!fin.eof()) { string str; getline( fin, str ); if (str[0] == '#') { // 以‘＃’开头的是注释 continue; } int pos = str.find("="); if (pos == -1) continue; string key = str.substr( 0, pos ); string value = str.substr( pos+1, str.length() ); data[key] = value; if ( !fin.good() ) break; } } string getData( string key ) { map<string, string>::iterator iter = data.find(key); if (iter == data.end()) { cerr<<"Parameter name "<<key<<" not found!"<<endl; return string("NOT_FOUND"); } return iter->second; } public: map<string, string> data; }; 它读的参数文件是长这个样子的： # 这是一个参数文件 # part 4 里定义的参数 detector=SIFT descriptor=SIFT good_match_threshold=4 # camera camera.cx=325.5; camera.cy=253.5; camera.fx=518.0; camera.fy=519.0; camera.scale=1000.0; 参数文件里，以“变量名=值”的形式定义变量。以井号开头的是注释啦 拼接点云 点云变换 要把OpenCV里的旋转向量、位移向量转换成这个矩阵 OpenCV认为旋转矩阵R，虽然有3×3那么大，自由变量却只有三个，不够节省空间。 所以在OpenCV里使用了一个向量来表达旋转。向量的方向是旋转轴，大小则是转过的弧度 先用罗德里格斯变换（Rodrigues）将旋转向量转换为矩阵，然后“组装”成变换矩阵 代码如下：vim src/joinPointCloud.cpp /************************************************************************* > File Name: src/jointPointCloud.cpp > Author: Xiang gao > Mail: gaoxiang12@mails.tsinghua.edu.cn > Created Time: 2015年07月22日 星期三 20时46分08秒 ************************************************************************/ #include<iostream> using namespace std; #include "slamBase.h" #include <opencv2/core/eigen.hpp> #include <pcl/common/transforms.h> #include <pcl/visualization/cloud_viewer.h> // Eigen ! #include <Eigen/Core> #include <Eigen/Geometry> int main( int argc, char** argv ) { //本节要拼合data中的两对图像 ParameterReader pd; // 声明两个帧，FRAME结构请见include/slamBase.h FRAME frame1, frame2; //读取图像 frame1.rgb = cv::imread( "./data/rgb1.png" ); frame1.depth = cv::imread( "./data/depth1.png", -1); frame2.rgb = cv::imread( "./data/rgb2.png" ); frame2.depth = cv::imread( "./data/depth2.png", -1 ); // 提取特征并计算描述子 cout<<"extracting features"<<endl; string detecter = pd.getData( "detector" ); string descriptor = pd.getData( "descriptor" ); computeKeyPointsAndDesp( frame1, detecter, descriptor ); computeKeyPointsAndDesp( frame2, detecter, descriptor ); // 相机内参 CAMERA_INTRINSIC_PARAMETERS camera; camera.fx = atof( pd.getData( "camera.fx" ).c_str()); camera.fy = atof( pd.getData( "camera.fy" ).c_str()); camera.cx = atof( pd.getData( "camera.cx" ).c_str()); camera.cy = atof( pd.getData( "camera.cy" ).c_str()); camera.scale = atof( pd.getData( "camera.scale" ).c_str() ); cout<<"solving pnp"<<endl; // 求解pnp RESULT_OF_PNP result = estimateMotion( frame1, frame2, camera ); cout<<result.rvec<<endl<<result.tvec<<endl; // 处理result // 将旋转向量转化为旋转矩阵 cv::Mat R; cv::Rodrigues( result.rvec, R ); Eigen::Matrix3d r; cv::cv2eigen(R, r); // 将平移向量和旋转矩阵转换成变换矩阵 Eigen::Isometry3d T = Eigen::Isometry3d::Identity(); Eigen::AngleAxisd angle(r); cout<<"translation"<<endl; Eigen::Translation<double,3> trans(result.tvec.at<double>(0,0), result.tvec.at<double>(0,1), result.tvec.at<double>(0,2)); T = angle; T(0,3) = result.tvec.at<double>(0,0); T(1,3) = result.tvec.at<double>(0,1); T(2,3) = result.tvec.at<double>(0,2); // 转换点云 cout<<"converting image to clouds"<<endl; PointCloud::Ptr cloud1 = image2PointCloud( frame1.rgb, frame1.depth, camera ); PointCloud::Ptr cloud2 = image2PointCloud( frame2.rgb, frame2.depth, camera ); // 合并点云 cout<<"combining clouds"<<endl; PointCloud::Ptr output (new PointCloud()); pcl::transformPointCloud( *cloud1, *output, T.matrix() ); *output += *cloud2; pcl::io::savePCDFile("data/result.pcd", *output); cout<<"Final result saved."<<endl; pcl::visualization::CloudViewer viewer( "viewer" ); viewer.showCloud( output ); while( !viewer.wasStopped() ) { } return 0; } 重点在于59至73行，讲述了这个转换的过程 变换完毕后，我们就得到了拼合的点云 额外： 已经实现了一个只有两帧的SLAM程序。然而，也许你还不知道，这已经是一个视觉里程计(Visual Odometry)啦！ 只要不断地把进来的数据与上一帧对比，就可以得到完整的运动轨迹以及地图了呢 要做完整的SLAM，还需要一些东西。以两两匹配为基础的里程计有明显的累积误差，我们需要通过回环检测来消除它。 先讲讲关键帧的处理，因为把每个图像都放进地图，会导致地图规模增长地太快，所以需要关键帧技术。 然后呢，我们要做一个SLAM后端，就要用到g2o
一起做RGB-D SLAM (5)-Visual Odometry (视觉里程计)
一起做RGB-D SLAM (5)-Visual Odometry (视觉里程计) 说明： 上节介绍两张图像间的匹配与运动估计 本节介绍视频流处理,完成一个视觉里程计(visual odometry)的编写 测试数据： 网盘：http://yun.baidu.com/s/1i33uvw5 (400多M) 打开这个数据集，你会看到里头有 和 两个文件夹，分别是RGB图与深度图。 前几讲的数据也是取自这里. 取自nyuv2数据集：http://cs.nyu.edu/~silberman/datasets/nyu_depth_v2.html 这可是一个国际上认可的，相当有名的数据集哦。 如果你想要跑自己的数据，当然也可以，不过需要你进行一些预处理啦 视觉里程计 什么是视觉里程计呢？ 简而言之，就是把新来的数据与上一帧进行匹配，估计其运动，然后再把运动累加起来的东西。 画成示意图的话，就是下面这个样子： 这实际上和滤波器很像，通过不断的两两匹配，估计机器人当前的位姿，过去的就给丢弃了。 这个思路比较简单，实际当中也比较有效，能够保证局部运动的正确性。 下面我们来实现一下visual odometry 函数封装： 首先工具函数：将cv的旋转矢量与位移矢量转换为变换矩阵，类型为Eigen::Isometry3d 编辑文件：vim src/slamBase.cpp 代码如下： // cvMat2Eigen Eigen::Isometry3d cvMat2Eigen( cv::Mat& rvec, cv::Mat& tvec ) { cv::Mat R; cv::Rodrigues( rvec, R ); Eigen::Matrix3d r; cv::cv2eigen(R, r); // 将平移向量和旋转矩阵转换成变换矩阵 Eigen::Isometry3d T = Eigen::Isometry3d::Identity(); Eigen::AngleAxisd angle(r); Eigen::Translation<double,3> trans(tvec.at<double>(0,0), tvec.at<double>(0,1), tvec.at<double>(0,2)); T = angle; T(0,3) = tvec.at<double>(0,0); T(1,3) = tvec.at<double>(0,1); T(2,3) = tvec.at<double>(0,2); return T; } 另一个函数：将新的帧合并到旧的点云里： // joinPointCloud // 输入：原始点云，新来的帧以及它的位姿 // 输出：将新来帧加到原始帧后的图像 PointCloud::Ptr joinPointCloud( PointCloud::Ptr original, FRAME& newFrame, Eigen::Isometry3d T, CAMERA_INTRINSIC_PARAMETERS& camera ) { PointCloud::Ptr newCloud = image2PointCloud( newFrame.rgb, newFrame.depth, camera ); // 合并点云 PointCloud::Ptr output (new PointCloud()); pcl::transformPointCloud( *original, *output, T.matrix() ); *newCloud += *output; // Voxel grid 滤波降采样 static pcl::VoxelGrid<PointT> voxel; static ParameterReader pd; double gridsize = atof( pd.getData("voxel_grid").c_str() ); voxel.setLeafSize( gridsize, gridsize, gridsize ); voxel.setInputCloud( newCloud ); PointCloud::Ptr tmp( new PointCloud() ); voxel.filter( *tmp ); return tmp; } 在parameters.txt中，我们增加了几个参数，以便调节程序的性能： # part 5 # 数据相关 # 起始与终止索引 start_index=1 end_index=700 # 数据所在目录 rgb_dir=../data/rgb_png/ rgb_extension=.png depth_dir=../data/depth_png/ depth_extension=.png # 点云分辨率 voxel_grid=0.02 # 是否实时可视化 visualize_pointcloud=yes # 最小匹配数量 min_good_match=10 # 最小内点 min_inliers=5 # 最大运动误差 max_norm=0.3 前面几个参数是相当直观的：指定RGB图与深度图所在的目录，起始与终止的图像索引（也就是第1张到第700张的slam啦）。 后面几个参数，会在后面进行解释。 实现VO 最后，利用之前写好的工具函数，实现一个VO: vim src/visualOdometry.cpp 代码如下： /************************************************************************* > File Name: rgbd-slam-tutorial-gx/part V/src/visualOdometry.cpp > Author: xiang gao > Mail: gaoxiang12@mails.tsinghua.edu.cn > Created Time: 2015年08月01日 星期六 15时35分42秒 ************************************************************************/ #include <iostream> #include <fstream> #include <sstream> using namespace std; #include "slamBase.h" // 给定index，读取一帧数据 FRAME readFrame( int index, ParameterReader& pd ); // 度量运动的大小 double normofTransform( cv::Mat rvec, cv::Mat tvec ); int main( int argc, char** argv ) { ParameterReader pd; int startIndex = atoi( pd.getData( "start_index" ).c_str() ); int endIndex = atoi( pd.getData( "end_index" ).c_str() ); // initialize cout<<"Initializing ..."<<endl; int currIndex = startIndex; // 当前索引为currIndex FRAME lastFrame = readFrame( currIndex, pd ); // 上一帧数据 // 我们总是在比较currFrame和lastFrame string detector = pd.getData( "detector" ); string descriptor = pd.getData( "descriptor" ); CAMERA_INTRINSIC_PARAMETERS camera = getDefaultCamera(); computeKeyPointsAndDesp( lastFrame, detector, descriptor ); PointCloud::Ptr cloud = image2PointCloud( lastFrame.rgb, lastFrame.depth, camera ); pcl::visualization::CloudViewer viewer("viewer"); // 是否显示点云 bool visualize = pd.getData("visualize_pointcloud")==string("yes"); int min_inliers = atoi( pd.getData("min_inliers").c_str() ); double max_norm = atof( pd.getData("max_norm").c_str() ); for ( currIndex=startIndex+1; currIndex<endIndex; currIndex++ ) { cout<<"Reading files "<<currIndex<<endl; FRAME currFrame = readFrame( currIndex,pd ); // 读取currFrame computeKeyPointsAndDesp( currFrame, detector, descriptor ); // 比较currFrame 和 lastFrame RESULT_OF_PNP result = estimateMotion( lastFrame, currFrame, camera ); if ( result.inliers < min_inliers ) //inliers不够，放弃该帧 continue; // 计算运动范围是否太大 double norm = normofTransform(result.rvec, result.tvec); cout<<"norm = "<<norm<<endl; if ( norm >= max_norm ) continue; Eigen::Isometry3d T = cvMat2Eigen( result.rvec, result.tvec ); cout<<"T="<<T.matrix()<<endl; //cloud = joinPointCloud( cloud, currFrame, T.inverse(), camera ); cloud = joinPointCloud( cloud, currFrame, T, camera ); if ( visualize == true ) viewer.showCloud( cloud ); lastFrame = currFrame; } pcl::io::savePCDFile( "data/result.pcd", *cloud ); return 0; } FRAME readFrame( int index, ParameterReader& pd ) { FRAME f; string rgbDir = pd.getData("rgb_dir"); string depthDir = pd.getData("depth_dir"); string rgbExt = pd.getData("rgb_extension"); string depthExt = pd.getData("depth_extension"); stringstream ss; ss<<rgbDir<<index<<rgbExt; string filename; ss>>filename; f.rgb = cv::imread( filename ); ss.clear(); filename.clear(); ss<<depthDir<<index<<depthExt; ss>>filename; f.depth = cv::imread( filename, -1 ); return f; } double normofTransform( cv::Mat rvec, cv::Mat tvec ) { return fabs(min(cv::norm(rvec), 2*M_PI-cv::norm(rvec)))+ fabs(cv::norm(tvec)); } FRAME readFrame( int index, ParameterReader& pd ) 是读取帧数据的函数。告诉它我要读第几帧的数据，它就会乖乖的把数据给找出来，返回一个FRAME结构体。 在得到匹配之后，我们判断了匹配是否成功，并把失败的数据丢弃。为什么这样做呢？ 因为之前的算法，对于任意两张图像都能做出一个结果。对于无关的图像，就明显是不对的。所以要去除匹配失败的情形。 如何检测匹配失败呢？我们采用了三个方法： 去掉goodmatch太少的帧，最少的goodmatch定义为：min_good_match=10 去掉solvePnPRASNAC里，inlier较少的帧，同理定义为：min_inliers=5 去掉求出来的变换矩阵太大的情况。因为假设运动是连贯的，两帧之间不会隔的太远：max_norm=0.3 经过这三道工序处理后，vo的结果基本能保持正确啦。图示： 提示： 当点云出现时，可按5显示颜色，然后按r重置视角，快速查看点云； 可以调节parameters.txt中的voxel_grid值来设置点云分辨率。0.01表示每1cm3 的格子里有一个点 如果在编译时期出现Link错误，请检查你是否链接到了PCL的visualization模块。如果实在不确定，就照着github源码抄一遍。 在运动时期，由于存在两张图像完全一样的情况，导致匹配时距离为0。由于本节程序的设置，这种情况会被当成没有匹配，导致VO丢失。请你自己fix一下这个bug，我在下一节当中也进行了检查。 参考： 本讲代码：https://github.com/gaoxiang12/rgbd-slam-tutorial-gx/tree/master/part%20V 数据链接：http://yun.baidu.com/s/1i33uvw5
一起做RGB-D SLAM (6)-图优化工具g2o的入门 说明： 上节介绍了如何使用两两匹配，搭建一个视觉里程计 本节介绍如何修复视觉里程计的不足 视觉里程计的不足： 一旦出现了错误匹配，整个程序就会跑飞。 误差会累积。常见的现象是：相机转过去的过程能够做对，但转回来之后则出现明显的偏差。 效率方面不尽如人意。在线的点云显示比较费时。 累积误差是里程计中不可避免的，后续的相机姿态依赖着前面的姿态。 想要保证地图的准确，必须要保证每次匹配都精确无误，而这是难以实现的。 所以，我们希望用更好的方法来做slam。 不仅仅考虑两帧的信息，而要把所有整的信息都考虑进来，成为一个全slam问题（full slam）。 下图为累积误差的一个例子。右侧是原有扫过的地图，左侧是新扫的，可以看到出现了明显的不重合。 所以，我们这一讲要介绍姿态图（pose graph），它是目前视觉slam里最常用的方法之一 姿态图（原理部分） 根据迭代策略的不同，又可分为Gauss-Netwon(GN)下山法，Levenberg-Marquardt(LM)方法等等。 这个问题也称为Bundle Adjustment(BA)，我们通常使用LM方法优化这个非线性平方误差函数。 BA方法是近年来视觉slam里用的很多的方法（所以很多研究者吐槽slam和sfm(structure from motion)越来越像了）。 早些年间（2005以前），人们还认为用BA求解slam非常困难，因为计算量太大。不过06年之后，人们注意到slam构建的ba问题的稀疏性质，所以用稀疏的BA算法（sparse BA）求解这个图，才使BA在slam里广泛地应用起来。 为什么说slam里的BA问题稀疏呢？因为同样的场景很少出现在许多位置中。这导致上面的pose graph中，图G离全图很远，只有少部分的节点存在直接边的联系。这就是姿态图的稀疏性。 求解BA的软件包有很多，感兴趣的读者可以去看wiki。我们这里介绍的g2o（Generalized Graph Optimizer），就是近年很流行的一个图优化求解软件包。下面我们通过实例代码，帮助大家入门g2o。 姿态图（实现部分） 安装g2o 下载并安装它：https://github.com/RainerKuemmerle/g2o 在ubuntu 12.04下，安装g2o步骤如下： 安装依赖库: sudo apt-get install libeigen3-dev libsuitesparse-dev libqt4-dev qt4-qmake libqglviewer-qt4-dev 1404或1604的最后一项改为 libqglviewer-dev 即可 解压g2o并编译安装,进入g2o的代码目录，并： mkdir build cd build cmake .. make sudo make install 你可以安装cmake-curses-gui这个包，通过gui来选择你想编译的g2o模块并设定cmake编译过程中的flags 例如，当你实在装不好上面的libqglviewer时，你可以选择不编译g2o可视化模块（把G2O_BUILD_APPS关掉)，这样即使没有libqglviewer，你也能编译过g2o 编译： cd build ccmake .. make sudo make install 安装成功后，你可以在/usr/local/include/g2o中找到它的头文件，而在/usr/local/lib中找到它的库文件 使用g2o 我们把g2o引入自己的cmake工程： # 添加g2o的依赖 # 因为g2o不是常用库，要添加它的findg2o.cmake文件 LIST( APPEND CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake_modules ) SET( G2O_ROOT /usr/local/include/g2o ) FIND_PACKAGE( G2O ) # CSparse FIND_PACKAGE( CSparse ) INCLUDE_DIRECTORIES( ${G2O_INCLUDE_DIR} ${CSPARSE_INCLUDE_DIR} ) 同时，在代码根目录下新建cmake_modules文件夹，把g2o代码目录下的cmake_modules里的东西都拷进来，保证cmake能够顺利找到g2o。 复制一个上一讲的visualOdometry.cpp，我们把它改成slamEnd.cpp： vim src/slamEnd.cpp 代码如下： /************************************************************************* > File Name: rgbd-slam-tutorial-gx/part V/src/visualOdometry.cpp > Author: xiang gao > Mail: gaoxiang12@mails.tsinghua.edu.cn > Created Time: 2015年08月15日 星期六 15时35分42秒 * add g2o slam end to visual odometry ************************************************************************/ #include <iostream> #include <fstream> #include <sstream> using namespace std; #include "slamBase.h" //g2o的头文件 #include <g2o/types/slam3d/types_slam3d.h> //顶点类型 #include <g2o/core/sparse_optimizer.h> #include <g2o/core/block_solver.h> #include <g2o/core/factory.h> #include <g2o/core/optimization_algorithm_factory.h> #include <g2o/core/optimization_algorithm_gauss_newton.h> #include <g2o/solvers/csparse/linear_solver_csparse.h> #include <g2o/core/robust_kernel.h> #include <g2o/core/robust_kernel_factory.h> #include <g2o/core/optimization_algorithm_levenberg.h> // 给定index，读取一帧数据 FRAME readFrame( int index, ParameterReader& pd ); // 估计一个运动的大小 double normofTransform( cv::Mat rvec, cv::Mat tvec ); int main( int argc, char** argv ) { // 前面部分和vo是一样的 ParameterReader pd; int startIndex = atoi( pd.getData( "start_index" ).c_str() ); int endIndex = atoi( pd.getData( "end_index" ).c_str() ); // initialize cout<<"Initializing ..."<<endl; int currIndex = startIndex; // 当前索引为currIndex FRAME lastFrame = readFrame( currIndex, pd ); // 上一帧数据 // 我们总是在比较currFrame和lastFrame string detector = pd.getData( "detector" ); string descriptor = pd.getData( "descriptor" ); CAMERA_INTRINSIC_PARAMETERS camera = getDefaultCamera(); computeKeyPointsAndDesp( lastFrame, detector, descriptor ); PointCloud::Ptr cloud = image2PointCloud( lastFrame.rgb, lastFrame.depth, camera ); pcl::visualization::CloudViewer viewer("viewer"); // 是否显示点云 bool visualize = pd.getData("visualize_pointcloud")==string("yes"); int min_inliers = atoi( pd.getData("min_inliers").c_str() ); double max_norm = atof( pd.getData("max_norm").c_str() ); /******************************* // 新增:有关g2o的初始化 *******************************/ // 选择优化方法 typedef g2o::BlockSolver_6_3 SlamBlockSolver; typedef g2o::LinearSolverCSparse< SlamBlockSolver::PoseMatrixType > SlamLinearSolver; // 初始化求解器 SlamLinearSolver* linearSolver = new SlamLinearSolver(); linearSolver->setBlockOrdering( false ); SlamBlockSolver* blockSolver = new SlamBlockSolver( linearSolver ); g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg( blockSolver ); g2o::SparseOptimizer globalOptimizer; // 最后用的就是这个东东 globalOptimizer.setAlgorithm( solver ); // 不要输出调试信息 globalOptimizer.setVerbose( false ); // 向globalOptimizer增加第一个顶点 g2o::VertexSE3* v = new g2o::VertexSE3(); v->setId( currIndex ); v->setEstimate( Eigen::Isometry3d::Identity() ); //估计为单位矩阵 v->setFixed( true ); //第一个顶点固定，不用优化 globalOptimizer.addVertex( v ); int lastIndex = currIndex; // 上一帧的id for ( currIndex=startIndex+1; currIndex<endIndex; currIndex++ ) { cout<<"Reading files "<<currIndex<<endl; FRAME currFrame = readFrame( currIndex,pd ); // 读取currFrame computeKeyPointsAndDesp( currFrame, detector, descriptor ); // 比较currFrame 和 lastFrame RESULT_OF_PNP result = estimateMotion( lastFrame, currFrame, camera ); if ( result.inliers < min_inliers ) //inliers不够，放弃该帧 continue; // 计算运动范围是否太大 double norm = normofTransform(result.rvec, result.tvec); cout<<"norm = "<<norm<<endl; if ( norm >= max_norm ) continue; Eigen::Isometry3d T = cvMat2Eigen( result.rvec, result.tvec ); cout<<"T="<<T.matrix()<<endl; // cloud = joinPointCloud( cloud, currFrame, T, camera ); // 向g2o中增加这个顶点与上一帧联系的边 // 顶点部分 // 顶点只需设定id即可 g2o::VertexSE3 *v = new g2o::VertexSE3(); v->setId( currIndex ); v->setEstimate( Eigen::Isometry3d::Identity() ); globalOptimizer.addVertex(v); // 边部分 g2o::EdgeSE3* edge = new g2o::EdgeSE3(); // 连接此边的两个顶点id edge->vertices() [0] = globalOptimizer.vertex( lastIndex ); edge->vertices() [1] = globalOptimizer.vertex( currIndex ); // 信息矩阵 Eigen::Matrix<double, 6, 6> information = Eigen::Matrix< double, 6,6 >::Identity(); // 信息矩阵是协方差矩阵的逆，表示我们对边的精度的预先估计 // 因为pose为6D的，信息矩阵是6*6的阵，假设位置和角度的估计精度均为0.1且互相独立 // 那么协方差则为对角为0.01的矩阵，信息阵则为100的矩阵 information(0,0) = information(1,1) = information(2,2) = 100; information(3,3) = information(4,4) = information(5,5) = 100; // 也可以将角度设大一些，表示对角度的估计更加准确 edge->setInformation( information ); // 边的估计即是pnp求解之结果 edge->setMeasurement( T ); // 将此边加入图中 globalOptimizer.addEdge(edge); lastFrame = currFrame; lastIndex = currIndex; } // pcl::io::savePCDFile( "data/result.pcd", *cloud ); // 优化所有边 cout<<"optimizing pose graph, vertices: "<<globalOptimizer.vertices().size()<<endl; globalOptimizer.save("./data/result_before.g2o"); globalOptimizer.initializeOptimization(); globalOptimizer.optimize( 100 ); //可以指定优化步数 globalOptimizer.save( "./data/result_after.g2o" ); cout<<"Optimization done."<<endl; globalOptimizer.clear(); return 0; } FRAME readFrame( int index, ParameterReader& pd ) { FRAME f; string rgbDir = pd.getData("rgb_dir"); string depthDir = pd.getData("depth_dir"); string rgbExt = pd.getData("rgb_extension"); string depthExt = pd.getData("depth_extension"); stringstream ss; ss<<rgbDir<<index<<rgbExt; string filename; ss>>filename; f.rgb = cv::imread( filename ); ss.clear(); filename.clear(); ss<<depthDir<<index<<depthExt; ss>>filename; f.depth = cv::imread( filename, -1 ); f.frameID = index; return f; } double normofTransform( cv::Mat rvec, cv::Mat tvec ) { return fabs(min(cv::norm(rvec), 2*M_PI-cv::norm(rvec)))+ fabs(cv::norm(tvec)); } 其中，大部分代码和上一讲是一样的，此外新增了几段g2o的初始化与简单使用。 使用g2o图优化的简要步骤：第一步，构建一个求解器：globalOptimizer // 选择优化方法 typedef g2o::BlockSolver_6_3 SlamBlockSolver; typedef g2o::LinearSolverCSparse< SlamBlockSolver::PoseMatrixType > SlamLinearSolver; // 初始化求解器 SlamLinearSolver* linearSolver = new SlamLinearSolver(); linearSolver->setBlockOrdering( false ); SlamBlockSolver* blockSolver = new SlamBlockSolver( linearSolver ); g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg( blockSolver ); g2o::SparseOptimizer globalOptimizer; // 最后用的就是这个东东 globalOptimizer.setAlgorithm( solver ); // 不要输出调试信息 globalOptimizer.setVerbose( false ); 然后，在求解器内添加点和边： // 添加点 g2o::VertexSE3* v = new g2o::VertexSE3(); // 设置点v ... globalOptimizer.addVertex( v ); // 添加边 g2o::EdgeSE3* edge = new g2o::EdgeSE3(); // 设置边 edge ... globalOptimizer.addEdge(edge); 最后，完成优化并存储优化结果： globalOptimizer.save("./data/result_before.g2o"); globalOptimizer.initializeOptimization(); globalOptimizer.optimize( 100 ); //可以指定优化步数 globalOptimizer.save( "./data/result_after.g2o" ); 代码解释: 顶点和边的类型 顶点和边有不同的类型，这要看我们想求解什么问题。由于我们是3D的slam，所以顶点取成了相机姿态：g2o::VertexSE3，而边则是连接两个VertexSE3的边：g2o::EdgeSE3。如果你想用别的类型的顶点（如2Dslam，路标点），你可以看看/usr/local/include/g2o/types/下的文件，基本上涵盖了各种slam的应用，应该能满足你的需求。 什么是SE3呢？简单地说，就是4×4的变换矩阵啦，也就是我们这里用的相机姿态了。更深层的解释需要李代数里的知识。相应的，2D slam就要用SE2作为姿态节点了。在我们引用<g2o/types/slam3d/types_slam3d.h>时，就已经把相关的点和边都包含进来了哦。 优化方法 g2o允许你使用不同的优化求解器（然而实际效果似乎差别不大）。你可以选用csparse, pcg, cholmod等等。 我们这里使用csparse为例啦。 g2o文件 g2o的优化结果是存储在一个.g2o的文本文件里的，你可以用gedit等编辑软件打开它，结构是这样的: 这个文件前面是顶点的定义，包含 ID, x,y,z,qx,qy,qz,qw。后边则是边的定义：ID1, ID2, dx, T 以及信息阵的上半角。实际上，你也可以自己写个程序去生成这样一个文件，交给g2o去优化，写文本文件不会有啥困难的啦。 这个文件也可以用g2o_viewer打开，你还能直观地看到里面的节点与边的位置。同时你可以选一个优化方法对该图进行优化，这样你可以直观地看到优化的过程哦。然而对于我们现在的VO例子来说，似乎没什么可以优化的…… 提示： 现在（2016.10）github上的g2o已经可以在14.04下正常编译安装了，所以本文当中有些迂回的安装步骤就没必要了。 请读者按照g2o的readme文件进行编译安装即可 参考： 本节github库：https://github.com/gaoxiang12/rgbd-slam-tutorial-gx/tree/master/part%20VI
一起做RGB-D SLAM (7)-添加回环检测 说明： 上节介绍了图优化软件g2o的使用。 本节介绍实现一个简单的回环检测程序，利用g2o提升slam轨迹与地图的质量，得到一个完整的slam程序 简单回环检测的流程 上一讲的程序离完整的slam还有哪些距离。主要说来有两点： 关键帧的提取。把每一帧都拼到地图是去是不明智的。因为帧与帧之间距离很近，导致地图需要频繁更新，浪费时间与空间。所以，我们希望，当机器人的运动超过一定间隔，就增加一个“关键帧”。最后只需把关键帧拼到地图里就行了。 回环的检测。回环的本质是识别曾经到过的地方。最简单的回环检测策略，就是把新来的关键帧与之前所有的关键帧进行比较，不过这样会导致越往后，需要比较的帧越多。所以，稍微快速一点的方法是在过去的帧里随机挑选一些，与之进行比较。更进一步的，也可以用图像处理/模式识别的方法计算图像间的相似性，对相似的图像进行检测。 把这两者合在一起，就得到了我们slam程序的基本流程。以下为伪码： slam流程大体上如此 例如在线跑的话呢，可以定时进行一次优化与拼图。 或者，在成功检测到回环时，同时检测这两个帧附近的帧，那样得到的边就更多啦。 再有呢，如果要做实用的程序，还要考虑机器人如何运动，如果跟丢了怎么进行恢复等一些实际的问题呢。 实现代码 代码依旧是在上一讲的代码上进行更改得来的。 由于是完整的程序，稍微有些长，请大家慢慢看： vim src/slam.cpp /************************************************************************* > File Name: rgbd-slam-tutorial-gx/part V/src/visualOdometry.cpp > Author: xiang gao > Mail: gaoxiang12@mails.tsinghua.edu.cn > Created Time: 2015年08月15日 星期六 15时35分42秒 * add g2o slam end to visual odometry * add keyframe and simple loop closure ************************************************************************/ #include <iostream> #include <fstream> #include <sstream> using namespace std; #include "slamBase.h" #include <pcl/filters/voxel_grid.h> #include <pcl/filters/passthrough.h> #include <g2o/types/slam3d/types_slam3d.h> #include <g2o/core/sparse_optimizer.h> #include <g2o/core/block_solver.h> #include <g2o/core/factory.h> #include <g2o/core/optimization_algorithm_factory.h> #include <g2o/core/optimization_algorithm_gauss_newton.h> #include <g2o/solvers/csparse/linear_solver_csparse.h> #include <g2o/core/robust_kernel.h> #include <g2o/core/robust_kernel_factory.h> #include <g2o/core/optimization_algorithm_levenberg.h> // 把g2o的定义放到前面 typedef g2o::BlockSolver_6_3 SlamBlockSolver; typedef g2o::LinearSolverCSparse< SlamBlockSolver::PoseMatrixType > SlamLinearSolver; // 给定index，读取一帧数据 FRAME readFrame( int index, ParameterReader& pd ); // 估计一个运动的大小 double normofTransform( cv::Mat rvec, cv::Mat tvec ); // 检测两个帧，结果定义 enum CHECK_RESULT {NOT_MATCHED=0, TOO_FAR_AWAY, TOO_CLOSE, KEYFRAME}; // 函数声明 CHECK_RESULT checkKeyframes( FRAME& f1, FRAME& f2, g2o::SparseOptimizer& opti, bool is_loops=false ); // 检测近距离的回环 void checkNearbyLoops( vector & frames, FRAME& currFrame, g2o::SparseOptimizer& opti ); // 随机检测回环 void checkRandomLoops( vector & frames, FRAME& currFrame, g2o::SparseOptimizer& opti ); int main( int argc, char** argv ) { // 前面部分和vo是一样的 ParameterReader pd; int startIndex = atoi( pd.getData( "start_index" ).c_str() ); int endIndex = atoi( pd.getData( "end_index" ).c_str() ); // 所有的关键帧都放在了这里 vector< FRAME > keyframes; // initialize cout<<"Initializing ..."<<endl; int currIndex = startIndex; // 当前索引为currIndex FRAME currFrame = readFrame( currIndex, pd ); // 当前帧数据 string detector = pd.getData( "detector" ); string descriptor = pd.getData( "descriptor" ); CAMERA_INTRINSIC_PARAMETERS camera = getDefaultCamera(); computeKeyPointsAndDesp( currFrame, detector, descriptor ); PointCloud::Ptr cloud = image2PointCloud( currFrame.rgb, currFrame.depth, camera ); /******************************* // 新增:有关g2o的初始化 *******************************/ // 初始化求解器 SlamLinearSolver* linearSolver = new SlamLinearSolver(); linearSolver->setBlockOrdering( false ); SlamBlockSolver* blockSolver = new SlamBlockSolver( linearSolver ); g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg( blockSolver ); g2o::SparseOptimizer globalOptimizer; // 最后用的就是这个东东 globalOptimizer.setAlgorithm( solver ); // 不要输出调试信息 globalOptimizer.setVerbose( false ); // 向globalOptimizer增加第一个顶点 g2o::VertexSE3* v = new g2o::VertexSE3(); v->setId( currIndex ); v->setEstimate( Eigen::Isometry3d::Identity() ); //估计为单位矩阵 v->setFixed( true ); //第一个顶点固定，不用优化 globalOptimizer.addVertex( v ); keyframes.push_back( currFrame ); double keyframe_threshold = atof( pd.getData("keyframe_threshold").c_str() ); bool check_loop_closure = pd.getData("check_loop_closure")==string("yes"); for ( currIndex=startIndex+1; currIndex<endIndex; currIndex++ ) { cout<<"Reading files "<<currIndex<<endl; FRAME currFrame = readFrame( currIndex,pd ); // 读取currFrame computeKeyPointsAndDesp( currFrame, detector, descriptor ); //提取特征 CHECK_RESULT result = checkKeyframes( keyframes.back(), currFrame, globalOptimizer ); //匹配该帧与keyframes里最后一帧 switch (result) // 根据匹配结果不同采取不同策略 { case NOT_MATCHED: //没匹配上，直接跳过 cout<<RED"Not enough inliers."<<endl; break; case TOO_FAR_AWAY: // 太近了，也直接跳 cout<<RED"Too far away, may be an error."<<endl; break; case TOO_CLOSE: // 太远了，可能出错了 cout<<RESET"Too close, not a keyframe"<<endl; break; case KEYFRAME: cout<<GREEN"This is a new keyframe"<<endl; // 不远不近，刚好 /** * This is important!! * This is important!! * This is important!! * (very important so I've said three times!) */ // 检测回环 if (check_loop_closure) { checkNearbyLoops( keyframes, currFrame, globalOptimizer ); checkRandomLoops( keyframes, currFrame, globalOptimizer ); } keyframes.push_back( currFrame ); break; default: break; } } // 优化 cout<<RESET"optimizing pose graph, vertices: "<<globalOptimizer.vertices().size()<<endl; globalOptimizer.save("./data/result_before.g2o"); globalOptimizer.initializeOptimization(); globalOptimizer.optimize( 100 ); //可以指定优化步数 globalOptimizer.save( "./data/result_after.g2o" ); cout<<"Optimization done."<<endl; // 拼接点云地图 cout<<"saving the point cloud map..."<<endl; PointCloud::Ptr output ( new PointCloud() ); //全局地图 PointCloud::Ptr tmp ( new PointCloud() ); pcl::VoxelGrid<PointT> voxel; // 网格滤波器，调整地图分辨率 pcl::PassThrough<PointT> pass; // z方向区间滤波器，由于rgbd相机的有效深度区间有限，把太远的去掉 pass.setFilterFieldName("z"); pass.setFilterLimits( 0.0, 4.0 ); //4m以上就不要了 double gridsize = atof( pd.getData( "voxel_grid" ).c_str() ); //分辨图可以在parameters.txt里调 voxel.setLeafSize( gridsize, gridsize, gridsize ); for (size_t i=0; i<keyframes.size(); i++) { // 从g2o里取出一帧 g2o::VertexSE3* vertex = dynamic_cast<g2o::VertexSE3*>(globalOptimizer.vertex( keyframes[i].frameID )); Eigen::Isometry3d pose = vertex->estimate(); //该帧优化后的位姿 PointCloud::Ptr newCloud = image2PointCloud( keyframes[i].rgb, keyframes[i].depth, camera ); //转成点云 // 以下是滤波 voxel.setInputCloud( newCloud ); voxel.filter( *tmp ); pass.setInputCloud( tmp ); pass.filter( *newCloud ); // 把点云变换后加入全局地图中 pcl::transformPointCloud( *newCloud, *tmp, pose.matrix() ); *output += *tmp; tmp->clear(); newCloud->clear(); } voxel.setInputCloud( output ); voxel.filter( *tmp ); //存储 pcl::io::savePCDFile( "./data/result.pcd", *tmp ); cout<<"Final map is saved."<<endl; globalOptimizer.clear(); return 0; } FRAME readFrame( int index, ParameterReader& pd ) { FRAME f; string rgbDir = pd.getData("rgb_dir"); string depthDir = pd.getData("depth_dir"); string rgbExt = pd.getData("rgb_extension"); string depthExt = pd.getData("depth_extension"); stringstream ss; ss<<rgbDir<<index<<rgbExt; string filename; ss>>filename; f.rgb = cv::imread( filename ); ss.clear(); filename.clear(); ss<<depthDir<<index<<depthExt; ss>>filename; f.depth = cv::imread( filename, -1 ); f.frameID = index; return f; } double normofTransform( cv::Mat rvec, cv::Mat tvec ) { return fabs(min(cv::norm(rvec), 2*M_PI-cv::norm(rvec)))+ fabs(cv::norm(tvec)); } CHECK_RESULT checkKeyframes( FRAME& f1, FRAME& f2, g2o::SparseOptimizer& opti, bool is_loops) { static ParameterReader pd; static int min_inliers = atoi( pd.getData("min_inliers").c_str() ); static double max_norm = atof( pd.getData("max_norm").c_str() ); static double keyframe_threshold = atof( pd.getData("keyframe_threshold").c_str() ); static double max_norm_lp = atof( pd.getData("max_norm_lp").c_str() ); static CAMERA_INTRINSIC_PARAMETERS camera = getDefaultCamera(); static g2o::RobustKernel* robustKernel = g2o::RobustKernelFactory::instance()->construct( "Cauchy" ); // 比较f1 和 f2 RESULT_OF_PNP result = estimateMotion( f1, f2, camera ); if ( result.inliers < min_inliers ) //inliers不够，放弃该帧 return NOT_MATCHED; // 计算运动范围是否太大 double norm = normofTransform(result.rvec, result.tvec); if ( is_loops == false ) { if ( norm >= max_norm ) return TOO_FAR_AWAY; // too far away, may be error } else { if ( norm >= max_norm_lp) return TOO_FAR_AWAY; } if ( norm <= keyframe_threshold ) return TOO_CLOSE; // too adjacent frame // 向g2o中增加这个顶点与上一帧联系的边 // 顶点部分 // 顶点只需设定id即可 if (is_loops == false) { g2o::VertexSE3 *v = new g2o::VertexSE3(); v->setId( f2.frameID ); v->setEstimate( Eigen::Isometry3d::Identity() ); opti.addVertex(v); } // 边部分 g2o::EdgeSE3* edge = new g2o::EdgeSE3(); // 连接此边的两个顶点id edge->vertices() [0] = opti.vertex( f1.frameID ); edge->vertices() [1] = opti.vertex( f2.frameID ); edge->setRobustKernel( robustKernel ); // 信息矩阵 Eigen::Matrix<double, 6, 6> information = Eigen::Matrix< double, 6,6 >::Identity(); // 信息矩阵是协方差矩阵的逆，表示我们对边的精度的预先估计 // 因为pose为6D的，信息矩阵是6*6的阵，假设位置和角度的估计精度均为0.1且互相独立 // 那么协方差则为对角为0.01的矩阵，信息阵则为100的矩阵 information(0,0) = information(1,1) = information(2,2) = 100; information(3,3) = information(4,4) = information(5,5) = 100; // 也可以将角度设大一些，表示对角度的估计更加准确 edge->setInformation( information ); // 边的估计即是pnp求解之结果 Eigen::Isometry3d T = cvMat2Eigen( result.rvec, result.tvec ); edge->setMeasurement( T.inverse() ); // 将此边加入图中 opti.addEdge(edge); return KEYFRAME; } void checkNearbyLoops( vector & frames, FRAME& currFrame, g2o::SparseOptimizer& opti ) { static ParameterReader pd; static int nearby_loops = atoi( pd.getData("nearby_loops").c_str() ); // 就是把currFrame和 frames里末尾几个测一遍 if ( frames.size() <= nearby_loops ) { // no enough keyframes, check everyone for (size_t i=0; i<frames.size(); i++) { checkKeyframes( frames[i], currFrame, opti, true ); } } else { // check the nearest ones for (size_t i = frames.size()-nearby_loops; i<frames.size(); i++) { checkKeyframes( frames[i], currFrame, opti, true ); } } } void checkRandomLoops( vector & frames, FRAME& currFrame, g2o::SparseOptimizer& opti ) { static ParameterReader pd; static int random_loops = atoi( pd.getData("random_loops").c_str() ); srand( (unsigned int) time(NULL) ); // 随机取一些帧进行检测 if ( frames.size() <= random_loops ) { // no enough keyframes, check everyone for (size_t i=0; i<frames.size(); i++) { checkKeyframes( frames[i], currFrame, opti, true ); } } else { // randomly check loops for (int i=0; i<random_loops; i++) { int index = rand()%frames.size(); checkKeyframes( frames[index], currFrame, opti, true ); } } } 代码注解： 回环检测是很怕"false positive"的，即“将实际上不同的地方当成了同一处”，这会导致地图出现明显的不一致。所以，在使用g2o时，要在边里添加"robust kernel"，保证一两个错误的边不会影响整体结果。 我在slambase.h里添加了一些彩色输出代码。运行此程序时，出现绿色信息则是添加新的关键帧，红色为出错。 parameters.txt里定义了检测回环的一些参数： #part 7 keyframe_threshold=0.1 max_norm_lp=5.0 # Loop closure check_loop_closure=yes nearby_loops=5 random_loops=5 其中，nearby_loops就是m，random_loops就是n啦。 这两个数如果设大一些，匹配的帧就会多，不过太大了就会影响整体速度了呢。 回环检测的效果 对代码进行编译，然后bin/slam即可看到程序运行啦。 添加了回环检测之后呢，g2o文件就不会像上次那样孤零零的啦，看起来是这样子的： 是不是感觉整条轨迹“如丝般顺滑”了呢？它不再是上一讲那样一根筋通到底，而是有很多帧间的匹配数据，保证了一两帧出错能被其他匹配数据给“拉回来”。 百度云上的数据最后拼出来是这样的哦（780帧，关键帧62张，帧率5Hz左右）： 咖啡台左侧有明显的人通过的痕迹，导致地图上出现了他的身影： 这个就可以算作是基本的地图啦。 至此，slam的两大目标：“轨迹”和“地图”我们都已得到了，可以算是基本上解决了这个问题了。 后话： slam仍是一个开放的问题，尽管有人曾说“在slam领域发文章越来越难”，然而现在机器人几大期刊和会议（IJRR/TRO/RAM/JFD/ICRA/IROS...）仍有不少slam方面的文章。虽然我们“获取轨迹与地图”的目标已基本实现。 但仍有许多工作等我们去做，包括： 更好的数学模型（新的滤波器/图优化理论）； 新的视觉特征/不使用特征的直接方法； 动态物体/人的处理； 地图描述/点云地图优化/语义地图 长时间/大规模/自动化slam 参考： 本讲代码：https://github.com/gaoxiang12/rgbd-slam-tutorial-gx/tree/master/part%20VII 数据：http://yun.baidu.com/s/1i33uvw5
一起做RGB-D SLAM (8)-关于调试与补充内容 说明： 介绍一些调试 介绍相关问题及解决 Q&A： Q: OpenCV的版本？ A: 我用的是2.4.9。可以使用2.4系列，不会有太大差别。而3.0系列则在接口上有一些明显的改动，可能需要修改源码。 Q: 第二讲的点云如何查看？ A: 使用pcl_viewer。当点云显示时，先用r键复位视角，再用鼠标调整。 Q: 如何调试代码？ A: 可以在源程序里用cout, cerr输出，善用linux的管道与IO流即可调试。如需断点，可用gdb, ddd等工具（虽然我不怎么用）。 Q：《一起做》系列的（6）、（7）两篇源程序在编译时通不过，错误如下图： A:该问题的具体原因不明，大家揣测是库冲突，出现在使用从帮主博客《一起做6》里给出的g2o的地址下载的g2o源代码进行编译安装时，解决的方法是，使用帮主在qq讨论群里给出的g2o的压缩包进行编译安装。这个版本的g2o源代码，估计帮主会放在自己的github里，与《一起做》系列的源代码打包在一起供各位小萝卜们下载学习。（我已经更新了我的git） 问题（一）的衍生问题，出现问题（一）并按照上述问题解决方案进行解决后出现运行g2o_viewer时的core dumped（或者cannot open library: libg2o_solver_eigen.so）问题，猜测原因是之前编译安装的g2o的相关共享库没有清除干净，解决方法为：（1）删除/usr/local/include/g2o，指令为sudo rm -rf /usr/local/include/g2o；（2）删除/usr/local/lib下有关libg2o_*.so的库文件，先进入目录cd /usr/local/lib，然后挨个（可多个同时）删除sudo rm -rf libg2o_*.so libg2o_*.so libg2o_*.so（我一个一个删的我会说……） 这个是和编译安装新源代码相关的问题，还是和g2o有关，具体是我的系统是12.04的，昨天手滑点了更新，结果导致pcl_viewer 使用不了了，而且看提示好像很多pcl相关的库都出现了问题，于是，我重装了系统，然后再把相关的软件都安装后，g2o_viewer运行不了了，问题是error while loading shared libraries: libg2o_viewer.so: cannot open shared object file: No such file or directory, 经帮主指点，发现问题的原因是在默认情况下，编译器只会使用/lib和/usr/lib这两个目录下的库文件，通过源码包进行安装时，如果不指定--prefix会将库安装在/usr/local目录下，而又没有在文件/etc/ld.so.conf中添加 /usr/local/lib这个目录。这样虽然安装了源码包，但是使用时仍然找不到相关的.so库，就会报错。也就是说系统不知道安装了源码包。网络上的解决方法是在/etc/ld.so.conf里添加绝对路径/usr/local/lib，作为一个新手，我实在不知道怎么样添加进去，用gedit编辑之后，不能保存，用echo /usr/local/lib >> /etc/ld.so.conf没反应，于是我就采用了奇葩方法：用gedit打开/etc/ld.so.conf，然后添加/usr/local/lib ，再把这个文件保存到home，然后sudo cp -i ld.so.conf /etc/ld.so.conf，之后，执行ldconfig（这一步是必须的，目的是利用ldconfig执行文件将 /etc/ld.so.conf的数据读入高速缓存中），再次运行g2o_viewer result_after.g2o(跑通《一起做7》，在data文件夹下生成的),成功！ Q：用的系统是什么版本？ A: 我使用的系统是Ubuntu14.04 for ros(indigo)，与群主的Ubuntu12.04不同，是易科机器人社区发布的镜像，网上可下载到。提供一个网址：http://blog.exbot.net/archives/1206 小萝卜2号同时是ros使用者，所以用U盘刻录，然后安装，如有不懂，可到网上搜索具体安装过程。这个镜像对于使用ros 的初学者非常方便，里面装好了集成环境，不必浪费很多时间去配置软件，想用ros又想节省时间的小萝卜N号们可以考虑下 Q: Ubuntu14.04和Ubuntu 12.04的区别是什么？ A: 这里我们要讲下依赖问题。在跑群主程序过程中，我们用到三个库opencv、PCL和g2o，而安装它们一般都需要系统依赖库，因此需要首先安装各种依赖项。系统版本不同，依赖库不同，而我们用到的库是基于某个依赖库开发的，其他版本（如高版本）的依赖库可能不识别，导致无法编译成功。因此，建议安装时参考库的说明文档，有安装要求和步骤，尽量按照说明安装。 Q: OpenCV的版本？ A: 我用的是opencv2.4.10，默认安装方式。群主说2.4系列差别不大，而3.0系列则在接口上有一些明显改动，可能需要修改源码。个人觉得，2.4.9和2.4.10没差别，无影响。 Q: PCL 安装问题？ A: 我开始用的是Ubuntu12.04，在用命令行安装PCL时，提示找不到依赖项，缺缺缺……于是采用源码安装（参考古月居的博客http://blog.csdn.net/hcx25909/article/details/8651840），装了依赖库，原因未知，还是编译通不过，缺依赖项，缺缺缺……源码安装行不通，果断改装系统，Ubuntu14.04 for ros（indigo），简单又粗暴吧，之后再用三句命令行安装PCL，完全没问题。建议：如果非要用源码安装，每个人电脑配置不同，遇到的问题不同，若实在解决不了，就忍痛割爱，系统重来。 Q: g2o安装问题？ A: 对于g2o，最好是用群主github中提供的，一般Ubuntu12.04编译没有问题。我尝试了三种方法，一一解析。（1）不过我的Ubuntu14.04中libQGLViewer有问题，libqglviewer-qt4-dev安装不了，尝试下载debin安装，依然会提示缺少依赖项。（2）所以干脆舍弃g2o的可视化模块g2o_viewer，安装cmake-curses-gui这个包，通过gui来选择想编译的g2o模块， 找到build apps，enter键修改状态为off，make，继续sudo make install,安装相应的lib、头文件、执行文件。缺少显示模块，只是无法用g2o显示图像，不过该有的数据还是都有的。（3）另外，若官网上下载最新g2o，参考安装说明文档，在Ubuntu14.04中编译也能神奇通过，但是在跑“一起做6和7”时，提示错误：core dumped。重启系统，始终不行，原因未知，考虑是库冲突。所以不推荐。 尝试安装libqglviewer-qt4-dev_2.3.4.4_amd64.deb的过程中的错误，大家可以清楚看到n重依赖以及相互冲突： Q: 用PCL的cloudviewer把点云显示出来，为什么会是上下颠倒? A: 关于图像上下翻转问题，是因为opencv定义的坐标系和pcl_viewer显示坐标系不同，opencv是x右y下，而pcl显示是x右y上。解决方法：找到群主程序image2PointCloud函数中，把计算点空间坐标的公式的p.y值添加负号，这样y方向就可以正常显示了,so easy。(或许还有别的方法) Q: 文件路径问题 A: 上面的错误是跑3时的一个提示，是说没有读到数据，没有点云。读取的parameters.txt以及存储的点云数据，最好给出具体的路径。如：/home/document/RGBD_tutorial/part3/code/data/，这样程序才能读取到数据或者保存到正确的文件夹里。 Q: 在编译6和7时遇到c++11不兼容的问题？ A: 在确保程序编译通过，而在运行时遇到c++0x_warning.h:32:2 error，我曾尝试在CMakeLists.txt中添加一段代码，然并没卵用，或许有人可以的（可尝试）。参考：http://blog.csdn.net/zyh821351004/article/details/46521319 添加的代码： 多半是因为用的不是群主github上提供的g2o版本，于是g2o重装，程序顺利运行，听着机子跑程序时哗哗的声音，是不是内心无比高兴。 Q: 在cmake时会出现类似的WARNING？ A: 注意这个是warning，不是error，不影响编译。原因可能是cmake里的find_package命令把/usr/lib给找出来了，引发了warning。（群主告诉偶滴，直接忽略） ubuntu 1404下是可以用g2o_viewer的，只要你手动把依赖项配好。一共有三个依赖项，配好之后g2o_viewer照常使用