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 ())
639 template <
int FeatureSize,
typename Po
intT,
typename NormalT> boost::shared_ptr<pcl::Feature<PointT, pcl::Histogram<FeatureSize> > >
646 template <
int FeatureSize,
typename Po
intT,
typename NormalT>
void 654 template <
int FeatureSize,
typename Po
intT,
typename NormalT>
unsigned int 661 template <
int FeatureSize,
typename Po
intT,
typename NormalT>
void 664 if (num_of_clusters > 0)
669 template <
int FeatureSize,
typename Po
intT,
typename NormalT> std::vector<float>
676 template <
int FeatureSize,
typename Po
intT,
typename NormalT>
void 680 std::vector<float> sigmas ( training_sigmas.begin (), training_sigmas.end () );
685 template <
int FeatureSize,
typename Po
intT,
typename NormalT>
bool 692 template <
int FeatureSize,
typename Po
intT,
typename NormalT>
void 699 template <
int FeatureSize,
typename Po
intT,
typename NormalT>
bool 704 if (trained_model == 0)
706 trained_model->reset ();
708 std::vector<pcl::Histogram<FeatureSize> > histograms;
709 std::vector<LocationInfo, Eigen::aligned_allocator<LocationInfo> > locations;
714 Eigen::MatrixXi labels;
719 std::vector<unsigned int> vec;
721 for (
size_t i_label = 0; i_label < locations.size (); i_label++)
722 trained_model->clusters_[labels (i_label)].push_back (i_label);
729 trained_model->sigmas_,
730 trained_model->clusters_,
731 trained_model->statistical_weights_,
732 trained_model->learned_weights_);
735 trained_model->number_of_visual_words_ =
static_cast<unsigned int> (histograms.size ());
737 trained_model->descriptors_dimension_ = FeatureSize;
739 trained_model->directions_to_center_.resize (locations.size (), 3);
740 trained_model->classes_.resize (locations.size ());
741 for (
size_t i_dir = 0; i_dir < locations.size (); i_dir++)
743 trained_model->directions_to_center_(i_dir, 0) = locations[i_dir].dir_to_center_.x;
744 trained_model->directions_to_center_(i_dir, 1) = locations[i_dir].dir_to_center_.y;
745 trained_model->directions_to_center_(i_dir, 2) = locations[i_dir].dir_to_center_.z;
753 template <
int FeatureSize,
typename Po
intT,
typename NormalT> boost::shared_ptr<pcl::features::ISMVoteList<PointT> >
758 int in_class_of_interest)
762 if (in_cloud->
points.size () == 0)
767 simplifyCloud (in_cloud, in_normals, sampled_point_cloud, sampled_normal_cloud);
768 if (sampled_point_cloud->
points.size () == 0)
772 estimateFeatures (sampled_point_cloud, sampled_normal_cloud, feature_cloud);
775 const unsigned int n_key_points =
static_cast<unsigned int> (sampled_point_cloud->
size ());
776 std::vector<int> min_dist_inds (n_key_points, -1);
777 for (
unsigned int i_point = 0; i_point < n_key_points; i_point++)
779 Eigen::VectorXf curr_descriptor (FeatureSize);
780 for (
int i_dim = 0; i_dim < FeatureSize; i_dim++)
781 curr_descriptor (i_dim) = feature_cloud->
points[i_point].histogram[i_dim];
783 float descriptor_sum = curr_descriptor.sum ();
784 if (descriptor_sum < std::numeric_limits<float>::epsilon ())
787 unsigned int min_dist_idx = 0;
788 Eigen::VectorXf clusters_center (FeatureSize);
789 for (
int i_dim = 0; i_dim < FeatureSize; i_dim++)
790 clusters_center (i_dim) = model->clusters_centers_ (min_dist_idx, i_dim);
795 for (
int i_dim = 0; i_dim < FeatureSize; i_dim++)
796 clusters_center (i_dim) = model->clusters_centers_ (i_clust_cent, i_dim);
798 if (curr_dist < best_dist)
800 min_dist_idx = i_clust_cent;
801 best_dist = curr_dist;
804 min_dist_inds[i_point] = min_dist_idx;
807 for (
size_t i_point = 0; i_point < n_key_points; i_point++)
809 int min_dist_idx = min_dist_inds[i_point];
810 if (min_dist_idx == -1)
813 const unsigned int n_words =
static_cast<unsigned int> (model->clusters_[min_dist_idx].size ());
816 for (
unsigned int i_word = 0; i_word < n_words; i_word++)
818 unsigned int index = model->clusters_[min_dist_idx][i_word];
819 unsigned int i_class = model->classes_[index];
820 if (static_cast<int> (i_class) != in_class_of_interest)
824 Eigen::Vector3f direction (
825 model->directions_to_center_(index, 0),
826 model->directions_to_center_(index, 1),
827 model->directions_to_center_(index, 2));
831 Eigen::Vector3f vote_pos = sampled_point_cloud->
points[i_point].getVector3fMap () + direction;
832 vote.x = vote_pos[0];
833 vote.y = vote_pos[1];
834 vote.z = vote_pos[2];
835 float statistical_weight = model->statistical_weights_[in_class_of_interest][min_dist_idx];
836 float learned_weight = model->learned_weights_[index];
837 float power = statistical_weight * learned_weight;
838 vote.strength = power;
839 if (vote.strength > std::numeric_limits<float>::epsilon ())
840 out_votes->addVote (vote, sampled_point_cloud->
points[i_point], i_class);
848 template <
int FeatureSize,
typename Po
intT,
typename NormalT>
bool 851 std::vector<
LocationInfo, Eigen::aligned_allocator<LocationInfo> >& locations)
856 int n_key_points = 0;
864 Eigen::Vector3f models_center (0.0f, 0.0f, 0.0f);
868 models_center += point_j->getVector3fMap ();
869 models_center /=
static_cast<float> (num_of_points);
875 if (sampled_point_cloud->
points.size () == 0)
879 shiftCloud (sampled_point_cloud, models_center);
881 n_key_points +=
static_cast<int> (sampled_point_cloud->
size ());
884 estimateFeatures (sampled_point_cloud, sampled_normal_cloud, feature_cloud);
888 for (point_i = sampled_point_cloud->
points.begin (); point_i != sampled_point_cloud->
points.end (); point_i++, point_index++)
890 float descriptor_sum = Eigen::VectorXf::Map (feature_cloud->
points[point_index].histogram, FeatureSize).sum ();
891 if (descriptor_sum < std::numeric_limits<float>::epsilon ())
894 histograms.insert ( histograms.end (), feature_cloud->
begin () + point_index, feature_cloud->
begin () + point_index + 1 );
896 int dist =
static_cast<int> (std::distance (sampled_point_cloud->
points.begin (), point_i));
898 Eigen::Vector3f zero;
902 Eigen::Vector3f new_dir = zero - point_i->getVector3fMap ();
905 PointT point (new_dir[0], new_dir[1], new_dir[2]);
906 LocationInfo info (static_cast<unsigned int> (i_cloud), point, *point_i, sampled_normal_cloud->
points[dist]);
907 locations.insert(locations.end (), info);
915 template <
int FeatureSize,
typename Po
intT,
typename NormalT>
bool 918 Eigen::MatrixXi& labels,
919 Eigen::MatrixXf& clusters_centers)
921 Eigen::MatrixXf points_to_cluster (histograms.size (), FeatureSize);
923 for (
unsigned int i_feature = 0; i_feature < histograms.size (); i_feature++)
924 for (
int i_dim = 0; i_dim < FeatureSize; i_dim++)
925 points_to_cluster (i_feature, i_dim) = histograms[i_feature].histogram[i_dim];
927 labels.resize (histograms.size(), 1);
932 TermCriteria(TermCriteria::EPS|TermCriteria::COUNT, 10, 0.01f),
941 template <
int FeatureSize,
typename Po
intT,
typename NormalT>
void 947 for (
unsigned int i_sigma = 0; i_sigma <
training_sigmas_.size (); i_sigma++)
954 sigmas.resize (number_of_classes, 0.0f);
956 std::vector<float> vec;
957 std::vector<std::vector<float> > objects_sigmas;
958 objects_sigmas.resize (number_of_classes, vec);
960 unsigned int number_of_objects =
static_cast<unsigned int> (
training_clouds_.size ());
961 for (
unsigned int i_object = 0; i_object < number_of_objects; i_object++)
963 float max_distance = 0.0f;
964 unsigned int number_of_points =
static_cast<unsigned int> (
training_clouds_[i_object]->points.size ());
965 for (
unsigned int i_point = 0; i_point < number_of_points - 1; i_point++)
966 for (
unsigned int j_point = i_point + 1; j_point < number_of_points; j_point++)
968 float curr_distance = 0.0f;
972 if (curr_distance > max_distance)
973 max_distance = curr_distance;
975 max_distance =
static_cast<float> (sqrt (max_distance));
977 objects_sigmas[i_class].push_back (max_distance);
980 for (
unsigned int i_class = 0; i_class < number_of_classes; i_class++)
983 unsigned int number_of_objects_in_class =
static_cast<unsigned int> (objects_sigmas[i_class].size ());
984 for (
unsigned int i_object = 0; i_object < number_of_objects_in_class; i_object++)
985 sig += objects_sigmas[i_class][i_object];
986 sig /= (
static_cast<float> (number_of_objects_in_class) * 10.0f);
987 sigmas[i_class] = sig;
992 template <
int FeatureSize,
typename Po
intT,
typename NormalT>
void 994 const std::vector<
LocationInfo, Eigen::aligned_allocator<LocationInfo> >& locations,
995 const Eigen::MatrixXi &labels,
996 std::vector<float>& sigmas,
997 std::vector<std::vector<unsigned int> >& clusters,
998 std::vector<std::vector<float> >& statistical_weights,
999 std::vector<float>& learned_weights)
1003 std::vector<float> vec;
1005 statistical_weights.clear ();
1006 learned_weights.clear ();
1007 statistical_weights.resize (number_of_classes, vec);
1008 learned_weights.resize (locations.size (), 0.0f);
1011 std::vector<int> vect;
1015 std::vector<int> n_ftr;
1018 std::vector<int> n_vot;
1021 std::vector<int> n_vw;
1024 std::vector<std::vector<int> > n_vot_2;
1028 n_ftr.resize (number_of_classes, 0);
1029 for (
size_t i_location = 0; i_location < locations.size (); i_location++)
1032 int i_cluster = labels (i_location);
1033 n_vot_2[i_cluster][i_class] += 1;
1034 n_vot[i_cluster] += 1;
1035 n_ftr[i_class] += 1;
1038 n_vw.resize (number_of_classes, 0);
1039 for (
unsigned int i_class = 0; i_class < number_of_classes; i_class++)
1041 if (n_vot_2[i_cluster][i_class] > 0)
1045 learned_weights.resize (locations.size (), 0.0);
1048 unsigned int number_of_words_in_cluster =
static_cast<unsigned int> (clusters[i_cluster].size ());
1049 for (
unsigned int i_visual_word = 0; i_visual_word < number_of_words_in_cluster; i_visual_word++)
1051 unsigned int i_index = clusters[i_cluster][i_visual_word];
1053 float square_sigma_dist = sigmas[i_class] * sigmas[i_class];
1054 if (square_sigma_dist < std::numeric_limits<float>::epsilon ())
1056 std::vector<float> calculated_sigmas;
1058 square_sigma_dist = calculated_sigmas[i_class] * calculated_sigmas[i_class];
1059 if (square_sigma_dist < std::numeric_limits<float>::epsilon ())
1063 Eigen::Vector3f direction = locations[i_index].dir_to_center_.getVector3fMap ();
1065 Eigen::Vector3f actual_center = locations[i_index].point_.getVector3fMap () + direction;
1068 std::vector<float> gauss_dists;
1069 for (
unsigned int j_visual_word = 0; j_visual_word < number_of_words_in_cluster; j_visual_word++)
1071 unsigned int j_index = clusters[i_cluster][j_visual_word];
1073 if (i_class != j_class)
1077 Eigen::Vector3f direction_2 = locations[i_index].dir_to_center_.getVector3fMap ();
1079 Eigen::Vector3f predicted_center = locations[j_index].point_.getVector3fMap () + direction_2;
1080 float residual = (predicted_center - actual_center).norm ();
1081 float value = -residual * residual / square_sigma_dist;
1082 gauss_dists.push_back (static_cast<float> (exp (value)));
1085 size_t mid_elem = (gauss_dists.size () - 1) / 2;
1086 std::nth_element (gauss_dists.begin (), gauss_dists.begin () + mid_elem, gauss_dists.end ());
1087 learned_weights[i_index] = *(gauss_dists.begin () + mid_elem);
1094 for (
unsigned int i_class = 0; i_class < number_of_classes; i_class++)
1096 if (n_vot_2[i_cluster][i_class] == 0)
1098 if (n_vw[i_class] == 0)
1100 if (n_vot[i_cluster] == 0)
1102 if (n_ftr[i_class] == 0)
1104 float part_1 =
static_cast<float> (n_vw[i_class]);
1105 float part_2 =
static_cast<float> (n_vot[i_cluster]);
1106 float part_3 =
static_cast<float> (n_vot_2[i_cluster][i_class]) / static_cast<float> (n_ftr[i_class]);
1107 float part_4 = 0.0f;
1112 for (
unsigned int j_class = 0; j_class < number_of_classes; j_class++)
1113 if (n_ftr[j_class] != 0)
1114 part_4 +=
static_cast<float> (n_vot_2[i_cluster][j_class]) / static_cast<float> (n_ftr[j_class]);
1116 statistical_weights[i_class][i_cluster] = (1.0f / part_1) * (1.0f / part_2) * part_3 / part_4;
1122 template <
int FeatureSize,
typename Po
intT,
typename NormalT>
void 1136 grid.
filter (temp_cloud);
1139 const float max_value = std::numeric_limits<float>::max ();
1141 const size_t num_source_points = in_point_cloud->
points.size ();
1142 const size_t num_sample_points = temp_cloud.
points.size ();
1144 std::vector<float> dist_to_grid_center (num_sample_points, max_value);
1145 std::vector<int> sampling_indices (num_sample_points, -1);
1147 for (
size_t i_point = 0; i_point < num_source_points; i_point++)
1156 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);
1157 if (distance < dist_to_grid_center[index])
1159 dist_to_grid_center[index] = distance;
1160 sampling_indices[index] =
static_cast<int> (i_point);
1169 final_inliers_indices->indices.reserve (num_sample_points);
1170 for (
size_t i_point = 0; i_point < num_sample_points; i_point++)
1172 if (sampling_indices[i_point] != -1)
1173 final_inliers_indices->indices.push_back ( sampling_indices[i_point] );
1177 extract_points.
setIndices (final_inliers_indices);
1178 extract_points.
filter (*out_sampled_point_cloud);
1181 extract_normals.
setIndices (final_inliers_indices);
1182 extract_normals.
filter (*out_sampled_normal_cloud);
1186 template <
int FeatureSize,
typename Po
intT,
typename NormalT>
void 1189 Eigen::Vector3f shift_point)
1192 for (point_it = in_cloud->
points.begin (); point_it != in_cloud->
points.end (); point_it++)
1194 point_it->x -= shift_point.x ();
1195 point_it->y -= shift_point.y ();
1196 point_it->z -= shift_point.z ();
1201 template <
int FeatureSize,
typename Po
intT,
typename NormalT> Eigen::Matrix3f
1204 Eigen::Matrix3f result;
1205 Eigen::Matrix3f rotation_matrix_X;
1206 Eigen::Matrix3f rotation_matrix_Z;
1212 float denom_X =
static_cast<float> (sqrt (in_normal.normal_z * in_normal.normal_z + in_normal.normal_y * in_normal.normal_y));
1213 A = in_normal.normal_y / denom_X;
1214 B = sign * in_normal.normal_z / denom_X;
1215 rotation_matrix_X << 1.0f, 0.0f, 0.0f,
1219 float denom_Z =
static_cast<float> (sqrt (in_normal.normal_x * in_normal.normal_x + in_normal.normal_y * in_normal.normal_y));
1220 A = in_normal.normal_y / denom_Z;
1221 B = sign * in_normal.normal_x / denom_Z;
1222 rotation_matrix_Z << A, -
B, 0.0f,
1226 result = rotation_matrix_X * rotation_matrix_Z;
1232 template <
int FeatureSize,
typename Po
intT,
typename NormalT>
void 1235 io_vec = in_transform * io_vec;
1239 template <
int FeatureSize,
typename Po
intT,
typename NormalT>
void 1264 template <
int FeatureSize,
typename Po
intT,
typename NormalT>
double 1266 const Eigen::MatrixXf& points_to_cluster,
1267 int number_of_clusters,
1268 Eigen::MatrixXi& io_labels,
1272 Eigen::MatrixXf& cluster_centers)
1274 const int spp_trials = 3;
1275 size_t number_of_points = points_to_cluster.rows () > 1 ? points_to_cluster.rows () : points_to_cluster.cols ();
1276 int feature_dimension = points_to_cluster.rows () > 1 ? FeatureSize : 1;
1278 attempts = std::max (attempts, 1);
1279 srand (static_cast<unsigned int> (time (0)));
1281 Eigen::MatrixXi labels (number_of_points, 1);
1288 Eigen::MatrixXf centers (number_of_clusters, feature_dimension);
1289 Eigen::MatrixXf old_centers (number_of_clusters, feature_dimension);
1290 std::vector<int> counters (number_of_clusters);
1291 std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> > boxes (feature_dimension);
1292 Eigen::Vector2f* box = &boxes[0];
1294 double best_compactness = std::numeric_limits<double>::max ();
1295 double compactness = 0.0;
1297 if (criteria.type_ & TermCriteria::EPS)
1298 criteria.epsilon_ = std::max (criteria.epsilon_, 0.0f);
1300 criteria.epsilon_ = std::numeric_limits<float>::epsilon ();
1302 criteria.epsilon_ *= criteria.epsilon_;
1304 if (criteria.type_ & TermCriteria::COUNT)
1305 criteria.max_count_ = std::min (std::max (criteria.max_count_, 2), 100);
1307 criteria.max_count_ = 100;
1309 if (number_of_clusters == 1)
1312 criteria.max_count_ = 2;
1315 for (
int i_dim = 0; i_dim < feature_dimension; i_dim++)
1316 box[i_dim] = Eigen::Vector2f (points_to_cluster (0, i_dim), points_to_cluster (0, i_dim));
1318 for (
unsigned int i_point = 0; i_point < number_of_points; i_point++)
1319 for (
int i_dim = 0; i_dim < feature_dimension; i_dim++)
1321 float v = points_to_cluster (i_point, i_dim);
1322 box[i_dim] (0) = std::min (box[i_dim] (0), v);
1323 box[i_dim] (1) = std::max (box[i_dim] (1), v);
1326 for (
int i_attempt = 0; i_attempt < attempts; i_attempt++)
1328 float max_center_shift = std::numeric_limits<float>::max ();
1329 for (
int iter = 0; iter < criteria.max_count_ && max_center_shift > criteria.epsilon_; iter++)
1331 Eigen::MatrixXf temp (centers.rows (), centers.cols ());
1333 centers = old_centers;
1336 if ( iter == 0 && ( i_attempt > 0 || !(flags & USE_INITIAL_LABELS) ) )
1342 for (
int i_cl_center = 0; i_cl_center < number_of_clusters; i_cl_center++)
1344 Eigen::VectorXf center (feature_dimension);
1346 for (
int i_dim = 0; i_dim < feature_dimension; i_dim++)
1347 centers (i_cl_center, i_dim) = center (i_dim);
1354 for (
int i_cluster = 0; i_cluster < number_of_clusters; i_cluster++)
1355 counters[i_cluster] = 0;
1356 for (
unsigned int i_point = 0; i_point < number_of_points; i_point++)
1358 int i_label = labels (i_point, 0);
1359 for (
int i_dim = 0; i_dim < feature_dimension; i_dim++)
1360 centers (i_label, i_dim) += points_to_cluster (i_point, i_dim);
1361 counters[i_label]++;
1364 max_center_shift = 0.0f;
1365 for (
int i_cl_center = 0; i_cl_center < number_of_clusters; i_cl_center++)
1367 if (counters[i_cl_center] != 0)
1369 float scale = 1.0f /
static_cast<float> (counters[i_cl_center]);
1370 for (
int i_dim = 0; i_dim < feature_dimension; i_dim++)
1371 centers (i_cl_center, i_dim) *= scale;
1375 Eigen::VectorXf center (feature_dimension);
1377 for(
int i_dim = 0; i_dim < feature_dimension; i_dim++)
1378 centers (i_cl_center, i_dim) = center (i_dim);
1384 for (
int i_dim = 0; i_dim < feature_dimension; i_dim++)
1386 float diff = centers (i_cl_center, i_dim) - old_centers (i_cl_center, i_dim);
1387 dist += diff * diff;
1389 max_center_shift = std::max (max_center_shift, dist);
1394 for (
unsigned int i_point = 0; i_point < number_of_points; i_point++)
1396 Eigen::VectorXf sample (feature_dimension);
1397 sample = points_to_cluster.row (i_point);
1400 float min_dist = std::numeric_limits<float>::max ();
1402 for (
int i_cluster = 0; i_cluster < number_of_clusters; i_cluster++)
1404 Eigen::VectorXf center (feature_dimension);
1405 center = centers.row (i_cluster);
1407 if (min_dist > dist)
1413 compactness += min_dist;
1414 labels (i_point, 0) = k_best;
1418 if (compactness < best_compactness)
1420 best_compactness = compactness;
1421 cluster_centers = centers;
1426 return (best_compactness);
1430 template <
int FeatureSize,
typename Po
intT,
typename NormalT>
void 1432 const Eigen::MatrixXf& data,
1433 Eigen::MatrixXf& out_centers,
1434 int number_of_clusters,
1437 size_t dimension = data.cols ();
1438 unsigned int number_of_points =
static_cast<unsigned int> (data.rows ());
1439 std::vector<int> centers_vec (number_of_clusters);
1440 int* centers = ¢ers_vec[0];
1441 std::vector<double> dist (number_of_points);
1442 std::vector<double> tdist (number_of_points);
1443 std::vector<double> tdist2 (number_of_points);
1446 unsigned int random_unsigned = rand ();
1447 centers[0] = random_unsigned % number_of_points;
1449 for (
unsigned int i_point = 0; i_point < number_of_points; i_point++)
1451 Eigen::VectorXf first (dimension);
1452 Eigen::VectorXf second (dimension);
1453 first = data.row (i_point);
1454 second = data.row (centers[0]);
1456 sum0 += dist[i_point];
1459 for (
int i_cluster = 0; i_cluster < number_of_clusters; i_cluster++)
1461 double best_sum = std::numeric_limits<double>::max ();
1462 int best_center = -1;
1463 for (
int i_trials = 0; i_trials < trials; i_trials++)
1465 unsigned int random_integer = rand () - 1;
1466 double random_double =
static_cast<double> (random_integer) / static_cast<double> (std::numeric_limits<unsigned int>::max ());
1467 double p = random_double * sum0;
1469 unsigned int i_point;
1470 for (i_point = 0; i_point < number_of_points - 1; i_point++)
1471 if ( (p -= dist[i_point]) <= 0.0)
1477 for (
unsigned int i_point = 0; i_point < number_of_points; i_point++)
1479 Eigen::VectorXf first (dimension);
1480 Eigen::VectorXf second (dimension);
1481 first = data.row (i_point);
1482 second = data.row (ci);
1483 tdist2[i_point] = std::min (static_cast<double> (
computeDistance (first, second)), dist[i_point]);
1484 s += tdist2[i_point];
1491 std::swap (tdist, tdist2);
1495 centers[i_cluster] = best_center;
1497 std::swap (dist, tdist);
1500 for (
int i_cluster = 0; i_cluster < number_of_clusters; i_cluster++)
1501 for (
unsigned int i_dim = 0; i_dim < dimension; i_dim++)
1502 out_centers (i_cluster, i_dim) = data (centers[i_cluster], i_dim);
1506 template <
int FeatureSize,
typename Po
intT,
typename NormalT>
void 1508 Eigen::VectorXf& center)
1510 size_t dimension = boxes.size ();
1511 float margin = 1.0f /
static_cast<float> (dimension);
1513 for (
unsigned int i_dim = 0; i_dim < dimension; i_dim++)
1515 unsigned int random_integer = rand () - 1;
1516 float random_float =
static_cast<float> (random_integer) / static_cast<float> (std::numeric_limits<unsigned int>::max ());
1517 center (i_dim) = (random_float * (1.0f + margin * 2.0f)- margin) * (boxes[i_dim] (1) - boxes[i_dim] (0)) + boxes[i_dim] (0);
1522 template <
int FeatureSize,
typename Po
intT,
typename NormalT>
float 1525 size_t dimension = vec_1.rows () > 1 ? vec_1.rows () : vec_1.cols ();
1526 float distance = 0.0f;
1527 for(
unsigned int i_dim = 0; i_dim < dimension; i_dim++)
1529 float diff = vec_1 (i_dim) - vec_2 (i_dim);
1530 distance += diff * diff;
1536 #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.
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.
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 setFeatureEstimator(boost::shared_ptr< pcl::Feature< PointT, pcl::Histogram< FeatureSize > > > feature)
Changes the feature estimator.
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.
VectorType::iterator iterator
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.
boost::shared_ptr< pcl::Feature< PointT, pcl::Histogram< FeatureSize > > > feature_estimator_
Stores the feature estimator.
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 beetween 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.
int getCentroidIndex(const PointT &p)
Returns the index in the resulting downsampled cloud of the specified point.
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 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.
void filter(PointCloud &output)
std::vector< typename pcl::PointCloud< NormalT >::Ptr > getTrainingNormals()
This method returns the coresponding 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).
boost::shared_ptr< pcl::Feature< PointT, pcl::Histogram< FeatureSize > > > getFeatureEstimator()
Returns the current feature estimator used for extraction of the descriptors.
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.
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.
Feature represents the base feature class.
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...