0.11.10: Database update with occupancy grid and laser scan info. Added class LaserScanInfo and OccupancyGrid (incremental 2d grid map). Gui: 2d grid and octomap are udpated using occupancy grids saved in nodes.

CMakeLists.txt

@@ -21,7 +21,7 @@ SET(CMAKE_MODULE_PATH "${PROJECT_SOURCE_DIR}/cmake_modules")
 #######################
 SET(RTABMAP_MAJOR_VERSION 0)
 SET(RTABMAP_MINOR_VERSION 11)
-SET(RTABMAP_PATCH_VERSION 9)
+SET(RTABMAP_PATCH_VERSION 10)
 SET(RTABMAP_VERSION
   ${RTABMAP_MAJOR_VERSION}.${RTABMAP_MINOR_VERSION}.${RTABMAP_PATCH_VERSION})
   

corelib/include/rtabmap/core/CameraModel.h

@@ -115,6 +115,7 @@ public:
 	bool save(const std::string & directory) const;
 
 	CameraModel scaled(double scale) const;
+	CameraModel roi(const cv::Rect & roi) const;
 
 	double horizontalFOV() const; // in degrees
 	double verticalFOV() const;   // in degrees

corelib/include/rtabmap/core/DBDriver.h

@@ -118,8 +118,8 @@ public:
 	void loadWords(const std::set<int> & wordIds, std::list<VisualWord *> & vws);
 
 	// Specific queries...
-	void loadNodeData(std::list<Signature *> & signatures) const;
+	void loadNodeData(std::list<Signature *> & signatures, bool images = true, bool scan = true, bool userData = true, bool occupancyGrid = true) const;
-	void getNodeData(int signatureId, SensorData & data) const;
+	void getNodeData(int signatureId, SensorData & data, bool images = true, bool scan = true, bool userData = true, bool occupancyGrid = true) const;
 	bool getCalibration(int signatureId, std::vector<CameraModel> & models, StereoCameraModel & stereoModel) const;
 	bool getNodeInfo(int signatureId, Transform & pose, int & mapId, int & weight, std::string & label, double & stamp, Transform & groundTruthPose) const;
 	void loadLinks(int signatureId, std::map<int, Link> & links, Link::Type type = Link::kUndef) const;
@@ -171,7 +171,7 @@ private:
 	virtual void loadWordsQuery(const std::set<int> & wordIds, std::list<VisualWord *> & vws) const = 0;
 	virtual void loadLinksQuery(int signatureId, std::map<int, Link> & links, Link::Type type = Link::kUndef) const = 0;
 
-	virtual void loadNodeDataQuery(std::list<Signature *> & signatures) const = 0;
+	virtual void loadNodeDataQuery(std::list<Signature *> & signatures, bool images=true, bool scan=true, bool userData=true, bool occupancyGrid=true) const = 0;
 	virtual bool getCalibrationQuery(int signatureId, std::vector<CameraModel> & models, StereoCameraModel & stereoModel) const = 0;
 	virtual bool getNodeInfoQuery(int signatureId, Transform & pose, int & mapId, int & weight, std::string & label, double & stamp, Transform & groundTruthPose) const = 0;
 	virtual void getAllNodeIdsQuery(std::set<int> & ids, bool ignoreChildren, bool ignoreBadSignatures) const = 0;

corelib/include/rtabmap/core/LaserScanInfo.h

@@ -0,0 +1,65 @@
+/*
+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_INCLUDE_RTABMAP_CORE_LASERSCANINFO_H_
+#define CORELIB_INCLUDE_RTABMAP_CORE_LASERSCANINFO_H_
+
+#include <rtabmap/utilite/ULogger.h>
+
+namespace rtabmap {
+
+class LaserScanInfo
+{
+public:
+	LaserScanInfo() :
+		maxPoints_(0),
+		maxRange_(0),
+		localTransform_(Transform::getIdentity())
+	{
+	}
+
+	LaserScanInfo(int maxPoints, float maxRange, const Transform & localTransform = Transform::getIdentity()) :
+		maxPoints_(maxPoints),
+		maxRange_(maxRange),
+		localTransform_(localTransform)
+	{
+		UASSERT(!localTransform.isNull());
+	}
+
+	int maxPoints() const {return maxPoints_;}
+	float maxRange() const {return maxRange_;}
+	Transform localTransform() const {return localTransform_;}
+
+private:
+	int maxPoints_;
+	float maxRange_;
+	Transform localTransform_;
+};
+
+}
+
+#endif /* CORELIB_INCLUDE_RTABMAP_CORE_LASERSCANINFO_H_ */

corelib/include/rtabmap/core/Memory.h

@@ -55,7 +55,7 @@ class Registration;
 class RegistrationInfo;
 class RegistrationIcp;
 class Stereo;
-class Occupancy;
+class OccupancyGrid;
 
 class RTABMAP_EXP Memory
 {
@@ -156,7 +156,7 @@ public:
 	void getNodeCalibration(int nodeId,
 			std::vector<CameraModel> & models,
 			StereoCameraModel & stereoModel);
-	SensorData getSignatureDataConst(int locationId) const;
+	SensorData getSignatureDataConst(int locationId, bool images = true, bool scan = true, bool userData = true, bool occupancyGrid = true) const;
 	std::set<int> getAllSignatureIds() const;
 	bool memoryChanged() const {return _memoryChanged;}
 	bool isIncremental() const {return _incrementalMemory;}
@@ -277,7 +277,7 @@ private:
 	Registration * _registrationPipeline;
 	RegistrationIcp * _registrationIcp;
 
-	Occupancy * _occupancy;
+	OccupancyGrid * _occupancy;
 };
 
 } // namespace rtabmap

corelib/include/rtabmap/core/OccupancyGrid.h

@@ -25,8 +25,8 @@ ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */
 
-#ifndef CORELIB_SRC_OCCUPANCY_H_
+#ifndef CORELIB_SRC_OCCUPANCYGRID_H_
-#define CORELIB_SRC_OCCUPANCY_H_
+#define CORELIB_SRC_OCCUPANCYGRID_H_
 
 #include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
 
@@ -35,34 +35,62 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 namespace rtabmap {
 
-class RTABMAP_EXP Occupancy
+class RTABMAP_EXP OccupancyGrid
 {
 public:
-	Occupancy(const ParametersMap & parameters = ParametersMap());
+	OccupancyGrid(const ParametersMap & parameters = ParametersMap());
 	void parseParameters(const ParametersMap & parameters);
+	void setCellSize(float cellSize);
 	float getCellSize() const {return cellSize_;}
-	void segment(const Signature & node, cv::Mat & obstacles, cv::Mat & ground);
+	void createLocalMap(const Signature & node, cv::Mat & ground, cv::Mat & obstacles, cv::Point3f & viewPoint) const;
+
+	void clear();
+	void addToCache(
+			int nodeId,
+			const cv::Mat & ground,
+			const cv::Mat & obstacles);
+	void update(const std::map<int, Transform> & poses, float minMapSize = 0.0f, float footprintRadius = 0.0f);
+	const cv::Mat & getMap(float & xMin, float & yMin) const
+	{
+		xMin = xMin_;
+		yMin = yMin_;
+		return map_;
+	}
 
 private:
 	ParametersMap parameters_;
 	int cloudDecimation_;
 	float cloudMaxDepth_;
 	float cloudMinDepth_;
+	std::vector<float> roiRatios_;
+	int scanDecimation_;
 	float cellSize_;
 	bool occupancyFromCloud_;
 	bool projMapFrame_;
 	float maxObstacleHeight_;
+	int normalKSearch_;
 	float maxGroundAngle_;
 	int minClusterSize_;
 	bool flatObstaclesDetected_;
+	float minGroundHeight_;
 	float maxGroundHeight_;
+	bool normalsSegmentation_;
 	bool grid3D_;
 	bool groundIsObstacle_;
 	float noiseFilteringRadius_;
 	int noiseFilteringMinNeighbors_;
+	bool scan2dUnknownSpaceFilled_;
+	double scan2dMaxUnknownSpaceFilledRange_;
 
+	std::map<int, std::pair<cv::Mat, cv::Mat> > cache_;
+	cv::Mat map_;
+	cv::Mat mapInfo_;
+	std::map<int, std::pair<int, int> > cellCount_; //<node Id, cells>
+	float xMin_;
+	float yMin_;
+	std::map<int, Transform> addedNodes_;
 };
 
 }
 
-#endif /* CORELIB_SRC_OCCUPANCY_H_ */
+#endif /* CORELIB_SRC_OCCUPANCYGRID_H_ */

corelib/include/rtabmap/core/OctoMap.h

@@ -62,7 +62,12 @@ public:
 	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);
+			pcl::PointCloud<pcl::PointXYZRGB>::Ptr & obstacles,
+			const pcl::PointXYZ & viewPoint);
+	void addToCache(int nodeId,
+			const cv::Mat & ground,
+			const cv::Mat & obstacles,
+			const cv::Point3f & viewPoint);
 	void update(const std::map<int, Transform> & poses);
 
 	const octomap::ColorOcTree * octree() const {return octree_;}
@@ -84,11 +89,14 @@ public:
 	void clear();
 
 private:
-	std::map<int, std::pair<pcl::PointCloud<pcl::PointXYZRGB>::Ptr, pcl::PointCloud<pcl::PointXYZRGB>::Ptr> > cache_;
+	std::map<int, std::pair<cv::Mat, cv::Mat> > cache_;
+	std::map<int, std::pair<pcl::PointCloud<pcl::PointXYZRGB>::Ptr, pcl::PointCloud<pcl::PointXYZRGB>::Ptr> > cacheClouds_;
+	std::map<int, cv::Point3f> cacheViewPoints_;
 	octomap::ColorOcTree * octree_;
 	std::map<octomap::ColorOcTreeNode*, OcTreeNodeInfo> occupiedCells_;
 	std::map<int, Transform> addedNodes_;
 	octomap::KeyRay keyRay_;
+	bool hasColor_;
 };
 
 } /* namespace rtabmap */

corelib/include/rtabmap/core/Parameters.h

@@ -31,6 +31,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 // default parameters
 #include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
 #include "rtabmap/core/Version.h" // DLL export/import defines
+#include <rtabmap/utilite/UConversion.h>
 #include <string>
 #include <map>
 
@@ -176,7 +177,7 @@ class RTABMAP_EXP Parameters
 	RTABMAP_PARAM(Rtabmap, MemoryThr, 		             int, 0, 	 "Maximum signatures in the Working Memory (ms) (0 means infinity).");
 	RTABMAP_PARAM(Rtabmap, DetectionRate,                float, 1,   "Detection rate. RTAB-Map will filter input images to satisfy this rate.");
 	RTABMAP_PARAM(Rtabmap, ImageBufferSize,              unsigned int, 1, "Data buffer size (0 min inf).");
-	RTABMAP_PARAM(Rtabmap, CreateIntermediateNodes,      bool, false, "Create intermediate nodes between loop closure detection. Only used when Rtabmap/DetectionRate>0.");
+	RTABMAP_PARAM(Rtabmap, CreateIntermediateNodes,      bool, false, uFormat("Create intermediate nodes between loop closure detection. Only used when %s>0.", kRtabmapDetectionRate().c_str()));
 	RTABMAP_PARAM_STR(Rtabmap, WorkingDirectory,         "", "Working directory.");
 	RTABMAP_PARAM(Rtabmap, MaxRetrieved,                 unsigned int, 2, "Maximum locations retrieved at the same time from LTM.");
 	RTABMAP_PARAM(Rtabmap, StatisticLogsBufferedInRAM,   bool, true, "Statistic logs buffered in RAM instead of written to hard drive after each iteration.");
@@ -210,7 +211,6 @@ class RTABMAP_EXP Parameters
 	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, 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");
@@ -308,7 +308,7 @@ class RTABMAP_EXP Parameters
 	RTABMAP_PARAM(RGBD, AngularUpdate,            float, 0.1,  "Minimum angular displacement to update the map. Rehearsal is done prior to this, so weights are still updated.");
 	RTABMAP_PARAM(RGBD, NewMapOdomChangeDistance, float, 0,    "A new map is created if a change of odometry translation greater than X m is detected (0 m = disabled).");
 	RTABMAP_PARAM(RGBD, OptimizeFromGraphEnd,     bool, false, "Optimize graph from the newest node. If false, the graph is optimized from the oldest node of the current graph (this adds an overhead computation to detect to oldest mode of the current graph, but it can be useful to preserve the map referential from the oldest node). Warning when set to false: when some nodes are transferred, the first referential of the local map may change, resulting in momentary changes in robot/map position (which are annoying in teleoperation).");
-	RTABMAP_PARAM(RGBD, OptimizeMaxError,         float, 1,    "Reject loop closures if optimization error is greater than this value (0=disabled). This will help to detect when a wrong loop closure is added to the graph. Not compatible with \"Optimizer/Robust\" if enabled.");
+	RTABMAP_PARAM(RGBD, OptimizeMaxError,         float, 1,    uFormat("Reject loop closures if optimization error is greater than this value (0=disabled). This will help to detect when a wrong loop closure is added to the graph. Not compatible with \"%s\" if enabled.", kOptimizerRobust().c_str()));
 	RTABMAP_PARAM(RGBD, GoalReachedRadius,        float, 0.5,  "Goal reached radius (m).");
 	RTABMAP_PARAM(RGBD, PlanStuckIterations,      int, 0,      "Mark the current goal node on the path as unreachable if it is not updated after X iterations (0=disabled). If all upcoming nodes on the path are unreachabled, the plan fails.");
 	RTABMAP_PARAM(RGBD, PlanLinearVelocity,       float, 0,    "Linear velocity (m/sec) used to compute path weights.");
@@ -320,6 +320,7 @@ class RTABMAP_EXP Parameters
 	RTABMAP_PARAM(RGBD, ScanMatchingIdsSavedInLinks, bool, true,      "Save scan matching IDs in link's user data.");
 	RTABMAP_PARAM(RGBD, NeighborLinkRefining,          bool, false,  "When a new node is added to the graph, the transformation of its neighbor link to the previous node is refined using ICP (laser scans required!).");
 	RTABMAP_PARAM(RGBD, LoopClosureReextractFeatures,  bool, false,  "Extract features even if there are some already in the nodes.");
+	RTABMAP_PARAM(RGBD, CreateOccupancyGrid,          bool, false,  "Create local occupancy grid maps. See \"Grid\" group for parameters.");
 
 	// Local/Proximity loop closure detection
 	RTABMAP_PARAM(RGBD, ProximityByTime,              bool, false, 	"Detection over all locations in STM.");
@@ -344,7 +345,7 @@ class RTABMAP_EXP Parameters
 	RTABMAP_PARAM(Optimizer, Slam2D,            bool, false,     "If optimization is done only on x,y and theta (3DoF). Otherwise, it is done on full 6DoF poses.");
 	RTABMAP_PARAM(Optimizer, VarianceIgnored,   bool, false,     "Ignore constraints' variance. If checked, identity information matrix is used for each constraint. Otherwise, an information matrix is generated from the variance saved in the links.");
 	RTABMAP_PARAM(Optimizer, Epsilon,           double, 0.0001,  "Stop optimizing when the error improvement is less than this value.");
-	RTABMAP_PARAM(Optimizer, Robust,            bool, false,      "Robust graph optimization using Vertigo (only work for g2o and GTSAM optimization strategies). Not compatible with \"RGBD/OptimizeMaxError\" if enabled.");
+	RTABMAP_PARAM(Optimizer, Robust,            bool, false,     uFormat("Robust graph optimization using Vertigo (only work for g2o and GTSAM optimization strategies). Not compatible with \"%s\" if enabled.", kRGBDOptimizeMaxError().c_str()));
 
 	RTABMAP_PARAM(g2o, Solver,                  int, 0,          "0=csparse 1=pcg 2=cholmod");
 	RTABMAP_PARAM(g2o, Optimizer,               int, 0,          "0=Levenberg 1=GaussNewton");
@@ -391,12 +392,12 @@ class RTABMAP_EXP Parameters
 	// Visual registration parameters
 	RTABMAP_PARAM(Vis, EstimationType,           int, 0,    	"Motion estimation approach: 0:3D->3D, 1:3D->2D (PnP), 2:2D->2D (Epipolar Geometry)");
 	RTABMAP_PARAM(Vis, ForwardEstOnly,           bool, true, 	"Forward estimation only (A->B). If false, a transformation is also computed in backward direction (B->A), then the two resulting transforms are merged (middle interpolation between the transforms).");
-	RTABMAP_PARAM(Vis, InlierDistance,           float, 0.1,    "[Vis/EstimationType = 0] Maximum distance for feature correspondences. Used by 3D->3D estimation approach.");
+	RTABMAP_PARAM(Vis, InlierDistance,           float, 0.1,    uFormat("[%s = 0] Maximum distance for feature correspondences. Used by 3D->3D estimation approach.", kVisEstimationType().c_str()));
-	RTABMAP_PARAM(Vis, RefineIterations,         int, 5,        "[Vis/EstimationType = 0] Number of iterations used to refine the transformation found by RANSAC. 0 means that the transformation is not refined.");
+	RTABMAP_PARAM(Vis, RefineIterations,         int, 5,        uFormat("[%s = 0] Number of iterations used to refine the transformation found by RANSAC. 0 means that the transformation is not refined.", kVisEstimationType().c_str()));
-	RTABMAP_PARAM(Vis, PnPReprojError, 	         float, 2,      "[Vis/EstimationType = 1] PnP reprojection error.");
+	RTABMAP_PARAM(Vis, PnPReprojError, 	         float, 2,      uFormat("[%s = 1] PnP reprojection error.", kVisEstimationType().c_str()));
-	RTABMAP_PARAM(Vis, PnPFlags,                 int, 1,        "[Vis/EstimationType = 1] PnP flags: 0=Iterative, 1=EPNP, 2=P3P");
+	RTABMAP_PARAM(Vis, PnPFlags,                 int, 1,        uFormat("[%s = 1] PnP flags: 0=Iterative, 1=EPNP, 2=P3P", kVisEstimationType().c_str()));
-	RTABMAP_PARAM(Vis, PnPRefineIterations,      int, 1,        "[Vis/EstimationType = 1] Refine iterations.");
+	RTABMAP_PARAM(Vis, PnPRefineIterations,      int, 1,        uFormat("[%s = 1] Refine iterations.", kVisEstimationType().c_str()));
-	RTABMAP_PARAM(Vis, EpipolarGeometryVar,      float, 0.02,   "[Vis/EstimationType = 2] Epipolar geometry maximum variance to accept the transformation.");
+	RTABMAP_PARAM(Vis, EpipolarGeometryVar,      float, 0.02,   uFormat("[%s = 2] Epipolar geometry maximum variance to accept the transformation.", kVisEstimationType().c_str()));
 	RTABMAP_PARAM(Vis, MinInliers,               int, 20, 		"Minimum feature correspondences to compute/accept the transformation.");
 	RTABMAP_PARAM(Vis, Iterations,               int, 100, 		"Maximum iterations to compute the transform.");
 #ifndef RTABMAP_NONFREE
@@ -418,13 +419,13 @@ class RTABMAP_EXP Parameters
 	RTABMAP_PARAM(Vis, SubPixIterations,         int, 0,        "See cv::cornerSubPix(). 0 disables sub pixel refining.");
 	RTABMAP_PARAM(Vis, SubPixEps,                float, 0.02,   "See cv::cornerSubPix().");
 	RTABMAP_PARAM(Vis, CorType,                  int, 0,        "Correspondences computation approach: 0=Features Matching, 1=Optical Flow");
-	RTABMAP_PARAM(Vis, CorNNType, 	             int, 1,        "[Vis/CorrespondenceType=0] kNNFlannNaive=0, kNNFlannKdTree=1, kNNFlannLSH=2, kNNBruteForce=3, kNNBruteForceGPU=4. Used for features matching approach.");
+	RTABMAP_PARAM(Vis, CorNNType, 	             int, 1,        uFormat("[%s=0] kNNFlannNaive=0, kNNFlannKdTree=1, kNNFlannLSH=2, kNNBruteForce=3, kNNBruteForceGPU=4. Used for features matching approach.", kVisCorType().c_str()));
-	RTABMAP_PARAM(Vis, CorNNDR,                  float, 0.8,    "[Vis/CorrespondenceType=0] NNDR: nearest neighbor distance ratio. Used for features matching approach.");
+	RTABMAP_PARAM(Vis, CorNNDR,                  float, 0.8,    uFormat("[%s=0] NNDR: nearest neighbor distance ratio. Used for features matching approach.", kVisCorType().c_str()));
-	RTABMAP_PARAM(Vis, CorGuessWinSize,          int, 50,       "[Vis/CorrespondenceType=0] Matching window size (pixels) around projected points when a guess transform is provided to find correspondences. 0 means disabled.");
+	RTABMAP_PARAM(Vis, CorGuessWinSize,          int, 50,       uFormat("[%s=0] Matching window size (pixels) around projected points when a guess transform is provided to find correspondences. 0 means disabled.", kVisCorType().c_str()));
-	RTABMAP_PARAM(Vis, CorFlowWinSize,           int, 16,       "[Vis/CorrespondenceType=1] See cv::calcOpticalFlowPyrLK(). Used for optical flow approach.");
+	RTABMAP_PARAM(Vis, CorFlowWinSize,           int, 16,       uFormat("[%s=1] See cv::calcOpticalFlowPyrLK(). Used for optical flow approach.", kVisCorType().c_str()));
-	RTABMAP_PARAM(Vis, CorFlowIterations,        int, 30,       "[Vis/CorrespondenceType=1] See cv::calcOpticalFlowPyrLK(). Used for optical flow approach.");
+	RTABMAP_PARAM(Vis, CorFlowIterations,        int, 30,       uFormat("[%s=1] See cv::calcOpticalFlowPyrLK(). Used for optical flow approach.", kVisCorType().c_str()));
-	RTABMAP_PARAM(Vis, CorFlowEps,               float, 0.01,   "[Vis/CorrespondenceType=1] See cv::calcOpticalFlowPyrLK(). Used for optical flow approach.");
+	RTABMAP_PARAM(Vis, CorFlowEps,               float, 0.01,   uFormat("[%s=1] See cv::calcOpticalFlowPyrLK(). Used for optical flow approach.", kVisCorType().c_str()));
-	RTABMAP_PARAM(Vis, CorFlowMaxLevel,          int, 3,        "[Vis/CorrespondenceType=1] See cv::calcOpticalFlowPyrLK(). Used for optical flow approach.");
+	RTABMAP_PARAM(Vis, CorFlowMaxLevel,          int, 3,        uFormat("[%s=1] See cv::calcOpticalFlowPyrLK(). Used for optical flow approach.", kVisCorType().c_str()));
 
 	// ICP registration parameters
 	RTABMAP_PARAM(Icp, MaxTranslation,            float, 0.2,   "Maximum ICP translation correction accepted (m).");
@@ -446,8 +447,8 @@ class RTABMAP_EXP Parameters
 	RTABMAP_PARAM(Stereo, MinDisparity,          int, 1,        "Minimum disparity.");
 	RTABMAP_PARAM(Stereo, MaxDisparity,          int, 128,      "Maximum disparity.");
 	RTABMAP_PARAM(Stereo, OpticalFlow,           bool, true,    "Use optical flow to find stereo correspondences, otherwise a simple block matching approach is used.");
-	RTABMAP_PARAM(Stereo, SSD,                   bool, true,    "[Stereo/OpticalFlow = false] Use Sum of Squared Differences (SSD) window, otherwise Sum of Absolute Differences (SAD) window is used.");
+	RTABMAP_PARAM(Stereo, SSD,                   bool, true,    uFormat("[%s=false] Use Sum of Squared Differences (SSD) window, otherwise Sum of Absolute Differences (SAD) window is used.", kStereoOpticalFlow().c_str()));
-	RTABMAP_PARAM(Stereo, Eps,                   double, 0.01,  "[Stereo/OpticalFlow = true] Epsilon stop criterion.");
+	RTABMAP_PARAM(Stereo, Eps,                   double, 0.01,  uFormat("[%s=true] Epsilon stop criterion.", kStereoOpticalFlow().c_str()));
 
 	RTABMAP_PARAM(StereoBM, BlockSize,           int, 15,       "See cv::StereoBM");
 	RTABMAP_PARAM(StereoBM, MinDisparity,        int, 0,        "See cv::StereoBM");
@@ -460,22 +461,32 @@ class RTABMAP_EXP Parameters
 	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, FromDepth,               bool,   true,    "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, DepthDecimation,         int,    4,       uFormat("[%s=true] Decimation of the depth image before creating cloud.", kGridDepthDecimation().c_str()));
-	RTABMAP_PARAM(Grid, DepthMin,                float,  0.0,     "[Grid/FromDepth=true]");
+	RTABMAP_PARAM(Grid, DepthMin,                float,  0.0,     uFormat("[%s=true] Minimum cloud's depth from sensor.", kGridDepthDecimation().c_str()));
-	RTABMAP_PARAM(Grid, DepthMax,                float,  0.0,     "[Grid/FromDepth=true]");
+	RTABMAP_PARAM(Grid, DepthMax,                float,  4.0,     uFormat("[%s=true] Maximum cloud's depth from sensor. 0=inf.", kGridDepthDecimation().c_str()));
-	RTABMAP_PARAM_STR(Grid, DepthRoiRatios,      "0.0 0.0 0.0 0.0", "Region of interest ratios [left, right, top, bottom].");
+	RTABMAP_PARAM_STR(Grid, DepthRoiRatios,      "0.0 0.0 0.0 0.0", uFormat("[%s=true] Region of interest ratios [left, right, top, bottom].", kGridDepthDecimation().c_str()));
-	RTABMAP_PARAM(Grid, CellSize,                float,  0.05,    "");
+	RTABMAP_PARAM(Grid, ScanDecimation,          int,    1,       uFormat("[%s=false] Decimation of the laser scan before creating cloud.", kGridDepthDecimation().c_str()));
-	RTABMAP_PARAM(Grid, MapFrameProjection,      bool,   false,   "");
+	RTABMAP_PARAM(Grid, CellSize,                float,  0.05,    "Resolution of the occupancy grid.");
-	RTABMAP_PARAM(Grid, MaxObstacleHeight,       float,  0.0,     "");
+	RTABMAP_PARAM(Grid, MapFrameProjection,      bool,   false,   "Projection in map frame. On a 3D terrain and a fixed local camera transform (the cloud is created relative to ground), you may want to disable this to do the projection in robot frame instead.");
-	RTABMAP_PARAM(Grid, MaxGroundHeight,         float,  0.0,     "");
+	RTABMAP_PARAM(Grid, NormalsSegmentation,     bool,   true,    "Segment ground from obstacles using point normals, otherwise a fast passthrough is used.");
-	RTABMAP_PARAM(Grid, MaxGroundAngle,          float,  0.78,    "");
+	RTABMAP_PARAM(Grid, MaxObstacleHeight,       float,  0.0,     "Maximum obstacles height (0=disabled).");
-	RTABMAP_PARAM(Grid, MinClusterSize,          int,    10,      "");
+	RTABMAP_PARAM(Grid, MinGroundHeight,         float,  0.0,     "Minimum ground height (0=disabled).");
-	RTABMAP_PARAM(Grid, FlatObstacleDetected,    bool,   false,   "");
+	RTABMAP_PARAM(Grid, MaxGroundHeight,         float,  0.0,     uFormat("Maximum ground height (0=disabled). Should be set if \"%s\" is true.", kGridNormalsSegmentation().c_str()));
-	RTABMAP_PARAM(Grid, 3D,                      bool,   false,   "Ignored if laser scan is 2D.");
+	RTABMAP_PARAM(Grid, MaxGroundAngle,          float,  45,      uFormat("[%s=true] Maximum angle (degrees) between point's normal to ground's normal to label it as ground. Points with higher angle difference are considered as obstacles.", kGridNormalsSegmentation().c_str()));
-	RTABMAP_PARAM(Grid, 3DGroundIsObstacle,      bool,   false,   "[Grid/3D=true] The ground is considered as an obstacle.");
+	RTABMAP_PARAM(Grid, NormalK,                 int,    10,      uFormat("[%s=true] K neighbors to compute normals.", kGridNormalsSegmentation().c_str()))
-	RTABMAP_PARAM(Grid, NoiseFilteringRadius,         float,   0.0, "0 means disabled.");
+	RTABMAP_PARAM(Grid, MinClusterSize,          int,    10,      uFormat("[%s=true] Minimum cluster size to project the points. The distance between clusters is defined by 2*\"%s\".", kGridNormalsSegmentation().c_str(), kGridCellSize().c_str()));
-	RTABMAP_PARAM(Grid, NoiseFilteringMinNeighbors,   int,     5,   "");
+	RTABMAP_PARAM(Grid, FlatObstacleDetected,    bool,   false,   uFormat("[%s=true] Flat obstacles detected.", kGridNormalsSegmentation().c_str()));
+#ifdef RTABMAP_OCTOMAP
+	RTABMAP_PARAM(Grid, 3D,                      bool,   true,    uFormat("A 3D occupancy grid is required if you want an Octomap. Set to false if you want only a 2D map, the cloud will be projected on xy plane. A 2D map can be still generated if checked, but it requires more memory and time to generate it. Ignored if laser scan is 2D and \"%s\" is false.", kGridFromDepth().c_str()));
+#else
+	RTABMAP_PARAM(Grid, 3D,                      bool,   false,   uFormat("A 3D occupancy grid is required if you want an Octomap. Set to false if you want only a 2D map, the cloud will be projected on xy plane. A 2D map can be still generated if checked, but it requires more memory and time to generate it. Ignored if laser scan is 2D and \"%s\" is false.", kGridFromDepth().c_str()));
+#endif
+	RTABMAP_PARAM(Grid, 3DGroundIsObstacle,      bool,   false,   uFormat("[%s=true] Ground is an obstacle. Use this only if you want an Octomap with ground identified as an obstacle (e.g., with an UAV).", kGrid3D().c_str()));
+	RTABMAP_PARAM(Grid, NoiseFilteringRadius,         float,   0.0,     "Noise filtering radius (0=disabled). Done after segmentation.");
+	RTABMAP_PARAM(Grid, NoiseFilteringMinNeighbors,   int,     5,       "Noise filtering minimum neighbors.");
+	RTABMAP_PARAM(Grid, Scan2dUnknownSpaceFilled,     bool,    false,   "Unknown space filled. Only used with 2D laser scans.");
+	RTABMAP_PARAM(Grid, Scan2dMaxFilledRange,         float,   6.0,     "Unknown space filled maximum range. If 0, the laser scan maximum range is used.");
 
 public:
 	virtual ~Parameters();
@@ -501,12 +512,12 @@ public:
 	 */
 	static std::string getDescription(const std::string & paramKey);
 
-	static void parse(const ParametersMap & parameters, const std::string & key, bool & value);
+	static bool parse(const ParametersMap & parameters, const std::string & key, bool & value);
-	static void parse(const ParametersMap & parameters, const std::string & key, int & value);
+	static bool parse(const ParametersMap & parameters, const std::string & key, int & value);
-	static void parse(const ParametersMap & parameters, const std::string & key, unsigned int & value);
+	static bool parse(const ParametersMap & parameters, const std::string & key, unsigned int & value);
-	static void parse(const ParametersMap & parameters, const std::string & key, float & value);
+	static bool parse(const ParametersMap & parameters, const std::string & key, float & value);
-	static void parse(const ParametersMap & parameters, const std::string & key, double & value);
+	static bool parse(const ParametersMap & parameters, const std::string & key, double & value);
-	static void parse(const ParametersMap & parameters, const std::string & key, std::string & value);
+	static bool parse(const ParametersMap & parameters, const std::string & key, std::string & value);
 	static void parse(const ParametersMap & parameters, ParametersMap & parametersOut);
 
 	static const char * showUsage();

corelib/include/rtabmap/core/SensorData.h

@@ -33,6 +33,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <rtabmap/core/CameraModel.h>
 #include <rtabmap/core/StereoCameraModel.h>
 #include <rtabmap/core/Transform.h>
+#include <rtabmap/core/LaserScanInfo.h>
 #include <opencv2/core/core.hpp>
 #include <opencv2/features2d/features2d.hpp>
 
