From fa6d6520c7325e44ff8a4e1202aa3e1bc39e6413 Mon Sep 17 00:00:00 2001
From: Manolis Lourakis <60357220+mlourakis@users.noreply.github.com>
Date: Sun, 6 Oct 2024 17:24:15 +0300
Subject: [PATCH] inversion checks

Extra checks for corner cases in 3x3 matrix inversion
---
 modules/calib3d/src/sqpnp.cpp | 3 ++-
 1 file changed, 2 insertions(+), 1 deletion(-)

diff --git a/modules/calib3d/src/sqpnp.cpp b/modules/calib3d/src/sqpnp.cpp
index 973e95d000..ef2a047d80 100644
--- a/modules/calib3d/src/sqpnp.cpp
+++ b/modules/calib3d/src/sqpnp.cpp
@@ -453,6 +453,7 @@ bool PoseSolver::invertSPD3x3(const cv::Matx<double, 3, 3>& A, cv::Matx<double,
     v[0]=L[6]*D[0];
     v[1]=L[7]*D[1];
     D[2]=A(2, 2)-L[6]*v[0]-L[7]*v[1];
+    if(D[2]<=1E-10) return false;
     //L[8]=1.0;
 
     D[0]=1.0/D[0];
@@ -510,7 +511,7 @@ bool PoseSolver::analyticalInverse3x3Symm(const cv::Matx<double, 3, 3>& Q,
     t12 = c * c;
     double det = -t4 * f + a * t2 + t7 * f - 2.0 * t9 * e + t12 * d;
 
-    if (fabs(det) < threshold) return false;
+    if (fabs(det) < threshold) { cv::invert(Q, Qinv, cv::DECOMP_SVD); return false; } // fall back to pseudoinverse
 
     // 3. Inverse
     double t15, t20, t24, t30;