Added new options to filter source laser scans.

SensorData can support laser scans CV_32FC6 format (point cloud with normals). Refactored RegistrationICP and updated CloudViewer to show PointNormal data.
Fixed bug with stereo clouds deterioration if decimation is set (CameraModel::scale()).

corelib/include/rtabmap/core/CameraInfo.h

@@ -42,6 +42,7 @@ public:
 		timeCapture(0.0f),
 		timeDisparity(0.0f),
 		timeMirroring(0.0f),
+		timeImageDecimation(0.0f),
 		timeScanFromDepth(0.0f)
 	{
 	}
@@ -52,6 +53,7 @@ public:
 	float timeCapture;
 	float timeDisparity;
 	float timeMirroring;
+	float timeImageDecimation;
 	float timeScanFromDepth;
 };
 

corelib/include/rtabmap/core/CameraModel.h

@@ -72,6 +72,8 @@ public:
 
 	virtual ~CameraModel() {}
 
+	void initRectificationMap();
+
 	bool isValid() const {return !K_.empty() &&
 									!D_.empty() &&
 									!R_.empty() &&
@@ -104,7 +106,7 @@ public:
 	bool load(const std::string & directory, const std::string & cameraName);
 	bool save(const std::string & directory) const;
 
-	void scale(double scale);
+	CameraModel scaled(double scale) const;
 
 	double horizontalFOV() const; // in degrees
 	double verticalFOV() const;   // in degrees

corelib/include/rtabmap/core/CameraRGB.h

@@ -79,11 +79,21 @@ public:
 	void setScanPath(
 			const std::string & dir,
 			int maxScanPts = 0,
+			int downsampleStep = 1,
+			float voxelSize = 0.0f,
+			int normalsK = 0, // compute normals if > 0
 			const Transform & localTransform=Transform::getIdentity())
 	{
 		_scanPath = dir;
 		_scanLocalTransform = localTransform;
 		_scanMaxPts = maxScanPts;
+		_scanDownsampleStep = downsampleStep;
+		_scanNormalsK = normalsK;
+		_scanVoxelSize = voxelSize;
+		if(_scanDownsampleStep>1)
+		{
+			_scanMaxPts /= _scanDownsampleStep;
+		}
 	}
 
 	void setGroundTruthPath(const std::string & filePath, int format = 0)
@@ -120,6 +130,9 @@ private:
 	std::string _scanPath;
 	Transform _scanLocalTransform;
 	int _scanMaxPts;
+	int _scanDownsampleStep;
+	float _scanVoxelSize;
+	int _scanNormalsK;
 
 	bool _filenamesAreTimestamps;
 	std::string timestampsPath_;

corelib/include/rtabmap/core/CameraThread.h

@@ -54,9 +54,22 @@ public:
 
 	void setMirroringEnabled(bool enabled) {_mirroring = enabled;}
 	void setColorOnly(bool colorOnly) {_colorOnly = colorOnly;}
+	void setImageDecimation(int decimation) {_imageDecimation = decimation;}
 	void setStereoToDepth(bool enabled) {_stereoToDepth = enabled;}
-	void setScanFromDepth(bool enabled, int decimation=4, float maxDepth=4.0f)
-	{_scanFromDepth = enabled; _scanDecimation=decimation; _scanMaxDepth = maxDepth;}
+
+	void setScanFromDepth(
+			bool enabled,
+			int decimation=4,
+			float maxDepth=4.0f,
+			float voxelSize = 0.0f,
+			int normalsK = 0)
+	{
+		_scanFromDepth = enabled;
+		_scanDecimation=decimation;
+		_scanMaxDepth = maxDepth;
+		_scanVoxelSize = voxelSize;
+		_scanNormalsK = normalsK;
+	}
 
 	//getters
 	bool isPaused() const {return !this->isRunning();}
@@ -73,10 +86,13 @@ private:
 	Camera * _camera;
 	bool _mirroring;
 	bool _colorOnly;
+	int _imageDecimation;
 	bool _stereoToDepth;
 	bool _scanFromDepth;
 	int _scanDecimation;
 	float _scanMaxDepth;
+	float _scanVoxelSize;
+	int _scanNormalsK;
 	StereoDense * _stereoDense;
 };
 

corelib/include/rtabmap/core/Odometry.h

@@ -61,7 +61,6 @@ public:
 	const Transform & getPose() const {return _pose;}
 	bool isInfoDataFilled() const {return _fillInfoData;}
 	const Transform & previousTransform() const {return previousTransform_;}
-	bool isVarianceFromInliersCount() const {return _varianceFromInliersCount;}
 
 private:
 	virtual Transform computeTransform(const SensorData & image, OdometryInfo * info = 0) = 0;
@@ -80,7 +79,6 @@ private:
 	float _particleNoiseR;
 	float _particleLambdaR;
 	bool _fillInfoData;
-	bool _varianceFromInliersCount;
 	float _kalmanProcessNoise;
 	float _kalmanMeasurementNoise;
 	Transform _pose;

corelib/include/rtabmap/core/OdometryInfo.h

@@ -37,21 +37,23 @@ class OdometryInfo
 public:
 	OdometryInfo() :
 		lost(true),
-		matches(-1),
-		inliers(-1),
-		variance(-1),
-		features(-1),
-		localMapSize(-1),
-		timeEstimation(-1),
-		timeParticleFiltering(-1),
+		matches(0),
+		inliers(0),
+		icpInliersRatio(0.0f),
+		variance(0.0f),
+		features(0),
+		localMapSize(0),
+		timeEstimation(0.0f),
+		timeParticleFiltering(0.0f),
 		stamp(0),
 		interval(0),
-		distanceTravelled(0),
-		type(-1)
+		distanceTravelled(0.0f),
+		type(0)
 	{}
 	bool lost;
 	int matches;
 	int inliers;
+	int icpInliersRatio;
 	float variance;
 	int features;
 	int localMapSize;

corelib/include/rtabmap/core/RegistrationInfo.h

@@ -17,18 +17,22 @@ public:
 	RegistrationInfo() :
 		variance(0),
 		inliers(0),
-		inliersRatio(0),
-		matches(0)
+		matches(0),
+		icpInliersRatio(0)
 	{
 	}
 
 	float variance;
+	std::string rejectedMsg;
+
+	// RegistrationVis
 	int inliers;
-	float inliersRatio;
 	std::vector<int> inliersIDs;
 	int matches;
 	std::vector<int> matchesIDs;
-	std::string rejectedMsg;
+
+	// RegistrationIcp
+	float icpInliersRatio;
 };
 
 }

corelib/include/rtabmap/core/SensorData.h

@@ -72,7 +72,7 @@ public:
 			double stamp = 0.0,
 			const cv::Mat & userData = cv::Mat());
 
-	// RGB-D constructor + 2d laser scan
+	// RGB-D constructor + laser scan
 	SensorData(
 			const cv::Mat & laserScan,
 			int laserScanMaxPts,
@@ -93,7 +93,7 @@ public:
 			double stamp = 0.0,
 			const cv::Mat & userData = cv::Mat());
 
-	// Multi-cameras RGB-D constructor + 2d laser scan
+	// Multi-cameras RGB-D constructor + laser scan
 	SensorData(
 			const cv::Mat & laserScan,
 			int laserScanMaxPts,
@@ -114,7 +114,7 @@ public:
 			double stamp = 0.0,
 			const cv::Mat & userData = cv::Mat());
 
