NOTES | HOME

$$ \newcommand{\RR}{\mathbb{R}} \newcommand{\GG}{\mathbb{G}} \newcommand{\PP}{\mathbb{P}} \newcommand{\PS}{\mathcal{P}} \newcommand{\SS}{\mathbb{S}} \newcommand{\NN}{\mathbb{N}} \newcommand{\ZZ}{\mathbb{Z}} \newcommand{\CC}{\mathbb{C}} \newcommand{\HH}{\mathbb{H}} \newcommand{\ones}{\mathbb{1\hspace{-0.4em}1}} \newcommand{\alg}[1]{\mathfrak{#1}} \newcommand{\mat}[1]{ \begin{pmatrix} #1 \end{pmatrix} } \renewcommand{\bar}{\overline} \renewcommand{\hat}{\widehat} \renewcommand{\tilde}{\widetilde} \newcommand{\inv}[1]{ {#1}^{-1} } \newcommand{\eqdef}{\overset{\text{def}}=} \newcommand{\block}[1]{\left(#1\right)} \newcommand{\set}[1]{\left\{#1\right\}} \newcommand{\abs}[1]{\left|#1\right|} \newcommand{\trace}[1]{\mathrm{tr}\block{#1}} \newcommand{\norm}[1]{ \left\| #1 \right\| } \newcommand{\argmin}[1]{ \underset{#1}{\mathrm{argmin}} } \newcommand{\argmax}[1]{ \underset{#1}{\mathrm{argmax}} } \newcommand{\st}{\ \mathrm{s.t.}\ } \newcommand{\sign}[1]{\mathrm{sign}\block{#1}} \newcommand{\half}{\frac{1}{2}} \newcommand{\inner}[1]{\langle #1 \rangle} \newcommand{\dd}{\mathrm{d}} \newcommand{\ddd}[2]{\frac{\partial #1}{\partial #2} } \newcommand{\db}{\dd^b} \newcommand{\ds}{\dd^s} \newcommand{\dL}{\dd_L} \newcommand{\dR}{\dd_R} \newcommand{\Ad}{\mathrm{Ad}} \newcommand{\ad}{\mathrm{ad}} \newcommand{\LL}{\mathcal{L}} \newcommand{\Krylov}{\mathcal{K}} \newcommand{\Span}[1]{\mathrm{Span}\block{#1}} \newcommand{\diag}{\mathrm{diag}} \newcommand{\tr}{\mathrm{tr}} \newcommand{\sinc}{\mathrm{sinc}} \newcommand{\cat}[1]{\mathcal{#1}} \newcommand{\Ob}[1]{\mathrm{Ob}\block{\cat{#1}}} \newcommand{\Hom}[1]{\mathrm{Hom}\block{\cat{#1}}} \newcommand{\op}[1]{\cat{#1}^{op}} \newcommand{\hom}[2]{\cat{#1}\block{#2}} \newcommand{\id}{\mathrm{id}} \newcommand{\Set}{\mathbb{Set}} \newcommand{\Cat}{\mathbb{Cat}} \newcommand{\Hask}{\mathbb{Hask}} \newcommand{\lim}{\mathrm{lim}\ } \newcommand{\funcat}[1]{\left[\cat{#1}\right]} \newcommand{\natsq}[6]{ \begin{matrix} & #2\block{#4} & \overset{#2\block{#6}}\longrightarrow & #2\block{#5} & \\ {#1}_{#4} \hspace{-1.5em} &\downarrow & & \downarrow & \hspace{-1.5em} {#1}_{#5}\\ & #3\block{#4} & \underset{#3\block{#6}}\longrightarrow & #3\block{#5} & \\ \end{matrix} } \newcommand{\comtri}[6]{ \begin{matrix} #1 & \overset{#4}\longrightarrow & #2 & \\ #6 \hspace{-1em} & \searrow & \downarrow & \hspace{-1em} #5 \\ & & #3 & \end{matrix} } \newcommand{\natism}[6]{ \begin{matrix} & #2\block{#4} & \overset{#2\block{#6}}\longrightarrow & #2\block{#5} & \\ {#1}_{#4} \hspace{-1.5em} &\downarrow \uparrow & & \downarrow \uparrow & \hspace{-1.5em} {#1}_{#5}\\ & #3\block{#4} & \underset{#3\block{#6}}\longrightarrow & #3\block{#5} & \\ \end{matrix} } \newcommand{\cone}[1]{\mathcal{#1}} $$

Procrustes

Quick notes about (semi-) rigid point registration.

Problem

Given two sets of $n$ 3-dimensional points $\block{x_i}_i, \block{y_i}_i \in \RR^3$, we want to find the best rigid registration i.e. a rigid transform $\mat{R & t \\ 0 & 1} \in SE(3)$ that minimizes the squared norm error:

\[\min_{R \in SO(3), t \in \RR^3} \quad \sum_i \norm{Rx_i + t - y_i}^2\]

Analysis

If we let $X, Y \in \mathcal{M}_{3, n}$ be the matrices of column vectors $x_i, y_i$, we get the following matrix form:

\[\min_{R \in SO(3), t \in \RR^3} \quad \underbrace{\norm{RX + t 1^T - Y}}_{E(R, t)}{}^2\]

Expanding the Frobenius norm, we get:

\[\begin{aligned} E(R, t) &= \tr\block{\block{RX + t1^T - Y}^T\block{RX + t1^T - Y}} \\ &= \tr\block{X^TX} + 2 \tr\block{X^TR^T\block{t1^T - Y}} + \tr\block{\block{t1^T - Y}^T\block{t1^T - Y}} \\ \end{aligned}\]

Dropping the constant term $\tr\block{X^TX}$, let us focus on the second term:

\[\begin{aligned} \tr\block{X^TR^T\block{t1^T - Y}} &= \tr\block{X^TR^Tt1^T} - \tr\block{X^TR^TY} \\ &= \tr\block{1^TX^TR^Tt} - \tr\block{\block{YX^T}^T R} \end{aligned}\]

We notice that $X 1 = \sum_i x_i$ so if we choose coordinates so that $X 1 = 0$ (which is always possible by centering around the mean), then the first term disappears and we’re left with a linear form in $R$. The total energy is then reduced to the following separable energy:

\[E(R, t) = -\tr\block{\block{YX^T}^T R} + \tr\block{\block{t1^T - Y}^T\block{t1^T - Y}}\]

Solving

The translation term corresponds to the following linear least-squares in $t$:

\[\min_t \norm{1 t^T - Y^T}^2\]

whose solution is given by the usual normal equations:

\[1^T1t^T = 1^TY^T\]

That is:

\[n t = \sum_i y_i\]

The rotation term is a linear form to be optimized on $SO(3)$:

\[\min_{R \in SO(3)} -\tr\block{\block{YX^T}^T R}\]

This is the same problem as the matrix projection onto $SO(3)$:

\[\min_{R \in SO(3)} \norm{F - R}^2 = \min_{R \in SO(3)} -\tr\block{F^TR}\]

with $F = YX^T$. Using the Singular Value Decomposition of $F = USV^T$ (flipping singular value signs¹ to get both $U, V \in SO(3)$, and assuming that the flipped $S \neq I$ otherwise $F \in SO(3)$ already), we obtain the following equivalent problem:

\[\min_{Q \in SO(3)} -\tr\block{S^TQ}\]

where $Q = V^T R U$. From there, two options:

express the gradient on $SO(3)$ tangent vectors and make this gradient zero,
introduce Lagrange multipliers for the constraint $Q \in SO(3)$ and solve the KKT system.

First Option: $SO(3)$ tangent vectors

Using left trivialization (body-fixed coordinates) we get $\dd Q = Q \omega$ where $\omega \in \alg{so(3)}$ is skew-symmetric, giving the following first-order conditions:

\[\forall \omega \in \alg{so(3)}: \tr\block{S^TQ\omega} = 0\]

which implies that $Q^TS$ must be symmetric (i.e. orthogonal to all skew-symmetric matrices).

Second Option: Lagrange multipliers

Introduce $\lambda \in \mathcal{M}_{3,3}$ and compute their pullback by the constraint equation:

\[Q^T Q = I\]

Differentiating the contraint equation gives:

\[\dd Q^T Q + Q^T \dd Q = 0\]

Pullback the multipliers:

\[\begin{aligned} \tr\block{\lambda^T \block{\dd Q^T Q + Q^T \dd Q}} &= \tr\block{\lambda Q^T \dd Q} + \tr\block{\lambda^T Q^T \dd Q} \\ &= \tr\block{\block{Q\block{\lambda + \lambda^T}}^T \dd Q} \\ \end{aligned}\]

whose inner product left-hand side gives the multiplier pullback. The objective function gradient is just $S$, therefore the first-order conditions are:

\[S = Q\block{\lambda + \lambda^T}\]

or, equivalently, that $Q^TS$ must be symmetric (as before).

Solution

It turns out that the only rotation making $Q^TS$ symmetric is $Q = I$, which means that $R = VU^T$. See also the page on quaternions for an alternate algorithm that finds the solution orientation directly as a quaternion.

Summary

Assuming $\sum_i x_i = 0$, the problem becomes separable and its solution is given by:

\[\begin{aligned} R &= VU^T \\ t &= \frac{1}{n} \sum_i y_i\\ \end{aligned}\]

Uniform Scaling

If we allow an extra scaling parameter $s > 0$ to the problem, we get the following matrix form:

\[\min_{s > 0, R \in SO(3), t \in \RR^3} \quad \underbrace{\norm{sRX + t 1^T - Y}}_{E(s, R, t)}{}^2\]

which we expand as before as:

\[\begin{aligned} E(s, R, t) &= \tr\block{\block{sRX + t1^T - Y}^T\block{sRX + t1^T - Y}} \\ &= s^2\tr\block{X^TX} + 2 s\tr\block{X^TR^T\block{t1^T - Y}} + \tr\block{\block{t1^T - Y}^T\block{t1^T - Y}} \\ \end{aligned}\]

As before, the term $\tr\block{X^TR^Tt1^T}$ vanishes when $X1 = 0$, and the translation part becomes separable again with the same solution as before. The scale/rotation part remains:

\[E(s, R) = s^2\ \tr\block{X^TX} - 2 s\ \tr\block{F^TR}\]

where $F=YX^T$. As before, we change variables with $Q=UV^T$ given the decomposition $F = UDV^T$ (again, flipping signs so that both $U$ and $V$ are rotations²) and obtain:

\[E(s, Q) = s^2\ \tr\block{X^TX} - 2 s\ \tr\block{D^TQ}\]

whose differential is given by:

\[\dd E(s, Q).\dd s.\dd Q = 2s\ \dd s\ \tr\block{X^TX} - 2\dd s\ \tr\block{D^TQ} - 2 s\ \tr\block{D^T\dd Q}\]

Considering only admissible tangent vectors $\dd s = s v$ and $\dd Q = Q \omega$ for $v \in \RR$ and $\omega \in \alg{so(3)}$, we get the first-order stationary conditions:

\[\forall v \in \RR, \omega \in \alg{so(3)}: v \block{s\ \tr\block{X^TX} - \tr\block{D^TQ}} - \tr\block{D^TQ\omega} = 0\]

As before, $Q^TD$ must be symmetric which is only possible if $Q = I$, and we get the scale factor as:

\[s = \frac{\tr\block{D}}{\tr\block{X^TX}}\]

Note that it should always be possible to flip/swap singular values¹² so that $s > 0$.

TODO

introduce non-uniform scale?

Notes

If neither $U$ not $V$ are rotations, just swap two singular values and update $U$, $V$ accordingly. If either $U$ or $V$ is already a rotation, a single singular value sign change (updating $U$ or $V$ accordingly) should do the trick. ↩ ↩²
In order to always get a positive scaling value $s$, the smallest singular value sign should be flipped so that $\tr\block{D} > 0$. ↩ ↩²