parameters: (traj_gt,traj_est,param_max_pairs=10000,param_fixed_delta=False,param_delta=1.00,param_delta_unit="s",param_offset=0.00,param_scale=1.00)
return_statement: return result
docstring: Compute the relative pose error between two trajectories.
      
      Input:
      traj_gt -- the first trajectory (ground truth)
      traj_est -- the second trajectory (estimated trajectory)
      param_max_pairs -- number of relative poses to be evaluated
      param_fixed_delta -- false: evaluate over all possible pairs
                           true: only evaluate over pairs with a given distance (delta)
      param_delta -- distance between the evaluated pairs
      param_delta_unit -- unit for comparison:
                          "s": seconds
                          "m": meters
                          "rad": radians
                          "deg": degrees
                          "f": frames
      param_offset -- time offset between two trajectories (to model the delay)
      param_scale -- scale to be applied to the second trajectory
      
      Output:
      list of compared poses and the resulting translation and rotation error
docstring_summary: Compute the relative pose error between two trajectories.
      
      Input:
      traj_gt -- the first trajectory (ground truth)
      traj_est -- the second trajectory (estimated trajectory)
      param_max_pairs -- number of relative poses to be evaluated
      param_fixed_delta -- false: evaluate over all possible pairs
                           true: only evaluate over pairs with a given distance (delta)
      param_delta -- distance between the evaluated pairs
      param_delta_unit -- unit for comparison:
                          "s": seconds
                          "m": meters
                          "rad": radians
                          "deg": degrees
                          "f": frames
      param_offset -- time offset between two trajectories (to model the delay)
      param_scale -- scale to be applied to the second trajectory
      
      Output:
      list of compared poses and the resulting translation and rotation error
docstring_tokens: Compute the relative pose error between two trajectories . Input : traj_gt -- the first trajectory ( ground truth ) traj_est -- the second trajectory ( estimated trajectory ) param_max_pairs -- number of relative poses to be evaluated param_fixed_delta -- false : evaluate over all possible pairs true : only evaluate over pairs with a given distance ( delta ) param_delta -- distance between the evaluated pairs param_delta_unit -- unit for comparison : s : seconds m : meters rad : radians deg : degrees f : frames param_offset -- time offset between two trajectories ( to model the delay ) param_scale -- scale to be applied to the second trajectory Output : list of compared poses and the resulting translation and rotation error
function: def evaluate_trajectory(traj_gt,traj_est,param_max_pairs=10000,param_fixed_delta=False,param_delta=1.00,param_delta_unit="s",param_offset=0.00,param_scale=1.00):
      """
      Compute the relative pose error between two trajectories.
      
      Input:
      traj_gt -- the first trajectory (ground truth)
      traj_est -- the second trajectory (estimated trajectory)
      param_max_pairs -- number of relative poses to be evaluated
      param_fixed_delta -- false: evaluate over all possible pairs
                           true: only evaluate over pairs with a given distance (delta)
      param_delta -- distance between the evaluated pairs
      param_delta_unit -- unit for comparison:
                          "s": seconds
                          "m": meters
                          "rad": radians
                          "deg": degrees
                          "f": frames
      param_offset -- time offset between two trajectories (to model the delay)
      param_scale -- scale to be applied to the second trajectory
      
      Output:
      list of compared poses and the resulting translation and rotation error
      """
      stamps_gt = list(traj_gt.keys())
      stamps_est = list(traj_est.keys())
      stamps_gt.sort()
      stamps_est.sort()
      
      stamps_est_return = []
      for t_est in stamps_est:
          t_gt = stamps_gt[find_closest_index(stamps_gt,t_est + param_offset)]
          t_est_return = stamps_est[find_closest_index(stamps_est,t_gt - param_offset)]
          t_gt_return = stamps_gt[find_closest_index(stamps_gt,t_est_return + param_offset)]
          if not t_est_return in stamps_est_return:
              stamps_est_return.append(t_est_return)
      if(len(stamps_est_return)<2):
          raise Exception("Number of overlap in the timestamps is too small. Did you run the evaluation on the right files?")
  
      if param_delta_unit=="s":
          index_est = list(traj_est.keys())
          index_est.sort()
      elif param_delta_unit=="m":
          index_est = distances_along_trajectory(traj_est)
      elif param_delta_unit=="rad":
          index_est = rotations_along_trajectory(traj_est,1)
      elif param_delta_unit=="deg":
          index_est = rotations_along_trajectory(traj_est,180/numpy.pi)
      elif param_delta_unit=="f":
          index_est = range(len(traj_est))
      else:
          raise Exception("Unknown unit for delta: '%s'"%param_delta_unit)
  
      if not param_fixed_delta:
          if(param_max_pairs==0 or len(traj_est)<numpy.sqrt(param_max_pairs)):
              pairs = [(i,j) for i in range(len(traj_est)) for j in range(len(traj_est))]
          else:
              pairs = [(random.randint(0,len(traj_est)-1),random.randint(0,len(traj_est)-1)) for i in range(param_max_pairs)]
      else:
          pairs = []
          for i in range(len(traj_est)):
              j = find_closest_index(index_est,index_est[i] + param_delta)
              if j!=len(traj_est)-1: 
                  pairs.append((i,j))
          if(param_max_pairs!=0 and len(pairs)>param_max_pairs):
              pairs = random.sample(pairs,param_max_pairs)
      
      gt_interval = numpy.median([s-t for s,t in zip(stamps_gt[1:],stamps_gt[:-1])])
      gt_max_time_difference = 2*gt_interval
      
      result = []
      for i,j in pairs:
          stamp_est_0 = stamps_est[i]
          stamp_est_1 = stamps_est[j]
  
          stamp_gt_0 = stamps_gt[ find_closest_index(stamps_gt,stamp_est_0 + param_offset) ]
          stamp_gt_1 = stamps_gt[ find_closest_index(stamps_gt,stamp_est_1 + param_offset) ]
          
          if(abs(stamp_gt_0 - (stamp_est_0 + param_offset)) > gt_max_time_difference  or
             abs(stamp_gt_1 - (stamp_est_1 + param_offset)) > gt_max_time_difference):
              continue
          
          error44 = ominus(  scale(
                             ominus( traj_est[stamp_est_1], traj_est[stamp_est_0] ),param_scale),
                             ominus( traj_gt[stamp_gt_1], traj_gt[stamp_gt_0] ) )
          
          trans = compute_distance(error44)
          rot = compute_angle(error44)
          
          result.append([stamp_est_0,stamp_est_1,stamp_gt_0,stamp_gt_1,trans,rot])
          
      if len(result)<2:
          raise Exception("Couldn't find matching timestamp pairs between groundtruth and estimated trajectory!")
          
      return result
