Added Occupancy class. Database SQL: added ground_cells, obstacle_cells and cell_size fields to Node table (updated Signature too).

corelib/include/rtabmap/core/Memory.h

@@ -55,6 +55,7 @@ class Registration;
 class RegistrationInfo;
 class RegistrationIcp;
 class Stereo;
+class Occupancy;
 
 class RTABMAP_EXP Memory
 {
@@ -252,6 +253,7 @@ private:
 	float _rehearsalMaxAngle;
 	bool _rehearsalWeightIgnoredWhileMoving;
 	bool _useOdometryFeatures;
+	bool _createOccupancyGrid;
 
 	int _idCount;
 	int _idMapCount;
@@ -274,6 +276,8 @@ private:
 
 	Registration * _registrationPipeline;
 	RegistrationIcp * _registrationIcp;
+
+	Occupancy * _occupancy;
 };
 
 } // namespace rtabmap

corelib/include/rtabmap/core/Occupancy.h

@@ -0,0 +1,68 @@
+/*
+Copyright (c) 2010-2016, 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 CORELIB_SRC_OCCUPANCY_H_
+#define CORELIB_SRC_OCCUPANCY_H_
+
+#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
+
+#include <rtabmap/core/Parameters.h>
+#include <rtabmap/core/Signature.h>
+
+namespace rtabmap {
+
+class RTABMAP_EXP Occupancy
+{
+public:
+	Occupancy(const ParametersMap & parameters = ParametersMap());
+	void parseParameters(const ParametersMap & parameters);
+	float getCellSize() const {return cellSize_;}
+	void segment(const Signature & node, cv::Mat & obstacles, cv::Mat & ground);
+
+private:
+	ParametersMap parameters_;
+	int cloudDecimation_;
+	float cloudMaxDepth_;
+	float cloudMinDepth_;
+	float cellSize_;
+	bool occupancyFromCloud_;
+	bool projMapFrame_;
+	float maxObstacleHeight_;
+	float maxGroundAngle_;
+	int minClusterSize_;
+	bool flatObstaclesDetected_;
+	float maxGroundHeight_;
+	bool grid3D_;
+	bool groundIsObstacle_;
+	float noiseFilteringRadius_;
+	int noiseFilteringMinNeighbors_;
+
+};
+
+}
+
+#endif /* CORELIB_SRC_OCCUPANCY_H_ */

corelib/include/rtabmap/core/Parameters.h

@@ -209,7 +209,8 @@ class RTABMAP_EXP Parameters
 	RTABMAP_PARAM(Mem, ImagePreDecimation,      int, 1,          "Image decimation (>=1) before features extraction.");
 	RTABMAP_PARAM(Mem, ImagePostDecimation,     int, 1,          "Image decimation (>=1) of saved data in created signatures (after features extraction). Decimation is done from the original image.");
 	RTABMAP_PARAM(Mem, LaserScanDownsampleStepSize, int, 1,      "If > 1, downsample the laser scans when creating a signature.");
-	RTABMAP_PARAM(Mem, UseOdomFeatures,             bool, false, "Use odometry features.");
+	RTABMAP_PARAM(Mem, UseOdomFeatures,         bool, false, "Use odometry features.");
+	RTABMAP_PARAM(Mem, CreateOccupancyGrid,     bool, true,  "Create local occupancy grid maps. See \"Grid\" group for parameters.");
 
 	// KeypointMemory (Keypoint-based)
 	RTABMAP_PARAM(Kp, NNStrategy,            int, 1,            "kNNFlannNaive=0, kNNFlannKdTree=1, kNNFlannLSH=2, kNNBruteForce=3, kNNBruteForceGPU=4");
@@ -458,6 +459,24 @@ class RTABMAP_EXP Parameters
 	RTABMAP_PARAM(StereoBM, SpeckleWindowSize,   int, 100,      "See cv::StereoBM");
 	RTABMAP_PARAM(StereoBM, SpeckleRange,        int, 4,        "See cv::StereoBM");
 
