From 4eab976e5c7ef00c5c3858a5f4af308b4ec78ce7 Mon Sep 17 00:00:00 2001
From: Chris Beall <chrisbeall@gmail.com>
Date: Thu, 14 Oct 2010 01:07:55 +0000
Subject: [PATCH] Basic Pose2SLAM example. Marginals still missing.

---
 examples/Makefile.am          |  1 +
 examples/Pose2SLAMExample.cpp | 77 +++++++++++++++++++++++++++++++++++
 2 files changed, 78 insertions(+)
 create mode 100644 examples/Pose2SLAMExample.cpp

diff --git a/examples/Makefile.am b/examples/Makefile.am
index ed746ccf0..fe1adefc9 100644
--- a/examples/Makefile.am
+++ b/examples/Makefile.am
@@ -13,6 +13,7 @@ check_PROGRAMS =
 # Examples
 noinst_PROGRAMS  = SimpleRotation      # Optimizes a single nonlinear rotation variable
 noinst_PROGRAMS += PlanarSLAMExample   # Solves SLAM example from tutorial by using planarSLAM
+noinst_PROGRAMS += Pose2SLAMExample    # Solves SLAM example from tutorial by using planarSLAM
 noinst_PROGRAMS += PlanarSLAMSelfContained   # Solves SLAM example from tutorial with all typedefs in the script
 
 #----------------------------------------------------------------------------------------------------
diff --git a/examples/Pose2SLAMExample.cpp b/examples/Pose2SLAMExample.cpp
new file mode 100644
index 000000000..7f4413482
--- /dev/null
+++ b/examples/Pose2SLAMExample.cpp
@@ -0,0 +1,77 @@
+/**
+ * @file Pose2SLAMExample.cpp
+ * @brief Simple Pose2SLAM Example using
+ * pre-built pose2SLAM domain
+ * @author Chris Beall
+ */
+
+#include <cmath>
+#include <iostream>
+#include <boost/shared_ptr.hpp>
+
+// pull in the Pose2 SLAM domain with all typedefs and helper functions defined
+#include <gtsam/slam/pose2SLAM.h>
+
+using namespace std;
+using namespace gtsam;
+using namespace boost;
+using namespace gtsam::pose2SLAM;
+
+int main(int argc, char** argv) {
+	// create keys for robot positions
+	Key x1(1), x2(2), x3(3);
+
+	/* 1. create graph container and add factors to it */
+	//Graph graph;
+	shared_ptr<Graph> graph(new Graph);
+
+	/* 2.a add prior  */
+	// gaussian for prior
+	SharedDiagonal prior_model = noiseModel::Diagonal::Sigmas(Vector_(3, 0.3, 0.3, 0.1));
+	Pose2 prior_measurement(0.0, 0.0, 0.0); // prior at origin
+	graph->addPrior(x1, prior_measurement, prior_model); // add directly to graph
+
+	/* 2.b add odometry */
+	// general noisemodel for odometry
+	SharedDiagonal odom_model = noiseModel::Diagonal::Sigmas(Vector_(3, 0.2, 0.2, 0.1));
+
+	/* Pose2 measurements take (x,y,theta), where theta is taken from the positive x-axis*/
+	Pose2 odom_measurement(2.0, 0.0, 0.0); // create a measurement for both factors (the same in this case)
+	graph->addConstraint(x1, x2, odom_measurement, odom_model);
+	graph->addConstraint(x2, x3, odom_measurement, odom_model);
+
+	graph->print("Full Graph");
+
+  /* 3. Create the data structure to hold the initial estinmate to the solution
+   * initialize to noisy points */
+	shared_ptr<Values> initialEstimate(new Values);
+	initialEstimate->insert(x1, Pose2(0.5, 0.0, 0.2));
+	initialEstimate->insert(x2, Pose2(2.3, 0.1,-0.2));
+	initialEstimate->insert(x3, Pose2(4.1, 0.1, 0.1));
+
+	initialEstimate->print("Initial Estimate");
+
+	/* There are several ways to solve the graph. */
+
+	/* 4.1 Single Step:
+	* optimize using Levenberg-Marquardt optimization with an ordering from colamd */
+  Optimizer::shared_values result = Optimizer::optimizeLM(*graph, *initialEstimate);
+  result->print("Final Result");
+
+	/* 4.2.1 Alternatively, you can go through the process step by step
+	 * Choose an ordering */
+	Ordering::shared_ptr ordering = graph->orderingCOLAMD(*initialEstimate).first;
+
+  /* 4.2.2 set up solver and optimize */
+	Optimizer::shared_solver solver(new Optimizer::solver(ordering));
+
+  Optimizer optimizer(graph, initialEstimate, solver);
+  Optimizer::verbosityLevel verbosity = pose2SLAM::Optimizer::SILENT;
+  Optimizer optimizer0 = optimizer.levenbergMarquardt(1e-15, 1e-15, verbosity);
+
+	Values result2 = *optimizer0.config();
+  result2.print("Final Result 2");
+
+	return 0;
+}
+