41 #include <pcl/memory.h>
42 #include <pcl/pcl_macros.h>
43 #include <pcl/point_cloud.h>
44 #include <pcl/features/feature.h>
45 #include <pcl/features/integral_image2D.h>
64 template <
typename Po
intInT,
typename Po
intOutT>
74 using Ptr = shared_ptr<IntegralImageNormalEstimation<PointInT, PointOutT> >;
75 using ConstPtr = shared_ptr<const IntegralImageNormalEstimation<PointInT, PointOutT> >;
110 , integral_image_DX_ (false)
111 , integral_image_DY_ (false)
112 , integral_image_depth_ (false)
113 , integral_image_XYZ_ (true)
114 , max_depth_change_factor_ (20.0f*0.001f)
137 border_policy_ = border_policy;
147 computePointNormal (
const int pos_x,
const int pos_y,
const unsigned point_index, PointOutT &normal);
165 max_depth_change_factor_ = max_depth_change_factor;
175 if (normal_smoothing_size < 2.0f)
177 PCL_ERROR (
"[pcl::%s::setNormalSmoothingSize] Invalid normal smoothing size given! (%g). Must be at least 2. Defaulting to %g.\n",
178 feature_name_.c_str (), normal_smoothing_size, normal_smoothing_size_);
181 normal_smoothing_size_ = normal_smoothing_size;
199 normal_estimation_method_ = normal_estimation_method;
208 use_depth_dependent_smoothing_ = use_depth_dependent_smoothing;
218 if (!cloud->isOrganized ())
220 PCL_ERROR (
"[pcl::IntegralImageNormalEstimation::setInputCloud] Input dataset is not organized (height = 1).\n");
224 init_covariance_matrix_ = init_average_3d_gradient_ = init_depth_change_ =
false;
226 if (use_sensor_origin_)
228 vpx_ =
input_->sensor_origin_.coeff (0);
229 vpy_ =
input_->sensor_origin_.coeff (1);
230 vpz_ =
input_->sensor_origin_.coeff (2);
242 return (distance_map_);
256 use_sensor_origin_ =
false;
282 use_sensor_origin_ =
true;
285 vpx_ =
input_->sensor_origin_.coeff (0);
286 vpy_ =
input_->sensor_origin_.coeff (1);
287 vpz_ =
input_->sensor_origin_.coeff (2);
338 flipNormalTowardsViewpoint (
const PointInT &point,
339 float vp_x,
float vp_y,
float vp_z,
340 float &nx,
float &ny,
float &nz)
348 float cos_theta = (vp_x * nx + vp_y * ny + vp_z * nz);
372 int rect_width_2_{0};
373 int rect_width_4_{0};
376 int rect_height_2_{0};
377 int rect_height_4_{0};
380 float distance_threshold_{0.0f};
383 IntegralImage2D<float, 3> integral_image_DX_;
385 IntegralImage2D<float, 3> integral_image_DY_;
387 IntegralImage2D<float, 1> integral_image_depth_;
389 IntegralImage2D<float, 3> integral_image_XYZ_;
392 float *diff_x_{
nullptr};
394 float *diff_y_{
nullptr};
397 float *depth_data_{
nullptr};
400 float *distance_map_{
nullptr};
403 bool use_depth_dependent_smoothing_{
false};
406 float max_depth_change_factor_;
409 float normal_smoothing_size_{10.0f};
412 bool init_covariance_matrix_{
false};
415 bool init_average_3d_gradient_{
false};
418 bool init_simple_3d_gradient_{
false};
421 bool init_depth_change_{
false};
425 float vpx_{0.0f}, vpy_{0.0f}, vpz_{0.0f};
428 bool use_sensor_origin_{
true};
432 initCompute ()
override;
436 initCovarianceMatrixMethod ();
440 initAverage3DGradientMethod ();
444 initAverageDepthChangeMethod ();
448 initSimple3DGradientMethod ();
455 #ifdef PCL_NO_PRECOMPILE
456 #include <pcl/features/impl/integral_image_normal.hpp>
~IntegralImageNormalEstimation() override
Destructor.
void computePointNormalMirror(const int pos_x, const int pos_y, const unsigned point_index, PointOutT &normal)
Computes the normal at the specified position with mirroring for border handling. ...
std::string feature_name_
The feature name.
int k_
The number of K nearest neighbors to use for each point.
void computePointNormal(const int pos_x, const int pos_y, const unsigned point_index, PointOutT &normal)
Computes the normal at the specified position.
IntegralImageNormalEstimation()
Constructor.
#define PCL_MAKE_ALIGNED_OPERATOR_NEW
Macro to signal a class requires a custom allocator.
KdTreePtr tree_
A pointer to the spatial search object.
void setBorderPolicy(const BorderPolicy border_policy)
Sets the policy for handling borders.
void getViewPoint(float &vpx, float &vpy, float &vpz)
Get the viewpoint.
shared_ptr< const Feature< pcl::PointXYZRGBA, pcl::pcl::Normal > > ConstPtr
void setRectSize(const int width, const int height)
Set the regions size which is considered for normal estimation.
void initData()
Initialize the data structures, based on the normal estimation method chosen.
Surface normal estimation on organized data using integral images.
typename Feature< PointInT, PointOutT >::PointCloudOut PointCloudOut
void setMaxDepthChangeFactor(float max_depth_change_factor)
The depth change threshold for computing object borders.
void useSensorOriginAsViewPoint()
sets whether the sensor origin or a user given viewpoint should be used.
void computeFeatureFull(const float *distance_map, const float &bad_point, PointCloudOut &output)
Computes the normal for the complete cloud.
void setNormalEstimationMethod(NormalEstimationMethod normal_estimation_method)
Set the normal estimation method.
BorderPolicy
Different types of border handling.
void setInputCloud(const typename PointCloudIn::ConstPtr &cloud) override
Provide a pointer to the input dataset (overwrites the PCLBase::setInputCloud method) ...
void computeFeature(PointCloudOut &output) override
Computes the normal for the complete cloud or only indices_ if provided.
void setViewPoint(float vpx, float vpy, float vpz)
Set the viewpoint.
float * getDistanceMap()
Returns a pointer to the distance map which was computed internally.
void computeFeaturePart(const float *distance_map, const float &bad_point, PointCloudOut &output)
Computes the normal for part of the cloud specified by indices_.
shared_ptr< const PointCloud< pcl::PointXYZRGBA > > ConstPtr
PointCloudConstPtr input_
The input point cloud dataset.
Feature represents the base feature class.
NormalEstimationMethod
Different normal estimation methods.
void setNormalSmoothingSize(float normal_smoothing_size)
Set the normal smoothing size.
shared_ptr< Feature< pcl::PointXYZRGBA, pcl::pcl::Normal > > Ptr
void setDepthDependentSmoothing(bool use_depth_dependent_smoothing)
Set whether to use depth depending smoothing or not.