Package: ptudes-lab
This is a playground of various experiments with SLAM, mapping and visualization
of lidar point clouds. (Ptudesname is an interplay ofP(oint) (e)Tudes,
derived fromEtude)It’s heavily using and relying on Ouster sensor lidar data, Ouster SDK, public
datasets that contain Ouster lidar data and lidar odometry poses obtained fromKISS-ICPpackage.Everything inptudes-labpackage works for multi Python (3.8 - 3.11) and
multi OS (Linux, MacOS, Windows).Table of contents:Flyby 3d visualizations of lidar data with posesROS bags visualizations of raw lidar dataOuster Lidar/IMUs + KissICP + trajectory smoothing (Error-State EKF)Flyby 3d visualizations of lidar data with posesReview the registered point cloud map using the per scan poses of the
odometry/slam pipeline with deskewing and point coloring byREFLECTIVITY,NEAR_IR,SIGNALandRANGEchannels (channels availability depends on
the UDP Lidar Profile of the data).Pre-requisite:0. InstallationYou can installptudes-labusing Pip from the PyPi
using:pip install ptudes-labor you can install it in editable mode if you plan to modify the code (or want
to use not yet released features):git clone https://github.com/bexcite/ptudes-lab.git
cd ptudes-lab
pip install -e .NOTE: Don’t forget to usevenvor any other means of controlling the Python
environments, they always save a lot of time later down the road.1. Get Ouster sensor lidar data in a.pcap/.bagformatYou can download a sample data from theofficial sensor docs:Or you can record it from the sensor if you have one, usingouster-sdk/cli:ouster-cli source <MY_SENSOR_IP> record2. Get the lidar scans poses in kitti format or NC GT formatYouKISS-ICPpose outputs in KITTI format directly running the officialkiss_icp_pipelineon the previously obtained Ouster.pcapdata using:kiss_icp_pipeline --deskew ./OS-0-128_v3.0.1_1024x10.pcapYou can use any pose source with--kitti-posesin the commandptudes flybybelow and not necessarilyKISS-ICPoutput. For example it can be
the result of some post-processing step (smoothing, loop closure, fusion with
other sensors etc) the only requirement is that the number of poses should be
the same as the number of scans in the.pcap/.bagfile.Alternatively you can runptudesekf-benchoustercommand with--save-nc-gt-posesposes.csvto get the trajectory in Newer College Dataset
Ground Truth format (NC GT), which has a much better utility value since it
contains timestamp per pose and can be used to calculate ATE between
trajectories.How to run:Once you have Ouster sensor.pcap/.bagdata and poses per every scan in
KITTI format (or NC GT format) you can runptudes flybycommand as:ptudes flyby ./OS-0-128_v3.0.1_1024x10.pcap --kitti-poses ./OS-0-128_v3.0.1_poses_kitti.txtor for example using.bagfromNewer Collegedataset and the NC GT ground truth data:ptudes flyby ./newer-college/2021-ouster-os0-128-alphasense/collection1/2021-07-01-10-37-38-quad-easy.bag \
    --meta ./newer-college/2021-ouster-os0-128-alphasense/beam_intrinsics_os0-128.json \
    --nc-gt-poses ./newer-college/2021-ouster-os0-128-alphasense/collection1/ground_truth/gt-nc-quad-easy.csv \
    --start-scan 20 \
    --end-scan 50Use--helpto see more options like--start-scan/--end-scanto view only
a specific range of scans.Some useful keyboard shortcuts forflybycommand:KeyActionSPACEStop/Start flying>Increase/decrease flying speed8Toggle poses/trajectory viewk / KCycle point cloud coloring mode of accumulated clouds or mapg / GCycle point cloud color palette of accumulated clouds or mapj / JIncrease/decrease point size of accumulated clouds or mapROS bags visualizations of raw lidar dataOuster sensors produce rawlidar_packets/imu_packetsdata in corresponding
ROS topics. To view the point cloud from such raw packets BAGs without spinning a
ROS and installing all drivers one can useptudes vizcommand.Not tested with ROS2 bags:(I wasn’t been able to locate the ROS2 bag with raw Ousterlidar_packets,
so if you by any chance have such a ROS2 bag that you can share with me I
can make sure that both ROS1 and ROS2 bags working for theptudes vizcommand. (i.e.ptudes.bag.OusterRawBagSourcepacket source can work with
ROS1/ROS2 bags)For example to visualizeNewer Collegedataset BAGS use:ptudes viz ./newer-college/2021-ouster-os0-128-alphasense/collection1/2021-07-01-10-37-38-quad-easy.bag \
    --meta ./newer-college/2021-ouster-os0-128-alphasense/beam_intrinsics_os0-128.jsonand it will open:Since the underlying Viz is thePointVizshipped with Ouster SDK the full
list of keyboard shortcuts can be foundhereOuster Lidar/IMUs + KissICP + trajectory smoothing (Error-State EKF)NOTE:Refer to the blog postLidar odometry smoothing using ES EKF and KissICP for
Ouster sensors with IMUsfor ES EKF formulation details and experiments described in details.Ouster Lidar raw.pcap/.bagrecordings almost always come withimu_packetsinside which may be used to get better trajectories estimation
on some tricky cases, like tunnels with less features, fast platform movements
or lower resolution sensors. (though it’s not universally better and need to be
used with caution).ptudesekf-benchCLI has various tools with ES EKF implementation that uses
the Ousterimu_packetstogether with KissICP.ES EKF as a smoothing filter for KissICP trajectoriesUseptudesekf-benchoustercommand that can run on Ouster Lidar raw
recordings in.pcap/.bagand outputs smoothed KissICP poses that can be
compared with available ground truth automatically (in Newer College Dataset
format) by plotting trajectories together and calculating Average Trajectory
Error (ATE).For example, result of the run onquad-easy.bagfrom thecollection1of
NCD looks like this:ptudes ekf-bench ouster ./newer-college/2021-ouster-os0-128-alphasense/collection1/2021-07-01-10-37-38-quad-easy.bag \
  --gt-file ./newer-college/2021-ouster-os0-128-alphasense/collection1/ground_truth/gt-nc-quad-easy.csv \
  --save-nc-gt-poses quad-easy.csv \
  -p graphsWith graphs showing the smoothing in action:and KissICP adaptive threshold estimates per scan with a corresponding pose
innovation from ICP update:To use ES EKF prediction as a KissICP initial guess, you can use--use-imu-predictionoption withptudesekf-benchouster.And then useptudesekf-benchcmpcommand to compare various trajectories:ptudes ekf-bench cmp ./newer-college/2021-ouster-os0-128-alphasense/collection1/ground_truth/gt-nc-quad-easy.csv \
  quad-easy.csv \
  quad-easy-imu-pred.csv \
  -p graphs --xy-plot