Merge branch 'develop'

README.md

@@ -63,22 +63,66 @@ The examples described below also provide code showing how to create customisati
 
 ## Running the examples from this repository
 
-TODO when the examples are actually done
+_Note: in the spirit of keeping this package light, some dependencies may not be installed; please install those manually_
 
-### `hello_manual_quadricslam` and `hello_quadricslam`
+This repository contains a number of examples to demonstrate how QuadricSLAM systems can be set up in different contexts.
 
-TODO
+Each example is a file in the `quadricslam_examples` module, with a standalone `run()` function. There are two possible ways to run each example:
+
+1. Directly through the command line:
+
+   ```
+   python -m quadricslam_examples.EXAMPLE_NAME ARGS ...
+   ```
+
+   e.g for the `hello_quadricslam` examples:
+
+   ```
+   python -m quadricslam_examples.hello_quadricslam
+   ```
+
+2. Or from within Python:
+
+   ```python
+   from quadricslam_examples.EXAMPLE_NAME import run
+   run()
+   ```
+
+### `hello_manual_quadricslam`
+
+Shows how to create a QuadricSLAM system from scratch using the primitives exposed by our [GTSAM Quadrics library](https://github.com/qcr/gtsam-quadrics). The scenario is 4 viewpoints in a square around 2 quadrics in the middle of the square:
+
+![hello_manual_quadricslam example](https://github.com/qcr/quadricslam/wiki/hello_quadricslam.jpg)
+
+### `hello_quadricslam`
+
+Same scenario as the `hello_manual_quadricslam` example, but uses the abstractions provided by this library. Shows how an entire QuadricSLAM system can be created with only a few lines of code when the appropriate components are available:
+
+![hello_quadricslam example](https://github.com/qcr/quadricslam/wiki/hello_quadricslam.jpg)
 
 ### `tum_rgbd_dataset`
 
-TODO
+Re-creation of the TUM RGBD dataset experiments used in our [initial publication](#citing-our-work). There is a script included for downloading the dataset.
+
+![tum_rgbd_dataset example](https://github.com/qcr/quadricslam/wiki/tum_rgbd.jpg)
+
+_Note: the paper used hand-annotated data to avoid the data association problem; as a result the example here requires a custom data associator to be created before it will run_
+
+### `realsense_python`
+
+Demonstrates how a system can be run using an RGBD RealSense, the [pyrealsense2](https://pypi.org/project/pyrealsense2/) library, and a barebones OpenCV visual odometry algorithm.
+
+The example is a simple plug-n-play system, with weak localisation and data association:
+
+![realsense_python example](https://github.com/qcr/quadricslam/wiki/realsense_python.jpg)
+
+### `realsense_ros`
+
+Demonstrates how a ROS QuadricSLAM system can be put together with an RGBD RealSense, the [ROS RealSense](https://github.com/IntelRealSense/realsense-ros) library, and [Kimera VIO's visual odometry system](https://github.com/MIT-SPARK/Kimera-VIO-ROS).
 
-<p align="center">
-<img alt="TUM RGBD QuadricSLAM still 1" src="https://github.com/qcr/quadricslam/wiki/quadricslam_still1.png" width="400"/>
-<img alt="TUM RGBD QuadricSLAM still 2" src="https://github.com/qcr/quadricslam/wiki/quadricslam_still2.png"  width="400"/>
-</p>
+This example includes a script for creating an entire ROS workspace containing all the required packages built from source. Once installed, it runs the same as the `realsense_python` example but with significantly better localisation:
 
-### TODO RealSense and BenchBot examples
+![realsense_ros example](https://github.com/qcr/quadricslam/wiki/realsense_ros.jpg)
 
 ## Citing our work
 

pyproject.toml

@@ -1,12 +1,12 @@
 [project]
 name = "quadricslam"
-version = "0.2.0"
+version = "1.0.0"
 description = "QuadricSLAM: SLAM systems with quadric landmarks"
 readme = "./README.md"
 license = { file = "./LICENSE.txt" }
 authors = [{name = "Lachlan Nicholson", email = "[email protected]"}]
 maintainers = [{name = "Ben Talbot", email = "[email protected]"}]
-dependencies = [ "gtsam-quadrics" ]
+dependencies = [ "gtsam-quadrics", "numpy", "opencv-python", "matplotlib", "spatialmath-python", "scipy", "dinstinctipy" ]
 classifiers = [
   "Development Status :: 4 - Beta",
   "License :: OSI Approved :: BSD License",

src/quadricslam/data_source/__init__.py

@@ -29,6 +29,7 @@ def done(self) -> bool:
     def next(
         self, state: QuadricSlamState
     ) -> Tuple[Optional[SE3], Optional[np.ndarray], Optional[np.ndarray]]:
+        # Tuple is (odom, RGB, depth)
         pass
 
     @abstractmethod

src/quadricslam/quadricslam.py

@@ -5,7 +5,6 @@
 import gtsam
 import gtsam_quadrics
 import numpy as np
-import pudb
 from spatialmath import SE3
 
 from .data_associator import DataAssociator
@@ -158,7 +157,6 @@ def step(self) -> None:
             n.odom = self.visual_odometry.odom(self.state)
         n.detections = (self.detector.detect(self.state)
                         if self.detector else [])
-        n.detections = n.detections[0:1]
         n.new_associated, s.associated, s.unassociated = (
             self.associator.associate(self.state))
 

src/quadricslam_examples/__main__.py

@@ -0,0 +1,12 @@
+from glob import glob
+import os
+
+if __name__ == '__main__':
+    print("Please run with one of the following examples:")
+    for e in [
+            os.path.basename(p)
+            for p in glob(os.path.join(os.path.dirname(__file__), '*.py'))
+            if not p.endswith('__.py')
+    ]:
+        print("\t%s" % e[:-3])
+    print("\n(e.g. 'python3 -m quadricslam_examples.hello_quadricslam')")

src/quadricslam_examples/create_ros_ws

@@ -0,0 +1,129 @@
+#!/usr/bin/env bash
+#
+# Creates, configures, and builds a ROS Workspace containing all ROS packages
+# necessary for running Kimera VIO ROS. You need to have a version of ROS
+# installed on your system before this script will proceed.
+set -euo pipefail
+
+if [ -z "${1:-}" ]; then
+  printf "\e[31m%s\e[0m\n" \
+    "Run script with a single argument for ROS workspace install location."
+  exit 1;
+fi
+ros_root="$1"
+
+if [ ! -d "/opt/ros/" ] || \
+    [ "$(find /opt/ros -mindepth 1 -maxdepth 1 -type d | wc -l)" == "0" ]; then
+  printf "\e[31m%s\e[0m\n" \
+    "No ROS install found in /opt/ros. Are you sure you have ROS installed?"
+  exit 1;
+fi
+
+ros_distro="$(ls -t1 /opt/ros | head -n1)"
+printf "\e[33m%s\e[0m\n\n" "Using found distro '$ros_distro'"
+
+################################################################################
+########################### Install APT dependencies ###########################
+################################################################################
+
+printf "\e[32m%s\e[0m\n" "Installing Kimera-VIO ROS APT dependencies:"
+
+sudo apt -y install --no-install-recommends apt-utils
+sudo apt -y install \
+      build-essential \
+      cmake \
+      gfortran \
+      libatlas-base-dev \
+      libboost-all-dev \
+      libgtk-3-dev \
+      libjpeg-dev \
+      libparmetis-dev \
+      libpng-dev \
+      libtiff-dev \
+      pkg-config autoconf \
+      python3-wstool \
+      python3-catkin-tools \
+      ros-$ros_distro-image-geometry \
+      ros-$ros_distro-pcl-ros \
+      ros-$ros_distro-cv-bridge \
+      unzip 
+
+# Misc missing dependencies
+# printf "\n\e[32m - %s\e[0m\n" "Missing undeclared dependencies"
+# sudo apt -y install \
+#   ros-$ros_distro-camera-info-manager \
+#   ros-$ros_distro-depth-image-proc \
+#   ros-$ros_distro-eigen-conversions \
+#   ros-$ros_distro-image-proc \
+#   ros-$ros_distro-interactive-markers \
+#   ros-$ros_distro-rviz \
+#   ros-$ros_distro-stereo-msgs \
+#   ros-$ros_distro-tf-conversions
+
+################################################################################
+########################## Build the Catkin workspace ##########################
+################################################################################
+
+printf "\n\e[32m%s\e[0m\n" "Constructing Catkin workspace in '$ros_root'"
+
+mkdir -p "$ros_root/src"
+pushd "$ros_root"
+catkin config --init \
+  --extend "/opt/ros/$ros_distro" \
+  --merge-devel \
+  --cmake-args -DCMAKE_BUILD_TYPE=Release -DGTSAM_TANGENT_PREINTEGRATION=OFF
+
+################################################################################
+#################### Add Kimera VIO's ROS packages (& deps) ####################
+################################################################################
+
+printf "\n\e[32m%s\e[0m\n" \
+  "Adding ROS package, plus dependencies, to '$ros_root/src'"
+
+pushd src
+
+if [ ! -f .rosinstall ]; then
+  rm -rf * .rosinstall || true
+  git clone https://github.com/MIT-SPARK/Kimera-VIO-ROS.git
+
+  wstool init
+
+  wstool merge -y Kimera-VIO-ROS/install/kimera_vio_ros_https.rosinstall
+
+  # Monkey patch in forks for broken codebases...
+  wstool merge -y - <<EOF
+- git:
+    local-name: image_undistort
+    uri: https://github.com/berndpfrommer/image_undistort.git
+    version: ubuntu_20.04
+EOF
+
+  wstool update
+
+  # Pick a more stable GTSAM
+  pushd gtsam
+  gtsam switch -d 4ea2b2ee9  # Random last working commit... sorry...
+  popd
+
+  # Monkey patch in code mods for broken codebases...
+  #   https://github.com/ToniRV/mesh_rviz_plugins/issues/3
+  sed -i 's/\(add_definitions(-std=c++1\)./\14/' \
+    "$ros_root/src/mesh_rviz_plugins/CMakeLists.txt"
+fi
+
+################################################################################
+################### Link in the ROS package for this example ###################
+################################################################################
+
+ln -sfn "$(dirname $(realpath $0))/realsense_ros" \
+  "$ros_root/src/quadricslam_realsense_ros"
+sudo rosdep init || true
+rosdep update
+rosdep install --from-paths ./quadricslam_realsense_ros -y
+
+
+################################################################################
+########################## Build the Catkin workspace ##########################
+################################################################################
+
+catkin build

src/quadricslam_examples/realsense_ros.py

@@ -0,0 +1,32 @@
+import inspect
+import os
+import textwrap
+
+
+def run():
+    # Print a verbose note each time this is run about ROS workspace & Kimera
+    print(
+        "%s\n\nThe script can be run via:\n\t%s DESTINATION\n" %
+        (textwrap.fill(
+            "NOTE: This example requires a ROS workspace running in the "
+            "background with the RealSense and Kimera VIO ROS nodes running. "
+            "A helper script exists to setup a ROS workspace with these "
+            "dependencies.",
+            width=80), os.path.join(os.path.dirname(__file__),
+                                    'create_ros_ws')))
+
+    print(
+        "%s\n\n\t%s\n\t%s\n\t%s\n" %
+        (textwrap.fill(
+            "Once this has built there will be a ROS workspace with all of the "
+            "manually built dependencies required to run this example. Once the "
+            "workspace has been sourced, the following 3 launch files must be run "
+            "in order (ensuring there is appropriate delay between each to support "
+            "initialisation):",
+            width=80), "roslaunch quadricslam_realsense_ros realsense.launch",
+         "roslaunch quadricslam_realsense_ros kimera_vio.launch",
+         "roslaunch quadricslam_realsense_ros quadricslam.launch"))
+
+
+if __name__ == '__main__':
+    run()