Point Cloud Library (PCL) 1.15.1-dev
Loading...
Searching...
No Matches
harris_6d.hpp
1/*
2 * Software License Agreement (BSD License)
3 *
4 * Point Cloud Library (PCL) - www.pointclouds.org
5 * Copyright (c) 2010-2011, Willow Garage, Inc.
6 *
7 * All rights reserved.
8 *
9 * Redistribution and use in source and binary forms, with or without
10 * modification, are permitted provided that the following conditions
11 * are met:
12 *
13 * * Redistributions of source code must retain the above copyright
14 * notice, this list of conditions and the following disclaimer.
15 * * Redistributions in binary form must reproduce the above
16 * copyright notice, this list of conditions and the following
17 * disclaimer in the documentation and/or other materials provided
18 * with the distribution.
19 * * Neither the name of Willow Garage, Inc. nor the names of its
20 * contributors may be used to endorse or promote products derived
21 * from this software without specific prior written permission.
22 *
23 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
24 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
25 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
26 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
27 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
28 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
29 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
30 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
31 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
32 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
33 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
34 * POSSIBILITY OF SUCH DAMAGE.
35 *
36 */
37
38#ifndef PCL_HARRIS_KEYPOINT_6D_IMPL_H_
39#define PCL_HARRIS_KEYPOINT_6D_IMPL_H_
40
41#include <Eigen/Eigenvalues> // for SelfAdjointEigenSolver
42#include <pcl/keypoints/harris_6d.h>
43#include <pcl/common/io.h>
44#include <pcl/features/normal_3d.h>
45//#include <pcl/features/fast_intensity_gradient.h>
46#include <pcl/features/intensity_gradient.h>
47#include <pcl/features/integral_image_normal.h>
48#include <pcl/search/auto.h>
49
50template <typename PointInT, typename PointOutT, typename NormalT> void
51pcl::HarrisKeypoint6D<PointInT, PointOutT, NormalT>::setThreshold (float threshold)
52{
53 threshold_= threshold;
54}
55
56template <typename PointInT, typename PointOutT, typename NormalT> void
57pcl::HarrisKeypoint6D<PointInT, PointOutT, NormalT>::setRadius (float radius)
58{
59 search_radius_ = radius;
60}
61
62template <typename PointInT, typename PointOutT, typename NormalT> void
63pcl::HarrisKeypoint6D<PointInT, PointOutT, NormalT>::setRefine (bool do_refine)
64{
65 refine_ = do_refine;
66}
67
68template <typename PointInT, typename PointOutT, typename NormalT> void
73
74//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
75template <typename PointInT, typename PointOutT, typename NormalT> void
76pcl::HarrisKeypoint6D<PointInT, PointOutT, NormalT>::calculateCombinedCovar (const pcl::Indices& neighbors, float* coefficients) const
77{
78 std::fill_n(coefficients, 21, 0);
79 unsigned count = 0;
80 for (const auto &neighbor : neighbors)
81 {
82 if (std::isfinite ((*normals_)[neighbor].normal_x) && std::isfinite ((*intensity_gradients_)[neighbor].gradient [0]))
83 {
84 coefficients[ 0] += (*normals_)[neighbor].normal_x * (*normals_)[neighbor].normal_x;
85 coefficients[ 1] += (*normals_)[neighbor].normal_x * (*normals_)[neighbor].normal_y;
86 coefficients[ 2] += (*normals_)[neighbor].normal_x * (*normals_)[neighbor].normal_z;
87 coefficients[ 3] += (*normals_)[neighbor].normal_x * (*intensity_gradients_)[neighbor].gradient [0];
88 coefficients[ 4] += (*normals_)[neighbor].normal_x * (*intensity_gradients_)[neighbor].gradient [1];
89 coefficients[ 5] += (*normals_)[neighbor].normal_x * (*intensity_gradients_)[neighbor].gradient [2];
90
91 coefficients[ 6] += (*normals_)[neighbor].normal_y * (*normals_)[neighbor].normal_y;
92 coefficients[ 7] += (*normals_)[neighbor].normal_y * (*normals_)[neighbor].normal_z;
93 coefficients[ 8] += (*normals_)[neighbor].normal_y * (*intensity_gradients_)[neighbor].gradient [0];
94 coefficients[ 9] += (*normals_)[neighbor].normal_y * (*intensity_gradients_)[neighbor].gradient [1];
95 coefficients[10] += (*normals_)[neighbor].normal_y * (*intensity_gradients_)[neighbor].gradient [2];
96
97 coefficients[11] += (*normals_)[neighbor].normal_z * (*normals_)[neighbor].normal_z;
98 coefficients[12] += (*normals_)[neighbor].normal_z * (*intensity_gradients_)[neighbor].gradient [0];
99 coefficients[13] += (*normals_)[neighbor].normal_z * (*intensity_gradients_)[neighbor].gradient [1];
100 coefficients[14] += (*normals_)[neighbor].normal_z * (*intensity_gradients_)[neighbor].gradient [2];
101
102 coefficients[15] += (*intensity_gradients_)[neighbor].gradient [0] * (*intensity_gradients_)[neighbor].gradient [0];
103 coefficients[16] += (*intensity_gradients_)[neighbor].gradient [0] * (*intensity_gradients_)[neighbor].gradient [1];
104 coefficients[17] += (*intensity_gradients_)[neighbor].gradient [0] * (*intensity_gradients_)[neighbor].gradient [2];
105
106 coefficients[18] += (*intensity_gradients_)[neighbor].gradient [1] * (*intensity_gradients_)[neighbor].gradient [1];
107 coefficients[19] += (*intensity_gradients_)[neighbor].gradient [1] * (*intensity_gradients_)[neighbor].gradient [2];
108
109 coefficients[20] += (*intensity_gradients_)[neighbor].gradient [2] * (*intensity_gradients_)[neighbor].gradient [2];
110
111 ++count;
112 }
113 }
114 if (count > 0)
115 {
116 float norm = 1.0 / static_cast<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;
138 }
139}
140
141//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
142template <typename PointInT, typename PointOutT, typename NormalT> void
143pcl::HarrisKeypoint6D<PointInT, PointOutT, NormalT>::detectKeypoints (PointCloudOut &output)
144{
145 if (normals_->empty ())
146 {
147 normals_->reserve (surface_->size ());
148 if (!surface_->isOrganized ())
149 {
150 pcl::NormalEstimation<PointInT, NormalT> normal_estimation;
151 normal_estimation.setInputCloud (surface_);
152 normal_estimation.setRadiusSearch (search_radius_);
153 normal_estimation.compute (*normals_);
154 }
155 else
156 {
157 IntegralImageNormalEstimation<PointInT, NormalT> normal_estimation;
158 normal_estimation.setNormalEstimationMethod (pcl::IntegralImageNormalEstimation<PointInT, NormalT>::SIMPLE_3D_GRADIENT);
159 normal_estimation.setInputCloud (surface_);
160 normal_estimation.setNormalSmoothingSize (5.0);
161 normal_estimation.compute (*normals_);
162 }
163 }
164
165 pcl::PointCloud<pcl::PointXYZI>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZI>);
166 cloud->resize (surface_->size ());
167#pragma omp parallel for \
168 default(none) \
169 num_threads(threads_)
170 for (unsigned idx = 0; idx < surface_->size (); ++idx)
171 {
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;
175 //grayscale = 0.2989 * R + 0.5870 * G + 0.1140 * B
176
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));
178 }
179 pcl::copyPointCloud (*surface_, *cloud);
180
181 IntensityGradientEstimation<PointXYZI, NormalT, IntensityGradient> grad_est;
182 grad_est.setInputCloud (cloud);
183 grad_est.setInputNormals (normals_);
184 grad_est.setRadiusSearch (search_radius_);
185 grad_est.compute (*intensity_gradients_);
186
187#pragma omp parallel for \
188 default(none) \
189 num_threads(threads_)
190 for (std::size_t idx = 0; idx < intensity_gradients_->size (); ++idx)
191 {
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 ;
195
196 // Suat: ToDo: remove this magic number or expose using set/get
197 if (len > 200.0)
198 {
199 len = 1.0 / std::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;
203 }
204 else
205 {
206 intensity_gradients_->points [idx].gradient_x = 0;
207 intensity_gradients_->points [idx].gradient_y = 0;
208 intensity_gradients_->points [idx].gradient_z = 0;
209 }
210 }
211
212 typename pcl::PointCloud<PointOutT>::Ptr response (new pcl::PointCloud<PointOutT>);
213 response->points.reserve (input_->size());
214 responseTomasi(*response);
215
216 // just return the response
217 if (!nonmax_)
218 {
219 output = *response;
220 // we do not change the denseness in this case
221 output.is_dense = input_->is_dense;
222 for (std::size_t i = 0; i < response->size (); ++i)
223 keypoints_indices_->indices.push_back (i);
224 }
225 else
226 {
227 output.clear ();
228 output.reserve (response->size());
229
230#pragma omp parallel for \
231 default(none) \
232 num_threads(threads_)
233 for (std::size_t idx = 0; idx < response->size (); ++idx)
234 {
235 if (!isFinite ((*response)[idx]) || (*response)[idx].intensity < threshold_)
236 continue;
237
238 pcl::Indices 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 (const auto& index : nn_indices)
243 {
244 if ((*response)[idx].intensity < (*response)[index].intensity)
245 {
246 is_maxima = false;
247 break;
248 }
249 }
250 if (is_maxima)
251 #pragma omp critical
252 {
253 output.push_back ((*response)[idx]);
254 keypoints_indices_->indices.push_back (idx);
255 }
256 }
257
258 if (refine_)
259 refineCorners (output);
260
261 output.height = 1;
262 output.width = output.size();
263 output.is_dense = true;
264 }
265}
266
267template <typename PointInT, typename PointOutT, typename NormalT> void
268pcl::HarrisKeypoint6D<PointInT, PointOutT, NormalT>::responseTomasi (PointCloudOut &output) const
269{
270 // get the 6x6 covar-mat
271 PointOutT pointOut;
272 PCL_ALIGN (16) float covar [21];
273 Eigen::SelfAdjointEigenSolver <Eigen::Matrix<float, 6, 6> > solver;
274 Eigen::Matrix<float, 6, 6> covariance;
275
276#pragma omp parallel for \
277 default(none) \
278 firstprivate(pointOut, covar, covariance, solver) \
279 num_threads(threads_)
280 for (unsigned pIdx = 0; pIdx < input_->size (); ++pIdx)
281 {
282 const PointInT& pointIn = input_->points [pIdx];
283 pointOut.intensity = 0.0; //std::numeric_limits<float>::quiet_NaN ();
284 if (isFinite (pointIn))
285 {
286 pcl::Indices nn_indices;
287 std::vector<float> nn_dists;
288 tree_->radiusSearch (pointIn, search_radius_, nn_indices, nn_dists);
289 calculateCombinedCovar (nn_indices, covar);
290
291 float trace = covar [0] + covar [6] + covar [11] + covar [15] + covar [18] + covar [20];
292 if (trace != 0)
293 {
294 covariance.coeffRef ( 0) = covar [ 0];
295 covariance.coeffRef ( 1) = covar [ 1];
296 covariance.coeffRef ( 2) = covar [ 2];
297 covariance.coeffRef ( 3) = covar [ 3];
298 covariance.coeffRef ( 4) = covar [ 4];
299 covariance.coeffRef ( 5) = covar [ 5];
300
301 covariance.coeffRef ( 7) = covar [ 6];
302 covariance.coeffRef ( 8) = covar [ 7];
303 covariance.coeffRef ( 9) = covar [ 8];
304 covariance.coeffRef (10) = covar [ 9];
305 covariance.coeffRef (11) = covar [10];
306
307 covariance.coeffRef (14) = covar [11];
308 covariance.coeffRef (15) = covar [12];
309 covariance.coeffRef (16) = covar [13];
310 covariance.coeffRef (17) = covar [14];
311
312 covariance.coeffRef (21) = covar [15];
313 covariance.coeffRef (22) = covar [16];
314 covariance.coeffRef (23) = covar [17];
315
316 covariance.coeffRef (28) = covar [18];
317 covariance.coeffRef (29) = covar [19];
318
319 covariance.coeffRef (35) = covar [20];
320
321 covariance.coeffRef ( 6) = covar [ 1];
322
323 covariance.coeffRef (12) = covar [ 2];
324 covariance.coeffRef (13) = covar [ 7];
325
326 covariance.coeffRef (18) = covar [ 3];
327 covariance.coeffRef (19) = covar [ 8];
328 covariance.coeffRef (20) = covar [12];
329
330 covariance.coeffRef (24) = covar [ 4];
331 covariance.coeffRef (25) = covar [ 9];
332 covariance.coeffRef (26) = covar [13];
333 covariance.coeffRef (27) = covar [16];
334
335 covariance.coeffRef (30) = covar [ 5];
336 covariance.coeffRef (31) = covar [10];
337 covariance.coeffRef (32) = covar [14];
338 covariance.coeffRef (33) = covar [17];
339 covariance.coeffRef (34) = covar [19];
340
341 solver.compute (covariance);
342 pointOut.intensity = solver.eigenvalues () [3];
343 }
344 }
345
346 pointOut.x = pointIn.x;
347 pointOut.y = pointIn.y;
348 pointOut.z = pointIn.z;
349
350 #pragma omp critical
351 output.push_back(pointOut);
352 }
353 output.height = input_->height;
354 output.width = input_->width;
355}
356
357template <typename PointInT, typename PointOutT, typename NormalT> void
358pcl::HarrisKeypoint6D<PointInT, PointOutT, NormalT>::refineCorners (PointCloudOut &corners) const
359{
360 typename pcl::search::Search<PointInT>::Ptr search(pcl::search::autoSelectMethod<PointInT>(surface_, false, pcl::search::Purpose::radius_search));
361
362 Eigen::Matrix3f nnT;
363 Eigen::Matrix3f NNT;
364 Eigen::Vector3f NNTp;
365 const Eigen::Vector3f* normal;
366 const Eigen::Vector3f* point;
367 float diff;
368 const unsigned max_iterations = 10;
369 for (typename PointCloudOut::iterator cornerIt = corners.begin(); cornerIt != corners.end(); ++cornerIt)
370 {
371 unsigned iterations = 0;
372 do {
373 NNT.setZero();
374 NNTp.setZero();
375 PointInT corner;
376 corner.x = cornerIt->x;
377 corner.y = cornerIt->y;
378 corner.z = cornerIt->z;
379 pcl::Indices nn_indices;
380 std::vector<float> nn_dists;
381 search->radiusSearch (corner, search_radius_, nn_indices, nn_dists);
382 for (const auto& index : nn_indices)
383 {
384 normal = reinterpret_cast<const Eigen::Vector3f*> (&((*normals_)[index].normal_x));
385 point = reinterpret_cast<const Eigen::Vector3f*> (&((*surface_)[index].x));
386 nnT = (*normal) * (normal->transpose());
387 NNT += nnT;
388 NNTp += nnT * (*point);
389 }
390 if (NNT.determinant() != 0)
391 *(reinterpret_cast<Eigen::Vector3f*>(&(cornerIt->x))) = NNT.inverse () * NNTp;
392
393 diff = (cornerIt->x - corner.x) * (cornerIt->x - corner.x) +
394 (cornerIt->y - corner.y) * (cornerIt->y - corner.y) +
395 (cornerIt->z - corner.z) * (cornerIt->z - corner.z);
396
397 } while (diff > 1e-6 && ++iterations < max_iterations);
398 }
399}
400
401#define PCL_INSTANTIATE_HarrisKeypoint6D(T,U,N) template class PCL_EXPORTS pcl::HarrisKeypoint6D<T,U,N>;
402#endif // #ifndef PCL_HARRIS_KEYPOINT_6D_IMPL_H_
403
pcl::FeatureFromNormals::setInputNormals
void setInputNormals(const PointCloudNConstPtr &normals)
Provide a pointer to the input dataset that contains the point normals of the XYZ dataset.
Definition feature.h:339
pcl::Feature::setRadiusSearch
void setRadiusSearch(double radius)
Set the sphere radius that is to be used for determining the nearest neighbors used for the feature e...
Definition feature.h:198
pcl::Feature::compute
void compute(PointCloudOut &output)
Base method for feature estimation for all points given in <setInputCloud (), setIndices ()> using th...
Definition feature.hpp:195
pcl::HarrisKeypoint6D::setNonMaxSupression
void setNonMaxSupression(bool=false)
whether non maxima suppression should be applied or the response for each point should be returned
Definition harris_6d.hpp:69
pcl::HarrisKeypoint6D::setThreshold
void setThreshold(float threshold)
set the threshold value for detecting corners.
Definition harris_6d.hpp:51
pcl::HarrisKeypoint6D::setRefine
void setRefine(bool do_refine)
whether the detected key points should be refined or not.
Definition harris_6d.hpp:63
pcl::HarrisKeypoint6D::setRadius
void setRadius(float radius)
set the radius for normal estimation and non maxima suppression.
Definition harris_6d.hpp:57
pcl::HarrisKeypoint6D::responseTomasi
void responseTomasi(PointCloudOut &output) const
Definition harris_6d.hpp:268
pcl::HarrisKeypoint6D::detectKeypoints
void detectKeypoints(PointCloudOut &output)
Definition harris_6d.hpp:143
pcl::HarrisKeypoint6D::PointCloudOut
typename Keypoint< PointInT, PointOutT >::PointCloudOut PointCloudOut
Definition harris_6d.h:57
pcl::HarrisKeypoint6D::refineCorners
void refineCorners(PointCloudOut &corners) const
Definition harris_6d.hpp:358
pcl::HarrisKeypoint6D::calculateCombinedCovar
void calculateCombinedCovar(const pcl::Indices &neighbors, float *coefficients) const
Definition harris_6d.hpp:76
pcl::IntegralImageNormalEstimation
Surface normal estimation on organized data using integral images.
Definition integral_image_normal.h:66
pcl::IntegralImageNormalEstimation::setNormalEstimationMethod
void setNormalEstimationMethod(NormalEstimationMethod normal_estimation_method)
Set the normal estimation method.
Definition integral_image_normal.h:197
pcl::IntegralImageNormalEstimation::setInputCloud
void setInputCloud(const typename PointCloudIn::ConstPtr &cloud) override
Provide a pointer to the input dataset (overwrites the PCLBase::setInputCloud method)
Definition integral_image_normal.h:215
pcl::IntegralImageNormalEstimation::setNormalSmoothingSize
void setNormalSmoothingSize(float normal_smoothing_size)
Set the normal smoothing size.
Definition integral_image_normal.h:173
pcl::IntensityGradientEstimation
IntensityGradientEstimation estimates the intensity gradient for a point cloud that contains position...
Definition intensity_gradient.h:57
pcl::NormalEstimation
NormalEstimation estimates local surface properties (surface normals and curvatures)at each 3D point.
Definition normal_3d.h:244
pcl::NormalEstimation::setInputCloud
void setInputCloud(const PointCloudConstPtr &cloud) override
Provide a pointer to the input dataset.
Definition normal_3d.h:328
pcl::PCLBase::setInputCloud
virtual void setInputCloud(const PointCloudConstPtr &cloud)
Provide a pointer to the input dataset.
Definition pcl_base.hpp:65
pcl::PointCloud
PointCloud represents the base class in PCL for storing collections of 3D points.
Definition point_cloud.h:174
pcl::PointCloud::is_dense
bool is_dense
True if no points are invalid (e.g., have NaN or Inf values in any of their floating point fields).
Definition point_cloud.h:404
pcl::PointCloud::resize
void resize(std::size_t count)
Resizes the container to contain count elements.
Definition point_cloud.h:463
pcl::PointCloud::size
std::size_t size() const
Definition point_cloud.h:444
pcl::PointCloud::Ptr
shared_ptr< PointCloud< PointT > > Ptr
Definition point_cloud.h:414
pcl::PointCloud::points
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
Definition point_cloud.h:396
pcl::search::Search::Ptr
shared_ptr< pcl::search::Search< PointT > > Ptr
Definition search.h:81
pcl::search::Search::radiusSearch
virtual int radiusSearch(const PointT &point, double radius, Indices &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.
pcl::copyPointCloud
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.
Definition io.hpp:208
pcl::search::Purpose::radius_search
@ radius_search
The search method will mainly be used for radiusSearch.
pcl::isFinite
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...
Definition point_tests.h:56
pcl::Indices
IndicesAllocator<> Indices
Type used for indices in PCL.
Definition types.h:133