computer vision -...

Computer VisionColorado School of Mines

Colorado School of Mines

Computer Vision

Professor William HoffDept of Electrical Engineering &Computer Science

http://inside.mines.edu/~whoff/ 1


3D-3D Coordinate Transforms

An excellent reference is the book “Introduction to Robotics” by John Craig


3D Coordinate Systems

• Coordinate frames – Denote as {A}, {B}, etc– Examples: camera, world, model

• The pose of {B} with respect to {A} is described by– Translation vector t– Rotation matrix R

• Rotation is a 3x3 matrix– It represents 3 angles

{A}

{B}

11 12 13

21 22 23

31 32 33

r r rr r rr r r

=

R

3


Rotations in 3D

4

• 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


XYZ angles to represent rotations

5

• 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

X

Y

Z

• However, it is not the best way for several reasons:– The result depends on the order in which the transforms are

applied; i.e., XYZ or ZYX– 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


• Rotation about the Z axis

• Rotation about the X axis

• Rotation about the Y axis

XYZ Angles

−=

zyx

zyx

A

A

A

XX

XXB

B

B

θθθθ

cossin0sincos0

001

−=

zyx

zyx

A

A

A

YY

YY

B

B

B

θθ

θθ

cos0sin010

sin0cos

−=

zyx

zyx

A

A

A

ZZ

ZZ

B

B

B

1000cossin0sincos

θθθθ

yx

z

Points toward me

Points away from me

x

y z

xz

y

6

+θZ

+θX

+θY

Note signs are different than in the other cases


3D Rotation Matrix

• We can concatenate the 3 rotations to yield a single 3x3 rotation matrix; e.g.,

• 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

−

−

−=

=

XX

XX

YY

YY

ZZ

ZZ

XYZ

θθθθ

θθ

θθθθθθ

cossin0sincos0

001

cos0sin010

sin0cos

1000cossin0sincos

RRRR


XYZ “Fixed Angles”

• This convention first rotates about the x axis, then the y axis, then the z axis

• Each rotation takes place relative about the fixed axes

• Note that the matrices are actually multiplied in the order Rz Ry Rx

( )

etc),sin(),cos(where

00

001

0010

0

10000

)()()(,,

YX

XXYYZZZYXXYZAB

sycx

cxsxsxcx

cysy

sycyczszszcz

RRRR

θθ

θθθθθθ

==

−

−

−=

=

8


3D Rotation Matrix

• R can represent a rotational transformation of one frame to another

• We can rotate a vector represented in frame A to obtain its representation in frame B

• Note: as in 2D, rotation matrices are orthonormal so the inverse of a rotation matrix is just its transpose

=

333231

232221

131211

rrrrrrrrr

BA R

{A}{B}

( ) ( ) RRR AB

TBA

BA ==

−1

vRv ABA

B =

9


Notation

• For vectors, such as 𝐴𝐴𝐯𝐯, the leading superscript represents the coordinate frame that the vector is expressed in

• For transforms, such as 𝐴𝐴𝐵𝐵𝐑𝐑, this matrix represents a rotational transformation of frame A to frame B– The leading subscript indicates “from”– The leading superscript indicates “to”

10

𝐴𝐴𝐯𝐯 =

𝐴𝐴𝑥𝑥𝐴𝐴𝑦𝑦𝐴𝐴𝑧𝑧


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

11

Ax

Ay

By

BxθΖ

Example rotation about the Z axis, showing a rotation of +20 degrees

°°°−°

=

zyx

zyx

A

A

A

B

B

B

100020cos20sin020sin20cos


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)

• 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}

⋅⋅⋅⋅⋅⋅⋅⋅⋅

=

BABABA

BABABA

BABABABA

zzzyzxyzyyyxxzxyxx

Rˆˆˆˆˆˆˆˆˆˆˆˆˆˆˆˆˆˆ

= A

BA

BA

BBA zyxR ˆˆˆ

( )( )( )

=T

BA

TB

A

TB

A

BA

zyx

Rˆˆˆ

12


Matlab: Creating a Rotation Matrix

ax = 0.1; ay = -0.2; az = 0.3; % radiansRx = [ 1 0 0; 0 cos(ax) -sin(ax); 0 sin(ax) cos(ax)];Ry = [ cos(ay) 0 sin(ay); 0 1 0; -sin(ay) 0 cos(ay)];Rz = [ cos(az) -sin(az) 0; sin(az) cos(az) 0; 0 0 1];

% Apply X rotation first, then Y, then ZR = Rz * Ry * Rx

% Apply Z rotation first, then Y, then XR = Rx * Ry * Rz

13


Matlab: Creating a Rotation Matrix

ax = 0.1; ay = -0.2; az = 0.3; % radiansRx = [ 1 0 0; 0 cos(ax) -sin(ax); 0 sin(ax) cos(ax)];Ry = [ cos(ay) 0 sin(ay); 0 1 0; -sin(ay) 0 cos(ay)];Rz = [ cos(az) -sin(az) 0; sin(az) cos(az) 0; 0 0 1];

% Apply X rotation first, then Y, then ZR = Rz * Ry * Rx

R =0.9363 -0.3130 -0.15930.2896 0.9447 -0.15380.1987 0.0978 0.9752

% Apply Z rotation first, then Y, then XR = Rx * Ry * Rz

R =0.9363 -0.2896 -0.19870.2751 0.9564 -0.09780.2184 0.0370 0.9752

Note: rotation matrices are not the same

14


Transforming a Point

• We can use R,t to transform a point from coordinate frame {B} to frame {A}

• Where– Ap is the representation of p in

frame {A}– Bp is the representation of p in

frame {B}

• Note

{B}

{A}

ApBp

t

is the translation of B's origin in the A frame, ABorgt t

tpRp += BAB

A

15


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 ever becomes ≠ 1, we divide through by it

• Then

=

=

sszsysx

zyx

A

1

p

The leading superscript indicates what coordinate frame the point is represented in

16

𝐵𝐵𝐩𝐩 = 𝐇𝐇 𝐴𝐴𝐩𝐩 where 𝐇𝐇 =

𝑟𝑟11 𝑟𝑟12𝑟𝑟21 𝑟𝑟22

𝑟𝑟13 𝑡𝑡𝑥𝑥𝑟𝑟23 𝑡𝑡𝑦𝑦

𝑟𝑟31 𝑟𝑟320 0

𝑟𝑟33 𝑡𝑡𝑧𝑧0 1


General Rigid Transformation

• A general rigid transformation is a rotation followed by a translation

• Can be represented by a single 4x4 homogeneous transformation matrix

• A note on notation:

pHp BAB

A =Cancel leading subscript with trailing superscript

17

BorgABA

BA tpRp +=

=

1000Borg

AABA

BtR

H

+++++++++

==

1333231

232221

131211

z

y

x

BAB

A

tzryrxrtzryrxrtzryrxr

pHp


Example

• In coordinate frame A, point p is (-1,0,1)• Frame A is located at (1,2,4) with respect to B, and is rotated 90

degrees about the x axis with respect to frame B• What is point p in frame B?

18


Example

• In coordinate frame A, point p is (-1,0,1)• Frame A is located at (1,2,4) with respect to B, and is rotated 90

degrees about the x axis with respect to frame B• What is point p in frame B?

19

𝐵𝐵𝐩𝐩 = 𝐴𝐴𝐵𝐵𝐇𝐇 𝐴𝐴𝐩𝐩 𝐴𝐴

𝐵𝐵𝐇𝐇 = 𝐴𝐴𝐵𝐵𝐑𝐑 𝐵𝐵𝐭𝐭𝐴𝐴𝐴𝐴𝐴𝐴𝐴𝐴

0 0 0 1

We want to do where

𝐴𝐴𝐵𝐵𝐑𝐑 = 𝐑𝐑𝑥𝑥 90 =

1 0 00 cos(90) −sin(90)0 sin(90) cos(90)

=1 0 00 0 −10 1 0 {B}

{A}

y

z

y

z


Matlab: Transforming a point

% Construct 4x4 transformation matrix to transform A to BR_A_B = [1 0 0; 0 0 -1; 0 1 0] % 3x3 rotation matrixtAorg_B = [1; 2; 4] % translation (origin of A in B)

