diff --git a/gtsam/slam/Ellipse.cpp b/gtsam/slam/Ellipse.cpp deleted file mode 100644 index 0aec14d40..000000000 --- a/gtsam/slam/Ellipse.cpp +++ /dev/null @@ -1,56 +0,0 @@ -/** - * @file Ellipse.cpp - * @author Alex Cunningham - */ - -#include -#include - -namespace gtsam { - -/* ***************************************************************** */ -void Ellipse::calculateMoments(const Matrix& covariance) { - // CODE SOURCE: pulled from MastSLAM linearDrawing, derived from Richard's code - // eigenvalue decomposition - Matrix U,V; - Vector S; - svd(covariance,U,S,V); - - // invert and sqrt diagonal of S - // We also arbitrarily choose sign to make result have positive signs - for(size_t i = 0; i<2; i++) - S(i) = pow(S(i), 0.5); - Matrix eig = vector_scale(U, S); // U*S; - - // draw ellipse - moment1_ = Point2(eig(0,0), eig(1,0)); - moment2_ = Point2(eig(0,1), eig(1,1)); -} - -/* ***************************************************************** */ -Ellipse::Ellipse(const Point2& m, const Matrix& covariance) - : mean_(m), cov_(covariance) -{ - calculateMoments(cov_); - for (double angle = 0.0; angle < 2.0 * M_PI; angle += (2.0 * M_PI / 50.0)) - unit_circle_.push_back(Point2(cos(angle), sin(angle))); -} - -/* ***************************************************************** */ -Ellipse::Ellipse(const Pose2& m, const Matrix& covariance) - : mean_(m.t()), cov_(sub(covariance, 0, 2, 0, 2)) -{ - calculateMoments(cov_); - for (double angle = 0.0; angle < 2.0 * M_PI; angle += (2.0 * M_PI / 50.0)) - unit_circle_.push_back(Point2(cos(angle), sin(angle))); -} - -/* ***************************************************************** */ -void Ellipse::print(const std::string& s) const { - mean_.print(s + std::string(" mean")); - gtsam::print(cov_, "covariance"); - moment1_.print("moment1"); - moment2_.print("moment2"); -} - -} // \namespace gtsam diff --git a/gtsam/slam/Ellipse.h b/gtsam/slam/Ellipse.h deleted file mode 100644 index b13b4e19e..000000000 --- a/gtsam/slam/Ellipse.h +++ /dev/null @@ -1,43 +0,0 @@ -/** - * @file Ellipse.h - * @brief Data structure for managing covariance ellipses with visualization - * @author Alex Cunningham - */ - -#pragma once - -#include - -namespace gtsam { - -/** Parameters and functions for a 2D covariance ellipse */ -class Ellipse { -protected: - Point2 mean_; - Matrix cov_; - Point2 moment1_, moment2_; /// moments of the ellipse - - // TODO: make this static so it isn't calculated each time - std::vector unit_circle_; /// unit circle for ellipses - -public: - - /** finds the moments on construction */ - Ellipse(const Point2& mean, const Matrix& covariance); - Ellipse(const Pose2& mean, const Matrix& covariance); - - // access - const Point2& mean() const { return mean_; } - const Matrix& covariance() const { return cov_; } - const Point2& moment1() const { return moment1_; } - const Point2& moment2() const { return moment2_; } - const std::vector& unit_circle() const { return unit_circle_; } - - void print(const std::string& s="") const; - -protected: - /** calculates moments from the covariance matrix */ - void calculateMoments(const Matrix& covariance); -}; - -} // \namespace gtsam diff --git a/gtsam/slam/Makefile.am b/gtsam/slam/Makefile.am index ea36faed8..42db68d6f 100644 --- a/gtsam/slam/Makefile.am +++ b/gtsam/slam/Makefile.am @@ -39,10 +39,6 @@ check_PROGRAMS += tests/testPose2SLAM sources += planarSLAM.cpp check_PROGRAMS += tests/testPlanarSLAM -# Visualization -sources += Ellipse.cpp -check_PROGRAMS += tests/testEllipse - # 3D Pose constraints sources += pose3SLAM.cpp check_PROGRAMS += tests/testPose3SLAM diff --git a/gtsam/slam/tests/testEllipse.cpp b/gtsam/slam/tests/testEllipse.cpp deleted file mode 100644 index 091c71f83..000000000 --- a/gtsam/slam/tests/testEllipse.cpp +++ /dev/null @@ -1,49 +0,0 @@ -/** - * @file testCorePlanarSLAM - * @brief Test single robot, planar case, data association (for Freiburg) - * @author Alex Cunningham - */ - -#include -#include -#include -#include - -#include -#include - -using namespace std; -using namespace gtsam; - -const double tol = 1e-5; - -/* ************************************************************************* */ -TEST( testEllipse, point_covariances ) { - - // Using a bearing/range measurement model with two observations - double range = 0.5; - double range_sigma = 0.1, bearing_sigma = 0.1; - double s1 = range_sigma; - double s2 = sin(bearing_sigma)*range; - Matrix covariance1 = Matrix_(2,2, s1*s1, 0.0, - 0.0, s2*s2); - Point2 mean(0.5, 0); - Ellipse actEllipse1(mean, covariance1); - EXPECT(assert_equal(mean, actEllipse1.mean(), tol)); - EXPECT(assert_equal(covariance1, actEllipse1.covariance(), tol)); - EXPECT(assert_equal(Point2( 0.1, 0.0), actEllipse1.moment1(), tol)); - EXPECT(assert_equal(Point2( 0.0, 0.05), actEllipse1.moment2(), 1e-3)); - - // ellipse parameters - Matrix covariance2 = Matrix_(2,2, 0.00666667, 0.0, - 0.0, 0.00210191); - Ellipse actEllipse2(mean, covariance2); - EXPECT(assert_equal(mean, actEllipse2.mean(), tol)); - EXPECT(assert_equal(covariance2, actEllipse2.covariance(), tol)); - EXPECT(assert_equal(Point2(0.0816497, 0.0), actEllipse2.moment1(), tol)); - EXPECT(assert_equal(Point2(0.0, 0.0458466), actEllipse2.moment2(), tol)); -} - -/* ************************************************************************* */ -int main() { TestResult tr; return TestRegistry::runAllTests(tr); } -/* ************************************************************************* */