一种基于时间序列的视觉地图定位方法及系统与流程

本发明涉及计算机视觉，特别涉及一种基于时间序列的视觉地图定位方法及系统。

背景技术：

1、现有技术的计算机视觉定位技术，通过相机等多种传感器重建某个区域，生成视觉地图，再用包含可见光等传感器的移动设备采集一定的数据，通过计算的方式得到当前移动设备在视觉地图中位置的一种技术。

2、视觉地图是指利用相机拍摄相关场景获取建图所使用的图片，然后通过structure from motion等形式，融合或者不融合其他传感器(深度、激光等)生成的包含视觉信息的地图文件，主要包含生成的空间地图、图像中的2d特征等信息。当前拍摄的画面，可附加或不附加额外的传感器信息，定位当前画面在视觉地图中所在的位置的技术。对于纯点云地图，一般是提取待定位图像的特征及描述子，与3d点云的描述子进行对比，寻找最相似的点形成点对，最后利用点对进行位姿的解算。而针对关键帧为主的地图，则一般是对图像聚合出一个描述子，整体的直接和关键帧聚合出的描述子进行对比。搜索出与当前待定位图像相似的关键帧候选。最后通过关键帧之间的局部描述匹配解算位姿或者直接估计图像之间的sim3变换等求解当前图像的位姿。

3、现有定位技术都是基于单个图像为数据源，而定位算法中的2d-3d匹配中总是包含误差的，且可能所有匹配都是错误的，计算出来的位姿误差也就比较大了，甚至是得到比较离谱的位姿，限制了相关产品的使用。

技术实现思路

1、本发明技术方案所解决的技术问题为：如何提高视觉地图的定位精确性，如何为当前帧计算出更加鲁棒的位姿。

2、为了解决上述技术问题，本发明技术方案提供了一种基于时间序列的视觉地图定位方法，包括：

3、获取视觉地图数据，所述视觉地图数据包括：第一图像特征集及对应的第一三维点云坐标集；

4、接收待定位的序列图像f，f＝{f1,f2,f3,…,fm}；m为图像序列编号，m为大于1的自然数；

5、计算所述序列图像f在设备坐标系下的初始位姿集p1，p1＝{p11,p21,p31,…,pm1}；

6、基于初始位姿集p1获取序列图像f两两之间的相对位姿以得到第一位姿集t，t＝{t1,t2,t3,…,tm}；

7、提取每幅序列图像的图像特征以得到对应第二图像特征集j＝{j1,j2,j3,…,jm}及二维图像特征坐标集r＝{r1,r2,r3,…,rm}；

8、根据所述第二图像特征集j中每幅序列图像的图像特征，寻找所述第一图像特征集中与之匹配的图像特征，从而组成第三图像特征集j’，j’＝{j1’,j2’,j3’,…,jm’}；

9、基于所述第三图像特征集j’从所述第一三维点云坐标集中得到与所述第三图像特征集j’中图像特征对应的三维点云坐标集，以形成与所述第二图像特征集j对应的第二三维点云坐标集s＝{s1,s2,s3,…,sm}；

10、基于所述二维图像特征坐标集及对应的第二三维点云坐标集求得所述序列图像的第二位姿集p2，p2＝{p12,p22,p32,…,pm2}；

11、基于所述第一位姿集t及第二位姿集p2，计算每幅序列图像相对于其他幅序列图像的第三位姿集p3，p3＝{p13,p23,p33,…,pm3}；p13为序列图像f1在其位姿p12的基础上根据相对位姿集t1推算得到的序列图像f1,f2,f3,…,fm的关键帧位姿集；p23为序列图像f2在其位姿p22的基础上根据相对位姿集t2推算得到的序列图像f1,f2,f3,…,fm的关键帧位姿集；p33为序列图像f3在其位姿p32的基础上根据相对位姿集t3推算得到的序列图像f1,f2,f3,f4,…,fm的关键帧位姿集；以此类推，pm3为序列图像fm在其位姿pm2的基础上根据相对位姿集tm推算得到的序列图像f1,f2,f3,…,f(m-1),fm的关键帧位姿集；

