def test_synthetic(actual_map_name, template, robot_name, line_detector_name,
image_prep_name, lane_filter_name, pose_or_location, outd):
np.random.seed(42)
db = get_easy_algo_db()
actual_map = db.create_instance(FAMILY_SEGMAPS, actual_map_name)
# first, load calibration for robot
easy_algo_db = get_easy_algo_db()
dtu.logger.debug('looking for localization template %r' % template)
localization_template = easy_algo_db.create_instance(
FAMILY_LOC_TEMPLATES, template)
gp = get_ground_projection(robot_name)
# GroundProjectionGeometry
gpg = gp.get_ground_projection_geometry()
if pose_or_location.shape == (3, 3): # SE(2)
pose = pose_or_location
location = localization_template.coords_from_pose(pose)
else:
location = pose_or_location
pose = localization_template.pose_from_coords(location)
simulation_data = simulate_image(actual_map, pose, gpg, blur_sigma=0.3)
image = simulation_data.distorted_synthetic_bgr
# anti_instagram_name='identity' # skip
anti_instagram_name = 'baseline'
all_details = False
res, stats = run_pipeline(image,
gp,
line_detector_name=line_detector_name,
image_prep_name=image_prep_name,
lane_filter_name=lane_filter_name,
anti_instagram_name=anti_instagram_name,
all_details=all_details,
ground_truth=pose,
actual_map=actual_map)
error = np.empty_like(location)
for k in error.dtype.fields:
error[k] = stats['estimate'][k] - location[k]
stats['error'] = error
res = dtu.resize_small_images(res)
dtu.write_bgr_images_as_jpgs(res, outd, extra_string=outd.split('/')[-1])
return res, stats
def go(self):
db = get_easy_algo_db()
extra = self.options.get_extra()
if len(extra) > 1:
msg = 'Only one extra param accepted.'
raise DTUserError(msg)
if len(extra) == 1:
query = extra[0]
else:
query = 'all'
persons = db.query_and_instance('person', query)
if self.options.roster:
out_file = expand_all(self.options.roster)
outd = os.path.dirname(out_file)
roster = create_roster(persons, outd)
write_data_to_file(roster, out_file)
else:
# instance ll
tags = set()
for p in persons.values():
tags.update(p.get_tags())
print(list(persons))
print('tags: %s' % list(tags))
table = table_people(persons)
print(table)
def process_log(self, bag_in, prefix: str, bag_out, prefix_out: str, utils: ProcessorUtilsInterface):
algo_db = get_easy_algo_db()
line_detector = algo_db.create_instance(FAMILY_LINE_DETECTOR, self.line_detector)
image_prep = algo_db.create_instance("image_prep", self.image_prep)
vehicle = dbu.which_robot(bag_in)
topic = f"/{vehicle}/camera_node/image/compressed"
context = FakeContext()
transform = None
frame = 0
for compressed_img_msg in dbu.d8n_bag_read_with_progress(bag_in, topic):
with context.phase("decoding"):
try:
image_cv = dtu.bgr_from_jpg(compressed_img_msg.data)
except ValueError as e:
msg = f"Could not decode image: {e}"
dtu.raise_wrapped(ValueError, e, msg)
segment_list = image_prep.process(context, image_cv, line_detector, transform)
rendered = vs_fancy_display(image_prep.image_cv, segment_list)
rendered = dtu.d8_image_zoom_linear(rendered, 2)
log_name = "log_name"
time = 12
rendered = dtu.add_duckietown_header(rendered, log_name, time, frame)
out = dru.d8n_image_msg_from_cv_image(rendered, "bgr8", same_timestamp_as=compressed_img_msg)
# Write to the bag
bag_out.write("processed", out)
# out = d8n_image_msg_from_cv_image(image_cv, "bgr8", same_timestamp_as=compressed_img_msg)
bag_out.write("image", compressed_img_msg)
frame += 1
def define_jobs_context(self, context):
easy_algo_db = get_easy_algo_db()
expect = self.options["expect"]
write_to_db = self.options["write"]
delete = not self.options["debug_no_delete"]
if not expect in RTCheck.CHECK_RESULTS:
msg = f"Invalid expect status {expect}; must be one of {RTCheck.CHECK_RESULTS}."
raise dtu.DTUserError(msg)
query = self.options["tests"]
regression_tests = easy_algo_db.query("regression_test",
query,
raise_if_no_matches=True)
for rt_name in regression_tests:
rt = easy_algo_db.create_instance("regression_test", rt_name)
easy_logs_db = self.get_easy_logs_db()
c = context.child(rt_name)
outd = os.path.join(self.options["output"], "regression_tests",
rt_name)
jobs_rt(c,
rt_name,
rt,
easy_logs_db,
outd,
expect,
write_data_to_db=write_to_db,
delete=delete)
def create_visuals(robots, actual_map_name, out):
db = get_easy_algo_db()
actual_map = db.create_instance(FAMILY_SEGMAPS, actual_map_name)
res = OrderedDict()
res2 = OrderedDict()
for i, robot_name in enumerate(sorted(robots)):
dtu.logger.info('%d/%d: %s' % (i, len(robots), robot_name))
try:
gp = GroundProjection(robot_name)
except (NoCameraInfoAvailable, NoHomographyInfoAvailable) as e:
dtu.logger.warning('skipping %r: %s' % (robot_name, e))
continue
gpg = gp.get_ground_projection_geometry()
pose = np.eye(3)
simulated_data = \
simulate_image(actual_map, pose, gpg, blur_sigma=1)
res[robot_name] = simulated_data.rectified_synthetic_bgr
res2[robot_name] = simulated_data.distorted_synthetic_bgr
if not res:
msg = 'No images to draw.'
dtu.logger.error(msg)
else:
output = os.path.join(out, 'distorted')
dtu.write_bgr_images_as_jpgs(res2, output)
output = os.path.join(out, 'rectified')
dtu.write_bgr_images_as_jpgs(res, output)
def compose_maps(grid_spacing, tiles):
db = get_easy_algo_db()
tinfo = TransformationsInfo()
FRAME_GLOBAL = 'global'
partial = []
for tile in tiles:
cell = tile['cell']
name = tile['name']
rotation = tile['rotation']
tile_map = db.create_instance(FAMILY_SEGMAPS, name)
x = cell[0] * grid_spacing
y = cell[1] * grid_spacing
theta = np.deg2rad(rotation)
g = SE2_from_xytheta([x,y, theta])
tinfo.add_transformation(frame1=FRAME_GLOBAL, frame2=FRAME_TILE, g=g)
tile_map2 = tinfo.transform_map_to_frame(tile_map, FRAME_GLOBAL)
partial.append(tile_map2)
result = SegmentsMap.merge(partial)
return result
def process_one_processor(processor_name_or_spec, prefix_in, prefix_out,
bag_filename, next_bag_filename, t0_absolute,
t1_absolute, log):
dtu.DuckietownConstants.show_timeit_benchmarks = True
easy_algo_db = get_easy_algo_db()
processor = easy_algo_db.create_instance(ProcessorInterface.FAMILY,
processor_name_or_spec)
dtu.logger.info('in: bag_filename: %s' % bag_filename)
if not os.path.exists(bag_filename):
msg = 'File does not exist: %s' % bag_filename
raise ValueError(msg)
dtu.logger.info('out: next_bag_filename: %s' % next_bag_filename)
dtu.logger.info('t0_absolute: %s' % t0_absolute)
dtu.logger.info('t1_absolute: %s' % t1_absolute)
log = download_if_necessary(log)
filename = get_local_bag_file(log)
original_bag = rosbag.Bag(filename)
bag_absolute_t0_ref = original_bag.get_start_time()
original_bag.close()
dtu.d8n_make_sure_dir_exists(next_bag_filename)
out_bag = rosbag.Bag(next_bag_filename, 'w')
bag0 = rosbag.Bag(bag_filename)
t0_rel = t0_absolute - bag0.get_start_time()
t1_rel = t1_absolute - bag0.get_start_time()
in_bag = dtu.BagReadProxy(bag0,
t0_rel,
t1_rel,
bag_absolute_t0_ref=bag_absolute_t0_ref)
utils = ProcessorUtils(bag_out=out_bag, log=log)
processor.process_log(in_bag, prefix_in, out_bag, prefix_out, utils)
in_bag.close()
# also copy the other messages
in_bag1 = rosbag.Bag(bag_filename)
for topic, msg, t in in_bag1.read_messages(raw=True):
out_bag.write(topic, msg, t, raw=True)
in_bag1.close()
out_bag.close()
# r = rosbag.Bag(next_bag_filename)
# for topic, msg, t in r.read_messages(raw=True):
# pass
# r.close()
# cmd = ['rosbag', 'fix', next_bag_filename, next_bag_filename]
# dtu.system_cmd_result(cwd='.', cmd=cmd, raise_on_error=True, display_stdout=True,
# display_stderr=True)
# cmd = ['rosbag', 'reindex', next_bag_filename]
# dtu.system_cmd_result(cwd='.', cmd=cmd, raise_on_error=True, display_stdout=True,
# display_stderr=True)
return next_bag_filename
def on_parameters_changed(self, _first_time, updated):
if "verbose" in updated:
self.info(f"Verbose is now {self.config.verbose!r}")
if "line_detector" in updated:
db = get_easy_algo_db()
self.detector = db.create_instance("line_detector", self.config.line_detector)
def on_parameters_changed(self, _first_time, updated):
if 'verbose' in updated:
self.info('Verbose is now %r' % self.config.verbose)
if 'line_detector' in updated:
db = get_easy_algo_db()
self.detector = db.create_instance('line_detector',
self.config.line_detector)
def job_analyze(log: str, analyzer: str):
easy_algo_db = get_easy_algo_db()
analyzer_instance = easy_algo_db.create_instance("analyzer", analyzer)
in_bag = rosbag.Bag(log)
results = {}
dtu.logger.info(f"Running {analyzer} on {log}")
analyzer_instance.analyze_log(in_bag, results)
in_bag.close()
return results
def initialize(self):
easy_algo_db = get_easy_algo_db()
self._localization_template = easy_algo_db.create_instance(
FAMILY_LOC_TEMPLATES, self.localization_template
)
self.initialize_belief()
sm = self._localization_template.get_map()
self.rep_map = get_compat_representation_map(sm, self.delta_segment)
def job_analyze(log, analyzer):
easy_algo_db = get_easy_algo_db()
analyzer_instance = easy_algo_db.create_instance('analyzer', analyzer)
in_bag = rosbag.Bag(log)
results = OrderedDict()
dtu.logger.info('Running %s on %s' % (analyzer, log))
analyzer_instance.analyze_log(in_bag, results)
in_bag.close()
return results
def job_merge(results, analyzer):
"""
results: log name -> results dict
"""
easy_algo_db = get_easy_algo_db()
analyzer_instance = easy_algo_db.create_instance('analyzer', analyzer)
results = list(results.values())
total = merge_n(analyzer_instance, results)
return total
def display_map(id_map, out):
dtu.logger.info('id_map == %s' % id_map)
db = get_easy_algo_db()
smap = db.create_instance(FAMILY_SEGMAPS, id_map)
texture_png = get_texture(smap, dpi=600)
fn = os.path.join(out, '%s-texture.png' % (id_map))
dtu.write_data_to_file(texture_png, fn)
simdata = simulate_camera_view(smap)
fn = os.path.join(out, '%s-view.jpg' % (id_map))
dtu.write_bgr_to_file_as_jpg(simdata.rectified_synthetic_bgr, fn)
fn = os.path.join(out, '%s-segments.jpg' % (id_map))
dtu.write_bgr_to_file_as_jpg(simdata.rectified_segments_bgr, fn)
def get_map(self):
if self._map is None:
# Need to do this here and not in constructor
# because otherwise there is a loop in EasyAlgo
db = get_easy_algo_db()
self._map = db.create_instance(FAMILY_SEGMAPS, self.tile_name)
frames = set(_.id_frame for _ in self._map.points.values())
if frames != set([FRAME_GLOBAL]):
msg = ('Expected that all points in the map %r are in the frame %r.' %
(self.tile_name, FRAME_GLOBAL))
msg += ' These are the frames: %s.' % frames
raise ValueError(msg)
return self._map
def get_map(self) -> SegmentsMap:
if self._map is None:
# Need to do this here and not in constructor
# because otherwise there is a loop in EasyAlgo
db = get_easy_algo_db()
self._map = db.create_instance(FAMILY_SEGMAPS, self.tile_name)
frames = set(_.id_frame for _ in list(self._map.points.values()))
if frames != {FRAME_GLOBAL}:
msg = f"Expected that all points in the map {self.tile_name!r} are in the frame " \
f"{FRAME_GLOBAL!r}."
msg += f" These are the frames: {frames}."
raise ValueError(msg)
return self._map
def go(self):
out = 'out-maps'
extra = self.options.get_extra()
if len(extra) == 0:
query = '*'
else:
query = extra
db = get_easy_algo_db()
maps = list(db.query_and_instance(FAMILY_SEGMAPS, query))
self.debug('maps: %s' % maps)
for id_map in maps:
display_map(id_map, out)
def go(self):
out = "out/maps"
extra = self.options.get_extra()
if len(extra) == 0:
query = "*"
else:
query = extra
db = get_easy_algo_db()
maps = list(db.query_and_instance(FAMILY_SEGMAPS, query))
self.debug(f"maps: {maps}")
for id_map in maps:
display_map(id_map, out)
def display_map(id_map: str, out: str):
logger.info(f"id_map = {id_map}")
db = get_easy_algo_db()
smap = db.create_instance(FAMILY_SEGMAPS, id_map)
texture_png = get_texture(smap, dpi=600)
fn = os.path.join(out, id_map, f"{id_map}-texture.png")
dtu.write_data_to_file(texture_png, fn)
simdata = simulate_camera_view(smap, robot_name=dtu.DuckietownConstants.ROBOT_NAME_FOR_TESTS)
fn = os.path.join(out, id_map, f"{id_map}-rectified_synthetic.jpg")
dtu.write_bgr_to_file_as_jpg(simdata.rectified_synthetic_bgr, fn)
fn = os.path.join(out, id_map, f"{id_map}-rectified_segments.jpg")
dtu.write_bgr_to_file_as_jpg(simdata.rectified_segments_bgr, fn)
fn = os.path.join(out, id_map, f"{id_map}-distorted_synthetic.jpg")
dtu.write_bgr_to_file_as_jpg(simdata.distorted_synthetic_bgr, fn)
def call_summary():
db = get_easy_algo_db()
s = format_db(db)
dtu.logger.info(s)
errors = []
for f in db.family_name2config.values():
if not f.valid:
errors.append('Family %s: %s' %
(f.family_name, f.error_if_invalid))
for i in f.instances.values():
if not i.valid:
errors.append('Family %s / instance %r:\n\n%s' %
(f.family_name, i.instance_name,
dtu.indent(i.error_if_invalid, ' ')))
if errors:
msg = '\n' + '\n'.join(errors)
raise Exception(msg)
def call_summary():
db = get_easy_algo_db()
s = format_db(db)
dtu.logger.info(s)
errors = []
for f in list(db.family_name2config.values()):
if not f.valid:
errors.append(f"Family {f.family_name}: {f.error_if_invalid}")
for i in list(f.instances.values()):
if not i.valid:
errors.append(
f"Family {f.family_name} / instance {i.instance_name!r}:\n\n"
f"{dtu.indent(i.error_if_invalid, ' ')}"
)
if errors:
msg = "\n" + "\n".join(errors)
raise Exception(msg)
def go(self):
extra = self.options.get_extra()
db = get_easy_algo_db()
if len(extra) == 0:
query = dtu.get_current_robot_name()
else:
query = extra
robots = db.query('robot', query, raise_if_no_matches=True)
self.debug('robots: %s' % sorted(robots))
actual_map_name = 'DT17_scenario_four_way'
out = self.options.output
create_visuals(robots, actual_map_name, out)
for robot_name in robots:
_ok = try_simulated_localization(robot_name)
def compute_check_results(rt_name, rt, results_all):
current = make_entry(rt_name, results_all)
algo_db = get_easy_algo_db()
entries_names = algo_db.query("rdbe", f"parameters:regression_test_name:{rt_name}")
dtu.logger.info(f"entries: {list(entries_names)}")
entries = []
for name in entries_names:
e = algo_db.create_instance("rdbe", name)
entries.append(e)
rdb = ResultDB(current=current, entries=entries)
res = []
for cwc in rt.get_checks():
for check in cwc.checks:
r = check.check(rdb)
assert isinstance(r, CheckResult)
res.append(r)
return res
def process_log(self, bag_in, bag_out):
algo_db = get_easy_algo_db()
line_detector = algo_db.create_instance(FAMILY_LINE_DETECTOR,
self.line_detector)
image_prep = algo_db.create_instance('image_prep', self.image_prep)
vehicle = dtu.which_robot(bag_in)
topic = '/%s/camera_node/image/compressed' % vehicle
context = FakeContext()
transform = None
frame = 0
for compressed_img_msg in dtu.d8n_bag_read_with_progress(
bag_in, topic):
with context.phase('decoding'):
try:
image_cv = dtu.bgr_from_jpg(compressed_img_msg.data)
except ValueError as e:
msg = 'Could not decode image: %s' % e
dtu.raise_wrapped(ValueError, e, msg)
segment_list = image_prep.process(context, image_cv, line_detector,
transform)
rendered = vs_fancy_display(image_prep.image_cv, segment_list)
rendered = dtu.d8_image_zoom_linear(rendered, 2)
log_name = 'log_name'
time = 12
rendered = dtu.add_duckietown_header(rendered, log_name, time,
frame)
out = dtu.d8n_image_msg_from_cv_image(
rendered, "bgr8", same_timestamp_as=compressed_img_msg)
# Write to the bag
bag_out.write('processed', out)
# out = d8n_image_msg_from_cv_image(image_cv, "bgr8", same_timestamp_as=compressed_img_msg)
bag_out.write('image', compressed_img_msg)
frame += 1
def create_visuals(robots: List[str], actual_map_name: str, out: str):
db = get_easy_algo_db()
actual_map = db.create_instance(FAMILY_SEGMAPS, actual_map_name)
res = {}
res2 = {}
for i, robot_name in enumerate(sorted(robots)):
logger.info(f"{i}/{len(robots)}: {robot_name}")
rcg = get_robot_camera_geometry(robot_name)
pose = np.eye(3)
simulated_data = simulate_image(actual_map, pose, gpg=rcg.gpg, rectifier=rcg.rectifier, blur_sigma=1)
res[robot_name] = simulated_data.rectified_synthetic_bgr
res2[robot_name] = simulated_data.distorted_synthetic_bgr
if not res:
msg = "No images to draw."
dtu.logger.error(msg)
else:
output = os.path.join(out, "distorted")
dtu.write_bgr_images_as_jpgs(res2, output)
output = os.path.join(out, "rectified")
dtu.write_bgr_images_as_jpgs(res, output)
def define_jobs_context(self, context):
easy_algo_db = get_easy_algo_db()
expect = self.options.expect
write_to_db = self.options.write
delete = not self.options.debug_no_delete
if not expect in RTCheck.CHECK_RESULTS:
msg = 'Invalid expect status %s; must be one of %s.' % (expect, RTCheck.CHECK_RESULTS)
raise dtu.DTUserError(msg)
query = self.options.tests
regression_tests = easy_algo_db.query('regression_test', query, raise_if_no_matches=True)
for rt_name in regression_tests:
rt = easy_algo_db.create_instance('regression_test', rt_name)
easy_logs_db = self.get_easy_logs_db()
c = context.child(rt_name)
outd = os.path.join(self.options.output, 'regression_tests', rt_name)
jobs_rt(c, rt_name, rt, easy_logs_db, outd, expect, write_data_to_db=write_to_db, delete=delete)
def coordinates():
easy_algo_db = get_easy_algo_db()
localization_template = easy_algo_db.create_instance(
FAMILY_LOC_TEMPLATES, template)
localization_template._init_metrics()
center = localization_template.center
offset = localization_template.offset
pose = dtu.geo.SE2_from_translation_angle([center[0], center[1] - offset],
np.deg2rad(0))
location = localization_template.coords_from_pose(pose)
assert_almost_equal(location['phi'], 0)
assert_almost_equal(location['d'], 0)
DX = 0.01
pose = dtu.geo.SE2_from_translation_angle(
[center[0], center[1] - offset + DX], np.deg2rad(0))
location = localization_template.coords_from_pose(pose)
assert_almost_equal(location['phi'], 0)
assert_almost_equal(location['d'], DX)
pose = dtu.geo.SE2_from_translation_angle(
[center[0], center[1] - offset + DX], np.deg2rad(10))
location = localization_template.coords_from_pose(pose)
assert_almost_equal(location['phi'], np.deg2rad(10))
assert_almost_equal(location['d'], DX)
pose = dtu.geo.SE2_from_translation_angle([center[0] + offset, center[1]],
np.deg2rad(90))
location = localization_template.coords_from_pose(pose)
assert_almost_equal(location['phi'], np.deg2rad(0))
assert_almost_equal(location['d'], 0)
a = 0
pose = dtu.geo.SE2_from_translation_angle(
[center[0] + offset * np.cos(a), center[1] + offset * np.sin(a)],
np.deg2rad(45))
location = localization_template.coords_from_pose(pose)
assert_almost_equal(location['phi'], np.deg2rad(-45))
assert_almost_equal(location['d'], 0)
a = np.deg2rad(-40)
pose = dtu.geo.SE2_from_translation_angle(
[center[0] + offset * np.cos(a), center[1] + offset * np.sin(a)],
np.deg2rad(50))
location = localization_template.coords_from_pose(pose)
# print('offset: %s' % offset)
# print('pose: %s' % SE2.friendly(pose))
# print('location: %s' % phi_d_friendly(location))
#
assert_almost_equal(location['phi'], np.deg2rad(0))
assert_almost_equal(location['d'], 0)
a = np.deg2rad(-40)
D = 0.01
pose = dtu.geo.SE2_from_translation_angle([
center[0] + (offset + D) * np.cos(a), center[1] +
(offset + D) * np.sin(a)
], np.deg2rad(50))
location = localization_template.coords_from_pose(pose)
# print('offset: %s' % offset)
# print('pose: %s' % SE2.friendly(pose))
# print('location: %s' % phi_d_friendly(location))
#
assert_almost_equal(location['phi'], np.deg2rad(0))
assert_almost_equal(location['d'], -D)
pose = dtu.geo.SE2_from_translation_angle([+0.1095, 0], np.deg2rad(0))
check_coords(localization_template, pose)
pose = dtu.geo.SE2_from_translation_angle([+0.1095, 0], np.deg2rad(15))
check_coords(localization_template, pose)
#
pose = dtu.geo.SE2_from_translation_angle([0.15, -0.1], np.deg2rad(5))
check_coords(localization_template, pose)
def run_pipeline(image, all_details=False, ground_truth=None, actual_map=None):
"""
Image: numpy (H,W,3) == BGR
Returns a dictionary, res with the following fields:
res['input_image']
ground_truth = pose
"""
print('backend: %s' % matplotlib.get_backend())
print('fname: %s' % matplotlib.matplotlib_fname())
quick = False
dtu.check_isinstance(image, np.ndarray)
res = OrderedDict()
stats = OrderedDict()
vehicle_name = dtu.get_current_robot_name()
res['Raw input image'] = image
gp = get_ground_projection(vehicle_name)
gpg = gp.get_ground_projection_geometry
line_detector = LineDetector()
lane_filter = LaneFilterHistogram(None)
print("pass lane_filter")
ai = AntiInstagram()
pts = ProcessingTimingStats()
pts.reset()
pts.received_message()
pts.decided_to_process()
with pts.phase('calculate AI transform'):
[lower, upper] = ai.calculate_color_balance_thresholds(image)
with pts.phase('apply AI transform'):
transformed = ai.apply_color_balance(lower, upper, image)
with pts.phase('edge detection'):
# note: do not apply transform twice!
segment_list2 = image_prep.process(pts,
image,
line_detector,
transform=ai.apply_color_balance)
if all_details:
res['resized and corrected'] = image_prep.image_corrected
dtu.logger.debug('segment_list2: %s' % len(segment_list2.segments))
if all_details:
res['segments_on_image_input_transformed'] = \
vs_fancy_display(image_prep.image_cv, segment_list2)
if all_details:
res['segments_on_image_input_transformed_resized'] = \
vs_fancy_display(image_prep.image_resized, segment_list2)
if all_details:
grid = get_grid(image.shape[:2])
res['grid'] = grid
res['grid_remapped'] = gpg.rectify(grid)
# res['difference between the two'] = res['image_input']*0.5 + res['image_input_rect']*0.5
with pts.phase('rectify_segments'):
segment_list2_rect = rectify_segments(gpg, segment_list2)
# Project to ground
with pts.phase('find_ground_coordinates'):
sg = find_ground_coordinates(gpg, segment_list2_rect)
lane_filter.initialize()
if all_details:
res['prior'] = lane_filter.get_plot_phi_d()
with pts.phase('lane filter update'):
print(type(lane_filter).__name__)
if type(lane_filter).__name__ == 'LaneFilterHistogram':
# XXX merging pain
_likelihood = lane_filter.update(sg.segments)
else:
_likelihood = lane_filter.update(sg)
if not quick:
with pts.phase('lane filter plot'):
res['likelihood'] = lane_filter.get_plot_phi_d(
ground_truth=ground_truth)
easy_algo_db = get_easy_algo_db()
if isinstance(lane_filter, LaneFilterMoreGeneric):
template_name = lane_filter.localization_template
else:
template_name = 'DT17_template_straight_straight'
dtu.logger.debug('Using default template %r for visualization' %
template_name)
localization_template = \
easy_algo_db.create_instance(FAMILY_LOC_TEMPLATES, template_name)
with pts.phase('lane filter get_estimate()'):
est = lane_filter.get_estimate()
# Coordinates in TILE frame
g = localization_template.pose_from_coords(est)
tinfo = TransformationsInfo()
tinfo.add_transformation(frame1=FRAME_GLOBAL, frame2=FRAME_AXLE, g=g)
if all_details:
if actual_map is not None:
# sm_axle = tinfo.transform_map_to_frame(actual_map, FRAME_AXLE)
res['real'] = plot_map_and_segments(actual_map,
tinfo,
sg.segments,
dpi=120,
ground_truth=ground_truth)
with pts.phase('rectify'):
rectified0 = gpg.rectify(image)
rectified = ai.apply_color_balance(rectified0)
if all_details:
res['image_input_rect'] = rectified
res['segments rectified on image rectified'] = \
vs_fancy_display(rectified, segment_list2_rect)
assumed = localization_template.get_map()
if not quick:
with pts.phase('plot_map_and_segments'):
res['model assumed for localization'] = plot_map_and_segments(
assumed,
tinfo,
sg.segments,
dpi=120,
ground_truth=ground_truth)
assumed_axle = tinfo.transform_map_to_frame(assumed, FRAME_AXLE)
with pts.phase('plot_map reprojected'):
res['map reprojected on image'] = plot_map(rectified,
assumed_axle,
gpg,
do_ground=False,
do_horizon=True,
do_faces=False,
do_faces_outline=True,
do_segments=False)
with pts.phase('quality computation'):
predicted_segment_list_rectified = predict_segments(sm=assumed_axle,
gpg=gpg)
quality_res, quality_stats = judge_quality(
image, segment_list2_rect, predicted_segment_list_rectified)
res.update(quality_res)
# res['blurred']= cv2.medianBlur(image, 11)
stats['estimate'] = est
stats.update(quality_stats)
dtu.logger.info(pts.get_stats())
return res, stats
def run_pipeline(
image: dtu.NPImageBGR,
gpg: GroundProjectionGeometry,
rectifier: Rectify,
line_detector_name: str,
image_prep_name: str,
lane_filter_name: str,
anti_instagram_name: str,
all_details: bool = False,
ground_truth=None,
actual_map=None,
) -> Tuple[Dict[str, dtu.NPImageBGR], Dict[str, float]]:
"""
Image: numpy (H,W,3) == BGR
Returns a dictionary, res with the following fields:
res['input_image']
ground_truth = pose
"""
logger.debug(f"backend: {matplotlib.get_backend()}")
logger.debug(f"fname: {matplotlib.matplotlib_fname()}")
quick: bool = False
dtu.check_isinstance(image, np.ndarray)
res: Dict[str, dtu.NPImageBGR] = {}
stats = {}
res["Raw input image"] = image
algo_db = get_easy_algo_db()
line_detector = algo_db.create_instance(FAMILY_LINE_DETECTOR,
line_detector_name)
lane_filter = algo_db.create_instance(FAMILY_LANE_FILTER, lane_filter_name)
image_prep = algo_db.create_instance(ImagePrep.FAMILY, image_prep_name)
ai = algo_db.create_instance(AntiInstagramInterface.FAMILY,
anti_instagram_name)
pts = ProcessingTimingStats()
pts.reset()
pts.received_message()
pts.decided_to_process()
if all_details:
segment_list = image_prep.process(FakeContext(),
image,
line_detector,
transform=None)
res["segments_on_image_input"] = vs_fancy_display(
image_prep.image_cv, segment_list)
res["segments_on_image_resized"] = vs_fancy_display(
image_prep.image_resized, segment_list)
with pts.phase("calculate AI transform"):
ai.calculateTransform(image)
with pts.phase("apply AI transform"):
transformed = ai.applyTransform(image)
if all_details:
res["image_input_transformed"] = transformed
with pts.phase("edge detection"):
# note: do not apply transform twice!
segment_list2 = image_prep.process(pts,
image,
line_detector,
transform=ai.applyTransform)
if all_details:
res["resized and corrected"] = image_prep.image_corrected
logger.debug(f"segment_list2: {len(segment_list2.segments)}")
if all_details:
res["segments_on_image_input_transformed"] = vs_fancy_display(
image_prep.image_cv, segment_list2)
if all_details:
res["segments_on_image_input_transformed_resized"] = vs_fancy_display(
image_prep.image_resized, segment_list2)
if all_details:
grid = get_grid(image.shape[:2])
res["grid"] = grid
res["grid_remapped"] = rectifier.rectify(grid)
# res['difference between the two'] = res['image_input']*0.5 + res['image_input_rect']*0.5
with pts.phase("rectify_segments"):
segment_list2_rect = rectify_segments(rectifier, gpg, segment_list2)
# Project to ground
with pts.phase("find_ground_coordinates"):
sg = find_ground_coordinates(gpg, segment_list2_rect)
lane_filter.initialize()
# lane_filter.get_plot_phi_d()
if all_details:
res["prior"] = lane_filter.get_plot_phi_d()
with pts.phase("lane filter update"):
logger.info(type(lane_filter).__name__)
if type(lane_filter).__name__ == "LaneFilterHistogram":
# XXX merging pain
_likelihood = lane_filter.update(sg.segments)
else:
_likelihood = lane_filter.update(sg)
if not quick:
with pts.phase("lane filter plot"):
res["likelihood"] = lane_filter.get_plot_phi_d(
ground_truth=ground_truth)
easy_algo_db = get_easy_algo_db()
if isinstance(lane_filter, LaneFilterMoreGeneric):
template_name = lane_filter.localization_template
else:
template_name = "DT17_template_straight_straight"
dtu.logger.debug(
f"Using default template {template_name!r} for visualization")
localization_template = easy_algo_db.create_instance(
FAMILY_LOC_TEMPLATES, template_name)
with pts.phase("lane filter get_estimate()"):
est = lane_filter.get_estimate()
# Coordinates in TILE frame
g = localization_template.pose_from_coords(est)
tinfo = TransformationsInfo()
tinfo.add_transformation(frame1=FRAME_GLOBAL, frame2=FRAME_AXLE, g=g)
if all_details:
if actual_map is not None:
# sm_axle = tinfo.transform_map_to_frame(actual_map, FRAME_AXLE)
res["real"] = plot_map_and_segments(actual_map,
tinfo,
sg.segments,
dpi=120,
ground_truth=ground_truth)
with pts.phase("rectify"):
rectified0 = rectifier.rectify(image)
rectified = ai.applyTransform(rectified0)
if all_details:
res["image_input_rect"] = rectified
res["segments rectified on image rectified"] = vs_fancy_display(
rectified, segment_list2_rect)
assumed = localization_template.get_map()
if not quick:
with pts.phase("plot_map_and_segments"):
res["model assumed for localization"] = plot_map_and_segments(
assumed,
tinfo,
sg.segments,
dpi=120,
ground_truth=ground_truth)
assumed_axle = tinfo.transform_map_to_frame(assumed, FRAME_AXLE)
with pts.phase("plot_map reprojected"):
res["map reprojected on image"] = plot_map(
rectified,
assumed_axle,
gpg,
do_ground=False,
do_horizon=True,
do_faces=False,
do_faces_outline=True,
do_segments=False,
)
with pts.phase("quality computation"):
predicted_segment_list_rectified = predict_segments(sm=assumed_axle,
gpg=gpg)
quality_res, quality_stats = judge_quality(
image, segment_list2_rect, predicted_segment_list_rectified)
res.update(quality_res)
# res['blurred']= cv2.medianBlur(image, 11)
stats["estimate"] = est
stats.update(quality_stats)
dtu.logger.info(pts.get_stats())
return res, stats
def get_scuderia_contents():
db = get_easy_algo_db()
robots = db.query_and_instance('robot', 'all')
return robots
