From ff522a73c05c1f9a76b7bde2c3017e9a1e1eecb0 Mon Sep 17 00:00:00 2001 From: Stephen Williams Date: Sun, 22 Jul 2012 05:43:44 +0000 Subject: [PATCH] Updated documentation on SimpleRotation example --- examples/SimpleRotation.cpp | 54 ++++++++++++++++++++++++------------- 1 file changed, 35 insertions(+), 19 deletions(-) diff --git a/examples/SimpleRotation.cpp b/examples/SimpleRotation.cpp index 603d345bf..cff9d754e 100644 --- a/examples/SimpleRotation.cpp +++ b/examples/SimpleRotation.cpp @@ -17,20 +17,41 @@ * @author Alex Cunningham */ -#include -#include -#include + /** + * This example will perform a relatively trivial optimization on + * a single variable with a single factor. + */ + +// In this example, a 2D rotation will be used as the variable of interest #include -#include + +// Each variable in the system (poses) must be identified with a unique key. +// We can either use simple integer keys (1, 2, 3, ...) or symbols (X1, X2, L1). +// Here we will use symbols #include + +// In GTSAM, measurement functions are represented as 'factors'. Several common factors +// have been provided with the library for solving robotics/SLAM/Bundle Adjustment problems. +// We will apply a simple prior on the rotation +#include + +// When the factors are created, we will add them to a Factor Graph. As the factors we are using +// are nonlinear factors, we will need a Nonlinear Factor Graph. #include + +// The nonlinear solvers within GTSAM are iterative solvers, meaning they linearize the +// nonlinear functions around an initial linearization point, then solve the linear system +// to update the linearization point. This happens repeatedly until the solver converges +// to a consistent set of variable values. This requires us to specify an initial guess +// for each variable, held in a Values container. +#include + +// Finally, once all of the factors have been added to our factor graph, we will want to +// solve/optimize to graph to find the best (Maximum A Posteriori) set of variable values. +// GTSAM includes several nonlinear optimizers to perform this step. Here we will use the +// standard Levenberg-Marquardt solver #include -/* - * TODO: make factors independent of RotValues - * TODO: make toplevel documentation - * TODO: Clean up nonlinear optimization API - */ using namespace std; using namespace gtsam; @@ -40,12 +61,7 @@ const double degree = M_PI / 180; int main() { /** - * This example will perform a relatively trivial optimization on - * a single variable with a single factor. - */ - - /** - * Step 1: create a factor on to express a unary constraint + * Step 1: Create a factor to express a unary constraint * The "prior" in this case is the measurement from a sensor, * with a model of the noise on the measurement. * @@ -60,12 +76,12 @@ int main() { */ Rot2 prior = Rot2::fromAngle(30 * degree); prior.print("goal angle"); - SharedDiagonal model = noiseModel::Isotropic::Sigma(1, 1 * degree); + noiseModel::Isotropic::shared_ptr model = noiseModel::Isotropic::Sigma(1, 1 * degree); Symbol key('x',1); PriorFactor factor(key, prior, model); /** - * Step 2: create a graph container and add the factor to it + * Step 2: Create a graph container and add the factor to it * Before optimizing, all factors need to be added to a Graph container, * which provides the necessary top-level functionality for defining a * system of constraints. @@ -78,7 +94,7 @@ int main() { graph.print("full graph"); /** - * Step 3: create an initial estimate + * Step 3: Create an initial estimate * An initial estimate of the solution for the system is necessary to * start optimization. This system state is the "RotValues" structure, * which is similar in structure to a STL map, in that it maps @@ -98,7 +114,7 @@ int main() { initial.print("initial estimate"); /** - * Step 4: optimize + * Step 4: Optimize * After formulating the problem with a graph of constraints * and an initial estimate, executing optimization is as simple * as calling a general optimization function with the graph and