读之前提醒:首先,该文章是按照时间顺序讲述,并且不提前讲各种类关系和数据结构关系,而是在讲述系统时间执行流成的时候及时的补充相关的东西。意思就是先知道这个东西在流程中是干嘛的,然后告诉你这个东西是啥。
在slam中,rgbd,双目和单目相机的工作原理不太一样。因为单目得不到每个特征点的深度,而rgbd和双目可以得到每个特征点的深度,进而可以求解每个特征点在相机坐标系下的坐标。虽然单目可以通过三角化求得深度,但是由于极平面约束的性质,使得单目求得的位姿在尺度方面是不确定的,一般往往将其归一化处理。
这里主要讲述rgbd的流成,穿插着单目的不同。双目只是在计算深度和后端优化求解时和rgbd不太一样,其余差不多了。
在orbslam中主要有三个线程并行处理,主线程tracking负责图片的读入和vo,就是通过读入图片,提取特征点,建立初步的特征点和地图点的对应关系。从而决定是否建立新的关键帧,然后将新生成的关键帧插入localmapping线程。localmapping线程负责将局部地图里的地图点和当前帧的关键点匹配,根据匹配关系进行重投影误差的优化,进而优化当前帧的位姿。还有一个loopclose线程,这个线程主要时检测当前帧和过去所有的关键帧之间是否存在回环,也就是说是否存在当前帧恰好和之前的某个已经在地图里的关键帧看到的是同一个场景,如果是,那么就执行pose graph的优化,就是不优化地图点,只优化所有相机的位姿。
下面开始将tracking(以rgbd为例,暂时只考虑建图模式,定位模式的逻辑暂时忽略,个人觉得定位模式就是建图模式不增加地图的一个特例)
在rgbd_tum.cc中main函数进去就是一个操作,循环的读图片,然后执行函数 SLAM.TrackRGBD(imRGB,imD,tframe); 这个函数将图片信息传进去slam系统了。TrackRGBD(imRGB,imD,tframe) 这里imRGB是彩色图片,imD是视差图,tframe是时间戳。然后执行 mpTracker->GrabImageRGBD(im,depthmap,timestamp); 在这个函数内部,将rgb转换成灰度图,将视差图转换为深度图,然后构造Frame,执行track()函数。
下面开始讲说track()函数,主要以时间顺序讲述,中间会穿插各种所需要的知识。进入track(),首先看当前是否已经初始化完成,如果没有就开始进行初始化,也就是说,一般正常情况下,第一帧是开始初始化的。那么开始讲述初始化。先讲rgbd的初始化。在前面构造Frame的时候,会将图片提取金字塔,在每层金字塔上提取关键点,然后将关键点的坐标恢复到原始的图片尺寸,并且按照30*30像素的大小将图片分成grid,然后将每个关键点(也即是按照关键点的坐标),分配到每个格子中。在Frame中,记住一个比较重要,就是所有关于关键点的操作,一般都是通过关键点的索引得到的。首先用于初始化的关键点的个数应该大于500个,然后将第一帧也就是当前帧的位姿设置为单位矩阵,这里的位姿是SE3形式的。然后用这一帧构造关键帧,将关键帧添加到地图里,然后根据深度信息,将二维的图片提取的关键点的坐标根据相机模型反投影得到该点的相机三维坐标,由于第一帧位姿为单位矩阵,所以这里的相机坐标也就是世界坐标了。然后添加观测,主要是对于每个地图点来说,它可以被哪个关键帧看到,并且记录该地图点对应的关键点在该关键帧中的索引。然后计算每个地图点的描述子。这里因为每个地图点可能被很多个关键点对应,所以就有很多个描述子,这里采用一个最好的描述子,说白了就是最好的描述子距离其他所有描述子的距离和最小。然后跟新该地图点的平均观测方向和测距离范围。地图点的平均观测方向指的是,一个地图点和观测到该地图点的关键帧的相机中心的连线组成的向量,方向由相机中心指向地图点,然后将所有的这样的向量归一化之后相加,然后取平均,这就是该地图点的平均观测方向。距离范围指的是,每个地图点都有一个参考关键帧,得出该参考关键帧和该地图点的距离为一个基准,那么观测该地图点的最大距离就是那个基准距离乘以该地图点的对应的关键的所在的金字塔层对应的尺度,金字塔从0到8层,第0层尺度为1,向上依次乘以1.2。最小尺度为那个最