#3 fixed (#5)

* changed cmakelists and ellipse class

* package is successfully imported by other packages

* Committing clang-format changes

---------

Co-authored-by: Clang Robot <[email protected]>

vortex-filtering/CMakeLists.txt

@@ -12,7 +12,6 @@ if(NOT CMAKE_CXX_STANDARD)
 if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
   add_compile_options(
     -Wall -Wextra -Wpedantic 
-    # -Wno-unused-local-typedefs # Suppress warnings from unused typedefs
     -fopenmp # For parallel processing with Eigen
   )
 endif()
@@ -33,33 +32,34 @@ find_package(Boost REQUIRED
     filesystem
 ) # for gnuplot
 
-
-# === Include directories ===
-include_directories(include)
-
-# === Libraries ===
-add_library(${PROJECT_NAME}_lib INTERFACE)
-
-# = Specify the dependencies
+# = Specify dependencies
 set(lib_deps
   Eigen3
   OpenMP
 )
 set(lib_src
-  src/plotting/utils.cpp
+  src/utils/plotting.cpp
+  src/utils/ellipse.cpp
 )
 
-# = Specify the dependencies to link against
-ament_target_dependencies(${PROJECT_NAME}_lib INTERFACE
+# === Include directories ===
+include_directories(include)
+
+# === Libraries ===
+add_library(${PROJECT_NAME}_lib
+  ${lib_src}
+)
+# = Specify dependencies to link against
+ament_target_dependencies(${PROJECT_NAME}_lib PUBLIC
   ${lib_deps}
 )
-# = Specify the namespaced dependencies to link against
-target_link_libraries(${PROJECT_NAME}_lib INTERFACE
+# = Specify namespaced dependencies to link against
+target_link_libraries(${PROJECT_NAME}_lib PUBLIC
   Eigen3::Eigen
   OpenMP::OpenMP_CXX
 )
-# = Specify the include directories for the INTERFACE of the library
-target_include_directories(${PROJECT_NAME}_lib INTERFACE
+# = Specify the include directories for the library
+target_include_directories(${PROJECT_NAME}_lib PUBLIC
     "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
     "$<INSTALL_INTERFACE:include/${PROJECT_NAME}>"
 )
@@ -84,6 +84,7 @@ install(
   DESTINATION include
 )
 
+# === Build tests ===
 if(BUILD_TESTING)
   add_subdirectory(test)
 endif()

vortex-filtering/include/vortex_filtering/utils/ellipse.hpp

@@ -0,0 +1,96 @@
+/**
+ * @file ellipse.hpp
+ * @author Eirik Kolås
+ * @brief Ellipse class and ellipsoid class
+ * @version 0.1
+ * @date 2024-02-29
+ *
+ * @copyright Copyright (c) 2024
+ *
+ */
+
+#pragma once
+#include <Eigen/Dense>
+#include <vortex_filtering/probability/multi_var_gauss.hpp>
+
+namespace vortex {
+namespace utils {
+
+/** Class for representing an ellipse.
+ *
+ */
+class Ellipse {
+public:
+  /** Construct a new Ellipse object
+   * @param center
+   * @param a
+   * @param b
+   * @param angle
+   */
+  Ellipse(const Eigen::Vector2d &center, double a, double b, double angle);
+
+  /** Construct a new Ellipse object from a Gaussian
+   * @param gauss
+   * @param scale_factor
+   */
+  Ellipse(const vortex::prob::Gauss2d &gauss, double scale_factor = 1.0);
+
+  /** Get the center of the ellipse
+   * @return Eigen::Vector2d
+   */
+  Eigen::Vector2d center() const;
+
+  /** Get x coordinate of the center
+   * @return double
+   */
+  double x() const;
+
+  /** Get y coordinate of the center
+   * @return double
+   */
+  double y() const;
+
+  /** Get the a parameter of the ellipse
+   * @return double
+   */
+  double a() const;
+
+  /** Get the b parameter of the ellipse
+   * @return double
+   */
+  double b() const;
+
+  /** Get the major axis length of the ellipse
+   * @return double
+   */
+  double major_axis() const;
+
+  /** Get the minor axis length of the ellipse
+   * @return double
+   */
+  double minor_axis() const;
+
+  /** Get the axes lengths of the ellipse
+   * @return Eigen::Vector2d
+   */
+  Eigen::Vector2d axes() const;
+
+  /** Get the angle of the ellipse with respect to the x-axis
+   * @return double
+   */
+  double angle_rad() const;
+
+  /** Get the angle in degrees
+   * @return double
+   */
+  double angle_deg() const;
+
+private:
+  Eigen::Vector2d center_;
+  double a_;
+  double b_;
+  double angle_;
+};
+
+} // namespace utils
+} // namespace vortex

vortex-filtering/include/vortex_filtering/plotting/utils.hpp → vortex-filtering/include/vortex_filtering/utils/plotting.hpp

@@ -14,23 +14,17 @@
 #include <gnuplot-iostream.h>
 #include <vector>
 #include <vortex_filtering/probability/multi_var_gauss.hpp>
+#include <vortex_filtering/utils/ellipse.hpp>
 
 namespace vortex {
 namespace plotting {
 
-struct Ellipse {
-  double x;     // center
-  double y;     // center
-  double a;     // major axis length
-  double b;     // minor axis length
-  double angle; // angle in degrees
-};
-
 /** Convert a Gaussian to an ellipse.
- * @param MultiVarGauss
+ * @param gauss 2D Gaussian
+ * @param scale_factor (optional) Scale factor for the ellipse
  * @return Ellipse
  */
-Ellipse gauss_to_ellipse(const vortex::prob::Gauss2d &gauss);
+utils::Ellipse gauss_to_ellipse(const vortex::prob::Gauss2d &gauss, double scale_factor = 1.0);
 
 /** Create a normalized-error-squared NEES series from a series of errors and a covariance matrix.
  * @param errors

vortex-filtering/include/vortex_filtering/vortex_filtering.hpp

@@ -24,8 +24,9 @@
 // Numerical Integration
 #include <vortex_filtering/numerical_integration/erk_methods.hpp>
 
-// Plotting
-#include <vortex_filtering/plotting/utils.hpp>
+// Utils
+#include <vortex_filtering/utils/ellipse.hpp>
+#include <vortex_filtering/utils/plotting.hpp>
 
 // Probability
 #include <vortex_filtering/probability/gaussian_mixture.hpp>

vortex-filtering/src/utils/ellipse.cpp

@@ -0,0 +1,32 @@
+#include <vortex_filtering/utils/ellipse.hpp>
+
+namespace vortex {
+namespace utils {
+
+Ellipse::Ellipse(const Eigen::Vector2d &center, double a, double b, double angle) : center_(center), a_(a), b_(b), angle_(angle) {}
+
+Ellipse::Ellipse(const vortex::prob::Gauss2d &gauss, double scale_factor)
+{
+  Eigen::SelfAdjointEigenSolver<Eigen::Matrix2d> eigenSolver(gauss.cov());
+  Eigen::Vector2d eigenValues  = eigenSolver.eigenvalues();
+  Eigen::Matrix2d eigenVectors = eigenSolver.eigenvectors();
+
+  a_      = scale_factor * sqrt(eigenValues(1));
+  b_      = scale_factor * sqrt(eigenValues(0));
+  angle_  = atan2(eigenVectors(1, 1), eigenVectors(0, 1));
+  center_ = gauss.mean();
+}
+
+Eigen::Vector2d Ellipse::center() const { return center_; }
+double Ellipse::x() const { return center_(0); }
+double Ellipse::y() const { return center_(1); }
+double Ellipse::a() const { return a_; }
+double Ellipse::b() const { return b_; }
+double Ellipse::major_axis() const { return 2 * a_; }
+double Ellipse::minor_axis() const { return 2 * b_; }
+Eigen::Vector2d Ellipse::axes() const { return Eigen::Vector2d(2 * a_, 2 * b_); }
+double Ellipse::angle_rad() const { return angle_; }
+double Ellipse::angle_deg() const { return angle_ * 180 / M_PI; }
+
+} // namespace utils
+} // namespace vortex

vortex-filtering/src/utils/plotting.cpp

@@ -0,0 +1,10 @@
+#include <vortex_filtering/utils/ellipse.hpp>
+#include <vortex_filtering/utils/plotting.hpp>
+
+namespace vortex {
+namespace plotting {
+
+utils::Ellipse gauss_to_ellipse(const vortex::prob::Gauss2d &gauss, double scale_factor) { return utils::Ellipse(gauss, scale_factor); }
+
+} // namespace plotting
+} // namespace vortex

vortex-filtering/test/CMakeLists.txt

@@ -2,7 +2,9 @@ find_package(ament_cmake_gtest REQUIRED)
 ament_add_gtest(${PROJECT_NAME}_test 
   gtest_main.cpp
   gtest_assertions.cpp
-  ../${lib_src}
+
+  ../src/utils/plotting.cpp
+  ../src/utils/ellipse.cpp
 
   dynamic_model_test.cpp
   ekf_test.cpp

vortex-filtering/test/dynamic_model_test.cpp

@@ -5,7 +5,7 @@
 
 #include <vortex_filtering/models/dynamic_model_interfaces.hpp>
 #include <vortex_filtering/models/dynamic_models.hpp>
-#include <vortex_filtering/plotting/utils.hpp>
+#include <vortex_filtering/utils/plotting.hpp>
 
 #include "gtest_assertions.hpp"
 #include "test_models.hpp"
@@ -59,9 +59,9 @@ TEST(DynamicModel, sampleSimpleModel)
   gp << "plot '-' with circles title 'Samples' fs transparent solid 0.05 noborder\n";
   gp.send1d(samples);
 
-  vortex::plotting::Ellipse cov_ellipse = vortex::plotting::gauss_to_ellipse(true_gauss);
-  gp << "set object 1 ellipse center " << cov_ellipse.x << "," << cov_ellipse.y << " size " << 3 * cov_ellipse.a << "," << 3 * cov_ellipse.b << " angle "
-     << cov_ellipse.angle << "fs empty border lc rgb 'cyan'\n";
+  vortex::utils::Ellipse cov_ellipse = vortex::plotting::gauss_to_ellipse(true_gauss);
+  gp << "set object 1 ellipse center " << cov_ellipse.x() << "," << cov_ellipse.y() << " size " << 3 * cov_ellipse.a() << "," << 3 * cov_ellipse.b()
+     << " angle " << cov_ellipse.angle_deg() << "fs empty border lc rgb 'cyan'\n";
   gp << "replot\n";
 }
 

vortex-filtering/test/numerical_integration_test.cpp

@@ -8,7 +8,7 @@
 #include <vector>
 
 #include <vortex_filtering/numerical_integration/erk_methods.hpp>
-#include <vortex_filtering/plotting/utils.hpp>
+#include <vortex_filtering/utils/plotting.hpp>
 
 namespace sin_func_test {
 constexpr int N_DIM_x = 1, N_DIM_z = 1, N_DIM_u = 1, N_DIM_v = 1, N_DIM_w = 1;

vortex-filtering/test/probability_test.cpp

@@ -6,6 +6,7 @@
 #include <vortex_filtering/probability/gaussian_mixture.hpp>
 #include <vortex_filtering/probability/multi_var_gauss.hpp>
 #include <vortex_filtering/probability/poisson.hpp>
+#include <vortex_filtering/utils/ellipse.hpp>
 
 #include "gtest_assertions.hpp"
 
@@ -87,15 +88,11 @@ TEST(MultiVarGauss, sample)
   }
   cov /= samples.size();
 
-  // Plot points and an ellipse for the true mean and covariance. use the gp ellipse function
-  double num_std_dev = 3.0; // Number of standard deviations to plot the ellipse at
-  Eigen::SelfAdjointEigenSolver<Eigen::Matrix2d> eigenSolver(true_cov);
-  auto eigenValues  = eigenSolver.eigenvalues();
-  auto eigenVectors = eigenSolver.eigenvectors();
+  vortex::utils::Ellipse cov_ellipse({true_mean, true_cov});
 
-  double majorAxisLength = sqrt(eigenValues(1)) * num_std_dev;
-  double minorAxisLength = sqrt(eigenValues(0)) * num_std_dev;
-  double angle           = atan2(eigenVectors(1, 1), eigenVectors(0, 1)) * 180.0 / M_PI; // Convert to degrees
+  double majorAxisLength = cov_ellipse.major_axis();
+  double minorAxisLength = cov_ellipse.minor_axis();
+  double angle           = cov_ellipse.angle_deg();
 
   Gnuplot gp;
   gp << "set xrange [-10:10]\nset yrange [-10:10]\n";