TEST (PCL, IINormalEstimationSimple3DGradient)
{
PointCloud<Normal> output;
ne.setRectSize (3, 3);
ne.setNormalEstimationMethod (ne.SIMPLE_3D_GRADIENT);
ne.compute (output);
EXPECT_EQ (output.points.size (), cloud.points.size ());
EXPECT_EQ (output.width, cloud.width);
EXPECT_EQ (output.height, cloud.height);
for (size_t v = 0; v < cloud.height; ++v)
{
for (size_t u = 0; u < cloud.width; ++u)
{
if (!pcl_isfinite(output (u, v).normal_x) &&
!pcl_isfinite(output (u, v).normal_y) &&
!pcl_isfinite(output (u, v).normal_z))
continue;
if (fabs(fabs (output (u, v).normal_z) - 1) > 1e-2)
{
std::cout << "T:" << u << " , " << v << " : " << output (u, v).normal_x << " , " << output (u, v).normal_y << " , " << output (u, v).normal_z <<std::endl;
}
EXPECT_NEAR (fabs (output (u, v).normal_x), 0, 1e-2);
EXPECT_NEAR (fabs (output (u, v).normal_y), 0, 1e-2);
//EXPECT_NEAR (fabs (output (u, v).normal_z), 1.0, 1e-2);
}
}
}
TEST (PCL, IINormalEstimationAverageDepthChange)
{
PointCloud<Normal> output;
ne.setRectSize (3, 3);
ne.setNormalEstimationMethod (ne.AVERAGE_DEPTH_CHANGE);
ne.compute (output);
EXPECT_EQ (output.points.size (), cloud.points.size ());
EXPECT_EQ (output.width, cloud.width);
EXPECT_EQ (output.height, cloud.height);
for (size_t v = 0; v < cloud.height; ++v)
{
for (size_t u = 0; u < cloud.width; ++u)
{
if (!pcl_isfinite(output (u, v).normal_x) &&
!pcl_isfinite(output (u, v).normal_y) &&
!pcl_isfinite(output (u, v).normal_z))
continue;
if (fabs(fabs (output (u, v).normal_z) - 1) > 1e-2)
{
std::cout << "T:" << u << " , " << v << " : " << output (u, v).normal_x << " , " << output (u, v).normal_y << " , " << output (u, v).normal_z <<std::endl;
}
EXPECT_NEAR (fabs (output (u, v).normal_x), 0, 1e-2);
EXPECT_NEAR (fabs (output (u, v).normal_y), 0, 1e-2);
//EXPECT_NEAR (fabs (output (u, v).normal_z), 1.0, 1e-2);
}
}
}
TEST (PCL, IINormalEstimationCovariance)
{
PointCloud<Normal> output;
ne.setRectSize (3, 3);
ne.setNormalEstimationMethod (ne.COVARIANCE_MATRIX);
ne.compute (output);
EXPECT_EQ (output.points.size (), cloud.points.size ());
EXPECT_EQ (output.width, cloud.width);
EXPECT_EQ (output.height, cloud.height);
for (size_t v = 0; v < cloud.height; ++v)
{
for (size_t u = 0; u < cloud.width; ++u)
{
if (!pcl_isfinite(output (u, v).normal_x) &&
!pcl_isfinite(output (u, v).normal_y) &&
!pcl_isfinite(output (u, v).normal_z))
continue;
EXPECT_NEAR (fabs (output (u, v).normal_x), 0, 1e-2);
EXPECT_NEAR (fabs (output (u, v).normal_y), 0, 1e-2);
EXPECT_NEAR (fabs (output (u, v).normal_z), 1.0, 1e-2);
}
}
}
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
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);
}
}
/* ---[ */
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;
}
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);
}
