40 #ifndef PCL_RECOGNITION_HOUGH_3D_IMPL_H_
41 #define PCL_RECOGNITION_HOUGH_3D_IMPL_H_
43 #include <pcl/recognition/cg/hough_3d.h>
44 #include <pcl/registration/correspondence_types.h>
45 #include <pcl/registration/correspondence_rejection_sample_consensus.h>
48 #include <pcl/features/normal_3d.h>
49 #include <pcl/features/board.h>
52 template<
typename Po
intModelT,
typename Po
intSceneT,
typename Po
intModelRfT,
typename Po
intSceneRfT>
53 template<
typename Po
intType,
typename Po
intRfType>
void
56 if (local_rf_search_radius_ == 0)
58 PCL_WARN (
"[pcl::Hough3DGrouping::computeRf()] Warning! Reference frame search radius not set. Computing with default value. Results might be incorrect, algorithm might be slow.\n");
59 local_rf_search_radius_ =
static_cast<float> (hough_bin_size_);
64 if (local_rf_normals_search_radius_ <= 0.0f)
72 norm_est.
compute (*normal_cloud);
83 template<
typename Po
intModelT,
typename Po
intSceneT,
typename Po
intModelRfT,
typename Po
intSceneRfT>
bool
88 PCL_ERROR (
"[pcl::Hough3DGrouping::train()] Error! Input cloud not set.\n");
95 computeRf<PointModelT, PointModelRfT> (input_, *new_input_rf);
96 input_rf_ = new_input_rf;
102 if (input_->size () != input_rf_->size ())
104 PCL_ERROR (
"[pcl::Hough3DGrouping::train()] Error! Input cloud size != Input RF cloud size.\n");
108 model_votes_.clear ();
109 model_votes_.resize (input_->size ());
112 Eigen::Vector3f centroid (0, 0, 0);
113 for (std::size_t i = 0; i < input_->size (); ++i)
115 centroid += input_->at (i).getVector3fMap ();
117 centroid /=
static_cast<float> (input_->size ());
120 for (std::size_t i = 0; i < input_->size (); ++i)
122 Eigen::Vector3f x_ax ((*input_rf_)[i].x_axis[0], (*input_rf_)[i].x_axis[1], (*input_rf_)[i].x_axis[2]);
123 Eigen::Vector3f y_ax ((*input_rf_)[i].y_axis[0], (*input_rf_)[i].y_axis[1], (*input_rf_)[i].y_axis[2]);
124 Eigen::Vector3f z_ax ((*input_rf_)[i].z_axis[0], (*input_rf_)[i].z_axis[1], (*input_rf_)[i].z_axis[2]);
126 model_votes_[i].x () = x_ax.dot (centroid - input_->at (i).getVector3fMap ());
127 model_votes_[i].y () = y_ax.dot (centroid - input_->at (i).getVector3fMap ());
128 model_votes_[i].z () = z_ax.dot (centroid - input_->at (i).getVector3fMap ());
131 needs_training_ =
false;
136 template<
typename Po
intModelT,
typename Po
intSceneT,
typename Po
intModelRfT,
typename Po
intSceneRfT>
bool
155 computeRf<PointSceneT, PointSceneRfT> (scene_, *new_scene_rf);
156 scene_rf_ = new_scene_rf;
162 if (scene_->size () != scene_rf_->size ())
164 PCL_ERROR (
"[pcl::Hough3DGrouping::recognizeModelInstances()] Error! Scene cloud size != Scene RF cloud size.\n");
168 if (!model_scene_corrs_)
170 PCL_ERROR (
"[pcl::Hough3DGrouping::recognizeModelInstances()] Error! Correspondences not set, please set them before calling again this function.\n");
174 int n_matches =
static_cast<int> (model_scene_corrs_->size ());
180 std::vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d> > scene_votes (n_matches);
181 Eigen::Vector3d d_min, d_max, bin_size;
183 d_min.setConstant (std::numeric_limits<double>::max ());
184 d_max.setConstant (-std::numeric_limits<double>::max ());
185 bin_size.setConstant (hough_bin_size_);
187 float max_distance = -std::numeric_limits<float>::max ();
190 for (
int i=0; i< n_matches; ++i)
192 int scene_index = model_scene_corrs_->at (i).index_match;
193 int model_index = model_scene_corrs_->at (i).index_query;
195 const Eigen::Vector3f& scene_point = scene_->at (scene_index).getVector3fMap ();
196 const PointSceneRfT& scene_point_rf = scene_rf_->at (scene_index);
198 Eigen::Vector3f scene_point_rf_x (scene_point_rf.x_axis[0], scene_point_rf.x_axis[1], scene_point_rf.x_axis[2]);
199 Eigen::Vector3f scene_point_rf_y (scene_point_rf.y_axis[0], scene_point_rf.y_axis[1], scene_point_rf.y_axis[2]);
200 Eigen::Vector3f scene_point_rf_z (scene_point_rf.z_axis[0], scene_point_rf.z_axis[1], scene_point_rf.z_axis[2]);
203 const Eigen::Vector3f& model_point_vote = model_votes_[model_index];
205 scene_votes[i].x () = scene_point_rf_x[0] * model_point_vote.x () + scene_point_rf_y[0] * model_point_vote.y () + scene_point_rf_z[0] * model_point_vote.z () + scene_point.x ();
206 scene_votes[i].y () = scene_point_rf_x[1] * model_point_vote.x () + scene_point_rf_y[1] * model_point_vote.y () + scene_point_rf_z[1] * model_point_vote.z () + scene_point.y ();
207 scene_votes[i].z () = scene_point_rf_x[2] * model_point_vote.x () + scene_point_rf_y[2] * model_point_vote.y () + scene_point_rf_z[2] * model_point_vote.z () + scene_point.z ();
209 if (scene_votes[i].x () < d_min.x ())
210 d_min.x () = scene_votes[i].x ();
211 if (scene_votes[i].x () > d_max.x ())
212 d_max.x () = scene_votes[i].x ();
214 if (scene_votes[i].y () < d_min.y ())
215 d_min.y () = scene_votes[i].y ();
216 if (scene_votes[i].y () > d_max.y ())
217 d_max.y () = scene_votes[i].y ();
219 if (scene_votes[i].z () < d_min.z ())
220 d_min.z () = scene_votes[i].z ();
221 if (scene_votes[i].z () > d_max.z ())
222 d_max.z () = scene_votes[i].z ();
225 if (use_interpolation_ && max_distance < model_scene_corrs_->at (i).
distance)
227 max_distance = model_scene_corrs_->at (i).distance;
234 for (
int i = 0; i < n_matches; ++i)
237 if (use_distance_weight_ && max_distance != 0)
239 weight = 1.0 - (model_scene_corrs_->at (i).distance / max_distance);
241 if (use_interpolation_)
243 hough_space_->voteInt (scene_votes[i], weight, i);
247 hough_space_->vote (scene_votes[i], weight, i);
251 hough_space_initialized_ =
true;
257 template<
typename Po
intModelT,
typename Po
intSceneT,
typename Po
intModelRfT,
typename Po
intSceneRfT>
void
260 model_instances.clear ();
261 found_transformations_.clear ();
263 if (!hough_space_initialized_ && !houghVoting ())
269 std::vector<double> max_values;
270 std::vector<std::vector<int> > max_ids;
272 hough_space_->findMaxima (hough_threshold_, max_values, max_ids);
285 for (std::size_t j = 0; j < max_values.size (); ++j)
288 for (
const int &i : max_ids[j])
290 temp_corrs.push_back (model_scene_corrs_->at (i));
297 model_instances.push_back (filtered_corrs);
332 template <
typename Po
intModelT,
typename Po
intSceneT,
typename Po
intModelRfT,
typename Po
intSceneRfT>
bool
334 std::vector<Eigen::Matrix4f, Eigen::aligned_allocator<Eigen::Matrix4f> > &transformations)
336 std::vector<pcl::Correspondences> model_instances;
337 return (this->recognize (transformations, model_instances));
341 template <
typename Po
intModelT,
typename Po
intSceneT,
typename Po
intModelRfT,
typename Po
intSceneRfT>
bool
343 std::vector<Eigen::Matrix4f, Eigen::aligned_allocator<Eigen::Matrix4f> > &transformations, std::vector<pcl::Correspondences> &clustered_corrs)
345 transformations.clear ();
346 if (!this->initCompute ())
348 PCL_ERROR (
"[pcl::Hough3DGrouping::recognize()] Error! Model cloud or Scene cloud not set, please set them before calling again this function.\n");
352 clusterCorrespondences (clustered_corrs);
354 transformations = found_transformations_;
367 this->deinitCompute ();
368 return (transformations.size ());
372 #define PCL_INSTANTIATE_Hough3DGrouping(T,ST,RFT,SRFT) template class PCL_EXPORTS pcl::Hough3DGrouping<T,ST,RFT,SRFT>;
374 #endif // PCL_RECOGNITION_HOUGH_3D_IMPL_H_