From 6f3f61c91ff28a5206a17cfd49ce4568aef3dd3b Mon Sep 17 00:00:00 2001 From: Mateo de Mayo Date: Wed, 20 Jul 2022 18:53:55 +0000 Subject: [PATCH] Implement REPROJ_AVG_DEPTH --- data/d455_640x480_calib.json | 1 - data/d455_848x480_calib.json | 1 - data/d455_calib.json | 1 - data/euroc_config.json | 2 + data/euroc_config_no_factors.json | 2 + data/euroc_config_no_weights.json | 2 + data/euroc_config_vo.json | 2 + data/euroc_ds_calib.json | 3 +- data/euroc_eucm_calib.json | 3 +- data/euroc_rt8_calib.json | 3 +- data/iccv21/basalt_batch_config.toml | 2 + data/kitti_config.json | 2 + data/monado/wmr-tools/wmr2bslt_calib.py | 2 +- data/odysseyplus_kb4_calib.json | 4 - data/odysseyplus_rt8_calib.json | 4 - data/reverbg1_calib.json | 1 - data/tumvi_512_config.json | 2 + data/tumvi_512_ds_calib.json | 1 - data/tumvi_512_eucm_calib.json | 1 - data/x3pro_calib.json | 1 - .../frame_to_frame_optical_flow.h | 35 ++++-- .../multiscale_frame_to_frame_optical_flow.h | 59 +++++---- include/basalt/optical_flow/optical_flow.h | 4 + .../basalt/optical_flow/patch_optical_flow.h | 83 ++++++++----- include/basalt/utils/vio_config.h | 3 + .../basalt/vi_estimator/sqrt_keypoint_vo.h | 3 +- include/basalt/vi_estimator/vio_estimator.h | 5 +- scripts/basalt_convert_kitti_calib.py | 3 +- src/mapper_sim_naive.cpp | 4 +- src/monado/slam_tracker.cpp | 27 +---- src/monado/slam_tracker_ui.hpp | 114 ++++++++++++++++-- src/rs_t265_vio.cpp | 1 + src/utils/vio_config.cpp | 28 +++++ src/vi_estimator/sqrt_keypoint_vio.cpp | 35 ++++-- src/vi_estimator/sqrt_keypoint_vo.cpp | 58 ++++++++- src/vio.cpp | 66 +++++++++- src/vio_sim.cpp | 4 +- thirdparty/basalt-headers | 2 +- 38 files changed, 429 insertions(+), 145 deletions(-) diff --git a/data/d455_640x480_calib.json b/data/d455_640x480_calib.json index 65972fc..7335d64 100644 --- a/data/d455_640x480_calib.json +++ b/data/d455_640x480_calib.json @@ -97,7 +97,6 @@ 0.0007071067802939111 ], "cam_time_offset_ns": 0, - "view_offset": [0, 0], "vignette": [] } } diff --git a/data/d455_848x480_calib.json b/data/d455_848x480_calib.json index f011b89..37016bd 100644 --- a/data/d455_848x480_calib.json +++ b/data/d455_848x480_calib.json @@ -97,7 +97,6 @@ 0.0007071067802939111 ], "cam_time_offset_ns": 0, - "view_offset": [0, 0], "vignette": [] } } diff --git a/data/d455_calib.json b/data/d455_calib.json index a53ba26..d42780c 100644 --- a/data/d455_calib.json +++ b/data/d455_calib.json @@ -99,7 +99,6 @@ 0.0007071067802939111 ], "cam_time_offset_ns": 0, - "view_offset": [0, 0], "vignette": [] } } diff --git a/data/euroc_config.json b/data/euroc_config.json index 0cf10f2..a11cc23 100644 --- a/data/euroc_config.json +++ b/data/euroc_config.json @@ -8,6 +8,8 @@ "config.optical_flow_epipolar_error": 0.005, "config.optical_flow_levels": 3, "config.optical_flow_skip_frames": 1, + "config.optical_flow_matching_guess_type": "REPROJ_AVG_DEPTH", + "config.optical_flow_matching_default_depth": 2.0, "config.vio_linearization_type": "ABS_QR", "config.vio_sqrt_marg": true, "config.vio_max_states": 3, diff --git a/data/euroc_config_no_factors.json b/data/euroc_config_no_factors.json index 9ed26d2..e599b92 100644 --- a/data/euroc_config_no_factors.json +++ b/data/euroc_config_no_factors.json @@ -8,6 +8,8 @@ "config.optical_flow_epipolar_error": 0.005, "config.optical_flow_levels": 3, "config.optical_flow_skip_frames": 1, + "config.optical_flow_matching_guess_type": "SAME_PIXEL", + "config.optical_flow_matching_default_depth": 2.0, "config.vio_linearization_type": "ABS_QR", "config.vio_sqrt_marg": true, "config.vio_max_states": 3, diff --git a/data/euroc_config_no_weights.json b/data/euroc_config_no_weights.json index c4962b5..76a0c2f 100644 --- a/data/euroc_config_no_weights.json +++ b/data/euroc_config_no_weights.json @@ -8,6 +8,8 @@ "config.optical_flow_epipolar_error": 0.005, "config.optical_flow_levels": 3, "config.optical_flow_skip_frames": 1, + "config.optical_flow_matching_guess_type": "SAME_PIXEL", + "config.optical_flow_matching_default_depth": 2.0, "config.vio_linearization_type": "ABS_QR", "config.vio_sqrt_marg": true, "config.vio_max_states": 3, diff --git a/data/euroc_config_vo.json b/data/euroc_config_vo.json index c11c0b4..00fbce8 100644 --- a/data/euroc_config_vo.json +++ b/data/euroc_config_vo.json @@ -8,6 +8,8 @@ "config.optical_flow_epipolar_error": 0.005, "config.optical_flow_levels": 3, "config.optical_flow_skip_frames": 1, + "config.optical_flow_matching_guess_type": "SAME_PIXEL", + "config.optical_flow_matching_default_depth": 2.0, "config.vio_linearization_type": "ABS_QR", "config.vio_sqrt_marg": true, "config.vio_max_states": 3, diff --git a/data/euroc_ds_calib.json b/data/euroc_ds_calib.json index 41010d7..25c22a0 100644 --- a/data/euroc_ds_calib.json +++ b/data/euroc_ds_calib.json @@ -224,7 +224,6 @@ }, "mocap_time_offset_ns": 0, "mocap_to_imu_offset_ns": 140763258159875, - "cam_time_offset_ns": 0, - "view_offset": [0, 0] + "cam_time_offset_ns": 0 } } diff --git a/data/euroc_eucm_calib.json b/data/euroc_eucm_calib.json index 3df2282..347d6b0 100644 --- a/data/euroc_eucm_calib.json +++ b/data/euroc_eucm_calib.json @@ -208,7 +208,6 @@ }, "mocap_time_offset_ns": 0, "mocap_to_imu_offset_ns": 140763258159875, - "cam_time_offset_ns": 0, - "view_offset": [0, 0] + "cam_time_offset_ns": 0 } } diff --git a/data/euroc_rt8_calib.json b/data/euroc_rt8_calib.json index 6a8a7f7..4e00671 100644 --- a/data/euroc_rt8_calib.json +++ b/data/euroc_rt8_calib.json @@ -238,7 +238,6 @@ }, "mocap_time_offset_ns": 0, "mocap_to_imu_offset_ns": 140763258159875, - "cam_time_offset_ns": 0, - "view_offset": [0, 0] + "cam_time_offset_ns": 0 } } diff --git a/data/iccv21/basalt_batch_config.toml b/data/iccv21/basalt_batch_config.toml index a5c51f2..638969a 100644 --- a/data/iccv21/basalt_batch_config.toml +++ b/data/iccv21/basalt_batch_config.toml @@ -15,6 +15,8 @@ "config.optical_flow_epipolar_error" = 0.005 "config.optical_flow_levels" = 3 "config.optical_flow_skip_frames" = 1 +"config.optical_flow_matching_guess_type" = "SAME_PIXEL" +"config.optical_flow_matching_default_depth" = 2.0 "config.vio_linearization_type" = "ABS_QR" "config.vio_sqrt_marg" = true diff --git a/data/kitti_config.json b/data/kitti_config.json index 6200044..813d76f 100644 --- a/data/kitti_config.json +++ b/data/kitti_config.json @@ -8,6 +8,8 @@ "config.optical_flow_epipolar_error": 0.001, "config.optical_flow_levels": 4, "config.optical_flow_skip_frames": 1, + "config.optical_flow_matching_guess_type": "SAME_PIXEL", + "config.optical_flow_matching_default_depth": 2.0, "config.vio_linearization_type": "ABS_QR", "config.vio_sqrt_marg": true, "config.vio_max_states": 3, diff --git a/data/monado/wmr-tools/wmr2bslt_calib.py b/data/monado/wmr-tools/wmr2bslt_calib.py index 6235baf..9abe3b1 100755 --- a/data/monado/wmr-tools/wmr2bslt_calib.py +++ b/data/monado/wmr-tools/wmr2bslt_calib.py @@ -246,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(j), + # "view_offset": view_offset(j), "vignette": [], } } diff --git a/data/odysseyplus_kb4_calib.json b/data/odysseyplus_kb4_calib.json index da128ae..bc0d7fb 100644 --- a/data/odysseyplus_kb4_calib.json +++ b/data/odysseyplus_kb4_calib.json @@ -108,10 +108,6 @@ 0.009999999873689375 ], "cam_time_offset_ns": 0, - "view_offset": [ - 248.7971689190779, - -5.785840906463989 - ], "vignette": [] } } diff --git a/data/odysseyplus_rt8_calib.json b/data/odysseyplus_rt8_calib.json index 27fa279..48de92b 100644 --- a/data/odysseyplus_rt8_calib.json +++ b/data/odysseyplus_rt8_calib.json @@ -115,10 +115,6 @@ 0.009999999873689375 ], "cam_time_offset_ns": 0, - "view_offset": [ - 248.7971689190779, - -5.785840906463989 - ], "vignette": [] } } diff --git a/data/reverbg1_calib.json b/data/reverbg1_calib.json index 94bcd6c..b406958 100644 --- a/data/reverbg1_calib.json +++ b/data/reverbg1_calib.json @@ -115,7 +115,6 @@ 0.009999999873689375 ], "cam_time_offset_ns": 0, - "view_offset": [0, 0], "vignette": [] } } diff --git a/data/tumvi_512_config.json b/data/tumvi_512_config.json index 08a4be7..12d590c 100644 --- a/data/tumvi_512_config.json +++ b/data/tumvi_512_config.json @@ -8,6 +8,8 @@ "config.optical_flow_epipolar_error": 0.005, "config.optical_flow_levels": 3, "config.optical_flow_skip_frames": 1, + "config.optical_flow_matching_guess_type": "REPROJ_AVG_DEPTH", + "config.optical_flow_matching_default_depth": 2.0, "config.vio_linearization_type": "ABS_QR", "config.vio_sqrt_marg": true, "config.vio_max_states": 3, diff --git a/data/tumvi_512_ds_calib.json b/data/tumvi_512_ds_calib.json index 058c1d8..a9a441c 100644 --- a/data/tumvi_512_ds_calib.json +++ b/data/tumvi_512_ds_calib.json @@ -105,7 +105,6 @@ "mocap_time_offset_ns": 0, "mocap_to_imu_offset_ns": 0, "cam_time_offset_ns": 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 8deb1df..98d2cff 100644 --- a/data/tumvi_512_eucm_calib.json +++ b/data/tumvi_512_eucm_calib.json @@ -105,7 +105,6 @@ "mocap_time_offset_ns": 0, "mocap_to_imu_offset_ns": 0, "cam_time_offset_ns": 0, - "view_offset": [0, 0], "vignette": [ { "value0": 0, diff --git a/data/x3pro_calib.json b/data/x3pro_calib.json index 76dade8..51abf2c 100644 --- a/data/x3pro_calib.json +++ b/data/x3pro_calib.json @@ -97,7 +97,6 @@ 0.0001 ], "cam_time_offset_ns": 0, - "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 63c66d2..a4dc185 100644 --- a/include/basalt/optical_flow/frame_to_frame_optical_flow.h +++ b/include/basalt/optical_flow/frame_to_frame_optical_flow.h @@ -79,6 +79,7 @@ class FrameToFrameOpticalFlow : public OpticalFlowBase { this->calib = calib.cast(); patch_coord = PatchT::pattern2.template cast(); + depth_guess = config.optical_flow_matching_default_depth; if (calib.intrinsics.size() > 1) { Eigen::Matrix4d Ed; @@ -97,6 +98,8 @@ class FrameToFrameOpticalFlow : public OpticalFlowBase { OpticalFlowInput::Ptr input_ptr; while (true) { + while (input_depth_queue.try_pop(depth_guess)) continue; + input_queue.pop(input_ptr); if (!input_ptr.get()) { @@ -186,7 +189,7 @@ class FrameToFrameOpticalFlow : public OpticalFlowBase { const Eigen::aligned_map& transform_map_1, Eigen::aligned_map& transform_map_2, - Vector2 view_offset = Vector2::Zero()) const { + bool matching = false) const { size_t num_points = transform_map_1.size(); std::vector ids; @@ -210,26 +213,35 @@ class FrameToFrameOpticalFlow : public OpticalFlowBase { const Eigen::AffineCompact2f& transform_1 = init_vec[r]; Eigen::AffineCompact2f transform_2 = transform_1; - transform_2.translation() -= view_offset; - bool valid = transform_2.translation()(0) >= 0 && - transform_2.translation()(1) && - transform_2.translation()(0) < pyr_2.lvl(0).w && - transform_2.translation()(1) < pyr_2.lvl(0).h; + auto t1 = transform_1.translation(); + auto t2 = transform_2.translation(); + + Eigen::Vector2f off{0, 0}; + MatchingGuessType guess_type = config.optical_flow_matching_guess_type; + if (matching && guess_type != MatchingGuessType::SAME_PIXEL) { + off = calib.viewOffset(t1, depth_guess).template cast(); + transforms->input_images->depth_guess = depth_guess; // For UI + } + + t2 -= off; // This modifies transform_2 + + bool valid = t2(0) >= 0 && t2(1) >= 0 && t2(0) < pyr_2.lvl(0).w && + t2(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() += view_offset; + auto t1_recovered = transform_1_recovered.translation(); + + t1_recovered += off; valid = trackPoint(pyr_2, pyr_1, transform_2, transform_1_recovered); if (!valid) continue; - Scalar dist2 = - (transform_1.translation() - transform_1_recovered.translation()) - .squaredNorm(); + Scalar dist2 = (t1 - t1_recovered).squaredNorm(); if (dist2 < config.optical_flow_max_recovered_dist2) { result[id] = transform_2; @@ -341,8 +353,7 @@ class FrameToFrameOpticalFlow : public OpticalFlowBase { } if (calib.intrinsics.size() > 1) { - trackPoints(pyramid->at(0), pyramid->at(1), new_poses0, new_poses1, - calib.view_offset); + trackPoints(pyramid->at(0), pyramid->at(1), new_poses0, new_poses1, true); for (const auto& kv : new_poses1) { transforms->observations.at(1).emplace(kv); 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 dfae746..3fc341d 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 @@ -81,6 +81,7 @@ class MultiscaleFrameToFrameOpticalFlow : public OpticalFlowBase { this->calib = calib.cast(); patch_coord = PatchT::pattern2.template cast(); + depth_guess = config.optical_flow_matching_default_depth; if (calib.intrinsics.size() > 1) { Eigen::Matrix4d Ed; @@ -99,12 +100,15 @@ class MultiscaleFrameToFrameOpticalFlow : public OpticalFlowBase { OpticalFlowInput::Ptr input_ptr; while (true) { + while (input_depth_queue.try_pop(depth_guess)) continue; + input_queue.pop(input_ptr); if (!input_ptr.get()) { if (output_queue) output_queue->push(nullptr); break; } + input_ptr->addTime("frames_received"); processFrame(input_ptr->t_ns, input_ptr); } @@ -182,6 +186,7 @@ class MultiscaleFrameToFrameOpticalFlow : public OpticalFlowBase { } if (frame_counter % config.optical_flow_skip_frames == 0) { + transforms->input_images->addTime("opticalflow_produced"); try { output_queue->push(transforms); } catch (const tbb::user_abort&) { @@ -201,7 +206,7 @@ class MultiscaleFrameToFrameOpticalFlow : public OpticalFlowBase { const std::map& pyramid_levels_1, Eigen::aligned_map& transform_map_2, std::map& pyramid_levels_2, - Vector2 view_offset = Vector2::Zero()) const { + bool matching = false) const { size_t num_points = transform_map_1.size(); std::vector ids; @@ -230,34 +235,42 @@ class MultiscaleFrameToFrameOpticalFlow : public OpticalFlowBase { const Eigen::AffineCompact2f& transform_1 = init_vec[r]; Eigen::AffineCompact2f transform_2 = transform_1; - transform_2.translation() -= view_offset; - 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; + auto t1 = transform_1.translation(); + auto t2 = transform_2.translation(); + + Eigen::Vector2f off{0, 0}; + MatchingGuessType guess_type = config.optical_flow_matching_guess_type; + if (matching && guess_type != MatchingGuessType::SAME_PIXEL) { + off = calib.viewOffset(t1, depth_guess).template cast(); + transforms->input_images->depth_guess = depth_guess; // For UI + } + + t2 -= off; // This modifies transform_2 + + bool valid = t2(0) >= 0 && t2(1) >= 0 && t2(0) < pyr_2.lvl(0).w && + t2(1) < pyr_2.lvl(0).h; if (!valid) continue; valid = trackPoint(pyr_1, pyr_2, transform_1, pyramid_level[r], transform_2); - if (valid) { - Eigen::AffineCompact2f transform_1_recovered = transform_2; - transform_1_recovered.translation() += view_offset; - valid = trackPoint(pyr_2, pyr_1, transform_2, pyramid_level[r], - transform_1_recovered); + if (!valid) continue; - if (valid) { - const Scalar scale = 1 << pyramid_level[r]; - Scalar dist2 = (transform_1.translation() / scale - - transform_1_recovered.translation() / scale) - .squaredNorm(); + Eigen::AffineCompact2f transform_1_recovered = transform_2; + auto t1_recovered = transform_1_recovered.translation(); - if (dist2 < config.optical_flow_max_recovered_dist2) { - result_transforms[id] = transform_2; - result_pyramid_level[id] = pyramid_level[r]; - } - } + t1_recovered += off; + + valid = trackPoint(pyr_2, pyr_1, transform_2, pyramid_level[r], + transform_1_recovered); + if (!valid) continue; + + const Scalar scale = 1 << pyramid_level[r]; + Scalar dist2 = ((t1 - t1_recovered) / scale).squaredNorm(); + + if (dist2 < config.optical_flow_max_recovered_dist2) { + result_transforms[id] = transform_2; + result_pyramid_level[id] = pyramid_level[r]; } } }; @@ -400,7 +413,7 @@ class MultiscaleFrameToFrameOpticalFlow : public OpticalFlowBase { trackPoints(pyramid->at(0), pyramid->at(1), new_poses_main, new_pyramid_levels_main, new_poses_stereo, - new_pyramid_levels_stereo, calib.view_offset); + new_pyramid_levels_stereo, true); for (const auto& kv : new_poses_stereo) { transforms->observations.at(1).emplace(kv); diff --git a/include/basalt/optical_flow/optical_flow.h b/include/basalt/optical_flow/optical_flow.h index bb47c7d..445b760 100644 --- a/include/basalt/optical_flow/optical_flow.h +++ b/include/basalt/optical_flow/optical_flow.h @@ -60,6 +60,8 @@ struct OpticalFlowInput { int64_t t_ns; std::vector img_data; + double depth_guess = -1; + timestats stats; //!< Keeps track of internal metrics for this t_ns void addTime(const char* name, int64_t custom_ts = INT64_MIN) { stats.addTime(name, custom_ts); @@ -83,9 +85,11 @@ class OpticalFlowBase { using Ptr = std::shared_ptr; tbb::concurrent_bounded_queue input_queue; + tbb::concurrent_queue input_depth_queue; tbb::concurrent_bounded_queue* output_queue = nullptr; Eigen::MatrixXf patch_coord; + double depth_guess = -1; }; class OpticalFlowFactory { diff --git a/include/basalt/optical_flow/patch_optical_flow.h b/include/basalt/optical_flow/patch_optical_flow.h index 72a2466..9d883c2 100644 --- a/include/basalt/optical_flow/patch_optical_flow.h +++ b/include/basalt/optical_flow/patch_optical_flow.h @@ -77,19 +77,20 @@ class PatchOpticalFlow : public OpticalFlowBase { PatchOpticalFlow(const VioConfig& config, const basalt::Calibration& calib) - : t_ns(-1), - frame_counter(0), - last_keypoint_id(0), - config(config), - calib(calib) { + : t_ns(-1), frame_counter(0), last_keypoint_id(0), config(config) { patches.reserve(3000); input_queue.set_capacity(10); + this->calib = calib.cast(); + patch_coord = PatchT::pattern2.template cast(); + depth_guess = config.optical_flow_matching_default_depth; if (calib.intrinsics.size() > 1) { + Eigen::Matrix4d Ed; Sophus::SE3d T_i_j = calib.T_i_c[0].inverse() * calib.T_i_c[1]; - computeEssential(T_i_j, E); + computeEssential(T_i_j, Ed); + E = Ed.cast(); } processing_thread.reset( @@ -102,12 +103,15 @@ class PatchOpticalFlow : public OpticalFlowBase { OpticalFlowInput::Ptr input_ptr; while (true) { + while (input_depth_queue.try_pop(depth_guess)) continue; + input_queue.pop(input_ptr); if (!input_ptr.get()) { output_queue->push(nullptr); break; } + input_ptr->addTime("frames_received"); processFrame(input_ptr->t_ns, input_ptr); } @@ -168,17 +172,19 @@ class PatchOpticalFlow : 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, + bool matching = false) const { size_t num_points = transform_map_1.size(); std::vector ids; @@ -203,24 +209,39 @@ class PatchOpticalFlow : public OpticalFlowBase { const Eigen::AffineCompact2f& transform_1 = init_vec[r]; Eigen::AffineCompact2f transform_2 = transform_1; + auto t1 = transform_1.translation(); + auto t2 = transform_2.translation(); + + Eigen::Vector2f off{0, 0}; + MatchingGuessType guess_type = config.optical_flow_matching_guess_type; + if (matching && guess_type != MatchingGuessType::SAME_PIXEL) { + off = calib.viewOffset(t1, depth_guess).template cast(); + transforms->input_images->depth_guess = depth_guess; // For UI + } + + t2 -= off; // This modifies transform_2 + + bool valid = t2(0) >= 0 && t2(1) >= 0 && t2(0) < pyr_2.lvl(0).w && + t2(1) < pyr_2.lvl(0).h; + if (!valid) continue; + const Eigen::aligned_vector& patch_vec = patches.at(id); - bool valid = trackPoint(pyr_2, patch_vec, transform_2); + valid = trackPoint(pyr_2, patch_vec, transform_2); + if (!valid) continue; - if (valid) { - Eigen::AffineCompact2f transform_1_recovered = transform_2; + Eigen::AffineCompact2f transform_1_recovered = transform_2; + auto t1_recovered = transform_1_recovered.translation(); - valid = trackPoint(pyr_1, patch_vec, transform_1_recovered); + t1_recovered += off; - if (valid) { - Scalar dist2 = (transform_1.translation() - - transform_1_recovered.translation()) - .squaredNorm(); + valid = trackPoint(pyr_1, patch_vec, transform_1_recovered); + if (!valid) continue; - if (dist2 < config.optical_flow_max_recovered_dist2) { - result[id] = transform_2; - } - } + Scalar dist2 = (t1 - t1_recovered).squaredNorm(); + + if (dist2 < config.optical_flow_max_recovered_dist2) { + result[id] = transform_2; } } }; @@ -334,7 +355,7 @@ class PatchOpticalFlow : 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, true); for (const auto& kv : new_poses1) { transforms->observations.at(1).emplace(kv); @@ -348,19 +369,19 @@ class PatchOpticalFlow : public OpticalFlowBase { std::set lm_to_remove; std::vector kpid; - Eigen::aligned_vector proj0, proj1; + Eigen::aligned_vector proj0, proj1; for (const auto& kv : transforms->observations.at(1)) { auto it = transforms->observations.at(0).find(kv.first); if (it != transforms->observations.at(0).end()) { - proj0.emplace_back(it->second.translation().cast()); - proj1.emplace_back(kv.second.translation().cast()); + proj0.emplace_back(it->second.translation()); + proj1.emplace_back(kv.second.translation()); kpid.emplace_back(kv.first); } } - Eigen::aligned_vector p3d0, p3d1; + Eigen::aligned_vector p3d0, p3d1; std::vector p3d0_success, p3d1_success; calib.intrinsics[0].unproject(proj0, p3d0, p3d0_success); @@ -393,7 +414,7 @@ class PatchOpticalFlow : public OpticalFlowBase { KeypointId last_keypoint_id; VioConfig config; - basalt::Calibration calib; + basalt::Calibration calib; Eigen::aligned_unordered_map> patches; @@ -402,7 +423,7 @@ class PatchOpticalFlow : public OpticalFlowBase { std::shared_ptr>> old_pyramid, pyramid; - Eigen::Matrix4d E; + Matrix4 E; std::shared_ptr processing_thread; }; diff --git a/include/basalt/utils/vio_config.h b/include/basalt/utils/vio_config.h index cebf67f..610e99f 100644 --- a/include/basalt/utils/vio_config.h +++ b/include/basalt/utils/vio_config.h @@ -39,6 +39,7 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. namespace basalt { enum class LinearizationType { ABS_QR, ABS_SC, REL_SC }; +enum class MatchingGuessType { SAME_PIXEL, REPROJ_FIX_DEPTH, REPROJ_AVG_DEPTH }; struct VioConfig { VioConfig(); @@ -53,6 +54,8 @@ struct VioConfig { int optical_flow_levels; float optical_flow_epipolar_error; int optical_flow_skip_frames; + MatchingGuessType optical_flow_matching_guess_type; + float optical_flow_matching_default_depth; LinearizationType vio_linearization_type; bool vio_sqrt_marg; diff --git a/include/basalt/vi_estimator/sqrt_keypoint_vo.h b/include/basalt/vi_estimator/sqrt_keypoint_vo.h index b20124b..7279bee 100644 --- a/include/basalt/vi_estimator/sqrt_keypoint_vo.h +++ b/include/basalt/vi_estimator/sqrt_keypoint_vo.h @@ -113,7 +113,8 @@ class SqrtKeypointVoEstimator : public VioEstimatorBase, // int64_t propagate(); // void addNewState(int64_t data_t_ns); - void optimize_and_marg(const std::map& num_points_connected, + void optimize_and_marg(const OpticalFlowInput::Ptr& input_images, + const std::map& num_points_connected, const std::unordered_set& lost_landmaks); void marginalize(const std::map& num_points_connected, diff --git a/include/basalt/vi_estimator/vio_estimator.h b/include/basalt/vi_estimator/vio_estimator.h index 9b755a0..4561d6f 100644 --- a/include/basalt/vi_estimator/vio_estimator.h +++ b/include/basalt/vi_estimator/vio_estimator.h @@ -54,7 +54,8 @@ struct VioVisualizationData { OpticalFlowResult::Ptr opt_flow_res; - std::vector> projections; + std::shared_ptr>> + projections; EIGEN_MAKE_ALIGNED_OPERATOR_NEW }; @@ -85,6 +86,8 @@ class VioEstimatorBase { tbb::concurrent_bounded_queue* out_vis_queue = nullptr; + tbb::concurrent_queue* opt_flow_depth_guess_queue = nullptr; + virtual void initialize(int64_t t_ns, const Sophus::SE3d& T_w_i, const Eigen::Vector3d& vel_w_i, const Eigen::Vector3d& bg, diff --git a/scripts/basalt_convert_kitti_calib.py b/scripts/basalt_convert_kitti_calib.py index d2b050a..a2f8964 100755 --- a/scripts/basalt_convert_kitti_calib.py +++ b/scripts/basalt_convert_kitti_calib.py @@ -111,8 +111,7 @@ 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, - "view_offset": [0.0, 0.0] + "cam_time_offset_ns": 0 } } ''') diff --git a/src/mapper_sim_naive.cpp b/src/mapper_sim_naive.cpp index 2ba362a..4c62c05 100644 --- a/src/mapper_sim_naive.cpp +++ b/src/mapper_sim_naive.cpp @@ -439,8 +439,8 @@ void draw_image_overlay(pangolin::View& v, size_t cam_id) { auto it = vis_map.find(gt_frame_t_ns[frame_id]); - if (it != vis_map.end() && cam_id < it->second->projections.size()) { - const auto& points = it->second->projections[cam_id]; + if (it != vis_map.end() && cam_id < it->second->projections->size()) { + const auto& points = it->second->projections->at(cam_id); if (points.size() > 0) { double min_id = points[0][2], max_id = points[0][2]; diff --git a/src/monado/slam_tracker.cpp b/src/monado/slam_tracker.cpp index a055896..65d9897 100644 --- a/src/monado/slam_tracker.cpp +++ b/src/monado/slam_tracker.cpp @@ -252,6 +252,7 @@ struct slam_tracker::implementation { while (running) { cout << "[in] frames: " << image_data_queue->size() << "/" << image_data_queue->capacity() << " \t" << "[in] imu: " << imu_data_queue->size() << "/" << imu_data_queue->capacity() << " \t" + << "[in] depth: " << opt_flow_ptr->input_depth_queue.unsafe_size() << "/-- \t" << "[mid] keypoints: " << opt_flow_ptr->output_queue->size() << "/" << opt_flow_ptr->output_queue->capacity() << " \t" << "[out] pose: " << out_state_queue.size() << "/" << out_state_queue.capacity() << "\n"; @@ -274,13 +275,6 @@ struct slam_tracker::implementation { apply_cam_calibration(c); } - bool calib_from_monado = added_cam_calibs.size() == NUM_CAMS; - bool view_offset_unknown = calib.view_offset(0) == 0 && calib.view_offset(1) == 0; - if (calib_from_monado || view_offset_unknown) { - compute_view_offset(); - cout << "Computed view_offset = " << calib.view_offset.transpose() << "\n"; - } - // Overwrite IMU calibration data for (const auto &c : added_imu_calibs) { apply_imu_calibration(c); @@ -305,6 +299,7 @@ struct slam_tracker::implementation { vio->out_vis_queue = &ui.out_vis_queue; }; vio->out_state_queue = &out_state_queue; + vio->opt_flow_depth_guess_queue = &opt_flow_ptr->input_depth_queue; if (!marg_data_path.empty()) { marg_data_saver.reset(new MargDataSaver(marg_data_path)); @@ -316,7 +311,7 @@ struct slam_tracker::implementation { running = true; vio->initialize(Eigen::Vector3d::Zero(), Eigen::Vector3d::Zero()); - if (show_gui) ui.start(vio->getT_w_i_init(), calib); + if (show_gui) ui.start(vio->getT_w_i_init(), calib, vio_config, &opt_flow_ptr->input_depth_queue, opt_flow_ptr); state_consumer_thread = thread(&slam_tracker::implementation::state_consumer, this); if (print_queue) queues_printer_thread = thread(&slam_tracker::implementation::queues_printer, this); } @@ -441,22 +436,6 @@ struct slam_tracker::implementation { return true; } - void compute_view_offset() { - constexpr double DISTANCE_TO_WALL = 2; // In meters - double width = calib.resolution[0][0]; - double height = calib.resolution[0][1]; - Sophus::SE3d T_i_c0 = calib.T_i_c[0]; - Sophus::SE3d T_i_c1 = calib.T_i_c[1]; - Sophus::SE3d T_c1_i = T_i_c1.inverse(); - Sophus::SE3d T_c1_c0 = T_c1_i * T_i_c0; // Maps a point in c0 space to c1 space - Eigen::Vector4d p3d{0, 0, DISTANCE_TO_WALL, 1}; - Eigen::Vector4d p3d_in_c1 = T_c1_c0 * p3d; - Eigen::Vector2d p2d; - calib.intrinsics[1].project(p3d_in_c1, p2d); - calib.view_offset.x() = (width / 2) - p2d.x(); - calib.view_offset.y() = (height / 2) - p2d.y(); - } - void add_cam_calibration(const cam_calibration &cam_calib) { added_cam_calibs.push_back(cam_calib); } void apply_cam_calibration(const cam_calibration &cam_calib) { diff --git a/src/monado/slam_tracker_ui.hpp b/src/monado/slam_tracker_ui.hpp index 3c22e08..98cf51d 100644 --- a/src/monado/slam_tracker_ui.hpp +++ b/src/monado/slam_tracker_ui.hpp @@ -5,10 +5,13 @@ #include +#include #include #include #include #include +#include "basalt/optical_flow/optical_flow.h" +#include "basalt/utils/vio_config.h" #include "sophus/se3.hpp" #include @@ -51,6 +54,8 @@ class slam_tracker_ui { public: tbb::concurrent_bounded_queue out_vis_queue{}; + tbb::concurrent_queue *opt_flow_depth_queue = nullptr; + OpticalFlowBase::Ptr opt_flow; void initialize(int ncams) { vio_data_log.Clear(); @@ -58,10 +63,13 @@ class slam_tracker_ui { num_cams = ncams; } - void start(const Sophus::SE3d &T_w_i_init, const basalt::Calibration &calib) { + void start(const Sophus::SE3d &T_w_i_init, const Calibration &calib, const VioConfig &config, + tbb::concurrent_queue *ofq_depth, OpticalFlowBase::Ptr of) { + opt_flow_depth_queue = ofq_depth; + opt_flow = of; running = true; start_visualization_thread(); - start_ui(T_w_i_init, calib); + start_ui(T_w_i_init, calib, config); } void stop() { @@ -74,6 +82,7 @@ class slam_tracker_ui { vis_thread = thread([&]() { while (true) { out_vis_queue.pop(curr_vis_data); + show_frame = show_frame + 1; if (curr_vis_data.get() == nullptr) break; } cout << "Finished vis_thread\n"; @@ -109,9 +118,10 @@ class slam_tracker_ui { pangolin::Plotter *plotter; pangolin::DataLog imu_data_log{}; - basalt::Calibration calib; - void start_ui(const Sophus::SE3d &T_w_i_init, const basalt::Calibration &c) { - ui_runner_thread = thread(&slam_tracker_ui::ui_runner, this, T_w_i_init, c); + Calibration calib; + VioConfig config; + void start_ui(const Sophus::SE3d &T_w_i_init, const Calibration &cal, const VioConfig &conf) { + ui_runner_thread = thread(&slam_tracker_ui::ui_runner, this, T_w_i_init, cal, conf); } pangolin::Var follow{"ui.follow", true, false, true}; @@ -120,11 +130,12 @@ class slam_tracker_ui { pangolin::Var show_est_bg{"ui.show_est_bg", false, false, true}; pangolin::Var show_est_ba{"ui.show_est_ba", false, false, true}; - void ui_runner(const Sophus::SE3d &T_w_i_init, const basalt::Calibration &c) { + void ui_runner(const Sophus::SE3d &T_w_i_init, const Calibration &cal, const VioConfig &conf) { constexpr int UI_WIDTH = 200; - calib = c; - string window_name = "Basalt SLAM Tracker for Monado"; + calib = cal; + config = conf; + string window_name = "Basalt 6DoF Tracker for Monado"; pangolin::CreateWindowAndBind(window_name, 1800, 1000); glEnable(GL_DEPTH_TEST); @@ -143,6 +154,7 @@ class slam_tracker_ui { std::vector> img_view; while (img_view.size() < calib.intrinsics.size()) { std::shared_ptr iv(new pangolin::ImageView); + iv->UseNN() = true; // Disable antialiasing (toggle it back with the N key) size_t idx = img_view.size(); img_view.push_back(iv); @@ -185,6 +197,11 @@ class slam_tracker_ui { img_view_display.Activate(); + if (fixed_depth.GuiChanged() && opt_flow_depth_queue != nullptr) { + opt_flow_depth_queue->push(fixed_depth); + } + depth_guess = opt_flow->depth_guess; + { pangolin::GlPixFormat fmt; fmt.glformat = GL_LUMINANCE; @@ -217,8 +234,19 @@ class slam_tracker_ui { cout << "Finished ui_runner\n"; } + pangolin::Var show_frame{"ui.show_frame", 0, pangolin::META_FLAG_READONLY}; pangolin::Var show_obs{"ui.show_obs", true, false, true}; pangolin::Var show_ids{"ui.show_ids", false, false, true}; + pangolin::Var show_invdist{"ui.show_invdist", false, false, true}; + + pangolin::Var show_guesses{"ui.Show matching guesses", false, false, true}; + pangolin::Var show_same_pixel_guess{"ui.SAME_PIXEL", true, false, true}; + pangolin::Var show_reproj_avg_depth_guess{"ui.REPROJ_AVG_DEPTH", true, false, true}; + pangolin::Var show_reproj_fix_depth_guess{"ui.REPROJ_FIX_DEPTH", true, false, true}; + pangolin::Var fixed_depth{"ui.FIX_DEPTH", 2, 0, 10}; + pangolin::Var show_active_guess{"ui.Active Guess", true, false, true}; + + pangolin::Var depth_guess{"ui.depth_guess", 2, pangolin::META_FLAG_READONLY}; void draw_image_overlay(pangolin::View &v, size_t cam_id) { UNUSED(v); @@ -229,14 +257,14 @@ class slam_tracker_ui { glEnable(GL_BLEND); glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA); - if (curr_vis_data && cam_id < curr_vis_data->projections.size()) { - const auto &points = curr_vis_data->projections[cam_id]; + if (curr_vis_data && cam_id < curr_vis_data->projections->size()) { + const auto &points = curr_vis_data->projections->at(cam_id); if (!points.empty()) { double min_id = points[0][2]; double max_id = points[0][2]; - for (const auto &points2 : curr_vis_data->projections) { + for (const auto &points2 : *curr_vis_data->projections) { for (const auto &p : points2) { min_id = std::min(min_id, p[2]); max_id = std::max(max_id, p[2]); @@ -255,6 +283,70 @@ class slam_tracker_ui { pangolin::glDrawCirclePerimeter(c[0], c[1], radius); if (show_ids) pangolin::GlFont::I().Text("%d", int(c[3])).Draw(c[0], c[1]); + if (show_invdist) pangolin::GlFont::I().Text("%.3lf", c[2]).Draw(c[0], c[1] + 5); + } + } + + if (show_guesses && cam_id == 1) { + using namespace Eigen; + const auto keypoints0 = curr_vis_data->projections->at(0); + const auto keypoints1 = curr_vis_data->projections->at(1); + + double avg_invdepth = 0; + double num_features = 0; + for (const auto &cam_projs : *curr_vis_data->projections) { + for (const Vector4d &v : cam_projs) avg_invdepth += v.z(); + num_features += cam_projs.size(); + } + bool valid = avg_invdepth > 0 && num_features > 0; + float default_depth = config.optical_flow_matching_default_depth; + double avg_depth = valid ? num_features / avg_invdepth : default_depth; + + for (const Vector4d kp1 : keypoints1) { + double u1 = kp1.x(); + double v1 = kp1.y(); + // double invdist1 = kp1.z(); + double id1 = kp1.w(); + + double u0 = 0; + double v0 = 0; + bool found = false; + for (const Vector4d &kp0 : keypoints0) { + double id0 = kp0.w(); + if (id1 != id0) continue; + u0 = kp0.x(); + v0 = kp0.y(); + found = true; + break; + } + + if (found) { + if (show_same_pixel_guess) { + glColor3f(0, 1, 1); // Cyan + pangolin::glDrawLine(u1, v1, u0, v0); + } + + if (show_reproj_fix_depth_guess) { + glColor3f(1, 1, 0); // Yellow + auto off = calib.viewOffset({u0, v0}, fixed_depth); + pangolin::glDrawLine(u1, v1, u0 - off.x(), v0 - off.y()); + } + + if (show_reproj_avg_depth_guess) { + glColor3f(1, 0, 1); // Magenta + auto off = calib.viewOffset({u0, v0}, avg_depth); + pangolin::glDrawLine(u1, v1, u0 - off.x(), v0 - off.y()); + } + + if (show_active_guess) { + glColor3f(1, 0, 0); // Red + Vector2d off{0, 0}; + if (config.optical_flow_matching_guess_type != MatchingGuessType::SAME_PIXEL) { + off = calib.viewOffset({u0, v0}, opt_flow->depth_guess); + } + pangolin::glDrawLine(u1, v1, u0 - off.x(), v0 - off.y()); + } + } } } diff --git a/src/rs_t265_vio.cpp b/src/rs_t265_vio.cpp index 6efcec7..589101b 100644 --- a/src/rs_t265_vio.cpp +++ b/src/rs_t265_vio.cpp @@ -199,6 +199,7 @@ int main(int argc, char** argv) { opt_flow_ptr->output_queue = &vio->vision_data_queue; if (show_gui) vio->out_vis_queue = &out_vis_queue; vio->out_state_queue = &out_state_queue; + vio->opt_flow_depth_guess_queue = &opt_flow_ptr->input_depth_queue; basalt::MargDataSaver::Ptr marg_data_saver; diff --git a/src/utils/vio_config.cpp b/src/utils/vio_config.cpp index 21f3449..d3017ef 100644 --- a/src/utils/vio_config.cpp +++ b/src/utils/vio_config.cpp @@ -54,6 +54,8 @@ VioConfig::VioConfig() { optical_flow_levels = 3; optical_flow_epipolar_error = 0.005; optical_flow_skip_frames = 1; + optical_flow_matching_guess_type = MatchingGuessType::REPROJ_AVG_DEPTH; + optical_flow_matching_default_depth = 2.0; vio_linearization_type = LinearizationType::ABS_QR; vio_sqrt_marg = true; @@ -134,6 +136,30 @@ void VioConfig::load(const std::string& filename) { namespace cereal { +template +std::string save_minimal(const Archive& ar, + const basalt::MatchingGuessType& guess_type) { + UNUSED(ar); + auto name = magic_enum::enum_name(guess_type); + return std::string(name); +} + +template +void load_minimal(const Archive& ar, basalt::MatchingGuessType& guess_type, + const std::string& name) { + UNUSED(ar); + + auto guess_enum = magic_enum::enum_cast(name); + + if (guess_enum.has_value()) { + guess_type = guess_enum.value(); + } else { + std::cerr << "Could not find the MatchingGuessType for " << name + << std::endl; + std::abort(); + } +} + template std::string save_minimal(const Archive& ar, const basalt::LinearizationType& linearization_type) { @@ -169,6 +195,8 @@ void serialize(Archive& ar, basalt::VioConfig& config) { ar(CEREAL_NVP(config.optical_flow_epipolar_error)); ar(CEREAL_NVP(config.optical_flow_levels)); ar(CEREAL_NVP(config.optical_flow_skip_frames)); + ar(CEREAL_NVP(config.optical_flow_matching_guess_type)); + ar(CEREAL_NVP(config.optical_flow_matching_default_depth)); ar(CEREAL_NVP(config.vio_linearization_type)); ar(CEREAL_NVP(config.vio_sqrt_marg)); diff --git a/src/vi_estimator/sqrt_keypoint_vio.cpp b/src/vi_estimator/sqrt_keypoint_vio.cpp index 0dec1be..bdbcdb7 100644 --- a/src/vi_estimator/sqrt_keypoint_vio.cpp +++ b/src/vi_estimator/sqrt_keypoint_vio.cpp @@ -36,6 +36,7 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include +#include #include #include #include @@ -487,12 +488,17 @@ bool SqrtKeypointVioEstimator::measure( optimize_and_marg(opt_flow_meas->input_images, num_points_connected, lost_landmaks); - const size_t num_cams = opt_flow_meas->observations.size(); - const bool features_ext = opt_flow_meas->input_images->stats.features_enabled; - std::vector> projections{}; - if (features_ext || out_vis_queue) { - projections.resize(num_cams); - computeProjections(projections, last_state_t_ns); + size_t num_cams = opt_flow_meas->observations.size(); + bool features_ext = opt_flow_meas->input_images->stats.features_enabled; + bool avg_depth_needed = + opt_flow_depth_guess_queue && config.optical_flow_matching_guess_type == + MatchingGuessType::REPROJ_AVG_DEPTH; + + using Projections = std::vector>; + std::shared_ptr projections = nullptr; + if (features_ext || out_vis_queue || avg_depth_needed) { + projections = std::make_shared(num_cams); + computeProjections(*projections, last_state_t_ns); } if (out_state_queue) { @@ -504,9 +510,24 @@ bool SqrtKeypointVioEstimator::measure( data->input_images = opt_flow_meas->input_images; data->input_images->addTime("pose_produced"); + if (avg_depth_needed) { + double avg_invdepth = 0; + double num_features = 0; + for (const auto& cam_projs : *projections) { + for (const Eigen::Vector4d& v : cam_projs) avg_invdepth += v.z(); + num_features += cam_projs.size(); + } + + bool valid = avg_invdepth > 0 && num_features > 0; + float default_depth = config.optical_flow_matching_default_depth; + double avg_depth = valid ? num_features / avg_invdepth : default_depth; + + opt_flow_depth_guess_queue->push(avg_depth); + } + if (features_ext) { for (size_t i = 0; i < num_cams; i++) { - for (const Eigen::Vector4d& v : projections[i]) { + for (const Eigen::Vector4d& v : projections->at(i)) { using Feature = xrt::auxiliary::tracking::slam::pose_ext_features_data::feature; Feature lm{}; diff --git a/src/vi_estimator/sqrt_keypoint_vo.cpp b/src/vi_estimator/sqrt_keypoint_vo.cpp index 78e9236..7d443fd 100644 --- a/src/vi_estimator/sqrt_keypoint_vo.cpp +++ b/src/vi_estimator/sqrt_keypoint_vo.cpp @@ -52,6 +52,7 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include +#include namespace basalt { @@ -139,6 +140,8 @@ void SqrtKeypointVoEstimator::initialize(const Eigen::Vector3d& bg, if (!curr_frame.get()) { break; } + curr_frame->input_images->addTime("vio_start"); + curr_frame->input_images->addTime("imu_preintegrated"); // Correct camera time offset (relevant for VIO) // curr_frame->t_ns += calib.cam_time_offset_ns; @@ -408,8 +411,23 @@ bool SqrtKeypointVoEstimator::measure( } } } + opt_flow_meas->input_images->addTime("landmarks_updated"); - optimize_and_marg(num_points_connected, lost_landmaks); + optimize_and_marg(opt_flow_meas->input_images, num_points_connected, + lost_landmaks); + + size_t num_cams = opt_flow_meas->observations.size(); + bool features_ext = opt_flow_meas->input_images->stats.features_enabled; + bool avg_depth_needed = + opt_flow_depth_guess_queue && config.optical_flow_matching_guess_type == + MatchingGuessType::REPROJ_AVG_DEPTH; + + using Projections = std::vector>; + std::shared_ptr projections = nullptr; + if (features_ext || out_vis_queue || avg_depth_needed) { + projections = std::make_shared(num_cams); + computeProjections(*projections, last_state_t_ns); + } if (out_state_queue) { const PoseStateWithLin& p = frame_poses.at(last_state_t_ns); @@ -419,6 +437,38 @@ bool SqrtKeypointVoEstimator::measure( Eigen::Vector3d::Zero(), Eigen::Vector3d::Zero(), Eigen::Vector3d::Zero())); + data->input_images = opt_flow_meas->input_images; + data->input_images->addTime("pose_produced"); + + if (avg_depth_needed) { + double avg_invdepth = 0; + double num_features = 0; + for (const auto& cam_projs : *projections) { + for (const Eigen::Vector4d& v : cam_projs) avg_invdepth += v.z(); + num_features += cam_projs.size(); + } + + bool valid = avg_invdepth > 0 && num_features > 0; + float default_depth = config.optical_flow_matching_default_depth; + double avg_depth = valid ? num_features / avg_invdepth : default_depth; + + opt_flow_depth_guess_queue->push(avg_depth); + } + + if (features_ext) { + for (size_t i = 0; i < num_cams; i++) { + for (const Eigen::Vector4d& v : projections->at(i)) { + using Feature = + xrt::auxiliary::tracking::slam::pose_ext_features_data::feature; + Feature lm{}; + lm.id = v.w(); + lm.u = v.x(); + lm.v = v.y(); + lm.depth = v.z(); + data->input_images->stats.addFeature(i, lm); + } + } + } out_state_queue->push(data); } @@ -435,8 +485,7 @@ bool SqrtKeypointVoEstimator::measure( get_current_points(data->points, data->point_ids); - data->projections.resize(opt_flow_meas->observations.size()); - computeProjections(data->projections, last_state_t_ns); + data->projections = projections; data->opt_flow_res = prev_opt_flow_res[last_state_t_ns]; @@ -1309,10 +1358,13 @@ void SqrtKeypointVoEstimator::optimize() { template void SqrtKeypointVoEstimator::optimize_and_marg( + const OpticalFlowInput::Ptr& input_images, const std::map& num_points_connected, const std::unordered_set& lost_landmaks) { optimize(); + input_images->addTime("optimized"); marginalize(num_points_connected, lost_landmaks); + input_images->addTime("marginalized"); } template diff --git a/src/vio.cpp b/src/vio.cpp index 8cdd386..43f5c01 100644 --- a/src/vio.cpp +++ b/src/vio.cpp @@ -95,6 +95,12 @@ pangolin::Var show_frame("ui.show_frame", 0, 0, 1500); pangolin::Var show_flow("ui.show_flow", false, false, true); pangolin::Var show_obs("ui.show_obs", true, false, true); pangolin::Var show_ids("ui.show_ids", false, false, true); +pangolin::Var show_same_pixel_guess("ui.show_same_pixel_guess", false, + false, true); +pangolin::Var show_active_guess("ui.show_active_guess", false, false, + true); +pangolin::Var depth_guess{"ui.depth_guess", 2, + pangolin::META_FLAG_READONLY}; pangolin::Var show_est_pos("ui.show_est_pos", true, false, true); pangolin::Var show_est_vel("ui.show_est_vel", false, false, true); @@ -310,6 +316,7 @@ int main(int argc, char** argv) { opt_flow_ptr->output_queue = &vio->vision_data_queue; if (show_gui) vio->out_vis_queue = &out_vis_queue; vio->out_state_queue = &out_state_queue; + vio->opt_flow_depth_guess_queue = &opt_flow_ptr->input_depth_queue; } basalt::MargDataSaver::Ptr marg_data_saver; @@ -442,6 +449,7 @@ int main(int argc, char** argv) { std::vector> img_view; while (img_view.size() < calib.intrinsics.size()) { std::shared_ptr iv(new pangolin::ImageView); + iv->UseNN() = true; // Disable antialiasing, can be toggled with N key size_t idx = img_view.size(); img_view.push_back(iv); @@ -693,13 +701,13 @@ void draw_image_overlay(pangolin::View& v, size_t cam_id) { glEnable(GL_BLEND); glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA); - if (it != vis_map.end() && cam_id < it->second->projections.size()) { - const auto& points = it->second->projections[cam_id]; + if (it != vis_map.end() && cam_id < it->second->projections->size()) { + const auto& points = it->second->projections->at(cam_id); if (points.size() > 0) { double min_id = points[0][2], max_id = points[0][2]; - for (const auto& points2 : it->second->projections) + for (const auto& points2 : *it->second->projections) for (const auto& p : points2) { min_id = std::min(min_id, p[2]); max_id = std::max(max_id, p[2]); @@ -726,6 +734,58 @@ void draw_image_overlay(pangolin::View& v, size_t cam_id) { } } + depth_guess = it->second->opt_flow_res->input_images->depth_guess; + bool show_guesses = (show_active_guess || show_same_pixel_guess) && + cam_id == 1 && it != vis_map.end() && + it->second->projections->size() >= 2; + if (show_guesses) { + const auto keypoints0 = it->second->projections->at(0); + const auto keypoints1 = it->second->projections->at(1); + + for (const Eigen::Vector4d& kp1 : keypoints1) { + double u1 = kp1.x(); + double v1 = kp1.y(); + double id1 = kp1.w(); + + // Find match in keypoints0 + double u0 = 0; + double v0 = 0; + bool found = false; + for (const Eigen::Vector4d& kp0 : keypoints0) { + double id0 = kp0.w(); + + if (id1 != id0) continue; + u0 = kp0.x(); + v0 = kp0.y(); + found = true; + break; + } + + // Display guess error if this is a stereo feature + // NOTE: keep in mind that these guesses are not really the guesses + // used to detect the feature, but the guess we would use if we were + // to detect the feature right now. + if (found) { + // Guess if we were using SAME_PIXEL + if (show_same_pixel_guess) { + glColor3f(0, 1, 1); // Cyan + pangolin::glDrawLine(u1, v1, u0, v0); + } + + // Guess with the current guess type + if (show_active_guess) { + glColor3f(1, 0, 1); // Magenta + Eigen::Vector2d off{0, 0}; + if (vio_config.optical_flow_matching_guess_type != + basalt::MatchingGuessType::SAME_PIXEL) { + off = calib.viewOffset({u0, v0}, depth_guess); + } + pangolin::glDrawLine(u1, v1, u0 - off.x(), v0 - off.y()); + } + } + } + } + if (show_flow) { glLineWidth(1.0); glColor3f(1.0, 0.0, 0.0); diff --git a/src/vio_sim.cpp b/src/vio_sim.cpp index f5dd80a..215c147 100644 --- a/src/vio_sim.cpp +++ b/src/vio_sim.cpp @@ -482,8 +482,8 @@ void draw_image_overlay(pangolin::View& v, size_t cam_id) { auto it = vis_map.find(gt_frame_t_ns[frame_id]); - if (it != vis_map.end() && cam_id < it->second->projections.size()) { - const auto& points = it->second->projections[cam_id]; + if (it != vis_map.end() && cam_id < it->second->projections->size()) { + const auto& points = it->second->projections->at(cam_id); if (points.size() > 0) { double min_id = points[0][2], max_id = points[0][2]; diff --git a/thirdparty/basalt-headers b/thirdparty/basalt-headers index 1a92e76..ff99f3d 160000 --- a/thirdparty/basalt-headers +++ b/thirdparty/basalt-headers @@ -1 +1 @@ -Subproject commit 1a92e765bd65037313bb482c56886842c4d3d547 +Subproject commit ff99f3d040fb6aeb3535338ba69c6300191c9bda