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_ bool train()
Call this function after setting the input, the input_rf and the hough_bin_size parameters to perform...
shared_ptr< PointCloud< PointT > > Ptr
typename PointCloud::Ptr PointCloudPtr
typename ModelRfCloud::Ptr ModelRfCloudPtr
void setRadiusSearch(double radius)
Set the sphere radius that is to be used for determining the nearest neighbors used for the feature e...
NormalEstimation estimates local surface properties (surface normals and curvatures)at each 3D point...
bool houghVoting()
The Hough space voting procedure.
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.
void setFindHoles(bool find_holes)
Sets whether holes in the margin of the support, for each point, are searched and accounted for in th...
Eigen::Matrix4f getBestTransformation()
Get the best transformation after RANSAC rejection.
virtual void setInputTarget(const PointCloudConstPtr &cloud)
Provide a target point cloud dataset (must contain XYZ data!)
void computeRf(const typename pcl::PointCloud< PointType >::ConstPtr &input, pcl::PointCloud< PointRfType > &rf)
Computes the reference frame for an input cloud.
void copyPointCloud(const pcl::PointCloud< PointInT > &cloud_in, pcl::PointCloud< PointOutT > &cloud_out)
Copy all the fields from a given point cloud into a new point cloud.
void setMaximumIterations(int max_iterations)
Set the maximum number of iterations.
float distance(const PointT &p1, const PointT &p2)
void setInlierThreshold(double threshold)
Set the maximum distance between corresponding points.
void clusterCorrespondences(std::vector< Correspondences > &model_instances) override
Cluster the input correspondences in order to distinguish between different instances of the model in...
PointCloud represents the base class in PCL for storing collections of 3D points. ...
CorrespondenceRejectorSampleConsensus implements a correspondence rejection using Random Sample Conse...
virtual void setInputCloud(const PointCloudConstPtr &cloud)
Provide a pointer to the input dataset.
shared_ptr< const PointCloud< PointT > > ConstPtr
virtual void setInputSource(const PointCloudConstPtr &cloud)
Provide a source point cloud dataset (must contain XYZ data!)
std::vector< pcl::Correspondence, Eigen::aligned_allocator< pcl::Correspondence > > Correspondences
void compute(PointCloudOut &output)
Base method for feature estimation for all points given in <setInputCloud (), setIndices ()> using th...
void getRemainingCorrespondences(const pcl::Correspondences &original_correspondences, pcl::Correspondences &remaining_correspondences) override
Get a list of valid correspondences after rejection from the original set of correspondences.
BOARDLocalReferenceFrameEstimation implements the BOrder Aware Repeatable Directions algorithm for lo...
void setKSearch(int k)
Set the number of k nearest neighbors to use for the feature estimation.
void setInputCloud(const PointCloudConstPtr &cloud) override
Provide a pointer to the input dataset.
void setInputNormals(const PointCloudNConstPtr &normals)
Provide a pointer to the input dataset that contains the point normals of the XYZ dataset...
HoughSpace3D is a 3D voting space.