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;
A point structure representing normal coordinates and the surface curvature estimate.
void computeAndQuantizeSurfaceNormals()
Computes and quantizes the surface normals.
int offset_y
The offset in y-direction.
boost::shared_ptr< const PointCloud< PointT > > ConstPtr
void computeAndQuantizeSurfaceNormals2()
Computes and quantizes the surface normals.
std::vector< PointT, Eigen::aligned_allocator< PointT > > VectorType
~LINEMOD_OrientationMap()
Destructor.
void setDepthDependentSmoothing(bool use_depth_dependent_smoothing)
Set whether to use depth depending smoothing or not.
void filterQuantizedSurfaceNormals()
Filters the quantized surface normals.
LINEMOD_OrientationMap()
Constructor.
Feature that defines a position and quantized value in a specific modality.
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 ...
void setNormalSmoothingSize(float normal_smoothing_size)
Set the normal smoothing size.
void quantizeSurfaceNormals()
Quantizes the surface normals.
int offset_x
The offset in x-direction.
int range_y
The range of the LUT in y-direction.
virtual void setInputCloud(const typename PointCloudIn::ConstPtr &cloud)
Provide a pointer to the input dataset (overwrites the PCLBase::setInputCloud method) ...
pcl::PointCloud< PointInT > PointCloudIn
size_t getHeight() const
Returns the height of the modality data map.
void setVariableFeatureNr(const bool enabled)
Enables/disables the use of extracting a variable number of features.
int range_z
The range of the LUT in z-direction.
virtual ~SurfaceNormalModality()
Destructor.
void resize(const size_t width, const size_t height)
Resizes the map to the specified size.
Map that stores orientations.
unsigned char quantized_value
the quantized value attached to the feature.
Represents a distance map obtained from a distance transformation.
void setMaxDepthChangeFactor(float max_depth_change_factor)
The depth change threshold for computing object borders.
SurfaceNormalModality()
Constructor.
QuantizedNormalLookUpTable()
Constructor.
bool operator<(const Candidate &rhs)
Compares two candidates based on their distance to the next different quantized value.
const pcl::PointCloud< pcl::Normal > & getSurfaceNormals() const
Returns the surface normals.
unsigned char * getData()
void setSpreadingSize(const size_t spreading_size)
Sets the spreading size.
Candidate for a feature (used in feature extraction methods).
void initializeLUT(const int range_x_arg, const int range_y_arg, const int range_z_arg)
Initializes the LUT.
void computeSurfaceNormals()
Computes the surface normals from the input cloud.
A point structure representing Euclidean xyz coordinates.
pcl::PointCloud< pcl::Normal > & getSurfaceNormals()
Returns the surface normals.
void computeDistanceMap(const MaskMap &input, DistanceMap &output) const
Computes a distance map from the supplied input mask.
virtual void processInputDataFromFiltered()
Processes the input data assuming that everything up to filtering is already done/available (so only ...
virtual void processInputData()
Processes the input data (smoothing, computing gradients, quantizing, filtering, spreading).
int size_x
The size of the LUT in x-direction.
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.
LINEMOD_OrientationMap & getOrientationMap()
Returns a reference to the orientation map.
int offset_z
The offset in z-direction.
size_t x
x-position of the feature.
Interface for a quantizable modality.
Look-up-table for fast surface normal quantization.
Modality based on surface normals.
unsigned char * lut
The LUT data.
unsigned char bin_index
Quantized value.
~QuantizedNormalLookUpTable()
Destructor.
QuantizedMap & getQuantizedMap()
Returns a reference to the internal quantized map.
unsigned char operator()(const float x, const float y, const float z) const
Operator to access an element in the LUT.
PointCloudConstPtr input_
The input point cloud dataset.
float distance
Distance to the next different quantized value.
void compute(PointCloudOut &output)
Base method for feature estimation for all points given in <setInputCloud (), setIndices ()> using th...
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.
static void spreadQuantizedMap(const QuantizedMap &input_map, QuantizedMap &output_map, size_t spreading_size)
Surface normal estimation on dense data using a least-squares estimation based on a first-order Taylo...
float * getData()
Returns a pointer to the beginning of map.
int range_x
The range of the LUT in x-direction.
int size_y
The size of the LUT in y-direction.
virtual void setInputCloud(const typename PointCloudIn::ConstPtr &cloud)
Provide a pointer to the input dataset (overwrites the PCLBase::setInputCloud method) ...
size_t modality_index
the index of the corresponding modality.
size_t y
y-position of the feature.
QuantizedMap & getSpreadedQuantizedMap()
Returns a reference to the internal spreaded quantized map.
int size_z
The size of the LUT in z-direction.
size_t getWidth() const
Returns the width of the modality data map.