Added OctoMap class

corelib/include/rtabmap/core/OctoMap.h

@@ -0,0 +1,92 @@
+/*
+Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#ifndef SRC_OCTOMAP_H_
+#define SRC_OCTOMAP_H_
+
+#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
+
+#include <octomap/ColorOcTree.h>
+#include <octomap/OcTreeKey.h>
+
+#include <pcl/pcl_base.h>
+#include <pcl/point_types.h>
+
+#include <rtabmap/core/Transform.h>
+
+#include <map>
+
+namespace rtabmap {
+
+class OcTreeNodeInfo
+{
+public:
+	OcTreeNodeInfo(int nodeRefId, const octomap::OcTreeKey & key, bool isObstacle) :
+		nodeRefId_(nodeRefId),
+		key_(key),
+		isObstacle_(isObstacle) {}
+	int nodeRefId_;
+	octomap::OcTreeKey key_;
+	bool isObstacle_;
+};
+
+class RTABMAP_EXP OctoMap {
+public:
+	OctoMap(float voxelSize = 0.1f);
+
+	const std::map<int, Transform> & addedNodes() const {return addedNodes_;}
+	void addToCache(int nodeId,
+			pcl::PointCloud<pcl::PointXYZRGB>::Ptr & ground,
+			pcl::PointCloud<pcl::PointXYZRGB>::Ptr & obstacles);
+	void update(const std::map<int, Transform> & poses);
+
+	const octomap::ColorOcTree * octree() const {return octree_;}
+
+	pcl::PointCloud<pcl::PointXYZRGB>::Ptr createCloud(
+			std::vector<int> * obstacleIndices = 0,
+			std::vector<int> * groundIndices = 0) const;
+
+	cv::Mat createProjectionMap(
+			float & xMin,
+			float & yMin,
+			float & gridCellSize,
+			float minGridSize);
+
+	virtual ~OctoMap();
+	void clear();
+
+private:
+	std::map<int, std::pair<pcl::PointCloud<pcl::PointXYZRGB>::Ptr, pcl::PointCloud<pcl::PointXYZRGB>::Ptr> > cache_;
+	octomap::ColorOcTree * octree_;
+	std::map<octomap::ColorOcTreeNode*, OcTreeNodeInfo> occupiedCells_;
+	std::map<int, Transform> addedNodes_;
+	octomap::KeyRay keyRay_;
+};
+
+} /* namespace rtabmap */
+
+#endif /* SRC_OCTOMAP_H_ */

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

