Skip to content

Commit

Permalink
Calibration with scipy (#74)
Browse files Browse the repository at this point in the history 
* removed obsolete text

* added the scipy part

* added scipy optimizer for k1,k2,k3,p1,p2, affine
  • Loading branch information
@alexlib
alexlib authored Nov 28, 2024
1 parent 2178c21 commit 1ed7478
Show file tree
Hide file tree
Showing 2 changed files with 137 additions and 135 deletions.
5 changes: 3 additions & 2 deletions pyproject.toml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -4,7 +4,7 @@ build-backend = "setuptools.build_meta"

[project]
name = "pyptv"
version = "0.2.6"
version = "0.2.7"
description = "Python GUI for the OpenPTV library `liboptv`"
authors = [
{ name = "Alex Liberzon", email = "[email protected]" }
Expand All @@ -29,7 +29,8 @@ dependencies = [
"tables",
"pyparsing",
"tqdm",
"matplotlib"
"matplotlib",
"scipy"
]

[project.urls]
Expand Down
267 changes: 134 additions & 133 deletions pyptv/calibration_gui.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -36,7 +36,6 @@
from pyptv.quiverplot import QuiverPlot



from optv.imgcoord import image_coordinates
from optv.transforms import convert_arr_metric_to_pixel
from optv.orientation import match_detection_to_ref
Expand All @@ -49,6 +48,9 @@

from scipy.optimize import minimize

# recognized names for the flags:
NAMES = ["cc", "xh", "yh", "k1", "k2", "k3", "p1", "p2", "scale", "shear"]

# -------------------------------------------
class ClickerTool(ImageInspectorTool):
left_changed = Int(1)
Expand Down Expand Up @@ -769,28 +771,6 @@ def _button_sort_grid_fired(self):
self.status_text = "Sort grid finished."
self.pass_sortgrid = True

# def _button_sort_grid_init_fired(self):
# """ TODO: Not implemented yet """
# if self.need_reset:
# self.reset_show_images()
# self.need_reset = 0
#
#
# ptv.py_calibration(14)
# x = []
# y = []
# x1_cyan = []
# y1_cyan = []
# pnr = []
# ptv.py_get_from_sortgrid(x, y, pnr)
# self.drawcross("sort_x_init", "sort_y_init", x, y, "white", 4)
# ptv.py_get_from_calib(x1_cyan, y1_cyan)
# self.drawcross("init_x", "init_y", x1_cyan, y1_cyan, "cyan", 4)
# for i in range(len(self.camera)):
# self.camera[i]._plot.overlays = []
# self.camera[i].plot_num_overlay(x[i], y[i], pnr[i])
# self.status_text = "Sort grid initial guess finished."

def _button_raw_orient_fired(self):
"""
update the external calibration with results of raw orientation, i.e.
Expand Down Expand Up @@ -856,37 +836,26 @@ def _button_fine_orient_fired(self):
op = par.OrientParams()
op.read()

# recognized names for the flags:
names = [
"cc",
"xh",
"yh",
"k1",
"k2",
"k3",
"p1",
"p2",
"scale",
"shear",
]
op_names = [
op.cc,
op.xh,
op.yh,
op.k1,
op.k2,
op.k3,
op.p1,
op.p2,
op.scale,
op.shear,
]

# set flags for cc, xh, yh only
flags = []
for name, op_name in zip(names[:3], op_names[:3]):
if op_name == 1:
flags.append(name)
flags = [name for name in NAMES if getattr(op, name) == 1]

# op_names = [
# op.cc,
# op.xh,
# op.yh,
# op.k1,
# op.k2,
# op.k3,
# op.p1,
# op.p2,
# op.scale,
# op.shear,
# ]

# # set flags for cc, xh, yh only
# flags = []
# for name, op_name in zip(names[:3], op_names[:3]):
# if op_name == 1:
# flags.append(name)

for i_cam in range(self.n_cams): # iterate over all cameras

Expand Down Expand Up @@ -986,87 +955,94 @@ def _button_fine_orient_fired(self):
else:
targs = self.sorted_targs[i_cam]

# try:
print(f"First calibrate only external and flags: {flags} \n")
residuals, targ_ix, err_est = full_calibration(
self.cals[i_cam],
self.cal_points["pos"],
targs,
self.cpar,
flags,
)

# # this chunk optimizes for radial distortion
# if np.any(op_names[3:6]):
# sol = minimize(self._residuals_k,
# self.cals[i_cam].get_radial_distortion(),
# args=(self.cals[i_cam],
# all_known,
# all_detected,
# self.cpar
# ),
# method='Nelder-Mead',
# tol=1e-11,
# options={'disp':True},
# )
# radial = sol.x
# self.cals[i_cam].set_radial_distortion(radial)
# else:
# radial = self.cals[i_cam].get_radial_distortion()

# if np.any(op_names[5:8]):
# # now decentering
# sol = minimize(self._residuals_p,
# self.cals[i_cam].get_decentering(),
# args=(self.cals[i_cam],
# all_known,
# all_detected,
# self.cpar
# ),
# method='Nelder-Mead',
# tol=1e-11,
# options={'disp':True},
# )
# decentering = sol.x
# self.cals[i_cam].set_decentering(decentering)
# else:
# decentering = self.cals[i_cam].get_decentering()

# if np.any(op_names[8:]):
# # now affine
# sol = minimize(self._residuals_s,
# self.cals[i_cam].get_affine(),
# args=(self.cals[i_cam],
# all_known,
# all_detected,
# self.cpar
# ),
# method='Nelder-Mead',
# tol=1e-11,
# options={'disp':True},
# )
# affine = sol.x
# self.cals[i_cam].set_affine_trans(affine)

# else:
# affine = self.cals[i_cam].get_affine()
# end of multiplane calibration loop that combines planes

try:
print(f"Calibrating external (6DOF) and flags: {flags} \n")
residuals, targ_ix, err_est = full_calibration(
self.cals[i_cam],
self.cal_points["pos"],
targs,
self.cpar,
flags,
)
except BaseException:
print("Error in OPTV full_calibration, attempting Scipy")
# raise

# this chunk optimizes for radial distortion

if any(flag in flags for flag in ['k1', 'k2', 'k3']):
sol = minimize(self._residuals_k,
self.cals[i_cam].get_radial_distortion(),
args=(self.cals[i_cam],
self.cal_points["pos"],
targs,
self.cpar
),
method='Nelder-Mead',
tol=1e-11,
options={'disp':True},
)
radial = sol.x
self.cals[i_cam].set_radial_distortion(radial)
else:
radial = self.cals[i_cam].get_radial_distortion()

if any(flag in flags for flag in ['p1', 'p2']):
# now decentering
sol = minimize(self._residuals_p,
self.cals[i_cam].get_decentering(),
args=(self.cals[i_cam],
self.cal_points["pos"],
targs,
self.cpar
),
method='Nelder-Mead',
tol=1e-11,
options={'disp':True},
)
decentering = sol.x
self.cals[i_cam].set_decentering(decentering)
else:
decentering = self.cals[i_cam].get_decentering()

if any(flag in flags for flag in ['scale', 'shear']):
# now affine
sol = minimize(self._residuals_s,
self.cals[i_cam].get_affine(),
args=(self.cals[i_cam],
self.cal_points["pos"],
targs,
self.cpar
),
method='Nelder-Mead',
tol=1e-11,
options={'disp':True},
)
affine = sol.x
self.cals[i_cam].set_affine_trans(affine)

else:
affine = self.cals[i_cam].get_affine()


# # Now project and estimate full residuals
# self._project_cal_points(i_cam)

# residuals = self._residuals_combined(
# np.r_[radial, decentering, affine],
# self.cals[i_cam],
# all_known,
# all_detected,
# self.cpar
# )
# Now project and estimate full residuals
self._project_cal_points(i_cam)

# residuals /= 100
residuals = self._residuals_combined(
np.r_[radial, decentering, affine],
self.cals[i_cam],
self.cal_points["pos"],
targs,
self.cpar
)

# targ_ix = np.arange(len(all_detected))
residuals /= 100

targ_ix = [t.pnr() for t in targs if t.pnr() != -999]
# targ_ix = np.arange(len(all_detected))

# save the results from self.cals[i_cam]
self._write_ori(i_cam, addpar_flag=True)
Expand Down Expand Up @@ -1132,6 +1108,14 @@ def _residuals_k(self, x, cal, XYZ, xy, cpar):
xy (array-like): 2D image coordinates.
cpar (CPar): Camera parameters.
args=(self.cals[i_cam],
self.cal_points["pos"],
targs,
self.cpar
)
Returns:
residuals: Distortion in pixels
"""
Expand All @@ -1141,7 +1125,9 @@ def _residuals_k(self, x, cal, XYZ, xy, cpar):
image_coordinates(XYZ, cal, cpar.get_multimedia_params()),
cpar
)
residuals = xy[:,1:] - targets
xyt = np.array([t.pos() if t.pnr() != -999 else [np.nan, np.nan] for t in xy])
residuals = np.nan_to_num(xyt - targets)
# residuals = xy[:,1:] - targets
return np.sum(residuals**2)

def _residuals_p(self, x, cal, XYZ, xy, cpar):
Expand All @@ -1151,7 +1137,8 @@ def _residuals_p(self, x, cal, XYZ, xy, cpar):
image_coordinates(XYZ, cal, cpar.get_multimedia_params()),
cpar
)
residuals = xy[:,1:] - targets
xyt = np.array([t.pos() if t.pnr() != -999 else [np.nan, np.nan] for t in xy])
residuals = np.nan_to_num(xyt - targets)
return np.sum(residuals**2)

