DatabaseViewer: added regeneration of occupancy grids action and saving back to database on close

corelib/include/rtabmap/core/DBDriver.h

@@ -86,6 +86,12 @@ public:
 	void addLink(const Link & link);
 	void removeLink(int from, int to);
 	void updateLink(const Link & link);
+	void updateOccupancyGrid(
+				int nodeId,
+				const cv::Mat & ground,
+				const cv::Mat & obstacles,
+				float cellSize,
+				const cv::Point3f & viewpoint);
 
 public:
 	void addStatisticsAfterRun(int stMemSize, int lastSignAdded, int processMemUsed, int databaseMemUsed, int dictionarySize, const ParametersMap & parameters) const;
@@ -164,6 +170,13 @@ private:
 	virtual void addLinkQuery(const Link & link) const = 0;
 	virtual void updateLinkQuery(const Link & link) const = 0;
 
+	virtual void updateOccupancyGridQuery(
+				int nodeId,
+				const cv::Mat & ground,
+				const cv::Mat & obstacles,
+				float cellSize,
+				const cv::Point3f & viewpoint) const = 0;
+
 	// Load objects
 	virtual void loadQuery(VWDictionary * dictionary) const = 0;
 	virtual void loadLastNodesQuery(std::list<Signature *> & signatures) const = 0;

corelib/src/DBDriver.cpp

@@ -405,6 +405,25 @@ void DBDriver::updateLink(const Link & link)
 	this->updateLinkQuery(link);
 	_dbSafeAccessMutex.unlock();
 }
+void DBDriver::updateOccupancyGrid(
+		int nodeId,
+		const cv::Mat & ground,
+		const cv::Mat & obstacles,
+		float cellSize,
+		const cv::Point3f & viewpoint)
+{
+	_dbSafeAccessMutex.lock();
+	//just to make sure the occupancy grids are compressed for convenience
+	SensorData data;
+	data.setOccupancyGrid(ground, obstacles, cellSize, viewpoint);
+	this->updateOccupancyGridQuery(
+			nodeId,
+			data.gridGroundCellsCompressed(),
+			data.gridObstacleCellsCompressed(),
+			cellSize,
+			viewpoint);
+	_dbSafeAccessMutex.unlock();
+}
 
 void DBDriver::load(VWDictionary * dictionary) const
 {

corelib/src/DBDriverSqlite3.cpp

@@ -3040,6 +3040,43 @@ void DBDriverSqlite3::updateLinkQuery(const Link & link) const
 	}
 }
 
+void DBDriverSqlite3::updateOccupancyGridQuery(
+			int nodeId,
+			const cv::Mat & ground,
+			const cv::Mat & obstacles,
+			float cellSize,
+			const cv::Point3f & viewpoint) const
+{
+	UDEBUG("");
+	if(_ppDb)
+	{
+		std::string type;
+		UTimer timer;
+		timer.start();
+		int rc = SQLITE_OK;
+		sqlite3_stmt * ppStmt = 0;
+
+		// Create query
+		std::string query = queryStepOccupancyGridUpdate();
+		rc = sqlite3_prepare_v2(_ppDb, query.c_str(), -1, &ppStmt, 0);
+		UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
+
+		// Save occupancy grid
+		stepOccupancyGrid(ppStmt,
+				nodeId,
+				ground,
+				obstacles,
+				cellSize,
+				viewpoint);
+
+		// Finalize (delete) the statement
+		rc = sqlite3_finalize(ppStmt);
+		UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
+
+		UDEBUG("Time=%fs", timer.ticks());
+	}
+}
+
 std::string DBDriverSqlite3::queryStepNode() const
 {
 	if(uStrNumCmp(_version, "0.11.1") >= 0)
@@ -3503,7 +3540,7 @@ void DBDriverSqlite3::stepSensorData(sqlite3_stmt * ppStmt,
 		UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
 		rc = sqlite3_bind_double(ppStmt, index++, sensorData.gridViewPoint().z);
 		UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
-}
+	}
 
 	//step
 	rc=sqlite3_step(ppStmt);
@@ -3717,4 +3754,85 @@ void DBDriverSqlite3::stepKeypoint(sqlite3_stmt * ppStmt,
 	UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
 }
 
