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_)
1057 Candidate candidate;
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_)
1328 Candidate candidate;
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;