def _residuals_s(self, x, cal, XYZ, xy, cpar):
Expand All @@ -1161,7 +1148,8 @@ def _residuals_s(self, x, cal, XYZ, xy, cpar):
image_coordinates(XYZ, cal, cpar.get_multimedia_params()),
cpar
)
residuals = xy[:,1:] - targets
xyt = np.array([t.pos() if t.pnr() != -999 else [np.nan, np.nan] for t in xy])
residuals = np.nan_to_num(xyt - targets)
return np.sum(residuals**2)

def _residuals_combined(self, x, cal, XYZ, xy, cpar):
Expand All @@ -1175,7 +1163,8 @@ def _residuals_combined(self, x, cal, XYZ, xy, cpar):
image_coordinates(XYZ, cal, cpar.get_multimedia_params()),
cpar
)
residuals = xy[:,1:] - targets
xyt = np.array([t.pos() if t.pnr() != -999 else [np.nan, np.nan] for t in xy])
residuals = np.nan_to_num(xyt - targets)
return residuals

def _write_ori(self, i_cam, addpar_flag=False):
Expand All @@ -1184,6 +1173,18 @@ def _write_ori(self, i_cam, addpar_flag=False):
addpar_flag is a boolean that allows to keep previous addpar
otherwise external_calibration overwrites zeros
"""
# protect ORI files from NaNs
# Check for NaNs in self.cals[i_cam]
tmp = np.array([
self.cals[i_cam].get_pos(),
self.cals[i_cam].get_angles(),
self.cals[i_cam].get_affine(),
self.cals[i_cam].get_decentering(),
self.cals[i_cam].get_radial_distortion(),
],dtype=object)

if np.any(np.isnan(np.hstack(tmp))):
raise ValueError(f"Calibration parameters for camera {i_cam} contain NaNs. Aborting write operation.")

ori = self.calParams.img_ori[i_cam]
if addpar_flag:
Expand Down

0 comments on commit 1ed7478

Please sign in to comment.