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 c3473e3..18aee95 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,7 @@ class MultiscaleFrameToFrameOpticalFlow : public OpticalFlowBase { transform_map_1, const std::map& pyramid_levels_1, Eigen::aligned_map& transform_map_2, - std::map& pyramid_levels_2) const { + std::map& pyramid_levels_2, int64_t view_offset = 0) const { size_t num_points = transform_map_1.size(); std::vector ids; @@ -229,13 +229,17 @@ class MultiscaleFrameToFrameOpticalFlow : 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, pyramid_level[r], + bool valid = transform_2.translation()(0) >= 0 && + transform_2.translation()(0) < pyr_2.lvl(pyramid_level[r]).w; + 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()(0) += view_offset; valid = trackPoint(pyr_2, pyr_1, transform_2, pyramid_level[r], transform_1_recovered); @@ -392,7 +396,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); + new_pyramid_levels_stereo, calib.view_offset); for (const auto& kv : new_poses_stereo) { transforms->observations.at(1).emplace(kv);