Added a nonlinear factor to approximate a linear factor from MastSLAM

slam/LinearApproxFactor.h

@@ -0,0 +1,112 @@
+/*
+ * @file LinearApproxFactor.h
+ * @brief A dummy factor that allows a linear factor to act as a nonlinear factor
+ * @author Alex Cunningham
+ */
+
+#pragma once
+
+#include <vector>
+#include <iostream>
+#include <boost/foreach.hpp>
+#include "NonlinearFactor.h"
+#include "VectorConfig.h"
+#include "Matrix.h"
+
+namespace gtsam {
+
+/**
+ * A dummy factor that takes a linearized factor and inserts it into
+ * a nonlinear graph.  This version uses exactly one type of variable.
+ */
+template <class Config, class Key, class X>
+class LinearApproxFactor : public NonlinearFactor<Config> {
+
+public:
+	/** base type */
+	typedef NonlinearFactor<Config> Base;
+
+	/** shared pointer for convenience */
+	typedef boost::shared_ptr<LinearApproxFactor<Config,Key,X> > shared_ptr;
+
+	/** typedefs for key vectors */
+	typedef std::vector<Key> KeyVector;
+
+protected:
+	/** hold onto the factor itself */
+	GaussianFactor::shared_ptr lin_factor_;
+
+	/** linearization points for error calculation */
+	Config lin_points_;
+
+	/** keep keys for the factor */
+	KeyVector nonlinearKeys_;
+
+	/**
+	 * use this for derived classes with keys that don't copy easily
+	 */
+	LinearApproxFactor(size_t dim, const Config& lin_points)
+		: Base(noiseModel::Unit::Create(dim)), lin_points_(lin_points) {}
+
+public:
+
+	/** use this constructor when starting with nonlinear keys */
+	LinearApproxFactor(GaussianFactor::shared_ptr lin_factor, const Config& lin_points)
+		: Base(noiseModel::Unit::Create(lin_factor->get_model()->dim())),
+		  lin_factor_(lin_factor), lin_points_(lin_points)
+	{
+		// create the keys and store them
+		BOOST_FOREACH(Symbol key, lin_factor->keys()) {
+			nonlinearKeys_.push_back(Key(key.index()));
+			this->keys_.push_back(key);
+		}
+	}
+
+	virtual ~LinearApproxFactor() {}
+
+	/** Vector of errors, unwhitened ! */
+	virtual Vector unwhitenedError(const Config& c) const {
+		// extract the points in the new config
+		VectorConfig delta;
+		BOOST_FOREACH(const Key& key, nonlinearKeys_) {
+			X newPt = c[key], linPt = lin_points_[key];
+			Vector d = logmap(linPt, newPt);
+			delta.insert(key, d);
+		}
+
+		return lin_factor_->unweighted_error(delta);
+	}
+
+	/**
+	 * linearize to a GaussianFactor
+	 * Just returns a copy of the existing factor
+	 * NOTE: copies to avoid actual factor getting destroyed
+	 * during elimination
+	 */
+	virtual boost::shared_ptr<GaussianFactor>
+	linearize(const Config& c) const {
+		Vector b = lin_factor_->get_b();
+		SharedDiagonal model = lin_factor_->get_model();
+		std::vector<std::pair<Symbol, Matrix> > terms;
+		BOOST_FOREACH(Symbol key, lin_factor_->keys()) {
+			terms.push_back(std::make_pair(key, lin_factor_->get_A(key)));
+		}
+
+		return boost::shared_ptr<GaussianFactor>(
+				new GaussianFactor(terms, b, model));
+	}
+
+	/** get access to nonlinear keys */
+	KeyVector nonlinearKeys() const { return nonlinearKeys_; }
+
+	/** override print function */
+	virtual void print(const std::string& s="") const {
+		Base::print(s);
+		lin_factor_->print();
+	}
+
+	/** access to b vector of gaussian */
+	Vector get_b() const { return lin_factor_->get_b(); }
+};
+
+} // \namespace gtsam

slam/Makefile.am

@@ -30,6 +30,9 @@ headers += BetweenFactor.h PriorFactor.h
 # General constraints
 headers += BetweenConstraint.h BoundingConstraint.h TransformConstraint.h
 
+# Utility factors
+headers += LinearApproxFactor.h
+
 # 2D Pose SLAM
 sources += pose2SLAM.cpp Pose2SLAMOptimizer.cpp dataset.cpp
 check_PROGRAMS += tests/testPose2Factor tests/testPose2Config tests/testPose2SLAM tests/testPose2Prior

tests/Makefile.am

@@ -11,7 +11,7 @@ check_PROGRAMS += testNonlinearEquality testNonlinearFactor testNonlinearFactorG
 check_PROGRAMS += testNonlinearOptimizer testSubgraphPreconditioner
 check_PROGRAMS += testSymbolicBayesNet testSymbolicFactorGraph testTupleConfig
 check_PROGRAMS += testNonlinearEqualityConstraint testBoundingConstraint 
-check_PROGRAMS += testTransformConstraint
+check_PROGRAMS += testTransformConstraint testLinearApproxFactor
 
 if USE_LDL
 check_PROGRAMS += testConstraintOptimizer 

tests/testLinearApproxFactor.cpp

@@ -0,0 +1,43 @@
+/*
+ * @file testLinearApproxFactor.cpp
+ * @brief tests for dummy factor that contains a linear factor
+ * @author Alex Cunningham
+ */
+
+#include <iostream>
+#include <CppUnitLite/TestHarness.h>
+#include <TestableAssertions.h> // allow assert_equal() for vectors
+#include <GaussianFactor.h>
+#include <planarSLAM.h>
+#include <LinearApproxFactor.h>
+
+using namespace std;
+using namespace gtsam;
+
+typedef LinearApproxFactor<planarSLAM::Config,planarSLAM::PointKey,Point2> ApproxFactor;
+
+/* ************************************************************************* */
+TEST ( LinearApproxFactor, basic ) {
+	Symbol key1('x', 1);
+	Matrix A1 = eye(2);
+	Vector b = repeat(2, 1.2);
+	SharedDiagonal model = noiseModel::Unit::Create(2);
+	GaussianFactor::shared_ptr lin_factor(new GaussianFactor(key1, A1, b, model));
+	planarSLAM::Config lin_points;
+	ApproxFactor f1(lin_factor, lin_points);
+
+	EXPECT(f1.size() == 1);
+	ApproxFactor::KeyVector expKeyVec;
+	expKeyVec.push_back(planarSLAM::PointKey(key1.index()));
+	EXPECT(assert_equal(expKeyVec, f1.nonlinearKeys()));
+
+	planarSLAM::Config config; // doesn't really need to have any data
+	GaussianFactor::shared_ptr actual = f1.linearize(config);
+
+	// Check the linearization
+	CHECK(assert_equal(*lin_factor, *actual));
+}
+
+/* ************************************************************************* */
+int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
+/* ************************************************************************* */