View offset improvements: Automatically compute it for WMR and make it 2D

- Make view_offset two-dimensional
- Compute it automatically from WMR extrinsics

data/d455_640x480_calib.json

@@ -97,7 +97,7 @@
             0.0007071067802939111
         ],
         "cam_time_offset_ns": 0,
-        "view_offset": 0,
+        "view_offset": [0, 0],
         "vignette": []
     }
 }

data/d455_848x480_calib.json

@@ -97,7 +97,7 @@
             0.0007071067802939111
         ],
         "cam_time_offset_ns": 0,
-        "view_offset": 0,
+        "view_offset": [0, 0],
         "vignette": []
     }
 }

data/d455_calib.json

@@ -99,7 +99,7 @@
             0.0007071067802939111
         ],
         "cam_time_offset_ns": 0,
-        "view_offset": 0,
+        "view_offset": [0, 0],
         "vignette": []
     }
 }

data/euroc_ds_calib.json

@@ -225,6 +225,6 @@
         "mocap_time_offset_ns": 0,
         "mocap_to_imu_offset_ns": 140763258159875,
         "cam_time_offset_ns": 0,
-        "view_offset": 0
+        "view_offset": [0, 0]
     }
 }

data/euroc_eucm_calib.json

@@ -209,6 +209,6 @@
         "mocap_time_offset_ns": 0,
         "mocap_to_imu_offset_ns": 140763258159875,
         "cam_time_offset_ns": 0,
-        "view_offset": 0
+        "view_offset": [0, 0]
     }
 }

data/euroc_rt8_calib.json

@@ -239,6 +239,6 @@
         "mocap_time_offset_ns": 0,
         "mocap_to_imu_offset_ns": 140763258159875,
         "cam_time_offset_ns": 0,
-        "view_offset": 0
+        "view_offset": [0, 0]
     }
 }

data/monado/wmr-tools/wmr2bslt_calib.py

@@ -37,6 +37,37 @@ def rmat2quat(r):
     return np.array([x, y, z, w])
 
 
+def project(intrinsics, x, y, z):
+    fx, fy, cx, cy, k1, k2, k3, k4, k5, k6, p1, p2 = (
+        intrinsics["fx"],
+        intrinsics["fy"],
+        intrinsics["cx"],
+        intrinsics["cy"],
+        intrinsics["k1"],
+        intrinsics["k2"],
+        intrinsics["k3"],
+        intrinsics["k4"],
+        intrinsics["k5"],
+        intrinsics["k6"],
+        intrinsics["p1"],
+        intrinsics["p2"],
+    )
+
+    xp = x / z
+    yp = y / z
+    r2 = xp * xp + yp * yp
+    cdist = (1 + r2 * (k1 + r2 * (k2 + r2 * k3))) / (
+        1 + r2 * (k4 + r2 * (k5 + r2 * k6))
+    )
+    deltaX = 2 * p1 * xp * yp + p2 * (r2 + 2 * xp * xp)
+    deltaY = 2 * p2 * xp * yp + p1 * (r2 + 2 * yp * yp)
+    xpp = xp * cdist + deltaX
+    ypp = yp * cdist + deltaY
+    u = fx * xpp + cx
+    v = fy * ypp + cy
+    return u, v
+
+
 def extrinsics(j, cam):
     # NOTE: The `Rt` field seems to be a transform from the sensor to HT0 (i.e.,
     # from HT0 space to sensor space). For basalt we need the transforms
@@ -106,11 +137,37 @@ def intrinsics(j, cam):
             "k4": model_params[7],
             "k5": model_params[8],
             "k6": model_params[9],
-            "rpmax": model_params[14]
+            "rpmax": model_params[14],
         },
     }
 
 
