i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-09-02 01:20:25 +08:00
Code Issues Packages Projects Releases Wiki Activity

Some fixes:

Browse Source 
-Memory: Fixed wrongly rejected scan matching transform because of too large correction
-DBViewer: Fixed not shown proximity scans
-DBDriver: Fixed not loaded links' user_data on getAllLinks() method
-MainWindow: Added planning statistics
-DBReader: set a maximum of 10 sec sleep if the map ID has changed
This commit is contained in:
matlabbe
2015-10-26 12:59:20 -04:00
parent 463a5d973c
commit 02ebb3c7e8
12 changed files with 161 additions and 54 deletions
Download Patch File Download Diff File Expand all files Collapse all files

7
guilib/include/rtabmap/gui/StatsToolBox.h
View File

@@ -48,7 +48,8 @@ class RTABMAPGUI_EXP StatItem : public QWidget
public:
StatItem(const QString & name, const std::vector<float> & x, const std::vector<float> & y, const QString & unit = QString(), const QMenu * menu = 0, QGridLayout * grid = 0, QWidget * parent = 0);
virtual ~StatItem();
void setValue(float x, float y);
void addValue(float y);
void addValue(float x, float y);
void setValues(const std::vector<float> & x, const std::vector<float> & y);
QString value() const;
@@ -56,7 +57,8 @@ public slots:
void updateMenu(const QMenu * menu);
signals:
void valueChanged(float, float);
void valueAdded(float);
void valueAdded(float, float);
void valuesChanged(const std::vector<float> &, const std::vector<float> &);
void plotRequested(const StatItem *, const QString &);
@@ -91,6 +93,7 @@ public:
void closeFigures();
public slots:
void updateStat(const QString & statFullName, float y);
void updateStat(const QString & statFullName, float x, float y);
void updateStat(const QString & statFullName, const std::vector<float> & x, const std::vector<float> & y);

18
guilib/src/DatabaseViewer.cpp
View File

