HW 02 - Occupancy grid

In the previous assignment, you worked with a single grid cell and used a Bayesian update to estimate whether it was free or occupied based on repeated sensor measurements (0 and 1). In this assignment, you will extend this to an entire map, where the robot maintains probability estimates for multiple cells in a map.

The occupancy grid represents the environment as a 2D array of square cells. Each cell corresponds to a small patch of the real world of size cell_size × cell_size meters.

The overall map dimensions are given in meters as two values (H, W) — the height and width of the map. From these dimensions, the number of grid cells is computed as: $$ N_{height} = ceil(\dfrac{H}{cell\_size}), \ \ \ N_{width} = ceil(\dfrac{W}{cell\_size}). $$

where ceil() is the ceiling function, mapping the input to the least integer greater than or equal to it.

Each cell stores the probability that the area is occupied (contains an obstacle) or free (empty). At the start of mapping, all cells are initialized with a prior probability $P = 0.5$, meaning that the robot is equally uncertain about every cell if it is free or occupied.

The map represents a rectangular area of size H × W meters in the real world, with:

• the origin (0, 0) at the bottom-left corner of the map,
• the x-axis increasing to the right,
• the y-axis increasing upward.

In memory, the map is stored as a 2D array map[rows][cols], where:

• row indices increase downward (top → bottom),
• col indices increase to the right.

Because the array indexing and map coordinate system differ in orientation, you must invert the y-coordinates accordingly when converting from map coordinates to array indices. Think of coordinate (0, 0) as the bottom-left corner of the map in the world, but in memory, row index 0 corresponds to the top of the grid.

The robot is stationary – its position and orientation are fixed during the entire mapping process:

• position: two float values ($x_{robot}$, $y_{robot}$) in meters
• orientation: a float value $α_{robot}$ in degrees, ranging from 0 to 360, increasing counterclockwise

Although the robot does not move, it has an omnidirectional laser-like sensor that can measure distances to obstacles in various directions. Each measurement is a beam starting from the robot’s position and ends at the obstacle:

• It travels in a straight line toward the detected obstacle.
• All cells along the beam (except the last one) are considered free.
• The last cell, where the beam ends, is considered occupied.

Each measurement provides two values:

• angle: a float value $α_{sensor}$ in degrees, ranging from -180 to 180, direction of the beam is relative to the robot’s orientation.
• distance: a float value $d_{sensor}$ in meters, how far away an obstacle was detected along that direction.

To determine the absolute direction of the beam in the map coordinates, you will need to combine the robot's orientation and the sensor's angle: $$ α_{map} = α_{robot} + α_{sensor} $$

For each sensor measurement, you will get a set of cells that the laser beam passes through:

• all but the last cell are marked as free (measurement = 0)
• the last cell (the obstacle) is occupied (measurement = 1)

Each affected cell’s probability must be updated using the same Bayesian update rule as in HW01:

• If measurement = 1:

$$ P_{next} = \dfrac{0.8 \cdot P}{0.8 \cdot P + 0.2 \cdot (1-P)} $$

• If measurement = 0:

$$ P_{next} = \dfrac{0.2 \cdot P}{0.2 \cdot P + 0.8 \cdot (1-P)} $$

Implement an occupancy grid mapping system for a stationary robot in C that receives distance measurements from its sensors. Your program will construct a 2D map from these measurements by updating the occupancy probabilities of grid cells.

1. Load command-line arguments
2. Compute the number of grid cells and create a 2D variable-length array representing the occupancy grid
3. Initialize all cells to prior probability $P=0.5$ (e.g., using for-loop)
4. Sequentially read a sequence of sensor measurements (angle and distance) from standard input
5. For each sensor measurement
   • Compute the absolute beam angle
   • Use the provided raycast() function to obtain all grid cells along the beam
   • Apply the Bayesian update to all affected cells (free vs. occupied) to obtain the new probability estimates in the occupancy grid
6. After processing all measurements, print the final occupancy grid to standard output

Note: when calculating the number of grid cells ($N_{height}$, $N_{width}$) in the occupancy grid, use function ceil() from the math.h library to ensure that the map covers the full area even if $H$ or $W$ are not exact multiples of the $cell\_size$. You will need to link the math library when compiling by using the -lm flag.

You will get 6 configuration parameters provided via the command-line arguments (argv):

$ ./main H W cell_size x_robot y_robot alpha_robot

where:

• H, W - float values (in meters): map height and width
• cell_size - float (in meters): physical size of one map cell
• x_robot, y_robot - float values (in meters): robot position in map coordinates
• alpha_robot - float value (in degrees, 0-360): robot’s orientation, measured counterclockwise

Note: Invalid or missing arguments must result in an error (see Error handling section).

Note: When converting the robot's position into grid coordinates, use typecasting to integer, e.g., the x robot's position can be calculated as: (int)$x_{robot} / cell\_size$.

Sensor measurements are provided on standard input as pairs of floating-point values:

<angle> <distance>

where:

• angle - float value (in degrees, range -180 to +180): direction of the beam relative to the robot’s current orientation
• distance - float value (in meters), distance from the robot to the detected obstacle

Use the scanf() function, read all measurements until the end of file (EOF).

Note: If reading from stdin fails, print an error and terminate the program (see Error handling section).

A helper function raycast() is provided to compute which grid cells a laser beam passes through as well as the obstacle cell. It is based on a Bresenham-style algorithm that traces discrete cells along a continuous line. The function prototype is:

int raycast(double angle_deg, double distance_m, double cell_size_m, int ***out_cells, int *out_count)

where:

