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>
53template<
typename Po
intModelT,
typename Po
intSceneT,
typename Po
intModelRfT,
typename Po
intSceneRfT>
54template<
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)
71 norm_est.setRadiusSearch (local_rf_normals_search_radius_);
78 rf_est.setFindHoles (
true);
79 rf_est.setRadiusSearch (local_rf_search_radius_);
84template<
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");
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;
137template<
typename Po
intModelT,
typename Po
intSceneT,
typename Po
intModelRfT,
typename Po
intSceneRfT>
bool
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);
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 ();
193 int scene_index = model_scene_corrs_->at (i).index_match;
194 int model_index = model_scene_corrs_->at (i).index_query;
240 weight = 1.0 - (model_scene_corrs_->at (i).distance /
max_distance);
242 if (use_interpolation_)
252 hough_space_initialized_ =
true;
258template<
typename Po
intModelT,
typename Po
intSceneT,
typename Po
intModelRfT,
typename Po
intSceneRfT>
void
262 found_transformations_.clear ();
264 if (!hough_space_initialized_ && !houghVoting ())
271 std::vector<std::vector<int> >
max_ids;
289 for (
const int &i :
max_ids[j])
291 temp_corrs.push_back (model_scene_corrs_->at (i));
296 found_transformations_.push_back (
corr_rejector.getBestTransformation ());
333template <
typename Po
intModelT,
typename Po
intSceneT,
typename Po
intModelRfT,
typename Po
intSceneRfT>
bool
335 std::vector<Eigen::Matrix4f, Eigen::aligned_allocator<Eigen::Matrix4f> > &
transformations)
342template <
typename Po
intModelT,
typename Po
intSceneT,
typename Po
intModelRfT,
typename Po
intSceneRfT>
bool
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");
368 this->deinitCompute ();
373#define PCL_INSTANTIATE_Hough3DGrouping(T,ST,RFT,SRFT) template class PCL_EXPORTS pcl::Hough3DGrouping<T,ST,RFT,SRFT>;
Iterator class for point clouds with or without given indices.
ConstCloudIterator(const PointCloud< PointT > &cloud)
std::size_t size() const
Size of the range the iterator is going through.
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.
typename ModelRfCloud::Ptr ModelRfCloudPtr
bool houghVoting()
The Hough space voting procedure.
bool train()
Call this function after setting the input, the input_rf and the hough_bin_size parameters to perform...
void computeRf(const typename pcl::PointCloud< PointType >::ConstPtr &input, pcl::PointCloud< PointRfType > &rf)
Computes the reference frame for an input cloud.
typename PointCloud::Ptr PointCloudPtr
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.
shared_ptr< PointCloud< PointT > > Ptr
shared_ptr< const PointCloud< PointT > > ConstPtr
HoughSpace3D is a 3D voting space.
CorrespondenceRejectorSampleConsensus implements a correspondence rejection using Random Sample Conse...
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.
std::vector< pcl::Correspondence, Eigen::aligned_allocator< pcl::Correspondence > > Correspondences