Poincloud rotor estimation

Consider the following challenge. We are presented with an input pointcloud \(p_i\), and an output pointcloud \(q_i = R[p_i] + \eta_i\), where \(R\) is an unknown tranformation, and \(\eta_i\) is Gaussian noise. The challenge is to reconstruct the transformation \(R\).

In order to do this, we construct a symbolic tranformation \(R\), whose entries are symfit.Parameter objects. We can then use symfit to find the rotor \(R\).

from kingdon import Algebra
from symfit import Fit, Model, CallableModel, Variable, Parameter, Eq, Mul
from symfit.core.minimizers import *
import numpy as np

We set up the number of points n_points in the pointcloud, the number of (Euclidean) dimensions of the modeling space d, and the standard deviation sig of the Gaussian distribution.

n_points = 10
d = 2
sig = 0.02

point_vals = np.zeros((d+1, n_points))
noise_vals = np.zeros((d+1, n_points))
point_vals[0, :] = np.ones(n_points)
point_vals[1:, :] = np.random.random((d, n_points))
noise_vals[1:, :] = np.random.normal(0.0, sig, (d, n_points))

alg = Algebra(d, 0, 1)
locals().update(alg.blades)

Create the points and noise as pseudovector of grade d.

noise = alg.vector(noise_vals).dual()
p = alg.vector(point_vals).dual()
p

[0.6193865  0.05568265 0.10402929 0.62240814 0.63319893 0.57874796
 0.47162778 0.78012503 0.70366406 0.10142499] 𝐞₀₁ + [-0.80124716 -0.13055387 -0.27026671 -0.44351767 -0.98847871 -0.94480035
 -0.07393811 -0.63619458 -0.97730121 -0.76623394] 𝐞₀₂ + [1. 1. 1. 1. 1. 1. 1. 1. 1. 1.] 𝐞₁₂

As the input rotor \(R\), we use a translation by \(0.5\) in the \(\mathbb{e}_{20}\) direction, followed by a rotation around \(\mathbb{e}_{12}\).

t = np.pi/3
T = alg.multivector(e=1, e02=-0.5)
S = alg.multivector(e=np.cos(t), e12=np.sin(t))
R = S*T
print(f'{T=!s}')
print(f'{S=!s}')
print(f'{R=!s}')

T=1 + -0.5 𝐞₀₂
S=0.5 + 0.866 𝐞₁₂
R=0.5 + -0.433 𝐞₀₁ + -0.25 𝐞₀₂ + 0.866 𝐞₁₂

We can now create the transformed pointcloud \(q\), and visualize both both \(p\) and \(q\).

q = R.sw(p).grade(d) + noise

alg.graph(
    0xff0000, p, 'p',
    0x0000ff, q, 'q',
    0x000000, R.grade(2), 'axis',
    scale=1.0,
)

We will now setup a symfit model to describe this transformation, where the rotor \(R\) consists of Parameter’s, and the pointclouds \(p\) and \(q\) are symfit Variable’s.

R_par = alg.evenmv(name='R', symbolcls=Parameter)
p_var = alg.multivector(name='p', symbolcls=Variable, grades=(d,))
q_var = alg.multivector(name='q', symbolcls=Variable, grades=(d,))
print(R_par)
print(p_var)
print(q_var)

R + R01 𝐞₀₁ + R02 𝐞₀₂ + R12 𝐞₁₂
p01 𝐞₀₁ + p02 𝐞₀₂ + p12 𝐞₁₂
q01 𝐞₀₁ + q02 𝐞₀₂ + q12 𝐞₁₂

p_var_trans = (R_par >> p_var).filter()
# model = Model({q_var[k]: expr for k, expr in p_var_trans.items()})
model = Model({var: expr for var, expr in zip(q_var.values(), p_var_trans.values())})
model

\begin{align}q_{01}(p_{01}, p_{02}, p_{12}; R, R_{01}, R_{02}, R_{12}) & = - R R_{02} p_{12} + R \left(R p_{01} - R_{02} p_{12} + R_{12} p_{02}\right) + R_{01} R_{12} p_{12} + R_{12} \left(R p_{02} + R_{01} p_{12} - R_{12} p_{01}\right)\\q_{02}(p_{01}, p_{02}, p_{12}; R, R_{01}, R_{02}, R_{12}) & = R R_{01} p_{12} + R \left(R p_{02} + R_{01} p_{12} - R_{12} p_{01}\right) + R_{02} R_{12} p_{12} - R_{12} \left(R p_{01} - R_{02} p_{12} + R_{12} p_{02}\right)\\q_{12}(p_{12}; R, R_{12}) & = R^{2} p_{12} + R_{12}^{2} p_{12}\end{align}

Prepare the data for symfit.

datadict = {var.name: data for var, data in zip(p_var.values(), p.values())}
datadict.update({var.name: data for var, data in zip(q_var.values(), q.values())})

Initiate a symfit.Fit object with the model and data. We additionally supply the demand \(R \widetilde{R} = 1\), since rotors should be normalized (i.e., othonormal transformations).

constraints = [
    Eq(R_par.normsq().e, 1)
]
fit = Fit(model, **datadict, constraints=constraints)

results = fit.execute()
print(results)

Parameter Value        Standard Deviation
R         5.026128e-01 9.797416e-03
R01       -4.261785e-01 6.756469e-03
R02       -2.477012e-01 1.336769e-02
R12       8.645116e-01 6.683008e-03
Status message         Optimization terminated successfully
Number of iterations   18
Objective              <symfit.core.objectives.LeastSquares object at 0x0000028B62F303D0>
Minimizer              <symfit.core.minimizers.SLSQP object at 0x0000028B63160E20>

Goodness of fit qualifiers:
chi_squared            0.005224097869669531
objective_value        0.0026120489348347656
r_squared              0.9970428183477802

Constraints:
--------------------
Question: R**2 + R12**2 - 1 == 0?
Answer:   2.525091247207456e-12

symfit has used SLSQP because of the constraint, and we see that we have high accuracy on this constraint. Let us print the reconstructed rotor and it’s norm. Furthermore, we can now apply \(\widetilde{R}\) to \(q\) to transform it back to the location of \(p\) so we can visually inspect the quality of the reconstruction.

R_re = R_par(**results.params)
print(R_re)
print(R_re.normsq())

0.503 + -0.426 𝐞₀₁ + -0.248 𝐞₀₂ + 0.865 𝐞₁₂
1.0

p_reconstructed = (~R_re) >> q

from timeit import default_timer

def animate_q():
    """ Make cloud q rotate towards p. """
    s0 = R_re.grade(2).norm().e
    t0 = np.arctan2(s0, R_re.e)
    logR = R_re.grade(2) / s0
    t = t0 * (np.sin(default_timer() / 2) + 1 ) / 2 # [0, t0]

    R = np.cos(t) + logR * np.sin(t)
    return ~R >> q

alg.graph(
    0xff0000, p,
    0x0000ff, q, 'q',
    0x880088, p_reconstructed, 'p reconstructed',
    0x000000, R_re.grade(2), 'reconst. axis',
    animate_q, 'q',
    animate=True,
    scale=1.0,
)

We see that we have excellent agreement between the original and reconstructed pointclouds.