# ZED Spatial Mapping

###### tags: `視覺組`

> [name=沈尚緯([e-mail])]

## obj file

ZED可以利用拍攝到的景象建立場景並輸出成`.obj`檔

`.obj`檔案內容格式：

- vertex 頂點

v -2.800000 0.050000 -1.744223

- vertex normal 頂點法線

vn 0.551215 0.202074 0.809523

- face 面

f 22072 22073 22074

## types of spatial map

there are 2 types of spatial map：
- fused point cloud (a set of 3d points)
- `obj` file includes v, vn
- mesh (a set of triangles)
- `obj` file includes v, vn, f

## one-time mapping

擷取1000個frame並建立point cloud場景：

import pyzed.sl as sl

def main():
    # Create a ZEDCamera object
    zed = sl.Camera()

    # Create a InitParameters object and set configuration parameters
    init_params = sl.InitParameters()
    init_params.camera_resolution = sl.RESOLUTION.HD720  # Use HD720 video mode (default fps: 60)
    # Use a right-handed Y-up coordinate system
    init_params.coordinate_system = sl.COORDINATE_SYSTEM.RIGHT_HANDED_Y_UP
    init_params.coordinate_units = sl.UNIT.METER  # Set units in meters

    # Open the camera
    err = zed.open(init_params)
    if err != sl.ERROR_CODE.SUCCESS:
        exit(1)

    # Enable positional tracking with default parameters.
    # Positional tracking needs to be enabled before using spatial mapping
    py_transform = sl.Transform()
    tracking_parameters = sl.PositionalTrackingParameters(_init_pos=py_transform)
    err = zed.enable_positional_tracking(tracking_parameters)
    if err != sl.ERROR_CODE.SUCCESS:
        exit(1)

    # Enable spatial mapping
    mapping_parameters = sl.SpatialMappingParameters(map_type=sl.SPATIAL_MAP_TYPE.FUSED_POINT_CLOUD)
    err = zed.enable_spatial_mapping(mapping_parameters)
    if err != sl.ERROR_CODE.SUCCESS:
        exit(1)

    # Grab data during 1000 frames
    i = 0
    py_fpc = sl.FusedPointCloud()  
    # py_fpc = sl.Mesh() # Create a Mesh object
    runtime_parameters = sl.RuntimeParameters()

    while i < 1000:
        # For each new grab, mesh data is updated
        if zed.grab(runtime_parameters) == sl.ERROR_CODE.SUCCESS:
            # In the background, spatial mapping will use newly retrieved images, depth and pose to update the mesh
            mapping_state = zed.get_spatial_mapping_state()

            print("\rImages captured: {0} / 1000 || {1}".format(i, mapping_state))

            i = i + 1

    print("\n")

    # Extract, filter and save the mesh in an obj file
    print("Extracting Point Cloud...\n")
    err = zed.extract_whole_spatial_map(py_fpc)
    print(repr(err))

    print("Saving Point Cloud...\n")
    py_fpc.save("fpc.obj")

    # Disable tracking and mapping and close the camera
    zed.disable_spatial_mapping()
    zed.disable_positional_tracking()
    zed.close()

if __name__ == "__main__":
    main()

## continuous mapping

[official document](https://www.stereolabs.com/docs/spatial-mapping/using-mapping/#continuous-mapping)

> Requesting and retrieving a spatial map is resource-intensive. To improve performance, do not request a new spatial map too frequently.

擷取1000個frame並建立mesh場景，每隔100 frames輸出一次obj檔：

<img src="https://i.imgur.com/wF0aN6H.png" width=400 >
<img src="https://i.imgur.com/cH96qwL.png" width=400 >
<img src="https://i.imgur.com/zuT1TfX.png" width=400 >

import pyzed.sl as sl

def main():
    # Create a ZEDCamera object
    zed = sl.Camera()

    # Create a InitParameters object and set configuration parameters
    init_params = sl.InitParameters()
    init_params.camera_resolution = sl.RESOLUTION.HD720  # Use HD720 video mode (default fps: 60)
    # Use a right-handed Y-up coordinate system
    init_params.coordinate_system = sl.COORDINATE_SYSTEM.RIGHT_HANDED_Y_UP
    init_params.coordinate_units = sl.UNIT.CENTIMETER  # Set units

    # Open the camera
    err = zed.open(init_params)
    if err != sl.ERROR_CODE.SUCCESS:
        exit(1)

    # Enable positional tracking with default parameters.
    # Positional tracking needs to be enabled before using spatial mapping
    py_transform = sl.Transform()
    tracking_parameters = sl.PositionalTrackingParameters(_init_pos=py_transform)
    err = zed.enable_positional_tracking(tracking_parameters)
    if err != sl.ERROR_CODE.SUCCESS:
        exit(1)

    # Enable spatial mapping
    mapping_parameters = sl.SpatialMappingParameters(map_type=sl.SPATIAL_MAP_TYPE.FUSED_POINT_CLOUD)
    err = zed.enable_spatial_mapping(mapping_parameters)
    if err != sl.ERROR_CODE.SUCCESS:
        exit(1)

    # Grab data during 1000 frames
    i = 0
    # py_fpc = sl.FusedPointCloud()  
    py_fpc = sl.Mesh() # Create a Mesh object
    runtime_parameters = sl.RuntimeParameters()

    while i < 10000:
        # For each new grab, mesh data is updated
        if zed.grab(runtime_parameters) == sl.ERROR_CODE.SUCCESS:
            # In the background, spatial mapping will use newly retrieved images, depth and pose to update the mesh
            mapping_state = zed.get_spatial_mapping_state()

            # print("Images captured: {0} || {1}\n".format(i, mapping_state))

            if i % 100 == 0:
                zed.request_spatial_map_async()

            if zed.get_spatial_map_request_status_async() == sl.ERROR_CODE.SUCCESS and i > 0 :
                zed.retrieve_spatial_map_async(py_fpc)
                err = zed.extract_whole_spatial_map(py_fpc)
                print(repr(err))
                print("Saving Point Cloud...\n")
                py_fpc.save("fpc" + str(i) + ".obj")

            i = i + 1

    # Disable tracking and mapping and close the camera
    zed.disable_spatial_mapping()
    zed.disable_positional_tracking()
    zed.close()

if __name__ == "__main__":
    main()

https://github.com/stereolabs/zed-ros-examples/tree/master/examples/zed_rtabmap_example