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); }