Пример #1
0
        def euclidean_error_function(arr):
            error = 0.0
            ctr = 0
            buf = []
            cbuf = []
            for m in matches:
                d, q = m['db'], m['query']
                feature = map3d[int(d[0]), int(d[1])]
                if not feature:
                    continue
                ctr += 1
                pz, px = geom.lltom(self.lat + meter * arr[0],
                                    self.lon + meter * arr[1], feature['lat'],
                                    feature['lon'])
                py = feature['alt']  # feature['alt'] - arr[2]
                x, y, z = geom.camera_transform(px, py, pz, self.pitch,
                                                self.yaw, self.roll)
                coord = geom.project2d(x, y, z, self.focal_length)
                pixel = geom.center(coord, self.imsize)
                error += abs(pixel[0] - q[0])
                buf.append((pixel[0], pixel[1]))
                cbuf.append((q[0], q[1]))
#      for i,j in buf:
#        print i,j
#      print
#      for i,j in cbuf:
#        print i,j
            if error < best[0]:
                best[0] = min(error, best[0])
#        print best[0]/ctr
            return error / ctr
Пример #2
0
    def euclidean_error_function(arr):
      error = 0.0
      ctr = 0
      buf = []
      cbuf = []
      for m in matches:
        d, q = m['db'], m['query']
        feature = map3d[int(d[0]), int(d[1])]
        if not feature:
          continue
        ctr += 1
        pz, px = geom.lltom(self.lat + meter*arr[0], self.lon + meter*arr[1], feature['lat'], feature['lon'])
        py = feature['alt'] # feature['alt'] - arr[2]
        x, y, z = geom.camera_transform(px, py, pz, self.pitch, self.yaw, self.roll)
        coord = geom.project2d(x, y, z, self.focal_length)
        pixel = geom.center(coord, self.imsize)
        error += abs(pixel[0] - q[0])
        buf.append((pixel[0], pixel[1]))
        cbuf.append((q[0], q[1]))
#      for i,j in buf:
#        print i,j
#      print
#      for i,j in cbuf:
#        print i,j
      if error < best[0]:
        best[0] = min(error, best[0])
#        print best[0]/ctr
      return error/ctr
Пример #3
0
def solve(C, Q, matches, dbsiftpath, dbimgpath):

  # open EarthMine info
  info = os.path.join(C.infodir, os.path.basename(dbimgpath)[:-4] + '.info')
  em = render_tags.EarthmineImageInfo(dbimgpath, info)
  map3d = C.pixelmap.open(dbsiftpath)

  # find non-None features
  vector = []
  for i, m in enumerate(matches):
    d = m['db']

    # get 3d pt of feature
    feature = map3d[int(d[0]), int(d[1])]
    if not feature:
      continue

    # convert from latlon to meters relative to earthmine camera
    pz, px = geom.lltom(em.lat, em.lon, feature['lat'], feature['lon'])
    py = feature['alt'] - em.alt
    vector.append((m['query'][:2], (px, py, -pz)))

  print vector[0]

  # reference camera matrix
  # f 0 0
  # 0 f 0
  # 0 0 1
  A = cv.CreateMat(3, 3, cv.CV_64F)
  cv.SetZero(A)
  f = 662 # focal len?
  cv.Set2D(A, 0, 0, cv.Scalar(f))
  cv.Set2D(A, 1, 1, cv.Scalar(f))
  cv.Set2D(A, 2, 2, cv.Scalar(1))

  # convert vector to to cvMats
  objectPoints3d = cv.CreateMat(len(vector), 1, cv.CV_64FC3)
  imagePoints2d = cv.CreateMat(len(vector), 1, cv.CV_64FC2)
  for i, (p2d, p3d) in enumerate(vector):
    cv.Set2D(imagePoints2d, i, 0, cv.Scalar(*p2d))
    cv.Set2D(objectPoints3d, i, 0, cv.Scalar(*p3d))

  coeff = cv.CreateMat(4, 1, cv.CV_64F)
  rvec = cv.CreateMat(3, 1, cv.CV_64F)
  tvec = cv.CreateMat(3, 1, cv.CV_64F)
  cv.SetZero(coeff)
  cv.SetZero(rvec)
  cv.SetZero(tvec)

  # since rvec, tvec are zero the initial guess is the earthmine camera
  ret = cv.FindExtrinsicCameraParams2(objectPoints3d, imagePoints2d, A,
    coeff, rvec, tvec, useExtrinsicGuess=False)
  np_rvec = np.matrix(rvec)
  np_tvec = np.matrix(tvec)
  print np_rvec
  print np_tvec
  return np_rvec, np_tvec
