Moved LinearizedFactor from MastSLAM, started on a test for LinearContainerFactor

.cproject

@@ -2114,6 +2114,14 @@
 				<useDefaultCommand>true</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>
 			</target>
+			<target name="check.nonlinear_unstable" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j5</buildArguments>
+				<buildTarget>check.nonlinear_unstable</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
 			<target name="testRot3.run" path="geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
 				<buildArguments>-j2</buildArguments>

gtsam_unstable/CMakeLists.txt

@@ -5,6 +5,7 @@ set (gtsam_unstable_subdirs
     discrete 
     dynamics
     linear
+    nonlinear
     slam 
 )
 

gtsam_unstable/nonlinear/CMakeLists.txt

@@ -0,0 +1,25 @@
+# Install headers
+file(GLOB nonlinear_headers "*.h")
+install(FILES ${nonlinear_headers} DESTINATION include/gtsam_unstable/nonlinear)
+
+# Components to link tests in this subfolder against
+set(nonlinear_local_libs 
+   nonlinear_unstable
+   nonlinear
+   linear
+   inference
+   geometry
+   base
+   ccolamd
+)
+
+set (nonlinear_full_libs
+    gtsam-static
+    gtsam_unstable-static)
+
+# Exclude tests that don't work
+set (nonlinear_excluded_tests "")
+
+# Add all tests
+gtsam_add_subdir_tests(nonlinear_unstable "${nonlinear_local_libs}" "${nonlinear_full_libs}" "${nonlinear_excluded_tests}") 
+add_dependencies(check.unstable check.nonlinear_unstable)

gtsam_unstable/nonlinear/LinearizedFactor.cpp

@@ -0,0 +1,147 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation, 
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+/**
+ * @file LinearizedFactor.cpp
+ * @brief A dummy factor that allows a linear factor to act as a nonlinear factor
+ * @author Alex Cunningham
+ */
+
+#include <iostream>
+#include <boost/foreach.hpp>
+
+#include <gtsam_unstable/nonlinear/LinearizedFactor.h>
+
+namespace gtsam {
+
+using namespace std;
+
+/* ************************************************************************* */
+LinearizedFactor::LinearizedFactor(JacobianFactor::shared_ptr lin_factor,
+		const KeyLookup& decoder, const Values& lin_points)
+: Base(lin_factor->get_model()), b_(lin_factor->getb()), model_(lin_factor->get_model()) {
+	BOOST_FOREACH(const Index& idx, *lin_factor) {
+		// find full symbol
+		KeyLookup::const_iterator decode_it = decoder.find(idx);
+		if (decode_it == decoder.end())
+			throw runtime_error("LinearizedFactor: could not find index in decoder!");
+		Key key(decode_it->second);
+
+		// extract linearization point
+		assert(lin_points.exists(key));
+		this->lin_points_.insert(key, lin_points.at(key));  // NOTE: will not overwrite
+
+		// extract Jacobian
+		Matrix A = lin_factor->getA(lin_factor->find(idx));
+		this->matrices_.insert(std::make_pair(key, A));
+
+		// store keys
+		this->keys_.push_back(key);
+	}
+}
+
+/* ************************************************************************* */
+LinearizedFactor::LinearizedFactor(JacobianFactor::shared_ptr lin_factor,
+			const Ordering& ordering, const Values& lin_points)
+: Base(lin_factor->get_model()), b_(lin_factor->getb()), model_(lin_factor->get_model()) {
+	const KeyLookup decoder = ordering.invert();
+	BOOST_FOREACH(const Index& idx, *lin_factor) {
+		// find full symbol
+		KeyLookup::const_iterator decode_it = decoder.find(idx);
+		if (decode_it == decoder.end())
+			throw runtime_error("LinearizedFactor: could not find index in decoder!");
+		Key key(decode_it->second);
+
+		// extract linearization point
+		assert(lin_points.exists(key));
+		this->lin_points_.insert(key, lin_points.at(key));  // NOTE: will not overwrite
+
+		// extract Jacobian
+		Matrix A = lin_factor->getA(lin_factor->find(idx));
+		this->matrices_.insert(std::make_pair(key, A));
+
+		// store keys
+		this->keys_.push_back(key);
+	}
+}
+
+/* ************************************************************************* */
+Vector LinearizedFactor::unwhitenedError(const Values& c,
+		boost::optional<std::vector<Matrix>&> H) const {
+	// extract the points in the new values
+	Vector ret = - b_;
+
+	if (H) H->resize(this->size());
+	size_t i=0;
+	BOOST_FOREACH(Key key, this->keys()) {
+		const Value& newPt = c.at(key);
+		const Value& linPt = lin_points_.at(key);
+		Vector d = linPt.localCoordinates_(newPt);
+		const Matrix& A = matrices_.at(key);
+		ret += A * d;
+		if (H) (*H)[i++] = A;
+	}
+
+	return ret;
+}
+
+/* ************************************************************************* */
+boost::shared_ptr<GaussianFactor>
+LinearizedFactor::linearize(const Values& c, const Ordering& ordering) const {
+
+	// sort by varid - only known at linearization time
+	typedef std::map<Index, Matrix> VarMatrixMap;
+	VarMatrixMap sorting_terms;
+	BOOST_FOREACH(const KeyMatrixMap::value_type& p, matrices_)
+		sorting_terms.insert(std::make_pair(ordering[p.first], p.second));
+
+	// move into terms
+	std::vector<std::pair<Index, Matrix> > terms;
+	BOOST_FOREACH(const VarMatrixMap::value_type& p, sorting_terms)
+		terms.push_back(p);
+
+	// compute rhs: adjust current by whitened update
+	Vector b = unwhitenedError(c) + b_; // remove original b
+	this->noiseModel_->whitenInPlace(b);
+
+	return boost::shared_ptr<GaussianFactor>(new JacobianFactor(terms, b - b_, model_));
+}
+
+/* ************************************************************************* */
+void LinearizedFactor::print(const std::string& s, const KeyFormatter& keyFormatter) const {
+	this->noiseModel_->print(s + std::string(" model"));
+	BOOST_FOREACH(const KeyMatrixMap::value_type& p, matrices_) {
+		gtsam::print(p.second, keyFormatter(p.first));
+	}
+	gtsam::print(b_, std::string("b"));
+	std::cout << " nonlinear keys: ";
+	BOOST_FOREACH(const Key& key, this->keys())
+		cout << keyFormatter(key) << "  ";
+	lin_points_.print("Linearization Point");
+}
+
+/* ************************************************************************* */
+bool LinearizedFactor::equals(const LinearizedFactor& other, double tol) const {
+	if (!Base::equals(other, tol)
+			|| !lin_points_.equals(other.lin_points_, tol)
+			|| !equal_with_abs_tol(b_, other.b_, tol)
+			|| !model_->equals(*other.model_, tol)
+			|| matrices_.size() != other.matrices_.size())
+		return false;
+
+	KeyMatrixMap::const_iterator map1 = matrices_.begin(), map2 = other.matrices_.begin();
+	for (; map1 != matrices_.end(), map2 != other.matrices_.end(); ++map1, ++map2)
+		if ((map1->first != map2->first) || !equal_with_abs_tol(map1->second, map2->second, tol))
+			return false;
+	return true;
+}
+
+} // \namespace gtsam

