1005人阅读
SLAM（28）
ROS vision（36）

开始做SLAM（机器人同时定位与建图）研究已经近一年了。从一年级开始对这个方向产生兴趣，到现在为止，也算是对这个领域有了大致的了解。然而越了解，越觉得这个方向难度很大。总体来讲有以下几个原因：
入门资料很少。虽然国内也有不少人在做，但这方面现在没有太好的入门教程。《SLAM for dummies》可以算是一篇。中文资料几乎没有。SLAM研究已进行了三十多年，从上世纪的九十年代开始。其中又有若干历史分枝和争论，要把握它的走向就很费工夫。难以实现。SLAM是一个完整的系统，由许多个分支模块组成。现在经典的方案是“图像前端，优化后端，闭环检测”的三部曲，很多文献看完了自己实现不出来。自己动手编程需要学习大量的先决知识。首先你要会C和C++，网上很多代码还用了11标准的C++。第二要会用Linux。第三要会cmake，vim/emacs及一些编程工具。第四要会用openCV, PCL, Eigen等第三方库。只有学会了这些东西之后，你才能真正上手编一个SLAM系统。如果你要跑实际机器人，还要会ROS。
　　当然，困难多意味着收获也多，坎坷的道路才能锻炼人（比如说走着走着才发现Linux和C++才是我的真爱之类的。）鉴于目前网上关于视觉SLAM的资料极少，我于是想把自己这一年多的经验与大家分享一下。说的不对的地方请大家批评指正。
　　这篇文章关注视觉SLAM，专指用摄像机，Kinect等深度像机来做导航和探索，且主要关心室内部分。到目前为止，室内的视觉SLAM仍处于研究阶段，远未到实际应用的程度。一方面，编写和使用视觉SLAM需要大量的专业知识，算法的实时性未达到实用要求；另一方面，视觉SLAM生成的地图（多数是点云）还不能用来做机器人的路径规划，需要科研人员进一步的探索和研究。以下，我会介绍SLAM的历史、理论以及实现的方式，且主要介绍视觉（Kinect）的实现方式。
2.&&&&SLAM问题
　　SLAM，全称叫做Simultaneous Localization and Mapping，中文叫做同时定位与建图。啊不行，这么讲下去，这篇文章肯定没有人读，所以我们换一个讲法。
3.&&&&小萝卜的故事
　　从前，有一个机器人叫“小萝卜”。它长着一双乌黑发亮的大眼睛，叫做Kinect。有一天，它被邪恶的科学家关进了一间空屋子，里面放满了杂七杂八的东西。
　　小萝卜感到很害怕，因为这个地方他从来没来过，一点儿也不了解。让他感到害怕的主要是三个问题：
　　1.&&&&&&&&&&自己在哪里？
　　2.&&&&&&&&&&这是什么地方？
　　3.&&&&&&&&&&怎么离开这个地方？
　　在SLAM理论中，第一个问题称为定位&(Localization)，第二个称为建图&(Mapping)，第三个则是随后的路径规划。我们希望借助Kinect工具，帮小萝卜解决这个难题。各位同学有什么思路呢？
4.&&&&Kinect数据
　　要打败敌人，首先要了解你的武器。不错，我们先介绍一下Kinect。众所周知这是一款深度相机，你或许还听说过别的牌子，但Kinect的价格便宜，测量范围在3m-12m之间，精度约3cm，较适合于小萝卜这样的室内机器人。它采到的图像是这个样子的（从左往右依次为rgb图，深度图与点云图）：
　　Kinect的一大优势在于能比较廉价地获得每个像素的深度值，不管是从时间上还是从经济上来说。OK，有了这些信息，小萝卜事实上可以知道它采集到的图片中，每一个点的3d位置。只要我们事先标定了Kinect，或者采用出厂的标定值。
　　我们把坐标系设成这个样子，这也是openCV中采用的默认坐标系。
　　o’-uv是图片坐标系，o-xyz是Kinect的坐标系。假设图片中的点为(u,v)，对应的三维点位置在(x,y,z)，那么它们之间的转换关系是这样的：
　　或者更简单的：
　　后一个公式给出了计算三维点的方法。先从深度图中读取深度数据（Kinect给的是16位无符号整数），除掉z方向的缩放因子，这样你就把一个整数变到了以米为单位的数据。然后，x,y用上面的公式算出。一点都不难，就是一个中心点位置和一个焦距而已。f代表焦距，c代表中心。如果你没有自己标定你的Kinect，也可以采用默认的值：s=5000, cx = 320, cy=240, fx=fy=525。实际值会有一点偏差，但不会太大。
5.&&&&定位问题
　　知道了Kinect中每个点的位置后，接下来我们要做的，就是根据两帧图像间的差别计算小萝卜的位移。比如下面两张图，后一张是在前一张之后1秒采集到的：
　　你肯定可以看出，小萝卜往右转过了一定的角度。但究竟转过多少度呢？这就要靠计算机来求解了。这个问题称为相机相对姿态估计，经典的算法是ICP（Iterative Closest Point，迭代最近点）。这个算法要求知道这两个图像间的一组匹配点，说的通俗点，就是左边图像哪些点和右边是一样的。你当然看见那块黑白相间的板子同时出现在两张图像中。在小萝卜看来，这里牵涉到两个简单的问题：特征点的提取和匹配。
　　如果你熟悉计算机视觉，那你应该听说过SIFT, SURF之类的特征。不错，要解决定位问题，首先要得到两张图像的一个匹配。匹配的基础是图像的特征，下图就是SIFT提取的关键点与匹配结果：
　　对实现代码感兴趣的同学请Google“opencv&匹配”即可，在openCV的教程上也有很明白的例子。上面的例子可以看出，我们找到了一些匹配，但其中有些是对的（基本平等的匹配线），有些是错的。这是由于图像中存在周期性出现的纹理（黑白块），所以容易搞错。但这并不是问题，在接下来的处理中我们会将这些影响消去。
　　得到了一组匹配点后，我们就可以计算两个图像间的转换关系，也叫PnP问题。它的模型是这样的：
　　R为相机的姿态，C为相机的标定矩阵。R是不断运动的，而C则是随着相机做死的。ICP的模型稍有不同，但原理上也是计算相机的姿态矩阵。原则上，只要有四组匹配点，就可以算这个矩阵。你可以调用openCV的SolvePnPRANSAC函数或者PCL的ICP算法来求解。openCV提供的算法是RANSAC（Random Sample Consensus，随机采样一致性）架构，可以剔除错误匹配。所以代码实际运行时，可以很好地找到匹配点。以下是一个结果的示例。
　　上面两张图转过了16.63度，位移几乎没有。
　　有同学会说，那只要不断匹配下去，定位问题不就解决了吗？表面上看来，的确是这样的，只要我们引入一个关键帧的结构（发现位移超过一个固定值时，定义成一个关键帧）。然后，把新的图像与关键帧比较就行了。至于建图，就是把这些关键帧的点云拼起来，看着还有模有样，煞有介事的：
1－200帧的匹配结果
　　然而，如果事情真这么简单，SLAM理论就不用那么多人研究三十多年了（它是从上世纪90年代开始研究的）（上面讲的那些东西简直随便哪里找个小硕士就能做出来……）。那么，问题难在什么地方呢？
6.&&&&SLAM端优化理论
　　最麻烦的问题，就是“噪声”。这种渐近式的匹配方式，和那些惯性测量设备一样，存在着累积噪声。因为我们在不断地更新关键帧，把新图像与最近的关键帧比较，从而获得机器人的位移信息。但是你要想到，如果有一个关键帧出现了偏移，那么剩下的位移估计都会多出一个误差。这个误差还会累积，因为后面的估计都基于前面的机器人位置……哇！这后果简直不堪设想啊（例如，你的机器人往右转了30度，再往左转了30度回到原来的位置。然而由于误差，你算成了向右转29度，再向左转31度，这样你构建的地图中，会出现初始位置的两个“重影”）。我们能不能想办法消除这个该死的误差呢？
　　朋友们，这才是SLAM的研究，前面的可以说是“图像前端”的处理方法。我们的解决思路是：如果你和最近的关键帧相比，会导致累计误差。那么，我们最好是和更前面的关键帧相比，而且多比较几个帧，不要只比较一次。
　　我们用数学来描述这个问题。设：
　　不要怕，只有借助数学才能把这个问题讲清楚。上面的公式中，xp是机器人小萝卜的位置，我们假定由n个帧组成。xL则是路标，在我们的图像处理过程中就是指SIFT提出来的关键点。如果你做2D SLAM，那么机器人位置就是x, y加一个转角theta。如果是3D SLAM，就是x,y,z加一个四元数姿态（或者rpy姿态）。这个过程叫做参数化（Parameterization）。
　　不管你用哪种参数，后面两个方程你都需要知道。前一个叫运动方程，描述机器人怎样运动。u是机器人的输入，w是噪声。这个方程最简单的形式，就是你能通过什么方式（码盘等）获得两帧间的位移差，那么这个方程就直接是上一帧与u相加即得。另外，你也可以完全不用惯性测量设备，这样我们就只依靠图像设备来估计，这也是可以的。
　　后一个方程叫观测方程，描述那些路标是怎么来的。你在第i帧看到了第j个路标，产生了一个测量值，就是图像中的横纵坐标。最后一项是噪声。偷偷告诉你，这个方程形式上和上一页的那个方程是一模一样的。
　　在求解SLAM问题前，我们要看到，我们拥有的数据是什么？在上面的模型里，我们知道的是运动信息u以及观测z。用示意图表示出来是这样的：
　　我们要求解的，就是根据这些u和z，确定所有的xp和xL。这就是SLAM问题的理论。从SLAM诞生开始科学家们就一直在解决这个问题。最初，我们用Kalman滤波器，所以上面的模型（运动方程和观测方程）被建成这个样子。直到21世纪初，卡尔曼滤波器仍在SLAM系统占据最主要的地位，Davison经典的单目SLAM就是用EKF做的。但是后来，出现了基于图优化的SLAM方法，渐渐有取而代之的地位[1]。我们在这里不介绍卡尔曼滤波器，有兴趣的同学可以在wiki上找卡尔曼滤波器，另有一篇中文的《卡尔曼滤波器介绍》也很棒。由于滤波器方法存储n个路标要消耗n平方的空间，在计算量上有点对不住大家。尽管08年有人提出分治法的滤波器能把复杂度弄到O(n)
[2]，但实现手段比较复杂。我们要介绍那种新兴的方法: Graph-based SLAM。
　　图优化方法把SLAM问题做成了一个优化问题。学过运筹学的同学应该明白，优化问题对我们有多么重要。我们不是要求解机器人的位置和路标位置吗？我们可以先做一个猜测，猜想它们大概在什么地方。这其实是不难的。然后呢，将猜测值与运动模型／观测模型给出的值相比较，可以算出误差：
　　通俗一点地讲，例如，我猜机器人第一帧在(0,0,0)，第二帧在(0,0,1)。但是u1告诉我机器人往z方向（前方）走了0.9米，那么运动方程就出现了0.1m的误差。同时，第一帧中机器人发现了路标1，它在该机器人图像的正中间；第二帧却发现它在中间偏右的位置。这时我们猜测机器人只是往前走，也是存在误差的。至于这个误差是多少，可以根据观测方程算出来。
　　我们得到了一堆误差，把这些误差平方后加起来（因为单纯的误差有正有负，然而平方误差可以改成其他的范数，只是平方更常用），就得到了平方误差和。我们把这个和记作phi，就是我们优化问题的目标函数。而优化变量就是那些个xp, xL。
　　改变优化变量，误差平方和（目标函数）就会相应地变大或变小，我们可以用数值方法求它们的梯度和二阶梯度矩阵，然后用梯度下降法求最优值。这些东西学过优化的同学都懂的。
　　注意到，一次机器人SLAM过程中，往往会有成千上万帧。而每一帧我们都有几百个关键点，一乘就是几百万个优化变量。这个规模的优化问题放到小萝卜的机载小破本上可解吗？是的，过去的同学都以为，Graph-based SLAM是无法计算的。但就在21世纪06，07年后，有些同学发现了，这个问题规模没有想象的那么大。上面的J和H两个矩阵是“稀疏矩阵”，于是呢，我们可以用稀疏代数的方法来解这个问题。“稀疏”的原因，在于每一个路标，往往不可能出现在所有运动过程中，通常只出现在一小部分图像里。正是这个稀疏性，使得优化思路成为了现实。
　　优化方法利用了所有可以用到的信息（称为full-SLAM, global SLAM），其精确度要比我们一开始讲的帧间匹配高很多。当然计算量也要高一些。
　　由于优化的稀疏性，人们喜欢用“图”来表达这个问题。所谓图，就是由节点和边组成的东西。我写成G={V,E}，大家就明白了。V是优化变量节点，E表示运动/观测方程的约束。什么，更糊涂了吗？那我就上一张图，来自[3]。
　　图有点模糊，而且数学符号和我用的不太一样，我用它来给大家一个图优化的直观形象。上图中，p是机器人位置，l是路标，z是观测，t是位移。其中呢，p, l是优化变量，而z,t是优化的约束。看起来是不是像一些弹簧连接了一些质点呢？因为每个路标不可能出现在每一帧中，所以这个图是蛮稀疏的。不过，“图”优化只是优化问题的一个表达形式，并不影响优化的含义。实际解起来时还是要用数值法找梯度的。这种思路在计算机视觉里，也叫做Bundle Adjustment。它的具体方法请参见一篇经典文章[4]。
　　不过，BA的实现方法太复杂，不太建议同学们拿C来写。好在2010年的ICRA上，其他的同学们提供了一个通用的开发包：g2o [5]。它是有图优化通用求解器，很好用，我改天再详细介绍这个软件包。总之，我们只要把观测和运动信息丢到求解器里就行。这个优化器会为我们求出机器人的轨迹和路标位置。如下图，红点是路标，蓝色箭头是机器人的位置和转角（2D SLAM）。细心的同学会发现它往右偏转了一些。：
7.&&&&闭环检测
　　上面提到，仅用帧间匹配最大的问题在于误差累积，图优化的方法可以有效地减少累计误差。然而，如果把所有测量都丢进g2o，计算量还是有点儿大的。根据我自己测试，约10000多条边，g2o跑起来就有些吃力了。这样，就有同学说，能把这个图构造地简洁一些吗？我们用不着所有的信息，只需要把有用的拿出来就行了。
　　事实上，小萝卜在探索房间时，经常会左转一下，右转一下。如果在某个时刻他回到了以前去过的地方，我们就直接与那时候采集的关键帧做比较，可以吗？我们说，可以，而且那是最好的方法。这个问题叫做闭环检测。
　　闭环检测是说，新来一张图像时，如何判断它以前是否在图像序列中出现过？有两种思路：一是根据我们估计的机器人位置，看是否与以前某个位置邻近；二是根据图像的外观，看它是否和以前关键帧相似。目前主流方法是后一种，因为很多科学家认为前一种依靠有噪声的位置来减少位置的噪声，有点循环论证的意思。后一种方法呢，本质上是个模式识别问题（非监督聚类，分类），常用的是Bag-of-Words (BOW)。但是BOW需要事先对字典进行训练，因此SLAM研究者仍在探讨有没有更合适的方法。
　　在Kinect SLAM经典大作中[6]，作者采用了比较简单的闭环方法：在前面n个关键帧中随机采k个，与当前帧两两匹配。匹配上后认为出现闭环。这个真是相当的简单实用，效率也过得去。
　　高效的闭环检测是SLAM精确求解的基础。这方面还有很多工作可以做。
8.&&&&小结
　　本文我们介绍了SLAM的基本概念，重点介绍了图优化解决SLAM问题的思路。我最近正在编写SLAM程序，它是一个Linux下基于cmake的工程。目前仍在开发当中。欢迎感兴趣的同学来交流研究心得，我的邮件是：gaoxiang12@mails.。
[1] Visual SLAM: Why filter? Strasdat et. al., Image and Vision Computing, 2012.
[2] Divide and Conquer: EKF SLAM in O(n), Paz Lina M et al., IEEE Transaction on Robotics, 2008
[3] Relative bundle adjustment, Sibley, Gabe, 2009
[4] Bundle adjustment - a Modern Synthesis. Triggs B et. el., Springer, 2000
[5] g2o: A General Framework for Graph Optimization, Kummerle Rainer, et. al., ICRA, 2011
[6] 3-D Mapping with an RGB-D Camera, IEEE Transaction on Robotics, Endres et al., 2014&
　　各位朋友，自从上一篇《视觉SLAM漫谈》写成以来已经有一段时间了。我收到几位热心读者的邮件。有的希望我介绍一下当前视觉SLAM程序的实用程度，更多的人希望了解一下前文提到的g2o优化库。因此我另写一篇小文章来专门介绍这个新玩意。
　　在开始本篇文章正文以前，我们先来回顾一下图优化SLAM问题的提法。至于SLAM更基础的内容，例如SLAM是什么东西等等，请参见上一篇文章。我们直接进入较深层次的讨论。首先，关于我们要做的事情，你可以这样想：
　　l& &已知的东西：传感器数据（图像，点云，惯性测量设备等）。我们的传感器主要是一个Kinect，因此数据就是一个视频序列，说的再详细点就是一个RGB位图序列与一个深度图序列。至于惯性测量设备，可以有也可以没有。
　　l&& 待求的东西：机器人的运动轨迹，地图的描述。运动轨迹，画出来应该就像是一条路径。而地图的描述，通常是点云的描述。但是点云描述是否可用于导航、规划等后续问题，还有待研究。
　　这两个点之间还是有挺长的路要走的。如果我们使用图优化，往往会在整个视频序列中，定义若干个关键帧：
　　这个图着实画的有点丑，请大家不要吐槽……不管怎么说，它表达出我想表达的意思。在这张图中，我们有一个路标点（五角星），并在各个关键帧中都看到了这个点。于是，我们就能用PnP或ICP求解相邻关键点的运动方向。这些在上篇文章都介绍过了，包括特征选择，匹配及计算等等。那么，这个过程中有什么问题呢？
2&&& 为什么要用全局优化
　　你一定已经注意到，理想的计算总和实际有差距的。好比说理想的科研就是“看论文——产生想法——做实验——发文章”，那么现实的科研就是“看论文——产生想法——做实验——发现该想法在二十年前就有人做过了”，这样一个过程。实际当中，仅通过帧间运动（ego-motion）来计算机器人轨迹是远远不够的。如下图所示：
　　如果你只用帧间匹配，那么每一帧的误差将对后面所有的运动轨迹都要产生影响。例如第二帧往右偏了0.1，那么后面第三、四、五帧都要往右偏0.1，还要加上它们自己的估算误差。所以结果就是：当程序跑上十几秒之后早就不知道飞到哪儿去了。这是经典的SLAM现象，在EKF实现中，也会发现，当机器人不断运动时，不确定性会不断增长。当然不是我们所希望的结果。
　　那么怎么办才好呢？想象你到了一个陌生的城市，安全地走出了火车站，并在附近游荡了一会儿。当你走的越远，看到许多未知的建筑。你就越搞不清楚自己在什么地方。如果是你，你会怎么办？
　　通常的做法是认准一个标志性建筑物，在它周围转上几圈，弄清楚附近的环境。然后再一点点儿扩大我们走过的范围。在这个过程中，我们会时常回到之前已经见过的场景，因此对它周围的景象就会很熟悉。
　　机器人的情形也差不多，除了大多数时候是人在遥控它行走。因而我们希望，机器人不要仅和它上一个帧进行比较，而是和更多先前的帧比较，找出其中的相似之处。这就是所谓的回环检测（Loop closure detection）。用下面的示意图来说明：
　　没有回环时，由于误差对后续帧产生影响，机器人路径估计很不稳定。加上一些局部回环，几个相邻帧就多了一些约束，因而误差就减少了。你可以把它看成一个由弹簧连起来的链条（质点-弹簧模型）。当机器人经过若干时间，回到最初地方时，检测出了大回环时，整个环内的结构都会变得稳定很多。我们就可以籍此知道一个房间是方的还是圆的，面前这堵墙对应着以前哪一堵墙，等等。
　　相信讲到这里，大家对回环检测都有了一个感性的认识。那么，这件事情具体是怎么建模，怎么计算，怎么编程呢？下面我们就一步步来介绍。
3&&& 图优化的数学模型
　　SLAM问题的优化模型可以有几种不同的建模方式。我们挑选其中较简单的一种进行介绍，即FrameSLAM，在2008年提出。它的特点是只用位姿约束而不用特征约束，减少了很多计算量，表达起来也比较直观。下面我们给出一种6自由度的3D SLAM建模方法。
　　符号：
　　注意到这里的建模与前文有所不同，是一个简化版的模型。因为我们假设帧间匹配时得到了相邻帧的变换矩阵，而不是把所有特征也放到优化问题里面来。所以这个模型看上去相对简单。但是它很实用，因为不用引入特征，所以结点和边的数量大大减少，要知道在图像里提特征动辄成百上千的。
4 & &g2o是什么
　　g2o，就是对上述问题的一个求解器。它原理上是一个通用的求解器，并不限定于某些SLAM问题。你可以用它来求SLAM，也可以用ICP, PnP以及其他你能想到的可以用图来表达的优化问题。它的代码很规范，就是有一个缺点：文档太少。唯一的说明文档还有点太装叉（个人感觉）了，有点摆弄作者数学水平的意思，反正那篇文档很难懂就是了。话说程序文档不应该是告诉我怎么用才对么……
　　言归正传。如果你想用g2o，请去它的github上面下载：
　　它的API在：
4.1&&& &安装
　　g2o是一个用cmake管理的C++工程，我是用Linux编译的，所以不要问我怎么在win下面用g2o，因为我也不会……不管怎么说，你下载了它的zip包或者用git拷下来之后，里面有一个README文件。告诉你它的依赖项。在ubuntu下，直接键入命令：
　　sudo apt-get install cmake libeigen3-dev libsuitesparse-dev libqt4-dev qt4-qmake libqglviewer-qt4-dev
　　我个人感觉还要 libcsparse-dev和freeglut3这两个库，反正多装了也无所谓。注意libqglviewer-qt4-dev只在ubuntu 12.04库里有，14.04 里换成另一个库了。g2o的可视化工具g2o_viewer是依赖这个库的，所以，如果你在14.04下面编，要么是去把12.04那个deb（以及它的依赖项）找出来装好，要么用ccmake，把build apps一项给去掉，这样就不编译这个工具了。否则编译过不去。
　　解开zip后，新建一个build文件夹，然后就是：
　　cmake ..
　　sudo make install
　　这样g2o就装到了你的/usr/local/lib和/usr/local/include下面。你可以到这两个地方去看它的库文件与头文件。
4.2&&& &学习g2o的使用
　　因为g2o的文档真的很装叉（不能忍），所以建议你直接看它的源代码，耐心看，应该比文档好懂些。它的example文档夹下有一些示例代码，其中有一个tutorial_slam2d文件夹下有2d slam仿真的一个程序。值得仔细阅读。
　　使用g2o来实现图优化还是比较容易的。它帮你把节点和边的类型都定义好了，基本上只需使用它内置的类型而不需自己重新定义。要构造一个图，要做以下几件事：
　　l&& 定义一个SparseOptimizer. 编写方式参见tutorial_slam2d的声明方式。你还要写明它使用的算法。通常是Gauss-Newton或LM算法。个人觉得后者更好一些。
　　l&& 定义你要用到的边、节点的类型。例如我们实现一个3D SLAM。那么就要看它的g2o/types/slam3d下面的头文件。节点头文件都以vertex_开头，而边则以edge_开头。在我们上面的模型中，可以选择vertex_se3作为节点，edge_se3作为边。这两个类型的节点和边的数据都可以直接来自于Eigen::Isometry，即上面讲到过的变换矩阵T。
　　l&& 编写一个帧间匹配程序，通过两张图像算出变换矩阵。这个用opencv, pcl都可以做。
　　l&& 把你得到的关键帧作为节点，变换矩阵作为边，加入到optimizer中。同时设定节点的估计值（如果没有惯性测量就设成零）与边的约束（变换矩阵）。此外，每条边还需设定一个信息矩阵（协方差矩阵之逆）作为不确定性的度量。例如你觉得帧间匹配精度在0.1m，那么把信息矩阵设成100的对角阵即可。
　　l&& 在程序运行过程中不断作帧间检测，维护你的图。
　　l&& 程序结束时调用optimizer.optimize( steps )进行优化。优化完毕后读取每个节点的估计值，此时就是优化后的机器人轨迹。
　　代码这种东西展开来说会变得像字典一样枯燥，所以具体的东西需要大家自己去看，自己去体会。这里有我自己写的一个程序，可以供大家参考。不过这个程序需要带着数据集才能跑，学习g2o的同学只需参考里面代码的写法即可：
　　最近我跑了几个公开数据集（）上的例子（fr1_desk, fr2_slam)(，感觉效果还不错。有些数据集还是挺难的。最后一张图是g2o_viewer，可以看到那些关键路径点与边的样子。
　　以上，如有什么问题，欢迎与我交流：gaoxiang12@mails.

参考知识库
* 以上用户言论只代表其个人观点，不代表CSDN网站的观点或立场
访问：94662次
积分：1538
积分：1538
排名：千里之外
原创：15篇
转载：267篇
评论：13条
(25)(17)(38)(14)(17)(20)(31)(22)(47)(47)(17)(1)现在位置：
作者：盛盛
作者简介：盛盛目前在西工大学习，接触机器人2年。学习使用ROS大概1年多，玩过履带式全地型机器人、参加RoboCup Rescue项目，事无巨细、负责机器人上与图像有关的一切，2013年所在团队全国冠军。
本文发表于exbot.net,如需转载请注明出处！
因为觉得教程里面没有介绍从视觉传感器入手介绍的文章我就写一下吧。（古月居前辈的/262这个是2013年的版本，但是因为是fuerte版本，有一些就不合适放在现在使用了，但是大部分都是ok的，当然本文也不会只有前辈的那一些东西，有一些扩展。至于包的安装等问题，可以看前辈的教程，下文不会太多提及安装问题。如有错误，请务必指出来，谢谢！下面主要介绍是单目的传感器，双目传感器暂未涉及。
本文将从相机的驱动开始介绍，然后标定，再是图像压缩，再是ROS与OpenCV，最后是由此扩展的两个包 pluginlib 以及nodelet。其实也没多少东西，全部是围绕视觉传感器来讲的，可能扯得有一点远，但也是正常的关联。当然，ROS提供了更多的图像处理的包，他们主要位于APIs（）
说详细点就是介绍ROS中对于普通的相机常用的两个驱动——usb与uvc以及uvc的一个未来维护版本libuvc_camera,由于libuvc在wiki上的教程是无法使用的，所以下面会介绍一个可用的教程，这个教程也可以解决多个相机因为usb多次插拔导致的问题。此外包括支持RGBD相机的驱动的使用，包括一些遇到的问题。以及对于image_transport进行图像压缩的一些使用的讲解，还有就是与视觉传感器相关的一些初级的教程，包括pluginlib、nodelet，因为exbot的文本教程中貌似也没有介绍他们。个人的经验是初学者要么跑完20篇基础之后就不知道要干什么了，要么就是有各种前辈的指导可以从项目中练手来获取深一些的认知，当然还有就是根本无从下手。
那些昂贵的相机我反正是没用过，我就介绍一下可以使用在价格亲民的相机上的一些驱动，至于在昂贵的上面是否可以使用，没试过，不知道。
下面这个部分主要比较usb_cam （http://wiki.ros.org/usb_cam）与 uvc_camera（http://wiki.ros.org/uvc_camera）因为uvc_cam在indigo版本之后已经不再维护了，转为libuvc_camera(http://wiki.ros.org/libuvc_camera)，所以也介绍一下libuvc_camera，附带一个可用的教程。
----------------------usb_cam----START---------------------------
usb_cam可以驱动标准的USB相机,像罗技的一些相机，这个大家都明白如何使用，我就不多说了，直接放出命令行以及launch吧。
launch文件
因为默认打开的都是/dev/video0,所以这边就小小修改一下。
对于命令行，由于usb_cam的video_device选择必须改为_video_device，而对于launch文件则不需要。可以看到在（http://wiki.ros.org/usb_cam）中的参数栏目明确显示了如下信息：
但是我们发现，一旦设置过这个参数，在系统没有关闭roscore的情况下，之后其他的终端默认打开的将是这个被我们修改了的参数。也就是说，在其他的终端中，默认打开的是/dev/video1而不是/dev/video0.同样的对于其他的参数，比如图片的宽、高、像素格式、输入输出模式、相机的ID、图像的锐度、对比度、明度等等也将遵循这个规律。（一般也没人修改这一些参数吧）。但是这个规律的优势貌似不是很明显，单纯方便我们操作。
至于在launch中的name设置为your_image_node_name 纯粹告知这个node的名字随便修改,launch文件的配置可以看（http://blog.exbot.net/archives/1413）以及（http://wiki.ros.org/roslaunch/）,但是launch文件这个东西是有很多坑的，光看官网的可能不够，具体的一些比较怪异的问题还得在实际中解决，比如某一些看似没什么关系的地方多加一个空格都不行。
上图的node如下图所示
一般这样使用就ok了，至于出现的WARN，比如丢失head_camera.yaml这样的配置文件；由于相机本身不支持自动对焦导致unknown control ‘focus_auto’这样的问题，都没有关系。相机本身的基本功能，提供图像，已经实现了，使用rqt工具或者在rviz中都可以查看。
----------------------usb_cam----END-------------------------------
----------------------uvc_camera----START-------------------------
对于uvc_camera,个人认为它的一些可调参数经过了选择，更常用。
不仅可以对双目相机进行操作，同时也发布了相机本身的参数信息的topic，另外还可以调整FPS，像对比度之类的就没法调整，不过一般没人调节对比度吧。
它的使用与usb_cam类似，直接放出命令行与launch文件。
launch文件
一个明显的、常用的区别在于，uvc_camera的设备参数名为device,所以使用‘_device’即可。
另一个是uvc_cam提供了双目的节点，可以做双目的驱动。
其余的参数调节，查看http://wiki.ros.org/uvc_camera完全可以解决。
但是uvc_camera相比usb_cam的一个缺点，在我使用的过程中，对于同一个usbhub上的多个相机，使用uvc_camera无法同时驱动，而使用usb_cam则没有这个问题。
----------------------uvc_camera----END-------------------------------
----------------------libuvc_camera--START----------------------------
对于libuvc_camera，这个官网的教程（http://wiki.ros.org/libuvc_camera）实在是不敢恭维，至少无法正常下跑下来。另外他要求的rules，在未添加的情况下，还是可以正常跑下来的。所以下面介绍如何使用libuvc_camera。
使用libuvc_camera的步骤如下：
获得相机的设备参数
使用cat /proc/bus/input/devices 得到插入的usb相机的vendor ID以及product ID，vendor ID是罗技厂商的ID，product ID是设备的型号。
上图中，得到C920的vendor为0x046d,product为 0x082d;C170的vendor 为 0x046d,product 为 0x082b.
使用libuvc_camera
launch文件
上面这个使用libuvc_camera驱动C920,同样由于直接使用了这个产品的厂商编号以及设备型号，因此对于usb多次插拔的情况，可以避免视频流混乱，保证设备与视频对应。
libuvc_camera相对uvc_camera 提供了更加多的图像配置选择，同时根据设备号选择设备，所以它在三种驱动中算是最强大的一个。至于具体的选择，还看具体环境。
----------------------libuvc_camera----END-------------------------------
下面这个部分介绍驱动RGBD-camera的两个驱动。
现在买到的Xtion pro live一般是2代，主要区别在于红外相机的镜头那边是否存在一个1x0.3cm左右的矩形缺口，可以使用openni2进行驱动。
由于已经提供了launch文件，所以可以直接修改来配置个人的参数，launch文件的位置[/opt/ros/indigo/share/openni2_launch/launch]
对于kinect 1代，可以使用freenect_launch进行驱动。
在下图所示位置存在其launch文件，可以copy为你所用。
如果需要在自己的launch中包含openni2或者freenect_launch的启动文件，则使用如下include句式。
对于这一些设备，对于是否支持USB3.0我已经是混乱的了，基本上来得看传感器的心情，有时候可以有时候不行。
-----ok----
上面的驱动部分我大概就讲这一些，个人理解就是对于图像获取的过程，之后我们才可以做图像的预处理等等其他的分门别类的工作。
说到图像之后就必须要提标定的问题了。如果单纯是拿来看周围的环境，用于人眼，那标定可能没有那么重要，但是如果需要做一些其他的工作，比如视觉里程记（高翔前辈的教程：/gaoxiang12/p/4719156.html，ROS中已经有的两个包：（libviso2：http://wiki.ros.org/libviso2fovis_ros:http://wiki.ros.org/fovis_ros），RGBD-SLAM等等，那么标定就是必不可少的工作了。
一般的单目标定手法如下
详见（http://wiki.ros.org/camera_calibration/）
表示将要被用于标定的黑白格需要的个数
--square 表示实际的格子的大小，单位为米
image为订阅的图像topic。
之后面对相机移动标定板，再完成标定之后在当前的目录下可以得到相应的 .yaml文件，使用camera_calibration_parsers转化为 ini文件
（http://wiki.ros.org/camera_calibration_parsers?distro=indigo）
如果得到的是ini文件，同样可以使用camera_calibration_parsers将ini文件转化为yaml文件。
在标定的过程中，如果标定板子较小的话，如上面选择的标定板的一个方块只有0.038m，也就是3.8cm，那么对标定的结果就有一定的影响。此外，也相对不容易标定，需要将标定板与相机的镜头靠的很近，那么图像的畸变也就很难直接看出来了。
对于xtion等提供了RGB图像以及Depth图像的设备，提供RGB数据的传感器可以使用上面的方法进行标定，但是对于Depth图像，由于xtion使用红外光来计算Depth，所以Depth的标定需要对红外图像进行标定，红外图像的标定，其实就是让标定板发出红外光或者反射红外光。使用红外光发射器对准标定板再进行深度图的标定即可。因为本质上他们都属于单目的标定，所以与RGB的标定是一样的。
ROS本身提供了标定程序所以就拿来直接用来了，其他的还有很多的标定工具，如matlab的工具箱等。
此外上面提到过FPS这个参数，自然少不了介绍图像的压缩。一般使用的时候直接会使用没有压缩的图像，但是在需要传输的时候，这一些大图像非常占用带宽，所以对图像进行压缩也就是必然的选择了。
ROS中已有的图像压缩的包有image_transport，它提供了一系列的类以及节点允许用户自己选择特定的传输策略，同时将传输的数据统一为sensors_msgs/Image。但是它本身并不完成数据的压缩工作，本身只完成对原始数据的传输，真正的数据压缩由继承自它的插件们来完成，关于插件的问题下面再说。这一些插件构成的集合是image_transport_plugins（http://wiki.ros.org/image_transport_plugins）, 内部含有对图片的JPEG／PNG格式的压缩以及对视频数据的Theroa编码，这是一个免费但是有损的编码方式。视频编码没用过就不多说了，但是视频同样也可以逐帧通过图片压缩进行传输，当然效率上的问题没有与直接对视频编码进行比较因而也不好推荐。
这一些插件可以通过apt-get安装
在具有image_transport的插件的基础上，我们才可以选择采用哪种插件进行图像的压缩，当然实际是image_transport来选择。
通过使用rqt_reconfigure工具可以动态调节压缩比，直观显示采用具体哪一种类型或者压缩比。
当然也可以直接使用rqt将各类插件集中在一起。
下面显示了不同的JPEG压缩率下的不同图像
上图使用了uvc_camera，内部已经使用了image_transport的类，所以在驱动相机之后，自然提供了压缩的一些选择。但是如果没有提供压缩的数据呢？我想对于某一些相机的驱动，应该是只提供原始数据的吧，因此，对于此类驱动，我们可以继续使用image_transport来进行数据的压缩。Image_transport 提供了republish节点，方便我们直接做处理：选择压缩格式，进行数据的重发。
在使用republish之前，需要将image_transport完全安装，如下所示。
否则无法使用republish.
对于wiki上的例子，（http://wiki.ros.org/image_transport），Nodes部分的代码是有问题的。
这样肯定没法使用啊。
但上面大概说明了republish的用法，对于视频或者图像输入，in_transport 表示接受的数据的传输形式，out_transport 表示重发数据的传输形式。in：=之后必须为原始图像的topic，out：=为重发的topic的基本名称。
wiki给出了目前可以使用的三种数据格式，如下所示。
如果in_transport 设置为 raw，即订阅原始数据；设置为compressed,则订阅压缩数据；若设置为theora，则订阅视频的theora编码的数据。如果out_transport 设置为raw，则重只发原始数据；若为compressed，则只重发压缩数据；设置为theora，则只重发theora编码的视频数据。
但是如果out_transport 缺省，则重发全部的插件提供的数据，即包括原始数据、压缩数据、深度压缩数据以及theora编码的视频数据。
如果相机本身没有提供深度数据，则订阅深度数据失败，即没有深度数据。
下面是一个具体的例子，in_transport设置为 raw，out_transport 设置为raw，通过比较左右两图发现，右图多的一个节点就是/republish_name
下图则是设置out_transport缺省的情况，发现将原来所有的节点全部重发。
此外，我们也可以自行使用image_transport作为基类来编写自己的插件，实现满足自己需求的要求。这方面的代码在wiki上已经给出。
（http://wiki.ros.org/image_transport/Tutorials/WritingNewTransport）
由于这个部分同时涉及到了pluginlib的使用，因而需要先看pluginlib的部分。
此外，我们在自定义使用image_transport时常打算自己设定压缩的比率，但是目前image_transport没有开放参数的接口。观察transport_hints.h 的代码，我们发现只有image_transport 一个参数可以调，即选择插件的选项。
关于图像压缩的部分就暂时到这里为止。
在ROS中使用OpenCV
上面我们注意到，image_transport涉及到在ROS中使用OpenCV，因而也就简单介绍一下。对于上面的图像压缩，使用OpenCV同样可以做到,CV的知识就借助其他的书籍来学习了，入门可以看《OpenCV3编程入门》。OpenCV放在ROS中使用，主要由vision_opencv这个stack提供帮助。它包括了cv_bridge以及image_geometry。
cv_bridge主要提供的帮助是将OpenCV的图像数据与ROS的image消息进行转换。OpenCV提供了图像的数据可以作为ROS中的image消息的data部分，而其他的部分，需要由用户自行定义。
ROS的sensor_msgs/Image消息如下所示。其中的data数据可以由OpenCV的图像数据进行填充。当然，data数据也就可以变成OpenCV的Mat数据格式。
关于cv_bridge的使用基本没有很大的难度，使用image_transport的subscriber与publisher进行ROS节点之间的图像收发，在需要使用OpenCV的时候使用cv_bridge将消息转化为Mat类型，由此进行OpenCV的处理，处理结束之后如果还需要处理之后的数据，则再利用cv_bridge将Mat转化为sensor_msgs/Image即可。所以，cv_bridge就如他的名字一样，是bridge，为OpenCV的使用搭建桥梁，桥上面运输的就是关键的图像像素数据。
关于cv_bridge的使用，wiki上的详细的教程见此处（http://wiki.ros.org/cv_bridge/Tutorials/UsingCvBridgeToConvertBetweenROSImagesAndOpenCVImages）
当然，由于sensor_msgs/Image的data需要的数据是uint8类型的，对于opencv中提供的众多数据类型，在进行数据的使用（拷贝或是引用）时，需要指明编码的格式，防止出现图片编码的混乱。具体的使用细节还是根据现场的情况进行分析及时调试即可。
说完cv_bridge，那么vision_opencv中的image_geometry是干什么的呢？
image_geometry借助sensor_msgs/CameraInfo的参数简化对图像的几何描述，包含了C++与python的库。这么说的话就感觉和没说差不多啊，所以我们看一下头文件的位置就明白了，如下图所示。
image_geometry中包含的两个头文件 pinhole_camera_model.h和stereo_camera_model.h，由此我们便从名字上得知，它的主要功能在于对单目相机以及双目相机进行建立模型。建立模型干嘛？干什么就是用户的事情了，但是模型建立之后，便可以简化对图像的几何描述，这也就是上面的意思了吧。
wiki推荐使用这个库，原文写到“Although CameraInfo contains all the information required to rectify a raw image and project points onto it, it is highly recommended that you use this library, since performing these operations correctly over the space of all camera options can be non-trivial.”。总之用image_geometry可以让你对在相机空间进行操作感到不那么繁琐。wiki提供了在图中标注TF例子（http://wiki.ros.org/image_geometry/Tutorials/ProjectTfFrameToImage）但是这个例子对于理解image_geometry感觉不是很有帮助啊，而且需要你事先了解TF的内容。所以为了了解image_geometry，还是得从源码入手。
（http://docs.ros.org/api/image_geometry/html/c++/classimage__geometry_1_1PinholeCameraModel.html）。我个人不喜欢看源码，因为不会看....但是通过查看函数的功能，大概还是猜的出来整个头文件想要干嘛。通过查看源码，我个人的理解是：image_transport就是一个比sensor_msgs/CameraInfo更加详细、更加通用，为我们在不同的分辨率下得到相机的各类物理参数（如x,y坐标的光学中心，x,y坐标的焦距等等等）的工具。
此外，各种与CV、AR相关的C／C++资料可以查看（http://blog.exbot.net/archives/957）
视觉以及图像处理、3D重构等工具以及网站（http://blog.exbot.net/archives/949）
pluginlib与nodelet
Ok，那么终于到了最后面了，因为在之前的使用中，我们发现除了从0开始编写程序之外，还有一种方法是站在前人的基础上再进行编写，有一种手法叫做编写插件。在使用uvc_camera的时候，我们从他的源码查看，也发现了这个问题，uvc_camera本身带有是两个插件的，他们均继承自nodelet。nodelet下面会讲。
其中的nodelet_uvc_camera.xml如下所示。
那么我们先从pluginlib说起。
pluginlib可以向一个现有的包中动态添加需要扩展的功能，在包的package.xml中注册插件，即可使用。一大优点是我们不需要知道源码就可以对你这个包进行扩展。最后，使用pluginlib产生的是一个库。
因为个人感觉一开始plugin是比较难明白的。但是，wiki的教程中，pluginlib（http://wiki.ros.org/pluginlib）的部分，讲的比较清楚，提供的例子的确是少了一点，尤其是如何使用一个已有的plugin的部分。wiki提供的例子
（http://wiki.ros.org/pluginlib/Tutorials/Writing%20and%20Using%20a%20Simple%20Plugin）是计算矩形以及三角形的面积，的确已经说明白了，但是使用的时候还是存在一些不是很方便理解、可能存在误解的地方。比如，因为教程中的包名(pluginlib_tutorials_)、命名空间(polygon_base)、xml文件名(polygon_plugins.xml)，与在xml文件内部&library path=”lib/libpolygon_plugins”&之间的命名上存在容易混淆的地方。
wiki提供的例子可以很好的帮助我们了解pluginlib，它的结构如下所示。
因为pluginlib提供了动态添加的功能，就像cfg提供对参数的动态修改而不需要再次编译，自然在很多场合都被使用，比如navigation。Ok，那么nodelet干嘛的呢？Nodelet的使用与pluginlib比较像，都需要继承一个基类，都需要注册插件，但是不同之处在于，pluginlib比nodelet更加底层，毕竟nodelet的基类已有现成的了。或者这么说吧，pluginlib是纯牛奶，nodelet是香蕉味的纯牛奶，我们想要在吃奥利奥的时候有香蕉牛奶的味道，对于nodelet这款，我们只要蘸一下就行了，而对于pluginlib，我们还需要在加一些香精搅拌一下，再蘸一蘸，才可以感受到香蕉牛奶的味道。当然，他们的具体区别用这个比喻来说就有问题了，但是如果说是文件的格式，那可以算是差不多的比喻。
nodelet主要想要实现的功能是，在单台机器上的单个进程中，同时跑多个算法，且在消息传入进程时，该进程内的算法都可以获得消息而不需要复制。nodelet为了满足零复制成本，允许不同的算法动态加载到相同节点，然而他们各自的命名空间不同，因此看上去像是多个节点一样，但是他们的确是在同一个进程中。
Nodelet的主要应用对象是高吞吐量的数据流，像视频数据这种使用nodelet就比较合适。所以上文的uvc_camera就继承了nodelet。把他们放在同一个进程中防止信息的拷贝以及网络阻塞。
Nodelet的例子在wiki上也已经比较详细了（http://wiki.ros.org/nodelet/Tutorials/Running%20a%20nodelet）
看一下一定可以明白。
所以对于nodelet，因为文件的组成上与pluginlib很像，同时又比pluginlib要小一些，所以把这两者一起看，可以加深对pluginlib使用的理解（毕竟nodelet就使用了pluginlib），如果感觉之前wiki上介绍使用pluginlib例子的polygon_loader.cpp不是很好看，那么看nodelet的例子就可以较为方便地理解。
Ok，暂时就讲这一些。
至于其他的，像直接选择硬件的，可以看下面这个
如何选择镜头以及摄像机，请看（）
编辑：dajianli
阅读 4,938 次
阅读 5,917 次
阅读 9,027 次
阅读 4,066 次
阅读 5,458 次