You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
800 lines
22 KiB
800 lines
22 KiB
import numpy as np
|
|
import scipy.spatial
|
|
import scipy.linalg
|
|
|
|
def nullspace(A, atol=1e-13, rtol=0):
|
|
u, s, vh = np.linalg.svd(A)
|
|
tol = max(atol, rtol * s[0])
|
|
nnz = (s >= tol).sum()
|
|
ns = vh[nnz:].conj().T
|
|
return ns
|
|
|
|
def nearest_orthogonal_matrix(R):
|
|
U,S,Vt = np.linalg.svd(R)
|
|
return U @ np.eye(3,dtype=R.dtype) @ Vt
|
|
|
|
def power_iters(A, n_iters=10):
|
|
b = np.random.uniform(-1,1, size=(A.shape[0], A.shape[1], 1))
|
|
for iter in range(n_iters):
|
|
b = A @ b
|
|
b = b / np.linalg.norm(b, axis=1, keepdims=True)
|
|
return b
|
|
|
|
def rayleigh_quotient(A, b):
|
|
return (b.transpose(0,2,1) @ A @ b) / (b.transpose(0,2,1) @ b)
|
|
|
|
|
|
def cross_prod_mat(x):
|
|
x = x.reshape(-1,3)
|
|
X = np.empty((x.shape[0],3,3), dtype=x.dtype)
|
|
X[:,0,0] = 0
|
|
X[:,0,1] = -x[:,2]
|
|
X[:,0,2] = x[:,1]
|
|
X[:,1,0] = x[:,2]
|
|
X[:,1,1] = 0
|
|
X[:,1,2] = -x[:,0]
|
|
X[:,2,0] = -x[:,1]
|
|
X[:,2,1] = x[:,0]
|
|
X[:,2,2] = 0
|
|
return X.squeeze()
|
|
|
|
def hat_operator(x):
|
|
return cross_prod_mat(x)
|
|
|
|
def vee_operator(X):
|
|
X = X.reshape(-1,3,3)
|
|
x = np.empty((X.shape[0], 3), dtype=X.dtype)
|
|
x[:,0] = X[:,2,1]
|
|
x[:,1] = X[:,0,2]
|
|
x[:,2] = X[:,1,0]
|
|
return x.squeeze()
|
|
|
|
|
|
def rot_x(x, dtype=np.float32):
|
|
x = np.array(x, copy=False)
|
|
x = x.reshape(-1,1)
|
|
R = np.zeros((x.shape[0],3,3), dtype=dtype)
|
|
R[:,0,0] = 1
|
|
R[:,1,1] = np.cos(x).ravel()
|
|
R[:,1,2] = -np.sin(x).ravel()
|
|
R[:,2,1] = np.sin(x).ravel()
|
|
R[:,2,2] = np.cos(x).ravel()
|
|
return R.squeeze()
|
|
|
|
def rot_y(y, dtype=np.float32):
|
|
y = np.array(y, copy=False)
|
|
y = y.reshape(-1,1)
|
|
R = np.zeros((y.shape[0],3,3), dtype=dtype)
|
|
R[:,0,0] = np.cos(y).ravel()
|
|
R[:,0,2] = np.sin(y).ravel()
|
|
R[:,1,1] = 1
|
|
R[:,2,0] = -np.sin(y).ravel()
|
|
R[:,2,2] = np.cos(y).ravel()
|
|
return R.squeeze()
|
|
|
|
def rot_z(z, dtype=np.float32):
|
|
z = np.array(z, copy=False)
|
|
z = z.reshape(-1,1)
|
|
R = np.zeros((z.shape[0],3,3), dtype=dtype)
|
|
R[:,0,0] = np.cos(z).ravel()
|
|
R[:,0,1] = -np.sin(z).ravel()
|
|
R[:,1,0] = np.sin(z).ravel()
|
|
R[:,1,1] = np.cos(z).ravel()
|
|
R[:,2,2] = 1
|
|
return R.squeeze()
|
|
|
|
def xyz_from_rotm(R):
|
|
R = R.reshape(-1,3,3)
|
|
xyz = np.empty((R.shape[0],3), dtype=R.dtype)
|
|
for bidx in range(R.shape[0]):
|
|
if R[bidx,0,2] < 1:
|
|
if R[bidx,0,2] > -1:
|
|
xyz[bidx,1] = np.arcsin(R[bidx,0,2])
|
|
xyz[bidx,0] = np.arctan2(-R[bidx,1,2], R[bidx,2,2])
|
|
xyz[bidx,2] = np.arctan2(-R[bidx,0,1], R[bidx,0,0])
|
|
else:
|
|
xyz[bidx,1] = -np.pi/2
|
|
xyz[bidx,0] = -np.arctan2(R[bidx,1,0],R[bidx,1,1])
|
|
xyz[bidx,2] = 0
|
|
else:
|
|
xyz[bidx,1] = np.pi/2
|
|
xyz[bidx,0] = np.arctan2(R[bidx,1,0], R[bidx,1,1])
|
|
xyz[bidx,2] = 0
|
|
return xyz.squeeze()
|
|
|
|
def zyx_from_rotm(R):
|
|
R = R.reshape(-1,3,3)
|
|
zyx = np.empty((R.shape[0],3), dtype=R.dtype)
|
|
for bidx in range(R.shape[0]):
|
|
if R[bidx,2,0] < 1:
|
|
if R[bidx,2,0] > -1:
|
|
zyx[bidx,1] = np.arcsin(-R[bidx,2,0])
|
|
zyx[bidx,0] = np.arctan2(R[bidx,1,0], R[bidx,0,0])
|
|
zyx[bidx,2] = np.arctan2(R[bidx,2,1], R[bidx,2,2])
|
|
else:
|
|
zyx[bidx,1] = np.pi / 2
|
|
zyx[bidx,0] = -np.arctan2(-R[bidx,1,2], R[bidx,1,1])
|
|
zyx[bidx,2] = 0
|
|
else:
|
|
zyx[bidx,1] = -np.pi / 2
|
|
zyx[bidx,0] = np.arctan2(-R[bidx,1,2], R[bidx,1,1])
|
|
zyx[bidx,2] = 0
|
|
return zyx.squeeze()
|
|
|
|
def rotm_from_xyz(xyz):
|
|
xyz = np.array(xyz, copy=False).reshape(-1,3)
|
|
return (rot_x(xyz[:,0]) @ rot_y(xyz[:,1]) @ rot_z(xyz[:,2])).squeeze()
|
|
|
|
def rotm_from_zyx(zyx):
|
|
zyx = np.array(zyx, copy=False).reshape(-1,3)
|
|
return (rot_z(zyx[:,0]) @ rot_y(zyx[:,1]) @ rot_x(zyx[:,2])).squeeze()
|
|
|
|
def rotm_from_quat(q):
|
|
q = q.reshape(-1,4)
|
|
w, x, y, z = q[:,0], q[:,1], q[:,2], q[:,3]
|
|
R = np.array([
|
|
[1 - 2*y*y - 2*z*z, 2*x*y - 2*z*w, 2*x*z + 2*y*w],
|
|
[2*x*y + 2*z*w, 1 - 2*x*x - 2*z*z, 2*y*z - 2*x*w],
|
|
[2*x*z - 2*y*w, 2*y*z + 2*x*w, 1 - 2*x*x - 2*y*y]
|
|
], dtype=q.dtype)
|
|
R = R.transpose((2,0,1))
|
|
return R.squeeze()
|
|
|
|
def rotm_from_axisangle(a):
|
|
# exponential
|
|
a = a.reshape(-1,3)
|
|
phi = np.linalg.norm(a, axis=1).reshape(-1,1,1)
|
|
iphi = np.zeros_like(phi)
|
|
np.divide(1, phi, out=iphi, where=phi != 0)
|
|
A = cross_prod_mat(a) * iphi
|
|
R = np.eye(3, dtype=a.dtype) + np.sin(phi) * A + (1 - np.cos(phi)) * A @ A
|
|
return R.squeeze()
|
|
|
|
def rotm_from_lookat(dir, up=None):
|
|
dir = dir.reshape(-1,3)
|
|
if up is None:
|
|
up = np.zeros_like(dir)
|
|
up[:,1] = 1
|
|
dir /= np.linalg.norm(dir, axis=1, keepdims=True)
|
|
up /= np.linalg.norm(up, axis=1, keepdims=True)
|
|
x = dir[:,None,:] @ cross_prod_mat(up).transpose(0,2,1)
|
|
y = x @ cross_prod_mat(dir).transpose(0,2,1)
|
|
x = x.squeeze()
|
|
y = y.squeeze()
|
|
x /= np.linalg.norm(x, axis=1, keepdims=True)
|
|
y /= np.linalg.norm(y, axis=1, keepdims=True)
|
|
R = np.empty((dir.shape[0],3,3), dtype=dir.dtype)
|
|
R[:,0,0] = x[:,0]
|
|
R[:,0,1] = y[:,0]
|
|
R[:,0,2] = dir[:,0]
|
|
R[:,1,0] = x[:,1]
|
|
R[:,1,1] = y[:,1]
|
|
R[:,1,2] = dir[:,1]
|
|
R[:,2,0] = x[:,2]
|
|
R[:,2,1] = y[:,2]
|
|
R[:,2,2] = dir[:,2]
|
|
return R.transpose(0,2,1).squeeze()
|
|
|
|
def rotm_distance_identity(R0, R1):
|
|
# https://link.springer.com/article/10.1007%2Fs10851-009-0161-2
|
|
# in [0, 2*sqrt(2)]
|
|
R0 = R0.reshape(-1,3,3)
|
|
R1 = R1.reshape(-1,3,3)
|
|
dists = np.linalg.norm(np.eye(3,dtype=R0.dtype) - R0 @ R1.transpose(0,2,1), axis=(1,2))
|
|
return dists.squeeze()
|
|
|
|
def rotm_distance_geodesic(R0, R1):
|
|
# https://link.springer.com/article/10.1007%2Fs10851-009-0161-2
|
|
# in [0, pi)
|
|
R0 = R0.reshape(-1,3,3)
|
|
R1 = R1.reshape(-1,3,3)
|
|
RtR = R0 @ R1.transpose(0,2,1)
|
|
aa = axisangle_from_rotm(RtR)
|
|
S = cross_prod_mat(aa).reshape(-1,3,3)
|
|
dists = np.linalg.norm(S, axis=(1,2))
|
|
return dists.squeeze()
|
|
|
|
|
|
|
|
def axisangle_from_rotm(R):
|
|
# logarithm of rotation matrix
|
|
# R = R.reshape(-1,3,3)
|
|
# tr = np.trace(R, axis1=1, axis2=2)
|
|
# phi = np.arccos(np.clip((tr - 1) / 2, -1, 1))
|
|
# scale = np.zeros_like(phi)
|
|
# div = 2 * np.sin(phi)
|
|
# np.divide(phi, div, out=scale, where=np.abs(div) > 1e-6)
|
|
# A = (R - R.transpose(0,2,1)) * scale.reshape(-1,1,1)
|
|
# aa = np.stack((A[:,2,1], A[:,0,2], A[:,1,0]), axis=1)
|
|
# return aa.squeeze()
|
|
R = R.reshape(-1,3,3)
|
|
omega = np.empty((R.shape[0], 3), dtype=R.dtype)
|
|
omega[:,0] = R[:,2,1] - R[:,1,2]
|
|
omega[:,1] = R[:,0,2] - R[:,2,0]
|
|
omega[:,2] = R[:,1,0] - R[:,0,1]
|
|
r = np.linalg.norm(omega, axis=1).reshape(-1,1)
|
|
t = np.trace(R, axis1=1, axis2=2).reshape(-1,1)
|
|
omega = np.arctan2(r, t-1) * omega
|
|
aa = np.zeros_like(omega)
|
|
np.divide(omega, r, out=aa, where=r != 0)
|
|
return aa.squeeze()
|
|
|
|
def axisangle_from_quat(q):
|
|
q = q.reshape(-1,4)
|
|
phi = 2 * np.arccos(q[:,0])
|
|
denom = np.zeros_like(q[:,0])
|
|
np.divide(1, np.sqrt(1 - q[:,0]**2), out=denom, where=q[:,0] != 1)
|
|
axis = q[:,1:] * denom.reshape(-1,1)
|
|
denom = np.linalg.norm(axis, axis=1).reshape(-1,1)
|
|
a = np.zeros_like(axis)
|
|
np.divide(phi.reshape(-1,1) * axis, denom, out=a, where=denom != 0)
|
|
aa = a.astype(q.dtype)
|
|
return aa.squeeze()
|
|
|
|
def axisangle_apply(aa, x):
|
|
# working only with single aa and single x at the moment
|
|
xshape = x.shape
|
|
aa = aa.reshape(3,)
|
|
x = x.reshape(3,)
|
|
phi = np.linalg.norm(aa)
|
|
e = np.zeros_like(aa)
|
|
np.divide(aa, phi, out=e, where=phi != 0)
|
|
xr = np.cos(phi) * x + np.sin(phi) * np.cross(e, x) + (1 - np.cos(phi)) * (e.T @ x) * e
|
|
return xr.reshape(xshape)
|
|
|
|
|
|
def exp_so3(R):
|
|
w = axisangle_from_rotm(R)
|
|
return w
|
|
|
|
def log_so3(w):
|
|
R = rotm_from_axisangle(w)
|
|
return R
|
|
|
|
def exp_se3(R, t):
|
|
R = R.reshape(-1,3,3)
|
|
t = t.reshape(-1,3)
|
|
|
|
w = exp_so3(R).reshape(-1,3)
|
|
|
|
phi = np.linalg.norm(w, axis=1).reshape(-1,1,1)
|
|
A = cross_prod_mat(w)
|
|
Vi = np.eye(3, dtype=R.dtype) - A/2 + (1 - (phi * np.sin(phi) / (2 * (1 - np.cos(phi))))) / phi**2 * A @ A
|
|
u = t.reshape(-1,1,3) @ Vi.transpose(0,2,1)
|
|
|
|
# v = (u, w)
|
|
v = np.empty((R.shape[0],6), dtype=R.dtype)
|
|
v[:,:3] = u.squeeze()
|
|
v[:,3:] = w
|
|
|
|
return v.squeeze()
|
|
|
|
def log_se3(v):
|
|
# v = (u, w)
|
|
v = v.reshape(-1,6)
|
|
u = v[:,:3]
|
|
w = v[:,3:]
|
|
|
|
R = log_so3(w)
|
|
|
|
phi = np.linalg.norm(w, axis=1).reshape(-1,1,1)
|
|
A = cross_prod_mat(w)
|
|
V = np.eye(3, dtype=v.dtype) + (1 - np.cos(phi)) / phi**2 * A + (phi - np.sin(phi)) / phi**3 * A @ A
|
|
t = u.reshape(-1,1,3) @ V.transpose(0,2,1)
|
|
|
|
return R.squeeze(), t.squeeze()
|
|
|
|
|
|
def quat_from_rotm(R):
|
|
R = R.reshape(-1,3,3)
|
|
q = np.empty((R.shape[0], 4,), dtype=R.dtype)
|
|
q[:,0] = np.sqrt( np.maximum(0, 1 + R[:,0,0] + R[:,1,1] + R[:,2,2]) )
|
|
q[:,1] = np.sqrt( np.maximum(0, 1 + R[:,0,0] - R[:,1,1] - R[:,2,2]) )
|
|
q[:,2] = np.sqrt( np.maximum(0, 1 - R[:,0,0] + R[:,1,1] - R[:,2,2]) )
|
|
q[:,3] = np.sqrt( np.maximum(0, 1 - R[:,0,0] - R[:,1,1] + R[:,2,2]) )
|
|
q[:,1] *= np.sign(q[:,1] * (R[:,2,1] - R[:,1,2]))
|
|
q[:,2] *= np.sign(q[:,2] * (R[:,0,2] - R[:,2,0]))
|
|
q[:,3] *= np.sign(q[:,3] * (R[:,1,0] - R[:,0,1]))
|
|
q /= np.linalg.norm(q,axis=1,keepdims=True)
|
|
return q.squeeze()
|
|
|
|
def quat_from_axisangle(a):
|
|
a = a.reshape(-1, 3)
|
|
phi = np.linalg.norm(a, axis=1)
|
|
iphi = np.zeros_like(phi)
|
|
np.divide(1, phi, out=iphi, where=phi != 0)
|
|
a = a * iphi.reshape(-1,1)
|
|
theta = phi / 2.0
|
|
r = np.cos(theta)
|
|
stheta = np.sin(theta)
|
|
q = np.stack((r, stheta*a[:,0], stheta*a[:,1], stheta*a[:,2]), axis=1)
|
|
q /= np.linalg.norm(q, axis=1).reshape(-1,1)
|
|
return q.squeeze()
|
|
|
|
def quat_identity(n=1, dtype=np.float32):
|
|
q = np.zeros((n,4), dtype=dtype)
|
|
q[:,0] = 1
|
|
return q.squeeze()
|
|
|
|
def quat_conjugate(q):
|
|
shape = q.shape
|
|
q = q.reshape(-1,4).copy()
|
|
q[:,1:] *= -1
|
|
return q.reshape(shape)
|
|
|
|
def quat_product(q1, q2):
|
|
# q1 . q2 is equivalent to R(q1) @ R(q2)
|
|
shape = q1.shape
|
|
q1, q2 = q1.reshape(-1,4), q2.reshape(-1, 4)
|
|
q = np.empty((max(q1.shape[0], q2.shape[0]), 4), dtype=q1.dtype)
|
|
a1,b1,c1,d1 = q1[:,0], q1[:,1], q1[:,2], q1[:,3]
|
|
a2,b2,c2,d2 = q2[:,0], q2[:,1], q2[:,2], q2[:,3]
|
|
q[:,0] = a1 * a2 - b1 * b2 - c1 * c2 - d1 * d2
|
|
q[:,1] = a1 * b2 + b1 * a2 + c1 * d2 - d1 * c2
|
|
q[:,2] = a1 * c2 - b1 * d2 + c1 * a2 + d1 * b2
|
|
q[:,3] = a1 * d2 + b1 * c2 - c1 * b2 + d1 * a2
|
|
return q.squeeze()
|
|
|
|
def quat_apply(q, x):
|
|
xshape = x.shape
|
|
x = x.reshape(-1, 3)
|
|
qshape = q.shape
|
|
q = q.reshape(-1, 4)
|
|
|
|
p = np.empty((x.shape[0], 4), dtype=x.dtype)
|
|
p[:,0] = 0
|
|
p[:,1:] = x
|
|
|
|
r = quat_product(quat_product(q, p), quat_conjugate(q))
|
|
if r.ndim == 1:
|
|
return r[1:].reshape(xshape)
|
|
else:
|
|
return r[:,1:].reshape(xshape)
|
|
|
|
|
|
def quat_random(rng=None, n=1):
|
|
# http://planning.cs.uiuc.edu/node198.html
|
|
if rng is not None:
|
|
u = rng.uniform(0, 1, size=(3,n))
|
|
else:
|
|
u = np.random.uniform(0, 1, size=(3,n))
|
|
q = np.array((
|
|
np.sqrt(1 - u[0]) * np.sin(2 * np.pi * u[1]),
|
|
np.sqrt(1 - u[0]) * np.cos(2 * np.pi * u[1]),
|
|
np.sqrt(u[0]) * np.sin(2 * np.pi * u[2]),
|
|
np.sqrt(u[0]) * np.cos(2 * np.pi * u[2])
|
|
)).T
|
|
q /= np.linalg.norm(q,axis=1,keepdims=True)
|
|
return q.squeeze()
|
|
|
|
def quat_distance_angle(q0, q1):
|
|
# https://math.stackexchange.com/questions/90081/quaternion-distance
|
|
# https://link.springer.com/article/10.1007%2Fs10851-009-0161-2
|
|
q0 = q0.reshape(-1,4)
|
|
q1 = q1.reshape(-1,4)
|
|
dists = np.arccos(np.clip(2 * np.sum(q0 * q1, axis=1)**2 - 1, -1, 1))
|
|
return dists
|
|
|
|
def quat_distance_normdiff(q0, q1):
|
|
# https://link.springer.com/article/10.1007%2Fs10851-009-0161-2
|
|
# \phi_4
|
|
# [0, 1]
|
|
q0 = q0.reshape(-1,4)
|
|
q1 = q1.reshape(-1,4)
|
|
return 1 - np.sum(q0 * q1, axis=1)**2
|
|
|
|
def quat_distance_mineucl(q0, q1):
|
|
# https://link.springer.com/article/10.1007%2Fs10851-009-0161-2
|
|
# http://users.cecs.anu.edu.au/~trumpf/pubs/Hartley_Trumpf_Dai_Li.pdf
|
|
q0 = q0.reshape(-1,4)
|
|
q1 = q1.reshape(-1,4)
|
|
diff0 = ((q0 - q1)**2).sum(axis=1)
|
|
diff1 = ((q0 + q1)**2).sum(axis=1)
|
|
return np.minimum(diff0, diff1)
|
|
|
|
def quat_slerp_space(q0, q1, num=100, endpoint=True):
|
|
q0 = q0.ravel()
|
|
q1 = q1.ravel()
|
|
dot = q0.dot(q1)
|
|
if dot < 0:
|
|
q1 *= -1
|
|
dot *= -1
|
|
t = np.linspace(0, 1, num=num, endpoint=endpoint, dtype=q0.dtype)
|
|
t = t.reshape((-1,1))
|
|
if dot > 0.9995:
|
|
ret = q0 + t * (q1 - q0)
|
|
return ret
|
|
dot = np.clip(dot, -1, 1)
|
|
theta0 = np.arccos(dot)
|
|
theta = theta0 * t
|
|
s0 = np.cos(theta) - dot * np.sin(theta) / np.sin(theta0)
|
|
s1 = np.sin(theta) / np.sin(theta0)
|
|
return (s0 * q0) + (s1 * q1)
|
|
|
|
def cart_to_spherical(x):
|
|
shape = x.shape
|
|
x = x.reshape(-1,3)
|
|
y = np.empty_like(x)
|
|
y[:,0] = np.linalg.norm(x, axis=1) # r
|
|
y[:,1] = np.arccos(x[:,2] / y[:,0]) # theta
|
|
y[:,2] = np.arctan2(x[:,1], x[:,0]) # phi
|
|
return y.reshape(shape)
|
|
|
|
def spherical_to_cart(x):
|
|
shape = x.shape
|
|
x = x.reshape(-1,3)
|
|
y = np.empty_like(x)
|
|
y[:,0] = x[:,0] * np.sin(x[:,1]) * np.cos(x[:,2])
|
|
y[:,1] = x[:,0] * np.sin(x[:,1]) * np.sin(x[:,2])
|
|
y[:,2] = x[:,0] * np.cos(x[:,1])
|
|
return y.reshape(shape)
|
|
|
|
def spherical_random(r=1, n=1):
|
|
# http://mathworld.wolfram.com/SpherePointPicking.html
|
|
# https://math.stackexchange.com/questions/1585975/how-to-generate-random-points-on-a-sphere
|
|
x = np.empty((n,3))
|
|
x[:,0] = r
|
|
x[:,1] = 2 * np.pi * np.random.uniform(0,1, size=(n,))
|
|
x[:,2] = np.arccos(2 * np.random.uniform(0,1, size=(n,)) - 1)
|
|
return x.squeeze()
|
|
|
|
def color_pcl(pcl, K, im, color_axis=0, as_int=True, invalid_color=[0,0,0]):
|
|
uvd = K @ pcl.T
|
|
uvd /= uvd[2]
|
|
uvd = np.round(uvd).astype(np.int32)
|
|
mask = np.logical_and(uvd[0] >= 0, uvd[1] >= 0)
|
|
color = np.empty((pcl.shape[0], 3), dtype=im.dtype)
|
|
if color_axis == 0:
|
|
mask = np.logical_and(mask, uvd[0] < im.shape[2])
|
|
mask = np.logical_and(mask, uvd[1] < im.shape[1])
|
|
uvd = uvd[:,mask]
|
|
color[mask,:] = im[:,uvd[1],uvd[0]].T
|
|
elif color_axis == 2:
|
|
mask = np.logical_and(mask, uvd[0] < im.shape[1])
|
|
mask = np.logical_and(mask, uvd[1] < im.shape[0])
|
|
uvd = uvd[:,mask]
|
|
color[mask,:] = im[uvd[1],uvd[0], :]
|
|
else:
|
|
raise Exception('invalid color_axis')
|
|
color[np.logical_not(mask),:3] = invalid_color
|
|
if as_int:
|
|
color = (255.0 * color).astype(np.int32)
|
|
return color
|
|
|
|
def center_pcl(pcl, robust=False, copy=False, axis=1):
|
|
if copy:
|
|
pcl = pcl.copy()
|
|
if robust:
|
|
mu = np.median(pcl, axis=axis, keepdims=True)
|
|
else:
|
|
mu = np.mean(pcl, axis=axis, keepdims=True)
|
|
return pcl - mu
|
|
|
|
def to_homogeneous(x):
|
|
# return np.hstack((x, np.ones((x.shape[0],1),dtype=x.dtype)))
|
|
return np.concatenate((x, np.ones((*x.shape[:-1],1),dtype=x.dtype)), axis=-1)
|
|
|
|
def from_homogeneous(x):
|
|
return x[:,:-1] / x[:,-1]
|
|
|
|
def project_uvn(uv, Ki=None):
|
|
if uv.shape[1] == 2:
|
|
uvn = to_homogeneous(uv)
|
|
else:
|
|
uvn = uv
|
|
if uvn.shape[1] != 3:
|
|
raise Exception('uv should have shape Nx2 or Nx3')
|
|
if Ki is None:
|
|
return uvn
|
|
else:
|
|
return uvn @ Ki.T
|
|
|
|
def project_uvd(uv, depth, K=np.eye(3), R=np.eye(3), t=np.zeros((3,1)), ignore_negative_depth=True, return_uvn=False):
|
|
Ki = np.linalg.inv(K)
|
|
|
|
if ignore_negative_depth:
|
|
mask = depth >= 0
|
|
uv = uv[mask,:]
|
|
d = depth[mask]
|
|
else:
|
|
d = depth.ravel()
|
|
|
|
uv1 = to_homogeneous(uv)
|
|
|
|
uvn1 = uv1 @ Ki.T
|
|
xyz = d.reshape(-1,1) * uvn1
|
|
xyz = (xyz - t.reshape((1,3))) @ R
|
|
|
|
if return_uvn:
|
|
return xyz, uvn1
|
|
else:
|
|
return xyz
|
|
|
|
def project_depth(depth, K, R=np.eye(3,3), t=np.zeros((3,1)), ignore_negative_depth=True, return_uvn=False):
|
|
u, v = np.meshgrid(range(depth.shape[1]), range(depth.shape[0]))
|
|
uv = np.hstack((u.reshape(-1,1), v.reshape(-1,1)))
|
|
return project_uvd(uv, depth.ravel(), K, R, t, ignore_negative_depth, return_uvn)
|
|
|
|
|
|
def project_xyz(xyz, K=np.eye(3), R=np.eye(3,3), t=np.zeros((3,1))):
|
|
uvd = K @ (R @ xyz.T + t.reshape((3,1)))
|
|
uvd[:2] /= uvd[2]
|
|
return uvd[:2].T, uvd[2]
|
|
|
|
|
|
def relative_motion(R0, t0, R1, t1, Rt_from_global=True):
|
|
t0 = t0.reshape((3,1))
|
|
t1 = t1.reshape((3,1))
|
|
if Rt_from_global:
|
|
Rr = R1 @ R0.T
|
|
tr = t1 - Rr @ t0
|
|
else:
|
|
Rr = R1.T @ R0
|
|
tr = R1.T @ (t0 - t1)
|
|
return Rr, tr.ravel()
|
|
|
|
|
|
def translation_to_cameracenter(R, t):
|
|
t = t.reshape(-1,3,1)
|
|
R = R.reshape(-1,3,3)
|
|
C = -R.transpose(0,2,1) @ t
|
|
return C.squeeze()
|
|
|
|
def cameracenter_to_translation(R, C):
|
|
C = C.reshape(-1,3,1)
|
|
R = R.reshape(-1,3,3)
|
|
t = -R @ C
|
|
return t.squeeze()
|
|
|
|
def decompose_projection_matrix(P, return_t=True):
|
|
if P.shape[0] != 3 or P.shape[1] != 4:
|
|
raise Exception('P has to be 3x4')
|
|
M = P[:, :3]
|
|
C = -np.linalg.inv(M) @ P[:, 3:]
|
|
|
|
R,K = np.linalg.qr(np.flipud(M).T)
|
|
K = np.flipud(K.T)
|
|
K = np.fliplr(K)
|
|
R = np.flipud(R.T)
|
|
|
|
T = np.diag(np.sign(np.diag(K)))
|
|
K = K @ T
|
|
R = T @ R
|
|
|
|
if np.linalg.det(R) < 0:
|
|
R *= -1
|
|
|
|
K /= K[2,2]
|
|
if return_t:
|
|
return K, R, cameracenter_to_translation(R, C)
|
|
else:
|
|
return K, R, C
|
|
|
|
|
|
def compose_projection_matrix(K=np.eye(3), R=np.eye(3,3), t=np.zeros((3,1))):
|
|
return K @ np.hstack((R, t.reshape((3,1))))
|
|
|
|
|
|
|
|
def point_plane_distance(pts, plane):
|
|
pts = pts.reshape(-1,3)
|
|
return np.abs(np.sum(plane[:3] * pts, axis=1) + plane[3]) / np.linalg.norm(plane[:3])
|
|
|
|
def fit_plane(pts):
|
|
pts = pts.reshape(-1,3)
|
|
center = np.mean(pts, axis=0)
|
|
A = pts - center
|
|
u, s, vh = np.linalg.svd(A, full_matrices=False)
|
|
# if pts.shape[0] > 100:
|
|
# import ipdb; ipdb.set_trace()
|
|
plane = np.array([*vh[2], -vh[2].dot(center)])
|
|
return plane
|
|
|
|
def tetrahedron(dtype=np.float32):
|
|
verts = np.array([
|
|
(np.sqrt(8/9), 0, -1/3), (-np.sqrt(2/9), np.sqrt(2/3), -1/3),
|
|
(-np.sqrt(2/9), -np.sqrt(2/3), -1/3), (0, 0, 1)], dtype=dtype)
|
|
faces = np.array([(0,1,2), (0,2,3), (0,1,3), (1,2,3)], dtype=np.int32)
|
|
normals = -np.mean(verts, axis=0) + verts
|
|
normals /= np.linalg.norm(normals, axis=1).reshape(-1,1)
|
|
return verts, faces, normals
|
|
|
|
def cube(dtype=np.float32):
|
|
verts = np.array([
|
|
[-0.5,-0.5,-0.5], [-0.5,0.5,-0.5], [0.5,0.5,-0.5], [0.5,-0.5,-0.5],
|
|
[-0.5,-0.5,0.5], [-0.5,0.5,0.5], [0.5,0.5,0.5], [0.5,-0.5,0.5]], dtype=dtype)
|
|
faces = np.array([
|
|
(0,1,2), (0,2,3), (4,5,6), (4,6,7),
|
|
(0,4,7), (0,7,3), (1,5,6), (1,6,2),
|
|
(3,2,6), (3,6,7), (0,1,5), (0,5,4)], dtype=np.int32)
|
|
normals = -np.mean(verts, axis=0) + verts
|
|
normals /= np.linalg.norm(normals, axis=1).reshape(-1,1)
|
|
return verts, faces, normals
|
|
|
|
def octahedron(dtype=np.float32):
|
|
verts = np.array([
|
|
(+1,0,0), (0,+1,0), (0,0,+1),
|
|
(-1,0,0), (0,-1,0), (0,0,-1)], dtype=dtype)
|
|
faces = np.array([
|
|
(0,1,2), (1,2,3), (3,2,4), (4,2,0),
|
|
(0,1,5), (1,5,3), (3,5,4), (4,5,0)], dtype=np.int32)
|
|
normals = -np.mean(verts, axis=0) + verts
|
|
normals /= np.linalg.norm(normals, axis=1).reshape(-1,1)
|
|
return verts, faces, normals
|
|
|
|
def icosahedron(dtype=np.float32):
|
|
p = (1 + np.sqrt(5)) / 2
|
|
verts = np.array([
|
|
(-1,0,p), (1,0,p), (1,0,-p), (-1,0,-p),
|
|
(0,-p,1), (0,p,1), (0,p,-1), (0,-p,-1),
|
|
(-p,-1,0), (p,-1,0), (p,1,0), (-p,1,0)
|
|
], dtype=dtype)
|
|
faces = np.array([
|
|
(0,1,4), (0,1,5), (1,4,9), (1,9,10), (1,10,5), (0,4,8), (0,8,11), (0,11,5),
|
|
(5,6,11), (5,6,10), (4,7,8), (4,7,9),
|
|
(3,2,6), (3,2,7), (2,6,10), (2,10,9), (2,9,7), (3,6,11), (3,11,8), (3,8,7),
|
|
], dtype=np.int32)
|
|
normals = -np.mean(verts, axis=0) + verts
|
|
normals /= np.linalg.norm(normals, axis=1).reshape(-1,1)
|
|
return verts, faces, normals
|
|
|
|
def xyplane(dtype=np.float32, z=0, interleaved=False):
|
|
if interleaved:
|
|
eps = 1e-6
|
|
verts = np.array([
|
|
(-1,-1,z), (-1,1,z), (1,1,z),
|
|
(1-eps,1,z), (1-eps,-1,z), (-1-eps,-1,z)], dtype=dtype)
|
|
faces = np.array([(0,1,2), (3,4,5)], dtype=np.int32)
|
|
else:
|
|
verts = np.array([(-1,-1,z), (-1,1,z), (1,1,z), (1,-1,z)], dtype=dtype)
|
|
faces = np.array([(0,1,2), (0,2,3)], dtype=np.int32)
|
|
normals = np.zeros_like(verts)
|
|
normals[:,2] = -1
|
|
return verts, faces, normals
|
|
|
|
def mesh_independent_verts(verts, faces, normals=None):
|
|
new_verts = []
|
|
new_normals = []
|
|
for f in faces:
|
|
new_verts.append(verts[f[0]])
|
|
new_verts.append(verts[f[1]])
|
|
new_verts.append(verts[f[2]])
|
|
if normals is not None:
|
|
new_normals.append(normals[f[0]])
|
|
new_normals.append(normals[f[1]])
|
|
new_normals.append(normals[f[2]])
|
|
new_verts = np.array(new_verts)
|
|
new_faces = np.arange(0, faces.size, dtype=faces.dtype).reshape(-1,3)
|
|
if normals is None:
|
|
return new_verts, new_faces
|
|
else:
|
|
new_normals = np.array(new_normals)
|
|
return new_verts, new_faces, new_normals
|
|
|
|
|
|
def stack_mesh(verts, faces):
|
|
n_verts = 0
|
|
mfaces = []
|
|
for idx, f in enumerate(faces):
|
|
mfaces.append(f + n_verts)
|
|
n_verts += verts[idx].shape[0]
|
|
verts = np.vstack(verts)
|
|
faces = np.vstack(mfaces)
|
|
return verts, faces
|
|
|
|
def normalize_mesh(verts):
|
|
# all the verts have unit distance to the center (0,0,0)
|
|
return verts / np.linalg.norm(verts, axis=1, keepdims=True)
|
|
|
|
|
|
def mesh_triangle_areas(verts, faces):
|
|
a = verts[faces[:,0]]
|
|
b = verts[faces[:,1]]
|
|
c = verts[faces[:,2]]
|
|
x = np.empty_like(a)
|
|
x = a - b
|
|
y = a - c
|
|
t = np.empty_like(a)
|
|
t[:,0] = (x[:,1] * y[:,2] - x[:,2] * y[:,1]);
|
|
t[:,1] = (x[:,2] * y[:,0] - x[:,0] * y[:,2]);
|
|
t[:,2] = (x[:,0] * y[:,1] - x[:,1] * y[:,0]);
|
|
return np.linalg.norm(t, axis=1) / 2
|
|
|
|
def subdivde_mesh(verts_in, faces_in, n=1):
|
|
for iter in range(n):
|
|
verts = []
|
|
for v in verts_in:
|
|
verts.append(v)
|
|
faces = []
|
|
verts_dict = {}
|
|
for f in faces_in:
|
|
f = np.sort(f)
|
|
i0,i1,i2 = f
|
|
v0,v1,v2 = verts_in[f]
|
|
|
|
k = i0*len(verts_in)+i1
|
|
if k in verts_dict:
|
|
i01 = verts_dict[k]
|
|
else:
|
|
i01 = len(verts)
|
|
verts_dict[k] = i01
|
|
v01 = (v0 + v1) / 2
|
|
verts.append(v01)
|
|
|
|
k = i0*len(verts_in)+i2
|
|
if k in verts_dict:
|
|
i02 = verts_dict[k]
|
|
else:
|
|
i02 = len(verts)
|
|
verts_dict[k] = i02
|
|
v02 = (v0 + v2) / 2
|
|
verts.append(v02)
|
|
|
|
k = i1*len(verts_in)+i2
|
|
if k in verts_dict:
|
|
i12 = verts_dict[k]
|
|
else:
|
|
i12 = len(verts)
|
|
verts_dict[k] = i12
|
|
v12 = (v1 + v2) / 2
|
|
verts.append(v12)
|
|
|
|
faces.append((i0,i01,i02))
|
|
faces.append((i01,i1,i12))
|
|
faces.append((i12,i2,i02))
|
|
faces.append((i01,i12,i02))
|
|
|
|
verts_in = np.array(verts, dtype=verts_in.dtype)
|
|
faces_in = np.array(faces, dtype=np.int32)
|
|
return verts_in, faces_in
|
|
|
|
|
|
def mesh_adjust_winding_order(verts, faces, normals):
|
|
n0 = normals[faces[:,0]]
|
|
n1 = normals[faces[:,1]]
|
|
n2 = normals[faces[:,2]]
|
|
fnormals = (n0 + n1 + n2) / 3
|
|
|
|
v0 = verts[faces[:,0]]
|
|
v1 = verts[faces[:,1]]
|
|
v2 = verts[faces[:,2]]
|
|
|
|
e0 = v1 - v0
|
|
e1 = v2 - v0
|
|
fn = np.cross(e0, e1)
|
|
|
|
dot = np.sum(fnormals * fn, axis=1)
|
|
ma = dot < 0
|
|
|
|
nfaces = faces.copy()
|
|
nfaces[ma,1], nfaces[ma,2] = nfaces[ma,2], nfaces[ma,1]
|
|
|
|
return nfaces
|
|
|
|
|
|
def pcl_to_shapecl(verts, colors=None, shape='cube', width=1.0):
|
|
if shape == 'tetrahedron':
|
|
cverts, cfaces, _ = tetrahedron()
|
|
elif shape == 'cube':
|
|
cverts, cfaces, _ = cube()
|
|
elif shape == 'octahedron':
|
|
cverts, cfaces, _ = octahedron()
|
|
elif shape == 'icosahedron':
|
|
cverts, cfaces, _ = icosahedron()
|
|
else:
|
|
raise Exception('invalid shape')
|
|
|
|
sverts = np.tile(cverts, (verts.shape[0], 1))
|
|
sverts *= width
|
|
sverts += np.repeat(verts, cverts.shape[0], axis=0)
|
|
|
|
sfaces = np.tile(cfaces, (verts.shape[0], 1))
|
|
sfoffset = cverts.shape[0] * np.arange(0, verts.shape[0])
|
|
sfaces += np.repeat(sfoffset, cfaces.shape[0]).reshape(-1,1)
|
|
|
|
if colors is not None:
|
|
scolors = np.repeat(colors, cverts.shape[0], axis=0)
|
|
else:
|
|
scolors = None
|
|
|
|
return sverts, sfaces, scolors
|
|
|