Input focal length guess if initial guess fails.

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