From 28e006ad4f9fd13e96b44e3552178a18befe3a06 Mon Sep 17 00:00:00 2001
From: Alexey Spizhevoy <no@email>
Date: Mon, 24 Oct 2011 12:35:11 +0000
Subject: [PATCH] Fixed median focal length estimation in the stitching module

---
 modules/stitching/src/autocalib.cpp | 11 +++++++++--
 modules/stitching/src/stitcher.cpp  |  9 +++++++--
 samples/cpp/stitching_detailed.cpp  | 10 ++++++++--
 3 files changed, 24 insertions(+), 6 deletions(-)

diff --git a/modules/stitching/src/autocalib.cpp b/modules/stitching/src/autocalib.cpp
index b82c286c3..4532f77ad 100644
--- a/modules/stitching/src/autocalib.cpp
+++ b/modules/stitching/src/autocalib.cpp
@@ -120,9 +120,16 @@ void estimateFocal(const vector<ImageFeatures> &features, const vector<MatchesIn
 
     if (static_cast<int>(all_focals.size()) >= num_images - 1)
     {
-        nth_element(all_focals.begin(), all_focals.begin() + all_focals.size()/2, all_focals.end());
+        double median;
+
+        sort(all_focals.begin(), all_focals.end());
+        if (all_focals.size() % 2 == 1)
+            median = all_focals[all_focals.size() / 2];
+        else
+            median = (all_focals[all_focals.size() / 2 - 1] + all_focals[all_focals.size() / 2]) * 0.5;
+
         for (int i = 0; i < num_images; ++i)
-            focals[i] = all_focals[all_focals.size()/2];
+            focals[i] = median;
     }
     else
     {
diff --git a/modules/stitching/src/stitcher.cpp b/modules/stitching/src/stitcher.cpp
index d7b7ef79b..14c1ce4b5 100644
--- a/modules/stitching/src/stitcher.cpp
+++ b/modules/stitching/src/stitcher.cpp
@@ -442,8 +442,13 @@ void Stitcher::estimateCameraParams()
         LOGLN("Camera #" << indices_[i] + 1 << ":\n" << cameras_[i].K());
         focals.push_back(cameras_[i].focal);
     }
-    nth_element(focals.begin(), focals.begin() + focals.size()/2, focals.end());
-    warped_image_scale_ = static_cast<float>(focals[focals.size() / 2]);
+
+    sort(focals.begin(), focals.end());
+    float warped_image_scale;
+    if (focals.size() % 2 == 1)
+        warped_image_scale = static_cast<float>(focals[focals.size() / 2]);
+    else
+        warped_image_scale = static_cast<float>(focals[focals.size() / 2 - 1] + focals[focals.size() / 2]) * 0.5f;
 
     if (do_wave_correct_)
     {
diff --git a/samples/cpp/stitching_detailed.cpp b/samples/cpp/stitching_detailed.cpp
index d6fd68fa5..4f01f72ef 100644
--- a/samples/cpp/stitching_detailed.cpp
+++ b/samples/cpp/stitching_detailed.cpp
@@ -486,14 +486,20 @@ int main(int argc, char* argv[])
     (*adjuster)(features, pairwise_matches, cameras);
 
     // Find median focal length
+
     vector<double> focals;
     for (size_t i = 0; i < cameras.size(); ++i)
     {
         LOGLN("Camera #" << indices[i]+1 << ":\n" << cameras[i].K());
         focals.push_back(cameras[i].focal);
     }
-    nth_element(focals.begin(), focals.begin() + focals.size()/2, focals.end());
-    float warped_image_scale = static_cast<float>(focals[focals.size() / 2]);
+
+    sort(focals.begin(), focals.end());
+    float warped_image_scale;
+    if (focals.size() % 2 == 1)
+        warped_image_scale = static_cast<float>(focals[focals.size() / 2]);
+    else
+        warped_image_scale = static_cast<float>(focals[focals.size() / 2 - 1] + focals[focals.size() / 2]) * 0.5f;
 
     if (do_wave_correct)
     {