Fix stereo_euroc example

Soldann · Jul 20, 2022 · 9edd50c · 9edd50c
1 parent be5bab7
commit 9edd50c
Show file tree

Hide file tree

Showing 4 changed files with 120 additions and 115 deletions.
diff --git a/Examples/Stereo/stereo_euroc.cc b/Examples/Stereo/stereo_euroc.cc
@@ -22,9 +22,10 @@
 #include<iomanip>
 #include<chrono>
 
-#include<opencv2/core/core.hpp>
+#include<opencv2/opencv.hpp>
 
 #include<System.h>
+#include<Viewer.h>
 
 using namespace std;
 
@@ -88,17 +89,20 @@ int main(int argc, char **argv)
     cout.precision(17);
 
     // Create SLAM system. It initializes all system threads and gets ready to process frames.
-    ORB_SLAM3::System SLAM(argv[1],argv[2],ORB_SLAM3::CameraType::STEREO, true);
+    ORB_SLAM3::System_ptr SLAM = std::make_shared<ORB_SLAM3::System>(argv[1],argv[2],ORB_SLAM3::CameraType::STEREO);
+    ORB_SLAM3::Viewer viewer(SLAM, argv[2]);
 
     cv::Mat imLeft, imRight;
     for (seq = 0; seq<num_seq; seq++)
     {
 
         // Seq loop
+#ifdef REGISTER_TIMES
         double t_resize = 0;
         double t_rect = 0;
         double t_track = 0;
         int num_rect = 0;
+#endif
         int proccIm = 0;
         for(int ni=0; ni<nImages[seq]; ni++, proccIm++)
         {
@@ -125,10 +129,12 @@ int main(int argc, char **argv)
             std::chrono::steady_clock::time_point t1 = std::chrono::steady_clock::now();
 
             // Pass the images to the SLAM system
-            SLAM.TrackStereo(imLeft,imRight,tframe, vector<ORB_SLAM3::IMU::Point>(), vstrImageLeft[seq][ni]);
+            auto pos = SLAM->TrackStereo(imLeft,imRight,tframe, vector<ORB_SLAM3::IMU::Point>(), vstrImageLeft[seq][ni]);
 
             std::chrono::steady_clock::time_point t2 = std::chrono::steady_clock::now();
 
+            viewer.update(pos);
+
 #ifdef REGISTER_TIMES
             t_track = t_resize + t_rect + std::chrono::duration_cast<std::chrono::duration<double,std::milli> >(t2 - t1).count();
             SLAM.InsertTrackTime(t_track);
@@ -153,25 +159,25 @@ int main(int argc, char **argv)
         {
             cout << "Changing the dataset" << endl;
 
-            SLAM.ChangeDataset();
+            SLAM->ChangeDataset();
         }
 
     }
     // Stop all threads
-    SLAM.Shutdown();
+    // SLAM.Shutdown();
 
     // Save camera trajectory
     if (bFileName)
     {
         const string kf_file =  "kf_" + string(argv[argc-1]) + ".txt";
         const string f_file =  "f_" + string(argv[argc-1]) + ".txt";
-        SLAM.SaveTrajectoryEuRoC(f_file);
-        SLAM.SaveKeyFrameTrajectoryEuRoC(kf_file);
+        SLAM->SaveTrajectoryEuRoC(f_file);
+        SLAM->SaveKeyFrameTrajectoryEuRoC(kf_file);
     }
     else
     {
-        SLAM.SaveTrajectoryEuRoC("CameraTrajectory.txt");
-        SLAM.SaveKeyFrameTrajectoryEuRoC("KeyFrameTrajectory.txt");
+        SLAM->SaveTrajectoryEuRoC("CameraTrajectory.txt");
+        SLAM->SaveKeyFrameTrajectoryEuRoC("KeyFrameTrajectory.txt");
     }
 
     return 0;

diff --git a/include/LoopClosing.h b/include/LoopClosing.h
@@ -17,9 +17,9 @@
 */
 
 
-#ifndef LOOPCLOSING_H
-#define LOOPCLOSING_H
+#pragma once
 
+#include "ImprovedTypes.hpp"
 #include "KeyFrame.h"
 #include "LocalMapping.h"
 #include "Atlas.h"
@@ -52,7 +52,7 @@ class LoopClosing
 
 public:
 
-    LoopClosing(Atlas* pAtlas, KeyFrameDatabase* pDB, ORBVocabulary* pVoc,const bool bFixScale, const bool bActiveLC);
+    LoopClosing(const Atlas_ptr &pAtlas, KeyFrameDatabase* pDB, ORBVocabulary* pVoc,const bool bFixScale, const bool bActiveLC);
 
     void SetTracker(Tracking* pTracker);
 
@@ -154,7 +154,7 @@ class LoopClosing
     bool mbFinished;
     std::mutex mMutexFinish;
 
-    Atlas* mpAtlas;
+    Atlas_ptr mpAtlas;
     Tracking* mpTracker;
 
     KeyFrameDatabase* mpKeyFrameDB;
@@ -243,4 +243,3 @@ class LoopClosing
 
 } //namespace ORB_SLAM
 
