【Roban教程】定位和图像追踪ORB-SLAM2

这里我们采用ORB-SLAM2 包来实现此功能。ORB-SLAM2 是一个服务千单目、双目和RGB-D 相机的完整的SLAM 系统，包括地图重用、闭环和重定位的功能。该系统可在各种环境中的标准CPU 上实时工作，从小型手持室内序列到在工业环境中飞行的无人机以及在城市周围行驶的汽车。对千一个未知区域，该系统能从视觉上进行简单的定位以及测量追踪。

rqt_graph 界面

一、数据接收和程序初始化

ORB-SLAM2 系统对千RGB-D 相机的处理，采用了光束法平差方法(BA) ，从而实现了精确度的最大化以及深度误差的最小化。其中可执行程序代码主体位千ros_rgbd.cc文件中。在数据处理之前，我们需要调整Realsense D435 相机在文件rgbd. yaml 中的参数。以下是具体的参数以及获取方法（可以通过相机标定获取．yaml 文件中的对应参数）：

我们首先创建SLAM 系统实例并初始化所有系统进程：

ORB-SLAM2 的进程主要包括：

(1) TRACKING: 通过找到各个帧之间的特征以定位相机位置并实行跟踪。

(2) LOCAL MAPPING: 管理本地地图并进行优化（调用本地BA) 。

(3) LOOP CLOSING: 通过执行姿势图优化来检测大回路并校正累积的漂移。

在ORB_SLAM2::System 的构造函数中，有这三个进程的调用：

二、点云地图创建／重用

在开始之前，我们需要选择创建新地图还是重新调用地图。

1、点云地图创建

当我们到一个新的环境中时，需要获取周围环境的地图并储存为pcd 文件，此时我们执行语句最后选择false:

此时在System 类中，我们将创建新的地图变量：

当RGB-D 程序结束时，我们期望把所挑选出的关键帧打印成一个连续的点云地图。在打印生成的点云图像时，我们应在文件PointCloudMapping.cc 的generatePointCloud()函数里面做好相应参数的调整：

以及在PointCloudMapping 类的构造函数中，调整适当的树叶大小：

若出现了在相机完成闭环时的段错误，是因为在CorrectLoop(）中使用了一个叫KeyFrame AndPose 的数据类型，这个数据结构中会创建Eigen 对象， 根据Eigen 文档，调用Eigen对象的数据结构需要使用Eigen 的字节对齐，所以在LoopClosing.h 中，加入此宏定义语旬：

至此，我们获得了清晰可见的点云图pointcloud.pcd, 下图为办公室走廊及办公桌附近的点云图演示。

走廊及办公桌点云图

2、点云地图重用(reuse)

当我们已经有当前环境的地图时，我们需要调用此地图，此时我们执行语句最后选择true:

此时，在system 的构造函数中进行原有地图的加载工作：

我们获取并将带有时间戳的实时位置以geometry_msgs: : Pose WithCovarianceStamped的消息发布出去。

rqt_graph 界面

到此为止，如上图所示，我们将image_publisher 节点发布的RGB 图以及深度图进行处理，得到了pcd 文件以及实时地发布当前位置坐标。

 处理后的RGB 图

如上图所示，在RGB 图片中，紫色的点代表原来地图上已经有的关键点，绿色的点代表重新识别出来的关键点。

处理后的点云图

如上图所示，在点云图中，绿色的方框代表当前位置，红色的点云代表当前RGB 图像在点云图上的位置，黑色的点代表已经识别的点云。