connecting_the_dots/co/geometry.py

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