closed-form solution of absolute orientation using …hod/papers/unsorted/absolute...introduction...
TRANSCRIPT
![Page 1: Closed-Form Solution Of Absolute Orientation Using …hod/papers/Unsorted/absolute...Introduction Quaternions Solving the Absolute Orientation Problem Summary Closed-Form Solution](https://reader031.vdocuments.net/reader031/viewer/2022022522/5b3206d07f8b9a2c328d0c50/html5/thumbnails/1.jpg)
IntroductionQuaternions
Solving the Absolute Orientation ProblemSummary
Closed-Form Solution Of Absolute OrientationUsing Unit Quaternions
Berthold K. P. Horn
Jan Oberländer
Department of Computer and Information Sciences
November 11, 2004
Jan Oberländer Absolute Orientation using Unit Quaternions
![Page 2: Closed-Form Solution Of Absolute Orientation Using …hod/papers/Unsorted/absolute...Introduction Quaternions Solving the Absolute Orientation Problem Summary Closed-Form Solution](https://reader031.vdocuments.net/reader031/viewer/2022022522/5b3206d07f8b9a2c328d0c50/html5/thumbnails/2.jpg)
IntroductionQuaternions
Solving the Absolute Orientation ProblemSummary
Outline
1 Introduction
2 Quaternions
3 Solving the Absolute Orientation ProblemTranslationScaleRotation
Jan Oberländer Absolute Orientation using Unit Quaternions
![Page 3: Closed-Form Solution Of Absolute Orientation Using …hod/papers/Unsorted/absolute...Introduction Quaternions Solving the Absolute Orientation Problem Summary Closed-Form Solution](https://reader031.vdocuments.net/reader031/viewer/2022022522/5b3206d07f8b9a2c328d0c50/html5/thumbnails/3.jpg)
IntroductionQuaternions
Solving the Absolute Orientation ProblemSummary
The Problem
• Given: two sets of corresponding points in different coordinatesystems
• Task: find the absolute orientation of the two systems (scale,rotation, translation)
• Previous approaches:• Only iterative solutions• Only close to least-squares solution• Only handle three points• Selectively neglect constraints
• This paper:• Closed-form, exact least-squares solution• No constraints neglected
Jan Oberländer Absolute Orientation using Unit Quaternions
![Page 4: Closed-Form Solution Of Absolute Orientation Using …hod/papers/Unsorted/absolute...Introduction Quaternions Solving the Absolute Orientation Problem Summary Closed-Form Solution](https://reader031.vdocuments.net/reader031/viewer/2022022522/5b3206d07f8b9a2c328d0c50/html5/thumbnails/4.jpg)
IntroductionQuaternions
Solving the Absolute Orientation ProblemSummary
Quaternions
Quaternion Notation
q̊ = q0 + iqx + jqy + kqz
q̊ = (q0, qx, qy, qz)T
Quaternions: Basic Properties
i2 = −1 j2 = −1 k2 = −1
ij = k jk = i ki = j
ji = −k kj = −i ik = −j
Jan Oberländer Absolute Orientation using Unit Quaternions
![Page 5: Closed-Form Solution Of Absolute Orientation Using …hod/papers/Unsorted/absolute...Introduction Quaternions Solving the Absolute Orientation Problem Summary Closed-Form Solution](https://reader031.vdocuments.net/reader031/viewer/2022022522/5b3206d07f8b9a2c328d0c50/html5/thumbnails/5.jpg)
IntroductionQuaternions
Solving the Absolute Orientation ProblemSummary
Products of Quaternions
Quaternion Product as a Matrix–Vector Product
r̊q̊ =
r0 − rx − ry − rz
rx r0 − rz ry
ry rz r0 − rx
rz − ry rx r0
q̊ = Rq̊
q̊r̊ =
r0 − rx − ry − rz
rx r0 rz − ry
ry − rz r0 rx
rz ry − rx r0
q̊ = R̄q̊
Watch Out!
The quaternion product is not commutative!
Jan Oberländer Absolute Orientation using Unit Quaternions
![Page 6: Closed-Form Solution Of Absolute Orientation Using …hod/papers/Unsorted/absolute...Introduction Quaternions Solving the Absolute Orientation Problem Summary Closed-Form Solution](https://reader031.vdocuments.net/reader031/viewer/2022022522/5b3206d07f8b9a2c328d0c50/html5/thumbnails/6.jpg)
IntroductionQuaternions
Solving the Absolute Orientation ProblemSummary
Dot Product
Dot Product of Two Quaternions
p̊ · q̊ = p0q0 + pxqx + pyqy + pzqz
‖q̊‖2 = q̊ · q̊
Conjugate and Inverse
p̊∗ = q0 − iqx − jqy − kqz
q̊q̊∗ = q20 + q2
x + q2y + q2
z = q̊ · q̊
A non-zero quaternion has an inverse
q̊−1 =q̊∗
q̊ · q̊
Jan Oberländer Absolute Orientation using Unit Quaternions
![Page 7: Closed-Form Solution Of Absolute Orientation Using …hod/papers/Unsorted/absolute...Introduction Quaternions Solving the Absolute Orientation Problem Summary Closed-Form Solution](https://reader031.vdocuments.net/reader031/viewer/2022022522/5b3206d07f8b9a2c328d0c50/html5/thumbnails/7.jpg)
IntroductionQuaternions
Solving the Absolute Orientation ProblemSummary
Useful Properties of Quaternions
Dot Products
(q̊p̊) · (q̊r̊) = (q̊ · q̊)(p̊ · r̊)(p̊q̊) · r̊ = p̊ · (r̊q̊∗)
Representing Vectors
Let r = (x, y, z)T ∈ R3, then
r̊ = 0 + ix + iy + iz .
Matrices associated with such purely imaginary quaternions areskew symmetric: (go back to matrices)
RT = −R R̄T = −R̄
Jan Oberländer Absolute Orientation using Unit Quaternions
![Page 8: Closed-Form Solution Of Absolute Orientation Using …hod/papers/Unsorted/absolute...Introduction Quaternions Solving the Absolute Orientation Problem Summary Closed-Form Solution](https://reader031.vdocuments.net/reader031/viewer/2022022522/5b3206d07f8b9a2c328d0c50/html5/thumbnails/8.jpg)
IntroductionQuaternions
Solving the Absolute Orientation ProblemSummary
Representing Rotations With Quaternions
Rotation Representation Using a Unit Quaternion
q̊ = cosφ
2+ sin
φ
2(iωx + jωy + kωz)︸ ︷︷ ︸
unit vector ω
The imaginary part gives the direction of the axis of rotation. Theangle is encoded into the real part and the magnitude of theimaginary part.
Rotating a Vector
Note that a vector is represented using an imaginary quaternion!
r̊′ = q̊r̊q̊∗ r̊′′ = p̊r̊′p̊∗ = p̊(q̊r̊q̊∗)p̊∗ = p̊q̊r̊(p̊q̊)∗
Jan Oberländer Absolute Orientation using Unit Quaternions
![Page 9: Closed-Form Solution Of Absolute Orientation Using …hod/papers/Unsorted/absolute...Introduction Quaternions Solving the Absolute Orientation Problem Summary Closed-Form Solution](https://reader031.vdocuments.net/reader031/viewer/2022022522/5b3206d07f8b9a2c328d0c50/html5/thumbnails/9.jpg)
IntroductionQuaternions
Solving the Absolute Orientation ProblemSummary
TranslationScaleRotation
Solving the Absolute Orientation Problem
• Given: n points, each measured in a left and right coordinatesystem
{rl,i} and {rr,i}• Try to find a transformation of the form
rr = sR(rl) + r0
from the left to the right coordinate system.• There will be errors
ei = rr,i − sR(rl,i)− r0
• Minimize the sum of squares of errorsn∑
i=1
‖ei‖2
Jan Oberländer Absolute Orientation using Unit Quaternions
![Page 10: Closed-Form Solution Of Absolute Orientation Using …hod/papers/Unsorted/absolute...Introduction Quaternions Solving the Absolute Orientation Problem Summary Closed-Form Solution](https://reader031.vdocuments.net/reader031/viewer/2022022522/5b3206d07f8b9a2c328d0c50/html5/thumbnails/10.jpg)
IntroductionQuaternions
Solving the Absolute Orientation ProblemSummary
TranslationScaleRotation
Translation
Working Relative to the Centroids
r̄l =1
n
n∑i=1
rl,i r̄r =1
n
n∑i=1
rr,i
r′l,i = rl,i − r̄l r′r,i = rr,i − r̄r
• Rewrite the error term:
ei = r′r,i − sR(r′l,i)− r′0 where r′0 = r0 − r̄r + sR(r̄l)
• The sum of squares of errors becomes
n∑i=1
‖ei‖2 =n∑
i=1
‖r′r,i − sR(r′l,i)− r′0‖2
Jan Oberländer Absolute Orientation using Unit Quaternions
![Page 11: Closed-Form Solution Of Absolute Orientation Using …hod/papers/Unsorted/absolute...Introduction Quaternions Solving the Absolute Orientation Problem Summary Closed-Form Solution](https://reader031.vdocuments.net/reader031/viewer/2022022522/5b3206d07f8b9a2c328d0c50/html5/thumbnails/11.jpg)
IntroductionQuaternions
Solving the Absolute Orientation ProblemSummary
TranslationScaleRotation
Translation
• Decompose the sum:
n∑i=1
‖r′r,i − sR(r′l,i)− r′0‖2
=n∑
i=1
‖r′r,i − sR(r′l,i)‖2
︸ ︷︷ ︸independent of translation
− 2r′0 ·
(n∑
i=1
r′r,i − sR(r′l,i)
)︸ ︷︷ ︸
=0
+n‖r′0‖2
• The total error is minimized with r′0 = 0, i.e. r0 = r̄r − sR(r̄l).• Remaining error term: ei = r′r,i − sR(r′l,i). Now minimize
n∑i=1
‖r′r,i − sR(r′l,i)‖2 .
Jan Oberländer Absolute Orientation using Unit Quaternions
![Page 12: Closed-Form Solution Of Absolute Orientation Using …hod/papers/Unsorted/absolute...Introduction Quaternions Solving the Absolute Orientation Problem Summary Closed-Form Solution](https://reader031.vdocuments.net/reader031/viewer/2022022522/5b3206d07f8b9a2c328d0c50/html5/thumbnails/12.jpg)
IntroductionQuaternions
Solving the Absolute Orientation ProblemSummary
TranslationScaleRotation
Finding the Scale
• Decompose the sum:
n∑i=1
‖r′r,i − sR(r′l,i)‖2
=n∑
i=1
‖r′r,i‖2 − 2s
n∑i=1
r′r,i ·R(r′l,i) + s2n∑
i=1
‖r′l,i‖2
R()
= Sr − 2sD + s2Sl
• Complete the square:
(s√
Sl −D/√
Sl)2︸ ︷︷ ︸
=0
+ (SrSl −D2)/Sl︸ ︷︷ ︸independent of scale
• Best scale is s = D/Sl.
Jan Oberländer Absolute Orientation using Unit Quaternions
![Page 13: Closed-Form Solution Of Absolute Orientation Using …hod/papers/Unsorted/absolute...Introduction Quaternions Solving the Absolute Orientation Problem Summary Closed-Form Solution](https://reader031.vdocuments.net/reader031/viewer/2022022522/5b3206d07f8b9a2c328d0c50/html5/thumbnails/13.jpg)
IntroductionQuaternions
Solving the Absolute Orientation ProblemSummary
TranslationScaleRotation
Finding the Scale
s = D/Sl =
n∑i=1
r′r,i ·R(r′l,i)
n∑i=1
‖r′l,i‖2
• This scale factor is not symmetric! When going from the rightto the left system, we get
s̄ 6= 1
s
• Use a symmetrical expression for the error term instead:
e′i =1√sr′r,i −
√sR(r′l,i) =
ei√s
Jan Oberländer Absolute Orientation using Unit Quaternions
![Page 14: Closed-Form Solution Of Absolute Orientation Using …hod/papers/Unsorted/absolute...Introduction Quaternions Solving the Absolute Orientation Problem Summary Closed-Form Solution](https://reader031.vdocuments.net/reader031/viewer/2022022522/5b3206d07f8b9a2c328d0c50/html5/thumbnails/14.jpg)
IntroductionQuaternions
Solving the Absolute Orientation ProblemSummary
TranslationScaleRotation
Finding the Scale
• Total error is then
1
s
n∑i=1
‖r′r,i‖2−2n∑
i=1
r′r,i·R(r′l,i)+s
n∑i=1
‖r′l,i‖2 =1
sSr−2D−sSl
• Complete the square (slightly different from Horn p. 632)
(√
s√
Sl −√
Sr/√
s)2︸ ︷︷ ︸=0
+ 2(√
SlSr −D)︸ ︷︷ ︸independent of scale
• Best scale is now independent of rotation:
s =√
Sr/Sl =
√√√√ n∑i=1
‖r′r,i‖2
/ n∑i=1
‖r′l,i‖2
Jan Oberländer Absolute Orientation using Unit Quaternions
![Page 15: Closed-Form Solution Of Absolute Orientation Using …hod/papers/Unsorted/absolute...Introduction Quaternions Solving the Absolute Orientation Problem Summary Closed-Form Solution](https://reader031.vdocuments.net/reader031/viewer/2022022522/5b3206d07f8b9a2c328d0c50/html5/thumbnails/15.jpg)
IntroductionQuaternions
Solving the Absolute Orientation ProblemSummary
TranslationScaleRotation
Rotation
• Remaining error term to be minimized:
(SrSl −D2)/Sl or 2(√
SlSr −D)(asymmetric) (symmetric)
I.e., maximize
D =n∑
i=1
r′r,i ·R(r′l,i)
• Use the quaternion representation: find the unit quaternion q̊that maximizes
D =n∑
i=1
(q̊r̊′l,iq̊∗) · r̊′r,i
Jan Oberländer Absolute Orientation using Unit Quaternions
![Page 16: Closed-Form Solution Of Absolute Orientation Using …hod/papers/Unsorted/absolute...Introduction Quaternions Solving the Absolute Orientation Problem Summary Closed-Form Solution](https://reader031.vdocuments.net/reader031/viewer/2022022522/5b3206d07f8b9a2c328d0c50/html5/thumbnails/16.jpg)
IntroductionQuaternions
Solving the Absolute Orientation ProblemSummary
TranslationScaleRotation
Rotation
Using (p̊q̊) · r̊ = p̊ · (r̊q̊∗) from earlier, rewrite the term asn∑
i=1
(q̊r̊′l,iq̊∗) · r̊′r,i =
n∑i=1
(q̊r̊′l,i) · (r̊′r,iq̊)
=n∑
i=1
(R̄l,iq̊) · (Rr,iq̊)
=n∑
i=1
q̊T R̄Tl,iRr,iq̊
= q̊T
(n∑
i=1
R̄Tl,iRr,i
)q̊
= q̊T
(n∑
i=1
Ni
)q̊ = q̊T Nq̊
Jan Oberländer Absolute Orientation using Unit Quaternions
![Page 17: Closed-Form Solution Of Absolute Orientation Using …hod/papers/Unsorted/absolute...Introduction Quaternions Solving the Absolute Orientation Problem Summary Closed-Form Solution](https://reader031.vdocuments.net/reader031/viewer/2022022522/5b3206d07f8b9a2c328d0c50/html5/thumbnails/17.jpg)
IntroductionQuaternions
Solving the Absolute Orientation ProblemSummary
TranslationScaleRotation
Rotation
Utilize the 3× 3 matrix
M :=n∑
i=1
r′l,ir′Tr,i :=
Sxx Sxy Sxz
Syx Syy Syz
Szx Szy Szz
for a convenient representation of
N =
(Sxx + Syy + Szz Syz − Szy Szx − Sxz Sxy − Syx
Syz − Szy Sxx − Syy − Szz Sxy + Syx Szx + SxzSzx − Sxz Sxy + Syx − Sxx + Syy − Szz Syz + SzySxy − Syx Szx + Sxz Syz + Szy − Sxx − Syy + Szz
)Note that N is symmetric, and trace(N) = 0. This contains allinformation to solve the least-squares problem for rotation!
Jan Oberländer Absolute Orientation using Unit Quaternions
![Page 18: Closed-Form Solution Of Absolute Orientation Using …hod/papers/Unsorted/absolute...Introduction Quaternions Solving the Absolute Orientation Problem Summary Closed-Form Solution](https://reader031.vdocuments.net/reader031/viewer/2022022522/5b3206d07f8b9a2c328d0c50/html5/thumbnails/18.jpg)
IntroductionQuaternions
Solving the Absolute Orientation ProblemSummary
TranslationScaleRotation
Rotation
• The unit quaternion that maximizes q̊TNq̊ is the eigenvectorto the most positive eigenvalue of N
• To find this eigenvalue solve the quartic obtained from
det(N − λI) = 0
Use e.g. Ferrari’s method.
• For the eigenvalue λm, the eigenvector e̊m is found by solving
(N − λmI)e̊m = 0
• A lot easier nowadays using SVD. . .
Jan Oberländer Absolute Orientation using Unit Quaternions
![Page 19: Closed-Form Solution Of Absolute Orientation Using …hod/papers/Unsorted/absolute...Introduction Quaternions Solving the Absolute Orientation Problem Summary Closed-Form Solution](https://reader031.vdocuments.net/reader031/viewer/2022022522/5b3206d07f8b9a2c328d0c50/html5/thumbnails/19.jpg)
IntroductionQuaternions
Solving the Absolute Orientation ProblemSummary
TranslationScaleRotation
The Algorithm
1 Find the centroids r̄l, r̄r, substract from measurements
2 For each pair(r′l = (x′l, y
′l, z
′l), r
′r = (x′r, y
′r, z
′r))compute all
possible products x′lx′r, x
′ly
′r, . . . , z
′lz
′r and add up to obtain
Sxx, Sxy, . . . , Szz
3 Compute elements of N
4 Calculate the coefficients of the quartic and solve quartic
5 Pick the most positive root and obtain correspondingeigenvector. The quaternion representing the rotation is a unitvector in the same direction
6 Compute the scale
7 Compute the translation
Jan Oberländer Absolute Orientation using Unit Quaternions
![Page 20: Closed-Form Solution Of Absolute Orientation Using …hod/papers/Unsorted/absolute...Introduction Quaternions Solving the Absolute Orientation Problem Summary Closed-Form Solution](https://reader031.vdocuments.net/reader031/viewer/2022022522/5b3206d07f8b9a2c328d0c50/html5/thumbnails/20.jpg)
IntroductionQuaternions
Solving the Absolute Orientation ProblemSummary
TranslationScaleRotation
Special Cases and Extensions
• If points are coplanar (e.g. only three points given), thecalculation simplifies greatly
• Can also use weighted errors:
• Minimize weighted sum of errors:n∑
i=1wi‖ei‖2
• Calculate weighted centroids: rl =n∑
i=1wirl,i
/ n∑i=1
wi etc.
• Change scale factor calculation: Sr =n∑
i=1wi‖r′
r,i‖2 etc.
• Change components of matrix for rotation recovery:Sxx =
∑ni=1 wix
′l,ix
′r,i
Jan Oberländer Absolute Orientation using Unit Quaternions
![Page 21: Closed-Form Solution Of Absolute Orientation Using …hod/papers/Unsorted/absolute...Introduction Quaternions Solving the Absolute Orientation Problem Summary Closed-Form Solution](https://reader031.vdocuments.net/reader031/viewer/2022022522/5b3206d07f8b9a2c328d0c50/html5/thumbnails/21.jpg)
IntroductionQuaternions
Solving the Absolute Orientation ProblemSummary
TranslationScaleRotation
MatLab Implementation
Jan Oberländer Absolute Orientation using Unit Quaternions
![Page 22: Closed-Form Solution Of Absolute Orientation Using …hod/papers/Unsorted/absolute...Introduction Quaternions Solving the Absolute Orientation Problem Summary Closed-Form Solution](https://reader031.vdocuments.net/reader031/viewer/2022022522/5b3206d07f8b9a2c328d0c50/html5/thumbnails/22.jpg)
IntroductionQuaternions
Solving the Absolute Orientation ProblemSummary
Summary
• I presented a closed-form solution to the absolute orientationproblem
• Given a mechanism for SVD or eigenvalue computation, thesolution is straightforward
• Non-Gaussian noise, statistical outliers are not handled well
• Numerical stability?
• Project• Combine with registration?• Use for non-rigid motion? E.g., by doing absolute orientation
on local point sets?
Jan Oberländer Absolute Orientation using Unit Quaternions
![Page 23: Closed-Form Solution Of Absolute Orientation Using …hod/papers/Unsorted/absolute...Introduction Quaternions Solving the Absolute Orientation Problem Summary Closed-Form Solution](https://reader031.vdocuments.net/reader031/viewer/2022022522/5b3206d07f8b9a2c328d0c50/html5/thumbnails/23.jpg)
Appendix References
References
Berthold K. P. Horn.Closed-form solution of absolute orientation using unitquaternions. Journal of the Optical Society of America A,4(4):629–642, April 1987.
Ross J. Micheals, Terrance E. Boult.On the Robustness of Absolute Orientation. 2000.www.cs.uccs.edu/~tboult/PAPERS/
F. Sansò.An exact solution of the roto-translation problem.Photogrammetria 29:203–216, 1973.
Jan Oberländer Absolute Orientation using Unit Quaternions