2 BIG changes:

(1) FactorGraph and NonlinearOptimizer now no longer have a .cpp file, but a -inl.h file as in [http://google-styleguide.googlecode.com/svn/trunk/cppguide.xml Google's C++ Style Guide]. This means if you expect to instantiate one of the functions in a cpp file, you have to include the -inl.h file.
(1) getOrdering is now in FactorGraph, and the non-linear version does *not* take a config anymore. 
Long version: I made this change because colamd works on the graph structure alone, and should not depend on the type of graph. Instead, because getOrdering happened to implemented in LinearFactorGraph first, the non-linear version converted to a linear factor graph (at the cost of an unnecessary linearization), and then threw all that away to call colamd. To implement this in a key-neutral way (a hidden agenda), i had to modify the keys_ type to a list, so a lot of changes resulted from that.

.cproject

@@ -300,6 +300,7 @@
 <buildTargets>
 <target name="check" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 <buildCommand>make</buildCommand>
+<buildArguments></buildArguments>
 <buildTarget>check</buildTarget>
 <stopOnError>true</stopOnError>
 <useDefaultCommand>true</useDefaultCommand>
@@ -307,7 +308,6 @@
 </target>
 <target name="testSimpleCamera.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 <buildCommand>make</buildCommand>
-<buildArguments></buildArguments>
 <buildTarget>testSimpleCamera.run</buildTarget>
 <stopOnError>true</stopOnError>
 <useDefaultCommand>true</useDefaultCommand>
@@ -315,7 +315,6 @@
 </target>
 <target name="testCal3_S2.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 <buildCommand>make</buildCommand>
-<buildArguments></buildArguments>
 <buildTarget>testCal3_S2.run</buildTarget>
 <stopOnError>true</stopOnError>
 <useDefaultCommand>true</useDefaultCommand>
@@ -323,6 +322,7 @@
 </target>
 <target name="testVSLAMFactor.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 <buildCommand>make</buildCommand>
+<buildArguments></buildArguments>
 <buildTarget>testVSLAMFactor.run</buildTarget>
 <stopOnError>true</stopOnError>
 <useDefaultCommand>true</useDefaultCommand>
@@ -330,7 +330,6 @@
 </target>
 <target name="testCalibratedCamera.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 <buildCommand>make</buildCommand>
-<buildArguments></buildArguments>
 <buildTarget>testCalibratedCamera.run</buildTarget>
 <stopOnError>true</stopOnError>
 <useDefaultCommand>true</useDefaultCommand>
@@ -338,6 +337,7 @@
 </target>
 <target name="testConditionalGaussian.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 <buildCommand>make</buildCommand>
+<buildArguments></buildArguments>
 <buildTarget>testConditionalGaussian.run</buildTarget>
 <stopOnError>true</stopOnError>
 <useDefaultCommand>true</useDefaultCommand>
@@ -345,7 +345,6 @@
 </target>
 <target name="testPose2.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 <buildCommand>make</buildCommand>
-<buildArguments></buildArguments>
 <buildTarget>testPose2.run</buildTarget>
 <stopOnError>true</stopOnError>
 <useDefaultCommand>true</useDefaultCommand>
@@ -353,6 +352,7 @@
 </target>
 <target name="testFGConfig.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 <buildCommand>make</buildCommand>
+<buildArguments></buildArguments>
 <buildTarget>testFGConfig.run</buildTarget>
 <stopOnError>true</stopOnError>
 <useDefaultCommand>true</useDefaultCommand>
@@ -360,7 +360,6 @@
 </target>
 <target name="testRot3.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 <buildCommand>make</buildCommand>
-<buildArguments></buildArguments>
 <buildTarget>testRot3.run</buildTarget>
 <stopOnError>true</stopOnError>
 <useDefaultCommand>true</useDefaultCommand>
@@ -368,6 +367,7 @@
 </target>
 <target name="testNonlinearOptimizer.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 <buildCommand>make</buildCommand>
+<buildArguments></buildArguments>
 <buildTarget>testNonlinearOptimizer.run</buildTarget>
 <stopOnError>true</stopOnError>
 <useDefaultCommand>true</useDefaultCommand>
@@ -375,14 +375,22 @@
 </target>
 <target name="testLinearFactor.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 <buildCommand>make</buildCommand>
-<buildArguments></buildArguments>
 <buildTarget>testLinearFactor.run</buildTarget>
 <stopOnError>true</stopOnError>
 <useDefaultCommand>true</useDefaultCommand>
 <runAllBuilders>true</runAllBuilders>
 </target>
+<target name="testLinearFactorGraph.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+<buildCommand>make</buildCommand>
+<buildArguments></buildArguments>
+<buildTarget>testLinearFactorGraph.run</buildTarget>
+<stopOnError>true</stopOnError>
+<useDefaultCommand>true</useDefaultCommand>
+<runAllBuilders>true</runAllBuilders>
+</target>
 <target name="install" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 <buildCommand>make</buildCommand>
+<buildArguments></buildArguments>
 <buildTarget>install</buildTarget>
 <stopOnError>true</stopOnError>
 <useDefaultCommand>true</useDefaultCommand>
@@ -390,6 +398,7 @@
 </target>
 <target name="clean" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 <buildCommand>make</buildCommand>
+<buildArguments></buildArguments>
 <buildTarget>clean</buildTarget>
 <stopOnError>true</stopOnError>
 <useDefaultCommand>true</useDefaultCommand>
@@ -397,6 +406,7 @@
 </target>
 <target name="check" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 <buildCommand>make</buildCommand>
+<buildArguments></buildArguments>
 <buildTarget>check</buildTarget>
 <stopOnError>true</stopOnError>
 <useDefaultCommand>true</useDefaultCommand>

cpp/ConstrainedLinearFactorGraph.cpp

@@ -7,6 +7,7 @@
 
 #include <iostream>
 #include "ConstrainedLinearFactorGraph.h"
+#include "FactorGraph-inl.h" // for getOrdering
 
 using namespace std;
 
@@ -181,14 +182,10 @@ void ConstrainedLinearFactorGraph::eq_combine_and_eliminate(
 	lf->set_b(b);
 	string j; Matrix A;
 	LinearFactor::const_iterator it = joint_factor.begin();
-	for (; it != joint_factor.end(); it++)
+	for (; it != joint_factor.end(); it++) {
-	{
 		j = it->first;
 		A = it->second;
-		if (j != key)
+		if (j != key)	lf->insert(j, A);
-		{
-			lf->insert(j, A);
-		}
 	}
 
 	// insert factor
@@ -199,9 +196,7 @@ Ordering ConstrainedLinearFactorGraph::getOrdering() const
 {
 	Ordering ord = LinearFactorGraph::getOrdering();
 	BOOST_FOREACH(EqualityFactor::shared_ptr e, eq_factors)
-	{
 		ord.push_back(e->get_key());
-	}
 	return ord;
 }
 
@@ -209,9 +204,7 @@ LinearFactorGraph ConstrainedLinearFactorGraph::convert() const
 {
 	LinearFactorGraph ret;
 	BOOST_FOREACH(LinearFactor::shared_ptr f, factors_)
-	{
 		ret.push_back(f);
-	}
 	return ret;
 }
 

cpp/DeltaFunction.cpp

@@ -18,12 +18,12 @@ DeltaFunction::DeltaFunction() {
 }
 
 DeltaFunction::DeltaFunction(const Vector& v, const std::string& id)
-: value(v), key(id)
+: value_(v), key_(id)
 {
 }
 
 DeltaFunction::DeltaFunction(const DeltaFunction& df)
-: boost::noncopyable(), value(df.value), key(df.key)
+: boost::noncopyable(), value_(df.value_), key_(df.key_)
 {
 }
 
@@ -33,13 +33,13 @@ DeltaFunction::~DeltaFunction() {
 
 bool DeltaFunction::equals(const DeltaFunction &df) const
 {
-	return equal_with_abs_tol(value, df.value) && key == df.key;
+	return equal_with_abs_tol(value_, df.value_) && key_ == df.key_;
 }
 
 void DeltaFunction::print() const
 {
-	cout << "DeltaFunction: [" << key << "]";
+	cout << "DeltaFunction: [" << key_ << "]";
-	gtsam::print(value);
+	gtsam::print(value_);
 	cout << endl;
 }
 

cpp/DeltaFunction.h

@@ -17,8 +17,8 @@ namespace gtsam {
 
 class DeltaFunction : boost::noncopyable {
 protected:
-	Vector value; /// location of the delta function
+	Vector value_; /// location of the delta function
-	std::string key; /// id of node with delta function
+	std::string key_; /// id of node with delta function
 
 public:
 	typedef boost::shared_ptr<DeltaFunction> shared_ptr;
@@ -43,8 +43,8 @@ public:
 	/**
 	 * basic get functions
 	 */
-	Vector get_value() const {return value;}
+	Vector get_value() const {return value_;}
-	std::string get_key() const {return key;}
+	std::string get_key() const {return key_;}
 
 
 	/** equals function */

cpp/EqualityFactor.cpp

@@ -12,15 +12,21 @@ namespace gtsam {
 using namespace std;
 
 EqualityFactor::EqualityFactor()
-: key(""), value(Vector(0))
+: key_(""), value_(Vector(0))
 {
 }
 
 EqualityFactor::EqualityFactor(const Vector& constraint, const std::string& id)
-: key(id), value(constraint)
+: key_(id), value_(constraint)
 {
 }
 
+list<string> EqualityFactor::keys() const {
+	list<string> keys;
+	keys.push_back(key_);
+	return keys;
+}
+
 double EqualityFactor::error(const FGConfig& c) const
 {
 	return 0.0;
@@ -33,7 +39,7 @@ void EqualityFactor::print(const string& s) const
 
 bool EqualityFactor::equals(const EqualityFactor& f, double tol) const
 {
-	return equal_with_abs_tol(value, f.get_value(), tol) && key == f.get_key();
+	return equal_with_abs_tol(value_, f.get_value(), tol) && key_ == f.get_key();
 }
 
 bool EqualityFactor::equals(const Factor& f, double tol) const
@@ -45,19 +51,19 @@ bool EqualityFactor::equals(const Factor& f, double tol) const
 
 string EqualityFactor::dump() const
 {
-	string ret = "[" + key + "] " + gtsam::dump(value);
+	string ret = "[" + key_ + "] " + gtsam::dump(value_);
 	return ret;
 }
 
 DeltaFunction::shared_ptr EqualityFactor::getDeltaFunction() const
 {
-	DeltaFunction::shared_ptr ret(new DeltaFunction(value, key));
+	DeltaFunction::shared_ptr ret(new DeltaFunction(value_, key_));
 	return ret;
 }
 
 EqualityFactor::shared_ptr EqualityFactor::linearize() const
 {
-	EqualityFactor::shared_ptr ret(new EqualityFactor(zero(value.size()), key));
+	EqualityFactor::shared_ptr ret(new EqualityFactor(zero(value_.size()), key_));
 	return ret;
 }
 

cpp/EqualityFactor.h

@@ -19,8 +19,8 @@ public:
 
 
 protected:
-	Vector value; /// forces a variable be equal to this value
+	Vector value_; /// forces a variable be equal to this value
-	std::string key; /// name of variable factor is attached to
+	std::string key_; /// name of variable factor is attached to
 
 public:
 	/**
@@ -68,8 +68,13 @@ public:
     std::string dump() const;
 
     // get functions
-    std::string get_key() const {return key;}
+    std::string get_key() const {return key_;}
-    Vector get_value() const {return value;}
+    Vector get_value() const {return value_;}
+
+    /**
+     * return keys in preferred order
+     */
+    std::list<std::string> keys() const;
 
     /**
      * @return the number of nodes the factor connects

cpp/Factor.h

@@ -10,6 +10,7 @@
 #pragma once
 
 #include <set>
+#include <list>
 #include "FGConfig.h"
 
 namespace gtsam {
@@ -61,6 +62,11 @@ namespace gtsam {
     virtual bool equals(const Factor& f, double tol=1e-9) const = 0;
 
     virtual std::string dump() const = 0;
+
+    /**
+     * return keys in preferred order
+     */
+    virtual std::list<std::string> keys() const = 0;
 		
     /** 
      * @return the number of nodes the factor connects

cpp/FactorGraph-inl.h

@@ -0,0 +1,97 @@
+/**
+ * @file   FactorGraph-inl.h
+ * This is a template definition file, include it where needed (only!)
+ * so that the appropriate code is generated and link errors avoided.
+ * @brief  Factor Graph Base Class
+ * @author Carlos Nieto
+ * @author Frank Dellaert
+ * @author Alireza Fathi
+ */
+
+#pragma once
+
+#include <boost/foreach.hpp>
+#include <boost/tuple/tuple.hpp>
+#include "Ordering.h"
+#include "FactorGraph.h"
+
+#include <colamd/colamd.h>
+
+namespace gtsam {
+
+/* ************************************************************************* */
+
+/**
+ * Call colamd given a column-major symbolic matrix A
+ * @param n_col colamd arg 1: number of rows in A
+ * @param n_row colamd arg 2: number of columns in A
+ * @param nrNonZeros number of non-zero entries in A
+ * @param columns map from keys to a sparse column of non-zero row indices
+ */
+template <class Key>
+Ordering colamd(int n_col, int n_row, int nrNonZeros, const std::map<Key, std::vector<int> >& columns) {
+
+	// Convert to compressed column major format colamd wants it in (== MATLAB format!)
+	std::vector<Key> initialOrder;
+	int Alen = nrNonZeros*30;     /* colamd arg 3: size of the array A TODO: use Tim's function ! */
+	int * A = new int[Alen];      /* colamd arg 4: row indices of A, of size Alen */
+	int * p = new int[n_col + 1]; /* colamd arg 5: column pointers of A, of size n_col+1 */
+
+	p[0] = 0;
+	int j = 1;
+	int count = 0;
+	typedef typename std::map<Key, std::vector<int> >::const_iterator iterator;
+	for(iterator it = columns.begin(); it != columns.end(); it++) {
+		const Key& key = it->first;
+		const std::vector<int>& column = it->second;
+		initialOrder.push_back(key);
+		BOOST_FOREACH(int i, column) A[count++] = i; // copy sparse column
+		p[j] = count; // column j (base 1) goes from A[j-1] to A[j]-1
+		j+=1;
+	}
+
+	double* knobs = NULL;    /* colamd arg 6: parameters (uses defaults if NULL) */
+	int stats[COLAMD_STATS]; /* colamd arg 7: colamd output statistics and error codes */
+
+	// call colamd, result will be in p *************************************************
+	/* TODO: returns (1) if successful, (0) otherwise*/
+	::colamd(n_row, n_col, Alen, A, p, knobs, stats);
+	// **********************************************************************************
+	delete [] A; // delete symbolic A
+
+	// Convert elimination ordering in p to an ordering
+	Ordering result;
+	for(int j = 0; j < n_col; j++)
+		result.push_back(initialOrder[j]);
+	delete [] p; // delete colamd result vector
+
+	return result;
+}
+
+/* ************************************************************************* */
+template<class Factor>
+Ordering FactorGraph<Factor>::getOrdering() const {
+
+	// A factor graph is really laid out in row-major format, each factor a row
+	// Below, we compute a symbolic matrix stored in sparse columns.
+	typedef std::string Key;                  // default case  with string keys
+	std::map<Key, std::vector<int> > columns; // map from keys to a sparse column of non-zero row indices
+	int nrNonZeros = 0;             // number of non-zero entries
+	int n_row = factors_.size();    /* colamd arg 1: number of rows in A */
+
+	// loop over all factors = rows
+	for (int i = 0; i < n_row; i++) {
+		std::list<Key> keys = factors_[i]->keys();
+		BOOST_FOREACH(Key key, keys) columns[key].push_back(i);
+		nrNonZeros+= keys.size();
+	}
+	int n_col = (int)(columns.size()); /* colamd arg 2: number of columns in A */
+
+	if(n_col == 0)
+		return Ordering(); // empty ordering
+	else
+		return colamd(n_col, n_row, nrNonZeros, columns);
+}
+
+/* ************************************************************************* */
+}

cpp/FactorGraph.cpp

@@ -1,9 +0,0 @@
-/**
- * @file   FactorGraph.cpp
- * @brief  Factor Graph Base Class
- * @author Carlos Nieto
- */
-
-#include "FactorGraph.h"
-
-

cpp/FactorGraph.h

@@ -18,13 +18,15 @@
 
 namespace gtsam {
 
+	class Ordering;
+
 	/**
 	 * A factor graph is a bipartite graph with factor nodes connected to variable nodes.
 	 * In this class, however, only factor nodes are kept around.
 	 */
-	template<class T> class FactorGraph {
+	template<class Factor> class FactorGraph {
 	public:
-		typedef typename boost::shared_ptr<T> shared_factor;
+		typedef typename boost::shared_ptr<Factor> shared_factor;
 		typedef typename std::vector<shared_factor>::iterator iterator;
 		typedef typename std::vector<shared_factor>::const_iterator const_iterator;
 
@@ -103,6 +105,11 @@ namespace gtsam {
 			return false;
 		}
 
+		/**
+		 * Compute colamd ordering
+		 */
+		Ordering getOrdering() const;
+
 	private:
 
 		/** Serialization function */

cpp/LinearFactor.cpp

@@ -77,11 +77,11 @@ bool LinearFactor::equals(const Factor& f, double tol) const {
 }
 
 /* ************************************************************************* */
-set<string> LinearFactor::keys() const {
+list<string> LinearFactor::keys() const {
-	set<string> result;
+	list<string> result;
   string j; Matrix A;
   FOREACH_PAIR(j,A,As)
-    result.insert(j);
+    result.push_back(j);
   return result;
 }
 

cpp/LinearFactor.h

@@ -148,7 +148,7 @@ public:
 	 * Find all variables
 	 * @return The set of all variable keys
 	 */
-	std::set<std::string> keys() const;
+	 std::list<std::string> keys() const;
 
 	/**
 	 * Find all variables and their dimensions

cpp/LinearFactorGraph.cpp

@@ -228,77 +228,4 @@ pair<Matrix,Vector> LinearFactorGraph::matrix(const Ordering& ordering) const {
 	return lf.matrix(ordering);
 }
 
-/* ************************************************************************* */  
-
-/**
- * Call colamd given a column-major symbolic matrix A
- * @param n_col colamd arg 1: number of rows in A
- * @param n_row colamd arg 2: number of columns in A
- * @param nrNonZeros number of non-zero entries in A
- * @param columns map from keys to a sparse column of non-zero row indices
- */
-template <class Key>
-Ordering colamd(int n_col, int n_row, int nrNonZeros, const map<Key, vector<int> >& columns) {
-
-	// Convert to compressed column major format colamd wants it in (== MATLAB format!)
-	vector<Key> initialOrder;
-	int Alen = nrNonZeros*30;     /* colamd arg 3: size of the array A TODO: use Tim's function ! */
-	int * A = new int[Alen];      /* colamd arg 4: row indices of A, of size Alen */
-	int * p = new int[n_col + 1]; /* colamd arg 5: column pointers of A, of size n_col+1 */
-
-	p[0] = 0;
-	int j = 1;
-	int count = 0;
-	typedef typename map<Key, vector<int> >::const_iterator iterator;
-	for(iterator it = columns.begin(); it != columns.end(); it++) {
-		const Key& key = it->first;
-		const vector<int>& column = it->second;
-		initialOrder.push_back(key);
-		BOOST_FOREACH(int i, column) A[count++] = i; // copy sparse column
-		p[j] = count; // column j (base 1) goes from A[j-1] to A[j]-1
-		j+=1;
-	}
-
-	double* knobs = NULL;    /* colamd arg 6: parameters (uses defaults if NULL) */
-	int stats[COLAMD_STATS]; /* colamd arg 7: colamd output statistics and error codes */
-
-	// call colamd, result will be in p *************************************************
-	/* TODO: returns (1) if successful, (0) otherwise*/
-	colamd(n_row, n_col, Alen, A, p, knobs, stats);
-	// **********************************************************************************
-	delete [] A; // delete symbolic A
-
-	// Convert elimination ordering in p to an ordering
-	Ordering result;
-	for(int j = 0; j < n_col; j++)
-		result.push_back(initialOrder[j]);
-	delete [] p; // delete colamd result vector
-
-	return result;
-}
-
-/* ************************************************************************* */
-Ordering LinearFactorGraph::getOrdering() const {
-
-	// A factor graph is really laid out in row-major format, each factor a row
-	// Below, we compute a symbolic matrix stored in sparse columns.
-	typedef string Key;             // default case  with string keys
-	map<Key, vector<int> > columns; // map from keys to a sparse column of non-zero row indices
-	int nrNonZeros = 0;             // number of non-zero entries
-	int n_row = factors_.size();    /* colamd arg 1: number of rows in A */
-
-	// loop over all factors = rows
-	for (int i = 0; i < n_row; i++) {
-		set<Key> keys = factors_[i]->keys();
-		BOOST_FOREACH(Key key, keys) columns[key].push_back(i);
-		nrNonZeros+= keys.size();
-	}
-	int n_col = (int)(columns.size()); /* colamd arg 2: number of columns in A */
-
-	if(n_col == 0)
-		return Ordering(); // empty ordering
-	else
-		return colamd(n_col, n_row, nrNonZeros, columns);
-}
-
 /* ************************************************************************* */

cpp/LinearFactorGraph.h

@@ -40,12 +40,6 @@ namespace gtsam {
      */
     void setCBN(const ChordalBayesNet& CBN);
 
-    /**
-     * This function returns the best ordering for this linear factor
-     * graph, computed using colamd
-     */
-    Ordering getOrdering() const;
-
     /**
      * find the separator, i.e. all the nodes that have at least one
      * common factor with the given node. FD: not used AFAIK.

cpp/Makefile.am

@@ -80,7 +80,7 @@ timeLinearFactor: CXXFLAGS += -I /opt/local/include
 timeLinearFactor: LDFLAGS += -L.libs -lgtsam
 
 # graphs 
-sources += ConstrainedChordalBayesNet.cpp ChordalBayesNet.cpp FactorGraph.cpp 
+sources += ConstrainedChordalBayesNet.cpp ChordalBayesNet.cpp
 sources += LinearFactorGraph.cpp NonlinearFactorGraph.cpp
 sources += ConstrainedNonlinearFactorGraph.cpp ConstrainedLinearFactorGraph.cpp 
 check_PROGRAMS += testChordalBayesNet testConstrainedChordalBayesNet testFactorgraph 
@@ -146,9 +146,11 @@ testVSLAMFactor_LDADD = libgtsam.la
 
 # The header files will be installed in ~/include/gtsam
 headers = gtsam.h Value.h Testable.h Factor.h LinearFactorSet.h Point2Prior.h 
-headers += Simulated2DOdometry.h Simulated2DMeasurement.h smallExample.h 
+headers += Simulated2DOdometry.h Simulated2DMeasurement.h smallExample.h
-headers += NonlinearOptimizer.h NonlinearOptimizer.cpp # template ! 
 headers += $(sources:.cpp=.h)
+# templates: 
+headers += FactorGraph.h FactorGraph-inl.h
+headers += NonlinearOptimizer.h NonlinearOptimizer-inl.h 
 
 # create both dynamic and static libraries
 AM_CXXFLAGS = -I$(boost) -fPIC

cpp/NonlinearFactor.cpp

@@ -14,11 +14,8 @@ using namespace std;
 using namespace gtsam;
 
 /* ************************************************************************* */
-NonlinearFactor::NonlinearFactor(const Vector& z,	                
+NonlinearFactor::NonlinearFactor(const Vector& z, const double sigma) :
-                                 const double sigma) 
+	z_(z), sigma_(sigma) {
-{ 
-  z_ =  z; 
-  sigma_ = sigma;
 }
 
 /* ************************************************************************* */

cpp/NonlinearFactor.h

@@ -15,7 +15,7 @@
 #include <list>
 
 #include <boost/shared_ptr.hpp>
-#include <boost/serialization/list.hpp>
+#include <boost/serialization/set.hpp>
 
 #include "Factor.h"
 #include "Matrix.h"
@@ -63,10 +63,9 @@ namespace gtsam {
     virtual void print(const std::string& s = "") const = 0;
 
     /** get functions */
-    double get_sigma() const {return sigma_;}
+    double sigma() const {return sigma_;}
-    Vector get_measurement() const {return z_;}
+    Vector measurement() const {return z_;}
-    const std::list<std::string>& get_keys() const {return keys_;}
+    std::list<std::string> keys() const {return keys_;}
-    void set_keys(std::list<std::string> keys) {keys_ = keys;}
 
     /** calculate the error of the factor */
     double error(const FGConfig& c) const {
@@ -106,8 +105,15 @@ namespace gtsam {
    * Specialized derived classes could do this
   */
   class NonlinearFactor1 : public NonlinearFactor {
+  private:
+
+		std::string key1_;
+
   public:
 
+		Vector (*h_)(const Vector&);
+		Matrix (*H_)(const Vector&);
+
     /** Constructor */
     NonlinearFactor1(const Vector& z,		  // measurement
 		     const double sigma,	  // variance
@@ -117,10 +123,6 @@ namespace gtsam {
 
     void print(const std::string& s = "") const;
 
-    Vector (*h_)(const Vector&);
-    std::string key1_;
-    Matrix (*H_)(const Vector&);
-
     /** error function */
     inline Vector error_vector(const FGConfig& c) const { 
       return z_ - h_(c[key1_]);
@@ -141,8 +143,18 @@ namespace gtsam {
 	 * Specialized derived classes could do this
 	*/
   class NonlinearFactor2 : public NonlinearFactor {
+
+  private:
+
+    std::string key1_;
+    std::string key2_;
+
   public:
 
+  	Vector (*h_)(const Vector&, const Vector&);
+    Matrix (*H1_)(const Vector&, const Vector&);
+    Matrix (*H2_)(const Vector&, const Vector&);
+
     /** Constructor */
     NonlinearFactor2(const Vector& z,	                         // the measurement
 		     const double sigma,	                 // the variance
@@ -154,12 +166,6 @@ namespace gtsam {
 			
     void print(const std::string& s = "") const;
 
-    Vector (*h_)(const Vector&, const Vector&);
-    std::string key1_;
-    Matrix (*H1_)(const Vector&, const Vector&);
-    std::string key2_;
-    Matrix (*H2_)(const Vector&, const Vector&);
-		
     /** error function */
     inline Vector error_vector(const FGConfig& c) const { 
       return z_ - h_(c[key1_], c[key2_]); 

cpp/NonlinearFactorGraph.cpp

@@ -66,13 +66,6 @@ namespace gtsam {
 		return converged;
 	}
 
-	/* ************************************************************************* */
-	Ordering NonlinearFactorGraph::getOrdering(const FGConfig& config) const {
-		// TODO: FD: Whoa! This is crazy !!!!! re-linearizing just to get ordering ?
-		LinearFactorGraph lfg = linearize(config);
-		return lfg.getOrdering();
-	}
-
 	/* ************************************************************************* */
 	bool NonlinearFactorGraph::check_convergence(const FGConfig& config1,
 			const FGConfig& config2, double relativeErrorTreshold,

cpp/NonlinearFactorGraph.h

@@ -24,11 +24,6 @@ typedef FactorGraph<NonlinearFactor> BaseFactorGraph;
 /** Factor Graph consisting of non-linear factors */
 class NonlinearFactorGraph : public BaseFactorGraph
 {
-public:
-	// internal, exposed for testing only, doc in .cpp file
-
-	Ordering getOrdering(const FGConfig& config) const;
-
 public: // these you will probably want to use
 	/**
 	 * linearize a non linear factor

cpp/NonlinearOptimizer-inl.h

@@ -1,10 +1,14 @@
 /**
- * NonlinearOptimizer.cpp
+ * NonlinearOptimizer-inl.h
+ * This is a template definition file, include it where needed (only!)
+ * so that the appropriate code is generated and link errors avoided.
  * @brief: Encapsulates nonlinear optimization state
  * @Author: Frank Dellaert
  * Created on: Sep 7, 2009
  */
 
+#pragma once
+
 #include <iostream>
 #include <boost/tuple/tuple.hpp>
 #include "NonlinearOptimizer.h"

cpp/Point2Prior.h

@@ -6,9 +6,11 @@
 
 namespace gtsam {
 
-  struct Point2Prior : public NonlinearFactor1 {
+	class Point2Prior: public NonlinearFactor1 {
-  Point2Prior(const Vector& mu, double sigma, const std::string& key):
+	public:
-    NonlinearFactor1(mu, sigma, prior, key, Dprior) {}
+		Point2Prior(const Vector& mu, double sigma, const std::string& key) :
-  };
+			NonlinearFactor1(mu, sigma, prior, key, Dprior) {
+		}
+	};
 
 } // namespace gtsam

cpp/VSLAMFactor.cpp

@@ -85,11 +85,10 @@ string VSLAMFactor::dump() const
 {
   int i = getCameraFrameNumber();
   int j = getLandmarkNumber();
-  double sigma = get_sigma();
+  Vector z = measurement();
-  Vector z = get_measurement();
   char buffer[200];
   buffer[0] = 0;
-  sprintf(buffer, "1 %d %d %f %d", i, j , sigma, z.size());
+  sprintf(buffer, "1 %d %d %f %d", i, j , sigma(), z.size());
   for(size_t i = 0; i < z.size(); i++)
     sprintf(buffer, "%s %f", buffer, z(i));
   sprintf(buffer, "%s %s", buffer, K_.dump().c_str());

cpp/smallExample.cpp

@@ -14,6 +14,7 @@
 
 using namespace std;
 
+#include "Ordering.h"
 #include "Matrix.h"
 #include "NonlinearFactor.h"
 #include "EqualityFactor.h"

cpp/testLinearFactor.cpp

@@ -48,9 +48,9 @@ TEST( LinearFactor, keys )
   // get the factor "f2" from the small linear factor graph
   LinearFactorGraph fg = createLinearFactorGraph();
   LinearFactor::shared_ptr lf = fg[1];
-  set<string> expected;
+  list<string> expected;
-  expected.insert("x1");
+  expected.push_back("x1");
-  expected.insert("x2");
+  expected.push_back("x2");
   CHECK(lf->keys() == expected);
 }
 

cpp/testLinearFactorGraph.cpp

@@ -13,6 +13,7 @@ using namespace std;
 #include <boost/tuple/tuple.hpp>
 #include "Matrix.h"
 #include "smallExample.h"
+#include "FactorGraph-inl.h" // template definitions
 
 using namespace gtsam;
 

cpp/testNonlinearOptimizer.cpp

@@ -11,7 +11,8 @@ using namespace std;
 
 #include "Matrix.h"
 #include "smallExample.h"
-#include "NonlinearOptimizer.cpp"
+// template definitions
+#include "NonlinearOptimizer-inl.h"
 
 using namespace gtsam;