Name:
ID number:
LiU-ID:
Passed:
Date:
TSBB06 Computer Exercise B
Representation of 3D rotations
Estimation of rigid transformations
Developed by Klas Nordberg
Computer Vision Laboratory, Linköping University, Sweden
February 24, 2015
Introduction
In this exercise you will have a brief look at various ways of representing 3D rotations, as has been discussed in lecture 1G. Furthermore, you will work with the
specific problem of estimating a rigid transformation given a set of 3D points before and after the transformation. Since these points are affected by measurement
noise, the resulting estimated transformation cannot be computed exactly, it must
be estimated. The estimation is done according to lecture 1H.
1
Preparatory questions
Before coming to the computer exercise it is necessary that you have a clear picture
of the material related to rotations and estimation of rigid transformations (lectures
1G-1H). Below, you will find a number of preparatory exercises to be answered
before the session in order to save time for you, the assisting teacher, and your
fellow students. You need to read and understand the specific tasks of the rest of
the guide in order to answer some of the preparatory exercises.
1
1. A rotation matrix R is defined by a rotation axis n̂ and rotation angle α about
the axis. If you extract the rotation axis and rotation angle from R, what possible
outcomes do you expect other than (n̂, α)?
ANSWER:
2. Formulate Matlab code that computes the rotation matrix R that is required
in Section 4.1. Use Rodrigues’ formula to map a rotation axis n and a rotation
angle a to the rotation matrix.
ANSWER:
3. What is the expected result of R*n, where R is a rotation matrix and n is the
corresponding rotation axis?
ANSWER:
4. Given only a rotation matrix R, how can you determine the corresponding
rotation axis n and rotation angle a? Formulate Matlab code that computes n
and a without using an EVD of R.
ANSWER:
5. Is there any rotation for which the rotation axis is not well-defined? Why?
ANSWER:
2
6. What is the relation between the rotation matrix R and the corresponding
rotation axis n and rotation angle a, based on the matrix exponential? Formulate
Matlab code that computes R from n and a.
ANSWER:
7. How are the eigenvalues and eigenvectors of R and a*liu crossop(n)
related?
ANSWER:
8. What quaternion represents the rotation defined by axis/angle: (n̂, α)?
ANSWER:
9. Given a pure quaternion p representing some 3D point, and a unit quaternion q
representing a 3D rotation, what is the corresponding quaternion based operation
for rotating the 3D point?
ANSWER:
10. What is the minimum number of points in general positions that you need to
determine a rigid transformation? Motivate
ANSWER:
3
11. In Section 5.2 you will estimate a rotation matrix Rest from the data matrices A and B based on the orthogonal Procrustes problem (OPP). Describe how
to do this.
ANSWER:
12. The result of the previous operation is always an orthogonal matrix. How
can you check if it also is a rotation matrix?
ANSWER:
13. Given that the rotation Rest has been determined, how do you compute the
corresponding estimate of the translation, test?
ANSWER:
14. The Frobenius norm that is computed at the end of Section 5.2 represents a
specific geometric error. What error?
ANSWER:
15. If the solution of OPP is not a rotation in SO(3), how can you modify the
method to produce the optimal rotation that lies in SO(3)?
ANSWER:
4
2
Useful Matlab functions
These standard Matlab functions will be used in the exercise:
• rand(X,Y): Produces an X × Y matrix of random value, uniformly distributed
in the interval [0, 1[.
• randn(X,Y): Produces an X × Y matrix of random value, of normal distribution with 0 mean and standard deviation 1.
• mean(M,dim): Computes the mean of a matrix M along dimension dim.
• norm: Computes the norm of a vector or a matrix. In order to get the Frobenius
norm of a matrix, use option 'fro'.
• ones(X,Y): Produces an X × Y matrix where all elements are =1.
• svd(M): Computes a singular value decomposition of a matrix M.
• expm(M): Computes the matrix exponential function of square matrix M.
Additional functions provided by Linköping University can be used for simplifying
geometric computations:
• liu_rodrigues: Given a normalized 3-dimensional vector n and an angle α,
it computes the corresponding rotation matrix using Rodrigues’ formula.
• liu_crossop: Computes the 3 × 3 cross product matrix of a vector. Alternatively: computes the inverse cross product operator on an anti-symmetric 3 × 3
matrix.
• liu_qmult(a,b): Computes the quaternion product between quaternions a
and b, represented as 4-dimensional vectors.
• liu_qinv(a): Computes the inverse of quaternion a, represented as a 4dimensional vector.
• liu_qconj(a): Computes the conjugate of quaternion a, represented as a
4-dimensional vector.
5
3
Starting and setting things up
Start Matlab and change directory to the folder that is used in these exercises, and
add the above mentioned functions to the path by typing in Matlab’s command
window:
cd /site/edu/bb/MultidimensionalSignalAnalysis/ComputerExercises/ExerciseB/
addpath functions
Use help or type plus the function name to see which arguments they have and
more in detail what they do.
In order to simplify the exercises, you should write the Matlab code that you use
into a script file (with extension .m) that can be executed in order to generate all
results that are requested. This script file should be placed in your own home
directory, so you need to add it to your path (if it is not already there)
addpath /edu/dir
Replace dir with your user name
In the exercise there are several questions that you should answer, before or during
the session. Notice that some of the answers are best illustrated by an image. Make
sure to keep this image in order to demonstrate the result to the examiner.
4
Representations of 3D rotations
In this part of the exercise you will investigate four distinct ways of representing
a 3D rotation: as a 3 × 3 rotation matrix, as a combination of rotation axis and
rotation angle, as a 3 × 3 anti-symmetric matrix, and as a unit quaternion.
4.1
Rotation axis, rotation angle, and rotation matrix
A 3D rotation is defined by a rotation axis and a rotation angle about this axis. The
axis can be represented by a normalized vector n̂. The vector and the angle α can
be randomly selected:
n = 2*rand(3,1)-1;
n = n/norm(n)
alpha = 2*pi*rand(1,1)
The rotation can also be represented by a 3 × 3 matrix R ∈ SO(3). This matrix
is given by n̂ and α by means of Rodrigues’ formula, in accordance to preparatory
exercise 2:
6
R = ...
One way to verify that matrix R rotates about n is to apply R to n, in accordance
with preparatory exercise 3. Check this:
R*n
QUESTION: Did it give you the expected result?
ANSWER:
Given only the rotation matrix R, you may want to determine n and α. Use the
Matlab code that you produced in preparatory exercise 4:
...
QUESTION: Did you get a rotation axis and rotation angle that are consistent
with the original ones?
ANSWER:
4.2
Matrix exponentials
Another way to produce R from n̂ and α is to use the matrix exponential. Use
the Matlab code that you wrote in preparatory exercise 6 to compute the rotation
matrix:
Rexp = expm(...)
QUESTION: Did it produce the same R as before?
ANSWER:
7
Compute the eigenvalues and eigenvectors of R and of α[n̂]×
[e1 l1]=eig(R)
[e2 l2]=eig(a*liu_crossop(n))
QUESTION: Are they related in accordance with your answer of preparatory
exercise 7?
ANSWER:
4.3
Unit quaternions
Another representation of 3D rotations is based on unit quaternions. A quaternion
can be represented by a 4-dimensional vector.
Compute this quaternion in terms of a 4-dimensional vector in accordance with
preparatory exercise 8:
q = [...;...]
Verify that this vector has unit norm:
norm(q)
Choose some arbitrary 3D point x0 . This point is represented as an imaginary
(pure) quaternion p:
x0 = randn(3,1)
p = [0;x0]
You will now rotate the point x̄0 according to our rotation R. This can either be
done by applying R onto x̄0 or by computing the corresponding operation based
on quaternions, the latter in accordance with preparatory exercise 9. Compute both
expressions for the rotated point and compare them:
R*x0
...
8
QUESTION: Did they produce the expected result?
ANSWER:
Transform the unit quaternion q to a rotation matrix and compare to R:
liu_R_from_q(q)
R
QUESTION: Are they the same?
ANSWER:
5
Estimation of rigid transformations
In this part of the exercise you will estimate a rigid transformation that relates two
sets of corresponding 3D points, before and after the transformation.
5.1
Creating synthetic data
The data will be created by you, so-called synthetic data, which means that you
will have access to the correct rigid transformation, the ground truth. In this way,
you will be able to determine if your estimation method works or not.
First decide N : many 3D points you want to have in the point set. There is a minimum number of points needed to determine a rigid transformation, see preparatory
exercise 10. Choose at least three times as many as that:
N = ...
Produce N 3D points in random positions in a cube with corners at (±1, ±1, ±1):
x1 = 2*rand(3,N)-1
Produce a random translation and rotation:
9
t
n
n
a
R
=
=
=
=
=
2*rand(3,1)-1
2*rand(3,1)-1;
n/norm(n)
2*pi*rand(1,1)
liu_rodrigues(n,a)
% translation vector
% normalized rotation axis
% rotation angle
% R from (n,a)
This gives a random translation in the same cube as above, but you may chose other
ranges for the translation if you want to. Now, transform the point set with the rigid
transformation defined by the translation and the rotation:
x2 = R*x1+t*ones(1,N)
Finally, also add some measurement noise to the two sets of 3D coordinates. This
can, e.g., be in the form of Gaussian noise of some relatively small standard deviation:
s = 0.01
x1n = x1 + s*randn(3,N)
x2n = x2 + s*randn(3,N)
You now have all the data needed to estimate the rigid transformation: two sets of
corresponding 3D points before and after the transformation, as well as the ground
truth for the transformation: the translation t̄ and the rotation R.
5.2
Estimating R and t̄
To solve the estimation problem, you first need the centroids (means) of each of
the two point sets, and the deviations from the corresponding centroid:
a0 = mean(x1n,2)
b0 = mean(x2n,2)
A = x1n - a0*ones(1,N)
B = x2n - b0*ones(1,N)
You can now estimate the rotation Rest relative the two sets A and B. This is
done by solving the orthogonal Procrustes problem (OPP) for these two sets, in
accordance with preparatory exercise 11:
[U S V]=svd(...)
Rest = ...
In general, this results in the matrix Rest that is orthogonal, but not necessary a
rotation matrix in SO(3). This can be tested using the procedure described in
preparatory exercise 12.
10
QUESTION: Does Rest satisfy this condition?
ANSWER:
Finally, form the optimal translation vector t̄est in accordance with preparatory
exercise 13:
test = ...
You can now verify that the estimated rotation and translation are able to transform
the first point set to the second one. Since there is noise involved, there will not
be an exact correspondence after the transformation is applied, but the difference
should be of a magnitude determined by the noise.
Compute the second point set x2e from the first using the estimated transformation and compute the norm of the difference between the resulting points and x2n
x2e = Rest*x1n + test*ones(1,N)
err = norm(x2e - x2n,'fro')
QUESTION: Is the resulting error of the expected magnitude? Why?
ANSWER:
5.3
Estimation from real data
Given that you now feel confident that your estimation calculations work properly,
you will now apply them to two real data sets for which you do not know the ground
truth. Each data set consists of N points before and after a rigid transformation, but
the data also contains some amount of noise. The goal is to determine an estimate
of the rigid transformation, in terms of a rotation axis n, a rotation angle α and a
translation t.
Start by loading the first data set
load rigiddataA
11
This results in two 3×N matrices: data1 and data2 that hold the coordinates in
their columns of 3D points before and after the rigid transformation, respectively.
Measurements are in mm.
Apply the same method for determining a rotation matrix and a translation vector
as you did above on the synthetic data:
RdataA = ...
tdataA = ...
Use the methods described in Section 1 to determine the rotation axis and rotation
angle from RdataA. Also, compute the error as before:
ndataA = ...
alphadataA = ...
err = ...
QUESTION: What values do you get for the rotation axis, the rotation angle,
and the translation vector?.
ANSWER:
Verify that the estimated rotation matrix really lies in SO(3).
QUESTION: Does it?
ANSWER:
Load the second set
load rigiddataB
...
and estimate the corresponding rigid transformation, and corresponding error, in
the same manner as before.
12
QUESTION: What rigid transformation do you get this time? How large is the
error?
ANSWER:
Verify that the estimated rotation matrix really lies in SO(3).
QUESTION: Does it?
ANSWER:
Fix this using the approach described in preparatory exercise 15.
QUESTION: What rigid transformation do you get this time? Check this with
the tutor.
ANSWER:
QUESTION: What happened with the error? Explain this.
ANSWER:
13