Fixes for examples and VlcServerRos

Author: GoldenZephyr

examples/place_descriptor_debugging.py

@@ -58,8 +58,9 @@ def combine_images(left, right):
         pose = data.pose
         full_poses.append(pose.matrix())
 
-        uid = vlc_db.add_image(session_id, datetime.now(), ob.SparkImage(rgb=image))
-        embedding = embedding_model(image)
+        simg = ob.SparkImage(rgb=image)
+        uid = vlc_db.add_image(session_id, datetime.now(), simg)
+        embedding = embedding_model.infer(simg)
         vlc_db.update_embedding(uid, embedding)
 
         # To check our estimate vs. GT later

examples/salad_example.py

@@ -1,6 +1,7 @@
 import pathlib
 import imageio.v3 as iio
 from ouroboros_salad.salad_model import get_salad_model
+import ouroboros as ob
 
 
 def resource_dir():
@@ -11,4 +12,6 @@ def resource_dir():
     model = get_salad_model()
     img_d = iio.imread(resource_dir() / "arch.jpg")
 
-    out = model(img_d)
+    simg = ob.SparkImage(rgb=img_d)
+    out = model.infer(simg)
+    print("Salad model returned: ", out)

examples/uhumans_example_batch.py

@@ -6,11 +6,8 @@
 from spark_dataset_interfaces.rosbag_dataloader import RosbagDataLoader
 
 import ouroboros as ob
-from ouroboros.vlc_db.gt_lc_utils import (
-    VlcPose,
-    recover_pose,
-)
-from ouroboros_salad import get_salad_model
+from ouroboros_gt.gt_pose_recovery import get_gt_pose_model
+from ouroboros_salad.salad_model import get_salad_model
 
 
 def plot_heading(positions, rpy):
@@ -63,8 +60,9 @@ def plot_heading(positions, rpy):
 
         if not idx % 2 == 0:
             continue
-        uid = vlc_db.add_image(session_id, datetime.now(), ob.SparkImage(rgb=image))
-        embedding = embedding_model(image)
+        simg = ob.SparkImage(rgb=image)
+        uid = vlc_db.add_image(session_id, datetime.now(), simg)
+        embedding = embedding_model.infer(simg)
         # embedding = VlcPose(
         #    time_ns=data.timestamp,
         #    position=pose.translation,
@@ -95,7 +93,7 @@ def plot_heading(positions, rpy):
         continue
     putative_loop_closures.append((matches_to_frame[0][1], matches_to_frame[0][2]))
 
-
+pose_model = get_gt_pose_model()
 # Recover poses from matching loop closures
 for img_from, img_to in putative_loop_closures:
     key_from = img_from.metadata.image_uuid
@@ -106,7 +104,7 @@ def plot_heading(positions, rpy):
         # descriptors = generate_descriptors(img_from.image)
         keypoints = np.zeros([1, 2])
         pose = uid_to_pose[key_from]
-        desc = VlcPose(
+        desc = ob.VlcPose(
             time_ns=img_from.metadata.epoch_ns,
             position=pose[:3, 3],
             rotation=R.from_matrix(pose[:3, :3]).as_quat(),
@@ -123,7 +121,7 @@ def plot_heading(positions, rpy):
         # descriptors = generate_descriptors(img_to.image)
         keypoints = np.zeros([1, 2])
         pose = uid_to_pose[key_to]
-        desc = VlcPose(
+        desc = ob.VlcPose(
             time_ns=img_to.metadata.epoch_ns,
             position=pose[:3, 3],
             rotation=R.from_matrix(pose[:3, :3]).as_quat(),
@@ -134,8 +132,7 @@ def plot_heading(positions, rpy):
         vlc_db.update_keypoints(key_to, keypoints, descriptors)
         img_to = vlc_db.get_image(key_to)
 
-    # relative_pose = recover_pose(img_from, img_to)
-    relative_pose = recover_pose(img_from.descriptors, img_to.descriptors)
+    relative_pose = pose_model.infer(img_from, img_to)
     loop_closure = ob.SparkLoopClosure(
         from_image_uuid=key_from,
         to_image_uuid=key_to,

extra/ouroboros_ros/config/vlc_server_node.yaml

@@ -2,7 +2,7 @@ fixed_frame: world
 camera_frame: ground_truth/husky/base
 show_plots: true
 lc_recent_pose_lockout_s: 20
-lc_similarity_threshold: 0.55
+lc_similarity_threshold: 0.65
 lc_send_delay_s: 15
 place_method: "salad"
 keypoint_method: "ground_truth"

extra/ouroboros_ros/src/ouroboros_ros/vlc_server_ros.py

@@ -169,7 +169,7 @@ def image_callback(self, msg):
         if loop_closures is None:
             return
 
-        pose_cov_mat = self.build_pose_cov_mat
+        pose_cov_mat = self.build_pose_cov_mat()
         pg = PoseGraph()
         pg.header.stamp = rospy.Time.now()
         for lc in loop_closures:
@@ -189,7 +189,7 @@ def image_callback(self, msg):
 
             pg.edges.append(lc_edge)
         self.loop_closure_delayed_queue.append(
-            (rospy.Time.now().to_sec() + self.lc_send_delay, pg)
+            (rospy.Time.now().to_sec() + self.lc_send_delay_s, pg)
         )
 
     def build_pose_cov_mat(self):

setup.py

@@ -10,5 +10,12 @@
     package_dir={"": "src"},
     packages=find_packages("src"),
     package_data={"": ["*.yaml"]},
-    install_requires=["numpy", "imageio", "pyyaml", "pytest", "importlib-resources"],
+    install_requires=[
+        "numpy",
+        "imageio",
+        "pyyaml",
+        "pytest",
+        "importlib-resources",
+        "scipy",
+    ],
 )

src/ouroboros/utils/plotting_utils.py

@@ -32,6 +32,4 @@ def create_image_pair(left, right):
 def display_image_pair(left, right, window="matches"):
     img = create_image_pair(left, right)
     cv2.imshow(window, img.astype(np.uint8))
-    print("going to show")
     cv2.waitKey(10)
-    print("showed")

src/ouroboros_salad/salad_model.py

@@ -16,7 +16,7 @@ def __init__(self, model):
 
         self.input_transform = get_input_transform((322, 434))
 
-    def infer(self, image: ob.SparkImage, pose_hint: ob.VlcPose):
+    def infer(self, image: ob.SparkImage, pose_hint: ob.VlcPose = None):
         img_float = torch.tensor((image.rgb.transpose() / 255.0).astype(np.float32))
         img = self.input_transform(img_float)
         out = self.model(img.unsqueeze(0).to("cuda")).cpu().detach().numpy()