0.11.10: Database update with occupancy grid and laser scan info. Added class LaserScanInfo and OccupancyGrid (incremental 2d grid map). Gui: 2d grid and octomap are udpated using occupancy grids saved in nodes.

2026-10-05 17:27:46 +08:00 · 2016-08-31 12:43:53 -04:00
parent af02e02978
commit 013eba1d58
49 changed files with 3376 additions and 1259 deletions
@@ -38,8 +38,7 @@ namespace rtabmap
 SensorData::SensorData() :
 		_id(0),
 		_stamp(0.0),
-		_laserScanMaxPts(0),
-		_laserScanMaxRange(0.0f)
+		_cellSize(0.0f)
 {
 }

@@ -51,8 +50,7 @@ SensorData::SensorData(
 		const cv::Mat & userData) :
 		_id(id),
 		_stamp(stamp),
-		_laserScanMaxPts(0),
-		_laserScanMaxRange(0.0f)
+		_cellSize(0.0f)
 {
 	if(image.rows == 1)
 	{
@@ -85,9 +83,8 @@ SensorData::SensorData(
 		const cv::Mat & userData) :
 		_id(id),
 		_stamp(stamp),
-		_laserScanMaxPts(0),
-		_laserScanMaxRange(0.0f),
-		_cameraModels(std::vector<CameraModel>(1, cameraModel))
+		_cameraModels(std::vector<CameraModel>(1, cameraModel)),
+		_cellSize(0.0f)
 {
 	if(image.rows == 1)
 	{
@@ -121,9 +118,8 @@ SensorData::SensorData(
 		const cv::Mat & userData) :
 		_id(id),
 		_stamp(stamp),
-		_laserScanMaxPts(0),
-		_laserScanMaxRange(0.0f),
-		_cameraModels(std::vector<CameraModel>(1, cameraModel))
+		_cameraModels(std::vector<CameraModel>(1, cameraModel)),
+		_cellSize(0.0f)
 {
 	if(rgb.rows == 1)
 	{
@@ -162,8 +158,7 @@ SensorData::SensorData(
 // RGB-D constructor + laser scan
 SensorData::SensorData(
 		const cv::Mat & laserScan,
-		int laserScanMaxPts,
-		float laserScanMaxRange,
+		const LaserScanInfo & laserScanInfo,
 		const cv::Mat & rgb,
 		const cv::Mat & depth,
 		const CameraModel & cameraModel,
@@ -172,9 +167,9 @@ SensorData::SensorData(
 		const cv::Mat & userData) :
 		_id(id),
 		_stamp(stamp),
-		_laserScanMaxPts(laserScanMaxPts),
-		_laserScanMaxRange(laserScanMaxRange),
-		_cameraModels(std::vector<CameraModel>(1, cameraModel))
+		_cameraModels(std::vector<CameraModel>(1, cameraModel)),
+		_laserScanInfo(laserScanInfo),
+		_cellSize(0.0f)
 {
 	if(rgb.rows == 1)
 	{
@@ -199,7 +194,7 @@ SensorData::SensorData(
 		_depthOrRightRaw = depth;
 	}

-	if(laserScan.type() == CV_32FC2 || laserScan.type() == CV_32FC3 || laserScan.type() == CV_32FC(6))
+	if(laserScan.type() == CV_32FC2 || laserScan.type() == CV_32FC3  || laserScan.type() == CV_32FC(4) || laserScan.type() == CV_32FC(6))
 	{
 		_laserScanRaw = laserScan;
 	}
@@ -229,9 +224,8 @@ SensorData::SensorData(
 		const cv::Mat & userData) :
 		_id(id),
 		_stamp(stamp),
-		_laserScanMaxPts(0),
-		_laserScanMaxRange(0.0f),
-		_cameraModels(cameraModels)
+		_cameraModels(cameraModels),
+		_cellSize(0.0f)
 {
 	if(rgb.rows == 1)
 	{
@@ -269,8 +263,7 @@ SensorData::SensorData(
 // Multi-cameras RGB-D constructor + laser scan
 SensorData::SensorData(
 		const cv::Mat & laserScan,
-		int laserScanMaxPts,
-		float laserScanMaxRange,
+		const LaserScanInfo & laserScanInfo,
 		const cv::Mat & rgb,
 		const cv::Mat & depth,
 		const std::vector<CameraModel> & cameraModels,
@@ -279,9 +272,9 @@ SensorData::SensorData(
 		const cv::Mat & userData) :
 		_id(id),
 		_stamp(stamp),
-		_laserScanMaxPts(laserScanMaxPts),
-		_laserScanMaxRange(laserScanMaxRange),
-		_cameraModels(cameraModels)
+		_cameraModels(cameraModels),
+		_laserScanInfo(laserScanInfo),
+		_cellSize(0.0f)
 {
 	if(rgb.rows == 1)
 	{
@@ -306,7 +299,7 @@ SensorData::SensorData(
 		_depthOrRightRaw = depth;
 	}

-	if(laserScan.type() == CV_32FC2 || laserScan.type() == CV_32FC3 || laserScan.type() == CV_32FC(6))
+	if(laserScan.type() == CV_32FC2 || laserScan.type() == CV_32FC3 || laserScan.type() == CV_32FC(4) || laserScan.type() == CV_32FC(6))
 	{
 		_laserScanRaw = laserScan;
 	}
@@ -336,9 +329,8 @@ SensorData::SensorData(
 		const cv::Mat & userData):
 		_id(id),
 		_stamp(stamp),
-		_laserScanMaxPts(0),
-		_laserScanMaxRange(0.0f),
-		_stereoCameraModel(cameraModel)
+		_stereoCameraModel(cameraModel),
+		_cellSize(0.0f)
 {
 	if(left.rows == 1)
 	{
@@ -378,8 +370,7 @@ SensorData::SensorData(
 // Stereo constructor + 2d laser scan
 SensorData::SensorData(
 		const cv::Mat & laserScan,
-		int laserScanMaxPts,
-		float laserScanMaxRange,
+		const LaserScanInfo & laserScanInfo,
 		const cv::Mat & left,
 		const cv::Mat & right,
 		const StereoCameraModel & cameraModel,
@@ -388,9 +379,9 @@ SensorData::SensorData(
 		const cv::Mat & userData) :
 		_id(id),
 		_stamp(stamp),
-		_laserScanMaxPts(laserScanMaxPts),
-		_laserScanMaxRange(laserScanMaxRange),
-		_stereoCameraModel(cameraModel)
+		_stereoCameraModel(cameraModel),
+		_laserScanInfo(laserScanInfo),
+		_cellSize(0.0f)
 {
 	if(left.rows == 1)
 	{
@@ -414,7 +405,7 @@ SensorData::SensorData(
 		_depthOrRightRaw = right;
 	}

-	if(laserScan.type() == CV_32FC2 || laserScan.type() == CV_32FC3 || laserScan.type() == CV_32FC(6))
+	if(laserScan.type() == CV_32FC2 || laserScan.type() == CV_32FC3 || laserScan.type() == CV_32FC(4) || laserScan.type() == CV_32FC(6))
 	{
 		_laserScanRaw = laserScan;
 	}
@@ -480,18 +471,97 @@ void SensorData::setUserData(const cv::Mat & userData)
 	}
 }

+void SensorData::setOccupancyGrid(
+			const cv::Mat & ground,
+			const cv::Mat & obstacles,
+			float cellSize,
+			const cv::Point3f & viewPoint)
+{
+	UDEBUG("ground=%d obstacles=%d", ground.cols, obstacles.cols);
+	if((!ground.empty() && (!_groundCellsCompressed.empty() || !_groundCellsRaw.empty())) ||
+	   (!obstacles.empty() && (!_obstacleCellsCompressed.empty() || !_obstacleCellsRaw.empty())))
+	{
+		UWARN("Occupancy grid cannot be overwritten! id=%d", this->id());
+		return;
+	}
+
+	_groundCellsRaw = cv::Mat();
+	_groundCellsCompressed = cv::Mat();
+	_obstacleCellsRaw = cv::Mat();
+	_obstacleCellsCompressed = cv::Mat();
+
+	CompressionThread ctGround(ground);
+	CompressionThread ctObstacles(obstacles);
+
+	if(!ground.empty())
+	{
+		if(ground.type() == CV_32FC2 || ground.type() == CV_32FC3 || ground.type() == CV_32FC(4) || ground.type() == CV_32FC(6))
+		{
+			_groundCellsRaw = ground;
+			ctGround.start();
+		}
+		else if(ground.type() == CV_8UC1)
+		{
+			UASSERT(ground.type() == CV_8UC1); // Bytes
+			_groundCellsCompressed = ground;
+		}
+	}
+	if(!obstacles.empty())
+	{
+		if(obstacles.type() == CV_32FC2 || obstacles.type() == CV_32FC3 || obstacles.type() == CV_32FC(4) || obstacles.type() == CV_32FC(6))
+		{
+			_obstacleCellsRaw = obstacles;
+			ctObstacles.start();
+		}
+		else if(obstacles.type() == CV_8UC1)
+		{
+			UASSERT(obstacles.type() == CV_8UC1); // Bytes
+			_obstacleCellsCompressed = obstacles;
+		}
+	}
+	ctGround.join();
+	ctObstacles.join();
+	if(!_groundCellsRaw.empty())
+	{
+		_groundCellsCompressed = ctGround.getCompressedData();
+	}
+	if(!_obstacleCellsRaw.empty())
+	{
+		_obstacleCellsCompressed = ctObstacles.getCompressedData();
+	}
+
+	_cellSize = cellSize;
+	_viewPoint = viewPoint;
+}
+
 void SensorData::uncompressData()
 {
-	cv::Mat tmpA, tmpB, tmpC, tmpD;
+	cv::Mat tmpA, tmpB, tmpC, tmpD, tmpE, tmpF;
 	uncompressData(_imageCompressed.empty()?0:&tmpA,
 				_depthOrRightCompressed.empty()?0:&tmpB,
 				_laserScanCompressed.empty()?0:&tmpC,
-				_userDataCompressed.empty()?0:&tmpD);
+				_userDataCompressed.empty()?0:&tmpD,
+				_groundCellsCompressed.empty()?0:&tmpE,
+				_obstacleCellsCompressed.empty()?0:&tmpF);
 }

-void SensorData::uncompressData(cv::Mat * imageRaw, cv::Mat * depthRaw, cv::Mat * laserScanRaw, cv::Mat * userDataRaw)
+void SensorData::uncompressData(
+		cv::Mat * imageRaw,
+		cv::Mat * depthRaw,
+		cv::Mat * laserScanRaw,
+		cv::Mat * userDataRaw,
+		cv::Mat * groundCellsRaw,
+		cv::Mat * obstacleCellsRaw)
 {
-	uncompressDataConst(imageRaw, depthRaw, laserScanRaw, userDataRaw);
+	UDEBUG("%d", this->id());
+	uncompressDataConst(
+			imageRaw,
+			depthRaw,
+			laserScanRaw,
+			userDataRaw,
+			groundCellsRaw,
+			obstacleCellsRaw);
+
 	if(imageRaw && !imageRaw->empty() && _imageRaw.empty())
 	{
 		_imageRaw = *imageRaw;
@@ -520,9 +590,23 @@ void SensorData::uncompressData(cv::Mat * imageRaw, cv::Mat * depthRaw, cv::Mat
 	{
 		_userDataRaw = *userDataRaw;
 	}
+	if(groundCellsRaw && !groundCellsRaw->empty() && _groundCellsRaw.empty())
+	{
+		_groundCellsRaw = *groundCellsRaw;
+	}
+	if(obstacleCellsRaw && !obstacleCellsRaw->empty() && _obstacleCellsRaw.empty())
+	{
+		_obstacleCellsRaw = *obstacleCellsRaw;
+	}
 }

-void SensorData::uncompressDataConst(cv::Mat * imageRaw, cv::Mat * depthRaw, cv::Mat * laserScanRaw, cv::Mat * userDataRaw) const
+void SensorData::uncompressDataConst(
+		cv::Mat * imageRaw,
+		cv::Mat * depthRaw,
+		cv::Mat * laserScanRaw,
+		cv::Mat * userDataRaw,
+		cv::Mat * groundCellsRaw,
+		cv::Mat * obstacleCellsRaw) const
 {
 	if(imageRaw)
 	{
@@ -540,35 +624,64 @@ void SensorData::uncompressDataConst(cv::Mat * imageRaw, cv::Mat * depthRaw, cv:
 	{
 		*userDataRaw = _userDataRaw;
 	}
+	if(groundCellsRaw)
+	{
+		*groundCellsRaw = _groundCellsRaw;
+	}
+	if(obstacleCellsRaw)
+	{
+		*obstacleCellsRaw = _obstacleCellsRaw;
+	}
 	if( (imageRaw && imageRaw->empty()) ||
 		(depthRaw && depthRaw->empty()) ||
 		(laserScanRaw && laserScanRaw->empty()) ||
-		(userDataRaw && userDataRaw->empty()))
+		(userDataRaw && userDataRaw->empty()) ||
+		(groundCellsRaw && groundCellsRaw->empty()) ||
+		(obstacleCellsRaw && obstacleCellsRaw->empty()))
 	{
 		rtabmap::CompressionThread ctImage(_imageCompressed, true);
 		rtabmap::CompressionThread ctDepth(_depthOrRightCompressed, true);
 		rtabmap::CompressionThread ctLaserScan(_laserScanCompressed, false);
 		rtabmap::CompressionThread ctUserData(_userDataCompressed, false);
-		if(imageRaw && imageRaw->empty())
+		rtabmap::CompressionThread ctGroundCells(_groundCellsCompressed, false);
+		rtabmap::CompressionThread ctObstacleCells(_obstacleCellsCompressed, false);
+		if(imageRaw && imageRaw->empty() && !_imageCompressed.empty())
 		{
+			UASSERT(_imageCompressed.type() == CV_8UC1);
 			ctImage.start();
 		}
-		if(depthRaw && depthRaw->empty())
+		if(depthRaw && depthRaw->empty() && !_depthOrRightCompressed.empty())
 		{
+			UASSERT(_depthOrRightCompressed.type() == CV_8UC1);
 			ctDepth.start();
 		}
-		if(laserScanRaw && laserScanRaw->empty())
+		if(laserScanRaw && laserScanRaw->empty() && !_laserScanCompressed.empty())
 		{
+			UASSERT(_laserScanCompressed.type() == CV_8UC1);
 			ctLaserScan.start();
 		}
-		if(userDataRaw && userDataRaw->empty())
+		if(userDataRaw && userDataRaw->empty() && !_userDataCompressed.empty())
 		{
+			UASSERT(_userDataCompressed.type() == CV_8UC1);
 			ctUserData.start();
 		}
+		if(groundCellsRaw && groundCellsRaw->empty() && !_groundCellsCompressed.empty())
+		{
+			UASSERT(_groundCellsCompressed.type() == CV_8UC1);
+			ctGroundCells.start();
+		}
+		if(obstacleCellsRaw && obstacleCellsRaw->empty() && !_obstacleCellsCompressed.empty())
+		{
+			UASSERT(_obstacleCellsCompressed.type() == CV_8UC1);
+			ctObstacleCells.start();
+		}
 		ctImage.join();
 		ctDepth.join();
 		ctLaserScan.join();
 		ctUserData.join();
+		ctGroundCells.join();
+		ctObstacleCells.join();
+
 		if(imageRaw && imageRaw->empty())
 		{
 			*imageRaw = ctImage.getUncompressedData();
@@ -603,6 +716,14 @@ void SensorData::uncompressDataConst(cv::Mat * imageRaw, cv::Mat * depthRaw, cv:
 				UWARN("Requested user data, but the sensor data (%d) doesn't have user data.", this->id());
 			}
 		}
+		if(groundCellsRaw && groundCellsRaw->empty())
+		{
+			*groundCellsRaw = ctGroundCells.getUncompressedData();
+		}
+		if(obstacleCellsRaw && obstacleCellsRaw->empty())
+		{
+			*obstacleCellsRaw = ctObstacleCells.getUncompressedData();
+		}
 	}
 }

@@ -615,7 +736,11 @@ long SensorData::getMemoryUsed() const // Return memory usage in Bytes
 			_userDataCompressed.total()*_userDataCompressed.elemSize() +
 			_userDataRaw.total()*_userDataRaw.elemSize() +
 			_laserScanCompressed.total()*_laserScanCompressed.elemSize() +
-			_laserScanRaw.total()*_laserScanRaw.elemSize();
+			_laserScanRaw.total()*_laserScanRaw.elemSize() +
+			_groundCellsCompressed.total()*_groundCellsCompressed.elemSize() +
+			_groundCellsRaw.total()*_groundCellsRaw.elemSize() +
+			_obstacleCellsCompressed.total()*_obstacleCellsCompressed.elemSize() +
+			_obstacleCellsRaw.total()*_obstacleCellsRaw.elemSize();
 }

 } // namespace rtabmap