From c146e488919e5bd9107c6b8baaf073247ad0ab91 Mon Sep 17 00:00:00 2001 From: Jeremie Papon Date: Mon, 2 Dec 2013 14:30:13 +0100 Subject: [PATCH] Changed the Hue distance calculation in tracking HSVColorCoherence to be correct --- .../pcl/tracking/impl/hsv_color_coherence.hpp | 14 +++++++++++--- 1 file changed, 11 insertions(+), 3 deletions(-) diff --git a/tracking/include/pcl/tracking/impl/hsv_color_coherence.hpp b/tracking/include/pcl/tracking/impl/hsv_color_coherence.hpp index e526e9616d7..49ef8979366 100644 --- a/tracking/include/pcl/tracking/impl/hsv_color_coherence.hpp +++ b/tracking/include/pcl/tracking/impl/hsv_color_coherence.hpp @@ -156,11 +156,19 @@ namespace pcl 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; - if (_h_diff > 0.5f) - h_diff = static_cast (h_weight_) * (_h_diff - 0.5f) * (_h_diff - 0.5f); - else + //Now we need to choose the smaller distance + if (_h_diff < _h_diff2) h_diff = static_cast (h_weight_) * _h_diff * _h_diff; + else + h_diff = static_cast (h_weight_) * _h_diff2 * _h_diff2; const float s_diff = static_cast (s_weight_) * (source_s - target_s) * (source_s - target_s); const float v_diff = static_cast (v_weight_) * (source_v - target_v) * (source_v - target_v);