template <typename PointInT, typename PointNT, typename PointOutT, typename PointRFT> void
pcl::SHOTEstimationBase<PointInT, PointNT, PointOutT, PointRFT>::createBinDistanceShape (
int index,
const std::vector<int> &indices,
std::vector<double> &bin_distance_shape)
{
bin_distance_shape.resize (indices.size ());
const PointRFT& current_frame = frames_->points[index];
//if (!pcl_isfinite (current_frame.rf[0]) || !pcl_isfinite (current_frame.rf[4]) || !pcl_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);
for (size_t i_idx = 0; i_idx < indices.size (); ++i_idx)
{
//double cosineDesc = feat[i].rf[6]*normal[0] + feat[i].rf[7]*normal[1] + feat[i].rf[8]*normal[2];
double cosineDesc = normals_->points[indices[i_idx]].getNormalVector4fMap ().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;
}
}
template <typename PointInT, typename PointNT, typename PointOutT, typename PointRFT> void
pcl::SHOTEstimationBase<PointInT, PointNT, PointOutT, PointRFT>::createBinDistanceShape (
int index,
const std::vector<int> &indices,
std::vector<double> &bin_distance_shape)
{
bin_distance_shape.resize (indices.size ());
const PointRFT& current_frame = frames_->points[index];
//if (!pcl_isfinite (current_frame.rf[0]) || !pcl_isfinite (current_frame.rf[4]) || !pcl_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 (size_t i_idx = 0; i_idx < indices.size (); ++i_idx)
{
// check NaN normal
const Eigen::Vector4f& normal_vec = normals_->points[indices[i_idx]].getNormalVector4fMap ();
if (!pcl_isfinite (normal_vec[0]) ||
!pcl_isfinite (normal_vec[1]) ||
!pcl_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>::interpolateSingleChannel (
const std::vector<int> &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 (size_t i_idx = 0; i_idx < indices.size (); ++i_idx)
{
if (!pcl_isfinite(binDistance[i_idx]))
continue;
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 (current_frame_x);
double yInFeatRef = delta.dot (current_frame_y);
double zInFeatRef = delta.dot (current_frame_z);
// 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 = 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 += (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)] += 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 = 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] -= 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 = 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);
}
