From a24f23374d1e33d8cb3aa434a0205c71b4c81756 Mon Sep 17 00:00:00 2001 From: Alex Cunningham Date: Fri, 7 Oct 2011 14:46:44 +0000 Subject: [PATCH] Added simple planar covariance ellipse class to slam --- gtsam/slam/Ellipse.cpp | 56 ++++++++++++++++++++++++++++++++ gtsam/slam/Ellipse.h | 43 ++++++++++++++++++++++++ gtsam/slam/Makefile.am | 6 +++- gtsam/slam/tests/testEllipse.cpp | 49 ++++++++++++++++++++++++++++ 4 files changed, 153 insertions(+), 1 deletion(-) create mode 100644 gtsam/slam/Ellipse.cpp create mode 100644 gtsam/slam/Ellipse.h create mode 100644 gtsam/slam/tests/testEllipse.cpp diff --git a/gtsam/slam/Ellipse.cpp b/gtsam/slam/Ellipse.cpp new file mode 100644 index 000000000..0aec14d40 --- /dev/null +++ b/gtsam/slam/Ellipse.cpp @@ -0,0 +1,56 @@ +/** + * @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 new file mode 100644 index 000000000..b13b4e19e --- /dev/null +++ b/gtsam/slam/Ellipse.h @@ -0,0 +1,43 @@ +/** + * @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 48d2eb7f5..ea36faed8 100644 --- a/gtsam/slam/Makefile.am +++ b/gtsam/slam/Makefile.am @@ -26,6 +26,7 @@ check_PROGRAMS += tests/testSimulated3D # Generic SLAM headers headers += BetweenFactor.h PriorFactor.h PartialPriorFactor.h +headers += BearingFactor.h RangeFactor.h BearingRangeFactor.h # Generic constraint headers headers += BetweenConstraint.h BoundingConstraint.h @@ -35,10 +36,13 @@ sources += pose2SLAM.cpp dataset.cpp check_PROGRAMS += tests/testPose2SLAM # 2D SLAM using Bearing and Range -headers += BearingFactor.h RangeFactor.h BearingRangeFactor.h 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 new file mode 100644 index 000000000..091c71f83 --- /dev/null +++ b/gtsam/slam/tests/testEllipse.cpp @@ -0,0 +1,49 @@ +/** + * @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); } +/* ************************************************************************* */