fixed ori file reading

OpenPTV · Feb 15, 2024 · 228baac · 228baac
1 parent add5f35
commit 228baac
Show file tree

Hide file tree

Showing 2 changed files with 27 additions and 23 deletions.
diff --git a/openptv_python/calibration.py b/openptv_python/calibration.py
@@ -145,9 +145,9 @@ def from_file(cls, ori_file: str, add_file: str | None):
         ret = cls()
 
         with open(ori_file, "r", encoding="utf-8") as fp:
-            data  = fp.read()
+            tmp  = fp.read()
 
-        data = np.fromstring(data, dtype=float, sep=" ")
+        data = np.fromstring(tmp, dtype=float, sep=" ")
         # print(data)
 
         ret.set_pos(data[:3])

diff --git a/openptv_python/trafo.py b/openptv_python/trafo.py
@@ -3,7 +3,6 @@
 
 import numpy as np
 from numba import float64, int32, njit
-from numpy import cos, sin, sqrt
 
 from .calibration import Calibration
 from .parameters import ControlPar
@@ -153,7 +152,7 @@ def distort_brown_affine(x: float,
     if x == 0 and y == 0:
         return 0, 0
 
-    r = sqrt(x**2 + y**2)
+    r = np.sqrt(x**2 + y**2)
 
     x += (
         x * (ap.k1 * r**2 + ap.k2 * r**4 + ap.k3 * r**6)
@@ -166,15 +165,13 @@ def distort_brown_affine(x: float,
         + 2 * ap.p1 * x * y
     )
 
-    x1 = ap.scx * x - sin(ap.she) * y
-    y1 = cos(ap.she) * y
+    x1 = ap.scx * x - np.sin(ap.she) * y
+    y1 = np.cos(ap.she) * y
 
     return x1, y1
 
 @njit(fastmath=True, cache=True, nogil=True)
-def correct_brown_affine(
-    x: float, y: float, ap: np.recarray, tol: float = 1e-5
-) -> Tuple[float, float]:
+def correct_brown_affine(x: float, y: float, ap: np.recarray, tol: float = 1e-5) -> Tuple[float, float]:
     """Correct a distorted point using the Brown affine model."""
     r, rq, xq, yq = 0.0, 0.0, x, y
     itnum = 0
@@ -183,21 +180,29 @@ def correct_brown_affine(
         return xq, yq
 
     rq = np.sqrt(x**2 + y**2)
+    two_p1 = 2 * ap.p1
+    two_p2 = 2 * ap.p2
+    cos_she = np.cos(ap.she)
+    sin_she = np.sin(ap.she)
 
     while True:
         r = rq
+        common_term = (ap.k1 * r**2 + ap.k2 * r**4 + ap.k3 * r**6)
+        xq_common = xq * common_term
+        yq_common = yq * common_term
+
         xq = (
-            (x + yq * np.sin(ap.she)) / ap.scx
-            - xq * (ap.k1 * r**2 + ap.k2 * r**4 + ap.k3 * r**6)
+            (x + yq * sin_she) / ap.scx
+            - xq_common
             - ap.p1 * (r**2 + 2 * xq**2)
-            - 2 * ap.p2 * xq * yq
+            - two_p2 * xq * yq
         )
 
         yq = (
-            y / np.cos(ap.she)
-            - yq * (ap.k1 * r**2 + ap.k2 * r**4 + ap.k3 * r**6)
+            y / cos_she
+            - yq_common
             - ap.p2 * (r**2 + 2 * yq**2)
-            - 2 * ap.p1 * xq * yq
+            - two_p1 * xq * yq
         )
 
         rq = np.sqrt(xq**2 + yq**2)
@@ -207,27 +212,26 @@ def correct_brown_affine(
 
         itnum += 1
 
-        if itnum >= 201 or abs(rq - r) / r <= tol:
+        if itnum >= 201 or np.abs(rq - r) <= tol * r:
             break
 
     r = rq
     x1 = (
-        (x + yq * np.sin(ap.she)) / ap.scx
-        - xq * (ap.k1 * r**2 + ap.k2 * r**4 + ap.k3 * r**6)
+        (x + yq * sin_she) / ap.scx
+        - xq * common_term
         - ap.p1 * (r**2 + 2 * xq**2)
-        - 2 * ap.p2 * xq * yq
+        - two_p2 * xq * yq
     )
 
     y1 = (
-        y / np.cos(ap.she)
-        - yq * (ap.k1 * r**2 + ap.k2 * r**4 + ap.k3 * r**6)
+        y / cos_she
+        - yq * common_term
         - ap.p2 * (r**2 + 2 * yq**2)
-        - 2 * ap.p1 * xq * yq
+        - two_p1 * xq * yq
     )
 
     return x1, y1
 
-
 def flat_to_dist(flat_x: float, flat_y: float, cal: Calibration) -> Tuple[float, float]:
     """Convert flat-image coordinates to real-image coordinates.