Cleaned up planar SLAM example and created new Localization example

examples/LocalizationExample.cpp

@@ -0,0 +1,63 @@
+/* ----------------------------------------------------------------------------
+
+ * 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 LocalizationExample.cpp
+ * @brief Simple robot localization example
+ * @author Frank Dellaert
+ */
+
+// pull in the 2D PoseSLAM domain with all typedefs and helper functions defined
+#include <gtsam/slam/pose2SLAM.h>
+
+using namespace std;
+using namespace gtsam;
+
+/**
+ * Example of a simple 2D localization example
+ *  - Robot poses are facing along the X axis (horizontal, to the right in 2D)
+ *  - The robot moves 2 meters each step
+ *  - We have full odometry between poses
+ */
+int main(int argc, char** argv) {
+
+  // create the graph (defined in pose2SLAM.h, derived from NonlinearFactorGraph)
+	pose2SLAM::Graph graph;
+
+	// add a Gaussian prior on pose x_1
+	Pose2 priorMean(0.0, 0.0, 0.0); // prior mean is at origin
+	SharedDiagonal priorNoise(Vector_(3, 0.3, 0.3, 0.1)); // 30cm std on x,y, 0.1 rad on theta
+	graph.addPrior(1, priorMean, priorNoise); // add directly to graph
+
+	// add two odometry factors
+	Pose2 odometry(2.0, 0.0, 0.0); // create a measurement for both factors (the same in this case)
+	SharedDiagonal odometryNoise(Vector_(3, 0.2, 0.2, 0.1)); // 20cm std on x,y, 0.1 rad on theta
+	graph.addOdometry(1, 2, odometry, odometryNoise);
+	graph.addOdometry(2, 3, odometry, odometryNoise);
+
+	// print
+	graph.print("\nFactor graph:\n");
+
+	// create (deliberatly inaccurate) initial estimate
+	pose2SLAM::Values initialEstimate;
+	initialEstimate.insertPose(1, Pose2(0.5, 0.0, 0.2));
+	initialEstimate.insertPose(2, Pose2(2.3, 0.1,-0.2));
+	initialEstimate.insertPose(3, Pose2(4.1, 0.1, 0.1));
+
+	initialEstimate.print("\nInitial estimate:\n  ");
+
+	// optimize using Levenberg-Marquardt optimization with an ordering from colamd
+	pose2SLAM::Values result = graph.optimize(initialEstimate);
+	result.print("\nFinal result:\n  ");
+
+	return 0;
+}
+

examples/PlanarSLAMExample_easy.cpp

@@ -15,64 +15,57 @@
  * @author Alex Cunningham
  */
 
-#include <cmath>
-#include <iostream>
-
 // pull in the planar SLAM domain with all typedefs and helper functions defined
 #include <gtsam/slam/planarSLAM.h>
-#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
 
 using namespace std;
 using namespace gtsam;
