From 252224181851469bf5dce60362ecb84bbd7b5cc6 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Eirik=20Kol=C3=A5s?= <kolaseirik@gmail.com>
Date: Fri, 1 Mar 2024 14:01:22 +0100
Subject: [PATCH] fixed scaling factor in ellipse from gauss constructor

---
 .../vortex_filtering/utils/ellipse.hpp        | 28 +++++++++----------
 vortex-filtering/src/utils/ellipse.cpp        | 13 +++++++--
 2 files changed, 24 insertions(+), 17 deletions(-)

diff --git a/vortex-filtering/include/vortex_filtering/utils/ellipse.hpp b/vortex-filtering/include/vortex_filtering/utils/ellipse.hpp
index ea480249..b81a2839 100644
--- a/vortex-filtering/include/vortex_filtering/utils/ellipse.hpp
+++ b/vortex-filtering/include/vortex_filtering/utils/ellipse.hpp
@@ -22,16 +22,16 @@ namespace utils {
 class Ellipse {
 public:
   /** Construct a new Ellipse object
-   * @param center
-   * @param a
-   * @param b
-   * @param angle
+   * @param center center of the ellipse
+   * @param a half the length of the major axis (radius of the circumscribed circle)
+   * @param b half the length of the minor axis (radius of the inscribed circle)
+   * @param angle angle in radians
    */
   Ellipse(const Eigen::Vector2d &center, double a, double b, double angle);
 
   /** Construct a new Ellipse object from a Gaussian
-   * @param gauss
-   * @param scale_factor
+   * @param gauss 2D Gaussian distribution
+   * @param scale_factor scale factor for the ellipse
    */
   Ellipse(const vortex::prob::Gauss2d &gauss, double scale_factor = 1.0);
 
@@ -50,15 +50,15 @@ class Ellipse {
    */
   double y() const;
 
-  /** Get the a parameter of the ellipse
-   * @return double
-   */
-  double a() const;
+    /** Get the a parameter of the ellipse (half the length of the major axis)
+     * @return double
+     */
+    double a() const;
 
-  /** Get the b parameter of the ellipse
-   * @return double
-   */
-  double b() const;
+    /** Get the b parameter of the ellipse (half the length of the minor axis)
+     * @return double
+     */
+    double b() const;
 
   /** Get the major axis length of the ellipse
    * @return double
diff --git a/vortex-filtering/src/utils/ellipse.cpp b/vortex-filtering/src/utils/ellipse.cpp
index 2cf6384a..214f422b 100644
--- a/vortex-filtering/src/utils/ellipse.cpp
+++ b/vortex-filtering/src/utils/ellipse.cpp
@@ -11,9 +11,16 @@ Ellipse::Ellipse(const vortex::prob::Gauss2d &gauss, double scale_factor)
   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));
+  // Sort eigenvalues in descending order
+  if (eigenValues(0) > eigenValues(1))
+  {
+    std::swap(eigenValues(0), eigenValues(1));
+    eigenVectors.col(0).swap(eigenVectors.col(1));
+  }
+
+  a_     = sqrt(scale_factor * eigenValues(1));
+  b_     = sqrt(scale_factor * eigenValues(0));
+  angle_ = atan2(eigenVectors(1, 1), eigenVectors(0, 1));
   center_ = gauss.mean();
 }