Merge branch 'master' of https://github.com/filtering-library/fl

include/fl/distribution/gaussian.hpp

@@ -89,7 +89,7 @@ struct Traits<Gaussian<Var>>
     typedef Moments<Variate, SecondMoment> MomentsBase;
 
     /**
-     * \brief Evalluation interface of a Gaussian
+     * \brief Evaluation interface of a Gaussian
      */
     typedef Evaluation<Variate, Scalar> EvaluationBase;
 

include/fl/filter/gaussian/point_set.hpp

@@ -157,7 +157,7 @@ struct Traits<PointSet<Point_, Points_>>
  * \tparam Point    Gaussian variable type
  * \tparam Points_  Number of points representing the gaussian
  */
-template <typename Point_, int Points_>
+template <typename Point_, int Points_ = -1>
 class PointSet
 {
 public:
@@ -177,7 +177,7 @@ class PointSet
      * \param dimension      Sample space dimension
      */
     PointSet(int dimension,
-             int points_count)
+             int points_count = ToDimension<Points_>())
         : points_(dimension, points_count),
           weights_(points_count, 1)
     {

include/fl/model/observation/joint_observation_model_id.hpp

@@ -147,7 +147,6 @@ class JointObservationModel<Models...>
         return prediction;
     }
 
-
     /**
      * \copydoc ObservationModelInterface::state_dimension
      *

include/fl/util/math/linear_algebra.hpp

@@ -305,6 +305,40 @@ void invert_diagonal_Vector(const SrcDiagonalMatrix& diagonal,
     }
 }
 
+
+/**
+ * \ingroup linear_algebra
+ * Computes the Frobenius norm of a given real matrix.
+ *
+ * The Frobenius norm is given by
+ *
+ * \f$
+ *   \| A \|_F := \sqrt{\sum_{i=1}^m \sum_{j=1}^n |a_{ij} |^2 }
+ * \f$
+ */
+template <typename Derived>
+double frobenius_norm(const Eigen::MatrixBase<Derived>& a)
+{
+    return std::sqrt(a.array().abs().square().sum());
+}
+
+
+/**
+ * Checks whether two matrices of the same size are similiar w.r.t. some
+ * \f$\epsilon\f$.
+ *
+ * The similarity check is based of the Frobenius Norm:
+ *
+ * \f$ are_similar(A, B) = \| A - B \|_F < \epsilon\f$
+ */
+template <typename DerivedA, typename DerivedB>
+bool are_similar(const Eigen::MatrixBase<DerivedA>& a,
+               const Eigen::MatrixBase<DerivedB>& b,
+               const double epsilon = 1.e-6)
+{
+    return frobenius_norm(a - b) < epsilon;
+}
+
 }
 
 #endif

include/fl/util/meta/operator/not_adaptive.hpp

@@ -239,7 +239,7 @@ class NotAdaptive<Model, internal::ObsrvModelType, Options<IsAdaptive>>
     }
 
     virtual void param(Param) {  }
-    virtual const Param& param() const { return param_; }
+//    virtual const Param& param() const { return param_; }
     virtual int param_dimension() const { return 0; }
 
 protected:
@@ -363,7 +363,7 @@ class NotAdaptive<Model, internal::ProcessModelType, Options<IsAdaptive>>
     }
 
     virtual void param(Param) {  }
-    virtual const Param& param() const { return param_; }
+//    virtual const Param& param() const { return param_; }
     virtual int param_dimension() const { return 0; }
 
 protected:

test/CMakeLists.txt

@@ -1,22 +1,22 @@
 
 ## some quick tests ##
-#catkin_add_gtest(some_tests
-#                 utils/some_tests.cpp
+#catkin_add_gtest(some_test
+#                 utils/some_test.cpp
 #                 gtest_main.cpp)
-#target_link_libraries(some_tests
+#target_link_libraries(some_test
 #                      ${catkin_LIBRARIES})
 
 ## trait tests ##
