i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-09-02 09:30:25 +08:00
Code Issues Packages Projects Releases Wiki Activity

added fast normal estimation on obstacle segmentation

Browse Source
This commit is contained in:
matlabbe
2023-06-02 16:05:05 -07:00
parent 1b67d6a86a
commit d48e2093f5
9 changed files with 278 additions and 61 deletions
Download Patch File Download Diff File Expand all files Collapse all files

1
corelib/include/rtabmap/core/OccupancyGrid.h
View File

@@ -132,6 +132,7 @@ private:
bool normalsSegmentation_;
bool grid3D_;
bool groundIsObstacle_;
bool labelUndergroundObstaclesAsGround_;
float noiseFilteringRadius_;
int noiseFilteringMinNeighbors_;
bool scan2dUnknownSpaceFilled_;

1
corelib/include/rtabmap/core/Parameters.h
View File

@@ -760,6 +760,7 @@ class RTABMAP_CORE_EXPORT Parameters
RTABMAP_PARAM(Grid, 3D, bool, false, uFormat("A 3D occupancy grid is required if you want an OctoMap (3D ray tracing). Set to false if you want only a 2D map, the cloud will be projected on xy plane. A 2D map can be still generated if checked, but it requires more memory and time to generate it. Ignored if laser scan is 2D and \"%s\" is 0.", kGridSensor().c_str()));
#endif
RTABMAP_PARAM(Grid, GroundIsObstacle, bool, false, uFormat("[%s=true] Ground segmentation (%s) is ignored, all points are obstacles. Use this only if you want an OctoMap with ground identified as an obstacle (e.g., with an UAV).", kGrid3D().c_str(), kGridNormalsSegmentation().c_str()));
RTABMAP_PARAM(Grid, UndergroundIsGround, bool, false, uFormat("[%s=true] Label all underground points under largest flat surface detected as ground.", kGridNormalsSegmentation().c_str()));
RTABMAP_PARAM(Grid, NoiseFilteringRadius, float, 0.0, "Noise filtering radius (0=disabled). Done after segmentation.");
RTABMAP_PARAM(Grid, NoiseFilteringMinNeighbors, int, 5, "Noise filtering minimum neighbors.");
RTABMAP_PARAM(Grid, Scan2dUnknownSpaceFilled, bool, false, uFormat("Unknown space filled. Only used with 2D laser scans. Use %s to set maximum range if laser scan max range is to set.", kGridRangeMax().c_str()));

43
corelib/include/rtabmap/core/impl/OccupancyGrid.hpp
View File

