Reformatted some and use of auto

examples/LocalizationExample.cpp

@@ -66,12 +66,11 @@ using namespace gtsam;
 #include <gtsam/nonlinear/NonlinearFactor.h>
 
 class UnaryFactor: public NoiseModelFactor1<Pose2> {
-
   // The factor will hold a measurement consisting of an (X,Y) location
   // We could this with a Point2 but here we just use two doubles
   double mx_, my_;
 
-public:
+ public:
   /// shorthand for a smart pointer to a factor
   typedef boost::shared_ptr<UnaryFactor> shared_ptr;
 
@@ -85,15 +84,15 @@ public:
   // The first is the 'evaluateError' function. This function implements the desired measurement
   // function, returning a vector of errors when evaluated at the provided variable value. It
   // must also calculate the Jacobians for this measurement function, if requested.
-  Vector evaluateError(const Pose2& q, boost::optional<Matrix&> H = boost::none) const
+  Vector evaluateError(const Pose2& q,
-  {
+                       boost::optional<Matrix&> H = boost::none) const {
     // The measurement function for a GPS-like measurement is simple:
     // error_x = pose.x - measurement.x
     // error_y = pose.y - measurement.y
     // Consequently, the Jacobians are:
     // [ derror_x/dx  derror_x/dy  derror_x/dtheta ] = [1 0 0]
     // [ derror_y/dx  derror_y/dy  derror_y/dtheta ] = [0 1 0]
-    if (H) (*H) = (Matrix(2,3) << 1.0,0.0,0.0, 0.0,1.0,0.0).finished();
+    if (H) (*H) = (Matrix(2, 3) << 1.0, 0.0, 0.0, 0.0, 1.0, 0.0).finished();
     return (Vector(2) << q.x() - mx_, q.y() - my_).finished();
   }
 
@@ -107,29 +106,28 @@ public:
   // Additionally, we encourage you the use of unit testing your custom factors,
   // (as all GTSAM factors are), in which you would need an equals and print, to satisfy the
   // GTSAM_CONCEPT_TESTABLE_INST(T) defined in Testable.h, but these are not needed below.
-
+};  // UnaryFactor
-}; // UnaryFactor
 
 
 int main(int argc, char** argv) {
-
   // 1. Create a factor graph container and add factors to it
   NonlinearFactorGraph graph;
 
   // 2a. 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));
+  auto odometryNoise = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1));
   // Create odometry (Between) factors between consecutive poses
   graph.emplace_shared<BetweenFactor<Pose2> >(1, 2, Pose2(2.0, 0.0, 0.0), odometryNoise);
   graph.emplace_shared<BetweenFactor<Pose2> >(2, 3, Pose2(2.0, 0.0, 0.0), odometryNoise);
 
   // 2b. Add "GPS-like" measurements
   // We will use our custom UnaryFactor for this.
-  noiseModel::Diagonal::shared_ptr unaryNoise = noiseModel::Diagonal::Sigmas(Vector2(0.1, 0.1)); // 10cm std on x,y
+  auto unaryNoise =
+      noiseModel::Diagonal::Sigmas(Vector2(0.1, 0.1));  // 10cm std on x,y
   graph.emplace_shared<UnaryFactor>(1, 0.0, 0.0, unaryNoise);
   graph.emplace_shared<UnaryFactor>(2, 2.0, 0.0, unaryNoise);
   graph.emplace_shared<UnaryFactor>(3, 4.0, 0.0, unaryNoise);
-  graph.print("\nFactor Graph:\n"); // print
+  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
@@ -137,7 +135,7 @@ int main(int argc, char** argv) {
   initialEstimate.insert(1, Pose2(0.5, 0.0, 0.2));
   initialEstimate.insert(2, Pose2(2.3, 0.1, -0.2));
   initialEstimate.insert(3, Pose2(4.1, 0.1, 0.1));
-  initialEstimate.print("\nInitial Estimate:\n"); // print
+  initialEstimate.print("\nInitial Estimate:\n");  // print
 
   // 4. Optimize using Levenberg-Marquardt optimization. The optimizer
   // accepts an optional set of configuration parameters, controlling

examples/METISOrderingExample.cpp

@@ -11,7 +11,8 @@
 
 /**
  * @file METISOrdering.cpp
- * @brief Simple robot motion example, with prior and two odometry measurements, using a METIS ordering
+ * @brief Simple robot motion example, with prior and two odometry measurements,
+ * using a METIS ordering
  * @author Frank Dellaert
  * @author Andrew Melim
  */
@@ -22,11 +23,11 @@
  */
 
 #include <gtsam/geometry/Pose2.h>
-#include <gtsam/slam/BetweenFactor.h>
-#include <gtsam/nonlinear/NonlinearFactorGraph.h>
 #include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
 #include <gtsam/nonlinear/Marginals.h>
+#include <gtsam/nonlinear/NonlinearFactorGraph.h>
 #include <gtsam/nonlinear/Values.h>
+#include <gtsam/slam/BetweenFactor.h>
 
 using namespace std;
 using namespace gtsam;
@@ -34,26 +35,26 @@ using namespace gtsam;
 int main(int argc, char** argv) {
   NonlinearFactorGraph graph;
 
-  Pose2 priorMean(0.0, 0.0, 0.0); // prior at origin
+  Pose2 priorMean(0.0, 0.0, 0.0);  // prior at origin
-  noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas(Vector3(0.3, 0.3, 0.1));
+  auto priorNoise = noiseModel::Diagonal::Sigmas(Vector3(0.3, 0.3, 0.1));
   graph.addPrior(1, priorMean, priorNoise);
 
   Pose2 odometry(2.0, 0.0, 0.0);
-  noiseModel::Diagonal::shared_ptr odometryNoise = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1));
+  auto odometryNoise = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1));
   graph.emplace_shared<BetweenFactor<Pose2> >(1, 2, odometry, odometryNoise);
   graph.emplace_shared<BetweenFactor<Pose2> >(2, 3, odometry, odometryNoise);
-  graph.print("\nFactor Graph:\n"); // print
+  graph.print("\nFactor Graph:\n");  // print
 
   Values initial;
   initial.insert(1, Pose2(0.5, 0.0, 0.2));
   initial.insert(2, Pose2(2.3, 0.1, -0.2));
   initial.insert(3, Pose2(4.1, 0.1, 0.1));
-  initial.print("\nInitial Estimate:\n"); // print
+  initial.print("\nInitial Estimate:\n");  // print
 
   // optimize using Levenberg-Marquardt optimization
   LevenbergMarquardtParams params;
-  // In order to specify the ordering type, we need to se the NonlinearOptimizerParameter "orderingType"
+  // In order to specify the ordering type, we need to se the "orderingType". By
-  // By default this parameter is set to OrderingType::COLAMD
+  // default this parameter is set to OrderingType::COLAMD
   params.orderingType = Ordering::METIS;
   LevenbergMarquardtOptimizer optimizer(graph, initial, params);
   Values result = optimizer.optimize();

examples/OdometryExample.cpp

@@ -56,24 +56,23 @@ using namespace std;
 using namespace gtsam;
 
 int main(int argc, char** argv) {
-
   // Create an empty nonlinear factor graph
   NonlinearFactorGraph graph;
 
   // 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 priorMean(0.0, 0.0, 0.0); // prior at origin
+  Pose2 priorMean(0.0, 0.0, 0.0);  // prior at origin
-  noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas(Vector3(0.3, 0.3, 0.1));
+  auto priorNoise = noiseModel::Diagonal::Sigmas(Vector3(0.3, 0.3, 0.1));
   graph.addPrior(1, priorMean, priorNoise);
 
   // Add odometry factors
   Pose2 odometry(2.0, 0.0, 0.0);
   // 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));
+  auto odometryNoise = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1));
   // Create odometry (Between) factors between consecutive poses
   graph.emplace_shared<BetweenFactor<Pose2> >(1, 2, odometry, odometryNoise);
   graph.emplace_shared<BetweenFactor<Pose2> >(2, 3, odometry, odometryNoise);
-  graph.print("\nFactor Graph:\n"); // print
+  graph.print("\nFactor Graph:\n");  // print
 
   // Create the data structure to hold the initialEstimate estimate to the solution
   // For illustrative purposes, these have been deliberately set to incorrect values