gtsam_unstable/nonlinear/LinearizedFactor.h

@@ -0,0 +1,122 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation, 
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+/**
+ * @file LinearizedFactor.h
+ * @brief A dummy factor that allows a linear factor to act as a nonlinear factor
+ * @author Alex Cunningham
+ */
+
+#pragma once
+
+#include <vector>
+#include <gtsam/nonlinear/Ordering.h>
+#include <gtsam/nonlinear/NonlinearFactor.h>
+
+namespace gtsam {
+
+/**
+ * A dummy factor that takes a linearized factor and inserts it into
+ * a nonlinear graph.
+ */
+class LinearizedFactor : public gtsam::NoiseModelFactor {
+
+public:
+	/** base type */
+	typedef gtsam::NoiseModelFactor Base;
+	typedef LinearizedFactor This;
+
+	/** shared pointer for convenience */
+	typedef boost::shared_ptr<LinearizedFactor > shared_ptr;
+
+
+	/** decoder for keys - avoids the use of a full ordering */
+	typedef gtsam::Ordering::InvertedMap KeyLookup;
+
+protected:
+	/** hold onto the factor itself */
+	// store components of a jacobian factor
+	typedef std::map<Key, gtsam::Matrix> KeyMatrixMap;
+	KeyMatrixMap matrices_;
+	gtsam::Vector b_;
+	gtsam::SharedDiagonal model_; /// separate from the noisemodel in NonlinearFactor due to Diagonal/Gaussian
+
+	/** linearization points for error calculation */
+	gtsam::Values lin_points_;
+
+	/** default constructor for serialization */
+	LinearizedFactor() {}
+
+public:
+
+	virtual ~LinearizedFactor() {}
+
+	ADD_CLONE_NONLINEAR_FACTOR(This);
+
+	/**
+	 * Use this constructor with pre-constructed decoders
+	 * @param lin_factor is a gaussian factor with linear keys (no labels baked in)
+	 * @param decoder is a structure to look up the correct keys
+	 * @param values is assumed to have the correct key structure with labels
+	 */
+	LinearizedFactor(gtsam::JacobianFactor::shared_ptr lin_factor,
+			const KeyLookup& decoder, const gtsam::Values& lin_points);
+
+	/**
+	 * Use this constructor with the ordering used to linearize the graph
+	 * @param lin_factor is a gaussian factor with linear keys (no labels baked in)
+	 * @param ordering is the full ordering used to linearize the graph
+	 * @param values is assumed to have the correct key structure with labels
+	 */
+	LinearizedFactor(gtsam::JacobianFactor::shared_ptr lin_factor,
+			const gtsam::Ordering& ordering, const gtsam::Values& lin_points);
+
+	/** Vector of errors, unwhitened, with optional derivatives (ordered by key) */
+	virtual gtsam::Vector unwhitenedError(const gtsam::Values& c,
+			boost::optional<std::vector<gtsam::Matrix>&> H = boost::none) const;
+
+	/**
+	 * linearize to a GaussianFactor
+	 * Reimplemented from NoiseModelFactor to directly copy out the
+	 * matrices and only update the RHS b with an updated residual
+	 */
+	virtual boost::shared_ptr<gtsam::GaussianFactor> linearize(
+			const gtsam::Values& c, const gtsam::Ordering& ordering) const;
+
+	// Testable
+
+	/** print function */
+	virtual void print(const std::string& s="", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const;
+
+	/** equals function with optional tolerance */
+	virtual bool equals(const LinearizedFactor& other, double tol = 1e-9) const;
+
+	// access functions
+
+	const gtsam::Vector& b() const { return b_; }
+	inline const gtsam::Values& linearizationPoint() const { return lin_points_; }
+
+private:
+	/** Serialization function */
+	friend class boost::serialization::access;
+	template<class ARCHIVE>
+	void serialize(ARCHIVE & ar, const unsigned int version) {
+		ar & boost::serialization::make_nvp("NonlinearFactor",
+				boost::serialization::base_object<Base>(*this));
+		ar & BOOST_SERIALIZATION_NVP(matrices_);
+		ar & BOOST_SERIALIZATION_NVP(b_);
+		ar & BOOST_SERIALIZATION_NVP(model_);
+		ar & BOOST_SERIALIZATION_NVP(lin_points_);
+	}
+};
+
+} // \namespace gtsam
+

gtsam_unstable/nonlinear/tests/testLinearContainerFactors.cpp

@@ -0,0 +1,22 @@
+/**
+ * @file testLinearContainerFactors.cpp
+ *
+ * @brief Tests the use of nonlinear containers for simple integration of linear factors into nonlinear graphs
+ *
+ * @date Jun 7, 2012
+ * @author Alex Cunningham
+ */
+
+#include <CppUnitLite/TestHarness.h>
+
+
+/* ************************************************************************* */
+TEST( testLinearContainerFactors, jacobian_factor ) {
+
+//	LinearContainerFactor actualLinFactor();
+
+}
+
+/* ************************************************************************* */
+int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
+/* ************************************************************************* */

gtsam_unstable/nonlinear/tests/testLinearizedFactor.cpp

@@ -0,0 +1,68 @@
+/**
+ * @file testLinearizedFactor
+ * @author Alex Cunningham
+ */
+
+#include <boost/assign/std/vector.hpp>
+#include <CppUnitLite/TestHarness.h>
+#include <gtsam/base/TestableAssertions.h>
+#include <gtsam/slam/planarSLAM.h>
+
+#include <gtsam_unstable/nonlinear/LinearizedFactor.h>
+
+using namespace std;
+using namespace boost::assign;
+using namespace gtsam;
+
+static const double tol = 1e-5;
+
+/* ************************************************************************* */
+TEST( testLinearizedFactor, creation ) {
+	// Create a set of local keys (No robot label)
+	Key	l1 = 11, l2 = 12,
+			l3 = 13, l4 = 14,
+			l5 = 15, l6 = 16,
+			l7 = 17, l8 = 18;
+
+	// creating an ordering to decode the linearized factor
+	Ordering ordering;
+	ordering += l1,l2,l3,l4,l5,l6,l7,l8;
+
+	// create a decoder for only the relevant variables
+	LinearizedFactor::KeyLookup decoder;
+	decoder[2] = l3;
+	decoder[4] = l5;
+
+	// create a linear factor
+	SharedDiagonal model = noiseModel::Unit::Create(2);
+	JacobianFactor::shared_ptr linear_factor(new JacobianFactor(
+			ordering[l3], eye(2,2), ordering[l5], 2.0 * eye(2,2), zero(2), model));
+
+	// create a set of values - build with full set of values
+	gtsam::Values full_values, exp_values;
+	full_values.insert(l3, Point2(1.0, 2.0));
+	full_values.insert(l5, Point2(4.0, 3.0));
+	exp_values = full_values;
+	full_values.insert(l1, Point2(3.0, 7.0));
+
+	// create the test LinearizedFactor
+	// This is called in the constructor of DDFSlot for each linear factor
+	LinearizedFactor actual1(linear_factor, decoder, full_values);
+	LinearizedFactor actual2(linear_factor, ordering, full_values);
+
+	EXPECT(assert_equal(actual1, actual2, tol));
+
+	// Verify the keys
+	vector<gtsam::Key> expKeys;
+	expKeys += l3, l5;
+	EXPECT(assert_container_equality(expKeys, actual1.keys()));
+	EXPECT(assert_container_equality(expKeys, actual2.keys()));
+
+	// Verify subset of linearization points
+	EXPECT(assert_equal(exp_values, actual1.linearizationPoint(), tol));
+	EXPECT(assert_equal(exp_values, actual2.linearizationPoint(), tol));
+}
+
+/* ************************************************************************* */
+int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
+/* ************************************************************************* */