Dev/separate modalities

CMakeLists.txt

@@ -96,8 +96,12 @@ add_library(${PROJECT_NAME} SHARED
         src/SettingsLoader.cc
         src/Sim3Solver.cc
         src/System.cc
+        src/System/Atlas.cc
+        src/System/Exporters.cc
+        src/System/Factory.cc
         src/Tracking.cc
         src/TwoViewReconstruction.cc
+        src/Utils/Checksum.cc
         src/Viewer.cc
 )
 target_compile_definitions(${PROJECT_NAME} PUBLIC REGISTER_TIMES)

include/Logging.h

@@ -161,7 +161,7 @@ class Verbose {
         oslog::info("{}", str);
         break;
       case VERBOSITY_NORMAL:
-        oslog::error("{}", str);
+        oslog::warn("{}", str);
         break;
       case VERBOSITY_QUIET:
         oslog::critical("{}", str);

include/MapPoint.h

@@ -92,7 +92,9 @@ class MapPoint {
     ar& boost::serialization::make_array(mNormalVector.data(),
                                          mNormalVector.size());
     // ar & BOOST_SERIALIZATION_NVP(mBackupObservationsId);
-    ar & mObservations;
+
+    // ar & mObservations;
+
     ar & mBackupObservationsId1;
     ar & mBackupObservationsId2;
     serializeMatrix(ar, mDescriptor, version);

include/Settings.h

@@ -36,6 +36,7 @@
 
 #include "CameraModels/GeometricCamera.h"
 #include "Expected.h"
+#include "Logging.h"
 #include "Types.h"
 
 namespace ORB_SLAM3 {
@@ -48,7 +49,7 @@ class Settings;
 class SettingsLoader {
  public:
   typedef tl::expected<std::shared_ptr<Settings>, ExpectedError> Expected;
-  static Expected load(const std::string& configFile, const SensorType sensor,
+  static Expected Load(const std::string& configFile, const SensorType sensor,
                        const std::string& vocabFile = "");
 
   explicit SettingsLoader(const SensorType sensor);
@@ -74,12 +75,11 @@ class SettingsLoader {
     cv::FileNode node = fSettings[name];
     if (node.empty()) {
       if (required) {
-        std::cerr << name << " required parameter does not exist, aborting..."
-                  << std::endl;
+        oslog::error("Required parameter \"{}\" does not exist, aborting...",
+                     name);
         exit(-1);
       } else {
-        std::cerr << name << " optional parameter does not exist..."
-                  << std::endl;
+        oslog::warn("Optional parameter \"{}\" does not exist.", name);
         found = false;
         return T();
       }

include/System.h

@@ -42,6 +42,7 @@
 #include "MapDrawer.h"
 #include "ORBVocabulary.h"
 #include "Settings.h"
+#include "System/Factory.h"
 #include "Tracking.h"
 #include "Utils/FpsEstimator.h"
 #include "Viewer.h"
@@ -57,25 +58,6 @@ class LocalMapping;
 class LoopClosing;
 class Settings;
 
-class SystemFactory {
- public:
-  typedef tl::expected<std::shared_ptr<System>, ExpectedError> Expected;
-
-  static Expected create(const std::shared_ptr<Settings> &settings,
-                         bool initFr = false,
-                         const string &strSequence = std::string());
-
-  static Expected create(const std::string &configFile, const SensorType sensor,
-                         bool initFr = false,
-                         const string &strSequence = std::string());
-
-  // Provided for compatibility with old API
-  static Expected create(const std::string &configFile,
-                         const std::string &vocabFile, const SensorType sensor,
-                         bool initFr = false,
-                         const string &strSequence = std::string());
-};
-
 // System should be created using  SystemFactory::create()
 //
 // It will validate settings and catch errors on startup
@@ -84,19 +66,13 @@ class System : public std::enable_shared_from_this<System> {
   friend SystemFactory::Expected SystemFactory::create(
       const std::shared_ptr<Settings> &, bool, const string &);
 
-  // File type
-  enum FileType {
-    TEXT_FILE = 0,
-    BINARY_FILE = 1,
-  };
-
   EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 
   // Proccess the given stereo frame. Images must be synchronized and rectified.
   // Input images: RGB (CV_8UC3) or grayscale (CV_8U). RGB is converted to
   // grayscale. Returns the camera pose (empty if tracking fails).
   Sophus::SE3f TrackStereo(
-      const cv::Mat &imLeft, const cv::Mat &imRight, const double &timestamp,
+      cv::InputArray imLeft, cv::InputArray imRight, double timestamp,
       const vector<IMU::Point> &vImuMeas = vector<IMU::Point>(),
       string filename = "");
 
@@ -105,15 +81,15 @@ class System : public std::enable_shared_from_this<System> {
   // grayscale. Input depthmap: Float (CV_32F). Returns the camera pose (empty
   // if tracking fails).
   Sophus::SE3f TrackRGBD(
-      const cv::Mat &im, const cv::Mat &depthmap, const double &timestamp,
+      cv::InputArray im, cv::InputArray depthmap, double timestamp,
       const vector<IMU::Point> &vImuMeas = vector<IMU::Point>(),
       string filename = "");
 
   // Proccess the given monocular frame and optionally imu data
   // Input images: RGB (CV_8UC3) or grayscale (CV_8U). RGB is converted to
   // grayscale. Returns the camera pose (empty if tracking fails).
   Sophus::SE3f TrackMonocular(
-      const cv::Mat &im, const double &timestamp,
+      cv::InputArray im, double timestamp,
       const vector<IMU::Point> &vImuMeas = vector<IMU::Point>(),
       string filename = "");
 
@@ -213,15 +189,12 @@ class System : public std::enable_shared_from_this<System> {
   bool initialize(bool initFr = false,
                   const string &strSequence = std::string());
 
- private:
   void processLocalizationModeChange(void);
   void processReset(void);
   void updateTrackingState();
 
-  void SaveAtlas(int type);
-  bool LoadAtlas(int type);
-
-  string CalculateCheckSum(string filename, int type);
+  void SaveAtlas(FileType type);
+  bool LoadAtlas(FileType type);
 
   // ORB vocabulary used for place recognition and feature matching.
   std::shared_ptr<ORBVocabulary> mpVocabulary;

include/System/Factory.h

@@ -0,0 +1,51 @@
+/**
+ * This file is part of ORB-SLAM3
+ *
+ * Copyright (C) 2017-2021 Carlos Campos, Richard Elvira, Juan J. Gómez
+ * Rodríguez, José M.M. Montiel and Juan D. Tardós, University of Zaragoza.
+ * Copyright (C) 2014-2016 Raúl Mur-Artal, José M.M. Montiel and Juan D. Tardós,
+ * University of Zaragoza.
+ *
+ * ORB-SLAM3 is free software: you can redistribute it and/or modify it under
+ * the terms of the GNU General Public License as published by the Free Software
+ * Foundation, either version 3 of the License, or (at your option) any later
+ * version.
+ *
+ * ORB-SLAM3 is distributed in the hope that it will be useful, but WITHOUT ANY
+ * WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR
+ * A PARTICULAR PURPOSE. See the GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * ORB-SLAM3. If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#pragma once
+
+#include <memory>
+#include <string>
+
+#include "Settings.h"
+
+namespace ORB_SLAM3 {
+
+class System;
+
+class SystemFactory {
+ public:
+  typedef tl::expected<std::shared_ptr<System>, ExpectedError> Expected;
+
+  static Expected create(const std::shared_ptr<Settings> &settings,
+                         bool initFr = false,
+                         const std::string &strSequence = std::string());
+
+  static Expected create(const std::string &configFile, const SensorType sensor,
+                         bool initFr = false,
+                         const std::string &strSequence = std::string());
+
+  // Provided for compatibility with old API
+  static Expected create(const std::string &configFile,
+                         const std::string &vocabFile, const SensorType sensor,
+                         bool initFr = false,
+                         const std::string &strSequence = std::string());
+};
+}  // namespace ORB_SLAM3

include/Types.h

@@ -21,10 +21,13 @@
 
 #pragma once
 
+#include <cstdint>
 #include <string>
 
 namespace ORB_SLAM3 {
 
+// Rather than a conventional enum class, use this pattern which lets us
+// add member functions
 class SensorType {
  public:
   enum Value : uint8_t {
@@ -77,4 +80,6 @@ class SensorType {
   Value value;
 };
 
+enum class FileType { TEXT_FILE, BINARY_FILE };
+
 }  // namespace ORB_SLAM3

include/Utils/Checksum.h

@@ -0,0 +1,37 @@
+/**
+ * This file was added to ORB-SLAM3
+ *
+ * Copyright (C) 2026b Aaron Marburg
+ * Copyright (C) 2017-2021 Carlos Campos, Richard Elvira, Juan J. Gómez
+ * Rodríguez, José M.M. Montiel and Juan D. Tardós, University of Zaragoza.
+ * Copyright (C) 2014-2016 Raúl Mur-Artal, José M.M. Montiel and Juan D. Tardós,
+ * University of Zaragoza.
+ *
+ * ORB-SLAM3 is free software: you can redistribute it and/or modify it under
+ * the terms of the GNU General Public License as published by the Free Software
+ * Foundation, either version 3 of the License, or (at your option) any later
+ * version.
+ *
+ * ORB-SLAM3 is distributed in the hope that it will be useful, but WITHOUT ANY
+ * WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR
+ * A PARTICULAR PURPOSE. See the GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * ORB-SLAM3. If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#pragma once
+
+#include <string>
+
+#include "Types.h"
+
+namespace ORB_SLAM3 {
+
+namespace Checksum {
+
+std::string Calculate(std::string filename, FileType type);
+
+}
+
+}  // namespace ORB_SLAM3

src/LocalMapping.cc

@@ -1255,8 +1255,7 @@ void LocalMapping::InitializeIMU(float priorG, float priorA, bool bFIBA) {
 
   std::chrono::steady_clock::time_point t5 = std::chrono::steady_clock::now();
 
-  Verbose::PrintMess("Global Bundle Adjustment finished\nUpdating map ...",
-                     Verbose::VERBOSITY_NORMAL);
+  oslog::info("Global Bundle Adjustment finished.  Updating map ...");
 
   // Get Map Mutex
   unique_lock<mutex> lock(mpAtlas->GetCurrentMap()->mMutexMapUpdate);
@@ -1292,8 +1291,7 @@ void LocalMapping::InitializeIMU(float priorG, float priorA, bool bFIBA) {
         if (pChild->isVelocitySet()) {
           pChild->mVwbGBA = Rcor * pChild->GetVelocity();
         } else {
-          Verbose::PrintMess("Child velocity empty!! ",
-                             Verbose::VERBOSITY_NORMAL);
+          oslog::warn("Child velocity empty!! ");
         }
 
         pChild->mBiasGBA = pChild->GetImuBias();
@@ -1341,7 +1339,7 @@ void LocalMapping::InitializeIMU(float priorG, float priorA, bool bFIBA) {
     }
   }
 
-  Verbose::PrintMess("Map updated!", Verbose::VERBOSITY_NORMAL);
+  oslog::info("Map updated!");
 
   mnKFs = vpKF.size();
   mIdxInit++;

src/LoopClosing.cc

@@ -171,8 +171,7 @@ void LoopClosing::Run() {
                 mvpMergeMPs.clear();
                 mnMergeNumNotFound = 0;
                 mbMergeDetected = false;
-                Verbose::PrintMess("scale bad estimated. Abort merging",
-                                   Verbose::VERBOSITY_NORMAL);
+                oslog::warn("scale bad estimated. Abort merging");
                 continue;
               }
               // If inertial, force only yaw
@@ -193,7 +192,7 @@ void LoopClosing::Run() {
 
             // mpTracker->SetStepByStep(true);
 
-            Verbose::PrintMess("*Merge detected", Verbose::VERBOSITY_QUIET);
+            oslog::info("*Merge detected");
 
 #ifdef REGISTER_TIMES
             std::chrono::steady_clock::time_point time_StartMerge =
@@ -219,7 +218,7 @@ void LoopClosing::Run() {
             vdMergeTotal_ms.push_back(timeMergeTotal);
 #endif
 
-            Verbose::PrintMess("Merge finished!", Verbose::VERBOSITY_QUIET);
+            oslog::info("Merge finished!");
           }
 
           vdPR_CurrentTime.push_back(mpCurrentKF->mTimeStamp);
@@ -253,7 +252,7 @@ void LoopClosing::Run() {
           vdPR_MatchedTime.push_back(mpLoopMatchedKF->mTimeStamp);
           vnPR_TypeRecogn.push_back(0);
 
-          Verbose::PrintMess("*Loop detected", Verbose::VERBOSITY_QUIET);
+          oslog::debug("*Loop detected");
 
           mg2oLoopScw = mg2oLoopSlw;  // *mvg2oSim3LoopTcw[nCurrentIndex];
           if (mpCurrentKF->GetMap()->IsInertial()) {
@@ -1487,13 +1486,12 @@ void LoopClosing::MergeLocal() {
 #endif
   for (auto pKFi : spLocalWindowKFs) {
     if (!pKFi || pKFi->isBad()) {
-      Verbose::PrintMess("Bad KF in correction", Verbose::VERBOSITY_DEBUG);
+      oslog::debug("Bad KF in correction");
       continue;
     }
 
     if (pKFi->GetMap() != pCurrentMap)
-      Verbose::PrintMess("Other map KF, this should't happen",
-                         Verbose::VERBOSITY_DEBUG);
+      oslog::debug("Other map KF, this should't happen");
 
     g2o::Sim3 g2oCorrectedSiw;
 
@@ -2330,8 +2328,7 @@ void LoopClosing::ResetIfRequested() {
 
 void LoopClosing::RunGlobalBundleAdjustment(
     const std::shared_ptr<Map>& pActiveMap, unsigned long nLoopKF) {
-  Verbose::PrintMess("Starting Global Bundle Adjustment",
-                     Verbose::VERBOSITY_NORMAL);
+  oslog::debug("Starting Global Bundle Adjustment");
 
 #ifdef REGISTER_TIMES
   std::chrono::steady_clock::time_point time_StartFGBA =
@@ -2381,9 +2378,8 @@ void LoopClosing::RunGlobalBundleAdjustment(
     if (!bImuInit && pActiveMap->isImuInitialized()) return;
 
     if (!mbStopGBA) {
-      Verbose::PrintMess("Global Bundle Adjustment finished",
-                         Verbose::VERBOSITY_NORMAL);
-      Verbose::PrintMess("Updating map ...", Verbose::VERBOSITY_NORMAL);
+      oslog::info("Global Bundle Adjustment finished");
+      oslog::info("Updating map ...");
 
       mpLocalMapper->RequestStop();
       // Wait until Local Mapping has effectively stopped
@@ -2428,8 +2424,7 @@ void LoopClosing::RunGlobalBundleAdjustment(
             if (pChild->isVelocitySet()) {
               pChild->mVwbGBA = Rcor * pChild->GetVelocity();
             } else {
-              Verbose::PrintMess("Child velocity empty!! ",
-                                 Verbose::VERBOSITY_NORMAL);
+              oslog::info("Child velocity empty!!");
             }
 
             // cout << "Child bias: " << pChild->GetImuBias() << endl;
@@ -2574,7 +2569,7 @@ void LoopClosing::RunGlobalBundleAdjustment(
               .count();
       vdFGBATotal_ms.push_back(timeFGBA);
 #endif
-      Verbose::PrintMess("Map updated!", Verbose::VERBOSITY_NORMAL);
+      oslog::info("Map updated!");
     }
 
     mbFinishedGBA = true;