Merge branch 'devel' of https://github.com/introlab/rtabmap

2026-09-01 17:10:26 +08:00 · 2017-09-19 14:40:19 -04:00
parent 68fb5d7252 fd18c0b2e9
commit b737df9c40
61 changed files with 2653 additions and 1063 deletions
--- a/corelib/src/CMakeLists.txt
+++ b/corelib/src/CMakeLists.txt
@@ -325,12 +325,12 @@ ENDIF(dvo_core_FOUND)

 IF(ORB_SLAM2_FOUND)
    SET(INCLUDE_DIRS
+        ${ORB_SLAM2_INCLUDE_DIRS} #before so that g2o includes are taken from ORB_SLAM2 directory before the official g2o one
 		${INCLUDE_DIRS}
-		${ORB_SLAM2_INCLUDE_DIRS}
 	)
 	SET(LIBRARIES
+	    ${ORB_SLAM2_LIBRARIES}
 		${LIBRARIES}
-		${ORB_SLAM2_LIBRARIES}
 	)
 ENDIF(ORB_SLAM2_FOUND)

--- a/corelib/src/CameraRGB.cpp
+++ b/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());
--- a/corelib/src/CameraThread.cpp
+++ b/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),
@@ -297,7 +298,7 @@ void CameraThread::postUpdate(SensorData * dataPtr, CameraInfo * info) const
 			UASSERT(_scanDecimation >= 1);
 			UTimer timer;
 			pcl::IndicesPtr validIndices(new std::vector<int>);
-			pcl::PointCloud<pcl::PointXYZ>::Ptr cloud = util3d::cloudFromSensorData(
+			pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud = util3d::cloudRGBFromSensorData(
 					data,
 					_scanDecimation,
 					_scanMaxDepth,
@@ -316,18 +317,18 @@ void CameraThread::postUpdate(SensorData * dataPtr, CameraInfo * info) const
 				}
 				else if(!cloud->is_dense)
 				{
-					pcl::PointCloud<pcl::PointXYZ>::Ptr denseCloud(new pcl::PointCloud<pcl::PointXYZ>);
+					pcl::PointCloud<pcl::PointXYZRGB>::Ptr denseCloud(new pcl::PointCloud<pcl::PointXYZRGB>);
 					pcl::copyPointCloud(*cloud, *validIndices, *denseCloud);
 					cloud = denseCloud;
 				}

 				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::PointNormal>::Ptr cloudNormals(new pcl::PointCloud<pcl::PointNormal>);
+						pcl::PointCloud<pcl::Normal>::Ptr normals = util3d::computeNormals(cloud, _scanNormalsK, _scanNormalsRadius, viewPoint);
+						pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloudNormals(new pcl::PointCloud<pcl::PointXYZRGBNormal>);
 						pcl::concatenateFields(*cloud, *normals, *cloudNormals);
 						scan = util3d::laserScanFromPointCloud(*cloudNormals, baseToScan.inverse());
 					}
--- a/corelib/src/Graph.cpp
+++ b/corelib/src/Graph.cpp
@@ -1603,7 +1603,7 @@ int findNearestNode(
 		kdTree->nearestKSearch(pt, 1, ind, dist);
 		if(ind.size() && dist.size() && ind[0] >= 0)
 		{
-			UDEBUG("Nearest node = %d: %f", ids[ind[0]], dist[0]);
+			//UDEBUG("Nearest node = %d: %f", ids[ind[0]], dist[0]);
 			id = ids[ind[0]];
 		}
 	}
--- a/corelib/src/Memory.cpp
+++ b/corelib/src/Memory.cpp
@@ -88,7 +88,9 @@ Memory::Memory(const ParametersMap & parameters) :
 	_imagePostDecimation(Parameters::defaultMemImagePostDecimation()),
 	_compressionParallelized(Parameters::defaultMemCompressionParallelized()),
 	_laserScanDownsampleStepSize(Parameters::defaultMemLaserScanDownsampleStepSize()),
+	_laserScanVoxelSize(Parameters::defaultMemLaserScanVoxelSize()),
 	_laserScanNormalK(Parameters::defaultMemLaserScanNormalK()),
+	_laserScanNormalRadius(Parameters::defaultMemLaserScanNormalRadius()),
 	_reextractLoopClosureFeatures(Parameters::defaultRGBDLoopClosureReextractFeatures()),
 	_rehearsalMaxDistance(Parameters::defaultRGBDLinearUpdate()),
 	_rehearsalMaxAngle(Parameters::defaultRGBDAngularUpdate()),
@@ -439,7 +441,9 @@ void Memory::parseParameters(const ParametersMap & parameters)
 	Parameters::parse(parameters, Parameters::kMemImagePostDecimation(), _imagePostDecimation);
 	Parameters::parse(parameters, Parameters::kMemCompressionParallelized(), _compressionParallelized);
 	Parameters::parse(parameters, Parameters::kMemLaserScanDownsampleStepSize(), _laserScanDownsampleStepSize);
+	Parameters::parse(parameters, Parameters::kMemLaserScanVoxelSize(), _laserScanVoxelSize);
 	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);
