Colorado School of Mines
Computer Vision
Professor William Hoff
Dept of Electrical Engineering &Computer Science
Colorado School of Mines
Computer Vision
http://inside.mines.edu/~whoff/
1
3D-3D Coordinate Transforms
An excellent reference is the book “Introduction to
Robotics” by John Craig
Colorado School of Mines
Computer Vision
3D Coordinate Systems
• Coordinate frames
– Denote as {A}, {B}, etc
– Examples: camera, world, model
{B}
• The pose of {B} with respect to
{A} is described by
{A}
– Translation vector t
– Rotation matrix R
• Rotation is a 3x3 matrix
– It represents 3 angles
Colorado School of Mines
 r11

R   r21
r
 31
Computer Vision
r12
r22
r32
r13 

r23 
r33 
3
Rotations in 3D
• The rotation matrix has 9 elements
• However, a 3D rotation has only 3
degrees of freedom
– I.e., it takes 3 numbers to describe the
orientation of an object in the world
– Think of “roll”, “pitch”, “yaw” for an airplane
• Unfortunately, there are many possible ways to
represent a rotation with 3 numbers, depending on the
convention that you use
– We will look at a few of them
• However, the rotation matrix is always unique
4
Colorado School of Mines
Computer Vision
XYZ angles to represent rotations
• One way to represent a 3D rotation is by
doing successive rotations about the X,Y,
and Z axes
• We’ll present this first, because it is easy to
understand
Z
Y
X
• However, it is not the best way for several reasons:
– The result depends on the order in which the transforms are
applied
– Sometimes one or more angles change dramatically in response
to a small change in orientation
– Some orientations have singularities; i.e., the angles are not well
defined
5
Colorado School of Mines
Computer Vision
XYZ Angles
• Rotation about the Z axis
y
+Z
Points toward me
x
z
Points away from
me
• Rotation about the X axis
z
+X
y
x
• Rotation about the Y axis
x
+Y
y
Colorado School of Mines
z
 B x   cos  Z
B  
 y    sin  Z
 Bz  0
  
 sin  Z
cos  Z
 B x  1
0
B  
 y    0 cos  X
 B z   0 sin 
X
  
 B x   cos Y
B  
 y   0
 B z    sin 
Y
  
Computer Vision
0
0  A x 
 A 
0  y 
1   A z 
  x
 A 
 sin  X   y 
cos  X   A z 
0
A
0 sin Y   A x 
 A 
1
0   y
0 cos Y   A z 
Note signs are different than in
the other cases
6
3D Rotation Matrix
• We can concatenate the 3 rotations to yield a single 3x3
rotation matrix; e.g.,
R  R Z RY R X
 cos  Z

  sin  Z
 0

 sin  Z
cos  Z
0
0   cos Y

0  0
1    sin Y
0 sin Y   1
0

1
0   0 cos  X
0 cos Y   0 sin  X


 sin  X 
cos  X 
0
• Note: we use the convention that to rotate a vector, we
pre-multiply it; i.e., v’ = R v
– This means that if R = RZ RY RX, we actually apply the X rotation
first, then the Y rotation, then the Z rotation
7
Colorado School of Mines
Computer Vision
3D Rotation Matrix
• R represents a rotational
transformation of frame A to frame B
– I’ll use the leading subscript to indicate “from”
– I’ll use the leading superscript to indicate “to”
 r11

B
A R   r21
r
 31
r13 

r23 
r33 
r12
r22
r32
• We can rotate a vector in frame A to
obtain its representation in frame B
B
v  BA R A v
{B}
{A}
• Note: as in 2D, rotation matrices are
orthonormal so the inverse of a
rotation matrix is just its transpose
 R   R 
B
A
1
B
A
T
A
B
R
8
Colorado School of Mines
Computer Vision
Note on Direction of Rotation
• The rotation matrix 𝐵𝐴𝐑 describes the orientation of the {A}
frame with respect to the {B} frame
• Example:
– Consider a rotation about the Z axis
– We start with the {A} frame aligned with the {B} frame
– Then rotate about Z until you get to the desired orientation
By
Example rotation about the Z axis,
showing a rotation of +20 degrees
Ay
Ax
Z
Bx
 B x   cos 20  sin 20 0   A x 
B  
 A 
 y    sin 20 cos 20 0   y 
 Bz  0
  Az 
0
1

 
 
