LibMultiSense

https://github.com/carnegierobotics/LibMultiSense

LibMultiSense is a C++/Python library which implements the wire protocol used to communicate with and command the camera. LibMultisense contains convenience classes and utility functions which handle the creation, transmission, and processing of MultiSense configuration and sensor data. There are two underlying C++ implementations for LibMultiSense both which are currently supported to provide backwards compatibility for client migrations.

  • New API (Recommended): This implementation uses C++17 features to provide a RAII memory safe interface to the MultiSense camera. It also includes a collection of utility functions which can be used to perform common tasks like point cloud creation, BGR image creation, and data association between aux color and depth images. Optional integrations with OpenCV and nlohmann-json exist to provide convenient interfaces to image processing and serialization libraries. Python bindings were also added for this interface using pybind11. This interface requires a MultiSense firmware version >v7.22

  • Legacy API: This implementation LibMultiSense was originally developed in 2012, before most standard C++ compilers provided full support for C++11 features. As a result, LibMultiSense was written in a style more consistent with C with classes. Users are required to perform their own data association, and memory management.

The following camera models are supported by both the new and legacy LibMultiSense implementations:

  • MultiSense S27

  • MultiSense S30

  • MultiSense KS21

  • MultiSense KS21i

  • MultiSense S21

  • MultiSense ST25

The following camera models are only supported by the legacy LibMultiSense implementation since they are end-of-life:

  • MultiSense ST21

  • MultiSense SL

  • MultiSense S7

  • MultiSense S7S

  • MultiSense Remote Head

API Documentation

The details of LibMultiSense APIs can be found in the LibMultiSense Doxygen documentation

Building

Please follow the LibMultiSense README to build the LibMultiSense library and its utilities https://github.com/carnegierobotics/LibMultiSense/blob/master/README.md

New API Overview

This documentation applies for the new LibMultiSense API which can be found here: https://github.com/carnegierobotics/LibMultiSense/tree/master/source/LibMultiSense. This is the recommended version of LibMultiSense to integrate with.

Channel

The Channel class is the primary means of communicating with a MultiSense stereo camera. On creation, a Channel object negotiates the details of the UDP communication configuration, provides facilities to query and set camera configuration parameters, and internally processes and signals or dispatches MultiSense sensor data to external user-defined fuctions. The Channel can be configured with a Config object. The channel object provides a pure virtual interface for implementing Channels for various camera backends, and uses a static factory Create function to instantiate specific variants.

Data Source

A data source can stream various types of data from the camera. These data sources include mono image data, color image data, disparity image data, and IMU data.

The LibMultiSense DataSource type is a bitmask that can be used to describe these data sources. Variables of this type are used to query which data sources are supported by a given sensor, and to control the streaming of those data sources from the sensor. DataSource values can be combined using the bitwise OR operator to represent multiple sources. The bitmask definitions can be found here

MultiSenseConfig

MultiSenseConfig is the means by which a user changes configuration of a MultiSense camera. Configuration items include settings like exposure parameters, camera framerate, and operation resolution.

It is recommended you first query the MultiSense configuration, and only update the relevant sections for your application. The default configuration the camera loads on boot represents the recommended operating configuration for most user applications. This recommended configuration changes for each camera type, making it impractical to manage via default construction of a MultiSenseConfig object.

MultiSense Info

MultiSenseInfo contains a set of static configuration information which was written to the camera during factory commissioning. This configuration is unique to a specific camera, and cannot be updated during runtime with the exception of the network info.

MultiSense Status

MultiSenseStatus contains dynamic information which changes during the operation of the camera. This includes things like power, temperature, and network statistics. This status information is useful for debugging problems with the camera hardware.

Stereo Calibration

StereoCalibration contains the full set of calibration parameters for all the imagers contained in the MultiSense camera. This includes both the intrinsics and extrinsics calibration parameters outlined in the stereo calibration overview

Image Frame

A ImageFrame contains a synchronized collection of images which were all captured during the same image exposure.

MultiSense API Utilities

The new LibMultiSense API contains a collection of convenience utilities which can be used to transform data transmitted from the camera into more complex types. These include converting disparity images to depth images in both the left rectified and aux camera frame, converting disparity images into colorized point clouds, converting YCbCr420 images to BGR images, and saving images to disk. These utilities operate directly on the LibMultiSense ImageFrame and StereoCalibration types.

Utilities