@@ -81,7 +80,7 @@ int main(int argc, char** argv) {
   initial.insert(1, Pose2(0.5, 0.0, 0.2));
   initial.insert(2, Pose2(2.3, 0.1, -0.2));
   initial.insert(3, Pose2(4.1, 0.1, 0.1));
-  initial.print("\nInitial Estimate:\n"); // print
+  initial.print("\nInitial Estimate:\n");  // print
 
   // optimize using Levenberg-Marquardt optimization
   Values result = LevenbergMarquardtOptimizer(graph, initial).optimize();

examples/PlanarSLAMExample.cpp

@@ -69,35 +69,36 @@ using namespace std;
 using namespace gtsam;
 
 int main(int argc, char** argv) {
-
   // Create a factor graph
   NonlinearFactorGraph graph;
 
   // Create the keys we need for this simple example
-  static Symbol x1('x',1), x2('x',2), x3('x',3);
+  static Symbol x1('x', 1), x2('x', 2), x3('x', 3);
-  static Symbol l1('l',1), l2('l',2);
+  static Symbol l1('l', 1), l2('l', 2);
 
-  // Add a prior on pose x1 at the origin. A prior factor consists of a mean and a noise model (covariance matrix)
+  // Add a prior on pose x1 at the origin. A prior factor consists of a mean and
-  Pose2 prior(0.0, 0.0, 0.0); // prior mean is at origin
+  // a noise model (covariance matrix)
-  noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas(Vector3(0.3, 0.3, 0.1)); // 30cm std on x,y, 0.1 rad on theta
+  Pose2 prior(0.0, 0.0, 0.0);  // prior mean is at origin
-  graph.addPrior(x1, prior, priorNoise); // add directly to graph
+  auto priorNoise = noiseModel::Diagonal::Sigmas(
+      Vector3(0.3, 0.3, 0.1));            // 30cm std on x,y, 0.1 rad on theta
+  graph.addPrior(x1, prior, 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)
+  Pose2 odometry(2.0, 0.0, 0.0);
-  noiseModel::Diagonal::shared_ptr odometryNoise = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1)); // 20cm std on x,y, 0.1 rad on theta
+  // create a measurement for both factors (the same in this case)
+  auto odometryNoise = noiseModel::Diagonal::Sigmas(
+      Vector3(0.2, 0.2, 0.1));  // 20cm std on x,y, 0.1 rad on theta
   graph.emplace_shared<BetweenFactor<Pose2> >(x1, x2, odometry, odometryNoise);
   graph.emplace_shared<BetweenFactor<Pose2> >(x2, x3, odometry, odometryNoise);
 
   // Add Range-Bearing measurements to two different landmarks
   // create a noise model for the landmark measurements
-  noiseModel::Diagonal::shared_ptr measurementNoise = noiseModel::Diagonal::Sigmas(Vector2(0.1, 0.2)); // 0.1 rad std on bearing, 20cm on range
+  auto measurementNoise = noiseModel::Diagonal::Sigmas(
+      Vector2(0.1, 0.2));  // 0.1 rad std on bearing, 20cm on range
   // create the measurement values - indices are (pose id, landmark id)
-  Rot2 bearing11 = Rot2::fromDegrees(45),
+  Rot2 bearing11 = Rot2::fromDegrees(45), bearing21 = Rot2::fromDegrees(90),
-       bearing21 = Rot2::fromDegrees(90),
        bearing32 = Rot2::fromDegrees(90);
-  double range11 = std::sqrt(4.0+4.0),
+  double range11 = std::sqrt(4.0 + 4.0), range21 = 2.0, range32 = 2.0;
-         range21 = 2.0,
-         range32 = 2.0;
 
   // Add Bearing-Range factors
   graph.emplace_shared<BearingRangeFactor<Pose2, Point2> >(x1, l1, bearing11, range11, measurementNoise);
@@ -110,7 +111,7 @@ int main(int argc, char** argv) {
   // Create (deliberately inaccurate) initial estimate
   Values initialEstimate;
   initialEstimate.insert(x1, Pose2(0.5, 0.0, 0.2));
-  initialEstimate.insert(x2, Pose2(2.3, 0.1,-0.2));
+  initialEstimate.insert(x2, Pose2(2.3, 0.1, -0.2));
   initialEstimate.insert(x3, Pose2(4.1, 0.1, 0.1));
   initialEstimate.insert(l1, Point2(1.8, 2.1));
   initialEstimate.insert(l2, Point2(4.1, 1.8));
@@ -138,4 +139,3 @@ int main(int argc, char** argv) {
 
   return 0;
 }
-

examples/Pose2SLAMExample.cpp

@@ -64,21 +64,20 @@ using namespace std;
 using namespace gtsam;
 
 int main(int argc, char** argv) {
-
   // 1. Create a factor graph container and add factors to it
   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)
-  noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas(Vector3(0.3, 0.3, 0.1));
+  auto priorNoise = noiseModel::Diagonal::Sigmas(Vector3(0.3, 0.3, 0.1));
   graph.addPrior(1, Pose2(0, 0, 0), priorNoise);
 
   // For simplicity, we will use the same noise model for odometry and loop closures
-  noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1));
+  auto model = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1));
 
   // 2b. Add odometry factors
   // Create odometry (Between) factors between consecutive poses
-  graph.emplace_shared<BetweenFactor<Pose2> >(1, 2, Pose2(2, 0, 0     ), model);
+  graph.emplace_shared<BetweenFactor<Pose2> >(1, 2, Pose2(2, 0, 0), model);
   graph.emplace_shared<BetweenFactor<Pose2> >(2, 3, Pose2(2, 0, M_PI_2), model);
   graph.emplace_shared<BetweenFactor<Pose2> >(3, 4, Pose2(2, 0, M_PI_2), model);
   graph.emplace_shared<BetweenFactor<Pose2> >(4, 5, Pose2(2, 0, M_PI_2), model);
@@ -88,17 +87,17 @@ int main(int argc, char** argv) {
   // 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:
   graph.emplace_shared<BetweenFactor<Pose2> >(5, 2, Pose2(2, 0, M_PI_2), model);
-  graph.print("\nFactor Graph:\n"); // print
+  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(1, Pose2(0.5, 0.0, 0.2));
-  initialEstimate.insert(2, Pose2(2.3, 0.1, -0.2   ));
+  initialEstimate.insert(2, Pose2(2.3, 0.1, -0.2));
-  initialEstimate.insert(3, Pose2(4.1, 0.1,  M_PI_2));
+  initialEstimate.insert(3, Pose2(4.1, 0.1, M_PI_2));
-  initialEstimate.insert(4, Pose2(4.0, 2.0,  M_PI  ));
+  initialEstimate.insert(4, Pose2(4.0, 2.0, M_PI));
   initialEstimate.insert(5, Pose2(2.1, 2.1, -M_PI_2));
-  initialEstimate.print("\nInitial Estimate:\n"); // print
+  initialEstimate.print("\nInitial Estimate:\n");  // print
 
   // 4. Optimize the initial values using a Gauss-Newton nonlinear optimizer
   // The optimizer accepts an optional set of configuration parameters,

examples/Pose2SLAMExampleExpressions.cpp

@@ -30,7 +30,6 @@ using namespace std;
 using namespace gtsam;
 
 int main(int argc, char** argv) {
-
   // 1. Create a factor graph container and add factors to it
   ExpressionFactorGraph graph;
 
@@ -38,31 +37,32 @@ int main(int argc, char** argv) {
   Pose2_ x1(1), x2(2), x3(3), x4(4), x5(5);
 
   // 2a. Add a prior on the first pose, setting it to the origin
-  noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas(Vector3(0.3, 0.3, 0.1));
+  auto priorNoise = noiseModel::Diagonal::Sigmas(Vector3(0.3, 0.3, 0.1));
   graph.addExpressionFactor(x1, Pose2(0, 0, 0), priorNoise);
 
-  // For simplicity, we will use the same noise model for odometry and loop closures
+  // For simplicity, we use the same noise model for odometry and loop closures
-  noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1));
+  auto model = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1));
 
   // 2b. Add odometry factors
-  graph.addExpressionFactor(between(x1,x2), Pose2(2, 0, 0     ), model);
+  graph.addExpressionFactor(between(x1, x2), Pose2(2, 0, 0), model);
-  graph.addExpressionFactor(between(x2,x3), Pose2(2, 0, M_PI_2), model);
+  graph.addExpressionFactor(between(x2, x3), Pose2(2, 0, M_PI_2), model);
-  graph.addExpressionFactor(between(x3,x4), Pose2(2, 0, M_PI_2), model);
+  graph.addExpressionFactor(between(x3, x4), Pose2(2, 0, M_PI_2), model);
-  graph.addExpressionFactor(between(x4,x5), Pose2(2, 0, M_PI_2), model);
+  graph.addExpressionFactor(between(x4, x5), Pose2(2, 0, M_PI_2), model);
 
   // 2c. Add the loop closure constraint
-  graph.addExpressionFactor(between(x5,x2), Pose2(2, 0, M_PI_2), model);
+  graph.addExpressionFactor(between(x5, x2), Pose2(2, 0, M_PI_2), model);
-  graph.print("\nFactor Graph:\n"); // print
+  graph.print("\nFactor Graph:\n");  // print
 
-  // 3. Create the data structure to hold the initialEstimate estimate to the solution
+  // 3. Create the data structure to hold the initialEstimate estimate to the
-  // For illustrative purposes, these have been deliberately set to incorrect values
+  // 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(1, Pose2(0.5, 0.0, 0.2));
-  initialEstimate.insert(2, Pose2(2.3, 0.1, -0.2   ));
+  initialEstimate.insert(2, Pose2(2.3, 0.1, -0.2));
-  initialEstimate.insert(3, Pose2(4.1, 0.1,  M_PI_2));
+  initialEstimate.insert(3, Pose2(4.1, 0.1, M_PI_2));
-  initialEstimate.insert(4, Pose2(4.0, 2.0,  M_PI  ));
+  initialEstimate.insert(4, Pose2(4.0, 2.0, M_PI));
   initialEstimate.insert(5, Pose2(2.1, 2.1, -M_PI_2));
-  initialEstimate.print("\nInitial Estimate:\n"); // print
+  initialEstimate.print("\nInitial Estimate:\n");  // print
 
   // 4. Optimize the initial values using a Gauss-Newton nonlinear optimizer
   GaussNewtonParams parameters;

examples/Pose2SLAMExample_g2o.cpp

