Merge pull request #9 from ibois-epfl/fix-merge-map

Fix: combine map problem
ibois-epfl · Aug 26, 2024 · a245db2 · a245db2
2 parents 4dd49bb + e4e0487
commit a245db2
Show file tree

Hide file tree

Showing 4 changed files with 15 additions and 11 deletions.
diff --git a/src/map.cpp b/src/map.cpp
@@ -1516,7 +1516,7 @@ void Map::merge(std::shared_ptr<Map> mapB){
                 kfIter->ids[i] = pointIdB2A[kfIter->ids[i]];
             }
         }
-        mapManager->addKeyFrame(&(*kfIter));
+        mapManager->addKeyFrame(&(*kfIter), true);
     }
 }
 

diff --git a/src/tslam.cpp b/src/tslam.cpp
@@ -28,6 +28,7 @@ authors and should not be interpreted as representing official policies, either
 or implied Andrea Settimi and Hong-Bin Yang.
 */
 #include <regex>
+#include <filesystem>
 
 #include "tslam.h"
 #include "utils/system.h"
@@ -183,15 +184,18 @@ namespace tslam{
             *estimatedImageParam = TheMapA->keyframes.begin()->imageParams;
         }
 
-        string basePath = outputPath;
-        if(std::regex_match(basePath, std::regex("\\.map$"))){
-            basePath.substr(0, basePath.length() - 4);
-        }
+        std::filesystem::path basePath = outputPath;
+
         if(exportYml){
-            TheMapA->saveToMarkerMap(basePath + ".yml");
+            TheMapA->saveToMarkerMap(basePath.replace_extension("yml"));
         }
         if(exportPly){
-            TheMapA->exportToFile(basePath + ".ply",cv::Scalar(125,125,125),cv::Scalar(255,0,0),cv::Scalar(0,0,255),{1111,1195,1129,1196,1141},cv::Scalar(0,255,0));
+            TheMapA->exportToFile(basePath.replace_extension("ply"),
+                                  cv::Scalar(125,125,125),
+                                  cv::Scalar(255,0,0),
+                                  cv::Scalar(0,0,255),
+                                  {},
+                                  cv::Scalar(0,255,0));
         }
 
         TheMapA->saveToFile(std::move(outputPath));

diff --git a/src/utils/mapmanager.cpp b/src/utils/mapmanager.cpp
@@ -238,7 +238,7 @@ void MapManager::reset(){
 
 
 
-Frame& MapManager::addKeyFrame(Frame *newPtrFrame){
+Frame& MapManager::addKeyFrame(Frame *newPtrFrame, bool forceAddNewMarker){
     auto getNOFValidMarkers=[this](){
         int nValidMarkers=0;
         for(auto &m:TheMap->map_markers)
@@ -247,9 +247,9 @@ Frame& MapManager::addKeyFrame(Frame *newPtrFrame){
     };
 
     // in localizeOnly mode, filter out the marker that is not in the map
-    if(System::getParams().localizeOnly){
+    if(!forceAddNewMarker && System::getParams().localizeOnly){
         std::vector<tslam::MarkerObservation> filteredMarkers;
-        for(auto m : newPtrFrame->markers) {
+        for(const auto &m : newPtrFrame->markers) {
             if (TheMap->map_markers.count(m.id)!=0){
                 filteredMarkers.push_back(m);
             }

diff --git a/src/utils/mapmanager.h b/src/utils/mapmanager.h
@@ -69,7 +69,7 @@ struct NewPointInfo{
     //call whenever a new frame is avaiable.
     //return 0 if nothing is done
     int newFrame(Frame &kf,  int32_t curkeyFrame);
-    Frame &addKeyFrame(Frame *f);
+    Frame &addKeyFrame(Frame *f, bool forceAddNewMarker = false);
 
     //applies the changes to map require tracking to be stoped, such as removing frames  or points
     bool mapUpdate(void);