LibMultiSense contains a set of example utilities which can be used as both configuration tools and example usage interfaces. Please reference the Utilities directory for the full set of existing utilities https://github.com/carnegierobotics/LibMultiSense/tree/master/source/Utilities. Both the New API and the Legacy API have functional sets of utilities. These can be useful starting points for developing custom applications using LibMultiSense. Python variants of these utilities exist for each utility for the new LibMultiSense API.

Below are a collection LibMultiSense utilities which are particularly useful for standard operations

SaveImageUtility

The SaveImageUtility provides an example of how to save a fixed number of images from the camera. Conversions to depth, and BGR color images are done with the LibMultiSense convenience utilities before they are saved to disk. Camera status info is also printed at 1Hz to provide basic diagnostics on how the camera is operating.

USAGE: ./SaveImageUtility [<options>]
Where <options> are:
    -a <current-address>    : CURRENT IPV4 address (default=10.66.171.21)
    -m <mtu>                : MTU to use to communicate with the camera (default=1500)
    -n <number-of-images>   : Number of images to save (default=1)
    -d                      : Save depth images
    -x                      : Save aux depth images
    -l                      : Save left rectified images
    -c                      : Save color images

The following command will save 5 depth, left rectified, and color images to disk

./SaveImageUtility -a 10.66.171.21 -n 5 -d -l -c

ImageCalUtility

The ImageCalUtility is a convenient way to both query and load new camera calibrations onto the MultiSense. This utility saves and loads yaml files containing the intrinsics M and D matrices and extrinsics P and R matrices. These yaml files follow the matrix serialization formatting defined by OpenCV’s FileStorage object.

It is strongly recommended to backup the current calibration on the camera before attempting to load a new calibration. If you need to recovery the factory calibration for a MultiSense, please open a ticket on the support portal with the serial number of your MultiSense.

USAGE: ./ImageCalUtility -e <extrinsics_file> -i <intrinsics_file> [<options>]
Where <options> are:
    -a <ip_address>      : ip address (default=10.66.171.21)
    -s                   : set the calibration (default is query)
    -y                   : disable confirmation prompts

The following command will query the stereo calibration from a MultiSense at the default 10.66.171.21 IP address, and save the intrinsics calibration data to a intrinsics.yml file and the extrinsic calibration data to a extrinsics.yml file in the same directory the ImageCalUtility ran

./ImageCalUtility -a 10.66.171.21 -e extrinsics.yml -i intrinsics.yml

The following command will load the stereo calibration saved in the /tmp/intrinsics.yml and /tmp/extrinsics.yml files onto the MultiSense at the default 10.66.171.21 IP address

./ImageCalUtility -a 10.66.171.21 -e /tmp/extrinsics.yml -i /tmp/intrinsics.yml -s

Note

The MultiSense will need to be power cycled after a new calibration is loaded

ChangeIpUtility

The ChangeIpUtility is the preferred way to change and reset the static IP address of the camera.

USAGE: ./ChangeIpUtility [<options>]
Where <options> are:
    -a <current_address>    : CURRENT IPV4 address (default=10.66.171.21)
    -A <new_address>        : NEW IPV4 address     (default=10.66.171.21)
    -G <new_gateway>        : NEW IPV4 gateway     (default=10.66.171.1)
    -N <new_netmask>        : NEW IPV4 address     (default=255.255.240.0)
    -b <interface>          : send broadcast packet to specified network interface. This sets the ip address to the default 10.66.171.21
    -y                      : disable confirmation prompt

Example command to change the IP address of a camera at the default 10.66.171.21 IP address to the 192.168.50.200 IP address

./ChangeIpUtility -a 10.66.171.21 -A 192.168.50.200 -G 192.168.50.1

Example command to reset all the MultiSense cameras connected to the eth0 subnet to the default 10.66.171.21 IP address

./ChangeIpUtility -b eth0

VersionInfoUtility

The VersionInfoUtility is a convenient way to query the version info of the firmware running on the camera.

USAGE: ./VersionInfoUtility [<options>]
Where <options> are:
    -a <ip_address>    : ip address (default=10.66.171.21)

Example command to query the version info for a MultiSense at the default 10.66.171.21 IP address

./VersionInfoUtility -a 10.66.171.21

DeviceInfoUtility

The DeviceInfoUtility is a convenient way to query device specific configuration of the camera. This includes details of the cameras hardware configuration, and serial number information.

USAGE: ./DeviceInfoUtility [<options>]
Where <options> are:
    -a <ip_address>    : ip address (default=10.66.171.21)
    -q                 : query device info (default)

