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_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 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 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 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 LfromVP(vp,midpts,lineqs,lengths,maxangle):
# get mask of those within angle tolerance
line_bear = np.arctan(lineqs[:,0]/lineqs[:,1])
vp_bear = np.arctan( (midpts[:,1]-vp[1]/vp[2])/(vp[0]/vp[2]-midpts[:,0]) )
dbear = np.mod(line_bear-vp_bear,np.pi)
dbear = dbear + (dbear>np.pi/2) * (np.pi-2*dbear)
anglemask = dbear < maxangle*np.pi/180
# get mask of those that don't "cross" vanishing point
vpdist = geom.vecnorm(geom.vecsub(midpts,vp/vp[2],1))
crossmask = vpdist > lengths/2
return np.logical_and(anglemask,crossmask)
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
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 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 VPfromSeeds(midpts, lineqs, lengths, Rot, tol):
# Generate seeds from rotation matrix
total_len = np.sum(lengths)
seed_tol = 2*tol
yaw, Rpr = geom.YfromR(Rot)
angles = np.arange(0,180,3)
nvps = len(angles)
vps, vlens = np.zeros((nvps,3)), np.zeros(nvps)
for i in xrange(nvps):
vps[i,:] = np.dot( tp(Rpr) , [np.sin(np.pi/180*angles[i]),0,np.cos(np.pi/180*angles[i])] )
# Iterate through each line and assign its weight to top N seeds
nseeds = 3
for i in xrange(len(lengths)):
midpt, lineq, length = midpts[i,:], lineqs[i,:], lengths[i]
line_bear = np.arctan(lineq[0]/lineq[1])
vp_bear = np.arctan( (midpt[1]-vps[:,1]/vps[:,2]) / (vps[:,0]/vps[:,2]-midpt[0]) )
dbear = np.mod(line_bear-vp_bear,np.pi)
dbear = dbear + (dbear>np.pi/2) * (np.pi-2*dbear)
vpdist = geom.vecnorm(geom.vecsub(geom.vecdiv(vps,vps[:,2],0),midpt,1))
mask = vpdist < length/2
dbear[mask] = np.pi
minidx = np.argsort(dbear)[:nseeds]
vlens[minidx] += length/total_len
# Pick true vanishing points from seeds
# if True:
# file = '/media/DATAPART2/ah/pose_runs/tmp.txt'
# open(file,'w').close()
# with open(file,'a') as f:
# for tmp in vlens: print >>f, '%.3f' % tmp
seedlens = vlens
neighbors = np.amax([np.roll(vlens,-3),np.roll(vlens,-2),np.roll(vlens,-1), \
np.roll(vlens,1),np.roll(vlens,2),np.roll(vlens,3)],0)
localmax = vlens > neighbors
random_fraction = float(nseeds) / len(angles)
lengthmask = vlens > 1.5*random_fraction
vpmask = np.logical_and(localmax,lengthmask)
vps, vlens, nvps = vps[vpmask,:], vlens[vpmask], np.sum(vpmask)
# Guided matching to refine the vanishing points
vcents = np.zeros((nvps,3))
maxiter = 10
for i in xrange(nvps):
vp, vlen = vps[i,:], vlens[i]
gmtol, gmit, llen = tol, 0, 0
while vlen!=llen and gmit<maxiter:
gmit += 1
linemask = LfromVP(vp,midpts,lineqs,lengths,gmtol)
vp = VPfromLines(lineqs[linemask,:])
vlen, llen = np.sum(lengths[linemask]), vlen
vcent = np.mean(midpts[linemask,:],0)
vps[i,:], vlens[i], vcents[i,:] = vp, vlen, vcent
vlens = vlens / total_len
# eliminate vanishing points without a significant contribution from lines
keepmask = vlens > 5/90 # "random" line length expected
vps, vlens, vcents = vps[keepmask,:], vlens[keepmask], vcents[keepmask,:]
# adjust the sign of vanishing points so that normal associated with vp cross down faces toward camera
for i in range(len(vlens)):
vpnorm = np.cross( vps[i,:] , np.dot(tp(Rpr),[0,1,0]) )
if vpnorm[2] > 0:
vps[i,:] *= -1
return vps, vlens, vcents, seedlens
def bestH_d(prm,numi,minI,iconf,bconf):
dist = prm[-1]*geom.vecnorm(prm[:3])
ty = prm[-1]*prm[1]
return numi>=minI and iconf>bconf and dist<75 and abs(ty-2)<3
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
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
