Occupancy grid

Description

Parameters

Output

Name	Type	Description
`timestamp_ns`	uint64	timestamp of occupancy grid in [ns] - 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[2][3]	affine mapping between grid cells and user coordinate system
`image`	uint8[200*200]	uint8 array of width x height pixels; 0: object probability 0; 255: object probability 1

Timestamp

Every ODS data package (chunk) also contains a timestamp[ns]. If a NTP-server is provided, the timestamp[ns] is synchronized.

Width & Height

Width[int] and height[int] of the occupancy grid (amount of cells within the grid).

Image

The occupancy grid information as an array[width*height], containing the probability of the cell being occupied. Every ODS application forwards one single occupancy grid. All connected heads are publishing their information into this single grid. The center of the grid corresponds to the center of the reference coordinate frame, that is the robot’s coordinate frame defined during the calibration.

We recommend using a probability threshold of 0.5 (127) to assess whether a cell is occupied or not.

transformCellCenterToUser - transformation parameters

Affine mapping between grid cells and user coordinate system. Multiplying the matrix with [0,0,1] gives the user coordinate of the center of the upper left cell.

Below an example of the transformation instruction from occupancy grid (affine) coordinates to the Robot Coordinate System (RCS) is documented.

Transformation matrix parameters

This matrix allows to transform the occupancy grid into the user frame. It is a two dimensional array like:

[
    [0,1,1],
    [1,0,1],
]

Occupancy grid transformation example

By default the parsed occupancy grid is oriented is such a way that:

(pixel coordinates) rows correspond to Y-coordinates in the ODS coordinates system
(pixel coordinates) columns correspond to X-coordinates in the ODS coordinates system
the occupancy grid image origin (pixel coordinates: r=0, c=0) corresponds to c_0 = min(X), r_0 = max(Y).

If you require the orientation of the occupancy grid to be flipped; this can be achieved via a transposition of the occupancy grid matrix. Please also consider axis dependency and zone dependency when transposing.

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 os

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():
    # %%
    import logging
    logger = logging.getLogger(__name__)
    # %%################################################
    # Getting IP address and instantiating O3R object
    ################################################
    ADDR = os.environ.get("IFM3D_IP", "192.168.0.69")
    logger.info(f"Device IP: {ADDR}")
    from ifm3dpy.device import O3R
    o3r = O3R(ADDR)
    ################################################
    # Configure an app and start ODS data stream
    ################################################
    from ods_config import load_config_from_file, validate_json
    from ods_stream import ODSStream
    o3r.reset("/applications")
    schema = o3r.get_schema()

    config_snippet_extrinsics = validate_json(
        schema, load_config_from_file("configs/ods_one_head_config.json"))
    o3r.set(config_snippet_extrinsics)
    # 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)
    # %%


if __name__ == "__main__":
    main()

# %%