Point cloud to point cloud rigid
transformations
Russell Taylor
600.445
1
600.445 Fall 2000-2015
Copyright © R. H. Taylor
Engineering Research Center for Computer Integrated Surgical Systems and Technology
Minimizing Rigid Registration Errors
Typically, given a set of points {ai } in one coordinate system
and another set of points {bi } in a second coordinate system
Goal is to find [R,p] that minimizes
η = ∑ ei • ei
i
where
e i = (R • a i + p) −bi
This is tricky, because of R.
2
600.445 Fall 2000-2015
Copyright © R. H. Taylor
Engineering Research Center for Computer Integrated Surgical Systems and Technology
1
Point cloud to point cloud registration
!
a ki
!
bki
!
! !
Riak + p = bk
3
600.445 Fall 2000-2015
Copyright © R. H. Taylor
Engineering Research Center for Computer Integrated Surgical Systems and Technology
Minimizing Rigid Registration Errors
Step 1: Compute
1 N !
∑ bi
N i =1
!
b" i = bi − b
1 N !
∑ ai
N i =1
!
a" i = ai − a
a=
b=
Step 2: Find R that minimizes
∑ (R ⋅ a"
i
− b" i ) 2
i
!
Step 3: Find p
!
p =b − R ⋅a
Step 4: Desired transformation is
!
F = Frame(R, p)
4
600.445 Fall 2000-2015
Copyright © R. H. Taylor
Engineering Research Center for Computer Integrated Surgical Systems and Technology
2
Point cloud to point cloud registration
! ki
a
a
b
!
b
i
k
!
! !
Riak + p = bk
5
600.445 Fall 2000-2015
Copyright © R. H. Taylor
Engineering Research Center for Computer Integrated Surgical Systems and Technology
Point cloud to point cloud registration
!
p = b −Ria
6
600.445 Fall 2000-2015
Copyright © R. H. Taylor
Engineering Research Center for Computer Integrated Surgical Systems and Technology
3
Rotation Estimation
7
600.445 Fall 2000-2015
Copyright © R. H. Taylor
Engineering Research Center for Computer Integrated Surgical Systems and Technology
Rotation Estimation
?
8
600.445 Fall 2000-2015
Copyright © R. H. Taylor
Engineering Research Center for Computer Integrated Surgical Systems and Technology
4
Rotation Estimation
9
600.445 Fall 2000-2015
Copyright © R. H. Taylor
Engineering Research Center for Computer Integrated Surgical Systems and Technology
Rotation Estimation
10
600.445 Fall 2000-2015
Copyright © R. H. Taylor
Engineering Research Center for Computer Integrated Surgical Systems and Technology
5
Point cloud to point cloud registration
11
600.445 Fall 2000-2015
Copyright © R. H. Taylor
Engineering Research Center for Computer Integrated Surgical Systems and Technology
Solving for R: iteration method
{ (
) }
Given !, a" i , b" i ,! , want to find R = arg min ∑ R"a i − b" i
2
i
Step 0: Make an initial guess R 0
⌣
Step 1: Given R k , compute b i = R −1
b" i
k
Step 2: Compute ΔR that minimizes
∑ (ΔR
⌣
a" i − b i )2
i
Step 3: Set R k +1 = R k ΔR
Step 4: Iterate Steps 1-3 until residual error is sufficiently small
(or other termination condition)
12
600.445 Fall 2000-2015
Copyright © R. H. Taylor
Engineering Research Center for Computer Integrated Surgical Systems and Technology
6
Iterative method: Getting Initial Guess
We want to find an approximate solution R 0 to
R 0 i ⎡⎣! a" i !⎤⎦ ≈ ⎡⎣! b" i !⎤⎦
One way to do this is as follows. Form matrices
A = ⎡⎣! a" i !⎤⎦ B= ⎡⎣! b" i !⎤⎦
Solve least-squares problem M3 x3 A3 xN ≈ B3 xN
Note : You may find it easier to solve AT3 xN MT3 x3 ≈ BT3 xN
Set R 0 = orthogonalize(M3 x3 ). Verify that R is a rotation
Our problem is now to solve R 0 ΔRA ≈ B. I.e., ΔRA ≈ R 0−1B
13
600.445 Fall 2000-2015
Copyright © R. H. Taylor
Engineering Research Center for Computer Integrated Surgical Systems and Technology
Iterative method: Solving for ΔR
Approximate ΔR as (I + skew(α)). I.e.,
ΔR • v ≈ v + α × v
for any vector v. Then, our least squares problem becomes
⌣
⌣
min ∑ (ΔR • a" i − bi ) 2 ≈ min ∑ (a" i − bi + α × a" i ) 2
ΔR
α
i
i
This is linear least squares problem in α.
Then compute ΔR (α ).
14
600.445 Fall 2000-2015
Copyright © R. H. Taylor
Engineering Research Center for Computer Integrated Surgical Systems and Technology
7
Direct Iterative approach for Rigid Frame
" "
" "
Given !, a i ,bi ,! , want to find F = argmin ∑ Fa − b
{ (
) }
2
i
Step 0: Make an initial guess F0
"
"
Step 1: Given Fk , compute aki = Fk a i
Step 2: Compute ΔF that minimizes
" 2
"
∑ ΔFaki − bi
i
Step 3: Set Fk+1 = ΔFFk
Step 4: Iterate Steps 1-3 until residual error is sufficiently small
(or other termination condition)
15
600.445 Fall 2000-2015
Copyright © R. H. Taylor
Engineering Research Center for Computer Integrated Surgical Systems and Technology
Direct Iterative approach for Rigid Frame
!
!
To solve for ΔF = argmin ∑ ΔFaki − bi
2
i
!
!
!
! !
! !
ΔFaki − bi ≈ α× aki + ε + aki − bi
! ! !
! !
α× aki + ε ≈ bi − aki
! ! ! ! !
sk(−aki )α + ε ≈ bi − aki
Solve the least-squares problem
⎡
⎡
"
"
" ⎤⎥ ⎡⎢ α! ⎤⎥ ⎢
⎢
⎢ sk(−a! k ) I ⎥ ⎢ ! ⎥ ≈ ⎢ b! − a! k
⎢
⎥⎢ ε ⎥ ⎢ i
i
i
⎢
⎢
⎥⎢
⎥
"
" ⎥⎦ ⎣
"
⎢⎣
⎦ ⎢⎣
! !
Now set ΔF=[ΔR(α),ε ]
16
600.445 Fall 2000-2015
Copyright © R. H. Taylor
⎤
⎥
⎥
⎥
⎥
⎥⎦
Engineering Research Center for Computer Integrated Surgical Systems and Technology
8
Direct Techniques to solve for R
•  Method due to K. Arun, et. al., IEEE PAMI, Vol 9, no
5, pp 698-700, Sept 1987
Step 1: Compute
⎡ a! i,x b!i,x a! i,x b!i,y a! i,x b!i,z ⎤
⎢
⎥
H = ∑ ⎢ a! i,y b!i,x a! i,y b!i,y a! i,y b!i,z ⎥
i ⎢
⎥
⎢⎣ a! i,z b!i,x a! i,z b!i,y a! i,z b!i,z ⎥⎦
Step 2: Compute the SVD of H = USV t
Step 3: R = VU t
Step 4: Verify Det(R) = 1. If not, then algorithm may fail.
•  Failure is rare, and mostly fixable. The paper has details.
17
600.445 Fall 2000-2015
Copyright © R. H. Taylor
Engineering Research Center for Computer Integrated Surgical Systems and Technology
Quarternion Technique to solve for R
•  B.K.P. Horn, “Closed form solution of absolute
orientation using unit quaternions”, J. Opt. Soc.
America, A vol. 4, no. 4, pp 629-642, Apr. 1987.
•  Method described as reported in Besl and McKay,
“A method for registration of 3D shapes”, IEEE
Trans. on Pattern Analysis and Machine
Intelligence, vol. 14, no. 2, February 1992.
•  Solves a 4x4 eigenvalue problem to find a unit
quaternion corresponding to the rotation
•  This quaternion may be converted in closed form to
get a more conventional rotation matrix
18
600.445 Fall 2000-2015
Copyright © R. H. Taylor
Engineering Research Center for Computer Integrated Surgical Systems and Technology
9
Digression: quaternions
Invented by Hamilton in 1843. Can be thought of as
4 elements:
scalar & vector:
Complex number:
q = ⎡⎣ q0 ,q1 ,q2 ,q3 ⎤⎦
!
!
q = s + v = ⎡⎣ s, v ⎤⎦
q = q0 + q1i + q2 j + q3k
where i 2 = j 2 = k 2 = i j k = −1
Properties:
!
!
!
Linearity: λq1 + µq 2 = ⎡⎣ λs1 + µs2 ,λv1 + µv 2 ⎤⎦
!
!
Conjugate:
q* = s − v = ⎡⎣ s,− v ⎤⎦
! ! !
! ! !
Product:
q1 " q 2 = ⎡⎣ s1s2 − v1 i v 2 ,s1v 2 + s2 v1 + v1 × v 2 ⎤⎦
!
!
Transform vector:
q " p = q " ⎡⎣0, p ⎤⎦ " q*
! !
Norm:
q = s 2 + v • v = q0 2 + q12 + q2 2 + q32
19
600.445 Fall 2000-2015
Copyright © R. H. Taylor
Engineering Research Center for Computer Integrated Surgical Systems and Technology
Digression continued: unit quaternions
We can associate a rotation by angle θ about an
!
axis n with the unit quaternion:
!
θ
θ !⎤
⎡
Rot (n, θ) ⇔ ⎢cos , sin n ⎥
2
2 ⎦
⎣
Exercise: Demonstrate this relationship. I.e., show
!
! ⎡
! ⎡
θ
θ !⎤
θ
θ !⎤
Rot ((n, θ)ip = ⎢cos , sin n ⎥ " [0, p ]" ⎢cos , − sin n ⎥
2
2 ⎦
2
2 ⎦
⎣
⎣
20
600.445 Fall 2000-2015
Copyright © R. H. Taylor
Engineering Research Center for Computer Integrated Surgical Systems and Technology
10
A bit more on quaternions
Exercise: show by substitution that the various formulations for
quaternions are equivalent
A few web references:
http://mathworld.wolfram.com/Quaternion.html
http://en.wikipedia.org/wiki/Quaternion
http://en.wikipedia.org/wiki/Quaternions_and_spatial_rotation
http://www.euclideanspace.com/maths/algebra/
realNormedAlgebra/quaternions/index.htm
21
600.445 Fall 2000-2015
Copyright © R. H. Taylor
Engineering Research Center for Computer Integrated Surgical Systems and Technology
Rotation matrix from unit quaternion
q = ⎡⎣q0 ,q1,q2 ,q3 ⎤⎦ ;
q =1
⎡ 2
2
2
2
⎢ q0 + q1 − q2 − q3
⎢
R(q) = ⎢ 2(q1q2 + q0 q3 )
⎢
⎢ 2(q q − q q )
⎢⎣
1 3
0 2
22
600.445 Fall 2000-2015
Copyright © R. H. Taylor
2(q1q2 − q0 q3 )
q02 − q12 + q22 − q32
2(q2q3 + q0 q1 )
2(q1q3 + q0 q2 )
2(q2q3 − q0 q1 )
2
0
q − q12 − q22 + q32
⎤
⎥
⎥
⎥
⎥
⎥
⎥⎦
Engineering Research Center for Computer Integrated Surgical Systems and Technology
11
Unit quaternion from rotation matrix
⎡ r
⎢ xx
R(q) = ⎢ rxy
⎢
⎢ rxz
⎣
ryx
ryy
ryz
rzx ⎤
⎥
rzy ⎥ ;
⎥
rzz ⎥
⎦
a0 = 1+ rxx + ryy + rzz ; a1 = 1+ rxx − ryy − rzz
a2 = 1− rxx + ryy − rzz ; a3 = 1− rxx − ryy + rzz
a0 = max{ak } a1 = max{ak } a2 = max{ak } a3 = max{ak }
q0 =
q1 =
q2 =
q3 =
23
600.445 Fall 2000-2015
Copyright © R. H. Taylor
a0
2
rxy − ryx
4q0
rzx − rxz
4q0
ryz − rzy
4q0
q0 =
q1 =
q2 =
q3 =
ryz − rzy
4q1
a1
2
rxy + ryx
4q1
rxz + rzx
4q1
q0 =
q1 =
q2 =
q3 =
rzx − rxz
4q2
rxy + ryx
4q2
a2
2
ryz + rzy
4q2
q0 =
q1 =
q2 =
q3 =
rxy − ryx
4q3
rxz + rzx
4q3
ryz + rzy
4q3
a3
2
Engineering Research Center for Computer Integrated Surgical Systems and Technology
Rotation axis and angle from rotation matrix
Many options, including direct trigonemetric solution.
But this works:
!
[n,θ ] ← ExtractAxisAngle(R)
{
!
[s, v] ← ConvertToQuaternion(R)
! !
!
return([ v / v ,2atan(s / v )
}
24
600.445 Fall 2000-2015
Copyright © R. H. Taylor
Engineering Research Center for Computer Integrated Surgical Systems and Technology
12
Quaternion method for R
Step 1: Compute
⎡ a! b!
⎢ i,x i,x
H = ∑ ⎢ a!i, y b!i,x
i ⎢
⎢ a!i,z b!i,x
⎣
a!i,x b!i, y
a!i, y b!i, y
a!i,z b!i, y
a!i,x b!i,z ⎤
⎥
a!i, y b!i,z ⎥
⎥
a!i,z b!i,z ⎥
⎦
Step 2: Compute
⎡ trace(H)
ΔT
G=⎢
T
⎢
Δ
H + H − trace(H)I
⎣
⎤
⎥
⎥
⎦
where Δ T = ⎡ H 2,3 − H3,2 H3,1 − H1,3 H1,2 − H 2,1 ⎤
⎣
⎦
Step 3: Compute eigen value decomposition of G
diag(λ)=QT GQ
Step 4: The eigenvector Q k = ⎡⎣ q0 ,q1 ,q2 ,q3 ⎤⎦ corresponding to
the largest eigenvalue λ k is a unit quaternion corresponding
to the rotation.
25
600.445 Fall 2000-2015
Copyright © R. H. Taylor
Engineering Research Center for Computer Integrated Surgical Systems and Technology
Another Quaternion Method for R
!
Let q = s + v be the unit quaternion corresponding
!
!
!
!
to R. Let a and b be vectors with b = R i a then we have
the quaternion equation
!
!
!
!
(s + v) i (0 + a)(s − v) = 0 + b
!
!
!
!
!
!
!
(s + v) i (0 + a) = (0 + b) i (s + v) since (s − v)(s + v) = 1+ 0
Expanding the scalar and vector parts gives
! !
! !
−v i a = −v i b
! ! !
! ! !
sa + v × a = sb + b × v
Rearranging ...
! ! !
(b − a) i v = 0
! !
! ! ! !
s(b − a) + (b + a) × v = 03
26
600.445 Fall 2000-2015
Copyright © R. H. Taylor
Engineering Research Center for Computer Integrated Surgical Systems and Technology
13
Another Quaternion Method for R
Expressing this as a matrix equation
⎡
⎢ 0
⎢ ! !
⎢⎣ b − a
(
!
(b − a! )
! !
sk
b
) ( + a)
T
⎤
⎥ ⎡ s ⎤ ⎡ !0 ⎤
⎥ ⎢ v! ⎥ = ⎢ 0 ⎥
⎦ ⎢⎣ 3 ⎥⎦
⎥⎦ ⎣
!
! T
If we now express the quaternion q as a 4-vector q = ⎡⎣s, v ⎤⎦ ,
we can express the rotation problem as the constrained linear
system
! ! ! !
M(a, b)q = 0 4
!
q =1
27
600.445 Fall 2000-2015
Copyright © R. H. Taylor
Engineering Research Center for Computer Integrated Surgical Systems and Technology
Another Quaternion Method for R
In general, we have many observations, and we want to
solve the problem in a least squares sense:
!
!
min Mq subject to q = 1
where
!
⎡ M(a! ,b ) ⎤
1 1
⎢
⎥
M= ⎢
⎥ and n is the number of observations
"
!
!
⎢ M(a ,b ) ⎥
n n ⎥
⎢⎣
⎦
Taking the singular value decomposition of M=UΣVT reduces
this to the easier problem
!
!
!
!
!
!
min UΣVT qX = U Σy = Σy subject to y = VT q = q = 1
( )
28
600.445 Fall 2000-2015
Copyright © R. H. Taylor
Engineering Research Center for Computer Integrated Surgical Systems and Technology
14
Another Quaternion Method for R
This problem is just
⎡
⎢
⎢
!
min Σy = ⎢
⎢
⎢
⎢⎣
σ1
0
0
0
σ2
0
0
0
σ3
0
0
0
0 ⎤
⎥
0 ⎥!
⎥y
0 ⎥
σ 4 ⎥⎥
⎦
!
subject to y = 1
where σ i are the singular values. Recall that SVD routines typically
return the σ i ≥ 0 and sorted in decreasing magnitude. So σ 4
!
!
is the smallest singular value and the value of y with y = 1
T
!
!
that minimizes Σy is y= ⎡⎣0,0,0,1⎤⎦ . The corresponding value
!
!
!
of q is given by q = Vy = V4 . Where V4 is the 4th column of V.
29
600.445 Fall 2000-2015
Copyright © R. H. Taylor
Engineering Research Center for Computer Integrated Surgical Systems and Technology
Non-reflective spatial similarity (rigid+scale)
!
a ki
!
bki
!
! !
σRiak + p = bk
30
600.445 Fall 2000-2015
Copyright © R. H. Taylor
Engineering Research Center for Computer Integrated Surgical Systems and Technology
15
Non-reflective spatial similarity
Step 1: Compute
1 N !
∑b
N i=1 i
!
b" i = b i − b
1 N !
∑a
N i=1 i
!
a" i = a i − a
a=
b=
Step 2: Estimate scale
σ=
∑
∑
i
b" i
i
a" i
Step 3: Find R that minimizes
∑ (R ⋅ ( σ"a ) −
i
b" i )2
i
!
Step 4: Find p
!
p = b − R ⋅a
Step 5: Desired transformation is
!
F = SimilarityFrame(σ,R, p)
31
600.445 Fall 2000-2015
Copyright © R. H. Taylor
Engineering Research Center for Computer Integrated Surgical Systems and Technology
16