Skip to content

Dev/aarons fixes #12

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merged
amarburg merged 3 commits into from
Feb 10, 2026
Merged

Dev/aarons fixes #12

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
a1d0a24
Convert cout to oslog
amarburg Feb 8, 2026
bd28161
Improve MapPoint serialization
amarburg Feb 8, 2026
81d421b
Add milliseconds to log output
amarburg Feb 8, 2026
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
2 changes: 1 addition & 1 deletion Examples/EuRoC/euroc_common.cc
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -148,5 +148,5 @@ void setupEurocSpdLogger() {
ORB_SLAM3::Logger::add_sink(stdout_sink);
spdlog::set_default_logger(
std::make_shared<spdlog::logger>("euroc", stdout_sink));
spdlog::set_pattern("%E [%-8n] [%6!l] %v");
spdlog::set_pattern("%E.%e [%-8n] [%6!l] %v");
}
57 changes: 31 additions & 26 deletions include/MapPoint.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -52,33 +52,38 @@ class MapPoint {
ar & mnFirstKFid;
ar & mnFirstFrame;
ar & nObs;

// Variables used by the tracking
// ar & mTrackProjX;
// ar & mTrackProjY;
// ar & mTrackDepth;
// ar & mTrackDepthR;
// ar & mTrackProjXR;
// ar & mTrackProjYR;
// ar & mbTrackInView;
// ar & mbTrackInViewR;
// ar & mnTrackScaleLevel;
// ar & mnTrackScaleLevelR;
// ar & mTrackViewCos;
// ar & mTrackViewCosR;
// ar & mnTrackReferenceForFrame;
// ar & mnLastFrameSeen;
ar & mTrackProjX;
ar & mTrackProjY;
ar & mTrackDepth;
ar & mTrackDepthR;
ar & mTrackProjXR;
ar & mTrackProjYR;
ar & mbTrackInView;
ar & mbTrackInViewR;
ar & mnTrackScaleLevel;
ar & mnTrackScaleLevelR;
ar & mTrackViewCos;
ar & mTrackViewCosR;
ar & mnTrackReferenceForFrame;
ar & mnLastFrameSeen;

// Variables used by local mapping
// ar & mnBALocalForKF;
// ar & mnFuseCandidateForKF;
ar & mnBALocalForKF;
ar & mnFuseCandidateForKF;

// Variables used by loop closing and merging
// ar & mnLoopPointForKF;
// ar & mnCorrectedByKF;
// ar & mnCorrectedReference;
ar & mnLoopPointForKF;
ar & mnCorrectedByKF;
ar & mnCorrectedReference;
ar& boost::serialization::make_array(mPosGBA.data(), mPosGBA.size());
// serializeMatrix(ar,mPosGBA,version);
// ar & mnBAGlobalForKF;
// ar & mnBALocalForMerge;
ar & mnBAGlobalForKF;
ar & mnBALocalForMerge;
ar& boost::serialization::make_array(mPosMerge.data(), mPosMerge.size());
ar& boost::serialization::make_array(mNormalVectorMerge.data(),
mNormalVectorMerge.size());
// serializeMatrix(ar,mPosMerge,version);
// serializeMatrix(ar,mNormalVectorMerge,version);

Expand All @@ -88,12 +93,12 @@ class MapPoint {
mNormalVector.size());
// ar & BOOST_SERIALIZATION_NVP(mBackupObservationsId);
// ar & mObservations;
ar & mBackupObservationsId1;
ar & mBackupObservationsId2;
// ar & mBackupObservationsId1;
// ar & mBackupObservationsId2;
serializeMatrix(ar, mDescriptor, version);
ar & mBackupRefKFId;
// ar & mnVisible;
// ar & mnFound;
// ar & mBackupRefKFId;
ar & mnVisible;
ar & mnFound;

ar & mbBad;
ar & mBackupReplacedId;
Expand Down
2 changes: 1 addition & 1 deletion src/ORBmatcher.cc
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -71,7 +71,7 @@ int ORBmatcher::SearchByProjection(const std::shared_ptr<Frame> &F,

const vector<size_t> vIndices =
F->GetFeaturesInArea(pMP->mTrackProjX, pMP->mTrackProjY,
r * F->mvScaleFactors[nPredictedLevel],
r * F->mvScaleFactors.at(nPredictedLevel),
nPredictedLevel - 1, nPredictedLevel);

if (!vIndices.empty()) {
Expand Down
94 changes: 31 additions & 63 deletions src/System.cc
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -458,7 +458,7 @@
mbShutDown = true;
}

cout << "Shutdown" << endl;
oslog::warn("Shutdown");

mpLocalMapper->RequestFinish();
mpLoopCloser->RequestFinish();
Expand Down Expand Up @@ -517,7 +517,7 @@
return;
}

cout << endl << "Saving camera trajectory to " << filename << " ..." << endl;
oslog::info("Saving camera trajectory to {} ...", filename);
if (sensorType() == SensorType::MONOCULAR) {
cerr << "ERROR: SaveTrajectoryTUM cannot be used for monocular." << endl;
return;
Expand Down Expand Up @@ -574,12 +574,10 @@
<< " " << q.w() << endl;
}
f.close();
// cout << endl << "trajectory saved!" << endl;
}

