[Mutliview/P3P] fix openMVG#1597 thx @mpizenberg & @midjji

JoonVan · Nov 9, 2019 · 3bc6dc6 · 3bc6dc6
1 parent 854fda9
commit 3bc6dc6
Showing 1 changed file with 20 additions and 16 deletions.
diff --git a/src/openMVG/multiview/solver_resection_p3p_nordberg.cpp b/src/openMVG/multiview/solver_resection_p3p_nordberg.cpp
@@ -393,31 +393,35 @@ bool computePosesNordberg(
     {
       double tau1, tau2;
       root2real(b, c, tau1, tau2);
-      if (tau1 > 0)
+      if (tau1 > 0.0)
       {
         double tau = tau1;
         double d = a23 / (tau * (b23 + tau) + 1.0);
-        double l2 = std::sqrt(d);
-        double l3 = tau * l2;
-
-        double l1 = w0 * l2 + w1 * l3;
-        if (l1 >= 0.0)
-        {
-          Ls[valid] = Vec3(l1, l2, l3);
-          ++valid;
+        if(d > 0.0) {
+          double l2 = std::sqrt(d);
+          double l3 = tau * l2;
+
+          double l1 = w0 * l2 + w1 * l3;
+          if (l1 >= 0.0)
+          {
+            Ls[valid] = Vec3(l1, l2, l3);
+            ++valid;
+          }          
         }
       }
       if (tau2 > 0.0)
       {
         double tau = tau2;
         double d = a23 / (tau * (b23 + tau) + 1.0);
-        double l2 = std::sqrt(d);
-        double l3 = tau * l2;
-        double l1 = w0 * l2 + w1 * l3;
-        if (l1 >= 0.0)
-        {
-          Ls[valid] = Vec3(l1, l2, l3);
-          ++valid;
+        if(d > 0.0) {
+          double l2 = std::sqrt(d);
+          double l3 = tau * l2;
+          double l1 = w0 * l2 + w1 * l3;
+          if (l1 >= 0.0)
+          {
+            Ls[valid] = Vec3(l1, l2, l3);
+            ++valid;
+          }
         }
       }
     }