% Multiple Point Sets Registration Paper
%
% Jan Kybic <xkybic@sun.felk.cvut.cz>
%

\input pstricks
\input pst-node

% define paper size
% \input plaina4
\hsize= 159.2 mm
\vsize= 239.2 mm
{\catcode64=11                          % \catcode`\@=11
\gdef\m@g{\mag\count@
  \hsize=159.2truemm \vsize=239.2truemm \dimen\footins8truein}
}

% setup fonts
\font\rmf=cmr12
\font\bf=cmbx12
\font\it=cmti12
\font\tt=cmtt12
\font\bit=cmbxti10 scaled \magstep2
\font\secfont=cmssbx10 scaled \magstep3
\font\title=cmssdc10 scaled \magstep5
\font\ssbx=cmssbx10 scaled \magstep1
\font\big=cmr17
\font\rmeight=cmr8
\font\twit=cmmi12
\font\iteight=cmmi8
\font\twsy=cmsy10 scaled \magstep1
\font\eightsy=cmsy8
\font\twex=cmex10 scaled \magstep1

\textfont0=\rmf
\scriptfont0=\rmeight
\scriptscriptfont0=\sevenrm
\textfont1=\twit
\scriptfont1=\iteight
\scriptscriptfont1=\seveni
\textfont2=\twsy
\scriptfont2=\eightsy
\scriptscriptfont2=\sevensy
\textfont3=\twex \scriptfont3=\twex \scriptscriptfont3=\twex


% setup parameters
\def\rm{\fam0 \rmf} \rm
\baselineskip=14.4pt
\parindent=4em
\pretolerance=300 \tolerance=600 \emergencystretch=0.5em
\pageno=1
\footline={\hss\rm{--- \folio{} ---}\hss}
\smallskipamount=1\baselineskip plus 0.25\baselineskip
\medskipamount=2\baselineskip plus 1\baselineskip
\bigskipamount=3\baselineskip plus 2\baselineskip

% front page will be made separately
\if0
{\footline{}
\centerline{\big Department of Control Engineering, Czech Technical
University}
\smallskip
\centerline{\big Computer Vision Laboratory}
\vskip 0pt plus .1fil
\centerline{\title Multiple Point Sets Registration}
\bigskip
\centerline{\big Jan Kybic (xkybic@sun.felk.cvut.cz)}
\bigskip
\centerline{\big December 1996}
\vskip 0pt plus .3fil
\eject
}
\fi
% some macros
\newcount\secno \secno=1
\outer\def\sec#1\par{\medbreak\leftline{\secfont\the\secno. #1}%
 \xdef\writeit{
  \write\obs{\noexpand\noexpand\noexpand\obsline 
    \noexpand\noexpand\the\secno . #1.
    \noexpand\the\pageno .}}
 \writeit
\advance\secno by 1%
\nobreak\smallskip\nobreak\noindent}
\def\abstract#1{{\narrower\leftline{%
 \hskip\parindent\bit Abstract}\noindent\it#1\par}}
\def\cit#1{[#1]}
%\def\bitem{\hang\relax\indent\llap{$\bullet$ }}
%\def\bitem#1\par{{\vskip 6pt plus 2pt\narrower\noindent #1\par
%\vskip 6pt plus 2pt}}
\def\bitem#1\par{{\vskip 6pt plus 2pt\hang\relax\indent #1\par
\vskip 6pt plus 2pt}}

\def\real{\hbox{\ssbx R}}
\def\argmin{\mathop{\hbox{arg min}}}
\def\d{{\rm d}}
\def\rf#1#2{\line{\vtop{\hbox to 3em{[#1]\hfil}}%
\vtop{\advance\hsize by -3em \noindent #2\par}}%
\smallskip}

%Contents
\centerline{}\bigskip
\leftline{\secfont Contents}
\bigskip
\def\obsline#1. #2. #3.{%
 \line{\hbox to 1.5em{\bf #1.\hfil} #2\dotfill
       \hbox to 1.5em{\hfil #3}}%
}

{\baselineskip=2\baselineskip\par
\newread\contfile \immediate\openin\contfile=mreg.obs
\ifeof\contfile
 \immediate\closein\contfile
 No contents file created yet.
\else
 \immediate\closein\contfile
 \input mreg.obs
\fi}\par

\vskip 0pt plus 1fil
\eject
\newwrite\obs \immediate\openout\obs=mreg.obs

% Abstract 
\abstract{Registering 3D point sets subject to rigid body
transformations is a common problem in computer vision. It occurs in areas
like building 3D model from multiple range images and estimation of motion.
In this report we shall present the problem and the knowledge we
obtained in implementing, developing, combining and testing an algorithm
for simultaneous registration of multiple partially corresponding surfaces
represented as point sets.}


\sec Introduction

To explain the virtue of the problem, let us imagine that we are to obtain a
surface model of some complex, free-form rigid object. We may suppose that we
have been able to get a set of 3D images, called {\it point sets\/}, by
means of some range sensor. However, due to the physical limitations of the
sensing method, none of the images contains information about the whole
surface. We usually have some information associated with each picture,
about the mutual position of the object and the sensor but it is rarely
accurate enough. As our task is to combine all the images into one model of
the entire object surface, the first step is to calculate the exact
relationship between the images. This first step is often called {\it
registration} and it will be the main topic of this paper.

It is surprisingly difficult to define what should be considered a good
registration. A human being performing the same task, i.e. assembling a
picture from a set of partly overlapped sub-pictures, usually keeps shifting
the sub-pictures until the overlapped parts visually match, or at least
resemble each other. To perform the visual matching, human perception and
brain automatically reduce the complex picture on a relatively few simple
patterns that are then easy to match. 

Unfortunately, computer vision algorithms have not yet achieved the degree
of proficiency in pattern finding, recognition and matching needed to
effectively simulate the human approach. Instead, a simplified procedure is
used that generally comprises the following two steps:

\item{(1)} Identify image points in all the images that correspond to
a single model point, i.e. {\it find the correspondences\/} between the
point sets.

\item{(2)} Find a set of {\it rigid body transformations\/} 
(i.e. consisting of rotations and translations) to be applied to  
each image, leading to the best unification of the common image points.

\noindent Point (1) is easier to accomplish if there are some significant
points on the model we can identify. However, this is rarely the case, as
these {\it feature points\/} are usually difficult to find. Many different
approaches for finding feature points are currently being developed
exploiting different properties of the examined surfaces such as structural
patterns or curvature. Alternatively the correspondence can be established
on the basis of the Iterative Closest Point (ICP) algorithm (\cit{7})
described bellow.

Point (2) can be solved quickly using closed form formula in the case of
two point sets (\cit{6}). For more point sets, no such formula seems to exist, we
therefore have to employ an iterative algorithm presented bellow (\cit{2}).  

\sec Iterative Closest Point Algorithm

ICP (\cit{7}) attempts to find  correspondences between two point sets, i.e.
for each point in one of the sets (called {\it data\/}) find a corresponding
point ({\it partner\/}) from the second set (called {\it model}). The
ultimate goal would be to find a transformation $Q_{\infty}$ (the subscript
denotes the number of iterations, $\infty$ represents the limit) mapping
each data point to its partner. Let us assume for start that such a
transformation exists and that we have an initial estimate $Q_0$ of this
transformation. If the estimate is sufficiently good, the transformed data
points will be closer to their partners than to any other point. From this
observation directly emerges the basic principle of the ICP algorithm,
corresponding to point (1) above:

\bitem For each point $x$ from the data set transformed by $Q_0$
find the closest point $y$ from the model data set. Then label $y$ as $x$'s
partner, symbolically $y=P_0(x)$,  
$ P_0(x)=\hbox{arg min}_y \;\| Q_0 (x) - y \|$.

\noindent
Although our initial estimate $Q_0$ of the transformation $Q_{\infty}$
is rarely good enough for the labeling to be fully correct, 
we may still use it to

\bitem Determine a transformation $Q_1$ minimizing distances
 between partner points (point
(2) above), i.e. $Q_1=\hbox{arg min}_Q \,e$, 
$e=\sum_i\, \| Q(x_i)-P(x_i) \|^2$

\noindent
To further improve the transformation ($Q_1$) 
we shall apply the process {\it iteratively\/},
i.e. use $Q_1$ to obtain a new labeling $P_1$, use the new labeling 
$P_1$ to get $Q_2$ etc. The iterative process may be stopped after a predetermined 
number of iterations or better when the average square error  
falls bellow a prescribed precision limit.  

If $Q_{\infty}$ exists and $Q_0$ was sufficiently close, the process
converges quickly. As $Q_0$ moves away from $Q_{\infty}$ the
convergence slows down but is usually acceptable. 
For data coming from measurement $Q_{\infty}$ does not exist due to noise
and resolution limitation. 
However, by increasing the resolution we improve the likelihood of finding 
a good approximation.

\sec Iterative Reciprocal Closest Point Algorithm

The classical ICP algorithm is well suited for matching two images resulting
from transformation of the same original model point set. Such two sets
correspond entirely to each other. However, in our case, we have point sets
corresponding only in limited overlapping regions. To obtain the correct
matching we should calculate the transformation $Q$ only from points within
the overlapping region. The key question is, how to determine whether a
given point $x$ from the first point set belongs to the overlapping region,
i.e. whether it has a partner $y$ from the model set. Even if some point
$x_1$ has
in fact no partner $y$, the ICP algorithm would still find $y=P(x_1)$ as the
closest (although not close) point from the model set. The remedy is to
temporarily {\it revert the roles\/} of the two sets and find the closest 
$x_2$ for
a given $y$ from the data set. If $x_1$ and $y$ were real partners, then
$x_2=x_1$, otherwise $x_2 \not = x_1$. Allowing for the inaccuracy, we
permit $x$ and $x'$ to be `close' (not necessarily identical). Ideally, 
a geodetic distance between $x$ and $x'$ should be measured. However, for
close points, Euclidean distance is a good approximation and much easier to
compute, so we shall use it instead.
The observation we have just described makes the core of
the Iterative Reciprocal Closest Point (IRCP) algorithm (\cit{8}):

\bitem $y$ is labeled $x$'s partner if and only if the data point closest to $y$ 
when transformed is closer to  $x$ than some predetermined threshold $\varepsilon$.
Symbolically $$y=P(x) \;  \Longleftrightarrow \; 
\Biggl\Vert x - \mathop{\hbox{arg min}}_{x'} \; 
\Bigl\Vert Q(x') - \mathop{\hbox{arg min}}_y \, \| Q(x) - y \| \, \Bigr\Vert 
\; \Biggr\Vert < \varepsilon$$

\noindent 
To determine the optimal transformation $Q$ in the second step we naturally
use only those data points that have partners. In this way we eliminate
points not having partners in the other set. This helps us to find the
correct matching, i.e. matching with respect to the corresponding
(overlapping) parts only.

Although we shall go on calling the point sets {\it data\/} respective {\it
model\/}, please note that in the ICRP algorithm both point sets are 
equivalent as far as results are concerned. So far, data point set is the
set being transformed but this will not be the case later on when {\it
both\/} sets will be transformed.

The ICRP algorithm is generally much less robust than the original ICP algorithm and 
much more prone to be mislead to an unwanted local minimum by a wrong estimate 
$Q_0$ of the ideal transformation. It is therefore strongly recommended, mainly when no 
good estimate $Q_0$ is available, to start the algorithm several times with different initial
$Q_0$. When no estimate is available at all, a set of uniformly laid out rotations
can be used. 

The performance of the ICRP algorithm is also strongly influenced  by the choice of 
the threshold $\varepsilon$. When set too high, too few points will be eliminated and 
the calculated transformation will be adversely influenced by points not belonging 
to the common region. When set too low, on the other hand, too many points will be eliminated, 
resulting in $Q$ being calculated from  too few points and therefore suffering from 
noise and error. 

It is generally difficult to find a good criteria as to when to stop the  ICRP algorithm 
as because of the changing number of points considered, the average square error function is 
not monotonously decreasing with time. A solution might be to compare the average error 
for an {\it identical\/} set of point pairs for successive transformations $Q_i$ and $Q_{i+1}$.
Alternatively the computation can be stopped after the average error has not changed 
significantly in a certain number of iterations.

The convergence properties of the ICRP algorithm are yet to be
studied. The number of iterations depends in a complicated way on the
initial transformation and also on the spatial distribution of the two point
sets. Therefore, no exact measure of the time consumption of the ICRP
algorithm can be given here, however it is evident that the algorithm is
inherently slower than the ICP algorithm as the closest point search must be
performed twice as many times and also because its convergence is much
slower.

\sec Closest Point Search

A simplified study on the speed of the ICRP algorithm reveals that an efficient
implementation of the closest point search, forming the innermost part of
the ICRP algorithm, is of a paramount importance. Let us briefly restate the
problem:

\bitem Given a set of points $Y=\{y_1,y_2,\ldots,y_n\} \subset \real^3$ and
a point $x$.\hfill\break\hang{}%
Find~\hbox{$y=\argmin_{y \in Y} \, \| x - y\, \|$.} For one set $Y$
many points $x$ will be queried, so it is feasible to use preprocessing.

\noindent 
A number of approaches is available, ranging from the simplest
conceivable method going sequentially through all the points in $Y$ (which
takes $O(n)$ time) up to some very clever methods based on Voronoi diagram
(\cit{3})
(which take only $O(\log n)$ time per query). However, as the task of
effectively constructing and applying Voronoi diagram in 3D is by no means
an easy one, we have deferred implementing the probably optimal solution and
have used simpler {\it range search tree\/} (\cit{4}) based method that has a
worst-case performance $O(n)$ but on the average takes also $O(\log n)$
time. According to our present knowledge the exact behavior of this
algorithm has yet to be fully analyzed and the average case property stated
above is only an estimate based on the algorithm structure. An empirical
evidence shows great dependence of the processing time on the spatial
distribution of $Y$ and the position of $x$.

During the preprocessing phase an ideally balanced binary search tree is
constructed having in each node one point from $Y$. The number of left
descendants of each node differs at most by one from the number of its right
descendants. Furthermore, each node has a {\it key coordinate\/} and it
holds that each of the left descendants is less or equal in this key
coordinate, while all the right descendants are greater or equal in this
coordinate. (See picture.) The key coordinate can be altered regularly from level to level,
we obtained slightly better results by choosing the coordinate with the
greatest difference between the maximum and minimum values among all the
descendants to be.

\topinsert
\centerline{%
\pspicture(1,-1)(7,3)
      \psset{arrows=-}
      \psset{nodesep=2pt}
      \psset{linearc=5pt}
      \rput(4,3){\rnode{A}{\psframebox{6,{\bf 7},2}}}
      \rput(2,1){\rnode{B}{\psframebox{3,1,{\bf 9}}}}
      \rput(6,1){\rnode{C}{\psframebox{8,{\bf 2},3}}}
      \rput(1,-1){\rnode{D}{\psframebox{5,{\bf 3},8}}}
      \rput(3,-1){\rnode{E}{\psframebox{4,{\bf 2},9}}}
      \rput(5,-1){\rnode{F}{\psframebox{9,{\bf 1},1}}}
      \rput(7,-1){\rnode{G}{\psframebox{{\bf 7},9,5}}}
      \ncline{A}{B} \ncline{A}{C} \ncline{B}{D} \ncline{B}{E}
      \ncline{C}{F} \ncline{C}{G}
\endpspicture}
\smallskip
\centerline{\it Binary search tree, key coordinates set in \bf bold face}
\medskip
\endinsert

Let us begin with a simple problem first. To find a point that is guaranteed
to belong to $Y$ in $O(\log n)$ time we can simply traverse the tree starting
from the root, comparing the key coordinates of the query point and of the
point in the actual node, proceeding to left or right child accordingly.

A more complex problem is to find all points falling in a given rectangle
$\{y=(a,b,c) \in Y \;\vert \;a \in \langle a_1 ; a_2\rangle\,,\, b \in
\langle b_1 ; b_2\rangle\,,\, c \in \langle c_1 ; c_2\rangle\}$ --- {\it
range search\/}. For this purpose a breadth-first search beginning from the
tree root can be used where we shall descend only the branches that may
contain some of the points searched. More precisely if a key coordinate of a
node falls within the desired interval, we shall search both sub-trees, if
it is less than the lower boundary or greater than the upper boundary we
shall search only the right or left sub-tree respectively. Experiments show
that this algorithm seem to perform about $O(r+\log n)$ operations, where
$r$ is the number of points found. Additional small performance gain can be
obtained by storing information about the smallest and largest points in key
coordinates within left and right sub-trees respectively in each node.

Finally, let us show how to use the tree to speed up the closest point
search. The algorithm used is very similar to the range search described
above. At the very beginning we consider the rectangle to be infinitely
large in all dimensions. Later on, as we proceed with the search, we limit
the rectangle to 
$\{ y=(a,b,c) \in Y \;\vert \;a \in \langle x_1-\delta ; x_1+\delta\rangle\,,\, 
b \in \langle x_2-\delta ; x_2+\delta\rangle\,,\, 
c \in \langle x_3-\delta ; x_3+\delta\rangle\}$, where $x=(x_1,x_2,x_3)$
is the query point we search for and $\delta=\min_i \left| x-y_i\right|$
is the minimum of distances between $x$ and points from $Y$ encountered so
far. We can do so, because no points laying {\it outside\/} this rectangle
can be closer to $x$ than $\delta$. This way, we gradually make the search 
rectangle shrink and limit the number of points in question. 

Ideally, only a small percentage of the points is searched, in some cases it
was less than 60~points out of 5000. However, it can also happen that a node
is so far away from $x$ that no points can be eliminated. If this happens
for all the $n$ nodes, we need to examine all of them. This accounts for the
worst case of $O(n)$ time needed.

\sec Registration of Multiple Point Sets

In the preceding sections we have described the apparatus used to register
two point sets. The fundamental difference of the multiple set problem is
that to solve point (2) --- finding the optimal rigid body transformation
--- no analytic method is known and thus numerical solution must be used.
However, the numerical method is still faster than finding the mutually 
corresponding points (point (1)).

We assume that there are $m$ point set $Y^1,\ldots,Y^m$, each forming (after
being properly transformed by a rigid body transformation $Q^i_{\infty}$) a
part of the surface of the same object. Let us denote the points belonging
to the set $Y^i=\{y^i_1,y^i_2,\ldots,y^i_{n(i)}\}$. 
% 
Assume an algorithm such as ICRP has been run for each pair $Y^i,Y^j, \>
i\not=j$ to obtain pairwise correspondences $P_0$ using the initial
estimates of the transformations $Q^i_0$, $Q^j_0$. Symbolically if $y^i_k
\in Y_i$ is a partner of $y^j_l \in Y_j$, we shall write $y^i_k = P ^{(i,j)}
(y^j_l)$ and $y^j_l = P ^{(j,i)} (y^i_k)$, as the partnership is a
symmetrical relation.
  
Now, the task is to find such a set of transformation 
$Q^1_1,Q^2_1,\ldots,Q^m_1$ that the total sum of weighted Euclidean
distances between partners is minimal, i.e.
$$ (Q^1_1,\ldots,Q^m_1)=\argmin_{(Q^1_1,\ldots,Q^m_1)} e_{tot}\;,
\hskip 2em
e_{tot}=\sum^m_{i=1} \sum^m_{j=i+1} e(i,j)$$
$$ \eqalign{e(i,j)&=\sum^{n(i)}_{k=1\atop P^{(j,i)}_0(y^i_k)\not=\oslash} \left\|
Q^i_0\Bigl(y^i_k\Bigr) - Q^j_0\Bigl(P^{(j,i)}_0(y^i_k)\Bigr) \right\|^2=\cr
&=\sum^{n(j)}_{l=1\atop P^{(i,j)}_0(y^j_l)\not=\oslash} \left\|
Q^j_0\Bigl(y^j_l\Bigr) - Q^i_0\Bigl(P^{(i,j)}_0(y^j_l)\Bigr) \right\|^2}$$
where the notation under the summations mean: ``Sum over all points from a
given set having partners in the other set.''

A useful model of the situation appears when we imagine a small spring
between each pair of corresponding points. The total difference $e_{tot}$
plays the role of the total {\it potential energy\/} the system has. From
this physical analogy we know that the equilibrium exists. In order to find
it we let the system evolve exactly as if it were a real physical system.

We shall define an useful notation shortcut to denote an average value:
\hbox{$\langle x \rangle = \sum_{x \in X} x \>\big/\> N$.}
($N$ is the number of members of $X$.) 
Similarly, the average of points from $Y^i$ having partners in $Y^j$ will be denoted as
$\langle y^i \rangle^{(j,i)}$. 

Let us define a {\it center of mass\/} for each point set. It will later
serve us as a turning point around which the set will rotate. It could be
set to an arbitrary point in space but an experience shows that good
results are obtained by following the physical analogy and setting 
$y^i_{CM}= \langle y^i \rangle$.

Finally, let us assume that the transformation $Q^i$ can be written
as $$Q^i(x) = R^i(x) + r^i\eqno{(*)}$$
where $R^i$ is a $3\times3$ rotation matrix and
is a $r^i$ vector of translation which permits us to express $e_{tot}$
as:
$$e_{tot}=\sum^m_{i=1} \sum^m_{j=i+1}
\sum^{n(i)}_{k=1\atop P^{(j,i)}_0(y^i_k)\not=\oslash} \left\|
R^i \Bigl(y^i_k\Bigr) + r^i - 
R^j \Bigl(P^{(j,i)}_0(y^i_k)\Bigr) - r^j \right\|^2
$$

The force acting on the set $Y^i$ due to the set $Y^j$ may be computed as:
$$ F^{(i,j)}=- \sum_{k=1}^{n(i)} {\partial e_{tot} \over \partial y^i_k} =
-2\left(R^i \langle y^i \rangle^{(j,i)}+R^i y^i_{CM} + r^i
-R^j \langle y^j \rangle^{(i,j)} - R^j y^j_{CM} - r^j \right)$$

\noindent The torque acting on set $Y^i$ due to the set $Y^j$ may be 
computed as:
$$ \eqalign{ \tau^{(i,j)} &= - \sum_{k=1}^{n(i)} y^i_k \times 
{\partial e_{tot} \over \partial y^i_k}=\cr
&= -2\left(R^i \langle y^i \rangle^{(j,i)}\right) \times
\left( R^i y^i_{CM} + r^i - R^j y^j_{CM} - r^j \right)
+2\langle R^i y^i_k \times R^j P^{(j,i)}(y^i_k) \rangle^{(j,i)}}$$

\noindent Denoting  the covariance matrix between sets $Y^i$ and $Y^j$ as 
$$\Theta^{(i,j)}=\sum^{n(i)}_{k=1\atop P^{(j,i)}_0(y^i_k)\not=\oslash}
y^i_k \left( P^{(j,i)}(y^i_k)\right )^T 
\hskip 1em\hbox{and}\hskip 1em 
R^i=\pmatrix{R^i_1 \cr R^i_2 \cr R^i_3 \cr}$$
%
an identity holds:
$$\langle R^i y^i_k \times R^j P^{(j,i)}(y^i_k) \rangle^{(j,i)}=
\pmatrix{R^i_2 \Theta^{(i,j)} {R^j_3}^T -
R^i_3 \Theta^{(i,j)} {R^j_2}^T \cr
R^i_3 \Theta^{(i,j)} {R^j_1}^T -
R^i_1 \Theta^{(i,j)} {R^j_3}^T \cr
R^i_1 \Theta^{(i,j)} {R^j_2}^T -
R^i_2 \Theta^{(i,j)} {R^j_1}^T \cr}$$

\noindent By summing over all the sets $Y^j$, $j\not=i$ we obtain the total 
force and torque acting upon set $Y^i$:

$$ F^i_{tot}= \sum_{j=1 \atop j\not=i}^m F^{(i,j)}
\hskip 1em\hbox{and}\hskip 1em 
\tau^i_{tot}= \sum_{j=1 \atop j\not=i}^m \tau^{(i,j)} $$

\noindent It is noteworthy to observe that it suffices to know only the mean values
$\langle y^i \rangle^{(j,i)}$, $\langle y^j \rangle^{(i,j)}$
and the covariance matrix $\Theta^{(i,j)}$ to calculate the total force
$F^i_{tot}$ and torque $\tau^i_{tot}$ for any set of rigid body
transformation $Q^1,Q^2,\ldots,Q^m$. Consequently, after the
preprocessing has been done (in $O(m n)$ time), we can perform this
calculation very quickly, in {\it constant time\/}, regardless of the number 
of points in the sets.

Let us return now to the physical analogy. The physical system (rigid
bodies connected with springs) moves according to the laws of motion,
depending on the total force and torque acting on it, unless it reaches a
(local) potential minimum or it starts to oscillate. To avoid oscillation we
add {\it friction\/}, so much friction that the equations of motion becomes:

$$ \gamma { \d y^i_{CM} \over \d t} =  F^i_{tot}
\;,\;\;\; \Gamma \omega^i = \tau^i_{tot} $$

\noindent where $\gamma$ and $\Gamma$ are constant parameters resembling mass
and moment of inertia. $\omega$ denotes angular velocity, its direction
represents the axis of rotation and its magnitude the speed in 
$\hbox{rad}/\hbox{s}$. 

Given the angular and linear velocities and a time difference $\d t$ we want
to update the rotation $R^i$ and translation $t^i$. First of all we convert 
$\omega^i=(\omega_1,\omega_2,\omega_3)$ to a quaternion $q$:

$$ 
\let\oldcr=\cr
\def\cr{\oldcr\noalign{\vskip 1pt}}
q=\pmatrix{ q_0 \cr q_1 \cr q_2 \cr q_3 } =
\pmatrix{ \hphantom{\sin}\llap{$\cos$} (\|\omega\| \d t\, /2) \,
          \hphantom{\omega_1 / \|\omega\|}\cr
          \sin (\|\omega\| \d t\, /2) \,\omega_1 / \|\omega\| \cr
          \sin (\|\omega\| \d t\, /2) \,\omega_2 / \|\omega\| \cr
          \sin (\|\omega\| \d t\, /2) \,\omega_3 / \|\omega\|}
\;,\;\;\;
q=\pmatrix{ q_0 \cr q_1 \cr q_2 \cr q_3 }={q' \over \| q' \|}$$

\noindent From the quaternion we proceed to a rotation matrix:
$$ R_{\omega \d t}=\pmatrix{
q^2_0+q^2_1-q^2_2-q^2_3 & 2(q_1 q_2 - q_0 q_3)    & 2 (q_1 q_3 + q_0 q_2) \cr
2(q_1 q_2 + q_0 q_3)    & q^2_0+q^2_2-q^2_1-q^2_3 & 2 (q_2 q_3 - q_0 q_1) \cr
2(q_1 q_3 - q_0 q_2)    & 2(q_2 q_3 + q_0 q_1)    & q^2_0+q^2_3-q^2_1-q^2_2 \cr
}$$  

\noindent Taking into account that the rotation is around the center of mass, the
update equations are straightforward:
$$ \eqalign{R^i (t+\d t) &= R^i_{\omega \d t} R^i (t) \cr
r^i (t+\d t) &= R^i_{\omega \d t} \left( r^i(t) - y^i_{CM} \right)
 + y^i_{CM}+ F^i_{tot} \d t} $$

\noindent The above mentioned equations can be in fact combined into one:
$$ \left( R^1,\ldots,R^m,r^1,\ldots,r^m \right) (t+\d t)
= {\cal F} \Bigl( \left( 
R^1,\ldots,R^m,r^1,\ldots,r^m\right) (t), {\cal P} \Bigr)$$

\noindent where ${\cal P}$ is a vector of time-independent parameters
$${\cal P}=\left( \gamma, \Gamma, y^1_{CM},\ldots,y^m_{CM},
\langle y^1 \rangle^{(2,1)},
\langle y^1 \rangle^{(3,1)},\ldots,
\langle y^{m-1} \rangle^{(m,m-1)},
\Theta^{(2,1)}, \Theta^{(3,1)},\ldots,
\Theta^{(m,m-1)}\right)$$

\noindent
The equation can be readily transformed into a set 
of first order ordinary differential equations and the form above represents
directly the Euler method to solve it. The equation is guaranteed to have
a {\it steady point\/} that is a local minimum of the cost function
$e_{tot}$ we were to minimize.  An adaptive step-size modification of the
Euler method is recommended (\cit{5}), however a constant step-size variety will do
for most cases when small steps are used, at the expense of some speed.
We experimentally used a simple feedback step-size control --- when 
${\cal E}_1=\sum_i {\left\| F^i_{tot} \right\|^2 + 
\left\| \tau^i_{tot} \right\|^2}$ increased, step-size was reduced ten times
and the last step was redone, otherwise the step-size was increased by 20\%,
unless the step-size left the bounds $10^{-10}<\d t<10^2$ (to prevent
pathological cases). This method speeded up the process but led to some
undesirable glitches in the time behaviour of the estimated parameters.
Apparently more elaborate method should be used.

All that rests is to give some guidance on the choice of parameters. The
following values are known to give good results but experimenting is
encouraged:

$$ \gamma=1\;,\;\;\; \Gamma={\gamma \over 2} 
\max_i 
\left( \sum_{k=1}^{n(i)} \left\| y^i_k - y^i_{CM} \right\|^2 \right)
\;,\;\;\; \d t=0.1
$$
  
\sec Complete Multiple Point Sets Registration Algorithm

In the preceding sections we have described all the important parts of the
algorithm. Here we have it in full:
\smallskip

{\parindent=0pt\parskip=0pt
\def\1{\hskip 2em} \def\2{\hskip 4em} \def\3{\hskip 6em}
\obeylines
{\bf begin}
Compute $y^1_{CM},\ldots,y^m_{CM}$, set $\gamma$ and $\Gamma$.
Set $R^1,\ldots,R^m,r^1,\ldots,r^m$ to initial values supplied by user.
{\bf repeat}
\1 Transform all points from all sets using the appropriate transformations %
   see $(*)$. 
\1 {\bf for each} pair of point sets $(i,j)$, $i<j$ {\bf do}
\2 {\bf begin}
\2 Calculate the correspondences $P^{(j,i)}$ using the %
   {\it Closest Point Search\/} algorithm
\2 Calculate the mean values $\langle y^i \rangle^{(j,i)}$,%
   $\langle y^j \rangle^{(i,j)}$ and the covariances %
   $\Theta^{(i,j)}$, $\Theta^{(j,i)}$.
\2 {\bf end}
\1 {\bf repeat}
\2 {\bf for each} point set $i$ {\bf do}
\3 {\bf begin}   
\3 Calculate $F^i_{tot}$,$\tau^i_tot$.
\3 Update $R^i$, $r^i$.
\3 {\bf end}
\1 {\bf until} $\left|{\cal E}_1(k-1) - {\cal E}_1(k)\right| < \varepsilon_1$
{\bf until} $\left|{\cal E}_2(k-1) - {\cal E}_2(k) \right|< \varepsilon_2$
}

\medskip
\noindent The symbols ${\cal E}$ represent some suitable quantities
representing the quality of the temporary solution. Ideally we would use
$e_{tot}$ but it is slow to compute. Instead, we have used
%
$${\cal E}_1={\cal E}_2=\sum_i {\left\| F^i_{tot} \right\|^2 + 
\left\| \tau^i_{tot} \right\|^2}$$
%
that is easily computed as a by-product of the inner loop calculations.
$\varepsilon_1=10^{-2}$, $\varepsilon_2=10^{-4}$ is fine for the first 
experiments and can be decreased to get more accurate results.

The most time consuming part of the algorithm is the closest point search.
Assuming one query can be resolved in $O(\log n)$ time with $O(n \log n)$ 
time for preprocessing, the algorithm as a whole takes $O(m^2 n \log n)$
time.

\sec Results

We have conducted several groups of tests. In the first group of test we
tried to register very small point sets consisting of just two or three
points to test the basic functionality of the algorithm. For these sets the
algorithm always converged to a suitable, albeit sometimes sub-optimal,
solution. The number of outer loop iterations, i.e. the number of
registrations needed was usually less than 5. The number of inner loop
iterations varied depending on the exact configuration usually between
10-100 but could be certainly reduced, if need be, by using a better
adaptive step-size integration method.

The input for the second group of tests was based on real scanned data of
an object, containing about 5000 points. This data was mathematicaly transformed 
using a predetermined rigid-body transformation to obtain the point sets 
we later used for registration. For these sets the algorithm rarely converged
if the initial estimate of the transformation yeilded points
further apart from their true partners than the resultion of the image.
We assume that when the initial closest points search gives an incorrect
correspondence and many points are eliminated for not fulfilling the
reciprocity condition, the remaining points do not provide enough
information for improvement and the algorithm almost randomly changes the
transformations {\it ad infimum\/}.
  

The third group of test was based on real data obtained by scanning an object
from multiple directions. As we could guess from the results from the second
group of test, no succesful registration was found.

\sec Conclusion

We have implemented an algorithm for registering multiple point sets by
combining the ICRP algorithm (\cit{1}) and the Multiple Point Sets
registration algorithm (\cit{2}). Although both algorithms separately
perform satisfactorily, the combined algorithm suffered from convergence
problems that are yet to be resolved. 

Other areas of improvement could be using 3D Voronoi diagrams for closest
point search and faster (higher order) integration method of solving the
differential equations from the algorithm (\cit{2}).  

\sec References

\smallskip
{\parskip=\baselineskip
\rf{1}{Pavel Ku\v{c}era. Registrace prostorov\'ych ploch, 
Diplomov\'a pr\'ace, FEL \v{C}VUT, Praha 1996}
\rf{2}{A.~Stoddart, A.~Hilton. Registration of Multiple Point Sets,
Proceedings of ICPR'96, p.~40-44, IEEE 1996}
\rf{3}{???. Geometric Algorithms, ???}
\rf{4}{Robert Sedgewick. Algorithms, Addison-Wesley 1988}
\rf{5}{???. Numerical Recepies in C, ???} 
\rf{6}{B. Horn, Closed-form solution of absolute orientation using unit 
quaternions. J. Opt. Soc. Amer. A,4(4): p.~629-642, April 1987}
\rf{7}{P. Besl, N. McKay. A method for registration of 3D shapes.
IEEE Trans. Pattern Analysis and Machine Intell., 14(2), p.~239-256,
1992}
\rf{8}{Tom\'a\v s Pajdla, Luc Van Gool. Matching of 3D curves using
semidifferential invariants, 5th International Conference on Computer
Vision, Cambridge, MA, July 1995} 
}

\sec Appendix --- Description of the program MREG

To evaluate the algorithms presented, an experimental program called {\it mreg\/}
has been written in {\it C\/}. The program expects at its standard input the
information about what to register formatted as follows:

On the first line there should be the number of views to register. Then, for
each view there should be a block of two lines --- one line containing the
name of the file to read and number of points in this file. The second line contains
seven real numbers representing the initial transformation. The first four
represent the quaternion of rotation, the remaining three the translation.
There can be blank lines between blocks. An example of the input data
follows:

{\parskip=0pt\obeylines\tt
2
cuthar1	5182
1.0 0 0 0 0 0 0


cuthar2 5372
0.99 0.01 0 0 1 0 0
}

The files to read for each view are standard text files with one line per
point, each containing three real numbers --- the coordinates of the points.

Besides the parameters expected at the standard input, there are also
parameters to be changed in the source code, namely {\tt MATLABOUT}, 
{\tt REGISTER}, {\tt DEBUG}, {\tt MAXNVIEWS}, {\tt DTINIT}, {\tt eliminate},
{tt elimdist}, {\tt INNERLIM}, {\tt OUTERLIM}, {\tt NITIN},
{\tt NITOUT}. Their description can be found in the source code.

\bye  
