DatabaseViewer: ask saving changes to database on Exit

New parameter: Mem/InitWMWithAllNodes

git-svn-id: http://rtabmap.googlecode.com/svn/trunk/rtabmap@1680 f169173b-cf89-36c8-b27e-44dbe73f0c83

corelib/include/rtabmap/core/Parameters.h

@@ -157,7 +157,8 @@ class RTABMAP_EXP Parameters
 	RTABMAP_PARAM(Mem, RecentWmRatio,           float, 0.2, 	"Ratio of locations after the last loop closure in WM that cannot be transferred.");
 	RTABMAP_PARAM(Mem, RehearsalIdUpdatedToNewOne, bool, false, "On merge, update to new id. When false, no copy.");
 	RTABMAP_PARAM(Mem, GenerateIds,             bool, true,     "True=Generate location Ids, False=use input image ids.");
-	RTABMAP_PARAM(Mem, BadSignaturesIgnored,    bool, false,     "Bad signatures are ignored.")
+	RTABMAP_PARAM(Mem, BadSignaturesIgnored,    bool, false,     "Bad signatures are ignored.");
+	RTABMAP_PARAM(Mem, InitWMWithAllNodes,      bool, false,    "Initialize the Working Memory with all nodes in Long-Term Memory. When false, it is initialized with nodes of the previous session.")
 
 	// KeypointMemory (Keypoint-based)
 	RTABMAP_PARAM(Kp, PublishKeypoints,      bool, true, 		"Publishing keypoints.");

corelib/src/Memory.cpp

@@ -110,6 +110,8 @@ bool Memory::init(const std::string & dbUrl, bool dbOverwritten, const Parameter
 
 	UDEBUG("");
 	this->parseParameters(parameters);
+	bool loadAllNodesInWM = Parameters::defaultMemInitWMWithAllNodes();
+	Parameters::parse(parameters, Parameters::kMemInitWMWithAllNodes(), loadAllNodesInWM);
 
 	if(postInitEvents) UEventsManager::post(new RtabmapEventInit("Clearing memory..."));
 	DBDriver * tmpDriver = 0;
@@ -152,9 +154,21 @@ bool Memory::init(const std::string & dbUrl, bool dbOverwritten, const Parameter
 			if(postInitEvents) UEventsManager::post(new RtabmapEventInit(std::string("Connecting to database ") + dbUrl + ", done!"));
 
 			// Load the last working memory...
-			if(postInitEvents) UEventsManager::post(new RtabmapEventInit(std::string("Loading last signatures...")));
 			std::list<Signature*> dbSignatures;
-			_dbDriver->loadLastNodes(dbSignatures);
+
+			if(loadAllNodesInWM)
+			{
+				if(postInitEvents) UEventsManager::post(new RtabmapEventInit(std::string("Loading all nodes to WM...")));
+				std::set<int> ids;
+				_dbDriver->getAllNodeIds(ids);
+				_dbDriver->loadSignatures(std::list<int>(ids.begin(), ids.end()), dbSignatures);
+			}
+			else
+			{
+				// load previous session working memory
+				if(postInitEvents) UEventsManager::post(new RtabmapEventInit(std::string("Loading last nodes to WM...")));
+				_dbDriver->loadLastNodes(dbSignatures);
+			}
 			for(std::list<Signature*>::reverse_iterator iter=dbSignatures.rbegin(); iter!=dbSignatures.rend(); ++iter)
 			{
 				// ignore bad signatures
@@ -175,7 +189,7 @@ bool Memory::init(const std::string & dbUrl, bool dbOverwritten, const Parameter
 					delete *iter;
 				}
 			}
-			if(postInitEvents) UEventsManager::post(new RtabmapEventInit(std::string("Loading last signatures, done! (") + uNumber2Str(int(_workingMem.size() + _stMem.size())) + " loaded)"));
+			if(postInitEvents) UEventsManager::post(new RtabmapEventInit(std::string("Loading nodes to WM, done! (") + uNumber2Str(int(_workingMem.size() + _stMem.size())) + " loaded)"));
 
 			// Assign the last signature
 			if(_stMem.size()>0)
@@ -2130,6 +2144,7 @@ bool Memory::addLoopClosureLink(int oldId, int newId, const Transform & transfor
 	Signature * newS = _getSignature(newId);
 	if(oldS && newS)
 	{
+		_memoryChanged = true;
 		const std::map<int, Transform> & oldLoopclosureIds = oldS->getLoopClosureIds();
 		if(oldLoopclosureIds.size() && oldLoopclosureIds.find(newS->id()) != oldLoopclosureIds.end())
 		{

guilib/include/rtabmap/gui/DatabaseViewer.h

@@ -64,6 +64,7 @@ public:
 
 protected:
 	virtual void resizeEvent(QResizeEvent* anEvent);
+	virtual void closeEvent(QCloseEvent* event);
 
 private slots:
 	void openDatabase();

guilib/src/DatabaseViewer.cpp

@@ -32,6 +32,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <QtGui/QFileDialog>
 #include <QtGui/QInputDialog>
 #include <QtGui/QGraphicsLineItem>
+#include <QtGui/QCloseEvent>
 #include <QtCore/QBuffer>
 #include <QtCore/QTextStream>
 #include <rtabmap/utilite/ULogger.h>
@@ -186,6 +187,7 @@ bool DatabaseViewer::openDatabase(const QString & path)
 		rtabmap::ParametersMap parameters;
 		parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kDbSqlite3InMemory(), "false"));
 		parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kMemIncrementalMemory(), "false"));
+		parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kMemInitWMWithAllNodes(), "true"));
 		// use BruteForce dictionary because we don't know which type of descriptors are saved in database
 		parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kKpNNStrategy(), "3"));
 
