DBViewer: Added option to crop empty space from local grid and scans after 2d occupancy grid is manually edited

corelib/include/rtabmap/core/DBDriver.h

@@ -96,6 +96,7 @@ public:
 				float cellSize,
 				const cv::Point3f & viewpoint);
 	void updateDepthImage(int nodeId, const cv::Mat & image);
+	void updateLaserScan(int nodeId, const LaserScan & scan);
 
 public:
 	void addInfoAfterRun(int stMemSize, int lastSignAdded, int processMemUsed, int databaseMemUsed, int dictionarySize, const ParametersMap & parameters) const;
@@ -219,16 +220,20 @@ protected:
 	virtual void updateLinkQuery(const Link & link) const = 0;
 
 	virtual void updateOccupancyGridQuery(
-				int nodeId,
-				const cv::Mat & ground,
-				const cv::Mat & obstacles,
-				const cv::Mat & empty,
-				float cellSize,
-				const cv::Point3f & viewpoint) const = 0;
+			int nodeId,
+			const cv::Mat & ground,
+			const cv::Mat & obstacles,
+			const cv::Mat & empty,
+			float cellSize,
+			const cv::Point3f & viewpoint) const = 0;
 
 	virtual void updateDepthImageQuery(
-					int nodeId,
-					const cv::Mat & image) const = 0;
+			int nodeId,
+			const cv::Mat & image) const = 0;
+
+	virtual void updateLaserScanQuery(
+			int nodeId,
+			const LaserScan & scan) const = 0;
 
 	virtual void addStatisticsQuery(const Statistics & statistics) const = 0;
 	virtual void savePreviewImageQuery(const cv::Mat & image) const = 0;

corelib/include/rtabmap/core/DBDriverSqlite3.h

@@ -100,6 +100,10 @@ protected:
 			int nodeId,
 			const cv::Mat & image) const;
 
+	void updateLaserScanQuery(
+			int nodeId,
+			const LaserScan & scan) const;
+
 	virtual void addStatisticsQuery(const Statistics & statistics) const;
 	virtual void savePreviewImageQuery(const cv::Mat & image) const;
 	virtual cv::Mat loadPreviewImageQuery() const;
@@ -150,6 +154,7 @@ private:
 	std::string queryStepImage() const;
 	std::string queryStepDepth() const;
 	std::string queryStepDepthUpdate() const;
+	std::string queryStepScanUpdate() const;
 	std::string queryStepSensorData() const;
 	std::string queryStepLinkUpdate() const;
 	std::string queryStepLink() const;
@@ -160,6 +165,7 @@ private:
 	void stepImage(sqlite3_stmt * ppStmt, int id, const cv::Mat & imageBytes) const;
 	void stepDepth(sqlite3_stmt * ppStmt, const SensorData & sensorData) const;
 	void stepDepthUpdate(sqlite3_stmt * ppStmt, int nodeId, const cv::Mat & imageCompressed) const;
+	void stepScanUpdate(sqlite3_stmt * ppStmt, int nodeId, const LaserScan & image) const;
 	void stepSensorData(sqlite3_stmt * ppStmt, const SensorData & sensorData) const;
 	void stepLink(sqlite3_stmt * ppStmt, const Link & link) const;
 	void stepWordsChanged(sqlite3_stmt * ppStmt, int signatureId, int oldWordId, int newWordId) const;

corelib/include/rtabmap/core/util3d.h

@@ -196,7 +196,7 @@ pcl::PointCloud<pcl::PointXYZ> RTABMAP_EXP laserScanFromDepthImages(
 		float maxDepth,
 		float minDepth);
 
-LaserScan RTABMAP_EXP laserScanFromPointCloud(const pcl::PCLPointCloud2 & cloud, bool filterNaNs = true);
+LaserScan RTABMAP_EXP laserScanFromPointCloud(const pcl::PCLPointCloud2 & cloud, bool filterNaNs = true, bool is2D = false, const Transform & transform = Transform());
 // return CV_32FC3  (x,y,z)
 cv::Mat RTABMAP_EXP laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZ> & cloud, const Transform & transform = Transform(), bool filterNaNs = true);
 cv::Mat RTABMAP_EXP laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZ> & cloud, const pcl::IndicesPtr & indices, const Transform & transform = Transform(), bool filterNaNs = true);

corelib/include/rtabmap/core/util3d_filtering.h

@@ -235,6 +235,13 @@ pcl::PointCloud<pcl::PointXYZINormal>::Ptr RTABMAP_EXP passThrough(
 		float max,
 		bool negative = false);
 
+pcl::IndicesPtr RTABMAP_EXP cropBox(
+		const pcl::PCLPointCloud2::Ptr & cloud,
+		const pcl::IndicesPtr & indices,
+		const Eigen::Vector4f & min,
+		const Eigen::Vector4f & max,
+		const Transform & transform = Transform::getIdentity(),
+		bool negative = false);
 pcl::IndicesPtr RTABMAP_EXP cropBox(
 		const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
 		const pcl::IndicesPtr & indices,

corelib/src/DBDriver.cpp

@@ -511,6 +511,15 @@ void DBDriver::updateDepthImage(int nodeId, const cv::Mat & image)
 	_dbSafeAccessMutex.unlock();
 }
 