12、给定投影误差阈值th，根据所述第三位姿集p3中每幅序列图像相对于其他幅序列图像的位姿元素集p13,p23,p33,…,pm3，计算每个位姿元素所对应序列图像之图像特征的三维点投影至设备坐标系下对应二维点之间的误差值集合△x，△x＝△x1,△x2,△x3,…,△xm；

13、基于所述误差值集合△x＝{△x1,△x2,△x3,…,△xm}，统计所述序列图像f1,f2,f3,…,fm位姿元素投影误差值小于所述阈值th的内点数量集合inlier；inlier＝{inlier1,inlier2,inlier3,…,inlierm}；

14、分别基于序列图像f1,f2,f3,…,fm所对应内点数量，统计内点数量集合inlier中inlier1,inlier2,inlier3,…,inlierm的最大值，设内点数量最大值对应的集合为inliermax,其中max为图像序号；

15、将对应的位姿元素集pmax3作为所述序列图像f1,f3,…,fm的当前位姿p4，p4＝pmax3。

16、可选的，基于所述第一位姿集t，由于t＝{t1,t2,t3,…,tm}：

17、t1＝{t11,t12,t13,t14,…,t1m}，t1为图像序列编号为1的序列图像f1相对于图像序列编号为1至m的序列图像f1,f2,f3,…,fm的相对位姿集；

18、t2＝{t21,t22,t23,t24,…,t2m}，t2为图像序列编号为2的序列图像f2相对于图像序列编号为1至m的序列图像f1,f2,f3,…,fm的相对位姿集；

19、t3＝{t31,t32,t33,…,t3m}，t3为图像序列编号为3的序列图像f3相对于图像序列编号为1至m的序列图像f1,f2,f3,…,fm的相对位姿集；以此类推，

20、tm＝{tm1,tm2,tm3,…,tmm}，tm为图像序列编号为m的序列图像fm相对于图像序列编号为1至m的序列图像f1,f2,f3,…,fm的相对位姿集。

21、可选的，设n1、n2、n3、…、(nm)为大于1的自然数,分别为序列图像f1,f2,f3,…,fm所提取的图像特征数目；

22、基于第二图像特征集j＝{j1,j2,j3,…,jm}、二维图像特征坐标集r＝{r1,r2,r3,…,rm}、第三图像特征集j’，j’＝{j1’,j2’,j3’,…,jm’}以及第二三维点云坐标集s＝{s1,s2,s3,…,sm}；

23、j1＝{j11,j12,j13,…,j1n1}、r1＝{r11,r12,r13,…,r1n1}、j1’＝{j11’,j12’,j13’,…,j1n1’}、s1＝{s11,s12,s13,…,s1n1}；j11,j12,j13,…,j1n1为序列图像f1所提取的图像特征元素，r11,r12,r13,…,r1n1为序列图像f1所提取图像特征元素j11,j12,j13,…,j1n1在设备坐标系下的二维坐标元素，j11’,j12’,j13’,…,j1n1’为所述第一图像特征集中分别与j11,j12,j13,…,j1n1匹配的图像特征，s11,s12,s13,…,s1n1为所述第一三维点云坐标集中与图像特征j11’,j12’,j13’,…,j1n1’对应的三维点云坐标元素；

24、j2＝{j21,j22,j23,…,j2n2}、r2＝{r21,r22,r23,…,r2n2}、j2’＝{j21’,j22’,j23’,…,j2n2’}、s2＝{s21,s22,s23,…,s2n2}；j21,j22,j23,…,j2n2为序列图像f2所提取的图像特征元素，r21,r22,r23,…,r2n2为序列图像f2所提取图像特征元素j21,j22,j23,…,j2n2在设备坐标系下的二维坐标元素，j21’,j22’,j23’,…,j2n2’为所述第一图像特征集中分别与j21,j22,j23,…,j2n2匹配的图像特征，s21,s22,s23,…,s2n2为所述第一三维点云坐标集中与图像特征j21’,j22’,j23’,…,j2n2’对应的三维点云坐标元素；

