Geometry Processing Course: Registration

min_(u ∈ ℝ⁶) uᵀ(∑_i [x_i×n̂_i
                       n̂_i  ][(x_i×n̂_i)ᵀ n̂_iᵀ])u - 2uᵀ(∑_i [x_i×n̂_i
                                                               n̂_i  ]n̂_iᵀ(p_i-x_i)) + ∑_i(p_i-x_i)ᵀn̂_i n̂_iᵀ(p_i-x_i)

where

x_i ∈ ℝ³
n̂_i ∈ ℝ³ 
p_i ∈ ℝ³

"""
min_(u ∈ ℝ⁶) uᵀ(∑_i [x_i×n̂_i
                       n̂_i  ][(x_i×n̂_i)ᵀ n̂_iᵀ])u - 2uᵀ(∑_i [x_i×n̂_i
                                                               n̂_i  ]n̂_iᵀ(p_i-x_i)) + ∑_i(p_i-x_i)ᵀn̂_i n̂_iᵀ(p_i-x_i)

where

x_i ∈ ℝ³
n̂_i ∈ ℝ³ 
p_i ∈ ℝ³
"""
import numpy as np
import scipy
import scipy.linalg
from scipy import sparse
from scipy.integrate import quad
from scipy.optimize import minimize


class course_registrationResultType:
    def __init__( self, ret):
        self.ret = ret


def course_registration(x, n̂, p):
    x = np.asarray(x, dtype=np.float64)
    n̂ = np.asarray(n̂, dtype=np.float64)
    p = np.asarray(p, dtype=np.float64)

    dim_0 = x.shape[0]
    assert x.shape == (dim_0, 3, )
    assert n̂.shape == (dim_0, 3, )
    assert p.shape == (dim_0, 3, )

    def target_0(u):
        sum_0 = np.zeros((6, 6))
        for i in range(1, len(x)+1):
            ret_0 = np.vstack(((np.cross(x[i-1], n̂[i-1])).reshape(3, 1), (n̂[i-1]).reshape(3, 1)))
            ret_1 = np.hstack(((np.cross(x[i-1], n̂[i-1])).T.reshape(1, 3), n̂[i-1].T.reshape(1, 3)))
            sum_0 += ret_0 @ ret_1
        sum_1 = np.zeros((6, ))
        for i in range(1, len(p)+1):
            ret_2 = np.vstack(((np.cross(x[i-1], n̂[i-1])).reshape(3, 1), (n̂[i-1]).reshape(3, 1)))
            sum_1 += ret_2 @ n̂[i-1].T.reshape(1, 3) @ (p[i-1] - x[i-1])
        sum_2 = 0
        for i in range(1, len(p)+1):
            sum_2 += (((p[i-1] - x[i-1]).T.reshape(1, 3) @ n̂[i-1]).item() * n̂[i-1].T.reshape(1, 3) @ (p[i-1] - x[i-1])).item()
        return (u.T.reshape(1, 6) @ (sum_0) @ u).item() - (2 * u.T.reshape(1, 6) @ (sum_1)).item() + sum_2
    ret = minimize(target_0, np.zeros(6)).fun
    return course_registrationResultType(ret)


def generateRandomData():
    dim_0 = np.random.randint(10)
    x = np.random.randn(dim_0, 3, )
    n̂ = np.random.randn(dim_0, 3, )
    p = np.random.randn(dim_0, 3, )
    return x, n̂, p


if __name__ == '__main__':
    x, n̂, p = generateRandomData()
    print("x:", x)
    print("n̂:", n̂)
    print("p:", p)
    func_value = course_registration(x, n̂, p)
    print("return value: ", func_value.ret)

