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

GUI: GraphView's referential updated with odometry events

Browse Source
This commit is contained in:
Mathieu Labbe
2015-02-17 17:53:44 -05:00
parent 2f6426f029
commit f708e7c040
3 changed files with 36 additions and 21 deletions
Download Patch File Download Diff File Expand all files Collapse all files

32
guilib/src/GraphViewer.cpp
View File

@@ -162,7 +162,7 @@ GraphViewer::GraphViewer(QWidget * parent) :
_nodeRadius(0.01), _nodeRadius(0.01),
_linkWidth(0), _linkWidth(0),
_gridMap(0), _gridMap(0),
_lastReferential(0), _referential(0),
_gridCellSize(0.0f) _gridCellSize(0.0f)
{ {
this->setScene(new QGraphicsScene(this)); this->setScene(new QGraphicsScene(this));
@@ -181,16 +181,16 @@ GraphViewer::GraphViewer(QWidget * parent) :
item->setParentItem(_root); item->setParentItem(_root);
// current pose // current pose
_lastReferential = new QGraphicsItemGroup(); _referential = new QGraphicsItemGroup();
this->scene()->addItem(_lastReferential); this->scene()->addItem(_referential);
item = this->scene()->addLine(0,0,0,-0.5, QPen(QBrush(Qt::red), _linkWidth)); item = this->scene()->addLine(0,0,0,-0.5, QPen(QBrush(Qt::red), _linkWidth));
item->setZValue(100); item->setZValue(100);
item->setParentItem(_root); item->setParentItem(_root);
_lastReferential->addToGroup(item); _referential->addToGroup(item);
item = this->scene()->addLine(0,0,-0.5,0, QPen(QBrush(Qt::green), _linkWidth)); item = this->scene()->addLine(0,0,-0.5,0, QPen(QBrush(Qt::green), _linkWidth));
item->setZValue(100); item->setZValue(100);
item->setParentItem(_root); item->setParentItem(_root);
_lastReferential->addToGroup(item); _referential->addToGroup(item);
_gridMap = this->scene()->addPixmap(QPixmap()); _gridMap = this->scene()->addPixmap(QPixmap());
@@ -360,23 +360,21 @@ void GraphViewer::updateGraph(const std::map<int, Transform> & poses,
{ {
(--_nodeItems.end()).value()->setColor(Qt::green); (--_nodeItems.end()).value()->setColor(Qt::green);
} }
if(poses.size())
{
Transform t = poses.rbegin()->second;
QTransform qt(t.r11(), t.r12(), t.r21(), t.r22(), -t.o24(), -t.o14());
_lastReferential->setTransform(qt);
}
this->scene()->setSceneRect(this->scene()->itemsBoundingRect()); // Re-shrink the scene to it's bounding contents this->scene()->setSceneRect(this->scene()->itemsBoundingRect()); // Re-shrink the scene to it's bounding contents
if(poses.size() == 0 || wasEmpty) if(wasEmpty)
{ {
this->fitInView(this->scene()->itemsBoundingRect(), Qt::KeepAspectRatio); this->fitInView(this->scene()->itemsBoundingRect(), Qt::KeepAspectRatio);
} }
else }
{
this->centerOn(_lastReferential); void GraphViewer::updateReferentialPosition(const Transform & t)
} {
QTransform qt(t.r11(), t.r12(), t.r21(), t.r22(), -t.o24(), -t.o14());
_referential->setTransform(qt);
this->ensureVisible(_referential);
} }
void GraphViewer::updateMap(const cv::Mat & map8U, float resolution, float xMin, float yMin) void GraphViewer::updateMap(const cv::Mat & map8U, float resolution, float xMin, float yMin)
@@ -458,7 +456,7 @@ void GraphViewer::clearGraph()
_nodeItems.clear(); _nodeItems.clear();
qDeleteAll(_linkItems); qDeleteAll(_linkItems);
_linkItems.clear(); _linkItems.clear();
_lastReferential->resetTransform(); _referential->resetTransform();
this->scene()->setSceneRect(this->scene()->itemsBoundingRect()); // Re-shrink the scene to it's bounding contents this->scene()->setSceneRect(this->scene()->itemsBoundingRect()); // Re-shrink the scene to it's bounding contents
} }

