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),
_linkWidth(0),
_gridMap(0),
_lastReferential(0),
_referential(0),
_gridCellSize(0.0f)
{
this->setScene(new QGraphicsScene(this));
@@ -181,16 +181,16 @@ GraphViewer::GraphViewer(QWidget * parent) :
item->setParentItem(_root);
// current pose
_lastReferential = new QGraphicsItemGroup();
this->scene()->addItem(_lastReferential);
_referential = new QGraphicsItemGroup();
this->scene()->addItem(_referential);
item = this->scene()->addLine(0,0,0,-0.5, QPen(QBrush(Qt::red), _linkWidth));
item->setZValue(100);
item->setParentItem(_root);
_lastReferential->addToGroup(item);
_referential->addToGroup(item);
item = this->scene()->addLine(0,0,-0.5,0, QPen(QBrush(Qt::green), _linkWidth));
item->setZValue(100);
item->setParentItem(_root);
_lastReferential->addToGroup(item);
_referential->addToGroup(item);
_gridMap = this->scene()->addPixmap(QPixmap());
@@ -360,23 +360,21 @@ void GraphViewer::updateGraph(const std::map<int, Transform> & poses,
{
(--_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
if(poses.size() == 0 || wasEmpty)
if(wasEmpty)
{
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)
@@ -458,7 +456,7 @@ void GraphViewer::clearGraph()
_nodeItems.clear();
qDeleteAll(_linkItems);
_linkItems.clear();
_lastReferential->resetTransform();
_referential->resetTransform();
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,
const std::multimap<int, Link> & constraints);
void updateReferentialPosition(const Transform & t);
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);
@@ -112,7 +113,7 @@ private:
float _nodeRadius;
float _linkWidth;
QGraphicsPixmapItem * _gridMap;
QGraphicsItemGroup * _lastReferential;
QGraphicsItemGroup * _referential;
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_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(!pose.isNull())
{
_lastOdomPose = pose;
_odometryReceived = true;
// 3d cloud
if(data.depthOrRightImage().cols == data.image().cols &&
data.depthOrRightImage().rows == data.image().rows &&
@@ -773,6 +776,15 @@ void MainWindow::processOdometry(const rtabmap::SensorData & data, const rtabmap
_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() &&
!data.image().empty())
{
@@ -1390,6 +1402,10 @@ void MainWindow::updateMapCloud(
if(_ui->graphicsView_graphView->isVisible())
{
_ui->graphicsView_graphView->updateGraph(posesIn, constraints);
if(!currentPose.isNull())
{
_ui->graphicsView_graphView->updateReferentialPosition(currentPose);
}
}
cv::Mat map8U;
if((_ui->graphicsView_graphView->isVisible() || _preferencesDialog->getGridMapShown()) && (_createdScans.size() || _preferencesDialog->isGridMapFrom3DCloud()))