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); } }
TEST (PCL, IINormalEstimationSimple3DGradientUnorganized) { PointCloud<Normal> output; cloud.height = 1; cloud.points.resize (cloud.height * cloud.width); ne.setInputCloud (cloud.makeShared ()); ne.setRectSize (3, 3); ne.setNormalEstimationMethod (ne.SIMPLE_3D_GRADIENT); ne.compute (output); EXPECT_EQ (output.points.size (), 0); EXPECT_EQ (output.width, 0); EXPECT_EQ (output.height, 0); }
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); }
/* ---[ */ int main (int argc, char** argv) { cloud.width = 640; cloud.height = 480; cloud.points.resize (cloud.width * cloud.height); cloud.is_dense = true; for (size_t v = 0; v < cloud.height; ++v) { for (size_t u = 0; u < cloud.width; ++u) { cloud (u, v).x = u; cloud (u, v).y = v; cloud (u, v).z = 10; } } ne.setInputCloud (cloud.makeShared ()); testing::InitGoogleTest (&argc, argv); return (RUN_ALL_TESTS ()); return 1; }