shot.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_FEATURES_IMPL_SHOT_H_
#define PCL_FEATURES_IMPL_SHOT_H_

#include "pcl/features/shot.h"
#include "pcl/features/shot_common.h"
#include <utility>


inline bool
areEquals (double val1, double val2, double zeroDoubleEps = zeroDoubleEps15)
{
  return (fabs (val1 - val2)<zeroDoubleEps);
}


inline bool
areEquals (float val1, float val2, float zeroFloatEps = zeroFloatEps8)
{
  return (fabs (val1 - val2)<zeroFloatEps);
}

template <typename PointNT, typename PointOutT> float
pcl::SHOTEstimation<pcl::PointXYZRGBA, PointNT, PointOutT>::sRGB_LUT[256] = {- 1};

template <typename PointNT, typename PointOutT> float
pcl::SHOTEstimation<pcl::PointXYZRGBA, PointNT, PointOutT>::sXYZ_LUT[4000] = {- 1};

template <typename PointNT, typename PointOutT> void
pcl::SHOTEstimation<pcl::PointXYZRGBA, PointNT, PointOutT>::RGB2CIELAB (unsigned char R, unsigned char G,
                                                                        unsigned char B, float &L, float &A,
                                                                        float &B2)
{
  if (sRGB_LUT[0] < 0)
  {
    for (int i = 0; i < 256; i++)
    {
      float f = i / 255.0f;
      if (f > 0.04045)
        sRGB_LUT[i] = (float)pow ((f + 0.055) / 1.055, 2.4);
      else
        sRGB_LUT[i] = f / 12.92;
    }

    for (int i = 0; i < 4000; i++)
    {
      float f = i / 4000.0f;
      if (f > 0.008856)
        sXYZ_LUT[i] = pow (f, (float)0.3333);
      else
        sXYZ_LUT[i] = (7.787 * f) + (16.0 / 116.0);
    }
  }

  float fr = sRGB_LUT[R];
  float fg = sRGB_LUT[G];
  float fb = sRGB_LUT[B];

  // Use white = D65
  const float x = fr * 0.412453 + fg * 0.357580 + fb * 0.180423;
  const float y = fr * 0.212671 + fg * 0.715160 + fb * 0.072169;
  const float z = fr * 0.019334 + fg * 0.119193 + fb * 0.950227;

  float vx = x / 0.95047;
  float vy = y;
  float vz = z / 1.08883;

  vx = sXYZ_LUT[int(vx*4000)];
  vy = sXYZ_LUT[int(vy*4000)];
  vz = sXYZ_LUT[int(vz*4000)];

  L = 116.0 * vy - 16.0;
  if (L>100)
    L=100;

  A = 500.0 * (vx - vy);
  if (A>120)
    A=120;
  else if (A<- 120)
    A=- 120;

  B2 = 200.0 * (vy - vz);
  if (B2>120)
    B2=120;
  else if (B2<- 120)
    B2=- 120;
}

template <typename PointInT, typename PointNT, typename PointOutT> void
pcl::SHOTEstimationBase<PointInT, PointNT, PointOutT>::createBinDistanceShape (
    int index,
    const std::vector<int> &indices, 
    const std::vector<float> &sqr_dists,
    const pcl::PointCloud<PointInT> &input,
    const pcl::PointCloud<PointNT> &normals,
    const pcl::PointCloud<PointInT> &surface,
    double search_radius,
    std::vector<double> &bin_distance_shape,
    std::vector<Eigen::Vector4f, Eigen::aligned_allocator<Eigen::Vector4f> > &rf)
{
  bin_distance_shape.resize (indices.size ());

  if (rf.size () != 3)
    rf.resize (3);

  Eigen::Vector4f central_point = input.points[index].getVector4fMap ();
  central_point[3] = 0;
  if (pcl::getLocalRF (surface, search_radius, central_point, indices, sqr_dists, rf))
    return;

  for (size_t i_idx = 0; i_idx < indices.size (); ++i_idx)
  {
    double cosineDesc = normals.points[indices[i_idx]].getNormalVector4fMap ().dot (rf[2]); //feat[i].rf[6]*normal[0] + feat[i].rf[7]*normal[1] + feat[i].rf[8]*normal[2];

    if (cosineDesc > 1.0)
      cosineDesc = 1.0;
    if (cosineDesc < - 1.0)
      cosineDesc = - 1.0;

    bin_distance_shape[i_idx] = ((1.0 + cosineDesc) * nr_shape_bins_) / 2;
  }
}

template <typename PointInT, typename PointNT, typename PointOutT> void
pcl::SHOTEstimationBase<PointInT, PointNT, PointOutT>::normalizeHistogram (
    Eigen::VectorXf &shot, int desc_length)
{
  double acc_norm = 0;
  for (int j = 0; j < desc_length; j++)
    acc_norm += shot[j] * shot[j];
  acc_norm = sqrt (acc_norm);
  for (int j = 0; j < desc_length; j++)
    shot[j] /= acc_norm;
}

