[Draft] adding landmark and imu support to RTABMapSLAM

.github/workflows/python-main.yml

@@ -182,6 +182,7 @@ jobs:
   build-windows-x86_64:
     needs: build-docstrings
     runs-on: windows-latest
+    timeout-minutes: 1440  # Set timeout to 24 hours
     strategy:
       matrix:
         python-version: [3.7, 3.8, 3.9, '3.10', '3.11', '3.12', '3.13']
@@ -336,13 +337,15 @@ jobs:
   build-linux-x86_64:
     needs: build-docstrings
     runs-on: ubuntu-latest
+    timeout-minutes: 1440  # Set timeout to 24 hours
     container:
       image: quay.io/pypa/manylinux_2_28_x86_64:2024.12.05-1
       env:
         PLAT: manylinux_2_28_x86_64
     strategy:
       matrix:
         python-set: ["cp37-cp37m", "cp38-cp38", "cp39-cp39", "cp310-cp310", "cp311-cp311", "cp312-cp312", "cp313-cp313"]
+      fail-fast: false
     env:
       DEPTHAI_BUILD_BASALT: ON
       DEPTHAI_BUILD_PCL: ON
@@ -419,6 +422,7 @@ jobs:
     strategy:
       matrix:
         python-set: ["cp37-cp37m", "cp38-cp38", "cp39-cp39", "cp310-cp310", "cp311-cp311", "cp312-cp312", "cp313-cp313"]
+      fail-fast: false
     env:
       # workaround required for cache@v3, https://github.com/actions/cache/issues/1428
       VCPKG_FORCE_SYSTEM_BINARIES: "1" # Needed so vpckg can bootstrap itself
@@ -443,7 +447,7 @@ jobs:
         with:
           submodules: 'recursive'
       - name: Installing libusb1-devel dependency
-        run: yum install -y libusb1-devel perl-core curl zip unzip tar zlib-devel curl-devel libxcb-devel xcb-util-renderutil-devel xcb-util-devel xcb-util-image-devel xcb-util-keysyms-devel xcb-util-wm-devel mesa-libGL-devel libxkbcommon-devel libxkbcommon-x11-devel libXi-devel libXrandr-devel libXtst-devel libudev-devel lapack-devel nasm libtool autoconf automake libX11-devel pkgconfig 
+        run: yum install -y libusb1-devel perl-core curl zip unzip tar zlib-devel curl-devel libxcb-devel xcb-util-renderutil-devel xcb-util-devel xcb-util-image-devel xcb-util-keysyms-devel xcb-util-wm-devel mesa-libGL-devel libxkbcommon-devel libxkbcommon-x11-devel libXi-devel libXrandr-devel libXtst-devel libudev-devel lapack-devel nasm libtool autoconf automake libX11-devel pkgconfig
       - name: Setup ninja required for arm64 builds
         run: |
           git clone https://github.com/ninja-build/ninja.git

.gitignore