function output = course_registration(x, n_hat, p)
% output = course_registration(x, n̂, p)
%
%    min_(u ∈ ℝ⁶) uᵀ(∑_i [x_i×n̂_i
%                           n̂_i  ][(x_i×n̂_i)ᵀ n̂_iᵀ])u - 2uᵀ(∑_i [x_i×n̂_i
%                                                                   n̂_i  ]n̂_iᵀ(p_i-x_i)) + ∑_i(p_i-x_i)ᵀn̂_i n̂_iᵀ(p_i-x_i)
%    
%    where
%    
%    x_i ∈ ℝ³
%    n̂_i ∈ ℝ³ 
%    p_i ∈ ℝ³
    if nargin==0
        warning('generating random input data');
        [x, n_hat, p] = generateRandomData();
    end
    function [x, n_hat, p] = generateRandomData()
        dim_0 = randi(10);
        x = randn(dim_0,3);
        n_hat = randn(dim_0,3);
        p = randn(dim_0,3);
    end

    dim_0 = size(x, 1);
    assert( isequal(size(x), [dim_0, 3]) );
    assert( isequal(size(n_hat), [dim_0, 3]) );
    assert( isequal(size(p), [dim_0, 3]) );

    function ret = target_1(u)
        sum_0 = zeros(6, 6);
        for i = 1:size(x, 1)
            ret_0 = [[reshape(cross(x(i,:)', n_hat(i,:)'), [3, 1])]; [reshape(n_hat(i,:)', [3, 1])]];
            ret_1 = [[(cross(x(i,:)', n_hat(i,:)'))', n_hat(i,:)'']];
            sum_0 = sum_0 + ret_0 * ret_1;
        end
        sum_1 = zeros(6,1);
        for i = 1:size(p, 1)
            ret_2 = [[reshape(cross(x(i,:)', n_hat(i,:)'), [3, 1])]; [reshape(n_hat(i,:)', [3, 1])]];
            sum_1 = sum_1 + ret_2 * n_hat(i,:)'' * (p(i,:)' - x(i,:)');
        end
        sum_2 = 0;
        for i = 1:size(p, 1)
            sum_2 = sum_2 + (p(i,:)' - x(i,:)')' * n_hat(i,:)' * n_hat(i,:)'' * (p(i,:)' - x(i,:)');
        end
        ret = u' * (sum_0) * u - 2 * u' * (sum_1) + sum_2;
    end
    [~,optimize_0] = fminunc(@target_1,zeros(6,1));
    ret = optimize_0;
    output.ret = ret;
end

\documentclass[12pt]{article}
\usepackage{mathdots}
\usepackage[bb=boondox]{mathalfa}
\usepackage{mathtools}
\usepackage{amssymb}
\usepackage{libertine}
\DeclareMathOperator*{\argmax}{arg\,max}
\DeclareMathOperator*{\argmin}{arg\,min}
\usepackage[paperheight=8in,paperwidth=4in,margin=.3in,heightrounded]{geometry}
\let\originalleft\left
\let\originalright\right
\renewcommand{\left}{\mathopen{}\mathclose\bgroup\originalleft}
\renewcommand{\right}{\aftergroup\egroup\originalright}
\begin{document}

\begin{center}
\resizebox{\textwidth}{!} 
{
\begin{minipage}[c]{\textwidth}
\begin{align*}
 \omit \span \begin{aligned} \min_{\mathit{u} \in \mathbb{R}^{ 6}} \quad & {\mathit{u}}^T\left( \sum_\mathit{i} \begin{bmatrix}
\mathit{x}_{ \mathit{i} } × \textit{n̂}_{ \mathit{i} }\\
\textit{n̂}_{ \mathit{i} }\\
\end{bmatrix}\begin{bmatrix}
{\left( \mathit{x}_{ \mathit{i} } × \textit{n̂}_{ \mathit{i} } \right)}^T & {\textit{n̂}_{ \mathit{i} }}^T\\
\end{bmatrix} \right)\mathit{u} - 2{\mathit{u}}^T\left( \sum_\mathit{i} \begin{bmatrix}
\mathit{x}_{ \mathit{i} } × \textit{n̂}_{ \mathit{i} }\\
\textit{n̂}_{ \mathit{i} }\\
\end{bmatrix}{\textit{n̂}_{ \mathit{i} }}^T\left( \mathit{p}_{ \mathit{i} } - \mathit{x}_{ \mathit{i} } \right) \right) + \sum_\mathit{i} {\left( \mathit{p}_{ \mathit{i} } - \mathit{x}_{ \mathit{i} } \right)}^T\textit{n̂}_{ \mathit{i} }{\textit{n̂}_{ \mathit{i} }}^T\left( \mathit{p}_{ \mathit{i} } - \mathit{x}_{ \mathit{i} } \right) \\
\end{aligned} \\
\intertext{where} 
\mathit{x}_{\mathit{i}} & \in \mathbb{R}^{ 3} \\
\textit{n̂}_{\mathit{i}} & \in \mathbb{R}^{ 3} \\
\mathit{p}_{\mathit{i}} & \in \mathbb{R}^{ 3} \\
\\
\end{align*}
\end{minipage}
}
\end{center}

\end{document}