25、j3＝{j31,j32,j33,…,j3n3}、r3＝{r31,r32,r33,…,r3n3}、j3’＝{j31’,j32’,j33’,…,j3n3’}、s3＝{s31,s32,s33,…,s3n3}；j31,j32,j33,…,j3n3为序列图像f3所提取的图像特征元素，r31,r32,r33,…,r3n3为序列图像f3所提取图像特征元素j31,j32,j33,…,j3n3在设备坐标系下的二维坐标元素，j31’,j32’,j33’,…,j3n3’为所述第一图像特征集中分别与j31,j32,j33,…,j3n3匹配的图像特征，s31,s32,s33,…,s3n3为所述第一三维点云坐标集中与图像特征j31’,j32’,j33’,…,j3n3’对应的三维点云坐标元素；

26、以此类推，

27、jm＝{jm1,jm2,jm3,…,jm(nm)}、rm＝{rm1,rm2,rm3,…,rm(nm)}、jm’＝{jm1’,jm2’,jm3’,…,jm(nm)’}、sm＝{sm1,sm2,sm3,…,sm(nm)}；jm1,jm2,jm3,…,jm(nm)为序列图像fm所提取的图像特征元素，rm1,rm2,rm3,…,rm(nm)为序列图像fm所提取图像特征元素jm1,jm2,jm3,…,jm(nm)在设备坐标系下的二维坐标元素，jm1’,jm2’,jm3’,…,jm(nm)’为所述第一图像特征集中分别与jm1,jm2,jm3,…,jm(nm)匹配的图像特征，sm1,sm2,sm3,…,sm(nm)为所述第一三维点云坐标集中与图像特征jm1’,jm2’,jm3’,…,jm(nm)’对应的三维点云坐标元素。

28、可选的，基于所述第三位姿集p3＝{p13,p23,p33,…,pm3}：

29、p13为图像序列编号为1的序列图像f1在其位姿p12的基础上根据相对位姿集t1＝{t11,t12,t13,t14,…,t1m}推算得到序列图像f1相对于图像序列编号为1至m的序列图像f1,f2,f3,…,fm的关键帧位姿集，p113,p123,p133,p143,…,p1m3为序列图像f1相对于序列图像f1,f2,f3,…,fm的位姿元素；

30、p23＝{p213,p223,p233,p243,…,p2m3}，p23为图像序列编号为2的序列图像f2在其位姿p22的基础上根据相对位姿集t2＝{t21,t22,t23,t24,…,t2m}推算得到序列图像f2相对于图像序列编号为1至m的序列图像f1,f2,f3,…,fm的关键帧位姿集，p213,p223,p233,p243,…,p2m3为序列图像f2相对于序列图像f1,f2,f3,…,fm的位姿元素；

31、p33＝{p313,p323,p333,…,p3m3}，p33为图像序列编号为3的序列图像f3在其位姿p32的基础上根据相对位姿集t3＝{t31,t32,t33,…,t3m}推算得到序列图像f3相对于图像序列编号为1至m的序列图像f1,f2,f3,…,fm的关键帧位姿集，p313,p323,p333,…,p3m3为序列图像f3相对于序列图像f1,f2,f3,…,fm的位姿元素；以此类推，

32、pm3＝{pm13,pm23,pm33,…,pmm3}，pm3为图像序列编号为m的序列图像fm在其位姿pm2的基础上根据相对位姿集tm＝{tm1,tm2,tm3,…,tmm}推算得到序列图像fm相对于图像序列编号为1至m的序列图像f1,f2,f4,…,fm的关键帧位姿集，pm13,pm23,pm33,…,pmm3为序列图像f1,f2,f4,…,fm的位姿元素。

33、可选的，△x1为位姿元素p113,p123,p133,p143,…,p1m3所对应误差值集合，△x1＝{△x11,△x12,△x13,△x14,…,△x1m}；△x11,△x12,△x13,△x14,…,△x1m分别是序列图像f1,f2,f4,…,fm对应的三维点云坐标集s1,s2,s3,…,sm根据对应序列图像的位姿元素p113,p123,p133,p143,…,p1m3，投影到设备坐标系下所得投影坐标集合与二维图像特征坐标集r1,r2,r3,…,rm之间的误差值；

