0.13.3: scan2d with normals support/registration

2026-09-01 17:10:26 +08:00 · 2017-08-25 18:02:40 -04:00
parent 964a052be1
commit 52a4e8964f
28 changed files with 1029 additions and 337 deletions
--- 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),
@@ -323,10 +324,10 @@ void CameraThread::postUpdate(SensorData * dataPtr, CameraInfo * info) const

 				if(cloud->size())
 				{
-					if(_scanNormalsK>0)
+					if(_scanNormalsK>0 || _scanNormalsRadius>0.0f)
 					{
 						Eigen::Vector3f viewPoint(baseToScan.x(), baseToScan.y(), baseToScan.z());
-						pcl::PointCloud<pcl::Normal>::Ptr normals = util3d::computeNormals(cloud, _scanNormalsK, viewPoint);
+						pcl::PointCloud<pcl::Normal>::Ptr normals = util3d::computeNormals(cloud, _scanNormalsK, _scanNormalsRadius, viewPoint);
 						pcl::PointCloud<pcl::PointNormal>::Ptr cloudNormals(new pcl::PointCloud<pcl::PointNormal>);
 						pcl::concatenateFields(*cloud, *normals, *cloudNormals);
 						scan = util3d::laserScanFromPointCloud(*cloudNormals, baseToScan.inverse());
--- a/corelib/src/Memory.cpp
+++ b/corelib/src/Memory.cpp
@@ -89,6 +89,7 @@ Memory::Memory(const ParametersMap & parameters) :
 	_compressionParallelized(Parameters::defaultMemCompressionParallelized()),
 	_laserScanDownsampleStepSize(Parameters::defaultMemLaserScanDownsampleStepSize()),
 	_laserScanNormalK(Parameters::defaultMemLaserScanNormalK()),
+	_laserScanNormalRadius(Parameters::defaultMemLaserScanNormalRadius()),
 	_reextractLoopClosureFeatures(Parameters::defaultRGBDLoopClosureReextractFeatures()),
 	_rehearsalMaxDistance(Parameters::defaultRGBDLinearUpdate()),
 	_rehearsalMaxAngle(Parameters::defaultRGBDAngularUpdate()),
@@ -440,6 +441,7 @@ void Memory::parseParameters(const ParametersMap & parameters)
 	Parameters::parse(parameters, Parameters::kMemCompressionParallelized(), _compressionParallelized);
 	Parameters::parse(parameters, Parameters::kMemLaserScanDownsampleStepSize(), _laserScanDownsampleStepSize);
 	Parameters::parse(parameters, Parameters::kMemLaserScanNormalK(), _laserScanNormalK);
+	Parameters::parse(parameters, Parameters::kMemLaserScanNormalRadius(), _laserScanNormalRadius);
 	Parameters::parse(parameters, Parameters::kRGBDLoopClosureReextractFeatures(), _reextractLoopClosureFeatures);
 	Parameters::parse(parameters, Parameters::kRGBDLinearUpdate(), _rehearsalMaxDistance);
 	Parameters::parse(parameters, Parameters::kRGBDAngularUpdate(), _rehearsalMaxAngle);
@@ -3718,13 +3720,12 @@ Signature * Memory::createSignature(const SensorData & data, const Transform & p
 		if(stats) stats->addStatistic(Statistics::kTimingMemScan_downsampling(), t*1000.0f);
 		UDEBUG("time downsampling scan = %fs", t);
 	}
-	if(!laserScan.empty() && _laserScanNormalK > 0 && laserScan.channels() == 3 && !isIntermediateNode)
+	if(!laserScan.empty() &&
+		(_laserScanNormalK > 0 || _laserScanNormalRadius>0.0f) &&
+		laserScan.channels() > 1 && laserScan.channels() < 5 &&
+		!isIntermediateNode)
 	{
-		pcl::PointCloud<pcl::PointXYZ>::Ptr cloud = util3d::laserScanToPointCloud(laserScan);
-		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);
+		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/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,8 +36,6 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <rtabmap/utilite/UConversion.h>
 #include <rtabmap/utilite/UMath.h>
 #include <rtabmap/utilite/UTimer.h>
-#include <pcl/io/pcd_io.h>
-#include <pcl/io/vtk_io.h>
 #include <pcl/conversions.h>

 #ifdef RTABMAP_POINTMATCHER
@@ -225,7 +223,8 @@ RegistrationIcp::RegistrationIcp(const ParametersMap & parameters, Registration
 	_epsilon(Parameters::defaultIcpEpsilon()),
 	_correspondenceRatio(Parameters::defaultIcpCorrespondenceRatio()),
 	_pointToPlane(Parameters::defaultIcpPointToPlane()),
-	_pointToPlaneNormalNeighbors(Parameters::defaultIcpPointToPlaneNormalNeighbors()),
+	_pointToPlaneK(Parameters::defaultIcpPointToPlaneK()),
+	_pointToPlaneRadius(Parameters::defaultIcpPointToPlaneRadius()),
 	_libpointmatcher(Parameters::defaultIcpPM()),
 	_libpointmatcherConfig(Parameters::defaultIcpPMConfig()),
 	_libpointmatcherOutlierRatio(Parameters::defaultIcpPMOutlierRatio()),
@@ -257,7 +256,8 @@ void RegistrationIcp::parseParameters(const ParametersMap & parameters)
 	Parameters::parse(parameters, Parameters::kIcpEpsilon(), _epsilon);
 	Parameters::parse(parameters, Parameters::kIcpCorrespondenceRatio(), _correspondenceRatio);
 	Parameters::parse(parameters, Parameters::kIcpPointToPlane(), _pointToPlane);
-	Parameters::parse(parameters, Parameters::kIcpPointToPlaneNormalNeighbors(), _pointToPlaneNormalNeighbors);
+	Parameters::parse(parameters, Parameters::kIcpPointToPlaneK(), _pointToPlaneK);
+	Parameters::parse(parameters, Parameters::kIcpPointToPlaneRadius(), _pointToPlaneRadius);

 	Parameters::parse(parameters, Parameters::kIcpPM(), _libpointmatcher);
 	Parameters::parse(parameters, Parameters::kIcpPMConfig(), _libpointmatcherConfig);
@@ -342,7 +342,7 @@ void RegistrationIcp::parseParameters(const ParametersMap & parameters)
 	UASSERT_MSG(_maxIterations > 0, uFormat("value=%d", _maxIterations).c_str());
 	UASSERT(_epsilon >= 0.0f);
 	UASSERT_MSG(_correspondenceRatio >=0.0f && _correspondenceRatio <=1.0f, uFormat("value=%f", _correspondenceRatio).c_str());
-	UASSERT_MSG(_pointToPlaneNormalNeighbors > 0, uFormat("value=%d", _pointToPlaneNormalNeighbors).c_str());
+	UASSERT_MSG(!_pointToPlane || (_pointToPlane && (_pointToPlaneK > 0 || _pointToPlaneRadius > 0.0f)), uFormat("_pointToPlaneK=%d _pointToPlaneRadius=%f", _pointToPlaneK, _pointToPlaneRadius).c_str());
 }

 Transform RegistrationIcp::computeTransformationImpl(
@@ -354,7 +354,8 @@ Transform RegistrationIcp::computeTransformationImpl(
 	UDEBUG("Guess transform = %s", guess.prettyPrint().c_str());
 	UDEBUG("Voxel size=%f", _voxelSize);
 	UDEBUG("PointToPlane=%d", _pointToPlane?1:0);
-	UDEBUG("Normal neighborhood=%d", _pointToPlaneNormalNeighbors);
+	UDEBUG("Normal neighborhood=%d", _pointToPlaneK);
+	UDEBUG("Normal radius=%d", _pointToPlaneRadius);
 	UDEBUG("Max correspondence distance=%f", _maxCorrespondenceDistance);
 	UDEBUG("Max Iterations=%d", _maxIterations);
 	UDEBUG("Correspondence Ratio=%f", _correspondenceRatio);
@@ -406,8 +407,8 @@ Transform RegistrationIcp::computeTransformationImpl(

 			if( _pointToPlane &&
 				_voxelSize == 0.0f &&
-				fromScan.channels() == 6 &&
-				toScan.channels() == 6)
+				fromScan.channels() >= 6 &&
+				toScan.channels() >= 6)
 			{
 				//special case if we have already normals computed and there is no filtering
 				pcl::PointCloud<pcl::PointNormal>::Ptr fromCloudNormals = util3d::laserScanToPointCloudNormal(fromScan, fromLocalTransform);
@@ -450,6 +451,9 @@ Transform RegistrationIcp::computeTransformationImpl(
 				else
 #endif
 				{
+					fromCloudNormals = util3d::removeNaNNormalsFromPointCloud(fromCloudNormals);
+					toCloudNormals = util3d::removeNaNNormalsFromPointCloud(toCloudNormals);
+
 					icpT = util3d::icpPointToPlane(
 							fromCloudNormals,
 							toCloudNormals,
@@ -497,15 +501,41 @@ Transform RegistrationIcp::computeTransformationImpl(
 				}

 				pcl::PointCloud<pcl::PointXYZ>::Ptr fromCloudRegistered(new pcl::PointCloud<pcl::PointXYZ>());
-				if(_pointToPlane) // ICP Point To Plane, only in 3D
+				if(_pointToPlane && // ICP Point To Plane
+					!((fromScan.channels() == 2 || toScan.channels() == 2) && !_libpointmatcher)) // PCL crashes if 2D
 				{
 					pcl::PointCloud<pcl::Normal>::Ptr normals;
+					Eigen::Vector3f viewpointFrom(fromLocalTransform.x(), fromLocalTransform.y(), fromLocalTransform.z());
+					Transform toT = guess * toLocalTransform;
+					Eigen::Vector3f viewpointTo(toT.x(), toT.y(), toT.z());

-					normals = util3d::computeNormals(fromCloudFiltered, _pointToPlaneNormalNeighbors);
+					if(fromScan.channels() == 2)
+					{
+						normals = util3d::computeFastOrganizedNormals2D(
+								fromCloudFiltered,
+								_pointToPlaneK,
+								_pointToPlaneRadius,
+								viewpointFrom);
+					}
+					else
+					{
+						normals = util3d::computeNormals(fromCloudFiltered, _pointToPlaneK, _pointToPlaneRadius, viewpointFrom);
+					}
 					pcl::PointCloud<pcl::PointNormal>::Ptr fromCloudNormals(new pcl::PointCloud<pcl::PointNormal>);
 					pcl::concatenateFields(*fromCloudFiltered, *normals, *fromCloudNormals);

-					normals = util3d::computeNormals(toCloudFiltered, _pointToPlaneNormalNeighbors);
+					if(toScan.channels() == 2)
+					{
+						normals = util3d::computeFastOrganizedNormals2D(
+								toCloudFiltered,
+								_pointToPlaneK,
+								_pointToPlaneRadius,
+								viewpointTo);
+					}
+					else
+					{
+						normals = util3d::computeNormals(toCloudFiltered, _pointToPlaneK, _pointToPlaneRadius, viewpointTo);
+					}
 					pcl::PointCloud<pcl::PointNormal>::Ptr toCloudNormals(new pcl::PointCloud<pcl::PointNormal>);
 					pcl::concatenateFields(*toCloudFiltered, *normals, *toCloudNormals);

@@ -517,7 +547,7 @@ Transform RegistrationIcp::computeTransformationImpl(
 					fromSignature.sensorData().setLaserScanRaw(util3d::laserScanFromPointCloud(*fromCloudNormals, fromLocalTransform.inverse()), LaserScanInfo(maxLaserScansFrom, fromSignature.sensorData().laserScanInfo().maxRange(), fromLocalTransform));
 					toSignature.sensorData().setLaserScanRaw(util3d::laserScanFromPointCloud(*toCloudNormals, (guess*toLocalTransform).inverse()), LaserScanInfo(maxLaserScansTo, toSignature.sensorData().laserScanInfo().maxRange(), toLocalTransform));

-					UDEBUG("Compute normals time = %f s", timer.ticks());
+					UDEBUG("Compute normals (%d,%d) time = %f s", (int)fromCloudNormals->size(), (int)toCloudNormals->size(), timer.ticks());

 					if(toCloudNormals->size() && fromCloudNormals->size())
 					{
@@ -569,7 +599,6 @@ Transform RegistrationIcp::computeTransformationImpl(
 								   this->force3DoF());
 						}

-
 						if(!icpT.isNull() && hasConverged)
 						{
 							util3d::computeVarianceAndCorrespondences(
@@ -583,6 +612,11 @@ Transform RegistrationIcp::computeTransformationImpl(
 				}
 				else // ICP Point to Point
 				{
+					if(_pointToPlane && ((fromScan.channels() == 2 || toScan.channels() == 2) && !_libpointmatcher))
+					{
+						UWARN("ICP PointToPlane ignored for 2d scans with PCL registration (some crash issues). Use libpointmatcher (%s) or disable %s to avoid this warning.", Parameters::kIcpPM().c_str(), Parameters::kIcpPointToPlane().c_str());
+					}
+
 					if(_voxelSize > 0.0f)
 					{
 						// update output scans
--- 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));
--- 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_surface.cpp
+++ b/corelib/src/util3d_surface.cpp
@@ -1994,18 +1994,51 @@ cv::Mat mergeTextures(
 	return globalTextures;
 }

+cv::Mat computeNormals(
+		const cv::Mat & laserScan,
+		int searchK,
+		float searchRadius)
+{
+	if(laserScan.empty() || laserScan.channels()<2 || laserScan.channels()>4)
+	{
+		return laserScan;
+	}
+
+	pcl::PointCloud<pcl::Normal>::Ptr normals;
+	if(laserScan.channels() < 4)
+	{
+		pcl::PointCloud<pcl::PointXYZ>::Ptr cloud = util3d::laserScanToPointCloud(laserScan);
+		if(laserScan.channels() == 2)
+		{
+			normals = util3d::computeNormals2D(cloud, searchK, searchRadius);
+		}
+		else
+		{
+			normals = util3d::computeNormals(cloud, searchK, searchRadius);
+		}
+		return util3d::laserScanFromPointCloud(*cloud, *normals);
+	}
+	else // 4 channels
+	{
+		pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud = util3d::laserScanToPointCloudRGB(laserScan);
+		normals = util3d::computeNormals(cloud, searchK, searchRadius);
+		return util3d::laserScanFromPointCloud(*cloud, *normals);
+	}
+}
 pcl::PointCloud<pcl::Normal>::Ptr computeNormals(
 		const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
-		int normalKSearch,
+		int searchK,
+		float searchRadius,
 		const Eigen::Vector3f & viewPoint)
 {
 	pcl::IndicesPtr indices(new std::vector<int>);
-	return computeNormals(cloud, indices, normalKSearch, viewPoint);
+	return computeNormals(cloud, indices, searchK, searchRadius, viewPoint);
 }
 pcl::PointCloud<pcl::Normal>::Ptr computeNormals(
 		const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
 		const pcl::IndicesPtr & indices,
-		int normalKSearch,
+		int searchK,
+		float searchRadius,
 		const Eigen::Vector3f & viewPoint)
 {
 	pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ>);
@@ -2032,7 +2065,8 @@ pcl::PointCloud<pcl::Normal>::Ptr computeNormals(
 	//	n.setIndices(indices);
 	//}
 	n.setSearchMethod (tree);
-	n.setKSearch (normalKSearch);
+	n.setKSearch (searchK);
+	n.setRadiusSearch (searchRadius);
 	n.setViewPoint(viewPoint[0], viewPoint[1], viewPoint[2]);
 	n.compute (*normals);

@@ -2041,16 +2075,18 @@ pcl::PointCloud<pcl::Normal>::Ptr computeNormals(

 pcl::PointCloud<pcl::Normal>::Ptr computeNormals(
 		const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
-		int normalKSearch,
+		int searchK,
+		float searchRadius,
 		const Eigen::Vector3f & viewPoint)
 {
 	pcl::IndicesPtr indices(new std::vector<int>);
-	return computeNormals(cloud, indices, normalKSearch, viewPoint);
+	return computeNormals(cloud, indices, searchK, searchRadius, viewPoint);
 }
 pcl::PointCloud<pcl::Normal>::Ptr computeNormals(
 		const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
 		const pcl::IndicesPtr & indices,
-		int normalKSearch,
+		int searchK,
+		float searchRadius,
 		const Eigen::Vector3f & viewPoint)
 {
 	pcl::search::KdTree<pcl::PointXYZRGB>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZRGB>);
@@ -2077,13 +2113,182 @@ pcl::PointCloud<pcl::Normal>::Ptr computeNormals(
 	//	n.setIndices(indices);
 	//}
 	n.setSearchMethod (tree);
-	n.setKSearch (normalKSearch);
+	n.setKSearch (searchK);
+	n.setRadiusSearch(searchRadius);
 	n.setViewPoint(viewPoint[0], viewPoint[1], viewPoint[2]);
 	n.compute (*normals);

 	return normals;
 }

+pcl::PointCloud<pcl::Normal>::Ptr computeNormals2D(
+		const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
+		int searchK,
+		float searchRadius,
+		const Eigen::Vector3f & viewPoint)
+{
+	UASSERT(searchK>0 || searchRadius>0.0f);
+	pcl::PointCloud<pcl::Normal>::Ptr normals (new pcl::PointCloud<pcl::Normal>);
+
+	pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ>);
+	tree->setInputCloud (cloud);
+
+	normals->resize(cloud->size());
+
+	float bad_point = std::numeric_limits<float>::quiet_NaN ();
+
+	// assuming that points are ordered
+	for(unsigned int i=0; i<cloud->size(); ++i)
+	{
+		const pcl::PointXYZ & pt = cloud->at(i);
+		std::vector<Eigen::Vector3f> neighborNormals;
+		Eigen::Vector3f direction;
+		direction[0] = viewPoint[0] - pt.x;
+		direction[1] = viewPoint[1] - pt.y;
+		direction[2] = viewPoint[2] - pt.z;
+
+		std::vector<int> k_indices;
+		std::vector<float> k_sqr_distances;
+		if(searchRadius>0.0f)
+		{
+			tree->radiusSearch(cloud->at(i), searchRadius, k_indices, k_sqr_distances, searchK);
+		}
+		else
+		{
+			tree->nearestKSearch(cloud->at(i), searchK, k_indices, k_sqr_distances);
+		}
+
+		for(unsigned int j=0; j<k_indices.size(); ++j)
+		{
+			if(k_indices.at(j) != (int)i)
+			{
+				const pcl::PointXYZ & pt2 = cloud->at(k_indices.at(j));
+				Eigen::Vector3f v(pt2.x-pt.x, pt2.y - pt.y, pt2.z - pt.z);
+				Eigen::Vector3f up = v.cross(direction);
+				Eigen::Vector3f n = up.cross(v);
+				n.normalize();
+				neighborNormals.push_back(n);
+			}
+		}
+
+		if(neighborNormals.empty())
+		{
+			normals->at(i).normal_x = bad_point;
+			normals->at(i).normal_y = bad_point;
+			normals->at(i).normal_z = bad_point;
+		}
+		else
+		{
+			Eigen::Vector3f meanNormal(0,0,0);
+			for(unsigned int j=0; j<neighborNormals.size(); ++j)
+			{
+				meanNormal+=neighborNormals[j];
+			}
+			meanNormal /= (float)neighborNormals.size();
+			meanNormal.normalize();
+			normals->at(i).normal_x = meanNormal[0];
+			normals->at(i).normal_y = meanNormal[1];
+			normals->at(i).normal_z = meanNormal[2];
+		}
+	}
+
+	return normals;
+}
+
+pcl::PointCloud<pcl::Normal>::Ptr computeFastOrganizedNormals2D(
+		const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
+		int searchK,
+		float searchRadius,
+		const Eigen::Vector3f & viewPoint)
+{
+	UASSERT(searchK>0);
+	pcl::PointCloud<pcl::Normal>::Ptr normals (new pcl::PointCloud<pcl::Normal>);
+
+	normals->resize(cloud->size());
+	searchRadius *= searchRadius; // squared distance
+
+	float bad_point = std::numeric_limits<float>::quiet_NaN ();
+
+	// assuming that points are ordered
+	for(int i=0; i<(int)cloud->size(); ++i)
+	{
+		int li = i-searchK;
+		if(li<0)
+		{
+			li=0;
+		}
+		int hi = i+searchK;
+		if(hi>=(int)cloud->size())
+		{
+			hi=(int)cloud->size()-1;
+		}
+
+		// get points before not too far
+		const pcl::PointXYZ & pt = cloud->at(i);
+		std::vector<Eigen::Vector3f> neighborNormals;
+		Eigen::Vector3f direction;
+		direction[0] = viewPoint[0] - cloud->at(i).x;
+		direction[1] = viewPoint[1] - cloud->at(i).y;
+		direction[2] = viewPoint[2] - cloud->at(i).z;
+		for(int j=i-1; j>=li; --j)
+		{
+			const pcl::PointXYZ & pt2 = cloud->at(j);
+			Eigen::Vector3f vd(pt2.x-pt.x, pt2.y - pt.y, pt2.z - pt.z);
+			if(searchRadius<=0.0f || (vd[0]*vd[0] + vd[1]*vd[1] + vd[2]*vd[2]) < searchRadius)
+			{
+				Eigen::Vector3f v(pt2.x-pt.x, pt2.y - pt.y, pt2.z - pt.z);
+				Eigen::Vector3f up = v.cross(direction);
+				Eigen::Vector3f n = up.cross(v);
+				n.normalize();
+				neighborNormals.push_back(n);
+			}
+			else
+			{
+				break;
+			}
+		}
+		for(int j=i+1; j<=hi; ++j)
+		{
+			const pcl::PointXYZ & pt2 = cloud->at(j);
+			Eigen::Vector3f vd(pt2.x-pt.x, pt2.y - pt.y, pt2.z - pt.z);
+			if(searchRadius<=0.0f || (vd[0]*vd[0] + vd[1]*vd[1] + vd[2]*vd[2]) < searchRadius)
+			{
+				Eigen::Vector3f v(pt2.x-pt.x, pt2.y - pt.y, pt2.z - pt.z);
+				Eigen::Vector3f up = v[2]==0.0f?Eigen::Vector3f(0,0,1):v.cross(direction);
+				Eigen::Vector3f n = up.cross(v);
+				n.normalize();
+				neighborNormals.push_back(n);
+			}
+			else
+			{
+				break;
+			}
+		}
+
+		if(neighborNormals.empty())
+		{
+			normals->at(i).normal_x = bad_point;
+			normals->at(i).normal_y = bad_point;
+			normals->at(i).normal_z = bad_point;
+		}
+		else
+		{
+			Eigen::Vector3f meanNormal(0,0,0);
+			for(unsigned int j=0; j<neighborNormals.size(); ++j)
+			{
+				meanNormal+=neighborNormals[j];
+			}
+			meanNormal /= (float)neighborNormals.size();
+			meanNormal.normalize();
+			normals->at(i).normal_x = meanNormal[0];
+			normals->at(i).normal_y = meanNormal[1];
+			normals->at(i).normal_z = meanNormal[2];
+		}
+	}
+
+	return normals;
+}
+
 pcl::PointCloud<pcl::Normal>::Ptr computeFastOrganizedNormals(
 		const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
 		float maxDepthChangeFactor,