From 57452264523c1c37476bfe8b469f28a8cd27604b Mon Sep 17 00:00:00 2001
From: Alex Cunningham <alexgcunningham@gmail.com>
Date: Wed, 22 Sep 2010 15:34:03 +0000
Subject: [PATCH] split LinearApproxFactor into an implementation header file

---
 slam/LinearApproxFactor-inl.h    | 65 ++++++++++++++++++++++++++++++++
 slam/LinearApproxFactor.h        | 42 ++-------------------
 slam/Makefile.am                 |  2 +-
 tests/testLinearApproxFactor.cpp |  2 +-
 4 files changed, 71 insertions(+), 40 deletions(-)
 create mode 100644 slam/LinearApproxFactor-inl.h

diff --git a/slam/LinearApproxFactor-inl.h b/slam/LinearApproxFactor-inl.h
new file mode 100644
index 000000000..a701c79e3
--- /dev/null
+++ b/slam/LinearApproxFactor-inl.h
@@ -0,0 +1,65 @@
+/*
+ * @file LinearApproxFactor.h
+ * @brief A dummy factor that allows a linear factor to act as a nonlinear factor
+ * @author Alex Cunningham
+ */
+
+#pragma once
+
+#include <iostream>
+#include <boost/foreach.hpp>
+
+#include <gtsam/slam/LinearApproxFactor.h>
+
+namespace gtsam {
+
+/* ************************************************************************* */
+template <class Config, class Key>
+LinearApproxFactor<Config,Key>::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);
+	}
+  }
+
+/* ************************************************************************* */
+template <class Config, class Key>
+Vector LinearApproxFactor<Config,Key>::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 = linPt.logmap(newPt);
+		delta.insert(key, d);
+	}
+
+	return lin_factor_->unweighted_error(delta);
+}
+
+/* ************************************************************************* */
+template <class Config, class Key>
+boost::shared_ptr<GaussianFactor>
+LinearApproxFactor<Config,Key>::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));
+}
+
+/* ************************************************************************* */
+template <class Config, class Key>
+void LinearApproxFactor<Config,Key>::print(const std::string& s) const {
+	LinearApproxFactor<Config,Key>::Base::print(s);
+	lin_factor_->print();
+}
+
+} // \namespace gtsam
diff --git a/slam/LinearApproxFactor.h b/slam/LinearApproxFactor.h
index ac23183f2..98918b897 100644
--- a/slam/LinearApproxFactor.h
+++ b/slam/LinearApproxFactor.h
@@ -7,8 +7,6 @@
 #pragma once
 
 #include <vector>
-#include <iostream>
-#include <boost/foreach.hpp>
 #include <gtsam/nonlinear/NonlinearFactor.h>
 #include <gtsam/linear/VectorConfig.h>
 #include <gtsam/base/Matrix.h>
@@ -54,31 +52,12 @@ protected:
 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);
-		}
-	}
+	LinearApproxFactor(GaussianFactor::shared_ptr lin_factor, const Config& lin_points);
 
 	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 = linPt.logmap(newPt);
-			delta.insert(key, d);
-		}
-
-		return lin_factor_->unweighted_error(delta);
-	}
+	virtual Vector unwhitenedError(const Config& c) const;
 
 	/**
 	 * linearize to a GaussianFactor
@@ -87,26 +66,13 @@ public:
 	 * 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));
-	}
+	linearize(const Config& c) const;
 
 	/** 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();
-	}
+	virtual void print(const std::string& s="") const;
 
 	/** access to b vector of gaussian */
 	Vector get_b() const { return lin_factor_->get_b(); }
diff --git a/slam/Makefile.am b/slam/Makefile.am
index 07e8a244d..73a20710e 100644
--- a/slam/Makefile.am
+++ b/slam/Makefile.am
@@ -35,7 +35,7 @@ headers += BetweenFactor.h PriorFactor.h
 headers += BetweenConstraint.h BoundingConstraint.h TransformConstraint.h
 
 # Utility factors
-headers += LinearApproxFactor.h
+headers += LinearApproxFactor.h LinearApproxFactor-inl.h
 
 # 2D Pose SLAM
 sources += pose2SLAM.cpp Pose2SLAMOptimizer.cpp dataset.cpp
diff --git a/tests/testLinearApproxFactor.cpp b/tests/testLinearApproxFactor.cpp
index c6a1ef387..a7b63cf58 100644
--- a/tests/testLinearApproxFactor.cpp
+++ b/tests/testLinearApproxFactor.cpp
@@ -9,7 +9,7 @@
 #include <gtsam/base/TestableAssertions.h>
 #include <gtsam/linear/GaussianFactor.h>
 #include <gtsam/slam/planarSLAM.h>
-#include <gtsam/slam/LinearApproxFactor.h>
+#include <gtsam/slam/LinearApproxFactor-inl.h>
 
 using namespace std;
 using namespace gtsam;