34、△x2为位姿元素p213,p223,p233,p243,…,p2m3所对应误差值集合，△x2＝{△x21,△x22,△x23,△x24,…,△x2m}；△x21,△x22,△x23,△x24,…,△x2m分别是序列图像f1,f2,f4,…,fm对应的三维点云坐标集s1,s2,s3,…,sm根据对应序列图像的位姿元素p213,p223,p233,p243,…,p2m3，投影到设备坐标系下所得投影坐标集合与二维图像特征坐标集r1,r2,r3,…,rm之间的误差值；

35、△x3为位姿元素p313,p323,p333,…,p3m3所对应误差值集合，△x3＝{△x31,△x32,△x33,…,△x3m}；△x31,△x32,△x33,…,△x3m分别是序列图像f1,f2,f4,…,fm对应的三维点云坐标集s1,s2,s3,…,sm根据对应序列图像的位姿元素p313,p323,p333,…,p3m3，投影到设备坐标系下所得投影坐标集合与二维图像特征坐标集r1,r2,r3,…,rm之间的误差值；以此类推，

36、△xm为位姿元素pm13,pm23,pm33,…,pmm3所对应误差值集合，△xm＝{△xm1,△xm2,△xm3,…,△xmm}；△xm1,△xm2,△xm3,…,△xmm分别是序列图像f1,f2,f4,…,fm对应的三维点云坐标集s1,s2,s3,…,sm根据对应序列图像的位姿元素pm13,pm23,pm33,…,pmm3，投影到设备坐标系下所得投影坐标集合与二维图像特征坐标集r1,r2,r3,…,rm之间的误差值。

37、可选的，通过如下方式统计所述序列图像f1,f2,f3,…,fm位姿元素投影误差值小于所述阈值th的内点数量集合inlier：

38、inlier1为位姿元素p113,p123,p133,p143,…,p1m3所对应的内点数量统计之和，inlier1＝inlier11+inlier12+inlier13+…+inlier1m；分别统计位姿元素p113,p123,p133,p143,…,p1m3对应的误差值集合△x11,△x12,△x13,△x14,…,△x1m中小于阈值th的误差值数量，并将该数量依次计为位姿元素p113,p123,p133,p143,…,p1m3对应的内点数量inlier11，inlier12,inlier13,inlier14,…,inlier1m；

39、inlier2为位姿元素p213,p223,p233,p243,…,p2m3所对应的内点数量统计之和，inlier2＝inlier21+inlier22+inlier23+…+inlier2m；分别统计位姿元素p213,p223,p233,…,p2m3对应的误差值集合△x21,△x22,△x23,△x24,…,△x2m中小于阈值th的误差值数量，并将该数量依次计为位姿元素p213,p223,p233,p243,…,p2m3对应的内点数量inlier21,inlier22,inlier23,inlier14,…,inlier2m；

40、inlier3为位姿元素p313,p323,p333,…,p3m3所对应的内点数量统计之和，inlier3＝inlier31+inlier32+inlier33+…+inlier3m；分别统计位姿元素p313,p323,p333,…,p3m3对应的误差值集合△x31,△x32,△x33,…,△x3m中小于阈值th的误差值数量，并将该数量之和依次计为位姿元素p313,p323,p333,…,p3m3对应的内点数量inlier31,inlier32,inlier33,…,inlier3m；以此类推，

41、inlierm为位姿元素pm13,pm23,pm33,…,pmm3所对应的内点数量统计集合，inlierm＝{inlierm1,inlierm2,inlierm3,…,inliermm}；分别统计位姿元素pm13,pm23,pm33,…,pmm3对应的误差值集合△xm1,△xm2,△xm3,…,△xmm中小于阈值th的误差值数量，并将该数量之和依次计为位姿元素pm13,pm23,pm33,…,pmm3对应的内点数量inlierm1,inlierm2,inlierm3,…,inliermm；取内点数量集合inlier＝{inlier1,inlier2,inlier3,…,inlierm}中内点数量最大值，记为inliermax，其中max为对应的图像序列编号；

42、设内点数量inliermax所对应的关键帧位姿pmax3；pmax3＝{pmax13,pmax23,pmax33,…,pmax(m)3}；