-#endif // LOOPCLOSING_H
diff --git a/src/LoopClosing.cc b/src/LoopClosing.cc
@@ -24,6 +24,7 @@
 #include <mutex>
 #include <thread>
 
+#include "ImprovedTypes.hpp"
 #include "Converter.h"
 #include "G2oTypes.h"
 #include "ORBmatcher.h"
@@ -32,7 +33,7 @@
 
 namespace ORB_SLAM3 {
 
-LoopClosing::LoopClosing(Atlas* pAtlas, KeyFrameDatabase* pDB,
+LoopClosing::LoopClosing(const Atlas_ptr &pAtlas, KeyFrameDatabase* pDB,
                          ORBVocabulary* pVoc, const bool bFixScale,
                          const bool bActiveLC)
     : 
@@ -108,9 +109,9 @@ void LoopClosing::Run() {
 #endif
       if (bFindedRegion) {
         if (mbMergeDetected) {
-          if ((mpTracker->mSensor == System::IMU_MONOCULAR ||
-               mpTracker->mSensor == System::IMU_STEREO ||
-               mpTracker->mSensor == System::IMU_RGBD) &&
+          if ((mpTracker->mSensor == CameraType::IMU_MONOCULAR ||
+               mpTracker->mSensor == CameraType::IMU_STEREO ||
+               mpTracker->mSensor == CameraType::IMU_RGBD) &&
               (!mpCurrentKF->GetMap()->isImuInitialized())) {
             cout << "IMU is not initilized, merge is aborted" << endl;
           } else {
@@ -139,9 +140,9 @@ void LoopClosing::Run() {
                 continue;
               }
               // If inertial, force only yaw
-              if ((mpTracker->mSensor == System::IMU_MONOCULAR ||
-                   mpTracker->mSensor == System::IMU_STEREO ||
-                   mpTracker->mSensor == System::IMU_RGBD) &&
+              if ((mpTracker->mSensor == CameraType::IMU_MONOCULAR ||
+                   mpTracker->mSensor == CameraType::IMU_STEREO ||
+                   mpTracker->mSensor == CameraType::IMU_RGBD) &&
                   mpCurrentKF->GetMap()->GetIniertialBA1()) {
                 Eigen::Vector3d phi =
                     LogSO3(mSold_new.rotation().toRotationMatrix());
@@ -167,9 +168,9 @@ void LoopClosing::Run() {
             nMerges += 1;
 #endif
             // TODO UNCOMMENT
-            if (mpTracker->mSensor == System::IMU_MONOCULAR ||
-                mpTracker->mSensor == System::IMU_STEREO ||
-                mpTracker->mSensor == System::IMU_RGBD)
+            if (mpTracker->mSensor == CameraType::IMU_MONOCULAR ||
+                mpTracker->mSensor == CameraType::IMU_STEREO ||
+                mpTracker->mSensor == CameraType::IMU_RGBD)
               MergeLocal2();
             else
               MergeLocal();
@@ -235,9 +236,9 @@ void LoopClosing::Run() {
                 fabs(phi(2)) < 0.349f) {
               if (mpCurrentKF->GetMap()->IsInertial()) {
                 // If inertial, force only yaw
-                if ((mpTracker->mSensor == System::IMU_MONOCULAR ||
-                     mpTracker->mSensor == System::IMU_STEREO ||
-                     mpTracker->mSensor == System::IMU_RGBD) &&
+                if ((mpTracker->mSensor == CameraType::IMU_MONOCULAR ||
+                     mpTracker->mSensor == CameraType::IMU_STEREO ||
+                     mpTracker->mSensor == CameraType::IMU_RGBD) &&
                     mpCurrentKF->GetMap()->GetIniertialBA2()) {
                   phi(0) = 0;
                   phi(1) = 0;
@@ -337,7 +338,7 @@ bool LoopClosing::NewDetectCommonRegions() {
     return false;
   }
 
-  if (mpTracker->mSensor == System::STEREO &&
+  if (mpTracker->mSensor == CameraType::STEREO &&
       mpLastMap->GetAllKeyFrames().size() < 5)  // 12
   {
     // cout << "LoopClousure: Stereo KF inserted without check: " <<
@@ -558,7 +559,7 @@ bool LoopClosing::DetectAndReffineSim3FromLastKF(
 
     bool bFixedScale =
         mbFixScale;  // TODO CHECK; Solo para el monocular inertial
-    if (mpTracker->mSensor == System::IMU_MONOCULAR &&
+    if (mpTracker->mSensor == CameraType::IMU_MONOCULAR &&
         !pCurrentKF->GetMap()->GetIniertialBA2())
       bFixedScale = false;
     int numOptMatches =
@@ -697,7 +698,7 @@ bool LoopClosing::DetectCommonRegionsFromBoW(
     {
       // Geometric validation
       bool bFixedScale = mbFixScale;
-      if (mpTracker->mSensor == System::IMU_MONOCULAR &&
+      if (mpTracker->mSensor == CameraType::IMU_MONOCULAR &&
           !mpCurrentKF->GetMap()->GetIniertialBA2())
         bFixedScale = false;
 
@@ -781,7 +782,7 @@ bool LoopClosing::DetectCommonRegionsFromBoW(
 
           /* The bool below ('bFixedScale') is UNUSED and since all function calls below do not change state. this is not needed.
           bool bFixedScale = mbFixScale; 
-          if(mpTracker->mSensor==System::IMU_MONOCULAR && !mpCurrentKF->GetMap()->GetIniertialBA2())
+          if(mpTracker->mSensor==CameraType::IMU_MONOCULAR && !mpCurrentKF->GetMap()->GetIniertialBA2())
               bFixedScale=false;
           */
 
@@ -1181,7 +1182,7 @@ void LoopClosing::CorrectLoop() {
   // Optimize graph
   bool bFixedScale = mbFixScale;
   // TODO CHECK; Solo para el monocular inertial
-  if (mpTracker->mSensor == System::IMU_MONOCULAR &&
+  if (mpTracker->mSensor == CameraType::IMU_MONOCULAR &&
       !mpCurrentKF->GetMap()->GetIniertialBA2())
     bFixedScale = false;
 
@@ -1642,9 +1643,9 @@ void LoopClosing::MergeLocal() {
             std::back_inserter(vpLocalCurrentWindowKFs));
   std::copy(spMergeConnectedKFs.begin(), spMergeConnectedKFs.end(),
             std::back_inserter(vpMergeConnectedKFs));
-  if (mpTracker->mSensor == System::IMU_MONOCULAR ||
-      mpTracker->mSensor == System::IMU_STEREO ||
-      mpTracker->mSensor == System::IMU_RGBD) {
+  if (mpTracker->mSensor == CameraType::IMU_MONOCULAR ||
+      mpTracker->mSensor == CameraType::IMU_STEREO ||
+      mpTracker->mSensor == CameraType::IMU_RGBD) {
     Optimizer::MergeInertialBA(mpCurrentKF, mpMergeMatchedKF, &bStop,
                                pCurrentMap, vCorrectedSim3);
   } else {
@@ -1673,7 +1674,7 @@ void LoopClosing::MergeLocal() {
 
   if (vpCurrentMapKFs.size() == 0) {
   } else {
-    if (mpTracker->mSensor == System::MONOCULAR) {
+    if (mpTracker->mSensor == CameraType::MONOCULAR) {
       unique_lock<mutex> currentLock(
           pCurrentMap->mMutexMapUpdate);  // We update the current map with the
                                           // Merge information
@@ -1742,7 +1743,7 @@ void LoopClosing::MergeLocal() {
 
     // Optimize graph (and update the loop position for each element form the
     // begining to the end)
-    if (mpTracker->mSensor != System::MONOCULAR) {
+    if (mpTracker->mSensor != CameraType::MONOCULAR) {
       Optimizer::OptimizeEssentialGraph(mpCurrentKF, vpMergeConnectedKFs,
                                         vpLocalCurrentWindowKFs,
                                         vpCurrentMapKFs, vpCurrentMapMPs);
@@ -1890,9 +1891,9 @@ void LoopClosing::MergeLocal2() {
 
   const int numKFnew = pCurrentMap->KeyFramesInMap();
 
-  if ((mpTracker->mSensor == System::IMU_MONOCULAR ||
-       mpTracker->mSensor == System::IMU_STEREO ||
-       mpTracker->mSensor == System::IMU_RGBD) &&
+  if ((mpTracker->mSensor == CameraType::IMU_MONOCULAR ||
+       mpTracker->mSensor == CameraType::IMU_STEREO ||
+       mpTracker->mSensor == CameraType::IMU_RGBD) &&
       !pCurrentMap->GetIniertialBA2()) {
     // Map is not completly initialized
     Eigen::Vector3d bg, ba;