def _load_from_dict(self, init_dict):
self.name = init_dict["name"]
self.real_world_size = init_dict["real_world_size"]
self.registered_markers_undist = [
Surface_Marker_Aggregate(marker["id"], verts_uv=marker["verts_uv"])
for marker in init_dict["reg_markers"]
]
self.registered_markers_undist = {
m.id: m
for m in self.registered_markers_undist
}
self.registered_markers_dist = [
Surface_Marker_Aggregate(marker["id"], verts_uv=marker["verts_uv"])
for marker in init_dict["registered_markers_dist"]
]
self.registered_markers_dist = {
m.id: m
for m in self.registered_markers_dist
}
self.build_up_status = init_dict["build_up_status"]
try:
self.deprecated_definition = init_dict["deprecated"]
except KeyError:
pass
else:
logger.warning(
"You have loaded an old and deprecated surface definition! "
"Please re-define this surface for increased mapping accuracy!"
)
def _update_definition(self, idx, visible_markers, camera_model):
if not visible_markers:
return
all_verts_dist = np.array(
[m.verts_px for m in visible_markers.values()], dtype=np.float32
)
all_verts_dist.shape = (-1, 2)
all_verts_undist = camera_model.undistort_points_on_image_plane(all_verts_dist)
hull_undist = self._bounding_quadrangle(all_verts_undist)
undist_img_to_surf_trans_candidate = self._get_trans_to_norm_corners(
hull_undist
)
all_verts_undist.shape = (-1, 1, 2)
marker_surf_coords_undist = cv2.perspectiveTransform(
all_verts_undist, undist_img_to_surf_trans_candidate
)
hull_dist = self._bounding_quadrangle(all_verts_dist)
dist_img_to_surf_trans_candidate = self._get_trans_to_norm_corners(hull_dist)
all_verts_dist.shape = (-1, 1, 2)
marker_surf_coords_dist = cv2.perspectiveTransform(
all_verts_dist, dist_img_to_surf_trans_candidate
)
# Reshape to [marker, marker...]
# where marker = [[u1, v1], [u2, v2], [u3, v3], [u4, v4]]
marker_surf_coords_undist.shape = (-1, 4, 2)
marker_surf_coords_dist.shape = (-1, 4, 2)
# Add observations to library
for marker, uv_undist, uv_dist in zip(
visible_markers.values(), marker_surf_coords_undist, marker_surf_coords_dist
):
try:
self.registered_markers_undist[marker.id].add_observation(uv_undist)
self.registered_markers_dist[marker.id].add_observation(uv_dist)
except KeyError:
self.registered_markers_undist[marker.id] = Surface_Marker_Aggregate(
marker.id
)
self.registered_markers_undist[marker.id].add_observation(uv_undist)
self.registered_markers_dist[marker.id] = Surface_Marker_Aggregate(
marker.id
)
self.registered_markers_dist[marker.id].add_observation(uv_dist)
num_observations = sum(
[len(m.observations) for m in self.registered_markers_undist.values()]
)
self._avg_obs_per_marker = num_observations / len(
self.registered_markers_undist
)
self.build_up_status = self._avg_obs_per_marker / self._REQUIRED_OBS_PER_MARKER
if self.build_up_status >= 1:
self.prune_markers()
def _add_marker(self, marker_id, verts_px, camera_model, markers,
compensate_distortion):
surface_marker_dist = Surface_Marker_Aggregate(marker_id)
marker_verts_dist = np.array(verts_px).reshape((4, 2))
uv_coords_dist = self.map_to_surf(
marker_verts_dist,
camera_model,
compensate_distortion=compensate_distortion)
surface_marker_dist.add_observation(uv_coords_dist)
markers[marker_id] = surface_marker_dist
def _add_marker(
self, marker_id, verts_px, camera_model, markers, compensate_distortion
):
surface_marker_dist = Surface_Marker_Aggregate(marker_id)
marker_verts_dist = np.array(verts_px).reshape((4, 2))
uv_coords_dist = self.map_to_surf(
marker_verts_dist, camera_model, compensate_distortion=compensate_distortion
)
surface_marker_dist.add_observation(uv_coords_dist)
markers[marker_id] = surface_marker_dist
SURFACE_MARKER_AGGREGATE_V01_SQUARE_SERIALIZED_0_UNDIST,
SURFACE_MARKER_AGGREGATE_V01_SQUARE_SERIALIZED_1_UNDIST,
],
'registered_markers_dist': [
SURFACE_MARKER_AGGREGATE_V01_SQUARE_SERIALIZED_0_DIST,
SURFACE_MARKER_AGGREGATE_V01_SQUARE_SERIALIZED_1_DIST,
],
'build_up_status':
1.0,
'deprecated':
False
}
SURFACE_MARKER_AGGREGATE_V01_SQUARE_DESERIALIZED_0_UNDIST = Surface_Marker_Aggregate(
uid=Surface_Marker_UID("legacy:7"),
verts_uv=np.asarray([[1.4891731985457006e-14, -1.675372893802235e-14],
[0.07250381261110306, -0.000125938662677072],
[0.07127536088228226, 0.06990551203489304],
[-0.00038871169090270996, 0.07084081321954727]]))
SURFACE_MARKER_AGGREGATE_V01_SQUARE_DESERIALIZED_1_UNDIST = Surface_Marker_Aggregate(
uid=Surface_Marker_UID("legacy:57"),
verts_uv=np.asarray([[0.9311530590057373, 0.9295246005058289],
[1.000424861907959, 0.9302592873573303], [1.0, 1.0],
[0.9305340051651001, 0.9993915557861328]]))
SURFACE_MARKER_AGGREGATE_V01_SQUARE_DESERIALIZED_0_DIST = Surface_Marker_Aggregate(
uid=Surface_Marker_UID("legacy:7"),
verts_uv=np.asarray([[1.1920822368235908e-14, -1.7095567718611662e-14],
[0.06276015192270279, -0.0030732755549252033],
[0.0531853586435318, 0.06535717844963074],
[-0.010146145708858967, 0.06841480731964111]]))
SURFACE_MARKER_AGGREGATE_V01_SQUARE_DESERIALIZED_1_DIST = Surface_Marker_Aggregate(
uid=Surface_Marker_UID("legacy:57"),
],
"registered_markers_dist": [
SURFACE_MARKER_AGGREGATE_V01_APRILTAG_SERIALIZED_0_DIST,
SURFACE_MARKER_AGGREGATE_V01_APRILTAG_SERIALIZED_1_DIST,
],
"build_up_status":
1.0,
"deprecated":
False,
}
SURFACE_MARKER_AGGREGATE_V01_APRILTAG_DESERIALIZED_0_UNDIST = Surface_Marker_Aggregate(
uid=Surface_Marker_UID("apriltag_v3:tag36h11:72"),
verts_uv=np.asarray([
[0.9084336161613464, 0.9975847005844116],
[0.908658504486084, 0.8936676979064941],
[1.000919222831726, 0.8954850435256958],
[1.0, 1.0],
]),
)
SURFACE_MARKER_AGGREGATE_V01_APRILTAG_DESERIALIZED_1_UNDIST = Surface_Marker_Aggregate(
uid=Surface_Marker_UID("apriltag_v3:tag36h11:8"),
verts_uv=np.asarray([
[0.008291917853057384, 0.12046175450086594],
[0.0049503217451274395, 0.016743427142500877],
[0.0993560180068016, 0.01397931668907404],
[0.09943299740552902, 0.11729079484939575],
]),
)
SURFACE_MARKER_AGGREGATE_V01_APRILTAG_DESERIALIZED_0_DIST = Surface_Marker_Aggregate(
uid=Surface_Marker_UID("apriltag_v3:tag36h11:72"),
SURFACE_MARKER_AGGREGATE_V00_SERIALIZED_0_UNDIST,
SURFACE_MARKER_AGGREGATE_V00_SERIALIZED_1_UNDIST,
],
'registered_markers_dist': [
SURFACE_MARKER_AGGREGATE_V00_SERIALIZED_0_DIST,
SURFACE_MARKER_AGGREGATE_V00_SERIALIZED_1_DIST,
],
'build_up_status':
1.0,
'deprecated':
False
}
SURFACE_MARKER_AGGREGATE_V00_DESERIALIZED_0_UNDIST = Surface_Marker_Aggregate(
uid=Surface_Marker_UID("legacy:7"),
verts_uv=np.asarray([[2.0279725503600556e-14, -2.0718593602363743e-14],
[0.09232430905103683, 0.0054827057756483555],
[0.09320462495088577, 0.07479614019393921],
[0.008808332495391369, 0.07134716212749481]]))
SURFACE_MARKER_AGGREGATE_V00_DESERIALIZED_1_UNDIST = Surface_Marker_Aggregate(
uid=Surface_Marker_UID("legacy:57"),
verts_uv=np.asarray([[0.9255635738372803, 0.9278208017349243],
[0.9941799640655518, 0.928483784198761],
[0.9941900372505188, 0.9999602437019348],
[0.9251440763473511, 0.998592734336853]]))
SURFACE_MARKER_AGGREGATE_V00_DESERIALIZED_0_DIST = Surface_Marker_Aggregate(
uid=Surface_Marker_UID("legacy:7"),
verts_uv=np.asarray([[1.9851928125457982e-14, -1.923472062778219e-14],
[0.060702838003635406, -0.004638743586838245],
[0.05217646434903145, 0.06511983275413513],
[-0.009258653968572617, 0.06691507995128632]]))
SURFACE_MARKER_AGGREGATE_V00_DESERIALIZED_1_DIST = Surface_Marker_Aggregate(
