void
estimateNormals (const typename PointCloud<PointT>::ConstPtr &input, PointCloud<Normal> &normals)
{
if (input->isOrganized ())
{
IntegralImageNormalEstimation<PointT, Normal> ne;
// Set the parameters for normal estimation
ne.setNormalEstimationMethod (ne.COVARIANCE_MATRIX);
ne.setMaxDepthChangeFactor (0.02f);
ne.setNormalSmoothingSize (20.0f);
// Estimate normals in the cloud
ne.setInputCloud (input);
ne.compute (normals);
// Save the distance map for the plane comparator
float *map=ne.getDistanceMap ();// This will be deallocated with the IntegralImageNormalEstimation object...
distance_map_.assign(map, map+input->size() ); //...so we must copy the data out
plane_comparator_->setDistanceMap(distance_map_.data());
}
else
{
NormalEstimation<PointT, Normal> ne;
ne.setInputCloud (input);
ne.setRadiusSearch (0.02f);
ne.setSearchMethod (search_);
ne.compute (normals);
}
}
void
compute (const sensor_msgs::PointCloud2::ConstPtr &input, sensor_msgs::PointCloud2 &output,
int k, double radius)
{
// Convert data to PointCloud<T>
PointCloud<PointXYZ>::Ptr xyz (new PointCloud<PointXYZ>);
fromROSMsg (*input, *xyz);
TicToc tt;
tt.tic ();
PointCloud<Normal> normals;
// Try our luck with organized integral image based normal estimation
if (xyz->isOrganized ())
{
IntegralImageNormalEstimation<PointXYZ, Normal> ne;
ne.setInputCloud (xyz);
ne.setNormalEstimationMethod (IntegralImageNormalEstimation<PointXYZ, Normal>::COVARIANCE_MATRIX);
ne.setNormalSmoothingSize (float (radius));
ne.setDepthDependentSmoothing (true);
ne.compute (normals);
}
else
{
NormalEstimation<PointXYZ, Normal> ne;
ne.setInputCloud (xyz);
ne.setSearchMethod (search::KdTree<PointXYZ>::Ptr (new search::KdTree<PointXYZ>));
ne.setKSearch (k);
ne.setRadiusSearch (radius);
ne.compute (normals);
}
print_highlight ("Computed normals in "); print_value ("%g", tt.toc ()); print_info (" ms for "); print_value ("%d", normals.width * normals.height); print_info (" points.\n");
// Convert data back
sensor_msgs::PointCloud2 output_normals;
toROSMsg (normals, output_normals);
concatenateFields (*input, output_normals, output);
}