@@ -8,6 +8,7 @@ build*/*
 shared/version.hpp
 
 #pybind11
+bindings/python/docstrings/
 build/
 dist/
 _build/

CMakeLists.txt

@@ -303,6 +303,7 @@ set(TARGET_CORE_SOURCES
     src/pipeline/datatype/SpatialLocationCalculatorConfig.cpp
     src/pipeline/datatype/AprilTags.cpp
     src/pipeline/datatype/AprilTagConfig.cpp
+    src/pipeline/datatype/Landmarks.cpp
     src/pipeline/datatype/Tracklets.cpp
     src/pipeline/datatype/IMUData.cpp
     src/pipeline/datatype/StereoDepthConfig.cpp
@@ -373,6 +374,7 @@ set(TARGET_OPENCV_SOURCES
     src/pipeline/node/host/Replay.cpp
     src/opencv/RecordReplay.cpp
     src/opencv/HolisticRecordReplay.cpp
+    src/opencv/Landmarks.cpp
 )
 
 set(TARGET_PCL_SOURCES src/pcl/PointCloudData.cpp)

bindings/js/bindings.cpp

@@ -68,6 +68,7 @@ EMSCRIPTEN_BINDINGS(depthai_js) {
         .value("EdgeDetectorConfig", dai::DatatypeEnum::EdgeDetectorConfig)
         .value("AprilTagConfig", dai::DatatypeEnum::AprilTagConfig)
         .value("AprilTags", dai::DatatypeEnum::AprilTags)
+        .value("Landmarks", dai::DatatypeEnum::Landmarks)
         .value("Tracklets", dai::DatatypeEnum::Tracklets)
         .value("IMUData", dai::DatatypeEnum::IMUData)
         .value("StereoDepthConfig", dai::DatatypeEnum::StereoDepthConfig)

bindings/python/CMakeLists.txt

@@ -109,6 +109,7 @@ set(SOURCE_LIST
     src/pipeline/datatype/ADatatypeBindings.cpp
     src/pipeline/datatype/AprilTagConfigBindings.cpp
     src/pipeline/datatype/AprilTagsBindings.cpp
+    src/pipeline/datatype/LandmarksBindings.cpp
     src/pipeline/datatype/BufferBindings.cpp
     src/pipeline/datatype/CameraControlBindings.cpp
     src/pipeline/datatype/EdgeDetectorConfigBindings.cpp

bindings/python/src/DatatypeBindings.cpp

@@ -31,6 +31,7 @@ void bind_benchmarkreport(pybind11::module& m, void* pCallstack);
 void bind_pointcloudconfig(pybind11::module& m, void* pCallstack);
 void bind_pointclouddata(pybind11::module& m, void* pCallstack);
 void bind_transformdata(pybind11::module& m, void* pCallstack);
+void bind_landmarks(pybind11::module& m, void* pCallstack);
 void bind_rgbddata(pybind11::module& m, void* pCallstack);
 void bind_imagealignconfig(pybind11::module& m, void* pCallstack);
 void bind_imageannotations(pybind11::module& m, void* pCallstack);
@@ -68,6 +69,7 @@ void DatatypeBindings::addToCallstack(std::deque<StackFunction>& callstack) {
     callstack.push_front(bind_pointcloudconfig);
     callstack.push_front(bind_pointclouddata);
     callstack.push_front(bind_transformdata);
+    callstack.push_front(bind_landmarks);
     callstack.push_front(bind_imagealignconfig);
     callstack.push_front(bind_imageannotations);
     callstack.push_front(bind_rgbddata);
@@ -119,6 +121,7 @@ void DatatypeBindings::bind(pybind11::module& m, void* pCallstack) {
         .value("BenchmarkReport", DatatypeEnum::BenchmarkReport)
         .value("MessageGroup", DatatypeEnum::MessageGroup)
         .value("TransformData", DatatypeEnum::TransformData)
+        .value("Landmarks", DatatypeEnum::Landmarks)
         .value("PointCloudConfig", DatatypeEnum::PointCloudConfig)
         .value("PointCloudData", DatatypeEnum::PointCloudData)
         .value("ImageAlignConfig", DatatypeEnum::ImageAlignConfig)

bindings/python/src/pipeline/datatype/LandmarksBindings.cpp

@@ -0,0 +1,54 @@
+#include "DatatypeBindings.hpp"
+#include "pipeline/CommonBindings.hpp"
+
+// depthai
+#include "depthai/pipeline/datatype/Landmarks.hpp"
+
+//pybind
+#include <pybind11/chrono.h>
+#include <pybind11/numpy.h>
+
+
+
+void bind_landmarks(pybind11::module& m, void* pCallstack) {
+   using namespace dai;
+
+   py::class_<Landmark> landmark(m, "Landmark", DOC(dai, Landmark));
+   py::class_<Landmarks, Py<Landmarks>, Buffer, std::shared_ptr<Landmarks>> landmarks(m, "Landmarks", DOC(dai, Landmarks));
+
+   ///////////////////////////////////////////////////////////////////////
+   ///////////////////////////////////////////////////////////////////////
+   ///////////////////////////////////////////////////////////////////////
+   // Call the rest of the type defines, then perform the actual bindings
+   Callstack* callstack = (Callstack*)pCallstack;
+   auto cb = callstack->top();
+   callstack->pop();
+   cb(m, pCallstack);
+   // Actual bindings
+   ///////////////////////////////////////////////////////////////////////
+   ///////////////////////////////////////////////////////////////////////
+   ///////////////////////////////////////////////////////////////////////
+
+   landmark.def(py::init<>())
+      .def_readwrite("id", &Landmark::id, DOC(dai, Landmark, id))
+      .def_readwrite("size", &Landmark::size, DOC(dai, Landmark, size))
+      .def_readwrite("translation", &Landmark::translation, DOC(dai, Landmark, translation))
+      .def_readwrite("quaternion", &Landmark::quaternion, DOC(dai, Landmark, quaternion))
+      .def_readwrite("covariance", &Landmark::covariance, DOC(dai, Landmark, covariance));
+
+
+   // Message
+   landmarks.def(py::init<>())
+       .def("__repr__", &Landmarks::str)
+       .def_property(
+           "landmarks", [](Landmarks& det) { return &det.landmarks; }, [](Landmarks& det, std::vector<Landmark> val) { det.landmarks = val; })
+       .def("getTimestamp", &Landmarks::Buffer::getTimestamp, DOC(dai, Buffer, getTimestamp))
+       .def("getTimestampDevice", &Landmarks::Buffer::getTimestampDevice, DOC(dai, Buffer, getTimestampDevice))
+       .def("getSequenceNum", &Landmarks::Buffer::getSequenceNum, DOC(dai, Buffer, getSequenceNum))
+       // .def("setTimestamp", &AprilTags::setTimestamp, DOC(dai, Buffer, setTimestamp))
+       // .def("setTimestampDevice", &AprilTags::setTimestampDevice, DOC(dai, Buffer, setTimestampDevice))
+       // .def("setSequenceNum", &AprilTags::setSequenceNum, DOC(dai, Buffer, setSequenceNum))
+       ;
+
+
+}

bindings/python/src/pipeline/node/RTABMapSLAMBindings.cpp

@@ -32,8 +32,14 @@ void bind_rtabmapslamnode(pybind11::module& m, void* pCallstack) {
                        "rect", [](RTABMapSLAM& node) { return &node.rect; }, py::return_value_policy::reference_internal)
         .def_property_readonly(
             "depth", [](RTABMapSLAM& node) { return &node.depth; }, py::return_value_policy::reference_internal)
-        .def_readonly("features", &RTABMapSLAM::features, DOC(dai, node, RTABMapSLAM, features))
+        .def_property_readonly(
+            "features", [](RTABMapSLAM& node) { return &node.features; }, py::return_value_policy::reference_internal)
+        .def_property_readonly(
+            "landmarks", [](RTABMapSLAM& node) { return &node.landmarks; }, py::return_value_policy::reference_internal)
+        // .def_readonly("features", &RTABMapSLAM::features, DOC(dai, node, RTABMapSLAM, features))
+        // .def_readonly("landmarks", &RTABMapSLAM::landmarks, DOC(dai, node, RTABMapSLAM, landmarks))
         .def_readonly("odom", &RTABMapSLAM::odom, DOC(dai, node, RTABMapSLAM, odom))
+        .def_readonly("imu", &RTABMapSLAM::imu, DOC(dai, node, RTABMapSLAM, imu))
         .def_readonly("transform", &RTABMapSLAM::transform, DOC(dai, node, RTABMapSLAM, transform))
         .def_readonly("odomCorrection", &RTABMapSLAM::odomCorrection, DOC(dai, node, RTABMapSLAM, odomCorrection))
         .def_readonly("obstaclePCL", &RTABMapSLAM::obstaclePCL, DOC(dai, node, RTABMapSLAM, obstaclePCL))
@@ -42,6 +48,7 @@ void bind_rtabmapslamnode(pybind11::module& m, void* pCallstack) {
         .def_readonly("passthroughRect", &RTABMapSLAM::passthroughRect, DOC(dai, node, RTABMapSLAM, passthroughRect))
         .def_readonly("passthroughDepth", &RTABMapSLAM::passthroughDepth, DOC(dai, node, RTABMapSLAM, passthroughDepth))
         .def_readonly("passthroughFeatures", &RTABMapSLAM::passthroughFeatures, DOC(dai, node, RTABMapSLAM, passthroughFeatures))
+        .def_readonly("passthroughLandmarks", &RTABMapSLAM::passthroughLandmarks, DOC(dai, node, RTABMapSLAM, passthroughLandmarks))
         .def_readonly("passthroughOdom", &RTABMapSLAM::passthroughOdom, DOC(dai, node, RTABMapSLAM, passthroughOdom))
         .def("setParams", &RTABMapSLAM::setParams, py::arg("params"), DOC(dai, node, RTABMapSLAM, setParams))
         .def("setDatabasePath", &RTABMapSLAM::setDatabasePath, py::arg("path"), DOC(dai, node, RTABMapSLAM, setDatabasePath))
@@ -57,7 +64,8 @@ void bind_rtabmapslamnode(pybind11::module& m, void* pCallstack) {
         .def("setFreq", &RTABMapSLAM::setFreq, py::arg("f"), DOC(dai, node, RTABMapSLAM, setFreq))
         .def("setAlphaScaling", &RTABMapSLAM::setAlphaScaling, py::arg("alpha"), DOC(dai, node, RTABMapSLAM, setAlphaScaling))
         .def("setUseFeatures", &RTABMapSLAM::setUseFeatures, py::arg("useFeatures"), DOC(dai, node, RTABMapSLAM, setUseFeatures))
+        .def("setUseLandmarks", &RTABMapSLAM::setUseLandmarks, py::arg("useLandmarks"), DOC(dai, node, RTABMapSLAM, setUseLandmarks))
         .def("setLocalTransform", &RTABMapSLAM::setLocalTransform, py::arg("transform"), DOC(dai, node, RTABMapSLAM, setLocalTransform))
         .def("getLocalTransform", &RTABMapSLAM::getLocalTransform, DOC(dai, node, RTABMapSLAM, getLocalTransform))
         .def("triggerNewMap", &RTABMapSLAM::triggerNewMap, DOC(dai, node, RTABMapSLAM, triggerNewMap));
-}
+}

include/depthai/pipeline/datatype/DatatypeEnum.hpp

@@ -31,12 +31,13 @@ enum class DatatypeEnum : std::int32_t {
     BenchmarkReport,
     MessageGroup,
     TransformData,
+    Landmarks,
     PointCloudConfig,
     PointCloudData,
     RGBDData,
     ImageAlignConfig,
     ImgAnnotations,
-    ObjectTrackerConfig
+    ObjectTrackerConfig,
 };
 bool isDatatypeSubclassOf(DatatypeEnum parent, DatatypeEnum children);
 

include/depthai/pipeline/datatype/Landmarks.hpp

@@ -0,0 +1,69 @@
+#pragma once
+
+#include <array>
+#include <vector>
+
+#include "depthai/common/Point3d.hpp"
+#include "depthai/common/Quaterniond.hpp"
+#include "depthai/pipeline/datatype/Buffer.hpp"
+
+// optional
+#ifdef DEPTHAI_HAVE_OPENCV_SUPPORT
+    #include <opencv2/core/mat.hpp>
+    #include <opencv2/opencv.hpp>
+#endif
+
+
+
+// #include "utility/Serialization.hpp"
+
+namespace dai {
+struct Landmark {
+    /**
+     * The ID of the Landmark
+     */
+    int id;
+
+    /**
+     * The size of the landmark in meters
+     */
+    double size;
+
+    /**
+     * The translation of the landmark reletive to base_frame (the camera)
+     */
+    dai::Point3d translation;
+
+    /**
+     * The orientation of the landmark relative to base_frame (the camera)
+     */
+    dai::Quaterniond quaternion;
+
+    /**
+     * The covariance of the landmark. If you do not know what this means, you can probably just fill with 0.01.
+     */
+    std::array<std::array<double, 6>, 6> covariance;
+
+    #ifdef DEPTHAI_HAVE_OPENCV_SUPPORT
+    cv::Mat getCovarianceAsCvMat();
+    #endif
+};
+
+DEPTHAI_SERIALIZE_EXT(Landmark, id, size, translation, quaternion, covariance);
+
+class Landmarks : public Buffer {
+   public:
+    Landmarks() = default;
+    virtual ~Landmarks() = default;
+
+   public:
+    std::vector<Landmark> landmarks;
+
+    DEPTHAI_SERIALIZE(Landmarks, Buffer::sequenceNum, Buffer::ts, Buffer::tsDevice, landmarks);
+    void serialize(std::vector<std::uint8_t>& metadata, DatatypeEnum& datatype) const override {
+        metadata = utility::serialize(*this);
+        datatype = DatatypeEnum::Landmarks;
+    }
+};
+
+}  // namespace dai