+def view_offset(j):
+    """
+    This is a very rough offset in pixels between the two cameras. Originally we
+    needed to manually estimate it like explained and shown here
+    https://youtu.be/jyQKjyRVMS4?t=670.
+    With this calculation we get a similar number without the need to open Gimp.
+
+    In reality this offset changes based on distance to the point, nonetheless
+    it helps to get some features tracked in the right camera.
+    """
+
+    # Rough approximation of how far from the cameras features will likely be in your room
+    DISTANCE_TO_WALL = 2  # In meters
+
+    cam1 = get(j, "HT1")
+    width = cam1["SensorWidth"]
+    height = cam1["SensorHeight"]
+    cam1_intrinsics = intrinsics(j, "HT1")["intrinsics"]
+    T_c1_c0 = rt2mat(cam1["Rt"])  # Maps a point in c0 space to c1 space
+    p = np.array([0, 0, DISTANCE_TO_WALL, 1])  # Fron tof c0, in homogeneous coords
+    p_in_c1 = T_c1_c0 @ p  # Point in c1 coordinates
+    u, v = project(cam1_intrinsics, *p_in_c1[0:3])
+    view_offset = [width / 2 - u, height / 2 - v]  # We used a point in the middle of c0
+    return view_offset
+
+
 def calib_accel_bias(j):
     # https://github.com/microsoft/Azure-Kinect-Sensor-SDK/blob/2feb3425259bf803749065bb6d628c6c180f8e77/include/k4ainternal/calibration.h#L48-L77
     # https://vladyslavusenko.gitlab.io/basalt-headers/classbasalt_1_1CalibAccelBias.html#details
@@ -176,13 +233,6 @@ def main():
     # But in monado we just average those 4 samples to reduce the noise. So we have 250hz.
     IMU_UPDATE_RATE = 250
 
-    # This is a very rough offset in pixels between the two cameras. I manually
-    # measured it for some particular point in some particular pair of images
-    # for my Odyssey+. In reality this offset changes based on distance to the
-    # point, nonetheless it helps to get some features tracked in the right
-    # camera.
-    VIEW_OFFSET = 247
-
     out_calib = {
         "value0": {
             "T_imu_cam": [extrinsics(j, "HT0"), extrinsics(j, "HT1")],
@@ -196,7 +246,7 @@ def main():
             "accel_bias_std": bias_std(j, "Accelerometer"),
             "gyro_bias_std": bias_std(j, "Gyro"),
             "cam_time_offset_ns": 0,
-            "view_offset": VIEW_OFFSET,
+            "view_offset": view_offset(j),
             "vignette": [],
         }
     }

data/odysseyplus_kb4_calib.json

@@ -108,7 +108,10 @@
             0.009999999873689375
         ],
         "cam_time_offset_ns": 0,
-        "view_offset": 247,
+        "view_offset": [
+            248.7971689190779,
+            -5.785840906463989
+        ],
         "vignette": []
     }
 }

data/odysseyplus_rt8_calib.json

@@ -5,19 +5,19 @@
                 "px": -0.08394275605678558,
                 "py": -0.0025952591095119715,
                 "pz": 0.0026445253752171993,
-                "qx": -0.17704728245735168,
+                "qx": -0.17704726835670878,
-                "qy": -0.19861078262329102,
+                "qy": -0.19861076922141732,
-                "qz": 0.035641565918922424,
+                "qz": 0.03564156642768224,
-                "qw": 0.9632952809333801
+                "qw": 0.9632952306248018
             },
             {
-                "px": 0.021131351590156555,
+                "px": 0.02113135496007891,
-                "py": -0.002306802896782756,
+                "py": -0.0023068031820524327,
-                "pz": 0.0023415968753397465,
+                "pz": 0.002341595642298417,
-                "qx": -0.18363775312900543,
+                "qx": -0.1836377420226455,
-                "qy": 0.19827023148536682,
+                "qy": 0.1982701960061761,
-                "qz": -0.03922446444630623,
+                "qz": -0.03922445856481098,
-                "qw": 0.961991548538208
+                "qw": 0.9619914477874183
             }
         ],
         "intrinsics": [
@@ -29,14 +29,14 @@
                     "cx": 324.3333053588867,
                     "cy": 245.22674560546875,
                     "k1": 0.6257319450378418,
-                    "k2": 0.46612036228179932,
+                    "k2": 0.4661203622817993,
                     "p1": -0.00018502399325370789,
-                    "p2": -4.2882973502855748e-5,
+                    "p2": -4.288297350285575e-05,
-                    "k3": 0.0041795829311013222,
+                    "k3": 0.004179582931101322,
-                    "k4": 0.89431935548782349,
+                    "k4": 0.8943193554878235,
-                    "k5": 0.54253977537155151,
+                    "k5": 0.5425397753715515,
                     "k6": 0.06621214747428894,
-                    "rpmax": 2.7941114902496338
+                    "rpmax": 2.794111490249634
                 }
             },
             {
@@ -46,15 +46,15 @@
                     "fy": 270.2496528625488,
                     "cx": 323.54679107666016,
                     "cy": 250.34966468811035,
-                    "k1": 0.55718272924423218,
+                    "k1": 0.5571827292442322,
                     "k2": 0.22437196969985962,
                     "p1": 0.00011782468209275976,
                     "p2": 0.0001220563062815927,
-                    "k3": 0.0068156048655509949,
+                    "k3": 0.006815604865550995,
-                    "k4": 0.83317267894744873,
+                    "k4": 0.8331726789474487,
-                    "k5": 0.26174271106719971,
+                    "k5": 0.2617427110671997,
-                    "k6": 0.043505862355232239,
+                    "k6": 0.04350586235523224,
-                    "rpmax": 2.7899987697601318
+                    "rpmax": 2.789998769760132
                 }
             }
         ],
