Ray casting Algorithm
Over the weekend, I was working on my robotics localization coursework and needed a 2D map of the simulated environment in which the robot would be moving. Localization algorithms require a map of the environment to estimate where the robot might be. So, how do you generate a 2D map showing free and occupied spaces from a 3D environment? The answer: the Ray Casting Algorithm!
It is a collision detection algorithm.
The ray casting algorithm is likely familiar to those in game development or 3D software design. It allows you to map out free and occupied spaces:
- Free spaces: Areas where no objects or obstacles are present.
- Occupied spaces: Areas where objects exist or collisions occur.
Robots navigating on a 2D plane typically rely on free spaces for safe movement.
How Does It Work?
Imagine a square-shaped space containing 3D objects or collision geometries. You divide this space into a grid, with the number of cells determined by a resolution value. After dividing the space, you cast imaginary rays into the environment from specific points within each grid cell.
For a typical 2D map:
- Rays are cast from the center of each grid cell in a downward direction.
- The ray parameters, such as origin and direction, are passed to the physics engine to compute intersections. The physics engine checks whether each ray intersects with any object’s collision geometry:
- If an intersection is detected: The corresponding grid cell is marked as occupied.
- If no intersection is found: The cell is marked as free.
The physics engine handles the complex computations:
- It traces the path of the ray based on its direction.
- It stops early when the first intersection is detected or continues to the ray’s maximum range to confirm that the space is free.
Constructing the 2D Image
The results of the ray casting algorithm are then used to construct a 2D image. Each grid cell corresponds to a pixel in the image:
- Occupied cells are represented as black pixels (RGB:
[0, 0, 0]). - Free cells are represented as white pixels (RGB:
[255, 255, 255]).
Implementation
Initialize a 2D grid of size (rows, columns)
Set grid resolution and ray casting parameters:
resolution = <cell size>
ray_max_range = <maximum ray length>
ray_direction = [0, 0, -1] # Downward direction in 3D space
For each grid cell (i, j):
Calculate the ray's origin:
x = cell_center_x(i)
y = cell_center_y(j)
z = ray_starting_height
origin = [x, y, z]
Calculate the ray's end point:
end = origin + (ray_direction * ray_max_range)
Pass the ray parameters to the physics engine:
intersection = physics_engine.cast_ray(origin, end)
If intersection is detected:
grid[i][j] = 1 # Mark cell as occupied
Else:
grid[i][j] = 0 # Mark cell as free
Generate the 2D image:
For each grid cell:
If grid[i][j] == 1:
Set pixel (i, j) to black (RGB: [0, 0, 0])
Else:
Set pixel (i, j) to white (RGB: [255, 255, 255])
Save the image
For full code implementation: https://github.com/udacity/pgm_map_creator/blob/master/src/collision_map_creator.cc#L45-L138