40#include <pcl/recognition/quantizable_modality.h>
41#include <pcl/recognition/distance_map.h>
43#include <pcl/pcl_base.h>
44#include <pcl/point_cloud.h>
46#include <pcl/features/linear_least_squares_normal.h>
91 resize (
const std::size_t width,
const std::size_t height,
const float value)
96 map_.resize (width*height, value);
125 std::vector<float> map_;
237 const float length = std::sqrt (normal.x*normal.x + normal.y*normal.y + normal.z*normal.z);
238 const float inv_length = 1.0f / (length + 0.00001f);
273 operator() (
const float x,
const float y,
const float z)
const
275 const std::size_t
x_index =
static_cast<std::size_t
> (x *
static_cast<float> (
offset_x) +
static_cast<float> (
offset_x));
276 const std::size_t
y_index =
static_cast<std::size_t
> (y *
static_cast<float> (
offset_y) +
static_cast<float> (
offset_y));
277 const std::size_t
z_index =
static_cast<std::size_t
> (z *
static_cast<float> (
range_z) +
static_cast<float> (
range_z));
298 template <
typename Po
intInT>
356 variable_feature_nr_ =
enabled;
363 return surface_normals_;
370 return surface_normals_;
377 return (filtered_quantized_surface_normals_);
384 return (spreaded_quantized_surface_normals_);
391 return (surface_normal_orientations_);
403 std::vector<QuantizedMultiModFeature> & features)
const override;
413 std::vector<QuantizedMultiModFeature> & features)
const override;
465 bool variable_feature_nr_;
468 float feature_distance_threshold_;
470 float min_distance_to_border_;
476 std::size_t spreading_size_;
495template <
typename Po
intInT>
498 : variable_feature_nr_ (
false)
499 , feature_distance_threshold_ (2.0f)
500 , min_distance_to_border_ (2.0f)
501 , spreading_size_ (8)
506template <
typename Po
intInT>
512template <
typename Po
intInT>
void
521 computeAndQuantizeSurfaceNormals2 ();
524 filterQuantizedSurfaceNormals ();
528 spreaded_quantized_surface_normals_,
533template <
typename Po
intInT>
void
538 spreaded_quantized_surface_normals_,
543template <
typename Po
intInT>
void
548 ne.setMaxDepthChangeFactor(0.05f);
549 ne.setNormalSmoothingSize(5.0f);
550 ne.setDepthDependentSmoothing(
false);
551 ne.setInputCloud (input_);
552 ne.compute (surface_normals_);
556template <
typename Po
intInT>
void
568 const float bad_point = std::numeric_limits<float>::quiet_NaN ();
570 const int width = input_->width;
571 const int height = input_->height;
573 surface_normals_.resize (width*height);
574 surface_normals_.width = width;
575 surface_normals_.height = height;
576 surface_normals_.is_dense =
false;
578 quantized_surface_normals_.resize (width, height);
592 for (
int y = 0; y < height; ++y)
594 for (
int x = 0; x < width; ++x)
596 const int index = y * width + x;
598 const float px = (*input_)[index].x;
599 const float py = (*input_)[index].y;
600 const float pz = (*input_)[index].z;
602 if (std::isnan(
px) ||
pz > 2.0f)
604 surface_normals_[index].normal_x =
bad_point;
605 surface_normals_[index].normal_y =
bad_point;
606 surface_normals_[index].normal_z =
bad_point;
607 surface_normals_[index].curvature =
bad_point;
609 quantized_surface_normals_ (x, y) = 0;
629 const std::size_t
index2 = v * width + u;
631 const float qx = (*input_)[
index2].x;
632 const float qy = (*input_)[
index2].y;
633 const float qz = (*input_)[
index2].z;
635 if (std::isnan(
qx))
continue;
638 const float i =
qx -
px;
639 const float j =
qy -
py;
641 const float f = std::abs(
delta) < 0.05f ? 1.0f : 0.0f;
655 const float nx =
ddx;
656 const float ny =
ddy;
663 surface_normals_[index].normal_x =
bad_point;
664 surface_normals_[index].normal_y =
bad_point;
665 surface_normals_[index].normal_z =
bad_point;
666 surface_normals_[index].curvature =
bad_point;
668 quantized_surface_normals_ (x, y) = 0;
672 const float normInv = 1.0f / std::sqrt (length);
678 surface_normals_[index].normal_x = normal_x;
679 surface_normals_[index].normal_y = normal_y;
680 surface_normals_[index].normal_z = normal_z;
681 surface_normals_[index].curvature =
bad_point;
683 float angle = 11.25f + std::atan2 (normal_y, normal_x)*180.0f/3.14f;
685 if (angle < 0.0f) angle += 360.0f;
686 if (angle >= 360.0f) angle -= 360.0f;
688 int bin_index =
static_cast<int> (angle*8.0f/360.0f) & 7;
690 quantized_surface_normals_ (x, y) =
static_cast<unsigned char> (bin_index);
701static void accumBilateral(
long delta,
long i,
long j,
long *
A,
long * b,
int threshold)
703 long f = std::abs(
delta) < threshold ? 1 : 0;
705 const long fi = f * i;
706 const long fj = f * j;
722template <
typename Po
intInT>
void
725 const int width = input_->width;
726 const int height = input_->height;
728 unsigned short *
lp_depth =
new unsigned short[width*height];
729 unsigned char *
lp_normals =
new unsigned char[width*height];
732 surface_normal_orientations_.resize (width, height, 0.0f);
739 if (std::isfinite (value))
750 const int l_W = width;
751 const int l_H = height;
877 float l_nx =
static_cast<float>(1150 *
l_ddx);
878 float l_ny =
static_cast<float>(1150 *
l_ddy);
903 float angle = 22.5f + std::atan2 (
l_ny,
l_nx) * 180.0f / 3.14f;
905 if (angle < 0.0f) angle += 360.0f;
906 if (angle >= 360.0f) angle -= 360.0f;
908 int bin_index =
static_cast<int> (angle*8.0f/360.0f) & 7;
910 surface_normal_orientations_ (
l_x,
l_y) = angle;
919 *
lp_norm =
static_cast<unsigned char> (0x1 << bin_index);
936 unsigned char map[255];
948 quantized_surface_normals_.resize (width, height);
963template <
typename Po
intInT>
void
965 const std::size_t nr_features,
966 const std::size_t modality_index,
967 std::vector<QuantizedMultiModFeature> & features)
const
969 const std::size_t width =
mask.getWidth ();
970 const std::size_t height =
mask.getHeight ();
986 unsigned char map[255];
1008 const unsigned char quantized_value = filtered_quantized_surface_normals_ (
col_index,
row_index);
1010 if (quantized_value == 0)
1028 std::list<Candidate>
list1;
1029 std::list<Candidate>
list2;
1031 float weights[8] = {0,0,0,0,0,0,0,0};
1033 const std::size_t off = 4;
1041 const unsigned char quantized_value = filtered_quantized_surface_normals_ (
col_index,
row_index);
1047 if (quantized_value != 0)
1055 if (distance >= feature_distance_threshold_ &&
distance_to_border >= min_distance_to_border_)
1073 for (
typename std::list<Candidate>::iterator iter =
list1.begin (); iter !=
list1.end (); ++iter)
1074 iter->distance *= 1.0f / weights[iter->bin_index];
1078 if (variable_feature_nr_)
1080 int distance =
static_cast<int> (
list1.
size ());
1090 const int dx =
static_cast<int> (
iter1->x) -
static_cast<int> (
iter2->x);
1091 const int dy =
static_cast<int> (
iter1->y) -
static_cast<int> (
iter2->y);
1112 const float dx =
static_cast<float> (
iter2->x) -
static_cast<float> (
iter3->x);
1113 const float dy =
static_cast<float> (
iter2->y) -
static_cast<float> (
iter3->y);
1128 const float dx =
static_cast<float> (
iter2->x) -
static_cast<float> (
iter1->x);
1129 const float dy =
static_cast<float> (
iter2->y) -
static_cast<float> (
iter1->y);
1176 for (
typename std::list<Candidate>::iterator iter =
list1.begin (); iter !=
list1.end (); ++iter)
1180 feature.
x =
static_cast<int> (iter->x);
1181 feature.
y =
static_cast<int> (iter->y);
1183 feature.
quantized_value = filtered_quantized_surface_normals_ (iter->x, iter->y);
1185 features.push_back (feature);
1191 int distance =
static_cast<int> (
list1.
size () / nr_features + 1);
1201 const int dx =
static_cast<int> (
iter1->x) -
static_cast<int> (
iter2->x);
1202 const int dy =
static_cast<int> (
iter1->y) -
static_cast<int> (
iter2->y);
1215 if (
list2.
size () == nr_features)
break;
1225 feature.
x =
static_cast<int> (
iter2->x);
1226 feature.
y =
static_cast<int> (
iter2->y);
1230 features.push_back (feature);
1235template <
typename Po
intInT>
void
1237 const MaskMap &
mask,
const std::size_t,
const std::size_t modality_index,
1238 std::vector<QuantizedMultiModFeature> & features)
const
1240 const std::size_t width =
mask.getWidth ();
1241 const std::size_t height =
mask.getHeight ();
1257 unsigned char map[255];
1279 const unsigned char quantized_value = filtered_quantized_surface_normals_ (
col_index,
row_index);
1281 if (quantized_value == 0)
1299 std::list<Candidate>
list1;
1300 std::list<Candidate>
list2;
1302 float weights[8] = {0,0,0,0,0,0,0,0};
1304 const std::size_t off = 4;
1312 const unsigned char quantized_value = filtered_quantized_surface_normals_ (
col_index,
row_index);
1318 if (quantized_value != 0)
1326 if (distance >= feature_distance_threshold_ &&
distance_to_border >= min_distance_to_border_)
1344 for (
typename std::list<Candidate>::iterator iter =
list1.begin (); iter !=
list1.end (); ++iter)
1345 iter->distance *= 1.0f / weights[iter->bin_index];
1350 for (
typename std::list<Candidate>::iterator iter =
list1.begin (); iter !=
list1.end (); ++iter)
1354 feature.
x =
static_cast<int> (iter->x);
1355 feature.
y =
static_cast<int> (iter->y);
1357 feature.
quantized_value = filtered_quantized_surface_normals_ (iter->x, iter->y);
1359 features.push_back (feature);
1364template <
typename Po
intInT>
void
1367 const std::size_t width = input_->width;
1368 const std::size_t height = input_->height;
1370 quantized_surface_normals_.resize (width, height);
1380 if (std::isnan(normal_x) || std::isnan(normal_y) || std::isnan(normal_z) || normal_z > 0)
1389 float angle = 11.25f + std::atan2 (normal_y, normal_x)*180.0f/3.14f;
1391 if (angle < 0.0f) angle += 360.0f;
1392 if (angle >= 360.0f) angle -= 360.0f;
1394 int bin_index =
static_cast<int> (angle*8.0f/360.0f);
1397 quantized_surface_normals_ (
col_index,
row_index) =
static_cast<unsigned char> (bin_index);
1405template <
typename Po
intInT>
void
1408 const int width = input_->width;
1409 const int height = input_->height;
1411 filtered_quantized_surface_normals_.resize (width, height);
1472 unsigned char histogram[9] = {0,0,0,0,0,0,0,0,0};
1577template <
typename Po
intInT>
void
1580 const std::size_t width =
input.getWidth ();
1581 const std::size_t height =
input.getHeight ();
1583 output.resize (width, height);
1589 for (std::size_t index = 0; index < width*height; ++index)
1594 distance_map[index] =
static_cast<float> (width + height);
1600 for (std::size_t
ri = 1;
ri < height; ++
ri)
1602 for (std::size_t
ci = 1;
ci < width; ++
ci)
1622 for (
int ri =
static_cast<int> (height)-2;
ri >= 0; --
ri)
1624 for (
int ci =
static_cast<int> (width)-2;
ci >= 0; --
ci)
Iterator class for point clouds with or without given indices.
std::size_t size() const
Size of the range the iterator is going through.
Represents a distance map obtained from a distance transformation.
PointCloudConstPtr input_
The input point cloud dataset.
std::vector< PointT, Eigen::aligned_allocator< PointT > > VectorType
shared_ptr< const PointCloud< PointInT > > ConstPtr
Interface for a quantizable modality.
static void spreadQuantizedMap(const QuantizedMap &input_map, QuantizedMap &output_map, std::size_t spreading_size)
Modality based on surface normals.
void computeSurfaceNormals()
Computes the surface normals from the input cloud.
~SurfaceNormalModality()
Destructor.
void computeAndQuantizeSurfaceNormals()
Computes and quantizes the surface normals.
void setSpreadingSize(const std::size_t spreading_size)
Sets the spreading size.
LINEMOD_OrientationMap & getOrientationMap()
Returns a reference to the orientation map.
const pcl::PointCloud< pcl::Normal > & getSurfaceNormals() const
Returns the surface normals.
void computeAndQuantizeSurfaceNormals2()
Computes and quantizes the surface normals.
virtual void processInputData()
Processes the input data (smoothing, computing gradients, quantizing, filtering, spreading).
QuantizedMap & getQuantizedMap() override
Returns a reference to the internal quantized map.
virtual void processInputDataFromFiltered()
Processes the input data assuming that everything up to filtering is already done/available (so only ...
void quantizeSurfaceNormals()
Quantizes the surface normals.
void setInputCloud(const typename PointCloudIn::ConstPtr &cloud) override
Provide a pointer to the input dataset (overwrites the PCLBase::setInputCloud method)
void extractAllFeatures(const MaskMap &mask, std::size_t nr_features, std::size_t modality_index, std::vector< QuantizedMultiModFeature > &features) const override
Extracts all possible features from the modality within the specified mask.
void computeDistanceMap(const MaskMap &input, DistanceMap &output) const
Computes a distance map from the supplied input mask.
QuantizedMap & getSpreadedQuantizedMap() override
Returns a reference to the internal spread quantized map.
SurfaceNormalModality()
Constructor.
void setVariableFeatureNr(const bool enabled)
Enables/disables the use of extracting a variable number of features.
void filterQuantizedSurfaceNormals()
Filters the quantized surface normals.
pcl::PointCloud< pcl::Normal > & getSurfaceNormals()
Returns the surface normals.
void extractFeatures(const MaskMap &mask, std::size_t nr_features, std::size_t modality_index, std::vector< QuantizedMultiModFeature > &features) const override
Extracts features from this modality within the specified mask.
Defines all the PCL implemented PointT point type structures.
Map that stores orientations.
~LINEMOD_OrientationMap()
Destructor.
LINEMOD_OrientationMap()
Constructor.
std::size_t getWidth() const
Returns the width of the modality data map.
std::size_t getHeight() const
Returns the height of the modality data map.
void resize(const std::size_t width, const std::size_t height, const float value)
Resizes the map to the specific width and height and initializes all new elements with the specified ...
A point structure representing normal coordinates and the surface curvature estimate.
A point structure representing Euclidean xyz coordinates.
Feature that defines a position and quantized value in a specific modality.
std::size_t modality_index
the index of the corresponding modality.
unsigned char quantized_value
the quantized value attached to the feature.
Look-up-table for fast surface normal quantization.
int size_y
The size of the LUT in y-direction.
void initializeLUT(const int range_x_arg, const int range_y_arg, const int range_z_arg)
Initializes the LUT.
int size_x
The size of the LUT in x-direction.
unsigned char operator()(const float x, const float y, const float z) const
Operator to access an element in the LUT.
QuantizedNormalLookUpTable()
Constructor.
~QuantizedNormalLookUpTable()
Destructor.
int range_y
The range of the LUT in y-direction.
int offset_x
The offset in x-direction.
unsigned char * lut
The LUT data.
int offset_z
The offset in z-direction.
int range_z
The range of the LUT in z-direction.
int size_z
The size of the LUT in z-direction.
int range_x
The range of the LUT in x-direction.
int offset_y
The offset in y-direction.
Candidate for a feature (used in feature extraction methods).
float distance
Distance to the next different quantized value.
std::size_t x
x-position of the feature.
std::size_t y
y-position of the feature.
bool operator<(const Candidate &rhs) const
Compares two candidates based on their distance to the next different quantized value.
unsigned char bin_index
Quantized value.