def lineSegment(im, x0, x1=None, theta=None, w=2, l=12, H=200, ml=0):
""" Plot half-line into image
>>> lineSegment(np.zeros( (160,240)), [60,40], [70,40], w=10, l=60)
>>> lineSegment(np.zeros( (160,240)), [60,40], theta=np.deg2rad(20), w=10, l=60)
"""
x0 = np.array(x0).flatten()
if x1 is None:
thetar = -theta
dx = l
else:
x1 = np.array(x1).flatten()
theta = x0 - x1
theta = np.arctan2(theta[1], theta[0]) + np.pi
thetar = -theta
dx = np.linalg.norm(x0 - x1)
xx0, yy0 = np.meshgrid(np.arange(im.shape[1]) - x0[0], np.arange(im.shape[0]) - x0[1])
xx0 = xx0.astype(np.float32)
yy0 = yy0.astype(np.float32)
xxr = math.cos(thetar) * xx0 - math.sin(thetar) * yy0
yyr = math.sin(thetar) * xx0 + math.cos(thetar) * yy0
yyr = pgeometry.signedmin(yyr, w / 2.)
data = H * np.cos((np.pi / w) * yyr) * (pgeometry.smoothstep(xxr, ml, 4)) * (1 - pgeometry.smoothstep(xxr, dx, 4))
im += data
return im
def semiLine(im, x0, theta, w, l, H=200, dohalf=True):
""" Plot half-line into image
Args:
im (array)
x0 (array): starting point of semi-line
theta (float): angle in radians
w (float): width
l (float): length of line segment
H (float): intensity of line segment
dohalf (bool): add smoothing?
>>> im=semiLine(np.zeros( (160,240)), [60,40], theta=np.deg2rad(20), w=10, l=60)
>>> plt.imshow(im)
"""
thetar = -theta
xx0, yy0 = np.meshgrid(np.arange(im.shape[1]) - x0[0], np.arange(im.shape[0]) - x0[1])
xx0 = xx0.astype(np.float32)
yy0 = yy0.astype(np.float32)
xxr = math.cos(thetar) * xx0 - math.sin(thetar) * yy0
yyr = math.sin(thetar) * xx0 + math.cos(thetar) * yy0
yyr = pgeometry.signedmin(yyr, w / 2.)
data = H * np.cos((np.pi / w) * yyr)
if dohalf:
data *= (pgeometry.smoothstep(xxr, 0, 10))
im += data
return im
def costFunctionLine(pp,
imx,
istep,
maxshift=12,
verbose=0,
fig=None,
maxangle=np.deg2rad(70),
ksizemv=12,
dthr=8,
dwidth=3,
alldata=None,
px=None):
""" Cost function for line fitting
pp (list or array): line parameters
imx (numpy array): image to fit to
istep (float)
px (array): translational offset to operate from
"""
istepmodel = .5
samplesize = [
int(imx.shape[1] * istep / istepmodel),
int(imx.shape[0] * istep / istepmodel)
]
LW = 2 # [mV]
LL = 15 # [mV]
H = costFunctionLine.scaling0.copy()
H[0, 0] = istep / istepmodel
H[1, 1] = istep / istepmodel
#patch=linetools.sampleImage(im, pp, samplesize, fig=11, clearfig=True, nrsub=1)
dsize = (samplesize[0], samplesize[1])
patch = cv2.warpPerspective(imx.astype(np.float32), H, dsize, None,
(cv2.INTER_LINEAR), cv2.BORDER_CONSTANT, -1)
pm0 = np.array(pp[0:2]).reshape((1, 2)) / istepmodel # [pixel]
if px is None:
pxpatch = [patch.shape[1] / 2, patch.shape[0] / 2]
else:
pxpatch = (float(istep) / istepmodel) * np.array(px)
pm = pm0 + pxpatch
#modelpatch, cdata=createCross(param, samplesize, centermodel=False, istep=istepmodel, verbose=0)
lowv = np.percentile(imx, 1)
highv = np.percentile(imx, 95)
theta = pp[2]
if verbose:
print('costFunctionLine: sample line patch: lowv %.1f, highv %.1f' %
(lowv, highv))
# print(px)
linepatch = lowv + np.zeros((samplesize[1], samplesize[0]))
lineSegment(linepatch,
pm,
theta=pp[2],
w=LW / istepmodel,
l=LL / istepmodel,
H=highv - lowv,
ml=-6 / istepmodel)
#plt.figure(99); plt.clf(); plt.imshow(lineseg, interpolation='nearest'); plt.colorbar()
#plt.figure(99); plt.clf(); plt.imshow(linepatch-lineseg, interpolation='nearest'); plt.colorbar()
#plt.figure(99); plt.clf(); plt.imshow(linepatch, interpolation='nearest'); plt.colorbar()
dd = patch - (linepatch)
cost = np.linalg.norm(dd)
cost0 = cost
if 1:
ddx0 = np.linalg.norm(pm0) # [pixel]
ddx = np.linalg.norm(pm0) # [pixel]
if verbose:
print(
'costFunctionLine: calculate additonal costs: dist %.1f [mV]' %
(ddx * istepmodel))
ddx = pmatlab.smoothstep(ddx, dthr / istepmodel, dwidth / istepmodel)
if verbose >= 2:
print(' ddx: %.3f, thr %.3f' % (ddx, dthr / istepmodel))
cost += 100000 * ddx
#cost = sLimits(cost, plocal, pm, maxshift, maxangle)
if fig is not None:
pmatlab.cfigure(fig)
plt.clf()
plt.imshow(patch, interpolation='nearest')
plt.title('patch: cost %.2f, dist %.1f' % (cost, ddx0 * istep))
plt.colorbar()
pm = pm.flatten()
#plt.plot(pm0.flatten()[0], pm0.flatten()[1], 'dk', markersize=12, label='initial starting point?')
plt.plot(pm[0], pm[1], '.g', markersize=24, label='fitted point')
plt.plot(pxpatch[0],
pxpatch[1],
'.m',
markersize=18,
label='offset for parameters')
qq = np.array(
pm.reshape(2, 1) + (LL / istepmodel) *
pmatlab.rot2D(theta).dot(np.array([[1, -1], [0, 0]])))
plt.plot(qq[0, :], qq[1, :], '--k', markersize=24, linewidth=2)
# print(pm)
plt.axis('image')
# plt.colorbar()
pmatlab.cfigure(fig + 1)
plt.clf()
plt.imshow(linepatch, interpolation='nearest')
plt.title('line patch')
plt.plot(px[0], px[1], '.m', markersize=24)
plt.axis('image')
plt.colorbar()
pmatlab.tilefigs([fig, fig + 1])
if verbose >= 2:
pmatlab.cfigure(fig + 2)
plt.clf()
xx = np.arange(0, 20, .1)
xxstep = istepmodel * pmatlab.smoothstep(
xx / istepmodel, dthr / istepmodel, (1 / dwidth) / istepmodel)
plt.plot(xx, xxstep, '.-b', label='distance step')
plt.xlabel('Distance [mV]')
plt.legend()
if verbose:
print('costFucntion: cost: base %.2f -> final %.2f' % (cost0, cost))
if verbose >= 2:
ww = np.abs(dd).mean(axis=0)
print('costFunction: dd %s ' % ww)
return cost