diff --git a/.gitignore b/.gitignore index 52697a6..f200498 100644 --- a/.gitignore +++ b/.gitignore @@ -29,4 +29,4 @@ venv *.rucha .vscode/ -.ruff_cache/ +.ruff_cache/ \ No newline at end of file diff --git a/configs/ground_truth_mapping.json b/configs/ground_truth_mapping.json index 09ea748..e43b851 100644 --- a/configs/ground_truth_mapping.json +++ b/configs/ground_truth_mapping.json @@ -321,6 +321,7 @@ "r1-mac-long-long", "r1-small-2_24", "r1-bad-mac2", - "r1-triple-straight" + "r1-triple-straight", + "desnat_endcap_to_other_endcap 55969329185666" ] } \ No newline at end of file diff --git a/map_processing/__init__.py b/map_processing/__init__.py index d965667..c65f5b5 100644 --- a/map_processing/__init__.py +++ b/map_processing/__init__.py @@ -40,6 +40,7 @@ class VertexType(Enum): TAG = 1 TAGPOINT = 2 WAYPOINT = 3 + CLOUD_ANCHOR = 4 # noinspection GrazieInspection diff --git a/map_processing/data_models.py b/map_processing/data_models.py index 69d4d32..50fddf7 100644 --- a/map_processing/data_models.py +++ b/map_processing/data_models.py @@ -393,7 +393,9 @@ def get_weights_from_end_vertex_mode(self, end_vertex_mode: Optional[VertexType] elif end_vertex_mode is None: return np.array(self.gravity) elif end_vertex_mode == VertexType.WAYPOINT: - return np.ones(6) # TODO: set to something other than identity? + return np.ones(6) # TODO: set to something other than identity? + elif end_vertex_mode == VertexType.CLOUD_ANCHOR: + return np.ones(6) # TODO: set to something other than identity? else: raise Exception(f"Edge of end type {end_vertex_mode} not recognized") @@ -524,6 +526,17 @@ class UGLocationDatum(BaseModel): pose_id: int +class UGCloudAnchorDatum(BaseModel): + """ + TODO: documentation + """ + + timestamp: float + cloudIdentifier: str + pose: conlist(Union[float, int], min_items=16, max_items=16) + poseId: int + + class GenerateParams(BaseModel): # noinspection PyUnresolvedReferences """ @@ -983,6 +996,7 @@ class UGDataSet(BaseModel): pose_data: List[UGPoseDatum] tag_data: List[List[UGTagDatum]] = [] generated_from: Optional[GenerateParams] = None + cloud_data: List[List[UGCloudAnchorDatum]] = [] # TODO: Add documentation for the following properties @@ -1047,6 +1061,16 @@ def tag_edge_measurements_matrix(self) -> np.ndarray: ) ) + @property + def cloud_anchor_edge_measurements_matrix(self) -> np.ndarray: + return ( + np.zeros((0, 4, 4)) + if len(self.cloud_data) == 0 + else np.vstack( + [[x.pose for x in frame] for frame in self.cloud_data] + ).reshape([-1, 4, 4], order="C") + ) + @property def timestamps(self) -> np.ndarray: return np.array([pose_datum.timestamp for pose_datum in self.pose_data]) @@ -1182,6 +1206,28 @@ def pose_ids(self) -> np.ndarray: ) ) + @property + def cloud_anchor_ids(self) -> np.ndarray: + return list( + itertools.chain( + *[ + [cloud_anchor.cloudIdentifier for cloud_anchor in frame] + for frame in self.cloud_data + ] + ) + ) + + @property + def cloud_anchor_pose_ids(self) -> np.ndarray: + return list( + itertools.chain( + *[ + [cloud_anchor.poseId for cloud_anchor in frame] + for frame in self.cloud_data + ] + ) + ) + @property def waypoint_names(self) -> List[str]: return [location_data.name for location_data in self.location_data] @@ -1542,6 +1588,7 @@ class OG2oOptimizer(BaseModel): tags: np.ndarray = Field(default_factory=lambda: np.zeros((0, 8))) tagpoints: np.ndarray = Field(default_factory=lambda: np.zeros((0, 3))) waypoints_arr: np.ndarray = Field(default_factory=lambda: np.zeros((0, 8))) + cloud_anchors: np.ndarray = Field(default_factory=lambda: np.zeros((0, 8))) waypoints_metadata: List[Dict] locationsAdjChi2: Optional[np.ndarray] = None visibleTagsCount: Optional[np.ndarray] = None diff --git a/map_processing/graph.py b/map_processing/graph.py index fa3a876..9179afd 100644 --- a/map_processing/graph.py +++ b/map_processing/graph.py @@ -782,6 +782,7 @@ def as_graph( data_set: Union[Dict, UGDataSet], fixed_vertices: Optional[Union[VertexType, Set[VertexType]]] = None, prescaling_opt: PrescalingOptEnum = PrescalingOptEnum.USE_SBA, + use_cloud_anchors: bool = False, ) -> Graph: """Convert a dictionary decoded from JSON into a Graph object. @@ -1000,11 +1001,62 @@ def as_graph( (waypoint_vertex_id, waypoint_index) ) + if use_cloud_anchors: + cloud_anchor_ids = data_set.cloud_anchor_ids + cloud_anchor_pose_ids = data_set.cloud_anchor_pose_ids + + unique_cloud_anchor_ids = set(cloud_anchor_ids) + num_unique_cloud_anchor_names = len(unique_cloud_anchor_ids) + cloud_anchor_vertex_id_by_cloud_anchor_id = dict( + zip( + unique_cloud_anchor_ids, + range( + unique_tag_ids.size + num_unique_waypoint_names, + unique_tag_ids.size + + num_unique_waypoint_names + + num_unique_cloud_anchor_names, + ), + ) + ) + cloud_anchor_id_by_cloud_anchor_vertex_id = dict( + zip( + cloud_anchor_vertex_id_by_cloud_anchor_id.values(), + cloud_anchor_vertex_id_by_cloud_anchor_id.keys(), + ) + ) + + cloud_anchor_vertex_id_and_index_by_frame_id: Dict[ + int, List[Tuple[int, int]] + ] = {} + for cloud_anchor_index, (cloud_anchor_id, cloud_anchor_frame) in enumerate( + zip(cloud_anchor_ids, cloud_anchor_pose_ids) + ): + cloud_anchor_vertex_id = cloud_anchor_vertex_id_by_cloud_anchor_id[ + cloud_anchor_id + ] + cloud_anchor_vertex_id_and_index_by_frame_id[ + cloud_anchor_frame + ] = cloud_anchor_vertex_id_and_index_by_frame_id.get( + cloud_anchor_frame, [] + ) + cloud_anchor_vertex_id_and_index_by_frame_id[cloud_anchor_frame].append( + (cloud_anchor_vertex_id, cloud_anchor_index) + ) + + cloud_anchor_edge_measurements_matrix = ( + data_set.cloud_anchor_edge_measurements_matrix + ) + cloud_anchor_edge_measurements = transform_matrix_to_vector( + cloud_anchor_edge_measurements_matrix + ) + num_tag_edges = edge_counter = 0 vertices = {} edges = {} counted_tag_vertex_ids = set() counted_waypoint_vertex_ids = set() + if use_cloud_anchors: + counted_cloud_anchor_vertex_ids = set() previous_vertex_uid = None first_odom_processed = False if use_sba: @@ -1127,6 +1179,45 @@ def as_graph( num_tag_edges += 1 edge_counter += 1 + if use_cloud_anchors: + for ( + cloud_anchor_vertex_id, + cloud_anchor_index, + ) in cloud_anchor_vertex_id_and_index_by_frame_id.get( + int(odom_frame), [] + ): + if cloud_anchor_vertex_id not in counted_cloud_anchor_vertex_ids: + vertices[cloud_anchor_vertex_id] = Vertex( + mode=VertexType.CLOUD_ANCHOR, + estimate=transform_matrix_to_vector( + pose_matrices[i].dot( + cloud_anchor_edge_measurements_matrix[ + cloud_anchor_index + ] + ) + ), + fixed=VertexType.CLOUD_ANCHOR in fixed_vertices, + meta_data={ + "cloud_anchor_id": cloud_anchor_id_by_cloud_anchor_vertex_id[ + cloud_anchor_vertex_id + ] + }, + ) + counted_cloud_anchor_vertex_ids.add(cloud_anchor_vertex_id) + edges[edge_counter] = Edge( + startuid=current_odom_vertex_uid, + enduid=cloud_anchor_vertex_id, + corner_verts=None, + information_prescaling=None, # TODO: investigate + camera_intrinsics=None, + measurement=cloud_anchor_edge_measurements[cloud_anchor_index], + start_end=( + vertices[current_odom_vertex_uid], + vertices[cloud_anchor_vertex_id], + ), + ) + edge_counter += 1 + # Connect odom to waypoint vertex for ( waypoint_vertex_id, diff --git a/map_processing/graph_opt_hl_interface.py b/map_processing/graph_opt_hl_interface.py index 307b0c6..78620fc 100644 --- a/map_processing/graph_opt_hl_interface.py +++ b/map_processing/graph_opt_hl_interface.py @@ -393,6 +393,8 @@ def optimize_graph( orig_odometry=before_opt_map.locations, opt_tag_verts=opt_result_map.tags, opt_tag_corners=opt_result_map.tagpoints, + orig_cloud_anchor=opt_result_map.cloud_anchors, + opt_cloud_anchor=before_opt_map.cloud_anchors, opt_waypoint_verts=( opt_result_map.waypoints_metadata, opt_result_map.waypoints_arr, diff --git a/map_processing/graph_opt_plot_utils.py b/map_processing/graph_opt_plot_utils.py index aeaf6e2..f065f31 100644 --- a/map_processing/graph_opt_plot_utils.py +++ b/map_processing/graph_opt_plot_utils.py @@ -107,6 +107,8 @@ def plot_optimization_result( orig_odometry: np.ndarray, opt_tag_verts: np.ndarray, opt_tag_corners: np.ndarray, + opt_cloud_anchor: np.ndarray, + orig_cloud_anchor: np.ndarray, opt_waypoint_verts: Tuple[List, np.ndarray], orig_tag_verts: Optional[np.ndarray] = None, ground_truth_tags: Optional[List[SE3Quat]] = None, @@ -264,7 +266,7 @@ def plot_optimization_result( ) plt.plot( opt_odometry[:, 0], - orig_odometry[:, 1], + opt_odometry[:, 1], opt_odometry[:, 2], "-", label="Optimized Odom Vertices", @@ -288,6 +290,40 @@ def plot_optimization_result( linewidth=0.75, c="b", ) + + if three_dimensional: + plt.scatter( + opt_cloud_anchor[:, 0], + opt_cloud_anchor[:, 1], + opt_cloud_anchor[:, 2], + facecolors="none", + edgecolors="r", + label="Optimized Cloud Anchors", + ) + plt.scatter( + orig_cloud_anchor[:, 0], + orig_cloud_anchor[:, 1], + orig_cloud_anchor[:, 2], + facecolors="none", + edgecolors="y", + label="Raw Cloud Anchors", + ) + else: + plt.scatter( + opt_cloud_anchor[:, 0], + opt_cloud_anchor[:, 2], + facecolors="none", + edgecolors="r", + label="Optimized Cloud Anchors", + ) + plt.scatter( + orig_cloud_anchor[:, 0], + orig_cloud_anchor[:, 2], + facecolors="none", + edgecolors="y", + label="Raw Cloud Anchors", + ) + plt.legend(bbox_to_anchor=(1.05, 1), fontsize="small") axis_equal(ax, three_dimensional) plt.gcf().set_dpi(300) diff --git a/map_processing/graph_opt_utils.py b/map_processing/graph_opt_utils.py index 5cccb54..714f6f3 100644 --- a/map_processing/graph_opt_utils.py +++ b/map_processing/graph_opt_utils.py @@ -55,6 +55,7 @@ def optimizer_to_map( tagpoints = [] tags = [] waypoints = [] + cloud_anchors = [] waypoint_metadata = [] exaggerate_tag_corners = True for i in optimizer.vertices(): @@ -96,6 +97,9 @@ def optimizer_to_map( pose_with_metadata = np.concatenate([pose, [i]]) waypoints.append(pose_with_metadata) waypoint_metadata.append(vertices[i].meta_data) + elif mode == VertexType.CLOUD_ANCHOR: + pose_with_metadata = np.concatenate([pose, [i]]) + cloud_anchors.append(pose_with_metadata) locations_arr = np.array(locations) locations_arr = ( locations_arr[locations_arr[:, -1].argsort()] @@ -105,12 +109,16 @@ def optimizer_to_map( tags_arr = np.array(tags) if len(tags) > 0 else np.zeros((0, 8)) tagpoints_arr = np.array(tagpoints) if len(tagpoints) > 0 else np.zeros((0, 3)) waypoints_arr = np.array(waypoints) if len(waypoints) > 0 else np.zeros((0, 8)) + cloud_anchors_arr = ( + np.array(cloud_anchors) if len(cloud_anchors) > 0 else np.zeros((0, 8)) + ) return OG2oOptimizer( locations=locations_arr, tags=tags_arr, tagpoints=tagpoints_arr, waypoints_arr=waypoints_arr, waypoints_metadata=waypoint_metadata, + cloud_anchors=cloud_anchors_arr, ) diff --git a/map_processing/graph_vertex_edge_classes.py b/map_processing/graph_vertex_edge_classes.py index f959ca9..b0126e5 100644 --- a/map_processing/graph_vertex_edge_classes.py +++ b/map_processing/graph_vertex_edge_classes.py @@ -148,6 +148,8 @@ def compute_information( lin_vel_var=compute_inf_params.lin_vel_var, ang_vel_var=compute_inf_params.ang_vel_var, ) + elif self.start_end[1].mode == VertexType.CLOUD_ANCHOR: + self._compute_information_cloud_anchor(weights_vec) else: self._compute_information_se3_obs( weights_vec, compute_inf_params.tag_var @@ -164,6 +166,10 @@ def compute_information( f"vector argument was an array of shape {weights_vec.shape}" ) + def _compute_information_cloud_anchor(self, weights_vec: np.ndarray): + # TODO: ratio between pos and orientation? + self.information = np.diag(weights_vec) + def _compute_information_se3_nonzero_delta_t( self, weights_vec: np.ndarray, diff --git a/map_processing/sweep.py b/map_processing/sweep.py index 70a4fbc..9746bc5 100644 --- a/map_processing/sweep.py +++ b/map_processing/sweep.py @@ -47,6 +47,7 @@ def run_param_sweep( fixed_vertices: Optional[Set[VertexType]] = None, verbose: bool = False, num_processes: int = 1, + use_cloud_anchors: bool = False, ) -> Tuple[float, int, OResult]: graph_to_opt = Graph.as_graph( mi.map_dct, @@ -54,6 +55,7 @@ def run_param_sweep( prescaling_opt=PrescalingOptEnum.USE_SBA if base_oconfig.is_sba else PrescalingOptEnum.FULL_COV, + use_cloud_anchors=use_cloud_anchors, ) sweep_arrs: Dict[OConfig.OConfigEnum, np.ndarray] = {} @@ -167,6 +169,7 @@ def sweep_params( cache_results: bool = False, upload_best: bool = False, cms: CacheManagerSingleton = None, + use_cloud_anchors: bool = False, ) -> OSweepResults: """ TODO: Documentation and add SBA weighting to the sweeping @@ -180,6 +183,7 @@ def sweep_params( fixed_vertices=fixed_vertices, verbose=verbose, num_processes=num_processes, + use_cloud_anchors=use_cloud_anchors, ) # Find min metrics from all the parameters @@ -291,6 +295,8 @@ def sweep_params( orig_odometry=pre_map.locations, opt_tag_verts=opt_map.tags, opt_tag_corners=opt_map.tagpoints, + orig_cloud_anchor=pre_map.cloud_anchors, + opt_cloud_anchor=opt_map.cloud_anchors, opt_waypoint_verts=(opt_map.waypoints_metadata, opt_map.waypoints_arr), orig_tag_verts=pre_map.tags, ground_truth_tags=GTDataSet.gt_data_set_from_dict_of_arrays( diff --git a/run_scripts/optimize_graphs_and_manage_cache.py b/run_scripts/optimize_graphs_and_manage_cache.py index 96ed6ee..302f5ba 100644 --- a/run_scripts/optimize_graphs_and_manage_cache.py +++ b/run_scripts/optimize_graphs_and_manage_cache.py @@ -131,6 +131,12 @@ def make_parser() -> argparse.ArgumentParser: "running. This option is mutually exclusive with the -c option.", default=False, ) + p.add_argument( + "-ca", + action="store_true", + help="Optimize Graph with Cloud Anchors", + default=False, + ) p.add_argument( "-c", action="store_true", @@ -233,8 +239,12 @@ def make_parser() -> argparse.ArgumentParser: args = parser.parse_args() if args.c and (args.F or args.s): - print("Mutually exclusive flags with -c used") - exit(-1) + raise ValueError("Mutually exclusive flags with -c used") + + if args.ca and (args.pso == PrescalingOptEnum.USE_SBA.value or not args.s): + raise ValueError( + "Cloud Anchors are currently only supported in no SBA parameter sweeps" + ) # Fetch the service account key JSON file contents env_variable = os.environ.get("GOOGLE_APPLICATION_CREDENTIALS") @@ -292,6 +302,7 @@ def make_parser() -> argparse.ArgumentParser: num_processes=args.np, upload_best=args.F, cms=cms, + use_cloud_anchors=args.ca, ) # If you simply want to run the optimizer with specified weights