ENH: add compile time testing for Distribution types.

oddkiva · Dec 5, 2023 · a0bd43a · a0bd43a
1 parent 80a5013
commit a0bd43a
Show file tree

Hide file tree

Showing 7 changed files with 151 additions and 73 deletions.
diff --git a/cpp/src/DO/Sara/KalmanFilter/DistributionConcepts.hpp b/cpp/src/DO/Sara/KalmanFilter/DistributionConcepts.hpp
@@ -1,37 +1,36 @@
 #pragma once
 
-#include <DO/Sara/KalmanFilter/EigenMatrixConcepts.hpp>
+#include <DO/Sara/KalmanFilter/MatrixConcepts.hpp>
 
 #include <concepts>
 #include <limits>
 
 
-namespace DO::Sara::KalmanFilter {
+namespace DO::Sara {
 
   template <typename T>
-  concept GaussianDistribution = requires(T dist)
+  concept ZeroMeanGaussianDistribution = requires(T dist)
   {
-    typename T::scalar_type;
-    typename T::mean_type;
-    typename T::covariance_matrix_type;
+    typename T::Scalar;
+    typename T::Mean;
+    typename T::CovarianceMatrix;
     // clang-format off
-    // Constructor.
-    { T{typename T::mean_type{}, typename T::covariance_matrix_type{}} } -> std::same_as<T>;
-    // Methods.
-    { dist.mean() } -> std::same_as<const typename T::mean_type&>;
-    { dist.covariance_matrix() } -> std::same_as<const typename T::covariance_matrix_type&>;
+    { T{typename T::CovarianceMatrix{}} } -> std::same_as<T>;
+    { dist.covariance_matrix() } -> std::same_as<const typename T::CovarianceMatrix&>;
     // clang-format on
   };
 
   template <typename T>
-  concept ZeroMeanGaussianDistribution = requires(T dist)
+  concept GaussianDistribution = ZeroMeanGaussianDistribution<T> &&
+      requires(T dist)
   {
-    typename T::scalar_type;
-    typename T::mean_type;
-    typename T::covariance_matrix_type;
     // clang-format off
-    { T{typename T::covariance_matrix_type{}} } -> std::same_as<T>;
-    { dist.covariance_matrix() } -> std::same_as<const typename T::covariance_matrix_type&>;
+    // Constructor.
+    {
+      T{typename T::Mean{}, typename T::CovarianceMatrix{}}
+    } -> std::same_as<T>;
+    // Methods.
+    { dist.mean() } -> std::same_as<const typename T::Mean&>;
     // clang-format on
   };
 
@@ -44,13 +43,13 @@ namespace DO::Sara::KalmanFilter {
   template <typename T>
   concept FixedSizeStateDistribution =  //
       GaussianDistribution<T> &&        //
-      CompileTimeFixedMatrix<decltype(T{}.mean())> &&
-      CompileTimeFixedMatrix<decltype(T{}.covariance_matrix())>;
+      CompileTimeSizedMatrixConcept<decltype(T{}.mean())> &&
+      CompileTimeSquareMatrixConcept<decltype(T{}.covariance_matrix())>;
 
   template <typename T>
   concept FixedSizeNoiseDistribution =  //
       ZeroMeanGaussianDistribution<T> &&
-      CompileTimeFixedMatrix<decltype(T{}.mean())> &&
-      CompileTimeFixedMatrix<decltype(T{}.covariance_matrix())>;
+      CompileTimeSizedMatrixConcept<decltype(T{}.mean())> &&
+      CompileTimeSquareMatrixConcept<decltype(T{}.covariance_matrix())>;
 
-}  // namespace DO::Sara::KalmanFilter
+}  // namespace DO::Sara
diff --git a/cpp/src/DO/Sara/KalmanFilter/EigenMatrixConcepts.hpp b/cpp/src/DO/Sara/KalmanFilter/EigenMatrixConcepts.hpp
diff --git a/cpp/src/DO/Sara/KalmanFilter/MatrixConcepts.hpp b/cpp/src/DO/Sara/KalmanFilter/MatrixConcepts.hpp
@@ -0,0 +1,49 @@
+#pragma once
+
+#include <Eigen/Core>
+
+#include <concepts>
+
+
+namespace DO::Sara {
+
+
+  template <typename T>
+  concept MatrixConcept = requires(T)
+  {
+    // clang-format off
+    { T{}.rows() };
+    { T{}.cols() };
+    { T{}(int{}, int{}) };
+    { T{}.transpose() };
+    // clang-format on
+  };
+
+  template <typename T>
+  concept VectorConcept = MatrixConcept<T> && requires(T)
+  {
+    T::ColsAtCompileTime == 1;
+    // clang-format off
+    { T{}(int{}) };
+    // clang-format on
+  };
+
+  template <typename T>
+  concept CompileTimeSizedMatrixConcept = MatrixConcept<T> && requires(T)
+  {
+    std::same_as<T, Eigen::Matrix<typename T::Scalar, T::RowsAtCompileTime,
+                                  T::ColsAtCompileTime>>;
+    // clang-format off
+    { T::RowsAtCompileTime };
+    { T::ColsAtCompileTime };
+    // clang-format on
+  };
+
+  template <typename T>
+  concept CompileTimeSquareMatrixConcept =
+      CompileTimeSizedMatrixConcept<T> && requires
+  {
+    T::RowsAtCompileTime == T::ColsAtCompileTime;
+  };
+
+}  // namespace DO::Sara
diff --git a/cpp/src/DO/Sara/MultipleObjectTracking/BaseDefinitions.hpp b/cpp/src/DO/Sara/MultipleObjectTracking/BaseDefinitions.hpp
@@ -38,18 +38,43 @@ namespace DO::Sara::MultipleObjectTracking {
   template <typename T>
   struct ObservationDistribution
   {
+    using Scalar = T;
     using Mean = CylindricBoxObservationVector<T>;
     using CovarianceMatrix = Eigen::Matrix4<T>;
 
+    auto mean() -> const Mean&
+    {
+      return μ;
+    }
+
+    auto covariance_matrix() -> const CovarianceMatrix&
+    {
+      return Σ;
+    }
+
     Mean μ;
     CovarianceMatrix Σ;
   };
 
   template <typename T>
   struct StateDistribution
   {
+    static constexpr auto dimension =
+        CylindricBoxObservationVector<T>::RowsAtCompileTime;
+
+    using Scalar = T;
     using Mean = CylindricBoxStateVector<T>;
-    using CovarianceMatrix = Eigen::Matrix<T, 12, 12>;
+    using CovarianceMatrix = Eigen::Matrix<T, dimension, dimension>;
+
+    auto mean() -> const Mean&
+    {
+      return μ;
+    }
+
+    auto covariance_matrix() -> const CovarianceMatrix&
+    {
+      return Σ;
+    }
 
     Mean μ;
     CovarianceMatrix Σ;

diff --git a/cpp/test/Sara/CMakeLists.txt b/cpp/test/Sara/CMakeLists.txt
@@ -29,6 +29,7 @@ add_subdirectory(FeatureMatching)
 add_subdirectory(RANSAC)
 add_subdirectory(MultiViewGeometry)
 
+add_subdirectory(KalmanFilter)
 add_subdirectory(MultipleObjectTracking)
 
 add_subdirectory(Visualization)
diff --git a/cpp/test/Sara/KalmanFilter/CMakeLists.txt b/cpp/test/Sara/KalmanFilter/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 KalmanFilter)
+endforeach ()
diff --git a/cpp/test/Sara/KalmanFilter/test_kalman_filter_concepts.cpp b/cpp/test/Sara/KalmanFilter/test_kalman_filter_concepts.cpp
@@ -0,0 +1,44 @@
+// ========================================================================== //
+// 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 "Kalman Filter/Concepts"
+
+#include <DO/Sara/KalmanFilter/DistributionConcepts.hpp>
+#include <DO/Sara/KalmanFilter/MatrixConcepts.hpp>
+#include <DO/Sara/MultipleObjectTracking/BaseDefinitions.hpp>
+
+#include <boost/test/unit_test.hpp>
+
+
+namespace sara = DO::Sara;
+namespace mot = DO::Sara::MultipleObjectTracking;
+
+
+BOOST_AUTO_TEST_SUITE(TestKalmanFilter)
+
+BOOST_AUTO_TEST_CASE(test_matrix_concepts)
+{
+  [[maybe_unused]] const sara::VectorConcept auto x = Eigen::Vector2d{};
+  [[maybe_unused]] const sara::CompileTimeSquareMatrixConcept auto m =
+      Eigen::Matrix4f{};
+}
+
+BOOST_AUTO_TEST_CASE(test_distribution_concepts)
+{
+  [[maybe_unused]] const sara::GaussianDistribution auto x =
+      mot::StateDistribution<double>{};
+
+  [[maybe_unused]] const sara::GaussianDistribution auto z =
+      mot::ObservationDistribution<double>{};
+}
+
+
+BOOST_AUTO_TEST_SUITE_END()