include/depthai/rtabmap/RTABMapSLAM.hpp

@@ -37,6 +37,7 @@ class RTABMapSLAM : public dai::NodeCRTP<dai::node::ThreadedHostNode, RTABMapSLA
     std::string rectInputName = "rect";
     std::string depthInputName = "depth";
     std::string featuresInputName = "features";
+    std::string landmarksInputName = "landmarks";
 
     /**
      * Input rectified image on which SLAM is performed.
@@ -46,14 +47,26 @@ class RTABMapSLAM : public dai::NodeCRTP<dai::node::ThreadedHostNode, RTABMapSLA
      * Input depth image on which SLAM is performed.
      */
     Input& depth = inputs[depthInputName];
+
+    Input& landmarks = inputs[landmarksInputName];
+
+    Input& features = inputs[featuresInputName];
     /**
      * Input tracked features on which SLAM is performed (optional).
      */
-    Input features{*this, {featuresInputName, DEFAULT_GROUP, DEFAULT_BLOCKING, 15, {{{DatatypeEnum::TrackedFeatures, true}}}}};
+    // Input features{*this, {featuresInputName, DEFAULT_GROUP, DEFAULT_BLOCKING, 15, {{{DatatypeEnum::TrackedFeatures, true}}}}};
     /**
      * Input odometry pose.
      */
     Input odom{*this, {"odom", DEFAULT_GROUP, DEFAULT_BLOCKING, 15, {{{dai::DatatypeEnum::TransformData, true}}}}};
