F1TENTH Autonomous Racing

​

�Pose Representation and Coordinate Transformation

​

​

​

Safe Autonomy Lab

University of Pennsylvania

​

Acknowledgements

This course is a collaborative development with significant contributions from:

Hongrui Zheng (lead), Matthew O’Kelly (lead), Johannes Betz (lead), Joseph Auckley, Madhur Behl, Luca Carlone, Jack Harkins, Paril Jain, Kuk Jang, Sertac Karaman, Dhruv Karthik, Nischal KN, Thejas Kesari, Matthew Lebermann, Kim Luong, Yash Pant, Varundev Shukla, Nitesh Singh, Siddharth Singh, and many others.

We are grateful for learning from each other

Frames of Reference

What we’ll cover today

1. Coordinate Frames and Transformations: why?
2. Reference Frames and Rigid Body Transformations
3. ROS specific frames and transformations using tf2_ros

​

​

4

https://imgs.xkcd.com/comics/matrix_transform.png

Frames and Transformations on

Autonomous Vehicles

x

Why do we need Frame Transformations on AV (1)

Reason 1:

Sensors provides measurement in the frame of reference specific to that sensor

​

www.autonomousvehicletech.com

RGBD

Tesla

Cameras! Lots of cameras!

Waymo

LIDARS! Lots of LIDARS!

Uber

Cameras and LIDAR

Navigation requires fusing multiple sensor perspectives

Each sensor

is processed

individually

Navigation requires fusing multiple sensor perspectives

Multiple sensor outputs are combined to provide a combined Birds Eye View

Input semantic segmentation images and predicted BEV map in Cam2BEV (source)

Why do we need Frame Transformations on AV (2)

Reason 2:

Mapping is usually done by tying submaps together

​

https://github.com/googlecartographer/cartographer_ros

Coordinate Frames

x

What is a Coordinate Frame?

1. A set of orthogonal axes attached to a body that serves to describe position of points relative to that body

​

• axes intersect at a point known as the origin

x

y

z

x

y

x

y

In many robotics problems,

the first step is to assign a coordinate frame to all objects of interest

Why do we need Coordinate Frames?

1. Compact representation of points
2. Compact representation of orientations

y

x

Absolute coordinates in World Frame Relative coordinates between robots

Why do we need Coordinate Frames?

y

x

pose = position & rotation

Coordinates have no meaning without specifying coordinate frame

Why do we need Coordinate Frames?

1. Compact representation of points
2. Compact representation of orientations
3. Understand and relate quantities observed by
1. different robots and sensors
2. at different time steps

Rigid body transformations transform points from one frame to another

time t

time t+1

Right-handed Coordinate Frames

Direction of axes is important!

There is a common mnemonic:

• positive x axis points along your index finger, positive y axis points along your middle finger.
• In so-called "right-handed" coordinate systems, positive z-axis points along the thumb of your right hand.
• In so-called "left-handed" coordinate systems, positive z-axis points along the thumb of your left hand.

• robotics always uses right-handed coordinate systems

• left-handed systems are sometimes used in graphics

right- handed

left- handed

Transformations and Frames

x

Transformations and Frames: Map Frame

– where are you with respect to the map – coordinates from origin

​

Map frame

Transformations and Frames: Sensor Frame

The – how does the world look from the sensor

Does this tell you anything about where obstacles are in the map?

Does this tell you anything about where we are in the map?

​

We must link frames together

Sensor frame

Transformations →

Transformations and Frames

The frame of reference in which the measurement is taken

Distance measurements returned by LIDAR

𝛥𝑧

Transformations and Frames

The scan values from the LIDAR will not tell us how far the obstacles are. We must take care of the offsets

𝛥𝑥

Transformations and Frames

The frame of reference in which the measurement is taken

Distance measurements returned by LIDAR

A world without frames and transformations

The actual motion of the car

Transformations and Frames

To convert measurements from one frame to another we need to perform transformations

X

Z

Y

Y

Y

Map frame

Car frame

laser frame

Between frames we need transformations that convert measurements from one frame to another

Transformations and Frames

X

Z

Y

Y

Y

Map frame

Car frame

laser frame

Y

Y

Y

Transform from map to car

Transform from car to laser

To convert measurements from one frame to another we need to perform transformations

Static Transformations Dynamic Transformations

Rigid Body Transforms: An aside

What’s with it being Rigid?

The distance between any two given points of a rigid body remains constant in time regardless of external forces exerted on it.

​

Rigid Body Transforms

• Deformations experienced by a rigid body are small relative to its motion.
• Translation, rectilinear and curvilinear: The motion of the body is completely specified by the motion of any point in the body. All points of the body have the same velocity and same acceleration.
• Rotation about a fixed axis: All particles move in circular paths about the axis of rotation. The motion of the body is completely determined by the angular velocity of the rotation.