template <typename PointInT, typename PointNT, typename PointOutT> void
pcl::SHOTEstimationBase<PointInT, PointNT, PointOutT>::interpolateSingleChannel (
    const std::vector<int> &indices,
    const std::vector<float> &sqr_dists,
    const Eigen::Vector4f &central_point,
    const std::vector<Eigen::Vector4f, Eigen::aligned_allocator<Eigen::Vector4f> > &rf,
    std::vector<double> &binDistance,
    const int nr_bins,
    Eigen::VectorXf &shot)
{
  if (rf.size () != 3)
  {
    PCL_ERROR ("[pcl::%s::interpolateSingleChannel] RF size different than 9! Aborting...\n");
    return;
  }

  for (size_t i_idx = 0; i_idx < indices.size (); ++i_idx)
  {
    Eigen::Vector4f delta = surface_->points[indices[i_idx]].getVector4fMap () - central_point;
  delta[3] = 0;

    // Compute the Euclidean norm
   double distance = sqrt (sqr_dists[i_idx]);

    if (areEquals (distance, 0.0))
      continue;

    double xInFeatRef = delta.dot (rf[0]); //(x * feat[i].rf[0] + y * feat[i].rf[1] + z * feat[i].rf[2]);
    double yInFeatRef = delta.dot (rf[1]); //(x * feat[i].rf[3] + y * feat[i].rf[4] + z * feat[i].rf[5]);
    double zInFeatRef = delta.dot (rf[2]); //(x * feat[i].rf[6] + y * feat[i].rf[7] + z * feat[i].rf[8]);

    // To avoid numerical problems afterwards
    if (fabs (yInFeatRef) < 1E-30)
      yInFeatRef  = 0;
    if (fabs (xInFeatRef) < 1E-30)
      xInFeatRef  = 0;
    if (fabs (zInFeatRef) < 1E-30)
      zInFeatRef  = 0;


    unsigned char bit4 = ((yInFeatRef > 0) || ((yInFeatRef == 0.0) && (xInFeatRef < 0))) ? 1 : 0;
    unsigned char bit3 = ((xInFeatRef > 0) || ((xInFeatRef == 0.0) && (yInFeatRef > 0))) ? !bit4 : bit4;

    assert (bit3 == 0 || bit3 == 1);

    int desc_index = (bit4<<3) + (bit3<<2);

    desc_index = desc_index << 1;

    if ((xInFeatRef * yInFeatRef > 0) || (xInFeatRef == 0.0))
      desc_index += (fabs (xInFeatRef) >= fabs (yInFeatRef)) ? 0 : 4;
    else
      desc_index += (fabs (xInFeatRef) > fabs (yInFeatRef)) ? 4 : 0;

    desc_index += zInFeatRef > 0 ? 1 : 0;

    // 2 RADII
    desc_index += (distance > radius1_2_) ? 2 : 0;

    int step_index = static_cast<int>(floor (binDistance[i_idx] +0.5));
    int volume_index = desc_index * (nr_bins+1);

    //Interpolation on the cosine (adjacent bins in the histogram)
    binDistance[i_idx] -= step_index;
    double intWeight = (1- fabs (binDistance[i_idx]));

    if (binDistance[i_idx] > 0)
      shot[volume_index + ((step_index+1) % nr_bins)] += binDistance[i_idx];
    else
      shot[volume_index + ((step_index - 1 + nr_bins) % nr_bins)] += - binDistance[i_idx];

    //Interpolation on the distance (adjacent husks)
   
    if (distance > radius1_2_)   //external sphere
    {
      double radiusDistance = (distance - radius3_4_) / radius1_2_;

      if (distance > radius3_4_) //most external sector, votes only for itself
        intWeight += 1 - radiusDistance;  //peso=1-d
      else  //3/4 of radius, votes also for the internal sphere
      {
        intWeight += 1 + radiusDistance;
        shot[(desc_index - 2) * (nr_bins+1) + step_index] -= radiusDistance;
      }
    }
    else    //internal sphere
    {
      double radiusDistance = (distance - radius1_4_) / radius1_2_;

      if (distance < radius1_4_) //most internal sector, votes only for itself
        intWeight += 1 + radiusDistance;  //weight=1-d
      else  //3/4 of radius, votes also for the external sphere
      {
        intWeight += 1 - radiusDistance;
        shot[(desc_index + 2) * (nr_bins+1) + step_index] += radiusDistance;
      }
    }

    //Interpolation on the inclination (adjacent vertical volumes)
    double inclinationCos = zInFeatRef / distance;
    if (inclinationCos < - 1.0)
      inclinationCos = - 1.0;
    if (inclinationCos > 1.0)
      inclinationCos = 1.0;

    double inclination = acos (inclinationCos);

    assert (inclination >= 0.0 && inclination <= PST_RAD_180);

    if (inclination > PST_RAD_90 || (fabs (inclination - PST_RAD_90) < 1e-30 && zInFeatRef <= 0))
    {
      double inclinationDistance = (inclination - PST_RAD_135) / PST_RAD_90;
      if (inclination > PST_RAD_135)
        intWeight += 1 - inclinationDistance;
      else
      {
        intWeight += 1 + inclinationDistance;
        assert ((desc_index + 1) * (nr_bins+1) + step_index >= 0 && (desc_index + 1) * (nr_bins+1) + step_index < descLength_);
        shot[(desc_index + 1) * (nr_bins+1) + step_index] -= inclinationDistance;
      }
    }
    else
    {
      double inclinationDistance = (inclination - PST_RAD_45) / PST_RAD_90;
      if (inclination < PST_RAD_45)
        intWeight += 1 + inclinationDistance;
      else
      {
        intWeight += 1 - inclinationDistance;
        assert ((desc_index - 1) * (nr_bins+1) + step_index >= 0 && (desc_index - 1) * (nr_bins+1) + step_index < descLength_);
        shot[(desc_index - 1) * (nr_bins+1) + step_index] += inclinationDistance;
      }
    }

    if (yInFeatRef != 0.0 || xInFeatRef != 0.0)
    {
      //Interpolation on the azimuth (adjacent horizontal volumes)
      double azimuth = atan2 (yInFeatRef, xInFeatRef);

      int sel = desc_index >> 2;
      double angularSectorSpan = PST_RAD_45;
      double angularSectorStart = - PST_RAD_PI_7_8;

      double azimuthDistance = (azimuth - (angularSectorStart + angularSectorSpan*sel)) / angularSectorSpan;

      assert ((azimuthDistance < 0.5 || areEquals (azimuthDistance, 0.5)) && (azimuthDistance > - 0.5 || areEquals (azimuthDistance, - 0.5)));

      azimuthDistance = (std::max)(- 0.5, std::min (azimuthDistance, 0.5));

      if (azimuthDistance > 0)
      {
        intWeight += 1 - azimuthDistance;
        int interp_index = (desc_index + 4) % maxAngularSectors_;
        assert (interp_index * (nr_bins+1) + step_index >= 0 && interp_index * (nr_bins+1) + step_index < descLength_);
        shot[interp_index * (nr_bins+1) + step_index] += azimuthDistance;
      }
      else
      {
        int interp_index = (desc_index - 4 + maxAngularSectors_) % maxAngularSectors_;
        assert (interp_index * (nr_bins+1) + step_index >= 0 && interp_index * (nr_bins+1) + step_index < descLength_);
        intWeight += 1 + azimuthDistance;
        shot[interp_index * (nr_bins+1) + step_index] -= azimuthDistance;
      }

    }

    assert (volume_index + step_index >= 0 &&  volume_index + step_index < descLength_);
    shot[volume_index + step_index] += intWeight;
  }
}

