Fix numerical problems in sub_pixel_peak()

Sometimes, finding sub-pixel maximum fails (solution is very far from
the maximal value pixel) and the tracked object is lost. These changes
fix some the these cases. This problem was present also in the
original implementation.

diff --git a/src/kcf.cpp b/src/kcf.cpp
index 64baea78d4f3c8105459c9ec1c03dbe082631791..4cccea58002aca42bf2765b3ed9c1d685eb6ac26 100644 (file)
--- a/src/kcf.cpp
+++ b/src/kcf.cpp
@@ -833,9 +833,12 @@ cv::Point2f KCF_Tracker::sub_pixel_peak(cv::Point &max_loc, cv::Mat &response) c
     float a = x.at<float>(0), b = x.at<float>(1), c = x.at<float>(2), d = x.at<float>(3), e = x.at<float>(4);
 
     cv::Point2f sub_peak(max_loc.x, max_loc.y);
-    if (b > 0 || b < 0) {
+    if (4 * a * c - b * b > p_floating_error) {
         sub_peak.y = ((2.f * a * e) / b - d) / (b - (4 * a * c) / b);
         sub_peak.x = (-2 * c * sub_peak.y - e) / b;
+        if (fabs(sub_peak.x - max_loc.x) > 1 ||
+            fabs(sub_peak.y - max_loc.y) > 1)
+            sub_peak = max_loc;
     }
 
     return sub_peak;