diff --git a/README.md b/README.md index 8735b89b6..d6f1449f2 100644 --- a/README.md +++ b/README.md @@ -1,17 +1,33 @@
- acfr-rpg + acfr-rpg -
+ # DynoSAM: Dynamic Object Smoothing and Mapping + + **A Stereo/RGB-D Visual Odometry pipeline for Dynamic SLAM.** + + DynoSAM estimates camera poses, object motions/poses, as well as static background and temporal dynamic object maps. It provides **full-batch**, **sliding-window**, and **incremental** optimization procedures and is fully integrated with **ROS2**. + + [![ROS2](https://img.shields.io/badge/ROS2-Kilted-blue.svg)](https://docs.ros.org/en/kilted/index.html) + [![License](https://img.shields.io/badge/License-BSD-green.svg)](./LICENSE) + [![Paper](https://img.shields.io/badge/arXiv-2501.11893-b31b1b.svg)](https://arxiv.org/pdf/2501.11893) + + -# DynoSAM: Dynamic Object Smoothing and Mapping for Dynamic SLAM +
-DynoSAM is a Stereo/RGB-D Visual Odometry pipeline for Dynamic SLAM and estiamtes camera poses, object motions/poses as well as static background and temporal dynamic object maps. +
+ +

DynoSAM running Parallel-Hybrid formulation in incremental optimisation mode on a indoor sequence recorded with an Intel RealSense. Playback is 2x speed.

+
-DynoSAM current provides full-batch, sliding-window and incremental optimisation procedures and is integrated with ROS2. +
+ +

Example output running on the Oxford Multimotion Dataset (OMD, 'Swinging 4 Unconstrained'). This visualisation was generated using playback after full-batch optimisation.

+
-## Publication +## 📚 Publications The offical code used for our paper: - [Jesse Morris](https://jessemorris.github.io/), Yiduo Wang, Mikolaj Kliniewski, Viorela Ila, [*DynoSAM: Open-Source Smoothing and Mapping Framework for Dynamic SLAM*](https://arxiv.org/pdf/2501.11893), Arxiv. Submitted Transactions on Robotics (T-RO) Visual SLAM Special Issue (2025). @@ -44,7 +60,7 @@ We kindly ask to cite our papers if you find these works useful: } ``` -## Related Publications +### Related Publications DynoSAM was build as a culmination of several works: @@ -52,242 +68,234 @@ DynoSAM was build as a culmination of several works: - J. Zhang, M. Henein, R. Mahony, V. Ila [**VDO-SLAM: A Visual Dynamic Object-aware SLAM System**](https://arxiv.org/abs/2005.11052), ArXiv - M. Henein, J. Zhang, R. Mahony, V. Ila. [**Dynamic SLAM: The Need for Speed**](https://ieeexplore.ieee.org/abstract/document/9196895).2020 IEEE International Conference on Robotics and Automation (ICRA). IEEE, 2020. -## Demo - +## 📖 Overview -
- -

DynoSAM running Parallel-Hybrid formulation in incremental optimisation mode on a indoor sequence recorded with an Intel RealSense. Playback is 2x speed.

-
+### Key Features (Nov 2025 Update) +* 🚀 **CUDA Integration:** Front-end acceleration. +* 🧠 **TensorRT:** Integrated object detection and tracking. +* ⚡ **Sparse Tracking:** Options to move away from dense optical flow. +* 📦 **Modular:** System broken into packages for faster compile times. +* 🐳 **Docker:** Updated image with new dependencies. -
- -

Example output running on the Oxford Multimotion Dataset (OMD, 'Swinging 4 Unconstrained'). This visualisation was generated using playback after full-batch optimisation.

-
- - -## Feature Updates -November 2025 -- CUDA integration for front-end acceleration -- TensorRT for integrated object detection and tracking -- Options to move to sparse tracking for static and dynamic objects. This moves away from the need to calculate dense optical flow. -- Custom visualisation options per module. -- System is broken out into more packages for a cleaner interface and faster compile time. -- Updated docker image with new dependancies. -- Additional support for IMU and stereo SLAM systems (although still under active development) - -# 1. Installtion - -Tested on Ubuntu 20.04 - - -## Prerequisites -- [ROS2](https://docs.ros.org/en/humble/Installation.html) - > NOTE: this links to the Humble distribution which used during development. Other distro's will probably work. -- [GTSAM](https://github.com/borglab/gtsam) >= 4.1 -- [OpenCV](https://github.com/opencv/opencv) >= 3.4 -- [OpenGV](https://github.com/MIT-SPARK/opengv) -- [Glog](http://rpg.ifi.uzh.ch/docs/glog.html), [Gflags](https://gflags.github.io/gflags/) -- [Gtest](https://github.com/google/googletest/blob/master/googletest/docs/primer.md) (installed automagically) -- [config_utilities](https://github.com/MIT-SPARK/config_utilities) -- [dynamic_slam_interfaces](https://github.com/ACFR-RPG/dynamic_slam_interfaces) (Required by default. Can be optionally made not a requirement. See Insallation instructions below) - -External dependancies (for visualization) not required for compilation. -- [rviz_dynamic_slam_plugins](https://github.com/ACFR-RPG/rviz_dynamic_slam_plugins) (Plugin to display custom `dynamic_slam_interfaces` messages which are advertised by default.) - - -GPU acceleration: -- [TensorRT](https://github.com/NVIDIA/TensorRT) -- [CUDA](https://docs.nvidia.com/cuda/cuda-runtime-api/index.html) +## Documentation +We auto generate [Doxygen code docs](https://acfr-rpg.github.io/DynOSAM/doxygen/) for all classes in DynoSAM. The code docs are up-to-date with the _main_ branch. -are now supported with the new Docker image. -This provides support for TensorRT accellerated object-instance segmentation (which is now part of the DynoSAM pipeline) and CUDA acceleration in the front-end. -Backwards compatability (i.e no CUDA support) is not currently a priority. -> NOTE: documentation coming soon... +# 1. ⚙️ Installation +We provide a detailed installation guide including dependencies and Docker support. +See detailed instructions here: [Insallation instructions](./docs/media/INSTALL.md) -## Installation Instructions -DynoSAM is currently built within the ROS2 infrastructure (if there is enough interest I will split out each component into ROS and non-ROS modules.) +# 2. 🚀 Running DynoSAM -We provide a development [Dockerfile](./docker/Dockerfile) that will install all dependancies but expects DynoSAM to be cloned locally. The associated [container creation](./docker/create_container.sh) will then mount the local DynoSAM folder into the container along with local results/dataset folders. +## 2.1 Paramters +DynoSAM is configured using a combination of **YAML files** (pipeline, frontend, datasets) and **GFLAGS** (overridable command-line parameters). ROS parameters are used only for input file paths. +All `.yaml` and `.flags` files must be placed in a single parameter folder which defines the `params_path`. +``` +params/ + FrontendParams.yaml + PipelineParams.yaml + [DatasetParams.yaml] + [CameraParams.yaml] + *.flags +``` +- YAML files are loaded using config_utilities. +- GFlags provide run-time reconfiguration (important for automated experiments). +- ROS parameters are used sparingly (mainly for file paths). -The general ROS2 build procedure holds as all relevant subfolders in DynoSAM are built as packages. -More detailed instructions are found here: [Insallation instructions](./docs/media/INSTALL.md) +> NOTE: Note: Additional GFlags cannot be passed through ros2 launch. +To override GFlags pass them directly with ros2 run, or modify the flag files inside the params folder. +To print active parameters: ``` -# Finally compile -cd ros_ws && colcon build - -# Refresh workspace -source ~/ros_ws/install/setup.bash +ros2 run dynosam_utils eval_launch.py --show_dyno_args=true ``` - -`dynamic_slam_interfaces` is a require dependacy by default. This package is used to include custom messages that represet the state of each dynamic object per frame and is used by the ROS publishers. - -To disable this dependancy compile the code as +To see all gflag options run: ``` -colcon build --cmake-args -DENABLE_DYNAMIC_SLAM_INTERFACES=OFF +ros2 run dynosam_ros dynosam_node --help ``` -By default `ENABLE_DYNAMIC_SLAM_INTERFACES=ON` in the [CMakeLists.txt](./dynosam_ros/CMakeLists.txt). This CMake option will additionally change the _visualisation_ (and the output topics) used by DynoSAM. See the [ROS Visualisation](#ros-visualisation) section below. +## 2.2 Quick Start (Launch File) +Launch the full pipeline with ROS2: +``` +ros2 launch dynosam_ros dyno_sam_launch.py \ + params_path:= \ + dataset_path:= \ + v:= \ + --data_provider_type= \ + --output_path= +``` -# 2. Usage +The launch file: +- Loads all `.flags` files in the parameter folder, +- Applies dataset provider selection via `--data_provider_type`, +- Logs outputs to `--output_path` (must exist beforehand). -## Documentation -We auto generate [Doxygen code docs](https://acfr-rpg.github.io/DynOSAM/doxygen/) for all classes in DynoSAM. The code docs are up-to-date with the _main_ branch. +## 2.3 Experiment & Evaluation Launcher -## Running and Configuration +For fine‑grained control and automated experiments, use: -DynoSAM uses a combination of yaml files and GFLAGS (these are being simplified but GFLAGS allows an easier way to programatically set variables over cmd-line) to configure the system. ROS params are used sparingly and are only used to set input file paths. +``` +ros2 run dynosam_utils eval_launch.py \ + --dataset_path \ + --params_path \ + --output_path \ + --name \ + --run_pipeline \ + --run_analysis \ + +``` -### Running with Launch files -All .yaml and .flag files should be placed in the same params folder, which is specified by the command line. -To specify the dataset loader, the GFLAG `--data_provider_type` should be set (see [pipeline.flags](./dynosam/params/pipeline.flags)). Eventually, this will also include the option to take data live from a sensor. For data providers that are specific to each dataset used for evaluation, a dataset path must also be set. +This script: +- Automates running the pipeline and evaluations, +- Forwards all extra CLI arguments to DynoSAM (allowing any GFLAG override), +- Creates result folders `output_path/name/` automatically. -DynoSAM will also log all output configuration to an output-folder specified by `--output_path` (see [pipeline.flags](./dynosam/params/pipeline.flags)). -__Data will only be logged if this folder exists.__ +Example: -The DynoSAM pipeline can be launched via launch file: ``` -ros2 launch dynosam_ros dyno_sam_launch.py params_path:= dataset_path:=<> v:=<> +ros2 run dynosam_utils eval_launch.py \ + --output_path=/tmp/results \ + --name=test \ + --run_pipeline \ + --data_provider_type=2 ``` -The launch file will load all the GFLAG's from all .flag files found in the params folder. -### Running with complex input -For evaluation and more refined control over the input to the system we also provide an evaluation launch script and can be run as: -``` -ros2 run dynosam_utils eval_launch.py - --dataset_path - --params_path - --launch_file - --output_path - --name - --run_pipeline - --run_analysis - *args... +## 2.4 Programmatic Execution (Python) -``` -This script automated the process of running the evaluation suite (ie. `--run_analysis_`) and set all rosparams/re-direct input and output paths (e.g. `--output_path`, `name`, etc...). +All command‑line behaviour can be replicated in Python. +See: [run_experiments_tro.py](./dynosam_utils/src/run_experiments_tro.py) for examples. -In addition to these arguments, this script takes all additional cmd-line arguments and parses them to the DynoSAM node, allowing any GFLAGS to be overwritten directly by specifying them in the commandline. e.g the dataset provider type can be specified as: -``` -ros2 run dynosam_utils eval_launch.py --output_path=/path/to/results --name test --run_pipeline --data_provider_type=2 -``` -This script will also construct the corresponding output folder (e.g. ouput_path/name) and make it, if it does not exist. In the aboce example, the program will make the folder _'/path/to/results/test/'_ and deposit all output logs in that folder. -> NOTE: for all evaluations and metrics, this script was used to run the program. +# 3. 📂 Datasets -### Running programtically -All the cmdline functionality can be replicated programtically using python in order to run experiments and evaluations. -See [run_experiments_tro.py](./dynosam_utils/src/run_experiments_tro.py) for examples. +## 3.1 Pre-processing Image data +DynoSAM requires input image data in the form: +- RGB +- Depth/Stereo +- Dense Optical Flow +- Dense Semantic Instance mask +Each image is expected in the following form: +- __rgb__ image is expected to be a valid 8bit image (1, 3 and 4 channel images are accepted). +- __depth__ must be a _CV_64F_ image where the value of each pixel represents the _metric depth_. +- __mask__ must be a CV_32SC1 where the static background is of value $0$ and all other objects are lablled with a tracking label $j$. $j$ is assumed to be globally consistent and is used to map the tracked object $j$ to the ground truth. +- __flow__ must be a CV_32FC2 representing a standard optical-flow image representation. -## Running different backends -Most of the research here is associated with different backend formulations. -To run the different backends set `--backend_updater_enum` +For dense optical flow (ie. pre Novemeber 2025) we use [RAFT](https://link.springer.com/chapter/10.1007/978-3-030-58536-5_24). Currently this pre-processing code is not available. -> NOTE: this enum value is mapped to the enum [RGBDFormulationType](./dynosam/include/dynosam/backend/RGBDBackendDefinitions.hpp) +For instance segmentation we use [YOLOv8](https://docs.ultralytics.com/models/yolov8/) for both image pre-processing and online processing. Both Python and C++ (using TensorRT acceleration) models can be found in the [dynosam_nn](./dynosam_nn/) package. See the [REAMDE](./dynosam_nn/README.md) for more details. -- WCME (`backend_updater_enum=0`) and WCPE(`backend_updater_enum=1`) are from TRO-2025, ICRA 2024 and previous works -- HYBRID (`backend_updater_enum=2`) and PARALLEL_HYBRID (`backend_updater_enum=3`) are from RA-L 2025 +- If `prefer_provided_optical_flow: true` (YAML), the pipeline expects a dense flow image. Otherwise, it falls back to sparse KLT. +- If `prefer_provided_object_detection: true` (YAML) , an instance mask must be provided. If false, masks are generated online via. -All others are internal/experimental. -## Tests -We use [gtest](https://github.com/google/googletest) for unit testing. This is installed automagically. When building with ROS, all tests will go into the install folder. +## 3.2 Running with pre-processed data -To run the unit tests: build the code, navigate inside the `install` folder and run the tests. Both `dynosam` and `dynosam_ros` packages come with unit tests. +DynoSAM provides dataset loaders that parse pre-processed images (ie. depth, optical flow, masks), and ground truth into a unified format. -We provide a useful script to make running tests easier. Run -``` -ros2 run dynosam_ros run_dynosam_gtest.py +All official datasets are hosted at the [ACFR-RPG Datasets page](https://data.acfr.usyd.edu.au/rpg/). +To download a dataset, create a directory for the relevant dataset and, within the directory, run the command +```bash +wget -m -np -nH --cut-dirs=4 -R "index.html*" https://data.acfr.usyd.edu.au/rpg/dynosam/[Dataset]/[Subset] ``` -from anywhere on the system to run tests. The unit tests for a particular package (ie. `dynosam` and `dynosam_ros`) can be specified using the `--package` argumement. This script forwards all arguments to the test executable so that GFLAGS can still be used e.g. +For example, for the `kitti` dataset with subset `0004`, create and enter the directory `kitti-0004` download all files: ``` -run dynosam_ros run_dynosam_gtest.py --package=dynosam_ros --gtest_filter=TestConcepts* +wget -m -np -nH --cut-dirs=4 -R "index.html*" https://data.acfr.usyd.edu.au/rpg/dynosam/kitti/0004 ``` -## Datasets +> NOTE: when developing using docker, download the sequences into the data folder mounted into the docker container so it may be accessed by the program. -We provide a number of data providers which process datasets into the input format specified by DynoSAM which includes input images for the pipeline and ground truth data for evaluation. +The following datasets are officially supported: +| Dataset | Dataset ID | Notes | +|--------|--------------|-------| +| KITTI Tracking | 0 | Uses modified version with GT motion, flow, masks. | +| Virtual KITTI 2 | 1 | Raw dataset supported directly. | +| Cluster-SLAM (CARLA) | 2 | Raw dataset available; we recommend our processed version. | +| Oxford Multimotion (OMD) | 3 | Modified 2024 version available. | +| TartanAir Shibuya | 5 | Preprocessed version supported. | +| VIODE | 6 | IMU-enabled sequences supported. | -The provided dataset loaders are written to parse the datasets as provided and only -`--data_provider_type` and `--dataset_path` must be set to load the data. +To run a specific dataset only two GFLAGS are required: +``` +--dataset_path +--data_provider_type +``` +where `--dataset_path` points to the location of the dataset and `--data_provider_type` should be set to Dataset ID. -All datasets (including pre-processed images) can be found at the [ACFR-RPG Datasets page](https://data.acfr.usyd.edu.au/rpg/). -To download the datasets use the [`curl`](https://www.geeksforgeeks.org/linux-unix/curl-command-in-linux-with-examples/) command. +> NOTE: when using ground truth for evaluation it is important to ensure that `prefer_provided_object_detection: false` so that the pre-processing object masks are used. This ensures that tracking label $j$ used within the pipeline aligns with the ground truth label. +## 3.3 Online Data +DynoSAM can also run from data provided by ROS. -### i. KITTI Tracking Dataset -We use a modified version of the KITTI tracking dataset which includes ground truth motion data, as well dense optical-flow, depth and segmentation masks. -The required dataset loader can be specified by setting `--data_provider_type=0` +To run with online data (e.g. from raw sensor data) set the ROS parameter: +``` +online:=True +``` -### ii. Oxford Multimotion Dataset (OMD) -Raw data can be downloaded from the [project page](https://robotic-esp.com/datasets/omd/). -For our 2024 T-RO paper we used a modified version of the dataset which can be downloaded from the above link. +DynoSAM can run in two input modes which can be set using the ROS parameter `input_image_mode`: +- `ALL` (`input_image_mode=0`) +- `RGBD` (`input_image_mode=1`). -The required dataset loader can be specified by setting `--data_provider_type=3` +When in `ALL` mode, all image-preprocessing is done upstream and five inputs are required: +By default this node subscribes to five topics: + - `dataprovider/image/camera_info` (sensor_msgs.msg.CameraInfo) + - `dataprovider/image/rgb` (sensor_msgs.msg.Image) + - `dataprovider/image/depth` (sensor_msgs.msg.Image) + - `dataprovider/image/mask` (sensor_msgs.msg.Image) + - `dataprovider/image/flow` (sensor_msgs.msg.Image) +In `RGBD` mode, only camera intrinsics, rgb and depth images are required and all other image processing (including object detection) is done as part of the pipeline. -### iii. Cluster Dataset -The [raw dataset](https://huangjh-pub.github.io/page/clusterslam-dataset/) can be downloaded for the the CARLA-* sequences, although we recommend using our provided data. -The required dataset loader can be specified by setting `--data_provider_type=2` +### 3.2.1 Running using RBG-D camera -### iv. Virtual KITTI 2 -Access [raw dataset](https://europe.naverlabs.com/research/computer-vision/proxy-virtual-worlds-vkitti-2/) and extract in a folder. No pre-processing is needed on this dataset and the raw data can be parsed by DynoSAM directly. +To run from data provided by an RGB-D camera use +``` +ros2 launch dynosam_ros dyno_sam_online_rgbd_launch.py +``` +and remap the topics accordingly. -The required dataset loader can be specified by setting `--data_provider_type=1` -### v. TartanAir Shibuya -The required dataset loader can be specified by setting `--data_provider_type=5` +# 4. 🕹️ System Configuration +## 4.1 Backend Setup -### vi. VIODE -The required dataset loader can be specified by setting `--data_provider_type=6` +Running Different Backends -### Online Dataprovider -An online data-provider can be specified using the ROS arg `online:=True`. +DynoSAM supports multiple backend formulations. Set the `backend_updater_enum` flag to switch between them. -By default this node subscribes to five topics: - - `dataprovider/image/camera_info` (sensor_msgs.msg.CameraInfo) - - `dataprovider/image/rgb` (sensor_msgs.msg.Image) - - `dataprovider/image/depth` (sensor_msgs.msg.Image) - - `dataprovider/image/mask` (sensor_msgs.msg.Image) - - `dataprovider/image/flow` (sensor_msgs.msg.Image) +| Enum | Formulation | Paper | Description | +| :---: | :--- | :--- | :--- | +| **0** | `WCME` | TRO 2025 | World-Centric Motion Estimator | +| **1** | `WCPE` | TRO 2025 | World-Centric Pose Estimator | +| **2** | `HYBRID` | RA-L 2025 | Hybrid formulation | +| **3** | `PARALLEL_HYBRID` | RA-L 2025 | Parallel Hybrid (Recommended for speed) | -where each topic represents one of the expected input images in the following form: -The __rgb__ image is expected to be a valid 8bit image (1, 3 and 4 channel images are accepted). -The __depth__ must be a _CV_64F_ image where the value of each pixel represents the _metric depth_. -The __mask__ must be a CV_32SC1 where the static background is of value 0 and all other objects are lablled with a tracking label $j$. -The __flow__ must be a CV_32FC2 representing a standard optical-flow image representation. +Many factor-graph related paramters exist in the `backend.flags` file. +## 4.2 Instance Segmentation +DynoSAM uses YOLOv8 for object detection. The model is integrated into the front-end and accelerated using CUDA and TensorRT. -We also provide a launch file specified for online usage: +To run the pipeline with online segmentation and object tracking set ``` -ros2 launch dynosam_ros dyno_sam_online_launch.py +prefer_provided_object_detection: true ``` -> NOTE: see the launch file for example topic remapping - -Prior to September 2025 DynoSAM relied on pre-computing the flow and segmentation mask data offline. -New features have been added to allow the segementation and feature tracking to be done internally, with specific caviats; this means that only RGB and Depth images must be provided, replicating a proper RGB-D pipeline. +See the [dynosam_nn README.md](./dynosam_nn//README.md) for more detail on the models used and how to export the weights. -Specific launch files for this are coming soon... +> NOTE: the provided datasets contain pre-computed object masks with tracking labels $j$ that align with the ground truth. Use `prefer_provided_object_detection: false`. - -### IMU integration +## 4.3 IMU integration We additionally support IMU integration using the `PreintegrationFactor` from GTSAM in the backend. However, this has only been tested on VIODE. - -## ROS Visualisation +## 4.4 ROS Visualisation All 3D visualisation in DynoSAM is done using RVIZ. Camera pose and point clouds are vizualised using standard ROS messages. Visualising the objects is more complex and we provide two different ways to do this which can be controlled at compile time using the cmake flag `-DENABLE_DYNAMIC_SLAM_INTERFACES=ON/OFF` 1. (`ON`, now default) Usage of the custom `dynamic_slam_interfaces::msg::ObjectOdometry` (in [dynamic_slam_interfaces](https://github.com/ACFR-RPG/dynamic_slam_interfaces)) to publish to current state of the each object per frame. Our custom RVIZ plugin [rviz_dynamic_slam_plugins](https://github.com/ACFR-RPG/rviz_dynamic_slam_plugins) can be used to visualize this message type. The object id, current pose, velocity, path etc... will be shown for each object. See this [README.md](./dynosam_ros/include/dynosam_ros/displays/dynamic_slam_displays/README.md) for more detail. @@ -295,130 +303,53 @@ All 3D visualisation in DynoSAM is done using RVIZ. Camera pose and point clouds > NOTE: the publishing of object states is meant primilarily for __visualisation__ purposes and not for recording data used for evaluation. This is done using csv files in a different submodule: see [Evaluation](#4-evaluation). +## 4.5 Tests +We use [gtest](https://github.com/google/googletest) for unit testing. This is installed automagically. When building with ROS, all tests will go into the install folder. -# 2. Image Pre-processing -DynoSAM requires input image data in the form: -- RGB -- Depth/Stereo -- Dense Optical Flow -- Dense Semantic Instance mask - -and can usually be obtained by pre-processing the input RGB image. - -As of November 2025 we no longer fully rely on dense optical flow and instead use sparse tracking for static features and optionally for dynamic features. - -A dense optical flow image can be provided in the input `ImageContainer`. To use a provided optical flow image, set `prefer_provided_optical_flow: true`. Usually we use RAFT to generate dense optical flow. If this argument is true but no image is provided, it falls back to sparse KLT tracking. - -A similar logic follows with the dense semantic instance mask where each pixel value $>0$ represents the tracked object index $j$ and background pixels are labelled $0$. -If `prefer_provided_object_detection: true` then this data is expected to be provided in the input `ImageContainer`. For example, this is how we load data from all datasets. - -As of November 2025 we can also generate tracked objects masks _online_ using TensorRT accelerated inference. This is the default option if `prefer_provided_object_detection: false`. -See the [dynosam_nn](./dynosam_nn/) package for details. - - -# 3. Parameters - -We use a two different sets of parameters for our pipeline: - -- YAML files: contain params for the Pipeline and Frontend and some dataset related parameters. We use the wonderful [config_utilies](https://github.com/MIT-SPARK/config_utilities) library to load these files. -- [gflag](https://gflags.github.io/gflags/) for all other params which are found in .flag files. - - -Structurally DynoSAM expects all params files (.yaml and .flag) to be found in the same folder. By default we use the param files in this repo but you can specify any path as long as the folder follows the required structure: +We provide a useful script to make running tests easier: ``` -params_folder: - - FrontendParams.yaml - - PipelineParams.yaml - [- DatasetParams.yaml ] - [- CameraParams.yaml ] - - *.flags +ros2 run dynosam_ros run_dynosam_gtest.py ``` +Running unit tests for a specific package (ie. `dynosam` and `dynosam_ros`) can be specified using the `--package` argumement. -The [dynosam launch file](./dynosam_ros//launch/dyno_sam_launch.py) has some smarts in it so that it will search the given the folder (`params path`) for any file with the ".flags" suffix and load the specified flags. - -From a design persepctive, we use gflags in addition to yaml files becuase we are able to programmatically change these params as additional arguments to the DynoSAM program. This is used extensively for automated experiments where we re-configure the pipeline for different runs. - -To see all params (yaml and gflags) run the eval launch script with the gflag `--show_dyno_args=true`. - -> NOTE: We have to manually print the params and there are some minor issues printing all params (e.g. we currently cannot print the camera params with the _config_utilities_ library due to the way we currently load the variables!) - -To see all gflag options run: +This script forwards all arguments to the test executable. This allows GFLAGS to be specified directly via the CLI. ``` -ros2 run dynosam_ros dynosam_node --help +run dynosam_ros run_dynosam_gtest.py --package=dynosam_ros --gtest_filter=TestConcepts* ``` -> NOTE: there may be some minor discrepeancies between the parameters as some values that are in the config files are then overwritten by gflag values so we can configure them easily. - -During experiments we programatically change gflag by passing them executable node as cmdline arguments. This can be done by - -1. Passing arguments to the `unknown_args` arg of [runner.run](./dynosam_utils/dynosam_utils/evaluation/runner.py) -2. Passing them directly to the `ros2 run` as the additional args. - -Gflags cannot be changed when using `ros2 launch` as this command does not allow additional arguments (ie. non-declared ROS arguments) to be passed through. In this case, the gflags can be manually changed in the relevant flag file, or a parameter folder can be specified with your custom settings in it. - - -## System Parameters - - -## Camera Parameters - -Camera Parameters (ie. intrinsics) two possible sources - - Provided by the loaded data-provider - - Loaded from [CameraParams.yaml](./dynosam//params/CameraParams.yaml) - -If the param `prefer_data_provider_camera_params` is set to True, the parameters provided by the data-loader (if available) will be used. Otherwise, the paramters from the yaml file will be used. -This allows each data-loader to specfy/loaded the camera params from the dataset itself, without having to additionally specify the intrinsics in another file. -# 4. Evaluation +# 5. 📊 Output & Metrics -## Replicating results from papers -This code base contains implementations for many papers (as noted in Related Publications). -The `main` or `devel` branches should be able to run each method as described in the papers, however their may be discrepencies as new updates are added to different parts of the system. - -Additionally, I try to maintain backwards compatability as new code gets pushed to main but cannot ensure this. +The eval script should be used when generating results +``` +ros2 run dynosam_utils eval_launch.py + --output_path=/tmp/results \ + --name=test \ + --run_pipeline \ + --run_analysis +``` -See different package releases associated with a paper. +## 5.1 Reproducing Paper Results -When running evaluations for each paper there usually is an associated python script that includes all the experiments. +Scripts for reproducing experiments: - [TRO 2025 experiments](./dynosam_utils/src/run_experiments_tro.py) - [RAL 2025 experiments](./dynosam_utils/src/run_experiments_ecmr.py) +While we attempt to maintain compatibility, minor differences may arise due to ongoing development. -## Output logs -When the DynoSAM pipeline is run it will output log files in the given output directory. The log files record the output of the state-estimation as well as various interal data. -As mentioned, the output directory is specified by `--output_path=/path/to/results` (and may be further specified using `--name` when using `eval_launch.py`). - -The log files include (but are not limited to) files in the form - -- __*camera_pose_log.csv__ -- __*object_motion_log.csv__ -- __*object_pose_log.csv__ - -The presence of (at least ) these three files in a folder define a "dynosam results folder", from which the provided evaluation suite can process and produce results and metrics. - -Some additional statistics files will also be logged. The * specifies a module prefix (e.g. frontend, rgbd_world_backend) that is used to specify where the logged state-estimates have come from. -This is used to separate out the front-end estimation from the back-end. Each back-end formulation (ie. Motion Estimator and Pose Estimator) provides its own module name, which is used as the module prefix. An additional suffix can be added to the module name through the `--updater_suffix` gflag. - -Camera Pose log files are in the form: -``` -frame id, tx, ty, tz, qx, qy, qz, qw, gt_tx, gt_ty, gt_tz, gt_qx, gt_qy, gt_qz, gt_qw -``` -where the transformation `T_world_camera` is reported in translation and quaternion form. - -Object Pose/Motion log files log in the form: -``` -frame id, object id, tx, ty, tz, qx, qy, qz, qw, gt_tx, gt_ty, gt_tz, gt_qx, gt_qy, gt_qz, gt_qw -``` -In the case of motion logs, the frame id is the __to__ motion, such that the motion logged takes the object frame __from frame id - 1 to frame id__. +## 5.2 Output Logs +When `eval_launch.py` is run with `--run_analysis`, DynoSAM generates a "results folder" containing: -## Results and Metrics +- __Logs__: `camera_pose_log.csv`, `object_motion_log.csv`, etc. +- __Plots__: `*results.pdf` (ATE, RPE, Trajectory plots). +- __Tables__: `result_tables.pdf` (Summary statistics). -The DynoSAM framework comes with automated [evaluation tools](./dynosam_utils/dynosam_utils//evaluation/) that run as part of the pipeline. This can be run with the`--run_analysis` argument when using `eval_launch.py`. The evaluation module will look for a valid dynosam results folder in the provided output directory and will run evaluations _per prefix found_ in the folder. This enables one folder to contain multiple sets of log files, each defined with a different prefix, i.e for one dataset, multiple configurations of the back-end can be run and logged to the same folder, as long as the prefix is different. +at the path `/tmp/results/test`. -From the logged files the evaluation suite will produce ATE and RPE results for visual odometry and AME, RME and RPE results for objects. See our TRO paper for more details on these metrics. Per-frame numerical errors are logged in __*results.pdf__ with error plots for each metric as well as plots of the object and camera trajectory additionally included. +The * specifies a module prefix (e.g. frontend, wcme) that is used to specify where the logged state-estimates have come from. +An additional suffix can be added to the module name through the `--updater_suffix` flag. -> Two examples of our automated result generation are shown below @@ -426,82 +357,47 @@ From the logged files the evaluation suite will produce ATE and RPE results for
Image 1 description
-A full summary of numerical results are also generated in __result_tables.pdf__. This file includes all metrics for all sets of dynosam results found. The per-object error (for each metric) is recorded, as is the mean error over all objects for a particular metric. - -The `DatasetEvaluator` does most of the heavy lifting and can be found in the [evaluation_lib.py](./dynosam_utils/dynosam_utils/evaluation/evaluation_lib.py) module. As shown in some of the other evaluation scripts (e.g [sliding_window_vs_batch_error_plot.py](./dynosam_utils/src/sliding_window_vs_batch_error_plot.py)) we can use the tools and classes defined to do more complex evaluation and visualisation. - -# 5. Program Structure and Modules - - -
- -
+ -We follow a similar module/pipeline structure as developed in [Kimera](https://github.com/MIT-SPARK/Kimera-VIO) and I would like to thank the authors for their wondeful open-source code that has given me much inspiration. +# 6. 🧠 System Architecture +We follow a modular pipeline structure inspired by Kimera-VIO: -Where possible, I have streamlined their 'Modular Pipelining'; each high-level component (effectively the data-provider, front-end, back-end and visualiser) is defined by a [Pipeline](./dynosam/include/dynosam/pipeline/PipelineBase.hpp) and a [Module](./dynosam/include/dynosam/common/ModuleBase.hpp). +
System Diagram
+where a Pipeline/Module architecture connected by thread-safe queues -A Pipeline is a connecting class, derived from `PipelineBase`, is responsible for controlling the movement and transformation of data too and from modules. In most cases, this involves some data-packet that is produced as the output of another module and sending through the pipeline. This is achieved by connecting a [`ThreadSafeQueue`](./dynosam/include/dynosam/pipeline/ThreadSafeQueue.hpp) to the input and output of a pipeline. Each pipeline has an associated module which actually does the processing on the input data and produced some output data, which is then sent along the connected ouput queue to any connecting modules. - - -## References to our TRO Paper -How many times have you read a research paper and then gone to the code only to have absolutely no idea how the implementation connects to the theory/discussion? - -Too many in my experience. - -### Notation - -The notation from our paper has the following implementation: - -- $k$ (used as discrete time-step) is denoted with `FrameId` in the code. - > NOTE: $k$ is treated as a 'time-step' but in the code this is assocaited with a frame number (an integer value from 0... N). We also have assocaited `Timestamp` information associated with each frame but this is not actually used. -- $i$ (used as unique tracking id) is denoted as `TrackletId` in the code. -- $j$ (used as unique object id) is denoted as `ObjectId` in the code. - -All transform types (e.g. poses $X$ and motions $H$) are implemented using `gtsam::Pose3` and measurement types (e.g. 3D landmarks/points and 2D Features) are `Eigen::Vectors` as implemented through gtsam. - -> NOTE: I was very inconsistent with my variable naming through this code, mainly becuase I did not find a good way to deal with the funky three-indexed notation very well. I am fixing this as I go ;) - -### Modules -The front-end and back-ends are implemented in associated subfolders. - - -__Front-end__ - -- Main feature tracker defined in [FeatureTracker.hpp](./dynosam/include/dynosam/frontend/vision/FeatureTracker.hpp) although the implementation is spread across a few impl and .cc files. - -- Motion Estimation modules (PnP + RANSAC and the two refinement steps) are all defined in [MotionSolver.hpp](./dynosam/include/dynosam/frontend/vision/MotionSolver.hpp). - -__Back-end__ - -As per our key contributions, our back-end is structured to facilitate new implementations for Dynamic SLAM systems. This is achieved through two key classes - -- [`Formulation`](./dynosam/include/dynosam/backend/Formulation.hpp) base class which acts as the foundational structure for building and managin factor graphs in Dynamic SLAM. - - It is reponsible for Constructs factor graphs with initial values. - - Manages the high-level bookkeeping for when points and objects are added. - - Integrates new observations into the graph and updates it accordingly. - - It creates an `Acccessor` object which is used externally to extract and interact with the internal representation. - -- [`Accessor`](./dynosam/include/dynosam/backend/Accessor.hpp) defines the interface between the derived `Formulation` and the backend modules and facilitates the extraction and conversion of variables into a format that aligns with backend expectations. This format is specified in our paper as $\mathcal{O}_k$. - -Each formulation will need to derive from `Formulation` and define their own `Accessor`. The formulations discussed in our various works are implemented as - - [`WorldMotionFormulation`](./dynosam/include/dynosam/backend/rgbd/WorldMotionEstimator.hpp) - - [`WorldPoseFormulation`](./dynosam/include/dynosam/backend/rgbd/WorldPoseEstimator.hpp) - - [`Hybrid`](./dynosam/include/dynosam/backend/rgbd/HybridEstimator.hpp) - - [`Parallel-Hybrid`](./dynosam/include/dynosam/backend/ParallelHybridBackendModule.hpp) +```mermaid +graph LR + A[Input Data] --> B(Pipeline Wrapper) + B --> C{ThreadSafe Queue} + C --> D[Module Processing] + D --> E{Output Queue} + E --> F[Next Module] +``` +is employed to connect data-provider, frontend, backend and visualizer modules. +### Code-to-Theory Mapping +To help researchers connect the theory from our papers to the implementation, here is a mapping of our mathematical notation to the corresponding variable names and classes in the DynoSAM framework. +| Mathematical Notation | Code Implementation | Description | +| :---: | :--- | :--- | +| $k$ (Time-step) | `FrameId` | Integer frame number (0...N). Used as the primary time index. | +| $i$ (Feature ID) | `TrackletId` | Unique feature tracking identifier. | +| $j$ (Object ID) | `ObjectId` | Unique tracked object label. | +| $X$ (Pose), $H$ (Motion) | `gtsam::Pose3` | Transformations for poses and motions, represented using the GTSAM library. | +| $\mathcal{O}_k$ (Observations) | `Accessor` | Interface class used by backend modules to extract and interact with internal factor graph data. | -# 6. Contribution Guidelines +# 7. Contribution Guidelines We strongly encourage you to submit issues, feedback and potential improvements. We follow the branch, open PR, review, and merge workflow. -### Support ### +## Support The developpers will be happy to assist you or to consider bug reports / feature requests. But questions that can be answered reading this document will be ignored. Please contact @@ -524,6 +420,6 @@ pre-commit install We also provide a `.clang-format` file with the style rules that the repo uses, so that you can use [`clang-format`](https://clang.llvm.org/docs/ClangFormat.html) to reformat your code. -# 7. BSD License +# 8. BSD License The DynoSAM framework is open-sourced under the BSD License, see [LICENSE](./LICENSE). diff --git a/docker/Dockerfile b/docker/Dockerfile.amd64 similarity index 98% rename from docker/Dockerfile rename to docker/Dockerfile.amd64 index 85a2a4f1b..7d0806905 100644 --- a/docker/Dockerfile +++ b/docker/Dockerfile.amd64 @@ -32,7 +32,8 @@ RUN apt-get update \ # ROS installs RUN apt-get install -y \ ros-kilted-ros2cli-common-extensions \ - ros-kilted-vision-opencv + ros-kilted-vision-opencv \ + nlohmann-json3-dev # other deps RUN apt-get install libpng++-dev diff --git a/docker/Dockerfile.l4t_jetpack6 b/docker/Dockerfile.l4t_jetpack6 new file mode 100644 index 000000000..5b1beb20e --- /dev/null +++ b/docker/Dockerfile.l4t_jetpack6 @@ -0,0 +1,182 @@ +FROM rwthika/ros2-torch:jazzy-ros-base-torch2.5.0 + +MAINTAINER Jesse Morris "jesse.morris@sydney.edu.au" + + +# To avoid tzdata asking for geographic location... +ENV DEBIAN_FRONTEND=noninteractive + +#Install build dependencies +RUN apt-get update && apt-get upgrade -y --allow-downgrades --no-install-recommends apt-utils +RUN apt-get update && apt-get install -y --allow-downgrades git cmake build-essential pkg-config +# Install xvfb to provide a display to container for GUI realted testing. +RUN apt-get update && apt-get install -y --allow-downgrades xvfb + +# In the arm64 version of the base image we do not have the nvidia-cuda-development toolkit +# as explained https://github.com/ika-rwth-aachen/docker-ros-ml-images/issues/16 +# this contains nvcc (ie we dont have it) +# we need nvcc to build opencv with cuda +# add extra nvidia container apt repository (otherwise we cannot find nvidia-cuda-toolkit) +RUN curl -fsSL https://nvidia.github.io/libnvidia-container/gpgkey | sudo gpg --dearmor -o /usr/share/keyrings/nvidia-container-toolkit-keyring.gpg \ + && curl -s -L https://nvidia.github.io/libnvidia-container/stable/deb/nvidia-container-toolkit.list | \ + sed 's#deb https://#deb [signed-by=/usr/share/keyrings/nvidia-container-toolkit-keyring.gpg] https://#g' | \ + sudo tee /etc/apt/sources.list.d/nvidia-container-toolkit.list + + +#NOTE: both nvidia-cuda-toolkit and cuda-toolkit are needed (particularly for header files) +RUN apt-get update \ + && apt-get install -y \ + python3-pip \ + openssh-client \ + software-properties-common \ + nano \ + vim \ + clang-format \ + nvidia-cuda-toolkit \ + cuda-toolkit \ + libnvinfer-dev \ + libnvonnxparsers-dev \ + tensorrt-dev \ + && pip3 install black pre-commit \ + && rm -rf /var/lib/apt/lists/* + + + +# # ROS installs +RUN apt-get update && apt-get install -y \ + ros-jazzy-ros2cli-common-extensions \ + ros-jazzy-cv-bridge \ + ros-jazzy-vision-opencv \ + ros-jazzy-pcl-ros \ + ros-jazzy-rmw-fastrtps-cpp \ + ros-jazzy-rmw-zenoh-cpp \ + ros-jazzy-image-transport \ + libpcl-conversions-dev + +# other deps +# RUN apt-get install libpng++-dev +# is libpng-dev different from libpng++? I think not available as a apt for aarm64 +# RUN apt-get install libpng-dev + +RUN python3 -m pip install pylatex evo setuptools pre-commit scipy argcomplete black pre-commit + +# Install glog, gflags +RUN apt-get update && apt-get install -y libgflags2.2 libgflags-dev libgoogle-glog-dev + +# Install xvfb to provide a display to container for GUI realted testing. +# vtk is needed for OpenCV to build its viz module (from contrib!) +RUN apt-get update && apt-get install -y xvfb python3-dev python3-setuptools libvtk9-dev + +RUN pip3 install setuptools pre-commit scipy matplotlib argcomplete + +# install CSparse +RUN DEBIAN_FRONTEND=noninteractive apt install -y libsuitesparse-dev + +RUN python3 -m pip install "ultralytics==8.3.0" "numpy<2.0" "opencv-python<5.0" +RUN python3 -m pip install https://github.com/ultralytics/assets/releases/download/v0.0.0/onnxruntime_gpu-1.23.0-cp310-cp310-linux_aarch64.whl + +# Parallelism C++ for CPU +RUN apt-get update && apt-get install -y libboost-all-dev libtbb-dev + +WORKDIR /root/ + + + +# Install OpenCV for Ubuntu +RUN apt-get update && apt-get install -y \ + unzip \ + libjpeg-dev libpng-dev libpng++-dev libtiff-dev libgtk2.0-dev \ + libatlas-base-dev gfortran + + +RUN git clone https://github.com/opencv/opencv.git +RUN cd opencv && \ + git checkout tags/4.10.0 && \ + mkdir build + +RUN git clone https://github.com/opencv/opencv_contrib.git +RUN cd opencv_contrib && \ + git checkout tags/4.10.0 + +# on aarch64 there is no binary for nlohmann-json3 +RUN git clone https://github.com/nlohmann/json.git +RUN cd json && mkdir build && \ + cd build && \ + cmake .. && make -j$(nproc) install + +# OpenCV looks for the cuDNN version in cudnn_version.h, but it's been renamed to cudnn_version_v8.h +RUN ln -sfnv /usr/include/$(uname -i)-linux-gnu/cudnn_version_v*.h /usr/include/$(uname -i)-linux-gnu/cudnn_version.h + +# IMPORTANT: must specify CXX_17 version. Default is c++11 which will cause compilation issues +# On aarch64 we can enable NEON for CPU level optimisations +# It is vital we use gcc-11 as otheriwise we cannot compile the cuda level code (ie. CUDA 12.9) +RUN cd opencv/build && \ + cmake -DCMAKE_BUILD_TYPE=Release \ + -DCMAKE_INSTALL_PREFIX=/usr/local \ + -DCMAKE_CXX_STANDARD=17 \ + -D BUILD_opencv_python=OFF \ + -D BUILD_opencv_python2=OFF \ + -D BUILD_opencv_python3=ON \ + -D WITH_CUDA=ON \ + -D WITH_CUDNN=ON \ + -D INSTALL_CUDA_LIBS=OFF \ + -D CUDA_ARCH_PTX= \ + -D CUDA_ARCH_BIN=8.7 \ + -D CUDNN_INCLUDE_DIR=/usr/include/$(uname -i)-linux-gnu \ + -D OPENCV_DNN_CUDA=ON \ + -D ENABLE_FAST_MATH=ON \ + -D CUDA_FAST_MATH=ON \ + -D WITH_VTK=ON \ + -D WITH_CUFFT=ON \ + -D WITH_CUBLAS=ON \ + -D WITH_TBB=ON \ + -DENABLE_NEON=ON \ + -D BUILD_TESTS=OFF \ + -D BUILD_PERF_TESTS=OFF \ + -D BUILD_opencv_ts=OFF \ + -D BUILD_opencv_sfm=OFF \ + -D BUILD_JAVA=OFF \ + -DOPENCV_EXTRA_MODULES_PATH=/root/opencv_contrib/modules .. && \ + make -j$(nproc) install + +RUN git clone https://github.com/MIT-SPARK/config_utilities.git +RUN cd config_utilities/config_utilities && mkdir build && \ + cd build && \ + cmake .. && make -j$(nproc) install + + + +# Install GTSAM +RUN git clone https://github.com/borglab/gtsam.git +RUN cd gtsam && \ + git fetch && \ + git checkout tags/4.2.0 && \ + mkdir build && \ + cd build && \ + cmake -DCMAKE_INSTALL_PREFIX=/usr/local \ + -DGTSAM_USE_SYSTEM_EIGEN=ON \ + -DGTSAM_BUILD_TESTS=OFF -DGTSAM_BUILD_EXAMPLES_ALWAYS=OFF -DCMAKE_BUILD_TYPE=Release -DGTSAM_BUILD_UNSTABLE=ON -DGTSAM_POSE3_EXPMAP=ON -DGTSAM_ROT3_EXPMAP=ON -DGTSAM_TANGENT_PREINTEGRATION=OFF .. && \ + make -j$(nproc) install + + +# Install Open_GV +RUN git clone https://github.com/MIT-SPARK/opengv +RUN cd opengv && \ + mkdir build +RUN cd opengv/build && \ + cmake -DCMAKE_BUILD_TYPE=Release \ + -DCMAKE_INSTALL_PREFIX=/usr/local \ + .. && make -j$(nproc) install + + +RUN echo 'export RMW_IMPLEMENTATION=rmw_zenoh_cpp' >> ~/.bashrc + +RUN mkdir -p /home/user/dev_ws/src/core +RUN mkdir -p /home/user/dev_ws/src/third_parties +RUN mkdir -p /home/user/upstream_ws/src + +SHELL ["/bin/bash", "-c"] + +RUN source /opt/ros/jazzy/setup.bash + +WORKDIR /home/user/dev_ws diff --git a/docker/README.md b/docker/README.md new file mode 100644 index 000000000..4db1cbc4e --- /dev/null +++ b/docker/README.md @@ -0,0 +1,26 @@ +# Docker Files for DynoSAM + +Base images are pulled from [docker-ros-ml-images](https://github.com/ika-rwth-aachen/docker-ros-ml-images) + +- Dockerfile.amd64 is a linux/amd64 image tested on x86_64 desktop +- Dockerfile.l4t_jetpack6 is build from linux/arm64 tested on an NVIDIA ORIN NX with Jetpack 6 + +## Jetson Settings +Architecture | aarch64 +Ubuntu | 22.04.5 LTS (Jammy Jellyfish) +Jetson Linux | 36.4.7 +Python | 3.10.12 +ROS | jazzy +CMake | 3.22.1 +CUDA | 12.6.77-1 +cuDNN | 9.3.0.75-1 +TensorRT | 10.7.0.23-1+cuda12.6 +PyTorch | 2.8.0 +GPUs | (Orin (nvgpu)) +OpenCV | 4.10.0 + +> NOTE: The CUDA/Pytorch/TensorRT versions settings come with the base dockerfile but in practice we have been using CUDA 12.9. + +## Other versioning +matplotlib=3.6.3 +numpy=1.26.4 \ No newline at end of file diff --git a/docker/build_docker.sh b/docker/build_docker.sh deleted file mode 100755 index b87559f38..000000000 --- a/docker/build_docker.sh +++ /dev/null @@ -1,9 +0,0 @@ -#!/usr/bin/env bash -TAG=$1 - -if [ -z $TAG ]; then - echo "Usage: ./docker/build_docker.sh TAG [BASE_IMAGE]" - exit -1 -fi - -DOCKER_BUILDKIT=1 docker build -f docker/Dockerfile -t $TAG . diff --git a/docker/build_docker_amd64.sh b/docker/build_docker_amd64.sh new file mode 100755 index 000000000..412a652a8 --- /dev/null +++ b/docker/build_docker_amd64.sh @@ -0,0 +1,2 @@ +#!/usr/bin/env bash +bash build_docker_base.sh Dockerfile.amd64 acfr_rpg/dyno_sam_cuda diff --git a/docker/build_docker_base.sh b/docker/build_docker_base.sh new file mode 100755 index 000000000..e9b1edc3f --- /dev/null +++ b/docker/build_docker_base.sh @@ -0,0 +1,29 @@ +#!/usr/bin/env bash +DOCKERFILE=$1 +TAG=$2 + +if [[ -z $TAG ]] || [[ -z $DOCKERFILE ]]; then + SCRIPT_NAME=$(basename "$0") + echo "Usage: ./$SCRIPT_NAME DOCKERFILE TAG" + exit -1 +fi + + +# 2. Identify the directory where this script is located +# This resolves the path even if called from a different location +SCRIPT_DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" &> /dev/null && pwd )" + +# 3. Temporarily change into the script's directory +echo "Changing working directory to: $SCRIPT_DIR" +cd "$SCRIPT_DIR" || { echo "Failed to change directory"; exit 1; } + +# 4. Verify the file exists in this directory +if [ -f "$DOCKERFILE" ]; then + echo "Building dockerfile $DOCKERFILE with tag $TAG" + DOCKER_BUILDKIT=1 docker build --network=host -f $DOCKERFILE -t $TAG . + # ---------------------------- + +else + echo "Error: File '$DOCKERFILE' not found in $SCRIPT_DIR" + exit 1 +fi diff --git a/docker/build_docker_l4t_jetpack6.sh b/docker/build_docker_l4t_jetpack6.sh new file mode 100755 index 000000000..66481c5c3 --- /dev/null +++ b/docker/build_docker_l4t_jetpack6.sh @@ -0,0 +1,2 @@ +#!/usr/bin/env bash +bash build_docker_base.sh Dockerfile.l4t_jetpack6 acfr_rpg/dynosam_cuda_l4t diff --git a/docker/create_container_amd64.sh b/docker/create_container_amd64.sh new file mode 100755 index 000000000..ef1c0860f --- /dev/null +++ b/docker/create_container_amd64.sh @@ -0,0 +1,10 @@ +#!/usr/bin/env bash + +### EDIT THIS TO WHEREVER YOU'RE STORING YOU DATA ### +# folder should exist before you mount it +LOCAL_DATA_FOLDER=/media/jmor6670/T7/datasets +LOCAL_RESULTS_FOLDER=~/results/ +LOCAL_DYNO_SAM_FOLDER=~/Code/src/DynOSAM/ +LOCAL_THIRD_PARTY_DYNO_SAM_FOLDER=~/Code/src/third_party_dynosam/ + +bash create_container_base.sh acfr_rpg/dyno_sam_cuda dyno_sam $LOCAL_DATA_FOLDER $LOCAL_RESULTS_FOLDER $LOCAL_DYNO_SAM_FOLDER $LOCAL_THIRD_PARTY_DYNO_SAM_FOLDER diff --git a/docker/create_container.sh b/docker/create_container_base.sh similarity index 72% rename from docker/create_container.sh rename to docker/create_container_base.sh index 85401770f..d75ab6420 100755 --- a/docker/create_container.sh +++ b/docker/create_container_base.sh @@ -1,14 +1,18 @@ #!/usr/bin/env bash -CONTAINER_NAME=dyno_sam -CONTAINER_IMAGE_NAME=acfr_rpg/dyno_sam_cuda +CONTAINER_IMAGE_NAME=$1 +CONTAINER_NAME=$2 -### EDIT THIS TO WHEREVER YOU'RE STORING YOU DATA ### -# folder should exist before you mount it -LOCAL_DATA_FOLDER=/media/jmor6670/T7/datasets -LOCAL_RESULTS_FOLDER=~/results/ -LOCAL_DYNO_SAM_FOLDER=~/Code/src/DynOSAM/ -LOCAL_THIRD_PARTY_DYNO_SAM_FOLDER=~/Code/src/third_party_dynosam/ +LOCAL_DATA_FOLDER=$3 +LOCAL_RESULTS_FOLDER=$4 +LOCAL_DYNO_SAM_FOLDER=$5 +LOCAL_THIRD_PARTY_DYNO_SAM_FOLDER=$6 + +echo "Creating dynosam container ($CONTAINER_NAME) from image: $CONTAINER_IMAGE_NAME" +echo "Local data folder: $LOCAL_DATA_FOLDER" +echo "Local results folder: $LOCAL_RESULTS_FOLDER" +echo "Local DynoSAM folder: $LOCAL_DYNO_SAM_FOLDER" +echo "Local third party dynosam folder: $LOCAL_THIRD_PARTY_DYNO_SAM_FOLDER" CONTAINER_DATA_FOLDER=/root/data @@ -17,8 +21,6 @@ CONTAINER_WORKSPACE_FOLDER=/home/user/dev_ws/src/core/ CONTAINER_WORKSPACE_FOLDER_THIRD_PARTY=/home/user/dev_ws/src/third_parties/ - - USE_NVIDIA=false # If we are running in a docker-in-docker scenario NVIDIA_SOS will be populated @@ -78,11 +80,34 @@ if "$USE_NVIDIA"; then else TERMINAL_FLAGS='-t' fi + + echo "$@" # Create the container based on the launchfile it's launching (if any) # removes '.launch' from the last argument echo "Container name will be: $CONTAINER_NAME" # docker run $DOCKER_NVIDIA_SO_VOLUMES \ - docker run \ + # docker run \ + # --privileged \ + # --gpus all \ + # -i -d \ + # --volume $XSOCK:$XSOCK:rw \ + # -v $LOCAL_DATA_FOLDER:$CONTAINER_DATA_FOLDER \ + # -v $LOCAL_RESULTS_FOLDER:$CONTAINER_RESULTS_FOLDER \ + # -v $LOCAL_DYNO_SAM_FOLDER:$CONTAINER_WORKSPACE_FOLDER \ + # -v $LOCAL_THIRD_PARTY_DYNO_SAM_FOLDER:$CONTAINER_WORKSPACE_FOLDER_THIRD_PARTY \ + # -v /var/run/docker.sock:/var/run/docker.sock \ + # --env DISPLAY=$DISPLAY \ + # --env XAUTHORITY=$XAUTH \ + # --env QT_X11_NO_MITSHM=0 \ + # --env QT_X11_NO_XRENDER=0 \ + # --volume $XAUTH:$XAUTH:rw \ + # --net host \ + # --pid host \ + # --ipc host \ + # -it \ + # --name=$CONTAINER_NAME \ + # $CONTAINER_IMAGE_NAME "$@" + docker run \ --privileged \ --gpus all \ -i -d \ @@ -102,7 +127,8 @@ if "$USE_NVIDIA"; then --ipc host \ -it \ --name=$CONTAINER_NAME \ - $CONTAINER_IMAGE_NAME "$@" + $CONTAINER_IMAGE_NAME fi +# FOR NOW? xhost +local:`docker inspect --format='{{ .Config.Hostname }}' $CONTAINER_NAME` diff --git a/docker/create_container_l4t_jetpack6.sh b/docker/create_container_l4t_jetpack6.sh new file mode 100755 index 000000000..3e429ae95 --- /dev/null +++ b/docker/create_container_l4t_jetpack6.sh @@ -0,0 +1,11 @@ +#!/usr/bin/env bash + + +### EDIT THIS TO WHEREVER YOU'RE STORING YOU DATA ### +# folder should exist before you mount it +LOCAL_DATA_FOLDER=/media/jmor6670/T7/datasets/ +LOCAL_RESULTS_FOLDER=/home/usyd/dynosam/results/ +LOCAL_DYNO_SAM_FOLDER=/home/usyd/dynosam/DynOSAM/ +LOCAL_THIRD_PARTY_DYNO_SAM_FOLDER=/home/usyd/dynosam/extra + +bash create_container_base.sh acfr_rpg/dynosam_cuda_l4t dyno_sam_l4t $LOCAL_DATA_FOLDER $LOCAL_RESULTS_FOLDER $LOCAL_DYNO_SAM_FOLDER $LOCAL_THIRD_PARTY_DYNO_SAM_FOLDER diff --git a/docs/media/INSTALL.md b/docs/media/INSTALL.md index 63c0ebb4e..6de60c608 100644 --- a/docs/media/INSTALL.md +++ b/docs/media/INSTALL.md @@ -1,134 +1,150 @@ # DynoSAM Installation -Tested on Ubuntu 20.04 +## Prerequisites +- [ROS2](https://docs.ros.org/en/kilted/Installation.html) + > NOTE: this links to the kilted distribution which used during development. We have also tested on Jazzy for the NVIDIA ORIN +- [GTSAM](https://github.com/borglab/gtsam) >= 4.1 +- [OpenCV](https://github.com/opencv/opencv) >= 3.4 +- [OpenGV](https://github.com/MIT-SPARK/opengv) +- [Glog](http://rpg.ifi.uzh.ch/docs/glog.html), [Gflags](https://gflags.github.io/gflags/) +- [Gtest](https://github.com/google/googletest/blob/master/googletest/docs/primer.md) (installed automagically) +- [config_utilities](https://github.com/MIT-SPARK/config_utilities) +- [dynamic_slam_interfaces](https://github.com/ACFR-RPG/dynamic_slam_interfaces) (Required by default. Can be optionally made not a requirement. See Insallation instructions below) -> NOTE: this instructions are taken essentially verbatum from the included [Dockerfile](./../../docker/Dockerfile) +External dependancies (for visualization) not required for compilation. +- [rviz_dynamic_slam_plugins](https://github.com/ACFR-RPG/rviz_dynamic_slam_plugins) (Plugin to display custom `dynamic_slam_interfaces` messages which are advertised by default.) -## CUDA dependancies -As of September 2025 we are adding cuda dependancies for certain OpenCV operations (and evenautually for running inference on images). - -> NOTE: I have a very old GPU (RTX 2080 which is only running CIDA 12.2 with Driver Version: 535.183.01), get CUDA support however you are able! - -The docker file has been updated and based off the [docker-ros-ml-images](https://github.com/ika-rwth-aachen/docker-ros-ml-images?tab=readme-ov-file#rwthikaros2-torch-ros-2-nvidia-cuda-nvidia-tensorrt-pytorch) to include -- CUDA -- TensorRT -- Pytorch -built ontop of ROS2. - -The CUDA dependancies are mostly for OpenCV modules but also eventually for adding DNN inference into DynoSAM itself so processing of images -can be handled internally (ie YOLO). -- **Important:** Check [GPU Compute Capability](https://developer.nvidia.com/cuda-gpus) to set `CUDA_ARCH_BIN` flag -- NVIDIA GeForce RTX 2080 is 7.5 +GPU acceleration: +- [TensorRT](https://github.com/NVIDIA/TensorRT) +- [CUDA](https://docs.nvidia.com/cuda/cuda-runtime-api/index.html) -## Dependancies +are now supported with the new Docker image. +This provides support for TensorRT accellerated object-instance segmentation (which is now part of the DynoSAM pipeline) and CUDA acceleration in the front-end. +Backwards compatability (i.e no CUDA support) is not currently a priority. -```bash -sudo apt-get install -y --no-install-recommends apt-utils +We provide detailed install instructions when using the Docker. -sudo apt-get install -y cmake +To install natively, install the dependancies as required by docker and build as a ROS2 package. -sudo apt install unzip libjpeg-dev libpng-dev libpng++-dev libtiff-dev libgtk2.0-dev libatlas-base-dev gfortran libgflags2.2 libgflags-dev libgoogle-glog0v5 libgoogle-glog-dev python3-dev python3-setuptools clang-format python3-pip nlohmann-json3-dev libsuitesparse-dev -``` +## Docker Install instructions -GTSAM's Optional dependencies (highly recommended for speed) also include -```bash -sudo apt install libboost-all-dev libtbb-dev -``` +DynoSAM has been tested on x86_64 and aarm64 (with a NVIDIA ORIN) devices using the [two docker files](../../docker/) provided. See the [REAMDE.md](../../docker/README.md) for more detail on hardware used etc. -For evaluation we also need various formatting tools -```bash -sudo apt-get install texlive-pictures texlive-science texlive-latex-extra latexmk -``` +We provide scripts to build and create docker containers to run and develop DynoSAM which is intendend to be mounted within the created container. -Python (pip) dependancies: -```bash -python3 -m pip install pylatex evo setuptools pre-commit scipy matplotlib argcomplete black pre-commit -``` +### Folder Structure +To DynoSAM code to be changed within the docker environment, we mount the local version of the code within the container. To ensure this happens smoothly please download the DynoSAM code in the following structure -NOTE: There seems to be a conflict between the version of matplotlib and numpy. In this case following worked for me: ``` -sudo apt remove python3-matplotlib -python3 -m pip install numpy==1.26.3 +dynosam_pkg/ + DynoSAM/ + extras/ + rviz_dynamic_slam_plugins + dynamic_slam_interfaces + results/ ``` +where DynoSAM is this repository and anything in `extras` are other ROS packages that wish to be built alongside DynoSAM. -## Install OpenCV +> NOTE: `dynosam_pkg' may be any folder, its just a place to put all the DynoSAM related code. -Note that you can use `apt-get install libopencv-dev libopencv-contrib-dev` on 20.04 instead of building from source. +> NOTE: the reason for this structure is historical and due to the way the _create_container_ scripts are written; you don't need to do this but I provide complete instructures for simplicity. -> Also note, I have not actually tried this! -To build from source - -```bash -git git clone https://github.com/opencv/opencv_contrib.git -cd opencv_contrib && \ - git checkout tags/4.8.0 - -cd ../ && -git clone git clone https://github.com/opencv/opencv.git - -cd opencv && \ -git checkout tags/4.8.0 && mkdir build -cmake -DCMAKE_BUILD_TYPE=Release \ - -DCMAKE_INSTALL_PREFIX=/usr/local \ - -D BUILD_opencv_python=OFF \ - -D BUILD_opencv_python2=OFF \ - -D BUILD_opencv_python3=ON \ - -DOPENCV_EXTRA_MODULES_PATH=../../opencv_contrib/modules .. # Use -DWITH_TBB=On if you have TBB - -sudo make -j$(nproc) install +### Docker Build +Build the relevant docker file: ``` +cd dynosam_pkg/DynoSAM/docker && +./build_docker_amd64.sh //or build_docker_l4t.sh +``` +### Create Container +Once built, you should have a docker image called `acfr_rpg/dyno_sam_cuda` or `acfr_rpg/dynosam_cuda_l4t` -## Install GTSAM - -Tested with release 4.2 - -```bash - -git clone https://github.com/borglab/gtsam.git -cd gtsam && \ - git fetch && \ - git checkout tags/4.2.0 && \ - mkdir build && \ - cd build && \ - cmake -DCMAKE_INSTALL_PREFIX=/usr/local \ - -DGTSAM_USE_SYSTEM_EIGEN=ON \ - -DGTSAM_BUILD_TESTS=OFF -DGTSAM_BUILD_EXAMPLES_ALWAYS=OFF -DCMAKE_BUILD_TYPE=Release -DGTSAM_BUILD_UNSTABLE=ON -DGTSAM_POSE3_EXPMAP=ON -DGTSAM_ROT3_EXPMAP=ON -DGTSAM_TANGENT_PREINTEGRATION=OFF .. && \ +In the associated _create_container_ scripts modify the local variables to match the folder paths on the local machine. These folders will be mounted as a volume within the container +``` +LOCAL_DATA_FOLDER=/path/to/some/datasets/ +LOCAL_RESULTS_FOLDER=/path/to/dynosam_pkg/results/ +LOCAL_DYNO_SAM_FOLDER=/path/to/dynosam_pkg/DynoSAM/ +LOCAL_THIRD_PARTY_DYNO_SAM_FOLDER=/path/to/dynosam_pkg/extras +``` +Run the creation script. The created container will be called `dyno_sam` +``` +./create_container_amd64.sh //or ./create_container_l4t_jetpack6.sh +``` -sudo make -j$(nproc) install +Finally enter into the container and build DynoSAM +``` +cd /home/user/dev_w && +export MAKEFLAGS="-j10" && clear && colcon build ``` -## Install OpenGV +## CUDA dependancies +As of September 2025 we are adding cuda dependancies for certain OpenCV operations (and evenautually for running inference on images). -```bash -git clone https://github.com/MIT-SPARK/opengv -cd opengv && mkdir build && cd build && -cmake -DCMAKE_BUILD_TYPE=Release \ - -DCMAKE_INSTALL_PREFIX=/usr/local .. +> NOTE: I have a very old GPU (RTX 2080 which is only running CUDA 12.2 with Driver Version: 535.183.01), get CUDA support however you are able! -sudo make -j$(nproc) install +The docker file has been updated and based off the [docker-ros-ml-images](https://github.com/ika-rwth-aachen/docker-ros-ml-images?tab=readme-ov-file#rwthikaros2-torch-ros-2-nvidia-cuda-nvidia-tensorrt-pytorch) to include +- CUDA +- TensorRT +- Pytorch +built ontop of ROS2. -``` -Note: This links to a forked version of OpenGV which contains an updated fix the CMakeLists.txt due to issues with -march=native being set. This links to original version of [OpenGV](https://github.com/laurentkneip/opengv), which contains the problematic flag. +The CUDA dependancies are mostly for OpenCV modules but also eventually for adding DNN inference into DynoSAM itself so processing of images +can be handled internally (ie YOLO). -## Install Config Utilities +- **Important:** Check [GPU Compute Capability](https://developer.nvidia.com/cuda-gpus) to set `CUDA_ARCH_BIN` flag +- NVIDIA GeForce RTX 2080 is 7.5 -```bash -git clone git@github.com:MIT-SPARK/config_utilities.git +## Additional Installation Notes -cd config_utilities/config_utilities -mkdir build -cd build -cmake .. -make -j +`dynamic_slam_interfaces` is a require dependacy by default. This package is used to include custom messages that represet the state of each dynamic object per frame and is used by the ROS publishers. -sudo make install +To disable this dependancy compile the code as ``` - -## Additional Installation Notes +colcon build --cmake-args -DENABLE_DYNAMIC_SLAM_INTERFACES=OFF +``` +By default `ENABLE_DYNAMIC_SLAM_INTERFACES=ON` in the [CMakeLists.txt](./dynosam_ros/CMakeLists.txt). This CMake option will additionally change the _visualisation_ (and the output topics) used by DynoSAM. See the [ROS Visualisation](#ros-visualisation) section below. Due to DynoSAM being build within ROS: - Need to build GTSAM with `-DGTSAM_USE_SYSTEM_EIGEN=ON` to avoid issues with ROS and OpenCV-Eigen compatability. Confirmed from https://discourse.ros.org/t/announcing-gtsam-as-a-ros-1-2-package/32739 which explains that the GTSAM ROS package is build with this flag set to ON (and describes it as "problematic"). We still want to build GTSAM from source so we can control the version and other compiler flags. Kimera-VIO's install instructions indicate that OpenGV must use the same version of Eigen as GTSAM, which can be set using compiler flags. Since we are using the ROS install Eigen, I have removed these flags and hope that the package manager with CMake can find the right (and only) version. This has not proved problematic... yet... + +## Possible Compilation Issues +### Missing MPI Header Error +When first compiling DynoSAM, this error may appear as MPI relies on a non-existent directory. +This issue _should_ be fixed a patch in the `dynosam_common` CMakeLists.txt which directly updates the `MPI_INCLUDE_PATH`. + +However, if the issue persists, the following is a known fix. +1. Unset HPC-X variables + ```bash + unset OPENMPI_VERSION + unset OMPI_MCA_coll_hcoll_enable + unset OPAL_PREFIX + export PATH=$(echo $PATH | tr ':' '\n' | grep -v '/opt/hpcx' | grep -v '/usr/local/mpi/bin' | paste -sd:) + ``` + Now verify `env | grep -i mpi echo $PATH` that the `OPAL_PREFIX` and HPC-X paths no longer appear +2. Install OpenMPI + ```bash + sudo apt update + sudo apt install libopenmpi-dev openmpi-bin + ``` + and verify that the following points to `/usr/bin/mpicc` + ```bash + which mpicc + mpicc --version + ``` +3. Clear the ROS2 workspace + ```bash + rm -rf build/ install/ log/ + ``` +4. Build with the system MPI + ```bash + export MPI_C_COMPILER=/usr/bin/mpicc + export MPI_CXX_COMPILER=/usr/bin/mpicxx + colcon build + ``` +### Missing Dependencies +- `nlohmannjson` may not be installed, so install with `sudo apt install nlohmann-json3-dev` +- GTSAM may not be installed, so install with `pip install gtsam` + diff --git a/dynosam/include/dynosam/backend/Accessor-impl.hpp b/dynosam/include/dynosam/backend/Accessor-impl.hpp index 06b5a24cb..72043afef 100644 --- a/dynosam/include/dynosam/backend/Accessor-impl.hpp +++ b/dynosam/include/dynosam/backend/Accessor-impl.hpp @@ -36,23 +36,28 @@ namespace dyno { -template -AccessorT::AccessorT(const SharedFormulationData& shared_data, - typename Map::Ptr map) - : shared_data_(shared_data), map_(map) { +template +template +AccessorT::AccessorT( + const SharedFormulationData& shared_data, typename Map::Ptr map, + DerivedArgs&&... derived_args) + : DerivedAccessor(std::forward(derived_args)...), + shared_data_(shared_data), + map_(map) { CHECK_NOTNULL(shared_data.values); CHECK_NOTNULL(shared_data.hooks); } -template -StateQuery AccessorT::getStaticLandmark( +template +StateQuery AccessorT::getStaticLandmark( TrackletId tracklet_id) const { const auto lmk = CHECK_NOTNULL(map()->getLandmark(tracklet_id)); - return this->query(lmk->makeStaticKey()); + return this->template query(lmk->makeStaticKey()); } -template -MotionEstimateMap AccessorT::getObjectMotions(FrameId frame_id) const { +template +MotionEstimateMap AccessorT::getObjectMotions( + FrameId frame_id) const { MotionEstimateMap motion_estimates; const auto frame_node = map()->getFrame(frame_id); @@ -72,25 +77,9 @@ MotionEstimateMap AccessorT::getObjectMotions(FrameId frame_id) const { return motion_estimates; } -template -StateQuery AccessorT::getObjectMotionReferenceFrame( - FrameId frame_id, ObjectId object_id) const { - StateQuery motion_query = this->getObjectMotion(frame_id, object_id); - if (motion_query) { - return StateQuery( - motion_query.key_, - Motion3ReferenceFrame(motion_query.get(), - Motion3ReferenceFrame::Style::F2F, - ReferenceFrame::GLOBAL, frame_id - 1u, frame_id)); - } else { - return StateQuery(motion_query.key_, - motion_query.status_); - } -} - -template -EstimateMap AccessorT::getObjectPoses( - FrameId frame_id) const { +template +EstimateMap +AccessorT::getObjectPoses(FrameId frame_id) const { EstimateMap pose_estimates; const auto frame_node = map()->getFrame(frame_id); @@ -112,8 +101,8 @@ EstimateMap AccessorT::getObjectPoses( return pose_estimates; } -template -ObjectPoseMap AccessorT::getObjectPoses() const { +template +ObjectPoseMap AccessorT::getObjectPoses() const { ObjectPoseMap object_poses; for (FrameId frame_id : map()->getFrameIds()) { EstimateMap per_object_pose = @@ -132,8 +121,8 @@ ObjectPoseMap AccessorT::getObjectPoses() const { return object_poses; } -template -ObjectMotionMap AccessorT::getObjectMotions() const { +template +ObjectMotionMap AccessorT::getObjectMotions() const { ObjectMotionMap object_motions; for (FrameId frame_id : map()->getFrameIds()) { MotionEstimateMap per_object_motions = this->getObjectMotions(frame_id); @@ -150,8 +139,9 @@ ObjectMotionMap AccessorT::getObjectMotions() const { return object_motions; } -template -StatusLandmarkVector AccessorT::getDynamicLandmarkEstimates( +template +StatusLandmarkVector +AccessorT::getDynamicLandmarkEstimates( FrameId frame_id) const { const auto frame_node = map()->getFrame(frame_id); CHECK_NOTNULL(frame_node); @@ -165,8 +155,9 @@ StatusLandmarkVector AccessorT::getDynamicLandmarkEstimates( return estimates; } -template -StatusLandmarkVector AccessorT::getDynamicLandmarkEstimates( +template +StatusLandmarkVector +AccessorT::getDynamicLandmarkEstimates( FrameId frame_id, ObjectId object_id) const { const auto frame_node = map()->getFrame(frame_id); CHECK_NOTNULL(frame_node); @@ -198,8 +189,9 @@ StatusLandmarkVector AccessorT::getDynamicLandmarkEstimates( return estimates; } -template -StatusLandmarkVector AccessorT::getStaticLandmarkEstimates( +template +StatusLandmarkVector +AccessorT::getStaticLandmarkEstimates( FrameId frame_id) const { // dont go over the frames as this contains references to the landmarks // multiple times @@ -226,8 +218,8 @@ StatusLandmarkVector AccessorT::getStaticLandmarkEstimates( return estimates; } -template -StatusLandmarkVector AccessorT::getFullStaticMap() const { +template +StatusLandmarkVector AccessorT::getFullStaticMap() const { // dont go over the frames as this contains references to the landmarks // multiple times e.g. the ones seen in that frame StatusLandmarkVector estimates; @@ -250,10 +242,9 @@ StatusLandmarkVector AccessorT::getFullStaticMap() const { return estimates; } -template -bool AccessorT::hasObjectMotionEstimate(FrameId frame_id, - ObjectId object_id, - Motion3* motion) const { +template +bool AccessorT::hasObjectMotionEstimate( + FrameId frame_id, ObjectId object_id, Motion3* motion) const { const auto frame_node = map()->getFrame(frame_id); StateQuery motion_query = this->getObjectMotion(frame_id, object_id); @@ -266,9 +257,9 @@ bool AccessorT::hasObjectMotionEstimate(FrameId frame_id, return false; } -template -bool AccessorT::hasObjectPoseEstimate(FrameId frame_id, ObjectId object_id, - gtsam::Pose3* pose) const { +template +bool AccessorT::hasObjectPoseEstimate( + FrameId frame_id, ObjectId object_id, gtsam::Pose3* pose) const { const auto frame_node = map()->getFrame(frame_id); StateQuery pose_query = this->getObjectPose(frame_id, object_id); @@ -282,8 +273,9 @@ bool AccessorT::hasObjectPoseEstimate(FrameId frame_id, ObjectId object_id, return false; } -template -gtsam::FastMap AccessorT::computeObjectCentroids( +template +gtsam::FastMap +AccessorT::computeObjectCentroids( FrameId frame_id) const { gtsam::FastMap centroids; @@ -295,7 +287,8 @@ gtsam::FastMap AccessorT::computeObjectCentroids( const auto object_seen = frame_node->objects_seen.template collectIds(); for (ObjectId object_id : object_seen) { - const auto [centroid, result] = computeObjectCentroid(frame_id, object_id); + const auto [centroid, result] = + this->computeObjectCentroid(frame_id, object_id); if (result) { centroids.insert2(object_id, centroid); @@ -304,23 +297,14 @@ gtsam::FastMap AccessorT::computeObjectCentroids( return centroids; } -template -bool AccessorT::exists(gtsam::Key key) const { - const gtsam::Values* theta = shared_data_.values; - CHECK_NOTNULL(theta); - return theta->exists(key); -} - -template -template -StateQuery AccessorT::query(gtsam::Key key) const { +template +boost::optional +AccessorT::getValueImpl(const gtsam::Key key) const { const gtsam::Values* theta = shared_data_.values; - CHECK_NOTNULL(theta); if (theta->exists(key)) { - return StateQuery(key, theta->at(key)); - } else { - return StateQuery::NotInMap(key); + return theta->at(key); } + return boost::none; } } // namespace dyno diff --git a/dynosam/include/dynosam/backend/Accessor.hpp b/dynosam/include/dynosam/backend/Accessor.hpp index 0c4584060..de17395a4 100644 --- a/dynosam/include/dynosam/backend/Accessor.hpp +++ b/dynosam/include/dynosam/backend/Accessor.hpp @@ -30,6 +30,8 @@ #pragma once +#include + #include "dynosam/backend/BackendDefinitions.hpp" #include "dynosam_common/Types.hpp" #include "dynosam_opt/Map.hpp" @@ -40,6 +42,9 @@ namespace dyno { * @brief Accessor defines the interface between the structured output of a * BackendModule and any formulation used to construct the Dynamic SLAM problem. * + * Accesors are shared between the frontend and backend and therefore should be + * threadsafe! + * * Each derived Accessor is associated with a derived Formulation and knows how * to access the variables in that problem and convert them into the form * expected by the backend. This is equivalent to constructing \mathbf{O}_k = [ @@ -73,17 +78,6 @@ class Accessor { virtual StateQuery getObjectMotion( FrameId frame_id, ObjectId object_id) const = 0; - /** - * @brief Gets the motion _{k-1}^wH_k attime-step (k) and object (j) with the - * associated reference frame. - * - * @param frame_id FrameId - * @param object_id ObjectId - * @return StateQuery - */ - virtual StateQuery getObjectMotionReferenceFrame( - FrameId frame_id, ObjectId object_id) const = 0; - /** * @brief Get the pose (^wL_k) of an object (j) at time-step (k). * @@ -278,23 +272,6 @@ class Accessor { */ bool hasObjectMotionEstimate(FrameId frame_id, ObjectId object_id, Motion3& motion) const; -}; - -/** - * @brief Derived Accessor that knows about the MAP used with the formulation. - * - * @tparam MAP - */ -template -class AccessorT : public Accessor { - public: - using Map = MAP; - using This = AccessorT; - - DYNO_POINTER_TYPEDEFS(This) - - AccessorT(const SharedFormulationData& shared_data, typename Map::Ptr map); - virtual ~AccessorT() {} /** * @brief Gets the absolute motion (_{k-1}^wH_k) from theta the requested @@ -310,7 +287,93 @@ class AccessorT : public Accessor { * @return StateQuery */ StateQuery getObjectMotionReferenceFrame( - FrameId frame_id, ObjectId object_id) const override; + FrameId frame_id, ObjectId object_id) const; + + /** + * @brief Check if the key exists in the current theta. + * Utilises the derived Accessor#getValueImpl function + * to check the existance of the variable + * + * @param key gtsam::Key + * @return true + * @return false + */ + bool exists(gtsam::Key key) const; + + /** + * @brief Access a key in the current theta. + * Utilises the derived Accessor#getValueImpl function + * to check the existance of the variable and get the value. + * + * Throws gtsam::ValuesIncorrectType if ValueType does not match the internal + * type. + * + * @tparam ValueType + * @param key gtsam::Key + * @return StateQuery + */ + template + StateQuery query(gtsam::Key key) const { + boost::optional value_opt = this->getValueImpl(key); + if (value_opt) { + // Check the type and throw exception if incorrect + // h() split in two lines to avoid internal compiler error (MSVC2017) + const gtsam::Value* value = value_opt.get_ptr(); + auto h = gtsam::internal::handle(); + return StateQuery(key, h(key, value)); + } else { + return StateQuery::NotInMap(key); + } + } + + protected: + mutable std::mutex mutex_; + + /** + * @brief Gets the gtsam::Value object as an optional. + * The existance of the optional indicates existance of the value. + * We use this functionality as a replacement for the if(exists()) -> return + * at() paradigm so that the derived class can check and return the value as a + * single unit which reduces overhead. + * + * @param key + * @return boost::optional + */ + virtual boost::optional getValueImpl( + const gtsam::Key key) const = 0; +}; + +/** + * @brief Derived Accessor that knows about the MAP used with the formulation. + * The class will inherit from DerivedAccessor (which must itself be an + * Accessor), which allows this class to act as an Acessor. The DerivedAccessor + * class allows additional functionality to be added to the accessor beyond the + * base functionality (i.e. the virtual functions provided by Accessor) while + * still being an accessor. + * + * By default DerivedAccessor = Acessor, which therefore provides the base level + * functionlity of an acessor. + * + * @tparam MAP + * @tparam DerivedAccessor + */ +template +class AccessorT : public DerivedAccessor { + public: + // Compile-time assertion to ensure DerivedAccessor is a type of Accessor + // This is optional but highly recommended for safety + static_assert(std::is_base_of_v, + "DerivedAccessor must be derived from Accessor."); + + using Map = MAP; + using This = AccessorT; + + DYNO_POINTER_TYPEDEFS(This) + + template + AccessorT(const SharedFormulationData& shared_data, typename Map::Ptr map, + DerivedArgs&&... derived_args); + virtual ~AccessorT() {} /** * @brief Get a static landmark (^wm) with tracklet id (i). @@ -442,24 +505,8 @@ class AccessorT : public Accessor { gtsam::FastMap computeObjectCentroids( FrameId frame_id) const override; - /** - * @brief Check if the key exists in the current theta. - * - * @param key gtsam::Key - * @return true - * @return false - */ - bool exists(gtsam::Key key) const; - - /** - * @brief Access a key in the current theta. - * - * @tparam ValueType - * @param key gtsam::Key - * @return StateQuery - */ - template - StateQuery query(gtsam::Key key) const; + boost::optional getValueImpl( + const gtsam::Key key) const override; protected: auto map() const { return map_; } @@ -471,7 +518,6 @@ class AccessorT : public Accessor { return *CHECK_NOTNULL(shared_data_.hooks); } - private: private: //! in the associated formulation const SharedFormulationData shared_data_; typename Map::Ptr map_; //! Pointer to internal map structure; diff --git a/dynosam/include/dynosam/backend/BackendModule.hpp b/dynosam/include/dynosam/backend/BackendModule.hpp index 435e71513..0825054ce 100644 --- a/dynosam/include/dynosam/backend/BackendModule.hpp +++ b/dynosam/include/dynosam/backend/BackendModule.hpp @@ -63,6 +63,9 @@ struct BackendModuleTraits { using MapType = Map; }; +using FrontendUpdateInterface = + std::function; + /** * @brief Base class to actually do processing. Data passed to this module from * the frontend @@ -84,6 +87,19 @@ class BackendModule const NoiseModels& getNoiseModels() const { return noise_models_; } const BackendSpinState& getSpinState() const { return spin_state_; } + /** + * @brief Get the accessor the the underlying formulation, allowing the + * optimised values to be directly accessed + * + * @return Accessor::Ptr + */ + virtual Accessor::Ptr getAccessor() = 0; + + void registerFrontendUpdateInterface(const FrontendUpdateInterface& cb) { + CHECK(cb); + frontend_update_callback_ = cb; + } + protected: // called in ModuleBase immediately before the spin function is called virtual void validateInput( @@ -105,6 +121,7 @@ class BackendModule spin_state_; //! Spin state of the backend. Updated in the backend module //! base via InputCallback (see BackendModule constructor). NoiseModels noise_models_; + FrontendUpdateInterface frontend_update_callback_; private: }; diff --git a/dynosam/include/dynosam/backend/Formulation-impl.hpp b/dynosam/include/dynosam/backend/Formulation-impl.hpp index 32510e3c9..c5de43509 100644 --- a/dynosam/include/dynosam/backend/Formulation-impl.hpp +++ b/dynosam/include/dynosam/backend/Formulation-impl.hpp @@ -263,21 +263,17 @@ class StaticFormulationUpdater : public StaticFormulationUpdaterImpl { CHECK_NOTNULL(camera->safeGetRGBDCamera()); K_stereo_ = rgbd_camera->getFakeStereoCalib(); CHECK_NOTNULL(K_stereo_); - - K_stereo_->print("Stero K\n"); K_ = camera->getGtsamCalibration(); } bool addLandmark(LmkNode& lmk, const FrameNode& frame, - const UpdateObservationParams& update_params, + const UpdateObservationParams& /*update_params*/, gtsam::Values& values, gtsam::NonlinearFactorGraph& graph, gtsam::Key& point_key, UpdateObservationResult& result, std::optional& initial) override { point_key = lmk->makeStaticKey(); const FrameId frame_k = FrameId(frame->getId()); - const auto& params = this->formulation_->params(); - // NOTE: not handling the case a lmk becomes an outlier once already added // to the opt // hoping the robust cost funcion handles this! @@ -449,7 +445,10 @@ Formulation::Formulation(const FormulationParams& params, map_(map), noise_models_(noise_models), sensors_(sensors), - hooks_(hooks) {} + hooks_(hooks) { + static_updater_ = + std::make_unique>(this); +} template void Formulation::setTheta(const gtsam::Values& linearization) { @@ -569,15 +568,13 @@ UpdateObservationResult Formulation::updateStaticObservations( auto frame_node_k = map->getFrame(frame_id_k); CHECK_NOTNULL(frame_node_k); - internal::StaticFormulationUpdater static_updater(this); - VLOG(20) << "Looping over " << frame_node_k->static_landmarks.size() << " static lmks for frame " << frame_id_k; for (auto lmk_node : frame_node_k->static_landmarks) { gtsam::Key point_key; std::optional initial_value; - bool lmk_result = static_updater.addLandmark( + bool lmk_result = static_updater_->addLandmark( lmk_node, frame_node_k, update_params, new_values, internal_new_factors, point_key, result, initial_value); @@ -627,29 +624,47 @@ UpdateObservationResult Formulation::updateDynamicObservations( // as long as the new factor slot is calculated before adding a new factor const Slot starting_factor_slot = new_factors.size(); - const FrameId frame_id_k_1 = frame_id_k - 1u; - VLOG(20) << "Add dynamic observations between frames " << frame_id_k_1 - << " and " << frame_id_k; const auto frame_node_k = map->getFrame(frame_id_k); + // well not true for keyframes!! + // const FrameId frame_id_k_1 = frame_id_k - 1u; + VLOG(20) << "Add dynamic observations at " << frame_id_k << " for objects " + << container_to_string(frame_node_k->getObservedObjects()); + utils::TimingStatsCollector dyn_obj_itr_timer(this->loggerPrefix() + ".dynamic_object_itr"); for (const auto& object_node : frame_node_k->objects_seen) { DebugInfo::ObjectInfo object_debug_info; const ObjectId object_id = object_node->getId(); - // first check that object exists in the previous frame - if (!frame_node_k->objectMotionExpected(object_id)) { + const FrameId seen_most_rectently = object_node->getLastSeenFrame(); + CHECK_EQ(seen_most_rectently, frame_id_k); + // the frame id the object was observed in immediately prior to this one + + FrameId last_seen; + if (!object_node->previouslySeenFrame(&last_seen)) { + VLOG(20) << "Skipping j=" << object_id + << " as it has not been seen twice!"; continue; } + // auto frame_node + + // // first check that object exists in the previous frame + // objectMotionExpected looks at a hardcoded k-1 (should instead use last + // seen!?) if (!frame_node_k->objectMotionExpected(object_id)) { + // continue; + // } // possibly the longest call? // landmarks on this object seen at frame k auto seen_lmks_k = object_node->getLandmarksSeenAtFrame(frame_id_k); + VLOG(20) << "Adding N= " << seen_lmks_k.size() + << " measurements j=" << object_id << " from=" << last_seen + << " to= " << frame_id_k; // if we dont have at least N observations of this object in this frame AND // the previous frame if (seen_lmks_k.size() < params_.min_dynamic_observations || - object_node->getLandmarksSeenAtFrame(frame_id_k_1).size() < + object_node->getLandmarksSeenAtFrame(last_seen).size() < params_.min_dynamic_observations) { continue; } @@ -730,8 +745,8 @@ UpdateObservationResult Formulation::updateDynamicObservations( auto query_frame_node_k = *seen_frames_itr; auto query_frame_node_k_1 = *seen_frames_itr_prev; - CHECK_EQ(query_frame_node_k->frame_id, - query_frame_node_k_1->frame_id + 1u); + // CHECK_EQ(query_frame_node_k->frame_id, + // query_frame_node_k_1->frame_id + 1u); // add points UP TO AND INCLUDING the current frame if (query_frame_node_k->frame_id > frame_id_k) { @@ -782,7 +797,7 @@ UpdateObservationResult Formulation::updateDynamicObservations( // new_values.insert(local_new_values); } } else { - const auto frame_node_k_1 = map->getFrame(frame_id_k_1); + const auto frame_node_k_1 = map->getFrame(last_seen); CHECK_NOTNULL(frame_node_k_1); // these tracklets should already be in the graph so we should only need diff --git a/dynosam/include/dynosam/backend/Formulation.hpp b/dynosam/include/dynosam/backend/Formulation.hpp index 97af8da4c..c90b8a45c 100644 --- a/dynosam/include/dynosam/backend/Formulation.hpp +++ b/dynosam/include/dynosam/backend/Formulation.hpp @@ -40,6 +40,7 @@ #include "dynosam_opt/Map.hpp" // really this should be in a more 'core' file +#include "dynosam/frontend/VisionImuOutputPacket.hpp" #include "dynosam_opt/IncrementalOptimization.hpp" // only for ErrorHandlingHooks namespace dyno { @@ -74,6 +75,16 @@ struct UpdateObservationParams { bool enable_incremental_detail = false; }; +/** + * @brief Result of a static/dynamic point update by calling + * Formulation#updateStaticObservations or + * Formulation#updateDynamicObservations + * + * The objects affected per frame should be specified and any additional + * paramters for the optimisation (i.e gtsam::ISAM2UpdateParams) can be set, + * which will then be parsed to the optimiser by the BackendModule. + * + */ struct UpdateObservationResult { gtsam::FastMap> objects_affected_per_frame; // per frame @@ -158,7 +169,9 @@ class BackendParams; * */ struct PreUpdateData { + //! Should be keyframe id FrameId frame_id; + VisionImuPacket::ConstPtr input; PreUpdateData() {} PreUpdateData(FrameId _frame_id) : frame_id(_frame_id) {} @@ -170,7 +183,11 @@ struct PreUpdateData { */ struct PostUpdateData { FrameId frame_id; + //! Result from constructing the factor-graph based on dynamic point + //! observations UpdateObservationResult dynamic_update_result; + //! Result from constructing the factor-graph based on static point + //! observations UpdateObservationResult static_update_result; struct IncrementalResult { @@ -182,6 +199,9 @@ struct PostUpdateData { // TODO: batch result!!! + //! Result from the optimiser after updating/optimising the lastest + //! factor-graph Incremental result is set only if incremental optimisation + //! used. std::optional incremental_result = {}; PostUpdateData() {} @@ -275,8 +295,12 @@ class Formulation { using PointUpdateContextType = PointUpdateContext; using ObjectUpdateContextType = ObjectUpdateContext; - using AccessorType = AccessorT; - using AccessorTypePointer = typename AccessorT::Ptr; + + //! We only have these specalised alias for backwards compatability + //! We used to not have a base Accessor that was independant of the map type + //! so we needed an alias like Accessor. No longer! + using AccessorType = Accessor; + using AccessorTypePointer = Accessor::Ptr; DYNO_POINTER_TYPEDEFS(This) @@ -611,6 +635,9 @@ class Formulation { mutable std::optional fully_qualified_name_{std::nullopt}; friend class internal::StaticFormulationUpdaterImpl; + + using StaticFormulationUpdaterT = internal::StaticFormulationUpdaterImpl; + std::unique_ptr static_updater_; }; } // namespace dyno diff --git a/dynosam/include/dynosam/backend/ParallelHybridBackendModule.hpp b/dynosam/include/dynosam/backend/ParallelHybridBackendModule.hpp index d3ddc5cb0..7c87f23c9 100644 --- a/dynosam/include/dynosam/backend/ParallelHybridBackendModule.hpp +++ b/dynosam/include/dynosam/backend/ParallelHybridBackendModule.hpp @@ -45,6 +45,98 @@ namespace dyno { +class ParallelHybridBackendModule; + +/** + * @brief Special accessor class to access all values via a single interface + * since the optimisation of the static scene and dynamic objects are decoupled + * and therefore handled by their own formulation. + * + * Inherits from HybridAccessorCommon to extends the functionality of the base + * Acessor with functionality specific to the Hybrid representation. + * + */ +class ParallelHybridAccessor : public HybridAccessorCommon { + public: + DYNO_POINTER_TYPEDEFS(ParallelHybridAccessor) + + ParallelHybridAccessor(ParallelHybridBackendModule* parallel_hybrid_module); + + StateQuery getSensorPose(FrameId frame_id) const override; + + StateQuery getObjectMotion(FrameId frame_id, + ObjectId object_id) const override; + + StateQuery getObjectPose(FrameId frame_id, + ObjectId object_id) const override; + + StateQuery getDynamicLandmark( + FrameId frame_id, TrackletId tracklet_id) const override; + + StateQuery getStaticLandmark( + TrackletId tracklet_id) const override; + + EstimateMap getObjectPoses( + FrameId frame_id) const override; + + MotionEstimateMap getObjectMotions(FrameId frame_id) const override; + + ObjectPoseMap getObjectPoses() const override; + + ObjectMotionMap getObjectMotions() const override; + + StatusLandmarkVector getDynamicLandmarkEstimates( + FrameId frame_id) const override; + + StatusLandmarkVector getDynamicLandmarkEstimates( + FrameId frame_id, ObjectId object_id) const override; + + StatusLandmarkVector getStaticLandmarkEstimates( + FrameId frame_id) const override; + + StatusLandmarkVector getFullStaticMap() const override; + + StatusLandmarkVector getLocalDynamicLandmarkEstimates( + ObjectId object_id) const override; + + TrackletIds collectPointsAtKeyFrame( + ObjectId object_id, FrameId frame_id, + FrameId* keyframe_id = nullptr) const override; + + bool getObjectKeyFrameHistory(ObjectId object_id, + const KeyFrameRanges*& ranges) const override; + + bool hasObjectKeyFrame(ObjectId object_id, FrameId frame_id) const override; + + std::pair getObjectKeyFrame( + ObjectId object_id, FrameId frame_id) const override; + + StateQuery getEstimatedMotion( + ObjectId object_id, FrameId frame_id) const override; + + bool hasObjectMotionEstimate(FrameId frame_id, ObjectId object_id, + Motion3* motion) const override; + + bool hasObjectPoseEstimate(FrameId frame_id, ObjectId object_id, + gtsam::Pose3* pose) const override; + + gtsam::FastMap computeObjectCentroids( + FrameId frame_id) const override; + + protected: + // Helper function to call a function on an ObjectEstimator if it exists + // in the map of sam estimators + // otherwise the result of the FallbackFunc will be used (as default) + template + auto withOr(ObjectId object_id, Func&& func, FallbackFunc&& fallback) const; + + boost::optional getValueImpl( + const gtsam::Key key) const override; + + ParallelHybridBackendModule* parallel_hybrid_module_; + HybridAccessor::Ptr static_accessor_; +}; + class ParallelHybridBackendModule : public VisionImuBackendModule { public: @@ -62,11 +154,18 @@ class ParallelHybridBackendModule const gtsam::FastMap& objectEstimators() const; - HybridFormulation::Ptr staticEstimator() const; + HybridFormulationV1::Ptr staticEstimator() const; std::pair getActiveOptimisation() const override; + /** + * @brief Get the Accessor objecs implemented as the ParallelHybridAccessor + * + * @return Accessor::Ptr + */ + Accessor::Ptr getAccessor() override; + private: using SpinReturn = Base::SpinReturn; @@ -92,6 +191,7 @@ class ParallelHybridBackendModule Timestamp timestamp) const; void logBackendFromEstimators(); + void updateTrackletMapping(const VisionImuPacket::ConstPtr input); private: Camera::Ptr camera_; @@ -99,7 +199,7 @@ class ParallelHybridBackendModule mutable std::mutex mutex_; gtsam::ISAM2Params static_isam2_params_; - HybridFormulation::Ptr static_formulation_; + HybridFormulationV1::Ptr static_formulation_; gtsam::IncrementalFixedLagSmoother static_estimator_; gtsam::ISAM2Params dynamic_isam2_params_; @@ -108,8 +208,25 @@ class ParallelHybridBackendModule //! Vector of object ids that are new for this frame. Cleared after each spin ObjectIds new_objects_estimators_; + friend class ParallelHybridAccessor; + ParallelHybridAccessor::Ptr combined_accessor_; + + //! Fast look up of object ids for each point + //! Needed to look up the right object estimator for each point + FastUnorderedMap tracklet_id_to_object_; + //! used to cache the result of each update which will we log to file GenericObjectCentricMap result_map_; }; +// implement function after ParallelHybridBackendModule has been fully defined +template +auto ParallelHybridAccessor::withOr(ObjectId object_id, Func&& func, + FallbackFunc&& fallback) const { + const auto& object_estimators = parallel_hybrid_module_->sam_estimators_; + auto it = object_estimators.find(object_id); + if (it != object_estimators.end()) return func(it->second); + return fallback(); +} + } // namespace dyno diff --git a/dynosam/include/dynosam/backend/ParallelObjectISAM.hpp b/dynosam/include/dynosam/backend/ParallelObjectISAM.hpp index 45df1dad3..4165b6ce8 100644 --- a/dynosam/include/dynosam/backend/ParallelObjectISAM.hpp +++ b/dynosam/include/dynosam/backend/ParallelObjectISAM.hpp @@ -147,8 +147,20 @@ class ParallelObjectISAM { ObjectMotionMap getKeyFramedMotions() const; // due to the nature of this formulation, this will be the accumulated cloud!! + // points are in the world frame + // this is the same as calling this->getDynamicLandmarkEstimates(frame_id, + // object_id_); StatusLandmarkVector getDynamicLandmarks(FrameId frame_id) const; + // should return at most a map of size 1 (with the key being object_id_) or of + // size 0 if the object does not exist at the requested frame + EstimateMap getObjectPoses(FrameId frame_id) const { + return accessor_->getObjectPoses(frame_id); + } + + StateQuery getObjectPose(FrameId frame_id) const { + return accessor_->getObjectPose(frame_id, object_id_); + } std::pair insertNewKeyFrame(FrameId frame_id); inline const gtsam::ISAM2& getSmoother() const { return *smoother_; } @@ -196,7 +208,7 @@ class ParallelObjectISAM { const Params params_; const ObjectId object_id_; Map::Ptr map_; - HybridFormulation::Ptr decoupled_formulation_; + HybridFormulationV1::Ptr decoupled_formulation_; HybridAccessor::Ptr accessor_; std::shared_ptr smoother_; Result result_; diff --git a/dynosam/include/dynosam/backend/RegularBackendModule.hpp b/dynosam/include/dynosam/backend/RegularBackendModule.hpp index aea98fa0d..c2677b54c 100644 --- a/dynosam/include/dynosam/backend/RegularBackendModule.hpp +++ b/dynosam/include/dynosam/backend/RegularBackendModule.hpp @@ -100,6 +100,8 @@ class RegularBackendModule std::pair getActiveOptimisation() const override; + Accessor::Ptr getAccessor() override; + protected: void setupUpdates(); diff --git a/dynosam/include/dynosam/backend/VisionImuBackendModule.hpp b/dynosam/include/dynosam/backend/VisionImuBackendModule.hpp index 47f2b739c..740f2d6f0 100644 --- a/dynosam/include/dynosam/backend/VisionImuBackendModule.hpp +++ b/dynosam/include/dynosam/backend/VisionImuBackendModule.hpp @@ -86,7 +86,7 @@ class VisionImuBackendModule : public BackendModuleType { } gtsam::NavState addInitialVisualState( - FrameId frame_id_k, FormulationType* formulation, + FrameId frame_id_k, Timestamp timestamp_k, FormulationType* formulation, gtsam::Values& new_values, gtsam::NonlinearFactorGraph& new_factors, const NoiseModels& noise_models, const gtsam::Pose3& X_k_w, const gtsam::Vector3& V_k_body = gtsam::Vector3(0, 0, 0)) { @@ -98,21 +98,21 @@ class VisionImuBackendModule : public BackendModuleType { new_factors); // update nav state - return updateNavState(frame_id_k, X_k_w, V_k_body); + return updateNavState(frame_id_k, timestamp_k, X_k_w, V_k_body); } gtsam::NavState addInitialVisualInertialState( - FrameId frame_id_k, FormulationType* formulation, + FrameId frame_id_k, Timestamp timestamp_k, FormulationType* formulation, gtsam::Values& new_values, gtsam::NonlinearFactorGraph& new_factors, const NoiseModels& noise_models, const gtsam::NavState& nav_state_k, const gtsam::imuBias::ConstantBias& imu_bias) { return addInitialVisualInertialState( - frame_id_k, formulation, new_values, new_factors, noise_models, - nav_state_k.pose(), nav_state_k.bodyVelocity(), imu_bias); + frame_id_k, timestamp_k, formulation, new_values, new_factors, + noise_models, nav_state_k.pose(), nav_state_k.bodyVelocity(), imu_bias); } gtsam::NavState addInitialVisualInertialState( - FrameId frame_id_k, FormulationType* formulation, + FrameId frame_id_k, Timestamp timestamp_k, FormulationType* formulation, gtsam::Values& new_values, gtsam::NonlinearFactorGraph& new_factors, const NoiseModels& noise_models, const gtsam::Pose3& X_k_w, const gtsam::Vector3& V_k_body, @@ -122,8 +122,8 @@ class VisionImuBackendModule : public BackendModuleType { CHECK_NOTNULL(formulation); // update pose and pose covariance - addInitialVisualState(frame_id_k, formulation, new_values, new_factors, - noise_models, X_k_w); + addInitialVisualState(frame_id_k, timestamp_k, formulation, new_values, + new_factors, noise_models, X_k_w); gtsam::SharedNoiseModel noise_init_vel_prior = gtsam::noiseModel::Isotropic::Sigma(3, 1e-5); @@ -162,16 +162,19 @@ class VisionImuBackendModule : public BackendModuleType { // update internal state information imu_bias_ = imu_bias; - return updateNavState(frame_id_k, X_k_w, V_k_body); + return updateNavState(frame_id_k, timestamp_k, X_k_w, V_k_body); } gtsam::NavState addVisualInertialStates( - FrameId frame_id_k, FormulationType* formulation, + FrameId frame_id_k, Timestamp timestamp_k, FormulationType* formulation, gtsam::Values& new_values, gtsam::NonlinearFactorGraph& new_factors, const NoiseModels& noise_models, const gtsam::Pose3& T_k_1_k, const ImuFrontend::PimPtr& pim = nullptr) { CHECK_GT(frame_id_k, 0); - FrameId frame_id_k_1 = frame_id_k - 1u; + // FrameId frame_id_k_1 = frame_id_k - 1u; + // NOTE: this should becomes last_kf_id + FrameId frame_id_k_1 = last_nav_state_frame_id_; + LOG(INFO) << "Last kf id " << frame_id_k_1; auto from_id = frame_id_k_1; auto to_id = frame_id_k; @@ -180,7 +183,7 @@ class VisionImuBackendModule : public BackendModuleType { // could be from VO or IMU. Do we need to know? const gtsam::NavState predicted_navstate_k = - predictNewState(frame_id_k, T_k_1_k, pim); + predictNewState(frame_id_k, timestamp_k, T_k_1_k, pim); // always add sensor pose formulation->addSensorPoseValue(predicted_navstate_k.pose(), frame_id_k, @@ -239,13 +242,13 @@ class VisionImuBackendModule : public BackendModuleType { return predicted_navstate_k; } - gtsam::NavState predictNewState(FrameId frame_id_k, + gtsam::NavState predictNewState(FrameId frame_id_k, Timestamp timestamp_k, const gtsam::Pose3& T_k_1_k, const ImuFrontend::PimPtr& pim) const { CHECK_GT(frame_id_k, 0); CHECK_EQ(frame_id_k, (last_nav_state_frame_id_ + 1)) << "State frame id's are not incrementally ascending. Are we dropping " - "IMU data per frame?"; + "inertial/VO data per frame?"; gtsam::NavState navstate_k; @@ -274,10 +277,6 @@ class VisionImuBackendModule : public BackendModuleType { // apply relative pose gtsam::Pose3 X_w_k = X_w_k_1 * T_k_1_k; - // calculate dt for a rough body velocity - Timestamp timestamp_k; - CHECK(this->shared_module_info.getTimestamp(frame_id_k, timestamp_k)); - // assume previous timestep is frame frameid - 1 CHECK_GT(timestamp_k, last_nav_state_time_); @@ -291,13 +290,14 @@ class VisionImuBackendModule : public BackendModuleType { } const gtsam::NavState& updateNavStateFromFormulation( - FrameId frame_id_k, const FormulationType* formulation) { - const auto nav_state = navState(frame_id_k, formulation); - return updateNavState(frame_id_k, nav_state); + FrameId frame_id_k, Timestamp timestamp_k, + const FormulationType* formulation) { + const auto nav_state = navState(frame_id_k, timestamp_k, formulation); + return updateNavState(frame_id_k, timestamp_k, nav_state); } // Ideally this should be const but it updated the imu_bias_... why? - gtsam::NavState navState(FrameId frame_id_k, + gtsam::NavState navState(FrameId frame_id_k, Timestamp timestamp_k, const FormulationType* formulation) { auto accessor = CHECK_NOTNULL(formulation)->accessorFromTheta(); const gtsam::Values& values = formulation->getTheta(); @@ -315,9 +315,6 @@ class VisionImuBackendModule : public BackendModuleType { nav_state = gtsam::NavState(X_w_k_query.value(), V_body_k); } else { - Timestamp timestamp_k; - CHECK(this->shared_module_info.getTimestamp(frame_id_k, timestamp_k)); - // assume previous timestep is frame frameid - 1 CHECK_GT(timestamp_k, last_nav_state_time_); @@ -336,21 +333,15 @@ class VisionImuBackendModule : public BackendModuleType { } const gtsam::NavState& updateNavState(FrameId frame_id_k, + Timestamp timestamp_k, const gtsam::Pose3& X_k_w, const gtsam::Vector3& V_k_body) { - return updateNavState(frame_id_k, gtsam::NavState(X_k_w, V_k_body)); + return updateNavState(frame_id_k, timestamp_k, + gtsam::NavState(X_k_w, V_k_body)); } const gtsam::NavState& updateNavState(FrameId frame_id_k, - const gtsam::NavState& nav_state_k) { - Timestamp timestamp_k; - CHECK(this->shared_module_info.getTimestamp(frame_id_k, timestamp_k)); - return updateNavState(timestamp_k, frame_id_k, nav_state_k); - } - - private: - const gtsam::NavState& updateNavState(Timestamp timestamp_k, - FrameId frame_id_k, + Timestamp timestamp_k, const gtsam::NavState& nav_state_k) { nav_state_ = nav_state_k; last_nav_state_time_ = timestamp_k; @@ -364,7 +355,7 @@ class VisionImuBackendModule : public BackendModuleType { // is the previous timestamp (ie. no KF) gtsam::NavState nav_state_; Timestamp last_nav_state_time_; - FrameId last_nav_state_frame_id_; + FrameId last_nav_state_frame_id_{0}; gtsam::imuBias::ConstantBias imu_bias_; private: diff --git a/dynosam/include/dynosam/backend/rgbd/HybridEstimator.hpp b/dynosam/include/dynosam/backend/rgbd/HybridEstimator.hpp index 15f1e0a25..7e61e09fa 100644 --- a/dynosam/include/dynosam/backend/rgbd/HybridEstimator.hpp +++ b/dynosam/include/dynosam/backend/rgbd/HybridEstimator.hpp @@ -1154,8 +1154,8 @@ class SmartMotionFactor : public gtsam::NonlinearFactor, using HybridSmartFactor = SmartMotionFactor<3, gtsam::Pose3, gtsam::Pose3>; struct HybridFormulationProperties { - inline gtsam::Symbol makeDynamicKey(TrackletId tracklet_id) const { - return (gtsam::Symbol)DynamicLandmarkSymbol(0u, tracklet_id); + static inline gtsam::Symbol makeDynamicKey(TrackletId tracklet_id) { + return static_cast(DynamicLandmarkSymbol(0u, tracklet_id)); } }; @@ -1171,13 +1171,106 @@ struct SharedHybridFormulationData { //! Maps an object j with an embedded frame (gtsam::Pose3) for a range of //! timesteps k. Each embedded frame L_e is defined such that the range is e //! to e + N. + //! Pointer to HybridEstimator#key_frame_data_ const KeyFrameData* key_frame_data; //! Tracklet Id to the embedded frame (e) the point is represented in (ie. - //! which timestep, k) + //! which timestep, k). + //! Pointer to HybridEstimator#all_dynamic_landmarks_ const gtsam::FastMap* tracklet_id_to_keyframe; }; -class HybridAccessor : public AccessorT, +/** + * @brief Accesor class that extends the basic functionlity of the Accessor + * with additional getter functions that are specific to the Hybrid formualtion. + * + * This will then form the base class for the HybridAccessor as well as the + * ParallelHybridAccessor. It must inherit from Acessor to allow it to be used + * within AccessorT + * + */ +class HybridAccessorCommon : public Accessor { + public: + DYNO_POINTER_TYPEDEFS(HybridAccessorCommon) + HybridAccessorCommon() = default; + virtual ~HybridAccessorCommon() = default; + + /** + * @brief Get all dynamic object estimates in the local object frame + * + * @param object_id + * @return StatusLandmarkVector + */ + virtual StatusLandmarkVector getLocalDynamicLandmarkEstimates( + ObjectId object_id) const = 0; + + /** + * @brief Collect all tracklet ids associated with a keyframe (ie. constructed + * using L_e where e is the keyframe id). + * + * The frame_id argument is not necessarily a keyframe but is used to find the + * associated keyframe id + * + * + * + * @param frame_id + * @return TrackletIds + */ + virtual TrackletIds collectPointsAtKeyFrame(ObjectId object_id, + FrameId frame_id, + FrameId* keyframe_id) const = 0; + + /** + * @brief Sets a pointer to the keyframe range information for the requested + * object. + * + * The ranges argument is intended to be parsed in as a pointer which is then + * updated to point towards the intended KeyFrameRanges object which is + * managed internally. + * + * We parse by reference-to-pointer to allow the pointer value to be updated. + * + * @param object_id ObjectId + * @param ranges const KeyFrameRanges*& + * @return true + * @return false + */ + virtual bool getObjectKeyFrameHistory( + ObjectId object_id, const KeyFrameRanges*& ranges) const = 0; + + virtual bool hasObjectKeyFrame(ObjectId object_id, + FrameId frame_id) const = 0; + + /** + * @brief Get the the objects keyframe information for the give frame id. + * The frame_id argument is used to look up the keyframe_id that was/is active + * for the frame_id. + * + * @param object_id + * @param frame_id + * @return std::pair + */ + virtual std::pair getObjectKeyFrame( + ObjectId object_id, FrameId frame_id) const = 0; + + /** + * @brief Get the directly estiamted object motion (i.e H_w_e_k) + * + * @param object_id + * @param frame_id + * @return StateQuery + */ + virtual StateQuery getEstimatedMotion( + ObjectId object_id, FrameId frame_id) const = 0; +}; + +/** + * @brief Accessor for the Hybrid Formulation. + * The internal AccessorT is templated on MapVision to define the map type and + * HybridAccessorCommon which extends the functionality of the base Acessor with + * functionality specific to the Hybrid representation. + * + */ +class HybridAccessor : public AccessorT, public HybridFormulationProperties { public: DYNO_POINTER_TYPEDEFS(HybridAccessor) @@ -1185,7 +1278,7 @@ class HybridAccessor : public AccessorT, HybridAccessor( const SharedFormulationData& shared_data, MapVision::Ptr map, const SharedHybridFormulationData& shared_hybrid_formulation_data) - : AccessorT(shared_data, map), + : AccessorT(shared_data, map), shared_hybrid_formulation_data_(shared_hybrid_formulation_data) {} virtual ~HybridAccessor() {} @@ -1206,14 +1299,23 @@ class HybridAccessor : public AccessorT, std::optional getRelativeLocalMotion( FrameId frame_id, ObjectId object_id) const; - /** - * @brief Get all dynamic object estimates in the local object frame - * - * @param object_id - * @return StatusLandmarkVector - */ StatusLandmarkVector getLocalDynamicLandmarkEstimates( - ObjectId object_id) const; + ObjectId object_id) const override; + + TrackletIds collectPointsAtKeyFrame( + ObjectId object_id, FrameId frame_id, + FrameId* keyframe_id = nullptr) const override; + + bool getObjectKeyFrameHistory(ObjectId object_id, + const KeyFrameRanges*& ranges) const override; + + bool hasObjectKeyFrame(ObjectId object_id, FrameId frame_id) const override; + + std::pair getObjectKeyFrame( + ObjectId object_id, FrameId frame_id) const override; + + StateQuery getEstimatedMotion( + ObjectId object_id, FrameId frame_id) const override; private: struct DynamicLandmarkQuery { @@ -1262,8 +1364,7 @@ class HybridFormulation : public Formulation, HybridFormulation(const FormulationParams& params, typename Map::Ptr map, const NoiseModels& noise_models, const Sensors& sensors, - const FormulationHooks& hooks) - : Base(params, map, noise_models, sensors, hooks) {} + const FormulationHooks& hooks); virtual ~HybridFormulation() {} virtual void dynamicPointUpdateCallback( @@ -1281,33 +1382,6 @@ class HybridFormulation : public Formulation, return is_dynamic_tracklet_in_map_.exists(tracklet_id); } - // TODO: functions should be shared with accessor - bool getObjectKeyFrameHistory(ObjectId object_id, - const KeyFrameRanges* ranges) const; - - bool hasObjectKeyFrame(ObjectId object_id, FrameId frame_id) const; - std::pair getObjectKeyFrame(ObjectId object_id, - FrameId frame_id) const; - // get the estimated motion in the representation used directly by the - // estimation (ie. not frame-2-frame) - // TODO: should be in accessor!!!! - StateQuery getEstimatedMotion(ObjectId object_id, - FrameId frame_id) const; - - std::pair forceNewKeyFrame(FrameId frame_id, - ObjectId object_id); - /** - * @brief - * - * frame_id is not necessary a keyframe but will be used to search for the - * keyframe in the range - * - * @param frame_id - * @return TrackletIds - */ - TrackletIds collectPointsAtKeyFrame(ObjectId object_id, - FrameId frame_id) const; - protected: // TODO: make this virtual for now - eventual move structureless etc // properties into a class to encapsulate! @@ -1324,15 +1398,36 @@ class HybridFormulation : public Formulation, virtual std::string loggerPrefix() const override { return "hybrid"; } protected: - std::pair getOrConstructL0(ObjectId object_id, - FrameId frame_id); - - // hacky update solution for now!! - gtsam::Pose3 computeInitialH(ObjectId object_id, FrameId frame_id, - bool* keyframe_updated = nullptr); + // bool addHybridMotionFactor3( + // typename MapTraitsType::FrameNodePtr frame_node, + // typename MapTraitsType::LandmarkNodePtr landmark_node, + // const gtsam::Pose3& L_e, + // const gtsam::Key& camera_pose_key, + // const gtsam::Key& object_motion_key, + // const gtsam::Key& m_key, + // gtsam::NonlinearFactorGraph& graph) const; + + // bool addStereoHybridMotionFactor( + // typename MapTraitsType::FrameNodePtr frame_node, + // typename MapTraitsType::LandmarkNodePtr landmark_node, + // const gtsam::Pose3& L_e, + // const gtsam::Key& camera_pose_key, + // const gtsam::Key& object_motion_key, + // const gtsam::Key& m_key, + // gtsam::NonlinearFactorGraph& graph) const; + + // TODO: this should be KF0_to_k + struct IntermediateMotionInfo { + //! frame id of the object keyframe (i.e. e) + FrameId kf_id; + //! Fixed pose associated with the embedded frame (i.e L_e) + gtsam::Pose3 keyframe_pose; + //! Initial guess of object motion + gtsam::Pose3 H_W_e_k_initial; + }; - gtsam::Pose3 calculateObjectCentroid(ObjectId object_id, - FrameId frame_id) const; + virtual IntermediateMotionInfo getIntermediateMotionInfo( + ObjectId object_id, FrameId frame_id) = 0; // TODO: in the sliding window case the formulation gets reallcoated every // time so that L0 map is different, but the values will share the same H @@ -1369,12 +1464,52 @@ class HybridFormulation : public Formulation, protected: KeyFrameData key_frame_data_; + //! Virtual RGBD camera + RGBDCamera::Ptr rgbd_camera_; }; -// additional functionality when solved with the Regular Backend! -class RegularHybridFormulation : public HybridFormulation { +/** + * @brief The original Hybrid motion implementation (as presented in + * "Online Dynamic SLAM with Incremental Smoothing and Mapping" from RA-L) - + * this is independant from the frontend and ONLY accepts frame-to-frame motion + * (i.e H_W_k_1_k) as input. It constructs object keyframes independantly of the + * front-end and therefore has slightly different embedded poses + * + */ +class HybridFormulationV1 : public HybridFormulation { public: using Base = HybridFormulation; + DYNO_POINTER_TYPEDEFS(HybridFormulationV1) + + HybridFormulationV1(const FormulationParams& params, typename Map::Ptr map, + const NoiseModels& noise_models, const Sensors& sensors, + const FormulationHooks& hooks) + : Base(params, map, noise_models, sensors, hooks) {} + + // SHOULD be in variation of Hybrid that is independant of the front-end + // leave in for now! + std::pair forceNewKeyFrame(FrameId frame_id, + ObjectId object_id); + + protected: + IntermediateMotionInfo getIntermediateMotionInfo(ObjectId object_id, + FrameId frame_id) override; + + std::pair getOrConstructL0(ObjectId object_id, + FrameId frame_id); + + // hacky update solution for now!! + gtsam::Pose3 computeInitialH(ObjectId object_id, FrameId frame_id, + bool* keyframe_updated = nullptr); + + gtsam::Pose3 calculateObjectCentroid(ObjectId object_id, + FrameId frame_id) const; +}; + +// additional functionality when solved with the Regular Backend! +class RegularHybridFormulation : public HybridFormulationV1 { + public: + using Base = HybridFormulationV1; RegularHybridFormulation(const FormulationParams& params, typename Map::Ptr map, diff --git a/dynosam/include/dynosam/dataprovider/DataProvider.hpp b/dynosam/include/dynosam/dataprovider/DataProvider.hpp index 3d1fae26b..030bf991d 100644 --- a/dynosam/include/dynosam/dataprovider/DataProvider.hpp +++ b/dynosam/include/dynosam/dataprovider/DataProvider.hpp @@ -38,6 +38,10 @@ class DataProvider { // this one will not guarnatee a binding of bind the data prover module DataProvider() = default; + /** + * @brief Calls the virtual DataProvider#shutdown() function + * + */ virtual ~DataProvider(); inline void registerImuSingleCallback( diff --git a/dynosam/include/dynosam/factors/HybridFormulationFactors.hpp b/dynosam/include/dynosam/factors/HybridFormulationFactors.hpp index f1cf4bdcf..32da71a01 100644 --- a/dynosam/include/dynosam/factors/HybridFormulationFactors.hpp +++ b/dynosam/include/dynosam/factors/HybridFormulationFactors.hpp @@ -54,10 +54,12 @@ struct HybridObjectMotion { * frame. * @return gtsam::Point3 point in the object frame (m_L). */ - static gtsam::Point3 projectToObject3(const gtsam::Pose3& X_k, - const gtsam::Pose3& e_H_k_world, - const gtsam::Pose3& L_s0, - const gtsam::Point3& Z_k); + static gtsam::Point3 projectToObject3( + const gtsam::Pose3& X_k, const gtsam::Pose3& e_H_k_world, + const gtsam::Pose3& L_s0, const gtsam::Point3& Z_k, + gtsam::OptionalJacobian<3, 6> J1 = boost::none, + gtsam::OptionalJacobian<3, 6> J2 = boost::none, + gtsam::OptionalJacobian<3, 6> J3 = boost::none); /** * @brief Project a point in the object frame to the camera frame (at time k) @@ -72,10 +74,13 @@ struct HybridObjectMotion { * @param m_L gtsam::Point3 point in the object frame (m_L). * @return gtsam::Point3 measured 3D point in the camera frame (z_k). */ - static gtsam::Point3 projectToCamera3(const gtsam::Pose3& X_k, - const gtsam::Pose3& e_H_k_world, - const gtsam::Pose3& L_e, - const gtsam::Point3& m_L); + static gtsam::Point3 projectToCamera3( + const gtsam::Pose3& X_k, const gtsam::Pose3& e_H_k_world, + const gtsam::Pose3& L_e, const gtsam::Point3& m_L, + gtsam::OptionalJacobian<3, 6> J1 = boost::none, + gtsam::OptionalJacobian<3, 6> J2 = boost::none, + gtsam::OptionalJacobian<3, 6> J3 = boost::none, + gtsam::OptionalJacobian<3, 3> J4 = boost::none); /** * @brief Constructs the transform that projects a point/pose in L_e into W. @@ -87,9 +92,11 @@ struct HybridObjectMotion { * @param L_e const gtsam::Pose3& embedded object frame. * @return gtsam::Pose3 */ - static gtsam::Pose3 projectToCamera3Transform(const gtsam::Pose3& X_k, - const gtsam::Pose3& e_H_k_world, - const gtsam::Pose3& L_e); + static gtsam::Pose3 projectToCamera3Transform( + const gtsam::Pose3& X_k, const gtsam::Pose3& e_H_k_world, + const gtsam::Pose3& L_e, gtsam::OptionalJacobian<6, 6> J1 = boost::none, + gtsam::OptionalJacobian<6, 6> J2 = boost::none, + gtsam::OptionalJacobian<6, 6> J3 = boost::none); /** * @brief Residual 3D error for a measured 3D point (z_k) and an estimated @@ -148,6 +155,49 @@ class HybridMotionFactor boost::optional J3 = boost::none) const override; }; +class StereoHybridMotionFactor + : public gtsam::NoiseModelFactor3 { + private: + gtsam::StereoPoint2 measured_; + // fixed reference frame + gtsam::Pose3 L_e_; + gtsam::Cal3_S2Stereo::shared_ptr K_; + + // with identity pose, acts as the refenence frame + gtsam::StereoCamera camera_; + + bool throw_cheirality_; + + public: + using This = StereoHybridMotionFactor; + using Base = + gtsam::NoiseModelFactor3; + + StereoHybridMotionFactor(const gtsam::StereoPoint2& measured, + const gtsam::Pose3& L_e, + const gtsam::SharedNoiseModel& model, + gtsam::Cal3_S2Stereo::shared_ptr K, + gtsam::Key X_k_key, gtsam::Key e_H_k_world_key, + gtsam::Key m_L_key, bool throw_cheirality = false); + + gtsam::NonlinearFactor::shared_ptr clone() const override; + void print(const std::string& s = "", + const gtsam::KeyFormatter& keyFormatter = + DynosamKeyFormatter) const override; + + gtsam::Vector evaluateError( + const gtsam::Pose3& X_k, const gtsam::Pose3& e_H_k_world, + const gtsam::Point3& m_L, + boost::optional J1 = boost::none, + boost::optional J2 = boost::none, + boost::optional J3 = boost::none) const override; + + const gtsam::StereoPoint2& measured() const; + const gtsam::Cal3_S2Stereo::shared_ptr calibration() const; + const gtsam::Pose3& embeddedPose() const; +}; + /** * @brief Implements a 3-way smoothing factor on the (key-framed) object motion. * This is analgous to a constant motion prior and minimises the change in diff --git a/dynosam/include/dynosam/frontend/FrontendModule.hpp b/dynosam/include/dynosam/frontend/FrontendModule.hpp index d1b7b5f01..1cbc140cc 100644 --- a/dynosam/include/dynosam/frontend/FrontendModule.hpp +++ b/dynosam/include/dynosam/frontend/FrontendModule.hpp @@ -30,6 +30,7 @@ #pragma once +#include "dynosam/backend/Accessor.hpp" #include "dynosam/frontend/FrontendParams.hpp" #include "dynosam/frontend/VisionImuOutputPacket.hpp" #include "dynosam/pipeline/PipelineParams.hpp" @@ -38,6 +39,8 @@ #include "dynosam_common/ModuleBase.hpp" #include "dynosam_common/SharedModuleInfo.hpp" #include "dynosam_common/Types.hpp" +#include "dynosam/frontend/vision/FeatureTracker.hpp" + // #include "dynosam_common" @@ -75,6 +78,16 @@ class FrontendModule return params_.frontend_params_; } + void setAccessor(Accessor::Ptr accessor) { + CHECK_NOTNULL(accessor); + accessor_ = accessor; + } + + virtual void onBackendUpdateCallback(const FrameId /*frame_id*/, + const Timestamp /*timestamp*/) {} + + void FrameToClassMap(const Frame::Ptr& frame) const; + protected: /** * @brief Defines the result of checking the image container which is a done @@ -118,6 +131,8 @@ class FrontendModule gtsam::Pose3Vector camera_poses_; //! Keeps track of current camera trajectory. Really just //! for (viz) and drawn everytime + //! Accessor for the backend (i.e optimized values) + Accessor::Ptr accessor_{nullptr}; }; } // namespace dyno diff --git a/dynosam/include/dynosam/frontend/RGBDInstanceFrontendModule.hpp b/dynosam/include/dynosam/frontend/RGBDInstanceFrontendModule.hpp index 2ef25acbd..3545f3584 100644 --- a/dynosam/include/dynosam/frontend/RGBDInstanceFrontendModule.hpp +++ b/dynosam/include/dynosam/frontend/RGBDInstanceFrontendModule.hpp @@ -53,7 +53,8 @@ class RGBDInstanceFrontendModule : public FrontendModule { private: Camera::Ptr camera_; EgoMotionSolver motion_solver_; - ObjectMotionSolver::UniquePtr object_motion_solver_; + // TODO: shared pointer for now during debig phase! + ObjectMotionSolver::Ptr object_motion_solver_; FeatureTracker::UniquePtr tracker_; RGBDFrontendLogger::UniquePtr logger_; @@ -98,21 +99,40 @@ class RGBDInstanceFrontendModule : public FrontendModule { // FLAGS_save_frontend_json flag // std::map output_packet_record_; + //! Imu frontend - mantains pre-integration from last kf to current k ImuFrontend imu_frontend_; - gtsam::NavState nav_state_; + //! Nav state at k + gtsam::NavState nav_state_curr_; // this is always udpated with the best X_k pose but the velocity may be wrong // if no IMU... - gtsam::NavState previous_nav_state_; + //! Nav state at k-1 + gtsam::NavState nav_state_prev_; + + //! Nav state of the last key-frame + gtsam::NavState nav_state_last_kf_; //! Tracks when the nav state was updated using IMU, else VO //! in the front-end, when updating with VO, the velocity will (currently) be //! wrong!! - FrameId last_imu_nav_state_update_{0}; + FrameId last_imu_k_{0}; //! The relative camera pose (T_k_1_k) from the previous frame //! this is used as a constant velocity model when VO tracking fails and the //! IMU is not available! gtsam::Pose3 vo_velocity_; + + // DEBUGGING FOR KF + mutable gtsam::Pose3 T_lkf_; + + //! Last keyframe + Frame::Ptr frame_lkf_; + + // Previous packet + VisionImuPacket::Ptr previous_vision_imu_packet_; + + }; + + } // namespace dyno diff --git a/dynosam/include/dynosam/frontend/VisionImuOutputPacket.hpp b/dynosam/include/dynosam/frontend/VisionImuOutputPacket.hpp index acf4e814a..286b44583 100644 --- a/dynosam/include/dynosam/frontend/VisionImuOutputPacket.hpp +++ b/dynosam/include/dynosam/frontend/VisionImuOutputPacket.hpp @@ -39,6 +39,9 @@ #include "dynosam_common/Types.hpp" #include "dynosam_cv/Camera.hpp" +// only for HybridObjectMotionSRIF +#include "dynosam/frontend/vision/MotionSolver.hpp" + namespace dyno { /** @@ -77,7 +80,7 @@ class VisionImuPacket { }; /** - * @brief Object track information epresenting visual measurements for a + * @brief Object track information representing visual measurements for a * single object as well as frame-to-frame (and possibly other) motion/pose * information. * @@ -96,9 +99,17 @@ class VisionImuPacket { ImuFrontend::PimPtr pim() const; Camera::ConstPtr camera() const; PointCloudLabelRGB::Ptr denseLabelledCloud() const; + gtsam::Vector6 getBodyVelocity() const; + + bool isCameraKeyFrame() const; + bool isObjectKeyFrame(ObjectId object_id) const; + + // static or dynamic!! + bool isKeyFrame() const; const CameraTracks& cameraTracks() const; const gtsam::Pose3& cameraPose() const; + /** * @brief Returns the relative camera motion T_k_1_k, representing the motion * of the camera from k-1 to k in the camera local frame (at k-1) @@ -141,6 +152,7 @@ class VisionImuPacket { VisionImuPacket& groundTruthPacket( const GroundTruthInputPacket::Optional& gt); VisionImuPacket& debugImagery(const DebugImagery::Optional& dbg); + VisionImuPacket& updateBodyVelocity(const VisionImuPacket& prev_state); bool operator==(const VisionImuPacket& other) const { return frame_id_ == other.frame_id_ && timestamp_ == other.timestamp_; @@ -188,6 +200,11 @@ class VisionImuPacket { static void fillLandmarkMeasurements( StatusLandmarkVector& landmarks, const CameraMeasurementStatusVector& camera_measurements); + + + private: + std::optional stored_body_velocity; + }; } // namespace dyno diff --git a/dynosam/include/dynosam/frontend/vision/FeatureTracker.hpp b/dynosam/include/dynosam/frontend/vision/FeatureTracker.hpp index d1d3886c4..7a8ee78b7 100644 --- a/dynosam/include/dynosam/frontend/vision/FeatureTracker.hpp +++ b/dynosam/include/dynosam/frontend/vision/FeatureTracker.hpp @@ -36,10 +36,12 @@ #include "dynosam/frontend/FrontendParams.hpp" #include "dynosam/frontend/vision/FeatureTrackerBase.hpp" #include "dynosam/frontend/vision/Frame.hpp" +#include "dynosam/frontend/FrontendModule.hpp" #include "dynosam/frontend/vision/StaticFeatureTracker.hpp" #include "dynosam/visualizer/Visualizer-Definitions.hpp" #include "dynosam_cv/Camera.hpp" #include "dynosam_cv/Feature.hpp" +#include "dynosam_common/SharedModuleInfo.hpp" // #include "dynosam_common/DynamicObjects.hpp" #include "dynosam_nn/ObjectDetector.hpp" @@ -135,7 +137,7 @@ class FeatureTracker : public FeatureTrackerBase { * @param dynamic_tracking_mask const cv::Mat& */ void requiresSampling( - std::set& objects_to_sample, const FeatureTrackerInfo& info, + std::set& objects_to_sample, FeatureTrackerInfo& info, const ImageContainer& image_container, const gtsam::FastMap& features_per_object, const vision_tools::ObjectBoundaryMaskResult& boundary_mask_result, @@ -146,7 +148,7 @@ class FeatureTracker : public FeatureTrackerBase { private: // TODO: for now we loose the actual object detection result if inference was // run! - bool objectDetection( + ObjectDetectionResult objectDetection( vision_tools::ObjectBoundaryMaskResult& boundary_mask_result, ImageContainer& image_container); diff --git a/dynosam/include/dynosam/frontend/vision/MotionSolver-inl.hpp b/dynosam/include/dynosam/frontend/vision/MotionSolver-inl.hpp index 3a7b98a03..a10fbd855 100644 --- a/dynosam/include/dynosam/frontend/vision/MotionSolver-inl.hpp +++ b/dynosam/include/dynosam/frontend/vision/MotionSolver-inl.hpp @@ -172,8 +172,7 @@ OpticalFlowAndPoseOptimizer::Result OpticalFlowAndPoseOptimizer::optimize( Result result; result.best_result.object_id = *object_id.begin(); - // double error_before = graph.error(values); - std::vector post_errors; + const double error_before = graph.error(values); std::unordered_set outlier_flows; // graph we will mutate by removing outlier factors gtsam::NonlinearFactorGraph mutable_graph = graph; @@ -198,8 +197,6 @@ OpticalFlowAndPoseOptimizer::Result OpticalFlowAndPoseOptimizer::optimize( mutable_graph, optimised_values, opt_params) .optimize(); } - // double error_after = mutable_graph.error(optimised_values); - // post_errors.push_back(error_after); gtsam::FactorIndices outlier_factors; // if we have outliers, enter iteration loop @@ -251,12 +248,12 @@ OpticalFlowAndPoseOptimizer::Result OpticalFlowAndPoseOptimizer::optimize( // size_t initial_size = graph.size(); // size_t inlier_size = mutable_graph.size(); - // error_after = mutable_graph.error(optimised_values); + const double error_after = mutable_graph.error(optimised_values); // recover values result.best_result.refined_pose = optimised_values.at(pose_key); - // result.error_before = error_before; - // result.error_after = error_after; + result.error_before = error_before; + result.error_after = error_after; // for each outlier edge, update the set of inliers for (TrackletId tracklet_id : tracklets) { diff --git a/dynosam/include/dynosam/frontend/vision/MotionSolver.hpp b/dynosam/include/dynosam/frontend/vision/MotionSolver.hpp index 4202d4876..bc8f04bcb 100644 --- a/dynosam/include/dynosam/frontend/vision/MotionSolver.hpp +++ b/dynosam/include/dynosam/frontend/vision/MotionSolver.hpp @@ -30,6 +30,8 @@ #pragma once #include +#include +#include #include #include @@ -345,9 +347,20 @@ class ObjectMotionSolver { using Result = std::pair; - virtual Result solve(Frame::Ptr frame_k, Frame::Ptr frame_k_1) = 0; + Result solve(Frame::Ptr frame_k, Frame::Ptr frame_k_1); protected: + virtual bool solveImpl(Frame::Ptr frame_k, Frame::Ptr frame_k_1, + ObjectId object_id, + MotionEstimateMap& motion_estimates) = 0; + + virtual void updatePoses(ObjectPoseMap& object_poses, + const MotionEstimateMap& motion_estimates, + Frame::Ptr frame_k, Frame::Ptr frame_k_1) = 0; + + virtual void updateMotions(ObjectMotionMap& object_motions, + const MotionEstimateMap& motion_estimates, + Frame::Ptr frame_k, Frame::Ptr frame_k_1) = 0; }; class ObjectMotionSovlerF2F : public ObjectMotionSolver, @@ -376,8 +389,6 @@ class ObjectMotionSovlerF2F : public ObjectMotionSolver, ObjectMotionSovlerF2F(const Params& params, const CameraParams& camera_params); - Result solve(Frame::Ptr frame_k, Frame::Ptr frame_k_1) override; - Motion3SolverResult geometricOutlierRejection3d2d( Frame::Ptr frame_k_1, Frame::Ptr frame_k, const gtsam::Pose3& T_world_k, ObjectId object_id); @@ -386,15 +397,19 @@ class ObjectMotionSovlerF2F : public ObjectMotionSolver, return object_motion_params; } + protected: + virtual bool solveImpl(Frame::Ptr frame_k, Frame::Ptr frame_k_1, + ObjectId object_id, + MotionEstimateMap& motion_estimates) override; + private: - bool solveImpl(Frame::Ptr frame_k, Frame::Ptr frame_k_1, ObjectId object_id, - MotionEstimateMap& motion_estimates); - const ObjectPoseMap& updatePoses(MotionEstimateMap& motion_estimates, - Frame::Ptr frame_k, Frame::Ptr frame_k_1); + void updatePoses(ObjectPoseMap& object_poses, + const MotionEstimateMap& motion_estimates, + Frame::Ptr frame_k, Frame::Ptr frame_k_1) override; - const ObjectMotionMap& updateMotions(MotionEstimateMap& motion_estimates, - Frame::Ptr frame_k, - Frame::Ptr frame_k_1); + void updateMotions(ObjectMotionMap& object_motions, + const MotionEstimateMap& motion_estimates, + Frame::Ptr frame_k, Frame::Ptr frame_k_1) override; private: //! All object poses (from k to K) and updated by updatePoses at each @@ -408,6 +423,312 @@ class ObjectMotionSovlerF2F : public ObjectMotionSolver, const ObjectMotionSovlerF2F::Params object_motion_params; }; +// namespace testing { +// using namespace Eigen; +// using namespace std; +// using namespace gtsam; + +// // Define common matrix types for EKF +// using StateCovariance = gtsam::Matrix66; // P (6x6) +// using PerturbationVector = gtsam::Vector6; // delta_x (6x1) +// using MeasurementCovariance = gtsam::Matrix22; // R (2x2) + +// using MeasurementCovarianceStereo = gtsam::Matrix33; + +// // // Define common matrix types for EKF +// // using StateCovariance = Matrix6d; // P (6x6) +// // using PerturbationVector = Vector6d; // delta_w (6x1) +// // using MeasurementCovariance = Matrix2d; // R (2x2) + +// class ExtendedKalmanFilterGTSAM { +// private: +// Pose3 H_w_; +// StateCovariance P_; // State Covariance Matrix +// Cal3_S2::shared_ptr K_gtsam_; // GTSAM Camera Intrinsic (shared pointer) +// MeasurementCovariance +// R_; // Individual Measurement Noise Covariance (assumed constant) +// StateCovariance Q_; // Process Noise Covariance (for prediction step) + +// public: +// // Constructor initializes state, covariance, and GTSAM camera model +// ExtendedKalmanFilterGTSAM(const Pose3& initial_pose, +// const StateCovariance& initial_P, +// const Matrix3d& K_eigen, +// const MeasurementCovariance& R) +// : H_w_(initial_pose), P_(initial_P), R_(R) { +// // Extract intrinsic parameters from Eigen matrix K +// double fx = K_eigen(0, 0); +// double fy = K_eigen(1, 1); +// double s = K_eigen(0, 1); // Skew is usually zero +// double u0 = K_eigen(0, 2); +// double v0 = K_eigen(1, 2); + +// // Create GTSAM intrinsic object +// K_gtsam_ = boost::make_shared(fx, fy, s, u0, v0); + +// // For simplicity, initialize process noise Q +// Q_ = StateCovariance::Identity() * 1e-4; +// } + +// // EKF Prediction Step (Trivial motion model) +// void predict() { +// // P_k = P_{k-1} + Q +// // H_w_ = H_w_; +// P_ = P_ + Q_; +// // H_w remains unchanged +// cout << "Prediction Step Complete. Covariance inflated by Q." << endl; +// } + +// void update(const vector& P_w_list, const vector& +// z_obs_list, +// const gtsam::Pose3& X_W_k); + +// const Pose3& getPose() const { return H_w_; } +// const StateCovariance& getCovariance() const { return P_; } +// }; + +// } // namespace testing + +/** + * @brief Implements a Square Root Information Filter (SRIF) for 6-DoF pose + * estimation. + * * Instead of propagating a state (W) and covariance (P), this filter + * propagates: + * 1. R_info_ (R): An 6x6 upper-triangular matrix, the Cholesky factor of the + * information matrix (Lambda = R^T * R). + * 2. d_info_ (d): A 6x1 vector, where R^T * d = y (the information vector). + * 3. W_linearization_point_: The nominal state (Pose3) around which the filter + * is linearized. + * + * The state is recovered as a perturbation (delta_w) from this linearization + * point by solving R * delta_w = d. + */ +class SquareRootInfoFilterGTSAM { + private: + // --- SRIF State Variables --- + gtsam::Matrix66 + R_info_; // R (6x6) - Upper triangular Cholesky factor of Info Matrix + gtsam::Vector6 d_info_; // d (6x1) - Transformed information vector + gtsam::Pose3 H_linearization_point_; // Nominal state (linearization point) + + // --- System Parameters --- + gtsam::Cal3_S2::shared_ptr K_gtsam_; // GTSAM Camera Intrinsic + gtsam::Matrix22 R_noise_; // 2x2 Measurement Noise + gtsam::Matrix66 Q_; // Process Noise Covariance (for prediction step) + + public: + SquareRootInfoFilterGTSAM(const gtsam::Pose3& initial_state_H, + const gtsam::Matrix66& initial_P, + const gtsam::Cal3_S2::shared_ptr& K, + const gtsam::Matrix22& R); + + /** + * @brief Recovers the state perturbation delta_w by solving R * delta_w = d. + */ + gtsam::Vector6 getStatePerturbation() const; + + const gtsam::Pose3& getCurrentLinearization() const; + + /** + * @brief Recovers the full state pose W by applying the perturbation + * to the linearization point. + */ + gtsam::Pose3 getStatePoseW() const; + + /** + * @brief Recovers the state covariance P by inverting the information matrix. + * @note This is a slow operation (O(N^3)) and should only be called + * for inspection, not inside the filter loop. + */ + gtsam::Matrix66 getCovariance() const; + + /** + * @brief Recovers the information matrix Lambda = R^T * R. + */ + gtsam::Matrix66 getInformationMatrix() const; + + /** + * @brief EKF Prediction Step (Trivial motion model for W) + * @note Prediction is the hard/slow part of an Information Filter. + * This implementation is a "hack" that converts to covariance, + * adds noise, and converts back. A "pure" SRIF predict is complex. + */ + void predict(); + /** + * @brief SRIF Update Step using Iteratively Reweighted Least Squares (IRLS) + * with QR decomposition to achieve robustness. + */ + void update(const std::vector& P_w_list, + const std::vector& z_obs_list, + const gtsam::Pose3& X_W_k, const int num_irls_iterations = 1); +}; + +/** + * @brief Hybrid Object motion Square-Root Information Filter + * + */ +class HybridObjectMotionSRIF { + public: + struct Result { + double error{0.0}; + double reweighted_error{0.0}; + // gtsam::Pose3 H_W_e_k; + // gtsam::Pose3 H_W_km1_k; + }; + + // FOR TESTING! + public: + // --- SRIF State Variables --- + gtsam::Pose3 H_linearization_point_; // Nominal state (linearization point) + const gtsam::Matrix66 Q_; // Process Noise Covariance (for prediction step) + const gtsam::Matrix33 R_noise_; // 3x3 Measurement Noise + //! Cached R_noise inverse + const gtsam::Matrix33 R_inv_; + const gtsam::Matrix66 initial_P_; + + gtsam::Pose3 L_e_; + // Frame Id for the reference frame e + FrameId frame_id_e_; + //! Last camera pose used within predict + gtsam::Pose3 X_K_; + + gtsam::Matrix66 + R_info_; // R (6x6) - Upper triangular Cholesky factor of Info Matrix + gtsam::Vector6 d_info_; // d (6x1) - Transformed information vector + + // --- System Parameters --- + std::shared_ptr rgbd_camera_; + gtsam::Cal3_S2Stereo::shared_ptr stereo_calibration_; + + //! Points in L (current linearization) + gtsam::FastMap m_linearized_; + + //! should be from e to k-1. Currently set in predict + gtsam::Pose3 previous_H_; + double huber_k_{1.23}; + + constexpr static int StateDim = gtsam::traits::dimension; + constexpr static int ZDim = gtsam::traits::dimension; + + public: + std::vector keyframe_history; + //! Need access to the motion estimate before the filter is resrt + //! AS this is the information of how we get from e -> k, and k is when we + //! reset the filter! + gtsam::Pose3 H_W_e_k_before_reset; + FrameId frame_id_e_before_reset; + + public: + HybridObjectMotionSRIF(const gtsam::Pose3& initial_state_H, + const gtsam::Pose3& L_e, const FrameId& frame_id_e, + const gtsam::Matrix66& initial_P, + const gtsam::Matrix66& Q, const gtsam::Matrix33& R, + Camera::Ptr camera, double huber_k = 1.23); + + inline const gtsam::Pose3& getKeyFramePose() const { return L_e_; } + inline const gtsam::Pose3& lastCameraPose() const { return X_K_; } + inline FrameId getKeyFrameId() const { return frame_id_e_; } + inline const gtsam::FastMap& + getCurrentLinearizedPoints() const { + return m_linearized_; + } + + /** + * @brief Recovers the state perturbation delta_w by solving R * delta_w = d. + */ + gtsam::Vector6 getStatePerturbation() const; + + // this is H_W_e_k + const gtsam::Pose3& getCurrentLinearization() const; + + // this is H_W_e_k + // calculate best estimate!! + gtsam::Pose3 getKeyFramedMotion() const; + + /** + * @brief Recovers the full state pose W by applying the perturbation + * to the linearization point. + * + * LIES: thie is H_W_km1_k + */ + gtsam::Pose3 getF2FMotion() const; + + /** + * @brief Recovers the state covariance P by inverting the information matrix. + * @note This is a slow operation (O(N^3)) and should only be called + * for inspection, not inside the filter loop. + */ + gtsam::Matrix66 getCovariance() const; + + /** + * @brief Recovers the information matrix Lambda = R^T * R. + */ + gtsam::Matrix66 getInformationMatrix() const; + + /** + * @brief EKF Prediction Step (Trivial motion model for W) + * @note Prediction is the hard/slow part of an Information Filter. + * This implementation is a "hack" that converts to covariance, + * adds noise, and converts back. A "pure" SRIF predict is complex. + */ + void predict(const gtsam::Pose3& H_W_km1_k); + /** + * @brief SRIF Update Step using Iteratively Reweighted Least Squares (IRLS) + * with QR decomposition to achieve robustness. + */ + Result update(Frame::Ptr frame, const TrackletIds& tracklets, + const int num_irls_iterations = 1); + + /** + * @brief Resets information d_info_ and R_info. + * d_inifo is set to zero and R_info is constructed from the initial + * covariance P. L_e_ is updated with new value and previous_H_ reset to + * identity + * + * @param L_e + * @param frame_id_e + */ + void resetState(const gtsam::Pose3& L_e, FrameId frame_id_e); +}; + +class ObjectMotionSolverFilter : public ObjectMotionSolver, + protected EgoMotionSolver { + public: + //! Result from solve including the object motions and poses + using ObjectMotionSolver::Result; + + struct Params : public EgoMotionSolver::Params { + // TODO: filter params!! + }; + + ObjectMotionSolverFilter(const Params& params, + const CameraParams& camera_params); + + protected: + bool solveImpl(Frame::Ptr frame_k, Frame::Ptr frame_k_1, ObjectId object_id, + MotionEstimateMap& motion_estimates) override; + + void updatePoses(ObjectPoseMap& object_poses, + const MotionEstimateMap& motion_estimates, + Frame::Ptr frame_k, Frame::Ptr frame_k_1) override; + + void updateMotions(ObjectMotionMap& object_motions, + const MotionEstimateMap& motion_estimates, + Frame::Ptr frame_k, Frame::Ptr frame_k_1) override; + + private: + const Params filter_params_; + //! All object poses (from k to K) and updated by updatePoses at each + //! iteration of sovled + ObjectPoseMap object_poses_; + //! All object motions (from k to K) and updated by updatedMotions at each + //! iteration of sovled + ObjectMotionMap object_motions_; + + public: // TODO: for testing! + gtsam::FastMap> filters_; +}; + void declare_config(OpticalFlowAndPoseOptimizer::Params& config); void declare_config(MotionOnlyRefinementOptimizer::Params& config); diff --git a/dynosam/include/dynosam/frontend/vision/Vision-Definitions.hpp b/dynosam/include/dynosam/frontend/vision/Vision-Definitions.hpp index 98bd9c108..7ceed194f 100644 --- a/dynosam/include/dynosam/frontend/vision/Vision-Definitions.hpp +++ b/dynosam/include/dynosam/frontend/vision/Vision-Definitions.hpp @@ -87,6 +87,8 @@ struct PerObjectStatus { 0}; // number of points tracked from previous frame wehre current label // is the background // would be nice to have some histogram data about each tracked point etc... + bool object_new{false}; + bool object_resampled{false}; PerObjectStatus(ObjectId id) : object_id(id) {} }; @@ -99,6 +101,11 @@ struct FeatureTrackerInfo { size_t static_track_optical_flow{0}; size_t static_track_detections{0}; + //! If new features were extracted at this frame + //! Set in the (static) tracker and partially used to indicate if a new static + //! kf is needed + bool new_static_detections{false}; + inline PerObjectStatus& getObjectStatus(ObjectId object_id) { if (!dynamic_track.exists(object_id)) { dynamic_track.insert2(object_id, PerObjectStatus(object_id)); @@ -117,12 +124,17 @@ inline std::string to_string(const FeatureTrackerInfo& info) { << " - frame id: " << info.frame_id << "\n" << " - timestamp: " << std::setprecision(15) << info.timestamp << "\n" << "\t- # optical flow: " << info.static_track_optical_flow << "\n" - << "\t- # detections: " << info.static_track_detections << "\n"; + << "\t- # detections: " << info.static_track_detections << "\n" + << "\t- # new detections: " << info.new_static_detections << "\n"; for (const auto& [object_id, object_status] : info.dynamic_track) { ss << "\t- Object: " << object_id << ": \n"; ss << "\t\t - num_track " << object_status.num_track << "\n"; ss << "\t\t - num_sampled " << object_status.num_sampled << "\n"; + ss << "\t\t - is new " << std::boolalpha << object_status.object_new + << "\n"; + ss << "\t\t - resampled " << std::boolalpha + << object_status.object_resampled << "\n"; if (VLOG_IS_ON(20)) { ss << "\t\t - num_previous_track " << object_status.num_previous_track diff --git a/dynosam/include/dynosam/pipeline/PipelineBase-inl.hpp b/dynosam/include/dynosam/pipeline/PipelineBase-inl.hpp index 70efe16d3..0a69dc8fd 100644 --- a/dynosam/include/dynosam/pipeline/PipelineBase-inl.hpp +++ b/dynosam/include/dynosam/pipeline/PipelineBase-inl.hpp @@ -44,23 +44,49 @@ PipelineBase::ReturnCode PipelineModule::spinOnce() { return ReturnCode::IS_SHUTDOWN; } + // log get_input, process and get_output packet timing explicitly if VLOG >= + // 10 + static constexpr int intermediate_timing_glog_verbosity = 5; + auto getInputPacketWrapped = [&]() -> InputConstSharedPtr { + // LOG(INFO) << "construct_intermediate_timers_as_stopped " << + // construct_intermediate_timers_as_stopped; + utils::TimingStatsCollector timing(module_name_ + ".get_input", + intermediate_timing_glog_verbosity); + InputConstSharedPtr input = nullptr; + is_thread_working_ = false; + input = getInputPacket(); + is_thread_working_ = true; + return input; + }; + + auto processWrapped = [&](InputConstSharedPtr input) -> OutputConstSharedPtr { + utils::TimingStatsCollector timing(module_name_ + ".process", + intermediate_timing_glog_verbosity); + OutputConstSharedPtr output = nullptr; + output = process(input); + return output; + }; + + auto pushOutputPacketWrapped = [&](OutputConstSharedPtr output) -> bool { + // Received a valid output, send to output queue + utils::TimingStatsCollector timing(module_name_ + ".push_output", + intermediate_timing_glog_verbosity); + const bool push_packet_result = pushOutputPacket(output); + return push_packet_result; + }; + ReturnCode return_code; utils::TimingStatsCollector timing_stats(module_name_); - InputConstSharedPtr input = nullptr; - is_thread_working_ = false; - input = getInputPacket(); - is_thread_working_ = true; + InputConstSharedPtr input = getInputPacketWrapped(); if (input) { // Transfer the ownership of input to the actual pipeline module. // From this point on, you cannot use input, since process owns it. - OutputConstSharedPtr output = nullptr; - output = process(input); + OutputConstSharedPtr output = processWrapped(input); if (output) { - // Received a valid output, send to output queue - if (!pushOutputPacket(output)) { + if (!pushOutputPacketWrapped(output)) { LOG_EVERY_N(WARNING, 100) << "Module: " << module_name_ << " - Output push failed."; is_thread_working_ = false; diff --git a/dynosam/include/dynosam/visualizer/Display.hpp b/dynosam/include/dynosam/visualizer/Display.hpp index a139a5a8d..7d4612002 100644 --- a/dynosam/include/dynosam/visualizer/Display.hpp +++ b/dynosam/include/dynosam/visualizer/Display.hpp @@ -68,8 +68,9 @@ class DisplayBase : public SharedModuleInterface { void spinOnce(const InputConstPtr& input) { CHECK(input); - auto [timestamp, frame_id] = internal::collectTemporalData(*input); - this->shared_module_info.updateTimestampMapping(frame_id, timestamp); + // auto [timestamp, frame_id] = + // internal::collectTemporalData(*input); + // this->shared_module_info.updateTimestampMapping(frame_id, timestamp); spinOnceImpl(input); } diff --git a/dynosam/params/FrontendParams.yaml b/dynosam/params/FrontendParams.yaml index 46ef54652..2b34e5670 100644 --- a/dynosam/params/FrontendParams.yaml +++ b/dynosam/params/FrontendParams.yaml @@ -5,8 +5,8 @@ scene_flow_dist_threshold: 0.5 # # max_object_depth: 30.0 # max_object_depth: 20.0 # max_background_depth: 40.0 -max_background_depth: 100.0 -max_object_depth: 25.0 +max_background_depth: 200.0 +max_object_depth: 30.0 # max_object_depth: 8.0 #indoors @@ -47,7 +47,7 @@ tracker_params: shrink_row: 0 shrink_col: 0 max_nr_keypoints_before_anms: 1000 - max_features_per_frame: 600 + max_features_per_frame: 800 max_feature_track_age: 20 min_features_per_frame: 600 # This should be wide to get a good spread. @@ -59,7 +59,7 @@ tracker_params: # use_clahe_filter: false prefer_provided_optical_flow: false - prefer_provided_object_detection: true + prefer_provided_object_detection: false max_dynamic_features_per_frame: 400 min_distance_btw_tracked_and_detected_dynamic_features: 2 @@ -69,7 +69,9 @@ tracker_params: # dynamic keyframing criteria dynamic_feature_age_buffer: 3 min_dynamic_tracks: 20 - min_dynamic_mask_iou: 0.3 + # causes retracking if current mask IOU with new mask < min_dynamic_mask_iou + # ie. smaller causes less triggering + min_dynamic_mask_iou: 0.1 image_tracks_vis_params: is_debug: true diff --git a/dynosam/params/PipelineParams.yaml b/dynosam/params/PipelineParams.yaml index 6d3dc88f1..1cc4ff1c4 100644 --- a/dynosam/params/PipelineParams.yaml +++ b/dynosam/params/PipelineParams.yaml @@ -1,4 +1,4 @@ -parallel_run: False +parallel_run: True #RGBD instance = 0, MONO instance = 1 (same as FrontendType) diff --git a/dynosam/params/backend.flags b/dynosam/params/backend.flags index 583692c5c..78d11792e 100644 --- a/dynosam/params/backend.flags +++ b/dynosam/params/backend.flags @@ -46,13 +46,16 @@ --use_smoothing_factor=true # 0: Full-batch, 1: sliding-window, 2: incremental ---optimization_mode=0 +--optimization_mode=2 # 0: PTP, 1: Generic Projection, 2: Stereo Projection --static_formulation_type=2 +# if true, only run as Static SLAM (mostly for debug/testing) +--regular_backend_static_only=false + --min_static_observations=2 ---min_dynamic_observations=2 +--min_dynamic_observations=0 # for ISAM2 params using ParallelObjectSAM diff --git a/dynosam/params/frontend.flags b/dynosam/params/frontend.flags index bbb322106..f9615d6c0 100644 --- a/dynosam/params/frontend.flags +++ b/dynosam/params/frontend.flags @@ -3,7 +3,7 @@ --use_byte_tracker=false --init_object_pose_from_gt=false ---use_frontend_logger=true +--use_frontend_logger=false # --semantic_mask_step_size=16 @@ -14,6 +14,10 @@ --use_dynamic_track=false --log_projected_masks=false +--set_dense_labelled_cloud=false +--use_object_motion_filtering=false + +--set_dense_labelled_cloud=false --refine_motion_estimate=true --refine_with_optical_flow=true diff --git a/dynosam/params/pipeline.flags b/dynosam/params/pipeline.flags index 76c33a88a..a7d6a2d1e 100644 --- a/dynosam/params/pipeline.flags +++ b/dynosam/params/pipeline.flags @@ -13,7 +13,7 @@ # if -1, defaults to 0 for starting frame (i.e the first frame) and ending_frame=dataset_size --starting_frame=-1 --ending_frame=-1 ---use_backend=true +--use_backend=false --save_frontend_json=false @@ -22,3 +22,5 @@ # and the bson file is searched for on output_path / .json # where the output file name is determined by the frontend mode (RGB, mono) --frontend_from_file=false + +--use_opencv_display=true diff --git a/dynosam/src/backend/Accessor.cc b/dynosam/src/backend/Accessor.cc index 9cc3a1262..33aa886bf 100644 --- a/dynosam/src/backend/Accessor.cc +++ b/dynosam/src/backend/Accessor.cc @@ -48,4 +48,23 @@ StatusLandmarkVector Accessor::getLandmarkEstimates(FrameId frame_id) const { return estimates; } +StateQuery Accessor::getObjectMotionReferenceFrame( + FrameId frame_id, ObjectId object_id) const { + StateQuery motion_query = this->getObjectMotion(frame_id, object_id); + if (motion_query) { + return StateQuery( + motion_query.key_, + Motion3ReferenceFrame(motion_query.get(), + Motion3ReferenceFrame::Style::F2F, + ReferenceFrame::GLOBAL, frame_id - 1u, frame_id)); + } else { + return StateQuery(motion_query.key_, + motion_query.status_); + } +} + +bool Accessor::exists(gtsam::Key key) const { + return (bool)this->getValueImpl(key); +} + } // namespace dyno diff --git a/dynosam/src/backend/ParallelHybridBackendModule.cc b/dynosam/src/backend/ParallelHybridBackendModule.cc index 137789678..7cb8af129 100644 --- a/dynosam/src/backend/ParallelHybridBackendModule.cc +++ b/dynosam/src/backend/ParallelHybridBackendModule.cc @@ -140,8 +140,10 @@ ParallelHybridBackendModule::ParallelHybridBackendModule( FormulationParams formulation_params(base_params_); formulation_params.updater_suffix = "static"; - static_formulation_ = std::make_shared( + static_formulation_ = std::make_shared( formulation_params, RGBDMap::create(), noise_models_, sensors, hooks); + + combined_accessor_ = std::make_shared(this); } ParallelHybridBackendModule::~ParallelHybridBackendModule() { @@ -168,7 +170,7 @@ ParallelHybridBackendModule::objectEstimators() const { return sam_estimators_; } -HybridFormulation::Ptr ParallelHybridBackendModule::staticEstimator() const { +HybridFormulationV1::Ptr ParallelHybridBackendModule::staticEstimator() const { return static_formulation_; } @@ -176,6 +178,8 @@ ParallelHybridBackendModule::SpinReturn ParallelHybridBackendModule::boostrapSpinImpl(VisionImuPacket::ConstPtr input) { const FrameId frame_k = input->frameId(); const Timestamp timestamp = input->timestamp(); + + updateTrackletMapping(input); // TODO: sovle smoother // non-sequentially? Pose3Measurement optimized_camera_pose = @@ -208,6 +212,8 @@ ParallelHybridBackendModule::nominalSpinImpl(VisionImuPacket::ConstPtr input) { const FrameId frame_k = input->frameId(); const Timestamp timestamp = input->timestamp(); + updateTrackletMapping(input); + const ObjectIds tracked_objects = input->getObjectIds(); const bool has_objects = tracked_objects.size() > 0u; const bool requires_covariance_calc = has_objects; @@ -330,11 +336,16 @@ ParallelHybridBackendModule::getActiveOptimisation() const { return {theta, graph}; } +Accessor::Ptr ParallelHybridBackendModule::getAccessor() { + return combined_accessor_; +}; + Pose3Measurement ParallelHybridBackendModule::bootstrapUpdateStaticEstimator( VisionImuPacket::ConstPtr input) { utils::TimingStatsCollector timer("parallel_object_sam.static_estimator"); const FrameId frame_k = input->frameId(); + const Timestamp timestamp_k = input->timestamp(); auto map = static_formulation_->map(); const auto& X_k_initial = input->cameraPose(); @@ -352,16 +363,16 @@ Pose3Measurement ParallelHybridBackendModule::bootstrapUpdateStaticEstimator( if (input->pim()) { LOG(INFO) << "Initialising backend with IMU states!"; nav_state = this->addInitialVisualInertialState( - frame_k, static_formulation_.get(), new_values, new_factors, - static_formulation_->noiseModels(), + frame_k, timestamp_k, static_formulation_.get(), new_values, + new_factors, static_formulation_->noiseModels(), gtsam::NavState(X_k_initial, gtsam::Vector3(0, 0, 0)), gtsam::imuBias::ConstantBias{}); } else { LOG(INFO) << "Initialising backend with VO only states!"; nav_state = this->addInitialVisualState( - frame_k, static_formulation_.get(), new_values, new_factors, - static_formulation_->noiseModels(), X_k_initial); + frame_k, timestamp_k, static_formulation_.get(), new_values, + new_factors, static_formulation_->noiseModels(), X_k_initial); } // marginalise all values @@ -394,6 +405,8 @@ Pose3Measurement ParallelHybridBackendModule::nominalUpdateStaticEstimator( utils::TimingStatsCollector timer("parallel_object_sam.static_estimator"); const FrameId frame_k = input->frameId(); + const Timestamp timestamp_k = input->timestamp(); + auto map = static_formulation_->map(); map->updateObservations(input->staticMeasurements()); @@ -401,7 +414,7 @@ Pose3Measurement ParallelHybridBackendModule::nominalUpdateStaticEstimator( gtsam::NonlinearFactorGraph new_factors; const gtsam::NavState predicted_nav_state = this->addVisualInertialStates( - frame_k, static_formulation_.get(), new_values, new_factors, + frame_k, timestamp_k, static_formulation_.get(), new_values, new_factors, noise_models_, input->relativeCameraTransform(), input->pim()); // we dont have an uncertainty from the frontend map->updateSensorPoseMeasurement( @@ -452,8 +465,8 @@ Pose3Measurement ParallelHybridBackendModule::nominalUpdateStaticEstimator( gtsam::Values optimised_values = static_estimator_.calculateEstimate(); static_formulation_->updateTheta(optimised_values); - const gtsam::NavState& updated_nav_state = - updateNavStateFromFormulation(frame_k, static_formulation_.get()); + const gtsam::NavState& updated_nav_state = updateNavStateFromFormulation( + frame_k, timestamp_k, static_formulation_.get()); auto accessor = static_formulation_->accessorFromTheta(); StateQuery X_w_k_opt_query = accessor->getSensorPose(frame_k); @@ -734,4 +747,285 @@ void ParallelHybridBackendModule::logGraphs() { } } +void ParallelHybridBackendModule::updateTrackletMapping( + const VisionImuPacket::ConstPtr input) { + for (const auto& static_track : input->staticMeasurements()) { + const TrackletId tracklet_id = static_track.trackletId(); + const ObjectId object_id = static_track.trackletId(); + + tracklet_id_to_object_.insert2(tracklet_id, object_id); + } + + for (const auto& dynamic_track : input->objectMeasurements()) { + const TrackletId tracklet_id = dynamic_track.trackletId(); + const ObjectId object_id = dynamic_track.trackletId(); + + tracklet_id_to_object_.insert2(tracklet_id, object_id); + } +} + +ParallelHybridAccessor::ParallelHybridAccessor( + ParallelHybridBackendModule* parallel_hybrid_module) + : parallel_hybrid_module_(CHECK_NOTNULL(parallel_hybrid_module)) { + auto static_formulation = parallel_hybrid_module_->staticEstimator(); + static_accessor_ = static_formulation->derivedAccessor(); + CHECK_NOTNULL(static_accessor_); +} + +StateQuery ParallelHybridAccessor::getSensorPose( + FrameId frame_id) const { + return static_accessor_->getSensorPose(frame_id); +} + +StateQuery ParallelHybridAccessor::getObjectMotion( + FrameId frame_id, ObjectId object_id) const { + return withOr( + object_id, + [frame_id, object_id](ParallelObjectISAM::Ptr estimator) { + return estimator->accessor()->getObjectMotion(frame_id, object_id); + }, + [frame_id, object_id]() { + return StateQuery::NotInMap( + ObjectMotionSymbol(object_id, frame_id)); + }); +} + +StateQuery ParallelHybridAccessor::getObjectPose( + FrameId frame_id, ObjectId object_id) const { + return withOr( + object_id, + [frame_id, object_id](ParallelObjectISAM::Ptr estimator) { + return estimator->accessor()->getObjectPose(frame_id, object_id); + }, + [frame_id, object_id]() { + return StateQuery::NotInMap( + ObjectPoseSymbol(object_id, frame_id)); + }); +} + +StateQuery ParallelHybridAccessor::getDynamicLandmark( + FrameId frame_id, TrackletId tracklet_id) const { + const auto& tracklet_id_to_object = + parallel_hybrid_module_->tracklet_id_to_object_; + if (tracklet_id_to_object.exists(tracklet_id)) { + const ObjectId object_id = tracklet_id_to_object.at(tracklet_id); + return withOr( + object_id, + [frame_id, tracklet_id](ParallelObjectISAM::Ptr estimator) { + return estimator->accessor()->getDynamicLandmark(frame_id, + tracklet_id); + }, + [tracklet_id]() { + return StateQuery::NotInMap( + HybridAccessor::makeDynamicKey(tracklet_id)); + }); + } else { + return StateQuery::NotInMap( + HybridAccessor::makeDynamicKey(tracklet_id)); + } +} + +StateQuery ParallelHybridAccessor::getStaticLandmark( + TrackletId tracklet_id) const { + return static_accessor_->getStaticLandmark(tracklet_id); +} + +EstimateMap ParallelHybridAccessor::getObjectPoses( + FrameId frame_id) const { + EstimateMap all_poses; + const auto& object_estimators = parallel_hybrid_module_->sam_estimators_; + for (const auto& [object_id, estimator] : object_estimators) { + auto poses = estimator->getObjectPoses(frame_id); + all_poses.insert(poses.begin(), poses.end()); + } + return all_poses; +} + +MotionEstimateMap ParallelHybridAccessor::getObjectMotions( + FrameId frame_id) const { + MotionEstimateMap all_motions; + const auto& object_estimators = parallel_hybrid_module_->sam_estimators_; + for (const auto& [object_id, estimator] : object_estimators) { + auto motions = estimator->accessor()->getObjectMotions(frame_id); + all_motions.insert(motions.begin(), motions.end()); + } + return all_motions; +} + +ObjectPoseMap ParallelHybridAccessor::getObjectPoses() const { + ObjectPoseMap all_poses; + const auto& object_estimators = parallel_hybrid_module_->sam_estimators_; + for (const auto& [object_id, estimator] : object_estimators) { + auto poses = estimator->accessor()->getObjectPoses(); + all_poses.insert(poses.begin(), poses.end()); + } + return all_poses; +} + +ObjectMotionMap ParallelHybridAccessor::getObjectMotions() const { + ObjectMotionMap all_motions; + const auto& object_estimators = parallel_hybrid_module_->sam_estimators_; + for (const auto& [object_id, estimator] : object_estimators) { + auto motions = estimator->accessor()->getObjectMotions(); + all_motions.insert(motions.begin(), motions.end()); + } + return all_motions; +} + +StatusLandmarkVector ParallelHybridAccessor::getDynamicLandmarkEstimates( + FrameId frame_id) const { + StatusLandmarkVector all_landmarks; + const auto& object_estimators = parallel_hybrid_module_->sam_estimators_; + for (const auto& [object_id, estimator] : object_estimators) { + auto landmarks = estimator->getDynamicLandmarks(frame_id); + all_landmarks.insert(all_landmarks.end(), landmarks.begin(), + landmarks.end()); + } + return all_landmarks; +} + +StatusLandmarkVector ParallelHybridAccessor::getDynamicLandmarkEstimates( + FrameId frame_id, ObjectId object_id) const { + return withOr( + object_id, + [frame_id](ParallelObjectISAM::Ptr estimator) { + return estimator->getDynamicLandmarks(frame_id); + }, + []() { return StatusLandmarkVector{}; }); +} + +StatusLandmarkVector ParallelHybridAccessor::getStaticLandmarkEstimates( + FrameId frame_id) const { + return static_accessor_->getStaticLandmarkEstimates(frame_id); +} + +StatusLandmarkVector ParallelHybridAccessor::getFullStaticMap() const { + return static_accessor_->getFullStaticMap(); +} + +StatusLandmarkVector ParallelHybridAccessor::getLocalDynamicLandmarkEstimates( + ObjectId object_id) const { + return withOr( + object_id, + [object_id](ParallelObjectISAM::Ptr estimator) { + return estimator->accessor()->getLocalDynamicLandmarkEstimates( + object_id); + }, + []() { return StatusLandmarkVector{}; }); +} + +TrackletIds ParallelHybridAccessor::collectPointsAtKeyFrame( + ObjectId object_id, FrameId frame_id, FrameId* keyframe_id) const { + return withOr( + object_id, + [object_id, frame_id, keyframe_id](ParallelObjectISAM::Ptr estimator) { + return estimator->accessor()->collectPointsAtKeyFrame( + object_id, frame_id, keyframe_id); + }, + []() { return TrackletIds{}; }); +} + +bool ParallelHybridAccessor::getObjectKeyFrameHistory( + ObjectId object_id, const KeyFrameRanges*& ranges) const { + return withOr( + object_id, + [object_id, &ranges](ParallelObjectISAM::Ptr estimator) { + return estimator->accessor()->getObjectKeyFrameHistory(object_id, + ranges); + }, + []() { return false; }); +} + +bool ParallelHybridAccessor::hasObjectKeyFrame(ObjectId object_id, + FrameId frame_id) const { + return withOr( + object_id, + [frame_id, object_id](ParallelObjectISAM::Ptr estimator) { + return estimator->accessor()->hasObjectKeyFrame(object_id, frame_id); + }, + []() { return false; }); +} + +std::pair ParallelHybridAccessor::getObjectKeyFrame( + ObjectId object_id, FrameId frame_id) const { + return withOr( + object_id, + [frame_id, object_id](ParallelObjectISAM::Ptr estimator) { + return estimator->accessor()->getObjectKeyFrame(object_id, frame_id); + }, + []() { return std::make_pair(FrameId(1), gtsam::Pose3::Identity()); }); +} + +StateQuery ParallelHybridAccessor::getEstimatedMotion( + ObjectId object_id, FrameId frame_id) const { + return withOr( + object_id, + [frame_id, object_id](ParallelObjectISAM::Ptr estimator) { + return estimator->accessor()->getEstimatedMotion(object_id, frame_id); + }, + [object_id, frame_id]() { + return StateQuery::NotInMap( + ObjectMotionSymbol(object_id, frame_id)); + }); +} + +bool ParallelHybridAccessor::hasObjectMotionEstimate(FrameId frame_id, + ObjectId object_id, + Motion3* motion) const { + return withOr( + object_id, + [frame_id, object_id, motion](ParallelObjectISAM::Ptr estimator) { + return estimator->accessor()->hasObjectMotionEstimate( + frame_id, object_id, motion); + }, + []() { return false; }); +} + +bool ParallelHybridAccessor::hasObjectPoseEstimate(FrameId frame_id, + ObjectId object_id, + gtsam::Pose3* pose) const { + return withOr( + object_id, + [frame_id, object_id, pose](ParallelObjectISAM::Ptr estimator) { + return estimator->accessor()->hasObjectPoseEstimate(frame_id, object_id, + pose); + }, + []() { return false; }); +} + +gtsam::FastMap +ParallelHybridAccessor::computeObjectCentroids(FrameId frame_id) const { + gtsam::FastMap centroids; + const auto& object_estimators = parallel_hybrid_module_->sam_estimators_; + for (const auto& [object_id, estimator] : object_estimators) { + const auto centroid_map = + estimator->accessor()->computeObjectCentroids(frame_id); + CHECK(centroid_map.size() == 0u || centroid_map.size() == 1u); + + centroids.insert(centroid_map.begin(), centroid_map.end()); + } + return centroids; +} + +boost::optional ParallelHybridAccessor::getValueImpl( + const gtsam::Key key) const { + // NOTE: returns the value with the requested key as soon as it is found, + // starting with the static accessor + // does not check that it does not exist in subsequent accessors (it should + // not!!) + boost::optional value_opt = + static_accessor_->getValueImpl(key); + if (value_opt) { + return value_opt; + } + const auto& object_estimators = parallel_hybrid_module_->sam_estimators_; + for (const auto& [_, estimator] : object_estimators) { + value_opt = estimator->accessor()->getValueImpl(key); + if (value_opt) { + return value_opt; + } + } + return boost::none; +} + } // namespace dyno diff --git a/dynosam/src/backend/ParallelObjectISAM.cc b/dynosam/src/backend/ParallelObjectISAM.cc index 5d6225ad7..b948d4866 100644 --- a/dynosam/src/backend/ParallelObjectISAM.cc +++ b/dynosam/src/backend/ParallelObjectISAM.cc @@ -56,7 +56,7 @@ ParallelObjectISAM::ParallelObjectISAM( // HACK for now so that we get object motions at every frame!!!? formulation_params.min_dynamic_observations = 2u; - decoupled_formulation_ = std::make_shared( + decoupled_formulation_ = std::make_shared( formulation_params, map_, noise_models, sensors, formulation_hooks); accessor_ = std::dynamic_pointer_cast( decoupled_formulation_->accessorFromTheta()); @@ -82,7 +82,7 @@ StateQuery ParallelObjectISAM::getFrame2FrameMotion( Motion3ReferenceFrame ParallelObjectISAM::getKeyFramedMotion( FrameId frame_id) const { StateQuery e_H_k_world = - decoupled_formulation_->getEstimatedMotion(object_id_, frame_id); + accessor_->getEstimatedMotion(object_id_, frame_id); CHECK(e_H_k_world); CHECK(e_H_k_world->style() == MotionRepresentationStyle::KF); CHECK(e_H_k_world->origin() == ReferenceFrame::GLOBAL); diff --git a/dynosam/src/backend/RegularBackendModule.cc b/dynosam/src/backend/RegularBackendModule.cc index b043a862e..1697a3a6b 100644 --- a/dynosam/src/backend/RegularBackendModule.cc +++ b/dynosam/src/backend/RegularBackendModule.cc @@ -72,6 +72,10 @@ DEFINE_bool( "If ISAM2 stats should be logged to file when running incrementally." " This will slow down compute!!"); +DEFINE_bool( + regular_backend_static_only, false, + "Run as a Static SLAM backend only (i.e ignore dynamic measurements!)"); + namespace dyno { RegularBackendModule::RegularBackendModule( @@ -114,9 +118,10 @@ RegularBackendModule::~RegularBackendModule() { RegularBackendModule::SpinReturn RegularBackendModule::boostrapSpinImpl( VisionImuPacket::ConstPtr input) { const FrameId frame_k = input->frameId(); + // const FrameId kf_id = input->kf_id; const Timestamp timestamp = input->timestamp(); CHECK_EQ(spin_state_.frame_id, frame_k); - LOG(INFO) << "Running backend " << frame_k; + LOG(INFO) << "Running backend kf " << frame_k; gtsam::Values new_values; gtsam::NonlinearFactorGraph new_factors; @@ -124,7 +129,9 @@ RegularBackendModule::SpinReturn RegularBackendModule::boostrapSpinImpl( CHECK(formulation_); + // should this be kf id ot frame id?? PreUpdateData pre_update_data(frame_k); + pre_update_data.input = input; formulation_->preUpdate(pre_update_data); UpdateObservationParams update_params; @@ -133,27 +140,8 @@ RegularBackendModule::SpinReturn RegularBackendModule::boostrapSpinImpl( false; // apparently this is v important for making the results == ICRA PostUpdateData post_update_data(frame_k); - // addMeasurements(update_params, frame_k, new_values, new_factors, - // post_update_data); - { - LOG(INFO) << "Starting updateStaticObservations"; - utils::TimingStatsCollector timer("backend.update_static_obs"); - post_update_data.static_update_result = - formulation_->updateStaticObservations(frame_k, new_values, new_factors, - update_params); - } - // DONT run dynamic updates on the first frame (if any...) - { - LOG(INFO) << "Starting updateDynamicObservations"; - utils::TimingStatsCollector timer("backend.update_dynamic_obs"); - post_update_data.dynamic_update_result = - formulation_->updateDynamicObservations(frame_k, new_values, - new_factors, update_params); - } - - if (post_formulation_update_cb_) { - post_formulation_update_cb_(formulation_, frame_k, new_values, new_factors); - } + addMeasurements(update_params, frame_k, new_values, new_factors, + post_update_data); LOG(INFO) << "Starting any updates"; @@ -168,10 +156,19 @@ RegularBackendModule::SpinReturn RegularBackendModule::boostrapSpinImpl( utils::TimingStatsCollector timer(formulation_->getFullyQualifiedName() + ".post_update"); + LOG(INFO) << "Starting any post updates"; formulation_->postUpdate(post_update_data); + LOG(INFO) << "Done any post updates"; + // use kf id for everything but update the actualy frame id after!! + LOG(INFO) << "Starting any backend output construct"; BackendOutputPacket::Ptr backend_output = constructOutputPacket(frame_k, timestamp); + // dont update the frame_id (yet!) as the visualisation will look for keys + // with with this frame + // however, eventaully will need to log with the original frame_id so that + // the evaluation is consistent!! backend_output->frame_id = frame_k; + LOG(INFO) << "Done any backend output construct"; debug_info_ = DebugInfo(); @@ -190,7 +187,10 @@ RegularBackendModule::SpinReturn RegularBackendModule::nominalSpinImpl( gtsam::NonlinearFactorGraph new_factors; addStates(input, formulation_.get(), new_values, new_factors); - formulation_->preUpdate(PreUpdateData(frame_k)); + + PreUpdateData pre_update_data(frame_k); + pre_update_data.input = input; + formulation_->preUpdate(pre_update_data); UpdateObservationParams update_params; update_params.enable_debug_info = true; @@ -210,7 +210,7 @@ RegularBackendModule::SpinReturn RegularBackendModule::nominalSpinImpl( // update internal nav state based on the initial/optimised estimated in the // formulation this is also necessary to update the internal timestamp/frameid // variables within the VisionImuBackendModule - updateNavStateFromFormulation(frame_k, formulation_.get()); + updateNavStateFromFormulation(frame_k, timestamp, formulation_.get()); // TODO: sanity checks that vision states are inline with the other frame idss // etc @@ -267,11 +267,13 @@ void RegularBackendModule::addMeasurements( } { - LOG(INFO) << "Starting updateDynamicObservations"; - utils::TimingStatsCollector timer("backend.update_dynamic_obs"); - post_update_data.dynamic_update_result = - formulation_->updateDynamicObservations(frame_k, new_values, - new_factors, update_params); + if (!FLAGS_regular_backend_static_only) { + LOG(INFO) << "Starting updateDynamicObservations"; + utils::TimingStatsCollector timer("backend.update_dynamic_obs"); + post_update_data.dynamic_update_result = + formulation_->updateDynamicObservations(frame_k, new_values, + new_factors, update_params); + } } if (post_formulation_update_cb_) { @@ -301,6 +303,10 @@ RegularBackendModule::getActiveOptimisation() const { } } +Accessor::Ptr RegularBackendModule::getAccessor() { + return formulation_->accessorFromTheta(); +} + void RegularBackendModule::updateAndOptimize( FrameId frame_id_k, const gtsam::Values& new_values, const gtsam::NonlinearFactorGraph& new_factors, @@ -317,6 +323,8 @@ void RegularBackendModule::updateAndOptimize( } else { LOG(FATAL) << "Unknown optimisation mode" << optimization_mode; } + + if (frontend_update_callback_) frontend_update_callback_(frame_id_k, 0); } void RegularBackendModule::updateIncremental( @@ -523,7 +531,7 @@ void RegularBackendModule::addInitialStates( CHECK(formulation); const FrameId frame_k = input->frameId(); - const Timestamp timestamp = input->timestamp(); + const Timestamp timestamp_k = input->timestamp(); const auto& X_k_initial = input->cameraPose(); // update map @@ -533,14 +541,14 @@ void RegularBackendModule::addInitialStates( if (input->pim()) { LOG(INFO) << "Initialising backend with IMU states!"; this->addInitialVisualInertialState( - frame_k, formulation, new_values, new_factors, noise_models_, - gtsam::NavState(X_k_initial, gtsam::Vector3(0, 0, 0)), + frame_k, timestamp_k, formulation, new_values, new_factors, + noise_models_, gtsam::NavState(X_k_initial, gtsam::Vector3(0, 0, 0)), gtsam::imuBias::ConstantBias{}); } else { LOG(INFO) << "Initialising backend with VO only states!"; - this->addInitialVisualState(frame_k, formulation, new_values, new_factors, - noise_models_, X_k_initial); + this->addInitialVisualState(frame_k, timestamp_k, formulation, new_values, + new_factors, noise_models_, X_k_initial); } LOG(INFO) << "Done!"; @@ -552,9 +560,10 @@ void RegularBackendModule::addStates(const VisionImuPacket::ConstPtr& input, CHECK(formulation); const FrameId frame_k = input->frameId(); + const Timestamp timestamp_k = input->timestamp(); const gtsam::NavState predicted_nav_state = this->addVisualInertialStates( - frame_k, formulation, new_values, new_factors, noise_models_, + frame_k, timestamp_k, formulation, new_values, new_factors, noise_models_, input->relativeCameraTransform(), input->pim()); updateMapWithMeasurements(frame_k, input, predicted_nav_state.pose()); @@ -564,6 +573,7 @@ void RegularBackendModule::updateMapWithMeasurements( FrameId frame_id_k, const VisionImuPacket::ConstPtr& input, const gtsam::Pose3& X_k_w) { CHECK_EQ(frame_id_k, input->frameId()); + // CHECK_EQ(frame_id_k, input->kf_id); // update static and ego motion map_->updateObservations(input->staticMeasurements()); @@ -648,7 +658,7 @@ void RegularBackendModule::setFormulation( CHECK_NOTNULL(formulation_); // add additional error handling for incremental based on formulation auto* hybrid_formulation = - static_cast(formulation_.get()); + static_cast(formulation_.get()); if (hybrid_formulation) { LOG(INFO) << "Adding additional error hooks for Hybrid formulation"; error_hooks.handle_failed_object = @@ -688,6 +698,8 @@ BackendOutputPacket::Ptr RegularBackendModule::constructOutputPacket( FrameId frame_k, Timestamp timestamp) const { auto accessor = formulation_->accessorFromTheta(); + LOG(INFO) << "Making output packet with kf id=" << frame_k; + auto backend_output = std::make_shared(); backend_output->timestamp = timestamp; backend_output->frame_id = frame_k; @@ -695,15 +707,20 @@ BackendOutputPacket::Ptr RegularBackendModule::constructOutputPacket( backend_output->static_landmarks = accessor->getFullStaticMap(); // backend_output->optimized_object_motions = // accessor->getObjectMotions(frame_k); + + auto map = formulation_->map(); + LOG(INFO) << "Map frame ids " << container_to_string(map->getFrameIds()); + backend_output->dynamic_landmarks = accessor->getDynamicLandmarkEstimates(frame_k); - auto map = formulation_->map(); + // auto map = formulation_->map(); for (FrameId frame_id : map->getFrameIds()) { backend_output->optimized_camera_poses.push_back( accessor->getSensorPose(frame_id).get()); } // fill temporal map information + LOG(INFO) << "Object ids " << container_to_string(map->getObjectIds()); for (ObjectId object_id : map->getObjectIds()) { const auto& object_node = map->getObject(object_id); CHECK_NOTNULL(object_node); diff --git a/dynosam/src/backend/rgbd/HybridEstimator.cc b/dynosam/src/backend/rgbd/HybridEstimator.cc index 36cf9b0b8..5e3a0a303 100644 --- a/dynosam/src/backend/rgbd/HybridEstimator.cc +++ b/dynosam/src/backend/rgbd/HybridEstimator.cc @@ -147,8 +147,10 @@ StateQuery HybridAccessor::getSensorPose(FrameId frame_id) const { StateQuery HybridAccessor::getObjectMotion( FrameId frame_id, ObjectId object_id) const { + const auto object_node = map()->getObject(object_id); const auto frame_node_k = map()->getFrame(frame_id); - const auto frame_node_k_1 = map()->getFrame(frame_id - 1u); + CHECK(object_node); + // const auto frame_node_k_1 = map()->getFrame(frame_id - 1u); if (!frame_node_k) { VLOG(30) << "Could not construct object motion frame id=" << frame_id @@ -168,17 +170,30 @@ StateQuery HybridAccessor::getObjectMotion( // first object motion (ie s0 -> s1) auto key_frame_data = CHECK_NOTNULL(shared_hybrid_formulation_data_.key_frame_data); - if (!frame_node_k_1) { - CHECK_NOTNULL(frame_node_k); - // FrameId s0 = L0_values_->at(object_id).first; + + FrameId last_seen; + if (!object_node->previouslySeenFrame(&last_seen)) { const auto range = CHECK_NOTNULL(key_frame_data->find(object_id, frame_id)); const auto [s0, L0] = range->dataPair(); // check that the first frame of the object motion is actually this frame // this motion should actually be identity CHECK_EQ(s0, frame_id); return StateQuery(motion_key, *e_H_k_world); - } else { + } + // if (!frame_node_k_1) { + // CHECK_NOTNULL(frame_node_k); + // // const auto range = CHECK_NOTNULL(key_frame_data->find(object_id, + // frame_id)); + // // const auto [s0, L0] = range->dataPair(); + // // // check that the first frame of the object motion is actually this + // frame + // // // this motion should actually be identity + // // CHECK_EQ(s0, frame_id); + // // return StateQuery(motion_key, *e_H_k_world); + // } + else { CHECK_NOTNULL(frame_node_k); + const auto frame_node_k_1 = map()->getFrame(last_seen); CHECK_NOTNULL(frame_node_k_1); StateQuery e_H_km1_world = this->query( @@ -313,6 +328,10 @@ StatusLandmarkVector HybridAccessor::getLocalDynamicLandmarkEstimates( } } + VLOG(40) << "Collecting points for j=" << object_id + << " kf with max tracks KF=" << kf_with_max_tracks + << " count=" << max_count; + StatusLandmarkVector estimates; if (max_count == 0) { return estimates; @@ -337,6 +356,81 @@ StatusLandmarkVector HybridAccessor::getLocalDynamicLandmarkEstimates( return estimates; } +TrackletIds HybridAccessor::collectPointsAtKeyFrame( + ObjectId object_id, FrameId frame_id, FrameId* keyframe_id) const { + if (!hasObjectKeyFrame(object_id, frame_id)) { + return {}; + } + + TrackletIds tracklets; + const auto& all_dynamic_landmarks = + *shared_hybrid_formulation_data_.tracklet_id_to_keyframe; + const auto [keyframe_k, _] = getObjectKeyFrame(object_id, frame_id); + for (const auto& [tracklet_id, tracklet_keyframe] : all_dynamic_landmarks) { + if (tracklet_keyframe == keyframe_k) { + tracklets.push_back(tracklet_id); + } + } + + if (keyframe_id) { + *keyframe_id = keyframe_k; + } + + return tracklets; +} + +bool HybridAccessor::getObjectKeyFrameHistory( + ObjectId object_id, const KeyFrameRanges*& ranges) const { + // CHECK_NOTNULL(ranges); + const auto& key_frame_data = shared_hybrid_formulation_data_.key_frame_data; + if (!key_frame_data->exists(object_id)) { + return false; + } + + ranges = &key_frame_data->at(object_id); + return true; +} + +bool HybridAccessor::hasObjectKeyFrame(ObjectId object_id, + FrameId frame_id) const { + const auto& key_frame_data = shared_hybrid_formulation_data_.key_frame_data; + return static_cast(key_frame_data->find(object_id, frame_id)); +} + +std::pair HybridAccessor::getObjectKeyFrame( + ObjectId object_id, FrameId frame_id) const { + const auto& key_frame_data = shared_hybrid_formulation_data_.key_frame_data; + const KeyFrameRange::ConstPtr range = + key_frame_data->find(object_id, frame_id); + CHECK_NOTNULL(range); + return range->dataPair(); +} + +StateQuery HybridAccessor::getEstimatedMotion( + ObjectId object_id, FrameId frame_id) const { + // not in form of accessor but in form of estimation + const auto frame_node_k = map()->getFrame(frame_id); + CHECK_NOTNULL(frame_node_k); + + auto motion_key = frame_node_k->makeObjectMotionKey(object_id); + StateQuery e_H_k_world = + this->template query(motion_key); + + if (!e_H_k_world) { + return StateQuery(e_H_k_world.key(), + e_H_k_world.status()); + } + + CHECK(this->hasObjectKeyFrame(object_id, frame_id)); + // s0 + auto [reference_frame, _] = this->getObjectKeyFrame(object_id, frame_id); + + Motion3ReferenceFrame motion(e_H_k_world.get(), MotionRepresentationStyle::KF, + ReferenceFrame::GLOBAL, reference_frame, + frame_id); + return StateQuery(e_H_k_world.key(), motion); +} + std::optional HybridAccessor::getRelativeLocalMotion( FrameId frame_id, ObjectId object_id) const { const auto from = frame_id - 1u; @@ -388,6 +482,8 @@ bool HybridAccessor::getDynamicLandmarkImpl(FrameId frame_id, FrameId point_embedded_frame = tracklet_id_to_keyframe->at(tracklet_id); const auto range = key_frame_data->find(object_id, frame_id); + // TODO: check the &= is right! + // we might mean result = result || (condition) bool result = true; // update intermediate queries @@ -462,110 +558,16 @@ bool HybridAccessor::getDynamicLandmarkImpl( return getDynamicLandmarkImpl(frame_id, tracklet_id, query); } -// this needs to happen (mostly) before factor graph construction to take -// effect!! -std::pair HybridFormulation::forceNewKeyFrame( - FrameId frame_id, ObjectId object_id) { - LOG(INFO) << "Starting new range of object k=" << frame_id - << " j=" << object_id; - gtsam::Pose3 center = calculateObjectCentroid(object_id, frame_id); - - auto result = - key_frame_data_.startNewActiveRange(object_id, frame_id, center) - ->dataPair(); - - // clear meta-data to start new tracklets - // TODO: somehow adding this back in causes a segfault when ISAM2::update step - // happens... in combinating with clearing the internal graph - // (this->clearGraph) and resetting the smoother in ParlallelObjectSAM. I - // think we should do this!! - // HACK - these vairblaes will still be in the values and therefore we will - // get some kind of 'gtsam::ValuesKeyAlreadyExists' when updating the - // formulation we therefore need to remove these from the theta - this will - // remove old data - // from the accessor (even though we keep track of the meta-data) - // when the frontend is updated to include keyframe information this should - // not be an issue as the frontend will ensure new measurements dont refer to - // landmarks in old keypoints - // for (const auto& [tracklet_id, _] : is_dynamic_tracklet_in_map_) { - // ObjectId lmk_object_id; - // CHECK(map()->getLandmarkObjectId(lmk_object_id, tracklet_id)); - // //only delete for requested object - // if(lmk_object_id == object_id) { - // theta_.erase(this->makeDynamicKey(tracklet_id)); - // } - // } - // is_dynamic_tracklet_in_map_.clear(); - - // sanity check - CHECK_EQ(result.first, frame_id); - return result; -} - -bool HybridFormulation::getObjectKeyFrameHistory( - ObjectId object_id, const KeyFrameRanges* ranges) const { - CHECK_NOTNULL(ranges); - if (!key_frame_data_.exists(object_id)) { - return false; - } - - ranges = &key_frame_data_.at(object_id); - return true; -} - -bool HybridFormulation::hasObjectKeyFrame(ObjectId object_id, - FrameId frame_id) const { - return static_cast(key_frame_data_.find(object_id, frame_id)); -} -std::pair HybridFormulation::getObjectKeyFrame( - ObjectId object_id, FrameId frame_id) const { - const KeyFrameRange::ConstPtr range = - key_frame_data_.find(object_id, frame_id); - CHECK_NOTNULL(range); - return range->dataPair(); -} - -StateQuery HybridFormulation::getEstimatedMotion( - ObjectId object_id, FrameId frame_id) const { - // not in form of accessor but in form of estimation - const auto frame_node_k = map()->getFrame(frame_id); - CHECK_NOTNULL(frame_node_k); - - auto motion_key = frame_node_k->makeObjectMotionKey(object_id); - // raw access the theta in the accessor!! - auto theta_accessor = this->accessorFromTheta(); - StateQuery e_H_k_world = - theta_accessor->query(motion_key); - - if (!e_H_k_world) { - return StateQuery(e_H_k_world.key(), - e_H_k_world.status()); - } - - CHECK(this->hasObjectKeyFrame(object_id, frame_id)); - // s0 - auto [reference_frame, _] = this->getObjectKeyFrame(object_id, frame_id); - - Motion3ReferenceFrame motion(e_H_k_world.get(), MotionRepresentationStyle::KF, - ReferenceFrame::GLOBAL, reference_frame, - frame_id); - return StateQuery(e_H_k_world.key(), motion); -} - -TrackletIds HybridFormulation::collectPointsAtKeyFrame(ObjectId object_id, - FrameId frame_id) const { - if (!hasObjectKeyFrame(object_id, frame_id)) { - return {}; - } - - TrackletIds tracklets; - const auto [keyframe_k, _] = getObjectKeyFrame(object_id, frame_id); - for (const auto& [tracklet_id, tracklet_keyframe] : all_dynamic_landmarks_) { - if (tracklet_keyframe == keyframe_k) { - tracklets.push_back(tracklet_id); - } - } - return tracklets; +HybridFormulation::HybridFormulation(const FormulationParams& params, + typename Map::Ptr map, + const NoiseModels& noise_models, + const Sensors& sensors, + const FormulationHooks& hooks) + : Base(params, map, noise_models, sensors, hooks) { + auto camera = sensors_.camera; + CHECK_NOTNULL(camera); + rgbd_camera_ = camera->safeGetRGBDCamera(); + CHECK_NOTNULL(rgbd_camera_); } void HybridFormulation::dynamicPointUpdateCallback( @@ -574,20 +576,28 @@ void HybridFormulation::dynamicPointUpdateCallback( const auto lmk_node = context.lmk_node; const auto frame_node_k_1 = context.frame_node_k_1; const auto frame_node_k = context.frame_node_k; + const auto object_id = context.getObjectId(); + const auto frame_id_k_1 = frame_node_k_1->getId(); auto theta_accessor = this->accessorFromTheta(); gtsam::Key point_key = this->makeDynamicKey(context.getTrackletId()); const gtsam::Key object_motion_key_k = - frame_node_k->makeObjectMotionKey(context.getObjectId()); + frame_node_k->makeObjectMotionKey(object_id); const gtsam::Key object_motion_key_k_1 = - frame_node_k_1->makeObjectMotionKey(context.getObjectId()); - - gtsam::Pose3 L_e; - FrameId s0; - std::tie(s0, L_e) = - getOrConstructL0(context.getObjectId(), frame_node_k_1->getId()); + frame_node_k_1->makeObjectMotionKey(object_id); + + // gtsam::Pose3 L_e; + const IntermediateMotionInfo keyframe_info = + getIntermediateMotionInfo(object_id, frame_id_k_1); + + const FrameId& s0 = keyframe_info.kf_id; + const gtsam::Pose3& L_e = keyframe_info.keyframe_pose; + const gtsam::Pose3& H_W_e_k_initial = keyframe_info.H_W_e_k_initial; + // FrameId s0; + // std::tie(s0, L_e) = + // getOrConstructL0(context.getObjectId(), frame_node_k_1->getId()); auto landmark_motion_noise = noise_models_.landmark_motion_noise; // check that the first frame id is at least the initial frame for s0 @@ -605,20 +615,20 @@ void HybridFormulation::dynamicPointUpdateCallback( // use first point as initalisation? // in this case k is k-1 as we use frame_node_k_1 - bool keyframe_updated; - gtsam::Pose3 e_H_k_world = computeInitialH( - context.getObjectId(), frame_node_k_1->getId(), &keyframe_updated); + // bool keyframe_updated; + // gtsam::Pose3 e_H_k_world = computeInitialH( + // context.getObjectId(), frame_node_k_1->getId(), &keyframe_updated); // TODO: we should never actually let this happen during an update // it should only happen before measurements are added // want to avoid somehow a situation where some (landmark)variables are at // an old keyframe I dont think this will happen with the current // implementation... - if (keyframe_updated) { - // TODO: gross I have to re-get them again!! - std::tie(s0, L_e) = - getOrConstructL0(context.getObjectId(), frame_node_k_1->getId()); - } + // if (keyframe_updated) { + // // TODO: gross I have to re-get them again!! + // std::tie(s0, L_e) = + // getOrConstructL0(context.getObjectId(), frame_node_k_1->getId()); + // } // mark as now in map and include associated frame!!s is_dynamic_tracklet_in_map_.insert2(context.getTrackletId(), s0); @@ -635,7 +645,7 @@ void HybridFormulation::dynamicPointUpdateCallback( // Landmark lmk_L0_init = // L_e.inverse() * k_H_s0_W * context.X_k_1_measured * m_camera; Landmark lmk_L0_init = HybridObjectMotion::projectToObject3( - context.X_k_1_measured, e_H_k_world, L_e, + context.X_k_1_measured, H_W_e_k_initial, L_e, MeasurementTraits::point(lmk_node->getMeasurement(frame_node_k_1))); // TODO: this should not every be true as this is a new value!!! @@ -710,13 +720,16 @@ void HybridFormulation::objectUpdateContext( const auto frame_id = context.getFrameId(); const auto object_id = context.getObjectId(); + const IntermediateMotionInfo keyframe_info = + getIntermediateMotionInfo(object_id, frame_id); + if (!is_other_values_in_map.exists(object_motion_key_k)) { // gtsam::Pose3 motion; const gtsam::Pose3 X_world = getInitialOrLinearizedSensorPose(frame_id); - gtsam::Pose3 motion = computeInitialH(object_id, frame_id); + // gtsam::Pose3 motion = computeInitialH(object_id, frame_id); VLOG(5) << "Added motion at " << DynosamKeyFormatter(object_motion_key_k); // gtsam::Pose3 motion; - new_values.insert(object_motion_key_k, motion); + new_values.insert(object_motion_key_k, keyframe_info.H_W_e_k_initial); is_other_values_in_map.insert2(object_motion_key_k, true); // for now lets treat num_motion_factors as motion (values) added!! @@ -724,8 +737,9 @@ void HybridFormulation::objectUpdateContext( result.debug_info->getObjectInfo(context.getObjectId()) .num_motion_factors++; - const auto [s0, L0] = getOrConstructL0(object_id, frame_id); - if (s0 == frame_id) { + // we are at object keyframe + // NOTE: this should never happen for hybrid KF!! + if (keyframe_info.kf_id == frame_id) { // add prior new_factors.addPrior(object_motion_key_k, gtsam::Pose3::Identity(), @@ -735,7 +749,7 @@ void HybridFormulation::objectUpdateContext( // test stuff FrameId first_seen_object_frame = object_node->getFirstSeenFrame(); if (first_seen_object_frame == frame_id) { - CHECK_EQ(s0, frame_id); + CHECK_EQ(keyframe_info.kf_id, frame_id); } } @@ -785,7 +799,7 @@ void HybridFormulation::objectUpdateContext( is_other_values_in_map.exists(object_motion_key_k)) { new_factors.emplace_shared( object_motion_key_k_2, object_motion_key_k_1, object_motion_key_k, - getOrConstructL0(object_id, frame_id).second, object_smoothing_noise); + keyframe_info.keyframe_pose, object_smoothing_noise); if (result.debug_info) result.debug_info->getObjectInfo(context.getObjectId()) .smoothing_factor_added = true; @@ -796,7 +810,113 @@ void HybridFormulation::objectUpdateContext( } } -std::pair HybridFormulation::getOrConstructL0( +// bool HybridFormulation::addHybridMotionFactor3( +// typename MapTraitsType::FrameNodePtr frame_node, +// typename MapTraitsType::LandmarkNodePtr landmark_node, +// const gtsam::Pose3& L_e, +// const gtsam::Key& camera_pose_key, +// const gtsam::Key& object_motion_key, +// const gtsam::Key& m_key, +// gtsam::NonlinearFactorGraph& graph) const +// { +// const TrackletId& tracklet_id = landmark_node.tracklet_id; +// const ObjectId& object_id = landmark_node.object_id; +// CHECK_EQ(camera_pose_key, frame_node->makePoseKey()); +// CHECK_EQ(object_motion_key, frame_node->makeObjectMotionKey(object_id)); +// CHECK_EQ(m_key, this->makeDynamicKey(tracklet_id)); + +// auto [measured_point_camera, measurement_covariance] = +// MeasurementTraits::pointWithCovariance( +// lmk_node->getMeasurement(frame_node)); + +// if (params_.makeDynamicMeasurementsRobust()) { +// measurement_covariance = factor_graph_tools::robustifyHuber( +// params_.k_huber_3d_points_, measurement_covariance); +// } + +// graph.emplace_shared( +// camera_pose_key, +// object_motion_key, +// m_key, +// measured_point_camera, +// L_e, +// measurement_covariance); + +// return true; + +// } + +// bool HybridFormulation::addStereoHybridMotionFactor( +// typename MapTraitsType::FrameNodePtr frame_node, +// typename MapTraitsType::LandmarkNodePtr landmark_node, +// const gtsam::Pose3& L_e, +// const gtsam::Key& camera_pose_key, +// const gtsam::Key& object_motion_key, +// const gtsam::Key& m_key, +// gtsam::NonlinearFactorGraph& graph) const +// { +// const TrackletId& tracklet_id = landmark_node.tracklet_id; +// const ObjectId& object_id = landmark_node.object_id; +// CHECK_EQ(camera_pose_key, frame_node->makePoseKey()); +// CHECK_EQ(object_motion_key, frame_node->makeObjectMotionKey(object_id)); +// CHECK_EQ(m_key, this->makeDynamicKey(tracklet_id)); +// } + +// this needs to happen (mostly) before factor graph construction to take +// effect!! +std::pair HybridFormulationV1::forceNewKeyFrame( + FrameId frame_id, ObjectId object_id) { + LOG(INFO) << "Starting new range of object k=" << frame_id + << " j=" << object_id; + gtsam::Pose3 center = calculateObjectCentroid(object_id, frame_id); + + auto result = + key_frame_data_.startNewActiveRange(object_id, frame_id, center) + ->dataPair(); + + // clear meta-data to start new tracklets + // TODO: somehow adding this back in causes a segfault when ISAM2::update step + // happens... in combinating with clearing the internal graph + // (this->clearGraph) and resetting the smoother in ParlallelObjectSAM. I + // think we should do this!! + // HACK - these vairblaes will still be in the values and therefore we will + // get some kind of 'gtsam::ValuesKeyAlreadyExists' when updating the + // formulation we therefore need to remove these from the theta - this will + // remove old data + // from the accessor (even though we keep track of the meta-data) + // when the frontend is updated to include keyframe information this should + // not be an issue as the frontend will ensure new measurements dont refer to + // landmarks in old keypoints + // for (const auto& [tracklet_id, _] : is_dynamic_tracklet_in_map_) { + // ObjectId lmk_object_id; + // CHECK(map()->getLandmarkObjectId(lmk_object_id, tracklet_id)); + // //only delete for requested object + // if(lmk_object_id == object_id) { + // theta_.erase(this->makeDynamicKey(tracklet_id)); + // } + // } + // is_dynamic_tracklet_in_map_.clear(); + + // sanity check + CHECK_EQ(result.first, frame_id); + return result; +} + +HybridFormulation::IntermediateMotionInfo +HybridFormulationV1::getIntermediateMotionInfo(ObjectId object_id, + FrameId frame_id) { + IntermediateMotionInfo info; + bool keyframe_updated; + info.H_W_e_k_initial = + computeInitialH(object_id, frame_id, &keyframe_updated); + (void)keyframe_updated; + + std::tie(info.kf_id, info.keyframe_pose) = + getOrConstructL0(object_id, frame_id); + return info; +} + +std::pair HybridFormulationV1::getOrConstructL0( ObjectId object_id, FrameId frame_id) { const KeyFrameRange::ConstPtr range = key_frame_data_.find(object_id, frame_id); @@ -811,9 +931,9 @@ std::pair HybridFormulation::getOrConstructL0( // should also check if the last object motion from the estimation can be used // as the last motion // so only one composition is needed to get the latest motion -gtsam::Pose3 HybridFormulation::computeInitialH(ObjectId object_id, - FrameId frame_id, - bool* keyframe_updated) { +gtsam::Pose3 HybridFormulationV1::computeInitialH(ObjectId object_id, + FrameId frame_id, + bool* keyframe_updated) { // TODO: could this ever update the keyframe? auto [s0, L_e] = getOrConstructL0(object_id, frame_id); @@ -919,11 +1039,12 @@ gtsam::Pose3 HybridFormulation::computeInitialH(ObjectId object_id, CHECK_EQ(initial_motion_frame.from(), s0); return initial_motion_frame; } else if (initial_motion_frame.style() == MotionRepresentationStyle::F2F) { + HybridAccessor::Ptr accessor = this->derivedAccessor(); // we have a motion from the frontend that is k-1 to k // first check if we have a previous estimation motion that takes us from // s0 to k-1 in the map StateQuery H_W_s0_km1 = - getEstimatedMotion(object_id, frame_id_km1); + accessor->getEstimatedMotion(object_id, frame_id_km1); if (H_W_s0_km1) { CHECK_EQ(H_W_s0_km1->from(), s0); CHECK_EQ(H_W_s0_km1->to(), frame_id_km1); @@ -970,7 +1091,7 @@ gtsam::Pose3 HybridFormulation::computeInitialH(ObjectId object_id, } } -gtsam::Pose3 HybridFormulation::calculateObjectCentroid( +gtsam::Pose3 HybridFormulationV1::calculateObjectCentroid( ObjectId object_id, FrameId frame_id) const { if (FLAGS_init_object_pose_from_gt) { const auto gt_packets = hooks().ground_truth_packets_request(); diff --git a/dynosam/src/factors/HybridFormulationFactors.cc b/dynosam/src/factors/HybridFormulationFactors.cc index 09bc5e5ff..454b96f70 100644 --- a/dynosam/src/factors/HybridFormulationFactors.cc +++ b/dynosam/src/factors/HybridFormulationFactors.cc @@ -36,80 +36,134 @@ namespace dyno { gtsam::Point3 HybridObjectMotion::projectToObject3( const gtsam::Pose3& X_k, const gtsam::Pose3& e_H_k_world, - const gtsam::Pose3& L_s0, const gtsam::Point3& Z_k) { - gtsam::Pose3 k_H_s0_k = (L_s0.inverse() * e_H_k_world * L_s0).inverse(); - gtsam::Pose3 L_k = e_H_k_world * L_s0; - gtsam::Pose3 k_H_s0_W = L_k * k_H_s0_k * L_k.inverse(); - gtsam::Point3 projected_m_object = L_s0.inverse() * k_H_s0_W * X_k * Z_k; - return projected_m_object; + const gtsam::Pose3& L_s0, const gtsam::Point3& Z_k, + gtsam::OptionalJacobian<3, 6> J1, gtsam::OptionalJacobian<3, 6> J2, + gtsam::OptionalJacobian<3, 6> J3) { + // gtsam::Pose3 k_H_s0_k = (L_s0.inverse() * e_H_k_world * L_s0).inverse(); + // gtsam::Pose3 L_k = e_H_k_world * L_s0; + // gtsam::Pose3 k_H_s0_W = L_k * k_H_s0_k * L_k.inverse(); + // gtsam::Point3 projected_m_object = L_s0.inverse() * k_H_s0_W * X_k * Z_k; + // return projected_m_object; + // Mathematical Simplification: + // The original kinematic chain simplifies algebraically to: + // P = L_s0.inverse() * e_H_k_world.inverse() * X_k * Z_k + + // 1. Invert L_s0 + gtsam::Matrix66 H_invL_L; + const gtsam::Pose3 invL = L_s0.inverse(J3 ? &H_invL_L : 0); + + // 2. Invert e_H_k_world + gtsam::Matrix66 H_invE_E; + const gtsam::Pose3 invE = e_H_k_world.inverse(J2 ? &H_invE_E : 0); + + // 3. Compose (invL * invE) + gtsam::Matrix66 H_comb1_invL, H_comb1_invE; + const gtsam::Pose3 comb1 = invL.compose(invE, J3 ? &H_comb1_invL : 0, + (J2 || J3 || J1) ? &H_comb1_invE : 0); + + // 4. Compose (comb1 * X_k) + gtsam::Matrix66 H_comb2_comb1, H_comb2_X; + const gtsam::Pose3 comb2 = + comb1.compose(X_k, (J3 || J2) ? &H_comb2_comb1 : 0, J1 ? &H_comb2_X : 0); + + // 5. Transform point (comb2 * Z_k) + gtsam::Matrix36 H_res_comb2; + gtsam::Matrix33 H_res_Z; + const gtsam::Point3 result = comb2.transformFrom( + Z_k, (J3 || J2 || J1) ? &H_res_comb2 : 0, boost::none); + + // Chain Rules + + // // dRes/dZ + // if (H_Z_k) *H_Z_k = H_res_Z; + + // dRes/dX = dRes/dComb2 * dComb2/dX + if (J1) *J1 = H_res_comb2 * H_comb2_X; + + // dRes/dE = dRes/dComb2 * dComb2/dComb1 * dComb1/dInvE * dInvE/dE + if (J2) { + *J2 = H_res_comb2 * H_comb2_comb1 * H_comb1_invE * H_invE_E; + } + + // dRes/dL = dRes/dComb2 * dComb2/dComb1 * dComb1/dInvL * dInvL/dL + if (J3) { + *J3 = H_res_comb2 * H_comb2_comb1 * H_comb1_invL * H_invL_L; + } + + return result; } gtsam::Point3 HybridObjectMotion::projectToCamera3( const gtsam::Pose3& X_k, const gtsam::Pose3& e_H_k_world, - const gtsam::Pose3& L_e, const gtsam::Point3& m_L) { - // apply transform to put map point into world via its motion - const auto A = projectToCamera3Transform(X_k, e_H_k_world, L_e); - gtsam::Point3 m_camera_k = A * m_L; + const gtsam::Pose3& L_e, const gtsam::Point3& m_L, + gtsam::OptionalJacobian<3, 6> J1, gtsam::OptionalJacobian<3, 6> J2, + gtsam::OptionalJacobian<3, 6> J3, gtsam::OptionalJacobian<3, 3> J4) { + // // apply transform to put map point into world via its motion + // const auto A = projectToCamera3Transform(X_k, e_H_k_world, L_e); + // gtsam::Point3 m_camera_k = A * m_L; + // return m_camera_k; + + // 1. Get the transform T = X_k^-1 * E * L_e + gtsam::Matrix66 H_T_X, H_T_E, H_T_L; + const gtsam::Pose3 T = projectToCamera3Transform( + X_k, e_H_k_world, L_e, J1 ? &H_T_X : 0, J2 ? &H_T_E : 0, J3 ? &H_T_L : 0); + + // 2. Transform the point: P = T * m_L + gtsam::Matrix36 H_P_T; + gtsam::Matrix33 H_P_m; + const gtsam::Point3 m_camera_k = + T.transformFrom(m_L, (J1 || J2 || J3) ? &H_P_T : 0, J4 ? &H_P_m : 0); + + // Chain Rules + if (J1) *J1 = H_P_T * H_T_X; + if (J2) *J2 = H_P_T * H_T_E; + if (J3) *J3 = H_P_T * H_T_L; + if (J4) *J4 = H_P_m; + return m_camera_k; } gtsam::Pose3 HybridObjectMotion::projectToCamera3Transform( const gtsam::Pose3& X_k, const gtsam::Pose3& e_H_k_world, - const gtsam::Pose3& L_e) { - return X_k.inverse() * e_H_k_world * L_e; -} + const gtsam::Pose3& L_e, gtsam::OptionalJacobian<6, 6> J1, + gtsam::OptionalJacobian<6, 6> J2, gtsam::OptionalJacobian<6, 6> J3) { + // return X_k.inverse() * e_H_k_world * L_e; + + // Formula: T = X_k.inverse() * e_H_k_world * L_e + + // 1. Calculate X_k inverse + gtsam::Matrix66 H_invX_Xk; + gtsam::Pose3 invX = X_k.inverse(J1 ? &H_invX_Xk : 0); + + // 2. Compose e_H_k_world and L_e + gtsam::Matrix66 H_comb1_E, H_comb1_L; + gtsam::Pose3 comb1 = + e_H_k_world.compose(L_e, (J2 || J1 || J3) ? &H_comb1_E : 0, + (J3 || J1 || J2) ? &H_comb1_L : 0); + + // 3. Compose result + gtsam::Matrix66 H_res_invX, H_res_comb1; + const gtsam::Pose3 result = + invX.compose(comb1, J1 ? &H_res_invX : 0, (J2 || J3) ? &H_res_comb1 : 0); + + // Chain Rules + if (J1) { + // dRes/dX = dRes/dInvX * dInvX/dX + *J1 = H_res_invX * H_invX_Xk; + } -// gtsam::Point3 HybridObjectMotion::projectToCamera3( -// const gtsam::Pose3& X_k, -// const gtsam::Pose3& e_H_k_world, -// const gtsam::Pose3& L_e, -// const gtsam::Point3& m_L, -// gtsam::OptionalJacobian<3, 6> H_Xk, -// gtsam::OptionalJacobian<3, 6> H_eHk, -// gtsam::OptionalJacobian<3, 6> H_Le, -// gtsam::OptionalJacobian<3, 6> H_mL) { - -// gtsam::Matrix36 H_A_Xk, H_A_eHk, H_A_Le; -// gtsam::Pose3 A = projectToCamera3Transform(X_k, e_H_k_world, L_e, -// H_Xk ? &H_A_Xk : {}, -// H_eHk ? &H_A_eHk : boost::none, -// H_Le ? &H_A_Le : boost::none); - -// gtsam::Matrix33 H_mL_local; -// gtsam::Point3 m_camera_k = A.transformFrom(m_L, H_mL ? &H_mL_local : -// boost::none); - -// // Chain Jacobians -// if (H_Xk) *H_Xk = H_mL_local * H_A_Xk; -// if (H_eHk) *H_eHk = H_mL_local * H_A_eHk; -// if (H_Le) *H_Le = H_mL_local * H_A_Le; -// if (H_mL) *H_mL = H_mL_local; - -// return m_camera_k; -// } - -// gtsam::Pose3 HybridObjectMotion::projectToCamera3Transform( -// const gtsam::Pose3& X_k, -// const gtsam::Pose3& e_H_k_world, -// const gtsam::Pose3& L_e, -// gtsam::OptionalJacobian<6, 6> H_Xk, -// gtsam::OptionalJacobian<6, 6> H_eHk, -// gtsam::OptionalJacobian<6, 6> H_Le) { - -// gtsam::Matrix66 H_inv_Xk, H_comp1, H_comp2; -// gtsam::Pose3 X_k_inv = X_k.inverse(H_Xk ? &H_inv_Xk : boost::none); -// gtsam::Pose3 comp1 = X_k_inv.compose(e_H_k_world, H_Xk ? &H_comp1 : -// boost::none, H_eHk ? &H_comp2 : nullptr); gtsam::Pose3 result = -// comp1.compose(L_e, H_Xk ? H_Xk : boost::none, H_Le ? &H_comp2 : -// boost::none); - -// if (H_Xk) { -// *H_Xk = H_comp2 * H_comp1 * H_inv_Xk; // chain rule: dR/dX = dR/dC1 * -// dC1/dXinv * dXinv/dX -// } -// return result; -// } + if (J2) { + // dRes/dE = dRes/dComb1 * dComb1/dE + *J2 = H_res_comb1 * H_comb1_E; + } + if (J3) { + // dRes/dL = dRes/dComb1 * dComb1/dL + *J3 = H_res_comb1 * H_comb1_L; + } + + return result; +} gtsam::Vector3 HybridObjectMotion::residual(const gtsam::Pose3& X_k, const gtsam::Pose3& e_H_k_world, const gtsam::Point3& m_L, @@ -123,40 +177,98 @@ gtsam::Vector HybridMotionFactor::evaluateError( const gtsam::Point3& m_L, boost::optional J1, boost::optional J2, boost::optional J3) const { - if (J1) { - // error w.r.t to camera pose - Eigen::Matrix df_dX = - gtsam::numericalDerivative31( - std::bind(&HybridMotionFactor::residual, std::placeholders::_1, - std::placeholders::_2, std::placeholders::_3, z_k_, L_e_), - X_k, e_H_k_world, m_L); - *J1 = df_dX; - } + return HybridObjectMotion::projectToCamera3( + X_k, e_H_k_world, L_e_, m_L, + J1, // J w.r.t X_k + J2, // J w.r.t e_H_k_world + boost::none, // J w.r.t L_e (fixed, not optimized here) + J3 // J w.r.t m_L + ) - + z_k_; +} - if (J2) { - // error w.r.t to motion - Eigen::Matrix df_dH = - gtsam::numericalDerivative32( - std::bind(&HybridMotionFactor::residual, std::placeholders::_1, - std::placeholders::_2, std::placeholders::_3, z_k_, L_e_), - X_k, e_H_k_world, m_L); - *J2 = df_dH; - } +StereoHybridMotionFactor::StereoHybridMotionFactor( + const gtsam::StereoPoint2& measured, const gtsam::Pose3& L_e, + const gtsam::SharedNoiseModel& model, gtsam::Cal3_S2Stereo::shared_ptr K, + gtsam::Key X_k_key, gtsam::Key e_H_k_world_key, gtsam::Key m_L_key, + bool throw_cheirality) + : Base(model, X_k_key, e_H_k_world_key, m_L_key), + measured_(measured), + K_(K), + L_e_(L_e), + camera_(gtsam::Pose3::Identity(), K), + throw_cheirality_(throw_cheirality) {} - if (J3) { - // error w.r.t to point in local - Eigen::Matrix df_dm = - gtsam::numericalDerivative33( - std::bind(&HybridMotionFactor::residual, std::placeholders::_1, - std::placeholders::_2, std::placeholders::_3, z_k_, L_e_), - X_k, e_H_k_world, m_L); - *J3 = df_dm; +gtsam::NonlinearFactor::shared_ptr StereoHybridMotionFactor::clone() const { + return boost::static_pointer_cast( + gtsam::NonlinearFactor::shared_ptr(new This(*this))); +} + +void StereoHybridMotionFactor::print( + const std::string& s, const gtsam::KeyFormatter& keyFormatter) const { + Base::print(s, keyFormatter); + measured_.print(s + ".z"); +} + +gtsam::Vector StereoHybridMotionFactor::evaluateError( + const gtsam::Pose3& X_k, const gtsam::Pose3& e_H_k_world, + const gtsam::Point3& m_L, boost::optional J1, + boost::optional J2, + boost::optional J3) const { + // 1. Project map point to camera frame using HybridObjectMotion + // We need Jacobians w.r.t inputs, but L_e is constant here so we pass none + // for it. + try { + gtsam::Matrix36 H_cam_X, H_cam_E; + gtsam::Matrix33 H_cam_m; + const gtsam::Point3 m_camera = HybridObjectMotion::projectToCamera3( + X_k, e_H_k_world, L_e_, m_L, J1 ? &H_cam_X : 0, J2 ? &H_cam_E : 0, + boost::none, // L_e is fixed + J3 ? &H_cam_m : 0); + + gtsam::Matrix36 + H_stereo_pose; // J1: Jacobian w.r.t the Camera Pose (Identity) - 3x6 + gtsam::Matrix33 + H_stereo_point; // J2: Jacobian w.r.t the Point (p_camera) - 3x3 + // We request both Jacobians as requested, though only + // J2 is needed for the chain rule since the camera + // pose (Identity) is constant and not part of the + // optimization. + const gtsam::StereoPoint2 projected_stereo = + camera_.project2(m_camera, (J1 || J2 || J3) ? &H_stereo_pose : 0, + (J1 || J2 || J3) ? &H_stereo_point : 0); + // dErr/dX + if (J1) *J1 = H_stereo_point * H_cam_X; + // dErr/dE + if (J2) *J2 = H_stereo_point * H_cam_E; + // dErr/dm + if (J3) *J3 = H_stereo_point * H_cam_m; + + // 3. Compute Error + const gtsam::Vector3 error = (projected_stereo - measured_).vector(); + return error; + } catch (gtsam::StereoCheiralityException& e) { + if (J1) *J1 = gtsam::Matrix36::Zero(); + if (J2) *J2 = gtsam::Matrix36::Zero(); + if (J3) *J3 = gtsam::Matrix33::Zero(); + + if (throw_cheirality_) { + throw gtsam::StereoCheiralityException(this->key3()); + } } + return gtsam::Vector3::Constant(2.0 * K_->fx()); +} + +const gtsam::StereoPoint2& StereoHybridMotionFactor::measured() const { + return measured_; +} +const gtsam::Cal3_S2Stereo::shared_ptr StereoHybridMotionFactor::calibration() + const { + return K_; +} - return residual(X_k, e_H_k_world, m_L, z_k_, L_e_); +const gtsam::Pose3& StereoHybridMotionFactor::embeddedPose() const { + return L_e_; } gtsam::Vector HybridSmoothingFactor::evaluateError( diff --git a/dynosam/src/frontend/FrontendModule.cc b/dynosam/src/frontend/FrontendModule.cc index e084817d3..0bfaf3b0c 100644 --- a/dynosam/src/frontend/FrontendModule.cc +++ b/dynosam/src/frontend/FrontendModule.cc @@ -66,4 +66,18 @@ void FrontendModule::validateInput( result.valid_, *input->image_container_, result.requirement_); } +void FrontendModule::FrameToClassMap(const Frame::Ptr& frame) const { + const auto& object_observations = frame->getObjectObservations(); + + for (const auto& [object_id, detection] : object_observations) { + if (!detection.class_name.empty()) { + shared_module_info.updateClassLabelMapping(object_id, detection.class_name); + std::cout << "UPDATING CLASS LABEL MAPPING " << object_id << " IS " << detection.class_name << std::endl; + } + else { + std::cout << "CANNOT UPDATE CLASS LABEL MAPPING; IT WAS EMPTY!" << std::endl; + } + } +} + } // namespace dyno diff --git a/dynosam/src/frontend/RGBDInstanceFrontendModule.cc b/dynosam/src/frontend/RGBDInstanceFrontendModule.cc index d82a7933f..ed886af18 100644 --- a/dynosam/src/frontend/RGBDInstanceFrontendModule.cc +++ b/dynosam/src/frontend/RGBDInstanceFrontendModule.cc @@ -52,6 +52,11 @@ DEFINE_bool(use_dynamic_track, true, DEFINE_bool(log_projected_masks, false, "If true, projected masks will be saved at every frame"); +DEFINE_bool(set_dense_labelled_cloud, false, + "If true, the dense labelled point cloud will be set"); + +DEFINE_bool(use_object_motion_filtering, false, "For testing!"); + namespace dyno { RGBDInstanceFrontendModule::RGBDInstanceFrontendModule( @@ -71,16 +76,21 @@ RGBDInstanceFrontendModule::RGBDInstanceFrontendModule( logger_ = std::make_unique(); } - ObjectMotionSovlerF2F::Params object_motion_solver_params = - getFrontendParams().object_motion_solver_params; - // add ground truth hook - object_motion_solver_params.ground_truth_packets_request = [&]() { - return this->shared_module_info.getGroundTruthPackets(); - }; - object_motion_solver_params.refine_motion_with_3d = false; - - object_motion_solver_ = std::make_unique( - object_motion_solver_params, camera->getParams()); + if (FLAGS_use_object_motion_filtering) { + ObjectMotionSolverFilter::Params filter_params; + object_motion_solver_ = std::make_shared( + filter_params, camera->getParams()); + } else { + ObjectMotionSovlerF2F::Params object_motion_solver_params = + getFrontendParams().object_motion_solver_params; + // add ground truth hook + object_motion_solver_params.ground_truth_packets_request = [&]() { + return this->shared_module_info.getGroundTruthPackets(); + }; + object_motion_solver_params.refine_motion_with_3d = false; + object_motion_solver_ = std::make_shared( + object_motion_solver_params, camera->getParams()); + } } RGBDInstanceFrontendModule::~RGBDInstanceFrontendModule() { @@ -106,6 +116,9 @@ FrontendModule::SpinReturn RGBDInstanceFrontendModule::boostrapSpin( Frame::Ptr frame = tracker_->track(input->getFrameId(), input->getTimestamp(), *image_container); + FrontendModule::FrameToClassMap(frame); + + CHECK(frame->updateDepths()); return {State::Nominal, nullptr}; @@ -118,28 +131,33 @@ FrontendModule::SpinReturn RGBDInstanceFrontendModule::nominalSpin( const bool has_imu = input->imu_measurements.has_value(); const bool has_stereo = image_container->hasRightRgb(); + //! Rotation from k-1 to k in k-1 std::optional R_curr_ref; ImuFrontend::PimPtr pim; if (has_imu) { pim = imu_frontend_.preintegrateImuMeasurements( input->imu_measurements.value()); - nav_state_ = - pim->predict(previous_nav_state_, gtsam::imuBias::ConstantBias{}); - - last_imu_nav_state_update_ = input->getFrameId(); + nav_state_curr_ = + pim->predict(nav_state_prev_, gtsam::imuBias::ConstantBias{}); + // nav_state_curr_ = + // pim->predict(nav_state_last_kf_, gtsam::imuBias::ConstantBias{}); + last_imu_k_ = input->getFrameId(); + // relative rotation R_curr_ref = - previous_nav_state_.attitude().inverse() * nav_state_.attitude(); + nav_state_prev_.attitude().inverse() * nav_state_curr_.attitude(); } Frame::Ptr frame = tracker_->track(input->getFrameId(), input->getTimestamp(), *image_container, R_curr_ref); + FrontendModule::FrameToClassMap(frame); Frame::Ptr previous_frame = tracker_->getPreviousFrame(); CHECK(previous_frame); - VLOG(1) << to_string(tracker_->getTrackerInfo()); + // const FeatureTrackerInfo& tracker_info = tracker_->getTrackerInfo(); + // VLOG(1) << to_string(tracker_info); { // this will mark some points as invalid if they are out of depth range @@ -158,10 +176,6 @@ FrontendModule::SpinReturn RGBDInstanceFrontendModule::nominalSpin( const cv::Mat& left_rgb = image_container->rgb(); const cv::Mat& right_rgb = image_container->rightRgb(); - auto usable_iterator = frame->static_features_.beginUsable(); - LOG(INFO) << "Counted intliers " - << std::distance(usable_iterator.begin(), usable_iterator.end()); - FeaturePtrs stereo_features_1; stereo_result = tracker_->stereoTrack(stereo_features_1, frame->static_features_, @@ -188,7 +202,7 @@ FrontendModule::SpinReturn RGBDInstanceFrontendModule::nominalSpin( // VERY important calculation const gtsam::Pose3 T_k_1_k = - previous_nav_state_.pose().inverse() * frame->T_world_camera_; + nav_state_prev_.pose().inverse() * frame->T_world_camera_; vo_velocity_ = T_k_1_k; // we currently use the frame pose as the nav state - this value can come from @@ -196,23 +210,23 @@ FrontendModule::SpinReturn RGBDInstanceFrontendModule::nominalSpin( // solveCameraMotion this is only relevant since we dont solve incremental so // the backend is not immediately updating the frontend at which point we can // just use the best estimate in the case of the VO, the nav_state velocity + const gtsam::NavState best_nav_state(frame->T_world_camera_, + nav_state_curr_.velocity()); // will be wrong (currently!!) - previous_nav_state_ = - gtsam::NavState(frame->T_world_camera_, nav_state_.velocity()); - // previous_nav_state_ = nav_state_; - - if (R_curr_ref) { - imu_frontend_.resetIntegration(); - } + nav_state_prev_ = best_nav_state; - // if (FLAGS_use_dynamic_track) { - // utils::TimingStatsCollector track_dynamic_timer("tracking_dynamic"); - // vision_tools::trackDynamic(getFrontendParams(), *previous_frame, frame); + // if (R_curr_ref) { + // imu_frontend_.resetIntegration(); // } const auto [object_motions, object_poses] = object_motion_solver_->solve(frame, previous_frame); + const FeatureTrackerInfo& tracker_info = *frame->getTrackingInfo(); + const FeatureTrackerInfo& tracker_info_prev = + *previous_frame->getTrackingInfo(); + VLOG(1) << to_string(tracker_info); + VisionImuPacket::Ptr vision_imu_packet = std::make_shared(); vision_imu_packet->frameId(frame->getFrameId()); vision_imu_packet->timestamp(frame->getTimestamp()); @@ -221,16 +235,27 @@ FrontendModule::SpinReturn RGBDInstanceFrontendModule::nominalSpin( fillOutputPacketWithTracks(vision_imu_packet, *frame, T_k_1_k, object_motions, object_poses); + // Update the body velocity according to the previous vision IMU packet + if (previous_vision_imu_packet_) { + vision_imu_packet->updateBodyVelocity(*previous_vision_imu_packet_); + } + + previous_vision_imu_packet_ = vision_imu_packet; + + if (R_curr_ref) { + imu_frontend_.resetIntegration(); + } + DebugImagery debug_imagery; debug_imagery.tracking_image = createTrackingImage(frame, previous_frame, object_poses); const ImageContainer& processed_image_container = frame->image_container_; debug_imagery.rgb_viz = ImageType::RGBMono::toRGB(processed_image_container.rgb()); - debug_imagery.flow_viz = - ImageType::OpticalFlow::toRGB(processed_image_container.opticalFlow()); - debug_imagery.mask_viz = ImageType::MotionMask::toRGB( - processed_image_container.objectMotionMask()); + // debug_imagery.flow_viz = + // ImageType::OpticalFlow::toRGB(processed_image_container.opticalFlow()); + // debug_imagery.mask_viz = ImageType::MotionMask::toRGB( + // processed_image_container.objectMotionMask()); debug_imagery.depth_viz = ImageType::Depth::toRGB(processed_image_container.depth()); @@ -238,20 +263,23 @@ FrontendModule::SpinReturn RGBDInstanceFrontendModule::nominalSpin( display_queue_->push( ImageToDisplay("Tracks", debug_imagery.tracking_image)); - // cv::Mat stereo_matches; - // if(tracker_->drawStereoMatches(stereo_matches, *frame)){ - // display_queue_->push( - // ImageToDisplay("Stereo Matches", stereo_matches)); - // } + cv::Mat stereo_matches; + if (tracker_->drawStereoMatches(stereo_matches, *frame)) { + display_queue_->push(ImageToDisplay("Stereo Matches", stereo_matches)); + } } vision_imu_packet->debugImagery(debug_imagery); - // // const cv::Mat& board_detection_mask = - // tracker_->getBoarderDetectionMask(); PointCloudLabelRGB::Ptr - // dense_labelled_cloud = - // frame->projectToDenseCloud(&board_detection_mask); - PointCloudLabelRGB::Ptr dense_labelled_cloud = nullptr; + if (FLAGS_set_dense_labelled_cloud) { + VLOG(30) << "Setting dense labelled cloud"; + utils::TimingStatsCollector labelled_clout_timer( + "frontend.dense_labelled_cloud"); + const cv::Mat& board_detection_mask = tracker_->getBoarderDetectionMask(); + PointCloudLabelRGB::Ptr dense_labelled_cloud = + frame->projectToDenseCloud(&board_detection_mask); + vision_imu_packet->denseLabelledCloud(dense_labelled_cloud); + } // if (FLAGS_save_frontend_json) // output_packet_record_.insert({output->getFrameId(), output}); @@ -314,16 +342,16 @@ bool RGBDInstanceFrontendModule::solveCameraMotion( // way of checking that we HAVE an imu). If we do we can use the nav state // directly to update the current pose as the nav state is the forward // prediction from the IMU - if (last_imu_nav_state_update_ == frame_k->getFrameId()) { - frame_k->T_world_camera_ = nav_state_.pose(); + if (last_imu_k_ == frame_k->getFrameId()) { + frame_k->T_world_camera_ = nav_state_curr_.pose(); ss << "Nav state was previous updated with IMU. Using predicted pose to " "set camera transform; k" << frame_k->getFrameId(); } else { // no IMU for forward prediction, use constant velocity model to propogate - // pose expect previous_nav_state_ to always be updated with the best + // pose expect nav_state_prev_ to always be updated with the best // pose! - frame_k->T_world_camera_ = previous_nav_state_.pose() * vo_velocity_; + frame_k->T_world_camera_ = nav_state_prev_.pose() * vo_velocity_; ss << "Nav state has no information from imu. Using constant velocity " "model to propofate pose; k" << frame_k->getFrameId(); diff --git a/dynosam/src/frontend/VisionImuOutputPacket.cc b/dynosam/src/frontend/VisionImuOutputPacket.cc index 31fec1ba8..5d1e79eca 100644 --- a/dynosam/src/frontend/VisionImuOutputPacket.cc +++ b/dynosam/src/frontend/VisionImuOutputPacket.cc @@ -40,10 +40,31 @@ ImuFrontend::PimPtr VisionImuPacket::pim() const { return pim_; } Camera::ConstPtr VisionImuPacket::camera() const { return camera_; } +gtsam::Vector6 VisionImuPacket::getBodyVelocity() const { return *stored_body_velocity; } + PointCloudLabelRGB::Ptr VisionImuPacket::denseLabelledCloud() const { return dense_labelled_cloud_; } +bool VisionImuPacket::isCameraKeyFrame() const { + return camera_tracks_.is_keyframe; +} + +bool VisionImuPacket::isObjectKeyFrame(ObjectId object_id) const { + if (object_tracks_.exists(object_id)) { + return object_tracks_.at(object_id).is_keyframe; + } + return false; +} + +bool VisionImuPacket::isKeyFrame() const { + bool is_any_keyframe = this->isCameraKeyFrame(); + for (const auto& [object_id, _] : this->objectTracks()) { + is_any_keyframe = is_any_keyframe || isObjectKeyFrame(object_id); + } + return is_any_keyframe; +} + const VisionImuPacket::CameraTracks& VisionImuPacket::cameraTracks() const { return camera_tracks_; } @@ -194,6 +215,32 @@ void VisionImuPacket::fillLandmarkMeasurements( } } +// Adding velocity information into the VisionImuPacket + +// * Function to update the previous body velocity * +VisionImuPacket& VisionImuPacket::updateBodyVelocity(const VisionImuPacket& prev_state) { + + // Getting the pose at k-1 + const gtsam::Pose3& w_L_k_1 = prev_state.cameraPose(); + + // Getting the motion from k-1 to k + const gtsam::Pose3& w_k_1_H_k = camera_tracks_.T_k_1_k; + + stored_body_velocity = calculateBodyMotion( + w_k_1_H_k, + w_L_k_1, + prev_state.timestamp(), + timestamp_ + ); + + return *this; + +} + +// * Function to get the body velocity * + + + } // namespace dyno namespace nlohmann { diff --git a/dynosam/src/frontend/vision/FeatureTracker.cc b/dynosam/src/frontend/vision/FeatureTracker.cc index 9b1eb998d..2a8765c9c 100644 --- a/dynosam/src/frontend/vision/FeatureTracker.cc +++ b/dynosam/src/frontend/vision/FeatureTracker.cc @@ -102,7 +102,13 @@ Frame::Ptr FeatureTracker::track(FrameId frame_id, Timestamp timestamp, // result after the function call) ObjectDetectionEngine is used if // params_.prefer_provided_object_detection is false vision_tools::ObjectBoundaryMaskResult boundary_mask_result; - objectDetection(boundary_mask_result, input_images); + + ObjectDetectionResult observations = objectDetection(boundary_mask_result, input_images); + + for (const auto& det : observations.detections) { + std::cout << "Detected object_id=" << det.object_id + << " class_name=" << det.class_name << std::endl; + } if (!initial_computation_ && params_.use_propogate_mask) { utils::TimingStatsCollector timer("propogate_mask"); @@ -156,18 +162,10 @@ Frame::Ptr FeatureTracker::track(FrameId frame_id, Timestamp timestamp, // TODO: SingleDetectionResult really does not need the tracklet ids they // are never actually used!! this prevents the frame from needing to do the // same calculations we've alrady done - std::map object_observations; - for (size_t i = 0; i < boundary_mask_result.objects_detected.size(); i++) { - ObjectId object_id = boundary_mask_result.objects_detected.at(i); - const cv::Rect& bb_detection = - boundary_mask_result.object_bounding_boxes.at(i); - SingleDetectionResult observation; - observation.object_id = object_id; - // observation.object_features = dynamic_features.getByObject(object_id); - observation.bounding_box = bb_detection; - - object_observations[object_id] = observation; + std::map object_observations; + for (const auto& detection : observations.detections) { + object_observations[detection.object_id] = detection; } utils::TimingStatsCollector f_timer("tracking_timer.frame_construction"); @@ -189,6 +187,8 @@ Frame::Ptr FeatureTracker::track(FrameId frame_id, Timestamp timestamp, previous_frame_ = new_frame; boarder_detection_mask_ = boundary_mask_result.boundary_mask; + // FrontendModule::FrameToClassMap(new_frame); + return new_frame; } @@ -239,19 +239,20 @@ bool FeatureTracker::stereoTrack(FeaturePtrs& stereo_features, return std::sqrt(dx * dx + dy * dy); }; // update klt status based on result from flow - for (size_t i = 0; i < klt_status.size(); i++) { - const bool both_status_good = klt_status.at(i) && klt_reverse_status.at(i); - const bool within_image = isWithinShrunkenImage(right_feature_points.at(i)); - const bool within_distance = - distance(left_feature_points.at(i), - reverse_left_feature_points.at(i)) <= 0.5; - - if (both_status_good && within_image && within_distance) { - klt_status.at(i) = 1; - } else { - klt_status.at(i) = 0; - } - } + // for (size_t i = 0; i < klt_status.size(); i++) { + // const bool both_status_good = klt_status.at(i) && + // klt_reverse_status.at(i); const bool within_image = + // isWithinShrunkenImage(right_feature_points.at(i)); const bool + // within_distance = + // distance(left_feature_points.at(i), + // reverse_left_feature_points.at(i)) <= 0.5; + + // if (both_status_good && within_image && within_distance) { + // klt_status.at(i) = 1; + // } else { + // klt_status.at(i) = 0; + // } + // } TrackletIds good_stereo_tracklets; @@ -617,8 +618,6 @@ void FeatureTracker::trackDynamicKLT( std::vector current_points; current_points.resize(previous_pts.size()); - LOG(INFO) << "Found " << tracklet_ids.size() << " dynamic inliers for KLT"; - if (tracklet_ids.size() > 0) { utils::TimingStatsCollector tracking_t( "dynamic_feature_track_klt.tracking"); @@ -771,8 +770,6 @@ void FeatureTracker::trackDynamicKLT( } for (const auto& [object_id, features_j] : tracks_per_object) { - LOG(INFO) << "Tracked " << features_j.size() << " j= " << object_id - << " with KLT"; dynamic_features += features_j; } } @@ -821,8 +818,9 @@ void FeatureTracker::trackDynamicKLT( cv::bitwise_and(obj_mask, detection_mask_impl, combined_mask); std::vector detected_points; - cv::goodFeaturesToTrack(mono, detected_points, 300, qualityLevel, - min_feature_distance, combined_mask); + cv::goodFeaturesToTrack(mono, detected_points, max_features_to_track, + qualityLevel, min_feature_distance, + combined_mask); std::vector keypoints; cv::KeyPoint::convert(detected_points, keypoints); @@ -1021,7 +1019,7 @@ void FeatureTracker::sampleDynamic(FrameId frame_id, } void FeatureTracker::requiresSampling( - std::set& objects_to_sample, const FeatureTrackerInfo& info, + std::set& objects_to_sample, FeatureTrackerInfo& info, const ImageContainer& image_container, const gtsam::FastMap& features_per_object, const vision_tools::ObjectBoundaryMaskResult& boundary_mask_result, @@ -1045,14 +1043,23 @@ void FeatureTracker::requiresSampling( << " this could happen if the object mask changes dramatically...!!"; } - if (!previous_frame_) { - if (!detected_objects.empty()) { - VLOG(5) << "All objects sampled as first frame"; - objects_to_sample.insert(detected_objects.begin(), - detected_objects.end()); - } - return; - } + // NOTE: the object_reampled info is epeated on objects_to_sample + // but during testing trying not to change the functional interface!! + // if (!previous_frame_) { + // if (!detected_objects.empty()) { + // VLOG(5) << "All objects sampled as first frame"; + // objects_to_sample.insert(detected_objects.begin(), + // detected_objects.end()); + // for(const ObjectId& object_id : objects_to_sample) { + // CHECK(!info.dynamic_track.exists(object_id)); + // // this will make a new object status + // auto& per_object_status = info.getObjectStatus(object_id); + // per_object_status.object_new = true; + // per_object_status.object_resampled = true; + // } + // } + // return; + // } const int& max_dynamic_point_age = params_.max_dynamic_feature_age; // bascially how early we want to retrack points based on their expiry @@ -1067,11 +1074,11 @@ void FeatureTracker::requiresSampling( CHECK_GT(expiry_age, 0u); for (size_t i = 0; i < detected_objects.size(); i++) { - ObjectId object_id = detected_objects.at(i); + const ObjectId object_id = detected_objects.at(i); // object is tracked and therefore should exist in the previous frame! if (info.dynamic_track.exists(object_id)) { - const auto& per_object_status = info.dynamic_track.at(object_id); + auto& per_object_status = info.dynamic_track.at(object_id); if (!features_per_object.exists(object_id)) { LOG(WARNING) << "Object " << object_id @@ -1123,6 +1130,7 @@ void FeatureTracker::requiresSampling( if (needs_sampling) { objects_to_sample.insert(object_id); + per_object_status.object_resampled = true; VLOG(5) << "Object " << info_string(info.frame_id, object_id) << " requires sampling"; @@ -1135,11 +1143,15 @@ void FeatureTracker::requiresSampling( objects_to_sample.insert(object_id); VLOG(5) << "Object " << info_string(info.frame_id, object_id) << " requires sampling. Sampling reason: new object"; + // this will make a new object status + auto& per_object_status = info.getObjectStatus(object_id); + per_object_status.object_new = true; + per_object_status.object_resampled = true; } } } -bool FeatureTracker::objectDetection( +ObjectDetectionResult FeatureTracker::objectDetection( vision_tools::ObjectBoundaryMaskResult& boundary_mask_result, ImageContainer& image_container) { // from some experimental testing 10 pixles is a good boarder to add around @@ -1159,7 +1171,8 @@ bool FeatureTracker::objectDetection( // detection mask is in the opencv mask form: CV_8UC1 where white pixels (255) // are valid and black pixels (0) should not be detected on static constexpr bool kUseAsFeatureDetectionMask = true; - // else run etection + + // else run detection if (params_.prefer_provided_object_detection) { if (image_container.hasObjectMask()) { cv::Mat object_mask = image_container.objectMotionMask(); @@ -1170,7 +1183,110 @@ bool FeatureTracker::objectDetection( vision_tools::computeObjectMaskBoundaryMask( boundary_mask_result, object_mask, scaled_boarder_thickness, kUseAsFeatureDetectionMask); - return false; + + ObjectDetectionResult detection_result; + detection_result.input_image = image_container.rgb(); + detection_result.labelled_mask = object_mask; + + // Run YOLO just for semantic labels + ObjectDetectionResult yolo_result; + if (object_detection_) { + VLOG(30) << "Running YOLO for semantic labeling k=" + << image_container.frameId(); + utils::TimingStatsCollector timing("tracking_timer.yolo_semantic_labeling"); + yolo_result = object_detection_->process(image_container.rgb()); + + // Debug: print YOLO detections + for (const auto& yolo_det : yolo_result.detections) { + VLOG(10) << "YOLO found: object_id=" << yolo_det.object_id + << " class=" << yolo_det.class_name + << " bbox=" << yolo_det.bounding_box; + } + } + + // Choose matching method based on parameter + // Set this to true if object IDs match between mask and YOLO tracker + // Set to false to use IoU-based matching + const bool use_direct_id_matching = false; // Change this based on your setup + + if (use_direct_id_matching) { + // METHOD 1: Direct ID lookup (when object IDs already match) + VLOG(10) << "Using direct ID matching for semantic labels"; + + // Create map of object_id -> class_name from YOLO + std::map yolo_labels; + for (const auto& yolo_det : yolo_result.detections) { + yolo_labels[yolo_det.object_id] = yolo_det.class_name; + } + + for (size_t i = 0; i < boundary_mask_result.objects_detected.size(); i++) { + ObjectId object_id = boundary_mask_result.objects_detected.at(i); + const cv::Rect& bb_detection = + boundary_mask_result.object_bounding_boxes.at(i); + + SingleDetectionResult observation; + observation.object_id = object_id; + observation.bounding_box = bb_detection; + observation.class_name = "unknown"; // default + + // Direct lookup by object_id + if (yolo_labels.find(object_id) != yolo_labels.end()) { + observation.class_name = yolo_labels[object_id]; + VLOG(10) << "Object " << object_id + << " matched to class '" << observation.class_name << "'"; + } else { + VLOG(10) << "No YOLO match found for object " << object_id; + } + + detection_result.detections.push_back(observation); + } + } else { + // METHOD 2: IoU-based matching (when object IDs don't match) + VLOG(10) << "Using IoU-based matching for semantic labels"; + + // IoU threshold for matching - adjust as needed + // Higher (0.5-0.7) = stricter, fewer false matches + // Lower (0.2-0.3) = more lenient, more objects get labels + const float iou_threshold = 0.3f; + + for (size_t i = 0; i < boundary_mask_result.objects_detected.size(); i++) { + ObjectId object_id = boundary_mask_result.objects_detected.at(i); + const cv::Rect& bb_detection = + boundary_mask_result.object_bounding_boxes.at(i); + + SingleDetectionResult observation; + observation.object_id = object_id; + observation.bounding_box = bb_detection; + observation.class_name = "unknown"; // default if no match + + // Find best matching YOLO detection by bounding box IoU + float best_iou = 0.0f; + std::string best_class = "unknown"; + + for (const auto& yolo_det : yolo_result.detections) { + float iou = utils::calculateIoU(bb_detection, yolo_det.bounding_box); + if (iou > best_iou) { + best_iou = iou; + best_class = yolo_det.class_name; + } + } + + // Only use YOLO class if IoU is above threshold + if (best_iou > iou_threshold) { + observation.class_name = best_class; + VLOG(10) << "Matched mask object " << object_id + << " to YOLO class '" << best_class + << "' with IoU=" << best_iou; + } else { + VLOG(10) << "No good YOLO match for mask object " << object_id + << " (best IoU=" << best_iou << ")"; + } + + detection_result.detections.push_back(observation); + } + } + + return detection_result; } else { LOG(FATAL) << "Params specify prefer provided object mask but input " "is missing!"; @@ -1183,9 +1299,15 @@ bool FeatureTracker::objectDetection( { utils::TimingStatsCollector timing("tracking_timer.detection_inference"); detection_result = object_detection_->process(image_container.rgb()); + + // Debug: print semantic labels from YOLO + for (const auto& det : detection_result.detections) { + VLOG(10) << "YOLO detected object_id=" << det.object_id + << " class_name='" << det.class_name << "' " + << "confidence=" << det.confidence; + } } cv::Mat object_mask = detection_result.labelled_mask; - { utils::TimingStatsCollector timing( "tracking_timer.compute_boundary_mask"); @@ -1193,11 +1315,10 @@ bool FeatureTracker::objectDetection( boundary_mask_result, detection_result, scaled_boarder_thickness, kUseAsFeatureDetectionMask); } - // update or insert image container with object mask image_container.replace(ImageContainer::kObjectMask, object_mask); - return true; + return detection_result; } } @@ -1213,6 +1334,7 @@ void FeatureTracker::propogateMask(ImageContainer& image_container) { // note reference cv::Mat& current_mask = image_container.objectMotionMask(); + ObjectIds instance_labels; for (const Feature::Ptr& dynamic_feature : previous_frame_->usableDynamicFeaturesBegin()) { @@ -1340,6 +1462,7 @@ void FeatureTracker::propogateMask(ImageContainer& image_container) { current_mask.at(functional_keypoint::v(predicted_kp), functional_keypoint::u(predicted_kp)) = instance_labels[i]; + // current_rgb // updated_mask_points++; } @@ -1350,4 +1473,5 @@ void FeatureTracker::propogateMask(ImageContainer& image_container) { } } + } // namespace dyno diff --git a/dynosam/src/frontend/vision/MotionSolver.cc b/dynosam/src/frontend/vision/MotionSolver.cc index 710015aa6..7bbb1d5bd 100644 --- a/dynosam/src/frontend/vision/MotionSolver.cc +++ b/dynosam/src/frontend/vision/MotionSolver.cc @@ -43,6 +43,7 @@ #include #include "dynosam/backend/BackendDefinitions.hpp" +#include "dynosam/factors/HybridFormulationFactors.hpp" #include "dynosam/factors/LandmarkMotionTernaryFactor.hpp" #include "dynosam/factors/Pose3FlowProjectionFactor.h" #include "dynosam/frontend/vision/VisionTools.hpp" @@ -52,8 +53,21 @@ #include "dynosam_common/utils/GtsamUtils.hpp" #include "dynosam_common/utils/Numerical.hpp" #include "dynosam_common/utils/TimingStats.hpp" +#include "dynosam_cv/RGBDCamera.hpp" #include "dynosam_opt/FactorGraphTools.hpp" //TODO: clean +// GTSAM Includes +// FOR TESTING! +#include +#include +#include +#include +#include +#include +// #include + +#include "dynosam/factors/HybridFormulationFactors.hpp" + namespace dyno { void declare_config(OpticalFlowAndPoseOptimizer::Params& config) { @@ -301,10 +315,8 @@ Pose3SolverResult EgoMotionSolver::geometricOutlierRejection3d2d( void OpticalFlowAndPoseOptimizer::updateFrameOutliersWithResult( const Result& result, Frame::Ptr frame_k_1, Frame::Ptr frame_k) const { utils::TimingStatsCollector timer("of_motion_solver.update_frame"); - // //original flow image that goes from k to k+1 (gross, im sorry!) - // TODO: use flow_is_future param - const cv::Mat& flow_image = frame_k->image_container_.opticalFlow(); - const cv::Mat& motion_mask = frame_k->image_container_.objectMotionMask(); + const auto& image_container_k = frame_k->image_container_; + const cv::Mat& motion_mask = image_container_k.objectMotionMask(); auto camera = frame_k->camera_; const auto& refined_inliers = result.inliers; @@ -317,7 +329,7 @@ void OpticalFlowAndPoseOptimizer::updateFrameOutliersWithResult( TrackletId tracklet_id = refined_inliers.at(i); gtsam::Point2 refined_flow = refined_flows.at(i); - const Feature::Ptr feature_k_1 = frame_k_1->at(tracklet_id); + Feature::Ptr feature_k_1 = frame_k_1->at(tracklet_id); Feature::Ptr feature_k = frame_k->at(tracklet_id); CHECK_EQ(feature_k->objectId(), result.best_result.object_id); @@ -340,20 +352,41 @@ void OpticalFlowAndPoseOptimizer::updateFrameOutliersWithResult( continue; } - // we now have to update the prediced keypoint using the original flow!! - // TODO: code copied from feature tracker - const int x = functional_keypoint::u(refined_keypoint); - const int y = functional_keypoint::v(refined_keypoint); - double flow_xe = static_cast(flow_image.at(y, x)[0]); - double flow_ye = static_cast(flow_image.at(y, x)[1]); - // the measured flow after the origin has been updated - OpticalFlow new_measured_flow(flow_xe, flow_ye); - feature_k->measuredFlow(new_measured_flow); - // TODO: check predicted flow is within image - Keypoint predicted_kp = Feature::CalculatePredictedKeypoint( - refined_keypoint, new_measured_flow); - feature_k->predictedKeypoint(predicted_kp); + // update current keypoint feature_k->keypoint(refined_keypoint); + // update refined frlow + feature_k_1->measuredFlow(refined_flow); + // update refined predicted keypoint + feature_k_1->predictedKeypoint(refined_keypoint); + + // Logic is a bit convoluted and dependant on other things + // if we have optical flow (from k to k+1 due to historical reaseons) + // then use this to set the predicted keypoint + // if we are tracking using the provided flow then this MUST get set + // otherwise tracking will fail as tracking with provided optical flow uses + // the predicted keypoint to get the next keypoint! + if (image_container_k.hasOpticalFlow()) { + const cv::Mat& flow_image = image_container_k.opticalFlow(); + const int x = functional_keypoint::u(refined_keypoint); + const int y = functional_keypoint::v(refined_keypoint); + double flow_xe = static_cast(flow_image.at(y, x)[0]); + double flow_ye = static_cast(flow_image.at(y, x)[1]); + + OpticalFlow new_measured_flow(flow_xe, flow_ye); + feature_k->measuredFlow(new_measured_flow); + // TODO: check predicted flow is within image + Keypoint predicted_kp = Feature::CalculatePredictedKeypoint( + refined_keypoint, new_measured_flow); + feature_k->predictedKeypoint(predicted_kp); + } else { + // we dont have a predicted flow but we assume we are using KLT tracking + // in this case we dont have a predicted keypoint. + // just use the refined flow as the measured flow + feature_k->measuredFlow(refined_flow); + Keypoint predicted_kp = + Feature::CalculatePredictedKeypoint(refined_keypoint, refined_flow); + feature_k->predictedKeypoint(predicted_kp); + } } // update tracks @@ -439,15 +472,8 @@ Pose3SolverResult EgoMotionSolver::geometricOutlierRejection3d3d( return result; } -ObjectMotionSovlerF2F::ObjectMotionSovlerF2F( - const ObjectMotionSovlerF2F::Params& params, - const CameraParams& camera_params) - : EgoMotionSolver(static_cast(params), - camera_params), - object_motion_params(params) {} - -ObjectMotionSovlerF2F::Result ObjectMotionSovlerF2F::solve( - Frame::Ptr frame_k, Frame::Ptr frame_k_1) { +ObjectMotionSolver::Result ObjectMotionSolver::solve(Frame::Ptr frame_k, + Frame::Ptr frame_k_1) { ObjectIds failed_object_tracks; MotionEstimateMap motion_estimates; @@ -488,14 +514,25 @@ ObjectMotionSovlerF2F::Result ObjectMotionSovlerF2F::solve( for (auto object_id : failed_object_tracks) { frame_k->object_observations_.erase(object_id); } - auto motions = updateMotions(motion_estimates, frame_k, frame_k_1); - auto poses = updatePoses(motion_estimates, frame_k, frame_k_1); - return std::make_pair(motions, poses); + + ObjectPoseMap poses; + ObjectMotionMap motions; + + updateMotions(motions, motion_estimates, frame_k, frame_k_1); + updatePoses(poses, motion_estimates, frame_k, frame_k_1); + return {motions, poses}; } -const ObjectPoseMap& ObjectMotionSovlerF2F::updatePoses( - MotionEstimateMap& motion_estimates, Frame::Ptr frame_k, - Frame::Ptr frame_k_1) { +ObjectMotionSovlerF2F::ObjectMotionSovlerF2F( + const ObjectMotionSovlerF2F::Params& params, + const CameraParams& camera_params) + : EgoMotionSolver(static_cast(params), + camera_params), + object_motion_params(params) {} + +void ObjectMotionSovlerF2F::updatePoses( + ObjectPoseMap& object_poses, const MotionEstimateMap& motion_estimates, + Frame::Ptr frame_k, Frame::Ptr frame_k_1) { gtsam::Point3Vector object_centroids_k_1, object_centroids_k; for (const auto& [object_id, motion_estimate] : motion_estimates) { @@ -551,16 +588,17 @@ const ObjectPoseMap& ObjectMotionSovlerF2F::updatePoses( frame_k->getFrameId()); } - return object_poses_; + object_poses = object_poses_; } -const ObjectMotionMap& ObjectMotionSovlerF2F::updateMotions( - MotionEstimateMap& motion_estimates, Frame::Ptr frame_k, Frame::Ptr) { +void ObjectMotionSovlerF2F::updateMotions( + ObjectMotionMap& object_motions, const MotionEstimateMap& motion_estimates, + Frame::Ptr frame_k, Frame::Ptr) { const FrameId frame_id_k = frame_k->getFrameId(); for (const auto& [object_id, motion_reference_frame] : motion_estimates) { object_motions_.insert22(object_id, frame_id_k, motion_reference_frame); } - return object_motions_; + object_motions = object_motions_; } bool ObjectMotionSovlerF2F::solveImpl(Frame::Ptr frame_k, Frame::Ptr frame_k_1, @@ -636,7 +674,6 @@ Motion3SolverResult ObjectMotionSovlerF2F::geometricOutlierRejection3d2d( gtsam::Pose3 G_w = pose_result.best_result.inverse(); if (object_motion_params.refine_motion_with_joint_of) { - VLOG(10) << "Refining object motion pose with joint of"; OpticalFlowAndPoseOptimizer flow_optimizer( object_motion_params.joint_of_params); // Use the original result as the input to the refine joint optical flow @@ -653,6 +690,11 @@ Motion3SolverResult ObjectMotionSovlerF2F::geometricOutlierRejection3d2d( G_w = flow_opt_result.best_result.refined_pose.inverse(); // inliers should be a subset of the original refined inlier tracks refined_inlier_tracklets = flow_opt_result.inliers; + + VLOG(10) << "Refined object " << object_id + << "pose with optical flow - error before: " + << flow_opt_result.error_before.value_or(NaN) + << " error_after: " << flow_opt_result.error_after.value_or(NaN); } // still need to take the inverse as we get the inverse of G out gtsam::Pose3 H_w = T_world_k * G_w; @@ -692,106 +734,1360 @@ Motion3SolverResult ObjectMotionSovlerF2F::geometricOutlierRejection3d2d( return motion_result; } -// Pose3SolverResult ObjectMotionSovler::geometricOutlierRejection3d3d( -// Frame::Ptr frame_k_1, Frame::Ptr frame_k, const gtsam::Pose3& T_world_k, -// ObjectId object_id) { -// PointCloudCorrespondences dynamic_correspondences; -// // get the corresponding feature pairs -// bool corr_result = frame_k->getDynamicCorrespondences( -// dynamic_correspondences, *frame_k_1, object_id, -// frame_k->landmarkWorldPointCloudCorrespondance()); +// namespace testing { + +// using namespace cv; +// using namespace std; -// const size_t& n_matches = dynamic_correspondences.size(); +// // Compute isotropic normalization transform for a set of 2D points +// static Mat computeNormalizationTransform(const vector& pts) { +// int n = (int)pts.size(); +// Point2f centroid(0, 0); +// for (const auto& p : pts) centroid += p; +// centroid.x /= n; +// centroid.y /= n; -// Pose3SolverResult result; -// result = -// EgoMotionSolver::geometricOutlierRejection3d3d(dynamic_correspondences); +// double meanDist = 0.0; +// for (const auto& p : pts) { +// double dx = p.x - centroid.x; +// double dy = p.y - centroid.y; +// meanDist += sqrt(dx * dx + dy * dy); +// } +// meanDist /= n; + +// double scale = (meanDist > 0) ? (sqrt(2.0) / meanDist) : 1.0; + +// Mat T = Mat::eye(3, 3, CV_64F); +// T.at(0, 0) = scale; +// T.at(1, 1) = scale; +// T.at(0, 2) = -scale * centroid.x; +// T.at(1, 2) = -scale * centroid.y; +// return T; +// } + +// // Normalized DLT homography estimation: src -> dst (both length >= 4) +// Mat estimateHomographyDLT(const vector& src, +// const vector& dst) { +// CV_Assert(src.size() >= 4 && src.size() == dst.size()); +// int n = (int)src.size(); + +// Mat T1 = computeNormalizationTransform(src); +// Mat T2 = computeNormalizationTransform(dst); + +// // Normalize points +// vector nsrc(n), ndst(n); +// for (int i = 0; i < n; i++) { +// Mat p = (Mat_(3, 1) << src[i].x, src[i].y, 1.0); +// Mat pn = T1 * p; +// nsrc[i] = Point2f((float)(pn.at(0, 0) / pn.at(2, 0)), +// (float)(pn.at(1, 0) / pn.at(2, 0))); + +// Mat q = (Mat_(3, 1) << dst[i].x, dst[i].y, 1.0); +// Mat qn = T2 * q; +// ndst[i] = Point2f((float)(qn.at(0, 0) / qn.at(2, 0)), +// (float)(qn.at(1, 0) / qn.at(2, 0))); +// } + +// // Build A matrix (2n x 9) +// Mat A = Mat::zeros(n * 2, 9, CV_64F); +// for (int i = 0; i < n; i++) { +// double x = nsrc[i].x; +// double y = nsrc[i].y; +// double u = ndst[i].x; +// double v = ndst[i].y; +// A.at(2 * i, 0) = -x; +// A.at(2 * i, 1) = -y; +// A.at(2 * i, 2) = -1; +// A.at(2 * i, 6) = x * u; +// A.at(2 * i, 7) = y * u; +// A.at(2 * i, 8) = u; + +// A.at(2 * i + 1, 3) = -x; +// A.at(2 * i + 1, 4) = -y; +// A.at(2 * i + 1, 5) = -1; +// A.at(2 * i + 1, 6) = x * v; +// A.at(2 * i + 1, 7) = y * v; +// A.at(2 * i + 1, 8) = v; +// } -// Motion3SolverResult motion_result; -// motion_result.status = result.status; +// // Solve Ah = 0 via SVD +// Mat w, u, vt; +// SVD::compute(A, w, u, vt, SVD::MODIFY_A); +// Mat h = +// vt.row(8).reshape(0, 3); // last row of V^T -> smallest singular value + +// // Denormalize: H = inv(T2) * h * T1 +// Mat H = Mat::zeros(3, 3, CV_64F); +// Mat Hn; +// h.convertTo(Hn, CV_64F); +// H = T2.inv() * Hn * T1; + +// // Normalize so H(2,2)=1 +// if (fabs(H.at(2, 2)) > 1e-12) H /= H.at(2, 2); +// return H; +// } -// if (result.status == TrackingStatus::VALID) { -// const gtsam::Pose3 G_w = result.best_result.inverse(); -// const gtsam::Pose3 H_w = T_world_k * G_w; -// result.best_result = H_w; +// // Decompose homography to possible poses and choose best using cheirality +// // (points in front) +// bool recoverPoseFromHomography(const Mat& H_in, const Mat& K, +// const vector& planePts3D, +// const vector& imagePts, Mat& bestR, +// Mat& bestT) { +// CV_Assert(H_in.size() == Size(3, 3)); +// Mat H; +// H_in.convertTo(H, CV_64F); + +// // OpenCV expects normalized homography such that H = K * [r1 r2 t] +// vector Rs, Ts, Ns; +// int solutions = decomposeHomographyMat(H, K, Rs, Ts, Ns); +// if (solutions == 0) return false; + +// int bestIdx = -1; +// int bestFront = -1; + +// // For each solution count number of points with positive depth +// for (int i = 0; i < solutions; i++) { +// Mat R = Rs[i]; +// Mat t = Ts[i]; + +// int frontCount = 0; +// vector proj; +// // Project 3D plane points with candidate pose +// projectPoints(planePts3D, R, t, K, Mat(), proj); +// for (size_t j = 0; j < proj.size(); j++) { +// // Reconstruct depth sign by transforming a 3D point and looking at z +// in +// // camera frame +// Mat X = (Mat_(3, 1) << planePts3D[j].x, planePts3D[j].y, +// planePts3D[j].z); +// Mat Xcam = R * X + t; +// if (Xcam.at(2, 0) > 0) frontCount++; +// } +// if (frontCount > bestFront) { +// bestFront = frontCount; +// bestIdx = i; +// } // } -// // if not valid, return motion result as is -// return motion_result; +// if (bestIdx < 0) return false; +// bestR = Rs[bestIdx].clone(); +// bestT = Ts[bestIdx].clone(); +// return true; // } -// // TODO: dont actually need all these variables -// Pose3SolverResult ObjectMotionSovler::motionModelOutlierRejection3d2d( -// const AbsolutePoseCorrespondences& dynamic_correspondences, -// Frame::Ptr frame_k_1, Frame::Ptr frame_k, const gtsam::Pose3& T_world_k, -// ObjectId object_id) { -// Pose3SolverResult result; +// bool poseFromHomograph(const std::vector& points3D, +// const std::vector& points2D, +// const cv::Mat& K, gtsam::Pose3& Gw) { +// // Ensure we have enough correspondences +// if (points3D.size() < 4 || points3D.size() != points2D.size()) { +// return false; +// } + +// // Convert 3D points to 2D by projecting onto XY plane (Z=0) +// std::vector points3D_2D; +// for (const auto& pt : points3D) { +// points3D_2D.push_back(cv::Point2f(pt.x, pt.y)); +// } + +// // Find homography matrix +// cv::Mat H = cv::findHomography(points3D_2D, points2D, cv::RANSAC, 3.0); -// // get object motions in previous frame (so k-2 to k-1) -// const MotionEstimateMap& motion_estiamtes_k_1 = -// frame_k_1->motion_estimates_; if (!motion_estiamtes_k_1.exists(object_id)) -// { -// result.status = TrackingStatus::INVALID; -// return result; +// if (H.empty()) { +// throw std::runtime_error("Failed to compute homography"); +// } + +// // Decompose homography to get rotation and translation +// // H = K * [r1 r2 t] where r1, r2 are first two columns of rotation matrix +// cv::Mat K_inv = K.inv(); + +// // Normalize: H_normalized = K^(-1) * H +// cv::Mat H_normalized = K_inv * H; + +// // Extract columns +// cv::Mat col1 = H_normalized.col(0); +// cv::Mat col2 = H_normalized.col(1); +// cv::Mat col3 = H_normalized.col(2); + +// // Normalize rotation columns +// double lambda1 = cv::norm(col1); +// double lambda2 = cv::norm(col2); +// double lambda = (lambda1 + lambda2) / 2.0; + +// cv::Mat r1 = col1 / lambda; +// cv::Mat r2 = col2 / lambda; +// cv::Mat t = col3 / lambda; + +// // Compute r3 as cross product of r1 and r2 +// cv::Mat r3 = r1.cross(r2); + +// // Build rotation matrix +// cv::Mat R = cv::Mat(3, 3, CV_64F); +// r1.copyTo(R.col(0)); +// r2.copyTo(R.col(1)); +// r3.copyTo(R.col(2)); + +// // Ensure R is a valid rotation matrix using SVD +// cv::Mat W, U, Vt; +// cv::SVD::compute(R, W, U, Vt); +// R = U * Vt; + +// // Convert OpenCV matrices to Eigen +// Eigen::Matrix3d R_eigen; +// Eigen::Vector3d t_eigen; + +// for (int i = 0; i < 3; i++) { +// for (int j = 0; j < 3; j++) { +// R_eigen(i, j) = R.at(i, j); +// } +// t_eigen(i) = t.at(i, 0); // } -// // previous motion model: k-2 to k-1 in w -// const gtsam::Pose3 motion_model = motion_estiamtes_k_1.at(object_id); +// // Create 4x4 homogeneous transformation matrix +// Eigen::Matrix4d transform = Eigen::Matrix4d::Identity(); +// transform.block<3, 3>(0, 0) = R_eigen; +// transform.block<3, 1>(0, 3) = t_eigen; + +// Gw = gtsam::Pose3(transform); +// return true; +// } + +// bool poseFromPnP(const std::vector& points3D, +// const std::vector& points2D, const cv::Mat& K, +// gtsam::Pose3& Gw, cv::Mat& inliers) { +// // Ensure we have enough correspondences +// if (points3D.size() < 4 || points3D.size() != points2D.size()) { +// return false; +// } -// using Calibration = Camera::CalibrationType; -// const auto calibration = -// camera_params_.constructGtsamCalibration(); +// // Output vectors +// Mat rvec1, tvec1; +// cv::Mat distCoeffs = cv::Mat::zeros(1, 4, CV_64FC1); +// cv::Mat inliers1; + +// // --- 1. Run solvePnPRansac --- +// bool success_ippe = +// solvePnPRansac(points3D, points2D, K, distCoeffs, rvec1, tvec1, +// false, // useExtrinsicGuess +// 500, // iterationsCount (number of RANSAC iterations) +// 0.4, // reprojectionError (max allowed error in +// pixels) 0.99, // confidence inliers1, // Output +// for inlier indices SOLVEPNP_IPPE // PnP method (EPnP is +// a good default) +// ); + +// Mat rvec2, tvec2, inliers2; +// bool success_epnp = +// solvePnPRansac(points3D, points2D, K, distCoeffs, rvec2, tvec2, +// false, // useExtrinsicGuess +// 500, // iterationsCount (number of RANSAC iterations) +// 0.4, // reprojectionError (max allowed error in +// pixels) 0.99, // confidence inliers2, // Output +// for inlier indices SOLVEPNP_AP3P // PnP method (EPnP is +// a good default) +// ); + +// Mat rvec, tvec; +// if (success_ippe && success_epnp) { +// LOG(INFO) << "Solved both object motions with ippe and epnp" +// " IPPE inliers: " +// << inliers1.rows << " EPnP inliers: " << inliers2.rows; +// ; + +// if (inliers1.rows > inliers2.rows) { +// // IPPE better +// rvec = rvec1; +// tvec = tvec1; +// inliers = inliers1; +// } else { +// rvec = rvec2; +// tvec = tvec2; +// inliers = inliers2; +// } +// } else if (success_ippe) { +// LOG(INFO) << "Only solved for IPPE"; +// rvec = rvec1; +// tvec = tvec1; +// inliers = inliers1; +// } else if (success_epnp) { +// LOG(INFO) << "Only solved for EPnP"; +// rvec = rvec2; +// tvec = tvec2; +// inliers = inliers2; +// } else { +// LOG(WARNING) << "Failed to solve object motion EPnP or IPPE"; +// return false; +// } -// auto I = gtsam::traits::Identity(); -// gtsam::PinholeCamera camera(I, calibration); +// // cout << "Pose calculation successful using " << inliers.rows << " +// inliers." +// // << endl; -// const double reprojection_error = params_.ransac_threshold_pnp; +// // --- 2. Convert rvec to a 3x3 Rotation Matrix (R) --- +// Mat R_mat; // OpenCV Mat for the 3x3 rotation matrix +// Rodrigues(rvec, R_mat); // rvec (Rodrigues form) -> R_mat (3x3 matrix) -// TrackletIds tracklets; -// TrackletIds inlier_tracklets; +// // --- 3. Assemble the Homogeneous Matrix (T) using Eigen --- +// Eigen::Matrix4d T_homo = +// Eigen::Matrix4d::Identity(); // Start with an Identity matrix -// utils::Accumulatord total_repr_error, inlier_repr_error; +// // Copy Rotation (R) from OpenCV Mat to Eigen 3x3 block +// for (int i = 0; i < 3; ++i) { +// for (int j = 0; j < 3; ++j) { +// // R_mat is CV_64F (double) by default from Rodrigues +// T_homo(i, j) = R_mat.at(i, j); +// } +// } -// const size_t& n_matches = dynamic_correspondences.size(); -// for (size_t i = 0u; i < n_matches; i++) { -// const AbsolutePoseCorrespondence& corres = dynamic_correspondences.at(i); -// tracklets.push_back(corres.tracklet_id_); +// // Copy Translation (t) from OpenCV Mat to Eigen 3x1 block +// T_homo(0, 3) = tvec.at(0, 0); // T_x +// T_homo(1, 3) = tvec.at(1, 0); // T_y +// T_homo(2, 3) = tvec.at(2, 0); // T_z -// const Keypoint& kp_k = corres.cur_; -// // the landmark int the world frame at k-1 -// const Landmark& w_lmk_k_1 = corres.ref_; +// Gw = gtsam::Pose3(T_homo); +// return true; +// } -// // using the motion, put the lmk in the world frame at k -// const Landmark w_lmk_k = motion_model * w_lmk_k_1; -// // using camera pose, put the lmk in the camera frame at k -// const Landmark c_lmk_c = T_world_k.inverse() * w_lmk_k; +// void testing::ExtendedKalmanFilterGTSAM::update( +// const vector& P_w_list, const vector& z_obs_list, +// const gtsam::Pose3& X_W_k) { +// if (P_w_list.empty() || P_w_list.size() != z_obs_list.size()) { +// cerr << "EKF Update skipped: Input lists are empty or mismatched." << +// endl; return; +// } +// const size_t num_measurements = P_w_list.size(); +// const size_t total_rows = +// num_measurements * 2; // Each measurement has 2 dimensions (u, v) + +// // Stacked Jacobian H (M x 6) and Stacked Residual y (M x 1) +// gtsam::Matrix H_stacked = gtsam::Matrix::Zero(total_rows, 6); +// gtsam::Vector y_stacked = gtsam::Vector::Zero(total_rows); + +// // Stacked Measurement Noise R_stacked (M x M) +// gtsam::Matrix R_stacked = gtsam::Matrix::Zero(total_rows, total_rows); +// // Calculate the Adjoint Matrix of X^{-1}, which links delta_h to +// delta_t_cw gtsam::Matrix Ad_X_inv = X_W_k.inverse().AdjointMap(); // 6x6 +// matrix + +// Pose3 G_w = X_W_k.inverse() * H_w_; +// gtsam::PinholePose camera(G_w.inverse(), K_gtsam_); +// // gtsam::Matrix Ad_X = X_W_k.AdjointMap(); // 6x6 matrix +// // 3. Calculate Correction Factor: J_corr = d(delta_t_wc)/d(delta_w) = +// // -Ad_{T_cw} This corrects the Jacobian returned by GTSAM (w.r.t. T_wc) to +// be +// // w.r.t. the state W. +// gtsam::Matrix Ad_G_w = G_w.AdjointMap(); +// gtsam::Matrix CorrectionFactor = -Ad_G_w; + +// for (size_t i = 0; i < num_measurements; ++i) { +// const gtsam::Point3& P_w = P_w_list[i]; +// const gtsam::Point2& z_obs = z_obs_list[i]; + +// gtsam::Matrix26 H_pose; // 2x6 Jacobian for this measurement (This was +// // missing/corrupted) + +// // J_pi: Jacobian of projection w.r.t. T_cw perturbation (delta_t_cw) +// gtsam::Matrix J_pi; // 2x6 matrix + +// // 1. Calculate Predicted Measurement (h(H)) and Jacobian (J_pi) +// gtsam::Point2 z_pred; // try { -// double repr_error = camera.reprojectionError(c_lmk_c, -// kp_k).squaredNorm(); - -// total_repr_error.Add(repr_error); -// if (repr_error < reprojection_error) { -// inlier_tracklets.push_back(corres.tracklet_id_); -// inlier_repr_error.Add(repr_error); -// } -// } catch (const gtsam::CheiralityException&) { +// // GTSAM's project() computes: z_pred = pi(T_cw * P_w), J_pi = +// // d(pi)/d(delta_t_cw) +// z_pred = camera.project(P_w, J_pi); +// } catch (const gtsam::CheiralityException& e) { +// // Handle points behind the camera +// cerr << "Warning: Point " << i << " behind camera. Skipping." << endl; +// continue; // } +// // 2. Calculate Residual (Error) y = z_obs - z_pred +// gtsam::Vector2 y_i = z_obs - z_pred; // 2x1 residual vector + +// // 3. Calculate EKF Jacobian: H_EKF = J_pi * Ad_X_inv +// // H_EKF = d(z)/d(delta_h) = (d(z)/d(delta_t_cw)) * +// // (d(delta_t_cw)/d(delta_h)) +// gtsam::Matrix H_ekf_i = J_pi * CorrectionFactor; // 2x6 matrix + +// // 3. Stack H and y +// H_stacked.block<2, 6>(i * 2, 0) = H_ekf_i; +// y_stacked.segment<2>(i * 2) = y_i; + +// // 4. Stack R (R_stacked is block diagonal with R) +// R_stacked.block<2, 2>(i * 2, i * 2) = R_; // } -// TrackletIds outliers; -// determineOutlierIds(inlier_tracklets, tracklets, outliers); +// // --- EKF Formulas using stacked matrices --- -// VLOG(20) << "(Object) motion model inliers/total(error) " -// << inlier_tracklets.size() << "(" << inlier_repr_error.Mean() -// << ") / " << tracklets.size() << "(" << total_repr_error.Mean() -// << ")"; +// // 1. Innovation Covariance (S) +// // S = H_stacked * P * H_stacked^T + R_stacked +// gtsam::Matrix S = H_stacked * P_ * H_stacked.transpose() + R_stacked; -// result.best_result = motion_model; -// result.inliers = inlier_tracklets; -// result.outliers = outliers; -// result.status = TrackingStatus::VALID; -// return result; +// // 2. Kalman Gain (K) +// // K = P * H_stacked^T * S^-1 +// // We use FullLinearSolver for robustness with potentially large S +// Eigen::LLT ldlt(S); +// gtsam::Matrix S_inv; +// { +// utils::TimingStatsCollector timer("ekfgtsam.inv"); +// S_inv = ldlt.solve(Eigen::MatrixXd::Identity(S.rows(), S.cols())); +// } +// // gtsam::Matrix S_inv = ldlt.solve(Eigen::MatrixXd::Identity(S.rows(), +// // S.cols())); gtsam::Matrix S_inv = S.inverse(); +// gtsam::Matrix K = P_ * H_stacked.transpose() * S_inv; // 6 x M + +// // 3. State Update (Perturbation Vector delta_x) +// // delta_x = K * y_stacked +// PerturbationVector delta_x = K * y_stacked; // 6 x 1 + +// // 4. State Retraction (Update T_cw) +// // T_new = T_old.retract(delta_x) +// // GTSAM's retract is the generalized exponential map on the manifold. +// H_w_ = H_w_.retract(delta_x); + +// // 5. Covariance Update +// // P = (I - K * H_stacked) * P +// gtsam::Matrix I = gtsam::Matrix66::Identity(); +// // P_ = (I - K * H_stacked) * P_; +// gtsam::Matrix I_KH = I - K * H_stacked; +// P_ = I_KH * P_ * I_KH.transpose() + K * R_stacked * K.transpose(); // } +// } // namespace testing + +SquareRootInfoFilterGTSAM::SquareRootInfoFilterGTSAM( + const gtsam::Pose3& initial_state_H, const gtsam::Matrix66& initial_P, + const gtsam::Cal3_S2::shared_ptr& K, const gtsam::Matrix22& R) + : H_linearization_point_(initial_state_H), K_gtsam_(K), R_noise_(R) { + Q_ = gtsam::Matrix66::Identity() * 1e-4; + + // 2. Initialize SRIF State (R, d) from EKF State (W, P) + // W_linearization_point_ is set to initial_state_W + // The initial perturbation is 0, so the initial d_info_ is 0. + d_info_ = gtsam::Vector6::Zero(); + + // Calculate initial Information Matrix Lambda = P^-1 + gtsam::Matrix66 Lambda_initial = initial_P.inverse(); + + // Calculate R_info_ from Cholesky decomposition: Lambda = R^T * R + // We use LLT (L*L^T) and take the upper-triangular factor of L^T. + // Or, more robustly, U^T*U from a U^T*U decomposition. + // Eigen's LLT computes L (lower) such that A = L * L^T. + // We need U (upper) such that A = U^T * U. + // A.llt().matrixU() gives U where A = L*L^T (U is L^T). No. + // A.llt().matrixL() gives L where A = L*L^T. + // Let's use Eigen's LDLT and reconstruct. + // A more direct way: + R_info_ = Lambda_initial.llt().matrixU(); // L^T +} + +/** + * @brief Recovers the state perturbation delta_w by solving R * delta_w = d. + */ +gtsam::Vector6 SquareRootInfoFilterGTSAM::getStatePerturbation() const { + // R is upper triangular, so this is a fast back-substitution + return R_info_.triangularView().solve(d_info_); +} + +const gtsam::Pose3& SquareRootInfoFilterGTSAM::getCurrentLinearization() const { + return H_linearization_point_; +} + +gtsam::Pose3 SquareRootInfoFilterGTSAM::getStatePoseW() const { + return H_linearization_point_.retract(getStatePerturbation()); +} + +gtsam::Matrix66 SquareRootInfoFilterGTSAM::getCovariance() const { + gtsam::Matrix66 Lambda = R_info_.transpose() * R_info_; + return Lambda.inverse(); +} + +gtsam::Matrix66 SquareRootInfoFilterGTSAM::getInformationMatrix() const { + return R_info_.transpose() * R_info_; +} + +void SquareRootInfoFilterGTSAM::predict() { + // 1. Get current mean state and covariance + gtsam::Vector6 delta_w = getStatePerturbation(); + gtsam::Pose3 H_current_mean = H_linearization_point_.retract(delta_w); + gtsam::Matrix66 P_current = getCovariance(); // Slow step + + // 2. Perform EKF prediction (add process noise) + // P_k = P_{k-1} + Q + gtsam::Matrix66 P_predicted = P_current + Q_; + + // 3. Re-linearize: Set new linearization point to the current mean + H_linearization_point_ = H_current_mean; + + // 4. Recalculate R and d based on new P and new linearization point + // The new perturbation is 0 relative to the new linearization point. + d_info_ = gtsam::Vector6::Zero(); + gtsam::Matrix66 Lambda_predicted = P_predicted.inverse(); + R_info_ = + Lambda_predicted.llt().matrixU(); // R_info_ = L^T where L*L^T = Lambda +} + +void SquareRootInfoFilterGTSAM::update( + const std::vector& P_w_list, + const std::vector& z_obs_list, const gtsam::Pose3& X_W_k, + const int num_irls_iterations) { + if (P_w_list.empty() || P_w_list.size() != z_obs_list.size()) { + LOG(WARNING) << "SRIF Update skipped: Input lists empty/mismatched."; + return; + } + + const size_t num_measurements = P_w_list.size(); + const size_t total_rows = num_measurements * 2; + const size_t state_dim = 6; + + gtsam::noiseModel::mEstimator::Huber::shared_ptr huber = + gtsam::noiseModel::mEstimator::Huber::Create(0.01); + + double huber_delta_ = 1.2; + + LOG(INFO) << "SRIF update with " << num_measurements << " measurements"; + + // 1. Calculate Jacobians (H) and Linearized Residuals (y_lin) + // These are calculated ONCE at the linearization point and are fixed + // for all IRLS iterations. + gtsam::Matrix H_stacked = gtsam::Matrix ::Zero(total_rows, state_dim); + gtsam::Vector y_linearized = gtsam::Vector::Zero(total_rows); + + // Pre-calculate constant values + gtsam::Pose3 G_w = X_W_k.inverse() * H_linearization_point_; + gtsam::Matrix6 Ad_G_w = G_w.AdjointMap(); + gtsam::Matrix6 CorrectionFactor = -Ad_G_w; + + const gtsam::Matrix22 R_inv = R_noise_.inverse(); + + // whitened error + gtsam::Vector initial_error = gtsam::Vector::Zero(num_measurements); + + gtsam::PinholeCamera camera(G_w.inverse(), *K_gtsam_); + for (size_t i = 0; i < num_measurements; ++i) { + const Point3& P_w = P_w_list[i]; + const Point2& z_obs = z_obs_list[i]; + gtsam::Matrix26 J_pi; // 2x6 + gtsam::Point2 z_pred; + try { + // Project using the *linearization point* + z_pred = camera.project(P_w, J_pi); + } catch (const gtsam::CheiralityException& e) { + LOG(WARNING) << "Warning: Point " << i << " behind camera. Skipping."; + continue; // Skip this measurement + } + + // Calculate linearized residual y_lin = z - h(x_nom) + gtsam::Vector2 y_i_lin = z_obs - z_pred; + + // Calculate EKF Jacobian H_i = J_pi * CorrectionFactor + Eigen::Matrix H_ekf_i = J_pi * CorrectionFactor; + + size_t row_idx = i * 2; + H_stacked.block<2, 6>(row_idx, 0) = H_ekf_i; + y_linearized.segment<2>(row_idx) = y_i_lin; + + double error_sq = y_i_lin.transpose() * R_inv * y_i_lin; + double error = std::sqrt(error_sq); + initial_error(i) = error; + } + + // 2. Store the prior information + gtsam::Matrix6 R_info_prior = R_info_; + gtsam::Vector6 d_info_prior = d_info_; + + gtsam::Vector re_weighted_error = gtsam::Vector::Zero(num_measurements); + + // 3. --- Start Iteratively Reweighted Least Squares (IRLS) Loop --- + // cout << "\n--- SRIF Robust Update (IRLS) Started ---" << endl; + for (int iter = 0; iter < num_irls_iterations; ++iter) { + // 3a. Get current state estimate (from previous iteration) + gtsam::Vector6 delta_w = + R_info_.triangularView().solve(d_info_); + gtsam::Pose3 H_current_mean = H_linearization_point_.retract(delta_w); + // Need to validate this: + // We intentionally do not recalculate the Jacobian block (H_stacked). + // to solve for the single best perturbation (delta_w) relative to the + // single linearization point (W_linearization_point_) that we had at the + // start of the update. + gtsam::Pose3 G_w = X_W_k.inverse() * H_current_mean; + PinholeCamera camera_current(G_w.inverse(), *K_gtsam_); + + gtsam::Vector weights = gtsam::Vector::Ones(num_measurements); + + for (size_t i = 0; i < num_measurements; ++i) { + const gtsam::Point3& P_w = P_w_list[i]; + const gtsam::Point2& z_obs = z_obs_list[i]; + gtsam::Point2 z_pred_current; + try { + z_pred_current = camera_current.project(P_w); + } catch (const gtsam::CheiralityException&) { + LOG(WARNING) << "Warning: Point " << i << " behind camera. Skipping."; + weights(i) = 0.0; // Skip point + continue; + } + + // Calculate non-linear residual + gtsam::Vector2 y_nonlinear = z_obs - z_pred_current; + + // //CHECK we cannot do this with gtsam's noise models (we can...) + // // Calculate Mahalanobis-like distance (whitened error) + double error_sq = y_nonlinear.transpose() * R_inv * y_nonlinear; + double error = std::sqrt(error_sq); + + re_weighted_error(i) = error; + + // Calculate Huber weight w(e) = min(1, delta / |e|) + weights(i) = (error <= huber_delta_) ? 1.0 : huber_delta_ / error; + if (/*weights(i) < 0.99 &&*/ iter == num_irls_iterations - 1) { + VLOG(20) << " [Meas " << i << "] Final Weight: " << weights(i) + << " (Error: " << error << ")"; + } + } + + // 3c. Construct the giant matrix A_qr for this iteration + gtsam::Matrix A_qr = + gtsam::Matrix::Zero(total_rows + state_dim, state_dim + 1); + + for (size_t i = 0; i < num_measurements; ++i) { + double w_i = weights(i); + if (w_i < 1e-6) continue; // Skip 0-weighted points + + // R_robust = R / w_i (Note: w is |e|^-1, so R_robust = R * |e|/delta) + // R_robust_inv = R_inv * w_i + // We need W_i such that W_i^T * W_i = R_robust_inv + // W_i = sqrt(w_i) * R_inv_sqrt + + gtsam::Matrix22 R_robust_i = R_noise_ / w_i; + gtsam::Matrix22 L_robust_i = R_robust_i.llt().matrixL(); + gtsam::Matrix22 R_robust_inv_sqrt = L_robust_i.inverse(); + + size_t row_idx = i * 2; + + // Fill matrix [ W_i * H | W_i * y_lin ] + A_qr.block<2, 6>(row_idx, 0) = + R_robust_inv_sqrt * H_stacked.block<2, 6>(row_idx, 0); + A_qr.block<2, 1>(row_idx, 6) = + R_robust_inv_sqrt * y_linearized.segment<2>(row_idx); + } + + // 3d. Fill the prior part of the QR matrix + // This is *always* the original prior + A_qr.block<6, 6>(total_rows, 0) = R_info_prior; + A_qr.block<6, 1>(total_rows, 6) = d_info_prior; + + // 3e. Perform QR decomposition + Eigen::HouseholderQR qr(A_qr); + gtsam::Matrix R_full = qr.matrixQR().triangularView(); + + // 3f. Extract the new R_info and d_info for the *next* iteration + R_info_ = R_full.block<6, 6>(0, 0); + d_info_ = R_full.block<6, 1>(0, 6); + } + + // (Optional) Enforce R is upper-triangular + R_info_ = R_info_.triangularView(); + + LOG(INFO) << "--- SRIF Robust Update Complete --- initial error " + << initial_error.norm() + << " re-weighted error: " << re_weighted_error.norm(); +} + +/////////////////////////////////////////////////////////////////////////// + +HybridObjectMotionSRIF::HybridObjectMotionSRIF( + const gtsam::Pose3& initial_state_H, const gtsam::Pose3& L_e, + const FrameId& frame_id_e, const gtsam::Matrix66& initial_P, + const gtsam::Matrix66& Q, const gtsam::Matrix33& R, Camera::Ptr camera, + double huber_k) + : H_linearization_point_(initial_state_H), + Q_(Q), + R_noise_(R), + R_inv_(R.inverse()), + initial_P_(initial_P), + huber_k_(huber_k) { + // set camera + rgbd_camera_ = CHECK_NOTNULL(camera)->safeGetRGBDCamera(); + CHECK(rgbd_camera_); + stereo_calibration_ = rgbd_camera_->getFakeStereoCalib(); + + resetState(L_e, frame_id_e); +} + +/** + * @brief Recovers the state perturbation delta_w by solving R * delta_w = d. + */ +gtsam::Vector6 HybridObjectMotionSRIF::getStatePerturbation() const { + // R is upper triangular, so this is a fast back-substitution + return R_info_.triangularView().solve(d_info_); +} + +const gtsam::Pose3& HybridObjectMotionSRIF::getCurrentLinearization() const { + return H_linearization_point_; +} + +gtsam::Pose3 HybridObjectMotionSRIF::getKeyFramedMotion() const { + return H_linearization_point_.retract(getStatePerturbation()); +} + +gtsam::Pose3 HybridObjectMotionSRIF::getF2FMotion() const { + gtsam::Pose3 H_W_e_k = H_linearization_point_.retract(getStatePerturbation()); + gtsam::Pose3 H_W_e_km1 = previous_H_; + return H_W_e_k * H_W_e_km1.inverse(); +} + +gtsam::Matrix66 HybridObjectMotionSRIF::getCovariance() const { + gtsam::Matrix66 Lambda = R_info_.transpose() * R_info_; + return Lambda.inverse(); +} + +gtsam::Matrix66 HybridObjectMotionSRIF::getInformationMatrix() const { + return R_info_.transpose() * R_info_; +} + +void HybridObjectMotionSRIF::predict(const gtsam::Pose3&) { + // 1. Get current mean state and covariance + gtsam::Vector6 delta_w = getStatePerturbation(); + gtsam::Pose3 H_current_mean = H_linearization_point_.retract(delta_w); + gtsam::Matrix66 P_current = getCovariance(); // Slow step + + // call before updating the previous_H_ + // previous motion -> assume constant! + gtsam::Pose3 H_W_km1_k = getF2FMotion(); + + // gtsam::Pose3 L_km1 = previous_H_ * L_e_; + // gtsam::Pose3 L_k = H_current_mean * L_e_; + + // //calculate motion relative to object + // gtsam::Pose3 local_motion = L_km1.inverse() * L_k; + // // forward predict pose using constant velocity model + // gtsam::Pose3 predicted_pose = L_k * local_motion; + // // convert pose back to motion + // gtsam::Pose3 predicted_motion = predicted_pose * L_e_.inverse(); + + previous_H_ = H_current_mean; + + // 2. Perform EKF prediction (add process noise) + // P_k = P_{k-1} + Q + gtsam::Matrix66 P_predicted = P_current + Q_; + + // 3. Re-linearize: Set new linearization point to the current mean + H_linearization_point_ = H_W_km1_k * H_current_mean; + // predict forward with constant velocity model + // H_linearization_point_ = predicted_motion; + + // 4. Recalculate R and d based on new P and new linearization point + // The new perturbation is 0 relative to the new linearization point. + d_info_ = gtsam::Vector6::Zero(); + gtsam::Matrix66 Lambda_predicted = P_predicted.inverse(); + R_info_ = + Lambda_predicted.llt().matrixU(); // R_info_ = L^T where L*L^T = Lambda +} + +HybridObjectMotionSRIF::Result HybridObjectMotionSRIF::update( + Frame::Ptr frame, const TrackletIds& tracklets, + const int num_irls_iterations) { + const size_t num_measurements = tracklets.size(); + const size_t total_rows = num_measurements * ZDim; + + const gtsam::Pose3& X_W_k = frame->getPose(); + const gtsam::Pose3 X_W_k_inv = X_W_k.inverse(); + X_K_ = X_W_k; + + // 1. Calculate Jacobians (H) and Linearized Residuals (y_lin) + // These are calculated ONCE at the linearization point and are fixed + // for all IRLS iterations. + gtsam::Matrix H_stacked = gtsam::Matrix ::Zero(total_rows, StateDim); + gtsam::Vector y_linearized = gtsam::Vector::Zero(total_rows); + + const gtsam::Pose3 A = X_W_k_inv * H_linearization_point_; + // G will project the point from the object frame into the camera frame + const gtsam::Pose3 G_w = A * L_e_; + const gtsam::Matrix6 J_correction = -A.AdjointMap(); + + gtsam::StereoCamera gtsam_stereo_camera(G_w.inverse(), stereo_calibration_); + // whitened error + gtsam::Vector initial_error = gtsam::Vector::Zero(num_measurements); + gtsam::Vector re_weighted_error = gtsam::Vector::Zero(num_measurements); + + size_t num_new_features = 0u, num_tracked_features = 0u; + + for (size_t i = 0; i < num_measurements; ++i) { + const TrackletId& tracklet_id = tracklets.at(i); + const Feature::Ptr feature = frame->at(tracklet_id); + CHECK(feature); + + if (!m_linearized_.exists(tracklet_id)) { + // this is initalising from a predicted motion (bad!) Should use previous + // linearization (ie k-1) + const gtsam::Point3 m_X_k = frame->backProjectToCamera(tracklet_id); + Landmark m_init = HybridObjectMotion::projectToObject3( + X_W_k, H_linearization_point_, L_e_, m_X_k); + m_linearized_.insert2(tracklet_id, m_init); + // VLOG(10) << "Initalising new point i=" << tracklet_id << " Le " << + // L_e_; + num_new_features++; + } else { + num_tracked_features++; + } + + gtsam::Point3 m_L = m_linearized_.at(tracklet_id); + + const auto [stereo_keypoint_status, stereo_measurement] = + rgbd_camera_->getStereo(feature); + if (!stereo_keypoint_status) { + continue; + } + + const auto& z_obs = stereo_measurement; + + // const Point2& z_obs = feature->keypoint(); + gtsam::Matrix36 J_pi; // 2x6 + gtsam::StereoPoint2 z_pred; + try { + // Project using the *linearization point* + // z_pred = camera.project(m_L, J_pi); + z_pred = gtsam_stereo_camera.project2(m_L, J_pi); + } catch (const gtsam::StereoCheiralityException& e) { + LOG(WARNING) << "Warning: Point " << i << " behind camera. Skipping."; + continue; // Skip this measurement + } + + // Calculate linearized residual y_lin = z - h(x_nom) + gtsam::Vector3 y_i_lin = (z_obs - z_pred).vector(); + Eigen::Matrix H_ekf_i = J_pi * J_correction; + + size_t row_idx = i * ZDim; + + H_stacked.block(row_idx, 0) = H_ekf_i; + y_linearized.segment(row_idx) = y_i_lin; + + double error_sq = y_i_lin.transpose() * R_inv_ * y_i_lin; + double error = std::sqrt(error_sq); + initial_error(i) = error; + } + + VLOG(30) << "Feature stats. New features: " << num_new_features << "/" + << num_measurements << " Tracked features " << num_tracked_features + << "/" << num_measurements; + + // 2. Store the prior information + gtsam::Matrix6 R_info_prior = R_info_; + gtsam::Vector6 d_info_prior = d_info_; + + // 3. --- Start Iteratively Reweighted Least Squares (IRLS) Loop --- + // cout << "\n--- SRIF Robust Update (IRLS) Started ---" << endl; + for (int iter = 0; iter < num_irls_iterations; ++iter) { + // 3a. Get current state estimate (from previous iteration) + const gtsam::Vector6 delta_w = + R_info_.triangularView().solve(d_info_); + const gtsam::Pose3 H_current_mean = H_linearization_point_.retract(delta_w); + // Need to validate this: + // We intentionally do not recalculate the Jacobian block (H_stacked). + // to solve for the single best perturbation (delta_w) relative to the + // single linearization point (W_linearization_point_) that we had at the + // start of the update. + const gtsam::Pose3 A = X_W_k_inv * H_current_mean; + const gtsam::Pose3 G_w = A * L_e_; + gtsam::StereoCamera gtsam_stereo_camera_current(G_w.inverse(), + stereo_calibration_); + + gtsam::Vector weights = gtsam::Vector::Ones(num_measurements); + + for (size_t i = 0; i < num_measurements; ++i) { + const TrackletId& tracklet_id = tracklets.at(i); + const Feature::Ptr feature = frame->at(tracklet_id); + CHECK(feature); + + const auto [stereo_keypoint_status, stereo_measurement] = + rgbd_camera_->getStereo(feature); + if (!stereo_keypoint_status) { + weights(i) = 0.0; // Skip point + continue; + } + + auto z_obs = stereo_measurement; + + gtsam::Point3 m_L = m_linearized_.at(tracklet_id); + + gtsam::StereoPoint2 z_pred_current; + try { + z_pred_current = gtsam_stereo_camera_current.project(m_L); + } catch (const gtsam::StereoCheiralityException& e) { + LOG(WARNING) << "Warning: Point " << i << " behind camera. Skipping."; + weights(i) = 0.0; // Skip point + continue; + } + + // Calculate non-linear residual + gtsam::Vector3 y_nonlinear = (z_obs - z_pred_current).vector(); + + // //CHECK we cannot do this with gtsam's noise models (we can...) + // // Calculate Mahalanobis-like distance (whitened error) + double error_sq = y_nonlinear.transpose() * R_inv_ * y_nonlinear; + double error = std::sqrt(error_sq); + + re_weighted_error(i) = error; + + // Calculate Huber weight w(e) = min(1, delta / |e|) + weights(i) = (error <= huber_k_) ? 1.0 : huber_k_ / error; + if (/*weights(i) < 0.99 &&*/ iter == num_irls_iterations - 1) { + VLOG(50) << " [Meas " << i << "] Final Weight: " << weights(i) + << " (Error: " << error << ")"; + } + } + + // 3c. Construct the giant matrix A_qr for this iteration + gtsam::Matrix A_qr = + gtsam::Matrix::Zero(total_rows + StateDim, StateDim + 1); + + for (size_t i = 0; i < num_measurements; ++i) { + double w_i = weights(i); + if (w_i < 1e-6) continue; // Skip 0-weighted points + + // R_robust = R / w_i (Note: w is |e|^-1, so R_robust = R * |e|/delta) + // R_robust_inv = R_inv * w_i + // We need W_i such that W_i^T * W_i = R_robust_inv + // W_i = sqrt(w_i) * R_inv_sqrt + + gtsam::Matrix33 R_robust_i = R_noise_ / w_i; + gtsam::Matrix33 L_robust_i = R_robust_i.llt().matrixL(); + gtsam::Matrix33 R_robust_inv_sqrt = L_robust_i.inverse(); + + size_t row_idx = i * ZDim; + + A_qr.block(row_idx, 0) = + R_robust_inv_sqrt * H_stacked.block<3, StateDim>(row_idx, 0); + A_qr.block(row_idx, StateDim) = + R_robust_inv_sqrt * y_linearized.segment<3>(row_idx); + } + + // 3d. Fill the prior part of the QR matrix + // This is *always* the original prior + A_qr.block(total_rows, 0) = R_info_prior; + A_qr.block(total_rows, StateDim) = d_info_prior; + + // 3e. Perform QR decomposition + Eigen::HouseholderQR qr(A_qr); + gtsam::Matrix R_full = qr.matrixQR().triangularView(); + + // 3f. Extract the new R_info and d_info for the *next* iteration + R_info_ = R_full.block(0, 0); + d_info_ = R_full.block(0, StateDim); + } + + // (Optional) Enforce R is upper-triangular + R_info_ = R_info_.triangularView(); + + Result result; + result.error = initial_error.norm(); + result.reweighted_error = re_weighted_error.norm(); + return result; + + // LOG(INFO) << "--- SRIF Robust Update Complete --- initial error " + // << initial_error.norm() + // << " re-weighted error: " << re_weighted_error.norm(); +} + +void HybridObjectMotionSRIF::resetState(const gtsam::Pose3& L_e, + FrameId frame_id_e) { + // 2. Initialize SRIF State (R, d) from EKF State (W, P) + // W_linearization_point_ is set to initial_state_W + // The initial perturbation is 0, so the initial d_info_ is 0. + d_info_ = gtsam::Vector6::Zero(); + + // Calculate initial Information Matrix Lambda = P^-1 + gtsam::Matrix66 Lambda_initial = initial_P_.inverse(); + + // Calculate R_info_ from Cholesky decomposition: Lambda = R^T * R + // We use LLT (L*L^T) and take the upper-triangular factor of L^T. + // Or, more robustly, U^T*U from a U^T*U decomposition. + // Eigen's LLT computes L (lower) such that A = L * L^T. + // We need U (upper) such that A = U^T * U. + // A.llt().matrixU() gives U where A = L*L^T (U is L^T). No. + // A.llt().matrixL() gives L where A = L*L^T. + // Let's use Eigen's LDLT and reconstruct. + // A more direct way: + R_info_ = Lambda_initial.llt().matrixU(); // L^T + + // will this give us discontinuinities betweek keyframes? + // reset other variables + previous_H_ = gtsam::Pose3::Identity(); + L_e_ = L_e; + frame_id_e_ = frame_id_e; + X_K_ = gtsam::Pose3::Identity(); + + keyframe_history.push_back(L_e); + + m_linearized_.clear(); + + // should always be identity since the deviation from L_e = I when frame = e + // the initial H should not be parsed into the constructor by this logic as + // well! + H_linearization_point_ = gtsam::Pose3::Identity(); +} + +ObjectMotionSolverFilter::ObjectMotionSolverFilter( + const ObjectMotionSolverFilter::Params& params, + const CameraParams& camera_params) + : EgoMotionSolver(static_cast(params), + camera_params), + filter_params_(params) {} + +bool ObjectMotionSolverFilter::solveImpl(Frame::Ptr frame_k, + Frame::Ptr frame_k_1, + ObjectId object_id, + MotionEstimateMap& motion_estimates) { + AbsolutePoseCorrespondences dynamic_correspondences; + // get the corresponding feature pairs + bool corr_result = frame_k->getDynamicCorrespondences( + dynamic_correspondences, *frame_k_1, object_id, + frame_k->landmarkWorldKeypointCorrespondance()); + + // FeaturePairs correspondences; + // frame_k->getDynamicCorrespondences(correspondences, *frame_k_1, object_id); + + const size_t& n_matches = dynamic_correspondences.size(); + + cv::Mat rgb = frame_k->image_container_.rgb(); + cv::Mat viz; + rgb.copyTo(viz); + + // TrackletIds all_tracklets(n_matches); + + TrackletIds all_tracklets; + std::transform(dynamic_correspondences.begin(), dynamic_correspondences.end(), + std::back_inserter(all_tracklets), + [](const AbsolutePoseCorrespondence& corres) { + return corres.tracklet_id_; + }); + CHECK_EQ(all_tracklets.size(), n_matches); + + Pose3SolverResult geometric_result = + EgoMotionSolver::geometricOutlierRejection3d2d(dynamic_correspondences); + + auto gtsam_camera_calibration = frame_k->camera_->getGtsamCalibration(); + + auto construct_initial_frame = + [](const Frame::Ptr frame, const TrackletIds& tracklets) -> gtsam::Pose3 { + // L_e + gtsam::Point3 initial_object_frame( + 0, 0, 0); // important to initliase with zero values (otherwise nan's!) + size_t count = 0; + for (TrackletId tracklet : tracklets) { + const Feature::Ptr feature = frame->at(tracklet); + CHECK_NOTNULL(feature); + + gtsam::Point3 lmk = frame->backProjectToCamera(feature->trackletId()); + initial_object_frame += lmk; + + count++; + } + + initial_object_frame /= count; + initial_object_frame = frame->getPose() * initial_object_frame; + return gtsam::Pose3(gtsam::Rot3::Identity(), initial_object_frame); + }; + + //# if retracked - reset filter (HACk) + // what happens when re-track - initial point values are wrong (this is maybe + // the case when we loos all points!) what about in the OMD case? we get a + // bunch more points but now linearization of points is wrong!? WOW okay this + // fixed all the things!!!!! + // TODO: repeated information in the FeatureTrackingInfo (which one to use!!) + auto it = std::find(frame_k->retracked_objects_.begin(), + frame_k->retracked_objects_.end(), object_id); + + // TODO: refine complex logic! + const bool object_resampled = it != frame_k->retracked_objects_.end(); + const bool object_in_previous = + frame_k_1->tracking_info_->dynamic_track.exists(object_id); + const bool object_in_filter = filters_.exists(object_id); + + const bool object_new_in_previous = + object_in_previous && + frame_k_1->tracking_info_->dynamic_track.at(object_id).object_new; + const bool object_new = !filters_.exists(object_id); + + // TODO: object is new is not getting set!!! (this should be for when the + // object is re-tracked too!!!) + // maybe somehow its in the previous frame!!? + // this is becuase when an object is new it just has sampled points and no + // tracked points and therefore no points go to the backend and therefore we + // do not update it!! therefore the ObjectTrack object wont exist!! + + bool new_or_reset_object = false; + // bool new_object = false; + // bool object_reset = false; + if (object_resampled && object_in_filter) { + LOG(INFO) << object_id + << " retracked - resetting filter k=" << frame_k->getFrameId(); + // Not thread safe! + // filters_.erase(object_id); + // new object false so we dont + // new_object = true; + new_or_reset_object = true; + gtsam::Pose3 keyframe_pose = + construct_initial_frame(frame_k_1, geometric_result.inliers); + + auto filter = filters_.at(object_id); + // must set value before reset to get the last value + // NOTE: we currently KF to k-1 so this will be a little bit wrong!!! + // assume that we only take this value if the object was retracked!! + // as we dont set it anywhere else!! + filter->H_W_e_k_before_reset = filter->getCurrentLinearization(); + // filter->frame_id_e_before_reset = filter->getKeyFrameId(); + filter->frame_id_e_before_reset = filter->getKeyFrameId(); + + // instead of using new pose (use last motion) (if last frame id was k-1?) + // we start to drift from the center of the object!! + // keyframe_pose = filter->getCurrentLinearization() * + // filter->getKeyFramePose(); filters_.erase(object_id); + filter->resetState(keyframe_pose, frame_k_1->getFrameId()); + filter->frame_id_e_ = frame_k->getFrameId(); + frame_k->tracking_info_->dynamic_track.at(object_id).object_resampled = + true; + } + + // // hack to handle object new is to update it from fame_k-1! + // if(object_new_in_previous) { + // LOG(INFO) << "Object " << object_id << " was new in previous - + // clearing!"; + // // //TODO: currently only setting both object new and object resampled to + // true + // // // becuause logic in frontend says if new then must be resampled (not + // sure how to handle this yet!) + // // frame_k->tracking_info_->dynamic_track.at(object_id).object_new = + // true; + // frame_k->tracking_info_->dynamic_track.at(object_id).object_resampled + // = + // true; + // filters_.erase(object_id); + // } + + // becuuse we erase it if object new in previous! + // not sure this is the best way to handle reappearing objects! + if (!filters_.exists(object_id)) { + new_or_reset_object = true; + + // update new object tracking + // the tracking cannot do it (for now) as this is delayed one frame with the + // backend! frame_k->tracking_info_->dynamic_track.at(object_id).object_new + // = true; + // frame_k->tracking_info_->dynamic_track.at(object_id).object_resampled = + // true; + + // gtsam::Vector3 noise; + // noise << 1.0, 1.0, 3.0; + // gtsam::Matrix33 R = noise.array().matrix().asDiagonal(); + gtsam::Matrix33 R = gtsam::Matrix33::Identity() * 1.0; + + // Initial State Covariance P (6x6) + gtsam::Matrix66 P = gtsam::Matrix66::Identity() * 0.3; + + // Process Model noise (6x6) + gtsam::Matrix66 Q = gtsam::Matrix66::Identity() * 0.2; + + if (geometric_result.inliers.size() < 4) { + LOG(WARNING) << "Could not make initial frame for object " << object_id + << " as not enough inlier tracks!"; + return false; + } + + frame_k->tracking_info_->dynamic_track.at(object_id).object_new = true; + frame_k->tracking_info_->dynamic_track.at(object_id).object_resampled = + true; + + // keyframe at k not k-1 + // this means we drop the information at k-1 but due to the system design + // we cannot send k-1 and k to the backend + // and now the front-end and backend need to be synchronized + gtsam::Pose3 keyframe_pose = + construct_initial_frame(frame_k_1, geometric_result.inliers); + + constexpr static double kHuberKFilter = 0.1; + filters_.insert2(object_id, std::make_shared( + gtsam::Pose3::Identity(), keyframe_pose, + frame_k_1->getFrameId(), P, Q, R, + frame_k_1->getCamera(), kHuberKFilter)); + // fake KF is this frame + filters_.at(object_id)->frame_id_e_ = frame_k->getFrameId(); + } + auto filter = filters_.at(object_id).get(); + + // std::vector object_points; + // std::vector image_points; + + // for (TrackletId inlier_tracklet : geometric_result.inliers) { + // const Feature::Ptr feature_k_1 = frame_k_1->at(inlier_tracklet); + // const Feature::Ptr feature_k = frame_k->at(inlier_tracklet); + // CHECK_NOTNULL(feature_k_1); + // CHECK_NOTNULL(feature_k); + + // const Keypoint kp_k = feature_k->keypoint(); + // const gtsam::Point3 lmk_k_1_world = + // frame_k_1->backProjectToWorld(inlier_tracklet); + + // object_points.push_back(lmk_k_1_world); + // image_points.push_back(kp_k); + // } + + // update and predict should be one step so that if we dont have enough points + // NOTE: this logic seemed pretty important to ensure the estimate was good!!! + // we dont predict? + if (new_or_reset_object) { + filter->predict(gtsam::Pose3::Identity()); + { + utils::TimingStatsCollector timer("motion_solver.ekf_update"); + // first time we should run this twice (for k-1 and k)! + filter->update(frame_k_1, geometric_result.inliers, 2); + // filter->updateStereo(object_points, stereo_measurements, + // frame_k->getPose()); + } + } + + // on the first frame we want to init everything (ie. points) + // so that they align with the keyframe but dont need to update anyting + // as the motion should be identity!!! + gtsam::Pose3 G_w_inv_pnp = geometric_result.best_result.inverse(); + gtsam::Pose3 H_w_km1_k_pnp = frame_k->getPose() * G_w_inv_pnp; + // predict with identity if mew + // if(new_or_reset_object) { + // H_w_km1_k_pnp = gtsam::Pose3::Identity(); + // } + + // only estimate if new - otherwise motion is identity as this is the current + // keyframe! + // if(!new_or_reset_object) { + + filter->predict(H_w_km1_k_pnp); + { + utils::TimingStatsCollector timer("motion_solver.ekf_update"); + // first time we should run this twice (for k-1 and k)! + filter->update(frame_k, geometric_result.inliers, 2); + // filter->updateStereo(object_points, stereo_measurements, + // frame_k->getPose()); + } + // } + + // gtsam::Pose3 G_w; + // cv::Mat inliers_ransac; + // bool homography_result = + // testing::poseFromPnP(object_points, image_points, K, G_w, + // inliers_ransac); + + bool homography_result = false; + + if (geometric_result.status == TrackingStatus::VALID) { + TrackletIds refined_inlier_tracklets = geometric_result.inliers; + // if (homography_result) { + // gtsam::Pose3 G_w = geometric_result.best_result.inverse(); + // gtsam::Pose3 H_w = filter->getPose(); + gtsam::Pose3 H_w_filter = filter->getF2FMotion(); + gtsam::Pose3 G_w_filter_inv = + (frame_k->getPose().inverse() * H_w_filter).inverse(); + // gtsam::Pose3 H_w = frame_k->getPose() * G_w; + + // OpticalFlowAndPoseOptimizer flow_optimizer( + // object_motion_params.joint_of_params); + + // auto flow_opt_result = flow_optimizer.optimizeAndUpdate( + // frame_k_1, frame_k, refined_inlier_tracklets, + // G_w_filter_inv); + + // // still need to take the inverse as we get the inverse of G out + // const auto G_w_flow = flow_opt_result.best_result.refined_pose.inverse(); + // // inliers should be a subset of the original refined inlier tracks + // refined_inlier_tracklets = flow_opt_result.inliers; + // gtsam::Pose3 H_w = frame_k->getPose() * G_w_flow; + + gtsam::Pose3 H_w = H_w_filter; + + // camera at frame_k->getPose() + auto gtsam_camera = frame_k->getFrameCamera(); + + // calcuate reprojection error + // double inlier_error = 0, outlier_error = 0; + // int inlier_count = 0, outlier_count = 0; + // for(const AbsolutePoseCorrespondence& corr : dynamic_correspondences) { + // gtsam::Point3 lmk_W_k_1 = corr.ref_; + // gtsam::Point3 lmk_W_k = H_w * lmk_W_k_1; + // Keypoint kp_k_measured = corr.cur_; + + // Keypoint kp_k_projected = gtsam_camera.project2(lmk_W_k); + // double repr = (kp_k_measured - kp_k_projected).norm(); + + // cv::Scalar colour; + + // auto it = std::find(geometric_result.inliers.begin(), + // geometric_result.inliers.end(), corr.tracklet_id_); if(it != + // geometric_result.inliers.end()) { + // //inlier + // inlier_error += repr; + // inlier_count++; + // colour = Color::green().bgra(); + // } + // else { + // //outlier + // outlier_error += repr; + // outlier_count++; + // colour = Color::red().bgra(); + // } + // cv::arrowedLine(viz, utils::gtsamPointToCv(kp_k_measured), + // utils::gtsamPointToCv(kp_k_projected), colour, 1, 8, 0, + // 0.1); + // cv::circle(viz, utils::gtsamPointToCv(kp_k_measured), 2, colour, -1); + // } + + // LOG(INFO) << "Inlier repr " << inlier_error/(double)inlier_count << + // " outlier rpr " << outlier_error/(double)outlier_count; + + // cv::imshow("Inlier/Outlier", viz); + // cv::waitKey(0); + + Motion3SolverResult motion_result; + motion_result.status = geometric_result.status; + motion_result.inliers = geometric_result.inliers; + motion_result.outliers = geometric_result.outliers; + // motion_result.inliers = refined_inlier_tracklets; + // determineOutlierIds(motion_result.inliers, all_tracklets, + // motion_result.outliers); + + motion_result.best_result = Motion3ReferenceFrame( + H_w, Motion3ReferenceFrame::Style::F2F, ReferenceFrame::GLOBAL, + frame_k_1->getFrameId(), frame_k->getFrameId()); + + frame_k->dynamic_features_.markOutliers(motion_result.outliers); + motion_estimates.insert({object_id, motion_result.best_result}); + return true; + } else { + return false; + } +} + +void ObjectMotionSolverFilter::updatePoses( + ObjectPoseMap& object_poses, const MotionEstimateMap& motion_estimates, + Frame::Ptr frame_k, Frame::Ptr /*frame_k_1*/) { + const FrameId frame_id_k = frame_k->getFrameId(); + for (const auto& [object_id, _] : motion_estimates) { + CHECK(filters_.exists(object_id)); + + auto filter = filters_.at(object_id); + gtsam::Pose3 L_k_j = + filter->getKeyFramedMotion() * filter->getKeyFramePose(); + object_poses_.insert22(object_id, frame_id_k, L_k_j); + } + object_poses = object_poses_; +} + +void ObjectMotionSolverFilter::updateMotions( + ObjectMotionMap& object_motions, const MotionEstimateMap& motion_estimates, + Frame::Ptr frame_k, Frame::Ptr frame_k_1) { + // same as ObjectMotionSovlerF2F + const FrameId frame_id_k = frame_k->getFrameId(); + for (const auto& [object_id, motion_reference_frame] : motion_estimates) { + object_motions_.insert22(object_id, frame_id_k, motion_reference_frame); + } + object_motions = object_motions_; +} + } // namespace dyno diff --git a/dynosam/src/frontend/vision/StaticFeatureTracker.cc b/dynosam/src/frontend/vision/StaticFeatureTracker.cc index cea600a9b..8a3268b64 100644 --- a/dynosam/src/frontend/vision/StaticFeatureTracker.cc +++ b/dynosam/src/frontend/vision/StaticFeatureTracker.cc @@ -615,6 +615,7 @@ bool KltFeatureTracker::trackPoints(const cv::Mat& current_processed_img, // if we do not have enough features, detect more on the current image detectFeatures(current_processed_img, image_container, tracked_features, tracked_features, detection_mask); + tracker_info.new_static_detections = true; const auto n_detected = tracked_features.size() - n_tracked; tracker_info.static_track_detections += n_detected; diff --git a/dynosam/src/pipeline/PipelineBase.cc b/dynosam/src/pipeline/PipelineBase.cc index c666ca38b..7aa40e8e4 100644 --- a/dynosam/src/pipeline/PipelineBase.cc +++ b/dynosam/src/pipeline/PipelineBase.cc @@ -45,8 +45,8 @@ bool PipelineBase::spin() { LOG(INFO) << "Starting module " << module_name_; while (!isShutdown()) { spinOnce(); - // using namespace std::chrono_literals; - // std::this_thread::sleep_for(1ms); // give CPU thread some sleep + using namespace std::chrono_literals; + std::this_thread::sleep_for(5ns); // give CPU thread some sleep // time... //TODO: only if threaded? } return true; diff --git a/dynosam/src/pipeline/PipelineManager.cc b/dynosam/src/pipeline/PipelineManager.cc index 3e13217d2..309d4685c 100644 --- a/dynosam/src/pipeline/PipelineManager.cc +++ b/dynosam/src/pipeline/PipelineManager.cc @@ -39,6 +39,8 @@ #include "dynosam_opt/Map.hpp" DEFINE_bool(use_backend, false, "If any backend should be initalised"); +DEFINE_bool(use_opencv_display, true, + "If true, the OpenCVImageDisplayQueue will be processed"); namespace dyno { @@ -210,10 +212,9 @@ bool DynoPipelineManager::spin() { } bool DynoPipelineManager::spinViz() { - // if() - displayer_ - .process(); // when enabled this gives a segafault when the process ends. - // when commented out the program just waits at thee end + if (FLAGS_use_opencv_display) { + displayer_.process(); + } return true; } @@ -252,6 +253,7 @@ void DynoPipelineManager::loadPipelines(const CameraParams& camera_params, FrontendDisplay::Ptr frontend_display, BackendDisplay::Ptr backend_display, BackendModuleFactory::Ptr factory) { + FrontendModule::Ptr frontend = nullptr; BackendModule::Ptr backend = nullptr; BackendModuleDisplay::Ptr additional_backend_display = nullptr; // the registra for the frontend pipeline @@ -266,7 +268,6 @@ void DynoPipelineManager::loadPipelines(const CameraParams& camera_params, switch (params_.frontend_type_) { case FrontendType::kRGBD: { LOG(INFO) << "Making RGBDInstance frontend"; - FrontendModule::Ptr frontend = nullptr; CameraParams mutable_camera_params = camera_params; // TODO: make this conversion to RGBD param based @@ -355,11 +356,12 @@ void DynoPipelineManager::loadPipelines(const CameraParams& camera_params, additional_backend_display = backend_wrapper.backend_viz; CHECK(backend); - // if(frontend && backend) { - // backend->registerMapUpdater(std::bind(&FrontendModule::mapUpdate, - // frontend.get(), std::placeholders::_1)); LOG(INFO) << "Bound map - // update between frontend and backend"; - // } + if (frontend && backend) { + backend->registerFrontendUpdateInterface(std::bind( + &FrontendModule::onBackendUpdateCallback, frontend.get(), + std::placeholders::_1, std::placeholders::_2)); + LOG(INFO) << "Bound update between frontend and backend"; + } } else if (use_offline_frontend_) { LOG(WARNING) @@ -387,11 +389,23 @@ void DynoPipelineManager::loadPipelines(const CameraParams& camera_params, backend_pipeline_ = std::make_unique( "backend-pipeline", &backend_input_queue_, backend); backend_pipeline_->parallelRun(parallel_run); - // also register connection between front and back - frontend_output_registra->registerQueue(&backend_input_queue_); + // // also register connection between front and back + // frontend_output_registra->registerQueue(&backend_input_queue_); + FrontendPipeline::OutputQueue& backend_input_queue = backend_input_queue_; + frontend_output_registra->registerCallback( + [&backend_input_queue](const auto& frontend_output) -> void { + backend_input_queue.push(frontend_output); + }); backend_pipeline_->registerOutputQueue(&backend_output_queue_); + if (frontend) { + LOG(INFO) << "Setting frontend accessor"; + // set frontend accessor + frontend->setAccessor(backend->getAccessor()); + } + + // set backend display (if any!) if (additional_backend_display) { VLOG(10) << "Connecting BackendModuleDisplay"; backend_pipeline_->registerOutputCallback( @@ -408,9 +422,11 @@ void DynoPipelineManager::loadPipelines(const CameraParams& camera_params, if (backend && backend_display) { backend_viz_pipeline_ = std::make_unique( "backend-viz-pipeline", &backend_output_queue_, backend_display); + backend_viz_pipeline_->parallelRun(parallel_run); } frontend_viz_pipeline_ = std::make_unique( "frontend-viz-pipeline", &frontend_viz_input_queue_, frontend_display); + frontend_viz_pipeline_->parallelRun(parallel_run); } } diff --git a/dynosam/test/test_hybrid_motion.cc b/dynosam/test/test_hybrid_motion.cc index 1b05c2661..8a3bd1db7 100644 --- a/dynosam/test/test_hybrid_motion.cc +++ b/dynosam/test/test_hybrid_motion.cc @@ -9,6 +9,39 @@ #include "internal/simulator.hpp" using namespace dyno; +using namespace gtsam; + +namespace Original { + +// --- Original Implementations for Comparison --- +gtsam::Point3 projectToObject3(const gtsam::Pose3& X_k, + const gtsam::Pose3& e_H_k_world, + const gtsam::Pose3& L_s0, + const gtsam::Point3& Z_k) { + gtsam::Pose3 k_H_s0_k = (L_s0.inverse() * e_H_k_world * L_s0).inverse(); + gtsam::Pose3 L_k = e_H_k_world * L_s0; + gtsam::Pose3 k_H_s0_W = L_k * k_H_s0_k * L_k.inverse(); + gtsam::Point3 projected_m_object = L_s0.inverse() * k_H_s0_W * X_k * Z_k; + return projected_m_object; +} + +gtsam::Pose3 projectToCamera3Transform(const gtsam::Pose3& X_k, + const gtsam::Pose3& e_H_k_world, + const gtsam::Pose3& L_e) { + return X_k.inverse() * e_H_k_world * L_e; +} + +gtsam::Point3 projectToCamera3(const gtsam::Pose3& X_k, + const gtsam::Pose3& e_H_k_world, + const gtsam::Pose3& L_e, + const gtsam::Point3& m_L) { + // apply transform to put map point into world via its motion + const auto A = projectToCamera3Transform(X_k, e_H_k_world, L_e); + gtsam::Point3 m_camera_k = A * m_L; + return m_camera_k; +} + +} // namespace Original TEST(HybridObjectMotion, testProjections) { gtsam::Pose3 e_H_k_world = @@ -35,6 +68,280 @@ TEST(HybridObjectMotion, testProjections) { EXPECT_TRUE(gtsam::assert_equal(projected_m_camera, m_camera)); } +class HybridMotionTest : public ::testing::Test { + protected: + Pose3 X_k, e_H_k_world, L_e; + Point3 m_L, Z_k; + SharedNoiseModel model; + + void SetUp() override { + // Initialize with non-identity values to avoid trivial derivatives + // X_k = Pose3(Rot3::Ypr(0.1, 0.2, 0.3), Point3(1, 2, 3)); + // e_H_k_world = Pose3(Rot3::Ypr(0.4, 0.1, -0.2), Point3(-1, 0.5, 2)); + // L = Pose3(Rot3::Ypr(-0.3, 0.2, 0.1), Point3(0.5, -1, 1)); + // L_s0 = Pose3(Rot3::Ypr(0.1, -0.1, 0.4), Point3(2, 1, -1)); + // m_L = Point3(1.5, -0.5, 2.0); + // Z_k = Point3(-0.5, 1.2, 3.0); + X_k = Pose3(Rot3::Ypr(0.1, 0.2, 0.3), Point3(1, 2, 3)); + e_H_k_world = Pose3(Rot3::Ypr(0.4, 0.1, -0.2), Point3(-1, 0.5, 2)); + L_e = Pose3(Rot3::Ypr(-0.1, 0.0, 0.1), Point3(0.1, 0.0, 0.0)); + m_L = Point3(1.5, -0.5, 2.0); + + // Generate a "measured" point based on the current state (perfect + // prediction) This ensures the error is zero, but derivatives are still + // valid. + Z_k = HybridObjectMotion::projectToCamera3(X_k, e_H_k_world, L_e, m_L); + + // Isotropic noise model + model = noiseModel::Isotropic::Sigma(3, 1.0); + } +}; + +// --- Logic Comparison Tests --- +TEST_F(HybridMotionTest, JacobianEvaluation) { + // Keys for the factor + Key key_X = Symbol('x', 1); + Key key_E = Symbol('e', 1); + Key key_M = Symbol('m', 1); + + // Instantiate the factor + HybridMotionFactor factor(key_X, key_E, key_M, Z_k, L_e, model); + + // 1. Analytical Jacobians + Matrix H1_act, H2_act, H3_act; + Vector error = + factor.evaluateError(X_k, e_H_k_world, m_L, H1_act, H2_act, H3_act); + + // 2. Numerical Jacobians + // Wrap the evaluateError function for use with numericalDerivative + auto funcX = [&](const Pose3& x) { + return factor.evaluateError(x, e_H_k_world, m_L); + }; + auto funcE = [&](const Pose3& e) { + return factor.evaluateError(X_k, e, m_L); + }; + auto funcM = [&](const Point3& m) { + return factor.evaluateError(X_k, e_H_k_world, m); + }; + + Matrix H1_num = numericalDerivative11(funcX, X_k); + Matrix H2_num = numericalDerivative11(funcE, e_H_k_world); + Matrix H3_num = numericalDerivative11(funcM, m_L); + + // 3. Verify Dimensions + EXPECT_EQ(H1_act.rows(), 3); + EXPECT_EQ(H1_act.cols(), 6); + EXPECT_EQ(H2_act.rows(), 3); + EXPECT_EQ(H2_act.cols(), 6); + EXPECT_EQ(H3_act.rows(), 3); + EXPECT_EQ(H3_act.cols(), 3); + + // 4. Verify Values + EXPECT_TRUE(assert_equal(H1_num, H1_act, 1e-5)); + EXPECT_TRUE(assert_equal(H2_num, H2_act, 1e-5)); + EXPECT_TRUE(assert_equal(H3_num, H3_act, 1e-5)); +} + +TEST_F(HybridMotionTest, CompareOriginal_ProjectToObject3) { + // Compute using the new simplified implementation + Point3 result_new = + HybridObjectMotion::projectToObject3(X_k, e_H_k_world, L_e, Z_k); + + // Compute using the original implementation + Point3 result_orig = Original::projectToObject3(X_k, e_H_k_world, L_e, Z_k); + + // Assert equality + EXPECT_TRUE(assert_equal(result_orig, result_new, 1e-9)); +} + +TEST_F(HybridMotionTest, CompareOriginal_ProjectToCamera3Transform) { + Pose3 result_new = + HybridObjectMotion::projectToCamera3Transform(X_k, e_H_k_world, L_e); + Pose3 result_orig = + Original::projectToCamera3Transform(X_k, e_H_k_world, L_e); + + EXPECT_TRUE(assert_equal(result_orig, result_new, 1e-9)); +} + +TEST_F(HybridMotionTest, CompareOriginal_ProjectToCamera3) { + Point3 result_new = + HybridObjectMotion::projectToCamera3(X_k, e_H_k_world, L_e, m_L); + Point3 result_orig = Original::projectToCamera3(X_k, e_H_k_world, L_e, m_L); + + EXPECT_TRUE(assert_equal(result_orig, result_new, 1e-9)); +} + +// --- Jacobian Tests --- + +TEST_F(HybridMotionTest, ProjectToObject3_Jacobians) { + Matrix36 H_X, H_E, H_L; + Point3 result = HybridObjectMotion::projectToObject3(X_k, e_H_k_world, L_e, + Z_k, H_X, H_E, H_L); + + auto func = [=](const Pose3& x, const Pose3& e, const Pose3& l, + const Point3& z) -> Point3 { + return HybridObjectMotion::projectToObject3(x, e, l, z); + }; + + Matrix36 num_H_X = numericalDerivative41( + func, X_k, e_H_k_world, L_e, Z_k); + + Matrix36 num_H_E = numericalDerivative42( + func, X_k, e_H_k_world, L_e, Z_k); + + Matrix36 num_H_L = numericalDerivative43( + func, X_k, e_H_k_world, L_e, Z_k); + + // Matrix33 num_H_Z = numericalDerivative44( + // func, X_k, e_H_k_world, L_s0, Z_k); + + EXPECT_TRUE(assert_equal(num_H_X, H_X, 1e-5)); + EXPECT_TRUE(assert_equal(num_H_E, H_E, 1e-5)); + EXPECT_TRUE(assert_equal(num_H_L, H_L, 1e-5)); + // EXPECT_TRUE(assert_equal(num_H_Z, H_Z, 1e-5)); +} + +TEST_F(HybridMotionTest, ProjectToCamera3Transform_Jacobians) { + Matrix66 H_X, H_E, H_L; + Pose3 result = HybridObjectMotion::projectToCamera3Transform( + X_k, e_H_k_world, L_e, H_X, H_E, H_L); + + auto func = [=](const Pose3& x, const Pose3& e, + const Pose3& l) -> gtsam::Pose3 { + return HybridObjectMotion::projectToCamera3Transform(x, e, l); + }; + + // Numerical Derivatives + Matrix66 num_H_X = numericalDerivative31( + func, X_k, e_H_k_world, L_e); + + Matrix66 num_H_E = numericalDerivative32( + func, X_k, e_H_k_world, L_e); + + Matrix66 num_H_L = numericalDerivative33( + func, X_k, e_H_k_world, L_e); + + EXPECT_TRUE(assert_equal(num_H_X, H_X, 1e-5)); + EXPECT_TRUE(assert_equal(num_H_E, H_E, 1e-5)); + EXPECT_TRUE(assert_equal(num_H_L, H_L, 1e-5)); +} + +TEST_F(HybridMotionTest, ProjectToCamera3_Jacobians) { + Matrix36 H_X, H_E, H_L; + Matrix33 H_m; + Point3 result = HybridObjectMotion::projectToCamera3(X_k, e_H_k_world, L_e, + m_L, H_X, H_E, H_L, H_m); + + // Wrapper for numerical derivative because m_L is 4th argument + auto func = [=](const Pose3& x, const Pose3& e, const Pose3& l, + const Point3& m) { + return HybridObjectMotion::projectToCamera3(x, e, l, m); + }; + + Matrix36 num_H_X = numericalDerivative41( + func, X_k, e_H_k_world, L_e, m_L); + + Matrix36 num_H_E = numericalDerivative42( + func, X_k, e_H_k_world, L_e, m_L); + + Matrix36 num_H_L = numericalDerivative43( + func, X_k, e_H_k_world, L_e, m_L); + + Matrix33 num_H_m = numericalDerivative44( + func, X_k, e_H_k_world, L_e, m_L); + + EXPECT_TRUE(assert_equal(num_H_X, H_X, 1e-5)); + EXPECT_TRUE(assert_equal(num_H_E, H_E, 1e-5)); + EXPECT_TRUE(assert_equal(num_H_L, H_L, 1e-5)); + EXPECT_TRUE(assert_equal(num_H_m, H_m, 1e-5)); +} + +class StereoHybridFactorTest : public ::testing::Test { + protected: + Pose3 X_k, e_H_k_world, L_e; + Point3 m_L; + Cal3_S2Stereo::shared_ptr K; + SharedNoiseModel model; + StereoPoint2 measured; + + void SetUp() override { + // Setup Arbitrary Poses (Non-identity to test rotations) + X_k = Pose3(Rot3::Ypr(0.1, 0.2, 0.3), Point3(1, 2, 3)); + e_H_k_world = Pose3(Rot3::Ypr(0.4, 0.1, -0.2), Point3(-1, 0.5, 2)); + L_e = Pose3(Rot3::Ypr(-0.1, 0.0, 0.1), Point3(0.1, 0.0, 0.0)); + m_L = Point3(1.5, -0.5, 5.0); // Point needs to be in front of camera + + // Setup Calibration (fx, fy, s, u0, v0, b) + K = boost::make_shared(1000, 1000, 0, 320, 240, 0.5); + + // Setup Noise Model + model = noiseModel::Isotropic::Sigma(3, 1.0); + + // Create a "Perfect" measurement using the exact same logic as the Factor + // 1. Project to camera frame + Point3 p_cam = + HybridObjectMotion::projectToCamera3(X_k, e_H_k_world, L_e, m_L); + + // 2. Project using StereoCamera at Identity + StereoCamera cam(Pose3::Identity(), K); + StereoPoint2 perfect = cam.project2(p_cam); + + // Perturb slightly for test + measured = + StereoPoint2(perfect.uL() + 2.0, perfect.uR() - 1.0, perfect.v() + 0.5); + } +}; + +TEST_F(StereoHybridFactorTest, JacobianEvaluation) { + // Keys + Key key_X = Symbol('x', 1); + Key key_E = Symbol('e', 1); + Key key_M = Symbol('m', 1); + + // Create Factor + StereoHybridMotionFactor factor(measured, L_e, model, K, key_X, key_E, key_M); + + // 1. Compute Analytical Jacobians via evaluateError + Matrix H1_act, H2_act, H3_act; + Vector error = + factor.evaluateError(X_k, e_H_k_world, m_L, H1_act, H2_act, H3_act); + + // 2. Compute Numerical Jacobians + // We use a lambda to bind the factor's evaluateError function for numerical + // derivative + + auto funcX = [&](const Pose3& x) { + return factor.evaluateError(x, e_H_k_world, m_L); + }; + + auto funcE = [&](const Pose3& e) { + return factor.evaluateError(X_k, e, m_L); + }; + + auto funcM = [&](const Point3& m) { + return factor.evaluateError(X_k, e_H_k_world, m); + }; + + Matrix H1_num = numericalDerivative11(funcX, X_k); + Matrix H2_num = numericalDerivative11(funcE, e_H_k_world); + Matrix H3_num = numericalDerivative11(funcM, m_L); + + // 3. Compare + // Dimensions check + EXPECT_EQ(H1_act.rows(), 3); + EXPECT_EQ(H1_act.cols(), 6); + EXPECT_EQ(H2_act.rows(), 3); + EXPECT_EQ(H2_act.cols(), 6); + EXPECT_EQ(H3_act.rows(), 3); + EXPECT_EQ(H3_act.cols(), 3); + + // Value check (using 1e-5 tolerance for chain rule complexity) + EXPECT_TRUE(assert_equal(H1_num, H1_act, 1e-5)); + EXPECT_TRUE(assert_equal(H2_num, H2_act, 1e-5)); + EXPECT_TRUE(assert_equal(H3_num, H3_act, 1e-5)); +} + // TEST(HybridObjectMotion, ProjectToCamera3Jacobian) { // using namespace gtsam; diff --git a/dynosam/test/test_types.cc b/dynosam/test/test_types.cc index d1c5e0c61..51c8dc1af 100644 --- a/dynosam/test/test_types.cc +++ b/dynosam/test/test_types.cc @@ -323,7 +323,7 @@ enum class TestOptions : std::uint8_t { }; template <> -struct internal::EnableBitMaskOperators : std::true_type {}; +struct dyno::internal::EnableBitMaskOperators : std::true_type {}; TEST(BitwiseFlags, testUnderlyingTypeSpecalization) { using TestFlags = Flags; diff --git a/dynosam_common/CMakeLists.txt b/dynosam_common/CMakeLists.txt index 8e605bcfc..d9dc25ff4 100644 --- a/dynosam_common/CMakeLists.txt +++ b/dynosam_common/CMakeLists.txt @@ -14,8 +14,29 @@ set(CMAKE_CXX_STANDARD 17) list(INSERT CMAKE_MODULE_PATH 0 ${CMAKE_SOURCE_DIR}/cmake) +if(CMAKE_SYSTEM_NAME STREQUAL "Linux") + # Determine the processor part of the triplet + if(CMAKE_SYSTEM_PROCESSOR STREQUAL "x86_64") + set(ARCH_TRIPLET "x86_64-linux-gnu") + elseif(CMAKE_SYSTEM_PROCESSOR STREQUAL "aarch64") + set(ARCH_TRIPLET "aarch64-linux-gnu") + # Add more architectures (e.g., armv7, ppc64) as needed + else() + # Fallback or error if architecture is not explicitly handled + set(ARCH_TRIPLET "") + message(WARNING "Unknown processor architecture: ${CMAKE_SYSTEM_PROCESSOR}") + endif() + + if(ARCH_TRIPLET) + set(ARCHITECTURE_INCLUDE_PATH "/usr/include/${ARCH_TRIPLET}") + message("Architecture-specific include path: ${ARCHITECTURE_INCLUDE_PATH}") + endif() +endif() +# on some systems it seems that openmpi install path is wrong (or the find_package macro is wrong...) set(WITH_MPI OFF CACHE BOOL "Disable MPI in PCL") +# set(MPI_INCLUDE_PATH /usr/include/x86_64-linux-gnu/openmpi/) +set(MPI_INCLUDE_PATH ${ARCHITECTURE_INCLUDE_PATH}/openmpi/) find_package(ament_cmake REQUIRED) find_package(ament_index_cpp REQUIRED) diff --git a/dynosam_common/include/dynosam_common/DynamicObjects.hpp b/dynosam_common/include/dynosam_common/DynamicObjects.hpp index c0ae18a90..82d33a61f 100644 --- a/dynosam_common/include/dynosam_common/DynamicObjects.hpp +++ b/dynosam_common/include/dynosam_common/DynamicObjects.hpp @@ -37,6 +37,32 @@ namespace dyno { +// TODO: eventually this should be the same as from the object-track module! +// right now detected by frontend +enum class ObjectTrackingStatus { New = 0, Tracked = 1, ReTracked = 2 }; + +// kind of repeatition of TrackingStatus (which is not really used anywhere!!!) +enum class ObjectFeatureTrackingStatus { + Valid = 0, + //! New features were detected not that the object itself is retracked + Resampled = 1 + // TODO: maybe too few etc +}; + +// should probably go in SingleDetecionResult! +struct ObjectStatus { + ObjectTrackingStatus tracking_status; + ObjectFeatureTrackingStatus feature_status; +}; + +class ObjectStatusMap : public gtsam::FastMap { + public: + using Base = gtsam::FastMap; + using Base::Base; + + ObjectIds getResampledObjects() const; +}; + /** * @brief The result of an object detection from a detection network * @@ -66,7 +92,7 @@ struct SingleDetectionResult : public ObjectDetection { // This is implicitly checks that object_id is not -1 (ie not set) // AND is a valiid object label (i.e. is non-zero which is the background // label) - return object_id > background_label || well_tracked; + return (object_id > background_label) && well_tracked; } }; @@ -96,8 +122,10 @@ struct ObjectDetectionResult { * @param w_L_k_1 const gtsam::Pose3& * @return gtsam::Vector3 */ -gtsam::Vector3 calculateBodyMotion(const gtsam::Pose3& w_k_1_H_k, - const gtsam::Pose3& w_L_k_1); +gtsam::Vector6 calculateBodyMotion(const gtsam::Pose3& w_k_1_H_k, + const gtsam::Pose3& w_L_k_1, + Timestamp timestamp_km1, + Timestamp timestamp_k); enum PropogateType { InitGT, diff --git a/dynosam_common/include/dynosam_common/PointCloudProcess.hpp b/dynosam_common/include/dynosam_common/PointCloudProcess.hpp index 2471a4809..3f7e1fc8e 100644 --- a/dynosam_common/include/dynosam_common/PointCloudProcess.hpp +++ b/dynosam_common/include/dynosam_common/PointCloudProcess.hpp @@ -89,6 +89,14 @@ void convert(const StatusLandmarkVector& status_vector, convert_pcl(status_vector, cloud); } +template +bool convert(const gtsam::Point3& gtsam_point, PointT& pcl_point) { + pcl_point.x = static_cast(gtsam_point(0)); + pcl_point.y = static_cast(gtsam_point(1)); + pcl_point.z = static_cast(gtsam_point(2)); + return true; +} + /** * @brief Constructs a gtsam::Point3 from the x,y,z components of the PointT. * diff --git a/dynosam_common/include/dynosam_common/SharedModuleInfo.hpp b/dynosam_common/include/dynosam_common/SharedModuleInfo.hpp index f88d28c2d..a7648b025 100644 --- a/dynosam_common/include/dynosam_common/SharedModuleInfo.hpp +++ b/dynosam_common/include/dynosam_common/SharedModuleInfo.hpp @@ -43,6 +43,7 @@ class SharedModuleInfo { std::optional getGroundTruthPackets() const; const FrameIdTimestampMap& getTimestampMap() const; + const ObjectIdClassMap& getClassLabelMap() const; bool getTimestamp(FrameId frame_id, Timestamp& timestamp) const; @@ -50,6 +51,7 @@ class SharedModuleInfo { FrameId frame_id, const GroundTruthInputPacket& ground_truth_packet); SharedModuleInfo& updateTimestampMapping(FrameId frame_id, Timestamp timestamp); + SharedModuleInfo& updateClassLabelMapping(ObjectId object_id, std::string class_label); private: static std::unique_ptr instance_; @@ -58,6 +60,7 @@ class SharedModuleInfo { mutable std::mutex mutex_; GroundTruthPacketMap gt_packet_map_; FrameIdTimestampMap frame_id_to_timestamp_map_; + ObjectIdClassMap object_id_to_class_map_; }; struct SharedModuleInterface { diff --git a/dynosam_common/include/dynosam_common/StructuredContainers.hpp b/dynosam_common/include/dynosam_common/StructuredContainers.hpp index c0224f8bd..8046ca9b1 100644 --- a/dynosam_common/include/dynosam_common/StructuredContainers.hpp +++ b/dynosam_common/include/dynosam_common/StructuredContainers.hpp @@ -397,3 +397,29 @@ class FastUnorderedMap }; } // namespace dyno + +// allow convenience tuple like getters for FrameRange +namespace std { +template +struct tuple_size> : std::integral_constant {}; + +template +struct tuple_element<0, dyno::FrameRange> { + using type = dyno::FrameId; +}; + +template +struct tuple_element<1, dyno::FrameRange> { + using type = T; +}; +} // namespace std + +namespace dyno { +template +auto get(const FrameRange& p) { + if constexpr (N == 0) + return p.dataPair().first; + else if constexpr (N == 1) + return p.dataPair().second; +} +} // namespace dyno diff --git a/dynosam_common/include/dynosam_common/Types.hpp b/dynosam_common/include/dynosam_common/Types.hpp index d19bbe26d..9a764b1d3 100644 --- a/dynosam_common/include/dynosam_common/Types.hpp +++ b/dynosam_common/include/dynosam_common/Types.hpp @@ -111,6 +111,9 @@ using MotionMap = /// @brief Map of FrameIds (k) to Timestamps using FrameIdTimestampMap = gtsam::FastMap; +/// @brief Map of Object Ids to CLass Labels +using ObjectIdClassMap = gtsam::FastMap; + // T is expected to have (at least) bitwise | (OR) support template class Flags { diff --git a/dynosam_common/include/dynosam_common/utils/Timing.hpp b/dynosam_common/include/dynosam_common/utils/Timing.hpp index e3c568d12..1a1f6ea8c 100644 --- a/dynosam_common/include/dynosam_common/utils/Timing.hpp +++ b/dynosam_common/include/dynosam_common/utils/Timing.hpp @@ -65,10 +65,15 @@ class Timer { 1e9; } + template + static double toUnits(const T& time) { + const auto unit = std::chrono::duration_cast(time); + return static_cast(unit.count()); + } + template static double toSeconds(const T& time) { - auto seconds = std::chrono::duration_cast(time); - return static_cast(seconds.count()); + return toUnits(time); } }; diff --git a/dynosam_common/include/dynosam_common/utils/TimingStats.hpp b/dynosam_common/include/dynosam_common/utils/TimingStats.hpp index b2d5152ad..427d82136 100644 --- a/dynosam_common/include/dynosam_common/utils/TimingStats.hpp +++ b/dynosam_common/include/dynosam_common/utils/TimingStats.hpp @@ -105,19 +105,20 @@ class TimingStatsCollector { public: DYNO_POINTER_TYPEDEFS(TimingStatsCollector) - TimingStatsCollector(const std::string& tag); + TimingStatsCollector(const std::string& tag, int glog_level = 0, + bool construct_stopped = false); ~TimingStatsCollector(); - /** - * @brief Resets the current tic_time and sets is_valid = true, - * such that when the collector tries to toc, the tic (comparison time) will - * be valid - * - * - */ - void reset(); + void start(); + void stop(); + bool isTiming() const; + void discardTiming(); + + std::chrono::milliseconds delta() const; - bool isValid() const; + // not that it will log to glog, but that the glog verbosity level is set + // such that it will log to the collector + bool shouldGlog() const; private: /** @@ -128,16 +129,19 @@ class TimingStatsCollector { * The collector then needs to be reset to be used again * */ - void tocAndLog(); + void log(); private: - Timer::TimePoint - tic_time_; //! Time on creation (the time to compare against) - StatsCollector collector_; - - std::atomic_bool is_valid_{ - true}; //! Indiactes validity of the tic_time and if the collector has - //! been reset allowing a new toc to be made + const std::string tag_; + const int glog_level_; + //! Comparison time + Timer::TimePoint tic_time_; + //! Timing state + std::atomic_bool is_timing_; + //! Internal logger. + //! Created only first time logging ocurs to ensure the tag only appears if + //! the timing actually logs and not just if it is instantiated. + std::unique_ptr collector_; }; } // namespace utils diff --git a/dynosam_common/src/DynamicObjects.cc b/dynosam_common/src/DynamicObjects.cc index 8c9551041..9b9fcacb6 100644 --- a/dynosam_common/src/DynamicObjects.cc +++ b/dynosam_common/src/DynamicObjects.cc @@ -73,24 +73,90 @@ std::ostream& operator<<(std::ostream& os, return os; } -gtsam::Vector3 calculateBodyMotion(const gtsam::Pose3& w_k_1_H_k, - const gtsam::Pose3& w_L_k_1) { + +// OLD BODY MOTION CALCULATION IMPLEMENTATION +// want this to also output angular velocity too +gtsam::Vector6 calculateBodyMotion(const gtsam::Pose3& w_k_1_H_k, + const gtsam::Pose3& w_L_k_1, + Timestamp timestamp_km1, + Timestamp timestamp_k) { const gtsam::Point3& t_motion = w_k_1_H_k.translation(); const gtsam::Rot3& R_motion = w_k_1_H_k.rotation(); const gtsam::Point3& t_pose = w_L_k_1.translation(); + const gtsam::Rot3& R_pose = w_L_k_1.rotation(); static const gtsam::Rot3 I = gtsam::Rot3::Identity(); - return t_motion - (gtsam::Rot3(I.matrix() - R_motion.matrix())) * t_pose; + double dt = timestamp_k - timestamp_km1; + if (dt <= 1e-6) { + dt = 0.1; // fallback to nominal frame interval + } + + gtsam::Vector3 trans_vel = (t_motion - (gtsam::Rot3(I.matrix() - R_motion.matrix())) * t_pose) / dt; + +// ===== NOW CALCULATING THE ANGULAR VELOCITY ===== + + // Finding the relative rotation over the timestep + gtsam::Rot3 R_pose_transpose = R_pose.inverse(); + gtsam::Rot3 R_new = R_motion * R_pose; + gtsam::Rot3 R_rel = R_pose_transpose * R_new; + + // Calculating the trace of the relative rotation and the angle of rotation + double trace = R_rel.matrix().trace(); + double angle = std::acos((trace - 1) / 2); + + gtsam::Vector3 angular_vel = gtsam::Vector3::Zero(); + + if (std::abs(angle) > 1e-6) { + gtsam::Matrix3 R_rel_m = R_rel.matrix(); + double sin_angle = std::sin(angle); + + double ux = (1 / (2 * sin_angle)) * (R_rel_m(2,1) - R_rel_m(1,2)); + double uy = (1 / (2 * sin_angle)) * (R_rel_m(0,2) - R_rel_m(2,0)); + double uz = (1 / (2 * sin_angle)) * (R_rel_m(1,0) - R_rel_m(0,1)); + + const gtsam::Vector3 axis(ux, uy, uz); + angular_vel = (angle / dt) * axis; + + + } + + gtsam::Vector6 body_velocity; + body_velocity.head<3>() = trans_vel; + body_velocity.tail<3>() = angular_vel; + + + return body_velocity; } -void propogateObjectPoses( - ObjectPoseMap& object_poses, const MotionEstimateMap& object_motions_k, - const gtsam::Point3Vector& object_centroids_k_1, - const gtsam::Point3Vector& - object_centroids_k, // TODO: dont actually need this one!! - FrameId frame_id_k, std::optional gt_packet_map, - PropogatePoseResult* result) { +// gtsam::Vector6 calculateBodyMotion( +// const gtsam::Pose3& w_H_k, +// const gtsam::Pose3& w_H_km1, +// Timestamp timestamp_km1, +// Timestamp timestamp_k) { + +// double dt = timestamp_k - timestamp_km1; +// if (dt <= 1e-6) { +// dt = 0.1; // fallback +// } + +// // Relative motion expressed in the BODY frame +// // T_km1_k = (W_H_km1)^{-1} * (W_H_k) +// gtsam::Pose3 T_km1_k = w_H_km1.inverse() * w_H_k; + +// // Logmap gives body-frame twist [wx wy wz vx vy vz] +// gtsam::Vector6 xi = gtsam::Pose3::Logmap(T_km1_k); + +// return xi / dt; +// } + +void propogateObjectPoses(ObjectPoseMap& object_poses, + const MotionEstimateMap& object_motions_k, + const gtsam::Point3Vector& object_centroids_k_1, + const gtsam::Point3Vector& object_centroids_k, + FrameId frame_id_k, + std::optional gt_packet_map, + PropogatePoseResult* result) { CHECK_EQ(object_motions_k.size(), object_centroids_k_1.size()); CHECK_EQ(object_centroids_k.size(), object_centroids_k_1.size()); const FrameId frame_id_k_1 = frame_id_k - 1; @@ -126,7 +192,6 @@ void propogateObjectPoses( size_t i = 0; // used to index the object centroid vectors for (const auto& [object_id, motion] : object_motions_k) { - const auto centroid_k = object_centroids_k.at(i); const auto centroid_k_1 = object_centroids_k_1.at(i); const gtsam::Pose3 prev_H_world_curr = motion; // new object - so we need to add at k-1 and k @@ -161,9 +226,10 @@ void propogateObjectPoses( const FrameId last_frame = last_record_itr->first; const gtsam::Pose3 last_recorded_pose = last_record_itr->second; + const gtsam::Point3& centroid_k = object_centroids_k.at(i); // construct current pose using last poses rotation (I guess?) gtsam::Pose3 current_pose = - gtsam::Pose3(last_recorded_pose.rotation(), object_centroids_k.at(i)); + gtsam::Pose3(last_recorded_pose.rotation(), centroid_k); CHECK_LT(last_frame, frame_id_k_1); if (frame_id_k - last_frame < min_diff_frames) { diff --git a/dynosam_common/src/SharedModuleInfo.cc b/dynosam_common/src/SharedModuleInfo.cc index 076b86213..bf5a37290 100644 --- a/dynosam_common/src/SharedModuleInfo.cc +++ b/dynosam_common/src/SharedModuleInfo.cc @@ -57,6 +57,11 @@ std::optional SharedModuleInfo::getGroundTruthPackets() return gt_packet_map_; } +const ObjectIdClassMap& SharedModuleInfo::getClassLabelMap() const { + const std::lock_guard lock(mutex_); + return object_id_to_class_map_; +} + bool SharedModuleInfo::getTimestamp(FrameId frame_id, Timestamp& timestamp) const { const FrameIdTimestampMap& timestamp_map = getTimestampMap(); @@ -88,4 +93,22 @@ SharedModuleInfo& SharedModuleInfo::updateTimestampMapping( return *this; } +SharedModuleInfo& SharedModuleInfo::updateClassLabelMapping( + ObjectId object_id, std::string class_label) { + const std::lock_guard lock(mutex_); + if (object_id_to_class_map_.exists(object_id)) { + // Allow updates but warn if class changes + const std::string& existing_label = object_id_to_class_map_.at(object_id); + if (existing_label != class_label) { + LOG(WARNING) << "UPDATING CLASS LABEL MAPPING " << object_id + << " from '" << existing_label << "' to '" << class_label << "'"; + } + object_id_to_class_map_.at(object_id) = class_label; // Update it + } else { + LOG(INFO) << "UPDATING CLASS LABEL MAPPING " << object_id << " IS " << class_label; + object_id_to_class_map_.insert2(object_id, class_label); + } + return *this; +} + } // namespace dyno diff --git a/dynosam_common/src/utils/Statistics.cc b/dynosam_common/src/utils/Statistics.cc index 80134735d..f6a502dd9 100644 --- a/dynosam_common/src/utils/Statistics.cc +++ b/dynosam_common/src/utils/Statistics.cc @@ -332,6 +332,7 @@ void Statistics::Print(std::ostream& out) { // NOLINT out << t.first << "\t"; out.setf(std::ios::right, std::ios::adjustfield); + out << std::setprecision(5); // Print # out << std::setw(5) << GetNumSamples(i) << "\t"; if (GetNumSamples(i) > 0) { diff --git a/dynosam_common/src/utils/TimingStats.cc b/dynosam_common/src/utils/TimingStats.cc index 75801d802..7e8ddae85 100644 --- a/dynosam_common/src/utils/TimingStats.cc +++ b/dynosam_common/src/utils/TimingStats.cc @@ -30,6 +30,8 @@ #include "dynosam_common/utils/TimingStats.hpp" +#include + namespace dyno { namespace utils { @@ -39,26 +41,58 @@ namespace utils { // return os; // } -TimingStatsCollector::TimingStatsCollector(const std::string& tag) - : tic_time_(Timer::tic()), collector_(tag + " [ms]") {} +TimingStatsCollector::TimingStatsCollector(const std::string& tag, + int glog_level, + bool construct_stopped) + : tag_(tag + " [ms]"), + glog_level_(glog_level), + tic_time_(Timer::tic()), + is_timing_(false) { + if (!construct_stopped) { + start(); + } +} + +TimingStatsCollector::~TimingStatsCollector() { stop(); } -TimingStatsCollector::~TimingStatsCollector() { tocAndLog(); } +void TimingStatsCollector::start() { + tic_time_ = Timer::tic(); + is_timing_ = true; +} + +void TimingStatsCollector::stop() { + if (isTiming() && shouldGlog()) { + log(); + } -void TimingStatsCollector::reset() { tic_time_ = Timer::tic(); - is_valid_ = true; + is_timing_ = false; +} + +bool TimingStatsCollector::isTiming() const { return is_timing_; } + +bool TimingStatsCollector::shouldGlog() const { + if (glog_level_ == 0) { + return true; + } + + return VLOG_IS_ON(glog_level_); +} + +void TimingStatsCollector::discardTiming() { is_timing_ = false; } + +std::chrono::milliseconds TimingStatsCollector::delta() const { + const auto toc = Timer::toc(tic_time_); + return std::chrono::duration_cast(toc); } -bool TimingStatsCollector::isValid() const { return is_valid_; } +void TimingStatsCollector::log() { + const auto milliseconds = delta(); -void TimingStatsCollector::tocAndLog() { - if (is_valid_) { - auto toc = Timer::toc(tic_time_); - auto milliseconds = - std::chrono::duration_cast(toc); - collector_.AddSample(static_cast(milliseconds.count())); - is_valid_ = false; + if (!collector_) { + collector_ = std::make_unique(tag_); } + collector_->AddSample(static_cast(milliseconds.count())); } } // namespace utils diff --git a/dynosam_cv/include/dynosam_cv/Feature.hpp b/dynosam_cv/include/dynosam_cv/Feature.hpp index 2e205391d..564226372 100644 --- a/dynosam_cv/include/dynosam_cv/Feature.hpp +++ b/dynosam_cv/include/dynosam_cv/Feature.hpp @@ -95,33 +95,23 @@ class Feature { Feature() : data_() {} - Feature(const Feature& other) { - // TODO: use both mutexs? - std::lock_guard lk(other.mutex_); - data_ = other.data_; - } + Feature(const Feature& other); - bool operator==(const Feature& other) const { return data_ == other.data_; } + bool operator==(const Feature& other) const; /** * @brief Gets keypoint observation. * * @return Keypoint */ - Keypoint keypoint() const { - std::lock_guard lk(mutex_); - return data_.keypoint; - } + Keypoint keypoint() const; /** * @brief Gets the measured flow. * * @return OpticalFlow */ - OpticalFlow measuredFlow() const { - std::lock_guard lk(mutex_); - return data_.measured_flow; - } + OpticalFlow measuredFlow() const; /** * @brief Gets the predicted keypoint. @@ -134,10 +124,7 @@ class Feature { * * @return Keypoint */ - Keypoint predictedKeypoint() const { - std::lock_guard lk(mutex_); - return data_.predicted_keypoint; - } + Keypoint predictedKeypoint() const; /** * @brief Get the number of consequative frames this feature has been @@ -145,51 +132,35 @@ class Feature { * * @return size_t */ - size_t age() const { - std::lock_guard lk(mutex_); - return data_.age; - } + size_t age() const; /** * @brief Get keypoint type (static or dynamci). * * @return KeyPointType */ - KeyPointType keypointType() const { - std::lock_guard lk(mutex_); - return data_.type; - } + KeyPointType keypointType() const; /** * @brief Get the keypoints unique tracklet id (ie. i). * * @return TrackletId */ - TrackletId trackletId() const { - std::lock_guard lk(mutex_); - return data_.tracklet_id; - } + TrackletId trackletId() const; /** * @brief Get the frame id this feature was observed in (ie. k). * * @return FrameId */ - FrameId frameId() const { - std::lock_guard lk(mutex_); - return data_.frame_id; - } - + FrameId frameId() const; /** * @brief If the feature is an inlier. * * @return true * @return false */ - bool inlier() const { - std::lock_guard lk(mutex_); - return data_.inlier; - } + bool inlier() const; /** * @brief Get the object id associated with this feature (0 if @@ -197,10 +168,7 @@ class Feature { * * @return ObjectId */ - ObjectId objectId() const { - std::lock_guard lk(mutex_); - return data_.tracking_label; - } + ObjectId objectId() const; /** * @brief Depth of this keypoint (will be Feature::invalid_depth is depth is @@ -208,10 +176,7 @@ class Feature { * * @return Depth */ - Depth depth() const { - std::lock_guard lk(mutex_); - return data_.depth; - } + Depth depth() const; /** * @brief Gets the right keypoint. The value will be @@ -219,17 +184,10 @@ class Feature { * * @return Keypoint */ - Keypoint rightKeypoint() const { - std::lock_guard lk(mutex_); - if (data_.right_kp.has_value()) return data_.right_kp.value(); - DYNO_THROW_MSG(DynosamException) - << "Right Kp requested for" << data_.tracklet_id << " but not set"; - } + Keypoint rightKeypoint() const; static Keypoint CalculatePredictedKeypoint(const Keypoint& keypoint, - const OpticalFlow& measured_flow) { - return keypoint + measured_flow; - } + const OpticalFlow& measured_flow); /** * @brief Sets the measured optical flow (which should start at the features @@ -240,72 +198,27 @@ class Feature { * * @param measured_flow */ - void setPredictedKeypoint(const OpticalFlow& measured_flow) { - std::lock_guard lk(mutex_); - data_.measured_flow = measured_flow; - data_.predicted_keypoint = - CalculatePredictedKeypoint(data_.keypoint, measured_flow); - } + void setPredictedKeypoint(const OpticalFlow& measured_flow); - Feature& keypoint(const Keypoint& kp) { - std::lock_guard lk(mutex_); - data_.keypoint = kp; - return *this; - } + Feature& keypoint(const Keypoint& kp); - Feature& measuredFlow(const OpticalFlow& measured_flow) { - std::lock_guard lk(mutex_); - data_.measured_flow = measured_flow; - return *this; - } + Feature& measuredFlow(const OpticalFlow& measured_flow); - Feature& predictedKeypoint(const Keypoint& predicted_kp) { - std::lock_guard lk(mutex_); - data_.predicted_keypoint = predicted_kp; - return *this; - } + Feature& predictedKeypoint(const Keypoint& predicted_kp); - Feature& age(const size_t& a) { - std::lock_guard lk(mutex_); - data_.age = a; - return *this; - } + Feature& age(const size_t& a); - Feature& keypointType(const KeyPointType& kp_type) { - std::lock_guard lk(mutex_); - data_.type = kp_type; - return *this; - } + Feature& keypointType(const KeyPointType& kp_type); - Feature& trackletId(const TrackletId& tracklet_id) { - std::lock_guard lk(mutex_); - data_.tracklet_id = tracklet_id; - return *this; - } + Feature& trackletId(const TrackletId& tracklet_id); - Feature& frameId(const FrameId& frame_id) { - std::lock_guard lk(mutex_); - data_.frame_id = frame_id; - return *this; - } + Feature& frameId(const FrameId& frame_id); - Feature& objectId(ObjectId id) { - std::lock_guard lk(mutex_); - data_.tracking_label = id; - return *this; - } + Feature& objectId(ObjectId id); - Feature& depth(Depth d) { - std::lock_guard lk(mutex_); - data_.depth = d; - return *this; - } + Feature& depth(Depth d); - Feature& rightKeypoint(const Keypoint& right_kp) { - std::lock_guard lk(mutex_); - data_.right_kp = right_kp; - return *this; - } + Feature& rightKeypoint(const Keypoint& right_kp); /** * @brief If the feature is valid - a combination of inlier and if the @@ -316,43 +229,21 @@ class Feature { * @return true * @return false */ - bool usable() const { - std::lock_guard lk(mutex_); - return data_.inlier && (data_.tracklet_id != invalid_id); - } + bool usable() const; - bool isStatic() const { - std::lock_guard lk(mutex_); - return data_.type == KeyPointType::STATIC; - } + bool isStatic() const; - Feature& markOutlier() { - std::lock_guard lk(mutex_); - data_.inlier = false; - return *this; - } + Feature& markOutlier(); - Feature& markInlier() { - std::lock_guard lk(mutex_); - data_.inlier = true; - return *this; - } + Feature& markInlier(); - Feature& markInvalid() { - std::lock_guard lk(mutex_); - data_.tracklet_id = invalid_id; - return *this; - } + Feature& markInvalid(); - bool hasDepth() const { - std::lock_guard lk(mutex_); - return !std::isnan(data_.depth); - } + bool hasDepth() const; - bool hasRightKeypoint() const { - std::lock_guard lk(mutex_); - return data_.right_kp.has_value(); - } + bool hasRightKeypoint() const; + + bool stereoPoint(gtsam::StereoPoint2& stereo) const; inline static bool IsUsable(const Feature::Ptr& f) { return f->usable(); } diff --git a/dynosam_cv/include/dynosam_cv/RGBDCamera.hpp b/dynosam_cv/include/dynosam_cv/RGBDCamera.hpp index 25d20a6f8..1216857d1 100644 --- a/dynosam_cv/include/dynosam_cv/RGBDCamera.hpp +++ b/dynosam_cv/include/dynosam_cv/RGBDCamera.hpp @@ -69,6 +69,26 @@ class RGBDCamera : public Camera { Keypoint rightKeypoint(double depth, const Keypoint& left_keypoint) const; double rightKeypoint(double depth, double uL) const; + /** + * @brief Get the stereo measurement for this feature. + * + * If the feature already has a right keypoint then the corresponding stereo + * measurement is returned. + * + * If it does not, RGBDCamera#projectRight is used to construct the right + * feature from depth and the resulting StereoPoint2 is returned. This will + * also update the right keypoint in feature. + * + * If force_recalculation is true, the right keypoint will be recalculated + * regardless + * + * @param feature + * @param force_recalculation + * @return std::pair + */ + std::pair getStereo( + Feature::Ptr feature, const bool force_recalculation = false); + double fxb() const; Baseline baseline() const; diff --git a/dynosam_cv/src/Feature.cc b/dynosam_cv/src/Feature.cc index ebdbea887..796c951bc 100644 --- a/dynosam_cv/src/Feature.cc +++ b/dynosam_cv/src/Feature.cc @@ -34,6 +34,195 @@ namespace dyno { +Feature::Feature(const Feature& other) { + // TODO: use both mutexs? + std::lock_guard lk(other.mutex_); + data_ = other.data_; +} + +bool Feature::operator==(const Feature& other) const { + return data_ == other.data_; +} + +Keypoint Feature::keypoint() const { + std::lock_guard lk(mutex_); + return data_.keypoint; +} + +OpticalFlow Feature::measuredFlow() const { + std::lock_guard lk(mutex_); + return data_.measured_flow; +} + +Keypoint Feature::predictedKeypoint() const { + std::lock_guard lk(mutex_); + return data_.predicted_keypoint; +} + +size_t Feature::age() const { + std::lock_guard lk(mutex_); + return data_.age; +} + +KeyPointType Feature::keypointType() const { + std::lock_guard lk(mutex_); + return data_.type; +} + +TrackletId Feature::trackletId() const { + std::lock_guard lk(mutex_); + return data_.tracklet_id; +} + +FrameId Feature::frameId() const { + std::lock_guard lk(mutex_); + return data_.frame_id; +} + +bool Feature::inlier() const { + std::lock_guard lk(mutex_); + return data_.inlier; +} + +ObjectId Feature::objectId() const { + std::lock_guard lk(mutex_); + return data_.tracking_label; +} + +Depth Feature::depth() const { + std::lock_guard lk(mutex_); + return data_.depth; +} + +Keypoint Feature::rightKeypoint() const { + std::lock_guard lk(mutex_); + if (data_.right_kp.has_value()) return data_.right_kp.value(); + DYNO_THROW_MSG(DynosamException) + << "Right Kp requested for" << data_.tracklet_id << " but not set"; + return Keypoint{}; +} + +Keypoint Feature::CalculatePredictedKeypoint(const Keypoint& keypoint, + const OpticalFlow& measured_flow) { + return keypoint + measured_flow; +} + +void Feature::setPredictedKeypoint(const OpticalFlow& measured_flow) { + std::lock_guard lk(mutex_); + data_.measured_flow = measured_flow; + data_.predicted_keypoint = + CalculatePredictedKeypoint(data_.keypoint, measured_flow); +} + +Feature& Feature::keypoint(const Keypoint& kp) { + std::lock_guard lk(mutex_); + data_.keypoint = kp; + return *this; +} + +Feature& Feature::measuredFlow(const OpticalFlow& measured_flow) { + std::lock_guard lk(mutex_); + data_.measured_flow = measured_flow; + return *this; +} + +Feature& Feature::predictedKeypoint(const Keypoint& predicted_kp) { + std::lock_guard lk(mutex_); + data_.predicted_keypoint = predicted_kp; + return *this; +} + +Feature& Feature::age(const size_t& a) { + std::lock_guard lk(mutex_); + data_.age = a; + return *this; +} + +Feature& Feature::keypointType(const KeyPointType& kp_type) { + std::lock_guard lk(mutex_); + data_.type = kp_type; + return *this; +} + +Feature& Feature::trackletId(const TrackletId& tracklet_id) { + std::lock_guard lk(mutex_); + data_.tracklet_id = tracklet_id; + return *this; +} + +Feature& Feature::frameId(const FrameId& frame_id) { + std::lock_guard lk(mutex_); + data_.frame_id = frame_id; + return *this; +} + +Feature& Feature::objectId(ObjectId id) { + std::lock_guard lk(mutex_); + data_.tracking_label = id; + return *this; +} + +Feature& Feature::depth(Depth d) { + std::lock_guard lk(mutex_); + data_.depth = d; + return *this; +} + +Feature& Feature::rightKeypoint(const Keypoint& right_kp) { + std::lock_guard lk(mutex_); + data_.right_kp = right_kp; + return *this; +} + +bool Feature::usable() const { + std::lock_guard lk(mutex_); + return data_.inlier && (data_.tracklet_id != invalid_id); +} + +bool Feature::isStatic() const { + std::lock_guard lk(mutex_); + return data_.type == KeyPointType::STATIC; +} + +Feature& Feature::markOutlier() { + std::lock_guard lk(mutex_); + data_.inlier = false; + return *this; +} + +Feature& Feature::markInlier() { + std::lock_guard lk(mutex_); + data_.inlier = true; + return *this; +} + +Feature& Feature::markInvalid() { + std::lock_guard lk(mutex_); + data_.tracklet_id = invalid_id; + return *this; +} + +bool Feature::hasDepth() const { + std::lock_guard lk(mutex_); + return !std::isnan(data_.depth); +} + +bool Feature::hasRightKeypoint() const { + std::lock_guard lk(mutex_); + return data_.right_kp.has_value(); +} + +bool Feature::stereoPoint(gtsam::StereoPoint2& stereo) const { + if (!hasRightKeypoint()) { + return false; + } + + const Keypoint& L = this->keypoint(); + const Keypoint& R = this->rightKeypoint(); + stereo = gtsam::StereoPoint2(L(0), R(0), L(1)); + return true; +} + FeatureContainer::FeatureContainer() : feature_map_() {} FeatureContainer::FeatureContainer(const FeaturePtrs feature_vector) { diff --git a/dynosam_cv/src/ImageContainer.cc b/dynosam_cv/src/ImageContainer.cc index 2dca31a91..91de1336a 100644 --- a/dynosam_cv/src/ImageContainer.cc +++ b/dynosam_cv/src/ImageContainer.cc @@ -62,6 +62,8 @@ const ImageWrapper& ImageContainer::objectMotionMask() const { return this->at(kObjectMask); } + + const ImageWrapper& ImageContainer::rightRgb() const { return this->at(kRightRgb); } @@ -78,6 +80,7 @@ ImageWrapper& ImageContainer::opticalFlow() { ImageWrapper& ImageContainer::objectMotionMask() { return this->at(kObjectMask); } + ImageWrapper& ImageContainer::rightRgb() { return this->at(kRightRgb); } diff --git a/dynosam_cv/src/RGBDCamera.cc b/dynosam_cv/src/RGBDCamera.cc index 8275e4c58..fb6287d2e 100644 --- a/dynosam_cv/src/RGBDCamera.cc +++ b/dynosam_cv/src/RGBDCamera.cc @@ -86,6 +86,21 @@ double RGBDCamera::rightKeypoint(double depth, double uL) const { return uL - disparity; } +std::pair RGBDCamera::getStereo( + Feature::Ptr feature, const bool force_recalculation) { + gtsam::StereoPoint2 stereo_point; + // if no right feature or recalculation flag is true + if (!feature->hasRightKeypoint() || force_recalculation) { + // update the right keypoint of feature + if (!this->projectRight(feature)) { + return {false, stereo_point}; + } + } + + CHECK(feature->stereoPoint(stereo_point)); + return {true, stereo_point}; +} + double RGBDCamera::fxb() const { return fx_b_; } StereoCalibPtr RGBDCamera::getFakeStereoCalib() const { diff --git a/dynosam_cv/test/test_cuda_cache.cc b/dynosam_cv/test/test_cuda_cache.cc index 81860b843..d23b27faf 100644 --- a/dynosam_cv/test/test_cuda_cache.cc +++ b/dynosam_cv/test/test_cuda_cache.cc @@ -13,43 +13,43 @@ void printRefcount(const cv::cuda::GpuMat& gpu_mat, LOG(INFO) << prefix << " ref count is " << *gpu_mat.refcount; } -struct Buffer; - -struct GpuContext { - Buffer acquire(int rows, int cols, int type); - - std::unordered_map all_entries; - std::vector active_entries; -}; - -struct Buffer { - struct Internal { - Internal(Buffer* b) : buffer(b) {} - ~Internal() { - LOG(INFO) << "Destricting internal with ref count " << buffer->refCount(); - } - - Buffer* buffer; - }; - // must be before gpu_mat so its destructor is called after! - GpuContext* context_; - Internal internal_; - cv::cuda::GpuMat gpu_mat; - - Buffer(GpuContext* context) : context_(context), internal_(this), gpu_mat() {} - Buffer(GpuContext* context, int rows, int cols, int type) - : context_(context), internal_(this), gpu_mat(rows, cols, type) {} - ~Buffer() { LOG(INFO) << "Destricting buffer with ref coutn " << refCount(); } - - int refCount() const { - if (gpu_mat.refcount == nullptr) - return 0; - else - return *gpu_mat.refcount; - } -}; - -Buffer GpuContext::acquire(int rows, int cols, int type) {} +// struct Buffer; + +// struct GpuContext { +// Buffer acquire(int rows, int cols, int type); + +// std::unordered_map all_entries; +// std::vector active_entries; +// }; + +// struct Buffer { +// struct Internal { +// Internal(Buffer* b) : buffer(b) {} +// ~Internal() { +// LOG(INFO) << "Destricting internal with ref count " << buffer->refCount(); +// } + +// Buffer* buffer; +// }; +// // must be before gpu_mat so its destructor is called after! +// GpuContext* context_; +// Internal internal_; +// cv::cuda::GpuMat gpu_mat; + +// Buffer(GpuContext* context) : context_(context), internal_(this), gpu_mat() {} +// Buffer(GpuContext* context, int rows, int cols, int type) +// : context_(context), internal_(this), gpu_mat(rows, cols, type) {} +// ~Buffer() { LOG(INFO) << "Destricting buffer with ref coutn " << refCount(); } + +// int refCount() const { +// if (gpu_mat.refcount == nullptr) +// return 0; +// else +// return *gpu_mat.refcount; +// } +// }; + +// Buffer GpuContext::acquire(int rows, int cols, int type) {} // TEST(GpuCache, testBasicBuffer) { // Buffer buf1; diff --git a/dynosam_nn/CMakeLists.txt b/dynosam_nn/CMakeLists.txt index 51ae3ce70..0e8bbaa65 100644 --- a/dynosam_nn/CMakeLists.txt +++ b/dynosam_nn/CMakeLists.txt @@ -33,9 +33,13 @@ find_package(TensorRT COMPONENTS onnxparser infer_plugin) set(ENABLE_TENSORRT_CXX_VALUE 1) if(DYNOSAM_NN_USE_TRT AND (NOT CUDAToolkit_FOUND OR NOT TensorRT_FOUND)) - message(WARNING "TensorRT is required for inference to run!" - " Follow the install instructions (if possible) to enable inference" - ) + message(WARNING "TensorRT is required for inference to run!") + if(NOT CUDAToolkit_FOUND) + message(WARNING "Missing CUDAToolkit") + endif() + if(NOT TensorRT_FOUND) + message(WARNING "Missing TensorRT") + endif() set(ENABLE_TENSORRT_CXX_VALUE 0) elseif(NOT DYNOSAM_NN_USE_TRT) message(STATUS "Disabling TensorRT! Inference will not work!") diff --git a/dynosam_nn/README.md b/dynosam_nn/README.md index 34a7322d7..fc2c391c9 100644 --- a/dynosam_nn/README.md +++ b/dynosam_nn/README.md @@ -14,13 +14,22 @@ To use instead of the pre-computed object masks, set the dynosam params set `pre For object-level tracking we use a modified C++ implementation of ByteTracker -# Model file -- YoloV8ObjectDetector requires an exported `.onnx` file which will be converted to a `.engine` file when first loaded. See [export_yolo_tensorrt.py](./export/export_yolo_tensorrt.py) for how to export this file. It should (and by default) be put in the _installed_ share directory of `dynsam_nn` under `weights`. By default the model config will look in this folder for all model weights. - -# Install +## Exporting Model and Weights +- DyoSAM looks for model weights in the `ros_ws/install.../dynosam_nn/weights` directory. +> NOTE: this is the _install_ directory (ie. in the docker container it will be `/home/user/dev_ws/install/dynosam_nn/share/dynosam_nn/weights/`) +- To export the model navigate to `dynosam_nn/export` and run +``` +run python3 export_yolo_tensorrt.py +``` +which should export a `.onnx` model to the weights directory. +- YoloV8ObjectDetector requires an exported `.onnx` file which will be converted to a `.engine` file when first loaded. + +## Install - python3 -m pip install "ultralytics==8.3.0" "numpy<2.0" "opencv-python<5.0" - sudo apt install python3-pybind11 +> NOTE: these dependancies should already be installed when using the Dockerfile + ## Details The OpenCV/Numpy converion code was taken verbatum from https://gitverse.ru/githubcopy/cvnp/content/master diff --git a/dynosam_nn/nodes/ImageSegmenterNode.cc b/dynosam_nn/nodes/ImageSegmenterNode.cc index d82bc1756..534ccabf6 100644 --- a/dynosam_nn/nodes/ImageSegmenterNode.cc +++ b/dynosam_nn/nodes/ImageSegmenterNode.cc @@ -7,6 +7,7 @@ #include #include "cv_bridge/cv_bridge.hpp" +#include "dynosam_common/utils/Statistics.hpp" #include "dynosam_nn/ModelConfig.hpp" #include "dynosam_nn/PyObjectDetector.hpp" #include "dynosam_nn/TrtUtilities.hpp" @@ -54,15 +55,18 @@ class ImageSegmenterNode : public rclcpp::Node { // resized.rows); // auto r = engine_->process(resized); - // // LOG(INFO) << r; + LOG(INFO) << result; // // // // Optional: visualize (disable in headless mode) - cv::imshow("View", result.colouredMask()); - cv::waitKey(1); + // cv::imshow("View", result.colouredMask()); + // cv::waitKey(1); } catch (const cv_bridge::Exception& e) { RCLCPP_ERROR(this->get_logger(), "cv_bridge exception: %s", e.what()); } + + RCLCPP_INFO_STREAM_THROTTLE(this->get_logger(), *this->get_clock(), 2000, + dyno::utils::Statistics::Print()); } rclcpp::Subscription::SharedPtr subscription_; @@ -77,6 +81,7 @@ int main(int argc, char** argv) { FLAGS_logtostderr = 1; FLAGS_colorlogtostderr = 1; FLAGS_log_prefix = 1; + FLAGS_v = 5; // const std::string enginePath = dyno::getNNWeightsPath() / // "yolov8n-seg.engine"; diff --git a/dynosam_nn/src/YoloObjectDetector.cc b/dynosam_nn/src/YoloObjectDetector.cc index 9df27a341..0b6841fd1 100644 --- a/dynosam_nn/src/YoloObjectDetector.cc +++ b/dynosam_nn/src/YoloObjectDetector.cc @@ -5,6 +5,7 @@ #include #include +#include "dynosam_common/utils/TimingStats.hpp" #include "dynosam_common/viz/Colour.hpp" #include "dynosam_nn/trackers/ObjectTracker.hpp" @@ -66,6 +67,9 @@ struct YoloV8ObjectDetector::Impl { const nvinfer1::Dims& output0_dims, const nvinfer1::Dims& output1_dims, ObjectDetectionResult& result) { + utils::TimingStatsCollector timing_all("yolov8_detection.post_process.run", + 5); + if (output1_dims.nbDims != 4 || output1_dims.d[0] != 1 || output1_dims.d[1] != 32) throw std::runtime_error( @@ -74,6 +78,11 @@ struct YoloV8ObjectDetector::Impl { const cv::Size required_size = input_info.shape(); const cv::Size original_size = rgb.size(); + // result result regardless + result.labelled_mask = + cv::Mat::zeros(original_size, ObjectDetectionEngine::MaskDType); + result.input_image = rgb; + const float* output0_data = output0.data(); const float* output1_data = output1.data(); @@ -96,6 +105,8 @@ struct YoloV8ObjectDetector::Impl { // 1. Process prototype masks // Store all prototype masks in a vector for easy access + utils::TimingStatsCollector timing_proto( + "yolov8_detection.post_process.proto", 5); std::vector prototypeMasks; prototypeMasks.reserve(32); for (int m = 0; m < 32; ++m) { @@ -105,6 +116,7 @@ struct YoloV8ObjectDetector::Impl { prototypeMasks.emplace_back( proto.clone()); // Clone to ensure data integrity } + timing_proto.stop(); // 2. Process detections std::vector boxes; @@ -116,6 +128,8 @@ struct YoloV8ObjectDetector::Impl { std::vector> mask_coefficients; mask_coefficients.reserve(num_boxes); + utils::TimingStatsCollector timing_boxes( + "yolov8_detection.post_process.boxes", 5); for (int i = 0; i < num_boxes; ++i) { // Extract box coordinates float xc = output0_data[BoxOffset * num_boxes + i]; @@ -156,6 +170,7 @@ struct YoloV8ObjectDetector::Impl { } mask_coefficients.emplace_back(std::move(mask_coeffs)); } + timing_boxes.stop(); // Early exit if no boxes after confidence threshold if (boxes.empty()) { @@ -163,9 +178,12 @@ struct YoloV8ObjectDetector::Impl { } // 3. Apply NMS + utils::TimingStatsCollector timing_nms("yolov8_detection.post_process.nms", + 5); std::vector nms_indices; cv::dnn::NMSBoxes(boxes, confidences, yolo_config_.conf_threshold, yolo_config_.nms_threshold, nms_indices); + timing_nms.stop(); if (nms_indices.empty()) { return false; } @@ -184,11 +202,9 @@ struct YoloV8ObjectDetector::Impl { const float mask_scale_y = static_cast(mask_h) / required_size.height; + utils::TimingStatsCollector timing_detections( + "yolov8_detection.post_process.detections", 5); std::vector detections; - std::vector binary_detection_masks; - detections.reserve(nms_indices.size()); - binary_detection_masks.reserve(nms_indices.size()); - for (const int idx : nms_indices) { const float confidence = confidences[idx]; const int class_id = class_ids[idx]; @@ -259,23 +275,26 @@ struct YoloV8ObjectDetector::Impl { binary_mask(roi).copyTo(final_binary_mask(roi)); } - binary_detection_masks.push_back(final_binary_mask); - ObjectDetection detection{final_binary_mask, bounding_box, class_label, confidence}; detections.push_back(detection); } + timing_detections.stop(); - cv::Mat labelled_mask = - cv::Mat::zeros(original_size, ObjectDetectionEngine::MaskDType); + // return false; + utils::TimingStatsCollector timing_track( + "yolov8_detection.post_process.track", 5); std::vector tracking_result = tracker_->track(detections); + timing_track.stop(); - // //construct label mask from tracked result - for (size_t i = 0; i < tracking_result.size(); i++) { - const SingleDetectionResult& single_result = tracking_result.at(i); + // return false; + // //construct label mask from tracked result + utils::TimingStatsCollector timing_finalise( + "yolov8_detection.post_process.finalise", 5); + for (const SingleDetectionResult& single_result : tracking_result) { // this may happen if the object was not well tracked if (!single_result.isValid()) { continue; @@ -286,14 +305,13 @@ struct YoloV8ObjectDetector::Impl { single_label_mask.setTo(single_result.object_id, single_result.mask); // set pixel values to object label and update full labelled mask // cv::Mat binary_mask = single_result.mask * single_result.object_id; - labelled_mask += single_label_mask; + result.labelled_mask += single_label_mask; } + // timing_finalise.stop(); result.detections = tracking_result; - result.labelled_mask = labelled_mask; - result.input_image = rgb; - return false; + return true; } inline cv::Mat sigmoid(const cv::Mat& src) { @@ -463,12 +481,18 @@ YoloV8ObjectDetector::YoloV8ObjectDetector(const ModelConfig& config, YoloV8ObjectDetector::~YoloV8ObjectDetector() = default; ObjectDetectionResult YoloV8ObjectDetector::process(const cv::Mat& image) { + static constexpr int kTimingVerbosityLevel = 5; + const auto& input_info = model_info_.input(); const auto& output0_info = model_info_.output0(); const auto& output1_info = model_info_.output1(); std::vector input_data; - impl_->preprocess(input_info, image, input_data); + { + utils::TimingStatsCollector timing("yolov8_detection.pre_process", + kTimingVerbosityLevel); + impl_->preprocess(input_info, image, input_data); + } // allocate input data CHECK(input_device_ptr_.allocate(input_info)); CHECK_EQ(input_device_ptr_.tensor_size, input_data.size()); @@ -489,11 +513,14 @@ ObjectDetectionResult YoloV8ObjectDetector::process(const cv::Mat& image) { context_->setTensorAddress(output1_info.name.c_str(), output1_device_ptr_.device_pointer.get()); - cudaStreamSynchronize(stream_); - bool status = context_->enqueueV3(stream_); - if (!status) { - LOG(ERROR) << "initializing inference failed!"; - return ObjectDetectionResult{}; + { + utils::TimingStatsCollector timing("yolov8_detection.infer"); + cudaStreamSynchronize(stream_); + bool status = context_->enqueueV3(stream_); + if (!status) { + LOG(ERROR) << "initializing inference failed!"; + return ObjectDetectionResult{}; + } } std::vector output0_data, output1_data; @@ -506,8 +533,12 @@ ObjectDetectionResult YoloV8ObjectDetector::process(const cv::Mat& image) { const auto output1_dims = context_->getTensorShape(output1_info.name.c_str()); ObjectDetectionResult result; - impl_->postprocess(input_info, image, output0_data, output1_data, - output0_dims, output1_dims, result); + { + utils::TimingStatsCollector timing("yolov8_detection.post_process", + kTimingVerbosityLevel); + impl_->postprocess(input_info, image, output0_data, output1_data, + output0_dims, output1_dims, result); + } result_ = result; return result_; diff --git a/dynosam_nn/src/trackers/ObjectTracker.cc b/dynosam_nn/src/trackers/ObjectTracker.cc index 9ad79bed9..a23fefd07 100644 --- a/dynosam_nn/src/trackers/ObjectTracker.cc +++ b/dynosam_nn/src/trackers/ObjectTracker.cc @@ -51,7 +51,7 @@ std::vector ByteObjectTracker::track( SingleDetectionResult single_result; single_result.bounding_box = fromByeTrackRect(object_track->getRect()); single_result.mask = object_track->getMask(); - // single_result.class_name = detection.class_name; + single_result.class_name = object_track->getClassName(); single_result.confidence = object_track->getScore(); single_result.object_id = static_cast(object_track->getTrackId()); // //is this the only indicater we should be using!? diff --git a/dynosam_nn/src/trackers/byte_tracker/ByteTracker.cc b/dynosam_nn/src/trackers/byte_tracker/ByteTracker.cc index 9350de283..06add3b4a 100644 --- a/dynosam_nn/src/trackers/byte_tracker/ByteTracker.cc +++ b/dynosam_nn/src/trackers/byte_tracker/ByteTracker.cc @@ -20,7 +20,7 @@ byte_track::ByteTracker::ByteTracker(const int &frame_rate, frame_id_(0), // very important to start track id count at 1 (i.e greater than the // background label!) - track_id_count_(1) {} + track_id_count_(0) {} byte_track::ByteTracker::~ByteTracker() {} @@ -37,8 +37,8 @@ std::vector byte_track::ByteTracker::update( const cv::Rect_ cv_rect_f = object.bounding_box; byte_track::Rect rect(cv_rect_f.x, cv_rect_f.y, cv_rect_f.width, cv_rect_f.height); - const auto strack = std::make_shared( - rect, object.mask, object.class_name, object.confidence); + auto strack = std::make_shared(rect, object.mask, object.class_name, + object.confidence); if (object.confidence >= track_thresh_) { det_stracks.push_back(strack); } else { diff --git a/dynosam_nn/src/trackers/byte_tracker/STrack.cc b/dynosam_nn/src/trackers/byte_tracker/STrack.cc index f689f8a52..b04653acc 100644 --- a/dynosam_nn/src/trackers/byte_tracker/STrack.cc +++ b/dynosam_nn/src/trackers/byte_tracker/STrack.cc @@ -8,8 +8,8 @@ byte_track::STrack::STrack(const Rect& rect, const cv::Mat& mask, mean_(), covariance_(), rect_(rect), - mask_(mask), - class_name_(class_name_), + mask_(mask.clone()), + class_name_(class_name), state_(STrackState::New), is_activated_(false), score_(score), diff --git a/dynosam_opt/include/dynosam_opt/MapNodes.hpp b/dynosam_opt/include/dynosam_opt/MapNodes.hpp index 65128bf85..0a91a76fc 100644 --- a/dynosam_opt/include/dynosam_opt/MapNodes.hpp +++ b/dynosam_opt/include/dynosam_opt/MapNodes.hpp @@ -447,6 +447,7 @@ class FrameNode : public MapNodeBase { /** * @brief True if the requested object was observed at the previous frame. + * TODO: this is hardcoded with k-1!!! Not valid for keyframing!!! * * @param object_id ObjectId * @return true @@ -607,6 +608,25 @@ class ObjectNode : public MapNodeBase { return getSeenFrames().template getLastIndex(); } + /** + * @brief Gets the frame id seen immediately before the latest one! + * If the object has only been seen once, return false + * + * @param frame_id + * @return true + * @return false + */ + bool previouslySeenFrame(FrameId* frame_id = nullptr) const { + const FrameIds all_frames_seen = this->getSeenFrameIds(); + if (all_frames_seen.size() < 2) { + return false; + } + if (frame_id) { + *frame_id = *(all_frames_seen.end() - 2); + } + return true; + } + /** * @brief Get all the frame nodes that have observed this object. * diff --git a/dynosam_ros/CMakeLists.txt b/dynosam_ros/CMakeLists.txt index 26a9c2acb..09c966bce 100644 --- a/dynosam_ros/CMakeLists.txt +++ b/dynosam_ros/CMakeLists.txt @@ -35,6 +35,7 @@ find_package(rclpy REQUIRED) find_package(message_filters REQUIRED) + # setup targets include_directories( include @@ -42,6 +43,7 @@ include_directories( ${EIGEN3_INCLUDE_DIRS} ${PCL_INCLUDE_DIRS} ${dynosam_INCLUDE_DIRS} + ${pcl_conversions_INCLUDE_DIRS} ) @@ -68,7 +70,6 @@ set(LINK_LIBS dynosam::dynosam image_transport::image_transport message_filters::message_filters - pcl_conversions::pcl_conversions rclcpp::rclcpp sensor_msgs::sensor_msgs_library tf2_ros::static_transform_broadcaster_node @@ -76,6 +77,25 @@ set(LINK_LIBS ) +# conditionally compile for message filters +# as the API changes between Jazzy and Kilted +# in Jazzy the version is 4 and Kilted is 7 +# also for some reason in the kilted version we need pcl_conversions +# in the link librariers but in Jazzy we dont +if(${message_filters_VERSION_MAJOR} GREATER_EQUAL 7) +message( + "message filters version detected > 7. Assumuing ROS Kilted is used. " + " Setting MESSAGE_FILTERS_USES_NODE_INTERFACE=1") +add_compile_definitions(MESSAGE_FILTERS_USES_NODE_INTERFACE=1) +# add pcl conversions lib +list(APPEND LINK_LIBS pcl_conversions::pcl_conversions) +else() +message( + "message filters version detected < 7. Assumuing at least ROS Jazzy is used." + " Setting MESSAGE_FILTERS_USES_NODE_INTERFACE=0") +add_compile_definitions(MESSAGE_FILTERS_USES_NODE_INTERFACE=0) +endif() + set(TARGET_EXPORT_DEPS rclcpp std_msgs @@ -162,13 +182,24 @@ target_link_libraries(dynosam_node ${PCL_LIBRARIES} ) - install(TARGETS dynosam_node DESTINATION lib/${PROJECT_NAME}) +add_executable(dynosam_tracking_demo_node nodes/dynosam_tracking_demo_node.cc) +target_link_libraries(dynosam_tracking_demo_node + ${PROJECT_NAME} + ${PCL_LIBRARIES} + +) +install(TARGETS + dynosam_tracking_demo_node + DESTINATION lib/${PROJECT_NAME}) + + + # Install Python executables install(PROGRAMS diff --git a/dynosam_ros/dynosam_ros/dynosam_node.py b/dynosam_ros/dynosam_ros/dynosam_node.py new file mode 100644 index 000000000..adcbda437 --- /dev/null +++ b/dynosam_ros/dynosam_ros/dynosam_node.py @@ -0,0 +1,72 @@ +from launch_ros.actions import Node +from launch.substitutions import LaunchConfiguration + +import os, copy, rclpy +import launch.logging + + +class DynosamNode(Node): + """Custom Node that auto-builds gflags + dynamic args.""" + + DEFAULT_ROS_PACKGE = "dynosam_ros" + DEFAULT_EXECUTABLE_NAME = "dynosam_node" + + def __init__(self, **kwargs): + + if "package" not in kwargs: + kwargs.update("package", DynosamNode.DEFAULT_ROS_PACKGE) + + if "executable" not in kwargs: + kwargs.update("executable", DynosamNode.DEFAULT_EXECUTABLE_NAME) + + super().__init__(**kwargs) + self._logger = launch.logging.get_logger("dynosam_launch.DynoSAMNode") + + def _get_dynamic_gflags(self, context): + """Builds the GFlag argument list at runtime.""" + params_path = LaunchConfiguration("params_path").perform(context) + verbose = LaunchConfiguration("v").perform(context) + output_path = LaunchConfiguration("output_path").perform(context) + + flagfiles = [ + f"--flagfile={os.path.join(params_path, f)}" + for f in os.listdir(params_path) + if f.endswith(".flags") + ] + + args = flagfiles + [f"--v={verbose}", f"--output_path={output_path}"] + + # add non-ROS args from CLI + # should come from the LaunchContext + all_argv = copy.deepcopy(context.argv) + self._logger.info(f"All argv {all_argv}") + non_ros_argv = rclpy.utilities.remove_ros_args(all_argv) + if non_ros_argv: + self._logger.info(f"Appending extra non-ROS argv: {non_ros_argv}") + args.extend(non_ros_argv) + return args + + def execute(self, context): + actions = super().execute(context) + self._logger.info("IN context") + """Called by the LaunchService when this node is executed.""" + # Compute dynamic arguments + gflags_args = self._get_dynamic_gflags(context) + + self._logger.info(f"Resolved DynoSAM gflags: {gflags_args}") + + # Merge any existing static arguments + existing_args = self.cmd + self._logger.info(f"Existing args: {existing_args}") + + # insert gflag commends at the start (immediately after the executable) so + # any additional flags (ie. provided by arguments) may be overwritten + self.cmd[1:1] = gflags_args + + # If needed, modify parameters dynamically + # (you can even call LaunchConfiguration.perform(context) here) + # For example: + # params_path = LaunchConfiguration("params_path").perform(context) + # self.parameters.append({"params_folder_path": params_path}) + + return actions diff --git a/dynosam_ros/dynosam_ros/launch_utils.py b/dynosam_ros/dynosam_ros/launch_utils.py index 1d3196ff1..fd4f66eae 100644 --- a/dynosam_ros/dynosam_ros/launch_utils.py +++ b/dynosam_ros/dynosam_ros/launch_utils.py @@ -105,6 +105,8 @@ def load_dynosam_node(context, *args, **kwargs): motion_mask_cam_topic_config = LaunchConfiguration("motion_mask_cam_topic") optical_flow_cam_topic_config = LaunchConfiguration("optical_flow_cam_topic") + input_image_mode_config = LaunchConfiguration("input_image_mode") + # additional cmdline arguments # args_config = LaunchConfiguration("argv") @@ -189,7 +191,8 @@ def construct_additional_arguments(argv): {"dataset_path": dynosam_dataset_path_config}, {"online": online_config}, {"wait_for_camera_params": wait_for_camera_params_config}, - {"camera_params_timeout": camera_params_timeout_config} + {"camera_params_timeout": camera_params_timeout_config}, + {"input_image_mode": input_image_mode_config} ] is_online = bool(online_config.perform(context)) @@ -203,11 +206,17 @@ def construct_additional_arguments(argv): executable=executable, parameters=parameters, remappings=[ - ("dataprovider/image/camera_info", camera_info_config), - ("dataprovider/image/rgb", rgb_cam_topic_config), - ("dataprovider/image/depth", depth_cam_topic_config), - ("dataprovider/image/mask", motion_mask_cam_topic_config), - ("dataprovider/image/flow", optical_flow_cam_topic_config) + ("dataprovider/camera/camera_info", camera_info_config), + # ("dataprovider/image/rgb", rgb_cam_topic_config), + # ("dataprovider/image/depth", depth_cam_topic_config), + # ("dataprovider/image/mask", motion_mask_cam_topic_config), + # ("dataprovider/image/flow", optical_flow_cam_topic_config) + # for now the dataprovider sub-node is not being extended properly + # but only when using the MultiSync class? + ("image/rgb", rgb_cam_topic_config), + ("image/depth", depth_cam_topic_config), + ("image/mask", motion_mask_cam_topic_config), + ("image/flow", optical_flow_cam_topic_config) ], # prefix='valgrind --tool=massif --massif-out-file=/tmp/massif.out', arguments=arguments, @@ -283,7 +292,8 @@ def generate_dynosam_launch_description(**kwargs): "rgb_cam_topic": "/dyno/camera/rgb", "depth_cam_topic": "/dyno/camera/depth", "motion_mask_cam_topic": "/dyno/camera/motion_mask", - "optical_flow_cam_topic": "/dyno/camera/optical_flow"}, + "optical_flow_cam_topic": "/dyno/camera/optical_flow", + "input_image_mode": 0}, **kwargs) return LaunchDescription([ diff --git a/dynosam_ros/include/dynosam_ros/DataProviderRos.hpp b/dynosam_ros/include/dynosam_ros/DataProviderRos.hpp index 3d53944e8..643b74353 100644 --- a/dynosam_ros/include/dynosam_ros/DataProviderRos.hpp +++ b/dynosam_ros/include/dynosam_ros/DataProviderRos.hpp @@ -45,6 +45,49 @@ namespace dyno { +/** + * @brief Gets CameraParams from a sensor_msgs::msg::CameraInfo recieved on + * the specified topic. This function is blocking until a message is recieved + * (or until the time_to_wait) elapses. + * + * While this function returns a const ref to the CameraParams it also sets + * the internal camera_params_. The camera params are then returned by the + * overwritten getCameraParams, allowing the PipelineManager to access the + * correct camera paramters. + * + * @tparam Rep int64_t, + * @tparam Period std::milli + * @param time_to_wait const std::chrono::duration& + * @param topic const std::string&. Defaults to "image/camera_info" + * @return const CameraParams& + */ +template +CameraParams waitAndSetCameraParams( + std::shared_ptr node, const std::string& topic, + const std::chrono::duration& time_to_wait = + std::chrono::duration(-1)) { + RCLCPP_INFO_STREAM(node->get_logger(), + "Waiting for camera params on topic: " << topic); + // it seems rclcpp::Adaptors do not work yet with wait for message + sensor_msgs::msg::CameraInfo camera_info; + if (rclcpp::wait_for_message( + camera_info, node, topic, time_to_wait)) { + using Adaptor = + rclcpp::TypeAdapter; + CameraParams camera_params; + Adaptor::convert_to_custom(camera_info, camera_params); + RCLCPP_INFO_STREAM(node->get_logger(), + "Received camera params: " << camera_params.toString()); + return camera_params; + } else { + const auto milliseconds = + std::chrono::duration_cast(time_to_wait); + throw DynosamException("Failed to receive camera params on topic " + topic + + " (waited with timeout " + + std::to_string(milliseconds.count()) + " ms)."); + } +} + /** * @brief Base Dataprovider for ROS that implements common image processing * functionalities. @@ -99,54 +142,6 @@ class DataProviderRos : public DataProvider { const cv::Mat readMaskRosImage( const sensor_msgs::msg::Image::ConstSharedPtr& img_msg) const; - /** - * @brief Gets CameraParams from a sensor_msgs::msg::CameraInfo recieved on - * the specified topic. This function is blocking until a message is recieved - * (or until the time_to_wait) elapses. - * - * While this function returns a const ref to the CameraParams it also sets - * the internal camera_params_. The camera params are then returned by the - * overwritten getCameraParams, allowing the PipelineManager to access the - * correct camera paramters. - * - * @tparam Rep int64_t, - * @tparam Period std::milli - * @param time_to_wait const std::chrono::duration& - * @param topic const std::string&. Defaults to "image/camera_info" - * @return const CameraParams& - */ - template - const CameraParams& waitAndSetCameraParams( - const std::chrono::duration& time_to_wait = - std::chrono::duration(-1), - const std::string& topic = "image/camera_info") { - RCLCPP_INFO_STREAM(node_->get_logger(), - "Waiting for camera params on topic: " << topic); - // it seems rclcpp::Adaptors do not work yet with wait for message - sensor_msgs::msg::CameraInfo camera_info; - if (rclcpp::wait_for_message( - camera_info, node_, topic, time_to_wait)) { - using Adaptor = - rclcpp::TypeAdapter; - CameraParams camera_params; - Adaptor::convert_to_custom(camera_info, camera_params); - RCLCPP_INFO_STREAM(node_->get_logger(), "Received camera params: " - << camera_params.toString()); - camera_params_ = camera_params; - return *camera_params_; - } else { - const auto milliseconds = - std::chrono::duration_cast(time_to_wait); - throw DynosamException("Failed to receive camera params on topic " + - topic + " (waited with timeout " + - std::to_string(milliseconds.count()) + " ms)."); - } - } - - CameraParams::Optional getCameraParams() const override { - return camera_params_; - } - protected: /** * @brief Helper function to convert a ROS Image message to a CvImageConstPtr @@ -198,7 +193,6 @@ class DataProviderRos : public DataProvider { protected: rclcpp::Node::SharedPtr node_; - CameraParams::Optional camera_params_; }; } // namespace dyno diff --git a/dynosam_ros/include/dynosam_ros/MultiSync.hpp b/dynosam_ros/include/dynosam_ros/MultiSync.hpp index 9f4513013..66b215166 100644 --- a/dynosam_ros/include/dynosam_ros/MultiSync.hpp +++ b/dynosam_ros/include/dynosam_ros/MultiSync.hpp @@ -1,5 +1,7 @@ #pragma once +#include + #include "dynosam/dataprovider/DataProvider.hpp" // for ImageContainerCallback #include "dynosam_common/Types.hpp" #include "dynosam_common/utils/Tuple.hpp" @@ -8,9 +10,20 @@ #include "message_filters/synchronizer.hpp" #include "rclcpp/node.hpp" #include "rclcpp/node_interfaces/node_interfaces.hpp" +#include "rclcpp/node_interfaces/node_topics.hpp" #include "rclcpp/node_options.hpp" #include "sensor_msgs/msg/image.hpp" +#if MESSAGE_FILTERS_USES_NODE_INTERFACE +// for Kilted and above +#pragma message("ROS2 message filters version >=Kilted detected") +#else +// for at least Jazzy +#pragma message("ROS2 message filters version <=Jazzy detected") +#endif + +// struct + namespace dyno { /** @@ -29,13 +42,26 @@ struct CallbackTypeHelpers { namespace { -template -auto exact_time_policy_helper_impl(std::index_sequence) - -> message_filters::sync_policies::ExactTime< - std::remove_reference_t()))>...>; +template +struct ExactTimePolicyHelperImpl { + // in Jazzy ExactTime filter requires at least two message types to be defined + // (ie. does not support N==1) +#if !MESSAGE_FILTERS_USES_NODE_INTERFACE + static_assert(N > 1, + "Message filters (in at least Jazzy) does not support N==1. " + "MultiSync with N>=2 must be used!"); +#endif + template + static auto get_policy(std::index_sequence) + -> message_filters::sync_policies::ExactTime()))>...>; + + using Type = decltype(get_policy(std::make_index_sequence{})); +}; + template using exact_time_policy_helper = - decltype(exact_time_policy_helper_impl(std::make_index_sequence{})); + typename ExactTimePolicyHelperImpl::Type; template auto callback_type_helper_impl(std::index_sequence) @@ -58,6 +84,17 @@ class MultiSyncBase { virtual void shutdown() = 0; }; +struct MultiSyncConfig { + uint32_t queue_size = 20u; + //! Initalised with SensorDataQoS + rclcpp::QoS subscriber_qos = rclcpp::QoS( + rclcpp::QoSInitialization::from_rmw(rmw_qos_profile_sensor_data)); + rclcpp::SubscriptionOptions subscriber_options{}; + + MultiSyncConfig() = default; + MultiSyncConfig(uint32_t _queue_size) : queue_size(_queue_size) {} +}; + /** * @brief Wrapper for a message_filters::Synchronizer that encapsualtes * subscribing to N topics of type Msg. @@ -83,6 +120,7 @@ class MultiSync : public MultiSyncBase { Config(uint32_t _queue_size) : queue_size(_queue_size) {} }; +#if MESSAGE_FILTERS_USES_NODE_INTERFACE // In ROS Kilted curent version the message filter subscriber base requires a // node interface to the patramters and topics not the node itself. See: // https://docs.ros.org/en/humble/Tutorials/Intermediate/Using-Node-Interfaces-Template-Class.html @@ -94,7 +132,7 @@ class MultiSync : public MultiSyncBase { using RequiredInterfaces = rclcpp::node_interfaces::NodeInterfaces; - +#endif using MessageType = Msg; using SyncPolicy = exact_time_policy_helper; @@ -105,7 +143,7 @@ class MultiSync : public MultiSyncBase { // tuple of pointers must be explicitly initalised MultiSync(rclcpp::Node& node, const std::array& topics, - const Config& config = Config()) + const MultiSyncConfig& config = MultiSyncConfig()) : node_(node), topics_(topics), config_(config) {} bool connect() override { @@ -128,17 +166,41 @@ class MultiSync : public MultiSyncBase { std::stringstream ss; ss << "MultiSync of type " << msg_name << " and size " << N << " is subscribing to topics: "; +#if MESSAGE_FILTERS_USES_NODE_INTERFACE RequiredInterfaces interface(node_); + auto make_subscriber = + [&](const std::string& topic) -> std::shared_ptr { + return std::make_shared( + interface, topic, config_.subscriber_qos, config_.subscriber_options); + }; +#else + rclcpp::Node* interface = &node_; + CHECK_NOTNULL(interface); + auto make_subscriber = + [&](const std::string& topic) -> std::shared_ptr { + auto subscriber = std::make_shared(); + subscriber->subscribe(interface, topic, + config_.subscriber_qos.get_rmw_qos_profile(), + config_.subscriber_options); + return subscriber; + }; +#endif + + auto node_topics = node_.get_node_topics_interface(); + CHECK_NOTNULL(node_topics); + for (size_t i = 0; i < N; i++) { internal::select_apply(i, [&](auto I) { - std::get(subs_) = std::make_shared( - interface, topics_.at(i), config_.subscriber_qos, - config_.subscriber_options); + // for some reason using the message_filter::Subscriber does not adhere + // to remapping manually resolve the topic node + // false is for only_expand + const std::string resolved_topic = + node_topics->resolve_topic_name(topics_.at(i), false); + std::get(subs_) = make_subscriber(resolved_topic); ss << std::get(subs_)->getSubscriber()->get_topic_name() << " "; }); } ss << "\n"; - RCLCPP_INFO_STREAM(node_.get_logger(), ss.str()); } @@ -160,7 +222,18 @@ class MultiSync : public MultiSyncBase { *std::get(subs_)...); // Use lambda to forward messages to callDerived +#if MESSAGE_FILTERS_USES_NODE_INTERFACE sync_->registerCallback(callback_); +#else + // 1. Lambda accepts all 9 arguments (as auto). + // 2. The arguments are passed to the slice_and_call helper, along with + // the compile-time index sequence (Is...), which dictates how many to + // keep (N). + sync_->registerCallback([this](const auto&... args) { + // Is... is visible here as a compile-time constant parameter pack + this->slice_and_call(std::index_sequence(), args...); + }); +#endif RCLCPP_INFO_STREAM(node_.get_logger(), "MultiSync connected and subscribed"); return true; @@ -175,16 +248,37 @@ class MultiSync : public MultiSyncBase { } } + private: +#if !MESSAGE_FILTERS_USES_NODE_INTERFACE + /** + * @brief Helper function to slice the 9-argument signal down to N for the + * callback_. + * @tparam Is Indices 0 to N-1 (provided by the outer createSyncImpl) + * @tparam Args The full 9 arguments from the Signal9 + */ + template + void slice_and_call(std::index_sequence, const Args&... args) { + // Create a tuple containing the 9 incoming arguments (by const reference) + auto args_tuple = std::forward_as_tuple(args...); + // Call the user callback, expanding the tuple only for indices 0 to N-1 + this->callback_(std::get(args_tuple)...); + } +#endif + protected: +#if MESSAGE_FILTERS_USES_NODE_INTERFACE using Subscriber = message_filters::Subscriber; +#else + using Subscriber = message_filters::Subscriber; +#endif /// @brief a tuple of Subscribers of size N using SubscriberTuple = typename internal::repeat_type, N>::type; rclcpp::Node& node_; std::array topics_; - Config config_; + MultiSyncConfig config_; SubscriberTuple subs_{}; std::shared_ptr sync_; @@ -196,7 +290,7 @@ template using MultiImageSync = MultiSync; /// @brief Some common MultiImageSync typedefs -typedef MultiImageSync<1> MultiImageSync1; +// typedef MultiImageSync<1> MultiImageSync1; typedef MultiImageSync<2> MultiImageSync2; typedef MultiImageSync<3> MultiImageSync3; typedef MultiImageSync<4> MultiImageSync4; diff --git a/dynosam_ros/include/dynosam_ros/OnlineDataProviderRos.hpp b/dynosam_ros/include/dynosam_ros/OnlineDataProviderRos.hpp index 6bfc523e6..82f3e54b8 100644 --- a/dynosam_ros/include/dynosam_ros/OnlineDataProviderRos.hpp +++ b/dynosam_ros/include/dynosam_ros/OnlineDataProviderRos.hpp @@ -30,6 +30,7 @@ #pragma once +#include "dynosam/pipeline/PipelineParams.hpp" #include "dynosam_ros/DataProviderRos.hpp" #include "dynosam_ros/MultiSync.hpp" #include "dynosam_ros/adaptors/ImuMeasurementAdaptor.hpp" @@ -40,23 +41,35 @@ namespace dyno { +enum InputImageMode : int { + //! Expects rgb, depth, flow and semantics to be provided + ALL = 0, + //! Expects only rgb and depth images to be provided + RGBD = 1, + STEREO = 2 +}; + struct OnlineDataProviderRosParams { + bool subscribe_imu{true}; bool wait_for_camera_params{true}; int32_t camera_params_timeout{-1}; }; /** - * @brief Online data-provider for DynoSAM. + * @brief Online data-provider for DynoSAM which handles subcription to the IMU + * and the interface for subscribing to images but does not implement this. + * + * Instead input image mode specific classes derive from this class to implement + * the image subscription * - * Subscribes to four synchronized image topics (rgb, depth, mask and optical - * flow) and a camera_info topic (to define the camera intrinsics of the - * system). * * * */ class OnlineDataProviderRos : public DataProviderRos { public: + DYNO_POINTER_TYPEDEFS(OnlineDataProviderRos) + /** * @brief Construct a new OnlineDataProviderRos. * @@ -67,7 +80,9 @@ class OnlineDataProviderRos : public DataProviderRos { * @param params const OnlineDataProviderRosParams& */ OnlineDataProviderRos(rclcpp::Node::SharedPtr node, - const OnlineDataProviderRosParams ¶ms); + const OnlineDataProviderRosParams& params); + + virtual ~OnlineDataProviderRos() = default; /** * @brief Indicates that there is no known end to the dataset @@ -94,21 +109,126 @@ class OnlineDataProviderRos : public DataProviderRos { * @brief Connects all subscribers. * */ - void connect(); + void setupSubscribers(); - private: - void connectImages(); - void connectImu(); + /** + * @brief Checks all params are configured correctly for this (derived) + * data-loader and updates them if necessary + * + * @param dyno_params + */ + virtual void updateAndCheckParams(DynoParams&){}; - private: + protected: + virtual void subscribeImages() = 0; + virtual void unsubscribeImages() = 0; + + OnlineDataProviderRosParams params_; //! Driving frame id for the enture dynosam pipeline FrameId frame_id_; - MultiSyncBase::Ptr image_subscriber_; + + private: + void subscribeImu(); rclcpp::CallbackGroup::SharedPtr imu_callback_group_; using ImuAdaptedType = rclcpp::adapt_type::as; rclcpp::Subscription::SharedPtr imu_sub_; + + std::atomic_bool is_connected{false}; +}; + +/** + * @brief Class to help setup calibration for undistortion/resizing etc + * for an RGBD style input where only a single CameraParams is needed to + * represent a pinhole camera, rather than a stereo setup which needs different + * management + * + * TODO: why not use the UndistortRectifier?! + * + */ +class RGBDTypeCalibrationHelper { + public: + RGBDTypeCalibrationHelper(rclcpp::Node::SharedPtr node, + const OnlineDataProviderRosParams& params); + + void processRGB(const cv::Mat& src, cv::Mat& dst); + void processDepth(const cv::Mat& src, cv::Mat& dst); + + const CameraParams::Optional& getOriginalCameraParams() const; + const CameraParams::Optional& getCameraParams() const; + + private: + void setupNewCameraParams(const CameraParams& original_camera_params, + CameraParams& new_camera_params, + const int& rescale_width, + const int& rescale_height); + + void getParamsFromRos(const CameraParams& original_camera_params, + int& rescale_width, int& rescale_height, + double& depth_scale); + + private: + rclcpp::Node::SharedPtr node_; + CameraParams::Optional original_camera_params_; + CameraParams::Optional camera_params_; + + //! Scale that will be multiplied by the depth image to convert to metric + //! depth Set by ros params + double depth_scale_{0.001}; + + //! Undistort maps + cv::Mat mapx_; + cv::Mat mapy_; +}; + +/** + * @brief Updates and checks that all dyno params are set correctly + * for when only raw image data is providd (ie. RBGD or Stereo) and no + * object/flow pre-processing has been done + * + * @param dyno_params + */ +void updateAndCheckDynoParamsForRawImageInput(DynoParams& dyno_params); + +/** + * @brief Class that subscribes to rgb, depth, motion mask and dense optical + * flow topics. + * + */ +class AllImagesOnlineProviderRos : public OnlineDataProviderRos { + public: + AllImagesOnlineProviderRos(rclcpp::Node::SharedPtr node, + const OnlineDataProviderRosParams& params); + + void subscribeImages() override; + void unsubscribeImages() override; + CameraParams::Optional getCameraParams() const override; + + private: + std::unique_ptr calibration_helper_; + MultiSyncBase::Ptr image_subscriber_; +}; + +/** + * @brief Class that subscribes to rgb, depth, motion mask and dense optical + * flow topics. + * + */ +class RGBDOnlineProviderRos : public OnlineDataProviderRos { + public: + RGBDOnlineProviderRos(rclcpp::Node::SharedPtr node, + const OnlineDataProviderRosParams& params); + + void subscribeImages() override; + void unsubscribeImages() override; + CameraParams::Optional getCameraParams() const override; + + void updateAndCheckParams(DynoParams& dyno_params) override; + + private: + std::unique_ptr calibration_helper_; + MultiSyncBase::Ptr image_subscriber_; }; } // namespace dyno diff --git a/dynosam_ros/include/dynosam_ros/PipelineRos.hpp b/dynosam_ros/include/dynosam_ros/PipelineRos.hpp index b6b50d4f5..ebf232f37 100644 --- a/dynosam_ros/include/dynosam_ros/PipelineRos.hpp +++ b/dynosam_ros/include/dynosam_ros/PipelineRos.hpp @@ -67,6 +67,9 @@ class DynoNode : public rclcpp::Node { // NOT cached!! virtual dyno::DataProvider::Ptr createDataProvider(); + dyno::DataProvider::Ptr createOnlineDataProvider(); + dyno::DataProvider::Ptr createDatasetDataProvider(); + //! Set by the param 'online' and indicates if the OnlineDataProviderRos //! should be used or not bool is_online_; diff --git a/dynosam_ros/include/dynosam_ros/displays/DisplaysCommon.hpp b/dynosam_ros/include/dynosam_ros/displays/DisplaysCommon.hpp index 78f0f946e..daab0c1ce 100644 --- a/dynosam_ros/include/dynosam_ros/displays/DisplaysCommon.hpp +++ b/dynosam_ros/include/dynosam_ros/displays/DisplaysCommon.hpp @@ -68,7 +68,8 @@ struct DisplayCommon { static void publishOdometry(OdometryPub::SharedPtr pub, const gtsam::Pose3& T_world_camera, Timestamp timestamp, const std::string& frame_id, - const std::string& child_frame_id); + const std::string& child_frame_id, + const gtsam::Vector6& velocity = gtsam::Vector6::Zero()); static void publishOdometryPath(PathPub::SharedPtr pub, const gtsam::Pose3Vector& poses, Timestamp latest_timestamp, diff --git a/dynosam_ros/include/dynosam_ros/displays/backend_displays/HybridBackendDisplay.hpp b/dynosam_ros/include/dynosam_ros/displays/backend_displays/HybridBackendDisplay.hpp index c101812f4..b4a0e33da 100644 --- a/dynosam_ros/include/dynosam_ros/displays/backend_displays/HybridBackendDisplay.hpp +++ b/dynosam_ros/include/dynosam_ros/displays/backend_displays/HybridBackendDisplay.hpp @@ -10,14 +10,31 @@ namespace dyno { template struct BackendModuleDisplayTraits; +static HybridAccessorCommon::Ptr hybridAccessorCommonHelper( + Accessor::Ptr accessor) { + CHECK(accessor); + HybridAccessorCommon::Ptr hybrid_accessor_common = + std::dynamic_pointer_cast(accessor); + if (!hybrid_accessor_common) { + throw DynosamException( + "Accessor could not be cast to HybridAccessorCommon"); + } + return hybrid_accessor_common; +} + class HybridModuleDisplayCommon : public BackendModuleDisplayRos { public: - HybridModuleDisplayCommon(const DisplayParams& params, rclcpp::Node* node); + HybridModuleDisplayCommon(const DisplayParams& params, rclcpp::Node* node, + HybridAccessorCommon::Ptr hybrid_accessor); - void publishObjectBoundingBoxes(const BackendOutputPacket::ConstPtr& outpu); + void publishObjectBoundingBoxes(const BackendOutputPacket::ConstPtr& output); + void publishObjectKeyFrames(const FrameId frame_id, + const Timestamp timestamp); private: + HybridAccessorCommon::Ptr hybrid_accessor_; MarkerArrayPub::SharedPtr object_bounding_box_pub_; + MarkerArrayPub::SharedPtr object_key_frame_pub_; }; class ParalleHybridModuleDisplay : public HybridModuleDisplayCommon { @@ -25,7 +42,8 @@ class ParalleHybridModuleDisplay : public HybridModuleDisplayCommon { ParalleHybridModuleDisplay( const DisplayParams& params, rclcpp::Node* node, std::shared_ptr module) - : HybridModuleDisplayCommon(params, node), + : HybridModuleDisplayCommon( + params, node, hybridAccessorCommonHelper(module->getAccessor())), module_(CHECK_NOTNULL(module)) {} void spin(const BackendOutputPacket::ConstPtr& output) override; @@ -39,7 +57,8 @@ class RegularHybridFormulationDisplay : public HybridModuleDisplayCommon { RegularHybridFormulationDisplay( const DisplayParams& params, rclcpp::Node* node, std::shared_ptr module) - : HybridModuleDisplayCommon(params, node), + : HybridModuleDisplayCommon( + params, node, module->derivedAccessor()), module_(CHECK_NOTNULL(module)) { CHECK_NOTNULL(module); } @@ -50,6 +69,10 @@ class RegularHybridFormulationDisplay : public HybridModuleDisplayCommon { std::shared_ptr module_; }; +// TODO: in light of now having many Hybrid formulations we should template on +// HybridFormulation not on RegularHybridFormulation as the display should work +// for all!! + /// @brief Register ParalleHybridModuleDisplay as the acting backend display for /// the Factory policy template <> diff --git a/dynosam_ros/include/dynosam_ros/displays/dynamic_slam_displays/DSDCommonRos.hpp b/dynosam_ros/include/dynosam_ros/displays/dynamic_slam_displays/DSDCommonRos.hpp index d0e38aa15..684765c4c 100644 --- a/dynosam_ros/include/dynosam_ros/displays/dynamic_slam_displays/DSDCommonRos.hpp +++ b/dynosam_ros/include/dynosam_ros/displays/dynamic_slam_displays/DSDCommonRos.hpp @@ -82,20 +82,23 @@ class DSDTransport { // this is technically wrong as we should have a motion at k and a pose at k-1 // to get velocity... static ObjectOdometry constructObjectOdometry( - const gtsam::Pose3& e_H_k_world, const gtsam::Pose3& pose_k, - ObjectId object_id, FrameId frame_id_k, Timestamp timestamp_k, - const std::string& frame_id_link, const std::string& child_frame_id_link); + const gtsam::Pose3& e_H_k_world, const gtsam::Pose3& pose_km1, + ObjectId object_id, FrameId frame_id_k, Timestamp timestamp_k, Timestamp timestamp_km1, + const std::string& frame_id_link, const std::string& child_frame_id_link, + const ObjectIdClassMap& object_class_map); static ObjectOdometryMap constructObjectOdometries( const ObjectMotionMap& motions, const ObjectPoseMap& poses, - FrameId frame_id_k, Timestamp timestamp_k, - const std::string& frame_id_link); + FrameId frame_id_k, Timestamp timestamp_k, Timestamp timestamp_km1, + const std::string& frame_id_link, + const ObjectIdClassMap& object_class_map); static MultiObjectOdometryPath constructMultiObjectOdometryPaths( const ObjectMotionMap& motions, const ObjectPoseMap& poses, Timestamp timestamp_k, const FrameIdTimestampMap& frame_timestamp_map, const std::string& frame_id_link, - bool interpolate_missing_segments = true); + bool interpolate_missing_segments = true, + const ObjectIdClassMap& object_class_map = {}); /** * @brief Nested Publisher that publishes all the object odometries for a @@ -146,11 +149,14 @@ class DSDTransport { std::string frame_id_link_; FrameId frame_id_; Timestamp timestamp_; + Timestamp timestamp_km1_; //! Object odometries for this frame ObjectOdometryMap object_odometries_; MultiObjectOdometryPath object_paths_; + ObjectIdClassMap object_class_map_; + friend class DSDTransport; /** @@ -180,7 +186,8 @@ class DSDTransport { const ObjectMotionMap& motions, const ObjectPoseMap& poses, const std::string& frame_id_link, const FrameIdTimestampMap& frame_timestamp_map, FrameId frame_id, - Timestamp timestamp); + Timestamp timestamp, + const ObjectIdClassMap& object_class_map = {}); }; /** @@ -201,7 +208,8 @@ class DSDTransport { const ObjectPoseMap& poses, const std::string& frame_id_link, const FrameIdTimestampMap& frame_timestamp_map, - FrameId frame_id, Timestamp timestamp); + FrameId frame_id, Timestamp timestamp, + const ObjectIdClassMap& object_class_map = {}); private: rclcpp::Node::SharedPtr node_; @@ -231,7 +239,8 @@ class DSDRos { DSDRos(const DisplayParams& params, rclcpp::Node::SharedPtr node); void publishVisualOdometry(const gtsam::Pose3& T_world_camera, - Timestamp timestamp, const bool publish_tf); + Timestamp timestamp, const bool publish_tf, + const gtsam::Vector6& velocity = gtsam::Vector6::Zero()); void publishVisualOdometryPath(const gtsam::Pose3Vector& poses, Timestamp latest_timestamp); diff --git a/dynosam_ros/include/dynosam_ros/displays/dynamic_slam_displays/FrontendDSDRos.hpp b/dynosam_ros/include/dynosam_ros/displays/dynamic_slam_displays/FrontendDSDRos.hpp index 64ad73130..12a9b4a1d 100644 --- a/dynosam_ros/include/dynosam_ros/displays/dynamic_slam_displays/FrontendDSDRos.hpp +++ b/dynosam_ros/include/dynosam_ros/displays/dynamic_slam_displays/FrontendDSDRos.hpp @@ -44,7 +44,8 @@ namespace dyno { class FrontendDSDRos : public FrontendDisplay, DSDRos { public: - FrontendDSDRos(const DisplayParams params, rclcpp::Node::SharedPtr node); + FrontendDSDRos(const DisplayParams params, rclcpp::Node::SharedPtr node, + rclcpp::Node::SharedPtr ground_truth_node = nullptr); ~FrontendDSDRos() = default; void spinOnceImpl(const VisionImuPacket::ConstPtr& frontend_output) override; @@ -62,17 +63,21 @@ class FrontendDSDRos : public FrontendDisplay, DSDRos { const VisionImuPacket::ConstPtr& frontend_output); private: - //! Transport for ground truth publishing - DSDTransport::UniquePtr dsd_ground_truth_transport_; + struct GroundTruthPublishers { + //! Transport for ground truth publishing + DSDTransport dsd_transport_; + OdometryPub::SharedPtr vo_publisher_; + PathPub::SharedPtr vo_path_publisher_; + + GroundTruthPublishers(rclcpp::Node::SharedPtr ground_truth_node); + }; //! Image Transport for tracking image image_transport::Publisher tracking_image_pub_; - - OdometryPub::SharedPtr vo_ground_truth_publisher_; - PathPub::SharedPtr vo_path_ground_truth_publisher_; - rclcpp::Publisher::SharedPtr dense_dynamic_cloud_pub_; + std::optional ground_truth_publishers_; + //! Accumulated camera poses gtsam::Pose3Vector camera_poses_; //! Accumulated object motions diff --git a/dynosam_ros/launch/bl_dynosam.launch.py b/dynosam_ros/launch/bl_dynosam.launch.py index 482b9930e..de0088f6a 100644 --- a/dynosam_ros/launch/bl_dynosam.launch.py +++ b/dynosam_ros/launch/bl_dynosam.launch.py @@ -53,7 +53,7 @@ def first_steps( dataset_path, params_folder_path=None, output_path="/root/results/DynoSAM", - online=False, + online=True, # CHANGED THIS FROM FALSE -> TRUE wait_for_camera_params=True, camera_params_timeout=-1, camera_frame_id="camera", diff --git a/dynosam_ros/launch/dyno_sam_launch.py b/dynosam_ros/launch/dyno_sam_launch.py index fae18047e..c8dae202d 100644 --- a/dynosam_ros/launch/dyno_sam_launch.py +++ b/dynosam_ros/launch/dyno_sam_launch.py @@ -1,9 +1,32 @@ -from dynosam_ros.launch_utils import generate_dynosam_launch_description + +from dynosam_ros.dynosam_node import DynosamNode +from launch.substitutions import LaunchConfiguration +from launch.actions import DeclareLaunchArgument +from launch import LaunchDescription +from launch_ros.actions import Node + + +from dynosam_ros.launch_utils import get_default_dynosam_params_path def generate_launch_description(): - return generate_dynosam_launch_description( - executable = "dynosam_node", - should_output = True, - world_to_robot_tf = True, - v=30 - ) + return LaunchDescription([ + DeclareLaunchArgument("params_path", default_value=get_default_dynosam_params_path()), + DeclareLaunchArgument("v", default_value="30"), + DeclareLaunchArgument("output_path", default_value="/root/results/DynoSAM/"), + DeclareLaunchArgument("dataset_path", default_value="/root/data/VDO/kitti/0004"), + DynosamNode( + package="dynosam_ros", + executable="dynosam_node", + output="screen", + parameters=[ + {"dataset_path": LaunchConfiguration("dataset_path")}, + {"params_folder_path": LaunchConfiguration("params_path")}, + {"online": False}, + ], + ), + Node( + package='tf2_ros', + executable='static_transform_publisher', + arguments=["0.0", "0.0", "0.0", "1.57", "-1.57", "0.0", "world", "robot"] + ) + ]) diff --git a/dynosam_ros/launch/dyno_sam_online_launch.py b/dynosam_ros/launch/dyno_sam_online_launch.py deleted file mode 100644 index b3f8e17ee..000000000 --- a/dynosam_ros/launch/dyno_sam_online_launch.py +++ /dev/null @@ -1,36 +0,0 @@ -from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription -from launch.launch_description_sources import PythonLaunchDescriptionSource -import os -from ament_index_python.packages import get_package_share_directory -from launch.substitutions import LaunchConfiguration - -def generate_launch_description(): - # Define arguments - declared_arguments = [ - DeclareLaunchArgument('camera_info', default_value='/dyno/camera/camera_info', description='Camera info topic'), - DeclareLaunchArgument('rgb_cam_topic', default_value='/dyno/camera/rgb', description='RGB camera topic'), - DeclareLaunchArgument('depth_cam_topic', default_value='/dyno/camera/depth', description='Depth camera topic'), - DeclareLaunchArgument('motion_mask_cam_topic', default_value='/dyno/camera/motion_mask', description='Motion mask camera topic'), - DeclareLaunchArgument('optical_flow_cam_topic', default_value='/dyno/camera/optical_flow', description='Optical flow camera topic'), - DeclareLaunchArgument('output_path', default_value='/output_path', description='Output path directory') - ] - - # Get the child launch file - dynosam_launch_file = os.path.join( - get_package_share_directory('dynosam_ros'), 'launch', 'dyno_sam_launch.py') - # Include the child launch file and pass all arguments - child_launch = IncludeLaunchDescription( - PythonLaunchDescriptionSource(dynosam_launch_file), - launch_arguments={ - 'camera_info': LaunchConfiguration('camera_info'), - 'rgb_cam_topic': LaunchConfiguration('rgb_cam_topic'), - 'depth_cam_topic': LaunchConfiguration('depth_cam_topic'), - 'motion_mask_cam_topic': LaunchConfiguration('motion_mask_cam_topic'), - 'optical_flow_cam_topic': LaunchConfiguration('optical_flow_cam_topic'), - 'online': 'True', - 'output_path': LaunchConfiguration('output_path') - }.items() - ) - - return LaunchDescription(declared_arguments + [child_launch]) diff --git a/dynosam_ros/launch/dyno_sam_online_rgbd_launch.py b/dynosam_ros/launch/dyno_sam_online_rgbd_launch.py new file mode 100644 index 000000000..ed68cb12b --- /dev/null +++ b/dynosam_ros/launch/dyno_sam_online_rgbd_launch.py @@ -0,0 +1,42 @@ +from dynosam_ros.dynosam_node import DynosamNode +from launch.substitutions import LaunchConfiguration +from launch.actions import DeclareLaunchArgument +from launch import LaunchDescription +from launch_ros.actions import Node +from dynosam_ros.launch_utils import get_default_dynosam_params_path + + +def generate_launch_description(): + return LaunchDescription([ + DeclareLaunchArgument("params_path", default_value=get_default_dynosam_params_path()), + DeclareLaunchArgument("v", default_value="30"), + DeclareLaunchArgument("output_path", default_value="/root/results/DynoSAM/"), + DeclareLaunchArgument("camera_info_topic", default_value="/camera/color/camera_info"), + DeclareLaunchArgument("rgb_cam_topic", default_value="/camera/color/image_rect_color"), + DeclareLaunchArgument("depth_cam_topic", default_value="/camera/aligned_depth_to_color/image_raw"), + DeclareLaunchArgument("rescale_width", default_value="640", description="Image width to rescale to"), + DeclareLaunchArgument("rescale_height", default_value="480", description="Image height to rescale to"), + + DynosamNode( + package="dynosam_ros", + executable="dynosam_node", + output="screen", + parameters=[ + {"params_folder_path": LaunchConfiguration("params_path")}, + {"rescale_width": LaunchConfiguration("rescale_width")}, + {"rescale_height": LaunchConfiguration("rescale_height")}, + {"online": True}, + {"input_image_mode": 1} # Corresponds with InputImageMode::RGBD} + ], + remappings=[ + ("dataprovider/camera/camera_info", LaunchConfiguration("camera_info_topic")), + ("image/rgb", LaunchConfiguration("rgb_cam_topic")), + ("image/depth", LaunchConfiguration("depth_cam_topic")), + ] + ), + Node( + package='tf2_ros', + executable='static_transform_publisher', + arguments=["0.0", "0.0", "0.0", "1.57", "-1.57", "0.0", "world", "robot"] + ) + ]) diff --git a/dynosam_ros/launch/dynosam_tracking_demo_launch.xml b/dynosam_ros/launch/dynosam_tracking_demo_launch.xml new file mode 100644 index 000000000..4a87558c0 --- /dev/null +++ b/dynosam_ros/launch/dynosam_tracking_demo_launch.xml @@ -0,0 +1,18 @@ + + + + + + + + + + + + + + + diff --git a/dynosam_ros/nodes/dynosam_tracking_demo_node.cc b/dynosam_ros/nodes/dynosam_tracking_demo_node.cc new file mode 100644 index 000000000..3e15e847f --- /dev/null +++ b/dynosam_ros/nodes/dynosam_tracking_demo_node.cc @@ -0,0 +1,105 @@ +#include + +#include "cv_bridge/cv_bridge.hpp" +#include "dynosam/frontend/vision/FeatureTracker.hpp" +#include "dynosam_cv/Camera.hpp" +#include "dynosam_cv/ImageContainer.hpp" +#include "dynosam_ros/DataProviderRos.hpp" +#include "dynosam_ros/RosUtils.hpp" +#include "image_transport/image_transport.hpp" +#include "rclcpp/rclcpp.hpp" +#include "sensor_msgs/image_encodings.hpp" +#include "sensor_msgs/msg/image.hpp" + +using namespace dyno; + +int main(int argc, char** argv) { + rclcpp::init(argc, argv); + + google::InitGoogleLogging(argv[0]); + FLAGS_logtostderr = 1; + FLAGS_colorlogtostderr = 1; + FLAGS_log_prefix = 1; + + auto node = rclcpp::Node::make_shared("dynosam_tracking_node"); + + FrontendParams fp; + fp.tracker_params.feature_detector_type = + TrackerParams::FeatureDetectorType::GFFT_CUDA; + fp.tracker_params.max_dynamic_features_per_frame = 300; + fp.tracker_params.prefer_provided_optical_flow = false; + fp.tracker_params.prefer_provided_object_detection = false; + + const CameraParams original_camera_params = + waitAndSetCameraParams(node, "camera_info"); + + const auto original_size = original_camera_params.imageSize(); + cv::Size rescale_size = original_size; + cv::Mat original_K = original_camera_params.getCameraMatrix(); + cv::Mat distortion = original_camera_params.getDistortionCoeffs(); + + cv::Mat new_K = cv::getOptimalNewCameraMatrix( + original_K, distortion, original_size, 1.0, rescale_size); + + cv::Mat mapx, mapy; + LOG(INFO) << CV_32FC1; + cv::initUndistortRectifyMap(original_K, distortion, cv::Mat(), new_K, + rescale_size, CV_32FC1, mapx, mapy); + + dyno::CameraParams::IntrinsicsCoeffs intrinsics; + cv::Mat K_double; + new_K.convertTo(K_double, CV_64F); + // TODO: should make this a constructor... + dyno::CameraParams::convertKMatrixToIntrinsicsCoeffs(K_double, intrinsics); + dyno::CameraParams::DistortionCoeffs zero_distortion(4, 0); + + CameraParams new_camera_params(intrinsics, zero_distortion, rescale_size, + original_camera_params.getDistortionModel(), + original_camera_params.getExtrinsics()); + + LOG(INFO) << "New camera " << new_camera_params.toString(); + + auto camera = std::make_shared(new_camera_params); + auto tracker = std::make_shared(fp, camera); + + FrameId frame_id{0}; + + auto subscription = node->create_subscription( + "image_raw", 100, + [&tracker, &frame_id, &mapx, + &mapy](const sensor_msgs::msg::Image::ConstSharedPtr& msg) { + cv_bridge::CvImageConstPtr cv_ptr = cv_bridge::toCvCopy(msg); + const cv::Mat rgb = cv_ptr->image; + + cv::Mat rgb_undistort; + cv::remap(rgb, rgb_undistort, mapx, mapy, cv::INTER_LINEAR); + + // output will have the same type as mapx/y so covnert back to required + // type + rgb_undistort.convertTo(rgb_undistort, rgb.type()); + + const Timestamp timestamp = utils::fromRosTime(msg->header.stamp); + + ImageContainer image_container(frame_id, timestamp); + image_container.rgb(rgb); + + frame_id++; + + auto frame = tracker->track(frame_id, timestamp, image_container); + Frame::Ptr previous_frame = tracker->getPreviousFrame(); + + if (previous_frame) { + ImageTracksParams track_viz_params(true); + track_viz_params.show_intermediate_tracking = true; + cv::Mat tracking = tracker->computeImageTracks( + *previous_frame, *frame, track_viz_params); + + if (!tracking.empty()) cv::imshow("Tracking", tracking); + cv::waitKey(1); + } + }); + + rclcpp::spin(node); + rclcpp::shutdown(); + return 0; +} diff --git a/dynosam_ros/rviz/rviz_dsd.rviz b/dynosam_ros/rviz/rviz_dsd.rviz index da70b450f..209194702 100644 --- a/dynosam_ros/rviz/rviz_dsd.rviz +++ b/dynosam_ros/rviz/rviz_dsd.rviz @@ -7,15 +7,19 @@ Panels: - /Global Options1 - /Status1 - /Backend1 + - /Backend1/ObjectOdometryPath1 - /Backend1/Visual Odometry1/Status1 - /Backend1/Visual Odometry1/Shape1 - /Frontend1 - - /Frontend1/Static Map1 + - /Frontend1/Dynamic Map1 - /Frontend1/Visual Odometry1/Shape1 - /Ground Truth1 + - /Ground Truth1/Visual Odometry1/Topic1 - /Ground Truth1/Visual Odometry1/Shape1 - Splitter Ratio: 0.5 - Tree Height: 378 + - /Ground Truth1/Camera Path1/Topic1 + - /Ground Truth1/Object Odometry1/Topic1 + Splitter Ratio: 0.49902912974357605 + Tree Height: 752 - Class: rviz_common/Selection Name: Selection - Class: rviz_common/Tool Properties @@ -80,7 +84,7 @@ Visualization Manager: Selectable: true Size (Pixels): 3 Size (m): 0.07000000029802322 - Style: Flat Squares + Style: Points Topic: Depth: 5 Durability Policy: Volatile @@ -160,7 +164,7 @@ Visualization Manager: Head Diameter: 0.30000001192092896 Head Length: 0.20000000298023224 Length: 0.30000001192092896 - Line Width: 0.10000000149011612 + Line Width: 0.05000000074505806 Name: ObjectOdometryPath Pose Style: None Radius: 0.029999999329447746 @@ -268,7 +272,7 @@ Visualization Manager: Selectable: true Size (Pixels): 3 Size (m): 0.03999999910593033 - Style: Flat Squares + Style: Points Topic: Depth: 5 Durability Policy: Volatile @@ -469,8 +473,8 @@ Visualization Manager: Durability Policy: Volatile Filter size: 10 History Policy: Keep Last - Reliability Policy: Reliable - Value: /dynosam/frontend/ground_truth/odometry + Reliability Policy: Best Effort + Value: /dynosam/ground_truth/odometry Value: true - Alpha: 1 Buffer Length: 1 @@ -497,8 +501,8 @@ Visualization Manager: Durability Policy: Volatile Filter size: 10 History Policy: Keep Last - Reliability Policy: Reliable - Value: /dynosam/frontend/ground_truth/odometry_path + Reliability Policy: Best Effort + Value: /dynosam/ground_truth/odometry_path Value: true - Axes Length: 0.4000000059604645 Axes Radius: 0.029999999329447746 @@ -518,7 +522,7 @@ Visualization Manager: Scale: 1 Value: true Value: false - Enabled: false + Enabled: true Keep: 1 Name: Object Odometry Object Label: true @@ -527,14 +531,14 @@ Visualization Manager: Durability Policy: Volatile Filter size: 10 History Policy: Keep Last - Reliability Policy: Reliable - Value: /dynosam/frontend/ground_truth/object_odometry - Value: false + Reliability Policy: Best Effort + Value: /dynosam/ground_truth/object_odometry + Value: true Velocity: true Enabled: true Name: Ground Truth - Class: rviz_default_plugins/Image - Enabled: true + Enabled: false Max Value: 1 Median window: 5 Min Value: 0 @@ -546,7 +550,7 @@ Visualization Manager: History Policy: Keep Last Reliability Policy: Reliable Value: /dynosam/tracking_image - Value: true + Value: false - Alpha: 1 Autocompute Intensity Bounds: true Autocompute Value Bounds: @@ -580,6 +584,18 @@ Visualization Manager: Use Fixed Frame: true Use rainbow: true Value: false + - Class: rviz_default_plugins/MarkerArray + Enabled: false + Name: MarkerArray + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /dynosam/object_bounding_boxes + Value: false Enabled: true Global Options: Background Color: 186; 189; 182 @@ -626,25 +642,25 @@ Visualization Manager: Views: Current: Class: rviz_default_plugins/Orbit - Distance: 8.840835571289062 + Distance: 8.309072494506836 Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 Swap Stereo Eyes: false Value: false Focal Point: - X: 0.019038256257772446 - Y: -1.3721035718917847 - Z: 0.9078776240348816 + X: 1.266749382019043 + Y: 1.5098763704299927 + Z: 1.9570939540863037 Focal Shape Fixed Size: false Focal Shape Size: 0.05000000074505806 Invert Z Axis: false Name: Current View Near Clip Distance: 0.009999999776482582 - Pitch: -0.9497978687286377 + Pitch: -1.0147963762283325 Target Frame: camera Value: Orbit (rviz_default_plugins) - Yaw: 4.900396823883057 + Yaw: 4.7804036140441895 Saved: ~ Window Geometry: Displays: @@ -652,7 +668,7 @@ Window Geometry: Height: 1043 Hide Left Dock: false Hide Right Dock: false - QMainWindow State: 000000ff00000000fd00000004000000000000020500000379fc020000000cfb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b00000203000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000001c0054007200610063006b0069006e006700200049006d0061006700650100000244000001700000001600fffffffb0000000a0049006d0061006700650000000000ffffffff0000000000000000fb0000000a0049006d00610067006501000002650000014f0000000000000000fb0000001c0054007200610063006b0069006e006700200049006d00610067006501000002e2000000d20000000000000000000000010000010f00000379fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003b00000379000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007800000003efc0100000002fb0000000800540069006d00650100000000000007800000025300fffffffb0000000800540069006d00650100000000000004500000000000000000000004600000037900000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + QMainWindow State: 000000ff00000000fd00000004000000000000020500000379fc020000000cfb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b00000379000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000001c0054007200610063006b0069006e006700200049006d0061006700650000000244000001700000001600fffffffb0000000a0049006d0061006700650000000000ffffffff0000000000000000fb0000000a0049006d00610067006501000002650000014f0000000000000000fb0000001c0054007200610063006b0069006e006700200049006d00610067006501000002e2000000d20000000000000000000000010000010f00000379fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003b00000379000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007800000003efc0100000002fb0000000800540069006d00650100000000000007800000025300fffffffb0000000800540069006d00650100000000000004500000000000000000000004600000037900000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Selection: collapsed: false Time: diff --git a/dynosam_ros/src/DataProviderRos.cc b/dynosam_ros/src/DataProviderRos.cc index 0019a76bc..2dbc5947b 100644 --- a/dynosam_ros/src/DataProviderRos.cc +++ b/dynosam_ros/src/DataProviderRos.cc @@ -40,6 +40,45 @@ namespace dyno { DataProviderRos::DataProviderRos(rclcpp::Node::SharedPtr node) : DataProvider(), node_(node) {} +// specalise conversion function for depth image to handle the case we are given +// float32 and 16UC1 bit images +template <> +const cv::Mat DataProviderRos::convertRosImage( + const sensor_msgs::msg::Image::ConstSharedPtr& img_msg) const { + const cv_bridge::CvImageConstPtr cvb_image = readRosImage(img_msg); + const cv::Mat img = cvb_image->image; + + try { + image_traits::validate(img); + return img; + + } catch (const InvalidImageTypeException& exception) { + // handle the case its a float32 + if (img.type() == CV_32FC1) { + cv::Mat depth64; + img.convertTo(depth64, CV_64FC1); + image_traits::validate(depth64); + return depth64; + } else if (img.type() == CV_16UC1) { + cv::Mat depth64; + img.convertTo(depth64, CV_64FC1); + image_traits::validate(depth64); + return depth64; + } + + RCLCPP_FATAL_STREAM(node_->get_logger(), + image_traits::name() + << " Image msg was of the wrong type (validate " + "failed with exception " + << exception.what() << "). " + << "ROS encoding type used was " + << cvb_image->encoding); + + rclcpp::shutdown(); + return cv::Mat(); + } +} + const cv::Mat DataProviderRos::readRgbRosImage( const sensor_msgs::msg::Image::ConstSharedPtr& img_msg) const { return convertRosImage(img_msg); diff --git a/dynosam_ros/src/OnlineDataProviderRos.cc b/dynosam_ros/src/OnlineDataProviderRos.cc index 7e0ab21e6..e9407aaf4 100644 --- a/dynosam_ros/src/OnlineDataProviderRos.cc +++ b/dynosam_ros/src/OnlineDataProviderRos.cc @@ -30,51 +30,252 @@ #include "dynosam_ros/OnlineDataProviderRos.hpp" +#include "dynosam_common/Types.hpp" #include "dynosam_ros/RosUtils.hpp" namespace dyno { -OnlineDataProviderRos::OnlineDataProviderRos( - rclcpp::Node::SharedPtr node, const OnlineDataProviderRosParams ¶ms) - : DataProviderRos(node), frame_id_(0u) { - if (params.wait_for_camera_params) { - waitAndSetCameraParams( - std::chrono::milliseconds(params.camera_params_timeout)); +template <> +std::string to_string(const InputImageMode& input_image_mode) { + std::string status_str = ""; + switch (input_image_mode) { + case InputImageMode::ALL: { + status_str = "ALL"; + break; + } + case InputImageMode::RGBD: { + status_str = "RGBD"; + break; + } + case InputImageMode::STEREO: { + status_str = "STEREO"; + break; + } } + return status_str; +} - connect(); +OnlineDataProviderRos::OnlineDataProviderRos( + rclcpp::Node::SharedPtr node, const OnlineDataProviderRosParams& params) + : DataProviderRos(node), params_(params), frame_id_(0u) { CHECK_EQ(shutdown_, false); } -bool OnlineDataProviderRos::spin() { return !shutdown_; } +bool OnlineDataProviderRos::spin() { + if (!is_connected) { + RCLCPP_ERROR_THROTTLE( + node_->get_logger(), *node_->get_clock(), 1000, + "OnlineDataProviderRos spinning but subscribers are not connected. " + "Did you forget to call OnlineDataProviderRos::setupSubscribers()?"); + } + return !shutdown_; +} void OnlineDataProviderRos::shutdown() { shutdown_ = true; // shutdown synchronizer RCLCPP_INFO_STREAM(node_->get_logger(), "Shutting down OnlineDataProviderRos"); - image_subscriber_->shutdown(); + if (imu_sub_) imu_sub_.reset(); + unsubscribeImages(); + is_connected = false; } -void OnlineDataProviderRos::connect() { - connectImages(); - connectImu(); +void OnlineDataProviderRos::setupSubscribers() { + subscribeImages(); + subscribeImu(); shutdown_ = false; + is_connected = true; +} + +void OnlineDataProviderRos::subscribeImu() { + if (imu_sub_) imu_sub_.reset(); + + imu_callback_group_ = + node_->create_callback_group(rclcpp::CallbackGroupType::Reentrant); + + rclcpp::SubscriptionOptions imu_sub_options; + imu_sub_options.callback_group = imu_callback_group_; + + imu_sub_ = node_->create_subscription( + "imu", rclcpp::SensorDataQoS(), + [&](const dyno::ImuMeasurement& imu) -> void { + if (!imu_single_input_callback_) { + RCLCPP_ERROR_THROTTLE( + node_->get_logger(), *node_->get_clock(), 1000, + "Imu callback triggered but " + "imu_single_input_callback_ is not registered!"); + return; + } + imu_single_input_callback_(imu); + }, + imu_sub_options); +} + +RGBDTypeCalibrationHelper::RGBDTypeCalibrationHelper( + rclcpp::Node::SharedPtr node, const OnlineDataProviderRosParams& params) + : node_(node) { + if (params.wait_for_camera_params) { + const CameraParams original_camera_params = waitAndSetCameraParams( + node, "camera/camera_info", + std::chrono::milliseconds(params.camera_params_timeout)); + + int rescale_width, rescale_height; + getParamsFromRos(original_camera_params, rescale_width, rescale_height, + depth_scale_); + + CameraParams camera_params; + setupNewCameraParams(original_camera_params, camera_params, rescale_width, + rescale_height); + + original_camera_params_ = original_camera_params; + camera_params_ = camera_params; + } +} + +void RGBDTypeCalibrationHelper::processRGB(const cv::Mat& src, cv::Mat& dst) { + cv::remap(src, dst, mapx_, mapy_, cv::INTER_LINEAR); + // output will have the same type as mapx/y so covnert back to required type + dst.convertTo(dst, src.type()); +} +void RGBDTypeCalibrationHelper::processDepth(const cv::Mat& src, cv::Mat& dst) { + cv::remap(src, dst, mapx_, mapy_, cv::INTER_LINEAR); + CHECK(src.type() == ImageType::Depth::OpenCVType); + // output will have the same type as mapx/y so covnert back to required type + dst.convertTo(dst, src.type()); + + // convert the depth map to metirc scale + // data-type shoule match + src *= depth_scale_; +} + +const CameraParams::Optional& +RGBDTypeCalibrationHelper::getOriginalCameraParams() const { + return original_camera_params_; +} +const CameraParams::Optional& RGBDTypeCalibrationHelper::getCameraParams() + const { + return camera_params_; +} + +void RGBDTypeCalibrationHelper::setupNewCameraParams( + const CameraParams& original_camera_params, CameraParams& new_camera_params, + const int& rescale_width, const int& rescale_height) { + const auto original_size = original_camera_params.imageSize(); + const cv::Size rescale_size = cv::Size(rescale_width, rescale_height); + cv::Mat original_K = original_camera_params.getCameraMatrix(); + const cv::Mat distortion = original_camera_params.getDistortionCoeffs(); + + cv::Mat new_K = cv::getOptimalNewCameraMatrix( + original_K, distortion, original_size, 1.0, rescale_size); + + cv::initUndistortRectifyMap(original_K, distortion, cv::Mat(), new_K, + rescale_size, CV_32FC1, mapx_, mapy_); + + dyno::CameraParams::IntrinsicsCoeffs intrinsics; + cv::Mat K_double; + new_K.convertTo(K_double, CV_64F); + dyno::CameraParams::convertKMatrixToIntrinsicsCoeffs(K_double, intrinsics); + dyno::CameraParams::DistortionCoeffs zero_distortion(4, 0); + + new_camera_params = CameraParams(intrinsics, zero_distortion, rescale_size, + original_camera_params.getDistortionModel(), + original_camera_params.getExtrinsics()); +} + +void RGBDTypeCalibrationHelper::getParamsFromRos( + const CameraParams& original_camera_params, int& rescale_width, + int& rescale_height, double& depth_scale) { + rescale_width = ParameterConstructor(node_.get(), "rescale_width", + original_camera_params.ImageWidth()) + .description( + "Image width to rescale to. If not provided or -1 " + "image will be inchanged") + .finish() + .get(); + if (rescale_width == -1) { + rescale_width = original_camera_params.ImageWidth(); + } + + rescale_height = ParameterConstructor(node_.get(), "rescale_height", + original_camera_params.ImageHeight()) + .description( + "Image height to rescale to. If not provided or -1 " + "image will be inchanged") + .finish() + .get(); + if (rescale_height == -1) { + rescale_height = original_camera_params.ImageHeight(); + } + + depth_scale = ParameterConstructor(node_.get(), "depth_scale", 0.001) + .description( + "Value to scale the depth image from a disparity map " + "to metric depth") + .finish() + .get(); } -void OnlineDataProviderRos::connectImages() { - rclcpp::Node &node_ref = *node_; +void updateAndCheckDynoParamsForRawImageInput(DynoParams& dyno_params) { + auto& tracker_params = dyno_params.frontend_params_.tracker_params; + if (tracker_params.prefer_provided_optical_flow) { + LOG(WARNING) + << "InputImageMode not set to ALL but prefer_provided_optical_flow is " + "true - param will be updated!"; + tracker_params.prefer_provided_optical_flow = false; + } + if (tracker_params.prefer_provided_object_detection) { + LOG(WARNING) + << "InputImageMode not set to ALL but prefer_provided_object_detection " + "is true - param will be updated!"; + tracker_params.prefer_provided_object_detection = false; + // TODO: should also warn in this case that gt tracking will not match!! + } +} + +AllImagesOnlineProviderRos::AllImagesOnlineProviderRos( + rclcpp::Node::SharedPtr node, const OnlineDataProviderRosParams& params) + : OnlineDataProviderRos(node, params) { + LOG(INFO) << "Creating AllImagesOnlineProviderRos"; + calibration_helper_ = + std::make_unique(node, params); + + // All Images only works with undisroted images as the pre-processing must be + // done on undistorted + // images particularly for optical flow + auto original_camera_params = calibration_helper_->getOriginalCameraParams(); + if (original_camera_params) { + const cv::Mat distortion = original_camera_params->getDistortionCoeffs(); + if (cv::countNonZero(distortion.reshape(1)) != 0) { + // not all zeros + DYNO_THROW_MSG(DynosamException) + << "In AllImagesOnlineProviderRos the original camera params has " + "distortion coeffs which means the images " + " propvided have not been undisroted!"; + } + } else { + DYNO_THROW_MSG(DynosamException) + << "No original camera params found for AllImagesOnlineProviderRos"; + } +} - static const std::array &topics = { +void AllImagesOnlineProviderRos::subscribeImages() { + rclcpp::Node& node_ref = *node_; + static const std::array& topics = { "image/rgb", "image/depth", "image/flow", "image/mask"}; + MultiSyncConfig config; + config.queue_size = 20; + // config.subscriber_options.callback_group = + // node_ref.create_callback_group(rclcpp::CallbackGroupType::Reentrant); + std::shared_ptr multi_image_sync = - std::make_shared(node_ref, topics, 20); + std::make_shared(node_ref, topics, config); multi_image_sync->registerCallback( - [this](const sensor_msgs::msg::Image::ConstSharedPtr &rgb_msg, - const sensor_msgs::msg::Image::ConstSharedPtr &depth_msg, - const sensor_msgs::msg::Image::ConstSharedPtr &flow_msg, - const sensor_msgs::msg::Image::ConstSharedPtr &mask_msg) { + [this](const sensor_msgs::msg::Image::ConstSharedPtr& rgb_msg, + const sensor_msgs::msg::Image::ConstSharedPtr& depth_msg, + const sensor_msgs::msg::Image::ConstSharedPtr& flow_msg, + const sensor_msgs::msg::Image::ConstSharedPtr& mask_msg) { if (!image_container_callback_) { RCLCPP_ERROR_THROTTLE(node_->get_logger(), *node_->get_clock(), 1000, "Image Sync callback triggered but " @@ -104,28 +305,74 @@ void OnlineDataProviderRos::connectImages() { image_subscriber_ = multi_image_sync; } -void OnlineDataProviderRos::connectImu() { - if (imu_sub_) imu_sub_.reset(); +void AllImagesOnlineProviderRos::unsubscribeImages() { + if (image_subscriber_) image_subscriber_->shutdown(); +} - imu_callback_group_ = node_->create_callback_group( - rclcpp::CallbackGroupType::MutuallyExclusive); +CameraParams::Optional AllImagesOnlineProviderRos::getCameraParams() const { + return calibration_helper_->getCameraParams(); +} - rclcpp::SubscriptionOptions imu_sub_options; - imu_sub_options.callback_group = imu_callback_group_; +RGBDOnlineProviderRos::RGBDOnlineProviderRos( + rclcpp::Node::SharedPtr node, const OnlineDataProviderRosParams& params) + : OnlineDataProviderRos(node, params) { + LOG(INFO) << "Creating RGBDOnlineProviderRos"; + calibration_helper_ = + std::make_unique(node, params); +} - imu_sub_ = node_->create_subscription( - "imu", rclcpp::SensorDataQoS(), - [&](const dyno::ImuMeasurement &imu) -> void { - if (!imu_single_input_callback_) { - RCLCPP_ERROR_THROTTLE( - node_->get_logger(), *node_->get_clock(), 1000, - "Imu callback triggered but " - "imu_single_input_callback_ is not registered!"); +void RGBDOnlineProviderRos::subscribeImages() { + rclcpp::Node& node_ref = *node_; + static const std::array& topics = {"image/rgb", + "image/depth"}; + + MultiSyncConfig config; + config.queue_size = 20; + // config.subscriber_options.callback_group = + // node_ref.create_callback_group(rclcpp::CallbackGroupType::Reentrant); + + std::shared_ptr multi_image_sync = + std::make_shared(node_ref, topics, config); + multi_image_sync->registerCallback( + [this](const sensor_msgs::msg::Image::ConstSharedPtr& rgb_msg, + const sensor_msgs::msg::Image::ConstSharedPtr& depth_msg) { + if (!image_container_callback_) { + RCLCPP_ERROR_THROTTLE(node_->get_logger(), *node_->get_clock(), 1000, + "Image Sync callback triggered but " + "image_container_callback_ is not registered!"); return; } - imu_single_input_callback_(imu); - }, - imu_sub_options); + + cv::Mat rgb = readRgbRosImage(rgb_msg); + cv::Mat depth = readDepthRosImage(depth_msg); + + calibration_helper_->processRGB(rgb, rgb); + calibration_helper_->processDepth(depth, depth); + + const Timestamp timestamp = utils::fromRosTime(rgb_msg->header.stamp); + const FrameId frame_id = frame_id_; + frame_id_++; + + ImageContainer image_container(frame_id, timestamp); + image_container.rgb(rgb).depth(depth); + + image_container_callback_( + std::make_shared(image_container)); + }); + CHECK(multi_image_sync->connect()); + image_subscriber_ = multi_image_sync; +} + +void RGBDOnlineProviderRos::unsubscribeImages() { + if (image_subscriber_) image_subscriber_->shutdown(); +} + +CameraParams::Optional RGBDOnlineProviderRos::getCameraParams() const { + return calibration_helper_->getCameraParams(); +} + +void RGBDOnlineProviderRos::updateAndCheckParams(DynoParams& dyno_params) { + updateAndCheckDynoParamsForRawImageInput(dyno_params); } } // namespace dyno diff --git a/dynosam_ros/src/PipelineRos.cc b/dynosam_ros/src/PipelineRos.cc index f6f4ee3a9..ad5f157c0 100644 --- a/dynosam_ros/src/PipelineRos.cc +++ b/dynosam_ros/src/PipelineRos.cc @@ -69,30 +69,65 @@ DynoNode::DynoNode(const std::string& node_name, dyno::DataProvider::Ptr DynoNode::createDataProvider() { if (is_online_) { - RCLCPP_INFO_STREAM( - this->get_logger(), - "Online DataProvider selected. Waiting for ROS topics..."); - OnlineDataProviderRosParams online_params; - online_params.wait_for_camera_params = - ParameterConstructor(this, "wait_for_camera_params", - online_params.wait_for_camera_params) - .description( - "If the online DataProvider should wait for the camera params " - "on a ROS topic!") - .finish() - .get(); - online_params.camera_params_timeout = - ParameterConstructor(this, "camera_params_timeout", - online_params.camera_params_timeout) - .description( - "When waiting for camera params, how long the online " - "DataProvider should wait before time out (ms)") - .finish() - .get(); - return std::make_shared( - this->create_sub_node("dataprovider"), online_params); + return createOnlineDataProvider(); + } else { + return createDatasetDataProvider(); + } +} + +dyno::DataProvider::Ptr DynoNode::createOnlineDataProvider() { + RCLCPP_INFO_STREAM(this->get_logger(), + "Online DataProvider selected. Waiting for ROS topics..."); + + OnlineDataProviderRosParams online_params; + online_params.wait_for_camera_params = + ParameterConstructor(this, "wait_for_camera_params", + online_params.wait_for_camera_params) + .description( + "If the online DataProvider should wait for the camera params " + "on a ROS topic!") + .finish() + .get(); + online_params.camera_params_timeout = + ParameterConstructor(this, "camera_params_timeout", + online_params.camera_params_timeout) + .description( + "When waiting for camera params, how long the online " + "DataProvider should wait before time out (ms)") + .finish() + .get(); + InputImageMode image_mode = static_cast( + ParameterConstructor(this, "input_image_mode", + static_cast(InputImageMode::ALL)) + .description("Which input image mode to run the pipeline in (e.g " + "ALL, RGBD, STEREO)...") + .finish() + .get()); + + OnlineDataProviderRos::Ptr online_data_provider = nullptr; + switch (image_mode) { + case InputImageMode::ALL: + online_data_provider = std::make_shared( + this->create_sub_node("dataprovider"), online_params); + break; + case InputImageMode::RGBD: + online_data_provider = std::make_shared( + this->create_sub_node("dataprovider"), online_params); + break; + + default: + LOG(FATAL) << "Unknown image_mode"; + return nullptr; } + CHECK(online_data_provider); + // update any params in case they do not conflixt with the expected input + online_data_provider->updateAndCheckParams(*dyno_params_); + online_data_provider->setupSubscribers(); + return online_data_provider; +} + +dyno::DataProvider::Ptr DynoNode::createDatasetDataProvider() { auto params_path = getParamsPath(); auto dataset_path = getDatasetPath(); auto dyno_params = getDynoParams(); @@ -107,9 +142,9 @@ dyno::DataProvider::Ptr DynoNode::createDataProvider() { return data_loader; } -std::string DynoNode::searchForPathWithParams(const std::string& param_name, - const std::string& default_path, - const std::string& description) { +std::string DynoNode::searchForPathWithParams( + const std::string& param_name, const std::string& /*default_path*/, + const std::string& description) { // check if we've alrady declared this param // use non-default version so that ParameterConstructor throws exception if no // parameter is provided on the param server @@ -128,6 +163,9 @@ DynoPipelineManagerRos::DynoPipelineManagerRos( void DynoPipelineManagerRos::initalisePipeline() { RCLCPP_INFO_STREAM(this->get_logger(), "Starting DynoPipelineManagerRos"); + // load data provider first as this could change some params to ensure + // they match with the data-provider selected! + auto data_loader = createDataProvider(); auto params = getDynoParams(); // setup display params @@ -147,7 +185,8 @@ void DynoPipelineManagerRos::initalisePipeline() { .get(); auto frontend_display = std::make_shared( - display_params, this->create_sub_node("frontend")); + display_params, this->create_sub_node("frontend"), + this->create_sub_node("ground_truth")); auto backend_display = std::make_shared( display_params, this->create_sub_node("backend")); @@ -180,7 +219,6 @@ void DynoPipelineManagerRos::initalisePipeline() { auto factory = RosBackendFactory::Create(params.backend_type, display_params, this); - auto data_loader = createDataProvider(); pipeline_ = std::make_unique( params, data_loader, frontend_display, backend_display, factory, hooks); } diff --git a/dynosam_ros/src/RosUtils.cc b/dynosam_ros/src/RosUtils.cc index 73d197092..6be26d1e6 100644 --- a/dynosam_ros/src/RosUtils.cc +++ b/dynosam_ros/src/RosUtils.cc @@ -158,6 +158,24 @@ bool dyno::convert(const gtsam::Pose3& pose, return true; } +// NEW! Template to add twist to the message +template <> +bool dyno::convert(const gtsam::Vector6& vel, + geometry_msgs::msg::Twist& twist) +{ + // linear velocity components + twist.linear.x = vel(0); + twist.linear.y = vel(1); + twist.linear.z = vel(2); + + // angular velocity components + twist.angular.x = vel(3); + twist.angular.y = vel(4); + twist.angular.z = vel(5); + + return true; +} + template <> bool dyno::convert(const gtsam::Pose3& pose, geometry_msgs::msg::TransformStamped& transform) { diff --git a/dynosam_ros/src/displays/DisplaysCommon.cc b/dynosam_ros/src/displays/DisplaysCommon.cc index 548523ef0..0cd0f9a2d 100644 --- a/dynosam_ros/src/displays/DisplaysCommon.cc +++ b/dynosam_ros/src/displays/DisplaysCommon.cc @@ -44,15 +44,23 @@ CloudPerObject DisplayCommon::publishPointCloud( return clouds_per_obj; } +// ================================================================================================================ +// TODO: look here edit this function to include velocities +// ================================================================================================================ + void DisplayCommon::publishOdometry(OdometryPub::SharedPtr pub, const gtsam::Pose3& T_world_camera, Timestamp timestamp, const std::string& frame_id, - const std::string& child_frame_id) { + const std::string& child_frame_id, + const gtsam::Vector6& velocity + ) { nav_msgs::msg::Odometry odom_msg; utils::convertWithHeader(T_world_camera, odom_msg, timestamp, frame_id, child_frame_id); - pub->publish(odom_msg); + + dyno::convert(velocity, odom_msg.twist.twist); + pub->publish(odom_msg); } void DisplayCommon::publishOdometryPath(PathPub::SharedPtr pub, diff --git a/dynosam_ros/src/displays/backend_displays/HybridBackendDisplay.cc b/dynosam_ros/src/displays/backend_displays/HybridBackendDisplay.cc index 2d095f86f..f24e2963e 100644 --- a/dynosam_ros/src/displays/backend_displays/HybridBackendDisplay.cc +++ b/dynosam_ros/src/displays/backend_displays/HybridBackendDisplay.cc @@ -2,15 +2,20 @@ #include "dynosam_common/PointCloudProcess.hpp" #include "dynosam_ros/BackendDisplayPolicyRos.hpp" +#include "dynosam_ros/RosUtils.hpp" #include "dynosam_ros/displays/BackendDisplayRos.hpp" namespace dyno { HybridModuleDisplayCommon::HybridModuleDisplayCommon( - const DisplayParams& params, rclcpp::Node* node) - : BackendModuleDisplayRos(params, node) { + const DisplayParams& params, rclcpp::Node* node, + HybridAccessorCommon::Ptr hybrid_accessor) + : BackendModuleDisplayRos(params, node), + hybrid_accessor_(CHECK_NOTNULL(hybrid_accessor)) { object_bounding_box_pub_ = node_->create_publisher("object_bounding_boxes", 1); + object_key_frame_pub_ = + node_->create_publisher("object_keyframes", 1); } void HybridModuleDisplayCommon::publishObjectBoundingBoxes( @@ -32,14 +37,103 @@ void HybridModuleDisplayCommon::publishObjectBoundingBoxes( object_bounding_box_pub_->publish(array); } +void HybridModuleDisplayCommon::publishObjectKeyFrames( + const FrameId frame_id, const Timestamp timestamp) { + // bit weird, use the object motion map to find which objects exist at the + // current frame + const MotionEstimateMap motions = + hybrid_accessor_->getObjectMotions(frame_id); + auto ros_time = utils::toRosTime(timestamp); + + visualization_msgs::msg::MarkerArray array; + for (const auto& [object_id, _] : motions) { + const KeyFrameRanges* object_range = nullptr; + if (hybrid_accessor_->getObjectKeyFrameHistory(object_id, object_range)) { + CHECK_NOTNULL(object_range); + std_msgs::msg::ColorRGBA colour_msg; + convert(Color::uniqueId(object_id), colour_msg); + + // iterate over all keyframes and draw + size_t count = 0; + for (const FrameRange::Ptr& frame_range : *object_range) { + const auto [keyframe_id, L_e] = frame_range->dataPair(); + + visualization_msgs::msg::Marker marker; + // Header and Metadata + marker.header.frame_id = params_.world_frame_id; + marker.header.stamp = ros_time; + marker.ns = "obj_" + std::to_string(object_id) + "_keyframe"; + marker.id = count; + marker.action = visualization_msgs::msg::Marker::ADD; + + // Marker Type: LINE_LIST allows us to draw multiple lines (the three + // axes) + marker.type = visualization_msgs::msg::Marker::LINE_LIST; + // marker.lifetime = + // Translation + marker.pose.position.x = L_e.x(); + marker.pose.position.y = L_e.y(); + marker.pose.position.z = L_e.z(); + + // --- Line Properties --- + marker.scale.x = 0.04; // Line width in meters + constexpr static double axis_length = 0.4; // Length of the axes + + // Orientation (Convert GTSAM Rotation to ROS Quaternion) + const gtsam::Quaternion gtsam_q = L_e.rotation().toQuaternion(); + marker.pose.orientation.w = gtsam_q.w(); + marker.pose.orientation.x = gtsam_q.x(); + marker.pose.orientation.y = gtsam_q.y(); + marker.pose.orientation.z = gtsam_q.z(); + + geometry_msgs::msg::Point origin; // (0, 0, 0) + origin.x = 0.0; + origin.y = 0.0; + origin.z = 0.0; + + // X-Axis (start at origin, end at +X) + geometry_msgs::msg::Point x_end = origin; + x_end.x = axis_length; + marker.points.push_back(origin); + marker.points.push_back(x_end); + marker.colors.push_back(colour_msg); + marker.colors.push_back(colour_msg); + + // Y-Axis (start at origin, end at +Y) + geometry_msgs::msg::Point y_end = origin; + y_end.y = axis_length; + marker.points.push_back(origin); + marker.points.push_back(y_end); + marker.colors.push_back(colour_msg); + marker.colors.push_back(colour_msg); + + // Z-Axis (start at origin, end at +Z) + geometry_msgs::msg::Point z_end = origin; + z_end.z = axis_length; + marker.points.push_back(origin); + marker.points.push_back(z_end); + marker.colors.push_back(colour_msg); + marker.colors.push_back(colour_msg); + + array.markers.push_back(marker); + + count++; + } + } + } + object_key_frame_pub_->publish(array); +} + void ParalleHybridModuleDisplay::spin( const BackendOutputPacket::ConstPtr& output) { this->publishObjectBoundingBoxes(output); + this->publishObjectKeyFrames(output->getFrameId(), output->getTimestamp()); } void RegularHybridFormulationDisplay::spin( const BackendOutputPacket::ConstPtr& output) { this->publishObjectBoundingBoxes(output); + this->publishObjectKeyFrames(output->getFrameId(), output->getTimestamp()); } } // namespace dyno diff --git a/dynosam_ros/src/displays/dynamic_slam_displays/BackendDSDRos.cc b/dynosam_ros/src/displays/dynamic_slam_displays/BackendDSDRos.cc index 2c3f1e520..27396a38c 100644 --- a/dynosam_ros/src/displays/dynamic_slam_displays/BackendDSDRos.cc +++ b/dynosam_ros/src/displays/dynamic_slam_displays/BackendDSDRos.cc @@ -62,13 +62,41 @@ void BackendDSDRos::spinOnceImpl( this->publishDynamicPointCloud(backend_output->dynamic_landmarks, backend_output->pose()); - publishTemporalDynamicMaps(backend_output); + // publishTemporalDynamicMaps(backend_output); auto clouds_toc = utils::Timer::toc(tic); const auto& object_motions = backend_output->optimized_object_motions; const auto& object_poses = backend_output->optimized_object_poses; const auto& timestamp_map = this->shared_module_info.getTimestampMap(); + ObjectMotionMap keyframed_motions; + ObjectMotionMap keyframed_poses; + + // this is awful - hack to make the frame ids consequative (not sure if this + // is the final way of doing this) + // only now for when camera keyframe every pose but objects do not! + // for(const auto& [object_id, per_frame_motion] : object_motions) { + // // this is awful + // // hack for now to make frames consecutaive. may not be the same for + // each object/pose!! FrameId key_frame_id = 0; for(const auto& [frame_id, + // motion] : per_frame_motion) { + + // } + // ss << "\n"; + // } + + // this is awful + // FrameId key_frame_id = 0; + std::stringstream ss; + for (const auto& [object_id, per_frame_motion] : object_poses) { + ss << "j= " << object_id << " appeared at "; + for (const auto& [frame_id, motion] : per_frame_motion) { + ss << frame_id << " "; + } + ss << "\n"; + } + LOG(INFO) << ss.str(); + // // publish objects DSDTransport::Publisher object_poses_publisher = dsd_transport_.addObjectInfo( object_motions, object_poses, params_.world_frame_id, timestamp_map, diff --git a/dynosam_ros/src/displays/dynamic_slam_displays/DSDCommonRos.cc b/dynosam_ros/src/displays/dynamic_slam_displays/DSDCommonRos.cc index a5ef18c2f..0cf068766 100644 --- a/dynosam_ros/src/displays/dynamic_slam_displays/DSDCommonRos.cc +++ b/dynosam_ros/src/displays/dynamic_slam_displays/DSDCommonRos.cc @@ -25,33 +25,50 @@ std::string DSDTransport::constructObjectFrameLink(ObjectId object_id) { return "object_" + std::to_string(object_id) + "_link"; } +// ================================================================================================================ +// TODO: Fill in the velocity and check calculateBodyMotion is correct (only linear, add angular component too) +// ================================================================================================================ ObjectOdometry DSDTransport::constructObjectOdometry( - const gtsam::Pose3& e_H_k_world, const gtsam::Pose3& pose_k, - ObjectId object_id, FrameId frame_id_k, Timestamp timestamp_k, - const std::string& frame_id_link, const std::string& child_frame_id_link) { + const gtsam::Pose3& e_H_k_world, const gtsam::Pose3& pose_km1, + ObjectId object_id, FrameId frame_id_k, Timestamp timestamp_k, Timestamp timestamp_km1, + const std::string& frame_id_link, const std::string& child_frame_id_link, + const ObjectIdClassMap& object_class_map) { ObjectOdometry object_odom; - // technically this shoudl be k-1 - gtsam::Point3 body_velocity = calculateBodyMotion(e_H_k_world, pose_k); + // technically this should be k-1 + gtsam::Vector6 body_velocity = calculateBodyMotion(e_H_k_world, pose_km1, timestamp_km1, timestamp_k); nav_msgs::msg::Odometry odom_msg; - utils::convertWithHeader(pose_k, odom_msg, timestamp_k, frame_id_link, + utils::convertWithHeader(pose_km1, odom_msg, timestamp_k, frame_id_link, child_frame_id_link); object_odom.odom = odom_msg; // TODO: can check if correct representation? dyno::convert(e_H_k_world, object_odom.h_w_km1_k.pose); - // NO velocity!! + + dyno::convert(body_velocity, object_odom.odom.twist.twist); // Add velocity to message + object_odom.object_id = object_id; object_odom.sequence = frame_id_k; + + // Assigning the class label + auto observation = object_class_map.find(object_id); + if (observation != object_class_map.end()) { + object_odom.class_label=observation->second; + } else { + object_odom.class_label="unknown"; + } + + return object_odom; } ObjectOdometryMap DSDTransport::constructObjectOdometries( const ObjectMotionMap& motions, const ObjectPoseMap& poses, - FrameId frame_id_k, Timestamp timestamp_k, - const std::string& frame_id_link) { + FrameId frame_id_k, Timestamp timestamp_k, Timestamp timestamp_km1, + const std::string& frame_id_link, + const ObjectIdClassMap& object_class_map) { // need to get poses for k-1 // TODO: no way to ensure that the motions are for frame k // this is a weird data-structure to use and motions are per frame and @@ -79,8 +96,8 @@ ObjectOdometryMap DSDTransport::constructObjectOdometries( object_odom_map.insert2( child_frame_id_link, constructObjectOdometry(e_H_k_world, pose_k, object_id, frame_id_k, - timestamp_k, frame_id_link, - child_frame_id_link)); + timestamp_k, timestamp_km1, + frame_id_link, child_frame_id_link, object_class_map)); } return object_odom_map; @@ -89,7 +106,7 @@ ObjectOdometryMap DSDTransport::constructObjectOdometries( MultiObjectOdometryPath DSDTransport::constructMultiObjectOdometryPaths( const ObjectMotionMap& motions, const ObjectPoseMap& poses, Timestamp timestamp_k, const FrameIdTimestampMap& frame_timestamp_map, - const std::string& frame_id_link, bool interpolate_missing_segments) { + const std::string& frame_id_link, bool interpolate_missing_segments, const ObjectIdClassMap& object_class_map) { MultiObjectOdometryPath multi_path; multi_path.header.stamp = utils::toRosTime(timestamp_k); @@ -131,18 +148,28 @@ MultiObjectOdometryPath DSDTransport::constructMultiObjectOdometryPaths( const Timestamp& timestamp = frame_timestamp_map.at(frame_id); - if (!first && static_cast(frame_id) != previous_frame_id + 1) { - path_segment++; - } + // if (!first && static_cast(frame_id) != previous_frame_id + 1) { + // path_segment++; + // } first = false; previous_frame_id = static_cast(frame_id); // RIGHT NOW MOTION IDENTITY // timestamp is wrong + + // Adding the (k-1)th timestamp + Timestamp timestamp_km1; + if (previous_frame_id >= 0) { + timestamp_km1 = frame_timestamp_map.at(previous_frame_id); + } + else { + timestamp_km1 = timestamp; + } + gtsam::Pose3 motion; const ObjectOdometry object_odometry = constructObjectOdometry( - object_motion, object_pose, object_id, frame_id, timestamp, - frame_id_link, child_frame_id_link); + object_motion, object_pose, object_id, frame_id, timestamp, timestamp_km1, + frame_id_link, child_frame_id_link, object_class_map); if (!segmented_paths.exists(path_segment)) { ObjectOdometryPath path; @@ -224,6 +251,23 @@ void DSDTransport::Publisher::publishObjectPaths() { multi_object_odom_path_publisher_->publish(object_paths_); } +// ============================== ADDED HELPER FUNCTION TO GET PREVIOUS TIMESTAMP ============================== +Timestamp getPrevTimestamp(const FrameIdTimestampMap& map, FrameId id, Timestamp current_timestamp) { + + if (id < 0) return current_timestamp; + + FrameId prev_id = id - 1; + + auto it = map.find(prev_id); + if (it != map.end()) { + return it->second; + } + else { + return current_timestamp; + } +} +// ============================================================================================================ + DSDTransport::Publisher::Publisher( rclcpp::Node::SharedPtr node, ObjectOdometryPub::SharedPtr object_odom_publisher, @@ -232,7 +276,8 @@ DSDTransport::Publisher::Publisher( const ObjectMotionMap& motions, const ObjectPoseMap& poses, const std::string& frame_id_link, const FrameIdTimestampMap& frame_timestamp_map, FrameId frame_id, - Timestamp timestamp) + Timestamp timestamp, + const ObjectIdClassMap& object_class_map) : node_(node), object_odom_publisher_(object_odom_publisher), multi_object_odom_path_publisher_(multi_object_odom_path_publisher), @@ -240,20 +285,25 @@ DSDTransport::Publisher::Publisher( frame_id_link_(frame_id_link), frame_id_(frame_id), timestamp_(timestamp), + timestamp_km1_(getPrevTimestamp(frame_timestamp_map, frame_id, timestamp)), // added to get (k-1)th timestamp object_odometries_(DSDTransport::constructObjectOdometries( - motions, poses, frame_id, timestamp, frame_id_link)), + motions, poses, frame_id, timestamp, timestamp_km1_, frame_id_link, object_class_map)), object_paths_(DSDTransport::constructMultiObjectOdometryPaths( - motions, poses, timestamp, frame_timestamp_map, frame_id_link)) {} + motions, poses, timestamp, frame_timestamp_map, frame_id_link, true, object_class_map)), + object_class_map_(object_class_map){} + DSDTransport::Publisher DSDTransport::addObjectInfo( const ObjectMotionMap& motions, const ObjectPoseMap& poses, const std::string& frame_id_link, - const FrameIdTimestampMap& frame_timestamp_map, FrameId frame_id, - Timestamp timestamp) { + const FrameIdTimestampMap& frame_timestamp_map, + FrameId frame_id, + Timestamp timestamp, + const ObjectIdClassMap& object_class_map) { return Publisher(node_, object_odom_publisher_, multi_object_odom_path_publisher_, tf_broadcaster_, motions, poses, frame_id_link, frame_timestamp_map, frame_id, - timestamp); + timestamp, object_class_map); } DSDRos::DSDRos(const DisplayParams& params, rclcpp::Node::SharedPtr node) @@ -271,10 +321,12 @@ DSDRos::DSDRos(const DisplayParams& params, rclcpp::Node::SharedPtr node) } void DSDRos::publishVisualOdometry(const gtsam::Pose3& T_world_camera, - Timestamp timestamp, const bool publish_tf) { + Timestamp timestamp, const bool publish_tf, + const gtsam::Vector6& velocity) { DisplayCommon::publishOdometry(vo_publisher_, T_world_camera, timestamp, params_.world_frame_id, - params_.camera_frame_id); + params_.camera_frame_id, + velocity); if (publish_tf) { geometry_msgs::msg::TransformStamped t; diff --git a/dynosam_ros/src/displays/dynamic_slam_displays/FrontendDSDRos.cc b/dynosam_ros/src/displays/dynamic_slam_displays/FrontendDSDRos.cc index fb0acddee..e7099fa35 100644 --- a/dynosam_ros/src/displays/dynamic_slam_displays/FrontendDSDRos.cc +++ b/dynosam_ros/src/displays/dynamic_slam_displays/FrontendDSDRos.cc @@ -38,26 +38,30 @@ namespace dyno { FrontendDSDRos::FrontendDSDRos(const DisplayParams params, - rclcpp::Node::SharedPtr node) + rclcpp::Node::SharedPtr node, + rclcpp::Node::SharedPtr ground_truth_node) : FrontendDisplay(), DSDRos(params, node) { tracking_image_pub_ = image_transport::create_publisher(node.get(), "tracking_image"); // const rclcpp::SensorDataQoS sensor_qos; - - auto ground_truth_node = node->create_sub_node("ground_truth"); - dsd_ground_truth_transport_ = - std::make_unique(ground_truth_node); - vo_ground_truth_publisher_ = - ground_truth_node->create_publisher("odometry", - 1); - vo_path_ground_truth_publisher_ = - ground_truth_node->create_publisher("odometry_path", - 1); - dense_dynamic_cloud_pub_ = node->create_publisher( - "dense_labelled_cloud", 1); + "dense_labelled_cloud", rclcpp::SensorDataQoS()); + + if (ground_truth_node) { + RCLCPP_INFO_STREAM(node->get_logger(), "Creating ground truth publishers"); + ground_truth_publishers_ = GroundTruthPublishers(ground_truth_node); + } +} + +FrontendDSDRos::GroundTruthPublishers::GroundTruthPublishers( + rclcpp::Node::SharedPtr ground_truth_node) + : dsd_transport_(CHECK_NOTNULL(ground_truth_node)) { + vo_publisher_ = ground_truth_node->create_publisher( + "odometry", rclcpp::SensorDataQoS()); + vo_path_publisher_ = ground_truth_node->create_publisher( + "odometry_path", rclcpp::SensorDataQoS()); } void FrontendDSDRos::spinOnceImpl( @@ -94,7 +98,8 @@ void FrontendDSDRos::tryPublishDebugImagery( void FrontendDSDRos::tryPublishGroundTruth( const VisionImuPacket::ConstPtr& frontend_output) { - if (!frontend_output->groundTruthPacket() || !frontend_output->debugImagery()) + if (!ground_truth_publishers_ || !frontend_output->groundTruthPacket() || + !frontend_output->debugImagery()) return; const DebugImagery& debug_imagery = *frontend_output->debugImagery(); @@ -132,10 +137,11 @@ void FrontendDSDRos::tryPublishGroundTruth( motions.insert22(object_pose_gt.object_id_, gt_packet.frame_id_, gt_motion); } + GroundTruthPublishers& ground_truth_pubs = ground_truth_publishers_.value(); // will this result in confusing tf's since the gt object and estimated // objects use the same link? DSDTransport::Publisher publisher = - dsd_ground_truth_transport_->addObjectInfo( + ground_truth_pubs.dsd_transport_.addObjectInfo( motions, poses, params_.world_frame_id, timestamp_map, frame_id, timestamp); publisher.publishObjectOdometry(); @@ -145,7 +151,7 @@ void FrontendDSDRos::tryPublishGroundTruth( nav_msgs::msg::Odometry odom_msg; utils::convertWithHeader(T_world_camera, odom_msg, timestamp, params_.world_frame_id, params_.camera_frame_id); - vo_ground_truth_publisher_->publish(odom_msg); + ground_truth_pubs.vo_publisher_->publish(odom_msg); // odom path gt // make static variable since we dont build up the path anywhere else @@ -162,14 +168,17 @@ void FrontendDSDRos::tryPublishGroundTruth( gt_odom_path_msg.header = header; gt_odom_path_msg.poses.push_back(pose_stamped); - vo_path_ground_truth_publisher_->publish(gt_odom_path_msg); + ground_truth_pubs.vo_path_publisher_->publish(gt_odom_path_msg); } void FrontendDSDRos::tryPublishVisualOdometry( const VisionImuPacket::ConstPtr& frontend_output) { // publish vo - constexpr static bool kPublishOdomAsTf = true; + constexpr static bool kPublishOdomAsTf = true; + this->publishVisualOdometry(frontend_output->cameraPose(), - frontend_output->timestamp(), kPublishOdomAsTf); + frontend_output->timestamp(), + kPublishOdomAsTf, + frontend_output->getBodyVelocity()); // relies on correct accumulation of internal objects this->publishVisualOdometryPath(camera_poses_, frontend_output->timestamp()); @@ -207,10 +216,11 @@ void FrontendDSDRos::tryPublishObjects( const auto& object_motions = object_motions_; const auto& object_poses = object_poses_; const auto& timestamp_map = this->shared_module_info.getTimestampMap(); + const auto& label_map = this->shared_module_info.getClassLabelMap(); DSDTransport::Publisher object_poses_publisher = dsd_transport_.addObjectInfo( object_motions, object_poses, params_.world_frame_id, timestamp_map, - frontend_output->frameId(), frontend_output->timestamp()); + frontend_output->frameId(), frontend_output->timestamp(), label_map); object_poses_publisher.publishObjectOdometry(); object_poses_publisher.publishObjectTransforms(); object_poses_publisher.publishObjectPaths(); @@ -225,42 +235,4 @@ void FrontendDSDRos::updateAccumulatedDataStructured( frontend_output->objectPoses()); } -// void FrontendDSDRos::processRGBDOutputpacket( -// const RGBDInstanceOutputPacket::ConstPtr& rgbd_packet) { -// // publish path -// // why the camera poses are only in the RGBDInstanceOutputPacket and not in -// // the base... I have no idea :) -// this->publishVisualOdometryPath(rgbd_packet->camera_poses_, -// rgbd_packet->getTimestamp()); - -// // publish static cloud -// CHECK(rgbd_packet); -// this->publishStaticPointCloud(rgbd_packet->static_landmarks_, -// rgbd_packet->T_world_camera_); - -// // publish and collect dynamic cloud -// CloudPerObject clouds_per_obj = this->publishDynamicPointCloud( -// rgbd_packet->dynamic_landmarks_, rgbd_packet->T_world_camera_); - -// const auto& object_motions = rgbd_packet->object_motions_; -// const auto& object_poses = rgbd_packet->propogated_object_poses_; -// const auto& timestamp_map = rgbd_packet->involved_timestamps_; - -// DSDTransport::Publisher object_poses_publisher = -// dsd_transport_.addObjectInfo( -// object_motions, object_poses, params_.world_frame_id, timestamp_map, -// rgbd_packet->getFrameId(), rgbd_packet->getTimestamp()); -// object_poses_publisher.publishObjectOdometry(); -// object_poses_publisher.publishObjectTransforms(); -// object_poses_publisher.publishObjectPaths(); - -// if (rgbd_packet->dense_labelled_cloud_) { -// sensor_msgs::msg::PointCloud2 pc2_msg; -// pcl::toROSMsg(*rgbd_packet->dense_labelled_cloud_, pc2_msg); -// pc2_msg.header.frame_id = params_.camera_frame_id; -// pc2_msg.header.stamp = utils::toRosTime(rgbd_packet->getTimestamp()); -// dense_dynamic_cloud_pub_->publish(pc2_msg); -// } -// } - } // namespace dyno diff --git a/dynosam_ros/test/test_online_dataprovider_ros.cc b/dynosam_ros/test/test_online_dataprovider_ros.cc index 425b50fb4..fcb8d5088 100644 --- a/dynosam_ros/test/test_online_dataprovider_ros.cc +++ b/dynosam_ros/test/test_online_dataprovider_ros.cc @@ -72,12 +72,12 @@ TEST(OnlineDataProviderRos, testwaitCameraInfoSubscribe) { camera_info_msg.distortion_model = "plumb_bob"; auto received = false; - std::shared_ptr odpr = nullptr; + std::shared_ptr odpr = nullptr; std::shared_future wait = std::async(std::launch::async, [&]() { OnlineDataProviderRosParams params; params.wait_for_camera_params = true; params.camera_params_timeout = -1; - odpr = std::make_shared(node, params); + odpr = std::make_shared(node, params); received = true; return true; }); @@ -98,25 +98,33 @@ TEST(OnlineDataProviderRos, testNowaitCameraInfoSubscribe) { OnlineDataProviderRosParams params; params.wait_for_camera_params = false; - auto odpr = std::make_shared(node, params); + auto odpr = std::make_shared(node, params); EXPECT_FALSE(odpr->getCameraParams()); } +TEST(MultiSync, printVersionMessageFilters) { + std::cout << "--- Detection Check ---" << std::endl; + // This will print the version number defined by the preprocessor logic + std::cout << "Defined MESSAGE_FILTERS_USES_NODE_INTERFACE: " + << MESSAGE_FILTERS_USES_NODE_INTERFACE << std::endl; +} + TEST(MultiSync, basicInvalidConnect) { auto node = std::make_shared("test"); - using MIS = MultiImageSync<1>; - MIS image_sync(*node, {"image_raw"}, 10); + using MIS = MultiImageSync<2>; + MIS image_sync(*node, {"image_raw1", "image_raw2"}, 10); EXPECT_FALSE(image_sync.connect()); } TEST(MultiSync, basicConnect) { auto node = std::make_shared("test"); - using MIS = MultiImageSync<1>; - MIS image_sync(*node, {"image_raw"}, 10); + using MIS = MultiImageSync<2>; + MIS image_sync(*node, {"image_raw1", "image_raw2"}, 10); image_sync.registerCallback( - [](const sensor_msgs::msg::Image::ConstSharedPtr&) {}); + [](const sensor_msgs::msg::Image::ConstSharedPtr&, + const sensor_msgs::msg::Image::ConstSharedPtr&) {}); EXPECT_TRUE(image_sync.connect()); } diff --git a/dynosam_utils/src/frontend_filtering_experiments.py b/dynosam_utils/src/frontend_filtering_experiments.py new file mode 100644 index 000000000..1b07a18a7 --- /dev/null +++ b/dynosam_utils/src/frontend_filtering_experiments.py @@ -0,0 +1,198 @@ +from dynosam_utils.evaluation.runner import run +import os +import sys + +# runs new incremental backend (parallel-hybrid) +parallel_hybrid = 3 +full_hybrid=2 +# runs world centric backend as batch (now called wcme) +motion_world_backend_type = 0 + +test_hybrid_smf = 7 # TESTING_HYBRID_SMF + +output_path = "/root/results/frontend_filtering/" + +def run_sequnce(path, name, data_loader_num, backend_type, *args, **kwargs): + run_as_frontend = kwargs.get("run_as_frontend", True) + # use the dyno_sam_experiments_launch file + run_as_experiment = kwargs.get("run_as_experiment", False) + run_analysis = kwargs.get("run_analysis", False) + + parsed_args = { + "dataset_path": path, + "output_path": output_path, + "name": name, + "run_pipeline": True, + "run_analysis": run_analysis, + } + + additional_args = [ + "--data_provider_type={}".format(data_loader_num), + "--v=30" + ] + + parsed_args["launch_file"] = "dyno_sam_launch.py" + + if run_as_frontend: + additional_args.extend([ + "--use_backend=0", + "--save_frontend_json=true" + ]) + else: + additional_args.extend([ + "--backend_updater_enum={}".format(backend_type), + "--use_backend=1" + ]) + if run_as_experiment: + parsed_args["launch_file"] = "dyno_sam_experiments_launch.py" + + if len(args) > 0: + additional_args.extend(list(args)) + + # print(additional_args) + run(parsed_args, additional_args) + + +def run_analysis(name): + parsed_args = { + "dataset_path": "", + "output_path": output_path, + "name": name, + "run_pipeline": False, + "run_analysis": True, + } + parsed_args["launch_file"] = "dyno_sam_launch.py" + run(parsed_args, []) + +kitti_dataset = 0 +virtual_kitti_dataset = 1 +cluster_dataset = 2 +omd_dataset = 3 +aria=4 +tartan_air = 5 +viode = 6 + +def prep_dataset(path, name, data_loader_num, *args): + backend_type = parallel_hybrid + run_as_frontend=True + run_sequnce( + path, + name, + data_loader_num, + backend_type, + *args, + run_as_frontend=run_as_frontend) + +# from saved data +def run_saved_sequence(path, name, data_loader_num, *args, **kwargs): + backend_type = kwargs.get("backend_type", parallel_hybrid) + kwargs_dict = dict(kwargs) + kwargs_dict["run_as_frontend"] = False + args_list = list(args) + # args_list.append("--init_object_pose_from_gt=true") + run_sequnce( + path, + name, + data_loader_num, + backend_type, + *args_list, + **kwargs_dict) + + +# kitti stuff +def prep_kitti_sequence(path, name, *args): + args_list = list(args) + args_list.append("--shrink_row=25") + args_list.append("--shrink_col=50") + # args_list.append("--use_propogate_mask=true") + prep_dataset(path, name, kitti_dataset, *args_list) + +def run_kitti_sequence(path, name, *args, **kwargs): + run_saved_sequence(path, name, kitti_dataset, *args, **kwargs) + # run_analysis(name) + +# cluster +def prep_cluster_sequence(path, name, *args, **kwargs): + prep_dataset(path, name, cluster_dataset, *args, **kwargs) + +def run_cluster_sequence(path, name, *args, **kwargs): + run_saved_sequence(path, name, cluster_dataset, *args, **kwargs) + +# omd +def prep_omd_sequence(path, name, *args, **kwargs): + args_list = list(args) + args_list.append("--shrink_row=0") + args_list.append("--shrink_col=0") + prep_dataset(path, name, omd_dataset, *args_list, **kwargs) + +def run_omd_sequence(path, name, *args, **kwargs): + run_saved_sequence(path, name, omd_dataset, *args, **kwargs) + + +def run_experiment_sequences(dataset_path, dataset_name, dataset_loader, *args): + + def append_args_list(*specific_args): + args_list = list(args) + args_list.extend(list(specific_args)) + return args_list + # run fukk hybrid in (full)batch mode to get results!! + run_sequnce(dataset_path, dataset_name, dataset_loader, parallel_hybrid, *append_args_list(), run_as_frontend=False, run_as_experiment=False, run_analysis=False) + + +def run_viodes(): + +# run_experiment_sequences("/root/data/VIODE/city_day/mid", "viode_city_day_mid", viode, "--v=100") + # run_experiment_sequences("/root/data/VIODE/city_day/high","viode_city_day_high", viode, "--ending_frame=1110") + run_experiment_sequences("/root/data/VIODE/city_day/high","test_viode", viode,"--starting_frame=0", "--ending_frame=1110", "--v=10", "--use_backend=true", "--use_object_motion_filtering=false") +# # zero_elements_ratio +# run_experiment_sequences("/root/data/VIODE/city_night/mid", "viode_city_night_mid", viode) + # run_experiment_sequences("/root/data/VIODE/city_night/high", "viode_city_night_high", viode) + + # run_experiment_sequences("/root/data/VIODE/parking_lot/mid", "parking_lot_night_mid", viode) + # run_experiment_sequences("/root/data/VIODE/parking_lot/high", "parking_lot_night_high", viode) + +def run_omd(): + run_experiment_sequences("/root/data/vdo_slam/omd/omd/swinging_4_unconstrained_stereo/","test", omd_dataset, "--ending_frame=300", "--use_backend=false", "--v=40", "--use_object_motion_filtering=true") + + +def run_tartan_air(): + # run_experiment_sequences("/root/data/TartanAir_shibuya/RoadCrossing03", "tas_rc3", tartan_air) #max_object_depth: 10.0 + # run_experiment_sequences("/root/data/TartanAir_shibuya/RoadCrossing04", "test_tartan", tartan_air, "--use_backend=false", "--use_object_motion_filtering=true") + run_experiment_sequences("/root/data/TartanAir_shibuya/RoadCrossing05", "test_tartan", tartan_air, "--use_backend=false", "--use_object_motion_filtering=true") + # run_experiment_sequences("/root/data/TartanAir_shibuya/RoadCrossing06", "tas_rc6", tartan_air, "--use_backend=false") + # run_experiment_sequences("/root/data/TartanAir_shibuya/RoadCrossing07", "tas_rc7", tartan_air, "--starting_frame=5", "--ending_frame=65") + # run_analysis("tas_rc7") + + # run_experiment_sequences("/root/data/TartanAir_shibuya/Standing01", "tas_s1", tartan_air) + # run_experiment_sequences("/root/data/TartanAir_shibuya/Standing02", "tas_s2", tartan_air) + +def run_cluster(): + # run_experiment_sequences("/root/data/cluster_slam/CARLA-L2/", "cluster_l2_static_only", cluster_dataset) + run_experiment_sequences("/root/data/cluster_slam/CARLA-L1/", "test", cluster_dataset, "--use_backend=false", "--use_object_motion_filtering=true") + # run_experiment_sequences("/root/data/cluster_slam/CARLA-S2/", "cluster_s2_static_only", cluster_dataset) + # run_experiment_sequences("/root/data/cluster_slam/CARLA-S1/", "cluster_s1_static_only", cluster_dataset) + +def run_kitti(): + # run_experiment_sequences("/root/data/vdo_slam/kitti/kitti/0000/", "test", kitti_dataset, "--shrink_row=25", "--shrink_col=50", "--use_backend=true", "--use_object_motion_filtering=true") + # run_experiment_sequences("/root/data/vdo_slam/kitti/kitti/0001/", "kitti_0001_static_only", kitti_dataset, "--shrink_row=25", "--shrink_col=50") + # run_experiment_sequences("/root/data/vdo_slam/kitti/kitti/0002/", "kitti_0002_static_only", kitti_dataset, "--shrink_row=25", "--shrink_col=50") + # run_experiment_sequences("/root/data/vdo_slam/kitti/kitti/0003/", "test", kitti_dataset, "--shrink_row=25", "--shrink_col=50", "--use_backend=true", "--use_object_motion_filtering=true") + # run_experiment_sequences("/root/data/vdo_slam/kitti/kitti/0005/", "kitti_0005_static_only", kitti_dataset, "--shrink_row=25", "--shrink_col=50") + # run_experiment_sequences("/root/data/vdo_slam/kitti/kitti/0006/", "kitti_0006_static_only", kitti_dataset, "--shrink_row=25", "--shrink_col=50") + # run_experiment_sequences("/root/data/vdo_slam/kitti/kitti/0018/", "kitti_0018_static_only", kitti_dataset, "--shrink_row=25", "--shrink_col=50") + # run_experiment_sequences("/root/data/vdo_slam/kitti/kitti/0020/", "test", kitti_dataset, "--shrink_row=25", "--shrink_col=50", "--use_backend=true", "--use_object_motion_filtering=true") + run_experiment_sequences("/root/data/vdo_slam/kitti/kitti/0004/", "test", kitti_dataset, "--shrink_row=25", "--shrink_col=50", "--use_backend=false", "--v=30", "--use_object_motion_filtering=true") + + +def run_aria(): + run_experiment_sequences("/root/data/zed/acfr_2_moving_small", "test_small", aria, "--use_backend=false", "--v=30", "--use_object_motion_filtering=true") + + +if __name__ == '__main__': + # run_tartan_air() + # run_kitti() + run_viodes() + # run_cluster() + # run_tartan_air() + # run_aria() + # run_omd() diff --git a/dynosam_utils/src/install/.colcon_install_layout b/dynosam_utils/src/install/.colcon_install_layout new file mode 100644 index 000000000..3aad5336a --- /dev/null +++ b/dynosam_utils/src/install/.colcon_install_layout @@ -0,0 +1 @@ +isolated diff --git a/dynosam_utils/src/install/COLCON_IGNORE b/dynosam_utils/src/install/COLCON_IGNORE new file mode 100644 index 000000000..e69de29bb diff --git a/dynosam_utils/src/install/_local_setup_util_ps1.py b/dynosam_utils/src/install/_local_setup_util_ps1.py new file mode 100644 index 000000000..3c6d9e877 --- /dev/null +++ b/dynosam_utils/src/install/_local_setup_util_ps1.py @@ -0,0 +1,407 @@ +# Copyright 2016-2019 Dirk Thomas +# Licensed under the Apache License, Version 2.0 + +import argparse +from collections import OrderedDict +import os +from pathlib import Path +import sys + + +FORMAT_STR_COMMENT_LINE = '# {comment}' +FORMAT_STR_SET_ENV_VAR = 'Set-Item -Path "Env:{name}" -Value "{value}"' +FORMAT_STR_USE_ENV_VAR = '$env:{name}' +FORMAT_STR_INVOKE_SCRIPT = '_colcon_prefix_powershell_source_script "{script_path}"' # noqa: E501 +FORMAT_STR_REMOVE_LEADING_SEPARATOR = '' # noqa: E501 +FORMAT_STR_REMOVE_TRAILING_SEPARATOR = '' # noqa: E501 + +DSV_TYPE_APPEND_NON_DUPLICATE = 'append-non-duplicate' +DSV_TYPE_PREPEND_NON_DUPLICATE = 'prepend-non-duplicate' +DSV_TYPE_PREPEND_NON_DUPLICATE_IF_EXISTS = 'prepend-non-duplicate-if-exists' +DSV_TYPE_SET = 'set' +DSV_TYPE_SET_IF_UNSET = 'set-if-unset' +DSV_TYPE_SOURCE = 'source' + + +def main(argv=sys.argv[1:]): # noqa: D103 + parser = argparse.ArgumentParser( + description='Output shell commands for the packages in topological ' + 'order') + parser.add_argument( + 'primary_extension', + help='The file extension of the primary shell') + parser.add_argument( + 'additional_extension', nargs='?', + help='The additional file extension to be considered') + parser.add_argument( + '--merged-install', action='store_true', + help='All install prefixes are merged into a single location') + args = parser.parse_args(argv) + + packages = get_packages(Path(__file__).parent, args.merged_install) + + ordered_packages = order_packages(packages) + for pkg_name in ordered_packages: + if _include_comments(): + print( + FORMAT_STR_COMMENT_LINE.format_map( + {'comment': 'Package: ' + pkg_name})) + prefix = os.path.abspath(os.path.dirname(__file__)) + if not args.merged_install: + prefix = os.path.join(prefix, pkg_name) + for line in get_commands( + pkg_name, prefix, args.primary_extension, + args.additional_extension + ): + print(line) + + for line in _remove_ending_separators(): + print(line) + + +def get_packages(prefix_path, merged_install): + """ + Find packages based on colcon-specific files created during installation. + + :param Path prefix_path: The install prefix path of all packages + :param bool merged_install: The flag if the packages are all installed + directly in the prefix or if each package is installed in a subdirectory + named after the package + :returns: A mapping from the package name to the set of runtime + dependencies + :rtype: dict + """ + packages = {} + # since importing colcon_core isn't feasible here the following constant + # must match colcon_core.location.get_relative_package_index_path() + subdirectory = 'share/colcon-core/packages' + if merged_install: + # return if workspace is empty + if not (prefix_path / subdirectory).is_dir(): + return packages + # find all files in the subdirectory + for p in (prefix_path / subdirectory).iterdir(): + if not p.is_file(): + continue + if p.name.startswith('.'): + continue + add_package_runtime_dependencies(p, packages) + else: + # for each subdirectory look for the package specific file + for p in prefix_path.iterdir(): + if not p.is_dir(): + continue + if p.name.startswith('.'): + continue + p = p / subdirectory / p.name + if p.is_file(): + add_package_runtime_dependencies(p, packages) + + # remove unknown dependencies + pkg_names = set(packages.keys()) + for k in packages.keys(): + packages[k] = {d for d in packages[k] if d in pkg_names} + + return packages + + +def add_package_runtime_dependencies(path, packages): + """ + Check the path and if it exists extract the packages runtime dependencies. + + :param Path path: The resource file containing the runtime dependencies + :param dict packages: A mapping from package names to the sets of runtime + dependencies to add to + """ + content = path.read_text() + dependencies = set(content.split(os.pathsep) if content else []) + packages[path.name] = dependencies + + +def order_packages(packages): + """ + Order packages topologically. + + :param dict packages: A mapping from package name to the set of runtime + dependencies + :returns: The package names + :rtype: list + """ + # select packages with no dependencies in alphabetical order + to_be_ordered = list(packages.keys()) + ordered = [] + while to_be_ordered: + pkg_names_without_deps = [ + name for name in to_be_ordered if not packages[name]] + if not pkg_names_without_deps: + reduce_cycle_set(packages) + raise RuntimeError( + 'Circular dependency between: ' + ', '.join(sorted(packages))) + pkg_names_without_deps.sort() + pkg_name = pkg_names_without_deps[0] + to_be_ordered.remove(pkg_name) + ordered.append(pkg_name) + # remove item from dependency lists + for k in list(packages.keys()): + if pkg_name in packages[k]: + packages[k].remove(pkg_name) + return ordered + + +def reduce_cycle_set(packages): + """ + Reduce the set of packages to the ones part of the circular dependency. + + :param dict packages: A mapping from package name to the set of runtime + dependencies which is modified in place + """ + last_depended = None + while len(packages) > 0: + # get all remaining dependencies + depended = set() + for pkg_name, dependencies in packages.items(): + depended = depended.union(dependencies) + # remove all packages which are not dependent on + for name in list(packages.keys()): + if name not in depended: + del packages[name] + if last_depended: + # if remaining packages haven't changed return them + if last_depended == depended: + return packages.keys() + # otherwise reduce again + last_depended = depended + + +def _include_comments(): + # skipping comment lines when COLCON_TRACE is not set speeds up the + # processing especially on Windows + return bool(os.environ.get('COLCON_TRACE')) + + +def get_commands(pkg_name, prefix, primary_extension, additional_extension): + commands = [] + package_dsv_path = os.path.join(prefix, 'share', pkg_name, 'package.dsv') + if os.path.exists(package_dsv_path): + commands += process_dsv_file( + package_dsv_path, prefix, primary_extension, additional_extension) + return commands + + +def process_dsv_file( + dsv_path, prefix, primary_extension=None, additional_extension=None +): + commands = [] + if _include_comments(): + commands.append(FORMAT_STR_COMMENT_LINE.format_map({'comment': dsv_path})) + with open(dsv_path, 'r') as h: + content = h.read() + lines = content.splitlines() + + basenames = OrderedDict() + for i, line in enumerate(lines): + # skip over empty or whitespace-only lines + if not line.strip(): + continue + # skip over comments + if line.startswith('#'): + continue + try: + type_, remainder = line.split(';', 1) + except ValueError: + raise RuntimeError( + "Line %d in '%s' doesn't contain a semicolon separating the " + 'type from the arguments' % (i + 1, dsv_path)) + if type_ != DSV_TYPE_SOURCE: + # handle non-source lines + try: + commands += handle_dsv_types_except_source( + type_, remainder, prefix) + except RuntimeError as e: + raise RuntimeError( + "Line %d in '%s' %s" % (i + 1, dsv_path, e)) from e + else: + # group remaining source lines by basename + path_without_ext, ext = os.path.splitext(remainder) + if path_without_ext not in basenames: + basenames[path_without_ext] = set() + assert ext.startswith('.') + ext = ext[1:] + if ext in (primary_extension, additional_extension): + basenames[path_without_ext].add(ext) + + # add the dsv extension to each basename if the file exists + for basename, extensions in basenames.items(): + if not os.path.isabs(basename): + basename = os.path.join(prefix, basename) + if os.path.exists(basename + '.dsv'): + extensions.add('dsv') + + for basename, extensions in basenames.items(): + if not os.path.isabs(basename): + basename = os.path.join(prefix, basename) + if 'dsv' in extensions: + # process dsv files recursively + commands += process_dsv_file( + basename + '.dsv', prefix, primary_extension=primary_extension, + additional_extension=additional_extension) + elif primary_extension in extensions and len(extensions) == 1: + # source primary-only files + commands += [ + FORMAT_STR_INVOKE_SCRIPT.format_map({ + 'prefix': prefix, + 'script_path': basename + '.' + primary_extension})] + elif additional_extension in extensions: + # source non-primary files + commands += [ + FORMAT_STR_INVOKE_SCRIPT.format_map({ + 'prefix': prefix, + 'script_path': basename + '.' + additional_extension})] + + return commands + + +def handle_dsv_types_except_source(type_, remainder, prefix): + commands = [] + if type_ in (DSV_TYPE_SET, DSV_TYPE_SET_IF_UNSET): + try: + env_name, value = remainder.split(';', 1) + except ValueError: + raise RuntimeError( + "doesn't contain a semicolon separating the environment name " + 'from the value') + try_prefixed_value = os.path.join(prefix, value) if value else prefix + if os.path.exists(try_prefixed_value): + value = try_prefixed_value + if type_ == DSV_TYPE_SET: + commands += _set(env_name, value) + elif type_ == DSV_TYPE_SET_IF_UNSET: + commands += _set_if_unset(env_name, value) + else: + assert False + elif type_ in ( + DSV_TYPE_APPEND_NON_DUPLICATE, + DSV_TYPE_PREPEND_NON_DUPLICATE, + DSV_TYPE_PREPEND_NON_DUPLICATE_IF_EXISTS + ): + try: + env_name_and_values = remainder.split(';') + except ValueError: + raise RuntimeError( + "doesn't contain a semicolon separating the environment name " + 'from the values') + env_name = env_name_and_values[0] + values = env_name_and_values[1:] + for value in values: + if not value: + value = prefix + elif not os.path.isabs(value): + value = os.path.join(prefix, value) + if ( + type_ == DSV_TYPE_PREPEND_NON_DUPLICATE_IF_EXISTS and + not os.path.exists(value) + ): + comment = f'skip extending {env_name} with not existing ' \ + f'path: {value}' + if _include_comments(): + commands.append( + FORMAT_STR_COMMENT_LINE.format_map({'comment': comment})) + elif type_ == DSV_TYPE_APPEND_NON_DUPLICATE: + commands += _append_unique_value(env_name, value) + else: + commands += _prepend_unique_value(env_name, value) + else: + raise RuntimeError( + 'contains an unknown environment hook type: ' + type_) + return commands + + +env_state = {} + + +def _append_unique_value(name, value): + global env_state + if name not in env_state: + if os.environ.get(name): + env_state[name] = set(os.environ[name].split(os.pathsep)) + else: + env_state[name] = set() + # append even if the variable has not been set yet, in case a shell script sets the + # same variable without the knowledge of this Python script. + # later _remove_ending_separators() will cleanup any unintentional leading separator + extend = FORMAT_STR_USE_ENV_VAR.format_map({'name': name}) + os.pathsep + line = FORMAT_STR_SET_ENV_VAR.format_map( + {'name': name, 'value': extend + value}) + if value not in env_state[name]: + env_state[name].add(value) + else: + if not _include_comments(): + return [] + line = FORMAT_STR_COMMENT_LINE.format_map({'comment': line}) + return [line] + + +def _prepend_unique_value(name, value): + global env_state + if name not in env_state: + if os.environ.get(name): + env_state[name] = set(os.environ[name].split(os.pathsep)) + else: + env_state[name] = set() + # prepend even if the variable has not been set yet, in case a shell script sets the + # same variable without the knowledge of this Python script. + # later _remove_ending_separators() will cleanup any unintentional trailing separator + extend = os.pathsep + FORMAT_STR_USE_ENV_VAR.format_map({'name': name}) + line = FORMAT_STR_SET_ENV_VAR.format_map( + {'name': name, 'value': value + extend}) + if value not in env_state[name]: + env_state[name].add(value) + else: + if not _include_comments(): + return [] + line = FORMAT_STR_COMMENT_LINE.format_map({'comment': line}) + return [line] + + +# generate commands for removing prepended underscores +def _remove_ending_separators(): + # do nothing if the shell extension does not implement the logic + if FORMAT_STR_REMOVE_TRAILING_SEPARATOR is None: + return [] + + global env_state + commands = [] + for name in env_state: + # skip variables that already had values before this script started prepending + if name in os.environ: + continue + commands += [ + FORMAT_STR_REMOVE_LEADING_SEPARATOR.format_map({'name': name}), + FORMAT_STR_REMOVE_TRAILING_SEPARATOR.format_map({'name': name})] + return commands + + +def _set(name, value): + global env_state + env_state[name] = value + line = FORMAT_STR_SET_ENV_VAR.format_map( + {'name': name, 'value': value}) + return [line] + + +def _set_if_unset(name, value): + global env_state + line = FORMAT_STR_SET_ENV_VAR.format_map( + {'name': name, 'value': value}) + if env_state.get(name, os.environ.get(name)): + line = FORMAT_STR_COMMENT_LINE.format_map({'comment': line}) + return [line] + + +if __name__ == '__main__': # pragma: no cover + try: + rc = main() + except RuntimeError as e: + print(str(e), file=sys.stderr) + rc = 1 + sys.exit(rc) diff --git a/dynosam_utils/src/install/_local_setup_util_sh.py b/dynosam_utils/src/install/_local_setup_util_sh.py new file mode 100644 index 000000000..f67eaa989 --- /dev/null +++ b/dynosam_utils/src/install/_local_setup_util_sh.py @@ -0,0 +1,407 @@ +# Copyright 2016-2019 Dirk Thomas +# Licensed under the Apache License, Version 2.0 + +import argparse +from collections import OrderedDict +import os +from pathlib import Path +import sys + + +FORMAT_STR_COMMENT_LINE = '# {comment}' +FORMAT_STR_SET_ENV_VAR = 'export {name}="{value}"' +FORMAT_STR_USE_ENV_VAR = '${name}' +FORMAT_STR_INVOKE_SCRIPT = 'COLCON_CURRENT_PREFIX="{prefix}" _colcon_prefix_sh_source_script "{script_path}"' # noqa: E501 +FORMAT_STR_REMOVE_LEADING_SEPARATOR = 'if [ "$(echo -n ${name} | head -c 1)" = ":" ]; then export {name}=${{{name}#?}} ; fi' # noqa: E501 +FORMAT_STR_REMOVE_TRAILING_SEPARATOR = 'if [ "$(echo -n ${name} | tail -c 1)" = ":" ]; then export {name}=${{{name}%?}} ; fi' # noqa: E501 + +DSV_TYPE_APPEND_NON_DUPLICATE = 'append-non-duplicate' +DSV_TYPE_PREPEND_NON_DUPLICATE = 'prepend-non-duplicate' +DSV_TYPE_PREPEND_NON_DUPLICATE_IF_EXISTS = 'prepend-non-duplicate-if-exists' +DSV_TYPE_SET = 'set' +DSV_TYPE_SET_IF_UNSET = 'set-if-unset' +DSV_TYPE_SOURCE = 'source' + + +def main(argv=sys.argv[1:]): # noqa: D103 + parser = argparse.ArgumentParser( + description='Output shell commands for the packages in topological ' + 'order') + parser.add_argument( + 'primary_extension', + help='The file extension of the primary shell') + parser.add_argument( + 'additional_extension', nargs='?', + help='The additional file extension to be considered') + parser.add_argument( + '--merged-install', action='store_true', + help='All install prefixes are merged into a single location') + args = parser.parse_args(argv) + + packages = get_packages(Path(__file__).parent, args.merged_install) + + ordered_packages = order_packages(packages) + for pkg_name in ordered_packages: + if _include_comments(): + print( + FORMAT_STR_COMMENT_LINE.format_map( + {'comment': 'Package: ' + pkg_name})) + prefix = os.path.abspath(os.path.dirname(__file__)) + if not args.merged_install: + prefix = os.path.join(prefix, pkg_name) + for line in get_commands( + pkg_name, prefix, args.primary_extension, + args.additional_extension + ): + print(line) + + for line in _remove_ending_separators(): + print(line) + + +def get_packages(prefix_path, merged_install): + """ + Find packages based on colcon-specific files created during installation. + + :param Path prefix_path: The install prefix path of all packages + :param bool merged_install: The flag if the packages are all installed + directly in the prefix or if each package is installed in a subdirectory + named after the package + :returns: A mapping from the package name to the set of runtime + dependencies + :rtype: dict + """ + packages = {} + # since importing colcon_core isn't feasible here the following constant + # must match colcon_core.location.get_relative_package_index_path() + subdirectory = 'share/colcon-core/packages' + if merged_install: + # return if workspace is empty + if not (prefix_path / subdirectory).is_dir(): + return packages + # find all files in the subdirectory + for p in (prefix_path / subdirectory).iterdir(): + if not p.is_file(): + continue + if p.name.startswith('.'): + continue + add_package_runtime_dependencies(p, packages) + else: + # for each subdirectory look for the package specific file + for p in prefix_path.iterdir(): + if not p.is_dir(): + continue + if p.name.startswith('.'): + continue + p = p / subdirectory / p.name + if p.is_file(): + add_package_runtime_dependencies(p, packages) + + # remove unknown dependencies + pkg_names = set(packages.keys()) + for k in packages.keys(): + packages[k] = {d for d in packages[k] if d in pkg_names} + + return packages + + +def add_package_runtime_dependencies(path, packages): + """ + Check the path and if it exists extract the packages runtime dependencies. + + :param Path path: The resource file containing the runtime dependencies + :param dict packages: A mapping from package names to the sets of runtime + dependencies to add to + """ + content = path.read_text() + dependencies = set(content.split(os.pathsep) if content else []) + packages[path.name] = dependencies + + +def order_packages(packages): + """ + Order packages topologically. + + :param dict packages: A mapping from package name to the set of runtime + dependencies + :returns: The package names + :rtype: list + """ + # select packages with no dependencies in alphabetical order + to_be_ordered = list(packages.keys()) + ordered = [] + while to_be_ordered: + pkg_names_without_deps = [ + name for name in to_be_ordered if not packages[name]] + if not pkg_names_without_deps: + reduce_cycle_set(packages) + raise RuntimeError( + 'Circular dependency between: ' + ', '.join(sorted(packages))) + pkg_names_without_deps.sort() + pkg_name = pkg_names_without_deps[0] + to_be_ordered.remove(pkg_name) + ordered.append(pkg_name) + # remove item from dependency lists + for k in list(packages.keys()): + if pkg_name in packages[k]: + packages[k].remove(pkg_name) + return ordered + + +def reduce_cycle_set(packages): + """ + Reduce the set of packages to the ones part of the circular dependency. + + :param dict packages: A mapping from package name to the set of runtime + dependencies which is modified in place + """ + last_depended = None + while len(packages) > 0: + # get all remaining dependencies + depended = set() + for pkg_name, dependencies in packages.items(): + depended = depended.union(dependencies) + # remove all packages which are not dependent on + for name in list(packages.keys()): + if name not in depended: + del packages[name] + if last_depended: + # if remaining packages haven't changed return them + if last_depended == depended: + return packages.keys() + # otherwise reduce again + last_depended = depended + + +def _include_comments(): + # skipping comment lines when COLCON_TRACE is not set speeds up the + # processing especially on Windows + return bool(os.environ.get('COLCON_TRACE')) + + +def get_commands(pkg_name, prefix, primary_extension, additional_extension): + commands = [] + package_dsv_path = os.path.join(prefix, 'share', pkg_name, 'package.dsv') + if os.path.exists(package_dsv_path): + commands += process_dsv_file( + package_dsv_path, prefix, primary_extension, additional_extension) + return commands + + +def process_dsv_file( + dsv_path, prefix, primary_extension=None, additional_extension=None +): + commands = [] + if _include_comments(): + commands.append(FORMAT_STR_COMMENT_LINE.format_map({'comment': dsv_path})) + with open(dsv_path, 'r') as h: + content = h.read() + lines = content.splitlines() + + basenames = OrderedDict() + for i, line in enumerate(lines): + # skip over empty or whitespace-only lines + if not line.strip(): + continue + # skip over comments + if line.startswith('#'): + continue + try: + type_, remainder = line.split(';', 1) + except ValueError: + raise RuntimeError( + "Line %d in '%s' doesn't contain a semicolon separating the " + 'type from the arguments' % (i + 1, dsv_path)) + if type_ != DSV_TYPE_SOURCE: + # handle non-source lines + try: + commands += handle_dsv_types_except_source( + type_, remainder, prefix) + except RuntimeError as e: + raise RuntimeError( + "Line %d in '%s' %s" % (i + 1, dsv_path, e)) from e + else: + # group remaining source lines by basename + path_without_ext, ext = os.path.splitext(remainder) + if path_without_ext not in basenames: + basenames[path_without_ext] = set() + assert ext.startswith('.') + ext = ext[1:] + if ext in (primary_extension, additional_extension): + basenames[path_without_ext].add(ext) + + # add the dsv extension to each basename if the file exists + for basename, extensions in basenames.items(): + if not os.path.isabs(basename): + basename = os.path.join(prefix, basename) + if os.path.exists(basename + '.dsv'): + extensions.add('dsv') + + for basename, extensions in basenames.items(): + if not os.path.isabs(basename): + basename = os.path.join(prefix, basename) + if 'dsv' in extensions: + # process dsv files recursively + commands += process_dsv_file( + basename + '.dsv', prefix, primary_extension=primary_extension, + additional_extension=additional_extension) + elif primary_extension in extensions and len(extensions) == 1: + # source primary-only files + commands += [ + FORMAT_STR_INVOKE_SCRIPT.format_map({ + 'prefix': prefix, + 'script_path': basename + '.' + primary_extension})] + elif additional_extension in extensions: + # source non-primary files + commands += [ + FORMAT_STR_INVOKE_SCRIPT.format_map({ + 'prefix': prefix, + 'script_path': basename + '.' + additional_extension})] + + return commands + + +def handle_dsv_types_except_source(type_, remainder, prefix): + commands = [] + if type_ in (DSV_TYPE_SET, DSV_TYPE_SET_IF_UNSET): + try: + env_name, value = remainder.split(';', 1) + except ValueError: + raise RuntimeError( + "doesn't contain a semicolon separating the environment name " + 'from the value') + try_prefixed_value = os.path.join(prefix, value) if value else prefix + if os.path.exists(try_prefixed_value): + value = try_prefixed_value + if type_ == DSV_TYPE_SET: + commands += _set(env_name, value) + elif type_ == DSV_TYPE_SET_IF_UNSET: + commands += _set_if_unset(env_name, value) + else: + assert False + elif type_ in ( + DSV_TYPE_APPEND_NON_DUPLICATE, + DSV_TYPE_PREPEND_NON_DUPLICATE, + DSV_TYPE_PREPEND_NON_DUPLICATE_IF_EXISTS + ): + try: + env_name_and_values = remainder.split(';') + except ValueError: + raise RuntimeError( + "doesn't contain a semicolon separating the environment name " + 'from the values') + env_name = env_name_and_values[0] + values = env_name_and_values[1:] + for value in values: + if not value: + value = prefix + elif not os.path.isabs(value): + value = os.path.join(prefix, value) + if ( + type_ == DSV_TYPE_PREPEND_NON_DUPLICATE_IF_EXISTS and + not os.path.exists(value) + ): + comment = f'skip extending {env_name} with not existing ' \ + f'path: {value}' + if _include_comments(): + commands.append( + FORMAT_STR_COMMENT_LINE.format_map({'comment': comment})) + elif type_ == DSV_TYPE_APPEND_NON_DUPLICATE: + commands += _append_unique_value(env_name, value) + else: + commands += _prepend_unique_value(env_name, value) + else: + raise RuntimeError( + 'contains an unknown environment hook type: ' + type_) + return commands + + +env_state = {} + + +def _append_unique_value(name, value): + global env_state + if name not in env_state: + if os.environ.get(name): + env_state[name] = set(os.environ[name].split(os.pathsep)) + else: + env_state[name] = set() + # append even if the variable has not been set yet, in case a shell script sets the + # same variable without the knowledge of this Python script. + # later _remove_ending_separators() will cleanup any unintentional leading separator + extend = FORMAT_STR_USE_ENV_VAR.format_map({'name': name}) + os.pathsep + line = FORMAT_STR_SET_ENV_VAR.format_map( + {'name': name, 'value': extend + value}) + if value not in env_state[name]: + env_state[name].add(value) + else: + if not _include_comments(): + return [] + line = FORMAT_STR_COMMENT_LINE.format_map({'comment': line}) + return [line] + + +def _prepend_unique_value(name, value): + global env_state + if name not in env_state: + if os.environ.get(name): + env_state[name] = set(os.environ[name].split(os.pathsep)) + else: + env_state[name] = set() + # prepend even if the variable has not been set yet, in case a shell script sets the + # same variable without the knowledge of this Python script. + # later _remove_ending_separators() will cleanup any unintentional trailing separator + extend = os.pathsep + FORMAT_STR_USE_ENV_VAR.format_map({'name': name}) + line = FORMAT_STR_SET_ENV_VAR.format_map( + {'name': name, 'value': value + extend}) + if value not in env_state[name]: + env_state[name].add(value) + else: + if not _include_comments(): + return [] + line = FORMAT_STR_COMMENT_LINE.format_map({'comment': line}) + return [line] + + +# generate commands for removing prepended underscores +def _remove_ending_separators(): + # do nothing if the shell extension does not implement the logic + if FORMAT_STR_REMOVE_TRAILING_SEPARATOR is None: + return [] + + global env_state + commands = [] + for name in env_state: + # skip variables that already had values before this script started prepending + if name in os.environ: + continue + commands += [ + FORMAT_STR_REMOVE_LEADING_SEPARATOR.format_map({'name': name}), + FORMAT_STR_REMOVE_TRAILING_SEPARATOR.format_map({'name': name})] + return commands + + +def _set(name, value): + global env_state + env_state[name] = value + line = FORMAT_STR_SET_ENV_VAR.format_map( + {'name': name, 'value': value}) + return [line] + + +def _set_if_unset(name, value): + global env_state + line = FORMAT_STR_SET_ENV_VAR.format_map( + {'name': name, 'value': value}) + if env_state.get(name, os.environ.get(name)): + line = FORMAT_STR_COMMENT_LINE.format_map({'comment': line}) + return [line] + + +if __name__ == '__main__': # pragma: no cover + try: + rc = main() + except RuntimeError as e: + print(str(e), file=sys.stderr) + rc = 1 + sys.exit(rc) diff --git a/dynosam_utils/src/install/local_setup.bash b/dynosam_utils/src/install/local_setup.bash new file mode 100644 index 000000000..03f00256c --- /dev/null +++ b/dynosam_utils/src/install/local_setup.bash @@ -0,0 +1,121 @@ +# generated from colcon_bash/shell/template/prefix.bash.em + +# This script extends the environment with all packages contained in this +# prefix path. + +# a bash script is able to determine its own path if necessary +if [ -z "$COLCON_CURRENT_PREFIX" ]; then + _colcon_prefix_bash_COLCON_CURRENT_PREFIX="$(builtin cd "`dirname "${BASH_SOURCE[0]}"`" > /dev/null && pwd)" +else + _colcon_prefix_bash_COLCON_CURRENT_PREFIX="$COLCON_CURRENT_PREFIX" +fi + +# function to prepend a value to a variable +# which uses colons as separators +# duplicates as well as trailing separators are avoided +# first argument: the name of the result variable +# second argument: the value to be prepended +_colcon_prefix_bash_prepend_unique_value() { + # arguments + _listname="$1" + _value="$2" + + # get values from variable + eval _values=\"\$$_listname\" + # backup the field separator + _colcon_prefix_bash_prepend_unique_value_IFS="$IFS" + IFS=":" + # start with the new value + _all_values="$_value" + _contained_value="" + # iterate over existing values in the variable + for _item in $_values; do + # ignore empty strings + if [ -z "$_item" ]; then + continue + fi + # ignore duplicates of _value + if [ "$_item" = "$_value" ]; then + _contained_value=1 + continue + fi + # keep non-duplicate values + _all_values="$_all_values:$_item" + done + unset _item + if [ -z "$_contained_value" ]; then + if [ -n "$COLCON_TRACE" ]; then + if [ "$_all_values" = "$_value" ]; then + echo "export $_listname=$_value" + else + echo "export $_listname=$_value:\$$_listname" + fi + fi + fi + unset _contained_value + # restore the field separator + IFS="$_colcon_prefix_bash_prepend_unique_value_IFS" + unset _colcon_prefix_bash_prepend_unique_value_IFS + # export the updated variable + eval export $_listname=\"$_all_values\" + unset _all_values + unset _values + + unset _value + unset _listname +} + +# add this prefix to the COLCON_PREFIX_PATH +_colcon_prefix_bash_prepend_unique_value COLCON_PREFIX_PATH "$_colcon_prefix_bash_COLCON_CURRENT_PREFIX" +unset _colcon_prefix_bash_prepend_unique_value + +# check environment variable for custom Python executable +if [ -n "$COLCON_PYTHON_EXECUTABLE" ]; then + if [ ! -f "$COLCON_PYTHON_EXECUTABLE" ]; then + echo "error: COLCON_PYTHON_EXECUTABLE '$COLCON_PYTHON_EXECUTABLE' doesn't exist" + return 1 + fi + _colcon_python_executable="$COLCON_PYTHON_EXECUTABLE" +else + # try the Python executable known at configure time + _colcon_python_executable="/usr/bin/python3" + # if it doesn't exist try a fall back + if [ ! -f "$_colcon_python_executable" ]; then + if ! /usr/bin/env python3 --version > /dev/null 2> /dev/null; then + echo "error: unable to find python3 executable" + return 1 + fi + _colcon_python_executable=`/usr/bin/env python3 -c "import sys; print(sys.executable)"` + fi +fi + +# function to source another script with conditional trace output +# first argument: the path of the script +_colcon_prefix_sh_source_script() { + if [ -f "$1" ]; then + if [ -n "$COLCON_TRACE" ]; then + echo "# . \"$1\"" + fi + . "$1" + else + echo "not found: \"$1\"" 1>&2 + fi +} + +# get all commands in topological order +_colcon_ordered_commands="$($_colcon_python_executable "$_colcon_prefix_bash_COLCON_CURRENT_PREFIX/_local_setup_util_sh.py" sh bash)" +unset _colcon_python_executable +if [ -n "$COLCON_TRACE" ]; then + echo "$(declare -f _colcon_prefix_sh_source_script)" + echo "# Execute generated script:" + echo "# <<<" + echo "${_colcon_ordered_commands}" + echo "# >>>" + echo "unset _colcon_prefix_sh_source_script" +fi +eval "${_colcon_ordered_commands}" +unset _colcon_ordered_commands + +unset _colcon_prefix_sh_source_script + +unset _colcon_prefix_bash_COLCON_CURRENT_PREFIX diff --git a/dynosam_utils/src/install/local_setup.ps1 b/dynosam_utils/src/install/local_setup.ps1 new file mode 100644 index 000000000..6f68c8ded --- /dev/null +++ b/dynosam_utils/src/install/local_setup.ps1 @@ -0,0 +1,55 @@ +# generated from colcon_powershell/shell/template/prefix.ps1.em + +# This script extends the environment with all packages contained in this +# prefix path. + +# check environment variable for custom Python executable +if ($env:COLCON_PYTHON_EXECUTABLE) { + if (!(Test-Path "$env:COLCON_PYTHON_EXECUTABLE" -PathType Leaf)) { + echo "error: COLCON_PYTHON_EXECUTABLE '$env:COLCON_PYTHON_EXECUTABLE' doesn't exist" + exit 1 + } + $_colcon_python_executable="$env:COLCON_PYTHON_EXECUTABLE" +} else { + # use the Python executable known at configure time + $_colcon_python_executable="/usr/bin/python3" + # if it doesn't exist try a fall back + if (!(Test-Path "$_colcon_python_executable" -PathType Leaf)) { + if (!(Get-Command "python3" -ErrorAction SilentlyContinue)) { + echo "error: unable to find python3 executable" + exit 1 + } + $_colcon_python_executable="python3" + } +} + +# function to source another script with conditional trace output +# first argument: the path of the script +function _colcon_prefix_powershell_source_script { + param ( + $_colcon_prefix_powershell_source_script_param + ) + # source script with conditional trace output + if (Test-Path $_colcon_prefix_powershell_source_script_param) { + if ($env:COLCON_TRACE) { + echo ". '$_colcon_prefix_powershell_source_script_param'" + } + . "$_colcon_prefix_powershell_source_script_param" + } else { + Write-Error "not found: '$_colcon_prefix_powershell_source_script_param'" + } +} + +# get all commands in topological order +$_colcon_ordered_commands = & "$_colcon_python_executable" "$(Split-Path $PSCommandPath -Parent)/_local_setup_util_ps1.py" ps1 + +# execute all commands in topological order +if ($env:COLCON_TRACE) { + echo "Execute generated script:" + echo "<<<" + $_colcon_ordered_commands.Split([Environment]::NewLine, [StringSplitOptions]::RemoveEmptyEntries) | Write-Output + echo ">>>" +} +if ($_colcon_ordered_commands) { + $_colcon_ordered_commands.Split([Environment]::NewLine, [StringSplitOptions]::RemoveEmptyEntries) | Invoke-Expression +} diff --git a/dynosam_utils/src/install/local_setup.sh b/dynosam_utils/src/install/local_setup.sh new file mode 100644 index 000000000..e6ca4169e --- /dev/null +++ b/dynosam_utils/src/install/local_setup.sh @@ -0,0 +1,137 @@ +# generated from colcon_core/shell/template/prefix.sh.em + +# This script extends the environment with all packages contained in this +# prefix path. + +# since a plain shell script can't determine its own path when being sourced +# either use the provided COLCON_CURRENT_PREFIX +# or fall back to the build time prefix (if it exists) +_colcon_prefix_sh_COLCON_CURRENT_PREFIX="/home/user/dev_ws/src/core/dynosam_utils/src/install" +if [ -z "$COLCON_CURRENT_PREFIX" ]; then + if [ ! -d "$_colcon_prefix_sh_COLCON_CURRENT_PREFIX" ]; then + echo "The build time path \"$_colcon_prefix_sh_COLCON_CURRENT_PREFIX\" doesn't exist. Either source a script for a different shell or set the environment variable \"COLCON_CURRENT_PREFIX\" explicitly." 1>&2 + unset _colcon_prefix_sh_COLCON_CURRENT_PREFIX + return 1 + fi +else + _colcon_prefix_sh_COLCON_CURRENT_PREFIX="$COLCON_CURRENT_PREFIX" +fi + +# function to prepend a value to a variable +# which uses colons as separators +# duplicates as well as trailing separators are avoided +# first argument: the name of the result variable +# second argument: the value to be prepended +_colcon_prefix_sh_prepend_unique_value() { + # arguments + _listname="$1" + _value="$2" + + # get values from variable + eval _values=\"\$$_listname\" + # backup the field separator + _colcon_prefix_sh_prepend_unique_value_IFS="$IFS" + IFS=":" + # start with the new value + _all_values="$_value" + _contained_value="" + # iterate over existing values in the variable + for _item in $_values; do + # ignore empty strings + if [ -z "$_item" ]; then + continue + fi + # ignore duplicates of _value + if [ "$_item" = "$_value" ]; then + _contained_value=1 + continue + fi + # keep non-duplicate values + _all_values="$_all_values:$_item" + done + unset _item + if [ -z "$_contained_value" ]; then + if [ -n "$COLCON_TRACE" ]; then + if [ "$_all_values" = "$_value" ]; then + echo "export $_listname=$_value" + else + echo "export $_listname=$_value:\$$_listname" + fi + fi + fi + unset _contained_value + # restore the field separator + IFS="$_colcon_prefix_sh_prepend_unique_value_IFS" + unset _colcon_prefix_sh_prepend_unique_value_IFS + # export the updated variable + eval export $_listname=\"$_all_values\" + unset _all_values + unset _values + + unset _value + unset _listname +} + +# add this prefix to the COLCON_PREFIX_PATH +_colcon_prefix_sh_prepend_unique_value COLCON_PREFIX_PATH "$_colcon_prefix_sh_COLCON_CURRENT_PREFIX" +unset _colcon_prefix_sh_prepend_unique_value + +# check environment variable for custom Python executable +if [ -n "$COLCON_PYTHON_EXECUTABLE" ]; then + if [ ! -f "$COLCON_PYTHON_EXECUTABLE" ]; then + echo "error: COLCON_PYTHON_EXECUTABLE '$COLCON_PYTHON_EXECUTABLE' doesn't exist" + return 1 + fi + _colcon_python_executable="$COLCON_PYTHON_EXECUTABLE" +else + # try the Python executable known at configure time + _colcon_python_executable="/usr/bin/python3" + # if it doesn't exist try a fall back + if [ ! -f "$_colcon_python_executable" ]; then + if ! /usr/bin/env python3 --version > /dev/null 2> /dev/null; then + echo "error: unable to find python3 executable" + return 1 + fi + _colcon_python_executable=`/usr/bin/env python3 -c "import sys; print(sys.executable)"` + fi +fi + +# function to source another script with conditional trace output +# first argument: the path of the script +_colcon_prefix_sh_source_script() { + if [ -f "$1" ]; then + if [ -n "$COLCON_TRACE" ]; then + echo "# . \"$1\"" + fi + . "$1" + else + echo "not found: \"$1\"" 1>&2 + fi +} + +# get all commands in topological order +_colcon_ordered_commands="$($_colcon_python_executable "$_colcon_prefix_sh_COLCON_CURRENT_PREFIX/_local_setup_util_sh.py" sh)" +unset _colcon_python_executable +if [ -n "$COLCON_TRACE" ]; then + echo "_colcon_prefix_sh_source_script() { + if [ -f \"\$1\" ]; then + if [ -n \"\$COLCON_TRACE\" ]; then + echo \"# . \\\"\$1\\\"\" + fi + . \"\$1\" + else + echo \"not found: \\\"\$1\\\"\" 1>&2 + fi + }" + echo "# Execute generated script:" + echo "# <<<" + echo "${_colcon_ordered_commands}" + echo "# >>>" + echo "unset _colcon_prefix_sh_source_script" +fi +eval "${_colcon_ordered_commands}" +unset _colcon_ordered_commands + +unset _colcon_prefix_sh_source_script + +unset _colcon_prefix_sh_COLCON_CURRENT_PREFIX diff --git a/dynosam_utils/src/install/local_setup.zsh b/dynosam_utils/src/install/local_setup.zsh new file mode 100644 index 000000000..b6487102f --- /dev/null +++ b/dynosam_utils/src/install/local_setup.zsh @@ -0,0 +1,134 @@ +# generated from colcon_zsh/shell/template/prefix.zsh.em + +# This script extends the environment with all packages contained in this +# prefix path. + +# a zsh script is able to determine its own path if necessary +if [ -z "$COLCON_CURRENT_PREFIX" ]; then + _colcon_prefix_zsh_COLCON_CURRENT_PREFIX="$(builtin cd -q "`dirname "${(%):-%N}"`" > /dev/null && pwd)" +else + _colcon_prefix_zsh_COLCON_CURRENT_PREFIX="$COLCON_CURRENT_PREFIX" +fi + +# function to convert array-like strings into arrays +# to workaround SH_WORD_SPLIT not being set +_colcon_prefix_zsh_convert_to_array() { + local _listname=$1 + local _dollar="$" + local _split="{=" + local _to_array="(\"$_dollar$_split$_listname}\")" + eval $_listname=$_to_array +} + +# function to prepend a value to a variable +# which uses colons as separators +# duplicates as well as trailing separators are avoided +# first argument: the name of the result variable +# second argument: the value to be prepended +_colcon_prefix_zsh_prepend_unique_value() { + # arguments + _listname="$1" + _value="$2" + + # get values from variable + eval _values=\"\$$_listname\" + # backup the field separator + _colcon_prefix_zsh_prepend_unique_value_IFS="$IFS" + IFS=":" + # start with the new value + _all_values="$_value" + _contained_value="" + # workaround SH_WORD_SPLIT not being set + _colcon_prefix_zsh_convert_to_array _values + # iterate over existing values in the variable + for _item in $_values; do + # ignore empty strings + if [ -z "$_item" ]; then + continue + fi + # ignore duplicates of _value + if [ "$_item" = "$_value" ]; then + _contained_value=1 + continue + fi + # keep non-duplicate values + _all_values="$_all_values:$_item" + done + unset _item + if [ -z "$_contained_value" ]; then + if [ -n "$COLCON_TRACE" ]; then + if [ "$_all_values" = "$_value" ]; then + echo "export $_listname=$_value" + else + echo "export $_listname=$_value:\$$_listname" + fi + fi + fi + unset _contained_value + # restore the field separator + IFS="$_colcon_prefix_zsh_prepend_unique_value_IFS" + unset _colcon_prefix_zsh_prepend_unique_value_IFS + # export the updated variable + eval export $_listname=\"$_all_values\" + unset _all_values + unset _values + + unset _value + unset _listname +} + +# add this prefix to the COLCON_PREFIX_PATH +_colcon_prefix_zsh_prepend_unique_value COLCON_PREFIX_PATH "$_colcon_prefix_zsh_COLCON_CURRENT_PREFIX" +unset _colcon_prefix_zsh_prepend_unique_value +unset _colcon_prefix_zsh_convert_to_array + +# check environment variable for custom Python executable +if [ -n "$COLCON_PYTHON_EXECUTABLE" ]; then + if [ ! -f "$COLCON_PYTHON_EXECUTABLE" ]; then + echo "error: COLCON_PYTHON_EXECUTABLE '$COLCON_PYTHON_EXECUTABLE' doesn't exist" + return 1 + fi + _colcon_python_executable="$COLCON_PYTHON_EXECUTABLE" +else + # try the Python executable known at configure time + _colcon_python_executable="/usr/bin/python3" + # if it doesn't exist try a fall back + if [ ! -f "$_colcon_python_executable" ]; then + if ! /usr/bin/env python3 --version > /dev/null 2> /dev/null; then + echo "error: unable to find python3 executable" + return 1 + fi + _colcon_python_executable=`/usr/bin/env python3 -c "import sys; print(sys.executable)"` + fi +fi + +# function to source another script with conditional trace output +# first argument: the path of the script +_colcon_prefix_sh_source_script() { + if [ -f "$1" ]; then + if [ -n "$COLCON_TRACE" ]; then + echo "# . \"$1\"" + fi + . "$1" + else + echo "not found: \"$1\"" 1>&2 + fi +} + +# get all commands in topological order +_colcon_ordered_commands="$($_colcon_python_executable "$_colcon_prefix_zsh_COLCON_CURRENT_PREFIX/_local_setup_util_sh.py" sh zsh)" +unset _colcon_python_executable +if [ -n "$COLCON_TRACE" ]; then + echo "$(declare -f _colcon_prefix_sh_source_script)" + echo "# Execute generated script:" + echo "# <<<" + echo "${_colcon_ordered_commands}" + echo "# >>>" + echo "unset _colcon_prefix_sh_source_script" +fi +eval "${_colcon_ordered_commands}" +unset _colcon_ordered_commands + +unset _colcon_prefix_sh_source_script + +unset _colcon_prefix_zsh_COLCON_CURRENT_PREFIX diff --git a/dynosam_utils/src/install/setup.bash b/dynosam_utils/src/install/setup.bash new file mode 100644 index 000000000..396f8082a --- /dev/null +++ b/dynosam_utils/src/install/setup.bash @@ -0,0 +1,34 @@ +# generated from colcon_bash/shell/template/prefix_chain.bash.em + +# This script extends the environment with the environment of other prefix +# paths which were sourced when this file was generated as well as all packages +# contained in this prefix path. + +# function to source another script with conditional trace output +# first argument: the path of the script +_colcon_prefix_chain_bash_source_script() { + if [ -f "$1" ]; then + if [ -n "$COLCON_TRACE" ]; then + echo "# . \"$1\"" + fi + . "$1" + else + echo "not found: \"$1\"" 1>&2 + fi +} + +# source chained prefixes +# setting COLCON_CURRENT_PREFIX avoids determining the prefix in the sourced script +COLCON_CURRENT_PREFIX="/opt/ros/kilted" +_colcon_prefix_chain_bash_source_script "$COLCON_CURRENT_PREFIX/local_setup.bash" +# setting COLCON_CURRENT_PREFIX avoids determining the prefix in the sourced script +COLCON_CURRENT_PREFIX="/home/user/dev_ws/install" +_colcon_prefix_chain_bash_source_script "$COLCON_CURRENT_PREFIX/local_setup.bash" + +# source this prefix +# setting COLCON_CURRENT_PREFIX avoids determining the prefix in the sourced script +COLCON_CURRENT_PREFIX="$(builtin cd "`dirname "${BASH_SOURCE[0]}"`" > /dev/null && pwd)" +_colcon_prefix_chain_bash_source_script "$COLCON_CURRENT_PREFIX/local_setup.bash" + +unset COLCON_CURRENT_PREFIX +unset _colcon_prefix_chain_bash_source_script diff --git a/dynosam_utils/src/install/setup.ps1 b/dynosam_utils/src/install/setup.ps1 new file mode 100644 index 000000000..828baa9aa --- /dev/null +++ b/dynosam_utils/src/install/setup.ps1 @@ -0,0 +1,30 @@ +# generated from colcon_powershell/shell/template/prefix_chain.ps1.em + +# This script extends the environment with the environment of other prefix +# paths which were sourced when this file was generated as well as all packages +# contained in this prefix path. + +# function to source another script with conditional trace output +# first argument: the path of the script +function _colcon_prefix_chain_powershell_source_script { + param ( + $_colcon_prefix_chain_powershell_source_script_param + ) + # source script with conditional trace output + if (Test-Path $_colcon_prefix_chain_powershell_source_script_param) { + if ($env:COLCON_TRACE) { + echo ". '$_colcon_prefix_chain_powershell_source_script_param'" + } + . "$_colcon_prefix_chain_powershell_source_script_param" + } else { + Write-Error "not found: '$_colcon_prefix_chain_powershell_source_script_param'" + } +} + +# source chained prefixes +_colcon_prefix_chain_powershell_source_script "/opt/ros/kilted\local_setup.ps1" +_colcon_prefix_chain_powershell_source_script "/home/user/dev_ws/install\local_setup.ps1" + +# source this prefix +$env:COLCON_CURRENT_PREFIX=(Split-Path $PSCommandPath -Parent) +_colcon_prefix_chain_powershell_source_script "$env:COLCON_CURRENT_PREFIX\local_setup.ps1" diff --git a/dynosam_utils/src/install/setup.sh b/dynosam_utils/src/install/setup.sh new file mode 100644 index 000000000..ad88868fb --- /dev/null +++ b/dynosam_utils/src/install/setup.sh @@ -0,0 +1,49 @@ +# generated from colcon_core/shell/template/prefix_chain.sh.em + +# This script extends the environment with the environment of other prefix +# paths which were sourced when this file was generated as well as all packages +# contained in this prefix path. + +# since a plain shell script can't determine its own path when being sourced +# either use the provided COLCON_CURRENT_PREFIX +# or fall back to the build time prefix (if it exists) +_colcon_prefix_chain_sh_COLCON_CURRENT_PREFIX=/home/user/dev_ws/src/core/dynosam_utils/src/install +if [ ! -z "$COLCON_CURRENT_PREFIX" ]; then + _colcon_prefix_chain_sh_COLCON_CURRENT_PREFIX="$COLCON_CURRENT_PREFIX" +elif [ ! -d "$_colcon_prefix_chain_sh_COLCON_CURRENT_PREFIX" ]; then + echo "The build time path \"$_colcon_prefix_chain_sh_COLCON_CURRENT_PREFIX\" doesn't exist. Either source a script for a different shell or set the environment variable \"COLCON_CURRENT_PREFIX\" explicitly." 1>&2 + unset _colcon_prefix_chain_sh_COLCON_CURRENT_PREFIX + return 1 +fi + +# function to source another script with conditional trace output +# first argument: the path of the script +_colcon_prefix_chain_sh_source_script() { + if [ -f "$1" ]; then + if [ -n "$COLCON_TRACE" ]; then + echo "# . \"$1\"" + fi + . "$1" + else + echo "not found: \"$1\"" 1>&2 + fi +} + +# source chained prefixes +# setting COLCON_CURRENT_PREFIX avoids relying on the build time prefix of the sourced script +COLCON_CURRENT_PREFIX="/opt/ros/kilted" +_colcon_prefix_chain_sh_source_script "$COLCON_CURRENT_PREFIX/local_setup.sh" + +# setting COLCON_CURRENT_PREFIX avoids relying on the build time prefix of the sourced script +COLCON_CURRENT_PREFIX="/home/user/dev_ws/install" +_colcon_prefix_chain_sh_source_script "$COLCON_CURRENT_PREFIX/local_setup.sh" + + +# source this prefix +# setting COLCON_CURRENT_PREFIX avoids relying on the build time prefix of the sourced script +COLCON_CURRENT_PREFIX="$_colcon_prefix_chain_sh_COLCON_CURRENT_PREFIX" +_colcon_prefix_chain_sh_source_script "$COLCON_CURRENT_PREFIX/local_setup.sh" + +unset _colcon_prefix_chain_sh_COLCON_CURRENT_PREFIX +unset _colcon_prefix_chain_sh_source_script +unset COLCON_CURRENT_PREFIX diff --git a/dynosam_utils/src/install/setup.zsh b/dynosam_utils/src/install/setup.zsh new file mode 100644 index 000000000..fbb28ce08 --- /dev/null +++ b/dynosam_utils/src/install/setup.zsh @@ -0,0 +1,34 @@ +# generated from colcon_zsh/shell/template/prefix_chain.zsh.em + +# This script extends the environment with the environment of other prefix +# paths which were sourced when this file was generated as well as all packages +# contained in this prefix path. + +# function to source another script with conditional trace output +# first argument: the path of the script +_colcon_prefix_chain_zsh_source_script() { + if [ -f "$1" ]; then + if [ -n "$COLCON_TRACE" ]; then + echo "# . \"$1\"" + fi + . "$1" + else + echo "not found: \"$1\"" 1>&2 + fi +} + +# source chained prefixes +# setting COLCON_CURRENT_PREFIX avoids determining the prefix in the sourced script +COLCON_CURRENT_PREFIX="/opt/ros/kilted" +_colcon_prefix_chain_zsh_source_script "$COLCON_CURRENT_PREFIX/local_setup.zsh" +# setting COLCON_CURRENT_PREFIX avoids determining the prefix in the sourced script +COLCON_CURRENT_PREFIX="/home/user/dev_ws/install" +_colcon_prefix_chain_zsh_source_script "$COLCON_CURRENT_PREFIX/local_setup.zsh" + +# source this prefix +# setting COLCON_CURRENT_PREFIX avoids determining the prefix in the sourced script +COLCON_CURRENT_PREFIX="$(builtin cd -q "`dirname "${(%):-%N}"`" > /dev/null && pwd)" +_colcon_prefix_chain_zsh_source_script "$COLCON_CURRENT_PREFIX/local_setup.zsh" + +unset COLCON_CURRENT_PREFIX +unset _colcon_prefix_chain_zsh_source_script diff --git a/dynosam_utils/src/log/COLCON_IGNORE b/dynosam_utils/src/log/COLCON_IGNORE new file mode 100644 index 000000000..e69de29bb diff --git a/dynosam_utils/src/log/build_2026-01-27_04-03-52/events.log b/dynosam_utils/src/log/build_2026-01-27_04-03-52/events.log new file mode 100644 index 000000000..aac44b638 --- /dev/null +++ b/dynosam_utils/src/log/build_2026-01-27_04-03-52/events.log @@ -0,0 +1,2 @@ +[0.000000] (-) TimerEvent: {} +[0.000135] (-) EventReactorShutdown: {} diff --git a/dynosam_utils/src/log/build_2026-01-27_04-03-52/logger_all.log b/dynosam_utils/src/log/build_2026-01-27_04-03-52/logger_all.log new file mode 100644 index 000000000..e731db663 --- /dev/null +++ b/dynosam_utils/src/log/build_2026-01-27_04-03-52/logger_all.log @@ -0,0 +1,64 @@ +[0.220s] DEBUG:colcon:Command line arguments: ['/usr/bin/colcon', 'build', '--symlink-install'] +[0.220s] DEBUG:colcon:Parsed command line arguments: Namespace(log_base=None, log_level=None, verb_name='build', build_base='build', install_base='install', merge_install=False, symlink_install=True, test_result_base=None, continue_on_error=False, executor='parallel', parallel_workers=20, event_handlers=None, ignore_user_meta=False, metas=['./colcon.meta'], base_paths=['.'], packages_ignore=None, packages_ignore_regex=None, paths=None, packages_up_to=None, packages_up_to_regex=None, packages_above=None, packages_above_and_dependencies=None, packages_above_depth=None, packages_select_by_dep=None, packages_skip_by_dep=None, packages_skip_up_to=None, packages_select_build_failed=False, packages_skip_build_finished=False, packages_select_test_failures=False, packages_skip_test_passed=False, packages_select=None, packages_skip=None, packages_select_regex=None, packages_skip_regex=None, packages_start=None, packages_end=None, allow_overriding=[], cmake_args=None, cmake_target=None, cmake_target_skip_unavailable=False, cmake_clean_cache=False, cmake_clean_first=False, cmake_force_configure=False, ament_cmake_args=None, catkin_cmake_args=None, catkin_skip_building_tests=False, verb_parser=, verb_extension=, main=>) +[0.273s] Level 1:colcon.colcon_core.package_discovery:discover_packages(colcon_meta) check parameters +[0.273s] Level 1:colcon.colcon_core.package_discovery:discover_packages(recursive) check parameters +[0.273s] Level 1:colcon.colcon_core.package_discovery:discover_packages(ignore) check parameters +[0.273s] Level 1:colcon.colcon_core.package_discovery:discover_packages(path) check parameters +[0.273s] Level 1:colcon.colcon_core.package_discovery:discover_packages(colcon_meta) discover +[0.273s] Level 1:colcon.colcon_core.package_discovery:discover_packages(recursive) discover +[0.273s] INFO:colcon.colcon_core.package_discovery:Crawling recursively for packages in '/home/user/dev_ws/src/core/dynosam_utils/src' +[0.273s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extensions ['ignore', 'ignore_ament_install'] +[0.273s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extension 'ignore' +[0.273s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extension 'ignore_ament_install' +[0.273s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extensions ['colcon_pkg'] +[0.273s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extension 'colcon_pkg' +[0.273s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extensions ['colcon_meta'] +[0.273s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extension 'colcon_meta' +[0.273s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extensions ['ros'] +[0.273s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extension 'ros' +[0.295s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extensions ['cmake', 'python'] +[0.295s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extension 'cmake' +[0.295s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extension 'python' +[0.295s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extensions ['python_setup_py'] +[0.295s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extension 'python_setup_py' +[0.296s] Level 1:colcon.colcon_core.package_identification:_identify(build) by extensions ['ignore', 'ignore_ament_install'] +[0.296s] Level 1:colcon.colcon_core.package_identification:_identify(build) by extension 'ignore' +[0.296s] Level 1:colcon.colcon_core.package_identification:_identify(build) ignored +[0.296s] Level 1:colcon.colcon_core.package_identification:_identify(install) by extensions ['ignore', 'ignore_ament_install'] +[0.296s] Level 1:colcon.colcon_core.package_identification:_identify(install) by extension 'ignore' +[0.296s] Level 1:colcon.colcon_core.package_identification:_identify(install) ignored +[0.296s] Level 1:colcon.colcon_core.package_identification:_identify(log) by extensions ['ignore', 'ignore_ament_install'] +[0.296s] Level 1:colcon.colcon_core.package_identification:_identify(log) by extension 'ignore' +[0.296s] Level 1:colcon.colcon_core.package_identification:_identify(log) ignored +[0.296s] Level 1:colcon.colcon_core.package_discovery:discover_packages(recursive) using defaults +[0.296s] Level 1:colcon.colcon_core.package_discovery:discover_packages(ignore) discover +[0.296s] Level 1:colcon.colcon_core.package_discovery:discover_packages(ignore) using defaults +[0.296s] Level 1:colcon.colcon_core.package_discovery:discover_packages(path) discover +[0.296s] Level 1:colcon.colcon_core.package_discovery:discover_packages(path) using defaults +[0.321s] Level 1:colcon.colcon_core.package_discovery:discover_packages(prefix_path) check parameters +[0.321s] Level 1:colcon.colcon_core.package_discovery:discover_packages(prefix_path) discover +[0.326s] DEBUG:colcon.colcon_installed_package_information.package_discovery:Found 345 installed packages in /opt/ros/kilted +[0.327s] Level 1:colcon.colcon_core.package_discovery:discover_packages(prefix_path) using defaults +[0.479s] INFO:colcon.colcon_core.executor:Executing jobs using 'parallel' executor +[0.480s] DEBUG:colcon.colcon_parallel_executor.executor.parallel:run_until_complete +[0.480s] DEBUG:colcon.colcon_parallel_executor.executor.parallel:closing loop +[0.480s] DEBUG:colcon.colcon_parallel_executor.executor.parallel:loop closed +[0.480s] DEBUG:colcon.colcon_parallel_executor.executor.parallel:run_until_complete finished with '0' +[0.480s] DEBUG:colcon.colcon_core.event_reactor:joining thread +[0.496s] INFO:colcon.colcon_core.plugin_system:Skipping extension 'colcon_notification.desktop_notification.notify_send': Could not find 'notify-send' +[0.496s] INFO:colcon.colcon_core.plugin_system:Skipping extension 'colcon_notification.desktop_notification.terminal_notifier': Not used on non-Darwin systems +[0.496s] INFO:colcon.colcon_core.plugin_system:Skipping extension 'colcon_notification.desktop_notification.win32': Not used on non-Windows systems +[0.496s] INFO:colcon.colcon_notification.desktop_notification:Sending desktop notification using 'notify2' +[0.507s] DEBUG:colcon.colcon_notification.desktop_notification.notify2:Failed to initialize notify2: org.freedesktop.DBus.Error.Spawn.ExecFailed: /usr/bin/dbus-launch terminated abnormally without any error message +[0.508s] DEBUG:colcon.colcon_core.event_reactor:joined thread +[0.518s] INFO:colcon.colcon_core.plugin_system:Skipping extension 'colcon_core.shell.bat': Not used on non-Windows systems +[0.519s] INFO:colcon.colcon_core.shell:Creating prefix script '/home/user/dev_ws/src/core/dynosam_utils/src/install/local_setup.ps1' +[0.521s] INFO:colcon.colcon_core.shell:Creating prefix util module '/home/user/dev_ws/src/core/dynosam_utils/src/install/_local_setup_util_ps1.py' +[0.523s] INFO:colcon.colcon_core.shell:Creating prefix chain script '/home/user/dev_ws/src/core/dynosam_utils/src/install/setup.ps1' +[0.525s] INFO:colcon.colcon_core.shell:Creating prefix script '/home/user/dev_ws/src/core/dynosam_utils/src/install/local_setup.sh' +[0.526s] INFO:colcon.colcon_core.shell:Creating prefix util module '/home/user/dev_ws/src/core/dynosam_utils/src/install/_local_setup_util_sh.py' +[0.527s] INFO:colcon.colcon_core.shell:Creating prefix chain script '/home/user/dev_ws/src/core/dynosam_utils/src/install/setup.sh' +[0.529s] INFO:colcon.colcon_core.shell:Creating prefix script '/home/user/dev_ws/src/core/dynosam_utils/src/install/local_setup.bash' +[0.530s] INFO:colcon.colcon_core.shell:Creating prefix chain script '/home/user/dev_ws/src/core/dynosam_utils/src/install/setup.bash' +[0.531s] INFO:colcon.colcon_core.shell:Creating prefix script '/home/user/dev_ws/src/core/dynosam_utils/src/install/local_setup.zsh' +[0.532s] INFO:colcon.colcon_core.shell:Creating prefix chain script '/home/user/dev_ws/src/core/dynosam_utils/src/install/setup.zsh' diff --git a/dynosam_utils/src/log/build_2026-01-28_00-38-18/events.log b/dynosam_utils/src/log/build_2026-01-28_00-38-18/events.log new file mode 100644 index 000000000..d44325331 --- /dev/null +++ b/dynosam_utils/src/log/build_2026-01-28_00-38-18/events.log @@ -0,0 +1,2 @@ +[0.000000] (-) TimerEvent: {} +[0.000461] (-) EventReactorShutdown: {} diff --git a/dynosam_utils/src/log/build_2026-01-28_00-38-18/logger_all.log b/dynosam_utils/src/log/build_2026-01-28_00-38-18/logger_all.log new file mode 100644 index 000000000..4dcaa6f21 --- /dev/null +++ b/dynosam_utils/src/log/build_2026-01-28_00-38-18/logger_all.log @@ -0,0 +1,65 @@ +[0.726s] DEBUG:colcon:Command line arguments: ['/usr/bin/colcon', 'build', '--symlink-install'] +[0.726s] DEBUG:colcon:Parsed command line arguments: Namespace(log_base=None, log_level=None, verb_name='build', build_base='build', install_base='install', merge_install=False, symlink_install=True, test_result_base=None, continue_on_error=False, executor='parallel', parallel_workers=20, event_handlers=None, ignore_user_meta=False, metas=['./colcon.meta'], base_paths=['.'], packages_ignore=None, packages_ignore_regex=None, paths=None, packages_up_to=None, packages_up_to_regex=None, packages_above=None, packages_above_and_dependencies=None, packages_above_depth=None, packages_select_by_dep=None, packages_skip_by_dep=None, packages_skip_up_to=None, packages_select_build_failed=False, packages_skip_build_finished=False, packages_select_test_failures=False, packages_skip_test_passed=False, packages_select=None, packages_skip=None, packages_select_regex=None, packages_skip_regex=None, packages_start=None, packages_end=None, allow_overriding=[], cmake_args=None, cmake_target=None, cmake_target_skip_unavailable=False, cmake_clean_cache=False, cmake_clean_first=False, cmake_force_configure=False, ament_cmake_args=None, catkin_cmake_args=None, catkin_skip_building_tests=False, verb_parser=, verb_extension=, main=>) +[0.881s] Level 1:colcon.colcon_core.package_discovery:discover_packages(colcon_meta) check parameters +[0.882s] Level 1:colcon.colcon_core.package_discovery:discover_packages(recursive) check parameters +[0.882s] Level 1:colcon.colcon_core.package_discovery:discover_packages(ignore) check parameters +[0.882s] Level 1:colcon.colcon_core.package_discovery:discover_packages(path) check parameters +[0.882s] Level 1:colcon.colcon_core.package_discovery:discover_packages(colcon_meta) discover +[0.882s] Level 1:colcon.colcon_core.package_discovery:discover_packages(recursive) discover +[0.882s] INFO:colcon.colcon_core.package_discovery:Crawling recursively for packages in '/home/user/dev_ws/src/core/dynosam_utils/src' +[0.882s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extensions ['ignore', 'ignore_ament_install'] +[0.883s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extension 'ignore' +[0.883s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extension 'ignore_ament_install' +[0.883s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extensions ['colcon_pkg'] +[0.883s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extension 'colcon_pkg' +[0.883s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extensions ['colcon_meta'] +[0.883s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extension 'colcon_meta' +[0.883s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extensions ['ros'] +[0.884s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extension 'ros' +[0.941s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extensions ['cmake', 'python'] +[0.941s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extension 'cmake' +[0.941s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extension 'python' +[0.941s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extensions ['python_setup_py'] +[0.941s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extension 'python_setup_py' +[0.942s] Level 1:colcon.colcon_core.package_identification:_identify(build) by extensions ['ignore', 'ignore_ament_install'] +[0.942s] Level 1:colcon.colcon_core.package_identification:_identify(build) by extension 'ignore' +[0.942s] Level 1:colcon.colcon_core.package_identification:_identify(build) ignored +[0.942s] Level 1:colcon.colcon_core.package_identification:_identify(install) by extensions ['ignore', 'ignore_ament_install'] +[0.942s] Level 1:colcon.colcon_core.package_identification:_identify(install) by extension 'ignore' +[0.942s] Level 1:colcon.colcon_core.package_identification:_identify(install) ignored +[0.942s] Level 1:colcon.colcon_core.package_identification:_identify(log) by extensions ['ignore', 'ignore_ament_install'] +[0.942s] Level 1:colcon.colcon_core.package_identification:_identify(log) by extension 'ignore' +[0.942s] Level 1:colcon.colcon_core.package_identification:_identify(log) ignored +[0.942s] Level 1:colcon.colcon_core.package_discovery:discover_packages(recursive) using defaults +[0.942s] Level 1:colcon.colcon_core.package_discovery:discover_packages(ignore) discover +[0.942s] Level 1:colcon.colcon_core.package_discovery:discover_packages(ignore) using defaults +[0.942s] Level 1:colcon.colcon_core.package_discovery:discover_packages(path) discover +[0.942s] Level 1:colcon.colcon_core.package_discovery:discover_packages(path) using defaults +[0.990s] Level 1:colcon.colcon_core.package_discovery:discover_packages(prefix_path) check parameters +[0.990s] Level 1:colcon.colcon_core.package_discovery:discover_packages(prefix_path) discover +[1.029s] DEBUG:colcon.colcon_installed_package_information.package_discovery:Found 10 installed packages in /home/user/dev_ws/install +[1.038s] DEBUG:colcon.colcon_installed_package_information.package_discovery:Found 345 installed packages in /opt/ros/kilted +[1.039s] Level 1:colcon.colcon_core.package_discovery:discover_packages(prefix_path) using defaults +[1.335s] INFO:colcon.colcon_core.executor:Executing jobs using 'parallel' executor +[1.336s] DEBUG:colcon.colcon_parallel_executor.executor.parallel:run_until_complete +[1.337s] DEBUG:colcon.colcon_parallel_executor.executor.parallel:closing loop +[1.337s] DEBUG:colcon.colcon_parallel_executor.executor.parallel:loop closed +[1.337s] DEBUG:colcon.colcon_parallel_executor.executor.parallel:run_until_complete finished with '0' +[1.337s] DEBUG:colcon.colcon_core.event_reactor:joining thread +[1.377s] INFO:colcon.colcon_core.plugin_system:Skipping extension 'colcon_notification.desktop_notification.notify_send': Could not find 'notify-send' +[1.377s] INFO:colcon.colcon_core.plugin_system:Skipping extension 'colcon_notification.desktop_notification.terminal_notifier': Not used on non-Darwin systems +[1.377s] INFO:colcon.colcon_core.plugin_system:Skipping extension 'colcon_notification.desktop_notification.win32': Not used on non-Windows systems +[1.377s] INFO:colcon.colcon_notification.desktop_notification:Sending desktop notification using 'notify2' +[1.383s] DEBUG:colcon.colcon_notification.desktop_notification.notify2:Failed to initialize notify2: org.freedesktop.DBus.Error.Spawn.ExecFailed: /usr/bin/dbus-launch terminated abnormally without any error message +[1.384s] DEBUG:colcon.colcon_core.event_reactor:joined thread +[1.397s] INFO:colcon.colcon_core.plugin_system:Skipping extension 'colcon_core.shell.bat': Not used on non-Windows systems +[1.398s] INFO:colcon.colcon_core.shell:Creating prefix script '/home/user/dev_ws/src/core/dynosam_utils/src/install/local_setup.ps1' +[1.401s] INFO:colcon.colcon_core.shell:Creating prefix util module '/home/user/dev_ws/src/core/dynosam_utils/src/install/_local_setup_util_ps1.py' +[1.404s] INFO:colcon.colcon_core.shell:Creating prefix chain script '/home/user/dev_ws/src/core/dynosam_utils/src/install/setup.ps1' +[1.407s] INFO:colcon.colcon_core.shell:Creating prefix script '/home/user/dev_ws/src/core/dynosam_utils/src/install/local_setup.sh' +[1.408s] INFO:colcon.colcon_core.shell:Creating prefix util module '/home/user/dev_ws/src/core/dynosam_utils/src/install/_local_setup_util_sh.py' +[1.409s] INFO:colcon.colcon_core.shell:Creating prefix chain script '/home/user/dev_ws/src/core/dynosam_utils/src/install/setup.sh' +[1.412s] INFO:colcon.colcon_core.shell:Creating prefix script '/home/user/dev_ws/src/core/dynosam_utils/src/install/local_setup.bash' +[1.415s] INFO:colcon.colcon_core.shell:Creating prefix chain script '/home/user/dev_ws/src/core/dynosam_utils/src/install/setup.bash' +[1.417s] INFO:colcon.colcon_core.shell:Creating prefix script '/home/user/dev_ws/src/core/dynosam_utils/src/install/local_setup.zsh' +[1.420s] INFO:colcon.colcon_core.shell:Creating prefix chain script '/home/user/dev_ws/src/core/dynosam_utils/src/install/setup.zsh' diff --git a/dynosam_utils/src/log/latest b/dynosam_utils/src/log/latest new file mode 120000 index 000000000..b57d247c7 --- /dev/null +++ b/dynosam_utils/src/log/latest @@ -0,0 +1 @@ +latest_build \ No newline at end of file diff --git a/dynosam_utils/src/log/latest_build b/dynosam_utils/src/log/latest_build new file mode 120000 index 000000000..d799f4781 --- /dev/null +++ b/dynosam_utils/src/log/latest_build @@ -0,0 +1 @@ +build_2026-01-28_00-38-18 \ No newline at end of file diff --git a/dynosam_utils/src/run_experiments_ecmr.py b/dynosam_utils/src/run_experiments_ecmr.py old mode 100644 new mode 100755 index d569a1a93..4b4e77651 --- a/dynosam_utils/src/run_experiments_ecmr.py +++ b/dynosam_utils/src/run_experiments_ecmr.py @@ -154,13 +154,13 @@ def run_viodes(): # run_ecmr_experiment_sequences("/root/data/VIODE/city_day/mid", "viode_city_day_mid", viode, "--v=100") # run_ecmr_experiment_sequences("/root/data/VIODE/city_day/high","viode_city_day_high", viode, "--ending_frame=1110") - run_ecmr_experiment_sequences("/root/data/VIODE/city_day/high","test_viode", viode,"--starting_frame=0", "--ending_frame=1110", "--v=30", "--use_backend=true") + # run_ecmr_experiment_sequences("/root/data/VIODE/city_day/high","test_viode", viode,"--starting_frame=0", "--ending_frame=1110", "--v=30", "--use_backend=true") # # zero_elements_ratio # run_ecmr_experiment_sequences("/root/data/VIODE/city_night/mid", "viode_city_night_mid", viode) # run_ecmr_experiment_sequences("/root/data/VIODE/city_night/high", "viode_city_night_high", viode) # run_ecmr_experiment_sequences("/root/data/VIODE/parking_lot/mid", "parking_lot_night_mid", viode) - # run_ecmr_experiment_sequences("/root/data/VIODE/parking_lot/high", "parking_lot_night_high", viode) + run_ecmr_experiment_sequences("/root/data/viode_parking_lot/high", "parking_lot_night_high", viode) def run_omd(): run_ecmr_experiment_sequences("/root/data/vdo_slam/omd/omd/swinging_4_unconstrained_stereo/","test", omd_dataset, "--ending_frame=300") @@ -172,7 +172,7 @@ def run_tartan_air(): run_ecmr_experiment_sequences("/root/data/TartanAir_shibuya/RoadCrossing05", "test_tartan", tartan_air) # run_ecmr_experiment_sequences("/root/data/TartanAir_shibuya/RoadCrossing06", "tas_rc6", tartan_air) # run_ecmr_experiment_sequences("/root/data/TartanAir_shibuya/RoadCrossing07", "tas_rc7", tartan_air, "--starting_frame=5", "--ending_frame=65") - # run_analysis("tas_rc7") + # run_analysis("tas_rc7") # run_ecmr_experiment_sequences("/root/data/TartanAir_shibuya/Standing01", "tas_s1", tartan_air) # run_ecmr_experiment_sequences("/root/data/TartanAir_shibuya/Standing02", "tas_s2", tartan_air) @@ -191,8 +191,8 @@ def run_kitti(): # run_ecmr_experiment_sequences("/root/data/vdo_slam/kitti/kitti/0005/", "kitti_0005_static_only", kitti_dataset, "--shrink_row=25", "--shrink_col=50") # run_ecmr_experiment_sequences("/root/data/vdo_slam/kitti/kitti/0006/", "kitti_0006_static_only", kitti_dataset, "--shrink_row=25", "--shrink_col=50") # run_ecmr_experiment_sequences("/root/data/vdo_slam/kitti/kitti/0018/", "kitti_0018_static_only", kitti_dataset, "--shrink_row=25", "--shrink_col=50") - run_ecmr_experiment_sequences("/root/data/vdo_slam/kitti/kitti/0020/", "kitti_0020_static_only", kitti_dataset, "--shrink_row=25", "--shrink_col=50") - # run_ecmr_experiment_sequences("/root/data/vdo_slam/kitti/kitti/0004/", "test", kitti_dataset, "--shrink_row=25", "--shrink_col=50", "--use_backend=true") + # run_ecmr_experiment_sequences("/root/data/vdo_slam/kitti/kitti/0020/", "kitti_0020_static_only", kitti_dataset, "--shrink_row=25", "--shrink_col=50") + run_ecmr_experiment_sequences("/root/data/kitti_0000/", "test", kitti_dataset, "--shrink_row=25", "--shrink_col=50", "--use_backend=true") def run_aria(): @@ -209,9 +209,9 @@ def run_aria(): # run_viodes() # run_tartan_air() # run_cluster() - run_omd() + # run_omd() # run_aria() - # run_kitti() + run_kitti() # run_analysis("kitti_0001_static_only") # run_analysis("kitti_0002_static_only") # run_analysis("kitti_0003_static_only") @@ -498,4 +498,4 @@ def run_aria(): # "kitti_0003", # backend_type=object_centric_batch # ) - # run_analysis("kitti_0003") + # run_analysis("kitti_0003") \ No newline at end of file