Add primitive FPS estimator

include/Settings.h

@@ -141,7 +141,6 @@ class Settings {
   bool needToUndistort() { return bNeedToUndistort_; }
 
   cv::Size newImSize() { return newImSize_; }
-  float fps() { return fps_; }
   bool rgb() { return bRGB_; }
   bool needToResize() { return bNeedToResize1_; }
   bool needToRectify() { return bNeedToRectify_; }
@@ -214,7 +213,6 @@ class Settings {
   std::vector<float> vPinHoleDistorsion1_, vPinHoleDistorsion2_;
 
   cv::Size originalImSize_, newImSize_;
-  float fps_;
   bool bRGB_;
 
   bool bNeedToUndistort_;

include/System.h

@@ -43,6 +43,7 @@
 #include "ORBVocabulary.h"
 #include "Settings.h"
 #include "Tracking.h"
+#include "Utils/FpsEstimator.h"
 #include "Viewer.h"
 
 namespace ORB_SLAM3 {
@@ -186,6 +187,8 @@ class System : public std::enable_shared_from_this<System> {
   bool isLost();
   bool isFinished();
 
+  float fps() const { return fps_estimator_.fps(); }
+
   void ChangeDataset();
 
   float GetImageScale();
@@ -278,6 +281,8 @@ class System : public std::enable_shared_from_this<System> {
   std::mutex mMutexState;
 
   std::shared_ptr<Settings> settings_;
+
+  FpsEstimator fps_estimator_;
 };
 
 }  // namespace ORB_SLAM3

include/Utils/FpsEstimator.h

@@ -0,0 +1,68 @@
+/**
+ * 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 <deque>
+
+namespace ORB_SLAM3 {
+
+class FpsEstimator {
+ public:
+  const int DequeDepth = 50;
+
+  explicit FpsEstimator(float prior = -1.0) : prev_ts_(-1.0) {
+    if (prior > 0) {
+      fps_q_ = deque<float>(DequeDepth, prior);
+    }
+  }
+
+  void pushTimestamp(double ts) {
+    if (prev_ts_ < 0) {
+      prev_ts_ = ts;
+      return;
+    }
+
+    const double dt = fabs(ts - prev_ts_);
+    const double fps = 1 / dt;
+    if (!isnan(fps)) {
+      fps_q_.push_front(fps);
+
+      while (fps_q_.size() > DequeDepth) fps_q_.pop_back();
+    }
+
+    prev_ts_ = ts;
+  }
+  float fps() const {
+    if (fps_q_.size() == 0) return 0.0;
+
+    // Very simple average to start with
+    float sum = accumulate(fps_q_.begin(), fps_q_.end(), 0.0);
+    return sum / fps_q_.size();
+  }
+
+ private:
+  double prev_ts_;
+  deque<float> fps_q_;
+};
+
+}  // namespace ORB_SLAM3

src/Settings.cc

@@ -301,9 +301,11 @@ ostream& operator<<(std::ostream& output, const Settings& settings) {
   output << "SLAM settings: " << endl;
 
   output << "\t-Camera 1 parameters (";
-  if (settings.cameraType_ == Settings::PinHole ||
-      settings.cameraType_ == Settings::Rectified) {
+  if (settings.cameraType_ == Settings::PinHole) {
     output << "Pinhole";
+
+  } else if (settings.cameraType_ == Settings::Rectified) {
+    output << "Rectified";
   } else {
     output << "Kannala-Brandt";
   }
@@ -374,8 +376,6 @@ ostream& operator<<(std::ostream& output, const Settings& settings) {
     }
   }
 
-  output << "\t-Sequence FPS: " << settings.fps_ << endl;
-
   // Stereo stuff
   if (settings.sensor_.isStereo()) {
     output << "\t-Stereo baseline: " << settings.b_ << endl;

src/SettingsLoader.cc

@@ -379,7 +379,6 @@ void SettingsLoader::readImageInfo(cv::FileStorage& fSettings) {
   if (resizeHeightFound || resizeWidthFound)
     settings_->setResizeImageSize(resizeWidth, resizeHeight);
 
-  settings_->fps_ = readParameter<int>(fSettings, "Camera.fps", found);
   settings_->bRGB_ =
       static_cast<bool>(readParameter<int>(fSettings, "Camera.RGB", found));
 }

src/System.cc

@@ -277,6 +277,9 @@
   processLocalizationModeChange();
   processReset();
 
+  fps_estimator_.pushTimestamp(timestamp);
+  oslog::debug("[System] Current FPS estimate {:2f}", fps_estimator_.fps());
+
   if (sensorType().isImu()) {
     for (auto const &imuMeas : vImuMeas) {
       mpTracker->GrabImuData(imuMeas);
@@ -314,6 +317,8 @@
   processLocalizationModeChange();
   processReset();
 
+  fps_estimator_.pushTimestamp(timestamp);
+
   if (sensorType().isImu()) {
     for (auto const &imuMeas : vImuMeas) {
       mpTracker->GrabImuData(imuMeas);
@@ -353,6 +358,8 @@
   processLocalizationModeChange();
   processReset();
 
+  fps_estimator_.pushTimestamp(timestamp);
+
   if (sensorType().isImu()) {
     for (auto const &imuMeas : vImuMeas) {
       mpTracker->GrabImuData(imuMeas);
@@ -1234,14 +1241,14 @@
   MD5_CTX md5Context;
   char buffer[1024];
 
   MD5_Init(&md5Context);
   while (int count = f.readsome(buffer, sizeof(buffer))) {
     MD5_Update(&md5Context, buffer, count);
   }
 
   f.close();
 
   MD5_Final(c, &md5Context);
 
   for (int i = 0; i < MD5_DIGEST_LENGTH; i++) {
     char aux[10];

src/Tracking.cc

@@ -601,11 +601,8 @@ void Tracking::newParameterLoader(const std::shared_ptr<Settings>& settings) {
   }
 
   mMinFrames = 0;
-  mMaxFrames = settings->fps();
   mbRGB = settings->rgb();
 
-  mnFramesToResetIMU = mMaxFrames;
-
   {
     // ORB parameters
     const int nFeatures = settings->nFeatures();
@@ -1004,6 +1001,9 @@ void Tracking::Track() {
   std::chrono::steady_clock::time_point t_start =
       std::chrono::steady_clock::now();
 
+  mMaxFrames = mpSystem->fps();
+  mnFramesToResetIMU = mMaxFrames;
+
   if (bStepByStep) {
     oslog::trace("Tracking: Waiting to the next step");
     while (!mbStep && bStepByStep) usleep(500);

src/Viewer.cc

@@ -46,9 +46,7 @@ Viewer::Viewer(System *pSystem,
       mbStopTrack(false) {
   mImageViewerScale = 1.f;
 
-  float fps = settings->fps();
-  if (fps < 1) fps = 30;
-  mT = 1e3 / fps;
+  mT = 1e3 / 30;
 
   cv::Size imSize = settings->newImSize();
   mImageHeight = imSize.height;