@@ -68,9 +68,6 @@
                 480
             ]
         ],
-        "// about calib_accel/gyro_bias": "Bias values (first three elements) are negated, in basalt they get substracted, but in azure-kinect they are added",
-        "// -                          ": "Also the mixing matrix has I3x3 substracted, i.e. diagonal are one unit smaller",
-        "// about calib_accel_bias": "See https://gitlab.com/VladyslavUsenko/basalt-headers/-/issues/8 to understand why accel only uses 6 elements of the mixing matrix",
         "calib_accel_bias": [
             0.18563582003116608,
             -0.057621683925390244,
@@ -78,34 +75,34 @@
             -0.001865983009338379,
             0.00030630044057033956,
             -0.0001675997773418203,
-            0.0009244680404663,
+            0.0009244680404663086,
             0.000295753387035802,
             -0.0022075772285461426
         ],
         "calib_gyro_bias": [
             -0.016492774710059166,
             0.01642640121281147,
-            0.0045625129714608192,
+            0.004562512971460819,
-            0.0000957250595093,
+            9.572505950927734e-05,
-            -0.00082384591223672032,
+            -0.0008238459122367203,
             -0.0015064212493598461,
-            -0.00082343036774545908,
+            -0.0008234303677454591,
             -0.00041097402572631836,
-            -4.7357993935293052e-6,
+            -4.735799393529305e-06,
             -0.0015050634974613786,
-            -4.7339185584860388e-6,
+            -4.733918558486039e-06,
             -0.0008063316345214844
         ],
-        "imu_update_rate": 250.0,
+        "imu_update_rate": 250,
         "accel_noise_std": [
             0.010700000450015068,
             0.010700000450015068,
             0.010700000450015068
         ],
         "gyro_noise_std": [
-            0.00095000001601874828,
+            0.0009500000160187483,
-            0.00095000001601874828,
+            0.0009500000160187483,
-            0.00095000001601874828
+            0.0009500000160187483
         ],
         "accel_bias_std": [
             0.0999999988824129,
@@ -118,7 +115,10 @@
             0.009999999873689375
         ],
         "cam_time_offset_ns": 0,
-        "view_offset": 247,
+        "view_offset": [
+            248.7971689190779,
+            -5.785840906463989
+        ],
         "vignette": []
     }
 }

data/reverbg1_calib.json

@@ -115,7 +115,7 @@
             0.009999999873689375
         ],
         "cam_time_offset_ns": 0,
-        "view_offset": 247,
+        "view_offset": [247, 0],
         "vignette": []
     }
 }

data/tumvi_512_ds_calib.json

@@ -105,7 +105,7 @@
         "mocap_time_offset_ns": 0,
         "mocap_to_imu_offset_ns": 0,
         "cam_time_offset_ns": 0,
