From 6822ff95fce73a5cd47038347e995c2c67cd41bf Mon Sep 17 00:00:00 2001 From: Alex Liberzon Date: Sun, 17 Dec 2023 00:25:18 +0200 Subject: [PATCH] a bit faster numpy and numba --- openptv_python/correspondences.py | 9 ++-- openptv_python/multimed.py | 76 +++++++++++++++++++++---------- openptv_python/track.py | 47 +++++++++++-------- openptv_python/vec_utils.py | 34 ++++++++------ 4 files changed, 103 insertions(+), 63 deletions(-) diff --git a/openptv_python/correspondences.py b/openptv_python/correspondences.py index c1b7da8..5232d81 100644 --- a/openptv_python/correspondences.py +++ b/openptv_python/correspondences.py @@ -98,16 +98,16 @@ def get_by_pnrs(self, pnrs): def __del__(self): del self.buf + class Correspond: """Correspondence between two points in two cameras.""" def __init__(self): self.p1 = PT_UNUSED self.n = 0 - self.p2 = [0] * MAXCAND - self.corr = [0.0] * MAXCAND - self.dist = [0.0] * MAXCAND - + self.p2 = np.array([0] * MAXCAND) + self.corr = np.array([0.0] * MAXCAND) + self.dist = np.array([0.0] * MAXCAND) def safely_allocate_target_usage_marks( num_cams: int, nmax: int = NMAX @@ -151,7 +151,6 @@ def safely_allocate_adjacency_lists( return lists - def four_camera_matching( corr_list: List[List[List[Correspond]]], base_target_count, diff --git a/openptv_python/multimed.py b/openptv_python/multimed.py index 502adce..1e1e9b9 100644 --- a/openptv_python/multimed.py +++ b/openptv_python/multimed.py @@ -2,8 +2,8 @@ from typing import List, Tuple import numpy as np +from numba import njit -# from numba import njit from .calibration import Calibration, Exterior, Glass from .parameters import ( ControlPar, @@ -12,7 +12,7 @@ ) from .ray_tracing import ray_tracing from .trafo import correct_brown_affine, pixel_to_metric -from .vec_utils import norm, vec_dot, vec_norm, vec_set +from .vec_utils import norm, vec_set def multimed_nlay( @@ -23,8 +23,8 @@ def multimed_nlay( using radial shift from the multimedia model """ radial_shift = multimed_r_nlay(cal, mm, pos) - Xq = cal.ext_par.x0 + (pos[0] - cal.ext_par.x0) * radial_shift - Yq = cal.ext_par.y0 + (pos[1] - cal.ext_par.y0) * radial_shift + Xq = cal.ext_par.x0 + (pos[0] - cal.ext_par.x0) * radial_shift + Yq = cal.ext_par.y0 + (pos[1] - cal.ext_par.y0) * radial_shift return Xq, Yq @@ -59,7 +59,7 @@ def multimed_r_nlay(cal: Calibration, mm: MultimediaPar, pos: np.ndarray) -> flo # @njit def fast_multimed_r_nlay( - nlay:int, + nlay: int, n1: float, n2: np.ndarray, n3: float, @@ -68,7 +68,7 @@ def fast_multimed_r_nlay( y0, z0, pos: np.ndarray - ) -> float: +) -> float: """Calculate the radial shift for the multimedia model. mm = MultimediaPar.to_dict() @@ -92,7 +92,7 @@ def fast_multimed_r_nlay( it = 0 while (rdiff > 0.001 or rdiff < -0.001) and it < n_iter: - zdiff = z0 - Z + zdiff = z0 - Z if zdiff == 0: zdiff = 1.0 beta1 = math.atan(rq / zdiff) @@ -100,7 +100,7 @@ def fast_multimed_r_nlay( beta2[i] = math.asin(math.sin(beta1) * n1 / n2[i]) beta3 = math.asin(math.sin(beta1) * n1 / n3) - rbeta = ( z0 - d[0]) * math.tan(beta1) - zout * math.tan(beta3) + rbeta = (z0 - d[0]) * math.tan(beta1) - zout * math.tan(beta3) for i in range(nlay): rbeta += d[i] * math.tan(beta2[i]) @@ -117,7 +117,7 @@ def fast_multimed_r_nlay( def trans_cam_point( ex: Exterior, mm: MultimediaPar, glass: Glass, pos: np.ndarray -) -> Tuple[np.ndarray, np.ndarray, np.ndarray, float]: +) -> Tuple[np.ndarray, np.ndarray, np.ndarray, np.float64]: """Transform the camera and point coordinates to the glass coordinates. ex = Exterior(x0=ex_x, y0=ex_y, z0=ex_z) @@ -127,15 +127,34 @@ def trans_cam_point( pos_t, cross_p, cross_c = trans_cam_point(ex, mm, glass, pos, ex_t) """ - glass_dir = np.array([glass.vec_x, glass.vec_y, glass.vec_z]) - primary_point = np.array([ex.x0, ex.y0, ex.z0]) + origin = np.array([ex.x0, ex.y0, ex.z0], dtype=np.float64) + glass_dir = np.array([glass.vec_x, glass.vec_y, glass.vec_z], dtype=np.float64) + pos = pos.astype(np.float64) - dist_o_glass = vec_norm(glass_dir) # vector length - dist_cam_glas = ( - np.dot(primary_point, glass_dir) / dist_o_glass - dist_o_glass - mm.d[0] - ) + return fast_trans_cam_point( + origin, mm.d[0], glass_dir, pos) + + +@njit(fastmath=True) +def fast_trans_cam_point( + primary_point: np.ndarray, + d: float, + glass_dir: np.ndarray, + pos: np.ndarray + ) -> Tuple[np.ndarray, np.ndarray, np.ndarray, np.float64]: + """Derive translation of camera point.""" + dist_o_glass = float(np.linalg.norm(glass_dir)) # vector length + if dist_o_glass == 0.0: + dist_o_glass = 1.0 + + dist_cam_glas = primary_point.dot(glass_dir) + dist_cam_glas /= dist_o_glass + dist_cam_glas -= dist_o_glass + dist_cam_glas -= d - dist_point_glass = vec_dot(pos, glass_dir) / dist_o_glass - dist_o_glass + dist_point_glass = pos.dot(glass_dir) + dist_point_glass /= dist_o_glass + dist_point_glass -= dist_o_glass renorm_glass = glass_dir * (dist_cam_glas / dist_o_glass) cross_c = primary_point - renorm_glass @@ -143,12 +162,12 @@ def trans_cam_point( renorm_glass = glass_dir * (dist_point_glass / dist_o_glass) cross_p = pos - renorm_glass - z0 = dist_cam_glas + mm.d[0] + z0 = dist_cam_glas + d - renorm_glass = glass_dir * (mm.d[0] / dist_o_glass) + renorm_glass = glass_dir * (d / float(dist_o_glass)) temp = cross_c - renorm_glass temp = cross_p - temp - pos_t = np.r_[np.linalg.norm(temp), 0, dist_point_glass] + pos_t = np.array([np.linalg.norm(temp), 0, dist_point_glass]) return pos_t, cross_p, cross_c, z0 @@ -197,13 +216,15 @@ def back_trans_point( # If the norm of the vector temp is greater than zero, adjust the position # of the point in the camera coordinate system - if norm_temp > 0: + if norm_temp > 0.0: # type: ignore renorm_temp = temp * (-pos_t[0] / norm_temp) pos = pos - renorm_temp return pos # @njit + + def move_along_ray(glob_z: float, vertex: np.ndarray, direct: np.ndarray) -> np.ndarray: """Move along the ray to the global z plane. @@ -250,7 +271,7 @@ def init_mmlut(vpar: VolumePar, cpar: ControlPar, cal: Calibration) -> Calibrati z_max_t = z_max # intersect with image vertices rays - cal_t = Calibration(mmlut = cal.mmlut) + cal_t = Calibration(mmlut=cal.mmlut) for i in range(2): for j in range(2): @@ -269,7 +290,8 @@ def init_mmlut(vpar: VolumePar, cpar: ControlPar, cal: Calibration) -> Calibrati if xyz_t[2] > z_max_t: z_max_t = xyz_t[2] - R = norm(xyz_t[0] - cal_t.ext_par.x0, xyz_t[1] - cal_t.ext_par.y0, 0) + R = norm(xyz_t[0] - cal_t.ext_par.x0, + xyz_t[1] - cal_t.ext_par.y0, 0) if R > Rmax: Rmax = R @@ -284,7 +306,8 @@ def init_mmlut(vpar: VolumePar, cpar: ControlPar, cal: Calibration) -> Calibrati if xyz_t[2] > z_max_t: z_max_t = xyz_t[2] - R = norm(xyz_t[0] - cal_t.ext_par.x0, xyz_t[1] - cal_t.ext_par.y0, 0) + R = norm(xyz_t[0] - cal_t.ext_par.x0, + xyz_t[1] - cal_t.ext_par.y0, 0) if R > Rmax: Rmax = R @@ -309,7 +332,8 @@ def init_mmlut(vpar: VolumePar, cpar: ControlPar, cal: Calibration) -> Calibrati for i in range(nr): for j in range(nz): - xyz = vec_set(Ri[i] + cal_t.ext_par.x0, cal_t.ext_par.y0, Zi[j]) + xyz = vec_set(Ri[i] + cal_t.ext_par.x0, + cal_t.ext_par.y0, Zi[j]) data.flat[i * nz + j] = multimed_r_nlay(cal_t, cpar.mm, xyz) print("filled mmlut data with {data}") @@ -329,6 +353,8 @@ def get_mmf_from_mmlut(cal: Calibration, pos: np.ndarray) -> float: return fast_get_mmf_from_mmlut(rw, origin, data, nz, nr, pos) # @njit + + def fast_get_mmf_from_mmlut( rw: int, origin: np.ndarray, @@ -336,7 +362,7 @@ def fast_get_mmf_from_mmlut( nz: int, nr: int, pos: np.ndarray - ) -> float: +) -> float: """Get the refractive index of the medium at a given position.""" temp = pos - origin sz = temp[2] / rw diff --git a/openptv_python/track.py b/openptv_python/track.py index 6eaf445..8fa1904 100644 --- a/openptv_python/track.py +++ b/openptv_python/track.py @@ -620,7 +620,7 @@ def sorted_candidates_in_volume( right[cam], up[cam], down[cam], - points[cam * MAX_CANDS :], + points[cam * MAX_CANDS:], run.cpar, ) @@ -698,7 +698,8 @@ def assess_new_position( for cam in range(run.cpar.num_cams): if (targ_pos[cam][0] != COORD_UNUSED) and (targ_pos[cam][1] != COORD_UNUSED): # Convert pixel coordinates to metric coordinates - x, y = pixel_to_metric(targ_pos[cam][0], targ_pos[cam][1], run.cpar) + x, y = pixel_to_metric( + targ_pos[cam][0], targ_pos[cam][1], run.cpar) # Apply additional transformations targ_pos[cam][0], targ_pos[cam][1] = dist_to_flat( @@ -757,7 +758,6 @@ def add_particle(frm: Frame, pos: np.ndarray, cand_inds: np.ndarray) -> None: ref_path_inf.x = vec_copy(pos) ref_path_inf.reset_links() - ref_targets = frm.targets for cam in range(frm.num_cams): @@ -773,7 +773,6 @@ def add_particle(frm: Frame, pos: np.ndarray, cand_inds: np.ndarray) -> None: frm.num_parts += 1 - def track_forward_start(tr: TrackingRun): """Initialize the tracking frame buffer with the first frames. @@ -862,7 +861,8 @@ def trackcorr_c_loop(run_info, step): # Continue to find candidates for the candidates. count2 += 1 mm = 0 - while w[mm].ftnr != TR_UNUSED and len(fb.buf[2].path_info) > w[mm].ftnr: # counter1-loop + # counter1-loop + while w[mm].ftnr != TR_UNUSED and len(fb.buf[2].path_info) > w[mm].ftnr: # search for found corr of current the corr in next_frame with predicted location # found 3D-position @@ -916,14 +916,16 @@ def trackcorr_c_loop(run_info, step): or acc < tpar.dacc / 10 ): dl = ( - vec_diff_norm(X[1], X[3]) + vec_diff_norm(X[4], X[3]) + vec_diff_norm(X[1], X[3]) + + vec_diff_norm(X[4], X[3]) ) / 2 rr = ( dl / run_info.lmax + acc / tpar.dacc + angle / tpar.dangle ) / quali - curr_path_inf.register_link_candidate(rr, w[mm].ftnr) + curr_path_inf.register_link_candidate( + rr, w[mm].ftnr) # print(f"kk {kk}, rr {rr}, w[mm].ftnr {w[mm].ftnr}") kk += 1 # End of searching 2nd-frame candidates. @@ -963,7 +965,8 @@ def trackcorr_c_loop(run_info, step): # print(f"angle {angle}, acc {acc}") if acc < tpar.dacc and angle < tpar.dangle or acc < tpar.dacc / 10: - dl = (vec_diff_norm(X[1], X[3]) + vec_diff_norm(X[4], X[3])) / 2 + dl = (vec_diff_norm(X[1], X[3]) + + vec_diff_norm(X[4], X[3])) / 2 # print(f" dl {dl} ") rr = ( dl / run_info.lmax + acc / tpar.dacc + angle / tpar.dangle @@ -993,7 +996,8 @@ def trackcorr_c_loop(run_info, step): acc < tpar.dacc / 10 ): quali = w[mm].freq - dl = (vec_diff_norm(X[1], X[3]) + vec_diff_norm(X[0], X[1])) / 2 + dl = (vec_diff_norm(X[1], X[3]) + + vec_diff_norm(X[0], X[1])) / 2 rr = ( dl / run_info.lmax + acc / tpar.dacc + angle / tpar.dangle ) / quali @@ -1008,7 +1012,8 @@ def trackcorr_c_loop(run_info, step): # begin of inlist still zero if tpar.add: if curr_path_inf.inlist == 0 and curr_path_inf.prev_frame >= 0: - quali, v2, philf = assess_new_position(X[2], fb.buf[2], run_info) + quali, v2, philf = assess_new_position( + X[2], fb.buf[2], run_info) if quali >= 2: X[3] = vec_copy(X[2]) in_volume = 0 @@ -1029,7 +1034,8 @@ def trackcorr_c_loop(run_info, step): acc < tpar.dacc / 10 ): dl = ( - vec_diff_norm(X[1], X[3]) + vec_diff_norm(X[0], X[1]) + vec_diff_norm(X[1], X[3]) + + vec_diff_norm(X[0], X[1]) ) / 2 rr = ( dl / run_info.lmax @@ -1194,15 +1200,17 @@ def trackback_c(run_info: TrackingRun): or acc < tpar.dacc / 10 ): dl = ( - vec_diff_norm(X[1], X[3]) + vec_diff_norm(X[0], X[1]) - ) / 2 + vec_diff_norm(X[1], X[3]) + + vec_diff_norm(X[0], X[1]) + ) / 2 # type: ignore quali = w[i].freq rr = ( dl / run_info.lmax + acc / tpar.dacc + angle / tpar.dangle ) / quali - curr_path_inf.register_link_candidate(rr, w[i].ftnr) + curr_path_inf.register_link_candidate( + rr, w[i].ftnr) i += 1 @@ -1211,7 +1219,8 @@ def trackback_c(run_info: TrackingRun): # if old wasn't found try to create new particle position from rest if tpar.add: if curr_path_inf.inlist == 0: - quali, v2, philf = assess_new_position(X[2], fb.buf[2], run_info) + quali, v2, philf = assess_new_position( + X[2], fb.buf[2], run_info) if quali >= 2: # vec_copy(X[3], X[2]) in_volume = 0 @@ -1238,7 +1247,7 @@ def trackback_c(run_info: TrackingRun): dl = ( vec_diff_norm(X[1], X[3]) + vec_diff_norm(X[0], X[1]) - ) / 2 + ) / 2 # type: ignore rr = ( dl / run_info.lmax + acc / tpar.dacc @@ -1289,10 +1298,12 @@ def trackback_c(run_info: TrackingRun): ) X[1] = vec_copy(curr_path_inf.x) X[3] = vec_copy(ref_path_inf.x) - X[4] = vec_copy(fb.buf[3].path_info[ref_path_inf.prev_frame].x) + X[4] = vec_copy( + fb.buf[3].path_info[ref_path_inf.prev_frame].x) for j in range(3): - X[5][j] = 0.5 * (5.0 * X[3][j] - 4.0 * X[1][j] + X[0][j]) + X[5][j] = 0.5 * (5.0 * X[3][j] - + 4.0 * X[1][j] + X[0][j]) angle, acc = angle_acc(X[3], X[4], X[5]) diff --git a/openptv_python/vec_utils.py b/openptv_python/vec_utils.py index b401358..36c6be3 100644 --- a/openptv_python/vec_utils.py +++ b/openptv_python/vec_utils.py @@ -5,6 +5,8 @@ # system to decide whether to invest in loop peeling etc. Here we write # the logical structure, and allow optimizing for size as well. +import math + import numpy as np # Define the np.ndarray type as an numpy array of 3 floats @@ -46,39 +48,41 @@ def vec_scalar_mul(vec: np.ndarray, scalar: float) -> np.ndarray: def vec_diff_norm(vec1: np.ndarray, vec2: np.ndarray) -> float: """vec_diff_norm() gives the norm of the difference between two vectors.""" - return float(np.linalg.norm(vec1 - vec2)) + # return np.linalg.norm(vec1 - vec2) + vec = vec1 - vec2 + return math.sqrt(vec[0]**2 + vec[1]**2 + vec[2]**2) def vec_norm(vec: np.ndarray) -> float: """vec_norm() gives the norm of a vector.""" - return float(np.linalg.norm(vec)) + return math.sqrt(vec[0]**2 + vec[1]**2 + vec[2]**2) def vec_dot(vec1: np.ndarray, vec2: np.ndarray) -> float: """vec_dot() gives the dot product of two vectors as lists of floats.""" - val = np.dot(vec1, vec2) - return float(val) + # return np.dot(vec1, vec2) + return float(vec1[0]*vec2[0] + vec1[1]*vec2[1] + vec1[2]*vec2[2]) def vec_cross(vec1: np.ndarray, vec2: np.ndarray) -> np.ndarray: """Cross product of two vectors.""" - return np.cross(vec1, vec2) + # return np.cross(vec1, vec2) + return np.r_[vec1[1]*vec2[2] - vec1[2]*vec2[1], + vec1[2]*vec2[0] - vec1[0]*vec2[2], + vec1[0]*vec2[1] - vec1[1]*vec2[0]] def vec_cmp(vec1: np.ndarray, vec2: np.ndarray, tol: float = 1e-6) -> bool: """vec_cmp() checks whether two vectors are equal within a tolerance.""" return np.allclose(vec1, vec2, atol=tol) - def unit_vector(vec: np.ndarray) -> np.ndarray: - """Create unit vector as a list of floats.""" - normed = vec_norm(vec) - if normed == 0: - normed = 1.0 - out = vec_scalar_mul(vec, 1.0 / normed) - return out - + """Normalize a vector to a unit vector.""" + magnitude = np.linalg.norm(vec) + if magnitude == 0: + return vec # Avoid division by zero for zero vectors + return vec / magnitude -def vec_init(): +def vec_init(length=3) -> np.ndarray: """Initialize a vector to zero.""" - return np.zeros(3, dtype=float) + return np.zeros(length, dtype=float)