技术领域
[0001] 本
发明涉及同步定位与建图SLAM技术领域，具体涉及一种基于低性能处理器设备的半直接视觉定位方法。
背景技术
[0002] 视觉SLAM
算法根据从图像中提取信息方式的不同可分为两种：直接方法和间接方法。
[0003] 间接方法也称为基于特征的方法，从图像中提取特征与特征的描述子，通过特征的描述子在连续
帧中进行匹配，进而计算得到相机移动的
位姿(位姿包含旋转和平移，术语)。特征是具有高强度
对比度的
像素，出现在具有纹理的图像区域。通常情况下，特征点的三维
位置和相机位姿是通过最小化三维点的重投影误差(就是把第一张图片上特征点投影到第二张图像的相应位置，由于投影不一定是准确的，通过最小化误差求得它们之间的关系)获得。基于特征方法在特征的提取与描述子的计算方面比较耗时，获得的地图是稀疏的地图(因为是基于特征产生地图点的，所有地图点构成地图)。同时基于特征的方法通过特征匹配计算相机位姿，占据了大部分的
跟踪处理时间(而这花费时间太多通常会影响系统运行的实时性，SLAM中一般是有两部分组成，前端和后端，前端主要根据特征点法或直接法进行图像帧与图像帧之间的跟踪求解位姿，后端负责对前端的位姿进行优化，因为存在误差，系统实时性主要看前端处理的速度)。因为每个特征都必须在较大的图像区域中搜索，以防突然运动或特征与前一帧有很大的变化。特征点法生成的是稀疏的地图，对系统运算要求能
力并不高，可以在低性能处理器中运行。
[0004] 直接方法基于整幅图像的
亮度值，使用光度误差(两幅图像，对应位置是有像素值，通过对最小化这两个像素值的差值，可以得到它们之间对应关系)最小化来计算相机的位姿。相比特征法，直接法在图像匹配求解位姿中速度更加快，同时可以生成更稠密的地图，而稠密地图有助于提升定位的
精度。但是直接法计算效率较低，需要更强的处理器或者使用GPU
加速，在计算能力较低的设备(如手机或无人机)中表现不佳。
[0005]
现有技术中，也有一些提出结合使用特征点法和直接法的算法，其中代表性的为半直接视觉
里程计(SVO)算法，相比特征点法，该算法能够在速度较慢的处理器中以两倍的速度实时运行，但主要缺点是，它设计相机模式是向下的，主要为无人机设置的，而不适用于其它的场景。
发明内容
[0006] 本发明的目的在于克服现有技术的缺点与不足，提供一种基于低性能处理器设备的半直接视觉定位方法，该方法利用特征点法占用计算资源较低的优点和直接法用于图像匹配求解位姿速度更快和可生成稠密地图的特点，专为无人机、手机等低性能处理器设备设计，同时具有鲁棒性和准确性。
[0007] 本发明的目的通过下述技术方案实现：
[0008] 一种基于低性能处理器设备的半直接视觉定位方法，包括下述步骤：
[0009] S1，3D点参数化；
[0010] 使用逆深度的方法表示三维空间点；每个三维点在第一次检测到时，由图像帧的旋转RF和平移TF表示，在相机上坐标为[vx，vy，vz]，逆深度表示为
[0011] 点深度是在下面范围之内：
[0012]
[0013] 其中σρ为逆深度的方差，最开始时ρ和σρ是设置为1的，则深度是在 之间；
[0014] 该参数化可转化为欧几里德坐标，转化关系为：
[0015]
[0016] 在每个3D创建时使用逆深度表示，则可以防止点检测和使用之间的延迟，这在相机突然移动时是必不可少的；此外，距离摄像机位置较远的点可能具有较大的深度不确定性，但可以用来提高定位精度；
[0017] 每一帧都会更新每一个点的深度，直到它收敛为止，即直到它的深度不确定性足够小为止；为了确定一个点何时收敛到其三维位置，则使用线性指数L的判别方法：
[0018]
[0019] 其中d是3D点与相机坐标之间的距离，α是第一次和最后一次观察到的每个3D点之间的
视差；当L的值低于0.1时候，则一个点的位置是收敛的，即得到较为准确的深度值；
[0020] S2，跟踪线程；
[0021] 根据相机图像和地图中存储的3D点，计算相机自上次
迭代以来的运动；此外，相机跟踪必须是有效的实时工作；
[0022] 一旦从相机中获得一个新的帧，就对图像进行多次子
采样，得到一个图像金字塔；然后利用快速算法从每个金字塔级提取特征；
[0023] 这些特征将用于匹配当前帧中的3D点，并创建新的3D点；
[0024] S3，建图线程；
[0025] 建图线程管理相机周围环境的地图；地图会随着每一帧的更新而更新，跟踪线程使用它来计算相机的位姿；设定关键帧的数量，以便始终在当前视图中保留大量的地图点；当(1a)和(1b)条件满足或(2)条件为真时插入关键帧：
[0026] (1a)距离上一个关键帧插入已经超过30帧；
[0027] (1b)与上一帧关键帧相比，上一帧中出现的3D点的数量减少了10％以上；
[0028] (2)与上一帧关键帧相比，上一帧中出现的3D点的数量减少了30％以上；
[0029] 第一个条件使得在相机缓慢移动时插入几个关键帧，而第二个条件确保在极端的相机运动中插入关键帧；关键帧的数量是有限的，因此地图不会无限增加；当达到此限制时，远离当前相机位置的关键帧将被删除；当在地图中插入一个新的关键帧时，算法会初始化新的3D点，这些3D点作为候选点保存在列表中，直到它们收敛。
[0030] 优选地，所述S2中相机的位姿是通过下述步骤进行计算：
[0031] S2.1，运动模型；使用恒速模型，当前帧的位姿初始化为上一帧的位姿乘以运动速度；
[0032] S2.2，图像对齐；使用直接法，将当前图像帧与上一帧图像对齐，获得相机位姿xt；当这些帧看到相同的3D点p时，考虑到p的估计深度，将光度残差F定义为投影点p像素之间的光度差；最小化每个3D点的光度残差F，则相机位姿xt可表示为：
[0033]
[0034] 这一步得到当前相机的粗略估计位姿；
[0035] S2.3，特征匹配；在进行特征匹配之前，将
图像分割成固定大小的单元格；前一帧中可见的所有3D点都投影到当前帧中，并根据它们在图像中的位置排列在这些单元中；每个单元格最多匹配一个点，如果没有找到每个单元格中的第一个点，算法将尝试匹配另一个点，直到完成一个匹配，或者该单元格中没有剩余的点；此外，每次迭代匹配的点的数量是有限的，因此计算时间仍然很短；
[0036] 匹配点时考虑了深度不确定性，因此，在深度为95％置信区间对应的极线上进行点搜索，边缘误差较小；在这个搜索区域内，并不是所有的像素都被使用，只有快速算法选择的像素被使用；
[0037] S2.4，离群点抑制机制；在计算当前相机位姿前，在特征跟踪过程中使用离群点抑制机制来排除异常点；不使用当前帧中找到的所有匹配点，只选择3D点的子集来计算位姿，使用RANSAC算法迭代得到该子集。
[0038] 优选地，所述S2.3中分别使用两种方法进行特征的匹配：
[0039] ①
块比较；在第一次观察到一个3D点时，在像素周围得到一个小的块(8个像素)，并对每个像素进行匹配；根据相机位姿对特征进行调整后使用零均值SSD(SSD即比较像素的值)进行评分；
[0040] ②ORB描述符；只提取匹配所需的像素的ORB描述符，即提取块比较所需的相同像素；匹配是计算两个ORB描述符之间的距离；
[0041] 在当前帧内正确匹配N个三维点后，根据公式计算相机位姿xt，使再投影误差最小：
[0042]
[0043] 其中ρi是像素点位置，Xi是3D点位置，fp(xt，Xi)是Xi在当前相机的投影。
[0044] 优选地，所述S2.4中使用RANSAC算法具体为：
[0045] S2.4.1，随机选取m个匹配的三维点，仅用这些点计算相机
姿态，使重投影误差最小；默认情况下，m被设置为5；
[0046] S2.4.2，更新相机位置后，将匹配的其余三维点投影到当前帧中，将重投影误差较低的点保存为内点，其余点保存为离群点；
[0047] S2.4.3，如果当前假设中的离群点的数量大于之前的迭代，那么内点和离群点将被保存为目前为止最好的假设；
[0048] 最后，使用RANSAC算法迭代直到执行的迭代次数大于假设数nh，这确保至少一个没有异常值的假设以概率p(默认为99％)被选择；根据下述等式在每次迭代时更新此值：
[0049]
[0050] 其中ε为最后一次迭代的离群值比，m为随机选取的匹配点个数；当RANSAC停止迭代时，最佳假设的内点用于计算位姿，排除异常值；这些异常点中可能存在一些内点，由于距离相机较近的3D点可能被认为是异常点，但实际可能是内点；因此使用位姿重复S2.4.2以尝试更新一些错误标记的异常点；如果发生这种情况，则考虑到这些新的内点，再次重新计算位姿。
[0051] 优选地，所述S3具体步骤为：
[0052] S3.1，地图初始化；使用两帧进行地图的初始化，两帧的位移需足够大；它使用快速
角点检测器从第一帧中选择大量像素(至少50个)；在随后的帧中使用Lucas-Kanade光流跟踪这些像素，直到跟踪点之间的平均偏移超过某个
阈值；如果在这个过程中丢失了很多像素，那么在选取两帧后得到的下一帧，并假设它们观察到一个局部平面场景，重新启动初始化；利用RANSAC对像素位移估计单应性矩阵H，使两帧中对应的像素坐标之间的重投影误差最小；通过对H的分解，得到了摄像机在两帧之间的旋转和平移，并通过三角化计算出三维点；
[0053] S3.2，新的3D点初始化；随着相机在环境中移动，地图不断更新和扩展，在插入关键帧后初始化新的3D点；将图像分割为单元格，并将已存储在地图中的点投射到图像中，以丢弃非空单元格；然后为每个空单元格创建一个新的3D点；每个初始化的3D点都属于每个单元中Shi-Tomasi得分最高的FAST特征；
[0054] 三维点的初始深度可以用默认值来确定，例如平均深度，如果这个值是不准确的，在接下来的帧中匹配可能会花费更长的时间，因为不确定性会很高；为了避免这种情况，在之前的关键帧中搜索新的点，通过三角化来设置初始点深度；点深度和不确定性随着每一帧的更新而更新，直到它们在当前帧匹配收敛；如果没有在多个连续帧中找到它们，将永久删除它们；此外，当一个点的不确定性足够小时，它的深度是固定的；
[0055] S3.3，地图优化；使用Bundle Adjustment(BA)算法优化关键帧和三维点位姿；系统在地图初始化和插入新的关键帧之后使用BA优化；为了减少建图时间，BA只在局部执行，只在当前关键帧及其共视关键帧之间执行；此外，BA中只使用收敛的三维点，由于该算法不能处理深度不确定性，过早地将点转换为绝对坐标可能导致优化不准确。
[0056] 本发明与现有技术相比具有以下的有益效果：
[0057] (1)本发明结合使用直接法和特征点法用于求取位姿的策略，首先使用直接法对齐图像求得粗略的位姿，用求得位姿对齐图像的特征，加快特征匹配的速度，然后基于特征点法的重投影误差优化位姿，通过直接法和特征点法的结合加快SLAM系统在跟踪部分的速度，从而有效提高系统的实时性；
[0058] (2)本发明增加离群点抑制机制(ORM)可以使
SLAM算法有效应对动态环境，ORM通过在特征匹配时有效过滤离群点，提高特征匹配的精度，从而有效提高SLAM系统应对动态环境的鲁棒性。
附图说明
[0059] 图1为本发明跟踪和建图线程流程示意图；
[0060] 图2为本发明连续图像帧的旋转平移示意图。
具体实施方式
[0061] 下面结合
实施例及附图对本发明作进一步详细的描述，但本发明的实施方式不限于此。
[0062] 如图1所示，为本发明的方法流程示意图，该方法分为两个独立的线程：一个线程估计摄像机的运动，另一个线程创建和更新环境的三维地图。它使用一种混合(直接和间接)方法来估计相机的运动。本质上，它是利用直接方法进行图像的对齐，得到初始的相机位姿，然后通过特征匹配来优化相机位姿，与传统的基于特征的方法相比，加快了特征匹配的速度。创建的地图点(3D点)使用逆深度参数化，这些3D点与观察到的关键帧(选择部分的图像帧作为关键帧，因为不可能用到所有的图像帧)关联。此外，如果两个关键帧之间同时观测到一定的3D点(至少20个)则视为共视关键帧，将共视关键帧连接起来，生成关键帧图(把关键帧图的得到的位姿保存起来，就可以得到相机运动的轨迹，同时位姿信息就是物体在空间中定位)。
[0063] 1、3D点参数化。
[0064] 大多数SLAM方法直接将三维点存储为欧几里得坐标(X,Y,Z)，这种参数化简化了点的表示，但在使用单目相机时却有缺点：
[0065] ①新的三维点深度是未知的，直到从两个或多个位置观察到它们。SLAM方法使用欧几里德坐标三角测量两个或多个关键帧的检测点来计算它们的深度。由于匹配错误，此过程会生成异常值，计算的深度是不可信的。
[0066] ②由于相机轨迹与点之间的视差较低，无法正确处理远处的点。
[0067] 使用逆深度的方法表示三维空间点。每个三维点在第一次检测到时，由图像帧的旋转RF和平移TF表示，在相机上坐标为[vx，vy，vz]，逆深度表示为
[0068] 点深度是在下面范围之内的：
[0069]
[0070] 其中σρ为逆深度的方差，最开始时候ρ和σρ是设置为1的，则深度是在 之间的。
[0071] 这个参数化可以转化为欧几里德坐标，坐标转换如图2所示，转化关系为：
[0072]
[0073] 我们在每个3D创建时候使用逆深度表示，它可以防止点检测和使用之间的延迟，这在相机突然移动时是必不可少的。此外，距离摄像机位置较远的点可能具有较大的深度不确定性，但可以用来提高定位精度。
[0074] 每一帧都会更新每一个点的深度，直到它收敛为止，也就是说，直到它的深度不确定性足够小为止。为了确定一个点何时收敛到其三维位置，本系统提出了使用线性指数L的判别方法：
[0075]
[0076] 其中d是3D点与相机坐标之间的距离，α是第一次和最后一次观察到的每个3D点之间的视差。当L的值低于0.1时候，则一个点的位置是收敛的，即得到较为准确的深度值。
[0077] 2、跟踪线程。
[0078] 跟踪线程根据相机图像和地图中存储的3D点，计算相机自上次迭代以来的运动。此外，相机跟踪必须是有效的实时工作。
[0079] 一旦从相机中获得一个新的帧，就对图像进行多次子采样，得到一个图像金字塔。然后利用快速算法从每个金字塔级提取特征。(所谓金字塔，就是把原图进行缩小，底层是原图，上层是最小的)
[0080] 这些特征将用于匹配当前帧中的3D点，并(在某些情况下)创建新的3D点。相机的位姿是通过下面三步进行计算的。
[0081] 2.1、运动模型。
[0082] 使用恒速模型，当前帧的位姿初始化为上一帧的位姿乘以运动速度。
[0083] 2.2、图像对齐。
[0084] 使用直接法，将当前图像帧与上一帧图像对齐，获得相机位姿xt。当这些帧看到相同的3D点p时，考虑到p的估计深度，将光度残差δF定义为投影点p像素之间的光度差。
[0085] 最小化每个3D点的光度残差δF，则相机位姿xt可表示为：
[0086]
[0087] 这一步得到当前相机的粗略估计位姿。
[0088] 2.3、特征匹配。
[0089] 在进行特征匹配之前，将图像分割成固定大小的单元格。前一帧中可见的所有3D点都投影到当前帧中，并根据它们在图像中的位置排列在这些单元中。每个单元格最多匹配一个点，如果没有找到每个单元格中的第一个点，算法将尝试匹配另一个点，直到完成一个匹配，或者该单元格中没有剩余的点。此外，每次迭代匹配的点的数量是有限的，因此计算时间仍然很短。
[0090] 匹配点时考虑了深度不确定性。因此，在深度为95％置信区间对应的极线上进行点搜索，边缘误差较小。在这个搜索区域内，并不是所有的像素都被使用，只有快速算法选择的像素被使用。分别使用两种方法进行特征的匹配：
[0091] ①块比较。在第一次观察到一个3D点时，在像素周围得到一个小的块(8个像素)，并对每个像素进行匹配。根据相机位姿对特征进行调整后使用零均值SSD(SSD即比较像素的值)进行评分。
[0092] ②ORB描述符：只提取匹配所需的像素的ORB描述符，也就是说，提取块比较所需的相同像素。匹配是计算两个ORB描述符之间的距离。
[0093] 在当前帧内正确匹配N个三维点后，根据公式计算相机位姿xt，使再投影误差最小：
[0094]
[0095] 其中ρi是像素点位置，Xi是3D点位置，fp(xt，Xi)是Xi在当前相机的投影。
[0096] 2.4、离群点抑制机制。
[0097] 由于特征的错误匹配，可能产生一些错误的3D地图点(离群点)。而每帧图像的特征主要是跟大部分地图点进行匹配的，由于错误的地图点，进而计算不准确的位姿，这会很大影响定位的精度。
[0098] 大多数SLAM方法只能处理较少的异常值。相比用于相机跟踪的点总数相比，异常值的数量很小。但是，如果有许多异常值，则可能会降低精度，甚至丢失定位。
[0099] 为此我们设计一种离群点抑制机制，在计算当前相机位姿前，在特征跟踪过程中使用离群点抑制机制来排除异常点。提出方法是，不使用当前帧中找到的所有匹配点，只选择3D点的子集来计算位姿，使用RANSAC算法迭代得到该子集，该算法主要分成三步：
[0100] (1)随机选取m个匹配的三维点，仅用这些点计算相机姿态，使重投影误差最小。默认情况下，m被设置为5；
[0101] (2)更新相机位置后，将匹配的其余三维点投影到当前帧中，将重投影误差较低的点保存为内点，其余点保存为离群点；
[0102] (3)如果当前假设中的离群点的数量大于之前的迭代，那么内点和离群点将被保存为目前为止最好的假设。
[0103] 最后，使用RANSAC算法迭代直到执行的迭代次数大于假设数nh，这确保至少一个没有异常值的假设以概率p(默认为99％)被选择。根据以下等式在每次迭代时更新此值：
[0104]
[0105] 其中ε为最后一次迭代的离群值比，m为随机选取的匹配点个数。当RANSAC停止迭代时，最佳假设的内点用于计算位姿，排除异常值。这些异常点中可能存在一些内点，由于距离相机较近的3D点可能被认为是异常点，但实际可能是内点。因此使用位姿重复步骤(2)以尝试更新一些错误标记的异常点；如果发生这种情况，则考虑到这些新的内点，再次重新计算位姿。
[0106] 3、建图线程。
[0107] 建图线程管理相机周围环境的地图。地图会随着每一帧的更新而更新，跟踪线程使用它来计算相机的位姿。如果要提高算法的效率，在地图中存储所有帧和三维点是不可行的。大部分的处理方法是只存储表示场景的特定帧(关键帧)，而不存储冗余信息。许多SLAM方法只插入几个关键帧，以保持较低的计算时间，但由于相机的快速运动可能会导致跟踪失败。我们设定关键帧的数量，以便始终在当前视图中保留大量的地图点。当(1a)和(1b)条件满足或(2)条件为真时插入关键帧：
[0108] (1a)距离上一个关键帧插入已经超过30帧；
[0109] (1b)与上一帧关键帧相比，上一帧中出现的3D点的数量减少了10％以上；
[0110] (2)与上一帧关键帧相比，上一帧中出现的3D点的数量减少了30％以上。
[0111] 第一个条件使得在相机缓慢移动时插入几个关键帧，而第二个条件确保在极端的相机运动中插入关键帧。关键帧的数量是有限的，因此地图不会无限增加；当达到此限制时，远离当前相机位置的关键帧将被删除。所有这些阈值都是通过实验和误差经验选择的，当在地图中插入一个新的关键帧时，算法会初始化新的3D点。这些3D点作为候选点保存在列表中，直到它们收敛。
[0112] 3.1、地图初始化。
[0113] 使用两帧进行地图的初始化，两帧的位移需足够大。它使用快速角点检测器从第一帧中选择大量像素(至少50个)。在随后的帧中使用Lucas-Kanade光流跟踪这些像素，直到跟踪点之间的平均偏移超过某个阈值。如果在这个过程中丢失了很多像素，那么在选取两帧后得到的下一帧，并假设它们观察到一个局部平面场景，重新启动初始化。利用RANSAC对像素位移估计单应性矩阵H，使两帧中对应的像素坐标之间的重投影误差最小。通过对H的分解，得到了摄像机在两帧之间的旋转和平移，并通过三角化计算出三维点。
[0114] 3.2、新的3D点初始化。
[0115] 随着相机在环境中移动，地图不断更新和扩展，在插入关键帧后初始化新的3D点。将图像分割为单元格，并将已存储在地图中的点投射到图像中，以丢弃非空单元格。然后为每个空单元格创建一个新的3D点。每个初始化的3D点都属于每个单元中Shi-Tomasi得分最高的FAST特征。
[0116] 三维点的初始深度可以用默认值来确定，比如平均深度，如果这个值是不准确的，在接下来的帧中匹配可能会花费更长的时间，因为不确定性会很高。为了避免这种情况，在之前的关键帧中搜索新的点，通过三角化来设置初始点深度。点深度和不确定性随着每一帧的更新而更新，直到它们在当前帧匹配收敛。如果没有在多个连续帧中找到它们，将永久删除它们。此外，当一个点的不确定性足够小时，它的深度是固定的。
[0117] 3.3、地图优化。
[0118] 使用Bundle Adjustment(BA)算法优化关键帧和三维点位姿。系统在地图初始化和插入新的关键帧之后使用BA优化。为了减少建图时间，BA只在局部执行，只在当前关键帧及其共视关键帧之间执行。此外，BA中只使用收敛的三维点，由于该算法不能处理深度不确定性，过早地将点转换为绝对坐标可能导致优化不准确。
[0119] 本发明使用直接法和特征点法结合相比特征点法在求取位姿和跟踪部分速度更加快，有效提高系统的实时性。
[0120] 下表1显示了本系统(SL)和基于特征点法代表的ORB-SLAM2(ORB-SLAM2是特征点法中性能比较突出的算法)在跟踪线程的执行时间(以毫秒为单位)，运行在EuRoC数据集的Machine Hall 01序列。其中SL-Patch、SL-ORB分别指在特征匹配中使用
块匹配模式或者ORB匹配模式。
[0121] 表1本系统和ORB-SLAM2在序列Machine Hall 01上的跟踪部分执行时间(单位毫秒)
[0122]
[0123] 通过表格可看出本方法大部分跟踪时间用于初始化帧和匹配特征，使用ORB描述符进行匹配要慢一些，因为它的提取速度比使用块要慢3倍。看出使用直接法和特征点法结合的本方法在求取位姿和跟踪部分速度，大约是特征点法的2-3倍，提出的方法可以有效提高系统的实时性。
[0124] 上述为本发明较佳的实施方式，但本发明的实施方式并不受上述内容的限制，其他的任何未背离本发明的精神实质与原理下所作的改变、修饰、替代、组合、简化，均应为等效的置换方式，都包含在本发明的保护范围之内。
