一种基于fpga的orb_slam重定位特征点检索加速方法
阅读说明：本技术 一种基于fpga的orb_slam重定位特征点检索加速方法 (ORB-SLAM relocation feature point retrieval acceleration method based on FPGA ) 是由 张磊 汪成亮 张寻 任骜 陈咸彰 刘铎 于 2021-08-11 设计创作，主要内容包括：本发明提出了一种基于FPGA的ORB-SLAM重定位特征点检索加速方法,包括：S1,将输入的图片缓冲,提取描述子；S2,然后进入工作空间Workspace,通过计算电路对结点的距离求解；S3,每个计算电路的结果会一起流向一个并行比较电路,求出最小值所在的点；S4,最后再判断是否是底层,若是则搜索结束,得出最终结点；S5,每个结点会有一个偏移值,用于寻找求子结点的地址,从而获得关键帧,然后根据关键帧集合进行重新定位。本发明对于电路资源的消耗情况,在计数器前加上近似单元AU形成累计并行计数器APC的电路结构,基于近似计算的原理,在bit流比较长并且需要复制多个相同结构的情况下可以减少硬件资源的消耗,并且提高电路计算速度。(The invention provides an ORB _ SLAM relocation feature point retrieval acceleration method based on an FPGA, which comprises the following steps: s1, buffering the input picture and extracting a descriptor; s2, entering a working space Workspace, and solving the distance of the nodes through a computing circuit; s3, the results of each calculation circuit flow to a parallel comparison circuit together to find the point where the minimum value is located; s4, finally, judging whether the node is a bottom layer or not, if so, finishing the search to obtain a final node; s5, each node has an offset value for finding the address of the child node to obtain the key frame, and then relocates according to the key frame set. The invention adds the approximate unit AU in front of the counter to form the circuit structure of the accumulation parallel counter APC for the consumption of the circuit resource, based on the approximate calculation principle, the invention can reduce the consumption of the hardware resource and improve the circuit calculation speed under the condition that the bit stream is longer and a plurality of same structures need to be copied.)
技术领域
本发明涉及图像处理领域，尤其涉及一种基于FPGA的ORB_SLAM重定位特征点检索加速方法。
背景技术
在slam导航定位失败的时候，系统会启动重定位，基于系统当前拍的一帧，跟据其特征点从帧库里面找出一些相似的关键帧来更新当前帧的地图点匹配，从而进行重定位的操作。在现有ORB重定位算法中，帧库的构建采用了k均值(k-means)树的结构，如图1所示。将所有训练图片的特征点提取并计算成描述子，并通过d次k-means聚类，就得到了一个d层度为k的检索树，每个簇中心用平均距离表示，在最底层的类就记录着所关联的帧，因此每次重定位算法就是将当前帧所提取出来的所有特征点(256bit描述子表示)通过检索树找到它们最终所属的类，每一层都会与其所有子结点求距离即汉明距离，最小值为下一层搜索路径方向，直至到最后一层，对于根据最后所找到的关联的帧求相似性分数筛选出一些关键帧来完成重定位的计算。
然而当我们的库里面帧的数量变得很庞大的时候，检索树的深度和广度都会变得比较大，特征点检索的速度相应地就会变得很慢，尽管在ORB算法中维护了一个前向索引来便以快速检索，但是面对巨大的帧库的时候，实时性还是难以达到实际的要求。
发明内容
本发明旨在至少解决现有技术中存在的技术问题，特别创新地提出了一种基于FPGA的ORB_SLAM重定位特征点检索加速方法。
为了实现本发明的上述目的，本发明提供了一种基于FPGA的ORB_SLAM重定位特征点检索加速方法，包括：
S1，将输入的图片缓冲，提取描述子；
S2，然后进入工作空间Workspace，通过计算电路对结点的距离求解；
S3，每个计算电路的结果会一起流向一个并行比较电路，求出最小值所在的点；
S4，最后再判断是否是底层，若是则搜索结束，得出最终结点；
S5，每个结点会有一个偏移值，用于寻找求子结点的地址，从而获得关键帧，然后根据关键帧集合进行重新定位。
在本发明的一种优选实施方式中，所述计算电路包括：
先通过异或门再通过累计并行计数器APC或并行计数器PC；
所述累计并行计数器APC为在计数器PC前加上X级近似单元AU；
所述近似单元AU包括：一级AU近似单元即一列的与门和或门；
所述并行计数器PC包括多个全加器。
在本发明的一种优选实施方式中，所述并行计数器PC包括：
每三个bit一组送入全加器，每个全加器会有一个权重，其中三个输入就是同样的这种权重，输出本位和Sum会往同级权重全加器送值，而向相邻高位的进位数Cout则会往高一级权重送值；用同样的操作将中间结果“打一拍”，接着计算输入都已有值的全加器，一直将中间结果打拍，直至输出结果的最高位算出，最后输出的每个箭头都是一个bit。
在本发明的一种优选实施方式中，所述并行计数器PC还包括：
v表示输出位宽，输入位宽N＝2v，输出每增加一位消耗的全加器数量会变成2倍再加上v-1，即：
f(v)＝2*f(v-1)+v-1
其中，f(v)代表输出位宽为v需要的全加器数量，f(v-1)代表输出位宽为v-1需要的全加器数量。
在本发明的一种优选实施方式中，并行计数器PC消耗的门级资源为：
g(v)＝(2v-v-1)*5＝(N-log2 N-1)*5
其中N表示输入位宽，v表示输出位宽。
在本发明的一种优选实施方式中，累计并行计数器APC在全加器前加上X级近似单元AU，消耗的门级资源为：
其中N表示输入位宽，X表示近似单元的级数。
在本发明的一种优选实施方式中，所述全加器包括：
两个异或门，两个与门和一个或门，具体逻辑表达式为：
Sum＝(A^B)^Cin
Cout＝(A&B)|((A^B)&Cin)
其中A、B、Cin为全加器的输入，分别为被加数、加数、相邻低位来的进位数，Sum、Cout为全加器的输出，分别为输出本位和、向相邻高位的进位数，^表示异或运算，&表示与运算，|表示或运算。
在本发明的一种优选实施方式中，在步骤S5中包括以下步骤：
S51，从关键帧集合中任意选取I个特征点，所述I为大于或者等于1的正整数，计算出当前帧的位姿(α，γ)，其中α表示旋转角度，γ是平移量；
S52，根据步骤S51的位姿(α，γ)，计算剩余关键帧的重投影误差，若计算得到的重投影误差小于或者等于所设定的误差阈值，则该点为关键点；
S53，统计关键点的个数及对应的位姿(α，γ)；
S54，利用步骤S53的位姿(α，γ)作为位姿初始值，对当前帧的位姿进行局部优化，其优化的目标函数为：
其中，ex为相机观测到的第x个重投影误差，||·||表示范数，hx为观测数；O表示相机观测到的重投影个数；
S55，若优化出的关键点数超过设定的关键点数，则认为重定位成功。
在本发明的一种优选实施方式中，在步骤S51中当前帧的位姿(α，γ)的计算方法为：
其中，I表示参考帧中的特征点的总个数；
(Xi,Yi)表示在当前帧中第i特征点的位置坐标；
(Xj,Yj)表示在当前帧中第j特征点的位置坐标；j≠i；
(Xi′,Yi′)表示在参考帧中第i特征点对应当前帧中第i特征点的位置坐标；
(Xj′,Yj′)表示在参考帧中第j特征点对应当前帧中第i特征点的位置坐标；
(x0,y0)表示参考起始点；
[Xi-x0,Yi-y0]表示在当前帧中第i特征点的向量；
[Xj-x0,Yj-y0]表示在当前帧中第j特征点的向量；
|Xi-x0,Yi-y0|表示在当前帧中第i特征点的距离值；
|Xj-x0,Yj-y0|在当前帧中第j特征点的距离值；
表示旋转误差值；
在本发明的一种优选实施方式中，在步骤S52中剩余关键帧的重投影误差的计算方法为：
其中，ε表示平衡系数；
表示位姿(α，γ)在参考帧上偏移度；
S(α,γ)表示位姿(α，γ)在剩余关键帧上偏移度；
Kk表示第k剩余关键帧的重投影误差；
当其Kk≤τ，τ表示所设定的误差阈值，则选取的特征点为关键点；
当其Kk＞τ，则选取的特征点不为关键点。
综上所述，由于采用了上述技术方案，本发明的有益效果是：
1)本发明对于两个256bit的特征点求汉明距离采用并行计数器PC的电路结构，以形成流水的形式加速计算。
2)本发明对于电路资源的消耗情况，在计数器前加上近似单元AU形成累计并行计数器APC的电路结构，基于近似计算的原理，在bit流比较长并且需要复制多个相同结构的情况下可以减少硬件资源的消耗，并且提高电路计算速度。
3)本发明针对k-means树上层聚类粒度粗下层聚类粒度细的特性，上层采取多级近似单元，下层采用一层或者不用近似单元。
4)本发明对于每个子结点求距离计算的独立性，设计为k个Worker,其中k为聚类簇的数量，每个Worker都拥有加速电路的一个副本并且并行控制各自的数据的读取输入和计算。
5)本发明采用动态随机存取存储器DRAM来存储帧库的特征点，根据DRAM里各个内存库Bank读写并行的特性，将每个子结点分别存在不同的Bank可以提高数据的读取速度，并且由上述的每个Worker负责管理一个Bank。
本发明的附加方面和优点将在下面的描述中部分给出，部分将从下面的描述中变得明显，或通过本发明的实践了解到。
附图说明
本发明的上述和/或附加的方面和优点从结合下面附图对实施例的描述中将变得明显和容易理解，其中：
图1是现有技术特征点检索树的结构图；
图2是本发明的整体结构图；
图3是本发明的求汉明距离的并行计数电路示意图(16位输入为例)；
图4是全加器的门级电路示意图；
图5是采用近似单元的并行计数电路示意图；
图6是检索树数据在DRAM里的存储形式示意图。
具体实施方式
下面详细描述本发明的实施例，所述实施例的示例在附图中示出，其中自始至终相同或类似的标号表示相同或类似的元件或具有相同或类似功能的元件。下面通过参考附图描述的实施例是示例性的，仅用于解释本发明，而不能理解为对本发明的限制。
如图1所示，结合帧库的所有特征点离线训练一个检索树，每个特征点用256bit描述子表示，而这些树节点用属于该类的所有描述子的均值来表示。在检索时逐层找到所属的类即找到最小汉明距离，直至最底层。其中特征点是具体图片中的特征点，结点指的是树结构中的结点，一般表示的属于该类的均值。
本发明提供了一种基于FPGA的ORB_SLAM重定位特征点检索加速方法，包括：
S1，将输入的图片缓冲，提取描述子；
S2，然后进入工作空间Workspace，通过计算电路对结点的距离求解；
S3，每个计算电路的结果会一起流向一个并行比较电路，求出最小值所在的点；
S4，最后再判断是否是底层，若是则搜索结束，得出最终结点；
S5，每个结点会有一个偏移值，用于寻找求子结点的地址，从而获得关键帧，然后根据关键帧集合进行重新定位。
在本发明的一种优选实施方式中，所述计算电路包括：
先通过异或门再通过累计并行计数器APC或并行计数器PC；
所述累计并行计数器APC为在计数器PC前加上X级近似单元AU；
所述近似单元AU包括：一级AU近似单元即一列的与门和或门；
所述并行计数器PC包括多个全加器。
在本发明的一种优选实施方式中，所述并行计数器PC包括：
每三个bit一组送入全加器，每个全加器会有一个权重，其中三个输入就是同样的这种权重，输出本位和Sum会往同级权重全加器送值，而向相邻高位的进位数Cout则会往高一级权重送值；用同样的操作将中间结果“打一拍”，接着计算输入都已有值的全加器，一直将中间结果打拍，直至输出结果的最高位算出，最后输出的每个箭头都是一个bit。
在本发明的一种优选实施方式中，所述并行计数器PC还包括：
v表示输出位宽，输入位宽N＝2v，输出每增加一位消耗的全加器数量会变成2倍再加上v-1，即：
f(v)＝2*f(v-1)+v-1
其中，f(v)代表输出位宽为v需要的全加器数量，f(v-1)代表输出位宽为v-1需要的全加器数量。
在本发明的一种优选实施方式中，并行计数器PC消耗的门级资源为：
g(v)＝(2v-v-1)*5＝(N-log2 N-1)*5
其中N表示输入位宽，v表示输出位宽。
在本发明的一种优选实施方式中，累计并行计数器APC在全加器前加上X级近似单元AU，消耗的门级资源为：
其中N表示输入位宽，X表示近似单元的级数。
在本发明的一种优选实施方式中，所述全加器包括：
两个异或门，两个与门和一个或门，具体逻辑表达式为：
Sum＝(A^B)^Cin
Cout＝(A&B)|((A^B)&Cin)
其中A、B、Cin为全加器的输入，分别为被加数、加数、相邻低位来的进位数，Sum、Cout为全加器的输出，分别为输出本位和、向相邻高位的进位数，^表示异或运算，&表示与运算，|表示或运算。
图2是本发明的整体加速结构图，input query输入查询就是一副图会有很多特征点去进行检索查询，由于输入有很多，不能一次性全部送去查询，所以用了一种数据结构即具有先进先出特点的队列来存储。输入模块是一个队列用于缓冲输入的图片提出来的描述子；
Workspace是主要的工作空间，其中的每个Worker负责并行控制一个结点距离求解，每个Worker还分别对应一个高速缓存(cache)和计算电路(PC或者APC)，cache用于缓冲曾经访问过的子结点，因为相邻特征点之间的距离比较小，可能会有一些相似的访问路径；当cache中不存在数据时，就会访问DRAM；每个计算电路的结果会一起流向一个并行比较电路，求出最小值所在的点，最后再判断是否是底层，若是则搜索结束，每个结点会有一个偏移值，用于寻找求子结点的地址，然后重新定位。
DRAM是FPGA中的一种存储结构，可以理解成电脑中的内存，cache是一种缓存，比较小但是访问速度比DRAM快。Cache会存储一部分DRAM的数据，本身比较小，但是访问它的速度会比去访问DRAM速度快，所以在访问dram的时候同时会把数据缓冲在cache里面(由于本身比较小，可能会覆盖掉一些旧的数据)，每次取数据时，会先去访问cache，如果没有相应的数据就再去访问dram并把数据缓冲下来。这种机制主要是计算机里有个性质，最近访问的数据很可能会再次访问，这样可以提高取数据的速度。
图3是本发明的核心计算电路，采用并行计数的结构，以16位输入为例。每个单元为一个全加器，采用组合逻辑，具体逻辑表达式为：Sum＝(A^B)^Cin，Cout＝(A&B)|((A^B)&Cin)，总共消耗两个异或门，两个与门和一个或门，如图4所示。
设输出v表示输出需要几位来表示即输出位宽，输入位宽N＝2v，由图3可见输出每增加一位，输入增加一倍；输出每增加一位消耗的全加器数量会变成2倍再加上v-1，即：
f(v)＝2*f(v-1)+v-1
其中f(2)＝1
则消耗的门级资源为：
g(v)＝(2v-v-1)*5＝(N-log2 N-1)*5
其中N表示输入位宽，v表示结果的位数即输出位宽。基于近似计算的原理，APC在全加器前加上一级AU近似单元即一列的与门和或门，如图5所示，可以在很小的误差下得到计算结果(实验验证误差在5以内)，还能减少资源的消耗，此时消耗的门级资源变为：
类似的，APC在全加器前加上X级AU近似单元，消耗的门级资源变为：
对于k-means树上层聚类粒度粗下层聚类粒度细的特性，上层采取多级近似单元，下层采用一层或者不用近似单元。其中相同层级的结点采用同样结构的计算电路，结点间是并行的。
对于输入为256位bit，一级(门级)近似单元就可以减少约50％的资源消耗，同时还提高了计算速度。
两个求距离的256bit进入电路后，所述电路包括异或门在计算电路PC或计算电路APC之前。首先用组合逻辑求异或，对异或后的256位bit求1的个数就是它们两个之间的距离。异或后的256bit首先进入近似单位将输入减半，例如通过一级AU后输入将相当于变成了128位，但是每一位的权重都变成了21。然后在时钟上升沿用寄存器非阻塞赋值将中间结果“打一拍”，然后每三个bit一组送入全加器(一开始送进权重20的全加器)，每个全加器会有一个权重，其中三个输入就是同样的这种权重，输出Sum会往同级权重全加器送值，而进位Cout则会往高一级权重送值；用同样的操作将中间结果“打一拍”，接着计算输入都已有值的全加器，一直将中间结果打拍，直至输出结果的最高位算出，最后输出的每个箭头都是一个bit(0或者1)如图5是以16位为例的，输出可以用4位bit表示，2的多少次方就表示箭头指向4位中的哪一位，这是电路直接连线的，实际计算就是0或者1代表系数，2的多少次方代表权重然后累计起来。这样就形成了在计算电路内部的流水，第一个描述子会在第11个周期得到结果，接下来的每一个周期都会得到一个描述子的计算结果。由于与每个子结点求距是并行的，在求出结果后会全部送进比较树，求出最小值来确定下一层路径往哪里走。
在检索树上层簇与簇之间距离比较大，越往下差距会越来越小，根据数据集实际情况控制每一层采用的AU计算单元的级数，上层差距比较大，可以采用两到三层，底层的时候视数据集情况决定是否采用近似单元。
对于树在DRAM里面的存储，如图6所示，DRAM每个Bank共享一个I/O控制端口，但是在每个Bank内部的读写可以并行进行。将每个子结点存在对应的bank里，在cache里没有访问到数据需要访问DRAM时，每个Worker就可以并行读取自己的Bank。
在本发明的一种优选实施方式中，在步骤S5中包括以下步骤：
S51，从关键帧集合中任意选取I个特征点，所述I为大于或者等于1的正整数，计算出当前帧的位姿(α，γ)，其中α表示旋转角度，γ是平移量；
S52，根据步骤S51的位姿(α，γ)，计算剩余关键帧的重投影误差，若计算得到的重投影误差小于或者等于所设定的误差阈值，则该点为关键点；
S53，统计关键点的个数及对应的位姿(α，γ)；
S54，利用步骤S53的位姿(α，γ)作为位姿初始值，对当前帧的位姿进行局部优化，其优化的目标函数为：
其中，ex为相机观测到的第x个重投影误差，||·||表示范数，hx为观测数；O表示相机观测到的重投影个数；
S55，若优化出的关键点数超过设定的关键点数，则认为重定位成功。
在本发明的一种优选实施方式中，在步骤S51中当前帧的位姿(α，γ)的计算方法为：
其中，I表示参考帧中的特征点的总个数；
(Xi,Yi)表示在当前帧中第i特征点的位置坐标；
(Xj,Yj)表示在当前帧中第j特征点的位置坐标；j≠i；
(Xi′,Yi′)表示在参考帧中第i特征点对应当前帧中第i特征点的位置坐标；
(Xj′,Yj′)表示在参考帧中第j特征点对应当前帧中第i特征点的位置坐标；
(x0,y0)表示参考起始点；
[Xi-x0,Yi-y0]表示在当前帧中第i特征点的向量；
[Xj-x0,Yj-y0]表示在当前帧中第j特征点的向量；
|Xi-x0,Yi-y0|表示在当前帧中第i特征点的距离值；
|Xj-x0,Yj-y0|在当前帧中第j特征点的距离值；
表示旋转误差值；
在本发明的一种优选实施方式中，在步骤S52中剩余关键帧的重投影误差的计算方法为：
其中，ε表示平衡系数；
表示位姿(α，γ)在参考帧上偏移度；
S(α,γ)表示位姿(α，γ)在剩余关键帧上偏移度；
Kk表示第k剩余关键帧的重投影误差；
当其Kk≤τ，τ表示所设定的误差阈值，则选取的特征点为关键点；
当其Kk＞τ，则选取的特征点不为关键点。
尽管已经示出和描述了本发明的实施例，本领域的普通技术人员可以理解：在不脱离本发明的原理和宗旨的情况下可以对这些实施例进行多种变化、修改、替换和变型，本发明的范围由权利要求及其等同物限定。