130
A Tutorial on Rigid Registration Iterative Closed Point (ICP) By Shireen Elhabian, Amal Farag, Aly Farag University of Louisville, CVIP Lab March 2009
A Tutorial on Rigid Registration
Iterative Closed Point (ICP)
ByShireen Elhabian,
Amal Farag, Aly Farag
University of Louisville, CVIP LabMarch 2009
Outline
• The problem• Motivation• Data types• Mathematical preliminaries
– Somas statistics: centroid, variance and covariance– Inner product– Transformation matrices– Eigenvalues problem– Mean square error– Rigid transformations (scaling, rotation and translation in 2D and 3D)– Quaternions
Outline• Iterative closest point algorithm
– Problem statement– Main idea of ICP– Algorithm outline– Nomenclature– Finding correspondences– Alignment calculation
• Finding the translational offset• Finding the scaling factor• Finding the best rotation
– Let’s do it …– Experiments
The ProblemAlign two partially-overlapping meshesgiven initial guessfor relative transform
The Problem
Motivation
• Shape inspection
• Motion estimation
• Appearance analysis
• Texture Mapping
• Tracking http://blog.wired.com/defense/2007/08/danger-room-inb.html
Motivation
• Range images registration
Motivation
• Range images registration
Range Scanners
Data Types
• Point sets
• Line segment sets (polylines)
• Implicit curves : f(x,y,z) = 0
• Parametric curves : (x(u),y(u),z(u))
• Triangle sets (meshes)
• Implicit surfaces : s(x,y,z) = 0
• Parametric surfaces (x(u,v),y(u,v),z(u,v)))
Mathematical Preliminaries
Centroid
• The centroid of a data (point) set is theweighted mean of all data points presented inthe set.
• For a data set A, having n points, each denotedby ai , the centroid is given by :-
å=
=n
iiA a
n 1
1µ
13
Variance
• A measure of the spread of the data in a dataset A with mean µA:
• Variance is claimed to be the original statisticalmeasure of spread of data.
( )
( )11
2
2
-
-=å=
n
an
iAi
A
µs
Covariance
• Covariance matrix gives a measure of similaritybetween 2 data sets to be matched.
• If µA and µB are the centroids of the data setsA and B respectively then, the covariance matrixbetween the two sets is given by:-
( )( )
( )11
-
--=å
å =
n
ban
iBiAi
AB
µµ
Inner Product• Let a and b be two vectors defined as:
• Inner (dot) product:
• Length (Eucledian norm) of a vector a is normalized iff ||a||= 1
• The angle between two n-dimesional vectors• An inner product is a measure of collinearity:
– a and b are orthogonal iff
– a and b are collinear iff• A set of vectors is linearly independent if no vector is a linear
combination of other vectors.
å=
==×n
iii
T bababa1
å=
=×=n
ii
T aaaa1
2
||||||||cos
baba ×
=q
0=×ba
|||||||| baba =×
Tnaaa ],...,[ 1= T
nbbband ],...,[ , 1=
16
Transformation Matrices• Consider the following:
• The square (transformation) matrix scales (3,2)
• Now assume we take a multiple of (3,2)
úû
ùêë
é´=ú
û
ùêë
é=ú
û
ùêë
é´úû
ùêë
é
úû
ùêë
é=ú
û
ùêë
é´
46
41624
46
1232
46
23
2
17
Transformation Matrices• Scale vector (3,2) by a value 2 to get (6,4)
• Multiply by the square transformation matrix
• And we see that the result is still scaled by 4.
WHY?
A vector consists of both length and direction. Scaling avector only changes its length and not its direction. This isan important observation in the transformation of matricesleading to formation of eigenvectors and eigenvalues.
Irrespective of how much we scale (3,2) by, the solution(under the given transformation matrix) is always a multipleof 4.
18
Eigenvalue Problem• The eigenvalue problem is any problem having the
following form:A . v = λ . v
A: m x m matrixv: m x 1 non-zero vectorλ: scalar
• Any value of λ for which this equation has asolution is called the eigenvalue of A and the vectorv which corresponds to this value is called theeigenvector of A.
19
Eigenvalue Problem• Going back to our example:
A . v = λ . v
• Therefore, (3,2) is an eigenvector of the square matrix Aand 4 is an eigenvalue of A
• The question is:Given matrix A, how can we calculate the eigenvectorand eigenvalues for A?
20
Calculating Eigenvectors & Eigenvalues
• Simple matrix algebra shows that:
A . v = λ . v
Û A . v - λ . I . v = 0
Û (A - λ . I ). v = 0
• Finding the roots of |A - λ . I| will give the eigenvaluesand for each of these eigenvalues there will be aneigenvector
Example …
21
• Let
• Then:
• And setting the determinant to 0, we obtain 2 eigenvalues:λ1 = -1 and λ2 = -2
( )( ) ( ) 23123321
00
3210
1001
3210
.
2 ++=´----´-=úû
ùêë
é---
-=
úû
ùêë
é-úû
ùêë
é--
=úû
ùêë
é-úû
ùêë
é--
=-
lllll
l
ll
ll IA
úû
ùêë
é--
=3210
A
Calculating Eigenvectors & Eigenvalues
22
Calculating Eigenvectors & Eigenvalues
• For λ1 the eigenvector is:
• Therefore the first eigenvector is any column vector inwhich the two elements have equal magnitude and oppositesign.
( )
2:11:1
2:11:12:11:1
2:1
1:1
11
0220
0.2211
0..
vvvvandvv
vv
vIA
-==--=+
=úû
ùêë
éúû
ùêë
é--
=-l
23
Calculating Eigenvectors & Eigenvalues
• Therefore eigenvector v1 is
where k1 is some constant.
• Similarly we find that eigenvector v2
where k2 is some constant.
úû
ùêë
é-+
=11
11 kv
úû
ùêë
é-+
=21
22 kv
24
Properties of Eigenvectors and Eigenvalues
• Eigenvectors can only be found for square matrices andnot every square matrix has eigenvectors.
• Given an m x m matrix (with eigenvectors), we can findm eigenvectors.
• All eigenvectors of a symmetric* matrix areperpendicular to each other, no matter how manydimensions we have.
• In practice eigenvectors are normalized to have unitlength.
*Note:covariancematricesaresymmetric!
Mean Square Error• In statistics, the mean squared error or MSE
of an estimator is one of many ways to quantifythe amount by which an estimator differs fromthe true value of the quantity being estimated.
• The Mean Square error (MSE) between 2 datasets A and B having n points each is given by :-
where ||.|| denotes the L2-Norm/EuclideanNorm between two data points.
å=
-=n
iii ba
nMSE
1
21
Rigid Transformations
• Rigid transformations can be classified into:
– Scaling
– Rotation
– Translation
Transformations – Scaling in 2D
• Scaling a coordinate means multiplying each ofits components by a scalar
• Uniform scaling means this scalar is the samefor all components:
´ 2
Transformations - Scaling in 2D
• Non-uniform scaling: different scalars percomponent:
• How can we represent this in matrix form?
X ´ 2,Y ´ 0.5
Transformations - Scaling in 2D
• Scaling operation:
• Or, in matrix form:
úû
ùêë
é=ú
û
ùêë
ébyax
yx''
úû
ùêë
éúû
ùêë
é=ú
û
ùêë
éyx
ba
yx
00
''
scaling matrix
Transformations – Rotation in 2D
q
(x, y)
(x’, y’)x’ = x cos(q) - y sin(q)y’ = x sin(q) + y cos(q)
úû
ùêë
éúû
ùêë
é -=ú
û
ùêë
éyx
yx
qqqq
cossinsincos
''
Or, in matrix form
Transformations – Translation in 2D
y
x
tyytxx+=+=
''
tx = 2ty = 1
Transformations – Scaling in 3D
úúúú
û
ù
êêêê
ë
é
×
úúúú
û
ù
êêêê
ë
é
=
úúúú
û
ù
êêêê
ë
é
11000000000000
1'''
zyx
ss
s
zyx
z
y
x
z x
y
Transformations – Translation in 3D
úúúú
û
ù
êêêê
ë
é
×
úúúú
û
ù
êêêê
ë
é
=
úúúú
û
ù
êêêê
ë
é
11000100010001
1'''
zyx
ttt
zyx
z
y
x
(x’,y’,z’)
(x,y,z) T=(tx,ty,tz)
z
x
y
Transformations – Rotation in 3D• How should we specify rotations?
• In 2D, it was always counterclockwise in the xy-plane.
• In 3D, we have more choices
– xz-plane, yz-plane, an arbitrary plane.
• We could specify these in terms of the vectorperpendicular to the plane of rotation.
– z axis, y-axis, x-axis, arbitrary axis
Rotation in 3D – Euler Angles
úúúú
û
ù
êêêê
ë
é
×
úúúú
û
ù
êêêê
ë
é -
=
úúúú
û
ù
êêêê
ë
é
11000010000cossin00sincos
1'''
zyx
zyx
gggg
úúúú
û
ù
êêêê
ë
é
×
úúúú
û
ù
êêêê
ë
é
-=
úúúú
û
ù
êêêê
ë
é
110000cos0sin00100sin0cos
1'''
zyx
zyx
bb
bb
úúúú
û
ù
êêêê
ë
é
×
úúúú
û
ù
êêêê
ë
é-
=
úúúú
û
ù
êêêê
ë
é
110000cossin00sincos00001
1'''
zyx
zyx
aaaa
z x
y
z x
y
z x
y
Quaternions
• A quaternion q can be thought of as:
– A vector with four components: q = [q0,qx,qy,qz]T.
– A composite of a scalar and an ordinary vector: q =[q0, q].
– A complex number with three different imaginaryparts: q = q0 + iqx+jqy+kqz.
Products of Quaternions• Multiplication of quaternions can be defined in terms
of products of their components.
• Suppose that we let : i2 = j2 = k2 = -1
ij = k, jk = i, ki = j
and ji = -k, kj = -i, ik = -j
• Then if r = r0 + irx+jry+krz ,
we get;
• The product qr has a similar form
with six of the signs changed.
( )( )( )( )00
00
00
00
qrqrqrqrkqrqrqrqrjqrqrqrqriqrqrqrqr
zxyyxz
xzyzxy
yzzyxx
zzyyxx
+-++
++-+
-+++
---=rq
Products of Quaternions• The product of two quaternions can also be expressed in terms
of the product of an orthogonal 4x4 matrix and a vector withfour components.
• One may choose to expand either the first or the secondquaternion in a product into an orthogonal 4x4 matrix asfollows:
• Note that differs from R
in that the lower-right-hand
3x3 sub-matrix is transposed.
qRq
Rqq
=
úúúúú
û
ù
êêêêê
ë
é
--
----
=
=
úúúúú
û
ù
êêêêê
ë
é
--
----
=
0
0
0
0
0
0
0
0
rrrrrrrrrrrrrrrr
rrrrrrrrrrrrrrrr
xyz
xzy
yzx
zyx
xyz
xzy
yzx
zyx
qr
rqR
Dot Product of Quaternions• Considering a quaternion as a vector of four
components, the dot product of two quaternions is thesum of products of corresponding components:
• The square of the magnitude of a quaternion is the dotproduct of the quaternion with itself.
• A unit quaternion is a quaternion whose magnitudeequals 1.
zzyyxx qrqrqrqr +++= 00r.q
22220
2zyx qqqq +++== q.qq
1=q.q
Conjugate of Quaternions
• Taking the conjugate of a quaternion negates itsimaginary part, thus: q* = q0 – iqx – jqy – kqz.
• The 4x4 matrix associated with the conjugate ofa quaternion is just the transpose of the matrixassociated with the quaternion itself, i.e. QT
where
úúúúú
û
ù
êêêêê
ë
é
--
----
=
0
0
0
0
qqqqqqqqqqqqqqqq
xyz
xzy
yzx
zyx
Q
Conjugate of Quaternions• Since Q is an orthogonal matrix, thus the product with
its transpose is a diagonal matrix, that is: QQT = (q.q)I,where I is the 4x4 identity matrix.
• Correspondingly, the product of q and q* is real, thatis:
• We immediately conclude that a non-zero quaternionhas an inverse:
• In case of a unit quaternion (q.q = 1), the inverse isjust the conjugate
q.qqq* =+++= 22220 zyx qqqq
q.qqq*
1- =
Useful Properties of Products • Since the matrices associated with quaternions are
orthogonal, hence dot products are preserved, that is:(qv).(qr) = (q.q) (v.r)
• Proof:
• In the case of a unit quaternion : (qv).(qr)=(v.r)
• A special case follows immediately:
(qr).(qr) = (q.q) (r.r)
that is the magnitude of a product is just the product of the magnitudes.
( )( ) ( ) ( ) ( ) ( )( ) ( )( )v.rq.qq.q
qrqv.
.===
==
IrvQrQvQrQvQr.Qv
TTT
T
Vectors• Vectors can be represented by purely imaginary
quaternions. If r = (x, y, z)T, we can use thequaternion: r = 0 + i x + j y + k z.
• Similarly, scalars can be represented by using realquaternions.
• Note that the matrices R and associated witha purely imaginary quaternion and its conjugateare skew symmetric, that is
R
RRRR -=-=TT and
Unit Quaternions and Rotation• Since the length of a vector is not changed by rotation
nor is the angle between vectors, thus rotationpreserves dot product. (note that a.b = |a||b|cosθ).
• Now, we have already established that multiplication bya unit quaternion preserves dot products between twoquaternions, that is (qv).(qr) = (q.q) (v.r) = v.r
• And since a vector can be represented as a purelyimaginary quaternion, thus we can represent rotation byusing unit quaternions if we can find a way of mappingpurely imaginary quaternions into purely imaginaryquaternions in such a way that preserves dot products.
Unit Quaternions and Rotation• However, we can not use simple multiplication to
represent rotation, since the product of a unitquaternion and a purely imaginary quaternion isgenerally not a purely imaginary quaternion.
• What we can use instead is the composite productdefined as follows:
– Let r be a purely imaginary quaternion representing a 3Dpoint (vector) such that : r = 0 + i x + j y + k z.
– Let q be a unit quaternion such that q = q0 + iqx+jqy+kqz
– Rotating the vector (point) r by a unit quaternion q can bedefined as r’ = qrq*, where r’ is a purely imaginaryquaternion representing the vector r after rotation by q.
Unit Quaternions and RotationProve that the composite product leads to purely imaginary quaternion, henceit can be used to represent rotation.
Proof:
• The objective is representing the composite product as a matrix multiplied bya vector r. let’s find this matrix in terms of the matrices associated to the unitquaternion q and its conjugate.
Where Q and are the 4x4 matrices corresponding to the unit quaternion q.
( ) ( )
úúúúú
û
ù
êêêêê
ë
é
--
----
=
úúúúú
û
ù
êêêêê
ë
é
--
----
=
===
0
0
0
0
0
0
0
0
)(
qqqqqqqqqqqqqqqq
and
qqqqqqqqqqqqqqqq
where
xyz
xzy
yzx
zyx
xyz
xzy
yzx
zyx
TT
rQQQrQQr ** qqrq
Q
Unit Quaternions and RotationProof: cont
is a purely imaginary quaternion if r is a purely imaginaryquaternion.
22220
2222000
02222
00
002222
0
0
0
0
0
0
0
0
0
)()(2)(20)(2)()(20)(2)(2)(0
000
zyx
zyxxyzyxz
xzyzyxzxy
yzxzyxzyx
xyz
xzy
yzx
zyx
xyz
xzy
yzx
zyx
T
qqqqwhere
qqqqqqqqqqqqqqqqqqqqqqqqqqqqqqqqqqqq
qqqqqqqqqqqqqqqq
qqqqqqqqqqqqqqqq
+++=
úúúúú
û
ù
êêêêê
ë
é
+--+---+-++---+
=
úúúúú
û
ù
êêêêê
ë
é
--
----
úúúúú
û
ù
êêêêê
ë
é
----
--=\
q.q
q.q
rQQ )(T
=\ *qrq
Unit Quaternions and RotationProof: cont
• Since q is a quaternion, then Q and are orthogonal matricesby definition.
• Since q is a unit quaternion, then Q and are orthonormalmatrices,.
• Hence the lower-right-hand 3x3 sub-matrix of must also beorthonormal , hence it is the rotation matrix R that take r to r’such that r’=Rr.
• The expansion of provides an explicit method forcomputing the orthonormal rotation matrix R from thecomponents of the unit quaternion q.
■
Q
Q
QQT
QQT
Iterative Closest Point (ICP) Algorithm
Problem Statement• Given a model shape which maybe
represented as:– Point Sets, Line Segment Sets, Implicit Curves,
Parametric Curves, Triangle Sets, Implicit Surfaces,Parametric Surfaces
• Given a scene shape which is represented as apoint set, the scene shape may correspond to themodel shape
• It is required to estimate the optimal rotation,translation and scaling that aligns or registers thescene shape to the model shape
• Main Application is to register digitize(sensed)data from un-fixtured rigid objects with anidealized geometric model prior to shapeinspection.
Main Idea of ICP
1. Begin with initial rotation, translation and scaling (initial valuefor registration parameters).
2. Fix the model shape and start moving the scene shape byapplying the initial registration parameters. i.e. scale, rotate andthen translate.
3. Compute the error metric that reflects the dissimilarity of thescene shape from the model shape.
4. If the error is minimum, we have correctly aligned the sceneshape to the model shape, return with the aligned scene shape.
5. Else, calculate the new values for the registration parametersand go back to step 2 with the new parameter values.
Algorithm Outline1. Initialize registration parameters (R,t,s)
and registration error; Error = ∞2. For each point in the scene shape, find
the corresponding closest point in themodel shape.
3. Calculate registration parameters givenpoint correspondences obtained fromstep 2.
4. Apply the alignment to the scene shape.5. Calculate the registration error between
the currently aligned scene shape and themodel shape.
6. If error > threshold, return to step 2,else return with new scene shape.
Initialize the error to ∞
Find point correspondence
Find alignment
Apply alignment
Update error
If error > threshold
Nomenclature
• Let the model shape be represented as a set of points, mi, where ii =1,2,…NM
.
– Where NM is the number of points in the model shape and M = {mi}denotes the model point set
– Note: mi= [mxi, myi,mzi]T in the case of 3D
• Let the scene shape be represented as a set of points pi, wherei=1,2,..,NP
.
– Where NP is the number of points in the scene shape and P = {pi}denotes the scene point set.
– Note: pi = [pxi, pyi,pzi]T in the case of 3D
Nomenclature
• Registration parameters:
– s : a scalar value which represents the scaling parameter
– t : a vector representing translation parameters. In 3D case t= [tx, ty,tz]T
– R(.): an operator which applies rotation to its argument (a point).
Note: R(.) will have a different definition according to the way it is usedto represent rotation. E.g. Euler angles (Θx,Θy,Θz), rotation matrix R(3x3orthogonal matrix) or quaternion q (rotation angle and axis of rotation)
Finding Correspondences
• If correct correspondences are known, we can findcorrect relative rotation/translation
Finding Correspondences
• How to find correspondences: User input? Featuredetection? Signatures?
• Alternative: assume closest points correspond
Finding Correspondences
• How to find correspondences: User input? Featuredetection? Signatures?
• Alternative: assume closest points correspond
Finding Correspondences
• Converges if starting position “close enough“
Finding Correspondences• For every point, pi, in the scene shape P ={pi}, i=1,..NP, we
search for the closest point mj in the model shape M to the scenepoint pi using the Euclidean distance.
• Given two points pi and mi, the Euclidean distance can becomputes as follows:
• Given a scene point pi and the model point set M, the Euclideandistance between pi and M can be found as:
( ) ( ) ( )222
),(
zkziykyixkxi
kiki
mpmpmp
mpmpd
-+-+-=
-=
kiNkkiNki mpmpdMpdMM
-==== ,...1,...1min),(min),(
Finding Correspondences• The closest point mj Є M (model point set) satisfies the equality:
i.e.
• In other words, j is the index of the closest point pi
• The closest point in the model set M that yields the minimumdistance will be denoted by yi. That is yi = mj such thatd(pi,yi)=d(pi,M). Now pi corresponds to yi.
• Let Y denote the resulting set of closets points and C be theclosest point operator such that Y = C( P , M )
),(),( Mpdmpd iji =
),(minarg,..1
kiMNk
mpdj=
=
{mk}{pi}{yi}
Alignment Calculation• Given: A set of point correspondences between the
scene shape P and the model shape M, wheredenotes the set of closest points to P, such that yi is thecorresponding closest point to pi, where i=1,2,..,NP.
• Required: Find the optimal registration parameters(scaling, rotation and translation) which brings thescene points P to the closest model points Y.
• Approach: Quaternion based method
MY Í
Alignment Calculation• Let NP be the number of points correspondences.• The measurement coordinates in the scene and model
coordinate systems will be denoted by P={pi} and Y={yi},respectively, where i ranges from 1 to NP.
• We are looking for a transformation of the form:
which registers (aligns) the scene points P to the correspondingmodel points Y.where: s: is a scale factor
t: is the translational offsetR(P) denotes the rotated version of the points P.
Note: At this time we do not use any particular notation for rotation.
)1()( tPsRY +=
Alignment Calculation• Rotation is a linear operation and it preserves lengths such that:
Where is the square of the length of the vector point p.
• Since correspondences are not perfect, we will not be able tofind a scale factor, a translation and a rotation such that thetransformation equation(1) is satisfied for each point.
• Instead there will be a residual error for each point pair(correspondence) defined as follows:
22)( ppR =
ppp ×=2||||
( ) )2()( !"!#$i
tpsRye iii
p pointscene the of version dTransforme
+-=
Alignment Calculation
• We will minimize the total error defined as the sum of squaresof these errors:
• We will consider the variation of the total error first withtranslation, then with scale and finally with respect to rotation.
å=
=PN
iieE
1
2||||
Finding the Translation
• It is useful to refer all points to their centroids which are definedas follows:
• Lets denote the new points by:
• After this transformation, the point sets become zero mean, i.e
andpN
PN
ii
PP å
=
=1
1µ å=
=PN
ii
PY y
N 1
1µ
andpp pii µ-=' Yii yy µ-='
01011
'
1
' == åå==
PP N
ii
P
N
ii
P
yN
andpN
Finding the Translation
• Now, the error term can be re-written as follows:
t’ is the new translational offset after bringing the points to the origin (i.e. zero-mean).
Q: Prove equation (4)
( ))4()(')3(')'('
PY
iii
sRttwheretpsRye
µµ +-=+-=
Finding the TranslationProof:
Set equation (2) equal to equation (3) since the residual errors ofthe points before and after bringing the points to the origin areequal.
ThusSince
ThereforeSince rotation is a linear operator
■
( ) ( )')'(')( tpsRyitpsRy iii +-=+-PiiYii ppandyy µµ -=-= ''
')()( tpsRytpsRy PiYiii ----=-- µµ
)(''))()(
PY
PiYiii
sRtttsRpsRytpsRy
µµµµ
+-=-+--=--
Finding the TranslationHence the sum of squares of errors becomes:
Now the sum in the middle of the expression in (5) is zero sincethe points are referred to the centroid (i.e. zero-mean androtation and scaling don’t affect the mean.)
Hence, The first and third terms are left:
[ ]
)5(||'||||)'('||'2||)'('||
||'||||)'('||'2||)'('||
||')'('||||||
1
2
1
2
1
22
1 1
22
å å
å
å å
= =
=
= =
+---=
+---=
--==
P P
P
P P
N
iP
N
iiiii
N
iiiii
N
i
N
iiii
tNpsRytpsRy
tpsRytpsRy
tpsRyeE
)6(||'||||)'('||1
22å=
+-=PN
iPii tNpsRyE
Finding the Translation• Remember that we are looking for the optimal translational
offset t’ which will minimize the total error E.• Since the first term in (6) does not depend on the translation and
the second term in (6) can not be negative since:
• Thus the total error is obviously minimized with t’ = 0.• Therefore the translational offset can be found as follows:
That is, the translation is just the difference of the model points centroid andthe scaled and rotated scene points centroid.We return to this equation to find the translational offset once we have foundthe scale and rotation.
00||'|| 2 >³ PNandt
)7()(0)('
PY
PY
sRtsRttµµ
µµ-=\
=+-=!
Finding the Translation• At this point, we note that the error term can be
written as:
• Since t’ = 0, so the total error to be minimized can bere-written as follows:
)'(' iii psRye -=
)8(||)'('||1
2å=
-=PN
iii psRyE
Finding the Scale• Expanding the total error defined in (8), we will get:
• Since rotation preserves length, i.e.
• Therefore we obtain:
• Let
[ ]
åå å
å
== =
=
+-=
+-=
PP P
P
N
ii
N
i
N
iiii
N
iiiii
pRspRysy
psRpsRyyE
1
2
1 1
2
1
22
||)'(||)'('2||'||
||)'(||)'('2||'||
2
22 ||'||||)'(|| ii ppR =
åå å== =
+-=PP P N
ii
N
i
N
iiii pspRysyE
1
2
1 1
2 ||'||)'('2||'|| 2
ååå===
===PPP N
iii
N
ii
N
iPiy pRyandpSyS
11
2
1
2 )'('||'||||'|| D ,
Finding the Scale• Hence;
where Sy and Sp are the sums of the squares of the points lengthrelative to their centroids, while D is the sum of the dot productsof the corresponding points in the model with the rotated pointsin the scene.
• Since we are looking for the scaling factor s, complete the squarein (9) with respect to s, we will get:
Lets prove (10) L…
)9(2 2Py SssDSE +-=
(10)2
2
P
Py
PP S
DSS
SDSsE
-+÷
÷ø
öççè
æ-=
Finding the ScaleProof:
( )
÷÷ø
öççè
æ+
-=÷÷
ø
öççè
æ+-
÷÷ø
öççè
æ¾®¾
-=-
-=-
=+-=
+-=-
2
2
2
22
2
2
2
2
2
222
2
2:
2
02
2
PP
Y
PP
PP
P
Y
P
YP
YP
SD
SS
SD
SDss
SD
SD
SS
SDss
SDssS
SDssSE
AAssAs
:sides both to this Add
-
:itsquare and term-sthe of coffecientthe of halfTake
s of coffecientthe by Divide
:side otherthe to term constantthe Move
zero. to error totalthe equate s, forsolve To :Recall
2
Finding the ScaleProof: cont
■
( )
P
PY
P
PP
PY
P
PP
YPP
P
N
iiP
YPP
P
P
YPPP
SDSS
SSD
SsE
SDS
SSD
Ss
SSD
SDsS
pS
SSD
SDsS
S
SSD
SSDs
P
22
22
222
1
2
22
22
0
0
,0||||
1
-+÷
÷ø
öççè
æ-=
=-+÷÷ø
öççè
æ-
=÷÷ø
öççè
æ--÷÷
ø
öççè
æ-
³=
÷÷ø
öççè
æ-=÷÷
ø
öççè
æ-
÷÷ø
öççè
æ-=÷÷
ø
öççè
æ-
å=
:Therefore
rootsquare the take can we Hence, Since
by sides bothMulitply
:side hand rightthe simplify and form squaredthe toside hand leftthe Convert
Finding the Scale• The total error can be minimized with respect to the scaling
factor s when the first term in (10) is set to zero, since thesecond term doesn’t depend on s.
)11(
0
,0
1
2
1
2
å
å
=
==
==-
=÷÷ø
öççè
æ-
P
P
N
ii
N
iii
PP
PP
PP
'||||p
')'R(pys
SDS
SDSs
SDSs
:Hence
thus
:Therefore
Symmetry in Scale• If we exchange the roles of the scene and the model points as recommended
by Horn[2], we will be finding the best fit instead of to the transformation:
• We will find to the inverse transformation:
• The scale factor equation in this case becomes:
( ) t)sR(py ii +=
t)(yRsp iTT
i ÷øö
çèæ +=
_
÷÷÷÷
ø
ö
çççç
è
æ
=
å
å
=
=P
P
N
ii
N
ii
p
ys
1
2
1
2
||'||
||'||
Finding the Rotation• Recall (10):
• To minimize the error with respect to scaling, we set the firstterm to zero, since the second term doesn’t depend on scaling.
• Hence, the error term can now be re-written as:
• Hence Sy≥0 and Sp ≥0 , therefore SySp ≥0 and doesn’t depend onthe rotation, i.e. constant with respect to R(.), and D2 ≥0 (selfevident) is the only part in the error expression that depends onR(.).
P
Py
PP S
DSS
SDSsE
22 -+÷
÷ø
öççè
æ-=
02 =÷÷ø
öççè
æ-
SDSs
PP
)12(2
P
Py
SDSS
E-
=
ååå===
===PPP N
iii
N
ii
N
iPiy pRyDandp SySwhere:
11
2
1
2 )'('||'||||'|| ,
Finding the Rotation
• The total error can thus be re-written as:
• Thus is needed to minimized.
• Recall:
which is the sum of the dot products of the model points andthe rotated scene points.
02
=-
=P
Py
SDSS
E2DSSE Py -=
å=
=PN
iii ')'R(pyD
1
Finding the Rotation
• This maximization can be interpreted geometrically as follows:
where θ is the angle between the model points and the rotatedscene points.
• To obtain the optimal rotation, θ should be zero, therefore cos θ= 1 which is the maximum value obtained by the cosinefunction.
• Since and , having θ = 0 will lead to themaximum value of .
• Therefore, maximizing D implicitly means minimizing the anglebetween the model points and the rotated scene points.
qcos')||'||||R(p||y')R(p'y iiii =×
'||||yi 0³ 0³')||||R(pi')R(p'y ii ×
Finding the RotationRepresentation of rotation:
• There are many ways to represent rotation, including Eulerangles, axis and angle, orthonormal matrices and Hamilton’squaternion's.
• Orthonormal matrices have been used most often inphotogrammetry and robotics. However, there are a number ofadvantages to the unit-quternion notation.
• Also, unit-quaternions are closely allied to the geometricallyintuitive axis and angle notation.
• Here we solve the problem of finding the rotation thatmaximizes by using unit-quaternionså
=
=PN
iii pRy
1
)'('D
Finding the Rotation• We have to find the unit quaternion q that maximizes:
• Supposed that
• Then while
( ) ( )
( ) ( )( ) qqqq
*
*
'
1
''
1
'
1
''
1
''
..
..
i
N
iii
N
ii
N
iii
N
iii
ypyp
pypRyD
pp
pp
åå
åå
==
==
==
==
[ ] [ ]TziyixiiT
ziyixii yyyyandpppp '''''''' ,,,, ==
q
qP
úúúúú
û
ù
êêêêê
ë
é
--
----
=
=
00
00
'''
'''
'''
'''
'
xiyizi
xiziyi
yizixi
ziyixi
ii
pppppppppppp
pq
q
qY
úúúúú
û
ù
êêêêê
ë
é
--
----
=
=
00
00
'''
'''
'''
'''
'
xiyizi
xiziyi
yizixi
ziyixi
ii
yyyyyyyyyyyy
p q
Finding the Rotation• Note that and Yi are skew symmetric as well as orthogonal
since they are associated to purely imaginary quaternions.
• The sum that we have to maximize can now be written as:
iP
( )( ) ( )( ) ( ) ( )
å
ååå
ååå
=
===
===
==
=
÷÷ø
öççè
æ=÷
÷ø
öççè
æ==
===
p
ppp
ppp
N
iii
Tii
T
N
ii
Ti
N
i
Ti
Ti
N
i
Ti
T
i
N
i
Tii
N
iii
N
ii
andwhere
ypD
1
111
11
'
1
' ..
NNYPN
N
NYPYP
YPYP
qqqqqq
qqqq qq
Finding the Rotation
úúúúú
û
ù
êêêêê
ë
é
--
----
úúúúú
û
ù
êêêêê
ë
é
----
--=
=
00
00
00
00
'''
'''
'''
'''
'''
'''
'''
'''
xiyizi
xiziyi
yizixi
ziyixi
xiyizi
xiziyi
yizixi
ziyixi
iTii
yyyyyyyyyyyy
pppppppppppp
YPN
úúúúú
û
ù
êêêêê
ë
é
--+++-+--+-++--+--+--++
=
''''''''''''''''''
''''''''''''''''''
''''''''''''''''''
''''''''''''''''''
xixiyiyiziziziyiyizizixixiziyixixiyi
yiziziyixixiziziyiyiyixixiyizixixizi
xizizixixiyiyixiyiyizizixixiziyiyizi
xiyiyixixizizixiyiziziyiziziyiyixixi
ypypypypypypypypypypypypypypypypypypypypypypypypypypypypypypypypypypypyp
Finding the Rotation
etcypSypSypSwhere
SSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSS
ppp
p
N
izixixz
N
iyixixy
N
ixixixx
xxyyzzyzzyxzzxxyyx
zyyzxxzzyyxyyxxzzx
zxxzyxxyyyzzxxyzzy
yzxyzxxzzyyzzzyyxx
N
ii
...,,1
''
1
''
1
''
1
ååå
å
===
=
===
úúúúú
û
ù
êêêêê
ë
é
--+++-+--+-++--+--+--++
=
= NN
Finding the Rotation
• Hence we can define the S-matrix whose elements aresums of products of coordinates measured in the sceneshape with coordinates measured in the model shape,such that:
• Note that the scene and model points were brought tothe origin by subtracting their centroids.
úúú
û
ù
êêê
ë
é
=
zzzyzx
yzyyyx
xzxyxx
SSSSSSSSS
S
Finding the RotationEigenvector Maximizes Matrix Product:
• It can be shown that the unit quaternion that maximizes qTNq isthe eigenvector corresponding to the most positive eigenvalue ofthe matrix N.
Proof:
• To find the rotation that minimizes the sum of squares of erros,we have to find the quaternion q that maximizes qTNq subjectto the contraint that q.q = 1 (unit quaternion).
• The symmetric 4x4 matrix N will have four real eigenvalues, sayλ1, λ2, λ3 and λ4.
• A corresponding set of orthogonal unit eigenvectors v1, v2, v3and v4 can constructed such that Nvi = λivi where i = 1,2,3,4.
Finding the RotationProof: cont
• The eigenvectors span the 4D space, so an arbitrary quaternionq can be written as a linear combination in the form:
• Since the eigenvectors are orthogonal, we have:
• We know that this has to be equal to one since we are lookingfor a unit quaternion.
• Now
• We conclude that
44332211 vvvv aaaa +++=q
24
23
22
21 aaaa +++=q.q
444333222111 vλvλvλvλλ aaaa +++== qqN
( ) 4243
232
221
21 λλλλ aaaa +++== qq.qqT NN
Finding the RotationProof: cont
• Now, suppose we have arranged the eigenvalues in order suchthat λ1≥ λ2 ≥ λ3 ≥ λ4.
• Then we have:
( )
( )( )1
1
124
23
22
21
1241
231
221
21
4243
232
221
21
λλ
λλλλλλλλλ
==
+++=
+++£
+++==
q.q
qq.qqT
aaaa
aaaa
aaaaNN
Finding the RotationProof: cont
• Since we need to maximize qTNq , but we have qTNq ≤ λ1 ,hence qTNq is bounded above by λ1 which is the largesteigenvalue.
• Thus the maximum of qTNq is attained when qTNq = λ1 .
• This only happens if α1 = 1 and α2 = α3 = α4 = 0, i.e. :
which is the eigenvector corresponding to the largest positiveeigenvalue λ1.
■
1
44332211
vvvvv
=+++= aaaaq
Finding Alignment - Summary• We can now summarize the algorithm of finding the rotation,
scaling factor and the translational offset given pairs of pointscorrespondences as follows:
1. Find the centroids of the two point sets in the scene and model shapes.
2. Compute the points coordinates relative to their centroids.
3. For each pair of points {pi’,yi’}, compute the nine possible products ofthe two vectors pi’ and yi’. Then add them up to obtain Sxx, Sxy, … Szz.These nine totals contain all the information needed to find thesolution.
andpN
PN
ii
PP å
=
=1
1µ å=
=PN
ii
PY y
N 1
1µ
andpp pii µ-=' Yii yy µ-='
etcypSypSypSwhereppp N
izixixz
N
iyixixy
N
ixixixx ...,,
1
''
1
''
1
'' ååå===
===
Finding Alignment - Summary4. Compute the ten independent elements of the 4x4 symmetric matrix N
by combining the sums obtained in (3) as follows:
5. Find the eigenvalues and eigenvectors of N.
6. The quaternion q representing the rotation is the eigenvectorcorresponding to the largest positive eigenvalue of N.
úúúúú
û
ù
êêêêê
ë
é
--+++-+--+-++--+--+--++
=
xxyyzzyzzyxzzxxyyx
zyyzxxzzyyxyyxxzzx
zxxzyxxyyyzzxxyzzy
yzxyzxxzzyyzzzyyxx
SSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSSS
N
Finding Alignment - Summary7. Compute the scaling factor as follows:
8. Compute the translational offset as follows:
( )
( )
22220
2222000
02222
00
002222
0
''
1
2'
1
''
)()(2)(20)(2)()(20)(2)(2)(0
000
zyx
zyxxyzyxz
xzyzyxzxy
yzxzyxzyxT
iT
i
N
ii
N
iii
qqqqwhere
qqqqqqqqqqqqqqqqqqqqqqqqqqqqqqqqqqqq
ppwhere
ppyspp
+++=
úúúúú
û
ù
êêêêê
ë
é
+--+---+-++---+
=
=
= åå==
q.q
q.qq
q
*
*
QQq
q
( ) ( ) PT
YPY sst µµµµ QQq -=-= *q
Iterative Closest Point Algorithm
• Initialization
• Find correspondences
• Find alignment
• Apply alignment
• Compute residual error
Let’s do it …
Function Prototype
Initializations …
ICP Loop: Finding Correspondences
Loop to be continued …
( ) ( ) ( )222),( zkziykyixkxiki mpmpmpmpd -+-+-=
),(minarg,..1
kiNk
mpdjM=
=
ICP Loop: Finding/Applying Alignment
( )
å=
=
=+-=PN
ii
Piii
eE
Nitpsye
1
2||||
,...2,1R
Explanation to follow
Finding Alignment• Zero mean point sets.
• Quaternion computation.
• Rotation matrix computation.
• Scaling factor computation.
• Translational offset computation.
• Residual error computation.
Let’s do it …
Function Prototype
Test Given Point Sets
Zero Mean Point Sets
å=
=PN
ii
PP p
N 1
1µ
å=
=PN
ii
PY y
N 1
1µ
pii pp µ-='
Yii yy µ-='
Quaternion Computation
Rotation Matrix Computation
úúúúú
û
ù
êêêêê
ë
é
--
----
=
0
0
0
0
qqqqqqqqqqqqqqqq
xyz
xzy
yzx
zyx
Q
úúúúú
û
ù
êêêêê
ë
é
--
----
=
0
0
0
0
qqqqqqqqqqqqqqqq
xyz
xzy
yzx
zyx
Q
Scaling Factor Computation
÷÷÷÷
ø
ö
çççç
è
æ
=
å
å
=
=P
P
N
ii
N
ii
p
ys
1
2
1
2
||'||
||'||
Translation Offset Computation
( ) PYPT
Y sst µµµµ RQQ -=-=
Residual Error
( )
å=
=
=+-=PN
ii
Piii
eE
Nitpsye
1
2||||
,...2,1R
Experiments
Registering Line to Line
Registering Line to Line – with noise
Registering Line to Polygon
Registering Line to Polygon – with noise
Registering Circle to Polygon
Registering Circle to Circle
Registering Polygon to Circle
Registering Triangle to Triangle
Registering Line to Triangle
Registering Line to Triangle – with noise
Iterative Closest Point (ICP) Algorithm
Some Proofs …
A Tutorial on Rigid Registration
Iterative Closed Point (ICP)Some Proofs …
Aly A. FaragUniversity of Louisville
Acknowledgements: Help with these slides were provided by Amal Farag and Shireen Elhabian
Prove that quaternion multiplication preserves dot product, i.e. (qv).(qr) = (q.q) (v.r). What
happen in case q is a unit quaternion?
• Proof:
• In the case of a unit quaternion : (qv).(qr)=(v.r)
• A special case follows immediately:
(qr).(qr) = (q.q) (r.r)
that is the magnitude of a product is just the product of the magnitudes.
( )( ) ( ) ( ) ( ) ( )( ) ( )( )v.rq.qq.q
qrqv.
.===
==
IrvQrQvQrQvQr.Qv
TTT
T
Prove that the composite product leads to purely imaginary quaternion,hence it can be used to represent rotation, i.e. Rotating the vector (point) r bya unit quaternion q can be defined as r’ = qrq*, where r’ is a purely imaginaryquaternion representing the vector r after rotation by q.
Proof:
• The objective is representing the composite product as a matrix multiplied bya vector r. let’s find this matrix in terms of the matrices associated to the unitquaternion q and its conjugate.
Where Q and are the 4x4 matrices corresponding to the unit quaternion q.
( ) ( )
úúúúú
û
ù
êêêêê
ë
é
--
----
=
úúúúú
û
ù
êêêêê
ë
é
--
----
=
===
0
0
0
0
0
0
0
0
)(
qqqqqqqqqqqqqqqq
and
qqqqqqqqqqqqqqqq
where
xyz
xzy
yzx
zyx
xyz
xzy
yzx
zyx
TT
rQQQrQQr ** qqrq
Q
Proof: cont
is a purely imaginary quaternion if r is a purely imaginaryquaternion.
22220
2222000
02222
00
002222
0
0
0
0
0
0
0
0
0
)()(2)(20)(2)()(20)(2)(2)(0
000
zyx
zyxxyzyxz
xzyzyxzxy
yzxzyxzyx
xyz
xzy
yzx
zyx
xyz
xzy
yzx
zyx
T
qqqqwhere
qqqqqqqqqqqqqqqqqqqqqqqqqqqqqqqqqqqq
qqqqqqqqqqqqqqqq
qqqqqqqqqqqqqqqq
+++=
úúúúú
û
ù
êêêêê
ë
é
+--+---+-++---+
=
úúúúú
û
ù
êêêêê
ë
é
--
----
úúúúú
û
ù
êêêêê
ë
é
----
--=\
q.q
q.q
rQQ )(T
=\ *qrq
Proof: cont
• Since q is a quaternion, then Q and are orthogonal matricesby definition.
• Since q is a unit quaternion, then Q and are orthonormalmatrices,.
• Hence the lower-right-hand 3x3 sub-matrix of must also beorthonormal , hence it is the rotation matrix R that take r to r’such that r’=Rr.
• The expansion of provides an explicit method forcomputing the orthonormal rotation matrix R from thecomponents of the unit quaternion q.
■
Q
Q
QQT
QQT
Prove that that the unit quaternion that maximizes qTNq isthe eigenvector corresponding to the most positive eigenvalueof the matrix N.
Proof:
• To find the rotation that minimizes the sum of squares of erros,we have to find the quaternion q that maximizes qTNq subjectto the contraint that q.q = 1 (unit quaternion).
• The symmetric 4x4 matrix N will have four real eigenvalues, sayλ1, λ2, λ3 and λ4.
• A corresponding set of orthogonal unit eigenvectors v1, v2, v3and v4 can constructed such that Nvi = λivi where i = 1,2,3,4.
Proof: cont
• The eigenvectors span the 4D space, so an arbitrary quaternionq can be written as a linear combination in the form:
• Since the eigenvectors are orthogonal, we have:
• We know that this has to be equal to one since we are lookingfor a unit quaternion.
• Now
• We conclude that
44332211 vvvv aaaa +++=q
24
23
22
21 aaaa +++=q.q
444333222111 vλvλvλvλλ aaaa +++== qqN
( ) 4243
232
221
21 λλλλ aaaa +++== qq.qqT NN
Proof: cont
• Now, suppose we have arranged the eigenvalues in order suchthat λ1≥ λ2 ≥ λ3 ≥ λ4.
• Then we have:
( )
( )( )1
1
124
23
22
21
1241
231
221
21
4243
232
221
21
λλ
λλλλλλλλλ
==
+++=
+++£
+++==
q.q
qq.qqT
aaaa
aaaa
aaaaNN
Proof: cont
• Since we need to maximize qTNq , but we have qTNq ≤ λ1 ,hence qTNq is bounded above by λ1 which is the largesteigenvalue.
• Thus the maximum of qTNq is attained when qTNq = λ1 .
• This only happens if α1 = 1 and α2 = α3 = α4 = 0, i.e. :
which is the eigenvector corresponding to the largest positiveeigenvalue λ1.
■
1
44332211
vvvvv
=+++= aaaaq
References1. P.J. Besl, N.D. McKay, A method for registration of 3D shapes, IEEE
Transactions on Pattern Analysis and Machine Intellinegce 14 (1992) 239–254.
2. B. K. P. Horn, "Closed-form solution of absolute orientation using unitquaternions,"J. Opt. Soc. Amer. Avol. 4, no. 4, pp. 629-642, Apr. 1987.
3. www.cs.virginia.edu/~gfx/Courses/2004/Intro.Spring.04/Lectures/lecture05.ppt
4. gmm.fsksm.utm.my/~graphics/files/Week9-3D_Transformations.ppt
5. www.cs.tau.ac.il/~dcor/Graphics/adv-slides/ICP.ppt
Thank You
