Update patch jacobian computation

luxonis · Jan 22, 2022 · 3973aeb · 3973aeb
1 parent 92d754e
commit 3973aeb
Show file tree

Hide file tree

Showing 3 changed files with 147 additions and 25 deletions.
diff --git 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 <Eigen/Dense>
+#include <sophus/se2.hpp>
 
 #include <basalt/image/image.h>
 #include <basalt/optical_flow/patterns.h>
@@ -71,23 +72,57 @@ struct OpticalFlowPatch {
     setFromImage(img, pos);
   }
 
-  void setFromImage(const Image<const uint16_t> &img, const Vector2 &pos) {
-    this->pos = pos;
+  template <typename ImgT>
+  static void setData(const ImgT &img, const Vector2 &pos, Scalar &mean,
+                      VectorP &data, const Sophus::SE2<Scalar> *se2 = nullptr) {
+    int num_valid_points = 0;
+    Scalar sum = 0;
 
+    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<Scalar>(p);
+        data[i] = val;
+        sum += val;
+        num_valid_points++;
+      } else {
+        data[i] = -1;
+      }
+    }
+
+    mean = sum / num_valid_points;
+    data /= mean;
+  }
+
+  template <typename ImgT>
+  static void setDataJacSe2(const ImgT &img, const Vector2 &pos, Scalar &mean,
+                            VectorP &data, MatrixP3 &J_se2) {
     int num_valid_points = 0;
     Scalar sum = 0;
-    Vector2 grad_sum(0, 0);
+    Vector3 grad_sum_se2(0, 0, 0);
 
-    MatrixP2 grad;
+    Eigen::Matrix<Scalar, 2, 3> 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<Scalar>(p);
+        Vector3 valGrad = img.template interpGrad<Scalar>(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<Scalar, 2, 3> 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<const uint16_t> &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
@@ -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
@@ -0,0 +1,89 @@
+
+
+#include <basalt/optical_flow/patch.h>
+#include <sophus/se2.hpp>
+
+#include <iostream>
+
+#include "gtest/gtest.h"
+#include "test_utils.h"
+
+struct SmoothFunction {
+  template <typename Scalar>
+  Scalar interp(const Eigen::Matrix<Scalar, 2, 1>& p) const {
+    return sin(p[0] / 100.0 + p[1] / 20.0);
+  }
+
+  template <typename Scalar>
+  Eigen::Matrix<Scalar, 3, 1> interpGrad(
+      const Eigen::Matrix<Scalar, 2, 1>& p) const {
+    return Eigen::Matrix<Scalar, 3, 1>(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 <typename Derived>
+  BASALT_HOST_DEVICE inline bool InBounds(
+      const Eigen::MatrixBase<Derived>& p,
+      const typename Derived::Scalar border) const {
+    return true;
+  }
+};
+
+TEST(Patch, ImageInterpolateGrad) {
+  Eigen::Vector2i offset(231, 123);
+
+  SmoothFunction img;
+
+  Eigen::Vector2d pd = offset.cast<double>() + Eigen::Vector2d(0.4, 0.34345);
+
+  Eigen::Vector3d val_grad = img.interpGrad<double>(pd);
+  Eigen::Matrix<double, 1, 2> J_x = val_grad.tail<2>();
+
+  test_jacobian(
+      "d_res_d_x", J_x,
+      [&](const Eigen::Vector2d& x) {
+        return Eigen::Matrix<double, 1, 1>(img.interp<double>(pd + x));
+      },
+      Eigen::Vector2d::Zero(), 1);
+}
+
+TEST(Patch, PatchSe2Jac) {
+  Eigen::Vector2i offset(231, 123);
+
+  SmoothFunction img_view;
+
+  Eigen::Vector2d pd = offset.cast<double>() + Eigen::Vector2d(0.4, 0.34345);
+
+  using PatternT = basalt::Pattern52<double>;
+  using PatchT = basalt::OpticalFlowPatch<double, PatternT>;
+
+  double mean, mean2;
+
+  PatchT::VectorP data, data2;
+  PatchT::MatrixP3 J_se2;
+
+  basalt::OpticalFlowPatch<double, basalt::Pattern52<double>>::setDataJacSe2(
+      img_view, pd, mean, data, J_se2);
+
+  basalt::OpticalFlowPatch<double, basalt::Pattern52<double>>::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<double, basalt::Pattern52<double>>::setData(
+            img_view, pd, mean3, data3, &transform);
+
+        return data3;
+      },
+      Eigen::Vector3d::Zero());
+}