Input focal length guess if initial guess fails. (#346)

* switch to PIL library for Image

* deactivate threading for cv image displays

* Update MulticamGraph.py

Preserve some backward compatibility with older python versions.

* fix opencv windows

* add option to manually input forcal length

* remove debug option

* PR review @LBern

* switch to double in the first place

* Revert "Merge pull request #304 from ethz-asl/feature/input_focal_length_guess"

This reverts commit 2999e18, reversing
changes made to ee2b09d.

* Revert "Revert "Merge pull request #304 from ethz-asl/feature/input_focal_length_guess""

This reverts commit aac34c2.

* move comment and fix indentation

* revive compatibility for scripted usage

Co-authored-by: Hannes Sommer <[email protected]>

Author: floriantschopp

aslam_cv/aslam_cameras/include/aslam/cameras/implementation/PinholeProjection.hpp

@@ -1,5 +1,7 @@
 #include <opencv2/core/eigen.hpp>
 #include <Eigen/StdVector>
+#include <iostream>
+#include <cstdlib>
 
 namespace aslam {
 
@@ -776,10 +778,23 @@ bool PinholeProjection<DISTORTION_T>::initializeIntrinsics(const std::vector<Gri
       }
     }
   }
-
-  //get the median of the guesses
-  if(f_guesses.empty())
-    return false;
+  if(f_guesses.empty()) {
+    const char* manual_input = std::getenv("KALIBR_MANUAL_FOCAL_LENGTH_INIT");
+    if(manual_input != nullptr) {
+      double input_guess;
+      std::cout << "Initialization of focal length failed. Provide manual initialization: " << std::endl;
+      std::cin >> input_guess;
+      SM_ASSERT_GT(std::runtime_error, input_guess, 0.0, 
+                "Focal length needs to be positive.");
+      std::cout << "Initializing focal length to " << input_guess << std::endl;
+      f_guesses.push_back(input_guess);
+    } else {
+      std::cout << "Initialization of focal length failed. You can enable"
+        << " manual input by setting 'KALIBR_MANUAL_FOCAL_LENGTH_INIT'." << std::endl;
+      return false;
+    }
+  }
+  // Get the median of the guesses if available.
   double f0 = PinholeHelpers::medianOfVectorElements(f_guesses);
 
   //set the estimate