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

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)