Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Test optimize calibration #5

Merged
merged 3 commits into from
Mar 2, 2024
+6,239 −140
Merged

Test optimize calibration #5

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
04bfaa8
Create test_calibration_optimize.ipynb
alexlib Feb 29, 2024
a328447
Update test_calibration_optimize.ipynb
alexlib Feb 29, 2024
d843745
changed ap52, TargetArray and now we can compare optimization
alexlib Mar 2, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
50 changes: 30 additions & 20 deletions openptv_python/calibration.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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),
Expand Down Expand Up @@ -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}")
Expand Down Expand Up @@ -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):
Expand All @@ -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:
"""
Expand All @@ -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:
"""
Expand All @@ -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):
"""
Expand All @@ -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."""
Expand Down
107 changes: 58 additions & 49 deletions openptv_python/orientation.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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

Expand Down Expand Up @@ -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]
Expand Down Expand Up @@ -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
Expand Down Expand Up @@ -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):
Expand Down Expand Up @@ -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]
Expand Down Expand Up @@ -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

Expand Down Expand Up @@ -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,
Expand Down Expand Up @@ -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)

Expand All @@ -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

Expand All @@ -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]:
Expand Down
38 changes: 19 additions & 19 deletions openptv_python/tracking_frame_buf.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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]:
Expand Down
Loading
Loading