-
Notifications
You must be signed in to change notification settings - Fork 0
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
* Added ellipsoid class * added doxygen to ellispoid methods that missed it
- Loading branch information
1 parent
3d371c9
commit a8c1919
Showing
8 changed files
with
223 additions
and
28 deletions.
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
72 changes: 72 additions & 0 deletions
72
vortex-filtering/include/vortex_filtering/utils/ellipsoid.hpp
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,72 @@ | ||
| #pragma once | ||
|
|
||
| #include <Eigen/Dense> | ||
| #include <cmath> | ||
| #include <numbers> | ||
| #include <vortex_filtering/probability/multi_var_gauss.hpp> | ||
| #include <vortex_filtering/types/type_aliases.hpp> | ||
|
|
||
| namespace vortex::utils { | ||
|
|
||
| template <size_t n_dims> class Ellipsoid { | ||
| public: | ||
| using T = Types_n<n_dims>; | ||
|
|
||
| /** Construct a new Ellipsoid object | ||
| * @param gauss n-dimensional Gaussian distribution | ||
| * @param scale_factor scale factor for the number of standard deviations (NB! This is slightly different from the ellipse scale factor) | ||
| */ | ||
| Ellipsoid(T::Gauss_n gauss, double scale_factor = 1.0) | ||
| : gauss_(gauss) | ||
| , scale_factor_(scale_factor) | ||
| { | ||
| } | ||
|
|
||
| /** Get the center of the ellipsoid | ||
| * @return T::Vec_n | ||
| */ | ||
| T::Vec_n center() const { return gauss_.mean(); } | ||
|
|
||
| /** Get the semi-major axis lengths of the ellipsoid sorted in descending order | ||
| * @return T::Vec_n | ||
| */ | ||
| T::Vec_n semi_axis_lengths() const | ||
| { | ||
| Eigen::SelfAdjointEigenSolver<typename T::Mat_nn> eigen_solver(gauss_.cov()); | ||
| typename T::Vec_n eigenvalues = eigen_solver.eigenvalues(); | ||
| typename T::Vec_n lengths = eigenvalues.array().sqrt().reverse(); | ||
| return lengths * scale_factor_; | ||
| } | ||
|
|
||
| /** Get the axis lengths of the ellipsoid sorted in descending order | ||
| * @return T::Vec_n | ||
| */ | ||
| T::Vec_n axis_lengths() const { return 2.0 * semi_axis_lengths(); } | ||
|
|
||
| /** Get the orthonormal axes of the ellipsoid corresponding to the eigen values sorted in descending order | ||
| * @return T::Mat_nn | ||
| */ | ||
| T::Mat_nn orthonormal_axes() const | ||
| { | ||
| Eigen::SelfAdjointEigenSolver<typename T::Mat_nn> eigen_solver(gauss_.cov()); | ||
| typename T::Mat_nn eigen_vectors = eigen_solver.eigenvectors().colwise().reverse(); | ||
| return eigen_vectors; | ||
| } | ||
|
|
||
| /** Get the volume of the ellipsoid | ||
| * @return double | ||
| */ | ||
| double volume() const | ||
| { | ||
| constexpr double scaling = std::pow(std::numbers::pi, n_dims / 2.0) / std::tgamma(n_dims / 2.0 + 1); | ||
| typename T::Vec_n lengths = semi_axis_lengths(); | ||
| double volume = scaling * lengths.prod(); | ||
| return volume; | ||
| } | ||
|
|
||
| private: | ||
| typename T::Gauss_n gauss_; | ||
| double scale_factor_; | ||
| }; | ||
|
|
||
| } // namespace vortex::utils |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,37 @@ | ||
| #include <cmath> | ||
| #include <numbers> | ||
| #include <limits> | ||
|
|
||
| using std::numbers::pi; | ||
|
|
||
| constexpr double gamma_function(double x) { | ||
| if (x == 0.0) { | ||
| return std::numeric_limits<double>::infinity(); | ||
| } | ||
|
|
||
| if (x < 0.0) { | ||
| return pi / (std::sin(pi * x) * gamma_function(1 - x)); | ||
| } | ||
|
|
||
| double result = 1.0; | ||
| while (x < 1.0) { | ||
| result /= x; | ||
| x += 1.0; | ||
| } | ||
|
|
||
| constexpr double p[] = { | ||
| 0.99999999999980993, 676.5203681218851, -1259.1392167224028, | ||
| 771.32342877765313, -176.61502916214059, 12.507343278686905, | ||
| -0.13857109526572012, 9.9843695780195716e-6, 1.5056327351493116e-7 | ||
| }; | ||
|
|
||
| double y = x; | ||
| double tmp = x + 5.5; | ||
| tmp -= (x + 0.5) * std::log(tmp); | ||
| double ser = 1.000000000190015; | ||
| for (int i = 0; i < 9; ++i) { | ||
| ser += p[i] / ++y; | ||
| } | ||
|
|
||
| return std::exp(-tmp + std::log(2.5066282746310005 * ser / x)); | ||
| } |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,74 @@ | ||
| #include "gtest_assertions.hpp" | ||
|
|
||
| #include <gtest/gtest.h> | ||
| #include <numbers> | ||
| #include <vortex_filtering/utils/ellipse.hpp> | ||
| #include <vortex_filtering/utils/ellipsoid.hpp> | ||
|
|
||
| TEST(Ellipse, constructorFromParams) | ||
| { | ||
| Eigen::Vector2d center(1.0, 2.0); | ||
| double a = 3.0; | ||
| double b = 2.0; | ||
| double angle = std::numbers::pi / 2.0; | ||
| Eigen::Matrix2d cov; | ||
| vortex::utils::Ellipse ellipse(center, a, b, angle); | ||
|
|
||
| EXPECT_EQ(ellipse.center(), center); | ||
| EXPECT_EQ(ellipse.a(), a); | ||
| EXPECT_EQ(ellipse.b(), b); | ||
| } | ||
|
|
||
| TEST(Ellipse, constructorFromGauss) | ||
| { | ||
| Eigen::Vector2d center(1.0, 2.0); | ||
| double a = 3.0; | ||
| double b = 2.0; | ||
| double angle = std::numbers::pi / 2.0; | ||
|
|
||
| Eigen::Matrix2d eigenvectors{{std::cos(angle), -std::sin(angle)}, {std::sin(angle), std::cos(angle)}}; | ||
| Eigen::Vector2d eigenvalues = {a * a, b * b}; | ||
|
|
||
| Eigen::Matrix2d cov = eigenvectors * eigenvalues.asDiagonal() * eigenvectors.transpose(); | ||
|
|
||
| vortex::prob::Gauss2d gauss(center, cov); | ||
| vortex::utils::Ellipse ellipse(gauss); | ||
|
|
||
| EXPECT_EQ(ellipse.center(), center); | ||
| EXPECT_EQ(ellipse.a(), a); | ||
| EXPECT_EQ(ellipse.b(), b); | ||
| } | ||
|
|
||
| TEST(Ellipsoid, constructorFromGauss) | ||
| { | ||
| Eigen::Vector2d center(1.0, 2.0); | ||
| Eigen::Matrix2d cov{{1.0, 0.0}, {0.0, 9.0}}; | ||
| vortex::prob::Gauss2d gauss(center, cov); | ||
| vortex::utils::Ellipsoid<2> ellipsoid(gauss); | ||
|
|
||
| EXPECT_EQ(ellipsoid.center(), center); | ||
| EXPECT_EQ(ellipsoid.semi_axis_lengths()(0), 3.0); | ||
| EXPECT_EQ(ellipsoid.semi_axis_lengths()(1), 1.0); | ||
| EXPECT_EQ(ellipsoid.volume(), std::numbers::pi * 3.0 * 1.0); | ||
| } | ||
|
|
||
| TEST(Ellipsoid, sameAsEllipse) | ||
| { | ||
| Eigen::Vector2d center(1.0, 2.0); | ||
| double a = 3.0; | ||
| double b = 2.0; | ||
| double angle = std::numbers::pi / 2.0; | ||
|
|
||
| Eigen::Matrix2d eigenvectors{{std::cos(angle), -std::sin(angle)}, {std::sin(angle), std::cos(angle)}}; | ||
| Eigen::Vector2d eigenvalues = {a * a, b * b}; | ||
|
|
||
| Eigen::Matrix2d cov = eigenvectors * eigenvalues.asDiagonal() * eigenvectors.transpose(); | ||
|
|
||
| vortex::prob::Gauss2d gauss(center, cov); | ||
| vortex::utils::Ellipsoid<2> ellipsoid(gauss); | ||
| vortex::utils::Ellipse ellipse(gauss); | ||
|
|
||
| EXPECT_EQ(ellipsoid.center(), ellipse.center()); | ||
| EXPECT_EQ(ellipsoid.axis_lengths(), ellipse.axes()); | ||
| EXPECT_EQ(ellipsoid.volume(), ellipse.area()); | ||
| } |