def name_from_spec(x):
dtu.check_isinstance(x, (str, dict))
if isinstance(x, str):
return x
elif isinstance(x, dict):
name, _ = _parse_inline_spec(x)
return name
def create_instance(self, family_name, instance_name_or_spec):
dtu.check_isinstance(instance_name_or_spec, (dict, str))
family = self.get_family(family_name)
if not family.valid:
msg = ('Cannot instantiate %r because its family %r is invalid.' %
(instance_name_or_spec, family_name))
msg += '\n\n' + dtu.indent(family.error_if_invalid, " > ")
raise dtu.DTConfigException(msg)
if isinstance(instance_name_or_spec, str):
instance_name = instance_name_or_spec
dtu.check_is_in('instance', instance_name, family.instances)
instance = family.instances[instance_name]
if not instance.valid:
msg = ('Cannot instantiate %r because it is invalid:\n%s' %
(instance_name,
dtu.indent(instance.error_if_invalid, '> ')))
raise dtu.DTConfigException(msg)
res = dtu.instantiate(instance.constructor, instance.parameters)
elif isinstance(instance_name_or_spec, dict):
_name, spec = _parse_inline_spec(instance_name_or_spec)
res = dtu.instantiate(spec.constructor, spec.parameters)
else:
assert False
interface = dtu.import_name(family.interface)
if not isinstance(res, interface):
msg = ('I expected that %r would be a %s but it is a %s.' %
(instance_name, interface.__name__, type(res).__name__))
raise dtu.DTConfigException(msg)
return res
def __init__(self, logs=None):
# ordereddict str -> PhysicalLog
if logs is None:
logs = load_all_logs()
else:
check_isinstance(logs, OrderedDict)
self.logs = logs
def logs_from_yaml(data):
dtu.check_isinstance(data, dict)
res = OrderedDict()
for k, d in data.items():
res[k] = physical_log_from_yaml(d)
return res
def read_data_for_signals(bag_in, prefix_in, signals):
topic2messages = defaultdict(lambda: dict(timestamp=[], data=[]))
topics = []
for signal_spec in signals:
dtu.check_isinstance(signal_spec, PlotSignalSpec)
topic_fqn = prefix_in + signal_spec.topic
topics.append(topic_fqn)
for _j, mp in enumerate(bag_in.read_messages_plus(topics=topics)):
data = interpret_ros(mp.msg)
topic2messages[mp.topic]['data'].append(data)
topic2messages[mp.topic]['timestamp'].append(
mp.time_from_physical_log_start)
for v in topic2messages.values():
v['data'] = np.array(v['data'])
v['timestamp'] = np.array(v['timestamp'])
for signal_spec in signals:
topic_fqn = prefix_in + signal_spec.topic
if not topic_fqn in topic2messages:
msg = 'Could not found any value for topic %r.' % topic_fqn
raise ValueError(msg)
return topic2messages
def _parse_inline_spec(x):
dtu.check_isinstance(x, dict)
if len(x) != 1:
msg = 'Invalid spec: length is %d' % len(x)
dtu.raise_desc(ValueError, msg, x=x)
key = list(x)[0]
value = x[key]
value = copy.deepcopy(value)
if 'description' in value:
description = value.pop('description')
dtu.dt_check_isinstance('description', description, str)
else:
description = None
constructor = value.pop('constructor')
dtu.dt_check_isinstance('constructor', constructor, str)
parameters = value.pop('parameters')
dtu.dt_check_isinstance('parameters', parameters, (dict, NoneType))
if parameters is None: parameters = {}
res = SpecValues(constructor=constructor,
parameters=parameters,
description=description)
return key, res
def yaml_representation_of_phy_logs(logs):
dtu.check_isinstance(logs, dict)
res = OrderedDict()
for k, l in logs.items():
res[k] = yaml_from_physical_log(l)
s = yaml_dump_pretty(res)
return s
def __str__(self):
n = len(self.entries)
s = 'ResultsDB with %d entries' % n
s += '\n' + dtu.indent(str(self.current), '', ' current: ')
for i, p in enumerate(self.entries):
dtu.check_isinstance(p, ResultDBEntry)
s += '\n' + dtu.indent(str(p), '', ' %2d of %d: ' % (i + 1, n))
return s
def __init__(self, variables, precision='float32'):
self.precision = precision
dtu.check_isinstance(variables, OrderedDict)
if len(variables) != 2:
msg = 'I can only deal with 2 variables, obtained %s' % self._variables
raise ValueError(msg)
self._names = list(variables)
self._specs = [spec_from_yaml(variables[_]) for _ in self._names]
s0 = self._specs[0]
s1 = self._specs[1]
self._mgrids = list(np.mgrid[s0.min:s0.max:s0.resolution,
s1.min:s1.max:s1.resolution])
for a in [0, 1]:
new_max = self._specs[
a].min + self._mgrids[a].shape[a] * self._specs[a].resolution
self._specs[a] = self._specs[a]._replace(max=new_max)
H, W = self.shape = self._mgrids[0].shape
self._centers = [np.zeros(shape=(H, W)), np.zeros(shape=(H, W))]
for i, j in itertools.product(range(H), range(W)):
self._centers[0][i, j] = s0.min + (i + 0.5) * s0.resolution
self._centers[1][i, j] = s1.min + (j + 0.5) * s1.resolution
# these are the bounds you would give to pcolor
# there is one row and one column more
# self.d, self.phi are the lower corners
# Each cell captures this area:
# (X[i, j], Y[i, j]),
# (X[i, j+1], Y[i, j+1]),
# (X[i+1, j], Y[i+1, j]),
# (X[i+1, j+1], Y[i+1, j+1])
self._mgrids_plus = list(
np.mgrid[s0.min:s0.max + s0.resolution:s0.resolution,
s1.min:s1.max + s1.resolution:s1.resolution])
k0_o_sigma = 1.0 / self._specs[0].resolution
def K0(x):
d = x * k0_o_sigma
d2 = d * d
return np.exp(-d2)
k1_o_sigma = 1.0 / self._specs[1].resolution
def K1(x):
d = x * k1_o_sigma
d2 = d * d
return np.exp(-d2)
self.K0 = K0
self.K1 = K1
def get_homography_for_robot(robot_name):
dtu.check_isinstance(robot_name, str)
# Get the config file for the homography of the robot
filename = get_homography_info_config_file(robot_name, strict=False)
data = dtu.yaml_load_file(filename)
# Extract the homography from the YAML file
homography = homography_from_yaml(data)
return homography
def get_homography_for_robot(robot_name):
dtu.check_isinstance(robot_name, str)
# find the file
fn = get_homography_info_config_file(robot_name)
# load the YAML
data = dtu.yaml_load_file(fn, True) # True means "plain"
# convert the YAML
homography = homography_from_yaml(data)
return homography
def get_homography_for_robot(robot_name):
dtu.check_isinstance(robot_name, str)
# find the file
if robot_name == dtu.DuckietownConstants.ROBOT_NAME_FOR_TESTS:
data = dtu.yaml_load(homography_default)
else:
fn = get_homography_info_config_file(robot_name)
# load the YAML
data = dtu.yaml_load_file(fn)
# convert the YAML
homography = homography_from_yaml(data)
#check_homography_sane_for_DB17(homography)
return homography
def validate(self):
for k, p in self.points.items():
dtu.check_isinstance(p, SegMapPoint)
dtu.check_isinstance(k, str)
coords = p.coords
if not isinstance(coords, np.ndarray):
self.points[k] = p._replace(coords=np.array(coords))
for S in self.segments:
dtu.check_isinstance(S, SegMapSegment)
for p in S.points:
if not p in self.points:
msg = 'Invalid point %r' % p
raise ValueError(msg)
assert (len(S.points) == 2)
w1 = self.points[S.points[0]].coords
w2 = self.points[S.points[1]].coords
dist = np.linalg.norm(w1 - w2)
if dist == 0:
msg = 'This is a degenerate segment (points: %s %s) ' % (w1,
w2)
msg += 'names: %s' % S.points
raise ValueError(msg)
for F in self.faces:
dtu.check_isinstance(F, SegMapFace)
for p in F.points:
if not p in self.points:
msg = 'Invalid point %r' % p
raise ValueError(msg)
def query(self, query, raise_if_no_matches=True):
"""
query: a string
Returns an OrderedDict str -> PhysicalLog.
"""
check_isinstance(query, str)
filters = OrderedDict()
filters.update(filters_slice)
filters.update(filters0)
result = fuzzy_match(query,
self.logs,
filters=filters,
raise_if_no_matches=raise_if_no_matches)
return result
def download_if_necessary(log):
"""
Downloads the log if necessary.
Use like this:
log = ...
log2 = download_if_necessary(log)
"""
# dtu.logger.info('Log:\n%s' % log.resources)
dtu.check_isinstance(log, PhysicalLog)
resource_name = 'bag'
filename = get_log_if_not_exists(log, resource_name=resource_name)
local_uri = _create_file_uri(filename)
log.resources[resource_name]['urls'].append(local_uri)
return log
def query_results_one(self, branch, date, commit):
possible = self.query_results(branch, date, commit)
from easy_regression.conditions.eval import DataNotFound
if len(possible) == 0:
msg = 'Could not find any match for the query.'
msg += '\n branch: %s' % branch
msg += '\n date: %s' % date
msg += '\n commit: %s' % commit
raise DataNotFound(msg)
if len(possible) > 1:
n = len(possible)
msg = 'Found %d matches for this query.' % n
msg += '\n branch: %s' % branch
msg += '\n date: %s' % date
msg += '\n commit: %s' % commit
msg += '\nThese are the matches:'
for i, p in enumerate(possible):
dtu.check_isinstance(p, ResultDBEntry)
msg += '\n' + dtu.indent(str(p), ' %2d of %d: ' % (i + 1, n))
raise AmbiguousQuery(msg)
return possible[0]
def do_plot(bag_in, prefix_in, bag_out, prefix_out, signals, plot_name):
topic2messages = defaultdict(lambda: dict(timestamp=[], data=[]))
topics = []
for signal_spec in signals:
dtu.check_isinstance(signal_spec, PlotSignalSpec)
topic_fqn = prefix_in + signal_spec.topic
topics.append(topic_fqn)
for _j, mp in enumerate(bag_in.read_messages_plus(topics=topics)):
data = interpret_ros(mp.msg)
topic2messages[mp.topic]['data'].append(data)
topic2messages[mp.topic]['timestamp'].append(
mp.time_from_physical_log_start)
for signal_spec in signals:
topic_fqn = prefix_in + signal_spec.topic
if not topic_fqn in topic2messages:
msg = 'Could not found any value for topic %r.' % topic_fqn
raise ValueError(msg)
bgcolor = dtu.ColorConstants.RGB_DUCKIETOWN_YELLOW
dpi = 100
figure_args = dict(facecolor=dtu.matplotlib_01_from_rgb(bgcolor))
a = dtu.CreateImageFromPylab(dpi=dpi, figure_args=figure_args)
use_legend = len(signals) >= 3
# todo: check same units
with a as pylab:
axes = []
_fig, ax0 = pylab.subplots()
ax0.set_xlabel('time (s)')
axes.append(ax0)
if use_legend:
for i in range(len(signals) - 1):
axes.append(ax0)
else:
for i in range(len(signals) - 1):
axes.append(ax0.twinx())
for i, signal_spec in enumerate(signals):
ax = axes[i]
topic_fqn = prefix_in + signal_spec.topic
recorded = topic2messages[topic_fqn]
data = np.array(recorded['data'])
t = np.array(recorded['timestamp'])
color = signal_spec.color
markersize = 5
data_converted = convert_unit(data, signal_spec.units,
signal_spec.units_display)
ax.plot(t,
data_converted,
'o',
color=color,
label=signal_spec.label,
markersize=markersize,
clip_on=False)
if not use_legend:
label = '%s [%s]' % (signal_spec.label,
signal_spec.units_display)
ax.set_ylabel(label, color=signal_spec.color)
ax.tick_params('y', colors=color)
ax.set_ylim(signal_spec.min, signal_spec.max)
outward_offset = 20
ax.xaxis.set_tick_params(direction='out')
ax.yaxis.set_tick_params(direction='out')
ax.spines['left'].set_position(('outward', outward_offset))
ax.spines['top'].set_color('none') # don't draw spine
ax.spines['bottom'].set_position(('outward', outward_offset))
ax.spines['right'].set_position(('outward', outward_offset))
pos = {0: 'left', 1: 'right', 2: 'right'}[i]
ax.spines[pos].set_color(color)
ax.xaxis.set_ticks_position('bottom')
if use_legend:
label = '[%s]' % (signal_spec.units_display)
ax.set_ylabel(label)
pylab.legend()
bgr = a.get_bgr()
plot_name = plot_name.replace('/', '')
# output_filename = os.path.join('tmp', plot_name +'.png')
# dtu.write_bgr_as_jpg(bgr, output_filename)
t_inf = rospy.Time.from_sec(bag_in.get_end_time()) # @UndefinedVariable
omsg = dtu.d8_compressed_image_from_cv_image(bgr, timestamp=t_inf)
otopic = prefix_out + '/' + plot_name
bag_out.write(otopic, omsg, t=t_inf)
def __init__(self, current, entries):
for e in entries:
dtu.check_isinstance(e, ResultDBEntry)
self.entries = entries
self.current = current
dtu.check_isinstance(current, ResultDBEntry)
def query_logs(logs, query, raise_if_no_matches=True):
"""
query: a string or a list of strings
Returns an OrderedDict str -> PhysicalLog.
The query can also be a filename.
"""
if isinstance(query, list):
res = OrderedDict()
for q in query:
res.update(query_logs(logs, q, raise_if_no_matches=False))
if raise_if_no_matches and not res:
msg = "Could not find any match for the queries:"
for q in query:
msg += '\n- %s' % q
raise dtu.DTNoMatches(msg)
return res
else:
dtu.check_isinstance(query, str)
filters = OrderedDict()
filters.update(filters_slice)
filters.update(dtu.filters0)
aliases = OrderedDict()
aliases.update(logs)
# adding aliases unless we are asking for everything
if query != '*':
# print('adding more (query = %s)' % query)
for _, log in logs.items():
dtr = DTR.from_yaml(log.resources['bag'])
original_name = dtr.name
# print ('alias: %s %s' % (original_name, dtr.name))
aliases[original_name] = log
original_name = original_name.replace('.bag', '')
aliases[original_name] = log
result = dtu.fuzzy_match(query,
aliases,
filters=filters,
raise_if_no_matches=raise_if_no_matches)
# remove doubles after
# XXX: this still has bugs
present = defaultdict(set)
for k, v in result.items():
present[id(v)].add(k)
def choose(options):
if len(options) == 1:
return list(options)[0]
else:
options = sorted(options, key=len)
return options[0]
c = OrderedDict()
for k, v in result.items():
chosen = choose(present[id(v)])
if k == chosen:
c[k] = v
return c
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 go(self):
robot_name = dtu.get_current_robot_name()
output = self.options.output
if output is None:
output = 'out-calibrate-extrinsics' # + dtu.get_md5(self.options.image)[:6]
self.info('No --output given, using %s' % output)
if self.options.input is None:
print("{}\nCalibrating using the ROS image stream...\n".format(
"*" * 20))
import rospy
from sensor_msgs.msg import CompressedImage
topic_name = os.path.join('/', robot_name,
'camera_node/image/compressed')
print('Topic to listen to is: %s' % topic_name)
print('Let\'s wait for an image. Say cheese!')
# Dummy for getting a ROS message
rospy.init_node('calibrate_extrinsics')
img_msg = None
try:
img_msg = rospy.wait_for_message(topic_name,
CompressedImage,
timeout=10)
print('Image captured!')
except rospy.ROSException as e:
print(
'\n\n\nDidn\'t get any message!: %s\n MAKE SURE YOU USE DT SHELL COMMANDS OF VERSION 4.1.9 OR HIGHER!\n\n\n'
% (e, ))
bgr = dtu.bgr_from_rgb(dtu.rgb_from_ros(img_msg))
self.info('Picture taken: %s ' % str(bgr.shape))
else:
self.info('Loading input image %s' % self.options.input)
bgr = dtu.bgr_from_jpg_fn(self.options.input)
if bgr.shape[1] != 640:
interpolation = cv2.INTER_CUBIC
bgr = dtu.d8_image_resize_fit(bgr, 640, interpolation)
self.info('Resized to: %s ' % str(bgr.shape))
# Disable the old calibration file
disable_old_homography(robot_name)
camera_info = get_camera_info_for_robot(robot_name)
homography_dummy = get_homography_default()
gpg = GroundProjectionGeometry(camera_info, homography_dummy)
res = OrderedDict()
try:
bgr_rectified = gpg.rectify(bgr, interpolation=cv2.INTER_CUBIC)
res['bgr'] = bgr
res['bgr_rectified'] = bgr_rectified
_new_matrix, res['rectified_full_ratio_auto'] = gpg.rectify_full(
bgr, ratio=1.65)
result = estimate_homography(bgr_rectified)
dtu.check_isinstance(result, HomographyEstimationResult)
if result.bgr_detected_refined is not None:
res['bgr_detected_refined'] = result.bgr_detected_refined
if not result.success:
raise Exception(result.error)
save_homography(result.H, robot_name)
msg = '''
To check that this worked well, place the robot on the road, and run:
rosrun complete_image_pipeline single_image
Look at the produced jpgs.
'''
self.info(msg)
finally:
dtu.write_bgr_images_as_jpgs(res, output)
