根据经纬度来计算旋转矩阵

在C.M.Gosslin的《Development and Experimentation of a Fast 3-DOF Camera-Orienting Device》一文中，作者给出了通过相机视角的经度、纬度、扭转来得到相机相对于固定坐标系的旋转矩阵Q0\mathbf{Q_0}Q0的表达式如下:

利用三角函数恒等式sin⁡2ν+cos⁡2ν=1\sin^2\nu+\cos^2\nu=1sin2ν+cos2ν=1这个恒等式，该式等价于以下表达式：
Q0=[(cos⁡(ν)−1)cos⁡2(μ)+1(cos⁡(ν)−1)sin⁡(μ)cos⁡(μ)sin⁡(ν)cos⁡(μ)(cos⁡(ν)−1)sin⁡(μ)cos⁡(μ)(cos⁡(ν)−1)sin⁡2(μ)+1sin⁡(μ)sin⁡(ν)−sin⁡(ν)cos⁡(μ)−sin⁡(μ)sin⁡(ν)cos⁡(ν)]\mathbf{Q_0}=\left[\begin{matrix} \left( \cos{\left(\nu \right)} - 1 \right) \cos^{2}{\left(\mu \right)} + 1 & \left(\cos{\left(\nu \right)} - 1\right) \sin{\left(\mu \right)} \cos{\left(\mu \right)} & \sin{\left(\nu \right)} \cos{\left(\mu \right)}\\\left(\cos{\left(\nu \right)} - 1\right) \sin{\left(\mu \right)} \cos{\left(\mu \right)} & \left(\cos{\left(\nu \right)} - 1\right) \sin^{2}{\left(\mu \right)} + 1 & \sin{\left(\mu \right)} \sin{\left(\nu \right)}\\- \sin{\left(\nu \right)} \cos{\left(\mu \right)} & - \sin{\left(\mu \right)} \sin{\left(\nu \right)} & \cos{\left(\nu \right)}\end{matrix}\right]Q0=⎣⎡(cos(ν)−1)cos2(μ)+1(cos(ν)−1)sin(μ)cos(μ)−sin(ν)cos(μ)(cos(ν)−1)sin(μ)cos(μ)(cos(ν)−1)sin2(μ)+1−sin(μ)sin(ν)sin(ν)cos(μ)sin(μ)sin(ν)cos(ν)⎦⎤

然而作者并没有给出这个旋转矩阵的来历，经过分析之后，我根据罗德里格斯公式（参见罗德里格斯的代码详解一文）推出了其中μ\muμ和ν\nuν的意义，也就是文中提到的经度longitude和纬度latitude的意义，可以通过以下简图来进行表达：

也就是在本式中，经度角μ\muμ是以y轴基准以z轴按照右手法则计算的，而纬度角ν\nuν是以z轴为基准的。对于Q0Q_0Q0矩阵，先绕着Z轴旋转了π/2+μ\pi/2+\muπ/2+μ角度，然后再绕新的X’轴旋转了ν\nuν角度，如果按照轴角的观点，这个变换相当于绕轴[cos⁡(π/2+μ),sin⁡(π/2+μ),0][\cos(\pi/2+\mu),\sin(\pi/2+\mu),0][cos(π/2+μ),sin(π/2+μ),0]（即[−sin⁡μ,cos⁡μ,0][-\sin\mu,\cos\mu,0][−sinμ,cosμ,0]），旋转了ν\nuν角度，根据罗德里格斯公式R=eθAs=I+sinθAs+(1−cosθ)As2\mathbf R=e^{\theta \mathbf As} = \mathbf I + sin\theta \mathbf A_s + (1-cos\theta)\mathbf A_s^2R=eθAs=I+sinθAs+(1−cosθ)As2我们就可以计算其旋转矩阵，具体的代码如下所示：

import numpy as np
import sympy as sym
from sympy import sin,cos,pi
I = sym.Matrix([[1,0,0],[0,1,0],[0,0,1]
])mu,nu = sym.symbols("mu,nu")
s = [-sin(mu),cos(mu),0]
As = sym.Matrix([[0,-s[2],s[1]],[s[2],0,-s[0]],[-s[1],s[0],0]
])
As2 = As*As
print(sym.latex(sym.simplify(As2)))
#利用轴角公式计算旋转矩阵，其中轴为(-sin(mu),cos(mu),0),转角为nu
R = I + sin(nu)*As + (1-cos(nu))*As2
print(sym.latex(sym.simplify(R)))

