40 #ifndef PCL_RECOGNITION_HOUGH_3D_IMPL_H_
41 #define PCL_RECOGNITION_HOUGH_3D_IMPL_H_
43 #include <pcl/common/io.h>
44 #include <pcl/recognition/cg/hough_3d.h>
45 #include <pcl/registration/correspondence_types.h>
46 #include <pcl/registration/correspondence_rejection_sample_consensus.h>
49 #include <pcl/features/normal_3d.h>
50 #include <pcl/features/board.h>
53 template<
typename Po
intModelT,
typename Po
intSceneT,
typename Po
intModelRfT,
typename Po
intSceneRfT>
54 template<
typename Po
intType,
typename Po
intRfType>
void
57 if (local_rf_search_radius_ == 0)
59 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");
60 local_rf_search_radius_ =
static_cast<float> (hough_bin_size_);
65 if (local_rf_normals_search_radius_ <= 0.0f)
73 norm_est.
compute (*normal_cloud);
84 template<
typename Po
intModelT,
typename Po
intSceneT,
typename Po
intModelRfT,
typename Po
intSceneRfT>
bool
89 PCL_ERROR (
"[pcl::Hough3DGrouping::train()] Error! Input cloud not set.\n");
96 computeRf<PointModelT, PointModelRfT> (input_, *new_input_rf);
97 input_rf_ = new_input_rf;
103 if (input_->size () != input_rf_->size ())
105 PCL_ERROR (
"[pcl::Hough3DGrouping::train()] Error! Input cloud size != Input RF cloud size.\n");
109 model_votes_.clear ();
110 model_votes_.resize (input_->size ());
113 Eigen::Vector3f centroid (0, 0, 0);
114 for (std::size_t i = 0; i < input_->size (); ++i)
116 centroid += input_->at (i).getVector3fMap ();
118 centroid /=
static_cast<float> (input_->size ());
121 for (std::size_t i = 0; i < input_->size (); ++i)
123 Eigen::Vector3f x_ax ((*input_rf_)[i].x_axis[0], (*input_rf_)[i].x_axis[1], (*input_rf_)[i].x_axis[2]);
124 Eigen::Vector3f y_ax ((*input_rf_)[i].y_axis[0], (*input_rf_)[i].y_axis[1], (*input_rf_)[i].y_axis[2]);
125 Eigen::Vector3f z_ax ((*input_rf_)[i].z_axis[0], (*input_rf_)[i].z_axis[1], (*input_rf_)[i].z_axis[2]);
127 model_votes_[i].x () = x_ax.dot (centroid - input_->at (i).getVector3fMap ());
128 model_votes_[i].y () = y_ax.dot (centroid - input_->at (i).getVector3fMap ());
129 model_votes_[i].z () = z_ax.dot (centroid - input_->at (i).getVector3fMap ());
132 needs_training_ =
false;
137 template<
typename Po
intModelT,
typename Po
intSceneT,
typename Po
intModelRfT,
typename Po
intSceneRfT>
bool
156 computeRf<PointSceneT, PointSceneRfT> (scene_, *new_scene_rf);
157 scene_rf_ = new_scene_rf;
163 if (scene_->size () != scene_rf_->size ())
165 PCL_ERROR (
"[pcl::Hough3DGrouping::recognizeModelInstances()] Error! Scene cloud size != Scene RF cloud size.\n");
169 if (!model_scene_corrs_)
171 PCL_ERROR (
"[pcl::Hough3DGrouping::recognizeModelInstances()] Error! Correspondences not set, please set them before calling again this function.\n");
175 int n_matches =
static_cast<int> (model_scene_corrs_->size ());
181 std::vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d> > scene_votes (n_matches);
182 Eigen::Vector3d d_min, d_max, bin_size;
184 d_min.setConstant (std::numeric_limits<double>::max ());
185 d_max.setConstant (-std::numeric_limits<double>::max ());
186 bin_size.setConstant (hough_bin_size_);
188 float max_distance = -std::numeric_limits<float>::max ();
191 for (
int i=0; i< n_matches; ++i)
193 int scene_index = model_scene_corrs_->at (i).index_match;
194 int model_index = model_scene_corrs_->at (i).index_query;
196 const Eigen::Vector3f& scene_point = scene_->at (scene_index).getVector3fMap ();
197 const PointSceneRfT& scene_point_rf = scene_rf_->at (scene_index);
199 Eigen::Vector3f scene_point_rf_x (scene_point_rf.x_axis[0], scene_point_rf.x_axis[1], scene_point_rf.x_axis[2]);
200 Eigen::Vector3f scene_point_rf_y (scene_point_rf.y_axis[0], scene_point_rf.y_axis[1], scene_point_rf.y_axis[2]);
201 Eigen::Vector3f scene_point_rf_z (scene_point_rf.z_axis[0], scene_point_rf.z_axis[1], scene_point_rf.z_axis[2]);
204 const Eigen::Vector3f& model_point_vote = model_votes_[model_index];
206 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 ();
207 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 ();
208 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 ();
210 if (scene_votes[i].x () < d_min.x ())
211 d_min.x () = scene_votes[i].x ();
212 if (scene_votes[i].x () > d_max.x ())
213 d_max.x () = scene_votes[i].x ();
215 if (scene_votes[i].y () < d_min.y ())
216 d_min.y () = scene_votes[i].y ();
217 if (scene_votes[i].y () > d_max.y ())
218 d_max.y () = scene_votes[i].y ();
220 if (scene_votes[i].z () < d_min.z ())
221 d_min.z () = scene_votes[i].z ();
222 if (scene_votes[i].z () > d_max.z ())
223 d_max.z () = scene_votes[i].z ();
226 if (use_interpolation_ && max_distance < model_scene_corrs_->at (i).
distance)
228 max_distance = model_scene_corrs_->at (i).distance;
235 for (
int i = 0; i < n_matches; ++i)
238 if (use_distance_weight_ && max_distance != 0)
240 weight = 1.0 - (model_scene_corrs_->at (i).distance / max_distance);
242 if (use_interpolation_)
244 hough_space_->voteInt (scene_votes[i], weight, i);
248 hough_space_->vote (scene_votes[i], weight, i);
252 hough_space_initialized_ =
true;
258 template<
typename Po
intModelT,
typename Po
intSceneT,
typename Po
intModelRfT,
typename Po
intSceneRfT>
void
261 model_instances.clear ();
262 found_transformations_.clear ();
264 if (!hough_space_initialized_ && !houghVoting ())
270 std::vector<double> max_values;
271 std::vector<std::vector<int> > max_ids;
273 hough_space_->findMaxima (hough_threshold_, max_values, max_ids);
286 for (std::size_t j = 0; j < max_values.size (); ++j)
289 for (
const int &i : max_ids[j])
291 temp_corrs.push_back (model_scene_corrs_->at (i));
298 model_instances.push_back (filtered_corrs);
333 template <
typename Po
intModelT,
typename Po
intSceneT,
typename Po
intModelRfT,
typename Po
intSceneRfT>
bool
335 std::vector<Eigen::Matrix4f, Eigen::aligned_allocator<Eigen::Matrix4f> > &transformations)
337 std::vector<pcl::Correspondences> model_instances;
338 return (this->recognize (transformations, model_instances));
342 template <
typename Po
intModelT,
typename Po
intSceneT,
typename Po
intModelRfT,
typename Po
intSceneRfT>
bool
344 std::vector<Eigen::Matrix4f, Eigen::aligned_allocator<Eigen::Matrix4f> > &transformations, std::vector<pcl::Correspondences> &clustered_corrs)
346 transformations.clear ();
347 if (!this->initCompute ())
349 PCL_ERROR (
"[pcl::Hough3DGrouping::recognize()] Error! Model cloud or Scene cloud not set, please set them before calling again this function.\n");
353 clusterCorrespondences (clustered_corrs);
355 transformations = found_transformations_;
368 this->deinitCompute ();
369 return (transformations.size ());
373 #define PCL_INSTANTIATE_Hough3DGrouping(T,ST,RFT,SRFT) template class PCL_EXPORTS pcl::Hough3DGrouping<T,ST,RFT,SRFT>;
375 #endif // PCL_RECOGNITION_HOUGH_3D_IMPL_H_