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

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<std::string>::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<std::string>::iterator iter = _gridLines.begin(); iter!=_gridLines.end(); ++iter)
-			{
-				_visualizer->removeShape(*iter);
-			}
-			_gridLines.clear();
+			this->removeGrid();
 		}
 
 		this->render();

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())

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);
 }
 
 }

guilib/src/ui/preferencesDialog.ui

@@ -6,8 +6,8 @@
    <rect>
     <x>0</x>
     <y>0</y>
-    <width>876</width>
-    <height>526</height>
+    <width>1007</width>
+    <height>628</height>
    </rect>
   </property>
   <property name="sizePolicy">
@@ -63,9 +63,9 @@
           <property name="geometry">
            <rect>
             <x>0</x>
-            <y>0</y>
-            <width>577</width>
-            <height>632</height>
+            <y>-210</y>
+            <width>709</width>
+            <height>1089</height>
            </rect>
           </property>
           <layout class="QVBoxLayout" name="verticalLayout_16">
@@ -86,7 +86,7 @@
                 <enum>QFrame::Raised</enum>
                </property>
                <property name="currentIndex">
-                <number>0</number>
+                <number>17</number>
                </property>
                <widget class="QWidget" name="page_22">
                 <layout class="QVBoxLayout" name="verticalLayout_29">
@@ -4311,7 +4311,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                     <item>
                      <widget class="QLabel" name="label_83">
                       <property name="text">
-                       <string>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-&gt;Openni).</string>
+                       <string>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-&gt;RGB-D Camera).</string>
                       </property>
                       <property name="wordWrap">
                        <bool>true</bool>
@@ -4321,7 +4321,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                     <item>
                      <widget class="QGroupBox" name="groupBox_slam_update">
                       <property name="title">
-                       <string>Update</string>
+                       <string>Map update</string>
                       </property>
                       <layout class="QFormLayout" name="formLayout_20">
                        <item row="0" column="0">
@@ -4402,6 +4402,19 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                          </property>
                         </widget>
                        </item>
+                       <item row="3" column="1">
+                        <widget class="QLabel" name="label_scanMatching">
+                         <property name="text">
+                          <string>Laser scan matching history size for odometry correction. Set to 0 to disable odometry correction. ICP 2D only is used here.</string>
+                         </property>
+                         <property name="wordWrap">
+                          <bool>true</bool>
+                         </property>
+                        </widget>
+                       </item>
+                       <item row="3" column="0">
+                        <widget class="QSpinBox" name="odomScanHistory"/>
+                       </item>
                       </layout>
                      </widget>
                     </item>
@@ -4461,6 +4474,9 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                        </item>
                        <item>
                         <layout class="QFormLayout" name="formLayout_5">
+                         <property name="fieldGrowthPolicy">
+                          <enum>QFormLayout::AllNonFixedFieldsGrow</enum>
+                         </property>
                          <item row="0" column="0">
                           <widget class="QComboBox" name="globalDetection_icpType">
                            <property name="sizeAdjustPolicy">
@@ -4490,36 +4506,6 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                            </property>
                           </widget>
                          </item>
-                         <item row="2" column="1">
-                          <widget class="QLabel" name="label_31">
-                           <property name="text">
-                            <string>TORO graph optimization iterations</string>
-                           </property>
-                          </widget>
-                         </item>
-                         <item row="2" column="0">
-                          <widget class="QSpinBox" name="globalDetection_toroIterations">
-                           <property name="minimum">
-                            <number>1</number>
-                           </property>
-                           <property name="maximum">
-                            <number>10000</number>
-                           </property>
-                           <property name="value">
-                            <number>100</number>
-                           </property>
-                          </widget>
-                         </item>
-                         <item row="1" column="1">
-                          <widget class="QLabel" name="label_51">
-                           <property name="text">
-                            <string>Maximum ICP correction distance accepted. A large translation difference between the visual transformation and ICP transformation results in wrong transformations in most cases.</string>
-                           </property>
-                           <property name="wordWrap">
-                            <bool>true</bool>
-                           </property>
-                          </widget>
-                         </item>
                          <item row="1" column="0">
                           <widget class="QDoubleSpinBox" name="globalDetection_icpMaxDistance">
                            <property name="suffix">
