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

add option to manually input forcal length

Author: floriantschopp

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

@@ -1,5 +1,6 @@
 #include <opencv2/core/eigen.hpp>
 #include <Eigen/StdVector>
+#include <iostream>
 
 namespace aslam {
 
@@ -777,9 +778,16 @@ bool PinholeProjection<DISTORTION_T>::initializeIntrinsics(const std::vector<Gri
     }
   }
 
-  //get the median of the guesses
-  if(f_guesses.empty())
-    return false;
+  // Get the median of the guesses if available.
+  if(f_guesses.empty()) {
+   double input_guess;
+   std::cout << "Initialization of focal length failed. Provide manual initialization: ";
+   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 << "\n";
+   f_guesses.push_back(input_guess);
+  }
   double f0 = PinholeHelpers::medianOfVectorElements(f_guesses);
 
   //set the estimate