[WIP] Add Cloud Anchors to G2O Optiization

.gitignore

@@ -29,4 +29,4 @@ venv
 *.rucha
 
 .vscode/
-.ruff_cache/
+.ruff_cache/

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"
   ]
 }

map_processing/__init__.py

@@ -40,6 +40,7 @@ class VertexType(Enum):
     TAG = 1
     TAGPOINT = 2
     WAYPOINT = 3
+    CLOUD_ANCHOR = 4
 
 
 # noinspection GrazieInspection

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

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,

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,

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)

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,
     )
 
 

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,

map_processing/sweep.py

@@ -47,13 +47,15 @@ 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,
         fixed_vertices=fixed_vertices,
         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(