A Symmetric Objective Function for ICP Eq. 9

from trigonometry: tan, cos

t̃ = t/cos(θ)
ã = a tan(θ)
∑_i cos²(θ)((p_i - q_i)⋅n_i +((p_i+q_i)×n_i)⋅ã+n_i⋅t̃)² 

where
a ∈ ℝ³ : axis of rotation
θ ∈ ℝ  : angle of rotation
p_i ∈ ℝ³
q_i ∈ ℝ³
n_i ∈ ℝ³
t ∈ ℝ³

/*
from trigonometry: tan, cos

t̃ = t/cos(θ)
ã = a tan(θ)
∑_i cos²(θ)((p_i - q_i)⋅n_i +((p_i+q_i)×n_i)⋅ã+n_i⋅t̃)² 

where
a ∈ ℝ³ : axis of rotation
θ ∈ ℝ  : angle of rotation
p_i ∈ ℝ³
q_i ∈ ℝ³
n_i ∈ ℝ³
t ∈ ℝ³
*/
#include <Eigen/Core>
#include <Eigen/Dense>
#include <Eigen/Sparse>
#include <iostream>
#include <set>

struct symmetric_objective_function_9ResultType {
    Eigen::Matrix<double, 3, 1> t̃;
    Eigen::Matrix<double, 3, 1> ã;
    double ret;
    symmetric_objective_function_9ResultType(const Eigen::Matrix<double, 3, 1> & t̃,
               const Eigen::Matrix<double, 3, 1> & ã,
               const double & ret)
    : t̃(t̃),
    ã(ã),
    ret(ret)
    {}
};

/**
 * symmetric_objective_function_9
 *
 * @param a  axis of rotation
 * @param θ  angle of rotation
 * @return ret
 */
symmetric_objective_function_9ResultType symmetric_objective_function_9(
    const Eigen::Matrix<double, 3, 1> & a,
    const double & θ,
    const std::vector<Eigen::Matrix<double, 3, 1>> & p,
    const std::vector<Eigen::Matrix<double, 3, 1>> & q,
    const std::vector<Eigen::Matrix<double, 3, 1>> & n,
    const Eigen::Matrix<double, 3, 1> & t)
{
    const long dim_0 = p.size();
    assert( q.size() == dim_0 );
    assert( n.size() == dim_0 );

    Eigen::Matrix<double, 3, 1> t̃ = t / double(cos(θ));

    Eigen::Matrix<double, 3, 1> ã = a * tan(θ);

    double sum_0 = 0;
    for(int i=1; i<=q.size(); i++){
        sum_0 += pow(cos(θ), 2) * pow((((p.at(i-1) - q.at(i-1))).dot(n.at(i-1)) + ((((p.at(i-1) + q.at(i-1))).cross(n.at(i-1)))).dot(ã) + (n.at(i-1)).dot(t̃)), 2);
    }
    double ret = sum_0;
    return symmetric_objective_function_9ResultType(t̃, ã, ret);
}


void generateRandomData(Eigen::Matrix<double, 3, 1> & a,
    double & θ,
    std::vector<Eigen::Matrix<double, 3, 1>> & p,
    std::vector<Eigen::Matrix<double, 3, 1>> & q,
    std::vector<Eigen::Matrix<double, 3, 1>> & n,
    Eigen::Matrix<double, 3, 1> & t)
{
    θ = rand() % 10;
    const int dim_0 = rand()%10;
    a = Eigen::VectorXd::Random(3);
    p.resize(dim_0);
    for(int i=0; i<dim_0; i++){
        p[i] = Eigen::VectorXd::Random(3);
    }
    q.resize(dim_0);
    for(int i=0; i<dim_0; i++){
        q[i] = Eigen::VectorXd::Random(3);
    }
    n.resize(dim_0);
    for(int i=0; i<dim_0; i++){
        n[i] = Eigen::VectorXd::Random(3);
    }
    t = Eigen::VectorXd::Random(3);
}


int main(int argc, char *argv[])
{
    srand((int)time(NULL));
    Eigen::Matrix<double, 3, 1> a;
    double θ;
    std::vector<Eigen::Matrix<double, 3, 1>> p;
    std::vector<Eigen::Matrix<double, 3, 1>> q;
    std::vector<Eigen::Matrix<double, 3, 1>> n;
    Eigen::Matrix<double, 3, 1> t;
    generateRandomData(a, θ, p, q, n, t);
    symmetric_objective_function_9ResultType func_value = symmetric_objective_function_9(a, θ, p, q, n, t);
    std::cout<<"return value:\n"<<func_value.ret<<std::endl;
    return 0;
}

"""
from trigonometry: tan, cos

t̃ = t/cos(θ)
ã = a tan(θ)
∑_i cos²(θ)((p_i - q_i)⋅n_i +((p_i+q_i)×n_i)⋅ã+n_i⋅t̃)² 

where
a ∈ ℝ³ : axis of rotation
θ ∈ ℝ  : angle of rotation
p_i ∈ ℝ³
q_i ∈ ℝ³
n_i ∈ ℝ³
t ∈ ℝ³
"""
import numpy as np
import scipy
import scipy.linalg
from scipy import sparse
from scipy.integrate import quad
from scipy.optimize import minimize


class symmetric_objective_function_9ResultType:
    def __init__( self, t̃, ã, ret):
        self.t̃ = t̃
        self.ã = ã
        self.ret = ret