@@ -75,8 +76,7 @@ public:
 	// RGB-D constructor + laser scan
 	SensorData(
 			const cv::Mat & laserScan,
-			int laserScanMaxPts,
+			const LaserScanInfo & laserScanInfo,
-			float laserScanMaxRange,
 			const cv::Mat & rgb,
 			const cv::Mat & depth,
 			const CameraModel & cameraModel,
@@ -96,8 +96,7 @@ public:
 	// Multi-cameras RGB-D constructor + laser scan
 	SensorData(
 			const cv::Mat & laserScan,
-			int laserScanMaxPts,
+			const LaserScanInfo & laserScanInfo,
-			float laserScanMaxRange,
 			const cv::Mat & rgb,
 			const cv::Mat & depth,
 			const std::vector<CameraModel> & cameraModels,
@@ -117,8 +116,7 @@ public:
 	// Stereo constructor + laser scan
 	SensorData(
 			const cv::Mat & laserScan,
-			int laserScanMaxPts,
+			const LaserScanInfo & laserScanInfo,
-			float laserScanMaxRange,
 			const cv::Mat & left,
 			const cv::Mat & right,
 			const StereoCameraModel & cameraModel,
@@ -131,7 +129,6 @@ public:
 	bool isValid() const {
 		return !(_id == 0 &&
 			_stamp == 0.0 &&
-			_laserScanMaxPts == 0 &&
 			_imageRaw.empty() &&
 			_imageCompressed.empty() &&
 			_depthOrRightRaw.empty() &&
@@ -150,8 +147,7 @@ public:
 	void setId(int id) {_id = id;}
 	double stamp() const {return _stamp;}
 	void setStamp(double stamp) {_stamp = stamp;}
-	int laserScanMaxPts() const {return _laserScanMaxPts;}
+	const LaserScanInfo & laserScanInfo() const {return _laserScanInfo;}
-	float laserScanMaxRange() const {return _laserScanMaxRange;}
 
 	const cv::Mat & imageCompressed() const {return _imageCompressed;}
 	const cv::Mat & depthOrRightCompressed() const {return _depthOrRightCompressed;}
@@ -162,7 +158,7 @@ public:
 	const cv::Mat & laserScanRaw() const {return _laserScanRaw;}
 	void setImageRaw(const cv::Mat & imageRaw) {_imageRaw = imageRaw;}
 	void setDepthOrRightRaw(const cv::Mat & depthOrImageRaw) {_depthOrRightRaw =depthOrImageRaw;}
-	void setLaserScanRaw(const cv::Mat & laserScanRaw, int maxPts, float maxRange) {_laserScanRaw =laserScanRaw;_laserScanMaxPts = maxPts;_laserScanMaxRange=maxRange;}
+	void setLaserScanRaw(const cv::Mat & laserScanRaw, const LaserScanInfo & info) {_laserScanRaw =laserScanRaw;_laserScanInfo = info;}
 	void setCameraModel(const CameraModel & model) {_cameraModels.clear(); _cameraModels.push_back(model);}
 	void setCameraModels(const std::vector<CameraModel> & models) {_cameraModels = models;}
 	void setStereoCameraModel(const StereoCameraModel & stereoCameraModel) {_stereoCameraModel = stereoCameraModel;}
@@ -172,8 +168,20 @@ public:
 	cv::Mat rightRaw() const {return _depthOrRightRaw.type()==CV_8UC1?_depthOrRightRaw:cv::Mat();}
 
 	void uncompressData();
-	void uncompressData(cv::Mat * imageRaw, cv::Mat * depthOrRightRaw, cv::Mat * laserScanRaw = 0, cv::Mat * userDataRaw = 0);
+	void uncompressData(
-	void uncompressDataConst(cv::Mat * imageRaw, cv::Mat * depthOrRightRaw, cv::Mat * laserScanRaw = 0, cv::Mat * userDataRaw = 0) const;
+			cv::Mat * imageRaw,
+			cv::Mat * depthOrRightRaw,
+			cv::Mat * laserScanRaw = 0,
+			cv::Mat * userDataRaw = 0,
+			cv::Mat * groundCellsRaw = 0,
+			cv::Mat * obstacleCellsRaw = 0);
+	void uncompressDataConst(
+			cv::Mat * imageRaw,
+			cv::Mat * depthOrRightRaw,
+			cv::Mat * laserScanRaw = 0,
+			cv::Mat * userDataRaw = 0,
+			cv::Mat * groundCellsRaw = 0,
+			cv::Mat * obstacleCellsRaw = 0) const;
 
 	const std::vector<CameraModel> & cameraModels() const {return _cameraModels;}
 	const StereoCameraModel & stereoCameraModel() const {return _stereoCameraModel;}
@@ -183,6 +191,21 @@ public:
 	const cv::Mat & userDataRaw() const {return _userDataRaw;}
 	const cv::Mat & userDataCompressed() const {return _userDataCompressed;}
 
+	// detect automatically if raw or compressed. If raw, the data will be compressed.
+	void setOccupancyGrid(
+			const cv::Mat & ground,
+			const cv::Mat & obstacles,
+			float cellSize,
+			const cv::Point3f & viewPoint);
+	// remove raw occupancy grids
+	void clearOccupancyGridRaw() {_groundCellsRaw = cv::Mat(); _obstacleCellsRaw = cv::Mat();}
+	const cv::Mat & gridGroundCellsRaw() const {return _groundCellsRaw;}
+	const cv::Mat & gridGroundCellsCompressed() const {return _groundCellsCompressed;}
+	const cv::Mat & gridObstacleCellsRaw() const {return _obstacleCellsRaw;}
+	const cv::Mat & gridObstacleCellsCompressed() const {return _obstacleCellsCompressed;}
+	float gridCellSize() const {return _cellSize;}
+	const cv::Point3f & gridViewPoint() const {return _viewPoint;}
+
 	void setFeatures(const std::vector<cv::KeyPoint> & keypoints, const cv::Mat & descriptors)
 	{
 		_keypoints = keypoints;
@@ -199,8 +222,6 @@ public:
 private:
 	int _id;
 	double _stamp;
-	int _laserScanMaxPts;
-	float _laserScanMaxRange;
 
 	cv::Mat _imageCompressed;          // compressed image
 	cv::Mat _depthOrRightCompressed;   // compressed image
@@ -213,10 +234,20 @@ private:
 	std::vector<CameraModel> _cameraModels;
 	StereoCameraModel _stereoCameraModel;
 
+	LaserScanInfo _laserScanInfo;
+
 	// user data
 	cv::Mat _userDataCompressed;      // compressed data
 	cv::Mat _userDataRaw;
 
+	// occupancy grid
+	cv::Mat _groundCellsCompressed;
+	cv::Mat _obstacleCellsCompressed;
+	cv::Mat _groundCellsRaw;
+	cv::Mat _obstacleCellsRaw;
+	float _cellSize;
+	cv::Point3f _viewPoint;
+
 	// features
 	std::vector<cv::KeyPoint> _keypoints;
 	cv::Mat _descriptors;

corelib/include/rtabmap/core/Signature.h

@@ -115,22 +115,11 @@ 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;}
 
@@ -157,10 +146,6 @@ private:
 	Transform _pose;
 	Transform _groundTruthPose;
 
-	cv::Mat _groundCells;
-	cv::Mat _obstacleCells;
-	float _cellSize;
-
 	SensorData _sensorData;
 };
 

corelib/include/rtabmap/core/StereoCameraModel.h

@@ -108,6 +108,7 @@ public:
 	const cv::Mat & F() const {return F_;} //extrinsic fundamental matrix
 
 	void scale(double scale);
+	void roi(const cv::Rect & roi);
 
 	void setLocalTransform(const Transform & transform) {left_.setLocalTransform(transform);}
 	const Transform & localTransform() const {return left_.localTransform();}

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

@@ -242,7 +242,7 @@ void occupancy2DFromGroundObstacles(
 		//voxelize to grid cell size
 		groundCloudProjected = util3d::voxelize(groundCloudProjected, cellSize);
 
-		ground = cv::Mat((int)groundCloudProjected->size(), 1, CV_32FC2);
+		ground = cv::Mat(1, (int)groundCloudProjected->size(), CV_32FC2);
 		for(unsigned int i=0;i<groundCloudProjected->size(); ++i)
 		{
 			ground.at<cv::Vec2f>(i)[0] = groundCloudProjected->at(i).x;
@@ -259,7 +259,7 @@ void occupancy2DFromGroundObstacles(
 		//voxelize to grid cell size
 		obstaclesCloudProjected = util3d::voxelize(obstaclesCloudProjected, cellSize);
 
-		obstacles = cv::Mat((int)obstaclesCloudProjected->size(), 1, CV_32FC2);
+		obstacles = cv::Mat(1, (int)obstaclesCloudProjected->size(), CV_32FC2);
 		for(unsigned int i=0;i<obstaclesCloudProjected->size(); ++i)
 		{
 			obstacles.at<cv::Vec2f>(i)[0] = obstaclesCloudProjected->at(i).x;

corelib/include/rtabmap/core/util2d.h

@@ -109,6 +109,11 @@ float RTABMAP_EXP getDepth(
 		float maxZError = 0.02f,
 		bool estWithNeighborsIfNull = false);
 
+cv::Rect RTABMAP_EXP computeRoi(const cv::Mat & image, const std::string & roiRatios);
+cv::Rect RTABMAP_EXP computeRoi(const cv::Size & imageSize, const std::string & roiRatios);
+cv::Rect RTABMAP_EXP computeRoi(const cv::Mat & image, const std::vector<float> & roiRatios);
+cv::Rect RTABMAP_EXP computeRoi(const cv::Size & imageSize, const std::vector<float> & roiRatios);
+
 cv::Mat RTABMAP_EXP decimate(const cv::Mat & image, int d);
 cv::Mat RTABMAP_EXP interpolate(const cv::Mat & image, int factor, float depthErrorRatio = 0.02f);
 

corelib/include/rtabmap/core/util3d.h

@@ -141,7 +141,8 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP cloudFromSensorData(
 		float maxDepth = 0.0f,
 		float minDepth = 0.0f,
 		std::vector<int> * validIndices = 0,
-		const ParametersMap & parameters = ParametersMap());
+		const ParametersMap & parameters = ParametersMap(),
+		const std::vector<float> & roiRatios = std::vector<float>()); // ignored for stereo
 
 /**
  * Create an RGB cloud from the images contained in SensorData. If there is only one camera,
@@ -154,6 +155,7 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP cloudFromSensorData(
  * @param maxDepth, maximum depth of the projected points (farther points are set to null in case of an organized cloud).
  * @param minDepth, minimum depth of the projected points (closer points are set to null in case of an organized cloud).
  * @param validIndices, the indices of valid points in the cloud
+ * @param roiRatios, [left, right, top, bottom] region of interest (in ratios) of the image projected.
  * @return a RGB cloud.
  */
 pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_EXP cloudRGBFromSensorData(
@@ -162,7 +164,8 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_EXP cloudRGBFromSensorData(
 		float maxDepth = 0.0f,
 		float minDepth = 0.0f,
 		std::vector<int> * validIndices = 0,
-		const ParametersMap & parameters = ParametersMap());
+		const ParametersMap & parameters = ParametersMap(),
+		const std::vector<float> & roiRatios = std::vector<float>()); // ignored for stereo
 
 pcl::PointCloud<pcl::PointXYZ> RTABMAP_EXP laserScanFromDepthImage(
 					const cv::Mat & depthImage,
@@ -178,12 +181,24 @@ pcl::PointCloud<pcl::PointXYZ> RTABMAP_EXP laserScanFromDepthImage(
 cv::Mat RTABMAP_EXP laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZ> & cloud, const Transform & transform = Transform());
 // return CV_32FC6
 cv::Mat RTABMAP_EXP laserScanFromPointCloud(const pcl::PointCloud<pcl::PointNormal> & cloud, const Transform & transform = Transform());
+// return CV_32FC4
+cv::Mat RTABMAP_EXP laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZRGB> & cloud, const Transform & transform = Transform());
 // return CV_32FC2
 cv::Mat RTABMAP_EXP laserScan2dFromPointCloud(const pcl::PointCloud<pcl::PointXYZ> & cloud, const Transform & transform = Transform());
 // For laserScan of type CV_32FC2, z is set to null.
 pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP laserScanToPointCloud(const cv::Mat & laserScan, const Transform & transform = Transform());
-// For laserScan of type CV_32FC2 or CV_32FC3, normals are set to null.
+// For laserScan of type CV_32FC2, CV_32FC3 and CV_32FC4, normals are set to null.
 pcl::PointCloud<pcl::PointNormal>::Ptr RTABMAP_EXP laserScanToPointCloudNormal(const cv::Mat & laserScan, const Transform & transform = Transform());
+// For laserScan of type CV_32FC2, CV_32FC3 and CV_32FC6, rgb is set to null.
+pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_EXP laserScanToPointCloudRGB(const cv::Mat & laserScan, const Transform & transform = Transform());
+
+// For laserScan of type CV_32FC2, z is set to null.
+pcl::PointXYZ RTABMAP_EXP laserScanToPoint(const cv::Mat & laserScan, int index);
+// For laserScan of type CV_32FC2, CV_32FC3 and CV_32FC4, normals are set to null.
+pcl::PointNormal RTABMAP_EXP laserScanToPointNormal(const cv::Mat & laserScan, int index);
+// For laserScan of type CV_32FC2, CV_32FC3 and CV_32FC6, rgb is set to null.
+pcl::PointXYZRGB RTABMAP_EXP laserScanToPointRGB(const cv::Mat & laserScan, int index);
+
 
 cv::Point3f RTABMAP_EXP projectDisparityTo3D(
 		const cv::Point2f & pt,

corelib/src/CMakeLists.txt

@@ -65,7 +65,7 @@ SET(SRC_FILES
 	StereoDense.cpp
 	StereoCameraModel.cpp
 	
-	Occupancy.cpp
+	OccupancyGrid.cpp
 		
 	rtflann/ext/lz4.c
 	rtflann/ext/lz4hc.c

corelib/src/CameraModel.cpp

@@ -383,6 +383,36 @@ CameraModel CameraModel::scaled(double scale) const
 	return scaledModel;
 }
 
+CameraModel CameraModel::roi(const cv::Rect & roi) const
+{
+	CameraModel roiModel = *this;
+	if(this->isValidForProjection())
+	{
+		// has only effect on cx and cy
+		cv::Mat K;
+		if(!K_.empty())
+		{
+			K = K_.clone();
+			K.at<double>(0,2) -= roi.x;
+			K.at<double>(1,2) -= roi.y;
+		}
+
+		cv::Mat P;
+		if(!P_.empty())
+		{
+			P = P_.clone();
+			P.at<double>(0,2) -= roi.x;
+			P.at<double>(1,2) -= roi.y;
+		}
+		roiModel = CameraModel(name_, roi.size(), K, D_, R_, P, localTransform_);
+	}
+	else
+	{
+		UWARN("Trying to extract roi from a camera model not valid! Ignoring roi...");
+	}
+	return roiModel;
+}
+
 double CameraModel::horizontalFOV() const
 {
 	if(imageWidth() > 0 && fx() > 0.0)

corelib/src/CameraRGB.cpp

@@ -65,6 +65,7 @@ CameraImages::CameraImages() :
 		_dir(0),
 		_countScan(0),
 		_scanDir(0),
+		_scanLocalTransform(Transform::getIdentity()),
 		_scanMaxPts(0),
 		_scanDownsampleStep(1),
 		_scanVoxelSize(0.0f),
@@ -666,11 +667,11 @@ SensorData CameraImages::captureImage(CameraInfo * info)
 				pcl::PointCloud<pcl::Normal>::Ptr normals = util3d::computeNormals(cloud, _scanNormalsK);
 				pcl::PointCloud<pcl::PointNormal>::Ptr cloudNormals(new pcl::PointCloud<pcl::PointNormal>);
 				pcl::concatenateFields(*cloud, *normals, *cloudNormals);
-				scan = util3d::laserScanFromPointCloud(*cloudNormals);
+				scan = util3d::laserScanFromPointCloud(*cloudNormals, _scanLocalTransform.inverse());
 			}
 			else
 			{
-				scan = util3d::laserScanFromPointCloud(*cloud);
+				scan = util3d::laserScanFromPointCloud(*cloud, _scanLocalTransform.inverse());
 			}
 		}
 	}
@@ -684,7 +685,7 @@ SensorData CameraImages::captureImage(CameraInfo * info)
 		_model.setImageSize(img.size());
 	}
 
-	SensorData data(scan, scan.empty()?0:_scanMaxPts, 0, _isDepth?cv::Mat():img, _isDepth?img:depthFromScan, _model, this->getNextSeqID(), stamp);
+	SensorData data(scan, LaserScanInfo(scan.empty()?0:_scanMaxPts, 0, _scanLocalTransform), _isDepth?cv::Mat():img, _isDepth?img:depthFromScan, _model, this->getNextSeqID(), stamp);
 	data.setGroundTruth(groundTruthPose);
 	return data;
 }

corelib/src/CameraStereo.cpp

@@ -1164,7 +1164,7 @@ SensorData CameraStereoImages::captureImage(CameraInfo * info)
 				stereoModel_.setImageSize(leftImage.size());
 			}
 
-			data = SensorData(left.laserScanRaw(), left.laserScanMaxPts(), 0, leftImage, rightImage, stereoModel_, left.id()/(camera2_?1:2), left.stamp());
+			data = SensorData(left.laserScanRaw(), left.laserScanInfo(), leftImage, rightImage, stereoModel_, left.id()/(camera2_?1:2), left.stamp());
 			data.setGroundTruth(left.groundTruth());
 		}
 	}

corelib/src/CameraThread.cpp

@@ -175,9 +175,15 @@ void CameraThread::mainLoop()
 				UASSERT(_scanDecimation >= 1);
 				UTimer timer;
 				pcl::IndicesPtr validIndices(new std::vector<int>);
-				pcl::PointCloud<pcl::PointXYZ>::Ptr cloud = util3d::cloudFromSensorData(data, _scanDecimation, _scanMaxDepth, _scanMinDepth, validIndices.get());
+				pcl::PointCloud<pcl::PointXYZ>::Ptr cloud = util3d::cloudFromSensorData(
+						data,
+						_scanDecimation,
+						_scanMaxDepth,
+						_scanMinDepth,
+						validIndices.get());
 				float maxPoints = (data.depthRaw().rows/_scanDecimation)*(data.depthRaw().cols/_scanDecimation);
 				cv::Mat scan;
+				const Transform & baseToScan = data.cameraModels()[0].localTransform();
 				if(validIndices->size())
 				{
 					if(_scanVoxelSize>0.0f)
@@ -197,32 +203,19 @@ void CameraThread::mainLoop()
 					{
 						if(_scanNormalsK>0)
 						{
-							// view point
+							Eigen::Vector3f viewPoint(baseToScan.x(), baseToScan.y(), baseToScan.z());
-							Eigen::Vector3f viewPoint(0.0f,0.0f,0.0f);
-							if(data.cameraModels().size() && !data.cameraModels()[0].localTransform().isNull())
-							{
-								viewPoint[0] = data.cameraModels()[0].localTransform().x();
-								viewPoint[1] = data.cameraModels()[0].localTransform().y();
-								viewPoint[2] = data.cameraModels()[0].localTransform().z();
-							}
-							else if(!data.stereoCameraModel().localTransform().isNull())
-							{
-								viewPoint[0] = data.stereoCameraModel().localTransform().x();
-								viewPoint[1] = data.stereoCameraModel().localTransform().y();
-								viewPoint[2] = data.stereoCameraModel().localTransform().z();
-							}
 							pcl::PointCloud<pcl::Normal>::Ptr normals = util3d::computeNormals(cloud, _scanNormalsK, viewPoint);
 							pcl::PointCloud<pcl::PointNormal>::Ptr cloudNormals(new pcl::PointCloud<pcl::PointNormal>);
 							pcl::concatenateFields(*cloud, *normals, *cloudNormals);
-							scan = util3d::laserScanFromPointCloud(*cloudNormals);
+							scan = util3d::laserScanFromPointCloud(*cloudNormals, baseToScan.inverse());
 						}
 						else
 						{
-							scan = util3d::laserScanFromPointCloud(*cloud);
+							scan = util3d::laserScanFromPointCloud(*cloud, baseToScan.inverse());
 						}
 					}
 				}
-				data.setLaserScanRaw(scan, (int)maxPoints, _scanMaxDepth);
+				data.setLaserScanRaw(scan, LaserScanInfo((int)maxPoints, _scanMaxDepth, baseToScan));
 				info.timeScanFromDepth = timer.ticks();
 				UDEBUG("Computing scan from depth = %f s", info.timeScanFromDepth);
 			}

corelib/src/DBDriver.cpp

@@ -514,7 +514,7 @@ void DBDriver::loadWords(const std::set<int> & wordIds, std::list<VisualWord *>
 	}
 }
 
