fixed scaling factor in ellipse from gauss constructor

vortexntnu · Mar 1, 2024 · 2522241 · 2522241
1 parent 81924df
commit 2522241
Show file tree

Hide file tree

Showing 2 changed files with 24 additions and 17 deletions.
diff --git 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
@@ -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();
 }