• angle_deg (double): beam angle in degrees (in map coordinates)
• distance_m (double): beam length in meters (sensor distance)
• cell_size_m (double): size of one map cell in meters
• out_cells (int * * *): output pointer to a dynamically allocated 2D array of cell coordinates [N][2]
• out_count (int *): output pointer for the number of returned cells N

You will need to copy paste the function implementation to your solution, the full implementation is the following:

/**
 * @brief Performs a grid-based raycast using Bresenham's algorithm.
 * 
 * The ray starts at (0,0) and travels in the specified angle
 * and distance. Each grid cell has size `cell_size_m`. Requires stdlib.h, math.h
 * Variable out_cells is dynamically allocated memory, don't forget to free() it after the use.
 * 
 * @param angle_deg   Angle of the ray (in degrees)
 * @param distance_m  Length of the ray (in meters)
 * @param cell_size_m Size of one grid cell (in meters)
 * @param out_cells   Pointer to store dynamically allocated 2D array [N][2] of grid coords, undefined on failure
 * @param out_count   Pointer to store number of cells (N), undefined on failure
 * @return            1 on success, 0 on failure (e.g., allocation failure, 'distance_m' is less than 'cell_size_m')
 */
int raycast(double angle_deg, double distance_m, double cell_size_m, int ***out_cells, int *out_count)
{
    if (!out_cells || !out_count || distance_m < cell_size_m) {
        return 0;
    }
 
    double angle_rad = angle_deg * 3.14159265358979323846 / 180.0;
    double end_x = distance_m * cos(angle_rad);
    double end_y = distance_m * sin(angle_rad);
    int x0 = 0, y0 = 0;
    int x1 = (int)(end_x / cell_size_m);
    int y1 = (int)(end_y / cell_size_m);
    int dx = abs(x1 - x0);
    int dy = abs(y1 - y0);
    int sx = (x0 < x1) ? 1 : -1;
    int sy = (y0 < y1) ? 1 : -1;
    int err = dx - dy;
 
    int max_cells = dx + dy + 1;
    int **cells = malloc(max_cells * sizeof(int *));
    if (!cells) {
        return 0;
    }
 
    int count = 0;
 
    while (1) {
        cells[count] = malloc(2 * sizeof(int));
        if (!cells[count]) {
            // Clean up previously allocated memory
            for (int i = 0; i < count; i++) {
                free(cells[i]);
                cells[i] = NULL;
            }
            free(cells);
            cells = NULL;
            return 0;
        }
        cells[count][0] = x0;
        cells[count][1] = y0;
        count++;
 
        if (x0 == x1 && y0 == y1) {
            break;
        }
 
        int e2 = 2 * err;
        if (e2 > -dy) {
            err -= dy;
            x0 += sx;
        }
        if (e2 < dx) {
            err += dx;
            y0 += sy;
        }
    }
 
    *out_cells = cells;
    *out_count = count;
    return 1;
}

$\qquad\,$ You can also find the implementation, together with a usage example and a simple visualisation here.

This function computes all grid cells intersected by a laser beam starting at the robot’s position (local origin (0,0)) and travelling in the specified direction and distance. The last cell in the array is the obstacle cell. It allocates a dynamic 2D array of integer cell coordinates [N][2] and stores:

• out_cells[i][0] → x offset (integer)
• out_cells[i][1] → y offset (integer)

both relative to the robot’s cell. The last element of the array is the obstacle, all cells up to that point should be considered free.

The function returns 1 on success, 0 on failure (e.g., allocation failure).

Note: These coordinates are relative, not absolute map indices. You must offset them by the robot’s actual grid cell position before accessing the map array. The out_cells array is dynamically allocated, don't forget to free() the memory after the use.

After all sensor data have been processed, print the final occupancy grid to standard output (stdout), using the following format:

• Cells with $P > 0.5$ are printed as hashtag symbol # (occupied)
• Cells with $P < 0.5$ are printed as space symbol (free)
• Cells with $P == 0.5$ are printed as dash symbol - (unknown)
• The robot position should be printed as a star symbol *

Print the map starting from the top row down to the bottom, so that it corresponds to the map coordinate system (x → right, y → up).

After successful execution, your program should return 0, if an error was encountered, it should return with an appropriate exit value and print an error message to the stderr (see Error handling section).

If any error occurs, print an error message to stderr and exit with the corresponding return code:

Raycast function failure

101

ERROR: raycast failure

Invalid sensor data

102

ERROR: invalid sensor data

Invalid command-line arguments

103

ERROR: invalid arguments

Any other error

100

ERROR: unknown

Otherwise, when no error is encountered, your program should return 0 on successful completion.

You can find four examples of running the program here:

0.0 5.0

---------------------
---------------------
---------------------
---------------------
---------------------
---------------------
---------------------
---------------------
---------------------
-----------*    #----
---------------------
---------------------
---------------------
---------------------
---------------------
---------------------
---------------------
---------------------
---------------------
---------------------
---------------------

0

21.0 21.0 1.0 10.0 10.0 0.0

-90.0 8.0

---------------------
-----------#---------
----------- ---------
----------- ---------
----------- ---------
----------- ---------
----------- ---------
----------- ---------
----------- ---------
-----------*---------
---------------------
---------------------
---------------------
---------------------
---------------------
---------------------
---------------------
---------------------
---------------------
---------------------
---------------------

0

21.0 21.0 1.0 10.0 10.0 180.0

143.0 14.0

103

40.0 50.0 2.0 22.0 18.0

ERROR: invalid arguments

9.0 a

102

10.0 10.0 1.0 4.0 4.0 0.0

ERROR: invalid sensor data