@misc{gaal2023featsense,
title={FeatSense -- A Feature-based Registration Algorithm with GPU-accelerated TSDF-Mapping Backend for NVIDIA Jetson Boards},
author={Julian Gaal and Thomas Wiemann and Alexander Mock and Mario Porrmann},
year={2023},
eprint={2310.05766},
archivePrefix={arXiv},
primaryClass={cs.RO}
}
Please cite this work published on arxiv and accepted by IRC 2023 if you found it useful.
This repo contains warpsense
and fastsense
, both SLAM approaches highly tailored to the OS1-128.
warpsense
is a CUDA-based GPU-implementation of HATSDF-SLAM (pdf | repo), an Point-to-TSDF SLAM system.
fastsense
uses the TSDF-mapping backend of warpsense
but replaces registration with a significantly improved F-LOAM-based feature-registration approach (see performance and runtime comparison) with the following changes
- Highly optimized for Ouster OS1-128 (significant speedup in preprocessing)
- Fewer, but better features selected by sparse subsampling of neighborhood in scan lines
- LOAM-style feature-outlier rejection
- improved kdtree-performance with nanoflann
- optimization enables higher number of Ceres optimization steps
- fully configurable
- Usable on embedded ARM devices: reduction of ROS networking overhead
Both approaches are evaluated in the evaluation chapter
- Folder Structure
- Build Instructions
- Parameterization Instructions
- Usage Instructions
- Misc
- Evaluation
src/warpsense
contains GPU-implementation of HATSDF-SLAM, "Warpsense"src/featsense
contains F-LOAM-based feature-based registration algorithm with GPU-accelerated TSDF-Mapping backend, "Featsense"scripts
contains helper scriptsrviz
contains RViz config filesparams
contains all relevant config filestest
includes all unit and integration testsext
contains external libraries
Tested with ROS Noetic on Ubuntu 20.04 and AGX-Xavier on Ubuntu 18.04.
-
CUDA
-
Follow CUDA installation instructions here and make sure CUDA is accessible by sourcing
.bashrc
with these additional entriesexport PATH=${PATH}:/usr/local/cuda/bin export LD_LIBRARY_PATH=${LD_LIBRARY_PATH}:/usr/local/cuda/lib64
-
Verify installation
- Verify that
nvcc
can be found in path:which nvcc
- verify that
nvidia-smi | grep CUDA
-Version matchesnvcc --version
-Version
- Verify that
-
-
clone
fast_vgicp
into ROS workspace for (post-registration)git clone https://github.com/SMRT-AIST/fast_gicp && cd fast_gicp git checkout ee1fdbae7dd86284b52a06141358c89982d67850 # last confirmed working commit git submodule update --init --recursive
-
clone warpsense
git clone https://github.com/juliangaal/warpsense.git --recursive
-
Install Ceres Solver (Feature-based registration)
Latest stable release 2.1.0, dependencies:
sudo apt-get install cmake sudo apt-get install libgoogle-glog-dev libgflags-dev sudo apt-get install libatlas-base-dev sudo apt-get install libeigen3-dev sudo apt-get install libsuitesparse-dev
Ceres
wget http://ceres-solver.org/ceres-solver-2.1.0.tar.gz sudo apt-get install cmake tar zxf ceres-solver-2.1.0.tar.gz mkdir ceres-bin cd ceres-bin cmake ../ceres-solver-2.1.0 make -j3 make test sudo make install
-
Build workspace
All targets:
catkin_make -DCMAKE_BUILD_TYPE=Release -DBUILD_VGICP_CUDA=ON
Individual targets probably make more sense:
-
Featsense
catkin_make -DCMAKE_BUILD_TYPE=Release -DBUILD_VGICP_CUDA=ON featsense
-
Warpsense
catkin_make -DCMAKE_BUILD_TYPE=Release -DBUILD_VGICP_CUDA=ON warpsense
-
Use catkin_make docs
For featsense and warpsense, please refer to params/params.yaml
, where each parameter is explained.
When exporting pointclouds, e.g. roslaunch warpsense featsense_export.launch
, please refer to params/pcl_writer.yaml
Dataset-specific parameters are provided in the params
directory, as well.
Note: When using bagfiles, make sure to stop the recording before hitting Ctrl+C for warpsense/featsense to make sure the map is saved!
roslaunch warpsense warpsense.launch
subscribes to pointcloud topic and saves received cloud as ply and/or pcd file
roslaunch warpsense warpsense_export.launch
roslaunch warpsense featsense.launch
subscribes to pointcloud topic and saves received cloud as ply and/or pcd file.
roslaunch warpsense featsense_export.launch
Build and run tests:
catkin_make run_tests
Use LVR2 on warpsense
branch to generate a mesh from the .h5
file, for example:
./bin/lvr2_fastsense_reconstruction --h5 test.h5 --h --s --c --ns 5
For full options, simply run
./bin/lvr2_fastsense_reconstruction
Examplary visualization (CloudCompare):
- from data folder specified in
params/pcl_writer.yaml
:cloudcompare.CloudCompare $(ls "$PWD"/*.ply)
- set all scalar values to
None
- mark all points clouds and export coordinates to SF(s):
Edit -> Scalar Fields -> Export coordinate(s) to SF(s)
- enable EDL lighting:
Display -> Shaders and Filters -> Enable E.D.L
Note: If you encounter "Unable to open ply file" error, make sure the ply files are stored on a local disc, not external media! See this issue
A separate rviz config for Featsense, floam.rviz
, and Warpsense, warpsense.rviz
are provided in rviz/
Use NVIDIA-provided Ubuntu-based OS for your specific AGX board. AGX-Xavier uses provided Ubuntu-18.04-based OS.
cmake version 3.12 or higher is recommended.
cd ~/Downloads/cmake-<VERSION>/ # or wherever you downloaded cmake
./bootstrap --prefix=$HOME/cmake-install
make
make install
export PATH=$HOME/cmake-install/bin:$PATH
export CMAKE_PREFIX_PATH=$HOME/cmake-install:$CMAKE_PREFIX_PATH
The two export lines make sure that your updated cmake is used instead of the system cmake. You have to make sure that those lines are executed whenever you want to use the updated cmake. It's probably a good idea to put them into your .bashrc.
Example .bashrc
function new_cmake() {
export PATH=$HOME/cmake-install/bin:$PATH
export CMAKE_PREFIX_PATH=$HOME/cmake-install:$CMAKE_PREFIX_PATH
}
In /opt/ros/melodic/share/cv_bridge/cmake/cv_bridgeConfig.cmake
, change:
set(_include_dirs "include;/usr/include;/usr/include/opencv")
to
set(_include_dirs "include;/usr/include;/usr/include/opencv4")
To accurately evaluate the accuracy and performance of Featsense, it must be benchmarked against the original implementation of F-LOAM. However, Featsense extracts less (and – as will be shown – more valuable) features from each scan while increasing the number of Ceres optimization steps. Therefore, to make it a fair comparison, three results will be compared, each: F-LOAM in its default configuration, Featsense with sparse features and a higher number of optimization steps – 5 instead of 2 to be exact – and F-LOAM with a higher number of optimization steps, F-LOAM OPT. To be able to make the claim that Featsense uses less but more valuable features, Featsense must therefore perform better than F-LOAM OPT.
UZH Tracking Area | Lab Survey 2 Dataset | Campus 2 |
---|---|---|
Note
In datasets tracked with prism total station tracker, a large drift with both official and inofficial benchmarking tools can be observed at the very end and start of the trajectory plots. However, the map is consistent at both start and end position, in all datasets with prism ground truth, and neither F-LOAM nor Featsense are instable at those positions in the datasets. Further investigation is needed in these evaluation cases.
Performance measurements of preprocessing + odometry estimation.
Intel i7-4790K (ms) | ARM-v8.2(ms) | |
---|---|---|
F-LOAM | 23 | 58 |
Featsense | 13 | 26 |
Intel i7-4790K (ms) | ARM-v8.2(ms) | |
---|---|---|
F-LOAM | 97 | 257 |
Featsense | 42 | 93 |
Warpsense is instable in large environments or fast movement and needs further research, a runtime comparison to HATSDF-SLAM is still appropriate. Though warpsense
proved to be too instable for the HILTI dataset, here's an example from the UOS lab dataset
Pointcloud | Mesh |
---|---|
![]() |
![]() |
This following section compares the avg CPU, FPGA and GPU runtime of HATSDF-SLAM at respective local map sizes (NxNxN m) and resolution of 64mm per voxel and 20Hz sensor frequency (OS1-128 @ 1024x20) if not stated otherwise. All measurements in ms.
UOS Lab 20x20x15 | UOS Parking Lot 40x40x15 (10Hz | |
---|---|---|
i7-4790K x 8 Threads | 407 | 2752 |
Xilinx ZU15EG | 104 | n.a |
RTX 2080 SUPER | < 1 | < 1 |
AGX Xavier M0/NW | < 1 | < 1 |
AGX Xavier M2/15W | < 1 | 3 |
UOS Lab 20x20x15 | UOS Cafeteria 20x20x15 (10hz) | UOS Parking Lot 40x40x15 (10Hz) | |
---|---|---|---|
i7-4790K x 8 Threads | 261 | 653 | 1339 |
Xilinx ZU15EG | 380 | n.a. | n.a. |
RTX 2080 SUPER | 5 | 11 | 15 |
AGX Xavier M0/NW | 26 | 65 | 121 |
AGX Xavier M2/15W | 44 | 138 | 237 |