From 82f5235f7da5f6f63b997d2f61bc220b7149cea4 Mon Sep 17 00:00:00 2001 From: Alex Liberzon Date: Fri, 10 Nov 2023 19:34:08 +0200 Subject: [PATCH] fixed searcquader --- openptv_python/imgcoord.py | 14 +++ openptv_python/parameters.py | 12 +-- openptv_python/track.py | 99 ++++++++++---------- openptv_python/tracking_run.py | 2 +- openptv_python/trafo.py | 25 +++-- tests/test_parameters_bindings.py | 10 +- tests/test_tracking.py | 146 ++++++++++++++++++++++++++++++ 7 files changed, 240 insertions(+), 68 deletions(-) diff --git a/openptv_python/imgcoord.py b/openptv_python/imgcoord.py index b4914ab..6ddbb70 100644 --- a/openptv_python/imgcoord.py +++ b/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 diff --git a/openptv_python/parameters.py b/openptv_python/parameters.py index dae4f47..458825d 100644 --- a/openptv_python/parameters.py +++ b/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.""" diff --git a/openptv_python/track.py b/openptv_python/track.py index 30fff54..fe25611 100644 --- a/openptv_python/track.py +++ b/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]) diff --git a/openptv_python/tracking_run.py b/openptv_python/tracking_run.py index 6865a58..8f1ed05 100644 --- a/openptv_python/tracking_run.py +++ b/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], diff --git a/openptv_python/trafo.py b/openptv_python/trafo.py index 04b14f8..00a1bb5 100644 --- a/openptv_python/trafo.py +++ b/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 diff --git a/tests/test_parameters_bindings.py b/tests/test_parameters_bindings.py index dcb64dd..3426155 100644 --- a/tests/test_parameters_bindings.py +++ b/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): diff --git a/tests/test_tracking.py b/tests/test_tracking.py index 15dbf61..ee11fa5 100644 --- a/tests/test_tracking.py +++ b/tests/test_tracking.py @@ -13,22 +13,43 @@ import numpy as np +from openptv_python.calibration import Calibration, read_calibration from openptv_python.parameters import ( ControlPar, TrackPar, ) from openptv_python.track import ( + Foundpix, angle_acc, candsearch_in_pix, candsearch_in_pix_rest, + copy_foundpix_array, pos3d_in_bounds, predict, + reset_foundpix_array, search_volume_center_moving, + searchquader, + sort, ) from openptv_python.tracking_frame_buf import Target from openptv_python.vec_utils import vec_scalar_mul +def read_all_calibration(num_cams: int = 4) -> list[Calibration]: + """Read all calibration files.""" + ori_tmpl = "tests/testing_fodder/track/cal/cam%d.tif.ori" + added_tmpl = "tests/testing_fodder/track/cal/cam%d.tif.addpar" + + calib = [] + + for cam in range(num_cams): + ori_name = ori_tmpl % (cam + 1) + added_name = added_tmpl % (cam + 1) + calib.append(read_calibration(ori_name, added_name)) + + return calib + + class TestPredict(unittest.TestCase): """Test the predict function.""" @@ -219,5 +240,130 @@ def test_candsearch_in_pix(self): ) +class TestSort(unittest.TestCase): + def test_sort(self): + test_array = np.array([1.0, 2200.2, 0.3, -0.8, 100.0], dtype=float) + ix_array = np.array([0, 5, 13, 2, 124], dtype=int) + len_array = 5 + + sort(test_array, ix_array) + + self.assertTrue( + isclose(test_array[0], -0.8, rel_tol=1e-9), + f"Expected -0.8 but found { test_array[0] } ", + ) + self.assertNotEqual( + ix_array[len_array - 1], + 1, + f"Expected not to be 1 but found { ix_array[len_array - 1] }", + ) + + def test_copy_foundpix_array(self): + """Test the copy_foundpix_array function.""" + src = [Foundpix(1, 1, [1, 0]), Foundpix(2, 5, [1, 1])] + arr_len = 2 + num_cams = 2 + + dest = [Foundpix(0, 0, [0] * num_cams) for _ in range(arr_len)] + + reset_foundpix_array(dest, len(dest), num_cams) + + self.assertEqual( + dest[1].ftnr, -1, f"Expected dest[1].ftnr == -1 but found {dest[1].ftnr}" + ) + self.assertEqual( + dest[0].freq, 0, f"Expected dest[0].freq == 0 but found {dest[0].freq}" + ) + self.assertEqual( + dest[1].whichcam[0], 0, f"Expected 0 but found {dest[1].whichcam[0]}" + ) + + copy_foundpix_array(dest, src, arr_len, num_cams) + + self.assertEqual( + dest[1].ftnr, 2, f"Expected dest[1].ftnr == 2 but found {dest[1].ftnr}" + ) + + # print("Destination foundpix array:") + # for i in range(arr_len): + # print(f"ftnr = {dest[i].ftnr} freq = {dest[i].freq} whichcam = {dest[i].whichcam}") + + +class TestSearchQuader(unittest.TestCase): + def setUp(self): + self.cpar = ControlPar() + self.cpar.from_file("tests/testing_fodder/track/parameters/ptv.par") + self.cpar.mm.n2[0] = 1.0 + self.cpar.mm.n3 = 1.0 + + # self.calib = [None] * self.cpar.num_cams + self.calib = read_all_calibration(self.cpar.num_cams) + + def test_searchquader(self): + """Test the searchquader function.""" + point = np.array([185.5, 3.2, 203.9]) + + # print(f"cpar = {self.cpar}") + + tpar = TrackPar( + 0.4, 120, 0.2, -0.2, 0.1, -0.1, 0.1, -0.1, 0.0, 0.0, 0.0, 0.0, 1 + ) + xr, xl, yd, yu = searchquader(point, tpar, self.cpar, self.calib) + + # print(f"xr = {xr}, xl = {xl}, yd = {yd}, yu = {yu}") + + self.assertTrue( + isclose(xr[0], 0.560048, rel_tol=1e-6), + f"Expected 0.560048 but found {xr[0]}", + ) + self.assertTrue( + isclose(yu[1], 0.437303, rel_tol=1e-6), + f"Expected 0.437303 but found {yu[1]}", + ) + + # Let's test with just one camera to check borders + self.cpar.num_cams = 1 + tpar1 = TrackPar( + 0.4, 120, 0.0, -0.0, 0.0, -0.0, 0.0, -0.0, 0.0, 0.0, 0.0, 0.0, 1 + ) + xr, xl, yd, yu = searchquader(point, tpar1, self.cpar, self.calib) + + # print(f"xr = {xr}, xl = {xl}, yd = {yd}, yu = {yu}") + + self.assertTrue( + isclose(xr[0], 0.0, rel_tol=1e-9), f"Expected 0.0 but found {xr[0]}" + ) + + # Test with infinitely large values of tpar that should return about half the image size + tpar2 = TrackPar( + 0.4, + 120, + 1000.0, + -1000.0, + 1000.0, + -1000.0, + 1000.0, + -1000.0, + 0.0, + 0.0, + 0.0, + 0.0, + 1, + ) + + xr, xl, yd, yu = searchquader(point, tpar2, self.cpar, self.calib) + + # print(f"xr = {xr}, xl = {xl}, yd = {yd}, yu = {yu}") + + self.assertTrue( + isclose(xr[0] + xl[0], self.cpar.imx, rel_tol=1e-9), + f"Expected image size but found {xr[0] + xl[0]}", + ) + self.assertTrue( + isclose(yd[0] + yu[0], self.cpar.imy, rel_tol=1e-9), + f"Expected {self.cpar.imy} but found {yd[0] + yu[0]}", + ) + + if __name__ == "__main__": unittest.main()