Increased version to 0.20.7. OdometryF2M: added support for intensity field, removed ignored key frames when there is low scan complexity. RegistrationIcp: added Icp/PMMatcherIntensity, Icp/PointToPlaneGroundNormalsUp and Icp/PointToPlaneLowComplexityStrategy parameters. Rtabmap: when graph optimized from end, increased optimization error before warning that resulting map correction is not identity (this could happen with GTSAM as the root is not perfectly fixed). CloudViewer: added coordinate frame scaling option, added rainbow colormap option for scan intensity. DBViewer: fixed local proximity merged scans not shown modified after refining those links, show intensity, fixed constraints view not updated after rejecting a link. MainWindow: added intesity support with odometry scans.

2026-09-02 09:30:25 +08:00 · 2020-11-28 17:28:34 -05:00
parent 7859313beb
commit d733029565
24 changed files with 1032 additions and 237 deletions
--- a/corelib/src/CameraThread.cpp
+++ b/corelib/src/CameraThread.cpp
@@ -129,6 +129,39 @@ void CameraThread::disableIMUFiltering()
 	_imuFilter = 0;
 }

+void CameraThread::setScanParameters(
+	bool fromDepth,
+	int downsampleStep,
+	float rangeMin,
+	float rangeMax,
+	float voxelSize,
+	int normalsK,
+	int normalsRadius,
+	bool forceGroundNormalsUp)
+{
+	setScanParameters(fromDepth, downsampleStep, rangeMin, rangeMax, voxelSize, normalsK, normalsRadius, forceGroundNormalsUp?0.8f:0.0f);
+}
+
+void CameraThread::setScanParameters(
+			bool fromDepth,
+			int downsampleStep, // decimation of the depth image in case the scan is from depth image
+			float rangeMin,
+			float rangeMax,
+			float voxelSize,
+			int normalsK,
+			int normalsRadius,
+			float groundNormalsUp)
+{
+	_scanFromDepth = fromDepth;
+	_scanDownsampleStep=downsampleStep;
+	_scanRangeMin = rangeMin;
+	_scanRangeMax = rangeMax;
+	_scanVoxelSize = voxelSize;
+	_scanNormalsK = normalsK;
+	_scanNormalsRadius = normalsRadius;
+	_scanForceGroundNormalsUp = groundNormalsUp;
+}
+
 void CameraThread::mainLoopBegin()
 {
 	ULogger::registerCurrentThread("Camera");
--- a/corelib/src/Odometry.cpp
+++ b/corelib/src/Odometry.cpp
@@ -308,8 +308,11 @@ Transform Odometry::process(SensorData & data, const Transform & guessIn, Odomet
 			}
 			if(stereoModel_.isRectificationMapInitialized())
 			{
-				data.setImageRaw(stereoModel_.left().rectifyImage(data.imageRaw()));
-				data.setDepthOrRightRaw(stereoModel_.right().rectifyImage(data.rightRaw()));
+				data.setStereoImage(
+						stereoModel_.left().rectifyImage(data.imageRaw()),
+						stereoModel_.right().rectifyImage(data.rightRaw()),
+						stereoModel_,
+						false);
 			}
 		}
 		else
--- a/corelib/src/RegistrationIcp.cpp
+++ b/corelib/src/RegistrationIcp.cpp
@@ -43,6 +43,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #ifdef RTABMAP_POINTMATCHER
 #include <fstream>
 #include "pointmatcher/PointMatcher.h"
+#include "nabo/nabo.h"
 typedef PointMatcher<float> PM;
 typedef PM::DataPoints DP;

@@ -182,6 +183,7 @@ DP laserScanToDP(const rtabmap::LaserScan & scan)
 		descLabels.push_back(Label("intensity", 1));
 	}

+
 	// create cloud
 	DP cloud(featLabels, descLabels, scan.size());
 	cloud.getFeatureViewByName("pad").setConstant(1);
@@ -360,6 +362,110 @@ typename PointMatcher<T>::TransformationParameters eigenMatrixToDim(const typena
 	return out;
 }

