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

corelib/src/CMakeLists.txt

@@ -64,6 +64,8 @@ SET(SRC_FILES
 	Stereo.cpp
 	StereoDense.cpp
 	StereoCameraModel.cpp
+	
+	Occupancy.cpp
 		
 	rtflann/ext/lz4.c
 	rtflann/ext/lz4hc.c

corelib/src/Memory.cpp

@@ -57,6 +57,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "rtabmap/core/Compression.h"
 #include "rtabmap/core/Graph.h"
 #include "rtabmap/core/Stereo.h"
+#include "rtabmap/core/Occupancy.h"
 
 #include <pcl/io/pcd_io.h>
 #include <pcl/common/common.h>
@@ -91,6 +92,7 @@ Memory::Memory(const ParametersMap & parameters) :
 	_rehearsalMaxAngle(Parameters::defaultRGBDAngularUpdate()),
 	_rehearsalWeightIgnoredWhileMoving(Parameters::defaultMemRehearsalWeightIgnoredWhileMoving()),
 	_useOdometryFeatures(Parameters::defaultMemUseOdomFeatures()),
+	_createOccupancyGrid(Parameters::defaultMemCreateOccupancyGrid()),
 	_idCount(kIdStart),
 	_idMapCount(kIdStart),
 	_lastSignature(0),
@@ -107,6 +109,7 @@ Memory::Memory(const ParametersMap & parameters) :
 	_vwd = new VWDictionary(parameters);
 	_registrationPipeline = Registration::create(parameters);
 	_registrationIcp = new RegistrationIcp(parameters);
+	_occupancy = new Occupancy(parameters);
 	this->parseParameters(parameters);
 }
 
@@ -376,6 +379,10 @@ Memory::~Memory()
 	{
 		delete _registrationIcp;
 	}
+	if(_occupancy)
+	{
+		delete _occupancy;
+	}
 }
 
 void Memory::parseParameters(const ParametersMap & parameters)
@@ -406,6 +413,7 @@ void Memory::parseParameters(const ParametersMap & parameters)
 	Parameters::parse(parameters, Parameters::kRGBDAngularUpdate(), _rehearsalMaxAngle);
 	Parameters::parse(parameters, Parameters::kMemRehearsalWeightIgnoredWhileMoving(), _rehearsalWeightIgnoredWhileMoving);
 	Parameters::parse(parameters, Parameters::kMemUseOdomFeatures(), _useOdometryFeatures);
+	Parameters::parse(parameters, Parameters::kMemCreateOccupancyGrid(), _createOccupancyGrid);
 
 	UASSERT_MSG(_maxStMemSize >= 0, uFormat("value=%d", _maxStMemSize).c_str());
 	UASSERT_MSG(_similarityThreshold >= 0.0f && _similarityThreshold <= 1.0f, uFormat("value=%f", _similarityThreshold).c_str());
@@ -479,6 +487,11 @@ void Memory::parseParameters(const ParametersMap & parameters)
 		_registrationIcp->parseParameters(parameters);
 	}
 
+	if(_occupancy)
+	{
+		_occupancy->parseParameters(parameters);
+	}
+
 	// do this after all parameters are parsed
 	// SLAM mode vs Localization mode
 	iter = parameters.find(Parameters::kMemIncrementalMemory());
@@ -3481,6 +3494,10 @@ Signature * Memory::createSignature(const SensorData & data, const Transform & p
 		{
 			stereoCameraModel.scale(1.0/double(_imagePostDecimation));
 		}
+
+		t = timer.ticks();
+		if(stats) stats->addStatistic(Statistics::kTimingMemPost_decimation(), t*1000.0f);
+		UDEBUG("time post-decimation = %fs", t);
 	}
 
 	// downsampling the laser scan?
