Loading...
Searching...
No Matches
shot.hpp
/*
* Software License Agreement (BSD License)
*
* Point Cloud Library (PCL) - www.pointclouds.org
* Copyright (c) 2010-2011, Willow Garage, Inc.
* Copyright (c) 2012-, Open Perception, 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 the copyright holder(s) 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_lrf.h>
#include <pcl/common/colors.h> // for RGB2sRGB_LUT, XYZ2LAB_LUT
// Useful constants.
#define PST_PI 3.1415926535897932384626433832795
#define PST_RAD_45 0.78539816339744830961566084581988
#define PST_RAD_90 1.5707963267948966192313216916398
#define PST_RAD_135 2.3561944901923449288469825374596
#define PST_RAD_180 PST_PI
#define PST_RAD_360 6.283185307179586476925286766558
#define PST_RAD_PI_7_8 2.7488935718910690836548129603691
const double zeroDoubleEps15 = 1E-15;
const float zeroFloatEps8 = 1E-8f;
//////////////////////////////////////////////////////////////////////////////////////////////
/** \brief Check if val1 and val2 are equals.
*
* \param[in] val1 first number to check.
* \param[in] val2 second number to check.
* \param[in] zeroDoubleEps epsilon
* \return true if val1 is equal to val2, false otherwise.
*/
inline bool
areEquals (double val1, double val2, double zeroDoubleEps = zeroDoubleEps15)
{
return (std::abs (val1 - val2)<zeroDoubleEps);
}
//////////////////////////////////////////////////////////////////////////////////////////////
/** \brief Check if val1 and val2 are equals.
*
* \param[in] val1 first number to check.
* \param[in] val2 second number to check.
* \param[in] zeroFloatEps epsilon
* \return true if val1 is equal to val2, false otherwise.
*/
inline bool
areEquals (float val1, float val2, float zeroFloatEps = zeroFloatEps8)
{
return (std::fabs (val1 - val2)<zeroFloatEps);
}
//////////////////////////////////////////////////////////////////////////////////////////////
template <typename PointInT, typename PointNT, typename PointOutT, typename PointRFT>
std::array<float, 256>
pcl::SHOTColorEstimation<PointInT, PointNT, PointOutT, PointRFT>::sRGB_LUT = pcl::RGB2sRGB_LUT<float, 8>();
//////////////////////////////////////////////////////////////////////////////////////////////
template <typename PointInT, typename PointNT, typename PointOutT, typename PointRFT>
std::array<float, 4000>
pcl::SHOTColorEstimation<PointInT, PointNT, PointOutT, PointRFT>::sXYZ_LUT = pcl::XYZ2LAB_LUT<float, 4000>();
//////////////////////////////////////////////////////////////////////////////////////////////
template <typename PointInT, typename PointNT, typename PointOutT, typename PointRFT> void
pcl::SHOTColorEstimation<PointInT, PointNT, PointOutT, PointRFT>::RGB2CIELAB (unsigned char R, unsigned char G,
unsigned char B, float &L, float &A,
float &B2)
{
float fr = sRGB_LUT[R];
float fg = sRGB_LUT[G];
float fb = sRGB_LUT[B];
// Use white = D65
const float x = fr * 0.412453f + fg * 0.357580f + fb * 0.180423f;
const float y = fr * 0.212671f + fg * 0.715160f + fb * 0.072169f;
const float z = fr * 0.019334f + fg * 0.119193f + fb * 0.950227f;
float vx = x / 0.95047f;
float vy = y;
float vz = z / 1.08883f;
vx = sXYZ_LUT[static_cast<int>(vx*4000)];
vy = sXYZ_LUT[static_cast<int>(vy*4000)];
vz = sXYZ_LUT[static_cast<int>(vz*4000)];
L = 116.0f * vy - 16.0f;
if (L > 100)
L = 100.0f;
A = 500.0f * (vx - vy);
if (A > 120)
A = 120.0f;
else if (A <- 120)
A = -120.0f;
B2 = 200.0f * (vy - vz);
if (B2 > 120)
B2 = 120.0f;
else if (B2<- 120)
B2 = -120.0f;
}
//////////////////////////////////////////////////////////////////////////////////////////////
template <typename PointInT, typename PointNT, typename PointOutT, typename PointRFT> bool
pcl::SHOTEstimationBase<PointInT, PointNT, PointOutT, PointRFT>::initCompute ()
{
if (!FeatureFromNormals<PointInT, PointNT, PointOutT>::initCompute ())
{
PCL_ERROR ("[pcl::%s::initCompute] Init failed.\n", getClassName ().c_str ());
return (false);
}
// SHOT cannot work with k-search
if (this->getKSearch () != 0)
{
PCL_ERROR(
"[pcl::%s::initCompute] Error! Search method set to k-neighborhood. Call setKSearch(0) and setRadiusSearch( radius ) to use this class.\n",
getClassName().c_str ());
return (false);
}
// Default LRF estimation alg: SHOTLocalReferenceFrameEstimation
typename SHOTLocalReferenceFrameEstimation<PointInT, PointRFT>::Ptr lrf_estimator(new SHOTLocalReferenceFrameEstimation<PointInT, PointRFT>());
lrf_estimator->setRadiusSearch ((lrf_radius_ > 0 ? lrf_radius_ : search_radius_));
lrf_estimator->setInputCloud (input_);
lrf_estimator->setIndices (indices_);
if (!fake_surface_)
lrf_estimator->setSearchSurface(surface_);
if (!FeatureWithLocalReferenceFrames<PointInT, PointRFT>::initLocalReferenceFrames (indices_->size (), lrf_estimator))
{
PCL_ERROR ("[pcl::%s::initCompute] Init failed.\n", getClassName ().c_str ());
return (false);
}
return (true);
}
//////////////////////////////////////////////////////////////////////////////////////////////
template <typename PointInT, typename PointNT, typename PointOutT, typename PointRFT> void
pcl::SHOTEstimationBase<PointInT, PointNT, PointOutT, PointRFT>::createBinDistanceShape (
int index,
const pcl::Indices &indices,
std::vector<double> &bin_distance_shape)
{
bin_distance_shape.resize (indices.size ());
const PointRFT& current_frame = (*frames_)[index];
//if (!std::isfinite (current_frame.rf[0]) || !std::isfinite (current_frame.rf[4]) || !std::isfinite (current_frame.rf[11]))
//return;
Eigen::Vector4f current_frame_z (current_frame.z_axis[0], current_frame.z_axis[1], current_frame.z_axis[2], 0);
unsigned nan_counter = 0;
for (std::size_t i_idx = 0; i_idx < indices.size (); ++i_idx)
{
// check NaN normal
const Eigen::Vector4f& normal_vec = (*normals_)[indices[i_idx]].getNormalVector4fMap ();
if (!std::isfinite (normal_vec[0]) ||
!std::isfinite (normal_vec[1]) ||
!std::isfinite (normal_vec[2]))
{
bin_distance_shape[i_idx] = std::numeric_limits<double>::quiet_NaN ();
++nan_counter;
} else
{
//double cosineDesc = feat[i].rf[6]*normal[0] + feat[i].rf[7]*normal[1] + feat[i].rf[8]*normal[2];
double cosineDesc = normal_vec.dot (current_frame_z);
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;
}
}
if (nan_counter > 0)
PCL_WARN ("[pcl::%s::createBinDistanceShape] Point %d has %d (%f%%) NaN normals in its neighbourhood\n",
getClassName ().c_str (), index, nan_counter, (static_cast<float>(nan_counter)*100.f/static_cast<float>(indices.size ())));
}
//////////////////////////////////////////////////////////////////////////////////////////////
template <typename PointInT, typename PointNT, typename PointOutT, typename PointRFT> void
pcl::SHOTEstimationBase<PointInT, PointNT, PointOutT, PointRFT>::normalizeHistogram (
Eigen::VectorXf &shot, int desc_length)
{
// Normalization is performed by considering the L2 norm
// and not the sum of bins, as reported in the ECCV paper.
// This is due to additional experiments performed by the authors after its pubblication,
// where L2 normalization turned out better at handling point density variations.
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] /= static_cast<float> (acc_norm);
}
//////////////////////////////////////////////////////////////////////////////////////////////
template <typename PointInT, typename PointNT, typename PointOutT, typename PointRFT> void
pcl::SHOTEstimationBase<PointInT, PointNT, PointOutT, PointRFT>::interpolateSingleChannel (
const pcl::Indices &indices,
const std::vector<float> &sqr_dists,
const int index,
std::vector<double> &binDistance,
const int nr_bins,
Eigen::VectorXf &shot)
{
const Eigen::Vector4f& central_point = (*input_)[(*indices_)[index]].getVector4fMap ();
const PointRFT& current_frame = (*frames_)[index];
Eigen::Vector4f current_frame_x (current_frame.x_axis[0], current_frame.x_axis[1], current_frame.x_axis[2], 0);
Eigen::Vector4f current_frame_y (current_frame.y_axis[0], current_frame.y_axis[1], current_frame.y_axis[2], 0);
Eigen::Vector4f current_frame_z (current_frame.z_axis[0], current_frame.z_axis[1], current_frame.z_axis[2], 0);
for (std::size_t i_idx = 0; i_idx < indices.size (); ++i_idx)
{
if (!std::isfinite(binDistance[i_idx]))
continue;
Eigen::Vector4f delta = (*surface_)[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 (current_frame_x);
double yInFeatRef = delta.dot (current_frame_y);
double zInFeatRef = delta.dot (current_frame_z);
// To avoid numerical problems afterwards
if (std::abs (yInFeatRef) < 1E-30)
yInFeatRef = 0;
if (std::abs (xInFeatRef) < 1E-30)
xInFeatRef = 0;
if (std::abs (zInFeatRef) < 1E-30)
zInFeatRef = 0;
unsigned char bit4 = ((yInFeatRef > 0) || ((yInFeatRef == 0.0) && (xInFeatRef < 0))) ? 1 : 0;
auto bit3 = static_cast<unsigned char> (((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 += (std::abs (xInFeatRef) >= std::abs (yInFeatRef)) ? 0 : 4;
else
desc_index += (std::abs (xInFeatRef) > std::abs (yInFeatRef)) ? 4 : 0;
desc_index += zInFeatRef > 0 ? 1 : 0;
// 2 RADII
desc_index += (distance > radius1_2_) ? 2 : 0;
int step_index = static_cast<int>(std::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- std::abs (binDistance[i_idx]));
if (binDistance[i_idx] > 0)
shot[volume_index + ((step_index+1) % nr_bins)] += static_cast<float> (binDistance[i_idx]);
else
shot[volume_index + ((step_index - 1 + nr_bins) % nr_bins)] += - static_cast<float> (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] -= static_cast<float> (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] += static_cast<float> (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 = std::acos (inclinationCos);
assert (inclination >= 0.0 && inclination <= PST_RAD_180);
if (inclination > PST_RAD_90 || (std::abs (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] -= static_cast<float> (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] += static_cast<float> (inclinationDistance);
}
}
if (yInFeatRef != 0.0 || xInFeatRef != 0.0)
{
//Interpolation on the azimuth (adjacent horizontal volumes)
double azimuth = std::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] += static_cast<float> (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] -= static_cast<float> (azimuthDistance);
}
}
assert (volume_index + step_index >= 0 && volume_index + step_index < descLength_);
shot[volume_index + step_index] += static_cast<float> (intWeight);
}
}
//////////////////////////////////////////////////////////////////////////////////////////////
template <typename PointInT, typename PointNT, typename PointOutT, typename PointRFT> void
pcl::SHOTColorEstimation<PointInT, PointNT, PointOutT, PointRFT>::interpolateDoubleChannel (
const pcl::Indices &indices,
const std::vector<float> &sqr_dists,
const int index,
std::vector<double> &binDistanceShape,
std::vector<double> &binDistanceColor,
const int nr_bins_shape,
const int nr_bins_color,
Eigen::VectorXf &shot)
{
const Eigen::Vector4f &central_point = (*input_)[(*indices_)[index]].getVector4fMap ();
const PointRFT& current_frame = (*frames_)[index];
int shapeToColorStride = nr_grid_sector_*(nr_bins_shape+1);
Eigen::Vector4f current_frame_x (current_frame.x_axis[0], current_frame.x_axis[1], current_frame.x_axis[2], 0);
Eigen::Vector4f current_frame_y (current_frame.y_axis[0], current_frame.y_axis[1], current_frame.y_axis[2], 0);
Eigen::Vector4f current_frame_z (current_frame.z_axis[0], current_frame.z_axis[1], current_frame.z_axis[2], 0);
for (std::size_t i_idx = 0; i_idx < indices.size (); ++i_idx)
{
if (!std::isfinite(binDistanceShape[i_idx]))
continue;
Eigen::Vector4f delta = (*surface_)[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 (current_frame_x);
double yInFeatRef = delta.dot (current_frame_y);
double zInFeatRef = delta.dot (current_frame_z);
// To avoid numerical problems afterwards
if (std::abs (yInFeatRef) < 1E-30)
yInFeatRef = 0;
if (std::abs (xInFeatRef) < 1E-30)
xInFeatRef = 0;
if (std::abs (zInFeatRef) < 1E-30)
zInFeatRef = 0;
unsigned char bit4 = ((yInFeatRef > 0) || ((yInFeatRef == 0.0) && (xInFeatRef < 0))) ? 1 : 0;
auto bit3 = static_cast<unsigned char> (((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 += (std::abs (xInFeatRef) >= std::abs (yInFeatRef)) ? 0 : 4;
else
desc_index += (std::abs (xInFeatRef) > std::abs (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>(std::floor (binDistanceShape[i_idx] +0.5));
int step_index_color = static_cast<int>(std::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- std::abs (binDistanceShape[i_idx]));
double intWeightColor = (1- std::abs (binDistanceColor[i_idx]));
if (binDistanceShape[i_idx] > 0)
shot[volume_index_shape + ((step_index_shape + 1) % nr_bins_shape)] += static_cast<float> (binDistanceShape[i_idx]);
else
shot[volume_index_shape + ((step_index_shape - 1 + nr_bins_shape) % nr_bins_shape)] -= static_cast<float> (binDistanceShape[i_idx]);
if (binDistanceColor[i_idx] > 0)
shot[volume_index_color + ((step_index_color+1) % nr_bins_color)] += static_cast<float> (binDistanceColor[i_idx]);
else
shot[volume_index_color + ((step_index_color - 1 + nr_bins_color) % nr_bins_color)] -= static_cast<float> (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] -= static_cast<float> (radiusDistance);
shot[shapeToColorStride + (desc_index - 2) * (nr_bins_color+1) + step_index_color] -= static_cast<float> (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] += static_cast<float> (radiusDistance);
shot[shapeToColorStride + (desc_index + 2) * (nr_bins_color+1) + step_index_color] += static_cast<float> (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 = std::acos (inclinationCos);
assert (inclination >= 0.0 && inclination <= PST_RAD_180);
if (inclination > PST_RAD_90 || (std::abs (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] -= static_cast<float> (inclinationDistance);
shot[shapeToColorStride + (desc_index + 1) * (nr_bins_color+1) + step_index_color] -= static_cast<float> (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] += static_cast<float> (inclinationDistance);
shot[shapeToColorStride + (desc_index - 1) * (nr_bins_color+1) + step_index_color] += static_cast<float> (inclinationDistance);
}
}
if (yInFeatRef != 0.0 || xInFeatRef != 0.0)
{
//Interpolation on the azimuth (adjacent horizontal volumes)
double azimuth = std::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] += static_cast<float> (azimuthDistance);
shot[shapeToColorStride + interp_index * (nr_bins_color+1) + step_index_color] += static_cast<float> (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] -= static_cast<float> (azimuthDistance);
shot[shapeToColorStride + interp_index * (nr_bins_color+1) + step_index_color] -= static_cast<float> (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] += static_cast<float> (intWeightShape);
shot[volume_index_color + step_index_color] += static_cast<float> (intWeightColor);
}
}
//////////////////////////////////////////////////////////////////////////////////////////////
template <typename PointInT, typename PointNT, typename PointOutT, typename PointRFT> void
pcl::SHOTColorEstimation<PointInT, PointNT, PointOutT, PointRFT>::computePointSHOT (
const int index, const pcl::Indices &indices, const std::vector<float> &sqr_dists, Eigen::VectorXf &shot)
{
// Clear the resultant shot
shot.setZero ();
const auto 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 (), (*indices_)[index]);
shot.setConstant(descLength_, 1, std::numeric_limits<float>::quiet_NaN () );
return;
}
//If shape description is enabled, compute the bins activated by each neighbor of the current feature in the shape histogram
std::vector<double> binDistanceShape;
if (b_describe_shape_)
{
this->createBinDistanceShape (index, indices, binDistanceShape);
}
//If color description is enabled, compute the bins activated by each neighbor of the current feature in the color histogram
std::vector<double> binDistanceColor;
if (b_describe_color_)
{
binDistanceColor.reserve (nNeighbors);
//unsigned char redRef = (*input_)[(*indices_)[index]].rgba >> 16 & 0xFF;
//unsigned char greenRef = (*input_)[(*indices_)[index]].rgba >> 8& 0xFF;
//unsigned char blueRef = (*input_)[(*indices_)[index]].rgba & 0xFF;
unsigned char redRef = (*input_)[(*indices_)[index]].r;
unsigned char greenRef = (*input_)[(*indices_)[index]].g;
unsigned char blueRef = (*input_)[(*indices_)[index]].b;
float LRef, aRef, bRef;
RGB2CIELAB (redRef, greenRef, blueRef, LRef, aRef, bRef);
LRef /= 100.0f;
aRef /= 120.0f;
bRef /= 120.0f; //normalized LAB components (0<L<1, -1<a<1, -1<b<1)
for (const auto& idx: indices)
{
unsigned char red = (*surface_)[idx].r;
unsigned char green = (*surface_)[idx].g;
unsigned char blue = (*surface_)[idx].b;
float L, a, b;
RGB2CIELAB (red, green, blue, L, a, b);
L /= 100.0f;
a /= 120.0f;
b /= 120.0f; //normalized LAB components (0<L<1, -1<a<1, -1<b<1)
double colorDistance = (std::fabs (LRef - L) + ((std::fabs (aRef - a) + std::fabs (bRef - b)) / 2)) /3;
if (colorDistance > 1.0)
colorDistance = 1.0;
if (colorDistance < 0.0)
colorDistance = 0.0;
binDistanceColor.push_back (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, index, binDistanceShape, binDistanceColor,
nr_shape_bins_, nr_color_bins_,
shot);
else if (b_describe_color_)
interpolateSingleChannel (indices, sqr_dists, index, binDistanceColor, nr_color_bins_, shot);
else
interpolateSingleChannel (indices, sqr_dists, index, binDistanceShape, nr_shape_bins_, shot);
// Normalize the final histogram
this->normalizeHistogram (shot, descLength_);
}
//////////////////////////////////////////////////////////////////////////////////////////////
template <typename PointInT, typename PointNT, typename PointOutT, typename PointRFT> void
pcl::SHOTEstimation<PointInT, PointNT, PointOutT, PointRFT>::computePointSHOT (
const int index, const pcl::Indices &indices, const std::vector<float> &sqr_dists, Eigen::VectorXf &shot)
{
//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 (), (*indices_)[index]);
shot.setConstant(descLength_, 1, std::numeric_limits<float>::quiet_NaN () );
return;
}
// Clear the resultant shot
std::vector<double> binDistanceShape;
this->createBinDistanceShape (index, indices, binDistanceShape);
// Interpolate
shot.setZero ();
interpolateSingleChannel (indices, sqr_dists, index, binDistanceShape, nr_shape_bins_, shot);
// Normalize the final histogram
this->normalizeHistogram (shot, descLength_);
}
//////////////////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////////////////
template <typename PointInT, typename PointNT, typename PointOutT, typename PointRFT> void
pcl::SHOTEstimation<PointInT, PointNT, PointOutT, PointRFT>::computeFeature (pcl::PointCloud<PointOutT> &output)
{
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;
assert(descLength_ == 352);
Eigen::VectorXf shot;
shot.setZero (descLength_);
// Allocate enough space to hold the results
// \note This resize is irrelevant for a radiusSearch ().
pcl::Indices nn_indices (k_);
std::vector<float> nn_dists (k_);
output.is_dense = true;
// Iterating over the entire index vector
for (std::size_t idx = 0; idx < indices_->size (); ++idx)
{
bool lrf_is_nan = false;
const PointRFT& current_frame = (*frames_)[idx];
if (!std::isfinite (current_frame.x_axis[0]) ||
!std::isfinite (current_frame.y_axis[0]) ||
!std::isfinite (current_frame.z_axis[0]))
{
PCL_WARN ("[pcl::%s::computeFeature] The local reference frame is not valid! Aborting description of point with index %d\n",
getClassName ().c_str (), (*indices_)[idx]);
lrf_is_nan = true;
}
if (!isFinite ((*input_)[(*indices_)[idx]]) ||
lrf_is_nan ||
this->searchForNeighbors ((*indices_)[idx], search_parameter_, nn_indices, nn_dists) == 0)
{
// Copy into the resultant cloud
for (int d = 0; d < descLength_; ++d)
output[idx].descriptor[d] = std::numeric_limits<float>::quiet_NaN ();
for (int d = 0; d < 9; ++d)
output[idx].rf[d] = std::numeric_limits<float>::quiet_NaN ();
output.is_dense = false;
continue;
}
// Estimate the SHOT descriptor at each patch
computePointSHOT (static_cast<int> (idx), nn_indices, nn_dists, shot);
// Copy into the resultant cloud
for (int d = 0; d < descLength_; ++d)
output[idx].descriptor[d] = shot[d];
for (int d = 0; d < 3; ++d)
{
output[idx].rf[d + 0] = (*frames_)[idx].x_axis[d];
output[idx].rf[d + 3] = (*frames_)[idx].y_axis[d];
output[idx].rf[d + 6] = (*frames_)[idx].z_axis[d];
}
}
}
//////////////////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////////////////
template <typename PointInT, typename PointNT, typename PointOutT, typename PointRFT> void
pcl::SHOTColorEstimation<PointInT, PointNT, PointOutT, PointRFT>::computeFeature (pcl::PointCloud<PointOutT> &output)
{
// 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;
assert( (!b_describe_color_ && b_describe_shape_ && descLength_ == 352) ||
(b_describe_color_ && !b_describe_shape_ && descLength_ == 992) ||
(b_describe_color_ && b_describe_shape_ && descLength_ == 1344)
);
// Useful values
sqradius_ = search_radius_*search_radius_;
radius3_4_ = (search_radius_*3) / 4;
radius1_4_ = search_radius_ / 4;
radius1_2_ = search_radius_ / 2;
Eigen::VectorXf shot;
shot.setZero (descLength_);
// Allocate enough space to hold the results
// \note This resize is irrelevant for a radiusSearch ().
pcl::Indices nn_indices (k_);
std::vector<float> nn_dists (k_);
output.is_dense = true;
// Iterating over the entire index vector
for (std::size_t idx = 0; idx < indices_->size (); ++idx)
{
bool lrf_is_nan = false;
const PointRFT& current_frame = (*frames_)[idx];
if (!std::isfinite (current_frame.x_axis[0]) ||
!std::isfinite (current_frame.y_axis[0]) ||
!std::isfinite (current_frame.z_axis[0]))
{
PCL_WARN ("[pcl::%s::computeFeature] The local reference frame is not valid! Aborting description of point with index %d\n",
getClassName ().c_str (), (*indices_)[idx]);
lrf_is_nan = true;
}
if (!isFinite ((*input_)[(*indices_)[idx]]) ||
lrf_is_nan ||
this->searchForNeighbors ((*indices_)[idx], search_parameter_, nn_indices, nn_dists) == 0)
{
// Copy into the resultant cloud
for (int d = 0; d < descLength_; ++d)
output[idx].descriptor[d] = std::numeric_limits<float>::quiet_NaN ();
for (int d = 0; d < 9; ++d)
output[idx].rf[d] = std::numeric_limits<float>::quiet_NaN ();
output.is_dense = false;
continue;
}
// Compute the SHOT descriptor for the current 3D feature
computePointSHOT (static_cast<int> (idx), nn_indices, nn_dists, shot);
// Copy into the resultant cloud
for (int d = 0; d < descLength_; ++d)
output[idx].descriptor[d] = shot[d];
for (int d = 0; d < 3; ++d)
{
output[idx].rf[d + 0] = (*frames_)[idx].x_axis[d];
output[idx].rf[d + 3] = (*frames_)[idx].y_axis[d];
output[idx].rf[d + 6] = (*frames_)[idx].z_axis[d];
}
}
}
#define PCL_INSTANTIATE_SHOTEstimationBase(T,NT,OutT,RFT) template class PCL_EXPORTS pcl::SHOTEstimationBase<T,NT,OutT,RFT>;
#define PCL_INSTANTIATE_SHOTEstimation(T,NT,OutT,RFT) template class PCL_EXPORTS pcl::SHOTEstimation<T,NT,OutT,RFT>;
#define PCL_INSTANTIATE_SHOTColorEstimation(T,NT,OutT,RFT) template class PCL_EXPORTS pcl::SHOTColorEstimation<T,NT,OutT,RFT>;
#endif // PCL_FEATURES_IMPL_SHOT_H_
pcl::FeatureFromNormals
Definition feature.h:309
pcl::FeatureWithLocalReferenceFrames
FeatureWithLocalReferenceFrames provides a public interface for descriptor extractor classes which ne...
Definition feature.h:440
pcl::PointCloud
PointCloud represents the base class in PCL for storing collections of 3D points.
Definition point_cloud.h:173
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:403
pcl::SHOTColorEstimation
SHOTColorEstimation estimates the Signature of Histograms of OrienTations (SHOT) descriptor for a giv...
Definition shot.h:290
pcl::SHOTColorEstimation::interpolateDoubleChannel
void interpolateDoubleChannel(const pcl::Indices &indices, const std::vector< float > &sqr_dists, const int index, std::vector< double > &binDistanceShape, std::vector< double > &binDistanceColor, const int nr_bins_shape, const int nr_bins_color, Eigen::VectorXf &shot)
Quadrilinear interpolation; used when color and shape descriptions are both activated.
Definition shot.hpp:412
pcl::SHOTColorEstimation::computeFeature
void computeFeature(pcl::PointCloud< PointOutT > &output) override
Estimate the Signatures of Histograms of OrienTations (SHOT) descriptors at a set of points given by ...
Definition shot.hpp:810
pcl::SHOTColorEstimation::RGB2CIELAB
static void RGB2CIELAB(unsigned char R, unsigned char G, unsigned char B, float &L, float &A, float &B2)
Converts RGB triplets to CIELab space.
Definition shot.hpp:100
pcl::SHOTColorEstimation::computePointSHOT
void computePointSHOT(const int index, const pcl::Indices &indices, const std::vector< float > &sqr_dists, Eigen::VectorXf &shot) override
Estimate the SHOT descriptor for a given point based on its spatial neighborhood of 3D points with no...
Definition shot.hpp:628
pcl::SHOTEstimationBase::initCompute
bool initCompute() override
This method should get called before starting the actual computation.
Definition shot.hpp:140
pcl::SHOTEstimationBase::normalizeHistogram
void normalizeHistogram(Eigen::VectorXf &shot, int desc_length)
Normalize the SHOT histogram.
Definition shot.hpp:220
pcl::SHOTEstimationBase::interpolateSingleChannel
void interpolateSingleChannel(const pcl::Indices &indices, const std::vector< float > &sqr_dists, const int index, std::vector< double > &binDistance, const int nr_bins, Eigen::VectorXf &shot)
Quadrilinear interpolation used when color and shape descriptions are NOT activated simultaneously.
Definition shot.hpp:237
pcl::SHOTEstimationBase::createBinDistanceShape
void createBinDistanceShape(int index, const pcl::Indices &indices, std::vector< double > &bin_distance_shape)
Create a binned distance shape histogram.
Definition shot.hpp:176
pcl::SHOTEstimation::computeFeature
void computeFeature(pcl::PointCloud< PointOutT > &output) override
Estimate the Signatures of Histograms of OrienTations (SHOT) descriptors at a set of points given by ...
Definition shot.hpp:743
pcl::SHOTEstimation::computePointSHOT
void computePointSHOT(const int index, const pcl::Indices &indices, const std::vector< float > &sqr_dists, Eigen::VectorXf &shot) override
Estimate the SHOT descriptor for a given point based on its spatial neighborhood of 3D points with no...
Definition shot.hpp:713
pcl::SHOTLocalReferenceFrameEstimation
SHOTLocalReferenceFrameEstimation estimates the Local Reference Frame used in the calculation of the ...
Definition shot_lrf.h:66
pcl::SHOTLocalReferenceFrameEstimation::Ptr
shared_ptr< SHOTLocalReferenceFrameEstimation< PointInT, PointOutT > > Ptr
Definition shot_lrf.h:68
pcl::B
@ B
Definition norms.h:54
pcl::geometry::distance
float distance(const PointT &p1, const PointT &p2)
Definition geometry.h:60
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:55
pcl::Indices
IndicesAllocator<> Indices
Type used for indices in PCL.
Definition types.h:133