38 #ifndef PCL_RECOGNITION_SURFACE_NORMAL_MODALITY
39 #define PCL_RECOGNITION_SURFACE_NORMAL_MODALITY
41 #include <pcl/recognition/quantizable_modality.h>
42 #include <pcl/recognition/distance_map.h>
44 #include <pcl/pcl_base.h>
45 #include <pcl/point_cloud.h>
46 #include <pcl/point_types.h>
47 #include <pcl/features/linear_least_squares_normal.h>
84 resize (
const size_t width,
const size_t height,
const float value)
89 map_.resize (width*height, value);
97 operator() (
const size_t col_index,
const size_t row_index)
99 return map_[row_index * width_ + col_index];
107 operator() (
const size_t col_index,
const size_t row_index)
const
109 return map_[row_index * width_ + col_index];
118 std::vector<float> map_;
171 initializeLUT (
const int range_x_arg,
const int range_y_arg,
const int range_z_arg)
190 const int nr_normals = 8;
193 const float normal0_angle = 40.0f * 3.14f / 180.0f;
194 ref_normals[0].x = cosf (normal0_angle);
195 ref_normals[0].y = 0.0f;
196 ref_normals[0].z = -sinf (normal0_angle);
198 const float inv_nr_normals = 1.0f /
static_cast<float> (nr_normals);
199 for (
int normal_index = 1; normal_index < nr_normals; ++normal_index)
201 const float angle = 2.0f *
static_cast<float> (M_PI * normal_index * inv_nr_normals);
203 ref_normals[normal_index].x = cosf (angle) * ref_normals[0].x - sinf (angle) * ref_normals[0].y;
204 ref_normals[normal_index].y = sinf (angle) * ref_normals[0].x + cosf (angle) * ref_normals[0].y;
205 ref_normals[normal_index].z = ref_normals[0].z;
209 for (
int normal_index = 0; normal_index < nr_normals; ++normal_index)
211 const float length = sqrtf (ref_normals[normal_index].x * ref_normals[normal_index].x +
212 ref_normals[normal_index].y * ref_normals[normal_index].y +
213 ref_normals[normal_index].z * ref_normals[normal_index].z);
215 const float inv_length = 1.0f / length;
217 ref_normals[normal_index].x *= inv_length;
218 ref_normals[normal_index].y *= inv_length;
219 ref_normals[normal_index].z *= inv_length;
223 for (
int z_index = 0; z_index <
size_z; ++z_index)
225 for (
int y_index = 0; y_index <
size_y; ++y_index)
227 for (
int x_index = 0; x_index <
size_x; ++x_index)
230 static_cast<float> (y_index -
range_y/2),
231 static_cast<float> (z_index -
range_z));
232 const float length = sqrtf (normal.x*normal.x + normal.y*normal.y + normal.z*normal.z);
233 const float inv_length = 1.0f / (length + 0.00001f);
235 normal.x *= inv_length;
236 normal.y *= inv_length;
237 normal.z *= inv_length;
239 float max_response = -1.0f;
242 for (
int normal_index = 0; normal_index < nr_normals; ++normal_index)
244 const float response = normal.x * ref_normals[normal_index].x +
245 normal.y * ref_normals[normal_index].y +
246 normal.z * ref_normals[normal_index].z;
248 const float abs_response = fabsf (response);
249 if (max_response < abs_response)
251 max_response = abs_response;
252 max_index = normal_index;
255 lut[z_index*size_y*size_x + y_index*size_x + x_index] =
static_cast<unsigned char> (0x1 << max_index);
268 operator() (
const float x,
const float y,
const float z)
const
270 const size_t x_index =
static_cast<size_t> (x *
static_cast<float> (
offset_x) + static_cast<float> (
offset_x));
271 const size_t y_index =
static_cast<size_t> (y *
static_cast<float> (
offset_y) + static_cast<float> (
offset_y));
272 const size_t z_index =
static_cast<size_t> (z *
static_cast<float> (
range_z) + static_cast<float> (
range_z));
293 template <
typename Po
intInT>
342 spreading_size_ = spreading_size;
351 variable_feature_nr_ = enabled;
358 return surface_normals_;
365 return surface_normals_;
372 return (filtered_quantized_surface_normals_);
379 return (spreaded_quantized_surface_normals_);
386 return (surface_normal_orientations_);
398 std::vector<QuantizedMultiModFeature> & features)
const;
408 std::vector<QuantizedMultiModFeature> & features)
const;
460 bool variable_feature_nr_;
463 float feature_distance_threshold_;
465 float min_distance_to_border_;
471 size_t spreading_size_;
490 template <
typename Po
intInT>
493 : variable_feature_nr_ (false)
494 , feature_distance_threshold_ (2.0f)
495 , min_distance_to_border_ (2.0f)
497 , spreading_size_ (8)
498 , surface_normals_ ()
499 , quantized_surface_normals_ ()
500 , filtered_quantized_surface_normals_ ()
501 , spreaded_quantized_surface_normals_ ()
502 , surface_normal_orientations_ ()
507 template <
typename Po
intInT>
513 template <
typename Po
intInT>
void
522 computeAndQuantizeSurfaceNormals2 ();
525 filterQuantizedSurfaceNormals ();
529 spreaded_quantized_surface_normals_,
534 template <
typename Po
intInT>
void
539 spreaded_quantized_surface_normals_,
544 template <
typename Po
intInT>
void
557 template <
typename Po
intInT>
void
569 const float bad_point = std::numeric_limits<float>::quiet_NaN ();
571 const int width = input_->width;
572 const int height = input_->height;
574 surface_normals_.resize (width*height);
575 surface_normals_.width = width;
576 surface_normals_.height = height;
577 surface_normals_.is_dense =
false;
579 quantized_surface_normals_.resize (width, height);
593 for (
int y = 0; y < height; ++y)
595 for (
int x = 0; x < width; ++x)
597 const int index = y * width + x;
599 const float px = input_->points[index].x;
600 const float py = input_->points[index].y;
601 const float pz = input_->points[index].z;
603 if (pcl_isnan(px) || pz > 2.0f)
605 surface_normals_.points[index].normal_x = bad_point;
606 surface_normals_.points[index].normal_y = bad_point;
607 surface_normals_.points[index].normal_z = bad_point;
608 surface_normals_.points[index].curvature = bad_point;
610 quantized_surface_normals_ (x, y) = 0;
615 const int smoothingSizeInt = 5;
624 for (
int v = y - smoothingSizeInt; v <= y + smoothingSizeInt; v += smoothingSizeInt)
626 for (
int u = x - smoothingSizeInt; u <= x + smoothingSizeInt; u += smoothingSizeInt)
628 if (u < 0 || u >= width || v < 0 || v >= height)
continue;
630 const size_t index2 = v * width + u;
632 const float qx = input_->points[index2].x;
633 const float qy = input_->points[index2].y;
634 const float qz = input_->points[index2].z;
636 if (pcl_isnan(qx))
continue;
638 const float delta = qz - pz;
639 const float i = qx - px;
640 const float j = qy - py;
642 const float f = fabs(delta) < 0.05f ? 1.0f : 0.0f;
647 vecb0 += f * i * delta;
648 vecb1 += f * j * delta;
652 const float det = matA0 * matA3 - matA1 * matA1;
653 const float ddx = matA3 * vecb0 - matA1 * vecb1;
654 const float ddy = -matA1 * vecb0 + matA0 * vecb1;
656 const float nx = ddx;
657 const float ny = ddy;
658 const float nz = -det * pz;
660 const float length = nx * nx + ny * ny + nz * nz;
664 surface_normals_.points[index].normal_x = bad_point;
665 surface_normals_.points[index].normal_y = bad_point;
666 surface_normals_.points[index].normal_z = bad_point;
667 surface_normals_.points[index].curvature = bad_point;
669 quantized_surface_normals_ (x, y) = 0;
673 const float normInv = 1.0f / sqrtf (length);
675 const float normal_x = nx * normInv;
676 const float normal_y = ny * normInv;
677 const float normal_z = nz * normInv;
679 surface_normals_.points[index].normal_x = normal_x;
680 surface_normals_.points[index].normal_y = normal_y;
681 surface_normals_.points[index].normal_z = normal_z;
682 surface_normals_.points[index].curvature = bad_point;
684 float angle = 11.25f + atan2 (normal_y, normal_x)*180.0f/3.14f;
686 if (angle < 0.0f) angle += 360.0f;
687 if (angle >= 360.0f) angle -= 360.0f;
689 int bin_index =
static_cast<int> (angle*8.0f/360.0f) & 7;
691 quantized_surface_normals_ (x, y) =
static_cast<unsigned char> (bin_index);
702 static void accumBilateral(
long delta,
long i,
long j,
long * A,
long * b,
int threshold)
704 long f = std::abs(delta) < threshold ? 1 : 0;
706 const long fi = f * i;
707 const long fj = f * j;
730 template <
typename Po
intInT>
void
733 const int width = input_->width;
734 const int height = input_->height;
736 unsigned short * lp_depth =
new unsigned short[width*height];
737 unsigned char * lp_normals =
new unsigned char[width*height];
738 memset (lp_normals, 0, width*height);
740 surface_normal_orientations_.resize (width, height, 0.0f);
742 for (
size_t row_index = 0; row_index < height; ++row_index)
744 for (
size_t col_index = 0; col_index < width; ++col_index)
746 const float value = input_->points[row_index*width + col_index].z;
747 if (pcl_isfinite (value))
749 lp_depth[row_index*width + col_index] =
static_cast<unsigned short> (value * 1000.0f);
753 lp_depth[row_index*width + col_index] = 0;
758 const int l_W = width;
759 const int l_H = height;
771 const int offsets_i[] = {-l_r, 0, l_r, -l_r, l_r, -l_r, 0, l_r};
772 const int offsets_j[] = {-l_r, -l_r, -l_r, 0, 0, l_r, l_r, l_r};
773 const int offsets[] = { offsets_i[0] + offsets_j[0] * l_W
774 , offsets_i[1] + offsets_j[1] * l_W
775 , offsets_i[2] + offsets_j[2] * l_W
776 , offsets_i[3] + offsets_j[3] * l_W
777 , offsets_i[4] + offsets_j[4] * l_W
778 , offsets_i[5] + offsets_j[5] * l_W
779 , offsets_i[6] + offsets_j[6] * l_W
780 , offsets_i[7] + offsets_j[7] * l_W };
786 const int difference_threshold = 50;
787 const int distance_threshold = 2000;
793 for (
int l_y = l_r; l_y < l_H - l_r - 1; ++l_y)
795 unsigned short * lp_line = lp_depth + (l_y * l_W + l_r);
796 unsigned char * lp_norm = lp_normals + (l_y * l_W + l_r);
798 for (
int l_x = l_r; l_x < l_W - l_r - 1; ++l_x)
800 long l_d = lp_line[0];
805 if (l_d < distance_threshold)
808 long l_A[4]; l_A[0] = l_A[1] = l_A[2] = l_A[3] = 0;
809 long l_b[2]; l_b[0] = l_b[1] = 0;
813 accumBilateral(lp_line[offsets[0]] - l_d, offsets_i[0], offsets_j[0], l_A, l_b, difference_threshold);
814 accumBilateral(lp_line[offsets[1]] - l_d, offsets_i[1], offsets_j[1], l_A, l_b, difference_threshold);
815 accumBilateral(lp_line[offsets[2]] - l_d, offsets_i[2], offsets_j[2], l_A, l_b, difference_threshold);
816 accumBilateral(lp_line[offsets[3]] - l_d, offsets_i[3], offsets_j[3], l_A, l_b, difference_threshold);
817 accumBilateral(lp_line[offsets[4]] - l_d, offsets_i[4], offsets_j[4], l_A, l_b, difference_threshold);
818 accumBilateral(lp_line[offsets[5]] - l_d, offsets_i[5], offsets_j[5], l_A, l_b, difference_threshold);
819 accumBilateral(lp_line[offsets[6]] - l_d, offsets_i[6], offsets_j[6], l_A, l_b, difference_threshold);
820 accumBilateral(lp_line[offsets[7]] - l_d, offsets_i[7], offsets_j[7], l_A, l_b, difference_threshold);
879 long l_det = l_A[0] * l_A[3] - l_A[1] * l_A[1];
880 long l_ddx = l_A[3] * l_b[0] - l_A[1] * l_b[1];
881 long l_ddy = -l_A[1] * l_b[0] + l_A[0] * l_b[1];
885 float l_nx =
static_cast<float>(1150 * l_ddx);
886 float l_ny =
static_cast<float>(1150 * l_ddy);
887 float l_nz =
static_cast<float>(-l_det * l_d);
901 float l_sqrt = sqrtf (l_nx * l_nx + l_ny * l_ny + l_nz * l_nz);
905 float l_norminv = 1.0f / (l_sqrt);
911 float angle = 22.5f + atan2f (l_ny, l_nx) * 180.0f / 3.14f;
913 if (angle < 0.0f) angle += 360.0f;
914 if (angle >= 360.0f) angle -= 360.0f;
916 int bin_index =
static_cast<int> (angle*8.0f/360.0f) & 7;
918 surface_normal_orientations_ (l_x, l_y) = angle;
927 *lp_norm =
static_cast<unsigned char> (0x1 << bin_index);
944 unsigned char map[255];
956 quantized_surface_normals_.resize (width, height);
957 for (
size_t row_index = 0; row_index < height; ++row_index)
959 for (
size_t col_index = 0; col_index < width; ++col_index)
961 quantized_surface_normals_ (col_index, row_index) = map[lp_normals[row_index*width + col_index]];
971 template <
typename Po
intInT>
void
973 const size_t nr_features,
974 const size_t modality_index,
975 std::vector<QuantizedMultiModFeature> & features)
const
977 const size_t width = mask.
getWidth ();
991 for (
size_t map_index = 0; map_index < 8; ++map_index)
992 mask_maps[map_index].resize (width, height);
994 unsigned char map[255];
1009 for (
size_t row_index = 0; row_index < height; ++row_index)
1011 for (
size_t col_index = 0; col_index < width; ++col_index)
1013 if (mask (col_index, row_index) != 0)
1016 const unsigned char quantized_value = filtered_quantized_surface_normals_ (col_index, row_index);
1018 if (quantized_value == 0)
1020 const int dist_map_index = map[quantized_value];
1022 distance_map_indices (col_index, row_index) =
static_cast<unsigned char> (dist_map_index);
1024 mask_maps[dist_map_index] (col_index, row_index) = 255;
1030 for (
int map_index = 0; map_index < 8; ++map_index)
1031 computeDistanceMap (mask_maps[map_index], distance_maps[map_index]);
1034 computeDistanceMap (mask, mask_distance_maps);
1036 std::list<Candidate> list1;
1037 std::list<Candidate> list2;
1039 float weights[8] = {0,0,0,0,0,0,0,0};
1041 const size_t off = 4;
1042 for (
size_t row_index = off; row_index < height-off; ++row_index)
1044 for (
size_t col_index = off; col_index < width-off; ++col_index)
1046 if (mask (col_index, row_index) != 0)
1049 const unsigned char quantized_value = filtered_quantized_surface_normals_ (col_index, row_index);
1055 if (quantized_value != 0)
1057 const int distance_map_index = map[quantized_value];
1060 const float distance = distance_maps[distance_map_index] (col_index, row_index);
1061 const float distance_to_border = mask_distance_maps (col_index, row_index);
1063 if (distance >= feature_distance_threshold_ && distance_to_border >= min_distance_to_border_)
1068 candidate.
x = col_index;
1069 candidate.
y = row_index;
1070 candidate.
bin_index =
static_cast<unsigned char> (distance_map_index);
1072 list1.push_back (candidate);
1074 ++weights[distance_map_index];
1081 for (
typename std::list<Candidate>::iterator iter = list1.begin (); iter != list1.end (); ++iter)
1082 iter->distance *= 1.0f / weights[iter->bin_index];
1086 if (variable_feature_nr_)
1088 int distance =
static_cast<int> (list1.size ());
1089 bool feature_selection_finished =
false;
1090 while (!feature_selection_finished)
1092 const int sqr_distance = distance*distance;
1093 for (
typename std::list<Candidate>::iterator iter1 = list1.begin (); iter1 != list1.end (); ++iter1)
1095 bool candidate_accepted =
true;
1096 for (
typename std::list<Candidate>::iterator iter2 = list2.begin (); iter2 != list2.end (); ++iter2)
1098 const int dx =
static_cast<int> (iter1->x) - static_cast<int> (iter2->x);
1099 const int dy =
static_cast<int> (iter1->y) - static_cast<int> (iter2->y);
1100 const int tmp_distance = dx*dx + dy*dy;
1102 if (tmp_distance < sqr_distance)
1104 candidate_accepted =
false;
1110 float min_min_sqr_distance = std::numeric_limits<float>::max ();
1111 float max_min_sqr_distance = 0;
1112 for (
typename std::list<Candidate>::iterator iter2 = list2.begin (); iter2 != list2.end (); ++iter2)
1114 float min_sqr_distance = std::numeric_limits<float>::max ();
1115 for (
typename std::list<Candidate>::iterator iter3 = list2.begin (); iter3 != list2.end (); ++iter3)
1120 const float dx =
static_cast<float> (iter2->x) - static_cast<float> (iter3->x);
1121 const float dy =
static_cast<float> (iter2->y) - static_cast<float> (iter3->y);
1123 const float sqr_distance = dx*dx + dy*dy;
1125 if (sqr_distance < min_sqr_distance)
1127 min_sqr_distance = sqr_distance;
1136 const float dx =
static_cast<float> (iter2->x) - static_cast<float> (iter1->x);
1137 const float dy =
static_cast<float> (iter2->y) - static_cast<float> (iter1->y);
1139 const float sqr_distance = dx*dx + dy*dy;
1141 if (sqr_distance < min_sqr_distance)
1143 min_sqr_distance = sqr_distance;
1147 if (min_sqr_distance < min_min_sqr_distance)
1148 min_min_sqr_distance = min_sqr_distance;
1149 if (min_sqr_distance > max_min_sqr_distance)
1150 max_min_sqr_distance = min_sqr_distance;
1155 if (candidate_accepted)
1161 if (min_min_sqr_distance < 50)
1163 feature_selection_finished =
true;
1167 list2.push_back (*iter1);
1181 if (list1.size () <= nr_features)
1183 features.reserve (list1.size ());
1184 for (
typename std::list<Candidate>::iterator iter = list1.begin (); iter != list1.end (); ++iter)
1188 feature.
x =
static_cast<int> (iter->x);
1189 feature.
y =
static_cast<int> (iter->y);
1191 feature.
quantized_value = filtered_quantized_surface_normals_ (iter->x, iter->y);
1193 features.push_back (feature);
1199 int distance =
static_cast<int> (list1.size () / nr_features + 1);
1200 while (list2.size () != nr_features)
1202 const int sqr_distance = distance*distance;
1203 for (
typename std::list<Candidate>::iterator iter1 = list1.begin (); iter1 != list1.end (); ++iter1)
1205 bool candidate_accepted =
true;
1207 for (
typename std::list<Candidate>::iterator iter2 = list2.begin (); iter2 != list2.end (); ++iter2)
1209 const int dx =
static_cast<int> (iter1->x) - static_cast<int> (iter2->x);
1210 const int dy =
static_cast<int> (iter1->y) - static_cast<int> (iter2->y);
1211 const int tmp_distance = dx*dx + dy*dy;
1213 if (tmp_distance < sqr_distance)
1215 candidate_accepted =
false;
1220 if (candidate_accepted)
1221 list2.push_back (*iter1);
1223 if (list2.size () == nr_features)
break;
1229 for (
typename std::list<Candidate>::iterator iter2 = list2.begin (); iter2 != list2.end (); ++iter2)
1233 feature.
x =
static_cast<int> (iter2->x);
1234 feature.
y =
static_cast<int> (iter2->y);
1236 feature.
quantized_value = filtered_quantized_surface_normals_ (iter2->x, iter2->y);
1238 features.push_back (feature);
1243 template <
typename Po
intInT>
void
1245 const MaskMap & mask,
const size_t,
const size_t modality_index,
1246 std::vector<QuantizedMultiModFeature> & features)
const
1248 const size_t width = mask.
getWidth ();
1249 const size_t height = mask.
getHeight ();
1262 for (
size_t map_index = 0; map_index < 8; ++map_index)
1263 mask_maps[map_index].resize (width, height);
1265 unsigned char map[255];
1266 memset(map, 0, 255);
1280 for (
size_t row_index = 0; row_index < height; ++row_index)
1282 for (
size_t col_index = 0; col_index < width; ++col_index)
1284 if (mask (col_index, row_index) != 0)
1287 const unsigned char quantized_value = filtered_quantized_surface_normals_ (col_index, row_index);
1289 if (quantized_value == 0)
1291 const int dist_map_index = map[quantized_value];
1293 distance_map_indices (col_index, row_index) =
static_cast<unsigned char> (dist_map_index);
1295 mask_maps[dist_map_index] (col_index, row_index) = 255;
1301 for (
int map_index = 0; map_index < 8; ++map_index)
1302 computeDistanceMap (mask_maps[map_index], distance_maps[map_index]);
1305 computeDistanceMap (mask, mask_distance_maps);
1307 std::list<Candidate> list1;
1308 std::list<Candidate> list2;
1310 float weights[8] = {0,0,0,0,0,0,0,0};
1312 const size_t off = 4;
1313 for (
size_t row_index = off; row_index < height-off; ++row_index)
1315 for (
size_t col_index = off; col_index < width-off; ++col_index)
1317 if (mask (col_index, row_index) != 0)
1320 const unsigned char quantized_value = filtered_quantized_surface_normals_ (col_index, row_index);
1326 if (quantized_value != 0)
1328 const int distance_map_index = map[quantized_value];
1331 const float distance = distance_maps[distance_map_index] (col_index, row_index);
1332 const float distance_to_border = mask_distance_maps (col_index, row_index);
1334 if (distance >= feature_distance_threshold_ && distance_to_border >= min_distance_to_border_)
1339 candidate.
x = col_index;
1340 candidate.
y = row_index;
1341 candidate.
bin_index =
static_cast<unsigned char> (distance_map_index);
1343 list1.push_back (candidate);
1345 ++weights[distance_map_index];
1352 for (
typename std::list<Candidate>::iterator iter = list1.begin (); iter != list1.end (); ++iter)
1353 iter->distance *= 1.0f / weights[iter->bin_index];
1357 features.reserve (list1.size ());
1358 for (
typename std::list<Candidate>::iterator iter = list1.begin (); iter != list1.end (); ++iter)
1362 feature.
x =
static_cast<int> (iter->x);
1363 feature.
y =
static_cast<int> (iter->y);
1365 feature.
quantized_value = filtered_quantized_surface_normals_ (iter->x, iter->y);
1367 features.push_back (feature);
1372 template <
typename Po
intInT>
void
1375 const size_t width = input_->width;
1376 const size_t height = input_->height;
1378 quantized_surface_normals_.resize (width, height);
1380 for (
size_t row_index = 0; row_index < height; ++row_index)
1382 for (
size_t col_index = 0; col_index < width; ++col_index)
1384 const float normal_x = surface_normals_ (col_index, row_index).normal_x;
1385 const float normal_y = surface_normals_ (col_index, row_index).normal_y;
1386 const float normal_z = surface_normals_ (col_index, row_index).normal_z;
1388 if (pcl_isnan(normal_x) || pcl_isnan(normal_y) || pcl_isnan(normal_z) || normal_z > 0)
1390 quantized_surface_normals_ (col_index, row_index) = 0;
1397 float angle = 11.25f + atan2f (normal_y, normal_x)*180.0f/3.14f;
1399 if (angle < 0.0f) angle += 360.0f;
1400 if (angle >= 360.0f) angle -= 360.0f;
1402 int bin_index =
static_cast<int> (angle*8.0f/360.0f);
1405 quantized_surface_normals_ (col_index, row_index) =
static_cast<unsigned char> (bin_index);
1413 template <
typename Po
intInT>
void
1416 const int width = input_->width;
1417 const int height = input_->height;
1419 filtered_quantized_surface_normals_.resize (width, height);
1476 for (
int row_index = 2; row_index < height-2; ++row_index)
1478 for (
int col_index = 2; col_index < width-2; ++col_index)
1480 unsigned char histogram[9] = {0,0,0,0,0,0,0,0,0};
1502 unsigned char * dataPtr = quantized_surface_normals_.getData () + (row_index-2)*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)*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+1)*width+col_index-2;
1527 ++histogram[dataPtr[0]];
1528 ++histogram[dataPtr[1]];
1529 ++histogram[dataPtr[2]];
1530 ++histogram[dataPtr[3]];
1531 ++histogram[dataPtr[4]];
1534 unsigned char * dataPtr = quantized_surface_normals_.getData () + (row_index+2)*width+col_index-2;
1535 ++histogram[dataPtr[0]];
1536 ++histogram[dataPtr[1]];
1537 ++histogram[dataPtr[2]];
1538 ++histogram[dataPtr[3]];
1539 ++histogram[dataPtr[4]];
1543 unsigned char max_hist_value = 0;
1544 int max_hist_index = -1;
1546 if (max_hist_value < histogram[1]) {max_hist_index = 0; max_hist_value = histogram[1];}
1547 if (max_hist_value < histogram[2]) {max_hist_index = 1; max_hist_value = histogram[2];}
1548 if (max_hist_value < histogram[3]) {max_hist_index = 2; max_hist_value = histogram[3];}
1549 if (max_hist_value < histogram[4]) {max_hist_index = 3; max_hist_value = histogram[4];}
1550 if (max_hist_value < histogram[5]) {max_hist_index = 4; max_hist_value = histogram[5];}
1551 if (max_hist_value < histogram[6]) {max_hist_index = 5; max_hist_value = histogram[6];}
1552 if (max_hist_value < histogram[7]) {max_hist_index = 6; max_hist_value = histogram[7];}
1553 if (max_hist_value < histogram[8]) {max_hist_index = 7; max_hist_value = histogram[8];}
1555 if (max_hist_index != -1 && max_hist_value >= 1)
1557 filtered_quantized_surface_normals_ (col_index, row_index) =
static_cast<unsigned char> (0x1 << max_hist_index);
1561 filtered_quantized_surface_normals_ (col_index, row_index) = 0;
1585 template <
typename Po
intInT>
void
1588 const size_t width = input.
getWidth ();
1589 const size_t height = input.
getHeight ();
1591 output.
resize (width, height);
1595 const unsigned char * mask_map = input.
getData ();
1596 float * distance_map = output.
getData ();
1597 for (
size_t index = 0; index < width*height; ++index)
1599 if (mask_map[index] == 0)
1600 distance_map[index] = 0.0f;
1602 distance_map[index] =
static_cast<float> (width + height);
1606 float * previous_row = distance_map;
1607 float * current_row = previous_row + width;
1608 for (
size_t ri = 1; ri < height; ++ri)
1610 for (
size_t ci = 1; ci < width; ++ci)
1612 const float up_left = previous_row [ci - 1] + 1.4f;
1613 const float up = previous_row [ci] + 1.0f;
1614 const float up_right = previous_row [ci + 1] + 1.4f;
1615 const float left = current_row [ci - 1] + 1.0f;
1616 const float center = current_row [ci];
1618 const float min_value = std::min (std::min (up_left, up), std::min (left, up_right));
1620 if (min_value < center)
1621 current_row[ci] = min_value;
1623 previous_row = current_row;
1624 current_row += width;
1628 float * next_row = distance_map + width * (height - 1);
1629 current_row = next_row - width;
1630 for (
int ri = static_cast<int> (height)-2; ri >= 0; --ri)
1632 for (
int ci = static_cast<int> (width)-2; ci >= 0; --ci)
1634 const float lower_left = next_row [ci - 1] + 1.4f;
1635 const float lower = next_row [ci] + 1.0f;
1636 const float lower_right = next_row [ci + 1] + 1.4f;
1637 const float right = current_row [ci + 1] + 1.0f;
1638 const float center = current_row [ci];
1640 const float min_value = std::min (std::min (lower_left, lower), std::min (right, lower_right));
1642 if (min_value < center)
1643 current_row[ci] = min_value;
1645 next_row = current_row;
1646 current_row -= width;
const pcl::PointCloud< pcl::Normal > & getSurfaceNormals() const
Returns the surface normals.
bool operator<(const Candidate &rhs)
Compares two candidates based on their distance to the next different quantized value.
Represents a distance map obtained from a distance transformation.
unsigned char operator()(const float x, const float y, const float z) const
Operator to access an element in the LUT.
~LINEMOD_OrientationMap()
Destructor.
~QuantizedNormalLookUpTable()
Destructor.
int offset_x
The offset in x-direction.
int offset_y
The offset in y-direction.
void resize(const size_t width, const size_t height)
Resizes the map to the specified size.
QuantizedNormalLookUpTable()
Constructor.
void quantizeSurfaceNormals()
Quantizes the surface normals.
int offset_z
The offset in z-direction.
int range_z
The range of the LUT in z-direction.
Surface normal estimation on dense data using a least-squares estimation based on a first-order Taylo...
virtual void setInputCloud(const typename PointCloudIn::ConstPtr &cloud)
Provide a pointer to the input dataset (overwrites the PCLBase::setInputCloud method) ...
void setDepthDependentSmoothing(bool use_depth_dependent_smoothing)
Set whether to use depth depending smoothing or not.
void setNormalSmoothingSize(float normal_smoothing_size)
Set the normal smoothing size.
void computeAndQuantizeSurfaceNormals2()
Computes and quantizes the surface normals.
virtual void processInputData()
Processes the input data (smoothing, computing gradients, quantizing, filtering, spreading).
float distance
Distance to the next different quantized value.
int range_x
The range of the LUT in x-direction.
size_t getWidth() const
Returns the width of the modality data map.
void initializeLUT(const int range_x_arg, const int range_y_arg, const int range_z_arg)
Initializes the LUT.
Interface for a quantizable modality.
size_t x
x-position of the feature.
unsigned char * getData()
unsigned char * lut
The LUT data.
SurfaceNormalModality()
Constructor.
pcl::PointCloud< pcl::Normal > & getSurfaceNormals()
Returns the surface normals.
virtual ~SurfaceNormalModality()
Destructor.
int range_y
The range of the LUT in y-direction.
Map that stores orientations.
std::vector< PointT, Eigen::aligned_allocator< PointT > > VectorType
pcl::PointCloud< PointInT > PointCloudIn
LINEMOD_OrientationMap()
Constructor.
LINEMOD_OrientationMap & getOrientationMap()
Returns a reference to the orientation map.
void setSpreadingSize(const size_t spreading_size)
Sets the spreading size.
size_t getHeight() const
Returns the height of the modality data map.
virtual void setInputCloud(const typename PointCloudIn::ConstPtr &cloud)
Provide a pointer to the input dataset (overwrites the PCLBase::setInputCloud method) ...
QuantizedMap & getQuantizedMap()
Returns a reference to the internal quantized map.
Look-up-table for fast surface normal quantization.
void extractAllFeatures(const MaskMap &mask, size_t nr_features, size_t modality_index, std::vector< QuantizedMultiModFeature > &features) const
Extracts all possible features from the modality within the specified mask.
QuantizedMap & getSpreadedQuantizedMap()
Returns a reference to the internal spreaded quantized map.
float * getData()
Returns a pointer to the beginning of map.
A point structure representing normal coordinates and the surface curvature estimate.
void setMaxDepthChangeFactor(float max_depth_change_factor)
The depth change threshold for computing object borders.
int size_z
The size of the LUT in z-direction.
boost::shared_ptr< const PointCloud< PointT > > ConstPtr
size_t y
y-position of the feature.
virtual void processInputDataFromFiltered()
Processes the input data assuming that everything up to filtering is already done/available (so only ...
void compute(PointCloudOut &output)
Base method for feature estimation for all points given in <setInputCloud (), setIndices ()> using th...
unsigned char bin_index
Quantized value.
void filterQuantizedSurfaceNormals()
Filters the quantized surface normals.
void computeSurfaceNormals()
Computes the surface normals from the input cloud.
Feature that defines a position and quantized value in a specific modality.
int size_x
The size of the LUT in x-direction.
unsigned char quantized_value
the quantized value attached to the feature.
size_t modality_index
the index of the corresponding modality.
Modality based on surface normals.
void computeDistanceMap(const MaskMap &input, DistanceMap &output) const
Computes a distance map from the supplied input mask.
Candidate for a feature (used in feature extraction methods).
int size_y
The size of the LUT in y-direction.
A point structure representing Euclidean xyz coordinates.
void computeAndQuantizeSurfaceNormals()
Computes and quantizes the surface normals.
void resize(const size_t width, const size_t height, const float value)
Resizes the map to the specific width and height and initializes all new elements with the specified ...
PointCloudConstPtr input_
The input point cloud dataset.
void setVariableFeatureNr(const bool enabled)
Enables/disables the use of extracting a variable number of features.
static void spreadQuantizedMap(const QuantizedMap &input_map, QuantizedMap &output_map, size_t spreading_size)
void extractFeatures(const MaskMap &mask, size_t nr_features, size_t modality_index, std::vector< QuantizedMultiModFeature > &features) const
Extracts features from this modality within the specified mask.