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

merged master to 0.11.0

Browse Source
This commit is contained in:
matlabbe
2015-12-02 11:55:46 -05:00
parent 676cd8cbd9 60f34fe994
commit 818eda005f
5 changed files with 61 additions and 8 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
corelib/src/CMakeLists.txt
View File

@@ -162,8 +162,6 @@ IF(G2O_FOUND)
${LIBRARIES}
${G2O_LIBRARIES}
)
#Newest versions require std11
SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11")
SET(SRC_FILES
${SRC_FILES}

15
corelib/src/OptimizerG2O.cpp
View File

@@ -302,6 +302,8 @@ std::map<int, Transform> OptimizerG2O::optimize(
UDEBUG("Initial optimization...");
optimizer.initializeOptimization();
UASSERT(optimizer.verifyInformationMatrices());
UINFO("g2o optimizing begin (max iterations=%d, robust=%d)", iterations(), isRobust()?1:0);
int it = 0;
UTimer timer;
@@ -361,6 +363,13 @@ std::map<int, Transform> OptimizerG2O::optimize(
optimizer.computeActiveErrors();
double chi2 = optimizer.activeRobustChi2();
UDEBUG("iteration %d: %d nodes, %d edges, chi2: %f", i, (int)optimizer.vertices().size(), (int)optimizer.edges().size(), chi2);
if(i>0 && optimizer.activeRobustChi2() > 1000000000000.0)
{
UWARN("g2o: Large optimimzation error detected (%f), aborting optimization!");
return optimizedPoses;
}
double errorDelta = lastError - chi2;
if(i>0 && errorDelta < this->epsilon())
{
@@ -398,6 +407,12 @@ std::map<int, Transform> OptimizerG2O::optimize(
}
UINFO("g2o optimizing end (%d iterations done, error=%f, time = %f s)", it, optimizer.activeRobustChi2(), timer.ticks());
if(optimizer.activeRobustChi2() > 1000000000000.0)
{
UWARN("g2o: Large optimimzation error detected (%f), aborting optimization!");
return optimizedPoses;
}
if(isSlam2d())
{
for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)

32
corelib/src/Rtabmap.cpp
View File

@@ -874,7 +874,7 @@ bool Rtabmap::process(
"Image %d is ignored!", data.id());
return false;
}
else
else if(_memory->isIncremental()) // only in mapping mode
{
// Detect if the odometry is reset. If yes, trigger a new map.
if(_memory->getLastWorkingSignature())
@@ -2080,12 +2080,24 @@ bool Rtabmap::process(
std::map<int, Transform> poses = _optimizedPoses;
std::multimap<int, Link> constraints;
optimizeCurrentMap(signature->id(), false, poses, &constraints, &optimizationError, &optimizationIterations);
UASSERT(poses.find(signature->id()) != poses.end());
// Check added loop closures have broken the graph
// (in case of wrong loop closures).
bool updateConstraints = true;
if(_memory->isIncremental() && // FIXME: not tested in localization mode, so do it only in mapping mode
if(poses.empty())
{
UWARN("Graph optimization failed! Rejecting last loop closures added.");
for(std::list<std::pair<int, int> >::iterator iter=loopClosureLinksAdded.begin(); iter!=loopClosureLinksAdded.end(); ++iter)
{
_memory->removeLink(iter->first, iter->second);
UWARN("Loop closure %d->%d rejected!", iter->first, iter->second);
}
updateConstraints = false;
_loopClosureHypothesis.first = 0;
lastLocalSpaceClosureId = 0;
rejectedHypothesis = true;
}
else if(_memory->isIncremental() && // FIXME: not tested in localization mode, so do it only in mapping mode
_optimizationMaxLinearError > 0.0f &&
loopClosureLinksAdded.size())
{
@@ -2365,7 +2377,7 @@ bool Rtabmap::process(
}
else
{
UASSERT_MSG(uContains(_optimizedPoses, _lastLocalizationNodeId), uFormat("id=%d", _lastLocalizationNodeId).c_str());
UASSERT_MSG(uContains(_optimizedPoses, _lastLocalizationNodeId), uFormat("id=%d isInWM?=%d", _lastLocalizationNodeId, _memory->isInWM(_lastLocalizationNodeId)?1:0).c_str());
id = _lastLocalizationNodeId;
UDEBUG("Refresh local map from %d", id);
}
@@ -2379,6 +2391,10 @@ bool Rtabmap::process(
}
if(id > 0)
{
if(_lastLocalizationNodeId != 0)
{
_lastLocalizationNodeId = id;
}
UASSERT_MSG(_memory->getSignature(id) != 0, uFormat("id=%d", id).c_str());
std::map<int, int> ids = _memory->getNeighborsId(id, 0, 0, true);
for(std::map<int, Transform>::iterator iter=_optimizedPoses.begin(); iter!=_optimizedPoses.end();)
@@ -2386,6 +2402,7 @@ bool Rtabmap::process(
if(!uContains(ids, iter->first))
{
UDEBUG("Removed %d from local map", iter->first);
UASSERT(iter->first != _lastLocalizationNodeId);
_optimizedPoses.erase(iter++);
}
else
@@ -2824,7 +2841,12 @@ void Rtabmap::optimizeCurrentMap(
}
else
{
UERROR("Failed to optimize the graph! Keeping the graph without optimization...");
UERROR("Failed to optimize the graph! returning empty optimized poses...");
optimizedPoses.clear();
if(constraints)
{
constraints->clear();
}
}
}
}