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 const int nr_normals = 8;
198 const 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 const 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 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>
347 spreading_size_ = spreading_size;
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_;
495 template <
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)
506 template <
typename Po
intInT>
512 template <
typename Po
intInT>
void
521 computeAndQuantizeSurfaceNormals2 ();
524 filterQuantizedSurfaceNormals ();
528 spreaded_quantized_surface_normals_,
533 template <
typename Po
intInT>
void
538 spreaded_quantized_surface_normals_,
543 template <
typename Po
intInT>
void
556 template <
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;
614 const int smoothingSizeInt = 5;
623 for (
int v = y - smoothingSizeInt; v <= y + smoothingSizeInt; v += smoothingSizeInt)
625 for (
int u = x - smoothingSizeInt; u <= x + smoothingSizeInt; u += smoothingSizeInt)
627 if (u < 0 || u >= width || v < 0 || v >= height)
continue;
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;
637 const float delta = qz - pz;
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;
646 vecb0 += f * i * delta;
647 vecb1 += f * j * delta;
651 const float det = matA0 * matA3 - matA1 * matA1;
652 const float ddx = matA3 * vecb0 - matA1 * vecb1;
653 const float ddy = -matA1 * vecb0 + matA0 * vecb1;
655 const float nx = ddx;
656 const float ny = ddy;
657 const float nz = -det * pz;
659 const float length = nx * nx + ny * ny + nz * nz;
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);
674 const float normal_x = nx * normInv;
675 const float normal_y = ny * normInv;
676 const float normal_z = nz * normInv;
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);
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 unsigned short * lp_depth =
new unsigned short[width*height];
729 unsigned char * lp_normals =
new unsigned char[width*height];
730 memset (lp_normals, 0, width*height);
732 surface_normal_orientations_.resize (width, height, 0.0f);
734 for (
int row_index = 0; row_index < height; ++row_index)
736 for (
int col_index = 0; col_index < width; ++col_index)
738 const float value = (*input_)[row_index*width + col_index].z;
739 if (std::isfinite (value))
741 lp_depth[row_index*width + col_index] =
static_cast<unsigned short> (value * 1000.0f);
745 lp_depth[row_index*width + col_index] = 0;
750 const int l_W = width;
751 const int l_H = height;
763 const int offsets_i[] = {-l_r, 0, l_r, -l_r, l_r, -l_r, 0, l_r};
764 const int offsets_j[] = {-l_r, -l_r, -l_r, 0, 0, l_r, l_r, l_r};
765 const int offsets[] = { offsets_i[0] + offsets_j[0] * l_W
766 , offsets_i[1] + offsets_j[1] * l_W
767 , offsets_i[2] + offsets_j[2] * l_W
768 , offsets_i[3] + offsets_j[3] * l_W
769 , offsets_i[4] + offsets_j[4] * l_W
770 , offsets_i[5] + offsets_j[5] * l_W
771 , offsets_i[6] + offsets_j[6] * l_W
772 , offsets_i[7] + offsets_j[7] * l_W };
778 const int difference_threshold = 50;
779 const int distance_threshold = 2000;
785 for (
int l_y = l_r; l_y < l_H - l_r - 1; ++l_y)
787 unsigned short * lp_line = lp_depth + (l_y * l_W + l_r);
788 unsigned char * lp_norm = lp_normals + (l_y * l_W + l_r);
790 for (
int l_x = l_r; l_x < l_W - l_r - 1; ++l_x)
792 long l_d = lp_line[0];
797 if (l_d < distance_threshold)
800 long l_A[4]; l_A[0] = l_A[1] = l_A[2] = l_A[3] = 0;
801 long l_b[2]; l_b[0] = l_b[1] = 0;
805 accumBilateral(lp_line[offsets[0]] - l_d, offsets_i[0], offsets_j[0], l_A, l_b, difference_threshold);
806 accumBilateral(lp_line[offsets[1]] - l_d, offsets_i[1], offsets_j[1], l_A, l_b, difference_threshold);
807 accumBilateral(lp_line[offsets[2]] - l_d, offsets_i[2], offsets_j[2], l_A, l_b, difference_threshold);
808 accumBilateral(lp_line[offsets[3]] - l_d, offsets_i[3], offsets_j[3], l_A, l_b, difference_threshold);
809 accumBilateral(lp_line[offsets[4]] - l_d, offsets_i[4], offsets_j[4], l_A, l_b, difference_threshold);
810 accumBilateral(lp_line[offsets[5]] - l_d, offsets_i[5], offsets_j[5], l_A, l_b, difference_threshold);
811 accumBilateral(lp_line[offsets[6]] - l_d, offsets_i[6], offsets_j[6], l_A, l_b, difference_threshold);
812 accumBilateral(lp_line[offsets[7]] - l_d, offsets_i[7], offsets_j[7], l_A, l_b, difference_threshold);
871 long l_det = l_A[0] * l_A[3] - l_A[1] * l_A[1];
872 long l_ddx = l_A[3] * l_b[0] - l_A[1] * l_b[1];
873 long l_ddy = -l_A[1] * l_b[0] + l_A[0] * l_b[1];
877 float l_nx =
static_cast<float>(1150 * l_ddx);
878 float l_ny =
static_cast<float>(1150 * l_ddy);
879 float l_nz =
static_cast<float>(-l_det * l_d);
893 float l_sqrt = std::sqrt (l_nx * l_nx + l_ny * l_ny + l_nz * l_nz);
897 float l_norminv = 1.0f / (l_sqrt);
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);
949 for (
int row_index = 0; row_index < height; ++row_index)
951 for (
int col_index = 0; col_index < width; ++col_index)
953 quantized_surface_normals_ (col_index, row_index) = map[lp_normals[row_index*width + col_index]];
963 template <
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 ();
983 for (
auto &mask_map : mask_maps)
984 mask_map.
resize (width, height);
986 unsigned char map[255];
1001 for (std::size_t row_index = 0; row_index < height; ++row_index)
1003 for (std::size_t col_index = 0; col_index < width; ++col_index)
1005 if (mask (col_index, row_index) != 0)
1008 const unsigned char quantized_value = filtered_quantized_surface_normals_ (col_index, row_index);
1010 if (quantized_value == 0)
1012 const int dist_map_index = map[quantized_value];
1014 distance_map_indices (col_index, row_index) =
static_cast<unsigned char> (dist_map_index);
1016 mask_maps[dist_map_index] (col_index, row_index) = 255;
1022 for (
int map_index = 0; map_index < 8; ++map_index)
1023 computeDistanceMap (mask_maps[map_index], distance_maps[map_index]);
1026 computeDistanceMap (mask, mask_distance_maps);
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;
1034 for (std::size_t row_index = off; row_index < height-off; ++row_index)
1036 for (std::size_t col_index = off; col_index < width-off; ++col_index)
1038 if (mask (col_index, row_index) != 0)
1041 const unsigned char quantized_value = filtered_quantized_surface_normals_ (col_index, row_index);
1047 if (quantized_value != 0)
1049 const int distance_map_index = map[quantized_value];
1052 const float distance = distance_maps[distance_map_index] (col_index, row_index);
1053 const float distance_to_border = mask_distance_maps (col_index, row_index);
1055 if (
distance >= feature_distance_threshold_ && distance_to_border >= min_distance_to_border_)
1060 candidate.
x = col_index;
1061 candidate.
y = row_index;
1062 candidate.
bin_index =
static_cast<unsigned char> (distance_map_index);
1064 list1.push_back (candidate);
1066 ++weights[distance_map_index];
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 ());
1081 bool feature_selection_finished =
false;
1082 while (!feature_selection_finished)
1085 for (
typename std::list<Candidate>::iterator iter1 = list1.begin (); iter1 != list1.end (); ++iter1)
1087 bool candidate_accepted =
true;
1088 for (
typename std::list<Candidate>::iterator iter2 = list2.begin (); iter2 != list2.end (); ++iter2)
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);
1092 const int tmp_distance = dx*dx + dy*dy;
1094 if (tmp_distance < sqr_distance)
1096 candidate_accepted =
false;
1102 float min_min_sqr_distance = std::numeric_limits<float>::max ();
1103 float max_min_sqr_distance = 0;
1104 for (
typename std::list<Candidate>::iterator iter2 = list2.begin (); iter2 != list2.end (); ++iter2)
1106 float min_sqr_distance = std::numeric_limits<float>::max ();
1107 for (
typename std::list<Candidate>::iterator iter3 = list2.begin (); iter3 != list2.end (); ++iter3)
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);
1115 const float sqr_distance = dx*dx + dy*dy;
1117 if (sqr_distance < min_sqr_distance)
1119 min_sqr_distance = sqr_distance;
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);
1131 const float sqr_distance = dx*dx + dy*dy;
1133 if (sqr_distance < min_sqr_distance)
1135 min_sqr_distance = sqr_distance;
1139 if (min_sqr_distance < min_min_sqr_distance)
1140 min_min_sqr_distance = min_sqr_distance;
1141 if (min_sqr_distance > max_min_sqr_distance)
1142 max_min_sqr_distance = min_sqr_distance;
1147 if (candidate_accepted)
1153 if (min_min_sqr_distance < 50)
1155 feature_selection_finished =
true;
1159 list2.push_back (*iter1);
1173 if (list1.size () <= nr_features)
1175 features.reserve (list1.size ());
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);
1192 while (list2.size () != nr_features)
1195 for (
typename std::list<Candidate>::iterator iter1 = list1.begin (); iter1 != list1.end (); ++iter1)
1197 bool candidate_accepted =
true;
1199 for (
typename std::list<Candidate>::iterator iter2 = list2.begin (); iter2 != list2.end (); ++iter2)
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);
1203 const int tmp_distance = dx*dx + dy*dy;
1205 if (tmp_distance < sqr_distance)
1207 candidate_accepted =
false;
1212 if (candidate_accepted)
1213 list2.push_back (*iter1);
1215 if (list2.size () == nr_features)
break;
1221 for (
typename std::list<Candidate>::iterator iter2 = list2.begin (); iter2 != list2.end (); ++iter2)
1225 feature.
x =
static_cast<int> (iter2->x);
1226 feature.
y =
static_cast<int> (iter2->y);
1228 feature.
quantized_value = filtered_quantized_surface_normals_ (iter2->x, iter2->y);
1230 features.push_back (feature);
1235 template <
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 ();
1254 for (
auto &mask_map : mask_maps)
1255 mask_map.
resize (width, height);
1257 unsigned char map[255];
1258 memset(map, 0, 255);
1272 for (std::size_t row_index = 0; row_index < height; ++row_index)
1274 for (std::size_t col_index = 0; col_index < width; ++col_index)
1276 if (mask (col_index, row_index) != 0)
1279 const unsigned char quantized_value = filtered_quantized_surface_normals_ (col_index, row_index);
1281 if (quantized_value == 0)
1283 const int dist_map_index = map[quantized_value];
1285 distance_map_indices (col_index, row_index) =
static_cast<unsigned char> (dist_map_index);
1287 mask_maps[dist_map_index] (col_index, row_index) = 255;
1293 for (
int map_index = 0; map_index < 8; ++map_index)
1294 computeDistanceMap (mask_maps[map_index], distance_maps[map_index]);
1297 computeDistanceMap (mask, mask_distance_maps);
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;
1305 for (std::size_t row_index = off; row_index < height-off; ++row_index)
1307 for (std::size_t col_index = off; col_index < width-off; ++col_index)
1309 if (mask (col_index, row_index) != 0)
1312 const unsigned char quantized_value = filtered_quantized_surface_normals_ (col_index, row_index);
1318 if (quantized_value != 0)
1320 const int distance_map_index = map[quantized_value];
1323 const float distance = distance_maps[distance_map_index] (col_index, row_index);
1324 const float distance_to_border = mask_distance_maps (col_index, row_index);
1326 if (
distance >= feature_distance_threshold_ && distance_to_border >= min_distance_to_border_)
1331 candidate.
x = col_index;
1332 candidate.
y = row_index;
1333 candidate.
bin_index =
static_cast<unsigned char> (distance_map_index);
1335 list1.push_back (candidate);
1337 ++weights[distance_map_index];
1344 for (
typename std::list<Candidate>::iterator iter = list1.begin (); iter != list1.end (); ++iter)
1345 iter->distance *= 1.0f / weights[iter->bin_index];
1349 features.reserve (list1.size ());
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);
1364 template <
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);
1372 for (std::size_t row_index = 0; row_index < height; ++row_index)
1374 for (std::size_t col_index = 0; col_index < width; ++col_index)
1376 const float normal_x = surface_normals_ (col_index, row_index).normal_x;
1377 const float normal_y = surface_normals_ (col_index, row_index).normal_y;
1378 const float normal_z = surface_normals_ (col_index, row_index).normal_z;
1380 if (std::isnan(normal_x) || std::isnan(normal_y) || std::isnan(normal_z) || normal_z > 0)
1382 quantized_surface_normals_ (col_index, row_index) = 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);
1405 template <
typename Po
intInT>
void
1408 const int width = input_->width;
1409 const int height = input_->height;
1411 filtered_quantized_surface_normals_.resize (width, height);
1468 for (
int row_index = 2; row_index < height-2; ++row_index)
1470 for (
int col_index = 2; col_index < width-2; ++col_index)
1472 unsigned char histogram[9] = {0,0,0,0,0,0,0,0,0};
1494 unsigned char * dataPtr = quantized_surface_normals_.getData () + (row_index-2)*width+col_index-2;
1495 ++histogram[dataPtr[0]];
1496 ++histogram[dataPtr[1]];
1497 ++histogram[dataPtr[2]];
1498 ++histogram[dataPtr[3]];
1499 ++histogram[dataPtr[4]];
1502 unsigned char * dataPtr = quantized_surface_normals_.getData () + (row_index-1)*width+col_index-2;
1503 ++histogram[dataPtr[0]];
1504 ++histogram[dataPtr[1]];
1505 ++histogram[dataPtr[2]];
1506 ++histogram[dataPtr[3]];
1507 ++histogram[dataPtr[4]];
1510 unsigned char * dataPtr = quantized_surface_normals_.getData () + (row_index)*width+col_index-2;
1511 ++histogram[dataPtr[0]];
1512 ++histogram[dataPtr[1]];
1513 ++histogram[dataPtr[2]];
1514 ++histogram[dataPtr[3]];
1515 ++histogram[dataPtr[4]];
1518 unsigned char * dataPtr = quantized_surface_normals_.getData () + (row_index+1)*width+col_index-2;
1519 ++histogram[dataPtr[0]];
1520 ++histogram[dataPtr[1]];
1521 ++histogram[dataPtr[2]];
1522 ++histogram[dataPtr[3]];
1523 ++histogram[dataPtr[4]];
1526 unsigned char * dataPtr = quantized_surface_normals_.getData () + (row_index+2)*width+col_index-2;
1527 ++histogram[dataPtr[0]];
1528 ++histogram[dataPtr[1]];
1529 ++histogram[dataPtr[2]];
1530 ++histogram[dataPtr[3]];
1531 ++histogram[dataPtr[4]];
1535 unsigned char max_hist_value = 0;
1536 int max_hist_index = -1;
1538 if (max_hist_value < histogram[1]) {max_hist_index = 0; max_hist_value = histogram[1];}
1539 if (max_hist_value < histogram[2]) {max_hist_index = 1; max_hist_value = histogram[2];}
1540 if (max_hist_value < histogram[3]) {max_hist_index = 2; max_hist_value = histogram[3];}
1541 if (max_hist_value < histogram[4]) {max_hist_index = 3; max_hist_value = histogram[4];}
1542 if (max_hist_value < histogram[5]) {max_hist_index = 4; max_hist_value = histogram[5];}
1543 if (max_hist_value < histogram[6]) {max_hist_index = 5; max_hist_value = histogram[6];}
1544 if (max_hist_value < histogram[7]) {max_hist_index = 6; max_hist_value = histogram[7];}
1545 if (max_hist_value < histogram[8]) {max_hist_index = 7; max_hist_value = histogram[8];}
1547 if (max_hist_index != -1 && max_hist_value >= 1)
1549 filtered_quantized_surface_normals_ (col_index, row_index) =
static_cast<unsigned char> (0x1 << max_hist_index);
1553 filtered_quantized_surface_normals_ (col_index, row_index) = 0;
1577 template <
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);
1587 const unsigned char * mask_map = input.
getData ();
1588 float * distance_map = output.
getData ();
1589 for (std::size_t index = 0; index < width*height; ++index)
1591 if (mask_map[index] == 0)
1592 distance_map[index] = 0.0f;
1594 distance_map[index] =
static_cast<float> (width + height);
1598 float * previous_row = distance_map;
1599 float * current_row = previous_row + width;
1600 for (std::size_t ri = 1; ri < height; ++ri)
1602 for (std::size_t ci = 1; ci < width; ++ci)
1604 const float up_left = previous_row [ci - 1] + 1.4f;
1605 const float up = previous_row [ci] + 1.0f;
1606 const float up_right = previous_row [ci + 1] + 1.4f;
1607 const float left = current_row [ci - 1] + 1.0f;
1608 const float center = current_row [ci];
1610 const float min_value = std::min (std::min (up_left, up), std::min (left, up_right));
1612 if (min_value < center)
1613 current_row[ci] = min_value;
1615 previous_row = current_row;
1616 current_row += width;
1620 float * next_row = distance_map + width * (height - 1);
1621 current_row = next_row - width;
1622 for (
int ri =
static_cast<int> (height)-2; ri >= 0; --ri)
1624 for (
int ci =
static_cast<int> (width)-2; ci >= 0; --ci)
1626 const float lower_left = next_row [ci - 1] + 1.4f;
1627 const float lower = next_row [ci] + 1.0f;
1628 const float lower_right = next_row [ci + 1] + 1.4f;
1629 const float right = current_row [ci + 1] + 1.0f;
1630 const float center = current_row [ci];
1632 const float min_value = std::min (std::min (lower_left, lower), std::min (right, lower_right));
1634 if (min_value < center)
1635 current_row[ci] = min_value;
1637 next_row = current_row;
1638 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.
~SurfaceNormalModality()
Destructor.
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).
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()
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.