Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Increase tags' weight in solvePnP & Fix combine map util #7

Merged
merged 2 commits into from
Aug 26, 2024
Merged

Increase tags' weight in solvePnP & Fix combine map util #7

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
1d1e972
Fix: tslam combine map
Petingo Jul 28, 2024
bb61303
Merge branch 'main' into fix-combine-map
Petingo Aug 19, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
2 changes: 1 addition & 1 deletion src/optimization/pnpsolver.cpp
View file
Open in desktop
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
Open in desktop
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
Loading