​

Baymax: Obviously, not a rigid body

Summary: why do we need transforms between frames?

1. Data is usually provided in the most convenient frame to the data source
2. We want to localize ourselves on a map
3. If an obstacle is detected in the laser frame, maybe we want to know where it is in the world, i.e. in the map frame
4. We need to know the transforms between frames of components on the car for more accurate actuation
5. If we had two disconnected maps (e.g. submaps), we might want to know their location w.r.t. Each other within a global world frame

Reference Frames on F1TENTH

x

Multiple Reference Frames on F1TENTH

We need to know where the robot is in the world.

​

We need to know the relationship between the measurement frame and the actuation frame.

​

32

map

laser

Why?

Multiple Reference Frames

Measurement Frame

Measurement in laser frame: can see some information about the environment, and some information on the positional relationship between the robot and the world.

Multiple Reference Frames

World Frame

Measurement in map frame: the range measurement we see fully makes sense. The observation matches up with the environment. We know where the robot is in the environment.

Multiple Reference Frames

• The world makes more sense if we put the laser scans in the global map frame instead of the laser frame.

​

• More information available when planning in a global frame instead of extracting information in the observation frame.

​

Laser frame

Map frame

Frames we will use

1. map
2. base_link
3. laser

map

base_link

laser

P.S. In ROS, RGB → XYZ

Frames we will use: Environment

1. map
1. Represents the environment around the robot

​

• Map frame origin can be set arbitrarily when we make the map

map

base_link

laser

Frames we will use: Actuation

2. base_link

• Defined as the center of the car’s rear axle

​

• Moves with the car relative to the map frame

map

base_link

laser

Frames we will use: Measurement

3. laser

• The frame in which the laser scan measurement is taken

​

• Also moves with the car relative to the map frame

map

base_link

laser

Odometry

odom

• Not exactly a frame

​

• Describe the relationship between two frames

map

base_link

laser

Poses & Transformations

x

An Example

• In the safety lab, when you define only one Time to Collision (TTC) threshold for emergency braking, the distance between the wall and the car after the car is stopped is different depending on the traveling direction.

​

• Caused by range measurements taken in the laser frame, which is toward the front of the vehicle.

An Example

Could be remedied by adding a frame in the center of the chassis and calculated the TTC from there.

An Example

How do we transform the measurements in the original frame into the new frame that we just added?

Rigid Body Transforms

Rigid Body Transforms

Rigid Body Transforms

Rotating a point in 2D

Rotating a point in 2D

Rotating a point in 2D

Rotating a point in 2D

Rigid Body Transforms

Special type of matrices called Rotation matrices

Rigid Body Transforms

• Special type of matrices called Rotation matrices

Takes points in frame B and represents their orientation in frame A

Rigid Body Transforms: Rotation Matrices

Rigid Body Transforms: Rotation Matrices

Cosine component

Rigid Body Transforms: Rotation Matrices

Sine component

Rigid Body Transforms: Rotation Matrices

Rigid Body Transforms: Rotation Matrices

We have the Rotation Matrix, so now what?

For example

= (-2.5,4.3,0)

Important point to remember

The rotation matrix will take care of perspectives of orientation, what about displacement?

Origins of both the frames are at the same location

Rigid Body Transforms

Rigid Body Transforms

Rigid Body Transforms

Rigid Body Transforms

What we need is Point p with respect to Frame A, given its pose in Frame B

​

​

​

Rigid Body Transforms:

And we are back to the Future

Rigid Body Transforms:

And we are back to the Future

What we need is Point p with respect to Frame A, given its pose in Frame B

​

​

= Homogeneous transformations that transforms

measurements in Frame B to those in Frame A

Recap:

Rotation & Translation in a slightly different example

O₂

p₂

O₁

p’

First we apply the rotation

Frame 2

Frame 1

Putting everything together (with translation)

O₂

p₂

O₁

p’

p₁

Then we apply the translation

Frame 2

Frame 1

Putting everything together

O₂

p₂

O₁

What if we want a more compact representation?

Homogeneous Transformations

• A Homogeneous Transformation is a matrix representation of a rigid body transformation.

​

​

​

​

• Here R is a 2x2 (2D) / 3x3 (3D) rotation matrix and d is a 2x1 (2D) / 3x1 (3D) displacement vector.
• The Homogeneous Representation of a vector is:

​

​

​

• Here p is a 2x1 (2D) / 3x1 (3D) vector

Summary: Poses & Transformations

x

Points & Positions

2D

3D

How to express orientation of a coordinate frame with respect to another?

e.g., What is the orientation of the robot frame in the world frame?

Poses & Transformations

Pose = position & rotation

Poses are used to:

• express relations between coordinate frames
• transform points from one coordinate frame to another