void System::SaveKeyFrameTrajectoryTUM(const string &filename) {
cout << endl
<< "Saving keyframe trajectory to " << filename << " ..." << endl;
oslog::info("Saving keyframe trajectory to {} ...", filename);

vector<std::shared_ptr<KeyFrame>> vpKFs = mpAtlas->GetAllKeyFrames();
sort(vpKFs.begin(), vpKFs.end(), KeyFrame::lId);
Expand Down Expand Up @@ -616,12 +614,10 @@
vector<std::shared_ptr<Map>> vpMaps = mpAtlas->GetAllMaps();
size_t numMaxKFs = 0;
std::shared_ptr<Map> pBiggerMap;
std::cout << "There are " << std::to_string(vpMaps.size())
<< " maps in the atlas" << std::endl;
oslog::debug("There are {} maps in the atlas", std::to_string(vpMaps.size()));
for (auto pMap : vpMaps) {
std::cout << " Map " << std::to_string(pMap->GetId()) << " has "
<< std::to_string(pMap->GetAllKeyFrames().size()) << " KFs"
<< std::endl;
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;
Expand All @@ -648,7 +644,6 @@

ofstream f;
f.open(filename.c_str());
// cout << "file open" << endl;
f << fixed;

// Frame pose is stored relative to its reference keyframe (which is optimized
Expand All @@ -671,20 +666,16 @@
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<KeyFrame> pKF = *lRit;
// cout << "KF: " << pKF->mnId << endl;

Sophus::SE3f Trw;

// If the reference keyframe was culled, traverse the spanning tree to get a
// suitable keyframe.
if (!pKF) continue;

// cout << "2.5" << endl;

while (pKF->isBad()) {
// cout << " 2.bad" << endl;
Trw = Trw * pKF->mTcp;
Expand All @@ -697,13 +688,9 @@
continue;
}

// cout << "3" << endl;

Trw = Trw * pKF->GetPose() *
Twb; // Tcp*Tpw*Twb0=Tcb0 where b0 is the new world reference

// cout << "4" << endl;

if (sensorType().isImu()) {
Sophus::SE3f Twb = (pKF->mImuCalib.mTbc * (*lit) * Trw).inverse();
Eigen::Quaternionf q = Twb.unit_quaternion();
Expand All @@ -719,19 +706,15 @@
<< " " << twc(1) << " " << twc(2) << " " << q.x() << " " << q.y() << " "
<< q.z() << " " << q.w() << endl;
}

// cout << "5" << endl;
}
// cout << "end saving trajectory" << endl;
f.close();
cout << endl << "End of saving trajectory to " << filename << " ..." << endl;
oslog::info("End of saving trajectory to {} ...", filename);
}