+std::string DBDriverSqlite3::queryStepOccupancyGridUpdate() const
+{
+	UASSERT(uStrNumCmp(_version, "0.11.10") >= 0);
+	return "UPDATE Data SET ground_cells=?, obstacle_cells=?, cell_size=?, view_point_x=?, view_point_y=?, view_point_z=? WHERE id=?;";
+}
+void DBDriverSqlite3::stepOccupancyGrid(sqlite3_stmt * ppStmt,
+		int nodeId,
+		const cv::Mat & ground,
+		const cv::Mat & obstacles,
+		float cellSize,
+		const cv::Point3f & viewpoint) const
+{
+	UASSERT(uStrNumCmp(_version, "0.11.10") >= 0);
+	UASSERT(ground.empty() || ground.type() == CV_8UC1); // compressed
+	UASSERT(obstacles.empty() || obstacles.type() == CV_8UC1); // compressed
+	UDEBUG("Update occupancy grid %d: ground=%d obstacles=%d cell=%f viewpoint=(%f,%f,%f)",
+			nodeId,
+			ground.cols,
+			obstacles.cols,
+			cellSize,
+			viewpoint.x,
+			viewpoint.y,
+			viewpoint.z);
+	if(!ppStmt)
+	{
+		UFATAL("");
+	}
+
+	int rc = SQLITE_OK;
+	int index = 1;
+
+	//ground_cells
+	if(ground.empty())
+	{
+		rc = sqlite3_bind_null(ppStmt, index++);
+		UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
+	}
+	else
+	{
+		// compress
+		rc = sqlite3_bind_blob(ppStmt, index++, ground.data, ground.cols, SQLITE_STATIC);
+		UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
+	}
+
+	//obstacle_cells
+	if(obstacles.empty())
+	{
+		rc = sqlite3_bind_null(ppStmt, index++);
+		UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
+	}
+	else
+	{
+		rc = sqlite3_bind_blob(ppStmt, index++, obstacles.data, obstacles.cols, SQLITE_STATIC);
+		UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
+	}
+
+	//cell_size
+	rc = sqlite3_bind_double(ppStmt, index++, cellSize);
+	UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
+
+	//view_point
+	rc = sqlite3_bind_double(ppStmt, index++, viewpoint.x);
+	UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
+	rc = sqlite3_bind_double(ppStmt, index++, viewpoint.y);
+	UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
+	rc = sqlite3_bind_double(ppStmt, index++, viewpoint.z);
+	UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
+
+	// id
+	rc = sqlite3_bind_int(ppStmt, index++, nodeId);
+	UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
+
+
+	//step
+	rc=sqlite3_step(ppStmt);
+	UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
+
+	rc = sqlite3_reset(ppStmt);
+	UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
+}
+
 } // namespace rtabmap

corelib/src/DBDriverSqlite3.h

@@ -76,6 +76,13 @@ private:
 	virtual void addLinkQuery(const Link & link) const;
 	virtual void updateLinkQuery(const Link & link) const;
 
+	virtual void updateOccupancyGridQuery(
+			int nodeId,
+			const cv::Mat & ground,
+			const cv::Mat & obstacles,
+			float cellSize,
+			const cv::Point3f & viewpoint) const;
+
 	// Load objects
 	virtual void loadQuery(VWDictionary * dictionary) const;
 	virtual void loadLastNodesQuery(std::list<Signature *> & signatures) const;
@@ -102,6 +109,7 @@ private:
 	std::string queryStepLink() const;
 	std::string queryStepWordsChanged() const;
 	std::string queryStepKeypoint() const;
