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

DatabaseViewer: added graph depth param. Rtabmap: when the robot is not moving, now deleting location "after" retrieval

Browse Source
This commit is contained in:
Mathieu Labbe
2015-03-16 18:22:54 -04:00
parent 49883a67e7
commit b1827aeb89
4 changed files with 102 additions and 24 deletions
Download Patch File Download Diff File Expand all files Collapse all files

37
corelib/src/Graph.cpp
View File

@@ -148,16 +148,16 @@ void Optimizer::getConnectedGraph(
linksOut.clear();
std::set<int> ids;
std::list<int> curentDepthList;
std::set<int> curentDepth;
std::set<int> nextDepth;
nextDepth.insert(fromId);
int d = 0;
while((depth == 0 || d < depth) && nextDepth.size())
{
curentDepthList = std::list<int>(nextDepth.begin(), nextDepth.end());
curentDepth = nextDepth;
nextDepth.clear();
for(std::list<int>::iterator jter = curentDepthList.begin(); jter!=curentDepthList.end(); ++jter)
for(std::set<int>::iterator jter = curentDepth.begin(); jter!=curentDepth.end(); ++jter)
{
if(ids.find(*jter) == ids.end())
{
@@ -170,16 +170,33 @@ void Optimizer::getConnectedGraph(
{
if(ids.find(iter->second.to()) == ids.end() && uContains(posesIn, iter->second.to()))
{
linksOut.insert(*iter);
nextDepth.insert(iter->second.to());
if(depth == 0 || d < depth-1)
{
linksOut.insert(*iter);
}
else if(curentDepth.find(iter->second.to()) != curentDepth.end() ||
ids.find(iter->second.to()) != ids.end())
{
linksOut.insert(*iter);
}
}
}
else if(iter->second.to() == *jter)
{
if(ids.find(iter->second.from()) == ids.end() && uContains(posesIn, iter->second.from()))
{
linksOut.insert(*iter);
nextDepth.insert(iter->second.from());
if(depth == 0 || d < depth-1)
{
linksOut.insert(*iter);
}
else if(curentDepth.find(iter->second.from()) != curentDepth.end() ||
ids.find(iter->second.from()) != ids.end())
{
linksOut.insert(*iter);
}
}
}
}
@@ -199,7 +216,7 @@ std::map<int, Transform> TOROOptimizer::optimize(
std::list<std::map<int, Transform> > * intermediateGraphes) // contains poses after tree init to last one before the end
{
std::map<int, Transform> optimizedPoses;
UDEBUG("Optimizing graph...");
UDEBUG("Optimizing graph (pose=%d constraints=%d)...", (int)poses.size(), (int)edgeConstraints.size());
if(edgeConstraints.size()>=1 && poses.size()>=2 && iterations() > 0)
{
// Apply TORO optimization
@@ -263,6 +280,8 @@ std::map<int, Transform> TOROOptimizer::optimize(
int id2 = iter->second.to();
AISNavigation::TreePoseGraph2::Vertex* v1=pg2.vertex(id1);
AISNavigation::TreePoseGraph2::Vertex* v2=pg2.vertex(id2);
UASSERT(v1 != 0);
UASSERT(v2 != 0);
AISNavigation::TreePoseGraph2::Transformation t(p);
if (!pg2.addEdge(v1, v2, t, inf))
{
@@ -300,6 +319,8 @@ std::map<int, Transform> TOROOptimizer::optimize(
int id2 = iter->second.to();
AISNavigation::TreePoseGraph3::Vertex* v1=pg3.vertex(id1);
AISNavigation::TreePoseGraph3::Vertex* v2=pg3.vertex(id2);
UASSERT(v1 != 0);
UASSERT(v2 != 0);
AISNavigation::TreePoseGraph3::Transformation t(p);
if (!pg3.addEdge(v1, v2, t, inf))
{
@@ -652,6 +673,8 @@ std::map<int, Transform> G2OOptimizer::optimize(
g2o::EdgeSE2 * e = new g2o::EdgeSE2();
g2o::VertexSE2* v1 = (g2o::VertexSE2*)optimizer.vertex(id1);
g2o::VertexSE2* v2 = (g2o::VertexSE2*)optimizer.vertex(id2);
UASSERT(v1 != 0);
UASSERT(v2 != 0);
e->setVertex(0, v1);
e->setVertex(1, v2);
e->setMeasurement(g2o::SE2(iter->second.transform().x(), iter->second.transform().y(), iter->second.transform().theta()));
@@ -685,6 +708,8 @@ std::map<int, Transform> G2OOptimizer::optimize(
g2o::EdgeSE3 * e = new g2o::EdgeSE3();
g2o::VertexSE3* v1 = (g2o::VertexSE3*)optimizer.vertex(id1);
g2o::VertexSE3* v2 = (g2o::VertexSE3*)optimizer.vertex(id2);
UASSERT(v1 != 0);
UASSERT(v2 != 0);
e->setVertex(0, v1);
e->setVertex(1, v2);
e->setMeasurement(constraint);

34
corelib/src/Rtabmap.cpp
View File

@@ -855,6 +855,7 @@ bool Rtabmap::process(const SensorData & data)
//============================================================
// Metric
//============================================================
bool smallDisplacement = false;
if(_rgbdSlamMode)
{
//Verify if there was a rehearsal
@@ -882,10 +883,9 @@ bool Rtabmap::process(const SensorData & data)
fabs(pitch) < _rgbdAngularUpdate &&
fabs(yaw) < _rgbdAngularUpdate)))
{
UWARN("Ignoring location %d because the displacement is too small! (d=%f a=%f)",
signature->id(), _rgbdLinearUpdate, _rgbdAngularUpdate);
_memory->deleteLocation(signature->id());
return false;
// This will disable global loop closure detection, only retrieval will be done.
// The location will also be deleted at the end.
smallDisplacement = true;
}
}
}
@@ -1003,7 +1003,7 @@ bool Rtabmap::process(const SensorData & data)
//============================================================
// Bayes filter update
//============================================================
if(!signature->isBadSignature())
if(!signature->isBadSignature() && !smallDisplacement)
{
// 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
@@ -1037,6 +1037,11 @@ bool Rtabmap::process(const SensorData & data)
timePosteriorCalculation = timer.ticks();
ULOGGER_INFO("timePosteriorCalculation=%fs",timePosteriorCalculation);
// For statistics, copy weights
if(_publishStats && (_publishLikelihood || _publishPdf))
{
weights = _memory->getWeights();
}
timer.start();
//============================================================
@@ -1106,6 +1111,10 @@ bool Rtabmap::process(const SensorData & data)
}
} // if(_memory->getWorkingMemSize())
}// !isBadSignature
else if(!signature->isBadSignature() && smallDisplacement)
{
_highestHypothesis = lastHighestHypothesis;
}
//============================================================
// Before retrieval, make sure the trash has finished
@@ -1115,12 +1124,6 @@ bool Rtabmap::process(const SensorData & data)
timeJoiningTrash = timer.ticks();
ULOGGER_INFO("Time emptying memory trash = %fs, joining (actual overhead) = %fs", timeEmptyingTrash, timeJoiningTrash);
// also copy weights before the memory is changed
if(_publishStats && (_publishLikelihood || _publishPdf))
{
weights = _memory->getWeights();
}
//============================================================
// RETRIEVAL 1/3 : Loop closure neighbors reactivation
//============================================================
@@ -1771,6 +1774,15 @@ bool Rtabmap::process(const SensorData & data)
signature->getLinks().size() == 0 && // alone in the current map
_memory->getWorkingMem().size()>1) // The working memory should not be empty
{
UWARN("Ignoring location %d because a global loop closure is required before starting a new map!",
signature->id());
_memory->deleteLocation(signature->id());
}
else if(smallDisplacement)
{
UWARN("Ignoring location %d because the displacement is too small! (d=%f a=%f)",
signature->id(), _rgbdLinearUpdate, _rgbdAngularUpdate);
// If there is a too small displacement, remove the node
_memory->deleteLocation(signature->id());
}