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