@@ -209,6 +211,65 @@ bool DatabaseViewer::openDatabase(const QString & path)
 	return false;
 }
 
+void DatabaseViewer::closeEvent(QCloseEvent* event)
+{
+	if(linksAdded_.size() || linksRefined_.size() || linksRemoved_.size())
+	{
+		QMessageBox::StandardButton button = QMessageBox::question(this,
+				tr("Links modified"),
+				tr("Some links are modified (%1 added, %2 refined, %3 removed), do you want to save them?")
+				.arg(linksAdded_.size()).arg(linksRefined_.size()).arg(linksRemoved_.size()),
+				QMessageBox::Cancel | QMessageBox::Yes | QMessageBox::No,
+				QMessageBox::Cancel);
+
+		if(button == QMessageBox::Yes)
+		{
+			// Added links
+			for(std::multimap<int, rtabmap::Link>::iterator iter=linksAdded_.begin(); iter!=linksAdded_.end(); ++iter)
+			{
+				std::multimap<int, rtabmap::Link>::iterator refinedIter = this->findLink(linksRefined_, iter->second.from(), iter->second.to());
+				if(refinedIter != linksRefined_.end())
+				{
+					memory_->addLoopClosureLink(refinedIter->second.to(), refinedIter->second.from(), refinedIter->second.transform(), true);
+				}
+				else
+				{
+					memory_->addLoopClosureLink(iter->second.to(), iter->second.from(), iter->second.transform(), true);
+				}
+			}
+
+			//Refined links
+			for(std::multimap<int, rtabmap::Link>::iterator iter=linksRefined_.begin(); iter!=linksRefined_.end(); ++iter)
+			{
+				if(!containsLink(linksAdded_, iter->second.from(), iter->second.to()))
+				{
+					memory_->rejectLoopClosure(iter->second.to(), iter->second.from());
+					memory_->addLoopClosureLink(iter->second.to(), iter->second.from(), iter->second.transform(), true);
+				}
+			}
+
+			// Rejected links
+			for(std::multimap<int, rtabmap::Link>::iterator iter=linksRemoved_.begin(); iter!=linksRemoved_.end(); ++iter)
+			{
+				memory_->rejectLoopClosure(iter->second.to(), iter->second.from());
+			}
+		}
+
+		if(button == QMessageBox::Yes || button == QMessageBox::No)
+		{
+			event->accept();
+		}
+		else
+		{
+			event->ignore();
+		}
+	}
+	else
+	{
+		event->accept();
+	}
+}
+
 void DatabaseViewer::resizeEvent(QResizeEvent* anEvent)
 {
 	ui_->graphicsView_A->fitInView(ui_->graphicsView_A->sceneRect(), Qt::KeepAspectRatio);
@@ -1523,68 +1584,105 @@ void DatabaseViewer::refineConstraint(int from, int to)
 		return;
 	}
 
