\documentclass[9pt,letterpaper,twocolumn]{IEEEtran}

\usepackage{algorithm}
\usepackage{algpseudocode}
\usepackage{amsmath}
\usepackage{amssymb}
\usepackage{amsthm}

\allowdisplaybreaks

\renewcommand{\vec}[1]{\mathbf{#1}}
\newcommand{\mat}[1]{\mathbf{#1}}

\newtheorem{theorem}{Theorem}
\newtheorem{lemma}{Lemma}

\title{LL3D Aligner Formulation}
\date{}
\author{Zoox, Inc.}

\begin{document}
\maketitle

\section{Introduction}

The LL3D aligner aligns a new metaspin to the current voxel grid, so that
points can be added to the correct voxels. More specifically, the points in the
metaspin are aligned to \emph{surfels} built from the voxel grid using ICP. The
standard ICP iterates between a data association step and an optimization
step. For the aligner, we replace the data association step by a voxel
lookup and finds the closest point from the point to the surfel of the voxel it
is in.

This document contains the exact mathematical formulation of the aligner,
with derivations and proofs.

\section{Cost Function Definition}

Given the current voxel grid $G$, and set of points $\{\vec{p}_i\}$, we want to
find a transform $T\in\mbox{SE}(3)$ that minimizes the following cost function:
\begin{equation}
C(T) = \sum_{i=1}^{N} f(\vec{g}(\vec{p}_i,T),G)
\end{equation}
where $\vec{g}$ is a vector function that applies the $\mbox{SE}(3)$ transform
$T$ to a point $\vec{p}$, and $f$ is a scalar function that given a point
$\vec{p}$, gives a measure of its ``distance'' to the grid $G$.

To define precisely the grid $G$, we index its voxels by an integer triple
$\vec{v}\in\mathbb{Z}^3$, some voxels have surfels, some do not (unoccupied, too
few points etc.), let the set $H\subseteq\mathbb{Z}^3$ be the set of voxels
having valid surfels, and $P:H\rightarrow\mathcal{P}$ be a function that maps a valid
voxel to its surfel (here $\mathcal{P}$ is the set of all planes in
$\mathbb{R}^3$, any representation will do). Then $G = (H,P)$.

Next we define the function $f$ as follows:
\begin{equation}
\label{eq:f_function}
f(\vec{p},G) = \left\{
\begin{array}{ll}
||\vec{p}-\vec{r}(\vec{p},P_{\vec{v}(\vec{p})})||^2&
\mbox{if }\vec{v}(\vec{p})\in H\\
l^2&\mbox{otherwise}
\end{array}
\right.
\end{equation}
Here $\vec{v} : \mathbb{R}^3 \rightarrow \mathbb{Z}^3$ is a function that finds
the voxel a point falls in, $\vec{r}$ is a function that given a point $\vec{p}$ and a
plane $P$, returns the point on the plane that is closet to $\vec{p}$, and $l$
is the diagonal length of a voxel. Because $l^2$ is an upper bound on any square
distance from a point to a surfel (see Theorem~\ref{thm:upper_bound}),
Equation~\ref{eq:f_function} penalizes the case a point falls to an invalid voxel.

\section{ICP Solver}

We follow the standard ICP procedure of first fixing $T$ and compute the closest
points, then fixing the closest points and optimize for $T$. Let's rearrange the
cost function to make the two steps explicit. First define a sequence of voxel
indices $\{\vec{v}_i\}$ that is a function of $T$, each voxel index is given by
$\vec{v}_i = \vec{v}(\vec{g}(\vec{p}_i,T))$, that is, the index of voxel the
\emph{transformed} point falls in. Then define the set of closest points
$\{\vec{r}_i\}(T,\{\vec{v}_i\},G)$, where each $\vec{r}_i$ is given by:
\begin{equation}
\vec{r}_i = \left\{
\begin{array}{ll}
\vec{r}(\vec{g}(\vec{p}_i,T),P_{\vec{v}_i})&
\mbox{if }\vec{v}_i\in H\\
\vec{0}&\mbox{otherwise}
\end{array}
\right.
\end{equation}
And define a set $I$ that is a function of $\{\vec{v}_i\}$ and $G$:
\begin{equation}
I(\{\vec{v}_i\},G) = \{i:\vec{v}_i\in H\}
\end{equation}
Then the cost $C(T)$ can be written as
\begin{equation}
\begin{split}
C(T) &= C(T,\{\vec{v}_i\}(T),\{\vec{r}_i\}(T,\{\vec{v}_i\}(T)))\\
&= \sum_{i\in I(\{\vec{v}_i\}(T))}
  ||\vec{g}(\vec{p}_i,T)-\vec{r}_i(T,\vec{v}_i(T))||^2 + \sum_{i\notin
    I(\{\vec{v}_i\}(T))} l^2
\end{split}
\end{equation}
here we are only showing dependency on $T$ for clarity (you can think of $G$ as
a constant).

Then ICP amounts to first fix $T = T^*$ to find $\{\vec{v}_i\}(T^*)$ and
$\{\vec{r}_i\}(T^*,\{\vec{v}_i\}(T^*))$, then fix $\{\vec{v}_i\}(T^*)$ and
$\{\vec{r}_i\}(T^*,\{\vec{v}_i\}(T^*))$, and optimize:
\begin{equation}
\label{eq:c_star}
\begin{split}
C^*(T) &= C(T,\{\vec{v}_i\}(T^*),\{\vec{r}_i\}(T^*,\{\vec{v}_i\}(T^*))) \\
&= \sum_{i\in I(\{\vec{v}_i\}(T^*))}
  ||\vec{g}(\vec{p}_i,T)-\vec{r}_i(T^*,\vec{v}_i(T^*))||^2 + \sum_{i\notin
    I(\{\vec{v}_i\}(T^*))} l^2
\end{split}
\end{equation}

It turns out there is a closed-form solution to finding the \emph{global}
minimum of $C^*(T)$. If we parameterize the $\mbox{SE}(3)$ transform by a
unit quaternion and a translation vector $T = (\vec{q},\vec{t})$,
Algorithm~\ref{algo:closed_form}
\begin{algorithm*}[t]
\begin{algorithmic}[1]
\If{$I = \varnothing$}
  \State $\hat{T} \gets T^*$
  \State \Return $\hat{T}$
\EndIf
\State $\mat{M}_{rp} \gets \frac{1}{|I|}\sum_{i\in I} \vec{r}_i \vec{p}_i^{T}$
\State $\vec{\bar{r}}\gets\frac{1}{|I|}\sum_{i\in I} \vec{r}_i$
\State $\vec{\bar{p}}\gets\frac{1}{|I|}\sum_{i\in I} \vec{p}_i$
\State $\mat{M}\gets\mat{M}_{rp}-\vec{\bar{r}}\vec{\bar{p}}^{T}$
\State $\mat{Q}\gets\left[
\begin{array}{cccc}
M_{11}+M_{22}+M_{33} & -M_{23}+M_{32} & M_{13}-M_{31} & -M_{12}+M_{21}\\
-M_{23}+M_{32} & M_{11}-M_{22}-M_{33} & M_{12}+M_{21} & M_{13}+M_{31}\\
M_{13}-M_{31} & M_{12}+M_{21} & -M_{11}+M_{22}-M_{33} & M_{23}+M_{32}\\
-M_{12}+M_{21} & M_{13}+M_{31} & M_{23}+M_{32} & -M_{11}-M_{22}+M_{33}
\end{array}
\right]$
\State $\vec{\hat{q}}\gets\mbox{EigenvectorOfLargestEigenvalue}(\mat{Q})$
\State $\mat{\hat{R}}\gets\mbox{ToRotationMatrix}(\vec{\hat{q}})$
\State $\vec{\hat{t}}\gets\vec{\bar{r}}-\mat{\hat{R}}\vec{\bar{p}}$
\State $\hat{T}\gets(\vec{\hat{q}},\vec{\hat{t}})$
\State \Return $\hat{T}$
\end{algorithmic}
\caption{Closed-form global solution for $\hat{T} = \arg\min_{T} C^*(T)$}
\label{algo:closed_form}
\end{algorithm*}
lists the steps required to compute the global minimum in closed-form, based on
an eigenvalue decomposition. In the algorithm pseudo code, $I =
I(\{\vec{v}_i\}(T^*))$ and $\vec{r}_i = \vec{r}_i(T^*,\vec{v}_i(T^*))$ to keep
the presentation clear. The fact this algorithm indeed gives the global minimum
is proved in Theorem~\ref{thm:optimality}. Note that
Algorithm~\ref{algo:closed_form} is essentially the same as the algorithm
described in Section~III-C of the original ICP paper~\cite{Besl1992}, except the
small difference that the matrix $M$ defined in Algorithm~\ref{algo:closed_form}
is the \emph{transpose} of the cross-covariance matrix $\Sigma_{px}$ defined
in~\cite{Besl1992}.

\section{Regularized ICP}

Once alignment has been run long enough, we may drift arbitrarily. Drifting in
pitch and roll is particularly harmful because the ground plane may eventually
drift off to be vertical and extend outside of the limited $z$-bounds of the LL3D
volume. In this section we consider adding a regularization term to the standard
formulation to constrain drift in pitch and roll.

The local\_utm frame's $z$-axis is always opposite to the gravity
direction. Hence, we can use local\_utm frame's $z$-axis to constrain LL3D
aligner's pitch and roll. In order to do this, we can compute the 6dof
transformation $\mbox{local\_utm\_from\_grid}$. Last column of the rotation
matrix of $\mbox{local\_utm\_from\_grid}$ transformation gives us the drift in pitch
and roll from the reference vertical direction.

In the following equations, let's denote grid frame as $G$, local\_utm frame as
$L$, cloud frame as $C$ and rotation from source coordinate frame $A$ to
destination coordinate frame $B$ as $\mat{R}_A^B$.  So, the above constraint can
be added as a regularization term:
\begin{equation}
\zeta(\vec{q}) = ( \mat{R}_G^L(\vec{q}) \vec{\hat{z}} ) ^T \vec{\hat{z}}
\end{equation}
Here, $\vec{\hat{z}} = [0, 0, 1]^T$. Goal here is to maximize $\zeta(\vec{q})$.

Note that the goal of the aligner is to compute the transformation:
$\mbox{grid}\_\mbox{from}\_\mbox{cloud}$. So, $\zeta(\vec{q})$ becomes:
\begin{equation}
\zeta(\vec{q}) = ( \mat{R}_C^L {\mat{R}_C^G}^T(\vec{q}) \vec{\hat{z}})^T \vec{\hat{z}}
         = \vec{\hat{z}}^T\mat{R}_C^G(\vec{q}){\mat{R}_C^L}^T\vec{\hat{z}}
\end{equation}
Choosing
\begin{equation}
\vec{\hat{u}} = {\mat{R}_C^L}^T \vec{\hat{z}} = \mat{R}_L^C\vec{\hat{z}}
\end{equation}
and denoting
\begin{equation}
\mat{R}(\vec{q}) = \mat{R}_C^G(\vec{q})
\end{equation}
we have:
\begin{equation}
\label{eq:reg}
\begin{split}
\zeta(\vec{q})
&= \vec{\hat{z}}^T (\mat{R}(\vec{q}) \vec{\hat{u}} )\\
&= \mbox{tr}(\vec{\hat{z}} (\mat{R}(\vec{q}) \vec{\hat{u}}) ^ T) \\
&= \mbox{tr}(\vec{\hat{z}} \vec{\hat{u}} ^ T \mat{R}^T(\vec{q})) \\
\end{split}
\end{equation}

If $\vec{\hat{u}} = [u_1, u_2, u_3]$, then we have
\begin{equation}
\vec{\hat{z}} \vec{\hat{u}}^T = \left[
\begin{array}{ccc}
0 & 0 & 0 \\
0 & 0 & 0 \\
u_1 & u_2 & u_3 \\
\end{array}
\right]
\end{equation}

Maximizing the regularization term described in Equation~\ref{eq:reg} is
equivalent to minimizing its negative. Let $\lambda$ be the regularization
weight which is independent of the number of points. As $C(T)$ has been defined
as the sum of losses and not the average loss, we need to multiply the
regularization term with $N$.  So the overall cost function we optimize is:
\begin{equation}
\bar{C}(T) = \sum_{i=1}^{N} f(\vec{g}(\vec{p}_i,T),G) - \lambda N
(\zeta(\vec{q}) - 1)
\end{equation}
Note here that $\vec{q}$ is the quaternion representation of the rotation
component of $T$. We've also added a $-1$ to $\zeta(\vec{q})$ to ensure the
overall cost function is still $\ge 0$.

Observing that Equation~\ref{eq:reg} and Equation~\ref{eq:core}, have very
similar form.  This suggests that the optimal transformation can be achieved by
modifying the last row of $\mat{M}$ in Equation~\ref{eq:M_matrix}. Since the final
expression in Equation~\ref{eq:core} also has a negative sign and a $2|I|$ factor
outside the $\mbox{trace}$, we can first modify $\lambda$ to:
\begin{equation}
\lambda \gets \lambda N / (2 |I|)
\end{equation}
and then update $\mat{M}$ to be:
\begin{equation}
\begin{split}
M_{31} \gets M_{31} + \lambda u_1 \\
M_{32} \gets M_{32} + \lambda u_2 \\
M_{33} \gets M_{33} + \lambda u_3 \\
\end{split}
\end{equation}


\appendix

\begin{theorem}
\label{thm:upper_bound}
Given a cubic box $B$ in $\mathbb{R}^3$, a plane $P$ fitted to points in $B$ in the
least-square sense, and a point $\vec{p}\in B$, we have
\begin{equation}
||\vec{p}-\vec{r}(\vec{p},P)||^2\le l^2
\end{equation}
where $\vec{r}$ is a function returning the closest point on $P$ to $\vec{p}$,
and $l$ is the diagonal length of $B$.
\end{theorem}
\begin{proof}
Because $P$ is a least-square-fit plane of points in $B$, it goes through the
mean of the points $\vec{m}$, and we have $\vec{m}\in B$ (mean preserves inequalities), then
\begin{equation}
||\vec{p}-\vec{r}(\vec{p},P)||^2
= ((\vec{p}-\vec{m})\cdot\vec{\hat{n}})^2
\le||\vec{p}-\vec{m}||^2
\le l^2
\end{equation}
where $\vec{\hat{n}}$ is the unit normal of $P$.
\end{proof}

\begin{lemma}
\label{thm:trace}
Given two sequences of vectors $\{\vec{a}_i\}$ and $\{\vec{b}_i\}$ of the same
length $N > 0$, and each vector is D-dimensional (with $D>0$), we have
\begin{equation}
\frac{1}{N} \sum_{i=1}^N \vec{a}_i^{T} \vec{b}_i = \mbox{tr}(\frac{1}{N} \sum_{i=1}^N\vec{a}_i\vec{b}_i^{T})
\end{equation}
\end{lemma}
\begin{proof}
\begin{equation}
\begin{split}
\mbox{tr}(\frac{1}{N} \sum_{i=1}^N\vec{a}_i\vec{b}_i^{T})
&=\sum_{j=1}^D\frac{1}{N}\sum_{i=1}^N[\vec{a}_i]_j[\vec{b}_i]_j\\
&=\frac{1}{N}\sum_{i=1}^N\sum_{j=1}^D[\vec{a}_i]_j[\vec{b}_i]_j\\
&=\frac{1}{N} \sum_{i=1}^N \vec{a}_i^{T} \vec{b}_i
\end{split}
\end{equation}
\end{proof}

\begin{lemma}
\label{thm:max_quadratic_form}
Given a $D\times D$ ($D>0$) real symmetric matrix $\mat{A}$, we have
\begin{equation}
\forall (\vec{x}\in\mathbb{R}^D,||\vec{x}||=1),
\vec{x}^T\mat{A}\vec{x}\le\vec{u}^T\mat{A}\vec{u}
\end{equation}
where $\vec{u}$ is the eigenvector (normalized) corresponding to the largest
eigenvalue of $\mat{A}$.
\end{lemma}
\begin{proof}
Since $\mat{A}$ is real symmetric, it has an eigenvalue decomposition
\begin{equation}
\mat{A} = \mat{W}\mat{\Lambda}\mat{W}^T
\end{equation}
where $\mat{W}$ is orthogonal and $\mat{\Lambda}$ is the diagonal matrix with
the eigenvalues along the diagonal. Then we have
$\forall(\vec{x}\in\mathbb{R}^D, ||\vec{x}||=1)$:
\begin{equation}
\begin{split}
\vec{x}^T\mat{A}\vec{x} &= \vec{x}^T\mat{W}\mat{\Lambda}\mat{W}^T\vec{x}\\
&=(\mat{W}^T\vec{x})^T\mat{\Lambda}(\mat{W}^T\vec{x})\\
&=\vec{y}^T\mat{\Lambda}\vec{y}\\
&=\sum_{i=1}^D \lambda_i y_i^2
\end{split}
\end{equation}
We defined the variable $\vec{y} = \mat{W}^T\vec{x}$ in the above. Now because
$||\vec{x}||=1$ and $\mat{W}$ is orthogonal, we have
\begin{equation}
\sum_{i=1}^D y_i^2
=\vec{y}^T\vec{y}
=\vec{x}^T\mat{W}\mat{W}^T\vec{x}
=\vec{x}^T\vec{x}
=1
\end{equation}

Let $\lambda$ be an eigenvalue such that $\lambda=\max_{i}\lambda_i$, and
$\vec{u}$ be its (normalized) eigenvector, then because $\forall i, \lambda_i\le
\lambda, y_i^2\ge 0$
\begin{equation}
\vec{x}^T\mat{A}\vec{x}
= \sum_{i=1}^D \lambda_i y_i^2
\le \sum_{i=1}^D\lambda y_i^2
= \lambda \sum_{i=1}^D y_i^2
= \lambda
\end{equation}
Also we have
\begin{equation}
\vec{u}^T\mat{A}\vec{u}
=\vec{u}^T(\mat{A}\vec{u})
=\vec{u}^T(\lambda\vec{u})
=\lambda
\end{equation}
This completes the proof.
\end{proof}

\begin{theorem}
\label{thm:optimality}
Algorithm~\ref{algo:closed_form} returns a $\hat{T}$ that is the global minimum
of $C^*(T)$. In other words, $\hat{T}\in\mbox{SE}(3)$ and
$\forall T\in\mbox{SE}(3), C^*(T)\ge C^*(\hat{T})$.
\end{theorem}
\begin{proof}
If $I=\varnothing$, we have $C^*(T) = \sum_{i=1}^N l^2$, which is constant,
hence any $\hat{T}\in\mbox{SE}(3)$ is a global minimum. Since the transform from
the previous iteration $T^*$ is in $\mbox{SE(3)}$ by recursion, $\hat{T}$ is
also in $\mbox{SE}(3)$. Let us focus on the case $I\ne\varnothing$.

When $I\ne\varnothing$, by definition Algorithm~\ref{algo:closed_form} always
returns a unit vector $\vec{\hat{q}}$, hence we have
$\hat{T}\in\mbox{SE}(3)$. Now let's concentrate on proving the second half of
the theorem statement, that is $\forall T\in\mbox{SE}(3), C^*(T)\ge
C^*(\hat{T})$. First let's get rid of the translation component then we can
focus on $\vec{q}$. Expanding $\vec{g}(\vec{p}_i,T) =
\mat{R}(\vec{q})\vec{p}_i+\vec{t}$ into Equation~\ref{eq:c_star}, we have
\begin{equation}
C^*(T) = \sum_{i\in I} ||\mat{R}(\vec{q})\vec{p}_i+\vec{t}-\vec{r}_i||^2
+ \sum_{i\notin I}l^2
\end{equation}
Concentrating on the first term and define $\vec{b}_i =
\vec{r}_i-\mat{R}(\vec{q})\vec{p}_i$, we have
\begin{equation}
\label{eq:quadratic_form_part1}
\begin{split}
\sum_{i\in I} ||\mat{R}(\vec{q})\vec{p}_i+\vec{t}-\vec{r}_i||^2
&=\sum_{i\in I} ||\vec{t}-\vec{b}_i||^2\\
&=\sum_{i\in I} (\vec{t}-\vec{b}_i)^{T}(\vec{t}-\vec{b}_i)\\
&=\sum_{i\in I} (\vec{t}^T\vec{t}-2\vec{b}_i^T\vec{t}+\vec{b}_i^T\vec{b}_i)
\end{split}
\end{equation}
Now using the usual notation to denote the mean and covariance matrix of
$\{\vec{b}_i\}$, i.e\ $\vec{\bar{b}} = 1/|I| \sum_{i\in I}\vec{b}_i$ and
$\mat{\Sigma} = 1/|I|\sum_{i\in I}
(\vec{b}_i-\vec{\bar{b}})(\vec{b}_i-\vec{\bar{b}})^T$, by Lemma~\ref{thm:trace},
we have
\begin{equation}
\begin{split}
\mbox{tr}(\mat{\Sigma})
&= \mbox{tr}\left(\frac{1}{|I|}\sum_{i\in I}
(\vec{b}_i-\vec{\bar{b}})(\vec{b}_i-\vec{\bar{b}})^T\right)\\
&=\frac{1}{|I|}\sum_{i\in I}
(\vec{b}_i-\vec{\bar{b}})^T(\vec{b}_i-\vec{\bar{b}})\\
&=\frac{1}{|I|}\sum_{i\in I}
(\vec{b}_i^T\vec{b}_i-2\vec{\bar{b}}^T\vec{b}_i+\vec{\bar{b}}^T\vec{\bar{b}})\\
&=\frac{1}{|I|}\sum_{i\in I}\vec{b}_i^T\vec{b}_i
-2\vec{\bar{b}}^T\vec{\bar{b}}+\vec{\bar{b}}^T\vec{\bar{b}}\\
&=\frac{1}{|I|}\sum_{i\in I}\vec{b}_i^T\vec{b}_i
-\vec{\bar{b}}^T\vec{\bar{b}}
\end{split}
\end{equation}
With this result, let us go back to Equation~\ref{eq:quadratic_form_part1}
\begin{equation}
\begin{split}
\sum_{i\in I} ||\mat{R}(\vec{q})\vec{p}_i+\vec{t}-\vec{r}_i||^2
&=\sum_{i\in I} (\vec{t}^T\vec{t}-2\vec{b}_i^T\vec{t}+\vec{b}_i^T\vec{b}_i)\\
&=|I|(\vec{t}^T\vec{t}-2\vec{\bar{b}}^T\vec{t}+\mbox{tr}(\mat{\Sigma})+\vec{\bar{b}}^T\vec{\bar{b}})\\
&=|I|((\vec{t}-\vec{\bar{b}})^T(\vec{t}-\vec{\bar{b}})+\mbox{tr}(\mat{\Sigma}))\\
&\ge |I|\mbox{tr}(\mat{\Sigma})
\end{split}
\end{equation}
with equality at $\vec{t} = \vec{\bar{b}}
= \vec{\bar{r}}-\mat{R}(\vec{q})\vec{\bar{p}}$. Hence we have
\begin{equation}
C^*(T) = C^*(\vec{q},\vec{t}) \ge C^*(\vec{q},\vec{\bar{r}}-\mat{R}(\vec{q})\vec{\bar{p}})
\end{equation}

Let us define a new cost function on only $\vec{q}$ to be
\begin{equation}
C^*_q(\vec{q}) = C^*(\vec{q},\vec{\bar{r}}-\mat{R}(\vec{q})\vec{\bar{p}})
\end{equation}
Then we wish to show that $C^*_q(\vec{q})\ge C^*_q(\vec{\hat{q}})$ for any
$\vec{q}\in\mathbb{R}^4, ||\vec{q}||=1$. Let's first expand the expression for
$C^*_q(\vec{q})$
\begin{align}
C^*_q(\vec{q}) &= \sum_{i\in I} ||\mat{R}(\vec{q})\vec{p}_i
+\vec{\bar{r}}-\mat{R}(\vec{q})\vec{\bar{p}}-\vec{r}_i||^2
+ \sum_{i\notin I}l^2\nonumber\\
&=\sum_{i\in I} ||\mat{R}(\vec{q})(\vec{p}_i-\vec{\bar{p}})-(\vec{r}_i-\vec{\bar{r}})||^2
+ \sum_{i\notin I}l^2\nonumber\\
&=\sum_{i\in I}
(\mat{R}(\vec{q})(\vec{p}_i-\vec{\bar{p}})-(\vec{r}_i-\vec{\bar{r}}))^T
(\mat{R}(\vec{q})(\vec{p}_i-\vec{\bar{p}})-(\vec{r}_i-\vec{\bar{r}}))\nonumber\\
&\qquad+\sum_{i\notin I}l^2\nonumber\\
&=\sum_{i\in I}\left((\vec{p}_i-\vec{\bar{p}})^T\mat{R}^T(\vec{q})
-(\vec{r}_i-\vec{\bar{r}})^T\right)\nonumber\\
&\qquad\left(\mat{R}(\vec{q})(\vec{p}_i-\vec{\bar{p}})
-(\vec{r}_i-\vec{\bar{r}})\right)
+\sum_{i\notin I}l^2\nonumber\\
&=\sum_{i\in I}\left((\vec{p}_i-\vec{\bar{p}})^T\mat{R}^T(\vec{q})
\mat{R}(\vec{q})(\vec{p}_i-\vec{\bar{p}})\right.\nonumber\\
&\qquad\left.-2(\vec{r}_i-\vec{\bar{r}})^T\mat{R}(\vec{q})(\vec{p}_i-\vec{\bar{p}})
+(\vec{r}_i-\vec{\bar{r}})^T(\vec{r}_i-\vec{\bar{r}})\right)\nonumber\\
&\qquad+\sum_{i\notin I}l^2\nonumber\\
&=\sum_{i\in I}\left((\vec{p}_i-\vec{\bar{p}})^T(\vec{p}_i-\vec{\bar{p}})\right.\nonumber\\
&\qquad\left.-2(\vec{r}_i-\vec{\bar{r}})^T\mat{R}(\vec{q})(\vec{p}_i-\vec{\bar{p}})
+(\vec{r}_i-\vec{\bar{r}})^T(\vec{r}_i-\vec{\bar{r}})\right)\nonumber\\
&\qquad+\sum_{i\notin I}l^2\nonumber\\
&=\sum_{i\in I}(\vec{p}_i-\vec{\bar{p}})^T(\vec{p}_i-\vec{\bar{p}})\nonumber\\
&\qquad-2\sum_{i\in I}(\vec{r}_i-\vec{\bar{r}})^T\mat{R}(\vec{q})
(\vec{p}_i-\vec{\bar{p}})
+\sum_{i\in I}(\vec{r}_i-\vec{\bar{r}})^T(\vec{r}_i-\vec{\bar{r}})\nonumber\\
&\qquad+\sum_{i\notin I}l^2
\end{align}
Let us now concentrate on the term involving $\vec{q}$, the other terms are all
constant:
\begin{equation} \label{eq:core}
\begin{split}
&-2\sum_{i\in I}(\vec{r}_i-\vec{\bar{r}})^T\mat{R}(\vec{q})
(\vec{p}_i-\vec{\bar{p}})\\
&\qquad=-2|I|\left(\frac{1}{|I|}\sum_{i\in I}(\vec{r}_i-\vec{\bar{r}})^T\mat{R}(\vec{q})
(\vec{p}_i-\vec{\bar{p}})\right)\\
&\qquad=-2|I|\mbox{tr}\left(\frac{1}{|I|}\sum_{i\in I}(\vec{r}_i-\vec{\bar{r}})(\mat{R}(\vec{q})
(\vec{p}_i-\vec{\bar{p}}))^T\right)\\
&\qquad=-2|I|\mbox{tr}\left(\frac{1}{|I|}\sum_{i\in I}(\vec{r}_i-\vec{\bar{r}})
(\vec{p}_i-\vec{\bar{p}})^T\mat{R}^T(\vec{q})\right)\\
&\qquad=-2|I|\mbox{tr}\left(\mat{M}\mat{R}^T(\vec{q})\right)
\end{split}
\end{equation}
Here again we applied Lemma~\ref{thm:trace}, and the fact that $\mat{M} =
\mat{M}_{rp}-\vec{\bar{r}}\vec{\bar{p}}^T = \frac{1}{|I|}\sum_{i\in I}
(\vec{r}_i-\vec{\bar{r}}) (\vec{p}_i-\vec{\bar{p}})^T$. To see this, expand the
sum:
\begin{equation} \label{eq:M_matrix}
\begin{split}
\frac{1}{|I|}\sum_{i\in I}
(\vec{r}_i-\vec{\bar{r}}) (\vec{p}_i-\vec{\bar{p}})^T
&=\frac{1}{|I|}\sum_{i\in I}\left(\vec{r}_i\vec{p}_i^T-\vec{r}_i\vec{\bar{p}}^T
-\vec{\bar{r}}\vec{p}_i^T+\vec{\bar{r}}\vec{\bar{p}}^T\right)\\
&=\frac{1}{|I|}\sum_{i\in I}\vec{r}_i\vec{p}_i^T-\vec{\bar{r}}\vec{\bar{p}}^T
-\vec{\bar{r}}\vec{\bar{p}}^T+\vec{\bar{r}}\vec{\bar{p}}^T\\
&=\mat{M}_{rp}-\vec{\bar{r}}\vec{\bar{p}}^T = \mat{M}
\end{split}
\end{equation}
Using the standard conversion from a unit quaternion $\vec{q} = (w,\vec{v})$ to
a rotation matrix
\begin{equation}
\mat{R}(\vec{q}) = \left(w^2-||\vec{v}||^2\right)\mat{I}
+2\left(w[\vec{v}]_{\times}+\vec{v}\vec{v}^T\right)
\end{equation}
we have
\begin{equation}
\begin{split}
&\mat{R}(\vec{q})\\
&=\left[
\begin{array}{ccc}
w^2-v_1^2-v_2^2-v_3^2 & 0 & 0\\
0 & w^2-v_1^2-v_2^2-v_3^2 & 0\\
0 & 0 & w^2-v_1^2-v_2^2-v_3^2
\end{array}
\right]\\
&\qquad+2\left(w\left[
\begin{array}{ccc}
0 & -v_3 & v_2\\
v_3 & 0 & -v_1\\
-v_2 & v_1 & 0
\end{array}
\right]
+\left[
\begin{array}{ccc}
v_1^2 & v_1v_2 & v_1v_3\\
v_2v_1 & v_2^2 & v_2v_3\\
v_3v_1 & v_3v_2 & v_3^2
\end{array}
\right]\right)\\
&=\left[
\begin{array}{ccc}
w^2-v_1^2-v_2^2-v_3^2 & 0 & 0\\
0 & w^2-v_1^2-v_2^2-v_3^2 & 0\\
0 & 0 & w^2-v_1^2-v_2^2-v_3^2
\end{array}
\right]\\
&\qquad+\left[
\begin{array}{ccc}
0 & -2wv_3 & 2wv_2\\
2wv_3 & 0 & -2wv_1\\
-2wv_2 & 2wv_1 & 0
\end{array}
\right]
+\left[
\begin{array}{ccc}
2v_1^2 & 2v_1v_2 & 2v_1v_3\\
2v_2v_1 & 2v_2^2 & 2v_2v_3\\
2v_3v_1 & 2v_3v_2 & 2v_3^2
\end{array}
\right]\\
&=\left[
\begin{array}{ccc}
w^2+v_1^2-v_2^2-v_3^2 & 2(v_1v_2-wv_3) & 2(v_1v_3+wv_2)\\
2(v_2v_1+wv_3) & w^2-v_1^2+v_2^2-v_3^2 & 2(v_2v_3-wv_1)\\
2(v_3v_1-wv_2) & 2(v_3v_2+wv_1) & w^2-v_1^2-v_2^2+v_3^2
\end{array}
\right]
\end{split}
\end{equation}
Substituting this result into $\mbox{tr}(\mat{M}\mat{R}^T(\vec{q}))$, we get
\begin{align}
&\mbox{tr}(\mat{M}\mat{R}^T(\vec{q}))\nonumber\\
&=\sum_{i=1}^3\sum_{j=1}^3M_{ij}R_{ij}(\vec{q})\nonumber\\
&=M_{11}(w^2+v_1^2-v_2^2-v_3^2)+2M_{12}(v_1v_2-wv_3)
+2M_{13}(v_1v_3+wv_2)\nonumber\\
&\qquad+2M_{21}(v_2v_1+wv_3)+M_{22}(w^2-v_1^2+v_2^2-v_3^2)
+2M_{23}(v_2v_3-wv_1)\nonumber\\
&\qquad+2M_{31}(v_3v_1-wv_2)+2M_{32}(v_3v_2+wv_1)
+M_{33}(w^2-v_1^2-v_2^2+v_3^2)\nonumber\\
&=(M_{11}+M_{22}+M_{33})w^2+2(-M_{23}+M_{32})wv_1
+2(M_{13}-M_{31})wv_2\nonumber\\
&\qquad+2(-M_{12}+M_{21})wv_3+(M_{11}-M_{22}-M_{33})v_1^2
+2(M_{12}+M_{21})v_1v_2\nonumber\\
&\qquad+2(M_{13}+M_{31})v_1v_3+(-M_{11}+M_{22}-M_{33})v_2^2\nonumber\\
&\qquad+2(M_{23}+M_{32})v_2v_3+(-M_{11}-M_{22}+M_{33})v_3^2\nonumber\\
&=\vec{q}^T\mat{Q}\vec{q}
\end{align}
where $\mat{Q}$ is the matrix defined in Algorithm~\ref{algo:closed_form}. Now
because $\mat{Q}$ is real symmetric, Lemma~\ref{thm:max_quadratic_form} applies,
and we have
\begin{equation}
\forall(\vec{q}\in\mathbb{R}^4,||\vec{q}||=1),
\vec{q}^T\mat{Q}\vec{q}\le\vec{\hat{q}}^T\mat{Q}\vec{\hat{q}}
\end{equation}
This implies
\begin{equation}
\forall(\vec{q}\in\mathbb{R}^4,||\vec{q}||=1),
C^*_q(\vec{q})\ge C^*_q(\vec{\hat{q}})
\end{equation}
Hence $\forall(T=(\vec{q},\vec{t})\in\mbox{SE}(3))$,
\begin{equation}
\begin{split}
C^*(T)&=C^*(\vec{q},\vec{t})
\ge C^*(\vec{q},\vec{\bar{r}}-\mat{R}(\vec{q})\vec{\bar{p}})\\
&=C^*_q(\vec{q})\ge C^*_q(\vec{\hat{q}})
=C^*(\vec{\hat{q}},\vec{\bar{r}}-\mat{R}(\vec{\hat{q}})\vec{\bar{p}})\\
&=C^*(\vec{\hat{q}},\vec{\hat{t}}) = C^*(\hat{T})
\end{split}
\end{equation}
\end{proof}

\bibliographystyle{plain}
\bibliography{aligner}

\end{document}