43、所述序列图像f1,f3,…,fm的当前位姿集p4为：

44、p4＝pmax3＝{pmax13,pmax23,pmax33,…,pmax(m)3}。

45、可选的，所述的基于时间序列的视觉地图定位方法还包括：

46、设p4＝pmax3＝{pmax13,pmax23,pmax33,…,pmax(m)3}＝{p14,p24,p34,…,pm4}，依次为所述序列图像f1,f2,f3,…,fm的当前帧位姿；

47、计算每个序列图像f1,f2,f3,…,fm的当前帧位姿所对应图像特征的三维点投影至设备坐标系下对应二维点之间的误差值集合△x’，△x’＝{△x1’,△x2’,△x3’,…,△xm’}；

48、基于所述误差值集合△x’＝{△x1’,△x2’,△x3’,…,△xm’}，对当前位姿集p4中的当前帧位姿p14,p24,p34,…,pm4进行优化，以使误差值集合△x’中误差值元素△x1’,△x2’,△x3’,…,△xm’的误差值之和最小化；

49、设优化后的当前位姿集为p5,p5＝{p15,p25,p35,…,pm5}，p15,p25,p35,…,pm5分别为优化后的位姿元素p14,p24,p34,…,pm4；

50、输出优化后的当前位姿集p5。

51、可选的，通过如下方式对当前位姿集p4中的当前帧位姿p14,p24,p34,…,pm4进行优化：

52、△x1’,△x2’,△x3’,…,△xm’分别为序列图像f1,f2,f3,…,fm对应的图像特征集合j1,j2,j3,…,jm对应的第二三维点云坐标集s1,s2,s3,…,sm根据当前帧位姿p14,p24,p34,…,pm4投影到设备坐标系下所得投影坐标集与二维图像特征坐标集r1,r2,r3,…,rm之间的误差值；

53、令emin＝△x1’+△x2’+△x3’+,…,+△xm’，调整当前帧位姿p14,p24,p34,…,pm4的数值，使emin的误差值最小化；

54、设调整后的当前帧位姿依次为p15,p25,p35,…,pm5，优化后的当前位姿集p5＝{p15,p25,p35,…,pm5}。

55、为了解决上述技术问题，本发明技术方案还提供了一种基于时间序列的视觉地图定位系统，包括：

56、第一获取单元，适于获取视觉地图数据，所述视觉地图数据包括：第一图像特征集及对应的第一三维点云坐标集；

57、接收单元，适于接收待定位的序列图像f，f＝{f1,f2,f3,…,fm}；m为图像序列编号，m为大于1的自然数；

58、第一计算单元，适于计算所述序列图像f在设备坐标系下的初始位姿集p1，p1＝{p11,p21,p31,…,pm1}；

59、第二计算单元，适于基于初始位姿集p1获取序列图像f两两之间的相对位姿以得到第一位姿集t，t＝{t1,t2,t3,…,tm}；

60、提取单元，适于提取每幅序列图像的图像特征以得到对应第二图像特征集j＝{j1,j2,j3,…,jm}及二维图像特征坐标集r＝{r1,r2,r3,…,rm}；

61、匹配单元，适于根据所述第二图像特征集j中每幅序列图像的图像特征，寻找所述第一图像特征集中与之匹配的图像特征，从而组成第三图像特征集j’，j’＝{j1’,j2’,j3’,…,jm’}；

62、第二获取单元，适于基于所述第三图像特征集j’从所述第一三维点云坐标集中得到与所述第三图像特征集j’中图像特征对应的三维点云坐标集，以形成与所述第二图像特征集j对应的第二三维点云坐标集s＝{s1,s2,s3,…,sm}；

63、第三计算单元，适于基于所述二维图像特征坐标集及对应的第二三维点云坐标集求得所述序列图像的第二位姿集p2，p2＝{p12,p22,p32,…,pm2}；

