Merge pull request #5 from OpenPTV/test_optimize_calibration

Test optimize calibration

openptv_python/calibration.py

@@ -70,16 +70,17 @@ def rotation_matrix(ext: np.ndarray) -> None:
 Interior = np.array( (0, 0, 0), dtype = interior_dtype).view(np.recarray)
 
 
-ap52_dtype = np.dtype([
-    ('k1', np.float64),
-    ('k2', np.float64),
-    ('k3', np.float64),
-    ('p1', np.float64),
-    ('p2', np.float64),
-    ('scx', np.float64),
-    ('she', np.float64)
-    ])
-ap_52 = np.array((0, 0, 0, 0, 0, 1, 0), dtype = ap52_dtype).view(np.recarray)
+# ap52_dtype = np.dtype([
+#     ('k1', np.float64),
+#     ('k2', np.float64),
+#     ('k3', np.float64),
+#     ('p1', np.float64),
+#     ('p2', np.float64),
+#     ('scx', np.float64),
+#     ('she', np.float64)
+#     ])
+ap_52 = np.array((0, 0, 0, 0, 0, 1, 0), dtype = np.float64)
+# ap_52 = np.array((0, 0, 0, 0, 0, 1, 0), dtype = ap52_dtype).view(np.recarray)
 
 mmlut_dtype = np.dtype([
     ('origin', np.float64, 3),
@@ -177,9 +178,10 @@ def from_file(cls, ori_file: str, add_file: str | None):
         else:
             # print("no addpar fallback used")  # Waits for proper logging.
             print("No addpar file found. Using default values for radial distortion")
-            ret.added_par.k1 = ret.added_par.k2 = ret.added_par.k3 \
-                = ret.added_par.p1 = ret.added_par.p2 = ret.added_par.she = 0.0
-            ret.added_par.scx = 1.0
+            # ret.added_par.k1 = ret.added_par.k2 = ret.added_par.k3 \
+            #     = ret.added_par.p1 = ret.added_par.p2 = ret.added_par.she = 0.0
+            # ret.added_par.scx = 1.0
+            ret.added_par = np.array([0,0,0,0,0,1,0], dtype=np.float64)
 
         return ret
         # print(f"Calibration data read from files {ori_file} and {add_file}")
@@ -300,7 +302,9 @@ def set_radial_distortion(self, dist_coeffs: np.ndarray) -> None:
         if dist_coeffs.shape != (3,):
             raise ValueError("Expected a 3-element array")
 
-        self.added_par.k1, self.added_par.k2, self.added_par.k3 = dist_coeffs
+        self.added_par[:3] = dist_coeffs
+
+        # self.added_par.k1, self.added_par.k2, self.added_par.k3 = dist_coeffs
 
 
     def get_radial_distortion(self):
@@ -309,7 +313,8 @@ def get_radial_distortion(self):
 
         array, from lowest power to highest.
         """
-        return np.r_[self.added_par.k1, self.added_par.k2, self.added_par.k3]
+        # return np.r_[self.added_par.k1, self.added_par.k2, self.added_par.k3]
+        return self.added_par[:3]
 
     def set_decentering(self, decent: np.ndarray) -> None:
         """
@@ -322,11 +327,13 @@ def set_decentering(self, decent: np.ndarray) -> None:
         if decent.shape != (2,):
             raise ValueError("Expected a 2-element list")
 
-        self.added_par.p1, self.added_par.p2 = decent
+        # self.added_par.p1, self.added_par.p2 = decent
+        self.added_par[3:5] = decent
 
     def get_decentering(self):
         """Return the decentering parameters [1] as a 2 element array, (p_1, p_2)."""
-        return np.r_[self.added_par.p1, self.added_par.p2]
+        # return np.r_[self.added_par.p1, self.added_par.p2]
+        return self.added_par[3:5]
 
     def set_affine_distortion(self, affine: np.ndarray) -> None:
         """
@@ -339,11 +346,13 @@ def set_affine_distortion(self, affine: np.ndarray) -> None:
         if affine.shape != (2,):
             raise ValueError("Expected a 2-element list")
 
-        self.added_par.scx, self.added_par.she = affine
+        # self.added_par.scx, self.added_par.she = affine
+        self.added_par[5:] = affine
 
     def get_affine(self):
         """Return the affine transform parameters [1] as a 2 element array, (scx, she)."""
-        return np.r_[self.added_par.scx, self.added_par.she]
+        # return np.r_[self.added_par.scx, self.added_par.she]
+        return self.added_par[5:]
 
     def set_glass_vec(self, gvec: np.ndarray):
         """
@@ -368,7 +377,8 @@ def get_glass_vec(self) -> np.ndarray:
 
     def set_added_par(self, ap52_array: np.ndarray):
         """Set added par from an numpy array of parameters."""
-        self.added_par = np.array(tuple(ap52_array.tolist()), dtype = ap52_dtype).view(np.recarray)
+        # self.added_par = np.array(tuple(ap52_array.tolist()), dtype = ap52_dtype).view(np.recarray)
+        self.added_par = ap52_array
 
     def copy(self, new_copy):
         """Copy the calibration data to a new object."""

openptv_python/orientation.py

@@ -16,7 +16,7 @@
 from .parameters import ControlPar, MultimediaPar, OrientPar, VolumePar
 from .ray_tracing import ray_tracing
 from .sortgrid import sortgrid
-from .tracking_frame_buf import Target, TargetArray
+from .tracking_frame_buf import Target
 from .trafo import correct_brown_affine, pixel_to_metric
 from .vec_utils import unit_vector, vec_norm, vec_set
 
@@ -301,14 +301,14 @@ def orient(
         cal.int_par.cc,
         cal.int_par.xh,
         cal.int_par.yh,
-        cal.added_par.k1,
-        cal.added_par.k2,
-        cal.added_par.k3,
-        cal.added_par.p1,
-        cal.added_par.p2,
-        cal.added_par.scx,
-        cal.added_par.she,
-    ]
+        cal.added_par[0],
+        cal.added_par[1],
+        cal.added_par[2],
+        cal.added_par[3],
+        cal.added_par[4],
+        cal.added_par[5],
+        cal.added_par[6]
+        ]
 
     # backup for changing back and forth
     safety_x = cal.glass_par[0]
@@ -343,40 +343,40 @@ def orient(
             # derivatives of distortion parameters
             r = np.sqrt(xp * xp + yp * yp)
 
-            X[n][7] = cal.added_par.scx
-            X[n + 1][7] = np.sin(cal.added_par.she)
+            X[n][7] = cal.added_par[5] # cal.added_par[5]
+            X[n + 1][7] = np.sin(cal.added_par[6]) #np.sin(cal.added_par[6])
 
             X[n][8] = 0
             X[n + 1][8] = 1
 
-            X[n][9] = cal.added_par.scx * xp * r * r
+            X[n][9] = cal.added_par[5] * xp * r * r
             X[n + 1][9] = yp * r * r
 
-            X[n][10] = cal.added_par.scx * xp * pow(r, 4)
+            X[n][10] = cal.added_par[5] * xp * pow(r, 4)
             X[n + 1][10] = yp * pow(r, 4)
 
-            X[n][11] = cal.added_par.scx * xp * pow(r, 6)
+            X[n][11] = cal.added_par[5] * xp * pow(r, 6)
             X[n + 1][11] = yp * pow(r, 6)
 
-            X[n][12] = cal.added_par.scx * (2 * xp * xp + r * r)
+            X[n][12] = cal.added_par[5] * (2 * xp * xp + r * r)
             X[n + 1][12] = 2 * xp * yp
 
-            X[n][13] = 2 * cal.added_par.scx * xp * yp
+            X[n][13] = 2 * cal.added_par[5] * xp * yp
             X[n + 1][13] = 2 * yp * yp + r * r
 
-            qq = cal.added_par.k1 * r * r
-            qq += cal.added_par.k2 * pow(r, 4)
-            qq += cal.added_par.k3 * pow(r, 6)
+            qq = cal.added_par[0] * r * r
+            qq += cal.added_par[1] * pow(r, 4)
+            qq += cal.added_par[2] * pow(r, 6)
             qq += 1
             X[n][14] = (
                 xp * qq
-                + cal.added_par.p1 * (r * r + 2 * xp * xp)
-                + 2 * cal.added_par.p2 * xp * yp
+                + cal.added_par[3] * (r * r + 2 * xp * xp)
+                + 2 * cal.added_par[4] * xp * yp
             )
             X[n + 1][14] = 0
 
-            X[n][15] = -np.cos(cal.added_par.she) * yp
-            X[n + 1][15] = -np.sin(cal.added_par.she) * yp
+            X[n][15] = -np.cos(cal.added_par[6]) * yp
+            X[n + 1][15] = -np.sin(cal.added_par[6]) * yp
 
             # numeric derivatives of projection coordinates over external parameters,
             # 3D position and the angles
@@ -449,13 +449,13 @@ def orient(
         y[n_obs + 0] = ident[0] - cal.int_par.cc
         y[n_obs + 1] = ident[1] - cal.int_par.xh
         y[n_obs + 2] = ident[2] - cal.int_par.yh
-        y[n_obs + 3] = ident[3] - cal.added_par.k1
-        y[n_obs + 4] = ident[4] - cal.added_par.k2
-        y[n_obs + 5] = ident[5] - cal.added_par.k3
-        y[n_obs + 6] = ident[6] - cal.added_par.p1
-        y[n_obs + 7] = ident[7] - cal.added_par.p2
-        y[n_obs + 8] = ident[8] - cal.added_par.scx
-        y[n_obs + 9] = ident[9] - cal.added_par.she
+        y[n_obs + 3] = ident[3] - cal.added_par[0]
+        y[n_obs + 4] = ident[4] - cal.added_par[1]
+        y[n_obs + 5] = ident[5] - cal.added_par[2]
+        y[n_obs + 6] = ident[6] - cal.added_par[3]
+        y[n_obs + 7] = ident[7] - cal.added_par[4]
+        y[n_obs + 8] = ident[8] - cal.added_par[5]
+        y[n_obs + 9] = ident[9] - cal.added_par[6]
 
         # weights
         for i in range(n_obs):
@@ -540,13 +540,13 @@ def orient(
         cal.int_par.cc += beta[6]
         cal.int_par.xh += beta[7]
         cal.int_par.yh += beta[8]
-        cal.added_par.k1 += beta[9]
-        cal.added_par.k2 += beta[10]
-        cal.added_par.k3 += beta[11]
-        cal.added_par.p1 += beta[12]
-        cal.added_par.p2 += beta[13]
-        cal.added_par.scx += beta[14]
-        cal.added_par.she += beta[15]
+        cal.added_par[0] += beta[9]
+        cal.added_par[1] += beta[10]
+        cal.added_par[2] += beta[11]
+        cal.added_par[3] += beta[12]
+        cal.added_par[4] += beta[13]
+        cal.added_par[5] += beta[14]
+        cal.added_par[6] += beta[15]
 
         if flags.interfflag:
             cal.glass_par[0] += e1[0] * nGl * beta[16]
@@ -606,14 +606,14 @@ def raw_orient(
     beta = np.zeros(6)
     pos = np.zeros(3)
 
-    cal.added_par.k1 = 0
-    cal.added_par.k2 = 0
-    cal.added_par.k3 = 0
-    cal.added_par.p1 = 0
-    cal.added_par.p2 = 0
-    cal.added_par.scx = 1
-    cal.added_par.she = 0
-
+    # cal.added_par[0] = 0
+    # cal.added_par[1] = 0
+    # cal.added_par[2] = 0
+    # cal.added_par[3] = 0
+    # cal.added_par[4] = 0
+    # cal.added_par[5] = 1
+    # cal.added_par[6] = 0
+    cal.added_par = np.array([0, 0, 0, 0, 0, 1, 0], dtype=np.float64)
     itnum = 0
     stopflag = False
 
@@ -787,7 +787,7 @@ def external_calibration(
 def full_calibration(
     cal: Calibration,
     ref_pts: np.ndarray,
-    img_pts: TargetArray,
+    img_pts: np.ndarray,
     cparam: ControlPar,
     orient_par: OrientPar,
     dm: float = 1e-6,
@@ -832,7 +832,16 @@ def full_calibration(
     ValueError if iteration did not converge.
     """
     err_est = np.empty((NPAR + 1), dtype=np.float64)
-    residuals = orient(cal, cparam, len(ref_pts), ref_pts, img_pts, orient_par, err_est, dm=dm, drad=drad)
+
+    # convert numpy array to list of Target objects
+    targs = [Target() for _ in img_pts]
+
+    for ptx, pt in enumerate(img_pts):
+        targs[ptx].x = pt[0]
+        targs[ptx].y = pt[1]
+        targs[ptx].pnr = ptx
+
+    residuals = orient(cal, cparam, len(ref_pts), ref_pts, targs, orient_par, err_est, dm=dm, drad=drad)
 
     # free(orip)
 
@@ -844,7 +853,7 @@ def full_calibration(
     ret = np.empty((len(img_pts), 2))
     used = np.empty(len(img_pts), dtype=np.int_)
 
-    for ix, img_pt in enumerate(img_pts):
+    for ix, img_pt in enumerate(targs):
         ret[ix] = (residuals[2 * ix], residuals[2 * ix + 1])
         used[ix] = img_pt.pnr
 
@@ -855,7 +864,7 @@ def full_calibration(
 def match_detection_to_ref(
     cal: Calibration,
     ref_pts: np.ndarray,
-    img_pts: TargetArray,
+    img_pts: List[Target],
     cparam: ControlPar,
     eps: int = 25,
 ) -> List[Target]:

openptv_python/tracking_frame_buf.py

@@ -136,31 +136,31 @@ def sort_target_y(targets: List[Target]) -> List[Target]:
     """Sort targets by y coordinate."""
     return sorted(targets, key=lambda t: t.y)
 
-class TargetArray(list):
-    """Target array class."""
+# class TargetArray(list):
+#     """Target array class."""
 
-    def __init__(self, num_targets: int = 0):
-        super().__init__(Target() for item in range(num_targets))
+#     def __init__(self, num_targets: int = 0):
+#         super().__init__(Target() for item in range(num_targets))
 
-    def __setitem__(self, index, item):
-        super().__setitem__(index, item)
+#     def __setitem__(self, index, item):
+#         super().__setitem__(index, item)
 
-    def insert(self, index, item):
-        super().insert(index, item)
+#     def insert(self, index, item):
+#         super().insert(index, item)
 
-    def append(self, item):
-        super().append(str(item))
+#     def append(self, item):
+#         super().append(str(item))
 
-    def extend(self, other):
-        if isinstance(other, type(self)):
-            super().extend(other)
-        else:
-            super().extend(item for item in other)
+#     def extend(self, other):
+#         if isinstance(other, type(self)):
+#             super().extend(other)
+#         else:
+#             super().extend(item for item in other)
 
-    @property
-    def num_targs(self):
-        """Return the number of targets in the list."""
-        return len(self)
+#     @property
+#     def num_targs(self):
+#         """Return the number of targets in the list."""
+#         return len(self)
 
 
 def read_targets(file_base: str, frame_num: int) -> List[Target]: