一种快速稳健的单目相机位姿估计算法

摘要

本发明涉及一种快速稳健的单目相机位姿估计算法，所述方法包括使用S估计方法迭代计算标志点权值，通过加权正交迭代算法计算世界坐标系相对于相机的旋转矩阵和平移向量得到位姿，如此不断循环往复，直至收敛，得到优化后的位姿参数。本发明不仅可以自动识别粗差点、提高算法的稳健性，而且能实时、精确地求得旋转矩阵和平移向量，获得准确、可靠的单目相机位姿参数。