fixed searcquader

openptv_python/imgcoord.py

@@ -37,16 +37,26 @@ def flat_image_coord(
         cal.ext_par, mm, cal.glass_par, orig_pos, cal_t.ext_par
     )
 
+    # print(f"pos_t {pos_t}")
+    # print(f"cross_p {cross_p}")
+    # print(f"cros_c {cross_c}")
+
     x_t, y_t = multimed_nlay(cal_t, mm, pos_t)
+    # print(f"x_t {x_t}, y_t {y_t}")
+
     pos_t = vec_set(x_t, y_t, pos_t[2])
     pos = back_trans_point(pos_t, mm, cal.glass_par, cross_p, cross_c)
 
+    # print(f"pos {pos}")
+
     deno = (
         cal.ext_par.dm[0][2] * (pos[0] - cal.ext_par.x0)
         + cal.ext_par.dm[1][2] * (pos[1] - cal.ext_par.y0)
         + cal.ext_par.dm[2][2] * (pos[2] - cal.ext_par.z0)
     )
 
+    # print(f"deno {deno}")
+
     if deno == 0:
         deno = 1
 
@@ -70,6 +80,8 @@ def flat_image_coord(
         / deno
     )
 
+    # print(f"x {x}, y {y}")
+
     return x, y
 
 