H_A_B = [ R_A_B tAorg_B; % H_A_B means transform A to B0 0 0 1 ]

P_A = [-1; 0; 1; 1] % A point in the A frameP_B = H_A_B * P_A % Convert to B frame

20


Inverse Transformations

• The matrix inverse is the inverse transformation

• Note – unlike rotation matrices, the inverse of a full 4x4 homogeneous transformation matrix is not the transpose

• If you want, you can also calculate it explicitly

( ) 1−= HH BA

AB

( )TBA

AB HH ≠

21


Inverse Transformations

• The matrix inverse is the inverse transformation

• Note – unlike rotation matrices, the inverse of a full 4x4 homogeneous transformation matrix is not the transpose

• If you want, you can also calculate it explicitly

( ) 1−= HH BA

AB

( )TBA

AB HH ≠

22


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 C BA B A=H H H , etcD D C B

A C B A=H H H H

23


Example

• A camera is located at point (0,-5,3) with respect to the world. The camera is tilted down by 30 degrees from the horizontal. Find the transformation from {W} to {C}.

24

{W}

y

z

{C}

z

y


Example (continued)

• The origin of the camera in the world is 𝑊𝑊𝐭𝐭𝐶𝐶𝐴𝐴𝐴𝐴𝐴𝐴 = (0,−5, 3)𝑇𝑇

• The rotation matrix is 𝐶𝐶𝑊𝑊𝐑𝐑 = 𝐑𝐑𝑥𝑥(−120 𝑑𝑑𝑑𝑑𝑑𝑑)

• To check to see if this makes sense … see if the unit axes of the camera are pointing in the right direction in the world

25

𝐶𝐶𝑊𝑊𝐑𝐑 =

1 0 00 cos(−120) −sin(−120)0 sin(−120) cos(−120)

=1 0 00 −0.5 +0.860 −0.86 −0.5

= C

WC

WC

WWC zyxR ˆˆˆ

−=

−−=

=

5.086.00

ˆ,86.05.0

0ˆ,

001

ˆ CW

CW

CW zyx


Example (continued)

• The full 4x4 homogeneous transformation matrix from camera to world is

• But, we actually wanted the transformation from world to camera, so take the inverse

26

−−−−

=

100035.086.00586.05.00

0001

HWC

>> H_w_c = inv(H_c_w)

H_w_c =

1.0000 0 0 00 -0.5000 -0.8660 0.09810 0.8660 -0.5000 5.83010 0 0 1.0000

1−= HH WC

CW


Example (continued)

• We can take a point in the world and calculate its position with respect to the camera

• For example, transform the point Wp = (0,0,1)T

27

pHp WCW

C =

>> p_c = H_w_c * [0;0;1;1]

p_c =

0-0.76795.33011.0000


3D Plot

28

% Draw 3D scene plot.plot3(0,0,0,'+'); % Draw a dot at the world origindrawCoordinateAxes(eye(4), 'W'); % Draw world axesdrawCoordinateAxes(H_c_w, 'C'); % Draw camera frame axesaxis equalaxis vis3d

function drawCoordinateAxes(H, label)% This function draws xyz coordinate axes on the active figure.% Inputs: H is the 4x4 transformation matrix, label is a string.

p = H(1:3,4); % Origintext(p(1),p(2),p(3),label);

ux = H * [1;0;0;1]; % x axisuy = H * [0;1;0;1]; % y axisuz = H * [0;0;1;1]; % z axisline([p(1),ux(1)], [p(2),ux(2)], [p(3),ux(3)], 'Color', 'r'); % x axisline([p(1),uy(1)], [p(2),uy(2)], [p(3),uy(3)], 'Color', 'g'); % y axisline([p(1),uz(1)], [p(2),uz(2)], [p(3),uz(3)], 'Color', 'b'); % z axis

end


Combining Transforms

• Take the example of a camera mounted on a moving vehicle

• We have the transformation from the camera frame to the vehicle 𝐶𝐶𝑉𝑉𝐇𝐇, and the vehicle to the world 𝑉𝑉𝑊𝑊𝐇𝐇

29

{W}

z

{C}

y

{V}𝐶𝐶𝑊𝑊𝐇𝐇 = 𝑉𝑉

