mono inertial support

CMakeLists.txt

@@ -254,6 +254,7 @@ endfunction()
 # ROS2 nodes
 add_ros2_node(ros_rgbd          src/ros_rgbd.cc)
 add_ros2_node(ros_rgbd_inertial src/ros_rgbd_inertial.cc)
+add_ros2_node(ros_mono_inertial src/ros_mono_inertial.cc)
 
 # Install other resources
 install(

README.md

@@ -43,6 +43,7 @@ Once you have installed the required dependencies and configured the parameters,
 
 > 🛎️ Note: The current version of vS-Graphs supports **ROS2 Jazzy** and is primarily tested on Ubuntu 24.04.2 LTS.
 
+> 🛎️ Note: mono-inertial vS-Graphs is currently under development, having a stable version at commit ea5a79819e5f82a9a420cc4cc6b6709197dbf991
 ## 🐋 Docker
 
 For a fully reproducible, environment-independent setup, see the [Docker](/docker) section.

config/Monocular-Inertial/EuRoC.yaml

@@ -42,6 +42,7 @@ IMU.T_b_c1: !!opencv-matrix
          0.0, 0.0, 0.0, 1.0]
 
 # IMU noise
+IMU.Threshold: 0.5      # Default: 0.5
 IMU.NoiseGyro: 1.7e-4 #1.6968e-04
 IMU.NoiseAcc: 2.0000e-3 #2.0e-3
 IMU.GyroWalk: 1.9393e-05 
@@ -81,4 +82,3 @@ Viewer.ViewpointX: 0.0
 Viewer.ViewpointY: -1.7
 Viewer.ViewpointZ: 0.8
 Viewer.ViewpointF: -500.0
-

config/Monocular-Inertial/LuxVSD_RS_D435i_640.yaml

@@ -43,6 +43,7 @@ IMU.T_b_c1: !!opencv-matrix
 IMU.InsertKFsWhenLost: 0
 
 # IMU noise (Use those from VINS-mono)
+IMU.Threshold: 0.5      # Default: 0.5
 IMU.NoiseGyro: 1e-3 # 2.44e-4 #1e-3 # rad/s^0.5
 IMU.NoiseAcc: 1e-2 # 1.47e-3 #1e-2 # m/s^1.5
 IMU.GyroWalk: 1e-6 # rad/s^1.5

config/Monocular-Inertial/RealSense_D435i.yaml

@@ -43,6 +43,7 @@ IMU.T_b_c1: !!opencv-matrix
 IMU.InsertKFsWhenLost: 0
 
 # IMU noise (Use those from VINS-mono)
+IMU.Threshold: 0.5      # Default: 0.5
 IMU.NoiseGyro: 1e-3 # 2.44e-4 #1e-3 # rad/s^0.5
 IMU.NoiseAcc: 1e-2 # 1.47e-3 #1e-2 # m/s^1.5
 IMU.GyroWalk: 1e-6 # rad/s^1.5

config/Monocular-Inertial/RealSense_T265.yaml

@@ -43,6 +43,7 @@ IMU.T_b_c1: !!opencv-matrix
 IMU.InsertKFsWhenLost: 0
 
 # IMU noise (Use those from VINS-mono)
+IMU.Threshold: 0.5      # Default: 0.5
 IMU.NoiseGyro: 0.000005148030141 # rad/s^0.5 
 IMU.NoiseAcc: 0.000066952452471 # m/s^1.5
 IMU.GyroWalk: 0.000000499999999 # rad/s^1.5

config/Monocular-Inertial/TUM-VI.yaml

@@ -41,6 +41,7 @@ IMU.T_b_c1: !!opencv-matrix
           0.0, 0.0, 0.0, 1.0]
 
 # IMU noise (Use those from VINS-mono)
+IMU.Threshold: 0.5      # Default: 0.5
 IMU.NoiseGyro: 0.00016 # rad/s^0.5 
 IMU.NoiseAcc: 0.0028 # m/s^1.5
 IMU.GyroWalk: 0.000022 # rad/s^1.5

config/Monocular-Inertial/TUM-VI_far.yaml

@@ -42,6 +42,7 @@ IMU.T_b_c1: !!opencv-matrix
 
 
 # IMU noise (Use those from VINS-mono)
