0.13.3: scan2d with normals support/registration

CMakeLists.txt

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

corelib/include/rtabmap/core/CameraRGB.h

@@ -85,7 +85,8 @@ public:
 			int maxScanPts = 0,
 			int downsampleStep = 1,
 			float voxelSize = 0.0f,
-			int normalsK = 0, // compute normals if > 0
+			int normalsK = 0,        // compute normals if > 0
+			float normalsRadius = 0, // compute normals if > 0
 			const Transform & localTransform=Transform::getIdentity())
 	{
 		_scanPath = dir;
@@ -93,6 +94,7 @@ public:
 		_scanMaxPts = maxScanPts;
 		_scanDownsampleStep = downsampleStep;
 		_scanNormalsK = normalsK;
+		_scanNormalsRadius = normalsRadius;
 		_scanVoxelSize = voxelSize;
 		if(_scanDownsampleStep>1)
 		{
@@ -158,6 +160,7 @@ private:
 	int _scanDownsampleStep;
 	float _scanVoxelSize;
 	int _scanNormalsK;
+	float _scanNormalsRadius;
 
 	bool _depthFromScan;
 	int _depthFromScanFillHoles; // <0:horizontal 0:disabled >0:vertical

corelib/include/rtabmap/core/CameraThread.h

@@ -73,13 +73,15 @@ public:
 			int decimation=4,
 			float maxDepth=4.0f,
 			float voxelSize = 0.0f,
-			int normalsK = 0)
+			int normalsK = 0,
+			int normalsRadius = 0.0f)
 	{
 		_scanFromDepth = enabled;
 		_scanDecimation=decimation;
 		_scanMaxDepth = maxDepth;
 		_scanVoxelSize = voxelSize;
 		_scanNormalsK = normalsK;
+		_scanNormalsRadius = normalsRadius;
 	}
 
 	void postUpdate(SensorData * data, CameraInfo * info = 0) const;
@@ -107,6 +109,7 @@ private:
 	float _scanMinDepth;
 	float _scanVoxelSize;
 	int _scanNormalsK;
+	float _scanNormalsRadius;
 	StereoDense * _stereoDense;
 	clams::DiscreteDepthDistortionModel * _distortionModel;
 	bool _bilateralFiltering;

corelib/include/rtabmap/core/Memory.h

@@ -283,6 +283,7 @@ private:
 	bool _compressionParallelized;
 	float _laserScanDownsampleStepSize;
 	int _laserScanNormalK;
+	int _laserScanNormalRadius;
 	bool _reextractLoopClosureFeatures;
 	float _rehearsalMaxDistance;
 	float _rehearsalMaxAngle;

corelib/include/rtabmap/core/Parameters.h

@@ -211,7 +211,8 @@ class RTABMAP_EXP Parameters
     RTABMAP_PARAM(Mem, ImagePostDecimation,         int, 1,         "Image decimation (>=1) of saved data in created signatures (after features extraction). Decimation is done from the original image. Negative decimation is done from RGB size instead of depth size (if depth is smaller than RGB, it may be interpolated depending of the decimation value).");
     RTABMAP_PARAM(Mem, CompressionParallelized,     bool, true,     "Compression of sensor data is multi-threaded.");
     RTABMAP_PARAM(Mem, LaserScanDownsampleStepSize, int, 1,         "If > 1, downsample the laser scans when creating a signature.");
-    RTABMAP_PARAM(Mem, LaserScanNormalK,            int, 0,         "If > 0 and laser scans are 3D without normals, normals will be computed with K search neighbors when creating a signature.");
+    RTABMAP_PARAM(Mem, LaserScanNormalK,            int, 0,         "If > 0 and laser scans don't have normals, normals will be computed with K search neighbors when creating a signature.");
+    RTABMAP_PARAM(Mem, LaserScanNormalRadius,       int, 0,         "If > 0 and laser scans don't have normals, normals will be computed with radius search neighbors when creating a signature.");
     RTABMAP_PARAM(Mem, UseOdomFeatures,             bool, false,    "Use odometry features.");
 
     // KeypointMemory (Keypoint-based)
@@ -510,7 +511,8 @@ class RTABMAP_EXP Parameters
     RTABMAP_PARAM(Icp, Epsilon,                   float, 0,     "Set the transformation epsilon (maximum allowable difference between two consecutive transformations) in order for an optimization to be considered as having converged to the final solution.");
     RTABMAP_PARAM(Icp, CorrespondenceRatio,       float, 0.2,   "Ratio of matching correspondences to accept the transform.");
     RTABMAP_PARAM(Icp, PointToPlane,              bool, false,  "Use point to plane ICP.");
-    RTABMAP_PARAM(Icp, PointToPlaneNormalNeighbors, int, 20,    "Number of neighbors to compute normals for point to plane.");
+    RTABMAP_PARAM(Icp, PointToPlaneK,             int, 20,      "Number of neighbors to compute normals for point to plane if the cloud doesn't have already normals.");
+    RTABMAP_PARAM(Icp, PointToPlaneRadius,        float, 0.0,   "Search radius to compute normals for point to plane if the cloud doesn't have already normals.");
 
     // libpointmatcher
     RTABMAP_PARAM(Icp, PM,                       bool, false,   "Use libpointmatcher for ICP registration instead of PCL's implementation.");

corelib/include/rtabmap/core/RegistrationIcp.h

@@ -64,7 +64,8 @@ private:
 	float _epsilon;
 	float _correspondenceRatio;
 	bool _pointToPlane;
-	int _pointToPlaneNormalNeighbors;
+	int _pointToPlaneK;
+	float _pointToPlaneRadius;
 	bool _libpointmatcher;
 	std::string _libpointmatcherConfig;
 	float _libpointmatcherOutlierRatio;

corelib/include/rtabmap/core/util3d.h

@@ -198,6 +198,8 @@ cv::Mat RTABMAP_EXP laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZ>
 // return CV_32FC4 (x,y,z,rgb)
 cv::Mat RTABMAP_EXP laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZRGB> & cloud, const Transform & transform = Transform());
 // return CV_32FC7 (x,y,z,rgb,normal_z,normal_y,normalz)
+cv::Mat RTABMAP_EXP laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZRGB> & cloud, const pcl::PointCloud<pcl::Normal> & normals, const Transform & transform = Transform());
+// return CV_32FC7 (x,y,z,rgb,normal_z,normal_y,normalz)
 cv::Mat RTABMAP_EXP laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZRGBNormal> & cloud, const Transform & transform = Transform());
 // return CV_32FC2  (x,y)
 cv::Mat RTABMAP_EXP laserScan2dFromPointCloud(const pcl::PointCloud<pcl::PointXYZ> & cloud, const Transform & transform = Transform());

corelib/include/rtabmap/core/util3d_filtering.h

@@ -141,6 +141,12 @@ pcl::PointCloud<pcl::PointNormal>::Ptr RTABMAP_EXP passThrough(
 		float min,
 		float max,
 		bool negative = false);
+pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr RTABMAP_EXP passThrough(
+		const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud,
+		const std::string & axis,
+		float min,
+		float max,
+		bool negative = false);
 
 pcl::IndicesPtr RTABMAP_EXP cropBox(
 		const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,

corelib/include/rtabmap/core/util3d_surface.h

@@ -212,32 +212,50 @@ cv::Mat RTABMAP_EXP mergeTextures(
 		bool exposureFusion = false,         //Exposure fusion can be used only with OpenCV3
 		const ProgressState * state = 0);
 
-
+cv::Mat RTABMAP_EXP computeNormals(
+		const cv::Mat & laserScan,
+		int searchK,
+		float searchRadius);
 pcl::PointCloud<pcl::Normal>::Ptr RTABMAP_EXP computeNormals(
 		const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
-		int normalKSearch = 20,
+		int searchK = 20,
+		float searchRadius = 0.0f,
 		const Eigen::Vector3f & viewPoint = Eigen::Vector3f(0,0,0));
 pcl::PointCloud<pcl::Normal>::Ptr RTABMAP_EXP computeNormals(
 		const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
-		int normalKSearch = 20,
+		int searchK = 20,
+		float searchRadius = 0.0f,
 		const Eigen::Vector3f & viewPoint = Eigen::Vector3f(0,0,0));
 pcl::PointCloud<pcl::Normal>::Ptr RTABMAP_EXP computeNormals(
 		const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
 		const pcl::IndicesPtr & indices,
-		int normalKSearch = 20,
+		int searchK = 20,
+		float searchRadius = 0.0f,
 		const Eigen::Vector3f & viewPoint = Eigen::Vector3f(0,0,0));
 pcl::PointCloud<pcl::Normal>::Ptr RTABMAP_EXP computeNormals(
 		const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
 		const pcl::IndicesPtr & indices,
-		int normalKSearch = 20,
+		int searchK = 20,
+		float searchRadius = 0.0f,
 		const Eigen::Vector3f & viewPoint = Eigen::Vector3f(0,0,0));
 
-pcl::PointCloud<pcl::Normal>::Ptr computeFastOrganizedNormals(
+pcl::PointCloud<pcl::Normal>::Ptr RTABMAP_EXP computeNormals2D(
+		const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
+		int searchK = 5,
+		float searchRadius = 0.0f,
+		const Eigen::Vector3f & viewPoint = Eigen::Vector3f(0,0,0));
+pcl::PointCloud<pcl::Normal>::Ptr RTABMAP_EXP computeFastOrganizedNormals2D(
+		const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
+		int searchK = 5,
+		float searchRadius = 0.0f,
+		const Eigen::Vector3f & viewPoint = Eigen::Vector3f(0,0,0));
+
+pcl::PointCloud<pcl::Normal>::Ptr RTABMAP_EXP computeFastOrganizedNormals(
 		const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
 		float maxDepthChangeFactor = 0.02f,
 		float normalSmoothingSize = 10.0f,
 		const Eigen::Vector3f & viewPoint = Eigen::Vector3f(0,0,0));
-pcl::PointCloud<pcl::Normal>::Ptr computeFastOrganizedNormals(
+pcl::PointCloud<pcl::Normal>::Ptr RTABMAP_EXP computeFastOrganizedNormals(
 		const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
 		const pcl::IndicesPtr & indices,
 		float maxDepthChangeFactor = 0.02f,

corelib/src/CameraRGB.cpp

@@ -70,6 +70,7 @@ CameraImages::CameraImages() :
 		_scanDownsampleStep(1),
 		_scanVoxelSize(0.0f),
 		_scanNormalsK(0),
+		_scanNormalsRadius(0),
 		_depthFromScan(false),
 		_depthFromScanFillHoles(1),
 		_depthFromScanFillHolesFromBorder(false),
@@ -99,6 +100,7 @@ CameraImages::CameraImages(const std::string & path,
 	_scanDownsampleStep(1),
 	_scanVoxelSize(0.0f),
 	_scanNormalsK(0),
+	_scanNormalsRadius(0),
 	_depthFromScan(false),
 	_depthFromScanFillHoles(1),
 	_depthFromScanFillHolesFromBorder(false),
@@ -685,9 +687,9 @@ SensorData CameraImages::captureImage(CameraInfo * info)
 				cloud = util3d::voxelize(cloud, _scanVoxelSize);
 				UDEBUG("Voxel filtering scan (voxel=%f m): %d -> %d", _scanVoxelSize, previousSize, (int)cloud->size());
 			}
-			if(_scanNormalsK > 0 && cloud->size())
+			if((_scanNormalsK > 0 || _scanNormalsRadius) && cloud->size())
 			{
-				pcl::PointCloud<pcl::Normal>::Ptr normals = util3d::computeNormals(cloud, _scanNormalsK);
+				pcl::PointCloud<pcl::Normal>::Ptr normals = util3d::computeNormals(cloud, _scanNormalsK, _scanNormalsRadius);
 				pcl::PointCloud<pcl::PointNormal>::Ptr cloudNormals(new pcl::PointCloud<pcl::PointNormal>);
 				pcl::concatenateFields(*cloud, *normals, *cloudNormals);
 				scan = util3d::laserScanFromPointCloud(*cloudNormals, _scanLocalTransform.inverse());

corelib/src/CameraThread.cpp

@@ -58,6 +58,7 @@ CameraThread::CameraThread(Camera * camera, const ParametersMap & parameters) :
 		_scanMinDepth(0.0f),
 		_scanVoxelSize(0.0f),
 		_scanNormalsK(0),
+		_scanNormalsRadius(0.0f),
 		_stereoDense(new StereoBM(parameters)),
 		_distortionModel(0),
 		_bilateralFiltering(false),
@@ -323,10 +324,10 @@ void CameraThread::postUpdate(SensorData * dataPtr, CameraInfo * info) const
 
 				if(cloud->size())
 				{
-					if(_scanNormalsK>0)
+					if(_scanNormalsK>0 || _scanNormalsRadius>0.0f)
 					{
 						Eigen::Vector3f viewPoint(baseToScan.x(), baseToScan.y(), baseToScan.z());
-						pcl::PointCloud<pcl::Normal>::Ptr normals = util3d::computeNormals(cloud, _scanNormalsK, viewPoint);
+						pcl::PointCloud<pcl::Normal>::Ptr normals = util3d::computeNormals(cloud, _scanNormalsK, _scanNormalsRadius, viewPoint);
 						pcl::PointCloud<pcl::PointNormal>::Ptr cloudNormals(new pcl::PointCloud<pcl::PointNormal>);
 						pcl::concatenateFields(*cloud, *normals, *cloudNormals);
 						scan = util3d::laserScanFromPointCloud(*cloudNormals, baseToScan.inverse());

corelib/src/Memory.cpp

@@ -89,6 +89,7 @@ Memory::Memory(const ParametersMap & parameters) :
 	_compressionParallelized(Parameters::defaultMemCompressionParallelized()),
 	_laserScanDownsampleStepSize(Parameters::defaultMemLaserScanDownsampleStepSize()),
 	_laserScanNormalK(Parameters::defaultMemLaserScanNormalK()),
+	_laserScanNormalRadius(Parameters::defaultMemLaserScanNormalRadius()),
 	_reextractLoopClosureFeatures(Parameters::defaultRGBDLoopClosureReextractFeatures()),
 	_rehearsalMaxDistance(Parameters::defaultRGBDLinearUpdate()),
 	_rehearsalMaxAngle(Parameters::defaultRGBDAngularUpdate()),
@@ -440,6 +441,7 @@ void Memory::parseParameters(const ParametersMap & parameters)
 	Parameters::parse(parameters, Parameters::kMemCompressionParallelized(), _compressionParallelized);
 	Parameters::parse(parameters, Parameters::kMemLaserScanDownsampleStepSize(), _laserScanDownsampleStepSize);
 	Parameters::parse(parameters, Parameters::kMemLaserScanNormalK(), _laserScanNormalK);
+	Parameters::parse(parameters, Parameters::kMemLaserScanNormalRadius(), _laserScanNormalRadius);
 	Parameters::parse(parameters, Parameters::kRGBDLoopClosureReextractFeatures(), _reextractLoopClosureFeatures);
 	Parameters::parse(parameters, Parameters::kRGBDLinearUpdate(), _rehearsalMaxDistance);
 	Parameters::parse(parameters, Parameters::kRGBDAngularUpdate(), _rehearsalMaxAngle);
@@ -3718,13 +3720,12 @@ Signature * Memory::createSignature(const SensorData & data, const Transform & p
 		if(stats) stats->addStatistic(Statistics::kTimingMemScan_downsampling(), t*1000.0f);
 		UDEBUG("time downsampling scan = %fs", t);
 	}
-	if(!laserScan.empty() && _laserScanNormalK > 0 && laserScan.channels() == 3 && !isIntermediateNode)
+	if(!laserScan.empty() &&
+		(_laserScanNormalK > 0 || _laserScanNormalRadius>0.0f) &&
+		laserScan.channels() > 1 && laserScan.channels() < 5 &&
+		!isIntermediateNode)
 	{
-		pcl::PointCloud<pcl::PointXYZ>::Ptr cloud = util3d::laserScanToPointCloud(laserScan);
+		laserScan = util3d::computeNormals(laserScan, _laserScanNormalK, _laserScanNormalRadius);
-		float x,y,z;
-		data.laserScanInfo().localTransform().getTranslation(x,y,z);
-		pcl::PointCloud<pcl::Normal>::Ptr normals = util3d::computeNormals(cloud, _laserScanNormalK, Eigen::Vector3f(x,y,z));
-		laserScan = util3d::laserScanFromPointCloud(*cloud, *normals);
 		t = timer.ticks();
 		if(stats) stats->addStatistic(Statistics::kTimingMemScan_normals(), t*1000.0f);
 		UDEBUG("time normals scan = %fs", t);

corelib/src/Parameters.cpp

@@ -225,6 +225,10 @@ const std::map<std::string, std::pair<bool, std::string> > & Parameters::getRemo
 	{
 		// removed parameters
 
+		// 0.13.3
+		removedParameters_.insert(std::make_pair("Icp/PointToPlaneNormalNeighbors", std::make_pair(true,  Parameters::kIcpPointToPlaneK())));
+
+
 		// 0.13.1
 		removedParameters_.insert(std::make_pair("Rtabmap/VhStrategy",            std::make_pair(true,  Parameters::kVhEpEnabled())));
 
@@ -326,7 +330,7 @@ const std::map<std::string, std::pair<bool, std::string> > & Parameters::getRemo
 		removedParameters_.insert(std::make_pair("LccIcp3/Iterations",                  std::make_pair(false, Parameters::kIcpIterations())));
 		removedParameters_.insert(std::make_pair("LccIcp3/CorrespondenceRatio",         std::make_pair(false, Parameters::kIcpCorrespondenceRatio())));
 		removedParameters_.insert(std::make_pair("LccIcp3/PointToPlane",                std::make_pair(true,  Parameters::kIcpPointToPlane())));
-		removedParameters_.insert(std::make_pair("LccIcp3/PointToPlaneNormalNeighbors", std::make_pair(true,  Parameters::kIcpPointToPlaneNormalNeighbors())));
+		removedParameters_.insert(std::make_pair("LccIcp3/PointToPlaneNormalNeighbors", std::make_pair(true,  Parameters::kIcpPointToPlaneK())));
 
 		removedParameters_.insert(std::make_pair("LccIcp2/MaxCorrespondenceDistance",   std::make_pair(true,  Parameters::kIcpMaxCorrespondenceDistance())));
 		removedParameters_.insert(std::make_pair("LccIcp2/Iterations",                  std::make_pair(true,  Parameters::kIcpIterations())));

corelib/src/RegistrationIcp.cpp

@@ -36,8 +36,6 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <rtabmap/utilite/UConversion.h>
 #include <rtabmap/utilite/UMath.h>
 #include <rtabmap/utilite/UTimer.h>
-#include <pcl/io/pcd_io.h>
-#include <pcl/io/vtk_io.h>
 #include <pcl/conversions.h>
 
 #ifdef RTABMAP_POINTMATCHER
@@ -225,7 +223,8 @@ RegistrationIcp::RegistrationIcp(const ParametersMap & parameters, Registration
 	_epsilon(Parameters::defaultIcpEpsilon()),
 	_correspondenceRatio(Parameters::defaultIcpCorrespondenceRatio()),
 	_pointToPlane(Parameters::defaultIcpPointToPlane()),
-	_pointToPlaneNormalNeighbors(Parameters::defaultIcpPointToPlaneNormalNeighbors()),
+	_pointToPlaneK(Parameters::defaultIcpPointToPlaneK()),
+	_pointToPlaneRadius(Parameters::defaultIcpPointToPlaneRadius()),
 	_libpointmatcher(Parameters::defaultIcpPM()),
 	_libpointmatcherConfig(Parameters::defaultIcpPMConfig()),
 	_libpointmatcherOutlierRatio(Parameters::defaultIcpPMOutlierRatio()),
@@ -257,7 +256,8 @@ void RegistrationIcp::parseParameters(const ParametersMap & parameters)
 	Parameters::parse(parameters, Parameters::kIcpEpsilon(), _epsilon);
 	Parameters::parse(parameters, Parameters::kIcpCorrespondenceRatio(), _correspondenceRatio);
 	Parameters::parse(parameters, Parameters::kIcpPointToPlane(), _pointToPlane);
-	Parameters::parse(parameters, Parameters::kIcpPointToPlaneNormalNeighbors(), _pointToPlaneNormalNeighbors);
+	Parameters::parse(parameters, Parameters::kIcpPointToPlaneK(), _pointToPlaneK);
+	Parameters::parse(parameters, Parameters::kIcpPointToPlaneRadius(), _pointToPlaneRadius);
 
 	Parameters::parse(parameters, Parameters::kIcpPM(), _libpointmatcher);
 	Parameters::parse(parameters, Parameters::kIcpPMConfig(), _libpointmatcherConfig);
@@ -342,7 +342,7 @@ void RegistrationIcp::parseParameters(const ParametersMap & parameters)
 	UASSERT_MSG(_maxIterations > 0, uFormat("value=%d", _maxIterations).c_str());
 	UASSERT(_epsilon >= 0.0f);
 	UASSERT_MSG(_correspondenceRatio >=0.0f && _correspondenceRatio <=1.0f, uFormat("value=%f", _correspondenceRatio).c_str());
-	UASSERT_MSG(_pointToPlaneNormalNeighbors > 0, uFormat("value=%d", _pointToPlaneNormalNeighbors).c_str());
+	UASSERT_MSG(!_pointToPlane || (_pointToPlane && (_pointToPlaneK > 0 || _pointToPlaneRadius > 0.0f)), uFormat("_pointToPlaneK=%d _pointToPlaneRadius=%f", _pointToPlaneK, _pointToPlaneRadius).c_str());
 }
 
 Transform RegistrationIcp::computeTransformationImpl(
@@ -354,7 +354,8 @@ Transform RegistrationIcp::computeTransformationImpl(
 	UDEBUG("Guess transform = %s", guess.prettyPrint().c_str());
 	UDEBUG("Voxel size=%f", _voxelSize);
 	UDEBUG("PointToPlane=%d", _pointToPlane?1:0);
-	UDEBUG("Normal neighborhood=%d", _pointToPlaneNormalNeighbors);
+	UDEBUG("Normal neighborhood=%d", _pointToPlaneK);
+	UDEBUG("Normal radius=%d", _pointToPlaneRadius);
 	UDEBUG("Max correspondence distance=%f", _maxCorrespondenceDistance);
 	UDEBUG("Max Iterations=%d", _maxIterations);
 	UDEBUG("Correspondence Ratio=%f", _correspondenceRatio);
@@ -406,8 +407,8 @@ Transform RegistrationIcp::computeTransformationImpl(
 
 			if( _pointToPlane &&
 				_voxelSize == 0.0f &&
-				fromScan.channels() == 6 &&
+				fromScan.channels() >= 6 &&
-				toScan.channels() == 6)
+				toScan.channels() >= 6)
 			{
 				//special case if we have already normals computed and there is no filtering
 				pcl::PointCloud<pcl::PointNormal>::Ptr fromCloudNormals = util3d::laserScanToPointCloudNormal(fromScan, fromLocalTransform);
@@ -450,6 +451,9 @@ Transform RegistrationIcp::computeTransformationImpl(
 				else
 #endif
 				{
+					fromCloudNormals = util3d::removeNaNNormalsFromPointCloud(fromCloudNormals);
+					toCloudNormals = util3d::removeNaNNormalsFromPointCloud(toCloudNormals);
+
 					icpT = util3d::icpPointToPlane(
 							fromCloudNormals,
 							toCloudNormals,
@@ -497,15 +501,41 @@ Transform RegistrationIcp::computeTransformationImpl(
 				}
 
 				pcl::PointCloud<pcl::PointXYZ>::Ptr fromCloudRegistered(new pcl::PointCloud<pcl::PointXYZ>());
-				if(_pointToPlane) // ICP Point To Plane, only in 3D
+				if(_pointToPlane && // ICP Point To Plane
+					!((fromScan.channels() == 2 || toScan.channels() == 2) && !_libpointmatcher)) // PCL crashes if 2D
 				{
 					pcl::PointCloud<pcl::Normal>::Ptr normals;
+					Eigen::Vector3f viewpointFrom(fromLocalTransform.x(), fromLocalTransform.y(), fromLocalTransform.z());
+					Transform toT = guess * toLocalTransform;
+					Eigen::Vector3f viewpointTo(toT.x(), toT.y(), toT.z());
 
-					normals = util3d::computeNormals(fromCloudFiltered, _pointToPlaneNormalNeighbors);
+					if(fromScan.channels() == 2)
+					{
+						normals = util3d::computeFastOrganizedNormals2D(
+								fromCloudFiltered,
+								_pointToPlaneK,
+								_pointToPlaneRadius,
+								viewpointFrom);
+					}
+					else
+					{
+						normals = util3d::computeNormals(fromCloudFiltered, _pointToPlaneK, _pointToPlaneRadius, viewpointFrom);
+					}
 					pcl::PointCloud<pcl::PointNormal>::Ptr fromCloudNormals(new pcl::PointCloud<pcl::PointNormal>);
 					pcl::concatenateFields(*fromCloudFiltered, *normals, *fromCloudNormals);
 
-					normals = util3d::computeNormals(toCloudFiltered, _pointToPlaneNormalNeighbors);
+					if(toScan.channels() == 2)
+					{
+						normals = util3d::computeFastOrganizedNormals2D(
+								toCloudFiltered,
+								_pointToPlaneK,
+								_pointToPlaneRadius,
+								viewpointTo);
+					}
+					else
+					{
+						normals = util3d::computeNormals(toCloudFiltered, _pointToPlaneK, _pointToPlaneRadius, viewpointTo);
+					}
 					pcl::PointCloud<pcl::PointNormal>::Ptr toCloudNormals(new pcl::PointCloud<pcl::PointNormal>);
 					pcl::concatenateFields(*toCloudFiltered, *normals, *toCloudNormals);
 
@@ -517,7 +547,7 @@ Transform RegistrationIcp::computeTransformationImpl(
 					fromSignature.sensorData().setLaserScanRaw(util3d::laserScanFromPointCloud(*fromCloudNormals, fromLocalTransform.inverse()), LaserScanInfo(maxLaserScansFrom, fromSignature.sensorData().laserScanInfo().maxRange(), fromLocalTransform));
 					toSignature.sensorData().setLaserScanRaw(util3d::laserScanFromPointCloud(*toCloudNormals, (guess*toLocalTransform).inverse()), LaserScanInfo(maxLaserScansTo, toSignature.sensorData().laserScanInfo().maxRange(), toLocalTransform));
 
-					UDEBUG("Compute normals time = %f s", timer.ticks());
+					UDEBUG("Compute normals (%d,%d) time = %f s", (int)fromCloudNormals->size(), (int)toCloudNormals->size(), timer.ticks());
 
 					if(toCloudNormals->size() && fromCloudNormals->size())
 					{
@@ -569,7 +599,6 @@ Transform RegistrationIcp::computeTransformationImpl(
 								   this->force3DoF());
 						}
 
-
 						if(!icpT.isNull() && hasConverged)
 						{
 							util3d::computeVarianceAndCorrespondences(
@@ -583,6 +612,11 @@ Transform RegistrationIcp::computeTransformationImpl(
 				}
 				else // ICP Point to Point
 				{
+					if(_pointToPlane && ((fromScan.channels() == 2 || toScan.channels() == 2) && !_libpointmatcher))
+					{
+						UWARN("ICP PointToPlane ignored for 2d scans with PCL registration (some crash issues). Use libpointmatcher (%s) or disable %s to avoid this warning.", Parameters::kIcpPM().c_str(), Parameters::kIcpPointToPlane().c_str());
+					}
+
 					if(_voxelSize > 0.0f)
 					{
 						// update output scans

corelib/src/util3d.cpp

@@ -1363,6 +1363,46 @@ cv::Mat laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZRGB> & cloud,
 	return laserScan;
 }
 
+cv::Mat laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZRGB> & cloud, const pcl::PointCloud<pcl::Normal> & normals, const Transform & transform)
+{
+	UASSERT(cloud.size() == normals.size());
+	cv::Mat laserScan(1, (int)cloud.size(), CV_32FC(7));
+	bool nullTransform = transform.isNull() || transform.isIdentity();
+	for(unsigned int i=0; i<cloud.size(); ++i)
+	{
+		float * ptr = laserScan.ptr<float>(0, i);
+		if(!nullTransform)
+		{
+			pcl::PointXYZRGBNormal pt;
+			pt.x = cloud.at(i).x;
+			pt.y = cloud.at(i).y;
+			pt.z = cloud.at(i).z;
+			pt.normal_x = normals.at(i).normal_x;
+			pt.normal_y = normals.at(i).normal_y;
+			pt.normal_z = normals.at(i).normal_z;
+			pt = util3d::transformPoint(pt, transform);
+			ptr[0] = pt.x;
+			ptr[1] = pt.y;
+			ptr[2] = pt.z;
+			ptr[4] = pt.normal_x;
+			ptr[5] = pt.normal_y;
+			ptr[6] = pt.normal_z;
+		}
+		else
+		{
+			ptr[0] = cloud.at(i).x;
+			ptr[1] = cloud.at(i).y;
+			ptr[2] = cloud.at(i).z;
+			ptr[4] = normals.at(i).normal_x;
+			ptr[5] = normals.at(i).normal_y;
+			ptr[6] = normals.at(i).normal_z;
+		}
+		int * ptrInt = (int*)ptr;
+		ptrInt[3] = int(cloud.at(i).b) | (int(cloud.at(i).g) << 8) | (int(cloud.at(i).r) << 16);
+	}
+	return laserScan;
+}
+
 cv::Mat laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZRGBNormal> & cloud, const Transform & transform)
 {
 	cv::Mat laserScan(1, (int)cloud.size(), CV_32FC(7));

corelib/src/util3d_filtering.cpp

@@ -369,6 +369,26 @@ pcl::PointCloud<pcl::PointNormal>::Ptr passThrough(
 	return output;
 }
 
+pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr passThrough(
+		const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud,
+		const std::string & axis,
+		float min,
+		float max,
+		bool negative)
+{
+	UASSERT_MSG(max > min, uFormat("cloud=%d, max=%f min=%f axis=%s", (int)cloud->size(), max, min, axis.c_str()).c_str());
+	UASSERT(axis.compare("x") == 0 || axis.compare("y") == 0 || axis.compare("z") == 0);
+
+	pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr output(new pcl::PointCloud<pcl::PointXYZRGBNormal>);
+	pcl::PassThrough<pcl::PointXYZRGBNormal> filter;
+	filter.setNegative(negative);
+	filter.setFilterFieldName(axis);
+	filter.setFilterLimits(min, max);
+	filter.setInputCloud(cloud);
+	filter.filter(*output);
+	return output;
+}
+
 pcl::IndicesPtr cropBox(
 		const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
 		const pcl::IndicesPtr & indices,

corelib/src/util3d_surface.cpp

@@ -1994,18 +1994,51 @@ cv::Mat mergeTextures(
 	return globalTextures;
 }
 
+cv::Mat computeNormals(
+		const cv::Mat & laserScan,
+		int searchK,
+		float searchRadius)
+{
+	if(laserScan.empty() || laserScan.channels()<2 || laserScan.channels()>4)
+	{
+		return laserScan;
+	}
+
+	pcl::PointCloud<pcl::Normal>::Ptr normals;
+	if(laserScan.channels() < 4)
+	{
+		pcl::PointCloud<pcl::PointXYZ>::Ptr cloud = util3d::laserScanToPointCloud(laserScan);
+		if(laserScan.channels() == 2)
+		{
+			normals = util3d::computeNormals2D(cloud, searchK, searchRadius);
+		}
+		else
+		{
+			normals = util3d::computeNormals(cloud, searchK, searchRadius);
+		}
+		return util3d::laserScanFromPointCloud(*cloud, *normals);
+	}
+	else // 4 channels
+	{
+		pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud = util3d::laserScanToPointCloudRGB(laserScan);
+		normals = util3d::computeNormals(cloud, searchK, searchRadius);
+		return util3d::laserScanFromPointCloud(*cloud, *normals);
+	}
+}
 pcl::PointCloud<pcl::Normal>::Ptr computeNormals(
 		const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
-		int normalKSearch,
+		int searchK,
+		float searchRadius,
 		const Eigen::Vector3f & viewPoint)
 {
 	pcl::IndicesPtr indices(new std::vector<int>);
-	return computeNormals(cloud, indices, normalKSearch, viewPoint);
+	return computeNormals(cloud, indices, searchK, searchRadius, viewPoint);
 }
 pcl::PointCloud<pcl::Normal>::Ptr computeNormals(
 		const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
 		const pcl::IndicesPtr & indices,
-		int normalKSearch,
+		int searchK,
+		float searchRadius,
 		const Eigen::Vector3f & viewPoint)
 {
 	pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ>);
@@ -2032,7 +2065,8 @@ pcl::PointCloud<pcl::Normal>::Ptr computeNormals(
 	//	n.setIndices(indices);
 	//}
 	n.setSearchMethod (tree);
-	n.setKSearch (normalKSearch);
+	n.setKSearch (searchK);
+	n.setRadiusSearch (searchRadius);
 	n.setViewPoint(viewPoint[0], viewPoint[1], viewPoint[2]);
 	n.compute (*normals);
 
@@ -2041,16 +2075,18 @@ pcl::PointCloud<pcl::Normal>::Ptr computeNormals(
 
 pcl::PointCloud<pcl::Normal>::Ptr computeNormals(
 		const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
-		int normalKSearch,
+		int searchK,
+		float searchRadius,
 		const Eigen::Vector3f & viewPoint)
 {
 	pcl::IndicesPtr indices(new std::vector<int>);
-	return computeNormals(cloud, indices, normalKSearch, viewPoint);
+	return computeNormals(cloud, indices, searchK, searchRadius, viewPoint);
 }
 pcl::PointCloud<pcl::Normal>::Ptr computeNormals(
 		const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
 		const pcl::IndicesPtr & indices,
-		int normalKSearch,
+		int searchK,
+		float searchRadius,
 		const Eigen::Vector3f & viewPoint)
 {
 	pcl::search::KdTree<pcl::PointXYZRGB>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZRGB>);
@@ -2077,13 +2113,182 @@ pcl::PointCloud<pcl::Normal>::Ptr computeNormals(
 	//	n.setIndices(indices);
 	//}
 	n.setSearchMethod (tree);
-	n.setKSearch (normalKSearch);
+	n.setKSearch (searchK);
+	n.setRadiusSearch(searchRadius);
 	n.setViewPoint(viewPoint[0], viewPoint[1], viewPoint[2]);
 	n.compute (*normals);
 
 	return normals;
 }
 
+pcl::PointCloud<pcl::Normal>::Ptr computeNormals2D(
+		const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
+		int searchK,
+		float searchRadius,
+		const Eigen::Vector3f & viewPoint)
+{
+	UASSERT(searchK>0 || searchRadius>0.0f);
+	pcl::PointCloud<pcl::Normal>::Ptr normals (new pcl::PointCloud<pcl::Normal>);
+
+	pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ>);
+	tree->setInputCloud (cloud);
+
+	normals->resize(cloud->size());
+
+	float bad_point = std::numeric_limits<float>::quiet_NaN ();
+
+	// assuming that points are ordered
+	for(unsigned int i=0; i<cloud->size(); ++i)
+	{
+		const pcl::PointXYZ & pt = cloud->at(i);
+		std::vector<Eigen::Vector3f> neighborNormals;
+		Eigen::Vector3f direction;
+		direction[0] = viewPoint[0] - pt.x;
+		direction[1] = viewPoint[1] - pt.y;
+		direction[2] = viewPoint[2] - pt.z;
+
+		std::vector<int> k_indices;
+		std::vector<float> k_sqr_distances;
+		if(searchRadius>0.0f)
+		{
+			tree->radiusSearch(cloud->at(i), searchRadius, k_indices, k_sqr_distances, searchK);
+		}
+		else
+		{
+			tree->nearestKSearch(cloud->at(i), searchK, k_indices, k_sqr_distances);
+		}
+
+		for(unsigned int j=0; j<k_indices.size(); ++j)
+		{
+			if(k_indices.at(j) != (int)i)
+			{
+				const pcl::PointXYZ & pt2 = cloud->at(k_indices.at(j));
+				Eigen::Vector3f v(pt2.x-pt.x, pt2.y - pt.y, pt2.z - pt.z);
+				Eigen::Vector3f up = v.cross(direction);
+				Eigen::Vector3f n = up.cross(v);
+				n.normalize();
+				neighborNormals.push_back(n);
+			}
+		}
+
+		if(neighborNormals.empty())
+		{
+			normals->at(i).normal_x = bad_point;
+			normals->at(i).normal_y = bad_point;
+			normals->at(i).normal_z = bad_point;
+		}
+		else
+		{
+			Eigen::Vector3f meanNormal(0,0,0);
+			for(unsigned int j=0; j<neighborNormals.size(); ++j)
+			{
+				meanNormal+=neighborNormals[j];
+			}
+			meanNormal /= (float)neighborNormals.size();
+			meanNormal.normalize();
+			normals->at(i).normal_x = meanNormal[0];
+			normals->at(i).normal_y = meanNormal[1];
+			normals->at(i).normal_z = meanNormal[2];
+		}
+	}
+
+	return normals;
+}
+
+pcl::PointCloud<pcl::Normal>::Ptr computeFastOrganizedNormals2D(
+		const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
+		int searchK,
+		float searchRadius,
+		const Eigen::Vector3f & viewPoint)
+{
+	UASSERT(searchK>0);
+	pcl::PointCloud<pcl::Normal>::Ptr normals (new pcl::PointCloud<pcl::Normal>);
+
+	normals->resize(cloud->size());
+	searchRadius *= searchRadius; // squared distance
+
+	float bad_point = std::numeric_limits<float>::quiet_NaN ();
+
+	// assuming that points are ordered
+	for(int i=0; i<(int)cloud->size(); ++i)
+	{
+		int li = i-searchK;
+		if(li<0)
+		{
+			li=0;
+		}
+		int hi = i+searchK;
+		if(hi>=(int)cloud->size())
+		{
+			hi=(int)cloud->size()-1;
+		}
+
+		// get points before not too far
+		const pcl::PointXYZ & pt = cloud->at(i);
+		std::vector<Eigen::Vector3f> neighborNormals;
+		Eigen::Vector3f direction;
+		direction[0] = viewPoint[0] - cloud->at(i).x;
+		direction[1] = viewPoint[1] - cloud->at(i).y;
+		direction[2] = viewPoint[2] - cloud->at(i).z;
+		for(int j=i-1; j>=li; --j)
+		{
+			const pcl::PointXYZ & pt2 = cloud->at(j);
+			Eigen::Vector3f vd(pt2.x-pt.x, pt2.y - pt.y, pt2.z - pt.z);
+			if(searchRadius<=0.0f || (vd[0]*vd[0] + vd[1]*vd[1] + vd[2]*vd[2]) < searchRadius)
+			{
+				Eigen::Vector3f v(pt2.x-pt.x, pt2.y - pt.y, pt2.z - pt.z);
+				Eigen::Vector3f up = v.cross(direction);
+				Eigen::Vector3f n = up.cross(v);
+				n.normalize();
+				neighborNormals.push_back(n);
+			}
+			else
+			{
+				break;
+			}
+		}
+		for(int j=i+1; j<=hi; ++j)
+		{
+			const pcl::PointXYZ & pt2 = cloud->at(j);
+			Eigen::Vector3f vd(pt2.x-pt.x, pt2.y - pt.y, pt2.z - pt.z);
+			if(searchRadius<=0.0f || (vd[0]*vd[0] + vd[1]*vd[1] + vd[2]*vd[2]) < searchRadius)
+			{
+				Eigen::Vector3f v(pt2.x-pt.x, pt2.y - pt.y, pt2.z - pt.z);
+				Eigen::Vector3f up = v[2]==0.0f?Eigen::Vector3f(0,0,1):v.cross(direction);
+				Eigen::Vector3f n = up.cross(v);
+				n.normalize();
+				neighborNormals.push_back(n);
+			}
+			else
+			{
+				break;
+			}
+		}
+
+		if(neighborNormals.empty())
+		{
+			normals->at(i).normal_x = bad_point;
+			normals->at(i).normal_y = bad_point;
+			normals->at(i).normal_z = bad_point;
+		}
+		else
+		{
+			Eigen::Vector3f meanNormal(0,0,0);
+			for(unsigned int j=0; j<neighborNormals.size(); ++j)
+			{
+				meanNormal+=neighborNormals[j];
+			}
+			meanNormal /= (float)neighborNormals.size();
+			meanNormal.normalize();
+			normals->at(i).normal_x = meanNormal[0];
+			normals->at(i).normal_y = meanNormal[1];
+			normals->at(i).normal_z = meanNormal[2];
+		}
+	}
+
+	return normals;
+}
+
 pcl::PointCloud<pcl::Normal>::Ptr computeFastOrganizedNormals(
 		const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
 		float maxDepthChangeFactor,

guilib/include/rtabmap/gui/CloudViewer.h

@@ -274,8 +274,14 @@ public:
 	void setCameraFree();
 	void setCameraLockZ(bool enabled = true);
 	void setGridShown(bool shown);
+	void setNormalsShown(bool shown);
 	void setGridCellCount(unsigned int count);
 	void setGridCellSize(float size);
+	bool isNormalsShown() const;
+	int getNormalsStep() const;
+	float getNormalsScale() const;
+	void setNormalsStep(int step);
+	void setNormalsScale(float scale);
 
 public slots:
 	void setDefaultBackgroundColor(const QColor & color);
@@ -318,6 +324,9 @@ private:
     QAction * _aShowGrid;
     QAction * _aSetGridCellCount;
     QAction * _aSetGridCellSize;
+    QAction * _aShowNormals;
+	QAction * _aSetNormalsStep;
+	QAction * _aSetNormalsScale;
     QAction * _aSetBackgroundColor;
     QAction * _aSetRenderingRate;
     QAction * _aSetLighting;
@@ -336,6 +345,8 @@ private:
     QColor _frustumColor;
     unsigned int _gridCellCount;
     float _gridCellSize;
+    int _normalsStep;
+    float _normalsScale;
     cv::Vec3d _lastCameraOrientation;
     cv::Vec3d _lastCameraPose;
     QMap<std::string, Transform> _addedClouds; // include cloud, scan, meshes

guilib/include/rtabmap/gui/PreferencesDialog.h

@@ -164,9 +164,11 @@ public:
 	double getCeilingFilteringHeight() const;
 	double getFloorFilteringHeight() const;
 	int getNormalKSearch() const;
+	double getNormalRadiusSearch() const;
 	double getScanCeilingFilteringHeight() const;
 	double getScanFloorFilteringHeight() const;
 	int getScanNormalKSearch() const;
+	double getScanNormalRadiusSearch() const;
 	bool isCloudsShown(int index) const;      // 0=map, 1=odom
 	bool isOctomapUpdated() const;
 	bool isOctomapShown() const;
@@ -241,6 +243,7 @@ public:
 	double getSourceScanFromDepthMaxDepth() const;
 	double getSourceScanVoxelSize() const;
 	int getSourceScanNormalsK() const;
+	double getSourceScanNormalsRadius() const;
 	Transform getSourceLocalTransform() const;    //Openni group
 	Transform getLaserLocalTransform() const; // directory images
 	Camera * createCamera(bool useRawImages = false, bool useColor = true); // return camera should be deleted if not null

guilib/src/CloudViewer.cpp

@@ -28,6 +28,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "rtabmap/gui/CloudViewer.h"
 
 #include <rtabmap/core/Version.h>
+#include <rtabmap/core/util3d_transforms.h>
 #include <rtabmap/utilite/ULogger.h>
 #include <rtabmap/utilite/UTimer.h>
 #include <rtabmap/utilite/UMath.h>
@@ -190,6 +191,9 @@ CloudViewer::CloudViewer(QWidget *parent) :
 		_aShowGrid(0),
 		_aSetGridCellCount(0),
 		_aSetGridCellSize(0),
+		_aShowNormals(0),
+		_aSetNormalsStep(0),
+		_aSetNormalsScale(0),
 		_aSetBackgroundColor(0),
 		_aSetRenderingRate(0),
 		_aSetLighting(0),
@@ -203,6 +207,8 @@ CloudViewer::CloudViewer(QWidget *parent) :
 		_frustumColor(Qt::gray),
 		_gridCellCount(50),
 		_gridCellSize(1),
+		_normalsStep(1),
+		_normalsScale(0.2),
 		_lastCameraOrientation(0,0,0),
 		_lastCameraPose(0,0,0),
 		_defaultBgColor(Qt::black),
@@ -310,6 +316,10 @@ void CloudViewer::createMenu()
 	_aShowGrid->setCheckable(true);
 	_aSetGridCellCount = new QAction("Set cell count...", this);
 	_aSetGridCellSize = new QAction("Set cell size...", this);
+	_aShowNormals = new QAction("Show normals", this);
+	_aShowNormals->setCheckable(true);
+	_aSetNormalsStep = new QAction("Set normals step...", this);
+	_aSetNormalsScale = new QAction("Set normals scale...", this);
 	_aSetBackgroundColor = new QAction("Set background color...", this);	
 	_aSetRenderingRate = new QAction("Set rendering rate...", this);
 	_aSetLighting = new QAction("Lighting", this);
@@ -352,12 +362,18 @@ void CloudViewer::createMenu()
 	gridMenu->addAction(_aSetGridCellCount);
 	gridMenu->addAction(_aSetGridCellSize);
 
+	QMenu * normalsMenu = new QMenu("Normals", this);
+	normalsMenu->addAction(_aShowNormals);
+	normalsMenu->addAction(_aSetNormalsStep);
+	normalsMenu->addAction(_aSetNormalsScale);
+
 	//menus
 	_menu = new QMenu(this);
 	_menu->addMenu(cameraMenu);
 	_menu->addMenu(trajectoryMenu);
 	_menu->addMenu(frustumMenu);
 	_menu->addMenu(gridMenu);
+	_menu->addMenu(normalsMenu);
 	_menu->addAction(_aSetBackgroundColor);
 	_menu->addAction(_aSetRenderingRate);
 	_menu->addAction(_aSetLighting);
@@ -400,6 +416,10 @@ void CloudViewer::saveSettings(QSettings & settings, const QString & group) cons
 	settings.setValue("grid_cell_count", this->getGridCellCount());
 	settings.setValue("grid_cell_size", (double)this->getGridCellSize());
 
+	settings.setValue("normals", this->isNormalsShown());
+	settings.setValue("normals_step", this->getNormalsStep());
+	settings.setValue("normals_scale", (double)this->getNormalsScale());
+
 	settings.setValue("trajectory_shown", this->isTrajectoryShown());
 	settings.setValue("trajectory_size", this->getTrajectorySize());
 
@@ -439,6 +459,10 @@ void CloudViewer::loadSettings(QSettings & settings, const QString & group)
 	this->setGridCellCount(settings.value("grid_cell_count", this->getGridCellCount()).toUInt());
 	this->setGridCellSize(settings.value("grid_cell_size", this->getGridCellSize()).toFloat());
 
+	this->setNormalsShown(settings.value("normals", this->isNormalsShown()).toBool());
+	this->setNormalsStep(settings.value("normals_step", this->getNormalsStep()).toInt());
+	this->setNormalsScale(settings.value("normals_scale", this->getNormalsScale()).toFloat());
+
 	this->setTrajectoryShown(settings.value("trajectory_shown", this->isTrajectoryShown()).toBool());
 	this->setTrajectorySize(settings.value("trajectory_size", this->getTrajectorySize()).toUInt());
 
@@ -473,10 +497,21 @@ bool CloudViewer::updateCloudPose(
 	if(_addedClouds.contains(id))
 	{
 		UDEBUG("Updating pose %s to %s", id.c_str(), pose.prettyPrint().c_str());
-		if(_addedClouds.find(id).value() == pose ||
+		bool samePose = _addedClouds.find(id).value() == pose;
-		   _visualizer->updatePointCloudPose(id, pose.toEigen3f()))
+		Eigen::Affine3f posef = pose.toEigen3f();
+		if(samePose ||
+		   _visualizer->updatePointCloudPose(id, posef))
 		{
 			_addedClouds.find(id).value() = pose;
+			if(!samePose)
+			{
+				std::string idNormals = id+"-normals";
+				if(_addedClouds.find(idNormals)!=_addedClouds.end())
+				{
+					_visualizer->updatePointCloudPose(idNormals, posef);
+					_addedClouds.find(idNormals).value() = pose;
+				}
+			}
 			return true;
 		}
 	}
@@ -501,6 +536,18 @@ bool CloudViewer::addCloud(
 	Eigen::Vector4f origin(pose.x(), pose.y(), pose.z(), 0.0f);
 	Eigen::Quaternionf orientation = Eigen::Quaternionf(pose.toEigen3f().rotation());
 
+	if(haveNormals && _aShowNormals->isChecked())
+	{
+		pcl::PointCloud<pcl::PointNormal>::Ptr cloud_xyz (new pcl::PointCloud<pcl::PointNormal>);
+		pcl::fromPCLPointCloud2 (*binaryCloud, *cloud_xyz);
+		std::string idNormals = id + "-normals";
+		if(_visualizer->addPointCloudNormals<pcl::PointNormal>(cloud_xyz, _normalsStep, _normalsScale, idNormals, 0))
+		{
+			_visualizer->updatePointCloudPose(idNormals, pose.toEigen3f());
+			_addedClouds.insert(idNormals, pose);
+		}
+	}
+
 	// add random color channel
 	 pcl::visualization::PointCloudColorHandler<pcl::PCLPointCloud2>::Ptr colorHandler;
 	 colorHandler.reset (new pcl::visualization::PointCloudColorHandlerRandom<pcl::PCLPointCloud2> (binaryCloud));
@@ -1626,7 +1673,9 @@ void CloudViewer::removeAllClouds()
 bool CloudViewer::removeCloud(const std::string & id)
 {
 	bool success = _visualizer->removePointCloud(id);
+	_visualizer->removePointCloud(id+"-normals");
 	_addedClouds.remove(id); // remove after visualizer
+	_addedClouds.remove(id+"-normals");
 	return success;
 }
 
@@ -1929,6 +1978,12 @@ void CloudViewer::setCloudVisibility(const std::string & id, bool isVisible)
 	if(iter != cloudActorMap->end())
 	{
 		iter->second.actor->SetVisibility(isVisible?1:0);
+
+		iter = cloudActorMap->find(id+"-normals");
+		if(iter != cloudActorMap->end())
+		{
+			iter->second.actor->SetVisibility(isVisible&&_aShowNormals->isChecked()?1:0);
+		}
 	}
 	else
 	{
@@ -1992,20 +2047,6 @@ void CloudViewer::setCameraLockZ(bool enabled)
 	_lastCameraOrientation= _lastCameraPose = cv::Vec3f(0,0,0);
 	_aLockViewZ->setChecked(enabled);
 }
-
-void CloudViewer::setGridShown(bool shown)
-{
-	_aShowGrid->setChecked(shown);
-	if(shown)
-	{
-		this->addGrid();
-	}
-	else
-	{
-		this->removeGrid();
-	}
-}
-
 bool CloudViewer::isCameraTargetLocked() const
 {
 	return _aLockCamera->isChecked();
@@ -2022,6 +2063,23 @@ bool CloudViewer::isCameraLockZ() const
 {
 	return _aLockViewZ->isChecked();
 }
+double CloudViewer::getRenderingRate() const
+{
+	return _renderingRate;
+}
+
+void CloudViewer::setGridShown(bool shown)
+{
+	_aShowGrid->setChecked(shown);
+	if(shown)
+	{
+		this->addGrid();
+	}
+	else
+	{
+		this->removeGrid();
+	}
+}
 bool CloudViewer::isGridShown() const
 {
 	return _aShowGrid->isChecked();
@@ -2034,11 +2092,6 @@ float CloudViewer::getGridCellSize() const
 {
 	return _gridCellSize;
 }
-double CloudViewer::getRenderingRate() const
-{
-	return _renderingRate;
-}
-
 void CloudViewer::setGridCellCount(unsigned int count)
 {
 	if(count > 0)
@@ -2110,6 +2163,54 @@ void CloudViewer::removeGrid()
 	_gridLines.clear();
 }
 
+void CloudViewer::setNormalsShown(bool shown)
+{
+	_aShowNormals->setChecked(shown);
+	QList<std::string> ids = _addedClouds.keys();
+	for(QList<std::string>::iterator iter = ids.begin(); iter!=ids.end(); ++iter)
+	{
+		std::string idNormals = *iter + "-normals";
+		if(_addedClouds.find(idNormals) != _addedClouds.end())
+		{
+			this->setCloudVisibility(idNormals, this->getCloudVisibility(*iter) && shown);
+		}
+	}
+}
+bool CloudViewer::isNormalsShown() const
+{
+	return _aShowNormals->isChecked();
+}
+int CloudViewer::getNormalsStep() const
+{
+	return _normalsStep;
+}
+float CloudViewer::getNormalsScale() const
+{
+	return _normalsScale;
+}
+void CloudViewer::setNormalsStep(int step)
+{
+	if(step > 0)
+	{
+		_normalsStep = step;
+	}
+	else
+	{
+		UERROR("Cannot set normals step <= 0, step=%d", step);
+	}
+}
+void CloudViewer::setNormalsScale(float scale)
+{
+	if(scale > 0)
+	{
+		_normalsScale= scale;
+	}
+	else
+	{
+		UERROR("Cannot set normals scale <= 0, value=%f", scale);
+	}
+}
+
 Eigen::Vector3f rotatePointAroundAxe(
 		const Eigen::Vector3f & point,
 		const Eigen::Vector3f & axis,
@@ -2405,6 +2506,29 @@ void CloudViewer::handleAction(QAction * a)
 			this->setGridCellSize(value);
 		}
 	}
+	else if(a == _aShowNormals)
+	{
+		this->setNormalsShown(_aShowNormals->isChecked());
+		this->update();
+	}
+	else if(a == _aSetNormalsStep)
+	{
+		bool ok;
+		int value = QInputDialog::getInt(this, tr("Set normals step"), tr("Step"), _normalsStep, 1, 10000, 1, &ok);
+		if(ok)
+		{
+			this->setNormalsStep(value);
+		}
+	}
+	else if(a == _aSetNormalsScale)
+	{
+		bool ok;
+		double value = QInputDialog::getDouble(this, tr("Set normals scale"), tr("Scale (m)"), _normalsScale, 0.01, 10, 2, &ok);
+		if(ok)
+		{
+			this->setNormalsScale(value);
+		}
+	}
 	else if(a == _aSetBackgroundColor)
 	{
 		QColor color = this->getDefaultBackgroundColor();

guilib/src/ExportCloudsDialog.cpp

@@ -88,6 +88,7 @@ ExportCloudsDialog::ExportCloudsDialog(QWidget *parent) :
 	connect(_ui->checkBox_fromDepth, SIGNAL(stateChanged(int)), this, SLOT(updateReconstructionFlavor()));
 	connect(_ui->checkBox_binary, SIGNAL(stateChanged(int)), this, SIGNAL(configChanged()));
 	connect(_ui->spinBox_normalKSearch, SIGNAL(valueChanged(int)), this, SIGNAL(configChanged()));
+	connect(_ui->doubleSpinBox_normalRadiusSearch, SIGNAL(valueChanged(double)), this, SIGNAL(configChanged()));
 	connect(_ui->comboBox_pipeline, SIGNAL(currentIndexChanged(int)), this, SIGNAL(configChanged()));
 	connect(_ui->comboBox_pipeline, SIGNAL(currentIndexChanged(int)), this, SLOT(updateReconstructionFlavor()));
 	connect(_ui->comboBox_meshingApproach, SIGNAL(currentIndexChanged(int)), this, SIGNAL(configChanged()));
@@ -255,6 +256,7 @@ void ExportCloudsDialog::saveSettings(QSettings & settings, const QString & grou
 	settings.setValue("from_depth", _ui->checkBox_fromDepth->isChecked());
 	settings.setValue("binary", _ui->checkBox_binary->isChecked());
 	settings.setValue("normals_k", _ui->spinBox_normalKSearch->value());
+	settings.setValue("normals_radius", _ui->doubleSpinBox_normalRadiusSearch->value());
 
 	settings.setValue("regenerate", _ui->checkBox_regenerate->isChecked());
 	settings.setValue("regenerate_decimation", _ui->spinBox_decimation->value());
@@ -371,6 +373,7 @@ void ExportCloudsDialog::loadSettings(QSettings & settings, const QString & grou
 	_ui->checkBox_fromDepth->setChecked(settings.value("from_depth", _ui->checkBox_fromDepth->isChecked()).toBool());
 	_ui->checkBox_binary->setChecked(settings.value("binary", _ui->checkBox_binary->isChecked()).toBool());
 	_ui->spinBox_normalKSearch->setValue(settings.value("normals_k", _ui->spinBox_normalKSearch->value()).toInt());
+	_ui->doubleSpinBox_normalRadiusSearch->setValue(settings.value("normals_radius", _ui->doubleSpinBox_normalRadiusSearch->value()).toDouble());
 
 	_ui->checkBox_regenerate->setChecked(settings.value("regenerate", _ui->checkBox_regenerate->isChecked()).toBool());
 	_ui->spinBox_decimation->setValue(settings.value("regenerate_decimation", _ui->spinBox_decimation->value()).toInt());
@@ -487,6 +490,7 @@ void ExportCloudsDialog::restoreDefaults()
 	_ui->checkBox_fromDepth->setChecked(true);
 	_ui->checkBox_binary->setChecked(true);
 	_ui->spinBox_normalKSearch->setValue(20);
+	_ui->doubleSpinBox_normalRadiusSearch->setValue(0.0);
 
 	_ui->checkBox_regenerate->setChecked(_dbDriver!=0?true:false);
 	_ui->spinBox_decimation->setValue(1);
@@ -1384,7 +1388,7 @@ bool ExportCloudsDialog::getExportedClouds(
 				// recompute normals
 				pcl::PointCloud<pcl::PointXYZ>::Ptr cloudWithoutNormals(new pcl::PointCloud<pcl::PointXYZ>);
 				pcl::copyPointCloud(*assembledCloud, *cloudWithoutNormals);
-				pcl::PointCloud<pcl::Normal>::Ptr normals = util3d::computeNormals(cloudWithoutNormals, indices, _ui->spinBox_normalKSearch->value());
+				pcl::PointCloud<pcl::Normal>::Ptr normals = util3d::computeNormals(cloudWithoutNormals, indices, _ui->spinBox_normalKSearch->value(), _ui->doubleSpinBox_normalRadiusSearch->value());
 
 				UASSERT(assembledCloud->size() == normals->size());
 				for(unsigned int i=0; i<normals->size(); ++i)
@@ -2521,7 +2525,7 @@ std::map<int, std::pair<pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr, pcl::Indic
 							viewPoint[2] = data.stereoCameraModel().localTransform().z();
 						}
 
-						pcl::PointCloud<pcl::Normal>::Ptr normals = util3d::computeNormals(cloudWithoutNormals, indices, _ui->spinBox_normalKSearch->value(), viewPoint);
+						pcl::PointCloud<pcl::Normal>::Ptr normals = util3d::computeNormals(cloudWithoutNormals, indices, _ui->spinBox_normalKSearch->value(), _ui->doubleSpinBox_normalRadiusSearch->value(), viewPoint);
 						pcl::concatenateFields(*cloudWithoutNormals, *normals, *cloud);
 
 						if(_ui->checkBox_subtraction->isChecked() &&
@@ -2592,7 +2596,7 @@ std::map<int, std::pair<pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr, pcl::Indic
 						}
 						else
 						{
-							normals = util3d::computeNormals(cloudWithoutNormals, indices, _ui->spinBox_normalKSearch->value(), viewPoint);
+							normals = util3d::computeNormals(cloudWithoutNormals, indices, _ui->spinBox_normalKSearch->value(), _ui->doubleSpinBox_normalRadiusSearch->value(), viewPoint);
 						}
 						pcl::concatenateFields(*cloudWithoutNormals, *normals, *cloud);
 					}
@@ -2673,7 +2677,7 @@ std::map<int, std::pair<pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr, pcl::Indic
 					_progressDialog->appendText(tr("Cached cloud %1 is not found in cached data, the view point for normal computation will not be set (%2/%3).").arg(iter->first).arg(index).arg(poses.size()), Qt::darkYellow);
 				}
 
-				pcl::PointCloud<pcl::Normal>::Ptr normals = util3d::computeNormals(cloudWithoutNormals, indices, _ui->spinBox_normalKSearch->value(), viewPoint);
+				pcl::PointCloud<pcl::Normal>::Ptr normals = util3d::computeNormals(cloudWithoutNormals, indices, _ui->spinBox_normalKSearch->value(), _ui->doubleSpinBox_normalRadiusSearch->value(), viewPoint);
 				pcl::concatenateFields(*cloudWithoutNormals, *normals, *cloud);
 			}
 			else if(!_ui->checkBox_fromDepth->isChecked() && uContains(cachedScans, iter->first))
@@ -2731,7 +2735,7 @@ std::map<int, std::pair<pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr, pcl::Indic
 					}
 					else
 					{
-						normals = util3d::computeNormals(cloudWithoutNormals, indices, _ui->spinBox_normalKSearch->value(), viewPoint);
+						normals = util3d::computeNormals(cloudWithoutNormals, indices, _ui->spinBox_normalKSearch->value(), _ui->doubleSpinBox_normalRadiusSearch->value(), viewPoint);
 					}
 					pcl::concatenateFields(*cloudWithoutNormals, *normals, *cloud);
 				}

guilib/src/MainWindow.cpp

@@ -2651,9 +2651,9 @@ std::pair<pcl::PointCloud<pcl::PointXYZRGB>::Ptr, pcl::IndicesPtr> MainWindow::c
 					if(_preferencesDialog->getSubtractFilteringAngle() > 0.0f)
 					{
 						//normals required
-						if(_preferencesDialog->getNormalKSearch() > 0)
+						if(_preferencesDialog->getNormalKSearch() > 0 || _preferencesDialog->getNormalRadiusSearch() > 0)
 						{
-							pcl::PointCloud<pcl::Normal>::Ptr normals = util3d::computeNormals(cloud, indices, _preferencesDialog->getNormalKSearch(), viewPoint);
+							pcl::PointCloud<pcl::Normal>::Ptr normals = util3d::computeNormals(cloud, indices, _preferencesDialog->getNormalKSearch(), _preferencesDialog->getNormalRadiusSearch(), viewPoint);
 							pcl::concatenateFields(*cloud, *normals, *cloudWithNormals);
 						}
 						else
@@ -2790,7 +2790,7 @@ std::pair<pcl::PointCloud<pcl::PointXYZRGB>::Ptr, pcl::IndicesPtr> MainWindow::c
 
 				if(_preferencesDialog->getNormalKSearch() > 0 && cloudWithNormals->size() == 0)
 				{
-					pcl::PointCloud<pcl::Normal>::Ptr normals = util3d::computeNormals(cloud, indices, _preferencesDialog->getNormalKSearch(), viewPoint);
+					pcl::PointCloud<pcl::Normal>::Ptr normals = util3d::computeNormals(cloud, indices, _preferencesDialog->getNormalKSearch(), _preferencesDialog->getNormalRadiusSearch(), viewPoint);
 					pcl::concatenateFields(*cloud, *normals, *cloudWithNormals);
 				}
 
@@ -2880,22 +2880,48 @@ void MainWindow::createAndAddScanToMap(int nodeId, const Transform & pose, int m
 			scan = util3d::downsample(scan, _preferencesDialog->getDownsamplingStepScan(0));
 		}
 
-		if(scan.channels() == 6)
+		pcl::PointCloud<pcl::PointXYZ>::Ptr cloud;
+		pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudRGB;
+		pcl::PointCloud<pcl::PointNormal>::Ptr cloudWithNormals;
+		pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloudRGBWithNormals;
+		if(scan.channels() == 7 && _preferencesDialog->getCloudVoxelSizeScan(0) <= 0.0)
 		{
-			pcl::PointCloud<pcl::PointNormal>::Ptr cloud;
+			cloudRGBWithNormals = util3d::laserScanToPointCloudRGBNormal(scan, iter->sensorData().laserScanInfo().localTransform());
-			cloud = util3d::laserScanToPointCloudNormal(scan, iter->sensorData().laserScanInfo().localTransform());
+		}
-			if(_preferencesDialog->getCloudVoxelSizeScan(0) > 0.0)
+		else if(scan.channels() == 6 && _preferencesDialog->getCloudVoxelSizeScan(0) <= 0.0)
+		{
+			cloudWithNormals = util3d::laserScanToPointCloudNormal(scan, iter->sensorData().laserScanInfo().localTransform());
+		}
+		else if(scan.channels() == 4)
+		{
+			cloudRGB = util3d::laserScanToPointCloudRGB(scan, iter->sensorData().laserScanInfo().localTransform());
+		}
+		else
+		{
+			cloud = util3d::laserScanToPointCloud(scan, iter->sensorData().laserScanInfo().localTransform());
+		}
+
+		if(_preferencesDialog->getCloudVoxelSizeScan(0) > 0.0)
+		{
+			if(cloud.get())
 			{
 				cloud = util3d::voxelize(cloud, _preferencesDialog->getCloudVoxelSizeScan(0));
 			}
+			if(cloudRGB.get())
+			{
+				cloudRGB = util3d::voxelize(cloudRGB, _preferencesDialog->getCloudVoxelSizeScan(0));
+			}
+		}
 
-			// Do ceiling/floor filtering
+		// Do ceiling/floor filtering
-			if(cloud->size() &&
+		if(scan.channels() > 2 && // don't filter 2D scans
-			   (_preferencesDialog->getScanFloorFilteringHeight() != 0.0 ||
+		   (_preferencesDialog->getScanFloorFilteringHeight() != 0.0 ||
-			   _preferencesDialog->getScanCeilingFilteringHeight() != 0.0))
+		   _preferencesDialog->getScanCeilingFilteringHeight() != 0.0))
+		{
+			if(cloudRGBWithNormals.get())
 			{
 				// perform in /map frame
-				pcl::PointCloud<pcl::PointNormal>::Ptr cloudTransformed = util3d::transformPointCloud(cloud, pose);
+				pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloudTransformed = util3d::transformPointCloud(cloudRGBWithNormals, pose);
 				cloudTransformed = rtabmap::util3d::passThrough(
 						cloudTransformed,
 						"z",
@@ -2903,53 +2929,35 @@ void MainWindow::createAndAddScanToMap(int nodeId, const Transform & pose, int m
 						_preferencesDialog->getScanCeilingFilteringHeight()==0.0?(float)std::numeric_limits<int>::max():_preferencesDialog->getScanCeilingFilteringHeight());
 
 				//transform back in sensor frame
-				cloud = util3d::transformPointCloud(cloudTransformed, pose.inverse());
+				cloudRGBWithNormals = util3d::transformPointCloud(cloudTransformed, pose.inverse());
 			}
+			if(cloudWithNormals.get())
+			{
+				// perform in /map frame
+				pcl::PointCloud<pcl::PointNormal>::Ptr cloudTransformed = util3d::transformPointCloud(cloudWithNormals, pose);
+				cloudTransformed = rtabmap::util3d::passThrough(
+						cloudTransformed,
+						"z",
+						_preferencesDialog->getScanFloorFilteringHeight()==0.0?(float)std::numeric_limits<int>::min():_preferencesDialog->getScanFloorFilteringHeight(),
+						_preferencesDialog->getScanCeilingFilteringHeight()==0.0?(float)std::numeric_limits<int>::max():_preferencesDialog->getScanCeilingFilteringHeight());
 
-			QColor color = Qt::gray;
+				//transform back in sensor frame
-			if(mapId >= 0)
+				cloudWithNormals = util3d::transformPointCloud(cloudTransformed, pose.inverse());
-			{
-				color = (Qt::GlobalColor)(mapId+3 % 12 + 7 );
 			}
-			if(!_cloudViewer->addCloud(scanName, cloud, pose, color))
+			if(cloudRGB.get())
 			{
-				UERROR("Adding cloud %d to viewer failed!", nodeId);
+				// perform in /map frame
-			}
+				pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudTransformed = util3d::transformPointCloud(cloudRGB, pose);
-			else
+				cloudTransformed = rtabmap::util3d::passThrough(
-			{
+						cloudTransformed,
-				if(nodeId > 0)
+						"z",
-				{
+						_preferencesDialog->getScanFloorFilteringHeight()==0.0?(float)std::numeric_limits<int>::min():_preferencesDialog->getScanFloorFilteringHeight(),
-					if(_preferencesDialog->getCloudVoxelSizeScan(0) > 0.0)
+						_preferencesDialog->getScanCeilingFilteringHeight()==0.0?(float)std::numeric_limits<int>::max():_preferencesDialog->getScanCeilingFilteringHeight());
-					{
-						//reconvert the voxelized cloud
-						scan = util3d::laserScanFromPointCloud(*cloud);
-					}
-					else
-					{
-						scan = util3d::transformLaserScan(scan, iter->sensorData().laserScanInfo().localTransform());
-					}
-					_createdScans.insert(std::make_pair(nodeId, scan)); // keep scan in base_link frame
-				}
-				_cloudViewer->setCloudOpacity(scanName, _preferencesDialog->getScanOpacity(0));
-				_cloudViewer->setCloudPointSize(scanName, _preferencesDialog->getScanPointSize(0));
-			}
-		}
-		else
-		{
-			pcl::PointCloud<pcl::PointXYZ>::Ptr cloud;
-			cloud = util3d::laserScanToPointCloud(scan, iter->sensorData().laserScanInfo().localTransform());
-			bool filtered = false;
-			if(_preferencesDialog->getCloudVoxelSizeScan(0) > 0.0)
-			{
-				cloud = util3d::voxelize(cloud, _preferencesDialog->getCloudVoxelSizeScan(0));
-				filtered = true;
-			}
 
-			// Do ceiling/floor filtering
+				//transform back in sensor frame
-			if(scan.channels() > 2 && // don't filter 2D scans
+				cloudRGB = util3d::transformPointCloud(cloudTransformed, pose.inverse());
-				cloud->size() &&
+			}
-			   (_preferencesDialog->getScanFloorFilteringHeight() != 0.0 ||
+			if(cloud.get())
-			   _preferencesDialog->getScanCeilingFilteringHeight() != 0.0))
 			{
 				// perform in /map frame
 				pcl::PointCloud<pcl::PointXYZ>::Ptr cloudTransformed = util3d::transformPointCloud(cloud, pose);
@@ -2961,78 +2969,103 @@ void MainWindow::createAndAddScanToMap(int nodeId, const Transform & pose, int m
 
 				//transform back in sensor frame
 				cloud = util3d::transformPointCloud(cloudTransformed, pose.inverse());
-				filtered = true;
 			}
+		}
 
-			pcl::PointCloud<pcl::PointNormal>::Ptr cloudWithNormals;
+		if(	(cloud.get() || cloudRGB.get()) &&
-			if(scan.channels() > 2 && // don't compute normals for 2D scans
+		   (_preferencesDialog->getScanNormalKSearch() > 0 || _preferencesDialog->getScanNormalRadiusSearch() > 0.0))
-				cloud->size() &&
+		{
-			   _preferencesDialog->getScanNormalKSearch() > 0)
+			Eigen::Vector3f scanViewpoint(
-			{
+				iter->sensorData().laserScanInfo().localTransform().x(),
-				pcl::PointCloud<pcl::Normal>::Ptr normals = util3d::computeNormals(cloud, _preferencesDialog->getScanNormalKSearch());
+				iter->sensorData().laserScanInfo().localTransform().y(),
-				cloudWithNormals.reset(new pcl::PointCloud<pcl::PointNormal>);
+				iter->sensorData().laserScanInfo().localTransform().z());
-				pcl::concatenateFields(*cloud, *normals, *cloudWithNormals);
-				filtered = true;
-			}
 
-			QColor color = Qt::gray;
+			pcl::PointCloud<pcl::Normal>::Ptr normals;
-			if(mapId >= 0)
+			if(cloud->size())
 			{
-				color = (Qt::GlobalColor)(mapId+3 % 12 + 7 );
+				if(scan.channels() == 2)
-			}
-			if(cloudWithNormals.get())
-			{
-				if(!_cloudViewer->addCloud(scanName, cloudWithNormals, pose, color))
 				{
-					UERROR("Adding cloud %d to viewer failed!", nodeId);
+					normals = util3d::computeFastOrganizedNormals2D(cloud, _preferencesDialog->getScanNormalKSearch(), _preferencesDialog->getScanNormalRadiusSearch(), scanViewpoint);
 				}
 				else
 				{
-					if(nodeId > 0)
+					normals = util3d::computeNormals(cloud, _preferencesDialog->getScanNormalKSearch(), _preferencesDialog->getScanNormalRadiusSearch(), scanViewpoint);
-					{
-						//reconvert the voxelized cloud
-						scan = util3d::laserScanFromPointCloud(*cloudWithNormals);
-						_createdScans.insert(std::make_pair(nodeId, scan)); // keep scan in base_link frame
-					}
-
-					_cloudViewer->setCloudOpacity(scanName, _preferencesDialog->getScanOpacity(0));
-					_cloudViewer->setCloudPointSize(scanName, _preferencesDialog->getScanPointSize(0));
 				}
+				cloudWithNormals.reset(new pcl::PointCloud<pcl::PointNormal>);
+				pcl::concatenateFields(*cloud, *normals, *cloudWithNormals);
+				cloud.reset();
 			}
 			else
 			{
-				if(!_cloudViewer->addCloud(scanName, cloud, pose, color))
+				UASSERT(cloudRGB->size()); // Assuming 4 channels cannot be 2D
+				normals = util3d::computeNormals(cloudRGB, _preferencesDialog->getScanNormalKSearch(), _preferencesDialog->getScanNormalRadiusSearch(), scanViewpoint);
+				cloudRGBWithNormals.reset(new pcl::PointCloud<pcl::PointXYZRGBNormal>);
+				pcl::concatenateFields(*cloudRGB, *normals, *cloudRGBWithNormals);
+				cloudRGB.reset();
+			}
+		}
+
+		QColor color = Qt::gray;
+		if(mapId >= 0)
+		{
+			color = (Qt::GlobalColor)(mapId+3 % 12 + 7 );
+		}
+		bool added = false;
+		if(cloudRGBWithNormals.get())
+		{
+			added = _cloudViewer->addCloud(scanName, cloudRGBWithNormals, pose, color);
+			if(added && nodeId > 0)
+			{
+				scan = util3d::laserScanFromPointCloud(*cloudRGBWithNormals);
+			}
+		}
+		else if(cloudWithNormals.get())
+		{
+			added = _cloudViewer->addCloud(scanName, cloudWithNormals, pose, color);
+			if(added && nodeId > 0)
+			{
+				scan = util3d::laserScanFromPointCloud(*cloudWithNormals);
+			}
+		}
+		else if(cloudRGB.get())
+		{
+			added = _cloudViewer->addCloud(scanName, cloudWithNormals, pose, color);
+			if(added && nodeId > 0)
+			{
+				scan = util3d::laserScanFromPointCloud(*cloudWithNormals);
+			}
+		}
+		else
+		{
+			UASSERT(cloud.get());
+			added = _cloudViewer->addCloud(scanName, cloud, pose, color);
+			if(added && nodeId > 0)
+			{
+				if(scan.channels() == 2)
 				{
-					UERROR("Adding cloud %d to viewer failed!", nodeId);
+					scan = util3d::laserScan2dFromPointCloud(*cloud);
 				}
 				else
 				{
-					if(nodeId > 0)
+					scan = util3d::laserScanFromPointCloud(*cloud);
-					{
-						if(filtered)
-						{
-							//reconvert the voxelized cloud
-							if(scan.channels() == 2)
-							{
-								scan = util3d::laserScan2dFromPointCloud(*cloud);
-							}
-							else
-							{
-								scan = util3d::laserScanFromPointCloud(*cloud);
-							}
-						}
-						else
-						{
-							scan = util3d::transformLaserScan(scan, iter->sensorData().laserScanInfo().localTransform());
-						}
-						_createdScans.insert(std::make_pair(nodeId, scan)); // keep scan in base_link frame
-					}
-
-					_cloudViewer->setCloudOpacity(scanName, _preferencesDialog->getScanOpacity(0));
-					_cloudViewer->setCloudPointSize(scanName, _preferencesDialog->getScanPointSize(0));
 				}
 			}
 		}
+
+		if(!added)
+		{
+			UERROR("Adding cloud %d to viewer failed!", nodeId);
+		}
+		else
+		{
+			if(nodeId > 0)
+			{
+				_createdScans.insert(std::make_pair(nodeId, scan)); // keep scan in base_link frame
+			}
+
+			_cloudViewer->setCloudOpacity(scanName, _preferencesDialog->getScanOpacity(0));
+			_cloudViewer->setCloudPointSize(scanName, _preferencesDialog->getScanPointSize(0));
+		}
 	}
 }
 
@@ -4496,7 +4529,8 @@ void MainWindow::startDetection()
 			_preferencesDialog->getSourceScanFromDepthDecimation(),
 			_preferencesDialog->getSourceScanFromDepthMaxDepth(),
 			_preferencesDialog->getSourceScanVoxelSize(),
-			_preferencesDialog->getSourceScanNormalsK());
+			_preferencesDialog->getSourceScanNormalsK(),
+			_preferencesDialog->getSourceScanNormalsRadius());
 	if(_preferencesDialog->isDepthFilteringAvailable())
 	{
 		if(_preferencesDialog->isBilateralFiltering())

guilib/src/PreferencesDialog.cpp

@@ -410,9 +410,11 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	connect(_ui->doubleSpinBox_ceilingFilterHeight, SIGNAL(valueChanged(double)), this, SLOT(makeObsoleteCloudRenderingPanel()));
 	connect(_ui->doubleSpinBox_floorFilterHeight, SIGNAL(valueChanged(double)), this, SLOT(makeObsoleteCloudRenderingPanel()));
 	connect(_ui->spinBox_normalKSearch, SIGNAL(valueChanged(int)), this, SLOT(makeObsoleteCloudRenderingPanel()));
+	connect(_ui->doubleSpinBox_normalRadiusSearch, SIGNAL(valueChanged(double)), this, SLOT(makeObsoleteCloudRenderingPanel()));
 	connect(_ui->doubleSpinBox_ceilingFilterHeight_scan, SIGNAL(valueChanged(double)), this, SLOT(makeObsoleteCloudRenderingPanel()));
 	connect(_ui->doubleSpinBox_floorFilterHeight_scan, SIGNAL(valueChanged(double)), this, SLOT(makeObsoleteCloudRenderingPanel()));
 	connect(_ui->spinBox_normalKSearch_scan, SIGNAL(valueChanged(int)), this, SLOT(makeObsoleteCloudRenderingPanel()));
+	connect(_ui->doubleSpinBox_normalRadiusSearch_scan, SIGNAL(valueChanged(double)), this, SLOT(makeObsoleteCloudRenderingPanel()));
 
 	connect(_ui->checkBox_showGraphs, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteCloudRenderingPanel()));
 	connect(_ui->checkBox_showFrustums, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteCloudRenderingPanel()));
@@ -585,6 +587,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	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()));
+	connect(_ui->doubleSpinBox_cameraImages_scanNormalsRadius, SIGNAL(valueChanged(double)), this, SLOT(makeObsoleteSourcePanel()));
 
 	//Rtabmap basic
 	connect(_ui->general_doubleSpinBox_timeThr, SIGNAL(valueChanged(double)), _ui->general_doubleSpinBox_timeThr_2, SLOT(setValue(double)));
@@ -650,6 +653,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	_ui->spinBox_imagePostDecimation->setObjectName(Parameters::kMemImagePostDecimation().c_str());
 	_ui->general_spinBox_laserScanDownsample->setObjectName(Parameters::kMemLaserScanDownsampleStepSize().c_str());
 	_ui->general_spinBox_laserScanNormalK->setObjectName(Parameters::kMemLaserScanNormalK().c_str());
+	_ui->general_doubleSpinBox_laserScanNormalRadius->setObjectName(Parameters::kMemLaserScanNormalRadius().c_str());
 	_ui->checkBox_useOdomFeatures->setObjectName(Parameters::kMemUseOdomFeatures().c_str());
 
 	// Database
@@ -857,7 +861,8 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	_ui->loopClosure_icpEpsilon->setObjectName(Parameters::kIcpEpsilon().c_str());
 	_ui->loopClosure_icpRatio->setObjectName(Parameters::kIcpCorrespondenceRatio().c_str());
 	_ui->loopClosure_icpPointToPlane->setObjectName(Parameters::kIcpPointToPlane().c_str());
-	_ui->loopClosure_icpPointToPlaneNormals->setObjectName(Parameters::kIcpPointToPlaneNormalNeighbors().c_str());
+	_ui->loopClosure_icpPointToPlaneNormals->setObjectName(Parameters::kIcpPointToPlaneK().c_str());
+	_ui->loopClosure_icpPointToPlaneNormalsRadius->setObjectName(Parameters::kIcpPointToPlaneRadius().c_str());
 
 	_ui->groupBox_libpointmatcher->setObjectName(Parameters::kIcpPM().c_str());
 	_ui->lineEdit_IcpPMConfigPath->setObjectName(Parameters::kIcpPMConfig().c_str());
@@ -1374,10 +1379,12 @@ void PreferencesDialog::resetSettings(QGroupBox * groupBox)
 		_ui->doubleSpinBox_ceilingFilterHeight->setValue(0);
 		_ui->doubleSpinBox_floorFilterHeight->setValue(0);
 		_ui->spinBox_normalKSearch->setValue(10);
+		_ui->doubleSpinBox_normalRadiusSearch->setValue(0.0);
 
 		_ui->doubleSpinBox_ceilingFilterHeight_scan->setValue(0);
 		_ui->doubleSpinBox_floorFilterHeight_scan->setValue(0);
 		_ui->spinBox_normalKSearch_scan->setValue(0);
+		_ui->doubleSpinBox_normalRadiusSearch_scan->setValue(0.0);
 
 		_ui->checkBox_showGraphs->setChecked(true);
 		_ui->checkBox_showFrustums->setChecked(false);
@@ -1542,6 +1549,7 @@ void PreferencesDialog::resetSettings(QGroupBox * groupBox)
 		_ui->doubleSpinBox_cameraSCanFromDepth_maxDepth->setValue(4.0);
 		_ui->doubleSpinBox_cameraImages_scanVoxelSize->setValue(0.025f);
 		_ui->spinBox_cameraImages_scanNormalsK->setValue(20);
+		_ui->doubleSpinBox_cameraImages_scanNormalsRadius->setValue(0.0);
 
 		_ui->groupBox_depthFromScan->setChecked(false);
 		_ui->groupBox_depthFromScan_fillHoles->setChecked(true);
@@ -1769,9 +1777,11 @@ void PreferencesDialog::readGuiSettings(const QString & filePath)
 	_ui->doubleSpinBox_ceilingFilterHeight->setValue(settings.value("cloudCeilingHeight", _ui->doubleSpinBox_ceilingFilterHeight->value()).toDouble());
 	_ui->doubleSpinBox_floorFilterHeight->setValue(settings.value("cloudFloorHeight", _ui->doubleSpinBox_floorFilterHeight->value()).toDouble());
 	_ui->spinBox_normalKSearch->setValue(settings.value("normalKSearch", _ui->spinBox_normalKSearch->value()).toInt());
+	_ui->doubleSpinBox_normalRadiusSearch->setValue(settings.value("normalRadiusSearch", _ui->doubleSpinBox_normalRadiusSearch->value()).toDouble());
 	_ui->doubleSpinBox_ceilingFilterHeight_scan->setValue(settings.value("scanCeilingHeight", _ui->doubleSpinBox_ceilingFilterHeight_scan->value()).toDouble());
 	_ui->doubleSpinBox_floorFilterHeight_scan->setValue(settings.value("scanFloorHeight", _ui->doubleSpinBox_floorFilterHeight_scan->value()).toDouble());
 	_ui->spinBox_normalKSearch_scan->setValue(settings.value("scanNormalKSearch", _ui->spinBox_normalKSearch_scan->value()).toInt());
+	_ui->doubleSpinBox_normalRadiusSearch_scan->setValue(settings.value("scanNormalRadiusSearch", _ui->doubleSpinBox_normalRadiusSearch_scan->value()).toDouble());
 
 	_ui->checkBox_showGraphs->setChecked(settings.value("showGraphs", _ui->checkBox_showGraphs->isChecked()).toBool());
 	_ui->checkBox_showFrustums->setChecked(settings.value("showFrustums", _ui->checkBox_showFrustums->isChecked()).toBool());
@@ -1933,6 +1943,7 @@ void PreferencesDialog::readCameraSettings(const QString & filePath)
 	_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());
+	_ui->doubleSpinBox_cameraImages_scanNormalsRadius->setValue(settings.value("normalsRadius", _ui->doubleSpinBox_cameraImages_scanNormalsRadius->value()).toDouble());
 	settings.endGroup();//ScanFromDepth
 
 	settings.beginGroup("DepthFromScan");
@@ -2155,9 +2166,11 @@ void PreferencesDialog::writeGuiSettings(const QString & filePath) const
 	settings.setValue("cloudCeilingHeight",     _ui->doubleSpinBox_ceilingFilterHeight->value());
 	settings.setValue("cloudFloorHeight",       _ui->doubleSpinBox_floorFilterHeight->value());
 	settings.setValue("normalKSearch",          _ui->spinBox_normalKSearch->value());
+	settings.setValue("normalRadiusSearch",     _ui->doubleSpinBox_normalRadiusSearch->value());
 	settings.setValue("scanCeilingHeight",      _ui->doubleSpinBox_ceilingFilterHeight_scan->value());
 	settings.setValue("scanFloorHeight",        _ui->doubleSpinBox_floorFilterHeight_scan->value());
 	settings.setValue("scanNormalKSearch",      _ui->spinBox_normalKSearch_scan->value());
+	settings.setValue("scanNormalRadiusSearch", _ui->doubleSpinBox_normalRadiusSearch_scan->value());
 
 	settings.setValue("showGraphs", _ui->checkBox_showGraphs->isChecked());
 	settings.setValue("showFrustums", _ui->checkBox_showFrustums->isChecked());
@@ -2321,6 +2334,7 @@ void PreferencesDialog::writeCameraSettings(const QString & filePath) const
 	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.setValue("normalsRadius", 		_ui->doubleSpinBox_cameraImages_scanNormalsRadius->value());
 	settings.endGroup();
 
 	settings.beginGroup("DepthFromScan");
@@ -4286,6 +4300,10 @@ int PreferencesDialog::getNormalKSearch() const
 {
 	return _ui->spinBox_normalKSearch->value();
 }
+double PreferencesDialog::getNormalRadiusSearch() const
+{
+	return _ui->doubleSpinBox_normalRadiusSearch->value();
+}
 double PreferencesDialog::getScanCeilingFilteringHeight() const
 {
 	return _ui->doubleSpinBox_ceilingFilterHeight_scan->value();
@@ -4298,6 +4316,10 @@ int PreferencesDialog::getScanNormalKSearch() const
 {
 	return _ui->spinBox_normalKSearch_scan->value();
 }
+double PreferencesDialog::getScanNormalRadiusSearch() const
+{
+	return _ui->doubleSpinBox_normalRadiusSearch_scan->value();
+}
 
 bool PreferencesDialog::isGraphsShown() const
 {
@@ -4636,6 +4658,10 @@ int PreferencesDialog::getSourceScanNormalsK() const
 {
 	return _ui->spinBox_cameraImages_scanNormalsK->value();
 }
+double PreferencesDialog::getSourceScanNormalsRadius() const
+{
+	return _ui->doubleSpinBox_cameraImages_scanNormalsRadius->value();
+}
 
 Camera * PreferencesDialog::createCamera(bool useRawImages, bool useColor)
 {
@@ -4743,6 +4769,7 @@ Camera * PreferencesDialog::createCamera(bool useRawImages, bool useColor)
 						_ui->spinBox_cameraImages_scanDownsampleStep->value(),
 						_ui->doubleSpinBox_cameraImages_scanVoxelSize->value(),
 						_ui->spinBox_cameraImages_scanNormalsK->value(),
+						_ui->doubleSpinBox_cameraImages_scanNormalsRadius->value(),
 						this->getLaserLocalTransform());
 		((CameraRGBDImages*)camera)->setTimestamps(
 				_ui->checkBox_cameraImages_timestamps->isChecked(),
@@ -4788,6 +4815,7 @@ Camera * PreferencesDialog::createCamera(bool useRawImages, bool useColor)
 						_ui->spinBox_cameraImages_scanDownsampleStep->value(),
 						_ui->doubleSpinBox_cameraImages_scanVoxelSize->value(),
 						_ui->spinBox_cameraImages_scanNormalsK->value(),
+						_ui->doubleSpinBox_cameraImages_scanNormalsRadius->value(),
 						this->getLaserLocalTransform());
 		((CameraStereoImages*)camera)->setTimestamps(
 				_ui->checkBox_cameraImages_timestamps->isChecked(),
@@ -4893,6 +4921,7 @@ Camera * PreferencesDialog::createCamera(bool useRawImages, bool useColor)
 						_ui->spinBox_cameraImages_scanDownsampleStep->value(),
 						_ui->doubleSpinBox_cameraImages_scanVoxelSize->value(),
 						_ui->spinBox_cameraImages_scanNormalsK->value(),
+						_ui->doubleSpinBox_cameraImages_scanNormalsRadius->value(),
 						this->getLaserLocalTransform());
 		((CameraImages*)camera)->setDepthFromScan(
 				_ui->groupBox_depthFromScan->isChecked(),
@@ -5138,7 +5167,8 @@ void PreferencesDialog::testOdometry()
 			_ui->spinBox_cameraScanFromDepth_decimation->value(),
 			_ui->doubleSpinBox_cameraSCanFromDepth_maxDepth->value(),
 			_ui->doubleSpinBox_cameraImages_scanVoxelSize->value(),
-			_ui->spinBox_cameraImages_scanNormalsK->value());
+			_ui->spinBox_cameraImages_scanNormalsK->value(),
+			_ui->doubleSpinBox_cameraImages_scanNormalsRadius->value());
 	if(isDepthFilteringAvailable())
 	{
 		if(_ui->groupBox_bilateral->isChecked())
@@ -5186,7 +5216,8 @@ void PreferencesDialog::testCamera()
 				_ui->spinBox_cameraScanFromDepth_decimation->value(),
 				_ui->doubleSpinBox_cameraSCanFromDepth_maxDepth->value(),
 				_ui->doubleSpinBox_cameraImages_scanVoxelSize->value(),
-				_ui->spinBox_cameraImages_scanNormalsK->value());
+				_ui->spinBox_cameraImages_scanNormalsK->value(),
+				_ui->doubleSpinBox_cameraImages_scanNormalsRadius->value());
 		if(isDepthFilteringAvailable())
 		{
 			if(_ui->groupBox_bilateral->isChecked())

guilib/src/ui/exportCloudsDialog.ui

@@ -23,9 +23,9 @@
       <property name="geometry">
        <rect>
         <x>0</x>
-        <y>-2684</y>
+        <y>0</y>
-        <width>773</width>
+        <width>778</width>
-        <height>4103</height>
+        <height>4058</height>
        </rect>
       </property>
       <layout class="QVBoxLayout" name="verticalLayout_13">
@@ -52,21 +52,21 @@
            </property>
           </widget>
          </item>
-         <item row="8" column="0">
+         <item row="9" column="0">
           <widget class="QCheckBox" name="checkBox_regenerate">
            <property name="text">
             <string/>
            </property>
           </widget>
          </item>
-         <item row="9" column="0">
+         <item row="10" column="0">
           <widget class="QCheckBox" name="checkBox_filtering">
            <property name="text">
             <string/>
            </property>
           </widget>
          </item>
-         <item row="8" column="1">
+         <item row="9" column="1">
           <widget class="QLabel" name="label_regenerate">
            <property name="text">
             <string>Regenerate clouds. This can be used to regenerate the point clouds at higher density than those used for online visualization.</string>
@@ -76,7 +76,14 @@
            </property>
           </widget>
          </item>
-         <item row="7" column="1">
+         <item row="12" column="0">
+          <widget class="QCheckBox" name="checkBox_gainCompensation">
+           <property name="text">
+            <string/>
+           </property>
+          </widget>
+         </item>
+         <item row="8" column="1">
           <widget class="QLabel" name="label_voxel">
            <property name="text">
             <string>Voxel size. Set 0 to disable. When organized meshes are assembled, this is the radius in which the vertices of the polygons are merged.</string>
@@ -86,13 +93,6 @@
            </property>
           </widget>
          </item>
-         <item row="11" column="0">
-          <widget class="QCheckBox" name="checkBox_gainCompensation">
-           <property name="text">
-            <string/>
-           </property>
-          </widget>
-         </item>
          <item row="4" column="1">
           <widget class="QLabel" name="label_binaryFile_2">
            <property name="text">
@@ -103,7 +103,7 @@
            </property>
           </widget>
          </item>
-         <item row="11" column="1">
+         <item row="12" column="1">
           <widget class="QLabel" name="label_gainCompensation">
            <property name="text">
             <string>Gain compensation. Normalize brightness of images.</string>
@@ -133,7 +133,7 @@
            </property>
           </widget>
          </item>
-         <item row="12" column="1">
+         <item row="13" column="1">
           <widget class="QLabel" name="label_binaryFile_12">
            <property name="text">
             <string>Meshing.</string>
@@ -143,7 +143,14 @@
            </property>
           </widget>
          </item>
-         <item row="9" column="1">
+         <item row="11" column="0">
+          <widget class="QCheckBox" name="checkBox_smoothing">
+           <property name="text">
+            <string/>
+           </property>
+          </widget>
+         </item>
+         <item row="10" column="1">
           <widget class="QLabel" name="label_binaryFile_9">
            <property name="text">
             <string>Cloud filtering. Remove sparse points that are far from surfaces.</string>
@@ -153,8 +160,8 @@
            </property>
           </widget>
          </item>
-         <item row="10" column="0">
+         <item row="13" column="0">
-          <widget class="QCheckBox" name="checkBox_smoothing">
+          <widget class="QCheckBox" name="checkBox_meshing">
            <property name="text">
             <string/>
            </property>
@@ -170,14 +177,7 @@
            </property>
           </widget>
          </item>
-         <item row="12" column="0">
+         <item row="11" column="1">
-          <widget class="QCheckBox" name="checkBox_meshing">
-           <property name="text">
-            <string/>
-           </property>
-          </widget>
-         </item>
-         <item row="10" column="1">
           <widget class="QLabel" name="label_binaryFile_10">
            <property name="text">
             <string>Cloud smoothing using Moving Least Squares algorithm (MLS).</string>
@@ -187,7 +187,7 @@
            </property>
           </widget>
          </item>
-         <item row="7" column="0">
+         <item row="8" column="0">
           <widget class="QDoubleSpinBox" name="doubleSpinBox_voxelSize_assembled">
            <property name="suffix">
             <string> m</string>
@@ -206,6 +206,13 @@
            </property>
           </widget>
          </item>
+         <item row="4" column="0">
+          <widget class="QCheckBox" name="checkBox_assemble">
+           <property name="text">
+            <string/>
+           </property>
+          </widget>
+         </item>
          <item row="6" column="1">
           <widget class="QLabel" name="label_normal">
            <property name="text">
@@ -216,23 +223,6 @@
            </property>
           </widget>
          </item>
-         <item row="3" column="0">
-          <widget class="QCheckBox" name="checkBox_binary">
-           <property name="text">
-            <string/>
-           </property>
-           <property name="checked">
-            <bool>true</bool>
-           </property>
-          </widget>
-         </item>
-         <item row="4" column="0">
-          <widget class="QCheckBox" name="checkBox_assemble">
-           <property name="text">
-            <string/>
-           </property>
-          </widget>
-         </item>
          <item row="5" column="0">
           <widget class="QComboBox" name="comboBox_frame">
            <item>
@@ -257,6 +247,16 @@
            </item>
           </widget>
          </item>
+         <item row="3" column="0">
+          <widget class="QCheckBox" name="checkBox_binary">
+           <property name="text">
+            <string/>
+           </property>
+           <property name="checked">
+            <bool>true</bool>
+           </property>
+          </widget>
+         </item>
          <item row="0" column="1">
           <widget class="QLabel" name="label_binaryFile_11">
            <property name="text">
@@ -277,6 +277,23 @@
            </property>
           </widget>
          </item>
+         <item row="7" column="1">
+          <widget class="QLabel" name="label_normal_2">
+           <property name="text">
+            <string>Set the search radius for the normal estimation.</string>
+           </property>
+           <property name="wordWrap">
+            <bool>true</bool>
+           </property>
+          </widget>
+         </item>
+         <item row="7" column="0">
+          <widget class="QDoubleSpinBox" name="doubleSpinBox_normalRadiusSearch">
+           <property name="singleStep">
+            <double>0.010000000000000</double>
+           </property>
+          </widget>
+         </item>
         </layout>
        </item>
        <item>

guilib/src/ui/preferencesDialog.ui

@@ -63,7 +63,7 @@
           <property name="geometry">
            <rect>
             <x>0</x>
-            <y>0</y>
+            <y>-831</y>
             <width>678</width>
             <height>2739</height>
            </rect>
@@ -95,7 +95,7 @@
                 <enum>QFrame::Raised</enum>
                </property>
                <property name="currentIndex">
-                <number>21</number>
+                <number>1</number>
                </property>
                <widget class="QWidget" name="page_22">
                 <layout class="QVBoxLayout" name="verticalLayout_29" stretch="0,1">
@@ -512,6 +512,31 @@ Show a yellow background when the number of odometry inliers goes under this thr
                       <layout class="QVBoxLayout" name="verticalLayout_112">
                        <item>
                         <layout class="QGridLayout" name="gridLayout_2" columnstretch="0,0,1">
+                         <item row="3" column="1">
+                          <widget class="QDoubleSpinBox" name="doubleSpinBox_maxDepth_odom">
+                           <property name="suffix">
+                            <string> m</string>
+                           </property>
+                           <property name="decimals">
+                            <number>1</number>
+                           </property>
+                           <property name="maximum">
+                            <double>100.000000000000000</double>
+                           </property>
+                           <property name="singleStep">
+                            <double>0.100000000000000</double>
+                           </property>
+                           <property name="value">
+                            <double>0.000000000000000</double>
+                           </property>
+                          </widget>
+                         </item>
+                         <item row="5" column="1">
+                          <widget class="QLineEdit" name="lineEdit_roiRatios_odom"/>
+                         </item>
+                         <item row="5" column="0">
+                          <widget class="QLineEdit" name="lineEdit_roiRatios"/>
+                         </item>
                          <item row="0" column="0">
                           <widget class="QLabel" name="label_154">
                            <property name="text">
@@ -597,7 +622,7 @@ Show a yellow background when the number of odometry inliers goes under this thr
                            </property>
                           </widget>
                          </item>
-                         <item row="12" column="0">
+                         <item row="13" column="0">
                           <widget class="QDoubleSpinBox" name="doubleSpinBox_opacity">
                            <property name="suffix">
                             <string/>
@@ -616,7 +641,7 @@ Show a yellow background when the number of odometry inliers goes under this thr
                            </property>
                           </widget>
                          </item>
-                         <item row="12" column="1">
+                         <item row="13" column="1">
                           <widget class="QDoubleSpinBox" name="doubleSpinBox_opacity_odom">
                            <property name="suffix">
                             <string/>
@@ -635,7 +660,7 @@ Show a yellow background when the number of odometry inliers goes under this thr
                            </property>
                           </widget>
                          </item>
-                         <item row="12" column="2">
+                         <item row="13" column="2">
                           <widget class="QLabel" name="label_155">
                            <property name="text">
                             <string>Opacity.</string>
@@ -648,7 +673,7 @@ Show a yellow background when the number of odometry inliers goes under this thr
                            </property>
                           </widget>
                          </item>
-                         <item row="13" column="2">
+                         <item row="14" column="2">
                           <widget class="QLabel" name="label_157">
                            <property name="text">
                             <string>Point size (1..64).</string>
@@ -712,25 +737,6 @@ Show a yellow background when the number of odometry inliers goes under this thr
                            </property>
                           </widget>
                          </item>
-                         <item row="3" column="1">
-                          <widget class="QDoubleSpinBox" name="doubleSpinBox_maxDepth_odom">
-                           <property name="suffix">
-                            <string> m</string>
-                           </property>
-                           <property name="decimals">
-                            <number>1</number>
-                           </property>
-                           <property name="maximum">
-                            <double>100.000000000000000</double>
-                           </property>
-                           <property name="singleStep">
-                            <double>0.100000000000000</double>
-                           </property>
-                           <property name="value">
-                            <double>0.000000000000000</double>
-                           </property>
-                          </widget>
-                         </item>
                          <item row="3" column="2">
                           <widget class="QLabel" name="label_132">
                            <property name="text">
@@ -798,12 +804,6 @@ Show a yellow background when the number of odometry inliers goes under this thr
                            </property>
                           </widget>
                          </item>
-                         <item row="5" column="0">
-                          <widget class="QLineEdit" name="lineEdit_roiRatios"/>
-                         </item>
-                         <item row="5" column="1">
-                          <widget class="QLineEdit" name="lineEdit_roiRatios_odom"/>
-                         </item>
                          <item row="5" column="2">
                           <widget class="QLabel" name="label_353">
                            <property name="text">
@@ -858,7 +858,7 @@ Show a yellow background when the number of odometry inliers goes under this thr
                            </property>
                           </widget>
                          </item>
-                         <item row="13" column="0">
+                         <item row="14" column="0">
                           <widget class="QSpinBox" name="spinBox_ptsize">
                            <property name="minimum">
                             <number>1</number>
@@ -871,7 +871,7 @@ Show a yellow background when the number of odometry inliers goes under this thr
                            </property>
                           </widget>
                          </item>
-                         <item row="13" column="1">
+                         <item row="14" column="1">
                           <widget class="QSpinBox" name="spinBox_ptsize_odom">
                            <property name="minimum">
                             <number>1</number>
@@ -1010,6 +1010,26 @@ Show a yellow background when the number of odometry inliers goes under this thr
                            </property>
                           </widget>
                          </item>
+                         <item row="12" column="2">
+                          <widget class="QLabel" name="label_427">
+                           <property name="text">
+                            <string>Normal radius search. If not 0, normals will be computed and added to created cloud for visualization (keys  7, 8 and 9).</string>
+                           </property>
+                           <property name="wordWrap">
+                            <bool>true</bool>
+                           </property>
+                           <property name="textInteractionFlags">
+                            <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                           </property>
+                          </widget>
+                         </item>
+                         <item row="12" column="0">
+                          <widget class="QDoubleSpinBox" name="doubleSpinBox_normalRadiusSearch">
+                           <property name="singleStep">
+                            <double>0.010000000000000</double>
+                           </property>
+                          </widget>
+                         </item>
                         </layout>
                        </item>
                        <item>
@@ -1117,7 +1137,7 @@ Show a yellow background when the number of odometry inliers goes under this thr
                        <string>Laser Scan</string>
                       </property>
                       <layout class="QGridLayout" name="gridLayout_79" columnstretch="0,0,1">
-                       <item row="8" column="1">
+                       <item row="9" column="1">
                         <widget class="QSpinBox" name="spinBox_ptsize_odom_scan">
                          <property name="minimum">
                           <number>1</number>
@@ -1127,7 +1147,7 @@ Show a yellow background when the number of odometry inliers goes under this thr
                          </property>
                         </widget>
                        </item>
-                       <item row="8" column="2">
+                       <item row="9" column="2">
                         <widget class="QLabel" name="label_158">
                          <property name="text">
                           <string>Scan point size (1..64).</string>
@@ -1169,7 +1189,7 @@ Show a yellow background when the number of odometry inliers goes under this thr
                          </property>
                         </widget>
                        </item>
-                       <item row="7" column="2">
+                       <item row="8" column="2">
                         <widget class="QLabel" name="label_156">
                          <property name="text">
                           <string>Scan opacity.</string>
@@ -1195,7 +1215,7 @@ Show a yellow background when the number of odometry inliers goes under this thr
                          </property>
                         </widget>
                        </item>
-                       <item row="8" column="0">
+                       <item row="9" column="0">
                         <widget class="QSpinBox" name="spinBox_ptsize_scan">
                          <property name="minimum">
                           <number>1</number>
@@ -1235,7 +1255,7 @@ Show a yellow background when the number of odometry inliers goes under this thr
                          </property>
                         </widget>
                        </item>
-                       <item row="7" column="0">
+                       <item row="8" column="0">
                         <widget class="QDoubleSpinBox" name="doubleSpinBox_opacity_scan">
                          <property name="suffix">
                           <string/>
@@ -1267,7 +1287,7 @@ Show a yellow background when the number of odometry inliers goes under this thr
                          </property>
                         </widget>
                        </item>
-                       <item row="7" column="1">
+                       <item row="8" column="1">
                         <widget class="QDoubleSpinBox" name="doubleSpinBox_opacity_odom_scan">
                          <property name="suffix">
                           <string/>
@@ -1440,6 +1460,26 @@ Show a yellow background when the number of odometry inliers goes under this thr
                          </property>
                         </widget>
                        </item>
+                       <item row="7" column="2">
+                        <widget class="QLabel" name="label_428">
+                         <property name="text">
+                          <string>Normal radius search. If not 0, normals will be computed and added to created cloud for visualization (keys  7, 8 and 9).</string>
+                         </property>
+                         <property name="wordWrap">
+                          <bool>true</bool>
+                         </property>
+                         <property name="textInteractionFlags">
+                          <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                         </property>
+                        </widget>
+                       </item>
+                       <item row="7" column="0">
+                        <widget class="QDoubleSpinBox" name="doubleSpinBox_normalRadiusSearch_scan">
+                         <property name="singleStep">
+                          <double>0.010000000000000</double>
+                         </property>
+                        </widget>
+                       </item>
                       </layout>
                      </widget>
                     </item>
@@ -5283,6 +5323,29 @@ 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_425">
+                           <property name="text">
+                            <string>Search radius for normals computation (0=disabled). 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>
+                         <item row="3" column="0">
+                          <widget class="QDoubleSpinBox" name="doubleSpinBox_cameraImages_scanNormalsRadius">
+                           <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="singleStep">
+                            <double>0.010000000000000</double>
+                           </property>
+                          </widget>
+                         </item>
                         </layout>
                        </item>
                       </layout>
@@ -5995,6 +6058,16 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                    <layout class="QVBoxLayout" name="verticalLayout_10">
                     <item>
                      <layout class="QGridLayout" name="gridLayout_42" columnstretch="0,1">
+                      <item row="15" column="0">
+                       <widget class="QSpinBox" name="spinBox_imagePostDecimation">
+                        <property name="minimum">
+                         <number>-16</number>
+                        </property>
+                        <property name="maximum">
+                         <number>16</number>
+                        </property>
+                       </widget>
+                      </item>
                       <item row="0" column="0">
                        <widget class="QSpinBox" name="general_spinBox_maxStMemSize">
                         <property name="minimum">
@@ -6066,19 +6139,6 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                         </property>
                        </widget>
                       </item>
-                      <item row="6" column="1">
-                       <widget class="QLabel" name="label_retrieved_3">
-                        <property name="text">
-                         <string>Bad signatures are ignored.</string>
-                        </property>
-                        <property name="wordWrap">
-                         <bool>true</bool>
-                        </property>
-                        <property name="textInteractionFlags">
-                         <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
-                        </property>
-                       </widget>
-                      </item>
                       <item row="8" column="0">
                        <widget class="QCheckBox" name="general_checkBox_initWMWithAllNodes">
                         <property name="text">
@@ -6089,10 +6149,10 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                         </property>
                        </widget>
                       </item>
-                      <item row="9" column="1">
+                      <item row="6" column="1">
-                       <widget class="QLabel" name="label_retrieved_5">
+                       <widget class="QLabel" name="label_retrieved_3">
                         <property name="text">
-                         <string>Keep raw sensor data. Only useful to save loop closure computation time when features re-extraction is enabled. Disable to save RAM memory.</string>
+                         <string>Bad signatures are ignored.</string>
                         </property>
                         <property name="wordWrap">
                          <bool>true</bool>
@@ -6122,10 +6182,10 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                         </property>
                        </widget>
                       </item>
-                      <item row="5" column="1">
+                      <item row="9" column="1">
-                       <widget class="QLabel" name="label_retrieved_11">
+                       <widget class="QLabel" name="label_retrieved_5">
                         <property name="text">
-                         <string>Create map labels. The first node of a map will be labelled as &quot;map#&quot; where # is the map ID.</string>
+                         <string>Keep raw sensor data. Only useful to save loop closure computation time when features re-extraction is enabled. Disable to save RAM memory.</string>
                         </property>
                         <property name="wordWrap">
                          <bool>true</bool>
@@ -6145,6 +6205,19 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                         </property>
                        </widget>
                       </item>
+                      <item row="5" column="1">
+                       <widget class="QLabel" name="label_retrieved_11">
+                        <property name="text">
+                         <string>Create map labels. The first node of a map will be labelled as &quot;map#&quot; where # is the map ID.</string>
+                        </property>
+                        <property name="wordWrap">
+                         <bool>true</bool>
+                        </property>
+                        <property name="textInteractionFlags">
+                         <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                        </property>
+                       </widget>
+                      </item>
                       <item row="16" column="0">
                        <widget class="QSpinBox" name="general_spinBox_laserScanDownsample">
                         <property name="minimumSize">
@@ -6161,6 +6234,16 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                         </property>
                        </widget>
                       </item>
+                      <item row="11" column="0">
+                       <widget class="QCheckBox" name="general_checkBox_keepDescriptors">
+                        <property name="text">
+                         <string/>
+                        </property>
+                        <property name="checked">
+                         <bool>false</bool>
+                        </property>
+                       </widget>
+                      </item>
                       <item row="11" column="1">
                        <widget class="QLabel" name="label_retrieved_12">
                         <property name="text">
@@ -6174,16 +6257,6 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                         </property>
                        </widget>
                       </item>
-                      <item row="11" column="0">
-                       <widget class="QCheckBox" name="general_checkBox_keepDescriptors">
-                        <property name="text">
-                         <string/>
-                        </property>
-                        <property name="checked">
-                         <bool>false</bool>
-                        </property>
-                       </widget>
-                      </item>
                       <item row="14" column="1">
                        <widget class="QLabel" name="label_retrieved_13">
                         <property name="text">
@@ -6256,13 +6329,13 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                         </property>
                        </widget>
                       </item>
-                      <item row="15" column="0">
+                      <item row="6" column="0">
-                       <widget class="QSpinBox" name="spinBox_imagePostDecimation">
+                       <widget class="QCheckBox" name="general_checkBox_badSignaturesIgnored">
-                        <property name="minimum">
+                        <property name="text">
-                         <number>-16</number>
+                         <string/>
                         </property>
-                        <property name="maximum">
+                        <property name="checked">
-                         <number>16</number>
+                         <bool>false</bool>
                         </property>
                        </widget>
                       </item>
@@ -6292,16 +6365,6 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                         </property>
                        </widget>
                       </item>
-                      <item row="6" column="0">
-                       <widget class="QCheckBox" name="general_checkBox_badSignaturesIgnored">
-                        <property name="text">
-                         <string/>
-                        </property>
-                        <property name="checked">
-                         <bool>false</bool>
-                        </property>
-                       </widget>
-                      </item>
                       <item row="15" column="1">
                        <widget class="QLabel" name="label_retrieved_6">
                         <property name="text">
@@ -6315,6 +6378,16 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                         </property>
                        </widget>
                       </item>
+                      <item row="12" column="0">
+                       <widget class="QCheckBox" name="general_checkBox_saveDepth16bits">
+                        <property name="text">
+                         <string/>
+                        </property>
+                        <property name="checked">
+                         <bool>false</bool>
+                        </property>
+                       </widget>
+                      </item>
                       <item row="3" column="1">
                        <widget class="QLabel" name="label_retrieved_7">
                         <property name="text">
@@ -6341,16 +6414,6 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                         </property>
                        </widget>
                       </item>
-                      <item row="12" column="0">
-                       <widget class="QCheckBox" name="general_checkBox_saveDepth16bits">
-                        <property name="text">
-                         <string/>
-                        </property>
-                        <property name="checked">
-                         <bool>false</bool>
-                        </property>
-                       </widget>
-                      </item>
                       <item row="14" column="0">
                        <widget class="QSpinBox" name="spinBox_imagePreDecimation">
                         <property name="minimum">
@@ -6380,10 +6443,20 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                         </property>
                        </widget>
                       </item>
+                      <item row="13" column="0">
+                       <widget class="QCheckBox" name="general_checkBox_compressionParallelized">
+                        <property name="text">
+                         <string/>
+                        </property>
+                        <property name="checked">
+                         <bool>false</bool>
+                        </property>
+                       </widget>
+                      </item>
                       <item row="17" column="1">
                        <widget class="QLabel" name="label_retrieved_14">
                         <property name="text">
-                         <string>If &gt; 0 and laser scans are 3D without normals, normals will be computed with K search neighbors when creating a signature.</string>
+                         <string>If &gt; 0 and laser scans don't have normals, normals will be computed with K search neighbors when creating a signature.</string>
                         </property>
                         <property name="wordWrap">
                          <bool>true</bool>
@@ -6406,8 +6479,8 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                         </property>
                        </widget>
                       </item>
-                      <item row="13" column="0">
+                      <item row="10" column="0">
-                       <widget class="QCheckBox" name="general_checkBox_compressionParallelized">
+                       <widget class="QCheckBox" name="general_checkBox_saveIntermediateNodeData">
                         <property name="text">
                          <string/>
                         </property>
@@ -6429,13 +6502,29 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                         </property>
                        </widget>
                       </item>
-                      <item row="10" column="0">
+                      <item row="18" column="1">
-                       <widget class="QCheckBox" name="general_checkBox_saveIntermediateNodeData">
+                       <widget class="QLabel" name="label_retrieved_17">
                         <property name="text">
-                         <string/>
+                         <string>If &gt; 0 and laser scans don't have normals, normals will be computed with radius search when creating a signature.</string>
                         </property>
-                        <property name="checked">
+                        <property name="wordWrap">
-                         <bool>false</bool>
+                         <bool>true</bool>
+                        </property>
+                        <property name="textInteractionFlags">
+                         <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                        </property>
+                       </widget>
+                      </item>
+                      <item row="18" column="0">
+                       <widget class="QDoubleSpinBox" name="general_doubleSpinBox_laserScanNormalRadius">
+                        <property name="minimumSize">
+                         <size>
+                          <width>50</width>
+                          <height>0</height>
+                         </size>
+                        </property>
+                        <property name="singleStep">
+                         <double>0.010000000000000</double>
                         </property>
                        </widget>
                       </item>
@@ -13764,6 +13853,26 @@ Lower the ratio -&gt; higher the precision. 0 means disabled, matching the neare
                         </property>
                        </widget>
                       </item>
+                      <item row="10" column="1">
+                       <widget class="QLabel" name="label_426">
+                        <property name="text">
+                         <string>Search radius 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>
+                        </property>
+                        <property name="textInteractionFlags">
+                         <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                        </property>
+                       </widget>
+                      </item>
+                      <item row="10" column="0">
+                       <widget class="QDoubleSpinBox" name="loopClosure_icpPointToPlaneNormalsRadius">
+                        <property name="singleStep">
+                         <double>0.010000000000000</double>
+                        </property>
+                       </widget>
+                      </item>
                      </layout>
                     </item>
                     <item>

package.xml

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

tools/KittiDataset/main.cpp

@@ -60,6 +60,7 @@ void showUsage()
 			"  --scan_step #      Scan downsample step (default=10).\n"
 			"  --scan_voxel #.#   Scan voxel size (default 0.3 m).\n"
 			"  --scan_k           Scan normal K (default 20).\n"
+			"  --scan_radius      Scan normal radius (default 0).\n"
 			"  --map_update  #    Do map update each X odometry frames (default=10, which\n"
 			"                        gives 1 Hz map update assuming images are at 10 Hz).\n\n"
 			"%s\n"
@@ -104,6 +105,7 @@ int main(int argc, char * argv[])
 	int scanStep = 10;
 	float scanVoxel = 0.3f;
 	int scanNormalK = 20;
+	float scanNormalRadius = 0.0f;
 	std::string gtPath;
 	if(argc < 2)
 	{
@@ -153,6 +155,15 @@ int main(int argc, char * argv[])
 					showUsage();
 				}
 			}
+			else if(std::strcmp(argv[i], "--scan_radius") == 0)
+			{
+				scanNormalRadius = atof(argv[++i]);
+				if(scanNormalRadius < 0.0f)
+				{
+					printf("scanNormalRadius should be >= 0\n");
+					showUsage();
+				}
+			}
 			else if(std::strcmp(argv[i], "--gt") == 0)
 			{
 				gtPath = argv[++i];
@@ -233,10 +244,11 @@ int main(int argc, char * argv[])
 	if(scan)
 	{
 		pathScan = path+"/velodyne";
-		printf("   Scan:              %s\n", pathScan.c_str());
+		printf("   Scan:               %s\n", pathScan.c_str());
-		printf("   Scan step:         %d\n", scanStep);
+		printf("   Scan step:          %d\n", scanStep);
-		printf("   Scan voxel:        %fm\n", scanVoxel);
+		printf("   Scan voxel:         %fm\n", scanVoxel);
-		printf("   Scan normal k:     %d\n", scanNormalK);
+		printf("   Scan normal k:      %d\n", scanNormalK);
+		printf("   Scan normal radius: %f\n", scanNormalRadius);
 	}
 	if(!parameters.empty())
 	{
@@ -338,6 +350,7 @@ int main(int argc, char * argv[])
 						scanStep,
 						scanVoxel,
 						scanNormalK,
+						scanNormalRadius,
 						Transform(-0.27f, 0.0f, 0.08, 0.0f, 0.0f, 0.0f));
 	}
 

tools/OdometryViewer/main.cpp

@@ -191,6 +191,7 @@ int main (int argc, char * argv[])
 	float maxDepth = 4.0f;
 	float voxelSize = rtabmap::Parameters::defaultIcpVoxelSize();
 	int normalsK = 0;
+	float normalsRadius = 0.0f;
 	if(regStrategy == 1 || regStrategy == 2)
 	{
 		// icp requires scans
@@ -203,8 +204,10 @@ int main (int argc, char * argv[])
 		rtabmap::Parameters::parse(parameters, rtabmap::Parameters::kIcpPointToPlane(), pointToPlane);
 		if(pointToPlane)
 		{
-			normalsK = rtabmap::Parameters::defaultIcpPointToPlaneNormalNeighbors();
+			normalsK = rtabmap::Parameters::defaultIcpPointToPlaneK();
-			rtabmap::Parameters::parse(parameters, rtabmap::Parameters::kIcpPointToPlaneNormalNeighbors(), normalsK);
+			rtabmap::Parameters::parse(parameters, rtabmap::Parameters::kIcpPointToPlaneK(), normalsK);
+			normalsRadius = rtabmap::Parameters::defaultIcpPointToPlaneRadius();
+			rtabmap::Parameters::parse(parameters, rtabmap::Parameters::kIcpPointToPlaneRadius(), normalsRadius);
 		}
 
 		uInsert(parameters, rtabmap::ParametersPair(rtabmap::Parameters::kIcpDownsamplingStep(), "1"));
@@ -310,7 +313,7 @@ int main (int argc, char * argv[])
 		{
 			rtabmap::CameraThread cameraThread(camera, parameters);
 
-			cameraThread.setScanFromDepth(icp, decimation<1?1:decimation, maxDepth, voxelSize, normalsK);
+			cameraThread.setScanFromDepth(icp, decimation<1?1:decimation, maxDepth, voxelSize, normalsK, normalsRadius);
 
 			odomThread.start();
 			cameraThread.start();