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);
104 operator() (
const std::size_t col_index,
const std::size_t row_index)
106 return map_[row_index * width_ + col_index];
114 operator() (
const std::size_t col_index,
const std::size_t row_index)
const
116 return map_[row_index * width_ + col_index];
125 std::vector<float> map_;
177 initializeLUT (
const int range_x_arg,
const int range_y_arg,
const int range_z_arg)
195 constexpr
int nr_normals = 8;
198 constexpr
float normal0_angle = 40.0f * 3.14f / 180.0f;
199 ref_normals[0].x = std::cos (normal0_angle);
200 ref_normals[0].y = 0.0f;
201 ref_normals[0].z = -sinf (normal0_angle);
203 constexpr
float inv_nr_normals = 1.0f /
static_cast<float>(nr_normals);
204 for (
int normal_index = 1; normal_index < nr_normals; ++normal_index)
206 const float angle = 2.0f *
static_cast<float> (
M_PI * normal_index * inv_nr_normals);
208 ref_normals[normal_index].x = std::cos (angle) * ref_normals[0].x - sinf (angle) * ref_normals[0].y;
209 ref_normals[normal_index].y = sinf (angle) * ref_normals[0].x + std::cos (angle) * ref_normals[0].y;
210 ref_normals[normal_index].z = ref_normals[0].z;
214 for (
int normal_index = 0; normal_index < nr_normals; ++normal_index)
216 const float length = std::sqrt (ref_normals[normal_index].x * ref_normals[normal_index].x +
217 ref_normals[normal_index].y * ref_normals[normal_index].y +
218 ref_normals[normal_index].z * ref_normals[normal_index].z);
220 const float inv_length = 1.0f / length;
222 ref_normals[normal_index].x *= inv_length;
223 ref_normals[normal_index].y *= inv_length;
224 ref_normals[normal_index].z *= inv_length;
228 for (
int z_index = 0; z_index <
size_z; ++z_index)
230 for (
int y_index = 0; y_index <
size_y; ++y_index)
232 for (
int x_index = 0; x_index <
size_x; ++x_index)
235 static_cast<float> (y_index -
range_y/2),
236 static_cast<float> (z_index -
range_z));
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);
240 normal.x *= inv_length;
241 normal.y *= inv_length;
242 normal.z *= inv_length;
244 float max_response = -1.0f;
247 for (
int normal_index = 0; normal_index < nr_normals; ++normal_index)
249 const float response = normal.x * ref_normals[normal_index].x +
250 normal.y * ref_normals[normal_index].y +
251 normal.z * ref_normals[normal_index].z;
253 const float abs_response = std::abs (response);
254 if (max_response < abs_response)
256 max_response = abs_response;
257 max_index = normal_index;
260 lut[z_index*
size_y*
size_x + y_index*
size_x + x_index] =
static_cast<unsigned char> (0x1 << max_index);
273 operator() (
const float x,
const float y,
const float z)
const
275 const auto x_index =
static_cast<std::size_t
> (x *
static_cast<float> (
offset_x) +
static_cast<float> (
offset_x));
276 const auto y_index =
static_cast<std::size_t
> (y *
static_cast<float> (
offset_y) +
static_cast<float> (
offset_y));
277 const auto z_index =
static_cast<std::size_t
> (z *
static_cast<float> (
range_z) +
static_cast<float> (
range_z));
299 template <
typename Po
intInT>
348 spreading_size_ = spreading_size;
357 variable_feature_nr_ = enabled;
364 return surface_normals_;
371 return surface_normals_;
378 return (filtered_quantized_surface_normals_);
385 return (spreaded_quantized_surface_normals_);
392 return (surface_normal_orientations_);
404 std::vector<QuantizedMultiModFeature> & features)
const override;
414 std::vector<QuantizedMultiModFeature> & features)
const override;
466 bool variable_feature_nr_;
469 float feature_distance_threshold_;
471 float min_distance_to_border_;
477 std::size_t spreading_size_;
496 template <
typename Po
intInT>
499 : variable_feature_nr_ (false)
500 , feature_distance_threshold_ (2.0f)
501 , min_distance_to_border_ (2.0f)
502 , spreading_size_ (8)
507 template <
typename Po
intInT>
511 template <
typename Po
intInT>
void
520 computeAndQuantizeSurfaceNormals2 ();
523 filterQuantizedSurfaceNormals ();
527 spreaded_quantized_surface_normals_,
532 template <
typename Po
intInT>
void
537 spreaded_quantized_surface_normals_,
542 template <
typename Po
intInT>
void
555 template <
typename Po
intInT>
void
567 const float bad_point = std::numeric_limits<float>::quiet_NaN ();
569 const int width = input_->width;
570 const int height = input_->height;
572 surface_normals_.resize (width*height);
573 surface_normals_.width = width;
574 surface_normals_.height = height;
575 surface_normals_.is_dense =
false;
577 quantized_surface_normals_.resize (width, height);
591 for (
int y = 0; y < height; ++y)
593 for (
int x = 0; x < width; ++x)
595 const int index = y * width + x;
597 const float px = (*input_)[index].x;
598 const float py = (*input_)[index].y;
599 const float pz = (*input_)[index].z;
601 if (std::isnan(px) || pz > 2.0f)
603 surface_normals_[index].normal_x = bad_point;
604 surface_normals_[index].normal_y = bad_point;
605 surface_normals_[index].normal_z = bad_point;
606 surface_normals_[index].curvature = bad_point;
608 quantized_surface_normals_ (x, y) = 0;
613 const int smoothingSizeInt = 5;
622 for (
int v = y - smoothingSizeInt; v <= y + smoothingSizeInt; v += smoothingSizeInt)
624 for (
int u = x - smoothingSizeInt; u <= x + smoothingSizeInt; u += smoothingSizeInt)
626 if (u < 0 || u >= width || v < 0 || v >= height)
continue;
628 const std::size_t index2 = v * width + u;
630 const float qx = (*input_)[index2].x;
631 const float qy = (*input_)[index2].y;
632 const float qz = (*input_)[index2].z;
634 if (std::isnan(qx))
continue;
636 const float delta = qz - pz;
637 const float i = qx - px;
638 const float j = qy - py;
640 const float f = std::abs(delta) < 0.05f ? 1.0f : 0.0f;
645 vecb0 += f * i * delta;
646 vecb1 += f * j * delta;
650 const float det = matA0 * matA3 - matA1 * matA1;
651 const float ddx = matA3 * vecb0 - matA1 * vecb1;
652 const float ddy = -matA1 * vecb0 + matA0 * vecb1;
654 const float nx = ddx;
655 const float ny = ddy;
656 const float nz = -det * pz;
658 const float length = nx * nx + ny * ny + nz * nz;
662 surface_normals_[index].normal_x = bad_point;
663 surface_normals_[index].normal_y = bad_point;
664 surface_normals_[index].normal_z = bad_point;
665 surface_normals_[index].curvature = bad_point;
667 quantized_surface_normals_ (x, y) = 0;
671 const float normInv = 1.0f / std::sqrt (length);
673 const float normal_x = nx * normInv;
674 const float normal_y = ny * normInv;
675 const float normal_z = nz * normInv;
677 surface_normals_[index].normal_x = normal_x;
678 surface_normals_[index].normal_y = normal_y;
679 surface_normals_[index].normal_z = normal_z;
680 surface_normals_[index].curvature = bad_point;
682 float angle = 11.25f + std::atan2 (normal_y, normal_x)*180.0f/3.14f;
684 if (angle < 0.0f) angle += 360.0f;
685 if (angle >= 360.0f) angle -= 360.0f;
687 int bin_index =
static_cast<int> (angle*8.0f/360.0f + 1);
688 bin_index = (bin_index < 1) ? 1 : (8 < bin_index) ? 8 : bin_index;
690 quantized_surface_normals_ (x, y) =
static_cast<unsigned char> (bin_index);
701 static 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;
722 template <
typename Po
intInT>
void
725 const int width = input_->width;
726 const int height = input_->height;
728 auto * lp_depth =
new unsigned short[width*height]{};
729 auto * lp_normals =
new unsigned char[width*height]{};
731 surface_normal_orientations_.resize (width, height, 0.0f);
733 for (
int row_index = 0; row_index < height; ++row_index)
735 for (
int col_index = 0; col_index < width; ++col_index)
737 const float value = (*input_)[row_index*width + col_index].z;
738 if (std::isfinite (value))
740 lp_depth[row_index*width + col_index] =
static_cast<unsigned short> (value * 1000.0f);
744 lp_depth[row_index*width + col_index] = 0;
749 const int l_W = width;
750 const int l_H = height;
752 constexpr
int l_r = 5;
762 const int offsets_i[] = {-l_r, 0, l_r, -l_r, l_r, -l_r, 0, l_r};
763 const int offsets_j[] = {-l_r, -l_r, -l_r, 0, 0, l_r, l_r, l_r};
764 const int offsets[] = { offsets_i[0] + offsets_j[0] * l_W
765 , offsets_i[1] + offsets_j[1] * l_W
766 , offsets_i[2] + offsets_j[2] * l_W
767 , offsets_i[3] + offsets_j[3] * l_W
768 , offsets_i[4] + offsets_j[4] * l_W
769 , offsets_i[5] + offsets_j[5] * l_W
770 , offsets_i[6] + offsets_j[6] * l_W
771 , offsets_i[7] + offsets_j[7] * l_W };
777 constexpr
int difference_threshold = 50;
778 constexpr
int distance_threshold = 2000;
784 for (
int l_y = l_r; l_y < l_H - l_r - 1; ++l_y)
786 unsigned short * lp_line = lp_depth + (l_y * l_W + l_r);
787 unsigned char * lp_norm = lp_normals + (l_y * l_W + l_r);
789 for (
int l_x = l_r; l_x < l_W - l_r - 1; ++l_x)
791 long l_d = lp_line[0];
796 if (l_d < distance_threshold)
799 long l_A[4]; l_A[0] = l_A[1] = l_A[2] = l_A[3] = 0;
800 long l_b[2]; l_b[0] = l_b[1] = 0;
804 accumBilateral(lp_line[offsets[0]] - l_d, offsets_i[0], offsets_j[0], l_A, l_b, difference_threshold);
805 accumBilateral(lp_line[offsets[1]] - l_d, offsets_i[1], offsets_j[1], l_A, l_b, difference_threshold);
806 accumBilateral(lp_line[offsets[2]] - l_d, offsets_i[2], offsets_j[2], l_A, l_b, difference_threshold);
807 accumBilateral(lp_line[offsets[3]] - l_d, offsets_i[3], offsets_j[3], l_A, l_b, difference_threshold);
808 accumBilateral(lp_line[offsets[4]] - l_d, offsets_i[4], offsets_j[4], l_A, l_b, difference_threshold);
809 accumBilateral(lp_line[offsets[5]] - l_d, offsets_i[5], offsets_j[5], l_A, l_b, difference_threshold);
810 accumBilateral(lp_line[offsets[6]] - l_d, offsets_i[6], offsets_j[6], l_A, l_b, difference_threshold);
811 accumBilateral(lp_line[offsets[7]] - l_d, offsets_i[7], offsets_j[7], l_A, l_b, difference_threshold);
870 long l_det = l_A[0] * l_A[3] - l_A[1] * l_A[1];
871 long l_ddx = l_A[3] * l_b[0] - l_A[1] * l_b[1];
872 long l_ddy = -l_A[1] * l_b[0] + l_A[0] * l_b[1];
876 float l_nx =
static_cast<float>(1150 * l_ddx);
877 float l_ny =
static_cast<float>(1150 * l_ddy);
878 float l_nz =
static_cast<float>(-l_det * l_d);
892 float l_sqrt = std::sqrt (l_nx * l_nx + l_ny * l_ny + l_nz * l_nz);
896 float l_norminv = 1.0f / (l_sqrt);
902 float angle = 11.25f + std::atan2 (l_ny, l_nx) * 180.0f / 3.14f;
904 if (angle < 0.0f) angle += 360.0f;
905 if (angle >= 360.0f) angle -= 360.0f;
907 int bin_index =
static_cast<int> (angle*8.0f/360.0f);
909 surface_normal_orientations_ (l_x, l_y) = angle;
918 *lp_norm =
static_cast<unsigned char> (0x1 << bin_index);
935 unsigned char map[255]{};
946 quantized_surface_normals_.resize (width, height);
947 for (
int row_index = 0; row_index < height; ++row_index)
949 for (
int col_index = 0; col_index < width; ++col_index)
951 quantized_surface_normals_ (col_index, row_index) = map[lp_normals[row_index*width + col_index]];
961 template <
typename Po
intInT>
void
963 const std::size_t nr_features,
964 const std::size_t modality_index,
965 std::vector<QuantizedMultiModFeature> & features)
const
967 const std::size_t width = mask.
getWidth ();
968 const std::size_t height = mask.
getHeight ();
981 for (
auto &mask_map : mask_maps)
982 mask_map.
resize (width, height);
984 unsigned char map[255]{};
998 for (std::size_t row_index = 0; row_index < height; ++row_index)
1000 for (std::size_t col_index = 0; col_index < width; ++col_index)
1002 if (mask (col_index, row_index) != 0)
1005 const unsigned char quantized_value = filtered_quantized_surface_normals_ (col_index, row_index);
1007 if (quantized_value == 0)
1009 const int dist_map_index = map[quantized_value];
1011 distance_map_indices (col_index, row_index) =
static_cast<unsigned char> (dist_map_index);
1013 mask_maps[dist_map_index] (col_index, row_index) = 255;
1019 for (
int map_index = 0; map_index < 8; ++map_index)
1020 computeDistanceMap (mask_maps[map_index], distance_maps[map_index]);
1023 computeDistanceMap (mask, mask_distance_maps);
1025 std::list<Candidate> list1;
1026 std::list<Candidate> list2;
1028 float weights[8] = {0,0,0,0,0,0,0,0};
1030 constexpr std::size_t off = 4;
1031 for (std::size_t row_index = off; row_index < height-off; ++row_index)
1033 for (std::size_t col_index = off; col_index < width-off; ++col_index)
1035 if (mask (col_index, row_index) != 0)
1038 const unsigned char quantized_value = filtered_quantized_surface_normals_ (col_index, row_index);
1044 if (quantized_value != 0)
1046 const int distance_map_index = map[quantized_value];
1049 const float distance = distance_maps[distance_map_index] (col_index, row_index);
1050 const float distance_to_border = mask_distance_maps (col_index, row_index);
1052 if (
distance >= feature_distance_threshold_ && distance_to_border >= min_distance_to_border_)
1057 candidate.
x = col_index;
1058 candidate.
y = row_index;
1059 candidate.
bin_index =
static_cast<unsigned char> (distance_map_index);
1061 list1.push_back (candidate);
1063 ++weights[distance_map_index];
1070 for (
auto iter = list1.begin (); iter != list1.end (); ++iter)
1071 iter->distance *= 1.0f / weights[iter->bin_index];
1075 if (variable_feature_nr_)
1077 int distance =
static_cast<int> (list1.size ());
1078 bool feature_selection_finished =
false;
1079 while (!feature_selection_finished)
1082 for (
auto iter1 = list1.begin (); iter1 != list1.end (); ++iter1)
1084 bool candidate_accepted =
true;
1085 for (
auto iter2 = list2.begin (); iter2 != list2.end (); ++iter2)
1087 const int dx =
static_cast<int> (iter1->x) -
static_cast<int> (iter2->x);
1088 const int dy =
static_cast<int> (iter1->y) -
static_cast<int> (iter2->y);
1089 const int tmp_distance = dx*dx + dy*dy;
1091 if (tmp_distance < sqr_distance)
1093 candidate_accepted =
false;
1099 float min_min_sqr_distance = std::numeric_limits<float>::max ();
1100 float max_min_sqr_distance = 0;
1101 for (
auto iter2 = list2.begin (); iter2 != list2.end (); ++iter2)
1103 float min_sqr_distance = std::numeric_limits<float>::max ();
1104 for (
auto iter3 = list2.begin (); iter3 != list2.end (); ++iter3)
1109 const float dx =
static_cast<float> (iter2->x) -
static_cast<float> (iter3->x);
1110 const float dy =
static_cast<float> (iter2->y) -
static_cast<float> (iter3->y);
1112 const float sqr_distance = dx*dx + dy*dy;
1114 if (sqr_distance < min_sqr_distance)
1116 min_sqr_distance = sqr_distance;
1125 const float dx =
static_cast<float> (iter2->x) -
static_cast<float> (iter1->x);
1126 const float dy =
static_cast<float> (iter2->y) -
static_cast<float> (iter1->y);
1128 const float sqr_distance = dx*dx + dy*dy;
1130 if (sqr_distance < min_sqr_distance)
1132 min_sqr_distance = sqr_distance;
1136 if (min_sqr_distance < min_min_sqr_distance)
1137 min_min_sqr_distance = min_sqr_distance;
1138 if (min_sqr_distance > max_min_sqr_distance)
1139 max_min_sqr_distance = min_sqr_distance;
1144 if (candidate_accepted)
1150 if (min_min_sqr_distance < 50)
1152 feature_selection_finished =
true;
1156 list2.push_back (*iter1);
1170 if (list1.size () <= nr_features)
1172 features.reserve (list1.size ());
1173 for (
auto iter = list1.begin (); iter != list1.end (); ++iter)
1177 feature.
x =
static_cast<int> (iter->x);
1178 feature.
y =
static_cast<int> (iter->y);
1180 feature.
quantized_value = filtered_quantized_surface_normals_ (iter->x, iter->y);
1182 features.push_back (feature);
1188 int distance =
static_cast<int> (list1.size () / nr_features + 1);
1189 while (list2.size () != nr_features)
1192 for (
auto iter1 = list1.begin (); iter1 != list1.end (); ++iter1)
1194 bool candidate_accepted =
true;
1196 for (
auto iter2 = list2.begin (); iter2 != list2.end (); ++iter2)
1198 const int dx =
static_cast<int> (iter1->x) -
static_cast<int> (iter2->x);
1199 const int dy =
static_cast<int> (iter1->y) -
static_cast<int> (iter2->y);
1200 const int tmp_distance = dx*dx + dy*dy;
1202 if (tmp_distance < sqr_distance)
1204 candidate_accepted =
false;
1209 if (candidate_accepted)
1210 list2.push_back (*iter1);
1212 if (list2.size () == nr_features)
break;
1218 for (
auto iter2 = list2.begin (); iter2 != list2.end (); ++iter2)
1222 feature.
x =
static_cast<int> (iter2->x);
1223 feature.
y =
static_cast<int> (iter2->y);
1225 feature.
quantized_value = filtered_quantized_surface_normals_ (iter2->x, iter2->y);
1227 features.push_back (feature);
1232 template <
typename Po
intInT>
void
1234 const MaskMap & mask,
const std::size_t,
const std::size_t modality_index,
1235 std::vector<QuantizedMultiModFeature> & features)
const
1237 const std::size_t width = mask.
getWidth ();
1238 const std::size_t height = mask.
getHeight ();
1251 for (
auto &mask_map : mask_maps)
1252 mask_map.
resize (width, height);
1254 unsigned char map[255]{};
1268 for (std::size_t row_index = 0; row_index < height; ++row_index)
1270 for (std::size_t col_index = 0; col_index < width; ++col_index)
1272 if (mask (col_index, row_index) != 0)
1275 const unsigned char quantized_value = filtered_quantized_surface_normals_ (col_index, row_index);
1277 if (quantized_value == 0)
1279 const int dist_map_index = map[quantized_value];
1281 distance_map_indices (col_index, row_index) =
static_cast<unsigned char> (dist_map_index);
1283 mask_maps[dist_map_index] (col_index, row_index) = 255;
1289 for (
int map_index = 0; map_index < 8; ++map_index)
1290 computeDistanceMap (mask_maps[map_index], distance_maps[map_index]);
1293 computeDistanceMap (mask, mask_distance_maps);
1295 std::list<Candidate> list1;
1296 std::list<Candidate> list2;
1298 float weights[8] = {0,0,0,0,0,0,0,0};
1300 constexpr std::size_t off = 4;
1301 for (std::size_t row_index = off; row_index < height-off; ++row_index)
1303 for (std::size_t col_index = off; col_index < width-off; ++col_index)
1305 if (mask (col_index, row_index) != 0)
1308 const unsigned char quantized_value = filtered_quantized_surface_normals_ (col_index, row_index);
1314 if (quantized_value != 0)
1316 const int distance_map_index = map[quantized_value];
1319 const float distance = distance_maps[distance_map_index] (col_index, row_index);
1320 const float distance_to_border = mask_distance_maps (col_index, row_index);
1322 if (
distance >= feature_distance_threshold_ && distance_to_border >= min_distance_to_border_)
1327 candidate.
x = col_index;
1328 candidate.
y = row_index;
1329 candidate.
bin_index =
static_cast<unsigned char> (distance_map_index);
1331 list1.push_back (candidate);
1333 ++weights[distance_map_index];
1340 for (
auto iter = list1.begin (); iter != list1.end (); ++iter)
1341 iter->distance *= 1.0f / weights[iter->bin_index];
1345 features.reserve (list1.size ());
1346 for (
auto iter = list1.begin (); iter != list1.end (); ++iter)
1350 feature.
x =
static_cast<int> (iter->x);
1351 feature.
y =
static_cast<int> (iter->y);
1353 feature.
quantized_value = filtered_quantized_surface_normals_ (iter->x, iter->y);
1355 features.push_back (feature);
1360 template <
typename Po
intInT>
void
1363 const std::size_t width = input_->width;
1364 const std::size_t height = input_->height;
1366 quantized_surface_normals_.resize (width, height);
1368 for (std::size_t row_index = 0; row_index < height; ++row_index)
1370 for (std::size_t col_index = 0; col_index < width; ++col_index)
1372 const float normal_x = surface_normals_ (col_index, row_index).normal_x;
1373 const float normal_y = surface_normals_ (col_index, row_index).normal_y;
1374 const float normal_z = surface_normals_ (col_index, row_index).normal_z;
1376 if (std::isnan(normal_x) || std::isnan(normal_y) || std::isnan(normal_z) || normal_z > 0 || (normal_x == 0 && normal_y == 0))
1378 quantized_surface_normals_ (col_index, row_index) = 0;
1385 float angle = 11.25f + std::atan2 (normal_y, normal_x)*180.0f/3.14f;
1387 if (angle < 0.0f) angle += 360.0f;
1388 if (angle >= 360.0f) angle -= 360.0f;
1390 int bin_index =
static_cast<int> (angle*8.0f/360.0f + 1);
1391 bin_index = (bin_index < 1) ? 1 : (8 < bin_index) ? 8 : bin_index;
1394 quantized_surface_normals_ (col_index, row_index) =
static_cast<unsigned char> (bin_index);
1402 template <
typename Po
intInT>
void
1405 const int width = input_->width;
1406 const int height = input_->height;
1408 filtered_quantized_surface_normals_.resize (width, height);
1465 for (
int row_index = 2; row_index < height-2; ++row_index)
1467 for (
int col_index = 2; col_index < width-2; ++col_index)
1469 unsigned char histogram[9] = {0,0,0,0,0,0,0,0,0};
1491 unsigned char * dataPtr = quantized_surface_normals_.getData () + (row_index-2)*width+col_index-2;
1492 ++histogram[dataPtr[0]];
1493 ++histogram[dataPtr[1]];
1494 ++histogram[dataPtr[2]];
1495 ++histogram[dataPtr[3]];
1496 ++histogram[dataPtr[4]];
1499 unsigned char * dataPtr = quantized_surface_normals_.getData () + (row_index-1)*width+col_index-2;
1500 ++histogram[dataPtr[0]];
1501 ++histogram[dataPtr[1]];
1502 ++histogram[dataPtr[2]];
1503 ++histogram[dataPtr[3]];
1504 ++histogram[dataPtr[4]];
1507 unsigned char * dataPtr = quantized_surface_normals_.getData () + (row_index)*width+col_index-2;
1508 ++histogram[dataPtr[0]];
1509 ++histogram[dataPtr[1]];
1510 ++histogram[dataPtr[2]];
1511 ++histogram[dataPtr[3]];
1512 ++histogram[dataPtr[4]];
1515 unsigned char * dataPtr = quantized_surface_normals_.getData () + (row_index+1)*width+col_index-2;
1516 ++histogram[dataPtr[0]];
1517 ++histogram[dataPtr[1]];
1518 ++histogram[dataPtr[2]];
1519 ++histogram[dataPtr[3]];
1520 ++histogram[dataPtr[4]];
1523 unsigned char * dataPtr = quantized_surface_normals_.getData () + (row_index+2)*width+col_index-2;
1524 ++histogram[dataPtr[0]];
1525 ++histogram[dataPtr[1]];
1526 ++histogram[dataPtr[2]];
1527 ++histogram[dataPtr[3]];
1528 ++histogram[dataPtr[4]];
1532 unsigned char max_hist_value = 0;
1533 int max_hist_index = -1;
1535 if (max_hist_value < histogram[1]) {max_hist_index = 0; max_hist_value = histogram[1];}
1536 if (max_hist_value < histogram[2]) {max_hist_index = 1; max_hist_value = histogram[2];}
1537 if (max_hist_value < histogram[3]) {max_hist_index = 2; max_hist_value = histogram[3];}
1538 if (max_hist_value < histogram[4]) {max_hist_index = 3; max_hist_value = histogram[4];}
1539 if (max_hist_value < histogram[5]) {max_hist_index = 4; max_hist_value = histogram[5];}
1540 if (max_hist_value < histogram[6]) {max_hist_index = 5; max_hist_value = histogram[6];}
1541 if (max_hist_value < histogram[7]) {max_hist_index = 6; max_hist_value = histogram[7];}
1542 if (max_hist_value < histogram[8]) {max_hist_index = 7; max_hist_value = histogram[8];}
1544 if (max_hist_index != -1 && max_hist_value >= 1)
1546 filtered_quantized_surface_normals_ (col_index, row_index) =
static_cast<unsigned char> (0x1 << max_hist_index);
1550 filtered_quantized_surface_normals_ (col_index, row_index) = 0;
1574 template <
typename Po
intInT>
void
1577 const std::size_t width = input.
getWidth ();
1578 const std::size_t height = input.
getHeight ();
1580 output.
resize (width, height);
1584 const unsigned char * mask_map = input.
getData ();
1585 float * distance_map = output.
getData ();
1586 for (std::size_t index = 0; index < width*height; ++index)
1588 if (mask_map[index] == 0)
1589 distance_map[index] = 0.0f;
1591 distance_map[index] =
static_cast<float> (width + height);
1595 float * previous_row = distance_map;
1596 float * current_row = previous_row + width;
1597 for (std::size_t ri = 1; ri < height; ++ri)
1599 for (std::size_t ci = 1; ci < width; ++ci)
1601 const float up_left = previous_row [ci - 1] + 1.4f;
1602 const float up = previous_row [ci] + 1.0f;
1603 const float up_right = previous_row [ci + 1] + 1.4f;
1604 const float left = current_row [ci - 1] + 1.0f;
1605 const float center = current_row [ci];
1607 const float min_value = std::min (std::min (up_left, up), std::min (left, up_right));
1609 if (min_value < center)
1610 current_row[ci] = min_value;
1612 previous_row = current_row;
1613 current_row += width;
1617 float * next_row = distance_map + width * (height - 1);
1618 current_row = next_row - width;
1619 for (
int ri =
static_cast<int> (height)-2; ri >= 0; --ri)
1621 for (
int ci =
static_cast<int> (width)-2; ci >= 0; --ci)
1623 const float lower_left = next_row [ci - 1] + 1.4f;
1624 const float lower = next_row [ci] + 1.0f;
1625 const float lower_right = next_row [ci + 1] + 1.4f;
1626 const float right = current_row [ci + 1] + 1.0f;
1627 const float center = current_row [ci];
1629 const float min_value = std::min (std::min (lower_left, lower), std::min (right, lower_right));
1631 if (min_value < center)
1632 current_row[ci] = min_value;
1634 next_row = current_row;
1635 current_row -= width;
Represents a distance map obtained from a distance transformation.
float * getData()
Returns a pointer to the beginning of map.
void resize(const std::size_t width, const std::size_t height)
Resizes the map to the specified size.
void compute(PointCloudOut &output)
Base method for feature estimation for all points given in <setInputCloud (), setIndices ()> using th...
Surface normal estimation on dense data using a least-squares estimation based on a first-order Taylo...
void setNormalSmoothingSize(float normal_smoothing_size)
Set the normal smoothing size.
void setInputCloud(const typename PointCloudIn::ConstPtr &cloud) override
Provide a pointer to the input dataset (overwrites the PCLBase::setInputCloud method)
void setMaxDepthChangeFactor(float max_depth_change_factor)
The depth change threshold for computing object borders.
void setDepthDependentSmoothing(bool use_depth_dependent_smoothing)
Set whether to use depth depending smoothing or not.
std::size_t getWidth() const
unsigned char * getData()
void resize(std::size_t width, std::size_t height)
std::size_t getHeight() const
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.
void computeAndQuantizeSurfaceNormals()
Computes and quantizes the surface normals.
void setSpreadingSize(const std::size_t spreading_size)
Sets the spreading size.
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).
~SurfaceNormalModality() override
Destructor.
virtual void processInputDataFromFiltered()
Processes the input data assuming that everything up to filtering is already done/available (so only ...
QuantizedMap & getSpreadedQuantizedMap() override
Returns a reference to the internal spread quantized map.
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)
QuantizedMap & getQuantizedMap() override
Returns a reference to the internal quantized map.
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.
pcl::PointCloud< pcl::Normal > & getSurfaceNormals()
Returns the surface normals.
LINEMOD_OrientationMap & getOrientationMap()
Returns a reference to the orientation 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.
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.
float distance(const PointT &p1, const PointT &p2)
Map that stores orientations.
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.
~LINEMOD_OrientationMap()=default
Destructor.
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.