v01

thirdparty/basalt-headers/test/src/benchmark_camera.cpp

@@ -0,0 +1,152 @@
+#include <benchmark/benchmark.h>
+
+#include <basalt/camera/generic_camera.hpp>
+
+template <class CamT>
+void bmProject(benchmark::State &state) {
+  static constexpr int SIZE = 50;
+
+  using Vec4 = typename CamT::Vec4;
+  using Vec2 = typename CamT::Vec2;
+
+  Eigen::aligned_vector<CamT> test_cams = CamT::getTestProjections();
+
+  Vec4 p(0, 0, 5, 1);
+
+  for (auto _ : state) {
+    for (const CamT &cam : test_cams) {
+      for (int x = -SIZE; x < SIZE; x++) {
+        for (int y = -SIZE; y < SIZE; y++) {
+          p[0] = x;
+          p[1] = y;
+
+          Vec2 res;
+          benchmark::DoNotOptimize(cam.project(p, res));
+        }
+      }
+    }
+  }
+}
+
+template <class CamT>
+void bmProjectJacobians(benchmark::State &state) {
+  static constexpr int SIZE = 50;
+
+  using Vec2 = typename CamT::Vec2;
+  using Vec4 = typename CamT::Vec4;
+
+  using Mat24 = typename CamT::Mat24;
+  using Mat2N = typename CamT::Mat2N;
+
+  Eigen::aligned_vector<CamT> test_cams = CamT::getTestProjections();
+
+  Mat24 J_p;
+  Mat2N J_param;
+
+  Vec4 p(0, 0, 5, 1);
+
+  for (auto _ : state) {
+    for (const CamT &cam : test_cams) {
+      for (int x = -SIZE; x <= SIZE; x++) {
+        for (int y = -SIZE; y <= SIZE; y++) {
+          p[0] = x;
+          p[1] = y;
+
+          Vec2 res;
+          benchmark::DoNotOptimize(cam.project(p, res, &J_p, &J_param));
+        }
+      }
+    }
+  }
+}
+
+template <class CamT>
+void bmUnproject(benchmark::State &state) {
+  static constexpr int SIZE = 50;
+
+  using Vec2 = typename CamT::Vec2;
+  using Vec4 = typename CamT::Vec4;
+
+  Eigen::aligned_vector<CamT> test_cams = CamT::getTestProjections();
+
+  for (auto _ : state) {
+    for (const CamT &cam : test_cams) {
+      Vec2 p_center(cam.getParam()(2), cam.getParam()(3));
+
+      for (int x = -SIZE; x <= SIZE; x++) {
+        for (int y = -SIZE; y <= SIZE; y++) {
+          Vec2 p = p_center;
+          p[0] += x;
+          p[1] += y;
+
+          Vec4 res;
+          benchmark::DoNotOptimize(cam.unproject(p, res));
+        }
+      }
+    }
+  }
+}
+
+template <class CamT>
+void bmUnprojectJacobians(benchmark::State &state) {
+  static constexpr int SIZE = 50;
+
+  using Vec2 = typename CamT::Vec2;
+  using Vec4 = typename CamT::Vec4;
+
+  Eigen::aligned_vector<CamT> test_cams = CamT::getTestProjections();
+
+  using Mat42 = typename CamT::Mat42;
+  using Mat4N = typename CamT::Mat4N;
+
+  Mat42 J_p;
+  Mat4N J_param;
+
+  for (auto _ : state) {
+    for (const CamT &cam : test_cams) {
+      Vec2 p_center(cam.getParam()(2), cam.getParam()(3));
+
+      for (int x = -SIZE; x <= SIZE; x++) {
+        for (int y = -SIZE; y <= SIZE; y++) {
+          Vec2 p = p_center;
+          p[0] += x;
+          p[1] += y;
+
+          Vec4 res;
+
+          benchmark::DoNotOptimize(cam.unproject(p, res, &J_p, &J_param));
+        }
+      }
+    }
+  }
+}
+
+BENCHMARK_TEMPLATE(bmProject, basalt::PinholeCamera<double>);
+BENCHMARK_TEMPLATE(bmProject, basalt::ExtendedUnifiedCamera<double>);
+BENCHMARK_TEMPLATE(bmProject, basalt::UnifiedCamera<double>);
+BENCHMARK_TEMPLATE(bmProject, basalt::KannalaBrandtCamera4<double>);
+BENCHMARK_TEMPLATE(bmProject, basalt::DoubleSphereCamera<double>);
+BENCHMARK_TEMPLATE(bmProject, basalt::FovCamera<double>);
+
+BENCHMARK_TEMPLATE(bmProjectJacobians, basalt::PinholeCamera<double>);
+BENCHMARK_TEMPLATE(bmProjectJacobians, basalt::ExtendedUnifiedCamera<double>);
+BENCHMARK_TEMPLATE(bmProjectJacobians, basalt::UnifiedCamera<double>);
+BENCHMARK_TEMPLATE(bmProjectJacobians, basalt::KannalaBrandtCamera4<double>);
+BENCHMARK_TEMPLATE(bmProjectJacobians, basalt::DoubleSphereCamera<double>);
+BENCHMARK_TEMPLATE(bmProjectJacobians, basalt::FovCamera<double>);
+
+BENCHMARK_TEMPLATE(bmUnproject, basalt::PinholeCamera<double>);
+BENCHMARK_TEMPLATE(bmUnproject, basalt::ExtendedUnifiedCamera<double>);
+BENCHMARK_TEMPLATE(bmUnproject, basalt::UnifiedCamera<double>);
+BENCHMARK_TEMPLATE(bmUnproject, basalt::KannalaBrandtCamera4<double>);
+BENCHMARK_TEMPLATE(bmUnproject, basalt::DoubleSphereCamera<double>);
+BENCHMARK_TEMPLATE(bmUnproject, basalt::FovCamera<double>);
+
+BENCHMARK_TEMPLATE(bmUnprojectJacobians, basalt::PinholeCamera<double>);
+BENCHMARK_TEMPLATE(bmUnprojectJacobians, basalt::ExtendedUnifiedCamera<double>);
+BENCHMARK_TEMPLATE(bmUnprojectJacobians, basalt::UnifiedCamera<double>);
+BENCHMARK_TEMPLATE(bmUnprojectJacobians, basalt::KannalaBrandtCamera4<double>);
+BENCHMARK_TEMPLATE(bmUnprojectJacobians, basalt::DoubleSphereCamera<double>);
+BENCHMARK_TEMPLATE(bmUnprojectJacobians, basalt::FovCamera<double>);
+
+BENCHMARK_MAIN();

thirdparty/basalt-headers/test/src/test_camera.cpp

