diff --git a/aslam_cv/aslam_cameras/include/aslam/cameras/implementation/PinholeProjection.hpp b/aslam_cv/aslam_cameras/include/aslam/cameras/implementation/PinholeProjection.hpp index abd1f5fa4..4180b8456 100644 --- a/aslam_cv/aslam_cameras/include/aslam/cameras/implementation/PinholeProjection.hpp +++ b/aslam_cv/aslam_cameras/include/aslam/cameras/implementation/PinholeProjection.hpp @@ -780,20 +780,19 @@ bool PinholeProjection::initializeIntrinsics(const std::vector> 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); + double input_guess; + if(manual_input != nullptr && manual_input[0] != '\0') { + input_guess = std::stod(manual_input); } else { - std::cout << "Initialization of focal length failed. You can enable" - << " manual input by setting 'KALIBR_MANUAL_FOCAL_LENGTH_INIT'." << std::endl; - return false; + std::cout << "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); } // Get the median of the guesses if available. double f0 = PinholeHelpers::medianOfVectorElements(f_guesses);