64、第四计算单元，适于基于所述第一位姿集t及第二位姿集p2，计算每幅序列图像相对于其他幅序列图像的第三位姿集p3，p3＝{p13,p23,p33,…,pm3}；p13为序列图像f1在其位姿p12的基础上根据相对位姿集t1推算得到的序列图像f1,f2,f3,…,fm的关键帧位姿集；p23为序列图像f2在其位姿p22的基础上根据相对位姿集t2推算得到的序列图像f1,f2,f3,…,fm的关键帧位姿集；p33为序列图像f3在其位姿p32的基础上根据相对位姿集t3推算得到的序列图像f1,f2,f3,f4,…,fm的关键帧位姿集；以此类推，pm3为序列图像fm在其位姿pm2的基础上根据相对位姿集tm推算得到的序列图像f1,f2,f3,…,f(m-1),fm的关键帧位姿集；

65、第五计算单元，适于给定投影误差阈值th，根据所述第三位姿集p3中每幅序列图像相对于其他幅序列图像的位姿元素集p13,p23,p33,…,pm3，计算每个位姿元素所对应序列图像之图像特征的三维点投影至设备坐标系下对应二维点之间的误差值集合△x，△x＝△x1,△x2,△x3,…,△xm；

66、统计单元，适于基于所述误差值集合△x＝{△x1,△x2,△x3,…,△xm}，统计所述序列图像f1,f2,f3,…,fm位姿元素投影误差值小于所述阈值th的内点数量集合inlier；inlier＝{inlier1,inlier2,inlier3,…,inlierm}；分别基于序列图像f1,f2,f3,…,fm所对应内点数量，统计内点数量集合inlier中inlier1,inlier2,inlier3,…,inlierm的最大值；设内点数量最大值对应的集合为inliermax,其中max为图像序号；

67、第一输出单元，适于将对应的位姿元素集pmax3作为所述序列图像f1,f3,…,fm的当前位姿p4，p4＝pmax3。

68、可选的，所述基于时间序列的视觉地图定位系统还包括：

69、设p4＝pmax3＝{pmax13,pmax23,pmax33,…,pmax(m)3}＝{p14,p24,p34,…,pm4}，依次为所述序列图像f1,f2,f3,…,fm的当前帧位姿；

70、第六计算单元，适于计算每个序列图像f1,f2,f3,…,fm的当前帧位姿所对应图像特征的三维点投影至设备坐标系下对应二维点之间的误差值集合△x’，△x’＝{△x1’,△x2’,△x3’,…,△xm’}；

71、优化单元，适于基于所述误差值集合△x’＝{△x1’,△x2’,△x3’,…,△xm’}，对当前位姿集p4中的当前帧位姿p14,p24,p34,…,pm4进行优化，以使误差值集合△x’中误差值元素△x1’,△x2’,△x3’,…,△x0’的误差值之和最小化；设优化后的当前位姿集为p5,p5＝{p15,p25,p35,…,pm5}，p15,p25,p35,…,pm5分别为优化后的位姿元素p14,p24,p34,…,pm4；

72、第二输出单元，适于输出优化后的当前位姿集p5。

73、本发明技术方案的有益效果至少包括：

74、本发明技术方案能够解决现有技术中视觉地图定位应用场景下单个图像定位误差大的问题。本发明技术方案通过序列图像位姿及相对位姿，在已有的视觉地图中顾及序列图像的连续帧之间的内在关系，可为当前帧计算出更加鲁棒的位姿。

75、本发明技术方案能够应用于计算机窗口内的序列图像或视频离散化成序列图像，通过序列图像帧图像的2d-3d匹配，可以通过vio可以计算窗口内任意两个帧图像之间的相对位姿(delta pose)，将窗口内的序列帧匹配经过ransac计算出一个残差最小的当前帧初始pose，再用rigba的方式对初始位姿(pose)进行优化，得到最终高精度的位姿。

76、本发明技术方案能够适用于计算机处理窗口内所有序列图像数据，数据更加冗余，可以减少错误匹配带来的影响。冗余的数据可以使得约束变多，大量的约束可以有效提高定位精度。即使当前定位帧的匹配全部错的，通过vio提供的相对位姿(delta pose)，也可以将序列帧其他高精度的位姿(pose)反算至当前帧，同理也起到直接剔除单帧定位中的错误位姿的作用。