@@ -3490,6 +3507,10 @@ Signature * Memory::createSignature(const SensorData & data, const Transform & p
 	{
 		laserScan = util3d::downsample(laserScan, _laserScanDownsampleStepSize);
 		maxLaserScanMaxPts /= _laserScanDownsampleStepSize;
+
+		t = timer.ticks();
+		if(stats) stats->addStatistic(Statistics::kTimingMemDownsampling_scan(), t*1000.0f);
+		UDEBUG("time downsampling scan = %fs", t);
 	}
 
 	Signature * s;
@@ -3611,6 +3632,21 @@ Signature * Memory::createSignature(const SensorData & data, const Transform & p
 	{
 		s->setEnabled(true); // All references are already activated in the dictionary at this point (see _vwd->addNewWords())
 	}
+
+	// Occupancy grid map stuff
+	/*cv::Mat ground, obstacles;
+	float cellSize = 0.0f;
+	if(_createOccupancyGrid)
+	{
+		_occupancy->segment(s->sensorData(), ground, obstacles);
+		cellSize = _occupancy->getCellSize();
+
+		t = timer.ticks();
+		if(stats) stats->addStatistic(Statistics::kTimingMemOccupancy_grid(), t*1000.0f);
+		UDEBUG("time grid map (%d) = %fs", t);
+	}
+	s->setOccupancyGrid(ground, obstacles, cellSize);
+	*/
 	return s;
 }
 

corelib/src/Occupancy.cpp

@@ -0,0 +1,201 @@
+/*
+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.
+*/
+
+#include <rtabmap/core/Occupancy.h>
+#include <rtabmap/core/util3d.h>
+#include <rtabmap/core/util3d_mapping.h>
+#include <rtabmap/core/util3d_transforms.h>
+#include <rtabmap/utilite/ULogger.h>
+
+namespace rtabmap {
+
+Occupancy::Occupancy(const ParametersMap & parameters) :
+	parameters_(parameters),
+	cloudDecimation_(Parameters::defaultGridDepthDecimation()),
+	cloudMaxDepth_(Parameters::defaultGridDepthMax()),
+	cloudMinDepth_(Parameters::defaultGridDepthMin()),
+	cellSize_(Parameters::defaultGridCellSize()),
+	occupancyFromCloud_(Parameters::defaultGridFromDepth()),
+	projMapFrame_(Parameters::defaultGridMapFrameProjection()),
+	maxObstacleHeight_(Parameters::defaultGridMaxObstacleHeight()),
+	maxGroundAngle_(Parameters::defaultGridMaxGroundAngle()),
+	minClusterSize_(Parameters::defaultGridMinClusterSize()),
+	flatObstaclesDetected_(Parameters::defaultGridFlatObstacleDetected()),
+	maxGroundHeight_(Parameters::defaultGridMaxGroundHeight()),
+	grid3D_(Parameters::defaultGrid3D()),
+	groundIsObstacle_(Parameters::defaultGrid3DGroundIsObstacle()),
+	noiseFilteringRadius_(Parameters::defaultGridNoiseFilteringRadius()),
+	noiseFilteringMinNeighbors_(Parameters::defaultGridNoiseFilteringMinNeighbors())
+{
+	this->parseParameters(parameters);
+}
+
+void Occupancy::parseParameters(const ParametersMap & parameters)
+{
+	Parameters::parse(parameters, Parameters::kGridFromDepth(), occupancyFromCloud_);
+	Parameters::parse(parameters, Parameters::kGridDepthDecimation(), cloudDecimation_);
+	Parameters::parse(parameters, Parameters::kGridDepthMin(), cloudMinDepth_);
+	Parameters::parse(parameters, Parameters::kGridDepthMax(), cloudMaxDepth_);
+	Parameters::parse(parameters, Parameters::kGridCellSize(), cellSize_);
+	Parameters::parse(parameters, Parameters::kGridMapFrameProjection(), projMapFrame_);
+	Parameters::parse(parameters, Parameters::kGridMaxObstacleHeight(), maxObstacleHeight_);
+	Parameters::parse(parameters, Parameters::kGridMaxGroundHeight(), maxGroundHeight_);
+	Parameters::parse(parameters, Parameters::kGridMaxGroundAngle(), maxGroundAngle_);
+	Parameters::parse(parameters, Parameters::kGridMinClusterSize(), minClusterSize_);
+	Parameters::parse(parameters, Parameters::kGridFlatObstacleDetected(), flatObstaclesDetected_);
+	Parameters::parse(parameters, Parameters::kGrid3D(), grid3D_);
+	Parameters::parse(parameters, Parameters::kGrid3DGroundIsObstacle(), groundIsObstacle_);
+	Parameters::parse(parameters, Parameters::kGridNoiseFilteringRadius(), noiseFilteringRadius_);
+	Parameters::parse(parameters, Parameters::kGridNoiseFilteringMinNeighbors(), noiseFilteringMinNeighbors_);
+}
+
+void Occupancy::segment(const Signature & node, cv::Mat & obstacles, cv::Mat & ground)
+{
+	if(!occupancyFromCloud_ && node.sensorData().laserScanRaw().channels() == 2)
+	{
+		//2D
+		util3d::occupancy2DFromLaserScan(
+				node.sensorData().laserScanRaw(),
+				ground,
+				obstacles,
+				cellSize_);
+	}
+	else
+	{
+		// 3D
+		pcl::IndicesPtr indices(new std::vector<int>);
+		pcl::PointCloud<pcl::PointXYZ>::Ptr cloud;
+		if(!occupancyFromCloud_)
+		{
+			cloud =util3d::laserScanToPointCloud(node.sensorData().laserScanRaw());
+		}
+		else
+		{
+			cloud = util3d::cloudFromSensorData(
+					node.sensorData(),
+					cloudDecimation_,
+					cloudMaxDepth_,
+					cloudMinDepth_,
+					indices.get(),
+					parameters_);
+		}
+
+		if(cloud->size())
+		{
+			// voxelize to grid cell size
+			cloud = util3d::voxelize(cloud, indices, cellSize_);
+			indices->clear();
+
+			// Do radius filtering after voxel filtering ( a lot faster)
+			if(noiseFilteringRadius_ > 0.0 &&
+			   noiseFilteringMinNeighbors_ > 0)
+			{
+				indices = rtabmap::util3d::radiusFiltering(
+						cloud,
+						noiseFilteringRadius_,
+						noiseFilteringMinNeighbors_);
+
+				if(indices->empty())
+				{
+					UWARN("Cloud (with %d points) is empty after noise "
+							"filtering. Occupancy grid of node %d cannot be "
+							"created.",
+							(int)cloud->size(), node.id());
+					return;
+				}
+			}
+
+			// add pose rotation without yaw
+			float roll, pitch, yaw;
+			node.getPose().getEulerAngles(roll, pitch, yaw);
+			if(indices->size())
+			{
+				cloud = util3d::transformPointCloud(cloud, indices, Transform(0,0, projMapFrame_?node.getPose().z():0, roll, pitch, 0));
+			}
+			else
+			{
+				cloud = util3d::transformPointCloud(cloud, Transform(0,0, projMapFrame_?node.getPose().z():0, roll, pitch, 0));
+			}
+
+			if(maxObstacleHeight_ != 0.0f)
+			{
+				cloud = util3d::passThrough(cloud, "z", std::numeric_limits<int>::min(), maxObstacleHeight_);
+			}
+
+			pcl::IndicesPtr groundIndices, obstaclesIndices;
+			util3d::segmentObstaclesFromGround<pcl::PointXYZ>(
+					cloud,
+					groundIndices,
+					obstaclesIndices,
+					20,
+					maxGroundAngle_,
+					cellSize_*2.0f,
+					minClusterSize_,
+					flatObstaclesDetected_,
+					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);
+			}
+
+			if(obstaclesIndices->size())
+			{
+				pcl::copyPointCloud(*cloud, *obstaclesIndices, *obstaclesCloud);
+			}
+
+			if(grid3D_)
+			{
+				if(groundIsObstacle_)
+				{
+					*obstaclesCloud += *groundCloud;
+					groundCloud->clear();
+				}
+
+				// transform back in base frame
+				Transform tinv = Transform(0,0, projMapFrame_?node.getPose().z():0, roll, pitch, 0).inverse();
+				ground = util3d::laserScanFromPointCloud(*groundCloud, tinv);
+				obstacles = util3d::laserScanFromPointCloud(*obstaclesCloud, tinv);
+			}
+			else
+			{
+				// projection on the xy plane
+				util3d::occupancy2DFromGroundObstacles<pcl::PointXYZ>(
+						groundCloud,
+						obstaclesCloud,
+						ground,
+						obstacles,
+						cellSize_);
+			}
+		}
+	}
+}
+
+}

