From 1d1e972ddd20a37323f30ca764d4a73480cd5aa3 Mon Sep 17 00:00:00 2001
From: Andrea Settimi <eyanghongbin0315@gmail.com>
Date: Sun, 28 Jul 2024 12:32:10 +0200
Subject: [PATCH] Fix: tslam combine map

---
 src/optimization/pnpsolver.cpp |   2 +-
 utils/tslam_combine_map.cpp    | 104 +++++++++++----------------------
 2 files changed, 35 insertions(+), 71 deletions(-)

diff --git a/src/optimization/pnpsolver.cpp b/src/optimization/pnpsolver.cpp
index 4a17293..bf5efe1 100644
--- a/src/optimization/pnpsolver.cpp
+++ b/src/optimization/pnpsolver.cpp
@@ -316,7 +316,7 @@ int PnPSolver::solvePnp( const Frame &frame, std::shared_ptr<Map> TheMap, std::v
         //compute the weight of markers considering that w_markers+w_points must be 1.
         //the total sum of poits weigh is so far totalNEdges.
         //So first, count nunmber of marker edges
-        float w_markers=0.3;
+        float w_markers=0.95;
         int totalNEdges=map_matches.size()+ marker_poses.size();
         double weight_marker= ((w_markers *totalNEdges)/ (1.- w_markers))/float(KpWeightSum);
 
diff --git a/utils/tslam_combine_map.cpp b/utils/tslam_combine_map.cpp
index bc0377c..40e105f 100644
--- a/utils/tslam_combine_map.cpp
+++ b/utils/tslam_combine_map.cpp
@@ -45,90 +45,54 @@ or implied Andrea Settimi and Hong-Bin Yang.
 #include "utils/mapmanager.h"
 
 class CmdLineParser{int argc; char **argv;
-                public: CmdLineParser(int _argc,char **_argv):argc(_argc),argv(_argv){}  bool operator[] ( string param ) {int idx=-1;  for ( int i=0; i<argc && idx==-1; i++ ) if ( string ( argv[i] ) ==param ) idx=i;    return ( idx!=-1 ) ;    } string operator()(string param,string defvalue=""){int idx=-1;    for ( int i=0; i<argc && idx==-1; i++ ) if ( string ( argv[i] ) ==param ) idx=i; if ( idx==-1 ) return defvalue;   else  return ( argv[  idx+1] ); }
-                    std::vector<std::string> getAllInstances(string str){
-                        std::vector<std::string> ret;
-                        for(int i=0;i<argc-1;i++){
-                            if (string(argv[i])==str)
-                                ret.push_back(argv[i+1]);
-                        }
-                        return ret;
-                    }
-                   };
-
-namespace tslam {
-struct DebugTest{
-    static void removeMapPointObservation(tslam::Map &map,uint32_t point_idx,uint32_t frame_idx){
-        map.removeMapPointObservation(point_idx,frame_idx,3);
+public: CmdLineParser(int _argc,char **_argv):argc(_argc),argv(_argv){}  bool operator[] ( string param ) {int idx=-1;  for ( int i=0; i<argc && idx==-1; i++ ) if ( string ( argv[i] ) ==param ) idx=i;    return ( idx!=-1 ) ;    } string operator()(string param,string defvalue=""){int idx=-1;    for ( int i=0; i<argc && idx==-1; i++ ) if ( string ( argv[i] ) ==param ) idx=i; if ( idx==-1 ) return defvalue;   else  return ( argv[  idx+1] ); }
+    std::vector<std::string> getAllInstances(string str){
+        std::vector<std::string> ret;
+        for(int i=0;i<argc-1;i++){
+            if (string(argv[i])==str)
+                ret.push_back(argv[i+1]);
+        }
+        return ret;
     }
 };
+
+namespace tslam {
+    struct DebugTest{
+        static void removeMapPointObservation(tslam::Map &map,uint32_t point_idx,uint32_t frame_idx){
+            map.removeMapPointObservation(point_idx,frame_idx,3);
+        }
+    };
 }
 
 int main(int argc,char **argv){
     try {
         if(argc<4)throw std::runtime_error("Usage: inmap_A inmap_B outmap [iterations]");
+        string mapAPath=argv[1];
+        string mapBPath=argv[2];
+        string outputPath=argv[3];
 
-        std::shared_ptr<tslam::Map> TheMapA, TheMapB;
-        TheMapA = std::make_shared<tslam::Map>();
-        TheMapB = std::make_shared<tslam::Map>();
-
-        TheMapA->readFromFile(argv[1]);
-        TheMapB->readFromFile(argv[2]);
-
-        int niters=50;
-        if(argc>=5)niters=stoi(argv[4]);
-
-        TheMapA->merge(TheMapB);
-
-        // TheMapB->projectTo(*TheMapA);
-
-        // std::shared_ptr<tslam::MapManager> TheMapManager;
-        // TheMapManager = std::make_shared<tslam::MapManager>();
-        // TheMapManager->setParams(TheMapA, true);
-
-        // std::map<uint32_t, tslam::Frame*> frameMapB; // idx of TheMapB -> TheMapA
-        // std::map<uint32_t, tslam::MapPoint*> pointMapB;
-
-        // // Add point first
-        // cout << "Total points in A: " << TheMapA->map_points.size() << "points." << endl;
-        // cout << "Total points in B: " << TheMapB->map_points.size() << "points." << endl;
-
-        // for(auto ptIter = TheMapB->map_points.begin(); ptIter != TheMapB->map_points.end(); ++ptIter){
-        //     if(pointMapB.count(ptIter->id) == 0){
-        //         pointMapB[ptIter->id] = &TheMapA->addNewPoint(0);
-        //         pointMapB[ptIter->id]->setCoordinates(ptIter->getCoordinates());
-        //         pointMapB[ptIter->id]->setStable(true);
-        //         pointMapB[ptIter->id]->setBad(false);
-        //         pointMapB[ptIter->id]->setSeen();
-        //         pointMapB[ptIter->id]->setVisible();
-        //     }
-        // }
+        int niters = 50;
+        if(argc>=5)niters = stoi(argv[4]);
 
-        // for(auto kfIter = TheMapB->keyframes.begin(); kfIter != TheMapB->keyframes.end(); ++kfIter){
-        //     for(int i = 0 ; i < kfIter->ids.size() ; i++){
-        //         if (kfIter->ids[i] != std::numeric_limits<uint32_t>::max()){
-        //             kfIter->ids[i] = pointMapB[kfIter->ids[i]]->id;
-        //         }
-        //     }
-        //     TheMapManager->addKeyFrame(&(*kfIter));
-        // }
+        std::shared_ptr<tslam::Map> mapA, mapB;
+        mapA = std::make_shared<tslam::Map>();
+        mapB = std::make_shared<tslam::Map>();
 
-        // cout << "Markers: ";
-        // for(auto markerIter = TheMapA->map_markers.begin(); markerIter != TheMapA->map_markers.end(); ++markerIter){
-        //     cout << markerIter->first << " ";
-        // }
-        // cout << endl;
+        mapA->readFromFile(mapAPath);
+        mapB->readFromFile(mapBPath);
 
-        string filename = argv[3];
-        TheMapA->saveToFile(filename+".no-opt");
+        mapA->merge(mapB);
 
-        TheMapA->optimize(50);
+        auto outputBasePath = outputPath.substr(0, outputPath.find_last_of("."));
+        // mapA->saveToFile(outputBasePath+".no-opt.map");
+        mapA->optimize(50);
 
-        cout<<"Final Camera Params "<<endl;
-        cout<<TheMapA->keyframes.begin()->imageParams.CameraMatrix<<endl;
-        cout<<TheMapA->keyframes.begin()->imageParams.Distorsion<<endl;
+        mapA->saveToFile(outputPath);
+        mapA->saveToMarkerMap(outputBasePath + ".yml");
 
-        TheMapA->saveToFile(argv[3]);
+        cout << "Camera Params After Combine" << endl;
+        cout << mapA->keyframes.begin()->imageParams.CameraMatrix << endl;
+        cout << mapA->keyframes.begin()->imageParams.Distorsion << endl;
 
     } catch (const std::exception &ex) {
         std::cerr<<ex.what()<<std::endl;