From cf72788bea8c106275c45618ffcbe2b807f60175 Mon Sep 17 00:00:00 2001
From: Alex Cunningham <alexgcunningham@gmail.com>
Date: Wed, 7 Sep 2011 01:52:24 +0000
Subject: [PATCH] Added documentation for Simulated3D.h

---
 gtsam/slam/Simulated3D.h | 132 +++++++++++++++++++++++++--------------
 1 file changed, 86 insertions(+), 46 deletions(-)

diff --git a/gtsam/slam/Simulated3D.h b/gtsam/slam/Simulated3D.h
index 5689cefda..d8eac27b0 100644
--- a/gtsam/slam/Simulated3D.h
+++ b/gtsam/slam/Simulated3D.h
@@ -31,62 +31,102 @@
 namespace gtsam {
 namespace simulated3D {
 
-	typedef gtsam::TypedSymbol<Point3, 'x'> PoseKey;
-	typedef gtsam::TypedSymbol<Point3, 'l'> PointKey;
+/**
+ * This is a linear SLAM domain where both poses and landmarks are
+ * 3D points, without rotation. The structure and use is based on
+ * the simulated2D domain.
+ */
 
-	typedef LieValues<PoseKey> PoseValues;
-	typedef LieValues<PointKey> PointValues;
-	typedef TupleValues2<PoseValues, PointValues> Values;
+typedef gtsam::TypedSymbol<Point3, 'x'> PoseKey;
+typedef gtsam::TypedSymbol<Point3, 'l'> PointKey;
+
+typedef LieValues<PoseKey> PoseValues;
+typedef LieValues<PointKey> PointValues;
+typedef TupleValues2<PoseValues, PointValues> Values;
+
+/**
+ * Prior on a single pose
+ */
+Point3 prior(const Point3& x, boost::optional<Matrix&> H = boost::none);
+
+/**
+ * odometry between two poses
+ */
+Point3 odo(const Point3& x1, const Point3& x2,
+		boost::optional<Matrix&> H1 = boost::none,
+		boost::optional<Matrix&> H2 = boost::none);
+
+/**
+ *  measurement between landmark and pose
+ */
+Point3 mea(const Point3& x, const Point3& l,
+		boost::optional<Matrix&> H1 = boost::none,
+		boost::optional<Matrix&> H2 = boost::none);
+
+/**
+ * A prior factor on a single linear robot pose
+ */
+struct PointPrior3D: public NonlinearFactor1<Values, PoseKey> {
+
+	Point3 z_; ///< The prior pose value for the variable attached to this factor
 
 	/**
-	 * Prior on a single pose
+	 * Constructor for a prior factor
+	 * @param z is the measured/prior position for the pose
+	 * @param model is the measurement model for the factor (Dimension: 3)
+	 * @param j is the key for the pose
 	 */
-	Point3 prior(const Point3& x, boost::optional<Matrix&> H = boost::none);
-
-	/**
-	 * odometry between two poses
-	 */
-	Point3 odo(const Point3& x1, const Point3& x2,
-			boost::optional<Matrix&> H1 = boost::none,
-			boost::optional<Matrix&> H2 = boost::none);
-
-	/**
-	 *  measurement between landmark and pose
-	 */
-	Point3 mea(const Point3& x, const Point3& l,
-			boost::optional<Matrix&> H1 = boost::none,
-			boost::optional<Matrix&> H2 = boost::none);
-
-	struct PointPrior3D: public NonlinearFactor1<Values, PoseKey> {
-
-		Point3 z_;
-
-		PointPrior3D(const Point3& z,
-					const SharedNoiseModel& model, const PoseKey& j) :
+	PointPrior3D(const Point3& z,
+			const SharedNoiseModel& model, const PoseKey& j) :
 				NonlinearFactor1<Values, PoseKey> (model, j), z_(z) {
-			}
+	}
 
-		Vector evaluateError(const Point3& x, boost::optional<Matrix&> H =
-				boost::none) {
-			return (prior(x, H) - z_).vector();
-		}
-	};
+	/**
+	 * Evaluates the error at a given value of x,
+	 * with optional derivatives.
+	 * @param x is the current value of the variable
+	 * @param H is an optional Jacobian matrix (Dimension: 3x3)
+	 * @return Vector error between prior value and x (Dimension: 3)
+	 */
+	Vector evaluateError(const Point3& x, boost::optional<Matrix&> H =
+			boost::none) {
+		return (prior(x, H) - z_).vector();
+	}
+};
 
-	struct Simulated3DMeasurement: public NonlinearFactor2<Values,
-			PoseKey, PointKey> {
+/**
+ * Models a linear 3D measurement between 3D points
+ */
+struct Simulated3DMeasurement: public NonlinearFactor2<Values,
+PoseKey, PointKey> {
 
-		Point3 z_;
+	Point3 z_; ///< Linear displacement between a pose and landmark
 
-		Simulated3DMeasurement(const Point3& z,
-					const SharedNoiseModel& model, PoseKey& j1, PointKey j2) :
+	/**
+	 * Creates a measurement factor with a given measurement
+	 * @param z is the measurement, a linear displacement between poses and landmarks
+	 * @param model is a measurement model for the factor (Dimension: 3)
+	 * @param j1 is the pose key of the robot
+	 * @param j2 is the point key for the landmark
+	 */
+	Simulated3DMeasurement(const Point3& z,
+			const SharedNoiseModel& model, PoseKey& j1, PointKey j2) :
 				NonlinearFactor2<Values, PoseKey, PointKey> (
-								model, j1, j2), z_(z) {
-			}
+						model, j1, j2), z_(z) {
+	}
 
-		Vector evaluateError(const Point3& x1, const Point3& x2, boost::optional<
-				Matrix&> H1 = boost::none, boost::optional<Matrix&> H2 = boost::none) {
-			return (mea(x1, x2, H1, H2) - z_).vector();
-		}
-	};
+	/**
+	 * Error function with optional derivatives
+	 * @param x1 a robot pose value
+	 * @param x2 a landmark point value
+	 * @param H1 is an optional Jacobian matrix in terms of x1 (Dimension: 3x3)
+	 * @param H2 is an optional Jacobian matrix in terms of x2 (Dimension: 3x3)
+	 * @return vector error between measurement and prediction (Dimension: 3)
+	 */
+	Vector evaluateError(const Point3& x1, const Point3& x2, boost::optional<
+			Matrix&> H1 = boost::none, boost::optional<Matrix&> H2 = boost::none) {
+		return (mea(x1, x2, H1, H2) - z_).vector();
+	}
+};
 
 }} // namespace simulated3D