From 9f51aad0fcca568d1df66fe8af3e7589ec01ded8 Mon Sep 17 00:00:00 2001 From: dellaert Date: Wed, 18 Feb 2015 12:02:33 +0100 Subject: [PATCH] Cleaned up comments and headers in examples --- examples/Pose2SLAMExampleExpressions.cpp | 10 ++-- examples/Pose2SLAMExample_graph.cpp | 9 ++-- examples/Pose2SLAMExample_graphviz.cpp | 4 +- examples/Pose2SLAMwSPCG.cpp | 69 ++++++------------------ examples/SFMExample.cpp | 8 +-- examples/SFMExample_SmartFactor.cpp | 41 +++----------- 6 files changed, 35 insertions(+), 106 deletions(-) diff --git a/examples/Pose2SLAMExampleExpressions.cpp b/examples/Pose2SLAMExampleExpressions.cpp index 92f94c3f3..1f6de6cb1 100644 --- a/examples/Pose2SLAMExampleExpressions.cpp +++ b/examples/Pose2SLAMExampleExpressions.cpp @@ -14,20 +14,18 @@ * @brief Expressions version of Pose2SLAMExample.cpp * @date Oct 2, 2014 * @author Frank Dellaert - * @author Yong Dian Jian */ // The two new headers that allow using our Automatic Differentiation Expression framework #include #include -// Header order is close to far -#include +// For an explanation of headers below, please see Pose2SLAMExample.cpp +#include +#include +#include #include #include -#include -#include -#include using namespace std; using namespace gtsam; diff --git a/examples/Pose2SLAMExample_graph.cpp b/examples/Pose2SLAMExample_graph.cpp index 46ca02ee4..0c64634c5 100644 --- a/examples/Pose2SLAMExample_graph.cpp +++ b/examples/Pose2SLAMExample_graph.cpp @@ -16,11 +16,14 @@ * @author Frank Dellaert */ -#include +// For an explanation of headers below, please see Pose2SLAMExample.cpp #include -#include +#include #include -#include +#include + +// This new header allows us to read examples easily from .graph files +#include using namespace std; using namespace gtsam; diff --git a/examples/Pose2SLAMExample_graphviz.cpp b/examples/Pose2SLAMExample_graphviz.cpp index 818a9e577..314ccbdb4 100644 --- a/examples/Pose2SLAMExample_graphviz.cpp +++ b/examples/Pose2SLAMExample_graphviz.cpp @@ -16,11 +16,11 @@ * @author Frank Dellaert */ +// For an explanation of headers below, please see Pose2SLAMExample.cpp #include #include -#include -#include #include +#include #include using namespace std; diff --git a/examples/Pose2SLAMwSPCG.cpp b/examples/Pose2SLAMwSPCG.cpp index 4422586b0..2c25d2f8e 100644 --- a/examples/Pose2SLAMwSPCG.cpp +++ b/examples/Pose2SLAMwSPCG.cpp @@ -16,47 +16,15 @@ * @date June 2, 2012 */ -/** - * A simple 2D pose slam example solved using a Conjugate-Gradient method - * - The robot moves in a 2 meter square - * - The robot moves 2 meters each step, turning 90 degrees after each step - * - The robot initially faces along the X axis (horizontal, to the right in 2D) - * - We have full odometry between pose - * - We have a loop closure constraint when the robot returns to the first position - */ - -// As this is a planar SLAM example, we will use Pose2 variables (x, y, theta) to represent -// the robot positions -#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 simple integer keys -#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. -// Here we will use Between factors for the relative motion described by odometry measurements. -// We will also use a Between Factor to encode the loop closure constraint -// Also, we will initialize the robot at the origin using a Prior factor. +// For an explanation of headers below, please see Pose2SLAMExample.cpp #include #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 - -#include +#include #include +// In contrast to that example, however, we will use a PCG solver here +#include + using namespace std; using namespace gtsam; @@ -66,32 +34,24 @@ int main(int argc, char** argv) { NonlinearFactorGraph graph; // 2a. Add a prior on the first pose, setting it to the origin - // A prior factor consists of a mean and a noise model (covariance matrix) Pose2 prior(0.0, 0.0, 0.0); // prior at origin noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas(Vector3(0.3, 0.3, 0.1)); graph.push_back(PriorFactor(1, prior, priorNoise)); // 2b. Add odometry factors - // For simplicity, we will use the same noise model for each odometry factor noiseModel::Diagonal::shared_ptr odometryNoise = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1)); - // Create odometry (Between) factors between consecutive poses graph.push_back(BetweenFactor(1, 2, Pose2(2.0, 0.0, M_PI_2), odometryNoise)); graph.push_back(BetweenFactor(2, 3, Pose2(2.0, 0.0, M_PI_2), odometryNoise)); graph.push_back(BetweenFactor(3, 4, Pose2(2.0, 0.0, M_PI_2), odometryNoise)); graph.push_back(BetweenFactor(4, 5, Pose2(2.0, 0.0, M_PI_2), odometryNoise)); // 2c. Add the loop closure constraint - // This factor encodes the fact that we have returned to the same pose. In real systems, - // these constraints may be identified in many ways, such as appearance-based techniques - // with camera images. - // We will use another Between Factor to enforce this constraint, with the distance set to zero, noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1)); graph.push_back(BetweenFactor(5, 1, Pose2(0.0, 0.0, 0.0), model)); graph.print("\nFactor Graph:\n"); // print // 3. Create the data structure to hold the initialEstimate estimate to the solution - // For illustrative purposes, these have been deliberately set to incorrect values Values initialEstimate; initialEstimate.insert(1, Pose2(0.5, 0.0, 0.2)); initialEstimate.insert(2, Pose2(2.3, 0.1, 1.1)); @@ -104,15 +64,18 @@ int main(int argc, char** argv) { LevenbergMarquardtParams parameters; parameters.verbosity = NonlinearOptimizerParams::ERROR; parameters.verbosityLM = LevenbergMarquardtParams::LAMBDA; - parameters.linearSolverType = NonlinearOptimizerParams::Iterative; - { - parameters.iterativeParams = boost::make_shared(); - LevenbergMarquardtOptimizer optimizer(graph, initialEstimate, parameters); - Values result = optimizer.optimize(); - result.print("Final Result:\n"); - cout << "subgraph solver final error = " << graph.error(result) << endl; - } + // LM is still the outer optimization loop, but by specifying "Iterative" below + // We indicate that an iterative linear solver should be used. + // In addition, the *type* of the iterativeParams decides on the type of + // iterative solver, in this case the SPCG (subgraph PCG) + parameters.linearSolverType = NonlinearOptimizerParams::Iterative; + parameters.iterativeParams = boost::make_shared(); + + LevenbergMarquardtOptimizer optimizer(graph, initialEstimate, parameters); + Values result = optimizer.optimize(); + result.print("Final Result:\n"); + cout << "subgraph solver final error = " << graph.error(result) << endl; return 0; } diff --git a/examples/SFMExample.cpp b/examples/SFMExample.cpp index 5be266d11..38dd1ca81 100644 --- a/examples/SFMExample.cpp +++ b/examples/SFMExample.cpp @@ -15,13 +15,7 @@ * @author Duy-Nguyen Ta */ -/** - * A structure-from-motion example with landmarks - * - The landmarks form a 10 meter cube - * - The robot rotates around the landmarks, always facing towards the cube - */ - -// For loading the data +// For loading the data, see the comments therein for scenario (camera rotates around cube) #include "SFMdata.h" // Camera observations of landmarks (i.e. pixel coordinates) will be stored as Point2 (x, y). diff --git a/examples/SFMExample_SmartFactor.cpp b/examples/SFMExample_SmartFactor.cpp index b999e6600..fbd2581ef 100644 --- a/examples/SFMExample_SmartFactor.cpp +++ b/examples/SFMExample_SmartFactor.cpp @@ -17,46 +17,17 @@ * @author Frank Dellaert */ -/** - * A structure-from-motion example with landmarks - * - The landmarks form a 10 meter cube - * - The robot rotates around the landmarks, always facing towards the cube - */ - -// For loading the data -#include "SFMdata.h" - -// Camera observations of landmarks (i.e. pixel coordinates) will be stored as Point2 (x, y). -#include - // In GTSAM, measurement functions are represented as 'factors'. -// The factor we used here is SmartProjectionPoseFactor. Every smart factor represent a single landmark, -// The SmartProjectionPoseFactor only optimize the pose of camera, not the calibration, -// The calibration should be known. +// The factor we used here is SmartProjectionPoseFactor. +// Every smart factor represent a single landmark, seen from multiple cameras. +// The SmartProjectionPoseFactor only optimizes for the poses of a camera, +// not the calibration, which is assumed known. #include -// Also, we will initialize the robot at some location using a Prior factor. -#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 - -// 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 a -// trust-region method known as Powell's Degleg +// For an explanation of these headers, see SFMExample.cpp +#include "SFMdata.h" #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 - -#include - using namespace std; using namespace gtsam;