+
+	bool hasConverged = false;
+	double fitness = 0.0f;
+	Transform transform;
+
 	float fxA, fyA, cxA, cyA;
 	float fxB, fyB, cxB, cyB;
 	rtabmap::Transform localTransformA, localTransformB;
 
 	std::vector<unsigned char> imageBytesA, depthBytesA, depth2dBytesA;
 	memory_->getImageDepth(currentLink.from(), imageBytesA, depthBytesA, depth2dBytesA, fxA, fyA, cxA, cyA, localTransformA);
-	cv::Mat depthA = rtabmap::util3d::uncompressImage(depthBytesA);
 
 	std::vector<unsigned char> imageBytesB, depthBytesB, depth2dBytesB;
 	memory_->getImageDepth(currentLink.to(), imageBytesB, depthBytesB, depth2dBytesB, fxB, fyB, cxB, cyB, localTransformB);
-	cv::Mat depthB = rtabmap::util3d::uncompressImage(depthBytesB);
 
-	pcl::PointCloud<pcl::PointXYZ>::Ptr cloudA = util3d::getICPReadyCloud(depthA,
+	if(ui_->checkBox_icp_2d->isChecked())
-					fxA, fyA, cxA, cyA,
-					ui_->spinBox_icp_decimation->value(),
-					ui_->doubleSpinBox_icp_maxDepth->value(),
-					ui_->doubleSpinBox_icp_voxel->value(),
-					0, // no sampling
-					localTransformA);
-	pcl::PointCloud<pcl::PointXYZ>::Ptr cloudB = util3d::getICPReadyCloud(depthB,
-					fxB, fyB, cxB, cyB,
-					ui_->spinBox_icp_decimation->value(),
-					ui_->doubleSpinBox_icp_maxDepth->value(),
-					ui_->doubleSpinBox_icp_voxel->value(),
-					0, // no sampling
-					currentLink.transform() * localTransformB);
-
-	bool hasConverged = false;
-	double fitness;
-	Transform transform;
-	if(ui_->checkBox_icp_p2plane->isChecked())
 	{
-		pcl::PointCloud<pcl::PointNormal>::Ptr cloudANormals = util3d::computeNormals(cloudA, ui_->spinBox_icp_normalKSearch->value());
+		//2D
-		pcl::PointCloud<pcl::PointNormal>::Ptr cloudBNormals = util3d::computeNormals(cloudB, ui_->spinBox_icp_normalKSearch->value());
+		cv::Mat oldDepth2D = util3d::uncompressData(depth2dBytesA);
+		cv::Mat newDepth2D = util3d::uncompressData(depth2dBytesB);
 
-		cloudANormals = util3d::removeNaNNormalsFromPointCloud(cloudANormals);
+		if(!oldDepth2D.empty() && !newDepth2D.empty())
-		if(cloudA->size() != cloudANormals->size())
 		{
-			UWARN("removed nan normals...");
+			// 2D
-		}
+			pcl::PointCloud<pcl::PointXYZ>::Ptr oldCloud = util3d::cvMat2Cloud(oldDepth2D);
+			pcl::PointCloud<pcl::PointXYZ>::Ptr newCloud = util3d::cvMat2Cloud(newDepth2D, currentLink.transform());
 
-		cloudBNormals = util3d::removeNaNNormalsFromPointCloud(cloudBNormals);
+			//voxelize
-		if(cloudB->size() != cloudBNormals->size())
+			if(ui_->doubleSpinBox_icp_voxel->value() > 0.0f)
-		{
+			{
-			UWARN("removed nan normals...");
+				oldCloud = util3d::voxelize(oldCloud, ui_->doubleSpinBox_icp_voxel->value());
-		}
+				newCloud = util3d::voxelize(newCloud, ui_->doubleSpinBox_icp_voxel->value());
+			}
 
-		transform = util3d::icpPointToPlane(cloudBNormals,
+			if(newCloud->size() && oldCloud->size())
-				cloudANormals,
+			{
-				ui_->doubleSpinBox_icp_maxCorrespDistance->value(),
+				transform = util3d::icp2D(newCloud,
-				ui_->spinBox_icp_decimation->value(),
+						oldCloud,
-				hasConverged,
+						ui_->doubleSpinBox_icp_maxCorrespDistance->value(),
-				fitness);
+						ui_->spinBox_icp_iteration->value(),
+					   hasConverged,
+					   fitness);
+			}
+		}
 	}
 	else
 	{
-		transform = util3d::icp(cloudB,
+		//3D
-				cloudA,
+		cv::Mat depthA = rtabmap::util3d::uncompressImage(depthBytesA);
-				ui_->doubleSpinBox_icp_maxCorrespDistance->value(),
+		cv::Mat depthB = rtabmap::util3d::uncompressImage(depthBytesB);
-				ui_->spinBox_icp_decimation->value(),
+
-				hasConverged,
+		pcl::PointCloud<pcl::PointXYZ>::Ptr cloudA = util3d::getICPReadyCloud(depthA,
-				fitness);
+						fxA, fyA, cxA, cyA,
+						ui_->spinBox_icp_decimation->value(),
+						ui_->doubleSpinBox_icp_maxDepth->value(),
+						ui_->doubleSpinBox_icp_voxel->value(),
+						0, // no sampling
+						localTransformA);
+		pcl::PointCloud<pcl::PointXYZ>::Ptr cloudB = util3d::getICPReadyCloud(depthB,
+						fxB, fyB, cxB, cyB,
+						ui_->spinBox_icp_decimation->value(),
+						ui_->doubleSpinBox_icp_maxDepth->value(),
+						ui_->doubleSpinBox_icp_voxel->value(),
+						0, // no sampling
+						currentLink.transform() * localTransformB);
+
+		if(ui_->checkBox_icp_p2plane->isChecked())
+		{
+			pcl::PointCloud<pcl::PointNormal>::Ptr cloudANormals = util3d::computeNormals(cloudA, ui_->spinBox_icp_normalKSearch->value());
+			pcl::PointCloud<pcl::PointNormal>::Ptr cloudBNormals = util3d::computeNormals(cloudB, ui_->spinBox_icp_normalKSearch->value());
+
+			cloudANormals = util3d::removeNaNNormalsFromPointCloud(cloudANormals);
+			if(cloudA->size() != cloudANormals->size())
+			{
+				UWARN("removed nan normals...");
+			}
+
+			cloudBNormals = util3d::removeNaNNormalsFromPointCloud(cloudBNormals);
+			if(cloudB->size() != cloudBNormals->size())
+			{
+				UWARN("removed nan normals...");
+			}
+
+			transform = util3d::icpPointToPlane(cloudBNormals,
+					cloudANormals,
+					ui_->doubleSpinBox_icp_maxCorrespDistance->value(),
+					ui_->spinBox_icp_iteration->value(),
+					hasConverged,
+					fitness);
+		}
+		else
+		{
+			transform = util3d::icp(cloudB,
+					cloudA,
+					ui_->doubleSpinBox_icp_maxCorrespDistance->value(),
+					ui_->spinBox_icp_iteration->value(),
+					hasConverged,
+					fitness);
+		}
 	}
 
 	if(hasConverged && !transform.isNull())
