19CSE455-ARTIFICIAL INTELLIGENCE

AND ROBOTICS

POSITION AND ORIENTATION IN

3D
A A NIPPUN KUMAAR
DEPARTMENT OF CSE
AMRITA SCHOOL OF ENGINEERING, BANGALORE
PREVIOUSLY
▸ Point in 2D

▸ Object in 2D

▸ Pose in 2D

▸ Homogeneous Transformation Matrix for 2D

2
THIS SESSION…
▸ Point in 3D

▸ Pose in 3D

▸ Homogeneous Transformation Matrix for 3D

3
POINT IN 3D
Cartesian Coordinate System
z

O x

Counter Clockwise
4
POINT IN 3D
z

O x
Right-hand rule

In 2D frame Z is hidden
5
POINT IN 3D
z
P
P = (a, b, c)
y

c b

O a x

6
POINT AS VECTOR
z
BOUND P
a
[c]
VECTORS
c y p= b
p

O a x

7
POINT AS VECTOR
z
P
a
[c]
y p= b
p
c
ẑ b
ŷ
O a x̂ x
p = a x ̂ + bŷ + cz ̂

8
POINTS AND VECTORS
▸ Points

▸ Define a location

▸ Cannot add or multiply

▸ Vectors

▸ Do not define a location

▸ Specify how to get from one location to another

▸ Can add & Subtract vectors

▸ Can multiply a vector by a scalar

▸ Difference between two points is a vector

▸ A point is defined by a vector displacement from another point

9
 POSE IN 3D
▸ Pose of a coordinate frame can be
zb
described with respect to another
coordinate frame

A
ξB
za

xb yb
Pose of frame B with respect to frame A ya
A
ξB
Pose has:

3 Translational component

3 Rotational component {A} xa

10
ROTATION ABOUT AN AXIS

Right-hand rule 11
ROTATION ABOUT AN AXIS

Right-hand rule

In 2D, rotation is about hidden Z axis

12
RELATIVE POSE IN 3D
zb
A A B
Bp
P p = ξB . p
{B} “ . ” transforms vector from one
za frame to another
Ap

xb yb
ya
A
ξB

{A} xa
13
 RECAP - ROTATION IN 2D
yA
xB̂ = cosθx + sinθ y yB

yB̂ = − sinθx + cosθ yÂ

P xB
Ay

(Ay) ( sinθ cosθ ) (By)

Ax cosθ −sinθ Bx cosθ Ap B
= p
Bx sinθ
By θ
−sinθ{A} {B} Ax cosθ xA

Each Column is a vector or new axis after rotation

14
 RELATIVE POSE IN 3D
ROTATIONAL MATRIX
za Ax Bx
P r11 r12 r13
Ay = r21 r22 r23 By
Ap
ya Az r31 r32 r33 Bz
Bp
zb NEW
X-AXIS
NEW
Y-AXIS
NEW
Z-AXIS

Transforms vectors from frame {B} to frame {A}

{B}
{A} xa

xb yb
15
 ROTATION MATRIX
r11 r12 r13
za A
RB = r21 r22 r23
P r31 r32 r33

Ap
ya ▸ An orthogonal matrix
Bp
zb ▸ Each column is a unit vector

▸ Each column is orthogonal

to all other columns
{B}
{A} xa ▸ The inverse is the same as
the transpose

xb yb ▸ Determinant is +1

16
ELEMENTARY ROTATION MATRICES
1 0 0
Rx(θ) = 0 cosθ −sinθ
0 sinθ cosθ

cosθ 0 sinθ
Ry(θ) = 0 1 0
−sinθ 0 cosθ

cosθ −sinθ 0
Rz(θ) = sinθ cosθ 0
0 0 1

17
ROTATION ARE NON COMMUTATIVE

18
ROTATION AND TRANSLATION IN 3D
zb
P
{B} Bp

za

xb yb
ya

{A} xa
19
ROTATION AND TRANSLATION IN 3D
zb yb
VP = VRBBp
vp P
Ap = Atv + vp {V} | | {A}
{V} Bp
xb
za
Ap

A Ap = Atv + VRBBp
tV

ya Ap = AtB + ARBBp

{A} xa
20
HOMOGENEOUS MATRIX
zb
Ap = AtB + ARBBp

Ax A
r11 r12 r13 Bx xB
{B} Ay = r21 r22 r23 By + A
yB
za r31 r32 r33
Az Bz A
zB
A
ξB Ax r11 r12 r13 A
xB
Bx
xb yb Ay = r21 r22 r23 A
yB
By
ya Bz
Az r31 r32 r33 A
zB
1
A
Ax r11 r12 r13 xB Bx
Ay r21 r22 r23 A
yB By
=
xa Az r31 r32 r33 A
zB Bz
{A}
1 0 0 0 1 1
21
HOMOGENEOUS MATRIX
(0,0,0 1)
zb A A
RB tB B
Ap̃ = p̃
Homogeneous Homogeneous
{B} Vector Homogeneous Vector
za transform

A B
A Ap̃ = TB p̃
ξB
xb yb
ya

(0,0,0 1)
A A
A RB tB
TB =

xa A A
{A} ξB = TB
22
MULTIPLE FRAMES

23
MULTIPLE FRAMES

Pose Graph

F F C R
TR = TB . TB . TC
F F B C
TR = TB . TC . TR
B
TC = (CTB)−1 CTR = (RTC)−1
24
THANK YOU !!!
25