Пример #4
0
  def map_tags_camera(self, elat=0, elon=0, ep=0, ey=0, er=0):
    "Returns (tag, (dist, pixel)) pairs using camera transform."
    if not elat or not elon:
      elat = self.lat
      elon = self.lon
    tags = []
    possible_tags = self.get_frustum()

    for tag in possible_tags:
      pz, px = geom.lltom(elat, elon, tag.lat, tag.lon)
      py = tag.alt - self.alt;
      x, y, z = geom.camera_transform(px, py, pz, self.pitch + ep, self.yaw + ey, self.roll + er)
      coord = geom.project2d(x, y, z, self.source.focal_length)
      pixel = geom.center(coord, self.image.size)
#      tags.append((tag, (0, geom.constrain(pixel, self.image.size))))
      tags.append((tag, (0, pixel)))

    return tags
Пример #5
0
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
Пример #6
0
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
Пример #7
0
def qsolve(C, Q, matches, dbsiftpath, dbimgpath):

    # Get image information.
    info = os.path.join(C.infodir, os.path.basename(dbimgpath)[:-4] + '.info')
    qsource = render_tags.QueryImageInfo(Q.datasource)
    dbsource = render_tags.EarthmineImageInfo(dbimgpath, info)
    map3d = C.pixelmap.open(dbsiftpath)

    # Get 2d pixels and 3d locations of feature inliers
    matches = [(m['query'][0],m['query'][1],m['db'][0],m['db'][1]) for m in matches]
    matches = list(set(matches))
    q2d = [arr([m[0],m[1]]) for m in matches]
    db2d = [arr([m[2],m[3]]) for m in matches]
    db3d = [map3d[int(d[0]),int(d[1])] for d in db2d]
    i = 0
    while i < len(db3d):
        if db3d[i] is None:
            q2d.pop(i)
            db2d.pop(i)
            db3d.pop(i)
        else:
            i = i+1
    olat,olon,oalt = dbsource.lat,dbsource.lon,dbsource.alt
    qlat,qlon = qsource.lat,qsource.lon
    qzx = geom.lltom(olat,olon,qlat,qlon)
    zx = [geom.lltom(olat,olon,d['lat'],d['lon']) for d in db3d]
    y = [dbsource.alt-d['alt'] for d in db3d]
    xyz = [[zx[i][1],y[i],zx[i][0]] for i in range(len(y))]
    print len(xyz)

    # Set K, Rhat
    wx,wy = qsource.pgmsize[0], qsource.pgmsize[1]
    tx,ty = qsource.view_angle[0], qsource.view_angle[1]
    f1, f2 = (wx-1)/(2*np.tan(tx/2)), (wy-1)/(2*np.tan(ty/2))
    f = (f1+f2) / 2
    K = arr([[f,0,(wx-1)/2.0],
             [0,f,(wy-1)/2.0],
             [0,0,1]])
    y,p,r = qsource.yaw, qsource.pitch, qsource.roll
    print [180*y/np.pi,180*p/np.pi,180*r/np.pi]
    Ry = arr([[np.cos(y),0,np.sin(y)],
              [0,1,0],
              [-np.sin(y),0,np.cos(y)]])
    Rx = arr([[1,0,0],
              [0,np.cos(p),-np.sin(p)],
              [0,np.sin(p),np.cos(p)]])
    Rz = arr([[np.cos(r),-np.sin(r),0],
              [np.sin(r),np.cos(r),0],
              [0,0,1]])
    Rhat = dot(Ry,dot(Rx,Rz)) # camera orientation (camera to world)
    tRhat = tp(Rhat)

    # reference camera matrix
    # f 0 cx
    # 0 f cy
    # 0 0 1
    A = cv.CreateMat(3, 3, cv.CV_64F)
    cv.SetZero(A)
    cv.Set2D(A, 0, 0, cv.Scalar(f))
    cv.Set2D(A, 1, 1, cv.Scalar(f))
    cv.Set2D(A, 2, 2, cv.Scalar(1))
    cv.Set2D(A, 0, 2, cv.Scalar((wx-1)/2.0))
    cv.Set2D(A, 1, 2, cv.Scalar((wy-1)/2.0))

    # convert 2d, 3d points to cvMats
    objectPoints3d = cv.CreateMat(len(xyz), 1, cv.CV_64FC3)
    imagePoints2d = cv.CreateMat(len(xyz), 1, cv.CV_64FC2)
    for i in range(len(xyz)):
        cv.Set2D(imagePoints2d, i, 0, cv.Scalar(*q2d[i]))
        cv.Set2D(objectPoints3d, i, 0, cv.Scalar(*xyz[i]))

    # set initial rotation and translation vectors, distortion coefficients
    coeff = cv.CreateMat(4, 1, cv.CV_64F)
    cv.SetZero(coeff)
    rmat = cv.CreateMat(3, 3, cv.CV_64F)
    cv.Set2D(rmat, 0, 0, cv.Scalar(tRhat[0,0]))
    cv.Set2D(rmat, 0, 1, cv.Scalar(tRhat[0,1]))
    cv.Set2D(rmat, 0, 2, cv.Scalar(tRhat[0,2]))
    cv.Set2D(rmat, 1, 0, cv.Scalar(tRhat[1,0]))
    cv.Set2D(rmat, 1, 1, cv.Scalar(tRhat[1,1]))
    cv.Set2D(rmat, 1, 2, cv.Scalar(tRhat[1,2]))
    cv.Set2D(rmat, 2, 0, cv.Scalar(tRhat[2,0]))
    cv.Set2D(rmat, 2, 1, cv.Scalar(tRhat[2,1]))
    cv.Set2D(rmat, 2, 2, cv.Scalar(tRhat[2,2]))
    print 'YPR init for PnP'
    print 180*np.array(geom.ypr_fromR(Rhat))/np.pi
    rvec = cv.CreateMat(3, 1, cv.CV_64F)
    cv.SetZero(rvec)
    cv.Rodrigues2(rmat,rvec) # convert from rotation matrix to Rodrigues vector
    tvec = cv.CreateMat(3, 1, cv.CV_64F)
    cv.SetZero(tvec)

    # solvepnp
    ret = cv.FindExtrinsicCameraParams2(objectPoints3d, imagePoints2d, A,
        coeff, rvec, tvec, useExtrinsicGuess=False)
    np_rvec = np.matrix(rvec).A
    cv.Rodrigues2(rvec,rmat)
    np_rmat = np.transpose(np.matrix(rmat)).A
    np_tvec = np.dot(np_rmat,np.squeeze(np.matrix(tvec).A))
    print 'Rq from PnP'
    print np_rmat
    print 'YPR from PnP'
    print 180*np.array(geom.ypr_fromR(np_rmat))/np.pi

    return np_rmat, np_tvec