@@ -1707,6 +1805,14 @@ bool DatabaseViewer::addConstraint(int from, int to, bool silent)
 		}
 		else
 		{
+			if(ui_->checkBox_visual_2d->isChecked())
+			{
+				// We are 2D here, make sure the guess has only YAW rotation
+				float x,y,z,r,p,yaw;
+				t.getTranslationAndEulerAngles(x,y,z, r,p,yaw);
+				t = util3d::transformFromEigen3f(pcl::getTransformation(x,y,0, 0, 0, yaw));
+			}
+
 			// transform is valid, make a link
 			linksAdded_.insert(std::make_pair(from, Link(from, to, t, Link::kUserClosure)));
 			updateSlider = true;
@@ -1755,6 +1861,11 @@ void DatabaseViewer::resetConstraint()
 	{
 		this->updateConstraintView(iter->second);
 	}
+	iter = findLink(linksAdded_, from, to);
+	if(iter != linksAdded_.end())
+	{
+		this->updateConstraintView(iter->second);
+	}
 }
 
 void DatabaseViewer::rejectConstraint()

guilib/src/PreferencesDialog.cpp

@@ -102,6 +102,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 		// remove BruteForceGPU option
 		_ui->comboBox_dictionary_strategy->removeItem(4);
 		_ui->odom_bin_nn->removeItem(4);
