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>
83 resize (
const std::size_t width,
const std::size_t height,
const float value)
88 map_.resize (width*height, value);
96 operator() (
const std::size_t col_index,
const std::size_t row_index)
98 return map_[row_index * width_ + col_index];
106 operator() (
const std::size_t col_index,
const std::size_t row_index)
const
108 return map_[row_index * width_ + col_index];
117 std::vector<float> map_;
169 initializeLUT (
const int range_x_arg,
const int range_y_arg,
const int range_z_arg)
187 const int nr_normals = 8;
190 const float normal0_angle = 40.0f * 3.14f / 180.0f;
191 ref_normals[0].x = std::cos (normal0_angle);
192 ref_normals[0].y = 0.0f;
193 ref_normals[0].z = -sinf (normal0_angle);
195 const float inv_nr_normals = 1.0f / static_cast<float> (nr_normals);
196 for (
int normal_index = 1; normal_index < nr_normals; ++normal_index)
198 const float angle = 2.0f * static_cast<float> (M_PI * normal_index * inv_nr_normals);
200 ref_normals[normal_index].x = std::cos (angle) * ref_normals[0].x - sinf (angle) * ref_normals[0].y;
201 ref_normals[normal_index].y = sinf (angle) * ref_normals[0].x + std::cos (angle) * ref_normals[0].y;
202 ref_normals[normal_index].z = ref_normals[0].z;
206 for (
int normal_index = 0; normal_index < nr_normals; ++normal_index)
208 const float length = std::sqrt (ref_normals[normal_index].x * ref_normals[normal_index].x +
209 ref_normals[normal_index].y * ref_normals[normal_index].y +
210 ref_normals[normal_index].z * ref_normals[normal_index].z);
212 const float inv_length = 1.0f / length;
214 ref_normals[normal_index].x *= inv_length;
215 ref_normals[normal_index].y *= inv_length;
216 ref_normals[normal_index].z *= inv_length;
220 for (
int z_index = 0; z_index <
size_z; ++z_index)
222 for (
int y_index = 0; y_index <
size_y; ++y_index)
224 for (
int x_index = 0; x_index <
size_x; ++x_index)
227 static_cast<float> (y_index -
range_y/2),
228 static_cast<float> (z_index -
range_z));
229 const float length = std::sqrt (normal.x*normal.x + normal.y*normal.y + normal.z*normal.z);
230 const float inv_length = 1.0f / (length + 0.00001f);
232 normal.x *= inv_length;
233 normal.y *= inv_length;
234 normal.z *= inv_length;
236 float max_response = -1.0f;
239 for (
int normal_index = 0; normal_index < nr_normals; ++normal_index)
241 const float response = normal.x * ref_normals[normal_index].x +
242 normal.y * ref_normals[normal_index].y +
243 normal.z * ref_normals[normal_index].z;
245 const float abs_response = std::abs (response);
246 if (max_response < abs_response)
248 max_response = abs_response;
249 max_index = normal_index;
252 lut[z_index*
size_y*
size_x + y_index*
size_x + x_index] = static_cast<unsigned char> (0x1 << max_index);
265 operator() (
const float x,
const float y,
const float z)
const
267 const std::size_t x_index = static_cast<std::size_t> (x * static_cast<float> (
offset_x) + static_cast<float> (
offset_x));
268 const std::size_t y_index = static_cast<std::size_t> (y * static_cast<float> (
offset_y) + static_cast<float> (
offset_y));
269 const std::size_t z_index = static_cast<std::size_t> (z * static_cast<float> (
range_z) + static_cast<float> (
range_z));
290 template <
typename Po
intInT>
339 spreading_size_ = spreading_size;
348 variable_feature_nr_ = enabled;
355 return surface_normals_;
362 return surface_normals_;
369 return (filtered_quantized_surface_normals_);
376 return (spreaded_quantized_surface_normals_);
383 return (surface_normal_orientations_);
395 std::vector<QuantizedMultiModFeature> & features)
const override;
405 std::vector<QuantizedMultiModFeature> & features)
const override;
457 bool variable_feature_nr_;
460 float feature_distance_threshold_;
462 float min_distance_to_border_;
468 std::size_t spreading_size_;
487 template <
typename Po
intInT>
490 : variable_feature_nr_ (false)
491 , feature_distance_threshold_ (2.0f)
492 , min_distance_to_border_ (2.0f)
493 , spreading_size_ (8)
498 template <
typename Po
intInT>
504 template <
typename Po
intInT>
void
513 computeAndQuantizeSurfaceNormals2 ();
516 filterQuantizedSurfaceNormals ();
520 spreaded_quantized_surface_normals_,
525 template <
typename Po
intInT>
void
530 spreaded_quantized_surface_normals_,
535 template <
typename Po
intInT>
void
548 template <
typename Po
intInT>
void
560 const float bad_point = std::numeric_limits<float>::quiet_NaN ();
562 const int width = input_->width;
563 const int height = input_->height;
565 surface_normals_.resize (width*height);
566 surface_normals_.width = width;
567 surface_normals_.height = height;
568 surface_normals_.is_dense =
false;
570 quantized_surface_normals_.resize (width, height);
584 for (
int y = 0; y < height; ++y)
586 for (
int x = 0; x < width; ++x)
588 const int index = y * width + x;
590 const float px = input_->points[index].x;
591 const float py = input_->points[index].y;
592 const float pz = input_->points[index].z;
594 if (std::isnan(px) || pz > 2.0f)
596 surface_normals_.points[index].normal_x = bad_point;
597 surface_normals_.points[index].normal_y = bad_point;
598 surface_normals_.points[index].normal_z = bad_point;
599 surface_normals_.points[index].curvature = bad_point;
601 quantized_surface_normals_ (x, y) = 0;
606 const int smoothingSizeInt = 5;
615 for (
int v = y - smoothingSizeInt; v <= y + smoothingSizeInt; v += smoothingSizeInt)
617 for (
int u = x - smoothingSizeInt; u <= x + smoothingSizeInt; u += smoothingSizeInt)
619 if (u < 0 || u >= width || v < 0 || v >= height)
continue;
621 const std::size_t index2 = v * width + u;
623 const float qx = input_->points[index2].x;
624 const float qy = input_->points[index2].y;
625 const float qz = input_->points[index2].z;
627 if (std::isnan(qx))
continue;
629 const float delta = qz - pz;
630 const float i = qx - px;
631 const float j = qy - py;
633 const float f = std::abs(delta) < 0.05f ? 1.0f : 0.0f;
638 vecb0 += f * i * delta;
639 vecb1 += f * j * delta;
643 const float det = matA0 * matA3 - matA1 * matA1;
644 const float ddx = matA3 * vecb0 - matA1 * vecb1;
645 const float ddy = -matA1 * vecb0 + matA0 * vecb1;
647 const float nx = ddx;
648 const float ny = ddy;
649 const float nz = -det * pz;
651 const float length = nx * nx + ny * ny + nz * nz;
655 surface_normals_.points[index].normal_x = bad_point;
656 surface_normals_.points[index].normal_y = bad_point;
657 surface_normals_.points[index].normal_z = bad_point;
658 surface_normals_.points[index].curvature = bad_point;
660 quantized_surface_normals_ (x, y) = 0;
664 const float normInv = 1.0f / std::sqrt (length);
666 const float normal_x = nx * normInv;
667 const float normal_y = ny * normInv;
668 const float normal_z = nz * normInv;
670 surface_normals_.points[index].normal_x = normal_x;
671 surface_normals_.points[index].normal_y = normal_y;
672 surface_normals_.points[index].normal_z = normal_z;
673 surface_normals_.points[index].curvature = bad_point;
675 float angle = 11.25f + std::atan2 (normal_y, normal_x)*180.0f/3.14f;
677 if (angle < 0.0f) angle += 360.0f;
678 if (angle >= 360.0f) angle -= 360.0f;
680 int bin_index = static_cast<int> (angle*8.0f/360.0f) & 7;
682 quantized_surface_normals_ (x, y) = static_cast<unsigned char> (bin_index);
693 static void accumBilateral(
long delta,
long i,
long j,
long * A,
long * b,
int threshold)
695 long f = std::abs(delta) < threshold ? 1 : 0;
697 const long fi = f * i;
698 const long fj = f * j;
714 template <
typename Po
intInT>
void
717 const int width = input_->width;
718 const int height = input_->height;
720 unsigned short * lp_depth =
new unsigned short[width*height];
721 unsigned char * lp_normals =
new unsigned char[width*height];
722 memset (lp_normals, 0, width*height);
724 surface_normal_orientations_.resize (width, height, 0.0f);
726 for (
int row_index = 0; row_index < height; ++row_index)
728 for (
int col_index = 0; col_index < width; ++col_index)
730 const float value = input_->points[row_index*width + col_index].z;
731 if (std::isfinite (value))
733 lp_depth[row_index*width + col_index] = static_cast<unsigned short> (value * 1000.0f);
737 lp_depth[row_index*width + col_index] = 0;
742 const int l_W = width;
743 const int l_H = height;
755 const int offsets_i[] = {-l_r, 0, l_r, -l_r, l_r, -l_r, 0, l_r};
756 const int offsets_j[] = {-l_r, -l_r, -l_r, 0, 0, l_r, l_r, l_r};
757 const int offsets[] = { offsets_i[0] + offsets_j[0] * l_W
758 , offsets_i[1] + offsets_j[1] * l_W
759 , offsets_i[2] + offsets_j[2] * l_W
760 , offsets_i[3] + offsets_j[3] * l_W
761 , offsets_i[4] + offsets_j[4] * l_W
762 , offsets_i[5] + offsets_j[5] * l_W
763 , offsets_i[6] + offsets_j[6] * l_W
764 , offsets_i[7] + offsets_j[7] * l_W };
770 const int difference_threshold = 50;
771 const int distance_threshold = 2000;
777 for (
int l_y = l_r; l_y < l_H - l_r - 1; ++l_y)
779 unsigned short * lp_line = lp_depth + (l_y * l_W + l_r);
780 unsigned char * lp_norm = lp_normals + (l_y * l_W + l_r);
782 for (
int l_x = l_r; l_x < l_W - l_r - 1; ++l_x)
784 long l_d = lp_line[0];
789 if (l_d < distance_threshold)
792 long l_A[4]; l_A[0] = l_A[1] = l_A[2] = l_A[3] = 0;
793 long l_b[2]; l_b[0] = l_b[1] = 0;
797 accumBilateral(lp_line[offsets[0]] - l_d, offsets_i[0], offsets_j[0], l_A, l_b, difference_threshold);
798 accumBilateral(lp_line[offsets[1]] - l_d, offsets_i[1], offsets_j[1], l_A, l_b, difference_threshold);
799 accumBilateral(lp_line[offsets[2]] - l_d, offsets_i[2], offsets_j[2], l_A, l_b, difference_threshold);
800 accumBilateral(lp_line[offsets[3]] - l_d, offsets_i[3], offsets_j[3], l_A, l_b, difference_threshold);
801 accumBilateral(lp_line[offsets[4]] - l_d, offsets_i[4], offsets_j[4], l_A, l_b, difference_threshold);
802 accumBilateral(lp_line[offsets[5]] - l_d, offsets_i[5], offsets_j[5], l_A, l_b, difference_threshold);
803 accumBilateral(lp_line[offsets[6]] - l_d, offsets_i[6], offsets_j[6], l_A, l_b, difference_threshold);
804 accumBilateral(lp_line[offsets[7]] - l_d, offsets_i[7], offsets_j[7], l_A, l_b, difference_threshold);
863 long l_det = l_A[0] * l_A[3] - l_A[1] * l_A[1];
864 long l_ddx = l_A[3] * l_b[0] - l_A[1] * l_b[1];
865 long l_ddy = -l_A[1] * l_b[0] + l_A[0] * l_b[1];
869 float l_nx = static_cast<float>(1150 * l_ddx);
870 float l_ny = static_cast<float>(1150 * l_ddy);
871 float l_nz = static_cast<float>(-l_det * l_d);
885 float l_sqrt = std::sqrt (l_nx * l_nx + l_ny * l_ny + l_nz * l_nz);
889 float l_norminv = 1.0f / (l_sqrt);
895 float angle = 22.5f + std::atan2 (l_ny, l_nx) * 180.0f / 3.14f;
897 if (angle < 0.0f) angle += 360.0f;
898 if (angle >= 360.0f) angle -= 360.0f;
900 int bin_index = static_cast<int> (angle*8.0f/360.0f) & 7;
902 surface_normal_orientations_ (l_x, l_y) = angle;
911 *lp_norm = static_cast<unsigned char> (0x1 << bin_index);
928 unsigned char map[255];
940 quantized_surface_normals_.resize (width, height);
941 for (
int row_index = 0; row_index < height; ++row_index)
943 for (
int col_index = 0; col_index < width; ++col_index)
945 quantized_surface_normals_ (col_index, row_index) = map[lp_normals[row_index*width + col_index]];
955 template <
typename Po
intInT>
void
957 const std::size_t nr_features,
958 const std::size_t modality_index,
959 std::vector<QuantizedMultiModFeature> & features)
const
961 const std::size_t width = mask.
getWidth ();
962 const std::size_t height = mask.
getHeight ();
975 for (
auto &mask_map : mask_maps)
976 mask_map.
resize (width, height);
978 unsigned char map[255];
993 for (std::size_t row_index = 0; row_index < height; ++row_index)
995 for (std::size_t col_index = 0; col_index < width; ++col_index)
997 if (mask (col_index, row_index) != 0)
1000 const unsigned char quantized_value = filtered_quantized_surface_normals_ (col_index, row_index);
1002 if (quantized_value == 0)
1004 const int dist_map_index = map[quantized_value];
1006 distance_map_indices (col_index, row_index) = static_cast<unsigned char> (dist_map_index);
1008 mask_maps[dist_map_index] (col_index, row_index) = 255;
1014 for (
int map_index = 0; map_index < 8; ++map_index)
1015 computeDistanceMap (mask_maps[map_index], distance_maps[map_index]);
1018 computeDistanceMap (mask, mask_distance_maps);
1020 std::list<Candidate> list1;
1021 std::list<Candidate> list2;
1023 float weights[8] = {0,0,0,0,0,0,0,0};
1025 const std::size_t off = 4;
1026 for (std::size_t row_index = off; row_index < height-off; ++row_index)
1028 for (std::size_t col_index = off; col_index < width-off; ++col_index)
1030 if (mask (col_index, row_index) != 0)
1033 const unsigned char quantized_value = filtered_quantized_surface_normals_ (col_index, row_index);
1039 if (quantized_value != 0)
1041 const int distance_map_index = map[quantized_value];
1044 const float distance = distance_maps[distance_map_index] (col_index, row_index);
1045 const float distance_to_border = mask_distance_maps (col_index, row_index);
1047 if (
distance >= feature_distance_threshold_ && distance_to_border >= min_distance_to_border_)
1049 Candidate candidate;
1052 candidate.x = col_index;
1053 candidate.y = row_index;
1054 candidate.bin_index = static_cast<unsigned char> (distance_map_index);
1056 list1.push_back (candidate);
1058 ++weights[distance_map_index];
1065 for (
typename std::list<Candidate>::iterator iter = list1.begin (); iter != list1.end (); ++iter)
1066 iter->distance *= 1.0f / weights[iter->bin_index];
1070 if (variable_feature_nr_)
1072 int distance = static_cast<int> (list1.size ());
1073 bool feature_selection_finished =
false;
1074 while (!feature_selection_finished)
1077 for (
typename std::list<Candidate>::iterator iter1 = list1.begin (); iter1 != list1.end (); ++iter1)
1079 bool candidate_accepted =
true;
1080 for (
typename std::list<Candidate>::iterator iter2 = list2.begin (); iter2 != list2.end (); ++iter2)
1082 const int dx = static_cast<int> (iter1->x) - static_cast<int> (iter2->x);
1083 const int dy = static_cast<int> (iter1->y) - static_cast<int> (iter2->y);
1084 const int tmp_distance = dx*dx + dy*dy;
1086 if (tmp_distance < sqr_distance)
1088 candidate_accepted =
false;
1094 float min_min_sqr_distance = std::numeric_limits<float>::max ();
1095 float max_min_sqr_distance = 0;
1096 for (
typename std::list<Candidate>::iterator iter2 = list2.begin (); iter2 != list2.end (); ++iter2)
1098 float min_sqr_distance = std::numeric_limits<float>::max ();
1099 for (
typename std::list<Candidate>::iterator iter3 = list2.begin (); iter3 != list2.end (); ++iter3)
1104 const float dx = static_cast<float> (iter2->x) - static_cast<float> (iter3->x);
1105 const float dy = static_cast<float> (iter2->y) - static_cast<float> (iter3->y);
1107 const float sqr_distance = dx*dx + dy*dy;
1109 if (sqr_distance < min_sqr_distance)
1111 min_sqr_distance = sqr_distance;
1120 const float dx = static_cast<float> (iter2->x) - static_cast<float> (iter1->x);
1121 const float dy = static_cast<float> (iter2->y) - static_cast<float> (iter1->y);
1123 const float sqr_distance = dx*dx + dy*dy;
1125 if (sqr_distance < min_sqr_distance)
1127 min_sqr_distance = sqr_distance;
1131 if (min_sqr_distance < min_min_sqr_distance)
1132 min_min_sqr_distance = min_sqr_distance;
1133 if (min_sqr_distance > max_min_sqr_distance)
1134 max_min_sqr_distance = min_sqr_distance;
1139 if (candidate_accepted)
1145 if (min_min_sqr_distance < 50)
1147 feature_selection_finished =
true;
1151 list2.push_back (*iter1);
1165 if (list1.size () <= nr_features)
1167 features.reserve (list1.size ());
1168 for (
typename std::list<Candidate>::iterator iter = list1.begin (); iter != list1.end (); ++iter)
1172 feature.
x = static_cast<int> (iter->x);
1173 feature.
y = static_cast<int> (iter->y);
1175 feature.
quantized_value = filtered_quantized_surface_normals_ (iter->x, iter->y);
1177 features.push_back (feature);
1183 int distance = static_cast<int> (list1.size () / nr_features + 1);
1184 while (list2.size () != nr_features)
1187 for (
typename std::list<Candidate>::iterator iter1 = list1.begin (); iter1 != list1.end (); ++iter1)
1189 bool candidate_accepted =
true;
1191 for (
typename std::list<Candidate>::iterator iter2 = list2.begin (); iter2 != list2.end (); ++iter2)
1193 const int dx = static_cast<int> (iter1->x) - static_cast<int> (iter2->x);
1194 const int dy = static_cast<int> (iter1->y) - static_cast<int> (iter2->y);
1195 const int tmp_distance = dx*dx + dy*dy;
1197 if (tmp_distance < sqr_distance)
1199 candidate_accepted =
false;
1204 if (candidate_accepted)
1205 list2.push_back (*iter1);
1207 if (list2.size () == nr_features)
break;
1213 for (
typename std::list<Candidate>::iterator iter2 = list2.begin (); iter2 != list2.end (); ++iter2)
1217 feature.
x = static_cast<int> (iter2->x);
1218 feature.
y = static_cast<int> (iter2->y);
1220 feature.
quantized_value = filtered_quantized_surface_normals_ (iter2->x, iter2->y);
1222 features.push_back (feature);
1227 template <
typename Po
intInT>
void
1229 const MaskMap & mask,
const std::size_t,
const std::size_t modality_index,
1230 std::vector<QuantizedMultiModFeature> & features)
const
1232 const std::size_t width = mask.
getWidth ();
1233 const std::size_t height = mask.
getHeight ();
1246 for (
auto &mask_map : mask_maps)
1247 mask_map.
resize (width, height);
1249 unsigned char map[255];
1250 memset(map, 0, 255);
1264 for (std::size_t row_index = 0; row_index < height; ++row_index)
1266 for (std::size_t col_index = 0; col_index < width; ++col_index)
1268 if (mask (col_index, row_index) != 0)
1271 const unsigned char quantized_value = filtered_quantized_surface_normals_ (col_index, row_index);
1273 if (quantized_value == 0)
1275 const int dist_map_index = map[quantized_value];
1277 distance_map_indices (col_index, row_index) = static_cast<unsigned char> (dist_map_index);
1279 mask_maps[dist_map_index] (col_index, row_index) = 255;
1285 for (
int map_index = 0; map_index < 8; ++map_index)
1286 computeDistanceMap (mask_maps[map_index], distance_maps[map_index]);
1289 computeDistanceMap (mask, mask_distance_maps);
1291 std::list<Candidate> list1;
1292 std::list<Candidate> list2;
1294 float weights[8] = {0,0,0,0,0,0,0,0};
1296 const std::size_t off = 4;
1297 for (std::size_t row_index = off; row_index < height-off; ++row_index)
1299 for (std::size_t col_index = off; col_index < width-off; ++col_index)
1301 if (mask (col_index, row_index) != 0)
1304 const unsigned char quantized_value = filtered_quantized_surface_normals_ (col_index, row_index);
1310 if (quantized_value != 0)
1312 const int distance_map_index = map[quantized_value];
1315 const float distance = distance_maps[distance_map_index] (col_index, row_index);
1316 const float distance_to_border = mask_distance_maps (col_index, row_index);
1318 if (
distance >= feature_distance_threshold_ && distance_to_border >= min_distance_to_border_)
1320 Candidate candidate;
1323 candidate.x = col_index;
1324 candidate.y = row_index;
1325 candidate.bin_index = static_cast<unsigned char> (distance_map_index);
1327 list1.push_back (candidate);
1329 ++weights[distance_map_index];
1336 for (
typename std::list<Candidate>::iterator iter = list1.begin (); iter != list1.end (); ++iter)
1337 iter->distance *= 1.0f / weights[iter->bin_index];
1341 features.reserve (list1.size ());
1342 for (
typename std::list<Candidate>::iterator iter = list1.begin (); iter != list1.end (); ++iter)
1346 feature.
x = static_cast<int> (iter->x);
1347 feature.
y = static_cast<int> (iter->y);
1349 feature.
quantized_value = filtered_quantized_surface_normals_ (iter->x, iter->y);
1351 features.push_back (feature);
1356 template <
typename Po
intInT>
void
1359 const std::size_t width = input_->width;
1360 const std::size_t height = input_->height;
1362 quantized_surface_normals_.resize (width, height);
1364 for (std::size_t row_index = 0; row_index < height; ++row_index)
1366 for (std::size_t col_index = 0; col_index < width; ++col_index)
1368 const float normal_x = surface_normals_ (col_index, row_index).normal_x;
1369 const float normal_y = surface_normals_ (col_index, row_index).normal_y;
1370 const float normal_z = surface_normals_ (col_index, row_index).normal_z;
1372 if (std::isnan(normal_x) || std::isnan(normal_y) || std::isnan(normal_z) || normal_z > 0)
1374 quantized_surface_normals_ (col_index, row_index) = 0;
1381 float angle = 11.25f + std::atan2 (normal_y, normal_x)*180.0f/3.14f;
1383 if (angle < 0.0f) angle += 360.0f;
1384 if (angle >= 360.0f) angle -= 360.0f;
1386 int bin_index = static_cast<int> (angle*8.0f/360.0f);
1389 quantized_surface_normals_ (col_index, row_index) = static_cast<unsigned char> (bin_index);
1397 template <
typename Po
intInT>
void
1400 const int width = input_->width;
1401 const int height = input_->height;
1403 filtered_quantized_surface_normals_.resize (width, height);
1460 for (
int row_index = 2; row_index < height-2; ++row_index)
1462 for (
int col_index = 2; col_index < width-2; ++col_index)
1464 unsigned char histogram[9] = {0,0,0,0,0,0,0,0,0};
1486 unsigned char * dataPtr = quantized_surface_normals_.getData () + (row_index-2)*width+col_index-2;
1487 ++histogram[dataPtr[0]];
1488 ++histogram[dataPtr[1]];
1489 ++histogram[dataPtr[2]];
1490 ++histogram[dataPtr[3]];
1491 ++histogram[dataPtr[4]];
1494 unsigned char * dataPtr = quantized_surface_normals_.getData () + (row_index-1)*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)*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+1)*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+2)*width+col_index-2;
1519 ++histogram[dataPtr[0]];
1520 ++histogram[dataPtr[1]];
1521 ++histogram[dataPtr[2]];
1522 ++histogram[dataPtr[3]];
1523 ++histogram[dataPtr[4]];
1527 unsigned char max_hist_value = 0;
1528 int max_hist_index = -1;
1530 if (max_hist_value < histogram[1]) {max_hist_index = 0; max_hist_value = histogram[1];}
1531 if (max_hist_value < histogram[2]) {max_hist_index = 1; max_hist_value = histogram[2];}
1532 if (max_hist_value < histogram[3]) {max_hist_index = 2; max_hist_value = histogram[3];}
1533 if (max_hist_value < histogram[4]) {max_hist_index = 3; max_hist_value = histogram[4];}
1534 if (max_hist_value < histogram[5]) {max_hist_index = 4; max_hist_value = histogram[5];}
1535 if (max_hist_value < histogram[6]) {max_hist_index = 5; max_hist_value = histogram[6];}
1536 if (max_hist_value < histogram[7]) {max_hist_index = 6; max_hist_value = histogram[7];}
1537 if (max_hist_value < histogram[8]) {max_hist_index = 7; max_hist_value = histogram[8];}
1539 if (max_hist_index != -1 && max_hist_value >= 1)
1541 filtered_quantized_surface_normals_ (col_index, row_index) = static_cast<unsigned char> (0x1 << max_hist_index);
1545 filtered_quantized_surface_normals_ (col_index, row_index) = 0;
1569 template <
typename Po
intInT>
void
1572 const std::size_t width = input.
getWidth ();
1573 const std::size_t height = input.
getHeight ();
1575 output.
resize (width, height);
1579 const unsigned char * mask_map = input.
getData ();
1580 float * distance_map = output.
getData ();
1581 for (std::size_t index = 0; index < width*height; ++index)
1583 if (mask_map[index] == 0)
1584 distance_map[index] = 0.0f;
1586 distance_map[index] = static_cast<float> (width + height);
1590 float * previous_row = distance_map;
1591 float * current_row = previous_row + width;
1592 for (std::size_t ri = 1; ri < height; ++ri)
1594 for (std::size_t ci = 1; ci < width; ++ci)
1596 const float up_left = previous_row [ci - 1] + 1.4f;
1597 const float up = previous_row [ci] + 1.0f;
1598 const float up_right = previous_row [ci + 1] + 1.4f;
1599 const float left = current_row [ci - 1] + 1.0f;
1600 const float center = current_row [ci];
1602 const float min_value = std::min (std::min (up_left, up), std::min (left, up_right));
1604 if (min_value < center)
1605 current_row[ci] = min_value;
1607 previous_row = current_row;
1608 current_row += width;
1612 float * next_row = distance_map + width * (height - 1);
1613 current_row = next_row - width;
1614 for (
int ri = static_cast<int> (height)-2; ri >= 0; --ri)
1616 for (
int ci = static_cast<int> (width)-2; ci >= 0; --ci)
1618 const float lower_left = next_row [ci - 1] + 1.4f;
1619 const float lower = next_row [ci] + 1.0f;
1620 const float lower_right = next_row [ci + 1] + 1.4f;
1621 const float right = current_row [ci + 1] + 1.0f;
1622 const float center = current_row [ci];
1624 const float min_value = std::min (std::min (lower_left, lower), std::min (right, lower_right));
1626 if (min_value < center)
1627 current_row[ci] = min_value;
1629 next_row = current_row;
1630 current_row -= width;