From e4e0487ff8976fe6ca7fd71e9217d88f216eed60 Mon Sep 17 00:00:00 2001
From: Andrea Settimi <eyanghongbin0315@gmail.com>
Date: Fri, 23 Aug 2024 11:43:35 +0200
Subject: [PATCH] Fix: combine map problem

---
 src/map.cpp              |  2 +-
 src/tslam.cpp            | 16 ++++++++++------
 src/utils/mapmanager.cpp |  6 +++---
 src/utils/mapmanager.h   |  2 +-
 4 files changed, 15 insertions(+), 11 deletions(-)

diff --git a/src/map.cpp b/src/map.cpp
index 362f873..df5ae63 100644
--- 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
index f728635..63035e2 100644
--- 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
index 63078c1..26dd191 100644
--- 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
index 35c2ad7..3a50a82 100644
--- 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);