@@ -4539,33 +4525,64 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                            </property>
                           </widget>
                          </item>
+                         <item row="1" column="1">
+                          <widget class="QLabel" name="label_51">
+                           <property name="text">
+                            <string>Maximum ICP correction distance accepted. A large translation difference between the visual transformation and ICP transformation results in wrong transformations in most cases.</string>
+                           </property>
+                           <property name="wordWrap">
+                            <bool>true</bool>
+                           </property>
+                          </widget>
+                         </item>
                         </layout>
                        </item>
                       </layout>
                      </widget>
                     </item>
                     <item>
-                     <widget class="QGroupBox" name="groupBox_odom_correction">
+                     <widget class="QGroupBox" name="groupBox_graphOptimization">
                       <property name="title">
-                       <string>Odometry correction (laser scans are required)</string>
+                       <string>Graph optimization</string>
                       </property>
-                      <layout class="QFormLayout" name="formLayout_21">
-                       <property name="fieldGrowthPolicy">
-                        <enum>QFormLayout::AllNonFixedFieldsGrow</enum>
-                       </property>
+                      <layout class="QFormLayout" name="formLayout_19">
                        <item row="0" column="0">
-                        <widget class="QSpinBox" name="odomScanHistory"/>
+                        <widget class="QSpinBox" name="globalDetection_toroIterations">
+                         <property name="minimum">
+                          <number>1</number>
+                         </property>
+                         <property name="maximum">
+                          <number>10000</number>
+                         </property>
+                         <property name="value">
+                          <number>100</number>
+                         </property>
+                        </widget>
                        </item>
                        <item row="0" column="1">
-                        <widget class="QLabel" name="label_scanMatching">
+                        <widget class="QLabel" name="label_31">
                          <property name="text">
-                          <string>Laser scan matching history size for odometry correction. Set to 0 to disable odometry correction. ICP 2D only is used here.</string>
+                          <string>TORO graph optimization iterations</string>
+                         </property>
+                        </widget>
+                       </item>
+                       <item row="1" column="1">
+                        <widget class="QLabel" name="label_151">
+                         <property name="text">
+                          <string>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).</string>
                          </property>
                          <property name="wordWrap">
                           <bool>true</bool>
                          </property>
                         </widget>
                        </item>
+                       <item row="1" column="0">
+                        <widget class="QCheckBox" name="globalDetection_optimizeFromGraphEnd">
+                         <property name="text">
+                          <string/>
+                         </property>
+                        </widget>
+                       </item>
                       </layout>
                      </widget>
                     </item>
@@ -5218,7 +5235,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                       <item row="2" column="1">
                        <widget class="QLabel" name="label_111">
                         <property name="text">
-                         <string>Local history size: If &gt; 0 (example 5000), the odometry will maintain a local map of X maximum words.</string>
+                         <string>Local history size: If &gt; 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.</string>
                         </property>
                         <property name="wordWrap">
                          <bool>true</bool>
@@ -5333,7 +5350,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                       <item row="7" column="1">
                        <widget class="QLabel" name="label_149">
                         <property name="text">
-                         <string>Maximum distance for visual word correspondences. </string>
+                         <string>Maximum distance for visual word correspondences. Lower the value, higher the precision but higher the chance of RED screens (odometry lost).</string>
                         </property>
                         <property name="wordWrap">
                          <bool>true</bool>
@@ -5443,7 +5460,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                       <item row="11" column="1">
                        <widget class="QLabel" name="label_104">
                         <property name="text">
-                         <string>Max feature depth. For ICP, it is the max cloud depth.</string>
+                         <string>Max feature depth.</string>
                         </property>
                         <property name="wordWrap">
                          <bool>true</bool>