Point cloud to point cloud rigid transformations. Minimizing Rigid Registration Errors

(1)

Engineering Research Center for Computer Integrated Surgical Systems and Technology

1 Point cloud to point cloud rigid

transformations

Russell Taylor

600.445 Minimizing Rigid Registration Errors

Typically, given a set of points {a

_i

} in one coordinate system

and another set of points {b

_i

} in a second coordinate system

Goal is to find [R,p] that minimizes

η

=

e

_i

• e

_i

i

∑

where

e

_i

=

(R

• a

_i

+

p)

−b

_i

This is tricky, because of R.

(2)

3 Point cloud to point cloud registration

ki

a

!

ki

b

!

R

i

a

!

_k

+

p

!

=

b

!

_k

4 Minimizing Rigid Registration Errors

1

2

1

1 (

)

( , )

N

i

N

Frame

=

−

=

−

⋅ −

=

− ⋅

=

∑

a

b

a

b

R

R a

b

p

p b R a

F

R p

!

_"

"

!

Step 1: Compute

Step 2: Find that minimizes

Step 3: Find

(3)

5 Point cloud to point cloud registration

R

i

a

!

_k

+

p

!

=

b

!

_k

a

b

ki

a

!

ki

b

!

Point cloud to point cloud registration

!

(4)

7 Rotation Estimation

8

?

(5)

9 Rotation Estimation

(6)

11 Point cloud to point cloud registration

12 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

₀

Step 1: Given

R

_k

,

compute

b

⌣

_i

=

R

−1

_k

b

"

_i

Step 2: Compute

Δ

R

that minimizes

(

Δ

R

i

∑

a

"

_i

−

b

⌣

_i

)

2 Step 3: Set

R

_k

₊

₁

=

R

_k

Δ

R

Step 4: Iterate Steps 1-3 until residual error is sufficiently small

(or other termination condition)

(7)

13 Iterative method: Getting Initial Guess

We want to find an approximate solution

R

₀

to

R

₀

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

M

₃

_x

₃

A

₃

_xN

≈

B

₃

_xN

Note

: You may find it easier to solve

A

T

₃

_xN

_M

3 x

3 T

_≈

_B

3 xN

T

Set

R

₀

=

orthogonalize

(

M

₃

_x

₃

). Verify that

R

is a rotation

Our problem is now to solve

R

₀

ΔRA

≈

B

. I.e.,

ΔRA

≈

R

₀

−1

_B

Iterative method: Solving for

Δ

R

2

2 ( )).

.

(

)

min

(

)

.

i

skew

Δ

α

Δ

+

α

Δ

• ≈

+

α×

Δ

• −

≈

−

+

α×

α

∑

R

I

R v v

v

R a

"

b

⌣

a

"

b

⌣

a

"

Approximate

