def compH_dtn(qray,dray,ddep,constants):
# set variables
wRq, wRd, qYaw, nYaw = constants
# prm = np.array([0,0,0,0,1])
# prm = lsqH_dtn(prm,qray,dray,ddep,constants)
# valid = (errH_dtn(prm,qray,dray,ddep,constants)<.001).all()
# return prm, valid
xd, yq = dray, qray
xw = tp(np.dot(wRd,tp(xd)))
yw = tp(np.dot(wRq,tp(yq)))
z = np.cross(yw,xw)
# # compute homography parameters
t = geom.normalrows(np.cross(z[0,:],z[1,:])) # homography translation
w = np.cross(yw,t)
maxidx = np.argmax(w,1)
b = z[[0,1],maxidx]/w[[0,1],maxidx]
ka_init = np.array([0,np.pi+np.mean(np.arctan2(xw[:,0],xw[:,2]))])
errf = lambda prm,argb,argx: argb+prm[0]*(argx[:,0]*np.sin(prm[1])+argx[:,2]*np.cos(prm[1]))
k, a = tuple( opt.leastsq(errf,ka_init,args=(b,xw),warning=False)[0] )
t = k*t
if np.mean(np.inner(xw-yw,t)) < 0: t, a = -t, a+np.pi
dep = np.mean(ddep*np.inner(xw,[-np.sin(a),0,-np.cos(a)]))
prm = np.append(t,[180/np.pi*a,dep])
valid = (errH_dtn(prm,qray,dray,ddep,constants)<.01).all()
return prm, valid
def compH_dtq(qray,dray,ddep,constants):
# set variables
Rpr, wRd, qYaw, nYaw = constants
pr = geom.YPRfromR(Rpr)[1:] # pitch and roll
dRq = np.dot(tp(wRd),geom.RfromYPR(qYaw,pr[0],pr[1]))
xd, yq = dray, qray
yd = tp(np.dot(dRq,tp(yq)))
xw = tp(np.dot(wRd,tp(xd)))
tn = np.cross(yd,xd)
n = nYaw * np.pi/180 # homography normal bearing
# compute homography parameters based off guessed yaw
t = geom.normalrows(np.cross(tn[0,:],tn[1,:])) # homography translation
m = geom.vecnorm(tn)/(geom.vecnorm(np.cross(yd,t))*geom.vecnorm(xw[:,[0,2]]))
f = np.arctan2(xw[:,0],xw[:,2])
k = np.mean( m / np.cos(n-f) )
valid = np.std( m/(k*np.cos(n-f)) ) < 0.1
fe = np.mod(n-np.mean(f),2*np.pi)
if np.abs(fe) < np.pi/2: n = np.mod(n+np.pi,2*np.pi)
if np.mean(np.inner(xd-yd,t)) < 0: t = -t
# compute plane depth
nd = -np.dot(tp(wRd),[np.sin(n),0,np.cos(n)])
dep = ddep*np.inner(dray,nd)
pd = np.mean(dep)
valid = valid and np.std(dep/pd) < 0.1
# set parameters and refine
prm = np.append(np.abs(k)*np.dot(wRd,t),[qYaw,pd])
if valid: prm = lsqH_dtq(prm,qray,dray,ddep,constants)
valid = valid and geom.vecnorm(prm[:3]) < 5
return prm, valid
def esserrf_tq(prm, qray, dray, pr, wRd, domidx):
# set variables
dRq = np.dot(tp(wRd), geom.RfromYPR(prm[2], pr[0], pr[1]))
td = np.dot(tp(wRd), geom.normalrows(np.insert(prm[:2], domidx, 1)))
E = np.dot(tp(dRq), geom.xprodmat(td))
# Compute homography error
return np.sum(qray * tp(np.dot(E, tp(dray))), 1)
def LfromLSD(path, img, Kcal):
# load lines; if not already generated, run LSD
if not os.path.isdir(os.path.dirname(path)): os.path.mkdir(os.path.dirname(path))
if not os.path.isfile(path):
callLSD(path, img)
lines = loadLines(path)
# map the line segment endpoints to the image frame
nlines = lines.shape[0]
Kinv = alg.inv(Kcal)
end1 = tp( np.dot( Kinv , np.concatenate( ([lines[:,0]],[lines[:,1]],[np.ones(nlines)]) , 0 ) ) )
end2 = tp( np.dot( Kinv , np.concatenate( ([lines[:,2]],[lines[:,3]],[np.ones(nlines)]) , 0 ) ) )
# convert to midpoints, equations, and lengths
lineqs = np.zeros((nlines,3))
lineqs[:,0] , lineqs[:,1] = end2[:,1]-end1[:,1] , end1[:,0]-end2[:,0]
lineqs[:,2] = -np.sum(lineqs*end1,1)
lineqs = geom.normalrows(lineqs)
lengths = geom.vecnorm(end1-end2)
midpts = (end1+end2)/2.0
# remove lines that are too vertical
mask = np.abs(lineqs[:,1]/lineqs[:,0]) > np.tan(10*np.pi/180)
midpts, lineqs, lengths = midpts[mask,:], lineqs[mask,:], lengths[mask]
return midpts, lineqs, lengths
def esserrf_tq(prm,qray,dray,pr,wRd,domidx):
# set variables
dRq = np.dot(tp(wRd),geom.RfromYPR(prm[2],pr[0],pr[1]))
td = np.dot(tp(wRd),geom.normalrows(np.insert(prm[:2],domidx,1)))
E = np.dot(tp(dRq),geom.xprodmat(td))
# Compute homography error
return np.sum( qray * tp(np.dot(E,tp(dray))) , 1 )
def compH_dtqn(qray,dray,ddep,constants):
# set variables
Rpr, wRd, qYaw, nYaw = constants
pr = geom.YPRfromR(Rpr)[1:] # pitch and roll
dRq = np.dot(tp(wRd),geom.RfromYPR(qYaw,pr[0],pr[1]))
xd, yq = dray, qray
yd = tp(np.dot(dRq,tp(yq)))
xw = tp(np.dot(wRd,tp(xd)))
tn = np.cross(yd,xd) # no renormalization to bias more confident planes
# compute homography parameters
teig = alg.eig(np.dot(tp(tn),tn))
nullidx = np.argmin(teig[0])
valid = teig[0][nullidx] < 1e-2
t = geom.normalrows(teig[1][:,nullidx]) # homography translation
m = geom.vecnorm(tn)/(geom.vecnorm(np.cross(yd,t))*geom.vecnorm(xw[:,[0,2]]))
f = np.arctan2(xw[:,0],xw[:,2])
errf = lambda prm,argm,argf: prm[0]-argm/np.cos(prm[1]-argf)
kn_init = np.array([1.2*np.mean(m),np.mean(f)])
k, n = tuple( opt.leastsq(errf,kn_init,args=(m,f),warning=False)[0] )
valid = valid and np.std( m/(k*np.cos(n-f)) ) < 0.1
fe = np.mod(n-np.mean(f),2*np.pi)
if np.abs(fe) < np.pi/2: n = np.mod(n+np.pi,2*np.pi)
if np.mean(np.inner(xd-yd,t)) < 0: t = -t
# compute plane depth
nd = -np.dot(tp(wRd),[np.sin(n),0,np.cos(n)])
dep = ddep*np.inner(dray,nd)
pd = np.mean(dep)
valid = valid and np.std(dep/pd) < 0.1
# set parameters and refine
prm = np.append(np.abs(k)*np.dot(wRd,t),[qYaw,180/np.pi*n,pd])
if valid: prm = lsqH_dtqn(prm,qray,dray,ddep,constants)
valid = valid and geom.vecnorm(prm[:3]) < 5
return prm, valid
def compH_dt(qray,dray,ddep,constants):
# set variables
wRq, wRd, qYaw, nYaw = constants
# prm = np.array([0,0,0,1])
# prm = lsqH_dt(prm,qray,dray,ddep,constants)
# valid = (errH_dt(prm,qray,dray,ddep,constants)<.001).all()
# return prm, valid
xd, yq = dray, qray
xw = tp(np.dot(wRd,tp(xd)))
yw = tp(np.dot(wRq,tp(yq)))
z = np.cross(yw,xw)
a = nYaw * np.pi/180 # homography normal bearing
# # compute homography parameters
t = geom.normalrows(np.cross(z[0,:],z[1,:])) # homography translation
w = np.cross(yw,t)
maxidx = np.argmax(w,1)
b = z[[0,1],maxidx]/w[[0,1],maxidx]
# b = np.mean(z/w,1)
k = np.mean(-b/(xw[:,0]*np.sin(a)+xw[:,2]*np.cos(a)))
t = k*t
if np.mean(np.inner(xw-yw,t)) < 0: t, a = -t, a+np.pi
dep = np.mean(ddep*np.inner(xw,[-np.sin(a),0,-np.cos(a)]))
prm = np.append(t,dep)
valid = (errH_dt(prm,qray,dray,ddep,constants)<.01).all()
return prm, valid
def planefrom3d(C, Q, dbmatch, domplane, Kdinv, wRd):
if domplane == -1: return np.nan * np.zeros(5)
# get 3d points on plane
planes = np.asarray(
Image.open(os.path.join(C.hiresdir, dbmatch + '-planes.png')))
depths = np.asarray(
Image.open(os.path.join(C.hiresdir, dbmatch + '-depth.png')))
y, x = np.nonzero(planes == domplane)
npts = len(x)
pray = geom.normalrows(
tp(
np.dot(
wRd,
np.dot(Kdinv, np.concatenate(([x], [y], [np.ones(npts)]),
0)))))
pdep = depths[y, x] / 100.0
p3d = np.append(geom.vecmul(pray, pdep),
tp(np.array([np.ones(len(pray))])), 1)
xz_pts = p3d[:, [0, 2, 3]]
# RANSAC solve
threshold, g = 2, np.array([0, 1, 0]) # meters
bprm, bnumi, bmask = np.zeros(3), 0, np.bool_(np.zeros(npts))
for i in range(1000):
i1 = rnd.randint(0, npts)
i2 = rnd.randint(0, npts - 1)
i2 = i2 if i2 < i1 else i2 + 1
i3 = rnd.randint(0, npts - 2)
i3 = i3 if i3 < min(i1, i2) else (i3 +
1 if i3 + 1 < max(i1, i2) else i3 +
2)
inlpts = xz_pts[[i1, i2, i3], :]
prm = geom.smallestSingVector(inlpts)
prm = prm / geom.vecnorm(prm[:2])
prm = -prm if prm[2] < 0 else prm
errs = np.abs(np.inner(xz_pts, prm))
inlmask = errs < threshold
numi = np.sum(inlmask)
if numi > bnumi and float(numi) / npts > 0.5:
bprm, bmask, bnumi = prm, inlmask, numi
prm, numi, mask = bprm, bnumi, bmask
# guided matching
for i in range(10):
if numi == 0: break
prm = geom.smallestSingVector(xz_pts[mask, :])
prm = prm / geom.vecnorm(prm[:2])
prm = -prm if prm[2] < 0 else prm
errs = np.abs(np.inner(xz_pts, prm))
mask = errs < threshold
numi = np.sum(mask)
# get error
err = np.mean(np.abs(np.inner(xz_pts[mask, :], prm)))
return np.array([prm[0], 0, prm[1], prm[2], err])
def compE_t(qray,dray,constants):
# set variables
wRq, wRd, qYaw = constants
xd, yq = dray, qray
yw = tp(np.dot(wRq,tp(yq)))
xw = tp(np.dot(wRd,tp(xd)))
tn = np.cross(yw,xw)
# compute essential matrix parameters based off guessed yaw
t = geom.normalrows(np.cross(tn[0,:],tn[1,:])) # homography translation
return t, -1, True
def lsqE_t(prm,qray,dray,constants,domidx):
# set variables
wRq, wRd, qYaw = constants
xd, yq = dray, qray
yw = tp(np.dot(wRq,tp(yq)))
xw = tp(np.dot(wRd,tp(xd)))
tn = np.cross(yw,xw) # no renormalization to bias more confident planes
# compute essential matrix parameters based off guessed yaw
teig = alg.eig(np.dot(tp(tn),tn))
return geom.normalrows(teig[1][:,np.argmin(teig[0])]) # essential matrix translation
def compE_t(qray, dray, constants):
# set variables
wRq, wRd, qYaw = constants
xd, yq = dray, qray
yw = tp(np.dot(wRq, tp(yq)))
xw = tp(np.dot(wRd, tp(xd)))
tn = np.cross(yw, xw)
# compute essential matrix parameters based off guessed yaw
t = geom.normalrows(np.cross(tn[0, :], tn[1, :])) # homography translation
return t, -1, True
def lsqE_t(prm, qray, dray, constants, domidx):
# set variables
wRq, wRd, qYaw = constants
xd, yq = dray, qray
yw = tp(np.dot(wRq, tp(yq)))
xw = tp(np.dot(wRd, tp(xd)))
tn = np.cross(yw, xw) # no renormalization to bias more confident planes
# compute essential matrix parameters based off guessed yaw
teig = alg.eig(np.dot(tp(tn), tn))
return geom.normalrows(
teig[1][:, np.argmin(teig[0])]) # essential matrix translation
def planefrom3d(C, Q, dbmatch, domplane, Kdinv, wRd):
if domplane == -1: return np.nan * np.zeros(5)
# get 3d points on plane
planes = np.asarray( Image.open( os.path.join(C.hiresdir,dbmatch+'-planes.png') ) )
depths = np.asarray( Image.open( os.path.join(C.hiresdir,dbmatch+'-depth.png') ) )
y, x = np.nonzero(planes==domplane)
npts = len(x)
pray = geom.normalrows( tp( np.dot( wRd , np.dot( Kdinv , np.concatenate( ([x],[y],[np.ones(npts)]) , 0 ) ) ) ) )
pdep = depths[y,x]/100.0
p3d = np.append( geom.vecmul(pray,pdep) , tp(np.array([np.ones(len(pray))])) , 1 )
xz_pts = p3d[:,[0,2,3]]
# RANSAC solve
threshold, g = 2, np.array([0,1,0]) # meters
bprm, bnumi, bmask = np.zeros(3), 0, np.bool_(np.zeros(npts))
for i in range(1000):
i1 = rnd.randint(0,npts)
i2 = rnd.randint(0,npts-1)
i2 = i2 if i2<i1 else i2+1
i3 = rnd.randint(0,npts-2)
i3 = i3 if i3<min(i1,i2) else ( i3+1 if i3+1<max(i1,i2) else i3+2 )
inlpts = xz_pts[[i1,i2,i3],:]
prm = geom.smallestSingVector(inlpts)
prm = prm / geom.vecnorm(prm[:2])
prm = -prm if prm[2]<0 else prm
errs = np.abs(np.inner(xz_pts,prm))
inlmask = errs < threshold
numi = np.sum(inlmask)
if numi > bnumi and float(numi)/npts > 0.5: bprm, bmask, bnumi = prm, inlmask, numi
prm, numi, mask = bprm, bnumi, bmask
# guided matching
for i in range(10):
if numi == 0: break
prm = geom.smallestSingVector(xz_pts[mask,:])
prm = prm / geom.vecnorm(prm[:2])
prm = -prm if prm[2]<0 else prm
errs = np.abs(np.inner(xz_pts,prm))
mask = errs < threshold
numi = np.sum(mask)
# get error
err = np.mean(np.abs(np.inner(xz_pts[mask,:],prm)))
return np.array([prm[0],0,prm[1],prm[2],err])
def compE_tq(qray, dray, constants):
# set variables
Rpr, wRd, qYaw = constants
pr = geom.YPRfromR(Rpr)[1:] # pitch and roll
wRq = geom.RfromYPR(qYaw, pr[0], pr[1])
xd, yq = dray, qray
yw = tp(np.dot(wRq, tp(yq)))
xw = tp(np.dot(wRd, tp(xd)))
tn = np.cross(yw, xw) # no renormalization to bias more confident planes
# compute essential matrix parameters based off guessed yaw
teig = alg.eig(np.dot(tp(tn), tn))
nullidx = np.argmin(teig[0])
valid = teig[0][nullidx] / teig[0][np.argmax(teig[0])] < 1e-2
t = geom.normalrows(teig[1][:, nullidx]) # essential matrix translation
domidx = np.argmax(t)
prm = np.append(np.delete(t / t[domidx], domidx), qYaw)
if valid: prm = lsqE_tq(prm, qray, dray, constants, domidx)
return prm, domidx, valid
def compE_tq(qray,dray,constants):
# set variables
Rpr, wRd, qYaw = constants
pr = geom.YPRfromR(Rpr)[1:] # pitch and roll
wRq = geom.RfromYPR(qYaw,pr[0],pr[1])
xd, yq = dray, qray
yw = tp(np.dot(wRq,tp(yq)))
xw = tp(np.dot(wRd,tp(xd)))
tn = np.cross(yw,xw) # no renormalization to bias more confident planes
# compute essential matrix parameters based off guessed yaw
teig = alg.eig(np.dot(tp(tn),tn))
nullidx = np.argmin(teig[0])
valid = teig[0][nullidx]/teig[0][np.argmax(teig[0])] < 1e-2
t = geom.normalrows(teig[1][:,nullidx]) # essential matrix translation
domidx = np.argmax(t)
prm = np.append( np.delete(t/t[domidx],domidx) , qYaw )
if valid: prm = lsqE_tq(prm,qray,dray,constants,domidx)
return prm, domidx, valid
def compH_tn(qray,dray,ddep,constants):
# set variables
wRq, wRd, qYaw, nYaw = constants
dRq = np.dot(tp(wRd),wRq)
xd, yq = dray, qray
yd = tp(np.dot(dRq,tp(yq)))
xw = tp(np.dot(wRd,tp(xd)))
tn = np.cross(yd,xd)
# compute homography parameters
t = geom.normalrows(np.cross(tn[0,:],tn[1,:])) # homography translation
m = geom.vecnorm(tn)/(geom.vecnorm(np.cross(yd,t))*geom.vecnorm(xw[:,[0,2]]))
f = np.arctan2(xw[:,0],xw[:,2])
errf = lambda prm,argm,argf: prm[0]-argm/np.cos(prm[1]-argf)
kn_init = np.array([1.2*np.mean(m),np.mean(f)])
k, n = tuple( opt.leastsq(errf,kn_init,args=(m,f),warning=False)[0] )
valid = np.std( m/(k*np.cos(n-f)) ) < 0.1
fe = np.mod(n-np.mean(f),2*np.pi)
if np.abs(fe) < np.pi/2: n = np.mod(n+np.pi,2*np.pi)
if np.mean(np.inner(xd-yd,t)) < 0: t = -t
# set parameters and refine
prm = np.append(k*np.dot(wRd,t),180/np.pi*n)
valid = valid and geom.vecnorm(prm[:3]) < 5
return prm, valid
def compH_t(qray,dray,ddep,constants):
# set variables
wRq, wRd, qYaw, nYaw = constants
dRq = np.dot(tp(wRd),wRq)
xd, yq = dray, qray
yd = tp(np.dot(dRq,tp(yq)))
xw = tp(np.dot(wRd,tp(xd)))
tn = np.cross(yd,xd)
n = nYaw * np.pi/180 # homography normal bearing
# compute homography parameters
t = geom.normalrows(np.cross(tn[0,:],tn[1,:])) # homography translation
m = geom.vecnorm(tn)/(geom.vecnorm(np.cross(yd,t))*geom.vecnorm(xw[:,[0,2]]))
f = np.arctan2(xw[:,0],xw[:,2])
errf = lambda prm,argm,argf,argn: prm[0]-argm/np.cos(argn-argf)
k_init = np.mean( m / np.cos(n-f) )
k = tuple( opt.leastsq(errf,k_init,args=(m,f,n),warning=False)[0] )
# k = np.mean( m / np.cos(n-f) )
valid = np.std( m/(k*np.cos(n-f)) ) < 0.1
if np.mean(np.inner(xd-yd,t)) < 0: t = -t
# set parameters and refine
prm = np.abs(k)*np.dot(wRd,t)
valid = valid and geom.vecnorm(prm[:3]) < 5
return prm, valid
print 'Number of inliers / total correspondences : %d / %d' % (
matches['numi'], matches['nmat'])
for idx in match_idx:
start = scale * matches['q2d'][idx, :]
stop = matches['d2d'][idx, :]
stop[0] += off
xdrawcircle(start, 'red')
xdrawline((start, stop), 'green', width=3)
xdrawcircle(stop, 'red')
### draw homography boxes ###
# compute box center and corners
pd = matches['iprm'][-1]
tw = pose[5] * pose[:3]
cd, xd = np.array([.4, -.1, 1]), geom.normalrows(np.array([.5, -.2, 1]))
cw, xw = np.dot(wRd, cd), np.dot(wRd, xd)
nw = -np.array(
[np.sin(pose[4] * np.pi / 180), 0,
np.cos(pose[4] * np.pi / 180)])
cw, xw = pd / np.dot(nw, cw) * cw, pd / np.dot(nw, xw) * xw
trw, brw, tlw, blw = xw.copy(), xw.copy(), 2 * cw - xw, 2 * cw - xw
brw[1], tlw[1] = blw[1], trw[1]
trq, brq, tlq, blq = np.dot(tp(wRq),
trw - tw), np.dot(tp(wRq), brw - tw), np.dot(
tp(wRq),
tlw - tw), np.dot(tp(wRq), blw - tw)
trd, brd, tld, bld = np.dot(tp(wRd), trw), np.dot(tp(wRd), brw), np.dot(
tp(wRd), tlw), np.dot(tp(wRd), blw)
# draw query box
trp, brp, tlp, blp = np.dot(Kq, trq), np.dot(Kq, brq), np.dot(Kq,
def constrainedEssMatrix(matches,
wRd,
wRq,
qYaw=np.nan,
runflag=0,
maxerr=.05,
maxiter=1000):
print 'Solving constrained essential matrix...'
start = time.time()
# Homography parameters to solve for:
# translation: 2 parameters (never known)
# normal yaw: 1 parameter (may be known)
# query yaw: 1 parameter (may be known)
# Set the different run conditions to be used in the RANSAC loop
# Note that if qYaw is unknown, then wRq is assumed just pitch and roll (yaw=0)
if runflag == 0: # qYaw unknown
nprm, nrand = 3, 3
compE, lsqE, errE = compE_tq, lsqE_tq, errE_tq
else: # runflag == 1: qYaw known
nprm, nrand = 2, 2
compE, lsqE, errE = compE_t, lsqE_t, errE_t
# Set variables
nmat, numq = matches['nmat'], matches['numq']
constants = (wRq, wRd, qYaw)
bprm, bmask, bnumi, bdomidx, berr = np.zeros(nprm), np.zeros(
nmat), 0, -1, np.inf
qray, dray, qidx = matches['qray'], matches['dray'], matches['qidx']
# Ransac loop to eliminate outliers with essential matrix
# Solves essential matrix
iter = 0
while iter < maxiter:
iter += 1
q, d = randsamples(nrand, nmat, qray, dray)
prm, domidx, valid = compE(q, d, constants)
if not valid: continue
errs = errE(prm, qray, dray, constants, domidx)
imask, numi = getInliers(errs, maxerr, qidx, numq, nmat)
if numi >= bnumi:
bprm, bmask, bnumi, bdomidx = prm, imask, numi, domidx
# Guided matching
numi, imask, prm, domidx = bnumi, bmask, bprm, bdomidx
last_numi, iter, maxgm = 0, 0, 100
while last_numi != numi and iter < maxgm:
last_numi, iter = numi, iter + 1
q, d = qray[imask, :], dray[imask, :]
prm = lsqE(prm, q, d, constants, domidx)
errs = errE(prm, qray, dray, constants, domidx)
imask, numi = getInliers(errs, maxerr, qidx, numq, nmat)
# Set output parameters
matches['iprm'] = prm
matches['imask'] = imask
matches['ierr'] = geom.vecnorm(
errE(prm, qray[imask, :], dray[imask, :], constants,
domidx)) * numq / numi if numi != 0 else np.inf
matches['numi'] = sum(imask)
# Print output state
if matches['numi'] == 0:
print 'Constrained homography failed.'
pose = np.nan * np.zeros(4)
else:
print 'Result from error metric choosing best inlier set: %f' % matches[
'ierr']
print 'Number of inliers / total correspondences: ' + str(
matches['numi']) + ' / ' + str(nmat)
if runflag == 0: qYaw = prm[3]
pose = np.append(geom.normalrows(prm[:3]), qYaw)
print 'Constrained homography took %.1f seconds.' % (time.time() - start)
return matches, pose
def constrainedEssMatrix(matches, wRd, wRq, qYaw=np.nan, runflag=0, maxerr=.05, maxiter=1000):
print 'Solving constrained essential matrix...'
start = time.time()
# Homography parameters to solve for:
# translation: 2 parameters (never known)
# normal yaw: 1 parameter (may be known)
# query yaw: 1 parameter (may be known)
# Set the different run conditions to be used in the RANSAC loop
# Note that if qYaw is unknown, then wRq is assumed just pitch and roll (yaw=0)
if runflag == 0: # qYaw unknown
nprm, nrand = 3, 3
compE, lsqE, errE = compE_tq, lsqE_tq, errE_tq
else: # runflag == 1: qYaw known
nprm, nrand = 2, 2
compE, lsqE, errE = compE_t, lsqE_t, errE_t
# Set variables
nmat, numq = matches['nmat'], matches['numq']
constants = (wRq,wRd,qYaw)
bprm, bmask, bnumi, bdomidx, berr = np.zeros(nprm), np.zeros(nmat), 0, -1, np.inf
qray, dray, qidx = matches['qray'], matches['dray'], matches['qidx']
# Ransac loop to eliminate outliers with essential matrix
# Solves essential matrix
iter = 0
while iter < maxiter:
iter += 1
q, d = randsamples(nrand, nmat, qray, dray)
prm, domidx, valid = compE(q,d,constants)
if not valid: continue
errs = errE(prm,qray,dray,constants,domidx)
imask, numi = getInliers(errs,maxerr,qidx,numq,nmat)
if numi >= bnumi: bprm, bmask, bnumi, bdomidx = prm, imask, numi, domidx
# Guided matching
numi, imask, prm, domidx = bnumi, bmask, bprm, bdomidx
last_numi, iter, maxgm = 0, 0, 100
while last_numi != numi and iter < maxgm:
last_numi, iter = numi, iter+1
q, d = qray[imask,:], dray[imask,:]
prm = lsqE(prm,q,d,constants,domidx)
errs = errE(prm,qray,dray,constants,domidx)
imask, numi = getInliers(errs,maxerr,qidx,numq,nmat)
# Set output parameters
matches['iprm'] = prm
matches['imask'] = imask
matches['ierr'] = geom.vecnorm(errE(prm,qray[imask,:],dray[imask,:],constants,domidx)) * numq / numi if numi!=0 else np.inf
matches['numi'] = sum(imask)
# Print output state
if matches['numi'] == 0:
print 'Constrained homography failed.'
pose = np.nan * np.zeros(4)
else:
print 'Result from error metric choosing best inlier set: %f' % matches['ierr']
print 'Number of inliers / total correspondences: ' + str(matches['numi']) + ' / ' + str(nmat)
if runflag == 0: qYaw = prm[3]
pose = np.append( geom.normalrows(prm[:3]) , qYaw )
print 'Constrained homography took %.1f seconds.' % (time.time()-start)
return matches, pose
def VPNfromDatabase(C, Q, dimg, vp_threshold):
main_bias, off_bias = 1, 0
if off_bias == 0:
dname = os.path.basename(dimg)
himg, dinfo, dpath = os.path.join(C.hiresdir, dname[:-4] + '.jpg'), \
os.path.join(C.hiresdir, dname[:-4] + '.info'), \
os.path.join(C.hiresdir, dname[:-4] + '.lsd')
dsource = render_tags.EarthmineImageInfo(himg,dinfo)
Kd, wRd = viewparam(dsource,np.nan)
dmid, deqs, dlen = LfromLSD(dpath,himg,Kd)
dvps, dcon, dcent, dseeds = VPfromSeeds(dmid, deqs, dlen, wRd, vp_threshold)
vps, conf, cent = tp(np.dot(wRd,tp(dvps))), dcon, dcent
nvps = len(conf)
if nvps == 0: return np.zeros((0,3)), np.zeros((0,3)), np.zeros(0), np.zeros(0) # return if no vanishing points
else:
# get 3 database images
dname = os.path.basename(dimg)
view = int(dname[-6:-4])
if view < 6: # right side of street
limg, linfo, lpath = os.path.join(C.hiresdir, dname[:-6] + '02.jpg'), \
os.path.join(C.hiresdir, dname[:-6] + '02.info'), \
os.path.join(C.hiresdir, 'lsd', dname[:-6] + '02.lsd')
cimg, cinfo, cpath = os.path.join(C.hiresdir, dname[:-6] + '03.jpg'), \
os.path.join(C.hiresdir, dname[:-6] + '03.info'), \
os.path.join(C.hiresdir, 'lsd', dname[:-6] + '03.lsd')
rimg, rinfo, rpath = os.path.join(C.hiresdir, dname[:-6] + '04.jpg'), \
os.path.join(C.hiresdir, dname[:-6] + '04.info'), \
os.path.join(C.hiresdir, 'lsd', dname[:-6] + '04.lsd')
else: # left side of street
limg, linfo, lpath = os.path.join(C.hiresdir, dname[:-6] + '08.jpg'), \
os.path.join(C.hiresdir, dname[:-6] + '08.info'), \
os.path.join(C.hiresdir, 'lsd', dname[:-6] + '08.lsd')
cimg, cinfo, cpath = os.path.join(C.hiresdir, dname[:-6] + '09.jpg'), \
os.path.join(C.hiresdir, dname[:-6] + '09.info'), \
os.path.join(C.hiresdir, 'lsd', dname[:-6] + '09.lsd')
rimg, rinfo, rpath = os.path.join(C.hiresdir, dname[:-6] + '10.jpg'), \
os.path.join(C.hiresdir, dname[:-6] + '10.info'), \
os.path.join(C.hiresdir, 'lsd', dname[:-6] + '10.lsd')
lsource = render_tags.EarthmineImageInfo(limg, linfo)
csource = render_tags.EarthmineImageInfo(cimg, cinfo)
rsource = render_tags.EarthmineImageInfo(rimg, rinfo)
# extract view parameters
Kl, wRl = viewparam(lsource,np.nan)
Kc, wRc = viewparam(csource,np.nan)
Kr, wRr = viewparam(rsource,np.nan)
# get lines for each database image; image frame equations and segment lengths
lmid, leqs, llen = LfromLSD(lpath, limg, Kl)
cmid, ceqs, clen = LfromLSD(cpath, cimg, Kc)
rmid, reqs, rlen = LfromLSD(rpath, rimg, Kr)
# get candidate vanishing points from lines
lvps, lcon, lcent, lseeds = VPfromSeeds(lmid, leqs, llen, wRl, vp_threshold)
cvps, ccon, ccent, cseeds = VPfromSeeds(cmid, ceqs, clen, wRc, vp_threshold)
rvps, rcon, rcent, rseeds = VPfromSeeds(rmid, reqs, rlen, wRr, vp_threshold)
##### combine candidate vanishing points and into an estimate of #####
##### the building faces' horizontal vanishing points and normals #####
# increase the confidence of vps from the matched view and
if view==2 or view==8 : lcon, ccon, rcon, ccent, rcent, ndvps, seedlens = main_bias*lcon, off_bias*ccon, off_bias*rcon, 0*ccent, 0*rcent, len(lvps), lseeds
elif view==3 or view==9 : lcon, ccon, rcon, lcent, rcent, ndvps, seedlens = off_bias*lcon, main_bias*ccon, off_bias*rcon, 0*lcent, 0*rcent, len(cvps), cseeds
elif view==4 or view==10 : lcon, ccon, rcon, lcent, ccent, ndvps, seedlens = off_bias*lcon, off_bias*ccon, main_bias*rcon, 0*lcent, 0*ccent, len(rvps), rseeds
# map the vanishing points to the world frame (EDN - east/down/north) and combine all vps
lvps, cvps, rvps = tp(np.dot(wRl,tp(lvps))), tp(np.dot(wRc,tp(cvps))), tp(np.dot(wRr,tp(rvps)))
vps, conf, cent = np.concatenate( (lvps,cvps,rvps) , 0 ), np.concatenate((lcon,ccon,rcon)), np.concatenate( (lcent,ccent,rcent) , 0 )
nvps = len(conf)
if nvps == 0: return np.zeros((0,3)), np.zeros((0,3)), np.zeros(0), np.zeros(0) # return if no vanishing points
# get normals and remove vanishing points indicating more than a ~18 degree incline
normals = np.cross(vps,[0,1,0])
mask = geom.vecnorm(normals) > 0.95
vps, cent, normals, conf = vps[mask,:], cent[mask,:], geom.normalrows(normals[mask,:]), conf[mask]
nvps = len(conf)
# sort vanishing points by confidence
sort = np.argsort(conf)
vps, cent, conf = vps[sort[::-1],:], cent[sort[::-1],:], conf[sort[::-1]]
# combine all vanishing points
minconf = 0.2 # average 20% of line length in each image OR 50% of line length in retrieved image
bvps, bcenters, bnorms, bconfs = np.zeros((0,3)), np.zeros((0,3)), np.zeros(0), np.zeros(0)
while len(conf)!=0:
vp = vps[0,:]
mask = np.inner(vps,vp) > np.cos(vp_threshold*np.pi/180)
c = np.sum(conf[mask])/(2*off_bias+main_bias)
if c > minconf:
vp = geom.largestSingVector(geom.vecmul(vps[mask,:],conf[mask]))
if np.inner(vps[0,:],vp) < 0: vp = -vp
normal = np.cross(vp,[0,1,0])
nyaw = np.mod( 180/np.pi * np.arctan2(normal[0],normal[2]) , 360 )
bvps = np.concatenate( (bvps,[vp]) , 0 )
bnorms, bconfs = np.append(bnorms,nyaw), np.append(bconfs,c)
centmask = np.logical_and(mask,cent[:,2]!=0)
center = np.mean(cent[centmask,:],0)
bcenters = np.concatenate( (bcenters,[center]) , 0 )
keep = np.logical_not(mask)
vps, conf, cent = vps[keep,:], conf[keep], cent[keep,:]
else:
vps, conf, cent = np.delete(vps,0,0), np.delete(conf,0), np.delete(cent,0,0)
# sort best vanishing points by confidence
if len(bconfs) == 0: return bvps, bcenters, bnorms, bconfs
sort = np.argsort(bconfs)
bvps, bcenters, bnorms, bconfs = bvps[sort[::-1],:], bcenters[sort[::-1],:], bnorms[sort[::-1]], bconfs[sort[::-1]]
return bvps, bcenters, bnorms, bconfs, nvps
def estimate_pose(C, Q, dbmatch, gtStatus=None):
# settings
param = C.pose_param
runflag = param['runflag']
np.seterr(all='ignore')
Q.datafile = os.path.join(C.pose_param['resultsdir'],
'data_' + Q.name + '.txt')
open(Q.datafile, 'w').close()
#####----- PRINT RUN DETAILS -----#####
run_info = os.path.join(param['resultsdir'], param['run_info'])
open(run_info, 'w').close()
with open(run_info, 'a') as ri:
if runflag == 11:
print >> ri, 'Method: Yaw, planes from VPs. Scale computed with homography.'
elif runflag == 10:
print >> ri, 'Method: Yaw, planes from VPs. Scale computed after homography.'
if param['cheat']:
print >> ri, 'Ground truth yaw and plane used (cheating).'
print >> ri, 'Inlier base error threshold: %.3f' % param['inlier_error']
print >> ri, 'Base iteration scale: %d' % param['ransac_iter']
#####----- PRINT RUN DETAILS -----#####
# get high res db image and sift paths
dbinfo = os.path.join(C.hiresdir, dbmatch + '.info')
dbimg = os.path.join(C.hiresdir, dbmatch + '.jpg')
dbsift = os.path.join(C.hiresdir, dbmatch + 'sift.txt')
dbsource = render_tags.EarthmineImageInfo(dbimg, dbinfo)
# Set Kd, wRd, and db position
wx, wy = dbsource.image.size
fov = dbsource.fov
Kd = geom.cameramat(wx, wy, fov)
Kdinv = alg.inv(Kd)
y, p, r = dbsource.yaw, dbsource.pitch, dbsource.roll
wRd = geom.RfromYPR(y, p, r) # db camera orientation (camera to world)
olat, olon, oalt = dbsource.lat, dbsource.lon, dbsource.alt # database location
# get high res query information
qname = Q.name
qimg = os.path.join(C.querydir, 'hires', qname + '.jpg')
qsift = os.path.join(C.querydir, 'hires', qname + 'sift.txt')
qsource = render_tags.QueryImageInfo(Q.datasource)
glat, glon = qsource.lat, qsource.lon
gzx = geom.lltom(olat, olon, glat, glon)
timename = qname[-12:-10] + qname[-9:-7] + qname[-6:-4] #+qname[-3:]
# Set Kq
wx, wy = qsource.image.size
fov = qsource.fov
Kq = geom.cameramat(wx, wy, fov)
Kqinv = alg.inv(Kq)
cyaw, p, r = qsource.yaw, qsource.pitch, qsource.roll # cyaw - cell phone yaw
# get high res sift rematch
matches = highresSift(C, Q, dbmatch)
with open(Q.datafile, 'a') as df:
print >> df, 'Number of matches | number of queries | ratio: %.0f | %.0f | %.2f' % (
matches['nmat'], matches['numq'],
float(matches['nmat']) / matches['numq'])
print >> df, ''
# Get estimated ground truth query location and normal direction
tlat, tlon, tnorm, tyaw = getGTpose(C, Q)
qzx = geom.lltom(olat, olon, tlat, tlon)
# get query yaw and plane yaw from vanishing point anaylsis
yawforvp = tyaw if param['cheat'] else np.nan
vyaw, vnorms = vp_analysis.getQNyaws(C, Q, qimg, dbimg, qsource, yawforvp)
# get dominant planes
dplanes, psizes, planeprms = find_dbplanes(C, Q, dbmatch, Kdinv, wRd)
# match vanishing point planes to database planes
pyaws, planes, pconfs = combine_planes(runflag, vnorms, dplanes, psizes,
planeprms)
print 'VP and DB Planes: ' + str(np.int_(pyaws)) + ', ' + str(planes)
with open(Q.datafile, 'a') as df:
# print >>df, 'Planes detected with vanishing points:'
for i in range(len(pconfs)):
perr = np.round(np.mod(pyaws[i] - tnorm, 360))
perr = perr if perr < 180 else 360 - perr
print >> df, 'Plane Yaw | DB plane | Confidence | Error : %3.0f | %d | %.2f | %.0f' % (
pyaws[i], 0 if planes[i] < 0 else planes[i], pconfs[i], perr)
yerr = np.round(np.mod(vyaw - tyaw, 360))
yerr = yerr if yerr < 180 else 360 - yerr
print >> df, 'VP Yaw | Confidence | Error : %3.0f | %.2f | %.0f' % (
vyaw, np.nan, yerr)
print >> df, 'Cell yaw | True yaw | Plane : %3.0f | %3.0f | %3.0f' % (
cyaw, tyaw, tnorm)
print >> df, ''
# Set yaw value to be used
if runflag >= 10: # vanishing point methods
if np.isnan(vyaw): yaw, yawerr = cyaw, 0
else: yaw, yawerr = vyaw, 0
else: yaw, yawerr = cyaw, 0 # set cell phone yaw to use, plane normal
wRq = geom.RfromYPR(yaw, p, r) # camera orientation (camera to world)
### --- THIS IS FOR CHEATING --- ###
if param['cheat']:
if not np.isnan(tnorm):
pyaws, planes, pconfs = np.append(pyaws, tnorm), np.append(
planes, -1), np.append(pconfs, 1)
yaw, yawerr = tyaw, 0
wRq = geom.RfromYPR(yaw, p, r) # camera orientation (camera to world)
### --- THIS IS FOR CHEATING --- ###
# print pre-homography data to file
vyaw_err = np.round(np.round(np.mod(
tyaw - vyaw, 360))) if not np.isnan(vyaw) else np.nan
vyaw_err = vyaw_err if vyaw_err < 180 else 360 - vyaw_err
dbears = np.mod(180 / np.pi * np.arctan2(planeprms[:, 0], planeprms[:, 2]),
360)
print 'Computed / ground truth cell phone yaw: %.0f / %.0f' % (vyaw, tyaw)
with open(os.path.join(param['resultsdir'], param['extras_file']),
'a') as extras_file:
print >> extras_file, '\t'.join([
timename,
'%.0f' % tnorm,
'%.0f' % np.round(tyaw),
'%.0f' % cyaw,
'%.0f' % vyaw,
'%.4f' % np.nan,
str(vyaw_err)
])
print >> extras_file, '\t'.join(['%.4f' % 0 for vnorm in vnorms])
print >> extras_file, '\t'.join(['%.0f' % vnorm for vnorm in vnorms])
print >> extras_file, '\t'.join(['%.0f' % plane for plane in planes])
print >> extras_file, '\t'.join(
['%.0f' % dplane for dplane in dplanes])
print >> extras_file, '\t'.join(['%.0f' % dbear for dbear in dbears])
print >> extras_file, '\t'.join(
['%.3f' % dnerr for dnerr in planeprms[:, 4]])
# Fill out match information
nmat = matches['nmat']
matches['qray'] = geom.normalrows(
tp(np.dot(Kqinv, np.append(tp(matches['q2d']), [np.ones(nmat)], 0))))
matches['dray'] = geom.normalrows(
tp(np.dot(Kdinv, np.append(tp(matches['d2d']), [np.ones(nmat)], 0))))
matches = match_info(C, Q, matches, dbmatch, wRd)
matches_start = matches.copy()
# Solve for query pose using constrained image geometry
init_matches = initMatches(matches.copy())
matches['numi'], matches['hconf'] = 0, 0
runflag, ntry, planechose = param['runflag'], 0, 0
parameters = (wRq, wRd, yaw, np.nan, runflag, param['inlier_error'],
param['ransac_iter'], 10, True)
if param['ransac_iter'] == 0:
matches = init_matches
matches['numi'], matches['hconf'] == 0, 0
pose = np.zeros(6)
pose[3:5] = np.nan
elif runflag < 10:
matches, pose = solveGeom(init_matches, parameters, yawerr)
else:
ntry = 1
viter = -np.ones(3)
parameters = (wRq, wRd, yaw, np.nan, runflag, param['inlier_error'],
param['ransac_iter'], 15, True)
matches, pose, planechose = solveNorm(C, Q, dbmatch, pyaws, planes,
init_matches, parameters, yawerr)
viter[0] = matches['viter']
if matches['numi'] == 0 or matches['hconf'] == 0:
ntry = 2
parameters = (wRq, wRd, yaw, np.nan, runflag,
3 * param['inlier_error'], param['ransac_iter'], 10,
True)
matches, pose, planechose = solveNorm(C, Q, dbmatch, pyaws, planes,
init_matches, parameters,
yawerr)
viter[1] = matches['viter']
if matches['numi'] == 0 or matches['hconf'] == 0:
ntry, planechose = 3, 0
parameters = (wRq, wRd, yaw, np.nan, 7, 3 * param['inlier_error'],
param['ransac_iter'], 10, True)
matches, pose = solveYaw(init_matches, parameters, yawerr)
viter[2] = matches['viter']
if matches['numi'] == 0 or matches['hconf'] == 0:
ntry, planechose = 4, 0
# save matches to disk
matches_file = os.path.join(param['resultsdir'],
'matches_' + qname + '.pkl')
matches_out = open(matches_file, 'wb')
pickle.dump(matches, matches_out)
matches_out.close()
# extract pose parameters
comp_runflag = matches['runflag']
tray = pose[:3]
comp_yaw = pose[3]
comp_pyaw = pose[4] if runflag >= 0 else np.nan
scale = pose[5] if runflag >= 0 else np.nan
# Get scaled translation for query location
if np.isnan(scale):
wRq_pr = geom.YPRfromR(wRq)[1:]
comp_wRq = geom.RfromYPR(comp_yaw, wRq_pr[0], wRq_pr[1])
qloc = scalefrom3d(matches, tray, comp_wRq)[0]
else: # scale calculated in RANSAC loop
qloc = scale * tray
# temporarily get yaw error
qyaw_error = np.round(abs(np.mod(tyaw - comp_yaw, 360)))
qyaw_error = qyaw_error if qyaw_error < 180 else abs(qyaw_error - 360)
# compute location errors wrt estimated query locations
loc_err = ((qloc[0] - qzx[1])**2 + (qloc[2] - qzx[0])**2)**0.5
gps_err = ((gzx[1] - qzx[1])**2 + (gzx[0] - qzx[0])**2)**0.5
# compute the angle difference between T and ground truth translation
tyaw_error = np.round(
abs(180 / np.pi * np.arccos(
np.abs(qloc[0] * qzx[1] + qloc[2] * qzx[0]) /
(alg.norm([qloc[0], qloc[2]]) * alg.norm(qzx)))))
# compute the plane normal angle error
nyaw_error = np.nan if np.isnan(comp_pyaw) or np.isnan(tnorm) else np.mod(
np.round(abs(comp_pyaw - tnorm)), 180)
nyaw_error = nyaw_error if nyaw_error < 90 else abs(nyaw_error - 180)
# write pose estimation results to file
yaw_err = np.nan
pose_file = os.path.join(param['resultsdir'], param['pose_file'])
with open(pose_file, 'a') as pf:
print >>pf, '\t'.join([qname, str(loc_err), str(gps_err), \
str(tyaw_error), str(qyaw_error), str(nyaw_error), str(matches['numi']), \
str(matches['numq']), str(matches['nmat']), str(matches['hconf']), \
str(qloc[0]), str(qloc[2]), str(yaw_err), str(runflag)])
# print post-homography data to file
with open(Q.datafile, 'a') as df:
print >> df, ''
print >> df, '------------------'
print >> df, ''
if ntry == 1:
print >> df, 'Homography solution using low error threshold with restrictions.'
elif ntry == 2:
print >> df, 'Homography solution using high error threshold with restrictions.'
else:
print >> df, 'Solution not found. Setting T=0.'
if planechose == 0:
print >> df, 'Solution formed with unset plane normal.'
else:
'Solution chosen with plane normal %d chosen.' % planechose
print >> df, 'VP yaw | Computed yaw | Actual Yaw | Error : %3.0f | %3.0f | %3.0f | %3.0f' % (
vyaw, comp_yaw, tyaw, qyaw_error)
print >> df, 'Computed Normal | Actual Normal | Error : %3.0f | %3.0f | %3.0f' % (
comp_pyaw, tnorm, nyaw_error)
print >> df, 'Translation (x|y|z): %.1f | %.1f | %.1f' % (
qloc[0], qloc[1], qloc[2])
print >> df, 'True position (x|-|z): %.1f | - | %.1f' % (qzx[1],
qzx[0])
print >> df, 'Angle error | Location error: %.0f | %.1f' % (tyaw_error,
loc_err)
print >> df, 'Number of Inliers | Total matches | Ratio: %d | %d | %.2f' % (
matches['numi'], matches['nmat'], np.nan if matches['nmat'] == 0
else float(matches['numi']) / matches['nmat'])
print >> df, 'Reprojection error | Homography confidence: %.3f | %.1f' % (
matches['rperr'], matches['hconf'])
print >> df, 'Valid Homographies | Iterations | Ratio: %d | %d | %.3f' % (
matches['viter'], matches['niter'], np.nan if matches['niter'] == 0
else float(matches['viter']) / matches['niter'])
print >> df, ''
print >> df, '------------------'
print >> df, ''
booleans = [ loc_err<5, loc_err<10, not(5<np.mod(vyaw-tyaw,360)<355), \
not(10<np.mod(vyaw-tyaw,360)<350), \
not(5<np.mod(comp_yaw-tyaw,360)<355), ntry==1, ntry!=3, \
planechose!=0, matches['nmat']!=matches_start['nmat'], \
0 if planechose==0 else pconfs[planechose-1]>0, comp_yaw-vyaw ]
print >> df, '|'.join(['%.0f' % (b) for b in booleans])
# draw matches
close = int(loc_err < 5) + int(loc_err < 10)
yawclose = int(not (5 < np.mod(vyaw - tyaw, 360) < 355)) + int(not (
10 < np.mod(vyaw - tyaw, 360) < 350))
imgpath = os.path.join( param['resultsdir'] , qname + ';locerr=%.2f' % (loc_err) + ';locPerf_' + str(close) \
+ ';yawPerf_' + str(yawclose) + ';nplanes_' + str(len(pyaws)) + ';plane_' + str(planes[planechose]) + ';try_' + str(ntry) \
+ ';tAng=%.0f' % (tyaw_error) + ';qAng=%.0f' % (qyaw_error) + ';nAng=%.0f' % (nyaw_error) + ';' + dbmatch + '.jpg')
draw_matches(matches, qimg, dbimg, imgpath)
# imgpath = os.path.join( param['resultsdir'] , 'homography;' + qname + ';' + dbmatch + '.jpg')
# draw_hom(matches, pose, wRq, wRd, Kq, Kd, qimg, dbimg, imgpath)
if C.QUERY == 'oakland1': C.pose_param['draw_tags'] = False
if C.pose_param['draw_tags']:
draw_tags(C, Q, matches, pose, dbmatch, olat, olon, Kd, Kq)
print 'Computed yaw / ground truth yaw : %.0f / %.0f' % (comp_yaw,
tyaw)
if runflag < 10:
print 'Computed normal bearing / ground truth : %.0f / %.0f' % (
comp_pyaw, tnorm)
print 'Computed query location relative to db : %.1f, %.1f, %.1f' % tuple(
qloc)
print 'Ground truth query location relative to db : %.1f, - , %.1f' % (
qzx[1], qzx[0])
input = (wRq, wRd)
return qloc, loc_err, matches, input
def constrainedHomography(matches, wRd, wRq, qYaw=np.nan, nYaw=np.nan, runflag=0, maxerr=.01, maxiter=10000, minI=15, yrestrict=True):
# Homography parameters to solve for:
# translation: 2 parameters (never known, always solved)
# normal yaw: 1 parameter (may be known. always solved)
# query yaw: 1 parameter (may be known. always solved)
# scale factor: 1 parameter (never known, may not solve for this)
# Set the different run conditions to be used in the RANSAC loop
# Note that if qYaw is unknown, then wRq is assumed just pitch and roll (yaw=0)
if runflag == 0: # qYaw unknown, nYaw unknown
nprm, nrand = 5, 3
compH, lsqH, errH, bestH = compH_tqn, lsqH_tqn, errH_tqn, bestH_
elif runflag == 1: # qYaw unknown, nYaw known
nprm, nrand = 4, 2
compH, lsqH, errH, bestH = compH_tq, lsqH_tq, errH_tq, bestH_
elif runflag == 2: # qYaw known, nYaw unknown
nprm, nrand = 4, 2
compH, lsqH, errH, bestH = compH_tn, lsqH_tn, errH_tn, bestH_
elif runflag == 3: # qYaw known, nYaw known
nprm, nrand = 3, 2
compH, lsqH, errH, bestH = compH_t, lsqH_t, errH_t, bestH_
elif runflag == 4: # qYaw unknown, nYaw unknown, solve for depth
nprm, nrand = 6, 3
compH, lsqH, errH, bestH = compH_dtqn, lsqH_dtqn, errH_dtqn, bestH_d
elif runflag == 5: # qYaw unknown, nYaw known, solve for depth
nprm, nrand = 5, 2
compH, lsqH, errH, bestH = compH_dtq, lsqH_dtq, errH_dtq, bestH_d
elif runflag == 6: # qYaw known, nYaw unknown, solve for depth
nprm, nrand = 5, 2
compH, lsqH, errH, bestH = compH_dtn, lsqH_dtn, errH_dtn, bestH_d
else: # runflag == 7: qYaw known, nYaw known, solve for depth
nprm, nrand = 4, 2
compH, lsqH, errH, bestH = compH_dt, lsqH_dt, errH_dt, bestH_d
if not yrestrict: bestH = bestH_
# Compute the number of Ransac iterations to perform and print run parameters
rsiter = int( 10 * np.log(.01) / -np.abs(np.log(1-float(minI**nrand)/matches['nmat']**nrand)) )
maxiter = min(maxiter,rsiter)
print 'Solving constrained homography [%d]: %.3f error threshold, %d iterations...' % (runflag,maxerr,maxiter)
start = time.time()
# Set local variables
nmat, numq = matches['nmat'], matches['numq']
constants = (wRq,wRd,qYaw,nYaw)
bprm, bmask, bnumi, bconf, bfrc, iterstop = np.zeros(nprm), np.bool_(np.zeros(nmat)), 0, 0, 0, maxiter
qray, dray, ddep, qidx, weights = matches['qray'], matches['dray'], matches['ddep'], matches['qidx'], matches['weight']
# Ransac loop to eliminate outliers with homography
# Solves homography matrix for homography matrix H=qRd(I+tn') using y ~ Hx
iter, vcount = 0, 0
while iter < iterstop:
iter += 1
q, d, dep = randsamples(nrand, nmat, qray, dray, ddep)
prm, valid = compH(q,d,dep,constants)
if not valid: continue
errs = errH(prm,qray,dray,ddep,constants)
imask, numi, iconf = getInliers(errs,weights,maxerr,qidx,numq,nmat)
if numi < minI: continue
vcount += 1
if bestH(prm,numi,minI,iconf,bconf):
bprm, bmask, bnumi, bconf, bfrc = prm, imask, numi, iconf, float(numi)/matches['nmat']
iterstop = min( maxiter , 10*np.log(.01)/-np.abs(np.log(1-bfrc**nrand)) )
niter = iter
print 'Total valid samples / total iterations: %d / %d' % (vcount,iter)
# Guided matching
prm, numi, imask, iconf = bprm, bnumi, bmask, bconf
iter, maxgm = 0, 100
while numi >= minI:
iter += 1
q, d, dep = qray[imask,:], dray[imask,:], ddep[imask]
new_prm = lsqH(prm,q,d,dep,constants)
errs = errH(new_prm,qray,dray,ddep,constants)
new_imask, new_numi, new_iconf = getInliers(errs,weights,maxerr,qidx,numq,nmat)
if (new_imask==imask).all() or iter >= maxgm:
prm, numi, imask, iconf = new_prm, new_numi, new_imask, new_iconf
break
# calculate and store homography matrix
if runflag == 7:
wRq, wRd, qYaw, nYaw = constants
dRq = np.dot(tp(wRd),wRq)
td = np.dot(tp(wRd),prm[:3])
nd = -np.dot(tp(wRd),[np.sin(nYaw*np.pi/180),0,np.cos(nYaw*np.pi/180)])
H = np.dot(tp(dRq),np.eye(3,3)-np.outer(td,nd))
matches['estH'] = H
matches['wRd'] = wRd
matches['wRq'] = wRq
# Set output parameters
matches['constants'] = constants
matches['niter'] = niter
matches['viter'] = vcount
matches['iprm'] = prm
matches['imask'] = imask
matches['rperr'] = geom.vecnorm(errH(prm,qray[imask,:],dray[imask,:],ddep[imask],constants)) / numi**0.5
matches['numi'] = numi
matches['ifrc'] = float(numi) / matches['nmat']
matches['iconf'] = iconf / np.sum(weights)
matches['hconf'] = ( iconf / np.sum(weights) ) / matches['rperr']
matches['runflag'] = runflag
# Print output state
if matches['numi'] == 0:
print 'Constrained homography failed.'
pose = np.zeros(6)
pose[3:5] = np.nan
else:
print 'Resulting confidence of inlier set: %.4f' % matches['iconf']
print 'Number of inliers / total correspondences: ' + str(matches['numi']) + ' / ' + str(nmat)
if np.mod(runflag,4) == 0: qYaw, nYaw = prm[3], prm[4]
elif np.mod(runflag,4) == 1: qYaw, nYaw = prm[3], nYaw
elif np.mod(runflag,4) == 2: qYaw, nYaw = qYaw, prm[3]
if runflag == 4: scale = prm[5]*geom.vecnorm(prm[:3])
elif runflag == 5: scale = prm[4]*geom.vecnorm(prm[:3])
elif runflag == 6: scale = prm[4]*geom.vecnorm(prm[:3])
elif runflag == 7: scale = prm[3]*geom.vecnorm(prm[:3])
else: scale = np.nan
pose = np.append( geom.normalrows(prm[:3]) , [ qYaw , nYaw , scale ] )
print 'Constrained homography took %.1f seconds.' % (time.time()-start)
return matches, pose
#match_idx = np.nonzero( np.zeros(matches['nmat'] ) )[0]
print 'Number of inliers / total correspondences : %d / %d' % (matches['numi'],matches['nmat'])
for idx in match_idx:
start = scale*matches['q2d'][idx,:]
stop = matches['d2d'][idx,:]
stop[0] += off
xdrawcircle(start,'red')
xdrawline((start,stop),'green',width=3)
xdrawcircle(stop,'red')
### draw homography boxes ###
# compute box center and corners
pd = matches['iprm'][-1]
tw = pose[5]*pose[:3]
cd, xd = np.array([.4,-.1,1]), geom.normalrows(np.array([.5,-.2,1]))
cw, xw = np.dot(wRd,cd), np.dot(wRd,xd)
nw = -np.array([np.sin(pose[4]*np.pi/180),0,np.cos(pose[4]*np.pi/180)])
cw, xw = pd/np.dot(nw,cw)*cw, pd/np.dot(nw,xw)*xw
trw, brw, tlw, blw = xw.copy(), xw.copy(), 2*cw-xw, 2*cw-xw
brw[1], tlw[1] = blw[1], trw[1]
trq, brq, tlq, blq = np.dot(tp(wRq),trw-tw), np.dot(tp(wRq),brw-tw), np.dot(tp(wRq),tlw-tw), np.dot(tp(wRq),blw-tw)
trd, brd, tld, bld = np.dot(tp(wRd),trw), np.dot(tp(wRd),brw), np.dot(tp(wRd),tlw), np.dot(tp(wRd),blw)
# draw query box
trp, brp, tlp, blp = np.dot(Kq,trq), np.dot(Kq,brq), np.dot(Kq,tlq), np.dot(Kq,blq)
trp, brp, tlp, blp = (trp/trp[2])[:2], (brp/brp[2])[:2], (tlp/tlp[2])[:2], (blp/blp[2])[:2]
xdrawline((scale*trp,scale*brp),'green',off=0,width=10)
xdrawline((scale*brp,scale*blp),'green',off=0,width=10)
xdrawline((scale*blp,scale*tlp),'green',off=0,width=10)
xdrawline((scale*tlp,scale*trp),'green',off=0,width=10)
# draw database box
def VPQfromQuery(C, Q, qimg, qsource, vps, vnorms, vpconfs, vp_threshold, tyaw):
# get query vanishing points
qname = os.path.basename(qimg)
qpath = os.path.join(C.querydir, 'hires', 'lsd', qname[:-4] + '.lsd')
Kq, wRq = viewparam(qsource,tyaw)
qmid, qleq, qlen = LfromLSD(qpath, qimg, Kq)
qvps, conf, qcent, seedlens = VPfromSeeds(qmid, qleq, qlen, wRq, vp_threshold)
nqvps = len(conf)
##### combine candidate vanishing points and vp from db #####
##### into an estimate of the true query yaw orientation #####
# map vanishing points to world frame
qvps = tp(np.dot(wRq,tp(qvps)))
# align vanishing points based on normal and compute normals
qnorms = geom.normalrows(np.cross(qvps,[0,1,0]))
for i in range(len(conf)):
if np.dot(tp(wRq),qnorms[i,:])[2] > 0:
qnorms[i,:] *= -1
qvps[i,:] *= -1
# find optimal alignment of vanishing points
cyaw = geom.YPRfromR(wRq)[0] # cell phone yaw
byaw, bconf, bvps, bnorms, bvpconfs, nvps = np.nan, 0, vps, vnorms, vpconfs, len(vpconfs)
# print '------------------------'
# print vpconfs
# print np.mod( vnorms , 360)
# print conf
# print np.mod( 180/np.pi * np.arctan2(qnorms[:,0],qnorms[:,2]) , 360 )
# print '------------------------'
qnormyaws = 180/np.pi * np.arctan2(qnorms[:,0],qnorms[:,2])
for i in range(len(vpconfs)):
for j in range(len(conf)):
# compute relative yaw change
vnormyaw = vnorms[i] #180/np.pi * np.arctan2(vnorms[i,0],vnorms[i,2])
qnormyaw = qnormyaws[j]
dyaw = vnormyaw - qnormyaw
dyaw = dyaw if dyaw<180 else dyaw-360
if abs(dyaw) > 50: continue # skip if the yaw change is too great
# apply relative yaw change
dR = geom.RfromYPR(dyaw,0,0)
dvps, dnorms = tp(np.dot(dR,tp(qvps))), tp(np.dot(dR,tp(qnorms)))
# get list of matching vanishing points
dbidx, qidx, weights = np.zeros(0,np.int), np.zeros(0,np.int), np.zeros(0)
# Gather lise of aligned vanishing points
for k in range(len(vpconfs)):
vpalign = np.inner(dvps,vps[k,:])
alignidx = np.argmax(vpalign)
if vpalign[alignidx] < np.cos(np.pi/180*2*vp_threshold): continue
dbidx, qidx = np.append(dbidx,k), np.append(qidx,alignidx)
weights = np.append(weights,conf[alignidx]*vpconfs[k])
# Optimize for the yaw change
yawconf = np.sum(weights)
if yawconf <= bconf: continue
dyaws = np.mod(vnorms[dbidx]-qnormyaws[qidx],360)
if dyaws[0] < 90: dyaws[dyaws>270] = dyaws[dyaws>270]-360
elif dyaws[0] > 270: dyaws[dyaws<90] = dyaws[dyaws<90]+360
dyaw = np.sum(weights*dyaws) / yawconf
byaw, bconf, bvpconfs, nvps = np.mod(cyaw+dyaw,360), yawconf, np.ones(len(weights)), len(weights)
bnorms = np.mod( qnormyaws[qidx] + dyaw , 360 )
return byaw, bconf, bvps, bnorms, bvpconfs, nqvps
def estimate_pose(C, Q, dbmatch, gtStatus=None):
# settings
param = C.pose_param
runflag = param['runflag']
np.seterr(all='ignore')
Q.datafile = os.path.join(C.pose_param['resultsdir'],'data_'+Q.name+'.txt')
open(Q.datafile,'w').close()
#####----- PRINT RUN DETAILS -----#####
run_info = os.path.join(param['resultsdir'],param['run_info'])
open(run_info,'w').close()
with open(run_info,'a') as ri:
if runflag == 11: print >>ri, 'Method: Yaw, planes from VPs. Scale computed with homography.'
elif runflag == 10: print >>ri, 'Method: Yaw, planes from VPs. Scale computed after homography.'
if param['cheat']: print >>ri, 'Ground truth yaw and plane used (cheating).'
print >>ri, 'Inlier base error threshold: %.3f' % param['inlier_error']
print >>ri, 'Base iteration scale: %d' % param['ransac_iter']
#####----- PRINT RUN DETAILS -----#####
# get high res db image and sift paths
dbinfo = os.path.join(C.hiresdir, dbmatch + '.info')
dbimg = os.path.join(C.hiresdir,dbmatch+'.jpg')
dbsift = os.path.join(C.hiresdir,dbmatch+'sift.txt')
dbsource = render_tags.EarthmineImageInfo(dbimg, dbinfo)
# Set Kd, wRd, and db position
wx,wy = dbsource.image.size
fov = dbsource.fov
Kd = geom.cameramat(wx, wy, fov)
Kdinv = alg.inv(Kd)
y,p,r = dbsource.yaw, dbsource.pitch, dbsource.roll
wRd = geom.RfromYPR(y,p,r) # db camera orientation (camera to world)
olat,olon,oalt = dbsource.lat,dbsource.lon,dbsource.alt # database location
# get high res query information
qname = Q.name
qimg = os.path.join(C.querydir,'hires',qname+'.jpg')
qsift = os.path.join(C.querydir,'hires',qname+'sift.txt')
qsource = render_tags.QueryImageInfo(Q.datasource)
glat,glon = qsource.lat,qsource.lon
gzx = geom.lltom(olat,olon,glat,glon)
timename = qname[-12:-10]+qname[-9:-7]+qname[-6:-4]#+qname[-3:]
# Set Kq
wx,wy = qsource.image.size
fov = qsource.fov
Kq = geom.cameramat(wx, wy, fov)
Kqinv = alg.inv(Kq)
cyaw,p,r = qsource.yaw, qsource.pitch, qsource.roll # cyaw - cell phone yaw
# get high res sift rematch
matches = highresSift(C, Q, dbmatch)
with open(Q.datafile,'a') as df:
print >>df, 'Number of matches | number of queries | ratio: %.0f | %.0f | %.2f' % (matches['nmat'], matches['numq'], float(matches['nmat'])/matches['numq'])
print >>df, ''
# Get estimated ground truth query location and normal direction
tlat, tlon, tnorm, tyaw = getGTpose(C, Q)
qzx = geom.lltom(olat,olon,tlat,tlon)
# get query yaw and plane yaw from vanishing point anaylsis
yawforvp = tyaw if param['cheat'] else np.nan
vyaw, vnorms = vp_analysis.getQNyaws(C, Q, qimg, dbimg, qsource, yawforvp)
# get dominant planes
dplanes, psizes, planeprms = find_dbplanes(C, Q, dbmatch, Kdinv, wRd)
# match vanishing point planes to database planes
pyaws, planes, pconfs = combine_planes(runflag,vnorms,dplanes,psizes,planeprms)
print 'VP and DB Planes: ' + str(np.int_(pyaws)) + ', ' + str(planes)
with open(Q.datafile,'a') as df:
# print >>df, 'Planes detected with vanishing points:'
for i in range(len(pconfs)):
perr = np.round(np.mod(pyaws[i]-tnorm,360))
perr = perr if perr<180 else 360-perr
print >>df, 'Plane Yaw | DB plane | Confidence | Error : %3.0f | %d | %.2f | %.0f' % (pyaws[i],0 if planes[i]<0 else planes[i],pconfs[i],perr)
yerr = np.round(np.mod(vyaw-tyaw,360))
yerr = yerr if yerr<180 else 360-yerr
print >>df, 'VP Yaw | Confidence | Error : %3.0f | %.2f | %.0f' % (vyaw,np.nan,yerr)
print >>df, 'Cell yaw | True yaw | Plane : %3.0f | %3.0f | %3.0f' % (cyaw,tyaw,tnorm)
print >>df, ''
# Set yaw value to be used
if runflag >= 10: # vanishing point methods
if np.isnan(vyaw): yaw, yawerr = cyaw, 0
else: yaw, yawerr = vyaw, 0
else: yaw, yawerr = cyaw, 0 # set cell phone yaw to use, plane normal
wRq = geom.RfromYPR(yaw,p,r) # camera orientation (camera to world)
### --- THIS IS FOR CHEATING --- ###
if param['cheat']:
if not np.isnan(tnorm):
pyaws, planes, pconfs = np.append(pyaws,tnorm), np.append(planes,-1), np.append(pconfs,1)
yaw, yawerr = tyaw, 0
wRq = geom.RfromYPR(yaw,p,r) # camera orientation (camera to world)
### --- THIS IS FOR CHEATING --- ###
# print pre-homography data to file
vyaw_err = np.round(np.round(np.mod(tyaw-vyaw,360))) if not np.isnan(vyaw) else np.nan
vyaw_err = vyaw_err if vyaw_err<180 else 360-vyaw_err
dbears = np.mod( 180/np.pi*np.arctan2(planeprms[:,0],planeprms[:,2]) , 360 )
print 'Computed / ground truth cell phone yaw: %.0f / %.0f' % (vyaw,tyaw)
with open(os.path.join(param['resultsdir'],param['extras_file']),'a') as extras_file:
print >>extras_file, '\t'.join([timename, '%.0f' % tnorm, '%.0f' % np.round(tyaw), '%.0f' % cyaw, '%.0f' % vyaw, '%.4f'%np.nan, str(vyaw_err)])
print >>extras_file, '\t'.join([ '%.4f' % 0 for vnorm in vnorms ])
print >>extras_file, '\t'.join([ '%.0f' % vnorm for vnorm in vnorms ])
print >>extras_file, '\t'.join([ '%.0f' % plane for plane in planes ])
print >>extras_file, '\t'.join([ '%.0f' % dplane for dplane in dplanes ])
print >>extras_file, '\t'.join([ '%.0f' % dbear for dbear in dbears ])
print >>extras_file, '\t'.join([ '%.3f' % dnerr for dnerr in planeprms[:,4] ])
# Fill out match information
nmat = matches['nmat']
matches['qray'] = geom.normalrows(tp(np.dot(Kqinv,np.append(tp(matches['q2d']),[np.ones(nmat)],0))))
matches['dray'] = geom.normalrows(tp(np.dot(Kdinv,np.append(tp(matches['d2d']),[np.ones(nmat)],0))))
matches = match_info(C, Q, matches, dbmatch, wRd)
matches_start = matches.copy()
# Solve for query pose using constrained image geometry
init_matches = initMatches(matches.copy())
matches['numi'], matches['hconf'] = 0, 0
runflag, ntry, planechose = param['runflag'], 0, 0
parameters = ( wRq, wRd, yaw, np.nan, runflag, param['inlier_error'], param['ransac_iter'], 10, True )
if param['ransac_iter'] == 0:
matches = init_matches
matches['numi'], matches['hconf'] == 0, 0
pose = np.zeros(6)
pose[3:5] = np.nan
elif runflag < 10:
matches, pose = solveGeom(init_matches,parameters,yawerr)
else:
ntry = 1
viter = -np.ones(3)
parameters = ( wRq, wRd, yaw, np.nan, runflag, param['inlier_error'], param['ransac_iter'], 15, True )
matches, pose, planechose = solveNorm(C,Q,dbmatch,pyaws,planes,init_matches,parameters,yawerr)
viter[0] = matches['viter']
if matches['numi'] == 0 or matches['hconf'] == 0:
ntry = 2
parameters = ( wRq, wRd, yaw, np.nan, runflag, 3*param['inlier_error'], param['ransac_iter'], 10, True )
matches, pose, planechose = solveNorm(C,Q,dbmatch,pyaws,planes,init_matches,parameters,yawerr)
viter[1] = matches['viter']
if matches['numi'] == 0 or matches['hconf'] == 0:
ntry, planechose = 3, 0
parameters = ( wRq, wRd, yaw, np.nan, 7, 3*param['inlier_error'], param['ransac_iter'], 10, True )
matches, pose = solveYaw(init_matches,parameters,yawerr)
viter[2] = matches['viter']
if matches['numi'] == 0 or matches['hconf'] == 0:
ntry, planechose = 4, 0
# save matches to disk
matches_file = os.path.join(param['resultsdir'],'matches_'+qname+'.pkl')
matches_out = open(matches_file,'wb')
pickle.dump(matches,matches_out)
matches_out.close()
# extract pose parameters
comp_runflag = matches['runflag']
tray = pose[:3]
comp_yaw = pose[3]
comp_pyaw = pose[4] if runflag>=0 else np.nan
scale = pose[5] if runflag>=0 else np.nan
# Get scaled translation for query location
if np.isnan(scale):
wRq_pr = geom.YPRfromR(wRq)[1:]
comp_wRq = geom.RfromYPR(comp_yaw, wRq_pr[0], wRq_pr[1])
qloc = scalefrom3d(matches, tray, comp_wRq)[0]
else: # scale calculated in RANSAC loop
qloc = scale*tray
# temporarily get yaw error
qyaw_error = np.round(abs(np.mod(tyaw-comp_yaw,360)))
qyaw_error = qyaw_error if qyaw_error<180 else abs(qyaw_error-360)
# compute location errors wrt estimated query locations
loc_err = ( (qloc[0]-qzx[1])**2 + (qloc[2]-qzx[0])**2 )**0.5
gps_err = ( (gzx[1] -qzx[1])**2 + (gzx[0] -qzx[0])**2 )**0.5
# compute the angle difference between T and ground truth translation
tyaw_error = np.round(abs( 180/np.pi * np.arccos( np.abs(qloc[0]*qzx[1]+qloc[2]*qzx[0]) / (alg.norm([qloc[0],qloc[2]])*alg.norm(qzx)) ) ))
# compute the plane normal angle error
nyaw_error = np.nan if np.isnan(comp_pyaw) or np.isnan(tnorm) else np.mod(np.round(abs(comp_pyaw-tnorm)),180)
nyaw_error = nyaw_error if nyaw_error<90 else abs(nyaw_error-180)
# write pose estimation results to file
yaw_err = np.nan
pose_file = os.path.join(param['resultsdir'],param['pose_file'])
with open(pose_file,'a') as pf:
print >>pf, '\t'.join([qname, str(loc_err), str(gps_err), \
str(tyaw_error), str(qyaw_error), str(nyaw_error), str(matches['numi']), \
str(matches['numq']), str(matches['nmat']), str(matches['hconf']), \
str(qloc[0]), str(qloc[2]), str(yaw_err), str(runflag)])
# print post-homography data to file
with open(Q.datafile,'a') as df:
print >>df, ''
print >>df, '------------------'
print >>df, ''
if ntry==1: print >>df, 'Homography solution using low error threshold with restrictions.'
elif ntry==2: print >>df, 'Homography solution using high error threshold with restrictions.'
else: print >>df, 'Solution not found. Setting T=0.'
if planechose==0: print >>df, 'Solution formed with unset plane normal.'
else: 'Solution chosen with plane normal %d chosen.' % planechose
print >>df, 'VP yaw | Computed yaw | Actual Yaw | Error : %3.0f | %3.0f | %3.0f | %3.0f' % (vyaw,comp_yaw,tyaw,qyaw_error)
print >>df, 'Computed Normal | Actual Normal | Error : %3.0f | %3.0f | %3.0f' % (comp_pyaw,tnorm,nyaw_error)
print >>df, 'Translation (x|y|z): %.1f | %.1f | %.1f' % (qloc[0],qloc[1],qloc[2])
print >>df, 'True position (x|-|z): %.1f | - | %.1f' % (qzx[1],qzx[0])
print >>df, 'Angle error | Location error: %.0f | %.1f' % (tyaw_error,loc_err)
print >>df, 'Number of Inliers | Total matches | Ratio: %d | %d | %.2f' % (matches['numi'],matches['nmat'],np.nan if matches['nmat']==0 else float(matches['numi'])/matches['nmat'])
print >>df, 'Reprojection error | Homography confidence: %.3f | %.1f' % (matches['rperr'],matches['hconf'])
print >>df, 'Valid Homographies | Iterations | Ratio: %d | %d | %.3f' % (matches['viter'],matches['niter'],np.nan if matches['niter']==0 else float(matches['viter'])/matches['niter'])
print >>df, ''
print >>df, '------------------'
print >>df, ''
booleans = [ loc_err<5, loc_err<10, not(5<np.mod(vyaw-tyaw,360)<355), \
not(10<np.mod(vyaw-tyaw,360)<350), \
not(5<np.mod(comp_yaw-tyaw,360)<355), ntry==1, ntry!=3, \
planechose!=0, matches['nmat']!=matches_start['nmat'], \
0 if planechose==0 else pconfs[planechose-1]>0, comp_yaw-vyaw ]
print >>df, '|'.join(['%.0f' % (b) for b in booleans])
# draw matches
close = int(loc_err<5) + int(loc_err<10)
yawclose = int(not(5<np.mod(vyaw-tyaw,360)<355)) + int(not(10<np.mod(vyaw-tyaw,360)<350))
imgpath = os.path.join( param['resultsdir'] , qname + ';locerr=%.2f' % (loc_err) + ';locPerf_' + str(close) \
+ ';yawPerf_' + str(yawclose) + ';nplanes_' + str(len(pyaws)) + ';plane_' + str(planes[planechose]) + ';try_' + str(ntry) \
+ ';tAng=%.0f' % (tyaw_error) + ';qAng=%.0f' % (qyaw_error) + ';nAng=%.0f' % (nyaw_error) + ';' + dbmatch + '.jpg')
draw_matches(matches, qimg, dbimg, imgpath)
# imgpath = os.path.join( param['resultsdir'] , 'homography;' + qname + ';' + dbmatch + '.jpg')
# draw_hom(matches, pose, wRq, wRd, Kq, Kd, qimg, dbimg, imgpath)
if C.QUERY == 'oakland1': C.pose_param['draw_tags'] = False
if C.pose_param['draw_tags']: draw_tags(C, Q, matches, pose, dbmatch, olat, olon, Kd, Kq)
print 'Computed yaw / ground truth yaw : %.0f / %.0f' % (comp_yaw,tyaw)
if runflag < 10: print 'Computed normal bearing / ground truth : %.0f / %.0f' % (comp_pyaw,tnorm)
print 'Computed query location relative to db : %.1f, %.1f, %.1f' % tuple(qloc)
print 'Ground truth query location relative to db : %.1f, - , %.1f' % (qzx[1],qzx[0])
input = (wRq, wRd)
return qloc, loc_err, matches, input
def VPfrom2Lines(lineqs):
nlines = lineqs.shape[0]
i0 = rnd.randint(0,nlines)
i1 = rnd.randint(0,nlines-1)
i1 = i1+1 if i1>=i0 else i1
return geom.normalrows(np.cross(lineqs[i0,:],lineqs[i1,:]))