@@ -96,6 +108,8 @@ def img_coord(
     # Distort the metric coordinates using the Brown distortion model
     x, y = flat_to_dist(x, y, cal)
 
+    # print("f flat_to_dist: x = {x}, y = {y}")
+
     return x, y
 
 

openptv_python/parameters.py

@@ -147,8 +147,8 @@ class TrackPar:
     dvxmin: float = 0.0
     dvymax: float = 0.0
     dvymin: float = 0.0
-    dvz_max: float = 0.0
-    dvz_min: float = 0.0
+    dvzmax: float = 0.0
+    dvzmin: float = 0.0
     dsumg: float = 0.0
     dn: float = 0.0
     dnx: float = 0.0
@@ -171,8 +171,8 @@ def from_file(self, filename: str):
                 self.dvxmax = float(fpp.readline().rstrip())
                 self.dvymin = float(fpp.readline().rstrip())
                 self.dvymax = float(fpp.readline().rstrip())
-                self.dvz_min = float(fpp.readline().rstrip())
-                self.dvz_max = float(fpp.readline().rstrip())
+                self.dvzmin = float(fpp.readline().rstrip())
+                self.dvzmax = float(fpp.readline().rstrip())
                 self.add = int(fpp.readline().rstrip())
         except IOError as exc:
             raise (f"Error reading tracking parameters from {filename}") from exc
@@ -195,11 +195,11 @@ def get_dvymax(self):
 
     def get_dvz_min(self):
         """Return the minimum velocity in z direction."""
-        return self.dvz_min
+        return self.dvzmin
 
     def get_dvz_max(self):
         """Return the minimum velocity in z direction."""
-        return self.dvz_max
+        return self.dvzmax
 
     def get_dangle(self):
         """Return the maximum angle."""

openptv_python/track.py

@@ -149,7 +149,7 @@ def track_forward_start(tr: TrackingRun):
     tr.fb.prev()
 
 
-def reset_foundpix_array(arr, arr_len, num_cams):
+def reset_foundpix_array(arr: List[Foundpix], arr_len: int, num_cams: int) -> None:
     """Set default values for foundpix objects in an array.
 
     Arguments:
@@ -168,7 +168,9 @@ def reset_foundpix_array(arr, arr_len, num_cams):
             arr[i].whichcam[cam] = 0
 
 
-def copy_foundpix_array(dest, src, arr_len, num_cams):
+def copy_foundpix_array(
+    dest: List[Foundpix], src: List[Foundpix], arr_len: int, num_cams: int
+) -> None:
     """copy_foundpix_array() copies foundpix objects from one array to another.
 
     Arguments:
@@ -285,7 +287,7 @@ def pos3d_in_bounds(pos, bounds):
     return (
         bounds.dvxmin < pos[0] < bounds.dvxmax
         and bounds.dvymin < pos[1] < bounds.dvymax
-        and bounds.dvz_min < pos[2] < bounds.dvz_max
+        and bounds.dvzmin < pos[2] < bounds.dvzmax
     )
 
 
@@ -475,39 +477,28 @@ def candsearch_in_pix_rest(
     return counter
 
 
-def searchquader(point, tpar, cpar, cal):
-    def vec_set(vec, x, y, z):
-        vec[0], vec[1], vec[2] = x, y, z
+def searchquader(
+    point: np.ndarray, tpar: TrackPar, cpar: ControlPar, cal: List[Calibration]
+) -> Tuple[np.ndarray, np.ndarray, np.ndarray, np.ndarray]:
+    """Calculate the search volume in image space."""
+    mins = np.array([tpar.dvxmin, tpar.dvymin, tpar.dvzmin])
+    maxes = np.array([tpar.dvxmax, tpar.dvymax, tpar.dvzmax])
 
-    def vec_copy(dest, src):
-        dest[0], dest[1], dest[2] = src[0], src[1], src[2]
-
-    def point_to_pixel(pixel, point, cal, cpar):
-        pixel[0] = cal.camplane_u(cpar, point)
-        pixel[1] = cal.camplane_v(cpar, point)
-
-    def project_to_pixel(corners, point, mins, maxes, cal, cpar):
-        for pt in range(8):
-            vec_copy(corners[pt], point)
-            for dim in range(3):
-                if pt & 1 << dim:
-                    corners[pt][dim] += maxes[dim]
-                else:
-                    corners[pt][dim] += mins[dim]
-
-            point_to_pixel(corners[pt], corners[pt], cal, cpar)
-
-    xr, xl, yd, yu = np.zeros(4), np.zeros(4), np.zeros(4), np.zeros(4)
-    mins, maxes = np.zeros(3), np.zeros(3)
     quader = np.zeros((8, 3))
-    center = np.zeros(2)
-    corner = np.zeros(2)
-
-    vec_set(mins, tpar.dvxmin, tpar.dvymin, tpar.dvz_min)
-    vec_set(maxes, tpar.dvxmax, tpar.dvymax, tpar.dvz_max)
-
-    # 3D positions of search volume - eight corners of a box
-    project_to_pixel(quader, point, mins, maxes, cal[0], cpar)
+    xr = np.zeros(cpar.num_cams)
+    xl = np.zeros(cpar.num_cams)
+    yd = np.zeros(cpar.num_cams)
+    yu = np.zeros(cpar.num_cams)
+
+    for pt in range(8):
+        quader[pt] = point.copy()
+        # print(f" pt {pt} {quader[pt]}")
+        for dim in range(3):
+            if pt & (1 << dim):
+                quader[pt][dim] += maxes[dim]
+            else:
+                quader[pt][dim] += mins[dim]
+        # print(f" pt {pt} {quader[pt]}")
 
     # calculation of search area in each camera
     for i in range(cpar.num_cams):
@@ -518,11 +509,13 @@ def project_to_pixel(corners, point, mins, maxes, cal, cpar):
         yu[i] = cpar.imy
 
         # pixel position of a search center
-        point_to_pixel(center, point, cal[i], cpar)
+        center = point_to_pixel(point, cal[i], cpar)
+        # print(" center", center[0], center[1])
 
         # mark 4 corners of the search region in pixels
         for pt in range(8):
-            point_to_pixel(corner, quader[pt], cal[i], cpar)
+            corner = point_to_pixel(quader[pt], cal[i], cpar)
+            # print(" corner", corner[0], corner[1])
 
             if corner[0] < xl[i]:
                 xl[i] = corner[0]
@@ -542,22 +535,23 @@ def project_to_pixel(corners, point, mins, maxes, cal, cpar):
         if yd[i] > cpar.imy:
             yd[i] = cpar.imy
 
-        # eventually xr,xl,yd,yu are pixel distances relative to the point
+        # print(" xl", xl[i], " xr", xr[i], " yu", yu[i], " yd", yd[i])
+
+        # eventually xr, xl, yd, yu are pixel distances relative to the point
         xr[i] = xr[i] - center[0]
         xl[i] = center[0] - xl[i]
         yd[i] = yd[i] - center[1]
         yu[i] = center[1] - yu[i]
 
+        # print(" xl", xl[i], " xr", xr[i], " yu", yu[i], " yd", yd[i])
+
+    return xr, xl, yd, yu
 
-def sort_candidates_by_freq(item, num_cams):
-    class FoundPix:
-        def __init__(self, ftnr=0, whichcam=[0] * 4, freq=0):
-            self.ftnr = ftnr
-            self.whichcam = whichcam
-            self.freq = freq
 
+def sort_candidates_by_freq(item: Foundpix, num_cams: int):
+    """Sort candidates by frequency."""
     MAX_CANDS = 1000
-    foundpix = [FoundPix() for i in range(num_cams * MAX_CANDS)]
+    foundpix = [Foundpix() for i in range(num_cams * MAX_CANDS)]
     foundpix[: len(item)] = item
 
     different = 0
@@ -601,8 +595,8 @@ def __init__(self, ftnr=0, whichcam=[0] * 4, freq=0):
     return different
 
 
-def sort(a, b):
-    """Sorts a float array 'a' and an integer array 'b' both of length n.
+def sort(a: np.ndarray, b: np.ndarray) -> None:
+    """In-place sorts a float array 'a' and an integer array 'b' equal lengths.
 
     Arguments:
     ---------
@@ -613,11 +607,10 @@ def sort(a, b):
     -------
     Sorted arrays a and b.
     """
-    len(a)
     idx = np.argsort(a)
-    a = a[idx]
-    b = b[idx]
-    return a, b
+    a[...] = a[idx]
+    b[...] = b[idx]
+    # return a, b
 
 
 def point_to_pixel(point: np.ndarray, cal: Calibration, cpar: ControlPar) -> np.ndarray:
@@ -633,8 +626,14 @@ def point_to_pixel(point: np.ndarray, cal: Calibration, cpar: ControlPar) -> np.
     -------
     vec2d with pixel positions (x,y) in the camera.
     """
+    # print(f"point {point}")
+    # print(f"cal {cal}")
+    # print(f"cpar.mm {cpar.mm}")
+
     x, y = img_coord(point, cal, cpar.mm)
+    # print("img coord x, y", x, y)
     x, y = metric_to_pixel(x, y, cpar)
+    # print("metric to pixel x, y", x, y)
     return np.array([x, y])
 
 

openptv_python/tracking_run.py

@@ -80,7 +80,7 @@ def tr_new(
     tr.lmax = np.norm(
         tpar.dvxmin - tpar.dvxmax,
         tpar.dvymin - tpar.dvymax,
-        tpar.dvz_min - tpar.dvz_max,
+        tpar.dvzmin - tpar.dvzmax,
     )
     volumedimension(
         vpar.X_lay[1],

openptv_python/trafo.py

@@ -1,4 +1,5 @@
 """Module for coordinate transformations."""
+from math import cos, sin, sqrt
 from typing import Tuple
 
 import numpy as np
@@ -80,14 +81,14 @@ def arr_metric_to_pixel(metric: np.ndarray, parameters: ControlPar) -> np.ndarra
     return pixel
 
 
-def distort_brown_affine(
-    x: np.float64, y: np.float64, ap: ap_52
-) -> Tuple[float, float]:
+def distort_brown_affine(x: float, y: float, ap: ap_52) -> Tuple[float, float]:
     """Distort a point using the Brown affine model."""
     if x == 0 and y == 0:
         return 0, 0
 
-    r = np.sqrt(x**2 + y**2)
+    # print(f"x {x}, y {y}")
+
+    r = sqrt(x**2 + y**2)
 
     x += (
         x * (ap.k1 * r**2 + ap.k2 * r**4 + ap.k3 * r**6)
@@ -99,8 +100,14 @@ def distort_brown_affine(
         + ap.p2 * (r**2 + 2 * y**2)
         + 2 * ap.p1 * x * y
     )
-    x1 = ap.scx * x - np.sin(ap.she) * y
-    y1 = np.cos(ap.she) * y
+
+    # print(f"x {x}, y {y}")
+    # print(f"ap.she {ap.she} ap.scx {ap.scx}")
+
+    x1 = ap.scx * x - sin(ap.she) * y
+    y1 = cos(ap.she) * y
+
+    # print(f"x1 {x1}, y1 {y1}")
 
     return x1, y1
 
@@ -219,10 +226,16 @@ def flat_to_dist(flat_x: float, flat_y: float, cal: Calibration) -> Tuple[float,
     Make coordinates relative to sensor center rather than primary point
     image coordinates, because distortion formula assumes it, [1] p.180.
     """
+    # print(f"flat_x {flat_x}, flat_y {flat_y}")
+    # print(f"cal.int {cal.int_par.xh}, {cal.int_par.yh}")
     flat_x += cal.int_par.xh
     flat_y += cal.int_par.yh
 
+    # print(f"flat_x {flat_x}, flat_y {flat_y}")
+
     dist_x, dist_y = distort_brown_affine(flat_x, flat_y, cal.added_par)
+    # print(f"dist_x {dist_x}, dist_y {dist_y}")
+
     return dist_x, dist_y
 
 

tests/test_parameters_bindings.py

@@ -116,7 +116,7 @@ class Test_TrackingParams(unittest.TestCase):
         typedef struct
     {
         double  dacc, dangle, dvxmax, dvxmin;
-        double dvymax, dvymin, dvz_max, dvz_min;
+        double dvymax, dvymin, dvzmax, dvzmin;
         int dsumg, dn, dnx, dny, add;
     } track
 
@@ -138,8 +138,8 @@ def setUp(self):
             dvxmax=4.4,
             dvymin=5.5,
             dvymax=6.6,
-            dvz_min=7.7,
-            dvz_max=8.8,
+            dvzmin=7.7,
+            dvzmax=8.8,
         )
 
     # Testing getters according to the values passed in setUp
@@ -152,8 +152,8 @@ def test_TrackingParams_getters(self):
         self.assertTrue(self.track_obj1.dvxmax == 4.4)
         self.assertTrue(self.track_obj1.dvymin == 5.5)
         self.assertTrue(self.track_obj1.dvymax == 6.6)
-        self.assertTrue(self.track_obj1.dvz_min == 7.7)
-        self.assertTrue(self.track_obj1.dvz_max == 8.8)
+        self.assertTrue(self.track_obj1.dvzmin == 7.7)
+        self.assertTrue(self.track_obj1.dvzmax == 8.8)
         self.assertTrue(self.track_obj1.add == 1)
 
     def test_TrackingParams_read_from_file(self):