<jupyter_start><jupyter_text>回転の推定II:異方性誤差<jupyter_code>from pathlib import Path
import sys
from itertools import product

from itkwidgets import view
import numpy as np
from scipy import linalg
from scipy.stats import random_correlation, special_ortho_group
from scipy.spatial.transform import Rotation

sys.path.append('..')
import util
A_bar = util.load_point_cloud(Path('../bunny/data/bun180.ply').resolve())
points_num = A_bar.shape[0]
print(points_num)
view(point_sets=A_bar)
cov_a = random_correlation.rvs((0.5, 1.2, 1.3))
print(cov_a)
noise_level = 3e-3
A = A_bar + noise_level * np.random.multivariate_normal(np.zeros(3), cov_a, points_num)
view(point_sets=A)
ideal_R = special_ortho_group.rvs(3)
print(ideal_R)
cov_a_prime = random_correlation.rvs((0.1, 0.2, 2.7))
print(cov_a_prime)
A_prime = A_bar @ ideal_R.T + noise_level * np.random.multivariate_normal(np.zeros(3), cov_a_prime, points_num)
view(point_sets=[A, A_prime])<jupyter_output><empty_output><jupyter_text>特異値分解の場合<jupyter_code>R1 = util.estimate_R_using_SVD(A, A_prime)
print('error:', util.eval_R_error(R1, ideal_R))
view(point_sets=[A @ R1.T, A_prime])<jupyter_output><empty_output><jupyter_text>5.3 四元数表現による回転推定<jupyter_code>Xi = np.stack([
    np.hstack([
        A_prime[:, [0]] - A[:, [0]],
        np.zeros([points_num, 1]),
        -(A_prime[:, [2]] + A[:, [2]]),
        A_prime[:, [1]] + A[:, [1]]
    ]),
    np.hstack([
        A_prime[:, [1]] - A[:, [1]],
        A_prime[:, [2]] + A[:, [2]],
        np.zeros([points_num, 1]),
        -(A_prime[:, [0]] + A[:, [0]])
    ]),
    np.hstack([
        A_prime[:, [2]] - A[:, [2]],
        -(A_prime[:, [1]] + A[:, [1]]),
        A_prime[:, [0]] + A[:, [0]],
        np.zeros([points_num, 1])
    ])
])
print(Xi.shape)
T = np.array([
    [
        [-1,  0,  0,  1,  0,  0],
        [ 0,  0,  0,  0,  0,  0],
        [ 0,  0, -1,  0,  0, -1],
        [ 0,  1,  0,  0,  1,  0]
    ], [
        [ 0, -1,  0,  0,  1,  0],
        [ 0,  0,  1,  0,  0,  1],
        [ 0,  0,  0,  0,  0,  0],
        [-1,  0,  0, -1,  0,  0]
    ], [
        [ 0,  0, -1,  0,  0,  1],
        [ 0, -1,  0,  0, -1,  0],
        [ 1,  0,  0,  1,  0,  0],
        [ 0,  0,  0,  0,  0,  0]
    ]
])
print(T.shape)
cov_joined = linalg.block_diag(cov_a, cov_a_prime)
print(cov_joined)

V_0 = np.zeros([3, 3, T.shape[1], T.shape[1]])
for k, l in product(range(3), repeat=2):
    V_0[k, l] = T[k] @ cov_joined @ T[l].T
print(V_0.shape)<jupyter_output>[[ 1.         -0.25341891 -0.20405749  0.          0.          0.        ]
 [-0.25341891  1.         -0.29006792  0.          0.          0.        ]
 [-0.20405749 -0.29006792  1.          0.          0.          0.        ]
 [ 0.          0.          0.          1.          0.86487508 -0.88330608]
 [ 0.          0.          0.          0.86487508  1.         -0.80110017]
 [ 0.          0.          0.         -0.88330608 -0.80110017  1.        ]]
(3, 3, 4, 4)<jupyter_text>5.4 FNS法による最適化<jupyter_code>def calc_M(W, Xi):
    dim = Xi.shape[2]
    M = np.zeros([dim, dim])
    for k, l in product(range(3), repeat=2):
        M += W[k, l] * Xi[k].T @ Xi[l]
    return M
def calc_L(W, q, Xi, V_0):
    _, points_num, dim = Xi.shape
    V = np.zeros([3, points_num])
    for k, l in product(range(3), repeat=2):
        V[k] += W[k, l] * Xi[l] @ q
    L = np.zeros([dim, dim])
    for k, l in product(range(3), repeat=2):
        L += np.inner(V[k], V[l]) * V_0[k, l]
    return L
def FNS_method(Xi, V_0):
    # step 1
    q0 = np.zeros(4)
    W = np.eye(3)
    iters = 1

    while True:
        # step 2
        X = calc_M(W, Xi) - calc_L(W, q0, Xi, V_0)
        # step 3
        w, eigenvecs = linalg.eigh(X)
        q = eigenvecs[:, np.argmin(w)]
        # step 4
        if np.allclose(q, q0) or np.allclose(q, -q0):
            return q, iters
        W_inv = np.zeros_like(W)
        for k, l in product(range(3), repeat=2):
            W_inv[k, l] = np.inner(q, V_0[k, l] @ q)
        W = linalg.inv(W_inv)
        q0 = q
        iters += 1
q, iters = FNS_method(Xi, V_0)
R2 = Rotation.from_quat(q[[1, 2, 3, 0]]).as_matrix()
print('iterations:', iters)
print('error:', util.eval_R_error(R2, ideal_R))
view(point_sets=[A @ R2.T, A_prime])<jupyter_output><empty_output><jupyter_text>5.5 同次拘束条件による解法<jupyter_code>zeros = np.zeros([points_num, 3])
Xi = np.stack([
    np.hstack([A, zeros, zeros, -A_prime[:, [0]]]),
    np.hstack([zeros, A, zeros, -A_prime[:, [1]]]),
    np.hstack([zeros, zeros, A, -A_prime[:, [2]]])
])
del zeros
print(Xi.shape)
T = np.zeros([3 ,10, 6])
for i in range(3):
    T[i, i * 3, 0] = T[i, i * 3 + 1, 1] = T[i, i * 3 + 2, 2] = 1
    T[i, 9, 3 +  i] = -1
print(T.shape)
print(T)
V_0 = np.zeros([3, 3, T.shape[1], T.shape[1]])
for k, l in product(range(3), repeat=2):
    V_0[k, l] = T[k] @ cov_joined @ T[l].T
print(V_0.shape)
def projection_matrix(u):
    orthogonal_basis = np.array([
        [u[1], u[0], 0, u[4], u[3], 0, u[7], u[6], 0, 0],
        [0, u[2], u[1], 0, u[5], u[4], 0, u[8], u[7], 0],
        [u[2], 0, u[0], u[5], 0, u[3], u[8], 0, u[6], 0],
        [2*u[0], 0, 0, 2*u[3], 0, 0, 2*u[6], 0, 0, -2*u[9]],
        [0, 2*u[1], 0, 0, 2*u[4], 0, 0, 2*u[7], 0, -2*u[9]],
        [0, 0, 2*u[2], 0, 0, 2*u[5], 0, 0, 2*u[8], -2*u[9]],
    ]).T

    constraint_num = orthogonal_basis.shape[1]
    # Gram–Schmidt process
    Q, _ = linalg.qr(orthogonal_basis)
    P = np.eye(10)
    for i in range(6):
        P -= np.outer(Q[:, i], Q[:, i])
    return P, constraint_num
def EFNS_method(Xi, V_0):
    # step 1
    u = np.array([1., 0., 0.,
                  0., 1., 0.,
                  0., 0., 1., 1.])
    u /= linalg.norm(u)
    W = np.eye(3)
    iters = 1

    while True:
        # step 2
        M = calc_M(W, Xi)
        L = calc_L(W, u, Xi, V_0)
        # step 3, 4
        P, constraint_num = projection_matrix(u)
        # step 5
        X = P @ (M - L) @ P
        # step 6
        w, vecs = linalg.eigh(X)
        vecs = vecs[:, np.argsort(w)[:constraint_num + 1]]
        # step 7
        u_hat = np.zeros_like(u)
        for i in range(constraint_num + 1):
            u_hat += np.inner(u, vecs[:, i]) * vecs[:, i]
        # step 8
        u_prime = P @ u_hat
        u_prime /= linalg.norm(u_prime)
        if np.allclose(u_prime, u) or np.allclose(u_prime, -u):
            return u_prime, iters
        u += u_prime
        u /= linalg.norm(u)
        W_inv = np.zeros_like(W)
        for k, l in product(range(3), repeat=2):
            W_inv[k, l] = np.inner(u, V_0[k, l] @ u)
        W = linalg.inv(W_inv)
        iters += 1