+	std::string queryStepOccupancyGridUpdate() const;
 	void stepNode(sqlite3_stmt * ppStmt, const Signature * s) const;
 	void stepImage(
 			sqlite3_stmt * ppStmt,
@@ -112,6 +120,12 @@ private:
 	void stepLink(sqlite3_stmt * ppStmt, const Link & link) const;
 	void stepWordsChanged(sqlite3_stmt * ppStmt, int signatureId, int oldWordId, int newWordId) const;
 	void stepKeypoint(sqlite3_stmt * ppStmt, int signatureId, int wordId, const cv::KeyPoint & kp, const cv::Point3f & pt, const cv::Mat & descriptor) const;
+	void stepOccupancyGrid(sqlite3_stmt * ppStmt,
+			int nodeId,
+			const cv::Mat & ground,
+			const cv::Mat & obstacles,
+			float cellSize,
+			const cv::Point3f & viewpoint) const;
 
 private:
 	void loadLinksQuery(std::list<Signature *> & signatures) const;

guilib/include/rtabmap/gui/DatabaseViewer.h

@@ -83,6 +83,7 @@ private slots:
 	void generateLocalGraph();
 	void generateTOROGraph();
 	void generateG2OGraph();
+	void regenerateLocalMaps();
 	void view3DMap();
 	void view3DLaserScans();
 	void generate3DMap();
@@ -178,6 +179,8 @@ private:
 	std::multimap<int, rtabmap::Link> linksAdded_;
 	std::multimap<int, rtabmap::Link> linksRemoved_;
 	std::map<int, std::pair<cv::Mat, cv::Mat> > localMaps_; // <ground, obstacles>
+	std::map<int, std::pair<cv::Mat, cv::Mat> > generatedLocalMaps_; // <ground, obstacles>
+	std::map<int, std::pair<float, cv::Point3f> > generatedLocalMapsInfo_; // <cell size, viewpoint>
 
 	bool savedMaximized_;
 	bool firstCall_;

guilib/src/DatabaseViewer.cpp

@@ -65,6 +65,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "rtabmap/core/Optimizer.h"
 #include "rtabmap/core/RegistrationVis.h"
 #include "rtabmap/core/RegistrationIcp.h"
+#include "rtabmap/core/OccupancyGrid.h"
 #include "rtabmap/gui/DataRecorder.h"
 #include "rtabmap/core/SensorData.h"
 #include "ExportDialog.h"
@@ -183,6 +184,7 @@ DatabaseViewer::DatabaseViewer(const QString & ini, QWidget * parent) :
 	connect(ui_->actionRefine_all_loop_closure_links, SIGNAL(triggered()), this, SLOT(refineAllLoopClosureLinks()));
 	connect(ui_->actionVisual_Refine_all_neighbor_links, SIGNAL(triggered()), this, SLOT(refineVisuallyAllNeighborLinks()));
 	connect(ui_->actionVisual_Refine_all_loop_closure_links, SIGNAL(triggered()), this, SLOT(refineVisuallyAllLoopClosureLinks()));
+	connect(ui_->actionRegenerate_local_grid_maps, SIGNAL(triggered()), this, SLOT(regenerateLocalMaps()));
 	connect(ui_->actionReset_all_changes, SIGNAL(triggered()), this, SLOT(resetAllChanges()));
 
 	//ICP buttons
@@ -250,13 +252,7 @@ DatabaseViewer::DatabaseViewer(const QString & ini, QWidget * parent) :
 	connect(ui_->doubleSpinBox_posefilteringRadius, SIGNAL(editingFinished()), this, SLOT(updateGraphView()));
 	connect(ui_->doubleSpinBox_posefilteringAngle, SIGNAL(editingFinished()), this, SLOT(updateGraphView()));
 
-	connect(ui_->groupBox_gridFromProjection, SIGNAL(clicked(bool)), this, SLOT(updateGrid()));
 	connect(ui_->doubleSpinBox_gridCellSize, SIGNAL(editingFinished()), this, SLOT(updateGrid()));
-	connect(ui_->spinBox_projDecimation, SIGNAL(editingFinished()), this, SLOT(updateGrid()));
-	connect(ui_->doubleSpinBox_projMaxDepth, SIGNAL(editingFinished()), this, SLOT(updateGrid()));
-	connect(ui_->doubleSpinBox_projMinDepth, SIGNAL(editingFinished()), this, SLOT(updateGrid()));
-	connect(ui_->doubleSpinBox_projMaxAngle, SIGNAL(editingFinished()), this, SLOT(updateGrid()));
-	connect(ui_->spinBox_projClusterSize, SIGNAL(editingFinished()), this, SLOT(updateGrid()));
 
 	ui_->label_stereo_inliers_name->setStyleSheet("QLabel {color : blue; }");
 	ui_->label_stereo_flowOutliers_name->setStyleSheet("QLabel {color : red; }");
@@ -280,13 +276,7 @@ DatabaseViewer::DatabaseViewer(const QString & ini, QWidget * parent) :
 	connect(ui_->spinBox_optimizationDepth, SIGNAL(valueChanged(int)), this, SLOT(configModified()));
 	connect(ui_->checkBox_gridErode, SIGNAL(stateChanged(int)), this, SLOT(configModified()));
 	connect(ui_->checkBox_gridFillUnkownSpace, SIGNAL(stateChanged(int)), this, SLOT(configModified()));
-	connect(ui_->groupBox_gridFromProjection, SIGNAL(clicked(bool)), this, SLOT(configModified()));
 	connect(ui_->doubleSpinBox_gridCellSize, SIGNAL(valueChanged(double)), this, SLOT(configModified()));
-	connect(ui_->spinBox_projDecimation, SIGNAL(valueChanged(int)), this, SLOT(configModified()));
-	connect(ui_->doubleSpinBox_projMaxDepth, SIGNAL(valueChanged(double)), this, SLOT(configModified()));
-	connect(ui_->doubleSpinBox_projMinDepth, SIGNAL(valueChanged(double)), this, SLOT(configModified()));
-	connect(ui_->doubleSpinBox_projMaxAngle, SIGNAL(valueChanged(double)), this, SLOT(configModified()));
-	connect(ui_->spinBox_projClusterSize, SIGNAL(valueChanged(int)), this, SLOT(configModified()));
 	connect(ui_->groupBox_posefiltering, SIGNAL(clicked(bool)), this, SLOT(configModified()));
 	connect(ui_->doubleSpinBox_posefilteringRadius, SIGNAL(valueChanged(double)), this, SLOT(configModified()));
 	connect(ui_->doubleSpinBox_posefilteringAngle, SIGNAL(valueChanged(double)), this, SLOT(configModified()));
@@ -402,13 +392,7 @@ void DatabaseViewer::readSettings()
 	settings.endGroup();
 
 	settings.beginGroup("grid");
-	ui_->groupBox_gridFromProjection->setChecked(settings.value("gridFromProj", ui_->groupBox_gridFromProjection->isChecked()).toBool());
 	ui_->doubleSpinBox_gridCellSize->setValue(settings.value("gridCellSize", ui_->doubleSpinBox_gridCellSize->value()).toDouble());
-	ui_->spinBox_projDecimation->setValue(settings.value("projDecimation", ui_->spinBox_projDecimation->value()).toInt());
-	ui_->doubleSpinBox_projMaxDepth->setValue(settings.value("projMaxDepth", ui_->doubleSpinBox_projMaxDepth->value()).toDouble());
-	ui_->doubleSpinBox_projMinDepth->setValue(settings.value("projMinDepth", ui_->doubleSpinBox_projMinDepth->value()).toDouble());
-	ui_->doubleSpinBox_projMaxAngle->setValue(settings.value("projMaxAngle", ui_->doubleSpinBox_projMaxAngle->value()).toDouble());
-	ui_->spinBox_projClusterSize->setValue(settings.value("projClusterSize", ui_->spinBox_projClusterSize->value()).toInt());
 	ui_->groupBox_posefiltering->setChecked(settings.value("poseFiltering", ui_->groupBox_posefiltering->isChecked()).toBool());
 	ui_->doubleSpinBox_posefilteringRadius->setValue(settings.value("poseFilteringRadius", ui_->doubleSpinBox_posefilteringRadius->value()).toDouble());
 	ui_->doubleSpinBox_posefilteringAngle->setValue(settings.value("poseFilteringAngle", ui_->doubleSpinBox_posefilteringAngle->value()).toDouble());
@@ -490,13 +474,7 @@ void DatabaseViewer::writeSettings()
 
 	// save Grid settings
 	settings.beginGroup("grid");
-	settings.setValue("gridFromProj", ui_->groupBox_gridFromProjection->isChecked());
 	settings.setValue("gridCellSize", ui_->doubleSpinBox_gridCellSize->value());
-	settings.setValue("projDecimation", ui_->spinBox_projDecimation->value());
-	settings.setValue("projMaxDepth", ui_->doubleSpinBox_projMaxDepth->value());
-	settings.setValue("projMinDepth", ui_->doubleSpinBox_projMinDepth->value());
-	settings.setValue("projMaxAngle", ui_->doubleSpinBox_projMaxAngle->value());
-	settings.setValue("projClusterSize", ui_->spinBox_projClusterSize->value());
 	settings.setValue("poseFiltering", ui_->groupBox_posefiltering->isChecked());
 	settings.setValue("poseFilteringRadius", ui_->doubleSpinBox_posefilteringRadius->value());
 	settings.setValue("poseFilteringAngle", ui_->doubleSpinBox_posefilteringAngle->value());
@@ -580,6 +558,8 @@ bool DatabaseViewer::openDatabase(const QString & path)
 			linksRefined_.clear();
 			linksRemoved_.clear();
 			localMaps_.clear();
+			generatedLocalMaps_.clear();
+			generatedLocalMapsInfo_.clear();
 			ui_->graphViewer->clearAll();
 			ui_->actionGenerate_TORO_graph_graph->setEnabled(false);
 			ui_->actionGenerate_g2o_graph_g2o->setEnabled(false);
@@ -680,6 +660,7 @@ void DatabaseViewer::closeEvent(QCloseEvent* event)
 		writeSettings();
 	}
 