+	// Occupancy Grid
+	RTABMAP_PARAM(Grid, FromDepth,               bool,   false,   "Create occupancy grid from depth image(s), otherwise it is created from laser scan.");
+	RTABMAP_PARAM(Grid, DepthDecimation,         int,    1,       "[Grid/FromDepth=true]");
+	RTABMAP_PARAM(Grid, DepthMin,                float,  0.0,     "[Grid/FromDepth=true]");
+	RTABMAP_PARAM(Grid, DepthMax,                float,  0.0,     "[Grid/FromDepth=true]");
+	RTABMAP_PARAM_STR(Grid, DepthRoiRatios,      "0.0 0.0 0.0 0.0", "Region of interest ratios [left, right, top, bottom].");
+	RTABMAP_PARAM(Grid, CellSize,                float,  0.05,    "");
+	RTABMAP_PARAM(Grid, MapFrameProjection,      bool,   false,   "");
+	RTABMAP_PARAM(Grid, MaxObstacleHeight,       float,  0.0,     "");
+	RTABMAP_PARAM(Grid, MaxGroundHeight,         float,  0.0,     "");
+	RTABMAP_PARAM(Grid, MaxGroundAngle,          float,  0.78,    "");
+	RTABMAP_PARAM(Grid, MinClusterSize,          int,    10,      "");
+	RTABMAP_PARAM(Grid, FlatObstacleDetected,    bool,   false,   "");
+	RTABMAP_PARAM(Grid, 3D,                      bool,   false,   "Ignored if laser scan is 2D.");
+	RTABMAP_PARAM(Grid, 3DGroundIsObstacle,      bool,   false,   "[Grid/3D=true] The ground is considered as an obstacle.");
+	RTABMAP_PARAM(Grid, NoiseFilteringRadius,         float,   0.0, "0 means disabled.");
+	RTABMAP_PARAM(Grid, NoiseFilteringMinNeighbors,   int,     5,   "");
+
 public:
 	virtual ~Parameters();
 

corelib/include/rtabmap/core/Signature.h

@@ -115,11 +115,22 @@ public:
 	void setPose(const Transform & pose) {_pose = pose;}
 	void setGroundTruthPose(const Transform & pose) {_groundTruthPose = pose;}
 
+	void setOccupancyGrid(const cv::Mat & ground, const cv::Mat & obstacles, float cellSize)
+	{
+		_groundCells = ground.clone();
+		_obstacleCells = obstacles.clone();
+		_cellSize = cellSize;
+	}
+
 	const std::multimap<int, cv::Point3f> & getWords3() const {return _words3;}
 	const Transform & getPose() const {return _pose;}
 	cv::Mat getPoseCovariance() const;
 	const Transform & getGroundTruthPose() const {return _groundTruthPose;}
 
+	const cv::Mat & getGroundCells() const {return _groundCells;}
+	const cv::Mat & getObstacleCells() const {return _obstacleCells;}
+	const float getCellSize() const {return _cellSize;}
+
 	SensorData & sensorData() {return _sensorData;}
 	const SensorData & sensorData() const {return _sensorData;}
 
@@ -146,6 +157,10 @@ private:
 	Transform _pose;
 	Transform _groundTruthPose;
 
+	cv::Mat _groundCells;
+	cv::Mat _obstacleCells;
+	float _cellSize;
+
 	SensorData _sensorData;
 };
 

corelib/include/rtabmap/core/Statistics.h

@@ -126,6 +126,9 @@ class RTABMAP_EXP Statistics
 	RTABMAP_STATS(TimingMem, Joining_dictionary_update, ms);
 	RTABMAP_STATS(TimingMem, Add_new_words, ms);
 	RTABMAP_STATS(TimingMem, Compressing_data, ms);
+	RTABMAP_STATS(TimingMem, Post_decimation, ms);
+	RTABMAP_STATS(TimingMem, Downsampling_scan, ms);
+	RTABMAP_STATS(TimingMem, Occupancy_grid, ms);
 
 	RTABMAP_STATS(Keypoint, Dictionary_size, words);
 	RTABMAP_STATS(Keypoint, Indexed_words, words);

corelib/include/rtabmap/core/util3d_transforms.h

@@ -32,6 +32,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 #include <pcl/point_cloud.h>
 #include <pcl/point_types.h>
+#include <pcl/pcl_base.h>
 #include <rtabmap/core/Transform.h>
 
 namespace rtabmap
@@ -44,6 +45,9 @@ cv::Mat RTABMAP_EXP transformLaserScan(
 		const cv::Mat & laserScan,
 		const Transform & transform);
 
+pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP transformPointCloud(
+		const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
+		const Transform & transform);
 pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP transformPointCloud(
 		const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
 		const Transform & transform);
@@ -57,6 +61,27 @@ pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr RTABMAP_EXP transformPointCloud(
 		const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud,
 		const Transform & transform);
 
+pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP transformPointCloud(
+		const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
+		const pcl::IndicesPtr & indices,
+		const Transform & transform);
+pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP transformPointCloud(
+		const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
+		const pcl::IndicesPtr & indices,
+		const Transform & transform);
+pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_EXP transformPointCloud(
+		const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
+		const pcl::IndicesPtr & indices,
+		const Transform & transform);
+pcl::PointCloud<pcl::PointNormal>::Ptr RTABMAP_EXP transformPointCloud(
+		const pcl::PointCloud<pcl::PointNormal>::Ptr & cloud,
+		const pcl::IndicesPtr & indices,
+		const Transform & transform);
+pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr RTABMAP_EXP transformPointCloud(
+		const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud,
+		const pcl::IndicesPtr & indices,
+		const Transform & transform);
+
 cv::Point3f RTABMAP_EXP transformPoint(
 		const cv::Point3f & pt,
 		const Transform & transform);