Occupancy grids

Occupancy grid

The occupancy grid is a 2D grid-based representation of the environment, generated by post processing the depth data. Each grid cell contains a probability value indicating the likelihood of occupancy, encoded as an 8-bit unsigned integer (uint8):

0 (uint8) → Probability 0.0 (free space)
255 (uint8) → Probability 1.0 (fully occupied)
Values between 0 and 255 represent intermediate occupancy probabilities. We recommend using a probability threshold of 0.5 (127) to assess whether a cell is occupied or not.

Polar occupancy grid

Note

From the firmware versions >= 1.20.29 the user can also able to retrieve the polar occupancy grid information from the ODS application.

The polar occupancy grid is a compressed representation of the standard occupancy grid using polar coordinates. This grid is an array of 675 elements, each representing a specific direction around the vehicle.

Each index corresponds to an angular slice:

index i represents the direction from i*(360° / 675) to (i + 1)*(360° / 675).
This angle is determined using atan2(ry, rx), where (rx, ry) is the ray direction.

Note

The polar occupancy grid origin is always at user coordinates (0,0)

The value at each index represents the distance to the nearest occupied cell (obstacle) along the corresponding direction, measured in millimeters (mm). If no obstacle is detected along the ray then the value is set to 65535 (indicating an unlimited range).

These grids serve as an input for path planning, obstacle avoidance, and environment mapping applications.

Range of Interest

The default occupancy grid size is 10 x 10 m² with the calibrated reference coordinate system (RCS) positioned at the center of grid with X-range<=5 meters.

If large vehicles are used for ODS with translation offsets > 1.5 m with respect to the RCS, the occupancy grid size is limiting the detection range of ODS. The detection range on large objects is close to 4 m, which would get clipped by the available 3.5 m range in occupancy grid.

To address this issue, from firmware versions >= 1.10.13 a new parameter is introduced, rangeOfInterest, within the grid configuration. This parameter allows users to extend the grid size to accommodate larger translation offsets, ensuring optimal performance of the ODS in such scenarios.

The rangeOfInterest parameter directly affects the occupancy grid’s dimensions. The grid always maintains a square shape with default resolution of 5cm per cell i.e. 0.05m. Therefore the occupancy grid size is calculated as:

RoI_formula

If rangeOfInterest is 7, then the resultant occupancy grid shape is (280,280).

ODS application outputs

From the ifm3d API versions >= 1.6.6, the user is able to retrieve both standard occupancy grid and polar occupancy grid information from the application interface.

To retrieve the standard occupancy grid, please use ODSOccupancyGridV1 class in ifm3d API. For more information please visit ifm3d API documentation.

Name	Type	Description
`timestamp_ns`	uint64	timestamp of occupancy grid in nanoseconds - NTP time if NTP server is synchronized
`width`	uint16	number of grid cells - width (x)
`height`	uint16	number of grid cells - height (y)
`transformCellCenterToUser`	float[6]	used for affine mapping between occupancy grid cells and user coordinate system
`image`	uint8[`width`, `height`]	uint8 array of width x height pixels; 0: object probability is 0; 255: object probability is 1

Note

The occupancy grid information is generated by processing the data from all active cameras.
The reference coordinate system is located at the center of the occupancy grid i.e. [100,100] in standard occupancy grid image.

To retrieve the polar occupancy grid, please use ODSPolarOccupancyGridV1 class in ifm3d API. For more information please visit ifm3d API documentation.

Name	Type	Description
`version`	uint32	version of the grid
`timestamp_ns`	uint64	timestamp of the grid
`polarOccGrid`	uint16[675]	A compressed version of the grid using polar coordinates.

transformCellCenterToUser - transformation parameters

This information helps the user to convert grid-based cell coordinates (used in occupancy grids) into real-world user coordinates (e.g., in meters). This is especially useful when interpreting map data or visualizing positions on an occupancy grid.

The user can retrieve this information by deserializing the buffer received from buffer_id O3R_ODS_OCCUPANCY_GRID. The transformation can be achieved by multiplying the transformCellCenterToUser and grid based cell coordinates

transformCellCenterToUser

For more details on the mathematical relation, that is transformation chains, please see the example code below:

# %%##########################################
# Copyright 2023-present ifm electronic, gmbh
# SPDX-License-Identifier: Apache-2.0
#############################################

import numpy as np


def transform_cell_to_user(cells: np.ndarray, transform_matrix: np.ndarray):
    """transform the cell coordinates to cartesian map coordinates:
    transform[0, 0] * zone_0_x + transform[0, 1] * zone_0_y + transform[0, 2]

    Args:
        cells (np.ndarray): occupancy grid image
        transform_matrix (np.ndarray): matrix containing the transformation parameters

    Returns:
        tuple: cartesian map coordinates corresponding to the coordinates
                of the edge of the cell in X and Y directions.
    """
    gy, gx = np.indices(cells.shape)

    ux = transform_matrix[0] * gx + transform_matrix[1] * gy + transform_matrix[2]
    uy = transform_matrix[3] * gx + transform_matrix[4] * gy + transform_matrix[5]

    return ux, uy


# %%


def main(ip, ods_cfg_file, calib_cfg_file):
    # %%
    # Necessary imports to run the full example.
    import logging
    logger = logging.getLogger(__name__)
    from ifm3dpy.device import O3R
    try: 
        from ovp8xxexamples.ods.ods_config import load_config_from_file, validate_json
        from ovp8xxexamples.ods.ods_stream import ODSStream
    except:
        try:
            from ods_config import load_config_from_file, validate_json
            from ods_stream import ODSStream
        except ImportError:
            raise ImportError("Unable to import the configuration and streaming functions: we cannot run this example without them.")

    o3r = O3R(ip)
    ################################################
    # Configure an app and start ODS data stream
    ################################################
    o3r.reset("/applications")
    schema = o3r.get_schema()

    o3r.set(validate_json(
        schema, load_config_from_file(calib_cfg_file)
    ))
    o3r.set(validate_json(
        schema, load_config_from_file(ods_cfg_file)
    ))
    # Expecting an application in "app0"
    o3r.set(
        validate_json(
            schema, {"applications": {"instances": {"app0": {"state": "RUN"}}}}
        )
    )
    # %%
    ods_stream = ODSStream(o3r=o3r, app_name="app0")
    ods_stream.start_ods_stream()
    ################################################
    # Get data and transform cell to user coordinates
    ################################################
    occupancy_grid = ods_stream.get_occupancy_grid()
    ux, uy = transform_cell_to_user(
        cells=occupancy_grid.image,
        transform_matrix=np.array(occupancy_grid.transform_cell_center_to_user),
    )
    logger.info(ux)
    logger.info(uy)
    # %%
    ods_stream.stop_ods_stream()


if __name__ == "__main__":
    try:
        # If the example python package was build, import the configuration
        from ovp8xxexamples import config

        IP = config.IP
        ODS_CFG_FILE = config.ODS_CFG_FILE
        CALIB_CFG_FILE = config.CALIB_CFG_FILE

    except ImportError:
        # Otherwise, use default values
        print(
            "Unable to import the configuration.\nPlease run 'pip install -e .' from the python root directory"
        )
        print("Defaulting to the default configuration.")
        IP = "192.168.0.69"
        ODS_CFG_FILE = "configs/ods_one_head_config.json"
        CALIB_CFG_FILE = "configs/extrinsic_one_head.json"

    main(ip=IP, ods_cfg_file=ODS_CFG_FILE, calib_cfg_file=CALIB_CFG_FILE)

# %%