+	event->accept();
 	if(linksAdded_.size() || linksRefined_.size() || linksRemoved_.size())
 	{
 		QMessageBox::StandardButton button = QMessageBox::question(this,
@@ -723,20 +704,52 @@ void DatabaseViewer::closeEvent(QCloseEvent* event)
 				dbDriver_->removeLink(iter->second.to(), iter->second.from());
 				dbDriver_->removeLink(iter->second.from(), iter->second.to());
 			}
+			linksAdded_.clear();
+			linksRefined_.clear();
+			linksRemoved_.clear();
 		}
 
-		if(button == QMessageBox::Yes || button == QMessageBox::No)
+		if(button != QMessageBox::Yes && button != QMessageBox::No)
-		{
-			event->accept();
-		}
-		else
 		{
 			event->ignore();
 		}
 	}
-	else
+
+	if(event->isAccepted() && generatedLocalMaps_.size())
 	{
-		event->accept();
+		QMessageBox::StandardButton button = QMessageBox::question(this,
+				tr("Local occupancy grid maps modified"),
+				tr("%1 occupancy grid maps are modified, do you want to "
+					"save them? This will overwrite occupancy grids saved in the database.")
+				.arg(generatedLocalMaps_.size()),
+				QMessageBox::Cancel | QMessageBox::Yes | QMessageBox::No,
+				QMessageBox::Cancel);
+
+		if(button == QMessageBox::Yes)
+		{
+			// Rejected links
+			UASSERT(generatedLocalMaps_.size() == generatedLocalMapsInfo_.size());
+			std::map<int, std::pair<cv::Mat, cv::Mat> >::iterator mapIter = generatedLocalMaps_.begin();
+			std::map<int, std::pair<float, cv::Point3f> >::iterator infoIter = generatedLocalMapsInfo_.begin();
+			for(; mapIter!=generatedLocalMaps_.end(); ++mapIter, ++infoIter)
+			{
+				UASSERT(mapIter->first == infoIter->first);
+				dbDriver_->updateOccupancyGrid(
+						mapIter->first,
+						mapIter->second.first,
+						mapIter->second.second,
+						infoIter->second.first,
+						infoIter->second.second);
+			}
+			generatedLocalMaps_.clear();
+			generatedLocalMapsInfo_.clear();
+			localMaps_.clear();
+		}
+
+		if(button != QMessageBox::Yes && button != QMessageBox::No)
+		{
+			event->ignore();
+		}
 	}
 
 	if(event->isAccepted())