@@ -28,41 +28,41 @@ using namespace gtsam;
 
 // HOWTO: ./Pose2SLAMExample_g2o inputFile outputFile (maxIterations) (tukey/huber)
 int main(const int argc, const char *argv[]) {
-
   string kernelType = "none";
-  int maxIterations = 100; // default
+  int maxIterations = 100;                                    // default
-  string g2oFile = findExampleDataFile("noisyToyGraph.txt"); // default
+  string g2oFile = findExampleDataFile("noisyToyGraph.txt");  // default
 
   // Parse user's inputs
-  if (argc > 1){
+  if (argc > 1) {
-    g2oFile = argv[1]; // input dataset filename
+    g2oFile = argv[1];  // input dataset filename
-    // outputFile = g2oFile = argv[2]; // done later
   }
-  if (argc > 3){
+  if (argc > 3) {
-    maxIterations = atoi(argv[3]); // user can specify either tukey or huber
+    maxIterations = atoi(argv[3]);  // user can specify either tukey or huber
   }
-  if (argc > 4){
+  if (argc > 4) {
-    kernelType = argv[4]; // user can specify either tukey or huber
+    kernelType = argv[4];  // user can specify either tukey or huber
   }
 
   // reading file and creating factor graph
   NonlinearFactorGraph::shared_ptr graph;
   Values::shared_ptr initial;
   bool is3D = false;
-  if(kernelType.compare("none") == 0){
+  if (kernelType.compare("none") == 0) {
-    boost::tie(graph, initial) = readG2o(g2oFile,is3D);
+    boost::tie(graph, initial) = readG2o(g2oFile, is3D);
   }
-  if(kernelType.compare("huber") == 0){
+  if (kernelType.compare("huber") == 0) {
     std::cout << "Using robust kernel: huber " << std::endl;
-    boost::tie(graph, initial) = readG2o(g2oFile,is3D, KernelFunctionTypeHUBER);
+    boost::tie(graph, initial) =
+        readG2o(g2oFile, is3D, KernelFunctionTypeHUBER);
   }
-  if(kernelType.compare("tukey") == 0){
+  if (kernelType.compare("tukey") == 0) {
     std::cout << "Using robust kernel: tukey " << std::endl;
-    boost::tie(graph, initial) = readG2o(g2oFile,is3D, KernelFunctionTypeTUKEY);
+    boost::tie(graph, initial) =
+        readG2o(g2oFile, is3D, KernelFunctionTypeTUKEY);
   }
 
   // Add prior on the pose having index (key) = 0
-  noiseModel::Diagonal::shared_ptr priorModel = //
+  auto priorModel =  //
       noiseModel::Diagonal::Variances(Vector3(1e-6, 1e-6, 1e-8));
   graph->addPrior(0, Pose2(), priorModel);
   std::cout << "Adding prior on pose 0 " << std::endl;
@@ -71,7 +71,8 @@ int main(const int argc, const char *argv[]) {
   params.setVerbosity("TERMINATION");
   if (argc > 3) {
     params.maxIterations = maxIterations;
-    std::cout << "User required to perform maximum  " << params.maxIterations << " iterations "<< std::endl;
+    std::cout << "User required to perform maximum  " << params.maxIterations
+              << " iterations " << std::endl;
   }
 
   std::cout << "Optimizing the factor graph" << std::endl;
@@ -79,8 +80,8 @@ int main(const int argc, const char *argv[]) {
   Values result = optimizer.optimize();
   std::cout << "Optimization complete" << std::endl;
 
-  std::cout << "initial error=" <<graph->error(*initial)<< std::endl;
+  std::cout << "initial error=" << graph->error(*initial) << std::endl;
-  std::cout << "final error=" <<graph->error(result)<< std::endl;
+  std::cout << "final error=" << graph->error(result) << std::endl;
 
   if (argc < 3) {
     result.print("result");

examples/Pose2SLAMExample_graphviz.cpp

@@ -25,20 +25,20 @@
 using namespace std;
 using namespace gtsam;
 
-int main (int argc, char** argv) {
+int main(int argc, char** argv) {
-
   // 1. Create a factor graph container and add factors to it
   NonlinearFactorGraph graph;
 
   // 2a. Add a prior on the first pose, setting it to the origin
-  noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas(Vector3(0.3, 0.3, 0.1));
+  auto priorNoise = noiseModel::Diagonal::Sigmas(Vector3(0.3, 0.3, 0.1));
   graph.addPrior(1, Pose2(0, 0, 0), priorNoise);
 
-  // For simplicity, we will use the same noise model for odometry and loop closures
+  // For simplicity, we will use the same noise model for odometry and loop
-  noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1));
+  // closures
+  auto model = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1));
 
   // 2b. Add odometry factors
-  graph.emplace_shared<BetweenFactor<Pose2> >(1, 2, Pose2(2, 0, 0     ), model);
+  graph.emplace_shared<BetweenFactor<Pose2> >(1, 2, Pose2(2, 0, 0), model);
   graph.emplace_shared<BetweenFactor<Pose2> >(2, 3, Pose2(2, 0, M_PI_2), model);
   graph.emplace_shared<BetweenFactor<Pose2> >(3, 4, Pose2(2, 0, M_PI_2), model);
   graph.emplace_shared<BetweenFactor<Pose2> >(4, 5, Pose2(2, 0, M_PI_2), model);
@@ -49,10 +49,10 @@ int main (int argc, char** argv) {
   // 3. Create the data structure to hold the initial estimate to the solution
   // For illustrative purposes, these have been deliberately set to incorrect values
   Values initial;
-  initial.insert(1, Pose2(0.5, 0.0,  0.2   ));
+  initial.insert(1, Pose2(0.5, 0.0, 0.2));
-  initial.insert(2, Pose2(2.3, 0.1, -0.2   ));
+  initial.insert(2, Pose2(2.3, 0.1, -0.2));
-  initial.insert(3, Pose2(4.1, 0.1,  M_PI_2));
+  initial.insert(3, Pose2(4.1, 0.1, M_PI_2));
-  initial.insert(4, Pose2(4.0, 2.0,  M_PI  ));
+  initial.insert(4, Pose2(4.0, 2.0, M_PI));
   initial.insert(5, Pose2(2.1, 2.1, -M_PI_2));
 
   // Single Step Optimization using Levenberg-Marquardt

examples/Pose2SLAMExample_lago.cpp

@@ -28,7 +28,6 @@ using namespace std;
 using namespace gtsam;
 
 int main(const int argc, const char *argv[]) {
-
   // Read graph from file
   string g2oFile;
   if (argc < 2)
@@ -41,8 +40,7 @@ int main(const int argc, const char *argv[]) {
   boost::tie(graph, initial) = readG2o(g2oFile);
 
   // Add prior on the pose having index (key) = 0
-  noiseModel::Diagonal::shared_ptr priorModel = //
+  auto priorModel = noiseModel::Diagonal::Variances(Vector3(1e-6, 1e-6, 1e-8));
-      noiseModel::Diagonal::Variances(Vector3(1e-6, 1e-6, 1e-8));
   graph->addPrior(0, Pose2(), priorModel);
   graph->print();
 

examples/Pose2SLAMwSPCG.cpp

@@ -28,45 +28,45 @@ using namespace std;
 using namespace gtsam;
 
 int main(int argc, char** argv) {
-
   // 1. Create a factor graph container and add factors to it
   NonlinearFactorGraph graph;
 
   // 2a. Add a prior on the first pose, setting it to the origin
-  Pose2 prior(0.0, 0.0, 0.0); // prior at origin
+  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));
+  auto priorNoise = noiseModel::Diagonal::Sigmas(Vector3(0.3, 0.3, 0.1));
   graph.addPrior(1, prior, priorNoise);
 
   // 2b. Add odometry factors
-  noiseModel::Diagonal::shared_ptr odometryNoise = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1));
+  auto odometryNoise = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1));
   graph.emplace_shared<BetweenFactor<Pose2> >(1, 2, Pose2(2.0, 0.0, M_PI_2), odometryNoise);
   graph.emplace_shared<BetweenFactor<Pose2> >(2, 3, Pose2(2.0, 0.0, M_PI_2), odometryNoise);
   graph.emplace_shared<BetweenFactor<Pose2> >(3, 4, Pose2(2.0, 0.0, M_PI_2), odometryNoise);
   graph.emplace_shared<BetweenFactor<Pose2> >(4, 5, Pose2(2.0, 0.0, M_PI_2), odometryNoise);
 
   // 2c. Add the loop closure constraint
-  noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1));
+  auto model = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1));
-  graph.emplace_shared<BetweenFactor<Pose2> >(5, 1, Pose2(0.0, 0.0, 0.0), model);
+  graph.emplace_shared<BetweenFactor<Pose2> >(5, 1, Pose2(0.0, 0.0, 0.0),
-  graph.print("\nFactor Graph:\n"); // print
+                                              model);
+  graph.print("\nFactor Graph:\n");  // print
 
-
+  // 3. Create the data structure to hold the initialEstimate estimate to the
-  // 3. Create the data structure to hold the initialEstimate estimate to the solution
+  // solution
   Values initialEstimate;
   initialEstimate.insert(1, Pose2(0.5, 0.0, 0.2));
   initialEstimate.insert(2, Pose2(2.3, 0.1, 1.1));
   initialEstimate.insert(3, Pose2(2.1, 1.9, 2.8));
   initialEstimate.insert(4, Pose2(-.3, 2.5, 4.2));
-  initialEstimate.insert(5, Pose2(0.1,-0.7, 5.8));
+  initialEstimate.insert(5, Pose2(0.1, -0.7, 5.8));
-  initialEstimate.print("\nInitial Estimate:\n"); // print
+  initialEstimate.print("\nInitial Estimate:\n");  // print
 
   // 4. Single Step Optimization using Levenberg-Marquardt
   LevenbergMarquardtParams parameters;
   parameters.verbosity = NonlinearOptimizerParams::ERROR;
   parameters.verbosityLM = LevenbergMarquardtParams::LAMBDA;
 
-  // LM is still the outer optimization loop, but by specifying "Iterative" below
+  // LM is still the outer optimization loop, but by specifying "Iterative"
-  // We indicate that an iterative linear solver should be used.
+  // below We indicate that an iterative linear solver should be used. In
-  // In addition, the *type* of the iterativeParams decides on the type of
+  // 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<SubgraphSolverParameters>();

examples/Pose3Localization.cpp

@@ -10,9 +10,9 @@
  * -------------------------------------------------------------------------- */
 
 /**
- * @file Pose3SLAMExample_initializePose3.cpp
+ * @file Pose3Localization.cpp
  * @brief A 3D Pose SLAM example that reads input from g2o, and initializes the Pose3 using InitializePose3
- * Syntax for the script is ./Pose3SLAMExample_initializePose3 input.g2o output.g2o
+ * Syntax for the script is ./Pose3Localization input.g2o output.g2o
  * @date Aug 25, 2014
  * @author Luca Carlone
  */
@@ -26,8 +26,7 @@
 using namespace std;
 using namespace gtsam;
 
-int main(const int argc, const char *argv[]) {
+int main(const int argc, const char* argv[]) {
-
   // Read graph from file
   string g2oFile;
   if (argc < 2)
@@ -41,10 +40,10 @@ int main(const int argc, const char *argv[]) {
   boost::tie(graph, initial) = readG2o(g2oFile, is3D);
 
   // Add prior on the first key
-  noiseModel::Diagonal::shared_ptr priorModel = //
+  auto priorModel = noiseModel::Diagonal::Variances(
-      noiseModel::Diagonal::Variances((Vector(6) << 1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4).finished());
+      (Vector(6) << 1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4).finished());
   Key firstKey = 0;
-  for(const Values::ConstKeyValuePair& key_value: *initial) {
+  for (const Values::ConstKeyValuePair& key_value : *initial) {
     std::cout << "Adding prior to g2o file " << std::endl;
     firstKey = key_value.key;
     graph->addPrior(firstKey, Pose3(), priorModel);
@@ -53,13 +52,13 @@ int main(const int argc, const char *argv[]) {
 
   std::cout << "Optimizing the factor graph" << std::endl;
   GaussNewtonParams params;
-  params.setVerbosity("TERMINATION"); // this will show info about stopping conditions
+  params.setVerbosity("TERMINATION");  // show info about stopping conditions
   GaussNewtonOptimizer optimizer(*graph, *initial, params);
   Values result = optimizer.optimize();
   std::cout << "Optimization complete" << std::endl;
 
-  std::cout << "initial error=" <<graph->error(*initial)<< std::endl;
+  std::cout << "initial error=" << graph->error(*initial) << std::endl;
-  std::cout << "final error=" <<graph->error(result)<< std::endl;
+  std::cout << "final error=" << graph->error(result) << std::endl;
 
   if (argc < 3) {
     result.print("result");
@@ -75,7 +74,7 @@ int main(const int argc, const char *argv[]) {
 
   // Calculate and print marginal covariances for all variables
   Marginals marginals(*graph, result);
-  for(const auto& key_value: result) {
+  for (const auto& key_value : result) {
     auto p = dynamic_cast<const GenericValue<Pose3>*>(&key_value.value);
     if (!p) continue;
     std::cout << marginals.marginalCovariance(key_value.key) << endl;

examples/Pose3SLAMExample_g2o.cpp

@@ -10,9 +10,9 @@
  * -------------------------------------------------------------------------- */
 
 /**
- * @file Pose3SLAMExample_initializePose3.cpp
+ * @file Pose3SLAMExample_g2o.cpp
  * @brief A 3D Pose SLAM example that reads input from g2o, and initializes the Pose3 using InitializePose3
- * Syntax for the script is ./Pose3SLAMExample_initializePose3 input.g2o output.g2o
+ * Syntax for the script is ./Pose3SLAMExample_g2o input.g2o output.g2o
  * @date Aug 25, 2014
  * @author Luca Carlone
  */
@@ -25,8 +25,7 @@
 using namespace std;
 using namespace gtsam;
 
-int main(const int argc, const char *argv[]) {
+int main(const int argc, const char* argv[]) {
-
   // Read graph from file
   string g2oFile;
   if (argc < 2)
@@ -40,10 +39,10 @@ int main(const int argc, const char *argv[]) {
   boost::tie(graph, initial) = readG2o(g2oFile, is3D);
 
   // Add prior on the first key
-  noiseModel::Diagonal::shared_ptr priorModel = //
+  auto priorModel = noiseModel::Diagonal::Variances(
-      noiseModel::Diagonal::Variances((Vector(6) << 1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4).finished());
+      (Vector(6) << 1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4).finished());
   Key firstKey = 0;
-  for(const Values::ConstKeyValuePair& key_value: *initial) {
+  for (const Values::ConstKeyValuePair& key_value : *initial) {
     std::cout << "Adding prior to g2o file " << std::endl;
     firstKey = key_value.key;
     graph->addPrior(firstKey, Pose3(), priorModel);
@@ -52,13 +51,13 @@ int main(const int argc, const char *argv[]) {
 
   std::cout << "Optimizing the factor graph" << std::endl;
   GaussNewtonParams params;
-  params.setVerbosity("TERMINATION"); // this will show info about stopping conditions
+  params.setVerbosity("TERMINATION");  //  show info about stopping conditions
   GaussNewtonOptimizer optimizer(*graph, *initial, params);
   Values result = optimizer.optimize();
   std::cout << "Optimization complete" << std::endl;
 
-  std::cout << "initial error=" <<graph->error(*initial)<< std::endl;
+  std::cout << "initial error=" << graph->error(*initial) << std::endl;
-  std::cout << "final error=" <<graph->error(result)<< std::endl;
+  std::cout << "final error=" << graph->error(result) << std::endl;
 
   if (argc < 3) {
     result.print("result");

examples/Pose3SLAMExample_initializePose3Chordal.cpp

@@ -10,9 +10,9 @@
  * -------------------------------------------------------------------------- */
 
 /**
- * @file Pose3SLAMExample_initializePose3.cpp
+ * @file Pose3SLAMExample_initializePose3Chordal.cpp
  * @brief A 3D Pose SLAM example that reads input from g2o, and initializes the Pose3 using InitializePose3
- * Syntax for the script is ./Pose3SLAMExample_initializePose3 input.g2o output.g2o
+ * Syntax for the script is ./Pose3SLAMExample_initializePose3Chordal input.g2o output.g2o
  * @date Aug 25, 2014
  * @author Luca Carlone
  */
@@ -25,8 +25,7 @@
 using namespace std;
 using namespace gtsam;
 
-int main(const int argc, const char *argv[]) {
+int main(const int argc, const char* argv[]) {
-
   // Read graph from file
   string g2oFile;
   if (argc < 2)
@@ -40,10 +39,10 @@ int main(const int argc, const char *argv[]) {
   boost::tie(graph, initial) = readG2o(g2oFile, is3D);
 
   // Add prior on the first key
-  noiseModel::Diagonal::shared_ptr priorModel = //
+  auto priorModel = noiseModel::Diagonal::Variances(
-      noiseModel::Diagonal::Variances((Vector(6) << 1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4).finished());
+      (Vector(6) << 1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4).finished());
   Key firstKey = 0;
-  for(const Values::ConstKeyValuePair& key_value: *initial) {
+  for (const Values::ConstKeyValuePair& key_value : *initial) {
     std::cout << "Adding prior to g2o file " << std::endl;
     firstKey = key_value.key;
     graph->addPrior(firstKey, Pose3(), priorModel);
@@ -58,7 +57,7 @@ int main(const int argc, const char *argv[]) {
     initialization.print("initialization");
   } else {
     const string outputFile = argv[2];
-    std::cout << "Writing results to file: " << outputFile  << std::endl;
+    std::cout << "Writing results to file: " << outputFile << std::endl;
     NonlinearFactorGraph::shared_ptr graphNoKernel;
     Values::shared_ptr initial2;
     boost::tie(graphNoKernel, initial2) = readG2o(g2oFile);

examples/Pose3SLAMExample_initializePose3Gradient.cpp

@@ -10,9 +10,9 @@
  * -------------------------------------------------------------------------- */
 
 /**
- * @file Pose3SLAMExample_initializePose3.cpp
+ * @file Pose3SLAMExample_initializePose3Gradient.cpp
  * @brief A 3D Pose SLAM example that reads input from g2o, and initializes the Pose3 using InitializePose3
- * Syntax for the script is ./Pose3SLAMExample_initializePose3 input.g2o output.g2o
+ * Syntax for the script is ./Pose3SLAMExample_initializePose3Gradient input.g2o output.g2o
  * @date Aug 25, 2014
  * @author Luca Carlone
  */
@@ -25,8 +25,7 @@
 using namespace std;
 using namespace gtsam;
 
-int main(const int argc, const char *argv[]) {
+int main(const int argc, const char* argv[]) {
-
   // Read graph from file
   string g2oFile;
   if (argc < 2)
@@ -40,10 +39,10 @@ int main(const int argc, const char *argv[]) {
   boost::tie(graph, initial) = readG2o(g2oFile, is3D);
 
   // Add prior on the first key
-  noiseModel::Diagonal::shared_ptr priorModel = //
+  auto priorModel = noiseModel::Diagonal::Variances(
-      noiseModel::Diagonal::Variances((Vector(6) << 1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4).finished());
+      (Vector(6) << 1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4).finished());
   Key firstKey = 0;
-  for(const Values::ConstKeyValuePair& key_value: *initial) {
+  for (const Values::ConstKeyValuePair& key_value : *initial) {
     std::cout << "Adding prior to g2o file " << std::endl;
     firstKey = key_value.key;
     graph->addPrior(firstKey, Pose3(), priorModel);
@@ -52,17 +51,19 @@ int main(const int argc, const char *argv[]) {
 
   std::cout << "Initializing Pose3 - Riemannian gradient" << std::endl;
   bool useGradient = true;
-  Values initialization = InitializePose3::initialize(*graph, *initial, useGradient);
+  Values initialization =
+      InitializePose3::initialize(*graph, *initial, useGradient);
   std::cout << "done!" << std::endl;
 
-  std::cout << "initial error=" <<graph->error(*initial)<< std::endl;
+  std::cout << "initial error=" << graph->error(*initial) << std::endl;
-  std::cout << "initialization error=" <<graph->error(initialization)<< std::endl;
+  std::cout << "initialization error=" << graph->error(initialization)
+            << std::endl;
 
   if (argc < 3) {
     initialization.print("initialization");
   } else {
     const string outputFile = argv[2];
-    std::cout << "Writing results to file: " << outputFile  << std::endl;
+    std::cout << "Writing results to file: " << outputFile << std::endl;
     NonlinearFactorGraph::shared_ptr graphNoKernel;
     Values::shared_ptr initial2;
     boost::tie(graphNoKernel, initial2) = readG2o(g2oFile);

examples/SFMExample.cpp

@@ -56,12 +56,12 @@ using namespace gtsam;
 
 /* ************************************************************************* */
 int main(int argc, char* argv[]) {
-
   // Define the camera calibration parameters
   Cal3_S2::shared_ptr K(new Cal3_S2(50.0, 50.0, 0.0, 50.0, 50.0));
 
   // Define the camera observation noise model
-  noiseModel::Isotropic::shared_ptr measurementNoise = noiseModel::Isotropic::Sigma(2, 1.0); // one pixel in u and v
+  auto measurementNoise =
+      noiseModel::Isotropic::Sigma(2, 1.0);  // one pixel in u and v
 
   // Create the set of ground-truth landmarks
   vector<Point3> points = createPoints();
@@ -73,32 +73,45 @@ int main(int argc, char* argv[]) {
   NonlinearFactorGraph graph;
 
   // Add a prior on pose x1. This indirectly specifies where the origin is.
-  noiseModel::Diagonal::shared_ptr poseNoise = noiseModel::Diagonal::Sigmas((Vector(6) << Vector3::Constant(0.1), Vector3::Constant(0.3)).finished()); // 30cm std on x,y,z 0.1 rad on roll,pitch,yaw
+  auto poseNoise = noiseModel::Diagonal::Sigmas(
-  graph.addPrior(Symbol('x', 0), poses[0], poseNoise); // add directly to graph
+      (Vector(6) << Vector3::Constant(0.1), Vector3::Constant(0.3))
+          .finished());  // 30cm std on x,y,z 0.1 rad on roll,pitch,yaw
+  graph.addPrior(Symbol('x', 0), poses[0], poseNoise);  // add directly to graph
 
-  // Simulated measurements from each camera pose, adding them to the factor graph
+  // Simulated measurements from each camera pose, adding them to the factor
+  // graph
   for (size_t i = 0; i < poses.size(); ++i) {
     PinholeCamera<Cal3_S2> camera(poses[i], *K);
     for (size_t j = 0; j < points.size(); ++j) {
       Point2 measurement = camera.project(points[j]);
-      graph.emplace_shared<GenericProjectionFactor<Pose3, Point3, Cal3_S2> >(measurement, measurementNoise, Symbol('x', i), Symbol('l', j), K);
+      graph.emplace_shared<GenericProjectionFactor<Pose3, Point3, Cal3_S2> >(
+          measurement, measurementNoise, Symbol('x', i), Symbol('l', j), K);
     }
   }
 
-  // Because the structure-from-motion problem has a scale ambiguity, the problem is still under-constrained
+  // Because the structure-from-motion problem has a scale ambiguity, the
-  // Here we add a prior on the position of the first landmark. This fixes the scale by indicating the distance
+  // problem is still under-constrained Here we add a prior on the position of
-  // between the first camera and the first landmark. All other landmark positions are interpreted using this scale.
+  // the first landmark. This fixes the scale by indicating the distance between
-  noiseModel::Isotropic::shared_ptr pointNoise = noiseModel::Isotropic::Sigma(3, 0.1);
+  // the first camera and the first landmark. All other landmark positions are
-  graph.addPrior(Symbol('l', 0), points[0], pointNoise); // add directly to graph
+  // interpreted using this scale.
+  auto pointNoise = noiseModel::Isotropic::Sigma(3, 0.1);
+  graph.addPrior(Symbol('l', 0), points[0],
+                 pointNoise);  // add directly to graph
   graph.print("Factor Graph:\n");
 
   // Create the data structure to hold the initial estimate to the solution
   // Intentionally initialize the variables off from the ground truth
   Values initialEstimate;
-  for (size_t i = 0; i < poses.size(); ++i)
+  for (size_t i = 0; i < poses.size(); ++i) {
-    initialEstimate.insert(Symbol('x', i), poses[i].compose(Pose3(Rot3::Rodrigues(-0.1, 0.2, 0.25), Point3(0.05, -0.10, 0.20))));
+    auto corrupted_pose = poses[i].compose(
-  for (size_t j = 0; j < points.size(); ++j)
+        Pose3(Rot3::Rodrigues(-0.1, 0.2, 0.25), Point3(0.05, -0.10, 0.20)));
-    initialEstimate.insert<Point3>(Symbol('l', j), points[j] + Point3(-0.25, 0.20, 0.15));
+    initialEstimate.insert(
+        Symbol('x', i), corrupted_pose);
+  }
+  for (size_t j = 0; j < points.size(); ++j) {
+    auto corrupted_point = points[j] + Point3(-0.25, 0.20, 0.15);
+    initialEstimate.insert<Point3>(Symbol('l', j), corrupted_point);
+  }
   initialEstimate.print("Initial Estimates:\n");
 
   /* Optimize the graph and print results */

examples/SFMExampleExpressions_bal.cpp

@@ -28,7 +28,7 @@
 // Header order is close to far
 #include <gtsam/inference/Symbol.h>
 #include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
-#include <gtsam/slam/dataset.h> // for loading BAL datasets !
+#include <gtsam/slam/dataset.h>  // for loading BAL datasets !
 #include <vector>
 
 using namespace std;
@@ -40,20 +40,16 @@ using symbol_shorthand::P;
 // An SfmCamera is defined in datase.h as a camera with unknown Cal3Bundler calibration
 // and has a total of 9 free parameters
 
-/* ************************************************************************* */
 int main(int argc, char* argv[]) {
-
   // Find default file, but if an argument is given, try loading a file
   string filename = findExampleDataFile("dubrovnik-3-7-pre");
-  if (argc > 1)
+  if (argc > 1) filename = string(argv[1]);
-    filename = string(argv[1]);
 
   // Load the SfM data from file
   SfmData mydata;
   readBAL(filename, mydata);
-  cout
+  cout << boost::format("read %1% tracks on %2% cameras\n") %
-      << boost::format("read %1% tracks on %2% cameras\n")
+              mydata.number_tracks() % mydata.number_cameras();
-          % mydata.number_tracks() % mydata.number_cameras();
 
   // Create a factor graph
   ExpressionFactorGraph graph;
@@ -65,23 +61,23 @@ int main(int argc, char* argv[]) {
   Pose3_ pose0_(&SfmCamera::getPose, camera0_);
   // Finally, we say it should be equal to first guess
   graph.addExpressionFactor(pose0_, mydata.cameras[0].pose(),
-      noiseModel::Isotropic::Sigma(6, 0.1));
+                            noiseModel::Isotropic::Sigma(6, 0.1));
 
   // similarly, we create a prior on the first point
   Point3_ point0_(P(0));
   graph.addExpressionFactor(point0_, mydata.tracks[0].p,
-      noiseModel::Isotropic::Sigma(3, 0.1));
+                            noiseModel::Isotropic::Sigma(3, 0.1));
 
   // We share *one* noiseModel between all projection factors
-  noiseModel::Isotropic::shared_ptr noise = noiseModel::Isotropic::Sigma(2,
+  auto noise = noiseModel::Isotropic::Sigma(2, 1.0);  // one pixel in u and v
-      1.0); // one pixel in u and v
 
-  // Simulated measurements from each camera pose, adding them to the factor graph
+  // Simulated measurements from each camera pose, adding them to the factor
+  // graph
   size_t j = 0;
-  for(const SfmTrack& track: mydata.tracks) {
+  for (const SfmTrack& track : mydata.tracks) {
     // Leaf expression for j^th point
     Point3_ point_('p', j);
-    for(const SfmMeasurement& m: track.measurements) {
+    for (const SfmMeasurement& m : track.measurements) {
       size_t i = m.first;
       Point2 uv = m.second;
       // Leaf expression for i^th camera
@@ -98,10 +94,8 @@ int main(int argc, char* argv[]) {
   Values initial;
   size_t i = 0;
   j = 0;
-  for(const SfmCamera& camera: mydata.cameras)
+  for (const SfmCamera& camera : mydata.cameras) initial.insert(C(i++), camera);
-    initial.insert(C(i++), camera);
+  for (const SfmTrack& track : mydata.tracks) initial.insert(P(j++), track.p);
-  for(const SfmTrack& track: mydata.tracks)
-    initial.insert(P(j++), track.p);
 
   /* Optimize the graph and print results */
   Values result;
@@ -117,5 +111,3 @@ int main(int argc, char* argv[]) {
 
   return 0;
 }
-/* ************************************************************************* */
-

examples/SFMExample_SmartFactor.cpp

@@ -44,7 +44,7 @@ int main(int argc, char* argv[]) {
   Cal3_S2::shared_ptr K(new Cal3_S2(50.0, 50.0, 0.0, 50.0, 50.0));
 
   // Define the camera observation noise model
-  noiseModel::Isotropic::shared_ptr measurementNoise =
+  auto measurementNoise =
       noiseModel::Isotropic::Sigma(2, 1.0); // one pixel in u and v
 
   // Create the set of ground-truth landmarks and poses
@@ -80,7 +80,7 @@ int main(int argc, char* argv[]) {
 
   // Add a prior on pose x0. This indirectly specifies where the origin is.
   // 30cm std on x,y,z 0.1 rad on roll,pitch,yaw
-  noiseModel::Diagonal::shared_ptr noise = noiseModel::Diagonal::Sigmas(
+  auto noise = noiseModel::Diagonal::Sigmas(
       (Vector(6) << Vector3::Constant(0.1), Vector3::Constant(0.3)).finished());
   graph.addPrior(0, poses[0], noise);
 

examples/SFMExample_SmartFactorPCG.cpp

@@ -35,13 +35,12 @@ typedef PinholePose<Cal3_S2> Camera;
 
 /* ************************************************************************* */
 int main(int argc, char* argv[]) {
-
   // Define the camera calibration parameters
   Cal3_S2::shared_ptr K(new Cal3_S2(50.0, 50.0, 0.0, 50.0, 50.0));
 
   // Define the camera observation noise model
-  noiseModel::Isotropic::shared_ptr measurementNoise =
+  auto measurementNoise =
-      noiseModel::Isotropic::Sigma(2, 1.0); // one pixel in u and v
+      noiseModel::Isotropic::Sigma(2, 1.0);  // one pixel in u and v
 
   // Create the set of ground-truth landmarks and poses
   vector<Point3> points = createPoints();
@@ -52,17 +51,16 @@ int main(int argc, char* argv[]) {
 
   // Simulated measurements from each camera pose, adding them to the factor graph
   for (size_t j = 0; j < points.size(); ++j) {
-
+    // every landmark represent a single landmark, we use shared pointer to init
-    // every landmark represent a single landmark, we use shared pointer to init the factor, and then insert measurements.
+    // the factor, and then insert measurements.
     SmartFactor::shared_ptr smartfactor(new SmartFactor(measurementNoise, K));
 
     for (size_t i = 0; i < poses.size(); ++i) {
-
       // generate the 2D measurement
       Camera camera(poses[i], K);
       Point2 measurement = camera.project(points[j]);
 
-      // call add() function to add measurement into a single factor, here we need to add:
+      // call add() function to add measurement into a single factor
       smartfactor->add(measurement, i);
     }
 
@@ -72,7 +70,7 @@ int main(int argc, char* argv[]) {
 
   // Add a prior on pose x0. This indirectly specifies where the origin is.
   // 30cm std on x,y,z 0.1 rad on roll,pitch,yaw
-  noiseModel::Diagonal::shared_ptr noise = noiseModel::Diagonal::Sigmas(
+  auto noise = noiseModel::Diagonal::Sigmas(
       (Vector(6) << Vector3::Constant(0.1), Vector3::Constant(0.3)).finished());
   graph.addPrior(0, poses[0], noise);
 
@@ -85,9 +83,9 @@ int main(int argc, char* argv[]) {
   for (size_t i = 0; i < poses.size(); ++i)
     initialEstimate.insert(i, poses[i].compose(delta));
 
-  // We will use LM in the outer optimization loop, but by specifying "Iterative" below
+  // We will use LM in the outer optimization loop, but by specifying
-  // We indicate that an iterative linear solver should be used.
+  // "Iterative" below We indicate that an iterative linear solver should be
-  // In addition, the *type* of the iterativeParams decides on the type of
+  // used. In addition, the *type* of the iterativeParams decides on the type of
   // iterative solver, in this case the SPCG (subgraph PCG)
   LevenbergMarquardtParams parameters;
   parameters.linearSolverType = NonlinearOptimizerParams::Iterative;
@@ -110,11 +108,10 @@ int main(int argc, char* argv[]) {
   result.print("Final results:\n");
   Values landmark_result;
   for (size_t j = 0; j < points.size(); ++j) {
-    SmartFactor::shared_ptr smart = //
+    auto smart = boost::dynamic_pointer_cast<SmartFactor>(graph[j]);
-        boost::dynamic_pointer_cast<SmartFactor>(graph[j]);
     if (smart) {
       boost::optional<Point3> point = smart->point(result);
-      if (point) // ignore if boost::optional return nullptr
+      if (point)  // ignore if boost::optional return nullptr
         landmark_result.insert(j, *point);
     }
   }

examples/SFMExample_bal.cpp

@@ -49,7 +49,7 @@ int main (int argc, char* argv[]) {
   NonlinearFactorGraph graph;
 
   // We share *one* noiseModel between all projection factors
-  noiseModel::Isotropic::shared_ptr noise =
+  auto noise =
       noiseModel::Isotropic::Sigma(2, 1.0); // one pixel in u and v
 
   // Add measurements to the factor graph

examples/SFMExample_bal_COLAMD_METIS.cpp

@@ -10,7 +10,7 @@
  * -------------------------------------------------------------------------- */
 
 /**
- * @file    SFMExample.cpp
+ * @file    SFMExample_bal_COLAMD_METIS.cpp
  * @brief   This file is to compare the ordering performance for COLAMD vs METIS.
  * Example problem is to solve a structure-from-motion problem from a "Bundle Adjustment in the Large" file.
  * @author  Frank Dellaert, Zhaoyang Lv
@@ -22,7 +22,7 @@
 #include <gtsam/nonlinear/NonlinearFactorGraph.h>
 #include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
 #include <gtsam/slam/GeneralSFMFactor.h>
-#include <gtsam/slam/dataset.h> // for loading BAL datasets !
+#include <gtsam/slam/dataset.h>  // for loading BAL datasets !
 
 #include <gtsam/base/timing.h>
 
@@ -34,50 +34,52 @@ using symbol_shorthand::C;
 using symbol_shorthand::P;
 
 // We will be using a projection factor that ties a SFM_Camera to a 3D point.
-// An SFM_Camera is defined in datase.h as a camera with unknown Cal3Bundler calibration
+// An SFM_Camera is defined in datase.h as a camera with unknown Cal3Bundler
-// and has a total of 9 free parameters
+// calibration and has a total of 9 free parameters
-typedef GeneralSFMFactor<SfmCamera,Point3> MyFactor;
+typedef GeneralSFMFactor<SfmCamera, Point3> MyFactor;
 
-/* ************************************************************************* */
-int main (int argc, char* argv[]) {
 
+int main(int argc, char* argv[]) {
   // Find default file, but if an argument is given, try loading a file
   string filename = findExampleDataFile("dubrovnik-3-7-pre");
-  if (argc>1) filename = string(argv[1]);
+  if (argc > 1) filename = string(argv[1]);
 
   // Load the SfM data from file
   SfmData mydata;
   readBAL(filename, mydata);
-  cout << boost::format("read %1% tracks on %2% cameras\n") % mydata.number_tracks() % mydata.number_cameras();
+  cout << boost::format("read %1% tracks on %2% cameras\n") %
+              mydata.number_tracks() % mydata.number_cameras();
 
   // Create a factor graph
   NonlinearFactorGraph graph;
 
   // We share *one* noiseModel between all projection factors
-  noiseModel::Isotropic::shared_ptr noise =
+  auto noise = noiseModel::Isotropic::Sigma(2, 1.0);  // one pixel in u and v
-      noiseModel::Isotropic::Sigma(2, 1.0); // one pixel in u and v
 
   // Add measurements to the factor graph
   size_t j = 0;
-  for(const SfmTrack& track: mydata.tracks) {
+  for (const SfmTrack& track : mydata.tracks) {
-    for(const SfmMeasurement& m: track.measurements) {
+    for (const SfmMeasurement& m : track.measurements) {
       size_t i = m.first;
       Point2 uv = m.second;
-      graph.emplace_shared<MyFactor>(uv, noise, C(i), P(j)); // note use of shorthand symbols C and P
+      graph.emplace_shared<MyFactor>(
+          uv, noise, C(i), P(j));  // note use of shorthand symbols C and P
     }
     j += 1;
   }
 
   // Add a prior on pose x1. This indirectly specifies where the origin is.
   // and a prior on the position of the first landmark to fix the scale
-  graph.addPrior(C(0), mydata.cameras[0],  noiseModel::Isotropic::Sigma(9, 0.1));
+  graph.addPrior(C(0), mydata.cameras[0], noiseModel::Isotropic::Sigma(9, 0.1));
-  graph.addPrior(P(0), mydata.tracks[0].p, noiseModel::Isotropic::Sigma(3, 0.1));
+  graph.addPrior(P(0), mydata.tracks[0].p,
+                 noiseModel::Isotropic::Sigma(3, 0.1));
 
   // Create initial estimate
   Values initial;
-  size_t i = 0; j = 0;
+  size_t i = 0;
-  for(const SfmCamera& camera: mydata.cameras) initial.insert(C(i++), camera);
+  j = 0;
-  for(const SfmTrack& track: mydata.tracks)    initial.insert(P(j++), track.p);
+  for (const SfmCamera& camera : mydata.cameras) initial.insert(C(i++), camera);
+  for (const SfmTrack& track : mydata.tracks) initial.insert(P(j++), track.p);
 
   /** ---------------  COMPARISON  -----------------------**/
   /** ----------------------------------------------------**/
@@ -98,7 +100,7 @@ int main (int argc, char* argv[]) {
   }
 
   // expect they have different ordering results
-  if(params_using_COLAMD.ordering == params_using_METIS.ordering) {
+  if (params_using_COLAMD.ordering == params_using_METIS.ordering) {
     cout << "COLAMD and METIS produce the same ordering. "
          << "Problem here!!!" << endl;
   }
@@ -120,8 +122,7 @@ int main (int argc, char* argv[]) {
     cout << e.what();
   }
 
-
+  {  // printing the result
-  { // printing the result
 
     cout << "COLAMD final error: " << graph.error(result_COLAMD) << endl;
     cout << "METIS final error: " << graph.error(result_METIS) << endl;
@@ -129,15 +130,13 @@ int main (int argc, char* argv[]) {
     cout << endl << endl;
 
     cout << "Time comparison by solving " << filename << " results:" << endl;
-    cout << boost::format("%1% point tracks and %2% cameras\n") \
+    cout << boost::format("%1% point tracks and %2% cameras\n") %
-            % mydata.number_tracks() % mydata.number_cameras() \
+                mydata.number_tracks() % mydata.number_cameras()
          << endl;
 
     tictoc_print_();
   }
 
-
   return 0;
 }
-/* ************************************************************************* */
 

examples/SelfCalibrationExample.cpp

@@ -33,7 +33,7 @@
 #include <gtsam/nonlinear/Values.h>
 
 // SFM-specific factors
-#include <gtsam/slam/GeneralSFMFactor.h> // does calibration !
+#include <gtsam/slam/GeneralSFMFactor.h>  // does calibration !
 
 // Standard headers
 #include <vector>
@@ -41,9 +41,7 @@
 using namespace std;
 using namespace gtsam;
 
-/* ************************************************************************* */
 int main(int argc, char* argv[]) {
-
   // Create the set of ground-truth
   vector<Point3> points = createPoints();
   vector<Pose3> poses = createPoses();
@@ -52,28 +50,38 @@ int main(int argc, char* argv[]) {
   NonlinearFactorGraph graph;
 
   // Add a prior on pose x1.
-  noiseModel::Diagonal::shared_ptr poseNoise = noiseModel::Diagonal::Sigmas((Vector(6) << Vector3::Constant(0.1), Vector3::Constant(0.3)).finished()); // 30cm std on x,y,z 0.1 rad on roll,pitch,yaw
+  auto poseNoise = noiseModel::Diagonal::Sigmas(
+      (Vector(6) << Vector3::Constant(0.1), Vector3::Constant(0.3))
+          .finished());  // 30cm std on x,y,z 0.1 rad on roll,pitch,yaw
   graph.addPrior(Symbol('x', 0), poses[0], poseNoise);
 
-  // Simulated measurements from each camera pose, adding them to the factor graph
+  // Simulated measurements from each camera pose, adding them to the factor
+  // graph
   Cal3_S2 K(50.0, 50.0, 0.0, 50.0, 50.0);
-  noiseModel::Isotropic::shared_ptr measurementNoise = noiseModel::Isotropic::Sigma(2, 1.0);
+  auto measurementNoise =
+      noiseModel::Isotropic::Sigma(2, 1.0);
   for (size_t i = 0; i < poses.size(); ++i) {
     for (size_t j = 0; j < points.size(); ++j) {
       PinholeCamera<Cal3_S2> camera(poses[i], K);
       Point2 measurement = camera.project(points[j]);
-      // The only real difference with the Visual SLAM example is that here we use a
+      // The only real difference with the Visual SLAM example is that here we
-      // different factor type, that also calculates the Jacobian with respect to calibration
+      // use a different factor type, that also calculates the Jacobian with
-      graph.emplace_shared<GeneralSFMFactor2<Cal3_S2> >(measurement, measurementNoise, Symbol('x', i), Symbol('l', j), Symbol('K', 0));
+      // respect to calibration
+      graph.emplace_shared<GeneralSFMFactor2<Cal3_S2> >(
+          measurement, measurementNoise, Symbol('x', i), Symbol('l', j),
+          Symbol('K', 0));
     }
   }
 
   // Add a prior on the position of the first landmark.
-  noiseModel::Isotropic::shared_ptr pointNoise = noiseModel::Isotropic::Sigma(3, 0.1);
+  auto pointNoise =
-  graph.addPrior(Symbol('l', 0), points[0], pointNoise); // add directly to graph
+      noiseModel::Isotropic::Sigma(3, 0.1);
+  graph.addPrior(Symbol('l', 0), points[0],
+                 pointNoise);  // add directly to graph
 
   // Add a prior on the calibration.
-  noiseModel::Diagonal::shared_ptr calNoise = noiseModel::Diagonal::Sigmas((Vector(5) << 500, 500, 0.1, 100, 100).finished());
+  auto calNoise = noiseModel::Diagonal::Sigmas(
+      (Vector(5) << 500, 500, 0.1, 100, 100).finished());
   graph.addPrior(Symbol('K', 0), K, calNoise);
 
   // Create the initial estimate to the solution
@@ -81,9 +89,12 @@ int main(int argc, char* argv[]) {
   Values initialEstimate;
   initialEstimate.insert(Symbol('K', 0), Cal3_S2(60.0, 60.0, 0.0, 45.0, 45.0));
   for (size_t i = 0; i < poses.size(); ++i)
-    initialEstimate.insert(Symbol('x', i), poses[i].compose(Pose3(Rot3::Rodrigues(-0.1, 0.2, 0.25), Point3(0.05, -0.10, 0.20))));
+    initialEstimate.insert(
+        Symbol('x', i), poses[i].compose(Pose3(Rot3::Rodrigues(-0.1, 0.2, 0.25),
+                                               Point3(0.05, -0.10, 0.20))));
   for (size_t j = 0; j < points.size(); ++j)
-    initialEstimate.insert<Point3>(Symbol('l', j), points[j] + Point3(-0.25, 0.20, 0.15));
+    initialEstimate.insert<Point3>(Symbol('l', j),
+                                   points[j] + Point3(-0.25, 0.20, 0.15));
 
   /* Optimize the graph and print results */
   Values result = DoglegOptimizer(graph, initialEstimate).optimize();
@@ -91,5 +102,4 @@ int main(int argc, char* argv[]) {
 
   return 0;
 }
-/* ************************************************************************* */
 

examples/SimpleRotation.cpp

@@ -59,7 +59,6 @@ using namespace gtsam;
 const double degree = M_PI / 180;
 
 int main() {
-
   /**
    *    Step 1: Create a factor to express a unary constraint
    * The "prior" in this case is the measurement from a sensor,
@@ -76,8 +75,8 @@ int main() {
    */
   Rot2 prior = Rot2::fromAngle(30 * degree);
   prior.print("goal angle");
-  noiseModel::Isotropic::shared_ptr model = noiseModel::Isotropic::Sigma(1, 1 * degree);
+  auto model = noiseModel::Isotropic::Sigma(1, 1 * degree);
-  Symbol key('x',1);
+  Symbol key('x', 1);
 
   /**
    *    Step 2: Create a graph container and add the factor to it

examples/StereoVOExample.cpp

@@ -10,7 +10,7 @@
  * -------------------------------------------------------------------------- */
 
 /**
- * @file SteroVOExample.cpp
+ * @file StereoVOExample.cpp
  * @brief A stereo visual odometry example
  * @date May 25, 2014
  * @author Stephen Camp
@@ -34,17 +34,17 @@
 using namespace std;
 using namespace gtsam;
 
-int main(int argc, char** argv){
+int main(int argc, char** argv) {
-
+  // create graph object, add first pose at origin with key '1'
-  //create graph object, add first pose at origin with key '1'
   NonlinearFactorGraph graph;
   Pose3 first_pose;
   graph.emplace_shared<NonlinearEquality<Pose3> >(1, Pose3());
 
-  //create factor noise model with 3 sigmas of value 1
+  // create factor noise model with 3 sigmas of value 1
-  const noiseModel::Isotropic::shared_ptr model = noiseModel::Isotropic::Sigma(3,1);
+  const auto model = noiseModel::Isotropic::Sigma(3, 1);
-  //create stereo camera calibration object with .2m between cameras
+  // create stereo camera calibration object with .2m between cameras
-  const Cal3_S2Stereo::shared_ptr K(new Cal3_S2Stereo(1000, 1000, 0, 320, 240, 0.2));
+  const Cal3_S2Stereo::shared_ptr K(
+      new Cal3_S2Stereo(1000, 1000, 0, 320, 240, 0.2));
 
   //create and add stereo factors between first pose (key value 1) and the three landmarks
   graph.emplace_shared<GenericStereoFactor<Pose3,Point3> >(StereoPoint2(520, 480, 440), model, 1, 3, K);
@@ -56,17 +56,18 @@ int main(int argc, char** argv){
   graph.emplace_shared<GenericStereoFactor<Pose3,Point3> >(StereoPoint2(70, 20, 490), model, 2, 4, K);
   graph.emplace_shared<GenericStereoFactor<Pose3,Point3> >(StereoPoint2(320, 270, 115), model, 2, 5, K);
 
-  //create Values object to contain initial estimates of camera poses and landmark locations
+  // create Values object to contain initial estimates of camera poses and
+  // landmark locations
   Values initial_estimate;
 
-  //create and add iniital estimates
+  // create and add iniital estimates
   initial_estimate.insert(1, first_pose);
   initial_estimate.insert(2, Pose3(Rot3(), Point3(0.1, -0.1, 1.1)));
   initial_estimate.insert(3, Point3(1, 1, 5));
   initial_estimate.insert(4, Point3(-1, 1, 5));
   initial_estimate.insert(5, Point3(0, -0.5, 5));
 
-  //create Levenberg-Marquardt optimizer for resulting factor graph, optimize
+  // create Levenberg-Marquardt optimizer for resulting factor graph, optimize
   LevenbergMarquardtOptimizer optimizer(graph, initial_estimate);
   Values result = optimizer.optimize();
 

examples/StereoVOExample_large.cpp

@@ -10,7 +10,7 @@
 * -------------------------------------------------------------------------- */
 
 /**
-* @file SteroVOExample.cpp
+* @file StereoVOExample_large.cpp
 * @brief A stereo visual odometry example
 * @date May 25, 2014
 * @author Stephen Camp
@@ -41,31 +41,31 @@
 using namespace std;
 using namespace gtsam;
 
-int main(int argc, char** argv){
+int main(int argc, char** argv) {
-
   Values initial_estimate;
   NonlinearFactorGraph graph;
-  const noiseModel::Isotropic::shared_ptr model = noiseModel::Isotropic::Sigma(3,1);
+  const auto model = noiseModel::Isotropic::Sigma(3, 1);
 
   string calibration_loc = findExampleDataFile("VO_calibration.txt");
   string pose_loc = findExampleDataFile("VO_camera_poses_large.txt");
   string factor_loc = findExampleDataFile("VO_stereo_factors_large.txt");
 
-  //read camera calibration info from file
+  // read camera calibration info from file
   // focal lengths fx, fy, skew s, principal point u0, v0, baseline b
   double fx, fy, s, u0, v0, b;
   ifstream calibration_file(calibration_loc.c_str());
   cout << "Reading calibration info" << endl;
   calibration_file >> fx >> fy >> s >> u0 >> v0 >> b;
 
-  //create stereo camera calibration object
+  // create stereo camera calibration object
-  const Cal3_S2Stereo::shared_ptr K(new Cal3_S2Stereo(fx,fy,s,u0,v0,b));
+  const Cal3_S2Stereo::shared_ptr K(new Cal3_S2Stereo(fx, fy, s, u0, v0, b));
 
   ifstream pose_file(pose_loc.c_str());
   cout << "Reading camera poses" << endl;
   int pose_id;
-  MatrixRowMajor m(4,4);
+  MatrixRowMajor m(4, 4);
-  //read camera pose parameters and use to make initial estimates of camera poses
+  // read camera pose parameters and use to make initial estimates of camera
+  // poses
   while (pose_file >> pose_id) {
     for (int i = 0; i < 16; i++) {
       pose_file >> m.data()[i];
@@ -76,33 +76,37 @@ int main(int argc, char** argv){
   // camera and landmark keys
   size_t x, l;
 
-  // pixel coordinates uL, uR, v (same for left/right images due to rectification)
+  // pixel coordinates uL, uR, v (same for left/right images due to
-  // landmark coordinates X, Y, Z in camera frame, resulting from triangulation
+  // rectification) landmark coordinates X, Y, Z in camera frame, resulting from
+  // triangulation
   double uL, uR, v, X, Y, Z;
   ifstream factor_file(factor_loc.c_str());
   cout << "Reading stereo factors" << endl;
-  //read stereo measurement details from file and use to create and add GenericStereoFactor objects to the graph representation
+  // read stereo measurement details from file and use to create and add
+  // GenericStereoFactor objects to the graph representation
   while (factor_file >> x >> l >> uL >> uR >> v >> X >> Y >> Z) {
-    graph.emplace_shared<
+    graph.emplace_shared<GenericStereoFactor<Pose3, Point3> >(
-        GenericStereoFactor<Pose3, Point3> >(StereoPoint2(uL, uR, v), model,
+        StereoPoint2(uL, uR, v), model, Symbol('x', x), Symbol('l', l), K);
-            Symbol('x', x), Symbol('l', l), K);
+    // if the landmark variable included in this factor has not yet been added
-    //if the landmark variable included in this factor has not yet been added to the initial variable value estimate, add it
+    // to the initial variable value estimate, add it
     if (!initial_estimate.exists(Symbol('l', l))) {
       Pose3 camPose = initial_estimate.at<Pose3>(Symbol('x', x));
-      //transformFrom() transforms the input Point3 from the camera pose space, camPose, to the global space
+      // transformFrom() transforms the input Point3 from the camera pose space,
+      // camPose, to the global space
       Point3 worldPoint = camPose.transformFrom(Point3(X, Y, Z));
       initial_estimate.insert(Symbol('l', l), worldPoint);
     }
   }
 
-  Pose3 first_pose = initial_estimate.at<Pose3>(Symbol('x',1));
+  Pose3 first_pose = initial_estimate.at<Pose3>(Symbol('x', 1));
-  //constrain the first pose such that it cannot change from its original value during optimization
+  // constrain the first pose such that it cannot change from its original value
+  // during optimization
   // NOTE: NonlinearEquality forces the optimizer to use QR rather than Cholesky
   // QR is much slower than Cholesky, but numerically more stable
-  graph.emplace_shared<NonlinearEquality<Pose3> >(Symbol('x',1),first_pose);
+  graph.emplace_shared<NonlinearEquality<Pose3> >(Symbol('x', 1), first_pose);
 
   cout << "Optimizing" << endl;
-  //create Levenberg-Marquardt optimizer to optimize the factor graph
+  // create Levenberg-Marquardt optimizer to optimize the factor graph
   LevenbergMarquardtParams params;
   params.orderingType = Ordering::METIS;
   LevenbergMarquardtOptimizer optimizer(graph, initial_estimate, params);

examples/VisualISAMExample.cpp

@@ -56,13 +56,11 @@ using namespace gtsam;
 
 /* ************************************************************************* */
 int main(int argc, char* argv[]) {
-
   // Define the camera calibration parameters
   Cal3_S2::shared_ptr K(new Cal3_S2(50.0, 50.0, 0.0, 50.0, 50.0));
 
   // Define the camera observation noise model
-  noiseModel::Isotropic::shared_ptr noise = //
+  auto noise = noiseModel::Isotropic::Sigma(2, 1.0);  // one pixel in u and v
-      noiseModel::Isotropic::Sigma(2, 1.0); // one pixel in u and v
 
   // Create the set of ground-truth landmarks
   vector<Point3> points = createPoints();
@@ -81,7 +79,6 @@ int main(int argc, char* argv[]) {
 
   // Loop over the different poses, adding the observations to iSAM incrementally
   for (size_t i = 0; i < poses.size(); ++i) {
-
     // Add factors for each landmark observation
     for (size_t j = 0; j < points.size(); ++j) {
       // Create ground truth measurement
@@ -105,12 +102,12 @@ int main(int argc, char* argv[]) {
     // adding it to iSAM.
     if (i == 0) {
       // Add a prior on pose x0, with 30cm std on x,y,z 0.1 rad on roll,pitch,yaw
-      noiseModel::Diagonal::shared_ptr poseNoise = noiseModel::Diagonal::Sigmas(
+      auto poseNoise = noiseModel::Diagonal::Sigmas(
           (Vector(6) << Vector3::Constant(0.1), Vector3::Constant(0.3)).finished());
       graph.addPrior(Symbol('x', 0), poses[0], poseNoise);
 
       // Add a prior on landmark l0
-      noiseModel::Isotropic::shared_ptr pointNoise =
+      auto pointNoise =
           noiseModel::Isotropic::Sigma(3, 0.1);
       graph.addPrior(Symbol('l', 0), points[0], pointNoise);