38 #ifndef PCL_KEYPOINTS_IMPL_SMOOTHEDSURFACESKEYPOINT_H_
39 #define PCL_KEYPOINTS_IMPL_SMOOTHEDSURFACESKEYPOINT_H_
41 #include <pcl/keypoints/smoothed_surfaces_keypoint.h>
46 template <
typename Po
intT,
typename Po
intNT>
void
52 clouds_.push_back (cloud);
53 cloud_normals_.push_back (normals);
54 cloud_trees_.push_back (kdtree);
55 scales_.push_back (std::pair<float, std::size_t> (scale, scales_.size ()));
60 template <
typename Po
intT,
typename Po
intNT>
void
64 cloud_normals_.clear ();
70 template <
typename Po
intT,
typename Po
intNT>
void
74 std::vector<std::vector<float> > diffs (scales_.size ());
77 std::vector<float> aux_diffs (input_->size (), 0.0f);
78 diffs[scales_[0].second] = aux_diffs;
80 cloud_trees_[scales_[0].second]->setInputCloud (clouds_[scales_[0].second]);
81 for (std::size_t scale_i = 1; scale_i < clouds_.size (); ++scale_i)
83 std::size_t cloud_i = scales_[scale_i].second,
84 cloud_i_minus_one = scales_[scale_i - 1].second;
85 diffs[cloud_i].resize (input_->size ());
86 PCL_INFO (
"cloud_i %u cloud_i_minus_one %u\n", cloud_i, cloud_i_minus_one);
87 for (std::size_t point_i = 0; point_i < input_->size (); ++point_i)
88 diffs[cloud_i][point_i] = (*cloud_normals_[cloud_i])[point_i].getNormalVector3fMap ().dot (
89 (*clouds_[cloud_i])[point_i].getVector3fMap () - (*clouds_[cloud_i_minus_one])[point_i].getVector3fMap ());
92 cloud_trees_[cloud_i]->setInputCloud (clouds_[cloud_i]);
98 for (
int point_i = 0; point_i < static_cast<int> (input_->size ()); ++point_i)
100 std::vector<int> nn_indices;
101 std::vector<float> nn_distances;
102 input_tree->
radiusSearch (point_i, input_scale_ * neighborhood_constant_, nn_indices, nn_distances);
104 bool is_min =
true, is_max =
true;
105 for (
const int &nn_index : nn_indices)
106 if (nn_index != point_i)
108 if (diffs[input_index_][point_i] < diffs[input_index_][nn_index])
110 else if (diffs[input_index_][point_i] > diffs[input_index_][nn_index])
115 if (is_min || is_max)
117 bool passed_min =
true, passed_max =
true;
118 for (std::size_t scale_i = 0; scale_i < scales_.size (); ++scale_i)
120 std::size_t cloud_i = scales_[scale_i].second;
122 if (cloud_i == clouds_.size () - 1)
125 nn_indices.clear (); nn_distances.clear ();
126 cloud_trees_[cloud_i]->radiusSearch (point_i, scales_[scale_i].first * neighborhood_constant_, nn_indices, nn_distances);
128 bool is_min_other_scale =
true, is_max_other_scale =
true;
129 for (
const int &nn_index : nn_indices)
130 if (nn_index != point_i)
132 if (diffs[input_index_][point_i] < diffs[cloud_i][nn_index])
133 is_max_other_scale =
false;
134 else if (diffs[input_index_][point_i] > diffs[cloud_i][nn_index])
135 is_min_other_scale =
false;
138 if (is_min && !is_min_other_scale)
140 if (is_max && !is_max_other_scale)
143 if (!passed_min && !passed_max)
148 if (passed_min || passed_max)
151 keypoints_indices_->indices.push_back (point_i);
156 output.
header = input_->header;
181 template <
typename Po
intT,
typename Po
intNT>
bool
184 PCL_INFO (
"SmoothedSurfacesKeypoint initCompute () called\n");
187 PCL_ERROR (
"[pcl::SmoothedSurfacesKeypoints::initCompute] Keypoint::initCompute failed\n");
193 PCL_ERROR (
"[pcl::SmoothedSurfacesKeypoints::initCompute] Input normals were not set\n");
196 if (clouds_.empty ())
198 PCL_ERROR (
"[pcl::SmoothedSurfacesKeypoints::initCompute] No other clouds were set apart from the input\n");
202 if (input_->size () != normals_->size ())
204 PCL_ERROR (
"[pcl::SmoothedSurfacesKeypoints::initCompute] The input cloud and the input normals differ in size\n");
208 for (std::size_t cloud_i = 0; cloud_i < clouds_.size (); ++cloud_i)
210 if (clouds_[cloud_i]->size () != input_->size ())
212 PCL_ERROR(
"[pcl::SmoothedSurfacesKeypoints::initCompute] Cloud %zu does not have "
213 "the same number of points as the input cloud\n",
218 if (cloud_normals_[cloud_i]->size () != input_->size ())
220 PCL_ERROR(
"[pcl::SmoothedSurfacesKeypoints::initCompute] Normals for cloud %zu "
221 "do not have the same number of points as the input cloud\n",
228 scales_.push_back (std::pair<float, std::size_t> (input_scale_, scales_.size ()));
229 clouds_.push_back (input_);
230 cloud_normals_.push_back (normals_);
231 cloud_trees_.push_back (tree_);
233 sort (scales_.begin (), scales_.end (), compareScalesFunction);
236 for (std::size_t i = 0; i < scales_.size (); ++i)
237 if (scales_[i].second == scales_.size () - 1)
243 PCL_INFO (
"Scales: ");
244 for (std::size_t i = 0; i < scales_.size (); ++i) PCL_INFO (
"(%d %f), ", scales_[i].second, scales_[i].first);
251 #define PCL_INSTANTIATE_SmoothedSurfacesKeypoint(T,NT) template class PCL_EXPORTS pcl::SmoothedSurfacesKeypoint<T,NT>;