@@ -1092,14 +1092,25 @@ void DatabaseViewer::updateIds()
bool linksInserted = false;
for(std::map<int, Link>::iterator jter=links.find(ids_[i]); jter!=links.end() && jter->first == ids_[i]; ++jter)
{
std::map<int, Link>::iterator invertedLinkIter = graph::findLink(links, jter->second.to(), jter->second.from(), false);
if( jter->second.isValid() && // null transform means a rehearsed location
ids.find(jter->second.from()) != ids.end() &&
ids.find(jter->second.to()) != ids.end() &&
graph::findLink(links_, jter->second.from(), jter->second.to()) == links_.end() &&
graph::findLink(links, jter->second.from(), jter->second.to(), false) != links.end() &&
graph::findLink(links, jter->second.to(), jter->second.from(), false) != links.end())
invertedLinkIter != links.end())
{
links_.insert(std::make_pair(ids_[i], jter->second));
// check if user_data is set in opposite direction
if(jter->second.userDataCompressed().cols == 0 &&
invertedLinkIter->second.userDataCompressed().cols != 0)
{
links_.insert(std::make_pair(invertedLinkIter->second.from(), invertedLinkIter->second));
}
else
{
links_.insert(std::make_pair(ids_[i], jter->second));
}
linksInserted = true;
}
}
@@ -2611,7 +2622,8 @@ void DatabaseViewer::updateConstraintView(
ui_->constraintsViewer->removeCloud("scan2");
ui_->constraintsViewer->removeGraph("scan2graph");
if(link.type() == Link::kLocalSpaceClosure && !link.userDataCompressed().empty())
if(link.type() == Link::kLocalSpaceClosure &&
!link.userDataCompressed().empty())
{
std::vector<int> ids;
cv::Mat userData = link.uncompressUserDataConst();

46
guilib/src/MainWindow.cpp
View File

@@ -470,6 +470,12 @@ MainWindow::MainWindow(PreferencesDialog * prefDialog, QWidget * parent) :
_ui->statsToolBox->updateStat(QString((*iter).first.c_str()).replace('_', ' '), 0, (*iter).second);
}
}
// Specific MainWindow
_ui->statsToolBox->updateStat("Planning/From/", 0.0f);
_ui->statsToolBox->updateStat("Planning/Time/ms", 0.0f);
_ui->statsToolBox->updateStat("Planning/Goal/", 0.0f);
_ui->statsToolBox->updateStat("Planning/Poses/", 0.0f);
_ui->statsToolBox->updateStat("Planning/Length/m", 0.0f);
this->loadFigures();
connect(_ui->statsToolBox, SIGNAL(figuresSetupChanged()), this, SLOT(configGUIModified()));
@@ -1302,22 +1308,26 @@ void MainWindow::processStats(const rtabmap::Statistics & stat)
}
}
// update posterior on the graph view
if(_preferencesDialog->isPosteriorGraphView() && _ui->graphicsView_graphView->isVisible() && stat.posterior().size())
if( _ui->graphicsView_graphView->isVisible())
{
_ui->graphicsView_graphView->updatePosterior(stat.posterior());
}
// update local path on the graph view
_ui->graphicsView_graphView->updateLocalPath(stat.localPath());
if(stat.localPath().size() == 0)
{
// clear the global path if set (goal reached)
_ui->graphicsView_graphView->setGlobalPath(std::vector<std::pair<int, Transform> >());
}
// update current goal id
if(stat.currentGoalId() > 0)
{
_ui->graphicsView_graphView->setCurrentGoalID(stat.currentGoalId(), uValue(stat.poses(), stat.currentGoalId(), Transform()));
// update posterior on the graph view
if(_preferencesDialog->isPosteriorGraphView() &&
stat.posterior().size())
{
_ui->graphicsView_graphView->updatePosterior(stat.posterior());
}
// update local path on the graph view
_ui->graphicsView_graphView->updateLocalPath(stat.localPath());
if(stat.localPath().size() == 0)
{
// clear the global path if set (goal reached)
_ui->graphicsView_graphView->setGlobalPath(std::vector<std::pair<int, Transform> >());
}
// update current goal id
if(stat.currentGoalId() > 0)
{
_ui->graphicsView_graphView->setCurrentGoalID(stat.currentGoalId(), uValue(stat.poses(), stat.currentGoalId(), Transform()));
}
}
UDEBUG("");
@@ -2193,6 +2203,12 @@ void MainWindow::processRtabmapGlobalPathEvent(const rtabmap::RtabmapGlobalPathE
_ui->graphicsView_graphView->setGlobalPath(event.getPoses());
}
_ui->statsToolBox->updateStat("Planning/From/", float(event.getPoses().size()?event.getPoses().begin()->first:0));
_ui->statsToolBox->updateStat("Planning/Time/ms", float(event.getPlanningTime()*1000.0));
_ui->statsToolBox->updateStat("Planning/Goal/", float(event.getGoal()));
_ui->statsToolBox->updateStat("Planning/Poses/", float(event.getPoses().size()));
_ui->statsToolBox->updateStat("Planning/Length/m", float(graph::computePathLength(event.getPoses())));
if(_preferencesDialog->notifyWhenNewGlobalPathIsReceived())
{
// use MessageBox

29
guilib/src/StatsToolBox.cpp
View File

@@ -71,10 +71,16 @@ StatItem::~StatItem()
}
void StatItem::setValue(float x, float y)
void StatItem::addValue(float y)
{
_value->setText(QString::number(y, 'g', 3));
emit valueChanged(x,y);
emit valueAdded(y);
}
void StatItem::addValue(float x, float y)
{
_value->setText(QString::number(y, 'g', 3));
emit valueAdded(x,y);
}
void StatItem::setValues(const std::vector<float> & x, const std::vector<float> & y)
@@ -184,6 +190,13 @@ void StatsToolBox::closeFigures()
}
}
void StatsToolBox::updateStat(const QString & statFullName, float y)
{
std::vector<float> vx,vy(1);
vy[0] = y;
updateStat(statFullName, vx, vy);
}
void StatsToolBox::updateStat(const QString & statFullName, float x, float y)
{
std::vector<float> vx(1),vy(1);
@@ -203,7 +216,11 @@ void StatsToolBox::updateStat(const QString & statFullName, const std::vector<fl
{
if(y.size() == 1 && x.size() == 1)
{
item->setValue(x[0], y[0]);
item->addValue(x[0], y[0]);
}
else if(y.size() == 1 && x.size() == 0)
{
item->addValue(y[0]);
}
else
{
@@ -303,7 +320,8 @@ void StatsToolBox::plot(const StatItem * stat, const QString & plotName)
{
UPlotCurve * curve = new UPlotCurve(stat->objectName(), plot);
curve->setPen(plot->getRandomPenColored());
connect(stat, SIGNAL(valueChanged(float, float)), curve, SLOT(addValue(float, float)));
connect(stat, SIGNAL(valueAdded(float)), curve, SLOT(addValue(float)));
connect(stat, SIGNAL(valueAdded(float, float)), curve, SLOT(addValue(float, float)));
connect(stat, SIGNAL(valuesChanged(const std::vector<float> &, const std::vector<float> &)), curve, SLOT(setData(const std::vector<float> &, const std::vector<float> &)));
if(stat->value().compare("*") == 0)
{
@@ -351,7 +369,8 @@ void StatsToolBox::plot(const StatItem * stat, const QString & plotName)
//Add a new curve linked to the statBox
UPlotCurve * curve = new UPlotCurve(stat->objectName(), newPlot);
curve->setPen(newPlot->getRandomPenColored());
connect(stat, SIGNAL(valueChanged(float, float)), curve, SLOT(addValue(float, float)));
connect(stat, SIGNAL(valueAdded(float)), curve, SLOT(addValue(float)));
connect(stat, SIGNAL(valueAdded(float, float)), curve, SLOT(addValue(float, float)));
connect(stat, SIGNAL(valuesChanged(const std::vector<float> &, const std::vector<float> &)), curve, SLOT(setData(const std::vector<float> &, const std::vector<float> &)));
if(stat->value().compare("*") == 0)
{