as (

I.e.,

for any vector Then, our least squares problem becomes

min

This is linear least squares problem in

Then com

pute

Δ α

R

( )

.

(8)

15 Direct Iterative approach for Rigid Frame

Given

{

!

,

( )

a

"

_i

,

b

"

_i

,

!

}

, want to find F

=

argmin

F

a

"

−

b

"

2

i

∑

Step 0: Make an initial guess F

₀

Step 1: Given F

_k

, compute

a

"

_ik

₌

_F

k

"

a

_i

Step 2: Compute

Δ

F that minimizes

Δ

F

a

"

k_i

₋

_b

"

i 2 i

∑

Step 3: Set F

_k₊₁

=

Δ

FF

_k

Step 4: Iterate Steps 1-3 until residual error is sufficiently small

(or other termination condition)

16 Direct Iterative approach for Rigid Frame

To solve for

ΔF

=

argmin

ΔF

a

!

_ik

₋

_b

!

i 2 i

∑

ΔF

a

!

k_i

₋

_b

!

i

≈

!

α

×

a

!

_ik

₊

_ε

!

₊

_a

!

i k

₋

_b

!

i

!

α

×

a

!

_ik

₊

_ε

!

_≈

_b

!

i

−

!

a

_ik

sk

(

−

a

!

k_i

₎

_α

!

₊

_ε

!

_≈

_b

!

i

−

!

a

_ik

Solve the least-squares problem

"

sk

(

−

a

!

_ik

₎

_I

"

⎡

⎣

⎢

⎤

⎦

⎥

!

α

!

ε

⎡

⎣

⎢

⎤

⎦

⎥

≈

"

!

b

_i

−

a

!

_ik

"

⎡

⎣

⎢

⎤

⎦

⎥

Now set

ΔF

=[

ΔR

(

α

!

),

ε

!

]

(9)

17 Direct Techniques to solve for R

Step 1: Compute

H

=

!

a

_i,x

b

!

_i,x

a

!

_i,x

b

!

_i,y

a

!

_i,x

b

!

_i,z

!

a

_i,y

b

!

_i,x

a

!

_i,y

b

!

_i,y

a

!

_i,y

b

!

_i,z

!

a

_i,z

b

!

_i,x

a

!

_i,z

b

!

_i,y

a

!

_i,z

b

!

_i,z

⎡

⎣

⎢

⎤

⎦

⎥

i

∑

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.

• Method due to K. Arun, et. al., IEEE PAMI, Vol 9, no

5, pp 698-700, Sept 1987

• Failure is rare, and mostly fixable. The paper has details.

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

(10)

19 Digression: quaternions

Invented by Hamilton in 1843. Can be thought of as

4 elements:

q

=

_⎡⎣

q

₀

,

q

₁

,

q

₂

,

q

₃

_⎤⎦

scalar & vector:

q

=

s

+

v

!

=

_{⎡⎣ ⎤⎦}

s

,

v

!

Complex number:

q

=

q

₀

+

q

₁

i

+

q

₂

j

+

q

₃

k

where

i

2 ₌

_j

2 ₌

_k

2 ₌

_{i j k}

₌

₋

₁

Properties:

Linearity:

λ

q

₁

+ µ

q

!

₂

=

_⎡⎣

λ

s

₁

+ µ

s

₂

,

λ

v

!

₁

+ µ

!

v

₂

_⎤⎦

Conjugate:

q

*

₌

_s

₋

_v

!

₌

_s

_,

₋

!

_v

⎡⎣

⎤⎦

Product:

q

₁

"

q

₂

=

_⎡⎣

s

₁

s

₂

−

!

v

₁

i

!

v

₂

,

s

₁

!

v

₂

+

s

₂

!

v

₁

+

!

v

₁

×

v

!

₂

_⎤⎦

Transform vector:

q

"

p

!

=

q

"

⎡⎣ ⎤⎦

0,

p

!

"

q

*

Norm:

q

=

s

2 ₊

_v

!

_•

_v

!

₌

_q

0

2 ₊

_q

1

2 ₊

_q

2

2 ₊

_q

3

2

20 Digression continued: unit quaternions

( ,

cos , sin

2 (( ,

cos , sin

0

2 Rot

Rot

θ

⎡

⎤

θ ⇔ ⎢

_⎣

⎥

_⎦

θ

⎡

⎤

θ

=

_⎢

_⎥

⎣

⎦

n

p

n

!

_i

!

_"

We can associate a rotation by angle about an

axis with the unit quaternion:

)

2 Exercise: Demonstrate this relationship. I.e., show

)

2 [ ]

,

cos ,

2 sin

θ

⎡

₋

⎤

⎢

⎥

⎣

⎦

p

!

"

n

!

2

(11)

21 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

Rotation matrix from unit quaternion

q

=

⎡

_⎣

q

₀

,

q

₁

,

q

₂

,

q

₃

⎤

_⎦

;

q

=

1 R

(

q

)

=

q

₀2

₊

_q

1 2

₋

_q

2 2

₋

_q

3 2

₂₍

_q

1

q

2

−

q

0

q

3

)

2(

q

1

q

3

+

q

0

q

2

)

2(

q

₁

q

₂

+

q

₀

q

₃

)

q

₀2

₋

_q

1 2

₊

_q

2 2

₋

_q

3 2

₂₍

_q

2

q

3

−

q

0

q

1

)

2(

q

₁

q

₃

−

q

₀

q

₂

)

2(

q

₂

q

₃

+

q

₀

q

₁

)

q

₀2

₋

_q

1 2

₋

_q

2 2

₊

_q

3 2

⎡

⎣

⎢

⎤

⎦

⎥

(12)

23 Unit quaternion from rotation matrix

R

(

q

)

=

r

_xx

r

_yx

r

_zx

r

_xy

r

_yy

r

_zy

r

_xz

r

_yz

r

_zz

⎡

⎣

⎢

⎤

⎦

⎥

;

a

0

=

1 +

r

xx

+

r

yy

+

r

zz

;

a

1

=

1 +

r

xx

−

r

yy

−

r

zz

a

₂

=

1 −

r

_xx

+

r

_yy

−

r

_zz

;

a

₃

=

1 −

r

_xx

−

r

_yy

+

r

_zz

a

₀

=

max{

a

_k

}

a

₁

=

max{

a

_k

}

a

₂

=

max{

a

_k

}

a

₃

=

max{

a

_k

}

q

₀

=

a

0

2 q

0

=

r

_yz

−

r

_zy

4 q

₁

q

0

=

r

_zx

−

r

_xz

4 q

₂

q

0

=

r

_xy

−

r

_yx

4 q

₃

q

₁

=

r

xy

−

r

yx

4 q

₀

q

1

=

a

₁

2 q

1

=

r

_xy

+

r

_yx

4 q

₂

q

1

=

r

_xz

+

r

_zx

4 q

₃

q

₂

=

r

zx

−

r

xz

4 q

₀

q

2

=

r

_xy

+

r

_yx

4 q

₁

q

2

=

a

₂

2 q

2

=

r

_yz

+

r

_zy

4 q

₃

q

₃

=

r

yz

−

r

zy

4 q

₀

q

3

=

r

_xz

+

r

_zx

4 q

₁

q

3

=

r

_yz

+

r

_zy

4 q

₂

q

3

=

a

₃

2

24 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

!

)

}

