1.一种视觉惯导设备的位姿跟踪方法,其特征在于,所述方法包括:
获取图像采集单元采集的当前帧,以及惯性测量单元采集的当前惯性数据;
将三维地图中的地图点投影至所述当前帧,确定所述当前帧中特征点与所述三维地图中地图点对应的第一视觉约束关系;获取所述当前帧的上一帧中特征点与所述三维地图中地图点对应的第二视觉约束关系;根据所述当前惯性数据,确定所述当前帧与所述上一帧之间的惯性约束关系;获取所述上一帧之前的历史帧对所述上一帧的先验约束关系;
利用所述第一视觉约束关系、所述第二视觉约束关系、所述惯性约束关系及所述先验约束关系,确定视觉惯导设备的当前位姿;
其中,所述利用所述第一视觉约束关系、所述第二视觉约束关系、所述惯性约束关系及所述先验约束关系,确定视觉惯导设备的当前位姿,包括:利用所述第一视觉约束关系、所述第二视觉约束关系、所述惯性约束关系及所述先验约束关系,构建视觉惯导设备的联合优化函数;利用优化方法,对所述联合优化函数中的视觉惯导设备的位姿进行优化,得到视觉惯导设备的当前位姿。
2.根据权利要求1所述的方法,其特征在于,在所述将三维地图中的地图点投影至所述当前帧,确定所述当前帧中特征点与所述三维地图中地图点对应的第一视觉约束关系;获取所述当前帧的上一帧中特征点与所述三维地图中地图点对应的第二视觉约束关系;根据所述当前惯性数据,确定所述当前帧与所述上一帧之间的惯性约束关系;获取所述上一帧之前的历史帧对所述上一帧的先验约束关系之前,所述方法还包括:统计所述当前帧中特征点的数目;
所述将三维地图中的地图点投影至所述当前帧,确定所述当前帧中特征点与所述三维地图中地图点对应的第一视觉约束关系;获取所述当前帧的上一帧中特征点与所述三维地图中地图点对应的第二视觉约束关系;根据所述当前惯性数据,确定所述当前帧与所述上一帧之间的惯性约束关系;获取所述上一帧之前的历史帧对所述上一帧的先验约束关系,包括:若所述数目小于预设阈值,则将三维地图中的地图点投影至所述当前帧,确定所述当前帧中特征点与所述三维地图中地图点对应的第一视觉约束关系;获取所述当前帧的上一帧中特征点与所述三维地图中地图点对应的第二视觉约束关系;根据所述当前惯性数据,确定所述当前帧与所述上一帧之间的惯性约束关系;获取所述上一帧之前的历史帧对所述上一帧的先验约束关系。
3.根据权利要求1所述的方法,其特征在于,所述将三维地图中的地图点投影至所述当前帧,确定所述当前帧中特征点与所述三维地图中地图点对应的第一视觉约束关系,包括:获取所述视觉惯导设备的当前跟踪状态;
根据所述当前跟踪状态,估计引导匹配窗口的大小;
将三维地图中的地图点投影至所述当前帧,得到所述地图点对应的投影像素点;
将所述当前帧中所述投影像素点周围符合所述大小的引导匹配窗口内的特征点与所述地图点进行匹配,并根据匹配结果,确定所述当前帧中特征点与所述地图点对应的第一视觉约束关系。
4.根据权利要求3所述的方法,其特征在于,所述当前跟踪状态包括跟踪率及丢失时间,所述跟踪率为所述当前帧跟踪到所述三维地图中地图点的数目与所述上一帧跟踪到所述三维地图中地图点的数目的比值。
5.根据权利要求1所述的方法,其特征在于,所述根据所述当前惯性数据,确定所述当前帧与所述上一帧之间的惯性约束关系,包括:根据所述当前惯性数据,利用预积分策略,将所述当前惯性数据转换为所述当前帧与所述上一帧之间的惯性约束关系。
6.根据权利要求1所述的方法,其特征在于,所述获取所述上一帧之前的历史帧对所述上一帧的先验约束关系,包括:获取边缘化时刻所述上一帧的状态值、所述边缘化时刻上一帧位姿的信息矩阵以及所述上一帧的实时状态值;
根据所述边缘化时刻所述上一帧的状态值、所述边缘化时刻上一帧位姿的信息矩阵以及所述上一帧的实时状态值,确定所述上一帧之前的历史帧对所述上一帧的先验约束关系。
7.根据权利要求1所述的方法,其特征在于,在所述利用所述第一视觉约束关系、所述第二视觉约束关系、所述惯性约束关系及所述先验约束关系,确定视觉惯导设备的当前位姿之后,所述方法还包括:根据所述当前位姿,将所述三维地图中与所述当前帧的特征点匹配的地图点重新投影至所述当前帧,计算所述当前帧中特征点与所述三维地图中所述匹配的地图点对应的第三视觉约束关系;
确定第三视觉约束关系小于或等于预设值的多个地图点;
从所述当前帧的历史帧中,确定跟踪到所述多个地图点的共视关键帧,并根据所述共视关键帧的所有特征点对应的地图点,构建局部三维地图;
将所述局部三维地图中的地图点投影至所述当前帧,确定所述当前帧中特征点与所述局部三维地图中地图点对应的第四视觉约束关系;获取所述当前帧的上一帧中特征点与所述局部三维地图中地图点对应的第五视觉约束关系;根据所述当前惯性数据,确定所述当前帧与所述上一帧之间的惯性约束关系;获取所述上一帧之前的历史帧对所述上一帧的先验约束关系;
利用所述第四视觉约束关系、所述第五视觉约束关系、所述惯性约束关系及所述先验约束关系,更新所述视觉惯导设备的当前位姿。
8.一种视觉惯导设备的位姿跟踪装置,其特征在于,所述装置包括:
获取模块,用于获取图像采集单元采集的当前帧,以及惯性测量单元采集的当前惯性数据;
计算模块,用于将三维地图中的地图点投影至所述当前帧,确定所述当前帧中特征点与所述三维地图中地图点对应的第一视觉约束关系;获取所述当前帧的上一帧中特征点与所述三维地图中地图点对应的第二视觉约束关系;根据所述当前惯性数据,确定所述当前帧与所述上一帧之间的惯性约束关系;获取所述上一帧之前的历史帧对所述上一帧的先验约束关系;
预估模块,用于利用所述第一视觉约束关系、所述第二视觉约束关系、所述惯性约束关系及所述先验约束关系,确定视觉惯导设备的当前位姿;
所述预估模块具体用于,利用所述第一视觉约束关系、所述第二视觉约束关系、所述惯性约束关系及所述先验约束关系,构建视觉惯导设备的联合优化函数;利用优化方法,对所述联合优化函数中的视觉惯导设备的位姿进行优化,得到视觉惯导设备的当前位姿。
9.根据权利要求8所述的装置,其特征在于,所述装置还包括:
统计模块,用于统计所述当前帧中特征点的数目;
所述计算模块,具体用于:
若所述数目小于预设阈值,则将三维地图中的地图点投影至所述当前帧,确定所述当前帧中特征点与所述三维地图中地图点对应的第一视觉约束关系;获取所述当前帧的上一帧中特征点与所述三维地图中地图点对应的第二视觉约束关系;根据所述当前惯性数据,确定所述当前帧与所述上一帧之间的惯性约束关系;获取所述上一帧之前的历史帧对所述上一帧的先验约束关系。
10.根据权利要求8所述的装置,其特征在于,所述计算模块,在用于所述将三维地图中的地图点投影至所述当前帧,确定所述当前帧中特征点与所述三维地图中地图点对应的第一视觉约束关系时,具体用于:获取所述视觉惯导设备的当前跟踪状态;
根据所述当前跟踪状态,估计引导匹配窗口的大小;
将三维地图中的地图点投影至所述当前帧,得到所述地图点对应的投影像素点;
将所述当前帧中所述投影像素点周围符合所述大小的引导匹配窗口内的特征点与所述地图点进行匹配,并根据匹配结果,确定所述当前帧中特征点与所述地图点对应的第一视觉约束关系。
11.根据权利要求10所述的装置,其特征在于,所述当前跟踪状态包括跟踪率及丢失时间,所述跟踪率为所述当前帧跟踪到所述三维地图中地图点的数目与所述上一帧跟踪到所述三维地图中地图点的数目的比值。
12.根据权利要求8所述的装置,其特征在于,所述计算模块,在用于所述根据所述当前惯性数据,确定所述当前帧与所述上一帧之间的惯性约束关系时,具体用于:根据所述当前惯性数据,利用预积分策略,将所述当前惯性数据转换为所述当前帧与所述上一帧之间的惯性约束关系。
13.根据权利要求8所述的装置,其特征在于,所述计算模块,在用于所述获取所述上一帧之前的历史帧对所述上一帧的先验约束关系时,具体用于:获取边缘化时刻所述上一帧的状态值、所述边缘化时刻上一帧位姿的信息矩阵以及所述上一帧的实时状态值;
根据所述边缘化时刻所述上一帧的状态值、所述边缘化时刻上一帧位姿的信息矩阵以及所述上一帧的实时状态值,确定所述上一帧之前的历史帧对所述上一帧的先验约束关系。
14.根据权利要求8所述的装置,其特征在于,所述装置还包括:
投影模块,用于根据所述当前位姿,将所述三维地图中与所述当前帧的特征点匹配的地图点重新投影至所述当前帧,计算所述当前帧中特征点与所述三维地图中所述匹配的地图点对应的第三视觉约束关系;
确定模块,用于确定第三视觉约束关系小于或等于预设值的多个地图点;
构建模块,用于从所述当前帧的历史帧中,确定跟踪到所述多个地图点的共视关键帧,并根据所述共视关键帧的所有特征点对应的地图点,构建局部三维地图;
所述计算模块,还用于将所述局部三维地图中的地图点投影至所述当前帧,确定所述当前帧中特征点与所述局部三维地图中地图点对应的第四视觉约束关系;获取所述当前帧的上一帧中特征点与所述局部三维地图中地图点对应的第五视觉约束关系;根据所述当前惯性数据,确定所述当前帧与所述上一帧之间的惯性约束关系;获取所述上一帧之前的历史帧对所述上一帧的先验约束关系;
所述预估模块,还用于利用所述第四视觉约束关系、所述第五视觉约束关系、所述惯性约束关系及所述先验约束关系,更新所述视觉惯导设备的当前位姿。
15.一种视觉惯导设备,其特征在于,包括图像采集单元、惯性测量单元、处理器和存储器,其中,所述图像采集单元,用于采集视频帧,并将采集到的当前帧发送至所述处理器;
所述惯性测量单元,用于采集惯性数据,并将采集到的当前惯性数据发送至所述处理器;
所述存储器存储有能够被所述处理器执行的机器可执行指令,所述机器可执行指令由所述处理器加载并执行,以实现权利要求1‑7任一项所述的方法。