i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-09-01 17:10:26 +08:00
Code Issues Packages Projects Releases Wiki Activity

Graph optimization: fixed stop error criterion for g2o and gtsam, previous optimized poses are now used as guess for the optimized graph

Browse Source
This commit is contained in:
matlabbe
2015-11-18 19:40:46 -05:00
parent 1e5bcfded8
commit 10d683a38e
6 changed files with 192 additions and 63 deletions
Download Patch File Download Diff File Expand all files Collapse all files

16
corelib/include/rtabmap/core/Graph.h
View File

@@ -82,7 +82,9 @@ public:
int rootId,
const std::map<int, Transform> & poses,
const std::multimap<int, Link> & constraints,
std::list<std::map<int, Transform> > * intermediateGraphes = 0);
std::list<std::map<int, Transform> > * intermediateGraphes = 0,
double * finalError = 0,
int * iterationsDone = 0);
virtual std::map<int, Transform> optimizeBA(
int rootId,
const std::map<int, Transform> & poses,
@@ -137,7 +139,9 @@ public:
int rootId,
const std::map<int, Transform> & poses,
const std::multimap<int, Link> & edgeConstraints,
std::list<std::map<int, Transform> > * intermediateGraphes = 0);
std::list<std::map<int, Transform> > * intermediateGraphes = 0,
double * finalError = 0,
int * iterationsDone = 0);
};
class RTABMAP_EXP G2OOptimizer : public Optimizer
@@ -169,7 +173,9 @@ public:
int rootId,
const std::map<int, Transform> & poses,
const std::multimap<int, Link> & edgeConstraints,
std::list<std::map<int, Transform> > * intermediateGraphes = 0);
std::list<std::map<int, Transform> > * intermediateGraphes = 0,
double * finalError = 0,
int * iterationsDone = 0);
};
class RTABMAP_EXP GTSAMOptimizer : public Optimizer
@@ -196,7 +202,9 @@ public:
int rootId,
const std::map<int, Transform> & poses,
const std::multimap<int, Link> & edgeConstraints,
std::list<std::map<int, Transform> > * intermediateGraphes = 0);
std::list<std::map<int, Transform> > * intermediateGraphes = 0,
double * finalError = 0,
int * iterationsDone = 0);
};
class RTABMAP_EXP CVSBAOptimizer : public Optimizer

9
corelib/include/rtabmap/core/Rtabmap.h
View File

@@ -153,12 +153,17 @@ private:
void optimizeCurrentMap(int id,
bool lookInDatabase,
std::map<int, Transform> & optimizedPoses,
std::multimap<int, Link> * constraints = 0) const;
std::multimap<int, Link> * constraints = 0,
double * error = 0,
int * iterationsDone = 0) const;
std::map<int, Transform> optimizeGraph(
int fromId,
const std::set<int> & ids,
const std::map<int, Transform> & guessPoses,
bool lookInDatabase,
std::multimap<int, Link> * constraints = 0) const;
std::multimap<int, Link> * constraints = 0,
double * error = 0,
int * iterationsDone = 0) const;
void updateGoalIndex();
bool computePath(int targetNode, std::map<int, Transform> nodes, const std::multimap<int, rtabmap::Link> & constraints);

2
corelib/include/rtabmap/core/Statistics.h
View File

@@ -63,6 +63,8 @@ class RTABMAP_EXP Statistics
RTABMAP_STATS(Loop, Visual_inliers,);
RTABMAP_STATS(Loop, Last_id,);
RTABMAP_STATS(Loop, Optimization_max_error, m);
RTABMAP_STATS(Loop, Optimization_error, );
RTABMAP_STATS(Loop, Optimization_iterations, );
RTABMAP_STATS(LocalLoop, Time_closures,);
RTABMAP_STATS(LocalLoop, Space_last_closure_id,);