在ORB_SLAM2算法的基础上，新开一个线程用于进行octomap的构建。ZED_Depth.yaml为系统参数配置文件。系统适用于ROS系统下，配合ZED摄像头使用。启动命令如下：
roscore
roslaunch zed_wrapper zed.launch
rosrun ORB_SLAM2 RGBD /home/nvidia/catkin_ws/src/ORB_SLAM2/Vocabulary/ORBvoc.txt /home/nvidia/catkin_ws/src/ORB_SLAM2/Examples/ROS/ORB_SLAM2/Zed_Depth.yaml
使用ctrl+c退出系统。使用octomap带有的octovis进行对.bt地图文件进行观察。
这个版本新添加3个线程，分别是局部地图规划线程，全局地图规划线程，通信线程。
使用串口与下位机进行通信，下位机向上传输GPS信息以及实时机器人地磁传感器信息用于实时确定机器人的朝向。并将这些信息分发到局部路径规划线程以及全局路径规划线程中去。
这个线程的作用是通过获取的GPS定位信息，知道自己的全局坐标，然后根据我们人为输入的终点全局坐标，计算出实时的全局朝向。当然这个全局朝向比较粗略，因为暂时没有接入百度地图的API，我们没得全局大地图，全局朝向是以起点直指终点计算得出的。全局朝向是我们用于指示机器人运动大方向的。以正北方向为0度，然后以顺时针增加全局朝向角度，0~360度。
根据全局朝向角度，以及我们的octomap的数据，对机器人的正前方的三维地图进行二维压缩。然后根据机器人局部地图的初始建立角度，机器人实时地磁角度，全局朝向角度，对前方的二维地图进行A*的算法局部路径规划。不同的全局朝向和机器人实时朝向用于确定不同的局部规划终点。最终输出一个前方二维地图的矩阵，以及以地图的行进坐标点序列的方式输出局部路径规划的结果。
从该版本开始，机器人代码分为2个分支。一个是基于阿栖机器人的AXi，一个是基于全向轮小车的Om。他们主要由于底盘不同，所以写的控制指令模式不同。该版本主要的更新在于修复各线程之间由于传感器不完全导致的不安全访问，修复路径规划系统中一些BUG，添加路径规划的平滑策略，添加机器人的相关控制逻辑。
由于系统需要使用地磁传感器，GPS，摄像头等所有传感器均能正常工作的时候才能正常运行。如果一旦其中一个传感器崩溃，系统理应进行相关处理，而不是仍由其跑飞。这里添加当某种传感器出错后的紧急应对措施，以及相关提醒，防止当错误情况发生时造成意外崩溃。
而路径规划是其中最重要的部分。路径规划的开始，必须由点云线程和全局路径规划线程均就位才行。也就是必须当局部3D地图第一帧收录后，以及通过GPS定位将全局路径规划出来后才开始正式的局部路径规划以及发送控制指令。
原来的路径规划代码，是最简单的A算法。这里我们将A的H计算公式，由曼哈顿距离，改为斜直距离，这样更加符合我们机器人的行动模式。
由于A*算法的结果是得到一些列的前进点，这样不利于我们对于小车的控制。这里采用添加拐角惩罚的优化算法，对小车的形式轨迹进行优化，并进行直行检测后去掉中间点。使最后得到的小车轨迹是少量且少拐角的规划路径。
由于有AXi和Om两种不同底盘的机器人，他们的行动模式各有不同。现在根据其具体行动模式，对其各个添加相应的机器人底盘控制代码，通过通信进程发送给机器人，使其进行运动。
该版本是基于Om小车也就是全向轮小车的框架进行的开发。主要修复了系统的路径规划以往需在第2个关键帧后才能进行的BUG，以及注释添加了无显示器模式，添加了跟踪丢失过后的应对措施。具体如下：
原来pointcloud线程是通过创建新的关键帧时才获得帧，而没有获得初始化的第一帧。而局部路径规划是在pointcloud线程启动后才能运行的。现在在SLAM系统初始化的过程中，会把第一帧传递给pointcloud线程，达到一开机小车就能进行运动规划。
这个需要在ros-rgbd文件里面，在初始化SLAM系统时，将viewer的输入改为false即可。
在小车的运动过程中，会出现ORBSLAM跟踪失败的情况。现在当这种情况发生后，我们会令小车原地停止下来，也即不再发送路径规划指令。同时如果当30s后，小车都未重定位成功，表示此时小车已经彻底跟踪失败。此时我们清除掉原始SLAM系统中所有的数据，包括关键帧，以及OCTOMAP。并对系统进行重置，使小车以目前为坐标（0,0）重新开始路径规划。
该版本是基于Om全向轮小车开发的一个过渡版本。它在2.1的基础上对于建图精度进行了优化。同时会作为AXi机器人的初始代码。
该版本对于三维重建里面的滤波算法进行了改进。我们根据Zed相机110°广角，16:9成像原理，是滤波结果只接受0.5-5m深度范围内，左右视角90°，上下视角45°范围内的占据信息，防止深度畸变导致的建模错误。
该版本是基于Om全向轮小车的基础上开发出的AXi机器人2.3版本。从该版本之后两个机器人的SLAM系统，路径规划等代码全部一致。区别只在于，局部路径导航的主函数中，对于机器人的指令函数名的改变。需要注意的是，该版本由于未拿到AXi机器人的具体数据，在控制方面还不够完善。同时这也是在进行实际运行之前的版本。
从这一版本开始，我们的机器人完全脱离ROS系统。也即现在是ZED-SLAM的结构，而不是ZED-ROS-SLAM的结构。我们使用ZED提供的SDK，通过直接操作ZED摄像头进行深度图像的测量，将其结果放入SLAM系统，进行后续工作。这样能够节省ROS中的大量额外开销，例如ZED_ROS_WARPPER里面会进行位姿估计，点云生成等不需要的操作。使用ZED与SLAM直连的形式，将GPU的占用和CPU的占用都降低了许多，为以后我们基于GPU优化，提供了便利。同时我们进行系统运行时，也不需要开启3个系统，只需要启动一条指令即可。
/home/nvidia/ORB_SLAM2/Examples/RGB-D/rgbd_my /home/nvidia/ORB_SLAM2/Vocabulary/ORBvoc.txt /home/nvidia/ORB_SLAM2/Examples/RGB-D/Zed_Depth.yaml
在控制台下，按'q'键退出系统。
AXi新版有很多新的特性，例如转角有一定局限，转弯与直行模型之间需要等待等。同时由于路径规划算法的结果现在已经变为只有几个点了。所以几个控制指令全部进行更新。
该版本是实地运动测试前的最后一个版本，该版本对于Om机器人和AXi机器人均有较大改变，例如根据Om机器人的硬件特性改变其通信模块参数，以及AXi机器人的具体操作方法等。
现在ZED-SLAM系统我们使用的rgb图像和深度图分辨率均为672*376（从720P下调），15fps。这使SLAM系统的运行效率大大提升，同时降低了处理器的压力，使处理的帧率更加稳定。相对应的，由于分辨率的下降，我们将三维建图时的采样机制进行改变，现在更加符合采集范围的原理(详细参考“开发要点”)。
1.通信协议内容的变更，其中包括GPS信息，下位机向上位机传输的角度信息，波特率；
2.现在小车在执行完一条上位机的指令后会反馈一条last_op_down的符号，这样可以让上位机继续为其发送下一条指令，以保证每条指令能够正常完成。除此之外，每两条发送指令之间至少有0.5秒的间隔，这样一来防止多指令颤抖问题，二来防止初始化时指令序列还未生成就开始发送指令造成core dump；
3.小车在收到指令后会对上位机进行接收反馈，我们对接收反馈进行计数，并与我们的发送计数进行比较，测试小车是否有指令没有收到。
我们将GPS的有效性也加入局部路径规划的使能信号中。当GPS lost时，我们也不进行局部路径规划，防止小车乱动。
AXi机器人的通信协议和Om机器人的通信协议，现在在下位机对上位机的部分完全一致，这样上位机的接收处理函数可以一式两份了。而两个机器人的区别只在于，上位机对下位机的几条控制指令的具体实施方式不同，指令不同而已。就连指令的结构两个机器人都是一致的，达到代码迁移的最易性。
屏蔽了将前方地图在控制台打印的代码。 改变/了在控制台打印对下位机的操作的字符，现在更易让人看懂了。
修复了摄像机采集程序my_rgbd.cc文件里面图像采集程序的一个小BUG，使其SL和CV矩阵传值效率更高。
修复globalnavigator安全性BUG，我们对globalway生成和使用的地方分别加锁，防止多线程冲突。
修复蓝牙缓存未命中导致串口接收不了数据的BUG，现在的串口使用了新的设置项，包括读取等待等。
修复了程序停止，而小车不能停止的BUG。
该版本是室外实际测试的第一个版本，该版本还是采用RGB-D模式。我们发现RGB-D的深度测量有效值与分辨率有很大的关系，分辨率越高，能够测出的深度越大。
以往是从地面开始向上遍历5个点，现在改为从摄像头平时高度开始向上遍历5个点。这样防止由于视差问题将前方地面当做障碍。
现在当跟踪丢失后，6-15s，小车会在原地停止，等待快速大面积干扰过去。15-30s，小车会每3秒向后退0.6米，看是否能够重新定位；当30s后还没有重新定位，那么直接重启。
由于RGB-D模式下，在室外开阔环境中很容易出现匹配丢失的情况，原因是由于RGB-D在低分辨率下深度测量范围有限。从现版本开始，系统使用Stereo模式进行特征点跟踪，然后使用ZED提供的深度图进行局部地图建模。现在的启动指令如下：
/home/nvidia/ORB_SLAM2/Examples/Stereo/stereo_my /home/nvidia/ORB_SLAM2/Vocabulary/ORBvoc.bin /home/nvidia/ORB_SLAM2/Examples/Stereo/Zed_Stereo.yaml
具体模式设定为672x376分辨率下，1000个特征点提取。
我们现在将ORB词典从txt读入格式，改为从bin格式读入。这样系统启动的速度提高了几倍。
现在系统启动后，只会留有current frame窗口以及控制台，便于我们进行监控。而原来的map viewer窗口由于对于调试用于不大，被我们屏蔽掉了。
修复GPS信息与地图坐标转换的BUG。
该版本是在上个版本基础上，改进了发送指令效率过低的BUG，以及大大改变了局部路径规划的触发条件，现在的状态检测更加的科学以及高效。
局部路径规划的触发条件现在分为两步。第一步，检测GPS和IMU是否有效（GPS的有效性由全局路径规划线程每轮进行更新，IMU的有效性由UART接收IMU发送过来的信息更新，由于IMU发送周期为1s，局部路径规划的消耗周期为3-6s，所以正常情况下能一直检测有效），若2个传感器无效直接sleep等下一轮。第二步，直接检测tracking线程提供的mstate，这个标志位记录了SLAM是何种状态（一共有5种可能，但当正常运行过程中只会出现lost和ok），用这个标志位来检测我们是否跟踪丢失，更加的高效以及科学。
原来串口的配置是Vtime和Vmin都大于0，这会导致我们会阻塞到接收到指令后串口才会继续下去。这样势必会导致我们的串口很久才发送一次指令。现在我们设定Vtime大于0，Vmin等于0，这样保证每次串口的最大阻塞等待时间，我们就可以根据最大阻塞时间来控制发送逻辑。
因为localnavigator线程在系统重启后，需要重新获取originlocalway才能保证地图建立时的方向。
现在是改为last_op_down = true || wait times>4s。这种保证当上条指令如果一直未反馈后，我们也能因为指令时效超时重新发送下一条指令，减少指令卡死的状况。
修复发送指令显示的BUG。
修复char型转换不完整的BUG。
这个版本是经过了大量尝试过后对小车进行的调整。小车在室外ORB的提取率还是太低，主要是因为分辨率太低，同时室外纹理特征太少。该版本对机器人的控制策略进行了调整，同时解决了一个关键性BUG也就是地图三轴正方向的问题。同时添加了无SLAM运动模式和GPS滤波模块，试图解决室外GPS漂移以及ORB跟踪丢失时的问题。
现在机器人在很小角度的时候（10以内）是不会转向，直接当成直线行驶。而大于10到30之间的时候才会进行前进规划，大于30度过后会主动转向后再进行前进规划。这是方便摄像头的视角问题。
因为建立的三维点的y轴是上为负下为正。以往所有对三维地图的运用是错误的。现在是以y的负轴代表向上的方向。
以往是复杂的设定END点。现在一律以最后一排的中心为局部路径的END点。要是那一点被占据，此时会以最后一排为起点逐步向前扫描一个空闲的终点。
GPS由于定位不准，导致每次定位漂移太严重。现在对每10s内能够收到的所有GPS数据进行平均滤波。这样能够间接提高GPS的准确率。
由于室外纹理特征过少，现在添加一个无SLAM只靠当前帧的深度情况进行前方路径规划的模式。该模式和SLAM模式并行，当SLAM丢失过后会切换到该模式，在一定时间后会重启SLAM模式。
修复AXi和Om所有角度判断超过180度时无法判断的BUG。
修复GPS数据精度不够的问题，现在全为double型。
修复AXi机器人无法直接后退的BUG。
修复Om的距离计算BUG，原来没有定义一步有多长。
修复机器人360度过0度时无法正常识别差角的BUG。
这个版本是没有与上位机联合通信的最后一个版本。以这个版本的代码为基础用于撰写了SCI论文，参加了华为杯竞赛，撰写了相关专利。
现在GPS滤波算法改为在收到10个数据后，去掉经纬度的最大值和最小值后进行均值滤波。同时我们更换的新款的陶瓷天线，接收信号能力更强了。
现在SLAM模式也像深度模式一样，在视线方向上，被遮挡住的地方，默认为被占据了。
修复避障不灵敏的BUG，现在膨胀系数调整为6。
修复摄像头初始几帧过曝光问题，现在回丢弃前几帧图像，不用于SLAM。