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

Rtabmap: Added getPathNextPoses() and getPathNextNodes(). Local path is sent over statistics.

Browse Source 
GUI: local path is shown in GraphViewer. Now handling stereo odometry data.
This commit is contained in:
Mathieu Labbe
2015-02-03 11:09:10 -05:00
parent ee0051a585
commit ee885321aa
8 changed files with 168 additions and 23 deletions
Download Patch File Download Diff File Expand all files Collapse all files

96
guilib/src/GraphViewer.cpp
View File

@@ -157,6 +157,7 @@ GraphViewer::GraphViewer(QWidget * parent) :
_loopClosureLocalColor(Qt::yellow),
_loopClosureUserColor(Qt::red),
_loopClosureVirtualColor(Qt::magenta),
_pathColor(Qt::cyan),
_root(0),
_nodeRadius(0.01),
_linkWidth(0),
@@ -206,6 +207,7 @@ GraphViewer::~GraphViewer()
void GraphViewer::updateGraph(const std::map<int, Transform> & poses,
const std::multimap<int, Link> & constraints)
{
bool wasEmpty = _nodeItems.size() == 0 && _linkItems.size() == 0;
UDEBUG("poses=%d constraints=%d", (int)poses.size(), (int)constraints.size());
//Hide nodes and links
for(QMap<int, NodeItem*>::iterator iter = _nodeItems.begin(); iter!=_nodeItems.end(); ++iter)
@@ -244,25 +246,50 @@ void GraphViewer::updateGraph(const std::map<int, Transform> & poses,
for(std::multimap<int, Link>::const_iterator iter=constraints.begin(); iter!=constraints.end(); ++iter)
{
std::map<int, Transform>::const_iterator jterA = poses.find(iter->first);
std::map<int, Transform>::const_iterator jterB = poses.find(iter->second.to());
// make the first id the smallest one
int idFrom = iter->first<iter->second.to()?iter->first:iter->second.to();
int idTo = iter->first<iter->second.to()?iter->second.to():iter->first;
std::map<int, Transform>::const_iterator jterA = poses.find(idFrom);
std::map<int, Transform>::const_iterator jterB = poses.find(idTo);
if(jterA != poses.end() && jterB != poses.end() &&
_nodeItems.contains(iter->first) && _nodeItems.contains(iter->second.to()))
_nodeItems.contains(iter->first) && _nodeItems.contains(idTo))
{
const Transform & poseA = jterA->second;
const Transform & poseB = jterB->second;
bool added = false;
if(_linkItems.contains(iter->first))
if(_linkItems.contains(idFrom))
{
QMultiMap<int, LinkItem*>::iterator itemIter = _linkItems.find(iter->first);
while(itemIter.key() == iter->first && itemIter != _linkItems.end())
while(itemIter.key() == idFrom && itemIter != _linkItems.end())
{
if(itemIter.value()->to() == iter->second.to())
if(itemIter.value()->to() == idTo)
{
itemIter.value()->setPoses(poseA, poseB);
itemIter.value()->show();
added = true;
// reset color
if(iter->second.type() == Link::kNeighbor)
{
itemIter.value()->setColor(_neighborColor);
}
else if(iter->second.type() == Link::kVirtualClosure)
{
itemIter.value()->setColor(_loopClosureVirtualColor);
}
else if(iter->second.type() == Link::kUserClosure)
{
itemIter.value()->setColor(_loopClosureUserColor);
}
else if(iter->second.type() == Link::kLocalSpaceClosure || iter->second.type() == Link::kLocalTimeClosure)
{
itemIter.value()->setColor(_loopClosureLocalColor);
}
else
{
itemIter.value()->setColor(_loopClosureColor);
}
break;
}
++itemIter;
@@ -271,7 +298,7 @@ void GraphViewer::updateGraph(const std::map<int, Transform> & poses,
if(!added)
{
//create a link item
LinkItem * item = new LinkItem(iter->first, iter->second.to(), poseA, poseB, iter->second.type());
LinkItem * item = new LinkItem(idFrom, idTo, poseA, poseB, iter->second.type());
QPen p = item->pen();
p.setWidthF(_linkWidth);
item->setPen(p);
@@ -298,7 +325,7 @@ void GraphViewer::updateGraph(const std::map<int, Transform> & poses,
this->scene()->addItem(item);
item->setZValue(1);
item->setParentItem(_root);
_linkItems.insert(iter->first, item);
_linkItems.insert(idFrom, item);
}
}
}
@@ -341,7 +368,15 @@ void GraphViewer::updateGraph(const std::map<int, Transform> & poses,
}
this->scene()->setSceneRect(this->scene()->itemsBoundingRect()); // Re-shrink the scene to it's bounding contents
this->fitInView(this->scene()->itemsBoundingRect(), Qt::KeepAspectRatio);
if(poses.size() == 0 || wasEmpty)
{
this->fitInView(this->scene()->itemsBoundingRect(), Qt::KeepAspectRatio);
}
else
{
this->centerOn(_lastReferential);
}
}
void GraphViewer::updateMap(const cv::Mat & map8U, float resolution, float xMin, float yMin)
@@ -392,6 +427,31 @@ void GraphViewer::updatePosterior(const std::map<int, float> & posterior)
}
}
void GraphViewer::updateLocalPath(const std::vector<int> & localPath)
{
if(localPath.size() > 1)
{
for(unsigned int i=0; i<localPath.size()-1; ++i)
{
if(_linkItems.contains(localPath[i]))
{
int idFrom = localPath[i]<localPath[i+1]?localPath[i]:localPath[i+1];
int idTo = localPath[i]<localPath[i+1]?localPath[i+1]:localPath[i];
QMultiMap<int, LinkItem*>::iterator itemIter = _linkItems.find(idFrom);
while(itemIter.key() == idFrom && itemIter != _linkItems.end())
{
if(itemIter.value()->to() == idTo)
{
itemIter.value()->setColor(_pathColor);
break;
}
++itemIter;
}
}
}
}
}
void GraphViewer::clearGraph()
{
qDeleteAll(_nodeItems);
@@ -521,6 +581,10 @@ void GraphViewer::setVirtualLoopClosureColor(const QColor & color)
}
}
}
void GraphViewer::setLocalPathColor(const QColor & color)
{
_pathColor = color;
}
void GraphViewer::setGridMapVisible(bool visible)
{
_gridMap->setVisible(visible);
@@ -576,16 +640,19 @@ void GraphViewer::contextMenuEvent(QContextMenuEvent * event)
QAction * aChangeLocalLoopColor = menuLink->addAction(tr("Local loop closure"));
QAction * aChangeUserLoopColor = menuLink->addAction(tr("User loop closure"));
QAction * aChangeVirtualLoopColor = menuLink->addAction(tr("Virtual loop closure"));
QAction * aChangeLocalPathColor = menuLink->addAction(tr("Local path"));
aChangeNeighborColor->setIcon(createIcon(_neighborColor));
aChangeGlobalLoopColor->setIcon(createIcon(_loopClosureColor));
aChangeLocalLoopColor->setIcon(createIcon(_loopClosureLocalColor));
aChangeUserLoopColor->setIcon(createIcon(_loopClosureUserColor));
aChangeVirtualLoopColor->setIcon(createIcon(_loopClosureVirtualColor));
aChangeLocalPathColor->setIcon(createIcon(_pathColor));
aChangeNeighborColor->setIconVisibleInMenu(true);
aChangeGlobalLoopColor->setIconVisibleInMenu(true);
aChangeLocalLoopColor->setIconVisibleInMenu(true);
aChangeUserLoopColor->setIconVisibleInMenu(true);
aChangeVirtualLoopColor->setIconVisibleInMenu(true);
aChangeLocalPathColor->setIconVisibleInMenu(true);
menu.addSeparator();
QAction * aSetNodeSize = menu.addAction(tr("Set node radius..."));
@@ -672,7 +739,8 @@ void GraphViewer::contextMenuEvent(QContextMenuEvent * event)
r == aChangeGlobalLoopColor ||
r == aChangeLocalLoopColor ||
r == aChangeUserLoopColor ||
r == aChangeVirtualLoopColor)
r == aChangeVirtualLoopColor ||
r == aChangeLocalPathColor)
{
QColor color;
if(r == aChangeNodeColor)
@@ -695,6 +763,10 @@ void GraphViewer::contextMenuEvent(QContextMenuEvent * event)
{
color = _loopClosureVirtualColor;
}
else if(r == aChangeLocalPathColor)
{
color = _pathColor;
}
else //if(r == aChangeNeighborColor)
{
color = _neighborColor;
@@ -723,6 +795,10 @@ void GraphViewer::contextMenuEvent(QContextMenuEvent * event)
{
this->setVirtualLoopClosureColor(color);
}
else if(r == aChangeLocalPathColor)
{
this->setLocalPathColor(color);
}
else //if(r == aChangeNeighborColor)
{
this->setNeighborColor(color);

4
guilib/src/GraphViewer.h
View File

@@ -55,6 +55,7 @@ public:
const std::multimap<int, Link> & constraints);
void updateMap(const cv::Mat & map8U, float resolution, float xMin, float yMin);
void updatePosterior(const std::map<int, float> & posterior);
void updateLocalPath(const std::vector<int> & localPath);
void clearGraph();
void clearMap();
void clearPosterior();
@@ -70,6 +71,7 @@ public:
const QColor & getLocalLoopClosureColor() const {return _loopClosureLocalColor;}
const QColor & getUserLoopClosureColor() const {return _loopClosureUserColor;}
const QColor & getVirtualLoopClosureColor() const {return _loopClosureVirtualColor;}
const QColor & getLocalPathColor() const {return _pathColor;}
bool isGridMapVisible() const;
// setters
@@ -82,6 +84,7 @@ public:
void setLocalLoopClosureColor(const QColor & color);
void setUserLoopClosureColor(const QColor & color);
void setVirtualLoopClosureColor(const QColor & color);
void setLocalPathColor(const QColor & color);
void setGridMapVisible(bool visible);
signals:
@@ -102,6 +105,7 @@ private:
QColor _loopClosureLocalColor;
QColor _loopClosureUserColor;
QColor _loopClosureVirtualColor;
QColor _pathColor;
QGraphicsItem * _root;
QMap<int, NodeItem*> _nodeItems;
QMultiMap<int, LinkItem*> _linkItems;

20
guilib/src/MainWindow.cpp
View File

@@ -713,19 +713,19 @@ void MainWindow::processOdometry(const rtabmap::SensorData & data, const rtabmap
_odometryReceived = true;
// 3d cloud
if(data.depth().cols == data.image().cols &&
data.depth().rows == data.image().rows &&
!data.depth().empty() &&
if(data.depthOrRightImage().cols == data.image().cols &&
data.depthOrRightImage().rows == data.image().rows &&
!data.depthOrRightImage().empty() &&
data.fx() > 0.0f &&
data.fy() > 0.0f &&
data.fyOrBaseline() > 0.0f &&
_preferencesDialog->isCloudsShown(1))
{
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud;
cloud = createCloud(0,
data.image(),
data.depth(),
data.depthOrRightImage(),
data.fx(),
data.fy(),
data.fyOrBaseline(),
data.cx(),
data.cy(),
data.localTransform(),
@@ -1133,6 +1133,8 @@ void MainWindow::processStats(const rtabmap::Statistics & stat)
{
_ui->graphicsView_graphView->updatePosterior(stat.posterior());
}
// update local path on the graph view
_ui->graphicsView_graphView->updateLocalPath(stat.localPath());
UDEBUG("");
}
@@ -1246,7 +1248,8 @@ void MainWindow::updateMapCloud(
std::string cloudName = uFormat("cloud%d", iter->first);
// 3d point cloud
if(_preferencesDialog->isCloudsShown(0))
if((_ui->widget_cloudViewer->isVisible() && _preferencesDialog->isCloudsShown(0)) ||
(_ui->graphicsView_graphView->isVisible() && _ui->graphicsView_graphView->isGridMapVisible() && _preferencesDialog->isGridMapFrom3DCloud()))
{
if(viewerClouds.contains(cloudName))
{
@@ -1281,7 +1284,8 @@ void MainWindow::updateMapCloud(
// 2d point cloud
std::string scanName = uFormat("scan%d", iter->first);
if(_preferencesDialog->isScansShown(0) || _ui->graphicsView_graphView->isVisible() || _preferencesDialog->getGridMapShown())
if((_ui->widget_cloudViewer->isVisible() && (_preferencesDialog->isScansShown(0) || _preferencesDialog->getGridMapShown())) ||
(_ui->graphicsView_graphView->isVisible() && _ui->graphicsView_graphView->isGridMapVisible()))
{
if(viewerClouds.contains(scanName))
{

4
guilib/src/PreferencesDialog.cpp
View File

@@ -856,7 +856,7 @@ void PreferencesDialog::resetSettings(QGroupBox * groupBox)
_ui->doubleSpinBox_map_resolution->setValue(0.05);
_ui->checkBox_map_fillEmptySpace->setChecked(true);
_ui->checkBox_map_occupancyFrom3DCloud->setChecked(false);
_ui->spinbox_map_fillEmptyRadius->setValue(0);
_ui->spinbox_map_fillEmptyRadius->setValue(1);
_ui->doubleSpinBox_map_opacity->setValue(0.75);
}
else if(groupBox->objectName() == _ui->groupBox_logging1->objectName())
@@ -1795,6 +1795,7 @@ void PreferencesDialog::saveWidgetState(const QWidget * widget)
settings.setValue("local_color", graphViewer->getLocalLoopClosureColor());
settings.setValue("user_color", graphViewer->getUserLoopClosureColor());
settings.setValue("virtual_color", graphViewer->getVirtualLoopClosureColor());
settings.setValue("local_path_color", graphViewer->getLocalPathColor());
settings.setValue("grid_visible", graphViewer->isGridMapVisible());
}
else
@@ -1892,6 +1893,7 @@ void PreferencesDialog::loadWidgetState(QWidget * widget)
graphViewer->setLocalLoopClosureColor(settings.value("local_color", graphViewer->getLocalLoopClosureColor()).value<QColor>());
graphViewer->setUserLoopClosureColor(settings.value("user_color", graphViewer->getUserLoopClosureColor()).value<QColor>());
graphViewer->setVirtualLoopClosureColor(settings.value("virtual_color", graphViewer->getVirtualLoopClosureColor()).value<QColor>());
graphViewer->setLocalPathColor(settings.value("local_path_color", graphViewer->getLocalPathColor()).value<QColor>());
graphViewer->setGridMapVisible(settings.value("grid_visible", graphViewer->isGridMapVisible()).toBool());
}
else

8
guilib/src/ui/preferencesDialog.ui
View File

@@ -63,9 +63,9 @@
<property name="geometry">
<rect>
<x>0</x>
<y>-693</y>
<y>0</y>
<width>744</width>
<height>1378</height>
<height>1074</height>
</rect>
</property>
<layout class="QVBoxLayout" name="verticalLayout_16">
@@ -86,7 +86,7 @@
<enum>QFrame::Raised</enum>
</property>
<property name="currentIndex">
<number>19</number>
<number>1</number>
</property>
<widget class="QWidget" name="page_22">
<layout class="QVBoxLayout" name="verticalLayout_29">
@@ -543,7 +543,7 @@ Show a yellow background when the number of odometry inliers goes under this thr
<number>10</number>
</property>
<property name="value">
<number>0</number>
<number>1</number>
</property>
</widget>
</item>