From bcd74113058a0a3ac03422e30da280f2b4fb2a4d Mon Sep 17 00:00:00 2001 From: Aaron Marburg Date: Sun, 15 Feb 2026 15:55:22 +0000 Subject: [PATCH 1/4] Break apart System a bit for readability. --- CMakeLists.txt | 3 + include/System.h | 12 +- include/Types.h | 5 + include/Utils/Checksum.h | 37 +++ src/MapPoint.cc | 4 +- src/System.cc | 656 --------------------------------------- src/System/Atlas.cc | 166 ++++++++++ src/System/Exporters.cc | 553 +++++++++++++++++++++++++++++++++ src/Utils/Checksum.cc | 78 +++++ 9 files changed, 846 insertions(+), 668 deletions(-) create mode 100644 include/Utils/Checksum.h create mode 100644 src/System/Atlas.cc create mode 100644 src/System/Exporters.cc create mode 100644 src/Utils/Checksum.cc diff --git a/CMakeLists.txt b/CMakeLists.txt index 497e6727abf..25109a208e5 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -96,8 +96,11 @@ add_library(${PROJECT_NAME} SHARED src/SettingsLoader.cc src/Sim3Solver.cc src/System.cc + src/System/Atlas.cc + src/System/Exporters.cc src/Tracking.cc src/TwoViewReconstruction.cc + src/Utils/Checksum.cc src/Viewer.cc ) target_compile_definitions(${PROJECT_NAME} PUBLIC REGISTER_TIMES) diff --git a/include/System.h b/include/System.h index 55857c4a26d..175c82f19c0 100644 --- a/include/System.h +++ b/include/System.h @@ -84,12 +84,6 @@ class System : public std::enable_shared_from_this { friend SystemFactory::Expected SystemFactory::create( const std::shared_ptr &, 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. @@ -218,10 +212,8 @@ class System : public std::enable_shared_from_this { 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 mpVocabulary; diff --git a/include/Types.h b/include/Types.h index a389e126104..e1b4c74db80 100644 --- a/include/Types.h +++ b/include/Types.h @@ -21,10 +21,13 @@ #pragma once +#include #include 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 diff --git a/include/Utils/Checksum.h b/include/Utils/Checksum.h new file mode 100644 index 00000000000..30e6b81960e --- /dev/null +++ b/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 . + */ + +#pragma once + +#include + +#include "Types.h" + +namespace ORB_SLAM3 { + +namespace Checksum { + +std::string Calculate(std::string filename, FileType type); + +} + +} // namespace ORB_SLAM3 diff --git a/src/MapPoint.cc b/src/MapPoint.cc index 7bdf92a7b2c..9fcbc8fc7ab 100644 --- a/src/MapPoint.cc +++ b/src/MapPoint.cc @@ -638,8 +638,8 @@ void MapPoint::PostLoad( map& mpMPid) { mpRefKF = mpKFid[mBackupRefKFId]; if (!mpRefKF) { - cout << "ERROR: MP without KF reference " << mBackupRefKFId - << "; Num obs: " << nObs << endl; + oslog::warn("ERROR: MP without KF reference {}; Num obs: {}", + mBackupRefKFId, nObs); } mpReplaced = static_cast(NULL); if (mBackupReplacedId >= 0) { diff --git a/src/System.cc b/src/System.cc index 8294dcdabb6..2cf1dbfeadc 100644 --- a/src/System.cc +++ b/src/System.cc @@ -21,7 +21,6 @@ #include "System.h" -#include #include #include @@ -509,509 +508,6 @@ bool System::isShutDown() { return mbShutDown; } -void System::SaveTrajectoryTUM(const string &filename) { - vector> vpKFs = mpAtlas->GetAllKeyFrames(); - - if (vpKFs.size() == 0) { - oslog::error("Cannot save TUM trajectory, there are no Keyframes"); - return; - } - - oslog::info("Saving camera trajectory to {} ...", filename); - if (sensorType() == SensorType::MONOCULAR) { - cerr << "ERROR: SaveTrajectoryTUM cannot be used for monocular." << endl; - return; - } - - sort(vpKFs.begin(), vpKFs.end(), KeyFrame::lId); - - // Transform all keyframes so that the first keyframe is at the origin. - // After a loop closure the first keyframe might not be at the origin. - Sophus::SE3f Two = vpKFs[0]->GetPoseInverse(); - - ofstream f; - f.open(filename.c_str()); - f << fixed; - - // Frame pose is stored relative to its reference keyframe (which is optimized - // by BA and pose graph). We need to get first the keyframe pose and then - // concatenate the relative transformation. Frames not localized (tracking - // failure) are not saved. - - // For each frame we have a reference keyframe (lRit), the timestamp (lT) and - // a flag which is true when tracking failed (lbL). - list>::iterator lRit = - mpTracker->mlpReferences.begin(); - list::iterator lT = mpTracker->mlFrameTimes.begin(); - list::iterator lbL = mpTracker->mlbLost.begin(); - for (list::iterator - lit = mpTracker->mlRelativeFramePoses.begin(), - lend = mpTracker->mlRelativeFramePoses.end(); - lit != lend; lit++, lRit++, lT++, lbL++) { - if (*lbL) continue; - - std::shared_ptr pKF = *lRit; - - Sophus::SE3f Trw; - - // If the reference keyframe was culled, traverse the spanning tree to get a - // suitable keyframe. - while (pKF->isBad()) { - Trw = Trw * pKF->mTcp; - pKF = pKF->GetParent(); - } - - Trw = Trw * pKF->GetPose() * Two; - - Sophus::SE3f Tcw = (*lit) * Trw; - Sophus::SE3f Twc = Tcw.inverse(); - - Eigen::Vector3f twc = Twc.translation(); - Eigen::Quaternionf q = Twc.unit_quaternion(); - - f << setprecision(6) << *lT << " " << setprecision(9) << twc(0) << " " - << twc(1) << " " << twc(2) << " " << q.x() << " " << q.y() << " " << q.z() - << " " << q.w() << endl; - } - f.close(); -} - -void System::SaveKeyFrameTrajectoryTUM(const string &filename) { - oslog::info("Saving keyframe trajectory to {} ...", filename); - - vector> vpKFs = mpAtlas->GetAllKeyFrames(); - sort(vpKFs.begin(), vpKFs.end(), KeyFrame::lId); - - // Transform all keyframes so that the first keyframe is at the origin. - // After a loop closure the first keyframe might not be at the origin. - ofstream f; - f.open(filename.c_str()); - f << fixed; - - for (auto pKF : vpKFs) { - if (pKF->isBad()) continue; - - Sophus::SE3f Twc = pKF->GetPoseInverse(); - Eigen::Quaternionf q = Twc.unit_quaternion(); - Eigen::Vector3f t = Twc.translation(); - f << setprecision(6) << pKF->mTimeStamp << setprecision(7) << " " << t(0) - << " " << t(1) << " " << t(2) << " " << q.x() << " " << q.y() << " " - << q.z() << " " << q.w() << endl; - } - - f.close(); -} - -void System::SaveTrajectoryEuRoC(const string &filename) { - oslog::info("Saving trajectory to {} ...", filename); - - /*if(sensorType()==MONOCULAR) - { - cerr << "ERROR: SaveTrajectoryEuRoC cannot be used for monocular." << - endl; return; - }*/ - - vector> vpMaps = mpAtlas->GetAllMaps(); - size_t numMaxKFs = 0; - std::shared_ptr pBiggerMap; - oslog::debug("There are {} maps in the atlas", std::to_string(vpMaps.size())); - for (auto pMap : vpMaps) { - oslog::debug(" Map {} has {} KFs", std::to_string(pMap->GetId()), - std::to_string(pMap->GetAllKeyFrames().size())); - if (pMap->GetAllKeyFrames().size() > numMaxKFs) { - numMaxKFs = pMap->GetAllKeyFrames().size(); - pBiggerMap = pMap; - } - } - - auto vpKFs = pBiggerMap->GetAllKeyFrames(); - if (vpKFs.size() == 0) { - oslog::error("Cannot save EUROC trajectory, there are no Keyframes"); - return; - } - - sort(vpKFs.begin(), vpKFs.end(), KeyFrame::lId); - - // Transform all keyframes so that the first keyframe is at the origin. - // After a loop closure the first keyframe might not be at the origin. - Sophus::SE3f - Twb; // Can be word to cam0 or world to b depending on IMU or not. - if (sensorType().isImu()) { - Twb = vpKFs[0]->GetImuPose(); - } else { - Twb = vpKFs[0]->GetPoseInverse(); - } - - ofstream f; - f.open(filename.c_str()); - f << fixed; - - // Frame pose is stored relative to its reference keyframe (which is optimized - // by BA and pose graph). We need to get first the keyframe pose and then - // concatenate the relative transformation. Frames not localized (tracking - // failure) are not saved. - - // For each frame we have a reference keyframe (lRit), the timestamp (lT) and - // a flag which is true when tracking failed (lbL). - auto lRit = mpTracker->mlpReferences.begin(); - list::iterator lT = mpTracker->mlFrameTimes.begin(); - list::iterator lbL = mpTracker->mlbLost.begin(); - - // cout << "size mlpReferences: " << mpTracker->mlpReferences.size() << endl; - // cout << "size mlRelativeFramePoses: " << - // mpTracker->mlRelativeFramePoses.size() << endl; cout << "size - // mpTracker->mlFrameTimes: " << mpTracker->mlFrameTimes.size() << endl; cout - // << "size mpTracker->mlbLost: " << mpTracker->mlbLost.size() << endl; - - for (auto lit = mpTracker->mlRelativeFramePoses.begin(), - lend = mpTracker->mlRelativeFramePoses.end(); - lit != lend; lit++, lRit++, lT++, lbL++) { - if (*lbL) continue; - - std::shared_ptr pKF = *lRit; - - Sophus::SE3f Trw; - - // If the reference keyframe was culled, traverse the spanning tree to get a - // suitable keyframe. - if (!pKF) continue; - - while (pKF->isBad()) { - // cout << " 2.bad" << endl; - Trw = Trw * pKF->mTcp; - pKF = pKF->GetParent(); - // cout << "--Parent KF: " << pKF->mnId << endl; - } - - if (!pKF || pKF->GetMap() != pBiggerMap) { - // cout << "--Parent KF is from another map" << endl; - continue; - } - - Trw = Trw * pKF->GetPose() * - Twb; // Tcp*Tpw*Twb0=Tcb0 where b0 is the new world reference - - if (sensorType().isImu()) { - Sophus::SE3f Twb = (pKF->mImuCalib.mTbc * (*lit) * Trw).inverse(); - Eigen::Quaternionf q = Twb.unit_quaternion(); - Eigen::Vector3f twb = Twb.translation(); - f << setprecision(6) << 1e9 * (*lT) << " " << setprecision(9) << twb(0) - << " " << twb(1) << " " << twb(2) << " " << q.x() << " " << q.y() << " " - << q.z() << " " << q.w() << endl; - } else { - Sophus::SE3f Twc = ((*lit) * Trw).inverse(); - Eigen::Quaternionf q = Twc.unit_quaternion(); - Eigen::Vector3f twc = Twc.translation(); - f << setprecision(6) << 1e9 * (*lT) << " " << setprecision(9) << twc(0) - << " " << twc(1) << " " << twc(2) << " " << q.x() << " " << q.y() << " " - << q.z() << " " << q.w() << endl; - } - } - f.close(); - oslog::info("End of saving trajectory to {} ...", filename); -} - -void System::SaveTrajectoryEuRoC(const string &filename, - const std::shared_ptr &pMap) { - oslog::info("Saving trajectory of map {} to {} ...", pMap->GetId(), filename); - - /*if(sensorType()==MONOCULAR) - { - cerr << "ERROR: SaveTrajectoryEuRoC cannot be used for monocular." << - endl; return; - }*/ - - auto vpKFs = pMap->GetAllKeyFrames(); - sort(vpKFs.begin(), vpKFs.end(), KeyFrame::lId); - - // Transform all keyframes so that the first keyframe is at the origin. - // After a loop closure the first keyframe might not be at the origin. - Sophus::SE3f - Twb; // Can be word to cam0 or world to b dependingo on IMU or not. - if (sensorType().isImu()) { - Twb = vpKFs[0]->GetImuPose(); - } else { - Twb = vpKFs[0]->GetPoseInverse(); - } - ofstream f; - f.open(filename.c_str()); - f << fixed; - - // Frame pose is stored relative to its reference keyframe (which is optimized - // by BA and pose graph). We need to get first the keyframe pose and then - // concatenate the relative transformation. Frames not localized (tracking - // failure) are not saved. - - // For each frame we have a reference keyframe (lRit), the timestamp (lT) and - // a flag which is true when tracking failed (lbL). - auto lRit = mpTracker->mlpReferences.begin(); - list::iterator lT = mpTracker->mlFrameTimes.begin(); - list::iterator lbL = mpTracker->mlbLost.begin(); - - // cout << "size mlpReferences: " << mpTracker->mlpReferences.size() << endl; - // cout << "size mlRelativeFramePoses: " << - // mpTracker->mlRelativeFramePoses.size() << endl; cout << "size - // mpTracker->mlFrameTimes: " << mpTracker->mlFrameTimes.size() << endl; cout - // << "size mpTracker->mlbLost: " << mpTracker->mlbLost.size() << endl; - - for (auto lit = mpTracker->mlRelativeFramePoses.begin(), - lend = mpTracker->mlRelativeFramePoses.end(); - lit != lend; lit++, lRit++, lT++, lbL++) { - // cout << "1" << endl; - if (*lbL) continue; - - std::shared_ptr pKF = *lRit; - - Sophus::SE3f Trw; - - // If the reference keyframe was culled, traverse the spanning tree to get a - // suitable keyframe. - if (!pKF) continue; - - while (pKF->isBad()) { - Trw = Trw * pKF->mTcp; - pKF = pKF->GetParent(); - // cout << "--Parent KF: " << pKF->mnId << endl; - } - - if (!pKF || pKF->GetMap() != pMap) { - // cout << "--Parent KF is from another map" << endl; - continue; - } - - Trw = Trw * pKF->GetPose() * - Twb; // Tcp*Tpw*Twb0=Tcb0 where b0 is the new world reference - - if (sensorType().isImu()) { - Sophus::SE3f Twb = (pKF->mImuCalib.mTbc * (*lit) * Trw).inverse(); - Eigen::Quaternionf q = Twb.unit_quaternion(); - Eigen::Vector3f twb = Twb.translation(); - f << setprecision(6) << 1e9 * (*lT) << " " << setprecision(9) << twb(0) - << " " << twb(1) << " " << twb(2) << " " << q.x() << " " << q.y() << " " - << q.z() << " " << q.w() << endl; - } else { - Sophus::SE3f Twc = ((*lit) * Trw).inverse(); - Eigen::Quaternionf q = Twc.unit_quaternion(); - Eigen::Vector3f twc = Twc.translation(); - f << setprecision(6) << 1e9 * (*lT) << " " << setprecision(9) << twc(0) - << " " << twc(1) << " " << twc(2) << " " << q.x() << " " << q.y() << " " - << q.z() << " " << q.w() << endl; - } - } - f.close(); - oslog::info("End of saving trajectory to {} ...", filename); -} - -void System::SaveKeyFrameTrajectoryEuRoC(const string &filename) { - oslog::info("Saving keyframe trajectory to {} ...", filename); - - vector> vpMaps = mpAtlas->GetAllMaps(); - std::shared_ptr pBiggerMap; - size_t numMaxKFs = 0; - for (auto pMap : vpMaps) { - if (pMap && pMap->GetAllKeyFrames().size() > numMaxKFs) { - numMaxKFs = pMap->GetAllKeyFrames().size(); - pBiggerMap = pMap; - } - } - - if (!pBiggerMap) { - oslog::error("There is not a map!!"); - return; - } - - auto vpKFs = pBiggerMap->GetAllKeyFrames(); - sort(vpKFs.begin(), vpKFs.end(), KeyFrame::lId); - - // Transform all keyframes so that the first keyframe is at the origin. - // After a loop closure the first keyframe might not be at the origin. - ofstream f; - f.open(filename.c_str()); - f << fixed; - - for (auto pKF : vpKFs) { - // pKF->SetPose(pKF->GetPose()*Two); - - if (!pKF || pKF->isBad()) continue; - if (sensorType().isImu()) { - Sophus::SE3f Twb = pKF->GetImuPose(); - Eigen::Quaternionf q = Twb.unit_quaternion(); - Eigen::Vector3f twb = Twb.translation(); - f << setprecision(6) << 1e9 * pKF->mTimeStamp << " " << setprecision(9) - << twb(0) << " " << twb(1) << " " << twb(2) << " " << q.x() << " " - << q.y() << " " << q.z() << " " << q.w() << endl; - - } else { - Sophus::SE3f Twc = pKF->GetPoseInverse(); - Eigen::Quaternionf q = Twc.unit_quaternion(); - Eigen::Vector3f t = Twc.translation(); - f << setprecision(6) << 1e9 * pKF->mTimeStamp << " " << setprecision(9) - << t(0) << " " << t(1) << " " << t(2) << " " << q.x() << " " << q.y() - << " " << q.z() << " " << q.w() << endl; - } - } - f.close(); -} - -void System::SaveKeyFrameTrajectoryEuRoC(const string &filename, - const std::shared_ptr &pMap) { - oslog::info("Saving keyframe trajectory of map {} to {} ...", pMap->GetId(), - filename); - - auto vpKFs = pMap->GetAllKeyFrames(); - sort(vpKFs.begin(), vpKFs.end(), KeyFrame::lId); - - // Transform all keyframes so that the first keyframe is at the origin. - // After a loop closure the first keyframe might not be at the origin. - ofstream f; - f.open(filename.c_str()); - f << fixed; - - for (auto pKF : vpKFs) { - if (!pKF || pKF->isBad()) continue; - - if (sensorType().isImu()) { - Sophus::SE3f Twb = pKF->GetImuPose(); - Eigen::Quaternionf q = Twb.unit_quaternion(); - Eigen::Vector3f twb = Twb.translation(); - f << setprecision(6) << 1e9 * pKF->mTimeStamp << " " << setprecision(9) - << twb(0) << " " << twb(1) << " " << twb(2) << " " << q.x() << " " - << q.y() << " " << q.z() << " " << q.w() << endl; - } else { - Sophus::SE3f Twc = pKF->GetPoseInverse(); - Eigen::Quaternionf q = Twc.unit_quaternion(); - Eigen::Vector3f t = Twc.translation(); - f << setprecision(6) << 1e9 * pKF->mTimeStamp << " " << setprecision(9) - << t(0) << " " << t(1) << " " << t(2) << " " << q.x() << " " << q.y() - << " " << q.z() << " " << q.w() << endl; - } - } - - f.close(); -} - -void System::SaveTrajectoryKITTI(const string &filename) { - oslog::info("Saving camera trajectory to {} ...", filename); - if (sensorType() == SensorType::MONOCULAR) { - oslog::error("ERROR: SaveTrajectoryKITTI cannot be used for monocular."); - return; - } - - auto vpKFs = mpAtlas->GetAllKeyFrames(); - sort(vpKFs.begin(), vpKFs.end(), KeyFrame::lId); - - // Transform all keyframes so that the first keyframe is at the origin. - // After a loop closure the first keyframe might not be at the origin. - Sophus::SE3f Tow = vpKFs[0]->GetPoseInverse(); - - ofstream f; - f.open(filename.c_str()); - f << fixed; - - // Frame pose is stored relative to its reference keyframe (which is optimized - // by BA and pose graph). We need to get first the keyframe pose and then - // concatenate the relative transformation. Frames not localized (tracking - // failure) are not saved. - - // For each frame we have a reference keyframe (lRit), the timestamp (lT) and - // a flag which is true when tracking failed (lbL). - auto lRit = mpTracker->mlpReferences.begin(); - list::iterator lT = mpTracker->mlFrameTimes.begin(); - for (list::iterator - lit = mpTracker->mlRelativeFramePoses.begin(), - lend = mpTracker->mlRelativeFramePoses.end(); - lit != lend; lit++, lRit++, lT++) { - auto pKF = *lRit; - - Sophus::SE3f Trw; - - if (!pKF) continue; - - while (pKF->isBad()) { - Trw = Trw * pKF->mTcp; - pKF = pKF->GetParent(); - } - - Trw = Trw * pKF->GetPose() * Tow; - - Sophus::SE3f Tcw = (*lit) * Trw; - Sophus::SE3f Twc = Tcw.inverse(); - Eigen::Matrix3f Rwc = Twc.rotationMatrix(); - Eigen::Vector3f twc = Twc.translation(); - - f << setprecision(9) << Rwc(0, 0) << " " << Rwc(0, 1) << " " << Rwc(0, 2) - << " " << twc(0) << " " << Rwc(1, 0) << " " << Rwc(1, 1) << " " - << Rwc(1, 2) << " " << twc(1) << " " << Rwc(2, 0) << " " << Rwc(2, 1) - << " " << Rwc(2, 2) << " " << twc(2) << endl; - } - f.close(); -} - -void System::SaveDebugData(const int &initIdx) { - // 0. Save initialization trajectory - SaveTrajectoryEuRoC("init_FrameTrajectoy_" + - to_string(mpLocalMapper->mInitSect) + "_" + - to_string(initIdx) + ".txt"); - - // 1. Save scale - ofstream f; - f.open("init_Scale_" + to_string(mpLocalMapper->mInitSect) + ".txt", - ios_base::app); - f << fixed; - f << mpLocalMapper->mScale << endl; - f.close(); - - // 2. Save gravity direction - f.open("init_GDir_" + to_string(mpLocalMapper->mInitSect) + ".txt", - ios_base::app); - f << fixed; - f << mpLocalMapper->mRwg(0, 0) << "," << mpLocalMapper->mRwg(0, 1) << "," - << mpLocalMapper->mRwg(0, 2) << endl; - f << mpLocalMapper->mRwg(1, 0) << "," << mpLocalMapper->mRwg(1, 1) << "," - << mpLocalMapper->mRwg(1, 2) << endl; - f << mpLocalMapper->mRwg(2, 0) << "," << mpLocalMapper->mRwg(2, 1) << "," - << mpLocalMapper->mRwg(2, 2) << endl; - f.close(); - - // 3. Save computational cost - f.open("init_CompCost_" + to_string(mpLocalMapper->mInitSect) + ".txt", - ios_base::app); - f << fixed; - f << mpLocalMapper->mCostTime << endl; - f.close(); - - // 4. Save biases - f.open("init_Biases_" + to_string(mpLocalMapper->mInitSect) + ".txt", - ios_base::app); - f << fixed; - f << mpLocalMapper->mbg(0) << "," << mpLocalMapper->mbg(1) << "," - << mpLocalMapper->mbg(2) << endl; - f << mpLocalMapper->mba(0) << "," << mpLocalMapper->mba(1) << "," - << mpLocalMapper->mba(2) << endl; - f.close(); - - // 5. Save covariance matrix - f.open("init_CovMatrix_" + to_string(mpLocalMapper->mInitSect) + "_" + - to_string(initIdx) + ".txt", - ios_base::app); - f << fixed; - for (int i = 0; i < mpLocalMapper->mcovInertial.rows(); i++) { - for (int j = 0; j < mpLocalMapper->mcovInertial.cols(); j++) { - if (j != 0) f << ","; - f << setprecision(15) << mpLocalMapper->mcovInertial(i, j); - } - f << endl; - } - f.close(); - - // 6. Save initialization time - f.open("init_Time_" + to_string(mpLocalMapper->mInitSect) + ".txt", - ios_base::app); - f << fixed; - f << mpLocalMapper->mInitTime << endl; - f.close(); -} - int System::GetTrackingState() { unique_lock lock(mMutexState); return mTrackingState; @@ -1075,156 +571,4 @@ void System::InsertTrackTime(double &time) { } #endif -void System::SaveAtlas(int type) { - const string mStrSaveAtlasToFile = settings_->atlasSaveFile(); - - if (!mStrSaveAtlasToFile.empty()) { - // clock_t start = clock(); - - // Save the current session - mpAtlas->PreSave(); - - string pathSaveFileName = "./"; - pathSaveFileName = pathSaveFileName.append(mStrSaveAtlasToFile); - pathSaveFileName = pathSaveFileName.append(".osa"); - - const string vocabularyFilePath = settings_->strVocFile_; - - string strVocabularyChecksum = - CalculateCheckSum(vocabularyFilePath, TEXT_FILE); - std::size_t found = vocabularyFilePath.find_last_of("/\\"); - string strVocabularyName = vocabularyFilePath.substr(found + 1); - - if (type == TEXT_FILE) { - // File text - - oslog::debug("Starting to write the save text file "); - std::remove(pathSaveFileName.c_str()); - std::ofstream ofs(pathSaveFileName, std::ios::binary); - boost::archive::text_oarchive oa(ofs); - - oa << strVocabularyName; - oa << strVocabularyChecksum; - oa << mpAtlas; - oslog::debug("End to write the save text file"); - } else if (type == BINARY_FILE) { - // File binary - - oslog::debug("Starting to write the save binary file"); - std::remove(pathSaveFileName.c_str()); - std::ofstream ofs(pathSaveFileName, std::ios::binary); - boost::archive::binary_oarchive oa(ofs); - oa << strVocabularyName; - oa << strVocabularyChecksum; - oa << mpAtlas; - oslog::debug("End to write save binary file"); - } - } -} - -bool System::LoadAtlas(int type) { - string strFileVoc, strVocChecksum; - - const string mStrLoadAtlasFromFile = settings_->atlasLoadFile(); - const string vocabularyFilePath = settings_->strVocFile_; - bool isRead = false; - - string pathLoadFileName = "./"; - pathLoadFileName = pathLoadFileName.append(mStrLoadAtlasFromFile); - pathLoadFileName = pathLoadFileName.append(".osa"); - - if (type == TEXT_FILE) { - // File text - oslog::debug("Starting to read the save text file "); - std::ifstream ifs(pathLoadFileName, std::ios::binary); - if (!ifs.good()) { - oslog::error("Cannot find Atlas file {}", pathLoadFileName); - return false; - } - boost::archive::text_iarchive ia(ifs); - ia >> strFileVoc; - ia >> strVocChecksum; - ia >> mpAtlas; - - oslog::debug("Finished loading the saved text file "); - isRead = true; - } else if (type == BINARY_FILE) { - // File binary - oslog::debug("Starting to read the save binary file"); - std::ifstream ifs(pathLoadFileName, std::ios::binary); - if (!ifs.good()) { - oslog::error("Cannot find Atlas file {}", pathLoadFileName); - return false; - } - boost::archive::binary_iarchive ia(ifs); - ia >> strFileVoc; - ia >> strVocChecksum; - ia >> mpAtlas; - - oslog::debug("Finished loading the saved binary file"); - isRead = true; - } - - if (!mpAtlas) { - throw std::runtime_error("mpAtlas not initialized when it should be"); - } - - if (isRead) { - // Check if the vocabulary is the same - string strInputVocabularyChecksum = - CalculateCheckSum(vocabularyFilePath, TEXT_FILE); - - if (strInputVocabularyChecksum.compare(strVocChecksum) != 0) { - oslog::warn( - "The vocabulary load isn't the same which the load session was " - "created. Loading vocab file {}", - strFileVoc); - return false; // Both are differents - } - - mpAtlas->SetKeyFrameDababase(mpKeyFrameDatabase); - mpAtlas->SetORBVocabulary(mpVocabulary); - mpAtlas->PostLoad(); - - return true; - } - return false; -} - -string System::CalculateCheckSum(string filename, int type) { - string checksum = ""; - - unsigned char c[MD5_DIGEST_LENGTH]; - - std::ios_base::openmode flags = std::ios::in; - if (type == BINARY_FILE) // Binary file - flags = std::ios::in | std::ios::binary; - - ifstream f(filename.c_str(), flags); - if (!f.is_open()) { - oslog::error("[E] Unable to open the in file {} for Md5 hash.", filename); - return checksum; - } - - 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]; - snprintf(aux, sizeof(aux), "%02x", c[i]); - checksum = checksum + aux; - } - - return checksum; -} - } // namespace ORB_SLAM3 diff --git a/src/System/Atlas.cc b/src/System/Atlas.cc new file mode 100644 index 00000000000..260e27b6e00 --- /dev/null +++ b/src/System/Atlas.cc @@ -0,0 +1,166 @@ +/** + * 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 . + */ + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "Converter.h" +#include "System.h" +#include "Utils/Checksum.h" + +namespace ORB_SLAM3 { + +void System::SaveAtlas(FileType type) { + const string mStrSaveAtlasToFile = settings_->atlasSaveFile(); + + if (!mStrSaveAtlasToFile.empty()) { + // clock_t start = clock(); + + // Save the current session + mpAtlas->PreSave(); + + string pathSaveFileName = "./"; + pathSaveFileName = pathSaveFileName.append(mStrSaveAtlasToFile); + pathSaveFileName = pathSaveFileName.append(".osa"); + + const string vocabularyFilePath = settings_->strVocFile_; + + string strVocabularyChecksum = + Checksum::Calculate(vocabularyFilePath, FileType::TEXT_FILE); + std::size_t found = vocabularyFilePath.find_last_of("/\\"); + string strVocabularyName = vocabularyFilePath.substr(found + 1); + + if (type == FileType::TEXT_FILE) { + // File text + + oslog::debug("Starting to write the save text file "); + std::remove(pathSaveFileName.c_str()); + std::ofstream ofs(pathSaveFileName, std::ios::binary); + boost::archive::text_oarchive oa(ofs); + + oa << strVocabularyName; + oa << strVocabularyChecksum; + oa << mpAtlas; + oslog::debug("End to write the save text file"); + } else if (type == FileType::BINARY_FILE) { + // File binary + + oslog::debug("Starting to write the save binary file"); + std::remove(pathSaveFileName.c_str()); + std::ofstream ofs(pathSaveFileName, std::ios::binary); + boost::archive::binary_oarchive oa(ofs); + oa << strVocabularyName; + oa << strVocabularyChecksum; + oa << mpAtlas; + oslog::debug("End to write save binary file"); + } + } +} + +bool System::LoadAtlas(FileType type) { + string strFileVoc, strVocChecksum; + + const string mStrLoadAtlasFromFile = settings_->atlasLoadFile(); + const string vocabularyFilePath = settings_->strVocFile_; + bool isRead = false; + + string pathLoadFileName = "./"; + pathLoadFileName = pathLoadFileName.append(mStrLoadAtlasFromFile); + pathLoadFileName = pathLoadFileName.append(".osa"); + + if (type == FileType::TEXT_FILE) { + // File text + oslog::debug("Starting to read the save text file "); + std::ifstream ifs(pathLoadFileName, std::ios::binary); + if (!ifs.good()) { + oslog::error("Cannot find Atlas file {}", pathLoadFileName); + return false; + } + boost::archive::text_iarchive ia(ifs); + ia >> strFileVoc; + ia >> strVocChecksum; + ia >> mpAtlas; + + oslog::debug("Finished loading the saved text file "); + isRead = true; + } else if (type == FileType::BINARY_FILE) { + // File binary + oslog::debug("Starting to read the save binary file"); + std::ifstream ifs(pathLoadFileName, std::ios::binary); + if (!ifs.good()) { + oslog::error("Cannot find Atlas file {}", pathLoadFileName); + return false; + } + boost::archive::binary_iarchive ia(ifs); + ia >> strFileVoc; + ia >> strVocChecksum; + ia >> mpAtlas; + + oslog::debug("Finished loading the saved binary file"); + isRead = true; + } + + if (!mpAtlas) { + throw std::runtime_error("mpAtlas not initialized when it should be"); + } + + if (isRead) { + // Check if the vocabulary is the same + string strInputVocabularyChecksum = + Checksum::Calculate(vocabularyFilePath, FileType::TEXT_FILE); + + if (strInputVocabularyChecksum.compare(strVocChecksum) != 0) { + oslog::warn( + "The vocabulary load isn't the same which the load session was " + "created. Loading vocab file {}", + strFileVoc); + return false; // Both are differents + } + + mpAtlas->SetKeyFrameDababase(mpKeyFrameDatabase); + mpAtlas->SetORBVocabulary(mpVocabulary); + mpAtlas->PostLoad(); + + return true; + } + return false; +} + +} // namespace ORB_SLAM3 diff --git a/src/System/Exporters.cc b/src/System/Exporters.cc new file mode 100644 index 00000000000..0dd337aa8dc --- /dev/null +++ b/src/System/Exporters.cc @@ -0,0 +1,553 @@ +/** + * 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 . + */ + +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "Converter.h" +#include "System.h" + +namespace ORB_SLAM3 { + +void System::SaveTrajectoryTUM(const string &filename) { + vector> vpKFs = mpAtlas->GetAllKeyFrames(); + + if (vpKFs.size() == 0) { + oslog::error("Cannot save TUM trajectory, there are no Keyframes"); + return; + } + + oslog::info("Saving camera trajectory to {} ...", filename); + if (sensorType() == SensorType::MONOCULAR) { + cerr << "ERROR: SaveTrajectoryTUM cannot be used for monocular." << endl; + return; + } + + sort(vpKFs.begin(), vpKFs.end(), KeyFrame::lId); + + // Transform all keyframes so that the first keyframe is at the origin. + // After a loop closure the first keyframe might not be at the origin. + Sophus::SE3f Two = vpKFs[0]->GetPoseInverse(); + + ofstream f; + f.open(filename.c_str()); + f << fixed; + + // Frame pose is stored relative to its reference keyframe (which is optimized + // by BA and pose graph). We need to get first the keyframe pose and then + // concatenate the relative transformation. Frames not localized (tracking + // failure) are not saved. + + // For each frame we have a reference keyframe (lRit), the timestamp (lT) and + // a flag which is true when tracking failed (lbL). + list>::iterator lRit = + mpTracker->mlpReferences.begin(); + list::iterator lT = mpTracker->mlFrameTimes.begin(); + list::iterator lbL = mpTracker->mlbLost.begin(); + for (list::iterator + lit = mpTracker->mlRelativeFramePoses.begin(), + lend = mpTracker->mlRelativeFramePoses.end(); + lit != lend; lit++, lRit++, lT++, lbL++) { + if (*lbL) continue; + + std::shared_ptr pKF = *lRit; + + Sophus::SE3f Trw; + + // If the reference keyframe was culled, traverse the spanning tree to get a + // suitable keyframe. + while (pKF->isBad()) { + Trw = Trw * pKF->mTcp; + pKF = pKF->GetParent(); + } + + Trw = Trw * pKF->GetPose() * Two; + + Sophus::SE3f Tcw = (*lit) * Trw; + Sophus::SE3f Twc = Tcw.inverse(); + + Eigen::Vector3f twc = Twc.translation(); + Eigen::Quaternionf q = Twc.unit_quaternion(); + + f << setprecision(6) << *lT << " " << setprecision(9) << twc(0) << " " + << twc(1) << " " << twc(2) << " " << q.x() << " " << q.y() << " " << q.z() + << " " << q.w() << endl; + } + f.close(); +} + +void System::SaveKeyFrameTrajectoryTUM(const string &filename) { + oslog::info("Saving keyframe trajectory to {} ...", filename); + + vector> vpKFs = mpAtlas->GetAllKeyFrames(); + sort(vpKFs.begin(), vpKFs.end(), KeyFrame::lId); + + // Transform all keyframes so that the first keyframe is at the origin. + // After a loop closure the first keyframe might not be at the origin. + ofstream f; + f.open(filename.c_str()); + f << fixed; + + for (auto pKF : vpKFs) { + if (pKF->isBad()) continue; + + Sophus::SE3f Twc = pKF->GetPoseInverse(); + Eigen::Quaternionf q = Twc.unit_quaternion(); + Eigen::Vector3f t = Twc.translation(); + f << setprecision(6) << pKF->mTimeStamp << setprecision(7) << " " << t(0) + << " " << t(1) << " " << t(2) << " " << q.x() << " " << q.y() << " " + << q.z() << " " << q.w() << endl; + } + + f.close(); +} + +void System::SaveTrajectoryEuRoC(const string &filename) { + oslog::info("Saving trajectory to {} ...", filename); + + /*if(sensorType()==MONOCULAR) + { + cerr << "ERROR: SaveTrajectoryEuRoC cannot be used for monocular." << + endl; return; + }*/ + + vector> vpMaps = mpAtlas->GetAllMaps(); + size_t numMaxKFs = 0; + std::shared_ptr pBiggerMap; + oslog::debug("There are {} maps in the atlas", std::to_string(vpMaps.size())); + for (auto pMap : vpMaps) { + oslog::debug(" Map {} has {} KFs", std::to_string(pMap->GetId()), + std::to_string(pMap->GetAllKeyFrames().size())); + if (pMap->GetAllKeyFrames().size() > numMaxKFs) { + numMaxKFs = pMap->GetAllKeyFrames().size(); + pBiggerMap = pMap; + } + } + + auto vpKFs = pBiggerMap->GetAllKeyFrames(); + if (vpKFs.size() == 0) { + oslog::error("Cannot save EUROC trajectory, there are no Keyframes"); + return; + } + + sort(vpKFs.begin(), vpKFs.end(), KeyFrame::lId); + + // Transform all keyframes so that the first keyframe is at the origin. + // After a loop closure the first keyframe might not be at the origin. + Sophus::SE3f + Twb; // Can be word to cam0 or world to b depending on IMU or not. + if (sensorType().isImu()) { + Twb = vpKFs[0]->GetImuPose(); + } else { + Twb = vpKFs[0]->GetPoseInverse(); + } + + ofstream f; + f.open(filename.c_str()); + f << fixed; + + // Frame pose is stored relative to its reference keyframe (which is optimized + // by BA and pose graph). We need to get first the keyframe pose and then + // concatenate the relative transformation. Frames not localized (tracking + // failure) are not saved. + + // For each frame we have a reference keyframe (lRit), the timestamp (lT) and + // a flag which is true when tracking failed (lbL). + auto lRit = mpTracker->mlpReferences.begin(); + list::iterator lT = mpTracker->mlFrameTimes.begin(); + list::iterator lbL = mpTracker->mlbLost.begin(); + + // cout << "size mlpReferences: " << mpTracker->mlpReferences.size() << endl; + // cout << "size mlRelativeFramePoses: " << + // mpTracker->mlRelativeFramePoses.size() << endl; cout << "size + // mpTracker->mlFrameTimes: " << mpTracker->mlFrameTimes.size() << endl; cout + // << "size mpTracker->mlbLost: " << mpTracker->mlbLost.size() << endl; + + for (auto lit = mpTracker->mlRelativeFramePoses.begin(), + lend = mpTracker->mlRelativeFramePoses.end(); + lit != lend; lit++, lRit++, lT++, lbL++) { + if (*lbL) continue; + + std::shared_ptr pKF = *lRit; + + Sophus::SE3f Trw; + + // If the reference keyframe was culled, traverse the spanning tree to get a + // suitable keyframe. + if (!pKF) continue; + + while (pKF->isBad()) { + // cout << " 2.bad" << endl; + Trw = Trw * pKF->mTcp; + pKF = pKF->GetParent(); + // cout << "--Parent KF: " << pKF->mnId << endl; + } + + if (!pKF || pKF->GetMap() != pBiggerMap) { + // cout << "--Parent KF is from another map" << endl; + continue; + } + + Trw = Trw * pKF->GetPose() * + Twb; // Tcp*Tpw*Twb0=Tcb0 where b0 is the new world reference + + if (sensorType().isImu()) { + Sophus::SE3f Twb = (pKF->mImuCalib.mTbc * (*lit) * Trw).inverse(); + Eigen::Quaternionf q = Twb.unit_quaternion(); + Eigen::Vector3f twb = Twb.translation(); + f << setprecision(6) << 1e9 * (*lT) << " " << setprecision(9) << twb(0) + << " " << twb(1) << " " << twb(2) << " " << q.x() << " " << q.y() << " " + << q.z() << " " << q.w() << endl; + } else { + Sophus::SE3f Twc = ((*lit) * Trw).inverse(); + Eigen::Quaternionf q = Twc.unit_quaternion(); + Eigen::Vector3f twc = Twc.translation(); + f << setprecision(6) << 1e9 * (*lT) << " " << setprecision(9) << twc(0) + << " " << twc(1) << " " << twc(2) << " " << q.x() << " " << q.y() << " " + << q.z() << " " << q.w() << endl; + } + } + f.close(); + oslog::info("End of saving trajectory to {} ...", filename); +} + +void System::SaveTrajectoryEuRoC(const string &filename, + const std::shared_ptr &pMap) { + oslog::info("Saving trajectory of map {} to {} ...", pMap->GetId(), filename); + + /*if(sensorType()==MONOCULAR) + { + cerr << "ERROR: SaveTrajectoryEuRoC cannot be used for monocular." << + endl; return; + }*/ + + auto vpKFs = pMap->GetAllKeyFrames(); + sort(vpKFs.begin(), vpKFs.end(), KeyFrame::lId); + + // Transform all keyframes so that the first keyframe is at the origin. + // After a loop closure the first keyframe might not be at the origin. + Sophus::SE3f + Twb; // Can be word to cam0 or world to b dependingo on IMU or not. + if (sensorType().isImu()) { + Twb = vpKFs[0]->GetImuPose(); + } else { + Twb = vpKFs[0]->GetPoseInverse(); + } + ofstream f; + f.open(filename.c_str()); + f << fixed; + + // Frame pose is stored relative to its reference keyframe (which is optimized + // by BA and pose graph). We need to get first the keyframe pose and then + // concatenate the relative transformation. Frames not localized (tracking + // failure) are not saved. + + // For each frame we have a reference keyframe (lRit), the timestamp (lT) and + // a flag which is true when tracking failed (lbL). + auto lRit = mpTracker->mlpReferences.begin(); + list::iterator lT = mpTracker->mlFrameTimes.begin(); + list::iterator lbL = mpTracker->mlbLost.begin(); + + // cout << "size mlpReferences: " << mpTracker->mlpReferences.size() << endl; + // cout << "size mlRelativeFramePoses: " << + // mpTracker->mlRelativeFramePoses.size() << endl; cout << "size + // mpTracker->mlFrameTimes: " << mpTracker->mlFrameTimes.size() << endl; cout + // << "size mpTracker->mlbLost: " << mpTracker->mlbLost.size() << endl; + + for (auto lit = mpTracker->mlRelativeFramePoses.begin(), + lend = mpTracker->mlRelativeFramePoses.end(); + lit != lend; lit++, lRit++, lT++, lbL++) { + // cout << "1" << endl; + if (*lbL) continue; + + std::shared_ptr pKF = *lRit; + + Sophus::SE3f Trw; + + // If the reference keyframe was culled, traverse the spanning tree to get a + // suitable keyframe. + if (!pKF) continue; + + while (pKF->isBad()) { + Trw = Trw * pKF->mTcp; + pKF = pKF->GetParent(); + // cout << "--Parent KF: " << pKF->mnId << endl; + } + + if (!pKF || pKF->GetMap() != pMap) { + // cout << "--Parent KF is from another map" << endl; + continue; + } + + Trw = Trw * pKF->GetPose() * + Twb; // Tcp*Tpw*Twb0=Tcb0 where b0 is the new world reference + + if (sensorType().isImu()) { + Sophus::SE3f Twb = (pKF->mImuCalib.mTbc * (*lit) * Trw).inverse(); + Eigen::Quaternionf q = Twb.unit_quaternion(); + Eigen::Vector3f twb = Twb.translation(); + f << setprecision(6) << 1e9 * (*lT) << " " << setprecision(9) << twb(0) + << " " << twb(1) << " " << twb(2) << " " << q.x() << " " << q.y() << " " + << q.z() << " " << q.w() << endl; + } else { + Sophus::SE3f Twc = ((*lit) * Trw).inverse(); + Eigen::Quaternionf q = Twc.unit_quaternion(); + Eigen::Vector3f twc = Twc.translation(); + f << setprecision(6) << 1e9 * (*lT) << " " << setprecision(9) << twc(0) + << " " << twc(1) << " " << twc(2) << " " << q.x() << " " << q.y() << " " + << q.z() << " " << q.w() << endl; + } + } + f.close(); + oslog::info("End of saving trajectory to {} ...", filename); +} + +void System::SaveKeyFrameTrajectoryEuRoC(const string &filename) { + oslog::info("Saving keyframe trajectory to {} ...", filename); + + vector> vpMaps = mpAtlas->GetAllMaps(); + std::shared_ptr pBiggerMap; + size_t numMaxKFs = 0; + for (auto pMap : vpMaps) { + if (pMap && pMap->GetAllKeyFrames().size() > numMaxKFs) { + numMaxKFs = pMap->GetAllKeyFrames().size(); + pBiggerMap = pMap; + } + } + + if (!pBiggerMap) { + oslog::error("There is not a map!!"); + return; + } + + auto vpKFs = pBiggerMap->GetAllKeyFrames(); + sort(vpKFs.begin(), vpKFs.end(), KeyFrame::lId); + + // Transform all keyframes so that the first keyframe is at the origin. + // After a loop closure the first keyframe might not be at the origin. + ofstream f; + f.open(filename.c_str()); + f << fixed; + + for (auto pKF : vpKFs) { + // pKF->SetPose(pKF->GetPose()*Two); + + if (!pKF || pKF->isBad()) continue; + if (sensorType().isImu()) { + Sophus::SE3f Twb = pKF->GetImuPose(); + Eigen::Quaternionf q = Twb.unit_quaternion(); + Eigen::Vector3f twb = Twb.translation(); + f << setprecision(6) << 1e9 * pKF->mTimeStamp << " " << setprecision(9) + << twb(0) << " " << twb(1) << " " << twb(2) << " " << q.x() << " " + << q.y() << " " << q.z() << " " << q.w() << endl; + + } else { + Sophus::SE3f Twc = pKF->GetPoseInverse(); + Eigen::Quaternionf q = Twc.unit_quaternion(); + Eigen::Vector3f t = Twc.translation(); + f << setprecision(6) << 1e9 * pKF->mTimeStamp << " " << setprecision(9) + << t(0) << " " << t(1) << " " << t(2) << " " << q.x() << " " << q.y() + << " " << q.z() << " " << q.w() << endl; + } + } + f.close(); +} + +void System::SaveKeyFrameTrajectoryEuRoC(const string &filename, + const std::shared_ptr &pMap) { + oslog::info("Saving keyframe trajectory of map {} to {} ...", pMap->GetId(), + filename); + + auto vpKFs = pMap->GetAllKeyFrames(); + sort(vpKFs.begin(), vpKFs.end(), KeyFrame::lId); + + // Transform all keyframes so that the first keyframe is at the origin. + // After a loop closure the first keyframe might not be at the origin. + ofstream f; + f.open(filename.c_str()); + f << fixed; + + for (auto pKF : vpKFs) { + if (!pKF || pKF->isBad()) continue; + + if (sensorType().isImu()) { + Sophus::SE3f Twb = pKF->GetImuPose(); + Eigen::Quaternionf q = Twb.unit_quaternion(); + Eigen::Vector3f twb = Twb.translation(); + f << setprecision(6) << 1e9 * pKF->mTimeStamp << " " << setprecision(9) + << twb(0) << " " << twb(1) << " " << twb(2) << " " << q.x() << " " + << q.y() << " " << q.z() << " " << q.w() << endl; + } else { + Sophus::SE3f Twc = pKF->GetPoseInverse(); + Eigen::Quaternionf q = Twc.unit_quaternion(); + Eigen::Vector3f t = Twc.translation(); + f << setprecision(6) << 1e9 * pKF->mTimeStamp << " " << setprecision(9) + << t(0) << " " << t(1) << " " << t(2) << " " << q.x() << " " << q.y() + << " " << q.z() << " " << q.w() << endl; + } + } + + f.close(); +} + +void System::SaveTrajectoryKITTI(const string &filename) { + oslog::info("Saving camera trajectory to {} ...", filename); + if (sensorType() == SensorType::MONOCULAR) { + oslog::error("ERROR: SaveTrajectoryKITTI cannot be used for monocular."); + return; + } + + auto vpKFs = mpAtlas->GetAllKeyFrames(); + sort(vpKFs.begin(), vpKFs.end(), KeyFrame::lId); + + // Transform all keyframes so that the first keyframe is at the origin. + // After a loop closure the first keyframe might not be at the origin. + Sophus::SE3f Tow = vpKFs[0]->GetPoseInverse(); + + ofstream f; + f.open(filename.c_str()); + f << fixed; + + // Frame pose is stored relative to its reference keyframe (which is optimized + // by BA and pose graph). We need to get first the keyframe pose and then + // concatenate the relative transformation. Frames not localized (tracking + // failure) are not saved. + + // For each frame we have a reference keyframe (lRit), the timestamp (lT) and + // a flag which is true when tracking failed (lbL). + auto lRit = mpTracker->mlpReferences.begin(); + list::iterator lT = mpTracker->mlFrameTimes.begin(); + for (list::iterator + lit = mpTracker->mlRelativeFramePoses.begin(), + lend = mpTracker->mlRelativeFramePoses.end(); + lit != lend; lit++, lRit++, lT++) { + auto pKF = *lRit; + + Sophus::SE3f Trw; + + if (!pKF) continue; + + while (pKF->isBad()) { + Trw = Trw * pKF->mTcp; + pKF = pKF->GetParent(); + } + + Trw = Trw * pKF->GetPose() * Tow; + + Sophus::SE3f Tcw = (*lit) * Trw; + Sophus::SE3f Twc = Tcw.inverse(); + Eigen::Matrix3f Rwc = Twc.rotationMatrix(); + Eigen::Vector3f twc = Twc.translation(); + + f << setprecision(9) << Rwc(0, 0) << " " << Rwc(0, 1) << " " << Rwc(0, 2) + << " " << twc(0) << " " << Rwc(1, 0) << " " << Rwc(1, 1) << " " + << Rwc(1, 2) << " " << twc(1) << " " << Rwc(2, 0) << " " << Rwc(2, 1) + << " " << Rwc(2, 2) << " " << twc(2) << endl; + } + f.close(); +} + +void System::SaveDebugData(const int &initIdx) { + // 0. Save initialization trajectory + SaveTrajectoryEuRoC("init_FrameTrajectoy_" + + to_string(mpLocalMapper->mInitSect) + "_" + + to_string(initIdx) + ".txt"); + + // 1. Save scale + ofstream f; + f.open("init_Scale_" + to_string(mpLocalMapper->mInitSect) + ".txt", + ios_base::app); + f << fixed; + f << mpLocalMapper->mScale << endl; + f.close(); + + // 2. Save gravity direction + f.open("init_GDir_" + to_string(mpLocalMapper->mInitSect) + ".txt", + ios_base::app); + f << fixed; + f << mpLocalMapper->mRwg(0, 0) << "," << mpLocalMapper->mRwg(0, 1) << "," + << mpLocalMapper->mRwg(0, 2) << endl; + f << mpLocalMapper->mRwg(1, 0) << "," << mpLocalMapper->mRwg(1, 1) << "," + << mpLocalMapper->mRwg(1, 2) << endl; + f << mpLocalMapper->mRwg(2, 0) << "," << mpLocalMapper->mRwg(2, 1) << "," + << mpLocalMapper->mRwg(2, 2) << endl; + f.close(); + + // 3. Save computational cost + f.open("init_CompCost_" + to_string(mpLocalMapper->mInitSect) + ".txt", + ios_base::app); + f << fixed; + f << mpLocalMapper->mCostTime << endl; + f.close(); + + // 4. Save biases + f.open("init_Biases_" + to_string(mpLocalMapper->mInitSect) + ".txt", + ios_base::app); + f << fixed; + f << mpLocalMapper->mbg(0) << "," << mpLocalMapper->mbg(1) << "," + << mpLocalMapper->mbg(2) << endl; + f << mpLocalMapper->mba(0) << "," << mpLocalMapper->mba(1) << "," + << mpLocalMapper->mba(2) << endl; + f.close(); + + // 5. Save covariance matrix + f.open("init_CovMatrix_" + to_string(mpLocalMapper->mInitSect) + "_" + + to_string(initIdx) + ".txt", + ios_base::app); + f << fixed; + for (int i = 0; i < mpLocalMapper->mcovInertial.rows(); i++) { + for (int j = 0; j < mpLocalMapper->mcovInertial.cols(); j++) { + if (j != 0) f << ","; + f << setprecision(15) << mpLocalMapper->mcovInertial(i, j); + } + f << endl; + } + f.close(); + + // 6. Save initialization time + f.open("init_Time_" + to_string(mpLocalMapper->mInitSect) + ".txt", + ios_base::app); + f << fixed; + f << mpLocalMapper->mInitTime << endl; + f.close(); +} + +} // namespace ORB_SLAM3 diff --git a/src/Utils/Checksum.cc b/src/Utils/Checksum.cc new file mode 100644 index 00000000000..b9abc2229a7 --- /dev/null +++ b/src/Utils/Checksum.cc @@ -0,0 +1,78 @@ +/** + * 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 . + */ + +#include "Utils/Checksum.h" + +#include + +#include +#include +#include +#include + +#include "Logging.h" +#include "Types.h" + +namespace ORB_SLAM3 { + +namespace Checksum { + +using std::string; + +string Calculate(string filename, FileType type) { + string checksum = ""; + + unsigned char c[MD5_DIGEST_LENGTH]; + + std::ios_base::openmode flags = std::ios::in; + if (type == FileType::BINARY_FILE) // Binary file + flags = std::ios::in | std::ios::binary; + + std::ifstream f(filename.c_str(), flags); + if (!f.is_open()) { + oslog::error("[E] Unable to open the in file {} for Md5 hash.", filename); + return checksum; + } + + 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]; + snprintf(aux, sizeof(aux), "%02x", c[i]); + checksum = checksum + aux; + } + + return checksum; +} + +} // namespace Checksum + +} // namespace ORB_SLAM3 From e37b66b97dd7de6789b0a570e6020deebf050de3 Mon Sep 17 00:00:00 2001 From: Aaron Marburg Date: Sun, 15 Feb 2026 15:56:02 +0000 Subject: [PATCH 2/4] Don't serialize mObservations, boost can't handle it --- include/MapPoint.h | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/include/MapPoint.h b/include/MapPoint.h index 11674468de6..463afa38e8d 100644 --- a/include/MapPoint.h +++ b/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); From e5d2ec9e6e60c7b7c81f93078207fb146104bfad Mon Sep 17 00:00:00 2001 From: Aaron Marburg Date: Sun, 15 Feb 2026 17:38:08 +0000 Subject: [PATCH 3/4] Use InputArray, move SystemFactory to its own file. --- CMakeLists.txt | 1 + include/System.h | 27 ++---------- include/System/Factory.h | 51 ++++++++++++++++++++++ src/System.cc | 91 ++++++++++------------------------------ src/System/Factory.cc | 80 +++++++++++++++++++++++++++++++++++ 5 files changed, 158 insertions(+), 92 deletions(-) create mode 100644 include/System/Factory.h create mode 100644 src/System/Factory.cc diff --git a/CMakeLists.txt b/CMakeLists.txt index 25109a208e5..429a80aa3ae 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -98,6 +98,7 @@ add_library(${PROJECT_NAME} SHARED 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 diff --git a/include/System.h b/include/System.h index 175c82f19c0..fc6654ef570 100644 --- a/include/System.h +++ b/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, ExpectedError> Expected; - - static Expected create(const std::shared_ptr &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 @@ -90,7 +72,7 @@ class System : public std::enable_shared_from_this { // 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 ×tamp, + cv::InputArray imLeft, cv::InputArray imRight, double timestamp, const vector &vImuMeas = vector(), string filename = ""); @@ -99,7 +81,7 @@ class System : public std::enable_shared_from_this { // 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 ×tamp, + cv::InputArray im, cv::InputArray depthmap, double timestamp, const vector &vImuMeas = vector(), string filename = ""); @@ -107,7 +89,7 @@ class System : public std::enable_shared_from_this { // 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 ×tamp, + cv::InputArray im, double timestamp, const vector &vImuMeas = vector(), string filename = ""); @@ -207,7 +189,6 @@ class System : public std::enable_shared_from_this { bool initialize(bool initFr = false, const string &strSequence = std::string()); - private: void processLocalizationModeChange(void); void processReset(void); void updateTrackingState(); diff --git a/include/System/Factory.h b/include/System/Factory.h new file mode 100644 index 00000000000..15e6d6c5fde --- /dev/null +++ b/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 . + */ + +#pragma once + +#include +#include + +#include "Settings.h" + +namespace ORB_SLAM3 { + +class System; + +class SystemFactory { + public: + typedef tl::expected, ExpectedError> Expected; + + static Expected create(const std::shared_ptr &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 diff --git a/src/System.cc b/src/System.cc index 2cf1dbfeadc..da5235aa466 100644 --- a/src/System.cc +++ b/src/System.cc @@ -49,55 +49,6 @@ namespace ORB_SLAM3 { Verbose::eLevel Verbose::th = Verbose::VERBOSITY_NORMAL; -SystemFactory::Expected SystemFactory::create( - const std::shared_ptr &settings, bool initFr, - const string &strSequence) { - if (!settings->validate()) { - return tl::make_unexpected(ExpectedError::fmt("Settings do not validate")); - } - - // Cannot use make_shared with friend constructors? - auto sys = std::shared_ptr(new System(settings, initFr, strSequence)); - - // Initialization must occur separately because we use shared_from_this - if (!sys->initialize()) { - return tl::make_unexpected( - ExpectedError::fmt("Unable to initialize SLAM system")); - } - - return sys; -} - -SystemFactory::Expected SystemFactory::create(const std::string &configFile, - const SensorType sensor, - bool initFr, - const string &strSequence) { - auto exSettings = SettingsLoader::load(configFile, sensor); - - if (!exSettings) { - return tl::make_unexpected(ExpectedError::fmt("Unable to load settings")); - } - - return SystemFactory::create(exSettings.value(), initFr, strSequence); -} - -SystemFactory::Expected SystemFactory::create(const std::string &configFile, - const std::string &vocabFile, - const SensorType sensor, - bool initFr, - const string &strSequence) { - auto exSettings = SettingsLoader::load(configFile, sensor, vocabFile); - - if (!exSettings) { - return tl::make_unexpected(ExpectedError::fmt("Unable to load settings")); - } - - auto settings = exSettings.value(); - return SystemFactory::create(settings, initFr, strSequence); -} - -//=================================================================== - System::System(const std::shared_ptr &settings, bool initFr, const string &strSequence) : enable_shared_from_this(), @@ -245,8 +196,8 @@ bool System::initialize(bool initFr, const string &strSequence) { return true; } -Sophus::SE3f System::TrackStereo(const cv::Mat &imLeft, const cv::Mat &imRight, - const double ×tamp, +Sophus::SE3f System::TrackStereo(cv::InputArray imLeft, cv::InputArray imRight, + double timestamp, const vector &vImuMeas, string filename) { if (!sensorType().isStereo()) { @@ -263,14 +214,14 @@ Sophus::SE3f System::TrackStereo(const cv::Mat &imLeft, const cv::Mat &imRight, cv::Mat M1r = settings_->M1r(); cv::Mat M2r = settings_->M2r(); - cv::remap(imLeft, imLeftToFeed, M1l, M2l, cv::INTER_LINEAR); - cv::remap(imRight, imRightToFeed, M1r, M2r, cv::INTER_LINEAR); + cv::remap(imLeft.getMat(), imLeftToFeed, M1l, M2l, cv::INTER_LINEAR); + cv::remap(imRight.getMat(), imRightToFeed, M1r, M2r, cv::INTER_LINEAR); } else if (settings_ && settings_->needToResize()) { - cv::resize(imLeft, imLeftToFeed, settings_->newImSize()); - cv::resize(imRight, imRightToFeed, settings_->newImSize()); + cv::resize(imLeft.getMat(), imLeftToFeed, settings_->newImSize()); + cv::resize(imRight.getMat(), imRightToFeed, settings_->newImSize()); } else { - imLeftToFeed = imLeft.clone(); - imRightToFeed = imRight.clone(); + imLeftToFeed = imLeft.getMat().clone(); + imRightToFeed = imRight.getMat().clone(); } processLocalizationModeChange(); @@ -293,8 +244,8 @@ Sophus::SE3f System::TrackStereo(const cv::Mat &imLeft, const cv::Mat &imRight, return Tcw; } -Sophus::SE3f System::TrackRGBD(const cv::Mat &im, const cv::Mat &depthmap, - const double ×tamp, +Sophus::SE3f System::TrackRGBD(cv::InputArray im, cv::InputArray depthmap, + double timestamp, const vector &vImuMeas, string filename) { if (!sensorType().isRGBD()) { @@ -303,14 +254,14 @@ Sophus::SE3f System::TrackRGBD(const cv::Mat &im, const cv::Mat &depthmap, exit(-1); } - cv::Mat imToFeed = im.clone(); - cv::Mat imDepthToFeed = depthmap.clone(); + cv::Mat imToFeed; + cv::Mat imDepthToFeed; if (settings_ && settings_->needToResize()) { - cv::Mat resizedIm; - cv::resize(im, resizedIm, settings_->newImSize()); - imToFeed = resizedIm; - - cv::resize(depthmap, imDepthToFeed, settings_->newImSize()); + cv::resize(im.getMat(), imToFeed, settings_->newImSize()); + cv::resize(depthmap.getMat(), imDepthToFeed, settings_->newImSize()); + } else { + imToFeed = im.getMat().clone(); + imDepthToFeed = depthmap.getMat().clone(); } processLocalizationModeChange(); @@ -332,7 +283,7 @@ Sophus::SE3f System::TrackRGBD(const cv::Mat &im, const cv::Mat &depthmap, return Tcw; } -Sophus::SE3f System::TrackMonocular(const cv::Mat &im, const double ×tamp, +Sophus::SE3f System::TrackMonocular(cv::InputArray im, double timestamp, const vector &vImuMeas, string filename) { { @@ -347,11 +298,13 @@ Sophus::SE3f System::TrackMonocular(const cv::Mat &im, const double ×tamp, exit(-1); } - cv::Mat imToFeed = im.clone(); + cv::Mat imToFeed; if (settings_ && settings_->needToResize()) { cv::Mat resizedIm; - cv::resize(im, resizedIm, settings_->newImSize()); + cv::resize(im.getMat(), resizedIm, settings_->newImSize()); imToFeed = resizedIm; + } else { + imToFeed = im.getMat().clone(); } processLocalizationModeChange(); diff --git a/src/System/Factory.cc b/src/System/Factory.cc new file mode 100644 index 00000000000..5d0c343c302 --- /dev/null +++ b/src/System/Factory.cc @@ -0,0 +1,80 @@ +/** + * 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 . + */ + +#include "System/Factory.h" + +#include +#include + +#include "System.h" + +namespace ORB_SLAM3 { + +using std::string; + +SystemFactory::Expected SystemFactory::create( + const std::shared_ptr &settings, bool initFr, + const string &strSequence) { + if (!settings->validate()) { + return tl::make_unexpected(ExpectedError::fmt("Settings do not validate")); + } + + // Cannot use make_shared with friend constructors? + auto sys = std::shared_ptr(new System(settings, initFr, strSequence)); + + // Initialization must occur separately because we use shared_from_this + if (!sys->initialize()) { + return tl::make_unexpected( + ExpectedError::fmt("Unable to initialize SLAM system")); + } + + return sys; +} + +SystemFactory::Expected SystemFactory::create(const std::string &configFile, + const SensorType sensor, + bool initFr, + const string &strSequence) { + auto exSettings = SettingsLoader::load(configFile, sensor); + + if (!exSettings) { + return tl::make_unexpected(ExpectedError::fmt("Unable to load settings")); + } + + return SystemFactory::create(exSettings.value(), initFr, strSequence); +} + +SystemFactory::Expected SystemFactory::create(const std::string &configFile, + const std::string &vocabFile, + const SensorType sensor, + bool initFr, + const string &strSequence) { + auto exSettings = SettingsLoader::load(configFile, sensor, vocabFile); + + if (!exSettings) { + return tl::make_unexpected(ExpectedError::fmt("Unable to load settings")); + } + + auto settings = exSettings.value(); + return SystemFactory::create(settings, initFr, strSequence); +} + +} // namespace ORB_SLAM3 From 76c1704c37bd736449e9d3baa2c0207eb813512d Mon Sep 17 00:00:00 2001 From: Aaron Marburg Date: Tue, 17 Feb 2026 23:37:57 +0000 Subject: [PATCH 4/4] Propagating further use of oslog --- include/Logging.h | 2 +- include/Settings.h | 10 ++++----- src/LocalMapping.cc | 8 +++---- src/LoopClosing.cc | 27 ++++++++++------------- src/Optimizer.cc | 10 ++++----- src/SettingsLoader.cc | 47 ++++++++++++++++++++--------------------- src/System.cc | 14 ++++++------ src/System/Exporters.cc | 2 +- src/System/Factory.cc | 4 ++-- 9 files changed, 57 insertions(+), 67 deletions(-) diff --git a/include/Logging.h b/include/Logging.h index 00e54a7c32f..b79773170b8 100644 --- a/include/Logging.h +++ b/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); diff --git a/include/Settings.h b/include/Settings.h index db0a2bd70f1..c03daf89f61 100644 --- a/include/Settings.h +++ b/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, 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(); } diff --git a/src/LocalMapping.cc b/src/LocalMapping.cc index 1e420a5a3fe..084b47de735 100644 --- a/src/LocalMapping.cc +++ b/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 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++; diff --git a/src/LoopClosing.cc b/src/LoopClosing.cc index 4ffb0771eee..40d58b8640a 100644 --- a/src/LoopClosing.cc +++ b/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& 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; diff --git a/src/Optimizer.cc b/src/Optimizer.cc index b032896eb32..7c147b318e7 100644 --- a/src/Optimizer.cc +++ b/src/Optimizer.cc @@ -274,7 +274,7 @@ void Optimizer::BundleAdjustment(const vector>& vpKFs, optimizer.setVerbose(false); optimizer.initializeOptimization(); optimizer.optimize(nIterations); - Verbose::PrintMess("BA: End of the optimization", Verbose::VERBOSITY_NORMAL); + oslog::debug("BA: End of the optimization"); // Recover optimized data // Keyframes @@ -432,8 +432,7 @@ void Optimizer::FullInertialBA(const std::shared_ptr& pMap, int its, // IMU links for (auto pKFi : vpKFs) { if (!pKFi->mPrevKF) { - Verbose::PrintMess("NOT INERTIAL LINK TO PREVIOUS FRAME!", - Verbose::VERBOSITY_NORMAL); + oslog::warn("NOT INERTIAL LINK TO PREVIOUS FRAME!"); continue; } @@ -1104,9 +1103,8 @@ void Optimizer::LocalBundleAdjustment(const shared_ptr& pKF, num_fixedKF = lFixedCameras.size() + num_fixedKF; if (num_fixedKF == 0) { - Verbose::PrintMess( - "LM-LBA: There are 0 fixed KF in the optimizations, LBA aborted", - Verbose::VERBOSITY_NORMAL); + oslog::info( + "LM-LBA: There are 0 fixed KF in the optimizations, LBA aborted"); return; } diff --git a/src/SettingsLoader.cc b/src/SettingsLoader.cc index 5fca81262b7..4dc9c6c1a19 100644 --- a/src/SettingsLoader.cc +++ b/src/SettingsLoader.cc @@ -42,17 +42,16 @@ float SettingsLoader::readParameter(cv::FileStorage& fSettings, 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 0.0f; } } else if (!node.isReal()) { - std::cerr << name << " parameter must be a real number, aborting..." - << std::endl; + oslog::error("{} parameter must be a real number, aborting...", name); exit(-1); } else { found = true; @@ -67,17 +66,17 @@ int SettingsLoader::readParameter(cv::FileStorage& fSettings, 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 0; } } else if (!node.isInt()) { - std::cerr << name << " parameter must be an integer number, aborting..." - << std::endl; + oslog::error("{} parameter must be an integer number, aborting...", name); + exit(-1); } else { found = true; @@ -92,17 +91,17 @@ string SettingsLoader::readParameter(cv::FileStorage& fSettings, 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 string(); } } else if (!node.isString()) { - std::cerr << name << " parameter must be a string, aborting..." - << std::endl; + oslog::error("{} parameter must be a string, aborting...", name); exit(-1); } else { found = true; @@ -118,11 +117,11 @@ cv::Mat SettingsLoader::readParameter(cv::FileStorage& fSettings, 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 cv::Mat(); } @@ -132,7 +131,7 @@ cv::Mat SettingsLoader::readParameter(cv::FileStorage& fSettings, } } -SettingsLoader::Expected SettingsLoader::load(const std::string& configFile, +SettingsLoader::Expected SettingsLoader::Load(const std::string& configFile, const SensorType sensor, const std::string& vocabFile) { SettingsLoader loader(sensor); @@ -147,9 +146,9 @@ SettingsLoader::Expected SettingsLoader::load(const std::string& configFile, // Open settings file cv::FileStorage fSettings(configFile, cv::FileStorage::READ); if (!fSettings.isOpened()) { - cerr << "[ERROR]: could not open configuration file at: " << configFile - << endl; - cerr << "Aborting..." << endl; + oslog::error("[ERROR]: could not open configuration file \"{}\"", + configFile); + oslog::error("Aborting..."); return tl::make_unexpected( ExpectedError::fmt("Unable to open configuration file {}", configFile)); @@ -283,7 +282,7 @@ void SettingsLoader::readCamera1(cv::FileStorage& fSettings) { // vOverlapping; //} } else { - cerr << "Error: " << cameraModel << " not known" << endl; + oslog::error("Error: {} not known", cameraModel); exit(-1); } } diff --git a/src/System.cc b/src/System.cc index da5235aa466..86f27b232db 100644 --- a/src/System.cc +++ b/src/System.cc @@ -95,8 +95,8 @@ bool System::initialize(bool initFr, const string &strSequence) { mpVocabulary = std::make_shared(); bool bVocLoad = mpVocabulary->loadFromTextFile(vocabularyFilePath); if (!bVocLoad) { - cerr << "Wrong path to vocabulary. " << endl; - cerr << "Falied to open at: " << vocabularyFilePath << endl; + oslog::error("Wrong path to vocabulary. Failed to open {}", + vocabularyFilePath); return false; } oslog::info("Vocabulary loaded!"); @@ -249,8 +249,8 @@ Sophus::SE3f System::TrackRGBD(cv::InputArray im, cv::InputArray depthmap, const vector &vImuMeas, string filename) { if (!sensorType().isRGBD()) { - cerr << "ERROR: you called TrackRGBD but input sensor was not set to RGBD." - << endl; + oslog::error( + "ERROR: you called TrackRGBD but input sensor was not set to RGBD."); exit(-1); } @@ -292,9 +292,9 @@ Sophus::SE3f System::TrackMonocular(cv::InputArray im, double timestamp, } if (!sensorType().isMonocular()) { - cerr << "ERROR: you called TrackMonocular but input sensor was not set to " - "Monocular nor Monocular-Inertial." - << endl; + oslog::error( + "ERROR: you called TrackMonocular but input sensor was not set to " + "Monocular nor Monocular-Inertial."); exit(-1); } diff --git a/src/System/Exporters.cc b/src/System/Exporters.cc index 0dd337aa8dc..1742af00b11 100644 --- a/src/System/Exporters.cc +++ b/src/System/Exporters.cc @@ -57,7 +57,7 @@ void System::SaveTrajectoryTUM(const string &filename) { oslog::info("Saving camera trajectory to {} ...", filename); if (sensorType() == SensorType::MONOCULAR) { - cerr << "ERROR: SaveTrajectoryTUM cannot be used for monocular." << endl; + oslog::error("ERROR: SaveTrajectoryTUM cannot be used for monocular."); return; } diff --git a/src/System/Factory.cc b/src/System/Factory.cc index 5d0c343c302..a8507e0a9e8 100644 --- a/src/System/Factory.cc +++ b/src/System/Factory.cc @@ -53,7 +53,7 @@ SystemFactory::Expected SystemFactory::create(const std::string &configFile, const SensorType sensor, bool initFr, const string &strSequence) { - auto exSettings = SettingsLoader::load(configFile, sensor); + auto exSettings = SettingsLoader::Load(configFile, sensor); if (!exSettings) { return tl::make_unexpected(ExpectedError::fmt("Unable to load settings")); @@ -67,7 +67,7 @@ SystemFactory::Expected SystemFactory::create(const std::string &configFile, const SensorType sensor, bool initFr, const string &strSequence) { - auto exSettings = SettingsLoader::load(configFile, sensor, vocabFile); + auto exSettings = SettingsLoader::Load(configFile, sensor, vocabFile); if (!exSettings) { return tl::make_unexpected(ExpectedError::fmt("Unable to load settings"));