-void DBDriver::loadNodeData(std::list<Signature *> & signatures) const
+void DBDriver::loadNodeData(std::list<Signature *> & signatures, bool images, bool scan, bool userData, bool occupancyGrid) const
 {
 	// Don't look in the trash, we assume that if we want to load
 	// data of a signature, it is not in thrash! Print an error if so.
@@ -530,13 +530,14 @@ void DBDriver::loadNodeData(std::list<Signature *> & signatures) const
 	_trashesMutex.unlock();
 
 	_dbSafeAccessMutex.lock();
-	this->loadNodeDataQuery(signatures);
+	this->loadNodeDataQuery(signatures, images, scan, userData, occupancyGrid);
 	_dbSafeAccessMutex.unlock();
 }
 
 void DBDriver::getNodeData(
 		int signatureId,
-		SensorData & data) const
+		SensorData & data,
+		bool images, bool scan, bool userData, bool occupancyGrid) const
 {
 	bool found = false;
 	// look in the trash
@@ -544,7 +545,11 @@ void DBDriver::getNodeData(
 	if(uContains(_trashSignatures, signatureId))
 	{
 		const Signature * s = _trashSignatures.at(signatureId);
-		if(!s->sensorData().imageCompressed().empty() || !s->isSaved())
+		if(!s->sensorData().imageCompressed().empty() ||
+			!s->sensorData().laserScanCompressed().empty() ||
+			!s->sensorData().userDataCompressed().empty() ||
+			s->sensorData().gridCellSize() != 0.0f ||
+			!s->isSaved())
 		{
 			data = (SensorData)s->sensorData();
 			found = true;
@@ -558,7 +563,7 @@ void DBDriver::getNodeData(
 		std::list<Signature *> signatures;
 		Signature tmp(signatureId);
 		signatures.push_back(&tmp);
-		loadNodeDataQuery(signatures);
+		loadNodeDataQuery(signatures, images, scan, userData, occupancyGrid);
 		data = signatures.front()->sensorData();
 		_dbSafeAccessMutex.unlock();
 	}

corelib/src/DBDriverSqlite3.cpp

@@ -744,9 +744,16 @@ ParametersMap DBDriverSqlite3::getLastParametersQuery() const
 	return parameters;
 }
 
-void DBDriverSqlite3::loadNodeDataQuery(std::list<Signature *> & signatures) const
+void DBDriverSqlite3::loadNodeDataQuery(std::list<Signature *> & signatures, bool images, bool scan, bool userData, bool occupancyGrid) const
 {
 	UDEBUG("load data for %d signatures", (int)signatures.size());
+
+	if(!images && !scan && !userData && !occupancyGrid)
+	{
+		UWARN("All requested data fields are false! Nothing loaded...");
+		return;
+	}
+
 	if(_ppDb)
 	{
 		UTimer timer;
@@ -755,7 +762,44 @@ void DBDriverSqlite3::loadNodeDataQuery(std::list<Signature *> & signatures) con
 		sqlite3_stmt * ppStmt = 0;
 		std::stringstream query;
 
-		if(uStrNumCmp(_version, "0.10.7") >= 0)
+		if(uStrNumCmp(_version, "0.11.10") >= 0)
+		{
+			std::stringstream fields;
+			if(images)
+			{
+				fields << "image, depth, calibration";
+				if(scan || userData || occupancyGrid)
+				{
+					fields << ", ";
+				}
+			}
+			if(scan)
+			{
+				fields << "scan_info, scan";
+				if(userData || occupancyGrid)
+				{
+					fields << ", ";
+				}
+			}
+			if(userData)
+			{
+				fields << "user_data";
+				if(occupancyGrid)
+				{
+					fields << ", ";
+				}
+			}
+			if(occupancyGrid)
+			{
+				fields << "ground_cells, obstacle_cells, cell_size, view_point_x, view_point_y, view_point_z";
+			}
+
+			query << "SELECT " << fields.str().c_str() << " "
+				  << "FROM Data "
+				  << "WHERE id = ?"
+				  <<";";
+		}
+		else if(uStrNumCmp(_version, "0.10.7") >= 0)
 		{
 			query << "SELECT image, depth, calibration, scan_max_pts, scan_max_range, scan, user_data "
 				  << "FROM Data "
@@ -853,197 +897,259 @@ void DBDriverSqlite3::loadNodeDataQuery(std::list<Signature *> & signatures) con
 				cv::Mat scanCompressed;
 				cv::Mat userDataCompressed;
 
-				data = sqlite3_column_blob(ppStmt, index);
+				if(uStrNumCmp(_version, "0.11.10") < 0 || images)
-				dataSize = sqlite3_column_bytes(ppStmt, index++);
-
-				//Create the image
-				if(dataSize>4 && data)
-				{
-					imageCompressed = cv::Mat(1, dataSize, CV_8UC1, (void *)data).clone();
-				}
-
-				data = sqlite3_column_blob(ppStmt, index);
-				dataSize = sqlite3_column_bytes(ppStmt, index++);
-
-				//Create the depth image
-				if(dataSize>4 && data)
-				{
-					depthOrRightCompressed = cv::Mat(1, dataSize, CV_8UC1, (void *)data).clone();
-				}
-
-				if(uStrNumCmp(_version, "0.10.0") < 0)
-				{
-					data = sqlite3_column_blob(ppStmt, index); // local transform
-					dataSize = sqlite3_column_bytes(ppStmt, index++);
-					if((unsigned int)dataSize == localTransform.size()*sizeof(float) && data)
-					{
-						memcpy(localTransform.data(), data, dataSize);
-					}
-				}
-
-				// calibration
-				if(uStrNumCmp(_version, "0.10.0") >= 0)
 				{
+					//Create the image
 					data = sqlite3_column_blob(ppStmt, index);
 					dataSize = sqlite3_column_bytes(ppStmt, index++);
-					// multi-cameras [fx,fy,cx,cy,[width,height],local_transform, ... ,fx,fy,cx,cy,[width,height],local_transform] (4or6+12)*float * numCameras
+					if(dataSize>4 && data)
-					// stereo [fx, fy, cx, cy, baseline, local_transform] (5+12)*float
-					if(dataSize > 0 && data)
 					{
-						float * dataFloat = (float*)data;
+						imageCompressed = cv::Mat(1, dataSize, CV_8UC1, (void *)data).clone();
-						if(uStrNumCmp(_version, "0.11.2") >= 0 &&
+					}
-						   (unsigned int)dataSize % (6+localTransform.size())*sizeof(float) == 0)
+
+					//Create the depth image
+					data = sqlite3_column_blob(ppStmt, index);
+					dataSize = sqlite3_column_bytes(ppStmt, index++);
+					if(dataSize>4 && data)
+					{
+						depthOrRightCompressed = cv::Mat(1, dataSize, CV_8UC1, (void *)data).clone();
+					}
+
+					if(uStrNumCmp(_version, "0.10.0") < 0)
+					{
+						data = sqlite3_column_blob(ppStmt, index); // local transform
+						dataSize = sqlite3_column_bytes(ppStmt, index++);
+						if((unsigned int)dataSize == localTransform.size()*sizeof(float) && data)
 						{
-							int cameraCount = dataSize / ((6+localTransform.size())*sizeof(float));
+							memcpy(localTransform.data(), data, dataSize);
-							UDEBUG("Loading calibration for %d cameras (%d bytes)", cameraCount, dataSize);
+						}
-							int max = cameraCount*(6+localTransform.size());
+					}
-							for(int i=0; i<max; i+=6+localTransform.size())
+
+					// calibration
+					if(uStrNumCmp(_version, "0.10.0") >= 0)
+					{
+						data = sqlite3_column_blob(ppStmt, index);
+						dataSize = sqlite3_column_bytes(ppStmt, index++);
+						// multi-cameras [fx,fy,cx,cy,[width,height],local_transform, ... ,fx,fy,cx,cy,[width,height],local_transform] (4or6+12)*float * numCameras
+						// stereo [fx, fy, cx, cy, baseline, local_transform] (5+12)*float
+						if(dataSize > 0 && data)
+						{
+							float * dataFloat = (float*)data;
+							if(uStrNumCmp(_version, "0.11.2") >= 0 &&
+							   (unsigned int)dataSize % (6+localTransform.size())*sizeof(float) == 0)
 							{
-								// Reinitialize to a new Transform, to avoid copying in the same memory than the previous one
+								int cameraCount = dataSize / ((6+localTransform.size())*sizeof(float));
-								localTransform = Transform::getIdentity();
+								UDEBUG("Loading calibration for %d cameras (%d bytes)", cameraCount, dataSize);
-								memcpy(localTransform.data(), dataFloat+i+6, localTransform.size()*sizeof(float));
+								int max = cameraCount*(6+localTransform.size());
-								models.push_back(CameraModel(
+								for(int i=0; i<max; i+=6+localTransform.size())
-										(double)dataFloat[i],
+								{
-										(double)dataFloat[i+1],
+									// Reinitialize to a new Transform, to avoid copying in the same memory than the previous one
-										(double)dataFloat[i+2],
+									localTransform = Transform::getIdentity();
-										(double)dataFloat[i+3],
+									memcpy(localTransform.data(), dataFloat+i+6, localTransform.size()*sizeof(float));
-										localTransform));
+									models.push_back(CameraModel(
-								models.back().setImageSize(cv::Size(dataFloat[i+4], dataFloat[i+5]));
+											(double)dataFloat[i],
-								UDEBUG("%f %f %f %f %f %f %s", dataFloat[i], dataFloat[i+1], dataFloat[i+2],
+											(double)dataFloat[i+1],
-										dataFloat[i+3], dataFloat[i+4], dataFloat[i+5],
+											(double)dataFloat[i+2],
-										localTransform.prettyPrint().c_str());
+											(double)dataFloat[i+3],
+											localTransform));
+									models.back().setImageSize(cv::Size(dataFloat[i+4], dataFloat[i+5]));
+									UDEBUG("%f %f %f %f %f %f %s", dataFloat[i], dataFloat[i+1], dataFloat[i+2],
+											dataFloat[i+3], dataFloat[i+4], dataFloat[i+5],
+											localTransform.prettyPrint().c_str());
+								}
+							}
+							else if(uStrNumCmp(_version, "0.11.2") < 0 &&
+									(unsigned int)dataSize % (4+localTransform.size())*sizeof(float) == 0)
+							{
+								int cameraCount = dataSize / ((4+localTransform.size())*sizeof(float));
+								UDEBUG("Loading calibration for %d cameras (%d bytes)", cameraCount, dataSize);
+								int max = cameraCount*(4+localTransform.size());
+								for(int i=0; i<max; i+=4+localTransform.size())
+								{
+									// Reinitialize to a new Transform, to avoid copying in the same memory than the previous one
+									localTransform = Transform::getIdentity();
+									memcpy(localTransform.data(), dataFloat+i+4, localTransform.size()*sizeof(float));
+									models.push_back(CameraModel(
+											(double)dataFloat[i],
+											(double)dataFloat[i+1],
+											(double)dataFloat[i+2],
+											(double)dataFloat[i+3],
+											localTransform));
+								}
+							}
+							else if((unsigned int)dataSize == (5+localTransform.size())*sizeof(float))
+							{
+								UDEBUG("Loading calibration of a stereo camera");
+								memcpy(localTransform.data(), dataFloat+5, localTransform.size()*sizeof(float));
+								stereoModel = StereoCameraModel(
+										dataFloat[0],  // fx
+										dataFloat[1],  // fy
+										dataFloat[2],  // cx
+										dataFloat[3],  // cy
+										dataFloat[4], // baseline
+										localTransform);
+							}
+							else
+							{
+								UFATAL("Wrong format of the Data.calibration field (size=%d bytes)", dataSize);
 							}
 						}
-						else if(uStrNumCmp(_version, "0.11.2") < 0 &&
+
-								(unsigned int)dataSize % (4+localTransform.size())*sizeof(float) == 0)
+					}
+					else if(uStrNumCmp(_version, "0.7.0") >= 0)
+					{
+						double fx = sqlite3_column_double(ppStmt, index++);
+						double fyOrBaseline = sqlite3_column_double(ppStmt, index++);
+						double cx = sqlite3_column_double(ppStmt, index++);
+						double cy = sqlite3_column_double(ppStmt, index++);
+						if(fyOrBaseline < 1.0)
 						{
-							int cameraCount = dataSize / ((4+localTransform.size())*sizeof(float));
+							//it is a baseline
-							UDEBUG("Loading calibration for %d cameras (%d bytes)", cameraCount, dataSize);
+							stereoModel = StereoCameraModel(fx,fx,cx,cy,fyOrBaseline, localTransform);
-							int max = cameraCount*(4+localTransform.size());
-							for(int i=0; i<max; i+=4+localTransform.size())
-							{
-								// Reinitialize to a new Transform, to avoid copying in the same memory than the previous one
-								localTransform = Transform::getIdentity();
-								memcpy(localTransform.data(), dataFloat+i+4, localTransform.size()*sizeof(float));
-								models.push_back(CameraModel(
-										(double)dataFloat[i],
-										(double)dataFloat[i+1],
-										(double)dataFloat[i+2],
-										(double)dataFloat[i+3],
-										localTransform));
-							}
-						}
-						else if((unsigned int)dataSize == (5+localTransform.size())*sizeof(float))
-						{
-							UDEBUG("Loading calibration of a stereo camera");
-							memcpy(localTransform.data(), dataFloat+5, localTransform.size()*sizeof(float));
-							stereoModel = StereoCameraModel(
-									dataFloat[0],  // fx
-									dataFloat[1],  // fy
-									dataFloat[2],  // cx
-									dataFloat[3],  // cy
-									dataFloat[4], // baseline
-									localTransform);
 						}
 						else
 						{
-							UFATAL("Wrong format of the Data.calibration field (size=%d bytes)", dataSize);
+							models.push_back(CameraModel(fx, fyOrBaseline, cx, cy, localTransform));
 						}
 					}
-
-				}
-				else if(uStrNumCmp(_version, "0.7.0") >= 0)
-				{
-					double fx = sqlite3_column_double(ppStmt, index++);
-					double fyOrBaseline = sqlite3_column_double(ppStmt, index++);
-					double cx = sqlite3_column_double(ppStmt, index++);
-					double cy = sqlite3_column_double(ppStmt, index++);
-					if(fyOrBaseline < 1.0)
-					{
-						//it is a baseline
-						stereoModel = StereoCameraModel(fx,fx,cx,cy,fyOrBaseline, localTransform);
-					}
 					else
 					{
-						models.push_back(CameraModel(fx, fyOrBaseline, cx, cy, localTransform));
+						float depthConstant = sqlite3_column_double(ppStmt, index++);
+						float fx = 1.0f/depthConstant;
+						float fy = 1.0f/depthConstant;
+						float cx = 0.0f;
+						float cy = 0.0f;
+						models.push_back(CameraModel(fx, fy, cx, cy, localTransform));
 					}
 				}
-				else
-				{
-					float depthConstant = sqlite3_column_double(ppStmt, index++);
-					float fx = 1.0f/depthConstant;
-					float fy = 1.0f/depthConstant;
-					float cx = 0.0f;
-					float cy = 0.0f;
-					models.push_back(CameraModel(fx, fy, cx, cy, localTransform));
-				}
 
 				int laserScanMaxPts = 0;
-				if(uStrNumCmp(_version, "0.8.11") >= 0)
-				{
-					laserScanMaxPts = sqlite3_column_int(ppStmt, index++);
-				}
-
 				float laserScanMaxRange = 0.0f;
-				if(uStrNumCmp(_version, "0.10.7") >= 0)
+				Transform scanLocalTransform = Transform::getIdentity();
+				if(uStrNumCmp(_version, "0.11.10") < 0 || scan)
 				{
-					laserScanMaxRange = sqlite3_column_int(ppStmt, index++);
+					// scan_info
-				}
+					if(uStrNumCmp(_version, "0.11.10") >= 0)
+					{
+						data = sqlite3_column_blob(ppStmt, index);
+						dataSize = sqlite3_column_bytes(ppStmt, index++);
 
-				data = sqlite3_column_blob(ppStmt, index);
+						if(dataSize > 0 && data)
-				dataSize = sqlite3_column_bytes(ppStmt, index++);
+						{
-				//Create the laserScan
+							float * dataFloat = (float*)data;
-				if(dataSize>4 && data)
+							memcpy(scanLocalTransform.data(), dataFloat+2, scanLocalTransform.size()*sizeof(float));
-				{
+							laserScanMaxPts = (int)dataFloat[0];
-					scanCompressed = cv::Mat(1, dataSize, CV_8UC1, (void *)data).clone(); // depth2d
+							laserScanMaxRange = dataFloat[1];
-				}
+						}
+					}
+					else
+					{
+						if(uStrNumCmp(_version, "0.8.11") >= 0)
+						{
+							laserScanMaxPts = sqlite3_column_int(ppStmt, index++);
+						}
+
+						if(uStrNumCmp(_version, "0.10.7") >= 0)
+						{
+							laserScanMaxRange = sqlite3_column_int(ppStmt, index++);
+						}
+					}
 
-				if(uStrNumCmp(_version, "0.8.8") >= 0)
-				{
 					data = sqlite3_column_blob(ppStmt, index);
 					dataSize = sqlite3_column_bytes(ppStmt, index++);
-					//Create the userData
+					//Create the laserScan
 					if(dataSize>4 && data)
 					{
-						if(uStrNumCmp(_version, "0.10.1") >= 0)
+						scanCompressed = cv::Mat(1, dataSize, CV_8UC1, (void *)data).clone(); // depth2d
+					}
+				}
+
+				if(uStrNumCmp(_version, "0.11.10") < 0 || userData)
+				{
+					if(uStrNumCmp(_version, "0.8.8") >= 0)
+					{
+						data = sqlite3_column_blob(ppStmt, index);
+						dataSize = sqlite3_column_bytes(ppStmt, index++);
+						//Create the userData
+						if(dataSize>4 && data)
 						{
-							userDataCompressed = cv::Mat(1, dataSize, CV_8UC1, (void *)data).clone(); // userData
+							if(uStrNumCmp(_version, "0.10.1") >= 0)
-						}
+							{
-						else
+								userDataCompressed = cv::Mat(1, dataSize, CV_8UC1, (void *)data).clone(); // userData
-						{
+							}
-							// compress data (set uncompressed data to signed to make difference with compressed type)
+							else
-							userDataCompressed = compressData2(cv::Mat(1, dataSize, CV_8SC1, (void *)data));
+							{
+								// compress data (set uncompressed data to signed to make difference with compressed type)
+								userDataCompressed = compressData2(cv::Mat(1, dataSize, CV_8SC1, (void *)data));
+							}
 						}
 					}
 				}
 
+				// Occupancy grid
+				cv::Mat groundCellsCompressed;
+				cv::Mat obstacleCellsCompressed;
+				float cellSize = 0.0f;
+				cv::Point3f viewPoint;
+				if(uStrNumCmp(_version, "0.11.10") >= 0 && occupancyGrid)
+				{
+					// ground
+					data = sqlite3_column_blob(ppStmt, index);
+					dataSize = sqlite3_column_bytes(ppStmt, index++);
+					if(dataSize > 0 && data)
+					{
+						groundCellsCompressed = cv::Mat(1, dataSize, CV_8UC1);
+						memcpy((void*)groundCellsCompressed.data, data, dataSize);
+					}
+
+					// obstacle
+					data = sqlite3_column_blob(ppStmt, index);
+					dataSize = sqlite3_column_bytes(ppStmt, index++);
+					if(dataSize > 0 && data)
+					{
+						obstacleCellsCompressed = cv::Mat(1, dataSize, CV_8UC1);
+						memcpy((void*)obstacleCellsCompressed.data, data, dataSize);
+					}
+
+					cellSize = sqlite3_column_double(ppStmt, index++);
+					viewPoint.x = sqlite3_column_double(ppStmt, index++);
+					viewPoint.y = sqlite3_column_double(ppStmt, index++);
+					viewPoint.z = sqlite3_column_double(ppStmt, index++);
+				}
+
+				SensorData tmp = (*iter)->sensorData();
 				if(models.size())
 				{
 					(*iter)->sensorData() = SensorData(
-							scanCompressed,
+							scan?scanCompressed:tmp.laserScanCompressed(),
-							laserScanMaxPts,
+						    scan?LaserScanInfo(laserScanMaxPts, laserScanMaxRange, scanLocalTransform):tmp.laserScanInfo(),
-							laserScanMaxRange,
+							images?imageCompressed:tmp.imageCompressed(),
-							imageCompressed,
+							images?depthOrRightCompressed:tmp.depthOrRightCompressed(),
-							depthOrRightCompressed,
+							images?models:tmp.cameraModels(),
-							models,
 							(*iter)->id(),
-							0,
+							(*iter)->getStamp(),
-							userDataCompressed);
+							userData?userDataCompressed:tmp.userDataCompressed());
 				}
 				else
 				{
 					(*iter)->sensorData() = SensorData(
-							scanCompressed,
+							scan?scanCompressed:tmp.laserScanCompressed(),
-							laserScanMaxPts,
+							scan?LaserScanInfo(laserScanMaxPts, laserScanMaxRange, scanLocalTransform):tmp.laserScanInfo(),
-							laserScanMaxRange,
+							images?imageCompressed:tmp.imageCompressed(),
-							imageCompressed,
+							images?depthOrRightCompressed:tmp.depthOrRightCompressed(),
-							depthOrRightCompressed,
+							images?stereoModel:tmp.stereoCameraModel(),
-							stereoModel,
 							(*iter)->id(),
-							0,
+							(*iter)->getStamp(),
-							userDataCompressed);
+							userData?userDataCompressed:tmp.userDataCompressed());
+				}
+				if(occupancyGrid)
+				{
+					(*iter)->sensorData().setOccupancyGrid(groundCellsCompressed, obstacleCellsCompressed, cellSize, viewPoint);
+				}
+				else
+				{
+					(*iter)->sensorData().setOccupancyGrid(tmp.gridGroundCellsCompressed(), tmp.gridObstacleCellsCompressed(), tmp.gridCellSize(), tmp.gridViewPoint());
 				}
-
 				rc = sqlite3_step(ppStmt); // next result...
 			}
 			UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
@@ -1726,7 +1832,7 @@ void DBDriverSqlite3::loadSignaturesQuery(const std::list<int> & ids, std::list<
 
 				if(uStrNumCmp(_version, "0.11.1") >= 0)
 				{
-					data = sqlite3_column_blob(ppStmt, index); // pose
+					data = sqlite3_column_blob(ppStmt, index); // ground_truth_pose
 					dataSize = sqlite3_column_bytes(ppStmt, index++);
 					if((unsigned int)dataSize == groundTruthPose.size()*sizeof(float) && data)
 					{
@@ -1901,7 +2007,6 @@ void DBDriverSqlite3::loadSignaturesQuery(const std::list<int> & ids, std::list<
 			rc = sqlite3_prepare_v2(_ppDb, query3.str().c_str(), -1, &ppStmt, 0);
 			UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
 
-			int calibrationsLoaded = 0;
 			for(std::list<Signature*>::const_iterator iter=nodes.begin(); iter!=nodes.end(); ++iter)
 			{
 				// bind id
@@ -1925,7 +2030,6 @@ void DBDriverSqlite3::loadSignaturesQuery(const std::list<int> & ids, std::list<
 					// stereo [fx, fy, cx, cy, baseline, local_transform] (5+12)*float
 					if(dataSize > 0 && data)
 					{
-						++calibrationsLoaded;
 						float * dataFloat = (float*)data;
 						if(uStrNumCmp(_version, "0.11.2") >= 0 &&
 						   (unsigned int)dataSize % (6+localTransform.size())*sizeof(float) == 0)
@@ -2003,8 +2107,7 @@ void DBDriverSqlite3::loadSignaturesQuery(const std::list<int> & ids, std::list<
 
 			ULOGGER_DEBUG("Time load %d calibrations=%fs", (int)nodes.size(), timer.ticks());
 		}
-
+	if(ids.size() != loaded)
-		if(ids.size() != loaded)
 		{
 			UERROR("Some signatures not found in database");
 		}
@@ -3017,6 +3120,7 @@ void DBDriverSqlite3::stepNode(sqlite3_stmt * ppStmt, const Signature * s) const
 		UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
 	}
 
+
 	//step
 	rc=sqlite3_step(ppStmt);
 	UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
@@ -3163,7 +3267,7 @@ void DBDriverSqlite3::stepDepth(sqlite3_stmt * ppStmt, const SensorData & sensor
 
 	if(uStrNumCmp(_version, "0.8.11") >= 0)
 	{
-		rc = sqlite3_bind_int(ppStmt, index++, sensorData.laserScanMaxPts());
+		rc = sqlite3_bind_int(ppStmt, index++, sensorData.laserScanInfo().maxPoints());
 		UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
 	}
 
@@ -3178,7 +3282,11 @@ void DBDriverSqlite3::stepDepth(sqlite3_stmt * ppStmt, const SensorData & sensor
 std::string DBDriverSqlite3::queryStepSensorData() const
 {
 	UASSERT(uStrNumCmp(_version, "0.10.0") >= 0);
-	if(uStrNumCmp(_version, "0.10.7") >= 0)
+	if(uStrNumCmp(_version, "0.11.10") >= 0)
+	{
+		return "INSERT INTO Data(id, image, depth, calibration, scan_info, scan, user_data, ground_cells, obstacle_cells, cell_size, view_point_x, view_point_y, view_point_z) VALUES(?,?,?,?,?,?,?,?,?,?,?,?,?);";
+	}
+	else if(uStrNumCmp(_version, "0.10.7") >= 0)
 	{
 		return "INSERT INTO Data(id, image, depth, calibration, scan_max_pts, scan_max_range, scan, user_data) VALUES(?,?,?,?,?,?,?,?);";
 	}
@@ -3293,15 +3401,43 @@ void DBDriverSqlite3::stepSensorData(sqlite3_stmt * ppStmt,
 	}
 	UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
 
-	// scan_max_pts
+	std::vector<float> scanInfo;
-	rc = sqlite3_bind_int(ppStmt, index++, sensorData.laserScanMaxPts());
+	if(uStrNumCmp(_version, "0.11.10") >= 0)
-	UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
-
-	// scan_max_range
-	if(uStrNumCmp(_version, "0.10.7") >= 0)
 	{
-		rc = sqlite3_bind_double(ppStmt, index++, sensorData.laserScanMaxRange());
+		if(sensorData.laserScanInfo().maxPoints() > 0 ||
+			sensorData.laserScanInfo().maxRange() > 0 ||
+			(!sensorData.laserScanInfo().localTransform().isNull() && !sensorData.laserScanInfo().localTransform().isIdentity()))
+		{
+			scanInfo.resize(2 + Transform().size());
+			scanInfo[0] = sensorData.laserScanInfo().maxPoints();
+			scanInfo[1] = sensorData.laserScanInfo().maxRange();
+			const Transform & localTransform = sensorData.laserScanInfo().localTransform();
+			memcpy(scanInfo.data()+2, localTransform.data(), localTransform.size()*sizeof(float));
+		}
+
+		if(scanInfo.size())
+		{
+			rc = sqlite3_bind_blob(ppStmt, index++, scanInfo.data(), scanInfo.size()*sizeof(float), SQLITE_STATIC);
+		}
+		else
+		{
+			rc = sqlite3_bind_null(ppStmt, index++);
+		}
 		UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
+
+	}
+	else
+	{
+		// scan_max_pts
+		rc = sqlite3_bind_int(ppStmt, index++, sensorData.laserScanInfo().maxPoints());
+		UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
+
+		// scan_max_range
+		if(uStrNumCmp(_version, "0.10.7") >= 0)
+		{
+			rc = sqlite3_bind_double(ppStmt, index++, sensorData.laserScanInfo().maxRange());
+			UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
+		}
 	}
 
 	// scan
@@ -3329,6 +3465,46 @@ void DBDriverSqlite3::stepSensorData(sqlite3_stmt * ppStmt,
 		UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
 	}
 
+	if(uStrNumCmp(_version, "0.11.10") >= 0)
+	{
+		//ground_cells
+		if(sensorData.gridGroundCellsCompressed().empty())
+		{
+			rc = sqlite3_bind_null(ppStmt, index++);
+			UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
+		}
+		else
+		{
+			// compress
+			rc = sqlite3_bind_blob(ppStmt, index++, sensorData.gridGroundCellsCompressed().data, (int)sensorData.gridGroundCellsCompressed().cols, SQLITE_STATIC);
+			UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
+		}
+
+		//obstacle_cells
+		if(sensorData.gridObstacleCellsCompressed().empty())
+		{
+			rc = sqlite3_bind_null(ppStmt, index++);
+			UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
+		}
+		else
+		{
+			rc = sqlite3_bind_blob(ppStmt, index++, sensorData.gridObstacleCellsCompressed().data, (int)sensorData.gridObstacleCellsCompressed().cols, SQLITE_STATIC);
+			UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
+		}
+
+		//cell_size
+		rc = sqlite3_bind_double(ppStmt, index++, sensorData.gridCellSize());
+		UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
+
+		//view_point
+		rc = sqlite3_bind_double(ppStmt, index++, sensorData.gridViewPoint().x);
+		UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
+		rc = sqlite3_bind_double(ppStmt, index++, sensorData.gridViewPoint().y);
+		UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
+		rc = sqlite3_bind_double(ppStmt, index++, sensorData.gridViewPoint().z);
+		UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
+}
+
 	//step
 	rc=sqlite3_step(ppStmt);
 	UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());

corelib/src/DBDriverSqlite3.h

@@ -83,7 +83,7 @@ private:
 	virtual void loadWordsQuery(const std::set<int> & wordIds, std::list<VisualWord *> & vws) const;
 	virtual void loadLinksQuery(int signatureId, std::map<int, Link> & links, Link::Type type = Link::kUndef) const;
 
-	virtual void loadNodeDataQuery(std::list<Signature *> & signatures) const;
+	virtual void loadNodeDataQuery(std::list<Signature *> & signatures, bool images=true, bool scan=true, bool userData=true, bool occupancyGrid=true) const;
 	virtual bool getCalibrationQuery(int signatureId, std::vector<CameraModel> & models, StereoCameraModel & stereoModel) const;
 	virtual bool getNodeInfoQuery(int signatureId, Transform & pose, int & mapId, int & weight, std::string & label, double & stamp, Transform & groundTruthPose) const;
 	virtual void getAllNodeIdsQuery(std::set<int> & ids, bool ignoreChildren, bool ignoreBadSignatures) const;

corelib/src/Features2d.cpp

@@ -261,78 +261,12 @@ void Feature2D::limitKeypoints(std::vector<cv::KeyPoint> & keypoints, cv::Mat &
 
 cv::Rect Feature2D::computeRoi(const cv::Mat & image, const std::string & roiRatios)
 {
-	std::list<std::string> strValues = uSplit(roiRatios, ' ');
+	return util2d::computeRoi(image, roiRatios);
-	if(strValues.size() != 4)
-	{
-		UERROR("The number of values must be 4 (roi=\"%s\")", roiRatios.c_str());
-	}
-	else
-	{
-		std::vector<float> values(4);
-		unsigned int i=0;
-		for(std::list<std::string>::iterator iter = strValues.begin(); iter!=strValues.end(); ++iter)
-		{
-			values[i] = uStr2Float(*iter);
-			++i;
-		}
-
-		if(values[0] >= 0 && values[0] < 1 && values[0] < 1.0f-values[1] &&
-			values[1] >= 0 && values[1] < 1 && values[1] < 1.0f-values[0] &&
-			values[2] >= 0 && values[2] < 1 && values[2] < 1.0f-values[3] &&
-			values[3] >= 0 && values[3] < 1 && values[3] < 1.0f-values[2])
-		{
-			return computeRoi(image, values);
-		}
-		else
-		{
-			UERROR("The roi ratios are not valid (roi=\"%s\")", roiRatios.c_str());
-		}
-	}
-	return cv::Rect();
 }
 
 cv::Rect Feature2D::computeRoi(const cv::Mat & image, const std::vector<float> & roiRatios)
 {
-	if(!image.empty() && roiRatios.size() == 4)
+	return util2d::computeRoi(image, roiRatios);
-	{
-		float width = image.cols;
-		float height = image.rows;
-		cv::Rect roi(0, 0, width, height);
-		UDEBUG("roi ratios = %f, %f, %f, %f", roiRatios[0],roiRatios[1],roiRatios[2],roiRatios[3]);
-		UDEBUG("roi = %d, %d, %d, %d", roi.x, roi.y, roi.width, roi.height);
-
-		//left roi
-		if(roiRatios[0] > 0 && roiRatios[0] < 1 - roiRatios[1])
-		{
-			roi.x = width * roiRatios[0];
-		}
-
-		//right roi
-		if(roiRatios[1] > 0 && roiRatios[1] < 1 - roiRatios[0])
-		{
-			roi.width -= width * roiRatios[1] + width * roiRatios[0];
-		}
-
-		//top roi
-		if(roiRatios[2] > 0 && roiRatios[2] < 1 - roiRatios[3])
-		{
-			roi.y = height * roiRatios[2];
-		}
-
-		//bottom roi
-		if(roiRatios[3] > 0 && roiRatios[3] < 1 - roiRatios[2])
-		{
-			roi.height -= height * roiRatios[3] + height * roiRatios[2];
-		}
-		UDEBUG("roi = %d, %d, %d, %d", roi.x, roi.y, roi.width, roi.height);
-
-		return roi;
-	}
-	else
-	{
-		UERROR("Image is null or _roiRatios(=%d) != 4", roiRatios.size());
-		return cv::Rect();
-	}
 }
 
 /////////////////////

corelib/src/Memory.cpp

@@ -57,10 +57,9 @@ 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>
+#include <rtabmap/core/OccupancyGrid.h>
 
 namespace rtabmap {
 
@@ -92,7 +91,7 @@ Memory::Memory(const ParametersMap & parameters) :
 	_rehearsalMaxAngle(Parameters::defaultRGBDAngularUpdate()),
 	_rehearsalWeightIgnoredWhileMoving(Parameters::defaultMemRehearsalWeightIgnoredWhileMoving()),
 	_useOdometryFeatures(Parameters::defaultMemUseOdomFeatures()),
-	_createOccupancyGrid(Parameters::defaultMemCreateOccupancyGrid()),
+	_createOccupancyGrid(Parameters::defaultRGBDCreateOccupancyGrid()),
 	_idCount(kIdStart),
 	_idMapCount(kIdStart),
 	_lastSignature(0),
@@ -109,7 +108,7 @@ Memory::Memory(const ParametersMap & parameters) :
 	_vwd = new VWDictionary(parameters);
 	_registrationPipeline = Registration::create(parameters);
 	_registrationIcp = new RegistrationIcp(parameters);
-	_occupancy = new Occupancy(parameters);
+	_occupancy = new OccupancyGrid(parameters);
 	this->parseParameters(parameters);
 }
 
@@ -413,7 +412,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);
+	Parameters::parse(parameters, Parameters::kRGBDCreateOccupancyGrid(), _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());
@@ -2062,7 +2061,7 @@ void Memory::removeRawData(int id, bool image, bool scan, bool userData)
 		}
 		if(scan && !_registrationPipeline->isScanRequired())
 		{
-			s->sensorData().setLaserScanRaw(cv::Mat(), s->sensorData().laserScanMaxPts(), s->sensorData().laserScanMaxRange());
+			s->sensorData().setLaserScanRaw(cv::Mat(), s->sensorData().laserScanInfo());
 		}
 		if(userData && !_registrationPipeline->isUserDataRequired())
 		{
@@ -2305,7 +2304,9 @@ Transform Memory::computeIcpTransformMulti(
 				{
 					cv::Mat scan;
 					s->sensorData().uncompressData(0, 0, &scan);
-					pcl::PointCloud<pcl::PointXYZ>::Ptr cloud = util3d::laserScanToPointCloud(scan, toPose.inverse() * iter->second);
+					pcl::PointCloud<pcl::PointXYZ>::Ptr cloud = util3d::laserScanToPointCloud(
+							scan,
+							s->sensorData().laserScanInfo().localTransform() * toPose.inverse() * iter->second);
 					if(scan.cols > maxPoints)
 					{
 						maxPoints = scan.cols;
@@ -2320,7 +2321,12 @@ Transform Memory::computeIcpTransformMulti(
 		}
 		if(assembledToClouds->size())
 		{
-			assembledData.setLaserScanRaw(util3d::laserScanFromPointCloud(*assembledToClouds, Transform()), fromS->sensorData().laserScanMaxPts()?fromS->sensorData().laserScanMaxPts():maxPoints, fromS->sensorData().laserScanMaxRange());
+			assembledData.setLaserScanRaw(
+					util3d::laserScanFromPointCloud(*assembledToClouds),
+					LaserScanInfo(
+							fromS->sensorData().laserScanInfo().maxPoints()?fromS->sensorData().laserScanInfo().maxPoints():maxPoints,
+							fromS->sensorData().laserScanInfo().maxRange(),
+							Transform::getIdentity())); // scans are in base frame
 		}
 
 		Transform guess = poses.at(fromId).inverse() * poses.at(toId);
@@ -2985,52 +2991,23 @@ void Memory::getNodeCalibration(int nodeId,
 	}
 }
 
-SensorData Memory::getSignatureDataConst(int locationId) const
+SensorData Memory::getSignatureDataConst(int locationId,
+		bool images, bool scan, bool userData, bool occupancyGrid) const
 {
 	UDEBUG("");
 	SensorData r;
 	const Signature * s = this->getSignature(locationId);
-	if(s && !s->sensorData().imageCompressed().empty())
+	if(s && (!s->sensorData().imageCompressed().empty() ||
+			!s->sensorData().laserScanCompressed().empty() ||
+			!s->sensorData().userDataCompressed().empty() ||
+			s->sensorData().gridCellSize() != 0.0f))
 	{
 		r = s->sensorData();
 	}
 	else if(_dbDriver)
 	{
 		// load from database
-		if(s)
+		_dbDriver->getNodeData(locationId, r, images, scan, userData, occupancyGrid);
-		{
-			std::list<Signature*> signatures;
-			Signature tmp = *s;
-			signatures.push_back(&tmp);
-			_dbDriver->loadNodeData(signatures);
-			r = tmp.sensorData();
-		}
-		else
-		{
-			std::list<int> ids;
-			ids.push_back(locationId);
-			std::list<Signature*> signatures;
-			std::set<int> loadedFromTrash;
-			_dbDriver->loadSignatures(ids, signatures, &loadedFromTrash);
-			if(signatures.size())
-			{
-				Signature * sTmp = signatures.front();
-				if(sTmp->sensorData().imageCompressed().empty())
-				{
-					_dbDriver->loadNodeData(signatures);
-				}
-				r = sTmp->sensorData();
-				if(loadedFromTrash.size())
-				{
-					//put it back to trash
-					_dbDriver->asyncSave(sTmp);
-				}
-				else
-				{
-					delete sTmp;
-				}
-			}
-		}
 	}
 
 	return r;
@@ -3502,7 +3479,7 @@ Signature * Memory::createSignature(const SensorData & data, const Transform & p
 
 	// downsampling the laser scan?
 	cv::Mat laserScan = data.laserScanRaw();
-	int maxLaserScanMaxPts = data.laserScanMaxPts();
+	int maxLaserScanMaxPts = data.laserScanInfo().maxPoints();
 	if(!laserScan.empty() && _laserScanDownsampleStepSize > 1)
 	{
 		laserScan = util3d::downsample(laserScan, _laserScanDownsampleStepSize);
@@ -3554,8 +3531,7 @@ Signature * Memory::createSignature(const SensorData & data, const Transform & p
 			stereoCameraModel.isValidForProjection()?
 				SensorData(
 						ctLaserScan.getCompressedData(),
-						maxLaserScanMaxPts,
+						LaserScanInfo(maxLaserScanMaxPts, data.laserScanInfo().maxRange(), data.laserScanInfo().localTransform()),
-						data.laserScanMaxRange(),
 						ctImage.getCompressedData(),
 						ctDepth.getCompressedData(),
 						stereoCameraModel,
@@ -3564,8 +3540,7 @@ Signature * Memory::createSignature(const SensorData & data, const Transform & p
 						ctUserData.getCompressedData()):
 				SensorData(
 						ctLaserScan.getCompressedData(),
-						maxLaserScanMaxPts,
+						LaserScanInfo(maxLaserScanMaxPts, data.laserScanInfo().maxRange(), data.laserScanInfo().localTransform()),
-						data.laserScanMaxRange(),
 						ctImage.getCompressedData(),
 						ctDepth.getCompressedData(),
 						cameraModels,
@@ -3575,12 +3550,9 @@ Signature * Memory::createSignature(const SensorData & data, const Transform & p
 	}
 	else
 	{
-		// just compress laser and user data
+		// just compress user data
-		rtabmap::CompressionThread ctLaserScan(laserScan);
 		rtabmap::CompressionThread ctUserData(data.userDataRaw());
-		ctLaserScan.start();
 		ctUserData.start();
-		ctLaserScan.join();
 		ctUserData.join();
 
 		s = new Signature(id,
@@ -3592,9 +3564,8 @@ Signature * Memory::createSignature(const SensorData & data, const Transform & p
 			data.groundTruth(),
 			stereoCameraModel.isValidForProjection()?
 				SensorData(
-						ctLaserScan.getCompressedData(),
+						cv::Mat(),
-						maxLaserScanMaxPts,
+						LaserScanInfo(),
-						data.laserScanMaxRange(),
 						cv::Mat(),
 						cv::Mat(),
 						stereoCameraModel,
@@ -3602,9 +3573,8 @@ Signature * Memory::createSignature(const SensorData & data, const Transform & p
 						0,
 						ctUserData.getCompressedData()):
 				SensorData(
-						ctLaserScan.getCompressedData(),
+						cv::Mat(),
-						maxLaserScanMaxPts,
+						LaserScanInfo(),
-						data.laserScanMaxRange(),
 						cv::Mat(),
 						cv::Mat(),
 						cameraModels,
@@ -3620,7 +3590,7 @@ Signature * Memory::createSignature(const SensorData & data, const Transform & p
 	// set raw data
 	s->sensorData().setImageRaw(image);
 	s->sensorData().setDepthOrRightRaw(depthOrRightImage);
-	s->sensorData().setLaserScanRaw(laserScan, maxLaserScanMaxPts, data.laserScanMaxRange());
+	s->sensorData().setLaserScanRaw(laserScan, LaserScanInfo(maxLaserScanMaxPts, data.laserScanInfo().maxRange(), data.laserScanInfo().localTransform()));
 	s->sensorData().setUserDataRaw(data.userDataRaw());
 
 	s->sensorData().setGroundTruth(data.groundTruth());
@@ -3634,19 +3604,20 @@ Signature * Memory::createSignature(const SensorData & data, const Transform & p
 	}
 
 	// Occupancy grid map stuff
-	/*cv::Mat ground, obstacles;
+	cv::Mat ground, obstacles;
 	float cellSize = 0.0f;
+	cv::Point3f viewPoint(0,0,0);
 	if(_createOccupancyGrid)
 	{
-		_occupancy->segment(s->sensorData(), ground, obstacles);
+		_occupancy->createLocalMap(*s, ground, obstacles, viewPoint);
 		cellSize = _occupancy->getCellSize();
 
 		t = timer.ticks();
 		if(stats) stats->addStatistic(Statistics::kTimingMemOccupancy_grid(), t*1000.0f);
-		UDEBUG("time grid map (%d) = %fs", t);
+		UDEBUG("time grid map = %fs", t);
 	}
-	s->setOccupancyGrid(ground, obstacles, cellSize);
+	s->sensorData().setOccupancyGrid(ground, obstacles, cellSize, viewPoint);
-	*/
+
 	return s;
 }
 

corelib/src/Occupancy.cpp

@@ -1,201 +0,0 @@
-/*
-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/OccupancyGrid.cpp

@@ -0,0 +1,979 @@
+/*
+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/OccupancyGrid.h>
+#include <rtabmap/core/util3d.h>
+#include <rtabmap/core/util3d_mapping.h>
+#include <rtabmap/core/util3d_transforms.h>
+#include <rtabmap/utilite/ULogger.h>
+#include <rtabmap/utilite/UConversion.h>
+#include <rtabmap/utilite/UStl.h>
+#include <rtabmap/utilite/UTimer.h>
+
+#include <pcl/io/pcd_io.h>
+
+namespace rtabmap {
+
+OccupancyGrid::OccupancyGrid(const ParametersMap & parameters) :
+	parameters_(parameters),
+	cloudDecimation_(Parameters::defaultGridDepthDecimation()),
+	cloudMaxDepth_(Parameters::defaultGridDepthMax()),
+	cloudMinDepth_(Parameters::defaultGridDepthMin()),
+	//roiRatios_(Parameters::defaultGridDepthRoiRatios()), // initialized in parseParameters()
+	scanDecimation_(Parameters::defaultGridScanDecimation()),
+	cellSize_(Parameters::defaultGridCellSize()),
+	occupancyFromCloud_(Parameters::defaultGridFromDepth()),
+	projMapFrame_(Parameters::defaultGridMapFrameProjection()),
+	maxObstacleHeight_(Parameters::defaultGridMaxObstacleHeight()),
+	normalKSearch_(Parameters::defaultGridNormalK()),
+	maxGroundAngle_(Parameters::defaultGridMaxGroundAngle()*M_PI/180.0f),
+	minClusterSize_(Parameters::defaultGridMinClusterSize()),
+	flatObstaclesDetected_(Parameters::defaultGridFlatObstacleDetected()),
+	minGroundHeight_(Parameters::defaultGridMinGroundHeight()),
+	maxGroundHeight_(Parameters::defaultGridMaxGroundHeight()),
+	normalsSegmentation_(Parameters::defaultGridNormalsSegmentation()),
+	grid3D_(Parameters::defaultGrid3D()),
+	groundIsObstacle_(Parameters::defaultGrid3DGroundIsObstacle()),
+	noiseFilteringRadius_(Parameters::defaultGridNoiseFilteringRadius()),
+	noiseFilteringMinNeighbors_(Parameters::defaultGridNoiseFilteringMinNeighbors()),
+	scan2dUnknownSpaceFilled_(Parameters::defaultGridScan2dUnknownSpaceFilled()),
+	scan2dMaxUnknownSpaceFilledRange_(Parameters::defaultGridScan2dMaxFilledRange()),
+	xMin_(0.0f),
+	yMin_(0.0f)
+{
+	this->parseParameters(parameters);
+}
+
+void OccupancyGrid::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::kGridScanDecimation(), scanDecimation_);
+	float cellSize = cellSize_;
+	if(Parameters::parse(parameters, Parameters::kGridCellSize(), cellSize))
+	{
+		this->setCellSize(cellSize);
+	}
+	Parameters::parse(parameters, Parameters::kGridMapFrameProjection(), projMapFrame_);
+	Parameters::parse(parameters, Parameters::kGridMaxObstacleHeight(), maxObstacleHeight_);
+	Parameters::parse(parameters, Parameters::kGridMinGroundHeight(), minGroundHeight_);
+	Parameters::parse(parameters, Parameters::kGridMaxGroundHeight(), maxGroundHeight_);
+	if(maxGroundHeight_ > 0 &&
+		maxObstacleHeight_ > 0 &&
+		maxObstacleHeight_ < maxGroundHeight_)
+	{
+		UWARN("\"%s\" should be lower than \"%s\", setting \"%s\" to 0 (disabled).",
+				Parameters::kGridMaxGroundHeight().c_str(),
+				Parameters::kGridMaxObstacleHeight().c_str(),
+				Parameters::kGridMaxObstacleHeight().c_str());
+		maxObstacleHeight_ = 0;
+	}
+	if(maxGroundHeight_ > 0 &&
+			minGroundHeight_ > 0 &&
+		maxGroundHeight_ < minGroundHeight_)
+	{
+		UWARN("\"%s\" should be lower than \"%s\", setting \"%s\" to 0 (disabled).",
+				Parameters::kGridMinGroundHeight().c_str(),
+				Parameters::kGridMaxGroundHeight().c_str(),
+				Parameters::kGridMinGroundHeight().c_str());
+		minGroundHeight_ = 0;
+	}
+	Parameters::parse(parameters, Parameters::kGridNormalK(), normalKSearch_);
+	if(Parameters::parse(parameters, Parameters::kGridMaxGroundAngle(), maxGroundAngle_))
+	{
+		maxGroundAngle_ *= M_PI/180.0f;
+	}
+	Parameters::parse(parameters, Parameters::kGridMinClusterSize(), minClusterSize_);
+	Parameters::parse(parameters, Parameters::kGridFlatObstacleDetected(), flatObstaclesDetected_);
+	Parameters::parse(parameters, Parameters::kGridNormalsSegmentation(), normalsSegmentation_);
+	Parameters::parse(parameters, Parameters::kGrid3D(), grid3D_);
+	Parameters::parse(parameters, Parameters::kGrid3DGroundIsObstacle(), groundIsObstacle_);
+	Parameters::parse(parameters, Parameters::kGridNoiseFilteringRadius(), noiseFilteringRadius_);
+	Parameters::parse(parameters, Parameters::kGridNoiseFilteringMinNeighbors(), noiseFilteringMinNeighbors_);
+	Parameters::parse(parameters, Parameters::kGridScan2dUnknownSpaceFilled(), scan2dUnknownSpaceFilled_);
+	Parameters::parse(parameters, Parameters::kGridScan2dMaxFilledRange(), scan2dMaxUnknownSpaceFilledRange_);
+
+	// convert ROI from string to vector
+	ParametersMap::const_iterator iter;
+	if((iter=parameters.find(Parameters::kGridDepthRoiRatios())) != parameters.end())
+	{
+		std::list<std::string> strValues = uSplit(iter->second, ' ');
+		if(strValues.size() != 4)
+		{
+			ULOGGER_ERROR("The number of values must be 4 (%s=\"%s\")", iter->first.c_str(), iter->second.c_str());
+		}
+		else
+		{
+			std::vector<float> tmpValues(4);
+			unsigned int i=0;
+			for(std::list<std::string>::iterator jter = strValues.begin(); jter!=strValues.end(); ++jter)
+			{
+				tmpValues[i] = uStr2Float(*jter);
+				++i;
+			}
+
+			if(tmpValues[0] >= 0 && tmpValues[0] < 1 && tmpValues[0] < 1.0f-tmpValues[1] &&
+				tmpValues[1] >= 0 && tmpValues[1] < 1 && tmpValues[1] < 1.0f-tmpValues[0] &&
+				tmpValues[2] >= 0 && tmpValues[2] < 1 && tmpValues[2] < 1.0f-tmpValues[3] &&
+				tmpValues[3] >= 0 && tmpValues[3] < 1 && tmpValues[3] < 1.0f-tmpValues[2])
+			{
+				roiRatios_ = tmpValues;
+			}
+			else
+			{
+				ULOGGER_ERROR("The roi ratios are not valid (%s=\"%s\")", iter->first.c_str(), iter->second.c_str());
+			}
+		}
+	}
+
+	if(maxGroundHeight_ <= 0.0f && !normalsSegmentation_)
+	{
+		UWARN("\"%s\" should be greater than 0 if not using normals "
+				"segmentation approach. Setting it to cell size (%f).",
+				Parameters::kGridMaxGroundHeight().c_str(), cellSize_);
+		maxGroundHeight_ = cellSize_;
+	}
+}
+
+void OccupancyGrid::setCellSize(float cellSize)
+{
+	UASSERT_MSG(cellSize > 0.0f, uFormat("Param name is \"%s\"", Parameters::kGridCellSize().c_str()).c_str());
+	if(cellSize_ != cellSize)
+	{
+		if(!map_.empty())
+		{
+			UWARN("Grid cell size has changed, the map is cleared!");
+		}
+		this->clear();
+		cellSize_ = cellSize;
+	}
+}
+
+void OccupancyGrid::createLocalMap(const Signature & node, cv::Mat & ground, cv::Mat & obstacles, cv::Point3f & viewPoint) const
+{
+	UDEBUG("scan channels=%d, occupancyFromCloud_=%d normalsSegmentation_=%d grid3D_=%d",
+			node.sensorData().laserScanRaw().empty()?0:node.sensorData().laserScanRaw().channels(), occupancyFromCloud_?1:0, normalsSegmentation_?1:0, grid3D_?1:0);
+
+	if(node.sensorData().laserScanRaw().channels() == 2 && !occupancyFromCloud_)
+	{
+		UDEBUG("2D laser scan");
+		//2D
+		util3d::occupancy2DFromLaserScan(
+				node.sensorData().laserScanRaw(),
+				ground,
+				obstacles,
+				cellSize_,
+				scan2dUnknownSpaceFilled_,
+				node.sensorData().laserScanInfo().maxRange()>scan2dMaxUnknownSpaceFilledRange_?scan2dMaxUnknownSpaceFilledRange_:node.sensorData().laserScanInfo().maxRange());
+	}
+	else
+	{
+		// 3D
+		pcl::IndicesPtr indices(new std::vector<int>);
+		pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud;
+		if(!occupancyFromCloud_)
+		{
+			UDEBUG("3D laser scan");
+			const Transform & t = node.sensorData().laserScanInfo().localTransform();
+			cv::Mat scan = util3d::downsample(node.sensorData().laserScanRaw(), scanDecimation_);
+			cloud = util3d::laserScanToPointCloudRGB(
+					scan,
+					t);
+
+			// update viewpoint
+			viewPoint = cv::Point3f(t.x(), t.y(), t.z());
+		}
+		else
+		{
+			UDEBUG("Depth image");
+			cloud = util3d::cloudRGBFromSensorData(
+					node.sensorData(),
+					cloudDecimation_,
+					cloudMaxDepth_,
+					cloudMinDepth_,
+					indices.get(),
+					parameters_,
+					roiRatios_);
+
+			// update viewpoint
+			if(node.sensorData().cameraModels().size())
+			{
+				// average of all local transforms
+				float sum = 0;
+				for(unsigned int i=0; i<node.sensorData().cameraModels().size(); ++i)
+				{
+					const Transform & t = node.sensorData().cameraModels()[i].localTransform();
+					if(!t.isNull())
+					{
+						viewPoint.x += t.x();
+						viewPoint.y += t.y();
+						viewPoint.z += t.z();
+						sum += 1.0f;
+					}
+				}
+				if(sum > 0.0f)
+				{
+					viewPoint.x /= sum;
+					viewPoint.y /= sum;
+					viewPoint.z /= sum;
+				}
+			}
+			else
+			{
+				const Transform & t = node.sensorData().stereoCameraModel().localTransform();
+				viewPoint = cv::Point3f(t.x(), t.y(), t.z());
+			}
+		}
+
+		if(cloud->size())
+		{
+			// voxelize to grid cell size
+			cloud = util3d::voxelize(cloud, indices, cellSize_);
+			indices->resize(cloud->size());
+			for(unsigned int i=0; i<indices->size(); ++i)
+			{
+				indices->at(i) = i;
+			}
+
+			// add pose rotation without yaw
+			float roll, pitch, yaw;
+			node.getPose().getEulerAngles(roll, pitch, yaw);
+			UDEBUG("node.getPose()=%s projMapFrame_=%d", node.getPose().prettyPrint().c_str(), projMapFrame_?1:0);
+			cloud = util3d::transformPointCloud(cloud, Transform(0,0, projMapFrame_?node.getPose().z():0, roll, pitch, 0));
+
+			if(minGroundHeight_ != 0.0f || maxObstacleHeight_ > 0.0f)
+			{
+				indices = util3d::passThrough(cloud, indices, "z",
+						minGroundHeight_!=0.0f?minGroundHeight_:std::numeric_limits<int>::min(),
+						maxObstacleHeight_>0.0f?maxObstacleHeight_:std::numeric_limits<int>::max());
+			}
+
+			pcl::IndicesPtr groundIndices, obstaclesIndices;
+
+			if(normalsSegmentation_)
+			{
+				UDEBUG("normalKSearch=%d", normalKSearch_);
+				UDEBUG("maxGroundAngle=%f", maxGroundAngle_);
+				UDEBUG("Cluster radius=%f", cellSize_*2.0f);
+				UDEBUG("flatObstaclesDetected=%d", flatObstaclesDetected_?1:0);
+				UDEBUG("maxGroundHeight=%f", maxGroundHeight_?1:0);
+				util3d::segmentObstaclesFromGround<pcl::PointXYZRGB>(
+						cloud,
+						indices,
+						groundIndices,
+						obstaclesIndices,
+						normalKSearch_,
+						maxGroundAngle_,
+						cellSize_*2.0f,
+						minClusterSize_,
+						flatObstaclesDetected_,
+						maxGroundHeight_,
+						0,
+						Eigen::Vector4f(viewPoint.x, viewPoint.y, viewPoint.z+(projMapFrame_?node.getPose().z():0), 1));
+				UDEBUG("viewPoint=%f,%f,%f", viewPoint.x, viewPoint.y, viewPoint.z+(projMapFrame_?node.getPose().z():0));
+				//UWARN("Saving ground.pcd and obstacles.pcd");
+				//pcl::io::savePCDFile("ground.pcd", *cloud, *groundIndices);
+				//pcl::io::savePCDFile("obstacles.pcd", *cloud, *obstaclesIndices);
+			}
+			else
+			{
+				UDEBUG("");
+				// passthrough filter
+				groundIndices = rtabmap::util3d::passThrough(cloud, indices, "z", minGroundHeight_<0.0f?minGroundHeight_:std::numeric_limits<int>::min(), maxGroundHeight_);
+				obstaclesIndices = rtabmap::util3d::extractIndices(cloud, groundIndices, true);
+
+			}
+			UDEBUG("groundIndices=%d obstaclesIndices=%d", (int)groundIndices->size(), (int)obstaclesIndices->size());
+
+			// Do radius filtering after voxel filtering ( a lot faster)
+			if(noiseFilteringRadius_ > 0.0 && noiseFilteringMinNeighbors_ > 0)
+			{
+				UDEBUG("");
+				if(groundIndices->size())
+				{
+					groundIndices = rtabmap::util3d::radiusFiltering(cloud, groundIndices, noiseFilteringRadius_, noiseFilteringMinNeighbors_);
+				}
+				if(obstaclesIndices->size())
+				{
+					obstaclesIndices = rtabmap::util3d::radiusFiltering(cloud, obstaclesIndices, noiseFilteringRadius_, noiseFilteringMinNeighbors_);
+				}
+
+				if(groundIndices->empty() && obstaclesIndices->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;
+				}
+			}
+
+			pcl::PointCloud<pcl::PointXYZRGB>::Ptr groundCloud(new pcl::PointCloud<pcl::PointXYZRGB>);
+			pcl::PointCloud<pcl::PointXYZRGB>::Ptr obstaclesCloud(new pcl::PointCloud<pcl::PointXYZRGB>);
+
+			if(groundIndices->size())
+			{
+				pcl::copyPointCloud(*cloud, *groundIndices, *groundCloud);
+			}
+
+			if(obstaclesIndices->size())
+			{
+				pcl::copyPointCloud(*cloud, *obstaclesIndices, *obstaclesCloud);
+			}
+
+			if(grid3D_)
+			{
+				UDEBUG("");
+				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
+			{
+				UDEBUG("groundCloud=%d, obstaclesCloud=%d", (int)groundCloud->size(), (int)obstaclesCloud->size());
+				// projection on the xy plane
+				util3d::occupancy2DFromGroundObstacles<pcl::PointXYZRGB>(
+						groundCloud,
+						obstaclesCloud,
+						ground,
+						obstacles,
+						cellSize_);
+			}
+		}
+	}
+	UDEBUG("ground=%d obstacles=%d channels=%d", ground.cols, obstacles.cols, ground.cols?ground.channels():obstacles.channels());
+}
+
+void OccupancyGrid::clear()
+{
+	cache_.clear();
+	map_ = cv::Mat();
+	mapInfo_ = cv::Mat();
+	cellCount_.clear();
+	xMin_ = 0.0f;
+	yMin_ = 0.0f;
+	addedNodes_.clear();
+}
+
+void OccupancyGrid::addToCache(
+		int nodeId,
+		const cv::Mat & ground,
+		const cv::Mat & obstacles)
+{
+	UDEBUG("nodeId=%d", nodeId);
+	cache_.insert(std::make_pair(nodeId, std::make_pair(ground, obstacles)));
+}
+
+void OccupancyGrid::update(const std::map<int, Transform> & posesIn, float minMapSize, float footprintRadius)
+{
+	UTimer timer;
+	UDEBUG("Update (poses=%d addedNodes_=%d)", (int)posesIn.size(), (int)addedNodes_.size());
+
+	float margin = cellSize_*10.0f+footprintRadius;
+
+	float minX=-minMapSize/2.0f;
+	float minY=-minMapSize/2.0f;
+	float maxX=minMapSize/2.0f;
+	float maxY=minMapSize/2.0f;
+	bool undefinedSize = minMapSize == 0.0f;
+	std::map<int, cv::Mat> emptyLocalMaps;
+	std::map<int, cv::Mat> occupiedLocalMaps;
+
+	// First, check of the graph has changed. If so, re-create the octree by moving all occupied nodes.
+	bool graphChanged = false;
+	std::map<int, Transform> transforms;
+	for(std::map<int, Transform>::iterator iter=addedNodes_.begin(); iter!=addedNodes_.end(); ++iter)
+	{
+		std::map<int, Transform>::const_iterator jter = posesIn.find(iter->first);
+		if(jter != posesIn.end())
+		{
+			UASSERT(!iter->second.isNull() && !jter->second.isNull());
+			Transform t = Transform::getIdentity();
+			if(iter->second.getDistanceSquared(jter->second) > 0.0001)
+			{
+				t = jter->second * iter->second.inverse();
+				graphChanged = true;
+			}
+			transforms.insert(std::make_pair(jter->first, t));
+
+			float x = jter->second.x();
+			float y  =jter->second.y();
+			if(undefinedSize)
+			{
+				minX = maxX = x;
+				minY = maxY = y;
+				undefinedSize = false;
+			}
+			else
+			{
+				if(minX > x)
+					minX = x;
+				else if(maxX < x)
+					maxX = x;
+
+				if(minY > y)
+					minY = y;
+				else if(maxY < y)
+					maxY = y;
+			}
+		}
+		else
+		{
+			UDEBUG("Updated pose for node %d is not found, some points may not be copied if graph has changed.", iter->first);
+		}
+	}
+
+	if(graphChanged && !map_.empty())
+	{
+		UINFO("Graph changed!");
+
+		// 1) recreate all local maps
+		UASSERT(map_.cols == mapInfo_.cols &&
+				map_.rows == mapInfo_.rows);
+		std::map<int, std::pair<int, int> > tmpIndices;
+		for(std::map<int, std::pair<int, int> >::iterator iter=cellCount_.begin(); iter!=cellCount_.end(); ++iter)
+		{
+			if(iter->second.first)
+			{
+				emptyLocalMaps.insert(std::make_pair( iter->first, cv::Mat(1, iter->second.first, CV_32FC2)));
+			}
+			if(iter->second.second)
+			{
+				occupiedLocalMaps.insert(std::make_pair( iter->first, cv::Mat(1, iter->second.second, CV_32FC2)));
+			}
+			tmpIndices.insert(std::make_pair(iter->first, std::make_pair(0,0)));
+		}
+		for(int y=1; y<map_.rows-1; ++y)
+		{
+			for(int x=1; x<map_.cols-1; ++x)
+			{
+				float * info = mapInfo_.ptr<float>(y,x);
+				int nodeId = (int)info[0];
+				if(nodeId > 0 && map_.at<char>(y,x) >= 0)
+				{
+					std::map<int, Transform>::iterator tter = transforms.find(nodeId);
+					if(tter != transforms.end() && !uContains(cache_, nodeId))
+					{
+						cv::Point3f pt(info[1], info[2], 0.0f);
+						pt = util3d::transformPoint(pt, tter->second);
+
+						if(minX > pt.x)
+							minX = pt.x;
+						else if(maxX < pt.x)
+							maxX = pt.x;
+
+						if(minY > pt.y)
+							minY = pt.y;
+						else if(maxY < pt.y)
+							maxY = pt.y;
+
+						std::map<int, std::pair<int, int> >::iterator jter = tmpIndices.find(nodeId);
+						if(map_.at<char>(y, x) == 0)
+						{
+							// ground
+							std::map<int, cv::Mat>::iterator iter = emptyLocalMaps.find(nodeId);
+							UASSERT(iter != emptyLocalMaps.end());
+							UASSERT(jter->second.first < iter->second.cols);
+							float * ptf = iter->second.ptr<float>(0,jter->second.first++);
+							ptf[0] = pt.x;
+							ptf[1] = pt.y;
+						}
+						else
+						{
+							// obstacle
+							std::map<int, cv::Mat>::iterator iter = occupiedLocalMaps.find(nodeId);
+							UASSERT(iter != occupiedLocalMaps.end());
+							UASSERT(iter!=occupiedLocalMaps.end());
+							UASSERT(jter->second.second < iter->second.cols);
+							float * ptf = iter->second.ptr<float>(0,jter->second.second++);
+							ptf[0] = pt.x;
+							ptf[1] = pt.y;
+						}
+					}
+				}
+			}
+		}
+
+		UDEBUG("min (%f,%f) max(%f,%f)", minX, minY, maxX, maxY);
+		addedNodes_.clear();
+		map_ = cv::Mat();
+		mapInfo_ = cv::Mat();
+		cellCount_.clear();
+		xMin_ = 0.0f;
+		yMin_ = 0.0f;
+	}
+	else if(!map_.empty())
+	{
+		// update
+		minX=xMin_+margin;
+		minY=yMin_+margin;
+		maxX=xMin_+float(map_.cols)*cellSize_ - margin;
+		maxY=yMin_+float(map_.rows)*cellSize_ - margin;
+		undefinedSize = false;
+	}
+
+	std::list<std::pair<int, Transform> > poses;
+	// place negative poses at the end
+	for(std::map<int, Transform>::const_reverse_iterator iter = posesIn.rbegin(); iter!=posesIn.rend(); ++iter)
+	{
+		if(iter->first>0)
+		{
+			poses.push_front(*iter);
+		}
+		else
+		{
+			poses.push_back(*iter);
+		}
+	}
+	for(std::list<std::pair<int, Transform> >::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
+	{
+		UASSERT(!iter->second.isNull());
+
+		float x = iter->second.x();
+		float y  =iter->second.y();
+		if(undefinedSize)
+		{
+			minX = maxX = x;
+			minY = maxY = y;
+			undefinedSize = false;
+		}
+		else
+		{
+			if(minX > x)
+				minX = x;
+			else if(maxX < x)
+				maxX = x;
+
+			if(minY > y)
+				minY = y;
+			else if(maxY < y)
+				maxY = y;
+		}
+	}
+
+	if(!cache_.empty())
+	{
+		for(std::list<std::pair<int, Transform> >::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
+		{
+			if(uContains(cache_, iter->first))
+			{
+				const std::pair<cv::Mat, cv::Mat> & pair = cache_.at(iter->first);
+
+				//ground
+				if(pair.first.cols)
+				{
+					if(pair.first.rows > 1 && pair.first.cols == 1)
+					{
+						UFATAL("Occupancy local maps should be 1 row and X cols! (rows=%d cols=%d)", pair.first.rows, pair.first.cols);
+					}
+					cv::Mat ground(1, pair.first.cols, CV_32FC2);
+					for(int i=0; i<ground.cols; ++i)
+					{
+						const float * vi = pair.first.ptr<float>(0,i);
+						float * vo = ground.ptr<float>(0,i);
+						cv::Point3f vt;
+						if(pair.first.channels() > 2)
+						{
+							vt = util3d::transformPoint(cv::Point3f(vi[0], vi[1], vi[2]), iter->second);
+						}
+						else
+						{
+							vt = util3d::transformPoint(cv::Point3f(vi[0], vi[1], 0), iter->second);
+						}
+						vo[0] = vt.x;
+						vo[1] = vt.y;
+						if(minX > vo[0])
+							minX = vo[0];
+						else if(maxX < vo[0])
+							maxX = vo[0];
+
+						if(minY > vo[1])
+							minY = vo[1];
+						else if(maxY < vo[1])
+							maxY = vo[1];
+					}
+					uInsert(emptyLocalMaps, std::make_pair(iter->first, ground));
+				}
+
+				//obstacles
+				if(pair.second.cols)
+				{
+					if(pair.second.rows > 1 && pair.second.cols == 1)
+					{
+						UFATAL("Occupancy local maps should be 1 row and X cols! (rows=%d cols=%d)", pair.second.rows, pair.second.cols);
+					}
+					cv::Mat obstacles(1, pair.second.cols, CV_32FC2);
+					for(int i=0; i<obstacles.cols; ++i)
+					{
+						const float * vi = pair.second.ptr<float>(0,i);
+						float * vo = obstacles.ptr<float>(0,i);
+						cv::Point3f vt;
+						if(pair.second.channels() > 2)
+						{
+							vt = util3d::transformPoint(cv::Point3f(vi[0], vi[1], vi[2]), iter->second);
+						}
+						else
+						{
+							vt = util3d::transformPoint(cv::Point3f(vi[0], vi[1], 0), iter->second);
+						}
+						vo[0] = vt.x;
+						vo[1] = vt.y;
+						if(minX > vo[0])
+							minX = vo[0];
+						else if(maxX < vo[0])
+							maxX = vo[0];
+
+						if(minY > vo[1])
+							minY = vo[1];
+						else if(maxY < vo[1])
+							maxY = vo[1];
+					}
+					uInsert(occupiedLocalMaps, std::make_pair(iter->first, obstacles));
+				}
+			}
+		}
+	}
+
+	cv::Mat map;
+	cv::Mat mapInfo;
+	if(minX != maxX && minY != maxY)
+	{
+		//Get map size
+		float xMin = minX-margin;
+		float yMin = minY-margin;
+		float xMax = maxX+margin;
+		float yMax = maxY+margin;
+		if(fabs((yMax - yMin) / cellSize_) > 99999 ||
+		   fabs((xMax - xMin) / cellSize_) > 99999)
+		{
+			UERROR("Large map size!! map min=(%f, %f) max=(%f,%f). "
+					"There's maybe an error with the poses provided! The map will not be created!",
+					xMin, yMin, xMax, yMax);
+		}
+		else
+		{
+			UDEBUG("map min=(%f, %f) odlMin(%f,%f) max=(%f,%f)", xMin, yMin, xMin_, yMin_, xMax, yMax);
+			cv::Size newMapSize((xMax - xMin) / cellSize_ + 0.5f, (yMax - yMin) / cellSize_ + 0.5f);
+			if(map_.empty())
+			{
+				UDEBUG("Map empty!");
+				map = cv::Mat::ones(newMapSize, CV_8S)*-1;
+				mapInfo = cv::Mat::zeros(newMapSize, CV_32FC3);
+			}
+			else
+			{
+				if(xMin == xMin_ && yMin == yMin_ &&
+						newMapSize.width == map_.cols &&
+						newMapSize.height == map_.rows)
+				{
+					// same map size and origin, don't do anything
+					UDEBUG("Map same size!");
+					map = map_;
+					mapInfo = mapInfo_;
+				}
+				else
+				{
+					UDEBUG("Copy map");
+					// copy the old map in the new map
+					// make sure the translation is cellSize
+					int deltaX = 0;
+					if(xMin < xMin_)
+					{
+						deltaX = (xMin_ - xMin) / cellSize_ + 1.0f;
+						xMin = xMin_-float(deltaX)*cellSize_;
+					}
+					int deltaY = 0;
+					if(yMin < yMin_)
+					{
+						deltaY = (yMin_ - yMin) / cellSize_ + 1.0f;
+						yMin = yMin_-float(deltaY)*cellSize_;
+					}
+					UDEBUG("deltaX=%d, deltaY=%d", deltaX, deltaY);
+					newMapSize.width = (xMax - xMin) / cellSize_ + 0.5f;
+					newMapSize.height = (yMax - yMin) / cellSize_ + 0.5f;
+					map = cv::Mat::ones(newMapSize, CV_8S)*-1;
+					mapInfo = cv::Mat::zeros(newMapSize, mapInfo_.type());
+					map_.copyTo(map(cv::Rect(deltaX, deltaY, map_.cols, map_.rows)));
+					mapInfo_.copyTo(mapInfo(cv::Rect(deltaX, deltaY, map_.cols, map_.rows)));
+				}
+			}
+			UASSERT(map.cols == mapInfo.cols && map.rows == mapInfo.rows);
+			UDEBUG("map %d %d", map.cols, map.rows);
+			if(poses.size())
+			{
+				UDEBUG("first pose= %d last pose=%d", poses.begin()->first, poses.rbegin()->first);
+			}
+			for(std::list<std::pair<int, Transform> >::const_iterator kter = poses.begin(); kter!=poses.end(); ++kter)
+			{
+				if(kter->first > 0)
+				{
+					uInsert(addedNodes_, *kter);
+				}
+				std::map<int, cv::Mat >::iterator iter = emptyLocalMaps.find(kter->first);
+				std::map<int, cv::Mat >::iterator jter = occupiedLocalMaps.find(kter->first);
+				std::map<int, std::pair<int, int> >::iterator cter = cellCount_.find(kter->first);
+				if(cter == cellCount_.end() && kter->first > 0)
+				{
+					cter = cellCount_.insert(std::make_pair(kter->first, std::pair<int,int>(0,0))).first;
+				}
+				if(iter!=emptyLocalMaps.end())
+				{
+					for(int i=0; i<iter->second.cols; ++i)
+					{
+						float * ptf = iter->second.ptr<float>(0,i);
+						cv::Point2i pt((ptf[0]-xMin)/cellSize_ + 0.5f, (ptf[1]-yMin)/cellSize_ + 0.5f);
+						UASSERT_MSG(pt.y < map.rows && pt.x < map.cols,
+								uFormat("%d: pt=(%d,%d) map=%dx%d rawPt=(%f,%f) xMin=%f yMin=%f channels=%dvs%d",
+										kter->first, pt.x, pt.y, map.cols, map.rows, ptf[0], ptf[1], xMin, yMin, iter->second.channels(), mapInfo.channels()-1).c_str());
+						char & value = map.at<char>(pt.y, pt.x);
+						if(value != -2)
+						{
+							float * info = mapInfo.ptr<float>(pt.y, pt.x);
+							int nodeId = (int)info[0];
+							if(value != -1)
+							{
+								if(kter->first > 0 && (kter->first <  nodeId || nodeId < 0))
+								{
+									// cannot rewrite on cells referred by more recent nodes
+									continue;
+								}
+								if(nodeId > 0)
+								{
+									std::map<int, std::pair<int, int> >::iterator eter = cellCount_.find(nodeId);
+									UASSERT_MSG(eter != cellCount_.end(), uFormat("current pose=%d nodeId=%d", kter->first, nodeId).c_str());
+									if(value == 0)
+									{
+										eter->second.first -= 1;
+									}
+									else if(value == 100)
+									{
+										eter->second.second -= 1;
+									}
+									if(kter->first < 0)
+									{
+										eter->second.first += 1;
+									}
+								}
+							}
+							if(kter->first > 0)
+							{
+								info[0] = (float)kter->first;
+								info[1] = ptf[0];
+								info[2] = ptf[1];
+								cter->second.first+=1;
+							}
+							value = 0; // free space
+						}
+					}
+				}
+
+				if(footprintRadius >= cellSize_*1.5f)
+				{
+					// place free space under the footprint of the robot
+					cv::Point2i ptBegin((kter->second.x()-footprintRadius-xMin)/cellSize_ + 0.5f, (kter->second.y()-footprintRadius-yMin)/cellSize_ + 0.5f);
+					cv::Point2i ptEnd((kter->second.x()+footprintRadius-xMin)/cellSize_ + 0.5f, (kter->second.y()+footprintRadius-yMin)/cellSize_ + 0.5f);
+					if(ptBegin.x < 0)
+						ptBegin.x = 0;
+					if(ptEnd.x >= map.cols)
+						ptEnd.x = map.cols-1;
+
+					if(ptBegin.y < 0)
+						ptBegin.y = 0;
+					if(ptEnd.y >= map.rows)
+						ptEnd.y = map.rows-1;
+					for(int i=ptBegin.x; i<ptEnd.x; ++i)
+					{
+						for(int j=ptBegin.y; j<ptEnd.y; ++j)
+						{
+							UASSERT(j < map.rows && i < map.cols);
+							char & value = map.at<char>(j, i);
+							float * info = mapInfo.ptr<float>(j, i);
+							int nodeId = (int)info[0];
+							if(value != -1)
+							{
+								if(kter->first > 0 && (kter->first <  nodeId || nodeId < 0))
+								{
+									// cannot rewrite on cells referred by more recent nodes
+									continue;
+								}
+								if(nodeId>0)
+								{
+									std::map<int, std::pair<int, int> >::iterator eter = cellCount_.find(nodeId);
+									UASSERT_MSG(eter != cellCount_.end(), uFormat("current pose=%d nodeId=%d", kter->first, nodeId).c_str());
+									if(value == 0)
+									{
+										eter->second.first -= 1;
+									}
+									else if(value == 100)
+									{
+										eter->second.second -= 1;
+									}
+									if(kter->first < 0)
+									{
+										eter->second.first += 1;
+									}
+								}
+							}
+							if(kter->first > 0)
+							{
+								info[0] = (float)kter->first;
+								info[1] = float(i) * cellSize_ + xMin_ + 0.5f;
+								info[2] = float(j) * cellSize_ + yMin_ + 0.5f;
+								cter->second.first+=1;
+							}
+							value = -2; // free space (footprint)
+						}
+					}
+				}
+
+				if(jter!=occupiedLocalMaps.end())
+				{
+					for(int i=0; i<jter->second.cols; ++i)
+					{
+						float * ptf = jter->second.ptr<float>(0,i);
+						cv::Point2i pt((ptf[0]-xMin)/cellSize_ + 0.5f, (ptf[1]-yMin)/cellSize_ + 0.5f);
+						UASSERT_MSG(pt.y < map.rows && pt.x < map.cols,
+									uFormat("%d: pt=(%d,%d) map=%dx%d rawPt=(%f,%f) xMin=%f yMin=%f channels=%dvs%d",
+											kter->first, pt.x, pt.y, map.cols, map.rows, ptf[0], ptf[1], xMin, yMin, jter->second.channels(), mapInfo.channels()-1).c_str());
+						char & value = map.at<char>(pt.y, pt.x);
+						if(value != -2)
+						{
+							float * info = mapInfo.ptr<float>(pt.y, pt.x);
+							int nodeId = (int)info[0];
+							if(value != -1)
+							{
+								if(kter->first > 0 && (kter->first <  nodeId || nodeId < 0))
+								{
+									// cannot rewrite on cells referred by more recent nodes
+									continue;
+								}
+								if(nodeId>0)
+								{
+									std::map<int, std::pair<int, int> >::iterator eter = cellCount_.find(nodeId);
+									UASSERT_MSG(eter != cellCount_.end(), uFormat("current pose=%d nodeId=%d", kter->first, nodeId).c_str());
+									if(value == 0)
+									{
+										eter->second.first -= 1;
+									}
+									else if(value == 100)
+									{
+										eter->second.second -= 1;
+									}
+									if(kter->first < 0)
+									{
+										eter->second.second += 1;
+									}
+								}
+							}
+							if(kter->first > 0)
+							{
+								info[0] = (float)kter->first;
+								info[1] = ptf[0];
+								info[2] = ptf[1];
+								cter->second.second+=1;
+							}
+							value = 100; // obstacles
+						}
+					}
+				}
+			}
+
+			// fill holes and put footprint values to empty (0)
+			//pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
+			//cloud->resize(map.rows*map.cols);
+			//int oi=0;
+			for(int i=1; i<map.rows-1; ++i)
+			{
+				for(int j=1; j<map.cols-1; ++j)
+				{
+					char & value = map.at<char>(i, j);
+					if(value == -2)
+					{
+						value = 0;
+					}
+
+					char sum =  (map.at<char>(i+1, j) != -1?1:0) +
+								(map.at<char>(i-1, j) != -1?1:0) +
+								(map.at<char>(i, j+1) != -1?1:0) +
+								(map.at<char>(i, j-1) != -1?1:0);
+					if(value == -1 && sum >=3)
+					{
+						value = 0;
+					}
+
+					//float * info = mapInfo.ptr<float>(i,j);
+					//if(info[0] > 0)
+					//{
+					//	cloud->at(oi).x = info[1];
+					//	cloud->at(oi).y = info[2];
+					//	oi++;
+					//}
+				}
+			}
+			//if(graphChanged)
+			//{
+			//	cloud->resize(oi);
+			//	pcl::io::savePCDFileBinary("mapInfo.pcd", *cloud);
+			//	UWARN("Saved mapInfo.pcd");
+			//}
+
+			map_ = map;
+			mapInfo_ = mapInfo;
+			xMin_ = xMin;
+			yMin_ = yMin;
+
+			// clean cellCount_
+			for(std::map<int, std::pair<int, int> >::iterator iter= cellCount_.begin(); iter!=cellCount_.end();)
+			{
+				UASSERT(iter->second.first >= 0 && iter->second.second >= 0);
+				if(iter->second.first == 0 && iter->second.second == 0)
+				{
+					cellCount_.erase(iter++);
+				}
+				else
+				{
+					++iter;
+				}
+			}
+		}
+	}
+
+	cache_.clear();
+
+	UDEBUG("Occupancy Grid update time = %f s", timer.ticks());
+}
+
+}

corelib/src/OctoMap.cpp

@@ -31,11 +31,13 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <rtabmap/core/util3d_transforms.h>
 #include <rtabmap/core/util3d_filtering.h>
 #include <rtabmap/core/util3d_mapping.h>
+#include <pcl/common/transforms.h>
 
 namespace rtabmap {
 
 OctoMap::OctoMap(float voxelSize) :
-		octree_(new octomap::ColorOcTree(voxelSize))
+		octree_(new octomap::ColorOcTree(voxelSize)),
+		hasColor_(false)
 {
 	UASSERT(voxelSize>0.0f);
 }
@@ -51,16 +53,32 @@ void OctoMap::clear()
 	octree_->clear();
 	occupiedCells_.clear();
 	cache_.clear();
+	cacheClouds_.clear();
+	cacheViewPoints_.clear();
 	addedNodes_.clear();
 	keyRay_ = octomap::KeyRay();
+	hasColor_ = false;
 }
 
 void OctoMap::addToCache(int nodeId,
 		pcl::PointCloud<pcl::PointXYZRGB>::Ptr & ground,
-		pcl::PointCloud<pcl::PointXYZRGB>::Ptr & obstacles)
+		pcl::PointCloud<pcl::PointXYZRGB>::Ptr & obstacles,
+		const pcl::PointXYZ & viewPoint)
 {
+	UDEBUG("nodeId=%d", nodeId);
+	cacheClouds_.insert(std::make_pair(nodeId, std::make_pair(ground, obstacles)));
+	cacheViewPoints_.insert(std::make_pair(nodeId, cv::Point3f(viewPoint.x, viewPoint.y, viewPoint.z)));
+}
+void OctoMap::addToCache(int nodeId,
+		const cv::Mat & ground,
+		const cv::Mat & obstacles,
+		const cv::Point3f & viewPoint)
+{
+	UASSERT(ground.empty() || ground.type() == CV_32FC3 || ground.type() == CV_32FC(4) || ground.type() == CV_32FC(6));
+	UASSERT(obstacles.empty() || obstacles.type() == CV_32FC3 || obstacles.type() == CV_32FC(4) || obstacles.type() == CV_32FC(6));
 	UDEBUG("nodeId=%d", nodeId);
 	cache_.insert(std::make_pair(nodeId, std::make_pair(ground, obstacles)));
+	cacheViewPoints_.insert(std::make_pair(nodeId, viewPoint));
 }
 
 void OctoMap::update(const std::map<int, Transform> & poses)
@@ -174,12 +192,19 @@ void OctoMap::update(const std::map<int, Transform> & poses)
 		for(std::list<std::pair<int, Transform> >::const_iterator iter=orderedPoses.begin(); iter!=orderedPoses.end(); ++iter)
 		{
 			std::map<int, std::pair<pcl::PointCloud<pcl::PointXYZRGB>::Ptr, pcl::PointCloud<pcl::PointXYZRGB>::Ptr> >::iterator cloudIter;
-			cloudIter = cache_.find(iter->first);
+			std::map<int, std::pair<cv::Mat, cv::Mat> >::iterator occupancyIter;
-			if(cloudIter != cache_.end())
+			std::map<int, cv::Point3f>::iterator viewPointIter;
+			cloudIter = cacheClouds_.find(iter->first);
+			occupancyIter = cache_.find(iter->first);
+			viewPointIter = cacheViewPoints_.find(iter->first);
+			if(occupancyIter != cache_.end() || cloudIter != cacheClouds_.end())
 			{
 				UDEBUG("Adding %d to octomap (resolution=%f)", iter->first, octree_->getResolution());
 
+				UASSERT(viewPointIter != cacheViewPoints_.end());
 				octomap::point3d sensorOrigin(iter->second.x(), iter->second.y(), iter->second.z());
+				sensorOrigin += octomap::point3d(viewPointIter->second.x, viewPointIter->second.y, viewPointIter->second.z);
+
 				octomap::OcTreeKey tmpKey;
 				if (!octree_->coordToKeyChecked(sensorOrigin, tmpKey)
 						|| !octree_->coordToKeyChecked(sensorOrigin, tmpKey))
@@ -190,10 +215,21 @@ void OctoMap::update(const std::map<int, Transform> & poses)
 				// instead of direct scan insertion, compute update to filter ground:
 				octomap::KeySet free_cells, occupied_cells, ground_cells;
 				// insert ground points only as free:
-				UDEBUG("%d: compute free cells (from %d ground points)", iter->first, (int)cloudIter->second.first->size());
+				unsigned int maxGroundPts = occupancyIter != cache_.end()?occupancyIter->second.first.cols:cloudIter->second.first->size();
-				for (unsigned int i=0; i<cloudIter->second.first->size(); ++i)
+				UDEBUG("%d: compute free cells (from %d ground points)", iter->first, (int)maxGroundPts);
+				Eigen::Affine3f t = iter->second.toEigen3f();
+				for (unsigned int i=0; i<maxGroundPts; ++i)
 				{
-					pcl::PointXYZRGB pt = util3d::transformPoint(cloudIter->second.first->at(i), iter->second);
+					pcl::PointXYZRGB pt;
+					if(occupancyIter != cache_.end())
+					{
+						pt = util3d::laserScanToPointRGB(occupancyIter->second.first, i);
+						pt = pcl::transformPoint(pt, t);
+					}
+					else
+					{
+						pt = pcl::transformPoint(cloudIter->second.first->at(i), t);
+					}
 
 					octomap::point3d point(pt.x, pt.y, pt.z);
 
@@ -211,6 +247,10 @@ void OctoMap::update(const std::map<int, Transform> & poses)
 						octomap::ColorOcTreeNode * n = octree_->updateNode(key, false);
 						if(n)
 						{
+							if(!hasColor_ && (pt.r !=0 || pt.g != 0 || pt.b != 0))
+							{
+								hasColor_ = true;
+							}
 							octree_->averageNodeColor(key, pt.r, pt.g, pt.b);
 							if(iter->first > 0)
 							{
@@ -226,10 +266,20 @@ void OctoMap::update(const std::map<int, Transform> & poses)
 				UDEBUG("%d: free cells = %d", iter->first, (int)free_cells.size());
 
 				// all other points: free on ray, occupied on endpoint:
-				UDEBUG("%d: compute occupied cells (from %d obstacle points)", iter->first, (int) cloudIter->second.second->size());
+				unsigned int maxObstaclePts = occupancyIter != cache_.end()?occupancyIter->second.second.cols:cloudIter->second.second->size();
-				for (unsigned int i=0; i<cloudIter->second.second->size(); ++i)
+				UDEBUG("%d: compute occupied cells (from %d obstacle points)", iter->first, (int)maxObstaclePts);
+				for (unsigned int i=0; i<maxObstaclePts; ++i)
 				{
-					pcl::PointXYZRGB pt = util3d::transformPoint(cloudIter->second.second->at(i), iter->second);
+					pcl::PointXYZRGB pt;
+					if(occupancyIter != cache_.end())
+					{
+						pt = util3d::laserScanToPointRGB(occupancyIter->second.second, i);
+						pt = pcl::transformPoint(pt, t);
+					}
+					else
+					{
+						pt = pcl::transformPoint(cloudIter->second.second->at(i), t);
+					}
 
 					octomap::point3d point(pt.x, pt.y, pt.z);
 
@@ -247,6 +297,10 @@ void OctoMap::update(const std::map<int, Transform> & poses)
 						octomap::ColorOcTreeNode * n = octree_->updateNode(key, true);
 						if(n)
 						{
+							if(!hasColor_ && (pt.r !=0 || pt.g != 0 || pt.b != 0))
+							{
+								hasColor_ = true;
+							}
 							octree_->averageNodeColor(key, pt.r, pt.g, pt.b);
 							if(iter->first > 0)
 							{
@@ -298,6 +352,8 @@ void OctoMap::update(const std::map<int, Transform> & poses)
 		}
 	}
 	cache_.clear();
+	cacheClouds_.clear();
+	cacheViewPoints_.clear();
 }
 
 void HSVtoRGB( float *r, float *g, float *b, float h, float s, float v )
@@ -385,7 +441,7 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr OctoMap::createCloud(
 		if(octree_->isNodeOccupied(*it))
 		{
 			octomap::point3d pt = octree_->keyToCoord(it.getKey());
-			if(octree_->getTreeDepth() == it.getDepth())
+			if(octree_->getTreeDepth() == it.getDepth() && hasColor_)
 			{
 				(*cloud)[oi]  = pcl::PointXYZRGB(it->getColor().r, it->getColor().g, it->getColor().b);
 			}
@@ -475,14 +531,14 @@ cv::Mat OctoMap::createProjectionMap(float & xMin, float & yMin, float & gridCel
 		ground = util3d::voxelize(ground, gridCellSize);
 	}
 
-	cv::Mat obstaclesMat = cv::Mat((int)obstacles->size(), 1, CV_32FC2);
+	cv::Mat obstaclesMat = cv::Mat(1, (int)obstacles->size(), CV_32FC2);
 	for(unsigned int i=0;i<obstacles->size(); ++i)
 	{
 		obstaclesMat.at<cv::Vec2f>(i)[0] = obstacles->at(i).x;
 		obstaclesMat.at<cv::Vec2f>(i)[1] = obstacles->at(i).y;
 	}
 
-	cv::Mat groundMat = cv::Mat((int)ground->size(), 1, CV_32FC2);
+	cv::Mat groundMat = cv::Mat(1, (int)ground->size(), CV_32FC2);
 	for(unsigned int i=0;i<ground->size(); ++i)
 	{
 		groundMat.at<cv::Vec2f>(i)[0] = ground->at(i).x;

corelib/src/OdometryF2F.cpp

@@ -133,7 +133,7 @@ Transform OdometryF2F::computeTransform(
 			info->words = newFrame.getWords();
 
 			info->localScanMapSize = tmpRefFrame.sensorData().laserScanRaw().cols;
-			info->localScanMap = util3d::transformLaserScan(tmpRefFrame.sensorData().laserScanRaw(), t);
+			info->localScanMap = util3d::transformLaserScan(tmpRefFrame.sensorData().laserScanRaw(), tmpRefFrame.sensorData().laserScanInfo().localTransform()*t);
 		}
 	}
 	else
@@ -168,7 +168,7 @@ Transform OdometryF2F::computeTransform(
 			if((features >= registrationPipeline_->getMinVisualCorrespondences()) &&
 			   (registrationPipeline_->getMinGeometryCorrespondencesRatio()==0.0f ||
 					   (newFrame.sensorData().laserScanRaw().cols &&
-					   (newFrame.sensorData().laserScanMaxPts() == 0 || float(newFrame.sensorData().laserScanRaw().cols)/float(newFrame.sensorData().laserScanMaxPts())>=registrationPipeline_->getMinGeometryCorrespondencesRatio()))))
+					   (newFrame.sensorData().laserScanInfo().maxPoints() == 0 || float(newFrame.sensorData().laserScanRaw().cols)/float(newFrame.sensorData().laserScanInfo().maxPoints())>=registrationPipeline_->getMinGeometryCorrespondencesRatio()))))
 			{
 				refFrame_ = newFrame;
 
@@ -196,9 +196,9 @@ Transform OdometryF2F::computeTransform(
 				{
 					UWARN("Too low scan points (%d), keeping last key frame...", newFrame.sensorData().laserScanRaw().cols);
 				}
-				else if(registrationPipeline_->getMinGeometryCorrespondencesRatio()>0.0f && newFrame.sensorData().laserScanMaxPts() != 0 && float(newFrame.sensorData().laserScanRaw().cols)/float(newFrame.sensorData().laserScanMaxPts())<registrationPipeline_->getMinGeometryCorrespondencesRatio())
+				else if(registrationPipeline_->getMinGeometryCorrespondencesRatio()>0.0f && newFrame.sensorData().laserScanInfo().maxPoints() != 0 && float(newFrame.sensorData().laserScanRaw().cols)/float(newFrame.sensorData().laserScanInfo().maxPoints())<registrationPipeline_->getMinGeometryCorrespondencesRatio())
 				{
-					UWARN("Too low scan points ratio (%d < %d), keeping last key frame...", float(newFrame.sensorData().laserScanRaw().cols)/float(newFrame.sensorData().laserScanMaxPts()), registrationPipeline_->getMinGeometryCorrespondencesRatio());
+					UWARN("Too low scan points ratio (%d < %d), keeping last key frame...", float(newFrame.sensorData().laserScanRaw().cols)/float(newFrame.sensorData().laserScanInfo().maxPoints()), registrationPipeline_->getMinGeometryCorrespondencesRatio());
 				}
 			}
 		}

corelib/src/OdometryF2M.cpp

@@ -334,7 +334,7 @@ Transform OdometryF2M::computeTransform(
 						if(lastFrame_->sensorData().laserScanRaw().cols)
 						{
 							pcl::PointCloud<pcl::PointNormal>::Ptr mapCloudNormals = util3d::laserScanToPointCloudNormal(mapScan);
-							pcl::PointCloud<pcl::PointNormal>::Ptr frameCloudNormals = util3d::laserScanToPointCloudNormal(lastFrame_->sensorData().laserScanRaw(), newFramePose);
+							pcl::PointCloud<pcl::PointNormal>::Ptr frameCloudNormals = util3d::laserScanToPointCloudNormal(lastFrame_->sensorData().laserScanRaw(), lastFrame_->sensorData().laserScanInfo().localTransform() * newFramePose);
 
 							pcl::IndicesPtr frameCloudNormalsIndices(new std::vector<int>);
 							int newPoints;
@@ -444,7 +444,7 @@ Transform OdometryF2M::computeTransform(
 					{
 						*map_ = tmpMap;
 
-						map_->sensorData().setLaserScanRaw(mapScan, 0, 0);
+						map_->sensorData().setLaserScanRaw(mapScan, LaserScanInfo(0, 0));
 						map_->setWords(mapWords);
 						map_->setWords3(mapPoints);
 						map_->setWordsDescriptors(mapDescriptors);
@@ -534,9 +534,9 @@ Transform OdometryF2M::computeTransform(
 					frameValid = true;
 					if (fixedMapPath_.empty())
 					{
-						pcl::PointCloud<pcl::PointNormal>::Ptr mapCloudNormals = util3d::laserScanToPointCloudNormal(lastFrame_->sensorData().laserScanRaw(), newFramePose);
+						pcl::PointCloud<pcl::PointNormal>::Ptr mapCloudNormals = util3d::laserScanToPointCloudNormal(lastFrame_->sensorData().laserScanRaw(), lastFrame_->sensorData().laserScanInfo().localTransform() * newFramePose);
 						scansBuffer_.push_back(std::make_pair(mapCloudNormals, pcl::IndicesPtr(new std::vector<int>)));
-						map_->sensorData().setLaserScanRaw(util3d::laserScanFromPointCloud(*mapCloudNormals), 0,0);
+						map_->sensorData().setLaserScanRaw(util3d::laserScanFromPointCloud(*mapCloudNormals), LaserScanInfo(0,0));
 					}
 				}
 				else

corelib/src/OdometryThread.cpp

@@ -97,6 +97,7 @@ void OdometryThread::mainLoop()
 		Transform pose = _odometry->process(data, &info);
 		// a null pose notify that odometry could not be computed
 		double variance = info.variance>0?info.variance:1;
+		UDEBUG("Odom pose = %s", pose.prettyPrint().c_str());
 		this->post(new OdometryEvent(data, pose, variance, variance, info));
 	}
 }

corelib/src/Parameters.cpp

@@ -220,6 +220,9 @@ const std::map<std::string, std::pair<bool, std::string> > & Parameters::getRemo
 	{
 		// removed parameters
 
+		// 0.11.10 typos
+		removedParameters_.insert(std::make_pair("Grid/FlatObstaclesDetected",    std::make_pair(true,  Parameters::kGridFlatObstacleDetected())));
+
 		// 0.11.8
 		removedParameters_.insert(std::make_pair("Reg/Force2D",                   std::make_pair(true,  Parameters::kRegForce3DoF())));
 		removedParameters_.insert(std::make_pair("OdomF2M/ScanSubstractRadius",   std::make_pair(true,  Parameters::kOdomF2MScanSubtractRadius())));
@@ -402,53 +405,65 @@ std::string Parameters::getDescription(const std::string & paramKey)
 	return description;
 }
 
-void Parameters::parse(const ParametersMap & parameters, const std::string & key, bool & value)
+bool Parameters::parse(const ParametersMap & parameters, const std::string & key, bool & value)
 {
 	ParametersMap::const_iterator iter = parameters.find(key);
 	if(iter != parameters.end())
 	{
 		value = uStr2Bool(iter->second.c_str());
+		return true;
 	}
+	return false;
 }
-void Parameters::parse(const ParametersMap & parameters, const std::string & key, int & value)
+bool Parameters::parse(const ParametersMap & parameters, const std::string & key, int & value)
 {
 	ParametersMap::const_iterator iter = parameters.find(key);
 	if(iter != parameters.end())
 	{
 		value = uStr2Int(iter->second.c_str());
+		return true;
 	}
+	return false;
 }
-void Parameters::parse(const ParametersMap & parameters, const std::string & key, unsigned int & value)
+bool Parameters::parse(const ParametersMap & parameters, const std::string & key, unsigned int & value)
 {
 	ParametersMap::const_iterator iter = parameters.find(key);
 	if(iter != parameters.end())
 	{
 		value = uStr2Int(iter->second.c_str());
+		return true;
 	}
+	return false;
 }
-void Parameters::parse(const ParametersMap & parameters, const std::string & key, float & value)
+bool Parameters::parse(const ParametersMap & parameters, const std::string & key, float & value)
 {
 	ParametersMap::const_iterator iter = parameters.find(key);
 	if(iter != parameters.end())
 	{
 		value = uStr2Float(iter->second);
+		return true;
 	}
+	return false;
 }
-void Parameters::parse(const ParametersMap & parameters, const std::string & key, double & value)
+bool Parameters::parse(const ParametersMap & parameters, const std::string & key, double & value)
 {
 	ParametersMap::const_iterator iter = parameters.find(key);
 	if(iter != parameters.end())
 	{
 		value = uStr2Double(iter->second);
+		return true;
 	}
+	return false;
 }
-void Parameters::parse(const ParametersMap & parameters, const std::string & key, std::string & value)
+bool Parameters::parse(const ParametersMap & parameters, const std::string & key, std::string & value)
 {
 	ParametersMap::const_iterator iter = parameters.find(key);
 	if(iter != parameters.end())
 	{
 		value = iter->second;
+		return true;
 	}
+	return false;
 }
 void Parameters::parse(const ParametersMap & parameters, ParametersMap & parametersOut)
 {

corelib/src/RegistrationIcp.cpp

@@ -113,10 +113,12 @@ Transform RegistrationIcp::computeTransformationImpl(
 	if(!guess.isNull() && !dataFrom.laserScanRaw().empty() && !dataTo.laserScanRaw().empty())
 	{
 		// ICP with guess transform
-		int maxLaserScansTo = dataTo.laserScanMaxPts();
+		int maxLaserScansTo = dataTo.laserScanInfo().maxPoints();
-		int maxLaserScansFrom = dataFrom.laserScanMaxPts();
+		int maxLaserScansFrom = dataFrom.laserScanInfo().maxPoints();
 		cv::Mat fromScan = dataFrom.laserScanRaw();
 		cv::Mat toScan = dataTo.laserScanRaw();
+		Transform fromLocalTransform = dataFrom.laserScanInfo().localTransform();
+		Transform toLocalTransform = dataTo.laserScanInfo().localTransform();
 		if(_downsamplingStep>1)
 		{
 			fromScan = util3d::downsample(fromScan, _downsamplingStep);
@@ -140,8 +142,8 @@ Transform RegistrationIcp::computeTransformationImpl(
 				toScan.channels() == 6)
 			{
 				//special case if we have already normals computed and there is no filtering
-				pcl::PointCloud<pcl::PointNormal>::Ptr fromCloudNormals = util3d::laserScanToPointCloudNormal(fromScan, Transform());
+				pcl::PointCloud<pcl::PointNormal>::Ptr fromCloudNormals = util3d::laserScanToPointCloudNormal(fromScan, fromLocalTransform);
-				pcl::PointCloud<pcl::PointNormal>::Ptr toCloudNormals = util3d::laserScanToPointCloudNormal(toScan, guess);
+				pcl::PointCloud<pcl::PointNormal>::Ptr toCloudNormals = util3d::laserScanToPointCloudNormal(toScan, toLocalTransform * guess);
 
 				UDEBUG("Conversion time = %f s", timer.ticks());
 				pcl::PointCloud<pcl::PointNormal>::Ptr fromCloudNormalsRegistered(new pcl::PointCloud<pcl::PointNormal>());
@@ -180,8 +182,8 @@ Transform RegistrationIcp::computeTransformationImpl(
 			}
 			else
 			{
-				pcl::PointCloud<pcl::PointXYZ>::Ptr fromCloud = util3d::laserScanToPointCloud(fromScan, Transform());
+				pcl::PointCloud<pcl::PointXYZ>::Ptr fromCloud = util3d::laserScanToPointCloud(fromScan, fromLocalTransform);
-				pcl::PointCloud<pcl::PointXYZ>::Ptr toCloud = util3d::laserScanToPointCloud(toScan, guess);
+				pcl::PointCloud<pcl::PointXYZ>::Ptr toCloud = util3d::laserScanToPointCloud(toScan, toLocalTransform * guess);
 				UDEBUG("Conversion time = %f s", timer.ticks());
 
 				pcl::PointCloud<pcl::PointXYZ>::Ptr fromCloudFiltered = fromCloud;
@@ -223,8 +225,8 @@ Transform RegistrationIcp::computeTransformationImpl(
 					fromCloudNormals = util3d::removeNaNNormalsFromPointCloud(fromCloudNormals);
 
 					// update output scans
-					fromSignature.sensorData().setLaserScanRaw(util3d::laserScanFromPointCloud(*fromCloudNormals), maxLaserScansFrom, fromSignature.sensorData().laserScanMaxRange());
+					fromSignature.sensorData().setLaserScanRaw(util3d::laserScanFromPointCloud(*fromCloudNormals, fromLocalTransform.inverse()), LaserScanInfo(maxLaserScansFrom, fromSignature.sensorData().laserScanInfo().maxRange(), fromLocalTransform));
-					toSignature.sensorData().setLaserScanRaw(util3d::laserScanFromPointCloud(*toCloudNormals, guess.inverse()), maxLaserScansTo, toSignature.sensorData().laserScanMaxRange());
+					toSignature.sensorData().setLaserScanRaw(util3d::laserScanFromPointCloud(*toCloudNormals, (toLocalTransform * guess).inverse()), LaserScanInfo(maxLaserScansTo, toSignature.sensorData().laserScanInfo().maxRange(), toLocalTransform));
 
 					UDEBUG("Compute normals time = %f s", timer.ticks());
 
@@ -259,8 +261,8 @@ Transform RegistrationIcp::computeTransformationImpl(
 					if(_voxelSize > 0.0f)
 					{
 						// update output scans
-						fromSignature.sensorData().setLaserScanRaw(util3d::laserScanFromPointCloud(*fromCloudFiltered), maxLaserScansFrom, fromSignature.sensorData().laserScanMaxRange());
+						fromSignature.sensorData().setLaserScanRaw(util3d::laserScanFromPointCloud(*fromCloudFiltered, fromLocalTransform.inverse()), LaserScanInfo(maxLaserScansFrom, fromSignature.sensorData().laserScanInfo().maxRange(), fromLocalTransform));
-						toSignature.sensorData().setLaserScanRaw(util3d::laserScanFromPointCloud(*toCloudFiltered, guess.inverse()), maxLaserScansTo, toSignature.sensorData().laserScanMaxRange());
+						toSignature.sensorData().setLaserScanRaw(util3d::laserScanFromPointCloud(*toCloudFiltered, (toLocalTransform * guess).inverse()), LaserScanInfo(maxLaserScansTo, toSignature.sensorData().laserScanInfo().maxRange(), toLocalTransform));
 					}
 
 					icpT = util3d::icp(

corelib/src/Rtabmap.cpp

@@ -413,8 +413,10 @@ void Rtabmap::parseParameters(const ParametersMap & parameters)
 	Parameters::parse(parameters, Parameters::kRGBDProximityMaxPaths(), _proximityMaxPaths);
 	Parameters::parse(parameters, Parameters::kRGBDProximityPathFilteringRadius(), _proximityFilteringRadius);
 	Parameters::parse(parameters, Parameters::kRGBDProximityPathRawPosesUsed(), _proximityRawPosesUsed);
-	Parameters::parse(parameters, Parameters::kRGBDProximityAngle(), _proximityAngle);
+	if(Parameters::parse(parameters, Parameters::kRGBDProximityAngle(), _proximityAngle))
-	_proximityAngle *= M_PI/180.0f;
+	{
+		_proximityAngle *= M_PI/180.0f;
+	}
 	Parameters::parse(parameters, Parameters::kRGBDOptimizeFromGraphEnd(), _optimizeFromGraphEnd);
 	Parameters::parse(parameters, Parameters::kRGBDOptimizeMaxError(), _optimizationMaxLinearError);
 	Parameters::parse(parameters, Parameters::kRtabmapStartNewMapOnLoopClosure(), _startNewMapOnLoopClosure);
@@ -2386,7 +2388,7 @@ bool Rtabmap::process(
 		signaturesRemoved.insert(signaturesRemoved.end(), transferred.begin(), transferred.end());
 		if(!_someNodesHaveBeenTransferred && transferred.size())
 		{
-			_someNodesHaveBeenTransferred = true; // only used to hide a warning on close ndoes immunization
+			_someNodesHaveBeenTransferred = true; // only used to hide a warning on close nodes immunization
 		}
 	}
 	_lastProcessTime = totalTime;
@@ -2502,6 +2504,7 @@ bool Rtabmap::process(
 			UINFO("Adding data %d (rgb/left=%d depth/right=%d)", lastSignatureData.id(), lastSignatureData.sensorData().imageRaw().empty()?0:1, lastSignatureData.sensorData().depthOrRightRaw().empty()?0:1);
 			signatures.insert(std::make_pair(lastSignatureData.id(), lastSignatureData));
 		}
+		UDEBUG("");
 		// Set local graph
 		std::map<int, Transform> poses;
 		std::multimap<int, Link> constraints;
@@ -2516,6 +2519,7 @@ bool Rtabmap::process(
 			poses = _optimizedPoses;
 			constraints = _constraints;
 		}
+		UDEBUG("Get all node infos...");
 		for(std::map<int, Transform>::iterator iter=poses.begin(); iter!=poses.end(); ++iter)
 		{
 			Transform odomPose;
@@ -2540,15 +2544,18 @@ bool Rtabmap::process(
 		statistics_.setSignatures(signatures);
 		statistics_.addStatistic(Statistics::kMemoryLocal_graph_size(), poses.size());
 		localGraphSize = (int)poses.size();
+		UDEBUG("");
 	}
 
 	//Start trashing
+	UDEBUG("Empty trash...");
 	_memory->emptyTrash();
 
 	// Log info...
 	// TODO : use a specific class which will handle the RtabmapEvent
 	if(_foutFloat && _foutInt)
 	{
+		UDEBUG("Logging...");
 		std::string logF = uFormat("%f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f\n",
 									totalTime,
 									timeMemoryUpdate,

corelib/src/SensorData.cpp

@@ -38,8 +38,7 @@ namespace rtabmap
 SensorData::SensorData() :
 		_id(0),
 		_stamp(0.0),
-		_laserScanMaxPts(0),
+		_cellSize(0.0f)
-		_laserScanMaxRange(0.0f)
 {
 }
 
@@ -51,8 +50,7 @@ SensorData::SensorData(
 		const cv::Mat & userData) :
 		_id(id),
 		_stamp(stamp),
-		_laserScanMaxPts(0),
+		_cellSize(0.0f)
-		_laserScanMaxRange(0.0f)
 {
 	if(image.rows == 1)
 	{
@@ -85,9 +83,8 @@ SensorData::SensorData(
 		const cv::Mat & userData) :
 		_id(id),
 		_stamp(stamp),
-		_laserScanMaxPts(0),
+		_cameraModels(std::vector<CameraModel>(1, cameraModel)),
-		_laserScanMaxRange(0.0f),
+		_cellSize(0.0f)
-		_cameraModels(std::vector<CameraModel>(1, cameraModel))
 {
 	if(image.rows == 1)
 	{
@@ -121,9 +118,8 @@ SensorData::SensorData(
 		const cv::Mat & userData) :
 		_id(id),
 		_stamp(stamp),
-		_laserScanMaxPts(0),
+		_cameraModels(std::vector<CameraModel>(1, cameraModel)),
-		_laserScanMaxRange(0.0f),
+		_cellSize(0.0f)
-		_cameraModels(std::vector<CameraModel>(1, cameraModel))
 {
 	if(rgb.rows == 1)
 	{
@@ -162,8 +158,7 @@ SensorData::SensorData(
 // RGB-D constructor + laser scan
 SensorData::SensorData(
 		const cv::Mat & laserScan,
-		int laserScanMaxPts,
+		const LaserScanInfo & laserScanInfo,
-		float laserScanMaxRange,
 		const cv::Mat & rgb,
 		const cv::Mat & depth,
 		const CameraModel & cameraModel,
@@ -172,9 +167,9 @@ SensorData::SensorData(
 		const cv::Mat & userData) :
 		_id(id),
 		_stamp(stamp),
-		_laserScanMaxPts(laserScanMaxPts),
+		_cameraModels(std::vector<CameraModel>(1, cameraModel)),
-		_laserScanMaxRange(laserScanMaxRange),
+		_laserScanInfo(laserScanInfo),
-		_cameraModels(std::vector<CameraModel>(1, cameraModel))
+		_cellSize(0.0f)
 {
 	if(rgb.rows == 1)
 	{
@@ -199,7 +194,7 @@ SensorData::SensorData(
 		_depthOrRightRaw = depth;
 	}
 
-	if(laserScan.type() == CV_32FC2 || laserScan.type() == CV_32FC3 || laserScan.type() == CV_32FC(6))
+	if(laserScan.type() == CV_32FC2 || laserScan.type() == CV_32FC3  || laserScan.type() == CV_32FC(4) || laserScan.type() == CV_32FC(6))
 	{
 		_laserScanRaw = laserScan;
 	}
@@ -229,9 +224,8 @@ SensorData::SensorData(
 		const cv::Mat & userData) :
 		_id(id),
 		_stamp(stamp),
-		_laserScanMaxPts(0),
+		_cameraModels(cameraModels),
-		_laserScanMaxRange(0.0f),
+		_cellSize(0.0f)
-		_cameraModels(cameraModels)
 {
 	if(rgb.rows == 1)
 	{
@@ -269,8 +263,7 @@ SensorData::SensorData(
 // Multi-cameras RGB-D constructor + laser scan
 SensorData::SensorData(
 		const cv::Mat & laserScan,
-		int laserScanMaxPts,
+		const LaserScanInfo & laserScanInfo,
-		float laserScanMaxRange,
 		const cv::Mat & rgb,
 		const cv::Mat & depth,
 		const std::vector<CameraModel> & cameraModels,
@@ -279,9 +272,9 @@ SensorData::SensorData(
 		const cv::Mat & userData) :
 		_id(id),
 		_stamp(stamp),
-		_laserScanMaxPts(laserScanMaxPts),
+		_cameraModels(cameraModels),
-		_laserScanMaxRange(laserScanMaxRange),
+		_laserScanInfo(laserScanInfo),
-		_cameraModels(cameraModels)
+		_cellSize(0.0f)
 {
 	if(rgb.rows == 1)
 	{
@@ -306,7 +299,7 @@ SensorData::SensorData(
 		_depthOrRightRaw = depth;
 	}
 
-	if(laserScan.type() == CV_32FC2 || laserScan.type() == CV_32FC3 || laserScan.type() == CV_32FC(6))
+	if(laserScan.type() == CV_32FC2 || laserScan.type() == CV_32FC3 || laserScan.type() == CV_32FC(4) || laserScan.type() == CV_32FC(6))
 	{
 		_laserScanRaw = laserScan;
 	}
@@ -336,9 +329,8 @@ SensorData::SensorData(
 		const cv::Mat & userData):
 		_id(id),
 		_stamp(stamp),
-		_laserScanMaxPts(0),
+		_stereoCameraModel(cameraModel),
-		_laserScanMaxRange(0.0f),
+		_cellSize(0.0f)
-		_stereoCameraModel(cameraModel)
 {
 	if(left.rows == 1)
 	{
@@ -378,8 +370,7 @@ SensorData::SensorData(
 // Stereo constructor + 2d laser scan
 SensorData::SensorData(
 		const cv::Mat & laserScan,
-		int laserScanMaxPts,
+		const LaserScanInfo & laserScanInfo,
-		float laserScanMaxRange,
 		const cv::Mat & left,
 		const cv::Mat & right,
 		const StereoCameraModel & cameraModel,
@@ -388,9 +379,9 @@ SensorData::SensorData(
 		const cv::Mat & userData) :
 		_id(id),
 		_stamp(stamp),
-		_laserScanMaxPts(laserScanMaxPts),
+		_stereoCameraModel(cameraModel),
-		_laserScanMaxRange(laserScanMaxRange),
+		_laserScanInfo(laserScanInfo),
-		_stereoCameraModel(cameraModel)
+		_cellSize(0.0f)
 {
 	if(left.rows == 1)
 	{
@@ -414,7 +405,7 @@ SensorData::SensorData(
 		_depthOrRightRaw = right;
 	}
 
-	if(laserScan.type() == CV_32FC2 || laserScan.type() == CV_32FC3 || laserScan.type() == CV_32FC(6))
+	if(laserScan.type() == CV_32FC2 || laserScan.type() == CV_32FC3 || laserScan.type() == CV_32FC(4) || laserScan.type() == CV_32FC(6))
 	{
 		_laserScanRaw = laserScan;
 	}
@@ -480,18 +471,97 @@ void SensorData::setUserData(const cv::Mat & userData)
 	}
 }
 
+void SensorData::setOccupancyGrid(
+			const cv::Mat & ground,
+			const cv::Mat & obstacles,
+			float cellSize,
+			const cv::Point3f & viewPoint)
+{
+	UDEBUG("ground=%d obstacles=%d", ground.cols, obstacles.cols);
+	if((!ground.empty() && (!_groundCellsCompressed.empty() || !_groundCellsRaw.empty())) ||
+	   (!obstacles.empty() && (!_obstacleCellsCompressed.empty() || !_obstacleCellsRaw.empty())))
+	{
+		UWARN("Occupancy grid cannot be overwritten! id=%d", this->id());
+		return;
+	}
+
+	_groundCellsRaw = cv::Mat();
+	_groundCellsCompressed = cv::Mat();
+	_obstacleCellsRaw = cv::Mat();
+	_obstacleCellsCompressed = cv::Mat();
+
+	CompressionThread ctGround(ground);
+	CompressionThread ctObstacles(obstacles);
+
+	if(!ground.empty())
+	{
+		if(ground.type() == CV_32FC2 || ground.type() == CV_32FC3 || ground.type() == CV_32FC(4) || ground.type() == CV_32FC(6))
+		{
+			_groundCellsRaw = ground;
+			ctGround.start();
+		}
+		else if(ground.type() == CV_8UC1)
+		{
+			UASSERT(ground.type() == CV_8UC1); // Bytes
+			_groundCellsCompressed = ground;
+		}
+	}
+	if(!obstacles.empty())
+	{
+		if(obstacles.type() == CV_32FC2 || obstacles.type() == CV_32FC3 || obstacles.type() == CV_32FC(4) || obstacles.type() == CV_32FC(6))
+		{
+			_obstacleCellsRaw = obstacles;
+			ctObstacles.start();
+		}
+		else if(obstacles.type() == CV_8UC1)
+		{
+			UASSERT(obstacles.type() == CV_8UC1); // Bytes
+			_obstacleCellsCompressed = obstacles;
+		}
+	}
+	ctGround.join();
+	ctObstacles.join();
+	if(!_groundCellsRaw.empty())
+	{
+		_groundCellsCompressed = ctGround.getCompressedData();
+	}
+	if(!_obstacleCellsRaw.empty())
+	{
+		_obstacleCellsCompressed = ctObstacles.getCompressedData();
+	}
+
+	_cellSize = cellSize;
+	_viewPoint = viewPoint;
+}
+
 void SensorData::uncompressData()
 {
-	cv::Mat tmpA, tmpB, tmpC, tmpD;
+	cv::Mat tmpA, tmpB, tmpC, tmpD, tmpE, tmpF;
 	uncompressData(_imageCompressed.empty()?0:&tmpA,
 				_depthOrRightCompressed.empty()?0:&tmpB,
 				_laserScanCompressed.empty()?0:&tmpC,
-				_userDataCompressed.empty()?0:&tmpD);
+				_userDataCompressed.empty()?0:&tmpD,
+				_groundCellsCompressed.empty()?0:&tmpE,
+				_obstacleCellsCompressed.empty()?0:&tmpF);
 }
 
-void SensorData::uncompressData(cv::Mat * imageRaw, cv::Mat * depthRaw, cv::Mat * laserScanRaw, cv::Mat * userDataRaw)
+void SensorData::uncompressData(
+		cv::Mat * imageRaw,
+		cv::Mat * depthRaw,
+		cv::Mat * laserScanRaw,
+		cv::Mat * userDataRaw,
+		cv::Mat * groundCellsRaw,
+		cv::Mat * obstacleCellsRaw)
 {
-	uncompressDataConst(imageRaw, depthRaw, laserScanRaw, userDataRaw);
+	UDEBUG("%d", this->id());
+	uncompressDataConst(
+			imageRaw,
+			depthRaw,
+			laserScanRaw,
+			userDataRaw,
+			groundCellsRaw,
+			obstacleCellsRaw);
+
 	if(imageRaw && !imageRaw->empty() && _imageRaw.empty())
 	{
 		_imageRaw = *imageRaw;
@@ -520,9 +590,23 @@ void SensorData::uncompressData(cv::Mat * imageRaw, cv::Mat * depthRaw, cv::Mat
 	{
 		_userDataRaw = *userDataRaw;
 	}
+	if(groundCellsRaw && !groundCellsRaw->empty() && _groundCellsRaw.empty())
+	{
+		_groundCellsRaw = *groundCellsRaw;
+	}
+	if(obstacleCellsRaw && !obstacleCellsRaw->empty() && _obstacleCellsRaw.empty())
+	{
+		_obstacleCellsRaw = *obstacleCellsRaw;
+	}
 }
 
-void SensorData::uncompressDataConst(cv::Mat * imageRaw, cv::Mat * depthRaw, cv::Mat * laserScanRaw, cv::Mat * userDataRaw) const
+void SensorData::uncompressDataConst(
+		cv::Mat * imageRaw,
+		cv::Mat * depthRaw,
+		cv::Mat * laserScanRaw,
+		cv::Mat * userDataRaw,
+		cv::Mat * groundCellsRaw,
+		cv::Mat * obstacleCellsRaw) const
 {
 	if(imageRaw)
 	{
@@ -540,35 +624,64 @@ void SensorData::uncompressDataConst(cv::Mat * imageRaw, cv::Mat * depthRaw, cv:
 	{
 		*userDataRaw = _userDataRaw;
 	}
+	if(groundCellsRaw)
+	{
+		*groundCellsRaw = _groundCellsRaw;
+	}
+	if(obstacleCellsRaw)
+	{
+		*obstacleCellsRaw = _obstacleCellsRaw;
+	}
 	if( (imageRaw && imageRaw->empty()) ||
 		(depthRaw && depthRaw->empty()) ||
 		(laserScanRaw && laserScanRaw->empty()) ||
-		(userDataRaw && userDataRaw->empty()))
+		(userDataRaw && userDataRaw->empty()) ||
+		(groundCellsRaw && groundCellsRaw->empty()) ||
+		(obstacleCellsRaw && obstacleCellsRaw->empty()))
 	{
 		rtabmap::CompressionThread ctImage(_imageCompressed, true);
 		rtabmap::CompressionThread ctDepth(_depthOrRightCompressed, true);
 		rtabmap::CompressionThread ctLaserScan(_laserScanCompressed, false);
 		rtabmap::CompressionThread ctUserData(_userDataCompressed, false);
-		if(imageRaw && imageRaw->empty())
+		rtabmap::CompressionThread ctGroundCells(_groundCellsCompressed, false);
+		rtabmap::CompressionThread ctObstacleCells(_obstacleCellsCompressed, false);
+		if(imageRaw && imageRaw->empty() && !_imageCompressed.empty())
 		{
+			UASSERT(_imageCompressed.type() == CV_8UC1);
 			ctImage.start();
 		}
-		if(depthRaw && depthRaw->empty())
+		if(depthRaw && depthRaw->empty() && !_depthOrRightCompressed.empty())
 		{
+			UASSERT(_depthOrRightCompressed.type() == CV_8UC1);
 			ctDepth.start();
 		}
-		if(laserScanRaw && laserScanRaw->empty())
+		if(laserScanRaw && laserScanRaw->empty() && !_laserScanCompressed.empty())
 		{
+			UASSERT(_laserScanCompressed.type() == CV_8UC1);
 			ctLaserScan.start();
 		}
-		if(userDataRaw && userDataRaw->empty())
+		if(userDataRaw && userDataRaw->empty() && !_userDataCompressed.empty())
 		{
+			UASSERT(_userDataCompressed.type() == CV_8UC1);
 			ctUserData.start();
 		}
+		if(groundCellsRaw && groundCellsRaw->empty() && !_groundCellsCompressed.empty())
+		{
+			UASSERT(_groundCellsCompressed.type() == CV_8UC1);
+			ctGroundCells.start();
+		}
+		if(obstacleCellsRaw && obstacleCellsRaw->empty() && !_obstacleCellsCompressed.empty())
+		{
+			UASSERT(_obstacleCellsCompressed.type() == CV_8UC1);
+			ctObstacleCells.start();
+		}
 		ctImage.join();
 		ctDepth.join();
 		ctLaserScan.join();
 		ctUserData.join();
+		ctGroundCells.join();
+		ctObstacleCells.join();
+
 		if(imageRaw && imageRaw->empty())
 		{
 			*imageRaw = ctImage.getUncompressedData();
@@ -603,6 +716,14 @@ void SensorData::uncompressDataConst(cv::Mat * imageRaw, cv::Mat * depthRaw, cv:
 				UWARN("Requested user data, but the sensor data (%d) doesn't have user data.", this->id());
 			}
 		}
+		if(groundCellsRaw && groundCellsRaw->empty())
+		{
+			*groundCellsRaw = ctGroundCells.getUncompressedData();
+		}
+		if(obstacleCellsRaw && obstacleCellsRaw->empty())
+		{
+			*obstacleCellsRaw = ctObstacleCells.getUncompressedData();
+		}
 	}
 }
 
@@ -615,7 +736,11 @@ long SensorData::getMemoryUsed() const // Return memory usage in Bytes
 			_userDataCompressed.total()*_userDataCompressed.elemSize() +
 			_userDataRaw.total()*_userDataRaw.elemSize() +
 			_laserScanCompressed.total()*_laserScanCompressed.elemSize() +
-			_laserScanRaw.total()*_laserScanRaw.elemSize();
+			_laserScanRaw.total()*_laserScanRaw.elemSize() +
+			_groundCellsCompressed.total()*_groundCellsCompressed.elemSize() +
+			_groundCellsRaw.total()*_groundCellsRaw.elemSize() +
+			_obstacleCellsCompressed.total()*_obstacleCellsCompressed.elemSize() +
+			_obstacleCellsRaw.total()*_obstacleCellsRaw.elemSize();
 }
 
 } // namespace rtabmap

corelib/src/Signature.cpp

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

corelib/src/StereoCameraModel.cpp

@@ -344,6 +344,12 @@ void StereoCameraModel::scale(double scale)
 	right_ = right_.scaled(scale);
 }
 
+void StereoCameraModel::roi(const cv::Rect & roi)
+{
+	left_ = left_.roi(roi);
+	right_ = right_.roi(roi);
+}
+
 float StereoCameraModel::computeDepth(float disparity) const
 {
 	//depth = baseline * f / (disparity + cx1-cx0);

corelib/src/resources/DatabaseSchema.sql.in

@@ -21,9 +21,7 @@ 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)
 );
@@ -33,9 +31,17 @@ CREATE TABLE Data (
 	image BLOB,               -- compressed image (Grayscale or RGB)
 	depth BLOB,               -- compressed image (Depth or Right image)
 	calibration BLOB,         -- fx, fy, cx, cy, [baseline,] width, height, local_transform
+	
 	scan BLOB,                -- compressed data (Laser scan)
-	scan_max_pts INTEGER,     -- Laser scan max points
+	scan_info BLOB,           -- scan_max_pts, scan_max_range, local_transform
-	scan_max_range FLOAT,     -- Laser max range
+	
+	ground_cells BLOB,        -- compressed data (occupancy grid)
+	obstacle_cells BLOB,      -- compressed data (occupancy grid)
+	cell_size FLOAT,
+	view_point_x FLOAT,
+	view_point_y FLOAT,
+	view_point_z FLOAT,
+	
 	user_data BLOB,           -- compressed data (User data)
 	time_enter DATE,
 	PRIMARY KEY (id)

corelib/src/util2d.cpp

@@ -1089,6 +1089,92 @@ float getDepth(
 	return depth;
 }
 
+cv::Rect computeRoi(const cv::Mat & image, const std::string & roiRatios)
+{
+	return computeRoi(image.size(), roiRatios);
+}
+
+cv::Rect computeRoi(const cv::Size & imageSize, const std::string & roiRatios)
+{
+	std::list<std::string> strValues = uSplit(roiRatios, ' ');
+	if(strValues.size() != 4)
+	{
+		UERROR("The number of values must be 4 (roi=\"%s\")", roiRatios.c_str());
+	}
+	else
+	{
+		std::vector<float> values(4);
+		unsigned int i=0;
+		for(std::list<std::string>::iterator iter = strValues.begin(); iter!=strValues.end(); ++iter)
+		{
+			values[i] = uStr2Float(*iter);
+			++i;
+		}
+
+		if(values[0] >= 0 && values[0] < 1 && values[0] < 1.0f-values[1] &&
+			values[1] >= 0 && values[1] < 1 && values[1] < 1.0f-values[0] &&
+			values[2] >= 0 && values[2] < 1 && values[2] < 1.0f-values[3] &&
+			values[3] >= 0 && values[3] < 1 && values[3] < 1.0f-values[2])
+		{
+			return computeRoi(imageSize, values);
+		}
+		else
+		{
+			UERROR("The roi ratios are not valid (roi=\"%s\")", roiRatios.c_str());
+		}
+	}
+	return cv::Rect();
+}
+
+cv::Rect computeRoi(const cv::Mat & image, const std::vector<float> & roiRatios)
+{
+	return computeRoi(image.size(), roiRatios);
+}
+
+cv::Rect computeRoi(const cv::Size & imageSize, const std::vector<float> & roiRatios)
+{
+	if(imageSize.height!=0 && imageSize.width!= 0 && roiRatios.size() == 4)
+	{
+		float width = imageSize.width;
+		float height = imageSize.height;
+		cv::Rect roi(0, 0, width, height);
+		UDEBUG("roi ratios = %f, %f, %f, %f", roiRatios[0],roiRatios[1],roiRatios[2],roiRatios[3]);
+		UDEBUG("roi = %d, %d, %d, %d", roi.x, roi.y, roi.width, roi.height);
+
+		//left roi
+		if(roiRatios[0] > 0 && roiRatios[0] < 1 - roiRatios[1])
+		{
+			roi.x = width * roiRatios[0];
+		}
+
+		//right roi
+		if(roiRatios[1] > 0 && roiRatios[1] < 1 - roiRatios[0])
+		{
+			roi.width -= width * roiRatios[1] + width * roiRatios[0];
+		}
+
+		//top roi
+		if(roiRatios[2] > 0 && roiRatios[2] < 1 - roiRatios[3])
+		{
+			roi.y = height * roiRatios[2];
+		}
+
+		//bottom roi
+		if(roiRatios[3] > 0 && roiRatios[3] < 1 - roiRatios[2])
+		{
+			roi.height -= height * roiRatios[3] + height * roiRatios[2];
+		}
+		UDEBUG("roi = %d, %d, %d, %d", roi.x, roi.y, roi.width, roi.height);
+
+		return roi;
+	}
+	else
+	{
+		UERROR("Image is null or _roiRatios(=%d) != 4", roiRatios.size());
+		return cv::Rect();
+	}
+}
+
 cv::Mat decimate(const cv::Mat & image, int decimation)
 {
 	UASSERT(decimation >= 1);

corelib/src/util3d.cpp

@@ -30,6 +30,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <rtabmap/core/util3d_filtering.h>
 #include <rtabmap/core/util3d_surface.h>
 #include <rtabmap/core/util2d.h>
+#include <rtabmap/core/util2d.h>
 #include <rtabmap/utilite/ULogger.h>
 #include <rtabmap/utilite/UMath.h>
 #include <rtabmap/utilite/UConversion.h>
@@ -699,7 +700,8 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP cloudFromSensorData(
 		float maxDepth,
 		float minDepth,
 		std::vector<int> * validIndices,
-		const ParametersMap & parameters)
+		const ParametersMap & parameters,
+		const std::vector<float> & roiRatios)
 {
 	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
 
@@ -712,9 +714,46 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP cloudFromSensorData(
 		{
 			if(sensorData.cameraModels()[i].isValidForProjection())
 			{
+				cv::Mat depth = cv::Mat(sensorData.depthRaw(), cv::Rect(subImageWidth*i, 0, subImageWidth, sensorData.depthRaw().rows));
+				CameraModel model = sensorData.cameraModels()[i];
+				if( roiRatios.size() == 4 &&
+					roiRatios[0] != 0.0f &&
+					roiRatios[1] != 0.0f &&
+					roiRatios[2] != 0.0f &&
+					roiRatios[3] != 0.0f)
+				{
+					if(	int((roiRatios[0]+roiRatios[1])*double(depth.cols))%decimation==0 &&
+						int((roiRatios[2]+roiRatios[3])*double(depth.rows))%decimation==0 &&
+						(model.imageWidth() == 0 ||
+						 model.imageHeight() == 0 ||
+						 (int((roiRatios[0]+roiRatios[1])*double(model.imageWidth()))%decimation==0 &&
+						  int((roiRatios[2]+roiRatios[3])*double(model.imageHeight()))%decimation==0)))
+					{
+						cv::Rect roiDepth = util2d::computeRoi(depth, roiRatios);
+						depth = cv::Mat(depth, roiDepth);
+						if(model.imageWidth() != 0 && model.imageHeight() != 0)
+						{
+							model = model.roi(util2d::computeRoi(model.imageSize(), roiRatios));
+						}
+						else
+						{
+							model = model.roi(roiDepth);
+						}
+					}
+					else
+					{
+						UWARN("Cannot apply ROI ratios because resulting "
+							  "dimension (%dx%d) cannot be divided exactly "
+							  "by decimation parameter (%d). Ignoring ROI ratios...",
+							  int((roiRatios[0]+roiRatios[1])*double(depth.cols)),
+							  int((roiRatios[2]+roiRatios[3])*double(depth.rows)),
+							  decimation);
+					}
+				}
+
 				pcl::PointCloud<pcl::PointXYZ>::Ptr tmp = util3d::cloudFromDepth(
-						cv::Mat(sensorData.depthRaw(), cv::Rect(subImageWidth*i, 0, subImageWidth, sensorData.depthRaw().rows)),
+						depth,
-						sensorData.cameraModels()[i],
+						model,
 						decimation,
 						maxDepth,
 						minDepth,
@@ -722,7 +761,7 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP cloudFromSensorData(
 
 				if(tmp->size())
 				{
-					tmp = util3d::transformPointCloud(tmp, sensorData.cameraModels()[i].localTransform());
+					tmp = util3d::transformPointCloud(tmp, model.localTransform());
 
 					if(sensorData.cameraModels().size() > 1)
 					{
@@ -765,6 +804,7 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP cloudFromSensorData(
 		{
 			leftMono = sensorData.imageRaw();
 		}
+
 		cloud = cloudFromDisparity(
 				util2d::disparityFromStereoImages(leftMono, sensorData.rightRaw(), parameters),
 				sensorData.stereoCameraModel(),
@@ -790,7 +830,8 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_EXP cloudRGBFromSensorData(
 		float maxDepth,
 		float minDepth,
 		std::vector<int> * validIndices,
-		const ParametersMap & parameters)
+		const ParametersMap & parameters,
+		const std::vector<float> & roiRatios)
 {
 	UASSERT(!sensorData.imageRaw().empty());
 	UASSERT((!sensorData.depthRaw().empty() && sensorData.cameraModels().size()) ||
@@ -822,10 +863,43 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_EXP cloudRGBFromSensorData(
 		{
 			if(sensorData.cameraModels()[i].isValidForProjection())
 			{
+				cv::Mat depth(sensorData.imageRaw(), cv::Rect(subRGBWidth*i, 0, subRGBWidth, sensorData.imageRaw().rows));
+				cv::Mat rgb(sensorData.depthRaw(), cv::Rect(subDepthWidth*i, 0, subDepthWidth, sensorData.depthRaw().rows));
+				CameraModel model = sensorData.cameraModels()[i];
+				if( roiRatios.size() == 4 &&
+					roiRatios[0] != 0.0f &&
+					roiRatios[1] != 0.0f &&
+					roiRatios[2] != 0.0f &&
+					roiRatios[3] != 0.0f)
+				{
+					if(	int((roiRatios[0]+roiRatios[1])*double(depth.cols))%decimation==0 &&
+						int((roiRatios[2]+roiRatios[3])*double(depth.rows))%decimation==0 &&
+						int((roiRatios[0]+roiRatios[1])*double(rgb.cols))%decimation==0 &&
+						int((roiRatios[2]+roiRatios[3])*double(rgb.rows))%decimation==0)
+					{
+						cv::Rect roiDepth = util2d::computeRoi(depth, roiRatios);
+						cv::Rect roiRgb = util2d::computeRoi(rgb, roiRatios);
+						depth = cv::Mat(depth, roiDepth);
+						rgb = cv::Mat(rgb, roiRgb);
+						model = model.roi(roiRgb);
+					}
+					else
+					{
+						UWARN("Cannot apply ROI ratios because resulting "
+							  "dimension (depth=%dx%d rgb=%dx%d) cannot be divided exactly "
+							  "by decimation parameter (%d). Ignoring ROI ratios...",
+							  int((roiRatios[0]+roiRatios[1])*double(depth.cols)),
+							  int((roiRatios[2]+roiRatios[3])*double(depth.rows)),
+							  int((roiRatios[0]+roiRatios[1])*double(rgb.cols)),
+							  int((roiRatios[2]+roiRatios[3])*double(rgb.rows)),
+							  decimation);
+					}
+				}
+
 				pcl::PointCloud<pcl::PointXYZRGB>::Ptr tmp = util3d::cloudFromDepthRGB(
-						cv::Mat(sensorData.imageRaw(), cv::Rect(subRGBWidth*i, 0, subRGBWidth, sensorData.imageRaw().rows)),
+						depth,
-						cv::Mat(sensorData.depthRaw(), cv::Rect(subDepthWidth*i, 0, subDepthWidth, sensorData.depthRaw().rows)),
+						rgb,
-						sensorData.cameraModels()[i],
+						model,
 						decimation,
 						maxDepth,
 						minDepth,
@@ -833,7 +907,7 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_EXP cloudRGBFromSensorData(
 
 				if(tmp->size())
 				{
-					tmp = util3d::transformPointCloud(tmp, sensorData.cameraModels()[i].localTransform());
+					tmp = util3d::transformPointCloud(tmp, model.localTransform());
 
 					if(sensorData.cameraModels().size() > 1)
 					{
@@ -866,7 +940,8 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_EXP cloudRGBFromSensorData(
 	{
 		//stereo
 		UDEBUG("");
-		cloud = cloudFromStereoImages(sensorData.imageRaw(),
+		cloud = cloudFromStereoImages(
+				sensorData.imageRaw(),
 				sensorData.rightRaw(),
 				sensorData.stereoCameraModel(),
 				decimation,
@@ -973,6 +1048,31 @@ cv::Mat laserScanFromPointCloud(const pcl::PointCloud<pcl::PointNormal> & cloud,
 	return laserScan;
 }
 
+cv::Mat laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZRGB> & cloud, const Transform & transform)
+{
+	cv::Mat laserScan(1, (int)cloud.size(), CV_32FC(4));
+	bool nullTransform = transform.isNull() || transform.isIdentity();
+	Eigen::Affine3f transform3f = transform.toEigen3f();
+	for(unsigned int i=0; i<cloud.size(); ++i)
+	{
+		if(!nullTransform)
+		{
+			pcl::PointXYZRGB pt = pcl::transformPoint(cloud.at(i), transform3f);
+			laserScan.at<cv::Vec4f>(i)[0] = pt.x;
+			laserScan.at<cv::Vec4f>(i)[1] = pt.y;
+			laserScan.at<cv::Vec4f>(i)[2] = pt.z;
+		}
+		else
+		{
+			laserScan.at<cv::Vec4f>(i)[0] = cloud.at(i).x;
+			laserScan.at<cv::Vec4f>(i)[1] = cloud.at(i).y;
+			laserScan.at<cv::Vec4f>(i)[2] = cloud.at(i).z;
+		}
+		laserScan.at<cv::Vec4i>(i)[3] = int(cloud.at(i).b) | (int(cloud.at(i).g) << 8) | (int(cloud.at(i).r) << 16);
+	}
+	return laserScan;
+}
+
 cv::Mat laserScan2dFromPointCloud(const pcl::PointCloud<pcl::PointXYZ> & cloud, const Transform & transform)
 {
 	cv::Mat laserScan(1, (int)cloud.size(), CV_32FC2);
@@ -998,7 +1098,7 @@ cv::Mat laserScan2dFromPointCloud(const pcl::PointCloud<pcl::PointXYZ> & cloud,
 
 pcl::PointCloud<pcl::PointXYZ>::Ptr laserScanToPointCloud(const cv::Mat & laserScan, const Transform & transform)
 {
-	UASSERT(laserScan.empty() || laserScan.type() == CV_32FC2 || laserScan.type() == CV_32FC3 || laserScan.type() == CV_32FC(6));
+	UASSERT(laserScan.empty() || laserScan.type() == CV_32FC2 || laserScan.type() == CV_32FC3 || laserScan.type() == CV_32FC(4) || laserScan.type() == CV_32FC(6));
 
 	pcl::PointCloud<pcl::PointXYZ>::Ptr output(new pcl::PointCloud<pcl::PointXYZ>);
 	output->resize(laserScan.cols);
@@ -1006,24 +1106,7 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr laserScanToPointCloud(const cv::Mat & laserS
 	Eigen::Affine3f transform3f = transform.toEigen3f();
 	for(int i=0; i<laserScan.cols; ++i)
 	{
-		if(laserScan.type() == CV_32FC2)
+		output->at(i) = util3d::laserScanToPoint(laserScan, i);
-		{
-			output->at(i).x = laserScan.at<cv::Vec2f>(i)[0];
-			output->at(i).y = laserScan.at<cv::Vec2f>(i)[1];
-		}
-		else if(laserScan.type() == CV_32FC3)
-		{
-			output->at(i).x = laserScan.at<cv::Vec3f>(i)[0];
-			output->at(i).y = laserScan.at<cv::Vec3f>(i)[1];
-			output->at(i).z = laserScan.at<cv::Vec3f>(i)[2];
-		}
-		else
-		{
-			output->at(i).x = laserScan.at<cv::Vec6f>(i)[0];
-			output->at(i).y = laserScan.at<cv::Vec6f>(i)[1];
-			output->at(i).z = laserScan.at<cv::Vec6f>(i)[2];
-		}
-
 		if(!nullTransform)
 		{
 			output->at(i) = pcl::transformPoint(output->at(i), transform3f);
@@ -1034,34 +1117,14 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr laserScanToPointCloud(const cv::Mat & laserS
 
 pcl::PointCloud<pcl::PointNormal>::Ptr laserScanToPointCloudNormal(const cv::Mat & laserScan, const Transform & transform)
 {
-	UASSERT(laserScan.empty() || laserScan.type() == CV_32FC2 || laserScan.type() == CV_32FC3 || laserScan.type() == CV_32FC(6));
+	UASSERT(laserScan.empty() || laserScan.type() == CV_32FC2 || laserScan.type() == CV_32FC3 || laserScan.type() == CV_32FC(4) || laserScan.type() == CV_32FC(6));
 
 	pcl::PointCloud<pcl::PointNormal>::Ptr output(new pcl::PointCloud<pcl::PointNormal>);
 	output->resize(laserScan.cols);
 	bool nullTransform = transform.isNull();
 	for(int i=0; i<laserScan.cols; ++i)
 	{
-		if(laserScan.type() == CV_32FC2)
+		output->at(i) = laserScanToPointNormal(laserScan, i);
-		{
-			output->at(i).x = laserScan.at<cv::Vec2f>(i)[0];
-			output->at(i).y = laserScan.at<cv::Vec2f>(i)[1];
-		}
-		else if(laserScan.type() == CV_32FC3)
-		{
-			output->at(i).x = laserScan.at<cv::Vec3f>(i)[0];
-			output->at(i).y = laserScan.at<cv::Vec3f>(i)[1];
-			output->at(i).z = laserScan.at<cv::Vec3f>(i)[2];
-		}
-		else
-		{
-			output->at(i).x = laserScan.at<cv::Vec6f>(i)[0];
-			output->at(i).y = laserScan.at<cv::Vec6f>(i)[1];
-			output->at(i).z = laserScan.at<cv::Vec6f>(i)[2];
-			output->at(i).normal_x = laserScan.at<cv::Vec6f>(i)[3];
-			output->at(i).normal_y = laserScan.at<cv::Vec6f>(i)[4];
-			output->at(i).normal_z = laserScan.at<cv::Vec6f>(i)[5];
-		}
-
 		if(!nullTransform)
 		{
 			output->at(i) = util3d::transformPoint(output->at(i), transform);
@@ -1070,6 +1133,124 @@ pcl::PointCloud<pcl::PointNormal>::Ptr laserScanToPointCloudNormal(const cv::Mat
 	return output;
 }
 
+pcl::PointCloud<pcl::PointXYZRGB>::Ptr laserScanToPointCloudRGB(const cv::Mat & laserScan, const Transform & transform)
+{
+	UASSERT(laserScan.empty() || laserScan.type() == CV_32FC2 || laserScan.type() == CV_32FC3 || laserScan.type() == CV_32FC(4) || laserScan.type() == CV_32FC(6));
+
+	pcl::PointCloud<pcl::PointXYZRGB>::Ptr output(new pcl::PointCloud<pcl::PointXYZRGB>);
+	output->resize(laserScan.cols);
+	bool nullTransform = transform.isNull() || transform.isIdentity();
+	Eigen::Affine3f transform3f = transform.toEigen3f();
+	for(int i=0; i<laserScan.cols; ++i)
+	{
+		output->at(i) = util3d::laserScanToPointRGB(laserScan, i);
+		if(!nullTransform)
+		{
+			output->at(i) = pcl::transformPoint(output->at(i), transform3f);
+		}
+	}
+	return output;
+}
+
+pcl::PointXYZ laserScanToPoint(const cv::Mat & laserScan, int index)
+{
+	UASSERT(!laserScan.empty() && index < laserScan.cols);
+	UASSERT(laserScan.type() == CV_32FC2 || laserScan.type() == CV_32FC3 || laserScan.type() == CV_32FC(4) || laserScan.type() == CV_32FC(6));
+	pcl::PointXYZ output;
+	if(laserScan.type() == CV_32FC2)
+	{
+		output.x = laserScan.at<cv::Vec2f>(index)[0];
+		output.y = laserScan.at<cv::Vec2f>(index)[1];
+	}
+	else if(laserScan.type() == CV_32FC3)
+	{
+		output.x = laserScan.at<cv::Vec3f>(index)[0];
+		output.y = laserScan.at<cv::Vec3f>(index)[1];
+		output.z = laserScan.at<cv::Vec3f>(index)[2];
+	}
+	else if(laserScan.type() == CV_32FC(4))
+	{
+		output.x = laserScan.at<cv::Vec4f>(index)[0];
+		output.y = laserScan.at<cv::Vec4f>(index)[1];
+		output.z = laserScan.at<cv::Vec4f>(index)[2];
+	}
+	else
+	{
+		output.x = laserScan.at<cv::Vec6f>(index)[0];
+		output.y = laserScan.at<cv::Vec6f>(index)[1];
+		output.z = laserScan.at<cv::Vec6f>(index)[2];
+	}
+	return output;
+}
+
+pcl::PointNormal laserScanToPointNormal(const cv::Mat & laserScan, int index)
+{
+	UASSERT(!laserScan.empty() && index < laserScan.cols);
+	UASSERT(laserScan.type() == CV_32FC2 || laserScan.type() == CV_32FC3 || laserScan.type() == CV_32FC(4) || laserScan.type() == CV_32FC(6));
+	pcl::PointNormal output;
+	if(laserScan.type() == CV_32FC2)
+	{
+		output.x = laserScan.at<cv::Vec2f>(index)[0];
+		output.y = laserScan.at<cv::Vec2f>(index)[1];
+	}
+	else if(laserScan.type() == CV_32FC3)
+	{
+		output.x = laserScan.at<cv::Vec3f>(index)[0];
+		output.y = laserScan.at<cv::Vec3f>(index)[1];
+		output.z = laserScan.at<cv::Vec3f>(index)[2];
+	}
+	else if(laserScan.type() == CV_32FC(4))
+	{
+		output.x = laserScan.at<cv::Vec4f>(index)[0];
+		output.y = laserScan.at<cv::Vec4f>(index)[1];
+		output.z = laserScan.at<cv::Vec4f>(index)[2];
+	}
+	else
+	{
+		output.x = laserScan.at<cv::Vec6f>(index)[0];
+		output.y = laserScan.at<cv::Vec6f>(index)[1];
+		output.z = laserScan.at<cv::Vec6f>(index)[2];
+		output.normal_x = laserScan.at<cv::Vec6f>(index)[3];
+		output.normal_y = laserScan.at<cv::Vec6f>(index)[4];
+		output.normal_z = laserScan.at<cv::Vec6f>(index)[5];
+	}
+	return output;
+}
+
+pcl::PointXYZRGB laserScanToPointRGB(const cv::Mat & laserScan, int index)
+{
+	UASSERT(!laserScan.empty() && index < laserScan.cols);
+	UASSERT(laserScan.type() == CV_32FC2 || laserScan.type() == CV_32FC3 || laserScan.type() == CV_32FC(4) || laserScan.type() == CV_32FC(6));
+	pcl::PointXYZRGB output;
+	if(laserScan.type() == CV_32FC2)
+	{
+		output.x = laserScan.at<cv::Vec2f>(index)[0];
+		output.y = laserScan.at<cv::Vec2f>(index)[1];
+	}
+	else if(laserScan.type() == CV_32FC3)
+	{
+		output.x = laserScan.at<cv::Vec3f>(index)[0];
+		output.y = laserScan.at<cv::Vec3f>(index)[1];
+		output.z = laserScan.at<cv::Vec3f>(index)[2];
+	}
+	else if(laserScan.type() == CV_32FC(4))
+	{
+		output.x = laserScan.at<cv::Vec4f>(index)[0];
+		output.y = laserScan.at<cv::Vec4f>(index)[1];
+		output.z = laserScan.at<cv::Vec4f>(index)[2];
+		output.b = (unsigned char)(laserScan.at<cv::Vec4i>(index)[3] & 0xFF);
+		output.g = (unsigned char)((laserScan.at<cv::Vec4i>(index)[3] >> 8) & 0xFF);
+		output.r = (unsigned char)((laserScan.at<cv::Vec4i>(index)[3] >> 16) & 0xFF);
+	}
+	else
+	{
+		output.x = laserScan.at<cv::Vec6f>(index)[0];
+		output.y = laserScan.at<cv::Vec6f>(index)[1];
+		output.z = laserScan.at<cv::Vec6f>(index)[2];
+	}
+	return output;
+}
+
 // inspired from ROS image_geometry/src/stereo_camera_model.cpp
 cv::Point3f projectDisparityTo3D(
 		const cv::Point2f & pt,

corelib/src/util3d_mapping.cpp

@@ -84,7 +84,7 @@ void occupancy2DFromLaserScan(
 	ground = cv::Mat();
 	if(groundIndices.size())
 	{
-		ground = cv::Mat((int)groundIndices.size(), 1, CV_32FC2);
+		ground = cv::Mat(1, (int)groundIndices.size(), CV_32FC2);
 		int i=0;
 		for(std::list<int>::iterator iter=groundIndices.begin();iter!=groundIndices.end(); ++iter)
 		{
@@ -100,7 +100,7 @@ void occupancy2DFromLaserScan(
 	obstacles = cv::Mat();
 	if(obstaclesCloud->size())
 	{
-		obstacles = cv::Mat((int)obstaclesCloud->size(), 1, CV_32FC2);
+		obstacles = cv::Mat(1, (int)obstaclesCloud->size(), CV_32FC2);
 		for(unsigned int i=0;i<obstaclesCloud->size(); ++i)
 		{
 			obstacles.at<cv::Vec2f>(i)[0] = obstaclesCloud->at(i).x;
@@ -155,14 +155,12 @@ cv::Mat create2DMapFromOccupancyLocalMaps(
 
 	float minX=-minMapSize/2.0, minY=-minMapSize/2.0, maxX=minMapSize/2.0, maxY=minMapSize/2.0;
 	bool undefinedSize = minMapSize == 0.0f;
-	float x=0.0f,y=0.0f,z=0.0f,roll=0.0f,pitch=0.0f,yaw=0.0f,cosT=0.0f,sinT=0.0f;
-	cv::Mat affineTransform(2,3,CV_32FC1);
 	for(std::list<std::pair<int, Transform> >::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
 	{
 		UASSERT(!iter->second.isNull());
 
-		iter->second.getTranslationAndEulerAngles(x,y,z,roll,pitch,yaw);
+		float x = iter->second.x();
-
+		float y  =iter->second.y();
 		if(undefinedSize)
 		{
 			minX = maxX = x;
@@ -185,53 +183,75 @@ cv::Mat create2DMapFromOccupancyLocalMaps(
 		if(uContains(occupancy, iter->first))
 		{
 			const std::pair<cv::Mat, cv::Mat> & pair = occupancy.at(iter->first);
-			cosT = cos(yaw);
-			sinT = sin(yaw);
-			affineTransform.at<float>(0,0) = cosT;
-			affineTransform.at<float>(0,1) = -sinT;
-			affineTransform.at<float>(1,0) = sinT;
-			affineTransform.at<float>(1,1) = cosT;
-			affineTransform.at<float>(0,2) = x;
-			affineTransform.at<float>(1,2) = y;
 
 			//ground
-			if(pair.first.rows)
+			if(pair.first.cols)
 			{
-				UASSERT(pair.first.type() == CV_32FC2);
+				if(pair.first.rows > 1 && pair.first.cols == 1)
-				cv::Mat ground(pair.first.rows, pair.first.cols, pair.first.type());
-				cv::transform(pair.first, ground, affineTransform);
-				for(int i=0; i<ground.rows; ++i)
 				{
-					if(minX > ground.at<float>(i,0))
+					UFATAL("Occupancy local maps should be 1 row and X cols! (rows=%d cols=%d)", pair.first.rows, pair.first.cols);
-						minX = ground.at<float>(i,0);
+				}
-					else if(maxX < ground.at<float>(i,0))
+				cv::Mat ground(1, pair.first.cols, CV_32FC2);
-						maxX = ground.at<float>(i,0);
+				for(int i=0; i<ground.cols; ++i)
+				{
+					const float * vi = pair.first.ptr<float>(0,i);
+					float * vo = ground.ptr<float>(0,i);
+					cv::Point3f vt;
+					if(pair.first.channels() > 2)
+					{
+						vt = util3d::transformPoint(cv::Point3f(vi[0], vi[1], vi[2]), iter->second);
+					}
+					else
+					{
+						vt = util3d::transformPoint(cv::Point3f(vi[0], vi[1], 0), iter->second);
+					}
+					vo[0] = vt.x;
+					vo[1] = vt.y;
+					if(minX > vo[0])
+						minX = vo[0];
+					else if(maxX < vo[0])
+						maxX = vo[0];
 
-					if(minY > ground.at<float>(i,1))
+					if(minY > vo[1])
-						minY = ground.at<float>(i,1);
+						minY = vo[1];
-					else if(maxY < ground.at<float>(i,1))
+					else if(maxY < vo[1])
-						maxY = ground.at<float>(i,1);
+						maxY = vo[1];
 				}
 				emptyLocalMaps.insert(std::make_pair(iter->first, ground));
 			}
 
 			//obstacles
-			if(pair.second.rows)
+			if(pair.second.cols)
 			{
-				UASSERT(pair.second.type() == CV_32FC2);
+				if(pair.second.rows > 1 && pair.second.cols == 1)
-				cv::Mat obstacles(pair.second.rows, pair.second.cols, pair.second.type());
-				cv::transform(pair.second, obstacles, affineTransform);
-				for(int i=0; i<obstacles.rows; ++i)
 				{
-					if(minX > obstacles.at<float>(i,0))
+					UFATAL("Occupancy local maps should be 1 row and X cols! (rows=%d cols=%d)", pair.second.rows, pair.second.cols);
-						minX = obstacles.at<float>(i,0);
+				}
-					else if(maxX < obstacles.at<float>(i,0))
+				cv::Mat obstacles(1, pair.second.cols, CV_32FC2);
-						maxX = obstacles.at<float>(i,0);
+				for(int i=0; i<obstacles.cols; ++i)
+				{
+					const float * vi = pair.second.ptr<float>(0,i);
+					float * vo = obstacles.ptr<float>(0,i);
+					cv::Point3f vt;
+					if(pair.second.channels() > 2)
+					{
+						vt = util3d::transformPoint(cv::Point3f(vi[0], vi[1], vi[2]), iter->second);
+					}
+					else
+					{
+						vt = util3d::transformPoint(cv::Point3f(vi[0], vi[1], 0), iter->second);
+					}
+					vo[0] = vt.x;
+					vo[1] = vt.y;
+					if(minX > vo[0])
+						minX = vo[0];
+					else if(maxX < vo[0])
+						maxX = vo[0];
 
-					if(minY > obstacles.at<float>(i,1))
+					if(minY > vo[1])
-						minY = obstacles.at<float>(i,1);
+						minY = vo[1];
-					else if(maxY < obstacles.at<float>(i,1))
+					else if(maxY < vo[1])
-						maxY = obstacles.at<float>(i,1);
+						maxY = vo[1];
 				}
 				occupiedLocalMaps.insert(std::make_pair(iter->first, obstacles));
 			}
@@ -267,12 +287,14 @@ cv::Mat create2DMapFromOccupancyLocalMaps(
 				std::map<int, cv::Mat >::iterator jter = occupiedLocalMaps.find(kter->first);
 				if(iter!=emptyLocalMaps.end())
 				{
-					for(int i=0; i<iter->second.rows; ++i)
+					for(int i=0; i<iter->second.cols; ++i)
 					{
-						cv::Point2i pt((iter->second.at<float>(i,0)-xMin)/cellSize + 0.5f, (iter->second.at<float>(i,1)-yMin)/cellSize + 0.5f);
+						float * ptf = iter->second.ptr<float>(0, i);
-						if(map.at<char>(pt.y, pt.x) != -2)
+						cv::Point2i pt((ptf[0]-xMin)/cellSize + 0.5f, (ptf[1]-yMin)/cellSize + 0.5f);
+						char & value = map.at<char>(pt.y, pt.x);
+						if(value != -2)
 						{
-							map.at<char>(pt.y, pt.x) = 0; // free space
+							value = 0; // free space
 						}
 					}
 				}
@@ -302,12 +324,14 @@ cv::Mat create2DMapFromOccupancyLocalMaps(
 
 				if(jter!=occupiedLocalMaps.end())
 				{
-					for(int i=0; i<jter->second.rows; ++i)
+					for(int i=0; i<jter->second.cols; ++i)
 					{
-						cv::Point2i pt((jter->second.at<float>(i,0)-xMin)/cellSize + 0.5f, (jter->second.at<float>(i,1)-yMin)/cellSize + 0.5f);
+						float * ptf = jter->second.ptr<float>(0, i);
-						if(map.at<char>(pt.y, pt.x) != -2)
+						cv::Point2i pt((ptf[0]-xMin)/cellSize + 0.5f, (ptf[1]-yMin)/cellSize + 0.5f);
+						char & value = map.at<char>(pt.y, pt.x);
+						if(value != -2)
 						{
-							map.at<char>(pt.y, pt.x) = 100; // obstacles
+							value = 100; // obstacles
 						}
 					}
 				}
@@ -412,6 +436,7 @@ cv::Mat create2DMapFromOccupancyLocalMaps(
 			}
 		}
 	}
+
 	UDEBUG("timer=%fs", timer.ticks());
 	return map;
 }

corelib/src/util3d_transforms.cpp

@@ -38,7 +38,7 @@ namespace util3d
 
 cv::Mat transformLaserScan(const cv::Mat & laserScan, const Transform & transform)
 {
-	UASSERT(laserScan.empty() || laserScan.type() == CV_32FC2 || laserScan.type() == CV_32FC3 || laserScan.type() == CV_32FC(6));
+	UASSERT(laserScan.empty() || laserScan.type() == CV_32FC2 || laserScan.type() == CV_32FC3 || laserScan.type() == CV_32FC(4) || laserScan.type() == CV_32FC(6));
 
 	cv::Mat output = laserScan.clone();
 
@@ -66,6 +66,17 @@ cv::Mat transformLaserScan(const cv::Mat & laserScan, const Transform & transfor
 				output.at<cv::Vec3f>(i)[1] = pt.y;
 				output.at<cv::Vec3f>(i)[2] = pt.z;
 			}
+			else if(laserScan.type() == CV_32FC(4))
+			{
+				pcl::PointXYZ pt(
+						laserScan.at<cv::Vec4f>(i)[0],
+						laserScan.at<cv::Vec4f>(i)[1],
+						laserScan.at<cv::Vec4f>(i)[2]);
+				pt = util3d::transformPoint(pt, transform);
+				output.at<cv::Vec4f>(i)[0] = pt.x;
+				output.at<cv::Vec4f>(i)[1] = pt.y;
+				output.at<cv::Vec4f>(i)[2] = pt.z;
+			}
 			else
 			{
 				pcl::PointNormal pt;

guilib/include/rtabmap/gui/MainWindow.h

@@ -50,6 +50,7 @@ class CameraThread;
 class OdometryThread;
 class CloudViewer;
 class LoopClosureViewer;
+class OccupancyGrid;
 }
 
 class QGraphicsScene;
@@ -238,12 +239,6 @@ private:
 			const std::map<int, Transform> & groundTruths,
 			bool verboseProgress = false);
 	std::pair<pcl::PointCloud<pcl::PointXYZRGB>::Ptr, pcl::IndicesPtr> createAndAddCloudToMap(int nodeId,	const Transform & pose, int mapId);
-	void createAndAddProjectionMap(
-			const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
-			const pcl::IndicesPtr & indices,
-			int nodeId,
-			const Transform & pose,
-			bool updateOctomap = false);
 	void createAndAddScanToMap(int nodeId, const Transform & pose, int mapId);
 	void createAndAddFeaturesToMap(int nodeId, const Transform & pose, int mapId);
 	Transform alignPosesToGroundTruth(std::map<int, Transform> & poses, const std::map<int, Transform> & groundTruth);
@@ -299,9 +294,11 @@ private:
 	std::pair<int, std::pair<std::pair<pcl::PointCloud<pcl::PointXYZRGB>::Ptr, pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr>, pcl::IndicesPtr> > _previousCloud; // used for subtraction
 
 	std::map<int, cv::Mat> _createdScans;
-	std::map<int, std::pair<cv::Mat, cv::Mat> > _projectionLocalMaps; // <ground, obstacles>
 	std::map<int, std::pair<cv::Mat, cv::Mat> > _gridLocalMaps; // <ground, obstacles>
+	std::map<int, cv::Point3f> _gridViewPoints;
+	long _cachedGridsMemoryUsage;
 
+	rtabmap::OccupancyGrid * _occupancyGrid;
 	rtabmap::OctoMap * _octomap;
 
 	std::map<int, pcl::PointCloud<pcl::PointXYZRGB>::Ptr> _createdFeatures;

guilib/include/rtabmap/gui/PreferencesDialog.h

@@ -154,7 +154,9 @@ public:
 	double getMapNoiseRadius() const;
 	int getMapNoiseMinNeighbors() const;
 	bool isCloudsShown(int index) const;      // 0=map, 1=odom
+	bool isOctomapUpdated() const;
 	bool isOctomapShown() const;
+	bool isOctomap2dGrid() const;
 	int getOctomapTreeDepth() const;
 	bool isOctomapGroundAnObstacle() const;
 	int getCloudDecimation(int index) const;   // 0=map, 1=odom
@@ -184,6 +186,7 @@ public:
 	bool getGridMapShown() const;
 	double getGridMapResolution() const;;
 	bool isGridMapEroded() const;
+	bool isGridMapIncremental() const;
 	double getGridMapFootprintRadius() const;
 	bool isGridMapFrom3DCloud() const;
 	bool projMapFrame() const;

guilib/src/DatabaseViewer.cpp

@@ -294,7 +294,7 @@ DatabaseViewer::DatabaseViewer(const QString & ini, QWidget * parent) :
 	connect(ui_->spinBox_icp_decimation, SIGNAL(valueChanged(int)), this, SLOT(configModified()));
 	connect(ui_->doubleSpinBox_icp_maxDepth, SIGNAL(valueChanged(double)), this, SLOT(configModified()));
 	connect(ui_->doubleSpinBox_icp_minDepth, SIGNAL(valueChanged(double)), this, SLOT(configModified()));
-	connect(ui_->checkBox_icp_laserScan, SIGNAL(stateChanged(int)), this, SLOT(configModified()));
+	connect(ui_->checkBox_icp_from_depth, SIGNAL(stateChanged(int)), this, SLOT(configModified()));
 	
 	connect(ui_->doubleSpinBox_detectMore_radius, SIGNAL(valueChanged(double)), this, SLOT(configModified()));
 	connect(ui_->doubleSpinBox_detectMore_angle, SIGNAL(valueChanged(double)), this, SLOT(configModified()));
@@ -431,7 +431,7 @@ void DatabaseViewer::readSettings()
 	ui_->spinBox_icp_decimation->setValue(settings.value("decimation", ui_->spinBox_icp_decimation->value()).toInt());
 	ui_->doubleSpinBox_icp_maxDepth->setValue(settings.value("maxDepth", ui_->doubleSpinBox_icp_maxDepth->value()).toDouble());
 	ui_->doubleSpinBox_icp_minDepth->setValue(settings.value("minDepth", ui_->doubleSpinBox_icp_minDepth->value()).toDouble());
-	ui_->checkBox_icp_laserScan->setChecked(settings.value("icpLaserScan", ui_->checkBox_icp_laserScan->isChecked()).toBool());
+	ui_->checkBox_icp_from_depth->setChecked(settings.value("icpFromDepth", ui_->checkBox_icp_from_depth->isChecked()).toBool());
 	settings.endGroup();
 	// Visual parameters
 	settings.beginGroup("visual");
@@ -519,7 +519,7 @@ void DatabaseViewer::writeSettings()
 	settings.setValue("decimation", ui_->spinBox_icp_decimation->value());
 	settings.setValue("maxDepth", ui_->doubleSpinBox_icp_maxDepth->value());
 	settings.setValue("minDepth", ui_->doubleSpinBox_icp_minDepth->value());
-	settings.setValue("icpLaserScan", ui_->checkBox_icp_laserScan->isChecked());
+	settings.setValue("icpFromDepth", ui_->checkBox_icp_from_depth->isChecked());
 	settings.endGroup();
 	
 	// save Visual parameters
@@ -893,8 +893,9 @@ void DatabaseViewer::exportDatabase()
 					{
 						sensorData = rtabmap::SensorData(
 							scan,
-							dialog.isDepth2dExported()?data.laserScanMaxPts():0,
+							LaserScanInfo(dialog.isDepth2dExported()?data.laserScanInfo().maxPoints():0,
-							dialog.isDepth2dExported()?data.laserScanMaxRange():0,
+										  dialog.isDepth2dExported()?data.laserScanInfo().maxRange():0,
+										  dialog.isDepth2dExported()?data.laserScanInfo().localTransform():Transform::getIdentity()),
 							rgb,
 							depth,
 							data.cameraModels(),
@@ -906,8 +907,9 @@ void DatabaseViewer::exportDatabase()
 					{
 						sensorData = rtabmap::SensorData(
 							scan,
-							dialog.isDepth2dExported()?data.laserScanMaxPts():0,
+							LaserScanInfo(dialog.isDepth2dExported()?data.laserScanInfo().maxPoints():0,
-							dialog.isDepth2dExported()?data.laserScanMaxRange():0,
+										  dialog.isDepth2dExported()?data.laserScanInfo().maxRange():0,
+										  dialog.isDepth2dExported()?data.laserScanInfo().localTransform():Transform::getIdentity()),
 							rgb,
 							depth,
 							data.stereoCameraModel(),
@@ -2423,7 +2425,7 @@ void DatabaseViewer::update(int value,
 					}
 
 					//add scan
-					pcl::PointCloud<pcl::PointXYZ>::Ptr scan = util3d::laserScanToPointCloud(data.laserScanRaw());
+					pcl::PointCloud<pcl::PointXYZ>::Ptr scan = util3d::laserScanToPointCloud(data.laserScanRaw(), data.laserScanInfo().localTransform());
 					if(scan->size())
 					{
 						view3D->addCloud("1", scan);
@@ -3306,8 +3308,8 @@ void DatabaseViewer::updateConstraintView(
 
 				// Added loop closure scans
 				pcl::PointCloud<pcl::PointXYZ>::Ptr scanA, scanB;
-				scanA = rtabmap::util3d::laserScanToPointCloud(dataFrom.laserScanRaw());
+				scanA = rtabmap::util3d::laserScanToPointCloud(dataFrom.laserScanRaw(), dataFrom.laserScanInfo().localTransform());
-				scanB = rtabmap::util3d::laserScanToPointCloud(dataTo.laserScanRaw());
+				scanB = rtabmap::util3d::laserScanToPointCloud(dataTo.laserScanRaw(), dataTo.laserScanInfo().localTransform());
 				scanB = rtabmap::util3d::transformPointCloud(scanB, t);
 				if(scanA->size())
 				{
@@ -3509,7 +3511,7 @@ void DatabaseViewer::sliderIterationsValueChanged(int value)
 										obstacles,
 										ui_->doubleSpinBox_gridCellSize->value(),
 										ui_->checkBox_gridFillUnkownSpace->isChecked(),
-										data.laserScanMaxRange());
+										data.laserScanInfo().maxRange());
 								added = true;
 							}
 							localMaps_.insert(std::make_pair(ids.at(i), std::make_pair(ground, obstacles)));
@@ -3904,7 +3906,7 @@ void DatabaseViewer::refineConstraint(int from, int to, bool silent, bool update
 	ParametersMap parameters = ui_->parameters_toolbox->getParameters();
 
 	UTimer timer;
-	if(ui_->checkBox_icp_laserScan->isChecked())
+	if(ui_->checkBox_icp_from_depth->isChecked())
 	{
 		// generate laser scans from depth image
 		cv::Mat tmpA, tmpB, tmpC, tmpD;
@@ -3925,8 +3927,8 @@ void DatabaseViewer::refineConstraint(int from, int to, bool silent, bool update
 				0,
 				ui_->parameters_toolbox->getParameters());
 		int maxLaserScans = cloudFrom->size();
-		dataFrom.setLaserScanRaw(util3d::laserScanFromPointCloud(*util3d::removeNaNFromPointCloud(cloudFrom), Transform()), maxLaserScans, 0);
+		dataFrom.setLaserScanRaw(util3d::laserScanFromPointCloud(*util3d::removeNaNFromPointCloud(cloudFrom), Transform()), LaserScanInfo(maxLaserScans, 0));
-		dataTo.setLaserScanRaw(util3d::laserScanFromPointCloud(*util3d::removeNaNFromPointCloud(cloudTo), Transform()), maxLaserScans, 0);
+		dataTo.setLaserScanRaw(util3d::laserScanFromPointCloud(*util3d::removeNaNFromPointCloud(cloudTo), Transform()), LaserScanInfo(maxLaserScans, 0));
 
 		if(!dataFrom.laserScanCompressed().empty() || !dataTo.laserScanCompressed().empty())
 		{

guilib/src/MainWindow.cpp

@@ -40,6 +40,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "rtabmap/core/Memory.h"
 #include "rtabmap/core/DBDriver.h"
 #include "rtabmap/core/RegistrationVis.h"
+#include "rtabmap/core/OccupancyGrid.h"
 
 #include "rtabmap/gui/ImageView.h"
 #include "rtabmap/gui/KeypointItem.h"
@@ -151,6 +152,8 @@ MainWindow::MainWindow(PreferencesDialog * prefDialog, QWidget * parent) :
 	_waypointsIndex(0),
 	_cachedMemoryUsage(0),
 	_createdCloudsMemoryUsage(0),
+	_cachedGridsMemoryUsage(0),
+	_occupancyGrid(0),
 	_octomap(0),
 	_odometryCorrection(Transform::getIdentity()),
 	_processingOdometry(false),
@@ -234,6 +237,7 @@ MainWindow::MainWindow(PreferencesDialog * prefDialog, QWidget * parent) :
 	_preferencesDialog->loadWindowGeometry(_aboutDialog);
 	setupMainLayout(_preferencesDialog->isVerticalLayoutUsed());
 
+	_occupancyGrid = new OccupancyGrid(_preferencesDialog->getAllParameters());
 #ifdef RTABMAP_OCTOMAP
 	_octomap = new OctoMap(_preferencesDialog->getGridMapResolution());
 #endif
@@ -568,6 +572,10 @@ MainWindow::MainWindow(PreferencesDialog * prefDialog, QWidget * parent) :
 	_ui->statsToolBox->updateStat("GUI/Refresh stats/ms", 0.0f);
 	_ui->statsToolBox->updateStat("GUI/Cache Data Size/MB", 0.0f);
 	_ui->statsToolBox->updateStat("GUI/Cache Clouds Size/MB", 0.0f);
+	_ui->statsToolBox->updateStat("GUI/Cache Grids Size/MB", 0.0f);
+#ifdef RTABMAP_OCTOMAP
+	_ui->statsToolBox->updateStat("GUI/Octomap Size/MB", 0.0f);
+#endif
 
 	this->loadFigures();
 	connect(_ui->statsToolBox, SIGNAL(figuresSetupChanged()), this, SLOT(configGUIModified()));
@@ -589,6 +597,10 @@ MainWindow::~MainWindow()
 	this->stopDetection();
 	delete _ui;
 	delete _elapsedTime;
+#ifdef RTABMAP_OCTOMAP
+	delete _octomap;
+#endif
+	delete _occupancyGrid;
 	UDEBUG("");
 }
 
@@ -1024,7 +1036,7 @@ void MainWindow::processOdometry(const rtabmap::OdometryEvent & odom, bool dataI
 					}
 
 					pcl::PointCloud<pcl::PointNormal>::Ptr cloud;
-					cloud = util3d::laserScanToPointCloudNormal(scan, pose);
+					cloud = util3d::laserScanToPointCloudNormal(scan, odom.data().laserScanInfo().localTransform()*pose);
 					if(_preferencesDialog->getCloudVoxelSizeScan(1) > 0.0)
 					{
 						cloud = util3d::voxelize(cloud, _preferencesDialog->getCloudVoxelSizeScan(1));
@@ -1364,14 +1376,8 @@ void MainWindow::processStats(const rtabmap::Statistics & stat)
 
 			if(!smallMovement)
 			{
-				// keep in cache only compressed data
+				_cachedSignatures.insert(signature.id(), signature);
-				Signature signatureWithoutRawData = signature;
+				_cachedMemoryUsage += signature.sensorData().getMemoryUsed();
-				signatureWithoutRawData.sensorData().setImageRaw(cv::Mat());
-				signatureWithoutRawData.sensorData().setDepthOrRightRaw(cv::Mat());
-				signatureWithoutRawData.sensorData().setUserDataRaw(cv::Mat());
-				signatureWithoutRawData.sensorData().setLaserScanRaw(cv::Mat(), 0, 0);
-				_cachedSignatures.insert(signature.id(), signatureWithoutRawData);
-				_cachedMemoryUsage += signatureWithoutRawData.sensorData().getMemoryUsed();
 			}
 		}
 
@@ -1802,6 +1808,21 @@ void MainWindow::processStats(const rtabmap::Statistics & stat)
 				_ui->graphicsView_graphView->setCurrentGoalID(stat.currentGoalId(), uValue(stat.poses(), stat.currentGoalId(), Transform()));
 			}
 		}
+		UDEBUG("");
+
+		// keep only compressed data in cache
+		if(_cachedSignatures.contains(stat.refImageId()))
+		{
+			Signature & s = *_cachedSignatures.find(stat.refImageId());
+			_cachedMemoryUsage -= s.sensorData().getMemoryUsed();
+			s.sensorData().setImageRaw(cv::Mat());
+			s.sensorData().setDepthOrRightRaw(cv::Mat());
+			s.sensorData().setUserDataRaw(cv::Mat());
+			s.sensorData().setLaserScanRaw(cv::Mat(), signature.sensorData().laserScanInfo());
+			s.sensorData().clearOccupancyGridRaw();
+			_cachedMemoryUsage += s.sensorData().getMemoryUsed();
+		}
+
 
 		UDEBUG("");
 	}
@@ -1829,7 +1850,10 @@ void MainWindow::processStats(const rtabmap::Statistics & stat)
 	}
 	_ui->statsToolBox->updateStat("GUI/Cache Data Size/MB", _preferencesDialog->isTimeUsedInFigures()?stat.stamp()-_firstStamp:stat.refImageId(), _cachedMemoryUsage/(1024*1024));
 	_ui->statsToolBox->updateStat("GUI/Cache Clouds Size/MB", _preferencesDialog->isTimeUsedInFigures()?stat.stamp()-_firstStamp:stat.refImageId(), _createdCloudsMemoryUsage/(1024*1024));
-
+	_ui->statsToolBox->updateStat("GUI/Cache Grids Size/MB", _preferencesDialog->isTimeUsedInFigures()?stat.stamp()-_firstStamp:stat.refImageId(), _cachedGridsMemoryUsage/(1024*1024));
+#ifdef RTABMAP_OCTOMAP
+	_ui->statsToolBox->updateStat("GUI/Octomap Size/MB", _preferencesDialog->isTimeUsedInFigures()?stat.stamp()-_firstStamp:stat.refImageId(), _octomap->octree()->memoryUsage()/(1024*1024));
+#endif
 	if(_state != kMonitoring && _state != kDetecting)
 	{
 		_ui->actionExport_images_RGB_jpg_Depth_png->setEnabled(!_cachedSignatures.empty() && !_currentPosesMap.empty());
@@ -1936,19 +1960,7 @@ void MainWindow::updateMapCloud(
 
 			// 3d point cloud
 			bool update3dCloud = _cloudViewer->isVisible() && _preferencesDialog->isCloudsShown(0);
-			bool updateProjMap =
+			if(update3dCloud)
-					_ui->graphicsView_graphView->isVisible() &&
-					_ui->graphicsView_graphView->isGridMapVisible() &&
-					_preferencesDialog->isGridMapFrom3DCloud() &&
-					_projectionLocalMaps.find(iter->first) == _projectionLocalMaps.end();
-			bool updateOctomap = false;
-#ifdef RTABMAP_OCTOMAP
-			updateOctomap =
-					_cloudViewer->isVisible() &&
-					_preferencesDialog->isOctomapShown() &&
-					_octomap->addedNodes().find(iter->first) == _octomap->addedNodes().end();
-#endif
-			if(update3dCloud ||	updateProjMap || updateOctomap)
 			{
 				// update cloud
 				std::pair<pcl::PointCloud<pcl::PointXYZRGB>::Ptr, pcl::IndicesPtr> createdCloud;
@@ -1976,20 +1988,6 @@ void MainWindow::updateMapCloud(
 						_cloudViewer->setCloudVisibility(cloudName.c_str(), _cloudViewer->isVisible() && _preferencesDialog->isCloudsShown(0));
 					}
 				}
-
-				//Update projection map
-				if(updateProjMap || updateOctomap)
-				{
-					std::map<int, std::pair<pcl::PointCloud<pcl::PointXYZRGB>::Ptr, pcl::IndicesPtr> >::iterator cloudIter = _cachedClouds.find(iter->first);
-					if(cloudIter != _cachedClouds.end())
-					{
-						createAndAddProjectionMap(cloudIter->second.first, cloudIter->second.second, iter->first, iter->second, updateOctomap);
-					}
-					else if(createdCloud.first.get() && createdCloud.first->size() && createdCloud.second->size())
-					{
-						createAndAddProjectionMap(createdCloud.first, createdCloud.second, iter->first, iter->second, updateOctomap);
-					}
-				}
 			}
 			else if(viewerClouds.contains(cloudName))
 			{
@@ -1998,8 +1996,7 @@ void MainWindow::updateMapCloud(
 
 			// 2d point cloud
 			std::string scanName = uFormat("scan%d", iter->first);
-			if((_cloudViewer->isVisible() && (_preferencesDialog->isScansShown(0) || _preferencesDialog->getGridMapShown())) ||
+			if(_cloudViewer->isVisible() && _preferencesDialog->isScansShown(0))
-				(_ui->graphicsView_graphView->isVisible() && _ui->graphicsView_graphView->isGridMapVisible()))
 			{
 				if(viewerClouds.contains(scanName))
 				{
@@ -2031,6 +2028,59 @@ void MainWindow::updateMapCloud(
 				_cloudViewer->setCloudVisibility(scanName.c_str(), false);
 			}
 
+			// occupancy grids
+			bool updateGridMap =
+					((_ui->graphicsView_graphView->isVisible() && _ui->graphicsView_graphView->isGridMapVisible()) ||
+					 (_cloudViewer->isVisible() && _preferencesDialog->getGridMapShown())) &&
+					_gridLocalMaps.find(iter->first) == _gridLocalMaps.end();
+			bool updateOctomap = false;
+#ifdef RTABMAP_OCTOMAP
+			updateOctomap =
+					_cloudViewer->isVisible() &&
+					_preferencesDialog->isOctomapUpdated() &&
+					_octomap->addedNodes().find(iter->first) == _octomap->addedNodes().end();
+#endif
+			if(updateGridMap || updateOctomap)
+			{
+				QMap<int, Signature>::iterator jter = _cachedSignatures.find(iter->first);
+				if(jter!=_cachedSignatures.end())
+				{
+					if(_gridLocalMaps.find(iter->first) == _gridLocalMaps.end())
+					{
+						cv::Mat ground;
+						cv::Mat obstacles;
+						jter->sensorData().uncompressDataConst(0, 0, 0, 0, &ground, &obstacles);
+						_gridLocalMaps.insert(std::make_pair(iter->first, std::make_pair(ground, obstacles)));
+						_gridViewPoints.insert(std::make_pair(iter->first, jter->sensorData().gridViewPoint()));
+						_cachedGridsMemoryUsage += ground.total()*ground.elemSize() + obstacles.total()*obstacles.elemSize();
+
+						if(ground.cols || obstacles.cols)
+						{
+							_occupancyGrid->addToCache(iter->first, ground, obstacles);
+						}
+					}
+#ifdef RTABMAP_OCTOMAP
+					if(updateOctomap)
+					{
+						std::map<int, std::pair<cv::Mat, cv::Mat> >::iterator mter = _gridLocalMaps.find(iter->first);
+						std::map<int, cv::Point3f>::iterator pter = _gridViewPoints.find(iter->first);
+						if(mter != _gridLocalMaps.end() && pter!=_gridViewPoints.end())
+						{
+							if((mter->second.first.empty() || mter->second.first.channels() > 2) &&
+							   (mter->second.second.empty() || mter->second.second.channels() > 2))
+							{
+								_octomap->addToCache(iter->first, mter->second.first, mter->second.second, pter->second);
+							}
+							else if(!mter->second.first.empty() && !mter->second.second.empty())
+							{
+								UWARN("Node %d: Cannot update octomap with 2D occupancy grids.", iter->first);
+							}
+						}
+					}
+#endif
+				}
+			}
+
 			// 3d features
 			std::string featuresName = uFormat("features%d", iter->first);
 			if(_cloudViewer->isVisible() && _preferencesDialog->isFeaturesShown(0))
@@ -2084,7 +2134,7 @@ void MainWindow::updateMapCloud(
 		_ui->actionSave_point_cloud->setEnabled(!_cachedSignatures.empty() || !_cachedClouds.empty());
 		_ui->actionView_high_res_point_cloud->setEnabled(!_cachedSignatures.empty() || !_cachedClouds.empty());
 		_ui->actionExport_2D_scans_ply_pcd->setEnabled(!_createdScans.empty());
-		_ui->actionExport_2D_Grid_map_bmp_png->setEnabled(!_gridLocalMaps.empty() || !_projectionLocalMaps.empty());
+		_ui->actionExport_2D_Grid_map_bmp_png->setEnabled(!_gridLocalMaps.empty());
 		_ui->actionView_scans->setEnabled(!_createdScans.empty());
 #ifdef RTABMAP_OCTOMAP
 		_ui->actionExport_octomap->setEnabled(_octomap->octree()->size());
@@ -2196,19 +2246,37 @@ void MainWindow::updateMapCloud(
 	}
 	cv::Mat map8U;
 	if((_ui->graphicsView_graphView->isVisible() || _preferencesDialog->getGridMapShown()) &&
-			((_gridLocalMaps.size() && !_preferencesDialog->isGridMapFrom3DCloud()) ||
+		_gridLocalMaps.size())
-			 (_projectionLocalMaps.size() && _preferencesDialog->isGridMapFrom3DCloud())))
 	{
 		float xMin, yMin;
 		float resolution = _preferencesDialog->getGridMapResolution();
-		cv::Mat map8S = util3d::create2DMapFromOccupancyLocalMaps(
+		cv::Mat map8S;
-					poses,
+#ifdef RTABMAP_OCTOMAP
-					_preferencesDialog->isGridMapFrom3DCloud()?_projectionLocalMaps:_gridLocalMaps,
+		if(_preferencesDialog->isOctomap2dGrid())
-					resolution,
+		{
-					xMin, yMin,
+			map8S = _octomap->createProjectionMap(xMin, yMin, resolution, 0);
-					0,
+
-					_preferencesDialog->isGridMapEroded(),
+		}
-					_preferencesDialog->getGridMapFootprintRadius());
+		else
+#endif
+		{
+			if(_preferencesDialog->isGridMapIncremental())
+			{
+				_occupancyGrid->update(poses, 0, _preferencesDialog->getGridMapFootprintRadius());
+				map8S = _occupancyGrid->getMap(xMin, yMin);
+			}
+			else
+			{
+				map8S = util3d::create2DMapFromOccupancyLocalMaps(
+							poses,
+							_gridLocalMaps,
+							resolution,
+							xMin, yMin,
+							0,
+							_preferencesDialog->isGridMapEroded(),
+							_preferencesDialog->getGridMapFootprintRadius());
+			}
+		}
 		if(!map8S.empty())
 		{
 			//convert to gray scaled map
@@ -2237,14 +2305,20 @@ void MainWindow::updateMapCloud(
 
 #ifdef RTABMAP_OCTOMAP
 	_cloudViewer->removeOctomap();
-	if(_preferencesDialog->isOctomapShown())
+	if(_preferencesDialog->isOctomapUpdated())
 	{
 		UDEBUG("");
 		UTimer time;
 		_octomap->update(poses);
-		_cloudViewer->addOctomap(_octomap, _preferencesDialog->getOctomapTreeDepth());
 		UINFO("Octomap update time = %fs", time.ticks());
 	}
+	if(_preferencesDialog->isOctomapShown())
+	{
+		UDEBUG("");
+		UTimer time;
+		_cloudViewer->addOctomap(_octomap, _preferencesDialog->getOctomapTreeDepth());
+		UINFO("Octomap show 3d map time = %fs", time.ticks());
+	}
 #endif
 
 	if(viewerClouds.contains("cloudOdom"))
@@ -2576,103 +2650,6 @@ std::pair<pcl::PointCloud<pcl::PointXYZRGB>::Ptr, pcl::IndicesPtr> MainWindow::c
 	return outputPair;
 }
 
-void MainWindow::createAndAddProjectionMap(
-		const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
-		const pcl::IndicesPtr & indices,
-		int nodeId,
-		const Transform & pose,
-		bool updateOctomap)
-{
-	UDEBUG("");
-	UASSERT(!pose.isNull());
-
-	if(_projectionLocalMaps.find(nodeId) != _projectionLocalMaps.end() && !updateOctomap)
-	{
-		UERROR("Projection map %d already added.", nodeId);
-		return;
-	}
-
-	if(indices->size())
-	{
-		UTimer timer;
-		cv::Mat ground, obstacles;
-		pcl::PointCloud<pcl::PointXYZRGB>::Ptr voxelCloud = cloud;
-
-		// voxelize to grid cell size
-		if(_preferencesDialog->getMapVoxel() < _preferencesDialog->getGridMapResolution())
-		{
-			voxelCloud = util3d::voxelize(voxelCloud, indices, _preferencesDialog->getGridMapResolution());
-		}
-
-		// add pose rotation without yaw
-		float roll, pitch, yaw;
-		pose.getEulerAngles(roll, pitch, yaw);
-		voxelCloud = util3d::transformPointCloud(voxelCloud, Transform(0,0, _preferencesDialog->projMapFrame()?pose.z():0, roll, pitch, 0));
-
-		if(_preferencesDialog->projMaxObstaclesHeight())
-		{
-			voxelCloud = util3d::passThrough(voxelCloud, "z", std::numeric_limits<int>::min(), _preferencesDialog->projMaxObstaclesHeight());
-		}
-
-		pcl::IndicesPtr groundIndices, obstaclesIndices;
-		util3d::segmentObstaclesFromGround<pcl::PointXYZRGB>(
-				voxelCloud,
-				groundIndices,
-				obstaclesIndices,
-				20,
-				_preferencesDialog->projMaxGroundAngle(),
-				_preferencesDialog->getGridMapResolution()*2.0f,
-				_preferencesDialog->projMinClusterSize(),
-				_preferencesDialog->projFlatObstaclesDetected(),
-				_preferencesDialog->projMaxGroundHeight());
-
-		pcl::PointCloud<pcl::PointXYZRGB>::Ptr groundCloud(new pcl::PointCloud<pcl::PointXYZRGB>);
-		pcl::PointCloud<pcl::PointXYZRGB>::Ptr obstaclesCloud(new pcl::PointCloud<pcl::PointXYZRGB>);
-
-		if(groundIndices->size())
-		{
-			pcl::copyPointCloud(*voxelCloud, *groundIndices, *groundCloud);
-		}
-
-		if(obstaclesIndices->size())
-		{
-			pcl::copyPointCloud(*voxelCloud, *obstaclesIndices, *obstaclesCloud);
-		}
-
-		util3d::occupancy2DFromGroundObstacles<pcl::PointXYZRGB>(
-				groundCloud,
-				obstaclesCloud,
-				ground,
-				obstacles,
-				_preferencesDialog->getGridMapResolution());
-
-		if(updateOctomap)
-		{
-			// Update octomap
-#ifdef RTABMAP_OCTOMAP
-			if(_octomap->addedNodes().empty() ||
-				nodeId > _octomap->addedNodes().rbegin()->first)
-			{
-				Transform tinv = Transform(0,0,_preferencesDialog->projMapFrame()?pose.z():0, roll, pitch, 0).inverse();
-				groundCloud = util3d::transformPointCloud(groundCloud, tinv);
-				obstaclesCloud = util3d::transformPointCloud(obstaclesCloud, tinv);
-
-				if(_preferencesDialog->isOctomapGroundAnObstacle())
-				{
-					*obstaclesCloud += *groundCloud;
-					groundCloud->clear();
-				}
-				_octomap->addToCache(nodeId, groundCloud, obstaclesCloud);
-			}
-#endif
-		}
-
-		_projectionLocalMaps.insert(std::make_pair(nodeId, std::make_pair(ground, obstacles)));
-		UDEBUG("time gridMapFrom3DCloud = %f s", timer.ticks());
-	}
-	UDEBUG("");
-}
-
 void MainWindow::createAndAddScanToMap(int nodeId, const Transform & pose, int mapId)
 {
 	std::string scanName = uFormat("scan%d", nodeId);
@@ -2702,7 +2679,7 @@ void MainWindow::createAndAddScanToMap(int nodeId, const Transform & pose, int m
 		if(scan.channels() == 6)
 		{
 			pcl::PointCloud<pcl::PointNormal>::Ptr cloud;
-			cloud = util3d::laserScanToPointCloudNormal(scan);
+			cloud = util3d::laserScanToPointCloudNormal(scan, iter->sensorData().laserScanInfo().localTransform());
 			if(_preferencesDialog->getCloudVoxelSizeScan(0) > 0.0)
 			{
 				cloud = util3d::voxelize(cloud, _preferencesDialog->getCloudVoxelSizeScan(0));
@@ -2729,7 +2706,7 @@ void MainWindow::createAndAddScanToMap(int nodeId, const Transform & pose, int m
 		else
 		{
 			pcl::PointCloud<pcl::PointXYZ>::Ptr cloud;
-			cloud = util3d::laserScanToPointCloud(scan);
+			cloud = util3d::laserScanToPointCloud(scan, iter->sensorData().laserScanInfo().localTransform());
 			if(_preferencesDialog->getCloudVoxelSizeScan(0) > 0.0)
 			{
 				cloud = util3d::voxelize(cloud, _preferencesDialog->getCloudVoxelSizeScan(0));
@@ -2758,13 +2735,6 @@ void MainWindow::createAndAddScanToMap(int nodeId, const Transform & pose, int m
 					}
 				}
 				_createdScans.insert(std::make_pair(nodeId, scan));
-
-				if(scan.channels() == 2)
-				{
-					cv::Mat ground, obstacles;
-					util3d::occupancy2DFromLaserScan(scan, ground, obstacles, _preferencesDialog->getGridMapResolution());
-					_gridLocalMaps.insert(std::make_pair(nodeId, std::make_pair(ground, obstacles)));
-				}
 			}
 		}
 		_cloudViewer->setCloudOpacity(scanName, _preferencesDialog->getScanOpacity(0));
@@ -3340,6 +3310,7 @@ void MainWindow::applyPrefSettings(const rtabmap::ParametersMap & parameters)
 void MainWindow::applyPrefSettings(const rtabmap::ParametersMap & parameters, bool postParamEvent)
 {
 	ULOGGER_DEBUG("");
+	_occupancyGrid->parseParameters(parameters);
 	if(parameters.size())
 	{
 		for(rtabmap::ParametersMap::const_iterator iter = parameters.begin(); iter!=parameters.end(); ++iter)
@@ -4056,6 +4027,9 @@ void MainWindow::startDetection()
 	_octomap = new OctoMap(_preferencesDialog->getGridMapResolution());
 #endif
 
+	_occupancyGrid->clear();
+	_occupancyGrid->parseParameters(parameters);
+
 	emit stateChanged(kDetecting);
 }
 
@@ -5023,7 +4997,7 @@ void MainWindow::clearTheCache()
 	_previousCloud.second.second.reset();
 	_createdScans.clear();
 	_gridLocalMaps.clear();
-	_projectionLocalMaps.clear();
+	_cachedGridsMemoryUsage = 0;
 	_createdFeatures.clear();
 	_cloudViewer->clear();
 	_cloudViewer->setBackgroundColor(_cloudViewer->getDefaultBackgroundColor());
@@ -5072,6 +5046,7 @@ void MainWindow::clearTheCache()
 	delete _octomap;
 	_octomap = new OctoMap(_preferencesDialog->getGridMapResolution());
 #endif
+	_occupancyGrid->clear();
 }
 
 void MainWindow::updateElapsedTime()
@@ -5325,9 +5300,9 @@ void MainWindow::setAspectRatioCustom()
 
 void MainWindow::exportGridMap()
 {
-	double gridCellSize = 0.05;
+	float gridCellSize = 0.05f;
 	bool ok;
-	gridCellSize = QInputDialog::getDouble(this, tr("Grid cell size"), tr("Size (m):"), gridCellSize, 0.01, 1, 2, &ok);
+	gridCellSize = (float)QInputDialog::getDouble(this, tr("Grid cell size"), tr("Size (m):"), (double)gridCellSize, 0.01, 1, 2, &ok);
 	if(!ok)
 	{
 		return;
@@ -5337,13 +5312,24 @@ void MainWindow::exportGridMap()
 
 	// create the map
 	float xMin=0.0f, yMin=0.0f;
-	cv::Mat pixels = util3d::create2DMapFromOccupancyLocalMaps(
+	cv::Mat pixels;
+#ifdef RTABMAP_OCTOMAP
+		if(_preferencesDialog->isOctomap2dGrid())
+		{
+			pixels = _octomap->createProjectionMap(xMin, yMin, gridCellSize, 0);
+
+		}
+		else
+#endif
+		{
+			pixels = util3d::create2DMapFromOccupancyLocalMaps(
 				poses,
-				_preferencesDialog->isGridMapFrom3DCloud()?_projectionLocalMaps:_gridLocalMaps,
+				_gridLocalMaps,
 				gridCellSize,
 				xMin, yMin,
 				0,
 				_preferencesDialog->isGridMapEroded());
+		}
 
 	if(!pixels.empty())
 	{
@@ -5946,7 +5932,7 @@ void MainWindow::changeState(MainWindow::State newState)
 		_ui->actionSave_point_cloud->setEnabled(!_cachedSignatures.empty() || !_cachedClouds.empty());
 		_ui->actionView_high_res_point_cloud->setEnabled(!_cachedSignatures.empty() || !_cachedClouds.empty());
 		_ui->actionExport_2D_scans_ply_pcd->setEnabled(!_createdScans.empty());
-		_ui->actionExport_2D_Grid_map_bmp_png->setEnabled(!_gridLocalMaps.empty() || !_projectionLocalMaps.empty());
+		_ui->actionExport_2D_Grid_map_bmp_png->setEnabled(!_gridLocalMaps.empty());
 		_ui->actionView_scans->setEnabled(!_createdScans.empty());
 #ifdef RTABMAP_OCTOMAP
 		_ui->actionExport_octomap->setEnabled(_octomap->octree()->size());
@@ -6007,7 +5993,7 @@ void MainWindow::changeState(MainWindow::State newState)
 		_ui->actionSave_point_cloud->setEnabled(!_cachedSignatures.empty() || !_cachedClouds.empty());
 		_ui->actionView_high_res_point_cloud->setEnabled(!_cachedSignatures.empty() || !_cachedClouds.empty());
 		_ui->actionExport_2D_scans_ply_pcd->setEnabled(!_createdScans.empty());
-		_ui->actionExport_2D_Grid_map_bmp_png->setEnabled(!_gridLocalMaps.empty() || !_projectionLocalMaps.empty());
+		_ui->actionExport_2D_Grid_map_bmp_png->setEnabled(!_gridLocalMaps.empty());
 		_ui->actionView_scans->setEnabled(!_createdScans.empty());
 #ifdef RTABMAP_OCTOMAP
 		_ui->actionExport_octomap->setEnabled(_octomap->octree()->size());
@@ -6130,7 +6116,7 @@ void MainWindow::changeState(MainWindow::State newState)
 			_ui->actionSave_point_cloud->setEnabled(!_cachedSignatures.empty() || !_cachedClouds.empty());
 			_ui->actionView_high_res_point_cloud->setEnabled(!_cachedSignatures.empty() || !_cachedClouds.empty());
 			_ui->actionExport_2D_scans_ply_pcd->setEnabled(!_createdScans.empty());
-			_ui->actionExport_2D_Grid_map_bmp_png->setEnabled(!_gridLocalMaps.empty() || !_projectionLocalMaps.empty());
+			_ui->actionExport_2D_Grid_map_bmp_png->setEnabled(!_gridLocalMaps.empty());
 			_ui->actionView_scans->setEnabled(!_createdScans.empty());
 #ifdef RTABMAP_OCTOMAP
 			_ui->actionExport_octomap->setEnabled(_octomap->octree()->size());
@@ -6195,7 +6181,7 @@ void MainWindow::changeState(MainWindow::State newState)
 		_ui->actionSave_point_cloud->setEnabled(!_cachedSignatures.empty() || !_cachedClouds.empty());
 		_ui->actionView_high_res_point_cloud->setEnabled(!_cachedSignatures.empty() || !_cachedClouds.empty());
 		_ui->actionExport_2D_scans_ply_pcd->setEnabled(!_createdScans.empty());
-		_ui->actionExport_2D_Grid_map_bmp_png->setEnabled(!_gridLocalMaps.empty() || !_projectionLocalMaps.empty());
+		_ui->actionExport_2D_Grid_map_bmp_png->setEnabled(!_gridLocalMaps.empty());
 		_ui->actionView_scans->setEnabled(!_createdScans.empty());
 #ifdef RTABMAP_OCTOMAP
 		_ui->actionExport_octomap->setEnabled(_octomap->octree()->size());

guilib/src/PreferencesDialog.cpp

@@ -378,17 +378,11 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	connect(_ui->doubleSpinBox_map_opacity, SIGNAL(valueChanged(double)), this, SLOT(makeObsoleteCloudRenderingPanel()));
 	connect(_ui->checkBox_map_erode, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteCloudRenderingPanel()));
 	connect(_ui->doubleSpinBox_map_footprintRadius, SIGNAL(valueChanged(double)), this, SLOT(makeObsoleteCloudRenderingPanel()));
-	connect(_ui->groupBox_map_occupancyFrom3DCloud, SIGNAL(toggled(bool)), this, SLOT(makeObsoleteCloudRenderingPanel()));
-	connect(_ui->checkBox_projMapFrame, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteCloudRenderingPanel()));
-	connect(_ui->doubleSpinBox_projMaxGroundAngle, SIGNAL(valueChanged(double)), this, SLOT(makeObsoleteCloudRenderingPanel()));
-	connect(_ui->doubleSpinBox_projMaxGroundHeight, SIGNAL(valueChanged(double)), this, SLOT(makeObsoleteCloudRenderingPanel()));
-	connect(_ui->spinBox_projMinClusterSize, SIGNAL(valueChanged(int)), this, SLOT(makeObsoleteCloudRenderingPanel()));
-	connect(_ui->doubleSpinBox_projMaxObstaclesHeight, SIGNAL(valueChanged(double)), this, SLOT(makeObsoleteCloudRenderingPanel()));
-	connect(_ui->checkBox_projFlatObstaclesDetected, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteCloudRenderingPanel()));
 
 	connect(_ui->groupBox_octomap, SIGNAL(toggled(bool)), this, SLOT(makeObsoleteCloudRenderingPanel()));
 	connect(_ui->spinBox_octomap_treeDepth, SIGNAL(valueChanged(int)), this, SLOT(makeObsoleteCloudRenderingPanel()));
-	connect(_ui->checkBox_octomap_groundObstacle, SIGNAL(clicked(bool)), this, SLOT(makeObsoleteCloudRenderingPanel()));
+	connect(_ui->checkBox_octomap_2dgrid, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteCloudRenderingPanel()));
+	connect(_ui->checkBox_octomap_show3dMap, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteCloudRenderingPanel()));
 
 	connect(_ui->groupBox_organized, SIGNAL(toggled(bool)), this, SLOT(makeObsoleteCloudRenderingPanel()));
 	connect(_ui->doubleSpinBox_mesh_angleTolerance, SIGNAL(valueChanged(double)), this, SLOT(makeObsoleteCloudRenderingPanel()));
@@ -728,6 +722,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	_ui->checkBox_localSpacePathOdomPosesUsed->setObjectName(Parameters::kRGBDProximityPathRawPosesUsed().c_str());
 	_ui->rgdb_localImmunizationRatio->setObjectName(Parameters::kRGBDLocalImmunizationRatio().c_str());
 	_ui->loopClosure_reextract->setObjectName(Parameters::kRGBDLoopClosureReextractFeatures().c_str());
+	_ui->checkbox_rgbd_createOccupancyGRid->setObjectName(Parameters::kRGBDCreateOccupancyGrid().c_str());
 
 	// Registration
 	_ui->loopClosure_bowVarianceFromInliersCount->setObjectName(Parameters::kRegVarianceFromInliersCount().c_str());
@@ -778,6 +773,29 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	_ui->loopClosure_icpPointToPlane->setObjectName(Parameters::kIcpPointToPlane().c_str());
 	_ui->loopClosure_icpPointToPlaneNormals->setObjectName(Parameters::kIcpPointToPlaneNormalNeighbors().c_str());
 
+	// Occupancy grid
+	_ui->groupBox_grid_3d->setObjectName(Parameters::kGrid3D().c_str());
+	_ui->checkBox_grid_groundObstacle->setObjectName(Parameters::kGrid3DGroundIsObstacle().c_str());
+	_ui->doubleSpinBox_grid_resolution->setObjectName(Parameters::kGridCellSize().c_str());
+	_ui->spinBox_grid_decimation->setObjectName(Parameters::kGridDepthDecimation().c_str());
+	_ui->doubleSpinBox_grid_maxDepth->setObjectName(Parameters::kGridDepthMax().c_str());
+	_ui->doubleSpinBox_grid_minDepth->setObjectName(Parameters::kGridDepthMin().c_str());
+	_ui->lineEdit_grid_roi->setObjectName(Parameters::kGridDepthRoiRatios().c_str());
+	_ui->checkBox_grid_flatObstaclesDetected->setObjectName(Parameters::kGridFlatObstacleDetected().c_str());
+	_ui->groupBox_grid_fromDepthImage->setObjectName(Parameters::kGridFromDepth().c_str());
+	_ui->checkBox_grid_projMapFrame->setObjectName(Parameters::kGridMapFrameProjection().c_str());
+	_ui->doubleSpinBox_grid_maxGroundAngle->setObjectName(Parameters::kGridMaxGroundAngle().c_str());
+	_ui->spinBox_grid_normalK->setObjectName(Parameters::kGridNormalK().c_str());
+	_ui->doubleSpinBox_grid_maxGroundHeight->setObjectName(Parameters::kGridMaxGroundHeight().c_str());
+	_ui->doubleSpinBox_grid_maxObstacleHeight->setObjectName(Parameters::kGridMaxObstacleHeight().c_str());
+	_ui->spinBox_grid_minClusterSize->setObjectName(Parameters::kGridMinClusterSize().c_str());
+	_ui->doubleSpinBox_grid_minGroundHeight->setObjectName(Parameters::kGridMinGroundHeight().c_str());
+	_ui->spinBox_grid_noiseMinNeighbors->setObjectName(Parameters::kGridNoiseFilteringMinNeighbors().c_str());
+	_ui->doubleSpinBox_grid_noiseRadius->setObjectName(Parameters::kGridNoiseFilteringRadius().c_str());
+	_ui->groupBox_grid_normalsSegmentation->setObjectName(Parameters::kGridNormalsSegmentation().c_str());
+	_ui->checkBox_grid_unknownSpaceFilled->setObjectName(Parameters::kGridScan2dUnknownSpaceFilled().c_str());
+	_ui->doubleSpinBox_grid_unknownSpaceFilledMaxRange->setObjectName(Parameters::kGridScan2dMaxFilledRange().c_str());
+	_ui->spinBox_grid_scanDecimation->setObjectName(Parameters::kGridScanDecimation().c_str());
 
 	//Odometry
 	_ui->odom_strategy->setObjectName(Parameters::kOdomStrategy().c_str());
@@ -1221,19 +1239,14 @@ void PreferencesDialog::resetSettings(QGroupBox * groupBox)
 		_ui->checkBox_map_shown->setChecked(false);
 		_ui->doubleSpinBox_map_resolution->setValue(0.05);
 		_ui->checkBox_map_erode->setChecked(false);
+		_ui->checkBox_map_incremental->setChecked(false);
 		_ui->doubleSpinBox_map_footprintRadius->setValue(0);
 		_ui->doubleSpinBox_map_opacity->setValue(0.75);
-		_ui->groupBox_map_occupancyFrom3DCloud->setChecked(false);
-		_ui->checkBox_projMapFrame->setChecked(true);
-		_ui->doubleSpinBox_projMaxGroundAngle->setValue(30);
-		_ui->doubleSpinBox_projMaxGroundHeight->setValue(0);
-		_ui->spinBox_projMinClusterSize->setValue(20);
-		_ui->doubleSpinBox_projMaxObstaclesHeight->setValue(0);
-		_ui->checkBox_projFlatObstaclesDetected->setChecked(true);
 
 		_ui->groupBox_octomap->setChecked(false);
 		_ui->spinBox_octomap_treeDepth->setValue(16);
-		_ui->checkBox_octomap_groundObstacle->setChecked(true);
+		_ui->checkBox_octomap_2dgrid->setChecked(true);
+		_ui->checkBox_octomap_show3dMap->setChecked(true);
 	}
 	else if(groupBox->objectName() == _ui->groupBox_logging1->objectName())
 	{
@@ -1577,19 +1590,14 @@ void PreferencesDialog::readGuiSettings(const QString & filePath)
 	_ui->checkBox_map_shown->setChecked(settings.value("gridMapShown", _ui->checkBox_map_shown->isChecked()).toBool());
 	_ui->doubleSpinBox_map_resolution->setValue(settings.value("gridMapResolution", _ui->doubleSpinBox_map_resolution->value()).toDouble());
 	_ui->checkBox_map_erode->setChecked(settings.value("gridMapEroded", _ui->checkBox_map_erode->isChecked()).toBool());
+	_ui->checkBox_map_incremental->setChecked(settings.value("gridMapIncremental", _ui->checkBox_map_incremental->isChecked()).toBool());
 	_ui->doubleSpinBox_map_footprintRadius->setValue(settings.value("gridMapFootprintRadius", _ui->doubleSpinBox_map_footprintRadius->value()).toDouble());
 	_ui->doubleSpinBox_map_opacity->setValue(settings.value("gridMapOpacity", _ui->doubleSpinBox_map_opacity->value()).toDouble());
-	_ui->groupBox_map_occupancyFrom3DCloud->setChecked(settings.value("gridMapOccupancyFrom3DCloud", _ui->groupBox_map_occupancyFrom3DCloud->isChecked()).toBool());
-	_ui->checkBox_projMapFrame->setChecked(settings.value("projMapFrame", _ui->checkBox_projMapFrame->isChecked()).toBool());
-	_ui->doubleSpinBox_projMaxGroundAngle->setValue(settings.value("projMaxGroundAngle", _ui->doubleSpinBox_projMaxGroundAngle->value()).toDouble());
-	_ui->doubleSpinBox_projMaxGroundHeight->setValue(settings.value("projMaxGroundHeight", _ui->doubleSpinBox_projMaxGroundHeight->value()).toDouble());
-	_ui->spinBox_projMinClusterSize->setValue(settings.value("projMinClusterSize", _ui->spinBox_projMinClusterSize->value()).toInt());
-	_ui->doubleSpinBox_projMaxObstaclesHeight->setValue(settings.value("projMaxObstaclesHeight", _ui->doubleSpinBox_projMaxObstaclesHeight->value()).toDouble());
-	_ui->checkBox_projFlatObstaclesDetected->setChecked(settings.value("projFlatObstaclesDetected", _ui->checkBox_projFlatObstaclesDetected->isChecked()).toBool());
 
 	_ui->groupBox_octomap->setChecked(settings.value("octomap", _ui->groupBox_octomap->isChecked()).toBool());
 	_ui->spinBox_octomap_treeDepth->setValue(settings.value("octomap_depth", _ui->spinBox_octomap_treeDepth->value()).toInt());
-	_ui->checkBox_octomap_groundObstacle->setChecked(settings.value("octomap_ground_is_obstacle", _ui->checkBox_octomap_groundObstacle->isChecked()).toBool());
+	_ui->checkBox_octomap_2dgrid->setChecked(settings.value("octomap_2dgrid", _ui->checkBox_octomap_2dgrid->isChecked()).toBool());
+	_ui->checkBox_octomap_show3dMap->setChecked(settings.value("octomap_3dmap", _ui->checkBox_octomap_show3dMap->isChecked()).toBool());
 
 	_ui->groupBox_organized->setChecked(settings.value("meshing", _ui->groupBox_organized->isChecked()).toBool());
 	_ui->doubleSpinBox_mesh_angleTolerance->setValue(settings.value("meshing_angle", _ui->doubleSpinBox_mesh_angleTolerance->value()).toDouble());
@@ -1994,20 +2002,14 @@ void PreferencesDialog::writeGuiSettings(const QString & filePath) const
 	settings.setValue("gridMapShown",                _ui->checkBox_map_shown->isChecked());
 	settings.setValue("gridMapResolution",           _ui->doubleSpinBox_map_resolution->value());
 	settings.setValue("gridMapEroded",               _ui->checkBox_map_erode->isChecked());
+	settings.setValue("gridMapIncremental",          _ui->checkBox_map_incremental->isChecked());
 	settings.setValue("gridMapFootprintRadius",      _ui->doubleSpinBox_map_footprintRadius->value());
 	settings.setValue("gridMapOpacity",              _ui->doubleSpinBox_map_opacity->value());
 
-	settings.setValue("gridMapOccupancyFrom3DCloud", _ui->groupBox_map_occupancyFrom3DCloud->isChecked());
-	settings.setValue("projMapFrame",                _ui->checkBox_projMapFrame->isChecked());
-	settings.setValue("projMaxGroundAngle",          _ui->doubleSpinBox_projMaxGroundAngle->value());
-	settings.setValue("projMaxGroundHeight",         _ui->doubleSpinBox_projMaxGroundHeight->value());
-	settings.setValue("projMinClusterSize",          _ui->spinBox_projMinClusterSize->value());
-	settings.setValue("projMaxObstaclesHeight",      _ui->doubleSpinBox_projMaxObstaclesHeight->value());
-	settings.setValue("projFlatObstaclesDetected",   _ui->checkBox_projFlatObstaclesDetected->isChecked());
-
 	settings.setValue("octomap",                     _ui->groupBox_octomap->isChecked());
 	settings.setValue("octomap_depth",               _ui->spinBox_octomap_treeDepth->value());
-	settings.setValue("octomap_ground_is_obstacle", _ui->checkBox_octomap_groundObstacle->isChecked());
+	settings.setValue("octomap_2dgrid",              _ui->checkBox_octomap_2dgrid->isChecked());
+	settings.setValue("octomap_3dmap",               _ui->checkBox_octomap_show3dMap->isChecked());
 
 	settings.setValue("meshing",               _ui->groupBox_organized->isChecked());
 	settings.setValue("meshing_angle",         _ui->doubleSpinBox_mesh_angleTolerance->value());
@@ -2659,9 +2661,10 @@ QString PreferencesDialog::loadCustomConfig(const QString & section, const QStri
 
 rtabmap::ParametersMap PreferencesDialog::getAllParameters() const
 {
-	UASSERT_MSG(_parameters.size() == Parameters::getDefaultParameters().size(),
+	if(_parameters.size() != Parameters::getDefaultParameters().size())
-			uFormat("%d vs %d (Is PreferencesDialog::init() called?)", (int)_parameters.size(), (int)Parameters::getDefaultParameters().size()).c_str());
+	{
-
+		UWARN("%d vs %d (Is PreferencesDialog::init() called?)", (int)_parameters.size(), (int)Parameters::getDefaultParameters().size());
+	}
 	ParametersMap parameters = _parameters;
 	uInsert(parameters, _modifiedParameters);
 
@@ -3687,20 +3690,34 @@ bool PreferencesDialog::isCloudsShown(int index) const
 	UASSERT(index >= 0 && index <= 1);
 	return _3dRenderingShowClouds[index]->isChecked();
 }
-bool PreferencesDialog::isOctomapShown() const
+bool PreferencesDialog::isOctomapUpdated() const
 {
 #ifdef RTABMAP_OCTOMAP
 	return _ui->groupBox_octomap->isChecked();
 #endif
 	return false;
 }
+bool PreferencesDialog::isOctomapShown() const
+{
+#ifdef RTABMAP_OCTOMAP
+	return _ui->groupBox_octomap->isChecked() && _ui->checkBox_octomap_show3dMap->isChecked();
+#endif
+	return false;
+}
+bool PreferencesDialog::isOctomap2dGrid() const
+{
+#ifdef RTABMAP_OCTOMAP
+	return _ui->groupBox_octomap->isChecked() && _ui->checkBox_octomap_2dgrid->isChecked();
+#endif
+	return false;
+}
 int PreferencesDialog::getOctomapTreeDepth() const
 {
 	return _ui->spinBox_octomap_treeDepth->value();
 }
 bool PreferencesDialog::isOctomapGroundAnObstacle() const
 {
-	return _ui->checkBox_octomap_groundObstacle->isChecked();
+	return _ui->checkBox_grid_groundObstacle->isChecked();
 }
 
 double PreferencesDialog::getMapVoxel() const
@@ -3847,37 +3864,41 @@ bool PreferencesDialog::isGridMapEroded() const
 {
 	return _ui->checkBox_map_erode->isChecked();
 }
+bool PreferencesDialog::isGridMapIncremental() const
+{
+	return _ui->checkBox_map_incremental->isChecked();
+}
 double PreferencesDialog::getGridMapFootprintRadius() const
 {
 	return _ui->doubleSpinBox_map_footprintRadius->value();
 }
 bool PreferencesDialog::isGridMapFrom3DCloud() const
 {
-	return _ui->groupBox_map_occupancyFrom3DCloud->isChecked();
+	return _ui->groupBox_grid_fromDepthImage->isChecked();
 }
 bool PreferencesDialog::projMapFrame() const
 {
-	return _ui->checkBox_projMapFrame->isChecked();
+	return _ui->checkBox_grid_projMapFrame->isChecked();
 }
 double PreferencesDialog::projMaxGroundAngle() const
 {
-	return _ui->doubleSpinBox_projMaxGroundAngle->value()*M_PI/180.0;
+	return _ui->doubleSpinBox_grid_maxGroundAngle->value()*M_PI/180.0;
 }
 double PreferencesDialog::projMaxGroundHeight() const
 {
-	return _ui->doubleSpinBox_projMaxGroundHeight->value();
+	return _ui->doubleSpinBox_grid_maxGroundHeight->value();
 }
 int PreferencesDialog::projMinClusterSize() const
 {
-	return _ui->spinBox_projMinClusterSize->value();
+	return _ui->spinBox_grid_minClusterSize->value();
 }
 double PreferencesDialog::projMaxObstaclesHeight() const
 {
-	return _ui->doubleSpinBox_projMaxObstaclesHeight->value();
+	return _ui->doubleSpinBox_grid_maxObstacleHeight->value();
 }
 bool PreferencesDialog::projFlatObstaclesDetected() const
 {
-	return _ui->checkBox_projFlatObstaclesDetected->isChecked();
+	return _ui->checkBox_grid_flatObstaclesDetected->isChecked();
 }
 double PreferencesDialog::getGridMapOpacity() const
 {

guilib/src/ui/DatabaseViewer.ui

@@ -52,7 +52,7 @@
           <rect>
            <x>0</x>
            <y>0</y>
-           <width>198</width>
+           <width>202</width>
            <height>196</height>
           </rect>
          </property>
@@ -210,7 +210,7 @@
           <rect>
            <x>0</x>
            <y>0</y>
-           <width>197</width>
+           <width>201</width>
            <height>196</height>
           </rect>
          </property>
@@ -980,7 +980,7 @@
      <item>
       <widget class="QToolBox" name="toolBox">
        <property name="currentIndex">
-        <number>1</number>
+        <number>3</number>
        </property>
        <widget class="QWidget" name="page_3">
         <property name="geometry">
@@ -1122,8 +1122,8 @@
         <property name="geometry">
          <rect>
           <x>0</x>
-          <y>-245</y>
+          <y>0</y>
-          <width>284</width>
+          <width>278</width>
           <height>611</height>
          </rect>
         </property>
@@ -1634,8 +1634,8 @@
          <rect>
           <x>0</x>
           <y>0</y>
-          <width>175</width>
+          <width>289</width>
-          <height>191</height>
+          <height>182</height>
          </rect>
         </property>
         <attribute name="label">
@@ -1673,7 +1673,7 @@
           </widget>
          </item>
          <item row="0" column="0">
-          <widget class="QCheckBox" name="checkBox_icp_laserScan">
+          <widget class="QCheckBox" name="checkBox_icp_from_depth">
            <property name="text">
             <string/>
            </property>

package.xml

@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <package>
   <name>rtabmap</name>
-  <version>0.11.9</version>
+  <version>0.11.10</version>
   <description>RTAB-Map's standalone library. RTAB-Map is a RGB-D SLAM approach with real-time constraints.</description>
   <maintainer email="[email protected]">Mathieu Labbe</maintainer>
   <author>Mathieu Labbe</author>