From f5dac46252a04f569b62ed047e190fd4744b67f1 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Fri, 24 Jul 2015 16:29:45 -0400 Subject: [PATCH] CloudViewer: Added addOrUpdateCoordinate() method (working only with PCL >= 1.7.2) --- guilib/include/rtabmap/gui/CloudViewer.h | 8 ++ guilib/src/CloudViewer.cpp | 67 +++++++++++++++-- guilib/src/DatabaseViewer.cpp | 87 ++++++++++++++-------- guilib/src/ui/DatabaseViewer.ui | 94 ++++++++++++++++-------- 4 files changed, 186 insertions(+), 70 deletions(-) diff --git a/guilib/include/rtabmap/gui/CloudViewer.h b/guilib/include/rtabmap/gui/CloudViewer.h index 34379fc9..5be5e2c3 100644 --- a/guilib/include/rtabmap/gui/CloudViewer.h +++ b/guilib/include/rtabmap/gui/CloudViewer.h @@ -137,6 +137,13 @@ public: void updateCameraTargetPosition( const Transform & pose); + void addOrUpdateCoordinate( + const std::string & id, + const Transform & transform, + double scale); + void removeCoordinate(const std::string & id); + void removeAllCoordinates(); + void addOrUpdateGraph( const std::string & id, const pcl::PointCloud::Ptr & graph, @@ -226,6 +233,7 @@ private: QAction * _aSetBackgroundColor; QMenu * _menu; std::set _graphes; + std::set _coordinates; pcl::PointCloud::Ptr _trajectory; unsigned int _maxTrajectorySize; unsigned int _gridCellCount; diff --git a/guilib/src/CloudViewer.cpp b/guilib/src/CloudViewer.cpp index ad2e4634..f8f07c88 100644 --- a/guilib/src/CloudViewer.cpp +++ b/guilib/src/CloudViewer.cpp @@ -134,7 +134,7 @@ CloudViewer::CloudViewer(QWidget *parent) : 0, 0, 1); #ifndef _WIN32 // Crash on startup on Windows (vtk issue) - _visualizer->addCoordinateSystem(0.2, 0, 0, 0, 0); + this->addOrUpdateCoordinate("reference", Transform::getIdentity(), 0.2); #endif //setup menu/actions @@ -580,6 +580,61 @@ void CloudViewer::removeOccupancyGridMap() #endif } +void CloudViewer::addOrUpdateCoordinate( + const std::string & id, + const Transform & transform, + double scale) +{ + if(id.empty()) + { + UERROR("id should not be empty!"); + return; + } + + removeCoordinate(id); + + if(!transform.isNull()) + { + _coordinates.insert(id); +#if PCL_VERSION_COMPARE(>=, 1, 7, 2) + _visualizer->addCoordinateSystem(scale, transform.toEigen3f(), id); +#else + // Well, on older versions, just update the main coordinate + _visualizer->addCoordinateSystem(scale, transform.toEigen3f(), 0); +#endif + } +} + +void CloudViewer::removeCoordinate(const std::string & id) +{ + if(id.empty()) + { + UERROR("id should not be empty!"); + return; + } + + if(_coordinates.find(id) != _coordinates.end()) + { +#if PCL_VERSION_COMPARE(>=, 1, 7, 2) + _visualizer->removeCoordinateSystem(id); +#else + // Well, on older versions, just update the main coordinate + _visualizer->removeCoordinateSystem(0); +#endif + _coordinates.erase(id); + } +} + +void CloudViewer::removeAllCoordinates() +{ + std::set coordinates = _coordinates; + for(std::set::iterator iter = coordinates.begin(); iter!=coordinates.end(); ++iter) + { + this->removeCoordinate(*iter); + } + UASSERT(_coordinates.empty()); +} + void CloudViewer::addOrUpdateGraph( const std::string & id, const pcl::PointCloud::Ptr & graph, @@ -825,13 +880,8 @@ void CloudViewer::updateCameraTargetPosition(const Transform & pose) cameras.front().view[2] = _aLockViewZ->isChecked()?1:Fp[10]; } -#if PCL_VERSION_COMPARE(>=, 1, 7, 2) - _visualizer->removeCoordinateSystem("reference", 0); - _visualizer->addCoordinateSystem(0.2, m, "reference", 0); -#else - _visualizer->removeCoordinateSystem(0); - _visualizer->addCoordinateSystem(0.2, m, 0); -#endif + this->addOrUpdateCoordinate("reference", pose, 0.2); + _visualizer->setCameraPosition( cameras.front().pos[0], cameras.front().pos[1], cameras.front().pos[2], cameras.front().focal[0], cameras.front().focal[1], cameras.front().focal[2], @@ -1355,6 +1405,7 @@ void CloudViewer::handleAction(QAction * a) if(color.isValid()) { this->setDefaultBackgroundColor(color); + this->update(); } } else if(a == _aLockViewZ) diff --git a/guilib/src/DatabaseViewer.cpp b/guilib/src/DatabaseViewer.cpp index 1fa32b63..bb366ac8 100644 --- a/guilib/src/DatabaseViewer.cpp +++ b/guilib/src/DatabaseViewer.cpp @@ -1685,7 +1685,7 @@ void DatabaseViewer::update(int value, updateConstraintButtons(); updateWordsMatching(); - if(updateConstraintView) + if(updateConstraintView && ui_->dockWidget_constraints->isVisible()) { // update constraint view int from = ids_.at(ui_->horizontalSlider_A->value()); @@ -1703,7 +1703,7 @@ void DatabaseViewer::update(int value, ui_->horizontalSlider_loops->blockSignals(true); ui_->horizontalSlider_loops->setValue(i); ui_->horizontalSlider_loops->blockSignals(false); - this->updateConstraintView(loopLinks_.at(i), false); + this->updateConstraintView(loopLinks_[i].from() == from?loopLinks_.at(i):loopLinks_.at(i).inverse(), false); } ui_->horizontalSlider_neighbors->blockSignals(true); ui_->horizontalSlider_neighbors->setValue(0); @@ -1722,7 +1722,7 @@ void DatabaseViewer::update(int value, ui_->horizontalSlider_neighbors->blockSignals(true); ui_->horizontalSlider_neighbors->setValue(i); ui_->horizontalSlider_neighbors->blockSignals(false); - this->updateConstraintView(neighborLinks_.at(i), false); + this->updateConstraintView(neighborLinks_[i].from() == from?neighborLinks_.at(i):neighborLinks_.at(i).inverse(), false); } ui_->horizontalSlider_loops->blockSignals(true); ui_->horizontalSlider_loops->setValue(0); @@ -1738,10 +1738,30 @@ void DatabaseViewer::update(int value, ui_->horizontalSlider_neighbors->blockSignals(true); ui_->horizontalSlider_loops->setValue(0); ui_->horizontalSlider_neighbors->setValue(0); - ui_->constraintsViewer->removeAllClouds(); - ui_->constraintsViewer->update(); ui_->horizontalSlider_loops->blockSignals(false); ui_->horizontalSlider_neighbors->blockSignals(false); + + ui_->constraintsViewer->removeAllClouds(); + + // make a fake link using globally optimized poses + if(graphes_.size()) + { + std::map optimizedPoses = uValueAt(graphes_, ui_->horizontalSlider_iterations->value()); + if(optimizedPoses.size() > 0) + { + std::map::iterator fromIter = optimizedPoses.find(from); + std::map::iterator toIter = optimizedPoses.find(to); + if(fromIter != optimizedPoses.end() && + toIter != optimizedPoses.end()) + { + Link link(from, to, Link::kUndef, fromIter->second.inverse() * toIter->second); + this->updateConstraintView(link, false); + } + } + } + + ui_->constraintsViewer->update(); + } } @@ -2178,7 +2198,7 @@ void DatabaseViewer::updateConstraintView( if(cloudFrom->size() == 0 && cloudTo->size() == 0) { //cloud 3d - if(!ui_->checkBox_show3DWords->isChecked()) + if(ui_->checkBox_show3Dclouds->isChecked()) { pcl::PointCloud::Ptr cloudFrom, cloudTo; cloudFrom=util3d::cloudRGBFromSensorData(dataFrom, 1); @@ -2186,15 +2206,15 @@ void DatabaseViewer::updateConstraintView( if(cloudFrom->size()) { - ui_->constraintsViewer->addOrUpdateCloud("cloud0", cloudFrom, Transform::getIdentity(), Qt::red); + ui_->constraintsViewer->addOrUpdateCloud("words0", cloudFrom, Transform::getIdentity(), Qt::red); } if(cloudTo->size()) { cloudTo = rtabmap::util3d::transformPointCloud(cloudTo, t); - ui_->constraintsViewer->addOrUpdateCloud("cloud1", cloudTo, Transform::getIdentity(), Qt::cyan); + ui_->constraintsViewer->addOrUpdateCloud("words1", cloudTo, Transform::getIdentity(), Qt::cyan); } } - else + if(ui_->checkBox_show3DWords->isChecked()) { const Signature * sFrom = memory_->getSignature(link.from()); const Signature * sTo = memory_->getSignature(link.to()); @@ -2273,26 +2293,29 @@ void DatabaseViewer::updateConstraintView( if(scanFrom->size() == 0 && scanTo->size() == 0) { - //cloud 2d - pcl::PointCloud::Ptr scanA, scanB; - scanA = rtabmap::util3d::laserScanToPointCloud(dataFrom.laserScanRaw()); - scanB = rtabmap::util3d::laserScanToPointCloud(dataTo.laserScanRaw()); - scanB = rtabmap::util3d::transformPointCloud(scanB, t); - if(scanA->size()) + if(ui_->checkBox_show2DScans->isChecked()) { - ui_->constraintsViewer->addOrUpdateCloud("scan0", scanA, Transform::getIdentity(), Qt::yellow); - } - else - { - ui_->constraintsViewer->removeCloud("scan0"); - } - if(scanB->size()) - { - ui_->constraintsViewer->addOrUpdateCloud("scan1", scanB, Transform::getIdentity(), Qt::magenta); - } - else - { - ui_->constraintsViewer->removeCloud("scan1"); + //cloud 2d + pcl::PointCloud::Ptr scanA, scanB; + scanA = rtabmap::util3d::laserScanToPointCloud(dataFrom.laserScanRaw()); + scanB = rtabmap::util3d::laserScanToPointCloud(dataTo.laserScanRaw()); + scanB = rtabmap::util3d::transformPointCloud(scanB, t); + if(scanA->size()) + { + ui_->constraintsViewer->addOrUpdateCloud("scan0", scanA, Transform::getIdentity(), Qt::yellow); + } + else + { + ui_->constraintsViewer->removeCloud("scan0"); + } + if(scanB->size()) + { + ui_->constraintsViewer->addOrUpdateCloud("scan1", scanB, Transform::getIdentity(), Qt::magenta); + } + else + { + ui_->constraintsViewer->removeCloud("scan1"); + } } } else @@ -2315,8 +2338,12 @@ void DatabaseViewer::updateConstraintView( } } - //update cordinate - ui_->constraintsViewer->updateCameraTargetPosition(t); + //update coordinate + + ui_->constraintsViewer->addOrUpdateCoordinate("from_coordinate", Transform::getIdentity(), 0.2); + ui_->constraintsViewer->addOrUpdateCoordinate("to_coordinate", t, 0.2); + + ui_->constraintsViewer->clearTrajectory(); ui_->constraintsViewer->update(); diff --git a/guilib/src/ui/DatabaseViewer.ui b/guilib/src/ui/DatabaseViewer.ui index 329e2643..9df1291f 100644 --- a/guilib/src/ui/DatabaseViewer.ui +++ b/guilib/src/ui/DatabaseViewer.ui @@ -50,8 +50,8 @@ 0 0 - 166 - 173 + 154 + 184 @@ -236,8 +236,8 @@ 0 0 - 165 - 173 + 154 + 184 @@ -418,7 +418,7 @@ 0 0 1285 - 25 + 22 @@ -470,12 +470,56 @@ 2 - + - + + + + + Words + + + + + + + Clouds + + + true + + + + + + + Scans + + + true + + + + + + + Qt::Horizontal + + + + 40 + 20 + + + + + + + + @@ -572,20 +616,6 @@ - - - - Show 3D words - - - - - - - - - - @@ -805,8 +835,8 @@ 0 0 - 314 - 303 + 312 + 314 @@ -1025,8 +1055,8 @@ 0 0 - 351 - 347 + 366 + 361 @@ -1315,8 +1345,8 @@ 0 0 - 333 - 306 + 330 + 304 @@ -1532,8 +1562,8 @@ 0 0 - 243 - 284 + 248 + 319 @@ -1727,7 +1757,7 @@ 0 0 201 - 117 + 126 @@ -1826,8 +1856,8 @@ 0 0 - 285 - 309 + 283 + 322