def find_rigid_transform(a, b, visualize=False):
"""
Args:
a: a 3xN array of vertex locations
b: a 3xN array of vertex locations
Returns: (R,T) such that R.dot(a)+T ~= b
Based on Arun et al, "Least-squares fitting of two 3-D point sets," 1987.
See also Eggert et al, "Estimating 3-D rigid body transformations: a
comparison of four major algorithms," 1997.
"""
import numpy as np
import scipy.linalg
from blmath.numerics.matlab import col
if a.shape[0] != 3:
if a.shape[1] == 3:
a = a.T
if b.shape[0] != 3:
if b.shape[1] == 3:
b = b.T
assert a.shape[0] == 3
assert b.shape[0] == 3
a_mean = np.mean(a, axis=1)
b_mean = np.mean(b, axis=1)
a_centered = a - col(a_mean)
b_centered = b - col(b_mean)
c = a_centered.dot(b_centered.T)
u, s, v = np.linalg.svd(c, full_matrices=False)
v = v.T
R = v.dot(u.T)
if scipy.linalg.det(R) < 0:
if np.any(s == 0
): # This is only valid in the noiseless case; see the paper
v[:, 2] = -v[:, 2]
R = v.dot(u.T)
else:
raise ValueError(
"find_rigid_transform found a reflection that it cannot recover from. Try RANSAC or something..."
)
T = col(b_mean - R.dot(a_mean))
if visualize != False:
from lace.mesh import Mesh
from lace.meshviewer import MeshViewer
mv = MeshViewer() if visualize is True else visualize
a_T = R.dot(a) + T
mv.set_dynamic_meshes([
Mesh(v=a.T, f=[]).set_vertex_colors('red'),
Mesh(v=b.T, f=[]).set_vertex_colors('green'),
Mesh(v=a_T.T, f=[]).set_vertex_colors('orange'),
])
return R, T
def faces_per_edge(self):
"""Returns an Ex2 array of adjacencies between faces, where
each element in the array is a face index. Each edge is included
only once. Edges that are not shared by 2 faces are not included."""
import numpy as np
import scipy.sparse as sp
from blmath.numerics.matlab import col
IS = np.repeat(np.arange(len(self.f)), 3)
JS = self.f.ravel()
data = np.ones(IS.size)
f2v = sp.csc_matrix((data, (IS, JS)), shape=(len(self.f), np.max(self.f.ravel())+1))
f2f = f2v.dot(f2v.T)
f2f = f2f.tocoo()
f2f = np.hstack((col(f2f.row), col(f2f.col), col(f2f.data)))
which = (f2f[:, 0] < f2f[:, 1]) & (f2f[:, 2] >= 2)
return np.asarray(f2f[which, :2], np.uint32)
def compute_dr_wrt(self, wrt):
if wrt is not self.v:
return None
v = self.v.r.reshape(-1, 3)
blocks = -np.einsum('ij,ik->ijk', v, v) * (self.ss
**(-3. / 2.)).reshape(
(-1, 1, 1))
for i in range(3):
blocks[:, i, i] += self.s_inv
if True: # pylint: disable=using-constant-test
data = blocks.ravel()
indptr = np.arange(0, (self.v.r.size + 1) * 3, 3)
indices = col(np.arange(0, self.v.r.size))
indices = np.hstack([indices, indices, indices])
indices = indices.reshape((-1, 3, 3))
indices = indices.transpose((0, 2, 1)).ravel()
result = sp.csc_matrix((data, indices, indptr),
shape=(self.v.r.size, self.v.r.size))
return result
else:
matvec = lambda x: np.einsum(
'ijk,ik->ij', blocks, x.reshape(
(blocks.shape[0], 3))).ravel()
return sp.linalg.LinearOperator((self.v.r.size, self.v.r.size),
matvec=matvec)
def compute_dr_wrt(self, obj):
if obj not in (self.a, self.b):
return None
sz = self.a.r.size
if self.indices is None or self.indices.size != sz * 3:
self.indptr = np.arange(0, (sz + 1) * 3, 3)
idxs = col(np.arange(0, sz))
idxs = np.hstack([idxs, idxs, idxs])
idxs = idxs.reshape((-1, 3, 3))
idxs = idxs.transpose((0, 2, 1)).ravel()
self.indices = idxs
if obj is self.a:
# m = self.Bx
# matvec = lambda x : _call_einsum_matvec(m, x)
# matmat = lambda x : _call_einsum_matmat(m, x)
# return sp.linalg.LinearOperator((self.a1.size*3, self.a1.size*3), matvec=matvec, matmat=matmat)
data = self.Bx.ravel()
result = sp.csc_matrix((data, self.indices, self.indptr),
shape=(sz, sz))
return -result
elif obj is self.b:
# m = self.Ax
# matvec = lambda x : _call_einsum_matvec(m, x)
# matmat = lambda x : _call_einsum_matmat(m, x)
# return sp.linalg.LinearOperator((self.a1.size*3, self.a1.size*3), matvec=matvec, matmat=matmat)
data = self.Ax.ravel()
result = sp.csc_matrix((data, self.indices, self.indptr),
shape=(sz, sz))
return -result
def landm_xyz_linear_transform(self, ordering=None):
import numpy as np
from blmath.numerics.matlab import col, sparse
if ordering is None:
ordering = self.landm_names
# construct a sparse matrix that converts between the landmark pts and all vertices, with height (# landmarks * 3) and width (# vertices * 3)
if self.landm_regressors is not None:
landmark_coefficients = np.hstack([self.landm_regressors[name][1] for name in ordering])
landmark_indices = np.hstack([self.landm_regressors[name][0] for name in ordering])
column_indices = np.hstack([col(3*landmark_indices + i) for i in range(3)]).flatten()
row_indices = np.hstack([[3*index, 3*index + 1, 3*index + 2]*len(self.landm_regressors[ordering[index]][0]) for index in np.arange(len(ordering))])
values = np.hstack([col(landmark_coefficients) for i in range(3)]).flatten()
return sparse(row_indices, column_indices, values, 3*len(ordering), 3*self.v.shape[0])
elif hasattr(self, 'landm') and len(self.landm) > 0:
landmark_indices = np.array([self.landm[name] for name in ordering])
column_indices = np.hstack(([col(3*landmark_indices + i) for i in range(3)])).flatten()
row_indices = np.arange(3*len(ordering))
return sparse(row_indices, column_indices, np.ones(len(column_indices)), 3*len(ordering), 3*self.v.shape[0])
else:
return np.zeros((0, 0))
def compute_dr_wrt(self, wrt):
if wrt is not self.v:
return None
cplus = self.cplus
cminus = self.cminus
vplus = self.f[:, cplus]
vminus = self.f[:, cminus]
vplus3 = row(
np.hstack(
[col(vplus * 3),
col(vplus * 3 + 1),
col(vplus * 3 + 2)]))
vminus3 = row(
np.hstack([
col(vminus * 3),
col(vminus * 3 + 1),
col(vminus * 3 + 2)
]))
IS = row(np.arange(0, vplus3.size))
ones = np.ones(vplus3.size)
shape = (self.f.size, self.v.r.size)
dr_vplus, dr_vminus = [
sp.csc_matrix((ones, np.vstack([IS, item])), shape=shape)
for item in vplus3, vminus3 # FIXME change item to a DAMP
]
return dr_vplus - dr_vminus
def find_rigid_rotation(a, b, allow_scaling=False):
"""
Args:
a: a 3xN array of vertex locations
b: a 3xN array of vertex locations
Returns: R such that R.dot(a) ~= b
See link: http://en.wikipedia.org/wiki/Orthogonal_Procrustes_problem
"""
import numpy as np
import scipy.linalg
from blmath.numerics.matlab import col
assert a.shape[0] == 3
assert b.shape[0] == 3
if a.size == 3:
cx = np.cross(a.ravel(), b.ravel())
a = np.hstack((col(a), col(cx)))
b = np.hstack((col(b), col(cx)))
c = a.dot(b.T)
u, _, v = np.linalg.svd(c, full_matrices=False)
v = v.T
R = v.dot(u.T)
if scipy.linalg.det(R) < 0:
v[:, 2] = -v[:, 2]
R = v.dot(u.T)
if allow_scaling:
scalefactor = scipy.linalg.norm(b) / scipy.linalg.norm(a)
R = R * scalefactor
return R
def compute_r(self):
return (self.v.r.reshape(-1, 3) / col(self.s)).reshape(
self.v.r.shape)