template <typename PointNT, typename PointOutT> void
pcl::SHOTEstimation<pcl::PointXYZRGBA, PointNT, PointOutT>::interpolateDoubleChannel (
  const std::vector<int> &indices, 
  const std::vector<float> &sqr_dists,
  const Eigen::Vector4f &central_point, 
  const std::vector<Eigen::Vector4f, Eigen::aligned_allocator<Eigen::Vector4f> > &rf,
  std::vector<double> &binDistanceShape,
  std::vector<double> &binDistanceColor, 
  const int nr_bins_shape, 
  const int nr_bins_color, 
  Eigen::VectorXf &shot)
{
  if (rf.size () != 3)
  {
    PCL_ERROR ("[pcl::%s::interpolateDoubleChannel] RF size different than 9! Aborting...\n");
    return;
  }

  int shapeToColorStride = nr_grid_sector_*(nr_bins_shape+1);

  for (size_t i_idx = 0; i_idx < indices.size (); ++i_idx)
  {
    Eigen::Vector4f delta = surface_->points[indices[i_idx]].getVector4fMap () - central_point;
    delta[3] = 0;

    // Compute the Euclidean norm
    double distance = sqrt (sqr_dists[i_idx]);

    if (areEquals (distance, 0.0))
      continue;

    double xInFeatRef = delta.dot (rf[0]); //(x * feat[i].rf[0] + y * feat[i].rf[1] + z * feat[i].rf[2]);
    double yInFeatRef = delta.dot (rf[1]); //(x * feat[i].rf[3] + y * feat[i].rf[4] + z * feat[i].rf[5]);
    double zInFeatRef = delta.dot (rf[2]); //(x * feat[i].rf[6] + y * feat[i].rf[7] + z * feat[i].rf[8]);

    // To avoid numerical problems afterwards
    if (fabs (yInFeatRef) < 1E-30)
      yInFeatRef  = 0;
    if (fabs (xInFeatRef) < 1E-30)
      xInFeatRef  = 0;
    if (fabs (zInFeatRef) < 1E-30)
      zInFeatRef  = 0;

    unsigned char bit4 = ((yInFeatRef > 0) || ((yInFeatRef == 0.0) && (xInFeatRef < 0))) ? 1 : 0;
    unsigned char bit3 = ((xInFeatRef > 0) || ((xInFeatRef == 0.0) && (yInFeatRef > 0))) ? !bit4 : bit4;

    assert (bit3 == 0 || bit3 == 1);

    int desc_index = (bit4<<3) + (bit3<<2);

    desc_index = desc_index << 1;

    if ((xInFeatRef * yInFeatRef > 0) || (xInFeatRef == 0.0))
      desc_index += (fabs (xInFeatRef) >= fabs (yInFeatRef)) ? 0 : 4;
    else
      desc_index += (fabs (xInFeatRef) > fabs (yInFeatRef)) ? 4 : 0;

    desc_index += zInFeatRef > 0 ? 1 : 0;

    // 2 RADII
    desc_index += (distance > radius1_2_) ? 2 : 0;

    int step_index_shape = static_cast<int>(floor (binDistanceShape[i_idx] +0.5));
    int step_index_color = static_cast<int>(floor (binDistanceColor[i_idx] +0.5));

    int volume_index_shape = desc_index * (nr_bins_shape+1);
    int volume_index_color = shapeToColorStride + desc_index * (nr_bins_color+1);

    //Interpolation on the cosine (adjacent bins in the histrogram)
    binDistanceShape[i_idx] -= step_index_shape;
    binDistanceColor[i_idx] -= step_index_color;

    double intWeightShape = (1- fabs (binDistanceShape[i_idx]));
    double intWeightColor = (1- fabs (binDistanceColor[i_idx]));

    if (binDistanceShape[i_idx] > 0)
      shot[volume_index_shape + ((step_index_shape + 1) % nr_bins_shape)] += binDistanceShape[i_idx];
    else
      shot[volume_index_shape + ((step_index_shape - 1 + nr_bins_shape) % nr_bins_shape)] -= binDistanceShape[i_idx];

    if (binDistanceColor[i_idx] > 0)
      shot[volume_index_color + ((step_index_color+1) % nr_bins_color)] += binDistanceColor[i_idx];
    else
      shot[volume_index_color + ((step_index_color - 1 + nr_bins_color) % nr_bins_color)] -= binDistanceColor[i_idx];

    //Interpolation on the distance (adjacent husks)
   
    if (distance > radius1_2_)   //external sphere
    {
      double radiusDistance = (distance - radius3_4_) / radius1_2_;

      if (distance > radius3_4_) //most external sector, votes only for itself
      {
        intWeightShape += 1 - radiusDistance; //weight=1-d
        intWeightColor += 1 - radiusDistance; //weight=1-d
      }
      else  //3/4 of radius, votes also for the internal sphere
      {
        intWeightShape += 1 + radiusDistance;
        intWeightColor += 1 + radiusDistance;
        shot[(desc_index - 2) * (nr_bins_shape+1) + step_index_shape] -= radiusDistance;
        shot[shapeToColorStride + (desc_index - 2) * (nr_bins_color+1) + step_index_color] -= radiusDistance;
      }
    }
    else    //internal sphere
    {
      double radiusDistance = (distance - radius1_4_) / radius1_2_;

      if (distance < radius1_4_) //most internal sector, votes only for itself
      {
        intWeightShape += 1 + radiusDistance;
        intWeightColor += 1 + radiusDistance; //weight=1-d
      }
      else  //3/4 of radius, votes also for the external sphere
      {
        intWeightShape += 1 - radiusDistance; //weight=1-d
        intWeightColor += 1 - radiusDistance; //weight=1-d
        shot[(desc_index + 2) * (nr_bins_shape+1) + step_index_shape] += radiusDistance;
        shot[shapeToColorStride + (desc_index + 2) * (nr_bins_color+1) + step_index_color] += radiusDistance;
      }
    }

    //Interpolation on the inclination (adjacent vertical volumes)
    double inclinationCos = zInFeatRef / distance;
    if (inclinationCos < - 1.0)
      inclinationCos = - 1.0;
    if (inclinationCos > 1.0)
      inclinationCos = 1.0;

    double inclination = acos (inclinationCos);

    assert (inclination >= 0.0 && inclination <= PST_RAD_180);

    if (inclination > PST_RAD_90 || (fabs (inclination - PST_RAD_90) < 1e-30 && zInFeatRef <= 0))
    {
      double inclinationDistance = (inclination - PST_RAD_135) / PST_RAD_90;
      if (inclination > PST_RAD_135)
      {
        intWeightShape += 1 - inclinationDistance;
        intWeightColor += 1 - inclinationDistance;
      }
      else
      {
        intWeightShape += 1 + inclinationDistance;
        intWeightColor += 1 + inclinationDistance;
        assert ((desc_index + 1) * (nr_bins_shape+1) + step_index_shape >= 0 && (desc_index + 1) * (nr_bins_shape+1) + step_index_shape < descLength_);
        assert (shapeToColorStride + (desc_index + 1) * (nr_bins_color+ 1) + step_index_color >= 0 && shapeToColorStride + (desc_index + 1) * (nr_bins_color+1) + step_index_color < descLength_);
        shot[(desc_index + 1) * (nr_bins_shape+1) + step_index_shape] -= inclinationDistance;
        shot[shapeToColorStride + (desc_index + 1) * (nr_bins_color+1) + step_index_color] -= inclinationDistance;
      }
    }
    else
    {
      double inclinationDistance = (inclination - PST_RAD_45) / PST_RAD_90;
      if (inclination < PST_RAD_45)
      {
        intWeightShape += 1 + inclinationDistance;
        intWeightColor += 1 + inclinationDistance;
      }
      else
      {
        intWeightShape += 1 - inclinationDistance;
        intWeightColor += 1 - inclinationDistance;
        assert ((desc_index - 1) * (nr_bins_shape+1) + step_index_shape >= 0 && (desc_index - 1) * (nr_bins_shape+1) + step_index_shape < descLength_);
        assert (shapeToColorStride + (desc_index - 1) * (nr_bins_color+ 1) + step_index_color >= 0 && shapeToColorStride + (desc_index - 1) * (nr_bins_color+1) + step_index_color < descLength_);
        shot[(desc_index - 1) * (nr_bins_shape+1) + step_index_shape] += inclinationDistance;
        shot[shapeToColorStride + (desc_index - 1) * (nr_bins_color+1) + step_index_color] += inclinationDistance;
      }
    }

    if (yInFeatRef != 0.0 || xInFeatRef != 0.0)
    {
      //Interpolation on the azimuth (adjacent horizontal volumes)
      double azimuth = atan2 (yInFeatRef, xInFeatRef);

      int sel = desc_index >> 2;
      double angularSectorSpan = PST_RAD_45;
      double angularSectorStart = - PST_RAD_PI_7_8;

      double azimuthDistance = (azimuth - (angularSectorStart + angularSectorSpan*sel)) / angularSectorSpan;
      assert ((azimuthDistance < 0.5 || areEquals (azimuthDistance, 0.5)) && (azimuthDistance > - 0.5 || areEquals (azimuthDistance, - 0.5)));
      azimuthDistance = (std::max)(- 0.5, std::min (azimuthDistance, 0.5));

      if (azimuthDistance > 0)
      {
        intWeightShape += 1 - azimuthDistance;
        intWeightColor += 1 - azimuthDistance;
        int interp_index = (desc_index + 4) % maxAngularSectors_;
        assert (interp_index * (nr_bins_shape+1) + step_index_shape >= 0 && interp_index * (nr_bins_shape+1) + step_index_shape < descLength_);
        assert (shapeToColorStride + interp_index * (nr_bins_color+1) + step_index_color >= 0 && shapeToColorStride + interp_index * (nr_bins_color+1) + step_index_color < descLength_);
        shot[interp_index * (nr_bins_shape+1) + step_index_shape] += azimuthDistance;
        shot[shapeToColorStride + interp_index * (nr_bins_color+1) + step_index_color] += azimuthDistance;
      }
      else
      {
        int interp_index = (desc_index - 4 + maxAngularSectors_) % maxAngularSectors_;
        intWeightShape += 1 + azimuthDistance;
        intWeightColor += 1 + azimuthDistance;
        assert (interp_index * (nr_bins_shape+1) + step_index_shape >= 0 && interp_index * (nr_bins_shape+1) + step_index_shape < descLength_);
        assert (shapeToColorStride + interp_index * (nr_bins_color+1) + step_index_color >= 0 && shapeToColorStride + interp_index * (nr_bins_color+1) + step_index_color < descLength_);
        shot[interp_index * (nr_bins_shape+1) + step_index_shape] -= azimuthDistance;
        shot[shapeToColorStride + interp_index * (nr_bins_color+1) + step_index_color] -= azimuthDistance;
      }
    }

    assert (volume_index_shape + step_index_shape >= 0 &&  volume_index_shape + step_index_shape < descLength_);
    assert (volume_index_color + step_index_color >= 0 &&  volume_index_color + step_index_color < descLength_);
    shot[volume_index_shape + step_index_shape] += intWeightShape;
    shot[volume_index_color + step_index_color] += intWeightColor;
  }
}


