Source code for lfd.registration.transformation
"""
Register point clouds to each other
arrays are named like name_abc
abc are subscripts and indicate the what that tensor index refers to
index name conventions:
m: test point index
n: training point index
a: input coordinate
g: output coordinate
d: gripper coordinate
"""
from __future__ import division
import numpy as np
from lfd.rapprentice import svds, math_utils
[docs]class Transformation(object):
"""
Object oriented interface for transformations R^d -> R^d
"""
[docs] def transform_vectors(self, x_ma, v_ma):
grad_mga = self.compute_jacobian(x_ma)
return np.einsum('ijk,ik->ij', grad_mga, v_ma) # matrix multiply each jac with each vector
[docs] def transform_bases(self, x_ma, rot_mad, orthogonalize=True, orth_method = "cross"):
"""
orthogonalize: none, svd, qr
"""
grad_mga = self.compute_jacobian(x_ma)
newrot_mgd = np.array([grad_ga.dot(rot_ad) for (grad_ga, rot_ad) in zip(grad_mga, rot_mad)])
if orthogonalize:
if orth_method == "qr":
newrot_mgd = orthogonalize3_qr(newrot_mgd)
elif orth_method == "svd":
newrot_mgd = orthogonalize3_svd(newrot_mgd)
elif orth_method == "cross":
newrot_mgd = orthogonalize3_cross(newrot_mgd)
else: raise Exception("unknown orthogonalization method %s"%orthogonalize)
return newrot_mgd
[docs] def transform_hmats(self, hmat_mAD):
"""
Transform (D+1) x (D+1) homogenius matrices
"""
hmat_mGD = np.empty_like(hmat_mAD)
hmat_mGD[:,:3,3] = self.transform_points(hmat_mAD[:,:3,3])
hmat_mGD[:,:3,:3] = self.transform_bases(hmat_mAD[:,:3,3], hmat_mAD[:,:3,:3])
hmat_mGD[:,3,:] = np.array([0,0,0,1])
return hmat_mGD
[docs] def compute_numerical_jacobian(self, x_d, epsilon=0.0001):
"numerical jacobian"
x0 = np.asfarray(x_d)
f0 = self.transform_points(x0)
jac = np.zeros(len(x0), len(f0))
dx = np.zeros(len(x0))
for i in range(len(x0)):
dx[i] = epsilon
jac[i] = (self.transform_points(x0+dx) - f0) / epsilon
dx[i] = 0.
return jac.transpose()
[docs]class Affine(Transformation):
def __init__(self, lin_ag, trans_g):
self.lin_ag = lin_ag
self.trans_g = trans_g
[docs] def compute_jacobian(self, x_ma):
return np.repeat(self.lin_ag.T[None,:,:],len(x_ma), axis=0)
[docs]class Composition(Transformation):
def __init__(self, fs):
"applied from first to last (left to right)"
self.fs = fs
[docs] def transform_points(self, x_ma):
for f in self.fs: x_ma = f.transform_points(x_ma)
return x_ma
[docs] def compute_jacobian(self, x_ma):
grads = []
for f in self.fs:
grad_mga = f.compute_jacobian(x_ma)
grads.append(grad_mga)
x_ma = f.transform_points(x_ma)
totalgrad = grads[0]
for grad in grads[1:]:
totalgrad = (grad[:,:,:,None] * totalgrad[:,None,:,:]).sum(axis=-2)
return totalgrad
[docs]def orthogonalize3_cross(mats_n33):
"turns each matrix into a rotation"
x_n3 = mats_n33[:,:,0]
# y_n3 = mats_n33[:,:,1]
z_n3 = mats_n33[:,:,2]
znew_n3 = math_utils.normr(z_n3)
ynew_n3 = math_utils.normr(np.cross(znew_n3, x_n3))
xnew_n3 = math_utils.normr(np.cross(ynew_n3, znew_n3))
return np.concatenate([xnew_n3[:,:,None], ynew_n3[:,:,None], znew_n3[:,:,None]],2)
[docs]def orthogonalize3_svd(x_k33):
u_k33, _s_k3, v_k33 = svds.svds(x_k33)
return (u_k33[:,:,:,None] * v_k33[:,None,:,:]).sum(axis=2)