Example command to query the device info for a MultiSense at the default 10.66.171.21 IP address

./DeviceInfoUtility -a 10.66.171.21 -q

Note

Device info data can only be updated at the factory. Please open a ticket on the support portal if you have any issues with your camera’s device info

Rectified Focal Length Utility

The RectifiedFocalLengthUtility is a convenient way to update the focal length of the rectified camera. Larger focal lengths will decrease the FOV of the rectified images, and result in a zoom-like operation. Smaller focal lengths will increase the FOV of the rectified images, but will expose some rectification artifacts resulting from removing the lens distortion.

Rectified focal lengths are stored in the MultiSense extrinsic calibrations P matrices.

USAGE: ./RectifiedFocalLengthUtility -f <rectified_focal_length> [<options>]
Where <options> are:
    -a <ip_address>      : ip address (default=10.66.171.21)
    -s                   : set the rectified focal length (default is query)

Example command to set the rectified focal length MultiSense at the default 10.66.171.21 IP address

./RectifiedFocalLengthUtility -a 10.66.171.21 -f 1100 -s

Legacy API Overview

This documentation applies for the legacy LibMultiSense API which can be found here: https://github.com/carnegierobotics/LibMultiSense/tree/master/source/Legacy

Channel

The Channel class is the primary means of communicating with a MultiSense stereo camera. On creation, a Channel object negotiates the details of the UDP communication configuration, provides facilities to query and set camera configuration parameters, and internally processes and dispatches MultiSense sensor data to user-defined callbacks.

Data Source

A data source can stream various types of data from the camera. These data sources include mono image data, color image data, disparity image data, and IMU data.

The LibMultiSense DataSource type is a bitmask that can be used to describe these data sources. Variables of this type are used to query which data sources are supported by a given sensor, and to control the streaming of those data sources from the sensor. DataSource values can be combined using the bitwise OR operator to represent multiple sources. The bitmask definitions can be found here.

Headers

Headers are the structure used to store data the is received from the camera in callbacks. There are three types of header, image headers, IMU headers, and PPS headers.

Image Header

The image header contains all of the information about an image from the camera. This includes the image pixel data buffer and the information required to unpack it. It also includes important information such as the data source, time stamp, and exposure parameters Please see (Link to OpenCV cookbook example) for more information about unpacking the image data into an OpenCV Mat https://docs.carnegierobotics.com/libmultisense/classcrl_1_1multisense_1_1image_1_1_header.html

IMU Header

The IMU header contains a vector of IMU samples that are received at one time from the camera. Each sample contains a time stamp, a data source of either the accelerometer, gyroscope, or magnetometer, and the x, y, and z axis data from that data source. https://docs.carnegierobotics.com/libmultisense/classcrl_1_1multisense_1_1imu_1_1_header.html

PPS Header

The PPS (pulse per second) header is used to capture information about the camera’s internal time stamp when it triggered a hardware time sync pulse, as well as the time stamp at which the host system received the message. Please see the PPS time synchronization section for more information about how this mechanism works. https://docs.carnegierobotics.com/libmultisense/classcrl_1_1multisense_1_1pps_1_1_header.html

Isolated Callback

Isolated callbacks callback functions are used to process data as it arrives from the camera. Each callback function runs in its own thread and will be called as the data arrives. By default, header data in the callback is stored on the heap, and will be freed after the callback function exits. If data must be preserved after the callback is returned, it can either be copied to a new memory location, or the reserveCallbackBuffer function can be used to extend the lifetime. There are a limited number of callback buffers (5 by default, see https://docs.carnegierobotics.com/libmultisense/classcrl_1_1multisense_1_1_channel.html#af0daf86b806b54d5515cb54a900ec073), so it is important to free the callback buffer in a timely manner. If there are no buffers available, new callbacks will not be serviced and images will be dropped. Please see the cookbook for examples of subscribing to the callbacks and reserving callback buffers https://docs.carnegierobotics.com/libmultisense/classcrl_1_1multisense_1_1_channel.html#abb6f2e9872ed613976b640d136057755

Configuration

MultiSense image configuration parameters can be set using the Channel::SetImageConfig() function https://docs.carnegierobotics.com/libmultisense/classcrl_1_1multisense_1_1_channel.html#a838c3844c52b89d54e3c8789d7e0797f The image configuration contains all of the parameters that are used to define how the image is captured by the camera. For more detail about individual image parameters, please see the Camera Configuration section. For an example of querying and setting the image configuration, please see the LibMultiSense cookbook