template <typename PointNT, typename PointOutT> void
pcl::SHOTEstimation<pcl::PointXYZRGBA, PointNT, PointOutT>::computePointSHOT (
  const int index, const std::vector<int> &indices, const std::vector<float> &sqr_dists, Eigen::VectorXf &shot,
  std::vector<Eigen::Vector4f, Eigen::aligned_allocator<Eigen::Vector4f> > &rf)
{
  if (rf.size () != 3)
    rf.resize (3);

  // Clear the resultant shot
  shot.setZero ();
  std::vector<double> binDistanceShape;
  std::vector<double> binDistanceColor;
  int nNeighbors = indices.size ();
  //Skip the current feature if the number of its neighbors is not sufficient for its description
  if (nNeighbors < 5)
  {
    PCL_WARN ("[pcl::%s::computePointSHOT] Warning! Neighborhood has less than 5 vertexes. Aborting description of point with index %d\n", getClassName ().c_str (), index);
    return;
  }

  //Compute the local Reference Frame for the current 3D point
  Eigen::Vector4f central_point = input_->points[index].getVector4fMap ();
  central_point[3] = 0;
  if (pcl::getLocalRF (*surface_, search_radius_, central_point, indices, sqr_dists, rf))
    return;

  //If shape description is enabled, compute the bins activated by each neighbor of the current feature in the shape histogram
  if (b_describe_shape_)
  {
    binDistanceShape.resize (nNeighbors);

    for (size_t i_idx = 0; i_idx < indices.size (); ++i_idx)
    {
      double cosineDesc = normals_->points[indices[i_idx]].getNormalVector4fMap ().dot (rf[2]); //feat[i].rf[6]*normal[0] + feat[i].rf[7]*normal[1] + feat[i].rf[8]*normal[2];

      if (cosineDesc > 1.0)
        cosineDesc = 1.0;
      if (cosineDesc < - 1.0)
        cosineDesc = - 1.0;

      binDistanceShape[i_idx] = ((1.0 + cosineDesc) * nr_shape_bins_) / 2;
    }
  }

  //If color description is enabled, compute the bins activated by each neighbor of the current feature in the color histogram
  if (b_describe_color_)
  {
    binDistanceColor.resize (nNeighbors);

    unsigned char redRef = input_->points[index].rgba >> 16 & 0xFF;
    unsigned char greenRef = input_->points[index].rgba >> 8& 0xFF;
    unsigned char blueRef = input_->points[index].rgba & 0xFF;

    float LRef, aRef, bRef;

    RGB2CIELAB (redRef, greenRef, blueRef, LRef, aRef, bRef);
    LRef /= 100.0;
    aRef /= 120.0;
    bRef /= 120.0;    //normalized LAB components (0<L<1, -1<a<1, -1<b<1)

    for (size_t i_idx = 0; i_idx < indices.size (); ++i_idx)
    {
      unsigned char red = surface_->points[indices[i_idx]].rgba >> 16 & 0xFF;
      unsigned char green = surface_->points[indices[i_idx]].rgba >> 8 & 0xFF;
      unsigned char blue = surface_->points[indices[i_idx]].rgba & 0xFF;

      float L, a, b;

      RGB2CIELAB (red, green, blue, L, a, b);
      L /= 100.0;
      a /= 120.0;
      b /= 120.0;   //normalized LAB components (0<L<1, -1<a<1, -1<b<1)

      double colorDistance = (fabs (LRef - L) + ((fabs (aRef - a) + fabs (bRef - b)) / 2)) /3;

      if (colorDistance > 1.0)
        colorDistance = 1.0;
      if (colorDistance < 0.0)
        colorDistance = 0.0;

      binDistanceColor[i_idx] = colorDistance * nr_color_bins_;
    }
  }

  //Apply quadrilinear interpolation on the activated bins in the shape and/or color histogram(s)

  if (b_describe_shape_ && b_describe_color_)
    interpolateDoubleChannel (indices, sqr_dists, input_->points[index].getVector4fMap (), rf, binDistanceShape, binDistanceColor,
                              nr_shape_bins_, nr_color_bins_,
                              shot);
  else if (b_describe_color_)
    interpolateSingleChannel (indices, sqr_dists, input_->points[index].getVector4fMap (), rf, binDistanceColor, nr_color_bins_, shot);
  else
    interpolateSingleChannel (indices, sqr_dists, input_->points[index].getVector4fMap (), rf, binDistanceShape, nr_shape_bins_, shot);

  // Normalize the final histogram
  this->normalizeHistogram (shot, descLength_);
}