-        "view_offset": 0,
+        "view_offset": [0, 0],
         "vignette": [
             {
                 "value0": 0,

data/tumvi_512_eucm_calib.json

@@ -105,7 +105,7 @@
         "mocap_time_offset_ns": 0,
         "mocap_to_imu_offset_ns": 0,
         "cam_time_offset_ns": 0,
-        "view_offset": 0,
+        "view_offset": [0, 0],
         "vignette": [
             {
                 "value0": 0,

data/x3pro_calib.json

@@ -97,7 +97,7 @@
             0.0001
         ],
         "cam_time_offset_ns": 0,
-        "view_offset": 300,
+        "view_offset": [0, 0],
         "vignette": []
     }
 }

include/basalt/optical_flow/frame_to_frame_optical_flow.h

@@ -186,7 +186,7 @@ class FrameToFrameOpticalFlow : public OpticalFlowBase {
       const Eigen::aligned_map<KeypointId, Eigen::AffineCompact2f>&
           transform_map_1,
       Eigen::aligned_map<KeypointId, Eigen::AffineCompact2f>& transform_map_2,
-      int64_t view_offset = 0) const {
+      Vector2 view_offset = Vector2::Zero()) const {
     size_t num_points = transform_map_1.size();
 
     std::vector<KeypointId> ids;
@@ -210,17 +210,19 @@ class FrameToFrameOpticalFlow : public OpticalFlowBase {
 
         const Eigen::AffineCompact2f& transform_1 = init_vec[r];
         Eigen::AffineCompact2f transform_2 = transform_1;
-        transform_2.translation()(0) -= view_offset;
+        transform_2.translation() -= view_offset;
 
         bool valid = transform_2.translation()(0) >= 0 &&
-                     transform_2.translation()(0) < pyr_2.lvl(0).w;
+                     transform_2.translation()(1) &&
+                     transform_2.translation()(0) < pyr_2.lvl(0).w &&
+                     transform_2.translation()(1) < pyr_2.lvl(0).h;
         if (!valid) continue;
 
         valid = trackPoint(pyr_1, pyr_2, transform_1, transform_2);
         if (!valid) continue;
 
         Eigen::AffineCompact2f transform_1_recovered = transform_2;
-        transform_1_recovered.translation()(0) += view_offset;
+        transform_1_recovered.translation() += view_offset;
 
         valid = trackPoint(pyr_2, pyr_1, transform_2, transform_1_recovered);
         if (!valid) continue;

include/basalt/optical_flow/multiscale_frame_to_frame_optical_flow.h

@@ -200,7 +200,8 @@ class MultiscaleFrameToFrameOpticalFlow : public OpticalFlowBase {
           transform_map_1,
       const std::map<KeypointId, size_t>& pyramid_levels_1,
       Eigen::aligned_map<KeypointId, Eigen::AffineCompact2f>& transform_map_2,
-      std::map<KeypointId, size_t>& pyramid_levels_2, int64_t view_offset = 0) const {
+      std::map<KeypointId, size_t>& pyramid_levels_2,
+      Vector2 view_offset = Vector2::Zero()) const {
     size_t num_points = transform_map_1.size();
 
     std::vector<KeypointId> ids;
@@ -229,17 +230,20 @@ class MultiscaleFrameToFrameOpticalFlow : public OpticalFlowBase {
 
         const Eigen::AffineCompact2f& transform_1 = init_vec[r];
         Eigen::AffineCompact2f transform_2 = transform_1;
-        transform_2.translation()(0) -= view_offset;
+        transform_2.translation() -= view_offset;
 
-        bool valid = transform_2.translation()(0) >= 0 &&
+        bool valid =
-                     transform_2.translation()(0) < pyr_2.lvl(pyramid_level[r]).w;
+            transform_2.translation()(0) >= 0 &&
+            transform_2.translation()(1) >= 0 &&
+            transform_2.translation()(0) < pyr_2.lvl(pyramid_level[r]).w &&
+            transform_2.translation()(1) < pyr_2.lvl(pyramid_level[r]).h;
         if (!valid) continue;
 
         valid = trackPoint(pyr_1, pyr_2, transform_1, pyramid_level[r],
-                                transform_2);
+                           transform_2);
         if (valid) {
           Eigen::AffineCompact2f transform_1_recovered = transform_2;
-          transform_1_recovered.translation()(0) += view_offset;
+          transform_1_recovered.translation() += view_offset;
           valid = trackPoint(pyr_2, pyr_1, transform_2, pyramid_level[r],
                              transform_1_recovered);
 

scripts/basalt_convert_kitti_calib.py

@@ -112,7 +112,7 @@ calib_template = Template('''{
         "accel_bias_std": [0.0, 0.0, 0.0],
         "gyro_bias_std": [0.0, 0.0, 0.0],
         "cam_time_offset_ns": 0,
-        "view_offset": 0
+        "view_offset": [0.0, 0.0]
     }
 }
 ''')

thirdparty/basalt-headers

@@ -1 +1 @@
-Subproject commit 1473e8a417313082372bd0356c732cfcd60e183e
+Subproject commit 1a92e765bd65037313bb482c56886842c4d3d547