Rigid-body Transformations

How to transform points from one coordinate frame to another?

​

Given point “c” in coordinate frame “r” what is the position of the point in

frame “w”?

Remark about notation

Positions Rotations

Poses

• potential for confusion
• choice of notation depends on balance of simplicity / informativeness
• using consistent notation helps doing math right

Composition of Transforms

x

Orientation & Rotations: 2D Case

wait! is angle positive or negative?

2D rotation matrix

x

y

How to express orientation of a coordinate frame with respect to another?

e.g., What is the orientation of the robot frame in the world frame?

• let’s start from the 2D case

Orientation & Rotations: 3D Case

3D case

x

y

z

x y z

3D rotation matrix

• columns: unit norm, orthogonal
• R is an orthonormal matrix

How to express orientation of a coordinate frame with respect to another?

e.g., What is the orientation of the robot frame in the world frame?

3D Rotation Matrix

3D Rotation Matrix

Elementary Rotations

0

cosφ

⎡

⎣

R_x (φ) = _⎢ 0

⎢ ₀

⎤

⎥

_⎢ 1 0

cosφ −sinφ _⎥

sinφ

⎥

⎦

cosθ 0 sinθ

0 1 0

−sinθ 0 cosθ

⎣

⎡

⎢

R_y (θ ) = _⎢

⎢

⎦

⎤

⎥

⎥

⎥

z

0

⎢

R (ψ) = sinψ

⎢

⎢

⎣

₀ ⎤

⎦

^⎡ cosψ −sinψ

⎥

cosψ 0

0

⎥

₁ ⎥

Rotation around the x axis

Rotation around the y axis

Rotation around the z axis

Inverse of Rotations

Given

compute

Rotation Matrices

Euler Angles

Quaternions

​

Inverse

Composition of Rotations

Rotation Matrices

Euler Angles

Quaternions

​

Composition

(quaternion product)

Composition and Inverse of Transformations

Composition Inverse

Other representations of rotations

• Quaternions: (ROS standard)
• Supplementary materials in LaValle’s Virtual Reality lectures.

​

• Euler angles

​

• Axis angle

Euler Angles

Euler’s rotation theorem (1.0): Any rotation can be written as the product of no more than 3 elementary rotations (no two consecutive rotations along the same axis).

http://petercorke.com/wordpress/toolboxes/robotics-toolbox

roll

pitch

yaw

•

We only need 3 numbers to represent a rotation!

•

•

why not x-y-z, y-x-z, x-x-z, …?

are generally called Euler angles

Where will you use this?

Labs 2, 3: Automatic Emergency Braking,

Reactive Methods for navigation & planning in local frame

​

​

Later labs: Map based methods, need transforms to perform planning in global frame.

Next: How do we do this in ROS?

X

Z

Y

Y

Y

Map frame

Car frame

laser frame

Y

Y

Y

Transform from map to car

Transform from car to laser

To convert measurements from one frame to another we need to perform transformations

Static Transformations Dynamic Transformations

Pose Representation and Coordinate Transformation

Key Takeaways

1. Coordinates are numerical representations of geometric concepts, like points, directions, frames of reference, and movement.

​

• Points, directions, and displacements in 𝑛 -dimensional space are represented by 𝑛 -dimensional vectors, while rotations and scalings are represented by 𝑛×𝑛 matrices.

​

• Rigid transformations consist of a rotation followed by a translation. They represent both rigid body movement and changes of coordinate frame.

​

• Homogeneous coordinates represent rigid transforms using matrix multiplication in an 𝑛+1 dimensional space where the last coordinate is either 0 or 1.

​

• When working with coordinates it is easy to make mistakes. Having clear assumptions, clear notation and/or using coordinate management software can reduce the risk of error.

Questions?

x

Extra slides

Rotation Matrix

x₁

y₁

x

y

Rotation Matrix - from the racecar’s perspective

x

y

Rotation Matrix

x₁

x₂

y₁

y₂

P_{1, 2}

O

Recall we represented the basis vectors of one frame as linear combinations of the other frame’s basis vector. We can do the same here.

​

Rotation Matrix

How do we relate the two?

​

x₁

y₁

O

Rotation Matrix

x₁

y₁

O

Instead of using two frames, we can represent these two vectors in the same basis vector now.

​

Homogeneous Transformations

• How to transform points from one coordinate frame to another?

• Homogeneous coordinates:

• More elegant, matrix formulation:

Pose & rigid body transformation

Rotation Matrix

x₁

y₁

O

???

Example: Rotations in 3D

Rotation matrices in 3D around each axis.

See something similar?

Rigid Body Pose

Once chosen a frame, a rigid body is described in space by its position and orientation with respect to that reference frame.

100

map

base_link

laser

odom

Multiple Reference Frames

Laser frame

Map frame