+template<typename T>
+struct KDTreeMatcherIntensity : public PointMatcher<T>::Matcher
+{
+	typedef PointMatcherSupport::Parametrizable Parametrizable;
+	typedef PointMatcherSupport::Parametrizable P;
+	typedef Parametrizable::Parameters Parameters;
+	typedef Parametrizable::ParameterDoc ParameterDoc;
+	typedef Parametrizable::ParametersDoc ParametersDoc;
+
+	typedef typename Nabo::NearestNeighbourSearch<T> NNS;
+	typedef typename NNS::SearchType NNSearchType;
+
+	typedef typename PointMatcher<T>::DataPoints DataPoints;
+	typedef typename PointMatcher<T>::Matcher Matcher;
+	typedef typename PointMatcher<T>::Matches Matches;
+	typedef typename PointMatcher<T>::Matrix Matrix;
+
+	inline static const std::string description()
+	{
+		return "This matcher matches a point from the reading to its closest neighbors in the reference.";
+	}
+	inline static const ParametersDoc availableParameters()
+	{
+		return {
+			{"knn", "number of nearest neighbors to consider it the reference", "1", "1", "2147483647", &P::Comp<unsigned>},
+			{"epsilon", "approximation to use for the nearest-neighbor search", "0", "0", "inf", &P::Comp<T>},
+			{"searchType", "Nabo search type. 0: brute force, check distance to every point in the data (very slow), 1: kd-tree with linear heap, good for small knn (~up to 30) and 2: kd-tree with tree heap, good for large knn (~from 30)", "1", "0", "2", &P::Comp<unsigned>},
+			{"maxDist", "maximum distance to consider for neighbors", "inf", "0", "inf", &P::Comp<T>}
+		};
+	}
+
+	const int knn;
+	const T epsilon;
+	const NNSearchType searchType;
+	const T maxDist;
+
+protected:
+	std::shared_ptr<NNS> featureNNS;
+	Matrix filteredReferenceIntensity;
+
+public:
+	KDTreeMatcherIntensity(const Parameters& params = Parameters()) :
+		PointMatcher<T>::Matcher("KDTreeMatcherIntensity", KDTreeMatcherIntensity::availableParameters(), params),
+		knn(Parametrizable::get<int>("knn")),
+		epsilon(Parametrizable::get<T>("epsilon")),
+		searchType(NNSearchType(Parametrizable::get<int>("searchType"))),
+		maxDist(Parametrizable::get<T>("maxDist"))
+	{
+		UINFO("* KDTreeMatcherIntensity: initialized with knn=%d, epsilon=%f, searchType=%d and maxDist=%f", knn, epsilon, searchType, maxDist);
+	}
+	virtual ~KDTreeMatcherIntensity() {}
+	virtual void init(const DataPoints& filteredReference)
+	{
+		// build and populate NNS
+		if(knn>1)
+		{
+			filteredReferenceIntensity = filteredReference.getDescriptorCopyByName("intensity");
+		}
+		else
+		{
+			UWARN("KDTreeMatcherIntensity: knn is not over 1 (%d), intensity re-ordering will be ignored.", knn);
+		}
+		featureNNS.reset( NNS::create(filteredReference.features, filteredReference.features.rows() - 1, searchType, NNS::TOUCH_STATISTICS));
+	}
+	virtual PM::Matches findClosests(const DP& filteredReading)
+	{
+		const int pointsCount(filteredReading.features.cols());
+		Matches matches(
+			typename Matches::Dists(knn, pointsCount),
+			typename Matches::Ids(knn, pointsCount)
+		);
+
+		const BOOST_AUTO(filteredReadingIntensity, filteredReading.getDescriptorViewByName("intensity"));
+
+		static_assert(NNS::InvalidIndex == PM::Matches::InvalidId, "");
+		static_assert(NNS::InvalidValue == PM::Matches::InvalidDist, "");
+		this->visitCounter += featureNNS->knn(filteredReading.features, matches.ids, matches.dists, knn, epsilon, NNS::ALLOW_SELF_MATCH, maxDist);
+
+		if(knn > 1)
+		{
+			Matches matchesOrderedByIntensity(
+				typename Matches::Dists(1, pointsCount),
+				typename Matches::Ids(1, pointsCount)
+			);
+			#pragma omp parallel for
+			for (int i = 0; i < pointsCount; ++i)
+			{
+				float minDistance = std::numeric_limits<float>::max();
+				for(int k=0; k<knn && k<filteredReferenceIntensity.rows(); ++k)
+				{
+					float distIntensity = fabs(filteredReadingIntensity(0,i) - filteredReferenceIntensity(0, matches.ids.coeff(k, i)));
+					if(distIntensity < minDistance)
+					{
+						matchesOrderedByIntensity.ids.coeffRef(0, i) = matches.ids.coeff(k, i);
+						matchesOrderedByIntensity.dists.coeffRef(0, i) = matches.dists.coeff(k, i);
+						minDistance = distIntensity;
+					}
+				}
+			}
+			matches = matchesOrderedByIntensity;
+		}
+		return matches;
+	}
+};
 #endif

 namespace rtabmap {
@@ -379,11 +485,14 @@ RegistrationIcp::RegistrationIcp(const ParametersMap & parameters, Registration
 	_pointToPlane(Parameters::defaultIcpPointToPlane()),
 	_pointToPlaneK(Parameters::defaultIcpPointToPlaneK()),
 	_pointToPlaneRadius(Parameters::defaultIcpPointToPlaneRadius()),
+	_pointToPlaneGroundNormalsUp(Parameters::defaultIcpPointToPlaneGroundNormalsUp()),
 	_pointToPlaneMinComplexity(Parameters::defaultIcpPointToPlaneMinComplexity()),
+	_pointToPlaneLowComplexityStrategy(Parameters::defaultIcpPointToPlaneLowComplexityStrategy()),
 	_libpointmatcher(Parameters::defaultIcpPM()),
 	_libpointmatcherConfig(Parameters::defaultIcpPMConfig()),
 	_libpointmatcherKnn(Parameters::defaultIcpPMMatcherKnn()),
 	_libpointmatcherEpsilon(Parameters::defaultIcpPMMatcherEpsilon()),
+	_libpointmatcherIntensity(Parameters::defaultIcpPMMatcherIntensity()),
 	_libpointmatcherOutlierRatio(Parameters::defaultIcpPMOutlierRatio()),
 	_libpointmatcherICP(0)
 {
@@ -414,7 +523,10 @@ void RegistrationIcp::parseParameters(const ParametersMap & parameters)
 	Parameters::parse(parameters, Parameters::kIcpPointToPlane(), _pointToPlane);
 	Parameters::parse(parameters, Parameters::kIcpPointToPlaneK(), _pointToPlaneK);
 	Parameters::parse(parameters, Parameters::kIcpPointToPlaneRadius(), _pointToPlaneRadius);
+	Parameters::parse(parameters, Parameters::kIcpPointToPlaneGroundNormalsUp(), _pointToPlaneGroundNormalsUp);
 	Parameters::parse(parameters, Parameters::kIcpPointToPlaneMinComplexity(), _pointToPlaneMinComplexity);
+	Parameters::parse(parameters, Parameters::kIcpPointToPlaneLowComplexityStrategy(), _pointToPlaneLowComplexityStrategy);
+	UASSERT(_pointToPlaneGroundNormalsUp >= 0.0f && _pointToPlaneGroundNormalsUp <= 1.0f);
 	UASSERT(_pointToPlaneMinComplexity >= 0.0f && _pointToPlaneMinComplexity <= 1.0f);

 	Parameters::parse(parameters, Parameters::kIcpPM(), _libpointmatcher);
@@ -422,6 +534,7 @@ void RegistrationIcp::parseParameters(const ParametersMap & parameters)
 	Parameters::parse(parameters, Parameters::kIcpPMOutlierRatio(), _libpointmatcherOutlierRatio);
 	Parameters::parse(parameters, Parameters::kIcpPMMatcherKnn(), _libpointmatcherKnn);
 	Parameters::parse(parameters, Parameters::kIcpPMMatcherEpsilon(), _libpointmatcherEpsilon);
+	Parameters::parse(parameters, Parameters::kIcpPMMatcherIntensity(), _libpointmatcherIntensity);

 #ifndef RTABMAP_POINTMATCHER
 	if(_libpointmatcher)
@@ -474,11 +587,26 @@ void RegistrationIcp::parseParameters(const ParametersMap & parameters)
 			params["maxDist"] = uNumber2Str(_maxCorrespondenceDistance);
 			params["knn"] = uNumber2Str(_libpointmatcherKnn);
 			params["epsilon"] = uNumber2Str(_libpointmatcherEpsilon);
+
+			if(_libpointmatcherIntensity)
+			{
+				PointMatcher<float> matcher;
+				typedef typename PointMatcherSupport::Registrar< PointMatcher<float>::Matcher >::template GenericClassDescriptor< KDTreeMatcherIntensity<float> > Desc;
+				matcher.MatcherRegistrar.reg("KDTreeMatcherIntensity", std::make_shared<Desc>() );
 #if POINTMATCHER_VERSION_INT >= 10300
-			icp->matcher = PM::get().MatcherRegistrar.create("KDTreeMatcher", params);
+				icp->matcher = matcher.MatcherRegistrar.create("KDTreeMatcherIntensity", params);
 #else
-			icp->matcher.reset(PM::get().MatcherRegistrar.create("KDTreeMatcher", params));
+				icp->matcher.reset(matcher.MatcherRegistrar.create("KDTreeMatcherIntensity", params));
 #endif
+			}
+			else
+			{
+#if POINTMATCHER_VERSION_INT >= 10300
+				icp->matcher = PM::get().MatcherRegistrar.create("KDTreeMatcher", params);
+#else
+				icp->matcher.reset(PM::get().MatcherRegistrar.create("KDTreeMatcher", params));
+#endif
+			}
 			params.clear();

 			params["ratio"] = uNumber2Str(_libpointmatcherOutlierRatio);
@@ -638,15 +766,15 @@ Transform RegistrationIcp::computeTransformationImpl(
 				}
 				else
 				{
-					pcl::PointCloud<pcl::PointNormal>::Ptr fromCloudNormals = util3d::laserScanToPointCloudNormal(fromScan, fromScan.localTransform());
-					pcl::PointCloud<pcl::PointNormal>::Ptr toCloudNormals = util3d::laserScanToPointCloudNormal(toScan, guess * toScan.localTransform());
+					pcl::PointCloud<pcl::PointXYZINormal>::Ptr fromCloudNormals = util3d::laserScanToPointCloudINormal(fromScan, fromScan.localTransform());
+					pcl::PointCloud<pcl::PointXYZINormal>::Ptr toCloudNormals = util3d::laserScanToPointCloudINormal(toScan, guess * toScan.localTransform());

 					fromCloudNormals = util3d::removeNaNNormalsFromPointCloud(fromCloudNormals);
 					toCloudNormals = util3d::removeNaNNormalsFromPointCloud(toCloudNormals);

 					if(fromCloudNormals->size() > 2 && toCloudNormals->size() > 2)
 					{
-						pcl::PCA<pcl::PointNormal> pca;
+						pcl::PCA<pcl::PointXYZINormal> pca;
 						pca.setInputCloud(fromCloudNormals);
 						Eigen::Vector3f valuesFrom = pca.getEigenValues();
 						pca.setInputCloud(toCloudNormals);
@@ -662,7 +790,7 @@ Transform RegistrationIcp::computeTransformationImpl(
 					}

 					UDEBUG("Conversion time = %f s", timer.ticks());
-					pcl::PointCloud<pcl::PointNormal>::Ptr fromCloudNormalsRegistered(new pcl::PointCloud<pcl::PointNormal>());
+					pcl::PointCloud<pcl::PointXYZINormal>::Ptr fromCloudNormalsRegistered(new pcl::PointCloud<pcl::PointXYZINormal>());
 #ifdef RTABMAP_POINTMATCHER
 					if(_libpointmatcher)
 					{
@@ -727,13 +855,13 @@ Transform RegistrationIcp::computeTransformationImpl(
 			int maxLaserScansTo = toScan.maxPoints();
 			if(!transformComputed)
 			{
-				pcl::PointCloud<pcl::PointXYZ>::Ptr fromCloud = util3d::laserScanToPointCloud(fromScan, fromScan.localTransform());
-				pcl::PointCloud<pcl::PointXYZ>::Ptr toCloud = util3d::laserScanToPointCloud(toScan, guess * toScan.localTransform());
+				pcl::PointCloud<pcl::PointXYZI>::Ptr fromCloud = util3d::laserScanToPointCloudI(fromScan, fromScan.localTransform());
+				pcl::PointCloud<pcl::PointXYZI>::Ptr toCloud = util3d::laserScanToPointCloudI(toScan, guess * toScan.localTransform());
 				UDEBUG("Conversion time = %f s", timer.ticks());

 				if(fromCloud->size() > 2 && toCloud->size() > 2)
 				{
-					pcl::PCA<pcl::PointXYZ> pca;
+					pcl::PCA<pcl::PointXYZI> pca;
 					pca.setInputCloud(fromCloud);
 					Eigen::Vector3f valuesFrom = pca.getEigenValues();
 					pca.setInputCloud(toCloud);
@@ -746,10 +874,11 @@ Transform RegistrationIcp::computeTransformationImpl(
 					{
 						info.icpStructuralDistribution = sqrt(valuesTo[0]/toCloud->size());
 					}
+					UDEBUG("Computed icpStructuralDistribution %f s",timer.ticks());
 				}

-				pcl::PointCloud<pcl::PointXYZ>::Ptr fromCloudFiltered = fromCloud;
-				pcl::PointCloud<pcl::PointXYZ>::Ptr toCloudFiltered = toCloud;
+				pcl::PointCloud<pcl::PointXYZI>::Ptr fromCloudFiltered = fromCloud;
+				pcl::PointCloud<pcl::PointXYZI>::Ptr toCloudFiltered = toCloud;
 				if(_voxelSize > 0.0f)
 				{
 					float pointsBeforeFiltering = (float)fromCloudFiltered->size();
@@ -773,7 +902,7 @@ Transform RegistrationIcp::computeTransformationImpl(
 							timer.ticks());
 				}

-				pcl::PointCloud<pcl::PointXYZ>::Ptr fromCloudRegistered(new pcl::PointCloud<pcl::PointXYZ>());
+				pcl::PointCloud<pcl::PointXYZI>::Ptr fromCloudRegistered(new pcl::PointCloud<pcl::PointXYZI>());
 				if(_pointToPlane && // ICP Point To Plane
 					!tooLowComplexityForPlaneToPlane && // if previously rejected above
 					!((fromScan.is2d()|| toScan.is2d()) && !_libpointmatcher)) // PCL crashes if 2D
@@ -862,16 +991,31 @@ Transform RegistrationIcp::computeTransformationImpl(
 					}
 					else
 					{
-						pcl::PointCloud<pcl::PointNormal>::Ptr fromCloudNormals(new pcl::PointCloud<pcl::PointNormal>);
+						pcl::PointCloud<pcl::PointXYZINormal>::Ptr fromCloudNormals(new pcl::PointCloud<pcl::PointXYZINormal>);
 						pcl::concatenateFields(*fromCloudFiltered, *normalsFrom, *fromCloudNormals);

-						pcl::PointCloud<pcl::PointNormal>::Ptr toCloudNormals(new pcl::PointCloud<pcl::PointNormal>);
+						pcl::PointCloud<pcl::PointXYZINormal>::Ptr toCloudNormals(new pcl::PointCloud<pcl::PointXYZINormal>);
 						pcl::concatenateFields(*toCloudFiltered, *normalsTo, *toCloudNormals);

 						std::vector<int> indices;
 						toCloudNormals = util3d::removeNaNNormalsFromPointCloud(toCloudNormals);
 						fromCloudNormals = util3d::removeNaNNormalsFromPointCloud(fromCloudNormals);

+						if(!fromCloudNormals->empty() && !fromScan.is2d() && _pointToPlaneGroundNormalsUp>0.0f)
+						{
+							util3d::adjustNormalsToViewPoint(fromCloudNormals,
+									Eigen::Vector3f(fromScan.localTransform().x(),fromScan.localTransform().y(),fromScan.localTransform().z()+10),
+									_pointToPlaneGroundNormalsUp);
+						}
+						if(!toCloudNormals->empty() && !toScan.is2d() && _pointToPlaneGroundNormalsUp>0.0f)
+						{
+							Transform toT = guess * toScan.localTransform();
+							Eigen::Vector3f viewpointTo(toT.x(), toT.y(), toT.z()+10);
+							util3d::adjustNormalsToViewPoint(toCloudNormals,
+									viewpointTo,
+									_pointToPlaneGroundNormalsUp);
+						}
+
 						// update output scans
 						if(fromScan.is2d())
 						{
@@ -880,7 +1024,7 @@ Transform RegistrationIcp::computeTransformationImpl(
 											util3d::laserScan2dFromPointCloud(*fromCloudNormals, fromScan.localTransform().inverse()),
 											maxLaserScansFrom,
 											fromScan.rangeMax(),
-											LaserScan::kXYNormal,
+											LaserScan::kXYINormal,
 											fromScan.localTransform()));
 						}
 						else
@@ -890,7 +1034,7 @@ Transform RegistrationIcp::computeTransformationImpl(
 											util3d::laserScanFromPointCloud(*fromCloudNormals, fromScan.localTransform().inverse()),
 											maxLaserScansFrom,
 											fromScan.rangeMax(),
-											LaserScan::kXYZNormal,
+											LaserScan::kXYZINormal,
 											fromScan.localTransform()));
 						}
 						if(toScan.is2d())
@@ -900,7 +1044,7 @@ Transform RegistrationIcp::computeTransformationImpl(
 											util3d::laserScan2dFromPointCloud(*toCloudNormals, (guess*toScan.localTransform()).inverse()),
 											maxLaserScansTo,
 											toScan.rangeMax(),
-											LaserScan::kXYNormal,
+											LaserScan::kXYINormal,
 											toScan.localTransform()));
 						}
 						else
@@ -910,7 +1054,7 @@ Transform RegistrationIcp::computeTransformationImpl(
 											util3d::laserScanFromPointCloud(*toCloudNormals, (guess*toScan.localTransform()).inverse()),
 											maxLaserScansTo,
 											toScan.rangeMax(),
-											LaserScan::kXYZNormal,
+											LaserScan::kXYZINormal,
 											toScan.localTransform()));
 						}
 						UDEBUG("Compute normals (%d,%d) time = %f s", (int)fromCloudNormals->size(), (int)toCloudNormals->size(), timer.ticks());
@@ -919,7 +1063,7 @@ Transform RegistrationIcp::computeTransformationImpl(

 						if(toCloudNormals->size() && fromCloudNormals->size())
 						{
-							pcl::PointCloud<pcl::PointNormal>::Ptr fromCloudNormalsRegistered(new pcl::PointCloud<pcl::PointNormal>());
+							pcl::PointCloud<pcl::PointXYZINormal>::Ptr fromCloudNormalsRegistered(new pcl::PointCloud<pcl::PointXYZINormal>());

 #ifdef RTABMAP_POINTMATCHER
 							if(_libpointmatcher)
@@ -999,7 +1143,7 @@ Transform RegistrationIcp::computeTransformationImpl(
 											util3d::laserScan2dFromPointCloud(*fromCloudFiltered, fromScan.localTransform().inverse()),
 											maxLaserScansFrom,
 											fromScan.rangeMax(),
-											LaserScan::kXY,
+											LaserScan::kXYI,
 											fromScan.localTransform()));
 						}
 						else
@@ -1009,7 +1153,7 @@ Transform RegistrationIcp::computeTransformationImpl(
 											util3d::laserScanFromPointCloud(*fromCloudFiltered, fromScan.localTransform().inverse()),
 											maxLaserScansFrom,
 											fromScan.rangeMax(),
-											LaserScan::kXYZ,
+											LaserScan::kXYZI,
 											fromScan.localTransform()));
 						}
 						if(toScan.is2d())
@@ -1019,7 +1163,7 @@ Transform RegistrationIcp::computeTransformationImpl(
 											util3d::laserScan2dFromPointCloud(*toCloudFiltered, (guess*toScan.localTransform()).inverse()),
 											maxLaserScansTo,
 											toScan.rangeMax(),
-											LaserScan::kXY,
+											LaserScan::kXYI,
 											toScan.localTransform()));
 						}
 						else
