|
# -*- coding: utf-8 -*-
"""
coordinateframes
================
"""
import numpy as np
import numpy.linalg as la
import cProfile as profile
class _Mnumpy(object):
def Matrix(self, data):
return np.array(data)
def add(self, a, b):
return np.array(a) + np.array(b)
def sub(self, a, b):
return np.array(a) - np.array(b)
def mmul(self, a, b):
return np.dot(a,b)
def inv(self, a):
return la.inv(a)
def simplify(self, a):
return a
def isMatrix(self, a):
try:
return a.shape == (3,3)
except:
return False
def atan2(self, a, b):
return np.arctan2(a, b)
@property
def pi(self):
return np.pi
def __getattr__(self, name):
return getattr(np, name)
M = _Mnumpy()
class NotFound(Exception):
pass
def Rx(alpha):
return M.Matrix([[1, 0, 0],
[0, M.cos(alpha), M.sin(alpha)],
[0, -M.sin(alpha), M.cos(alpha)]])
def Ry(alpha):
return M.Matrix([[ M.cos(alpha), 0, -M.sin(alpha)],
[ 0, 1, 0],
[ M.sin(alpha), 0, M.cos(alpha)]])
def Rz(alpha):
return M.Matrix([[ M.cos(alpha), M.sin(alpha), 0],
[-M.sin(alpha), M.cos(alpha), 0],
[ 0, 0, 1]])
ned2enu = M.Matrix([[ 0, 1, 0],
[ 1, 0, 0],
[ 0, 0,-1]])
class Coordinate(object):
def __init__(self, coordinate, referenceFrame, isNatural=True):
self.referenceFrame = referenceFrame
62 ↛ 66line 62 didn't jump to line 66, because the condition on line 62 was never false if isNatural:
self._natural = np.array(coordinate)
self._cartesian = None
else:
self._natural = None
self._cartesian = np.array(coordinate)
@property
def natural(self):
70 ↛ 71line 70 didn't jump to line 71, because the condition on line 70 was never true if self._natural is None:
self._natural = np.array(self.referenceFrame.toNatural(*self._cartesian))
return self._natural
@property
def cartesian(self):
if self._cartesian is None:
self._cartesian = np.array(self.referenceFrame.toCartesian(*self._natural))
return self._cartesian
class Rotation(object):
def __init__(self):
self.R = None
@classmethod
def fromRPY(cls, roll, pitch, yaw):
new = cls()
new.rpy = [roll, pitch, yaw]
new.R = reduce(M.mmul, (Rx(roll),
Ry(pitch),
Rz(yaw)))
return new
@classmethod
def fromMatrix(cls, R):
new = cls()
new.R = R
return new
class CoordinateUniverse(object):
"""
A set of coordinate frames is called a :py:class:`CoordinateUniverse`.
"""
def __init__(self):
self._frames = {}
self._transformations = {}
def getCoordinateFrame(self, name, cls=None):
"""
Get the coordinate frame to a given name.
If the given coordinate frame does not exist, create it.
:param name: Name of the coordinate frame
:param cls: If set, use ``cls`` as baseclass for the frame. Otherwise :py:class:`CoordinateFrame` is used.
"""
try:
frame = self._frames[name]
114 ↛ 115line 114 didn't jump to line 115, because the condition on line 114 was never true if cls is not None:
assert type(frame) == cls
return frame
except KeyError:
if cls is None:
cls = CoordinateFrame
frame = cls(self, name)
self._frames[name] = frame
return frame
def getTransformation(self, source, target):
"""
Get the transformation from one frame into another.
:param source: Source coordinate frame (can be name or frame object)
:param target: Target coordinate frame (can be name or frame object)
"""
130 ↛ 132line 130 didn't jump to line 132, because the condition on line 130 was never false if isinstance(source, (str, unicode)):
source = self.getCoordinateFrame(source)
132 ↛ 134line 132 didn't jump to line 134, because the condition on line 132 was never false if isinstance(target, (str, unicode)):
target = self.getCoordinateFrame(target)
try:
return self._transformations[source.name, target.name]
except KeyError:
transformation = Transformation(source, target)
self._transformations[source.name, target.name] = transformation
return transformation
def coordLib(self, spec, name):
"""
Get a coordinate frame from the coordinate library.
:param spec: Specification of the coordinate system.
:param name: Name of the newly created coordinate frame.
Currently the following specifications are implemented:
* GRS-80
* WGS-84
"""
csdef = _coordLib[spec]
cf = self.getCoordinateFrame(name, csdef["class"])
cf.spec = spec
for k,v in csdef.items():
if k != "class":
setattr(cf, k, v)
return cf
universe = CoordinateUniverse()
class Transformation(object):
"""
Represents a coordinate transformation from one frame into another.
:param sourceFrame: Source coordinate frame
:param targetFrame: Target coordinate frame
If the position or orientation of any relavant coordinate frame is changed,
the transformation will automatically be updated the next time it is used.
.. note::
The ``universe`` of both coordinate frames must be the same.
"""
def __init__(self, sourceFrame, targetFrame):
assert sourceFrame.universe == targetFrame.universe
self.universe = sourceFrame.universe
self.sourceFrame = sourceFrame
self.targetFrame = targetFrame
self._R = None
self._t = None
self.path_rev = None
def _updateParameters(self):
"""
update internal transformation representation
:none: will be skipped if all participating coordinate systems did not change
"""
190 ↛ 191line 190 didn't jump to line 191, because the condition on line 190 was never true if self.sourceFrame == self.targetFrame:
self._R = M.eye(3)
self._t = M.zeros(3)
else:
path = self.sourceFrame._traceTo(self.targetFrame)
frames = (self.sourceFrame,) + tuple(frame for _, frame in path)
directions = tuple(direction for direction,_ in path)
path_rev = tuple(f.rev for f in frames)
if self.path_rev != path_rev:
rot_acc = M.eye(3)
trans_acc = M.zeros(3)
for frame_a, direction, frame_b in zip(frames[:-1], directions, frames[1:]):
if direction == 'down':
top_frame = frame_a
sub_frame = frame_b
else:
top_frame = frame_b
sub_frame = frame_a
rot = M.mmul(sub_frame.rotation.R,
top_frame.getLocalFrameRotation(sub_frame.position))
trans = sub_frame.position.cartesian
if direction == 'down':
trans = -M.mmul(rot, trans)
else:
rot = M.inv(rot)
rot_acc = M.mmul(rot, rot_acc)
trans_acc = M.mmul(rot, trans_acc) + trans
self._R = M.simplify(rot_acc)
self._t = M.simplify(trans_acc)
self.path_rev = path_rev
@property
def R(self):
"""
Current rotation matrix of the transformation
"""
self._updateParameters()
return self._R
@property
def t(self):
"""
Current translation vector of the transformation
"""
self._updateParameters()
return self._t
def applyDirection(self, direction):
"""
Apply transformation to a direction vector.
:param direction: ``numpy`` array of size ``3`` or ``Nx3`` representing one or many direction vectors in the source coordinate frame.
:returns: ``numpy`` array of the same shape as ``direction`` representing the same directions in the target coordinate frame.
"""
return M.dot(self.R, direction)
def applyPoint(self, point):
"""
Apply transformation to a point vector.
:param point: ``numpy`` array of size ``3`` or ``Nx3`` representing one or many point vectors in the source coordinate frame.
:returns: ``numpy`` array of the same shape as ``point`` representing the same points in the target coordinate frame.
"""
point = np.array(point, copy=False)
return M.dot(self.R, point) + self.t.reshape((3,) + (1,)*(len(point.shape)-1))
class CoordinateFrame(object):
"""
A coordinate frame.
:param universe: :py:class:`CoordinateUniverse` in which the frame should be registered.
:param name: Name of the frame.
"""
def __init__(self, universe, name):
self.rev = 0
self.universe = universe
self.name = name
self.spec = None
self.referenceSystem = None
self.referencedBy = []
self.localRotationConvention = 'RPY'
self.position = (0,0,0)
self.rotation = (0,0,0)
self.units = None
@property
def position(self):
"""
The origin of this frame in coordinates of the ``referenceSystem``.
"""
return self._position
@position.setter
def position(self, value):
280 ↛ 281line 280 didn't jump to line 281, because the condition on line 280 was never true if isinstance(value, Coordinate):
self._position = value
else:
self._position = Coordinate(value, self.referenceSystem, True)
self.rev += 1
@property
def rotation(self):
"""
The rotation of this coordinate frame
The rotation is defined such that a point in the referece system
**x_r** is related to a point in this system **x_s** as follows::
x_r = R^-1 x_s + t
Where **R** is ``rotation`` and ``t`` is position.
If the reference system provides any local rotation frame != 1,
**R** is defined as::
R = R_thisframe * R_locrot
"""
return self._rotation
@rotation.setter
def rotation(self, value):
305 ↛ 306line 305 didn't jump to line 306, because the condition on line 305 was never true if isinstance(value, Rotation):
self._rotation = value
elif M.isMatrix(value):
self._rotation = Rotation.fromMatrix(value)
309 ↛ 312line 309 didn't jump to line 312, because the condition on line 309 was never false elif self.localRotationConvention == 'RPY':
self._rotation = Rotation.fromRPY(*value)
else:
raise ValueError('unknown rotation type')
self.rev += 1
def referencedTo(self, other):
self.referenceSystem = other
self._position.referenceFrame = other
other.referencedBy.append(self)
self.rev += 1
other.rev += 1
return self
def getLocalFrameRotation(self, position):
return M.eye(3)
@property
def R(self):
return self.rotation.R
def toCartesian(self, x, y, z):
return [x, y, z]
def toNatural(self, x, y, z):
return [x, y, z]
def _traceTo(self, other, trail=None):
331 ↛ 332line 331 didn't jump to line 332, because the condition on line 331 was never true if isinstance(other, (str, unicode)):
other = self.universe.getCoordinateFrame(other)
if trail is None:
trail = [self]
else:
trail = trail + [self]
if self == other:
return []
revSystems = [('down', s) for s in self.referencedBy]
if self.referenceSystem is not None:
toCheck = [('up', self.referenceSystem)] + revSystems
else:
toCheck = revSystems
toCheck = filter(lambda x: x[1] not in trail, toCheck)
345 ↛ 350line 345 didn't jump to line 350, because the loop on line 345 didn't complete for dir, sys in toCheck:
try:
return [(dir, sys)] + sys._traceTo(other, trail)
except NotFound:
pass
raise NotFound('coordinate system "%s" not found' % other)
def __repr__(self):
return '<CoordinateFrame %s>'%self.name
def plot(self, ax, referenceFrame=None, size=1000000, **kwargs):
lines = [[(-size,0,0), (size,0,0)],
[(0,-size,0), (0,size,0)],
[(0,0,-size), (0,0,size)]]
if referenceFrame is None:
plotLines = lines
else:
plotLines = []
tr = self.universe.getTransformation(self, referenceFrame)
for line in lines:
l = []
for point in line:
l.append(tr.applyPoint(point))
plotLines.append(l)
for line in plotLines:
x,y,z = zip(*line)
ax.plot(x,y,z, **kwargs)
class OblateEllipsoidFrame(CoordinateFrame):
@property
def axes(self):
"""
length of axes (unit: meters)
:returns: (longer, shorter) axes == (xy axes, z axis)
"""
return self.a, self.b
@axes.setter
def axes(self, (a,b)):
self.a, self.b = (a,b)
self.excentricity2 = (a**2 - b**2) / a**2
self.excentricityt2 = (a**2 - b**2) / b**2
self.rev += 1
def _Nlat(self, slat):
return self.a / (1 - self.excentricity2*slat**2)**.5
def toCartesian(self, lat, lon, height):
# follows http://www.navipedia.net/index.php/Ellipsoidal_and_Cartesian_Coordinates_Conversion
rlat = lat*M.pi/180
rlon = lon*M.pi/180
slat = M.sin(rlat)
clat = M.cos(rlat)
slon = M.sin(rlon)
clon = M.cos(rlon)
Nlat = self._Nlat(slat)
xbase = (Nlat + height) * clat
x = xbase * clon
y = xbase * slon
z = (Nlat * (1 - self.excentricity2) + height) * slat
return [x, y, z]
def toNatural(self, x, y, z, iterations=3):
# follows http://www.navipedia.net/index.php/Ellipsoidal_and_Cartesian_Coordinates_Conversion
rlon = M.atan2(y, x)
xbase = (x**2 + y**2)**.5
eps2 = self.excentricity2
rlat = M.atan2(z, (1 + eps2) * xbase)
for i in xrange(iterations):
Nlat = self._Nlat(M.sin(rlat))
height = xbase / M.cos(rlat) - Nlat
rlat = M.atan2(z, (1 - eps2 * (Nlat / (Nlat + height))) * xbase)
return [180*rlat/M.pi, 180*rlon/M.pi, height]
def getLocalFrameRotation(self, position):
return reduce(M.mmul, (Ry(-M.deg2rad(position.natural[0])),
Rx(M.deg2rad(position.natural[1])),
Ry(-M.pi/2)))
def plot(self, ax, referenceFrame=None, **kwargs):
lat, lon = np.mgrid[-90:91:10, -180:181:10]
x,y,z = self.toCartesian(lat, lon, 0)
ax.plot_wireframe(x,y,z, **kwargs)
ax.plot([0,1.5*self.a],[0,0],[0,0], **kwargs)
class SphericalFrame(CoordinateFrame):
pass
_coordLib = {
"GRS-80": {
"class": OblateEllipsoidFrame,
"units": ("degree", "degree", "m"),
"axes": (6378137.0, 6356752.314140) # https://en.wikipedia.org/wiki/World_Geodetic_System
},
"WGS-84": {
"class": OblateEllipsoidFrame,
"units": ("degree", "degree", "m"),
"axes": (6378137.0, 6356752.314245) # https://en.wikipedia.org/wiki/World_Geodetic_System
}
}
|
