====== HW 02 - Occupancy grid ====== ===== Background ===== 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. ----- ==== 1. Occupancy Grid ==== 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. == Map Coordinate System == 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.// ----- ==== 2. Robot and Sensing ==== 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**. {{ :courses:be5b99cpl:hw:hw02_ogrobotm.png?nolink&550 | Occupancy grid example with a robot}} 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} $$ // ----- ==== 3. Bayesian Update of Cell Probabilities ==== 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 [[courses:be5b99cpl:hw:hw01|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)} $$ ---- ---- ===== Assignment ===== 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. - Load command-line arguments - Compute the number of grid cells and create a **2D variable-length array** representing the occupancy grid - **Initialize** all cells to prior probability $P=0.5$ (e.g., using for-loop) - Sequentially read a sequence of sensor measurements (angle and distance) from **standard input** - 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 - 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. // ---- ==== Input ==== === Command-line arguments === 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$.// === Standard input (sensor measurements) === Sensor measurements are provided on standard input as pairs of floating-point values: 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).// ---- ==== Raycast function ==== 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 {{:courses:be5b99cpl:hw:hw02_raycast.zip | 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.// ---- ==== Output ==== 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). Remember that comparing floating-point numbers for **equality** in C is unsafe due to limited machine precision. Always compare using a small tolerance, for example: ''fabs(a - b) < 1e-6'' ---- ==== Error handling ==== If any error occurs, print an error message to **stderr** and exit with the corresponding return code: ^ Reason ^ Exit code ^ Stderr output ^ | 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. ---- ==== Examples ==== You can find four examples of running the program {{:courses:be5b99cpl:hw:hw02_sample_data.zip | here}}: ^ Standard input ^ Expected output ^ Return value ^ CMD argument ^ Error message ^ | 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 | Upload your solution into [[https://cw.felk.cvut.cz/brute/|BRUTE]] as a zip archive containing only the file ''main.c'' The automatic evaluation will be available in BRUTE from **25.10.** onwards.