template <typename PointInT> double
CombinedCoherence<PointInT>::computeCoherence (PointInT &source, PointInT &target)
{
// convert color space from RGB to HSV
RGBValue source_rgb, target_rgb;
source_rgb.int_value = source.rgba;
target_rgb.int_value = target.rgba;
float source_h, source_s, source_v, target_h, target_s, target_v;
RGB2HSV (source_rgb.Red, source_rgb.Blue, source_rgb.Green,
source_h, source_s, source_v);
RGB2HSV (target_rgb.Red, target_rgb.Blue, target_rgb.Green,
target_h, target_s, target_v);
// hue value is in 0 ~ 2pi, but circulated.
const float _h_diff = fabsf (source_h - target_h);
// Also need to compute distance other way around circle - but need to check which is closer to 0
float _h_diff2;
if (source_h < target_h)
_h_diff2 = fabsf (1.0f + source_h - target_h); //Add 2pi to source, subtract target
else
_h_diff2 = fabsf (1.0f + target_h - source_h); //Add 2pi to target, subtract source
float h_diff;
//Now we need to choose the smaller distance
if (_h_diff < _h_diff2)
h_diff = static_cast<float> (h_weight_) * _h_diff * _h_diff;
else
h_diff = static_cast<float> (h_weight_) * _h_diff2 * _h_diff2;
const float s_diff = static_cast<float> (s_weight_) * (source_s - target_s) * (source_s - target_s);
const float v_diff = static_cast<float> (v_weight_) * (source_v - target_v) * (source_v - target_v);
//const float color_diff = h_diff + s_diff + v_diff;
//if (color_diff > 0.1)
// return 0;
Eigen::Vector4f p = source.getVector4fMap ();
Eigen::Vector4f p_dash = target.getVector4fMap ();
double d = (p - p_dash).norm () * dist_weight_;
const float diff2 = h_diff + s_diff + v_diff + d;
return (1.0 / (1.0 + weight_ * diff2));
}
template <typename PointInT, typename PointNT, typename PointOutT> bool
pcl::BoundaryEstimation<PointInT, PointNT, PointOutT>::isBoundaryPoint (
const pcl::PointCloud<PointInT> &cloud, const PointInT &q_point,
const std::vector<int> &indices,
const Eigen::Vector4f &u, const Eigen::Vector4f &v,
const float angle_threshold)
{
if (indices.size () < 3)
return (false);
if (!pcl_isfinite (q_point.x) || !pcl_isfinite (q_point.y) || !pcl_isfinite (q_point.z))
return (false);
// Compute the angles between each neighboring point and the query point itself
std::vector<float> angles (indices.size ());
float max_dif = FLT_MIN, dif;
int cp = 0;
for (size_t i = 0; i < indices.size (); ++i)
{
if (!pcl_isfinite (cloud.points[indices[i]].x) ||
!pcl_isfinite (cloud.points[indices[i]].y) ||
!pcl_isfinite (cloud.points[indices[i]].z))
continue;
Eigen::Vector4f delta = cloud.points[indices[i]].getVector4fMap () - q_point.getVector4fMap ();
if (delta == Eigen::Vector4f::Zero())
continue;
angles[cp++] = atan2f (v.dot (delta), u.dot (delta)); // the angles are fine between -PI and PI too
}
if (cp == 0)
return (false);
angles.resize (cp);
std::sort (angles.begin (), angles.end ());
// Compute the maximal angle difference between two consecutive angles
for (size_t i = 0; i < angles.size () - 1; ++i)
{
dif = angles[i + 1] - angles[i];
if (max_dif < dif)
max_dif = dif;
}
// Get the angle difference between the last and the first
dif = 2 * static_cast<float> (M_PI) - angles[angles.size () - 1] + angles[0];
if (max_dif < dif)
max_dif = dif;
// Check results
if (max_dif > angle_threshold)
return (true);
else
return (false);
}
