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 (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 (
size_t i = 0; i < input_->size (); ++i)
115 centroid += input_->at (i).getVector3fMap ();
117 centroid /=
static_cast<float> (input_->size ());
120 for (
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 (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> 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);
277 pcl::copyPointCloud<PointSceneT, PointModelT> (*scene_, *temp_scene_cloud_ptr);
281 corr_rejector.setInlierThreshold (hough_bin_size_);
282 corr_rejector.setInputSource (input_);
283 corr_rejector.setInputTarget (temp_scene_cloud_ptr);
285 for (
size_t j = 0; j < max_values.size (); ++j)
288 for (
size_t i = 0; i < max_ids[j].size (); ++i)
290 temp_corrs.push_back (model_scene_corrs_->at (max_ids[j][i]));
293 corr_rejector.getRemainingCorrespondences (temp_corrs, filtered_corrs);
295 found_transformations_.push_back (corr_rejector.getBestTransformation ());
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 ();
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_
NormalEstimation estimates local surface properties (surface normals and curvatures)at each 3D point...
std::vector< pcl::Correspondence, Eigen::aligned_allocator< pcl::Correspondence > > Correspondences
bool recognize(std::vector< Eigen::Matrix4f, Eigen::aligned_allocator< Eigen::Matrix4f > > &transformations)
The main function, recognizes instances of the model into the scene set by the user.
HoughSpace3D is a 3D voting space.
void setKSearch(int k)
Set the number of k nearest neighbors to use for the feature estimation.
ModelRfCloud::Ptr ModelRfCloudPtr
virtual void setInputCloud(const PointCloudConstPtr &cloud)
Provide a pointer to the input dataset.
bool train()
Call this function after setting the input, the input_rf and the hough_bin_size parameters to perform...
void clusterCorrespondences(std::vector< Correspondences > &model_instances)
Cluster the input correspondences in order to distinguish between different instances of the model in...
virtual void setInputCloud(const PointCloudConstPtr &cloud)
Provide a pointer to the input dataset.
CorrespondenceRejectorSampleConsensus implements a correspondence rejection using Random Sample Conse...
void setRadiusSearch(double radius)
Set the sphere radius that is to be used for determining the nearest neighbors used for the feature e...
void setInputNormals(const PointCloudNConstPtr &normals)
Provide a pointer to the input dataset that contains the point normals of the XYZ dataset...
void compute(PointCloudOut &output)
Base method for feature estimation for all points given in using th...
void setMaximumIterations(int max_iterations)
Set the maximum number of iterations.
BOARDLocalReferenceFrameEstimation implements the BOrder Aware Repeatable Directions algorithm for lo...
bool houghVoting()
Finds the transformation matrix between the input and the scene cloud for a set of correspondences us...
boost::shared_ptr< PointCloud< PointT > > Ptr
PointCloud represents the base class in PCL for storing collections of 3D points. ...
PointCloud::Ptr PointCloudPtr
void setFindHoles(bool find_holes)
Sets whether holes in the margin of the support, for each point, are searched and accounted for in th...
void computeRf(const boost::shared_ptr< const pcl::PointCloud< PointType > > &input, pcl::PointCloud< PointRfType > &rf)
Computes the reference frame for an input cloud.