From 8e4b5d8cc4ee8a00ebd24093df77f678989fc0c3 Mon Sep 17 00:00:00 2001 From: Mateo de Mayo Date: Fri, 6 May 2022 15:21:29 -0300 Subject: [PATCH] Add view_offset setting for cameras with low overlap --- data/euroc_ds_calib.json | 3 +- data/euroc_eucm_calib.json | 3 +- data/tumvi_512_ds_calib.json | 1 + data/tumvi_512_eucm_calib.json | 1 + .../frame_to_frame_optical_flow.h | 46 +++++++++++-------- scripts/basalt_convert_kitti_calib.py | 3 +- thirdparty/basalt-headers | 2 +- 7 files changed, 36 insertions(+), 23 deletions(-) diff --git a/data/euroc_ds_calib.json b/data/euroc_ds_calib.json index 25c22a0..9ed3eb1 100644 --- a/data/euroc_ds_calib.json +++ b/data/euroc_ds_calib.json @@ -224,6 +224,7 @@ }, "mocap_time_offset_ns": 0, "mocap_to_imu_offset_ns": 140763258159875, - "cam_time_offset_ns": 0 + "cam_time_offset_ns": 0, + "view_offset": 0 } } diff --git a/data/euroc_eucm_calib.json b/data/euroc_eucm_calib.json index 347d6b0..481527b 100644 --- a/data/euroc_eucm_calib.json +++ b/data/euroc_eucm_calib.json @@ -208,6 +208,7 @@ }, "mocap_time_offset_ns": 0, "mocap_to_imu_offset_ns": 140763258159875, - "cam_time_offset_ns": 0 + "cam_time_offset_ns": 0, + "view_offset": 0 } } diff --git a/data/tumvi_512_ds_calib.json b/data/tumvi_512_ds_calib.json index a9a441c..96a4b44 100644 --- a/data/tumvi_512_ds_calib.json +++ b/data/tumvi_512_ds_calib.json @@ -105,6 +105,7 @@ "mocap_time_offset_ns": 0, "mocap_to_imu_offset_ns": 0, "cam_time_offset_ns": 0, + "view_offset": 0, "vignette": [ { "value0": 0, diff --git a/data/tumvi_512_eucm_calib.json b/data/tumvi_512_eucm_calib.json index 98d2cff..f1c9c00 100644 --- a/data/tumvi_512_eucm_calib.json +++ b/data/tumvi_512_eucm_calib.json @@ -105,6 +105,7 @@ "mocap_time_offset_ns": 0, "mocap_to_imu_offset_ns": 0, "cam_time_offset_ns": 0, + "view_offset": 0, "vignette": [ { "value0": 0, 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 9ede196..342d430 100644 --- a/include/basalt/optical_flow/frame_to_frame_optical_flow.h +++ b/include/basalt/optical_flow/frame_to_frame_optical_flow.h @@ -103,6 +103,7 @@ class FrameToFrameOpticalFlow : public OpticalFlowBase { if (output_queue) output_queue->push(nullptr); break; } + input_ptr->addTime("opticalflow_received"); processFrame(input_ptr->t_ns, input_ptr); } @@ -172,18 +173,20 @@ class FrameToFrameOpticalFlow : public OpticalFlowBase { } if (output_queue && frame_counter % config.optical_flow_skip_frames == 0) { + transforms->input_images->addTime("opticalflow_produced"); output_queue->push(transforms); } frame_counter++; } - void trackPoints(const basalt::ManagedImagePyr& pyr_1, - const basalt::ManagedImagePyr& pyr_2, - const Eigen::aligned_map& - transform_map_1, - Eigen::aligned_map& - transform_map_2) const { + void trackPoints( + const basalt::ManagedImagePyr& pyr_1, + const basalt::ManagedImagePyr& pyr_2, + const Eigen::aligned_map& + transform_map_1, + Eigen::aligned_map& transform_map_2, + int64_t view_offset = 0) const { size_t num_points = transform_map_1.size(); std::vector ids; @@ -207,23 +210,27 @@ class FrameToFrameOpticalFlow : public OpticalFlowBase { const Eigen::AffineCompact2f& transform_1 = init_vec[r]; Eigen::AffineCompact2f transform_2 = transform_1; + transform_2.translation()(0) -= view_offset; - bool valid = trackPoint(pyr_1, pyr_2, transform_1, transform_2); + bool valid = transform_2.translation()(0) >= 0 && + transform_2.translation()(0) < pyr_2.lvl(0).w; + if (!valid) continue; - if (valid) { - Eigen::AffineCompact2f transform_1_recovered = transform_2; + valid = trackPoint(pyr_1, pyr_2, transform_1, transform_2); + if (!valid) continue; - valid = trackPoint(pyr_2, pyr_1, transform_2, transform_1_recovered); + Eigen::AffineCompact2f transform_1_recovered = transform_2; + transform_1_recovered.translation()(0) += view_offset; - if (valid) { - Scalar dist2 = (transform_1.translation() - - transform_1_recovered.translation()) - .squaredNorm(); + valid = trackPoint(pyr_2, pyr_1, transform_2, transform_1_recovered); + if (!valid) continue; - if (dist2 < config.optical_flow_max_recovered_dist2) { - result[id] = transform_2; - } - } + Scalar dist2 = + (transform_1.translation() - transform_1_recovered.translation()) + .squaredNorm(); + + if (dist2 < config.optical_flow_max_recovered_dist2) { + result[id] = transform_2; } } }; @@ -332,7 +339,8 @@ class FrameToFrameOpticalFlow : public OpticalFlowBase { } if (calib.intrinsics.size() > 1) { - trackPoints(pyramid->at(0), pyramid->at(1), new_poses0, new_poses1); + trackPoints(pyramid->at(0), pyramid->at(1), new_poses0, new_poses1, + calib.view_offset); for (const auto& kv : new_poses1) { transforms->observations.at(1).emplace(kv); diff --git a/scripts/basalt_convert_kitti_calib.py b/scripts/basalt_convert_kitti_calib.py index ccc8ce0..80ecc95 100755 --- a/scripts/basalt_convert_kitti_calib.py +++ b/scripts/basalt_convert_kitti_calib.py @@ -111,7 +111,8 @@ calib_template = Template('''{ "gyro_noise_std": [0.0, 0.0, 0.0], "accel_bias_std": [0.0, 0.0, 0.0], "gyro_bias_std": [0.0, 0.0, 0.0], - "cam_time_offset_ns": 0 + "cam_time_offset_ns": 0, + "view_offset": 0 } } ''') diff --git a/thirdparty/basalt-headers b/thirdparty/basalt-headers index 42b623e..8c39d10 160000 --- a/thirdparty/basalt-headers +++ b/thirdparty/basalt-headers @@ -1 +1 @@ -Subproject commit 42b623e812c12422e7522a652bc0a2e3ee6924d5 +Subproject commit 8c39d10b3d5463bf257701a32293caee088b8836