@@ -1528,6 +1541,52 @@ void DatabaseViewer::generateG2OGraph()
 	}
 }
 
+void DatabaseViewer::regenerateLocalMaps()
+{
+	UTimer time;
+	OccupancyGrid grid(ui_->parameters_toolbox->getParameters());
+
+	rtabmap::ProgressDialog progressDialog(this);
+	progressDialog.setMaximumSteps(ids_.size());
+	progressDialog.show();
+
+	for(int i =0; i<ids_.size(); ++i)
+	{
+		SensorData data;
+		dbDriver_->getNodeData(ids_.at(i), data);
+		data.uncompressData();
+
+		int mapId, weight;
+		Transform odomPose, groundTruth;
+		std::string label;
+		double stamp;
+		QString msg;
+		if(dbDriver_->getNodeInfo(data.id(), odomPose, mapId, weight, label, stamp, groundTruth))
+		{
+			Signature s = data;
+			s.setPose(odomPose);
+			cv::Mat ground, obstacles;
+			cv::Point3f viewpoint;
+			grid.createLocalMap(s, ground, obstacles, viewpoint);
+			uInsert(generatedLocalMaps_, std::make_pair(data.id(), std::make_pair(ground, obstacles)));
+			uInsert(generatedLocalMapsInfo_, std::make_pair(data.id(), std::make_pair(grid.getCellSize(), viewpoint)));
+			msg = QString("Generated local occupancy grid map %1/%2 (%3s)").arg(i+1).arg((int)ids_.size()).arg(time.ticks());
+			if(i==37)
+			{
+				pcl::io::savePCDFileBinary("ground.pcd", *util3d::laserScanToPointCloud(ground));
+				pcl::io::savePCDFileBinary("obstacles.pcd", *util3d::laserScanToPointCloud(obstacles));
+				UWARN("Saved ground.pcd and obstacles.pcd");
+			}
+		}
+
+		progressDialog.appendText(msg);
+		progressDialog.incrementStep();
+		QApplication::processEvents();
+	}
+	progressDialog.setValue(progressDialog.maximumSteps());
+	updateGrid();
+}
+
 void DatabaseViewer::view3DMap()
 {
 	if(!ids_.size() || !dbDriver_)
@@ -2162,6 +2221,8 @@ void DatabaseViewer::resetAllChanges()
 	linksAdded_.clear();
 	linksRefined_.clear();
 	linksRemoved_.clear();
+	generatedLocalMaps_.clear();
+	generatedLocalMapsInfo_.clear();
 	updateLoopClosuresSlider();
 	this->updateGraphView();
 }
@@ -3436,6 +3497,7 @@ void DatabaseViewer::sliderIterationsValueChanged(int value)
 					ui_->doubleSpinBox_posefilteringRadius->value(),
 					ui_->doubleSpinBox_posefilteringAngle->value()*CV_PI/180.0);
 		}
