Added Velocities to Odometry Message and Class Label Passthrough

README.md

@@ -29,15 +29,19 @@
 
 ## 📚 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).  Accepted Transactions on Robotics (T-RO) Visual SLAM Special Issue (2025).
+- [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).  Accepted Transactions on Robotics (T-RO) Visual SLAM Special Issue, 2025.
+
+
+#### ** Update December 2025 **
+Our work has been accepted to IEEE Robotics and Automation Letters (RA-L)
 
 #### ** Update November 2025 **
 Our work has been accepted to IEEE Transactions on Robotics (T-RO)
 
 
 #### ** Update September 2025 **
 This code now also contains the code for our new work
-- J.Morris, Y. Wang, V. Ila. [*Online Dynamic SLAM with Incremental Smoothing and Mapping*](https://www.arxiv.org/abs/2509.08197), Arxiv. Submitted RA-L (2025)
+- J.Morris, Y. Wang, V. Ila. [*Online Dynamic SLAM with Incremental Smoothing and Mapping*](https://www.arxiv.org/abs/2509.08197), Accepted Robotics and Automation Letters (RA-L), 2025
 
 
 We kindly ask to cite our papers if you find these works useful:
@@ -53,11 +57,15 @@ We kindly ask to cite our papers if you find these works useful:
   doi={10.1109/TRO.2025.3641813}
 }
 
-@article{morris2025online,
-  title={Online Dynamic SLAM with Incremental Smoothing and Mapping},
+@inproceedings{morris2025online,
   author={Morris, Jesse and Wang, Yiduo and Ila, Viorela},
-  journal={arXiv preprint arXiv:2509.08197},
-  year={2025}
+  journal={IEEE Robotics and Automation Letters},
+  title={Online Dynamic SLAM with Incremental Smoothing and Mapping},
+  year={2026},
+  volume={},
+  number={},
+  pages={1-8},
+  doi={10.1109/LRA.2026.3655286}}
 }
 ```
 
@@ -92,7 +100,7 @@ We currently support building for AARM64 devices and DynoSAM has been tested on
 
 > NOTE: DynoSAM does not currently run real-time on the ORIN NX (mostly bottlenecked by the object detection process). On a more powerful device better performance is expected.
 
-Also see the [Docker REAMDE.md](./docker/README.md) and the [dynosam_nn README.md](./dynosam_nn//README.md) for more information on hardware and performance.
+Also see the [Docker README.md](./docker/README.md) and the [dynosam_nn README.md](./dynosam_nn//README.md) for more information on hardware and performance.
 
 # 2. 🚀 Running DynoSAM
 

docker/Dockerfile.amd64

@@ -30,7 +30,7 @@ RUN apt-get update \
     && rm -rf /var/lib/apt/lists/*
 
 # ROS installs
-RUN apt-get install -y \
+RUN apt-get update && apt-get install -y \
       ros-kilted-ros2cli-common-extensions \
       ros-kilted-vision-opencv \
       nlohmann-json3-dev

docker/README.md

@@ -2,10 +2,11 @@
 
 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
+- `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
@@ -18,9 +19,12 @@ 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
+```txt
 matplotlib=3.6.3
-numpy=1.26.4
+numpy=1.26.4
+```

docs/media/INSTALL.md

@@ -29,9 +29,9 @@ To install natively, install the dependancies as required by docker and build as
 
 ## Docker Install instructions
 
-DynoSAM has been tested on __x86_64__ and __aarm64__ (with a NVIDIA ORIN NX) devices using the [two docker files](../../docker/) provided. See the [REAMDE.md](../../docker/README.md) for more detail on hardware used etc.
+DynoSAM has been tested on __x86_64__ and __aarm64__ (with a NVIDIA ORIN NX) devices using the [two docker files](../../docker/) provided. See the [README.md](../../docker/README.md) for more detail on hardware used etc.
 
-We provide scripts to build and create docker containers to run and develop DynoSAM which is intendend to be mounted within the created container.
+We provide scripts to build and create docker containers to run and develop DynoSAM which is intended to be mounted within the created container.
 
 > NOTE: with the current setup the embedded device only supports ROS Jazzy. The code should compile on either device without modification.
 

dynosam/include/dynosam/frontend/FrontendModule.hpp

@@ -39,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"
 
@@ -84,6 +86,8 @@ class FrontendModule
   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

dynosam/include/dynosam/frontend/RGBDInstanceFrontendModule.hpp

@@ -126,6 +126,13 @@ class RGBDInstanceFrontendModule : public FrontendModule {
 
   //! Last keyframe
   Frame::Ptr frame_lkf_;
+
+  // Previous packet
+  VisionImuPacket::Ptr previous_vision_imu_packet_;
+
+
 };
 
+
+
 }  // namespace dyno

dynosam/include/dynosam/frontend/VisionImuOutputPacket.hpp

@@ -80,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.
    *
@@ -99,6 +99,8 @@ 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;
 
@@ -107,6 +109,7 @@ class VisionImuPacket {
 
   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)
@@ -149,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_;
@@ -196,6 +200,11 @@ class VisionImuPacket {
   static void fillLandmarkMeasurements(
       StatusLandmarkVector& landmarks,
       const CameraMeasurementStatusVector& camera_measurements);
+
+
+ private:
+  std::optional<gtsam::Vector6> stored_body_velocity;
+
 };
 
 }  // namespace dyno

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"
@@ -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);
 

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

dynosam/src/frontend/RGBDInstanceFrontendModule.cc

@@ -116,6 +116,8 @@ FrontendModule::SpinReturn RGBDInstanceFrontendModule::boostrapSpin(
 
   Frame::Ptr frame = tracker_->track(input->getFrameId(), input->getTimestamp(),
                                      *image_container);
+
+
   CHECK(frame->updateDepths());
 
   return {State::Nominal, nullptr};
@@ -148,6 +150,7 @@ FrontendModule::SpinReturn RGBDInstanceFrontendModule::nominalSpin(
 
   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);
@@ -231,6 +234,13 @@ 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();
   }

dynosam/src/frontend/VisionImuOutputPacket.cc

@@ -40,6 +40,8 @@ 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_;
 }
@@ -213,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 {