diff --git a/include/basalt/optical_flow/patch.h b/include/basalt/optical_flow/patch.h index aee8e9a..29a6400 100644 --- a/include/basalt/optical_flow/patch.h +++ b/include/basalt/optical_flow/patch.h @@ -35,6 +35,7 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #pragma once #include +#include #include #include @@ -71,23 +72,57 @@ struct OpticalFlowPatch { setFromImage(img, pos); } - void setFromImage(const Image &img, const Vector2 &pos) { - this->pos = pos; - + template + static void setData(const ImgT &img, const Vector2 &pos, Scalar &mean, + VectorP &data, const Sophus::SE2 *se2 = nullptr) { int num_valid_points = 0; Scalar sum = 0; - Vector2 grad_sum(0, 0); - MatrixP2 grad; + for (int i = 0; i < PATTERN_SIZE; i++) { + Vector2 p; + if (se2) { + p = pos + (*se2) * pattern2.col(i); + } else { + p = pos + pattern2.col(i); + }; + + if (img.InBounds(p, 2)) { + Scalar val = img.template interp(p); + data[i] = val; + sum += val; + num_valid_points++; + } else { + data[i] = -1; + } + } + + mean = sum / num_valid_points; + data /= mean; + } + + template + static void setDataJacSe2(const ImgT &img, const Vector2 &pos, Scalar &mean, + VectorP &data, MatrixP3 &J_se2) { + int num_valid_points = 0; + Scalar sum = 0; + Vector3 grad_sum_se2(0, 0, 0); + + Eigen::Matrix Jw_se2; + Jw_se2.template topLeftCorner<2, 2>().setIdentity(); for (int i = 0; i < PATTERN_SIZE; i++) { Vector2 p = pos + pattern2.col(i); + + // Fill jacobians with respect to SE2 warp + Jw_se2(0, 2) = -pattern2(1, i); + Jw_se2(1, 2) = pattern2(0, i); + if (img.InBounds(p, 2)) { - Vector3 valGrad = img.interpGrad(p); + Vector3 valGrad = img.template interpGrad(p); data[i] = valGrad[0]; sum += valGrad[0]; - grad.row(i) = valGrad.template tail<2>(); - grad_sum += valGrad.template tail<2>(); + J_se2.row(i) = valGrad.template tail<2>().transpose() * Jw_se2; + grad_sum_se2 += J_se2.row(i); num_valid_points++; } else { data[i] = -1; @@ -96,30 +131,25 @@ struct OpticalFlowPatch { mean = sum / num_valid_points; - Scalar mean_inv = num_valid_points / sum; - - Eigen::Matrix Jw_se2; - Jw_se2.template topLeftCorner<2, 2>().setIdentity(); - - MatrixP3 J_se2; + const Scalar mean_inv = num_valid_points / sum; for (int i = 0; i < PATTERN_SIZE; i++) { if (data[i] >= 0) { - const Scalar data_i = data[i]; - const Vector2 grad_i = grad.row(i); - grad.row(i) = - num_valid_points * (grad_i * sum - grad_sum * data_i) / (sum * sum); - + J_se2.row(i) -= grad_sum_se2.transpose() * data[i] / sum; data[i] *= mean_inv; } else { - grad.row(i).setZero(); + J_se2.row(i).setZero(); } - - // Fill jacobians with respect to SE2 warp - Jw_se2(0, 2) = -pattern2(1, i); - Jw_se2(1, 2) = pattern2(0, i); - J_se2.row(i) = grad.row(i) * Jw_se2; } + J_se2 *= mean_inv; + } + + void setFromImage(const Image &img, const Vector2 &pos) { + this->pos = pos; + + MatrixP3 J_se2; + + setDataJacSe2(img, pos, mean, data, J_se2); Matrix3 H_se2 = J_se2.transpose() * J_se2; Matrix3 H_se2_inv; @@ -143,7 +173,6 @@ struct OpticalFlowPatch { const Matrix2P &transformed_pattern, VectorP &residual) const { Scalar sum = 0; - Vector2 grad_sum(0, 0); int num_valid_points = 0; for (int i = 0; i < PATTERN_SIZE; i++) { diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt index eb83837..7583fd7 100644 --- a/test/CMakeLists.txt +++ b/test/CMakeLists.txt @@ -20,6 +20,9 @@ target_link_libraries(test_qr gtest gtest_main basalt) add_executable(test_linearization src/test_linearization.cpp) target_link_libraries(test_linearization gtest gtest_main basalt) +add_executable(test_patch src/test_patch.cpp) +target_link_libraries(test_patch gtest gtest_main basalt) + enable_testing() include(GoogleTest) @@ -33,3 +36,4 @@ gtest_add_tests(TARGET test_vio AUTO) gtest_add_tests(TARGET test_nfr AUTO) gtest_add_tests(TARGET test_qr AUTO) gtest_add_tests(TARGET test_linearization AUTO) +gtest_add_tests(TARGET test_patch AUTO) diff --git a/test/src/test_patch.cpp b/test/src/test_patch.cpp new file mode 100644 index 0000000..6cedea2 --- /dev/null +++ b/test/src/test_patch.cpp @@ -0,0 +1,89 @@ + + +#include +#include + +#include + +#include "gtest/gtest.h" +#include "test_utils.h" + +struct SmoothFunction { + template + Scalar interp(const Eigen::Matrix& p) const { + return sin(p[0] / 100.0 + p[1] / 20.0); + } + + template + Eigen::Matrix interpGrad( + const Eigen::Matrix& p) const { + return Eigen::Matrix(sin(p[0] / 100.0 + p[1] / 20.0), + cos(p[0] / 100.0 + p[1] / 20.0) / 100.0, + cos(p[0] / 100.0 + p[1] / 20.0) / 20.0); + } + + template + BASALT_HOST_DEVICE inline bool InBounds( + const Eigen::MatrixBase& p, + const typename Derived::Scalar border) const { + return true; + } +}; + +TEST(Patch, ImageInterpolateGrad) { + Eigen::Vector2i offset(231, 123); + + SmoothFunction img; + + Eigen::Vector2d pd = offset.cast() + Eigen::Vector2d(0.4, 0.34345); + + Eigen::Vector3d val_grad = img.interpGrad(pd); + Eigen::Matrix J_x = val_grad.tail<2>(); + + test_jacobian( + "d_res_d_x", J_x, + [&](const Eigen::Vector2d& x) { + return Eigen::Matrix(img.interp(pd + x)); + }, + Eigen::Vector2d::Zero(), 1); +} + +TEST(Patch, PatchSe2Jac) { + Eigen::Vector2i offset(231, 123); + + SmoothFunction img_view; + + Eigen::Vector2d pd = offset.cast() + Eigen::Vector2d(0.4, 0.34345); + + using PatternT = basalt::Pattern52; + using PatchT = basalt::OpticalFlowPatch; + + double mean, mean2; + + PatchT::VectorP data, data2; + PatchT::MatrixP3 J_se2; + + basalt::OpticalFlowPatch>::setDataJacSe2( + img_view, pd, mean, data, J_se2); + + basalt::OpticalFlowPatch>::setData( + img_view, pd, mean2, data2); + + EXPECT_NEAR(mean, mean2, 1e-8); + EXPECT_TRUE(data.isApprox(data2)); + + test_jacobian( + "d_res_d_se2", J_se2, + [&](const Eigen::Vector3d& x) { + Sophus::SE2d transform = Sophus::SE2d::exp(x); + + double mean3; + PatchT::VectorP data3; + + basalt::OpticalFlowPatch>::setData( + img_view, pd, mean3, data3, &transform); + + return data3; + }, + Eigen::Vector3d::Zero()); +}