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());
}

24
guilib/src/DatabaseViewer.cpp
View File

@@ -194,6 +194,8 @@ DatabaseViewer::DatabaseViewer(QWidget * parent) :
connect(ui_->checkBox_ignoreCovariance, SIGNAL(stateChanged(int)), this, SLOT(updateGraphView()));
connect(ui_->comboBox_graphOptimizer, SIGNAL(currentIndexChanged(int)), this, SLOT(updateGraphView()));
connect(ui_->checkBox_2dslam, SIGNAL(stateChanged(int)), this, SLOT(updateGraphView()));
connect(ui_->spinBox_optimizationDepth, SIGNAL(editingFinished()), this, SLOT(updateGraphView()));
connect(ui_->groupBox_gridFromProjection, SIGNAL(clicked(bool)), this, SLOT(updateGrid()));
connect(ui_->doubleSpinBox_gridCellSize, SIGNAL(editingFinished()), this, SLOT(updateGrid()));
@@ -210,6 +212,7 @@ DatabaseViewer::DatabaseViewer(QWidget * parent) :
connect(ui_->checkBox_ignoreCovariance, SIGNAL(stateChanged(int)), this, SLOT(configModified()));
connect(ui_->comboBox_graphOptimizer, SIGNAL(currentIndexChanged(int)), this, SLOT(configModified()));
connect(ui_->checkBox_2dslam, SIGNAL(stateChanged(int)), this, SLOT(configModified()));
connect(ui_->spinBox_optimizationDepth, SIGNAL(valueChanged(int)), this, SLOT(configModified()));
connect(ui_->groupBox_gridFromProjection, SIGNAL(clicked(bool)), this, SLOT(configModified()));
connect(ui_->doubleSpinBox_gridCellSize, SIGNAL(valueChanged(double)), this, SLOT(configModified()));
connect(ui_->spinBox_projDecimation, SIGNAL(valueChanged(int)), this, SLOT(configModified()));
@@ -309,6 +312,7 @@ void DatabaseViewer::readSettings()
ui_->checkBox_ignoreCovariance->setChecked(settings.value("ignoreCovariance", ui_->checkBox_ignoreCovariance->isChecked()).toBool());
ui_->comboBox_graphOptimizer->setCurrentIndex(settings.value("strategy", ui_->comboBox_graphOptimizer->currentIndex()).toInt());
ui_->checkBox_2dslam->setChecked(settings.value("slam2d", ui_->checkBox_2dslam->isChecked()).toBool());
ui_->spinBox_optimizationDepth->setValue(settings.value("depth", ui_->spinBox_optimizationDepth->value()).toInt());
settings.endGroup();
settings.beginGroup("grid");
@@ -380,6 +384,7 @@ void DatabaseViewer::writeSettings()
settings.setValue("ignoreCovariance", ui_->checkBox_ignoreCovariance->isChecked());
settings.setValue("strategy", ui_->comboBox_graphOptimizer->currentIndex());
settings.setValue("slam2d", ui_->checkBox_2dslam->isChecked());
settings.setValue("depth", ui_->spinBox_optimizationDepth->value());
settings.endGroup();
// save Grid settings
@@ -947,7 +952,13 @@ void DatabaseViewer::view3DMap()
double maxDepth = QInputDialog::getDouble(this, tr("Camera depth?"), tr("Maximum depth (m, 0=no max):"), 4.0, 0, 10, 2, &ok);
if(ok)
{
const std::map<int, Transform> & optimizedPoses = uValueAt(graphes_, ui_->horizontalSlider_iterations->value());
std::map<int, Transform> optimizedPoses = uValueAt(graphes_, ui_->horizontalSlider_iterations->value());
if(ui_->groupBox_posefiltering->isChecked())
{
optimizedPoses = graph::radiusPosesFiltering(optimizedPoses,
ui_->doubleSpinBox_posefilteringRadius->value(),
ui_->doubleSpinBox_posefilteringAngle->value()*CV_PI/180.0);
}
if(optimizedPoses.size() > 0)
{
rtabmap::DetailedProgressDialog progressDialog(this);
@@ -1067,7 +1078,13 @@ void DatabaseViewer::generate3DMap()
QString path = QFileDialog::getExistingDirectory(this, tr("Save directory"), pathDatabase_);
if(!path.isEmpty())
{
const std::map<int, Transform> & optimizedPoses = uValueAt(graphes_, ui_->horizontalSlider_iterations->value());
std::map<int, Transform> optimizedPoses = uValueAt(graphes_, ui_->horizontalSlider_iterations->value());
if(ui_->groupBox_posefiltering->isChecked())
{
optimizedPoses = graph::radiusPosesFiltering(optimizedPoses,
ui_->doubleSpinBox_posefilteringRadius->value(),
ui_->doubleSpinBox_posefilteringAngle->value()*CV_PI/180.0);
}
if(optimizedPoses.size() > 0)
{
rtabmap::DetailedProgressDialog progressDialog;
@@ -2302,7 +2319,8 @@ void DatabaseViewer::updateGraphView()
poses_,
links,
posesOut,
linksOut);
linksOut,
ui_->spinBox_optimizationDepth->value());
QTime time;
time.start();

31
guilib/src/ui/DatabaseViewer.ui
View File

@@ -1186,7 +1186,7 @@
</property>
</widget>
</item>
<item row="3" column="1">
<item row="4" column="1">
<widget class="QLabel" name="label_41">
<property name="text">
<string>2d SLAM</string>
@@ -1200,14 +1200,14 @@
</property>
</widget>
</item>
<item row="2" column="1">
<item row="3" column="1">
<widget class="QLabel" name="label_34">
<property name="text">
<string>Ignore covariance</string>
</property>
</widget>
</item>
<item row="2" column="0">
<item row="3" column="0">
<widget class="QCheckBox" name="checkBox_ignoreCovariance">
<property name="text">
<string/>
@@ -1217,7 +1217,7 @@
</property>
</widget>
</item>
<item row="3" column="0">
<item row="4" column="0">
<widget class="QCheckBox" name="checkBox_2dslam">
<property name="text">
<string/>
@@ -1227,6 +1227,29 @@
</property>
</widget>
</item>
<item row="2" column="1">
<widget class="QLabel" name="label_8">
<property name="text">
<string>Depth (0=inf)</string>
</property>
</widget>
</item>
<item row="2" column="0">
<widget class="QSpinBox" name="spinBox_optimizationDepth">
<property name="suffix">
<string/>
</property>
<property name="minimum">
<number>0</number>
</property>
<property name="maximum">
<number>1000</number>
</property>
<property name="value">
<number>0</number>
</property>
</widget>
</item>
</layout>
</item>
<item>