Improved occupancy grid map construction performance

corelib/include/rtabmap/core/Memory.h

@@ -114,6 +114,7 @@ public:
 	int getMapId(int signatureId) const;
 	cv::Mat getImageCompressed(int signatureId) const;
 	Signature getSignatureData(int locationId, bool uncompressedData = false);
+	Signature getSignatureDataConst(int locationId) const;
 	std::set<int> getAllSignatureIds() const;
 	bool memoryChanged() const {return _memoryChanged;}
 	bool isIncremental() const {return _incrementalMemory;}

corelib/include/rtabmap/core/Rtabmap.h

@@ -88,6 +88,7 @@ public:
 	bool isIDsGenerated() const;
 	const Statistics & getStatistics() const;
 	//bool getMetricData(int locationId, cv::Mat & rgb, cv::Mat & depth, float & depthConstant, Transform & pose, Transform & localTransform) const;
+	const std::map<int, Transform> & getLocalOptimizedPoses() const {return _optimizedPoses;}
 	Transform getPose(int locationId) const;
 	Transform getMapCorrection() const {return _mapCorrection;}
 	const Memory * getMemory() const {return _memory;}

corelib/include/rtabmap/core/impl/util3d.hpp

@@ -431,6 +431,72 @@ pcl::IndicesPtr extractNegativeIndices(
 	return output;
 }
 
+template<typename PointT>
+void occupancy2DFromCloud3D(
+		const typename pcl::PointCloud<PointT>::Ptr & cloud,
+		cv::Mat & ground,
+		cv::Mat & obstacles,
+		float cellSize,
+		float groundNormalAngle,
+		int minClusterSize)
+{
+	if(cloud->size() == 0)
+	{
+		return;
+	}
+	pcl::IndicesPtr groundIndices, obstaclesIndices;
+
+	segmentObstaclesFromGround<PointT>(cloud,
+			groundIndices,
+			obstaclesIndices,
+			cellSize,
+			groundNormalAngle,
+			minClusterSize);
+
+	pcl::PointCloud<pcl::PointXYZ>::Ptr groundCloud(new pcl::PointCloud<pcl::PointXYZ>);
+	pcl::PointCloud<pcl::PointXYZ>::Ptr obstaclesCloud(new pcl::PointCloud<pcl::PointXYZ>);
+
+	if(groundIndices->size())
+	{
+		pcl::copyPointCloud(*cloud, *groundIndices, *groundCloud);
+		//project on XY plane
+		util3d::projectCloudOnXYPlane<pcl::PointXYZ>(groundCloud);
+		//voxelize to grid cell size
+		groundCloud = util3d::voxelize<pcl::PointXYZ>(groundCloud, cellSize);
+	}
+
+	if(obstaclesIndices->size())
+	{
+		pcl::copyPointCloud(*cloud, *obstaclesIndices, *obstaclesCloud);
+		//project on XY plane
+		util3d::projectCloudOnXYPlane<pcl::PointXYZ>(obstaclesCloud);
+		//voxelize to grid cell size
+		obstaclesCloud = util3d::voxelize<pcl::PointXYZ>(obstaclesCloud, cellSize);
+	}
+
+	ground = cv::Mat();
+	if(groundCloud->size())
+	{
+		ground = cv::Mat(groundCloud->size(), 1, CV_32FC2);
+		for(unsigned int i=0;i<groundCloud->size(); ++i)
+		{
+			ground.at<cv::Vec2f>(i)[0] = groundCloud->at(i).x;
+			ground.at<cv::Vec2f>(i)[1] = groundCloud->at(i).y;
+		}
+	}
+
+	obstacles = cv::Mat();
+	if(obstaclesCloud->size())
+	{
+		obstacles = cv::Mat(obstaclesCloud->size(), 1, CV_32FC2);
+		for(unsigned int i=0;i<obstaclesCloud->size(); ++i)
+		{
+			obstacles.at<cv::Vec2f>(i)[0] = obstaclesCloud->at(i).x;
+			obstacles.at<cv::Vec2f>(i)[1] = obstaclesCloud->at(i).y;
+		}
+	}
+}
+
 } // util3d
 } // rtabmap
 #endif //UTIL3D_HPP_

corelib/include/rtabmap/core/util3d.h

@@ -346,13 +346,11 @@ pcl::PolygonMesh::Ptr RTABMAP_EXP createMesh(
 		float gp3MaximumAngle = 2*M_PI/3,
 		bool gp3NormalConsistency = false);
 
-bool RTABMAP_EXP occupancy2DFromCloud3D(
-		const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
+void occupancy2DFromLaserScan(
+		const cv::Mat & scan,
 		cv::Mat & ground,
 		cv::Mat & obstacles,
-		float cellSize = 0.05f,
-		float groundNormalAngle = M_PI_4,
-		int minClusterSize = 20);
+		float cellSize);
 
 cv::Mat RTABMAP_EXP create2DMapFromOccupancyLocalMaps(
 		const std::map<int, Transform> & poses,
@@ -360,7 +358,6 @@ cv::Mat RTABMAP_EXP create2DMapFromOccupancyLocalMaps(
 		float cellSize,
 		float & xMin,
 		float & yMin,
-		int fillEmptyRadius = 0,
 		float minMapSize = 0.0f);
 
 cv::Mat RTABMAP_EXP create2DMap(const std::map<int, Transform> & poses,
@@ -557,6 +554,15 @@ pcl::IndicesPtr extractNegativeIndices(
 		const typename pcl::PointCloud<PointT>::Ptr & cloud,
 		const pcl::IndicesPtr & indices);
 
+template<typename PointT>
+void occupancy2DFromCloud3D(
+		const typename pcl::PointCloud<PointT>::Ptr & cloud,
+		cv::Mat & ground,
+		cv::Mat & obstacles,
+		float cellSize = 0.05f,
+		float groundNormalAngle = M_PI_4,
+		int minClusterSize = 20);
+
 } // namespace util3d
 } // namespace rtabmap