From 594603861b5f2ce2da9dafe0aed8b4b8cb18e7e3 Mon Sep 17 00:00:00 2001 From: Mateo de Mayo Date: Wed, 11 May 2022 19:56:13 +0000 Subject: [PATCH] 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 | 2 +- data/d455_848x480_calib.json | 2 +- data/d455_calib.json | 2 +- data/euroc_ds_calib.json | 2 +- data/euroc_eucm_calib.json | 2 +- data/euroc_rt8_calib.json | 2 +- data/monado/wmr-tools/wmr2bslt_calib.py | 68 ++++++++++++++--- data/odysseyplus_kb4_calib.json | 5 +- data/odysseyplus_rt8_calib.json | 76 +++++++++---------- data/reverbg1_calib.json | 2 +- data/tumvi_512_ds_calib.json | 2 +- data/tumvi_512_eucm_calib.json | 2 +- data/x3pro_calib.json | 2 +- .../frame_to_frame_optical_flow.h | 10 ++- .../multiscale_frame_to_frame_optical_flow.h | 16 ++-- scripts/basalt_convert_kitti_calib.py | 6 +- thirdparty/basalt-headers | 2 +- 17 files changed, 131 insertions(+), 72 deletions(-) diff --git a/data/d455_640x480_calib.json b/data/d455_640x480_calib.json index 41d0058..65972fc 100644 --- a/data/d455_640x480_calib.json +++ b/data/d455_640x480_calib.json @@ -97,7 +97,7 @@ 0.0007071067802939111 ], "cam_time_offset_ns": 0, - "view_offset": 0, + "view_offset": [0, 0], "vignette": [] } } diff --git a/data/d455_848x480_calib.json b/data/d455_848x480_calib.json index ea75e6e..f011b89 100644 --- a/data/d455_848x480_calib.json +++ b/data/d455_848x480_calib.json @@ -97,7 +97,7 @@ 0.0007071067802939111 ], "cam_time_offset_ns": 0, - "view_offset": 0, + "view_offset": [0, 0], "vignette": [] } } diff --git a/data/d455_calib.json b/data/d455_calib.json index 928df1d..a53ba26 100644 --- a/data/d455_calib.json +++ b/data/d455_calib.json @@ -99,7 +99,7 @@ 0.0007071067802939111 ], "cam_time_offset_ns": 0, - "view_offset": 0, + "view_offset": [0, 0], "vignette": [] } } diff --git a/data/euroc_ds_calib.json b/data/euroc_ds_calib.json index 9ed3eb1..41010d7 100644 --- a/data/euroc_ds_calib.json +++ b/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] } } diff --git a/data/euroc_eucm_calib.json b/data/euroc_eucm_calib.json index 481527b..3df2282 100644 --- a/data/euroc_eucm_calib.json +++ b/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] } } diff --git a/data/euroc_rt8_calib.json b/data/euroc_rt8_calib.json index 0c6ad31..6a8a7f7 100644 --- a/data/euroc_rt8_calib.json +++ b/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] } } diff --git a/data/monado/wmr-tools/wmr2bslt_calib.py b/data/monado/wmr-tools/wmr2bslt_calib.py index fe661f0..6235baf 100755 --- a/data/monado/wmr-tools/wmr2bslt_calib.py +++ b/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": [], } } diff --git a/data/odysseyplus_kb4_calib.json b/data/odysseyplus_kb4_calib.json index ca9c453..da128ae 100644 --- a/data/odysseyplus_kb4_calib.json +++ b/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": [] } } diff --git a/data/odysseyplus_rt8_calib.json b/data/odysseyplus_rt8_calib.json index ec0cb10..27fa279 100644 --- a/data/odysseyplus_rt8_calib.json +++ b/data/odysseyplus_rt8_calib.json @@ -5,19 +5,19 @@ "px": -0.08394275605678558, "py": -0.0025952591095119715, "pz": 0.0026445253752171993, - "qx": -0.17704728245735168, - "qy": -0.19861078262329102, - "qz": 0.035641565918922424, - "qw": 0.9632952809333801 + "qx": -0.17704726835670878, + "qy": -0.19861076922141732, + "qz": 0.03564156642768224, + "qw": 0.9632952306248018 }, { - "px": 0.021131351590156555, - "py": -0.002306802896782756, - "pz": 0.0023415968753397465, - "qx": -0.18363775312900543, - "qy": 0.19827023148536682, - "qz": -0.03922446444630623, - "qw": 0.961991548538208 + "px": 0.02113135496007891, + "py": -0.0023068031820524327, + "pz": 0.002341595642298417, + "qx": -0.1836377420226455, + "qy": 0.1982701960061761, + "qz": -0.03922445856481098, + "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, - "k3": 0.0041795829311013222, - "k4": 0.89431935548782349, - "k5": 0.54253977537155151, + "p2": -4.288297350285575e-05, + "k3": 0.004179582931101322, + "k4": 0.8943193554878235, + "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, - "k4": 0.83317267894744873, - "k5": 0.26174271106719971, - "k6": 0.043505862355232239, - "rpmax": 2.7899987697601318 + "k3": 0.006815604865550995, + "k4": 0.8331726789474487, + "k5": 0.2617427110671997, + "k6": 0.04350586235523224, + "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.0000957250595093, - -0.00082384591223672032, + 0.004562512971460819, + 9.572505950927734e-05, + -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.00095000001601874828, - 0.00095000001601874828 + 0.0009500000160187483, + 0.0009500000160187483, + 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": [] } } diff --git a/data/reverbg1_calib.json b/data/reverbg1_calib.json index b043ff6..70b485a 100644 --- a/data/reverbg1_calib.json +++ b/data/reverbg1_calib.json @@ -115,7 +115,7 @@ 0.009999999873689375 ], "cam_time_offset_ns": 0, - "view_offset": 247, + "view_offset": [247, 0], "vignette": [] } } diff --git a/data/tumvi_512_ds_calib.json b/data/tumvi_512_ds_calib.json index 96a4b44..058c1d8 100644 --- a/data/tumvi_512_ds_calib.json +++ b/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, diff --git a/data/tumvi_512_eucm_calib.json b/data/tumvi_512_eucm_calib.json index f1c9c00..8deb1df 100644 --- a/data/tumvi_512_eucm_calib.json +++ b/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, diff --git a/data/x3pro_calib.json b/data/x3pro_calib.json index b1b0e64..76dade8 100644 --- a/data/x3pro_calib.json +++ b/data/x3pro_calib.json @@ -97,7 +97,7 @@ 0.0001 ], "cam_time_offset_ns": 0, - "view_offset": 300, + "view_offset": [0, 0], "vignette": [] } } diff --git a/include/basalt/optical_flow/frame_to_frame_optical_flow.h b/include/basalt/optical_flow/frame_to_frame_optical_flow.h index d811a5f..63c66d2 100644 --- a/include/basalt/optical_flow/frame_to_frame_optical_flow.h +++ b/include/basalt/optical_flow/frame_to_frame_optical_flow.h @@ -186,7 +186,7 @@ class FrameToFrameOpticalFlow : public OpticalFlowBase { const Eigen::aligned_map& transform_map_1, Eigen::aligned_map& 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 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; diff --git a/include/basalt/optical_flow/multiscale_frame_to_frame_optical_flow.h b/include/basalt/optical_flow/multiscale_frame_to_frame_optical_flow.h index 18aee95..dfae746 100644 --- a/include/basalt/optical_flow/multiscale_frame_to_frame_optical_flow.h +++ b/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& pyramid_levels_1, Eigen::aligned_map& transform_map_2, - std::map& pyramid_levels_2, int64_t view_offset = 0) const { + std::map& pyramid_levels_2, + Vector2 view_offset = Vector2::Zero()) const { size_t num_points = transform_map_1.size(); std::vector 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 && - transform_2.translation()(0) < pyr_2.lvl(pyramid_level[r]).w; + bool valid = + 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); diff --git a/scripts/basalt_convert_kitti_calib.py b/scripts/basalt_convert_kitti_calib.py index 80ecc95..d2b050a 100755 --- a/scripts/basalt_convert_kitti_calib.py +++ b/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] } } ''') @@ -124,7 +124,7 @@ with open(kitti_calib_file, 'r') as stream: if len(lines) != 52: print('Issues loading calibration') print(lines) - + P0 = np.array([float(x) for x in lines[1:13]]).reshape(3,4) P1 = np.array([float(x) for x in lines[14:26]]).reshape(3,4) print('P0\n', P0) @@ -142,4 +142,4 @@ with open(kitti_calib_file, 'r') as stream: print(calib) with open(dataset_path + '/basalt_calib.json', 'w') as stream2: - stream2.write(calib) \ No newline at end of file + stream2.write(calib) diff --git a/thirdparty/basalt-headers b/thirdparty/basalt-headers index 1473e8a..1a92e76 160000 --- a/thirdparty/basalt-headers +++ b/thirdparty/basalt-headers @@ -1 +1 @@ -Subproject commit 1473e8a417313082372bd0356c732cfcd60e183e +Subproject commit 1a92e765bd65037313bb482c56886842c4d3d547