def symmetric_objective_function_9(a, θ, p, q, n, t):
    """
    :param :a : axis of rotation
    :param :θ : angle of rotation
    """
    a = np.asarray(a, dtype=np.float64)
    p = np.asarray(p, dtype=np.float64)
    q = np.asarray(q, dtype=np.float64)
    n = np.asarray(n, dtype=np.float64)
    t = np.asarray(t, dtype=np.float64)

    dim_0 = p.shape[0]
    assert a.shape == (3,)
    assert np.ndim(θ) == 0
    assert p.shape == (dim_0, 3, )
    assert q.shape == (dim_0, 3, )
    assert n.shape == (dim_0, 3, )
    assert t.shape == (3,)

    t̃ = t / np.cos(θ)
    ã = a * np.tan(θ)
    sum_0 = 0
    for i in range(1, len(q)+1):
        sum_0 += np.power(np.cos(θ), 2) * np.power((np.dot(((p[i-1] - q[i-1])).ravel(), (n[i-1]).ravel()) + np.dot(((np.cross((p[i-1] + q[i-1]), n[i-1]))).ravel(), (ã).ravel()) + np.dot((n[i-1]).ravel(), (t̃).ravel())), 2)
    ret = sum_0
    return symmetric_objective_function_9ResultType(t̃, ã, ret)


def generateRandomData():
    θ = np.random.randn()
    dim_0 = np.random.randint(10)
    a = np.random.randn(3)
    p = np.random.randn(dim_0, 3, )
    q = np.random.randn(dim_0, 3, )
    n = np.random.randn(dim_0, 3, )
    t = np.random.randn(3)
    return a, θ, p, q, n, t


if __name__ == '__main__':
    a, θ, p, q, n, t = generateRandomData()
    print("a:", a)
    print("θ:", θ)
    print("p:", p)
    print("q:", q)
    print("n:", n)
    print("t:", t)
    func_value = symmetric_objective_function_9(a, θ, p, q, n, t)
    print("return value: ", func_value.ret)

function output = symmetric_objective_function_9(a, theta, p, q, n, t)
% output = symmetric_objective_function_9(a, θ, p, q, n, t)
%
%    from trigonometry: tan, cos
%    
%    t̃ = t/cos(θ)
%    ã = a tan(θ)
%    ∑_i cos²(θ)((p_i - q_i)⋅n_i +((p_i+q_i)×n_i)⋅ã+n_i⋅t̃)² 
%    
%    where
%    a ∈ ℝ³ : axis of rotation
%    θ ∈ ℝ  : angle of rotation
%    p_i ∈ ℝ³
%    q_i ∈ ℝ³
%    n_i ∈ ℝ³
%    t ∈ ℝ³
    if nargin==0
        warning('generating random input data');
        [a, theta, p, q, n, t] = generateRandomData();
    end
    function [a, theta, p, q, n, t] = generateRandomData()
        theta = randn();
        dim_0 = randi(10);
        a = randn(3,1);
        p = randn(dim_0,3);
        q = randn(dim_0,3);
        n = randn(dim_0,3);
        t = randn(3,1);
    end

    a = reshape(a,[],1);
    t = reshape(t,[],1);

    dim_0 = size(p, 1);
    assert( numel(a) == 3 );
    assert(numel(theta) == 1);
    assert( isequal(size(p), [dim_0, 3]) );
    assert( isequal(size(q), [dim_0, 3]) );
    assert( isequal(size(n), [dim_0, 3]) );
    assert( numel(t) == 3 );

    t_tilde = t / cos(theta);
    a_tilde = a * tan(theta);
    sum_0 = 0;
    for i = 1:size(q, 1)
        sum_0 = sum_0 + cos(theta).^2 * (dot((p(i,:)' - q(i,:)'),n(i,:)') + dot((cross((p(i,:)' + q(i,:)'), n(i,:)')),a_tilde) + dot(n(i,:)',t_tilde)).^2;
    end
    ret = sum_0;
    output.t_tilde = t_tilde;

    output.a_tilde = a_tilde;

    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*}
\text{from trigonometry import tan, cos}\\
\textit{t̃} & = \frac{\mathit{t}}{cos\left( \mathit{θ} \right)} \\
\mathit{ã} & = \mathit{a}tan\left( \mathit{θ} \right) \\
 \omit \span \sum_\mathit{i} {cos\left( \mathit{θ} \right)}^{2}{\left( \left( \mathit{p}_{ \mathit{i} } - \mathit{q}_{ \mathit{i} } \right) \cdot \mathit{n}_{ \mathit{i} } + \left( \left( \mathit{p}_{ \mathit{i} } + \mathit{q}_{ \mathit{i} } \right) × \mathit{n}_{ \mathit{i} } \right) \cdot \mathit{ã} + \mathit{n}_{ \mathit{i} } \cdot \textit{t̃} \right)}^{2} \\
\intertext{where} 
\mathit{a} & \in \mathbb{R}^{ 3} \text{ axis of rotation} \\
\mathit{θ} & \in \mathbb{R} \text{ angle of rotation} \\
\mathit{p}_{\mathit{i}} & \in \mathbb{R}^{ 3} \\
\mathit{q}_{\mathit{i}} & \in \mathbb{R}^{ 3} \\
\mathit{n}_{\mathit{i}} & \in \mathbb{R}^{ 3} \\
\mathit{t} & \in \mathbb{R}^{ 3} \\
\\
\end{align*}
\end{minipage}
}
\end{center}

\end{document}