From f34907f92f85126a948f5f964c0c62cac219fcb0 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Wed, 25 Jun 2014 23:00:35 +0000 Subject: [PATCH] Added optimizeFromGraphEnd (default true) parameter. It can be used to keep the map referential from the oldest node in the current graph git-svn-id: http://rtabmap.googlecode.com/svn/trunk/rtabmap@1441 f169173b-cf89-36c8-b27e-44dbe73f0c83 --- corelib/include/rtabmap/core/Parameters.h | 3 +- corelib/include/rtabmap/core/Rtabmap.h | 1 + corelib/src/Rtabmap.cpp | 31 ++++-- guilib/include/rtabmap/gui/CloudViewer.h | 3 + guilib/src/CloudViewer.cpp | 76 +++++++++----- guilib/src/MainWindow.cpp | 13 ++- guilib/src/PreferencesDialog.cpp | 9 +- guilib/src/ui/preferencesDialog.ui | 117 +++++++++++++--------- tools/OdometryViewer/main.cpp | 6 +- 9 files changed, 171 insertions(+), 88 deletions(-) diff --git a/corelib/include/rtabmap/core/Parameters.h b/corelib/include/rtabmap/core/Parameters.h index b6d447fe..952d41b6 100644 --- a/corelib/include/rtabmap/core/Parameters.h +++ b/corelib/include/rtabmap/core/Parameters.h @@ -225,7 +225,8 @@ class RTABMAP_EXP Parameters RTABMAP_PARAM(RGBD, LinearUpdate, float, 0.0, "Min linear displacement to update the map. Rehearsal is done prior to this, so weights are still updated."); RTABMAP_PARAM(RGBD, AngularUpdate, float, 0.0, "Min angular displacement to update the map. Rehearsal is done prior to this, so weights are still updated."); RTABMAP_PARAM(RGBD, NewMapOdomChangeDistance, float, 1, "A new map is created if a change of odometry translation greater than X m is detected (0 m = disabled)."); - RTABMAP_PARAM(RGBD, ToroIterations, int, 100, "TORO graph optimization iterations") + RTABMAP_PARAM(RGBD, ToroIterations, int, 100, "TORO graph optimization iterations"); + RTABMAP_PARAM(RGBD, OptimizeFromGraphEnd, bool, true, "Optimize graph from the newest node. If false, the graph is optimized from the oldest node of the current graph (this adds an overhead computation to detect to oldest mode of the current graph, but it can be useful to preserve the map referential from the oldest node)."); // Local loop closure detection RTABMAP_PARAM(RGBD, LocalLoopDetectionTime, bool, true, "Detection over all locations in STM."); diff --git a/corelib/include/rtabmap/core/Rtabmap.h b/corelib/include/rtabmap/core/Rtabmap.h index 5cf3c936..8ee16880 100644 --- a/corelib/include/rtabmap/core/Rtabmap.h +++ b/corelib/include/rtabmap/core/Rtabmap.h @@ -152,6 +152,7 @@ private: int _localDetectMaxDiffID; int _toroIterations; std::string _databasePath; + bool _optimizeFromGraphEnd; int _lcHypothesisId; float _lcHypothesisValue; diff --git a/corelib/src/Rtabmap.cpp b/corelib/src/Rtabmap.cpp index 0e0b250e..3880393d 100644 --- a/corelib/src/Rtabmap.cpp +++ b/corelib/src/Rtabmap.cpp @@ -91,6 +91,7 @@ Rtabmap::Rtabmap() : _localDetectMaxDiffID(Parameters::defaultRGBDLocalLoopDetectionMaxDiffID()), _toroIterations(Parameters::defaultRGBDToroIterations()), _databasePath(""), + _optimizeFromGraphEnd(Parameters::defaultRGBDOptimizeFromGraphEnd()), _lcHypothesisId(0), _lcHypothesisValue(0), _retrievedId(0), @@ -339,6 +340,7 @@ void Rtabmap::parseParameters(const ParametersMap & parameters) Parameters::parse(parameters, Parameters::kRGBDLocalLoopDetectionNeighbors(), _localDetectMaxNeighbors); Parameters::parse(parameters, Parameters::kRGBDLocalLoopDetectionMaxDiffID(), _localDetectMaxDiffID); Parameters::parse(parameters, Parameters::kRGBDToroIterations(), _toroIterations); + Parameters::parse(parameters, Parameters::kRGBDOptimizeFromGraphEnd(), _optimizeFromGraphEnd); // RGB-D SLAM stuff if((iter=parameters.find(Parameters::kLccIcpType())) != parameters.end()) @@ -826,13 +828,6 @@ bool Rtabmap::process(const Image & image) } } - // Reset map correction if we are mapping! - if(_memory->isIncremental() && !_mapCorrection.isIdentity()) - { - UWARN("Reset map correction because we are now mapping!"); - _mapCorrection.setIdentity(); - } - Transform newPose = _mapCorrection * signature->getPose(); _optimizedPoses.insert(std::make_pair(signature->id(), newPose)); @@ -1358,6 +1353,13 @@ bool Rtabmap::process(const Image & image) UINFO("Update map correction: SLAM mode"); // SLAM mode! optimizeCurrentMap(signature->id(), false, _optimizedPoses, &_constraints); + + // Update map correction, it should be identify when optimizing from the last node + _mapCorrection = _optimizedPoses.at(signature->id()) * signature->getPose().inverse(); + if(_mapCorrection.getNormSquared() > 0.001f && _optimizeFromGraphEnd) + { + UERROR("Map correction should be identity when optimizing from the last node. T=%s", _mapCorrection.prettyPrint().c_str()); + } } else if(_lcHypothesisId > 0 || localSpaceClosureId > 0 || signaturesRetrieved.size()) { @@ -1957,6 +1959,21 @@ void Rtabmap::optimizeCurrentMap( std::map ids = _memory->getNeighborsId(id, 0, lookInDatabase?-1:0, true); UDEBUG("ids=%d", (int)ids.size()); + if(!_optimizeFromGraphEnd && ids.size() > 1) + { + UTimer timer; + UDEBUG("Optimize from the first location (%d) instead of the last (%d) " + "in the local graph. Recomputing neighbors depth..."); + + int first = ids.begin()->first; + ids = _memory->getNeighborsId(first, 0, lookInDatabase?-1:0, true); + + UDEBUG("Optimize from the first location (%d) instead of the last (%d) " + "in the local graph. Recomputing neighbors depth... time=%fs", + first, + id, + timer.ticks()); + } std::map poses; std::multimap edgeConstraints; diff --git a/guilib/include/rtabmap/gui/CloudViewer.h b/guilib/include/rtabmap/gui/CloudViewer.h index 4989ca3b..def3db3b 100644 --- a/guilib/include/rtabmap/gui/CloudViewer.h +++ b/guilib/include/rtabmap/gui/CloudViewer.h @@ -110,6 +110,7 @@ public: void setCameraTargetFollow(bool enabled = true); void setCameraFree(); void setCameraLockZ(bool enabled = true); + void setGridShown(bool shown); public slots: void render(); @@ -128,6 +129,8 @@ protected: private: void createMenu(); void mouseEventOccurred (const pcl::visualization::MouseEvent &event, void* viewer_void); + void addGrid(); + void removeGrid(); private: pcl::visualization::PCLVisualizer * _visualizer; diff --git a/guilib/src/CloudViewer.cpp b/guilib/src/CloudViewer.cpp index 2b5eddb2..315c0b14 100644 --- a/guilib/src/CloudViewer.cpp +++ b/guilib/src/CloudViewer.cpp @@ -557,6 +557,55 @@ void CloudViewer::setCameraLockZ(bool enabled) _aLockViewZ->setChecked(enabled); } +void CloudViewer::setGridShown(bool shown) +{ + _aShowGrid->setChecked(shown); + if(shown) + { + this->addGrid(); + } + else + { + this->removeGrid(); + } +} + +void CloudViewer::addGrid() +{ + if(_gridLines.empty()) + { + float cellSize = 1.0f; + int cellCount = 50; + double r=0.5; + double g=0.5; + double b=0.5; + int id = 0; + float min = -float(cellCount/2) * cellSize; + float max = float(cellCount/2) * cellSize; + std::string name; + for(float i=min; i<=max; i += cellSize) + { + //over x + name = uFormat("line%d", ++id); + _visualizer->addLine(pcl::PointXYZ(i, min, 0.0f), pcl::PointXYZ(i, max, 0.0f), r, g, b, name); + _gridLines.push_back(name); + //over y + name = uFormat("line%d", ++id); + _visualizer->addLine(pcl::PointXYZ(min, i, 0.0f), pcl::PointXYZ(max, i, 0.0f), r, g, b, name); + _gridLines.push_back(name); + } + } +} + +void CloudViewer::removeGrid() +{ + for(std::list::iterator iter = _gridLines.begin(); iter!=_gridLines.end(); ++iter) + { + _visualizer->removeShape(*iter); + } + _gridLines.clear(); +} + Eigen::Vector3f rotatePointAroundAxe( const Eigen::Vector3f & point, const Eigen::Vector3f & axis, @@ -742,34 +791,11 @@ void CloudViewer::handleAction(QAction * a) { if(_aShowGrid->isChecked()) { - float cellSize = 1.0f; - int cellCount = 50; - double r=0.5; - double g=0.5; - double b=0.5; - int id = 0; - float min = -float(cellCount/2) * cellSize; - float max = float(cellCount/2) * cellSize; - std::string name; - for(float i=min; i<=max; i += cellSize) - { - //over x - name = uFormat("line%d", ++id); - _visualizer->addLine(pcl::PointXYZ(i, min, 0.0f), pcl::PointXYZ(i, max, 0.0f), r, g, b, name); - _gridLines.push_back(name); - //over y - name = uFormat("line%d", ++id); - _visualizer->addLine(pcl::PointXYZ(min, i, 0.0f), pcl::PointXYZ(max, i, 0.0f), r, g, b, name); - _gridLines.push_back(name); - } + this->addGrid(); } else { - for(std::list::iterator iter = _gridLines.begin(); iter!=_gridLines.end(); ++iter) - { - _visualizer->removeShape(*iter); - } - _gridLines.clear(); + this->removeGrid(); } this->render(); diff --git a/guilib/src/MainWindow.cpp b/guilib/src/MainWindow.cpp index 0120d8aa..eb8b1413 100644 --- a/guilib/src/MainWindow.cpp +++ b/guilib/src/MainWindow.cpp @@ -513,7 +513,6 @@ void MainWindow::handleEvent(UEvent* anEvent) { OdometryEvent * odomEvent = (OdometryEvent*)anEvent; if(_ui->dockWidget_cloudViewer->isVisible() && - _preferencesDialog->isCloudsShown(1) && _lastOdometryProcessed && !_processingStatistics) { @@ -1279,6 +1278,11 @@ void MainWindow::updateNodeVisibility(int nodeId, bool visible) } else if(viewerClouds.contains(cloudName)) { + if(visible) + { + //make sure the transformation was done + _ui->widget_cloudViewer->updateCloudPose(cloudName, _currentPosesMap.find(nodeId)->second); + } _ui->widget_cloudViewer->setCloudVisibility(cloudName, visible); } } @@ -1292,6 +1296,11 @@ void MainWindow::updateNodeVisibility(int nodeId, bool visible) } else if(viewerClouds.contains(scanName)) { + if(visible) + { + //make sure the transformation was done + _ui->widget_cloudViewer->updateCloudPose(scanName, _currentPosesMap.find(nodeId)->second); + } _ui->widget_cloudViewer->setCloudVisibility(scanName, visible); } } @@ -1481,8 +1490,6 @@ void MainWindow::processRtabmapEvent3DMap(const rtabmap::RtabmapEvent3DMap & eve _initProgressDialog->appendText(tr("Inserted %1 local transforms.").arg(_localTransformsMap.size())); _initProgressDialog->incrementStep(); - _odometryCorrection.setIdentity(); - _initProgressDialog->appendText("Inserting data in the cache... done."); if(event.getPoses().size()) diff --git a/guilib/src/PreferencesDialog.cpp b/guilib/src/PreferencesDialog.cpp index 26b3a3a0..bfa3cff7 100644 --- a/guilib/src/PreferencesDialog.cpp +++ b/guilib/src/PreferencesDialog.cpp @@ -400,6 +400,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) : _ui->rgdb_newMapOdomChange->setObjectName(Parameters::kRGBDNewMapOdomChangeDistance().c_str()); _ui->odomScanHistory->setObjectName(Parameters::kRGBDScanMatchingSize().c_str()); _ui->globalDetection_toroIterations->setObjectName(Parameters::kRGBDToroIterations().c_str()); + _ui->globalDetection_optimizeFromGraphEnd->setObjectName(Parameters::kRGBDOptimizeFromGraphEnd().c_str()); _ui->groupBox_localDetection_time->setObjectName(Parameters::kRGBDLocalLoopDetectionTime().c_str()); _ui->groupBox_localDetection_space->setObjectName(Parameters::kRGBDLocalLoopDetectionSpace().c_str()); @@ -1934,7 +1935,7 @@ void PreferencesDialog::addParameter(const QObject * object, bool value) { // add all RGBD parameters! this->addParameters(_ui->groupBox_slam_update); - this->addParameters(_ui->groupBox_odom_correction); + this->addParameters(_ui->groupBox_graphOptimization); this->addParameters(_ui->groupBox_localDetection_time); this->addParameters(_ui->groupBox_localDetection_space); this->addParameters(_ui->groupBox_globalConstraints); @@ -2822,6 +2823,9 @@ void PreferencesDialog::testOdometry(int type) OdometryViewer * odomViewer = new OdometryViewer(10, 2, 0.0, this->getOdomQualityWarnThr(), window); + odomViewer->setCameraFree(); + odomViewer->setGridShown(true); + QVBoxLayout *layout = new QVBoxLayout(); layout->addWidget(odomViewer); window->setLayout(layout); @@ -2835,6 +2839,8 @@ void PreferencesDialog::testOdometry(int type) window->showNormal(); + _ui->pushButton_testOdometry->setEnabled(false); + QApplication::processEvents(); uSleep(500); QApplication::processEvents(); @@ -2859,6 +2865,7 @@ void PreferencesDialog::cleanOdometryTest() delete _odomThread; _odomThread = 0; } + _ui->pushButton_testOdometry->setEnabled(true); } } diff --git a/guilib/src/ui/preferencesDialog.ui b/guilib/src/ui/preferencesDialog.ui index 98a05697..0a7d91b4 100644 --- a/guilib/src/ui/preferencesDialog.ui +++ b/guilib/src/ui/preferencesDialog.ui @@ -6,8 +6,8 @@ 0 0 - 876 - 526 + 1007 + 628 @@ -63,9 +63,9 @@ 0 - 0 - 577 - 632 + -210 + 709 + 1089 @@ -86,7 +86,7 @@ QFrame::Raised - 0 + 17 @@ -4311,7 +4311,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - Rigid transformations between nodes are saved on the neighbor links of the RTAB-Map's graph. On loop closures, a new constraint is added to the graph and TORO optimizes the graph. RGB-D images must be sent to work (see Source->Openni). + Rigid transformations between nodes are saved on the neighbor links of the RTAB-Map's graph. On loop closures, a new constraint is added to the graph and TORO optimizes the graph. RGB-D images must be sent to work (see Source->RGB-D Camera). true @@ -4321,7 +4321,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - Update + Map update @@ -4402,6 +4402,19 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag + + + + Laser scan matching history size for odometry correction. Set to 0 to disable odometry correction. ICP 2D only is used here. + + + true + + + + + + @@ -4461,6 +4474,9 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag + + QFormLayout::AllNonFixedFieldsGrow + @@ -4490,36 +4506,6 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - - - - TORO graph optimization iterations - - - - - - - 1 - - - 10000 - - - 100 - - - - - - - Maximum ICP correction distance accepted. A large translation difference between the visual transformation and ICP transformation results in wrong transformations in most cases. - - - true - - - @@ -4539,33 +4525,64 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag + + + + Maximum ICP correction distance accepted. A large translation difference between the visual transformation and ICP transformation results in wrong transformations in most cases. + + + true + + + - + - Odometry correction (laser scans are required) + Graph optimization - - - QFormLayout::AllNonFixedFieldsGrow - + - + + + 1 + + + 10000 + + + 100 + + - + - Laser scan matching history size for odometry correction. Set to 0 to disable odometry correction. ICP 2D only is used here. + TORO graph optimization iterations + + + + + + + Optimize graph from the newest node. If false, the graph is optimized from the oldest node of the current graph (this adds an overhead computation to detect to oldest mode of the current graph, but it can be useful to preserve the map referential from the oldest node). true + + + + + + + @@ -5218,7 +5235,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - Local history size: If > 0 (example 5000), the odometry will maintain a local map of X maximum words. + Local history size: If > 0 (example 5000), the odometry will maintain a local map of X maximum words. This will decrease odometry drifting when the camera is not moving. true @@ -5333,7 +5350,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - Maximum distance for visual word correspondences. + Maximum distance for visual word correspondences. Lower the value, higher the precision but higher the chance of RED screens (odometry lost). true @@ -5443,7 +5460,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - Max feature depth. For ICP, it is the max cloud depth. + Max feature depth. true diff --git a/tools/OdometryViewer/main.cpp b/tools/OdometryViewer/main.cpp index 1613ef21..47c3a200 100644 --- a/tools/OdometryViewer/main.cpp +++ b/tools/OdometryViewer/main.cpp @@ -36,7 +36,7 @@ void showUsage() " -au # Angular update (default 0.0 radian)\n" " -reset # Reset countdown (default 0 = disabled)\n" " -gpu Use GPU\n" - " -lh # Local history (default 1)\n" + " -lh # Local history (default 0)\n" "\n" " -brief_bytes # BRIEF bytes (default 32)\n" " -fast_thr # FAST threshold (default 30)\n" @@ -52,6 +52,7 @@ void showUsage() " odometryViewer -odom 0 -lh 5000 SURF example\n" " odometryViewer -odom 1 -lh 10000 SIFT example\n" " odometryViewer -odom 4 -nn 2 -lh 1000 FAST/BRIEF example\n" + " odometryViewer -odom 3 -nn 2 -lh 1000 FAST/FREAK example\n" " odometryViewer -icp -in 0.05 -i 30 ICP example\n"); exit(1); } @@ -649,6 +650,9 @@ int main (int argc, char * argv[]) UEventsManager::addHandler(&odomThread); UEventsManager::addHandler(&odomViewer); + odomViewer.setCameraFree(); + odomViewer.setGridShown(true); + odomViewer.setWindowTitle("Odometry viewer"); odomViewer.setMinimumWidth(800); odomViewer.setMinimumHeight(500);