9
Colorado School of Mines
Computer Vision
3D Rotation Matrix
• The elements of R are direction cosines
(the projections of unit vectors from one
frame onto the unit vectors of the other
frame)
 xˆ A  xˆ B

B
R

 xˆ A  yˆ B
A
 xˆ  zˆ
 A B
yˆ A  xˆ B
yˆ A  yˆ B
yˆ A  zˆ B
zˆ A  xˆ B 

zˆ A  yˆ B 
zˆ A  zˆ B 





B
B
ˆ
R

x
 A 
A






B 
 yˆ A 






 B 
 zˆ A  




• To see this, apply R to a unit vector
• So the columns of R are the unit vectors
of A, expressed in the B frame
• And the rows of R are the unit vectors of
{B} expressed in {A}


B
AR  





T
xˆ B
T
A
yˆ B
A
A
T
zˆ B



10
Colorado School of Mines
Computer Vision
Matlab: Creating a Rotation Matrix
ax
Rx
Ry
Rz
=
=
=
=
0.1;
ay = -0.2; az = 0.3;
% radians
[ 1 0 0; 0 cos(ax) -sin(ax); 0 sin(ax) cos(ax)];
[ cos(ay) 0 sin(ay); 0 1 0; -sin(ay) 0 cos(ay)];
[ cos(az) -sin(az) 0; sin(az) cos(az) 0; 0 0 1];
% Apply X rotation first, then Y, then Z
R = Rz * Ry * Rx
% Apply Z rotation first, then Y, then X
R = Rx * Ry * Rz
Colorado School of Mines
Computer Vision
11
Matlab: Creating a Rotation Matrix
ax
Rx
Ry
Rz
=
=
=
=
0.1;
ay = -0.2; az = 0.3;
% radians
[ 1 0 0; 0 cos(ax) -sin(ax); 0 sin(ax) cos(ax)];
[ cos(ay) 0 sin(ay); 0 1 0; -sin(ay) 0 cos(ay)];
[ cos(az) -sin(az) 0; sin(az) cos(az) 0; 0 0 1];
% Apply X rotation first, then Y, then Z
R = Rz * Ry * Rx
R =
0.9363
0.2896
0.1987
-0.3130
0.9447
0.0978
-0.1593
-0.1538
0.9752
% Apply Z rotation first, then Y, then X
R = Rx * Ry * Rz
Note: rotation
matrices are
not the same
R =
0.9363
0.2751
0.2184
Colorado School of Mines
-0.2896
0.9564
0.0370
-0.1987
-0.0978
0.9752
Computer Vision
12
Transforming a Point
• We can use R,t to transform a point
from coordinate frame {B} to frame
{A}
A
AP
P  BA R B P  t
BP
• Where
–
AP
is the representation of P in
frame {A}
– BP is the representation of P in
frame {B}
t
{B}
{A}
• Note
t is the translation of B's origin in the A frame, A t Borg
13
Colorado School of Mines
Computer Vision
Homogeneous Coordinates
• We can represent the transformation with a single matrix
multiplication if we write P in homogeneous coordinates
– This simply means to append a 1 as a 4th element
– If the 4th element becomes ≠ 1, we divide through by it
The leading superscript
indicates what coordinate
frame the point is
represented in
• Then
B
P  H A P,
Colorado School of Mines
 X   sX 
   
 Y   sY 
A
P  
Z
sZ
   
1  s 
 r11 r12

r r
where H   21 22
 r31 r32

0 0
Computer Vision
r13 t x 

r23 t y 
r33 t z 

0 1
14
General Rigid Transformation
• A general rigid transformation is a
rotation followed by a translation
B
• Can be represented by a single
4x4 homogeneous transformation
matrix
 r11

r21
B

AH 
 r31

0
• A note on notation:
A
P  BA H B P
Cancel leading subscript with
trailing superscript
P  BA R A P  B t Aorg
r12
r22
r32
0
r13
r23
r33
0
x0 

y0 
z0 

1
 r11 x  r12 y  r13 z  x0 


r
x

r
y

r
z

y
21
22
23
0
B

P  BA H A P  
 r31 x  r32 y  r33 z  z0 


1