u, iters = EFNS_method(Xi, V_0)
R3 = u[:-1].reshape(3, 3) / u[-1]
print('iterations:', iters)
print('error:', util.eval_R_error(R3, ideal_R))
view(point_sets=[A @ R3.T, A_prime])<jupyter_output><empty_output><jupyter_text>6.6 最尤推定による回転の最適化（章は違うがやっていることは同じなので混ぜた）<jupyter_code>def calc_W(cov_a, cov_a_prime, R):
    return linalg.inv(R @ cov_a @ R.T + cov_a_prime)
def calc_g(A, A_prime, R, W, cov_a):
    ART = A @ R.T
    EWT = (A_prime - ART) @ W.T
    g = (-np.cross(ART, EWT, axis=1) + np.cross(EWT, EWT @ (R @ cov_a @ R.T), axis=1)).sum(axis=0)
    return g
def calc_H(A, R, W):
    ART = A @ R.T
    tmp = np.stack([
        # np.cross(ART, W[:, 0], axisa=1, axisb=0, axisc=1)
        np.cross(ART, W[[0]], axis=1),
        np.cross(ART, W[[1]], axis=1),
        np.cross(ART, W[[2]], axis=1),
    ], axis=2)
    # np.cross(tmp, ART.reshape(*ART.shape, 1), axisa=2, axisb=1, axisc=2).sum(axis=0)
    return np.cross(tmp, ART.reshape(-1, 1, 3), axis=2).sum(axis=0)
def calc_J(A, A_prime, cov_a, cov_a_prime, R):
    W = calc_W(cov_a, cov_a_prime, R)
    E = A_prime - A @ R.T
    return (E * (E @ W.T)).sum()
def lie_optimize(A, A_prime, cov_a, cov_a_prime):
    # step 1
    R = init_R = util.estimate_R_using_SVD(A, A_prime)
    J = init_J = calc_J(A, A_prime, cov_a, cov_a_prime, R)
    c = 0.0001

    while True:
        W = calc_W(cov_a, cov_a_prime, R)
        # step 2
        g = calc_g(A, A_prime, R, W, cov_a)
        H = calc_H(A, R, W)
        while True:
            # step 3
            omega = linalg.solve(H + c * np.eye(3), -g)
            # step 4
            new_R = util.exponential_map(omega) @ R
            # step 5
            new_J = calc_J(A, A_prime, cov_a, cov_a_prime, new_R)
            if new_J <= J:
                break
            c *= 10
        # step 6
        if linalg.norm(omega) < 1e-10:
            return new_R, new_J, init_R, init_J
        R = new_R
        J = new_J
        c /= 10
R4, J, init_R, init_J = lie_optimize(A, A_prime, cov_a, cov_a_prime)
print('initial error:', util.eval_R_error(R1, ideal_R))
print('final error:', util.eval_R_error(R4, ideal_R))
print('J:', init_J, '->', J)
view(point_sets=[A @ R4.T, A_prime])<jupyter_output><empty_output>
{'fraction_non_alphanumeric': 0.1914942265919744, 'fraction_numerical': 0.05659083114210586, 'mean_word_length': 2.1903719912472646, 'pattern_counts': {'":': 0, '<': 26, '<?xml version=': 0, '>': 25, 'https://': 0, 'lorem ipsum': 0, 'www.': 0, 'xml': 0}, 'pii_count': 0, 'substrings_counts': 0, 'word_list_counts': {'cursed_substrings.json': 18, 'profanity_word_list.json': 0, 'sexual_word_list.json': 0, 'zh_pornsignals.json': 0}}
{'chain_length': '6', 'dir': 'jupyter-structured-clean-dedup', 'id': '495608', 'license': 'MIT', 'n_tokens_mistral': 4225, 'n_tokens_neox': 3991, 'n_words': 873, 'path': 'notebooks/anisotropic_error.ipynb', 'repo_name': 'eduidl/3d-rotations', 'size': '8747'}