+    /**
+     * Input imu
+     */
+    Input imu{*this, {"imu", DEFAULT_GROUP, DEFAULT_BLOCKING, 15, {{DatatypeEnum::IMUData, true}}}};
+    /**
+     * Input Landmark poses (optional).
+     */
+    // Input landmarks{*this, {landmarksInputName, DEFAULT_GROUP, DEFAULT_BLOCKING, 15, {{{dai::DatatypeEnum::Landmarks, true}}}}};
 
     /**
      * Output transform.
@@ -88,6 +101,10 @@ class RTABMapSLAM : public dai::NodeCRTP<dai::node::ThreadedHostNode, RTABMapSLA
      * Output passthrough features.
      */
     Output passthroughFeatures{*this, {"passthroughFeatures", DEFAULT_GROUP, {{{dai::DatatypeEnum::TrackedFeatures, true}}}}};
+    /**
+     * Output passthrough landmarks.
+     */
+    Output passthroughLandmarks{*this, {"passthroughLandmarks", DEFAULT_GROUP, {{{dai::DatatypeEnum::Landmarks, true}}}}};
     /**
      * Output passthrough odometry pose.
      */
@@ -166,6 +183,11 @@ class RTABMapSLAM : public dai::NodeCRTP<dai::node::ThreadedHostNode, RTABMapSLA
      * Whether to use input features for SLAM. False by default.
      */
     void setUseFeatures(bool use);
