v01

2022-04-05 11:42:28 +03:00
commit 6dc0eb0fcf
5565 changed files with 1200500 additions and 0 deletions
--- a/thirdparty/basalt-headers/test/include/test_utils.h
+++ b/thirdparty/basalt-headers/test/include/test_utils.h
@@ -0,0 +1,64 @@
+#pragma once
+
+#include <sophus/common.hpp>
+
+#include "gtest/gtest.h"
+
+template <class Scalar>
+struct TestConstants;
+
+template <>
+struct TestConstants<double> {
+  static constexpr double epsilon = 1e-8;
+  static constexpr double max_norm = 1e-3;
+};
+
+template <>
+struct TestConstants<float> {
+  static constexpr double epsilon = 1e-2;
+  static constexpr double max_norm = 1e-2;
+};
+
+template <typename Derived1, typename Derived2, typename F>
+void test_jacobian(
+    const std::string &name, const Eigen::MatrixBase<Derived1> &Ja, F func,
+    const Eigen::MatrixBase<Derived2> &x0,
+    double eps = TestConstants<typename Derived1::Scalar>::epsilon,
+    double max_norm = TestConstants<typename Derived1::Scalar>::max_norm) {
+  typedef typename Derived1::Scalar Scalar;
+
+  Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> Jn = Ja;
+  Jn.setZero();
+
+  Eigen::Matrix<Scalar, Eigen::Dynamic, 1> inc = x0;
+  for (int i = 0; i < Jn.cols(); i++) {
+    inc.setZero();
+    inc[i] += eps;
+
+    Eigen::Matrix<Scalar, Eigen::Dynamic, 1> fpe = func(x0 + inc);
+    Eigen::Matrix<Scalar, Eigen::Dynamic, 1> fme = func(x0 - inc);
+
+    Jn.col(i) = (fpe - fme);
+  }
+
+  Jn /= (2 * eps);
+
+  EXPECT_TRUE(Ja.allFinite()) << name << ": Ja not finite\n " << Ja;
+  EXPECT_TRUE(Jn.allFinite()) << name << ": Jn not finite\n " << Jn;
+
+  if (Jn.isZero(max_norm) && Ja.isZero(max_norm)) {
+    EXPECT_TRUE((Jn - Ja).isZero(max_norm))
+        << name << ": Ja not equal to Jn(diff norm:" << (Jn - Ja).norm()
+        << ")\nJa: (norm: " << Ja.norm() << ")\n"
+        << Ja << "\nJn: (norm: " << Jn.norm() << ")\n"
+        << Jn;
+    //<< "\ndiff:\n" << Jn - Ja;
+  } else {
+    EXPECT_TRUE(Jn.isApprox(Ja, max_norm))
+        << name << ": Ja not equal to Jn (diff norm:" << (Jn - Ja).norm()
+        << ")\nJa: (norm: " << Ja.norm() << ")\n"
+        << Ja << "\nJn: (norm: " << Jn.norm() << ")\n"
+        << Jn;
+    //<< "\ndiff:\n" << Jn - Ja;
+  }
+}