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

updated some debug logs

Browse Source
This commit is contained in:
Mathieu Labbe
2015-03-19 16:55:43 -04:00
parent 9229c9887b
commit b518edf6ce
2 changed files with 9 additions and 4 deletions
Download Patch File Download Diff File Expand all files Collapse all files

4
corelib/src/Graph.cpp
View File

@@ -395,8 +395,8 @@ std::map<int, Transform> TOROOptimizer::optimize(
// compute the error and dump it
double mte, mre, are, ate;
double error=pg3.error(&mre, &mte, &are, &ate);
UDEBUG("iteration %d RotGain=%f global error=%f error/constraint=%f mte=%f mre=%f are=%f ate=%f",
i, pg3.getRotGain(), error, error/pg3.edges.size(), mte, mre, are, ate);
UDEBUG("i %d RotGain=%f global error=%f error/constraint=%f",
i, pg3.getRotGain(), error, error/pg3.edges.size());
}
}
UINFO("TORO iterate end");

9
corelib/src/Rtabmap.cpp
View File

@@ -2188,8 +2188,7 @@ void Rtabmap::optimizeCurrentMap(
std::map<int, Transform> poses;
std::multimap<int, Link> edgeConstraints;
_memory->getMetricConstraints(uKeys(ids), poses, edgeConstraints, lookInDatabase);
UDEBUG("poses=%d, edgeConstraints=%d", (int)poses.size(), (int)edgeConstraints.size());
UINFO("get constraints time %f s", timer.ticks());
UINFO("get constraints (%d poses, %d edges) time %f s", (int)poses.size(), (int)edgeConstraints.size(), timer.ticks());
if(constraints)
{
@@ -2207,6 +2206,12 @@ void Rtabmap::optimizeCurrentMap(
optimizedPoses = _graphOptimizer->optimize(id, poses, edgeConstraints);
}
UINFO("optimize time %f s", timer.ticks());
if(_memory->getSignature(id) && uContains(optimizedPoses, id))
{
Transform t = optimizedPoses.at(id) * _memory->getSignature(id)->getPose().inverse();
UINFO("Correction (from node %d) %s", id, t.prettyPrint().c_str());
}
}
}