From c7b2b8446f8be6d11732024156ddba03349198fe Mon Sep 17 00:00:00 2001 From: Aaron Marburg Date: Thu, 5 Feb 2026 04:56:30 +0000 Subject: [PATCH] Add primitive FPS estimator --- include/Settings.h | 2 -- include/System.h | 5 +++ include/Utils/FpsEstimator.h | 68 ++++++++++++++++++++++++++++++++++++ src/Settings.cc | 8 ++--- src/SettingsLoader.cc | 1 - src/System.cc | 7 ++++ src/Tracking.cc | 6 ++-- src/Viewer.cc | 4 +-- 8 files changed, 88 insertions(+), 13 deletions(-) create mode 100644 include/Utils/FpsEstimator.h diff --git a/include/Settings.h b/include/Settings.h index e77c8b618b1..db0a2bd70f1 100644 --- a/include/Settings.h +++ b/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 vPinHoleDistorsion1_, vPinHoleDistorsion2_; cv::Size originalImSize_, newImSize_; - float fps_; bool bRGB_; bool bNeedToUndistort_; diff --git a/include/System.h b/include/System.h index f6853812a92..55857c4a26d 100644 --- a/include/System.h +++ b/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 { 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 { std::mutex mMutexState; std::shared_ptr settings_; + + FpsEstimator fps_estimator_; }; } // namespace ORB_SLAM3 diff --git a/include/Utils/FpsEstimator.h b/include/Utils/FpsEstimator.h new file mode 100644 index 00000000000..d38c444a7f4 --- /dev/null +++ b/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 . + */ + +#pragma once + +#include + +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(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 fps_q_; +}; + +} // namespace ORB_SLAM3 diff --git a/src/Settings.cc b/src/Settings.cc index ea795eb1e02..61ced44a78e 100644 --- a/src/Settings.cc +++ b/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; diff --git a/src/SettingsLoader.cc b/src/SettingsLoader.cc index 3df10014340..5fca81262b7 100644 --- a/src/SettingsLoader.cc +++ b/src/SettingsLoader.cc @@ -379,7 +379,6 @@ void SettingsLoader::readImageInfo(cv::FileStorage& fSettings) { if (resizeHeightFound || resizeWidthFound) settings_->setResizeImageSize(resizeWidth, resizeHeight); - settings_->fps_ = readParameter(fSettings, "Camera.fps", found); settings_->bRGB_ = static_cast(readParameter(fSettings, "Camera.RGB", found)); } diff --git a/src/System.cc b/src/System.cc index e730d0bf5b3..f947c8caf60 100644 --- a/src/System.cc +++ b/src/System.cc @@ -277,6 +277,9 @@ Sophus::SE3f System::TrackStereo(const cv::Mat &imLeft, const cv::Mat &imRight, 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 @@ Sophus::SE3f System::TrackRGBD(const cv::Mat &im, const cv::Mat &depthmap, processLocalizationModeChange(); processReset(); + fps_estimator_.pushTimestamp(timestamp); + if (sensorType().isImu()) { for (auto const &imuMeas : vImuMeas) { mpTracker->GrabImuData(imuMeas); @@ -353,6 +358,8 @@ Sophus::SE3f System::TrackMonocular(const cv::Mat &im, const double ×tamp, processLocalizationModeChange(); processReset(); + fps_estimator_.pushTimestamp(timestamp); + if (sensorType().isImu()) { for (auto const &imuMeas : vImuMeas) { mpTracker->GrabImuData(imuMeas); diff --git a/src/Tracking.cc b/src/Tracking.cc index 56ddbbaae7e..89fb6fb2549 100644 --- a/src/Tracking.cc +++ b/src/Tracking.cc @@ -601,11 +601,8 @@ void Tracking::newParameterLoader(const std::shared_ptr& 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); diff --git a/src/Viewer.cc b/src/Viewer.cc index 072b8870c48..bac99d49fa8 100644 --- a/src/Viewer.cc +++ b/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;