Skip to content

Commit

Permalink
Merge pull request #7 from ibois-epfl/fix-combine-map
Browse files Browse the repository at this point in the history 
Increase tags' weight in solvePnP & Fix combine map util
  • Loading branch information
@9and3
9and3 authored Aug 26, 2024
2 parents 1bb9d99 + bb61303 commit 4dd49bb
Show file tree
Hide file tree
Showing 2 changed files with 35 additions and 71 deletions.
2 changes: 1 addition & 1 deletion src/optimization/pnpsolver.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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);

Expand Down
104 changes: 34 additions & 70 deletions utils/tslam_combine_map.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down

0 comments on commit 4dd49bb

Please sign in to comment.