def evaluate(self, context, result): assert isinstance(result, RuleEvaluationResult) interval = context.get_interval() lane_pose_seq = context.get_lane_pose_seq() ego_pose_sequence = context.get_ego_pose_global() timestamps = [] driven_any = [] driven_lanedir = [] tile_fqn2lane_fqn = {} for idt in iterate_with_dt(interval): t0, t1 = idt.v0, idt.v1 # not v try: name2lpr = lane_pose_seq.at(t0) p0 = ego_pose_sequence.at(t0).as_SE2() p1 = ego_pose_sequence.at(t1).as_SE2() except UndefinedAtTime: dr_any = dr_lanedir = 0.0 else: prel = relative_pose(p0, p1) translation, _ = geo.translation_angle_from_SE2(prel) dr_any = np.linalg.norm(translation) if name2lpr: ds = [] for k, lpr in name2lpr.items(): if lpr.tile_fqn in tile_fqn2lane_fqn: if lpr.lane_segment_fqn != tile_fqn2lane_fqn[ lpr.tile_fqn]: # msg = 'Backwards detected' # print(msg) continue tile_fqn2lane_fqn[lpr.tile_fqn] = lpr.lane_segment_fqn assert isinstance(lpr, GetLanePoseResult) c0 = lpr.center_point ctas = geo.translation_angle_scale_from_E2( c0.asmatrix2d().m) c0_ = geo.SE2_from_translation_angle( ctas.translation, ctas.angle) prelc0 = relative_pose(c0_, p1) tas = geo.translation_angle_scale_from_E2(prelc0) # otherwise this lane should not be reported # assert tas.translation[0] >= 0, tas ds.append(tas.translation[0]) dr_lanedir = max(ds) if ds else 0.0 else: # no lp dr_lanedir = 0.0 driven_any.append(dr_any) driven_lanedir.append(dr_lanedir) timestamps.append(t0) title = "Consecutive lane distance" driven_lanedir_incremental = SampledSequence(timestamps, driven_lanedir) driven_lanedir_cumulative = accumulate(driven_lanedir_incremental) description = textwrap.dedent("""\ This metric computes how far the robot drove **in the direction of the correct lane**, discounting whenever it was driven in the wrong direction with respect to the start. """) result.set_metric(name=('driven_lanedir_consec', ), total=driven_lanedir_cumulative.values[-1], incremental=driven_lanedir_incremental, title=title, description=description, cumulative=driven_lanedir_cumulative)
def evaluate(self, context, result): assert isinstance(result, RuleEvaluationResult) interval = context.get_interval() lane_pose_seq = context.get_lane_pose_seq() ego_pose_sequence = context.get_ego_pose_global() driven_any_builder = SampledSequenceBuilder() driven_lanedir_builder = SampledSequenceBuilder() for idt in iterate_with_dt(interval): t0, t1 = idt.v0, idt.v1 # not v try: name2lpr = lane_pose_seq.at(t0) p0 = ego_pose_sequence.at(t0).as_SE2() p1 = ego_pose_sequence.at(t1).as_SE2() except UndefinedAtTime: dr_any = dr_lanedir = 0.0 else: prel = relative_pose(p0, p1) translation, _ = geo.translation_angle_from_SE2(prel) dr_any = np.linalg.norm(translation) if name2lpr: ds = [] for k, lpr in name2lpr.items(): assert isinstance(lpr, GetLanePoseResult) c0 = lpr.center_point ctas = geo.translation_angle_scale_from_E2( c0.asmatrix2d().m) c0_ = geo.SE2_from_translation_angle( ctas.translation, ctas.angle) prelc0 = relative_pose(c0_, p1) tas = geo.translation_angle_scale_from_E2(prelc0) # otherwise this lane should not be reported # assert tas.translation[0] >= 0, tas ds.append(tas.translation[0]) dr_lanedir = max(ds) else: # no lp dr_lanedir = 0.0 driven_any_builder.add(t0, dr_any) driven_lanedir_builder.add(t0, dr_lanedir) driven_any_incremental = driven_any_builder.as_sequence() driven_any_cumulative = accumulate(driven_any_incremental) title = "Distance" description = textwrap.dedent("""\ This metric computes how far the robot drove. """) result.set_metric(name=('driven_any', ), total=driven_any_cumulative.values[-1], incremental=driven_any_incremental, title=title, description=description, cumulative=driven_any_cumulative) title = "Lane distance" driven_lanedir_incremental = driven_lanedir_builder.as_sequence() driven_lanedir_cumulative = accumulate(driven_lanedir_incremental) description = textwrap.dedent("""\ This metric computes how far the robot drove **in the direction of the lane**. """) result.set_metric(name=('driven_lanedir', ), total=driven_lanedir_cumulative.values[-1], incremental=driven_lanedir_incremental, title=title, description=description, cumulative=driven_lanedir_cumulative)