@@ -54,6 +54,7 @@ typename pcl::PointCloud<PointT>::Ptr OccupancyGrid::segmentCloud(
typename pcl::PointCloud<PointT>::Ptr cloud(new pcl::PointCloud<PointT>);
pcl::IndicesPtr indices(new std::vector<int>);
UDEBUG("preVoxelFiltering=%d", preVoxelFiltering_?1:0);
if(preVoxelFiltering_)
{
// voxelize to grid cell size
@@ -127,6 +128,8 @@ typename pcl::PointCloud<PointT>::Ptr OccupancyGrid::segmentCloud(
UDEBUG("flatObstaclesDetected=%d", flatObstaclesDetected_?1:0);
UDEBUG("maxGroundHeight=%f", maxGroundHeight_);
UDEBUG("groundNormalsUp=%f", groundNormalsUp_);
UDEBUG("labelUndergroundObstaclesAsGround=%d", labelUndergroundObstaclesAsGround_?1:0);
UDEBUG("viewPoint=%f,%f,%f", viewPoint.x, viewPoint.y, viewPoint.z+(projMapFrame_?pose.z():0));
util3d::segmentObstaclesFromGround<PointT>(
cloud,
indices,
@@ -140,8 +143,8 @@ typename pcl::PointCloud<PointT>::Ptr OccupancyGrid::segmentCloud(
maxGroundHeight_,
flatObstacles,
Eigen::Vector4f(viewPoint.x, viewPoint.y, viewPoint.z+(projMapFrame_?pose.z():0), 1),
groundNormalsUp_);
UDEBUG("viewPoint=%f,%f,%f", viewPoint.x, viewPoint.y, viewPoint.z+(projMapFrame_?pose.z():0));
groundNormalsUp_,
labelUndergroundObstaclesAsGround_);
//UWARN("Saving ground.pcd and obstacles.pcd");
//pcl::io::savePCDFile("ground.pcd", *cloud, *groundIndices);
//pcl::io::savePCDFile("obstacles.pcd", *cloud, *obstaclesIndices);
@@ -165,6 +168,42 @@ typename pcl::PointCloud<PointT>::Ptr OccupancyGrid::segmentCloud(
UDEBUG("groundIndices=%d obstaclesIndices=%d", (int)groundIndices->size(), (int)obstaclesIndices->size());
if(!preVoxelFiltering_ && (!groundIndices->empty() || !obstaclesIndices->empty()))
{
// voxelize to grid cell size
typename pcl::PointCloud<PointT>::Ptr cloudWithTransform = cloud;
cloud.reset(new pcl::PointCloud<PointT>);
if(!groundIndices->empty())
{
*cloud += *util3d::voxelize(cloudWithTransform, groundIndices, cellSize_);
groundIndices->resize(cloud->size());
for(size_t i=0; i<groundIndices->size(); ++i)
{
groundIndices->at(i) = i;
}
}
if(!obstaclesIndices->empty())
{
int previousSize = cloud->size();
*cloud += *util3d::voxelize(cloudWithTransform, obstaclesIndices, cellSize_);
obstaclesIndices->resize(cloud->size()-previousSize);
for(size_t i=0; i<obstaclesIndices->size(); ++i)
{
obstaclesIndices->at(i) = previousSize+i;
}
}
if(flatObstacles && !(*flatObstacles)->empty())
{
int previousSize = cloud->size();
*cloud += *util3d::voxelize(cloudWithTransform, *flatObstacles, cellSize_);
(*flatObstacles)->resize(cloud->size()-previousSize);
for(size_t i=0; i<(*flatObstacles)->size(); ++i)
{
(*flatObstacles)->at(i) = previousSize+i;
}
}
}
// Do radius filtering after voxel filtering ( a lot faster)
if(noiseFilteringRadius_ > 0.0 && noiseFilteringMinNeighbors_ > 0)
{

68
corelib/include/rtabmap/core/impl/util3d_mapping.hpp
View File

@@ -64,7 +64,8 @@ void segmentObstaclesFromGround(
float maxGroundHeight,
pcl::IndicesPtr * flatObstacles,
const Eigen::Vector4f & viewPoint,
float groundNormalsUp)
float groundNormalsUp,
bool labelUndergroundObstaclesAsGround)
{
ground.reset(new std::vector<int>);
obstacles.reset(new std::vector<int>);
@@ -84,10 +85,15 @@ void segmentObstaclesFromGround(
normalKSearch,
viewPoint,
groundNormalsUp);
UDEBUG("%ld points on flat surfaces (input indices = %ld, total cloud=%ld)",
flatSurfaces->size(), indices->size(), cloud->size());
if(segmentFlatObstacles && flatSurfaces->size())
{
int biggestFlatSurfaceIndex;
//If cloud is orgazized, cluster/floodfill using intergral image (can use radius to limit the flood)
std::vector<pcl::IndicesPtr> clusteredFlatSurfaces = extractClusters(
cloud,
flatSurfaces,
@@ -125,6 +131,8 @@ void segmentObstaclesFromGround(
if(biggestFlatSurfaceIndex>=0)
{
ground = clusteredFlatSurfaces.at(biggestFlatSurfaceIndex);
UDEBUG("Biggest flat surface size = %ld (z min=%f max=%f)",
ground->size(), biggestSurfaceMin[2], biggestSurfaceMax[2]);
}
if(!ground->empty() && (maxGroundHeight == 0.0f || biggestSurfaceMin[2] < maxGroundHeight))
@@ -135,7 +143,7 @@ void segmentObstaclesFromGround(
{
Eigen::Vector4f centroid(0,0,0,1);
pcl::compute3DCentroid(*cloud, *clusteredFlatSurfaces.at(i), centroid);
if(maxGroundHeight==0.0f || centroid[2] <= maxGroundHeight || centroid[2] <= biggestSurfaceMax[2]) // epsilon
if(centroid[2] <= biggestSurfaceMax[2]) // relative to ground detected
{
ground = util3d::concatenate(ground, clusteredFlatSurfaces.at(i));
}
@@ -171,25 +179,53 @@ void segmentObstaclesFromGround(
notObstacles = util3d::extractIndices(cloud, indices, true);
notObstacles = util3d::concatenate(notObstacles, ground);
}
pcl::IndicesPtr otherStuffIndices = util3d::extractIndices(cloud, notObstacles, true);
// If ground height is set, remove obstacles under it
if(maxGroundHeight != 0.0f)
// If ground height is set and if we label obstacles under it as ground
if(labelUndergroundObstaclesAsGround)
{
otherStuffIndices = rtabmap::util3d::passThrough(cloud, otherStuffIndices, "z", maxGroundHeight, std::numeric_limits<float>::max());
Eigen::Vector4f min,max;
if(!ground->empty())
{
pcl::getMinMax3D(*cloud, *ground, min, max);
if(maxGroundHeight>0)
{
max[2] += maxGroundHeight;
}
}
else
{
max[2] = maxGroundHeight;
}
pcl::IndicesPtr otherStuffIndices = util3d::extractIndices(cloud, notObstacles, true);
pcl::IndicesPtr underground = rtabmap::util3d::passThrough(cloud, otherStuffIndices, "z", (float)std::numeric_limits<int>::min(), max[2]);
if(!underground->empty())
{
ground = util3d::concatenate(ground, underground);
notObstacles = util3d::concatenate(underground, notObstacles);
}
}
pcl::IndicesPtr otherStuffIndices = util3d::extractIndices(cloud, notObstacles, true);
//Cluster remaining stuff (obstacles)
if(otherStuffIndices->size())
{
std::vector<pcl::IndicesPtr> clusteredObstaclesSurfaces = util3d::extractClusters(
cloud,
otherStuffIndices,
clusterRadius,
minClusterSize);
if(minClusterSize>1)
{
std::vector<pcl::IndicesPtr> clusteredObstaclesSurfaces = util3d::extractClusters(
cloud,
otherStuffIndices,
clusterRadius,
minClusterSize);
// merge indices
obstacles = util3d::concatenate(clusteredObstaclesSurfaces);
// merge indices
obstacles = util3d::concatenate(clusteredObstaclesSurfaces);
}
else
{
obstacles = otherStuffIndices;
}
}
}
}
@@ -208,7 +244,8 @@ void segmentObstaclesFromGround(
float maxGroundHeight,
pcl::IndicesPtr * flatObstacles,
const Eigen::Vector4f & viewPoint,
float groundNormalsUp)
float groundNormalsUp,
bool labelUndergroundObstaclesAsGround)
{
pcl::IndicesPtr indices(new std::vector<int>);
segmentObstaclesFromGround<PointT>(
@@ -224,7 +261,8 @@ void segmentObstaclesFromGround(
maxGroundHeight,
flatObstacles,
viewPoint,
groundNormalsUp);
groundNormalsUp,
labelUndergroundObstaclesAsGround);
}
template<typename PointT>

6
corelib/include/rtabmap/core/util3d_mapping.h
View File

@@ -157,7 +157,8 @@ void segmentObstaclesFromGround(
float maxGroundHeight = 0.0f,
pcl::IndicesPtr * flatObstacles = 0,
const Eigen::Vector4f & viewPoint = Eigen::Vector4f(0,0,100,0),
float groundNormalsUp = 0);
float groundNormalsUp = 0,
bool labelUndergroundObstaclesAsGround = false);
template<typename PointT>
void segmentObstaclesFromGround(
const typename pcl::PointCloud<PointT>::Ptr & cloud,
@@ -171,7 +172,8 @@ void segmentObstaclesFromGround(
float maxGroundHeight = 0.0f,
pcl::IndicesPtr * flatObstacles = 0,
const Eigen::Vector4f & viewPoint = Eigen::Vector4f(0,0,100,0),
float groundNormalsUp = 0);
float groundNormalsUp = 0,
bool labelUndergroundObstaclesAsGround = false);
template<typename PointT>
void occupancy2DFromGroundObstacles(

11
corelib/include/rtabmap/core/util3d_surface.h
View File

@@ -381,6 +381,17 @@ pcl::PointCloud<pcl::Normal>::Ptr RTABMAP_CORE_EXPORT computeFastOrganizedNormal
float searchRadius = 0.0f,
const Eigen::Vector3f & viewPoint = Eigen::Vector3f(0,0,0));
pcl::PointCloud<pcl::Normal>::Ptr RTABMAP_CORE_EXPORT computeFastOrganizedNormals(
const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
float maxDepthChangeFactor = 0.02f,
float normalSmoothingSize = 10.0f,
const Eigen::Vector3f & viewPoint = Eigen::Vector3f(0,0,0));
pcl::PointCloud<pcl::Normal>::Ptr RTABMAP_CORE_EXPORT computeFastOrganizedNormals(
const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
const pcl::IndicesPtr & indices,
float maxDepthChangeFactor = 0.02f,
float normalSmoothingSize = 10.0f,
const Eigen::Vector3f & viewPoint = Eigen::Vector3f(0,0,0));
pcl::PointCloud<pcl::Normal>::Ptr RTABMAP_CORE_EXPORT computeFastOrganizedNormals(
const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
float maxDepthChangeFactor = 0.02f,

2
corelib/src/OccupancyGrid.cpp
View File

@@ -66,6 +66,7 @@ OccupancyGrid::OccupancyGrid(const ParametersMap & parameters) :
normalsSegmentation_(Parameters::defaultGridNormalsSegmentation()),
grid3D_(Parameters::defaultGrid3D()),
groundIsObstacle_(Parameters::defaultGridGroundIsObstacle()),
labelUndergroundObstaclesAsGround_(Parameters::defaultGridUndergroundIsGround()),
noiseFilteringRadius_(Parameters::defaultGridNoiseFilteringRadius()),
noiseFilteringMinNeighbors_(Parameters::defaultGridNoiseFilteringMinNeighbors()),
scan2dUnknownSpaceFilled_(Parameters::defaultGridScan2dUnknownSpaceFilled()),
@@ -128,6 +129,7 @@ void OccupancyGrid::parseParameters(const ParametersMap & parameters)
Parameters::parse(parameters, Parameters::kGridNormalsSegmentation(), normalsSegmentation_);
Parameters::parse(parameters, Parameters::kGrid3D(), grid3D_);
Parameters::parse(parameters, Parameters::kGridGroundIsObstacle(), groundIsObstacle_);
Parameters::parse(parameters, Parameters::kGridUndergroundIsGround(), labelUndergroundObstaclesAsGround_);
Parameters::parse(parameters, Parameters::kGridNoiseFilteringRadius(), noiseFilteringRadius_);
Parameters::parse(parameters, Parameters::kGridNoiseFilteringMinNeighbors(), noiseFilteringMinNeighbors_);
Parameters::parse(parameters, Parameters::kGridScan2dUnknownSpaceFilled(), scan2dUnknownSpaceFilled_);

117
corelib/src/util3d.cpp
View File

@@ -372,34 +372,127 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr cloudFromDepth(
float depthCx = model.cx() * rgbToDepthFactorX;
float depthCy = model.cy() * rgbToDepthFactorY;
UDEBUG("depth=%dx%d fx=%f fy=%f cx=%f cy=%f (depth factors=%f %f) decimation=%d",
bool isMM = imageDepth.type() == CV_16UC1;
UDEBUG("depth=%dx%d (isMM=%d) fx=%f fy=%f cx=%f cy=%f (depth factors=%f %f) decimation=%d",
imageDepth.cols, imageDepth.rows,
isMM?1:0,
model.fx(), model.fy(), model.cx(), model.cy(),
rgbToDepthFactorX,
rgbToDepthFactorY,
decimation);
int decimationMode = 1;
int oi = 0;
for(int h = 0; h < imageDepth.rows && h/decimation < (int)cloud->height; h+=decimation)
if(isMM)
{
for(int w = 0; w < imageDepth.cols && w/decimation < (int)cloud->width; w+=decimation)
for(int h = 0; h < imageDepth.rows && h/decimation < (int)cloud->height; h+=decimation)
{
pcl::PointXYZ & pt = cloud->at((h/decimation)*cloud->width + (w/decimation));
pcl::PointXYZ ptXYZ = projectDepthTo3D(imageDepth, w, h, depthCx, depthCy, depthFx, depthFy, false);
if(pcl::isFinite(ptXYZ) && ptXYZ.z>=minDepth && (maxDepth<=0.0f || ptXYZ.z <= maxDepth))
const unsigned short * rowPtr = imageDepth.ptr<unsigned short>(h);
for(int w = 0; w < imageDepth.cols && w/decimation < (int)cloud->width; w+=decimation)
{
pt.x = ptXYZ.x;
pt.y = ptXYZ.y;
pt.z = ptXYZ.z;
if(validIndices)
pcl::PointXYZ & pt = cloud->at((h/decimation)*cloud->width + (w/decimation));
pt.x = pt.y = pt.z = std::numeric_limits<float>::quiet_NaN();
if(decimationMode == 1 && decimation>1)
{
// project closest point
cv::Point2i closestPixel(w,h);
unsigned short closestDepthMM = 0;
for(int v = h; v < h + decimation; ++v)
{
const unsigned short * roiRowPtr = imageDepth.ptr<unsigned short>(v);
for(int u = w; u < w + decimation; ++u)
{
const unsigned short & depthMM = roiRowPtr[u];
if(depthMM > 0 && (depthMM < closestDepthMM || closestDepthMM == 0))
{
closestDepthMM = depthMM;
closestPixel.x = u;
closestPixel.y = v;
}
}
}
if(closestDepthMM > 0)
{
float depth = ((float)closestDepthMM)/1000.0f;
if(depth>=minDepth && (maxDepth<=0.0f || depth <= maxDepth))
{
// Fill in XYZ
pt.z = depth;
pt.x = ((float)closestPixel.x - depthCx) * pt.z / depthFx;
pt.y = ((float)closestPixel.y - depthCy) * pt.z / depthFy;
}
}
}
else if(rowPtr[w]>0)
{
float depth = ((float)rowPtr[w])/1000.0f;
if(depth>=minDepth && (maxDepth<=0.0f || depth <= maxDepth))
{
// Fill in XYZ
pt.z = depth;
pt.x = ((float)w - depthCx) * pt.z / depthFx;
pt.y = ((float)h - depthCy) * pt.z / depthFy;
}
}
if(pcl::isFinite(pt) && validIndices)
{
validIndices->at(oi++) = (h/decimation)*cloud->width + (w/decimation);
}
}
else
}
}
else
{
for(int h = 0; h < imageDepth.rows && h/decimation < (int)cloud->height; h+=decimation)
{
const float * rowPtr = imageDepth.ptr<float>(h);
for(int w = 0; w < imageDepth.cols && w/decimation < (int)cloud->width; w+=decimation)
{
pcl::PointXYZ & pt = cloud->at((h/decimation)*cloud->width + (w/decimation));
pt.x = pt.y = pt.z = std::numeric_limits<float>::quiet_NaN();
if(decimationMode == 1 && decimation>1)
{
// project closest point
cv::Point2i closestPixel(w,h);
float closestDepth = 0.0f;
for(int v = h; v < h + decimation; ++v)
{
const float * roiRowPtr = imageDepth.ptr<float>(v);
for(int u = w; u < w + decimation; ++u)
{
const float & depth = roiRowPtr[u];
if(depth > 0.0f && (depth < closestDepth || closestDepth == 0.0f))
{
closestDepth = depth;
closestPixel.x = u;
closestPixel.y = v;
}
}
}
if(closestDepth > 0.0f && closestDepth>=minDepth && (maxDepth<=0.0f || closestDepth <= maxDepth))
{
// Fill in XYZ
pt.z = closestDepth;
pt.x = ((float)closestPixel.x - depthCx) * pt.z / depthFx;
pt.y = ((float)closestPixel.y - depthCy) * pt.z / depthFy;
}
}
else if(rowPtr[w] > 0 && rowPtr[w]>=minDepth && (maxDepth<=0.0f || rowPtr[w] <= maxDepth))
{
// Fill in XYZ
pt.z = rowPtr[w];
pt.x = ((float)w - depthCx) * pt.z / depthFx;
pt.y = ((float)h - depthCy) * pt.z / depthFy;
}
if(pcl::isFinite(pt) && validIndices)
{
validIndices->at(oi++) = (h/decimation)*cloud->width + (w/decimation);
}
}
}
}

90
corelib/src/util3d_surface.cpp
View File

@@ -3033,6 +3033,65 @@ pcl::PointCloud<pcl::Normal>::Ptr computeFastOrganizedNormals2D(
return computeFastOrganizedNormals2DImpl<pcl::PointXYZI>(cloud, searchK, searchRadius, viewPoint);
}
template<typename PointT>
pcl::PointCloud<pcl::Normal>::Ptr computeFastOrganizedNormalsImpl(
const typename pcl::PointCloud<PointT>::Ptr & cloud,
const pcl::IndicesPtr & indices,
float maxDepthChangeFactor,
float normalSmoothingSize,
const Eigen::Vector3f & viewPoint)
{
UASSERT(cloud->isOrganized());
// Normal estimation
pcl::PointCloud<pcl::Normal>::Ptr normals (new pcl::PointCloud<pcl::Normal>);
pcl::IntegralImageNormalEstimation<PointT, pcl::Normal> ne;
#if PCL_VERSION_COMPARE(<, 1, 7, 0)
ne.setNormalEstimationMethod (ne.AVERAGE_3D_GRADIENT);
ne.setBorderPolicy(ne.BORDER_POLICY_MIRROR);
#else
ne.setNormalEstimationMethod (ne.SIMPLE_3D_GRADIENT);
ne.setBorderPolicy(ne.BORDER_POLICY_IGNORE);
#endif
ne.setMaxDepthChangeFactor(maxDepthChangeFactor);
ne.setNormalSmoothingSize(normalSmoothingSize);
ne.setInputCloud(cloud);
// Commented: Keep the output normals size the same as the input cloud
//if(indices->size())
//{
// ne.setIndices(indices);
//}
// create kdtree search tree (not used by IntegralImageNormalEstimation) to avoid
// "[pcl::OrganizedNeighbor::radiusSearch] Input dataset is not from a projective device!"
// on clouds smaller than regular organized clouds from camera (640x480)
typename pcl::search::KdTree<PointT>::Ptr tree (new pcl::search::KdTree<PointT>);
ne.setSearchMethod(tree);
ne.setViewPoint(viewPoint[0], viewPoint[1], viewPoint[2]);
ne.compute(*normals);
return normals;
}
pcl::PointCloud<pcl::Normal>::Ptr computeFastOrganizedNormals(
const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
float maxDepthChangeFactor,
float normalSmoothingSize,
const Eigen::Vector3f & viewPoint)
{
pcl::IndicesPtr indices(new std::vector<int>);
return computeFastOrganizedNormals(cloud, indices, maxDepthChangeFactor, normalSmoothingSize, viewPoint);
}
pcl::PointCloud<pcl::Normal>::Ptr computeFastOrganizedNormals(
const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
const pcl::IndicesPtr & indices,
float maxDepthChangeFactor,
float normalSmoothingSize,
const Eigen::Vector3f & viewPoint)
{
return computeFastOrganizedNormalsImpl<pcl::PointXYZ>(cloud, indices, maxDepthChangeFactor, normalSmoothingSize, viewPoint);
}
pcl::PointCloud<pcl::Normal>::Ptr computeFastOrganizedNormals(
const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
float maxDepthChangeFactor,
@@ -3049,36 +3108,7 @@ pcl::PointCloud<pcl::Normal>::Ptr computeFastOrganizedNormals(
float normalSmoothingSize,
const Eigen::Vector3f & viewPoint)
{
UASSERT(cloud->isOrganized());
pcl::search::KdTree<pcl::PointXYZRGB>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZRGB>);
if(indices->size())
{
tree->setInputCloud(cloud, indices);
}
else
{
tree->setInputCloud (cloud);
}
// Normal estimation
pcl::PointCloud<pcl::Normal>::Ptr normals (new pcl::PointCloud<pcl::Normal>);
pcl::IntegralImageNormalEstimation<pcl::PointXYZRGB, pcl::Normal> ne;
ne.setNormalEstimationMethod (ne.AVERAGE_3D_GRADIENT);
ne.setMaxDepthChangeFactor(maxDepthChangeFactor);
ne.setNormalSmoothingSize(normalSmoothingSize);
ne.setBorderPolicy(ne.BORDER_POLICY_MIRROR);
ne.setInputCloud(cloud);
// Commented: Keep the output normals size the same as the input cloud
//if(indices->size())
//{
// ne.setIndices(indices);
//}
ne.setSearchMethod(tree);
ne.setViewPoint(viewPoint[0], viewPoint[1], viewPoint[2]);
ne.compute(*normals);
return normals;
return computeFastOrganizedNormalsImpl<pcl::PointXYZRGB>(cloud, indices, maxDepthChangeFactor, normalSmoothingSize, viewPoint);
}
float computeNormalsComplexity(