@@ -2404,6 +2408,18 @@ Transform Memory::computeIcpTransformMulti(
 	UASSERT(uContains(poses, toId) && uContains(_signatures, toId));

 	UDEBUG("Guess=%s", (poses.at(fromId).inverse() * poses.at(toId)).prettyPrint().c_str());
+	if(ULogger::level() == ULogger::kDebug)
+	{
+		std::string ids;
+		for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
+		{
+			if(iter->first != fromId)
+			{
+				ids += uNumber2Str(iter->first) + " ";
+			}
+		}
+		UDEBUG("%d vs %s", fromId, ids.c_str());
+	}

 	// make sure that all laser scans are loaded
 	std::list<Signature*> depthToLoad;
@@ -2436,6 +2452,8 @@ Transform Memory::computeIcpTransformMulti(
 		std::string msg;
 		int maxPoints = fromScan.cols;
 		pcl::PointCloud<pcl::PointXYZ>::Ptr assembledToClouds(new pcl::PointCloud<pcl::PointXYZ>);
+		pcl::PointCloud<pcl::PointNormal>::Ptr assembledToNormalClouds(new pcl::PointCloud<pcl::PointNormal>);
+		bool is2D = true;
 		for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
 		{
 			if(iter->first != fromId)
@@ -2445,14 +2463,33 @@ Transform Memory::computeIcpTransformMulti(
 				{
 					cv::Mat scan;
 					s->sensorData().uncompressData(0, 0, &scan);
-					pcl::PointCloud<pcl::PointXYZ>::Ptr cloud = util3d::laserScanToPointCloud(
-							scan,
-							s->sensorData().laserScanInfo().localTransform() * toPose.inverse() * iter->second);
-					if(scan.cols > maxPoints)
+					if(!scan.empty())
 					{
-						maxPoints = scan.cols;
+						if(scan.channels() != 2 && scan.channels() != 5)
+						{
+							is2D = false;
+						}
+
+						if(scan.channels() >= 5)
+						{
+							pcl::PointCloud<pcl::PointNormal>::Ptr cloudNormal = util3d::laserScanToPointCloudNormal(
+									scan,
+									s->sensorData().laserScanInfo().localTransform() * toPose.inverse() * iter->second);
+							*assembledToNormalClouds += *cloudNormal;
+						}
+						else
+						{
+							pcl::PointCloud<pcl::PointXYZ>::Ptr cloud = util3d::laserScanToPointCloud(
+									scan,
+									s->sensorData().laserScanInfo().localTransform() * toPose.inverse() * iter->second);
+							*assembledToClouds += *cloud;
+						}
+
+						if(scan.cols > maxPoints)
+						{
+							maxPoints = scan.cols;
+						}
 					}
-					*assembledToClouds += *cloud;
 				}
 				else
 				{
@@ -2460,15 +2497,22 @@ Transform Memory::computeIcpTransformMulti(
 				}
 			}
 		}
-		if(assembledToClouds->size())
+
+		cv::Mat assembledScan;
+		if(assembledToNormalClouds->size())
 		{
-			assembledData.setLaserScanRaw(
-					util3d::laserScanFromPointCloud(*assembledToClouds),
-					LaserScanInfo(
-							fromS->sensorData().laserScanInfo().maxPoints()?fromS->sensorData().laserScanInfo().maxPoints():maxPoints,
-							fromS->sensorData().laserScanInfo().maxRange(),
-							Transform::getIdentity())); // scans are in base frame
+			assembledScan = is2D?util3d::laserScan2dFromPointCloud(*assembledToNormalClouds):util3d::laserScanFromPointCloud(*assembledToNormalClouds);
 		}
+		else if(assembledToClouds->size())
+		{
+			assembledScan = is2D?util3d::laserScan2dFromPointCloud(*assembledToClouds):util3d::laserScanFromPointCloud(*assembledToClouds);
+		}
+		// scans are in base frame but for 2d scans, set the height so that correspondences matching works
+		assembledData.setLaserScanRaw(assembledScan,
+				LaserScanInfo(
+					fromS->sensorData().laserScanInfo().maxPoints()?fromS->sensorData().laserScanInfo().maxPoints():maxPoints,
+					fromS->sensorData().laserScanInfo().maxRange(),
+					is2D?Transform(0,0,fromS->sensorData().laserScanInfo().localTransform().z(),0,0,0):Transform::getIdentity()));

 		Transform guess = poses.at(fromId).inverse() * poses.at(toId);
 		std::vector<int> inliersV;
@@ -3268,7 +3312,7 @@ Signature * Memory::createSignature(const SensorData & data, const Transform & p
 						data.depthOrRightRaw().rows,
 						data.depthOrRightRaw().type(),
 						CV_16UC1, CV_32FC1, CV_8UC1).c_str());
-	UASSERT(data.laserScanRaw().empty() || data.laserScanRaw().type() == CV_32FC2 || data.laserScanRaw().type() == CV_32FC3 || data.laserScanRaw().type() == CV_32FC(4) || data.laserScanRaw().type() == CV_32FC(6));
+	UASSERT(data.laserScanRaw().empty() || data.laserScanRaw().type() == CV_32FC2 || data.laserScanRaw().type() == CV_32FC3 || data.laserScanRaw().type() == CV_32FC(4) || data.laserScanRaw().type() == CV_32FC(5) || data.laserScanRaw().type() == CV_32FC(6) || data.laserScanRaw().type() == CV_32FC(7));

 	if(!data.depthOrRightRaw().empty() &&
 		data.cameraModels().size() == 0 &&
@@ -3718,13 +3762,41 @@ 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() && _laserScanVoxelSize > 0.0f && !isIntermediateNode)
 	{
-		pcl::PointCloud<pcl::PointXYZ>::Ptr cloud = util3d::laserScanToPointCloud(laserScan);
-		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);
+		float pointsBeforeFiltering = laserScan.cols;
+		if(laserScan.channels() == 4 || laserScan.channels() == 7)
+		{
+			pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud = util3d::laserScanToPointCloudRGB(laserScan);
+			cloud = util3d::voxelize(cloud, _laserScanVoxelSize);
+			laserScan = util3d::laserScanFromPointCloud(*cloud);
+		}
+		else
+		{
+			pcl::PointCloud<pcl::PointXYZ>::Ptr cloud = util3d::laserScanToPointCloud(laserScan);
+			cloud = util3d::voxelize(cloud, _laserScanVoxelSize);
+			if(laserScan.channels() == 2 || laserScan.channels() == 5)
+			{
+				laserScan = util3d::laserScan2dFromPointCloud(*cloud);
+			}
+			else
+			{
+				laserScan = util3d::laserScanFromPointCloud(*cloud);
+			}
+		}
+		float ratio = float(laserScan.cols) / pointsBeforeFiltering;
+		maxLaserScanMaxPts = int(float(maxLaserScanMaxPts) * ratio);
+
+		t = timer.ticks();
+		if(stats) stats->addStatistic(Statistics::kTimingMemScan_voxel_filtering(), t*1000.0f);
+		UDEBUG("time voxel filtering scan = %fs", t);
+	}
+	if(!laserScan.empty() &&
+		(_laserScanNormalK > 0 || _laserScanNormalRadius>0.0f) &&
+		laserScan.channels() > 1 && laserScan.channels() < 5 &&
+		!isIntermediateNode)
+	{
+		laserScan = util3d::computeNormals(laserScan, _laserScanNormalK, _laserScanNormalRadius);
 		t = timer.ticks();
 		if(stats) stats->addStatistic(Statistics::kTimingMemScan_normals(), t*1000.0f);
 		UDEBUG("time normals scan = %fs", t);
--- a/corelib/src/OccupancyGrid.cpp
+++ b/corelib/src/OccupancyGrid.cpp
@@ -207,7 +207,7 @@ void OccupancyGrid::createLocalMap(
 	UDEBUG("scan channels=%d, occupancyFromCloud_=%d normalsSegmentation_=%d grid3D_=%d",
 			node.sensorData().laserScanRaw().empty()?0:node.sensorData().laserScanRaw().channels(), occupancyFromCloud_?1:0, normalsSegmentation_?1:0, grid3D_?1:0);

-	if(node.sensorData().laserScanRaw().channels() == 2 && !occupancyFromCloud_)
+	if((node.sensorData().laserScanRaw().channels() == 2 || node.sensorData().laserScanRaw().channels() == 5) && !occupancyFromCloud_)
 	{
 		UDEBUG("2D laser scan");
 		//2D
--- a/corelib/src/Odometry.cpp
+++ b/corelib/src/Odometry.cpp
@@ -102,7 +102,8 @@ Odometry::Odometry(const rtabmap::ParametersMap & parameters) :
 		_pose(Transform::getIdentity()),
 		_resetCurrentCount(0),
 		previousStamp_(0),
-		distanceTravelled_(0)
+		distanceTravelled_(0),
+		framesProcessed_(0)
 {
 	Parameters::parse(parameters, Parameters::kOdomResetCountdown(), _resetCountdown);

@@ -168,6 +169,7 @@ void Odometry::reset(const Transform & initialPose)
 	_resetCurrentCount = 0;
 	previousStamp_ = 0;
 	distanceTravelled_ = 0;
+	framesProcessed_ = 0;
 	if(_force3DoF || particleFilters_.size())
 	{
 		float x,y,z, roll,pitch,yaw;
@@ -544,6 +546,7 @@ Transform Odometry::process(SensorData & data, const Transform & guessIn, Odomet
 			distanceTravelled_ += t.getNorm();
 			info->distanceTravelled = distanceTravelled_;
 		}
+		++framesProcessed_;

 		return _pose *= t; // update
 	}
--- a/corelib/src/OdometryDVO.cpp
+++ b/corelib/src/OdometryDVO.cpp
@@ -28,7 +28,6 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "rtabmap/core/OdometryDVO.h"
 #include "rtabmap/core/OdometryInfo.h"
 #include "rtabmap/core/util2d.h"
-#include "rtabmap/core/Version.h"
 #include "rtabmap/utilite/ULogger.h"
 #include "rtabmap/utilite/UTimer.h"
 #include "rtabmap/utilite/UStl.h"
@@ -43,10 +42,12 @@ namespace rtabmap {

 OdometryDVO::OdometryDVO(const ParametersMap & parameters) :
 	Odometry(parameters),
+#ifdef RTABMAP_DVO
 	dvo_(0),
 	reference_(0),
 	camera_(0),
 	lost_(false),
+#endif
 	motionFromKeyFrame_(Transform::getIdentity())
 {
 }
@@ -270,7 +271,7 @@ Transform OdometryDVO::computeTransform(
 	if(info)
 	{
 		info->type = (int)kTypeDVO;
-		info->covariance = covariance;
+		info->reg.covariance = covariance;
 	}

 	UINFO("Odom update time = %fs", timer.elapsed());
--- a/corelib/src/OdometryF2F.cpp
+++ b/corelib/src/OdometryF2F.cpp
@@ -83,6 +83,7 @@ Transform OdometryF2F::computeTransform(
 		return output;
 	}

+	bool addKeyFrame = false;
 	RegistrationInfo regInfo;

 	UASSERT(!this->getPose().isNull());
@@ -99,8 +100,8 @@ Transform OdometryF2F::computeTransform(
 		output = registrationPipeline_->computeTransformationMod(
 				tmpRefFrame,
 				newFrame,
-				// special case for ICP-only odom, set guess to identity if we just started
-				!guess.isNull()?motionSinceLastKeyFrame*guess:!registrationPipeline_->isImageRequired()&&this->getPose().isIdentity()?Transform::getIdentity():Transform(),
+				// special case for ICP-only odom, set guess to identity if we just started or reset
+				!guess.isNull()?motionSinceLastKeyFrame*guess:!registrationPipeline_->isImageRequired()&&this->framesProcessed()<2?motionSinceLastKeyFrame:Transform(),
 				&regInfo);

 		if(output.isNull() && !guess.isNull() && registrationPipeline_->isImageRequired())
@@ -185,7 +186,7 @@ Transform OdometryF2F::computeTransform(
 		{
 			UDEBUG("Update key frame");
 			int features = newFrame.getWordsDescriptors().size();
-			if(features == 0)
+			if(registrationPipeline_->isImageRequired() && features == 0)
 			{
 				newFrame = Signature(data);
 				// this will generate features only for the first frame or if optical flow was used (no 3d words)
@@ -209,6 +210,8 @@ Transform OdometryF2F::computeTransform(

 				//reset motion
 				lastKeyFramePose_.setNull();
+
+				addKeyFrame = true;
 			}
 			else
 			{
@@ -243,12 +246,17 @@ Transform OdometryF2F::computeTransform(

 	if(info)
 	{
-		info->type = 1;
-		info->covariance = regInfo.covariance;
-		info->inliers = regInfo.inliers;
-		info->icpInliersRatio = regInfo.icpInliersRatio;
-		info->matches = regInfo.matches;
+		info->type = kTypeF2F;
 		info->features = newFrame.sensorData().keypoints().size();
+		info->keyFrameAdded = addKeyFrame;
+		if(this->isInfoDataFilled())
+		{
+			info->reg = regInfo;
+		}
+		else
+		{
+			info->reg = regInfo.copyWithoutData();
+		}
 	}

 	UINFO("Odom update time = %fs lost=%s inliers=%d, ref frame corners=%d, transform accepted=%s",
--- a/corelib/src/OdometryF2M.cpp
+++ b/corelib/src/OdometryF2M.cpp
@@ -65,6 +65,7 @@ OdometryF2M::OdometryF2M(const ParametersMap & parameters) :
 	scanKeyFrameThr_(Parameters::defaultOdomScanKeyFrameThr()),
 	scanMaximumMapSize_(Parameters::defaultOdomF2MScanMaxSize()),
 	scanSubtractRadius_(Parameters::defaultOdomF2MScanSubtractRadius()),
+	scanSubtractAngle_(Parameters::defaultOdomF2MScanSubtractAngle()),
 	bundleAdjustment_(Parameters::defaultOdomF2MBundleAdjustment()),
 	bundleMaxFrames_(Parameters::defaultOdomF2MBundleAdjustmentMaxFrames()),
 	map_(new Signature(-1)),
@@ -80,6 +81,10 @@ OdometryF2M::OdometryF2M(const ParametersMap & parameters) :
 	Parameters::parse(parameters, Parameters::kOdomScanKeyFrameThr(), scanKeyFrameThr_);
 	Parameters::parse(parameters, Parameters::kOdomF2MScanMaxSize(), scanMaximumMapSize_);
 	Parameters::parse(parameters, Parameters::kOdomF2MScanSubtractRadius(), scanSubtractRadius_);
+	if(Parameters::parse(parameters, Parameters::kOdomF2MScanSubtractAngle(), scanSubtractAngle_))
+	{
+		scanSubtractAngle_ *= M_PI/180.0f;
+	}
 	Parameters::parse(parameters, Parameters::kOdomF2MBundleAdjustment(), bundleAdjustment_);
 	Parameters::parse(parameters, Parameters::kOdomF2MBundleAdjustmentMaxFrames(), bundleMaxFrames_);
 	UASSERT(bundleMaxFrames_ >= 0);
@@ -186,9 +191,10 @@ Transform OdometryF2M::computeTransform(
 			Transform transform = regPipeline_->computeTransformationMod(
 					tmpMap,
 					*lastFrame_,
-					// special case for ICP-only odom, set guess to identity if we just started
-					!guess.isNull()?this->getPose()*guess:!regPipeline_->isImageRequired()&&this->getPose().isIdentity()?Transform::getIdentity():Transform(),
+					// special case for ICP-only odom, set guess to identity if we just started or reset
+					!guess.isNull()?this->getPose()*guess:!regPipeline_->isImageRequired()&&this->framesProcessed()<2?this->getPose():Transform(),
 					&regInfo);
+
 			if(transform.isNull() && !guess.isNull() && regPipeline_->isImageRequired())
 			{
 				tmpMap = *map_;
@@ -591,7 +597,7 @@ Transform OdometryF2M::computeTransform(

 					if(lastFrame_->sensorData().laserScanRaw().cols)
 					{
-						pcl::PointCloud<pcl::PointNormal>::Ptr mapCloudNormals = util3d::laserScanToPointCloudNormal(mapScan);
+						pcl::PointCloud<pcl::PointNormal>::Ptr mapCloudNormals = util3d::laserScanToPointCloudNormal(mapScan, tmpMap.sensorData().laserScanInfo().localTransform());
 						pcl::PointCloud<pcl::PointNormal>::Ptr frameCloudNormals = util3d::laserScanToPointCloudNormal(lastFrame_->sensorData().laserScanRaw(), newFramePose * lastFrame_->sensorData().laserScanInfo().localTransform());

 						pcl::IndicesPtr frameCloudNormalsIndices(new std::vector<int>);
@@ -604,7 +610,7 @@ Transform OdometryF2M::computeTransform(
 									mapCloudNormals,
 									pcl::IndicesPtr(new std::vector<int>),
 									scanSubtractRadius_,
-									0.0f);
+									scanSubtractAngle_);
 							newPoints = frameCloudNormalsIndices->size();
 						}
 						else
@@ -623,17 +629,6 @@ Transform OdometryF2M::computeTransform(
 									newPoints,
 									scanMaximumMapSize_);

-							if(newPoints < 20)
-							{
-								UWARN("The number of new scan points added to local odometry "
-									  "map is low (%d), you may want to decrease the parameter \"%s\" "
-									  "(current value=%f and ICP inliers ratio is %f)",
-										newPoints,
-										Parameters::kOdomScanKeyFrameThr().c_str(),
-										scanKeyFrameThr_,
-										regInfo.icpInliersRatio);
-							}
-
 							if(scansBuffer_.size() > 1 &&
 								int(mapCloudNormals->size() + newPoints) > scanMaximumMapSize_)
 							{
@@ -691,7 +686,16 @@ Transform OdometryF2M::computeTransform(
 									*mapCloudNormals += *scansBuffer_.back().first;
 								}
 							}
-							mapScan = util3d::laserScanFromPointCloud(*mapCloudNormals);
+							if(mapScan.channels() == 2 || mapScan.channels() == 5)
+							{
+								Transform mapViewpoint(-newFramePose.x(), -newFramePose.y(),0,0,0,0);
+								mapScan = util3d::laserScan2dFromPointCloud(*mapCloudNormals, mapViewpoint);
+							}
+							else
+							{
+								Transform mapViewpoint(-newFramePose.x(), -newFramePose.y(), -newFramePose.z(),0,0,0);
+								mapScan = util3d::laserScanFromPointCloud(*mapCloudNormals, mapViewpoint);
+							}
 							modified=true;
 						}
 					}
@@ -702,7 +706,18 @@ Transform OdometryF2M::computeTransform(
 				{
 					*map_ = tmpMap;

-					map_->sensorData().setLaserScanRaw(mapScan, LaserScanInfo(0, 0));
+					if(mapScan.channels() == 2 || mapScan.channels() == 5)
+					{
+
+						map_->sensorData().setLaserScanRaw(mapScan,
+									LaserScanInfo(0, 0.0f, Transform(newFramePose.x(), newFramePose.y(), lastFrame_->sensorData().laserScanInfo().localTransform().z(),0,0,0)));
+					}
+					else
+					{
+						map_->sensorData().setLaserScanRaw(mapScan,
+								LaserScanInfo(0, 0.0f, newFramePose.translation()));
+					}
+
 					map_->setWords(mapWords);
 					map_->setWords3(mapPoints);
 				 	map_->setWordsDescriptors(mapDescriptors);
@@ -717,7 +732,7 @@ Transform OdometryF2M::computeTransform(
 				if(this->isInfoDataFilled())
 				{
 					info->localMap = uMultimapToMap(tmpMap.getWords3());
-					info->localScanMap = tmpMap.sensorData().laserScanRaw();
+					info->localScanMap = util3d::transformLaserScan(tmpMap.sensorData().laserScanRaw(), tmpMap.sensorData().laserScanInfo().localTransform());
 				}
 			}
 		}
@@ -831,7 +846,18 @@ Transform OdometryF2M::computeTransform(
 					frameValid = true;
 					pcl::PointCloud<pcl::PointNormal>::Ptr mapCloudNormals = util3d::laserScanToPointCloudNormal(lastFrame_->sensorData().laserScanRaw(), newFramePose * lastFrame_->sensorData().laserScanInfo().localTransform());
 					scansBuffer_.push_back(std::make_pair(mapCloudNormals, pcl::IndicesPtr(new std::vector<int>)));
-					map_->sensorData().setLaserScanRaw(util3d::laserScanFromPointCloud(*mapCloudNormals), LaserScanInfo(0,0));
+					if(lastFrame_->sensorData().laserScanRaw().channels() == 2 || lastFrame_->sensorData().laserScanRaw().channels() == 5)
+					{
+						Transform mapViewpoint(-newFramePose.x(), -newFramePose.y(),0,0,0,0);
+						map_->sensorData().setLaserScanRaw(util3d::laserScan2dFromPointCloud(*mapCloudNormals, mapViewpoint),
+								LaserScanInfo(0, 0.0f, Transform(newFramePose.x(), newFramePose.y(), lastFrame_->sensorData().laserScanInfo().localTransform().z(),0,0,0)));
+					}
+					else
+					{
+						Transform mapViewpoint(-newFramePose.x(), -newFramePose.y(), -newFramePose.z(),0,0,0);
+						map_->sensorData().setLaserScanRaw(util3d::laserScanFromPointCloud(*mapCloudNormals, mapViewpoint),
+								LaserScanInfo(0, 0.0f, newFramePose.translation()));
+					}
 					addKeyFrame = true;
 				}
 				else
@@ -854,7 +880,7 @@ Transform OdometryF2M::computeTransform(
 				if(this->isInfoDataFilled())
 				{
 					info->localMap = uMultimapToMap(map_->getWords3());
-					info->localScanMap = map_->sensorData().laserScanRaw();
+					info->localScanMap = util3d::transformLaserScan(map_->sensorData().laserScanRaw(), map_->sensorData().laserScanInfo().localTransform());
 				}
 			}
 		}
@@ -873,10 +899,6 @@ Transform OdometryF2M::computeTransform(

 	if(info)
 	{
-		info->covariance = regInfo.covariance;
-		info->inliers = regInfo.inliers;
-		info->matches = regInfo.matches;
-		info->icpInliersRatio = regInfo.icpInliersRatio;
 		info->features = nFeatures;
 		info->localKeyFrames = (int)bundlePoses_.size();
 		info->keyFrameAdded = addKeyFrame;
@@ -886,8 +908,11 @@ Transform OdometryF2M::computeTransform(

 		if(this->isInfoDataFilled())
 		{
-			info->wordMatches = regInfo.matchesIDs;
-			info->wordInliers = regInfo.inliersIDs;
+			info->reg = regInfo;
+		}
+		else
+		{
+			info->reg = regInfo.copyWithoutData();
 		}
 	}

--- a/corelib/src/OdometryFovis.cpp
+++ b/corelib/src/OdometryFovis.cpp
@@ -28,7 +28,6 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "rtabmap/core/OdometryFovis.h"
 #include "rtabmap/core/OdometryInfo.h"
 #include "rtabmap/core/util2d.h"
-#include "rtabmap/core/Version.h"
 #include "rtabmap/utilite/ULogger.h"
 #include "rtabmap/utilite/UTimer.h"
 #include "rtabmap/utilite/UStl.h"
@@ -40,13 +39,16 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 namespace rtabmap {

 OdometryFovis::OdometryFovis(const ParametersMap & parameters) :
-	Odometry(parameters),
+	Odometry(parameters)
+#ifdef RTABMAP_FOVIS
+    ,
 	fovis_(0),
 	rect_(0),
 	stereoCalib_(0),
 	depthImage_(0),
 	stereoDepth_(0),
 	lost_(false)
+#endif
 {
 	fovisParameters_ = Parameters::filterParameters(parameters, "OdomFovis");
 	if(parameters.find(Parameters::kOdomVisKeyFrameThr()) != parameters.end())
@@ -381,9 +383,9 @@ Transform OdometryFovis::computeTransform(
 		info->type = (int)kTypeFovis;
 		info->keyFrameAdded = fovis_->getChangeReferenceFrames();
 		info->features = fovis_->getTargetFrame()->getNumDetectedKeypoints();
-		info->matches = fovis_->getMotionEstimator()->getNumMatches();
-		info->inliers = fovis_->getMotionEstimator()->getNumInliers();
-		info->covariance = covariance;
+		info->reg.matches = fovis_->getMotionEstimator()->getNumMatches();
+		info->reg.inliers = fovis_->getMotionEstimator()->getNumInliers();
+		info->reg.covariance = covariance;

 		if(this->isInfoDataFilled())
 		{
--- a/corelib/src/OdometryMono.cpp
+++ b/corelib/src/OdometryMono.cpp
@@ -352,7 +352,7 @@ Transform OdometryMono::computeTransform(SensorData & data, const Transform & gu

 					if(this->isInfoDataFilled() && info)
 					{
-						info->wordMatches.insert(info->wordMatches.end(), matches.begin(), matches.end());
+						info->reg.matchesIDs.insert(info->reg.matchesIDs.end(), matches.begin(), matches.end());
 					}
 					correspondences = (int)matches.size();

@@ -397,10 +397,10 @@ Transform OdometryMono::computeTransform(SensorData & data, const Transform & gu

 							if(this->isInfoDataFilled() && info && inliersV.size())
 							{
-								info->wordInliers.resize(inliersV.size());
+								info->reg.inliersIDs.resize(inliersV.size());
 								for(unsigned int i=0; i<inliersV.size(); ++i)
 								{
-									info->wordInliers[i] = matches[inliersV[i]]; // index and ID should match (index starts at 0, ID starts at 1)
+									info->reg.inliersIDs[i] = matches[inliersV[i]]; // index and ID should match (index starts at 0, ID starts at 1)
 								}
 							}

@@ -976,7 +976,7 @@ Transform OdometryMono::computeTransform(SensorData & data, const Transform & gu
 		if(info)
 		{
 			// a very high variance tells that the new pose is not linked with the previous one
-			info->covariance = cv::Mat::eye(6,6,CV_64FC1)*9999.0;
+			info->reg.covariance = cv::Mat::eye(6,6,CV_64FC1)*9999.0;
 		}

 			// generate kpts
@@ -1013,8 +1013,8 @@ Transform OdometryMono::computeTransform(SensorData & data, const Transform & gu
 	if(this->isInfoDataFilled() && info)
 	{
 		//info->variance = variance;
-		info->inliers = inliers;
-		info->matches = correspondences;
+		info->reg.inliers = inliers;
+		info->reg.matches = correspondences;
 		info->features = nFeatures;
 		info->localMapSize = (int)localMap_.size();
 		info->localMap = localMap_;
--- a/corelib/src/OdometryORBSLAM2.cpp
+++ b/corelib/src/OdometryORBSLAM2.cpp
@@ -28,7 +28,6 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "rtabmap/core/OdometryORBSLAM2.h"
 #include "rtabmap/core/OdometryInfo.h"
 #include "rtabmap/core/util2d.h"
-#include "rtabmap/core/Version.h"
 #include "rtabmap/core/util3d_transforms.h"
 #include "rtabmap/utilite/ULogger.h"
 #include "rtabmap/utilite/UTimer.h"
@@ -746,10 +745,13 @@ public:
 namespace rtabmap {

 OdometryORBSLAM2::OdometryORBSLAM2(const ParametersMap & parameters) :
-	Odometry(parameters),
+	Odometry(parameters)
+#ifdef RTABMAP_ORB_SLAM2
+    ,
 	orbslam2_(0),
 	system_(0),
 	firstFrame_(true)
+#endif
 {
 #ifdef RTABMAP_ORB_SLAM2
 	orbslam2_ = new ORBSLAM2System(parameters);
@@ -891,15 +893,15 @@ Transform OdometryORBSLAM2::computeTransform(
 	{
 		info->lost = t.isNull();
 		info->type = (int)kTypeORBSLAM2;
-		info->covariance = covariance;
+		info->reg.covariance = covariance;
 		info->localMapSize = totalMapPoints;
 		info->localKeyFrames = totalKfs;

 		if(this->isInfoDataFilled() && orbslam2_->mpTracker && orbslam2_->mpMap)
 		{
 			const std::vector<cv::KeyPoint> & kpts = orbslam2_->mpTracker->mCurrentFrame.mvKeys;
-			info->wordMatches.resize(kpts.size());
-			info->wordInliers.resize(kpts.size());
+			info->reg.matchesIDs.resize(kpts.size());
+			info->reg.inliersIDs.resize(kpts.size());
 			int oi = 0;
 			for (unsigned int i = 0; i < kpts.size(); ++i)
 			{
@@ -915,14 +917,15 @@ Transform OdometryORBSLAM2::computeTransform(
 				info->words.insert(std::make_pair(wordId, kpts[i]));
 				if(orbslam2_->mpTracker->mCurrentFrame.mvpMapPoints[i] != 0)
 				{
-					info->wordMatches[oi] = wordId;
-					info->wordInliers[oi] = wordId;
+					info->reg.matchesIDs[oi] = wordId;
+					info->reg.inliersIDs[oi] = wordId;
 					++oi;
 				}
 			}
-			info->wordMatches.resize(oi);
-			info->wordInliers.resize(oi);
-			info->inliers = oi;
+			info->reg.matchesIDs.resize(oi);
+			info->reg.inliersIDs.resize(oi);
+			info->reg.inliers = oi;
+			info->reg.matches = oi;

 			std::vector<ORB_SLAM2::MapPoint*> mapPoints = orbslam2_->mpMap->GetAllMapPoints();
 			for (unsigned int i = 0; i < mapPoints.size(); ++i)
--- a/corelib/src/OdometryViso2.cpp
+++ b/corelib/src/OdometryViso2.cpp
@@ -28,7 +28,6 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "rtabmap/core/OdometryViso2.h"
 #include "rtabmap/core/OdometryInfo.h"
 #include "rtabmap/core/util2d.h"
-#include "rtabmap/core/Version.h"
 #include "rtabmap/utilite/ULogger.h"
 #include "rtabmap/utilite/UTimer.h"
 #include "rtabmap/utilite/UStl.h"
@@ -53,15 +52,19 @@ namespace rtabmap {

 OdometryViso2::OdometryViso2(const ParametersMap & parameters) :
 	Odometry(parameters),
+#ifdef RTABMAP_VISO2
 	viso2_(0),
 	ref_frame_change_method_(0),
 	ref_frame_inlier_threshold_(Parameters::defaultOdomVisKeyFrameThr()),
 	ref_frame_motion_threshold_(5.0),
 	lost_(false),
 	keep_reference_frame_(false),
+#endif
 	reference_motion_(Transform::getIdentity())
 {
+#ifdef RTABMAP_VISO2
 	Parameters::parse(parameters, Parameters::kOdomVisKeyFrameThr(), ref_frame_inlier_threshold_);
+#endif
 	viso2Parameters_ = Parameters::filterParameters(parameters, "OdomViso2");
 }

@@ -273,11 +276,11 @@ Transform OdometryViso2::computeTransform(
 	{
 		info->type = (int)kTypeViso2;
 		info->keyFrameAdded = !keep_reference_frame_;
-		info->matches = viso2_->getNumberOfMatches();
-		info->inliers = viso2_->getNumberOfInliers();
+		info->reg.matches = viso2_->getNumberOfMatches();
+		info->reg.inliers = viso2_->getNumberOfInliers();
 		if(covariance.cols == 6 && covariance.rows == 6 && covariance.type() == CV_64FC1)
 		{
-			info->covariance = covariance;
+			info->reg.covariance = covariance;
 		}

 		if(this->isInfoDataFilled())
--- a/corelib/src/OptimizerG2O.cpp
+++ b/corelib/src/OptimizerG2O.cpp
@@ -37,19 +37,22 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <rtabmap/core/util3d_motion_estimation.h>
 #include <rtabmap/core/util3d.h>

-#ifdef RTABMAP_G2O
-#include "g2o/config.h"
+#if defined(RTABMAP_G2O) || defined(RTABMAP_ORB_SLAM2)
 #include "g2o/core/sparse_optimizer.h"
 #include "g2o/core/block_solver.h"
 #include "g2o/core/factory.h"
 #include "g2o/core/optimization_algorithm_factory.h"
 #include "g2o/core/optimization_algorithm_gauss_newton.h"
 #include "g2o/core/optimization_algorithm_levenberg.h"
+#include "g2o/core/robust_kernel_impl.h"
 #include "g2o/core/linear_solver.h"
+
+#ifdef RTABMAP_G2O
 #include "g2o/types/sba/types_sba.h"
+#include "g2o/solvers/eigen/linear_solver_eigen.h"
+#include "g2o/config.h"
 #include "g2o/types/slam2d/types_slam2d.h"
 #include "g2o/types/slam3d/types_slam3d.h"
-#include "g2o/core/robust_kernel_impl.h"
 #ifdef G2O_HAVE_CSPARSE
 #include "g2o/solvers/csparse/linear_solver_csparse.h"
 #endif
@@ -57,14 +60,20 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #ifdef G2O_HAVE_CHOLMOD
 #include "g2o/solvers/cholmod/linear_solver_cholmod.h"
 #endif
-#include "g2o/solvers/eigen/linear_solver_eigen.h"
-
 enum {
    PARAM_OFFSET=0,
 };
+#endif // RTABMAP_G2O
+
+#ifdef RTABMAP_ORB_SLAM2
+#include "g2o/types/types_sba.h"
+#include "g2o/types/types_six_dof_expmap.h"
+#include "g2o/solvers/linear_solver_eigen.h"
+#endif

 typedef g2o::BlockSolver< g2o::BlockSolverTraits<-1, -1> > SlamBlockSolver;
 typedef g2o::LinearSolverEigen<SlamBlockSolver::PoseMatrixType> SlamLinearEigenSolver;
+#ifdef RTABMAP_G2O
 typedef g2o::LinearSolverPCG<SlamBlockSolver::PoseMatrixType> SlamLinearPCGSolver;
 #ifdef G2O_HAVE_CSPARSE
 typedef g2o::LinearSolverCSparse<SlamBlockSolver::PoseMatrixType> SlamLinearCSparseSolver;
@@ -79,14 +88,15 @@ typedef g2o::LinearSolverCholmod<SlamBlockSolver::PoseMatrixType> SlamLinearChol
 #include "vertigo/g2o/edge_se3Switchable.h"
 #include "vertigo/g2o/vertex_switchLinear.h"
 #endif
+#endif

-#endif // end RTABMAP_G2O
+#endif // end defined(RTABMAP_G2O) || defined(RTABMAP_ORB_SLAM2)

 namespace rtabmap {

 bool OptimizerG2O::available()
 {
-#ifdef RTABMAP_G2O
+#if defined(RTABMAP_G2O) || defined(RTABMAP_ORB_SLAM2)
 	return true;
 #else
 	return false;
@@ -123,6 +133,13 @@ void OptimizerG2O::parseParameters(const ParametersMap & parameters)
 	UASSERT(pixelVariance_ > 0.0);
 	UASSERT(baseline_ >= 0.0);

+#ifdef RTABMAP_ORB_SLAM2
+	if(solver_ != 3)
+	{
+		UWARN("g2o built with ORB_SLAM2 has only Eigen solver available, using Eigen=3 instead of %d.", solver_);
+		solver_ = 3;
+	}
+#else
 #ifndef G2O_HAVE_CHOLMOD
 	if(solver_ == 2)
 	{
@@ -138,6 +155,8 @@ void OptimizerG2O::parseParameters(const ParametersMap & parameters)
 		solver_ = 1;
 	}
 #endif
+#endif
+
 }

 std::map<int, Transform> OptimizerG2O::optimize(
@@ -612,8 +631,12 @@ std::map<int, Transform> OptimizerG2O::optimize(
 		UWARN("This method should be called at least with 1 pose!");
 	}
 	UDEBUG("Optimizing graph...end!");
+#else
+#ifdef RTABMAP_ORB_SLAM2
+	UERROR("G2O graph optimization cannot be used with g2o built from ORB_SLAM2, only SBA is available.");
 #else
 	UERROR("Not built with G2O support!");
+#endif
 #endif
 	return optimizedPoses;
 }
@@ -628,7 +651,7 @@ std::map<int, Transform> OptimizerG2O::optimizeBA(
 		std::set<int> * outliers)
 {
 	std::map<int, Transform> optimizedPoses;
-#ifdef RTABMAP_G2O
+#if defined(RTABMAP_G2O) || defined(RTABMAP_ORB_SLAM2)
 	UDEBUG("Optimizing graph...");

 	optimizedPoses.clear();
@@ -638,6 +661,9 @@ std::map<int, Transform> OptimizerG2O::optimizeBA(
 		optimizer.setVerbose(ULogger::level()==ULogger::kDebug);
 		g2o::BlockSolver_6_3::LinearSolverType * linearSolver = 0;

+#ifdef RTABMAP_ORB_SLAM2
+		linearSolver = new g2o::LinearSolverEigen<g2o::BlockSolver_6_3::PoseMatrixType>();
+#else
 		if(solver_ == 3)
 		{
 			//eigen
@@ -663,14 +689,17 @@ std::map<int, Transform> OptimizerG2O::optimizeBA(
 			//pcg
 			linearSolver = new g2o::LinearSolverPCG<g2o::BlockSolver_6_3::PoseMatrixType>();
 		}
+#endif

 		g2o::BlockSolver_6_3 * solver_ptr = new g2o::BlockSolver_6_3(linearSolver);

+#ifndef RTABMAP_ORB_SLAM2
 		if(optimizer_ == 1)
 		{
 			optimizer.setAlgorithm(new g2o::OptimizationAlgorithmGaussNewton(solver_ptr));
 		}
 		else
+#endif
 		{
 			optimizer.setAlgorithm(new g2o::OptimizationAlgorithmLevenberg(solver_ptr));
 		}
@@ -686,9 +715,17 @@ std::map<int, Transform> OptimizerG2O::optimizeBA(

 			// Add node's pose
 			UASSERT(!camPose.isNull());
+#ifdef RTABMAP_ORB_SLAM2
+			g2o::VertexSE3Expmap * vCam = new g2o::VertexSE3Expmap();
+#else
 			g2o::VertexCam * vCam = new g2o::VertexCam();
+#endif

 			Eigen::Affine3d a = camPose.toEigen3d();
+#ifdef RTABMAP_ORB_SLAM2
+			a = a.inverse();
+			vCam->setEstimate(g2o::SE3Quat(a.rotation(), a.translation()));
+#else
 			g2o::SBACam cam(Eigen::Quaterniond(a.rotation()), a.translation());
 			cam.setKcam(
 					iterModel->second.fx(),
@@ -697,6 +734,7 @@ std::map<int, Transform> OptimizerG2O::optimizeBA(
 					iterModel->second.cy(),
 					iterModel->second.Tx()<0.0?-iterModel->second.Tx()/iterModel->second.fx():baseline_); // baseline in meters
 			vCam->setEstimate(cam);
+#endif
 			vCam->setId(iter->first);

 			// negative root means that all other poses should be fixed instead of the root
@@ -718,6 +756,7 @@ std::map<int, Transform> OptimizerG2O::optimizeBA(
 			++iter;
 		}

+#ifndef RTABMAP_ORB_SLAM2
 		UDEBUG("fill edges to g2o...");
 		for(std::multimap<int, Link>::const_iterator iter=links.begin(); iter!=links.end(); ++iter)
 		{
@@ -759,6 +798,7 @@ std::map<int, Transform> OptimizerG2O::optimizeBA(
 				}
 			}
 		}
+#endif

 		UDEBUG("fill 3D points to g2o...");
 		const int stepVertexId = poses.rbegin()->first+1;
@@ -775,7 +815,7 @@ std::map<int, Transform> OptimizerG2O::optimizeBA(
 				vpt3d->setMarginalized(true);
 				optimizer.addVertex(vpt3d);

-				//UDEBUG("Added 3D point %d (%f,%f,%f)", vpt3d->id()-stepVertexId, pt3d.x, pt3d.y, pt3d.z);
+				UDEBUG("Added 3D point %d (%f,%f,%f)", vpt3d->id()-stepVertexId, pt3d.x, pt3d.y, pt3d.z);

 				// set observations
 				for(std::map<int, cv::Point3f>::const_iterator jter=iter->second.begin(); jter!=iter->second.end(); ++jter)
@@ -786,25 +826,62 @@ std::map<int, Transform> OptimizerG2O::optimizeBA(
 						const cv::Point3f & pt = jter->second;
 						double depth = pt.z;

-						//UDEBUG("Added observation pt=%d to cam=%d (%f,%f) d=%f", vpt3d->id()-stepVertexId, camId, pt.x, pt.y, depth);
+						UDEBUG("Added observation pt=%d to cam=%d (%f,%f) d=%f", vpt3d->id()-stepVertexId, camId, pt.x, pt.y, depth);

 						g2o::OptimizableGraph::Edge * e;
+						double baseline = 0.0;
+#ifdef RTABMAP_ORB_SLAM2
+						g2o::VertexSE3Expmap* vcam = dynamic_cast<g2o::VertexSE3Expmap*>(optimizer.vertex(camId));
+						std::map<int, CameraModel>::const_iterator iterModel = models.find(camId);
+
+						cv::Point3f t = util3d::transformPoint(pt3d, Transform::fromEigen3d(vcam->estimate()).inverse());
+						UDEBUG("in cam %d frame=(%f,%f,%f)", camId, t.x, t.y, t.z);
+
+						cv::Point3f t2 = util3d::transformPoint(pt3d, (poses.at(camId)*iterModel->second.localTransform()).inverse());
+						UDEBUG("in cam2 %d frame=(%f,%f,%f)",camId, t2.x, t2.y, t2.z);
+
+						g2o::Vector3d t3 = vcam->estimate().map(g2o::Vector3d(pt3d.x, pt3d.y, pt3d.z));
+						UDEBUG("in cam3 %d frame=(%f,%f,%f)",camId, t3[0], t3[1], t3[2]);
+
+						cv::Point3f t4 = util3d::transformPoint(pt3d, (poses.at(camId)*iterModel->second.localTransform()));
+						UDEBUG("in cam4 %d frame=(%f,%f,%f)",camId, t4.x, t4.y, t4.z);
+
+						UASSERT(iterModel != models.end() && iterModel->second.isValidForProjection());
+						baseline = iterModel->second.Tx()<0.0?-iterModel->second.Tx()/iterModel->second.fx():baseline_;
+#else
 						g2o::VertexCam* vcam = dynamic_cast<g2o::VertexCam*>(optimizer.vertex(camId));
+						baseline = vcam->estimate().baseline;
+#endif
 						double variance = pixelVariance_;
-						if(uIsFinite(depth) && depth > 0.0 && vcam->estimate().baseline > 0.0)
+						if(uIsFinite(depth) && depth > 0.0 && baseline > 0.0)
 						{
 							// stereo edge
+#ifdef RTABMAP_ORB_SLAM2
+							g2o::EdgeStereoSE3ProjectXYZ* es = new g2o::EdgeStereoSE3ProjectXYZ();
+							float disparity = baseline * iterModel->second.fx() / depth;
+							Eigen::Vector3d obs( pt.x, pt.y, pt.x-disparity);
+							es->setMeasurement(obs);
+							//variance *= log(exp(1)+disparity);
+							es->setInformation(Eigen::Matrix3d::Identity() / variance);
+							es->fx = iterModel->second.fx();
+							es->fy = iterModel->second.fy();
+							es->cx = iterModel->second.cx();
+							es->cy = iterModel->second.cy();
+							es->bf = baseline*es->fx;
+							e = es;
+#else
 							g2o::EdgeProjectP2SC* es = new g2o::EdgeProjectP2SC();
-							float disparity = vcam->estimate().baseline * vcam->estimate().Kcam(0,0) / depth;
+							float disparity = baseline * vcam->estimate().Kcam(0,0) / depth;
 							Eigen::Vector3d obs( pt.x, pt.y, pt.x-disparity);
 							es->setMeasurement(obs);
 							//variance *= log(exp(1)+disparity);
 							es->setInformation(Eigen::Matrix3d::Identity() / variance);
 							e = es;
+#endif
 						}
 						else
 						{
-							if(vcam->estimate().baseline > 0.0)
+							if(baseline > 0.0)
 							{
 								UWARN("Stereo camera model detected but current "
 										"observation (pt=%d to cam=%d) has null depth (%f m), adding "
@@ -812,15 +889,28 @@ std::map<int, Transform> OptimizerG2O::optimizeBA(
 										vpt3d->id()-stepVertexId, camId, depth);
 							}
 							// mono edge
+#ifdef RTABMAP_ORB_SLAM2
+							g2o::EdgeSE3ProjectXYZ* em = new g2o::EdgeSE3ProjectXYZ();
+							Eigen::Vector2d obs( pt.x, pt.y);
+							em->setMeasurement(obs);
+							em->setInformation(Eigen::Matrix2d::Identity() / variance);
+							em->fx = iterModel->second.fx();
+							em->fy = iterModel->second.fy();
+							em->cx = iterModel->second.cx();
+							em->cy = iterModel->second.cy();
+							e = em;
+
+#else
 							g2o::EdgeProjectP2MC* em = new g2o::EdgeProjectP2MC();
 							Eigen::Vector2d obs( pt.x, pt.y);
 							em->setMeasurement(obs);
 							em->setInformation(Eigen::Matrix2d::Identity() / variance);
 							e = em;
+#endif
 						}
 						e->setVertex(0, vpt3d);
 						e->setVertex(1, vcam);
-
+						UDEBUG("");
 						if(robustKernelDelta_ > 0.0)
 						{
 							g2o::RobustKernelHuber* kernel = new g2o::RobustKernelHuber;
@@ -875,8 +965,20 @@ std::map<int, Transform> OptimizerG2O::optimizeBA(
 					{
 						(*iter)->setLevel(1);
 						++outliersCount;
-						double d = ((g2o::EdgeProjectP2SC*)(*iter))->measurement()[0]-((g2o::EdgeProjectP2SC*)(*iter))->measurement()[2];
+						double d = 0.0;
+#ifdef RTABMAP_ORB_SLAM2
+						if(dynamic_cast<g2o::EdgeStereoSE3ProjectXYZ*>(*iter) != 0)
+						{
+							d = ((g2o::EdgeStereoSE3ProjectXYZ*)(*iter))->measurement()[0]-((g2o::EdgeStereoSE3ProjectXYZ*)(*iter))->measurement()[2];
+						}
+						UDEBUG("Ignoring edge (%d<->%d) d=%f var=%f kernel=%f chi2=%f", (*iter)->vertex(0)->id()-stepVertexId, (*iter)->vertex(1)->id(), d, 1.0/((g2o::EdgeStereoSE3ProjectXYZ*)(*iter))->information()(0,0), (*iter)->robustKernel()->delta(), (*iter)->chi2());
+#else
+						if(dynamic_cast<g2o::EdgeProjectP2SC*>(*iter) != 0)
+						{
+							d = ((g2o::EdgeProjectP2SC*)(*iter))->measurement()[0]-((g2o::EdgeProjectP2SC*)(*iter))->measurement()[2];
+						}
 						UDEBUG("Ignoring edge (%d<->%d) d=%f var=%f kernel=%f chi2=%f", (*iter)->vertex(0)->id()-stepVertexId, (*iter)->vertex(1)->id(), d, 1.0/((g2o::EdgeProjectP2SC*)(*iter))->information()(0,0), (*iter)->robustKernel()->delta(), (*iter)->chi2());
+#endif

 						const cv::Point3f & pt3d = points3DMap.at((*iter)->vertex(0)->id()-stepVertexId);
 						((g2o::VertexSBAPointXYZ*)(*iter)->vertex(0))->setEstimate(Eigen::Vector3d(pt3d.x, pt3d.y, pt3d.z));
@@ -909,11 +1011,19 @@ std::map<int, Transform> OptimizerG2O::optimizeBA(
 		// update poses
 		for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
 		{
+#ifdef RTABMAP_ORB_SLAM2
+			const g2o::VertexSE3Expmap* v = (const g2o::VertexSE3Expmap*)optimizer.vertex(iter->first);
+#else
 			const g2o::VertexCam* v = (const g2o::VertexCam*)optimizer.vertex(iter->first);
+#endif
 			if(v)
 			{
 				Transform t = Transform::fromEigen3d(v->estimate());

+#ifdef RTABMAP_ORB_SLAM2
+				t=t.inverse();
+#endif
+
 				// remove model local transform
 				t *= models.at(iter->first).localTransform().inverse();
 				UDEBUG("%d from=%s to=%s", iter->first, iter->second.prettyPrint().c_str(), t.prettyPrint().c_str());
--- a/corelib/src/Parameters.cpp
+++ b/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())));
--- a/corelib/src/RegistrationIcp.cpp
+++ b/corelib/src/RegistrationIcp.cpp
@@ -36,16 +36,15 @@ 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
+#include <fstream>
 #include "pointmatcher/PointMatcher.h"
 typedef PointMatcher<float> PM;
 typedef PM::DataPoints DP;

-DP pclToDP(const pcl::PointCloud<pcl::PointXYZ>::Ptr & pclCloud)
+DP pclToDP(const pcl::PointCloud<pcl::PointXYZ>::Ptr & pclCloud, bool is2D)
 {
 	UDEBUG("");
 	typedef DP::Label Label;
@@ -65,8 +64,11 @@ DP pclToDP(const pcl::PointCloud<pcl::PointXYZ>::Ptr & pclCloud)
 	isFeature.push_back(true);
 	featLabels.push_back(Label("y", 1));
 	isFeature.push_back(true);
-	featLabels.push_back(Label("z", 1));
-	isFeature.push_back(true);
+	if(!is2D)
+	{
+		featLabels.push_back(Label("z", 1));
+		isFeature.push_back(true);
+	}
 	featLabels.push_back(Label("pad", 1));

 	// create cloud
@@ -74,20 +76,21 @@ DP pclToDP(const pcl::PointCloud<pcl::PointXYZ>::Ptr & pclCloud)
 	cloud.getFeatureViewByName("pad").setConstant(1);

 	// fill cloud
-	View viewX(cloud.getFeatureViewByName("x"));
-	View viewY(cloud.getFeatureViewByName("y"));
-	View viewZ(cloud.getFeatureViewByName("z"));
+	View view(cloud.getFeatureViewByName("x"));
 	for(unsigned int i=0; i<pclCloud->size(); ++i)
 	{
-		viewX(0, i) = pclCloud->at(i).x;
-		viewY(0, i) = pclCloud->at(i).y;
-		viewZ(0, i) = pclCloud->at(i).z;
+		view(0, i) = pclCloud->at(i).x;
+		view(1, i) = pclCloud->at(i).y;
+		if(!is2D)
+		{
+			view(2, i) = pclCloud->at(i).z;
+		}
 	}

 	return cloud;
 }

-DP pclToDP(const pcl::PointCloud<pcl::PointNormal>::Ptr & pclCloud)
+DP pclToDP(const pcl::PointCloud<pcl::PointNormal>::Ptr & pclCloud, bool is2D)
 {
 	UDEBUG("");
 	typedef DP::Label Label;
@@ -107,8 +110,11 @@ DP pclToDP(const pcl::PointCloud<pcl::PointNormal>::Ptr & pclCloud)
 	isFeature.push_back(true);
 	featLabels.push_back(Label("y", 1));
 	isFeature.push_back(true);
-	featLabels.push_back(Label("z", 1));
-	isFeature.push_back(true);
+	if(!is2D)
+	{
+		featLabels.push_back(Label("z", 1));
+		isFeature.push_back(true);
+	}

 	descLabels.push_back(Label("normals", 3));
 	isFeature.push_back(false);
@@ -122,17 +128,18 @@ DP pclToDP(const pcl::PointCloud<pcl::PointNormal>::Ptr & pclCloud)
 	cloud.getFeatureViewByName("pad").setConstant(1);

 	// fill cloud
-	View viewX(cloud.getFeatureViewByName("x"));
-	View viewY(cloud.getFeatureViewByName("y"));
-	View viewZ(cloud.getFeatureViewByName("z"));
+	View view(cloud.getFeatureViewByName("x"));
 	View viewNormalX(cloud.getDescriptorRowViewByName("normals",0));
 	View viewNormalY(cloud.getDescriptorRowViewByName("normals",1));
 	View viewNormalZ(cloud.getDescriptorRowViewByName("normals",2));
 	for(unsigned int i=0; i<pclCloud->size(); ++i)
 	{
-		viewX(0, i) = pclCloud->at(i).x;
-		viewY(0, i) = pclCloud->at(i).y;
-		viewZ(0, i) = pclCloud->at(i).z;
+		view(0, i) = pclCloud->at(i).x;
+		view(1, i) = pclCloud->at(i).y;
+		if(!is2D)
+		{
+			view(2, i) = pclCloud->at(i).z;
+		}
 		viewNormalX(0, i) = pclCloud->at(i).normal_x;
 		viewNormalY(0, i) = pclCloud->at(i).normal_y;
 		viewNormalZ(0, i) = pclCloud->at(i).normal_z;
@@ -153,14 +160,13 @@ void pclFromDP(const DP & cloud, pcl::PointCloud<pcl::PointXYZ> & pclCloud)
 	pclCloud.is_dense = true;

 		// fill cloud
-	ConstView viewX(cloud.getFeatureViewByName("x"));
-	ConstView viewY(cloud.getFeatureViewByName("y"));
-	ConstView viewZ(cloud.getFeatureViewByName("z"));
+	ConstView view(cloud.getFeatureViewByName("x"));
+	bool is3D = cloud.featureExists("z");
 	for(unsigned int i=0; i<pclCloud.size(); ++i)
 	{
-		pclCloud.at(i).x = viewX(0, i);
-		pclCloud.at(i).y = viewY(0, i);
-		pclCloud.at(i).z = viewZ(0, i);
+		pclCloud.at(i).x = view(0, i);
+		pclCloud.at(i).y = view(1, i);
+		pclCloud.at(i).z = is3D?view(2, i):0;
 	}
 }

@@ -176,17 +182,16 @@ void pclFromDP(const DP & cloud, pcl::PointCloud<pcl::PointNormal> & pclCloud)
 	pclCloud.is_dense = true;

 		// fill cloud
-	ConstView viewX(cloud.getFeatureViewByName("x"));
-	ConstView viewY(cloud.getFeatureViewByName("y"));
-	ConstView viewZ(cloud.getFeatureViewByName("z"));
+	ConstView view(cloud.getFeatureViewByName("x"));
+	bool is3D = cloud.featureExists("z");
 	ConstView viewNormalX(cloud.getDescriptorRowViewByName("normals",0));
 	ConstView viewNormalY(cloud.getDescriptorRowViewByName("normals",1));
 	ConstView viewNormalZ(cloud.getDescriptorRowViewByName("normals",2));
 	for(unsigned int i=0; i<pclCloud.size(); ++i)
 	{
-		pclCloud.at(i).x = viewX(0, i);
-		pclCloud.at(i).y = viewY(0, i);
-		pclCloud.at(i).z = viewZ(0, i);
+		pclCloud.at(i).x = view(0, i);
+		pclCloud.at(i).y = view(1, i);
+		pclCloud.at(i).z = is3D?view(2, i):0;
 		pclCloud.at(i).normal_x = viewNormalX(0, i);
 		pclCloud.at(i).normal_y = viewNormalY(0, i);
 		pclCloud.at(i).normal_z = viewNormalZ(0, i);
@@ -225,7 +230,9 @@ RegistrationIcp::RegistrationIcp(const ParametersMap & parameters, Registration
 	_epsilon(Parameters::defaultIcpEpsilon()),
 	_correspondenceRatio(Parameters::defaultIcpCorrespondenceRatio()),
 	_pointToPlane(Parameters::defaultIcpPointToPlane()),
-	_pointToPlaneNormalNeighbors(Parameters::defaultIcpPointToPlaneNormalNeighbors()),
+	_pointToPlaneK(Parameters::defaultIcpPointToPlaneK()),
+	_pointToPlaneRadius(Parameters::defaultIcpPointToPlaneRadius()),
+	_pointToPlaneMinComplexity(Parameters::defaultIcpPointToPlaneMinComplexity()),
 	_libpointmatcher(Parameters::defaultIcpPM()),
 	_libpointmatcherConfig(Parameters::defaultIcpPMConfig()),
 	_libpointmatcherOutlierRatio(Parameters::defaultIcpPMOutlierRatio()),
@@ -257,7 +264,10 @@ 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::kIcpPointToPlaneMinComplexity(), _pointToPlaneMinComplexity);
+	UASSERT(_pointToPlaneMinComplexity >= 0.0f && _pointToPlaneMinComplexity <= 1.0f);

 	Parameters::parse(parameters, Parameters::kIcpPM(), _libpointmatcher);
 	Parameters::parse(parameters, Parameters::kIcpPMConfig(), _libpointmatcherConfig);
@@ -319,8 +329,20 @@ void RegistrationIcp::parseParameters(const ParametersMap & parameters)
 			icp->outlierFilters.clear();
 			icp->outlierFilters.push_back(PM::get().OutlierFilterRegistrar.create("TrimmedDistOutlierFilter", params));
 			params.clear();
+			if(_pointToPlane)
+			{
+				params["maxAngle"] = uNumber2Str(_maxRotation<=0.0f?M_PI:_maxRotation);
+				icp->outlierFilters.push_back(PM::get().OutlierFilterRegistrar.create("SurfaceNormalOutlierFilter", params));
+				params.clear();

-			icp->errorMinimizer.reset(PM::get().ErrorMinimizerRegistrar.create(_pointToPlane?"PointToPlaneErrorMinimizer":"PointToPointErrorMinimizer"));
+				params["force2D"] = force3DoF()?"1":"0";
+				icp->errorMinimizer.reset(PM::get().ErrorMinimizerRegistrar.create("PointToPlaneErrorMinimizer", params));
+				params.clear();
+			}
+			else
+			{
+				icp->errorMinimizer.reset(PM::get().ErrorMinimizerRegistrar.create("PointToPointErrorMinimizer"));
+			}

 			icp->transformationCheckers.clear();
 			params["maxIterationCount"] = uNumber2Str(_maxIterations);
@@ -332,6 +354,11 @@ void RegistrationIcp::parseParameters(const ParametersMap & parameters)
 			params["smoothLength"] =    uNumber2Str(4);
 			icp->transformationCheckers.push_back(PM::get().TransformationCheckerRegistrar.create("DifferentialTransformationChecker", params));
 			params.clear();
+
+			params["maxRotationNorm"] = uNumber2Str(_maxRotation<=0.0f?M_PI:_maxRotation);
+			params["maxTranslationNorm"] =    uNumber2Str(_maxTranslation<=0.0f?std::numeric_limits<float>::max():_maxTranslation);
+			icp->transformationCheckers.push_back(PM::get().TransformationCheckerRegistrar.create("BoundTransformationChecker", params));
+			params.clear();
 		}
 	}
 #endif
@@ -342,7 +369,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 +381,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);
@@ -403,199 +431,46 @@ Transform RegistrationIcp::computeTransformationImpl(
 			float correspondencesRatio = 0.0f;
 			int correspondences = 0;
 			double variance = 1.0;
+			bool transformComputed = false;
+			bool tooLowComplexityForPlaneToPlane = false;
+			cv::Mat complexityVectors;

 			if( _pointToPlane &&
 				_voxelSize == 0.0f &&
-				fromScan.channels() == 6 &&
-				toScan.channels() == 6)
+				fromScan.channels() >= 5 &&
+				toScan.channels() >= 5 &&
+				!((fromScan.channels() == 5 || toScan.channels() == 5) && !_libpointmatcher)) // PCL crashes if 2D)
 			{
 				//special case if we have already normals computed and there is no filtering
-				pcl::PointCloud<pcl::PointNormal>::Ptr fromCloudNormals = util3d::laserScanToPointCloudNormal(fromScan, fromLocalTransform);
-				pcl::PointCloud<pcl::PointNormal>::Ptr toCloudNormals = util3d::laserScanToPointCloudNormal(toScan, guess * toLocalTransform);

-				UDEBUG("Conversion time = %f s", timer.ticks());
-				pcl::PointCloud<pcl::PointNormal>::Ptr fromCloudNormalsRegistered(new pcl::PointCloud<pcl::PointNormal>());
-#ifdef RTABMAP_POINTMATCHER
-				if(_libpointmatcher)
+				cv::Mat complexityVectorsFrom, complexityVectorsTo;
+				double fromComplexity = util3d::computeNormalsComplexity(fromScan, &complexityVectorsFrom);
+				double toComplexity = util3d::computeNormalsComplexity(toScan, &complexityVectorsTo);
+				float complexity = fromComplexity<toComplexity?fromComplexity:toComplexity;
+				info.icpStructuralComplexity = complexity;
+				if(complexity < _pointToPlaneMinComplexity)
 				{
-					// Load point clouds
-					DP data = pclToDP(fromCloudNormals);
-					DP ref = pclToDP(toCloudNormals);
-
-					// Compute the transformation to express data in ref
-					PM::TransformationParameters T;
-					try
-					{
-						UASSERT(_libpointmatcherICP != 0);
-						PM::ICP & icp = *((PM::ICP*)_libpointmatcherICP);
-						UDEBUG("libpointmatcher icp... (if there is a seg fault here, make sure all third party libraries are built with same Eigen version.)");
-						T = icp(data, ref);
-						UDEBUG("libpointmatcher icp...done!");
-						icpT = Transform::fromEigen3d(Eigen::Affine3d(Eigen::Matrix4d(eigenMatrixToDim<double>(T.template cast<double>(), 4))));
-
-						float matchRatio = icp.errorMinimizer->getWeightedPointUsedRatio();
-						UDEBUG("match ratio: %f", matchRatio);
-
-						if(!icpT.isNull())
-						{
-							fromCloudNormalsRegistered = util3d::transformPointCloud(fromCloudNormals, icpT);
-							hasConverged = true;
-						}
-					}
-					catch(const std::exception & e)
-					{
-						UWARN("libpointmatcher has failed: %s", e.what());
-					}
+					tooLowComplexityForPlaneToPlane = true;
+					complexityVectors = fromComplexity<toComplexity?complexityVectorsFrom:complexityVectorsTo;
+					UWARN("ICP PointToPlane ignored as structural complexity is too low (corridor-like environment): %f < %f (%s). PointToPoint is done instead.", complexity, _pointToPlaneMinComplexity, Parameters::kIcpPointToPlaneMinComplexity().c_str());
 				}
 				else
-#endif
 				{
-					icpT = util3d::icpPointToPlane(
-							fromCloudNormals,
-							toCloudNormals,
-						   _maxCorrespondenceDistance,
-						   _maxIterations,
-						   hasConverged,
-						   *fromCloudNormalsRegistered,
-						   _epsilon,
-						   this->force3DoF());
-				}
+					pcl::PointCloud<pcl::PointNormal>::Ptr fromCloudNormals = util3d::laserScanToPointCloudNormal(fromScan, fromLocalTransform);
+					pcl::PointCloud<pcl::PointNormal>::Ptr toCloudNormals = util3d::laserScanToPointCloudNormal(toScan, guess * toLocalTransform);

-				if(!icpT.isNull() && hasConverged)
-				{
-					util3d::computeVarianceAndCorrespondences(
-							fromCloudNormalsRegistered,
-							toCloudNormals,
-							_maxCorrespondenceDistance,
-							variance,
-							correspondences);
-				}
-			}
-			else
-			{
-				pcl::PointCloud<pcl::PointXYZ>::Ptr fromCloud = util3d::laserScanToPointCloud(fromScan, fromLocalTransform);
-				pcl::PointCloud<pcl::PointXYZ>::Ptr toCloud = util3d::laserScanToPointCloud(toScan, guess * toLocalTransform);
-				UDEBUG("Conversion time = %f s", timer.ticks());
-
-				pcl::PointCloud<pcl::PointXYZ>::Ptr fromCloudFiltered = fromCloud;
-				pcl::PointCloud<pcl::PointXYZ>::Ptr toCloudFiltered = toCloud;
-				if(_voxelSize > 0.0f)
-				{
-					int pointsBeforeFiltering = fromCloudFiltered->size();
-					fromCloudFiltered = util3d::voxelize(fromCloudFiltered, _voxelSize);
-					maxLaserScansFrom = maxLaserScansFrom * fromCloudFiltered->size() / pointsBeforeFiltering;
-
-					pointsBeforeFiltering = toCloudFiltered->size();
-					toCloudFiltered = util3d::voxelize(toCloudFiltered, _voxelSize);
-					maxLaserScansTo = maxLaserScansTo * toCloudFiltered->size() / pointsBeforeFiltering;
-
-					UDEBUG("Voxel filtering time (voxel=%f m, ratioFrom=%f ratioTo=%f) = %f s",
-							_voxelSize,
-							float(fromCloudFiltered->size()) / float(pointsBeforeFiltering),
-							float(toCloudFiltered->size()) / float(pointsBeforeFiltering),
-							timer.ticks());
-				}
-
-				pcl::PointCloud<pcl::PointXYZ>::Ptr fromCloudRegistered(new pcl::PointCloud<pcl::PointXYZ>());
-				if(_pointToPlane) // ICP Point To Plane, only in 3D
-				{
-					pcl::PointCloud<pcl::Normal>::Ptr normals;
-
-					normals = util3d::computeNormals(fromCloudFiltered, _pointToPlaneNormalNeighbors);
-					pcl::PointCloud<pcl::PointNormal>::Ptr fromCloudNormals(new pcl::PointCloud<pcl::PointNormal>);
-					pcl::concatenateFields(*fromCloudFiltered, *normals, *fromCloudNormals);
-
-					normals = util3d::computeNormals(toCloudFiltered, _pointToPlaneNormalNeighbors);
-					pcl::PointCloud<pcl::PointNormal>::Ptr toCloudNormals(new pcl::PointCloud<pcl::PointNormal>);
-					pcl::concatenateFields(*toCloudFiltered, *normals, *toCloudNormals);
-
-					std::vector<int> indices;
-					toCloudNormals = util3d::removeNaNNormalsFromPointCloud(toCloudNormals);
 					fromCloudNormals = util3d::removeNaNNormalsFromPointCloud(fromCloudNormals);
-
-					// update output scans
-					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());
-
-					if(toCloudNormals->size() && fromCloudNormals->size())
-					{
-						pcl::PointCloud<pcl::PointNormal>::Ptr fromCloudNormalsRegistered(new pcl::PointCloud<pcl::PointNormal>());
-
-#ifdef RTABMAP_POINTMATCHER
-						if(_libpointmatcher)
-						{
-							// Load point clouds
-							DP data = pclToDP(fromCloudNormals);
-							DP ref = pclToDP(toCloudNormals);
-
-							// Compute the transformation to express data in ref
-							PM::TransformationParameters T;
-							try
-							{
-								UASSERT(_libpointmatcherICP != 0);
-								PM::ICP & icp = *((PM::ICP*)_libpointmatcherICP);
-								UDEBUG("libpointmatcher icp... (if there is a seg fault here, make sure all third party libraries are built with same Eigen version.)");
-								T = icp(data, ref);
-								UDEBUG("libpointmatcher icp...done!");
-								icpT = Transform::fromEigen3d(Eigen::Affine3d(Eigen::Matrix4d(eigenMatrixToDim<double>(T.template cast<double>(), 4))));
-
-								float matchRatio = icp.errorMinimizer->getWeightedPointUsedRatio();
-								UDEBUG("match ratio: %f", matchRatio);
-
-								if(!icpT.isNull())
-								{
-									fromCloudNormalsRegistered = util3d::transformPointCloud(fromCloudNormals, icpT);
-									hasConverged = true;
-								}
-							}
-							catch(const std::exception & e)
-							{
-								UWARN("libpointmatcher has failed: %s", e.what());
-							}
-						}
-						else
-#endif
-						{
-							icpT = util3d::icpPointToPlane(
-									fromCloudNormals,
-									toCloudNormals,
-								   _maxCorrespondenceDistance,
-								   _maxIterations,
-								   hasConverged,
-								   *fromCloudNormalsRegistered,
-								   _epsilon,
-								   this->force3DoF());
-						}
+					toCloudNormals = util3d::removeNaNNormalsFromPointCloud(toCloudNormals);


-						if(!icpT.isNull() && hasConverged)
-						{
-							util3d::computeVarianceAndCorrespondences(
-									fromCloudNormalsRegistered,
-									toCloudNormals,
-									_maxCorrespondenceDistance,
-									variance,
-									correspondences);
-						}
-					}
-				}
-				else // ICP Point to Point
-				{
-					if(_voxelSize > 0.0f)
-					{
-						// update output scans
-						fromSignature.sensorData().setLaserScanRaw(util3d::laserScanFromPointCloud(*fromCloudFiltered, fromLocalTransform.inverse()), LaserScanInfo(maxLaserScansFrom, fromSignature.sensorData().laserScanInfo().maxRange(), fromLocalTransform));
-						toSignature.sensorData().setLaserScanRaw(util3d::laserScanFromPointCloud(*toCloudFiltered, (guess*toLocalTransform).inverse()), LaserScanInfo(maxLaserScansTo, toSignature.sensorData().laserScanInfo().maxRange(), toLocalTransform));
-					}
-
+					UDEBUG("Conversion time = %f s", timer.ticks());
+					pcl::PointCloud<pcl::PointNormal>::Ptr fromCloudNormalsRegistered(new pcl::PointCloud<pcl::PointNormal>());
 #ifdef RTABMAP_POINTMATCHER
 					if(_libpointmatcher)
 					{
 						// Load point clouds
-						DP data = pclToDP(fromCloudFiltered);
-						DP ref = pclToDP(toCloudFiltered);
+						DP data = pclToDP(fromCloudNormals, fromScan.channels() == 5);
+						DP ref = pclToDP(toCloudNormals, toScan.channels() == 5);

 						// Compute the transformation to express data in ref
 						PM::TransformationParameters T;
@@ -605,6 +480,311 @@ Transform RegistrationIcp::computeTransformationImpl(
 							PM::ICP & icp = *((PM::ICP*)_libpointmatcherICP);
 							UDEBUG("libpointmatcher icp... (if there is a seg fault here, make sure all third party libraries are built with same Eigen version.)");
 							T = icp(data, ref);
+							icpT = Transform::fromEigen3d(Eigen::Affine3d(Eigen::Matrix4d(eigenMatrixToDim<double>(T.template cast<double>(), 4))));
+							UDEBUG("libpointmatcher icp...done! T=%s", icpT.prettyPrint().c_str());
+
+							float matchRatio = icp.errorMinimizer->getWeightedPointUsedRatio();
+							UDEBUG("match ratio: %f", matchRatio);
+
+							if(!icpT.isNull())
+							{
+								fromCloudNormalsRegistered = util3d::transformPointCloud(fromCloudNormals, icpT);
+								hasConverged = true;
+							}
+						}
+						catch(const std::exception & e)
+						{
+							UWARN("libpointmatcher has failed: %s", e.what());
+						}
+					}
+					else
+#endif
+					{
+						icpT = util3d::icpPointToPlane(
+								fromCloudNormals,
+								toCloudNormals,
+							   _maxCorrespondenceDistance,
+							   _maxIterations,
+							   hasConverged,
+							   *fromCloudNormalsRegistered,
+							   _epsilon,
+							   this->force3DoF());
+					}
+
+					if(!icpT.isNull() && hasConverged)
+					{
+						util3d::computeVarianceAndCorrespondences(
+								fromCloudNormalsRegistered,
+								toCloudNormals,
+								_maxCorrespondenceDistance,
+								variance,
+								correspondences);
+					}
+					transformComputed = true;
+				}
+			}
+
+			if(!transformComputed)
+			{
+				pcl::PointCloud<pcl::PointXYZ>::Ptr fromCloud = util3d::laserScanToPointCloud(fromScan, fromLocalTransform);
+				pcl::PointCloud<pcl::PointXYZ>::Ptr toCloud = util3d::laserScanToPointCloud(toScan, guess * toLocalTransform);
+				UDEBUG("Conversion time = %f s", timer.ticks());
+
+				pcl::PointCloud<pcl::PointXYZ>::Ptr fromCloudFiltered = fromCloud;
+				pcl::PointCloud<pcl::PointXYZ>::Ptr toCloudFiltered = toCloud;
+				if(_voxelSize > 0.0f)
+				{
+					float pointsBeforeFiltering = (float)fromCloudFiltered->size();
+					fromCloudFiltered = util3d::voxelize(fromCloudFiltered, _voxelSize);
+					float ratioFrom = float(fromCloudFiltered->size()) / pointsBeforeFiltering;
+					maxLaserScansFrom = int(float(maxLaserScansFrom) * ratioFrom);
+
+					pointsBeforeFiltering = (float)toCloudFiltered->size();
+					toCloudFiltered = util3d::voxelize(toCloudFiltered, _voxelSize);
+					float ratioTo = float(toCloudFiltered->size()) / pointsBeforeFiltering;
+					maxLaserScansTo = int(float(maxLaserScansTo) * ratioTo);
+
+					UDEBUG("Voxel filtering time (voxel=%f m, ratioFrom=%f->%d/%d ratioTo=%f->%d/%d) = %f s",
+							_voxelSize,
+							ratioFrom,
+							(int)fromCloudFiltered->size(),
+							maxLaserScansFrom,
+							ratioTo,
+							(int)toCloudFiltered->size(),
+							maxLaserScansTo,
+							timer.ticks());
+				}
+
+				pcl::PointCloud<pcl::PointXYZ>::Ptr fromCloudRegistered(new pcl::PointCloud<pcl::PointXYZ>());
+				if(_pointToPlane && // ICP Point To Plane
+					!tooLowComplexityForPlaneToPlane && // if previously rejected above
+					!((fromScan.channels() == 2 || fromScan.channels() == 5 || toScan.channels() == 2 || toScan.channels() == 5) && !_libpointmatcher)) // PCL crashes if 2D
+				{
+					Eigen::Vector3f viewpointFrom(fromLocalTransform.x(), fromLocalTransform.y(), fromLocalTransform.z());
+					pcl::PointCloud<pcl::Normal>::Ptr normalsFrom;
+					if(fromScan.channels() == 2 || fromScan.channels() == 5)
+					{
+						if(_voxelSize > 0.0f)
+						{
+							normalsFrom = util3d::computeNormals2D(
+									fromCloudFiltered,
+									_pointToPlaneK,
+									_pointToPlaneRadius,
+									viewpointFrom);
+						}
+						else
+						{
+							normalsFrom = util3d::computeFastOrganizedNormals2D(
+									fromCloudFiltered,
+									_pointToPlaneK,
+									_pointToPlaneRadius,
+									viewpointFrom);
+						}
+					}
+					else
+					{
+						normalsFrom = util3d::computeNormals(fromCloudFiltered, _pointToPlaneK, _pointToPlaneRadius, viewpointFrom);
+					}
+
+					Transform toT = guess * toLocalTransform;
+					Eigen::Vector3f viewpointTo(toT.x(), toT.y(), toT.z());
+					pcl::PointCloud<pcl::Normal>::Ptr normalsTo;
+					if(toScan.channels() == 2 || toScan.channels() == 5)
+					{
+						if(_voxelSize > 0.0f)
+						{
+							normalsTo = util3d::computeNormals2D(
+									toCloudFiltered,
+									_pointToPlaneK,
+									_pointToPlaneRadius,
+									viewpointTo);
+						}
+						else
+						{
+							normalsTo = util3d::computeFastOrganizedNormals2D(
+									toCloudFiltered,
+									_pointToPlaneK,
+									_pointToPlaneRadius,
+									viewpointTo);
+						}
+					}
+					else
+					{
+						normalsTo = util3d::computeNormals(toCloudFiltered, _pointToPlaneK, _pointToPlaneRadius, viewpointTo);
+					}
+
+					cv::Mat complexityVectorsFrom, complexityVectorsTo;
+					double fromComplexity = util3d::computeNormalsComplexity(*normalsFrom, fromScan.channels() == 2 || fromScan.channels() == 5, &complexityVectorsFrom);
+					double toComplexity = util3d::computeNormalsComplexity(*normalsTo, toScan.channels() == 2 || toScan.channels() == 5, &complexityVectorsTo);
+					float complexity = fromComplexity<toComplexity?fromComplexity:toComplexity;
+					info.icpStructuralComplexity = complexity;
+					if(complexity < _pointToPlaneMinComplexity)
+					{
+						tooLowComplexityForPlaneToPlane = true;
+						complexityVectors = fromComplexity<toComplexity?complexityVectorsFrom:complexityVectorsTo;
+						UWARN("ICP PointToPlane ignored as structural complexity is too low (corridor-like environment): %f < %f (%s). PointToPoint is done instead.", complexity, _pointToPlaneMinComplexity, Parameters::kIcpPointToPlaneMinComplexity().c_str());
+					}
+					else
+					{
+						pcl::PointCloud<pcl::PointNormal>::Ptr fromCloudNormals(new pcl::PointCloud<pcl::PointNormal>);
+						pcl::concatenateFields(*fromCloudFiltered, *normalsFrom, *fromCloudNormals);
+
+						pcl::PointCloud<pcl::PointNormal>::Ptr toCloudNormals(new pcl::PointCloud<pcl::PointNormal>);
+						pcl::concatenateFields(*toCloudFiltered, *normalsTo, *toCloudNormals);
+
+						std::vector<int> indices;
+						toCloudNormals = util3d::removeNaNNormalsFromPointCloud(toCloudNormals);
+						fromCloudNormals = util3d::removeNaNNormalsFromPointCloud(fromCloudNormals);
+
+						// update output scans
+						if(fromScan.channels() == 2 || fromScan.channels() == 5)
+						{
+							fromSignature.sensorData().setLaserScanRaw(util3d::laserScan2dFromPointCloud(*fromCloudNormals, fromLocalTransform.inverse()), LaserScanInfo(maxLaserScansFrom, fromSignature.sensorData().laserScanInfo().maxRange(), fromLocalTransform));
+						}
+						else
+						{
+							fromSignature.sensorData().setLaserScanRaw(util3d::laserScanFromPointCloud(*fromCloudNormals, fromLocalTransform.inverse()), LaserScanInfo(maxLaserScansFrom, fromSignature.sensorData().laserScanInfo().maxRange(), fromLocalTransform));
+						}
+						if(toScan.channels() == 2 || toScan.channels() == 5)
+						{
+							toSignature.sensorData().setLaserScanRaw(util3d::laserScan2dFromPointCloud(*toCloudNormals, (guess*toLocalTransform).inverse()), LaserScanInfo(maxLaserScansTo, toSignature.sensorData().laserScanInfo().maxRange(), toLocalTransform));
+						}
+						else
+						{
+							toSignature.sensorData().setLaserScanRaw(util3d::laserScanFromPointCloud(*toCloudNormals, (guess*toLocalTransform).inverse()), LaserScanInfo(maxLaserScansTo, toSignature.sensorData().laserScanInfo().maxRange(), toLocalTransform));
+						}
+						UDEBUG("Compute normals (%d,%d) time = %f s", (int)fromCloudNormals->size(), (int)toCloudNormals->size(), timer.ticks());
+
+						if(toCloudNormals->size() && fromCloudNormals->size())
+						{
+							pcl::PointCloud<pcl::PointNormal>::Ptr fromCloudNormalsRegistered(new pcl::PointCloud<pcl::PointNormal>());
+
+#ifdef RTABMAP_POINTMATCHER
+							if(_libpointmatcher)
+							{
+								// Load point clouds
+								DP data = pclToDP(fromCloudNormals, fromScan.channels() == 2 || fromScan.channels() == 5);
+								DP ref = pclToDP(toCloudNormals, toScan.channels() == 2 || toScan.channels() == 5);
+
+								// Compute the transformation to express data in ref
+								PM::TransformationParameters T;
+								try
+								{
+									UASSERT(_libpointmatcherICP != 0);
+									PM::ICP & icp = *((PM::ICP*)_libpointmatcherICP);
+									UDEBUG("libpointmatcher icp... (if there is a seg fault here, make sure all third party libraries are built with same Eigen version.)");
+									T = icp(data, ref);
+									UDEBUG("libpointmatcher icp...done!");
+									icpT = Transform::fromEigen3d(Eigen::Affine3d(Eigen::Matrix4d(eigenMatrixToDim<double>(T.template cast<double>(), 4))));
+
+									float matchRatio = icp.errorMinimizer->getWeightedPointUsedRatio();
+									UDEBUG("match ratio: %f", matchRatio);
+
+									if(!icpT.isNull())
+									{
+										fromCloudNormalsRegistered = util3d::transformPointCloud(fromCloudNormals, icpT);
+										hasConverged = true;
+									}
+								}
+								catch(const std::exception & e)
+								{
+									UWARN("libpointmatcher has failed: %s", e.what());
+								}
+							}
+							else
+#endif
+							{
+								icpT = util3d::icpPointToPlane(
+										fromCloudNormals,
+										toCloudNormals,
+									   _maxCorrespondenceDistance,
+									   _maxIterations,
+									   hasConverged,
+									   *fromCloudNormalsRegistered,
+									   _epsilon,
+									   this->force3DoF());
+							}
+
+							if(!icpT.isNull() && hasConverged)
+							{
+								util3d::computeVarianceAndCorrespondences(
+										fromCloudNormalsRegistered,
+										toCloudNormals,
+										_maxCorrespondenceDistance,
+										variance,
+										correspondences);
+							}
+						}
+						transformComputed = true;
+					}
+				}
+
+				if(!transformComputed) // ICP Point to Point
+				{
+					if(_pointToPlane && !tooLowComplexityForPlaneToPlane && ((fromScan.channels() == 2 || fromScan.channels() == 5 || toScan.channels() == 2 || toScan.channels() == 5) && !_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 || !tooLowComplexityForPlaneToPlane)
+					{
+						// update output scans
+						if(fromScan.channels() == 2 || fromScan.channels() == 5)
+						{
+							fromSignature.sensorData().setLaserScanRaw(util3d::laserScan2dFromPointCloud(*fromCloudFiltered, fromLocalTransform.inverse()), LaserScanInfo(maxLaserScansFrom, fromSignature.sensorData().laserScanInfo().maxRange(), fromLocalTransform));
+						}
+						else
+						{
+							fromSignature.sensorData().setLaserScanRaw(util3d::laserScanFromPointCloud(*fromCloudFiltered, fromLocalTransform.inverse()), LaserScanInfo(maxLaserScansFrom, fromSignature.sensorData().laserScanInfo().maxRange(), fromLocalTransform));
+						}
+						if(toScan.channels() == 2 || toScan.channels() == 5)
+						{
+							toSignature.sensorData().setLaserScanRaw(util3d::laserScan2dFromPointCloud(*toCloudFiltered, (guess*toLocalTransform).inverse()), LaserScanInfo(maxLaserScansTo, toSignature.sensorData().laserScanInfo().maxRange(), toLocalTransform));
+						}
+						else
+						{
+							toSignature.sensorData().setLaserScanRaw(util3d::laserScanFromPointCloud(*toCloudFiltered, (guess*toLocalTransform).inverse()), LaserScanInfo(maxLaserScansTo, toSignature.sensorData().laserScanInfo().maxRange(), toLocalTransform));
+						}
+					}
+
+#ifdef RTABMAP_POINTMATCHER
+					if(_libpointmatcher)
+					{
+						// Load point clouds
+						DP data = pclToDP(fromCloudFiltered, fromScan.channels() == 2 || fromScan.channels() == 5);
+						DP ref = pclToDP(toCloudFiltered, toScan.channels() == 2 || toScan.channels() == 5);
+
+						// Compute the transformation to express data in ref
+						PM::TransformationParameters T;
+						try
+						{
+							UASSERT(_libpointmatcherICP != 0);
+							PM::ICP & icp = *((PM::ICP*)_libpointmatcherICP);
+							UDEBUG("libpointmatcher icp... (if there is a seg fault here, make sure all third party libraries are built with same Eigen version.)");
+							if(_pointToPlane)
+							{
+								// temporary set PointToPointErrorMinimizer
+								PM::ICP & icpTmp = icp;
+								icpTmp.errorMinimizer.reset(PM::get().ErrorMinimizerRegistrar.create("PointToPointErrorMinimizer"));
+
+								for(PM::OutlierFilters::iterator iter=icpTmp.outlierFilters.begin(); iter!=icpTmp.outlierFilters.end();)
+								{
+									if((*iter)->className.compare("SurfaceNormalOutlierFilter") == 0)
+									{
+										iter = icpTmp.outlierFilters.erase(iter);
+									}
+									else
+									{
+										++iter;
+									}
+								}
+
+								T = icpTmp(data, ref);
+							}
+							else
+							{
+								T = icp(data, ref);
+							}
 							UDEBUG("libpointmatcher icp...done!");
 							icpT = Transform::fromEigen3d(Eigen::Affine3d(Eigen::Matrix4d(eigenMatrixToDim<double>(T.template cast<double>(), 4))));

@@ -638,6 +818,39 @@ Transform RegistrationIcp::computeTransformationImpl(

 					if(!icpT.isNull() && hasConverged)
 					{
+						if(tooLowComplexityForPlaneToPlane)
+						{
+							Transform guessInv = guess.inverse();
+							Transform t = guessInv * icpT.inverse() * guess;
+							Eigen::Vector3f v(t.x(), t.y(), t.z());
+							if(complexityVectors.cols == 2)
+							{
+								// 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);
+								v = n*a;
+							}
+							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);
+								v = n1*a;
+								v += n2*b;
+							}
+							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;
+						}
+
 						util3d::computeVarianceAndCorrespondences(
 								fromCloudRegistered,
 								toCloudFiltered,
--- a/corelib/src/Rtabmap.cpp
+++ b/corelib/src/Rtabmap.cpp
@@ -1116,14 +1116,17 @@ bool Rtabmap::process(
 						else
 						{
 							UINFO("Odometry refining rejected: %s", info.rejectedMsg.c_str());
-							if(info.covariance.at<double>(0,0) > 0.0 && info.covariance.at<double>(5,5) > 0.0)
+							if(!info.covariance.empty() && info.covariance.at<double>(0,0) > 0.0 && info.covariance.at<double>(0,0) != 1.0 && info.covariance.at<double>(5,5) > 0.0 && info.covariance.at<double>(5,5) != 1.0)
 							{
 								_memory->updateLink(Link(oldId, signature->id(), signature->getLinks().begin()->second.type(), guess, (info.covariance*100.0).inv()));
 							}
 						}
 						statistics_.addStatistic(Statistics::kNeighborLinkRefiningAccepted(), !t.isNull()?1.0f:0);
 						statistics_.addStatistic(Statistics::kNeighborLinkRefiningInliers(), info.inliers);
-						statistics_.addStatistic(Statistics::kNeighborLinkRefiningInliers_ratio(), info.icpInliersRatio);
+						statistics_.addStatistic(Statistics::kNeighborLinkRefiningICP_inliers_ratio(), info.icpInliersRatio);
+						statistics_.addStatistic(Statistics::kNeighborLinkRefiningICP_rotation(), info.icpRotation);
+						statistics_.addStatistic(Statistics::kNeighborLinkRefiningICP_translation(), info.icpTranslation);
+						statistics_.addStatistic(Statistics::kNeighborLinkRefiningICP_complexity(), info.icpStructuralComplexity);
 						statistics_.addStatistic(Statistics::kNeighborLinkRefiningPts(), signature->sensorData().laserScanRaw().cols);
 					}
 				}
@@ -1228,9 +1231,7 @@ bool Rtabmap::process(
 		//============================================================
 		if(_proximityByTime &&
 		   rehearsedId == 0 && // don't do it if rehearsal happened
-		   signature->getWords3().size() &&
 		   _memory->isIncremental() && // don't do it in localization mode
-		   !signature->isBadSignature() &&
 		   signature->getWeight()>=0)
 		{
 			const std::set<int> & stm = _memory->getStMem();
@@ -1511,7 +1512,7 @@ bool Rtabmap::process(
 							++immunizedGlobally;
 						}

-						UDEBUG("nt=%d m=%d immunized=1", iter->first, iter->second);
+						//UDEBUG("nt=%d m=%d immunized=1", iter->first, iter->second);
 					}
 					neighbors.erase(iter++);
 				}
@@ -1557,7 +1558,7 @@ bool Rtabmap::process(
 						{
 							++nbDirectNeighborsInDb;
 						}
-						UDEBUG("nt=%d m=%d", iter->first, iter->second);
+						//UDEBUG("nt=%d m=%d", iter->first, iter->second);
 					}
 					neighbors.erase(iter++);
 				}
@@ -1698,7 +1699,7 @@ bool Rtabmap::process(
 							{
 								++immunizedLocally;
 							}
-							UDEBUG("local node %d on path immunized=1", iter->first);
+							//UDEBUG("local node %d on path immunized=1", iter->first);
 						}
 					}
 				}
@@ -1745,7 +1746,7 @@ bool Rtabmap::process(
 					{
 						++immunizedLocally;
 					}
-					UDEBUG("local node %d (%f m) immunized=1", iter->second, iter->first);
+					//UDEBUG("local node %d (%f m) immunized=1", iter->second, iter->first);
 				}
 			}
 		}
@@ -2006,7 +2007,7 @@ bool Rtabmap::process(
 						//find the nearest pose on the path
 						int nearestId = rtabmap::graph::findNearestNode(path, _optimizedPoses.at(signature->id()));
 						UASSERT(nearestId > 0);
-						UDEBUG("Path %d (size=%d) distance=%fm", nearestId, (int)path.size(), _optimizedPoses.at(signature->id()).getDistance(_optimizedPoses.at(nearestId)));
+						//UDEBUG("Path %d (size=%d) distance=%fm", nearestId, (int)path.size(), _optimizedPoses.at(signature->id()).getDistance(_optimizedPoses.at(nearestId)));

 						// nearest pose must be close and not linked to current location
 						if(!signature->hasLink(nearestId) &&
@@ -2101,7 +2102,7 @@ bool Rtabmap::process(
 						}
 						else
 						{
-							UDEBUG("Path %d ignored", nearestId);
+							//UDEBUG("Path %d ignored", nearestId);
 						}
 					}
 				}
@@ -2996,7 +2997,7 @@ std::map<int, std::map<int, Transform> > Rtabmap::getPaths(std::map<int, Transfo

 					if(valid)
 					{
-						UDEBUG("%d <- %d", nearestId, jter->first);
+						//UDEBUG("%d <- %d", nearestId, jter->first);
 						path.insert(*jter);
 						poses.erase(jter);
 					}
--- a/corelib/src/RtabmapThread.cpp
+++ b/corelib/src/RtabmapThread.cpp
@@ -336,7 +336,7 @@ bool RtabmapThread::handleEvent(UEvent* event)
 					if (!e->info().odomPose.isNull() || (_rtabmap->getMemory() && !_rtabmap->getMemory()->isIncremental()))
 					{
 						OdometryInfo infoCov;
-						infoCov.covariance = e->info().odomCovariance;
+						infoCov.reg.covariance = e->info().odomCovariance;
 						this->addData(OdometryEvent(e->data(), e->info().odomPose, infoCov));
 					}
 					else
@@ -347,7 +347,7 @@ bool RtabmapThread::handleEvent(UEvent* event)
 				else
 				{ 
 					OdometryInfo infoCov;
-					infoCov.covariance = e->info().odomCovariance;
+					infoCov.reg.covariance = e->info().odomCovariance;
 					this->addData(OdometryEvent(e->data(), e->info().odomPose, infoCov));
 				}
 				
@@ -570,7 +570,7 @@ void RtabmapThread::addData(const OdometryEvent & odomEvent)
 		}
 		if(!lastPose_.isIdentity() &&
 						(odomEvent.pose().isIdentity() ||
-						odomEvent.info().covariance.at<double>(0,0)>=9999))
+						odomEvent.info().reg.covariance.at<double>(0,0)>=9999))
 		{
 			if(odomEvent.pose().isIdentity())
 			{
@@ -578,20 +578,20 @@ void RtabmapThread::addData(const OdometryEvent & odomEvent)
 			}
 			else
 			{
-				UWARN("Odometry is reset (high variance (%f >=9999 detected). Increment map id!", odomEvent.info().covariance.at<double>(0,0));
+				UWARN("Odometry is reset (high variance (%f >=9999 detected). Increment map id!", odomEvent.info().reg.covariance.at<double>(0,0));
 			}
 			pushNewState(kStateTriggeringMap);
 			covariance_ = cv::Mat();
 		}

-		if(uIsFinite(odomEvent.info().covariance.at<double>(0,0)) &&
-			odomEvent.info().covariance.at<double>(0,0) != 1.0 &&
-			odomEvent.info().covariance.at<double>(0,0)>0.0)
+		if(uIsFinite(odomEvent.info().reg.covariance.at<double>(0,0)) &&
+			odomEvent.info().reg.covariance.at<double>(0,0) != 1.0 &&
+			odomEvent.info().reg.covariance.at<double>(0,0)>0.0)
 		{
 			// Use largest covariance error (to be independent of the odometry frame rate)
-			if(covariance_.empty() || odomEvent.info().covariance.at<double>(0,0) > covariance_.at<double>(0,0))
+			if(covariance_.empty() || odomEvent.info().reg.covariance.at<double>(0,0) > covariance_.at<double>(0,0))
 			{
-				covariance_ = odomEvent.info().covariance;
+				covariance_ = odomEvent.info().reg.covariance;
 			}
 		}

@@ -614,7 +614,7 @@ void RtabmapThread::addData(const OdometryEvent & odomEvent)
 			covariance_ = cv::Mat::eye(6,6,CV_64FC1);
 		}
 		OdometryInfo odomInfo = odomEvent.info().copyWithoutData();
-		odomInfo.covariance = covariance_;
+		odomInfo.reg.covariance = covariance_;
 		if(ignoreFrame)
 		{
 			// set negative id so rtabmap will detect it as an intermediate node
--- a/corelib/src/SensorData.cpp
+++ b/corelib/src/SensorData.cpp
@@ -195,7 +195,7 @@ SensorData::SensorData(
 		_depthOrRightRaw = depth;
 	}

-	if(laserScan.type() == CV_32FC2 || laserScan.type() == CV_32FC3  || laserScan.type() == CV_32FC(4) || laserScan.type() == CV_32FC(6))
+	if(laserScan.type() == CV_32FC2 || laserScan.type() == CV_32FC3  || laserScan.type() == CV_32FC(4) || laserScan.type() == CV_32FC(5) || laserScan.type() == CV_32FC(6) || laserScan.type() == CV_32FC(7))
 	{
 		_laserScanRaw = laserScan;
 	}
@@ -300,7 +300,7 @@ SensorData::SensorData(
 		_depthOrRightRaw = depth;
 	}

-	if(laserScan.type() == CV_32FC2 || laserScan.type() == CV_32FC3 || laserScan.type() == CV_32FC(4) || laserScan.type() == CV_32FC(6))
+	if(laserScan.type() == CV_32FC2 || laserScan.type() == CV_32FC3  || laserScan.type() == CV_32FC(4) || laserScan.type() == CV_32FC(5) || laserScan.type() == CV_32FC(6) || laserScan.type() == CV_32FC(7))
 	{
 		_laserScanRaw = laserScan;
 	}
@@ -406,7 +406,7 @@ SensorData::SensorData(
 		_depthOrRightRaw = right;
 	}

-	if(laserScan.type() == CV_32FC2 || laserScan.type() == CV_32FC3 || laserScan.type() == CV_32FC(4) || laserScan.type() == CV_32FC(6))
+	if(laserScan.type() == CV_32FC2 || laserScan.type() == CV_32FC3  || laserScan.type() == CV_32FC(4) || laserScan.type() == CV_32FC(5) || laserScan.type() == CV_32FC(6) || laserScan.type() == CV_32FC(7))
 	{
 		_laserScanRaw = laserScan;
 	}
@@ -496,7 +496,7 @@ void SensorData::setOccupancyGrid(

 	if(!ground.empty())
 	{
-		if(ground.type() == CV_32FC2 || ground.type() == CV_32FC3 || ground.type() == CV_32FC(4) || ground.type() == CV_32FC(6))
+		if(ground.type() == CV_32FC2 || ground.type() == CV_32FC3 || ground.type() == CV_32FC(4) || ground.type() == CV_32FC(5) || ground.type() == CV_32FC(6) || ground.type() == CV_32FC(7))
 		{
 			_groundCellsRaw = ground;
 			ctGround.start();
@@ -509,7 +509,7 @@ void SensorData::setOccupancyGrid(
 	}
 	if(!obstacles.empty())
 	{
-		if(obstacles.type() == CV_32FC2 || obstacles.type() == CV_32FC3 || obstacles.type() == CV_32FC(4) || obstacles.type() == CV_32FC(6))
+		if(obstacles.type() == CV_32FC2 || obstacles.type() == CV_32FC3 || obstacles.type() == CV_32FC(4) || obstacles.type() == CV_32FC(5) || obstacles.type() == CV_32FC(6) || obstacles.type() == CV_32FC(7))
 		{
 			_obstacleCellsRaw = obstacles;
 			ctObstacles.start();
--- a/corelib/src/util3d.cpp
+++ b/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));
@@ -1419,12 +1459,79 @@ cv::Mat laserScan2dFromPointCloud(const pcl::PointCloud<pcl::PointXYZ> & cloud,
 	return laserScan;
 }

+cv::Mat laserScan2dFromPointCloud(const pcl::PointCloud<pcl::PointNormal> & cloud, const Transform & transform)
+{
+	cv::Mat laserScan(1, (int)cloud.size(), CV_32FC(5));
+	bool nullTransform = transform.isNull();
+	for(unsigned int i=0; i<cloud.size(); ++i)
+	{
+		float * ptr = laserScan.ptr<float>(0, i);
+		if(!nullTransform)
+		{
+			pcl::PointNormal pt = util3d::transformPoint(cloud.at(i), transform);
+			ptr[0] = pt.x;
+			ptr[1] = pt.y;
+			ptr[2] = pt.normal_x;
+			ptr[3] = pt.normal_y;
+			ptr[4] = pt.normal_z;
+		}
+		else
+		{
+			const pcl::PointNormal & pt = cloud.at(i);
+			ptr[0] = pt.x;
+			ptr[1] = pt.y;
+			ptr[2] = pt.normal_x;
+			ptr[3] = pt.normal_y;
+			ptr[4] = pt.normal_z;
+		}
+
+	}
+	return laserScan;
+}
+
+cv::Mat laserScan2dFromPointCloud(const pcl::PointCloud<pcl::PointXYZ> & cloud, const pcl::PointCloud<pcl::Normal> & normals, const Transform & transform)
+{
+	UASSERT(cloud.size() == normals.size());
+	cv::Mat laserScan(1, (int)cloud.size(), CV_32FC(5));
+	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::PointNormal 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.normal_x;
+			ptr[3] = pt.normal_y;
+			ptr[4] = pt.normal_z;
+		}
+		else
+		{
+			ptr[0] = cloud.at(i).x;
+			ptr[1] = cloud.at(i).y;
+			ptr[2] = normals.at(i).normal_x;
+			ptr[3] = normals.at(i).normal_y;
+			ptr[4] = normals.at(i).normal_z;
+		}
+	}
+	return laserScan;
+}
+
 pcl::PointCloud<pcl::PointXYZ>::Ptr laserScanToPointCloud(const cv::Mat & laserScan, const Transform & transform)
 {
-	UASSERT(laserScan.empty() || laserScan.type() == CV_32FC2 || laserScan.type() == CV_32FC3 || laserScan.type() == CV_32FC(4) || laserScan.type() == CV_32FC(6)  || laserScan.type() == CV_32FC(7));
+	UASSERT(laserScan.empty() || laserScan.type() == CV_32FC2 || laserScan.type() == CV_32FC3 || laserScan.type() == CV_32FC(4) || laserScan.type() == CV_32FC(5) || laserScan.type() == CV_32FC(6)  || laserScan.type() == CV_32FC(7));

 	pcl::PointCloud<pcl::PointXYZ>::Ptr output(new pcl::PointCloud<pcl::PointXYZ>);
 	output->resize(laserScan.cols);
+	output->is_dense = true;
 	bool nullTransform = transform.isNull();
 	Eigen::Affine3f transform3f = transform.toEigen3f();
 	for(int i=0; i<laserScan.cols; ++i)
@@ -1440,10 +1547,11 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr laserScanToPointCloud(const cv::Mat & laserS

 pcl::PointCloud<pcl::PointNormal>::Ptr laserScanToPointCloudNormal(const cv::Mat & laserScan, const Transform & transform)
 {
-	UASSERT(laserScan.empty() || laserScan.type() == CV_32FC2 || laserScan.type() == CV_32FC3 || laserScan.type() == CV_32FC(4) || laserScan.type() == CV_32FC(6) || laserScan.type() == CV_32FC(7));
+	UASSERT(laserScan.empty() || laserScan.type() == CV_32FC2 || laserScan.type() == CV_32FC3 || laserScan.type() == CV_32FC(4) || laserScan.type() == CV_32FC(5) || laserScan.type() == CV_32FC(6) || laserScan.type() == CV_32FC(7));

 	pcl::PointCloud<pcl::PointNormal>::Ptr output(new pcl::PointCloud<pcl::PointNormal>);
 	output->resize(laserScan.cols);
+	output->is_dense = true;
 	bool nullTransform = transform.isNull();
 	for(int i=0; i<laserScan.cols; ++i)
 	{
@@ -1458,10 +1566,11 @@ pcl::PointCloud<pcl::PointNormal>::Ptr laserScanToPointCloudNormal(const cv::Mat

 pcl::PointCloud<pcl::PointXYZRGB>::Ptr laserScanToPointCloudRGB(const cv::Mat & laserScan, const Transform & transform,  unsigned char r, unsigned char g, unsigned char b)
 {
-	UASSERT(laserScan.empty() || laserScan.type() == CV_32FC2 || laserScan.type() == CV_32FC3 || laserScan.type() == CV_32FC(4) || laserScan.type() == CV_32FC(6) || laserScan.type() == CV_32FC(7));
+	UASSERT(laserScan.empty() || laserScan.type() == CV_32FC2 || laserScan.type() == CV_32FC3 || laserScan.type() == CV_32FC(4) || laserScan.type() == CV_32FC(5) || laserScan.type() == CV_32FC(6) || laserScan.type() == CV_32FC(7));

 	pcl::PointCloud<pcl::PointXYZRGB>::Ptr output(new pcl::PointCloud<pcl::PointXYZRGB>);
 	output->resize(laserScan.cols);
+	output->is_dense = true;
 	bool nullTransform = transform.isNull() || transform.isIdentity();
 	Eigen::Affine3f transform3f = transform.toEigen3f();
 	for(int i=0; i<laserScan.cols; ++i)
@@ -1477,10 +1586,11 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr laserScanToPointCloudRGB(const cv::Mat &

 pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr laserScanToPointCloudRGBNormal(const cv::Mat & laserScan, const Transform & transform,  unsigned char r, unsigned char g, unsigned char b)
 {
-	UASSERT(laserScan.empty() || laserScan.type() == CV_32FC2 || laserScan.type() == CV_32FC3 || laserScan.type() == CV_32FC(4) || laserScan.type() == CV_32FC(6) || laserScan.type() == CV_32FC(7));
+	UASSERT(laserScan.empty() || laserScan.type() == CV_32FC2 || laserScan.type() == CV_32FC3 || laserScan.type() == CV_32FC(4) || laserScan.type() == CV_32FC(5) || laserScan.type() == CV_32FC(6) || laserScan.type() == CV_32FC(7));

 	pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr output(new pcl::PointCloud<pcl::PointXYZRGBNormal>);
 	output->resize(laserScan.cols);
+	output->is_dense = true;
 	bool nullTransform = transform.isNull() || transform.isIdentity();
 	for(int i=0; i<laserScan.cols; ++i)
 	{
@@ -1496,12 +1606,12 @@ pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr laserScanToPointCloudRGBNormal(cons
 pcl::PointXYZ laserScanToPoint(const cv::Mat & laserScan, int index)
 {
 	UASSERT(!laserScan.empty() && index < laserScan.cols);
-	UASSERT(laserScan.type() == CV_32FC2 || laserScan.type() == CV_32FC3 || laserScan.type() == CV_32FC(4) || laserScan.type() == CV_32FC(6) || laserScan.type() == CV_32FC(7));
+	UASSERT(laserScan.type() == CV_32FC2 || laserScan.type() == CV_32FC3 || laserScan.type() == CV_32FC(4) || laserScan.type() == CV_32FC(5) || laserScan.type() == CV_32FC(6) || laserScan.type() == CV_32FC(7));
 	pcl::PointXYZ output;
 	const float * ptr = laserScan.ptr<float>(0, index);
 	output.x = ptr[0];
 	output.y = ptr[1];
-	if(laserScan.channels() >= 3)
+	if(laserScan.channels() >= 3 && laserScan.channels() != 5)
 	{
 		output.z = ptr[2];
 	}
@@ -1511,16 +1621,22 @@ pcl::PointXYZ laserScanToPoint(const cv::Mat & laserScan, int index)
 pcl::PointNormal laserScanToPointNormal(const cv::Mat & laserScan, int index)
 {
 	UASSERT(!laserScan.empty() && index < laserScan.cols);
-	UASSERT(laserScan.type() == CV_32FC2 || laserScan.type() == CV_32FC3 || laserScan.type() == CV_32FC(4) || laserScan.type() == CV_32FC(6) || laserScan.type() == CV_32FC(7));
+	UASSERT(laserScan.type() == CV_32FC2 || laserScan.type() == CV_32FC3 || laserScan.type() == CV_32FC(4) || laserScan.type() == CV_32FC(5) || laserScan.type() == CV_32FC(6) || laserScan.type() == CV_32FC(7));
 	pcl::PointNormal output;
 	const float * ptr = laserScan.ptr<float>(0, index);
 	output.x = ptr[0];
 	output.y = ptr[1];
-	if(laserScan.channels() >= 3)
+	if(laserScan.channels() >= 3 && laserScan.channels() != 5)
 	{
 		output.z = ptr[2];
 	}
-	if(laserScan.channels() == 6)
+	if(laserScan.channels() == 5)
+	{
+		output.normal_x = ptr[2];
+		output.normal_y = ptr[3];
+		output.normal_z = ptr[4];
+	}
+	else if(laserScan.channels() == 6)
 	{
 		output.normal_x = ptr[3];
 		output.normal_y = ptr[4];
@@ -1538,12 +1654,12 @@ pcl::PointNormal laserScanToPointNormal(const cv::Mat & laserScan, int index)
 pcl::PointXYZRGB laserScanToPointRGB(const cv::Mat & laserScan, int index, unsigned char r, unsigned char g, unsigned char b)
 {
 	UASSERT(!laserScan.empty() && index < laserScan.cols);
-	UASSERT(laserScan.type() == CV_32FC2 || laserScan.type() == CV_32FC3 || laserScan.type() == CV_32FC(4) || laserScan.type() == CV_32FC(6) || laserScan.type() == CV_32FC(7));
+	UASSERT(laserScan.type() == CV_32FC2 || laserScan.type() == CV_32FC3 || laserScan.type() == CV_32FC(4) || laserScan.type() == CV_32FC(5) || laserScan.type() == CV_32FC(6) || laserScan.type() == CV_32FC(7));
 	pcl::PointXYZRGB output;
 	const float * ptr = laserScan.ptr<float>(0, index);
 	output.x = ptr[0];
 	output.y = ptr[1];
-	if(laserScan.channels() >= 3)
+	if(laserScan.channels() >= 3 && laserScan.channels() != 5)
 	{
 		output.z = ptr[2];
 	}
@@ -1566,16 +1682,22 @@ pcl::PointXYZRGB laserScanToPointRGB(const cv::Mat & laserScan, int index, unsig
 pcl::PointXYZRGBNormal laserScanToPointRGBNormal(const cv::Mat & laserScan, int index, unsigned char r, unsigned char g, unsigned char b)
 {
 	UASSERT(!laserScan.empty() && index < laserScan.cols);
-	UASSERT(laserScan.type() == CV_32FC2 || laserScan.type() == CV_32FC3 || laserScan.type() == CV_32FC(4) || laserScan.type() == CV_32FC(6) || laserScan.type() == CV_32FC(7));
+	UASSERT(laserScan.type() == CV_32FC2 || laserScan.type() == CV_32FC3 || laserScan.type() == CV_32FC(4) || laserScan.type() == CV_32FC(5) || laserScan.type() == CV_32FC(6) || laserScan.type() == CV_32FC(7));
 	pcl::PointXYZRGBNormal output;
 	const float * ptr = laserScan.ptr<float>(0, index);
 	output.x = ptr[0];
 	output.y = ptr[1];
-	if(laserScan.channels() >= 3)
+	if(laserScan.channels() >= 3 && laserScan.channels() != 5)
 	{
 		output.z = ptr[2];
 	}
-	if(laserScan.channels() == 6)
+	if(laserScan.channels() == 5)
+	{
+		output.normal_x = ptr[2];
+		output.normal_y = ptr[3];
+		output.normal_z = ptr[4];
+	}
+	else if(laserScan.channels() == 6)
 	{
 		output.normal_x = ptr[3];
 		output.normal_y = ptr[4];
@@ -1606,12 +1728,13 @@ pcl::PointXYZRGBNormal laserScanToPointRGBNormal(const cv::Mat & laserScan, int
 void getMinMax3D(const cv::Mat & laserScan, cv::Point3f & min, cv::Point3f & max)
 {
 	UASSERT(!laserScan.empty());
-	UASSERT(laserScan.type() == CV_32FC2 || laserScan.type() == CV_32FC3 || laserScan.type() == CV_32FC(4) || laserScan.type() == CV_32FC(6) || laserScan.type() == CV_32FC(7));
+	UASSERT(laserScan.type() == CV_32FC2 || laserScan.type() == CV_32FC3 || laserScan.type() == CV_32FC(4) || laserScan.type() == CV_32FC(5) || laserScan.type() == CV_32FC(6) || laserScan.type() == CV_32FC(7));

 	const float * ptr = laserScan.ptr<float>(0, 0);
 	min.x = max.x = ptr[0];
 	min.y = max.y = ptr[1];
-	min.z = max.z = laserScan.channels() >= 3?ptr[2]:0.0f;
+	bool is3d = laserScan.channels() >= 3 && laserScan.channels() != 5;
+	min.z = max.z = is3d?ptr[2]:0.0f;
 	for(int i=1; i<laserScan.cols; ++i)
 	{
 		ptr = laserScan.ptr<float>(0, i);
@@ -1622,7 +1745,7 @@ void getMinMax3D(const cv::Mat & laserScan, cv::Point3f & min, cv::Point3f & max
 		if(ptr[1] < min.y) min.y = ptr[1];
 		else if(ptr[1] > max.y) max.y = ptr[1];

-		if(laserScan.channels() >= 3)
+		if(is3d)
 		{
 			if(ptr[2] < min.z) min.z = ptr[2];
 			else if(ptr[2] > max.z) max.z = ptr[2];
@@ -1689,7 +1812,7 @@ cv::Mat projectCloudToCamera(
 {
 	UASSERT(!cameraTransform.isNull());
 	UASSERT(!laserScan.empty());
-	UASSERT(laserScan.type() == CV_32FC2 || laserScan.type() == CV_32FC3 || laserScan.type() == CV_32FC(6) || laserScan.type() == CV_32FC(7));
+	UASSERT(laserScan.type() == CV_32FC2 || laserScan.type() == CV_32FC3 || laserScan.type() == CV_32FC(4) || laserScan.type() == CV_32FC(5) || laserScan.type() == CV_32FC(6) || laserScan.type() == CV_32FC(7));
 	UASSERT(cameraMatrixK.type() == CV_64FC1 && cameraMatrixK.cols == 3 && cameraMatrixK.cols == 3);

 	float fx = cameraMatrixK.at<double>(0,0);
@@ -1700,46 +1823,25 @@ cv::Mat projectCloudToCamera(
 	cv::Mat registered = cv::Mat::zeros(imageSize, CV_32FC1);
 	Transform t = cameraTransform.inverse();

-	const cv::Vec2f* vec2Ptr = laserScan.ptr<cv::Vec2f>();
-	const cv::Vec3f* vec3Ptr = laserScan.ptr<cv::Vec3f>();
-	const cv::Vec4f* vec4Ptr = laserScan.ptr<cv::Vec4f>();
-	const cv::Vec6f* vec6Ptr = laserScan.ptr<cv::Vec6f>();
-	const float* vec7Ptr = laserScan.ptr<float>();
-
 	int count = 0;
 	for(int i=0; i<laserScan.cols; ++i)
 	{
+		const float* ptr = laserScan.ptr<float>(0, i);
+
 		// Get 3D from laser scan
 		cv::Point3f ptScan;
-		if(laserScan.type() == CV_32FC2)
+		if(laserScan.type() == CV_32FC2 || laserScan.type() == CV_32FC(5))
 		{
-			ptScan.x = vec2Ptr[i][0];
-			ptScan.y = vec2Ptr[i][1];
+			// 2D scans
+			ptScan.x = ptr[0];
+			ptScan.y = ptr[1];
 			ptScan.z = 0;
 		}
-		else if(laserScan.type() == CV_32FC3)
+		else // 3D scans
 		{
-			ptScan.x = vec3Ptr[i][0];
-			ptScan.y = vec3Ptr[i][1];
-			ptScan.z = vec3Ptr[i][2];
-		}
-		else if(laserScan.type() == CV_32FC(4))
-		{
-			ptScan.x = vec4Ptr[i][0];
-			ptScan.y = vec4Ptr[i][1];
-			ptScan.z = vec4Ptr[i][2];
-		}
-		else if(laserScan.type() == CV_32FC(6))
-		{
-			ptScan.x = vec6Ptr[i][0];
-			ptScan.y = vec6Ptr[i][1];
-			ptScan.z = vec6Ptr[i][2];
-		}
-		else // 7f
-		{
-			ptScan.x = (vec7Ptr+i*7)[0];
-			ptScan.y = (vec7Ptr+i*7)[1];
-			ptScan.z = (vec7Ptr+i*7)[2];
+			ptScan.x = ptr[0];
+			ptScan.y = ptr[1];
+			ptScan.z = ptr[2];
 		}
 		ptScan = util3d::transformPoint(ptScan, t);

--- a/corelib/src/util3d_filtering.cpp
+++ b/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,
--- a/corelib/src/util3d_registration.cpp
+++ b/corelib/src/util3d_registration.cpp
@@ -244,8 +244,8 @@ void computeVarianceAndCorrespondences(
 	correspondencesOut = 0;
 	pcl::registration::CorrespondenceEstimation<pcl::PointNormal, pcl::PointNormal>::Ptr est;
 	est.reset(new pcl::registration::CorrespondenceEstimation<pcl::PointNormal, pcl::PointNormal>);
-	est->setInputTarget(cloudB);
-	est->setInputSource(cloudA);
+	est->setInputTarget(cloudA->size()>cloudB->size()?cloudA:cloudB);
+	est->setInputSource(cloudA->size()>cloudB->size()?cloudB:cloudA);
 	pcl::Correspondences correspondences;
 	est->determineCorrespondences(correspondences, maxCorrespondenceDistance);

@@ -277,8 +277,8 @@ void computeVarianceAndCorrespondences(
 	correspondencesOut = 0;
 	pcl::registration::CorrespondenceEstimation<pcl::PointXYZ, pcl::PointXYZ>::Ptr est;
 	est.reset(new pcl::registration::CorrespondenceEstimation<pcl::PointXYZ, pcl::PointXYZ>);
-	est->setInputTarget(cloudB);
-	est->setInputSource(cloudA);
+	est->setInputTarget(cloudA->size()>cloudB->size()?cloudA:cloudB);
+	est->setInputSource(cloudA->size()>cloudB->size()?cloudB:cloudA);
 	pcl::Correspondences correspondences;
 	est->determineCorrespondences(correspondences, maxCorrespondenceDistance);

--- a/corelib/src/util3d_surface.cpp
+++ b/corelib/src/util3d_surface.cpp
@@ -1994,18 +1994,52 @@ 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);
+			return util3d::laserScan2dFromPointCloud(*cloud, *normals);
+		}
+		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 +2066,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 +2076,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 +2114,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,
@@ -2132,6 +2338,204 @@ pcl::PointCloud<pcl::Normal>::Ptr computeFastOrganizedNormals(
 	return normals;
 }

+float computeNormalsComplexity(
+		const cv::Mat & scan,
+		cv::Mat * pcaEigenVectors,
+		cv::Mat * pcaEigenValues)
+{
+	if(!scan.empty() && (scan.channels() == 5 || scan.channels() == 6 || scan.channels() == 7))
+	{
+		 //Construct a buffer used by the pca analysis
+		int sz = static_cast<int>(scan.cols*2);
+		bool is2d = scan.channels() == 5;
+		cv::Mat data_normals = cv::Mat::zeros(sz, is2d?2:3, CV_32FC1);
+		int oi = 0;
+		for (int i = 0; i < scan.cols; ++i)
+		{
+			const float * ptrScan = scan.ptr<float>(0, i);
+			if(scan.channels() == 5)
+			{
+				if(uIsFinite(ptrScan[2]) && uIsFinite(ptrScan[3]))
+				{
+					float * ptr = data_normals.ptr<float>(oi++, 0);
+					ptr[0] = ptrScan[2];
+					ptr[1] = ptrScan[3];
+				}
+			}
+			else if(scan.channels() == 6)
+			{
+				if(uIsFinite(ptrScan[3]) && uIsFinite(ptrScan[4]) && uIsFinite(ptrScan[5]))
+				{
+					float * ptr = data_normals.ptr<float>(oi++, 0);
+					ptr[0] = ptrScan[3];
+					ptr[1] = ptrScan[4];
+					ptr[2] = ptrScan[5];
+				}
+			}
+			else
+			{
+				if(uIsFinite(ptrScan[4]) && uIsFinite(ptrScan[5]) && uIsFinite(ptrScan[6]))
+				{
+					float * ptr = data_normals.ptr<float>(oi++, 0);
+					ptr[0] = ptrScan[4];
+					ptr[1] = ptrScan[5];
+					ptr[2] = ptrScan[6];
+				}
+			}
+		}
+		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);
+		}
+	}
+	else if(!scan.empty())
+	{
+		UERROR("Scan doesn't have normals!");
+	}
+	return 0.0f;
+}
+
+float computeNormalsComplexity(
+		const pcl::PointCloud<pcl::PointNormal> & cloud,
+		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;
+	for (unsigned int i = 0; i < cloud.size(); ++i)
+	{
+		const pcl::PointNormal & pt = cloud.at(i);
+		if(uIsFinite(pt.normal_x) && uIsFinite(pt.normal_y) && uIsFinite(pt.normal_z))
+		{
+			float * ptr = data_normals.ptr<float>(oi++, 0);
+			ptr[0] = pt.normal_x;
+			ptr[1] = pt.normal_y;
+			if(!is2d)
+			{
+				ptr[2] = pt.normal_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::Normal> & normals,
+		bool is2d,
+		cv::Mat * pcaEigenVectors,
+		cv::Mat * pcaEigenValues)
+{
+	 //Construct a buffer used by the pca analysis
+	int sz = static_cast<int>(normals.size()*2);
+	cv::Mat data_normals = cv::Mat::zeros(sz, is2d?2:3, CV_32FC1);
+	int oi = 0;
+	for (unsigned int i = 0; i < normals.size(); ++i)
+	{
+		const pcl::Normal & pt = normals.at(i);
+		if(uIsFinite(pt.normal_x) && uIsFinite(pt.normal_y) && uIsFinite(pt.normal_z))
+		{
+			float * ptr = data_normals.ptr<float>(oi++, 0);
+			ptr[0] = pt.normal_x;
+			ptr[1] = pt.normal_y;
+			if(!is2d)
+			{
+				ptr[2] = pt.normal_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,
+		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;
+	for (unsigned int i = 0; i < cloud.size(); ++i)
+	{
+		const pcl::PointXYZRGBNormal & pt = cloud.at(i);
+		if(uIsFinite(pt.normal_x) && uIsFinite(pt.normal_y) && uIsFinite(pt.normal_z))
+		{
+			float * ptr = data_normals.ptr<float>(oi++, 0);
+			ptr[0] = pt.normal_x;
+			ptr[1] = pt.normal_y;
+			if(!is2d)
+			{
+				ptr[2] = pt.normal_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;
+}
+
 pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr mls(
 		const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
 		float searchRadius,
--- a/corelib/src/util3d_transforms.cpp
+++ b/corelib/src/util3d_transforms.cpp
@@ -38,61 +38,82 @@ namespace util3d

 cv::Mat transformLaserScan(const cv::Mat & laserScan, const Transform & transform)
 {
-	UASSERT(laserScan.empty() || laserScan.type() == CV_32FC2 || laserScan.type() == CV_32FC3 || laserScan.type() == CV_32FC(4) || laserScan.type() == CV_32FC(6));
+	UASSERT(laserScan.empty() || laserScan.type() == CV_32FC2 || laserScan.type() == CV_32FC3 || laserScan.type() == CV_32FC(4) || laserScan.type() == CV_32FC(5) || laserScan.type() == CV_32FC(6) || laserScan.type() == CV_32FC(7));

 	cv::Mat output = laserScan.clone();

 	if(!transform.isNull() && !transform.isIdentity())
 	{
+		Eigen::Affine3f transform3f = transform.toEigen3f();
 		for(int i=0; i<laserScan.cols; ++i)
 		{
+			const float * ptr = laserScan.ptr<float>(0, i);
+			float * out = output.ptr<float>(0, i);
 			if(laserScan.type() == CV_32FC2)
 			{
-				pcl::PointXYZ pt(
-						laserScan.at<cv::Vec2f>(i)[0],
-						laserScan.at<cv::Vec2f>(i)[1], 0);
-				pt = util3d::transformPoint(pt, transform);
-				output.at<cv::Vec2f>(i)[0] = pt.x;
-				output.at<cv::Vec2f>(i)[1] = pt.y;
+				pcl::PointXYZ pt(ptr[0], ptr[1], 0);
+				pt = pcl::transformPoint(pt, transform3f);
+				out[0] = pt.x;
+				out[1] = pt.y;
 			}
-			else if(laserScan.type() == CV_32FC3)
+			else if(laserScan.type() == CV_32FC3 || laserScan.type() == CV_32FC(4))
 			{
-				pcl::PointXYZ pt(
-						laserScan.at<cv::Vec3f>(i)[0],
-						laserScan.at<cv::Vec3f>(i)[1],
-						laserScan.at<cv::Vec3f>(i)[2]);
-				pt = util3d::transformPoint(pt, transform);
-				output.at<cv::Vec3f>(i)[0] = pt.x;
-				output.at<cv::Vec3f>(i)[1] = pt.y;
-				output.at<cv::Vec3f>(i)[2] = pt.z;
+				const float * ptr = laserScan.ptr<float>(0, i);
+				pcl::PointXYZ pt(ptr[0], ptr[1], ptr[2]);
+				pt = pcl::transformPoint(pt, transform3f);
+				out[0] = pt.x;
+				out[1] = pt.y;
+				out[2] = pt.z;
 			}
-			else if(laserScan.type() == CV_32FC(4))
-			{
-				pcl::PointXYZ pt(
-						laserScan.at<cv::Vec4f>(i)[0],
-						laserScan.at<cv::Vec4f>(i)[1],
-						laserScan.at<cv::Vec4f>(i)[2]);
-				pt = util3d::transformPoint(pt, transform);
-				output.at<cv::Vec4f>(i)[0] = pt.x;
-				output.at<cv::Vec4f>(i)[1] = pt.y;
-				output.at<cv::Vec4f>(i)[2] = pt.z;
-			}
-			else
+			else if(laserScan.type() == CV_32FC(5))
 			{
 				pcl::PointNormal pt;
-				pt.x=laserScan.at<cv::Vec6f>(i)[0];
-				pt.y=laserScan.at<cv::Vec6f>(i)[1];
-				pt.z=laserScan.at<cv::Vec6f>(i)[2];
-				pt.normal_x=laserScan.at<cv::Vec6f>(i)[3];
-				pt.normal_y=laserScan.at<cv::Vec6f>(i)[4];
-				pt.normal_z=laserScan.at<cv::Vec6f>(i)[5];
+				pt.x=ptr[0];
+				pt.y=ptr[1];
+				pt.z=0;
+				pt.normal_x=ptr[2];
+				pt.normal_y=ptr[3];
+				pt.normal_z=ptr[4];
 				pt = util3d::transformPoint(pt, transform);
-				output.at<cv::Vec6f>(i)[0] = pt.x;
-				output.at<cv::Vec6f>(i)[1] = pt.y;
-				output.at<cv::Vec6f>(i)[2] = pt.z;
-				output.at<cv::Vec6f>(i)[3] = pt.normal_x;
-				output.at<cv::Vec6f>(i)[4] = pt.normal_y;
-				output.at<cv::Vec6f>(i)[5] = pt.normal_z;
+				out[0] = pt.x;
+				out[1] = pt.y;
+				out[2] = pt.normal_x;
+				out[3] = pt.normal_y;
+				out[4] = pt.normal_z;
+			}
+			else if(laserScan.type() == CV_32FC(6))
+			{
+				pcl::PointNormal pt;
+				pt.x=ptr[0];
+				pt.y=ptr[1];
+				pt.z=ptr[2];
+				pt.normal_x=ptr[3];
+				pt.normal_y=ptr[4];
+				pt.normal_z=ptr[5];
+				pt = util3d::transformPoint(pt, transform);
+				out[0] = pt.x;
+				out[1] = pt.y;
+				out[2] = pt.z;
+				out[3] = pt.normal_x;
+				out[4] = pt.normal_y;
+				out[5] = pt.normal_z;
+			}
+			else // 7 channels
+			{
+				pcl::PointNormal pt;
+				pt.x=ptr[0];
+				pt.y=ptr[1];
+				pt.z=ptr[2];
+				pt.normal_x=ptr[4];
+				pt.normal_y=ptr[5];
+				pt.normal_z=ptr[6];
+				pt = util3d::transformPoint(pt, transform);
+				out[0] = pt.x;
+				out[1] = pt.y;
+				out[2] = pt.z;
+				out[4] = pt.normal_x;
+				out[5] = pt.normal_y;
+				out[6] = pt.normal_z;
 			}
 		}
 	}
@@ -189,7 +210,9 @@ pcl::PointXYZRGB transformPoint(
 		const pcl::PointXYZRGB & pt,
 		const Transform & transform)
 {
-	return pcl::transformPoint(pt, transform.toEigen3f());
+	pcl::PointXYZRGB ptRGB = pcl::transformPoint(pt, transform.toEigen3f());
+	ptRGB.rgb = pt.rgb;
+	return ptRGB;
 }
 pcl::PointNormal transformPoint(
 		const pcl::PointNormal & point,
@@ -223,6 +246,8 @@ pcl::PointXYZRGBNormal transformPoint(
 	ret.normal_x = static_cast<float> (transform (0, 0) * nt.coeffRef (0) + transform (0, 1) * nt.coeffRef (1) + transform (0, 2) * nt.coeffRef (2));
 	ret.normal_y = static_cast<float> (transform (1, 0) * nt.coeffRef (0) + transform (1, 1) * nt.coeffRef (1) + transform (1, 2) * nt.coeffRef (2));
 	ret.normal_z = static_cast<float> (transform (2, 0) * nt.coeffRef (0) + transform (2, 1) * nt.coeffRef (1) + transform (2, 2) * nt.coeffRef (2));
+
+	ret.rgb = point.rgb;
 	return ret;
 }