moved timing example here from EasySLAM

.cproject

@@ -458,6 +458,22 @@
 <useDefaultCommand>true</useDefaultCommand>
 <runAllBuilders>true</runAllBuilders>
 </target>
+<target name="timeLinearFactor.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+<buildCommand>make</buildCommand>
+<buildArguments/>
+<buildTarget>timeLinearFactor.run</buildTarget>
+<stopOnError>true</stopOnError>
+<useDefaultCommand>true</useDefaultCommand>
+<runAllBuilders>true</runAllBuilders>
+</target>
+<target name="timeLinearFactorGraph.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+<buildCommand>make</buildCommand>
+<buildArguments/>
+<buildTarget>timeLinearFactorGraph.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>
 <buildTarget>install</buildTarget>

cpp/Makefile.am

@@ -80,6 +80,11 @@ timeLinearFactor: timeLinearFactor.cpp
 timeLinearFactor: CXXFLAGS += -I /opt/local/include
 timeLinearFactor: LDFLAGS += -L.libs -lgtsam
 
+# not the correct way, I'm sure: Kai ?
+timeLinearFactorGraph: timeLinearFactorGraph.cpp  
+timeLinearFactorGraph: CXXFLAGS += -I /opt/local/include -I ..
+timeLinearFactorGraph: LDFLAGS += SmallExample.o -L.libs -lgtsam -L../CppUnitLite -lCppUnitLite
+
 # graphs 
 sources += ChordalBayesNet.cpp 
 sources += LinearFactorGraph.cpp 

cpp/smallExample.cpp

@@ -20,7 +20,10 @@ using namespace std;
 #include "Simulated2DOdometry.h"
 #include "Simulated2DMeasurement.h"
 #include "simulated2D.h"
+
+// template definitions
 #include "FactorGraph-inl.h"
+#include "NonlinearFactorGraph-inl.h"
 
 namespace gtsam {
 
@@ -236,6 +239,38 @@ ExampleNonlinearFactorGraph createReallyNonlinearFactorGraph() {
 	return *sharedReallyNonlinearFactorGraph();
 }
 
+/* ************************************************************************* */
+LinearFactorGraph createSmoother(int T) {
+	// Create
+	ExampleNonlinearFactorGraph nlfg;
+	VectorConfig poses;
+
+	// prior on x0
+	Vector x0 = zero(2);
+	string key0 = symbol('x', 0);
+	shared prior(new Point2Prior(x0, 1, key0));
+	nlfg.push_back(prior);
+	poses.insert(key0, x0);
+
+	for (int t = 1; t <= T; t++) {
+		// odometry between x_t and x_{t-1}
+		Vector odo = Vector_(2, 1.0, 0.0);
+		string key = symbol('x', t);
+		shared odometry(new Simulated2DOdometry(odo, 1, symbol('x', t - 1), key));
+		nlfg.push_back(odometry);
+
+		// measurement on x_t
+		double sigma3 = 0.2;
+		Vector z = Vector_(2, t, 0.0);
+		shared measurement(new Point2Prior(z, 1, key));
+		nlfg.push_back(measurement);
+		poses.insert(key, z);
+	}
+
+	LinearFactorGraph lfg = nlfg.linearize(poses);
+	return lfg;
+}
+
 /* ************************************************************************* */
 ConstrainedLinearFactorGraph createSingleConstraintGraph() {
 	// create unary factor

cpp/smallExample.h

@@ -69,6 +69,13 @@ namespace gtsam {
 	boost::shared_ptr<const ExampleNonlinearFactorGraph> sharedReallyNonlinearFactorGraph();
 	ExampleNonlinearFactorGraph createReallyNonlinearFactorGraph();
 
+	/**
+	 * Create a Kalman smoother by linearizing a non-linear factor graph
+	 * @param T number of time-steps
+	 */
+	LinearFactorGraph createSmoother(int T);
+
+
 	/* ******************************************************* */
 	// Constrained Examples
 	/* ******************************************************* */

cpp/testLinearFactorGraph.cpp

@@ -13,7 +13,9 @@ using namespace std;
 #include <boost/tuple/tuple.hpp>
 #include "Matrix.h"
 #include "smallExample.h"
-#include "FactorGraph-inl.h" // template definitions
+
+// template definitions
+#include "FactorGraph-inl.h"
 
 using namespace gtsam;
 
@@ -522,6 +524,15 @@ TEST( LinearFactorGraph, find_factors_and_remove__twice )
   LONGS_EQUAL(1,fg.size());
   }
 
+/* ************************************************************************* */
+TEST(timeLinearFactorGraph, createSmoother)
+{
+	LinearFactorGraph fg1 = createSmoother(1);
+	LONGS_EQUAL(3,fg1.size());
+	LinearFactorGraph fg2 = createSmoother(2);
+	LONGS_EQUAL(5,fg2.size());
+}
+
 /* ************************************************************************* */
 int main() { TestResult tr; return TestRegistry::runAllTests(tr);}
 /* ************************************************************************* */

cpp/timeLinearFactorGraph.cpp

@@ -0,0 +1,41 @@
+/**
+ * @file    timeLinearFactorGraph.cpp
+ * @brief   Time elimination with simple Kalman Smoothing example
+ * @author  Frank Dellaert
+ */
+
+#include <time.h>
+#include <CppUnitLite/TestHarness.h>
+#include "SmallExample.h"
+
+using namespace std;
+using namespace gtsam;
+
+/* ************************************************************************* */
+// Create a Kalman smoother for t=1:T and optimize
+double timeKalmanSmoother(int T) {
+	LinearFactorGraph smoother = createSmoother(T);
+	Ordering ordering;
+	for (int t = 0; t <= T; t++) ordering.push_back(symbol('x',t));
+	clock_t start = clock();
+	smoother.optimize(ordering);
+	clock_t end = clock ();
+	double dif = (double)(end - start) / CLOCKS_PER_SEC;
+	return dif;
+}
+
+/* ************************************************************************* */
+TEST(timeLinearFactorGraph, linearTime)
+{
+	double time1 = timeKalmanSmoother(1000); cout << time1 << endl;
+	double time2 = timeKalmanSmoother(2000); cout << time2 << endl;
+	DOUBLES_EQUAL(2*time1,time2,0.001);
+}
+
+/* ************************************************************************* */
+int main() {
+	TestResult tr;
+	TestRegistry::runAllTests(tr);
+	return 0;
+}
+/* ************************************************************************* */