38 #ifndef PCL_HARRIS_KEYPOINT_6D_IMPL_H_ 39 #define PCL_HARRIS_KEYPOINT_6D_IMPL_H_ 41 #include <pcl/keypoints/harris_6d.h> 42 #include <pcl/common/io.h> 43 #include <pcl/filters/passthrough.h> 44 #include <pcl/filters/extract_indices.h> 45 #include <pcl/features/normal_3d.h> 47 #include <pcl/features/intensity_gradient.h> 48 #include <pcl/features/integral_image_normal.h> 50 template <
typename Po
intInT,
typename Po
intOutT,
typename NormalT>
void 53 threshold_= threshold;
56 template <
typename Po
intInT,
typename Po
intOutT,
typename NormalT>
void 59 search_radius_ = radius;
62 template <
typename Po
intInT,
typename Po
intOutT,
typename NormalT>
void 68 template <
typename Po
intInT,
typename Po
intOutT,
typename NormalT>
void 75 template <
typename Po
intInT,
typename Po
intOutT,
typename NormalT>
void 78 memset (coefficients, 0,
sizeof (
float) * 21);
80 for (std::vector<int>::const_iterator iIt = neighbors.begin(); iIt != neighbors.end(); ++iIt)
82 if (pcl_isfinite (normals_->points[*iIt].normal_x) && pcl_isfinite (intensity_gradients_->points[*iIt].gradient [0]))
84 coefficients[ 0] += normals_->points[*iIt].normal_x * normals_->points[*iIt].normal_x;
85 coefficients[ 1] += normals_->points[*iIt].normal_x * normals_->points[*iIt].normal_y;
86 coefficients[ 2] += normals_->points[*iIt].normal_x * normals_->points[*iIt].normal_z;
87 coefficients[ 3] += normals_->points[*iIt].normal_x * intensity_gradients_->points[*iIt].gradient [0];
88 coefficients[ 4] += normals_->points[*iIt].normal_x * intensity_gradients_->points[*iIt].gradient [1];
89 coefficients[ 5] += normals_->points[*iIt].normal_x * intensity_gradients_->points[*iIt].gradient [2];
91 coefficients[ 6] += normals_->points[*iIt].normal_y * normals_->points[*iIt].normal_y;
92 coefficients[ 7] += normals_->points[*iIt].normal_y * normals_->points[*iIt].normal_z;
93 coefficients[ 8] += normals_->points[*iIt].normal_y * intensity_gradients_->points[*iIt].gradient [0];
94 coefficients[ 9] += normals_->points[*iIt].normal_y * intensity_gradients_->points[*iIt].gradient [1];
95 coefficients[10] += normals_->points[*iIt].normal_y * intensity_gradients_->points[*iIt].gradient [2];
97 coefficients[11] += normals_->points[*iIt].normal_z * normals_->points[*iIt].normal_z;
98 coefficients[12] += normals_->points[*iIt].normal_z * intensity_gradients_->points[*iIt].gradient [0];
99 coefficients[13] += normals_->points[*iIt].normal_z * intensity_gradients_->points[*iIt].gradient [1];
100 coefficients[14] += normals_->points[*iIt].normal_z * intensity_gradients_->points[*iIt].gradient [2];
102 coefficients[15] += intensity_gradients_->points[*iIt].gradient [0] * intensity_gradients_->points[*iIt].gradient [0];
103 coefficients[16] += intensity_gradients_->points[*iIt].gradient [0] * intensity_gradients_->points[*iIt].gradient [1];
104 coefficients[17] += intensity_gradients_->points[*iIt].gradient [0] * intensity_gradients_->points[*iIt].gradient [2];
106 coefficients[18] += intensity_gradients_->points[*iIt].gradient [1] * intensity_gradients_->points[*iIt].gradient [1];
107 coefficients[19] += intensity_gradients_->points[*iIt].gradient [1] * intensity_gradients_->points[*iIt].gradient [2];
109 coefficients[20] += intensity_gradients_->points[*iIt].gradient [2] * intensity_gradients_->points[*iIt].gradient [2];
116 float norm = 1.0 / float (count);
117 coefficients[ 0] *= norm;
118 coefficients[ 1] *= norm;
119 coefficients[ 2] *= norm;
120 coefficients[ 3] *= norm;
121 coefficients[ 4] *= norm;
122 coefficients[ 5] *= norm;
123 coefficients[ 6] *= norm;
124 coefficients[ 7] *= norm;
125 coefficients[ 8] *= norm;
126 coefficients[ 9] *= norm;
127 coefficients[10] *= norm;
128 coefficients[11] *= norm;
129 coefficients[12] *= norm;
130 coefficients[13] *= norm;
131 coefficients[14] *= norm;
132 coefficients[15] *= norm;
133 coefficients[16] *= norm;
134 coefficients[17] *= norm;
135 coefficients[18] *= norm;
136 coefficients[19] *= norm;
137 coefficients[20] *= norm;
142 template <
typename Po
intInT,
typename Po
intOutT,
typename NormalT>
void 145 if (normals_->empty ())
147 normals_->reserve (surface_->size ());
148 if (!surface_->isOrganized ())
153 normal_estimation.
compute (*normals_);
161 normal_estimation.
compute (*normals_);
166 cloud->
resize (surface_->size ());
168 #pragma omp parallel for num_threads(threads_) default(shared) 170 for (
unsigned idx = 0; idx < surface_->size (); ++idx)
172 cloud->
points [idx].x = surface_->points [idx].x;
173 cloud->
points [idx].y = surface_->points [idx].y;
174 cloud->
points [idx].z = surface_->points [idx].z;
177 cloud->
points [idx].intensity = 0.00390625 * (0.114 * float(surface_->points [idx].b) + 0.5870 * float(surface_->points [idx].g) + 0.2989 * float(surface_->points [idx].r));
185 grad_est.
compute (*intensity_gradients_);
188 #pragma omp parallel for num_threads(threads_) default (shared) 190 for (
unsigned idx = 0; idx < intensity_gradients_->size (); ++idx)
192 float len = intensity_gradients_->points [idx].gradient_x * intensity_gradients_->points [idx].gradient_x +
193 intensity_gradients_->points [idx].gradient_y * intensity_gradients_->points [idx].gradient_y +
194 intensity_gradients_->points [idx].gradient_z * intensity_gradients_->points [idx].gradient_z ;
199 len = 1.0 / sqrt (len);
200 intensity_gradients_->points [idx].gradient_x *= len;
201 intensity_gradients_->points [idx].gradient_y *= len;
202 intensity_gradients_->points [idx].gradient_z *= len;
206 intensity_gradients_->points [idx].gradient_x = 0;
207 intensity_gradients_->points [idx].gradient_y = 0;
208 intensity_gradients_->points [idx].gradient_z = 0;
213 response->points.reserve (input_->points.size());
214 responseTomasi(*response);
222 for (
size_t i = 0; i < response->size (); ++i)
223 keypoints_indices_->indices.push_back (i);
228 output.
points.reserve (response->points.size());
231 #pragma omp parallel for num_threads(threads_) default(shared) 233 for (
size_t idx = 0; idx < response->points.size (); ++idx)
235 if (!
isFinite (response->points[idx]) || response->points[idx].intensity < threshold_)
238 std::vector<int> nn_indices;
239 std::vector<float> nn_dists;
240 tree_->radiusSearch (idx, search_radius_, nn_indices, nn_dists);
241 bool is_maxima =
true;
242 for (std::vector<int>::const_iterator iIt = nn_indices.begin(); iIt != nn_indices.end(); ++iIt)
244 if (response->points[idx].intensity < response->points[*iIt].intensity)
255 output.
points.push_back (response->points[idx]);
256 keypoints_indices_->indices.push_back (idx);
261 refineCorners (output);
264 output.
width = static_cast<uint32_t> (output.
points.size());
269 template <
typename Po
intInT,
typename Po
intOutT,
typename NormalT>
void 274 PCL_ALIGN (16)
float covar [21];
275 Eigen::SelfAdjointEigenSolver <Eigen::Matrix<float, 6, 6> > solver;
276 Eigen::Matrix<float, 6, 6> covariance;
279 #pragma omp parallel for default (shared) private (pointOut, covar, covariance, solver) num_threads(threads_) 281 for (
unsigned pIdx = 0; pIdx < input_->size (); ++pIdx)
283 const PointInT& pointIn = input_->points [pIdx];
284 pointOut.intensity = 0.0;
287 std::vector<int> nn_indices;
288 std::vector<float> nn_dists;
289 tree_->radiusSearch (pointIn, search_radius_, nn_indices, nn_dists);
290 calculateCombinedCovar (nn_indices, covar);
292 float trace = covar [0] + covar [6] + covar [11] + covar [15] + covar [18] + covar [20];
295 covariance.coeffRef ( 0) = covar [ 0];
296 covariance.coeffRef ( 1) = covar [ 1];
297 covariance.coeffRef ( 2) = covar [ 2];
298 covariance.coeffRef ( 3) = covar [ 3];
299 covariance.coeffRef ( 4) = covar [ 4];
300 covariance.coeffRef ( 5) = covar [ 5];
302 covariance.coeffRef ( 7) = covar [ 6];
303 covariance.coeffRef ( 8) = covar [ 7];
304 covariance.coeffRef ( 9) = covar [ 8];
305 covariance.coeffRef (10) = covar [ 9];
306 covariance.coeffRef (11) = covar [10];
308 covariance.coeffRef (14) = covar [11];
309 covariance.coeffRef (15) = covar [12];
310 covariance.coeffRef (16) = covar [13];
311 covariance.coeffRef (17) = covar [14];
313 covariance.coeffRef (21) = covar [15];
314 covariance.coeffRef (22) = covar [16];
315 covariance.coeffRef (23) = covar [17];
317 covariance.coeffRef (28) = covar [18];
318 covariance.coeffRef (29) = covar [19];
320 covariance.coeffRef (35) = covar [20];
322 covariance.coeffRef ( 6) = covar [ 1];
324 covariance.coeffRef (12) = covar [ 2];
325 covariance.coeffRef (13) = covar [ 7];
327 covariance.coeffRef (18) = covar [ 3];
328 covariance.coeffRef (19) = covar [ 8];
329 covariance.coeffRef (20) = covar [12];
331 covariance.coeffRef (24) = covar [ 4];
332 covariance.coeffRef (25) = covar [ 9];
333 covariance.coeffRef (26) = covar [13];
334 covariance.coeffRef (27) = covar [16];
336 covariance.coeffRef (30) = covar [ 5];
337 covariance.coeffRef (31) = covar [10];
338 covariance.coeffRef (32) = covar [14];
339 covariance.coeffRef (33) = covar [17];
340 covariance.coeffRef (34) = covar [19];
342 solver.compute (covariance);
343 pointOut.intensity = solver.eigenvalues () [3];
347 pointOut.x = pointIn.x;
348 pointOut.y = pointIn.y;
349 pointOut.z = pointIn.z;
354 output.
points.push_back(pointOut);
356 output.
height = input_->height;
357 output.
width = input_->width;
360 template <
typename Po
intInT,
typename Po
intOutT,
typename NormalT>
void 368 Eigen::Vector3f NNTp;
369 const Eigen::Vector3f* normal;
370 const Eigen::Vector3f* point;
372 const unsigned max_iterations = 10;
375 unsigned iterations = 0;
380 corner.x = cornerIt->x;
381 corner.y = cornerIt->y;
382 corner.z = cornerIt->z;
383 std::vector<int> nn_indices;
384 std::vector<float> nn_dists;
385 search.
radiusSearch (corner, search_radius_, nn_indices, nn_dists);
386 for (std::vector<int>::const_iterator iIt = nn_indices.begin(); iIt != nn_indices.end(); ++iIt)
388 normal = reinterpret_cast<const Eigen::Vector3f*> (&(normals_->points[*iIt].normal_x));
389 point = reinterpret_cast<const Eigen::Vector3f*> (&(surface_->points[*iIt].x));
390 nnT = (*normal) * (normal->transpose());
392 NNTp += nnT * (*point);
394 if (NNT.determinant() != 0)
395 *(reinterpret_cast<Eigen::Vector3f*>(&(cornerIt->x))) = NNT.inverse () * NNTp;
397 diff = (cornerIt->x - corner.x) * (cornerIt->x - corner.x) +
398 (cornerIt->y - corner.y) * (cornerIt->y - corner.y) +
399 (cornerIt->z - corner.z) * (cornerIt->z - corner.z);
401 }
while (diff > 1e-6 && ++iterations < max_iterations);
405 #define PCL_INSTANTIATE_HarrisKeypoint6D(T,U,N) template class PCL_EXPORTS pcl::HarrisKeypoint6D<T,U,N>; 406 #endif // #ifndef PCL_HARRIS_KEYPOINT_6D_IMPL_H_ search::KdTree is a wrapper class which inherits the pcl::KdTree class for performing search function...
bool isFinite(const PointT &pt)
Tests if the 3D components of a point are all finite param[in] pt point to be tested return true if f...
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
void setNonMaxSupression(bool=false)
whether non maxima suppression should be applied or the response for each point should be returned
PCL_EXPORTS void copyPointCloud(const pcl::PCLPointCloud2 &cloud_in, const std::vector< int > &indices, pcl::PCLPointCloud2 &cloud_out)
Extract the indices of a given point cloud as a new point cloud.
void setRadiusSearch(double radius)
Set the sphere radius that is to be used for determining the nearest neighbors used for the feature e...
VectorType::iterator iterator
NormalEstimation estimates local surface properties (surface normals and curvatures)at each 3D point.
void detectKeypoints(PointCloudOut &output)
Abstract key point detection method.
uint32_t height
The point cloud height (if organized as an image-structure).
boost::shared_ptr< PointCloud< PointT > > Ptr
uint32_t width
The point cloud width (if organized as an image-structure).
Surface normal estimation on organized data using integral images.
int radiusSearch(const PointT &point, double radius, std::vector< int > &k_indices, std::vector< float > &k_sqr_distances, unsigned int max_nn=0) const
Search for all the nearest neighbors of the query point in a given radius.
void setRadius(float radius)
set the radius for normal estimation and non maxima supression.
void refineCorners(PointCloudOut &corners) const
virtual void setInputCloud(const PointCloudConstPtr &cloud)
Provide a pointer to the input dataset.
void setRefine(bool do_refine)
whether the detected key points should be refined or not.
PointCloud represents the base class in PCL for storing collections of 3D points.
void setNormalEstimationMethod(NormalEstimationMethod normal_estimation_method)
Set the normal estimation method.
IntensityGradientEstimation estimates the intensity gradient for a point cloud that contains position...
void setInputCloud(const PointCloudConstPtr &cloud, const IndicesConstPtr &indices=IndicesConstPtr())
Provide a pointer to the input dataset.
void setThreshold(float threshold)
set the threshold value for detecting corners.
virtual void setInputCloud(const typename PointCloudIn::ConstPtr &cloud)
Provide a pointer to the input dataset (overwrites the PCLBase::setInputCloud method)
virtual void setInputCloud(const PointCloudConstPtr &cloud)
Provide a pointer to the input dataset.
bool is_dense
True if no points are invalid (e.g., have NaN or Inf values).
void resize(size_t n)
Resize the cloud.
void compute(PointCloudOut &output)
Base method for feature estimation for all points given in <setInputCloud (), setIndices ()> using th...
void setInputNormals(const PointCloudNConstPtr &normals)
Provide a pointer to the input dataset that contains the point normals of the XYZ dataset.
void calculateCombinedCovar(const std::vector< int > &neighbors, float *coefficients) const
void setNormalSmoothingSize(float normal_smoothing_size)
Set the normal smoothing size.
void responseTomasi(PointCloudOut &output) const