41 #ifndef PCL_IMPLICIT_SHAPE_MODEL_HPP_ 42 #define PCL_IMPLICIT_SHAPE_MODEL_HPP_ 44 #include "../implicit_shape_model.h" 47 template <
typename Po
intT>
50 tree_is_valid_ (false),
60 template <
typename Po
intT>
72 template <
typename Po
intT>
void 90 colored_cloud->
width = 1;
94 colored_cloud->
height +=
static_cast<uint32_t
> (cloud->
points.size ());
98 for (
size_t i_point = 0; i_point < cloud->
points.size (); i_point++)
100 point.x = cloud->
points[i_point].x;
101 point.y = cloud->
points[i_point].y;
102 point.z = cloud->
points[i_point].z;
103 colored_cloud->
points.push_back (point);
110 for (
size_t i_vote = 0; i_vote <
votes_->
points.size (); i_vote++)
115 colored_cloud->
points.push_back (point);
119 return (colored_cloud);
123 template <
typename Po
intT>
void 125 std::vector<
pcl::ISMPeak, Eigen::aligned_allocator<pcl::ISMPeak> > &out_peaks,
127 double in_non_maxima_radius,
133 if (n_vote_classes == 0)
135 for (
size_t i = 0; i < n_vote_classes ; i++)
140 const int NUM_INIT_PTS = 100;
141 double SIGMA_DIST = in_sigma;
142 const double FINAL_EPS = SIGMA_DIST / 100;
144 std::vector<Eigen::Vector3f, Eigen::aligned_allocator<Eigen::Vector3f> > peaks (NUM_INIT_PTS);
145 std::vector<double> peak_densities (NUM_INIT_PTS);
146 double max_density = -1.0;
147 for (
int i = 0; i < NUM_INIT_PTS; i++)
149 Eigen::Vector3f old_center;
150 Eigen::Vector3f curr_center;
157 old_center = curr_center;
158 curr_center =
shiftMean (old_center, SIGMA_DIST);
159 }
while ((old_center - curr_center).norm () > FINAL_EPS);
162 point.x = curr_center (0);
163 point.y = curr_center (1);
164 point.z = curr_center (2);
166 assert (curr_density >= 0.0);
168 peaks[i] = curr_center;
169 peak_densities[i] = curr_density;
171 if ( max_density < curr_density )
172 max_density = curr_density;
176 std::vector<bool> peak_flag (NUM_INIT_PTS,
true);
177 for (
int i_peak = 0; i_peak < NUM_INIT_PTS; i_peak++)
180 double best_density = -1.0;
181 Eigen::Vector3f strongest_peak;
182 int best_peak_ind (-1);
183 int peak_counter (0);
184 for (
int i = 0; i < NUM_INIT_PTS; i++)
189 if ( peak_densities[i] > best_density)
191 best_density = peak_densities[i];
192 strongest_peak = peaks[i];
198 if( peak_counter == 0 )
202 peak.x = strongest_peak(0);
203 peak.y = strongest_peak(1);
204 peak.z = strongest_peak(2);
207 out_peaks.push_back ( peak );
210 peak_flag[best_peak_ind] =
false;
211 for (
int i = 0; i < NUM_INIT_PTS; i++)
217 double dist = (strongest_peak - peaks[i]).norm ();
218 if ( dist < in_non_maxima_radius )
219 peak_flag[i] =
false;
225 template <
typename Po
intT>
void 240 template <
typename Po
intT> Eigen::Vector3f
245 Eigen::Vector3f wgh_sum (0.0, 0.0, 0.0);
254 for (
size_t j = 0; j < n_pts; j++)
258 wgh_sum += vote_vec *
static_cast<float> (kernel);
261 assert (denom > 0.0);
263 return (wgh_sum / static_cast<float> (denom));
267 template <
typename Po
intT>
double 269 const PointT &point,
double sigma_dist)
274 if (n_vote_classes == 0)
277 double sum_vote = 0.0;
285 for (
size_t j = 0; j < num_of_pts; j++)
292 template <
typename Po
intT>
unsigned int 295 return (static_cast<unsigned int> (
votes_->
points.size ()));
300 statistical_weights_ (0),
301 learned_weights_ (0),
304 directions_to_center_ (),
305 clusters_centers_ (),
307 number_of_classes_ (0),
308 number_of_visual_words_ (0),
309 number_of_clusters_ (0),
310 descriptors_dimension_ (0)
324 std::vector<float> vec;
331 this->learned_weights_.resize (this->number_of_visual_words_, 0.0f);
335 this->classes_.resize (this->number_of_visual_words_, 0);
339 this->sigmas_.resize (this->number_of_classes_, 0.0f);
343 this->directions_to_center_.resize (this->number_of_visual_words_, 3);
345 for (
unsigned int i_dim = 0; i_dim < 3; i_dim++)
364 std::ofstream output_file (file_name.c_str (), std::ios::trunc);
367 output_file.close ();
387 output_file <<
classes_[i_visual_word] <<
" ";
391 output_file <<
sigmas_[i_class] <<
" ";
395 for (
unsigned int i_dim = 0; i_dim < 3; i_dim++)
406 output_file << static_cast<unsigned int> (
clusters_[i_cluster].size ()) <<
" ";
407 for (
unsigned int i_visual_word = 0; i_visual_word < static_cast<unsigned int> (
clusters_[i_cluster].size ()); i_visual_word++)
408 output_file <<
clusters_[i_cluster][i_visual_word] <<
" ";
411 output_file.close ();
420 std::ifstream input_file (file_name.c_str ());
429 input_file.getline (line, 256,
' ');
436 std::vector<float> vec;
449 classes_.resize (number_of_visual_words_, 0);
451 input_file >>
classes_[i_visual_word];
454 sigmas_.resize (number_of_classes_, 0.0f);
456 input_file >>
sigmas_[i_class];
461 for (
unsigned int i_dim = 0; i_dim < 3; i_dim++)
471 std::vector<unsigned int> vect;
472 clusters_.resize (number_of_clusters_, vect);
475 unsigned int size_of_current_cluster = 0;
476 input_file >> size_of_current_cluster;
477 clusters_[i_cluster].resize (size_of_current_cluster, 0);
478 for (
unsigned int i_visual_word = 0; i_visual_word < size_of_current_cluster; i_visual_word++)
479 input_file >>
clusters_[i_cluster][i_visual_word];
516 std::vector<float> vec;
523 this->learned_weights_.resize (this->number_of_visual_words_, 0.0f);
527 this->classes_.resize (this->number_of_visual_words_, 0);
531 this->sigmas_.resize (this->number_of_classes_, 0.0f);
535 this->directions_to_center_.resize (this->number_of_visual_words_, 3);
537 for (
unsigned int i_dim = 0; i_dim < 3; i_dim++)
549 template <
int FeatureSize,
typename Po
intT,
typename NormalT>
551 training_clouds_ (0),
552 training_classes_ (0),
553 training_normals_ (0),
554 training_sigmas_ (0),
555 sampling_size_ (0.1f),
556 feature_estimator_ (),
563 template <
int FeatureSize,
typename Po
intT,
typename NormalT>
574 template <
int FeatureSize,
typename Po
intT,
typename NormalT> std::vector<typename pcl::PointCloud<PointT>::Ptr>
581 template <
int FeatureSize,
typename Po
intT,
typename NormalT>
void 586 std::vector<typename pcl::PointCloud<PointT>::Ptr > clouds ( training_clouds.begin (), training_clouds.end () );
591 template <
int FeatureSize,
typename Po
intT,
typename NormalT> std::vector<unsigned int>
598 template <
int FeatureSize,
typename Po
intT,
typename NormalT>
void 602 std::vector<unsigned int> classes ( training_classes.begin (), training_classes.end () );
607 template <
int FeatureSize,
typename Po
intT,
typename NormalT> std::vector<typename pcl::PointCloud<NormalT>::Ptr>
614 template <
int FeatureSize,
typename Po
intT,
typename NormalT>
void 619 std::vector<typename pcl::PointCloud<NormalT>::Ptr > normals ( training_normals.begin (), training_normals.end () );
624 template <
int FeatureSize,
typename Po
intT,
typename NormalT>
float 631 template <
int FeatureSize,
typename Po
intT,
typename NormalT>
void 634 if (sampling_size >= std::numeric_limits<float>::epsilon ())
646 template <
int FeatureSize,
typename Po
intT,
typename NormalT>
void 653 template <
int FeatureSize,
typename Po
intT,
typename NormalT>
unsigned int 660 template <
int FeatureSize,
typename Po
intT,
typename NormalT>
void 663 if (num_of_clusters > 0)
668 template <
int FeatureSize,
typename Po
intT,
typename NormalT> std::vector<float>
675 template <
int FeatureSize,
typename Po
intT,
typename NormalT>
void 679 std::vector<float> sigmas ( training_sigmas.begin (), training_sigmas.end () );
684 template <
int FeatureSize,
typename Po
intT,
typename NormalT>
bool 691 template <
int FeatureSize,
typename Po
intT,
typename NormalT>
void 698 template <
int FeatureSize,
typename Po
intT,
typename NormalT>
bool 703 if (trained_model == 0)
705 trained_model->reset ();
707 std::vector<pcl::Histogram<FeatureSize> > histograms;
708 std::vector<LocationInfo, Eigen::aligned_allocator<LocationInfo> > locations;
713 Eigen::MatrixXi labels;
718 std::vector<unsigned int> vec;
720 for (
size_t i_label = 0; i_label < locations.size (); i_label++)
721 trained_model->clusters_[labels (i_label)].push_back (i_label);
728 trained_model->sigmas_,
729 trained_model->clusters_,
730 trained_model->statistical_weights_,
731 trained_model->learned_weights_);
734 trained_model->number_of_visual_words_ =
static_cast<unsigned int> (histograms.size ());
736 trained_model->descriptors_dimension_ = FeatureSize;
738 trained_model->directions_to_center_.resize (locations.size (), 3);
739 trained_model->classes_.resize (locations.size ());
740 for (
size_t i_dir = 0; i_dir < locations.size (); i_dir++)
742 trained_model->directions_to_center_(i_dir, 0) = locations[i_dir].dir_to_center_.x;
743 trained_model->directions_to_center_(i_dir, 1) = locations[i_dir].dir_to_center_.y;
744 trained_model->directions_to_center_(i_dir, 2) = locations[i_dir].dir_to_center_.z;
752 template <
int FeatureSize,
typename Po
intT,
typename NormalT> boost::shared_ptr<pcl::features::ISMVoteList<PointT> >
757 int in_class_of_interest)
761 if (in_cloud->
points.size () == 0)
766 simplifyCloud (in_cloud, in_normals, sampled_point_cloud, sampled_normal_cloud);
767 if (sampled_point_cloud->
points.size () == 0)
771 estimateFeatures (sampled_point_cloud, sampled_normal_cloud, feature_cloud);
774 const unsigned int n_key_points =
static_cast<unsigned int> (sampled_point_cloud->
size ());
775 std::vector<int> min_dist_inds (n_key_points, -1);
776 for (
unsigned int i_point = 0; i_point < n_key_points; i_point++)
778 Eigen::VectorXf curr_descriptor (FeatureSize);
779 for (
int i_dim = 0; i_dim < FeatureSize; i_dim++)
780 curr_descriptor (i_dim) = feature_cloud->
points[i_point].histogram[i_dim];
782 float descriptor_sum = curr_descriptor.sum ();
783 if (descriptor_sum < std::numeric_limits<float>::epsilon ())
786 unsigned int min_dist_idx = 0;
787 Eigen::VectorXf clusters_center (FeatureSize);
788 for (
int i_dim = 0; i_dim < FeatureSize; i_dim++)
789 clusters_center (i_dim) = model->clusters_centers_ (min_dist_idx, i_dim);
794 for (
int i_dim = 0; i_dim < FeatureSize; i_dim++)
795 clusters_center (i_dim) = model->clusters_centers_ (i_clust_cent, i_dim);
797 if (curr_dist < best_dist)
799 min_dist_idx = i_clust_cent;
800 best_dist = curr_dist;
803 min_dist_inds[i_point] = min_dist_idx;
806 for (
size_t i_point = 0; i_point < n_key_points; i_point++)
808 int min_dist_idx = min_dist_inds[i_point];
809 if (min_dist_idx == -1)
812 const unsigned int n_words =
static_cast<unsigned int> (model->clusters_[min_dist_idx].size ());
815 for (
unsigned int i_word = 0; i_word < n_words; i_word++)
817 unsigned int index = model->clusters_[min_dist_idx][i_word];
818 unsigned int i_class = model->classes_[index];
819 if (static_cast<int> (i_class) != in_class_of_interest)
823 Eigen::Vector3f direction (
824 model->directions_to_center_(index, 0),
825 model->directions_to_center_(index, 1),
826 model->directions_to_center_(index, 2));
830 Eigen::Vector3f vote_pos = sampled_point_cloud->
points[i_point].getVector3fMap () + direction;
831 vote.x = vote_pos[0];
832 vote.y = vote_pos[1];
833 vote.z = vote_pos[2];
834 float statistical_weight = model->statistical_weights_[in_class_of_interest][min_dist_idx];
835 float learned_weight = model->learned_weights_[index];
836 float power = statistical_weight * learned_weight;
837 vote.strength = power;
838 if (vote.strength > std::numeric_limits<float>::epsilon ())
839 out_votes->addVote (vote, sampled_point_cloud->
points[i_point], i_class);
847 template <
int FeatureSize,
typename Po
intT,
typename NormalT>
bool 850 std::vector<
LocationInfo, Eigen::aligned_allocator<LocationInfo> >& locations)
855 int n_key_points = 0;
863 Eigen::Vector3f models_center (0.0f, 0.0f, 0.0f);
866 models_center += point_j->getVector3fMap ();
867 models_center /=
static_cast<float> (num_of_points);
873 if (sampled_point_cloud->
points.size () == 0)
877 shiftCloud (sampled_point_cloud, models_center);
879 n_key_points +=
static_cast<int> (sampled_point_cloud->
size ());
882 estimateFeatures (sampled_point_cloud, sampled_normal_cloud, feature_cloud);
885 for (
auto point_i = sampled_point_cloud->
points.cbegin (); point_i != sampled_point_cloud->
points.cend (); point_i++, point_index++)
887 float descriptor_sum = Eigen::VectorXf::Map (feature_cloud->
points[point_index].histogram, FeatureSize).sum ();
888 if (descriptor_sum < std::numeric_limits<float>::epsilon ())
891 histograms.insert ( histograms.end (), feature_cloud->
begin () + point_index, feature_cloud->
begin () + point_index + 1 );
893 int dist =
static_cast<int> (std::distance (sampled_point_cloud->
points.cbegin (), point_i));
895 Eigen::Vector3f zero;
899 Eigen::Vector3f new_dir = zero - point_i->getVector3fMap ();
902 PointT point (new_dir[0], new_dir[1], new_dir[2]);
903 LocationInfo info (static_cast<unsigned int> (i_cloud), point, *point_i, sampled_normal_cloud->
points[dist]);
904 locations.insert(locations.end (), info);
912 template <
int FeatureSize,
typename Po
intT,
typename NormalT>
bool 915 Eigen::MatrixXi& labels,
916 Eigen::MatrixXf& clusters_centers)
918 Eigen::MatrixXf points_to_cluster (histograms.size (), FeatureSize);
920 for (
size_t i_feature = 0; i_feature < histograms.size (); i_feature++)
921 for (
int i_dim = 0; i_dim < FeatureSize; i_dim++)
922 points_to_cluster (i_feature, i_dim) = histograms[i_feature].histogram[i_dim];
924 labels.resize (histograms.size(), 1);
929 TermCriteria(TermCriteria::EPS|TermCriteria::COUNT, 10, 0.01f),
938 template <
int FeatureSize,
typename Po
intT,
typename NormalT>
void 951 sigmas.resize (number_of_classes, 0.0f);
953 std::vector<float> vec;
954 std::vector<std::vector<float> > objects_sigmas;
955 objects_sigmas.resize (number_of_classes, vec);
957 unsigned int number_of_objects =
static_cast<unsigned int> (
training_clouds_.size ());
958 for (
unsigned int i_object = 0; i_object < number_of_objects; i_object++)
960 float max_distance = 0.0f;
961 unsigned int number_of_points =
static_cast<unsigned int> (
training_clouds_[i_object]->points.size ());
962 for (
unsigned int i_point = 0; i_point < number_of_points - 1; i_point++)
963 for (
unsigned int j_point = i_point + 1; j_point < number_of_points; j_point++)
965 float curr_distance = 0.0f;
969 if (curr_distance > max_distance)
970 max_distance = curr_distance;
972 max_distance =
static_cast<float> (sqrt (max_distance));
974 objects_sigmas[i_class].push_back (max_distance);
977 for (
unsigned int i_class = 0; i_class < number_of_classes; i_class++)
980 unsigned int number_of_objects_in_class =
static_cast<unsigned int> (objects_sigmas[i_class].size ());
981 for (
unsigned int i_object = 0; i_object < number_of_objects_in_class; i_object++)
982 sig += objects_sigmas[i_class][i_object];
983 sig /= (
static_cast<float> (number_of_objects_in_class) * 10.0f);
984 sigmas[i_class] = sig;
989 template <
int FeatureSize,
typename Po
intT,
typename NormalT>
void 991 const std::vector<
LocationInfo, Eigen::aligned_allocator<LocationInfo> >& locations,
992 const Eigen::MatrixXi &labels,
993 std::vector<float>& sigmas,
994 std::vector<std::vector<unsigned int> >& clusters,
995 std::vector<std::vector<float> >& statistical_weights,
996 std::vector<float>& learned_weights)
1000 std::vector<float> vec;
1002 statistical_weights.clear ();
1003 learned_weights.clear ();
1004 statistical_weights.resize (number_of_classes, vec);
1005 learned_weights.resize (locations.size (), 0.0f);
1008 std::vector<int> vect;
1012 std::vector<int> n_ftr;
1015 std::vector<int> n_vot;
1018 std::vector<int> n_vw;
1021 std::vector<std::vector<int> > n_vot_2;
1025 n_ftr.resize (number_of_classes, 0);
1026 for (
size_t i_location = 0; i_location < locations.size (); i_location++)
1029 int i_cluster = labels (i_location);
1030 n_vot_2[i_cluster][i_class] += 1;
1031 n_vot[i_cluster] += 1;
1032 n_ftr[i_class] += 1;
1035 n_vw.resize (number_of_classes, 0);
1036 for (
unsigned int i_class = 0; i_class < number_of_classes; i_class++)
1038 if (n_vot_2[i_cluster][i_class] > 0)
1042 learned_weights.resize (locations.size (), 0.0);
1045 unsigned int number_of_words_in_cluster =
static_cast<unsigned int> (clusters[i_cluster].size ());
1046 for (
unsigned int i_visual_word = 0; i_visual_word < number_of_words_in_cluster; i_visual_word++)
1048 unsigned int i_index = clusters[i_cluster][i_visual_word];
1050 float square_sigma_dist = sigmas[i_class] * sigmas[i_class];
1051 if (square_sigma_dist < std::numeric_limits<float>::epsilon ())
1053 std::vector<float> calculated_sigmas;
1055 square_sigma_dist = calculated_sigmas[i_class] * calculated_sigmas[i_class];
1056 if (square_sigma_dist < std::numeric_limits<float>::epsilon ())
1060 Eigen::Vector3f direction = locations[i_index].dir_to_center_.getVector3fMap ();
1062 Eigen::Vector3f actual_center = locations[i_index].point_.getVector3fMap () + direction;
1065 std::vector<float> gauss_dists;
1066 for (
unsigned int j_visual_word = 0; j_visual_word < number_of_words_in_cluster; j_visual_word++)
1068 unsigned int j_index = clusters[i_cluster][j_visual_word];
1070 if (i_class != j_class)
1074 Eigen::Vector3f direction_2 = locations[i_index].dir_to_center_.getVector3fMap ();
1076 Eigen::Vector3f predicted_center = locations[j_index].point_.getVector3fMap () + direction_2;
1077 float residual = (predicted_center - actual_center).norm ();
1078 float value = -residual * residual / square_sigma_dist;
1079 gauss_dists.push_back (static_cast<float> (exp (value)));
1082 size_t mid_elem = (gauss_dists.size () - 1) / 2;
1083 std::nth_element (gauss_dists.begin (), gauss_dists.begin () + mid_elem, gauss_dists.end ());
1084 learned_weights[i_index] = *(gauss_dists.begin () + mid_elem);
1091 for (
unsigned int i_class = 0; i_class < number_of_classes; i_class++)
1093 if (n_vot_2[i_cluster][i_class] == 0)
1095 if (n_vw[i_class] == 0)
1097 if (n_vot[i_cluster] == 0)
1099 if (n_ftr[i_class] == 0)
1101 float part_1 =
static_cast<float> (n_vw[i_class]);
1102 float part_2 =
static_cast<float> (n_vot[i_cluster]);
1103 float part_3 =
static_cast<float> (n_vot_2[i_cluster][i_class]) / static_cast<float> (n_ftr[i_class]);
1104 float part_4 = 0.0f;
1109 for (
unsigned int j_class = 0; j_class < number_of_classes; j_class++)
1110 if (n_ftr[j_class] != 0)
1111 part_4 +=
static_cast<float> (n_vot_2[i_cluster][j_class]) / static_cast<float> (n_ftr[j_class]);
1113 statistical_weights[i_class][i_cluster] = (1.0f / part_1) * (1.0f / part_2) * part_3 / part_4;
1119 template <
int FeatureSize,
typename Po
intT,
typename NormalT>
void 1133 grid.
filter (temp_cloud);
1136 const float max_value = std::numeric_limits<float>::max ();
1138 const size_t num_source_points = in_point_cloud->
points.size ();
1139 const size_t num_sample_points = temp_cloud.
points.size ();
1141 std::vector<float> dist_to_grid_center (num_sample_points, max_value);
1142 std::vector<int> sampling_indices (num_sample_points, -1);
1144 for (
size_t i_point = 0; i_point < num_source_points; i_point++)
1153 float distance = (pt_1.x - pt_2.x) * (pt_1.x - pt_2.x) + (pt_1.y - pt_2.y) * (pt_1.y - pt_2.y) + (pt_1.z - pt_2.z) * (pt_1.z - pt_2.z);
1154 if (distance < dist_to_grid_center[index])
1156 dist_to_grid_center[index] = distance;
1157 sampling_indices[index] =
static_cast<int> (i_point);
1166 final_inliers_indices->indices.reserve (num_sample_points);
1167 for (
size_t i_point = 0; i_point < num_sample_points; i_point++)
1169 if (sampling_indices[i_point] != -1)
1170 final_inliers_indices->indices.push_back ( sampling_indices[i_point] );
1174 extract_points.
setIndices (final_inliers_indices);
1175 extract_points.
filter (*out_sampled_point_cloud);
1178 extract_normals.
setIndices (final_inliers_indices);
1179 extract_normals.
filter (*out_sampled_normal_cloud);
1183 template <
int FeatureSize,
typename Po
intT,
typename NormalT>
void 1186 Eigen::Vector3f shift_point)
1188 for (
auto point_it = in_cloud->
points.begin (); point_it != in_cloud->
points.end (); point_it++)
1190 point_it->x -= shift_point.x ();
1191 point_it->y -= shift_point.y ();
1192 point_it->z -= shift_point.z ();
1197 template <
int FeatureSize,
typename Po
intT,
typename NormalT> Eigen::Matrix3f
1200 Eigen::Matrix3f result;
1201 Eigen::Matrix3f rotation_matrix_X;
1202 Eigen::Matrix3f rotation_matrix_Z;
1208 float denom_X =
static_cast<float> (sqrt (in_normal.normal_z * in_normal.normal_z + in_normal.normal_y * in_normal.normal_y));
1209 A = in_normal.normal_y / denom_X;
1210 B = sign * in_normal.normal_z / denom_X;
1211 rotation_matrix_X << 1.0f, 0.0f, 0.0f,
1215 float denom_Z =
static_cast<float> (sqrt (in_normal.normal_x * in_normal.normal_x + in_normal.normal_y * in_normal.normal_y));
1216 A = in_normal.normal_y / denom_Z;
1217 B = sign * in_normal.normal_x / denom_Z;
1218 rotation_matrix_Z << A, -
B, 0.0f,
1222 result = rotation_matrix_X * rotation_matrix_Z;
1228 template <
int FeatureSize,
typename Po
intT,
typename NormalT>
void 1231 io_vec = in_transform * io_vec;
1235 template <
int FeatureSize,
typename Po
intT,
typename NormalT>
void 1260 template <
int FeatureSize,
typename Po
intT,
typename NormalT>
double 1262 const Eigen::MatrixXf& points_to_cluster,
1263 int number_of_clusters,
1264 Eigen::MatrixXi& io_labels,
1268 Eigen::MatrixXf& cluster_centers)
1270 const int spp_trials = 3;
1271 size_t number_of_points = points_to_cluster.rows () > 1 ? points_to_cluster.rows () : points_to_cluster.cols ();
1272 int feature_dimension = points_to_cluster.rows () > 1 ? FeatureSize : 1;
1274 attempts = std::max (attempts, 1);
1275 srand (static_cast<unsigned int> (time (0)));
1277 Eigen::MatrixXi labels (number_of_points, 1);
1284 Eigen::MatrixXf centers (number_of_clusters, feature_dimension);
1285 Eigen::MatrixXf old_centers (number_of_clusters, feature_dimension);
1286 std::vector<int> counters (number_of_clusters);
1287 std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> > boxes (feature_dimension);
1288 Eigen::Vector2f* box = &boxes[0];
1290 double best_compactness = std::numeric_limits<double>::max ();
1291 double compactness = 0.0;
1293 if (criteria.type_ & TermCriteria::EPS)
1294 criteria.epsilon_ = std::max (criteria.epsilon_, 0.0f);
1296 criteria.epsilon_ = std::numeric_limits<float>::epsilon ();
1298 criteria.epsilon_ *= criteria.epsilon_;
1300 if (criteria.type_ & TermCriteria::COUNT)
1301 criteria.max_count_ = std::min (std::max (criteria.max_count_, 2), 100);
1303 criteria.max_count_ = 100;
1305 if (number_of_clusters == 1)
1308 criteria.max_count_ = 2;
1311 for (
int i_dim = 0; i_dim < feature_dimension; i_dim++)
1312 box[i_dim] = Eigen::Vector2f (points_to_cluster (0, i_dim), points_to_cluster (0, i_dim));
1314 for (
size_t i_point = 0; i_point < number_of_points; i_point++)
1315 for (
int i_dim = 0; i_dim < feature_dimension; i_dim++)
1317 float v = points_to_cluster (i_point, i_dim);
1318 box[i_dim] (0) = std::min (box[i_dim] (0), v);
1319 box[i_dim] (1) = std::max (box[i_dim] (1), v);
1322 for (
int i_attempt = 0; i_attempt < attempts; i_attempt++)
1324 float max_center_shift = std::numeric_limits<float>::max ();
1325 for (
int iter = 0; iter < criteria.max_count_ && max_center_shift > criteria.epsilon_; iter++)
1327 Eigen::MatrixXf temp (centers.rows (), centers.cols ());
1329 centers = old_centers;
1332 if ( iter == 0 && ( i_attempt > 0 || !(flags & USE_INITIAL_LABELS) ) )
1338 for (
int i_cl_center = 0; i_cl_center < number_of_clusters; i_cl_center++)
1340 Eigen::VectorXf center (feature_dimension);
1342 for (
int i_dim = 0; i_dim < feature_dimension; i_dim++)
1343 centers (i_cl_center, i_dim) = center (i_dim);
1350 for (
int i_cluster = 0; i_cluster < number_of_clusters; i_cluster++)
1351 counters[i_cluster] = 0;
1352 for (
size_t i_point = 0; i_point < number_of_points; i_point++)
1354 int i_label = labels (i_point, 0);
1355 for (
int i_dim = 0; i_dim < feature_dimension; i_dim++)
1356 centers (i_label, i_dim) += points_to_cluster (i_point, i_dim);
1357 counters[i_label]++;
1360 max_center_shift = 0.0f;
1361 for (
int i_cl_center = 0; i_cl_center < number_of_clusters; i_cl_center++)
1363 if (counters[i_cl_center] != 0)
1365 float scale = 1.0f /
static_cast<float> (counters[i_cl_center]);
1366 for (
int i_dim = 0; i_dim < feature_dimension; i_dim++)
1367 centers (i_cl_center, i_dim) *= scale;
1371 Eigen::VectorXf center (feature_dimension);
1373 for(
int i_dim = 0; i_dim < feature_dimension; i_dim++)
1374 centers (i_cl_center, i_dim) = center (i_dim);
1380 for (
int i_dim = 0; i_dim < feature_dimension; i_dim++)
1382 float diff = centers (i_cl_center, i_dim) - old_centers (i_cl_center, i_dim);
1383 dist += diff * diff;
1385 max_center_shift = std::max (max_center_shift, dist);
1390 for (
size_t i_point = 0; i_point < number_of_points; i_point++)
1392 Eigen::VectorXf sample (feature_dimension);
1393 sample = points_to_cluster.row (i_point);
1396 float min_dist = std::numeric_limits<float>::max ();
1398 for (
int i_cluster = 0; i_cluster < number_of_clusters; i_cluster++)
1400 Eigen::VectorXf center (feature_dimension);
1401 center = centers.row (i_cluster);
1403 if (min_dist > dist)
1409 compactness += min_dist;
1410 labels (i_point, 0) = k_best;
1414 if (compactness < best_compactness)
1416 best_compactness = compactness;
1417 cluster_centers = centers;
1422 return (best_compactness);
1426 template <
int FeatureSize,
typename Po
intT,
typename NormalT>
void 1428 const Eigen::MatrixXf& data,
1429 Eigen::MatrixXf& out_centers,
1430 int number_of_clusters,
1433 size_t dimension = data.cols ();
1434 unsigned int number_of_points =
static_cast<unsigned int> (data.rows ());
1435 std::vector<int> centers_vec (number_of_clusters);
1436 int* centers = ¢ers_vec[0];
1437 std::vector<double> dist (number_of_points);
1438 std::vector<double> tdist (number_of_points);
1439 std::vector<double> tdist2 (number_of_points);
1442 unsigned int random_unsigned = rand ();
1443 centers[0] = random_unsigned % number_of_points;
1445 for (
unsigned int i_point = 0; i_point < number_of_points; i_point++)
1447 Eigen::VectorXf first (dimension);
1448 Eigen::VectorXf second (dimension);
1449 first = data.row (i_point);
1450 second = data.row (centers[0]);
1452 sum0 += dist[i_point];
1455 for (
int i_cluster = 0; i_cluster < number_of_clusters; i_cluster++)
1457 double best_sum = std::numeric_limits<double>::max ();
1458 int best_center = -1;
1459 for (
int i_trials = 0; i_trials < trials; i_trials++)
1461 unsigned int random_integer = rand () - 1;
1462 double random_double =
static_cast<double> (random_integer) / static_cast<double> (std::numeric_limits<unsigned int>::max ());
1463 double p = random_double * sum0;
1465 unsigned int i_point;
1466 for (i_point = 0; i_point < number_of_points - 1; i_point++)
1467 if ( (p -= dist[i_point]) <= 0.0)
1473 for (
unsigned int i_point = 0; i_point < number_of_points; i_point++)
1475 Eigen::VectorXf first (dimension);
1476 Eigen::VectorXf second (dimension);
1477 first = data.row (i_point);
1478 second = data.row (ci);
1479 tdist2[i_point] = std::min (static_cast<double> (
computeDistance (first, second)), dist[i_point]);
1480 s += tdist2[i_point];
1487 std::swap (tdist, tdist2);
1491 centers[i_cluster] = best_center;
1493 std::swap (dist, tdist);
1496 for (
int i_cluster = 0; i_cluster < number_of_clusters; i_cluster++)
1497 for (
size_t i_dim = 0; i_dim < dimension; i_dim++)
1498 out_centers (i_cluster, i_dim) = data (centers[i_cluster], i_dim);
1502 template <
int FeatureSize,
typename Po
intT,
typename NormalT>
void 1504 Eigen::VectorXf& center)
1506 size_t dimension = boxes.size ();
1507 float margin = 1.0f /
static_cast<float> (dimension);
1509 for (
size_t i_dim = 0; i_dim < dimension; i_dim++)
1511 unsigned int random_integer = rand () - 1;
1512 float random_float =
static_cast<float> (random_integer) / static_cast<float> (std::numeric_limits<unsigned int>::max ());
1513 center (i_dim) = (random_float * (1.0f + margin * 2.0f)- margin) * (boxes[i_dim] (1) - boxes[i_dim] (0)) + boxes[i_dim] (0);
1518 template <
int FeatureSize,
typename Po
intT,
typename NormalT>
float 1521 size_t dimension = vec_1.rows () > 1 ? vec_1.rows () : vec_1.cols ();
1522 float distance = 0.0f;
1523 for(
size_t i_dim = 0; i_dim < dimension; i_dim++)
1525 float diff = vec_1 (i_dim) - vec_2 (i_dim);
1526 distance += diff * diff;
1532 #endif //#ifndef PCL_IMPLICIT_SHAPE_MODEL_HPP_ A point structure representing normal coordinates and the surface curvature estimate.
void generateRandomCenter(const std::vector< Eigen::Vector2f, Eigen::aligned_allocator< Eigen::Vector2f > > &boxes, Eigen::VectorXf ¢er)
Generates random center for cluster.
KdTreeFLANN is a generic type of 3D spatial locator using kD-tree structures.
std::vector< typename pcl::PointCloud< PointT >::Ptr > getTrainingClouds()
This method simply returns the clouds that were set as the training clouds.
std::vector< float > learned_weights_
Stores learned weights.
void setNumberOfClusters(unsigned int num_of_clusters)
Changes the number of clusters.
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
ISMVoteList()
Empty constructor with member variables initialization.
unsigned int getNumberOfVotes()
This method simply returns the number of votes.
void generateCentersPP(const Eigen::MatrixXf &data, Eigen::MatrixXf &out_centers, int number_of_clusters, int trials)
Generates centers for clusters as described in Arthur, David and Sergei Vassilvitski (2007) k-means++...
bool loadModelFromfile(std::string &file_name)
This method loads the trained model from file.
The assignment of this structure is to store the statistical/learned weights and other information of...
void setTrainingNormals(const std::vector< typename pcl::PointCloud< NormalT >::Ptr > &training_normals)
Allows to set normals for the training clouds that were passed through setTrainingClouds method...
std::vector< int > k_ind_
Stores neighbours indices.
void simplifyCloud(typename pcl::PointCloud< PointT >::ConstPtr in_point_cloud, typename pcl::PointCloud< NormalT >::ConstPtr in_normal_cloud, typename pcl::PointCloud< PointT >::Ptr out_sampled_point_cloud, typename pcl::PointCloud< NormalT >::Ptr out_sampled_normal_cloud)
Simplifies the cloud using voxel grid principles.
struct PCL_EXPORTS pcl::ism::ImplicitShapeModelEstimation::TC TermCriteria
This structure is used for determining the end of the k-means clustering process. ...
Ptr makeShared() const
Copy the cloud to the heap and return a smart pointer Note that deep copy is performed, so avoid using this function on non-empty clouds.
static const int PP_CENTERS
This const value is used for indicating that for k-means clustering centers must be generated as desc...
std::vector< float > getSigmaDists()
Returns the array of sigma values.
This file defines compatibility wrappers for low level I/O functions.
virtual ~ISMVoteList()
virtual descriptor.
unsigned int descriptors_dimension_
Stores descriptors dimension.
unsigned int number_of_classes_
Stores the number of classes.
Eigen::MatrixXf directions_to_center_
Stores the directions to objects center for each visual word.
float sampling_size_
This value is used for the simplification.
pcl::PointCloud< PointT >::Ptr votes_origins_
Stores the origins of the votes.
int getCentroidIndex(const PointT &p) const
Returns the index in the resulting downsampled cloud of the specified point.
bool getNVotState()
Returns the state of Nvot coeff from [Knopp et al., 2010, (4)], if set to false then coeff is taken a...
pcl::PointCloud< pcl::InterestPoint >::Ptr votes_
Stores all votes.
std::vector< int > votes_class_
Stores classes for which every single vote was cast.
bool saveModelToFile(std::string &file_name)
This method simply saves the trained model for later usage.
Eigen::Matrix3f alignYCoordWithNormal(const NormalT &in_normal)
This method simply computes the rotation matrix, so that the given normal would match the Y axis afte...
std::vector< float > training_sigmas_
This array stores the sigma values for each training class.
ISMModel()
Simple constructor that initializes the structure.
void setSaveLeafLayout(bool save_leaf_layout)
Set to true if leaf layout information needs to be saved for later access.
void filter(PointCloud &output)
Calls the filtering method and returns the filtered dataset in output.
VoxelGrid assembles a local 3D grid over a given PointCloud, and downsamples + filters the data...
bool n_vot_ON_
If set to false then Nvot coeff from [Knopp et al., 2010, (4)] is equal 1.0.
void setNVotState(bool state)
Changes the state of the Nvot coeff from [Knopp et al., 2010, (4)].
Eigen::MatrixXf clusters_centers_
Stores the centers of the clusters that were obtained during the visual words clusterization.
uint32_t height
The point cloud height (if organized as an image-structure).
void setSigmaDists(const std::vector< float > &training_sigmas)
This method allows to set the value of sigma used for calculating the learned weights for every singl...
virtual ~ISMModel()
Destructor that frees memory.
boost::shared_ptr< PointCloud< PointT > > Ptr
A point structure representing an N-D histogram.
uint32_t width
The point cloud width (if organized as an image-structure).
bool trainISM(ISMModelPtr &trained_model)
This method performs training and forms a visual vocabulary.
ImplicitShapeModelEstimation()
Simple constructor that initializes everything.
unsigned int number_of_clusters_
Stores the number of clusters.
boost::shared_ptr< pcl::search::Search< PointT > > Ptr
double computeKMeansClustering(const Eigen::MatrixXf &points_to_cluster, int number_of_clusters, Eigen::MatrixXi &io_labels, TermCriteria criteria, int attempts, int flags, Eigen::MatrixXf &cluster_centers)
Performs K-means clustering.
virtual ~ImplicitShapeModelEstimation()
Simple destructor.
A point structure representing Euclidean xyz coordinates.
float computeDistance(Eigen::VectorXf &vec_1, Eigen::VectorXf &vec_2)
Computes the square distance between two vectors.
std::vector< std::vector< float > > statistical_weights_
Stores statistical weights.
A point structure representing an interest point with Euclidean xyz coordinates, and an interest valu...
std::vector< std::vector< unsigned int > > clusters_
This is an array of clusters.
boost::shared_ptr< pcl::features::ISMVoteList< PointT > > findObjects(ISMModelPtr model, typename pcl::PointCloud< PointT >::Ptr in_cloud, typename pcl::PointCloud< Normal >::Ptr in_normals, int in_class_of_interest)
This function is searching for the class of interest in a given cloud and returns the list of votes...
boost::shared_ptr< const PointCloud< PointT > > ConstPtr
void reset()
this method resets all variables and frees memory.
static const int USE_INITIAL_LABELS
This const value is used for indicating that input labels must be taken as the initial approximation ...
std::vector< float > k_sqr_dist_
Stores square distances to the corresponding neighbours.
PointCloud represents the base class in PCL for storing collections of 3D points. ...
double getDensityAtPoint(const PointT &point, double sigma_dist)
Returns the density at the specified point.
double density
Density of this peak.
virtual void setIndices(const IndicesPtr &indices)
Provide a pointer to the vector of indices that represents the input data.
void setFeatureEstimator(FeaturePtr feature)
Changes the feature estimator.
void calculateWeights(const std::vector< LocationInfo, Eigen::aligned_allocator< LocationInfo > > &locations, const Eigen::MatrixXi &labels, std::vector< float > &sigmas, std::vector< std::vector< unsigned int > > &clusters, std::vector< std::vector< float > > &statistical_weights, std::vector< float > &learned_weights)
This function forms a visual vocabulary and evaluates weights described in [Knopp et al...
void applyTransform(Eigen::Vector3f &io_vec, const Eigen::Matrix3f &in_transform)
This method applies transform set in in_transform to vector io_vector.
FeaturePtr getFeatureEstimator()
Returns the current feature estimator used for extraction of the descriptors.
void filter(PointCloud &output)
std::vector< typename pcl::PointCloud< NormalT >::Ptr > getTrainingNormals()
This method returns the corresponding cloud of normals for every training point cloud.
std::vector< unsigned int > getTrainingClasses()
Returns the array of classes that indicates which class the corresponding training cloud belongs...
pcl::KdTreeFLANN< pcl::InterestPoint >::Ptr tree_
Stores the search tree.
boost::shared_ptr< ::pcl::PointIndices > Ptr
bool clusterDescriptors(std::vector< pcl::Histogram< FeatureSize > > &histograms, Eigen::MatrixXi &labels, Eigen::MatrixXf &clusters_centers)
This method performs descriptor clustering.
void estimateFeatures(typename pcl::PointCloud< PointT >::Ptr sampled_point_cloud, typename pcl::PointCloud< NormalT >::Ptr normal_cloud, typename pcl::PointCloud< pcl::Histogram< FeatureSize > >::Ptr feature_cloud)
This method estimates features for the given point cloud.
ISMModel & operator=(const ISMModel &other)
Operator overloading for deep copy.
int class_id
Determines which class this peak belongs.
unsigned int number_of_clusters_
Number of clusters, is used for clustering descriptors during the training.
void findStrongestPeaks(std::vector< ISMPeak, Eigen::aligned_allocator< ISMPeak > > &out_peaks, int in_class_id, double in_non_maxima_radius, double in_sigma)
This method finds the strongest peaks (points were density has most higher values).
virtual void setInputCloud(const PointCloudConstPtr &cloud)
Provide a pointer to the input dataset.
std::vector< typename pcl::PointCloud< PointT >::Ptr > training_clouds_
Stores the clouds used for training.
Eigen::Vector3f shiftMean(const Eigen::Vector3f &snapPt, const double in_dSigmaDist)
This struct is used for storing peak.
Feature::Ptr feature_estimator_
Stores the feature estimator.
unsigned int number_of_visual_words_
Stores the number of visual words.
bool tree_is_valid_
Signalizes if the tree is valid.
std::vector< unsigned int > classes_
Stores the class label for every direction.
void addVote(pcl::InterestPoint &in_vote, const PointT &vote_origin, int in_class)
This method simply adds another vote to the list.
std::vector< float > sigmas_
Stores the sigma value for each class.
A point structure representing Euclidean xyz coordinates, and the RGB color.
bool extractDescriptors(std::vector< pcl::Histogram< FeatureSize > > &histograms, std::vector< LocationInfo, Eigen::aligned_allocator< LocationInfo > > &locations)
Extracts the descriptors from the input clouds.
float getSamplingSize()
Returns the sampling size used for cloud simplification.
boost::shared_ptr< pcl::features::ISMModel > ISMModelPtr
std::vector< typename pcl::PointCloud< NormalT >::Ptr > training_normals_
Stores the normals for each training cloud.
unsigned int getNumberOfClusters()
Returns the number of clusters used for descriptor clustering.
void setInputNormals(const PointCloudNConstPtr &normals)
Provide a pointer to the input dataset that contains the point normals of the XYZ dataset...
void validateTree()
this method is simply setting up the search tree.
This structure stores the information about the keypoint.
void setSamplingSize(float sampling_size)
Changes the sampling size used for cloud simplification.
void shiftCloud(typename pcl::PointCloud< PointT >::Ptr in_cloud, Eigen::Vector3f shift_point)
This method simply shifts the clouds points relative to the passed point.
void setTrainingClasses(const std::vector< unsigned int > &training_classes)
Allows to set the class labels for the corresponding training clouds.
void setTrainingClouds(const std::vector< typename pcl::PointCloud< PointT >::Ptr > &training_clouds)
Allows to set clouds for training the ISM model.
pcl::PointCloud< pcl::PointXYZRGB >::Ptr getColoredCloud(typename pcl::PointCloud< PointT >::Ptr cloud=0)
Returns the colored cloud that consists of votes for center (blue points) and initial point cloud (if...
std::vector< unsigned int > training_classes_
Stores the class number for each cloud from training_clouds_.
void calculateSigmas(std::vector< float > &sigmas)
This method calculates the value of sigma used for calculating the learned weights for every single c...
void setLeafSize(const Eigen::Vector4f &leaf_size)
Set the voxel grid leaf size.
This class is used for storing, analyzing and manipulating votes obtained from ISM algorithm...