38 #ifndef PCL_KEYPOINTS_IMPL_SMOOTHEDSURFACESKEYPOINT_H_ 39 #define PCL_KEYPOINTS_IMPL_SMOOTHEDSURFACESKEYPOINT_H_ 41 #include <pcl/keypoints/smoothed_surfaces_keypoint.h> 42 #include <pcl/kdtree/kdtree_flann.h> 47 template <
typename Po
intT,
typename Po
intNT>
void 53 clouds_.push_back (cloud);
54 cloud_normals_.push_back (normals);
55 cloud_trees_.push_back (kdtree);
56 scales_.push_back (std::pair<float, size_t> (scale, scales_.size ()));
61 template <
typename Po
intT,
typename Po
intNT>
void 65 cloud_normals_.clear ();
71 template <
typename Po
intT,
typename Po
intNT>
void 75 std::vector<std::vector<float> > diffs (scales_.size ());
78 std::vector<float> aux_diffs (input_->points.size (), 0.0f);
79 diffs[scales_[0].second] = aux_diffs;
81 cloud_trees_[scales_[0].second]->setInputCloud (clouds_[scales_[0].second]);
82 for (
size_t scale_i = 1; scale_i < clouds_.size (); ++scale_i)
84 size_t cloud_i = scales_[scale_i].second,
85 cloud_i_minus_one = scales_[scale_i - 1].second;
86 diffs[cloud_i].resize (input_->points.size ());
87 PCL_INFO (
"cloud_i %u cloud_i_minus_one %u\n", cloud_i, cloud_i_minus_one);
88 for (
size_t point_i = 0; point_i < input_->points.size (); ++point_i)
89 diffs[cloud_i][point_i] = cloud_normals_[cloud_i]->points[point_i].getNormalVector3fMap ().dot (
90 clouds_[cloud_i]->points[point_i].getVector3fMap () - clouds_[cloud_i_minus_one]->points[point_i].getVector3fMap ());
93 cloud_trees_[cloud_i]->setInputCloud (clouds_[cloud_i]);
99 for (
int point_i = 0; point_i < static_cast<int> (input_->points.size ()); ++point_i)
101 std::vector<int> nn_indices;
102 std::vector<float> nn_distances;
103 input_tree->
radiusSearch (point_i, input_scale_ * neighborhood_constant_, nn_indices, nn_distances);
105 bool is_min =
true, is_max =
true;
106 for (std::vector<int>::iterator nn_it = nn_indices.begin (); nn_it != nn_indices.end (); ++nn_it)
107 if (*nn_it != point_i)
109 if (diffs[input_index_][point_i] < diffs[input_index_][*nn_it])
111 else if (diffs[input_index_][point_i] > diffs[input_index_][*nn_it])
116 if (is_min || is_max)
118 bool passed_min =
true, passed_max =
true;
119 for (
size_t scale_i = 0; scale_i < scales_.size (); ++scale_i)
121 size_t cloud_i = scales_[scale_i].second;
123 if (cloud_i == clouds_.size () - 1)
126 nn_indices.clear (); nn_distances.clear ();
127 cloud_trees_[cloud_i]->radiusSearch (point_i, scales_[scale_i].first * neighborhood_constant_, nn_indices, nn_distances);
129 bool is_min_other_scale =
true, is_max_other_scale =
true;
130 for (std::vector<int>::iterator nn_it = nn_indices.begin (); nn_it != nn_indices.end (); ++nn_it)
131 if (*nn_it != point_i)
133 if (diffs[input_index_][point_i] < diffs[cloud_i][*nn_it])
134 is_max_other_scale =
false;
135 else if (diffs[input_index_][point_i] > diffs[cloud_i][*nn_it])
136 is_min_other_scale =
false;
139 if (is_min ==
true && is_min_other_scale ==
false)
141 if (is_max ==
true && is_max_other_scale ==
false)
144 if (!passed_min && !passed_max)
149 if (passed_min || passed_max)
151 output.
points.push_back (input_->points[point_i]);
152 keypoints_indices_->indices.push_back (point_i);
157 output.
header = input_->header;
158 output.
width =
static_cast<uint32_t
> (output.
points.size ());
182 template <
typename Po
intT,
typename Po
intNT>
bool 185 PCL_INFO (
"SmoothedSurfacesKeypoint initCompute () called\n");
188 PCL_ERROR (
"[pcl::SmoothedSurfacesKeypoints::initCompute] Keypoint::initCompute failed\n");
194 PCL_ERROR (
"[pcl::SmoothedSurfacesKeypoints::initCompute] Input normals were not set\n");
197 if (clouds_.size () == 0)
199 PCL_ERROR (
"[pcl::SmoothedSurfacesKeypoints::initCompute] No other clouds were set apart from the input\n");
203 if (input_->points.size () != normals_->points.size ())
205 PCL_ERROR (
"[pcl::SmoothedSurfacesKeypoints::initCompute] The input cloud and the input normals differ in size\n");
209 for (
size_t cloud_i = 0; cloud_i < clouds_.size (); ++cloud_i)
211 if (clouds_[cloud_i]->points.size () != input_->points.size ())
213 PCL_ERROR (
"[pcl::SmoothedSurfacesKeypoints::initCompute] Cloud %d does not have the same number of points as the input cloud\n", cloud_i);
217 if (cloud_normals_[cloud_i]->points.size () != input_->points.size ())
219 PCL_ERROR (
"[pcl::SmoothedSurfacesKeypoints::initCompute] Normals for cloud %d do not have the same number of points as the input cloud\n", cloud_i);
225 scales_.push_back (std::pair<float, size_t> (input_scale_, scales_.size ()));
226 clouds_.push_back (input_);
227 cloud_normals_.push_back (normals_);
228 cloud_trees_.push_back (tree_);
230 sort (scales_.begin (), scales_.end (), compareScalesFunction);
233 for (
size_t i = 0; i < scales_.size (); ++i)
234 if (scales_[i].second == scales_.size () - 1)
240 PCL_INFO (
"Scales: ");
241 for (
size_t i = 0; i < scales_.size (); ++i) PCL_INFO (
"(%d %f), ", scales_[i].second, scales_[i].first);
248 #define PCL_INSTANTIATE_SmoothedSurfacesKeypoint(T,NT) template class PCL_EXPORTS pcl::SmoothedSurfacesKeypoint<T,NT>; PointCloudT::ConstPtr PointCloudTConstPtr
virtual int radiusSearch(const PointT &point, double radius, std::vector< int > &k_indices, std::vector< float > &k_sqr_distances, unsigned int max_nn=0) const =0
Search for all the nearest neighbors of the query point in a given radius.
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
Keypoint< PointT, PointT >::KdTreePtr KdTreePtr
void detectKeypoints(PointCloudT &output)
Abstract key point detection method.
uint32_t height
The point cloud height (if organized as an image-structure).
PointCloudNT::ConstPtr PointCloudNTConstPtr
void addSmoothedPointCloud(const PointCloudTConstPtr &cloud, const PointCloudNTConstPtr &normals, KdTreePtr &kdtree, float &scale)
Keypoint represents the base class for key points.
uint32_t width
The point cloud width (if organized as an image-structure).
boost::shared_ptr< pcl::search::Search< PointT > > Ptr
PointCloud represents the base class in PCL for storing collections of 3D points. ...
pcl::PCLHeader header
The point cloud header.