template <typename PointInT, typename PointNT, typename PointOutT> void
pcl::SHOTEstimation<PointInT, PointNT, PointOutT>::computePointSHOT (
  const int index, const std::vector<int> &indices, const std::vector<float> &sqr_dists, Eigen::VectorXf &shot,
  std::vector<Eigen::Vector4f, Eigen::aligned_allocator<Eigen::Vector4f> > &rf)
{
  //Skip the current feature if the number of its neighbors is not sufficient for its description
  if (indices.size () < 5)
  {
    PCL_WARN ("[pcl::%s::computePointSHOT] Warning! Neighborhood has less than 5 vertexes. Aborting description of point with index %d\n", getClassName ().c_str (), index);
    return;
  }

   // Clear the resultant shot
  std::vector<double> binDistanceShape;
  this->createBinDistanceShape (index, indices, sqr_dists, *input_, *normals_, *surface_, search_radius_, binDistanceShape, rf);

  // Interpolate
  shot.setZero ();
  interpolateSingleChannel (indices, sqr_dists, input_->points[index].getVector4fMap (), rf, binDistanceShape, nr_shape_bins_, shot);

  // Normalize the final histogram
  this->normalizeHistogram (shot, descLength_);
}


template <typename PointInT, typename PointNT> void
pcl::SHOTEstimation<PointInT, PointNT, Eigen::MatrixXf>::computePointSHOT (
  const int index, const std::vector<int> &indices, const std::vector<float> &sqr_dists, Eigen::VectorXf &shot,
  std::vector<Eigen::Vector4f, Eigen::aligned_allocator<Eigen::Vector4f> > &rf)
{
  //Skip the current feature if the number of its neighbors is not sufficient for its description
  if (indices.size () < 5)
  {
    PCL_WARN ("[pcl::%s::computePointSHOT] Warning! Neighborhood has less than 5 vertexes. Aborting description of point with index %d\n", getClassName ().c_str (), index);
    return;
  }

   // Clear the resultant shot
  std::vector<double> binDistanceShape;
  this->createBinDistanceShape (index, indices, sqr_dists, *input_, *normals_, *surface_, search_radius_, binDistanceShape, rf);

  // Interpolate
  shot.setZero ();
  interpolateSingleChannel (indices, sqr_dists, input_->points[index].getVector4fMap (), rf, binDistanceShape, nr_shape_bins_, shot);

  // Normalize the final histogram
  this->normalizeHistogram (shot, descLength_);
}