𝑊𝑊𝐇𝐇 𝐶𝐶𝑉𝑉𝐇𝐇

• We can calculate the transformation from the camera frame to the world 𝐶𝐶

𝑊𝑊𝐇𝐇:


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°

( )

=

333231

232221

131211

,,rrrrrrrrr

R ZYXXYZAB θθθ

( )( )( )cyrcyr

cyrcyrrrr

X

Z

Y

/,/atan2/,/atan2

,atan2

3332

1121

221

21131

==

+−=

θθθ

( )( )2212XY

2212XY

,2atan:90 If,2atan:90 If

rrrr

−=−==+=

θθθθ

for XYZ fixed angles


Equivalent Angle-Axis• A general rotation can be expressed as a

rotation θ about an axis k

• The inverse solution (i.e., given a rotation matrix, find k and θ):

• The product of the unit vector k and angle θ , ω = θ k = (ωx, ωy, ωz) is a minimal representation for a 3D rotation

{A}{B}

k

θ

( )

( )Tzyx

zzxzyyzx

xzyyyzyx

yzxzyxxx

k

kkk

vsc

cvkkskvkkskvkkskvkkcvkkskvkkskvkkskvkkcvkk

R

,,ˆcos1,sin,cos

where

=

−===

++−−+++−+

=

k

θθθθθθ

θθθθθθθθθθθθθθθθθθ

θ

−−−

=

−++

=

1221

3113

2332

332211

sin21ˆ

21 acos

rrrrrr

rrr

θ

θ

k

Note that (-k,-θ) is also a solution

31

Computer VisionColorado School of Mines32

function R = AxisAngleToRot(k,ang)% Convert orientation from axis, angle to rotation matrix.% Angle is represented in radians. Returns 3x3 rotation matrix.

if nargin == 1% If just one input parameter k, assume that it is actually k*ang.ang = norm(k); % Extract angk = k/ang; % Convert k to a unit vector

else% We have two input parameters (k,ang). Make sure k is a unit vector.assert(norm(k)==1);

end

kx = k(1);ky = k(2);kz = k(3);ca = cos(ang);sa = sin(ang);va = 1 - ca;

R = [kx*kx*va + ca kx*ky*va - kz*sa kx*kz*va + ky*sa;kx*ky*va + kz*sa ky*ky*va + ca ky*kz*va - kx*sa;kx*kz*va - ky*sa ky*kz*va + kx*sa kz*kz*va + ca ];

end


Matlab

33

function [k,ang] = RotToAxisAngle(R)% Convert a rotation matrix to axis, angle representation.

ang = acos( (trace(R)-1)/2 );

k = (1/(2*sin(ang))) * [R(3,2) - R(2,3);R(1,3) - R(3,1);R(2,1) - R(1,2) ];

return


Repeating the previous example

34

{W} y

z

{C}

zy

>> R = AxisAngleToRot([1;0;0], -120*pi/180)

R =

1.0000 0 00 -0.5000 0.86600 -0.8660 -0.5000


Example – Equivalent axis of Rotation

• Let’s rotate about the x axis by 45 degrees, then the z axis by 45 degrees. What is the equivalent axis, angle?

35

ax = 45*pi/180;ay = 0;az = 45*pi/180;Rx = [ 1 0 0; 0 cos(ax) -sin(ax); 0 sin(ax) cos(ax)];Ry = [ cos(ay) 0 sin(ay); 0 1 0; -sin(ay) 0 cos(ay)];Rz = [ cos(az) -sin(az) 0; sin(az) cos(az) 0; 0 0 1];R = Rz * Ry * Rx

[k,ang] = RotToAxisAngle(R) % axis, anglew = k*ang % minimal representation

The axis direction is not obvious


Summary

• 3D rigid body transformations (i.e., a rotation and translation) can be represented by a single 4x4 homogeneous transformation matrix

• A 3D rotation is represented uniquely by a 3x3 rotation matrix

• 3D rotations can also be represented by– XYZ angles (the order matters, easy to understand, but

not computationally stable)– Axis, angle (minimal representation, computationally

stable, but axis is not intuitively obvious)

36

computer vision -...

Documents