+IMU.Threshold: 0.5      # Default: 0.5
 IMU.NoiseGyro: 0.00016 # 0.004 (VINS) # 0.00016 (TUM) # 0.00016    # rad/s^0.5 
 IMU.NoiseAcc: 0.0028 # 0.04 (VINS) # 0.0028 (TUM) # 0.0028     # m/s^1.5
 IMU.GyroWalk: 0.000022 # 0.000022 (VINS and TUM) rad/s^1.5

config/Monocular-Inertial/UniLu_RealSense_D435i_640.yaml

@@ -43,6 +43,7 @@ IMU.T_b_c1: !!opencv-matrix
 IMU.InsertKFsWhenLost: 0
 
 # IMU noise (Use those from VINS-mono)
+IMU.Threshold: 0.5      # Default: 0.5
 IMU.NoiseGyro: 1e-3 # 2.44e-4 #1e-3 # rad/s^0.5
 IMU.NoiseAcc: 1e-2 # 1.47e-3 #1e-2 # m/s^1.5
 IMU.GyroWalk: 1e-6 # rad/s^1.5

core/include/KeyFrame.h

@@ -447,6 +447,15 @@ namespace ORB_SLAM3
         // For Semantic Segmentation
         cv::Mat mImage;
         bool isPublished;
+        pcl::PointCloud<pcl::PointXYZRGB>::Ptr mAuxPointCloud;
+        bool mbHasAuxPointCloud = false;
+        double mAuxDepthTimestamp = 0.0;
+        std::string mAuxDepthFrameId;
+        void SetAuxPointCloudFromDepth(const cv::Mat &auxDepth, double auxDepthTimestamp,
+                                       const std::string &auxDepthFrameId, float auxDepthMin,
+                                       float auxDepthMax, int auxDepthStride,
+                                       const std::string &auxDepthScaleMode);
+        pcl::PointCloud<pcl::PointXYZRGB>::Ptr getAuxPointCloud();
 
         // The following variables need to be accessed trough a mutex to be thread safe.
     protected:

core/include/System.h

@@ -270,6 +270,10 @@ namespace ORB_SLAM3
          * @param tuple the address of the tuple of segmented image and pointcloud
          */
         void addSegmentedImage(std::tuple<uint64_t, cv::Mat, pcl::PCLPointCloud2::Ptr> *tuple);
