handle cameras of different resolutions in association vid

Author: mbpeterson70

demo/association_vid.py

@@ -10,14 +10,15 @@
 import open3d as o3d
 import tqdm
 
-from roman.map.map import Submap, SubmapParams, submaps_from_roman_map, load_roman_map
+from roman.map.map import ROMANMap
 from roman.align.results import SubmapAlignResults, plot_align_results, submaps_from_align_results
 from roman.params.data_params import DataParams
 from roman.viz import visualize_segment_on_img
 from roman.align.align_viz import create_association_geometries, create_ptcld_geometries
 
 from robotdatapy import transform
 from robotdatapy.exceptions import NoDataNearTimeException
+import robotdatapy as rdp
 
 @dataclass
 class VideoParams:
@@ -90,6 +91,8 @@ def rotate_pixel_coords(coords, img_w, img_h, rotation_code):
     pkl_file.close()
 
     submaps = submaps_from_align_results(results)
+    roman_map_paths = results.submap_io.inputs
+    roman_maps = [ROMANMap.from_pickle(roman_map_paths[i]) for i in range(2)]
 
     print(f'Loaded {len(submaps[0])} and {len(submaps[1])} submaps.')
 
@@ -152,8 +155,13 @@ def rotate_pixel_coords(coords, img_w, img_h, rotation_code):
     pose_data = []
     for i in range(2):
         os.environ[data_params[i].run_env] = args.runs[i]
-        pose_data.append(data_params[i].load_pose_data())
-        pose_data[-1].time_tol = 20.0
+        # for now, use the sparser trajectory from roman map for faster load time
+        # pose_data.append(data_params[i].load_pose_data())
+        # pose_data[-1].time_tol = 20.0
+        new_pose_data = rdp.data.PoseData.from_times_and_poses(roman_maps[i].times, roman_maps[i].trajectory)
+        new_pose_data.time_tol = 20.0
+        new_pose_data.T_postmultiply = np.linalg.inv(data_params[i].pose_data_params.T_camera_flu)
+        pose_data.append(new_pose_data)
 
     print(f"Loading image data...")
     img_data = []
@@ -166,6 +174,8 @@ def rotate_pixel_coords(coords, img_w, img_h, rotation_code):
     img_h = img_data[0].camera_params.height
     o3d_h = img_data[0].camera_params.height * 2
     o3d_w = int(o3d_h * vid_params.img_ratio - img_w)
+    # Assumes both cameras have the same width/height aspect ratio
+    img1_scale = img_w / img_data[1].camera_params.width
     if vid_params.camera_only:
         vid_w = img_w * 2
         vid_h = img_h
@@ -233,6 +243,8 @@ def rotate_pixel_coords(coords, img_w, img_h, rotation_code):
             # rotate image if specified
             if vid_params.img_rotations[i] is not None:
                 img_i = cv.rotate(img_i, vid_params.img_rotations[i])
+            if i == 1 and not np.isclose(img1_scale, 1.0):
+                img_i = cv.resize(img_i, dsize=(img_w, img_h))
             if not vid_params.camera_only:
                 viz_img[img_h*i:img_h*(i+1), o3d_w:] = img_i
             else:
@@ -254,6 +266,8 @@ def rotate_pixel_coords(coords, img_w, img_h, rotation_code):
                         continue
                     if vid_params.img_rotations[i] is not None:
                         new_outline = rotate_pixel_coords(new_outline, img_w, img_h, vid_params.img_rotations[i])
+                    if i == 1:
+                        new_outline *= img1_scale
                     seg_pixels.append(new_outline + pixel_offset)
                 if not success:
                     continue