template <typename PointNT, typename PointOutT> void
pcl::SHOTEstimation<pcl::PointXYZRGBA, PointNT, PointOutT>::computeFeature (PointCloudOut &output)
{
  if (rf_.size () != 3)
    rf_.resize (3);

  // Compute the current length of the descriptor
  descLength_ = (b_describe_shape_) ? nr_grid_sector_*(nr_shape_bins_+1) : 0;
  descLength_ +=   (b_describe_color_) ? nr_grid_sector_*(nr_color_bins_+1) : 0;

  // Useful values
  sqradius_ = search_radius_*search_radius_;
  radius3_4_ = (search_radius_*3) / 4;
  radius1_4_ = search_radius_ / 4;
  radius1_2_ = search_radius_ / 2;

  shot_.setZero (descLength_);
  rf_[0].setZero ();
  rf_[1].setZero ();
  rf_[2].setZero ();

  if (output.points[0].descriptor.size () != (size_t)descLength_)
    for (size_t idx = 0; idx < indices_->size (); ++idx)
      output.points[idx].descriptor.resize (descLength_);
//  if (output.points[0].size != (size_t)descLength_)
//  {
//    PCL_ERROR ("[pcl::%s::computeFeature] The desired descriptor size (%lu) does not match the output point type feature (%lu)! Aborting...\n", getClassName ().c_str (), descLength_, (unsigned long) output.points[0].size);
//    return;
//  }

  // Allocate enough space to hold the results
  // \note This resize is irrelevant for a radiusSearch ().
  std::vector<int> nn_indices (k_);
  std::vector<float> nn_dists (k_);

  output.is_dense = true;
  // Iterating over the entire index vector
  for (size_t idx = 0; idx < indices_->size (); ++idx)
  {
    if (!isFinite ((*input_)[(*indices_)[idx]]) ||
        this->searchForNeighbors ((*indices_)[idx], search_parameter_, nn_indices, nn_dists) == 0)
    {
      // Copy into the resultant cloud
      for (int d = 0; d < shot_.size (); ++d)
        output.points[idx].descriptor[d] = std::numeric_limits<float>::quiet_NaN ();
      for (int d = 0; d < 9; ++d)
        output.points[idx].rf[d] = std::numeric_limits<float>::quiet_NaN ();

      output.is_dense = false;
      continue;
     }

    // Compute the SHOT descriptor for the current 3D feature
    computePointSHOT ((*indices_)[idx], nn_indices, nn_dists, shot_, rf_);

    // Copy into the resultant cloud
    for (int d = 0; d < shot_.size (); ++d)
      output.points[idx].descriptor[d] = shot_[d];
    for (int d = 0; d < 9; ++d)
      output.points[idx].rf[d] = rf_[d/3][d%3];
  }
}

