Source code for klampt.math.geodesic

"""Common interface for geodesic interpolation and distances on manifolds.

from . import vectorops, so2, so3, se3
import math

[docs]class GeodesicSpace: """Base class for geodesic spaces. A geodesic is equipped with a a geodesic (interpolation via the interpolate(a,b,u) method), a natural arc length distance metric (distance(a,b) method), an intrinsic dimension (intrinsicDimension() method), an extrinsic dimension (extrinsicDimension() method), and natural tangents (the difference and integrate methods)."""
[docs] def intrinsicDimension(self): raise NotImplementedError()
[docs] def extrinsicDimension(self): raise NotImplementedError()
[docs] def distance(self,a,b): return vectorops.distance(a,b)
[docs] def interpolate(self,a,b,u): return vectorops.interpolate(a,b,u)
[docs] def difference(self,a,b): """For Lie groups, returns a difference vector that, when integrated would get to a from b. In Cartesian spaces it is a-b. In other spaces, it should be d/du interpolate(b,a,u) at u=0.""" return vectorops.sub(a,b)
[docs] def integrate(self,x,d): """For Lie groups, returns the point that would be arrived at via integrating the difference vector d starting from x. Must satisfy the relationship a = integrate(b,difference(a,b)). In Cartesian spaces it is x+d""" return vectorops.add(x,d)
[docs]class CartesianSpace(GeodesicSpace): """The standard geodesic on R^d""" def __init__(self,d): self.d = d
[docs] def intrinsicDimension(self): return self.d
[docs] def extrinsicDimension(self): return self.d
[docs]class MultiGeodesicSpace(GeodesicSpace): """This forms the cartesian product of one or more GeodesicSpace's. Distances are simply added together.""" def __init__(self,*components): self.components = components self.componentWeights = [1]*len(self.components)
[docs] def intrinsicDimension(self): return sum(c.intrinsicDimension() for c in self.components)
[docs] def extrinsicDimension(self): return sum(c.extrinsicDimension() for c in self.components)
[docs] def split(self,x): i = 0 res = [] for c in self.components: d = c.extrinsicDimension() res.append(x[i:i+d]) i += d return res
[docs] def join(self,xs): return sum(xs,[])
[docs] def distance(self,a,b): i = 0 res = 0.0 for c,w in zip(self.components,self.componentWeights): d = c.extrinsicDimension() res += (c.distance(a[i:i+d],b[i:i+d])**2)*w i += d return math.sqrt(res)
[docs] def interpolate(self,a,b,u): i = 0 res = [0]*len(a) for c in self.components: d = c.extrinsicDimension() res[i:i+d] = c.interpolate(a[i:i+d],b[i:i+d],u) i += d return res
[docs] def difference(self,a,b): i = 0 res = [0]*len(a) for c in self.components: d = c.extrinsicDimension() res[i:i+d] = c.difference(a[i:i+d],b[i:i+d]) i += d return res
[docs] def integrate(self,x,diff): i = 0 res = [0]*len(x) for c in self.components: d = c.extrinsicDimension() res[i:i+d] = c.integrate(x[i:i+d],diff[i:i+d]) i += d return res
[docs]class SO2Space(GeodesicSpace): """The space of 2D rotations SO(2)."""
[docs] def intrinsicDimension(self): return 1
[docs] def extrinsicDimension(self): return 1
[docs] def distance(self,a,b): return abs(so2.diff(a[0],b[0]))
[docs] def interpolate(self,a,b,u): return [so2.interp(a[0],b[0],u)]
[docs] def difference(self,a,b): return [so2.diff(a[0],b[0])]
[docs] def integrate(self,x,d): return [so2.normalize(x[0]+d[0])]
[docs]class SO3Space(GeodesicSpace): """The space of 3D rotations SO(3). The representation is 9 entries of the rotation matrix, laid out in column-major form, like the math.so3 module. """
[docs] def intrinsicDimension(self): return 3
[docs] def extrinsicDimension(self): return 9
[docs] def distance(self,a,b): return vectorops.norm(so3.error(a,b))
[docs] def interpolate(self,a,b,u): return so3.interpolate(a,b,u)
[docs] def difference(self,a,b): w = so3.error(a,b) return so3.mul(so3.cross_product(w),b)
[docs] def integrate(self,x,d): wcross = so3.mul(d,so3.inv(x)) w = so3.deskew(wcross) return so3.mul(so3.from_moment(w),x)
[docs]class SE3Space(GeodesicSpace): """The space of 3D rigid transforms SE(3). The representation is 9 entries of SO(3) + 3 entries of translation. """
[docs] def intrinsicDimension(self): return 6
[docs] def extrinsicDimension(self): return 12
[docs] def to_se3(self,x): return (x[:9],x[9:])
[docs] def from_se3(self,T): return list(T[0])+list(T[1])
[docs] def distance(self,a,b): return vectorops.norm(se3.error(self.to_se3(a),self.to_se3(b)))
[docs] def interpolate(self,a,b,u): r = se3.interpolate(self.to_se3(a),self.to_se3(b),u) return r[0]+r[1]
[docs] def difference(self,a,b): Rb,tb = self.to_se3(b) w = se3.error(self.to_se3(a),self.to_se3(b)) return so3.mul(Rb,so3.cross_product(w[:3]))+w[3:]
[docs] def integrate(self,x,d): assert len(x) == 12 Rx,rx = self.to_se3(x) w = so3.deskew(so3.mul(so3.inv(Rx),d[:9])) v = d[9:] wR = so3.from_moment(w) assert len(wR) == 9 Tx = self.to_se3(x) R = so3.mul(Tx[0],wR) t = vectorops.add(Tx[1],v) return R + t