@@ -0,0 +1,678 @@
+/**
+BSD 3-Clause License
+
+Copyright (c) 2019, Vladyslav Usenko and Nikolaus Demmel.
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+
+* Redistributions of source code must retain the above copyright notice, this
+  list of conditions and the following disclaimer.
+
+* Redistributions in binary form must reproduce the above copyright notice,
+  this list of conditions and the following disclaimer in the documentation
+  and/or other materials provided with the distribution.
+
+* Neither the name of the copyright holder nor the names of its
+  contributors may be used to endorse or promote products derived from
+  this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
+FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
+OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#include <basalt/camera/generic_camera.hpp>
+#include <basalt/camera/stereographic_param.hpp>
+
+#include "gtest/gtest.h"
+#include "test_utils.h"
+
+template <typename CamT>
+void testProjectJacobian() {
+  Eigen::aligned_vector<CamT> test_cams = CamT::getTestProjections();
+
+  using VecN = typename CamT::VecN;
+  using Vec2 = typename CamT::Vec2;
+  using Vec4 = typename CamT::Vec4;
+
+  using Mat24 = typename CamT::Mat24;
+  using Mat2N = typename CamT::Mat2N;
+
+  for (const CamT &cam : test_cams) {
+    for (int x = -10; x <= 10; x++) {
+      for (int y = -10; y <= 10; y++) {
+        for (int z = -1; z <= 5; z++) {
+          Vec4 p(x, y, z, 1);
+
+          Mat24 J_p;
+          Mat2N J_param;
+
+          Vec2 res1;
+
+          bool success = cam.project(p, res1, &J_p, &J_param);
+
+          if (success) {
+            test_jacobian(
+                "d_r_d_p", J_p,
+                [&](const Vec4 &x) {
+                  Vec2 res;
+                  cam.project(p + x, res);
+                  return res;
+                },
+                Vec4::Zero());
+
+            test_jacobian(
+                "d_r_d_param", J_param,
+                [&](const VecN &x) {
+                  Vec2 res;
+
+                  CamT cam1 = cam;
+                  cam1 += x;
+
+                  cam1.project(p, res);
+                  return res;
+                },
+                VecN::Zero());
+          }
+        }
+      }
+    }
+  }
+}
+
+template <typename CamT>
+void testProjectJacobian3() {
+  Eigen::aligned_vector<CamT> test_cams = CamT::getTestProjections();
+
+  constexpr int N = CamT::N;
+
+  using Scalar = typename CamT::Scalar;
+  using VecN = typename CamT::VecN;
+  using Vec2 = typename CamT::Vec2;
+  using Vec3 = Eigen::Matrix<Scalar, 3, 1>;
+
+  using Mat23 = Eigen::Matrix<Scalar, 2, 3, Eigen::RowMajor>;
+  using Mat2N = Eigen::Matrix<Scalar, 2, N, Eigen::RowMajor>;
+
+  for (const CamT &cam : test_cams) {
+    for (int x = -10; x <= 10; x++) {
+      for (int y = -10; y <= 10; y++) {
+        for (int z = -1; z <= 5; z++) {
+          Vec3 p_init(x, y, z);
+
+          Scalar p_raw_array[3];
+          Scalar res_raw_array[2];
+          Scalar J_p_raw_array[2 * 3];
+          Scalar J_param_raw_array[2 * N];
+
+          Eigen::Map<Vec3> p(p_raw_array);
+          p = p_init;
+
+          Eigen::Map<Mat23> J_p(J_p_raw_array);
+          Eigen::Map<Mat2N> J_param(J_param_raw_array);
+
+          Eigen::Map<Vec2> res1(res_raw_array);
+
+          bool success = cam.project(p, res1, &J_p, nullptr);
+          success = cam.project(p, res1, nullptr, &J_param);
+
+          if (success) {
+            test_jacobian(
+                "d_r_d_p", J_p,
+                [&](const Vec3 &x) {
+                  Vec2 res;
+                  cam.project(p + x, res);
+                  return res;
+                },
+                Vec3::Zero());
+
+            test_jacobian(
+                "d_r_d_param", J_param,
+                [&](const VecN &x) {
+                  Vec2 res;
+
+                  CamT cam1 = cam;
+                  cam1 += x;
+
+                  cam1.project(p, res);
+                  return res;
+                },
+                VecN::Zero());
+          }
+        }
+      }
+    }
+  }
+}
+
+template <typename CamT>
+void testProjectUnproject() {
+  Eigen::aligned_vector<CamT> test_cams = CamT::getTestProjections();
+
+  using Scalar = typename CamT::Vec2::Scalar;
+  using Vec2 = typename CamT::Vec2;
+  using Vec4 = typename CamT::Vec4;
+
+  for (const CamT &cam : test_cams) {
+    for (int x = -10; x <= 10; x++) {
+      for (int y = -10; y <= 10; y++) {
+        for (int z = 0; z <= 5; z++) {
+          Vec4 p(x, y, z, 0.23424);
+
+          Vec4 p_normalized = Vec4::Zero();
+          p_normalized.template head<3>() = p.template head<3>().normalized();
+          Vec2 res;
+          bool success = cam.project(p, res);
+
+          if (success) {
+            Vec4 p_uproj;
+            cam.unproject(res, p_uproj);
+
+            EXPECT_TRUE(p_normalized.isApprox(
+                p_uproj, Sophus::Constants<Scalar>::epsilonSqrt()))
+                << "p_normalized " << p_normalized.transpose() << " p_uproj "
+                << p_uproj.transpose();
+          }
+        }
+      }
+    }
+  }
+}
+
+template <typename CamT>
+void testUnprojectJacobians() {
+  Eigen::aligned_vector<CamT> test_cams = CamT::getTestProjections();
+
+  using VecN = typename CamT::VecN;
+  using Vec2 = typename CamT::Vec2;
+  using Vec4 = typename CamT::Vec4;
+
+  using Mat42 = typename CamT::Mat42;
+  using Mat4N = typename CamT::Mat4N;
+
+  for (const CamT &cam : test_cams) {
+    for (int x = -10; x <= 10; x++) {
+      for (int y = -10; y <= 10; y++) {
+        for (int z = 0; z <= 5; z++) {
+          Vec4 p_3d(x, y, z, 0);
+
+          Vec2 p;
+          bool success = cam.project(p_3d, p);
+
+          if (success) {
+            Mat42 J_p;
+            Mat4N J_param;
+
+            Vec4 res1;
+            cam.unproject(p, res1, &J_p, &J_param);
+
+            test_jacobian(
+                "d_r_d_p", J_p,
+                [&](const Vec2 &x) {
+                  Vec4 res = Vec4::Zero();
+                  cam.unproject(p + x, res);
+                  return res;
+                },
+                Vec2::Zero());
+
+            test_jacobian(
+                "d_r_d_param", J_param,
+                [&](const VecN &x) {
+                  Vec4 res = Vec4::Zero();
+                  CamT cam1 = cam;
+                  cam1 += x;
+
+                  cam1.unproject(p, res);
+                  return res;
+                },
+                VecN::Zero());
+          }
+        }
+      }
+    }
+  }
+}
+
+template <typename CamT>
+void testUnprojectJacobians3() {
+  Eigen::aligned_vector<CamT> test_cams = CamT::getTestProjections();
+
+  using Scalar = typename CamT::Scalar;
+  constexpr int N = CamT::N;
+
+  using VecN = typename CamT::VecN;
+  using Vec2 = typename CamT::Vec2;
+  using Vec3 = Eigen::Matrix<Scalar, 3, 1>;
+
+  using Mat32 = Eigen::Matrix<Scalar, 3, 2, Eigen::RowMajor>;
+  using Mat3N = Eigen::Matrix<Scalar, 3, N, Eigen::RowMajor>;
+
+  for (const CamT &cam : test_cams) {
+    for (int x = -10; x <= 10; x++) {
+      for (int y = -10; y <= 10; y++) {
+        for (int z = 0; z <= 5; z++) {
+          Vec3 p_3d(x, y, z);
+
+          Vec2 p;
+          bool success = cam.project(p_3d, p);
+
+          if (success) {
+            Mat32 J_p;
+            Mat3N J_param;
+
+            Vec3 res1;
+            cam.unproject(p, res1, &J_p, nullptr);
+            cam.unproject(p, res1, nullptr, &J_param);
+
+            test_jacobian(
+                "d_r_d_p", J_p,
+                [&](const Vec2 &x) {
+                  Vec3 res = Vec3::Zero();
+                  cam.unproject(p + x, res);
+                  return res;
+                },
+                Vec2::Zero());
+
+            test_jacobian(
+                "d_r_d_param", J_param,
+                [&](const VecN &x) {
+                  Vec3 res = Vec3::Zero();
+                  CamT cam1 = cam;
+                  cam1 += x;
+
+                  cam1.unproject(p, res);
+                  return res;
+                },
+                VecN::Zero());
+          }
+        }
+      }
+    }
+  }
+}
+
+////////////////////////////////////////////////////////////////
+
+TEST(CameraTestCase, PinholeProjectJacobians) {
+  testProjectJacobian<basalt::PinholeCamera<double>>();
+}
+TEST(CameraTestCase, PinholeProjectJacobiansFloat) {
+  testProjectJacobian<basalt::PinholeCamera<float>>();
+}
+
+TEST(CameraTestCase, UnifiedProjectJacobians) {
+  testProjectJacobian<basalt::UnifiedCamera<double>>();
+}
+TEST(CameraTestCase, UnifiedProjectJacobiansFloat) {
+  testProjectJacobian<basalt::UnifiedCamera<float>>();
+}
+
+TEST(CameraTestCase, ExtendedUnifiedProjectJacobians) {
+  testProjectJacobian<basalt::ExtendedUnifiedCamera<double>>();
+}
+TEST(CameraTestCase, ExtendedUnifiedProjectJacobiansFloat) {
+  testProjectJacobian<basalt::ExtendedUnifiedCamera<float>>();
+}
+
+TEST(CameraTestCase, KannalaBrandtProjectJacobians) {
+  testProjectJacobian<basalt::KannalaBrandtCamera4<double>>();
+}
+TEST(CameraTestCase, KannalaBrandtProjectJacobiansFloat) {
+  testProjectJacobian<basalt::KannalaBrandtCamera4<float>>();
+}
+
+TEST(CameraTestCase, DoubleSphereJacobians) {
+  testProjectJacobian<basalt::DoubleSphereCamera<double>>();
+}
+TEST(CameraTestCase, FovCameraJacobians) {
+  testProjectJacobian<basalt::FovCamera<double>>();
+}
+
+TEST(CameraTestCase, BalCameraJacobians) {
+  testProjectJacobian<basalt::BalCamera<double>>();
+}
+
+TEST(CameraTestCase, BalCameraJacobiansFloat) {
+  testProjectJacobian<basalt::BalCamera<float>>();
+}
+
+////////////////////////////////////////////////////////////////
+
+TEST(CameraTestCase, PinholeProjectUnproject) {
+  testProjectUnproject<basalt::PinholeCamera<double>>();
+}
+TEST(CameraTestCase, PinholeProjectUnprojectFloat) {
+  testProjectUnproject<basalt::PinholeCamera<float>>();
+}
+
+TEST(CameraTestCase, UnifiedProjectUnproject) {
+  testProjectUnproject<basalt::UnifiedCamera<double>>();
+}
+TEST(CameraTestCase, UnifiedProjectUnprojectFloat) {
+  testProjectUnproject<basalt::UnifiedCamera<float>>();
+}
+
+TEST(CameraTestCase, ExtendedUnifiedProjectUnproject) {
+  testProjectUnproject<basalt::ExtendedUnifiedCamera<double>>();
+}
+TEST(CameraTestCase, ExtendedUnifiedProjectUnprojectFloat) {
+  testProjectUnproject<basalt::ExtendedUnifiedCamera<float>>();
+}
+
+TEST(CameraTestCase, KannalaBrandtProjectUnproject) {
+  testProjectUnproject<basalt::KannalaBrandtCamera4<double>>();
+}
+TEST(CameraTestCase, KannalaBrandtProjectUnprojectFloat) {
+  testProjectUnproject<basalt::KannalaBrandtCamera4<float>>();
+}
+
+TEST(CameraTestCase, DoubleSphereProjectUnproject) {
+  testProjectUnproject<basalt::DoubleSphereCamera<double>>();
+}
+TEST(CameraTestCase, DoubleSphereProjectUnprojectFloat) {
+  testProjectUnproject<basalt::DoubleSphereCamera<float>>();
+}
+
+TEST(CameraTestCase, FovProjectUnproject) {
+  testProjectUnproject<basalt::FovCamera<double>>();
+}
+
+TEST(CameraTestCase, FovProjectUnprojectFloat) {
+  testProjectUnproject<basalt::FovCamera<float>>();
+}
+
+TEST(CameraTestCase, BalProjectUnproject) {
+  testProjectUnproject<basalt::BalCamera<double>>();
+}
+
+TEST(CameraTestCase, BalProjectUnprojectFloat) {
+  testProjectUnproject<basalt::BalCamera<float>>();
+}
+
+/////////////////////////////////////////////////////////////////////////
+
+TEST(CameraTestCase, PinholeUnprojectJacobians) {
+  testUnprojectJacobians<basalt::PinholeCamera<double>>();
+}
+TEST(CameraTestCase, PinholeUnprojectJacobiansFloat) {
+  testUnprojectJacobians<basalt::PinholeCamera<float>>();
+}
+
+TEST(CameraTestCase, UnifiedUnprojectJacobians) {
+  testUnprojectJacobians<basalt::UnifiedCamera<double>>();
+}
+TEST(CameraTestCase, UnifiedUnprojectJacobiansFloat) {
+  testUnprojectJacobians<basalt::UnifiedCamera<float>>();
+}
+
+TEST(CameraTestCase, ExtendedUnifiedUnprojectJacobians) {
+  testUnprojectJacobians<basalt::ExtendedUnifiedCamera<double>>();
+}
+TEST(CameraTestCase, ExtendedUnifiedUnprojectJacobiansFloat) {
+  testUnprojectJacobians<basalt::ExtendedUnifiedCamera<float>>();
+}
+
+TEST(CameraTestCase, KannalaBrandtUnprojectJacobians) {
+  testUnprojectJacobians<basalt::KannalaBrandtCamera4<double>>();
+}
+// TEST(CameraTestCase, KannalaBrandtUnprojectJacobiansFloat) {
+//  test_unproject_jacobians<basalt::KannalaBrandtCamera4<float>>();
+//}
+
+TEST(CameraTestCase, DoubleSphereUnprojectJacobians) {
+  testUnprojectJacobians<basalt::DoubleSphereCamera<double>>();
+}
+// TEST(CameraTestCase, DoubleSphereUnprojectJacobiansFloat) {
+//  test_unproject_jacobians<basalt::DoubleSphereCamera<float>>();
+//}
+
+TEST(CameraTestCase, FovUnprojectJacobians) {
+  testUnprojectJacobians<basalt::FovCamera<double>>();
+}
+TEST(CameraTestCase, FovUnprojectJacobiansFloat) {
+  testUnprojectJacobians<basalt::FovCamera<float>>();
+}
+
+////////////////////////////////////////////////////////////////
+
+TEST(CameraTestCase, PinholeProjectJacobians3) {
+  testProjectJacobian3<basalt::PinholeCamera<double>>();
+}
+TEST(CameraTestCase, PinholeProjectJacobiansFloat3) {
+  testProjectJacobian3<basalt::PinholeCamera<float>>();
+}
+
+TEST(CameraTestCase, UnifiedProjectJacobians3) {
+  testProjectJacobian3<basalt::UnifiedCamera<double>>();
+}
+TEST(CameraTestCase, UnifiedProjectJacobiansFloat3) {
+  testProjectJacobian3<basalt::UnifiedCamera<float>>();
+}
+
+TEST(CameraTestCase, ExtendedUnifiedProjectJacobians3) {
+  testProjectJacobian3<basalt::ExtendedUnifiedCamera<double>>();
+}
+TEST(CameraTestCase, ExtendedUnifiedProjectJacobiansFloat3) {
+  testProjectJacobian3<basalt::ExtendedUnifiedCamera<float>>();
+}
+
+TEST(CameraTestCase, KannalaBrandtProjectJacobians3) {
+  testProjectJacobian3<basalt::KannalaBrandtCamera4<double>>();
+}
+TEST(CameraTestCase, KannalaBrandtProjectJacobiansFloat3) {
+  testProjectJacobian3<basalt::KannalaBrandtCamera4<float>>();
+}
+
+TEST(CameraTestCase, DoubleSphereJacobians3) {
+  testProjectJacobian3<basalt::DoubleSphereCamera<double>>();
+}
+TEST(CameraTestCase, FovCameraJacobians3) {
+  testProjectJacobian3<basalt::FovCamera<double>>();
+}
+
+TEST(CameraTestCase, BalCameraJacobians3) {
+  testProjectJacobian3<basalt::BalCamera<double>>();
+}
+
+TEST(CameraTestCase, BalCameraJacobiansFloat3) {
+  testProjectJacobian3<basalt::BalCamera<float>>();
+}
+
+////////////////////////////////////////////////////////////////
+
+TEST(CameraTestCase, PinholeUnprojectJacobians3) {
+  testUnprojectJacobians3<basalt::PinholeCamera<double>>();
+}
+TEST(CameraTestCase, PinholeUnprojectJacobiansFloat3) {
+  testUnprojectJacobians3<basalt::PinholeCamera<float>>();
+}
+
+TEST(CameraTestCase, UnifiedUnprojectJacobians3) {
+  testUnprojectJacobians3<basalt::UnifiedCamera<double>>();
+}
+TEST(CameraTestCase, UnifiedUnprojectJacobiansFloat3) {
+  testUnprojectJacobians3<basalt::UnifiedCamera<float>>();
+}
+
+TEST(CameraTestCase, ExtendedUnifiedUnprojectJacobians3) {
+  testUnprojectJacobians3<basalt::ExtendedUnifiedCamera<double>>();
+}
+TEST(CameraTestCase, ExtendedUnifiedUnprojectJacobiansFloat3) {
+  testUnprojectJacobians3<basalt::ExtendedUnifiedCamera<float>>();
+}
+
+TEST(CameraTestCase, KannalaBrandtUnprojectJacobians3) {
+  testUnprojectJacobians3<basalt::KannalaBrandtCamera4<double>>();
+}
+// TEST(CameraTestCase, KannalaBrandtUnprojectJacobiansFloat3) {
+//  test_unproject_jacobians3<basalt::KannalaBrandtCamera4<float>>();
+//}
+
+TEST(CameraTestCase, DoubleSphereUnprojectJacobians3) {
+  testUnprojectJacobians3<basalt::DoubleSphereCamera<double>>();
+}
+// TEST(CameraTestCase, DoubleSphereUnprojectJacobiansFloat3) {
+//  test_unproject_jacobians3<basalt::DoubleSphereCamera<float>>();
+//}
+
+TEST(CameraTestCase, FovUnprojectJacobians3) {
+  testUnprojectJacobians3<basalt::FovCamera<double>>();
+}
+TEST(CameraTestCase, FovUnprojectJacobiansFloat3) {
+  testUnprojectJacobians3<basalt::FovCamera<float>>();
+}
+
+////////////////////////////////////////////////////////////////
+
+template <typename CamT>
+void testStereographicProjectJacobian() {
+  using Vec2 = typename CamT::Vec2;
+  using Vec4 = typename CamT::Vec4;
+  using Mat24 = typename CamT::Mat24;
+
+  for (int x = -10; x <= 10; x++) {
+    for (int y = -10; y <= 10; y++) {
+      Vec4 p(x, y, 5, 0.23424);
+
+      Mat24 J_p;
+
+      Vec2 res1 = CamT::project(p, &J_p);
+      Vec2 res2 = CamT::project(p);
+
+      ASSERT_TRUE(res1.isApprox(res2))
+          << "res1 " << res1.transpose() << " res2 " << res2.transpose();
+
+      test_jacobian(
+          "d_r_d_p", J_p, [&](const Vec4 &x) { return CamT::project(p + x); },
+          Vec4::Zero());
+    }
+  }
+}
+
+template <typename CamT>
+void testStereographicProjectUnproject() {
+  using Vec2 = typename CamT::Vec2;
+  using Vec4 = typename CamT::Vec4;
+
+  for (int x = -10; x <= 10; x++) {
+    for (int y = -10; y <= 10; y++) {
+      Vec4 p(x, y, 5, 0.23424);
+
+      Vec4 p_normalized = Vec4::Zero();
+      p_normalized.template head<3>() = p.template head<3>().normalized();
+      Vec2 res = CamT::project(p);
+      Vec4 p_uproj = CamT::unproject(res);
+
+      ASSERT_TRUE(p_normalized.isApprox(p_uproj))
+          << "p_normalized " << p_normalized.transpose() << " p_uproj "
+          << p_uproj.transpose();
+    }
+  }
+}
+
+template <typename CamT>
+void testStereographicUnprojectJacobian() {
+  using Vec2 = typename CamT::Vec2;
+  using Vec4 = typename CamT::Vec4;
+
+  using Mat42 = typename CamT::Mat42;
+
+  for (int x = -10; x <= 10; x++) {
+    for (int y = -10; y <= 10; y++) {
+      Vec4 p_3d(x, y, 5, 0.23424);
+
+      Vec2 p = CamT::project(p_3d);
+
+      Mat42 J_p;
+
+      Vec4 res1 = CamT::unproject(p, &J_p);
+      Vec4 res2 = CamT::unproject(p);
+
+      ASSERT_TRUE(res1.isApprox(res2))
+          << "res1 " << res1.transpose() << " res2 " << res2.transpose();
+
+      test_jacobian(
+          "d_r_d_p", J_p, [&](const Vec2 &x) { return CamT::unproject(p + x); },
+          Vec2::Zero());
+    }
+  }
+}
+
+TEST(CameraTestCase, StereographicParamProjectJacobians) {
+  testStereographicProjectJacobian<basalt::StereographicParam<double>>();
+}
+TEST(CameraTestCase, StereographicParamProjectJacobiansFloat) {
+  testStereographicProjectJacobian<basalt::StereographicParam<float>>();
+}
+
+TEST(CameraTestCase, StereographicParamProjectUnproject) {
+  testStereographicProjectUnproject<basalt::StereographicParam<double>>();
+}
+TEST(CameraTestCase, StereographicParamProjectUnprojectFloat) {
+  testStereographicProjectUnproject<basalt::StereographicParam<float>>();
+}
+
+TEST(CameraTestCase, StereographicParamUnprojectJacobians) {
+  testStereographicUnprojectJacobian<basalt::StereographicParam<double>>();
+}
+TEST(CameraTestCase, StereographicParamUnprojectJacobiansFloat) {
+  testStereographicUnprojectJacobian<basalt::StereographicParam<float>>();
+}
+
+template <class Scalar, int N>
+void testEvalOrReference() {
+  using VecType = Eigen::Matrix<Scalar, N, 1>;
+  using MapType = Eigen::Map<Eigen::Matrix<Scalar, N, 1>>;
+  using MatType = Eigen::Matrix<Scalar, N, N>;
+
+  Scalar raw_array[N];
+  MapType mapped_p(raw_array);
+  VecType p1, p2;
+  MatType m1;
+  p1.setRandom();
+  p2.setRandom();
+  m1.setRandom();
+  mapped_p.setRandom();
+
+  // Non-evaluated sum
+  auto sum = p1 + p2;
+  typename basalt::EvalOrReference<decltype(sum)>::Type res1(sum);
+  static_assert(std::is_same_v<VecType, decltype(res1)>);
+
+  // Non-evaluated operations with matrix
+  auto affine = m1 * p1 + p2;
+  typename basalt::EvalOrReference<decltype(affine)>::Type res2(affine);
+  static_assert(std::is_same_v<VecType, decltype(res2)>);
+
+  // Vector: Should be reference. No data copy.
+  typename basalt::EvalOrReference<decltype(p1)>::Type res3(p1);
+  static_assert(std::is_same_v<const VecType &, decltype(res3)>);
+  ASSERT_EQ(&res3[0], &p1[0]);
+
+  // Map: Should be reference. No data copy.
+  typename basalt::EvalOrReference<decltype(mapped_p)>::Type res4(mapped_p);
+  static_assert(
+      std::is_same_v<const Eigen::MatrixBase<MapType> &, decltype(res4)>);
+  ASSERT_EQ(&res4[0], &mapped_p[0]);
+
+  // Map with standard Eigen eval: Copies data. Should not be used in the code.
+  typename Eigen::internal::eval<MapType>::type res5(mapped_p);
+  static_assert(std::is_same_v<VecType, decltype(res5)>);
+  ASSERT_NE(&res5[0], &mapped_p[0]);
+}
+
+TEST(CameraTestCase, EvalOrReferenceTypeCast) {
+  testEvalOrReference<double, 3>();
+  testEvalOrReference<double, 4>();
+  testEvalOrReference<float, 3>();
+  testEvalOrReference<float, 4>();
+}

thirdparty/basalt-headers/test/src/test_ceres_spline_helper.cpp

@@ -0,0 +1,496 @@
+
+
+#include <iostream>
+
+#include "gtest/gtest.h"
+#include "test_utils.h"
+
+#include <sophus/sim3.hpp>
+
+#include <basalt/spline/ceres_spline_helper.h>
+#include <basalt/spline/rd_spline.h>
+#include <basalt/spline/so3_spline.h>
+
+template <int N>
+void test_ceres_spline_helper_so3() {
+  static const int64_t dt_ns = 2e9;
+
+  basalt::So3Spline<N> spline(dt_ns);
+  spline.genRandomTrajectory(3 * N);
+
+  for (int64_t t_ns = 0; t_ns < spline.maxTimeNs(); t_ns += 1e8) {
+    Sophus::SO3d pos1 = spline.evaluate(t_ns);
+    Eigen::Vector3d vel1 = spline.velocityBody(t_ns);
+    Eigen::Vector3d accel1 = spline.accelerationBody(t_ns);
+    Eigen::Vector3d jerk1 = spline.jerkBody(t_ns);
+
+    Sophus::SO3d pos2;
+    Eigen::Vector3d vel2, accel2, jerk2;
+
+    {
+      double pow_inv_dt = 1e9 / dt_ns;
+
+      int64_t st_ns = (t_ns);
+
+      BASALT_ASSERT_STREAM(st_ns >= 0, "st_ns " << st_ns << " time_ns " << t_ns
+                                                << " start_t_ns " << 0);
+
+      int64_t s = st_ns / dt_ns;
+      double u = double(st_ns % dt_ns) / double(dt_ns);
+
+      BASALT_ASSERT_STREAM(s >= 0, "s " << s);
+      BASALT_ASSERT_STREAM(size_t(s + N) <= spline.getKnots().size(),
+                           "s " << s << " N " << N << " knots.size() "
+                                << spline.getKnots().size());
+
+      std::vector<const double *> vec;
+      for (int i = 0; i < N; i++) {
+        vec.emplace_back(spline.getKnots()[s + i].data());
+      }
+
+      basalt::CeresSplineHelper<N>::template evaluate_lie<double, Sophus::SO3>(
+          &vec[0], u, pow_inv_dt, &pos2);
+      basalt::CeresSplineHelper<N>::template evaluate_lie<double, Sophus::SO3>(
+          &vec[0], u, pow_inv_dt, nullptr, &vel2);
+      basalt::CeresSplineHelper<N>::template evaluate_lie<double, Sophus::SO3>(
+          &vec[0], u, pow_inv_dt, nullptr, nullptr, &accel2);
+      basalt::CeresSplineHelper<N>::template evaluate_lie<double, Sophus::SO3>(
+          &vec[0], u, pow_inv_dt, nullptr, nullptr, nullptr, &jerk2);
+    }
+
+    EXPECT_TRUE(pos1.matrix().isApprox(pos2.matrix()));
+    EXPECT_TRUE(vel1.isApprox(vel2));
+    EXPECT_TRUE(accel1.isApprox(accel2));
+    EXPECT_TRUE(jerk1.isApprox(jerk2));
+  }
+}
+
+template <int N>
+void test_ceres_spline_helper_rd() {
+  static const int DIM = 3;
+  static const int64_t dt_ns = 2e9;
+
+  basalt::RdSpline<DIM, N> spline(dt_ns);
+  spline.genRandomTrajectory(3 * N);
+
+  for (int64_t t_ns = 0; t_ns < spline.maxTimeNs(); t_ns += 1e8) {
+    Eigen::Vector3d pos1 = spline.evaluate<0>(t_ns);
+    Eigen::Vector3d vel1 = spline.evaluate<1>(t_ns);
+    Eigen::Vector3d accel1 = spline.evaluate<2>(t_ns);
+
+    Eigen::Vector3d pos2, vel2, accel2;
+
+    {
+      double pow_inv_dt = 1e9 / dt_ns;
+
+      int64_t st_ns = (t_ns);
+
+      BASALT_ASSERT_STREAM(st_ns >= 0, "st_ns " << st_ns << " time_ns " << t_ns
+                                                << " start_t_ns " << 0);
+
+      int64_t s = st_ns / dt_ns;
+      double u = double(st_ns % dt_ns) / double(dt_ns);
+
+      BASALT_ASSERT_STREAM(s >= 0, "s " << s);
+      BASALT_ASSERT_STREAM(size_t(s + N) <= spline.getKnots().size(),
+                           "s " << s << " N " << N << " knots.size() "
+                                << spline.getKnots().size());
+
+      std::vector<const double *> vec;
+      for (int i = 0; i < N; i++) {
+        vec.emplace_back(spline.getKnots()[s + i].data());
+      }
+
+      basalt::CeresSplineHelper<N>::template evaluate<double, DIM, 0>(
+          &vec[0], u, pow_inv_dt, &pos2);
+      basalt::CeresSplineHelper<N>::template evaluate<double, DIM, 1>(
+          &vec[0], u, pow_inv_dt, &vel2);
+      basalt::CeresSplineHelper<N>::template evaluate<double, DIM, 2>(
+          &vec[0], u, pow_inv_dt, &accel2);
+    }
+
+    EXPECT_TRUE(pos1.isApprox(pos2));
+    EXPECT_TRUE(vel1.isApprox(vel2));
+    EXPECT_TRUE(accel1.isApprox(accel2));
+  }
+}
+
+template <int N>
+void test_ceres_spline_helper_vel_se3() {
+  static const int64_t dt_ns = 2e9;
+
+  Eigen::aligned_vector<Sophus::SE3d> knots;
+
+  for (int i = 0; i < 3 * N; i++) {
+    knots.emplace_back(Sophus::SE3d::exp(Sophus::Vector6d::Random()));
+  }
+
+  for (int i = 0; i < 2 * N; i++)
+    for (double u = 0.01; u < 0.99; u += 0.01) {
+      Sophus::Vector6d vel;
+      Sophus::SE3d pose;
+
+      {
+        double pow_inv_dt = 1e9 / dt_ns;
+
+        std::vector<const double *> vec;
+        for (int j = 0; j < N; j++) {
+          vec.emplace_back(knots[i + j].data());
+        }
+
+        basalt::CeresSplineHelper<N>::template evaluate_lie<double,
+                                                            Sophus::SE3>(
+            &vec[0], u, pow_inv_dt, &pose, &vel);
+
+        Eigen::Matrix<double, 1, 1> x0;
+        x0.setZero();
+
+        test_jacobian(
+            "ceres_se3_vel", vel,
+            [&](const Eigen::Matrix<double, 1, 1> &x) {
+              Sophus::SE3d pose_new;
+
+              double inc = x[0] / (dt_ns * 1e-9);
+
+              basalt::CeresSplineHelper<N>::template evaluate_lie<double,
+                                                                  Sophus::SE3>(
+                  &vec[0], u + inc, pow_inv_dt, &pose_new);
+
+              return (pose.inverse() * pose_new).log();
+            },
+            x0);
+      }
+    }
+}
+
+template <int N>
+void test_ceres_spline_helper_accel_se3() {
+  static const int64_t dt_ns = 2e9;
+
+  Eigen::aligned_vector<Sophus::SE3d> knots;
+
+  for (int i = 0; i < 3 * N; i++) {
+    knots.emplace_back(Sophus::SE3d::exp(Sophus::Vector6d::Random()));
+  }
+
+  for (int i = 0; i < 2 * N; i++)
+    for (double u = 0.01; u < 0.99; u += 0.01) {
+      Sophus::Vector6d accel;
+
+      {
+        double pow_inv_dt = 1e9 / dt_ns;
+
+        std::vector<const double *> vec;
+        for (int j = 0; j < N; j++) {
+          vec.emplace_back(knots[i + j].data());
+        }
+
+        basalt::CeresSplineHelper<N>::template evaluate_lie<double,
+                                                            Sophus::SE3>(
+            &vec[0], u, pow_inv_dt, nullptr, nullptr, &accel);
+
+        Eigen::Matrix<double, 1, 1> x0;
+        x0.setZero();
+
+        test_jacobian(
+            "ceres_se3_accel", accel,
+            [&](const Eigen::Matrix<double, 1, 1> &x) {
+              Sophus::Vector6d vel;
+
+              double inc = x[0] / (dt_ns * 1e-9);
+
+              basalt::CeresSplineHelper<N>::template evaluate_lie<double,
+                                                                  Sophus::SE3>(
+                  &vec[0], u + inc, pow_inv_dt, nullptr, &vel);
+
+              return vel;
+            },
+            x0);
+      }
+    }
+}
+
+template <int N>
+void test_ceres_spline_helper_jerk_se3() {
+  static const int64_t dt_ns = 2e9;
+
+  Eigen::aligned_vector<Sophus::SE3d> knots;
+
+  for (int i = 0; i < 3 * N; i++) {
+    knots.emplace_back(Sophus::SE3d::exp(Sophus::Vector6d::Random()));
+  }
+
+  for (int i = 0; i < 2 * N; i++)
+    for (double u = 0.01; u < 0.99; u += 0.01) {
+      Sophus::Vector6d jerk;
+
+      {
+        double pow_inv_dt = 1e9 / dt_ns;
+
+        std::vector<const double *> vec;
+        for (int j = 0; j < N; j++) {
+          vec.emplace_back(knots[i + j].data());
+        }
+
+        basalt::CeresSplineHelper<N>::template evaluate_lie<double,
+                                                            Sophus::SE3>(
+            &vec[0], u, pow_inv_dt, nullptr, nullptr, nullptr, &jerk);
+
+        Eigen::Matrix<double, 1, 1> x0;
+        x0.setZero();
+
+        test_jacobian(
+            "ceres_se3_jerk", jerk,
+            [&](const Eigen::Matrix<double, 1, 1> &x) {
+              Sophus::Vector6d accel;
+
+              double inc = x[0] / (dt_ns * 1e-9);
+
+              basalt::CeresSplineHelper<N>::template evaluate_lie<double,
+                                                                  Sophus::SE3>(
+                  &vec[0], u + inc, pow_inv_dt, nullptr, nullptr, &accel);
+
+              return accel;
+            },
+            x0);
+      }
+    }
+}
+
+template <int N>
+void test_ceres_spline_helper_vel_sim3() {
+  static const int64_t dt_ns = 2e9;
+
+  Eigen::aligned_vector<Sophus::Sim3d> knots;
+
+  for (int i = 0; i < 3 * N; i++) {
+    knots.emplace_back(Sophus::Sim3d::exp(Sophus::Vector7d::Random()));
+  }
+
+  for (int i = 0; i < 2 * N; i++)
+    for (double u = 0.01; u < 0.99; u += 0.01) {
+      Sophus::Vector7d vel;
+      Sophus::Sim3d pose;
+
+      {
+        double pow_inv_dt = 1e9 / dt_ns;
+
+        std::vector<const double *> vec;
+        for (int j = 0; j < N; j++) {
+          vec.emplace_back(knots[i + j].data());
+        }
+
+        basalt::CeresSplineHelper<N>::template evaluate_lie<double,
+                                                            Sophus::Sim3>(
+            &vec[0], u, pow_inv_dt, &pose, &vel);
+
+        Eigen::Matrix<double, 1, 1> x0;
+        x0.setZero();
+
+        test_jacobian(
+            "ceres_sim3_vel", vel,
+            [&](const Eigen::Matrix<double, 1, 1> &x) {
+              Sophus::Sim3d pose_new;
+
+              double inc = x[0] / (dt_ns * 1e-9);
+
+              basalt::CeresSplineHelper<N>::template evaluate_lie<double,
+                                                                  Sophus::Sim3>(
+                  &vec[0], u + inc, pow_inv_dt, &pose_new);
+
+              return (pose.inverse() * pose_new).log();
+            },
+            x0);
+      }
+    }
+}
+
+template <int N>
+void test_ceres_spline_helper_accel_sim3() {
+  static const int64_t dt_ns = 2e9;
+
+  Eigen::aligned_vector<Sophus::Sim3d> knots;
+
+  for (int i = 0; i < 3 * N; i++) {
+    knots.emplace_back(Sophus::Sim3d::exp(Sophus::Vector7d::Random()));
+  }
+
+  for (int i = 0; i < 2 * N; i++)
+    for (double u = 0.01; u < 0.99; u += 0.01) {
+      Sophus::Vector7d accel;
+
+      {
+        double pow_inv_dt = 1e9 / dt_ns;
+
+        std::vector<const double *> vec;
+        for (int j = 0; j < N; j++) {
+          vec.emplace_back(knots[i + j].data());
+        }
+
+        basalt::CeresSplineHelper<N>::template evaluate_lie<double,
+                                                            Sophus::Sim3>(
+            &vec[0], u, pow_inv_dt, nullptr, nullptr, &accel);
+
+        Eigen::Matrix<double, 1, 1> x0;
+        x0.setZero();
+
+        test_jacobian(
+            "ceres_sim3_accel", accel,
+            [&](const Eigen::Matrix<double, 1, 1> &x) {
+              Sophus::Vector7d vel;
+
+              double inc = x[0] / (dt_ns * 1e-9);
+
+              basalt::CeresSplineHelper<N>::template evaluate_lie<double,
+                                                                  Sophus::Sim3>(
+                  &vec[0], u + inc, pow_inv_dt, nullptr, &vel);
+
+              return vel;
+            },
+            x0);
+      }
+    }
+}
+
+template <int N>
+void test_ceres_spline_helper_jerk_sim3() {
+  static const int64_t dt_ns = 2e9;
+
+  Eigen::aligned_vector<Sophus::Sim3d> knots;
+
+  for (int i = 0; i < 3 * N; i++) {
+    knots.emplace_back(Sophus::Sim3d::exp(Sophus::Vector7d::Random()));
+  }
+
+  for (int i = 0; i < 2 * N; i++)
+    for (double u = 0.01; u < 0.99; u += 0.01) {
+      Sophus::Vector7d jerk;
+
+      {
+        double pow_inv_dt = 1e9 / dt_ns;
+
+        std::vector<const double *> vec;
+        for (int j = 0; j < N; j++) {
+          vec.emplace_back(knots[i + j].data());
+        }
+
+        basalt::CeresSplineHelper<N>::template evaluate_lie<double,
+                                                            Sophus::Sim3>(
+            &vec[0], u, pow_inv_dt, nullptr, nullptr, nullptr, &jerk);
+
+        Eigen::Matrix<double, 1, 1> x0;
+        x0.setZero();
+
+        test_jacobian(
+            "ceres_sim3_jerk", jerk,
+            [&](const Eigen::Matrix<double, 1, 1> &x) {
+              Sophus::Vector7d accel;
+
+              double inc = x[0] / (dt_ns * 1e-9);
+
+              basalt::CeresSplineHelper<N>::template evaluate_lie<double,
+                                                                  Sophus::Sim3>(
+                  &vec[0], u + inc, pow_inv_dt, nullptr, nullptr, &accel);
+
+              return accel;
+            },
+            x0);
+      }
+    }
+}
+
+TEST(CeresSplineTestSuite, CeresSplineHelperSO3_4) {
+  test_ceres_spline_helper_so3<4>();
+}
+
+TEST(CeresSplineTestSuite, CeresSplineHelperSO3_5) {
+  test_ceres_spline_helper_so3<5>();
+}
+
+TEST(CeresSplineTestSuite, CeresSplineHelperSO3_6) {
+  test_ceres_spline_helper_so3<6>();
+}
+
+TEST(CeresSplineTestSuite, CeresSplineHelperRd_4) {
+  test_ceres_spline_helper_so3<4>();
+}
+
+TEST(CeresSplineTestSuite, CeresSplineHelperRd_5) {
+  test_ceres_spline_helper_so3<5>();
+}
+
+TEST(CeresSplineTestSuite, CeresSplineHelperRd_6) {
+  test_ceres_spline_helper_so3<6>();
+}
+
+TEST(CeresSplineTestSuite, CeresSplineHelperSE3vel4) {
+  test_ceres_spline_helper_vel_se3<4>();
+}
+
+TEST(CeresSplineTestSuite, CeresSplineHelperSE3vel5) {
+  test_ceres_spline_helper_vel_se3<5>();
+}
+
+TEST(CeresSplineTestSuite, CeresSplineHelperSE3vel6) {
+  test_ceres_spline_helper_vel_se3<6>();
+}
+
+TEST(CeresSplineTestSuite, CeresSplineHelperSE3accel4) {
+  test_ceres_spline_helper_accel_se3<4>();
+}
+
+TEST(CeresSplineTestSuite, CeresSplineHelperSE3accel5) {
+  test_ceres_spline_helper_accel_se3<5>();
+}
+
+TEST(CeresSplineTestSuite, CeresSplineHelperSE3accel6) {
+  test_ceres_spline_helper_accel_se3<6>();
+}
+
+TEST(CeresSplineTestSuite, CeresSplineHelperSE3jerk4) {
+  test_ceres_spline_helper_jerk_se3<4>();
+}
+
+TEST(CeresSplineTestSuite, CeresSplineHelperSE3jerk5) {
+  test_ceres_spline_helper_jerk_se3<5>();
+}
+
+TEST(CeresSplineTestSuite, CeresSplineHelperSE3jerk6) {
+  test_ceres_spline_helper_jerk_se3<6>();
+}
+
+TEST(CeresSplineTestSuite, CeresSplineHelperSim3vel4) {
+  test_ceres_spline_helper_vel_sim3<4>();
+}
+
+TEST(CeresSplineTestSuite, CeresSplineHelperSim3vel5) {
+  test_ceres_spline_helper_vel_sim3<5>();
+}
+
+TEST(CeresSplineTestSuite, CeresSplineHelperSim3vel6) {
+  test_ceres_spline_helper_vel_sim3<6>();
+}
+
+TEST(CeresSplineTestSuite, CeresSplineHelperSim3accel4) {
+  test_ceres_spline_helper_accel_sim3<4>();
+}
+
+TEST(CeresSplineTestSuite, CeresSplineHelperSim3accel5) {
+  test_ceres_spline_helper_accel_sim3<5>();
+}
+
+TEST(CeresSplineTestSuite, CeresSplineHelperSim3accel6) {
+  test_ceres_spline_helper_accel_sim3<6>();
+}
+
+TEST(CeresSplineTestSuite, CeresSplineHelperSim3jerk4) {
+  test_ceres_spline_helper_jerk_sim3<4>();
+}
+
+TEST(CeresSplineTestSuite, CeresSplineHelperSim3jerk5) {
+  test_ceres_spline_helper_jerk_sim3<5>();
+}
+
+TEST(CeresSplineTestSuite, CeresSplineHelperSim3jerk6) {
+  test_ceres_spline_helper_jerk_sim3<6>();
+}

thirdparty/basalt-headers/test/src/test_image.cpp

@@ -0,0 +1,105 @@
+
+#include <Eigen/Dense>
+
+#include <basalt/image/image.h>
+
+#include "gtest/gtest.h"
+#include "test_utils.h"
+
+void setImageData(uint16_t* image_array, int size) {
+  double norm = RAND_MAX;
+  norm /= (double)std::numeric_limits<uint16_t>::max();
+
+  for (int i = 0; i < size; i++) {
+    image_array[i] = (unsigned char)(rand() / norm);
+  }
+}
+
+TEST(Image, ImageInterpolate) {
+  Eigen::Vector2i offset(231, 123);
+
+  basalt::ManagedImage<uint16_t> img(640, 480);
+  setImageData(img.ptr, img.size());
+
+  double eps = 1e-12;
+  double threshold = 1e-7;
+
+  {
+    const Eigen::Vector2i& pi = offset;
+    Eigen::Vector2d pd = pi.cast<double>() + Eigen::Vector2d(eps, eps);
+
+    uint16_t val1 = img(pi);
+    double val2 = img.interp(pd);
+    double val3 = img.interpGrad(pd)[0];
+
+    EXPECT_LE(std::abs(val2 - val1), threshold);
+    EXPECT_FLOAT_EQ(val2, val3);
+  }
+
+  {
+    const Eigen::Vector2i& pi = offset;
+    Eigen::Vector2d pd = pi.cast<double>() + Eigen::Vector2d(eps, eps);
+
+    uint16_t val1 = img(pi);
+    double val2 = img.interp(pd);
+    double val3 = img.interpGrad(pd)[0];
+
+    EXPECT_LE(std::abs(val2 - val1), threshold);
+    EXPECT_FLOAT_EQ(val2, val3);
+  }
+
+  {
+    Eigen::Vector2i pi = offset + Eigen::Vector2i(1, 0);
+    Eigen::Vector2d pd = pi.cast<double>() + Eigen::Vector2d(-eps, eps);
+
+    uint16_t val1 = img(pi);
+    double val2 = img.interp(pd);
+    double val3 = img.interpGrad(pd)[0];
+
+    EXPECT_LE(std::abs(val2 - val1), threshold);
+    EXPECT_FLOAT_EQ(val2, val3);
+  }
+
+  {
+    Eigen::Vector2i pi = offset + Eigen::Vector2i(0, 1);
+    Eigen::Vector2d pd = pi.cast<double>() + Eigen::Vector2d(eps, -eps);
+
+    uint16_t val1 = img(pi);
+    double val2 = img.interp(pd);
+    double val3 = img.interpGrad(pd)[0];
+
+    EXPECT_LE(std::abs(val2 - val1), threshold);
+    EXPECT_FLOAT_EQ(val2, val3);
+  }
+
+  {
+    Eigen::Vector2i pi = offset + Eigen::Vector2i(1, 1);
+    Eigen::Vector2d pd = pi.cast<double>() + Eigen::Vector2d(-eps, -eps);
+
+    uint16_t val1 = img(pi);
+    double val2 = img.interp(pd);
+    double val3 = img.interpGrad(pd)[0];
+
+    EXPECT_LE(std::abs(val2 - val1), threshold);
+    EXPECT_FLOAT_EQ(val2, val3);
+  }
+}
+
+TEST(Image, ImageInterpolateGrad) {
+  Eigen::Vector2i offset(231, 123);
+
+  basalt::ManagedImage<uint16_t> img(640, 480);
+  setImageData(img.ptr, img.size());
+
+  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);
+}

thirdparty/basalt-headers/test/src/test_preintegration.cpp

@@ -0,0 +1,755 @@
+/**
+BSD 3-Clause License
+
+Copyright (c) 2019, Vladyslav Usenko and Nikolaus Demmel.
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+
+* Redistributions of source code must retain the above copyright notice, this
+  list of conditions and the following disclaimer.
+
+* Redistributions in binary form must reproduce the above copyright notice,
+  this list of conditions and the following disclaimer in the documentation
+  and/or other materials provided with the distribution.
+
+* Neither the name of the copyright holder nor the names of its
+  contributors may be used to endorse or promote products derived from
+  this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
+FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
+OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#include <basalt/imu/preintegration.h>
+#include <basalt/spline/se3_spline.h>
+
+#include <iostream>
+
+#include "gtest/gtest.h"
+#include "test_utils.h"
+
+namespace basalt::constants {
+static const Eigen::Vector3d G(0, 0, -9.81);
+}  // namespace basalt::constants
+
+static const double ACCEL_STD_DEV = 0.23;
+static const double GYRO_STD_DEV = 0.0027;
+
+// Smaller noise for testing
+// static const double accel_std_dev = 0.00023;
+// static const double gyro_std_dev = 0.0000027;
+
+std::random_device rd{};
+std::mt19937 gen{rd()};
+
+std::normal_distribution<> gyro_noise_dist{0, GYRO_STD_DEV};
+std::normal_distribution<> accel_noise_dist{0, ACCEL_STD_DEV};
+
+TEST(ImuPreintegrationTestCase, PredictTestGT) {
+  int num_knots = 15;
+
+  basalt::IntegratedImuMeasurement<double> imu_meas(0, Eigen::Vector3d::Zero(),
+                                                    Eigen::Vector3d::Zero());
+
+  basalt::Se3Spline<5> gt_spline(int64_t(10e9));
+  gt_spline.genRandomTrajectory(num_knots);
+
+  basalt::PoseVelState<double> state0;
+  basalt::PoseVelState<double> state1;
+  basalt::PoseVelState<double> state1_gt;
+
+  state0.T_w_i = gt_spline.pose(int64_t(0));
+  state0.vel_w_i = gt_spline.transVelWorld(int64_t(0));
+
+  int64_t dt_ns = 1e7;
+  for (int64_t t_ns = dt_ns / 2;
+       t_ns < int64_t(20e9);  //  gt_spline.maxTimeNs() - int64_t(1e9);
+       t_ns += dt_ns) {
+    Sophus::SE3d pose = gt_spline.pose(t_ns);
+    Eigen::Vector3d accel_body =
+        pose.so3().inverse() *
+        (gt_spline.transAccelWorld(t_ns) - basalt::constants::G);
+    Eigen::Vector3d rot_vel_body = gt_spline.rotVelBody(t_ns);
+
+    basalt::ImuData<double> data;
+    data.accel = accel_body;
+    data.gyro = rot_vel_body;
+    data.t_ns = t_ns + dt_ns / 2;  // measurement in the middle of the interval;
+
+    imu_meas.integrate(data, Eigen::Vector3d::Ones(), Eigen::Vector3d::Ones());
+  }
+
+  state1_gt.T_w_i = gt_spline.pose(imu_meas.get_dt_ns());
+  state1_gt.vel_w_i = gt_spline.transVelWorld(imu_meas.get_dt_ns());
+
+  imu_meas.predictState(state0, basalt::constants::G, state1);
+
+  EXPECT_TRUE(state1_gt.vel_w_i.isApprox(state1.vel_w_i, 1e-4))
+      << "vel1_gt " << state1_gt.vel_w_i.transpose() << " vel1 "
+      << state1.vel_w_i.transpose();
+
+  EXPECT_LE(state1_gt.T_w_i.unit_quaternion().angularDistance(
+                state1.T_w_i.unit_quaternion()),
+            1e-6);
+
+  EXPECT_TRUE(
+      state1_gt.T_w_i.translation().isApprox(state1.T_w_i.translation(), 1e-4))
+      << "pose1_gt p " << state1_gt.T_w_i.translation().transpose()
+      << " pose1 p " << state1.T_w_i.translation().transpose();
+}
+
+TEST(ImuPreintegrationTestCase, PredictTest) {
+  int num_knots = 15;
+
+  basalt::Se3Spline<5> gt_spline(int64_t(2e9));
+  gt_spline.genRandomTrajectory(num_knots);
+
+  int64_t dt_ns = 1e7;
+  for (int64_t t_ns = dt_ns / 2; t_ns < gt_spline.maxTimeNs() - int64_t(1e9);
+       t_ns += dt_ns) {
+    Sophus::SE3d pose = gt_spline.pose(t_ns);
+    Eigen::Vector3d accel_body =
+        pose.so3().inverse() *
+        (gt_spline.transAccelWorld(t_ns) - basalt::constants::G);
+    Eigen::Vector3d rot_vel_body = gt_spline.rotVelBody(t_ns);
+
+    basalt::ImuData<double> data;
+    data.accel = accel_body;
+    data.gyro = rot_vel_body;
+    data.t_ns = t_ns + dt_ns / 2;  // measurement in the middle of the interval;
+
+    basalt::PoseVelState<double> next_state;
+
+    int64_t curr_state_t_ns = t_ns - dt_ns / 2;
+    basalt::PoseVelState<double> curr_state(
+        curr_state_t_ns, gt_spline.pose(curr_state_t_ns),
+        gt_spline.transVelWorld(curr_state_t_ns));
+
+    basalt::IntegratedImuMeasurement<double>::MatNN d_next_d_curr;
+    basalt::IntegratedImuMeasurement<double>::MatN3 d_next_d_accel;
+    basalt::IntegratedImuMeasurement<double>::MatN3 d_next_d_gyro;
+
+    basalt::IntegratedImuMeasurement<double>::propagateState(
+        curr_state, data, next_state, &d_next_d_curr, &d_next_d_accel,
+        &d_next_d_gyro);
+
+    {
+      basalt::PoseVelState<double>::VecN x0;
+      x0.setZero();
+      test_jacobian(
+          "F_TEST", d_next_d_curr,
+          [&](const basalt::PoseVelState<double>::VecN& x) {
+            basalt::PoseVelState<double> curr_state1 = curr_state;
+            curr_state1.applyInc(x);
+            basalt::PoseVelState<double> next_state1;
+
+            basalt::IntegratedImuMeasurement<double>::propagateState(
+                curr_state1, data, next_state1);
+
+            return next_state.diff(next_state1);
+          },
+          x0);
+    }
+
+    {
+      Eigen::Vector3d x0;
+      x0.setZero();
+      test_jacobian(
+          "A_TEST", d_next_d_accel,
+          [&](const Eigen::Vector3d& x) {
+            basalt::ImuData<double> data1 = data;
+            data1.accel += x;
+            basalt::PoseVelState<double> next_state1;
+
+            basalt::IntegratedImuMeasurement<double>::propagateState(
+                curr_state, data1, next_state1);
+
+            return next_state.diff(next_state1);
+          },
+          x0);
+    }
+
+    {
+      Eigen::Vector3d x0;
+      x0.setZero();
+      test_jacobian(
+          "G_TEST", d_next_d_gyro,
+          [&](const Eigen::Vector3d& x) {
+            basalt::ImuData<double> data1 = data;
+            data1.gyro += x;
+            basalt::PoseVelState<double> next_state1;
+
+            basalt::IntegratedImuMeasurement<double>::propagateState(
+                curr_state, data1, next_state1);
+
+            return next_state.diff(next_state1);
+          },
+          x0, 1e-8);
+    }
+  }
+}
+
+TEST(ImuPreintegrationTestCase, ResidualTest) {
+  int num_knots = 15;
+
+  Eigen::Vector3d bg;
+  Eigen::Vector3d ba;
+  bg = Eigen::Vector3d::Random() / 100;
+  ba = Eigen::Vector3d::Random() / 10;
+
+  basalt::IntegratedImuMeasurement<double> imu_meas(0, bg, ba);
+
+  basalt::Se3Spline<5> gt_spline(int64_t(10e9));
+  gt_spline.genRandomTrajectory(num_knots);
+
+  basalt::PoseVelState<double> state0;
+  basalt::PoseVelState<double> state1;
+  basalt::PoseVelState<double> state1_gt;
+
+  state0.T_w_i = gt_spline.pose(int64_t(0));
+  state0.vel_w_i = gt_spline.transVelWorld(int64_t(0));
+
+  int64_t dt_ns = 1e7;
+  for (int64_t t_ns = dt_ns / 2;
+       t_ns < int64_t(1e8);  //  gt_spline.maxTimeNs() - int64_t(1e9);
+       t_ns += dt_ns) {
+    Sophus::SE3d pose = gt_spline.pose(t_ns);
+    Eigen::Vector3d accel_body =
+        pose.so3().inverse() *
+        (gt_spline.transAccelWorld(t_ns) - basalt::constants::G);
+    Eigen::Vector3d rot_vel_body = gt_spline.rotVelBody(t_ns);
+
+    basalt::ImuData<double> data;
+    data.accel = accel_body + ba;
+    data.gyro = rot_vel_body + bg;
+    data.t_ns = t_ns + dt_ns / 2;  // measurement in the middle of the interval;
+
+    imu_meas.integrate(data, Eigen::Vector3d::Ones(), Eigen::Vector3d::Ones());
+  }
+
+  state1_gt.T_w_i = gt_spline.pose(imu_meas.get_dt_ns());
+  state1_gt.vel_w_i = gt_spline.transVelWorld(imu_meas.get_dt_ns());
+
+  basalt::PoseVelState<double>::VecN res_gt =
+      imu_meas.residual(state0, basalt::constants::G, state1_gt, bg, ba);
+
+  EXPECT_LE(res_gt.array().abs().maxCoeff(), 1e-6)
+      << "res_gt " << res_gt.transpose();
+
+  state1.T_w_i = gt_spline.pose(imu_meas.get_dt_ns()) *
+                 Sophus::se3_expd(Sophus::Vector6d::Random() / 10);
+  state1.vel_w_i = gt_spline.transVelWorld(imu_meas.get_dt_ns()) +
+                   Sophus::Vector3d::Random() / 10;
+
+  basalt::IntegratedImuMeasurement<double>::MatNN d_res_d_state0;
+  basalt::IntegratedImuMeasurement<double>::MatNN d_res_d_state1;
+  basalt::IntegratedImuMeasurement<double>::MatN3 d_res_d_bg;
+  basalt::IntegratedImuMeasurement<double>::MatN3 d_res_d_ba;
+
+  imu_meas.residual(state0, basalt::constants::G, state1, bg, ba,
+                    &d_res_d_state0, &d_res_d_state1, &d_res_d_bg, &d_res_d_ba);
+
+  {
+    basalt::PoseVelState<double>::VecN x0;
+    x0.setZero();
+    test_jacobian(
+        "d_res_d_state0", d_res_d_state0,
+        [&](const basalt::PoseVelState<double>::VecN& x) {
+          basalt::PoseVelState<double> state0_new = state0;
+          state0_new.applyInc(x);
+
+          return imu_meas.residual(state0_new, basalt::constants::G, state1, bg,
+                                   ba);
+        },
+        x0);
+  }
+
+  {
+    basalt::PoseVelState<double>::VecN x0;
+    x0.setZero();
+    test_jacobian(
+        "d_res_d_state1", d_res_d_state1,
+        [&](const basalt::PoseVelState<double>::VecN& x) {
+          basalt::PoseVelState<double> state1_new = state1;
+          state1_new.applyInc(x);
+
+          return imu_meas.residual(state0, basalt::constants::G, state1_new, bg,
+                                   ba);
+        },
+        x0);
+  }
+
+  {
+    Sophus::Vector3d x0;
+    x0.setZero();
+    test_jacobian(
+        "d_res_d_bg", d_res_d_bg,
+        [&](const Sophus::Vector3d& x) {
+          return imu_meas.residual(state0, basalt::constants::G, state1, bg + x,
+                                   ba);
+        },
+        x0);
+  }
+
+  {
+    Sophus::Vector3d x0;
+    x0.setZero();
+    test_jacobian(
+        "d_res_d_ba", d_res_d_ba,
+        [&](const Sophus::Vector3d& x) {
+          return imu_meas.residual(state0, basalt::constants::G, state1, bg,
+                                   ba + x);
+        },
+        x0);
+  }
+}
+
+TEST(ImuPreintegrationTestCase, BiasTest) {
+  int num_knots = 15;
+
+  Eigen::Vector3d bg;
+  Eigen::Vector3d ba;
+  bg = Eigen::Vector3d::Random() / 100;
+  ba = Eigen::Vector3d::Random() / 10;
+
+  basalt::Se3Spline<5> gt_spline(int64_t(10e9));
+  gt_spline.genRandomTrajectory(num_knots);
+
+  Eigen::aligned_vector<Eigen::Vector3d> accel_data_vec;
+  Eigen::aligned_vector<Eigen::Vector3d> gyro_data_vec;
+  Eigen::aligned_vector<int64_t> timestamps_vec;
+
+  int64_t dt_ns = 1e7;
+  for (int64_t t_ns = dt_ns / 2;
+       t_ns < int64_t(1e9);  //  gt_spline.maxTimeNs() - int64_t(1e9);
+       t_ns += dt_ns) {
+    Sophus::SE3d pose = gt_spline.pose(t_ns);
+    Eigen::Vector3d accel_body =
+        pose.so3().inverse() *
+        (gt_spline.transAccelWorld(t_ns) - basalt::constants::G);
+    Eigen::Vector3d rot_vel_body = gt_spline.rotVelBody(t_ns);
+
+    accel_data_vec.emplace_back(accel_body + ba);
+    gyro_data_vec.emplace_back(rot_vel_body + bg);
+    timestamps_vec.emplace_back(t_ns + dt_ns / 2);
+  }
+
+  basalt::IntegratedImuMeasurement<double> imu_meas(0, bg, ba);
+
+  for (size_t i = 0; i < timestamps_vec.size(); i++) {
+    basalt::ImuData<double> data;
+    data.accel = accel_data_vec[i];
+    data.gyro = gyro_data_vec[i];
+    data.t_ns = timestamps_vec[i];
+
+    imu_meas.integrate(data, Eigen::Vector3d::Ones(), Eigen::Vector3d::Ones());
+  }
+
+  basalt::IntegratedImuMeasurement<double>::MatN3 d_res_d_ba;
+  basalt::IntegratedImuMeasurement<double>::MatN3 d_res_d_bg;
+
+  basalt::PoseVelState<double> delta_state = imu_meas.getDeltaState();
+
+  {
+    Sophus::Vector3d x0;
+    x0.setZero();
+    test_jacobian(
+        "d_res_d_bg", imu_meas.get_d_state_d_bg(),
+        [&](const Sophus::Vector3d& x) {
+          basalt::IntegratedImuMeasurement<double> imu_meas1(0, bg + x, ba);
+
+          for (size_t i = 0; i < timestamps_vec.size(); i++) {
+            basalt::ImuData<double> data;
+            data.accel = accel_data_vec[i];
+            data.gyro = gyro_data_vec[i];
+            data.t_ns = timestamps_vec[i];
+
+            imu_meas1.integrate(data, Eigen::Vector3d::Ones(),
+                                Eigen::Vector3d::Ones());
+          }
+
+          basalt::PoseVelState<double> delta_state1 = imu_meas1.getDeltaState();
+          return delta_state.diff(delta_state1);
+        },
+        x0);
+  }
+
+  {
+    Sophus::Vector3d x0;
+    x0.setZero();
+    test_jacobian(
+        "d_res_d_ba", imu_meas.get_d_state_d_ba(),
+        [&](const Sophus::Vector3d& x) {
+          basalt::IntegratedImuMeasurement<double> imu_meas1(0, bg, ba + x);
+
+          for (size_t i = 0; i < timestamps_vec.size(); i++) {
+            basalt::ImuData<double> data;
+            data.accel = accel_data_vec[i];
+            data.gyro = gyro_data_vec[i];
+            data.t_ns = timestamps_vec[i];
+
+            imu_meas1.integrate(data, Eigen::Vector3d::Ones(),
+                                Eigen::Vector3d::Ones());
+          }
+
+          basalt::PoseVelState<double> delta_state1 = imu_meas1.getDeltaState();
+          return delta_state.diff(delta_state1);
+        },
+        x0);
+  }
+}
+
+TEST(ImuPreintegrationTestCase, ResidualBiasTest) {
+  int num_knots = 15;
+
+  Eigen::Vector3d bg;
+  Eigen::Vector3d ba;
+  bg = Eigen::Vector3d::Random() / 100;
+  ba = Eigen::Vector3d::Random() / 10;
+
+  basalt::Se3Spline<5> gt_spline(int64_t(10e9));
+  gt_spline.genRandomTrajectory(num_knots);
+
+  Eigen::aligned_vector<Eigen::Vector3d> accel_data_vec;
+  Eigen::aligned_vector<Eigen::Vector3d> gyro_data_vec;
+  Eigen::aligned_vector<int64_t> timestamps_vec;
+
+  int64_t dt_ns = 1e7;
+  for (int64_t t_ns = dt_ns / 2;
+       t_ns < int64_t(1e9);  //  gt_spline.maxTimeNs() - int64_t(1e9);
+       t_ns += dt_ns) {
+    Sophus::SE3d pose = gt_spline.pose(t_ns);
+    Eigen::Vector3d accel_body =
+        pose.so3().inverse() *
+        (gt_spline.transAccelWorld(t_ns) - basalt::constants::G);
+    Eigen::Vector3d rot_vel_body = gt_spline.rotVelBody(t_ns);
+
+    accel_data_vec.emplace_back(accel_body + ba);
+    gyro_data_vec.emplace_back(rot_vel_body + bg);
+    timestamps_vec.emplace_back(t_ns + dt_ns / 2);
+  }
+
+  basalt::IntegratedImuMeasurement<double> imu_meas(0, bg, ba);
+
+  for (size_t i = 0; i < timestamps_vec.size(); i++) {
+    basalt::ImuData<double> data;
+    data.accel = accel_data_vec[i];
+    data.gyro = gyro_data_vec[i];
+    data.t_ns = timestamps_vec[i];
+
+    imu_meas.integrate(data, Eigen::Vector3d::Ones(), Eigen::Vector3d::Ones());
+  }
+
+  basalt::PoseVelState<double> state0;
+  basalt::PoseVelState<double> state1;
+
+  state0.T_w_i = gt_spline.pose(int64_t(0));
+  state0.vel_w_i = gt_spline.transVelWorld(int64_t(0));
+
+  state1.T_w_i = gt_spline.pose(imu_meas.get_dt_ns()) *
+                 Sophus::se3_expd(Sophus::Vector6d::Random() / 10);
+  state1.vel_w_i = gt_spline.transVelWorld(imu_meas.get_dt_ns()) +
+                   Sophus::Vector3d::Random() / 10;
+
+  Eigen::Vector3d bg_test = bg + Eigen::Vector3d::Random() / 1000;
+  Eigen::Vector3d ba_test = ba + Eigen::Vector3d::Random() / 100;
+
+  basalt::IntegratedImuMeasurement<double>::MatN3 d_res_d_ba;
+  basalt::IntegratedImuMeasurement<double>::MatN3 d_res_d_bg;
+
+  basalt::PoseVelState<double>::VecN res =
+      imu_meas.residual(state0, basalt::constants::G, state1, bg_test, ba_test,
+                        nullptr, nullptr, &d_res_d_bg, &d_res_d_ba);
+
+  {
+    basalt::IntegratedImuMeasurement<double> imu_meas1(0, bg_test, ba_test);
+
+    for (size_t i = 0; i < timestamps_vec.size(); i++) {
+      basalt::ImuData<double> data;
+      data.accel = accel_data_vec[i];
+      data.gyro = gyro_data_vec[i];
+      data.t_ns = timestamps_vec[i];
+
+      imu_meas1.integrate(data, Eigen::Vector3d::Ones(),
+                          Eigen::Vector3d::Ones());
+    }
+
+    basalt::PoseVelState<double>::VecN res1 = imu_meas1.residual(
+        state0, basalt::constants::G, state1, bg_test, ba_test);
+
+    EXPECT_TRUE(res.isApprox(res1, 1e-4))
+        << "res\n"
+        << res.transpose() << "\nres1\n"
+        << res1.transpose() << "\ndiff\n"
+        << (res - res1).transpose() << std::endl;
+  }
+
+  {
+    Sophus::Vector3d x0;
+    x0.setZero();
+    test_jacobian(
+        "d_res_d_ba", d_res_d_ba,
+        [&](const Sophus::Vector3d& x) {
+          basalt::IntegratedImuMeasurement<double> imu_meas1(0, bg_test,
+                                                             ba_test + x);
+
+          for (size_t i = 0; i < timestamps_vec.size(); i++) {
+            basalt::ImuData<double> data;
+            data.accel = accel_data_vec[i];
+            data.gyro = gyro_data_vec[i];
+            data.t_ns = timestamps_vec[i];
+
+            imu_meas1.integrate(data, Eigen::Vector3d::Ones(),
+                                Eigen::Vector3d::Ones());
+          }
+
+          return imu_meas1.residual(state0, basalt::constants::G, state1,
+                                    bg_test, ba_test + x);
+        },
+        x0);
+  }
+
+  {
+    Sophus::Vector3d x0;
+    x0.setZero();
+    test_jacobian(
+        "d_res_d_bg", d_res_d_bg,
+        [&](const Sophus::Vector3d& x) {
+          basalt::IntegratedImuMeasurement<double> imu_meas1(0, bg_test + x,
+                                                             ba_test);
+
+          for (size_t i = 0; i < timestamps_vec.size(); i++) {
+            basalt::ImuData<double> data;
+            data.accel = accel_data_vec[i];
+            data.gyro = gyro_data_vec[i];
+            data.t_ns = timestamps_vec[i];
+
+            imu_meas1.integrate(data, Eigen::Vector3d::Ones(),
+                                Eigen::Vector3d::Ones());
+          }
+
+          return imu_meas1.residual(state0, basalt::constants::G, state1,
+                                    bg_test + x, ba_test);
+        },
+        x0, 1e-8, 1e-2);
+  }
+}
+
+TEST(ImuPreintegrationTestCase, CovarianceTest) {
+  int num_knots = 15;
+
+  basalt::Se3Spline<5> gt_spline(int64_t(10e9));
+  gt_spline.genRandomTrajectory(num_knots);
+
+  Eigen::aligned_vector<Eigen::Vector3d> accel_data_vec;
+  Eigen::aligned_vector<Eigen::Vector3d> gyro_data_vec;
+  Eigen::aligned_vector<int64_t> timestamps_vec;
+
+  int64_t dt_ns = 1e7;
+  for (int64_t t_ns = dt_ns / 2;
+       t_ns < int64_t(1e9);  //  gt_spline.maxTimeNs() - int64_t(1e9);
+       t_ns += dt_ns) {
+    Sophus::SE3d pose = gt_spline.pose(t_ns);
+    Eigen::Vector3d accel_body =
+        pose.so3().inverse() *
+        (gt_spline.transAccelWorld(t_ns) - basalt::constants::G);
+    Eigen::Vector3d rot_vel_body = gt_spline.rotVelBody(t_ns);
+
+    accel_data_vec.emplace_back(accel_body);
+    gyro_data_vec.emplace_back(rot_vel_body);
+    timestamps_vec.emplace_back(t_ns + dt_ns / 2);
+  }
+
+  Eigen::Vector3d accel_cov;
+  Eigen::Vector3d gyro_cov;
+  accel_cov.setConstant(ACCEL_STD_DEV * ACCEL_STD_DEV);
+  gyro_cov.setConstant(GYRO_STD_DEV * GYRO_STD_DEV);
+
+  basalt::IntegratedImuMeasurement<double> imu_meas(0, Eigen::Vector3d::Zero(),
+                                                    Eigen::Vector3d::Zero());
+
+  for (size_t i = 0; i < timestamps_vec.size(); i++) {
+    basalt::ImuData<double> data;
+    data.accel = accel_data_vec[i];
+    data.gyro = gyro_data_vec[i];
+    data.t_ns = timestamps_vec[i];
+
+    // std::cerr << "data.accel " << data.accel.transpose() << std::endl;
+
+    // std::cerr << "cov " << i << "\n" << imu_meas.get_cov() << std::endl;
+    imu_meas.integrate(data, accel_cov, gyro_cov);
+  }
+
+  // std::cerr << "cov\n" << imu_meas.get_cov() << std::endl;
+
+  basalt::PoseVelState<double> delta_state = imu_meas.getDeltaState();
+
+  basalt::IntegratedImuMeasurement<double>::MatNN cov_computed;
+  cov_computed.setZero();
+
+  const int num_samples = 1000;
+  for (int i = 0; i < num_samples; i++) {
+    basalt::IntegratedImuMeasurement<double> imu_meas1(
+        0, Eigen::Vector3d::Zero(), Eigen::Vector3d::Zero());
+
+    for (size_t i = 0; i < timestamps_vec.size(); i++) {
+      basalt::ImuData<double> data;
+      data.accel = accel_data_vec[i];
+      data.gyro = gyro_data_vec[i];
+      data.t_ns = timestamps_vec[i];
+
+      data.accel[0] += accel_noise_dist(gen);
+      data.accel[1] += accel_noise_dist(gen);
+      data.accel[2] += accel_noise_dist(gen);
+
+      data.gyro[0] += gyro_noise_dist(gen);
+      data.gyro[1] += gyro_noise_dist(gen);
+      data.gyro[2] += gyro_noise_dist(gen);
+
+      imu_meas1.integrate(data, accel_cov, gyro_cov);
+    }
+
+    basalt::PoseVelState<double> delta_state1 = imu_meas1.getDeltaState();
+
+    basalt::PoseVelState<double>::VecN diff = delta_state.diff(delta_state1);
+
+    cov_computed += diff * diff.transpose();
+  }
+
+  cov_computed /= num_samples;
+  // std::cerr << "cov_computed\n" << cov_computed << std::endl;
+
+  double kl =
+      (imu_meas.get_cov_inv() * cov_computed).trace() - 9 +
+      std::log(imu_meas.get_cov().determinant() / cov_computed.determinant());
+
+  // std::cerr << "kl " << kl << std::endl;
+  EXPECT_LE(kl, 0.08);
+
+  Eigen::VectorXd test_vec(num_samples);
+  for (int i = 0; i < num_samples; i++) {
+    test_vec[i] = accel_noise_dist(gen);
+  }
+
+  double mean = test_vec.mean();
+  double var = (test_vec.array() - mean).square().sum() / num_samples;
+
+  // Small test for rangdom generator
+  EXPECT_LE(std::abs(std::sqrt(var) - ACCEL_STD_DEV), 0.03);
+}
+
+TEST(ImuPreintegrationTestCase, RandomWalkTest) {
+  double dt = 0.005;
+
+  double period = 200;
+  double period_dt = period * dt;
+
+  int num_samples = 10000;
+
+  Eigen::VectorXd test_vec(num_samples);
+  for (int j = 0; j < num_samples; j++) {
+    double test = 0;
+    for (int i = 0; i < period; i++) {
+      test += gyro_noise_dist(gen) * std::sqrt(dt);
+    }
+    test_vec[j] = test;
+  }
+
+  double mean = test_vec.mean();
+  double var = (test_vec.array() - mean).square().sum() / num_samples;
+  double std = std::sqrt(var);
+
+  EXPECT_NEAR(GYRO_STD_DEV * std::sqrt(period_dt), std, 1e-4);
+  EXPECT_NEAR(GYRO_STD_DEV * GYRO_STD_DEV * period_dt, var, 1e-6);
+}
+
+TEST(ImuPreintegrationTestCase, ComputeCovInv) {
+  using MatNN = basalt::IntegratedImuMeasurement<double>::MatNN;
+
+  int num_knots = 15;
+
+  basalt::IntegratedImuMeasurement<double> imu_meas(0, Eigen::Vector3d::Zero(),
+                                                    Eigen::Vector3d::Zero());
+
+  basalt::Se3Spline<5> gt_spline(int64_t(10e9));
+  gt_spline.genRandomTrajectory(num_knots);
+
+  int64_t dt_ns = 1e7;
+  for (int64_t t_ns = dt_ns / 2;
+       t_ns < int64_t(20e9);  //  gt_spline.maxTimeNs() - int64_t(1e9);
+       t_ns += dt_ns) {
+    Sophus::SE3d pose = gt_spline.pose(t_ns);
+    Eigen::Vector3d accel_body =
+        pose.so3().inverse() *
+        (gt_spline.transAccelWorld(t_ns) - basalt::constants::G);
+    Eigen::Vector3d rot_vel_body = gt_spline.rotVelBody(t_ns);
+
+    basalt::ImuData<double> data;
+    data.accel = accel_body;
+    data.gyro = rot_vel_body;
+    data.t_ns = t_ns + dt_ns / 2;  // measurement in the middle of the interval;
+
+    imu_meas.integrate(data, 0.1 * Eigen::Vector3d::Ones(),
+                       0.01 * Eigen::Vector3d::Ones());
+  }
+
+  MatNN cov_inv_computed = imu_meas.get_cov_inv();
+  MatNN cov_inv_gt = imu_meas.get_cov().inverse();
+
+  EXPECT_TRUE(cov_inv_computed.isApprox(cov_inv_gt, 1e-12))
+      << "cov_inv_computed\n"
+      << cov_inv_computed << "\ncov_inv_gt\n"
+      << cov_inv_gt;
+}
+
+TEST(ImuPreintegrationTestCase, ComputeSqrtCovInv) {
+  using MatNN = basalt::IntegratedImuMeasurement<double>::MatNN;
+
+  int num_knots = 15;
+
+  basalt::IntegratedImuMeasurement<double> imu_meas(0, Eigen::Vector3d::Zero(),
+                                                    Eigen::Vector3d::Zero());
+
+  basalt::Se3Spline<5> gt_spline(int64_t(10e9));
+  gt_spline.genRandomTrajectory(num_knots);
+
+  int64_t dt_ns = 1e7;
+  for (int64_t t_ns = dt_ns / 2;
+       t_ns < int64_t(20e9);  //  gt_spline.maxTimeNs() - int64_t(1e9);
+       t_ns += dt_ns) {
+    Sophus::SE3d pose = gt_spline.pose(t_ns);
+    Eigen::Vector3d accel_body =
+        pose.so3().inverse() *
+        (gt_spline.transAccelWorld(t_ns) - basalt::constants::G);
+    Eigen::Vector3d rot_vel_body = gt_spline.rotVelBody(t_ns);
+
+    basalt::ImuData<double> data;
+    data.accel = accel_body;
+    data.gyro = rot_vel_body;
+    data.t_ns = t_ns + dt_ns / 2;  // measurement in the middle of the interval;
+
+    imu_meas.integrate(data, 0.1 * Eigen::Vector3d::Ones(),
+                       0.01 * Eigen::Vector3d::Ones());
+  }
+
+  MatNN sqrt_cov_inv_computed = imu_meas.get_sqrt_cov_inv();
+  MatNN cov_inv_computed =
+      sqrt_cov_inv_computed.transpose() * sqrt_cov_inv_computed;
+
+  MatNN cov_inv_gt = imu_meas.get_cov().inverse();
+
+  EXPECT_TRUE(cov_inv_computed.isApprox(cov_inv_gt, 1e-12))
+      << "cov_inv_computed\n"
+      << cov_inv_computed << "\ncov_inv_gt\n"
+      << cov_inv_gt;
+}

thirdparty/basalt-headers/test/src/test_sophus.cpp

@@ -0,0 +1,567 @@
+/**
+BSD 3-Clause License
+
+Copyright (c) 2019, Vladyslav Usenko and Nikolaus Demmel.
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+
+* Redistributions of source code must retain the above copyright notice, this
+  list of conditions and the following disclaimer.
+
+* Redistributions in binary form must reproduce the above copyright notice,
+  this list of conditions and the following disclaimer in the documentation
+  and/or other materials provided with the distribution.
+
+* Neither the name of the copyright holder nor the names of its
+  contributors may be used to endorse or promote products derived from
+  this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
+FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
+OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#include <basalt/utils/sophus_utils.hpp>
+
+#include <sophus/se2.hpp>
+#include <sophus/sim2.hpp>
+
+#include "gtest/gtest.h"
+#include "test_utils.h"
+
+TEST(SophusUtilsCase, RightJacobianSO3) {
+  Eigen::Vector3d phi;
+  phi.setRandom();
+
+  Eigen::Matrix3d J_a;
+  Sophus::rightJacobianSO3(phi, J_a);
+
+  Eigen::Vector3d x0;
+  x0.setZero();
+
+  test_jacobian(
+      "rightJacobianSO3", J_a,
+      [&](const Eigen::Vector3d &x) {
+        return (Sophus::SO3d::exp(phi).inverse() * Sophus::SO3d::exp(phi + x))
+            .log();
+      },
+      x0);
+}
+
+TEST(SophusUtilsCase, RightJacobianInvSO3) {
+  Eigen::Vector3d phi;
+  phi.setRandom();
+
+  Eigen::Matrix3d J_a;
+  Sophus::rightJacobianInvSO3(phi, J_a);
+
+  Eigen::Vector3d x0;
+  x0.setZero();
+
+  test_jacobian(
+      "rightJacobianInvSO3", J_a,
+      [&](const Eigen::Vector3d &x) {
+        return (Sophus::SO3d::exp(phi) * Sophus::SO3d::exp(x)).log();
+      },
+      x0);
+}
+
+TEST(SophusUtilsCase, LeftJacobianSO3) {
+  Eigen::Vector3d phi;
+  phi.setRandom();
+
+  Eigen::Matrix3d J_a;
+  Sophus::leftJacobianSO3(phi, J_a);
+
+  Eigen::Vector3d x0;
+  x0.setZero();
+
+  test_jacobian(
+      "leftJacobianSO3", J_a,
+      [&](const Eigen::Vector3d &x) {
+        return (Sophus::SO3d::exp(phi + x) * Sophus::SO3d::exp(phi).inverse())
+            .log();
+      },
+      x0);
+}
+
+TEST(SophusUtilsCase, LeftJacobianInvSO3) {
+  Eigen::Vector3d phi;
+  phi.setRandom();
+
+  Eigen::Matrix3d J_a;
+  Sophus::leftJacobianInvSO3(phi, J_a);
+
+  Eigen::Vector3d x0;
+  x0.setZero();
+
+  test_jacobian(
+      "leftJacobianInvSO3", J_a,
+      [&](const Eigen::Vector3d &x) {
+        return (Sophus::SO3d::exp(x) * Sophus::SO3d::exp(phi)).log();
+      },
+      x0);
+}
+
+TEST(SophusUtilsCase, RightJacobianSE3Decoupled) {
+  Sophus::Vector6d phi;
+  phi.setRandom();
+
+  Sophus::Matrix6d J_a;
+  Sophus::rightJacobianSE3Decoupled(phi, J_a);
+
+  Sophus::Vector6d x0;
+  x0.setZero();
+
+  test_jacobian(
+      "rightJacobianSE3Decoupled", J_a,
+      [&](const Sophus::Vector6d &x) {
+        return Sophus::se3_logd(Sophus::se3_expd(phi).inverse() *
+                                Sophus::se3_expd(phi + x));
+      },
+      x0);
+}
+
+TEST(SophusUtilsCase, RightJacobianInvSE3Decoupled) {
+  Sophus::Vector6d phi;
+  phi.setRandom();
+
+  Sophus::Matrix6d J_a;
+  Sophus::rightJacobianInvSE3Decoupled(phi, J_a);
+
+  Sophus::Vector6d x0;
+  x0.setZero();
+
+  test_jacobian(
+      "rightJacobianInvSE3Decoupled", J_a,
+      [&](const Sophus::Vector6d &x) {
+        return Sophus::se3_logd(Sophus::se3_expd(phi) * Sophus::se3_expd(x));
+      },
+      x0);
+}
+
+// Verify that the adjoint definition works equally for expd and logd.
+TEST(SophusUtilsCase, Adjoint) {
+  Sophus::SE3d pose = Sophus::SE3d::exp(Sophus::Vector6d::Random());
+
+  Sophus::Matrix6d J_a = pose.inverse().Adj();
+
+  Sophus::Vector6d x0;
+  x0.setZero();
+
+  test_jacobian(
+      "Adj", J_a,
+      [&](const Sophus::Vector6d &x) {
+        return Sophus::se3_logd(pose.inverse() * Sophus::se3_expd(x) * pose);
+      },
+      x0);
+}
+
+TEST(SophusUtilsCase, RotTestSO3) {
+  Eigen::Vector3d t1 = Eigen::Vector3d::Random();
+  Eigen::Vector3d t2 = Eigen::Vector3d::Random();
+
+  double k = 0.6234234;
+
+  Eigen::Matrix3d J_a;
+  J_a.setZero();
+
+  Sophus::rightJacobianSO3(k * t1, J_a);
+  J_a = -k * Sophus::SO3d::exp(k * t1).matrix() * Sophus::SO3d::hat(t2) * J_a;
+
+  Eigen::Vector3d x0;
+  x0.setZero();
+
+  test_jacobian(
+      "Rot Test", J_a,
+      [&](const Eigen::Vector3d &x) {
+        return Sophus::SO3d::exp(k * (t1 + x)) * t2;
+      },
+      x0);
+}
+
+// Jacobian of the SE3 right-increment w.r.t. the decoupled left-increment.
+TEST(SophusUtilsCase, incTest) {
+  Sophus::SE3d T_w_i = Sophus::SE3d::exp(Sophus::Vector6d::Random());
+
+  Sophus::Matrix6d J_a;
+  J_a.setZero();
+
+  Eigen::Matrix3d R_w_i = T_w_i.so3().inverse().matrix();
+  J_a.topLeftCorner<3, 3>() = R_w_i;
+  J_a.bottomRightCorner<3, 3>() = R_w_i;
+
+  Sophus::Vector6d x0;
+  x0.setZero();
+
+  test_jacobian(
+      "inc test", J_a,
+      [&](const Sophus::Vector6d &x) {
+        Sophus::SE3d pose1;
+        pose1.so3() = Sophus::SO3d::exp(x.tail<3>()) * T_w_i.so3();
+        pose1.translation() = T_w_i.translation() + x.head<3>();
+
+        return Sophus::se3_logd(T_w_i.inverse() * pose1);
+      },
+      x0);
+}
+
+// Jacobian of the SE3 left-increment w.r.t. the decoupled left-increment.
+TEST(SophusUtilsCase, incTest2) {
+  Sophus::SE3d pose = Sophus::SE3d::exp(Sophus::Vector6d::Random());
+
+  Sophus::Matrix6d J_a;
+  J_a.setIdentity();
+
+  J_a.topRightCorner<3, 3>() = Sophus::SO3d::hat(pose.translation());
+
+  Sophus::Vector6d x0;
+  x0.setZero();
+
+  test_jacobian(
+      "inc test", J_a,
+      [&](const Sophus::Vector6d &x) {
+        Sophus::SE3d pose1;
+        pose1.so3() = Sophus::SO3d::exp(x.tail<3>()) * pose.so3();
+        pose1.translation() = pose.translation() + x.head<3>();
+
+        return Sophus::se3_logd(pose1 * pose.inverse());
+      },
+      x0);
+}
+
+TEST(SophusUtilsCase, SO2Test) {
+  Sophus::Vector2d phi;
+  phi.setRandom();
+
+  // std::cout << "phi " << phi.transpose() << std::endl;
+
+  Sophus::Matrix<double, 2, 1> J_a;
+  J_a[0] = -phi[1];
+  J_a[1] = phi[0];
+
+  Eigen::Matrix<double, 1, 1> x0;
+  x0.setZero();
+
+  test_jacobian(
+      "inc test", J_a,
+      [&](const Eigen::Matrix<double, 1, 1> &x) {
+        return Sophus::SO2d::exp(x[0]) * phi;
+      },
+      x0);
+}
+
+TEST(SophusUtilsCase, Se3Test) {
+  Sophus::Vector2d phi;
+  phi.setRandom();
+
+  // std::cout << "phi " << phi.transpose() << std::endl;
+
+  Sophus::Matrix<double, 2, 3> J_a;
+  J_a.topLeftCorner<2, 2>().setIdentity();
+  J_a(0, 2) = -phi[1];
+  J_a(1, 2) = phi[0];
+
+  Eigen::Matrix<double, 3, 1> x0;
+  x0.setZero();
+
+  test_jacobian(
+      "inc test", J_a,
+      [&](const Eigen::Matrix<double, 3, 1> &x) {
+        return Sophus::SE2d::exp(x) * phi;
+      },
+      x0);
+}
+
+TEST(SophusUtilsCase, Sim2Test) {
+  Sophus::Vector2d phi;
+  phi.setRandom();
+
+  // std::cout << "phi " << phi.transpose() << std::endl;
+
+  Sophus::Matrix<double, 2, 4> J_a;
+  J_a.topLeftCorner<2, 2>().setIdentity();
+  J_a(0, 2) = -phi[1];
+  J_a(1, 2) = phi[0];
+  J_a(0, 3) = phi[0];
+  J_a(1, 3) = phi[1];
+
+  Eigen::Matrix<double, 4, 1> x0;
+  x0.setZero();
+
+  test_jacobian(
+      "inc test", J_a,
+      [&](const Eigen::Matrix<double, 4, 1> &x) {
+        return Sophus::Sim2d::exp(x) * phi;
+      },
+      x0);
+}
+
+TEST(SophusUtilsCase, RxSO2Test) {
+  Sophus::Vector2d phi;
+  phi.setRandom();
+
+  // std::cout << "phi " << phi.transpose() << std::endl;
+
+  Sophus::Matrix<double, 2, 2> J_a;
+  J_a(0, 0) = -phi[1];
+  J_a(1, 0) = phi[0];
+  J_a(0, 1) = phi[0];
+  J_a(1, 1) = phi[1];
+
+  Eigen::Matrix<double, 2, 1> x0;
+  x0.setZero();
+
+  test_jacobian(
+      "inc test", J_a,
+      [&](const Eigen::Matrix<double, 2, 1> &x) {
+        return Sophus::RxSO2d::exp(x) * phi;
+      },
+      x0);
+}
+
+TEST(SophusUtilsCase, RightJacobianSim3Decoupled) {
+  Sophus::Vector7d phi;
+  phi.setRandom();
+
+  Sophus::Matrix7d J_a;
+  Sophus::Matrix7d J_n;
+  Sophus::rightJacobianSim3Decoupled(phi, J_a);
+
+  Sophus::Vector7d x0;
+  x0.setZero();
+
+  test_jacobian(
+      "rightJacobianSim3Decoupled", J_a,
+      [&](const Sophus::Vector7d &x) {
+        return Sophus::sim3_logd(Sophus::sim3_expd(phi).inverse() *
+                                 Sophus::sim3_expd(phi + x));
+      },
+      x0);
+}
+
+TEST(SophusUtilsCase, RightJacobianInvSim3Decoupled) {
+  Sophus::Vector7d phi;
+  phi.setRandom();
+
+  Sophus::Matrix7d J_a;
+  Sophus::Matrix7d J_n;
+  Sophus::rightJacobianInvSim3Decoupled(phi, J_a);
+
+  Sophus::Vector7d x0;
+  x0.setZero();
+
+  test_jacobian(
+      "rightJacobianInvSim3Decoupled", J_a,
+      [&](const Sophus::Vector7d &x) {
+        return Sophus::sim3_logd(Sophus::sim3_expd(phi) * Sophus::sim3_expd(x));
+      },
+      x0);
+}
+
+// Verify adjoint definition holds for decoupled log/exp.
+TEST(SophusUtilsCase, AdjointSim3) {
+  Sophus::Sim3d pose = Sophus::Sim3d::exp(Sophus::Vector7d::Random());
+
+  Sophus::Matrix7d J_a = pose.inverse().Adj();
+
+  Sophus::Vector7d x0;
+  x0.setZero();
+
+  test_jacobian(
+      "AdjSim3", J_a,
+      [&](const Sophus::Vector7d &x) {
+        return Sophus::sim3_logd(pose.inverse() * Sophus::sim3_expd(x) * pose);
+      },
+      x0);
+}
+
+// Note: For the relative pose tests we test different measurement error
+// magnitudes. It is not sufficient to only test small errors. For example,
+// with 1e-4, the tests do also pass when approximating the rightJacobian with
+// identity.
+
+TEST(SophusUtilsCase, RelPoseTestRightIncSE3) {
+  Sophus::SE3d T_w_i = Sophus::SE3d::exp(Sophus::Vector6d::Random());
+  Sophus::SE3d T_w_j = Sophus::SE3d::exp(Sophus::Vector6d::Random());
+
+  for (const double meas_error : {1e0, 1e-1, 1e-2, 1e-4}) {
+    Sophus::SE3d T_ij_meas =
+        T_w_i.inverse() * T_w_j *
+        Sophus::SE3d::exp(Sophus::Vector6d::Random() * meas_error);
+
+    Sophus::SE3d T_j_i = T_w_j.inverse() * T_w_i;
+    Sophus::Vector6d res = Sophus::se3_logd(T_ij_meas * T_j_i);
+
+    Sophus::Matrix6d J_T_w_i;
+    Sophus::Matrix6d J_T_w_j;
+    Sophus::rightJacobianInvSE3Decoupled(res, J_T_w_i);
+    J_T_w_j = -J_T_w_i * T_j_i.inverse().Adj();
+
+    Sophus::Vector6d x0;
+    x0.setZero();
+
+    test_jacobian(
+        "d_res_d_T_w_i", J_T_w_i,
+        [&](const Sophus::Vector6d &x) {
+          Sophus::SE3d T_w_i_new = T_w_i * Sophus::se3_expd(x);
+
+          return Sophus::se3_logd(T_ij_meas * T_w_j.inverse() * T_w_i_new);
+        },
+        x0);
+
+    test_jacobian(
+        "d_res_d_T_w_j", J_T_w_j,
+        [&](const Sophus::Vector6d &x) {
+          Sophus::SE3d T_w_j_new = T_w_j * Sophus::se3_expd(x);
+
+          return Sophus::se3_logd(T_ij_meas * T_w_j_new.inverse() * T_w_i);
+        },
+        x0);
+  }
+}
+
+TEST(SophusUtilsCase, RelPoseTestLeftIncSE3) {
+  Sophus::SE3d T_w_i = Sophus::SE3d::exp(Sophus::Vector6d::Random());
+  Sophus::SE3d T_w_j = Sophus::SE3d::exp(Sophus::Vector6d::Random());
+
+  for (const double meas_error : {1e0, 1e-1, 1e-2, 1e-4}) {
+    Sophus::SE3d T_ij_meas =
+        T_w_i.inverse() * T_w_j *
+        Sophus::SE3d::exp(Sophus::Vector6d::Random() * meas_error);
+
+    Sophus::SE3d T_j_i = T_w_j.inverse() * T_w_i;
+    Sophus::Vector6d res = Sophus::se3_logd(T_ij_meas * T_j_i);
+
+    Sophus::Matrix6d J_T_w_i;
+    Sophus::Matrix6d J_T_w_j;
+    Sophus::rightJacobianInvSE3Decoupled(res, J_T_w_i);
+    J_T_w_i = J_T_w_i * T_w_i.inverse().Adj();
+    J_T_w_j = -J_T_w_i;
+
+    Sophus::Vector6d x0;
+    x0.setZero();
+
+    test_jacobian(
+        "d_res_d_T_w_i", J_T_w_i,
+        [&](const Sophus::Vector6d &x) {
+          Sophus::SE3d T_w_i_new = Sophus::se3_expd(x) * T_w_i;
+
+          return Sophus::se3_logd(T_ij_meas * T_w_j.inverse() * T_w_i_new);
+        },
+        x0);
+
+    test_jacobian(
+        "d_res_d_T_w_j", J_T_w_j,
+        [&](const Sophus::Vector6d &x) {
+          Sophus::SE3d T_w_j_new = Sophus::se3_expd(x) * T_w_j;
+
+          return Sophus::se3_logd(T_ij_meas * T_w_j_new.inverse() * T_w_i);
+        },
+        x0);
+  }
+}
+
+TEST(SophusUtilsCase, RelPoseTestDecoupledLeftIncSE3) {
+  Sophus::SE3d T_w_i = Sophus::SE3d::exp(Sophus::Vector6d::Random());
+  Sophus::SE3d T_w_j = Sophus::SE3d::exp(Sophus::Vector6d::Random());
+
+  for (const double meas_error : {1e0, 1e-1, 1e-2, 1e-4}) {
+    Sophus::SE3d T_ij_meas =
+        T_w_i.inverse() * T_w_j *
+        Sophus::SE3d::exp(Sophus::Vector6d::Random() * meas_error);
+
+    Sophus::SE3d T_j_i = T_w_j.inverse() * T_w_i;
+    Sophus::Vector6d res = Sophus::se3_logd(T_ij_meas * T_j_i);
+
+    Sophus::Matrix6d J_T_w_i;
+    Sophus::Matrix6d J_T_w_j;
+    Sophus::Matrix6d rr_i;
+    Sophus::Matrix6d rr_j;
+
+    Sophus::rightJacobianInvSE3Decoupled(res, J_T_w_i);
+    J_T_w_j = -J_T_w_i * T_j_i.inverse().Adj();
+
+    rr_i.setZero();
+    rr_i.topLeftCorner<3, 3>() = rr_i.bottomRightCorner<3, 3>() =
+        T_w_i.so3().inverse().matrix();
+
+    rr_j.setZero();
+    rr_j.topLeftCorner<3, 3>() = rr_j.bottomRightCorner<3, 3>() =
+        T_w_j.so3().inverse().matrix();
+
+    Sophus::Vector6d x0;
+    x0.setZero();
+
+    test_jacobian(
+        "d_res_d_T_w_i", J_T_w_i * rr_i,
+        [&](const Sophus::Vector6d &x) {
+          Sophus::SE3d T_w_i_new;
+          T_w_i_new.so3() = Sophus::SO3d::exp(x.tail<3>()) * T_w_i.so3();
+          T_w_i_new.translation() = T_w_i.translation() + x.head<3>();
+
+          return Sophus::se3_logd(T_ij_meas * T_w_j.inverse() * T_w_i_new);
+        },
+        x0);
+
+    test_jacobian(
+        "d_res_d_T_w_j", J_T_w_j * rr_j,
+        [&](const Sophus::Vector6d &x) {
+          Sophus::SE3d T_w_j_new;
+          T_w_j_new.so3() = Sophus::SO3d::exp(x.tail<3>()) * T_w_j.so3();
+          T_w_j_new.translation() = T_w_j.translation() + x.head<3>();
+
+          return Sophus::se3_logd(T_ij_meas * T_w_j_new.inverse() * T_w_i);
+        },
+        x0);
+  }
+}
+
+TEST(SophusUtilsCase, RelPoseTestRightIncSim3) {
+  Sophus::Sim3d T_w_i = Sophus::Sim3d::exp(Sophus::Vector7d::Random());
+  Sophus::Sim3d T_w_j = Sophus::Sim3d::exp(Sophus::Vector7d::Random());
+
+  for (const double meas_error : {1e0, 1e-1, 1e-2, 1e-4}) {
+    Sophus::Sim3d T_ij_meas =
+        T_w_i.inverse() * T_w_j *
+        Sophus::Sim3d::exp(Sophus::Vector7d::Random() * meas_error);
+
+    Sophus::Sim3d T_j_i = T_w_j.inverse() * T_w_i;
+    Sophus::Vector7d res = Sophus::sim3_logd(T_ij_meas * T_j_i);
+
+    Sophus::Matrix7d J_T_w_i;
+    Sophus::Matrix7d J_T_w_j;
+    Sophus::rightJacobianInvSim3Decoupled(res, J_T_w_i);
+    J_T_w_j = -J_T_w_i * T_j_i.inverse().Adj();
+
+    Sophus::Vector7d x0;
+    x0.setZero();
+
+    test_jacobian(
+        "d_res_d_T_w_i", J_T_w_i,
+        [&](const Sophus::Vector7d &x) {
+          Sophus::Sim3d T_w_i_new = T_w_i * Sophus::sim3_expd(x);
+
+          return Sophus::sim3_logd(T_ij_meas * T_w_j.inverse() * T_w_i_new);
+        },
+        x0);
+
+    test_jacobian(
+        "d_res_d_T_w_j", J_T_w_j,
+        [&](const Sophus::Vector7d &x) {
+          Sophus::Sim3d T_w_j_new = T_w_j * Sophus::sim3_expd(x);
+
+          return Sophus::sim3_logd(T_ij_meas * T_w_j_new.inverse() * T_w_i);
+        },
+        x0);
+  }
+}

thirdparty/basalt-headers/test/src/test_spline.cpp

@@ -0,0 +1,757 @@
+/**
+BSD 3-Clause License
+
+Copyright (c) 2019, Vladyslav Usenko and Nikolaus Demmel.
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+
+* Redistributions of source code must retain the above copyright notice, this
+  list of conditions and the following disclaimer.
+
+* Redistributions in binary form must reproduce the above copyright notice,
+  this list of conditions and the following disclaimer in the documentation
+  and/or other materials provided with the distribution.
+
+* Neither the name of the copyright holder nor the names of its
+  contributors may be used to endorse or promote products derived from
+  this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
+FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
+OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#include <basalt/spline/rd_spline.h>
+#include <basalt/spline/so3_spline.h>
+
+#include <iostream>
+
+#include "gtest/gtest.h"
+#include "test_utils.h"
+
+template <int DIM, int N, int DERIV>
+void testEvaluate(const basalt::RdSpline<DIM, N> &spline, int64_t t_ns) {
+  using VectorD = typename basalt::RdSpline<DIM, N>::VecD;
+  using MatrixD = typename basalt::RdSpline<DIM, N>::MatD;
+
+  typename basalt::RdSpline<DIM, N>::JacobianStruct J_spline;
+
+  spline.template evaluate<DERIV>(t_ns, &J_spline);
+
+  VectorD x0;
+  x0.setZero();
+
+  for (size_t i = 0; i < 3 * N; i++) {
+    std::stringstream ss;
+
+    ss << "d_val_d_knot" << i << " time " << t_ns;
+
+    MatrixD J_a;
+    J_a.setZero();
+
+    if (i >= J_spline.start_idx && i < J_spline.start_idx + N) {
+      J_a.diagonal().setConstant(J_spline.d_val_d_knot[i - J_spline.start_idx]);
+    }
+
+    test_jacobian(
+        ss.str(), J_a,
+        [&](const VectorD &x) {
+          basalt::RdSpline<DIM, N> spline1 = spline;
+          spline1.getKnot(i) += x;
+
+          return spline1.template evaluate<DERIV>(t_ns);
+        },
+        x0);
+  }
+}
+
+template <int DIM, int N, int DERIV>
+void testTimeDeriv(const basalt::RdSpline<DIM, N> &spline, int64_t t_ns) {
+  using VectorD = typename basalt::RdSpline<DIM, N>::VecD;
+
+  VectorD d_val_d_t = spline.template evaluate<DERIV + 1>(t_ns);
+
+  Eigen::Matrix<double, 1, 1> x0;
+  x0.setZero();
+
+  test_jacobian(
+      "d_val_d_t", d_val_d_t,
+      [&](const Eigen::Matrix<double, 1, 1> &x) {
+        int64_t inc = x[0] * 1e9;
+        return spline.template evaluate<DERIV>(t_ns + inc);
+      },
+      x0);
+}
+
+template <int N>
+void testEvaluateSo3(const basalt::So3Spline<N> &spline, int64_t t_ns) {
+  using VectorD = typename basalt::So3Spline<N>::Vec3;
+  using MatrixD = typename basalt::So3Spline<N>::Mat3;
+  using SO3 = typename basalt::So3Spline<N>::SO3;
+
+  typename basalt::So3Spline<N>::JacobianStruct J_spline;
+
+  SO3 res = spline.evaluate(t_ns, &J_spline);
+
+  VectorD x0;
+  x0.setZero();
+
+  for (size_t i = 0; i < 3 * N; i++) {
+    std::stringstream ss;
+
+    ss << "d_val_d_knot" << i << " time " << t_ns;
+
+    MatrixD J_a;
+    J_a.setZero();
+
+    if (i >= J_spline.start_idx && i < J_spline.start_idx + N) {
+      J_a = J_spline.d_val_d_knot[i - J_spline.start_idx];
+    }
+
+    test_jacobian(
+        ss.str(), J_a,
+        [&](const VectorD &x) {
+          basalt::So3Spline<N> spline1 = spline;
+          spline1.getKnot(i) = SO3::exp(x) * spline.getKnot(i);
+
+          SO3 res1 = spline1.evaluate(t_ns);
+
+          return (res1 * res.inverse()).log();
+        },
+        x0);
+  }
+}
+
+template <int N>
+void testVelSo3(const basalt::So3Spline<N> &spline, int64_t t_ns) {
+  using VectorD = typename basalt::So3Spline<N>::Vec3;
+  using SO3 = typename basalt::So3Spline<N>::SO3;
+
+  SO3 res = spline.evaluate(t_ns);
+
+  VectorD d_res_d_t = spline.velocityBody(t_ns);
+
+  Eigen::Matrix<double, 1, 1> x0;
+  x0.setZero();
+
+  test_jacobian(
+      "d_val_d_t", d_res_d_t,
+      [&](const Eigen::Matrix<double, 1, 1> &x) {
+        int64_t inc = x[0] * 1e9;
+        return (res.inverse() * spline.evaluate(t_ns + inc)).log();
+      },
+      x0);
+}
+
+template <int N>
+void testAccelSo3(const basalt::So3Spline<N> &spline, int64_t t_ns) {
+  using VectorD = typename basalt::So3Spline<5>::Vec3;
+
+  VectorD vel1;
+  VectorD d_res_d_t = spline.accelerationBody(t_ns, &vel1);
+
+  VectorD vel2 = spline.velocityBody(t_ns);
+  EXPECT_TRUE(vel1.isApprox(vel2));
+
+  Eigen::Matrix<double, 1, 1> x0;
+  x0.setZero();
+
+  test_jacobian(
+      "d_val_d_t", d_res_d_t,
+      [&](const Eigen::Matrix<double, 1, 1> &x) {
+        int64_t inc = x[0] * 1e9;
+        return spline.velocityBody(t_ns + inc);
+      },
+      x0);
+}
+
+template <int N>
+void testJerkSo3(const basalt::So3Spline<N> &spline, int64_t t_ns) {
+  using VectorD = typename basalt::So3Spline<5>::Vec3;
+
+  VectorD vel1;
+  VectorD accel1;
+  VectorD d_res_d_t = spline.jerkBody(t_ns, &vel1, &accel1);
+
+  VectorD vel2 = spline.velocityBody(t_ns);
+  EXPECT_TRUE(vel1.isApprox(vel2));
+
+  VectorD accel2 = spline.accelerationBody(t_ns);
+  EXPECT_TRUE(accel1.isApprox(accel2));
+
+  Eigen::Matrix<double, 1, 1> x0;
+  x0.setZero();
+
+  test_jacobian(
+      "d_val_d_t", d_res_d_t,
+      [&](const Eigen::Matrix<double, 1, 1> &x) {
+        int64_t inc = x[0] * 1e9;
+        return spline.accelerationBody(t_ns + inc);
+      },
+      x0);
+}
+
+template <int N>
+void testEvaluateSo3Vel(const basalt::So3Spline<N> &spline, int64_t t_ns) {
+  using VectorD = typename basalt::So3Spline<5>::Vec3;
+  using MatrixD = typename basalt::So3Spline<5>::Mat3;
+  using SO3 = typename basalt::So3Spline<5>::SO3;
+
+  typename basalt::So3Spline<N>::JacobianStruct J_spline;
+
+  VectorD res = spline.velocityBody(t_ns, &J_spline);
+  VectorD res_ref = spline.velocityBody(t_ns);
+
+  ASSERT_TRUE(res_ref.isApprox(res)) << "res and res_ref are not the same";
+
+  VectorD x0;
+  x0.setZero();
+
+  for (size_t i = 0; i < 3 * N; i++) {
+    std::stringstream ss;
+
+    ss << "d_vel_d_knot" << i << " time " << t_ns;
+
+    MatrixD J_a;
+    J_a.setZero();
+
+    if (i >= J_spline.start_idx && i < J_spline.start_idx + N) {
+      J_a = J_spline.d_val_d_knot[i - J_spline.start_idx];
+    }
+
+    test_jacobian(
+        ss.str(), J_a,
+        [&](const VectorD &x) {
+          basalt::So3Spline<N> spline1 = spline;
+          spline1.getKnot(i) = SO3::exp(x) * spline.getKnot(i);
+
+          return spline1.velocityBody(t_ns);
+        },
+        x0);
+  }
+}
+
+template <int N>
+void testEvaluateSo3Accel(const basalt::So3Spline<N> &spline, int64_t t_ns) {
+  using VectorD = typename basalt::So3Spline<N>::Vec3;
+  using MatrixD = typename basalt::So3Spline<N>::Mat3;
+  using SO3 = typename basalt::So3Spline<N>::SO3;
+
+  typename basalt::So3Spline<N>::JacobianStruct J_accel;
+  typename basalt::So3Spline<N>::JacobianStruct J_vel;
+
+  VectorD vel;
+  VectorD vel_ref;
+
+  VectorD res = spline.accelerationBody(t_ns, &J_accel, &vel, &J_vel);
+  VectorD res_ref = spline.accelerationBody(t_ns, &vel_ref);
+
+  ASSERT_TRUE(vel_ref.isApprox(vel)) << "vel and vel_ref are not the same";
+  ASSERT_TRUE(res_ref.isApprox(res)) << "res and res_ref are not the same";
+
+  VectorD x0;
+  x0.setZero();
+
+  // Test velocity Jacobian
+  for (size_t i = 0; i < 3 * N; i++) {
+    std::stringstream ss;
+
+    ss << "d_vel_d_knot" << i << " time " << t_ns;
+
+    MatrixD J_a;
+    J_a.setZero();
+
+    if (i >= J_vel.start_idx && i < J_vel.start_idx + N) {
+      J_a = J_vel.d_val_d_knot[i - J_vel.start_idx];
+    }
+
+    test_jacobian(
+        ss.str(), J_a,
+        [&](const VectorD &x) {
+          basalt::So3Spline<N> spline1 = spline;
+          spline1.getKnot(i) = SO3::exp(x) * spline.getKnot(i);
+
+          return spline1.velocityBody(t_ns);
+        },
+        x0);
+  }
+
+  // Test acceleration Jacobian
+  for (size_t i = 0; i < 3 * N; i++) {
+    std::stringstream ss;
+
+    ss << "d_accel_d_knot" << i << " time " << t_ns;
+
+    MatrixD J_a;
+    J_a.setZero();
+
+    if (i >= J_accel.start_idx && i < J_accel.start_idx + N) {
+      J_a = J_accel.d_val_d_knot[i - J_accel.start_idx];
+    }
+
+    test_jacobian(
+        ss.str(), J_a,
+        [&](const VectorD &x) {
+          basalt::So3Spline<N> spline1 = spline;
+          spline1.getKnot(i) = SO3::exp(x) * spline.getKnot(i);
+
+          return spline1.accelerationBody(t_ns);
+        },
+        x0);
+  }
+}
+
+TEST(SplineTest, UBSplineEvaluateKnots4) {
+  static constexpr int DIM = 3;
+  static constexpr int N = 4;
+
+  basalt::RdSpline<DIM, N> spline(int64_t(2e9));
+  spline.genRandomTrajectory(3 * N);
+
+  for (int64_t t_ns = 0; t_ns < spline.maxTimeNs(); t_ns += 1e8) {
+    testEvaluate<DIM, N, 0>(spline, t_ns);
+  }
+}
+
+TEST(SplineTest, UBSplineEvaluateKnots5) {
+  static constexpr int DIM = 3;
+  static constexpr int N = 5;
+
+  basalt::RdSpline<DIM, N> spline(int64_t(2e9));
+  spline.genRandomTrajectory(3 * N);
+
+  for (int64_t t_ns = 0; t_ns < spline.maxTimeNs(); t_ns += 1e8) {
+    testEvaluate<DIM, N, 0>(spline, t_ns);
+  }
+}
+
+TEST(SplineTest, UBSplineEvaluateKnots6) {
+  static constexpr int DIM = 3;
+  static constexpr int N = 6;
+
+  basalt::RdSpline<DIM, N> spline(int64_t(2e9));
+  spline.genRandomTrajectory(3 * N);
+
+  for (int64_t t_ns = 0; t_ns < spline.maxTimeNs(); t_ns += 1e8) {
+    testEvaluate<DIM, N, 0>(spline, t_ns);
+  }
+}
+
+TEST(SplineTest, UBSplineVelocityKnots4) {
+  static constexpr int DIM = 3;
+  static constexpr int N = 4;
+
+  basalt::RdSpline<DIM, N> spline(int64_t(2e9));
+  spline.genRandomTrajectory(3 * N);
+
+  for (int64_t t_ns = 0; t_ns < spline.maxTimeNs(); t_ns += 1e8) {
+    testEvaluate<DIM, N, 1>(spline, t_ns);
+  }
+}
+
+TEST(SplineTest, UBSplineVelocityKnots5) {
+  static constexpr int DIM = 3;
+  static constexpr int N = 5;
+
+  basalt::RdSpline<DIM, N> spline(int64_t(2e9));
+  spline.genRandomTrajectory(3 * N);
+
+  for (int64_t t_ns = 0; t_ns < spline.maxTimeNs(); t_ns += 1e8) {
+    testEvaluate<DIM, N, 1>(spline, t_ns);
+  }
+}
+
+TEST(SplineTest, UBSplineVelocityKnots6) {
+  static constexpr int DIM = 3;
+  static constexpr int N = 6;
+
+  basalt::RdSpline<DIM, N> spline(int64_t(2e9));
+  spline.genRandomTrajectory(3 * N);
+
+  for (int64_t t_ns = 0; t_ns < spline.maxTimeNs(); t_ns += 1e8) {
+    testEvaluate<DIM, N, 1>(spline, t_ns);
+  }
+}
+
+TEST(SplineTest, UBSplineAccelKnots4) {
+  static constexpr int DIM = 3;
+  static constexpr int N = 4;
+
+  basalt::RdSpline<DIM, N> spline(int64_t(2e9));
+  spline.genRandomTrajectory(3 * N);
+
+  for (int64_t t_ns = 0; t_ns < spline.maxTimeNs(); t_ns += 1e8) {
+    testEvaluate<DIM, N, 2>(spline, t_ns);
+  }
+}
+
+TEST(SplineTest, UBSplineAccelKnots5) {
+  static constexpr int DIM = 3;
+  static constexpr int N = 5;
+
+  basalt::RdSpline<DIM, N> spline(int64_t(2e9));
+  spline.genRandomTrajectory(3 * N);
+
+  for (int64_t t_ns = 0; t_ns < spline.maxTimeNs(); t_ns += 1e8) {
+    testEvaluate<DIM, N, 2>(spline, t_ns);
+  }
+}
+
+TEST(SplineTest, UBSplineAccelKnots6) {
+  static constexpr int DIM = 3;
+  static constexpr int N = 6;
+
+  basalt::RdSpline<DIM, N> spline(int64_t(2e9));
+  spline.genRandomTrajectory(3 * N);
+
+  for (int64_t t_ns = 0; t_ns < spline.maxTimeNs(); t_ns += 1e8) {
+    testEvaluate<DIM, N, 2>(spline, t_ns);
+  }
+}
+
+TEST(SplineTest, UBSplineEvaluateTimeDeriv4) {
+  static constexpr int DIM = 3;
+  static constexpr int N = 4;
+
+  basalt::RdSpline<DIM, N> spline(int64_t(2e9));
+  spline.genRandomTrajectory(3 * N);
+
+  for (int64_t t_ns = 1e8; t_ns < spline.maxTimeNs() - 1e8; t_ns += 1e8) {
+    testTimeDeriv<DIM, N, 0>(spline, t_ns);
+  }
+}
+
+TEST(SplineTest, UBSplineEvaluateTimeDeriv5) {
+  static constexpr int DIM = 3;
+  static constexpr int N = 5;
+
+  basalt::RdSpline<DIM, N> spline(int64_t(2e9));
+  spline.genRandomTrajectory(3 * N);
+
+  for (int64_t t_ns = 1e8; t_ns < spline.maxTimeNs() - 1e8; t_ns += 1e8) {
+    testTimeDeriv<DIM, N, 0>(spline, t_ns);
+  }
+}
+
+TEST(SplineTest, UBSplineEvaluateTimeDeriv6) {
+  static constexpr int DIM = 3;
+  static constexpr int N = 6;
+
+  basalt::RdSpline<DIM, N> spline(int64_t(2e9));
+  spline.genRandomTrajectory(3 * N);
+
+  for (int64_t t_ns = 1e8; t_ns < spline.maxTimeNs() - 1e8; t_ns += 1e8) {
+    testTimeDeriv<DIM, N, 0>(spline, t_ns);
+  }
+}
+
+TEST(SplineTest, UBSplineVelocityTimeDeriv4) {
+  static constexpr int DIM = 3;
+  static constexpr int N = 4;
+
+  basalt::RdSpline<DIM, N> spline(int64_t(2e9));
+  spline.genRandomTrajectory(3 * N);
+
+  for (int64_t t_ns = 1e8; t_ns < spline.maxTimeNs() - 1e8; t_ns += 1e8) {
+    testTimeDeriv<DIM, N, 1>(spline, t_ns);
+  }
+}
+
+TEST(SplineTest, UBSplineVelocityTimeDeriv5) {
+  static constexpr int DIM = 3;
+  static constexpr int N = 5;
+
+  basalt::RdSpline<DIM, N> spline(int64_t(2e9));
+  spline.genRandomTrajectory(3 * N);
+
+  for (int64_t t_ns = 1e8; t_ns < spline.maxTimeNs() - 1e8; t_ns += 1e8) {
+    testTimeDeriv<DIM, N, 1>(spline, t_ns);
+  }
+}
+
+TEST(SplineTest, UBSplineVelocityTimeDeriv6) {
+  static constexpr int DIM = 3;
+  static constexpr int N = 6;
+
+  basalt::RdSpline<DIM, N> spline(int64_t(2e9));
+  spline.genRandomTrajectory(3 * N);
+
+  for (int64_t t_ns = 1e8; t_ns < spline.maxTimeNs() - 1e8; t_ns += 1e8) {
+    testTimeDeriv<DIM, N, 1>(spline, t_ns);
+  }
+}
+
+TEST(SplineTest, SO3CUBSplineEvaluateKnots4) {
+  static constexpr int N = 4;
+
+  basalt::So3Spline<N> spline(int64_t(2e9));
+  spline.genRandomTrajectory(3 * N);
+
+  for (int64_t t_ns = 0; t_ns < spline.maxTimeNs(); t_ns += 1e8) {
+    testEvaluateSo3(spline, t_ns);
+  }
+}
+
+TEST(SplineTest, SO3CUBSplineEvaluateKnots5) {
+  static constexpr int N = 5;
+
+  basalt::So3Spline<N> spline(int64_t(2e9));
+  spline.genRandomTrajectory(3 * N);
+
+  for (int64_t t_ns = 0; t_ns < spline.maxTimeNs(); t_ns += 1e8) {
+    testEvaluateSo3(spline, t_ns);
+  }
+}
+
+TEST(SplineTest, SO3CUBSplineEvaluateKnots6) {
+  static constexpr int N = 6;
+
+  basalt::So3Spline<N> spline(int64_t(2e9));
+  spline.genRandomTrajectory(3 * N);
+
+  for (int64_t t_ns = 0; t_ns < spline.maxTimeNs(); t_ns += 1e8) {
+    testEvaluateSo3(spline, t_ns);
+  }
+}
+
+TEST(SplineTest, SO3CUBSplineVelocity4) {
+  static constexpr int N = 4;
+
+  basalt::So3Spline<N> spline(int64_t(2e9));
+  spline.genRandomTrajectory(3 * N);
+
+  for (int64_t t_ns = 1e8; t_ns < spline.maxTimeNs() - 1e8; t_ns += 1e8) {
+    testVelSo3(spline, t_ns);
+  }
+}
+
+TEST(SplineTest, SO3CUBSplineVelocity5) {
+  static constexpr int N = 5;
+
+  basalt::So3Spline<N> spline(int64_t(2e9));
+  spline.genRandomTrajectory(3 * N);
+
+  for (int64_t t_ns = 1e8; t_ns < spline.maxTimeNs() - 1e8; t_ns += 1e8) {
+    testVelSo3(spline, t_ns);
+  }
+}
+
+TEST(SplineTest, SO3CUBSplineVelocity6) {
+  static constexpr int N = 6;
+
+  basalt::So3Spline<N> spline(int64_t(2e9));
+  spline.genRandomTrajectory(3 * N);
+
+  for (int64_t t_ns = 1e8; t_ns < spline.maxTimeNs() - 1e8; t_ns += 1e8) {
+    testVelSo3(spline, t_ns);
+  }
+}
+
+TEST(SplineTest, SO3CUBSplineAcceleration4) {
+  static constexpr int N = 4;
+
+  basalt::So3Spline<N> spline(int64_t(2e9));
+  spline.genRandomTrajectory(3 * N);
+
+  for (int64_t t_ns = 1e8; t_ns < spline.maxTimeNs() - 1e8; t_ns += 1e8) {
+    testAccelSo3(spline, t_ns);
+  }
+}
+
+TEST(SplineTest, SO3CUBSplineAcceleration5) {
+  static constexpr int N = 5;
+
+  basalt::So3Spline<N> spline(int64_t(2e9));
+  spline.genRandomTrajectory(3 * N);
+
+  for (int64_t t_ns = 1e8; t_ns < spline.maxTimeNs() - 1e8; t_ns += 1e8) {
+    testAccelSo3(spline, t_ns);
+  }
+}
+
+TEST(SplineTest, SO3CUBSplineAcceleration6) {
+  static constexpr int N = 6;
+
+  basalt::So3Spline<N> spline(int64_t(2e9));
+  spline.genRandomTrajectory(3 * N);
+
+  for (int64_t t_ns = 1e8; t_ns < spline.maxTimeNs() - 1e8; t_ns += 1e8) {
+    testAccelSo3(spline, t_ns);
+  }
+}
+
+TEST(SplineTest, SO3CUBSplineJerk5) {
+  static constexpr int N = 5;
+
+  basalt::So3Spline<N> spline(int64_t(2e9));
+  spline.genRandomTrajectory(3 * N);
+
+  for (int64_t t_ns = 1e8; t_ns < spline.maxTimeNs() - 1e8; t_ns += 1e8) {
+    testJerkSo3(spline, t_ns);
+  }
+}
+
+TEST(SplineTest, SO3CUBSplineJerk6) {
+  static constexpr int N = 6;
+
+  basalt::So3Spline<N> spline(int64_t(2e9));
+  spline.genRandomTrajectory(3 * N);
+
+  for (int64_t t_ns = 1e8; t_ns < spline.maxTimeNs() - 1e8; t_ns += 1e8) {
+    testJerkSo3(spline, t_ns);
+  }
+}
+
+TEST(SplineTest, SO3CUBSplineVelocityKnots4) {
+  static constexpr int N = 4;
+
+  basalt::So3Spline<N> spline(int64_t(2e9));
+  spline.genRandomTrajectory(3 * N);
+
+  for (int64_t t_ns = 0; t_ns < spline.maxTimeNs(); t_ns += 1e8) {
+    testEvaluateSo3Vel(spline, t_ns);
+  }
+}
+
+TEST(SplineTest, SO3CUBSplineVelocityKnots5) {
+  static constexpr int N = 5;
+
+  basalt::So3Spline<N> spline(int64_t(2e9));
+  spline.genRandomTrajectory(3 * N);
+
+  for (int64_t t_ns = 0; t_ns < spline.maxTimeNs(); t_ns += 1e8) {
+    testEvaluateSo3Vel(spline, t_ns);
+  }
+}
+
+TEST(SplineTest, SO3CUBSplineVelocityKnots6) {
+  static constexpr int N = 6;
+
+  basalt::So3Spline<N> spline(int64_t(2e9));
+  spline.genRandomTrajectory(3 * N);
+
+  for (int64_t t_ns = 0; t_ns < spline.maxTimeNs(); t_ns += 1e8) {
+    testEvaluateSo3Vel(spline, t_ns);
+  }
+}
+
+TEST(SplineTest, SO3CUBSplineAccelerationKnots4) {
+  static constexpr int N = 4;
+
+  basalt::So3Spline<N> spline(int64_t(2e9));
+  spline.genRandomTrajectory(3 * N);
+
+  for (int64_t t_ns = 0; t_ns < spline.maxTimeNs(); t_ns += 1e8) {
+    testEvaluateSo3Accel(spline, t_ns);
+  }
+}
+
+TEST(SplineTest, SO3CUBSplineAccelerationKnots5) {
+  static constexpr int N = 5;
+
+  basalt::So3Spline<N> spline(int64_t(2e9));
+  spline.genRandomTrajectory(3 * N);
+
+  for (int64_t t_ns = 0; t_ns < spline.maxTimeNs(); t_ns += 1e8) {
+    testEvaluateSo3Accel(spline, t_ns);
+  }
+}
+
+TEST(SplineTest, SO3CUBSplineAccelerationKnots6) {
+  static constexpr int N = 6;
+
+  basalt::So3Spline<N> spline(int64_t(2e9));
+  spline.genRandomTrajectory(3 * N);
+
+  for (int64_t t_ns = 0; t_ns < spline.maxTimeNs(); t_ns += 1e8) {
+    testEvaluateSo3Accel(spline, t_ns);
+  }
+}
+
+TEST(SplineTest, SO3CUBSplineBounds) {
+  static constexpr int N = 5;
+
+  basalt::So3Spline<N> spline(int64_t(2e9));
+  spline.genRandomTrajectory(3 * N);
+
+  // std::cerr << "spline.maxTimeNs() " << spline.maxTimeNs() << std::endl;
+
+  spline.evaluate(spline.maxTimeNs());
+  // std::cerr << "res1\n" << res1.matrix() << std::endl;
+  spline.evaluate(spline.minTimeNs());
+  // std::cerr << "res2\n" << res2.matrix() << std::endl;
+
+  // Sophus::SO3d res3 = spline.evaluate(spline.maxTimeNs() + 1);
+  // std::cerr << "res3\n" << res1.matrix() << std::endl;
+  // Sophus::SO3d res4 = spline.evaluate(spline.minTimeNs() - 1);
+  // std::cerr << "res4\n" << res2.matrix() << std::endl;
+}
+
+TEST(SplineTest, UBSplineBounds) {
+  static constexpr int N = 5;
+  static constexpr int DIM = 3;
+
+  basalt::RdSpline<DIM, N> spline(int64_t(2e9));
+  spline.genRandomTrajectory(3 * N);
+
+  // std::cerr << "spline.maxTimeNs() " << spline.maxTimeNs() << std::endl;
+
+  spline.evaluate(spline.maxTimeNs());
+  // std::cerr << "res1\n" << res1.matrix() << std::endl;
+  spline.evaluate(spline.minTimeNs());
+  // std::cerr << "res2\n" << res2.matrix() << std::endl;
+
+  // Eigen::Vector3d res3 = spline.evaluate(spline.maxTimeNs() + 1);
+  // std::cerr << "res3\n" << res1.matrix() << std::endl;
+  // Eigen::Vector3d res4 = spline.evaluate(spline.minTimeNs() - 1);
+  // std::cerr << "res4\n" << res2.matrix() << std::endl;
+}
+
+TEST(SplineTest, CrossProductTest) {
+  Eigen::Matrix3d J_1;
+  Eigen::Matrix3d J_2;
+  Eigen::Matrix3d J_cross;
+  Eigen::Vector3d v1;
+  Eigen::Vector3d v2;
+  J_1.setRandom();
+  J_2.setRandom();
+  v1.setRandom();
+  v2.setRandom();
+
+  J_cross =
+      Sophus::SO3d::hat(J_1 * v1) * J_2 - Sophus::SO3d::hat(J_2 * v2) * J_1;
+
+  test_jacobian(
+      "cross_prod_test1", J_cross,
+      [&](const Eigen::Vector3d &x) {
+        return (J_1 * (v1 + x)).cross(J_2 * (v2 + x));
+      },
+      Eigen::Vector3d::Zero());
+
+  J_cross = -Sophus::SO3d::hat(J_2 * v2) * J_1;
+
+  test_jacobian(
+      "cross_prod_test2", J_cross,
+      [&](const Eigen::Vector3d &x) {
+        return (J_1 * (v1 + x)).cross(J_2 * v2);
+      },
+      Eigen::Vector3d::Zero());
+
+  J_cross = Sophus::SO3d::hat(J_1 * v1) * J_2;
+
+  test_jacobian(
+      "cross_prod_test2", J_cross,
+      [&](const Eigen::Vector3d &x) {
+        return (J_1 * v1).cross(J_2 * (v2 + x));
+      },
+      Eigen::Vector3d::Zero());
+}

thirdparty/basalt-headers/test/src/test_spline_se3.cpp

@@ -0,0 +1,368 @@
+/**
+BSD 3-Clause License
+
+Copyright (c) 2019, Vladyslav Usenko and Nikolaus Demmel.
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+
+* Redistributions of source code must retain the above copyright notice, this
+  list of conditions and the following disclaimer.
+
+* Redistributions in binary form must reproduce the above copyright notice,
+  this list of conditions and the following disclaimer in the documentation
+  and/or other materials provided with the distribution.
+
+* Neither the name of the copyright holder nor the names of its
+  contributors may be used to endorse or promote products derived from
+  this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
+FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
+OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#include <basalt/spline/se3_spline.h>
+
+#include <iostream>
+
+#include "gtest/gtest.h"
+#include "test_utils.h"
+
+template <int N>
+void testGyroRes(const basalt::Se3Spline<N> &s, int64_t t_ns) {
+  typename basalt::Se3Spline<N>::SO3JacobianStruct J_spline;
+  Eigen::Matrix<double, 3, 12> J_bias;
+
+  basalt::CalibGyroBias<double> bias;
+
+  bias.setRandom();
+
+  // bias << 0.01, -0.02, 0.03, 0, 0, 0, 0, 0, 0, 0, 0, 0;
+  Eigen::Vector3d measurement = s.rotVelBody(t_ns);
+
+  s.gyroResidual(t_ns, measurement, bias, &J_spline, &J_bias);
+
+  for (size_t i = 0; i < s.numKnots(); i++) {
+    Sophus::Vector3d x0;
+    x0.setZero();
+
+    std::stringstream ss;
+    ss << "Spline order " << N << " d_gyro_res_d_knot" << i << " time " << t_ns;
+
+    Sophus::Matrix3d J_a;
+
+    if (i >= J_spline.start_idx && i < J_spline.start_idx + N) {
+      J_a = J_spline.d_val_d_knot[i - J_spline.start_idx];
+    } else {
+      J_a.setZero();
+    }
+
+    test_jacobian(
+        ss.str(), J_a,
+        [&](const Sophus::Vector3d &x) {
+          basalt::Se3Spline<N> s1 = s;
+          s1.getKnotSO3(i) = Sophus::SO3d::exp(x) * s.getKnotSO3(i);
+
+          return s1.gyroResidual(t_ns, measurement, bias);
+        },
+        x0);
+  }
+
+  {
+    Eigen::Matrix<double, 12, 1> x0;
+    x0.setZero();
+
+    std::stringstream ss;
+    ss << "Spline order " << N << " d_gyro_res_d_bias";
+
+    test_jacobian(
+        ss.str(), J_bias,
+        [&](const Eigen::Matrix<double, 12, 1> &x) {
+          auto b1 = bias;
+          b1 += x;
+          return s.gyroResidual(t_ns, measurement, b1);
+        },
+        x0);
+  }
+}
+
+template <int N>
+void testAccelRes(const basalt::Se3Spline<N> &s, int64_t t_ns) {
+  typename basalt::Se3Spline<N>::AccelPosSO3JacobianStruct J_spline;
+  Eigen::Matrix3d J_g;
+
+  Eigen::Matrix<double, 3, 9> J_bias;
+
+  basalt::CalibAccelBias<double> bias;
+  bias.setRandom();
+  // bias << -0.4, 0.5, -0.6, 0, 0, 0, 0, 0, 0;
+
+  Eigen::Vector3d g(0, 0, -9.81);
+  Eigen::Vector3d measurement = s.transAccelWorld(t_ns) + g;
+
+  s.accelResidual(t_ns, measurement, bias, g, &J_spline, &J_bias, &J_g);
+
+  for (size_t i = 0; i < s.numKnots(); i++) {
+    Sophus::Vector6d x0;
+    x0.setZero();
+
+    std::stringstream ss;
+    ss << "Spline order " << N << " d_accel_res_d_knot" << i;
+
+    typename basalt::Se3Spline<N>::Mat36 J_a;
+
+    if (i >= J_spline.start_idx && i < J_spline.start_idx + N) {
+      J_a = J_spline.d_val_d_knot[i - J_spline.start_idx];
+    } else {
+      J_a.setZero();
+    }
+
+    test_jacobian(
+        ss.str(), J_a,
+        [&](const Sophus::Vector6d &x) {
+          basalt::Se3Spline<N> s1 = s;
+          s1.applyInc(i, x);
+
+          return s1.accelResidual(t_ns, measurement, bias, g);
+        },
+        x0);
+  }
+
+  {
+    Eigen::Matrix<double, 9, 1> x0;
+    x0.setZero();
+
+    std::stringstream ss;
+    ss << "Spline order " << N << " d_accel_res_d_bias";
+
+    test_jacobian(
+        ss.str(), J_bias,
+        [&](const Eigen::Matrix<double, 9, 1> &x) {
+          auto b1 = bias;
+          b1 += x;
+          return s.accelResidual(t_ns, measurement, b1, g);
+        },
+        x0);
+  }
+
+  {
+    Sophus::Vector3d x0;
+    x0.setZero();
+
+    std::stringstream ss;
+    ss << "Spline order " << N << " d_accel_res_d_g";
+
+    test_jacobian(
+        ss.str(), J_g,
+        [&](const Sophus::Vector3d &x) {
+          return s.accelResidual(t_ns, measurement, bias, g + x);
+        },
+        x0);
+  }
+}
+
+template <int N>
+void testOrientationRes(const basalt::Se3Spline<N> &s, int64_t t_ns) {
+  typename basalt::Se3Spline<N>::SO3JacobianStruct J_spline;
+
+  Sophus::SO3d measurement =
+      s.pose(t_ns).so3();  // * Sophus::expd(Sophus::Vector6d::Random() / 10);
+
+  s.orientationResidual(t_ns, measurement, &J_spline);
+
+  for (size_t i = 0; i < s.numKnots(); i++) {
+    std::stringstream ss;
+    ss << "Spline order " << N << " d_rot_res_d_knot" << i;
+
+    Sophus::Matrix3d J_a;
+
+    if (i >= J_spline.start_idx && i < J_spline.start_idx + N) {
+      J_a = J_spline.d_val_d_knot[i - J_spline.start_idx];
+    } else {
+      J_a.setZero();
+    }
+
+    Sophus::Vector3d x0;
+    x0.setZero();
+
+    test_jacobian(
+        ss.str(), J_a,
+        [&](const Sophus::Vector3d &x_rot) {
+          Sophus::Vector6d x;
+          x.setZero();
+          x.tail<3>() = x_rot;
+
+          basalt::Se3Spline<N> s1 = s;
+          s1.applyInc(i, x);
+
+          return s1.orientationResidual(t_ns, measurement);
+        },
+        x0);
+  }
+}
+
+template <int N>
+void testPositionRes(const basalt::Se3Spline<N> &s, int64_t t_ns) {
+  typename basalt::Se3Spline<N>::PosJacobianStruct J_spline;
+
+  Eigen::Vector3d measurement =
+      s.pose(t_ns)
+          .translation();  // * Sophus::expd(Sophus::Vector6d::Random() / 10);
+
+  s.positionResidual(t_ns, measurement, &J_spline);
+
+  for (size_t i = 0; i < s.numKnots(); i++) {
+    std::stringstream ss;
+    ss << "Spline order " << N << " d_pos_res_d_knot" << i;
+
+    Sophus::Matrix3d J_a;
+    J_a.setZero();
+
+    if (i >= J_spline.start_idx && i < J_spline.start_idx + N) {
+      J_a.diagonal().setConstant(J_spline.d_val_d_knot[i - J_spline.start_idx]);
+    }
+
+    Sophus::Vector3d x0;
+    x0.setZero();
+
+    test_jacobian(
+        ss.str(), J_a,
+        [&](const Sophus::Vector3d &x_rot) {
+          Sophus::Vector6d x;
+          x.setZero();
+          x.head<3>() = x_rot;
+
+          basalt::Se3Spline<N> s1 = s;
+          s1.applyInc(i, x);
+
+          return s1.positionResidual(t_ns, measurement);
+        },
+        x0);
+  }
+}
+
+template <int N>
+void testPose(const basalt::Se3Spline<N> &s, int64_t t_ns) {
+  typename basalt::Se3Spline<N>::PosePosSO3JacobianStruct J_spline;
+
+  Sophus::SE3d res = s.pose(t_ns, &J_spline);
+
+  Sophus::Vector6d x0;
+  x0.setZero();
+
+  for (size_t i = 0; i < s.numKnots(); i++) {
+    std::stringstream ss;
+    ss << "Spline order " << N << " d_pose_d_knot" << i;
+
+    typename basalt::Se3Spline<N>::Mat6 J_a;
+
+    if (i >= J_spline.start_idx && i < J_spline.start_idx + N) {
+      J_a = J_spline.d_val_d_knot[i - J_spline.start_idx];
+    } else {
+      J_a.setZero();
+    }
+
+    test_jacobian(
+        ss.str(), J_a,
+        [&](const Sophus::Vector6d &x) {
+          basalt::Se3Spline<N> s1 = s;
+          s1.applyInc(i, x);
+
+          return Sophus::se3_logd(res.inverse() * s1.pose(t_ns));
+        },
+        x0);
+  }
+
+  {
+    Eigen::Matrix<double, 1, 1> x0;
+    x0[0] = 0;
+
+    typename basalt::Se3Spline<N>::Vec6 J_a;
+
+    //    J.template head<3>() = res.so3().inverse() * s.transVelWorld(t_ns);
+    //    J.template tail<3>() = s.rotVelBody(t_ns);
+
+    s.d_pose_d_t(t_ns, J_a);
+
+    test_jacobian(
+        "J_pose_time", J_a,
+        [&](const Eigen::Matrix<double, 1, 1> &x) {
+          int64_t t_ns_new = t_ns;
+          t_ns_new += x[0] * 1e9;
+
+          return Sophus::se3_logd(res.inverse() * s.pose(t_ns_new));
+        },
+        x0);
+  }
+}
+
+TEST(SplineSE3, GyroResidualTest) {
+  static constexpr int N = 5;
+
+  const int num_knots = 3 * N;
+  basalt::Se3Spline<N> s(int64_t(2e9));
+  s.genRandomTrajectory(num_knots);
+
+  for (int64_t t_ns = 0; t_ns < s.maxTimeNs(); t_ns += 1e8) {
+    testGyroRes<N>(s, t_ns);
+  }
+}
+
+TEST(SplineSE3, AccelResidualTest) {
+  static constexpr int N = 5;
+
+  const int num_knots = 3 * N;
+  basalt::Se3Spline<N> s(int64_t(2e9));
+  s.genRandomTrajectory(num_knots);
+
+  for (int64_t t_ns = 0; t_ns < s.maxTimeNs(); t_ns += 1e8) {
+    testAccelRes<N>(s, t_ns);
+  }
+}
+
+TEST(SplineSE3, PositionResidualTest) {
+  static constexpr int N = 5;
+
+  const int num_knots = 3 * N;
+  basalt::Se3Spline<N> s(int64_t(2e9));
+  s.genRandomTrajectory(num_knots);
+
+  for (int64_t t_ns = 0; t_ns < s.maxTimeNs(); t_ns += 1e8) {
+    testPositionRes<N>(s, t_ns);
+  }
+}
+
+TEST(SplineSE3, OrientationResidualTest) {
+  static constexpr int N = 5;
+
+  const int num_knots = 3 * N;
+  basalt::Se3Spline<N> s(int64_t(2e9));
+  s.genRandomTrajectory(num_knots);
+
+  for (int64_t t_ns = 0; t_ns < s.maxTimeNs(); t_ns += 1e8) {
+    testOrientationRes<N>(s, t_ns);
+  }
+}
+
+TEST(SplineSE3, PoseTest) {
+  static constexpr int N = 5;
+
+  const int num_knots = 3 * N;
+  basalt::Se3Spline<N> s(int64_t(2e9));
+  s.genRandomTrajectory(num_knots);
+
+  int64_t offset = 100;
+
+  for (int64_t t_ns = offset; t_ns < s.maxTimeNs() - offset; t_ns += 1e8) {
+    testPose<N>(s, t_ns);
+  }
+}