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 03600184b..56521a082 100644 --- a/aslam_cv/aslam_cameras/include/aslam/cameras/implementation/PinholeProjection.hpp +++ b/aslam_cv/aslam_cameras/include/aslam/cameras/implementation/PinholeProjection.hpp @@ -1,5 +1,7 @@ #include #include +#include +#include namespace aslam { @@ -776,10 +778,23 @@ 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); + } 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