@@ -1029,7 +1173,7 @@ Transform RegistrationIcp::computeTransformationImpl(
 											util3d::laserScanFromPointCloud(*toCloudFiltered, (guess*toScan.localTransform()).inverse()),
 											maxLaserScansTo,
 											toScan.rangeMax(),
-											LaserScan::kXYZ,
+											LaserScan::kXYZI,
 											toScan.localTransform()));
 						}
 						fromScan = fromSignature.sensorData().laserScanRaw();
@@ -1122,73 +1266,86 @@ Transform RegistrationIcp::computeTransformationImpl(

 					if(!icpT.isNull() && hasConverged)
 					{
-						if(tooLowComplexityForPlaneToPlane)
+						if(tooLowComplexityForPlaneToPlane && _pointToPlaneLowComplexityStrategy<2)
 						{
-							Transform guessInv = guess.inverse();
-							Transform t = guessInv * icpT.inverse() * guess;
-							Eigen::Vector3f v(t.x(), t.y(), t.z());
-							if(complexityVectors.cols == 2)
+							if(_pointToPlaneLowComplexityStrategy == 0)
 							{
-								// limit translation in direction of the first eigen vector
-								Eigen::Vector3f n(complexityVectors.at<float>(0,0), complexityVectors.at<float>(0,1), 0.0f);
-								float a = v.dot(n);
-								Eigen::Vector3f vp = n*a;
-								UWARN("Normals low complexity: Limiting translation from (%f,%f) to (%f,%f)",
-										v[0], v[1], vp[0], vp[1]);
-								v= vp;
+								msg = uFormat("Rejecting transform because too low complexity (%s=0)", Parameters::kIcpPointToPlaneLowComplexityStrategy().c_str());
+								icpT.setNull();
+								UWARN(msg.c_str());
 							}
-							else if(complexityVectors.rows == 3)
+							else //if(_pointToPlaneLowComplexityStrategy == 1)
 							{
-								// limit translation in direction of the first and second eigen vectors
-								Eigen::Vector3f n1(complexityVectors.at<float>(0,0), complexityVectors.at<float>(0,1), complexityVectors.at<float>(0,2));
-								Eigen::Vector3f n2(complexityVectors.at<float>(1,0), complexityVectors.at<float>(1,1), complexityVectors.at<float>(1,2));
-								float a = v.dot(n1);
-								float b = v.dot(n2);
-								Eigen::Vector3f vp = n1*a;
-								if(secondEigenValue >= _pointToPlaneMinComplexity)
+								Transform guessInv = guess.inverse();
+								Transform t = guessInv * icpT.inverse() * guess;
+								Eigen::Vector3f v(t.x(), t.y(), t.z());
+								if(complexityVectors.cols == 2)
 								{
-									vp += n2*b;
+									// limit translation in direction of the first eigen vector
+									Eigen::Vector3f n(complexityVectors.at<float>(0,0), complexityVectors.at<float>(0,1), 0.0f);
+									float a = v.dot(n);
+									Eigen::Vector3f vp = n*a;
+									UWARN("Normals low complexity: Limiting translation from (%f,%f) to (%f,%f)",
+											v[0], v[1], vp[0], vp[1]);
+									v= vp;
 								}
-								UWARN("Normals low complexity: Limiting translation from (%f,%f,%f) to (%f,%f,%f)",
-										v[0], v[1], v[2], vp[0], vp[1], vp[2]);
-								v = vp;
-							}
-							else
-							{
-								UWARN("not supposed to be here!");
-								v = Eigen::Vector3f(0,0,0);
-							}
-							float roll, pitch, yaw;
-							t.getEulerAngles(roll, pitch, yaw);
-							t = Transform(v[0], v[1], v[2], roll, pitch, yaw);
-							icpT = guess * t.inverse() * guessInv;
+								else if(complexityVectors.rows == 3)
+								{
+									// limit translation in direction of the first and second eigen vectors
+									Eigen::Vector3f n1(complexityVectors.at<float>(0,0), complexityVectors.at<float>(0,1), complexityVectors.at<float>(0,2));
+									Eigen::Vector3f n2(complexityVectors.at<float>(1,0), complexityVectors.at<float>(1,1), complexityVectors.at<float>(1,2));
+									float a = v.dot(n1);
+									float b = v.dot(n2);
+									Eigen::Vector3f vp = n1*a;
+									if(secondEigenValue >= _pointToPlaneMinComplexity)
+									{
+										vp += n2*b;
+									}
+									UWARN("Normals low complexity: Limiting translation from (%f,%f,%f) to (%f,%f,%f)",
+											v[0], v[1], v[2], vp[0], vp[1], vp[2]);
+									v = vp;
+								}
+								else
+								{
+									UWARN("not supposed to be here!");
+									v = Eigen::Vector3f(0,0,0);
+								}
+								float roll, pitch, yaw;
+								t.getEulerAngles(roll, pitch, yaw);
+								t = Transform(v[0], v[1], v[2], roll, pitch, yaw);
+								icpT = guess * t.inverse() * guessInv;

-							if(fromScan.hasNormals() && toScan.hasNormals())
-							{
-								// we were using normals, so compute correspondences using normals
-								pcl::PointCloud<pcl::PointNormal>::Ptr fromCloudNormalsRegistered = util3d::laserScanToPointCloudNormal(fromScan, icpT * fromScan.localTransform());
-								pcl::PointCloud<pcl::PointNormal>::Ptr toCloudNormals = util3d::laserScanToPointCloudNormal(toScan, guess * toScan.localTransform());
+								if(fromScan.hasNormals() && toScan.hasNormals())
+								{
+									// we were using normals, so compute correspondences using normals
+									pcl::PointCloud<pcl::PointXYZINormal>::Ptr fromCloudNormalsRegistered = util3d::laserScanToPointCloudINormal(fromScan, icpT * fromScan.localTransform());
+									pcl::PointCloud<pcl::PointXYZINormal>::Ptr toCloudNormals = util3d::laserScanToPointCloudINormal(toScan, guess * toScan.localTransform());

-								util3d::computeVarianceAndCorrespondences(
-										fromCloudNormalsRegistered,
-										toCloudNormals,
-										_maxCorrespondenceDistance,
-										_maxRotation,
-										variance,
-										correspondences);
-							}
-							else
-							{
-								util3d::computeVarianceAndCorrespondences(
-										fromCloudRegistered,
-										toCloudFiltered,
-										_maxCorrespondenceDistance,
-										variance,
-										correspondences);
+									util3d::computeVarianceAndCorrespondences(
+											fromCloudNormalsRegistered,
+											toCloudNormals,
+											_maxCorrespondenceDistance,
+											_maxRotation,
+											variance,
+											correspondences);
+								}
+								else
+								{
+									util3d::computeVarianceAndCorrespondences(
+											fromCloudRegistered,
+											toCloudFiltered,
+											_maxCorrespondenceDistance,
+											variance,
+											correspondences);
+								}
 							}
 						}
 						else
 						{
+							if(tooLowComplexityForPlaneToPlane)
+							{
+								UWARN("Even if complexity is low , PointToPoint transformation is accepted \"as is\" (%s=2)", Parameters::kIcpPointToPlaneLowComplexityStrategy().c_str());
+							}
 							util3d::computeVarianceAndCorrespondences(
 									fromCloudRegistered,
 									toCloudFiltered,
@@ -1263,6 +1420,7 @@ Transform RegistrationIcp::computeTransformationImpl(
 					else
 					{
 						info.covariance = cv::Mat::eye(6,6,CV_64FC1)*variance;
+						info.covariance(cv::Range(3,6),cv::Range(3,6))/=10.0; //orientation error
 					}
 					info.icpInliersRatio = correspondencesRatio;
 					info.icpCorrespondences = correspondences;
--- a/corelib/src/Rtabmap.cpp
+++ b/corelib/src/Rtabmap.cpp
@@ -3098,7 +3098,7 @@ bool Rtabmap::process(
 		previousMapCorrection = _mapCorrection;
 		_mapCorrection = _optimizedPoses.at(signature->id()) * signature->getPose().inverse();
 		_lastLocalizationPose = _optimizedPoses.at(signature->id()); // update
-		if(_mapCorrection.getNormSquared() > 0.001f && _optimizeFromGraphEnd)
+		if(_mapCorrection.getNormSquared() > 0.1f && _optimizeFromGraphEnd)
 		{
 			bool hasPrior = signature->hasLink(signature->id());
 			if(!_graphOptimizer->priorsIgnored())
--- a/corelib/src/odometry/OdometryF2M.cpp
+++ b/corelib/src/odometry/OdometryF2M.cpp
@@ -591,14 +591,9 @@ Transform OdometryF2M::computeTransform(
 						  visKeyFrameThr_ == 0 ||
 						  float(regInfo.inliers) <= (keyFrameThr_*float(lastFrame_->getWords().size())) ||
 						  regInfo.inliers <= visKeyFrameThr_);
-				float minComplexity = Parameters::defaultIcpPointToPlaneMinComplexity();
-				bool p2n = Parameters::defaultIcpPointToPlane();
-				Parameters::parse(parameters_, Parameters::kIcpPointToPlane(), p2n);
-				Parameters::parse(parameters_, Parameters::kIcpPointToPlaneMinComplexity(), minComplexity);
-				bool addGeometricKeyFrame =
-					regPipeline_->isScanRequired() &&
-					(scanKeyFrameThr_==0 || regInfo.icpInliersRatio <= scanKeyFrameThr_) &&
-					(addVisualKeyFrame || !p2n || regInfo.icpStructuralComplexity>=minComplexity);
+
+				bool addGeometricKeyFrame = regPipeline_->isScanRequired() &&
+					(scanKeyFrameThr_==0 || regInfo.icpInliersRatio <= scanKeyFrameThr_);

 				addKeyFrame = false;//bundleLinks.rbegin()->second.transform().getNorm() > 5.0f*0.075f;
 				addKeyFrame = addKeyFrame || addVisualKeyFrame || addGeometricKeyFrame;
@@ -955,14 +950,13 @@ Transform OdometryF2M::computeTransform(

 					if(lastFrame_->sensorData().laserScanRaw().size())
 					{
-						pcl::PointCloud<pcl::PointNormal>::Ptr mapCloudNormals = util3d::laserScanToPointCloudNormal(mapScan, tmpMap.sensorData().laserScanRaw().localTransform());
+						pcl::PointCloud<pcl::PointXYZINormal>::Ptr mapCloudNormals = util3d::laserScanToPointCloudINormal(mapScan, tmpMap.sensorData().laserScanRaw().localTransform());
 						Transform viewpoint =  newFramePose * lastFrame_->sensorData().laserScanRaw().localTransform();
-						pcl::PointCloud<pcl::PointNormal>::Ptr frameCloudNormals (new pcl::PointCloud<pcl::PointNormal>());
+						pcl::PointCloud<pcl::PointXYZINormal>::Ptr frameCloudNormals (new pcl::PointCloud<pcl::PointXYZINormal>());
 						
 						if(scanMapMaxRange_ > 0)
 						{
-							frameCloudNormals = util3d::laserScanToPointCloudNormal(
-									lastFrame_->sensorData().laserScanRaw());
+							frameCloudNormals = util3d::laserScanToPointCloudINormal(lastFrame_->sensorData().laserScanRaw());
 							frameCloudNormals = util3d::cropBox(frameCloudNormals,
 									Eigen::Vector4f(-scanMapMaxRange_ / 2, -scanMapMaxRange_ / 2,-scanMapMaxRange_ / 2, 0),
 									Eigen::Vector4f(scanMapMaxRange_ / 2,scanMapMaxRange_ / 2,scanMapMaxRange_ / 2, 0)
@@ -970,8 +964,7 @@ Transform OdometryF2M::computeTransform(
 							frameCloudNormals = util3d::transformPointCloud(frameCloudNormals, viewpoint);
 						} else
 						{
-							frameCloudNormals = util3d::laserScanToPointCloudNormal(
-									lastFrame_->sensorData().laserScanRaw(), viewpoint);
+							frameCloudNormals = util3d::laserScanToPointCloudINormal(lastFrame_->sensorData().laserScanRaw(), viewpoint);
 						}
 						
 						pcl::IndicesPtr frameCloudNormalsIndices(new std::vector<int>);
@@ -998,7 +991,7 @@ Transform OdometryF2M::computeTransform(
 							if (scanMapMaxRange_ > 0) {
 								// Copying new points to tmp cloud
 								// These are the points that have no overlap between mapScan and lastFrame
-								pcl::PointCloud<pcl::PointNormal> tmp;
+								pcl::PointCloud<pcl::PointXYZINormal> tmp;
 								pcl::copyPointCloud(*frameCloudNormals, *frameCloudNormalsIndices, tmp);

 								if (int(mapCloudNormals->size() + newPoints) > scanMaximumMapSize_) // 20 000 points
@@ -1058,7 +1051,7 @@ Transform OdometryF2M::computeTransform(
 										{
 											if(scansBuffer_[i].second->size())
 											{
-												pcl::PointCloud<pcl::PointNormal> tmp;
+												pcl::PointCloud<pcl::PointXYZINormal> tmp;
 												pcl::copyPointCloud(*scansBuffer_[i].first, *scansBuffer_[i].second, tmp);
 												*mapCloudNormals += tmp;
 											}
@@ -1071,7 +1064,7 @@ Transform OdometryF2M::computeTransform(
 									// remove old clouds
 									if(i > 0)
 									{
-										std::vector<std::pair<pcl::PointCloud<pcl::PointNormal>::Ptr, pcl::IndicesPtr> > scansTmp(scansBuffer_.size()-i);
+										std::vector<std::pair<pcl::PointCloud<pcl::PointXYZINormal>::Ptr, pcl::IndicesPtr> > scansTmp(scansBuffer_.size()-i);
 										int oi = 0;
 										for(; i<(int)scansBuffer_.size(); ++i)
 										{
@@ -1086,7 +1079,7 @@ Transform OdometryF2M::computeTransform(
 									// just append the last cloud
 									if(scansBuffer_.back().second->size())
 									{
-										pcl::PointCloud<pcl::PointNormal> tmp;
+										pcl::PointCloud<pcl::PointXYZINormal> tmp;
 										pcl::copyPointCloud(*scansBuffer_.back().first, *scansBuffer_.back().second, tmp);
 										*mapCloudNormals += tmp;
 									}
@@ -1100,12 +1093,12 @@ Transform OdometryF2M::computeTransform(
 							if(mapScan.is2d())
 							{
 								Transform mapViewpoint(-newFramePose.x(), -newFramePose.y(),0,0,0,0);
-								mapScan = LaserScan(util3d::laserScan2dFromPointCloud(*mapCloudNormals, mapViewpoint), 0, 0.0f, LaserScan::kXYNormal);
+								mapScan = LaserScan(util3d::laserScan2dFromPointCloud(*mapCloudNormals, mapViewpoint), 0, 0.0f, LaserScan::kXYINormal);
 							}
 							else
 							{
 								Transform mapViewpoint(-newFramePose.x(), -newFramePose.y(), -newFramePose.z(),0,0,0);
-								mapScan = LaserScan(util3d::laserScanFromPointCloud(*mapCloudNormals, mapViewpoint), 0, 0.0f, LaserScan::kXYZNormal);
+								mapScan = LaserScan(util3d::laserScanFromPointCloud(*mapCloudNormals, mapViewpoint), 0, 0.0f, LaserScan::kXYZINormal);
 							}
 							modified=true;
 						}
@@ -1311,7 +1304,7 @@ Transform OdometryF2M::computeTransform(
 			{
 				if (lastFrame_->sensorData().laserScanRaw().size())
 				{
-					pcl::PointCloud<pcl::PointNormal>::Ptr mapCloudNormals = util3d::laserScanToPointCloudNormal(lastFrame_->sensorData().laserScanRaw(), newFramePose * lastFrame_->sensorData().laserScanRaw().localTransform());
+					pcl::PointCloud<pcl::PointXYZINormal>::Ptr mapCloudNormals = util3d::laserScanToPointCloudINormal(lastFrame_->sensorData().laserScanRaw(), newFramePose * lastFrame_->sensorData().laserScanRaw().localTransform());

 					double complexity = 0.0;;
 					if(!frameValid)
@@ -1357,7 +1350,7 @@ Transform OdometryF2M::computeTransform(
 											util3d::laserScan2dFromPointCloud(*mapCloudNormals, mapViewpoint),
 											0,
 											0.0f,
-											LaserScan::kXYNormal,
+											LaserScan::kXYINormal,
 											Transform(newFramePose.x(), newFramePose.y(), lastFrame_->sensorData().laserScanRaw().localTransform().z(),0,0,0)));
 						}
 						else
@@ -1368,7 +1361,7 @@ Transform OdometryF2M::computeTransform(
 											util3d::laserScanFromPointCloud(*mapCloudNormals, mapViewpoint),
 											0,
 											0.0f,
-											LaserScan::kXYZNormal,
+											LaserScan::kXYZINormal,
 											newFramePose.translation()));
 						}

--- a/corelib/src/optimizer/OptimizerGTSAM.cpp
+++ b/corelib/src/optimizer/OptimizerGTSAM.cpp
@@ -138,6 +138,7 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
 		{
 			UASSERT(uContains(poses, rootId));
 			const Transform & initialPose = poses.at(rootId);
+			UDEBUG("hasPriorPoses=%s, gpsPriorOnly=%s", hasPriorPoses?"true":"false", gpsPriorOnly?"true":"false");
 			if(isSlam2d())
 			{
 				gtsam::noiseModel::Diagonal::shared_ptr priorNoise = gtsam::noiseModel::Diagonal::Variances(gtsam::Vector3(0.01, 0.01, hasPriorPoses?1e-2:std::numeric_limits<double>::min()));
--- a/corelib/src/util3d_filtering.cpp
+++ b/corelib/src/util3d_filtering.cpp
@@ -79,11 +79,11 @@ LaserScan commonFiltering(
 		float voxelSize,
 		int normalK,
 		float normalRadius,
-		bool forceGroundNormalsUp)
+		float groundNormalsUp)
 {
 	LaserScan scan = scanIn;
-	UDEBUG("scan size=%d format=%d, step=%d, rangeMin=%f, rangeMax=%f, voxel=%f, normalK=%d, normalRadius=%f",
-			scan.size(), (int)scan.format(), downsamplingStep, rangeMin, rangeMax, voxelSize, normalK, normalRadius);
+	UDEBUG("scan size=%d format=%d, step=%d, rangeMin=%f, rangeMax=%f, voxel=%f, normalK=%d, normalRadius=%f, groundNormalsUp=%f",
+			scan.size(), (int)scan.format(), downsamplingStep, rangeMin, rangeMax, voxelSize, normalK, normalRadius, groundNormalsUp);
 	if(!scan.isEmpty())
 	{
 		// combined downsampling and range filtering step
@@ -293,14 +293,27 @@ LaserScan commonFiltering(
 			}
 		}

-		if(scan.size() && !scan.is2d() && scan.hasNormals() && forceGroundNormalsUp)
+		if(scan.size() && !scan.is2d() && scan.hasNormals() && groundNormalsUp>0.0f)
 		{
-			scan = util3d::adjustNormalsToViewPoint(scan, Eigen::Vector3f(0,0,0), forceGroundNormalsUp);
+			scan = util3d::adjustNormalsToViewPoint(scan, Eigen::Vector3f(0,0,10), groundNormalsUp);
 		}
 	}
 	return scan;
 }

+LaserScan commonFiltering(
+		const LaserScan & scanIn,
+		int downsamplingStep,
+		float rangeMin,
+		float rangeMax,
+		float voxelSize,
+		int normalK,
+		float normalRadius,
+		bool forceGroundNormalsUp)
+{
+	return commonFiltering(scanIn, downsamplingStep, rangeMin, rangeMax, voxelSize, normalK, normalRadius, forceGroundNormalsUp?0.8f:0.0f);
+}
+
 LaserScan rangeFiltering(
 		const LaserScan & scan,
 		float rangeMin,
@@ -769,6 +782,10 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr cropBox(const pcl::PointCloud<pcl::PointX
 {
 	return cropBoxImpl<pcl::PointXYZRGB>(cloud, min, max, transform, negative);
 }
+pcl::PointCloud<pcl::PointXYZINormal>::Ptr cropBox(const pcl::PointCloud<pcl::PointXYZINormal>::Ptr & cloud, const Eigen::Vector4f & min, const Eigen::Vector4f & max, const Transform & transform, bool negative)
+{
+	return cropBoxImpl<pcl::PointXYZINormal>(cloud, min, max, transform, negative);
+}
 pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cropBox(const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud, const Eigen::Vector4f & min, const Eigen::Vector4f & max, const Transform & transform, bool negative)
 {
 	return cropBoxImpl<pcl::PointXYZRGBNormal>(cloud, min, max, transform, negative);
@@ -1083,6 +1100,19 @@ pcl::PointCloud<pcl::PointNormal>::Ptr subtractFiltering(
 	pcl::copyPointCloud(*cloud, *indicesOut, *out);
 	return out;
 }
+pcl::PointCloud<pcl::PointXYZINormal>::Ptr subtractFiltering(
+		const pcl::PointCloud<pcl::PointXYZINormal>::Ptr & cloud,
+		const pcl::PointCloud<pcl::PointXYZINormal>::Ptr & substractCloud,
+		float radiusSearch,
+		float maxAngle,
+		int minNeighborsInRadius)
+{
+	pcl::IndicesPtr indices(new std::vector<int>);
+	pcl::IndicesPtr indicesOut = subtractFiltering(cloud, indices, substractCloud, indices, radiusSearch, maxAngle, minNeighborsInRadius);
+	pcl::PointCloud<pcl::PointXYZINormal>::Ptr out(new pcl::PointCloud<pcl::PointXYZINormal>);
+	pcl::copyPointCloud(*cloud, *indicesOut, *out);
+	return out;
+}
 pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr subtractFiltering(
 		const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud,
 		const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & substractCloud,
@@ -1237,6 +1267,17 @@ pcl::IndicesPtr subtractFiltering(
 {
 	return subtractFilteringImpl<pcl::PointNormal>(cloud, indices, substractCloud, substractIndices, radiusSearch, maxAngle, minNeighborsInRadius);
 }
+pcl::IndicesPtr subtractFiltering(
+		const pcl::PointCloud<pcl::PointXYZINormal>::Ptr & cloud,
+		const pcl::IndicesPtr & indices,
+		const pcl::PointCloud<pcl::PointXYZINormal>::Ptr & substractCloud,
+		const pcl::IndicesPtr & substractIndices,
+		float radiusSearch,
+		float maxAngle,
+		int minNeighborsInRadius)
+{
+	return subtractFilteringImpl<pcl::PointXYZINormal>(cloud, indices, substractCloud, substractIndices, radiusSearch, maxAngle, minNeighborsInRadius);
+}
 pcl::IndicesPtr subtractFiltering(
 		const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud,
 		const pcl::IndicesPtr & indices,
--- a/corelib/src/util3d_registration.cpp
+++ b/corelib/src/util3d_registration.cpp
@@ -237,9 +237,10 @@ Transform transformFromXYZCorrespondences(
 	return Transform();
 }

-void computeVarianceAndCorrespondences(
-		const pcl::PointCloud<pcl::PointNormal>::ConstPtr & cloudA,
-		const pcl::PointCloud<pcl::PointNormal>::ConstPtr & cloudB,
+template<typename PointNormalT>
+void computeVarianceAndCorrespondencesImpl(
+		const typename pcl::PointCloud<PointNormalT>::ConstPtr & cloudA,
+		const typename pcl::PointCloud<PointNormalT>::ConstPtr & cloudB,
 		double maxCorrespondenceDistance,
 		double maxCorrespondenceAngle,
 		double & variance,
@@ -247,10 +248,10 @@ void computeVarianceAndCorrespondences(
 {
 	variance = 1;
 	correspondencesOut = 0;
-	pcl::registration::CorrespondenceEstimation<pcl::PointNormal, pcl::PointNormal>::Ptr est;
-	est.reset(new pcl::registration::CorrespondenceEstimation<pcl::PointNormal, pcl::PointNormal>);
-	const pcl::PointCloud<pcl::PointNormal>::ConstPtr & target = cloudA->size()>cloudB->size()?cloudA:cloudB;
-	const pcl::PointCloud<pcl::PointNormal>::ConstPtr & source = cloudA->size()>cloudB->size()?cloudB:cloudA;
+	typename pcl::registration::CorrespondenceEstimation<PointNormalT, PointNormalT>::Ptr est;
+	est.reset(new pcl::registration::CorrespondenceEstimation<PointNormalT, PointNormalT>);
+	const typename pcl::PointCloud<PointNormalT>::ConstPtr & target = cloudA->size()>cloudB->size()?cloudA:cloudB;
+	const typename pcl::PointCloud<PointNormalT>::ConstPtr & source = cloudA->size()>cloudB->size()?cloudB:cloudA;
 	est->setInputTarget(target);
 	est->setInputSource(source);
 	pcl::Correspondences correspondences;
@@ -299,16 +300,39 @@ void computeVarianceAndCorrespondences(
 }

 void computeVarianceAndCorrespondences(
-		const pcl::PointCloud<pcl::PointXYZ>::ConstPtr & cloudA,
-		const pcl::PointCloud<pcl::PointXYZ>::ConstPtr & cloudB,
+		const pcl::PointCloud<pcl::PointNormal>::ConstPtr & cloudA,
+		const pcl::PointCloud<pcl::PointNormal>::ConstPtr & cloudB,
+		double maxCorrespondenceDistance,
+		double maxCorrespondenceAngle,
+		double & variance,
+		int & correspondencesOut)
+{
+	computeVarianceAndCorrespondencesImpl<pcl::PointNormal>(cloudA, cloudB, maxCorrespondenceDistance, maxCorrespondenceAngle, variance, correspondencesOut);
+}
+
+void computeVarianceAndCorrespondences(
+		const pcl::PointCloud<pcl::PointXYZINormal>::ConstPtr & cloudA,
+		const pcl::PointCloud<pcl::PointXYZINormal>::ConstPtr & cloudB,
+		double maxCorrespondenceDistance,
+		double maxCorrespondenceAngle,
+		double & variance,
+		int & correspondencesOut)
+{
+	computeVarianceAndCorrespondencesImpl<pcl::PointXYZINormal>(cloudA, cloudB, maxCorrespondenceDistance, maxCorrespondenceAngle, variance, correspondencesOut);
+}
+
+template<typename PointT>
+void computeVarianceAndCorrespondencesImpl(
+		const typename pcl::PointCloud<PointT>::ConstPtr & cloudA,
+		const typename pcl::PointCloud<PointT>::ConstPtr & cloudB,
 		double maxCorrespondenceDistance,
 		double & variance,
 		int & correspondencesOut)
 {
 	variance = 1;
 	correspondencesOut = 0;
-	pcl::registration::CorrespondenceEstimation<pcl::PointXYZ, pcl::PointXYZ>::Ptr est;
-	est.reset(new pcl::registration::CorrespondenceEstimation<pcl::PointXYZ, pcl::PointXYZ>);
+	typename pcl::registration::CorrespondenceEstimation<PointT, PointT>::Ptr est;
+	est.reset(new pcl::registration::CorrespondenceEstimation<PointT, PointT>);
 	est->setInputTarget(cloudA->size()>cloudB->size()?cloudA:cloudB);
 	est->setInputSource(cloudA->size()>cloudB->size()?cloudB:cloudA);
 	pcl::Correspondences correspondences;
@@ -331,25 +355,46 @@ void computeVarianceAndCorrespondences(
 	correspondencesOut = (int)correspondences.size();
 }

+void computeVarianceAndCorrespondences(
+		const pcl::PointCloud<pcl::PointXYZ>::ConstPtr & cloudA,
+		const pcl::PointCloud<pcl::PointXYZ>::ConstPtr & cloudB,
+		double maxCorrespondenceDistance,
+		double & variance,
+		int & correspondencesOut)
+{
+	computeVarianceAndCorrespondencesImpl<pcl::PointXYZ>(cloudA, cloudB, maxCorrespondenceDistance, variance, correspondencesOut);
+}
+
+void computeVarianceAndCorrespondences(
+		const pcl::PointCloud<pcl::PointXYZI>::ConstPtr & cloudA,
+		const pcl::PointCloud<pcl::PointXYZI>::ConstPtr & cloudB,
+		double maxCorrespondenceDistance,
+		double & variance,
+		int & correspondencesOut)
+{
+	computeVarianceAndCorrespondencesImpl<pcl::PointXYZI>(cloudA, cloudB, maxCorrespondenceDistance, variance, correspondencesOut);
+}
+
 // return transform from source to target (All points must be finite!!!)
-Transform icp(const pcl::PointCloud<pcl::PointXYZ>::ConstPtr & cloud_source,
-			  const pcl::PointCloud<pcl::PointXYZ>::ConstPtr & cloud_target,
+template<typename PointT>
+Transform icpImpl(const typename pcl::PointCloud<PointT>::ConstPtr & cloud_source,
+			  const typename pcl::PointCloud<PointT>::ConstPtr & cloud_target,
 			  double maxCorrespondenceDistance,
 			  int maximumIterations,
 			  bool & hasConverged,
-			  pcl::PointCloud<pcl::PointXYZ> & cloud_source_registered,
+			  pcl::PointCloud<PointT> & cloud_source_registered,
 			  float epsilon,
 			  bool icp2D)
 {
-	pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> icp;
+	pcl::IterativeClosestPoint<PointT, PointT> icp;
 	// Set the input source and target
 	icp.setInputTarget (cloud_target);
 	icp.setInputSource (cloud_source);

 	if(icp2D)
 	{
-		pcl::registration::TransformationEstimation2D<pcl::PointXYZ, pcl::PointXYZ>::Ptr est;
-		est.reset(new pcl::registration::TransformationEstimation2D<pcl::PointXYZ, pcl::PointXYZ>);
+		typename pcl::registration::TransformationEstimation2D<PointT, PointT>::Ptr est;
+		est.reset(new pcl::registration::TransformationEstimation2D<PointT, PointT>);
 		icp.setTransformationEstimation(est);
 	}

@@ -369,24 +414,51 @@ Transform icp(const pcl::PointCloud<pcl::PointXYZ>::ConstPtr & cloud_source,
 	return Transform::fromEigen4f(icp.getFinalTransformation());
 }

+// return transform from source to target (All points must be finite!!!)
+Transform icp(const pcl::PointCloud<pcl::PointXYZ>::ConstPtr & cloud_source,
+			  const pcl::PointCloud<pcl::PointXYZ>::ConstPtr & cloud_target,
+			  double maxCorrespondenceDistance,
+			  int maximumIterations,
+			  bool & hasConverged,
+			  pcl::PointCloud<pcl::PointXYZ> & cloud_source_registered,
+			  float epsilon,
+			  bool icp2D)
+{
+	return icpImpl(cloud_source, cloud_target, maxCorrespondenceDistance, maximumIterations, hasConverged, cloud_source_registered, epsilon, icp2D);
+}
+
+// return transform from source to target (All points must be finite!!!)
+Transform icp(const pcl::PointCloud<pcl::PointXYZI>::ConstPtr & cloud_source,
+			  const pcl::PointCloud<pcl::PointXYZI>::ConstPtr & cloud_target,
+			  double maxCorrespondenceDistance,
+			  int maximumIterations,
+			  bool & hasConverged,
+			  pcl::PointCloud<pcl::PointXYZI> & cloud_source_registered,
+			  float epsilon,
+			  bool icp2D)
+{
+	return icpImpl(cloud_source, cloud_target, maxCorrespondenceDistance, maximumIterations, hasConverged, cloud_source_registered, epsilon, icp2D);
+}
+
 // return transform from source to target (All points/normals must be finite!!!)
-Transform icpPointToPlane(
-		const pcl::PointCloud<pcl::PointNormal>::ConstPtr & cloud_source,
-		const pcl::PointCloud<pcl::PointNormal>::ConstPtr & cloud_target,
+template<typename PointNormalT>
+Transform icpPointToPlaneImpl(
+		const typename pcl::PointCloud<PointNormalT>::ConstPtr & cloud_source,
+		const typename pcl::PointCloud<PointNormalT>::ConstPtr & cloud_target,
 		double maxCorrespondenceDistance,
 		int maximumIterations,
 		bool & hasConverged,
-		pcl::PointCloud<pcl::PointNormal> & cloud_source_registered,
+		pcl::PointCloud<PointNormalT> & cloud_source_registered,
 		float epsilon,
 		bool icp2D)
 {
-	pcl::IterativeClosestPoint<pcl::PointNormal, pcl::PointNormal> icp;
+	pcl::IterativeClosestPoint<PointNormalT, PointNormalT> icp;
 	// Set the input source and target
 	icp.setInputTarget (cloud_target);
 	icp.setInputSource (cloud_source);

-	pcl::registration::TransformationEstimationPointToPlaneLLS<pcl::PointNormal, pcl::PointNormal>::Ptr est;
-	est.reset(new pcl::registration::TransformationEstimationPointToPlaneLLS<pcl::PointNormal, pcl::PointNormal>);
+	typename pcl::registration::TransformationEstimationPointToPlaneLLS<PointNormalT, PointNormalT>::Ptr est;
+	est.reset(new pcl::registration::TransformationEstimationPointToPlaneLLS<PointNormalT, PointNormalT>);
 	icp.setTransformationEstimation(est);

 	// Set the max correspondence distance to 5cm (e.g., correspondences with higher distances will be ignored)
@@ -413,6 +485,33 @@ Transform icpPointToPlane(
 	return t;
 }

+// return transform from source to target (All points/normals must be finite!!!)
+Transform icpPointToPlane(
+		const pcl::PointCloud<pcl::PointNormal>::ConstPtr & cloud_source,
+		const pcl::PointCloud<pcl::PointNormal>::ConstPtr & cloud_target,
+		double maxCorrespondenceDistance,
+		int maximumIterations,
+		bool & hasConverged,
+		pcl::PointCloud<pcl::PointNormal> & cloud_source_registered,
+		float epsilon,
+		bool icp2D)
+{
+	return icpPointToPlaneImpl(cloud_source, cloud_target, maxCorrespondenceDistance, maximumIterations, hasConverged, cloud_source_registered, epsilon, icp2D);
+}
+// return transform from source to target (All points/normals must be finite!!!)
+Transform icpPointToPlane(
+		const pcl::PointCloud<pcl::PointXYZINormal>::ConstPtr & cloud_source,
+		const pcl::PointCloud<pcl::PointXYZINormal>::ConstPtr & cloud_target,
+		double maxCorrespondenceDistance,
+		int maximumIterations,
+		bool & hasConverged,
+		pcl::PointCloud<pcl::PointXYZINormal> & cloud_source_registered,
+		float epsilon,
+		bool icp2D)
+{
+	return icpPointToPlaneImpl(cloud_source, cloud_target, maxCorrespondenceDistance, maximumIterations, hasConverged, cloud_source_registered, epsilon, icp2D);
+}
+
 }

 }
--- a/corelib/src/util3d_surface.cpp
+++ b/corelib/src/util3d_surface.cpp
@@ -3142,6 +3142,62 @@ float computeNormalsComplexity(
 	return 0.0f;
 }

+float computeNormalsComplexity(
+		const pcl::PointCloud<pcl::PointXYZINormal> & cloud,
+		const Transform & t,
+		bool is2d,
+		cv::Mat * pcaEigenVectors,
+		cv::Mat * pcaEigenValues)
+{
+	 //Construct a buffer used by the pca analysis
+	int sz = static_cast<int>(cloud.size()*2);
+	cv::Mat data_normals = cv::Mat::zeros(sz, is2d?2:3, CV_32FC1);
+	int oi = 0;
+	bool doTransform = false;
+	Transform tn;
+	if(!t.isIdentity())
+	{
+		tn = t.rotation();
+		doTransform = true;
+	}
+	for (unsigned int i = 0; i < cloud.size(); ++i)
+	{
+		const pcl::PointXYZINormal & pt = cloud.at(i);
+		cv::Point3f n(pt.normal_x, pt.normal_y, pt.normal_z);
+		if(doTransform)
+		{
+			n = util3d::transformPoint(n, tn);
+		}
+		if(uIsFinite(pt.normal_x) && uIsFinite(pt.normal_y) && uIsFinite(pt.normal_z))
+		{
+			float * ptr = data_normals.ptr<float>(oi++, 0);
+			ptr[0] = n.x;
+			ptr[1] = n.y;
+			if(!is2d)
+			{
+				ptr[2] = n.z;
+			}
+		}
+	}
+	if(oi>1)
+	{
+		cv::PCA pca_analysis(cv::Mat(data_normals, cv::Range(0, oi*2)), cv::Mat(), CV_PCA_DATA_AS_ROW);
+
+		if(pcaEigenVectors)
+		{
+			*pcaEigenVectors = pca_analysis.eigenvectors;
+		}
+		if(pcaEigenValues)
+		{
+			*pcaEigenValues = pca_analysis.eigenvalues;
+		}
+
+		// Get last eigen value, scale between 0 and 1: 0=low complexity, 1=high complexity
+		return pca_analysis.eigenvalues.at<float>(0, is2d?1:2)*(is2d?2.0f:3.0f);
+	}
+	return 0.0f;
+}
+
 float computeNormalsComplexity(
 		const pcl::PointCloud<pcl::PointXYZRGBNormal> & cloud,
 		const Transform & t,
@@ -3301,6 +3357,13 @@ LaserScan adjustNormalsToViewPoint(
 		const LaserScan & scan,
 		const Eigen::Vector3f & viewpoint,
 		bool forceGroundNormalsUp)
+{
+	return adjustNormalsToViewPoint(scan, viewpoint, forceGroundNormalsUp?0.8f:0.0f);
+}
+LaserScan adjustNormalsToViewPoint(
+		const LaserScan & scan,
+		const Eigen::Vector3f & viewpoint,
+		float groundNormalsUp)
 {
 	if(scan.size() && !scan.is2d() && scan.hasNormals())
 	{
@@ -3308,6 +3371,7 @@ LaserScan adjustNormalsToViewPoint(
 		int ny = nx+1;
 		int nz = ny+1;
 		cv::Mat output = scan.data().clone();
+		#pragma omp parallel for
 		for(int i=0; i<scan.size(); ++i)
 		{
 			float * ptr = output.ptr<float>(0, i);
@@ -3318,7 +3382,7 @@ LaserScan adjustNormalsToViewPoint(

 				float result = v.dot(n);
 				if(result < 0
-				 || (forceGroundNormalsUp && ptr[nz] < -0.8 && ptr[2] < viewpoint[3])) // some far velodyne rays on road can have normals toward ground
+				 || (groundNormalsUp>0.0f && ptr[nz] < -groundNormalsUp && ptr[2] < viewpoint[3])) // some far velodyne rays on road can have normals toward ground
 				{
 					//reverse normal
 					ptr[nx] *= -1.0f;
@@ -3339,10 +3403,11 @@ LaserScan adjustNormalsToViewPoint(
 	return scan;
 }

-void adjustNormalsToViewPoint(
-		pcl::PointCloud<pcl::PointNormal>::Ptr & cloud,
+template<typename PointNormalT>
+void adjustNormalsToViewPointImpl(
+		typename pcl::PointCloud<PointNormalT>::Ptr & cloud,
 		const Eigen::Vector3f & viewpoint,
-		bool forceGroundNormalsUp)
+		float groundNormalsUp)
 {
 	for(unsigned int i=0; i<cloud->size(); ++i)
 	{
@@ -3354,7 +3419,7 @@ void adjustNormalsToViewPoint(

 			float result = v.dot(n);
 			if(result < 0
-			 || (forceGroundNormalsUp && normal.z < -0.8 && cloud->points[i].z < viewpoint[3])) // some far velodyne rays on road can have normals toward ground
+				|| (groundNormalsUp>0.0f && normal.z < -groundNormalsUp && cloud->points[i].z < viewpoint[3])) // some far velodyne rays on road can have normals toward ground
 			{
 				//reverse normal
 				cloud->points[i].normal_x *= -1.0f;
@@ -3365,30 +3430,49 @@ void adjustNormalsToViewPoint(
 	}
 }

+void adjustNormalsToViewPoint(
+		pcl::PointCloud<pcl::PointNormal>::Ptr & cloud,
+		const Eigen::Vector3f & viewpoint,
+		bool forceGroundNormalsUp)
+{
+	adjustNormalsToViewPoint(cloud, viewpoint, forceGroundNormalsUp?0.8f:0.0f);
+}
+void adjustNormalsToViewPoint(
+		pcl::PointCloud<pcl::PointNormal>::Ptr & cloud,
+		const Eigen::Vector3f & viewpoint,
+		float groundNormalsUp)
+{
+	adjustNormalsToViewPointImpl<pcl::PointNormal>(cloud, viewpoint, groundNormalsUp);
+}
+
 void adjustNormalsToViewPoint(
 		pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud,
 		const Eigen::Vector3f & viewpoint,
 		bool forceGroundNormalsUp)
 {
-	for(unsigned int i=0; i<cloud->size(); ++i)
-	{
-		pcl::PointXYZ normal(cloud->points[i].normal_x, cloud->points[i].normal_y, cloud->points[i].normal_z);
-		if(pcl::isFinite(normal))
-		{
-			Eigen::Vector3f v = viewpoint - cloud->points[i].getVector3fMap();
-			Eigen::Vector3f n(normal.x, normal.y, normal.z);
+	adjustNormalsToViewPoint(cloud, viewpoint, forceGroundNormalsUp?0.8f:0.0f);
+}
+void adjustNormalsToViewPoint(
+		pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud,
+		const Eigen::Vector3f & viewpoint,
+		float groundNormalsUp)
+{
+	adjustNormalsToViewPointImpl<pcl::PointXYZRGBNormal>(cloud, viewpoint, groundNormalsUp);
+}

-			float result = v.dot(n);
-			if(result < 0
-				|| (forceGroundNormalsUp && normal.z < -0.8 && cloud->points[i].z < viewpoint[3])) // some far velodyne rays on road can have normals toward ground
-			{
-				//reverse normal
-				cloud->points[i].normal_x *= -1.0f;
-				cloud->points[i].normal_y *= -1.0f;
-				cloud->points[i].normal_z *= -1.0f;
-			}
-		}
-	}
+void adjustNormalsToViewPoint(
+		pcl::PointCloud<pcl::PointXYZINormal>::Ptr & cloud,
+		const Eigen::Vector3f & viewpoint,
+		bool forceGroundNormalsUp)
+{
+	adjustNormalsToViewPoint(cloud, viewpoint, forceGroundNormalsUp?0.8f:0.0f);
+}
+void adjustNormalsToViewPoint(
+		pcl::PointCloud<pcl::PointXYZINormal>::Ptr & cloud,
+		const Eigen::Vector3f & viewpoint,
+		float groundNormalsUp)
+{
+	adjustNormalsToViewPointImpl<pcl::PointXYZINormal>(cloud, viewpoint, groundNormalsUp);
 }

 void adjustNormalsToViewPoints(