@@ -17,6 +17,19 @@
 namespace rtabmap{
 namespace util3d{
 
+template<typename PointT>
+typename pcl::PointCloud<PointT>::Ptr projectCloudOnXYPlane(
+		const typename pcl::PointCloud<PointT> & cloud)
+{
+	typename pcl::PointCloud<PointT>::Ptr output(new pcl::PointCloud<PointT>);
+	*output = cloud;
+	for(unsigned int i=0; i<output->size(); ++i)
+	{
+		output->at(i).z = 0;
+	}
+	return output;
+}
+
 template<typename PointT>
 void segmentObstaclesFromGround(
 		const typename pcl::PointCloud<PointT>::Ptr & cloud,
@@ -159,6 +172,79 @@ void segmentObstaclesFromGround(
 			flatObstacles);
 }
 
+template<typename PointT>
+void occupancy2DFromGroundObstacles(
+		const typename pcl::PointCloud<PointT>::Ptr & cloud,
+		const pcl::IndicesPtr & groundIndices,
+		const pcl::IndicesPtr & obstaclesIndices,
+		cv::Mat & ground,
+		cv::Mat & obstacles,
+		float cellSize)
+{
+	typename pcl::PointCloud<PointT>::Ptr groundCloud(new pcl::PointCloud<PointT>);
+	typename pcl::PointCloud<PointT>::Ptr obstaclesCloud(new pcl::PointCloud<PointT>);
+
+	if(groundIndices->size())
+	{
+		pcl::copyPointCloud(*cloud, *groundIndices, *groundCloud);
+	}
+
+	if(obstaclesIndices->size())
+	{
+		pcl::copyPointCloud(*cloud, *obstaclesIndices, *obstaclesCloud);
+	}
+
+	occupancy2DFromGroundObstacles<PointT>(
+			groundCloud,
+			obstaclesCloud,
+			ground,
+			obstacles,
+			cellSize);
+}
+
+template<typename PointT>
+void occupancy2DFromGroundObstacles(
+		const typename pcl::PointCloud<PointT>::Ptr & groundCloud,
+		const typename pcl::PointCloud<PointT>::Ptr & obstaclesCloud,
+		cv::Mat & ground,
+		cv::Mat & obstacles,
+		float cellSize)
+{
+	ground = cv::Mat();
+	if(groundCloud->size())
+	{
+		//project on XY plane
+		typename pcl::PointCloud<PointT>::Ptr groundCloudProjected;
+		groundCloudProjected = util3d::projectCloudOnXYPlane(*groundCloud);
+		//voxelize to grid cell size
+		groundCloudProjected = util3d::voxelize(groundCloudProjected, cellSize);
+
+		ground = cv::Mat((int)groundCloudProjected->size(), 1, CV_32FC2);
+		for(unsigned int i=0;i<groundCloudProjected->size(); ++i)
+		{
+			ground.at<cv::Vec2f>(i)[0] = groundCloudProjected->at(i).x;
+			ground.at<cv::Vec2f>(i)[1] = groundCloudProjected->at(i).y;
+		}
+	}
+
+	obstacles = cv::Mat();
+	if(obstaclesCloud->size())
+	{
+		//project on XY plane
+		typename pcl::PointCloud<PointT>::Ptr obstaclesCloudProjected;
+		obstaclesCloudProjected = util3d::projectCloudOnXYPlane(*obstaclesCloud);
+		//voxelize to grid cell size
+		obstaclesCloudProjected = util3d::voxelize(obstaclesCloudProjected, cellSize);
+
+		obstacles = cv::Mat((int)obstaclesCloudProjected->size(), 1, CV_32FC2);
+		for(unsigned int i=0;i<obstaclesCloudProjected->size(); ++i)
+		{
+			obstacles.at<cv::Vec2f>(i)[0] = obstaclesCloudProjected->at(i).x;
+			obstacles.at<cv::Vec2f>(i)[1] = obstaclesCloudProjected->at(i).y;
+		}
+	}
+}
+
 template<typename PointT>
 void occupancy2DFromCloud3D(
 		const typename pcl::PointCloud<PointT>::Ptr & cloud,
@@ -189,48 +275,13 @@ void occupancy2DFromCloud3D(
 			segmentFlatObstacles,
 			maxGroundHeight);
 
-	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(groundCloud);
-		//voxelize to grid cell size
-		groundCloud = util3d::voxelize(groundCloud, cellSize);
-	}
-
-	if(obstaclesIndices->size())
-	{
-		pcl::copyPointCloud(*cloud, *obstaclesIndices, *obstaclesCloud);
-		//project on XY plane
-		util3d::projectCloudOnXYPlane(obstaclesCloud);
-		//voxelize to grid cell size
-		obstaclesCloud = util3d::voxelize(obstaclesCloud, cellSize);
-	}
-
-	ground = cv::Mat();
-	if(groundCloud->size())
-	{
-		ground = cv::Mat((int)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((int)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;
-		}
-	}
+	occupancy2DFromGroundObstacles<PointT>(
+			cloud,
+			groundIndices,
+			obstaclesIndices,
+			ground,
+			obstacles,
+			cellSize);
 }
 
 template<typename PointT>

corelib/include/rtabmap/core/util3d_mapping.h

@@ -76,8 +76,9 @@ void RTABMAP_EXP rayTrace(const cv::Point2i & start,
 
 cv::Mat RTABMAP_EXP convertMap2Image8U(const cv::Mat & map8S);
 
-void RTABMAP_EXP projectCloudOnXYPlane(
-		pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud);
+template<typename PointT>
+typename pcl::PointCloud<PointT>::Ptr projectCloudOnXYPlane(
+		const typename pcl::PointCloud<PointT> & cloud);
 
 // templated methods
 template<typename PointT>
@@ -106,6 +107,23 @@ void segmentObstaclesFromGround(
 		float maxGroundHeight = 0.0f,
 		pcl::IndicesPtr * flatObstacles = 0);
 
+template<typename PointT>
+void occupancy2DFromGroundObstacles(
+		const typename pcl::PointCloud<PointT>::Ptr & cloud,
+		const pcl::IndicesPtr & groundIndices,
+		const pcl::IndicesPtr & obstaclesIndices,
+		cv::Mat & ground,
+		cv::Mat & obstacles,
+		float cellSize);
+
+template<typename PointT>
+void occupancy2DFromGroundObstacles(
+		const typename pcl::PointCloud<PointT>::Ptr & groundCloud,
+		const typename pcl::PointCloud<PointT>::Ptr & obstaclesCloud,
+		cv::Mat & ground,
+		cv::Mat & obstacles,
+		float cellSize);
+
 template<typename PointT>
 void occupancy2DFromCloud3D(
 		const typename pcl::PointCloud<PointT>::Ptr & cloud,