computer vision -...
TRANSCRIPT
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
Computer VisionColorado School of Mines
3D-3D Coordinate Transforms
An excellent reference is the book “Introduction to Robotics” by John Craig
Computer VisionColorado School of Mines
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
Computer VisionColorado School of Mines
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
Computer VisionColorado School of Mines
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
Computer VisionColorado School of Mines
• 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
Computer VisionColorado School of Mines
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
Computer VisionColorado School of Mines
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
Computer VisionColorado School of Mines
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
Computer VisionColorado School of Mines
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
𝐴𝐴𝐯𝐯 =
𝐴𝐴𝑥𝑥𝐴𝐴𝑦𝑦𝐴𝐴𝑧𝑧
Computer VisionColorado School of Mines
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
Computer VisionColorado School of Mines
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
Computer VisionColorado School of Mines
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
Computer VisionColorado School of Mines
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
Computer VisionColorado School of Mines
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
Computer VisionColorado School of Mines
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
Computer VisionColorado School of Mines
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
Computer VisionColorado School of Mines
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
Computer VisionColorado School of Mines
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
Computer VisionColorado School of Mines
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
Computer VisionColorado School of Mines
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
Computer VisionColorado School of Mines
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
Computer VisionColorado School of Mines
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
Computer VisionColorado School of Mines
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
Computer VisionColorado School of Mines
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
Computer VisionColorado School of Mines
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
Computer VisionColorado School of Mines
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
Computer VisionColorado School of Mines
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
Computer VisionColorado School of Mines
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 𝐶𝐶
𝑊𝑊𝐇𝐇:
Computer VisionColorado School of Mines
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
Computer VisionColorado School of Mines
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
Computer VisionColorado School of Mines
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
Computer VisionColorado School of Mines
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
Computer VisionColorado School of Mines
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
Computer VisionColorado School of Mines
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