def InstSpeed(x0, y0, x1, y1, imc, imn, spd, spdv, nP_nc, scd):
"""# instantaneous speed estimation"""
#select a single feature based on difference along ep, position and current speed
d_eig = 0.5*difCrop(x0, y0, x1, y1, imc, imn, nP_nc)
dist = np.sqrt((y0-385)**2 + ((x0-600)**2) /6)
d_len = 2*turnSig(dist, 80, 40)
sd = SpdDif(x1, y1, x0, y0, spd, nP_nc, scd)
d_spd = 0.5*turnSig(sd, spdv, spdv/2)
minIdx=np.argmin(d_len+d_eig + d_spd)
zpl = get_z_plane(x0[minIdx], y0[minIdx], scd)
m = [minIdx]
ztr = t3d.GetZ(x0[m], y0[m], x1[m], y1[m], t3d.GetTfromPar(nP_nc), ktt.KI)[0]
inst = nP_nc[5]*(zpl/ztr)
# ...
if i >2:
ztr2 = t3d.GetZ(xc[m], yc[m], xp[m], yp[m], t3d.Invert(t3d.GetTfromPar(p0)), ktt.KI)[0]
inst2 = nP_nc[5]*(zpl/ztr2)
inst = (inst + inst2)/2
if ((x0[m]-x1[m])**2 +(y0[m]-y1[m])**2)<0.3**2:
inst = 0
d = np.min(d_len+d_eig + d_spd)
f = 1
return inst, d, f
def difCrop(vxc, vyc, vxn, vyn, imc, imn, nP_nc):
"""absolute differnce (SAD) along epipolar line"""
# window size and ground points
wsz=6
gd = (((vxc-600)**2)/4 + (vyc-375)**2) < (160*160)
gd = gd*(vyc<imc.shape[0]-wsz/2-2)
if gd.sum() == 0:
return np.zeros(len(vxc))
# project points with different depth to find position along ep. and normalize
zc = t3d.GetZ(vxc, vyc, vxn, vyn, t3d.GetTfromPar(nP_nc), ktt.KI)
xpr, ypr = t3d.ProjectImgPoints(vxc, vyc, zc, t3d.GetTfromPar(nP_nc), ktt.KI, ktt.K)
xqr, yqr = t3d.ProjectImgPoints(vxc, vyc, zc*0.8, t3d.GetTfromPar(nP_nc), ktt.KI, ktt.K)
d = np.sqrt((xpr-xqr)**2+(yqr-ypr)**2+0.1)/2
dx, dy = (xpr-xqr)/d, (ypr-yqr)/d
xqr, yqr = xpr-dx, ypr-dy
# blur images (smooth borders) and calculate SAD
imb = cv2.blur(imn, (6,6))
cd = np.zeros(np.sum(gd))
for k, (x0, y0, x1, y1) in enumerate(zip(xpr[gd], ypr[gd], xqr[gd], yqr[gd])):
w0 = cv2.getRectSubPix(imb, (wsz, wsz), (x0, y0))
w1 = cv2.getRectSubPix(imb, (wsz, wsz), (x1, y1))
cd[k] = np.sum(np.abs((w0).astype(np.int)-w1.astype(np.int)))
# return normilized difference (sigmoid used for smoothing and scaling)
cd = cd/(wsz*wsz)
cd = 1-turnSig(cd, 8, 4)
ret_ = np.zeros(len(vxc))
ret_[gd] = cd
#t3d.PrintImgPtsDelta(imb, xpr, ypr, xqr, yqr, ret_/800, idx=START+i, outPath='C:/tmp/multiView/im_%04d.png')
#t3d.PrintImgPtsDelta(imb, xpr[gd], ypr[gd], xqr[gd], yqr[gd], cd, idx=START+i, outPath='C:/tmp/multiView/im_%04d.png')
return ret_
def MarkBad(i_c, i_p, xp, yp, xc, yc, xn, yn, p_nc, p_cp, bl, mr):
"""mark as bad features when reprojection error is high"""
if 1:
its = np.intersect1d(i_c, i_p)
pp = np.array([(p in its) for p in i_p], dtype=np.bool)
pc = np.array([(p in its) for p in i_c], dtype=np.bool)
xp, yp, xc, yc, xn, yn = xp[pp], yp[pp], xc[pc], yc[pc], xn[pc], yn[pc]
ic = i_c[pc]
t0 = t3d.Invert(ktt.poses[START+i+1].reshape(3, 4))
t1 = ktt.poses[START+i].reshape(3, 4)
t = t3d.Compose(t0, t1)
zc = t3d.GetZ(xc, yc, xn, yn, t3d.GetTfromPar(p_nc), ktt.KI)
xpr, ypr = t3d.ProjectImgPoints(xc, yc, zc, t, ktt.KI, ktt.K)
d = np.sqrt((xpr-xn)**2+(ypr-yn)**2)
bad1 = np.zeros(len(xc), dtype=np.bool)
vzg = get_z_plane(xc, yc, 175)
bad1 = (yc>220)*(zc>(vzg*1.25))
if abs(p_nc[5])>1.8:
bad1 =bad1 + (yc>220)*(zc<(vzg/1.5))
if mr<0.5 and p_nc[5]<-1.8 and abs(p_nc[1]) < 0.04:
da = p_nc[1]*ktt.K[0,0]
xnn = xn - da
bad1 = (xc<550)*(xc<(xnn-3))+bad1
bad1 = (xc>650)*(xc>(xnn+3))+bad1
if mr>0.4 and False:
bad1 = bad1 + (d>3)*(zc>70)
bad1 = bad1 + (d>1)*(zc<3)
bl = np.hstack((bl, (ic[bad1])))
bl = np.intersect1d(bl, ic)
return bl
def PosesToWorld(poses):
"""change reference poses from position (n-1)->(n) to (0)->(n)"""
wposes = np.zeros((len(poses) + 1, 6))
t0 = np.eye(3, 4)
for i, p in enumerate(poses):
wposes[i] = t3d.GetParFromT(t0)
t0 = t3d.Compose(t0, t3d.Invert(t3d.GetTfromPar(p)))
wposes[-1] = t3d.GetParFromT(t0)
return wposes
def GetInitSpd(start, init, scd):
"""initial speed estimation"""
nP_nc = np.zeros(6)
imp, imc, imn = ktt.GetImg(start), ktt.GetImg(start+1), ktt.GetImg(start+2)
mrLst, spdLst, pLst = np.zeros(3), np.zeros(3), np.zeros((3,6))
for spd in [-1, -0.5, -2]:
nP_nc[5]=spd
pp, pc, pn = GetPtsTwoDepths(imp, imc, imn, t3d.GetTfromPar(nP_nc), scd)
(xp, yp), (xc, yc), (xn, yn), idc, i_p = pp.T, pc.T, pn.T, np.arange(len(pp), dtype=np.int), np.arange(len(pp), dtype=np.int)
p0, nP_nc, mr, cy, fac = BackBa3(idc, i_p, xp, yp, xc, yc, xn, yn, spd=spd, ncyc=max(cyc, 5), rep_thresh=0.25, P_cp=nP_nc, retAll=True, bl=bl)
inst, dSpd, f = InstSpeed(xc, yc, xn, yn, imc, imn, spd, 3, nP_nc, scd)
mrLst[i], spdLst[i], pLst[i] = mr, inst, p0
return spdLst[np.argmin(spdLst)], pLst[np.argmin(spdLst)]
def BackBa3(i_c, i_p, xp, yp, xc, yc, xn, yn, spd=None, ncyc=2, rep_thresh=0.25, P_cp=np.zeros(6), retAll=False, bl=None):
"""cyclic camera pose estimation algorithm:"""
# invert the last estimation (n-1)->(n) and stop if median projection error below mr0_brake
T_pc = t3d.Invert(t3d.GetTfromPar(P_cp))
mr0_brake = 0.15
for j in range(ncyc):
if spd is not None:
T_pc[2, 3] = -1*spd
# calculate depth using poses (n)->(n-1), and use to calculate pose (n-1)->(n+1)
vzp = t3d.GetZ(xp, yp, xc, yc, t3d.Invert(T_pc), ktt.KI)
mr0, P_np, T_np = LocateFarClose(xp, yp, vzp, xn, yn, mr0_lim=0.0, maxTry = RANSAC_TRY)
# calculate depth using poses (n-1)->(n+1), and use to calculate pose (n+1)->(n)
vzn = t3d.GetZ(xn, yn, xp, yp, t3d.Invert(T_np), ktt.KI)
mr1, P_cn, T_cn = LocateFarClose(xn, yn, vzn, xc, yc, mr0_lim=0.0, maxTry = RANSAC_TRY)
if (j == ncyc-1 or mr1<((j*0.075)+0.125)) and retAll==False:
break
# calculate depth using poses (n+1)->(n), and use to calculate pose (n)->(n-1)
vzc = t3d.GetZ(xc, yc, xn, yn, t3d.Invert(T_cn), ktt.KI)
mr2, P_pc, T_pc = LocateFarClose(xc, yc, vzc, xp, yp, mr0_lim=0.0, maxTry = RANSAC_TRY)
mr0_brake +=0.04
if mr1<((j*0.05)+0.12):
break
T_nc = t3d.Invert(T_cn)
f = 1#FixTz(xp, yp, xc, yc, xn, yn, P_pc, t3d.GetParFromT(T_nc))
if retAll:
if spd is not None:
T_pc[2, 3] = -1*spd
T_cp = t3d.Invert(T_pc)
return t3d.GetParFromT(T_cp), t3d.GetParFromT(T_nc), mr1, j, f
return t3d.GetParFromT(T_nc), mr1, j
spd, nP_nc, = GetInitSpd(START, 0, scd)
spdv = 0.3
xc, yc, xn, yn, idc = np.array([], dtype=np.float32), np.array([], dtype=np.float32), np.array([]), np.array([]), np.array([])
xp, yp, i_p = np.array([], dtype=np.float32), np.array([], dtype=np.float32), np.array([], dtype=np.float32)
imc, imn = ktt.GetImg(START), ktt.GetImg(START+1)
t_init, tc = time.time(), time.time()
i = 0
for j in range(1, NFRAMES):
i = i+1
# images (previous[n-1], current[n] and next [n+1]); P_nc is the camera pose
# from current->next positions. (xc, yc) are feature coordinates for current image
imp, imc, imn = imc, imn, ktt.GetImg(START+i+1)
nP_nc[5]=spd
pp, pc, pn = GetPtsTwoDepths(imp, imc, imn, t3d.GetTfromPar(nP_nc), scd)
(xp, yp), (xc, yc), (xn, yn), idc, i_p = pp.T, pc.T, pn.T, np.arange(len(pp), dtype=np.int), np.arange(len(pp), dtype=np.int)
#if DetectStop(xc, yc, xn, yn, spd):
# assert 0
# after feature tracking perform pose estimation
if len(xp > 10):
if i==1:
p0, nP_nc, mr, cy, fac = BackBa3(idc, i_p, xp, yp, xc, yc, xn, yn, spd=spd, ncyc=max(cyc, 5), rep_thresh=0.25, P_cp=nP_nc, retAll=True, bl=bl)
else:
p0, nP_nc, mr, cy, fac = BackBa3(idc, i_p, xp, yp, xc, yc, xn, yn, spd=spd, ncyc=cyc, rep_thresh=0.25, P_cp=nP_nc, retAll=True, bl=bl)
if i>2:
nP_nc = fixP(nP_nc, ep[START+i-1], spd)
scd = UpdateScaleDelta(nP_nc, scd)