Added simple planar covariance ellipse class to slam

gtsam/slam/Ellipse.cpp

@@ -0,0 +1,56 @@
+/**
+ * @file Ellipse.cpp
+ * @author Alex Cunningham
+ */
+
+#include <boost/foreach.hpp>
+#include <gtsam/slam/Ellipse.h>
+
+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

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 <gtsam/geometry/Pose2.h>
+
+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<Point2> 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<Point2>& 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

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 

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 <iostream>
+#include <boost/foreach.hpp>
+#include <CppUnitLite/TestHarness.h>
+#include <gtsam/base/TestableAssertions.h>
+
+#include <gtsam/slam/planarSLAM.h>
+#include <gtsam/slam/Ellipse.h>
+
+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); }
+/* ************************************************************************* */