def write_images(bag_filename, topic, basename):
""" Returns the name of the first created file """
dtu.logger.info(f"reading topic {topic!r} from {bag_filename!r}")
bag = rosbag.Bag(bag_filename)
nfound = bag.get_message_count(topic)
if nfound == 0:
msg = f"Found 0 messages for topic {topic}"
msg += "\n\n" + dtu.indent(dbu.get_summary_of_bag_messages(bag), " ")
raise ValueError(msg)
res = dbu.d8n_read_all_images_from_bag(bag, topic)
n = len(res)
filenames = []
for i in range(n):
rgb = res[i]["rgb"]
data: bytes = dtu.png_from_bgr(dtu.bgr_from_rgb(rgb))
if n == 1:
fn = basename + ".png"
else:
fn = basename + f"-{i:02d}.png"
filenames.append(fn)
dtu.write_data_to_file(data, fn)
bag.close()
return filenames[0]
def compare_faster2():
variables = {}
variables["alpha"] = dict(min=-180,
max=180,
description="angle",
resolution=5,
units="deg",
units_display="deg")
variables["r"] = dict(min=3,
max=5,
description="distance",
resolution=0.2,
units="m",
units_display="cm")
# this will fail if precision is float32
gh = GridHelper(variables, precision="float64")
val_fast = gh.create_new()
val_fast.fill(0)
val_slow = gh.create_new()
val_slow.fill(0)
od = dtu.get_output_dir_for_test()
F = 1
alpha0 = 177
# r0 = 4
r0 = 4.1
w0 = 1
value = dict(alpha=alpha0, r=r0)
gh.add_vote(val_slow, value, w0, F)
values = np.zeros((2, 1))
values[0, 0] = alpha0
values[1, 0] = r0
weights = np.zeros(1)
weights[0] = w0
gh.add_vote_faster(val_fast, values, weights, F)
print(f"sum slow: {np.sum(val_slow)}")
print(f"sum fast: {np.sum(val_fast)}")
d = grid_helper_plot(gh, val_slow)
fn = os.path.join(od, "compare_faster_slow.jpg")
dtu.write_data_to_file(d.get_png(), fn)
d = grid_helper_plot(gh, val_fast)
fn = os.path.join(od, "compare_faster_fast.jpg")
dtu.write_data_to_file(d.get_png(), fn)
f = lambda x: f" {x:5f}" if x else " "
print((dtu.indent(array_as_string(val_fast, f), "val_fast ")))
print((dtu.indent(array_as_string(val_slow, f), "val_slow ")))
assert_almost_equal(val_fast, val_slow)
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 save_homography(H: np.ndarray, robot_name: str) -> None:
dtu.logger.info(f"Homography:\n {H}")
# Check if specific point in matrix is larger than zero (this would definitly mean we're having a
# corrupted rotation matrix)
if H[1][2] > 0:
msg = "WARNING: Homography could be corrupt."
msg += f"\n {H}"
raise Exception(msg)
ob = {"homography": sum(H.reshape(9, 1).tolist(), [])}
import yaml as alt
s = alt.dump(ob)
s += "\n# Calibrated on dtu.format_time_as_YYYY_MM_DD(time.time())"
fn = get_extrinsics_filename(robot_name)
dtu.write_data_to_file(s, fn)
def grid_visualization():
variables = {}
variables["alpha"] = dict(
min=-np.pi / 2,
max=np.pi / 2,
description="angle",
resolution=np.deg2rad(10),
units="rad",
units_display="deg",
)
variables["r"] = dict(min=3, max=5, description="distance", resolution=0.1, units="m", units_display="cm")
gh = GridHelper(variables)
val = gh.create_new()
val.fill(0)
val += np.random.randn(*val.shape)
val[1, 2] = 0
d = grid_helper_plot(gh, val)
od = dtu.get_output_dir_for_test()
fn = os.path.join(od, "grid_visualization.jpg")
dtu.write_data_to_file(d.get_png(), fn)
def compare_faster():
variables = {}
variables["alpha"] = dict(min=-180,
max=180,
description="angle",
resolution=5,
units="deg",
units_display="deg")
variables["r"] = dict(min=3,
max=5,
description="distance",
resolution=0.1,
units="m",
units_display="cm")
# this will fail if precision is float32
gh = GridHelper(variables, precision="float64")
val_fast = gh.create_new()
val_fast.fill(0)
val_slow = gh.create_new()
val_slow.fill(0)
od = dtu.get_output_dir_for_test()
F = 1
alpha0 = 7
# r0 = 4
r0 = 4.1
w0 = 1.0
value = dict(alpha=alpha0, r=r0)
gh.add_vote(val_slow, value, w0, F)
assert_equal(np.sum(val_slow > 0), 9)
values = np.zeros((2, 1))
values[0, 0] = alpha0
values[1, 0] = r0
weights = np.zeros(1)
weights[0] = w0
gh.add_vote_faster(val_fast, values, weights, F)
assert_equal(np.sum(val_fast > 0), 9)
d = grid_helper_plot(gh, val_slow)
fn = os.path.join(od, "compare_faster_slow.jpg")
dtu.write_data_to_file(d.get_png(), fn)
d = grid_helper_plot(gh, val_fast)
fn = os.path.join(od, "compare_faster_fast.jpg")
dtu.write_data_to_file(d.get_png(), fn)
D = val_fast - val_slow
diff = np.max(np.abs(D))
print(f"diff: {diff!r}")
if diff > 1e-8:
print(dtu.indent(array_as_string_sign(val_fast), "val_fast "))
print(dtu.indent(array_as_string_sign(val_slow), "val_slow "))
print(dtu.indent(array_as_string_sign(D), "Diff "))
print(f"non zero val_fast: {val_fast[val_fast > 0]}")
print(f"non zero val_slow: {val_slow[val_slow > 0]}")
assert_almost_equal(val_fast, val_slow)
def voting_kernel1():
resolution = 10.0
variables = {}
variables["x"] = dict(
min=100, max=500, description="x", resolution=resolution, units="cm", units_display="cm"
)
variables["y"] = dict(
min=100, max=500, description="y", resolution=resolution, units="cm", units_display="cm"
)
gh = GridHelper(variables)
votes = gh.create_new()
votes.fill(0)
points = []
estimated = []
estimated_weighted = []
F = 1
N = 25
errors_x = []
errors_x_w = []
for i in range(N):
dx = resolution / N
Dy = 70
Dx = 70
u = i / 5
v = i % 5
x = 125 + dx * i + Dx * u
y = 127 + Dy * v
p = dict(x=x, y=y)
points.append(p)
weight = 1
gh.add_vote(votes, p, weight=weight, F=F)
tmp = gh.create_new()
tmp.fill(0)
gh.add_vote(tmp, p, weight=weight, F=F)
assert_almost_equal(tmp.sum(), weight)
estimate = gh.get_max(tmp)
estimated.append(estimate)
estimate_weigthed = gh.get_max_weighted(tmp, F=F)
estimated_weighted.append(estimate_weigthed)
errors_x.append(p["x"] - estimate["x"])
errors_x_w.append(p["x"] - estimate_weigthed["x"])
errors_x = np.array(errors_x)
errors_x_w = np.array(errors_x_w)
dtu.logger.debug(f"errors_x: {errors_x}")
dtu.logger.debug(f"mean: {np.abs(errors_x).mean()}")
dtu.logger.debug(f"errors_x_w: {errors_x_w}")
dtu.logger.debug(f"mean: {np.abs(errors_x_w).mean()}")
assert errors_x.max() <= +resolution / 2
assert errors_x.min() >= -resolution / 2
assert np.abs(errors_x_w).max() <= resolution / 10
a = dtu.CreateImageFromPylab(dpi=1000)
with a as pylab:
grid_helper_plot_field(gh, votes, pylab)
pylab.axis("equal")
grid_helper_annotate_axes(gh, pylab)
for p in points:
grid_helper_mark_point(gh, pylab, p, color="blue", markersize=4)
for e in estimated:
grid_helper_mark_point(gh, pylab, e, color="red", markersize=3)
for e in estimated_weighted:
grid_helper_mark_point(gh, pylab, e, color="green", markersize=3)
b = dtu.CreateImageFromPylab(dpi=1000)
with b as pylab:
x = np.array([_["x"] for _ in points])
xe = np.array([_["x"] for _ in estimated])
xew = np.array([_["x"] for _ in estimated_weighted])
xe -= x
xew -= x
x = x * 0 # XXX?
pylab.plot(x, ".", label="x")
pylab.plot(xe, ".", label="x estimated")
pylab.plot(xew, ".", label="x estimated weighted")
pylab.legend()
od = dtu.get_output_dir_for_test()
fn = os.path.join(od, "voting_kernel1.jpg")
dtu.write_data_to_file(a.get_png(), fn)
fn = os.path.join(od, "errors.jpg")
dtu.write_data_to_file(b.get_png(), fn)