[发明专利]用于超大场景测量多视角相机的位姿估计方法

权利要求书

1.一种用于超大场景测量多视角相机的位姿估计方法，其特征在于：包括如下步骤：

A)将旋转矩阵和平移矩阵转换到指数映射空间：

设R_c为相机坐标系下相对世界坐标系下的旋转矩阵，T_c为相机坐标系下相对世界坐标系下的平移矩阵，将旋转矩阵R_c和平移矩阵T_c转换到指数映射空间，得到

式中，H为相机外参数，τ为向量表示，τ^∧为反对称矩阵，取得：

式中，θ为旋转轴的旋转角度，ρ为旋转轴长度，x为x轴，y为y轴，z为z轴，是θ归一化后的向量，取得

θ、与旋转矩阵R_c的关系为：

将旋转矩阵R_c和平移矩阵T_c转化为指数映射中的θ、表示，其平移关系表示为：

Jρ＝T_c

式中，I为单位矩阵，J为旋转轴矩阵表示；

B)优化多视角位姿的参数：

根据小孔成像模型，由于多个视角位姿关系是相对的，选择第一个视角H₁＝I，则

S₁和S₂为缩放系数，[u v]^T为像素坐标，K_c为内参数矩阵，[X₁ Y₁ Z₁]为世界坐标系下的坐标，H₂为第二视角的透视变换矩阵，对于多视角模型，其所有相片的重投影误差最小，因此

其中f(τ^∧)为重投影误差的优化目标函数，u_i为像素坐标，i表示同一个视角下第i个数据点，j表示第j个视角，S_i为数据的缩尺比例系数，m为视角数量，n为每一个视角下的数据点数量，因此上述方差是优化各个相机的位姿τ^∧，令

式中，P_i′为数据点坐标，根据裂纹伯格-马夸尔特法：

J(τ^∧)^TJ(τ^∧)Δτ^∧＝-J(τ^∧)f(τ^∧)

Δτ^∧是待求的位姿，J(τ^∧)是梯度矩阵，将矩阵τ^∧写为向量形式τ，则

J(τ)^TJ(τ)Δτ＝-J(τ)f(τ)

其中

按照左扰动模型，利用链式法则：

则

则

将上述结果带入方程和J(τ)^TJ(τ)Δτ＝-J(τ)f(τ)，计算得到Δτ，更新τ：/

τ＝τ+Δτ

C)指数空间数据向旋转平移转化：

根据求解的指数空间得到θ和ρ，根据/将θ其变为/根据下面公式计算旋转矩阵

平移矩阵为T_c＝Jρ。