我们把输出的结果放到markdown格式的单元格中运行（注意记得加上美元符号标识符），就可以得到开篇的结果，即：
[(cos⁡(ν)−1)cos⁡2(μ)+1(cos⁡(ν)−1)sin⁡(μ)cos⁡(μ)sin⁡(ν)cos⁡(μ)(cos⁡(ν)−1)sin⁡(μ)cos⁡(μ)(cos⁡(ν)−1)sin⁡2(μ)+1sin⁡(μ)sin⁡(ν)−sin⁡(ν)cos⁡(μ)−sin⁡(μ)sin⁡(ν)cos⁡(ν)]\left[\begin{matrix}\left(\cos{\left(\nu \right)} - 1\right) \cos^{2}{\left(\mu \right)} + 1 & \left(\cos{\left(\nu \right)} - 1\right) \sin{\left(\mu \right)} \cos{\left(\mu \right)} & \sin{\left(\nu \right)} \cos{\left(\mu \right)}\\\left(\cos{\left(\nu \right)} - 1\right) \sin{\left(\mu \right)} \cos{\left(\mu \right)} & \left(\cos{\left(\nu \right)} - 1\right) \sin^{2}{\left(\mu \right)} + 1 & \sin{\left(\mu \right)} \sin{\left(\nu \right)}\\- \sin{\left(\nu \right)} \cos{\left(\mu \right)} & - \sin{\left(\mu \right)} \sin{\left(\nu \right)} & \cos{\left(\nu \right)}\end{matrix}\right]⎣⎡(cos(ν)−1)cos2(μ)+1(cos(ν)−1)sin(μ)cos(μ)−sin(ν)cos(μ)(cos(ν)−1)sin(μ)cos(μ)(cos(ν)−1)sin2(μ)+1−sin(μ)sin(ν)sin(ν)cos(μ)sin(μ)sin(ν)cos(ν)⎦⎤

参考文献：
[1] Gosselin C M , Éric St.-Pierre. Development and Experimentation of a Fast 3-DOF Camera-Orienting Device[J]. The International Journal of Robotics Research, 1997, 16(5):619-630.

根据经纬度来计算旋转矩阵相关推荐

给你两个经纬度，计算他们之间的距离
这些经纬线是怎样定出来的呢?地球是在不停地绕地轴旋转(地轴是一根通过地球南北两极和地球中心的假想线),在地球中腰画一个与地轴垂直的大圆圈,使圈上的每一点都和南北两极的距离相等,这个圆圈就叫作" ...
依据地图上的经纬度坐标计算某个点到多边形各边的距离
http://www.th2w.com/article/85 依据地图上的经纬度坐标计算某个点到多边形各边的距离最近公司有一个需求:依据地图上的经纬度坐标计算某个点到多边形各边的距离. 主要原理: ...
matlab如何测两点的角度_根据2点经纬度，计算方位角，以及计算2条线的夹角
以真北为0度起点,由东向南向西顺时针旋转360度,主要是用于控制象限. 根据2点经纬度,计算方位角 [csharp] /// /// 给定2点,获得经纬度 /// /// 起点经纬度,都是以度为单位 ...
android地图方位角_Android获取经纬度、计算距离、方位角
最近在Android上做GPS的东西,获取经纬度.计算距离.方位角,感觉在搞GIS样.当然,玩笑了,玩玩而已,稍微总结一下. 经度指示南北方向,纵向. 纬度指示东西方向,横向. 获取经纬度使用GPS ...
根据2点经纬度，计算方位角，以及计算2条线的夹角
项目场景1:根据2点经纬度,计算方位角以真北为0度起点,由北向东向南向西顺时针旋转360度,主要是用于控制象限. 提示:这里简述项目相关背景: [csharp] <summary> // ...
java关于地图经纬度的计算
一,坐标体系 wgs坐标系是国际上通用的坐标系,也称地球坐标系,gps和北斗系统都使用的是wgs坐标系.谷歌地图使用的是wgs坐标系(中国部分除外),openstreetmap使用的也是这种坐标系 g ...
已知点的经纬度坐标计算/读取DEM高程信息/海拔高度
已知点的经纬度坐标计算/读取DEM高程信息/海拔高度思路环境依赖安装GDAL 安装依赖包测试GDAL 数据依赖 python代码参考思路实现条件 1.GDAL库 2.DEM数据开发语 ...
根据经纬度坐标计算实际距离
double hypot( double x, double y) ... { return sqrt(x * x + y * y); } double distance( d ...
根据两个经纬度点计算距离
根据两个经纬度点计算距离假设要算的距离为A.B两点之间的距离,A.B两点的经线相交于南北极,纬线平行,找出C点和B点同一条纬线和A点同一条经线,同理找出D点.这时如果想要知道A和B距离只要知道角AO ...
python使用geopy包提取城市经纬度及计算距离矩阵
在做物流调度问题时,我们想要知道供应商发货城市的地理坐标及距离矩阵,为路径推荐算法模型做数据准备工作,这里记录一下怎么使用geopy包提取城市经纬度及计算距离矩阵. 一.读取城市名称首先使用pand ...

根据经纬度来计算旋转矩阵

根据经纬度来计算旋转矩阵相关推荐

最新文章

热门文章