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

Creating a new map when variance >= 9999 is detected

Browse Source
This commit is contained in:
matlabbe
2016-02-18 11:17:42 -05:00
parent b09429c5d8
commit c1c62b0738
6 changed files with 17 additions and 5 deletions
Download Patch File Download Diff File Expand all files Collapse all files

5
corelib/src/Memory.cpp
View File

@@ -2599,6 +2599,7 @@ bool Memory::rehearsalMerge(int oldId, int newId)
Signature * newS = _getSignature(newId); Signature * newS = _getSignature(newId);
if(oldS && newS && _incrementalMemory) if(oldS && newS && _incrementalMemory)
{ {
UASSERT_MSG(oldS->getWeight() >= 0 && newS->getWeight() >= 0, uFormat("%d %d", oldS->getWeight(), newS->getWeight()).c_str());
std::map<int, Link>::const_iterator iter = oldS->getLinks().find(newS->id()); std::map<int, Link>::const_iterator iter = oldS->getLinks().find(newS->id());
if(iter != oldS->getLinks().end() && if(iter != oldS->getLinks().end() &&
iter->second.type() != Link::kNeighbor && iter->second.type() != Link::kNeighbor &&
@@ -2610,7 +2611,9 @@ bool Memory::rehearsalMerge(int oldId, int newId)
} }
UASSERT(!newS->isSaved()); UASSERT(!newS->isSaved());
UINFO("Rehearsal merging %d and %d", oldS->id(), newS->id()); UINFO("Rehearsal merging %d (w=%d) and %d (w=%d)",
oldS->id(), oldS->getWeight(),
newS->id(), newS->getWeight());
bool fullMerge; bool fullMerge;
bool intermediateMerge = false; bool intermediateMerge = false;

2
corelib/src/OdometryF2F.cpp
View File

@@ -117,6 +117,8 @@ Transform OdometryF2F::computeTransform(
{ {
//return Identity //return Identity
output = Transform::getIdentity(); output = Transform::getIdentity();
// a very high variance tells that the new pose is not linked with the previous one
regInfo.variance = 9999;
} }
if(!output.isNull()) if(!output.isNull())

2
corelib/src/OdometryLocalMap.cpp
View File

@@ -331,6 +331,8 @@ Transform OdometryLocalMap::computeTransform(
if(fixedLocalMapPath_.empty() && (int)uniques.size() >= regVis_->getMinInliers()) if(fixedLocalMapPath_.empty() && (int)uniques.size() >= regVis_->getMinInliers())
{ {
output.setIdentity(); output.setIdentity();
// a very high variance tells that the new pose is not linked with the previous one
regInfo.variance = 9999;
Transform t = this->getPose(); // initial pose maybe not identity... Transform t = this->getPose(); // initial pose maybe not identity...
for(std::list<int>::iterator iter = uniques.begin(); iter!=uniques.end(); ++iter) for(std::list<int>::iterator iter = uniques.begin(); iter!=uniques.end(); ++iter)

5
corelib/src/OdometryMono.cpp
View File

@@ -981,6 +981,11 @@ Transform OdometryMono::computeTransform(const SensorData & data, OdometryInfo *
{ {
//return Identity //return Identity
output = Transform::getIdentity(); output = Transform::getIdentity();
if(info)
{
// a very high variance tells that the new pose is not linked with the previous one
info->variance = 9999;
}
// generate kpts // generate kpts
if(memory_->update(SensorData(newFrame))) if(memory_->update(SensorData(newFrame)))

4
corelib/src/Rtabmap.cpp
View File

@@ -1204,8 +1204,8 @@ bool Rtabmap::process(
// Bayes filter update // Bayes filter update
//============================================================ //============================================================
int previousId = signature->getLinks().size() == 1?signature->getLinks().begin()->first:0; int previousId = signature->getLinks().size() == 1?signature->getLinks().begin()->first:0;
// Not a bad signature, not a small displacement unless the previous signature didn't have a loop closure // Not a bad signature, not an intermediate node, not a small displacement unless the previous signature didn't have a loop closure
if(!signature->isBadSignature() && (!smallDisplacement || _memory->getLoopClosureLinks(previousId, false).size() == 0)) if(!signature->isBadSignature() && signature->getWeight()>=0 && (!smallDisplacement || _memory->getLoopClosureLinks(previousId, false).size() == 0))
{ {
// If the working memory is empty, don't do the detection. It happens when it // If the working memory is empty, don't do the detection. It happens when it
// is the first time the detector is started (there needs some images to // is the first time the detector is started (there needs some images to

4
corelib/src/RtabmapThread.cpp
View File

@@ -544,9 +544,9 @@ void RtabmapThread::addData(const OdometryEvent & odomEvent)
ignoreFrame = true; ignoreFrame = true;
} }
} }
if(_dataBufferMaxSize > 0 && !lastPose_.isIdentity() && odomEvent.pose().isIdentity()) if(_dataBufferMaxSize > 0 && !lastPose_.isIdentity() && (odomEvent.pose().isIdentity() || odomEvent.info().variance>=9999))
{ {
UWARN("Odometry is reset (identity pose detected). Increment map id!"); UWARN("Odometry is reset (identity pose or high variance (%f) detected). Increment map id!", odomEvent.info().variance);
pushNewState(kStateTriggeringMap); pushNewState(kStateTriggeringMap);
_rotVariance = 0; _rotVariance = 0;
_transVariance = 0; _transVariance = 0;