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