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 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 lsqE_tq(prm, qray, dray, constants, domidx): Rpr, wRd, qYaw = constants pr = geom.YPRfromR(Rpr)[1:] # pitch and roll return opt.leastsq(esserrf_tq, prm, args=(qray, dray, pr, wRd, domidx), warning=False)[0]
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 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 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 errH_dtq(prm,qray,dray,ddep,constants): Rpr, wRd, qYaw, nYaw = constants pr = geom.YPRfromR(Rpr)[1:] # pitch and roll return np.sqrt(np.sum(np.reshape(homerrf_dtq(prm,qray,dray,ddep,pr,wRd,nYaw),[3,-1])**2,0))
def lsqH_dtq(prm,qray,dray,ddep,constants): Rpr, wRd, qYaw, nYaw = constants pr = geom.YPRfromR(Rpr)[1:] # pitch and roll return opt.leastsq(homerrf_dtq,prm,args=(qray,dray,ddep,pr,wRd,nYaw),warning=False)[0]
def lsqH_dtqn(prm,qray,dray,ddep,constants): Rpr, wRd, qYaw, nYaw = constants pr = geom.YPRfromR(Rpr)[1:] # pitch and roll try: return opt.leastsq(homerrf_dtqn,prm,args=(qray,dray,ddep,pr,wRd),warning=False)[0] except TypeError: return prm
def errE_tq(prm, qray, dray, constants, domidx): Rpr, wRd, qYaw = constants pr = geom.YPRfromR(Rpr)[1:] # pitch and roll return np.abs(esserrf_tq(prm, qray, dray, pr, wRd, domidx))