def integrate(sequence):
total = 0.0
timestamps = []
values = []
for _ in iterate_with_dt(sequence):
v0 = _.v0
dt = _.dt
total += v0 * dt
timestamps.append(_.t0)
values.append(total)
return SampledSequence(timestamps, values)
def integrate(sequence):
""" Integrates with respect to time.
That is, it multiplies the value with the Delta T. """
total = 0.0
timestamps = []
values = []
for _ in iterate_with_dt(sequence):
v0 = _.v0
dt = _.dt
total += v0 * dt
timestamps.append(_.t0)
values.append(total)
return SampledSequence(timestamps, values)
def integrate(sequence: SampledSequence[float]) -> SampledSequence[float]:
""" Integrates with respect to time.
That is, it multiplies the value with the Delta T. """
if not sequence:
msg = "Cannot integrate empty sequence."
raise ValueError(msg)
total = 0.0
timestamps = []
values = []
for _ in iterate_with_dt(sequence):
v0 = float(_.v0)
dt = _.dt
total += float(v0 * dt)
timestamps.append(Timestamp(_.t0))
values.append(total)
return SampledSequence[float](timestamps, values)
def evaluate(self, context: RuleEvaluationContext,
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[float](timestamps,
driven_lanedir)
driven_lanedir_cumulative = accumulate(driven_lanedir_incremental)
if len(driven_lanedir_incremental) <= 1:
total = 0
else:
total = driven_lanedir_cumulative.values[-1]
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=total,
incremental=driven_lanedir_incremental,
title=title,
description=description,
cumulative=driven_lanedir_cumulative,
)
def evaluate(self, context: RuleEvaluationContext,
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[float]()
driven_lanedir_builder = SampledSequenceBuilder[float]()
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)
if len(driven_any_incremental) <= 1:
total = 0
else:
total = driven_any_cumulative.values[-1]
title = "Distance"
description = textwrap.dedent("""\
This metric computes how far the robot drove.
""")
result.set_metric(
name=("driven_any", ),
total=total,
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)
if len(driven_lanedir_incremental) <= 1:
total = 0
else:
total = driven_lanedir_cumulative.values[-1]
description = textwrap.dedent("""\
This metric computes how far the robot drove
**in the direction of the lane**.
""")
result.set_metric(
name=("driven_lanedir", ),
total=total,
incremental=driven_lanedir_incremental,
title=title,
description=description,
cumulative=driven_lanedir_cumulative,
)