corelib/src/Signature.cpp

@@ -44,7 +44,8 @@ Signature::Signature() :
 	_saved(false),
 	_modified(true),
 	_linksModified(true),
-	_enabled(false)
+	_enabled(false),
+	_cellSize(0.0f)
 {
 }
 
@@ -68,6 +69,7 @@ Signature::Signature(
 	_enabled(false),
 	_pose(pose),
 	_groundTruthPose(groundTruthPose),
+	_cellSize(0.0f),
 	_sensorData(sensorData)
 {
 	if(_sensorData.id() == 0)
@@ -89,6 +91,7 @@ Signature::Signature(const SensorData & data) :
 	_enabled(false),
 	_pose(Transform::getIdentity()),
 	_groundTruthPose(data.groundTruth()),
+	_cellSize(0.0f),
 	_sensorData(data)
 {
 

corelib/src/resources/DatabaseSchema.sql.in

@@ -21,6 +21,9 @@ CREATE TABLE Node (
 	pose BLOB,
 	ground_truth_pose BLOB,
 	label TEXT,
+	obstacle_cells BLOB,
+	ground_cells BLOB,
+	cell_size FLOAT,
 	time_enter DATE,
 	PRIMARY KEY (id)
 );

corelib/src/util3d_transforms.cpp

@@ -121,6 +121,43 @@ pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr transformPointCloud(
 	return output;
 }
 
+pcl::PointCloud<pcl::PointXYZ>::Ptr transformPointCloud(
+		const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
+		const pcl::IndicesPtr & indices,
+		const Transform & transform)
+{
+	pcl::PointCloud<pcl::PointXYZ>::Ptr output(new pcl::PointCloud<pcl::PointXYZ>);
+	pcl::transformPointCloud(*cloud, *indices, *output, transform.toEigen4f());
+	return output;
+}
+pcl::PointCloud<pcl::PointXYZRGB>::Ptr transformPointCloud(
+		const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
+		const pcl::IndicesPtr & indices,
+		const Transform & transform)
+{
+	pcl::PointCloud<pcl::PointXYZRGB>::Ptr output(new pcl::PointCloud<pcl::PointXYZRGB>);
+	pcl::transformPointCloud(*cloud, *indices, *output, transform.toEigen4f());
+	return output;
+}
+pcl::PointCloud<pcl::PointNormal>::Ptr transformPointCloud(
+		const pcl::PointCloud<pcl::PointNormal>::Ptr & cloud,
+		const pcl::IndicesPtr & indices,
+		const Transform & transform)
+{
+	pcl::PointCloud<pcl::PointNormal>::Ptr output(new pcl::PointCloud<pcl::PointNormal>);
+	pcl::transformPointCloudWithNormals(*cloud, *indices, *output, transform.toEigen4f());
+	return output;
+}
+pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr transformPointCloud(
+		const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud,
+		const pcl::IndicesPtr & indices,
+		const Transform & transform)
+{
+	pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr output(new pcl::PointCloud<pcl::PointXYZRGBNormal>);
+	pcl::transformPointCloudWithNormals(*cloud, *indices, *output, transform.toEigen4f());
+	return output;
+}
+
 cv::Point3f transformPoint(
 		const cv::Point3f & point,
 		const Transform & transform)