WIP: save work.

oddkiva · Nov 1, 2023 · 59769d6 · 59769d6
1 parent 7441852
commit 59769d6
Show file tree

Hide file tree

Showing 5 changed files with 81 additions and 20 deletions.
diff --git a/cpp/src/DO/Sara/MultipleObjectTracking/ObservationModel.hpp b/cpp/src/DO/Sara/MultipleObjectTracking/ObservationModel.hpp
@@ -6,32 +6,29 @@
 
 namespace DO::Sara::MultipleObjectTracking {
 
-  template <typename T>
-  using ObservationModelMatrix = Eigen::Matrix<T, 4, 12>;
-
-  template <typename T>
-  inline auto observation_model_matrix() -> ObservationModelMatrix<T>
-  {
-    auto H = ObservationModelMatrix<T>{};
-
-    static const auto I = Eigen::Matrix4<T>::Identity();
-    static const auto O = Eigen::Matrix<T, 4, 8>::Zero();
-    H << I, O;
-
-    return H;
-  }
-
   template <typename T>
   struct ObservationEquation
   {
     using State = StateDistribution<T>;
+
     using Observation = ObservationDistribution<T>;
-    using Innovation = Observation;
-    using ObservationMatrix = ObservationModelMatrix<T>;
     using ObservationNoise = ObservationNoiseDistribution<T>;
+    using ObservationModelMatrix = Eigen::Matrix<T, 4, 12>;
 
+    using Innovation = Observation;
     using KalmanGain = typename ObservationDistribution<T>::CovarianceMatrix;
 
+    inline static auto observation_model_matrix() -> ObservationModelMatrix
+    {
+      auto H = ObservationModelMatrix{};
+
+      static const auto I = Eigen::Matrix4<T>::Identity();
+      static const auto O = Eigen::Matrix<T, 4, 8>::Zero();
+      H << I, O;
+
+      return H;
+    }
+
     inline auto innovation(const State& x_a_priori,
                            const Observation& z) const  //
         -> Innovation
@@ -62,13 +59,13 @@ namespace DO::Sara::MultipleObjectTracking {
     }
 
     inline auto residual(const Observation& z,
-                         const State& x_a_posteriori) const  //
+                         const State& x) const  //
         -> typename Observation::Mean
     {
-      return z.μ - H * x_a_posteriori.μ;
+      return z.μ - H * x.μ;
     }
 
-    ObservationMatrix H;
+    const ObservationModelMatrix H = observation_model_matrix();
     ObservationNoise v;
   };
 

diff --git a/cpp/test/Sara/CMakeLists.txt b/cpp/test/Sara/CMakeLists.txt
@@ -29,4 +29,6 @@ add_subdirectory(FeatureMatching)
 add_subdirectory(RANSAC)
 add_subdirectory(MultiViewGeometry)
 
+add_subdirectory(MultipleObjectTracking)
+
 add_subdirectory(Visualization)
diff --git a/cpp/test/Sara/MultipleObjectTracking/CMakeLists.txt b/cpp/test/Sara/MultipleObjectTracking/CMakeLists.txt
@@ -0,0 +1,10 @@
+file(GLOB test_SOURCE_FILES FILES test_*.cpp)
+
+foreach (file ${test_SOURCE_FILES})
+  get_filename_component(filename "${file}" NAME_WE)
+  sara_add_test(
+    NAME ${filename}
+    SOURCES ${file}
+    DEPENDENCIES DO::Sara::Core
+    FOLDER RANSAC)
+endforeach ()
diff --git a/cpp/test/Sara/MultipleObjectTracking/test_mot_equations.cpp b/cpp/test/Sara/MultipleObjectTracking/test_mot_equations.cpp
@@ -0,0 +1,41 @@
+// ========================================================================== //
+// This file is part of Sara, a basic set of libraries in C++ for computer
+// vision.
+//
+// Copyright (C) 2023-present David Ok <[email protected]>
+//
+// This Source Code Form is subject to the terms of the Mozilla Public
+// License v. 2.0. If a copy of the MPL was not distributed with this file,
+// you can obtain one at http://mozilla.org/MPL/2.0/.
+// ========================================================================== //
+
+#define BOOST_TEST_MODULE "MultipleObjectTracking/Kalman Filter Equations"
+
+#include <DO/Sara/MultipleObjectTracking/ObservationModel.hpp>
+#include <DO/Sara/MultipleObjectTracking/StateTransitionModel.hpp>
+
+#include <boost/test/unit_test.hpp>
+
+
+namespace mot = DO::Sara::MultipleObjectTracking;
+
+
+BOOST_AUTO_TEST_SUITE(TestKalmanFilter)
+
+BOOST_AUTO_TEST_CASE(test_observation_equation)
+{
+  auto z = mot::ObservationDistribution<float>{};
+  z.μ << 1, 1, 0.5, 1.75;
+
+  auto x = mot::StateDistribution<float>{};
+  x.μ.setZero();
+  x.μ.head<4>() << z.μ;
+  x.Σ.setZero();
+
+  const auto obs_eq = mot::ObservationEquation<float>{};
+
+  const auto r = obs_eq.residual(z, x);
+  BOOST_CHECK_SMALL(r.norm(), 1e-6f);
+}
+
+BOOST_AUTO_TEST_SUITE_END()
diff --git a/cpp/test/Sara/RANSAC/test_ransac_fundamental_matrix.cpp b/cpp/test/Sara/RANSAC/test_ransac_fundamental_matrix.cpp
@@ -1,3 +1,14 @@
+// ========================================================================== //
+// This file is part of Sara, a basic set of libraries in C++ for computer
+// vision.
+//
+// Copyright (C) 2023-present David Ok <[email protected]>
+//
+// This Source Code Form is subject to the terms of the Mozilla Public
+// License v. 2.0. If a copy of the MPL was not distributed with this file,
+// you can obtain one at http://mozilla.org/MPL/2.0/.
+// ========================================================================== //
+
 #define BOOST_TEST_MODULE "MultiViewGeometry/RANSAC Algorithm"
 
 #include <DO/Sara/Core/DebugUtilities.hpp>