SLAM Changes in SDK 0.13
Ouster SDK 0.13 was released last week, and introduces numerous performance enhancements and bug fixes. It also comes with some breaking changes to the SLAMBackend interface in order to support multi-sensor SLAM processing.
Multi-Sensor SLAM
If you have multiple Ouster lidars, you can try out multi-sensor slam with ouster-cli - the command line interface that installs when you pip install ouster-sdk - with the following sequence in your terminal:
ouster-cli source --extrinsics-file FILENAME HOSTNAME_1, HOSTNAME_2 slam viz --accum-num 100
The extrinsics file is required to properly align the two sensors to a common coordinate frame. ouster-cli source --help can provide more info.
To learn more you can visit the SDK docs:
https://static.ouster.dev/sdk-docs/python/slam-api-example.html
https://static.ouster.dev/sdk-docs/cli/mapping-sessions.html#ouster-cli-mapping
If you need to download some sample data for a quick test, here are some links with download buttons in the visualizer:
https://static.ouster.dev/sensor-docs/#sample-data
Updated SLAM Script
Here is an updated version of the original SLAM script that works with 0.13. This script can be saved and run the same as before:
python ./slam_script.py FILE_OR_SENSOR To print slam progress to terminal
python ./slam_script.py --viz FILE_OR_SENSOR To visualize live with SimpleViz
from functools import partial
import numpy as np
import argparse
from ouster.sdk.client import SensorInfo, LidarScan, XYZLut, ScanSource, last_valid_column_pose, last_valid_column_ts, dewarp
from ouster.sdk.viz import SimpleViz
from ouster.sdk.open_source import open_source
from ouster.sdk.mapping.slam import KissBackend
# This iterator mimics a ScanSource object so that SimpleViz can replay the data while applying additional processing
class ScanIterator:
def __init__(self, scans: ScanSource):
self._metadata: SensorInfo = scans.metadata
self._xyzlut = XYZLut(self._metadata, use_extrinsics=True) # Create the XYZ lookup table
# Instantiate the slam object
self._slam = KissBackend([self._metadata])
# Map the self._update function on to the scans iterator
# the iterator will now run the self._update command before emitting the modified scan
self._scans = map(partial(self._update), scans) # Play on repeat
@property
def metadata(self) -> SensorInfo:
return self._metadata
# Return the scans iterator when instantiating the class
def __iter__(self):
return self._scans
def _update(self, scan: LidarScan) -> LidarScan:
# Run the slam update command to calculate column poses for the scan
scan = self._slam.update([scan])[0]
# scan.pose now contains an (N, 4, 4) array of pose transformations
# To get the dewarped XYZ points, use the dewarp command and the poses calculated from SLAM. The following lines are provided only as an example and can be commented out.
xyz = self._xyzlut(scan.field("RANGE"))
xyz_dewarped = dewarp(xyz, scan.pose)
# For fun, we can add this (m, n, 3) shaped array to the LidarScan for visualizing in SimpleViz
scan.add_field("XYZ_DEWARPED", xyz_dewarped)
return scan
if __name__ == "__main__":
# parse the command arguments
parser = argparse.ArgumentParser(prog='sdk slam demo',
description='Runs a minimal demo of SDK slam post-processing')
parser.add_argument('source', type=str, help='Sensor hostname or path to a sensor PCAP or OSF file')
parser.add_argument('--viz', help='Visualize the result.', action='store_true', default=False)
parser.add_argument('--accum-num', type=int, help='Number of scans to vizualize', default=50)
args = parser.parse_args()
# Open the file or live sensor
# This interface supports multiple sensors at once so we supply the sensor_idx to get just one ScanSource back
scans = open_source(source_url=args.source, sensor_idx=0)
# One option: Pass an interator that applies the slam to SimpleViz for live viewing.
if args.viz:
scans = ScanIterator(scans)
# Pass the scans iterator to SimpleViz
SimpleViz(scans.metadata, rate=0, accum_max_num=args.accum_num).run(scans)
# Alternative option: Run slam in a simple loop and print some information
else:
prev_scan_pose, prev_scan_time_sec = None, None
slam = KissBackend([scans.metadata])
for scan in scans:
scan = slam.update([scan])[0]
curr_scan_pose = last_valid_column_pose(scan)
curr_scan_time = last_valid_column_ts(scan)/1e9
if prev_scan_time_sec is None:
prev_scan_pose = curr_scan_pose
prev_scan_time_sec = curr_scan_time
continue
# Calculate and print the translation and linear velocity for each frame as an example
delta_pose = np.linalg.inv(prev_scan_pose) @ curr_scan_pose
delta_distance_m = np.linalg.norm(delta_pose[0:3, -1])
delta_time_sec = curr_scan_time - prev_scan_time_sec
prev_scan_pose = curr_scan_pose.copy()
prev_scan_time_sec = curr_scan_time
print(f"frame id {scan.frame_id}: {delta_distance_m:0.3f} meters moved at {delta_distance_m/delta_time_sec:0.3f}m/s")
