def genView(panopath, outpath, orientation, viewdir, detail):
# panopath: location of raster, depth and plane_pano images
# outpath: location to put generated view, depth, and plane images
# orientation: [yaw, pitch, roll] of panorama (in degrees)
# viewdir: clock-based view; in set [2,3,4,8,9,10] for database
# detail: 0 = 768x512 with 60d fov, 1 = 2500x1200 with 90d fov
# local constants
start = time.time()
pi = np.pi
width, height, fov = (2500, 1200, 90) if detail else (768, 512, 60)
# view details
Rpano = geom.RfromYPR(orientation[0],orientation[1],orientation[2])
Yview = np.mod( orientation[0] + 30*viewdir, 360 )
Rview = geom.RfromYPR(Yview, 0, 0)
Kview = geom.cameramat(width, height, fov*pi/180)
Kinv = alg.inv(Kview)
# Load image pano, depth pano, and plane pano images
cvIP = cv.LoadImageM( os.path.join(panopath,'raster.jpg'), cv.CV_LOAD_IMAGE_UNCHANGED )
cvDP = cv.fromarray( np.asarray( Image.open( os.path.join(panopath,'depth.png') ) ) )
pp = np.asarray( Image.open( os.path.join(panopath,'plane_pano.png') ) ).copy()
vals = set(list(pp.reshape(-1)))
vals.remove(255)
gnd = max(vals)
pp[pp==gnd] = np.uint8(0)
cvPP = cv.fromarray(pp)
# load pixel map
pix = np.append(np.array(np.meshgrid(range(width),range(height))).reshape(2,-1),np.ones([1,width*height]),0)
midpoint = time.time()
print 'Loading pano images took ' + str(midpoint-start) + ' seconds.'
# Create output openCV matrices
cvI = cv.CreateMat(height,width,cv.CV_8UC3)
cvD = cv.CreateMat(height,width,cv.CV_32SC1)
cvP = cv.CreateMat(height,width,cv.CV_8UC1)
# compute mappings
ray = np.dot( np.transpose(Rpano), np.dot( Rview, np.dot( Kinv, pix ) ) )
yaw, pitch = np.arctan2( ray[0,:] , ray[2,:] ) , np.arctan2( -ray[1,:] , np.sqrt((np.array([ray[0,:],ray[2,:]])**2).sum(0)) )
ix, iy = cv.fromarray(np.array(8192/(2*pi)*(pi+yaw),np.float32).reshape(height,width)), cv.fromarray(np.array(4096/pi*(pi/2-pitch),np.float32).reshape(height,width))
dx, dy = cv.fromarray(np.array(5000/(2*pi)*(pi+yaw),np.float32).reshape(height,width)), cv.fromarray(np.array(2500/pi*(pi/2-pitch),np.float32).reshape(height,width))
px, py = cv.fromarray(np.array(1000/(2*pi)*(pi+yaw),np.float32).reshape(height,width)), cv.fromarray(np.array( 500/pi*(pi/2-pitch),np.float32).reshape(height,width))
# call remap function
cv.Remap(cvIP,cvI,ix,iy,cv.CV_INTER_CUBIC)
cv.Remap(cvDP,cvD,dx,dy,cv.CV_INTER_NN)
cv.Remap(cvPP,cvP,px,py,cv.CV_INTER_NN)
# write images to file
Image.fromarray(np.array(cvI)[:,:,[2,1,0]]).save(os.path.join(outpath,'view.jpg'),'jpeg')
Image.fromarray(np.array(cvD)).save(os.path.join(outpath,'depth.png'),'png')
Image.fromarray(np.array(cvP)).save(os.path.join(outpath,'plane.png'),'png')
print 'Generating views from pano took ' + str(time.time()-midpoint) + ' seconds.'
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 viewparam(source,tyaw):
# camera calibration matrix
Kcal = geom.cameramat(source.image.size[0], source.image.size[1], source.fov)
# camera orientation (camera to world)
yaw = source.yaw if np.isnan(tyaw) else tyaw
Rot = geom.RfromYPR(yaw, source.pitch, source.roll)
return Kcal, Rot
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 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 homerrf_tq(prm,qray,dray,ddep,pr,wRd,nbear):
# set variables
dRq = np.dot(tp(wRd),geom.RfromYPR(prm[3],pr[0],pr[1]))
td = np.dot(tp(wRd),prm[:3])
nd = -np.dot(tp(wRd),[np.sin(nbear*np.pi/180),0,np.cos(nbear*np.pi/180)])
H = np.dot(tp(dRq),np.eye(3,3)-np.outer(td,nd))
# Compute homography error
Hd = tp(np.dot(H,tp(dray)))
err = np.append( qray[:,0]/qray[:,2]-Hd[:,0]/Hd[:,2] , qray[:,1]/qray[:,2]-Hd[:,1]/Hd[:,2] )
return err
def homerrf_dtq(prm,qray,dray,ddep,pr,wRd,nbear):
# set variables
dRq = np.dot(tp(wRd),geom.RfromYPR(prm[3],pr[0],pr[1]))
td = np.dot(tp(wRd),prm[:3])
nd = -np.dot(tp(wRd),[np.sin(nbear*np.pi/180),0,np.cos(nbear*np.pi/180)])
pd = prm[4]
H = np.dot(tp(dRq),np.eye(3,3)-np.outer(td,nd))
# Compute homography error
Hd = tp(np.dot(H,tp(dray)))
err = np.concatenate( [ qray[:,0]/qray[:,2]-Hd[:,0]/Hd[:,2] ,
qray[:,1]/qray[:,2]-Hd[:,1]/Hd[:,2] ,
alpha() * ( pd - ddep*np.inner(dray,nd) ) / pd ] )
return err
def solveYaw(matches, parameters, yawerr):
wRq, wRd, yaw, pyaw, rf, inlerr, rsiter, minI, yrestrict = parameters
pr = geom.YPRfromR(wRq)[1:]
dy = 5
dyawlimit = int(yawerr / dy) * dy
dyaws = range(-dyawlimit, dyawlimit + dy, dy)
yaw_matches, yaw_poses, yinls, yerrs, yconfs = [], [], np.array(
[]), np.array([]), np.array([])
for dyaw in dyaws:
yaw_wRq = geom.RfromYPR(yaw + dyaw, pr[0], pr[1])
runflag = rf - np.isnan(pyaw)
pyaw_in = pyaw + dyaw if matches['domplane'] == -1 else pyaw
parameters = (yaw_wRq, wRd, yaw + dyaw, pyaw_in, runflag, inlerr,
rsiter, minI, yrestrict)
yaw_matches.append(matches.copy())
print 'Query yaw set to %.0f degrees' % (yaw + dyaw)
m, p = solveHom(yaw_matches[-1], parameters)
tray = p[:3]
wRq_pr = geom.YPRfromR(wRq)[1:]
comp_wRq = geom.RfromYPR(p[3], wRq_pr[0], wRq_pr[1])
if np.isnan(p[5]): p[5] = scalefrom3d(m, tray, comp_wRq)[1]
if abs(p[5]) > 75:
m['numi'], m['ifrc'], m['iconf'], m['hconf'], m[
'rperr'] == 0, 0, 0, 0, np.inf
p[:] = 0
p[3:5] = np.nan
yinls = np.append(yinls, m['iconf'])
yerrs = np.append(yerrs, m['rperr'])
yconfs = np.append(yconfs, m['hconf'])
yaw_matches[-1] = m
yaw_poses.append(p)
ifrc = np.max(yinls)
mask = yinls > 0.95 * ifrc
yerrs[np.logical_not(mask)] = np.inf
maskidx = np.argmin(yerrs)
return yaw_matches[maskidx], yaw_poses[maskidx]
def solveNorm(C, Q, dbmatch, pyaws, planes, matches, parameters, yawerr):
wRq, wRd, yaw, pyaw, runflag, inlerr, rsiter, minI, yrestrict = parameters
if len(pyaws) == 0:
m, p = matches, np.zeros(6)
m['numi'], m['hconf'], m['viter'], p[3:5] = 0, 0, 0, np.nan
return m, p, 0
init_nmat = matches['nmat']
rf = 3 if runflag == 10 else 7
plane_matches, plane_poses, hinls, herrs, hconfs = [], [], np.array(
[]), np.array([]), np.array([])
for pyaw, plane in zip(*(pyaws, planes)):
parameters = (wRq, wRd, yaw, pyaw, rf, inlerr, rsiter, minI, yrestrict)
plane_matches.append(
match_info(C, Q, matches.copy(), dbmatch, wRd, plane))
if plane != -1 and plane_matches[-1][
'nmat'] < 0.3 * init_nmat: # don't allow planes to restrict features beyond this point
print 'Not using plane because it restricts features too greatly.'
plane_matches[-1]['nmat'] = 0
m = plane_matches[-1]
p = np.zeros(6)
p[3:5] = np.nan
plane_poses.append(p)
hinls = np.append(hinls, m['iconf'])
herrs = np.append(herrs, m['rperr'])
hconfs = np.append(hconfs, m['hconf'])
continue
print 'Normal yaw set to %.0f degrees' % pyaw
m, p = solveYaw(plane_matches[-1], parameters, yawerr)
tray = p[:3]
wRq_pr = geom.YPRfromR(wRq)[1:]
comp_wRq = geom.RfromYPR(p[3], wRq_pr[0], wRq_pr[1])
if np.isnan(p[5]): p[5] = scalefrom3d(m, tray, comp_wRq)[1]
if abs(p[5]) > 75:
m['numi'], m['ifrc'], m['iconf'], m['hconf'], m[
'rperr'] == 0, 0, 0, 0, np.inf
p[:] = 0
p[3:5] = np.nan
hinls = np.append(hinls, m['iconf'])
herrs = np.append(herrs, m['rperr'])
hconfs = np.append(hconfs, m['hconf'])
plane_matches[-1] = m
plane_poses.append(p)
idx = np.argmax(hinls)
return plane_matches[idx], plane_poses[idx], idx
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 extract_panorama(panopath, outbase, panoinfo, detail):
"""Generates raster, plane, and depth views at rot_degrees"""
print "Processing panorama " + '%d' % panoinfo['pano'][0]
# panopath: location of raster, depth and plane_pano images
# outbase: base name to put generated view, depth, and plane images
# panoinfo: Contains information about the panoramic scene
# detail: 0 = 768x512 with 60d fov, 1 = 2500x1200 with 90d fov
# local constants
pi = np.pi
width, height, fov = (2500, 1200, 90) if detail else (768, 512, 60)
# pano and view details details
orientation = [
panoinfo['yaw'][0], panoinfo['pitch'][0], panoinfo['roll'][0]
]
Rpano = geom.RfromYPR(orientation[0], orientation[1], orientation[2])
Kview = geom.cameramat(width, height, fov * pi / 180)
Kinv = alg.inv(Kview)
# Load image pano, depth pano, and plane pano images
cvIP = cv.LoadImageM(os.path.join(panopath, 'raster.jpg'),
cv.CV_LOAD_IMAGE_UNCHANGED)
cvDP = cv.fromarray(
np.asarray(Image.open(os.path.join(panopath, 'depth.png'))))
pp = np.asarray(Image.open(os.path.join(panopath,
'plane_pano.png'))).copy()
vals = set(list(pp.reshape(-1)))
vals.remove(255)
gnd = max(vals)
pp[pp == gnd] = np.uint8(0)
cvPP = cv.fromarray(pp)
# load pixel map
pix = np.append(
np.array(np.meshgrid(range(width), range(height))).reshape(2, -1),
np.ones([1, width * height]), 0)
# Create output openCV matrices
cvI = cv.CreateMat(height, width, cv.CV_8UC3)
cvD = cv.CreateMat(height, width, cv.CV_32SC1)
cvP = cv.CreateMat(height, width, cv.CV_8UC1)
for viewdir in [2, 3, 4, 8, 9, 10]:
# add to base name and generate info file
viewname = outbase + '%04d' % viewdir
gen_info_file(panoinfo, viewname + '.info', detail, 30 * viewdir)
# generate view orientation
Yview = np.mod(orientation[0] + 30 * viewdir, 360)
Rview = geom.RfromYPR(Yview, 0, 0)
# compute mappings
ray = np.dot(np.transpose(Rpano), np.dot(Rview, np.dot(Kinv, pix)))
yaw, pitch = np.arctan2(ray[0, :], ray[2, :]), np.arctan2(
-ray[1, :], np.sqrt((np.array([ray[0, :], ray[2, :]])**2).sum(0)))
ix, iy = cv.fromarray(
np.array(8192 / (2 * pi) * (pi + yaw),
np.float32).reshape(height, width)), cv.fromarray(
np.array(4096 / pi * (pi / 2 - pitch),
np.float32).reshape(height, width))
dx, dy = cv.fromarray(
np.array(5000 / (2 * pi) * (pi + yaw),
np.float32).reshape(height, width)), cv.fromarray(
np.array(2500 / pi * (pi / 2 - pitch),
np.float32).reshape(height, width))
px, py = cv.fromarray(
np.array(1000 / (2 * pi) * (pi + yaw),
np.float32).reshape(height, width)), cv.fromarray(
np.array(500 / pi * (pi / 2 - pitch),
np.float32).reshape(height, width))
# call remap function
cv.Remap(cvIP, cvI, ix, iy, cv.CV_INTER_CUBIC
) # if detail else cv.Remap(cvIP,cvI,ix,iy,cv.CV_INTER_AREA)
cv.Remap(cvDP, cvD, dx, dy, cv.CV_INTER_NN)
cv.Remap(cvPP, cvP, px, py, cv.CV_INTER_NN)
# write images to file
Image.fromarray(np.array(cvI)[:, :,
[2, 1, 0]]).save(viewname + '.jpg',
'jpeg')
Image.fromarray(np.array(cvD)).save(viewname + '-depth.png', 'png')
Image.fromarray(np.array(cvP)).save(viewname + '-planes.png', 'png')
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 alignVPcost(dyaw,vpmat1,vpmat2,weights):
dR = geom.RfromYPR(dyaw[0],0,0)
matchdot = np.sum( vpmat1*tp(np.dot(dR,tp(vpmat2))) , 1 )
yaw_weight = 1 # np.cos(np.pi/180*dyaw[0])
return -yaw_weight*np.sum(weights*matchdot)
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