function_tokens: def evaluate_trajectory ( traj_gt , traj_est , param_max_pairs = 10000 , param_fixed_delta = False , param_delta = 1.00 , param_delta_unit = "s" , param_offset = 0.00 , param_scale = 1.00 ) : stamps_gt = list ( traj_gt . keys ( ) ) stamps_est = list ( traj_est . keys ( ) ) stamps_gt . sort ( ) stamps_est . sort ( ) stamps_est_return = [ ] for t_est in stamps_est : t_gt = stamps_gt [ find_closest_index ( stamps_gt , t_est + param_offset ) ] t_est_return = stamps_est [ find_closest_index ( stamps_est , t_gt - param_offset ) ] t_gt_return = stamps_gt [ find_closest_index ( stamps_gt , t_est_return + param_offset ) ] if not t_est_return in stamps_est_return : stamps_est_return . append ( t_est_return ) if ( len ( stamps_est_return ) < 2 ) : raise Exception ( "Number of overlap in the timestamps is too small. Did you run the evaluation on the right files?" ) if param_delta_unit == "s" : index_est = list ( traj_est . keys ( ) ) index_est . sort ( ) elif param_delta_unit == "m" : index_est = distances_along_trajectory ( traj_est ) elif param_delta_unit == "rad" : index_est = rotations_along_trajectory ( traj_est , 1 ) elif param_delta_unit == "deg" : index_est = rotations_along_trajectory ( traj_est , 180 / numpy . pi ) elif param_delta_unit == "f" : index_est = range ( len ( traj_est ) ) else : raise Exception ( "Unknown unit for delta: '%s'" % param_delta_unit ) if not param_fixed_delta : if ( param_max_pairs == 0 or len ( traj_est ) < numpy . sqrt ( param_max_pairs ) ) : pairs = [ ( i , j ) for i in range ( len ( traj_est ) ) for j in range ( len ( traj_est ) ) ] else : pairs = [ ( random . randint ( 0 , len ( traj_est ) - 1 ) , random . randint ( 0 , len ( traj_est ) - 1 ) ) for i in range ( param_max_pairs ) ] else : pairs = [ ] for i in range ( len ( traj_est ) ) : j = find_closest_index ( index_est , index_est [ i ] + param_delta ) if j != len ( traj_est ) - 1 : pairs . append ( ( i , j ) ) if ( param_max_pairs != 0 and len ( pairs ) > param_max_pairs ) : pairs = random . sample ( pairs , param_max_pairs ) gt_interval = numpy . median ( [ s - t for s , t in zip ( stamps_gt [ 1 : ] , stamps_gt [ : - 1 ] ) ] ) gt_max_time_difference = 2 * gt_interval result = [ ] for i , j in pairs : stamp_est_0 = stamps_est [ i ] stamp_est_1 = stamps_est [ j ] stamp_gt_0 = stamps_gt [ find_closest_index ( stamps_gt , stamp_est_0 + param_offset ) ] stamp_gt_1 = stamps_gt [ find_closest_index ( stamps_gt , stamp_est_1 + param_offset ) ] if ( abs ( stamp_gt_0 - ( stamp_est_0 + param_offset ) ) > gt_max_time_difference or abs ( stamp_gt_1 - ( stamp_est_1 + param_offset ) ) > gt_max_time_difference ) : continue error44 = ominus ( scale ( ominus ( traj_est [ stamp_est_1 ] , traj_est [ stamp_est_0 ] ) , param_scale ) , ominus ( traj_gt [ stamp_gt_1 ] , traj_gt [ stamp_gt_0 ] ) ) trans = compute_distance ( error44 ) rot = compute_angle ( error44 ) result . append ( [ stamp_est_0 , stamp_est_1 , stamp_gt_0 , stamp_gt_1 , trans , rot ] ) if len ( result ) < 2 : raise Exception ( "Couldn't find matching timestamp pairs between groundtruth and estimated trajectory!" ) return result