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/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,