-using namespace planarSLAM;
 
 /**
- * In this version of the system we make the following assumptions:
+ * Example of a simple 2D planar slam example with landmarls
- *  - All values are axis aligned
+ *  - The robot and landmarks are on a 2 meter grid
- *  - Robot poses are facing along the X axis (horizontal, to the right in images)
+ *  - Robot poses are facing along the X axis (horizontal, to the right in 2D)
+ *  - The robot moves 2 meters each step
+ *  - We have full odometry between poses
  *  - We have bearing and range information for measurements
- *  - We have full odometry for measurements
- *  - The robot and landmarks are on a grid, moving 2 meters each step
  *  - Landmarks are 2 meters away from the robot trajectory
  */
 int main(int argc, char** argv) {
 
-  // create graph container and add factors to it
+  // create the graph (defined in planarSlam.h, derived from NonlinearFactorGraph)
-	Graph graph;
+	planarSLAM::Graph graph;
 
-	/* add prior  */
+	// add a Gaussian prior on pose x_1
-	// gaussian for prior
+	Pose2 priorMean(0.0, 0.0, 0.0); // prior mean is at origin
-	SharedDiagonal prior_model = noiseModel::Diagonal::Sigmas(Vector_(3, 0.3, 0.3, 0.1));
+	SharedDiagonal priorNoise(Vector_(3, 0.3, 0.3, 0.1)); // 30cm std on x,y, 0.1 rad on theta
-	Pose2 prior_measurement(0.0, 0.0, 0.0); // prior at origin
+	graph.addPrior(1, priorMean, priorNoise); // add directly to graph
-	graph.addPrior(1, prior_measurement, prior_model); // add directly to graph
 
-	/* add odometry */
+	// add two odometry factors
-	// general noisemodel for odometry
+	Pose2 odometry(2.0, 0.0, 0.0); // create a measurement for both factors (the same in this case)
-	SharedDiagonal odom_model = noiseModel::Diagonal::Sigmas(Vector_(3, 0.2, 0.2, 0.1));
+	SharedDiagonal odometryNoise(Vector_(3, 0.2, 0.2, 0.1)); // 20cm std on x,y, 0.1 rad on theta
-	Pose2 odom_measurement(2.0, 0.0, 0.0); // create a measurement for both factors (the same in this case)
+	graph.addOdometry(1, 2, odometry, odometryNoise);
-	graph.addOdometry(1, 2, odom_measurement, odom_model);
+	graph.addOdometry(2, 3, odometry, odometryNoise);
-	graph.addOdometry(2, 3, odom_measurement, odom_model);
 
-	/* add measurements */
+	// create a noise model for the landmark measurements
-	// general noisemodel for measurements
+	SharedDiagonal measurementNoise(Vector_(2, 0.1, 0.2)); // 0.1 rad std on bearing, 20cm on range
-	SharedDiagonal meas_model = noiseModel::Diagonal::Sigmas(Vector_(2, 0.1, 0.2));
 
 	// create the measurement values - indices are (pose id, landmark id)
 	Rot2 bearing11 = Rot2::fromDegrees(45),
-		 bearing21 = Rot2::fromDegrees(90),
+		   bearing21 = Rot2::fromDegrees(90),
-		 bearing32 = Rot2::fromDegrees(90);
+		   bearing32 = Rot2::fromDegrees(90);
 	double range11 = sqrt(4+4),
-		   range21 = 2.0,
+		     range21 = 2.0,
-		   range32 = 2.0;
+		     range32 = 2.0;
 
-	// create bearing/range factors and add them
+	// add bearing/range factors (created by "addBearingRange")
-	graph.addBearingRange(1, 1, bearing11, range11, meas_model);
+	graph.addBearingRange(1, 1, bearing11, range11, measurementNoise);
-	graph.addBearingRange(2, 1, bearing21, range21, meas_model);
+	graph.addBearingRange(2, 1, bearing21, range21, measurementNoise);
-	graph.addBearingRange(3, 2, bearing32, range32, meas_model);
+	graph.addBearingRange(3, 2, bearing32, range32, measurementNoise);
 
-	graph.print("full graph");
+	// print
+	graph.print("Factor graph");
 
-	// initialize to noisy points
+	// create (deliberatly inaccurate) initial estimate
 	planarSLAM::Values initialEstimate;
 	initialEstimate.insertPose(1, Pose2(0.5, 0.0, 0.2));
 	initialEstimate.insertPose(2, Pose2(2.3, 0.1,-0.2));
@@ -80,11 +73,11 @@ int main(int argc, char** argv) {
 	initialEstimate.insertPoint(1, Point2(1.8, 2.1));
 	initialEstimate.insertPoint(2, Point2(4.1, 1.8));
 
-	initialEstimate.print("initial estimate");
+	initialEstimate.print("Initial estimate:\n  ");
 
 	// optimize using Levenberg-Marquardt optimization with an ordering from colamd
-	planarSLAM::Values result = LevenbergMarquardtOptimizer(graph, initialEstimate).optimize();
+	planarSLAM::Values result = graph.optimize(initialEstimate);
-	result.print("final result");
+	result.print("Final result:\n  ");
 
 	return 0;
 }