template <typename PointNT> void
pcl::SHOTEstimation<pcl::PointXYZRGBA, PointNT, Eigen::MatrixXf>::computeFeatureEigen (pcl::PointCloud<Eigen::MatrixXf> &output)
{
  if (rf_.size () != 3)
    rf_.resize (3);

  // Compute the current length of the descriptor
  descLength_ = (b_describe_shape_) ? nr_grid_sector_*(nr_shape_bins_+1) : 0;
  descLength_ +=   (b_describe_color_) ? nr_grid_sector_*(nr_color_bins_+1) : 0;

  // Set up the output channels
  output.channels["shot"].name     = "shot";
  output.channels["shot"].offset   = 0;
  output.channels["shot"].size     = 4;
  output.channels["shot"].count    = descLength_ + 9;
  output.channels["shot"].datatype = sensor_msgs::PointField::FLOAT32;

  // Useful values
  sqradius_ = search_radius_*search_radius_;
  radius3_4_ = (search_radius_*3) / 4;
  radius1_4_ = search_radius_ / 4;
  radius1_2_ = search_radius_ / 2;

  shot_.setZero (descLength_);
  rf_[0].setZero ();
  rf_[1].setZero ();
  rf_[2].setZero ();

  output.points.resize (indices_->size (), descLength_ + 9);

  // Allocate enough space to hold the results
  // \note This resize is irrelevant for a radiusSearch ().
  std::vector<int> nn_indices (k_);
  std::vector<float> nn_dists (k_);

  output.is_dense = true;
  // Iterating over the entire index vector
  for (size_t idx = 0; idx < indices_->size (); ++idx)
  {
    if (!isFinite ((*input_)[(*indices_)[idx]]) ||
        this->searchForNeighbors ((*indices_)[idx], search_parameter_, nn_indices, nn_dists) == 0)
    {
      output.points.row (idx).setConstant (std::numeric_limits<float>::quiet_NaN ());

      output.is_dense = false;
      continue;
     }

    // Compute the SHOT descriptor for the current 3D feature
    this->computePointSHOT ((*indices_)[idx], nn_indices, nn_dists, shot_, rf_);

    // Copy into the resultant cloud
    for (int d = 0; d < shot_.size (); ++d)
      output.points (idx, d) = shot_[d];
    for (int d = 0; d < 9; ++d)
      output.points (idx, shot_.size () + d) = rf_[d/3][d%3];
  }
}