+        void AttachAuxDepthToKeyFrame(uint64_t keyFrameId, const cv::Mat &auxDepth,
+                                      double auxDepthTimestamp, const std::string &auxDepthFrameId,
+                                      float auxDepthMin, float auxDepthMax, int auxDepthStride,
+                                      const std::string &auxDepthScaleMode);
 
         /**
          * @brief Get the skeleton cluster coming from the current map

core/include/Types/SystemParams.h

@@ -160,7 +160,8 @@ namespace ORB_SLAM3
             {
                 GEOMETRIC = 0,
                 FREE_SPACE = 1,
-                GNN = 2
+                GNN = 2,
+                GNN_NEW = 3
             };
             Method method = FREE_SPACE;
 
@@ -185,4 +186,4 @@ namespace ORB_SLAM3
     };
 }
 
-#endif
+#endif

core/src/GeometricSegmentation.cc

@@ -130,6 +130,9 @@ namespace ORB_SLAM3
             // the depth from points using Machine Learning to get a better plane estimate.
             pointcloud = getCloudFromSparsePoints(pKF->getCurrentFrameMapPoints()); // mCurrentFrame.mvpMapPoints
 
+        if (pointcloud == nullptr)
+            return extractedPlanes;
+
         // Convert the pointcloud to one with PointXYZRGBA for consistency
         pcl::PointCloud<pcl::PointXYZRGBA>::Ptr cloudRGBA(new pcl::PointCloud<pcl::PointXYZRGBA>);
         pcl::copyPointCloud(*pointcloud, *cloudRGBA);
@@ -183,4 +186,4 @@ namespace ORB_SLAM3
         return Eigen::Vector4d(normal.x(), normal.y(), normal.z(), D);
     }
 
-}
+}

core/src/KeyFrame.cc

@@ -23,6 +23,9 @@
 #include "KeyFrame.h"
 #include "Converter.h"
 #include "ImuTypes.h"
+#include <algorithm>
+#include <cmath>
+#include <limits>
 #include <mutex>
 
 namespace ORB_SLAM3
@@ -208,13 +211,128 @@ namespace ORB_SLAM3
         return mCurrentFramePointClouds;
     }
 
+    void KeyFrame::SetAuxPointCloudFromDepth(const cv::Mat &auxDepth, double auxDepthTimestamp,
+                                             const std::string &auxDepthFrameId, float auxDepthMin,
+                                             float auxDepthMax, int auxDepthStride,
+                                             const std::string &auxDepthScaleMode)
+    {
+        if (auxDepth.empty() || auxDepth.type() != CV_32FC1)
+            return;
+
+        cv::Mat depth = auxDepth;
+        float depthScale = 1.0f;
+
+        if (auxDepthScaleMode == "map_median")
+        {
+            std::vector<float> mapDepths;
+            std::vector<float> auxDepths;
+            const Sophus::SE3f Tcw = GetPose();
+
+            for (size_t i = 0; i < mCurrentFrameMapPoints.size() && i < mvKeysUn.size(); ++i)
+            {
+                MapPoint *pMP = mCurrentFrameMapPoints[i];
+                if (!pMP || pMP->isBad())
+                    continue;
+
+                const cv::KeyPoint &kp = mvKeysUn[i];
+                const int u = cvRound(kp.pt.x);
+                const int v = cvRound(kp.pt.y);
+                if (u < 0 || v < 0 || u >= depth.cols || v >= depth.rows)
+                    continue;
+
+                const float aux = depth.at<float>(v, u);
+                if (!std::isfinite(aux) || aux <= 1e-6f)
+                    continue;
+
+                const Eigen::Vector3f Pc = Tcw * pMP->GetWorldPos();
+                if (Pc.z() <= auxDepthMin || Pc.z() >= auxDepthMax)
+                    continue;
+
+                mapDepths.push_back(Pc.z());
+                auxDepths.push_back(aux);
+            }
+
+            if (!mapDepths.empty() && mapDepths.size() == auxDepths.size())
+            {
+                std::nth_element(mapDepths.begin(), mapDepths.begin() + mapDepths.size() / 2, mapDepths.end());
+                std::nth_element(auxDepths.begin(), auxDepths.begin() + auxDepths.size() / 2, auxDepths.end());
+                const float medMap = mapDepths[mapDepths.size() / 2];
+                const float medAux = auxDepths[auxDepths.size() / 2];
+                if (medAux > 1e-6f)
+                    depthScale = medMap / medAux;
+            }
+        }
+
+        pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
+        cloud->width = depth.cols;
+        cloud->height = depth.rows;
+        cloud->is_dense = false;
+        cloud->points.resize(static_cast<size_t>(cloud->width) * cloud->height);
+
+        const int stride = std::max(1, auxDepthStride);
+        const float nan = std::numeric_limits<float>::quiet_NaN();
+
+        for (int v = 0; v < depth.rows; ++v)
+        {
+            for (int u = 0; u < depth.cols; ++u)
+            {
+                pcl::PointXYZRGB &pt = cloud->at(u, v);
+                pt.x = nan;
+                pt.y = nan;
+                pt.z = nan;
+
+                if ((u % stride) != 0 || (v % stride) != 0)
+                    continue;
+
+                const float z = depth.at<float>(v, u) * depthScale;
+                if (!std::isfinite(z) || z < auxDepthMin || z > auxDepthMax)
+                    continue;
+
+                pt.x = (static_cast<float>(u) - cx) * z * invfx;
+                pt.y = (static_cast<float>(v) - cy) * z * invfy;
+                pt.z = z;
+
+                if (!mImage.empty() && u < mImage.cols && v < mImage.rows)
+                {
+                    const cv::Vec3b bgr = mImage.channels() == 3 ? mImage.at<cv::Vec3b>(v, u) : cv::Vec3b(0, 0, 0);
+                    pt.r = bgr[2];
+                    pt.g = bgr[1];
+                    pt.b = bgr[0];
+                }
+            }
+        }
+
+        mAuxPointCloud = cloud;
+        mbHasAuxPointCloud = true;
+        mAuxDepthTimestamp = auxDepthTimestamp;
+        mAuxDepthFrameId = auxDepthFrameId;
+
+        // std::cout << "AuxDepth: projected and copied pointcloud into keyframe ID " << mnId
+        //           << " for segmented image, depth timestamp "
+        //           << mAuxDepthTimestamp << std::endl;
+    }
+
+    pcl::PointCloud<pcl::PointXYZRGB>::Ptr KeyFrame::getAuxPointCloud()
+    {
+        return mbHasAuxPointCloud ? mAuxPointCloud : nullptr;
+    }
+
     void KeyFrame::clearPointCloud()
     {
-        mCurrentFramePointClouds->clear();
-        mCurrentFramePointClouds = nullptr;
+        if (mCurrentFramePointClouds)
+        {
+            mCurrentFramePointClouds->clear();
+            mCurrentFramePointClouds = nullptr;
+        }
 
         // clear images
         mImage.release();
+        if (mAuxPointCloud)
+        {
+            mAuxPointCloud->clear();
+            mAuxPointCloud = nullptr;
+        }
+        mbHasAuxPointCloud = false;
     }
 
     std::vector<pcl::PointCloud<pcl::PointXYZRGBA>::Ptr> KeyFrame::getClsCloudPtrs() const

core/src/SemanticSegmentation.cc

@@ -49,11 +49,13 @@ namespace ORB_SLAM3
             KeyFrame *thisKF = mpAtlas->GetKeyFrameById(std::get<0>(segImgTuple));
             if (thisKF == nullptr || thisKF->isBad())
                 continue;
-            const pcl::PointCloud<pcl::PointXYZRGB>::Ptr thisKFPointCloud = thisKF->getCurrentFramePointCloud();
+            pcl::PointCloud<pcl::PointXYZRGB>::Ptr thisKFPointCloud = thisKF->getCurrentFramePointCloud();
+            if (thisKFPointCloud == nullptr && thisKF->mbHasAuxPointCloud)
+                thisKFPointCloud = thisKF->getAuxPointCloud();
             if (thisKFPointCloud == nullptr)
             {
                 std::cout << "SemSeg: skipping KF ID: " << thisKF->mnId << ". Missing pointcloud..." << std::endl;
-                exit(1);
+                // exit(1);
                 continue;
             }
 
@@ -305,4 +307,4 @@ namespace ORB_SLAM3
         // cast a vote for the plane semantics
         matchedPlane->castWeightedVote(planeType, confidence);
     }
-}
+}

core/src/SemanticsManager.cc

@@ -52,7 +52,8 @@ namespace ORB_SLAM3
             // Check for possible room candidates
             if (sysParams->room_seg.method == SystemParams::room_seg::Method::FREE_SPACE)
                 detectRoom_FreeSpaceCluster();
-            else if (sysParams->room_seg.method == SystemParams::room_seg::Method::GNN)
+            else if (sysParams->room_seg.method == SystemParams::room_seg::Method::GNN ||
+                     sysParams->room_seg.method == SystemParams::room_seg::Method::GNN_NEW)
                 detectRoom_GNN();
 
             // Re-associate rooms based on walls and clusters
@@ -653,4 +654,4 @@ namespace ORB_SLAM3
             }
         }
     }
-}
+}

core/src/System.cc

@@ -258,6 +258,24 @@ namespace ORB_SLAM3
         mpSemanticSegmentation->AddSegmentedFrameToBuffer(tuple);
     }
 
+    void System::AttachAuxDepthToKeyFrame(uint64_t keyFrameId, const cv::Mat &auxDepth,
+                                          double auxDepthTimestamp, const std::string &auxDepthFrameId,
+                                          float auxDepthMin, float auxDepthMax, int auxDepthStride,
+                                          const std::string &auxDepthScaleMode)
+    {
+        ORB_SLAM3::KeyFrame *pKF = mpAtlas->GetKeyFrameById(keyFrameId);
+        if (pKF == nullptr || pKF->isBad())
+        {
+            // std::cout << "AuxDepth: keyframe ID " << keyFrameId
+            //           << " not available for segmented pointcloud attachment" << std::endl;
+            return;
+        }
+
+        pKF->SetAuxPointCloudFromDepth(auxDepth, auxDepthTimestamp, auxDepthFrameId,
+                                       auxDepthMin, auxDepthMax, auxDepthStride,
+                                       auxDepthScaleMode);
+    }
+
     std::vector<std::vector<Eigen::Vector3d>> System::getSkeletonCluster()
     {
         return mpAtlas->GetSkeletoClusterPoints();

core/src/Types/SystemParams.cc

@@ -50,7 +50,7 @@ namespace ORB_SLAM3
         try
         {
             // General Parameters
-            general.env_database = mConfig["general"]["env_database"].as<std::string>();
+            general.env_database = mConfig["general"]["env_database"].as<std::string>("");
             general.mode_of_operation = static_cast<general::ModeOfOperation>(mConfig["general"]["mode_of_operation"].as<int>());
 
             // Marker Parameters

docker/Jazzy.Dockerfile

@@ -65,7 +65,7 @@ RUN groupadd --gid $USER_GID $USERNAME \
 
 # --- Python environment setup ---
 RUN pip3 install networkx==3.1
-RUN pip3 install --extra-index-url https://download.pytorch.org/whl/cu121 \
+RUN pip3 install --extra-index-url https://download.pytorch.org/whl/cu126 \
     torch \
     torchvision
 RUN apt remove --purge python3-typing-extensions -y
@@ -121,6 +121,7 @@ RUN --mount=type=ssh git clone git@github.com:snt-arg/visual_sgraphs.git
 RUN --mount=type=ssh git clone git@github.com:snt-arg/situational_graphs_msgs.git
 RUN --mount=type=ssh git clone -b ros2-jazzy git@github.com:snt-arg/scene_segment_ros.git
 RUN --mount=type=ssh git clone -b ros2-master git@github.com:IntelRealSense/realsense-ros.git
+RUN --mount=type=ssh git clone git@github.com:snt-arg/object_tracker_3d_ros.git
 # RUN --mount=type=ssh git clone -b humble-devel git@github.com:pal-robotics/aruco_ros.git
 
 # Repositories for GNN-based room detection and reasoning
@@ -133,6 +134,10 @@ RUN --mount=type=ssh git clone -b ros2-master git@github.com:IntelRealSense/real
 WORKDIR /home/$USERNAME/workspace/src/visual_sgraphs/docker
 RUN pip3 install --break-system-packages --ignore-installed -r requirements.txt
 
+# Install object tracker dependencies
+RUN pip3 install --break-system-packages --ignore-installed \
+    -r /home/$USERNAME/workspace/src/object_tracker_3d_ros/requirements.txt
+
 # [Hint] Temp. fix for installing ROS2 Humble repositories (GNN-based room detection) in Jazzy
 # (Read more: https://github.com/ros2/ros2/issues/1702)
 # RUN pip3 install --break-system-packages setuptools==79.0.1
@@ -161,7 +166,8 @@ RUN apt-get update && apt-get install -y \
 # Build the workspace
 WORKDIR /home/$USERNAME/workspace/
 RUN /bin/bash -c "source /opt/ros/$ROS_DISTRO/setup.bash && rosdep install --from-paths src --ignore-src -r -y"
-RUN /bin/bash -c "source /opt/ros/$ROS_DISTRO/setup.bash && colcon build --symlink-install --cmake-args -DCMAKE_BUILD_TYPE=Release"
+RUN /bin/bash -c "source /opt/ros/$ROS_DISTRO/setup.bash && colcon build --symlink-install --cmake-args -DCMAKE_BUILD_TYPE=Release --packages-skip realsense2_ros_mqtt_bridge"
+RUN /bin/bash -c "source /opt/ros/$ROS_DISTRO/setup.bash && colcon build --packages-select realsense2_ros_mqtt_bridge"
 
 # --- Miscalleanous ---
 RUN ldconfig
@@ -211,4 +217,4 @@ RUN echo "alias mprocs='mprocs -c /home/$USERNAME/workspace/src/visual_sgraphs/c
 ENTRYPOINT ["/entrypoint.sh"]
 USER $USERNAME
 CMD ["/bin/bash"]
-SHELL ["/bin/bash"]
+SHELL ["/bin/bash"]