Add saveMap() function

ECEBORG · Aug 28, 2018 · 5f017f7 · 5f017f7
1 parent d5f4f41
commit 5f017f7
Show file tree

Hide file tree

Showing 2 changed files with 25 additions and 3 deletions.
diff --git a/include/System.h b/include/System.h
@@ -113,7  113,7 @@ class System
     void SaveTrajectoryKITTI(const string &filename);
 
     // TODO: Save/Load functions
-    // SaveMap(const string &filename);
     void SaveMap(const string &filename);
     // LoadMap(const string &filename);
 
     // Information from most recent processed frame

diff --git a/src/System.cc b/src/System.cc
@@ -418,6  418,30 @@ void System::SaveKeyFrameTrajectoryTUM(const string &filename)
     cout << endl << "trajectory saved!" << endl;
 }
 
 void System::SaveMap(const string &filename)
 {
 
     vector<MapPoint*> vMPs = mpMap->GetAllMapPoints();
 
     ofstream f;
     f.open(filename.c_str());
     f << fixed;
 
     vector<MapPoint*>::iterator itvMps;
     // Iterate over all points
     for(itvMps = vMPs.begin(); itvMps != vMPs.end() ; itvMps   )
     {
         MapPoint* mp = *itvMps;
 
         cv::Mat pos = cv::Mat::eye(3,1,CV_32F);
 	pos = mp->GetWorldPos();
 
         f << pos.at<float>(0) << ", " << pos.at<float>(1)  << ", " << pos.at<float>(2) << endl;
     }
     f.close();
 }
 
 
 void System::SaveTrajectoryKITTI(const string &filename)
 {
     cout << endl << "Saving camera trajectory to " << filename << " ..." << endl;
@@ -475,8  499,6 @@ void System::SaveTrajectoryKITTI(const string &filename)
 
 
 
-
-
 int System::GetTrackingState()
 {
     unique_lock<mutex> lock(mMutexState);