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.

2026-09-02 09:30:25 +08:00 · 2016-08-21 19:33:01 -04:00
parent af02e02978
commit 013eba1d58
49 changed files with 3376 additions and 1259 deletions
--- a/corelib/include/rtabmap/core/CameraModel.h
+++ b/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
--- a/corelib/include/rtabmap/core/DBDriver.h
+++ b/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 getNodeData(int signatureId, SensorData & data) 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, 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;
--- a/corelib/include/rtabmap/core/LaserScanInfo.h
+++ b/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_ */
--- a/corelib/include/rtabmap/core/Memory.h
+++ b/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
--- a/corelib/include/rtabmap/core/OccupancyGrid.h
+++ b/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_
-#define CORELIB_SRC_OCCUPANCY_H_
+#ifndef CORELIB_SRC_OCCUPANCYGRID_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_ */
--- a/corelib/include/rtabmap/core/OctoMap.h
+++ b/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 */
--- a/corelib/include/rtabmap/core/Parameters.h
+++ b/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, 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, PnPReprojError, 	         float, 2,      "[Vis/EstimationType = 1] PnP reprojection error.");
-	RTABMAP_PARAM(Vis, PnPFlags,                 int, 1,        "[Vis/EstimationType = 1] PnP flags: 0=Iterative, 1=EPNP, 2=P3P");
-	RTABMAP_PARAM(Vis, PnPRefineIterations,      int, 1,        "[Vis/EstimationType = 1] Refine iterations.");
-	RTABMAP_PARAM(Vis, EpipolarGeometryVar,      float, 0.02,   "[Vis/EstimationType = 2] Epipolar geometry maximum variance to accept the transformation.");
+	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,        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,      uFormat("[%s = 1] PnP reprojection error.", kVisEstimationType().c_str()));
+	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,        uFormat("[%s = 1] Refine iterations.", kVisEstimationType().c_str()));
+	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, CorNNDR,                  float, 0.8,    "[Vis/CorrespondenceType=0] NNDR: nearest neighbor distance ratio. Used for features matching approach.");
-	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, CorFlowWinSize,           int, 16,       "[Vis/CorrespondenceType=1] See cv::calcOpticalFlowPyrLK(). Used for optical flow approach.");
-	RTABMAP_PARAM(Vis, CorFlowIterations,        int, 30,       "[Vis/CorrespondenceType=1] See cv::calcOpticalFlowPyrLK(). Used for optical flow approach.");
-	RTABMAP_PARAM(Vis, CorFlowEps,               float, 0.01,   "[Vis/CorrespondenceType=1] See cv::calcOpticalFlowPyrLK(). Used for optical flow approach.");
-	RTABMAP_PARAM(Vis, CorFlowMaxLevel,          int, 3,        "[Vis/CorrespondenceType=1] See cv::calcOpticalFlowPyrLK(). Used for optical flow 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,    uFormat("[%s=0] NNDR: nearest neighbor distance ratio. Used for features matching approach.", kVisCorType().c_str()));
+	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,       uFormat("[%s=1] See cv::calcOpticalFlowPyrLK(). Used for optical flow approach.", kVisCorType().c_str()));
+	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,   uFormat("[%s=1] See cv::calcOpticalFlowPyrLK(). Used for optical flow approach.", kVisCorType().c_str()));
+	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, Eps,                   double, 0.01,  "[Stereo/OpticalFlow = true] Epsilon stop criterion.");
+	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,  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, DepthDecimation,         int,    1,       "[Grid/FromDepth=true]");
-	RTABMAP_PARAM(Grid, DepthMin,                float,  0.0,     "[Grid/FromDepth=true]");
-	RTABMAP_PARAM(Grid, DepthMax,                float,  0.0,     "[Grid/FromDepth=true]");
-	RTABMAP_PARAM_STR(Grid, DepthRoiRatios,      "0.0 0.0 0.0 0.0", "Region of interest ratios [left, right, top, bottom].");
-	RTABMAP_PARAM(Grid, CellSize,                float,  0.05,    "");
-	RTABMAP_PARAM(Grid, MapFrameProjection,      bool,   false,   "");
-	RTABMAP_PARAM(Grid, MaxObstacleHeight,       float,  0.0,     "");
-	RTABMAP_PARAM(Grid, MaxGroundHeight,         float,  0.0,     "");
-	RTABMAP_PARAM(Grid, MaxGroundAngle,          float,  0.78,    "");
-	RTABMAP_PARAM(Grid, MinClusterSize,          int,    10,      "");
-	RTABMAP_PARAM(Grid, FlatObstacleDetected,    bool,   false,   "");
-	RTABMAP_PARAM(Grid, 3D,                      bool,   false,   "Ignored if laser scan is 2D.");
-	RTABMAP_PARAM(Grid, 3DGroundIsObstacle,      bool,   false,   "[Grid/3D=true] The ground is considered as an obstacle.");
-	RTABMAP_PARAM(Grid, NoiseFilteringRadius,         float,   0.0, "0 means disabled.");
-	RTABMAP_PARAM(Grid, NoiseFilteringMinNeighbors,   int,     5,   "");
+	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,    4,       uFormat("[%s=true] Decimation of the depth image before creating cloud.", kGridDepthDecimation().c_str()));
+	RTABMAP_PARAM(Grid, DepthMin,                float,  0.0,     uFormat("[%s=true] Minimum cloud's depth from sensor.", kGridDepthDecimation().c_str()));
+	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", uFormat("[%s=true] Region of interest ratios [left, right, top, bottom].", kGridDepthDecimation().c_str()));
+	RTABMAP_PARAM(Grid, ScanDecimation,          int,    1,       uFormat("[%s=false] Decimation of the laser scan before creating cloud.", kGridDepthDecimation().c_str()));
+	RTABMAP_PARAM(Grid, CellSize,                float,  0.05,    "Resolution of the occupancy grid.");
+	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, NormalsSegmentation,     bool,   true,    "Segment ground from obstacles using point normals, otherwise a fast passthrough is used.");
+	RTABMAP_PARAM(Grid, MaxObstacleHeight,       float,  0.0,     "Maximum obstacles height (0=disabled).");
+	RTABMAP_PARAM(Grid, MinGroundHeight,         float,  0.0,     "Minimum ground height (0=disabled).");
+	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, 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, NormalK,                 int,    10,      uFormat("[%s=true] K neighbors to compute normals.", kGridNormalsSegmentation().c_str()))
+	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, 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 void parse(const ParametersMap & parameters, const std::string & key, int & value);
-	static void parse(const ParametersMap & parameters, const std::string & key, unsigned int & value);
-	static void parse(const ParametersMap & parameters, const std::string & key, float & value);
-	static void 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, bool & value);
+	static bool parse(const ParametersMap & parameters, const std::string & key, int & value);
+	static bool parse(const ParametersMap & parameters, const std::string & key, unsigned int & value);
+	static bool parse(const ParametersMap & parameters, const std::string & key, float & value);
+	static bool parse(const ParametersMap & parameters, const std::string & key, double & 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();
--- a/corelib/include/rtabmap/core/SensorData.h
+++ b/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,
-			float laserScanMaxRange,
+			const LaserScanInfo & laserScanInfo,
 			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,
-			float laserScanMaxRange,
+			const LaserScanInfo & laserScanInfo,
 			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,
-			float laserScanMaxRange,
+			const LaserScanInfo & laserScanInfo,
 			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;}
-	float laserScanMaxRange() const {return _laserScanMaxRange;}
+	const LaserScanInfo & laserScanInfo() const {return _laserScanInfo;}

 	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 uncompressDataConst(cv::Mat * imageRaw, cv::Mat * depthOrRightRaw, cv::Mat * laserScanRaw = 0, cv::Mat * userDataRaw = 0) const;
+	void uncompressData(
+			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;
--- a/corelib/include/rtabmap/core/Signature.h
+++ b/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;
 };

--- a/corelib/include/rtabmap/core/StereoCameraModel.h
+++ b/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();}
--- a/corelib/include/rtabmap/core/impl/util3d_mapping.hpp
+++ b/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;
--- a/corelib/include/rtabmap/core/util2d.h
+++ b/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);

--- a/corelib/include/rtabmap/core/util3d.h
+++ b/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,