+		_ui->reextract_nn->removeItem(4);
 	}
 
 #if PCL_VERSION_COMPARE(<, 1, 7, 2)
@@ -307,6 +308,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	_ui->general_checkBox_RehearsalIdUpdatedToNewOne->setObjectName(Parameters::kMemRehearsalIdUpdatedToNewOne().c_str());
 	_ui->general_checkBox_generateIds->setObjectName(Parameters::kMemGenerateIds().c_str());
 	_ui->general_checkBox_badSignaturesIgnored->setObjectName(Parameters::kMemBadSignaturesIgnored().c_str());
+	_ui->general_checkBox_initWMWithAllNodes->setObjectName(Parameters::kMemInitWMWithAllNodes().c_str());
 
 	// Database
 	_ui->checkBox_dbInMemory->setObjectName(Parameters::kDbSqlite3InMemory().c_str());

guilib/src/ui/DatabaseViewer.ui

@@ -545,6 +545,13 @@
     <layout class="QVBoxLayout" name="verticalLayout_11">
      <item>
       <layout class="QGridLayout" name="gridLayout_4" columnminimumwidth="1,0">
+       <item row="4" column="0">
+        <widget class="QLabel" name="label_15">
+         <property name="text">
+          <string>Iteration</string>
+         </property>
+        </widget>
+       </item>
        <item row="0" column="0">
         <widget class="QLabel" name="label_14">
          <property name="text">
@@ -637,13 +644,6 @@
          </property>
         </widget>
        </item>
-       <item row="4" column="0">
-        <widget class="QLabel" name="label_15">
-         <property name="text">
-          <string>Iteration</string>
-         </property>
-        </widget>
-       </item>
        <item row="4" column="1">
         <widget class="QSpinBox" name="spinBox_icp_iteration">
          <property name="minimum">
@@ -694,6 +694,23 @@
          </property>
         </widget>
        </item>
+       <item row="7" column="0">
+        <widget class="QLabel" name="label_27">
+         <property name="text">
+          <string>2D icp</string>
+         </property>
+        </widget>
+       </item>
+       <item row="7" column="1">
+        <widget class="QCheckBox" name="checkBox_icp_2d">
+         <property name="text">
+          <string/>
+         </property>
+         <property name="checked">
+          <bool>false</bool>
+         </property>
+        </widget>
+       </item>
       </layout>
      </item>
      <item>
@@ -737,14 +754,14 @@
          </property>
         </widget>
        </item>
-       <item row="1" column="0">
+       <item row="2" column="0">
         <widget class="QLabel" name="label_25">
          <property name="text">
           <string>SURF hessian threshold</string>
          </property>
         </widget>
        </item>
-       <item row="1" column="1">
+       <item row="2" column="1">
         <widget class="QDoubleSpinBox" name="doubleSpinBox_visual_hessian">
          <property name="suffix">
           <string/>
@@ -763,14 +780,14 @@
          </property>
         </widget>
        </item>
-       <item row="2" column="0">
+       <item row="3" column="0">
         <widget class="QLabel" name="label_26">
          <property name="text">
           <string>NNDR</string>
          </property>
         </widget>
        </item>
-       <item row="2" column="1">
+       <item row="3" column="1">
         <widget class="QDoubleSpinBox" name="doubleSpinBox_visual_nndr">
          <property name="suffix">
           <string> m</string>
@@ -792,14 +809,14 @@
          </property>
         </widget>
        </item>
-       <item row="3" column="0">
+       <item row="4" column="0">
         <widget class="QLabel" name="label_21">
          <property name="text">
           <string>Min correspondences</string>
          </property>
         </widget>
        </item>