3
guilib/src/GraphViewer.h
View File

@@ -53,6 +53,7 @@ public:
void updateGraph(const std::map<int, Transform> & poses, void updateGraph(const std::map<int, Transform> & poses,
const std::multimap<int, Link> & constraints); const std::multimap<int, Link> & constraints);
void updateReferentialPosition(const Transform & t);
void updateMap(const cv::Mat & map8U, float resolution, float xMin, float yMin); void updateMap(const cv::Mat & map8U, float resolution, float xMin, float yMin);
void updatePosterior(const std::map<int, float> & posterior); void updatePosterior(const std::map<int, float> & posterior);
void updateLocalPath(const std::vector<int> & localPath); void updateLocalPath(const std::vector<int> & localPath);
@@ -112,7 +113,7 @@ private:
float _nodeRadius; float _nodeRadius;
float _linkWidth; float _linkWidth;
QGraphicsPixmapItem * _gridMap; QGraphicsPixmapItem * _gridMap;
QGraphicsItemGroup * _lastReferential; QGraphicsItemGroup * _referential;
float _gridCellSize; float _gridCellSize;
}; };

22
guilib/src/MainWindow.cpp
View File

@@ -710,13 +710,16 @@ void MainWindow::processOdometry(const rtabmap::SensorData & data, const rtabmap
_ui->statsToolBox->updateStat("Odometry/T_pitch/deg", (float)data.id(), pitch*180.0/CV_PI); _ui->statsToolBox->updateStat("Odometry/T_pitch/deg", (float)data.id(), pitch*180.0/CV_PI);
_ui->statsToolBox->updateStat("Odometry/T_yaw/deg", (float)data.id(), yaw*180.0/CV_PI); _ui->statsToolBox->updateStat("Odometry/T_yaw/deg", (float)data.id(), yaw*180.0/CV_PI);
if(!pose.isNull() && (_ui->dockWidget_cloudViewer->isVisible() || _ui->graphicsView_graphView->isVisible()))
{
_lastOdomPose = pose;
_odometryReceived = true;
}
if(_ui->dockWidget_cloudViewer->isVisible()) if(_ui->dockWidget_cloudViewer->isVisible())
{ {
if(!pose.isNull()) if(!pose.isNull())
{ {
_lastOdomPose = pose;
_odometryReceived = true;
// 3d cloud // 3d cloud
if(data.depthOrRightImage().cols == data.image().cols && if(data.depthOrRightImage().cols == data.image().cols &&
data.depthOrRightImage().rows == data.image().rows && data.depthOrRightImage().rows == data.image().rows &&
@@ -773,6 +776,15 @@ void MainWindow::processOdometry(const rtabmap::SensorData & data, const rtabmap
_ui->widget_cloudViewer->update(); _ui->widget_cloudViewer->update();
} }
if(_ui->graphicsView_graphView->isVisible())
{
if(!pose.isNull() && !data.pose().isNull())
{
_ui->graphicsView_graphView->updateReferentialPosition(_odometryCorrection*data.pose());
_ui->graphicsView_graphView->update();
}
}
if(_ui->dockWidget_odometry->isVisible() && if(_ui->dockWidget_odometry->isVisible() &&
!data.image().empty()) !data.image().empty())
{ {
@@ -1390,6 +1402,10 @@ void MainWindow::updateMapCloud(
if(_ui->graphicsView_graphView->isVisible()) if(_ui->graphicsView_graphView->isVisible())
{ {
_ui->graphicsView_graphView->updateGraph(posesIn, constraints); _ui->graphicsView_graphView->updateGraph(posesIn, constraints);
if(!currentPose.isNull())
{
_ui->graphicsView_graphView->updateReferentialPosition(currentPose);
}
} }
cv::Mat map8U; cv::Mat map8U;
if((_ui->graphicsView_graphView->isVisible() || _preferencesDialog->getGridMapShown()) && (_createdScans.size() || _preferencesDialog->isGridMapFrom3DCloud())) if((_ui->graphicsView_graphView->isVisible() || _preferencesDialog->getGridMapShown()) && (_createdScans.size() || _preferencesDialog->isGridMapFrom3DCloud()))