+    /**
+     * WhWhether to use input landmarks for SLAM. False by default.
+     */
+    void setUseLandmarks(bool use);
+
     void setLocalTransform(std::shared_ptr<TransformData> transform) {
         localTransform = transform->getRTABMapTransform();
     }
@@ -202,6 +224,7 @@ class RTABMapSLAM : public dai::NodeCRTP<dai::node::ThreadedHostNode, RTABMapSLA
     rtabmap::SensorData sensorData;
     float alphaScaling = -1.0;
     bool useFeatures = false;
+    bool useLandmarks = false;
     bool initialized = false;
     std::map<std::string, std::string> rtabParams;
     std::string databasePath = "";

src/basalt/BasaltVIO.cpp

@@ -260,11 +260,11 @@ void BasaltVIO::setDefaultVIOConfig() {
     vioConfig.vio_min_triangulation_dist = 0.05;
     vioConfig.vio_max_iterations = 7;
     vioConfig.vio_enforce_realtime = false;
-    vioConfig.vio_use_lm = true;
+    vioConfig.vio_use_lm = false;
     vioConfig.vio_lm_lambda_initial = 1e-4;
     vioConfig.vio_lm_lambda_min = 1e-6;
     vioConfig.vio_lm_lambda_max = 1e2;
-    vioConfig.vio_scale_jacobian = false;
+    vioConfig.vio_scale_jacobian = true;
     vioConfig.vio_init_pose_weight = 1e8;
     vioConfig.vio_init_ba_weight = 1e1;
     vioConfig.vio_init_bg_weight = 1e2;

src/opencv/Landmarks.cpp

@@ -0,0 +1,17 @@
+#include "depthai/pipeline/datatype/Landmarks.hpp"
+#include "iostream"
+
+namespace dai {
+cv::Mat Landmark::getCovarianceAsCvMat() {
+    //The matrix will always be 6x6 since that is what rtabmap expects.
+    cv::Mat cvMat(6, 6, CV_64F);
+
+    for (int i = 0; i < 6; i++) {
+        for (int j = 0; j < 6; j++) {
+            cvMat.at<double>(i, j) = covariance.at(i).at(j);
+        }
+    }
+
+    return cvMat;
+}
+}