+void DBDriver::updateLaserScan(int nodeId, const LaserScan & scan)
+{
+	_dbSafeAccessMutex.lock();
+	this->updateLaserScanQuery(
+			nodeId,
+			scan);
+	_dbSafeAccessMutex.unlock();
+}
+
 void DBDriver::load(VWDictionary * dictionary, bool lastStateOnly) const
 {
 	_dbSafeAccessMutex.lock();

corelib/src/DBDriverSqlite3.cpp

@@ -4408,12 +4408,12 @@ void DBDriverSqlite3::updateLinkQuery(const Link & link) const
 }
 
 void DBDriverSqlite3::updateOccupancyGridQuery(
-			int nodeId,
-			const cv::Mat & ground,
-			const cv::Mat & obstacles,
-			const cv::Mat & empty,
-			float cellSize,
-			const cv::Point3f & viewpoint) const
+		int nodeId,
+		const cv::Mat & ground,
+		const cv::Mat & obstacles,
+		const cv::Mat & empty,
+		float cellSize,
+		const cv::Point3f & viewpoint) const
 {
 	UDEBUG("");
 	if(_ppDb)
@@ -4447,8 +4447,8 @@ void DBDriverSqlite3::updateOccupancyGridQuery(
 }
 
 void DBDriverSqlite3::updateDepthImageQuery(
-				int nodeId,
-				const cv::Mat & image) const
+		int nodeId,
+		const cv::Mat & image) const
 {
 	UDEBUG("");
 	if(_ppDb)
@@ -4477,6 +4477,37 @@ void DBDriverSqlite3::updateDepthImageQuery(
 	}
 }
 