void System::SaveTrajectoryEuRoC(const string &filename,
const std::shared_ptr<Map> &pMap) {
cout << endl
<< "Saving trajectory of map " << pMap->GetId() << " to " << filename
<< " ..." << endl;
oslog::info("Saving trajectory of map {} to {} ...", pMap->GetId(), filename);

/*if(sensorType()==MONOCULAR)
{
cerr << "ERROR: SaveTrajectoryEuRoC cannot be used for monocular." <<
Expand All @@ -752,7 +735,6 @@
}
ofstream f;
f.open(filename.c_str());
// cout << "file open" << endl;
f << fixed;

// Frame pose is stored relative to its reference keyframe (which is optimized
Expand All @@ -779,18 +761,14 @@
if (*lbL) continue;

std::shared_ptr<KeyFrame> pKF = *lRit;
// cout << "KF: " << pKF->mnId << endl;

Sophus::SE3f Trw;

// If the reference keyframe was culled, traverse the spanning tree to get a
// suitable keyframe.
if (!pKF) continue;

// cout << "2.5" << endl;

while (pKF->isBad()) {
// cout << " 2.bad" << endl;
Trw = Trw * pKF->mTcp;
pKF = pKF->GetParent();
// cout << "--Parent KF: " << pKF->mnId << endl;
Expand All @@ -801,13 +779,9 @@
continue;
}

// cout << "3" << endl;

Trw = Trw * pKF->GetPose() *
Twb; // Tcp*Tpw*Twb0=Tcb0 where b0 is the new world reference

// cout << "4" << endl;

if (sensorType().isImu()) {
Sophus::SE3f Twb = (pKF->mImuCalib.mTbc * (*lit) * Trw).inverse();
Eigen::Quaternionf q = Twb.unit_quaternion();
Expand All @@ -823,17 +797,13 @@
<< " " << twc(1) << " " << twc(2) << " " << q.x() << " " << q.y() << " "
<< q.z() << " " << q.w() << endl;
}

// cout << "5" << endl;
}
// cout << "end saving trajectory" << endl;
f.close();
cout << endl << "End of saving trajectory to " << filename << " ..." << endl;
oslog::info("End of saving trajectory to {} ...", filename);
}

void System::SaveKeyFrameTrajectoryEuRoC(const string &filename) {
cout << endl
<< "Saving keyframe trajectory to " << filename << " ..." << endl;
oslog::info("Saving keyframe trajectory to {} ...", filename);

vector<std::shared_ptr<Map>> vpMaps = mpAtlas->GetAllMaps();
std::shared_ptr<Map> pBiggerMap;
Expand All @@ -846,7 +816,7 @@
}

if (!pBiggerMap) {
std::cout << "There is not a map!!" << std::endl;
oslog::error("There is not a map!!");
return;
}

Expand Down Expand Up @@ -885,9 +855,8 @@

void System::SaveKeyFrameTrajectoryEuRoC(const string &filename,
const std::shared_ptr<Map> &pMap) {
cout << endl
<< "Saving keyframe trajectory of map " << pMap->GetId() << " to "
<< filename << " ..." << endl;
oslog::info("Saving keyframe trajectory of map {} to {} ...", pMap->GetId(),
filename);

auto vpKFs = pMap->GetAllKeyFrames();
sort(vpKFs.begin(), vpKFs.end(), KeyFrame::lId);
Expand Down Expand Up @@ -922,9 +891,9 @@
}

void System::SaveTrajectoryKITTI(const string &filename) {
cout << endl << "Saving camera trajectory to " << filename << " ..." << endl;
oslog::info("Saving camera trajectory to {} ...", filename);
if (sensorType() == SensorType::MONOCULAR) {
cerr << "ERROR: SaveTrajectoryKITTI cannot be used for monocular." << endl;
oslog::error("ERROR: SaveTrajectoryKITTI cannot be used for monocular.");
return;
}

Expand Down Expand Up @@ -1129,26 +1098,26 @@
if (type == TEXT_FILE) {
// File text

cout << "Starting to write the save text file " << endl;
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;
cout << "End to write the save text file" << endl;
oslog::debug("End to write the save text file");
} else if (type == BINARY_FILE) {
// File binary

cout << "Starting to write the save binary file" << endl;
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;
cout << "End to write save binary file" << endl;
oslog::debug("End to write save binary file");
}
}
}
Expand All @@ -1166,33 +1135,33 @@

if (type == TEXT_FILE) {
// File text
cout << "Starting to read the save text file " << endl;
oslog::debug("Starting to read the save text file ");
std::ifstream ifs(pathLoadFileName, std::ios::binary);
if (!ifs.good()) {
cout << "Load file not found" << endl;
oslog::error("Cannot find Atlas file {}", pathLoadFileName);
return false;
}
boost::archive::text_iarchive ia(ifs);
ia >> strFileVoc;
ia >> strVocChecksum;
ia >> mpAtlas;

cout << "Finished loading the saved text file " << endl;
oslog::debug("Finished loading the saved text file ");
isRead = true;
} else if (type == BINARY_FILE) {
// File binary
cout << "Starting to read the save binary file" << endl;
oslog::debug("Starting to read the save binary file");
std::ifstream ifs(pathLoadFileName, std::ios::binary);
if (!ifs.good()) {
cout << "Load file not found" << endl;
oslog::error("Cannot find Atlas file {}", pathLoadFileName);
return false;
}
boost::archive::binary_iarchive ia(ifs);
ia >> strFileVoc;
ia >> strVocChecksum;
ia >> mpAtlas;

cout << "Finished loading the saved binary file" << endl;
oslog::debug("Finished loading the saved binary file");
isRead = true;
}

Expand All @@ -1206,10 +1175,10 @@
CalculateCheckSum(vocabularyFilePath, TEXT_FILE);

if (strInputVocabularyChecksum.compare(strVocChecksum) != 0) {
cout << "The vocabulary load isn't the same which the load session was "
"created "
<< endl;
cout << "-Vocabulary name: " << strFileVoc << endl;
oslog::warn(
"The vocabulary load isn't the same which the load session was "
"created. Loading vocab file {}",
strFileVoc);
return false; // Both are differents
}

Expand All @@ -1233,22 +1202,21 @@

ifstream f(filename.c_str(), flags);
if (!f.is_open()) {
cout << "[E] Unable to open the in file " << filename << " for Md5 hash."
<< endl;
oslog::error("[E] Unable to open the in file {} for Md5 hash.", filename);
return checksum;
}

MD5_CTX md5Context;
char buffer[1024];

MD5_Init(&md5Context);

Check warning on line 1212 in src/System.cc

View workflow job for this annotation

GitHub Actions / Build Project 

‘int MD5_Init(MD5_CTX*)’ is deprecated: Since OpenSSL 3.0 [-Wdeprecated-declarations]
while (int count = f.readsome(buffer, sizeof(buffer))) {
MD5_Update(&md5Context, buffer, count);

Check warning on line 1214 in src/System.cc

View workflow job for this annotation

GitHub Actions / Build Project 

‘int MD5_Update(MD5_CTX*, const void*, size_t)’ is deprecated: Since OpenSSL 3.0 [-Wdeprecated-declarations]
}

f.close();

MD5_Final(c, &md5Context);

Check warning on line 1219 in src/System.cc

View workflow job for this annotation

GitHub Actions / Build Project 

‘int MD5_Final(unsigned char*, MD5_CTX*)’ is deprecated: Since OpenSSL 3.0 [-Wdeprecated-declarations]

for (int i = 0; i < MD5_DIGEST_LENGTH; i++) {
char aux[10];
Expand Down
Loading