template <typename PointInT, typename PointNT, typename PointOutT> void
pcl::SHOTEstimationBase<PointInT, PointNT, PointOutT>::computeFeature (pcl::PointCloud<PointOutT> &output)
{
  if (rf_.size () != 3)
    rf_.resize (3);

  descLength_ = nr_grid_sector_ * (nr_shape_bins_+1);

  sqradius_ = search_radius_ * search_radius_;
  radius3_4_ = (search_radius_*3) / 4;
  radius1_4_ = search_radius_ / 4;
  radius1_2_ = search_radius_ / 2;

  shot_.setZero (descLength_);
  rf_[0].setZero ();
  rf_[1].setZero ();
  rf_[2].setZero ();

  if (output.points[0].descriptor.size () != (size_t)descLength_)
    for (size_t idx = 0; idx < indices_->size (); ++idx)
      output.points[idx].descriptor.resize (descLength_);
//  if (output.points[0].size != (size_t)descLength_)
//  {
//    PCL_ERROR ("[pcl::%s::computeFeature] The desired descriptor size (%lu) does not match the output point type feature (%lu)! Aborting...\n", getClassName ().c_str (), descLength_, (unsigned long) output.points[0].size);
//    return;
//  }

  // Allocate enough space to hold the results
  // \note This resize is irrelevant for a radiusSearch ().
  std::vector<int> nn_indices (k_);
  std::vector<float> nn_dists (k_);

  output.is_dense = true;
  // Iterating over the entire index vector
  for (size_t idx = 0; idx < indices_->size (); ++idx)
  {
    if (!isFinite ((*input_)[(*indices_)[idx]]) ||
        this->searchForNeighbors ((*indices_)[idx], search_parameter_, nn_indices, nn_dists) == 0)
    {
      // Copy into the resultant cloud
      for (int d = 0; d < shot_.size (); ++d)
        output.points[idx].descriptor[d] = std::numeric_limits<float>::quiet_NaN ();
      for (int d = 0; d < 9; ++d)
        output.points[idx].rf[d] = std::numeric_limits<float>::quiet_NaN ();

      output.is_dense = false;
      continue;
     }

    // Estimate the SHOT at each patch
    computePointSHOT ((*indices_)[idx], nn_indices, nn_dists, shot_, rf_);

    // Copy into the resultant cloud
    for (int d = 0; d < shot_.size (); ++d)
      output.points[idx].descriptor[d] = shot_[d];
    for (int d = 0; d < 9; ++d)
      output.points[idx].rf[d] = rf_[d/3][d%3];
  }
}

template <typename PointInT, typename PointNT> void
pcl::SHOTEstimationBase<PointInT, PointNT, Eigen::MatrixXf>::computeFeatureEigen (pcl::PointCloud<Eigen::MatrixXf> &output)
{
  if (rf_.size () != 3)
    rf_.resize (3);

  descLength_ = nr_grid_sector_ * (nr_shape_bins_+1);

  // Set up the output channels
  output.channels["shot"].name     = "shot";
  output.channels["shot"].offset   = 0;
  output.channels["shot"].size     = 4;
  output.channels["shot"].count    = descLength_ + 9;
  output.channels["shot"].datatype = sensor_msgs::PointField::FLOAT32;

  sqradius_ = search_radius_ * search_radius_;
  radius3_4_ = (search_radius_*3) / 4;
  radius1_4_ = search_radius_ / 4;
  radius1_2_ = search_radius_ / 2;

  shot_.setZero (descLength_);
  rf_[0].setZero ();
  rf_[1].setZero ();
  rf_[2].setZero ();

  output.points.resize (indices_->size (), descLength_ + 9);

  // Allocate enough space to hold the results
  // \note This resize is irrelevant for a radiusSearch ().
  std::vector<int> nn_indices (k_);
  std::vector<float> nn_dists (k_);

  output.is_dense = true;
  // Iterating over the entire index vector
  for (size_t idx = 0; idx < indices_->size (); ++idx)
  {
    if (!isFinite ((*input_)[(*indices_)[idx]]) ||
        this->searchForNeighbors ((*indices_)[idx], search_parameter_, nn_indices, nn_dists) == 0)
    {
      output.points.row (idx).setConstant (std::numeric_limits<float>::quiet_NaN ());

      output.is_dense = false;
      continue;
     }

    // Estimate the SHOT at each patch
    this->computePointSHOT ((*indices_)[idx], nn_indices, nn_dists, shot_, rf_);

    // Copy into the resultant cloud
    for (int d = 0; d < shot_.size (); ++d)
      output.points (idx, d) = shot_[d];
    for (int d = 0; d < 9; ++d)
      output.points (idx, shot_.size () + d) = rf_[d/3][d%3];
  }
}

#define PCL_INSTANTIATE_SHOTEstimationBase(T,NT,OutT) template class PCL_EXPORTS pcl::SHOTEstimationBase<T,NT,OutT>;
#define PCL_INSTANTIATE_SHOTEstimation(T,NT,OutT) template class PCL_EXPORTS pcl::SHOTEstimation<T,NT,OutT>;

#endif    // PCL_FEATURES_IMPL_SHOT_H_