-	// Stereo constructor + 2d laser scan
+	// Stereo constructor + laser scan
 	SensorData(
 			const cv::Mat & laserScan,
 			int laserScanMaxPts,
@@ -206,7 +206,7 @@ private:
 
 	cv::Mat _imageRaw;          // CV_8UC1 or CV_8UC3
 	cv::Mat _depthOrRightRaw;   // depth CV_16UC1 or CV_32FC1, right image CV_8UC1
-	cv::Mat _laserScanRaw;      // CV_32FC2
+	cv::Mat _laserScanRaw;      // CV_32FC2 or CV_32FC3
 
 	std::vector<CameraModel> _cameraModels;
 	StereoCameraModel _stereoCameraModel;

corelib/include/rtabmap/core/util3d.h

@@ -126,9 +126,14 @@ pcl::PointCloud<pcl::PointXYZ> RTABMAP_EXP laserScanFromDepthImage(
 
 // return CV_32FC3
 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_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.
+pcl::PointCloud<pcl::PointNormal>::Ptr RTABMAP_EXP laserScanToPointCloudNormal(const cv::Mat & laserScan, const Transform & transform = Transform());
 
 cv::Point3f RTABMAP_EXP projectDisparityTo3D(
 		const cv::Point2f & pt,
@@ -185,6 +190,15 @@ void RTABMAP_EXP savePCDWords(
 
 pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP loadBINCloud(const std::string & fileName, int dim);
 
+// Load *.pcd, *.ply or *.bin (KITTI format) with optional filtering.
+// If normals are computed (normalsK>0), the returned scan type is CV_32FC6 instead of CV_32FC3
+cv::Mat RTABMAP_EXP loadScan(
+		const std::string & path,
+		const Transform & transform = Transform::getIdentity(),
+		int downsampleStep = 1,
+		float voxelSize = 0.0f,
+		int normalsK = 0);
+
 } // namespace util3d
 } // namespace rtabmap
 

corelib/include/rtabmap/core/util3d_filtering.h

@@ -54,6 +54,9 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_EXP downsample(
 pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP voxelize(
 		const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
 		float voxelSize);
+pcl::PointCloud<pcl::PointNormal>::Ptr RTABMAP_EXP voxelize(
+		const pcl::PointCloud<pcl::PointNormal>::Ptr & cloud,
+		float voxelSize);
 pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_EXP voxelize(
 		const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
 		float voxelSize);

corelib/include/rtabmap/core/util3d_registration.h

@@ -74,7 +74,8 @@ Transform RTABMAP_EXP icp(
 		double maxCorrespondenceDistance,
 		int maximumIterations,
 		bool & hasConverged,
-		pcl::PointCloud<pcl::PointXYZ> & cloud_source_registered);
+		pcl::PointCloud<pcl::PointXYZ> & cloud_source_registered,
+		bool icp2D = false);
 
 Transform RTABMAP_EXP icpPointToPlane(
 		const pcl::PointCloud<pcl::PointNormal>::ConstPtr & cloud_source,
@@ -84,14 +85,6 @@ Transform RTABMAP_EXP icpPointToPlane(
 		bool & hasConverged,
 		pcl::PointCloud<pcl::PointNormal> & cloud_source_registered);
 
-Transform RTABMAP_EXP icp2D(
-		const pcl::PointCloud<pcl::PointXYZ>::ConstPtr & cloud_source,
-		const pcl::PointCloud<pcl::PointXYZ>::ConstPtr & cloud_target,
-		double maxCorrespondenceDistance,
-		int maximumIterations,
-		bool & hasConverged,
-		pcl::PointCloud<pcl::PointXYZ> & cloud_source_registered);
-
 pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP getICPReadyCloud(
 		const cv::Mat & depth,
 		float fx,

corelib/src/CameraModel.cpp

@@ -62,10 +62,6 @@ CameraModel::CameraModel(
 	UASSERT(D_.rows == 1 && (D_.cols == 4 || D_.cols == 5 || D_.cols == 8));
 	UASSERT(R_.rows == 3 && R_.cols == 3);
 	UASSERT(P_.rows == 3 && P_.cols == 4);
-
-	// init rectification map
-	UINFO("Initialize rectify map");
-	cv::initUndistortRectifyMap(K_, D_, R_, P_, imageSize_, CV_32FC1, mapX_, mapY_);
 }
 
 CameraModel::CameraModel(
@@ -128,6 +124,14 @@ CameraModel::CameraModel(
 	K_.at<double>(1,2) = cy;
 }
 
+void CameraModel::initRectificationMap()
+{
+	UASSERT(imageSize_.height > 0 && imageSize_.width > 0);
+	// init rectification map
+	UINFO("Initialize rectify map");
+	cv::initUndistortRectifyMap(K_, D_, R_, P_, imageSize_, CV_32FC1, mapX_, mapY_);
+}
+
 bool CameraModel::load(const std::string & directory, const std::string & cameraName)
 {
 	K_ = cv::Mat();
@@ -191,9 +195,7 @@ bool CameraModel::load(const std::string & directory, const std::string & camera
 
 		if(imageSize_.height > 0 && imageSize_.width > 0)
 		{
-			// init rectification map
-			UINFO("Initialize rectify map");
-			cv::initUndistortRectifyMap(K_, D_, R_, P_, imageSize_, CV_32FC1, mapX_, mapY_);
+			initRectificationMap();
 		}
 
 		return true;
@@ -261,20 +263,31 @@ bool CameraModel::save(const std::string & directory) const
 	return false;
 }
 
-void CameraModel::scale(double scale)
+CameraModel CameraModel::scaled(double scale) const
 {
+	CameraModel scaledModel = *this;
 	UASSERT(scale > 0.0);
-	// has only effect on K and P
-	imageSize_.width *= scale;
-	imageSize_.height *= scale;
-	K_.at<double>(0,0) *= scale;
-	K_.at<double>(1,1) *= scale;
-	K_.at<double>(0,2) *= scale;
-	K_.at<double>(1,2) *= scale;
-	P_.at<double>(0,0) *= scale;
-	P_.at<double>(1,1) *= scale;
-	P_.at<double>(0,2) *= scale;
-	P_.at<double>(1,2) *= scale;
+	if(this->isValid())
+	{
+		// has only effect on K and P
+		cv::Mat K = K_.clone();
+		K.at<double>(0,0) *= scale;
+		K.at<double>(1,1) *= scale;
+		K.at<double>(0,2) *= scale;
+		K.at<double>(1,2) *= scale;
+
+		cv::Mat P = P_.clone();
+		P.at<double>(0,0) *= scale;
+		P.at<double>(1,1) *= scale;
+		P.at<double>(0,2) *= scale;
+		P.at<double>(1,2) *= scale;
+		scaledModel = CameraModel(name_, cv::Size(double(imageSize_.width)*scale, double(imageSize_.height)*scale), K, D_, R_, P, localTransform_);
+	}
+	else
+	{
+		UWARN("Trying to scale a camera model not valid! Ignoring scaling...");
+	}
+	return scaledModel;
 }
 
 double CameraModel::horizontalFOV() const

corelib/src/CameraRGB.cpp

@@ -39,7 +39,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <opencv2/imgproc/imgproc.hpp>
 
 #include <rtabmap/core/util3d.h>
-#include <pcl/io/pcd_io.h>
+#include <rtabmap/core/util3d_filtering.h>
 
 #include <iostream>
 #include <cmath>
@@ -61,6 +61,9 @@ CameraImages::CameraImages() :
 		_countScan(0),
 		_scanDir(0),
 		_scanMaxPts(0),
+		_scanDownsampleStep(1),
+		_scanVoxelSize(0.0f),
+		_scanNormalsK(0),
 		_filenamesAreTimestamps(false),
 		_groundTruthFormat(0)
 	{}
@@ -79,6 +82,9 @@ CameraImages::CameraImages(const std::string & path,
 	_countScan(0),
 	_scanDir(0),
 	_scanMaxPts(0),
+	_scanDownsampleStep(1),
+	_scanVoxelSize(0.0f),
+	_scanNormalsK(0),
 	_filenamesAreTimestamps(false),
 	_groundTruthFormat(0)
 {
@@ -140,7 +146,7 @@ bool CameraImages::init(const std::string & calibrationFolder, const std::string
 	if(!_scanPath.empty())
 	{
 		UINFO("scan path=%s", _scanPath.c_str());
-		_scanDir = new UDirectory(_scanPath, "pcd bin"); // "bin" is for KITTI format
+		_scanDir = new UDirectory(_scanPath, "pcd bin ply"); // "bin" is for KITTI format
 		if(_scanPath[_scanPath.size()-1] != '\\' && _scanPath[_scanPath.size()-1] != '/')
 		{
 			_scanPath.append("/");
@@ -407,16 +413,8 @@ SensorData CameraImages::captureImage()
 					{
 						_lastScanFileName = *scanFileNames.rbegin();
 						std::string fullPath = _scanPath + _lastScanFileName;
-						pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
-						if(UFile::getExtension(_lastScanFileName).compare("bin") == 0)
-						{
-							cloud = util3d::loadBINCloud(fullPath, 4); // Assume KITTI velodyne format
-						}
-						else
-						{
-							pcl::io::loadPCDFile(fullPath, *cloud);
-						}
-						scan = util3d::laserScanFromPointCloud(*cloud, _scanLocalTransform);
+
+						scan = util3d::loadScan(fullPath, _scanLocalTransform, _scanDownsampleStep, _scanVoxelSize, _scanNormalsK);
 					}
 				}
 			}
@@ -508,17 +506,7 @@ SensorData CameraImages::captureImage()
 					}
 					if(fileName.size())
 					{
-						UDEBUG("Loading scan : %s", fullPath.c_str());
-						pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
-						if(UFile::getExtension(fileName).compare("bin") == 0)
-						{
-							cloud = util3d::loadBINCloud(fullPath, 4); // Assume KITTI velodyne format
-						}
-						else
-						{
-							pcl::io::loadPCDFile(fullPath, *cloud);
-						}
-						scan = util3d::laserScanFromPointCloud(*cloud, _scanLocalTransform);
+						scan = util3d::loadScan(fullPath, _scanLocalTransform, _scanDownsampleStep, _scanVoxelSize, _scanNormalsK);
 					}
 				}
 			}

corelib/src/CameraThread.cpp

@@ -31,6 +31,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "rtabmap/core/CameraRGBD.h"
 #include "rtabmap/core/util2d.h"
 #include "rtabmap/core/util3d.h"
+#include "rtabmap/core/util3d_surface.h"
 #include "rtabmap/core/StereoDense.h"
 
 #include <rtabmap/utilite/UTimer.h>
@@ -44,10 +45,13 @@ CameraThread::CameraThread(Camera * camera, const ParametersMap & parameters) :
 		_camera(camera),
 		_mirroring(false),
 		_colorOnly(false),
+		_imageDecimation(1),
 		_stereoToDepth(false),
 		_scanFromDepth(false),
 		_scanDecimation(4),
 		_scanMaxDepth(4.0f),
+		_scanVoxelSize(0.0f),
+		_scanNormalsK(0),
 		_stereoDense(new StereoBM(parameters))
 {
 	UASSERT(_camera != 0);
@@ -84,13 +88,47 @@ void CameraThread::mainLoop()
 		{
 			data.setDepthOrRightRaw(cv::Mat());
 		}
+		if(_imageDecimation>1 && !data.imageRaw().empty())
+		{
+			UDEBUG("");
+			UTimer timer;
+			if(!data.depthRaw().empty() &&
+			   !(data.depthRaw().rows % _imageDecimation == 0 && data.depthRaw().cols % _imageDecimation == 0))
+			{
+				UERROR("Decimation of depth images should be exact (decimation=%d, size=(%d,%d))! "
+					   "Images won't be resized.", _imageDecimation, data.depthRaw().cols, data.depthRaw().rows);
+			}
+			else
+			{
+				data.setImageRaw(util2d::decimate(data.imageRaw(), _imageDecimation));
+				data.setDepthOrRightRaw(util2d::decimate(data.depthOrRightRaw(), _imageDecimation));
+				std::vector<CameraModel> models = data.cameraModels();
+				for(unsigned int i=0; i<models.size(); ++i)
+				{
+					if(models[i].isValid())
+					{
+						models[i] = models[i].scaled(1.0/double(_imageDecimation));
+					}
+				}
+				data.setCameraModels(models);
+				StereoCameraModel stereoModel = data.stereoCameraModel();
+				if(stereoModel.isValid())
+				{
+					stereoModel.scale(1.0/double(_imageDecimation));
+					data.setStereoCameraModel(stereoModel);
+				}
+			}
+			info.timeImageDecimation = timer.ticks();
+		}
 		if(_mirroring && data.cameraModels().size() == 1)
 		{
+			UDEBUG("");
 			UTimer timer;
 			cv::Mat tmpRgb;
 			cv::flip(data.imageRaw(), tmpRgb, 1);
 			data.setImageRaw(tmpRgb);
-			if(data.cameraModels()[0].cx())
+			UASSERT_MSG(data.cameraModels().size() <= 1 && !data.stereoCameraModel().isValid(), "Only single RGBD cameras are supported for mirroring.");
+			if(data.cameraModels().size() && data.cameraModels()[0].cx())
 			{
 				CameraModel tmpModel(
 						data.cameraModels()[0].fx(),
@@ -110,6 +148,7 @@ void CameraThread::mainLoop()
 		}
 		if(_stereoToDepth && data.stereoCameraModel().isValid() && !data.rightRaw().empty())
 		{
+			UDEBUG("");
 			UTimer timer;
 			cv::Mat depth = util2d::depthFromDisparity(
 					_stereoDense->computeDisparity(data.imageRaw(), data.rightRaw()),
@@ -126,11 +165,21 @@ void CameraThread::mainLoop()
 			data.cameraModels().at(0).isValid() &&
 			!data.depthRaw().empty())
 		{
+			UDEBUG("");
 			if(data.laserScanRaw().empty())
 			{
 				UASSERT(_scanDecimation >= 1);
 				UTimer timer;
-				cv::Mat scan = util3d::laserScanFromPointCloud(*util3d::cloudFromSensorData(data, _scanDecimation, _scanMaxDepth));
+				pcl::PointCloud<pcl::PointXYZ>::Ptr cloud = util3d::cloudFromSensorData(data, _scanDecimation, _scanMaxDepth, _scanVoxelSize);
+				cv::Mat scan;
+				if(_scanNormalsK>0)
+				{
+					scan = util3d::laserScanFromPointCloud(*util3d::computeNormals(cloud, _scanNormalsK));
+				}
+				else
+				{
+					scan = util3d::laserScanFromPointCloud(*cloud);
+				}
 				data.setLaserScanRaw(scan, (data.depthRaw().rows/_scanDecimation)*(data.depthRaw().cols/_scanDecimation), _scanMaxDepth);
 				info.timeScanFromDepth = timer.ticks();
 				UINFO("Computing scan from depth = %f s", info.timeScanFromDepth);
@@ -142,6 +191,7 @@ void CameraThread::mainLoop()
 					  "depth will not be created.");
 			}
 		}
+
 		info.cameraName = _camera->getSerial();
 		this->post(new CameraEvent(data, info));
 	}

corelib/src/Memory.cpp

@@ -3013,7 +3013,7 @@ Signature * Memory::createSignature(const SensorData & data, const Transform & p
 						data.depthOrRightRaw().rows,
 						data.depthOrRightRaw().type(),
 						CV_16UC1, CV_32FC1, CV_8UC1).c_str());
-	UASSERT(data.laserScanRaw().empty() || data.laserScanRaw().type() == CV_32FC2 || data.laserScanRaw().type() == CV_32FC3);
+	UASSERT(data.laserScanRaw().empty() || data.laserScanRaw().type() == CV_32FC2 || data.laserScanRaw().type() == CV_32FC3 || data.laserScanRaw().type() == CV_32FC(6));
 
 	if(!data.depthOrRightRaw().empty() &&
 		data.cameraModels().size() == 0 &&
@@ -3247,7 +3247,7 @@ Signature * Memory::createSignature(const SensorData & data, const Transform & p
 		depthOrRightImage = util2d::decimate(depthOrRightImage, _imageDecimation);
 		for(unsigned int i=0; i<cameraModels.size(); ++i)
 		{
-			cameraModels[i].scale(1.0/double(_imageDecimation));
+			cameraModels[i] = cameraModels[i].scaled(1.0/double(_imageDecimation));
 		}
 		if(stereoCameraModel.isValid())
 		{

corelib/src/Odometry.cpp

@@ -72,7 +72,6 @@ Odometry::Odometry(const rtabmap::ParametersMap & parameters) :
 		_particleNoiseR(Parameters::defaultOdomParticleNoiseR()),
 		_particleLambdaR(Parameters::defaultOdomParticleLambdaR()),
 		_fillInfoData(Parameters::defaultOdomFillInfoData()),
-		_varianceFromInliersCount(Parameters::defaultRegVarianceFromInliersCount()),
 		_kalmanProcessNoise(Parameters::defaultOdomKalmanProcessNoise()),
 		_kalmanMeasurementNoise(Parameters::defaultOdomKalmanMeasurementNoise()),
 		_resetCurrentCount(0),
@@ -85,7 +84,6 @@ Odometry::Odometry(const rtabmap::ParametersMap & parameters) :
 	Parameters::parse(parameters, Parameters::kRegForce3DoF(), _force3DoF);
 	Parameters::parse(parameters, Parameters::kOdomHolonomic(), _holonomic);
 	Parameters::parse(parameters, Parameters::kOdomFillInfoData(), _fillInfoData);
-	Parameters::parse(parameters, Parameters::kRegVarianceFromInliersCount(), _varianceFromInliersCount);
 	Parameters::parse(parameters, Parameters::kOdomFilteringStrategy(), _filteringStrategy);
 	Parameters::parse(parameters, Parameters::kOdomParticleSize(), _particleSize);
 	Parameters::parse(parameters, Parameters::kOdomParticleNoiseT(), _particleNoiseT);
@@ -330,11 +328,6 @@ Transform Odometry::process(const SensorData & data, OdometryInfo * info)
 		{
 			distanceTravelled_ += t.getNorm();
 			info->distanceTravelled = distanceTravelled_;
-
-			if(_varianceFromInliersCount)
-			{
-				info->variance = info->inliers > 0?1.0/double(info->inliers):1.0;
-			}
 		}
 
 		return _pose *= t; // updated

corelib/src/OdometryF2F.cpp

@@ -179,6 +179,7 @@ Transform OdometryF2F::computeTransform(
 		info->type = 1;
 		info->variance = regInfo.variance;
 		info->inliers = regInfo.inliers;
+		info->icpInliersRatio = regInfo.icpInliersRatio;
 		info->matches = regInfo.matches;
 	}
 

corelib/src/Registration.cpp

@@ -181,6 +181,10 @@ Transform Registration::computeTransformationMod(
 		RegistrationInfo * infoOut) const
 {
 	RegistrationInfo info;
+	if(infoOut)
+	{
+		info = *infoOut;
+	}
 	Transform t = computeTransformationImpl(from, to, guess, info);
 	if(child_)
 	{
@@ -196,9 +200,9 @@ Transform Registration::computeTransformationMod(
 
 	if(varianceFromInliersCount_)
 	{
-		if(info.inliersRatio)
+		if(info.icpInliersRatio)
 		{
-			info.variance = info.inliersRatio > 0?1.0/double(info.inliersRatio):1.0;
+			info.variance = info.icpInliersRatio > 0?1.0/double(info.icpInliersRatio):1.0;
 		}
 		else
 		{

corelib/src/RegistrationIcp.cpp

@@ -35,6 +35,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <rtabmap/utilite/ULogger.h>
 #include <rtabmap/utilite/UConversion.h>
 #include <rtabmap/utilite/UMath.h>
+#include <rtabmap/utilite/UTimer.h>
 #include <pcl/io/pcd_io.h>
 
 namespace rtabmap {
@@ -93,12 +94,15 @@ Transform RegistrationIcp::computeTransformationImpl(
 	UDEBUG("Max rotation=%f", _maxRotation);
 	UDEBUG("Downsampling step=%d", _downsamplingStep);
 
+	UTimer timer;
 	std::string msg;
 	Transform transform;
 
 	SensorData & dataFrom = fromSignature.sensorData();
 	SensorData & dataTo = toSignature.sensorData();
 
+	UDEBUG("size from=%d to=%d", dataFrom.laserScanRaw().cols, dataTo.laserScanRaw().cols);
+
 	// ICP with guess transform
 	if(!dataFrom.laserScanRaw().empty() && !dataTo.laserScanRaw().empty())
 	{
@@ -110,34 +114,65 @@ Transform RegistrationIcp::computeTransformationImpl(
 			fromScan = util3d::downsample(fromScan, _downsamplingStep);
 			toScan = util3d::downsample(toScan, _downsamplingStep);
 			maxLaserScans/=_downsamplingStep;
+			UDEBUG("Downsampling time (step=%d) = %f s", _downsamplingStep, timer.ticks());
 		}
 
-		pcl::PointCloud<pcl::PointXYZ>::Ptr fromCloud = util3d::laserScanToPointCloud(fromScan, Transform());
-		pcl::PointCloud<pcl::PointXYZ>::Ptr toCloud = util3d::laserScanToPointCloud(toScan, guess);
 
-		if(toCloud->size() && fromCloud->size())
+		UDEBUG("Conversion time = %f s", timer.ticks());
+
+		if(fromScan.cols && toScan.cols)
 		{
-			//filtering
-			pcl::PointCloud<pcl::PointXYZ>::Ptr fromCloudFiltered = fromCloud;
-			pcl::PointCloud<pcl::PointXYZ>::Ptr toCloudFiltered = toCloud;
-			bool filtered = false;
-			if(_voxelSize > 0.0f)
-			{
-				fromCloudFiltered = util3d::voxelize(fromCloudFiltered, _voxelSize);
-				toCloudFiltered = util3d::voxelize(toCloudFiltered, _voxelSize);
-				filtered = true;
-			}
-
 			Transform icpT;
 			bool hasConverged = false;
 			float correspondencesRatio = 0.0f;
 			int correspondences = 0;
 			double variance = 1.0;
-			bool correspondencesComputed = false;
-			pcl::PointCloud<pcl::PointXYZ>::Ptr fromCloudRegistered(new pcl::PointCloud<pcl::PointXYZ>());
-			if(!force3DoF()) // 3D ICP
+
+			if( !force3DoF() &&
+				_pointToPlane &&
+				_voxelSize == 0.0f &&
+				fromScan.channels() == 6 &&
+				toScan.channels() == 6)
 			{
-				if(_pointToPlane)
+				//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 toCloudNormals = util3d::laserScanToPointCloudNormal(toScan, guess);
+				pcl::PointCloud<pcl::PointNormal>::Ptr fromCloudNormalsRegistered(new pcl::PointCloud<pcl::PointNormal>());
+				icpT = util3d::icpPointToPlane(
+						fromCloudNormals,
+						toCloudNormals,
+					   _maxCorrespondenceDistance,
+					   _maxIterations,
+					   hasConverged,
+					   *fromCloudNormalsRegistered);
+				if(!icpT.isNull() && hasConverged)
+				{
+					util3d::computeVarianceAndCorrespondences(
+							fromCloudNormalsRegistered,
+							toCloudNormals,
+							_maxCorrespondenceDistance,
+							variance,
+							correspondences);
+				}
+			}
+			else
+			{
+				pcl::PointCloud<pcl::PointXYZ>::Ptr fromCloud = util3d::laserScanToPointCloud(fromScan, Transform());
+				pcl::PointCloud<pcl::PointXYZ>::Ptr toCloud = util3d::laserScanToPointCloud(toScan, guess);
+				pcl::PointCloud<pcl::PointXYZ>::Ptr fromCloudFiltered = fromCloud;
+				pcl::PointCloud<pcl::PointXYZ>::Ptr toCloudFiltered = toCloud;
+				bool filtered = false;
+				if(_voxelSize > 0.0f)
+				{
+					fromCloudFiltered = util3d::voxelize(fromCloudFiltered, _voxelSize);
+					toCloudFiltered = util3d::voxelize(toCloudFiltered, _voxelSize);
+					filtered = true;
+					UDEBUG("Voxel filtering time (voxel=%f m) = %f s", _voxelSize, timer.ticks());
+				}
+
+				bool correspondencesComputed = false;
+				pcl::PointCloud<pcl::PointXYZ>::Ptr fromCloudRegistered(new pcl::PointCloud<pcl::PointXYZ>());
+				if(!force3DoF() && _pointToPlane) // ICP Point To Plane, only in 3D
 				{
 					pcl::PointCloud<pcl::PointNormal>::Ptr fromCloudNormals = util3d::computeNormals(fromCloudFiltered, _pointToPlaneNormalNeighbors);
 					pcl::PointCloud<pcl::PointNormal>::Ptr toCloudNormals = util3d::computeNormals(toCloudFiltered, _pointToPlaneNormalNeighbors);
@@ -146,6 +181,8 @@ Transform RegistrationIcp::computeTransformationImpl(
 					toCloudNormals = util3d::removeNaNNormalsFromPointCloud(toCloudNormals);
 					fromCloudNormals = util3d::removeNaNNormalsFromPointCloud(fromCloudNormals);
 
+					UDEBUG("Compute normals time = %f s", timer.ticks());
+
 					if(toCloudNormals->size() && fromCloudNormals->size())
 					{
 						pcl::PointCloud<pcl::PointNormal>::Ptr fromCloudNormalsRegistered(new pcl::PointCloud<pcl::PointNormal>());
@@ -161,8 +198,8 @@ Transform RegistrationIcp::computeTransformationImpl(
 							hasConverged)
 						{
 							util3d::computeVarianceAndCorrespondences(
-									fromCloudNormals,
 									fromCloudNormalsRegistered,
+									toCloudNormals,
 									_maxCorrespondenceDistance,
 									variance,
 									correspondences);
@@ -170,37 +207,50 @@ Transform RegistrationIcp::computeTransformationImpl(
 						}
 					}
 				}
-				else
+				else // ICP Point to Point
 				{
 					icpT = util3d::icp(
 							fromCloudFiltered,
 							toCloudFiltered,
+						   _maxCorrespondenceDistance,
+						   _maxIterations,
+						   hasConverged,
+						   *fromCloudRegistered,
+						   !this->force3DoF()); // icp2D
+				}
+
+				/*pcl::io::savePCDFile("fromCloud.pcd", *fromCloud);
+				pcl::io::savePCDFile("toCloud.pcd", *toCloud);
+				UWARN("saved fromCloud.pcd and toCloud.pcd");
+				if(!icpT.isNull())
+				{
+					pcl::PointCloud<pcl::PointXYZ>::Ptr fromCloudTmp = util3d::transformPointCloud(fromCloud, icpT);
+					pcl::io::savePCDFile("fromCloudFinal.pcd", *fromCloudTmp);
+					UWARN("saved fromCloudFinal.pcd");
+				}*/
+
+				if(!icpT.isNull() &&
+					hasConverged &&
+					!correspondencesComputed)
+				{
+					if(filtered)
+					{
+						fromCloud = util3d::transformPointCloud(fromCloud, icpT);
+					}
+					else
+					{
+						fromCloud = fromCloudRegistered;
+					}
+
+					util3d::computeVarianceAndCorrespondences(
+							fromCloud,
+							toCloud,
 							_maxCorrespondenceDistance,
-							_maxIterations,
-							hasConverged,
-							*fromCloudRegistered);
+							variance,
+							correspondences);
 				}
 			}
-			else // 2D ICP
-			{
-				icpT = util3d::icp2D(
-						fromCloudFiltered,
-						toCloudFiltered,
-					   _maxCorrespondenceDistance,
-					   _maxIterations,
-					   hasConverged,
-					   *fromCloudRegistered);
-			}
-
-			/*pcl::io::savePCDFile("fromCloud.pcd", *fromCloud);
-			pcl::io::savePCDFile("toCloud.pcd", *toCloud);
-			UWARN("saved fromCloud.pcd and toCloud.pcd");
-			if(!icpT.isNull())
-			{
-				pcl::PointCloud<pcl::PointXYZ>::Ptr fromCloudTmp = util3d::transformPointCloud(fromCloud, icpT);
-				pcl::io::savePCDFile("fromCloudFinal.pcd", *fromCloudTmp);
-				UWARN("saved fromCloudFinal.pcd");
-			}*/
+			UDEBUG("ICP (iterations=%d) time = %f s", _maxIterations, timer.ticks());
 
 			if(!icpT.isNull() &&
 				hasConverged)
@@ -226,25 +276,6 @@ Transform RegistrationIcp::computeTransformationImpl(
 				}
 				else
 				{
-					if(!correspondencesComputed)
-					{
-						if(filtered)
-						{
-							fromCloud = util3d::transformPointCloud(fromCloud, icpT);
-						}
-						else
-						{
-							fromCloud = fromCloudRegistered;
-						}
-
-						util3d::computeVarianceAndCorrespondences(
-								fromCloud,
-								toCloud,
-								_maxCorrespondenceDistance,
-								variance,
-								correspondences);
-					}
-
 					// verify if there are enough correspondences
 					if(maxLaserScans)
 					{
@@ -254,7 +285,7 @@ Transform RegistrationIcp::computeTransformationImpl(
 					{
 						UWARN("Maximum laser scans points not set for signature %d, correspondences ratio set relative instead of absolute!",
 								dataTo.id());
-						correspondencesRatio = float(correspondences)/float(toCloud->size()>fromCloud->size()?toCloud->size():fromCloud->size());
+						correspondencesRatio = float(correspondences)/float(toScan.cols>fromScan.cols?toScan.cols:fromScan.cols);
 					}
 
 					UDEBUG("%d->%d hasConverged=%s, variance=%f, correspondences=%d/%d (%f%%)",
@@ -262,12 +293,11 @@ Transform RegistrationIcp::computeTransformationImpl(
 							hasConverged?"true":"false",
 							variance,
 							correspondences,
-							maxLaserScans>0?maxLaserScans:dataTo.laserScanMaxPts()?dataTo.laserScanMaxPts():(int)(toCloud->size()>fromCloud->size()?toCloud->size():fromCloud->size()),
+							maxLaserScans>0?maxLaserScans:dataTo.laserScanMaxPts()?dataTo.laserScanMaxPts():(int)(toScan.cols>fromScan.cols?toScan.cols:fromScan.cols),
 							correspondencesRatio*100.0f);
 
 					info.variance = variance>0.0f?variance:0.0001; // epsilon if exact transform
-					info.inliers = correspondences;
-					info.inliersRatio = correspondencesRatio;
+					info.icpInliersRatio = correspondencesRatio;
 
 					if(correspondencesRatio < _correspondenceRatio)
 					{
@@ -287,21 +317,6 @@ Transform RegistrationIcp::computeTransformationImpl(
 						hasConverged?"true":"false", variance);
 				UINFO(msg.c_str());
 			}
-
-			// still compute the variance for information
-			/*if(variance == 1 && varianceOut)
-			{
-				util3d::computeVarianceAndCorrespondences(
-					toCloudFiltered,
-					fromCloudFiltered,
-					_icpMaxCorrespondenceDistance,
-					variance,
-					correspondences);
-				if(variance > 0)
-				{
-					*varianceOut = variance;
-				}
-			}*/
 		}
 		else
 		{

corelib/src/Rtabmap.cpp

@@ -1048,7 +1048,7 @@ bool Rtabmap::process(
 					}
 					statistics_.addStatistic(Statistics::kNeighborLinkRefiningAccepted(), !t.isNull()?1.0f:0);
 					statistics_.addStatistic(Statistics::kNeighborLinkRefiningInliers(), info.inliers);
-					statistics_.addStatistic(Statistics::kNeighborLinkRefiningInliers_ratio(), info.inliersRatio);
+					statistics_.addStatistic(Statistics::kNeighborLinkRefiningInliers_ratio(), info.icpInliersRatio);
 					statistics_.addStatistic(Statistics::kNeighborLinkRefiningVariance(), info.variance);
 					statistics_.addStatistic(Statistics::kNeighborLinkRefiningPts(), signature->sensorData().laserScanRaw().cols);
 				}

corelib/src/SensorData.cpp

@@ -159,7 +159,7 @@ SensorData::SensorData(
 	}
 }
 
-// RGB-D constructor + 2d laser scan
+// RGB-D constructor + laser scan
 SensorData::SensorData(
 		const cv::Mat & laserScan,
 		int laserScanMaxPts,
@@ -199,7 +199,7 @@ SensorData::SensorData(
 		_depthOrRightRaw = depth;
 	}
 
-	if(laserScan.type() == CV_32FC2 || laserScan.type() == CV_32FC3)
+	if(laserScan.type() == CV_32FC2 || laserScan.type() == CV_32FC3 || laserScan.type() == CV_32FC(6))
 	{
 		_laserScanRaw = laserScan;
 	}
@@ -266,7 +266,7 @@ SensorData::SensorData(
 	}
 }
 
-// Multi-cameras RGB-D constructor + 2d laser scan
+// Multi-cameras RGB-D constructor + laser scan
 SensorData::SensorData(
 		const cv::Mat & laserScan,
 		int laserScanMaxPts,
@@ -306,7 +306,7 @@ SensorData::SensorData(
 		_depthOrRightRaw = depth;
 	}
 
-	if(laserScan.type() == CV_32FC2 || laserScan.type() == CV_32FC3)
+	if(laserScan.type() == CV_32FC2 || laserScan.type() == CV_32FC3 || laserScan.type() == CV_32FC(6))
 	{
 		_laserScanRaw = laserScan;
 	}
@@ -412,7 +412,7 @@ SensorData::SensorData(
 		_depthOrRightRaw = right;
 	}
 
-	if(laserScan.type() == CV_32FC2 || laserScan.type() == CV_32FC3)
+	if(laserScan.type() == CV_32FC2 || laserScan.type() == CV_32FC3 || laserScan.type() == CV_32FC(6))
 	{
 		_laserScanRaw = laserScan;
 	}

corelib/src/StereoCameraModel.cpp

@@ -162,8 +162,8 @@ bool StereoCameraModel::save(const std::string & directory, bool ignoreStereoTra
 
 void StereoCameraModel::scale(double scale)
 {
-	left_.scale(scale);
-	right_.scale(scale);
+	left_ = left_.scaled(scale);
+	right_ = right_.scaled(scale);
 }
 
 float StereoCameraModel::computeDepth(float disparity) const

corelib/src/util3d.cpp

@@ -28,12 +28,14 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <rtabmap/core/util3d.h>
 #include <rtabmap/core/util3d_transforms.h>
 #include <rtabmap/core/util3d_filtering.h>
+#include <rtabmap/core/util3d_surface.h>
 #include <rtabmap/core/util2d.h>
 #include <rtabmap/utilite/ULogger.h>
 #include <rtabmap/utilite/UMath.h>
 #include <rtabmap/utilite/UConversion.h>
 #include <rtabmap/utilite/UFile.h>
 #include <pcl/io/pcd_io.h>
+#include <pcl/io/ply_io.h>
 #include <pcl/common/transforms.h>
 #include <opencv2/imgproc/imgproc.hpp>
 
@@ -807,6 +809,36 @@ cv::Mat laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZ> & cloud, co
 	return laserScan;
 }
 
+cv::Mat laserScanFromPointCloud(const pcl::PointCloud<pcl::PointNormal> & cloud, const Transform & transform)
+{
+	cv::Mat laserScan(1, (int)cloud.size(), CV_32FC(6));
+	bool nullTransform = transform.isNull();
+	Eigen::Affine3f transform3f = transform.toEigen3f();
+	for(unsigned int i=0; i<cloud.size(); ++i)
+	{
+		if(!nullTransform)
+		{
+			pcl::PointNormal pt = pcl::transformPoint(cloud.at(i), transform3f);
+			laserScan.at<cv::Vec6f>(i)[0] = pt.x;
+			laserScan.at<cv::Vec6f>(i)[1] = pt.y;
+			laserScan.at<cv::Vec6f>(i)[2] = pt.z;
+			laserScan.at<cv::Vec6f>(i)[3] = pt.normal_x;
+			laserScan.at<cv::Vec6f>(i)[4] = pt.normal_y;
+			laserScan.at<cv::Vec6f>(i)[5] = pt.normal_z;
+		}
+		else
+		{
+			laserScan.at<cv::Vec6f>(i)[0] = cloud.at(i).x;
+			laserScan.at<cv::Vec6f>(i)[1] = cloud.at(i).y;
+			laserScan.at<cv::Vec6f>(i)[2] = cloud.at(i).z;
+			laserScan.at<cv::Vec6f>(i)[3] = cloud.at(i).normal_x;
+			laserScan.at<cv::Vec6f>(i)[4] = cloud.at(i).normal_y;
+			laserScan.at<cv::Vec6f>(i)[5] = cloud.at(i).normal_z;
+		}
+	}
+	return laserScan;
+}
+
 cv::Mat laserScan2dFromPointCloud(const pcl::PointCloud<pcl::PointXYZ> & cloud, const Transform & transform)
 {
 	cv::Mat laserScan(1, (int)cloud.size(), CV_32FC2);
@@ -832,7 +864,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);
+	UASSERT(laserScan.empty() || laserScan.type() == CV_32FC2 || laserScan.type() == CV_32FC3 || laserScan.type() == CV_32FC(6));
 
 	pcl::PointCloud<pcl::PointXYZ>::Ptr output(new pcl::PointCloud<pcl::PointXYZ>);
 	output->resize(laserScan.cols);
@@ -845,12 +877,58 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr laserScanToPointCloud(const cv::Mat & laserS
 			output->at(i).x = laserScan.at<cv::Vec2f>(i)[0];
 			output->at(i).y = laserScan.at<cv::Vec2f>(i)[1];
 		}
-		else
+		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);
+		}
+	}
+	return output;
+}
+
+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));
+
+	pcl::PointCloud<pcl::PointNormal>::Ptr output(new pcl::PointCloud<pcl::PointNormal>);
+	output->resize(laserScan.cols);
+	bool nullTransform = transform.isNull();
+	Eigen::Affine3f transform3f = transform.toEigen3f();
+	for(int i=0; i<laserScan.cols; ++i)
+	{
+		if(laserScan.type() == CV_32FC2)
+		{
+			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) = pcl::transformPoint(output->at(i), transform3f);
@@ -865,10 +943,15 @@ cv::Point3f projectDisparityTo3D(
 		float disparity,
 		const StereoCameraModel & model)
 {
-	if(disparity != 0.0f && model.baseline() > 0.0f && model.left().fx() > 0.0f)
+	if(disparity > 0.0f && model.baseline() > 0.0f && model.left().fx() > 0.0f)
 	{
 		//Z = baseline * f / (d + cx1-cx0);
-		float W = model.baseline()/(disparity + model.right().cx() - model.left().cx());
+		float c = 0.0f;
+		if(model.right().cx()>0.0f && model.left().cx()>0.0f)
+		{
+			c = model.right().cx() - model.left().cx();
+		}
+		float W = model.baseline()/(disparity + c);
 		return cv::Point3f((pt.x - model.left().cx())*W, (pt.y - model.left().cy())*W, model.left().fx()*W);
 	}
 	float bad_point = std::numeric_limits<float>::quiet_NaN ();
@@ -1023,6 +1106,53 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr loadBINCloud(const std::string & fileName, i
 	return cloud;
 }
 
+cv::Mat loadScan(
+		const std::string & path,
+		const Transform & transform,
+		int downsampleStep,
+		float voxelSize,
+		int normalsK)
+{
+	cv::Mat scan;
+	UDEBUG("Loading scan (step=%d, voxel=%f m, normalsK=%d) : %s", downsampleStep, voxelSize, normalsK, path.c_str());
+	std::string fileName = UFile::getName(path);
+	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
+	if(UFile::getExtension(fileName).compare("bin") == 0)
+	{
+		cloud = util3d::loadBINCloud(path, 4); // Assume KITTI velodyne format
+	}
+	else if(UFile::getExtension(fileName).compare("pcd") == 0)
+	{
+		pcl::io::loadPCDFile(path, *cloud);
+	}
+	else
+	{
+		pcl::io::loadPLYFile(path, *cloud);
+	}
+	int previousSize = (int)cloud->size();
+	if(downsampleStep > 1 && cloud->size())
+	{
+		cloud = util3d::downsample(cloud, downsampleStep);
+		UDEBUG("Downsampling scan (step=%d): %d -> %d", downsampleStep, previousSize, (int)cloud->size());
+	}
+	previousSize = (int)cloud->size();
+	if(voxelSize > 0.0f && cloud->size())
+	{
+		cloud = util3d::voxelize(cloud, voxelSize);
+		UDEBUG("Voxel filtering scan (voxel=%f m): %d -> %d", voxelSize, previousSize, (int)cloud->size());
+	}
+	if(normalsK > 0 && cloud->size())
+	{
+		pcl::PointCloud<pcl::PointNormal>::Ptr cloudNormals = util3d::computeNormals(cloud, normalsK);
+		scan = util3d::laserScanFromPointCloud(*cloudNormals, transform);
+	}
+	else
+	{
+		scan = util3d::laserScanFromPointCloud(*cloud, transform);
+	}
+	return scan;
+}
+
 }
 
 }

corelib/src/util3d_filtering.cpp

@@ -53,8 +53,6 @@ cv::Mat downsample(
 		const cv::Mat & cloud,
 		int step)
 {
-	// 2D or 3D point clouds (laser scans)
-	UASSERT(cloud.type() == CV_32FC2 || cloud.type() == CV_32FC3);
 	UASSERT(step > 0);
 	cv::Mat output;
 	if(step == 1)
@@ -156,6 +154,18 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr voxelize(
 	filter.filter(*output);
 	return output;
 }
+pcl::PointCloud<pcl::PointNormal>::Ptr voxelize(
+		const pcl::PointCloud<pcl::PointNormal>::Ptr & cloud,
+		float voxelSize)
+{
+	UASSERT(voxelSize > 0.0f);
+	pcl::PointCloud<pcl::PointNormal>::Ptr output(new pcl::PointCloud<pcl::PointNormal>);
+	pcl::VoxelGrid<pcl::PointNormal> filter;
+	filter.setLeafSize(voxelSize, voxelSize, voxelSize);
+	filter.setInputCloud(cloud);
+	filter.filter(*output);
+	return output;
+}
 pcl::PointCloud<pcl::PointXYZRGB>::Ptr voxelize(
 		const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
 		float voxelSize)

corelib/src/util3d_registration.cpp

@@ -293,13 +293,21 @@ Transform icp(const pcl::PointCloud<pcl::PointXYZ>::ConstPtr & cloud_source,
 			  double maxCorrespondenceDistance,
 			  int maximumIterations,
 			  bool & hasConverged,
-			  pcl::PointCloud<pcl::PointXYZ> & cloud_source_registered)
+			  pcl::PointCloud<pcl::PointXYZ> & cloud_source_registered,
+			  bool icp2D)
 {
 	pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> icp;
 	// Set the input source and target
 	icp.setInputTarget (cloud_target);
 	icp.setInputSource (cloud_source);
 
+	if(icp2D)
+	{
+		pcl::registration::TransformationEstimation2D<pcl::PointXYZ, pcl::PointXYZ>::Ptr est;
+		est.reset(new pcl::registration::TransformationEstimation2D<pcl::PointXYZ, pcl::PointXYZ>);
+		icp.setTransformationEstimation(est);
+	}
+
 	// Set the max correspondence distance to 5cm (e.g., correspondences with higher distances will be ignored)
 	icp.setMaxCorrespondenceDistance (maxCorrespondenceDistance);
 	// Set the maximum number of iterations (criterion 1)
@@ -350,40 +358,6 @@ Transform icpPointToPlane(
 	return Transform::fromEigen4f(icp.getFinalTransformation());
 }
 
-// return transform from source to target (All points must be finite!!!)
-Transform icp2D(const pcl::PointCloud<pcl::PointXYZ>::ConstPtr & cloud_source,
-			  const pcl::PointCloud<pcl::PointXYZ>::ConstPtr & cloud_target,
-			  double maxCorrespondenceDistance,
-			  int maximumIterations,
-			  bool & hasConverged,
-			  pcl::PointCloud<pcl::PointXYZ> & cloud_source_registered)
-{
-	pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> icp;
-	// Set the input source and target
-	icp.setInputTarget (cloud_target);
-	icp.setInputSource (cloud_source);
-
-	pcl::registration::TransformationEstimation2D<pcl::PointXYZ, pcl::PointXYZ>::Ptr est;
-	est.reset(new pcl::registration::TransformationEstimation2D<pcl::PointXYZ, pcl::PointXYZ>);
-	icp.setTransformationEstimation(est);
-
-	// Set the max correspondence distance to 5cm (e.g., correspondences with higher distances will be ignored)
-	icp.setMaxCorrespondenceDistance (maxCorrespondenceDistance);
-	// Set the maximum number of iterations (criterion 1)
-	icp.setMaximumIterations (maximumIterations);
-	// Set the transformation epsilon (criterion 2)
-	//icp.setTransformationEpsilon (1e-8);
-	// Set the euclidean distance difference epsilon (criterion 3)
-	//icp.setEuclideanFitnessEpsilon (1);
-	//icp.setRANSACOutlierRejectionThreshold(maxCorrespondenceDistance);
-
-	// Perform the alignment
-	icp.align (cloud_source_registered);
-	hasConverged = icp.hasConverged();
-	return Transform::fromEigen4f(icp.getFinalTransformation());
-}
-
-
 // If "voxel" > 0, "samples" is ignored
 pcl::PointCloud<pcl::PointXYZ>::Ptr getICPReadyCloud(
 		const cv::Mat & depth,

guilib/include/rtabmap/gui/CloudViewer.h

@@ -80,16 +80,22 @@ public:
 			const QColor & color = QColor());
 
 	bool updateCloud(
-		const std::string & id,
-		const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
-		const Transform & pose = Transform::getIdentity(),
-		const QColor & color = QColor());
+			const std::string & id,
+			const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
+			const Transform & pose = Transform::getIdentity(),
+			const QColor & color = QColor());
 
 	bool updateCloud(
-		const std::string & id,
-		const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
-		const Transform & pose = Transform::getIdentity(),
-		const QColor & color = QColor());
+			const std::string & id,
+			const pcl::PointCloud<pcl::PointNormal>::Ptr & cloud,
+			const Transform & pose = Transform::getIdentity(),
+			const QColor & color = QColor());
+
+	bool updateCloud(
+			const std::string & id,
+			const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
+			const Transform & pose = Transform::getIdentity(),
+			const QColor & color = QColor());
 
 	bool addOrUpdateCloud(
 			const std::string & id,
@@ -98,10 +104,16 @@ public:
 			const QColor & color = QColor());
 
 	bool addOrUpdateCloud(
-		const std::string & id,
-		const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
-		const Transform & pose = Transform::getIdentity(),
-		const QColor & color = QColor());
+			const std::string & id,
+			const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
+			const Transform & pose = Transform::getIdentity(),
+			const QColor & color = QColor());
+
+	bool addOrUpdateCloud(
+			const std::string & id,
+			const pcl::PointCloud<pcl::PointNormal>::Ptr & cloud,
+			const Transform & pose = Transform::getIdentity(),
+			const QColor & color = QColor());
 
 	bool addOrUpdateCloud(
 			const std::string & id,

guilib/include/rtabmap/gui/PreferencesDialog.h

@@ -192,10 +192,13 @@ public:
 	int getSourceDatabaseStartPos() const; //Database group
 	bool getSourceDatabaseStampsUsed() const;//Database group
 	bool isSourceRGBDColorOnly() const;
+	int getSourceImageDecimation() const;
 	bool isSourceStereoDepthGenerated() const;
 	bool isSourceScanFromDepth() const;
 	int getSourceScanFromDepthDecimation() const;
 	double getSourceScanFromDepthMaxDepth() const;
+	double getSourceScanVoxelSize() const;
+	int getSourceScanNormalsK() const;
 	Transform getSourceLocalTransform() const;    //Openni group
 	Transform getLaserLocalTransform() const; // directory images
 	Camera * createCamera(bool useRawImages = false); // return camera should be deleted if not null

guilib/src/CloudViewer.cpp

@@ -357,6 +357,26 @@ bool CloudViewer::updateCloud(
 	return false;
 }
 
+bool CloudViewer::updateCloud(
+		const std::string & id,
+		const pcl::PointCloud<pcl::PointNormal>::Ptr & cloud,
+		const Transform & pose,
+		const QColor & color)
+{
+	if(_addedClouds.contains(id))
+	{
+		UDEBUG("Updating %s with %d points", id.c_str(), (int)cloud->size());
+		int index = _visualizer->getColorHandlerIndex(id);
+		this->removeCloud(id);
+		if(this->addCloud(id, cloud, pose, color))
+		{
+			_visualizer->updateColorHandlerIndex(id, index);
+			return true;
+		}
+	}
+	return false;
+}
+
 bool CloudViewer::updateCloud(
 		const std::string & id,
 		const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
@@ -403,6 +423,19 @@ bool CloudViewer::addOrUpdateCloud(
 	return true;
 }
 
+bool CloudViewer::addOrUpdateCloud(
+		const std::string & id,
+		const pcl::PointCloud<pcl::PointNormal>::Ptr & cloud,
+		const Transform & pose,
+		const QColor & color)
+{
+	if(!updateCloud(id, cloud, pose, color))
+	{
+		return addCloud(id, cloud, pose, color);
+	}
+	return true;
+}
+
 bool CloudViewer::addOrUpdateCloud(
 		const std::string & id,
 		const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,

guilib/src/MainWindow.cpp

@@ -738,6 +738,7 @@ void MainWindow::handleEvent(UEvent* anEvent)
 void MainWindow::processCameraInfo(const rtabmap::CameraInfo & info)
 {
 	_ui->statsToolBox->updateStat("Camera/Time capturing/ms", (float)info.id, (float)info.timeCapture*1000.0);
+	_ui->statsToolBox->updateStat("Camera/Time decimation/ms", (float)info.id, (float)info.timeImageDecimation*1000.0);
 	_ui->statsToolBox->updateStat("Camera/Time disparity/ms", (float)info.id, (float)info.timeDisparity*1000.0);
 	_ui->statsToolBox->updateStat("Camera/Time mirroring/ms", (float)info.id, (float)info.timeMirroring*1000.0);
 	_ui->statsToolBox->updateStat("Camera/Time scan from depth/ms", (float)info.id, (float)info.timeScanFromDepth*1000.0);
@@ -745,6 +746,7 @@ void MainWindow::processCameraInfo(const rtabmap::CameraInfo & info)
 
 void MainWindow::processOdometry(const rtabmap::OdometryEvent & odom)
 {
+	UDEBUG("");
 	_processingOdometry = true;
 	UTimer time;
 	// Process Data
@@ -989,6 +991,10 @@ void MainWindow::processOdometry(const rtabmap::OdometryEvent & odom)
 	{
 		_ui->statsToolBox->updateStat("Odometry/Inliers/", (float)odom.data().id(), (float)odom.info().inliers);
 	}
+	if(odom.info().icpInliersRatio >= 0)
+	{
+		_ui->statsToolBox->updateStat("Odometry/ICP_Inliers_Ratio/", (float)odom.data().id(), (float)odom.info().icpInliersRatio);
+	}
 	if(odom.info().matches >= 0)
 	{
 		_ui->statsToolBox->updateStat("Odometry/Matches/", (float)odom.data().id(), (float)odom.info().matches);
@@ -1003,11 +1009,11 @@ void MainWindow::processOdometry(const rtabmap::OdometryEvent & odom)
 	}
 	if(odom.info().timeEstimation > 0)
 	{
-		_ui->statsToolBox->updateStat("Odometry/TimeEstimation/ms", (float)odom.data().id(), (float)odom.info().timeEstimation*1000.0f);
+		_ui->statsToolBox->updateStat("Odometry/Time_Estimation/ms", (float)odom.data().id(), (float)odom.info().timeEstimation*1000.0f);
 	}
 	if(odom.info().timeParticleFiltering > 0)
 	{
-		_ui->statsToolBox->updateStat("Odometry/TimeFiltering/ms", (float)odom.data().id(), (float)odom.info().timeParticleFiltering*1000.0f);
+		_ui->statsToolBox->updateStat("Odometry/Time_Filtering/ms", (float)odom.data().id(), (float)odom.info().timeParticleFiltering*1000.0f);
 	}
 	if(odom.info().features >=0)
 	{
@@ -1015,7 +1021,7 @@ void MainWindow::processOdometry(const rtabmap::OdometryEvent & odom)
 	}
 	if(odom.info().localMapSize >=0)
 	{
-		_ui->statsToolBox->updateStat("Odometry/Local_map_size/", (float)odom.data().id(), (float)odom.info().localMapSize);
+		_ui->statsToolBox->updateStat("Odometry/Local_Map_Size/", (float)odom.data().id(), (float)odom.info().localMapSize);
 	}
 	_ui->statsToolBox->updateStat("Odometry/ID/", (float)odom.data().id(), (float)odom.data().id());
 
@@ -1085,7 +1091,7 @@ void MainWindow::processOdometry(const rtabmap::OdometryEvent & odom)
 		_ui->statsToolBox->updateStat("Odometry/Distance/m", (float)odom.data().id(), odom.info().distanceTravelled);
 	}
 
-	_ui->statsToolBox->updateStat("/Gui refresh odom/ms", (float)odom.data().id(), time.elapsed()*1000.0);
+	_ui->statsToolBox->updateStat("/Gui Refresh Odom/ms", (float)odom.data().id(), time.elapsed()*1000.0);
 	_processingOdometry = false;
 }
 
@@ -1414,7 +1420,7 @@ void MainWindow::processStats(const rtabmap::Statistics & stat)
 	}
 	float elapsedTime = static_cast<float>(totalTime.elapsed());
 	UINFO("Updating GUI time = %fs", elapsedTime/1000.0f);
-	_ui->statsToolBox->updateStat("/Gui refresh stats/ms", stat.refImageId(), elapsedTime);
+	_ui->statsToolBox->updateStat("/Gui Refresh Stats/ms", stat.refImageId(), elapsedTime);
 	if(_ui->actionAuto_screen_capture->isChecked() && !_autoScreenCaptureOdomSync)
 	{
 		this->captureScreen(_autoScreenCaptureRAM);
@@ -1562,7 +1568,6 @@ void MainWindow::updateMapCloud(
 			}
 			else if(viewerClouds.contains(cloudName))
 			{
-				UDEBUG("Hide cloud %s", cloudName.c_str());
 				_ui->widget_cloudViewer->setCloudVisibility(cloudName.c_str(), false);
 			}
 
@@ -1603,7 +1608,6 @@ void MainWindow::updateMapCloud(
 			}
 			else if(viewerClouds.contains(scanName))
 			{
-				UDEBUG("Hide scan %s", scanName.c_str());
 				_ui->widget_cloudViewer->setCloudVisibility(scanName.c_str(), false);
 			}
 
@@ -2034,37 +2038,65 @@ void MainWindow::createAndAddScanToMap(int nodeId, const Transform & pose, int m
 
 	if(!iter->sensorData().laserScanCompressed().empty())
 	{
-		cv::Mat depth2D;
-		iter->sensorData().uncompressData(0, 0, &depth2D);
+		cv::Mat scan;
+		iter->sensorData().uncompressData(0, 0, &scan);
 
-		pcl::PointCloud<pcl::PointXYZ>::Ptr cloud;
-		cloud = util3d::laserScanToPointCloud(depth2D);
 		if(_preferencesDialog->getDownsamplingStepScan(0) > 0)
 		{
-			cloud = util3d::downsample(cloud, _preferencesDialog->getDownsamplingStepScan(0));
+			scan = util3d::downsample(scan, _preferencesDialog->getDownsamplingStepScan(0));
 		}
-		if(_preferencesDialog->getCloudVoxelSizeScan(0) > 0.0)
+
+		if(scan.channels() == 6)
 		{
-			cloud = util3d::voxelize(cloud, _preferencesDialog->getCloudVoxelSizeScan(0));
-		}
-		QColor color = Qt::gray;
-		if(mapId >= 0)
-		{
-			color = (Qt::GlobalColor)(mapId+3 % 12 + 7 );
-		}
-		if(!_ui->widget_cloudViewer->addOrUpdateCloud(scanName, cloud, pose, color))
-		{
-			UERROR("Adding cloud %d to viewer failed!", nodeId);
+			pcl::PointCloud<pcl::PointNormal>::Ptr cloud;
+			cloud = util3d::laserScanToPointCloudNormal(scan);
+			if(_preferencesDialog->getCloudVoxelSizeScan(0) > 0.0)
+			{
+				cloud = util3d::voxelize(cloud, _preferencesDialog->getCloudVoxelSizeScan(0));
+			}
+			QColor color = Qt::gray;
+			if(mapId >= 0)
+			{
+				color = (Qt::GlobalColor)(mapId+3 % 12 + 7 );
+			}
+			if(!_ui->widget_cloudViewer->addOrUpdateCloud(scanName, cloud, pose, color))
+			{
+				UERROR("Adding cloud %d to viewer failed!", nodeId);
+			}
+			else
+			{
+				pcl::PointCloud<pcl::PointXYZ>::Ptr cloudXYZ(new pcl::PointCloud<pcl::PointXYZ>);
+				pcl::copyPointCloud(*cloud, *cloudXYZ);
+				_createdScans.insert(std::make_pair(nodeId, cloudXYZ));
+			}
 		}
 		else
 		{
-			_createdScans.insert(std::make_pair(nodeId, cloud));
-
-			if(depth2D.channels() == 2)
+			pcl::PointCloud<pcl::PointXYZ>::Ptr cloud;
+			cloud = util3d::laserScanToPointCloud(scan);
+			if(_preferencesDialog->getCloudVoxelSizeScan(0) > 0.0)
 			{
-				cv::Mat ground, obstacles;
-				util3d::occupancy2DFromLaserScan(depth2D, ground, obstacles, _preferencesDialog->getGridMapResolution());
-				_gridLocalMaps.insert(std::make_pair(nodeId, std::make_pair(ground, obstacles)));
+				cloud = util3d::voxelize(cloud, _preferencesDialog->getCloudVoxelSizeScan(0));
+			}
+			QColor color = Qt::gray;
+			if(mapId >= 0)
+			{
+				color = (Qt::GlobalColor)(mapId+3 % 12 + 7 );
+			}
+			if(!_ui->widget_cloudViewer->addOrUpdateCloud(scanName, cloud, pose, color))
+			{
+				UERROR("Adding cloud %d to viewer failed!", nodeId);
+			}
+			else
+			{
+				_createdScans.insert(std::make_pair(nodeId, cloud));
+
+				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)));
+				}
 			}
 		}
 		_ui->widget_cloudViewer->setCloudOpacity(scanName, _preferencesDialog->getScanOpacity(0));
@@ -2117,6 +2149,7 @@ Transform MainWindow::alignPosesToGroundTruth(
 				}
 			}
 		}
+		UDEBUG("t=%s", t.prettyPrint().c_str());
 	}
 	return t;
 }
@@ -3104,8 +3137,14 @@ void MainWindow::startDetection()
 		_camera = new CameraThread(camera, parameters);
 		_camera->setMirroringEnabled(_preferencesDialog->isSourceMirroring());
 		_camera->setColorOnly(_preferencesDialog->isSourceRGBDColorOnly());
+		_camera->setImageDecimation(_preferencesDialog->getSourceImageDecimation());
 		_camera->setStereoToDepth(_preferencesDialog->isSourceStereoDepthGenerated());
-		_camera->setScanFromDepth(_preferencesDialog->isSourceScanFromDepth(), _preferencesDialog->getSourceScanFromDepthDecimation(), _preferencesDialog->getSourceScanFromDepthMaxDepth());
+		_camera->setScanFromDepth(
+				_preferencesDialog->isSourceScanFromDepth(),
+				_preferencesDialog->getSourceScanFromDepthDecimation(),
+				_preferencesDialog->getSourceScanFromDepthMaxDepth(),
+				_preferencesDialog->getSourceScanVoxelSize(),
+				_preferencesDialog->getSourceScanNormalsK());
 
 		//Create odometry thread if rgbd slam
 		if(uStr2Bool(parameters.at(Parameters::kRGBDEnabled()).c_str()))

guilib/src/PreferencesDialog.cpp

@@ -401,6 +401,8 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	connect(_ui->lineEdit_cameraImages_path_scans, SIGNAL(textChanged(const QString &)), this, SLOT(makeObsoleteSourcePanel()));
 	connect(_ui->lineEdit_cameraImages_laser_transform, SIGNAL(textChanged(const QString &)), this, SLOT(makeObsoleteSourcePanel()));
 	connect(_ui->spinBox_cameraImages_max_scan_pts, SIGNAL(valueChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
+	connect(_ui->spinBox_cameraImages_scanDownsampleStep, SIGNAL(valueChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
+	connect(_ui->doubleSpinBox_cameraImages_scanVoxelSize, SIGNAL(valueChanged(double)), this, SLOT(makeObsoleteSourcePanel()));
 	connect(_ui->lineEdit_cameraImages_gt, SIGNAL(textChanged(const QString &)), this, SLOT(makeObsoleteSourcePanel()));
 	connect(_ui->comboBox_cameraImages_gtFormat, SIGNAL(currentIndexChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
 
@@ -415,6 +417,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	connect(_ui->checkBox_stereoVideo_rectify, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
 
 	connect(_ui->checkbox_rgbd_colorOnly, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
+	connect(_ui->spinBox_source_imageDecimation, SIGNAL(valueChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
 	connect(_ui->checkbox_stereo_depthGenerated, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
 	connect(_ui->pushButton_calibrate, SIGNAL(clicked()), this, SLOT(calibrate()));
 	connect(_ui->pushButton_calibrate_simple, SIGNAL(clicked()), this, SLOT(calibrateSimple()));
@@ -426,6 +429,8 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	connect(_ui->groupBox_scanFromDepth, SIGNAL(toggled(bool)), this, SLOT(makeObsoleteSourcePanel()));
 	connect(_ui->spinBox_cameraScanFromDepth_decimation, SIGNAL(valueChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
 	connect(_ui->doubleSpinBox_cameraSCanFromDepth_maxDepth, SIGNAL(valueChanged(double)), this, SLOT(makeObsoleteSourcePanel()));
+	connect(_ui->doubleSpinBox_cameraImages_scanVoxelSize, SIGNAL(valueChanged(double)), this, SLOT(makeObsoleteSourcePanel()));
+	connect(_ui->spinBox_cameraImages_scanNormalsK, SIGNAL(valueChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
 
 	//Rtabmap basic
 	connect(_ui->general_doubleSpinBox_timeThr, SIGNAL(valueChanged(double)), _ui->general_doubleSpinBox_timeThr_2, SLOT(setValue(double)));
@@ -1078,7 +1083,7 @@ void PreferencesDialog::resetSettings(QGroupBox * groupBox)
 			_3dRenderingMaxDepth[i]->setValue(0.0);
 			_3dRenderingShowScans[i]->setChecked(true);
 
-			_3dRenderingDownsamplingScan[i]->setValue(0);
+			_3dRenderingDownsamplingScan[i]->setValue(1);
 			_3dRenderingVoxelSizeScan[i]->setValue(0.0);
 			_3dRenderingOpacity[i]->setValue(i==0?1.0:0.5);
 			_3dRenderingPtSize[i]->setValue(2);
@@ -1168,6 +1173,7 @@ void PreferencesDialog::resetSettings(QGroupBox * groupBox)
 		}
 
 		_ui->checkbox_rgbd_colorOnly->setChecked(false);
+		_ui->spinBox_source_imageDecimation->setValue(1);
 		_ui->checkbox_stereo_depthGenerated->setChecked(false);
 		_ui->openni2_autoWhiteBalance->setChecked(true);
 		_ui->openni2_autoExposure->setChecked(true);
@@ -1199,12 +1205,16 @@ void PreferencesDialog::resetSettings(QGroupBox * groupBox)
 		_ui->lineEdit_cameraImages_path_scans->setText("");
 		_ui->lineEdit_cameraImages_laser_transform->setText("0 0 0 0 0 0");
 		_ui->spinBox_cameraImages_max_scan_pts->setValue(0);
+		_ui->spinBox_cameraImages_scanDownsampleStep->setValue(1);
+		_ui->doubleSpinBox_cameraImages_scanVoxelSize->setValue(0.0f);
 		_ui->lineEdit_cameraImages_gt->setText("");
 		_ui->comboBox_cameraImages_gtFormat->setCurrentIndex(0);
 
 		_ui->groupBox_scanFromDepth->setChecked(false);
 		_ui->spinBox_cameraScanFromDepth_decimation->setValue(8);
 		_ui->doubleSpinBox_cameraSCanFromDepth_maxDepth->setValue(4.0);
+		_ui->doubleSpinBox_cameraImages_scanVoxelSize->setValue(0.0f);
+		_ui->spinBox_cameraImages_scanNormalsK->setValue(0);
 	}
 	else if(groupBox->objectName() == _ui->groupBox_rtabmap_basic0->objectName())
 	{
@@ -1441,6 +1451,7 @@ void PreferencesDialog::readCameraSettings(const QString & filePath)
 	_ui->comboBox_sourceType->setCurrentIndex(settings.value("type", _ui->comboBox_sourceType->currentIndex()).toInt());
 	_ui->lineEdit_sourceDevice->setText(settings.value("device",_ui->lineEdit_sourceDevice->text()).toString());
 	_ui->lineEdit_sourceLocalTransform->setText(settings.value("localTransform",_ui->lineEdit_sourceLocalTransform->text()).toString());
+	_ui->spinBox_source_imageDecimation->setValue(settings.value("imageDecimation",_ui->spinBox_source_imageDecimation->value()).toInt());
 
 	settings.beginGroup("rgbd");
 	_ui->comboBox_cameraRGBD->setCurrentIndex(settings.value("driver", _ui->comboBox_cameraRGBD->currentIndex()).toInt());
@@ -1507,6 +1518,8 @@ void PreferencesDialog::readCameraSettings(const QString & filePath)
 	_ui->lineEdit_cameraImages_path_scans->setText(settings.value("path_scans", _ui->lineEdit_cameraImages_path_scans->text()).toString());
 	_ui->lineEdit_cameraImages_laser_transform->setText(settings.value("scan_transform", _ui->lineEdit_cameraImages_laser_transform->text()).toString());
 	_ui->spinBox_cameraImages_max_scan_pts->setValue(settings.value("scan_max_pts", _ui->spinBox_cameraImages_max_scan_pts->value()).toInt());
+	_ui->spinBox_cameraImages_scanDownsampleStep->setValue(settings.value("scan_downsample_step", _ui->spinBox_cameraImages_scanDownsampleStep->value()).toInt());
+	_ui->doubleSpinBox_cameraImages_scanVoxelSize->setValue(settings.value("scan_voxel_size", _ui->doubleSpinBox_cameraImages_scanVoxelSize->value()).toDouble());
 	_ui->lineEdit_cameraImages_gt->setText(settings.value("gt_path", _ui->lineEdit_cameraImages_gt->text()).toString());
 	_ui->comboBox_cameraImages_gtFormat->setCurrentIndex(settings.value("gt_format", _ui->comboBox_cameraImages_gtFormat->currentIndex()).toInt());
 	settings.endGroup(); // images
@@ -1520,6 +1533,8 @@ void PreferencesDialog::readCameraSettings(const QString & filePath)
 	_ui->groupBox_scanFromDepth->setChecked(settings.value("enabled", _ui->groupBox_scanFromDepth->isChecked()).toBool());
 	_ui->spinBox_cameraScanFromDepth_decimation->setValue(settings.value("decimation", _ui->spinBox_cameraScanFromDepth_decimation->value()).toInt());
 	_ui->doubleSpinBox_cameraSCanFromDepth_maxDepth->setValue(settings.value("maxDepth", _ui->doubleSpinBox_cameraSCanFromDepth_maxDepth->value()).toDouble());
+	_ui->doubleSpinBox_cameraImages_scanVoxelSize->setValue(settings.value("voxelSize", _ui->doubleSpinBox_cameraImages_scanVoxelSize->value()).toDouble());
+	_ui->spinBox_cameraImages_scanNormalsK->setValue(settings.value("normalsK", _ui->spinBox_cameraImages_scanNormalsK->value()).toInt());
 	settings.endGroup();//ScanFromDepth
 
 	settings.beginGroup("Database");
@@ -1794,6 +1809,7 @@ void PreferencesDialog::writeCameraSettings(const QString & filePath) const
 	settings.setValue("type",            _ui->comboBox_sourceType->currentIndex());
 	settings.setValue("device", 		 _ui->lineEdit_sourceDevice->text());
 	settings.setValue("localTransform",  _ui->lineEdit_sourceLocalTransform->text());
+	settings.setValue("imageDecimation",  _ui->spinBox_source_imageDecimation->value());
 
 	settings.beginGroup("rgbd");
 	settings.setValue("driver", 	   _ui->comboBox_cameraRGBD->currentIndex());
@@ -1859,6 +1875,8 @@ void PreferencesDialog::writeCameraSettings(const QString & filePath) const
 	settings.setValue("path_scans",          _ui->lineEdit_cameraImages_path_scans->text());
 	settings.setValue("scan_transform",      _ui->lineEdit_cameraImages_laser_transform->text());
 	settings.setValue("scan_max_pts",        _ui->spinBox_cameraImages_max_scan_pts->value());
+	settings.setValue("scan_downsample_step", _ui->spinBox_cameraImages_scanDownsampleStep->value());
+	settings.setValue("scan_voxel_size",     _ui->doubleSpinBox_cameraImages_scanVoxelSize->value());
 	settings.setValue("gt_path",             _ui->lineEdit_cameraImages_gt->text());
 	settings.setValue("gt_format",           _ui->comboBox_cameraImages_gtFormat->currentIndex());
 	settings.endGroup(); // images
@@ -1872,6 +1890,8 @@ void PreferencesDialog::writeCameraSettings(const QString & filePath) const
 	settings.setValue("enabled", 			_ui->groupBox_scanFromDepth->isChecked());
 	settings.setValue("decimation", 		_ui->spinBox_cameraScanFromDepth_decimation->value());
 	settings.setValue("maxDepth", 			_ui->doubleSpinBox_cameraSCanFromDepth_maxDepth->value());
+	settings.setValue("voxelSize", 			_ui->doubleSpinBox_cameraImages_scanVoxelSize->value());
+	settings.setValue("normalsK", 			_ui->spinBox_cameraImages_scanNormalsK->value());
 	settings.endGroup();
 
 	settings.beginGroup("Database");
@@ -3337,6 +3357,8 @@ void PreferencesDialog::updateSourceGrpVisibility()
 			(_ui->comboBox_sourceType->currentIndex() == 0 && _ui->comboBox_cameraRGBD->currentIndex() == kSrcRGBDImages-kSrcRGBD) ||
 			(_ui->comboBox_sourceType->currentIndex() == 1 && _ui->comboBox_cameraStereo->currentIndex() == kSrcStereoImages-kSrcStereo) ||
 			(_ui->comboBox_sourceType->currentIndex() == 2 && _ui->comboBox_sourceType->currentIndex() == kSrcImages-kSrcRGB));
+
+	_ui->groupBox_scan->setVisible(_ui->comboBox_sourceType->currentIndex() != 3);
 }
 
 /*** GETTERS ***/
@@ -3650,6 +3672,10 @@ bool PreferencesDialog::isSourceRGBDColorOnly() const
 {
 	return _ui->checkbox_rgbd_colorOnly->isChecked();
 }
+int PreferencesDialog::getSourceImageDecimation() const
+{
+	return _ui->spinBox_source_imageDecimation->value();
+}
 bool PreferencesDialog::isSourceStereoDepthGenerated() const
 {
 	return _ui->checkbox_stereo_depthGenerated->isChecked();
@@ -3666,6 +3692,14 @@ double PreferencesDialog::getSourceScanFromDepthMaxDepth() const
 {
 	return _ui->doubleSpinBox_cameraSCanFromDepth_maxDepth->value();
 }
+double PreferencesDialog::getSourceScanVoxelSize() const
+{
+	return _ui->doubleSpinBox_cameraImages_scanVoxelSize->value();
+}
+int PreferencesDialog::getSourceScanNormalsK() const
+{
+	return _ui->spinBox_cameraImages_scanNormalsK->value();
+}
 
 Camera * PreferencesDialog::createCamera(bool useRawImages)
 {
@@ -3765,6 +3799,9 @@ Camera * PreferencesDialog::createCamera(bool useRawImages)
 		((CameraRGBDImages*)camera)->setScanPath(
 						_ui->lineEdit_cameraImages_path_scans->text().isEmpty()?"":_ui->lineEdit_cameraImages_path_scans->text().append(QDir::separator()).toStdString(),
 						_ui->spinBox_cameraImages_max_scan_pts->value(),
+						_ui->spinBox_cameraImages_scanDownsampleStep->value(),
+						_ui->doubleSpinBox_cameraImages_scanVoxelSize->value(),
+						_ui->spinBox_cameraImages_scanNormalsK->value(),
 						this->getLaserLocalTransform());
 		((CameraRGBDImages*)camera)->setTimestamps(_ui->checkBox_cameraImages_timestamps->isChecked(), _ui->lineEdit_cameraImages_timestamps->text().toStdString());
 	}
@@ -3802,6 +3839,9 @@ Camera * PreferencesDialog::createCamera(bool useRawImages)
 		((CameraStereoImages*)camera)->setScanPath(
 						_ui->lineEdit_cameraImages_path_scans->text().isEmpty()?"":_ui->lineEdit_cameraImages_path_scans->text().append(QDir::separator()).toStdString(),
 						_ui->spinBox_cameraImages_max_scan_pts->value(),
+						_ui->spinBox_cameraImages_scanDownsampleStep->value(),
+						_ui->doubleSpinBox_cameraImages_scanVoxelSize->value(),
+						_ui->spinBox_cameraImages_scanNormalsK->value(),
 						this->getLaserLocalTransform());
 		((CameraStereoImages*)camera)->setTimestamps(_ui->checkBox_cameraImages_timestamps->isChecked(), _ui->lineEdit_cameraImages_timestamps->text().toStdString());
 	}
@@ -3843,6 +3883,9 @@ Camera * PreferencesDialog::createCamera(bool useRawImages)
 		((CameraRGBDImages*)camera)->setScanPath(
 						_ui->lineEdit_cameraImages_path_scans->text().isEmpty()?"":_ui->lineEdit_cameraImages_path_scans->text().append(QDir::separator()).toStdString(),
 						_ui->spinBox_cameraImages_max_scan_pts->value(),
+						_ui->spinBox_cameraImages_scanDownsampleStep->value(),
+						_ui->doubleSpinBox_cameraImages_scanVoxelSize->value(),
+						_ui->spinBox_cameraImages_scanNormalsK->value(),
 						this->getLaserLocalTransform());
 		((CameraRGBDImages*)camera)->setTimestamps(_ui->checkBox_cameraImages_timestamps->isChecked(), _ui->lineEdit_cameraImages_timestamps->text().toStdString());
 	}
@@ -4072,8 +4115,14 @@ void PreferencesDialog::testOdometry()
 		CameraThread cameraThread(camera, this->getAllParameters()); // take ownership of camera
 		cameraThread.setMirroringEnabled(isSourceMirroring());
 		cameraThread.setColorOnly(_ui->checkbox_rgbd_colorOnly->isChecked());
+		cameraThread.setImageDecimation(_ui->spinBox_source_imageDecimation->value());
 		cameraThread.setStereoToDepth(_ui->checkbox_stereo_depthGenerated->isChecked());
-		cameraThread.setScanFromDepth(_ui->groupBox_scanFromDepth->isChecked(), _ui->spinBox_cameraScanFromDepth_decimation->value(), _ui->doubleSpinBox_cameraSCanFromDepth_maxDepth->value());
+		cameraThread.setScanFromDepth(
+				_ui->groupBox_scanFromDepth->isChecked(),
+				_ui->spinBox_cameraScanFromDepth_decimation->value(),
+				_ui->doubleSpinBox_cameraSCanFromDepth_maxDepth->value(),
+				_ui->doubleSpinBox_cameraImages_scanVoxelSize->value(),
+				_ui->spinBox_cameraImages_scanNormalsK->value());
 		UEventsManager::createPipe(&cameraThread, &odomThread, "CameraEvent");
 		UEventsManager::createPipe(&odomThread, odomViewer, "OdometryEvent");
 		UEventsManager::createPipe(odomViewer, &odomThread, "OdometryResetEvent");
@@ -4140,8 +4189,14 @@ void PreferencesDialog::testCamera()
 			CameraThread cameraThread(camera, this->getAllParameters());
 			cameraThread.setMirroringEnabled(isSourceMirroring());
 			cameraThread.setColorOnly(_ui->checkbox_rgbd_colorOnly->isChecked());
+			cameraThread.setImageDecimation(_ui->spinBox_source_imageDecimation->value());
 			cameraThread.setStereoToDepth(_ui->checkbox_stereo_depthGenerated->isChecked());
-			cameraThread.setScanFromDepth(_ui->groupBox_scanFromDepth->isChecked(), _ui->spinBox_cameraScanFromDepth_decimation->value(), _ui->doubleSpinBox_cameraSCanFromDepth_maxDepth->value());
+			cameraThread.setScanFromDepth(
+					_ui->groupBox_scanFromDepth->isChecked(),
+					_ui->spinBox_cameraScanFromDepth_decimation->value(),
+					_ui->doubleSpinBox_cameraSCanFromDepth_maxDepth->value(),
+					_ui->doubleSpinBox_cameraImages_scanVoxelSize->value(),
+					_ui->spinBox_cameraImages_scanNormalsK->value());
 			UEventsManager::createPipe(&cameraThread, window, "CameraEvent");
 
 			cameraThread.start();

guilib/src/ui/preferencesDialog.ui

@@ -63,7 +63,7 @@
           <property name="geometry">
            <rect>
             <x>0</x>
-            <y>-335</y>
+            <y>0</y>
             <width>676</width>
             <height>1982</height>
            </rect>
@@ -86,7 +86,7 @@
                 <enum>QFrame::Raised</enum>
                </property>
                <property name="currentIndex">
-                <number>3</number>
+                <number>14</number>
                </property>
                <widget class="QWidget" name="page_22">
                 <layout class="QVBoxLayout" name="verticalLayout_29" stretch="0,1">
@@ -1357,6 +1357,9 @@ Show a yellow background when the number of odometry inliers goes under this thr
                        </item>
                        <item row="10" column="1">
                         <widget class="QSpinBox" name="spinBox_downsamplingScan_odom">
+                         <property name="minimum">
+                          <number>1</number>
+                         </property>
                          <property name="maximum">
                           <number>9999</number>
                          </property>
@@ -1364,6 +1367,9 @@ Show a yellow background when the number of odometry inliers goes under this thr
                        </item>
                        <item row="10" column="0">
                         <widget class="QSpinBox" name="spinBox_downsamplingScan">
+                         <property name="minimum">
+                          <number>1</number>
+                         </property>
                          <property name="maximum">
                           <number>9999</number>
                          </property>
@@ -1655,6 +1661,82 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                         </property>
                        </widget>
                       </item>
+                      <item row="3" column="1">
+                       <widget class="QLabel" name="label_42">
+                        <property name="text">
+                         <string>Local transform from /base_link to /camera_link. Mouse over the box to show formats.</string>
+                        </property>
+                        <property name="wordWrap">
+                         <bool>true</bool>
+                        </property>
+                        <property name="textInteractionFlags">
+                         <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                        </property>
+                       </widget>
+                      </item>
+                      <item row="0" column="1">
+                       <widget class="QLabel" name="label_19">
+                        <property name="text">
+                         <string>Source type. Select specific driver below.</string>
+                        </property>
+                        <property name="wordWrap">
+                         <bool>true</bool>
+                        </property>
+                        <property name="textInteractionFlags">
+                         <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                        </property>
+                       </widget>
+                      </item>
+                      <item row="2" column="1">
+                       <widget class="QLabel" name="label_39">
+                        <property name="text">
+                         <string>ID of the device, which might be a serial number, bus@address or the index of the device. If empty, the first device found is taken.</string>
+                        </property>
+                        <property name="wordWrap">
+                         <bool>true</bool>
+                        </property>
+                        <property name="textInteractionFlags">
+                         <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                        </property>
+                       </widget>
+                      </item>
+                      <item row="9" column="1">
+                       <widget class="QLabel" name="label_244">
+                        <property name="text">
+                         <string>Create a simple calibration file with known intrinsics (fx, fy, cx, cy). Useful if you already know the intrinsics of a source of rectified or registered images.</string>
+                        </property>
+                        <property name="wordWrap">
+                         <bool>true</bool>
+                        </property>
+                        <property name="textInteractionFlags">
+                         <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                        </property>
+                       </widget>
+                      </item>
+                      <item row="6" column="0">
+                       <layout class="QHBoxLayout" name="horizontalLayout_5" stretch="0,1">
+                        <item>
+                         <widget class="QToolButton" name="toolButton_source_path_calibration">
+                          <property name="text">
+                           <string>...</string>
+                          </property>
+                         </widget>
+                        </item>
+                        <item>
+                         <widget class="QLineEdit" name="lineEdit_calibrationFile">
+                          <property name="minimumSize">
+                           <size>
+                            <width>100</width>
+                            <height>0</height>
+                           </size>
+                          </property>
+                          <property name="text">
+                           <string/>
+                          </property>
+                         </widget>
+                        </item>
+                       </layout>
+                      </item>
                       <item row="1" column="0">
                        <widget class="QDoubleSpinBox" name="general_doubleSpinBox_imgRate">
                         <property name="suffix">
@@ -1697,52 +1779,13 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                       <item row="3" column="0">
                        <widget class="QLineEdit" name="lineEdit_sourceLocalTransform">
                         <property name="toolTip">
-                         <string>&lt;html&gt;&lt;head/&gt;&lt;body&gt;&lt;p&gt; Format (3 values): x y z&lt;/p&gt;&lt;p&gt; Format (6 values): x y z roll pitch yaw&lt;/p&gt;&lt;p&gt; Format (7 values): x y z qx qy qz qw&lt;/p&gt;&lt;p&gt; Format (9 values, 3x3 rotation): r11 r12 r13 r21 r22 r23 r31 r32 r33&lt;/p&gt;&lt;p&gt; Format (12 values, 3x4 transform): r11 r12 r13 tx r21 r22 r23 ty r31 r32 r33 tz&lt;/p&gt;&lt;/body&gt;&lt;/html&gt;</string>
+                         <string>&lt;html&gt;&lt;head/&gt;&lt;body&gt;&lt;p&gt;Format (3 values): x y z&lt;br/&gt;Format (6 values): x y z roll pitch yaw&lt;br/&gt;Format (7 values): x y z qx qy qz qw&lt;br/&gt;Format (9 values, 3x3 rotation): r11 r12 r13 r21 r22 r23 r31 r32 r33&lt;br/&gt;Format (12 values, 3x4 transform): r11 r12 r13 tx r21 r22 r23 ty r31 r32 r33 tz&lt;/p&gt;&lt;p&gt;KITTI: /base_link to /gray_camera = 0 0 1 -1 0 0 0 -1 0&lt;br/&gt;KITTI: /base_link to /color_camera = 0 0 1 0 -1 0 0 -0.06 0 -1 0 0&lt;/p&gt;&lt;/body&gt;&lt;/html&gt;</string>
                         </property>
                         <property name="text">
                          <string>0 0 1 -1 0 0 0 -1 0</string>
                         </property>
                        </widget>
                       </item>
-                      <item row="3" column="1">
-                       <widget class="QLabel" name="label_42">
-                        <property name="text">
-                         <string>Local transform from /base_link to /camera_link. Mouse over the box to show formats.</string>
-                        </property>
-                        <property name="wordWrap">
-                         <bool>true</bool>
-                        </property>
-                        <property name="textInteractionFlags">
-                         <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
-                        </property>
-                       </widget>
-                      </item>
-                      <item row="0" column="1">
-                       <widget class="QLabel" name="label_19">
-                        <property name="text">
-                         <string>Source type. Select specific driver below.</string>
-                        </property>
-                        <property name="wordWrap">
-                         <bool>true</bool>
-                        </property>
-                        <property name="textInteractionFlags">
-                         <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
-                        </property>
-                       </widget>
-                      </item>
-                      <item row="2" column="1">
-                       <widget class="QLabel" name="label_39">
-                        <property name="text">
-                         <string>ID of the device, which might be a serial number, bus@address or the index of the device. If empty, the first device found is taken.</string>
-                        </property>
-                        <property name="wordWrap">
-                         <bool>true</bool>
-                        </property>
-                        <property name="textInteractionFlags">
-                         <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
-                        </property>
-                       </widget>
-                      </item>
                       <item row="0" column="0">
                        <widget class="QComboBox" name="comboBox_sourceType">
                         <property name="sizeAdjustPolicy">
@@ -1770,7 +1813,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                         </item>
                        </widget>
                       </item>
-                      <item row="9" column="0">
+                      <item row="10" column="0">
                        <widget class="QPushButton" name="pushButton_test_camera">
                         <property name="sizePolicy">
                          <sizepolicy hsizetype="Fixed" vsizetype="Fixed">
@@ -1783,7 +1826,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                         </property>
                        </widget>
                       </item>
-                      <item row="7" column="0">
+                      <item row="8" column="0">
                        <widget class="QPushButton" name="pushButton_calibrate">
                         <property name="sizePolicy">
                          <sizepolicy hsizetype="Fixed" vsizetype="Fixed">
@@ -1796,7 +1839,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                         </property>
                        </widget>
                       </item>
-                      <item row="7" column="1">
+                      <item row="8" column="1">
                        <widget class="QLabel" name="label_24">
                         <property name="text">
                          <string>Calibration files are saved in &quot;camera_info&quot; folder of the working directory.</string>
@@ -1816,7 +1859,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                         </property>
                        </widget>
                       </item>
-                      <item row="8" column="0">
+                      <item row="9" column="0">
                        <widget class="QPushButton" name="pushButton_calibrate_simple">
                         <property name="sizePolicy">
                          <sizepolicy hsizetype="Fixed" vsizetype="Fixed">
@@ -1829,10 +1872,23 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                         </property>
                        </widget>
                       </item>
-                      <item row="8" column="1">
-                       <widget class="QLabel" name="label_244">
+                      <item row="6" column="1">
+                       <widget class="QLabel" name="label_18">
                         <property name="text">
-                         <string>Create a simple calibration file with known intrinsics (fx, fy, cx, cy). Useful if you already know the intrinsics of a source of rectified or registered images.</string>
+                         <string>Calibration file path (*.yaml). If empty, the GUID of the camera is used (for those having one). OpenNI and Freenect drivers use factory calibration by default (so they ignore this parameter). A calibrated camera is required for RGB-D SLAM mode.</string>
+                        </property>
+                        <property name="wordWrap">
+                         <bool>true</bool>
+                        </property>
+                        <property name="textInteractionFlags">
+                         <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                        </property>
+                       </widget>
+                      </item>
+                      <item row="5" column="1">
+                       <widget class="QLabel" name="label_36">
+                        <property name="text">
+                         <string>Image decimation. RGB/Mono and depth images will be resized according to this value (size*1/decimation). Note that if depth images are captured, decimation should be a multiple of the depth image size.</string>
                         </property>
                         <property name="wordWrap">
                          <bool>true</bool>
@@ -1843,39 +1899,9 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                        </widget>
                       </item>
                       <item row="5" column="0">
-                       <layout class="QHBoxLayout" name="horizontalLayout_5" stretch="0,1">
-                        <item>
-                         <widget class="QToolButton" name="toolButton_source_path_calibration">
-                          <property name="text">
-                           <string>...</string>
-                          </property>
-                         </widget>
-                        </item>
-                        <item>
-                         <widget class="QLineEdit" name="lineEdit_calibrationFile">
-                          <property name="minimumSize">
-                           <size>
-                            <width>100</width>
-                            <height>0</height>
-                           </size>
-                          </property>
-                          <property name="text">
-                           <string/>
-                          </property>
-                         </widget>
-                        </item>
-                       </layout>
-                      </item>
-                      <item row="5" column="1">
-                       <widget class="QLabel" name="label_18">
-                        <property name="text">
-                         <string>Calibration file path (*.yaml). If empty, the GUID of the camera is used (for those having one). OpenNI and Freenect drivers use factory calibration by default (so they ignore this parameter). A calibrated camera is required for RGB-D SLAM mode.</string>
-                        </property>
-                        <property name="wordWrap">
-                         <bool>true</bool>
-                        </property>
-                        <property name="textInteractionFlags">
-                         <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                       <widget class="QSpinBox" name="spinBox_source_imageDecimation">
+                        <property name="minimum">
+                         <number>1</number>
                         </property>
                        </widget>
                       </item>
@@ -3299,7 +3325,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                       <property name="title">
                        <string>Directory of images (optional settings)</string>
                       </property>
-                      <layout class="QGridLayout" name="gridLayout_62" columnstretch="0,0,1">
+                      <layout class="QGridLayout" name="gridLayout_67" columnstretch="0,0,1">
                        <item row="0" column="1">
                         <widget class="QCheckBox" name="checkBox_cameraImages_timestamps">
                          <property name="text">
@@ -3428,7 +3454,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                        <item row="4" column="2">
                         <widget class="QLabel" name="label_293">
                          <property name="text">
-                          <string>Path to directory containing optional laser scans (*.pcd,*.ply). The directory should have the same size has the images directory. </string>
+                          <string>Path to directory containing optional laser scans (*.pcd, *.ply, *.bin [KITTI format]). The directory should have the same size has the images directory. </string>
                          </property>
                          <property name="wordWrap">
                           <bool>true</bool>
@@ -3441,7 +3467,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                        <item row="5" column="1">
                         <widget class="QLineEdit" name="lineEdit_cameraImages_laser_transform">
                          <property name="toolTip">
-                          <string>&lt;html&gt;&lt;head/&gt;&lt;body&gt;&lt;p&gt; Format (3 values): x y z&lt;/p&gt;&lt;p&gt; Format (6 values): x y z roll pitch yaw&lt;/p&gt;&lt;p&gt; Format (7 values): x y z qx qy qz qw&lt;/p&gt;&lt;p&gt; Format (9 values, 3x3 rotation): r11 r12 r13 r21 r22 r23 r31 r32 r33&lt;/p&gt;&lt;p&gt; Format (12 values, 3x4 transform): r11 r12 r13 tx r21 r22 r23 ty r31 r32 r33 tz&lt;/p&gt;&lt;/body&gt;&lt;/html&gt;</string>
+                          <string>&lt;html&gt;&lt;head/&gt;&lt;body&gt;&lt;p&gt;Format (3 values): x y z&lt;br/&gt;Format (6 values): x y z roll pitch yaw&lt;br/&gt;Format (7 values): x y z qx qy qz qw&lt;br/&gt;Format (9 values, 3x3 rotation): r11 r12 r13 r21 r22 r23 r31 r32 r33&lt;br/&gt;Format (12 values, 3x4 transform): r11 r12 r13 tx r21 r22 r23 ty r31 r32 r33 tz&lt;/p&gt;&lt;p&gt;KITTI: /base_link to /scan = -0.27 0 0.08 0 0 0&lt;/p&gt;&lt;/body&gt;&lt;/html&gt;</string>
                          </property>
                          <property name="text">
                           <string>0 0 0 0 0 0</string>
@@ -3463,6 +3489,9 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                        </item>
                        <item row="6" column="1">
                         <widget class="QSpinBox" name="spinBox_cameraImages_max_scan_pts">
+                         <property name="toolTip">
+                          <string>&lt;html&gt;&lt;head/&gt;&lt;body&gt;&lt;p&gt;KITTI: 130 000 points&lt;/p&gt;&lt;/body&gt;&lt;/html&gt;</string>
+                         </property>
                          <property name="maximum">
                           <number>99999999</number>
                          </property>
@@ -3485,12 +3514,12 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                      </widget>
                     </item>
                     <item>
-                     <widget class="QGroupBox" name="groupBox_scanFromDepth">
+                     <widget class="QGroupBox" name="groupBox_scan">
                       <property name="title">
-                       <string>Generate laser scan from depth image</string>
+                       <string>Laser scans</string>
                       </property>
                       <property name="checkable">
-                       <bool>true</bool>
+                       <bool>false</bool>
                       </property>
                       <property name="checked">
                        <bool>false</bool>
@@ -3499,7 +3528,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                        <item>
                         <widget class="QLabel" name="label_16">
                          <property name="text">
-                          <string>If you want to use ICP registration, the sensor data should have laser scan. This option can be used to create a laser scan from the depth image.</string>
+                          <string>If you want to use ICP registration, the sensor data should have laser scan. Laser scans can be created from the depth images (see option below) or loaded from the source selected. The latter parameters can be used to reduce the point cloud size directly in the capturing thread.</string>
                          </property>
                          <property name="wordWrap">
                           <bool>true</bool>
@@ -3507,34 +3536,88 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                         </widget>
                        </item>
                        <item>
-                        <layout class="QGridLayout" name="gridLayout_63" columnstretch="0,1">
-                         <item row="1" column="1">
-                          <widget class="QLabel" name="label_301">
-                           <property name="text">
-                            <string>Maximum depth.</string>
+                        <widget class="QGroupBox" name="groupBox_scanFromDepth">
+                         <property name="title">
+                          <string>Generate laser scan from depth image</string>
+                         </property>
+                         <property name="checkable">
+                          <bool>true</bool>
+                         </property>
+                         <property name="checked">
+                          <bool>false</bool>
+                         </property>
+                         <layout class="QGridLayout" name="gridLayout_63" columnstretch="0,1">
+                          <item row="0" column="0">
+                           <widget class="QSpinBox" name="spinBox_cameraScanFromDepth_decimation">
+                            <property name="maximum">
+                             <number>99999</number>
+                            </property>
+                            <property name="value">
+                             <number>1</number>
+                            </property>
+                           </widget>
+                          </item>
+                          <item row="0" column="1">
+                           <widget class="QLabel" name="label_299">
+                            <property name="text">
+                             <string>Decimation (should be a multiple of the image size). Note that it is done after general image decimation above.</string>
+                            </property>
+                            <property name="wordWrap">
+                             <bool>true</bool>
+                            </property>
+                            <property name="textInteractionFlags">
+                             <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                            </property>
+                           </widget>
+                          </item>
+                          <item row="1" column="0">
+                           <widget class="QDoubleSpinBox" name="doubleSpinBox_cameraSCanFromDepth_maxDepth">
+                            <property name="suffix">
+                             <string> m</string>
+                            </property>
+                            <property name="maximum">
+                             <double>9999.000000000000000</double>
+                            </property>
+                           </widget>
+                          </item>
+                          <item row="1" column="1">
+                           <widget class="QLabel" name="label_301">
+                            <property name="text">
+                             <string>Maximum depth.</string>
+                            </property>
+                            <property name="wordWrap">
+                             <bool>true</bool>
+                            </property>
+                            <property name="textInteractionFlags">
+                             <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                            </property>
+                           </widget>
+                          </item>
+                         </layout>
+                        </widget>
+                       </item>
+                       <item>
+                        <layout class="QGridLayout" name="gridLayout_62" columnstretch="0,1">
+                         <item row="2" column="0">
+                          <widget class="QSpinBox" name="spinBox_cameraImages_scanNormalsK">
+                           <property name="toolTip">
+                            <string>&lt;html&gt;&lt;head/&gt;&lt;body&gt;&lt;p&gt;KITTI: 130 000 points&lt;/p&gt;&lt;/body&gt;&lt;/html&gt;</string>
                            </property>
-                           <property name="wordWrap">
-                            <bool>true</bool>
+                           <property name="minimum">
+                            <number>0</number>
                            </property>
-                           <property name="textInteractionFlags">
-                            <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
-                           </property>
-                          </widget>
-                         </item>
-                         <item row="0" column="0">
-                          <widget class="QSpinBox" name="spinBox_cameraScanFromDepth_decimation">
                            <property name="maximum">
-                            <number>99999</number>
+                            <number>99999999</number>
                            </property>
                            <property name="value">
-                            <number>1</number>
+                            <number>0</number>
                            </property>
                           </widget>
                          </item>
                          <item row="0" column="1">
-                          <widget class="QLabel" name="label_299">
+                          <widget class="QLabel" name="label_300">
                            <property name="text">
-                            <string>Decimation (should be a multiple of the image width and height).</string>
+                            <string>Downsample step size for laser scans. If you laser scans are created from depth images, use Decimation above instead (faster).</string>
                            </property>
                            <property name="wordWrap">
                             <bool>true</bool>
@@ -3545,12 +3628,57 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                           </widget>
                          </item>
                          <item row="1" column="0">
-                          <widget class="QDoubleSpinBox" name="doubleSpinBox_cameraSCanFromDepth_maxDepth">
+                          <widget class="QDoubleSpinBox" name="doubleSpinBox_cameraImages_scanVoxelSize">
+                           <property name="toolTip">
+                            <string>&lt;html&gt;&lt;head/&gt;&lt;body&gt;&lt;p&gt;KITTI: 130 000 points&lt;/p&gt;&lt;/body&gt;&lt;/html&gt;</string>
+                           </property>
                            <property name="suffix">
                             <string> m</string>
                            </property>
+                           <property name="decimals">
+                            <number>3</number>
+                           </property>
+                           <property name="singleStep">
+                            <double>0.010000000000000</double>
+                           </property>
+                          </widget>
+                         </item>
+                         <item row="0" column="0">
+                          <widget class="QSpinBox" name="spinBox_cameraImages_scanDownsampleStep">
+                           <property name="toolTip">
+                            <string>&lt;html&gt;&lt;head/&gt;&lt;body&gt;&lt;p&gt;KITTI: 130 000 points&lt;/p&gt;&lt;/body&gt;&lt;/html&gt;</string>
+                           </property>
+                           <property name="minimum">
+                            <number>1</number>
+                           </property>
                            <property name="maximum">
-                            <double>9999.000000000000000</double>
+                            <number>99999999</number>
+                           </property>
+                          </widget>
+                         </item>
+                         <item row="1" column="1">
+                          <widget class="QLabel" name="label_302">
+                           <property name="text">
+                            <string>Voxel size for uniform sampling. Note that it is done after downsampling.</string>
+                           </property>
+                           <property name="wordWrap">
+                            <bool>true</bool>
+                           </property>
+                           <property name="textInteractionFlags">
+                            <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                           </property>
+                          </widget>
+                         </item>
+                         <item row="2" column="1">
+                          <widget class="QLabel" name="label_304">
+                           <property name="text">
+                            <string>K nearest neighbors for normals computation (0=disabled, 20 can be a good default value). Useful if the ICP registration approach is point to plane.</string>
+                           </property>
+                           <property name="wordWrap">
+                            <bool>true</bool>
+                           </property>
+                           <property name="textInteractionFlags">
+                            <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
                            </property>
                           </widget>
                          </item>
@@ -8811,7 +8939,7 @@ Lower the ratio -&gt; higher the precision. 0 means disabled, matching the neare
                       <item row="8" column="1">
                        <widget class="QLabel" name="label_212">
                         <property name="text">
-                         <string>Number of neighbors to compute normals for point to plane.</string>
+                         <string>Number of neighbors to compute normals for point to plane. Normals won't be recomputed if uniform sampling is disabled and that there are already normals in the laser scans.</string>
                         </property>
                         <property name="wordWrap">
                          <bool>true</bool>