-catkin_add_gtest(traits_tests
+catkin_add_gtest(traits_test
                  utils/traits_test.cpp
                  gtest_main.cpp)
-target_link_libraries(traits_tests
+target_link_libraries(traits_test
                       ${catkin_LIBRARIES})
 
 
 ## meta tests##
-catkin_add_gtest(meta_tests utils/meta_test.cpp gtest_main.cpp)
-target_link_libraries(meta_tests ${catkin_LIBRARIES})
+catkin_add_gtest(meta_test utils/meta_test.cpp gtest_main.cpp)
+target_link_libraries(meta_test ${catkin_LIBRARIES})
 
 catkin_add_gtest(options_test utils/options_test.cpp gtest_main.cpp)
 target_link_libraries(options_test ${catkin_LIBRARIES})
@@ -28,34 +28,40 @@ catkin_add_gtest(not_adaptive_test utils/not_adaptive_test.cpp gtest_main.cpp)
 target_link_libraries(not_adaptive_test ${catkin_LIBRARIES})
 
 
-
  # filter interface tests ##
- catkin_add_gtest(filter_interface_tests
+ catkin_add_gtest(filter_interface_test
                   filter_interface/filter_interface_test.cpp
                   filter_interface/filter_interface_stubs.hpp
                   gtest_main.cpp)
- target_link_libraries(filter_interface_tests
+ target_link_libraries(filter_interface_test
                        ${catkin_LIBRARIES})
 
- ## point_set_tests tests ##
- catkin_add_gtest(point_set_tests
+ ## point_set_test tests ##
+ catkin_add_gtest(point_set_test
                   gaussian_filter/point_set_test.cpp
                   gtest_main.cpp)
- target_link_libraries(point_set_tests
+ target_link_libraries(point_set_test
                        ${catkin_LIBRARIES})
 
  ## unscented_transform tests ##
- catkin_add_gtest(unscented_transform_tests
+ catkin_add_gtest(unscented_transform_test
                   gaussian_filter/unscented_transform_test.cpp
                   gtest_main.cpp)
- target_link_libraries(unscented_transform_tests
+ target_link_libraries(unscented_transform_test
                        ${catkin_LIBRARIES})
 
+ ## monte_carlo trandform tests ##
+# catkin_add_gtest(monte_carlo_transform_test
+#                  gaussian_filter/monte_carlo_transform_test.cpp
+#                  gtest_main.cpp)
+# target_link_libraries(monte_carlo_transform_test
+#                       ${catkin_LIBRARIES})
+
  ## various filter tests ##
- catkin_add_gtest(distribution_tests
+ catkin_add_gtest(distribution_test
                   distribution/gaussian_test.cpp
                   gtest_main.cpp)
- target_link_libraries(distribution_tests
+ target_link_libraries(distribution_test
                        ${catkin_LIBRARIES})
 
  catkin_add_gtest(joint_distribution_id_test
@@ -70,17 +76,17 @@ target_link_libraries(not_adaptive_test ${catkin_LIBRARIES})
  target_link_libraries(joint_distribution_iid_test ${catkin_LIBRARIES})
 
  ## exceptions tests ##
- catkin_add_gtest(exception_tests
+ catkin_add_gtest(exception_test
                   exception/exception_test.cpp
                   gtest_main.cpp)
- target_link_libraries(exception_tests ${catkin_LIBRARIES})
+ target_link_libraries(exception_test ${catkin_LIBRARIES})
 
  ## linear models tests ##
- catkin_add_gtest(linear_models_tests
+ catkin_add_gtest(linear_models_test
                   model/linear_process_model_test.cpp
                   model/linear_observation_model_test.cpp
                   gtest_main.cpp)
- target_link_libraries(linear_models_tests ${catkin_LIBRARIES})
+ target_link_libraries(linear_models_test ${catkin_LIBRARIES})
 
  catkin_add_gtest(joint_observation_model_iid_test
                   model/joint_observation_model_iid_test.cpp
@@ -106,10 +112,10 @@ catkin_add_gtest(gaussian_filter_kf_test
 target_link_libraries(gaussian_filter_kf_test ${catkin_LIBRARIES})
 
 # Factorized SPKF #
-catkin_add_gtest(gaussian_filter_factorized_test
-                 gaussian_filter/gaussian_filter_factorized_test.cpp
-                 gtest_main.cpp)
-target_link_libraries(gaussian_filter_factorized_test ${catkin_LIBRARIES})
+#catkin_add_gtest(gaussian_filter_factorized_test
+#                 gaussian_filter/gaussian_filter_factorized_test.cpp
+#                 gtest_main.cpp)
+#target_link_libraries(gaussian_filter_factorized_test ${catkin_LIBRARIES})
 
 
 
@@ -144,23 +150,23 @@ target_link_libraries(gaussian_filter_factorized_test ${catkin_LIBRARIES})
 
 
 # ## Gaussian filter tests ##
-# catkin_add_gtest(gaussian_filter_tests
+# catkin_add_gtest(gaussian_filter_test
 #                  gaussian_filter/gaussian_filter_test.cpp
 #                  gaussian_filter/gaussian_filter_stubs.hpp
 #                  gtest_main.cpp)
-# target_link_libraries(gaussian_filter_tests
+# target_link_libraries(gaussian_filter_test
 #                       ${catkin_LIBRARIES})
 
 
-### Factorized UKF vs. Kalman filter tests ##
-# catkin_add_gtest(factorized_ukf_kf_tests
+## Factorized UKF vs. Kalman filter tests ##
+# catkin_add_gtest(factorized_ukf_kf_test
 #                  factorized_ukf/factorized_ukf_kf_test.cpp
 #                  gtest_main.cpp)
-# target_link_libraries(factorized_ukf_kf_tests
+# target_link_libraries(factorized_ukf_kf_test
 #                       ${catkin_LIBRARIES})
 ##
 ## ## Factorized UKF tests ##
-# catkin_add_gtest(factorized_ukf_tests
+# catkin_add_gtest(factorized_ukf_test
 ##                  factorized_ukf/linear_models_test.cpp
 #                  factorized_ukf/partitioned_ut_test.cpp
 #                  factorized_ukf/smw_inversion_test.cpp
@@ -171,7 +177,7 @@ target_link_libraries(gaussian_filter_factorized_test ${catkin_LIBRARIES})
 #                  factorized_ukf/factorized_ukf_dummies.hpp
 #                  factorized_ukf/factorized_ukf_stubs.hpp
 #                  gtest_main.cpp)
-# target_link_libraries(factorized_ukf_tests
+# target_link_libraries(factorized_ukf_test
 #                       ${PROJECT_NAME_TRACKING_INTERFACE}
 #                       ${PROJECT_NAME_TRACKING}
 #                       ${catkin_LIBRARIES})

test/distribution/gaussian_test.cpp

@@ -147,16 +147,12 @@ class GaussianTests:
     {
         EXPECT_GT(gaussian.dimension(), 0);
 
-        EXPECT_TRUE(gaussian.covariance().isApprox(covariance));
-        EXPECT_TRUE(gaussian.precision().isApprox(precision));
+        EXPECT_TRUE(fl::are_similar(gaussian.covariance(), covariance));
+        EXPECT_TRUE(fl::are_similar(gaussian.precision(), precision));
         const Covariance temp =
                 gaussian.square_root() * gaussian.square_root().transpose();
         const Covariance temp2 = square_root * square_root.transpose();
 
-        (void) temp;
-        (void) temp2;
-
-        //EXPECT_TRUE(temp.isApprox(temp2));
         EXPECT_TRUE(gaussian.has_full_rank());
     }
 };