Пример #8
0
def dbsolve(C, Rhat, matches, dbsiftpath, dbimgpath):

    # Get image information.
    info = os.path.join(C.infodir, os.path.basename(dbimgpath)[:-4] + '.info')
    dbsource = render_tags.EarthmineImageInfo(dbimgpath, info)
    map3d = C.pixelmap.open(dbsiftpath)

    # Get 2d pixels and 3d locations of feature inliers
    matches = [(m['query'][0],m['query'][1],m['db'][0],m['db'][1]) for m in matches]
    matches = list(set(matches))
    db2d = [arr([m[2],m[3]]) for m in matches]
    db3d = [map3d[int(d[0]),int(d[1])] for d in db2d]
    i = 0
    while i < len(db3d):
        if db3d[i] is None:
            db2d.pop(i)
            db3d.pop(i)
        else:
            i = i+1
    olat,olon,oalt = dbsource.lat,dbsource.lon,dbsource.alt
    zx = [geom.lltom(olat,olon,d['lat'],d['lon']) for d in db3d]
    y = [dbsource.alt-d['alt'] for d in db3d]
    xyz = [[zx[i][1],y[i],zx[i][0]] for i in range(len(y))]

    # reference camera matrix Kd
    wx,wy = dbsource.image.size
    fov = dbsource.fov
    f = (wx-1)/(2*np.tan(fov/2))
    # f 0 cx
    # 0 f cy
    # 0 0 1
    A = cv.CreateMat(3, 3, cv.CV_64F)
    cv.SetZero(A)
    cv.Set2D(A, 0, 0, cv.Scalar(f))
    cv.Set2D(A, 1, 1, cv.Scalar(f))
    cv.Set2D(A, 2, 2, cv.Scalar(1))
    cv.Set2D(A, 0, 2, cv.Scalar((wx-1)/2.0))
    cv.Set2D(A, 1, 2, cv.Scalar((wy-1)/2.0))

    # convert 2d, 3d points to cvMats
    objectPoints3d = cv.CreateMat(len(xyz), 1, cv.CV_64FC3)
    imagePoints2d = cv.CreateMat(len(xyz), 1, cv.CV_64FC2)
    for i in range(len(xyz)):
        cv.Set2D(imagePoints2d, i, 0, cv.Scalar(*db2d[i]))
        cv.Set2D(objectPoints3d, i, 0, cv.Scalar(*xyz[i]))

    # set initial rotation and translation vectors, distortion coefficients
    coeff = cv.CreateMat(4, 1, cv.CV_64F)
    cv.SetZero(coeff)
    rmat = cv.CreateMat(3, 3, cv.CV_64F)
    tRhat = tp(Rhat)
    cv.Set2D(rmat, 0, 0, cv.Scalar(tRhat[0,0]))
    cv.Set2D(rmat, 0, 1, cv.Scalar(tRhat[0,1]))
    cv.Set2D(rmat, 0, 2, cv.Scalar(tRhat[0,2]))
    cv.Set2D(rmat, 1, 0, cv.Scalar(tRhat[1,0]))
    cv.Set2D(rmat, 1, 1, cv.Scalar(tRhat[1,1]))
    cv.Set2D(rmat, 1, 2, cv.Scalar(tRhat[1,2]))
    cv.Set2D(rmat, 2, 0, cv.Scalar(tRhat[2,0]))
    cv.Set2D(rmat, 2, 1, cv.Scalar(tRhat[2,1]))
    cv.Set2D(rmat, 2, 2, cv.Scalar(tRhat[2,2]))
    rvec = cv.CreateMat(3, 1, cv.CV_64F)
    cv.SetZero(rvec)
    cv.Rodrigues2(rmat,rvec) # convert from rotation matrix to Rodrigues vector
    tvec = cv.CreateMat(3, 1, cv.CV_64F)
    cv.SetZero(tvec)

    #print Rhat
    # solvepnp
    ret = cv.FindExtrinsicCameraParams2(objectPoints3d, imagePoints2d, A,
        coeff, rvec, tvec, useExtrinsicGuess=False)
    np_rvec = np.matrix(rvec).A
    cv.Rodrigues2(rvec,rmat)
    np_rmat = np.transpose(np.matrix(rmat)).A
    np_tvec = np.dot(np_rmat,np.squeeze(np.matrix(tvec).A))

    return np_rmat, np_tvec