+void DBDriverSqlite3::updateLaserScanQuery(
+		int nodeId,
+		const LaserScan & scan) const
+{
+	UDEBUG("");
+	if(_ppDb)
+	{
+		std::string type;
+		UTimer timer;
+		timer.start();
+		int rc = SQLITE_OK;
+		sqlite3_stmt * ppStmt = 0;
+
+		// Create query
+		std::string query = queryStepScanUpdate();
+		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 depth
+		stepScanUpdate(ppStmt,
+				nodeId,
+				scan);
+
+		// 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());
+	}
+}
+
 void DBDriverSqlite3::addStatisticsQuery(const Statistics & statistics) const
 {
 	UDEBUG("Ref ID = %d", statistics.refImageId());
@@ -5622,6 +5653,132 @@ void DBDriverSqlite3::stepDepthUpdate(sqlite3_stmt * ppStmt, int nodeId, const c
 	UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
 }
 
+std::string DBDriverSqlite3::queryStepScanUpdate() const
+{
+	UASSERT(uStrNumCmp(_version, "0.10.0") >= 0);
+	if(uStrNumCmp(_version, "0.11.10") >= 0)
+	{
+		return "UPDATE Data SET scan_info=?, scan=? WHERE id=?;";
+	}
+	else if(uStrNumCmp(_version, "0.10.7") >= 0)
+	{
+		return "UPDATE Data SET scan_max_pts=?, scan_max_range=?, scan=? WHERE id=?;";
+	}
+	else
+	{
+		return "UPDATE Data SET scan_max_pts=? scan=? WHERE id=?;";
+	}
+}
+void DBDriverSqlite3::stepScanUpdate(sqlite3_stmt * ppStmt, int nodeId, const LaserScan & scan) const
+{
+	if(!ppStmt)
+	{
+		UFATAL("");
+	}
+
+	int rc = SQLITE_OK;
+	int index = 1;
+
+	std::vector<float> scanInfo;
+	if(uStrNumCmp(_version, "0.11.10") >= 0)
+	{
+		if(scan.maxPoints() > 0 ||
+			scan.rangeMax() > 0 ||
+			(uStrNumCmp(_version, "0.16.1")>=0 && scan.format() != LaserScan::kUnknown) ||
+			(!scan.localTransform().isNull() && !scan.localTransform().isIdentity()))
+		{
+			if(uStrNumCmp(_version, "0.16.1") >=0)
+			{
+				if(uStrNumCmp(_version, "0.18.0") >=0)
+				{
+					scanInfo.resize(7 + Transform().size());
+					scanInfo[0] = scan.format();
+					scanInfo[1] = scan.rangeMin();
+					scanInfo[2] = scan.rangeMax();
+					scanInfo[3] = scan.angleMin();
+					scanInfo[4] = scan.angleMax();
+					scanInfo[5] = scan.angleIncrement();
+					scanInfo[6] = scan.maxPoints(); // only for backward compatibility
+					const Transform & localTransform = scan.localTransform();
+					memcpy(scanInfo.data()+7, localTransform.data(), localTransform.size()*sizeof(float));
+				}
+				else
+				{
+					scanInfo.resize(3 + Transform().size());
+					scanInfo[0] = scan.maxPoints();
+					scanInfo[1] = scan.rangeMax();
+					scanInfo[2] = scan.format();
+					const Transform & localTransform = scan.localTransform();
+					memcpy(scanInfo.data()+3, localTransform.data(), localTransform.size()*sizeof(float));
+				}
+			}
+			else
+			{
+				scanInfo.resize(2 + Transform().size());
+				scanInfo[0] = scan.maxPoints();
+				scanInfo[1] = scan.rangeMax();
+				const Transform & localTransform = scan.localTransform();
+				memcpy(scanInfo.data()+2, localTransform.data(), localTransform.size()*sizeof(float));
+			}
+		}
+
+		if(scanInfo.size())
+		{
+			rc = sqlite3_bind_blob(ppStmt, index++, scanInfo.data(), scanInfo.size()*sizeof(float), SQLITE_STATIC);
+		}
+		else
+		{
+			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
+	{
+		// scan_max_pts
+		rc = sqlite3_bind_int(ppStmt, index++, scan.maxPoints());
+		UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
+
+		// scan_max_range
+		if(uStrNumCmp(_version, "0.10.7") >= 0)
+		{
+			rc = sqlite3_bind_double(ppStmt, index++, scan.rangeMax());
+			UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
+		}
+	}
+
+	// scan
+	cv::Mat scanCompressed;
+	if(scan.isCompressed())
+	{
+		scanCompressed = scan.data();
+	}
+	else
+	{
+		scanCompressed = compressData2(scan.data());
+	}
+	if(!scanCompressed.empty())
+	{
+		rc = sqlite3_bind_blob(ppStmt, index++, scanCompressed.data, scanCompressed.total(), SQLITE_STATIC);
+	}
+	else
+	{
+		rc = sqlite3_bind_null(ppStmt, index++);
+	}
+	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());
+}
+
 std::string DBDriverSqlite3::queryStepSensorData() const
 {
 	UASSERT(uStrNumCmp(_version, "0.10.0") >= 0);

corelib/src/util3d.cpp

@@ -1263,7 +1263,7 @@ pcl::PointCloud<pcl::PointXYZ> laserScanFromDepthImages(
 	return scan;
 }
 
-LaserScan laserScanFromPointCloud(const pcl::PCLPointCloud2 & cloud, bool filterNaNs)
+LaserScan laserScanFromPointCloud(const pcl::PCLPointCloud2 & cloud, bool filterNaNs, bool is2D, const Transform & transform)
 {
 	if(cloud.data.empty())
 	{
@@ -1284,7 +1284,7 @@ LaserScan laserScanFromPointCloud(const pcl::PCLPointCloud2 & cloud, bool filter
 			fieldStates[1] = 1;
 			fieldOffsets[1] = cloud.fields[i].offset;
 		}
-		else if(cloud.fields[i].name.compare("z") == 0)
+		else if(cloud.fields[i].name.compare("z") == 0 && !is2D)
 		{
 			fieldStates[2] = 1;
 			fieldOffsets[2] = cloud.fields[i].offset;
@@ -1332,15 +1332,23 @@ LaserScan laserScanFromPointCloud(const pcl::PCLPointCloud2 & cloud, bool filter
 	bool is3D = fieldStates[0] && fieldStates[1] && fieldStates[2];
 
 	LaserScan::Format format;
+	int outputNormalOffset = 0;
 	if(is3D)
 	{
 		if(hasNormals && hasIntensity)
 		{
 			format = LaserScan::kXYZINormal;
+			outputNormalOffset = 4;
+		}
+		else if(hasNormals && !hasIntensity && !hasRGB)
+		{
+			format = LaserScan::kXYZNormal;
+			outputNormalOffset = 3;
 		}
 		else if(hasNormals && hasRGB)
 		{
 			format = LaserScan::kXYZRGBNormal;
+			outputNormalOffset = 4;
 		}
 		else if(!hasNormals && hasIntensity)
 		{
@@ -1360,6 +1368,12 @@ LaserScan laserScanFromPointCloud(const pcl::PCLPointCloud2 & cloud, bool filter
 		if(hasNormals && hasIntensity)
 		{
 			format = LaserScan::kXYINormal;
+			outputNormalOffset = 3;
+		}
+		else if(hasNormals && !hasIntensity)
+		{
+			format = LaserScan::kXYNormal;
+			outputNormalOffset = 2;
 		}
 		else if(!hasNormals && hasIntensity)
 		{
@@ -1374,6 +1388,12 @@ LaserScan laserScanFromPointCloud(const pcl::PCLPointCloud2 & cloud, bool filter
 	UASSERT(cloud.data.size()/cloud.point_step == cloud.height*cloud.width);
 	cv::Mat laserScan = cv::Mat(1, (int)cloud.data.size()/cloud.point_step, CV_32FC(LaserScan::channels(format)));
 
+	bool transformValid = !transform.isNull() && !transform.isIdentity();
+	Transform transformRot;
+	if(transformValid)
+	{
+		transformRot = transform.rotation();
+	}
 	int oi=0;
 	for (uint32_t row = 0; row < cloud.height; ++row)
 	{
@@ -1479,6 +1499,24 @@ LaserScan laserScanFromPointCloud(const pcl::PCLPointCloud2 & cloud, bool filter
 
 			if(!filterNaNs || valid)
 			{
+				if(valid && transformValid)
+				{
+					cv::Point3f pt = util3d::transformPoint(cv::Point3f(ptr[0], ptr[1], is3D?ptr[3]:0), transform);
+					ptr[0] = pt.x;
+					ptr[1] = pt.y;
+					if(is3D)
+					{
+						ptr[2] = pt.z;
+					}
+					if(hasNormals)
+					{
+						pt = util3d::transformPoint(cv::Point3f(ptr[outputNormalOffset], ptr[outputNormalOffset+1], ptr[outputNormalOffset+2]), transformRot);
+						ptr[outputNormalOffset] = pt.x;
+						ptr[outputNormalOffset+1] = pt.y;
+						ptr[outputNormalOffset+2] = pt.z;
+					}
+				}
+
 				++oi;
 			}
 		}

corelib/src/util3d_filtering.cpp

@@ -58,10 +58,10 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <pcl/segmentation/impl/extract_labeled_clusters.hpp>
 #include <pcl/filters/impl/extract_indices.hpp>
 
-PCL_INSTANTIATE(EuclideanClusterExtraction, (pcl::PointXYZRGBNormal))
-PCL_INSTANTIATE(extractEuclideanClusters, (pcl::PointXYZRGBNormal))
-PCL_INSTANTIATE(extractEuclideanClusters_indices, (pcl::PointXYZRGBNormal))
-PCL_INSTANTIATE(ExtractIndices, (pcl::PointNormal))
+PCL_INSTANTIATE(EuclideanClusterExtraction, (pcl::PointXYZRGBNormal));
+PCL_INSTANTIATE(extractEuclideanClusters, (pcl::PointXYZRGBNormal));
+PCL_INSTANTIATE(extractEuclideanClusters_indices, (pcl::PointXYZRGBNormal));
+PCL_INSTANTIATE(ExtractIndices, (pcl::PointNormal));
 
 #endif
 
@@ -650,6 +650,24 @@ pcl::IndicesPtr cropBoxImpl(
 	return output;
 }
 
+pcl::IndicesPtr cropBox(const pcl::PCLPointCloud2::Ptr & cloud, const pcl::IndicesPtr & indices, const Eigen::Vector4f & min, const Eigen::Vector4f & max, const Transform & transform, bool negative)
+{
+	UASSERT(min[0] < max[0] && min[1] < max[1] && min[2] < max[2]);
+
+	pcl::IndicesPtr output(new std::vector<int>);
+	pcl::CropBox<pcl::PCLPointCloud2> filter;
+	filter.setNegative(negative);
+	filter.setMin(min);
+	filter.setMax(max);
+	if(!transform.isNull() && !transform.isIdentity())
+	{
+		filter.setTransform(transform.toEigen3f());
+	}
+	filter.setInputCloud(cloud);
+	filter.setIndices(indices);
+	filter.filter(*output);
+	return output;
+}
 pcl::IndicesPtr cropBox(const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud, const pcl::IndicesPtr & indices, const Eigen::Vector4f & min, const Eigen::Vector4f & max, const Transform & transform, bool negative)
 {
 	return cropBoxImpl<pcl::PointXYZ>(cloud, indices, min, max, transform, negative);

guilib/include/rtabmap/gui/DatabaseViewer.h

@@ -225,7 +225,7 @@ private:
 	std::map<int, std::pair<float, cv::Point3f> > localMapsInfo_; // <cell size, viewpoint>
 	std::map<int, std::pair<std::pair<cv::Mat, cv::Mat>, cv::Mat> > generatedLocalMaps_; // < <ground, obstacles>, empty>
 	std::map<int, std::pair<float, cv::Point3f> > generatedLocalMapsInfo_; // <cell size, viewpoint>
-	std::map<int, cv::Mat> modifiedDepthImages_;
+	std::map<int, LaserScan> modifiedLaserScans_;
 	std::vector<double> odomMaxInf_;
 	OctoMap * octomap_;
 	ExportCloudsDialog * exportDialog_;

guilib/src/DatabaseViewer.cpp

@@ -88,6 +88,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <pcl/io/ply_io.h>
 #include <pcl/io/obj_io.h>
 #include <pcl/filters/voxel_grid.h>
+#include <pcl/filters/crop_box.h>
 #include <pcl/common/transforms.h>
 #include <pcl/common/common.h>
 
@@ -434,6 +435,7 @@ DatabaseViewer::DatabaseViewer(const QString & ini, QWidget * parent) :
 	connect(ui_->toolButton_obstacleColor, SIGNAL(clicked(bool)), this, SLOT(selectObstacleColor()));
 	connect(ui_->toolButton_groundColor, SIGNAL(clicked(bool)), this, SLOT(selectGroundColor()));
 	connect(ui_->toolButton_emptyColor, SIGNAL(clicked(bool)), this, SLOT(selectEmptyColor()));
+	connect(ui_->spinBox_cropRadius, SIGNAL(valueChanged(int)), this, SLOT(configModified()));
 
 	connect(exportDialog_, SIGNAL(configChanged()), this, SLOT(configModified()));
 
@@ -545,6 +547,7 @@ void DatabaseViewer::readSettings()
 	ui_->lineEdit_obstacleColor->setText(settings.value("colorObstacle", ui_->lineEdit_obstacleColor->text()).toString());
 	ui_->lineEdit_groundColor->setText(settings.value("colorGround", ui_->lineEdit_groundColor->text()).toString());
 	ui_->lineEdit_emptyColor->setText(settings.value("colorEmpty", ui_->lineEdit_emptyColor->text()).toString());
+	ui_->spinBox_cropRadius->setValue(settings.value("cropRadius", ui_->spinBox_cropRadius->value()).toInt());
 	settings.endGroup();
 
 	settings.beginGroup("mesh");
@@ -629,6 +632,7 @@ void DatabaseViewer::writeSettings()
 	settings.setValue("colorObstacle", ui_->lineEdit_obstacleColor->text());
 	settings.setValue("colorGround", ui_->lineEdit_groundColor->text());
 	settings.setValue("colorEmpty", ui_->lineEdit_emptyColor->text());
+	settings.setValue("cropRadius", ui_->spinBox_cropRadius->value());
 	settings.endGroup();
 
 	settings.beginGroup("mesh");
@@ -713,6 +717,7 @@ void DatabaseViewer::restoreDefaultSettings()
 	ui_->lineEdit_obstacleColor->setText(QColor(Qt::red).name());
 	ui_->lineEdit_groundColor->setText(QColor(Qt::green).name());
 	ui_->lineEdit_emptyColor->setText(QColor(Qt::yellow).name());
+	ui_->spinBox_cropRadius->setValue(1);
 
 	ui_->checkBox_mesh_quad->setChecked(true);
 	ui_->spinBox_mesh_angleTolerance->setValue(15);
@@ -912,6 +917,7 @@ bool DatabaseViewer::closeDatabase()
 				}
 				// This will force rtabmap_ros to regenerate the global occupancy grid if there was one
 				dbDriver_->save2DMap(cv::Mat(), 0, 0, 0);
+				dbDriver_->saveOptimizedMesh(cv::Mat());
 			}
 
 			if(button != QMessageBox::Yes && button != QMessageBox::No)
@@ -962,6 +968,31 @@ bool DatabaseViewer::closeDatabase()
 			}
 		}
 
+		if(!modifiedLaserScans_.empty())
+		{
+			QMessageBox::StandardButton button = QMessageBox::question(this,
+					tr("Laser scans modified"),
+					tr("%1 laser scans are modified, do you want to "
+						"save them? This will overwrite laser scans saved in the database.")
+					.arg(modifiedLaserScans_.size()),
+					QMessageBox::Cancel | QMessageBox::Yes | QMessageBox::No,
+					QMessageBox::Cancel);
+
+			if(button == QMessageBox::Yes)
+			{
+				for(std::map<int, LaserScan>::iterator iter=modifiedLaserScans_.begin(); iter!=modifiedLaserScans_.end(); ++iter)
+				{
+					dbDriver_->updateLaserScan(iter->first, iter->second);
+				}
+				modifiedLaserScans_.clear();
+			}
+
+			if(button != QMessageBox::Yes && button != QMessageBox::No)
+			{
+				return false;
+			}
+		}
+
 		delete dbDriver_;
 		dbDriver_ = 0;
 		ids_.clear();
@@ -986,6 +1017,7 @@ bool DatabaseViewer::closeDatabase()
 		localMapsInfo_.clear();
 		generatedLocalMaps_.clear();
 		generatedLocalMapsInfo_.clear();
+		modifiedLaserScans_.clear();
 		ui_->graphViewer->clearAll();
 		occupancyGridViewer_->clear();
 		ui_->menuEdit->setEnabled(false);
@@ -2620,19 +2652,20 @@ void DatabaseViewer::editSaved2DMap()
 	}
 
 	editMapArea_->setMap(map8URotated);
-	if(editMapDialog_->exec() == QDialog::Accepted && editMapArea_->isModified())
+
+	if(editMapDialog_->exec() == QDialog::Accepted)
 	{
-		cv::Mat map = editMapArea_->getModifiedMap();
+		cv::Mat mapModified = editMapArea_->getModifiedMap();
 
 		if(!ui_->graphViewer->isOrientationENU())
 		{
 			//ROTATE_90_CLOCKWISE
-			cv::transpose(map, map8URotated);
+			cv::transpose(mapModified, map8URotated);
 			cv::flip(map8URotated, map8URotated, 1);
 		}
 		else
 		{
-			map8URotated = map;
+			map8URotated = mapModified;
 		}
 		cv::flip(map8URotated, map8UFlip, 0);
 
@@ -2640,8 +2673,208 @@ void DatabaseViewer::editSaved2DMap()
 		UASSERT(map8UFlip.cols == map8U.cols);
 		UASSERT(map8UFlip.rows == map8U.rows);
 
-		dbDriver_->save2DMap(rtabmap::util3d::convertImage8U2Map(map8UFlip, false), xMin, yMin, cellSize);
-		QMessageBox::information(this, tr("Edit 2D map"), tr("Map updated!"));
+		cv::Mat map8S = rtabmap::util3d::convertImage8U2Map(map8UFlip, false);
+
+		if(editMapArea_->isModified())
+		{
+			dbDriver_->save2DMap(map8S, xMin, yMin, cellSize);
+			QMessageBox::information(this, tr("Edit 2D map"), tr("Map updated!"));
+		}
+
+		int cropRadius = ui_->spinBox_cropRadius->value();
+		QMessageBox::StandardButton b = QMessageBox::question(this,
+				tr("Crop empty space"),
+				tr("Do you want to clear empty space from local occupancy grids and laser scans?\n\n"
+					"Advantages:\n"
+					"   * If the map needs to be regenerated in the future (e.g., when we re-use the map in SLAM mode), removed obstacles won't reappear.\n"
+					"   * The cropped laser scans will be also used for localization, so if dynamic obstacles have been removed, localization won't try to match them anymore.\n\n"
+					"Disadvantage:\n"
+					"   * Cropping the laser scans cannot be reverted after the viewer is closed and changes have been saved.\n\n"
+					"Parameter(s):\n"
+					"   Crop radius = %1 pixels\n\n"
+					"Press \"Yes\" to filter only grids.\n"
+					"Press \"Yes to All\" to filter both grids and laser scans.\n").arg(cropRadius),
+				QMessageBox::Yes | QMessageBox::YesToAll | QMessageBox::No, QMessageBox::No);
+		if(b == QMessageBox::Yes || b == QMessageBox::YesToAll)
+		{
+			std::map<int, Transform> poses = dbDriver_->loadOptimizedPoses(); // poses should match the grid map
+
+			modifiedLaserScans_.clear();
+
+			rtabmap::ProgressDialog progressDialog(this);
+			progressDialog.setMaximumSteps(poses.size()+1);
+			progressDialog.show();
+			progressDialog.setCancelButtonVisible(true);
+			progressDialog.appendText(QString("Cropping empty space... %1 scans to filter").arg(poses.size()));
+			progressDialog.setMinimumWidth(800);
+			QApplication::processEvents();
+
+			UINFO("Cropping empty space... poses=%d cropRadius=%d", poses.size(), cropRadius);
+			UASSERT(cropRadius>=0);
+			for(std::map<int, Transform>::iterator iter=poses.begin(); iter!=poses.end() && !progressDialog.isCanceled(); ++iter)
+			{
+				// local grid
+				cv::Mat gridGround;
+				cv::Mat gridObstacles;
+				cv::Mat gridEmpty;
+
+				// scan
+				SensorData data;
+				dbDriver_->getNodeData(iter->first, data);
+				LaserScan scan;
+				data.uncompressData(0,0,&scan,0,&gridGround,&gridObstacles,&gridEmpty);
+
+				if(generatedLocalMaps_.find(iter->first) != generatedLocalMaps_.end())
+				{
+					gridObstacles = generatedLocalMaps_.find(iter->first)->second.first.second;
+				}
+				if(!gridObstacles.empty())
+				{
+					cv::Mat filtered = cv::Mat(1, gridObstacles.cols, gridObstacles.type());
+					int oi = 0;
+					for(int i=0; i<gridObstacles.cols; ++i)
+					{
+						const float * ptr = gridObstacles.ptr<float>(0, i);
+						cv::Point3f pt(ptr[0], ptr[1], gridObstacles.channels()==2?0:ptr[2]);
+						pt = util3d::transformPoint(pt, iter->second);
+
+						int x = int((pt.x - xMin) / cellSize + 0.5f);
+						int y = int((pt.y - yMin) / cellSize + 0.5f);
+
+						if(x>=0 && x<map8S.cols &&
+						   y>=0 && y<map8S.rows)
+						{
+							bool obstacleDetected = false;
+
+							for(int j=-cropRadius; j<=cropRadius && !obstacleDetected; ++j)
+							{
+								for(int k=-cropRadius; k<=cropRadius && !obstacleDetected; ++k)
+								{
+									if(x+j>=0 && x+j<map8S.cols &&
+									   y+k>=0 && y+k<map8S.rows &&
+									   map8S.at<unsigned char>(y+k,x+j) == 100)
+									{
+										obstacleDetected = true;
+									}
+								}
+							}
+
+							if(map8S.at<unsigned char>(y,x) != 0 || obstacleDetected)
+							{
+								// Verify that we don't have an obstacle on neighbor cells
+								cv::Mat(gridObstacles, cv::Range::all(), cv::Range(i,i+1)).copyTo(cv::Mat(filtered, cv::Range::all(), cv::Range(oi,oi+1)));
+								++oi;
+							}
+						}
+					}
+
+					if(oi != gridObstacles.cols)
+					{
+						progressDialog.appendText(QString("Grid %1 filtered %2 pts -> %3 pts").arg(iter->first).arg(gridObstacles.cols).arg(oi));
+						UINFO("Grid %d filtered %d -> %d", iter->first, gridObstacles.cols, oi);
+
+						// update
+						std::pair<std::pair<cv::Mat, cv::Mat>, cv::Mat> value;
+						if(generatedLocalMaps_.find(iter->first) != generatedLocalMaps_.end())
+						{
+							value = generatedLocalMaps_.at(iter->first);
+						}
+						else
+						{
+							value.first.first = gridGround;
+							value.second = gridEmpty;
+							uInsert(generatedLocalMapsInfo_, std::make_pair(data.id(), std::make_pair(data.gridCellSize(), data.gridViewPoint())));
+						}
+						value.first.second = cv::Mat(filtered, cv::Range::all(), cv::Range(0, oi));
+						uInsert(generatedLocalMaps_, std::make_pair(iter->first, value));
+					}
+				}
+
+				if(!scan.isEmpty())
+				{
+					Transform mapToScan = iter->second * scan.localTransform();
+
+					cv::Mat filtered = cv::Mat(1, scan.size(), scan.dataType());
+					int oi = 0;
+					for(int i=0; i<scan.size(); ++i)
+					{
+						const float * ptr = scan.data().ptr<float>(0, i);
+						cv::Point3f pt(ptr[0], ptr[1], scan.is2d()?0:ptr[2]);
+						pt = util3d::transformPoint(pt, mapToScan);
+
+						int x = int((pt.x - xMin) / cellSize + 0.5f);
+						int y = int((pt.y - yMin) / cellSize + 0.5f);
+
+						if(x>=0 && x<map8S.cols &&
+						   y>=0 && y<map8S.rows)
+						{
+							bool obstacleDetected = false;
+
+							for(int j=-cropRadius; j<=cropRadius && !obstacleDetected; ++j)
+							{
+								for(int k=-cropRadius; k<=cropRadius && !obstacleDetected; ++k)
+								{
+									if(x+j>=0 && x+j<map8S.cols &&
+									   y+k>=0 && y+k<map8S.rows &&
+									   map8S.at<unsigned char>(y+k,x+j) == 100)
+									{
+										obstacleDetected = true;
+									}
+								}
+							}
+
+							if(map8S.at<unsigned char>(y,x) != 0 || obstacleDetected)
+							{
+								// Verify that we don't have an obstacle on neighbor cells
+								cv::Mat(scan.data(), cv::Range::all(), cv::Range(i,i+1)).copyTo(cv::Mat(filtered, cv::Range::all(), cv::Range(oi,oi+1)));
+								++oi;
+							}
+						}
+					}
+
+					if(oi != scan.size())
+					{
+						progressDialog.appendText(QString("Scan %1 filtered %2 pts -> %3 pts").arg(iter->first).arg(scan.size()).arg(oi));
+						UINFO("Scan %d filtered %d -> %d", iter->first, scan.size(), oi);
+
+						// update
+						if(scan.angleIncrement()!=0)
+						{
+							// copy meta data
+							scan = LaserScan(
									cv::Mat(filtered, cv::Range::all(), cv::Range(0, oi)),
+									scan.format(),
+									scan.rangeMin(),
+									scan.rangeMax(),
+									scan.angleMin(),
+									scan.angleMax(),
+									scan.angleIncrement(),
+									scan.localTransform());
+						}
+						else
+						{
+							// copy meta data
+							scan = LaserScan(
									cv::Mat(filtered, cv::Range::all(), cv::Range(0, oi)),
+									scan.maxPoints(),
+									scan.rangeMax(),
+									scan.format(),
+									scan.localTransform());
+						}
+						uInsert(modifiedLaserScans_, std::make_pair(iter->first, scan));
+					}
+				}
+				progressDialog.incrementStep();
+				QApplication::processEvents();
+			}
+			if(progressDialog.isCanceled())
+			{
+				modifiedLaserScans_.clear();
+			}
+			else
+			{
+				update3dView();
+			}
+			progressDialog.setValue(progressDialog.maximumSteps());
+		}
 	}
 }
 
@@ -2756,7 +2989,8 @@ void DatabaseViewer::regenerateSavedMap()
 	{
 		QMessageBox::warning(this, tr("Cannot import 2D map"),
 				tr("The database has modified links and/or modified local "
-				   "occupancy grids, the 2D optimized map cannot be modified."));
+				   "occupancy grids, the 2D optimized map cannot be modified. Try "
+				   "closing the database and re-open it to save the changes."));
 		return;
 	}
 
@@ -2787,6 +3021,11 @@ void DatabaseViewer::regenerateSavedMap()
 	else
 	{
 		dbDriver_->save2DMap(map, xMin, yMin, grid.getCellSize());
+		Transform lastlocalizationPose;
+		dbDriver_->loadOptimizedPoses(&lastlocalizationPose);
+		dbDriver_->saveOptimizedPoses(graphes_.back(), lastlocalizationPose);
+		// reset optimized mesh as poses have changed
+		dbDriver_->saveOptimizedMesh(cv::Mat());
 		QMessageBox::information(this, tr("Regenerate 2D map"), tr("Map regenerated!"));
 		ui_->actionEdit_optimized_2D_map->setEnabled(true);
 		ui_->actionExport_saved_2D_map->setEnabled(true);
@@ -2994,6 +3233,10 @@ void DatabaseViewer::updateOptimizedMesh()
 				ui_->doubleSpinBox_posefilteringRadius->value(),
 				ui_->doubleSpinBox_posefilteringAngle->value()*CV_PI/180.0);
 	}
+	Transform lastlocalizationPose;
+	dbDriver_->loadOptimizedPoses(&lastlocalizationPose);
+	//optimized poses have changed, reset 2d map
+	dbDriver_->save2DMap(cv::Mat(), 0, 0, 0);
 	if(optimizedPoses.size() > 0)
 	{
 		exportDialog_->setDBDriver(dbDriver_);
@@ -3021,7 +3264,7 @@ void DatabaseViewer::updateOptimizedMesh()
 		{
 			if(textureMeshes.size())
 			{
-				dbDriver_->saveOptimizedPoses(optimizedPoses, Transform());
+				dbDriver_->saveOptimizedPoses(optimizedPoses, lastlocalizationPose);
 
 				cv::Mat globalTextures;
 				pcl::TextureMeshPtr textureMesh = textureMeshes.at(0);
@@ -3057,7 +3300,7 @@ void DatabaseViewer::updateOptimizedMesh()
 			}
 			else if(meshes.size())
 			{
-				dbDriver_->saveOptimizedPoses(optimizedPoses, Transform());
+				dbDriver_->saveOptimizedPoses(optimizedPoses, lastlocalizationPose);
 				std::vector<std::vector<std::vector<unsigned int> > > polygons(1);
 				polygons.at(0) = util3d::convertPolygonsFromPCL(meshes.at(0)->polygons);
 				dbDriver_->saveOptimizedMesh(util3d::laserScanFromPointCloud(meshes.at(0)->cloud, false).data(), polygons);
@@ -3068,7 +3311,7 @@ void DatabaseViewer::updateOptimizedMesh()
 			}
 			else if(clouds.size())
 			{
-				dbDriver_->saveOptimizedPoses(optimizedPoses, Transform());
+				dbDriver_->saveOptimizedPoses(optimizedPoses, lastlocalizationPose);
 				dbDriver_->saveOptimizedMesh(util3d::laserScanFromPointCloud(*clouds.at(0)));
 				QMessageBox::information(this, tr("Update Optimized PointCloud"), tr("Updated!"));
 				ui_->actionView_optimized_mesh->setEnabled(true);
@@ -3793,6 +4036,7 @@ void DatabaseViewer::resetAllChanges()
 	linksRemoved_.clear();
 	generatedLocalMaps_.clear();
 	generatedLocalMapsInfo_.clear();
+	modifiedLaserScans_.clear();
 	updateLoopClosuresSlider();
 	this->updateGraphView();
 }
@@ -4289,31 +4533,36 @@ void DatabaseViewer::update(int value,
 					}
 
 					//add scan
-					if(ui_->checkBox_showScan->isChecked() && data.laserScanRaw().size())
+					LaserScan laserScanRaw = data.laserScanRaw();
+					if(modifiedLaserScans_.find(id)!=modifiedLaserScans_.end())
 					{
-						if(data.laserScanRaw().hasRGB() && data.laserScanRaw().hasNormals())
+						laserScanRaw = modifiedLaserScans_.at(id);
+					}
+					if(ui_->checkBox_showScan->isChecked() && laserScanRaw.size())
+					{
+						if(laserScanRaw.hasRGB() && laserScanRaw.hasNormals())
 						{
-							pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr scan = util3d::laserScanToPointCloudRGBNormal(data.laserScanRaw(), data.laserScanRaw().localTransform());
+							pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr scan = util3d::laserScanToPointCloudRGBNormal(laserScanRaw, laserScanRaw.localTransform());
 							cloudViewer_->addCloud("scan", scan, pose, Qt::yellow);
 						}
-						else if(data.laserScanRaw().hasIntensity() && data.laserScanRaw().hasNormals())
+						else if(laserScanRaw.hasIntensity() && laserScanRaw.hasNormals())
 						{
-							pcl::PointCloud<pcl::PointXYZINormal>::Ptr scan = util3d::laserScanToPointCloudINormal(data.laserScanRaw(), data.laserScanRaw().localTransform());
+							pcl::PointCloud<pcl::PointXYZINormal>::Ptr scan = util3d::laserScanToPointCloudINormal(laserScanRaw, laserScanRaw.localTransform());
 							cloudViewer_->addCloud("scan", scan, pose, Qt::yellow);
 						}
-						else if(data.laserScanRaw().hasNormals())
+						else if(laserScanRaw.hasNormals())
 						{
-							pcl::PointCloud<pcl::PointNormal>::Ptr scan = util3d::laserScanToPointCloudNormal(data.laserScanRaw(), data.laserScanRaw().localTransform());
+							pcl::PointCloud<pcl::PointNormal>::Ptr scan = util3d::laserScanToPointCloudNormal(laserScanRaw, laserScanRaw.localTransform());
 							cloudViewer_->addCloud("scan", scan, pose, Qt::yellow);
 						}
-						else if(data.laserScanRaw().hasRGB())
+						else if(laserScanRaw.hasRGB())
 						{
-							pcl::PointCloud<pcl::PointXYZRGB>::Ptr scan = util3d::laserScanToPointCloudRGB(data.laserScanRaw(), data.laserScanRaw().localTransform());
+							pcl::PointCloud<pcl::PointXYZRGB>::Ptr scan = util3d::laserScanToPointCloudRGB(laserScanRaw, laserScanRaw.localTransform());
 							cloudViewer_->addCloud("scan", scan, pose, Qt::yellow);
 						}
 						else
 						{
-							pcl::PointCloud<pcl::PointXYZ>::Ptr scan = util3d::laserScanToPointCloud(data.laserScanRaw(), data.laserScanRaw().localTransform());
+							pcl::PointCloud<pcl::PointXYZ>::Ptr scan = util3d::laserScanToPointCloud(laserScanRaw, laserScanRaw.localTransform());
 							cloudViewer_->addCloud("scan", scan, pose, Qt::yellow);
 						}
 					}

guilib/src/ui/DatabaseViewer.ui

@@ -61,7 +61,7 @@
           <rect>
            <x>0</x>
            <y>0</y>
-           <width>404</width>
+           <width>296</width>
            <height>311</height>
           </rect>
          </property>
@@ -304,7 +304,7 @@
           <rect>
            <x>0</x>
            <y>0</y>
-           <width>403</width>
+           <width>296</width>
            <height>311</height>
           </rect>
          </property>
@@ -1369,7 +1369,7 @@
      <item>
       <widget class="QToolBox" name="toolBox">
        <property name="currentIndex">
-        <number>0</number>
+        <number>1</number>
        </property>
        <widget class="QWidget" name="page_3">
         <property name="geometry">
@@ -1514,9 +1514,9 @@
         <property name="geometry">
          <rect>
           <x>0</x>
-          <y>0</y>
+          <y>-267</y>
           <width>519</width>
-          <height>791</height>
+          <height>831</height>
          </rect>
         </property>
         <attribute name="label">
@@ -1808,6 +1808,26 @@
              </property>
             </widget>
            </item>
+           <item row="13" column="1">
+            <widget class="QLabel" name="label_57">
+             <property name="text">
+              <string>Crop radius when filtering empty space from 2d occupancy grid.</string>
+             </property>
+             <property name="wordWrap">
+              <bool>true</bool>
+             </property>
+            </widget>
+           </item>
+           <item row="13" column="0">
+            <widget class="QSpinBox" name="spinBox_cropRadius">
+             <property name="suffix">
+              <string> pixels</string>
+             </property>
+             <property name="value">
+              <number>1</number>
+             </property>
+            </widget>
+           </item>
           </layout>
          </item>
          <item>