(13)

25 Quaternion method for R

Step 1: Compute

H

=

!

a

_i_,_x

b

!

_i_,_x

a

!

_i_,_x

b

!

_i_,_y

a

!

_i_,_x

b

!

_i_,_z

!

a

_i_,_y

b

!

_i_,_x

a

!

_i_,_y

b

!

_i_,_y

a

!

_i_,_y

b

!

_i_,_z

!

a

_i_,_z

b

!

_i_,_x

a

!

_i_,_z

b

!

_i_,_y

a

!

_i_,_z

b

!

_i_,_z

⎡

⎣

⎢

⎤

⎦

⎥

i

∑

Step 2: Compute

G

=

trace

(

H

)

Δ

T

Δ

H

+

H

T

₋

_trace

₍

_H

₎

_I

⎡

⎣

⎢

⎤

⎦

⎥

where

Δ

T

₌

_H

2,3

−

H

3,2

H

3,1

−

H

1,3

H

1,2

−

H

2,1

⎡

⎣

⎤

⎦

Step 3: Compute eigen value decomposition of

G

diag(

λ

)=

Q

T

_GQ

Step 4: The eigenvector

Q

_k

=

_⎡⎣

q

₀

,

q

₁

,

q

₂

,

q

₃

_⎤⎦

corresponding to

the largest eigenvalue

λ

k

is a unit quaternion corresponding

to the rotation.

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

!

s

a

!

+

v

!

×

a

!

=

s

b

!

+

b

!

×

v

!

Rearranging ...

(

b

!

−

a

!

)

i

v

!

=

0 !

!

(14)

27 Another Quaternion Method for R

Expressing this as a matrix equation

0

( )

b

!

−

a

!

T

!

b

−

a

!

( )

sk

( )

b

!

+

a

!

⎡

⎣

⎢

⎤

⎦

⎥

s

!

v

⎡

⎣

⎢

⎤

⎦

⎥

=

₀

!

0

3 ⎡

⎣

⎢

⎤

⎦

⎥

If we now express the quaternion

q

as a 4-vector

q

!

=

_{⎡⎣ ⎤⎦}

s

,

v

!

T

,

we can express the rotation problem as the constrained linear

system

M

(

a

!

,

b

!

)

q

!

=

0 !

₄

!

q

=

1

28 Another Quaternion Method for R

In general, we have many observations, and we want to

solve the problem in a least squares sense:

min

M

q

!

subject to

q

!

=

1 where

M

=

M(

a

!

₁

,

b

!

₁

)

"

M(

a

!

_n

,

b

!

_n

)

⎡

⎣

⎢

⎤

⎦

⎥

and

n

is the number of observations

Taking the singular value decomposition of M=U

Σ

V

T

_reduces

this to the easier problem

min

U

Σ

V

T

_q

!

X

=

U

Σ

!

y

(15)

29 Another Quaternion Method for R

This problem is just

min

Σ

y

!

=

σ

₁

0

0 σ

2

0

0 σ

3

0

0 σ

4 ⎡

⎣

⎢

⎤

⎦

⎥

!

y

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 that minimizes

Σ

y

!

is

y= 0,0,0,1

!

⎡⎣

⎤⎦

T

. The corresponding value

of

q is given by

!

q

!

=

V

y

!

=

V

₄

. Where V

₄

is the 4th column of V.

Non-reflective spatial similarity (rigid+scale)

ki

a

!

ki

b

!

(16)

31 Non-reflective spatial similarity

Step 1: Compute

a

=

1 N

!

a

_i i=1 N

∑

b

=

1 N

!

b

_i i=1 N

∑

"

a

_i

=

a

!

_i

−

a

b

"

_i

=

!

b

_i

−

b

Step 2: Estimate scale

σ

=

∑

i

b

"

i

"

a

_i i

∑

Step 3: Find

R

that minimizes

(R

⋅ σ

( )

a

"

_i

−

i

∑

_b

"

i

)

2

Step 4: Find

p

!

p

=

b

−

R

⋅

a

Step 5: Desired transformation is

F

=

SimilarityFrame

(

σ

,

R,

p)

!