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> 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_;
153 range_x (-1), range_y (-1), range_z (-1),
154 offset_x (-1), offset_y (-1), offset_z (-1),
155 size_x (-1), size_y (-1), size_z (-1), lut (NULL)
171 initializeLUT (
const int range_x_arg,
const int range_y_arg,
const int range_z_arg)
173 range_x = range_x_arg;
174 range_y = range_y_arg;
175 range_z = range_z_arg;
176 size_x = range_x_arg+1;
177 size_y = range_y_arg+1;
178 size_z = range_z_arg+1;
179 offset_x = range_x_arg/2;
180 offset_y = range_y_arg/2;
181 offset_z = range_z_arg;
188 lut =
new unsigned char[size_x*size_y*size_z];
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)
229 PointXYZ normal (static_cast<float> (x_index - range_x/2),
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));
274 const size_t index = z_index*size_y*size_x + y_index*size_x + x_index;
283 operator() (
const int index)
const 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_);
397 extractFeatures (
const MaskMap & mask,
size_t nr_features,
size_t modality_index,
398 std::vector<QuantizedMultiModFeature> & features)
const;
407 extractAllFeatures (
const MaskMap & mask,
size_t nr_features,
size_t modality_index,
408 std::vector<QuantizedMultiModFeature> & features)
const;
426 processInputDataFromFiltered ();
432 computeSurfaceNormals ();
436 computeAndQuantizeSurfaceNormals ();
440 computeAndQuantizeSurfaceNormals2 ();
444 quantizeSurfaceNormals ();
448 filterQuantizedSurfaceNormals ();
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;
723 template <
typename Po
intInT>
void 726 const int width = input_->width;
727 const int height = input_->height;
729 unsigned short * lp_depth =
new unsigned short[width*height];
730 unsigned char * lp_normals =
new unsigned char[width*height];
731 memset (lp_normals, 0, width*height);
733 surface_normal_orientations_.resize (width, height, 0.0f);
735 for (
int row_index = 0; row_index < height; ++row_index)
737 for (
int col_index = 0; col_index < width; ++col_index)
739 const float value = input_->points[row_index*width + col_index].z;
740 if (pcl_isfinite (value))
742 lp_depth[row_index*width + col_index] =
static_cast<unsigned short> (value * 1000.0f);
746 lp_depth[row_index*width + col_index] = 0;
751 const int l_W = width;
752 const int l_H = height;
764 const int offsets_i[] = {-l_r, 0, l_r, -l_r, l_r, -l_r, 0, l_r};
765 const int offsets_j[] = {-l_r, -l_r, -l_r, 0, 0, l_r, l_r, l_r};
766 const int offsets[] = { offsets_i[0] + offsets_j[0] * l_W
767 , offsets_i[1] + offsets_j[1] * l_W
768 , offsets_i[2] + offsets_j[2] * l_W
769 , offsets_i[3] + offsets_j[3] * l_W
770 , offsets_i[4] + offsets_j[4] * l_W
771 , offsets_i[5] + offsets_j[5] * l_W
772 , offsets_i[6] + offsets_j[6] * l_W
773 , offsets_i[7] + offsets_j[7] * l_W };
779 const int difference_threshold = 50;
780 const int distance_threshold = 2000;
786 for (
int l_y = l_r; l_y < l_H - l_r - 1; ++l_y)
788 unsigned short * lp_line = lp_depth + (l_y * l_W + l_r);
789 unsigned char * lp_norm = lp_normals + (l_y * l_W + l_r);
791 for (
int l_x = l_r; l_x < l_W - l_r - 1; ++l_x)
793 long l_d = lp_line[0];
798 if (l_d < distance_threshold)
801 long l_A[4]; l_A[0] = l_A[1] = l_A[2] = l_A[3] = 0;
802 long l_b[2]; l_b[0] = l_b[1] = 0;
806 accumBilateral(lp_line[offsets[0]] - l_d, offsets_i[0], offsets_j[0], l_A, l_b, difference_threshold);
807 accumBilateral(lp_line[offsets[1]] - l_d, offsets_i[1], offsets_j[1], l_A, l_b, difference_threshold);
808 accumBilateral(lp_line[offsets[2]] - l_d, offsets_i[2], offsets_j[2], l_A, l_b, difference_threshold);
809 accumBilateral(lp_line[offsets[3]] - l_d, offsets_i[3], offsets_j[3], l_A, l_b, difference_threshold);
810 accumBilateral(lp_line[offsets[4]] - l_d, offsets_i[4], offsets_j[4], l_A, l_b, difference_threshold);
811 accumBilateral(lp_line[offsets[5]] - l_d, offsets_i[5], offsets_j[5], l_A, l_b, difference_threshold);
812 accumBilateral(lp_line[offsets[6]] - l_d, offsets_i[6], offsets_j[6], l_A, l_b, difference_threshold);
813 accumBilateral(lp_line[offsets[7]] - l_d, offsets_i[7], offsets_j[7], l_A, l_b, difference_threshold);
872 long l_det = l_A[0] * l_A[3] - l_A[1] * l_A[1];
873 long l_ddx = l_A[3] * l_b[0] - l_A[1] * l_b[1];
874 long l_ddy = -l_A[1] * l_b[0] + l_A[0] * l_b[1];
878 float l_nx =
static_cast<float>(1150 * l_ddx);
879 float l_ny =
static_cast<float>(1150 * l_ddy);
880 float l_nz =
static_cast<float>(-l_det * l_d);
894 float l_sqrt = sqrtf (l_nx * l_nx + l_ny * l_ny + l_nz * l_nz);
898 float l_norminv = 1.0f / (l_sqrt);
904 float angle = 22.5f + atan2f (l_ny, l_nx) * 180.0f / 3.14f;
906 if (angle < 0.0f) angle += 360.0f;
907 if (angle >= 360.0f) angle -= 360.0f;
909 int bin_index =
static_cast<int> (angle*8.0f/360.0f) & 7;
911 surface_normal_orientations_ (l_x, l_y) = angle;
920 *lp_norm =
static_cast<unsigned char> (0x1 << bin_index);
937 unsigned char map[255];
949 quantized_surface_normals_.resize (width, height);
950 for (
int row_index = 0; row_index < height; ++row_index)
952 for (
int col_index = 0; col_index < width; ++col_index)
954 quantized_surface_normals_ (col_index, row_index) = map[lp_normals[row_index*width + col_index]];
964 template <
typename Po
intInT>
void 966 const size_t nr_features,
967 const size_t modality_index,
968 std::vector<QuantizedMultiModFeature> & features)
const 970 const size_t width = mask.
getWidth ();
984 for (
size_t map_index = 0; map_index < 8; ++map_index)
985 mask_maps[map_index].resize (width, height);
987 unsigned char map[255];
1002 for (
size_t row_index = 0; row_index < height; ++row_index)
1004 for (
size_t col_index = 0; col_index < width; ++col_index)
1006 if (mask (col_index, row_index) != 0)
1009 const unsigned char quantized_value = filtered_quantized_surface_normals_ (col_index, row_index);
1011 if (quantized_value == 0)
1013 const int dist_map_index = map[quantized_value];
1015 distance_map_indices (col_index, row_index) =
static_cast<unsigned char> (dist_map_index);
1017 mask_maps[dist_map_index] (col_index, row_index) = 255;
1023 for (
int map_index = 0; map_index < 8; ++map_index)
1024 computeDistanceMap (mask_maps[map_index], distance_maps[map_index]);
1027 computeDistanceMap (mask, mask_distance_maps);
1029 std::list<Candidate> list1;
1030 std::list<Candidate> list2;
1032 float weights[8] = {0,0,0,0,0,0,0,0};
1034 const size_t off = 4;
1035 for (
size_t row_index = off; row_index < height-off; ++row_index)
1037 for (
size_t col_index = off; col_index < width-off; ++col_index)
1039 if (mask (col_index, row_index) != 0)
1042 const unsigned char quantized_value = filtered_quantized_surface_normals_ (col_index, row_index);
1048 if (quantized_value != 0)
1050 const int distance_map_index = map[quantized_value];
1053 const float distance = distance_maps[distance_map_index] (col_index, row_index);
1054 const float distance_to_border = mask_distance_maps (col_index, row_index);
1056 if (distance >= feature_distance_threshold_ && distance_to_border >= min_distance_to_border_)
1058 Candidate candidate;
1061 candidate.x = col_index;
1062 candidate.y = row_index;
1063 candidate.bin_index =
static_cast<unsigned char> (distance_map_index);
1065 list1.push_back (candidate);
1067 ++weights[distance_map_index];
1074 for (
typename std::list<Candidate>::iterator iter = list1.begin (); iter != list1.end (); ++iter)
1075 iter->distance *= 1.0f / weights[iter->bin_index];
1079 if (variable_feature_nr_)
1081 int distance =
static_cast<int> (list1.size ());
1082 bool feature_selection_finished =
false;
1083 while (!feature_selection_finished)
1085 const int sqr_distance = distance*
distance;
1086 for (
typename std::list<Candidate>::iterator iter1 = list1.begin (); iter1 != list1.end (); ++iter1)
1088 bool candidate_accepted =
true;
1089 for (
typename std::list<Candidate>::iterator iter2 = list2.begin (); iter2 != list2.end (); ++iter2)
1091 const int dx =
static_cast<int> (iter1->x) - static_cast<int> (iter2->x);
1092 const int dy =
static_cast<int> (iter1->y) - static_cast<int> (iter2->y);
1093 const int tmp_distance = dx*dx + dy*dy;
1095 if (tmp_distance < sqr_distance)
1097 candidate_accepted =
false;
1103 float min_min_sqr_distance = std::numeric_limits<float>::max ();
1104 float max_min_sqr_distance = 0;
1105 for (
typename std::list<Candidate>::iterator iter2 = list2.begin (); iter2 != list2.end (); ++iter2)
1107 float min_sqr_distance = std::numeric_limits<float>::max ();
1108 for (
typename std::list<Candidate>::iterator iter3 = list2.begin (); iter3 != list2.end (); ++iter3)
1113 const float dx =
static_cast<float> (iter2->x) - static_cast<float> (iter3->x);
1114 const float dy =
static_cast<float> (iter2->y) - static_cast<float> (iter3->y);
1116 const float sqr_distance = dx*dx + dy*dy;
1118 if (sqr_distance < min_sqr_distance)
1120 min_sqr_distance = sqr_distance;
1129 const float dx =
static_cast<float> (iter2->x) - static_cast<float> (iter1->x);
1130 const float dy =
static_cast<float> (iter2->y) - static_cast<float> (iter1->y);
1132 const float sqr_distance = dx*dx + dy*dy;
1134 if (sqr_distance < min_sqr_distance)
1136 min_sqr_distance = sqr_distance;
1140 if (min_sqr_distance < min_min_sqr_distance)
1141 min_min_sqr_distance = min_sqr_distance;
1142 if (min_sqr_distance > max_min_sqr_distance)
1143 max_min_sqr_distance = min_sqr_distance;
1148 if (candidate_accepted)
1154 if (min_min_sqr_distance < 50)
1156 feature_selection_finished =
true;
1160 list2.push_back (*iter1);
1174 if (list1.size () <= nr_features)
1176 features.reserve (list1.size ());
1177 for (
typename std::list<Candidate>::iterator iter = list1.begin (); iter != list1.end (); ++iter)
1181 feature.
x =
static_cast<int> (iter->x);
1182 feature.
y =
static_cast<int> (iter->y);
1184 feature.
quantized_value = filtered_quantized_surface_normals_ (iter->x, iter->y);
1186 features.push_back (feature);
1192 int distance =
static_cast<int> (list1.size () / nr_features + 1);
1193 while (list2.size () != nr_features)
1195 const int sqr_distance = distance*
distance;
1196 for (
typename std::list<Candidate>::iterator iter1 = list1.begin (); iter1 != list1.end (); ++iter1)
1198 bool candidate_accepted =
true;
1200 for (
typename std::list<Candidate>::iterator iter2 = list2.begin (); iter2 != list2.end (); ++iter2)
1202 const int dx =
static_cast<int> (iter1->x) - static_cast<int> (iter2->x);
1203 const int dy =
static_cast<int> (iter1->y) - static_cast<int> (iter2->y);
1204 const int tmp_distance = dx*dx + dy*dy;
1206 if (tmp_distance < sqr_distance)
1208 candidate_accepted =
false;
1213 if (candidate_accepted)
1214 list2.push_back (*iter1);
1216 if (list2.size () == nr_features)
break;
1222 for (
typename std::list<Candidate>::iterator iter2 = list2.begin (); iter2 != list2.end (); ++iter2)
1226 feature.
x =
static_cast<int> (iter2->x);
1227 feature.
y =
static_cast<int> (iter2->y);
1229 feature.
quantized_value = filtered_quantized_surface_normals_ (iter2->x, iter2->y);
1231 features.push_back (feature);
1236 template <
typename Po
intInT>
void 1238 const MaskMap & mask,
const size_t,
const size_t modality_index,
1239 std::vector<QuantizedMultiModFeature> & features)
const 1241 const size_t width = mask.
getWidth ();
1242 const size_t height = mask.
getHeight ();
1255 for (
size_t map_index = 0; map_index < 8; ++map_index)
1256 mask_maps[map_index].resize (width, height);
1258 unsigned char map[255];
1259 memset(map, 0, 255);
1273 for (
size_t row_index = 0; row_index < height; ++row_index)
1275 for (
size_t col_index = 0; col_index < width; ++col_index)
1277 if (mask (col_index, row_index) != 0)
1280 const unsigned char quantized_value = filtered_quantized_surface_normals_ (col_index, row_index);
1282 if (quantized_value == 0)
1284 const int dist_map_index = map[quantized_value];
1286 distance_map_indices (col_index, row_index) =
static_cast<unsigned char> (dist_map_index);
1288 mask_maps[dist_map_index] (col_index, row_index) = 255;
1294 for (
int map_index = 0; map_index < 8; ++map_index)
1295 computeDistanceMap (mask_maps[map_index], distance_maps[map_index]);
1298 computeDistanceMap (mask, mask_distance_maps);
1300 std::list<Candidate> list1;
1301 std::list<Candidate> list2;
1303 float weights[8] = {0,0,0,0,0,0,0,0};
1305 const size_t off = 4;
1306 for (
size_t row_index = off; row_index < height-off; ++row_index)
1308 for (
size_t col_index = off; col_index < width-off; ++col_index)
1310 if (mask (col_index, row_index) != 0)
1313 const unsigned char quantized_value = filtered_quantized_surface_normals_ (col_index, row_index);
1319 if (quantized_value != 0)
1321 const int distance_map_index = map[quantized_value];
1324 const float distance = distance_maps[distance_map_index] (col_index, row_index);
1325 const float distance_to_border = mask_distance_maps (col_index, row_index);
1327 if (distance >= feature_distance_threshold_ && distance_to_border >= min_distance_to_border_)
1329 Candidate candidate;
1332 candidate.x = col_index;
1333 candidate.y = row_index;
1334 candidate.bin_index =
static_cast<unsigned char> (distance_map_index);
1336 list1.push_back (candidate);
1338 ++weights[distance_map_index];
1345 for (
typename std::list<Candidate>::iterator iter = list1.begin (); iter != list1.end (); ++iter)
1346 iter->distance *= 1.0f / weights[iter->bin_index];
1350 features.reserve (list1.size ());
1351 for (
typename std::list<Candidate>::iterator iter = list1.begin (); iter != list1.end (); ++iter)
1355 feature.
x =
static_cast<int> (iter->x);
1356 feature.
y =
static_cast<int> (iter->y);
1358 feature.
quantized_value = filtered_quantized_surface_normals_ (iter->x, iter->y);
1360 features.push_back (feature);
1365 template <
typename Po
intInT>
void 1368 const size_t width = input_->width;
1369 const size_t height = input_->height;
1371 quantized_surface_normals_.resize (width, height);
1373 for (
size_t row_index = 0; row_index < height; ++row_index)
1375 for (
size_t col_index = 0; col_index < width; ++col_index)
1377 const float normal_x = surface_normals_ (col_index, row_index).normal_x;
1378 const float normal_y = surface_normals_ (col_index, row_index).normal_y;
1379 const float normal_z = surface_normals_ (col_index, row_index).normal_z;
1381 if (pcl_isnan(normal_x) || pcl_isnan(normal_y) || pcl_isnan(normal_z) || normal_z > 0)
1383 quantized_surface_normals_ (col_index, row_index) = 0;
1390 float angle = 11.25f + atan2f (normal_y, normal_x)*180.0f/3.14f;
1392 if (angle < 0.0f) angle += 360.0f;
1393 if (angle >= 360.0f) angle -= 360.0f;
1395 int bin_index =
static_cast<int> (angle*8.0f/360.0f);
1398 quantized_surface_normals_ (col_index, row_index) =
static_cast<unsigned char> (bin_index);
1406 template <
typename Po
intInT>
void 1409 const int width = input_->width;
1410 const int height = input_->height;
1412 filtered_quantized_surface_normals_.resize (width, height);
1469 for (
int row_index = 2; row_index < height-2; ++row_index)
1471 for (
int col_index = 2; col_index < width-2; ++col_index)
1473 unsigned char histogram[9] = {0,0,0,0,0,0,0,0,0};
1495 unsigned char * dataPtr = quantized_surface_normals_.getData () + (row_index-2)*width+col_index-2;
1496 ++histogram[dataPtr[0]];
1497 ++histogram[dataPtr[1]];
1498 ++histogram[dataPtr[2]];
1499 ++histogram[dataPtr[3]];
1500 ++histogram[dataPtr[4]];
1503 unsigned char * dataPtr = quantized_surface_normals_.getData () + (row_index-1)*width+col_index-2;
1504 ++histogram[dataPtr[0]];
1505 ++histogram[dataPtr[1]];
1506 ++histogram[dataPtr[2]];
1507 ++histogram[dataPtr[3]];
1508 ++histogram[dataPtr[4]];
1511 unsigned char * dataPtr = quantized_surface_normals_.getData () + (row_index)*width+col_index-2;
1512 ++histogram[dataPtr[0]];
1513 ++histogram[dataPtr[1]];
1514 ++histogram[dataPtr[2]];
1515 ++histogram[dataPtr[3]];
1516 ++histogram[dataPtr[4]];
1519 unsigned char * dataPtr = quantized_surface_normals_.getData () + (row_index+1)*width+col_index-2;
1520 ++histogram[dataPtr[0]];
1521 ++histogram[dataPtr[1]];
1522 ++histogram[dataPtr[2]];
1523 ++histogram[dataPtr[3]];
1524 ++histogram[dataPtr[4]];
1527 unsigned char * dataPtr = quantized_surface_normals_.getData () + (row_index+2)*width+col_index-2;
1528 ++histogram[dataPtr[0]];
1529 ++histogram[dataPtr[1]];
1530 ++histogram[dataPtr[2]];
1531 ++histogram[dataPtr[3]];
1532 ++histogram[dataPtr[4]];
1536 unsigned char max_hist_value = 0;
1537 int max_hist_index = -1;
1539 if (max_hist_value < histogram[1]) {max_hist_index = 0; max_hist_value = histogram[1];}
1540 if (max_hist_value < histogram[2]) {max_hist_index = 1; max_hist_value = histogram[2];}
1541 if (max_hist_value < histogram[3]) {max_hist_index = 2; max_hist_value = histogram[3];}
1542 if (max_hist_value < histogram[4]) {max_hist_index = 3; max_hist_value = histogram[4];}
1543 if (max_hist_value < histogram[5]) {max_hist_index = 4; max_hist_value = histogram[5];}
1544 if (max_hist_value < histogram[6]) {max_hist_index = 5; max_hist_value = histogram[6];}
1545 if (max_hist_value < histogram[7]) {max_hist_index = 6; max_hist_value = histogram[7];}
1546 if (max_hist_value < histogram[8]) {max_hist_index = 7; max_hist_value = histogram[8];}
1548 if (max_hist_index != -1 && max_hist_value >= 1)
1550 filtered_quantized_surface_normals_ (col_index, row_index) =
static_cast<unsigned char> (0x1 << max_hist_index);
1554 filtered_quantized_surface_normals_ (col_index, row_index) = 0;
1578 template <
typename Po
intInT>
void 1581 const size_t width = input.
getWidth ();
1582 const size_t height = input.
getHeight ();
1584 output.
resize (width, height);
1588 const unsigned char * mask_map = input.
getData ();
1589 float * distance_map = output.
getData ();
1590 for (
size_t index = 0; index < width*height; ++index)
1592 if (mask_map[index] == 0)
1593 distance_map[index] = 0.0f;
1595 distance_map[index] =
static_cast<float> (width + height);
1599 float * previous_row = distance_map;
1600 float * current_row = previous_row + width;
1601 for (
size_t ri = 1; ri < height; ++ri)
1603 for (
size_t ci = 1; ci < width; ++ci)
1605 const float up_left = previous_row [ci - 1] + 1.4f;
1606 const float up = previous_row [ci] + 1.0f;
1607 const float up_right = previous_row [ci + 1] + 1.4f;
1608 const float left = current_row [ci - 1] + 1.0f;
1609 const float center = current_row [ci];
1611 const float min_value = std::min (std::min (up_left, up), std::min (left, up_right));
1613 if (min_value < center)
1614 current_row[ci] = min_value;
1616 previous_row = current_row;
1617 current_row += width;
1621 float * next_row = distance_map + width * (height - 1);
1622 current_row = next_row - width;
1623 for (
int ri = static_cast<int> (height)-2; ri >= 0; --ri)
1625 for (
int ci = static_cast<int> (width)-2; ci >= 0; --ci)
1627 const float lower_left = next_row [ci - 1] + 1.4f;
1628 const float lower = next_row [ci] + 1.0f;
1629 const float lower_right = next_row [ci + 1] + 1.4f;
1630 const float right = current_row [ci + 1] + 1.0f;
1631 const float center = current_row [ci];
1633 const float min_value = std::min (std::min (lower_left, lower), std::min (right, lower_right));
1635 if (min_value < center)
1636 current_row[ci] = min_value;
1638 next_row = current_row;
1639 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.
void computeAndQuantizeSurfaceNormals2()
Computes and quantizes the surface normals.
~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.
std::vector< PointT, Eigen::aligned_allocator< PointT > > VectorType
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 getWidth() const
Returns the width 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.
unsigned char * getData()
void setSpreadingSize(const size_t spreading_size)
Sets the spreading size.
Candidate for a feature (used in feature extraction methods).
size_t getHeight() const
Returns the height 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.
Defines all the PCL implemented PointT point type structures.
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.
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.
LINEMOD_OrientationMap & getOrientationMap()
Returns a reference to the orientation map.
int offset_z
The offset in z-direction.
float distance(const PointT &p1, const PointT &p2)
size_t x
x-position of the feature.
boost::shared_ptr< const PointCloud< PointXYZT > > ConstPtr
Interface for a quantizable modality.
Look-up-table for fast surface normal quantization.
Modality based on surface normals.
void computeDistanceMap(const MaskMap &input, DistanceMap &output) const
Computes a distance map from the supplied input mask.
unsigned char * lut
The LUT data.
unsigned char bin_index
Quantized value.
~QuantizedNormalLookUpTable()
Destructor.
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.
QuantizedMap & getQuantizedMap()
Returns a reference to the internal quantized map.
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...
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.
const pcl::PointCloud< pcl::Normal > & getSurfaceNormals() const
Returns the surface normals.
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.