15
Colorado School of Mines
Computer Vision
Example
• In coordinate frame A, point P is (1,0,1)
• Frame B is located at (0,0,10) and is rotated 180 degrees about the
x axis with respect to frame A
• What is point P in frame B?
16
Colorado School of Mines
Computer Vision
Example
• In coordinate frame A, point P is (1,0,1)
• Frame B is located at (0,0,10) and is rotated 180 degrees about the
x axis with respect to frame A
• What is point P in frame B?
17
Colorado School of Mines
Computer Vision
18
Colorado School of Mines
Computer Vision
Matlab: Transforming a point
% Construct 4x4 transformation matrix to transform A to B
R_A_B = [1 0 0; 0 -1 0; 0 0 -1]
% 3x3 rotation matrix
tAorg_B = [0; 0; 10]
% translation (origin of A in B)
H_A_B = [ R_A_B
0
0
tAorg_B;
0
1 ]
P_A = [1; 0; 1; 1]
P_B = H_A_B * P_A
Colorado School of Mines
% H_A_B means transform A to B
% A point in the A frame
% Convert to B frame
Computer Vision
19
Inverse Transformations
• The matrix inverse is the inverse transformation
A
B
H
 H
B
A
1
• Note – unlike rotation matrices, the inverse of a full 4x4
homogeneous transformation matrix is not the transpose
A
B
H
 H
B
A
T
• What is the transformation inverse?
Colorado School of Mines
Computer Vision
20
Inverse Transformations
• The matrix inverse is the inverse transformation
A
B
H
 H
B
A
1
• Note – unlike rotation matrices, the inverse of a full 4x4
homogeneous transformation matrix is not the transpose
A
B
H
 H
B
A
T
• What is the transformation inverse?
Colorado School of Mines
Computer Vision
21
Transformations
• Can think of a transformation as:
– A description of frame {A} relative to frame {B}
– A transform mapping a point in the {A} frame to its
representation in the {B} frame
• Can concatenate transformations together
– Leading subscripts cancel trailing superscripts
C
A
H  CB H BA H
D
A
H  DC H CB H BA H, etc
22
Colorado School of Mines
Computer Vision
Order of Rotations
A
B
• XYZ fixed angles
– Start with {B} coincident
with {A}. First rotate {B}
about xA by angle X, then
rotate it about yA by Y,
then rotate about zA by Z.
– Each rotation takes place
relative to the fixed frame
{A}
• The order matters
– Matrices are multiplied in
the order Rz Ry Rx
– Rz Ry Rx order not same
as Rx Ry Rz, etc
RXYZ  X , Y ,  Z   RZ ( Z ) RY (Y ) RX ( X )
0 
 cz  sz 0   cy 0 sy   1 0




  sz cz 0   0 1 0   0 cx  sx 
0
0 1    sy 0 cy   0 sx cx 

where
cx  cos( X ), sy  sin(Y ), etc
 cz cy

A


R

,

,


 sz cy
B XYZ
X
Y
Z
  sy

A
B
cz sy sx  szcx cz sy cx  sz sx 

sz sy sx  czcx sz sy cx  cz sx 

cy sx
cy cx

RXYZ  X ,Y ,Z   BARZYX  X ,Y ,Z 
23
Colorado School of Mines
Computer Vision
Example
• A robot vehicle has a range sensor, which observes a point P. Where is P in
world coordinates?
Colorado School of Mines
Computer Vision
24
Colorado School of Mines
Computer Vision
25
clear all
close all
H_V_W = [ 1 0 0
0 -1 0
0 0 -1
0 0 0
H_S_V = [ 0
1
0
0
0
0
1
0
1
0
0
0
P_S = [ 0; 4; 10;
5;
0;
1;
1]
1;
0;
-2;
1]
1]
P_W = H_V_W * H_S_V * P_S
Colorado School of Mines
Computer Vision
26
Recovering angles from rotation matrix
• Given a 3x3 rotation matrix,
and given a certain angle
set convention, you can
recover the three angles
• Two solutions exist, but can
restrict Y to -90° to +90°
• If Y = ±90° , we have a
degenerate solution: Let
Z =0°
 r11 r12 r13 


A
r22 r23 
B RXYZ  X , Y ,  Z    r21
r r

r
 31 32 33 

Y  atan2  r31, r112  r212

 Z  atan2r21 / cy , r11 / cy 
 X  atan2r32 / cy , r33 / cy 
for XYZ
fixed
angles
If  Y  90 :  X  atan 2r12 , r22 
If  Y  90 :  X   atan 2r12 , r22 
27
Colorado School of Mines
Computer Vision