爱折腾-创客智造实验室 - 一起做rgb-d slam
type: channel
link: https://www.ncnynl.com/tag/一起做rgb-d-slam
items: 一起做RGB-D SLAM-目录
items: 一起做RGB-D SLAM (1)-前言及准备
items: 一起做RGB-D SLAM (2)-从图像到点云
items: 一起做RGB-D SLAM (3)-特征提取与配准
items: 一起做RGB-D SLAM (4)-点云拼接
items: 一起做RGB-D SLAM (5)-Visual Odometry (视觉里程计)
items: 一起做RGB-D SLAM (6)-图优化工具g2o的入门
items: 一起做RGB-D SLAM (7)-添加回环检测
items: 一起做RGB-D SLAM (8)-关于调试与补充内容

一起做RGB-D SLAM-目录
type: item
date: 2017-01-22T06:03:00+00:00
description: 一起做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)-前言及准备
type: item
date: 2017-01-22T06:04:00+00:00
description: 一起做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_…

一起做RGB-D SLAM (2)-从图像到点云
type: item
date: 2017-01-22T06:04:00+00:00
description: 一起做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.…

一起做RGB-D SLAM (3)-特征提取与配准
type: item
date: 2017-01-22T06:05:00+00:00
description: 一起做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; …

一起做RGB-D SLAM (4)-点云拼接
type: item
date: 2017-01-22T06:05:00+00:00
description: 一起做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( matche…

一起做RGB-D SLAM (5)-Visual Odometry (视觉里程计)
type: item
date: 2017-01-22T06:05:00+00:00
description: 一起做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 …

一起做RGB-D SLAM (6)-图优化工具g2o的入门
type: item
date: 2017-01-22T06:06:00+00:00
description: 一起做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 代码如下： /***************************************************************…

一起做RGB-D SLAM (7)-添加回环检测
type: item
date: 2017-01-22T06:06:00+00:00
description: 一起做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, KEYFRAM…

一起做RGB-D SLAM (8)-关于调试与补充内容
type: item
date: 2017-01-22T06:06:00+00:00
description: 一起做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，而安装它们一般都需要系统依赖库，因此需要首先安装各种依赖项。系统版本不同，依…