harris_keypoint3D.hpp

Go to the documentation of this file.

/*
 * 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_3D_IMPL_H_
#define PCL_HARRIS_KEYPOINT_3D_IMPL_H_

#include "pcl/keypoints/harris_keypoint3D.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/integral_image_normal.h>
#include <pcl/common/time.h>
#include <pcl/common/centroid.h>
#ifdef __SSE__
#include <xmmintrin.h>
#endif

template <typename PointInT, typename PointOutT, typename NormalT> void
pcl::HarrisKeypoint3D<PointInT, PointOutT, NormalT>::setMethod (ResponseMethod method)
{
  method_ = method;
}

template <typename PointInT, typename PointOutT, typename NormalT> void
pcl::HarrisKeypoint3D<PointInT, PointOutT, NormalT>::setThreshold (float threshold)
{
  threshold_= threshold;
}

template <typename PointInT, typename PointOutT, typename NormalT> void
pcl::HarrisKeypoint3D<PointInT, PointOutT, NormalT>::setRadius (float radius)
{
  search_radius_ = radius;
}

template <typename PointInT, typename PointOutT, typename NormalT> void
pcl::HarrisKeypoint3D<PointInT, PointOutT, NormalT>::setRefine (bool do_refine)
{
  refine_ = do_refine;
}

template <typename PointInT, typename PointOutT, typename NormalT> void
pcl::HarrisKeypoint3D<PointInT, PointOutT, NormalT>::setNonMaxSupression (bool nonmax)
{
  nonmax_ = nonmax;
}

template <typename PointInT, typename PointOutT, typename NormalT> void
pcl::HarrisKeypoint3D<PointInT, PointOutT, NormalT>::setNormals (boost::shared_ptr<pcl::PointCloud<NormalT> > normals ) const
{
  normals_.reset (normals);
}

template <typename PointInT, typename PointOutT, typename NormalT> void
pcl::HarrisKeypoint3D<PointInT, PointOutT, NormalT>::calculateNormalCovar (const std::vector<int>& neighbors, float* coefficients) const
{
  unsigned count = 0;
  // indices        0   1   2   3   4   5   6   7
  // coefficients: xx  xy  xz  ??  yx  yy  yz  ??
#ifdef __SSE__
  // accumulator for xx, xy, xz
  __m128 vec1 = _mm_setzero_ps();
  // accumulator for yy, yz, zz
  __m128 vec2 = _mm_setzero_ps();

  __m128 norm1;

  __m128 norm2;

  float zz = 0;

  for (std::vector<int>::const_iterator iIt = neighbors.begin(); iIt != neighbors.end(); ++iIt)
  {
    if (pcl_isfinite (normals_->points[*iIt].normal_x))
    {
      // nx, ny, nz, h
      norm1 = _mm_load_ps (&(normals_->points[*iIt].normal_x));

      // nx, nx, nx, nx
      norm2 = _mm_set1_ps (normals_->points[*iIt].normal_x);

      // nx * nx, nx * ny, nx * nz, nx * h
      norm2 = _mm_mul_ps (norm1, norm2);

      // accumulate
      vec1 = _mm_add_ps (vec1, norm2);

      // ny, ny, ny, ny
      norm2 = _mm_set1_ps (normals_->points[*iIt].normal_y);

      // ny * nx, ny * ny, ny * nz, ny * h
      norm2 = _mm_mul_ps (norm1, norm2);

      // accumulate
      vec2 = _mm_add_ps (vec2, norm2);

      zz += normals_->points[*iIt].normal_z * normals_->points[*iIt].normal_z;
      ++count;
    }
  }
  if (count > 0)
  {
    norm2 = _mm_set1_ps (float(count));
    vec1 = _mm_div_ps (vec1, norm2);
    vec2 = _mm_div_ps (vec2, norm2);
    _mm_store_ps (coefficients, vec1);
    _mm_store_ps (coefficients + 4, vec2);
    coefficients [7] = zz / float(count);
  }
  else
    memset (coefficients, 0, sizeof (float) * 8);
#else
  memset (coefficients, 0, sizeof (float) * 8);
  for (std::vector<int>::const_iterator iIt = neighbors.begin(); iIt != neighbors.end(); ++iIt)
  {
    if (pcl_isfinite (normals_->points[*iIt].normal_x))
    {
      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[5] += normals_->points[*iIt].normal_y * normals_->points[*iIt].normal_y;
      coefficients[6] += normals_->points[*iIt].normal_y * normals_->points[*iIt].normal_z;
      coefficients[7] += normals_->points[*iIt].normal_z * normals_->points[*iIt].normal_z;

      ++count;
    }
  }
  if (count > 0)
  {
    float norm = 1.0 / float (count);
    coefficients[0] *= norm;
    coefficients[1] *= norm;
    coefficients[2] *= norm;
    coefficients[5] *= norm;
    coefficients[6] *= norm;
    coefficients[7] *= norm;
  }
#endif
}

template <typename PointInT, typename PointOutT, typename NormalT> void
pcl::HarrisKeypoint3D<PointInT, PointOutT, NormalT>::detectKeypoints (PointCloudOut &output)
{
  if (normals_->empty ())
  {
    normals_->reserve (surface_->size ());
    if (input_->height == 1 ) // not organized
    {
      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_);
    }
  }

  boost::shared_ptr<pcl::PointCloud<PointOutT> > response (new pcl::PointCloud<PointOutT> ());

  response->points.reserve (input_->points.size());

  switch (method_)
  {
    case HARRIS:
      responseHarris(*response);
      break;
    case NOBLE:
      responseNoble(*response);
      break;
    case LOWE:
      responseLowe(*response);
      break;
    case CURVATURE:
      responseCurvature(*response);
      break;
    case TOMASI:
      responseTomasi(*response);
      break;
  }

  if (!nonmax_)
    output = *response;
  else
  {
    output.points.clear ();
    output.points.reserve (response->points.size());
    std::vector<int> nn_indices;
    std::vector<float> nn_dists;

#if defined __GNUC__
#  if __GNUC__ == 4 && __GNUC_MINOR__ > 3
#    pragma omp parallel for shared (output) private (nn_indices, nn_dists) num_threads(threads_)
#  endif
#endif
    for (int idx = 0; idx < (int) response->points.size (); ++idx)
    {
      if (!isFinite (response->points[idx]) || response->points[idx].intensity < threshold_)
        continue;
      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)
        #pragma omp critical
        output.points.push_back (response->points[idx]);
    }

    if (refine_)
      refineCorners (output);

    output.height = 1;
    output.width = output.points.size();
  }

  // we don not change the denseness
  output.is_dense = input_->is_dense;
}

template <typename PointInT, typename PointOutT, typename NormalT> void
pcl::HarrisKeypoint3D<PointInT, PointOutT, NormalT>::responseHarris (PointCloudOut &output) const
{
  PCL_ALIGN (16) float covar [8];
  std::vector<int> nn_indices;
  std::vector<float> nn_dists;
  output.resize (input_->size ());
#if defined __GNUC__
#  if __GNUC__ == 4 && __GNUC_MINOR__ > 3
#    pragma omp parallel for shared (output) private (covar, nn_indices, nn_dists) num_threads(threads_)
#  endif
#endif
  for (int pIdx = 0; pIdx < (int) input_->size (); ++pIdx)
  {
    const PointInT& pointIn = input_->points [pIdx];
    output [pIdx].intensity = 0.0; //std::numeric_limits<float>::quiet_NaN ();
    if (isFinite (pointIn))
    {
      tree_->radiusSearch (pointIn, search_radius_, nn_indices, nn_dists);
      calculateNormalCovar (nn_indices, covar);

      float trace = covar [0] + covar [5] + covar [7];
      if (trace != 0)
      {
        float det = covar [0] * covar [5] * covar [7] + 2.0 * covar [1] * covar [2] * covar [6]
                  - covar [2] * covar [2] * covar [5]
                  - covar [1] * covar [1] * covar [7]
                  - covar [6] * covar [6] * covar [0];

        output [pIdx].intensity = 0.04 + det - 0.04 * trace * trace;
      }
    }
    output [pIdx].x = pointIn.x;
    output [pIdx].y = pointIn.y;
    output [pIdx].z = pointIn.z;
  }
  output.height = input_->height;
  output.width = input_->width;
}

template <typename PointInT, typename PointOutT, typename NormalT> void
pcl::HarrisKeypoint3D<PointInT, PointOutT, NormalT>::responseNoble (PointCloudOut &output) const
{
  std::vector<int> nn_indices;
  std::vector<float> nn_dists;
  PCL_ALIGN (16) float covar [8];
  output.resize (input_->size ());
#if defined __GNUC__
#  if __GNUC__ == 4 && __GNUC_MINOR__ > 3
#    pragma omp parallel for shared (output) private (covar, nn_indices, nn_dists) num_threads(threads_)
#  endif
#endif
  for (int pIdx = 0; pIdx < (int) input_->size (); ++pIdx)
  {
    const PointInT& pointIn = input_->points [pIdx];
    output [pIdx].intensity = 0.0;
    if (isFinite (pointIn))
    {
      tree_->radiusSearch (pointIn, search_radius_, nn_indices, nn_dists);
      calculateNormalCovar (nn_indices, covar);
      float trace = covar [0] + covar [5] + covar [7];
      if (trace != 0)
      {
        float det = covar [0] * covar [5] * covar [7] + 2.0 * covar [1] * covar [2] * covar [6]
                  - covar [2] * covar [2] * covar [5]
                  - covar [1] * covar [1] * covar [7]
                  - covar [6] * covar [6] * covar [0];

        output [pIdx].intensity = det / trace;
      }
    }
    output [pIdx].x = pointIn.x;
    output [pIdx].y = pointIn.y;
    output [pIdx].z = pointIn.z;
  }
  output.height = input_->height;
  output.width = input_->width;
}

template <typename PointInT, typename PointOutT, typename NormalT> void
pcl::HarrisKeypoint3D<PointInT, PointOutT, NormalT>::responseLowe (PointCloudOut &output) const
{
  std::vector<int> nn_indices;
  std::vector<float> nn_dists;
  PCL_ALIGN (16) float covar [8];
  output.resize (input_->size ());
#if defined __GNUC__
#  if __GNUC__ == 4 && __GNUC_MINOR__ > 3
#    pragma omp parallel for shared (output) private (covar, nn_indices, nn_dists) num_threads(threads_)
#  endif
#endif
  for (int pIdx = 0; pIdx < (int) input_->size (); ++pIdx)
  {
    const PointInT& pointIn = input_->points [pIdx];
    output [pIdx].intensity = 0.0;
    if (isFinite (pointIn))
    {
      tree_->radiusSearch (pointIn, search_radius_, nn_indices, nn_dists);
      calculateNormalCovar (nn_indices, covar);
      float trace = covar [0] + covar [5] + covar [7];
      if (trace != 0)
      {
        float det = covar [0] * covar [5] * covar [7] + 2.0 * covar [1] * covar [2] * covar [6]
                  - covar [2] * covar [2] * covar [5]
                  - covar [1] * covar [1] * covar [7]
                  - covar [6] * covar [6] * covar [0];

        output [pIdx].intensity = det / (trace * trace);
      }
    }
    output [pIdx].x = pointIn.x;
    output [pIdx].y = pointIn.y;
    output [pIdx].z = pointIn.z;
  }
  output.height = input_->height;
  output.width = input_->width;
}

template <typename PointInT, typename PointOutT, typename NormalT> void
pcl::HarrisKeypoint3D<PointInT, PointOutT, NormalT>::responseCurvature (PointCloudOut &output) const
{
  PointOutT point;
  for (unsigned idx = 0; idx < input_->points.size(); ++idx)
  {
    point.x = input_->points[idx].x;
    point.y = input_->points[idx].y;
    point.z = input_->points[idx].z;
    point.intensity = normals_->points[idx].curvature;
    output.points.push_back(point);
  }
  // does not change the order
  output.height = input_->height;
  output.width = input_->width;
}

template <typename PointInT, typename PointOutT, typename NormalT> void
pcl::HarrisKeypoint3D<PointInT, PointOutT, NormalT>::responseTomasi (PointCloudOut &output) const
{
  std::vector<int> nn_indices;
  std::vector<float> nn_dists;
  PCL_ALIGN (16) float covar [8];
  Eigen::Matrix3f covariance_matrix;
  output.resize (input_->size ());
#if defined __GNUC__
#  if __GNUC__ == 4 && __GNUC_MINOR__ > 3
#    pragma omp parallel for shared (output) private (covar, nn_indices, nn_dists, covariance_matrix) num_threads(threads_)
#  endif
#endif
  for (int pIdx = 0; pIdx < (int) input_->size (); ++pIdx)
  {
    const PointInT& pointIn = input_->points [pIdx];
    output [pIdx].intensity = 0.0;
    if (isFinite (pointIn))
    {
      tree_->radiusSearch (pointIn, search_radius_, nn_indices, nn_dists);
      calculateNormalCovar (nn_indices, covar);
      float trace = covar [0] + covar [5] + covar [7];
      if (trace != 0)
      {
        covariance_matrix.coeffRef (0) = covar [0];
        covariance_matrix.coeffRef (1) = covariance_matrix.coeffRef (3) = covar [1];
        covariance_matrix.coeffRef (2) = covariance_matrix.coeffRef (6) = covar [2];
        covariance_matrix.coeffRef (4) = covar [5];
        covariance_matrix.coeffRef (5) = covariance_matrix.coeffRef (7) = covar [6];
        covariance_matrix.coeffRef (8) = covar [7];

        EIGEN_ALIGN16 Eigen::Vector3f eigen_values;
        pcl::eigen33(covariance_matrix, eigen_values);
        output [pIdx].intensity = eigen_values[0];
      }
    }
    output [pIdx].x = pointIn.x;
    output [pIdx].y = pointIn.y;
    output [pIdx].z = pointIn.z;
  }
  output.height = input_->height;
  output.width = input_->width;
}

template <typename PointInT, typename PointOutT, typename NormalT> void
pcl::HarrisKeypoint3D<PointInT, PointOutT, NormalT>::refineCorners (PointCloudOut &corners) const
{
  std::vector<int> nn_indices;
  std::vector<float> nn_dists;

  Eigen::Matrix3f nnT;
  Eigen::Matrix3f NNT;
  Eigen::Matrix3f NNTInv;
  Eigen::Vector3f NNTp;
  float diff;
  const unsigned max_iterations = 10;
#if defined __GNUC__
#  if __GNUC__ == 4 && __GNUC_MINOR__ > 3
#    pragma omp parallel for shared (corners) private (nnT, NNT, NNTInv, NNTp, diff, nn_indices, nn_dists) num_threads(threads_)
#  endif
#endif
  //for (typename PointCloudOut::iterator cornerIt = corners.begin(); cornerIt != corners.end(); ++cornerIt)
  for (int cIdx = 0; cIdx < (int) corners.size (); ++cIdx)
  {
    unsigned iterations = 0;
    do {
      NNT.setZero();
      NNTp.setZero();
      PointInT corner;
      corner.x = corners[cIdx].x;
      corner.y = corners[cIdx].y;
      corner.z = corners[cIdx].z;
      tree_->radiusSearch (corner, search_radius_, nn_indices, nn_dists);
      for (std::vector<int>::const_iterator iIt = nn_indices.begin(); iIt != nn_indices.end(); ++iIt)
      {
        if (!pcl_isfinite (normals_->points[*iIt].normal_x))
          continue;

        nnT = normals_->points[*iIt].getNormalVector3fMap () * normals_->points[*iIt].getNormalVector3fMap ().transpose();
        NNT += nnT;
        NNTp += nnT * surface_->points[*iIt].getVector3fMap ();
      }
      if (invert3x3SymMatrix (NNT, NNTInv) != 0)
        corners[cIdx].getVector3fMap () = NNTInv * NNTp;

      diff = (corners[cIdx].getVector3fMap () - corner.getVector3fMap()).squaredNorm ();
//      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_HarrisKeypoint3D(T,U,N) template class PCL_EXPORTS pcl::HarrisKeypoint3D<T,U,N>;
#endif // #ifndef PCL_HARRIS_KEYPOINT_3D_IMPL_H_