三维坐标转换方法研究
合集下载
相关主题
- 1、下载文档前请自行甄别文档内容的完整性，平台不提供额外的编辑、内容补充、找答案等附加服务。
- 2、"仅部分预览"的文档,不可在线预览部分如存在完整性等问题,可反馈申请退款(可完整预览的文档不适用该条件!)。
- 3、如文档侵犯您的权益，请联系客服反馈,我们会尽快为您处理(人工客服工作时间：9:00-18:30)。
Yq 2 - Yq 1 Zq2 - Zq1
a、 b、 c表示
[5 ]
= R Yp 2 - Yp 1
Zp2 - Z p1
结合式 ( 5 ) 、 式 ( 6 ) 及罗德里格矩阵性质 , 得 0 Z p 21 + Z q 21 Yp 21 + Yq 21
Z p 21 + Z q 21
a b = c
0 - X p 21 - X q 21
Research on the M ethods of Ca lcula ti n g 3D Coord i na te Tran sform tion Parameters
YANG Fan, L I Guangyun,WANG L i
摘要 : 从理论上对几种坐标转换方法作严密对比 ,研究各自的适用性及优缺点 。推导基于罗德里格矩阵 3 参数的空间直角坐标转
中心化后的总误差为 Δ =
∑‖p ′i i =1
R q′ i‖
2
( 4)
当 Δ最小时 , 就得到所求的旋转矩阵 R 和位移 矢量 T。 将式 ( 4 ) 作适当变换
n
该方程组中只有两个方程独立 , 要解出 3 个未 [6 ] 知数需要构建另外方程组 , 整理可得 0 Z p 21 + Z q 21 Yp 21 + Yq 21
2010 年 第 6 期 杨 凡 ,等 : 三维坐标转换方法研究
5
文章编号 : 0494 2 0911 ( 2010 ) 06 2 0005 2 03
中图分类号 : P282. 2 文献标识码 : B
三维坐标转换方法研究
杨 凡 ,李广云 ,王 力
(信息工程大学 测绘学院 ,河南 郑州 450052 )
换模型 、 其线性化误差方程及最小二乘迭代计算公式 。通过实测数据计算表明 , 几种方法在精度上相似 , 四元组法利用了旋转矩 阵的特征和所有可用的特征点 ,计算过程简单快速 ,具有较强的稳定性和实用性 ; SVD 法理论简单 , 算法容易实现 , 但不能保证得 到旋转矩阵 ; 基于罗德里格矩阵的最小二乘迭代法理论较为复杂 ,不易实现 ,但精度可靠 。
Z p 21 + Z q 21
0 - X p 21 - X q 21
Z p 31 + Z q 31
- X p 21 - X q 21 0
Yp 31 + Yq 31 a b = c
- Yp 21 - Yq 21 0
Z p 31 + Z q 31
Δ=
‖ = ∑‖pi ′- R qi ′
2
i =1 n
0 - X p 31 - X q 31
关键词 : 坐标转换 ; 四元组 ; SVD; 迭代法 ; 线性化 ; 罗德里格矩阵 ; 最小二乘法
一、 引 言
理论研究和实际计算中 , 常涉及两个不同空间 直角坐标系之间的转换问题 。空间直角坐标系的 转换在大地测量 、 工程测量 、 摄影测量以及三维激 [1] 光扫描和跟踪等领域当中扮演着重要的角色 。 在大地测量中 ,有 1954 北京坐标系 、 1980 西安坐标 系、 2000 国家大地坐标系以及各种世界坐标系之间 的转换 ; 工程测量中有勘察设计坐标系 、 施工坐标 系、 监测坐标系之间的转换 ; 摄影测量中 , 空间后方 交会 、 共线方程的建立都要用到空间直角坐标系的 转换 ; 三维激光扫描和跟踪领域 , 不同测站之间的 点云拼接需要坐标转换 。因此 , 研究空间直角坐标 系的转换方法具有很强的理论和实际意义 。 在三维直角坐标转换中 , 常采用七参数 B ursa 2 [2] W lof模型 、 Molodensky模型和武测模型 。当两坐 标系统下有 3 个公共点时 , 就可唯一解算出 7 个转 换参数 ; 多于 3 个公共点时 ,就要进行平差计算 。在 平差计算过程中 ,转换参数初值 (特别是旋转角 ) 的 大小 ,直接影响平差系统的稳定性 、 精确性和计算 速度 ,精度差的初值可能使得解算的结果严重偏离 真值 。本文基于共同长度基准 , 不考虑尺度因子 , [3] 讨论几种比较成熟的参数估计方法 : 四元组法 、 奇异值分解法 ( SVD ) 、 迭代法 , 它们都是在最小平 方距离的目标函数下得到转换参数的最优解 。
( - 2 ab - 2 c) Yp i + ( - 2 b + 2 ac) Z p i ] + T1
+ T2
T3 Xq =MΔx + N ( 7)
N2 =
1 2 2 [ ( 2 c - 2 ab ) X p i + 1 +a +b +c
2
( 1 - a + b - c ) Yp i + ( - 2 a - 2 bc) Z p i ] + T2 N3 =
( 2 a - 2 bc) Yp i + ( 1 - a - b + c ) Z p i ] + T3
2 2 2
[8 ]
通过公式 Δx = (M M ) (M N ) 迭代计算可 求出改变量的最终值 , 最后求出矩阵 R。
T
- 1
T
五、 算 例
将 5 组实测点的三维坐标数据代入计算 , 可以 得到基 于 各 种 算 法 的 坐 标 转 换 参 数 R、 T。结 合 式 ( 1 ) 将点集 1 转换得到 P ′ = R P + T, 表 1 列出了 基于各算法的每个点的转换误差 。
Δ
解出 Q 的最大特征值 , 其对应特征向量就是四 3 3 元组 q 。将 q 代入式 ( 2 ) 即可求得旋转矩阵 R , 再 结合式 ( 1 ) 可得到位移矢量 T。 三、 奇异值分解法 点集 P、 Q 的质心分别以 p、 q 表示 , 则中心化后 的点集为 p′ i = pi - p
q′ i = qi - q 考虑到噪声干扰和获取数据的不准确性 , 可以 [3 ] 采用减少误差函数的方法 。L in 等 提出了一种基 于最小平方误差函数最小化的方法 。如果以 R 和 T 表示坐标转换参数 , 对于点集 P、 Q 中的任意点 pi , 经变换后为 R pi + T, 它与对应点 qi 的误差为 ‖qi ( R pi + T ) ‖ , 因此转换总误差为
6
测 绘 通 报 2010 年 第 6期 Σ = 1
n
n ∑
i =1
( pi - p ) ( qi - q )
T
T 设反对称矩阵 A =Σ - Σ , 列矢 Δ = [ A23 A31
A 12 ] , Q 为对称矩阵
Q =
T
trace (Σ)
ΔT Σ +ΣT - trace (Σ) I3
[5]
和 Faugeras等人研究
了基于单位四元组的参数估计方法 , 其算法最终化 为一个二次型最小化时求解最大特征值及对应特 征向量的迭代算法 。 旋转矩阵 R 可由单位四元组元素表示为
q0 + qx - qy - qz R=
2 2 2 2
2 ( qx qy - q0 qz )
q - qx + qy - qz
T
∑( p ′p ′+ q ′q ′T
i i =1
) 2 pi ′ R qi ′
T
因此 , 求 Δm in等价于求 Δ′ max
n n
Δ′ =
) Rq ′ = Trace ( R ∑p ′ q′ ∑p ′
T T
i i i i i =1 i =1 n
= Trace ( RH )
其中 , H =
q′ 。 ∑p ′
n
Βιβλιοθήκη Baidu
。设反对称矩阵 S 为 0 - c - b ( 5) S = c 0 - a b a 0 其中 , a、 b、 c 相 互独 立 。 R 可由 S 构成 罗 德 里 格 矩阵 - 1 ( 6) R = ( I +S) ( I - S) 其中 , I是 3 阶单位矩阵 。 任意两 组 点 分 别 按 照 式 ( 1 ) 构 建 方 程 组 , 整 理得 X q2 - X q1 X p2 - X p1
- X p 31 - X q 31 0
∑( p ′i i =1 n
) ( pi ′ ) = R qi ′ - R qi ′
T
i i i
T
- Yp 31 - Yq 31 - X q 21 + X p 21 - Yq 21 + Yp 21 - Z q 21 + Z p 21 - X q 31 + X p 31 - Yq 31 + Yp 31 - Z q 31 + Z p 31
2 2 2
7
T
- 2 ab - 2 c 1 - a +b - c 2 a - 2 bc
2 2 2
- 2 b + 2 ac - 2 a - 2 bc 1 - a - b +c
2 2 2
N = [N 1 N 2 N 3 ]
・
N1 =
1 2 2 2 2 2 2 [ ( 1 + a - b - c ) Xp i + 1 +a +b +c
Xq i
四、 最小二乘法
旋转矩阵 R 可由罗德里格矩阵 3 个独立参数
Yq i =
Zq i
1 2 2 ・ 1 +a +b +c
2
2010 年 第 6 期 杨 凡 ,等 : 三维坐标转换方法研究 1 +a - b - c 2 c - 2 ab 2 ac + 2 b
Xp i Yp i Zp i T1
2
2
2
将上式线性化得 其中 T Xq = [ X q i Yq i Z q i ] Δx = [Δa Δb Δc ΔT1 ΔT2 ΔT3 ]T 5X q i 5X q i 5X q i 5X q i 5X q i 5X q i 5a 5b 5c 5T1 5T2 5T3
M =
1 2 2 2 [ ( 2 ac + 2 b ) X p i + 1 +a +b +c
2 0 2 2 2
2 ( qx qz + q0 qy ) 2 ( qz qy - q0 qx )
q0 - qx - qy + qz
2 2 2 2
2 ( qx qy + q0 qz ) 2 ( qx qz - q0 qy )
3
2 ( qz qy + q0 qx )
T
( 2)
列向量 q = [ q0 qx qy qz ] 为单位四元组 , 满足 条件 q0 + qx + qy + qz = 1, q0 ≥0。 点集 P、 Q 的质心分别表示为 p、 q
- X p 21 - X q 21 0
- Yp 21 - Yq 21 - X q 21 + X p 21 - Yq 21 + Yp 21 - Z q 21 + Z p 21
2
Δ =
∑‖q
i =1 n
i
- ( R pi + T ) ‖
2
( 3)
其中 , X q21 = X q 2 - X q1 ; Yq 21 = Yq2 - Yq1 ; Z q21 = Z q2 Z q 1 ; X p 21 = X p 2 - X p 1 ; Yp 21 = Yp 2 - Yp 1 ; Z p 21 = Z p 2 - Z p 1 。
p = q =
2 2 2 2
1
n
n
∑p
i =1 n
i
二、 四元组法
一对点 pi 和 qi 的坐标关系可由旋转矩阵 R 和
1
n
∑q
i =1
i
由此得到点集 P、 Q 的交叉共生矩阵 Σ
收稿日期 : 2009 2 11 2 09 作者简介 : 杨 凡 ( 1986 —) ,男 ,四川眉山人 ,硕士生 ,主要研究方向为工业测量系统与测量机器人 。
T
i i i =1
计算出 H 的 SVD 矩阵 U 和 V , 当 R = VU 时 , Trace ( RH ) 达到最大值 。得到 R 后 , 再根据式 ( 1 ) 计算位移矢量 T。
求解 a、 b、 c, 结合式 ( 5 ) 、 式 ( 6 ) 得到旋转矩阵 R 的 初值 R0 。将 R0 代入式 ( 1 ) 解得位移矢量初值 T0 。 由式 ( 1 ) 、 式 ( 5) 、 式 ( 6 ) 可得
位移矢量 T 表示为
[4 ]
Xq Yq Zq
i
Xp
i
i
= R Yp
Zp
i
+T
( 1)
i
i
有很多用少量参数就可以描述 R 的运动参数 估计算法 , 这些参数有旋转轴夹角 、 反对称阵 、 欧拉 角以及单位四元组等 。其中单位四元组表示法具 有重要的地位 , 因为其描述 R 时不会有其他方法存 在的奇异点现象 , 而且单位四元组表示的旋转矩阵 运算比较简捷 。 Horn 等人
5Yq i 5a 5Z q i 5a
5Yq i 5b 5Z q i 5b
5Yq i 5c 5Z q i 5c
5Yq i 5T1 5Z q i 5T1
5Yq i 5T2 5Z q i 5T2
a、 b、 c表示
[5 ]
= R Yp 2 - Yp 1
Zp2 - Z p1
结合式 ( 5 ) 、 式 ( 6 ) 及罗德里格矩阵性质 , 得 0 Z p 21 + Z q 21 Yp 21 + Yq 21
Z p 21 + Z q 21
a b = c
0 - X p 21 - X q 21
Research on the M ethods of Ca lcula ti n g 3D Coord i na te Tran sform tion Parameters
YANG Fan, L I Guangyun,WANG L i
摘要 : 从理论上对几种坐标转换方法作严密对比 ,研究各自的适用性及优缺点 。推导基于罗德里格矩阵 3 参数的空间直角坐标转
中心化后的总误差为 Δ =
∑‖p ′i i =1
R q′ i‖
2
( 4)
当 Δ最小时 , 就得到所求的旋转矩阵 R 和位移 矢量 T。 将式 ( 4 ) 作适当变换
n
该方程组中只有两个方程独立 , 要解出 3 个未 [6 ] 知数需要构建另外方程组 , 整理可得 0 Z p 21 + Z q 21 Yp 21 + Yq 21
2010 年 第 6 期 杨 凡 ,等 : 三维坐标转换方法研究
5
文章编号 : 0494 2 0911 ( 2010 ) 06 2 0005 2 03
中图分类号 : P282. 2 文献标识码 : B
三维坐标转换方法研究
杨 凡 ,李广云 ,王 力
(信息工程大学 测绘学院 ,河南 郑州 450052 )
换模型 、 其线性化误差方程及最小二乘迭代计算公式 。通过实测数据计算表明 , 几种方法在精度上相似 , 四元组法利用了旋转矩 阵的特征和所有可用的特征点 ,计算过程简单快速 ,具有较强的稳定性和实用性 ; SVD 法理论简单 , 算法容易实现 , 但不能保证得 到旋转矩阵 ; 基于罗德里格矩阵的最小二乘迭代法理论较为复杂 ,不易实现 ,但精度可靠 。
Z p 21 + Z q 21
0 - X p 21 - X q 21
Z p 31 + Z q 31
- X p 21 - X q 21 0
Yp 31 + Yq 31 a b = c
- Yp 21 - Yq 21 0
Z p 31 + Z q 31
Δ=
‖ = ∑‖pi ′- R qi ′
2
i =1 n
0 - X p 31 - X q 31
关键词 : 坐标转换 ; 四元组 ; SVD; 迭代法 ; 线性化 ; 罗德里格矩阵 ; 最小二乘法
一、 引 言
理论研究和实际计算中 , 常涉及两个不同空间 直角坐标系之间的转换问题 。空间直角坐标系的 转换在大地测量 、 工程测量 、 摄影测量以及三维激 [1] 光扫描和跟踪等领域当中扮演着重要的角色 。 在大地测量中 ,有 1954 北京坐标系 、 1980 西安坐标 系、 2000 国家大地坐标系以及各种世界坐标系之间 的转换 ; 工程测量中有勘察设计坐标系 、 施工坐标 系、 监测坐标系之间的转换 ; 摄影测量中 , 空间后方 交会 、 共线方程的建立都要用到空间直角坐标系的 转换 ; 三维激光扫描和跟踪领域 , 不同测站之间的 点云拼接需要坐标转换 。因此 , 研究空间直角坐标 系的转换方法具有很强的理论和实际意义 。 在三维直角坐标转换中 , 常采用七参数 B ursa 2 [2] W lof模型 、 Molodensky模型和武测模型 。当两坐 标系统下有 3 个公共点时 , 就可唯一解算出 7 个转 换参数 ; 多于 3 个公共点时 ,就要进行平差计算 。在 平差计算过程中 ,转换参数初值 (特别是旋转角 ) 的 大小 ,直接影响平差系统的稳定性 、 精确性和计算 速度 ,精度差的初值可能使得解算的结果严重偏离 真值 。本文基于共同长度基准 , 不考虑尺度因子 , [3] 讨论几种比较成熟的参数估计方法 : 四元组法 、 奇异值分解法 ( SVD ) 、 迭代法 , 它们都是在最小平 方距离的目标函数下得到转换参数的最优解 。
( - 2 ab - 2 c) Yp i + ( - 2 b + 2 ac) Z p i ] + T1
+ T2
T3 Xq =MΔx + N ( 7)
N2 =
1 2 2 [ ( 2 c - 2 ab ) X p i + 1 +a +b +c
2
( 1 - a + b - c ) Yp i + ( - 2 a - 2 bc) Z p i ] + T2 N3 =
( 2 a - 2 bc) Yp i + ( 1 - a - b + c ) Z p i ] + T3
2 2 2
[8 ]
通过公式 Δx = (M M ) (M N ) 迭代计算可 求出改变量的最终值 , 最后求出矩阵 R。
T
- 1
T
五、 算 例
将 5 组实测点的三维坐标数据代入计算 , 可以 得到基 于 各 种 算 法 的 坐 标 转 换 参 数 R、 T。结 合 式 ( 1 ) 将点集 1 转换得到 P ′ = R P + T, 表 1 列出了 基于各算法的每个点的转换误差 。
Δ
解出 Q 的最大特征值 , 其对应特征向量就是四 3 3 元组 q 。将 q 代入式 ( 2 ) 即可求得旋转矩阵 R , 再 结合式 ( 1 ) 可得到位移矢量 T。 三、 奇异值分解法 点集 P、 Q 的质心分别以 p、 q 表示 , 则中心化后 的点集为 p′ i = pi - p
q′ i = qi - q 考虑到噪声干扰和获取数据的不准确性 , 可以 [3 ] 采用减少误差函数的方法 。L in 等 提出了一种基 于最小平方误差函数最小化的方法 。如果以 R 和 T 表示坐标转换参数 , 对于点集 P、 Q 中的任意点 pi , 经变换后为 R pi + T, 它与对应点 qi 的误差为 ‖qi ( R pi + T ) ‖ , 因此转换总误差为
6
测 绘 通 报 2010 年 第 6期 Σ = 1
n
n ∑
i =1
( pi - p ) ( qi - q )
T
T 设反对称矩阵 A =Σ - Σ , 列矢 Δ = [ A23 A31
A 12 ] , Q 为对称矩阵
Q =
T
trace (Σ)
ΔT Σ +ΣT - trace (Σ) I3
[5]
和 Faugeras等人研究
了基于单位四元组的参数估计方法 , 其算法最终化 为一个二次型最小化时求解最大特征值及对应特 征向量的迭代算法 。 旋转矩阵 R 可由单位四元组元素表示为
q0 + qx - qy - qz R=
2 2 2 2
2 ( qx qy - q0 qz )
q - qx + qy - qz
T
∑( p ′p ′+ q ′q ′T
i i =1
) 2 pi ′ R qi ′
T
因此 , 求 Δm in等价于求 Δ′ max
n n
Δ′ =
) Rq ′ = Trace ( R ∑p ′ q′ ∑p ′
T T
i i i i i =1 i =1 n
= Trace ( RH )
其中 , H =
q′ 。 ∑p ′
n
Βιβλιοθήκη Baidu
。设反对称矩阵 S 为 0 - c - b ( 5) S = c 0 - a b a 0 其中 , a、 b、 c 相 互独 立 。 R 可由 S 构成 罗 德 里 格 矩阵 - 1 ( 6) R = ( I +S) ( I - S) 其中 , I是 3 阶单位矩阵 。 任意两 组 点 分 别 按 照 式 ( 1 ) 构 建 方 程 组 , 整 理得 X q2 - X q1 X p2 - X p1
- X p 31 - X q 31 0
∑( p ′i i =1 n
) ( pi ′ ) = R qi ′ - R qi ′
T
i i i
T
- Yp 31 - Yq 31 - X q 21 + X p 21 - Yq 21 + Yp 21 - Z q 21 + Z p 21 - X q 31 + X p 31 - Yq 31 + Yp 31 - Z q 31 + Z p 31
2 2 2
7
T
- 2 ab - 2 c 1 - a +b - c 2 a - 2 bc
2 2 2
- 2 b + 2 ac - 2 a - 2 bc 1 - a - b +c
2 2 2
N = [N 1 N 2 N 3 ]
・
N1 =
1 2 2 2 2 2 2 [ ( 1 + a - b - c ) Xp i + 1 +a +b +c
Xq i
四、 最小二乘法
旋转矩阵 R 可由罗德里格矩阵 3 个独立参数
Yq i =
Zq i
1 2 2 ・ 1 +a +b +c
2
2010 年 第 6 期 杨 凡 ,等 : 三维坐标转换方法研究 1 +a - b - c 2 c - 2 ab 2 ac + 2 b
Xp i Yp i Zp i T1
2
2
2
将上式线性化得 其中 T Xq = [ X q i Yq i Z q i ] Δx = [Δa Δb Δc ΔT1 ΔT2 ΔT3 ]T 5X q i 5X q i 5X q i 5X q i 5X q i 5X q i 5a 5b 5c 5T1 5T2 5T3
M =
1 2 2 2 [ ( 2 ac + 2 b ) X p i + 1 +a +b +c
2 0 2 2 2
2 ( qx qz + q0 qy ) 2 ( qz qy - q0 qx )
q0 - qx - qy + qz
2 2 2 2
2 ( qx qy + q0 qz ) 2 ( qx qz - q0 qy )
3
2 ( qz qy + q0 qx )
T
( 2)
列向量 q = [ q0 qx qy qz ] 为单位四元组 , 满足 条件 q0 + qx + qy + qz = 1, q0 ≥0。 点集 P、 Q 的质心分别表示为 p、 q
- X p 21 - X q 21 0
- Yp 21 - Yq 21 - X q 21 + X p 21 - Yq 21 + Yp 21 - Z q 21 + Z p 21
2
Δ =
∑‖q
i =1 n
i
- ( R pi + T ) ‖
2
( 3)
其中 , X q21 = X q 2 - X q1 ; Yq 21 = Yq2 - Yq1 ; Z q21 = Z q2 Z q 1 ; X p 21 = X p 2 - X p 1 ; Yp 21 = Yp 2 - Yp 1 ; Z p 21 = Z p 2 - Z p 1 。
p = q =
2 2 2 2
1
n
n
∑p
i =1 n
i
二、 四元组法
一对点 pi 和 qi 的坐标关系可由旋转矩阵 R 和
1
n
∑q
i =1
i
由此得到点集 P、 Q 的交叉共生矩阵 Σ
收稿日期 : 2009 2 11 2 09 作者简介 : 杨 凡 ( 1986 —) ,男 ,四川眉山人 ,硕士生 ,主要研究方向为工业测量系统与测量机器人 。
T
i i i =1
计算出 H 的 SVD 矩阵 U 和 V , 当 R = VU 时 , Trace ( RH ) 达到最大值 。得到 R 后 , 再根据式 ( 1 ) 计算位移矢量 T。
求解 a、 b、 c, 结合式 ( 5 ) 、 式 ( 6 ) 得到旋转矩阵 R 的 初值 R0 。将 R0 代入式 ( 1 ) 解得位移矢量初值 T0 。 由式 ( 1 ) 、 式 ( 5) 、 式 ( 6 ) 可得
位移矢量 T 表示为
[4 ]
Xq Yq Zq
i
Xp
i
i
= R Yp
Zp
i
+T
( 1)
i
i
有很多用少量参数就可以描述 R 的运动参数 估计算法 , 这些参数有旋转轴夹角 、 反对称阵 、 欧拉 角以及单位四元组等 。其中单位四元组表示法具 有重要的地位 , 因为其描述 R 时不会有其他方法存 在的奇异点现象 , 而且单位四元组表示的旋转矩阵 运算比较简捷 。 Horn 等人
5Yq i 5a 5Z q i 5a
5Yq i 5b 5Z q i 5b
5Yq i 5c 5Z q i 5c
5Yq i 5T1 5Z q i 5T1
5Yq i 5T2 5Z q i 5T2