基于复合流形优化的视觉惯导融合位姿估计方法及装置

本发明公开了一种基于复合流形优化的视觉惯导融合位姿估计方法及装置，该方法包括：通过待估计设备的图像获取模块获取实时图像数据以及通过待估计设备的传感模块获取惯性测量数据；基于预设的图像获取模块对应的观测投影模型，根据实时图像数据，计算图像观测数据；基于预先建立的惯导测量与动态模型，根据惯性测量数据，计算惯导预积分数据；基于预先建立的复合全解耦流形<SO(3),R3,R3>优化的位姿估计框架，根据图像观测数据和惯导预积分数据，计算待估计设备的位姿数据。可见，实施本发明能够对待估计设备的位姿进行复合流形优化，提高视觉惯导融合优化算法的计算效率并保持了惯导预积分的一致性。