-       <item row="3" column="1">
+       <item row="4" column="1">
         <widget class="QSpinBox" name="spinBox_visual_minCorrespondences">
          <property name="minimum">
           <number>3</number>
@@ -812,14 +829,14 @@
          </property>
         </widget>
        </item>
-       <item row="4" column="0">
+       <item row="5" column="0">
         <widget class="QLabel" name="label_22">
          <property name="text">
           <string>Max correspondence distance</string>
          </property>
         </widget>
        </item>
-       <item row="4" column="1">
+       <item row="5" column="1">
         <widget class="QDoubleSpinBox" name="doubleSpinBox_visual_maxCorrespDistance">
          <property name="suffix">
           <string> m</string>
@@ -838,14 +855,14 @@
          </property>
         </widget>
        </item>
-       <item row="5" column="0">
+       <item row="6" column="0">
         <widget class="QLabel" name="label_23">
          <property name="text">
           <string>Iteration</string>
          </property>
         </widget>
        </item>
-       <item row="5" column="1">
+       <item row="6" column="1">
         <widget class="QSpinBox" name="spinBox_visual_iteration">
          <property name="minimum">
           <number>1</number>
@@ -858,14 +875,14 @@
          </property>
         </widget>
        </item>
-       <item row="6" column="0">
+       <item row="7" column="0">
         <widget class="QLabel" name="label_19">
          <property name="text">
           <string>Max feature depth</string>
          </property>
         </widget>
        </item>
-       <item row="6" column="1">
+       <item row="7" column="1">
         <widget class="QDoubleSpinBox" name="doubleSpinBox_visual_maxDepth">
          <property name="suffix">
           <string> m</string>
@@ -881,6 +898,20 @@
          </property>
         </widget>
        </item>
+       <item row="1" column="0">
+        <widget class="QLabel" name="label_28">
+         <property name="text">
+          <string>2D transform (x,y,yaw)</string>
+         </property>
+        </widget>
+       </item>
+       <item row="1" column="1">
+        <widget class="QCheckBox" name="checkBox_visual_2d">
+         <property name="text">
+          <string/>
+         </property>
+        </widget>
+       </item>
       </layout>
      </item>
      <item>

guilib/src/ui/preferencesDialog.ui

@@ -86,7 +86,7 @@
                 <enum>QFrame::Raised</enum>
                </property>
                <property name="currentIndex">
-                <number>19</number>
+                <number>7</number>
                </property>
                <widget class="QWidget" name="page_22">
                 <layout class="QVBoxLayout" name="verticalLayout_29">
@@ -2790,6 +2790,16 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                         </property>
                        </widget>
                       </item>
+                      <item row="4" column="0">
+                       <widget class="QCheckBox" name="general_checkBox_badSignaturesIgnored">
+                        <property name="text">
+                         <string/>
+                        </property>
+                        <property name="checked">
+                         <bool>true</bool>
+                        </property>
+                       </widget>
+                      </item>
                       <item row="4" column="1">
                        <widget class="QLabel" name="label_retrieved_3">
                         <property name="text">
@@ -2800,13 +2810,23 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                         </property>
                        </widget>
                       </item>
-                      <item row="4" column="0">
+                      <item row="5" column="1">
-                       <widget class="QCheckBox" name="general_checkBox_badSignaturesIgnored">
+                       <widget class="QLabel" name="label_retrieved_4">
+                        <property name="text">
+                         <string>Initialize the Woking Memory with all nodes from Long-Term memory, instead of only nodes of the last session. This may be useful in localization mode, where less processing time is required than in SLAM mode, so more nodes can be kept in Working Memory.</string>
+                        </property>
+                        <property name="wordWrap">
+                         <bool>true</bool>
+                        </property>
+                       </widget>
+                      </item>
+                      <item row="5" column="0">
+                       <widget class="QCheckBox" name="general_checkBox_initWMWithAllNodes">
                         <property name="text">
                          <string/>
                         </property>
                         <property name="checked">
-                         <bool>true</bool>
+                         <bool>false</bool>
                         </property>
                        </widget>
                       </item>