I am trying to use Gekko to find an optimal solution for a camera lens distortion calibration problem (Undistort). In my case I am interested in solving a simpler problem that is only concerned with determining two or four variables (K1 K2 || K1 K2 CX CY). I was able to use scipy.optimize.minimize to come to some reasonable solutions that I know are within a good range, however, my solution relies on a rather complex invertibility constraint of the final result and I found that this was not possible to implement in scipy. So, I am looking for some help on how to model this invertibility constraint using Gekko.
The invertibility constraint is as follows ( see div_constraint_invertibility() ):
def get_rad_norm_k(dc, w, h):
dmi = get_max_dist(dc=dc, w=w, h=h)
r2 = m.sqrt(dmi) / 2
r2_2 = r2*r2
r2_4 = r2_2*r2_2
denk1 = -12 * r2_2
denk2 = -12 * r2_4
return denk1, denk2
def get_max_dist(dc, h, w):
corner_distances = [(dc[0] - 0)**2 + (dc[1] - 0)**2,
(dc[0] - 0)**2 + (dc[1] - w)**2,
(dc[0] - h)**2 + (dc[1] - w)**2,
(dc[0] - h)**2 + (dc[1] - 0)**2]
mx = corner_distances[0]
for cd in corner_distances[1:]:
mx = m.max3(mx, cd)
return mx
def div_constraint_invertibility(x, ldm):
p1 = x[0]
p2 = x[1]
dc = [ldm.Cx, ldm.Cy]
w = ldm.width
h = ldm.height
denk1, denk2 = get_rad_norm_k(dc=dc, w=w, h=h)
r1sq = get_max_dist(dc=dc, w=w, h=h)
r1p4 = r1sq**2
k1 = (((-p1) / (1 + p1)) + ((16 * p2) / (1 + p2))) / denk1
k2 = (((-4 * p2) / (1 + p2)) + (p1 / (1 + p1))) / denk2
if -2 < r1sq * k1 < 2:
if -1. - r1sq * k1 < r1p4 * k2 < (1. - r1sq * k1) / 3:
return 1 # Passed Check
else:
if r1sq * k1 >= 2:
if -1. - r1sq * k1 < r1p4 * k2 < (-r1p4 * k1**2 / 12):
return 1 # Passed Check
return -1 # Failed Check
The list of values in argument x contains [P1, P2] or [P1, P2, CX, CY] where P1 and P2 are normalized values of K1 and K2. I have tried to implement the methods using Gekko functions, my next step was to try to implement the conditional checks in "div_constraint_invertibility" using m.if3(). Although, I am wondering if there is a better way to format the problem here as I feel that such a nested m.if3() structure is not a very elegant solution and moreover I am not certain what I should actually return from the function (I probably should be returning some function value rather than -1 or 1...).
EDIT 1 Scipy entry point and other contextual code. Here is the scipy minimization approach. The function cumulative_point_to_line_error takes a set of points belonging to an arc extracted from a distorted image and applies the distortion model to determine the line equation fit error after undistorting the image points. An initial starting point looks something like:
dm0 = [-0.0, 0.0, input_image_center_X, input_image_center_Y]
Along with an array of points that correspond to an arc. For example, depending on the image there may be 30 lists containing sets of points relating to the same extracted arc.
I can provide even more code but wanted to avoid dumping a wall of code...let me know and I can dm you it or I can provide more code here too.
def model_estimation(ip, w, h, oc):
ldm = ip.lens_model
dc = [ldm.Cx, ldm.Cy]
prevk1 = ldm.d[1]
if len(ldm.d) != 3:
ldm.d.append(0)
prevk2 = ldm.d[2]
if oc:
dm0 = [prevp1, prevp2, dc[0], dc[1]]
else:
dm0 = [prevp1, prevp2]
cons = []
bnds = []
if oc:
dc_drift = 2
bnds = [(-np.inf, np.inf), (-np.inf, np.inf),
(float(ldm.width / 2 - dc_drift), float(ldm.width / 2 + dc_drift)),
(float(ldm.height / 2 - dc_drift), float(ldm.height / 2 + dc_drift))]
cons.append({'type': 'ineq', 'fun': div_constraint_invertibility, 'args': (ip.lens_model,)})
print(f"Optimizing: {dm0}")
if cons:
print(f"Constraints:")
for con in cons:
print(f"Const: {con['fun'].__name__}")
if bnds:
print(f"Bounds:")
for idx, bnd in enumerate(bnds):
print(f"{bnd[0]} <= x{idx} <= {bnd[1]}")
result = spo.minimize(fun=cumulative_point_to_line_error, x0=np.array(dm0), args=(ip,),
constraints=cons, bounds=bnds, method='SLSQP', options={"disp": True})
print(f"Optimization Result: {result.x} | {result.fun}")
EDIT 2
I gave it a shot trying to convert my objective to one that is useable with Gekko. Is is possible to call m.Minimize on something like 'cumulative_point_to_line_error'? Currently working on how to handle the division by 0 check in the 'div_update_point' method:
def HornerPoly(poly, n, nrmx):
sol = poly[n]
i = n - 1
while i > -1:
sol = sol * nrmx + poly[i]
i -= 1
return sol
def div_update_point(m, px, py, K1, K2, dc_x, dc_y):
center_dist = (px - dc_x)**2 + (py - dc_y)**2
dist_coef = [1, K1, K2]
A = HornerPoly(dist_coef, len(dist_coef) - 1, center_dist)
# m.IF ?
if A == 0:
return px, py
else:
A = 1.0 / A
new_x = dc_x + (px - dc_x) * A
new_y = dc_y + (py - dc_y) * A
return new_x, new_y
def cumulative_point_to_line_error(m, p1, p2, dc_x, dc_y, w, h, houghlines):
n1 = m.Intermediate(((-p1) / (1 + p1)) + (16 * p2 / (1 + p2)))
n2 = m.Intermediate(((-4 * p2) / (1 + p2)) + (p1 / (1 + p1)))
denk1, denk2 = get_rad_norm_k(m, dc_x=dc_x, dc_y=dc_y, w=w, h=h)
cumulative_err = 0
for idx, hl in enumerate(houghlines):
pnts_o = hl.pnts
err = 0
zero = 10e-100
num_pnts = len(pnts_o)
um = vm = 0
suu = suv = svv = 0
pnts_e = []
for pnt in pnts_o:
x, y = div_update_point(px=pnt[0], py=pnt[1], K1=n1/denk1, K2=n2/denk2, dc_x=dc_x, dc_y=dc_y)
pnts_e.append([x, y])
for pnt in pnts_e:
x = pnt[0]
y = pnt[1]
um += x
vm += y
um = um / num_pnts
vm = vm / num_pnts
for pnt in pnts_e:
x = pnt[0]
y = pnt[1]
suu += (x - um)**2
suv += (x - um) * (y - vm)
svv += (y - vm)**2
suu = suu / num_pnts
suv = suv / num_pnts
svv = svv / num_pnts
if np.abs(suv) <= zero:
# print('XY <= ZERO*')
if suu < svv and svv > zero:
hl.a = 1.
hl.b = 0.
hl.c = -um
err = 0.
# print(f'Iter. Method Line: {self.a}x + {self.b}y + {self.c} = 0')
# print(f'Error == {err}')
return hl, err
if svv < suu and suu > zero:
hl.a = 0.
hl.b = 1.
hl.c = -vm
err = 0
# print(f'Iter. Method Line: {self.a}x + {self.b}y + {self.c} = 0')
# print(f'Error == {err}')
return hl, err
# print('Failed to recalculate!')
return None, err
r = np.array([[1, 0, 0],
[1, 0, 0],
[0, 1, 0],
[0, 1, 0]], dtype=float)
h = 0.5 * (suu - svv) / suv
if h > 0:
r[0, 1] = -h - np.sqrt(1. + np.square(h))
r[0, 2] = -(um + r[0, 1] * vm)
r[1, 1] = -1. / r[0, 1]
r[1, 2] = -(um + r[1, 1] * vm)
r[2, 0] = h + np.sqrt(1. + np.square(h))
r[2, 2] = -(r[2, 0] * um + vm)
r[3, 0] = -1. / r[2, 0]
r[3, 2] = -(r[3, 0] * um + vm)
else:
r[0, 1] = -h + np.sqrt(1. + np.square(h))
r[0, 2] = -(um + r[0, 1] * vm)
r[1, 1] = -1. / r[0, 1]
r[1, 2] = -(um + r[1, 1] * vm)
r[2, 0] = h - np.sqrt(1. + np.square(h))
r[2, 2] = -(r[2, 0] * um + vm)
r[3, 0] = -1. / r[2, 0]
r[3, 2] = -(r[3, 0] * um + vm)
for row in range(0, r.shape[0]):
norm = np.sqrt(np.square(r[row, 0]) + np.square(r[row, 1]))
for col in range(0, r.shape[1]):
r[row, col] /= norm
min = 0
k = 0
for pnt in pnts_e:
x = pnt[0]
y = pnt[1]
sol = r[0, 0] * x + r[0, 1] * y + r[0, 2]
min += np.square(sol)
for row in range(1, r.shape[0]):
h = 0
for pnt in pnts_e:
x = pnt[0]
y = pnt[1]
sol = r[row, 0] * x + r[row, 1] * y + r[row, 2]
h += np.square(sol)
if h < min:
k = row
min = h
hl.a = r[k, 0]
hl.b = r[k, 1]
hl.c = r[k, 2]
# err = min
# Update ip TODO: edit houghline list?
houghlines[idx] = hl
cumulative_err += min
return cumulative_err
No comments:
Post a Comment