+		std::map<int, std::pair<cv::Mat, cv::Mat> > localMaps;
 		if(ui_->dockWidget_graphView->isVisible())
 		{
 			//update scans
@@ -3443,84 +3505,21 @@ void DatabaseViewer::sliderIterationsValueChanged(int value)
 			std::vector<int> ids = uKeys(graphFiltered);
 			for(unsigned int i=0; i<ids.size(); ++i)
 			{
-				if(localMaps_.find(ids[i]) == localMaps_.end())
+				if(generatedLocalMaps_.find(ids[i]) != generatedLocalMaps_.end())
 				{
-					UTimer time;
+					localMaps.insert(*generatedLocalMaps_.find(ids[i]));
-					bool added = false;
+				}
-					if(ui_->groupBox_gridFromProjection->isChecked())
+				else if(localMaps_.find(ids[i]) != localMaps_.end())
-					{
+				{
-						SensorData data;
+					localMaps.insert(*localMaps_.find(ids.at(i)));
-						dbDriver_->getNodeData(ids.at(i), data);
+				}
-						data.uncompressData();
+				else
-						if(!data.depthOrRightRaw().empty())
+				{
-						{
+					SensorData data;
-							pcl::PointCloud<pcl::PointXYZ>::Ptr cloud;
+					dbDriver_->getNodeData(ids.at(i), data);
-							pcl::IndicesPtr validIndices(new std::vector<int>);
+					cv::Mat ground, obstacles;
-							cloud = util3d::cloudFromSensorData(data,
+					data.uncompressData(0, 0, 0, 0, &ground, &obstacles);
-									ui_->spinBox_projDecimation->value(),
+					localMaps_.insert(std::make_pair(ids.at(i), std::make_pair(ground, obstacles)));
-									ui_->doubleSpinBox_projMaxDepth->value(),
-									ui_->doubleSpinBox_projMinDepth->value(),
-									validIndices.get(),
-									ui_->parameters_toolbox->getParameters());
-							UASSERT(ui_->doubleSpinBox_gridCellSize->value() > 0);
-							cloud = util3d::voxelize(cloud, validIndices, ui_->doubleSpinBox_gridCellSize->value());
-
-							// add pose rotation without yaw
-							float roll, pitch, yaw;
-							graphFiltered.at(ids[i]).getEulerAngles(roll, pitch, yaw);
-							cloud = util3d::transformPointCloud(cloud, Transform(0,0,0, roll, pitch, 0));
-
-							if(cloud->size())
-							{
-								UTimer timer;
-								float cellSize = ui_->doubleSpinBox_gridCellSize->value();
-								float groundNormalMaxAngle = ui_->doubleSpinBox_projMaxAngle->value();
-								int minClusterSize = ui_->spinBox_projClusterSize->value();
-								cv::Mat ground, obstacles;
-
-								util3d::occupancy2DFromCloud3D<pcl::PointXYZ>(
-										cloud,
-										ground, obstacles,
-										cellSize,
-										groundNormalMaxAngle,
-										minClusterSize);
-
-								if(!ground.empty() || !obstacles.empty())
-								{
-									localMaps_.insert(std::make_pair(ids.at(i), std::make_pair(ground, obstacles)));
-									added = true;
-								}
-							}
-						}
-					}
-					else
-					{
-						SensorData data;
-						dbDriver_->getNodeData(ids.at(i), data);
-						if(!data.laserScanCompressed().empty())
-						{
-							pcl::PointCloud<pcl::PointXYZ>::Ptr cloud;
-							cv::Mat laserScan;
-							data.uncompressDataConst(0, 0, &laserScan);
-							cv::Mat ground, obstacles;
-							if(laserScan.type() == CV_32FC2)
-							{
-								util3d::occupancy2DFromLaserScan(
-										laserScan,
-										ground,
-										obstacles,
-										ui_->doubleSpinBox_gridCellSize->value(),
-										ui_->checkBox_gridFillUnkownSpace->isChecked(),
-										data.laserScanInfo().maxRange());
-								added = true;
-							}
-							localMaps_.insert(std::make_pair(ids.at(i), std::make_pair(ground, obstacles)));
-						}
-					}
-					if(added)
-					{
-						UINFO("Processed grid map %d/%d (%fs)", i+1, (int)ids.size(), time.ticks());
-					}
 				}
 			}
 			//cleanup
@@ -3540,14 +3539,14 @@ void DatabaseViewer::sliderIterationsValueChanged(int value)
 
 		ui_->graphViewer->updateGTGraph(groundTruthPoses_);
 		ui_->graphViewer->updateGraph(graph, graphLinks_, mapIds_);
-		if(graph.size() && localMaps_.size() && ui_->graphViewer->isGridMapVisible())
+		if(graph.size() && localMaps.size() && ui_->graphViewer->isGridMapVisible())
 		{
 			float xMin, yMin;
 			float cell = ui_->doubleSpinBox_gridCellSize->value();
 			cv::Mat map;
 			QTime time;
 			time.start();
-			map = rtabmap::util3d::create2DMapFromOccupancyLocalMaps(graphFiltered, localMaps_, cell, xMin, yMin, 0, ui_->checkBox_gridErode->isChecked());
+			map = rtabmap::util3d::create2DMapFromOccupancyLocalMaps(graphFiltered, localMaps, cell, xMin, yMin, 0, ui_->checkBox_gridErode->isChecked());
 			if(!map.empty())
 			{
 				ui_->graphViewer->updateMap(rtabmap::util3d::convertMap2Image8U(map), cell, xMin, yMin);
@@ -3798,16 +3797,7 @@ void DatabaseViewer::updateGraphView()
 
 void DatabaseViewer::updateGrid()
 {
-	if((sender() != ui_->spinBox_projDecimation && sender() != ui_->doubleSpinBox_projMaxDepth && sender() != ui_->doubleSpinBox_projMinDepth && sender()!=ui_->doubleSpinBox_projMaxAngle && sender()!=ui_->spinBox_projClusterSize) ||
+	updateGraphView();
-	   (sender() == ui_->spinBox_projDecimation && ui_->groupBox_gridFromProjection->isChecked()) ||
-	   (sender() == ui_->doubleSpinBox_projMaxDepth && ui_->groupBox_gridFromProjection->isChecked()) ||
-	   (sender() == ui_->doubleSpinBox_projMinDepth && ui_->groupBox_gridFromProjection->isChecked()) ||
-	   (sender() == ui_->doubleSpinBox_projMaxAngle && ui_->groupBox_gridFromProjection->isChecked()) ||
-	   (sender() == ui_->spinBox_projClusterSize && ui_->groupBox_gridFromProjection->isChecked()))
-	{
-		localMaps_.clear();
-		updateGraphView();
-	}
 }
 
 Link DatabaseViewer::findActiveLink(int from, int to)

guilib/src/ParametersToolBox.cpp

@@ -488,10 +488,6 @@ void ParametersToolBox::addParameter(QVBoxLayout * layout, const QString & key,
 	QHBoxLayout * hLayout = new QHBoxLayout();
 	layout->insertLayout(layout->count()-1, hLayout);
 	QString tmp = key.split('/').last();
-	if(tmp.at(0).isDigit())
-	{
-		tmp.remove(0,1);
-	}
 	QLabel * label = new QLabel(tmp, this);
 	label->setObjectName(key+"/label");
 	label->setToolTip(QString("<FONT>%1</FONT>").arg(Parameters::getDescription(key.toStdString()).c_str()));

guilib/src/ui/DatabaseViewer.ui

@@ -517,6 +517,8 @@
     <addaction name="actionVisual_Refine_all_loop_closure_links"/>
     <addaction name="actionRefine_all_loop_closure_links"/>
     <addaction name="separator"/>
+    <addaction name="actionRegenerate_local_grid_maps"/>
+    <addaction name="separator"/>
     <addaction name="actionReset_all_changes"/>
     <addaction name="separator"/>
     <addaction name="actionView_3D_map"/>
@@ -980,7 +982,7 @@
      <item>
       <widget class="QToolBox" name="toolBox">
        <property name="currentIndex">
-        <number>3</number>
+        <number>1</number>
        </property>
        <widget class="QWidget" name="page_3">
         <property name="geometry">
@@ -1124,7 +1126,7 @@
           <x>0</x>
           <y>0</y>
           <width>278</width>
-          <height>611</height>
+          <height>410</height>
          </rect>
         </property>
         <attribute name="label">
@@ -1133,32 +1135,6 @@
         <layout class="QVBoxLayout" name="verticalLayout_16">
          <item>
           <layout class="QGridLayout" name="gridLayout_9" columnstretch="0,1">
-           <item row="0" column="0">
-            <widget class="QDoubleSpinBox" name="doubleSpinBox_gridCellSize">
-             <property name="suffix">
-              <string> m</string>
-             </property>
-             <property name="minimum">
-              <double>0.010000000000000</double>
-             </property>
-             <property name="maximum">
-              <double>5.000000000000000</double>
-             </property>
-             <property name="singleStep">
-              <double>0.010000000000000</double>
-             </property>
-             <property name="value">
-              <double>0.050000000000000</double>
-             </property>
-            </widget>
-           </item>
-           <item row="0" column="1">
-            <widget class="QLabel" name="label_36">
-             <property name="text">
-              <string>Grid cell size</string>
-             </property>
-            </widget>
-           </item>
            <item row="1" column="1">
             <widget class="QLabel" name="label_44">
              <property name="text">
@@ -1187,150 +1163,28 @@
              </property>
             </widget>
            </item>
+           <item row="0" column="1">
+            <widget class="QLabel" name="label_46">
+             <property name="text">
+              <string>Grid cell size</string>
+             </property>
+            </widget>
+           </item>
+           <item row="0" column="0">
+            <widget class="QDoubleSpinBox" name="doubleSpinBox_gridCellSize">
+             <property name="decimals">
+              <number>3</number>
+             </property>
+             <property name="minimum">
+              <double>0.010000000000000</double>
+             </property>
+             <property name="singleStep">
+              <double>0.010000000000000</double>
+             </property>
+            </widget>
+           </item>
           </layout>
          </item>
-         <item>
-          <widget class="QGroupBox" name="groupBox_gridFromProjection">
-           <property name="title">
-            <string>Grid from 3D projection</string>
-           </property>
-           <property name="checkable">
-            <bool>true</bool>
-           </property>
-           <property name="checked">
-            <bool>true</bool>
-           </property>
-           <layout class="QGridLayout" name="gridLayout_8" columnstretch="0,1">
-            <item row="0" column="0">
-             <widget class="QSpinBox" name="spinBox_projDecimation">
-              <property name="minimum">
-               <number>1</number>
-              </property>
-              <property name="maximum">
-               <number>8</number>
-              </property>
-              <property name="value">
-               <number>4</number>
-              </property>
-             </widget>
-            </item>
-            <item row="0" column="1">
-             <widget class="QLabel" name="label_37">
-              <property name="text">
-               <string>Cloud decimation</string>
-              </property>
-             </widget>
-            </item>
-            <item row="1" column="0">
-             <widget class="QDoubleSpinBox" name="doubleSpinBox_projMaxDepth">
-              <property name="suffix">
-               <string> m</string>
-              </property>
-              <property name="decimals">
-               <number>1</number>
-              </property>
-              <property name="minimum">
-               <double>0.400000000000000</double>
-              </property>
-              <property name="maximum">
-               <double>999.000000000000000</double>
-              </property>
-              <property name="singleStep">
-               <double>1.000000000000000</double>
-              </property>
-              <property name="value">
-               <double>4.000000000000000</double>
-              </property>
-             </widget>
-            </item>
-            <item row="1" column="1">
-             <widget class="QLabel" name="label_38">
-              <property name="text">
-               <string>Cloud max depth</string>
-              </property>
-             </widget>
-            </item>
-            <item row="3" column="1">
-             <widget class="QLabel" name="label_66">
-              <property name="text">
-               <string>Cluster size</string>
-              </property>
-             </widget>
-            </item>
-            <item row="4" column="1">
-             <widget class="QLabel" name="label_67">
-              <property name="text">
-               <string>Max ground angle</string>
-              </property>
-             </widget>
-            </item>
-            <item row="4" column="0">
-             <widget class="QDoubleSpinBox" name="doubleSpinBox_projMaxAngle">
-              <property name="suffix">
-               <string> rad</string>
-              </property>
-              <property name="decimals">
-               <number>2</number>
-              </property>
-              <property name="minimum">
-               <double>0.000000000000000</double>
-              </property>
-              <property name="maximum">
-               <double>3.140000000000000</double>
-              </property>
-              <property name="singleStep">
-               <double>0.100000000000000</double>
-              </property>
-              <property name="value">
-               <double>0.790000000000000</double>
-              </property>
-             </widget>
-            </item>
-            <item row="3" column="0">
-             <widget class="QSpinBox" name="spinBox_projClusterSize">
-              <property name="minimum">
-               <number>1</number>
-              </property>
-              <property name="maximum">
-               <number>10000</number>
-              </property>
-              <property name="value">
-               <number>20</number>
-              </property>
-             </widget>
-            </item>
-            <item row="2" column="1">
-             <widget class="QLabel" name="label_40">
-              <property name="text">
-               <string>Cloud min depth</string>
-              </property>
-             </widget>
-            </item>
-            <item row="2" column="0">
-             <widget class="QDoubleSpinBox" name="doubleSpinBox_projMinDepth">
-              <property name="suffix">
-               <string> m</string>
-              </property>
-              <property name="decimals">
-               <number>1</number>
-              </property>
-              <property name="minimum">
-               <double>0.000000000000000</double>
-              </property>
-              <property name="maximum">
-               <double>999.000000000000000</double>
-              </property>
-              <property name="singleStep">
-               <double>1.000000000000000</double>
-              </property>
-              <property name="value">
-               <double>0.000000000000000</double>
-              </property>
-             </widget>
-            </item>
-           </layout>
-          </widget>
-         </item>
          <item>
           <widget class="QGroupBox" name="groupBox_posefiltering">
            <property name="title">
@@ -1634,8 +1488,8 @@
          <rect>
           <x>0</x>
           <y>0</y>
-          <width>289</width>
+          <width>175</width>
-          <height>182</height>
+          <height>191</height>
          </rect>
         </property>
         <attribute name="label">
@@ -2025,6 +1879,11 @@
     <string>Export 3D laser scans (*.ply *.pcd) ...</string>
    </property>
   </action>
+  <action name="actionRegenerate_local_grid_maps">
+   <property name="text">
+    <string>Regenerate local grid maps...</string>
+   </property>
+  </action>
  </widget>
  <customwidgets>
   <customwidget>