harris_6d.hpp
/*
* Software License Agreement (BSD License)
*
* Point Cloud Library (PCL) - www.pointclouds.org
* Copyright (c) 2010-2011, Willow Garage, Inc.
*
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of Willow Garage, Inc. nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
*/
#ifndef PCL_HARRIS_KEYPOINT_6D_IMPL_H_
#define PCL_HARRIS_KEYPOINT_6D_IMPL_H_
#include <pcl/keypoints/harris_6d.h>
#include <pcl/common/io.h>
#include <pcl/filters/passthrough.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/features/normal_3d.h>
//#include <pcl/features/fast_intensity_gradient.h>
#include <pcl/features/intensity_gradient.h>
#include <pcl/features/integral_image_normal.h>
template <typename PointInT, typename PointOutT, typename NormalT> void
pcl::HarrisKeypoint6D<PointInT, PointOutT, NormalT>::setThreshold (float threshold)
{
threshold_= threshold;
}
template <typename PointInT, typename PointOutT, typename NormalT> void
pcl::HarrisKeypoint6D<PointInT, PointOutT, NormalT>::setRadius (float radius)
{
search_radius_ = radius;
}
template <typename PointInT, typename PointOutT, typename NormalT> void
pcl::HarrisKeypoint6D<PointInT, PointOutT, NormalT>::setRefine (bool do_refine)
{
refine_ = do_refine;
}
template <typename PointInT, typename PointOutT, typename NormalT> void
pcl::HarrisKeypoint6D<PointInT, PointOutT, NormalT>::setNonMaxSupression (bool nonmax)
{
nonmax_ = nonmax;
}
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
template <typename PointInT, typename PointOutT, typename NormalT> void
pcl::HarrisKeypoint6D<PointInT, PointOutT, NormalT>::calculateCombinedCovar (const std::vector<int>& neighbors, float* coefficients) const
{
memset (coefficients, 0, sizeof (float) * 21);
unsigned count = 0;
for (std::vector<int>::const_iterator iIt = neighbors.begin(); iIt != neighbors.end(); ++iIt)
{
if (pcl_isfinite (normals_->points[*iIt].normal_x) && pcl_isfinite (intensity_gradients_->points[*iIt].gradient [0]))
{
coefficients[ 0] += normals_->points[*iIt].normal_x * normals_->points[*iIt].normal_x;
coefficients[ 1] += normals_->points[*iIt].normal_x * normals_->points[*iIt].normal_y;
coefficients[ 2] += normals_->points[*iIt].normal_x * normals_->points[*iIt].normal_z;
coefficients[ 3] += normals_->points[*iIt].normal_x * intensity_gradients_->points[*iIt].gradient [0];
coefficients[ 4] += normals_->points[*iIt].normal_x * intensity_gradients_->points[*iIt].gradient [1];
coefficients[ 5] += normals_->points[*iIt].normal_x * intensity_gradients_->points[*iIt].gradient [2];
coefficients[ 6] += normals_->points[*iIt].normal_y * normals_->points[*iIt].normal_y;
coefficients[ 7] += normals_->points[*iIt].normal_y * normals_->points[*iIt].normal_z;
coefficients[ 8] += normals_->points[*iIt].normal_y * intensity_gradients_->points[*iIt].gradient [0];
coefficients[ 9] += normals_->points[*iIt].normal_y * intensity_gradients_->points[*iIt].gradient [1];
coefficients[10] += normals_->points[*iIt].normal_y * intensity_gradients_->points[*iIt].gradient [2];
coefficients[11] += normals_->points[*iIt].normal_z * normals_->points[*iIt].normal_z;
coefficients[12] += normals_->points[*iIt].normal_z * intensity_gradients_->points[*iIt].gradient [0];
coefficients[13] += normals_->points[*iIt].normal_z * intensity_gradients_->points[*iIt].gradient [1];
coefficients[14] += normals_->points[*iIt].normal_z * intensity_gradients_->points[*iIt].gradient [2];
coefficients[15] += intensity_gradients_->points[*iIt].gradient [0] * intensity_gradients_->points[*iIt].gradient [0];
coefficients[16] += intensity_gradients_->points[*iIt].gradient [0] * intensity_gradients_->points[*iIt].gradient [1];
coefficients[17] += intensity_gradients_->points[*iIt].gradient [0] * intensity_gradients_->points[*iIt].gradient [2];
coefficients[18] += intensity_gradients_->points[*iIt].gradient [1] * intensity_gradients_->points[*iIt].gradient [1];
coefficients[19] += intensity_gradients_->points[*iIt].gradient [1] * intensity_gradients_->points[*iIt].gradient [2];
coefficients[20] += intensity_gradients_->points[*iIt].gradient [2] * intensity_gradients_->points[*iIt].gradient [2];
++count;
}
}
if (count > 0)
{
float norm = 1.0 / float (count);
coefficients[ 0] *= norm;
coefficients[ 1] *= norm;
coefficients[ 2] *= norm;
coefficients[ 3] *= norm;
coefficients[ 4] *= norm;
coefficients[ 5] *= norm;
coefficients[ 6] *= norm;
coefficients[ 7] *= norm;
coefficients[ 8] *= norm;
coefficients[ 9] *= norm;
coefficients[10] *= norm;
coefficients[11] *= norm;
coefficients[12] *= norm;
coefficients[13] *= norm;
coefficients[14] *= norm;
coefficients[15] *= norm;
coefficients[16] *= norm;
coefficients[17] *= norm;
coefficients[18] *= norm;
coefficients[19] *= norm;
coefficients[20] *= norm;
}
}
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
template <typename PointInT, typename PointOutT, typename NormalT> void
pcl::HarrisKeypoint6D<PointInT, PointOutT, NormalT>::detectKeypoints (PointCloudOut &output)
{
if (normals_->empty ())
{
normals_->reserve (surface_->size ());
if (!surface_->isOrganized ())
{
pcl::NormalEstimation<PointInT, NormalT> normal_estimation;
normal_estimation.setInputCloud (surface_);
normal_estimation.setRadiusSearch (search_radius_);
normal_estimation.compute (*normals_);
}
else
{
IntegralImageNormalEstimation<PointInT, NormalT> normal_estimation;
normal_estimation.setNormalEstimationMethod (pcl::IntegralImageNormalEstimation<PointInT, NormalT>::SIMPLE_3D_GRADIENT);
normal_estimation.setInputCloud (surface_);
normal_estimation.setNormalSmoothingSize (5.0);
normal_estimation.compute (*normals_);
}
}
pcl::PointCloud<pcl::PointXYZI>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZI>);
cloud->resize (surface_->size ());
#ifdef _OPENMP
#pragma omp parallel for num_threads(threads_) default(shared)
#endif
for (unsigned idx = 0; idx < surface_->size (); ++idx)
{
cloud->points [idx].x = surface_->points [idx].x;
cloud->points [idx].y = surface_->points [idx].y;
cloud->points [idx].z = surface_->points [idx].z;
//grayscale = 0.2989 * R + 0.5870 * G + 0.1140 * B
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));
}
pcl::copyPointCloud (*surface_, *cloud);
IntensityGradientEstimation<PointXYZI, NormalT, IntensityGradient> grad_est;
grad_est.setInputCloud (cloud);
grad_est.setInputNormals (normals_);
grad_est.setRadiusSearch (search_radius_);
grad_est.compute (*intensity_gradients_);
#ifdef _OPENMP
#pragma omp parallel for num_threads(threads_) default (shared)
#endif
for (unsigned idx = 0; idx < intensity_gradients_->size (); ++idx)
{
float len = intensity_gradients_->points [idx].gradient_x * intensity_gradients_->points [idx].gradient_x +
intensity_gradients_->points [idx].gradient_y * intensity_gradients_->points [idx].gradient_y +
intensity_gradients_->points [idx].gradient_z * intensity_gradients_->points [idx].gradient_z ;
// Suat: ToDo: remove this magic number or expose using set/get
if (len > 200.0)
{
len = 1.0 / sqrt (len);
intensity_gradients_->points [idx].gradient_x *= len;
intensity_gradients_->points [idx].gradient_y *= len;
intensity_gradients_->points [idx].gradient_z *= len;
}
else
{
intensity_gradients_->points [idx].gradient_x = 0;
intensity_gradients_->points [idx].gradient_y = 0;
intensity_gradients_->points [idx].gradient_z = 0;
}
}
boost::shared_ptr<pcl::PointCloud<PointOutT> > response (new pcl::PointCloud<PointOutT> ());
response->points.reserve (input_->points.size());
responseTomasi(*response);
// just return the response
if (!nonmax_)
{
output = *response;
// we do not change the denseness in this case
output.is_dense = input_->is_dense;
for (size_t i = 0; i < response->size (); ++i)
keypoints_indices_->indices.push_back (i);
}
else
{
output.points.clear ();
output.points.reserve (response->points.size());
#ifdef _OPENMP
#pragma omp parallel for num_threads(threads_) default(shared)
#endif
for (size_t idx = 0; idx < response->points.size (); ++idx)
{
if (!isFinite (response->points[idx]) || response->points[idx].intensity < threshold_)
continue;
std::vector<int> nn_indices;
std::vector<float> nn_dists;
tree_->radiusSearch (idx, search_radius_, nn_indices, nn_dists);
bool is_maxima = true;
for (std::vector<int>::const_iterator iIt = nn_indices.begin(); iIt != nn_indices.end(); ++iIt)
{
if (response->points[idx].intensity < response->points[*iIt].intensity)
{
is_maxima = false;
break;
}
}
if (is_maxima)
#ifdef _OPENMP
#pragma omp critical
#endif
{
output.points.push_back (response->points[idx]);
keypoints_indices_->indices.push_back (idx);
}
}
if (refine_)
refineCorners (output);
output.height = 1;
output.width = static_cast<uint32_t> (output.points.size());
output.is_dense = true;
}
}
template <typename PointInT, typename PointOutT, typename NormalT> void
pcl::HarrisKeypoint6D<PointInT, PointOutT, NormalT>::responseTomasi (PointCloudOut &output) const
{
// get the 6x6 covar-mat
PointOutT pointOut;
PCL_ALIGN (16) float covar [21];
Eigen::SelfAdjointEigenSolver <Eigen::Matrix<float, 6, 6> > solver;
Eigen::Matrix<float, 6, 6> covariance;
#ifdef _OPENMP
#pragma omp parallel for default (shared) private (pointOut, covar, covariance, solver) num_threads(threads_)
#endif
for (unsigned pIdx = 0; pIdx < input_->size (); ++pIdx)
{
const PointInT& pointIn = input_->points [pIdx];
pointOut.intensity = 0.0; //std::numeric_limits<float>::quiet_NaN ();
if (isFinite (pointIn))
{
std::vector<int> nn_indices;
std::vector<float> nn_dists;
tree_->radiusSearch (pointIn, search_radius_, nn_indices, nn_dists);
calculateCombinedCovar (nn_indices, covar);
float trace = covar [0] + covar [6] + covar [11] + covar [15] + covar [18] + covar [20];
if (trace != 0)
{
covariance.coeffRef ( 0) = covar [ 0];
covariance.coeffRef ( 1) = covar [ 1];
covariance.coeffRef ( 2) = covar [ 2];
covariance.coeffRef ( 3) = covar [ 3];
covariance.coeffRef ( 4) = covar [ 4];
covariance.coeffRef ( 5) = covar [ 5];
covariance.coeffRef ( 7) = covar [ 6];
covariance.coeffRef ( 8) = covar [ 7];
covariance.coeffRef ( 9) = covar [ 8];
covariance.coeffRef (10) = covar [ 9];
covariance.coeffRef (11) = covar [10];
covariance.coeffRef (14) = covar [11];
covariance.coeffRef (15) = covar [12];
covariance.coeffRef (16) = covar [13];
covariance.coeffRef (17) = covar [14];
covariance.coeffRef (21) = covar [15];
covariance.coeffRef (22) = covar [16];
covariance.coeffRef (23) = covar [17];
covariance.coeffRef (28) = covar [18];
covariance.coeffRef (29) = covar [19];
covariance.coeffRef (35) = covar [20];
covariance.coeffRef ( 6) = covar [ 1];
covariance.coeffRef (12) = covar [ 2];
covariance.coeffRef (13) = covar [ 7];
covariance.coeffRef (18) = covar [ 3];
covariance.coeffRef (19) = covar [ 8];
covariance.coeffRef (20) = covar [12];
covariance.coeffRef (24) = covar [ 4];
covariance.coeffRef (25) = covar [ 9];
covariance.coeffRef (26) = covar [13];
covariance.coeffRef (27) = covar [16];
covariance.coeffRef (30) = covar [ 5];
covariance.coeffRef (31) = covar [10];
covariance.coeffRef (32) = covar [14];
covariance.coeffRef (33) = covar [17];
covariance.coeffRef (34) = covar [19];
solver.compute (covariance);
pointOut.intensity = solver.eigenvalues () [3];
}
}
pointOut.x = pointIn.x;
pointOut.y = pointIn.y;
pointOut.z = pointIn.z;
#ifdef _OPENMP
#pragma omp critical
#endif
output.points.push_back(pointOut);
}
output.height = input_->height;
output.width = input_->width;
}
template <typename PointInT, typename PointOutT, typename NormalT> void
pcl::HarrisKeypoint6D<PointInT, PointOutT, NormalT>::refineCorners (PointCloudOut &corners) const
{
pcl::search::KdTree<PointInT> search;
search.setInputCloud(surface_);
Eigen::Matrix3f nnT;
Eigen::Matrix3f NNT;
Eigen::Vector3f NNTp;
const Eigen::Vector3f* normal;
const Eigen::Vector3f* point;
float diff;
const unsigned max_iterations = 10;
for (typename PointCloudOut::iterator cornerIt = corners.begin(); cornerIt != corners.end(); ++cornerIt)
{
unsigned iterations = 0;
do {
NNT.setZero();
NNTp.setZero();
PointInT corner;
corner.x = cornerIt->x;
corner.y = cornerIt->y;
corner.z = cornerIt->z;
std::vector<int> nn_indices;
std::vector<float> nn_dists;
search.radiusSearch (corner, search_radius_, nn_indices, nn_dists);
for (std::vector<int>::const_iterator iIt = nn_indices.begin(); iIt != nn_indices.end(); ++iIt)
{
normal = reinterpret_cast<const Eigen::Vector3f*> (&(normals_->points[*iIt].normal_x));
point = reinterpret_cast<const Eigen::Vector3f*> (&(surface_->points[*iIt].x));
nnT = (*normal) * (normal->transpose());
NNT += nnT;
NNTp += nnT * (*point);
}
if (NNT.determinant() != 0)
*(reinterpret_cast<Eigen::Vector3f*>(&(cornerIt->x))) = NNT.inverse () * NNTp;
diff = (cornerIt->x - corner.x) * (cornerIt->x - corner.x) +
(cornerIt->y - corner.y) * (cornerIt->y - corner.y) +
(cornerIt->z - corner.z) * (cornerIt->z - corner.z);
} while (diff > 1e-6 && ++iterations < max_iterations);
}
}
#define PCL_INSTANTIATE_HarrisKeypoint6D(T,U,N) template class PCL_EXPORTS pcl::HarrisKeypoint6D<T,U,N>;
#endif // #ifndef PCL_HARRIS_KEYPOINT_6D_IMPL_H_
pcl::HarrisKeypoint6D::calculateCombinedCovar
void calculateCombinedCovar(const std::vector< int > &neighbors, float *coefficients) const
Definition: harris_6d.hpp:76
pcl::search::KdTree
search::KdTree is a wrapper class which inherits the pcl::KdTree class for performing search function...
Definition: kdtree.h:62
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.
Definition: point_tests.h:53
pcl::PointCloud::points
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
Definition: point_cloud.h:410
pcl::HarrisKeypoint6D::refineCorners
void refineCorners(PointCloudOut &corners) const
Definition: harris_6d.hpp:361
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::PointCloud::end
iterator end()
Definition: point_cloud.h:435
pcl::HarrisKeypoint6D::responseTomasi
void responseTomasi(PointCloudOut &output) const
Definition: harris_6d.hpp:270
pcl::copyPointCloud
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.
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:200
pcl::PointCloud::iterator
VectorType::iterator iterator
Definition: point_cloud.h:432
pcl::NormalEstimation
NormalEstimation estimates local surface properties (surface normals and curvatures)at each 3D point...
Definition: normal_3d.h:197
pcl::HarrisKeypoint6D::detectKeypoints
void detectKeypoints(PointCloudOut &output)
Abstract key point detection method.
Definition: harris_6d.hpp:143
pcl::PointCloud::height
uint32_t height
The point cloud height (if organized as an image-structure).
Definition: point_cloud.h:415
pcl::PointCloud::Ptr
boost::shared_ptr< PointCloud< PointT > > Ptr
Definition: point_cloud.h:428
pcl::PointCloud::width
uint32_t width
The point cloud width (if organized as an image-structure).
Definition: point_cloud.h:413
pcl::IntegralImageNormalEstimation
Surface normal estimation on organized data using integral images.
Definition: integral_image_normal.h:68
pcl::HarrisKeypoint6D::setRadius
void setRadius(float radius)
set the radius for normal estimation and non maxima supression.
Definition: harris_6d.hpp:57
pcl::NormalEstimation::setInputCloud
virtual void setInputCloud(const PointCloudConstPtr &cloud)
Provide a pointer to the input dataset.
Definition: normal_3d.h:286
pcl::HarrisKeypoint6D::setRefine
void setRefine(bool do_refine)
whether the detected key points should be refined or not.
Definition: harris_6d.hpp:63
pcl::PointCloud
PointCloud represents the base class in PCL for storing collections of 3D points. ...
Definition: projection_matrix.h:53
pcl::IntegralImageNormalEstimation::setNormalEstimationMethod
void setNormalEstimationMethod(NormalEstimationMethod normal_estimation_method)
Set the normal estimation method.
Definition: integral_image_normal.h:217
pcl::IntensityGradientEstimation
IntensityGradientEstimation estimates the intensity gradient for a point cloud that contains position...
Definition: intensity_gradient.h:56
pcl::search::KdTree::setInputCloud
void setInputCloud(const PointCloudConstPtr &cloud, const IndicesConstPtr &indices=IndicesConstPtr())
Provide a pointer to the input dataset.
Definition: kdtree.hpp:77
pcl::HarrisKeypoint6D::setThreshold
void setThreshold(float threshold)
set the threshold value for detecting corners.
Definition: harris_6d.hpp:51
pcl::IntegralImageNormalEstimation::setInputCloud
virtual void setInputCloud(const typename PointCloudIn::ConstPtr &cloud)
Provide a pointer to the input dataset (overwrites the PCLBase::setInputCloud method) ...
Definition: integral_image_normal.h:235
pcl::PCLBase< PointInT >::setInputCloud
virtual void setInputCloud(const PointCloudConstPtr &cloud)
Provide a pointer to the input dataset.
pcl::PointCloud::is_dense
bool is_dense
True if no points are invalid (e.g., have NaN or Inf values).
Definition: point_cloud.h:418
pcl::search::KdTree::radiusSearch
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.
Definition: kdtree.hpp:97
pcl::PointCloud::begin
iterator begin()
Definition: point_cloud.h:434
pcl::PointCloud::resize
void resize(size_t n)
Resize the cloud.
Definition: point_cloud.h:447
pcl::Feature::compute
void compute(PointCloudOut &output)
Base method for feature estimation for all points given in <setInputCloud (), setIndices ()> using th...
Definition: feature.hpp:189
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:344
pcl::IntegralImageNormalEstimation::setNormalSmoothingSize
void setNormalSmoothingSize(float normal_smoothing_size)
Set the normal smoothing size.
Definition: integral_image_normal.h:193