Merge pull request #317 from christian-rauch/fix_len0

fix 0 length and determinant
AprilRobotics · Feb 5, 2024 · 51466ec · 51466ec
2 parents f8ce185 + 3a0a155
commit 51466ec
Show file tree

Hide file tree

Showing 2 changed files with 10 additions and 3 deletions.
diff --git a/apriltag.c b/apriltag.c
@@ -445,6 +445,8 @@ static matd_t* homography_compute2(double c[4][4]) {
             }
         }
 
+        assert(max_val_idx >= 0);
+
         if (max_val < epsilon) {
             debug_print("WRN: Matrix is singular.\n");
             return NULL;

diff --git a/apriltag_quad_thresh.c b/apriltag_quad_thresh.c
@@ -266,8 +266,13 @@ void fit_line(struct line_fit_pt *lfps, int sz, int i0, int i1, double *lineparm
         }
 
         double length = sqrtf(M);
-        lineparm[2] = nx/length;
-        lineparm[3] = ny/length;
+        if (fabs(length) < 1e-12) {
+            lineparm[2] = lineparm[3] = 0;
+        }
+        else {
+            lineparm[2] = nx/length;
+            lineparm[3] = ny/length;
+        }
     }
 
     // sum of squared errors =
@@ -899,11 +904,11 @@ int fit_quad(
         double det = A00 * A11 - A10 * A01;
 
         // inverse.
-        double W00 = A11 / det, W01 = -A01 / det;
         if (fabs(det) < 0.001) {
             res = 0;
             goto finish;
         }
+        double W00 = A11 / det, W01 = -A01 / det;
 
         // solve
         double L0 = W00*B0 + W01*B1;