From 351da4e18ef7d9a78b792fba840012dd40519291 Mon Sep 17 00:00:00 2001
From: Aymeric Dujardin <adujardin@users.noreply.github.com>
Date: Wed, 26 Feb 2020 17:48:02 +0100
Subject: [PATCH] Add validity check to focal guess

---
 .../include/aslam/cameras/implementation/PinholeProjection.hpp | 3 ++-
 1 file changed, 2 insertions(+), 1 deletion(-)

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 56521a082..abd1f5fa4 100644
--- a/aslam_cv/aslam_cameras/include/aslam/cameras/implementation/PinholeProjection.hpp
+++ b/aslam_cv/aslam_cameras/include/aslam/cameras/implementation/PinholeProjection.hpp
@@ -774,7 +774,8 @@ bool PinholeProjection<DISTORTION_T>::initializeIntrinsics(const std::vector<Gri
           continue;
 
          double f_guess = cv::norm(ipts.at(0) - ipts.at(1)) / M_PI;
-         f_guesses.push_back(f_guess);
+         if(std::isfinite(f_guess))
+            f_guesses.push_back(f_guess);
       }
     }
   }