Merged in feature/SmartCT (pull request #107)

Refactoring of Smart Factors

doc/.gitignore

@@ -0,0 +1 @@
+/html/

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 <gtsam/slam/expressions.h>
 #include <gtsam/nonlinear/ExpressionFactorGraph.h>
 
-// Header order is close to far
+// For an explanation of headers below, please see Pose2SLAMExample.cpp
-#include <gtsam/nonlinear/NonlinearFactorGraph.h>
+#include <gtsam/slam/PriorFactor.h>
+#include <gtsam/slam/BetweenFactor.h>
+#include <gtsam/geometry/Pose2.h>
 #include <gtsam/nonlinear/GaussNewtonOptimizer.h>
 #include <gtsam/nonlinear/Marginals.h>
-#include <gtsam/nonlinear/Values.h>
-#include <gtsam/geometry/Pose2.h>
-#include <gtsam/inference/Key.h>
 
 using namespace std;
 using namespace gtsam;

examples/Pose2SLAMExample_graph.cpp

@@ -16,11 +16,14 @@
  * @author Frank Dellaert
  */
 
-#include <gtsam/slam/dataset.h>
+// For an explanation of headers below, please see Pose2SLAMExample.cpp
 #include <gtsam/slam/PriorFactor.h>
-#include <gtsam/nonlinear/Marginals.h>
+#include <gtsam/slam/BetweenFactor.h>
 #include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
-#include <gtsam/geometry/Pose2.h>
+#include <gtsam/nonlinear/Marginals.h>
+
+// This new header allows us to read examples easily from .graph files
+#include <gtsam/slam/dataset.h>
 
 using namespace std;
 using namespace gtsam;

examples/Pose2SLAMExample_graphviz.cpp

@@ -16,11 +16,11 @@
  * @author Frank Dellaert
  */
 
+// For an explanation of headers below, please see Pose2SLAMExample.cpp
 #include <gtsam/slam/PriorFactor.h>
 #include <gtsam/slam/BetweenFactor.h>
-#include <gtsam/nonlinear/Marginals.h>
-#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
 #include <gtsam/geometry/Pose2.h>
+#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
 #include <fstream>
 
 using namespace std;

examples/Pose2SLAMwSPCG.cpp

@@ -16,47 +16,15 @@
  * @date June 2, 2012
  */
 
-/**
+// For an explanation of headers below, please see Pose2SLAMExample.cpp
- * 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 <gtsam/geometry/Pose2.h>
-#include <gtsam/geometry/Point2.h>
-
-// 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 <gtsam/inference/Key.h>
-
-// 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.
 #include <gtsam/slam/PriorFactor.h>
 #include <gtsam/slam/BetweenFactor.h>
-
+#include <gtsam/geometry/Pose2.h>
-// 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 <gtsam/nonlinear/NonlinearFactorGraph.h>
-
-// 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 <gtsam/nonlinear/Values.h>
-
-#include <gtsam/linear/SubgraphSolver.h>
 #include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
 
+// In contrast to that example, however, we will use a PCG solver here
+#include <gtsam/linear/SubgraphSolver.h>
+
 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<Pose2>(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<Pose2>(1, 2, Pose2(2.0, 0.0, M_PI_2),    odometryNoise));
   graph.push_back(BetweenFactor<Pose2>(2, 3, Pose2(2.0, 0.0, M_PI_2), odometryNoise));
   graph.push_back(BetweenFactor<Pose2>(3, 4, Pose2(2.0, 0.0, M_PI_2), odometryNoise));
   graph.push_back(BetweenFactor<Pose2>(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<Pose2>(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;
 
-  {
+  // 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<SubgraphSolverParameters>();
+
   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;
 }

examples/SFMExample.cpp

@@ -15,13 +15,7 @@
  * @author  Duy-Nguyen Ta
  */
 
-/**
+// For loading the data, see the comments therein for scenario (camera rotates around cube)
- * 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).

examples/SFMExample_SmartFactor.cpp

@@ -17,51 +17,22 @@
  * @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 <gtsam/geometry/Point2.h>
-
 // In GTSAM, measurement functions are represented as 'factors'.
-// The factor we used here is SmartProjectionPoseFactor. Every smart factor represent a single landmark,
+// The factor we used here is SmartProjectionPoseFactor.
-// The SmartProjectionPoseFactor only optimize the pose of camera, not the calibration,
+// Every smart factor represent a single landmark, seen from multiple cameras.
-// The calibration should be known.
+// The SmartProjectionPoseFactor only optimizes for the poses of a camera,
+// not the calibration, which is assumed known.
 #include <gtsam/slam/SmartProjectionPoseFactor.h>
 
-// Also, we will initialize the robot at some location using a Prior factor.
+// For an explanation of these headers, see SFMExample.cpp
-#include <gtsam/slam/PriorFactor.h>
+#include "SFMdata.h"
-
+#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
-// 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 <gtsam/nonlinear/NonlinearFactorGraph.h>
-
-// 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
-#include <gtsam/nonlinear/DoglegOptimizer.h>
-
-// 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 <gtsam/nonlinear/Values.h>
-
-#include <vector>
 
 using namespace std;
 using namespace gtsam;
 
 // Make the typename short so it looks much cleaner
-typedef gtsam::SmartProjectionPoseFactor<gtsam::Pose3, gtsam::Cal3_S2> SmartFactor;
+typedef SmartProjectionPoseFactor<Cal3_S2> SmartFactor;
 
 /* ************************************************************************* */
 int main(int argc, char* argv[]) {
@@ -127,7 +98,8 @@ int main(int argc, char* argv[]) {
   initialEstimate.print("Initial Estimates:\n");
 
   // Optimize the graph and print results
-  Values result = DoglegOptimizer(graph, initialEstimate).optimize();
+  LevenbergMarquardtOptimizer optimizer(graph, initialEstimate);
+  Values result = optimizer.optimize();
   result.print("Final results:\n");
 
   // A smart factor represent the 3D point inside the factor, not as a variable.
@@ -136,20 +108,20 @@ int main(int argc, char* argv[]) {
   Values landmark_result;
   for (size_t j = 0; j < points.size(); ++j) {
 
-    // The output of point() is in boost::optional<gtsam::Point3>, as sometimes
-    // the triangulation operation inside smart factor will encounter degeneracy.
-    boost::optional<Point3> point;
-
     // The graph stores Factor shared_ptrs, so we cast back to a SmartFactor first
     SmartFactor::shared_ptr smart = boost::dynamic_pointer_cast<SmartFactor>(graph[j]);
     if (smart) {
-      point = smart->point(result);
+      // The output of point() is in boost::optional<Point3>, as sometimes
+      // the triangulation operation inside smart factor will encounter degeneracy.
+      boost::optional<Point3> point = smart->point(result);
       if (point) // ignore if boost::optional return NULL
         landmark_result.insert(j, *point);
     }
   }
 
   landmark_result.print("Landmark results:\n");
+  cout << "final error: " << graph.error(result) << endl;
+  cout << "number of iterations: " << optimizer.iterations() << endl;
 
   return 0;
 }

examples/SFMExample_SmartFactorPCG.cpp

@@ -0,0 +1,126 @@
+/* ----------------------------------------------------------------------------
+
+ * 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    SFMExample_SmartFactorPCG.cpp
+ * @brief   Version of SFMExample_SmartFactor that uses Preconditioned Conjugate Gradient
+ * @author  Frank Dellaert
+ */
+
+// For an explanation of these headers, see SFMExample_SmartFactor.cpp
+#include "SFMdata.h"
+#include <gtsam/slam/SmartProjectionPoseFactor.h>
+
+// These extra headers allow us a LM outer loop with PCG linear solver (inner loop)
+#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
+#include <gtsam/linear/Preconditioner.h>
+#include <gtsam/linear/PCGSolver.h>
+
+using namespace std;
+using namespace gtsam;
+
+// Make the typename short so it looks much cleaner
+typedef SmartProjectionPoseFactor<Cal3_S2> SmartFactor;
+
+/* ************************************************************************* */
+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
+
+  // Create the set of ground-truth landmarks and poses
+  vector<Point3> points = createPoints();
+  vector<Pose3> poses = createPoses();
+
+  // Create a factor graph
+  NonlinearFactorGraph graph;
+
+  // 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 the factor, and then insert measurements.
+    SmartFactor::shared_ptr smartfactor(new SmartFactor());
+
+    for (size_t i = 0; i < poses.size(); ++i) {
+
+      // generate the 2D measurement
+      SimpleCamera 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:
+      smartfactor->add(measurement, i, measurementNoise, K);
+    }
+
+    // insert the smart factor in the graph
+    graph.push_back(smartfactor);
+  }
+
+  // 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 poseNoise = noiseModel::Diagonal::Sigmas(
+      (Vector(6) << Vector3::Constant(0.3), Vector3::Constant(0.1)).finished());
+  graph.push_back(PriorFactor<Pose3>(0, poses[0], poseNoise));
+
+  // Fix the scale ambiguity by adding a prior
+  graph.push_back(PriorFactor<Pose3>(1, poses[1], poseNoise));
+
+  // Create the initial estimate to the solution
+  Values initialEstimate;
+  Pose3 delta(Rot3::rodriguez(-0.1, 0.2, 0.25), Point3(0.05, -0.10, 0.20));
+  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 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)
+  LevenbergMarquardtParams parameters;
+  parameters.linearSolverType = NonlinearOptimizerParams::Iterative;
+  parameters.absoluteErrorTol = 1e-10;
+  parameters.relativeErrorTol = 1e-10;
+  parameters.maxIterations = 500;
+  PCGSolverParameters::shared_ptr pcg =
+      boost::make_shared<PCGSolverParameters>();
+  pcg->preconditioner_ =
+      boost::make_shared<BlockJacobiPreconditionerParameters>();
+  // Following is crucial:
+  pcg->setEpsilon_abs(1e-10);
+  pcg->setEpsilon_rel(1e-10);
+  parameters.iterativeParams = pcg;
+
+  LevenbergMarquardtOptimizer optimizer(graph, initialEstimate, parameters);
+  Values result = optimizer.optimize();
+
+  // Display result as in SFMExample_SmartFactor.run
+  result.print("Final results:\n");
+  Values landmark_result;
+  for (size_t j = 0; j < points.size(); ++j) {
+    SmartFactor::shared_ptr smart = //
+        boost::dynamic_pointer_cast<SmartFactor>(graph[j]);
+    if (smart) {
+      boost::optional<Point3> point = smart->point(result);
+      if (point) // ignore if boost::optional return NULL
+        landmark_result.insert(j, *point);
+    }
+  }
+
+  landmark_result.print("Landmark results:\n");
+  cout << "final error: " << graph.error(result) << endl;
+  cout << "number of iterations: " << optimizer.iterations() << endl;
+
+  return 0;
+}
+/* ************************************************************************* */
+

gtsam.h

@@ -2310,23 +2310,23 @@ virtual class GeneralSFMFactor2 : gtsam::NoiseModelFactor {
 };
 
 #include <gtsam/slam/SmartProjectionPoseFactor.h>
-template<POSE, CALIBRATION>
+template<CALIBRATION>
 virtual class SmartProjectionPoseFactor : gtsam::NonlinearFactor {
 
   SmartProjectionPoseFactor(double rankTol, double linThreshold,
-      bool manageDegeneracy, bool enableEPI, const POSE& body_P_sensor);
+      bool manageDegeneracy, bool enableEPI, const gtsam::Pose3& body_P_sensor);
 
   SmartProjectionPoseFactor(double rankTol);
   SmartProjectionPoseFactor();
 
-  void add(const gtsam::Point2& measured_i, size_t poseKey_i, const gtsam::noiseModel::Base* noise_i,
+  void add(const gtsam::Point2& measured_i, size_t poseKey_i,
-      const CALIBRATION* K_i);
+      const gtsam::noiseModel::Base* noise_i, const CALIBRATION* K_i);
 
   // enabling serialization functionality
   //void serialize() const;
 };
 
-typedef gtsam::SmartProjectionPoseFactor<gtsam::Pose3, gtsam::Cal3_S2> SmartProjectionPose3Factor;
+typedef gtsam::SmartProjectionPoseFactor<gtsam::Cal3_S2> SmartProjectionPose3Factor;
 
 
 #include <gtsam/slam/StereoFactor.h>

gtsam/geometry/CameraSet.h

@@ -0,0 +1,123 @@
+/* ----------------------------------------------------------------------------
+
+ * 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   CameraSet.h
+ * @brief  Base class to create smart factors on poses or cameras
+ * @author Frank Dellaert
+ * @date   Feb 19, 2015
+ */
+
+#pragma once
+
+#include <gtsam/geometry/Point3.h>
+#include <gtsam/geometry/CalibratedCamera.h> // for Cheirality exception
+#include <gtsam/base/Testable.h>
+#include <vector>
+
+namespace gtsam {
+
+/**
+ * @brief A set of cameras, all with their own calibration
+ * Assumes that a camera is laid out as 6 Pose3 parameters then calibration
+ */
+template<class CAMERA>
+class CameraSet: public std::vector<CAMERA> {
+
+protected:
+
+  /**
+   * 2D measurement and noise model for each of the m views
+   * The order is kept the same as the keys that we use to create the factor.
+   */
+  typedef typename CAMERA::Measurement Z;
+
+  static const int ZDim = traits<Z>::dimension; ///< Measurement dimension
+  static const int Dim = traits<CAMERA>::dimension; ///< Camera dimension
+
+public:
+
+  /// Definitions for blocks of F
+  typedef Eigen::Matrix<double, ZDim, Dim> MatrixZD; // F
+  typedef std::pair<Key, MatrixZD> FBlock; // Fblocks
+
+  /**
+   * print
+   * @param s optional string naming the factor
+   * @param keyFormatter optional formatter useful for printing Symbols
+   */
+  void print(const std::string& s = "") const {
+    std::cout << s << "CameraSet, cameras = \n";
+    for (size_t k = 0; k < this->size(); ++k)
+      this->at(k).print();
+  }
+
+  /// equals
+  virtual bool equals(const CameraSet& p, double tol = 1e-9) const {
+    if (this->size() != p.size())
+      return false;
+    bool camerasAreEqual = true;
+    for (size_t i = 0; i < this->size(); i++) {
+      if (this->at(i).equals(p.at(i), tol) == false)
+        camerasAreEqual = false;
+      break;
+    }
+    return camerasAreEqual;
+  }
+
+  /**
+   * Project a point, with derivatives in this, point, and calibration
+   * throws CheiralityException
+   */
+  std::vector<Z> project(const Point3& point, boost::optional<Matrix&> F =
+      boost::none, boost::optional<Matrix&> E = boost::none,
+      boost::optional<Matrix&> H = boost::none) const {
+
+    size_t nrCameras = this->size();
+    if (F) F->resize(ZDim * nrCameras, 6);
+    if (E) E->resize(ZDim * nrCameras, 3);
+    if (H && Dim > 6) H->resize(ZDim * nrCameras, Dim - 6);
+    std::vector<Z> z(nrCameras);
+
+    for (size_t i = 0; i < nrCameras; i++) {
+      Eigen::Matrix<double, ZDim, 6> Fi;
+      Eigen::Matrix<double, ZDim, 3> Ei;
+      Eigen::Matrix<double, ZDim, Dim - 6> Hi;
+      z[i] = this->at(i).project(point, F ? &Fi : 0, E ? &Ei : 0, H ? &Hi : 0);
+      if (F) F->block<ZDim, 6>(ZDim * i, 0) = Fi;
+      if (E) E->block<ZDim, 3>(ZDim * i, 0) = Ei;
+      if (H) H->block<ZDim, Dim - 6>(ZDim * i, 0) = Hi;
+    }
+    return z;
+  }
+
+private:
+
+  /// Serialization function
+  friend class boost::serialization::access;
+  template<class ARCHIVE>
+  void serialize(ARCHIVE & ar, const unsigned int version) {
+    ar & (*this);
+  }
+};
+
+template<class CAMERA>
+const int CameraSet<CAMERA>::ZDim;
+
+template<class CAMERA>
+struct traits<CameraSet<CAMERA> > : public Testable<CameraSet<CAMERA> > {
+};
+
+template<class CAMERA>
+struct traits<const CameraSet<CAMERA> > : public Testable<CameraSet<CAMERA> > {
+};
+
+} // \ namespace gtsam

gtsam/geometry/PinholeCamera.h

@@ -31,6 +31,14 @@ namespace gtsam {
 template<typename Calibration>
 class GTSAM_EXPORT PinholeCamera: public PinholeBaseK<Calibration> {
 
+public:
+
+  /**
+   *  Some classes template on either PinholeCamera or StereoCamera,
+   *  and this typedef informs those classes what "project" returns.
+   */
+  typedef Point2 Measurement;
+
 private:
 
   typedef PinholeBaseK<Calibration> Base; ///< base class has 3D pose as private member

gtsam/geometry/StereoCamera.cpp

@@ -31,7 +31,7 @@ namespace gtsam {
   /* ************************************************************************* */
   StereoPoint2 StereoCamera::project(const Point3& point,
       OptionalJacobian<3,6> H1, OptionalJacobian<3,3> H2,
-      OptionalJacobian<3,6> H3) const {
+      OptionalJacobian<3,0> H3) const {
 
 #ifdef STEREOCAMERA_CHAIN_RULE
     const Point3 q = leftCamPose_.transform_to(point, H1, H2);
@@ -78,8 +78,7 @@ namespace gtsam {
       }
 #endif
       if (H3)
-        // TODO, this is set to zero, as Cal3_S2Stereo cannot be optimized yet
+        throw std::runtime_error("StereoCamera::project does not support third derivative yet");
-        H3->setZero();
     }
 
     // finally translate

gtsam/geometry/StereoCamera.h

@@ -28,32 +28,47 @@ namespace gtsam {
 
 class GTSAM_EXPORT StereoCheiralityException: public std::runtime_error {
 public:
-  StereoCheiralityException() : std::runtime_error("Stereo Cheirality Exception") {}
+  StereoCheiralityException() :
+      std::runtime_error("Stereo Cheirality Exception") {
+  }
 };
 
-
 /**
  * A stereo camera class, parameterize by left camera pose and stereo calibration
  * @addtogroup geometry
  */
 class GTSAM_EXPORT StereoCamera {
 
+public:
+
+  /**
+   *  Some classes template on either PinholeCamera or StereoCamera,
+   *  and this typedef informs those classes what "project" returns.
+   */
+  typedef StereoPoint2 Measurement;
+
 private:
   Pose3 leftCamPose_;
   Cal3_S2Stereo::shared_ptr K_;
 
 public:
 
-  enum { dimension = 6 };
+  enum {
+    dimension = 6
+  };
 
   /// @name Standard Constructors
   /// @{
 
-  StereoCamera() {
+  /// Default constructor allocates a calibration!
+  StereoCamera() :
+      K_(new Cal3_S2Stereo()) {
   }
 
+  /// Construct from pose and shared calibration
   StereoCamera(const Pose3& leftCamPose, const Cal3_S2Stereo::shared_ptr K);
 
+  /// Return shared pointer to calibration
   const Cal3_S2Stereo::shared_ptr calibration() const {
     return K_;
   }
@@ -62,26 +77,28 @@ public:
   /// @name Testable
   /// @{
 
+  /// print
   void print(const std::string& s = "") const {
     leftCamPose_.print(s + ".camera.");
     K_->print(s + ".calibration.");
   }
 
+  /// equals
   bool equals(const StereoCamera &camera, double tol = 1e-9) const {
-    return leftCamPose_.equals(camera.leftCamPose_, tol) && K_->equals(
+    return leftCamPose_.equals(camera.leftCamPose_, tol)
-        *camera.K_, tol);
+        && K_->equals(*camera.K_, tol);
   }
 
   /// @}
   /// @name Manifold
   /// @{
 
-  /** Dimensionality of the tangent space */
+  /// Dimensionality of the tangent space
   inline size_t dim() const {
     return 6;
   }
 
-  /** Dimensionality of the tangent space */
+  /// Dimensionality of the tangent space
   static inline size_t Dim() {
     return 6;
   }
@@ -100,10 +117,12 @@ public:
   /// @name Standard Interface
   /// @{
 
+  /// pose
   const Pose3& pose() const {
     return leftCamPose_;
   }
 
+  /// baseline
   double baseline() const {
     return K_->baseline();
   }
@@ -114,19 +133,16 @@ public:
    * @param H2 derivative with respect to point
    * @param H3 IGNORED (for calibration)
    */
-  StereoPoint2 project(const Point3& point,
+  StereoPoint2 project(const Point3& point, OptionalJacobian<3, 6> H1 =
-      OptionalJacobian<3, 6> H1 = boost::none,
+      boost::none, OptionalJacobian<3, 3> H2 = boost::none,
-      OptionalJacobian<3, 3> H2 = boost::none,
+      OptionalJacobian<3, 0> H3 = boost::none) const;
-      OptionalJacobian<3, 6> H3 = boost::none) const;
 
-  /**
+  /// back-project a measurement
-   *
-   */
   Point3 backproject(const StereoPoint2& z) const {
     Vector measured = z.vector();
-    double Z = K_->baseline()*K_->fx()/(measured[0]-measured[1]);
+    double Z = K_->baseline() * K_->fx() / (measured[0] - measured[1]);
-    double X = Z *(measured[0]- K_->px()) / K_->fx();
+    double X = Z * (measured[0] - K_->px()) / K_->fx();
-    double Y = Z *(measured[2]- K_->py()) / K_->fy();
+    double Y = Z * (measured[2] - K_->py()) / K_->fy();
     Point3 world_point = leftCamPose_.transform_from(Point3(X, Y, Z));
     return world_point;
   }
@@ -134,7 +150,8 @@ public:
   /// @}
 
 private:
-  /** utility functions */
+
+  /// utility function
   Matrix3 Dproject_to_stereo_camera1(const Point3& P) const;
 
   friend class boost::serialization::access;
@@ -147,8 +164,10 @@ private:
 };
 
 template<>
-struct traits<StereoCamera> : public internal::Manifold<StereoCamera> {};
+struct traits<StereoCamera> : public internal::Manifold<StereoCamera> {
+};
 
 template<>
-struct traits<const StereoCamera> : public internal::Manifold<StereoCamera> {};
+struct traits<const StereoCamera> : public internal::Manifold<StereoCamera> {
+};
 }

gtsam/geometry/tests/testCameraSet.cpp

@@ -0,0 +1,114 @@
+/* ----------------------------------------------------------------------------
+
+ * 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   testCameraSet.cpp
+ *  @brief  Unit tests for testCameraSet Class
+ *  @author Frank Dellaert
+ *  @date   Feb 19, 2015
+ */
+
+#include <gtsam/geometry/CameraSet.h>
+#include <gtsam/geometry/Pose3.h>
+#include <gtsam/base/numericalDerivative.h>
+#include <CppUnitLite/TestHarness.h>
+#include <boost/bind.hpp>
+
+using namespace std;
+using namespace gtsam;
+
+/* ************************************************************************* */
+// Cal3Bundler test
+#include <gtsam/geometry/PinholeCamera.h>
+#include <gtsam/geometry/Cal3Bundler.h>
+TEST(CameraSet, Pinhole) {
+  typedef PinholeCamera<Cal3Bundler> Camera;
+  typedef vector<Point2> ZZ;
+  CameraSet<Camera> set;
+  Camera camera;
+  set.push_back(camera);
+  set.push_back(camera);
+  Point3 p(0, 0, 1);
+  CHECK(assert_equal(set, set));
+  CameraSet<Camera> set2 = set;
+  set2.push_back(camera);
+  CHECK(!set.equals(set2));
+
+  // Check measurements
+  Point2 expected;
+  ZZ z = set.project(p);
+  CHECK(assert_equal(expected, z[0]));
+  CHECK(assert_equal(expected, z[1]));
+
+  // Calculate expected derivatives using Pinhole
+  Matrix46 actualF;
+  Matrix43 actualE;
+  Matrix43 actualH;
+  {
+    Matrix26 F1;
+    Matrix23 E1;
+    Matrix23 H1;
+    camera.project(p, F1, E1, H1);
+    actualE << E1, E1;
+    actualF << F1, F1;
+    actualH << H1, H1;
+  }
+
+  // Check computed derivatives
+  Matrix F, E, H;
+  set.project(p, F, E, H);
+  CHECK(assert_equal(actualF, F));
+  CHECK(assert_equal(actualE, E));
+  CHECK(assert_equal(actualH, H));
+}
+
+/* ************************************************************************* */
+#include <gtsam/geometry/StereoCamera.h>
+TEST(CameraSet, Stereo) {
+  typedef vector<StereoPoint2> ZZ;
+  CameraSet<StereoCamera> set;
+  StereoCamera camera;
+  set.push_back(camera);
+  set.push_back(camera);
+  Point3 p(0, 0, 1);
+  EXPECT_LONGS_EQUAL(6, traits<StereoCamera>::dimension);
+
+  // Check measurements
+  StereoPoint2 expected(0, -1, 0);
+  ZZ z = set.project(p);
+  CHECK(assert_equal(expected, z[0]));
+  CHECK(assert_equal(expected, z[1]));
+
+  // Calculate expected derivatives using Pinhole
+  Matrix66 actualF;
+  Matrix63 actualE;
+  {
+    Matrix36 F1;
+    Matrix33 E1;
+    camera.project(p, F1, E1);
+    actualE << E1, E1;
+    actualF << F1, F1;
+  }
+
+  // Check computed derivatives
+  Matrix F, E;
+  set.project(p, F, E);
+  CHECK(assert_equal(actualF, F));
+  CHECK(assert_equal(actualE, E));
+}
+
+/* ************************************************************************* */
+int main() {
+  TestResult tr;
+  return TestRegistry::runAllTests(tr);
+}
+/* ************************************************************************* */
+

gtsam/linear/ConjugateGradientSolver.cpp

@@ -1,8 +1,20 @@
+/* ----------------------------------------------------------------------------
+
+ * 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
+
+ * -------------------------------------------------------------------------- */
+
 /*
- * ConjugateGradientSolver.cpp
+ *  @file   ConjugateGradientSolver.cpp
- *
+ *  @brief  Implementation of Conjugate Gradient solver for a linear system
- *  Created on: Jun 4, 2014
+ *  @author Yong-Dian Jian
- *      Author: Yong-Dian Jian
+ *  @author Sungtae An
+ *  @date   Nov 6, 2014
  */
 
 #include <gtsam/linear/ConjugateGradientSolver.h>
@@ -35,7 +47,8 @@ std::string ConjugateGradientParameters::blasTranslator(const BLASKernel value)
 }
 
 /*****************************************************************************/
-ConjugateGradientParameters::BLASKernel ConjugateGradientParameters::blasTranslator(const std::string &src) {
+ConjugateGradientParameters::BLASKernel ConjugateGradientParameters::blasTranslator(
+    const std::string &src) {
   std::string s = src;  boost::algorithm::to_upper(s);
   if (s == "GTSAM")  return ConjugateGradientParameters::GTSAM;
 
@@ -43,6 +56,7 @@ ConjugateGradientParameters::BLASKernel ConjugateGradientParameters::blasTransla
   return ConjugateGradientParameters::GTSAM;
 }
 
+/*****************************************************************************/
 
 }
 

gtsam/linear/ConjugateGradientSolver.h

@@ -20,7 +20,6 @@
 #pragma once
 
 #include <gtsam/linear/IterativeSolver.h>
-#include <iosfwd>
 
 namespace gtsam {
 
@@ -87,9 +86,8 @@ public:
   static BLASKernel blasTranslator(const std::string &s) ;
 };
 
-/*********************************************************************************************/
 /*
- * A template of linear preconditioned conjugate gradient method.
+ * A template for the linear preconditioned conjugate gradient method.
  * System class should support residual(v, g), multiply(v,Av), scal(alpha,v), dot(v,v), axpy(alpha,x,y)
  * leftPrecondition(v, L^{-1}v, rightPrecondition(v, L^{-T}v) where preconditioner M = L*L^T
  * Note that the residual is in the preconditioned domain. Refer to Section 9.2 of Saad's book.
@@ -98,8 +96,9 @@ public:
  * [1] Y. Saad, "Preconditioned Iterations," in Iterative Methods for Sparse Linear Systems,
  * 2nd ed. SIAM, 2003, ch. 9, sec. 2, pp.276-281.
  */
-template <class S, class V>
+template<class S, class V>
-V preconditionedConjugateGradient(const S &system, const V &initial, const ConjugateGradientParameters &parameters) {
+V preconditionedConjugateGradient(const S &system, const V &initial,
+    const ConjugateGradientParameters &parameters) {
 
   V estimate, residual, direction, q1, q2;
   estimate = residual = direction = q1 = q2 = initial;

gtsam/linear/IterativeSolver.cpp

@@ -1,6 +1,17 @@
+/* ----------------------------------------------------------------------------
+
+ * 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   IterativeSolver.cpp
- * @brief  
+ * @brief  Some support classes for iterative solvers
  * @date   Sep 3, 2012
  * @author Yong-Dian Jian
  */
@@ -9,18 +20,22 @@
 #include <gtsam/linear/GaussianFactorGraph.h>
 #include <gtsam/linear/VectorValues.h>
 #include <boost/algorithm/string.hpp>
+#include <boost/foreach.hpp>
 #include <iostream>
-#include <string>
 
 using namespace std;
 
 namespace gtsam {
 
 /*****************************************************************************/
-string IterativeOptimizationParameters::getVerbosity() const { return verbosityTranslator(verbosity_); }
+string IterativeOptimizationParameters::getVerbosity() const {
+  return verbosityTranslator(verbosity_);
+}
 
 /*****************************************************************************/
-void IterativeOptimizationParameters::setVerbosity(const string &src) { verbosity_ = verbosityTranslator(src); }
+void IterativeOptimizationParameters::setVerbosity(const string &src) {
+  verbosity_ = verbosityTranslator(src);
+}
 
 /*****************************************************************************/
 void IterativeOptimizationParameters::print() const {
@@ -29,78 +44,82 @@ void IterativeOptimizationParameters::print() const {
 
 /*****************************************************************************/
 void IterativeOptimizationParameters::print(ostream &os) const {
-  os << "IterativeOptimizationParameters:" <<  endl
+  os << "IterativeOptimizationParameters:" << endl << "verbosity:     "
-     << "verbosity:     " << verbosityTranslator(verbosity_) <<  endl;
+      << verbosityTranslator(verbosity_) << endl;
 }
 
 /*****************************************************************************/
- ostream& operator<<(ostream &os, const IterativeOptimizationParameters &p) {
+ostream& operator<<(ostream &os, const IterativeOptimizationParameters &p) {
   p.print(os);
   return os;
 }
 
 /*****************************************************************************/
-IterativeOptimizationParameters::Verbosity IterativeOptimizationParameters::verbosityTranslator(const  string &src)  {
+IterativeOptimizationParameters::Verbosity IterativeOptimizationParameters::verbosityTranslator(
-   string s = src;  boost::algorithm::to_upper(s);
+    const string &src) {
-  if (s == "SILENT") return IterativeOptimizationParameters::SILENT;
+  string s = src;
-  else if (s == "COMPLEXITY") return IterativeOptimizationParameters::COMPLEXITY;
+  boost::algorithm::to_upper(s);
-  else if (s == "ERROR") return IterativeOptimizationParameters::ERROR;
+  if (s == "SILENT")
+    return IterativeOptimizationParameters::SILENT;
+  else if (s == "COMPLEXITY")
+    return IterativeOptimizationParameters::COMPLEXITY;
+  else if (s == "ERROR")
+    return IterativeOptimizationParameters::ERROR;
   /* default is default */
-  else return IterativeOptimizationParameters::SILENT;
+  else
+    return IterativeOptimizationParameters::SILENT;
 }
 
 /*****************************************************************************/
- string IterativeOptimizationParameters::verbosityTranslator(IterativeOptimizationParameters::Verbosity verbosity)  {
+string IterativeOptimizationParameters::verbosityTranslator(
-  if (verbosity == SILENT) return "SILENT";
+    IterativeOptimizationParameters::Verbosity verbosity) {
-  else if (verbosity == COMPLEXITY) return "COMPLEXITY";
+  if (verbosity == SILENT)
-  else if (verbosity == ERROR) return "ERROR";
+    return "SILENT";
-  else return "UNKNOWN";
+  else if (verbosity == COMPLEXITY)
+    return "COMPLEXITY";
+  else if (verbosity == ERROR)
+    return "ERROR";
+  else
+    return "UNKNOWN";
 }
 
 /*****************************************************************************/
-VectorValues IterativeSolver::optimize(
+VectorValues IterativeSolver::optimize(const GaussianFactorGraph &gfg,
-    const GaussianFactorGraph &gfg,
     boost::optional<const KeyInfo&> keyInfo,
-    boost::optional<const std::map<Key, Vector>&> lambda)
+    boost::optional<const std::map<Key, Vector>&> lambda) {
-{
+  return optimize(gfg, keyInfo ? *keyInfo : KeyInfo(gfg),
-  return optimize(
+      lambda ? *lambda : std::map<Key, Vector>());
-           gfg,
-           keyInfo ? *keyInfo : KeyInfo(gfg),
-           lambda ? *lambda : std::map<Key,Vector>());
 }
 
 /*****************************************************************************/
-VectorValues IterativeSolver::optimize (
+VectorValues IterativeSolver::optimize(const GaussianFactorGraph &gfg,
-  const GaussianFactorGraph &gfg,
+    const KeyInfo &keyInfo, const std::map<Key, Vector> &lambda) {
-  const KeyInfo &keyInfo,
-  const std::map<Key, Vector> &lambda)
-{
   return optimize(gfg, keyInfo, lambda, keyInfo.x0());
 }
 
 /****************************************************************************/
-KeyInfo::KeyInfo(const GaussianFactorGraph &fg, const Ordering &ordering)
+KeyInfo::KeyInfo(const GaussianFactorGraph &fg, const Ordering &ordering) :
-  : ordering_(ordering) {
+    ordering_(ordering) {
   initialize(fg);
 }
 
 /****************************************************************************/
-KeyInfo::KeyInfo(const GaussianFactorGraph &fg)
+KeyInfo::KeyInfo(const GaussianFactorGraph &fg) :
-  : ordering_(Ordering::Natural(fg)) {
+    ordering_(Ordering::Natural(fg)) {
   initialize(fg);
 }
 
 /****************************************************************************/
-void KeyInfo::initialize(const GaussianFactorGraph &fg){
+void KeyInfo::initialize(const GaussianFactorGraph &fg) {
   const map<Key, size_t> colspec = fg.getKeyDimMap();
   const size_t n = ordering_.size();
   size_t start = 0;
 
-  for ( size_t i = 0 ; i < n ; ++i ) {
+  for (size_t i = 0; i < n; ++i) {
     const size_t key = ordering_[i];
     const size_t dim = colspec.find(key)->second;
     insert(make_pair(key, KeyInfoEntry(i, dim, start)));
-    start += dim ;
+    start += dim;
   }
   numCols_ = start;
 }
@@ -108,7 +127,7 @@ void KeyInfo::initialize(const GaussianFactorGraph &fg){
 /****************************************************************************/
 vector<size_t> KeyInfo::colSpec() const {
   std::vector<size_t> result(size(), 0);
-  BOOST_FOREACH ( const gtsam::KeyInfo::value_type &item, *this ) {
+  BOOST_FOREACH ( const KeyInfo::value_type &item, *this ) {
     result[item.second.index()] = item.second.dim();
   }
   return result;
@@ -117,7 +136,7 @@ vector<size_t> KeyInfo::colSpec() const {
 /****************************************************************************/
 VectorValues KeyInfo::x0() const {
   VectorValues result;
-  BOOST_FOREACH ( const gtsam::KeyInfo::value_type &item, *this ) {
+  BOOST_FOREACH ( const KeyInfo::value_type &item, *this ) {
     result.insert(item.first, Vector::Zero(item.second.dim()));
   }
   return result;
@@ -128,7 +147,5 @@ Vector KeyInfo::x0vector() const {
   return Vector::Zero(numCols_);
 }
 
-
 }
 
-

gtsam/linear/IterativeSolver.h

@@ -9,131 +9,178 @@
 
  * -------------------------------------------------------------------------- */
 
+/**
+ * @file   IterativeSolver.h
+ * @brief  Some support classes for iterative solvers
+ * @date   2010
+ * @author Yong-Dian Jian
+ */
+
 #pragma once
 
-#include <gtsam/base/Vector.h>
-#include <gtsam/global_includes.h>
 #include <gtsam/inference/Ordering.h>
+#include <gtsam/base/Vector.h>
+
 #include <boost/tuple/tuple.hpp>
-#include <boost/optional/optional.hpp>
+#include <boost/shared_ptr.hpp>
-#include <boost/none.hpp>
+#include <boost/optional.hpp>
+
 #include <iosfwd>
-#include <map>
 #include <string>
-#include <vector>
+#include <map>
 
 namespace gtsam {
 
-  // Forward declarations
+// Forward declarations
-  class KeyInfo;
+class KeyInfo;
-  class KeyInfoEntry;
+class KeyInfoEntry;
-  class GaussianFactorGraph;
+class GaussianFactorGraph;
-  class Values;
+class Values;
-  class VectorValues;
+class VectorValues;
 
-  /************************************************************************************/
+/**
-  /**
  * parameters for iterative linear solvers
  */
-  class GTSAM_EXPORT IterativeOptimizationParameters {
+class GTSAM_EXPORT IterativeOptimizationParameters {
 
-  public:
+public:
 
   typedef boost::shared_ptr<IterativeOptimizationParameters> shared_ptr;
-    enum Verbosity { SILENT = 0, COMPLEXITY, ERROR } verbosity_;
+  enum Verbosity {
+    SILENT = 0, COMPLEXITY, ERROR
+  } verbosity_;
 
-  public:
+public:
 
-    IterativeOptimizationParameters(Verbosity v = SILENT)
+  IterativeOptimizationParameters(Verbosity v = SILENT) :
-      : verbosity_(v) {}
+      verbosity_(v) {
+  }
 
-    virtual ~IterativeOptimizationParameters() {}
+  virtual ~IterativeOptimizationParameters() {
+  }
 
   /* utility */
-    inline Verbosity verbosity() const { return verbosity_; }
+  inline Verbosity verbosity() const {
+    return verbosity_;
+  }
   std::string getVerbosity() const;
-    void setVerbosity(const std::string &s) ;
+  void setVerbosity(const std::string &s);
 
   /* matlab interface */
-    void print() const ;
+  void print() const;
 
   /* virtual print function */
-    virtual void print(std::ostream &os) const ;
+  virtual void print(std::ostream &os) const;
 
   /* for serialization */
-    friend std::ostream& operator<<(std::ostream &os, const IterativeOptimizationParameters &p);
+  friend std::ostream& operator<<(std::ostream &os,
+      const IterativeOptimizationParameters &p);
 
   static Verbosity verbosityTranslator(const std::string &s);
   static std::string verbosityTranslator(Verbosity v);
-  };
+};
 
-  /************************************************************************************/
+/**
-  class GTSAM_EXPORT IterativeSolver {
+ * Base class for Iterative Solvers like SubgraphSolver
-  public:
+ */
+class GTSAM_EXPORT IterativeSolver {
+public:
   typedef boost::shared_ptr<IterativeSolver> shared_ptr;
-    IterativeSolver() {}
+  IterativeSolver() {
-    virtual ~IterativeSolver() {}
+  }
+  virtual ~IterativeSolver() {
+  }
 
   /* interface to the nonlinear optimizer, without metadata, damping and initial estimate */
-    VectorValues optimize (
+  VectorValues optimize(const GaussianFactorGraph &gfg,
-      const GaussianFactorGraph &gfg,
       boost::optional<const KeyInfo&> = boost::none,
-      boost::optional<const std::map<Key, Vector>&> lambda = boost::none
+      boost::optional<const std::map<Key, Vector>&> lambda = boost::none);
-    );
 
   /* interface to the nonlinear optimizer, without initial estimate */
-    VectorValues optimize (
+  VectorValues optimize(const GaussianFactorGraph &gfg, const KeyInfo &keyInfo,
-      const GaussianFactorGraph &gfg,
+      const std::map<Key, Vector> &lambda);
-      const KeyInfo &keyInfo,
-      const std::map<Key, Vector> &lambda
-    );
 
   /* interface to the nonlinear optimizer that the subclasses have to implement */
-    virtual VectorValues optimize (
+  virtual VectorValues optimize(const GaussianFactorGraph &gfg,
-      const GaussianFactorGraph &gfg,
+      const KeyInfo &keyInfo, const std::map<Key, Vector> &lambda,
-      const KeyInfo &keyInfo,
+      const VectorValues &initial) = 0;
-      const std::map<Key, Vector> &lambda,
-      const VectorValues &initial
-    ) = 0;
 
-  };
+};
 
-  /************************************************************************************/
+/**
-  /* Handy data structure for iterative solvers
+ * Handy data structure for iterative solvers
-   * key to (index, dimension, colstart) */
+ * key to (index, dimension, colstart)
-  class GTSAM_EXPORT KeyInfoEntry : public boost::tuple<size_t, size_t, size_t> {
+ */
-  public:
+class GTSAM_EXPORT KeyInfoEntry: public boost::tuple<size_t, size_t, size_t> {
-    typedef boost::tuple<Key,size_t,Key> Base;
+
-    KeyInfoEntry(){}
+public:
-    KeyInfoEntry(size_t idx, size_t d, Key start) : Base(idx, d, start) {}
+
-    size_t index() const { return this->get<0>(); }
+  typedef boost::tuple<Key, size_t, Key> Base;
-    size_t dim() const { return this->get<1>(); }
+
-    size_t colstart() const { return this->get<2>(); }
+  KeyInfoEntry() {
-  };
+  }
+  KeyInfoEntry(size_t idx, size_t d, Key start) :
+      Base(idx, d, start) {
+  }
+  size_t index() const {
+    return this->get<0>();
+  }
+  size_t dim() const {
+    return this->get<1>();
+  }
+  size_t colstart() const {
+    return this->get<2>();
+  }
+};
+
+/**
+ * Handy data structure for iterative solvers
+ */
+class GTSAM_EXPORT KeyInfo: public std::map<Key, KeyInfoEntry> {
+
+public:
 
-  /************************************************************************************/
-  class GTSAM_EXPORT KeyInfo : public std::map<Key, KeyInfoEntry> {
-  public:
   typedef std::map<Key, KeyInfoEntry> Base;
-    KeyInfo() : numCols_(0) {}
-    KeyInfo(const GaussianFactorGraph &fg);
-    KeyInfo(const GaussianFactorGraph &fg, const Ordering &ordering);
 
-    std::vector<size_t> colSpec() const ;
+protected:
-    VectorValues x0() const;
-    Vector x0vector() const;
-
-    inline size_t numCols() const { return numCols_; }
-    inline const Ordering & ordering() const { return ordering_; }
-
-  protected:
-
-    void initialize(const GaussianFactorGraph &fg);
 
   Ordering ordering_;
   size_t numCols_;
 
-  };
+  void initialize(const GaussianFactorGraph &fg);
 
+public:
 
-}
+  /// Default Constructor
+  KeyInfo() :
+      numCols_(0) {
+  }
+
+  /// Construct from Gaussian factor graph, use "Natural" ordering
+  KeyInfo(const GaussianFactorGraph &fg);
+
+  /// Construct from Gaussian factor graph and a given ordering
+  KeyInfo(const GaussianFactorGraph &fg, const Ordering &ordering);
+
+  /// Return the total number of columns (scalar variables = sum of dimensions)
+  inline size_t numCols() const {
+    return numCols_;
+  }
+
+  /// Return the ordering
+  inline const Ordering & ordering() const {
+    return ordering_;
+  }
+
+  /// Return a vector of dimensions ordered by ordering()
+  std::vector<size_t> colSpec() const;
+
+  /// Return VectorValues with zeros, of correct dimension
+  VectorValues x0() const;
+
+  /// Return zero Vector of correct dimension
+  Vector x0vector() const;
+
+};
+
+} // \ namespace gtsam

gtsam/linear/PCGSolver.cpp

@@ -1,16 +1,31 @@
+/* ----------------------------------------------------------------------------
+
+ * 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
+
+ * -------------------------------------------------------------------------- */
+
 /*
- * PCGSolver.cpp
+ * @file PCGSolver.cpp
- *
+ * @brief Preconditioned Conjugate Gradient Solver for linear systems
- *  Created on: Feb 14, 2012
+ * @date Feb 14, 2012
- *      Author: Yong-Dian Jian
+ * @author Yong-Dian Jian
- *      Author: Sungtae An
+ * @author Sungtae An
  */
 
-#include <gtsam/linear/GaussianFactorGraph.h>
 #include <gtsam/linear/PCGSolver.h>
+#include <gtsam/linear/GaussianFactorGraph.h>
 #include <gtsam/linear/Preconditioner.h>
+#include <gtsam/linear/VectorValues.h>
 
+#include <boost/foreach.hpp>
 #include <boost/algorithm/string.hpp>
+
+#include <algorithm>
 #include <iostream>
 #include <stdexcept>
 
@@ -32,30 +47,27 @@ PCGSolver::PCGSolver(const PCGSolverParameters &p) {
 }
 
 /*****************************************************************************/
-VectorValues PCGSolver::optimize (
+VectorValues PCGSolver::optimize(const GaussianFactorGraph &gfg,
-  const GaussianFactorGraph &gfg,
+    const KeyInfo &keyInfo, const std::map<Key, Vector> &lambda,
-  const KeyInfo &keyInfo,
+    const VectorValues &initial) {
-  const std::map<Key, Vector> &lambda,
-  const VectorValues &initial)
-{
   /* build preconditioner */
   preconditioner_->build(gfg, keyInfo, lambda);
 
   /* apply pcg */
-  const Vector sol = preconditionedConjugateGradient<GaussianFactorGraphSystem, Vector>(
+  GaussianFactorGraphSystem system(gfg, *preconditioner_, keyInfo, lambda);
-        GaussianFactorGraphSystem(gfg, *preconditioner_, keyInfo, lambda),
+  Vector x0 = initial.vector(keyInfo.ordering());
-        initial.vector(keyInfo.ordering()), parameters_);
+  const Vector sol = preconditionedConjugateGradient(system, x0, parameters_);
 
   return buildVectorValues(sol, keyInfo);
 }
 
 /*****************************************************************************/
 GaussianFactorGraphSystem::GaussianFactorGraphSystem(
-    const GaussianFactorGraph &gfg,
+    const GaussianFactorGraph &gfg, const Preconditioner &preconditioner,
-    const Preconditioner &preconditioner,
+    const KeyInfo &keyInfo, const std::map<Key, Vector> &lambda) :
-    const KeyInfo &keyInfo,
+    gfg_(gfg), preconditioner_(preconditioner), keyInfo_(keyInfo), lambda_(
-    const std::map<Key, Vector> &lambda)
+        lambda) {
-  : gfg_(gfg), preconditioner_(preconditioner), keyInfo_(keyInfo), lambda_(lambda) {}
+}
 
 /*****************************************************************************/
 void GaussianFactorGraphSystem::residual(const Vector &x, Vector &r) const {
@@ -67,7 +79,7 @@ void GaussianFactorGraphSystem::residual(const Vector &x, Vector &r) const {
   /* substract A*x */
   Vector Ax = Vector::Zero(r.rows(), 1);
   multiply(x, Ax);
-  r -= Ax ;
+  r -= Ax;
 }
 
 /*****************************************************************************/
@@ -75,7 +87,7 @@ void GaussianFactorGraphSystem::multiply(const Vector &x, Vector& AtAx) const {
   /* implement A^T*(A*x), assume x and AtAx are pre-allocated */
 
   // Build a VectorValues for Vector x
-  VectorValues vvX = buildVectorValues(x,keyInfo_);
+  VectorValues vvX = buildVectorValues(x, keyInfo_);
 
   // VectorValues form of A'Ax for multiplyHessianAdd
   VectorValues vvAtAx;
@@ -99,31 +111,33 @@ void GaussianFactorGraphSystem::getb(Vector &b) const {
 }
 
 /**********************************************************************************/
-void GaussianFactorGraphSystem::leftPrecondition(const Vector &x, Vector &y) const {
+void GaussianFactorGraphSystem::leftPrecondition(const Vector &x,
+    Vector &y) const {
   // For a preconditioner M = L*L^T
   // Calculate y = L^{-1} x
   preconditioner_.solve(x, y);
 }
 
 /**********************************************************************************/
-void GaussianFactorGraphSystem::rightPrecondition(const Vector &x, Vector &y) const {
+void GaussianFactorGraphSystem::rightPrecondition(const Vector &x,
+    Vector &y) const {
   // For a preconditioner M = L*L^T
   // Calculate y = L^{-T} x
   preconditioner_.transposeSolve(x, y);
 }
 
 /**********************************************************************************/
-VectorValues buildVectorValues(const Vector &v,
+VectorValues buildVectorValues(const Vector &v, const Ordering &ordering,
-                               const Ordering &ordering,
     const map<Key, size_t> & dimensions) {
   VectorValues result;
 
   DenseIndex offset = 0;
-  for ( size_t i = 0 ; i < ordering.size() ; ++i ) {
+  for (size_t i = 0; i < ordering.size(); ++i) {
     const Key &key = ordering[i];
     map<Key, size_t>::const_iterator it = dimensions.find(key);
-    if ( it == dimensions.end() ) {
+    if (it == dimensions.end()) {
-      throw invalid_argument("buildVectorValues: inconsistent ordering and dimensions");
+      throw invalid_argument(
+          "buildVectorValues: inconsistent ordering and dimensions");
     }
     const size_t dim = it->second;
     result.insert(key, v.segment(offset, dim));
@@ -137,7 +151,8 @@ VectorValues buildVectorValues(const Vector &v,
 VectorValues buildVectorValues(const Vector &v, const KeyInfo &keyInfo) {
   VectorValues result;
   BOOST_FOREACH ( const KeyInfo::value_type &item, keyInfo ) {
-    result.insert(item.first, v.segment(item.second.colstart(), item.second.dim()));
+    result.insert(item.first,
+        v.segment(item.second.colstart(), item.second.dim()));
   }
   return result;
 }

gtsam/linear/PCGSolver.h

@@ -1,20 +1,25 @@
+/* ----------------------------------------------------------------------------
+
+ * 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
+
+ * -------------------------------------------------------------------------- */
+
 /*
- * PCGSolver.h
+ * @file PCGSolver.h
- *
+ * @brief Preconditioned Conjugate Gradient Solver for linear systems
- *  Created on: Jan 31, 2012
+ * @date Jan 31, 2012
- *  Author: Yong-Dian Jian
+ * @author Yong-Dian Jian
+ * @author Sungtae An
  */
 
 #pragma once
 
-#include <gtsam/linear/IterativeSolver.h>
 #include <gtsam/linear/ConjugateGradientSolver.h>
-#include <gtsam/linear/VectorValues.h>
-#include <boost/shared_ptr.hpp>
-
-#include <algorithm>
-#include <iosfwd>
-#include <map>
 #include <string>
 
 namespace gtsam {
@@ -22,15 +27,19 @@ namespace gtsam {
 class GaussianFactorGraph;
 class KeyInfo;
 class Preconditioner;
+class VectorValues;
 struct PreconditionerParameters;
 
-/*****************************************************************************/
+/**
+ * Parameters for PCG
+ */
 struct GTSAM_EXPORT PCGSolverParameters: public ConjugateGradientParameters {
 public:
   typedef ConjugateGradientParameters Base;
   typedef boost::shared_ptr<PCGSolverParameters> shared_ptr;
 
-  PCGSolverParameters() {}
+  PCGSolverParameters() {
+  }
 
   virtual void print(std::ostream &os) const;
 
@@ -42,8 +51,9 @@ public:
   boost::shared_ptr<PreconditionerParameters> preconditioner_;
 };
 
-/*****************************************************************************/
+/**
-/* A virtual base class for the preconditioned conjugate gradient solver */
+ * A virtual base class for the preconditioned conjugate gradient solver
+ */
 class GTSAM_EXPORT PCGSolver: public IterativeSolver {
 public:
   typedef IterativeSolver Base;
@@ -57,7 +67,8 @@ protected:
 public:
   /* Interface to initialize a solver without a problem */
   PCGSolver(const PCGSolverParameters &p);
-  virtual ~PCGSolver() {}
+  virtual ~PCGSolver() {
+  }
 
   using IterativeSolver::optimize;
 
@@ -67,7 +78,9 @@ public:
 
 };
 
-/*****************************************************************************/
+/**
+ * System class needed for calling preconditionedConjugateGradient
+ */
 class GTSAM_EXPORT GaussianFactorGraphSystem {
 public:
 
@@ -97,13 +110,17 @@ public:
   void getb(Vector &b) const;
 };
 
-/* utility functions */
+/// @name utility functions
-/**********************************************************************************/
+/// @{
+
+/// Create VectorValues from a Vector
 VectorValues buildVectorValues(const Vector &v, const Ordering &ordering,
     const std::map<Key, size_t> & dimensions);
 
-/**********************************************************************************/
+/// Create VectorValues from a Vector and a KeyInfo class
 VectorValues buildVectorValues(const Vector &v, const KeyInfo &keyInfo);
 
+/// @}
+
 }
 

gtsam/linear/SubgraphSolver.cpp

@@ -9,73 +9,81 @@
 
  * -------------------------------------------------------------------------- */
 
-#include <gtsam/linear/Errors.h>
+/**
-#include <gtsam/linear/GaussianConditional.h>
+ * @file   SubgraphSolver.cpp
+ * @brief  Subgraph Solver from IROS 2010
+ * @date   2010
+ * @author Frank Dellaert
+ * @author Yong Dian Jian
+ */
+
+#include <gtsam/linear/SubgraphSolver.h>
 #include <gtsam/linear/GaussianBayesNet.h>
 #include <gtsam/linear/iterative-inl.h>
 #include <gtsam/linear/GaussianFactorGraph.h>
 #include <gtsam/linear/SubgraphPreconditioner.h>
-#include <gtsam/linear/SubgraphSolver.h>
-#include <gtsam/linear/VectorValues.h>
-#include <gtsam/inference/graph-inl.h>
 #include <gtsam/base/DSFVector.h>
-#include <boost/foreach.hpp>
-#include <boost/shared_ptr.hpp>
-#include <list>
 
 using namespace std;
 
 namespace gtsam {
 
 /**************************************************************************************************/
-SubgraphSolver::SubgraphSolver(const GaussianFactorGraph &gfg, const Parameters &parameters, const Ordering& ordering)
+SubgraphSolver::SubgraphSolver(const GaussianFactorGraph &gfg,
-  : parameters_(parameters), ordering_(ordering)
+    const Parameters &parameters, const Ordering& ordering) :
-{
+    parameters_(parameters), ordering_(ordering) {
   initialize(gfg);
 }
 
 /**************************************************************************************************/
-SubgraphSolver::SubgraphSolver(const GaussianFactorGraph::shared_ptr &jfg, const Parameters &parameters, const Ordering& ordering)
+SubgraphSolver::SubgraphSolver(const GaussianFactorGraph::shared_ptr &jfg,
-  : parameters_(parameters), ordering_(ordering)
+    const Parameters &parameters, const Ordering& ordering) :
-{
+    parameters_(parameters), ordering_(ordering) {
   initialize(*jfg);
 }
 
 /**************************************************************************************************/
-SubgraphSolver::SubgraphSolver(const GaussianFactorGraph &Ab1, const GaussianFactorGraph &Ab2, const Parameters &parameters, const Ordering& ordering)
+SubgraphSolver::SubgraphSolver(const GaussianFactorGraph &Ab1,
-  : parameters_(parameters), ordering_(ordering) {
+    const GaussianFactorGraph &Ab2, const Parameters &parameters,
+    const Ordering& ordering) :
+    parameters_(parameters), ordering_(ordering) {
 
-  GaussianBayesNet::shared_ptr Rc1 = Ab1.eliminateSequential(ordering_, EliminateQR);
+  GaussianBayesNet::shared_ptr Rc1 = Ab1.eliminateSequential(ordering_,
+      EliminateQR);
   initialize(Rc1, boost::make_shared<GaussianFactorGraph>(Ab2));
 }
 
 /**************************************************************************************************/
 SubgraphSolver::SubgraphSolver(const GaussianFactorGraph::shared_ptr &Ab1,
-    const GaussianFactorGraph::shared_ptr &Ab2, const Parameters &parameters, const Ordering& ordering)
+    const GaussianFactorGraph::shared_ptr &Ab2, const Parameters &parameters,
-  : parameters_(parameters), ordering_(ordering) {
+    const Ordering& ordering) :
+    parameters_(parameters), ordering_(ordering) {
 
-  GaussianBayesNet::shared_ptr Rc1 = Ab1->eliminateSequential(ordering_, EliminateQR);
+  GaussianBayesNet::shared_ptr Rc1 = Ab1->eliminateSequential(ordering_,
+      EliminateQR);
   initialize(Rc1, Ab2);
 }
 
 /**************************************************************************************************/
-SubgraphSolver::SubgraphSolver(const GaussianBayesNet::shared_ptr &Rc1, const GaussianFactorGraph &Ab2,
+SubgraphSolver::SubgraphSolver(const GaussianBayesNet::shared_ptr &Rc1,
-    const Parameters &parameters, const Ordering& ordering) : parameters_(parameters), ordering_(ordering)
+    const GaussianFactorGraph &Ab2, const Parameters &parameters,
-{
+    const Ordering& ordering) :
+    parameters_(parameters), ordering_(ordering) {
   initialize(Rc1, boost::make_shared<GaussianFactorGraph>(Ab2));
 }
 
 /**************************************************************************************************/
 SubgraphSolver::SubgraphSolver(const GaussianBayesNet::shared_ptr &Rc1,
-    const GaussianFactorGraph::shared_ptr &Ab2, const Parameters &parameters, const Ordering& ordering) :
+    const GaussianFactorGraph::shared_ptr &Ab2, const Parameters &parameters,
-parameters_(parameters), ordering_(ordering)
+    const Ordering& ordering) :
-{
+    parameters_(parameters), ordering_(ordering) {
   initialize(Rc1, Ab2);
 }
 
 /**************************************************************************************************/
 VectorValues SubgraphSolver::optimize() {
-  VectorValues ybar = conjugateGradients<SubgraphPreconditioner, VectorValues, Errors>(*pc_, pc_->zero(), parameters_);
+  VectorValues ybar = conjugateGradients<SubgraphPreconditioner, VectorValues,
+      Errors>(*pc_, pc_->zero(), parameters_);
   return pc_->x(ybar);
 }
 
@@ -86,29 +94,33 @@ VectorValues SubgraphSolver::optimize(const VectorValues &initial) {
 }
 
 /**************************************************************************************************/
-void SubgraphSolver::initialize(const GaussianFactorGraph &jfg)
+void SubgraphSolver::initialize(const GaussianFactorGraph &jfg) {
-{
+  GaussianFactorGraph::shared_ptr Ab1 =
-  GaussianFactorGraph::shared_ptr Ab1 = boost::make_shared<GaussianFactorGraph>(),
+      boost::make_shared<GaussianFactorGraph>(), Ab2 = boost::make_shared<
-                                  Ab2 = boost::make_shared<GaussianFactorGraph>();
+      GaussianFactorGraph>();
 
-  boost::tie(Ab1, Ab2) = splitGraph(jfg) ;
+  boost::tie(Ab1, Ab2) = splitGraph(jfg);
   if (parameters_.verbosity())
-    cout << "Split A into (A1) " << Ab1->size() << " and (A2) " << Ab2->size() << " factors" << endl;
+    cout << "Split A into (A1) " << Ab1->size() << " and (A2) " << Ab2->size()
+        << " factors" << endl;
 
-  GaussianBayesNet::shared_ptr Rc1 = Ab1->eliminateSequential(ordering_, EliminateQR);
+  GaussianBayesNet::shared_ptr Rc1 = Ab1->eliminateSequential(ordering_,
-  VectorValues::shared_ptr xbar = boost::make_shared<VectorValues>(Rc1->optimize());
+      EliminateQR);
+  VectorValues::shared_ptr xbar = boost::make_shared<VectorValues>(
+      Rc1->optimize());
   pc_ = boost::make_shared<SubgraphPreconditioner>(Ab2, Rc1, xbar);
 }
 
 /**************************************************************************************************/
-void SubgraphSolver::initialize(const GaussianBayesNet::shared_ptr &Rc1, const GaussianFactorGraph::shared_ptr &Ab2)
+void SubgraphSolver::initialize(const GaussianBayesNet::shared_ptr &Rc1,
-{
+    const GaussianFactorGraph::shared_ptr &Ab2) {
-  VectorValues::shared_ptr xbar = boost::make_shared<VectorValues>(Rc1->optimize());
+  VectorValues::shared_ptr xbar = boost::make_shared<VectorValues>(
+      Rc1->optimize());
   pc_ = boost::make_shared<SubgraphPreconditioner>(Ab2, Rc1, xbar);
 }
 
 /**************************************************************************************************/
-boost::tuple<GaussianFactorGraph::shared_ptr, GaussianFactorGraph::shared_ptr>
+boost::tuple<GaussianFactorGraph::shared_ptr, GaussianFactorGraph::shared_ptr> //
 SubgraphSolver::splitGraph(const GaussianFactorGraph &jfg) {
 
   const VariableIndex index(jfg);
@@ -116,39 +128,43 @@ SubgraphSolver::splitGraph(const GaussianFactorGraph &jfg) {
   DSFVector D(n);
 
   GaussianFactorGraph::shared_ptr At(new GaussianFactorGraph());
-  GaussianFactorGraph::shared_ptr Ac( new GaussianFactorGraph());
+  GaussianFactorGraph::shared_ptr Ac(new GaussianFactorGraph());
 
   size_t t = 0;
   BOOST_FOREACH ( const GaussianFactor::shared_ptr &gf, jfg ) {
 
-    if ( gf->keys().size() > 2 ) {
+    if (gf->keys().size() > 2) {
-      throw runtime_error("SubgraphSolver::splitGraph the graph is not simple, sanity check failed ");
+      throw runtime_error(
+          "SubgraphSolver::splitGraph the graph is not simple, sanity check failed ");
     }
 
-    bool augment = false ;
+    bool augment = false;
 
     /* check whether this factor should be augmented to the "tree" graph */
-    if ( gf->keys().size() == 1 ) augment = true;
+    if (gf->keys().size() == 1)
+      augment = true;
     else {
-      const Key u = gf->keys()[0], v = gf->keys()[1],
+      const Key u = gf->keys()[0], v = gf->keys()[1], u_root = D.findSet(u),
-                  u_root = D.findSet(u), v_root = D.findSet(v);
+          v_root = D.findSet(v);
-      if ( u_root != v_root ) {
+      if (u_root != v_root) {
-        t++; augment = true ;
+        t++;
+        augment = true;
         D.makeUnionInPlace(u_root, v_root);
       }
     }
-    if ( augment ) At->push_back(gf);
+    if (augment)
-    else Ac->push_back(gf);
+      At->push_back(gf);
+    else
+      Ac->push_back(gf);
   }
 
   return boost::tie(At, Ac);
 }
 
 /****************************************************************************/
-VectorValues SubgraphSolver::optimize (
+VectorValues SubgraphSolver::optimize(const GaussianFactorGraph &gfg,
-  const GaussianFactorGraph &gfg,
+    const KeyInfo &keyInfo, const std::map<Key, Vector> &lambda,
-  const KeyInfo &keyInfo,
+    const VectorValues &initial) {
-  const std::map<Key, Vector> &lambda,
+  return VectorValues();
-  const VectorValues &initial
+}
-) { return VectorValues(); }
 } // \namespace gtsam

gtsam/linear/SubgraphSolver.h

@@ -9,27 +9,37 @@
 
  * -------------------------------------------------------------------------- */
 
+/**
+ * @file   SubgraphSolver.h
+ * @brief  Subgraph Solver from IROS 2010
+ * @date   2010
+ * @author Frank Dellaert
+ * @author Yong Dian Jian
+ */
+
 #pragma once
 
-
 #include <gtsam/linear/ConjugateGradientSolver.h>
-#include <gtsam/inference/Ordering.h>
-#include <boost/tuple/tuple.hpp>
-#include <iosfwd>
 
 namespace gtsam {
 
-  // Forward declarations
+// Forward declarations
-  class GaussianFactorGraph;
+class GaussianFactorGraph;
-  class GaussianBayesNet;
+class GaussianBayesNet;
-  class SubgraphPreconditioner;
+class SubgraphPreconditioner;
 
-class GTSAM_EXPORT SubgraphSolverParameters : public ConjugateGradientParameters {
+class GTSAM_EXPORT SubgraphSolverParameters: public ConjugateGradientParameters {
 public:
   typedef ConjugateGradientParameters Base;
-  SubgraphSolverParameters() : Base() {}
+  SubgraphSolverParameters() :
-  void print() const { Base::print(); }
+      Base() {
-  virtual void print(std::ostream &os) const { Base::print(os); }
+  }
+  void print() const {
+    Base::print();
+  }
+  virtual void print(std::ostream &os) const {
+    Base::print(os);
+  }
 };
 
 /**
@@ -53,8 +63,7 @@ public:
  *
  * \nosubgrouping
  */
-
+class GTSAM_EXPORT SubgraphSolver: public IterativeSolver {
-class GTSAM_EXPORT SubgraphSolver : public IterativeSolver {
 
 public:
   typedef SubgraphSolverParameters Parameters;
@@ -65,38 +74,61 @@ protected:
   boost::shared_ptr<SubgraphPreconditioner> pc_; ///< preconditioner object
 
 public:
-  /* Given a gaussian factor graph, split it into a spanning tree (A1) + others (A2) for SPCG */
-  SubgraphSolver(const GaussianFactorGraph &A, const Parameters &parameters, const Ordering& ordering);
-  SubgraphSolver(const boost::shared_ptr<GaussianFactorGraph> &A, const Parameters &parameters, const Ordering& ordering);
 
-  /* The user specify the subgraph part and the constraint part, may throw exception if A1 is underdetermined */
+  /// Given a gaussian factor graph, split it into a spanning tree (A1) + others (A2) for SPCG
-  SubgraphSolver(const GaussianFactorGraph &Ab1, const GaussianFactorGraph &Ab2, const Parameters &parameters, const Ordering& ordering);
+  SubgraphSolver(const GaussianFactorGraph &A, const Parameters &parameters,
-  SubgraphSolver(const boost::shared_ptr<GaussianFactorGraph> &Ab1, const boost::shared_ptr<GaussianFactorGraph> &Ab2, const Parameters &parameters, const Ordering& ordering);
+      const Ordering& ordering);
+
+  /// Shared pointer version
+  SubgraphSolver(const boost::shared_ptr<GaussianFactorGraph> &A,
+      const Parameters &parameters, const Ordering& ordering);
+
+  /**
+   * The user specify the subgraph part and the constraint part
+   * may throw exception if A1 is underdetermined
+   */
+  SubgraphSolver(const GaussianFactorGraph &Ab1, const GaussianFactorGraph &Ab2,
+      const Parameters &parameters, const Ordering& ordering);
+
+  /// Shared pointer version
+  SubgraphSolver(const boost::shared_ptr<GaussianFactorGraph> &Ab1,
+      const boost::shared_ptr<GaussianFactorGraph> &Ab2,
+      const Parameters &parameters, const Ordering& ordering);
 
   /* The same as above, but the A1 is solved before */
-  SubgraphSolver(const boost::shared_ptr<GaussianBayesNet> &Rc1, const GaussianFactorGraph &Ab2, const Parameters &parameters, const Ordering& ordering);
+  SubgraphSolver(const boost::shared_ptr<GaussianBayesNet> &Rc1,
-  SubgraphSolver(const boost::shared_ptr<GaussianBayesNet> &Rc1, const boost::shared_ptr<GaussianFactorGraph> &Ab2, const Parameters &parameters, const Ordering& ordering);
+      const GaussianFactorGraph &Ab2, const Parameters &parameters,
+      const Ordering& ordering);
 
-  virtual ~SubgraphSolver() {}
+  /// Shared pointer version
+  SubgraphSolver(const boost::shared_ptr<GaussianBayesNet> &Rc1,
+      const boost::shared_ptr<GaussianFactorGraph> &Ab2,
+      const Parameters &parameters, const Ordering& ordering);
 
-  VectorValues optimize () ;
+  /// Destructor
-  VectorValues optimize (const VectorValues &initial) ;
+  virtual ~SubgraphSolver() {
+  }
 
-  /* interface to the nonlinear optimizer that the subclasses have to implement */
+  /// Optimize from zero
-  virtual VectorValues optimize (
+  VectorValues optimize();
-    const GaussianFactorGraph &gfg,
+
-    const KeyInfo &keyInfo,
+  /// Optimize from given initial values
-    const std::map<Key, Vector> &lambda,
+  VectorValues optimize(const VectorValues &initial);
-    const VectorValues &initial
+
-  ) ;
+  /** Interface that IterativeSolver subclasses have to implement */
+  virtual VectorValues optimize(const GaussianFactorGraph &gfg,
+      const KeyInfo &keyInfo, const std::map<Key, Vector> &lambda,
+      const VectorValues &initial);
 
 protected:
 
   void initialize(const GaussianFactorGraph &jfg);
-  void initialize(const boost::shared_ptr<GaussianBayesNet> &Rc1, const boost::shared_ptr<GaussianFactorGraph> &Ab2);
+  void initialize(const boost::shared_ptr<GaussianBayesNet> &Rc1,
+      const boost::shared_ptr<GaussianFactorGraph> &Ab2);
 
-  boost::tuple<boost::shared_ptr<GaussianFactorGraph>, boost::shared_ptr<GaussianFactorGraph> >
+  boost::tuple<boost::shared_ptr<GaussianFactorGraph>,
-  splitGraph(const GaussianFactorGraph &gfg) ;
+      boost::shared_ptr<GaussianFactorGraph> >
+  splitGraph(const GaussianFactorGraph &gfg);
 };
 
 } // namespace gtsam

gtsam/nonlinear/NonlinearConjugateGradientOptimizer.cpp

@@ -1,7 +1,18 @@
+/* ----------------------------------------------------------------------------
+
+ * 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   GradientDescentOptimizer.cpp
+ * @file   NonlinearConjugateGradientOptimizer.cpp
- * @brief  
+ * @brief  Simple non-linear optimizer that solves using *non-preconditioned* CG
- * @author ydjian
+ * @author Yong-Dian Jian
  * @date   Jun 11, 2012
  */
 
@@ -16,36 +27,49 @@ using namespace std;
 
 namespace gtsam {
 
-/* Return the gradient vector of a nonlinear factor given a linearization point and a variable ordering
+/**
- * Can be moved to NonlinearFactorGraph.h if desired */
+ * @brief Return the gradient vector of a nonlinear factor graph
-VectorValues gradientInPlace(const NonlinearFactorGraph &nfg, const Values &values) {
+ * @param nfg the graph
+ * @param values a linearization point
+ * Can be moved to NonlinearFactorGraph.h if desired
+ */
+static VectorValues gradientInPlace(const NonlinearFactorGraph &nfg,
+    const Values &values) {
   // Linearize graph
   GaussianFactorGraph::shared_ptr linear = nfg.linearize(values);
   return linear->gradientAtZero();
 }
 
-double NonlinearConjugateGradientOptimizer::System::error(const State &state) const {
+double NonlinearConjugateGradientOptimizer::System::error(
+    const State &state) const {
   return graph_.error(state);
 }
 
-NonlinearConjugateGradientOptimizer::System::Gradient NonlinearConjugateGradientOptimizer::System::gradient(const State &state) const {
+NonlinearConjugateGradientOptimizer::System::Gradient NonlinearConjugateGradientOptimizer::System::gradient(
+    const State &state) const {
   return gradientInPlace(graph_, state);
 }
-NonlinearConjugateGradientOptimizer::System::State NonlinearConjugateGradientOptimizer::System::advance(const State &current, const double alpha, const Gradient &g) const {
+
+NonlinearConjugateGradientOptimizer::System::State NonlinearConjugateGradientOptimizer::System::advance(
+    const State &current, const double alpha, const Gradient &g) const {
   Gradient step = g;
   step *= alpha;
   return current.retract(step);
 }
 
 void NonlinearConjugateGradientOptimizer::iterate() {
-  int dummy ;
+  int dummy;
-  boost::tie(state_.values, dummy) = nonlinearConjugateGradient<System, Values>(System(graph_), state_.values, params_, true /* single iterations */);
+  boost::tie(state_.values, dummy) = nonlinearConjugateGradient<System, Values>(
+      System(graph_), state_.values, params_, true /* single iterations */);
   ++state_.iterations;
   state_.error = graph_.error(state_.values);
 }
 
 const Values& NonlinearConjugateGradientOptimizer::optimize() {
-  boost::tie(state_.values, state_.iterations) = nonlinearConjugateGradient<System, Values>(System(graph_), state_.values, params_, false /* up to convergent */);
+  // Optimize until convergence
+  System system(graph_);
+  boost::tie(state_.values, state_.iterations) = //
+      nonlinearConjugateGradient(system, state_.values, params_, false);
   state_.error = graph_.error(state_.values);
   return state_.values;
 }

gtsam/nonlinear/NonlinearConjugateGradientOptimizer.h

@@ -1,8 +1,19 @@
+/* ----------------------------------------------------------------------------
+
+ * 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   GradientDescentOptimizer.cpp
+ * @file   NonlinearConjugateGradientOptimizer.h
- * @brief
+ * @brief  Simple non-linear optimizer that solves using *non-preconditioned* CG
  * @author Yong-Dian Jian
- * @date   Jun 11, 2012
+ * @date   June 11, 2012
  */
 
 #pragma once
@@ -13,15 +24,18 @@
 
 namespace gtsam {
 
-/**  An implementation of the nonlinear cg method using the template below */
+/**  An implementation of the nonlinear CG method using the template below */
-class GTSAM_EXPORT NonlinearConjugateGradientState : public NonlinearOptimizerState {
+class GTSAM_EXPORT NonlinearConjugateGradientState: public NonlinearOptimizerState {
 public:
   typedef NonlinearOptimizerState Base;
-  NonlinearConjugateGradientState(const NonlinearFactorGraph& graph, const Values& values)
+  NonlinearConjugateGradientState(const NonlinearFactorGraph& graph,
-    : Base(graph, values) {}
+      const Values& values) :
+      Base(graph, values) {
+  }
 };
 
-class GTSAM_EXPORT NonlinearConjugateGradientOptimizer : public NonlinearOptimizer {
+class GTSAM_EXPORT NonlinearConjugateGradientOptimizer: public NonlinearOptimizer {
+
   /* a class for the nonlinearConjugateGradient template */
   class System {
   public:
@@ -33,37 +47,49 @@ class GTSAM_EXPORT NonlinearConjugateGradientOptimizer : public NonlinearOptimiz
     const NonlinearFactorGraph &graph_;
 
   public:
-    System(const NonlinearFactorGraph &graph): graph_(graph) {}
+    System(const NonlinearFactorGraph &graph) :
-    double error(const State &state) const ;
+        graph_(graph) {
-    Gradient gradient(const State &state) const ;
+    }
-    State advance(const State &current, const double alpha, const Gradient &g) const ;
+    double error(const State &state) const;
+    Gradient gradient(const State &state) const;
+    State advance(const State &current, const double alpha,
+        const Gradient &g) const;
   };
 
 public:
+
   typedef NonlinearOptimizer Base;
-  typedef NonlinearConjugateGradientState States;
   typedef NonlinearOptimizerParams Parameters;
   typedef boost::shared_ptr<NonlinearConjugateGradientOptimizer> shared_ptr;
 
 protected:
-  States state_;
+  NonlinearConjugateGradientState state_;
   Parameters params_;
 
 public:
 
-  NonlinearConjugateGradientOptimizer(const NonlinearFactorGraph& graph, const Values& initialValues,
+  /// Constructor
-                                      const Parameters& params = Parameters())
+  NonlinearConjugateGradientOptimizer(const NonlinearFactorGraph& graph,
-    : Base(graph), state_(graph, initialValues), params_(params) {}
+      const Values& initialValues, const Parameters& params = Parameters()) :
+      Base(graph), state_(graph, initialValues), params_(params) {
+  }
+
+  /// Destructor
+  virtual ~NonlinearConjugateGradientOptimizer() {
+  }
 
-  virtual ~NonlinearConjugateGradientOptimizer() {}
   virtual void iterate();
-  virtual const Values& optimize ();
+  virtual const Values& optimize();
-  virtual const NonlinearOptimizerState& _state() const { return state_; }
+  virtual const NonlinearOptimizerState& _state() const {
-  virtual const NonlinearOptimizerParams& _params() const { return params_; }
+    return state_;
+  }
+  virtual const NonlinearOptimizerParams& _params() const {
+    return params_;
+  }
 };
 
 /** Implement the golden-section line search algorithm */
-template <class S, class V, class W>
+template<class S, class V, class W>
 double lineSearch(const S &system, const V currentValues, const W &gradient) {
 
   /* normalize it such that it becomes a unit vector */
@@ -71,37 +97,40 @@ double lineSearch(const S &system, const V currentValues, const W &gradient) {
 
   // perform the golden section search algorithm to decide the the optimal step size
   // detail refer to http://en.wikipedia.org/wiki/Golden_section_search
-  const double phi = 0.5*(1.0+std::sqrt(5.0)), resphi = 2.0 - phi, tau = 1e-5;
+  const double phi = 0.5 * (1.0 + std::sqrt(5.0)), resphi = 2.0 - phi, tau =
-  double minStep = -1.0/g, maxStep = 0,
+      1e-5;
-         newStep = minStep + (maxStep-minStep) / (phi+1.0) ;
+  double minStep = -1.0 / g, maxStep = 0, newStep = minStep
+      + (maxStep - minStep) / (phi + 1.0);
 
   V newValues = system.advance(currentValues, newStep, gradient);
   double newError = system.error(newValues);
 
   while (true) {
-    const bool flag = (maxStep - newStep > newStep - minStep) ? true : false ;
+    const bool flag = (maxStep - newStep > newStep - minStep) ? true : false;
-    const double testStep = flag ?
+    const double testStep =
-                            newStep + resphi * (maxStep - newStep) : newStep - resphi * (newStep - minStep);
+        flag ? newStep + resphi * (maxStep - newStep) :
+            newStep - resphi * (newStep - minStep);
 
-    if ( (maxStep- minStep) < tau * (std::fabs(testStep) + std::fabs(newStep)) ) {
+    if ((maxStep - minStep)
-      return  0.5*(minStep+maxStep);
+        < tau * (std::fabs(testStep) + std::fabs(newStep))) {
+      return 0.5 * (minStep + maxStep);
     }
 
     const V testValues = system.advance(currentValues, testStep, gradient);
     const double testError = system.error(testValues);
 
     // update the working range
-    if ( testError >= newError ) {
+    if (testError >= newError) {
-      if ( flag ) maxStep = testStep;
+      if (flag)
-      else minStep = testStep;
+        maxStep = testStep;
-    }
+      else
-    else {
+        minStep = testStep;
-      if ( flag ) {
+    } else {
+      if (flag) {
         minStep = newStep;
         newStep = testStep;
         newError = testError;
-      }
+      } else {
-      else {
         maxStep = newStep;
         newStep = testStep;
         newError = testError;
@@ -112,7 +141,7 @@ double lineSearch(const S &system, const V currentValues, const W &gradient) {
 }
 
 /**
- * Implement the nonlinear conjugate gradient method using the Polak-Ribieve formula suggested in
+ * Implement the nonlinear conjugate gradient method using the Polak-Ribiere formula suggested in
  * http://en.wikipedia.org/wiki/Nonlinear_conjugate_gradient_method.
  *
  * The S (system) class requires three member functions: error(state), gradient(state) and
@@ -120,8 +149,10 @@ double lineSearch(const S &system, const V currentValues, const W &gradient) {
  *
  * The last parameter is a switch between gradient-descent and conjugate gradient
  */
-template <class S, class V>
+template<class S, class V>
-boost::tuple<V, int> nonlinearConjugateGradient(const S &system, const V &initial, const NonlinearOptimizerParams &params, const bool singleIteration, const bool gradientDescent = false) {
+boost::tuple<V, int> nonlinearConjugateGradient(const S &system,
+    const V &initial, const NonlinearOptimizerParams &params,
+    const bool singleIteration, const bool gradientDescent = false) {
 
   // GTSAM_CONCEPT_MANIFOLD_TYPE(V);
 
@@ -129,57 +160,67 @@ boost::tuple<V, int> nonlinearConjugateGradient(const S &system, const V &initia
 
   // check if we're already close enough
   double currentError = system.error(initial);
-  if(currentError <= params.errorTol) {
+  if (currentError <= params.errorTol) {
-    if (params.verbosity >= NonlinearOptimizerParams::ERROR){
+    if (params.verbosity >= NonlinearOptimizerParams::ERROR) {
-      std::cout << "Exiting, as error = " << currentError << " < " << params.errorTol << std::endl;
+      std::cout << "Exiting, as error = " << currentError << " < "
+          << params.errorTol << std::endl;
     }
     return boost::tie(initial, iteration);
   }
 
   V currentValues = initial;
-  typename S::Gradient currentGradient = system.gradient(currentValues), prevGradient,
+  typename S::Gradient currentGradient = system.gradient(currentValues),
-                       direction = currentGradient;
+      prevGradient, direction = currentGradient;
 
   /* do one step of gradient descent */
-  V prevValues = currentValues; double prevError = currentError;
+  V prevValues = currentValues;
+  double prevError = currentError;
   double alpha = lineSearch(system, currentValues, direction);
   currentValues = system.advance(prevValues, alpha, direction);
   currentError = system.error(currentValues);
 
   // Maybe show output
-  if (params.verbosity >= NonlinearOptimizerParams::ERROR) std::cout << "Initial error: " << currentError << std::endl;
+  if (params.verbosity >= NonlinearOptimizerParams::ERROR)
+    std::cout << "Initial error: " << currentError << std::endl;
 
   // Iterative loop
   do {
-    if ( gradientDescent == true) {
+    if (gradientDescent == true) {
       direction = system.gradient(currentValues);
-    }
+    } else {
-    else {
       prevGradient = currentGradient;
       currentGradient = system.gradient(currentValues);
-      const double beta = std::max(0.0, currentGradient.dot(currentGradient-prevGradient)/currentGradient.dot(currentGradient));
+      // Polak-Ribiere: beta = g'*(g_n-g_n-1)/g_n-1'*g_n-1
-      direction = currentGradient + (beta*direction);
+      const double beta = std::max(0.0,
+          currentGradient.dot(currentGradient - prevGradient)
+              / prevGradient.dot(prevGradient));
+      direction = currentGradient + (beta * direction);
     }
 
     alpha = lineSearch(system, currentValues, direction);
 
-    prevValues = currentValues; prevError = currentError;
+    prevValues = currentValues;
+    prevError = currentError;
 
     currentValues = system.advance(prevValues, alpha, direction);
     currentError = system.error(currentValues);
 
     // Maybe show output
-    if(params.verbosity >= NonlinearOptimizerParams::ERROR) std::cout << "currentError: " << currentError << std::endl;
+    if (params.verbosity >= NonlinearOptimizerParams::ERROR)
-  } while( ++iteration < params.maxIterations &&
+      std::cout << "iteration: " << iteration << ", currentError: " << currentError << std::endl;
-           !singleIteration &&
+  } while (++iteration < params.maxIterations && !singleIteration
-           !checkConvergence(params.relativeErrorTol, params.absoluteErrorTol, params.errorTol, prevError, currentError, params.verbosity));
+      && !checkConvergence(params.relativeErrorTol, params.absoluteErrorTol,
+          params.errorTol, prevError, currentError, params.verbosity));
 
   // Printing if verbose
-  if (params.verbosity >= NonlinearOptimizerParams::ERROR && iteration >= params.maxIterations)
+  if (params.verbosity >= NonlinearOptimizerParams::ERROR
-    std::cout << "nonlinearConjugateGradient: Terminating because reached maximum iterations" << std::endl;
+      && iteration >= params.maxIterations)
+    std::cout
+        << "nonlinearConjugateGradient: Terminating because reached maximum iterations"
+        << std::endl;
 
   return boost::tie(currentValues, iteration);
 }
 
-}
+} // \ namespace gtsam
 

gtsam/slam/JacobianFactorQ.h

@@ -1,3 +1,14 @@
+/* ----------------------------------------------------------------------------
+
+ * 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  JacobianFactorQ.h
  * @date  Oct 27, 2013

gtsam/slam/JacobianFactorQR.h

@@ -7,8 +7,13 @@
 
 #pragma once
 #include <gtsam/slam/JacobianSchurFactor.h>
+#include <gtsam/linear/GaussianFactorGraph.h>
+#include <gtsam/inference/Symbol.h>
 
 namespace gtsam {
+
+class GaussianBayesNet;
+
 /**
  * JacobianFactor for Schur complement that uses Q noise model
  */
@@ -38,7 +43,7 @@ public:
     //gfg.print("gfg");
 
     // eliminate the point
-    GaussianBayesNet::shared_ptr bn;
+    boost::shared_ptr<GaussianBayesNet> bn;
     GaussianFactorGraph::shared_ptr fg;
     std::vector < Key > variables;
     variables.push_back(pointKey);

gtsam/slam/JacobianSchurFactor.h

@@ -1,3 +1,14 @@
+/* ----------------------------------------------------------------------------
+
+ * 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  JacobianSchurFactor.h
  * @brief Jacobianfactor that combines and eliminates points
@@ -7,11 +18,7 @@
 
 #pragma once
 
-#include <gtsam/inference/Symbol.h>
-#include <gtsam/linear/VectorValues.h>
 #include <gtsam/linear/JacobianFactor.h>
-#include <gtsam/linear/GaussianBayesNet.h>
-#include <gtsam/linear/GaussianFactorGraph.h>
 #include <boost/foreach.hpp>
 
 namespace gtsam {

gtsam/slam/RegularHessianFactor.h

@@ -65,13 +65,14 @@ public:
   mutable std::vector<DVector> y;
 
   /** y += alpha * A'*A*x */
-  void multiplyHessianAdd(double alpha, const VectorValues& x, VectorValues& y) const{
+  virtual void multiplyHessianAdd(double alpha, const VectorValues& x,
-    throw std::runtime_error(
+      VectorValues& y) const {
-          "RegularHessianFactor::forbidden use of multiplyHessianAdd without raw memory access, use HessianFactor instead");
+    HessianFactor::multiplyHessianAdd(alpha, x, y);
   }
 
   /** y += alpha * A'*A*x */
-  void multiplyHessianAdd(double alpha, const double* x, double* yvalues) const {
+  void multiplyHessianAdd(double alpha, const double* x,
+      double* yvalues) const {
     // Create a vector of temporary y values, corresponding to rows i
     y.resize(size());
     BOOST_FOREACH(DVector & yi, y)
@@ -104,6 +105,7 @@ public:
     }
   }
 
+  /// Raw memory version, with offsets TODO document reasoning
   void multiplyHessianAdd(double alpha, const double* x, double* yvalues,
       std::vector<size_t> offsets) const {
 
@@ -140,43 +142,38 @@ public:
 
     // copy to yvalues
     for (DenseIndex i = 0; i < (DenseIndex) size(); ++i)
-      DMap(yvalues + offsets[keys_[i]], offsets[keys_[i] + 1] - offsets[keys_[i]]) +=
+      DMap(yvalues + offsets[keys_[i]],
-          alpha * y[i];
+          offsets[keys_[i] + 1] - offsets[keys_[i]]) += alpha * y[i];
   }
 
   /** Return the diagonal of the Hessian for this factor (raw memory version) */
   virtual void hessianDiagonal(double* d) const {
 
     // Use eigen magic to access raw memory
-    //typedef Eigen::Matrix<double, 9, 1> DVector;
     typedef Eigen::Matrix<double, D, 1> DVector;
     typedef Eigen::Map<DVector> DMap;
 
     // Loop over all variables in the factor
-    for (DenseIndex pos = 0; pos < (DenseIndex)size(); ++pos) {
+    for (DenseIndex pos = 0; pos < (DenseIndex) size(); ++pos) {
       Key j = keys_[pos];
       // Get the diagonal block, and insert its diagonal
       const Matrix& B = info_(pos, pos).selfadjointView();
-      //DMap(d + 9 * j) += B.diagonal();
       DMap(d + D * j) += B.diagonal();
     }
   }
 
-  /* ************************************************************************* */
+  /// Add gradient at zero to d TODO: is it really the goal to add ??
-  // TODO: currently assumes all variables of the same size 9 and keys arranged from 0 to n
   virtual void gradientAtZero(double* d) const {
 
     // Use eigen magic to access raw memory
-    //typedef Eigen::Matrix<double, 9, 1> DVector;
     typedef Eigen::Matrix<double, D, 1> DVector;
     typedef Eigen::Map<DVector> DMap;
 
     // Loop over all variables in the factor
-    for (DenseIndex pos = 0; pos < (DenseIndex)size(); ++pos) {
+    for (DenseIndex pos = 0; pos < (DenseIndex) size(); ++pos) {
       Key j = keys_[pos];
       // Get the diagonal block, and insert its diagonal
-      VectorD dj =  -info_(pos,size()).knownOffDiagonal();
+      VectorD dj = -info_(pos, size()).knownOffDiagonal();
-      //DMap(d + 9 * j) += dj;
       DMap(d + D * j) += dj;
     }
   }

gtsam/slam/RegularImplicitSchurFactor.h

@@ -7,12 +7,10 @@
 
 #pragma once
 
-#include <gtsam/linear/GaussianFactorGraph.h>
 #include <gtsam/linear/JacobianFactor.h>
 #include <gtsam/linear/VectorValues.h>
 #include <boost/foreach.hpp>
-#include <boost/make_shared.hpp>
+#include <iosfwd>
-#include <iostream>
 
 namespace gtsam {
 

gtsam/slam/RegularJacobianFactor.h

@@ -52,11 +52,17 @@ public:
    *  factor. */
   template<typename KEYS>
   RegularJacobianFactor(const KEYS& keys,
-      const VerticalBlockMatrix& augmentedMatrix,
+      const VerticalBlockMatrix& augmentedMatrix, const SharedDiagonal& sigmas =
-      const SharedDiagonal& sigmas = SharedDiagonal()) :
+          SharedDiagonal()) :
       JacobianFactor(keys, augmentedMatrix, sigmas) {
   }
 
+  /** y += alpha * A'*A*x */
+  virtual void multiplyHessianAdd(double alpha, const VectorValues& x,
+      VectorValues& y) const {
+    JacobianFactor::multiplyHessianAdd(alpha, x, y);
+  }
+
   /** Raw memory access version of multiplyHessianAdd y += alpha * A'*A*x
    * Note: this is not assuming a fixed dimension for the variables,
    * but requires the vector accumulatedDims to tell the dimension of
@@ -78,10 +84,10 @@ public:
     /// Just iterate over all A matrices and multiply in correct config part (looping over keys)
     /// E.g.: Jacobian A = [A0 A1 A2] multiplies x = [x0 x1 x2]'
     /// Hence: Ax = A0 x0 + A1 x1 + A2 x2 (hence we loop over the keys and accumulate)
-    for (size_t pos = 0; pos < size(); ++pos)
+    for (size_t pos = 0; pos < size(); ++pos) {
-    {
       Ax += Ab_(pos)
-          * ConstMapD(x + accumulatedDims[keys_[pos]], accumulatedDims[keys_[pos] + 1] - accumulatedDims[keys_[pos]]);
+          * ConstMapD(x + accumulatedDims[keys_[pos]],
+              accumulatedDims[keys_[pos] + 1] - accumulatedDims[keys_[pos]]);
     }
     /// Deal with noise properly, need to Double* whiten as we are dividing by variance
     if (model_) {
@@ -93,8 +99,9 @@ public:
     Ax *= alpha;
 
     /// Again iterate over all A matrices and insert Ai^T into y
-    for (size_t pos = 0; pos < size(); ++pos){
+    for (size_t pos = 0; pos < size(); ++pos) {
-      MapD(y + accumulatedDims[keys_[pos]], accumulatedDims[keys_[pos] + 1] - accumulatedDims[keys_[pos]]) += Ab_(
+      MapD(y + accumulatedDims[keys_[pos]],
+          accumulatedDims[keys_[pos] + 1] - accumulatedDims[keys_[pos]]) += Ab_(
           pos).transpose() * Ax;
     }
   }
@@ -102,21 +109,25 @@ public:
   /** Raw memory access version of multiplyHessianAdd */
   void multiplyHessianAdd(double alpha, const double* x, double* y) const {
 
-    if (empty()) return;
+    if (empty())
+      return;
     Vector Ax = zero(Ab_.rows());
 
     // Just iterate over all A matrices and multiply in correct config part
-    for(size_t pos=0; pos<size(); ++pos)
+    for (size_t pos = 0; pos < size(); ++pos)
       Ax += Ab_(pos) * ConstDMap(x + D * keys_[pos]);
 
     // Deal with noise properly, need to Double* whiten as we are dividing by variance
-    if  (model_) { model_->whitenInPlace(Ax); model_->whitenInPlace(Ax); }
+    if (model_) {
+      model_->whitenInPlace(Ax);
+      model_->whitenInPlace(Ax);
+    }
 
     // multiply with alpha
     Ax *= alpha;
 
     // Again iterate over all A matrices and insert Ai^e into y
-    for(size_t pos=0; pos<size(); ++pos)
+    for (size_t pos = 0; pos < size(); ++pos)
       DMap(y + D * keys_[pos]) += Ab_(pos).transpose() * Ax;
   }
 
@@ -128,12 +139,12 @@ public:
     for (DenseIndex j = 0; j < (DenseIndex) size(); ++j) {
       // Get the diagonal block, and insert its diagonal
       DVector dj;
-      for (size_t k = 0; k < D; ++k){
+      for (size_t k = 0; k < D; ++k) {
-        if (model_){
+        if (model_) {
           Vector column_k = Ab_(j).col(k);
           column_k = model_->whiten(column_k);
           dj(k) = dot(column_k, column_k);
-        }else{
+        } else {
           dj(k) = Ab_(j).col(k).squaredNorm();
         }
       }
@@ -156,13 +167,13 @@ public:
     }
 
     // Just iterate over all A matrices
-    for (DenseIndex j = 0; j < (DenseIndex)size(); ++j) {
+    for (DenseIndex j = 0; j < (DenseIndex) size(); ++j) {
       DVector dj;
       // gradient -= A'*b/sigma^2
       // Computing with each column
-      for (size_t k = 0; k < D; ++k){
+      for (size_t k = 0; k < D; ++k) {
         Vector column_k = Ab_(j).col(k);
-        dj(k) = -1.0*dot(b, column_k);
+        dj(k) = -1.0 * dot(b, column_k);
       }
       DMap(d + D * j) += dj;
     }

gtsam/slam/SmartFactorBase.h

@@ -25,28 +25,39 @@
 #include <gtsam/slam/RegularHessianFactor.h>
 
 #include <gtsam/nonlinear/NonlinearFactor.h>
-#include <gtsam/geometry/PinholeCamera.h>  // for Cheirality exception
+#include <gtsam/geometry/CameraSet.h>
-#include <gtsam/geometry/StereoCamera.h>
-#include <gtsam/geometry/Pose3.h>
-#include <gtsam/inference/Symbol.h>
 
 #include <boost/optional.hpp>
 #include <boost/make_shared.hpp>
 #include <vector>
 
 namespace gtsam {
-/// Base class with no internal point, completely functional
+
-template<class POSE, class Z, class CAMERA, size_t D>
+/**
+ * @brief  Base class for smart factors
+ * This base class has no internal point, but it has a measurement, noise model
+ * and an optional sensor pose.
+ * This class mainly computes the derivatives and returns them as a variety of factors.
+ * The methods take a Cameras argument, which should behave like PinholeCamera, and
+ * the value of a point, which is kept in the base class.
+ */
+template<class CAMERA, size_t D>
 class SmartFactorBase: public NonlinearFactor {
 
 protected:
 
-  // Keep a copy of measurement and calibration for I/O
+  typedef typename CAMERA::Measurement Z;
-  std::vector<Z> measured_; ///< 2D measurement for each of the m views
-  std::vector<SharedNoiseModel> noise_; ///< noise model used
-  ///< (important that the order is the same as the keys that we use to create the factor)
 
-  boost::optional<POSE> body_P_sensor_; ///< The pose of the sensor in the body frame (one for all cameras)
+  /**
+   * 2D measurement and noise model for each of the m views
+   * We keep a copy of measurements for I/O and computing the error.
+   * The order is kept the same as the keys that we use to create the factor.
+   */
+  std::vector<Z> measured_;
+
+  std::vector<SharedNoiseModel> noise_; ///< noise model used
+
+  boost::optional<Pose3> body_P_sensor_; ///< The pose of the sensor in the body frame (one for all cameras)
 
   static const int ZDim = traits<Z>::dimension; ///< Measurement dimension
 
@@ -63,27 +74,22 @@ protected:
   typedef NonlinearFactor Base;
 
   /// shorthand for this class
-  typedef SmartFactorBase<POSE, Z, CAMERA, D> This;
+  typedef SmartFactorBase<CAMERA, D> This;
-
 
 public:
 
-
-
   EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 
   /// shorthand for a smart pointer to a factor
   typedef boost::shared_ptr<This> shared_ptr;
 
-  /// shorthand for a pinhole camera
+  typedef CameraSet<CAMERA> Cameras;
-  typedef CAMERA Camera;
-  typedef std::vector<CAMERA> Cameras;
 
   /**
    * Constructor
    * @param body_P_sensor is the transform from sensor to body frame (default identity)
    */
-  SmartFactorBase(boost::optional<POSE> body_P_sensor = boost::none) :
+  SmartFactorBase(boost::optional<Pose3> body_P_sensor = boost::none) :
       body_P_sensor_(body_P_sensor) {
   }
 
@@ -92,7 +98,7 @@ public:
   }
 
   /**
-   * add a new measurement and pose key
+   * Add a new measurement and pose key
    * @param measured_i is the 2m dimensional projection of a single landmark
    * @param poseKey is the index corresponding to the camera observing the landmark
    * @param noise_i is the measurement noise
@@ -105,9 +111,8 @@ public:
   }
 
   /**
-   * variant of the previous add: adds a bunch of measurements, together with the camera keys and noises
+   * Add a bunch of measurements, together with the camera keys and noises
    */
-  // ****************************************************************************************************
   void add(std::vector<Z>& measurements, std::vector<Key>& poseKeys,
       std::vector<SharedNoiseModel>& noises) {
     for (size_t i = 0; i < measurements.size(); i++) {
@@ -118,9 +123,8 @@ public:
   }
 
   /**
-   * variant of the previous add: adds a bunch of measurements and uses the same noise model for all of them
+   * Add a bunch of measurements and uses the same noise model for all of them
    */
-  // ****************************************************************************************************
   void add(std::vector<Z>& measurements, std::vector<Key>& poseKeys,
       const SharedNoiseModel& noise) {
     for (size_t i = 0; i < measurements.size(); i++) {
@@ -134,7 +138,6 @@ public:
    * Adds an entire SfM_track (collection of cameras observing a single point).
    * The noise is assumed to be the same for all measurements
    */
-  // ****************************************************************************************************
   template<class SFM_TRACK>
   void add(const SFM_TRACK& trackToAdd, const SharedNoiseModel& noise) {
     for (size_t k = 0; k < trackToAdd.number_measurements(); k++) {
@@ -144,12 +147,17 @@ public:
     }
   }
 
+  /// get the dimension (number of rows!) of the factor
+  virtual size_t dim() const {
+    return ZDim * this->measured_.size();
+  }
+
   /** return the measurements */
   const std::vector<Z>& measured() const {
     return measured_;
   }
 
-  /** return the noise model */
+  /** return the noise models */
   const std::vector<SharedNoiseModel>& noise() const {
     return noise_;
   }
@@ -187,30 +195,38 @@ public:
                 && body_P_sensor_->equals(*e->body_P_sensor_)));
   }
 
-  // ****************************************************************************************************
+  /// Calculate vector of re-projection errors, before applying noise model
-//  /// Calculate vector of re-projection errors, before applying noise model
   Vector reprojectionError(const Cameras& cameras, const Point3& point) const {
 
-    Vector b = zero(ZDim * cameras.size());
+    // Project into CameraSet
-
+    std::vector<Z> predicted;
-    size_t i = 0;
-    BOOST_FOREACH(const CAMERA& camera, cameras) {
-      const Z& zi = this->measured_.at(i);
     try {
-        Z e(camera.project(point) - zi);
+      predicted = cameras.project(point);
-        b[ZDim * i] = e.x();
+    } catch (CheiralityException&) {
-        b[ZDim * i + 1] = e.y();
+      std::cout << "reprojectionError: Cheirality exception " << std::endl;
-      } catch (CheiralityException& e) {
+      exit(EXIT_FAILURE); // TODO: throw exception
-        std::cout << "Cheirality exception " << std::endl;
-        exit(EXIT_FAILURE);
     }
-      i += 1;
+
+    // Calculate vector of errors
+    size_t nrCameras = cameras.size();
+    Vector b(ZDim * nrCameras);
+    for (size_t i = 0, row = 0; i < nrCameras; i++, row += ZDim) {
+      Z e = predicted[i] - measured_.at(i);
+      b.segment<ZDim>(row) = e.vector();
     }
 
     return b;
   }
 
-  // ****************************************************************************************************
+  /// Calculate vector of re-projection errors, noise model applied
+  Vector whitenedError(const Cameras& cameras, const Point3& point) const {
+    Vector b = reprojectionError(cameras, point);
+    size_t nrCameras = cameras.size();
+    for (size_t i = 0, row = 0; i < nrCameras; i++, row += ZDim)
+      b.segment<ZDim>(row) = noise_.at(i)->whiten(b.segment<ZDim>(row));
+    return b;
+  }
+
   /**
    * Calculate the error of the factor.
    * This is the log-likelihood, e.g. \f$ 0.5(h(x)-z)^2/\sigma^2 \f$ in case of Gaussian.
@@ -220,178 +236,233 @@ public:
    */
   double totalReprojectionError(const Cameras& cameras,
       const Point3& point) const {
-
+    Vector b = reprojectionError(cameras, point);
     double overallError = 0;
+    size_t nrCameras = cameras.size();
+    for (size_t i = 0; i < nrCameras; i++)
+      overallError += noise_.at(i)->distance(b.segment<ZDim>(i * ZDim));
+    return 0.5 * overallError;
+  }
 
-    size_t i = 0;
+  /**
-    BOOST_FOREACH(const CAMERA& camera, cameras) {
+   * Compute whitenedError, returning only the derivative E, i.e.,
-      const Z& zi = this->measured_.at(i);
+   * the stacked ZDim*3 derivatives of project with respect to the point.
+   * Assumes non-degenerate ! TODO explain this
+   */
+  Vector whitenedError(const Cameras& cameras, const Point3& point,
+      Matrix& E) const {
+
+    // Project into CameraSet, calculating derivatives
+    std::vector<Z> predicted;
     try {
-        Z reprojectionError(camera.project(point) - zi);
+      using boost::none;
-        overallError += 0.5
+      predicted = cameras.project(point, none, E, none);
-        * this->noise_.at(i)->distance(reprojectionError.vector());
     } catch (CheiralityException&) {
-        std::cout << "Cheirality exception " << std::endl;
+      std::cout << "whitenedError(E): Cheirality exception " << std::endl;
-        exit(EXIT_FAILURE);
+      exit(EXIT_FAILURE); // TODO: throw exception
-      }
-      i += 1;
-    }
-    return overallError;
     }
 
-  // ****************************************************************************************************
+    // if needed, whiten
-  /// Assumes non-degenerate !
+    size_t m = keys_.size();
-  void computeEP(Matrix& E, Matrix& PointCov, const Cameras& cameras,
+    Vector b(ZDim * m);
-      const Point3& point) const {
+    for (size_t i = 0, row = 0; i < m; i++, row += ZDim) {
 
-    int numKeys = this->keys_.size();
+      // Calculate error
-    E = zeros(ZDim * numKeys, 3);
+      const Z& zi = measured_.at(i);
-    Vector b = zero(2 * numKeys);
+      Vector bi = (zi - predicted[i]).vector();
 
-    Matrix Ei(ZDim, 3);
+      // if needed, whiten
-    for (size_t i = 0; i < this->measured_.size(); i++) {
+      SharedNoiseModel model = noise_.at(i);
+      if (model) {
+        // TODO: re-factor noiseModel to take any block/fixed vector/matrix
+        Vector dummy;
+        Matrix Ei = E.block<ZDim, 3>(row, 0);
+        model->WhitenSystem(Ei, dummy);
+        E.block<ZDim, 3>(row, 0) = Ei;
+      }
+      b.segment<ZDim>(row) = bi;
+    }
+    return b;
+  }
+
+  /**
+   *  Compute F, E, and optionally H, where F and E are the stacked derivatives
+   *  with respect to the cameras, point, and calibration, respectively.
+   *  The value of cameras/point are passed as parameters.
+   *  Returns error vector b
+   *  TODO: the treatment of body_P_sensor_ is weird: the transformation
+   *  is applied in the caller, but the derivatives are computed here.
+   */
+  Vector whitenedError(const Cameras& cameras, const Point3& point, Matrix& F,
+      Matrix& E, boost::optional<Matrix&> G = boost::none) const {
+
+    // Project into CameraSet, calculating derivatives
+    std::vector<Z> predicted;
     try {
-        cameras[i].project(point, boost::none, Ei, boost::none);
+      predicted = cameras.project(point, F, E, G);
-      } catch (CheiralityException& e) {
+    } catch (CheiralityException&) {
-        std::cout << "Cheirality exception " << std::endl;
+      std::cout << "whitenedError(E,F): Cheirality exception " << std::endl;
-        exit(EXIT_FAILURE);
+      exit(EXIT_FAILURE); // TODO: throw exception
-      }
-      this->noise_.at(i)->WhitenSystem(Ei, b);
-      E.block<ZDim, 3>(ZDim * i, 0) = Ei;
     }
 
-    // Matrix PointCov;
+    // Calculate error and whiten derivatives if needed
-    PointCov.noalias() = (E.transpose() * E).inverse();
+    size_t m = keys_.size();
-  }
+    Vector b(ZDim * m);
+    for (size_t i = 0, row = 0; i < m; i++, row += ZDim) {
 
-  // ****************************************************************************************************
+      // Calculate error
-  /// Compute F, E only (called below in both vanilla and SVD versions)
+      const Z& zi = measured_.at(i);
-  /// Given a Point3, assumes dimensionality is 3
+      Vector bi = (zi - predicted[i]).vector();
-  double computeJacobians(std::vector<KeyMatrix2D>& Fblocks, Matrix& E,
-      Vector& b, const Cameras& cameras, const Point3& point) const {
 
-    size_t numKeys = this->keys_.size();
+      // if we have a sensor offset, correct camera derivatives
-    E = zeros(ZDim * numKeys, 3);
+      if (body_P_sensor_) {
-    b = zero(ZDim * numKeys);
+        // TODO: no simpler way ??
-    double f = 0;
+        const Pose3& pose_i = cameras[i].pose();
-
+        Pose3 w_Pose_body = pose_i.compose(body_P_sensor_->inverse());
-    Matrix Fi(ZDim, 6), Ei(ZDim, 3), Hcali(ZDim, D - 6), Hcam(ZDim, D);
+        Matrix66 J;
-    for (size_t i = 0; i < this->measured_.size(); i++) {
-
-      Vector bi;
-      try {
-        bi = -(cameras[i].project(point, Fi, Ei, Hcali) - this->measured_.at(i)).vector();
-        if(body_P_sensor_){
-          Pose3 w_Pose_body = (cameras[i].pose()).compose(body_P_sensor_->inverse());
-          Matrix J(6, 6);
         Pose3 world_P_body = w_Pose_body.compose(*body_P_sensor_, J);
-          Fi = Fi * J;
+        F.block<ZDim, 6>(row, 0) *= J;
-        }
-      } catch (CheiralityException&) {
-        std::cout << "Cheirality exception " << std::endl;
-        exit(EXIT_FAILURE);
-      }
-      this->noise_.at(i)->WhitenSystem(Fi, Ei, Hcali, bi);
-
-      f += bi.squaredNorm();
-      if (D == 6) { // optimize only camera pose
-        Fblocks.push_back(KeyMatrix2D(this->keys_[i], Fi));
-      } else {
-        Hcam.block<ZDim, 6>(0, 0) = Fi; // ZDim x 6 block for the cameras
-        Hcam.block<ZDim, D - 6>(0, 6) = Hcali; // ZDim x nrCal block for the cameras
-        Fblocks.push_back(KeyMatrix2D(this->keys_[i], Hcam));
-      }
-      E.block<ZDim, 3>(ZDim * i, 0) = Ei;
-      subInsert(b, bi, ZDim * i);
-    }
-    return f;
       }
 
-  // ****************************************************************************************************
+      // if needed, whiten
-  /// Version that computes PointCov, with optional lambda parameter
+      SharedNoiseModel model = noise_.at(i);
-  double computeJacobians(std::vector<KeyMatrix2D>& Fblocks, Matrix& E,
+      if (model) {
-      Matrix3& PointCov, Vector& b, const Cameras& cameras, const Point3& point,
+        // TODO, refactor noiseModel so we can take blocks
-      double lambda = 0.0, bool diagonalDamping = false) const {
+        Matrix Fi = F.block<ZDim, 6>(row, 0);
+        Matrix Ei = E.block<ZDim, 3>(row, 0);
+        if (!G)
+          model->WhitenSystem(Fi, Ei, bi);
+        else {
+          Matrix Gi = G->block<ZDim, D - 6>(row, 0);
+          model->WhitenSystem(Fi, Ei, Gi, bi);
+          G->block<ZDim, D - 6>(row, 0) = Gi;
+        }
+        F.block<ZDim, 6>(row, 0) = Fi;
+        E.block<ZDim, 3>(row, 0) = Ei;
+      }
+      b.segment<ZDim>(row) = bi;
+    }
+    return b;
+  }
 
-    double f = computeJacobians(Fblocks, E, b, cameras, point);
+  /// Computes Point Covariance P from E
+  static Matrix3 PointCov(Matrix& E) {
+    return (E.transpose() * E).inverse();
+  }
+
+  /// Computes Point Covariance P, with lambda parameter
+  static Matrix3 PointCov(Matrix& E, double lambda,
+      bool diagonalDamping = false) {
 
-    // Point covariance inv(E'*E)
     Matrix3 EtE = E.transpose() * E;
 
     if (diagonalDamping) { // diagonal of the hessian
       EtE(0, 0) += lambda * EtE(0, 0);
       EtE(1, 1) += lambda * EtE(1, 1);
       EtE(2, 2) += lambda * EtE(2, 2);
-    }else{
+    } else {
       EtE(0, 0) += lambda;
       EtE(1, 1) += lambda;
       EtE(2, 2) += lambda;
     }
 
-    PointCov.noalias() = (EtE).inverse();
+    return (EtE).inverse();
+  }
 
+  /// Assumes non-degenerate !
+  void computeEP(Matrix& E, Matrix& P, const Cameras& cameras,
+      const Point3& point) const {
+    whitenedError(cameras, point, E);
+    P = PointCov(E);
+  }
+
+  /**
+   *  Compute F, E, and b (called below in both vanilla and SVD versions), where
+   *  F is a vector of derivatives wrpt the cameras, and E the stacked derivatives
+   *  with respect to the point. The value of cameras/point are passed as parameters.
+   */
+  double computeJacobians(std::vector<KeyMatrix2D>& Fblocks, Matrix& E,
+      Vector& b, const Cameras& cameras, const Point3& point) const {
+
+    // Project into Camera set and calculate derivatives
+    // TODO: if D==6 we optimize only camera pose. That is fairly hacky!
+    Matrix F, G;
+    using boost::none;
+    boost::optional<Matrix&> optionalG(G);
+    b = whitenedError(cameras, point, F, E, D == 6 ? none : optionalG);
+
+    // Now calculate f and divide up the F derivatives into Fblocks
+    double f = 0.0;
+    size_t m = keys_.size();
+    for (size_t i = 0, row = 0; i < m; i++, row += ZDim) {
+
+      // Accumulate normalized error
+      f += b.segment<ZDim>(row).squaredNorm();
+
+      // Get piece of F and possibly G
+      Matrix2D Fi;
+      if (D == 6)
+        Fi << F.block<ZDim, 6>(row, 0);
+      else
+        Fi << F.block<ZDim, 6>(row, 0), G.block<ZDim, D - 6>(row, 0);
+
+      // Push it onto Fblocks
+      Fblocks.push_back(KeyMatrix2D(keys_[i], Fi));
+    }
     return f;
   }
 
-  // ****************************************************************************************************
+  /// Create BIG block-diagonal matrix F from Fblocks
-  // TODO, there should not be a Matrix version, really
+  static void FillDiagonalF(const std::vector<KeyMatrix2D>& Fblocks, Matrix& F) {
-  double computeJacobians(Matrix& F, Matrix& E, Matrix3& PointCov, Vector& b,
+    size_t m = Fblocks.size();
-      const Cameras& cameras, const Point3& point,
+    F.resize(ZDim * m, D * m);
-      const double lambda = 0.0) const {
+    F.setZero();
+    for (size_t i = 0; i < m; ++i)
+      F.block<This::ZDim, D>(This::ZDim * i, D * i) = Fblocks.at(i).second;
+  }
 
-    size_t numKeys = this->keys_.size();
+  /**
+   *  Compute F, E, and b, where F and E are the stacked derivatives
+   *  with respect to the point. The value of cameras/point are passed as parameters.
+   */
+  double computeJacobians(Matrix& F, Matrix& E, Vector& b,
+      const Cameras& cameras, const Point3& point) const {
     std::vector<KeyMatrix2D> Fblocks;
-    double f = computeJacobians(Fblocks, E, PointCov, b, cameras, point,
+    double f = computeJacobians(Fblocks, E, b, cameras, point);
-        lambda);
+    FillDiagonalF(Fblocks, F);
-    F = zeros(This::ZDim * numKeys, D * numKeys);
-
-    for (size_t i = 0; i < this->keys_.size(); ++i) {
-      F.block<This::ZDim, D>(This::ZDim * i, D * i) = Fblocks.at(i).second; // ZDim x 6 block for the cameras
-    }
     return f;
   }
 
-  // ****************************************************************************************************
   /// SVD version
   double computeJacobiansSVD(std::vector<KeyMatrix2D>& Fblocks, Matrix& Enull,
-      Vector& b, const Cameras& cameras, const Point3& point, double lambda =
+      Vector& b, const Cameras& cameras, const Point3& point) const {
-          0.0, bool diagonalDamping = false) const {
 
     Matrix E;
-    Matrix3 PointCov; // useless
+    double f = computeJacobians(Fblocks, E, b, cameras, point);
-    double f = computeJacobians(Fblocks, E, PointCov, b, cameras, point, lambda,
-        diagonalDamping); // diagonalDamping should have no effect (only on PointCov)
 
     // Do SVD on A
     Eigen::JacobiSVD<Matrix> svd(E, Eigen::ComputeFullU);
     Vector s = svd.singularValues();
-    // Enull = zeros(ZDim * numKeys, ZDim * numKeys - 3);
+    size_t m = this->keys_.size();
-    size_t numKeys = this->keys_.size();
+    // Enull = zeros(ZDim * m, ZDim * m - 3);
-    Enull = svd.matrixU().block(0, 3, ZDim * numKeys, ZDim * numKeys - 3); // last ZDimm-3 columns
+    Enull = svd.matrixU().block(0, 3, ZDim * m, ZDim * m - 3); // last ZDim*m-3 columns
 
     return f;
   }
 
-  // ****************************************************************************************************
   /// Matrix version of SVD
   // TODO, there should not be a Matrix version, really
   double computeJacobiansSVD(Matrix& F, Matrix& Enull, Vector& b,
       const Cameras& cameras, const Point3& point) const {
-
-    int numKeys = this->keys_.size();
     std::vector<KeyMatrix2D> Fblocks;
     double f = computeJacobiansSVD(Fblocks, Enull, b, cameras, point);
-    F.resize(ZDim * numKeys, D * numKeys);
+    FillDiagonalF(Fblocks, F);
-    F.setZero();
-
-    for (size_t i = 0; i < this->keys_.size(); ++i)
-      F.block<ZDim, D>(ZDim * i, D * i) = Fblocks.at(i).second; // ZDim x 6 block for the cameras
-
     return f;
   }
 
-  // ****************************************************************************************************
+  /**
-  /// linearize returns a Hessianfactor that is an approximation of error(p)
+   * Linearize returns a Hessianfactor that is an approximation of error(p)
+   */
   boost::shared_ptr<RegularHessianFactor<D> > createHessianFactor(
       const Cameras& cameras, const Point3& point, const double lambda = 0.0,
       bool diagonalDamping = false) const {
@@ -400,10 +471,9 @@ public:
 
     std::vector<KeyMatrix2D> Fblocks;
     Matrix E;
-    Matrix3 PointCov;
     Vector b;
-    double f = computeJacobians(Fblocks, E, PointCov, b, cameras, point, lambda,
+    double f = computeJacobians(Fblocks, E, b, cameras, point);
-        diagonalDamping);
+    Matrix3 P = PointCov(E, lambda, diagonalDamping);
 
 //#define HESSIAN_BLOCKS // slower, as internally the Hessian factor will transform the blocks into SymmetricBlockMatrix
 #ifdef HESSIAN_BLOCKS
@@ -411,14 +481,13 @@ public:
     std::vector < Matrix > Gs(numKeys * (numKeys + 1) / 2);
     std::vector < Vector > gs(numKeys);
 
-    sparseSchurComplement(Fblocks, E, PointCov, b, Gs, gs);
+    sparseSchurComplement(Fblocks, E, P, b, Gs, gs);
-    // schurComplement(Fblocks, E, PointCov, b, Gs, gs);
+    // schurComplement(Fblocks, E, P, b, Gs, gs);
 
     //std::vector < Matrix > Gs2(Gs.begin(), Gs.end());
     //std::vector < Vector > gs2(gs.begin(), gs.end());
 
-    return boost::make_shared < RegularHessianFactor<D>
+    return boost::make_shared < RegularHessianFactor<D> > (this->keys_, Gs, gs, f);
-    > (this->keys_, Gs, gs, f);
 #else // we create directly a SymmetricBlockMatrix
     size_t n1 = D * numKeys + 1;
     std::vector<DenseIndex> dims(numKeys + 1); // this also includes the b term
@@ -426,17 +495,19 @@ public:
     dims.back() = 1;
 
     SymmetricBlockMatrix augmentedHessian(dims, Matrix::Zero(n1, n1)); // for 10 cameras, size should be (10*D+1 x 10*D+1)
-    sparseSchurComplement(Fblocks, E, PointCov, b, augmentedHessian); // augmentedHessian.matrix().block<D,D> (i1,i2) = ...
+    sparseSchurComplement(Fblocks, E, P, b, augmentedHessian); // augmentedHessian.matrix().block<D,D> (i1,i2) = ...
     augmentedHessian(numKeys, numKeys)(0, 0) = f;
     return boost::make_shared<RegularHessianFactor<D> >(this->keys_,
         augmentedHessian);
 #endif
   }
 
-  // ****************************************************************************************************
+  /**
-  // slow version - works on full (sparse) matrices
+   * Do Schur complement, given Jacobian as F,E,P.
+   * Slow version - works on full matrices
+   */
   void schurComplement(const std::vector<KeyMatrix2D>& Fblocks, const Matrix& E,
-      const Matrix& PointCov, const Vector& b,
+      const Matrix3& P, const Vector& b,
       /*output ->*/std::vector<Matrix>& Gs, std::vector<Vector>& gs) const {
     // Schur complement trick
     // Gs = F' * F - F' * E * inv(E'*E) * E' * F
@@ -446,16 +517,14 @@ public:
     int numKeys = this->keys_.size();
 
     /// Compute Full F ????
-    Matrix F = zeros(ZDim * numKeys, D * numKeys);
+    Matrix F;
-    for (size_t i = 0; i < this->keys_.size(); ++i)
+    FillDiagonalF(Fblocks, F);
-      F.block<ZDim, D>(ZDim * i, D * i) = Fblocks.at(i).second; // ZDim x 6 block for the cameras
 
     Matrix H(D * numKeys, D * numKeys);
     Vector gs_vector;
 
-    H.noalias() = F.transpose() * (F - (E * (PointCov * (E.transpose() * F))));
+    H.noalias() = F.transpose() * (F - (E * (P * (E.transpose() * F))));
-    gs_vector.noalias() = F.transpose()
+    gs_vector.noalias() = F.transpose() * (b - (E * (P * (E.transpose() * b))));
-        * (b - (E * (PointCov * (E.transpose() * b))));
 
     // Populate Gs and gs
     int GsCount2 = 0;
@@ -471,9 +540,12 @@ public:
     }
   }
 
-  // ****************************************************************************************************
+  /**
+   * Do Schur complement, given Jacobian as F,E,P, return SymmetricBlockMatrix
+   * Fast version - works on with sparsity
+   */
   void sparseSchurComplement(const std::vector<KeyMatrix2D>& Fblocks,
-      const Matrix& E, const Matrix& P /*Point Covariance*/, const Vector& b,
+      const Matrix& E, const Matrix3& P /*Point Covariance*/, const Vector& b,
       /*output ->*/SymmetricBlockMatrix& augmentedHessian) const {
     // Schur complement trick
     // Gs = F' * F - F' * E * P * E' * F
@@ -490,7 +562,8 @@ public:
 
       // D = (Dx2) * (2)
       // (augmentedHessian.matrix()).block<D,1> (i1,numKeys+1) = Fi1.transpose() * b.segment < 2 > (2 * i1); // F' * b
-      augmentedHessian(i1, numKeys) = Fi1.transpose() * b.segment<ZDim>(ZDim * i1) // F' * b
+      augmentedHessian(i1, numKeys) = Fi1.transpose()
+          * b.segment<ZDim>(ZDim * i1) // F' * b
       - Fi1.transpose() * (Ei1_P * (E.transpose() * b)); // D = (DxZDim) * (ZDimx3) * (3*ZDimm) * (ZDimm x 1)
 
       // (DxD) = (DxZDim) * ( (ZDimxD) - (ZDimx3) * (3xZDim) * (ZDimxD) )
@@ -508,9 +581,12 @@ public:
     } // end of for over cameras
   }
 
-  // ****************************************************************************************************
+  /**
+   * Do Schur complement, given Jacobian as F,E,P, return Gs/gs
+   * Fast version - works on with sparsity
+   */
   void sparseSchurComplement(const std::vector<KeyMatrix2D>& Fblocks,
-      const Matrix& E, const Matrix& P /*Point Covariance*/, const Vector& b,
+      const Matrix& E, const Matrix3& P /*Point Covariance*/, const Vector& b,
       /*output ->*/std::vector<Matrix>& Gs, std::vector<Vector>& gs) const {
     // Schur complement trick
     // Gs = F' * F - F' * E * P * E' * F
@@ -535,7 +611,7 @@ public:
       { // for i1 = i2
         // D = (Dx2) * (2)
         gs.at(i1) = Fi1.transpose() * b.segment<ZDim>(ZDim * i1) // F' * b
-                      -Fi1.transpose() * (Ei1_P * (E.transpose() * b));  // D = (DxZDim) * (ZDimx3) * (3*ZDimm) * (ZDimm x 1)
+        - Fi1.transpose() * (Ei1_P * (E.transpose() * b)); // D = (DxZDim) * (ZDimx3) * (3*ZDimm) * (ZDimm x 1)
 
         // (DxD) = (DxZDim) * ( (ZDimxD) - (ZDimx3) * (3xZDim) * (ZDimxD) )
         Gs.at(GsIndex) = Fi1.transpose()
@@ -554,27 +630,12 @@ public:
     } // end of for over cameras
   }
 
-  // ****************************************************************************************************
+  /**
-  void updateAugmentedHessian(const Cameras& cameras, const Point3& point,
+   * Applies Schur complement (exploiting block structure) to get a smart factor on cameras,
-      const double lambda, bool diagonalDamping,
+   * and adds the contribution of the smart factor to a pre-allocated augmented Hessian.
-      SymmetricBlockMatrix& augmentedHessian,
+   */
-      const FastVector<Key> allKeys) const {
-
-    // int numKeys = this->keys_.size();
-
-    std::vector<KeyMatrix2D> Fblocks;
-    Matrix E;
-    Matrix3 PointCov;
-    Vector b;
-    double f = computeJacobians(Fblocks, E, PointCov, b, cameras, point, lambda,
-        diagonalDamping);
-
-    updateSparseSchurComplement(Fblocks, E, PointCov, b, f, allKeys, augmentedHessian); // augmentedHessian.matrix().block<D,D> (i1,i2) = ...
-  }
-
-  // ****************************************************************************************************
   void updateSparseSchurComplement(const std::vector<KeyMatrix2D>& Fblocks,
-      const Matrix& E, const Matrix& P /*Point Covariance*/, const Vector& b,
+      const Matrix& E, const Matrix3& P /*Point Covariance*/, const Vector& b,
       const double f, const FastVector<Key> allKeys,
       /*output ->*/SymmetricBlockMatrix& augmentedHessian) const {
     // Schur complement trick
@@ -584,9 +645,9 @@ public:
     MatrixDD matrixBlock;
     typedef SymmetricBlockMatrix::Block Block; ///< A block from the Hessian matrix
 
-    FastMap<Key,size_t> KeySlotMap;
+    FastMap<Key, size_t> KeySlotMap;
-    for (size_t slot=0; slot < allKeys.size(); slot++)
+    for (size_t slot = 0; slot < allKeys.size(); slot++)
-      KeySlotMap.insert(std::make_pair(allKeys[slot],slot));
+      KeySlotMap.insert(std::make_pair(allKeys[slot], slot));
 
     // a single point is observed in numKeys cameras
     size_t numKeys = this->keys_.size(); // cameras observing current point
@@ -607,7 +668,8 @@ public:
       // information vector - store previous vector
       // vectorBlock = augmentedHessian(aug_i1, aug_numKeys).knownOffDiagonal();
       // add contribution of current factor
-      augmentedHessian(aug_i1, aug_numKeys) = augmentedHessian(aug_i1, aug_numKeys).knownOffDiagonal()
+      augmentedHessian(aug_i1, aug_numKeys) = augmentedHessian(aug_i1,
+          aug_numKeys).knownOffDiagonal()
           + Fi1.transpose() * b.segment<ZDim>(ZDim * i1) // F' * b
       - Fi1.transpose() * (Ei1_P * (E.transpose() * b)); // D = (DxZDim) * (ZDimx3) * (3*ZDimm) * (ZDimm x 1)
 
@@ -615,8 +677,11 @@ public:
       // main block diagonal - store previous block
       matrixBlock = augmentedHessian(aug_i1, aug_i1);
       // add contribution of current factor
-      augmentedHessian(aug_i1, aug_i1) = matrixBlock +
+      augmentedHessian(aug_i1, aug_i1) =
-          (  Fi1.transpose() * (Fi1 - Ei1_P * E.block<ZDim, 3>(ZDim * i1, 0).transpose() * Fi1)  );
+          matrixBlock
+              + (Fi1.transpose()
+                  * (Fi1
+                      - Ei1_P * E.block<ZDim, 3>(ZDim * i1, 0).transpose() * Fi1));
 
       // upper triangular part of the hessian
       for (size_t i2 = i1 + 1; i2 < numKeys; i2++) { // for each camera
@@ -629,48 +694,76 @@ public:
         // off diagonal block - store previous block
         // matrixBlock = augmentedHessian(aug_i1, aug_i2).knownOffDiagonal();
         // add contribution of current factor
-        augmentedHessian(aug_i1, aug_i2) = augmentedHessian(aug_i1, aug_i2).knownOffDiagonal()
+        augmentedHessian(aug_i1, aug_i2) =
-            - Fi1.transpose() * (Ei1_P * E.block<ZDim, 3>(ZDim * i2, 0).transpose() * Fi2);
+            augmentedHessian(aug_i1, aug_i2).knownOffDiagonal()
+                - Fi1.transpose()
+                    * (Ei1_P * E.block<ZDim, 3>(ZDim * i2, 0).transpose() * Fi2);
       }
     } // end of for over cameras
 
     augmentedHessian(aug_numKeys, aug_numKeys)(0, 0) += f;
   }
 
-  // ****************************************************************************************************
+  /**
+   * Add the contribution of the smart factor to a pre-allocated Hessian,
+   * using sparse linear algebra. More efficient than the creation of the
+   * Hessian without preallocation of the SymmetricBlockMatrix
+   */
+  void updateAugmentedHessian(const Cameras& cameras, const Point3& point,
+      const double lambda, bool diagonalDamping,
+      SymmetricBlockMatrix& augmentedHessian,
+      const FastVector<Key> allKeys) const {
+
+    // int numKeys = this->keys_.size();
+
+    std::vector<KeyMatrix2D> Fblocks;
+    Matrix E;
+    Vector b;
+    double f = computeJacobians(Fblocks, E, b, cameras, point);
+    Matrix3 P = PointCov(E, lambda, diagonalDamping);
+    updateSparseSchurComplement(Fblocks, E, P, b, f, allKeys, augmentedHessian); // augmentedHessian.matrix().block<D,D> (i1,i2) = ...
+  }
+
+  /**
+   * Return Jacobians as RegularImplicitSchurFactor with raw access
+   */
   boost::shared_ptr<RegularImplicitSchurFactor<D> > createRegularImplicitSchurFactor(
       const Cameras& cameras, const Point3& point, double lambda = 0.0,
       bool diagonalDamping = false) const {
     typename boost::shared_ptr<RegularImplicitSchurFactor<D> > f(
         new RegularImplicitSchurFactor<D>());
-    computeJacobians(f->Fblocks(), f->E(), f->PointCovariance(), f->b(),
+    computeJacobians(f->Fblocks(), f->E(), f->b(), cameras, point);
-        cameras, point, lambda, diagonalDamping);
+    f->PointCovariance() = PointCov(f->E(), lambda, diagonalDamping);
     f->initKeys();
     return f;
   }
 
-  // ****************************************************************************************************
+  /**
+   * Return Jacobians as JacobianFactorQ
+   */
   boost::shared_ptr<JacobianFactorQ<D, ZDim> > createJacobianQFactor(
       const Cameras& cameras, const Point3& point, double lambda = 0.0,
       bool diagonalDamping = false) const {
     std::vector<KeyMatrix2D> Fblocks;
     Matrix E;
-    Matrix3 PointCov;
     Vector b;
-    computeJacobians(Fblocks, E, PointCov, b, cameras, point, lambda,
+    computeJacobians(Fblocks, E, b, cameras, point);
-        diagonalDamping);
+    Matrix3 P = PointCov(E, lambda, diagonalDamping);
-    return boost::make_shared<JacobianFactorQ<D, ZDim> >(Fblocks, E, PointCov, b);
+    return boost::make_shared<JacobianFactorQ<D, ZDim> >(Fblocks, E, P, b);
   }
 
-  // ****************************************************************************************************
+  /**
+   * Return Jacobians as JacobianFactor
+   * TODO lambda is currently ignored
+   */
   boost::shared_ptr<JacobianFactor> createJacobianSVDFactor(
       const Cameras& cameras, const Point3& point, double lambda = 0.0) const {
     size_t numKeys = this->keys_.size();
     std::vector<KeyMatrix2D> Fblocks;
     Vector b;
-    Matrix Enull(ZDim*numKeys, ZDim*numKeys-3);
+    Matrix Enull(ZDim * numKeys, ZDim * numKeys - 3);
-    computeJacobiansSVD(Fblocks, Enull, b, cameras, point, lambda);
+    computeJacobiansSVD(Fblocks, Enull, b, cameras, point);
-    return boost::make_shared< JacobianFactorSVD<6, ZDim> >(Fblocks, Enull, b);
+    return boost::make_shared<JacobianFactorSVD<6, ZDim> >(Fblocks, Enull, b);
   }
 
 private:
@@ -683,9 +776,10 @@ private:
     ar & BOOST_SERIALIZATION_NVP(measured_);
     ar & BOOST_SERIALIZATION_NVP(body_P_sensor_);
   }
-};
+}
+;
 
-template<class POSE, class Z, class CAMERA, size_t D>
+template<class CAMERA, size_t D>
-const int SmartFactorBase<POSE, Z, CAMERA, D>::ZDim;
+const int SmartFactorBase<CAMERA, D>::ZDim;
 
 } // \ namespace gtsam

gtsam/slam/SmartProjectionFactor.h

@@ -58,11 +58,11 @@ enum LinearizationMode {
 };
 
 /**
- * SmartProjectionFactor: triangulates point
+ * SmartProjectionFactor: triangulates point and keeps an estimate of it around.
- * TODO: why LANDMARK parameter?
  */
-template<class POSE, class CALIBRATION, size_t D>
+template<class CALIBRATION, size_t D>
-class SmartProjectionFactor: public SmartFactorBase<POSE, gtsam::Point2, gtsam::PinholeCamera<CALIBRATION>, D> {
+class SmartProjectionFactor: public SmartFactorBase<PinholeCamera<CALIBRATION>,
+    D> {
 protected:
 
   // Some triangulation parameters
@@ -92,7 +92,7 @@ protected:
   typedef boost::shared_ptr<SmartProjectionFactorState> SmartFactorStatePtr;
 
   /// shorthand for base class type
-  typedef SmartFactorBase<POSE, gtsam::Point2, gtsam::PinholeCamera<CALIBRATION>, D> Base;
+  typedef SmartFactorBase<PinholeCamera<CALIBRATION>, D> Base;
 
   double landmarkDistanceThreshold_; // if the landmark is triangulated at a
   // distance larger than that the factor is considered degenerate
@@ -102,9 +102,7 @@ protected:
   // and the factor is disregarded if the error is large
 
   /// shorthand for this class
-  typedef SmartProjectionFactor<POSE, CALIBRATION, D> This;
+  typedef SmartProjectionFactor<CALIBRATION, D> This;
-
-  static const int ZDim = 2;    ///< Measurement dimension
 
 public:
 
@@ -113,7 +111,7 @@ public:
 
   /// shorthand for a pinhole camera
   typedef PinholeCamera<CALIBRATION> Camera;
-  typedef std::vector<Camera> Cameras;
+  typedef CameraSet<Camera> Cameras;
 
   /**
    * Constructor
@@ -126,16 +124,16 @@ public:
    */
   SmartProjectionFactor(const double rankTol, const double linThreshold,
       const bool manageDegeneracy, const bool enableEPI,
-      boost::optional<POSE> body_P_sensor = boost::none,
+      boost::optional<Pose3> body_P_sensor = boost::none,
       double landmarkDistanceThreshold = 1e10,
-      double dynamicOutlierRejectionThreshold = -1,
+      double dynamicOutlierRejectionThreshold = -1, SmartFactorStatePtr state =
-      SmartFactorStatePtr state = SmartFactorStatePtr(new SmartProjectionFactorState())) :
+          SmartFactorStatePtr(new SmartProjectionFactorState())) :
       Base(body_P_sensor), rankTolerance_(rankTol), retriangulationThreshold_(
           1e-5), manageDegeneracy_(manageDegeneracy), enableEPI_(enableEPI), linearizationThreshold_(
           linThreshold), degenerate_(false), cheiralityException_(false), throwCheirality_(
-          false), verboseCheirality_(false), state_(state),
+          false), verboseCheirality_(false), state_(state), landmarkDistanceThreshold_(
-          landmarkDistanceThreshold_(landmarkDistanceThreshold),
+          landmarkDistanceThreshold), dynamicOutlierRejectionThreshold_(
-          dynamicOutlierRejectionThreshold_(dynamicOutlierRejectionThreshold) {
+          dynamicOutlierRejectionThreshold) {
   }
 
   /** Virtual destructor */
@@ -252,11 +250,11 @@ public:
 
         // Check landmark distance and reprojection errors to avoid outliers
         double totalReprojError = 0.0;
-        size_t i=0;
+        size_t i = 0;
         BOOST_FOREACH(const Camera& camera, cameras) {
           Point3 cameraTranslation = camera.pose().translation();
           // we discard smart factors corresponding to points that are far away
-          if(cameraTranslation.distance(point_) > landmarkDistanceThreshold_){
+          if (cameraTranslation.distance(point_) > landmarkDistanceThreshold_) {
             degenerate_ = true;
             break;
           }
@@ -270,8 +268,8 @@ public:
           i += 1;
         }
         // we discard smart factors that have large reprojection error
-        if(dynamicOutlierRejectionThreshold_ > 0 &&
+        if (dynamicOutlierRejectionThreshold_ > 0
-            totalReprojError/m > dynamicOutlierRejectionThreshold_)
+            && totalReprojError / m > dynamicOutlierRejectionThreshold_)
           degenerate_ = true;
 
       } catch (TriangulationUnderconstrainedException&) {
@@ -300,7 +298,8 @@ public:
         || (!this->manageDegeneracy_
             && (this->cheiralityException_ || this->degenerate_))) {
       if (isDebug) {
-        std::cout << "createRegularImplicitSchurFactor: degenerate configuration"
+        std::cout
+            << "createRegularImplicitSchurFactor: degenerate configuration"
             << std::endl;
       }
       return false;
@@ -321,9 +320,9 @@ public:
     bool isDebug = false;
     size_t numKeys = this->keys_.size();
     // Create structures for Hessian Factors
-    std::vector < Key > js;
+    std::vector<Key> js;
-    std::vector < Matrix > Gs(numKeys * (numKeys + 1) / 2);
+    std::vector<Matrix> Gs(numKeys * (numKeys + 1) / 2);
-    std::vector < Vector > gs(numKeys);
+    std::vector<Vector> gs(numKeys);
 
     if (this->measured_.size() != cameras.size()) {
       std::cout
@@ -338,7 +337,7 @@ public:
         || (!this->manageDegeneracy_
             && (this->cheiralityException_ || this->degenerate_))) {
       // std::cout << "In linearize: exception" << std::endl;
-      BOOST_FOREACH(gtsam::Matrix& m, Gs)
+      BOOST_FOREACH(Matrix& m, Gs)
         m = zeros(D, D);
       BOOST_FOREACH(Vector& v, gs)
         v = zero(D);
@@ -373,9 +372,8 @@ public:
 
     // ==================================================================
     Matrix F, E;
-    Matrix3 PointCov;
     Vector b;
-    double f = computeJacobians(F, E, PointCov, b, cameras, lambda);
+    double f = computeJacobians(F, E, b, cameras);
 
     // Schur complement trick
     // Frank says: should be possible to do this more efficiently?
@@ -383,20 +381,20 @@ public:
     Matrix H(D * numKeys, D * numKeys);
     Vector gs_vector;
 
-    H.noalias() = F.transpose() * (F - (E * (PointCov * (E.transpose() * F))));
+    Matrix3 P = Base::PointCov(E, lambda);
-    gs_vector.noalias() = F.transpose()
+    H.noalias() = F.transpose() * (F - (E * (P * (E.transpose() * F))));
-        * (b - (E * (PointCov * (E.transpose() * b))));
+    gs_vector.noalias() = F.transpose() * (b - (E * (P * (E.transpose() * b))));
     if (isDebug)
       std::cout << "gs_vector size " << gs_vector.size() << std::endl;
 
     // Populate Gs and gs
     int GsCount2 = 0;
-    for (DenseIndex i1 = 0; i1 < (DenseIndex)numKeys; i1++) { // for each camera
+    for (DenseIndex i1 = 0; i1 < (DenseIndex) numKeys; i1++) { // for each camera
       DenseIndex i1D = i1 * D;
-      gs.at(i1) = gs_vector.segment < D > (i1D);
+      gs.at(i1) = gs_vector.segment<D>(i1D);
-      for (DenseIndex i2 = 0; i2 < (DenseIndex)numKeys; i2++) {
+      for (DenseIndex i2 = 0; i2 < (DenseIndex) numKeys; i2++) {
         if (i2 >= i1) {
-          Gs.at(GsCount2) = H.block < D, D > (i1D, i2 * D);
+          Gs.at(GsCount2) = H.block<D, D>(i1D, i2 * D);
           GsCount2++;
         }
       }
@@ -420,16 +418,16 @@ public:
   }
 
   /// create factor
-  boost::shared_ptr<JacobianFactorQ<D, ZDim> > createJacobianQFactor(
+  boost::shared_ptr<JacobianFactorQ<D, 2> > createJacobianQFactor(
       const Cameras& cameras, double lambda) const {
     if (triangulateForLinearize(cameras))
       return Base::createJacobianQFactor(cameras, point_, lambda);
     else
-      return boost::make_shared< JacobianFactorQ<D, ZDim> >(this->keys_);
+      return boost::make_shared<JacobianFactorQ<D, 2> >(this->keys_);
   }
 
   /// Create a factor, takes values
-  boost::shared_ptr<JacobianFactorQ<D, ZDim> > createJacobianQFactor(
+  boost::shared_ptr<JacobianFactorQ<D, 2> > createJacobianQFactor(
       const Values& values, double lambda) const {
     Cameras myCameras;
     // TODO triangulate twice ??
@@ -437,16 +435,16 @@ public:
     if (nonDegenerate)
       return createJacobianQFactor(myCameras, lambda);
     else
-      return boost::make_shared< JacobianFactorQ<D, ZDim> >(this->keys_);
+      return boost::make_shared<JacobianFactorQ<D, 2> >(this->keys_);
   }
 
   /// different (faster) way to compute Jacobian factor
-  boost::shared_ptr< JacobianFactor > createJacobianSVDFactor(const Cameras& cameras,
+  boost::shared_ptr<JacobianFactor> createJacobianSVDFactor(
-      double lambda) const {
+      const Cameras& cameras, double lambda) const {
     if (triangulateForLinearize(cameras))
       return Base::createJacobianSVDFactor(cameras, point_, lambda);
     else
-      return boost::make_shared< JacobianFactorSVD<D, ZDim> >(this->keys_);
+      return boost::make_shared<JacobianFactorSVD<D, 2> >(this->keys_);
   }
 
   /// Returns true if nonDegenerate
@@ -479,27 +477,27 @@ public:
     return true;
   }
 
+  /// Assumes non-degenerate !
+  void computeEP(Matrix& E, Matrix& P, const Cameras& cameras) const {
+    return Base::computeEP(E, P, cameras, point_);
+  }
+
   /// Takes values
-  bool computeEP(Matrix& E, Matrix& PointCov, const Values& values) const {
+  bool computeEP(Matrix& E, Matrix& P, const Values& values) const {
     Cameras myCameras;
     bool nonDegenerate = computeCamerasAndTriangulate(values, myCameras);
     if (nonDegenerate)
-      computeEP(E, PointCov, myCameras);
+      computeEP(E, P, myCameras);
     return nonDegenerate;
   }
 
-  /// Assumes non-degenerate !
-  void computeEP(Matrix& E, Matrix& PointCov, const Cameras& cameras) const {
-    return Base::computeEP(E, PointCov, cameras, point_);
-  }
-
   /// Version that takes values, and creates the point
   bool computeJacobians(std::vector<typename Base::KeyMatrix2D>& Fblocks,
-      Matrix& E, Matrix& PointCov, Vector& b, const Values& values) const {
+      Matrix& E, Vector& b, const Values& values) const {
     Cameras myCameras;
     bool nonDegenerate = computeCamerasAndTriangulate(values, myCameras);
     if (nonDegenerate)
-      computeJacobians(Fblocks, E, PointCov, b, myCameras, 0.0);
+      computeJacobians(Fblocks, E, b, myCameras);
     return nonDegenerate;
   }
 
@@ -538,7 +536,7 @@ public:
         this->noise_.at(i)->WhitenSystem(Fi, Ei, bi);
         f += bi.squaredNorm();
         Fblocks.push_back(typename Base::KeyMatrix2D(this->keys_[i], Fi));
-        E.block < 2, 2 > (2 * i, 0) = Ei;
+        E.block<2, 2>(2 * i, 0) = Ei;
         subInsert(b, bi, 2 * i);
       }
       return f;
@@ -548,19 +546,6 @@ public:
     } // end else
   }
 
-  /// Version that computes PointCov, with optional lambda parameter
-  double computeJacobians(std::vector<typename Base::KeyMatrix2D>& Fblocks,
-      Matrix& E, Matrix& PointCov, Vector& b, const Cameras& cameras,
-      const double lambda = 0.0) const {
-
-    double f = computeJacobians(Fblocks, E, b, cameras);
-
-    // Point covariance inv(E'*E)
-    PointCov.noalias() = (E.transpose() * E + lambda * eye(E.cols())).inverse();
-
-    return f;
-  }
-
   /// takes values
   bool computeJacobiansSVD(std::vector<typename Base::KeyMatrix2D>& Fblocks,
       Matrix& Enull, Vector& b, const Values& values) const {
@@ -585,9 +570,9 @@ public:
   }
 
   /// Returns Matrix, TODO: maybe should not exist -> not sparse !
-  double computeJacobians(Matrix& F, Matrix& E, Matrix3& PointCov, Vector& b,
+  double computeJacobians(Matrix& F, Matrix& E, Vector& b,
-      const Cameras& cameras, const double lambda) const {
+      const Cameras& cameras) const {
-    return Base::computeJacobians(F, E, PointCov, b, cameras, point_, lambda);
+    return Base::computeJacobians(F, E, b, cameras, point_);
   }
 
   /// Calculate vector of re-projection errors, before applying noise model
@@ -709,7 +694,4 @@ private:
   }
 };
 
-template<class POSE, class CALIBRATION, size_t D>
-const int SmartProjectionFactor<POSE, CALIBRATION, D>::ZDim;
-
 } // \ namespace gtsam

gtsam/slam/SmartProjectionPoseFactor.h

@@ -37,8 +37,8 @@ namespace gtsam {
  * The calibration is known here. The factor only constraints poses (variable dimension is 6)
  * @addtogroup SLAM
  */
-template<class POSE, class CALIBRATION>
+template<class CALIBRATION>
-class SmartProjectionPoseFactor: public SmartProjectionFactor<POSE, CALIBRATION, 6> {
+class SmartProjectionPoseFactor: public SmartProjectionFactor<CALIBRATION, 6> {
 protected:
 
   LinearizationMode linearizeTo_;  ///< How to linearize the factor (HESSIAN, JACOBIAN_SVD, JACOBIAN_Q)
@@ -48,10 +48,10 @@ protected:
 public:
 
   /// shorthand for base class type
-  typedef SmartProjectionFactor<POSE, CALIBRATION, 6> Base;
+  typedef SmartProjectionFactor<CALIBRATION, 6> Base;
 
   /// shorthand for this class
-  typedef SmartProjectionPoseFactor<POSE, CALIBRATION> This;
+  typedef SmartProjectionPoseFactor<CALIBRATION> This;
 
   /// shorthand for a smart pointer to a factor
   typedef boost::shared_ptr<This> shared_ptr;
@@ -67,7 +67,7 @@ public:
    */
   SmartProjectionPoseFactor(const double rankTol = 1,
       const double linThreshold = -1, const bool manageDegeneracy = false,
-      const bool enableEPI = false, boost::optional<POSE> body_P_sensor = boost::none,
+      const bool enableEPI = false, boost::optional<Pose3> body_P_sensor = boost::none,
       LinearizationMode linearizeTo = HESSIAN, double landmarkDistanceThreshold = 1e10,
       double dynamicOutlierRejectionThreshold = -1) :
         Base(rankTol, linThreshold, manageDegeneracy, enableEPI, body_P_sensor,
@@ -141,11 +141,6 @@ public:
     return e && Base::equals(p, tol);
   }
 
-  /// get the dimension of the factor
-  virtual size_t dim() const {
-    return 6 * this->keys_.size();
-  }
-
   /**
    * Collect all cameras involved in this factor
    * @param values Values structure which must contain camera poses corresponding
@@ -216,7 +211,9 @@ private:
 }; // end of class declaration
 
 /// traits
-template<class T1, class T2>
+template<class CALIBRATION>
-struct traits<SmartProjectionPoseFactor<T1, T2> > : public Testable<SmartProjectionPoseFactor<T1, T2> > {};
+struct traits<SmartProjectionPoseFactor<CALIBRATION> > : public Testable<
+    SmartProjectionPoseFactor<CALIBRATION> > {
+};
 
 } // \ namespace gtsam

gtsam/slam/tests/testRegularHessianFactor.cpp

@@ -15,11 +15,10 @@
  * @date    March 4, 2014
  */
 
-#include <gtsam/linear/VectorValues.h>
-#include <CppUnitLite/TestHarness.h>
-
-//#include <gtsam_unstable/slam/RegularHessianFactor.h>
 #include <gtsam/slam/RegularHessianFactor.h>
+#include <gtsam/linear/VectorValues.h>
+
+#include <CppUnitLite/TestHarness.h>
 
 #include <boost/assign/std/vector.hpp>
 #include <boost/assign/std/map.hpp>
@@ -29,8 +28,6 @@ using namespace std;
 using namespace gtsam;
 using namespace boost::assign;
 
-const double tol = 1e-5;
-
 /* ************************************************************************* */
 TEST(RegularHessianFactor, ConstructorNWay)
 {
@@ -77,15 +74,24 @@ TEST(RegularHessianFactor, ConstructorNWay)
   expected.insert(1, Y.segment<2>(2));
   expected.insert(3, Y.segment<2>(4));
 
+  // VectorValues version
+  double alpha = 1.0;
+  VectorValues actualVV;
+  actualVV.insert(0, zero(2));
+  actualVV.insert(1, zero(2));
+  actualVV.insert(3, zero(2));
+  factor.multiplyHessianAdd(alpha, x, actualVV);
+  EXPECT(assert_equal(expected, actualVV));
+
   // RAW ACCESS
   Vector expected_y(8); expected_y << 2633, 2674, 4465, 4501, 0, 0, 5669, 5696;
   Vector fast_y = gtsam::zero(8);
   double xvalues[8] = {1,2,3,4,0,0,5,6};
-  factor.multiplyHessianAdd(1, xvalues, fast_y.data());
+  factor.multiplyHessianAdd(alpha, xvalues, fast_y.data());
   EXPECT(assert_equal(expected_y, fast_y));
 
   // now, do it with non-zero y
-  factor.multiplyHessianAdd(1, xvalues, fast_y.data());
+  factor.multiplyHessianAdd(alpha, xvalues, fast_y.data());
   EXPECT(assert_equal(2*expected_y, fast_y));
 
   // check some expressions

gtsam/slam/tests/testRegularImplicitSchurFactor.cpp

@@ -1,16 +1,24 @@
+/* ----------------------------------------------------------------------------
+
+ * 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    testImplicitSchurFactor.cpp
+ * @file    testRegularImplicitSchurFactor.cpp
  * @brief   unit test implicit jacobian factors
  * @author  Frank Dellaert
  * @date    Oct 20, 2013
  */
 
-//#include <gtsam_unstable/slam/ImplicitSchurFactor.h>
-#include <gtsam/slam/RegularImplicitSchurFactor.h>
-//#include <gtsam_unstable/slam/JacobianFactorQ.h>
 #include <gtsam/slam/JacobianFactorQ.h>
-//#include "gtsam_unstable/slam/JacobianFactorQR.h"
+#include <gtsam/slam/JacobianFactorQR.h>
-#include "gtsam/slam/JacobianFactorQR.h"
+#include <gtsam/slam/RegularImplicitSchurFactor.h>
 
 #include <gtsam/base/timing.h>
 #include <gtsam/linear/VectorValues.h>

gtsam/slam/tests/testRegularJacobianFactor.cpp

@@ -1,3 +1,14 @@
+/* ----------------------------------------------------------------------------
+
+ * 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    testRegularJacobianFactor.cpp
  * @brief   unit test regular jacobian factors
@@ -5,13 +16,13 @@
  * @date    Nov 12, 2014
  */
 
-#include <gtsam/base/TestableAssertions.h>
-#include <CppUnitLite/TestHarness.h>
-
 #include <gtsam/slam/RegularJacobianFactor.h>
 #include <gtsam/linear/GaussianFactorGraph.h>
 #include <gtsam/linear/GaussianConditional.h>
 #include <gtsam/linear/VectorValues.h>
+#include <gtsam/base/TestableAssertions.h>
+
+#include <CppUnitLite/TestHarness.h>
 
 #include <boost/assign/std/vector.hpp>
 #include <boost/assign/list_of.hpp>

gtsam/slam/tests/testSmartFactorBase.cpp

@@ -0,0 +1,71 @@
+/* ----------------------------------------------------------------------------
+
+ * 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   testSmartFactorBase.cpp
+ *  @brief  Unit tests for testSmartFactorBase Class
+ *  @author Frank Dellaert
+ *  @date   Feb 2015
+ */
+
+#include "../SmartFactorBase.h"
+#include <gtsam/geometry/Pose3.h>
+#include <CppUnitLite/TestHarness.h>
+
+using namespace std;
+using namespace gtsam;
+
+/* ************************************************************************* */
+#include <gtsam/geometry/PinholeCamera.h>
+#include <gtsam/geometry/Cal3Bundler.h>
+class PinholeFactor: public SmartFactorBase<PinholeCamera<Cal3Bundler>, 9> {
+  virtual double error(const Values& values) const {
+    return 0.0;
+  }
+  virtual boost::shared_ptr<GaussianFactor> linearize(
+      const Values& values) const {
+    return boost::shared_ptr<GaussianFactor>(new JacobianFactor());
+  }
+};
+
+TEST(SmartFactorBase, Pinhole) {
+  PinholeFactor f;
+  f.add(Point2(), 1, SharedNoiseModel());
+  f.add(Point2(), 2, SharedNoiseModel());
+  EXPECT_LONGS_EQUAL(2 * 2, f.dim());
+}
+
+/* ************************************************************************* */
+#include <gtsam/geometry/StereoCamera.h>
+class StereoFactor: public SmartFactorBase<StereoCamera, 9> {
+  virtual double error(const Values& values) const {
+    return 0.0;
+  }
+  virtual boost::shared_ptr<GaussianFactor> linearize(
+      const Values& values) const {
+    return boost::shared_ptr<GaussianFactor>(new JacobianFactor());
+  }
+};
+
+TEST(SmartFactorBase, Stereo) {
+  StereoFactor f;
+  f.add(StereoPoint2(), 1, SharedNoiseModel());
+  f.add(StereoPoint2(), 2, SharedNoiseModel());
+  EXPECT_LONGS_EQUAL(2 * 3, f.dim());
+}
+
+/* ************************************************************************* */
+int main() {
+  TestResult tr;
+  return TestRegistry::runAllTests(tr);
+}
+/* ************************************************************************* */
+

gtsam_unstable/examples/SmartProjectionFactorExample.cpp

@@ -46,7 +46,7 @@ using namespace gtsam;
 
 int main(int argc, char** argv){
 
-  typedef SmartProjectionPoseFactor<Pose3, Cal3_S2> SmartFactor;
+  typedef SmartProjectionPoseFactor<Cal3_S2> SmartFactor;
 
   Values initial_estimate;
   NonlinearFactorGraph graph;

gtsam_unstable/examples/SmartStereoProjectionFactorExample.cpp

@@ -46,7 +46,7 @@ using namespace gtsam;
 
 int main(int argc, char** argv){
 
-  typedef SmartStereoProjectionPoseFactor<Pose3, Point3, Cal3_S2Stereo> SmartFactor;
+  typedef SmartStereoProjectionPoseFactor<Cal3_S2Stereo> SmartFactor;
 
   bool output_poses = true;
   string poseOutput("../../../examples/data/optimized_poses.txt");

gtsam_unstable/slam/SmartStereoProjectionFactor.h

@@ -62,10 +62,9 @@ enum LinearizationMode {
 
 /**
  * SmartStereoProjectionFactor: triangulates point
- * TODO: why LANDMARK parameter?
  */
-template<class POSE, class LANDMARK, class CALIBRATION, size_t D>
+template<class CALIBRATION, size_t D>
-class SmartStereoProjectionFactor: public SmartFactorBase<POSE, gtsam::StereoPoint2, gtsam::StereoCamera, D> {
+class SmartStereoProjectionFactor: public SmartFactorBase<StereoCamera, D> {
 protected:
 
   // Some triangulation parameters
@@ -95,7 +94,7 @@ protected:
   typedef boost::shared_ptr<SmartStereoProjectionFactorState> SmartFactorStatePtr;
 
   /// shorthand for base class type
-  typedef SmartFactorBase<POSE, gtsam::StereoPoint2, gtsam::StereoCamera, D> Base;
+  typedef SmartFactorBase<StereoCamera, D> Base;
 
   double landmarkDistanceThreshold_; // if the landmark is triangulated at a
   // distance larger than that the factor is considered degenerate
@@ -105,7 +104,7 @@ protected:
   // and the factor is disregarded if the error is large
 
   /// shorthand for this class
-  typedef SmartStereoProjectionFactor<POSE, LANDMARK, CALIBRATION, D> This;
+  typedef SmartStereoProjectionFactor<CALIBRATION, D> This;
 
   enum {ZDim = 3};    ///< Dimension trait of measurement type
 
@@ -118,7 +117,7 @@ public:
   typedef StereoCamera Camera;
 
   /// Vector of cameras
-  typedef std::vector<Camera> Cameras;
+  typedef CameraSet<Camera> Cameras;
 
   /**
    * Constructor
@@ -131,7 +130,7 @@ public:
    */
   SmartStereoProjectionFactor(const double rankTol, const double linThreshold,
       const bool manageDegeneracy, const bool enableEPI,
-      boost::optional<POSE> body_P_sensor = boost::none,
+      boost::optional<Pose3> body_P_sensor = boost::none,
       double landmarkDistanceThreshold = 1e10,
       double dynamicOutlierRejectionThreshold = -1,
       SmartFactorStatePtr state = SmartFactorStatePtr(new SmartStereoProjectionFactorState())) :
@@ -370,7 +369,7 @@ public:
         || (!this->manageDegeneracy_
             && (this->cheiralityException_ || this->degenerate_))) {
       if (isDebug) std::cout << "In linearize: exception" << std::endl;
-      BOOST_FOREACH(gtsam::Matrix& m, Gs)
+      BOOST_FOREACH(Matrix& m, Gs)
         m = zeros(D, D);
       BOOST_FOREACH(Vector& v, gs)
         v = zero(D);
@@ -408,9 +407,8 @@ public:
 
     // ==================================================================
     Matrix F, E;
-    Matrix3 PointCov;
     Vector b;
-    double f = computeJacobians(F, E, PointCov, b, cameras, lambda);
+    double f = computeJacobians(F, E, b, cameras);
 
     // Schur complement trick
     // Frank says: should be possible to do this more efficiently?
@@ -418,9 +416,9 @@ public:
     Matrix H(D * numKeys, D * numKeys);
     Vector gs_vector;
 
-    H.noalias() = F.transpose() * (F - (E * (PointCov * (E.transpose() * F))));
+    Matrix3 P = Base::PointCov(E,lambda);
-    gs_vector.noalias() = F.transpose()
+    H.noalias() = F.transpose() * (F - (E * (P * (E.transpose() * F))));
-        * (b - (E * (PointCov * (E.transpose() * b))));
+    gs_vector.noalias() = F.transpose() * (b - (E * (P * (E.transpose() * b))));
 
     if (isDebug) std::cout << "gs_vector size " << gs_vector.size() << std::endl;
     if (isDebug) std::cout << "H:\n" << H << std::endl;
@@ -515,6 +513,11 @@ public:
     return true;
   }
 
+  /// Assumes non-degenerate !
+  void computeEP(Matrix& E, Matrix& PointCov, const Cameras& cameras) const {
+    Base::computeEP(E, PointCov, cameras, point_);
+  }
+
   /// Takes values
   bool computeEP(Matrix& E, Matrix& PointCov, const Values& values) const {
     Cameras myCameras;
@@ -524,18 +527,13 @@ public:
     return nonDegenerate;
   }
 
-  /// Assumes non-degenerate !
-  void computeEP(Matrix& E, Matrix& PointCov, const Cameras& cameras) const {
-    return Base::computeEP(E, PointCov, cameras, point_);
-  }
-
   /// Version that takes values, and creates the point
   bool computeJacobians(std::vector<typename Base::KeyMatrix2D>& Fblocks,
-      Matrix& E, Matrix& PointCov, Vector& b, const Values& values) const {
+      Matrix& E, Vector& b, const Values& values) const {
     Cameras myCameras;
     bool nonDegenerate = computeCamerasAndTriangulate(values, myCameras);
     if (nonDegenerate)
-      computeJacobians(Fblocks, E, PointCov, b, myCameras, 0.0);
+      computeJacobians(Fblocks, E, b, myCameras, 0.0);
     return nonDegenerate;
   }
 
@@ -621,9 +619,9 @@ public:
 //  }
 
   /// Returns Matrix, TODO: maybe should not exist -> not sparse !
-  double computeJacobians(Matrix& F, Matrix& E, Matrix3& PointCov, Vector& b,
+  double computeJacobians(Matrix& F, Matrix& E, Vector& b,
-      const Cameras& cameras, const double lambda) const {
+      const Cameras& cameras) const {
-    return Base::computeJacobians(F, E, PointCov, b, cameras, point_, lambda);
+    return Base::computeJacobians(F, E, b, cameras, point_);
   }
 
   /// Calculate vector of re-projection errors, before applying noise model
@@ -746,9 +744,9 @@ private:
 };
 
 /// traits
-template<class POSE, class LANDMARK, class CALIBRATION, size_t D>
+template<class CALIBRATION, size_t D>
-struct traits<SmartStereoProjectionFactor<POSE, LANDMARK, CALIBRATION, D> > :
+struct traits<SmartStereoProjectionFactor<CALIBRATION, D> > :
-    public Testable<SmartStereoProjectionFactor<POSE, LANDMARK, CALIBRATION, D> > {
+    public Testable<SmartStereoProjectionFactor<CALIBRATION, D> > {
 };
 
 } // \ namespace gtsam

gtsam_unstable/slam/SmartStereoProjectionPoseFactor.h

@@ -37,8 +37,8 @@ namespace gtsam {
  * The calibration is known here. The factor only constraints poses (variable dimension is 6)
  * @addtogroup SLAM
  */
-template<class POSE, class LANDMARK, class CALIBRATION>
+template<class CALIBRATION>
-class SmartStereoProjectionPoseFactor: public SmartStereoProjectionFactor<POSE, LANDMARK, CALIBRATION, 6> {
+class SmartStereoProjectionPoseFactor: public SmartStereoProjectionFactor<CALIBRATION, 6> {
 protected:
 
   LinearizationMode linearizeTo_;  ///< How to linearize the factor (HESSIAN, JACOBIAN_SVD, JACOBIAN_Q)
@@ -50,10 +50,10 @@ public:
   EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 
   /// shorthand for base class type
-  typedef SmartStereoProjectionFactor<POSE, LANDMARK, CALIBRATION, 6> Base;
+  typedef SmartStereoProjectionFactor<CALIBRATION, 6> Base;
 
   /// shorthand for this class
-  typedef SmartStereoProjectionPoseFactor<POSE, LANDMARK, CALIBRATION> This;
+  typedef SmartStereoProjectionPoseFactor<CALIBRATION> This;
 
   /// shorthand for a smart pointer to a factor
   typedef boost::shared_ptr<This> shared_ptr;
@@ -69,7 +69,7 @@ public:
    */
   SmartStereoProjectionPoseFactor(const double rankTol = 1,
       const double linThreshold = -1, const bool manageDegeneracy = false,
-      const bool enableEPI = false, boost::optional<POSE> body_P_sensor = boost::none,
+      const bool enableEPI = false, boost::optional<Pose3> body_P_sensor = boost::none,
       LinearizationMode linearizeTo = HESSIAN, double landmarkDistanceThreshold = 1e10,
       double dynamicOutlierRejectionThreshold = -1) :
         Base(rankTol, linThreshold, manageDegeneracy, enableEPI, body_P_sensor,
@@ -143,11 +143,6 @@ public:
     return e && Base::equals(p, tol);
   }
 
-  /// get the dimension of the factor
-  virtual size_t dim() const {
-    return 6 * this->keys_.size();
-  }
-
   /**
    * Collect all cameras involved in this factor
    * @param values Values structure which must contain camera poses corresponding
@@ -216,9 +211,9 @@ private:
 }; // end of class declaration
 
 /// traits
-template<class POSE, class LANDMARK, class CALIBRATION>
+template<class CALIBRATION>
-struct traits<SmartStereoProjectionPoseFactor<POSE, LANDMARK, CALIBRATION> > :
+struct traits<SmartStereoProjectionPoseFactor<CALIBRATION> > : public Testable<
-    public Testable<SmartStereoProjectionPoseFactor<POSE, LANDMARK, CALIBRATION> > {
+    SmartStereoProjectionPoseFactor<CALIBRATION> > {
 };
 
 } // \ namespace gtsam

gtsam_unstable/slam/tests/testSmartStereoProjectionPoseFactor.cpp

@@ -32,22 +32,24 @@ using namespace std;
 using namespace boost::assign;
 using namespace gtsam;
 
-static bool isDebugTest = true;
+static bool isDebugTest = false;
 
 // make a realistic calibration matrix
 static double fov = 60; // degrees
-static size_t w=640,h=480;
+static size_t w = 640, h = 480;
 static double b = 1;
 
-static Cal3_S2Stereo::shared_ptr K(new Cal3_S2Stereo(fov,w,h,b));
+static Cal3_S2Stereo::shared_ptr K(new Cal3_S2Stereo(fov, w, h, b));
-static Cal3_S2Stereo::shared_ptr K2(new Cal3_S2Stereo(1500, 1200, 0, 640, 480,b));
+static Cal3_S2Stereo::shared_ptr K2(
-static boost::shared_ptr<Cal3Bundler> Kbundler(new Cal3Bundler(500, 1e-3, 1e-3, 1000, 2000));
+    new Cal3_S2Stereo(1500, 1200, 0, 640, 480, b));
+static boost::shared_ptr<Cal3Bundler> Kbundler(
+    new Cal3Bundler(500, 1e-3, 1e-3, 1000, 2000));
 
 static double rankTol = 1.0;
 static double linThreshold = -1.0;
 // static bool manageDegeneracy = true;
 // Create a noise model for the pixel error
-static SharedNoiseModel model(noiseModel::Unit::Create(3));
+static SharedNoiseModel model(noiseModel::Isotropic::Sigma(3, 0.1));
 
 // Convenience for named keys
 using symbol_shorthand::X;
@@ -60,14 +62,14 @@ static Symbol x3('X',  3);
 
 static Key poseKey1(x1);
 static StereoPoint2 measurement1(323.0, 300.0, 240.0); //potentially use more reasonable measurement value?
-static Pose3 body_P_sensor1(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2), Point3(0.25, -0.10, 1.0));
+static Pose3 body_P_sensor1(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2),
+    Point3(0.25, -0.10, 1.0));
 
-typedef SmartStereoProjectionPoseFactor<Pose3,Point3,Cal3_S2Stereo> SmartFactor;
+typedef SmartStereoProjectionPoseFactor<Cal3_S2Stereo> SmartFactor;
-typedef SmartStereoProjectionPoseFactor<Pose3,Point3,Cal3Bundler> SmartFactorBundler;
+typedef SmartStereoProjectionPoseFactor<Cal3Bundler> SmartFactorBundler;
 
-vector<StereoPoint2> stereo_projectToMultipleCameras(
+vector<StereoPoint2> stereo_projectToMultipleCameras(const StereoCamera& cam1,
-    const StereoCamera& cam1, const StereoCamera& cam2,
+    const StereoCamera& cam2, const StereoCamera& cam3, Point3 landmark) {
-    const StereoCamera& cam3, Point3 landmark){
 
   vector<StereoPoint2> measurements_cam;
 
@@ -83,7 +85,7 @@ vector<StereoPoint2> stereo_projectToMultipleCameras(
 
 /* ************************************************************************* */
 TEST( SmartStereoProjectionPoseFactor, Constructor) {
-  fprintf(stderr,"Test 1 Complete");
+  fprintf(stderr, "Test 1 Complete");
   SmartFactor::shared_ptr factor1(new SmartFactor());
 }
 
@@ -108,7 +110,8 @@ TEST( SmartStereoProjectionPoseFactor, Constructor4) {
 TEST( SmartStereoProjectionPoseFactor, ConstructorWithTransform) {
   bool manageDegeneracy = true;
   bool enableEPI = false;
-  SmartFactor factor1(rankTol, linThreshold, manageDegeneracy, enableEPI, body_P_sensor1);
+  SmartFactor factor1(rankTol, linThreshold, manageDegeneracy, enableEPI,
+      body_P_sensor1);
   factor1.add(measurement1, poseKey1, model, K);
 }
 
@@ -124,15 +127,16 @@ TEST( SmartStereoProjectionPoseFactor, Equals ) {
 }
 
 /* *************************************************************************/
-TEST_UNSAFE( SmartStereoProjectionPoseFactor, noiseless ){
+TEST_UNSAFE( SmartStereoProjectionPoseFactor, noiseless ) {
   // cout << " ************************ SmartStereoProjectionPoseFactor: noisy ****************************" << endl;
 
   // create first camera. Looking along X-axis, 1 meter above ground plane (x-y)
-  Pose3 level_pose = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1));
+  Pose3 level_pose = Pose3(Rot3::ypr(-M_PI / 2, 0., -M_PI / 2),
+      Point3(0, 0, 1));
   StereoCamera level_camera(level_pose, K2);
 
   // create second camera 1 meter to the right of first camera
-  Pose3 level_pose_right = level_pose * Pose3(Rot3(), Point3(1,0,0));
+  Pose3 level_pose_right = level_pose * Pose3(Rot3(), Point3(1, 0, 0));
   StereoCamera level_camera_right(level_pose_right, K2);
 
   // landmark ~5 meters infront of camera
@@ -164,75 +168,78 @@ TEST_UNSAFE( SmartStereoProjectionPoseFactor, noiseless ){
 }
 
 /* *************************************************************************/
-TEST( SmartStereoProjectionPoseFactor, noisy ){
+TEST( SmartStereoProjectionPoseFactor, noisy ) {
   // cout << " ************************ SmartStereoProjectionPoseFactor: noisy ****************************" << endl;
 
   // create first camera. Looking along X-axis, 1 meter above ground plane (x-y)
-  Pose3 level_pose = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1));
+  Pose3 level_pose = Pose3(Rot3::ypr(-M_PI / 2, 0., -M_PI / 2),
+      Point3(0, 0, 1));
   StereoCamera level_camera(level_pose, K2);
 
   // create second camera 1 meter to the right of first camera
-  Pose3 level_pose_right = level_pose * Pose3(Rot3(), Point3(1,0,0));
+  Pose3 level_pose_right = level_pose * Pose3(Rot3(), Point3(1, 0, 0));
   StereoCamera level_camera_right(level_pose_right, K2);
 
   // landmark ~5 meters infront of camera
   Point3 landmark(5, 0.5, 1.2);
 
   // 1. Project two landmarks into two cameras and triangulate
-  StereoPoint2 pixelError(0.2,0.2,0);
+  StereoPoint2 pixelError(0.2, 0.2, 0);
   StereoPoint2 level_uv = level_camera.project(landmark) + pixelError;
   StereoPoint2 level_uv_right = level_camera_right.project(landmark);
 
   Values values;
   values.insert(x1, level_pose);
-  Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), gtsam::Point3(0.5,0.1,0.3));
+  Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI / 10, 0., -M_PI / 10),
+      Point3(0.5, 0.1, 0.3));
   values.insert(x2, level_pose_right.compose(noise_pose));
 
   SmartFactor::shared_ptr factor1(new SmartFactor());
   factor1->add(level_uv, x1, model, K);
   factor1->add(level_uv_right, x2, model, K);
 
-  double actualError1= factor1->error(values);
+  double actualError1 = factor1->error(values);
 
   SmartFactor::shared_ptr factor2(new SmartFactor());
   vector<StereoPoint2> measurements;
   measurements.push_back(level_uv);
   measurements.push_back(level_uv_right);
 
-  std::vector< SharedNoiseModel > noises;
+  vector<SharedNoiseModel> noises;
   noises.push_back(model);
   noises.push_back(model);
 
-  std::vector< boost::shared_ptr<Cal3_S2Stereo> > Ks;  ///< shared pointer to calibration object (one for each camera)
+  vector<boost::shared_ptr<Cal3_S2Stereo> > Ks; ///< shared pointer to calibration object (one for each camera)
   Ks.push_back(K);
   Ks.push_back(K);
 
-  std::vector<Key> views;
+  vector<Key> views;
   views.push_back(x1);
   views.push_back(x2);
 
   factor2->add(measurements, views, noises, Ks);
 
-  double actualError2= factor2->error(values);
+  double actualError2 = factor2->error(values);
 
   DOUBLES_EQUAL(actualError1, actualError2, 1e-7);
 }
 
-
 /* *************************************************************************/
-TEST( SmartStereoProjectionPoseFactor, 3poses_smart_projection_factor ){
+TEST( SmartStereoProjectionPoseFactor, 3poses_smart_projection_factor ) {
-   cout << " ************************ SmartStereoProjectionPoseFactor: 3 cams + 3 landmarks **********************" << endl;
+  cout
+      << " ************************ SmartStereoProjectionPoseFactor: 3 cams + 3 landmarks **********************"
+      << endl;
 
   // create first camera. Looking along X-axis, 1 meter above ground plane (x-y)
-  Pose3 pose1 = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1));
+  Pose3 pose1 = Pose3(Rot3::ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 1));
   StereoCamera cam1(pose1, K2);
 
   // create second camera 1 meter to the right of first camera
-  Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1,0,0));
+  Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1, 0, 0));
   StereoCamera cam2(pose2, K2);
 
   // create third camera 1 meter above the first camera
-  Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0,-1,0));
+  Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0, -1, 0));
   StereoCamera cam3(pose3, K2);
 
   // three landmarks ~5 meters infront of camera
@@ -241,11 +248,14 @@ TEST( SmartStereoProjectionPoseFactor, 3poses_smart_projection_factor ){
   Point3 landmark3(3, 0, 3.0);
 
   // 1. Project three landmarks into three cameras and triangulate
-  vector<StereoPoint2> measurements_cam1 = stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark1);
+  vector<StereoPoint2> measurements_cam1 = stereo_projectToMultipleCameras(cam1,
-  vector<StereoPoint2> measurements_cam2 = stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark2);
+      cam2, cam3, landmark1);
-  vector<StereoPoint2> measurements_cam3 = stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark3);
+  vector<StereoPoint2> measurements_cam2 = stereo_projectToMultipleCameras(cam1,
+      cam2, cam3, landmark2);
+  vector<StereoPoint2> measurements_cam3 = stereo_projectToMultipleCameras(cam1,
+      cam2, cam3, landmark3);
 
-  std::vector<Key> views;
+  vector<Key> views;
   views.push_back(x1);
   views.push_back(x2);
   views.push_back(x3);
@@ -268,21 +278,25 @@ TEST( SmartStereoProjectionPoseFactor, 3poses_smart_projection_factor ){
   graph.push_back(PriorFactor<Pose3>(x1, pose1, noisePrior));
   graph.push_back(PriorFactor<Pose3>(x2, pose2, noisePrior));
 
-  //  Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), gtsam::Point3(0.5,0.1,0.3)); // noise from regular projection factor test below
+  //  Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below
-  Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/100, 0., -M_PI/100), gtsam::Point3(0.1,0.1,0.1)); // smaller noise
+  Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI / 100, 0., -M_PI / 100),
+      Point3(0.1, 0.1, 0.1)); // smaller noise
   Values values;
   values.insert(x1, pose1);
   values.insert(x2, pose2);
   // initialize third pose with some noise, we expect it to move back to original pose3
-  values.insert(x3, pose3*noise_pose);
+  values.insert(x3, pose3 * noise_pose);
-  if(isDebugTest) values.at<Pose3>(x3).print("Smart: Pose3 before optimization: ");
+  if (isDebugTest)
+    values.at<Pose3>(x3).print("Smart: Pose3 before optimization: ");
 
   LevenbergMarquardtParams params;
-  if(isDebugTest) params.verbosityLM = LevenbergMarquardtParams::TRYLAMBDA;
+  if (isDebugTest)
-  if(isDebugTest) params.verbosity = NonlinearOptimizerParams::ERROR;
+    params.verbosityLM = LevenbergMarquardtParams::TRYLAMBDA;
+  if (isDebugTest)
+    params.verbosity = NonlinearOptimizerParams::ERROR;
 
   Values result;
-  gttic_(SmartStereoProjectionPoseFactor);
+  gttic_ (SmartStereoProjectionPoseFactor);
   LevenbergMarquardtOptimizer optimizer(graph, values, params);
   result = optimizer.optimize();
   gttoc_(SmartStereoProjectionPoseFactor);
@@ -292,28 +306,29 @@ TEST( SmartStereoProjectionPoseFactor, 3poses_smart_projection_factor ){
 //  VectorValues delta = GFG->optimize();
 
   // result.print("results of 3 camera, 3 landmark optimization \n");
-  if(isDebugTest) result.at<Pose3>(x3).print("Smart: Pose3 after optimization: ");
+  if (isDebugTest)
-  EXPECT(assert_equal(pose3,result.at<Pose3>(x3)));
+    result.at<Pose3>(x3).print("Smart: Pose3 after optimization: ");
-  if(isDebugTest) tictoc_print_();
+  EXPECT(assert_equal(pose3, result.at<Pose3>(x3)));
+  if (isDebugTest)
+    tictoc_print_();
 }
 
-
 /* *************************************************************************/
-TEST( SmartStereoProjectionPoseFactor, jacobianSVD ){
+TEST( SmartStereoProjectionPoseFactor, jacobianSVD ) {
 
-  std::vector<Key> views;
+  vector<Key> views;
   views.push_back(x1);
   views.push_back(x2);
   views.push_back(x3);
 
   // create first camera. Looking along X-axis, 1 meter above ground plane (x-y)
-  Pose3 pose1 = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1));
+  Pose3 pose1 = Pose3(Rot3::ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 1));
   StereoCamera cam1(pose1, K);
   // create second camera 1 meter to the right of first camera
-  Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1,0,0));
+  Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1, 0, 0));
   StereoCamera cam2(pose2, K);
   // create third camera 1 meter above the first camera
-  Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0,-1,0));
+  Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0, -1, 0));
   StereoCamera cam3(pose3, K);
 
   // three landmarks ~5 meters infront of camera
@@ -322,17 +337,23 @@ TEST( SmartStereoProjectionPoseFactor, jacobianSVD ){
   Point3 landmark3(3, 0, 3.0);
 
   // 1. Project three landmarks into three cameras and triangulate
-  vector<StereoPoint2> measurements_cam1 = stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark1);
+  vector<StereoPoint2> measurements_cam1 = stereo_projectToMultipleCameras(cam1,
-  vector<StereoPoint2> measurements_cam2 = stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark2);
+      cam2, cam3, landmark1);
-  vector<StereoPoint2> measurements_cam3 = stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark3);
+  vector<StereoPoint2> measurements_cam2 = stereo_projectToMultipleCameras(cam1,
+      cam2, cam3, landmark2);
+  vector<StereoPoint2> measurements_cam3 = stereo_projectToMultipleCameras(cam1,
+      cam2, cam3, landmark3);
 
-  SmartFactor::shared_ptr smartFactor1(new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD));
+  SmartFactor::shared_ptr smartFactor1(
+      new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD));
   smartFactor1->add(measurements_cam1, views, model, K);
 
-  SmartFactor::shared_ptr smartFactor2(new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD));
+  SmartFactor::shared_ptr smartFactor2(
+      new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD));
   smartFactor2->add(measurements_cam2, views, model, K);
 
-  SmartFactor::shared_ptr smartFactor3(new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD));
+  SmartFactor::shared_ptr smartFactor3(
+      new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD));
   smartFactor3->add(measurements_cam3, views, model, K);
 
   const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
@@ -344,38 +365,39 @@ TEST( SmartStereoProjectionPoseFactor, jacobianSVD ){
   graph.push_back(PriorFactor<Pose3>(x1, pose1, noisePrior));
   graph.push_back(PriorFactor<Pose3>(x2, pose2, noisePrior));
 
-  //  Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), gtsam::Point3(0.5,0.1,0.3)); // noise from regular projection factor test below
+  //  Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below
-  Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/100, 0., -M_PI/100), gtsam::Point3(0.1,0.1,0.1)); // smaller noise
+  Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI / 100, 0., -M_PI / 100),
+      Point3(0.1, 0.1, 0.1)); // smaller noise
   Values values;
   values.insert(x1, pose1);
   values.insert(x2, pose2);
-  values.insert(x3, pose3*noise_pose);
+  values.insert(x3, pose3 * noise_pose);
 
   LevenbergMarquardtParams params;
   Values result;
   LevenbergMarquardtOptimizer optimizer(graph, values, params);
   result = optimizer.optimize();
-  EXPECT(assert_equal(pose3,result.at<Pose3>(x3)));
+  EXPECT(assert_equal(pose3, result.at<Pose3>(x3)));
 }
 
 /* *************************************************************************/
-TEST( SmartStereoProjectionPoseFactor, landmarkDistance ){
+TEST( SmartStereoProjectionPoseFactor, landmarkDistance ) {
 
   double excludeLandmarksFutherThanDist = 2;
 
-  std::vector<Key> views;
+  vector<Key> views;
   views.push_back(x1);
   views.push_back(x2);
   views.push_back(x3);
 
   // create first camera. Looking along X-axis, 1 meter above ground plane (x-y)
-  Pose3 pose1 = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1));
+  Pose3 pose1 = Pose3(Rot3::ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 1));
   StereoCamera cam1(pose1, K);
   // create second camera 1 meter to the right of first camera
-  Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1,0,0));
+  Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1, 0, 0));
   StereoCamera cam2(pose2, K);
   // create third camera 1 meter above the first camera
-  Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0,-1,0));
+  Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0, -1, 0));
   StereoCamera cam3(pose3, K);
 
   // three landmarks ~5 meters infront of camera
@@ -384,18 +406,26 @@ TEST( SmartStereoProjectionPoseFactor, landmarkDistance ){
   Point3 landmark3(3, 0, 3.0);
 
   // 1. Project three landmarks into three cameras and triangulate
-  vector<StereoPoint2> measurements_cam1 = stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark1);
+  vector<StereoPoint2> measurements_cam1 = stereo_projectToMultipleCameras(cam1,
-  vector<StereoPoint2> measurements_cam2 = stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark2);
+      cam2, cam3, landmark1);
-  vector<StereoPoint2> measurements_cam3 = stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark3);
+  vector<StereoPoint2> measurements_cam2 = stereo_projectToMultipleCameras(cam1,
+      cam2, cam3, landmark2);
+  vector<StereoPoint2> measurements_cam3 = stereo_projectToMultipleCameras(cam1,
+      cam2, cam3, landmark3);
 
-
+  SmartFactor::shared_ptr smartFactor1(
-  SmartFactor::shared_ptr smartFactor1(new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD, excludeLandmarksFutherThanDist));
+      new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD,
+          excludeLandmarksFutherThanDist));
   smartFactor1->add(measurements_cam1, views, model, K);
 
-  SmartFactor::shared_ptr smartFactor2(new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD, excludeLandmarksFutherThanDist));
+  SmartFactor::shared_ptr smartFactor2(
+      new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD,
+          excludeLandmarksFutherThanDist));
   smartFactor2->add(measurements_cam2, views, model, K);
 
-  SmartFactor::shared_ptr smartFactor3(new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD, excludeLandmarksFutherThanDist));
+  SmartFactor::shared_ptr smartFactor3(
+      new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD,
+          excludeLandmarksFutherThanDist));
   smartFactor3->add(measurements_cam3, views, model, K);
 
   const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
@@ -407,40 +437,41 @@ TEST( SmartStereoProjectionPoseFactor, landmarkDistance ){
   graph.push_back(PriorFactor<Pose3>(x1, pose1, noisePrior));
   graph.push_back(PriorFactor<Pose3>(x2, pose2, noisePrior));
 
-  //  Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), gtsam::Point3(0.5,0.1,0.3)); // noise from regular projection factor test below
+  //  Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below
-  Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/100, 0., -M_PI/100), gtsam::Point3(0.1,0.1,0.1)); // smaller noise
+  Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI / 100, 0., -M_PI / 100),
+      Point3(0.1, 0.1, 0.1)); // smaller noise
   Values values;
   values.insert(x1, pose1);
   values.insert(x2, pose2);
-  values.insert(x3, pose3*noise_pose);
+  values.insert(x3, pose3 * noise_pose);
 
   // All factors are disabled and pose should remain where it is
   LevenbergMarquardtParams params;
   Values result;
   LevenbergMarquardtOptimizer optimizer(graph, values, params);
   result = optimizer.optimize();
-  EXPECT(assert_equal(values.at<Pose3>(x3),result.at<Pose3>(x3)));
+  EXPECT(assert_equal(values.at<Pose3>(x3), result.at<Pose3>(x3)));
 }
 
 /* *************************************************************************/
-TEST( SmartStereoProjectionPoseFactor, dynamicOutlierRejection ){
+TEST( SmartStereoProjectionPoseFactor, dynamicOutlierRejection ) {
 
   double excludeLandmarksFutherThanDist = 1e10;
   double dynamicOutlierRejectionThreshold = 1; // max 1 pixel of average reprojection error
 
-  std::vector<Key> views;
+  vector<Key> views;
   views.push_back(x1);
   views.push_back(x2);
   views.push_back(x3);
 
   // create first camera. Looking along X-axis, 1 meter above ground plane (x-y)
-  Pose3 pose1 = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1));
+  Pose3 pose1 = Pose3(Rot3::ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 1));
   StereoCamera cam1(pose1, K);
   // create second camera 1 meter to the right of first camera
-  Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1,0,0));
+  Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1, 0, 0));
   StereoCamera cam2(pose2, K);
   // create third camera 1 meter above the first camera
-  Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0,-1,0));
+  Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0, -1, 0));
   StereoCamera cam3(pose3, K);
 
   // three landmarks ~5 meters infront of camera
@@ -450,27 +481,34 @@ TEST( SmartStereoProjectionPoseFactor, dynamicOutlierRejection ){
   Point3 landmark4(5, -0.5, 1);
 
   // 1. Project four landmarks into three cameras and triangulate
-   vector<StereoPoint2> measurements_cam1 = stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark1);
+  vector<StereoPoint2> measurements_cam1 = stereo_projectToMultipleCameras(cam1,
-   vector<StereoPoint2> measurements_cam2 = stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark2);
+      cam2, cam3, landmark1);
-   vector<StereoPoint2> measurements_cam3 = stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark3);
+  vector<StereoPoint2> measurements_cam2 = stereo_projectToMultipleCameras(cam1,
-   vector<StereoPoint2> measurements_cam4 = stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark4);
+      cam2, cam3, landmark2);
+  vector<StereoPoint2> measurements_cam3 = stereo_projectToMultipleCameras(cam1,
+      cam2, cam3, landmark3);
+  vector<StereoPoint2> measurements_cam4 = stereo_projectToMultipleCameras(cam1,
+      cam2, cam3, landmark4);
 
+  measurements_cam4.at(0) = measurements_cam4.at(0) + StereoPoint2(10, 10, 1); // add outlier
 
-  measurements_cam4.at(0) = measurements_cam4.at(0) + StereoPoint2(10,10,1); // add outlier
+  SmartFactor::shared_ptr smartFactor1(
-
+      new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD,
-  SmartFactor::shared_ptr smartFactor1(new SmartFactor(1, -1, false, false, boost::none,
+          excludeLandmarksFutherThanDist, dynamicOutlierRejectionThreshold));
-      JACOBIAN_SVD, excludeLandmarksFutherThanDist, dynamicOutlierRejectionThreshold));
   smartFactor1->add(measurements_cam1, views, model, K);
 
-  SmartFactor::shared_ptr smartFactor2(new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD,
+  SmartFactor::shared_ptr smartFactor2(
+      new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD,
           excludeLandmarksFutherThanDist, dynamicOutlierRejectionThreshold));
   smartFactor2->add(measurements_cam2, views, model, K);
 
-  SmartFactor::shared_ptr smartFactor3(new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD,
+  SmartFactor::shared_ptr smartFactor3(
+      new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD,
           excludeLandmarksFutherThanDist, dynamicOutlierRejectionThreshold));
   smartFactor3->add(measurements_cam3, views, model, K);
 
-  SmartFactor::shared_ptr smartFactor4(new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD,
+  SmartFactor::shared_ptr smartFactor4(
+      new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD,
           excludeLandmarksFutherThanDist, dynamicOutlierRejectionThreshold));
   smartFactor4->add(measurements_cam4, views, model, K);
 
@@ -484,7 +522,8 @@ TEST( SmartStereoProjectionPoseFactor, dynamicOutlierRejection ){
   graph.push_back(PriorFactor<Pose3>(x1, pose1, noisePrior));
   graph.push_back(PriorFactor<Pose3>(x2, pose2, noisePrior));
 
-  Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/100, 0., -M_PI/100), gtsam::Point3(0.1,0.1,0.1)); // smaller noise
+  Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI / 100, 0., -M_PI / 100),
+      Point3(0.1, 0.1, 0.1)); // smaller noise
   Values values;
   values.insert(x1, pose1);
   values.insert(x2, pose2);
@@ -495,19 +534,19 @@ TEST( SmartStereoProjectionPoseFactor, dynamicOutlierRejection ){
   Values result;
   LevenbergMarquardtOptimizer optimizer(graph, values, params);
   result = optimizer.optimize();
-  EXPECT(assert_equal(pose3,result.at<Pose3>(x3)));
+  EXPECT(assert_equal(pose3, result.at<Pose3>(x3)));
 }
 //
 ///* *************************************************************************/
 //TEST( SmartStereoProjectionPoseFactor, jacobianQ ){
 //
-//  std::vector<Key> views;
+//  vector<Key> views;
 //  views.push_back(x1);
 //  views.push_back(x2);
 //  views.push_back(x3);
 //
 //  // create first camera. Looking along X-axis, 1 meter above ground plane (x-y)
-//  Pose3 pose1 = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1));
+//  Pose3 pose1 = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), Point3(0,0,1));
 //  StereoCamera cam1(pose1, K);
 //  // create second camera 1 meter to the right of first camera
 //  Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1,0,0));
@@ -546,8 +585,8 @@ TEST( SmartStereoProjectionPoseFactor, dynamicOutlierRejection ){
 //  graph.push_back(PriorFactor<Pose3>(x1, pose1, noisePrior));
 //  graph.push_back(PriorFactor<Pose3>(x2, pose2, noisePrior));
 //
-//  //  Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), gtsam::Point3(0.5,0.1,0.3)); // noise from regular projection factor test below
+//  //  Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below
-//  Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/100, 0., -M_PI/100), gtsam::Point3(0.1,0.1,0.1)); // smaller noise
+//  Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/100, 0., -M_PI/100), Point3(0.1,0.1,0.1)); // smaller noise
 //  Values values;
 //  values.insert(x1, pose1);
 //  values.insert(x2, pose2);
@@ -564,13 +603,13 @@ TEST( SmartStereoProjectionPoseFactor, dynamicOutlierRejection ){
 //TEST( SmartStereoProjectionPoseFactor, 3poses_projection_factor ){
 //  //  cout << " ************************ Normal ProjectionFactor: 3 cams + 3 landmarks **********************" << endl;
 //
-//  std::vector<Key> views;
+//  vector<Key> views;
 //  views.push_back(x1);
 //  views.push_back(x2);
 //  views.push_back(x3);
 //
 //  // create first camera. Looking along X-axis, 1 meter above ground plane (x-y)
-//  Pose3 pose1 = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1));
+//  Pose3 pose1 = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), Point3(0,0,1));
 //  StereoCamera cam1(pose1, K2);
 //
 //  // create second camera 1 meter to the right of first camera
@@ -606,7 +645,7 @@ TEST( SmartStereoProjectionPoseFactor, dynamicOutlierRejection ){
 //  graph.push_back(PriorFactor<Pose3>(x1, pose1, noisePrior));
 //  graph.push_back(PriorFactor<Pose3>(x2, pose2, noisePrior));
 //
-//  Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), gtsam::Point3(0.5,0.1,0.3));
+//  Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3));
 //  Values values;
 //  values.insert(x1, pose1);
 //  values.insert(x2, pose2);
@@ -627,23 +666,23 @@ TEST( SmartStereoProjectionPoseFactor, dynamicOutlierRejection ){
 //}
 //
 /* *************************************************************************/
-TEST( SmartStereoProjectionPoseFactor, CheckHessian){
+TEST( SmartStereoProjectionPoseFactor, CheckHessian) {
 
-  std::vector<Key> views;
+  vector<Key> views;
   views.push_back(x1);
   views.push_back(x2);
   views.push_back(x3);
 
   // create first camera. Looking along X-axis, 1 meter above ground plane (x-y)
-  Pose3 pose1 = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1));
+  Pose3 pose1 = Pose3(Rot3::ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 1));
   StereoCamera cam1(pose1, K);
 
-  // create second camera 1 meter to the right of first camera
+  // create second camera
-  Pose3 pose2 = pose1 * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0,0,0));
+  Pose3 pose2 = pose1 * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0, 0, 0));
   StereoCamera cam2(pose2, K);
 
-  // create third camera 1 meter above the first camera
+  // create third camera
-  Pose3 pose3 = pose2 * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0,0,0));
+  Pose3 pose3 = pose2 * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0, 0, 0));
   StereoCamera cam3(pose3, K);
 
   // three landmarks ~5 meters infront of camera
@@ -651,12 +690,15 @@ TEST( SmartStereoProjectionPoseFactor, CheckHessian){
   Point3 landmark2(5, -0.5, 1.2);
   Point3 landmark3(3, 0, 3.0);
 
-  // 1. Project three landmarks into three cameras and triangulate
+  // Project three landmarks into three cameras and triangulate
-  vector<StereoPoint2> measurements_cam1 = stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark1);
+  vector<StereoPoint2> measurements_cam1 = stereo_projectToMultipleCameras(cam1,
-  vector<StereoPoint2> measurements_cam2 = stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark2);
+      cam2, cam3, landmark1);
-  vector<StereoPoint2> measurements_cam3 = stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark3);
+  vector<StereoPoint2> measurements_cam2 = stereo_projectToMultipleCameras(cam1,
-
+      cam2, cam3, landmark2);
+  vector<StereoPoint2> measurements_cam3 = stereo_projectToMultipleCameras(cam1,
+      cam2, cam3, landmark3);
 
+  // Create smartfactors
   double rankTol = 10;
 
   SmartFactor::shared_ptr smartFactor1(new SmartFactor(rankTol));
@@ -668,38 +710,47 @@ TEST( SmartStereoProjectionPoseFactor, CheckHessian){
   SmartFactor::shared_ptr smartFactor3(new SmartFactor(rankTol));
   smartFactor3->add(measurements_cam3, views, model, K);
 
+  // Create graph to optimize
   NonlinearFactorGraph graph;
   graph.push_back(smartFactor1);
   graph.push_back(smartFactor2);
   graph.push_back(smartFactor3);
 
-  //  Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), gtsam::Point3(0.5,0.1,0.3)); // noise from regular projection factor test below
-  Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/100, 0., -M_PI/100), gtsam::Point3(0.1,0.1,0.1)); // smaller noise
   Values values;
   values.insert(x1, pose1);
   values.insert(x2, pose2);
   // initialize third pose with some noise, we expect it to move back to original pose3
-  values.insert(x3, pose3*noise_pose);
+  Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI / 100, 0., -M_PI / 100),
-  if(isDebugTest) values.at<Pose3>(x3).print("Smart: Pose3 before optimization: ");
+      Point3(0.1, 0.1, 0.1)); // smaller noise
+  values.insert(x3, pose3 * noise_pose);
+  if (isDebugTest)
+    values.at<Pose3>(x3).print("Smart: Pose3 before optimization: ");
 
-  boost::shared_ptr<GaussianFactor> hessianFactor1 = smartFactor1->linearize(values);
+  // TODO: next line throws Cheirality exception on Mac
-  boost::shared_ptr<GaussianFactor> hessianFactor2 = smartFactor2->linearize(values);
+  boost::shared_ptr<GaussianFactor> hessianFactor1 = smartFactor1->linearize(
-  boost::shared_ptr<GaussianFactor> hessianFactor3 = smartFactor3->linearize(values);
+      values);
+  boost::shared_ptr<GaussianFactor> hessianFactor2 = smartFactor2->linearize(
+      values);
+  boost::shared_ptr<GaussianFactor> hessianFactor3 = smartFactor3->linearize(
+      values);
 
-  Matrix CumulativeInformation = hessianFactor1->information() +  hessianFactor2->information() + hessianFactor3->information();
+  Matrix CumulativeInformation = hessianFactor1->information()
+      + hessianFactor2->information() + hessianFactor3->information();
 
-  boost::shared_ptr<GaussianFactorGraph> GaussianGraph = graph.linearize(values);
+  boost::shared_ptr<GaussianFactorGraph> GaussianGraph = graph.linearize(
+      values);
   Matrix GraphInformation = GaussianGraph->hessian().first;
 
   // Check Hessian
   EXPECT(assert_equal(GraphInformation, CumulativeInformation, 1e-8));
 
-  Matrix AugInformationMatrix = hessianFactor1->augmentedInformation() +
+  Matrix AugInformationMatrix = hessianFactor1->augmentedInformation()
-      hessianFactor2->augmentedInformation() + hessianFactor3->augmentedInformation();
+      + hessianFactor2->augmentedInformation()
+      + hessianFactor3->augmentedInformation();
 
   // Check Information vector
   // cout << AugInformationMatrix.size() << endl;
-  Vector InfoVector = AugInformationMatrix.block(0,18,18,1); // 18x18 Hessian + information vector
+  Vector InfoVector = AugInformationMatrix.block(0, 18, 18, 1); // 18x18 Hessian + information vector
 
   // Check Hessian
   EXPECT(assert_equal(InfoVector, GaussianGraph->hessian().second, 1e-8));
@@ -709,13 +760,13 @@ TEST( SmartStereoProjectionPoseFactor, CheckHessian){
 //TEST( SmartStereoProjectionPoseFactor, 3poses_2land_rotation_only_smart_projection_factor ){
 //  // cout << " ************************ SmartStereoProjectionPoseFactor: 3 cams + 2 landmarks: Rotation Only**********************" << endl;
 //
-//  std::vector<Key> views;
+//  vector<Key> views;
 //  views.push_back(x1);
 //  views.push_back(x2);
 //  views.push_back(x3);
 //
 //  // create first camera. Looking along X-axis, 1 meter above ground plane (x-y)
-//  Pose3 pose1 = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1));
+//  Pose3 pose1 = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), Point3(0,0,1));
 //  StereoCamera cam1(pose1, K2);
 //
 //  // create second camera 1 meter to the right of first camera
@@ -745,7 +796,7 @@ TEST( SmartStereoProjectionPoseFactor, CheckHessian){
 //
 //  const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
 //  const SharedDiagonal noisePriorTranslation = noiseModel::Isotropic::Sigma(3, 0.10);
-//  Point3 positionPrior = gtsam::Point3(0,0,1);
+//  Point3 positionPrior = Point3(0,0,1);
 //
 //  NonlinearFactorGraph graph;
 //  graph.push_back(smartFactor1);
@@ -754,7 +805,7 @@ TEST( SmartStereoProjectionPoseFactor, CheckHessian){
 //  graph.push_back(PoseTranslationPrior<Pose3>(x2, positionPrior, noisePriorTranslation));
 //  graph.push_back(PoseTranslationPrior<Pose3>(x3, positionPrior, noisePriorTranslation));
 //
-//  Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), gtsam::Point3(0.1,0.1,0.1)); // smaller noise
+//  Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), Point3(0.1,0.1,0.1)); // smaller noise
 //  Values values;
 //  values.insert(x1, pose1);
 //  values.insert(x2, pose2*noise_pose);
@@ -775,7 +826,7 @@ TEST( SmartStereoProjectionPoseFactor, CheckHessian){
 //
 //  // result.print("results of 3 camera, 3 landmark optimization \n");
 //  if(isDebugTest) result.at<Pose3>(x3).print("Smart: Pose3 after optimization: ");
-//  std::cout << "TEST COMMENTED: rotation only version of smart factors has been deprecated " << std::endl;
+//  cout << "TEST COMMENTED: rotation only version of smart factors has been deprecated " << endl;
 //  // EXPECT(assert_equal(pose3,result.at<Pose3>(x3)));
 //  if(isDebugTest) tictoc_print_();
 //}
@@ -784,13 +835,13 @@ TEST( SmartStereoProjectionPoseFactor, CheckHessian){
 //TEST( SmartStereoProjectionPoseFactor, 3poses_rotation_only_smart_projection_factor ){
 //  // cout << " ************************ SmartStereoProjectionPoseFactor: 3 cams + 3 landmarks: Rotation Only**********************" << endl;
 //
-//  std::vector<Key> views;
+//  vector<Key> views;
 //  views.push_back(x1);
 //  views.push_back(x2);
 //  views.push_back(x3);
 //
 //  // create first camera. Looking along X-axis, 1 meter above ground plane (x-y)
-//  Pose3 pose1 = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1));
+//  Pose3 pose1 = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), Point3(0,0,1));
 //  StereoCamera cam1(pose1, K);
 //
 //  // create second camera 1 meter to the right of first camera
@@ -826,7 +877,7 @@ TEST( SmartStereoProjectionPoseFactor, CheckHessian){
 //
 //  const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
 //  const SharedDiagonal noisePriorTranslation = noiseModel::Isotropic::Sigma(3, 0.10);
-//  Point3 positionPrior = gtsam::Point3(0,0,1);
+//  Point3 positionPrior = Point3(0,0,1);
 //
 //  NonlinearFactorGraph graph;
 //  graph.push_back(smartFactor1);
@@ -836,8 +887,8 @@ TEST( SmartStereoProjectionPoseFactor, CheckHessian){
 //  graph.push_back(PoseTranslationPrior<Pose3>(x2, positionPrior, noisePriorTranslation));
 //  graph.push_back(PoseTranslationPrior<Pose3>(x3, positionPrior, noisePriorTranslation));
 //
-//  //  Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), gtsam::Point3(0.5,0.1,0.3)); // noise from regular projection factor test below
+//  //  Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below
-//  Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/100, 0., -M_PI/100), gtsam::Point3(0.1,0.1,0.1)); // smaller noise
+//  Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/100, 0., -M_PI/100), Point3(0.1,0.1,0.1)); // smaller noise
 //  Values values;
 //  values.insert(x1, pose1);
 //  values.insert(x2, pose2);
@@ -858,7 +909,7 @@ TEST( SmartStereoProjectionPoseFactor, CheckHessian){
 //
 //  // result.print("results of 3 camera, 3 landmark optimization \n");
 //  if(isDebugTest) result.at<Pose3>(x3).print("Smart: Pose3 after optimization: ");
-//  std::cout << "TEST COMMENTED: rotation only version of smart factors has been deprecated " << std::endl;
+//  cout << "TEST COMMENTED: rotation only version of smart factors has been deprecated " << endl;
 //  // EXPECT(assert_equal(pose3,result.at<Pose3>(x3)));
 //  if(isDebugTest) tictoc_print_();
 //}
@@ -867,12 +918,12 @@ TEST( SmartStereoProjectionPoseFactor, CheckHessian){
 //TEST( SmartStereoProjectionPoseFactor, Hessian ){
 //  // cout << " ************************ SmartStereoProjectionPoseFactor: Hessian **********************" << endl;
 //
-//  std::vector<Key> views;
+//  vector<Key> views;
 //  views.push_back(x1);
 //  views.push_back(x2);
 //
 //  // create first camera. Looking along X-axis, 1 meter above ground plane (x-y)
-//  Pose3 pose1 = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1));
+//  Pose3 pose1 = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), Point3(0,0,1));
 //  StereoCamera cam1(pose1, K2);
 //
 //  // create second camera 1 meter to the right of first camera
@@ -892,7 +943,7 @@ TEST( SmartStereoProjectionPoseFactor, CheckHessian){
 //  SmartFactor::shared_ptr smartFactor1(new SmartFactor());
 //  smartFactor1->add(measurements_cam1,views, model, K2);
 //
-//  Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), gtsam::Point3(0.5,0.1,0.3));
+//  Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3));
 //  Values values;
 //  values.insert(x1, pose1);
 //  values.insert(x2, pose2);
@@ -908,29 +959,30 @@ TEST( SmartStereoProjectionPoseFactor, CheckHessian){
 //
 
 /* *************************************************************************/
-TEST( SmartStereoProjectionPoseFactor, HessianWithRotation ){
+TEST( SmartStereoProjectionPoseFactor, HessianWithRotation ) {
   // cout << " ************************ SmartStereoProjectionPoseFactor: rotated Hessian **********************" << endl;
 
-  std::vector<Key> views;
+  vector<Key> views;
   views.push_back(x1);
   views.push_back(x2);
   views.push_back(x3);
 
   // create first camera. Looking along X-axis, 1 meter above ground plane (x-y)
-  Pose3 pose1 = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1));
+  Pose3 pose1 = Pose3(Rot3::ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 1));
   StereoCamera cam1(pose1, K);
 
   // create second camera 1 meter to the right of first camera
-  Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1,0,0));
+  Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1, 0, 0));
   StereoCamera cam2(pose2, K);
 
   // create third camera 1 meter above the first camera
-  Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0,-1,0));
+  Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0, -1, 0));
   StereoCamera cam3(pose3, K);
 
   Point3 landmark1(5, 0.5, 1.2);
 
-  vector<StereoPoint2> measurements_cam1 = stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark1);
+  vector<StereoPoint2> measurements_cam1 = stereo_projectToMultipleCameras(cam1,
+      cam2, cam3, landmark1);
 
   SmartFactor::shared_ptr smartFactorInstance(new SmartFactor());
   smartFactorInstance->add(measurements_cam1, views, model, K);
@@ -940,46 +992,54 @@ TEST( SmartStereoProjectionPoseFactor, HessianWithRotation ){
   values.insert(x2, pose2);
   values.insert(x3, pose3);
 
-  boost::shared_ptr<GaussianFactor> hessianFactor = smartFactorInstance->linearize(values);
+  boost::shared_ptr<GaussianFactor> hessianFactor =
+      smartFactorInstance->linearize(values);
   // hessianFactor->print("Hessian factor \n");
 
-  Pose3 poseDrift = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,0));
+  Pose3 poseDrift = Pose3(Rot3::ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 0));
 
   Values rotValues;
   rotValues.insert(x1, poseDrift.compose(pose1));
   rotValues.insert(x2, poseDrift.compose(pose2));
   rotValues.insert(x3, poseDrift.compose(pose3));
 
-  boost::shared_ptr<GaussianFactor> hessianFactorRot = smartFactorInstance->linearize(rotValues);
+  boost::shared_ptr<GaussianFactor> hessianFactorRot =
+      smartFactorInstance->linearize(rotValues);
   // hessianFactorRot->print("Hessian factor \n");
 
   // Hessian is invariant to rotations in the nondegenerate case
-  EXPECT(assert_equal(hessianFactor->information(), hessianFactorRot->information(), 1e-8) );
+  EXPECT(
+      assert_equal(hessianFactor->information(),
+          hessianFactorRot->information(), 1e-7));
 
-  Pose3 poseDrift2 = Pose3(Rot3::ypr(-M_PI/2, -M_PI/3, -M_PI/2), gtsam::Point3(10,-4,5));
+  Pose3 poseDrift2 = Pose3(Rot3::ypr(-M_PI / 2, -M_PI / 3, -M_PI / 2),
+      Point3(10, -4, 5));
 
   Values tranValues;
   tranValues.insert(x1, poseDrift2.compose(pose1));
   tranValues.insert(x2, poseDrift2.compose(pose2));
   tranValues.insert(x3, poseDrift2.compose(pose3));
 
-  boost::shared_ptr<GaussianFactor> hessianFactorRotTran = smartFactorInstance->linearize(tranValues);
+  boost::shared_ptr<GaussianFactor> hessianFactorRotTran =
+      smartFactorInstance->linearize(tranValues);
 
   // Hessian is invariant to rotations and translations in the nondegenerate case
-  EXPECT(assert_equal(hessianFactor->information(), hessianFactorRotTran->information(), 1e-8) );
+  EXPECT(
+      assert_equal(hessianFactor->information(),
+          hessianFactorRotTran->information(), 1e-7));
 }
 
 /* *************************************************************************/
-TEST( SmartStereoProjectionPoseFactor, HessianWithRotationDegenerate ){
+TEST( SmartStereoProjectionPoseFactor, HessianWithRotationDegenerate ) {
   // cout << " ************************ SmartStereoProjectionPoseFactor: rotated Hessian (degenerate) **********************" << endl;
 
-  std::vector<Key> views;
+  vector<Key> views;
   views.push_back(x1);
   views.push_back(x2);
   views.push_back(x3);
 
   // create first camera. Looking along X-axis, 1 meter above ground plane (x-y)
-  Pose3 pose1 = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1));
+  Pose3 pose1 = Pose3(Rot3::ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 1));
   StereoCamera cam1(pose1, K2);
 
   // Second and third cameras in same place, which is a degenerate configuration
@@ -990,49 +1050,60 @@ TEST( SmartStereoProjectionPoseFactor, HessianWithRotationDegenerate ){
 
   Point3 landmark1(5, 0.5, 1.2);
 
-  vector<StereoPoint2> measurements_cam1 = stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark1);
+  vector<StereoPoint2> measurements_cam1 = stereo_projectToMultipleCameras(cam1,
+      cam2, cam3, landmark1);
 
   SmartFactor::shared_ptr smartFactor(new SmartFactor());
   smartFactor->add(measurements_cam1, views, model, K2);
 
-
   Values values;
   values.insert(x1, pose1);
   values.insert(x2, pose2);
   values.insert(x3, pose3);
 
-  boost::shared_ptr<GaussianFactor> hessianFactor = smartFactor->linearize(values);
+  boost::shared_ptr<GaussianFactor> hessianFactor = smartFactor->linearize(
-  if(isDebugTest)  hessianFactor->print("Hessian factor \n");
+      values);
+  if (isDebugTest)
+    hessianFactor->print("Hessian factor \n");
 
-  Pose3 poseDrift = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,0));
+  Pose3 poseDrift = Pose3(Rot3::ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 0));
 
   Values rotValues;
   rotValues.insert(x1, poseDrift.compose(pose1));
   rotValues.insert(x2, poseDrift.compose(pose2));
   rotValues.insert(x3, poseDrift.compose(pose3));
 
-  boost::shared_ptr<GaussianFactor> hessianFactorRot = smartFactor->linearize(rotValues);
+  boost::shared_ptr<GaussianFactor> hessianFactorRot = smartFactor->linearize(
-  if(isDebugTest)  hessianFactorRot->print("Hessian factor \n");
+      rotValues);
+  if (isDebugTest)
+    hessianFactorRot->print("Hessian factor \n");
 
   // Hessian is invariant to rotations in the nondegenerate case
-  EXPECT(assert_equal(hessianFactor->information(), hessianFactorRot->information(), 1e-8) );
+  EXPECT(
+      assert_equal(hessianFactor->information(),
+          hessianFactorRot->information(), 1e-8));
 
-  Pose3 poseDrift2 = Pose3(Rot3::ypr(-M_PI/2, -M_PI/3, -M_PI/2), gtsam::Point3(10,-4,5));
+  Pose3 poseDrift2 = Pose3(Rot3::ypr(-M_PI / 2, -M_PI / 3, -M_PI / 2),
+      Point3(10, -4, 5));
 
   Values tranValues;
   tranValues.insert(x1, poseDrift2.compose(pose1));
   tranValues.insert(x2, poseDrift2.compose(pose2));
   tranValues.insert(x3, poseDrift2.compose(pose3));
 
-  boost::shared_ptr<GaussianFactor> hessianFactorRotTran = smartFactor->linearize(tranValues);
+  boost::shared_ptr<GaussianFactor> hessianFactorRotTran =
+      smartFactor->linearize(tranValues);
 
   // Hessian is invariant to rotations and translations in the nondegenerate case
-  EXPECT(assert_equal(hessianFactor->information(), hessianFactorRotTran->information(), 1e-8) );
+  EXPECT(
+      assert_equal(hessianFactor->information(),
+          hessianFactorRotTran->information(), 1e-8));
 }
 
-
 /* ************************************************************************* */
-int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
+int main() {
+  TestResult tr;
+  return TestRegistry::runAllTests(tr);
+}
 /* ************************************************************************* */
 
-

tests/testGradientDescentOptimizer.cpp

@@ -1,7 +1,14 @@
+/**
+ * @file   NonlinearConjugateGradientOptimizer.cpp
+ * @brief  Test simple CG optimizer
+ * @author Yong-Dian Jian
+ * @date   June 11, 2012
+ */
+
 /**
  * @file   testGradientDescentOptimizer.cpp
- * @brief  
+ * @brief  Small test of NonlinearConjugateGradientOptimizer
- * @author ydjian
+ * @author Yong-Dian Jian
  * @date   Jun 11, 2012
  */
 
@@ -18,77 +25,60 @@
 #include <boost/shared_ptr.hpp>
 #include <boost/tuple/tuple.hpp>
 
-
 using namespace std;
 using namespace gtsam;
 
-
+// Generate a small PoseSLAM problem
 boost::tuple<NonlinearFactorGraph, Values> generateProblem() {
 
   // 1. Create graph container and add factors to it
-  NonlinearFactorGraph graph ;
+  NonlinearFactorGraph graph;
 
   // 2a. Add Gaussian prior
   Pose2 priorMean(0.0, 0.0, 0.0); // prior at origin
-  SharedDiagonal priorNoise = noiseModel::Diagonal::Sigmas(Vector3(0.3, 0.3, 0.1));
+  SharedDiagonal priorNoise = noiseModel::Diagonal::Sigmas(
+      Vector3(0.3, 0.3, 0.1));
   graph += PriorFactor<Pose2>(1, priorMean, priorNoise);
 
   // 2b. Add odometry factors
-  SharedDiagonal odometryNoise = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1));
+  SharedDiagonal odometryNoise = noiseModel::Diagonal::Sigmas(
-  graph += BetweenFactor<Pose2>(1, 2, Pose2(2.0, 0.0, 0.0   ), odometryNoise);
+      Vector3(0.2, 0.2, 0.1));
+  graph += BetweenFactor<Pose2>(1, 2, Pose2(2.0, 0.0, 0.0), odometryNoise);
   graph += BetweenFactor<Pose2>(2, 3, Pose2(2.0, 0.0, M_PI_2), odometryNoise);
   graph += BetweenFactor<Pose2>(3, 4, Pose2(2.0, 0.0, M_PI_2), odometryNoise);
   graph += BetweenFactor<Pose2>(4, 5, Pose2(2.0, 0.0, M_PI_2), odometryNoise);
 
   // 2c. Add pose constraint
-  SharedDiagonal constraintUncertainty = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1));
+  SharedDiagonal constraintUncertainty = noiseModel::Diagonal::Sigmas(
-  graph += BetweenFactor<Pose2>(5, 2, Pose2(2.0, 0.0, M_PI_2), constraintUncertainty);
+      Vector3(0.2, 0.2, 0.1));
+  graph += BetweenFactor<Pose2>(5, 2, Pose2(2.0, 0.0, M_PI_2),
+      constraintUncertainty);
 
   // 3. Create the data structure to hold the initialEstimate estimate to the solution
   Values initialEstimate;
-  Pose2 x1(0.5, 0.0, 0.2   ); initialEstimate.insert(1, x1);
+  Pose2 x1(0.5, 0.0, 0.2);
-  Pose2 x2(2.3, 0.1,-0.2   ); initialEstimate.insert(2, x2);
+  initialEstimate.insert(1, x1);
-  Pose2 x3(4.1, 0.1, M_PI_2); initialEstimate.insert(3, x3);
+  Pose2 x2(2.3, 0.1, -0.2);
-  Pose2 x4(4.0, 2.0, M_PI  ); initialEstimate.insert(4, x4);
+  initialEstimate.insert(2, x2);
-  Pose2 x5(2.1, 2.1,-M_PI_2); initialEstimate.insert(5, x5);
+  Pose2 x3(4.1, 0.1, M_PI_2);
+  initialEstimate.insert(3, x3);
+  Pose2 x4(4.0, 2.0, M_PI);
+  initialEstimate.insert(4, x4);
+  Pose2 x5(2.1, 2.1, -M_PI_2);
+  initialEstimate.insert(5, x5);
 
   return boost::tie(graph, initialEstimate);
 }
 
 /* ************************************************************************* */
-TEST(optimize, GradientDescentOptimizer) {
+TEST(NonlinearConjugateGradientOptimizer, Optimize) {
 
   NonlinearFactorGraph graph;
   Values initialEstimate;
 
   boost::tie(graph, initialEstimate) = generateProblem();
-  // cout << "initial error = " << graph.error(initialEstimate) << endl ;
+//  cout << "initial error = " << graph.error(initialEstimate) << endl;
 
-  // Single Step Optimization using Levenberg-Marquardt
-  NonlinearOptimizerParams param;
-  param.maxIterations = 500;    /* requires a larger number of iterations to converge */
-  param.verbosity = NonlinearOptimizerParams::SILENT;
-
-  NonlinearConjugateGradientOptimizer optimizer(graph, initialEstimate, param);
-  Values result = optimizer.optimize();
-//  cout << "gd1 solver final error = " << graph.error(result) << endl;
-
-  /* the optimality of the solution is not comparable to the */
-  DOUBLES_EQUAL(0.0, graph.error(result), 1e-2);
-
-  CHECK(1);
-}
-
-/* ************************************************************************* */
-TEST(optimize, ConjugateGradientOptimizer) {
-
-  NonlinearFactorGraph graph;
-  Values initialEstimate;
-
-  boost::tie(graph, initialEstimate) = generateProblem();
-//  cout << "initial error = " << graph.error(initialEstimate) << endl ;
-
-  // Single Step Optimization using Levenberg-Marquardt
   NonlinearOptimizerParams param;
   param.maxIterations = 500; /* requires a larger number of iterations to converge */
   param.verbosity = NonlinearOptimizerParams::SILENT;
@@ -97,10 +87,12 @@ TEST(optimize, ConjugateGradientOptimizer) {
   Values result = optimizer.optimize();
 //  cout << "cg final error = " << graph.error(result) << endl;
 
-  /* the optimality of the solution is not comparable to the */
+  EXPECT_DOUBLES_EQUAL(0.0, graph.error(result), 1e-4);
-  DOUBLES_EQUAL(0.0, graph.error(result), 1e-2);
 }
 
 /* ************************************************************************* */
-int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
+int main() {
+  TestResult tr;
+  return TestRegistry::runAllTests(tr);
+}
 /* ************************************************************************* */

tests/testPCGSolver.cpp

@@ -17,21 +17,11 @@
  */
 
 #include <tests/smallExample.h>
-#include <gtsam/slam/PriorFactor.h>
-#include <gtsam/slam/BetweenFactor.h>
-#include <gtsam/nonlinear/NonlinearFactorGraph.h>
-#include <gtsam/nonlinear/Values.h>
-#include <gtsam/nonlinear/NonlinearFactorGraph.h>
-#include <gtsam/nonlinear/GaussNewtonOptimizer.h>
-#include <gtsam/nonlinear/DoglegOptimizer.h>
 #include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
 #include <gtsam/linear/GaussianFactorGraph.h>
 #include <gtsam/linear/PCGSolver.h>
-#include <gtsam/linear/Preconditioner.h>
 #include <gtsam/linear/SubgraphPreconditioner.h>
-#include <gtsam/linear/NoiseModel.h>
 #include <gtsam/inference/Symbol.h>
-#include <gtsam/geometry/Pose2.h>
 #include <gtsam/base/Matrix.h>
 
 #include <CppUnitLite/TestHarness.h>

tests/testPreconditioner.cpp

@@ -18,11 +18,12 @@
 
 #include <CppUnitLite/TestHarness.h>
 
-#include <gtsam/linear/GaussianFactorGraph.h>
-#include <gtsam/linear/Preconditioner.h>
 #include <gtsam/nonlinear/Values.h>
-#include <gtsam/geometry/Point2.h>
+#include <gtsam/linear/GaussianFactorGraph.h>
+#include <gtsam/linear/VectorValues.h>
+#include <gtsam/linear/Preconditioner.h>
 #include <gtsam/linear/PCGSolver.h>
+#include <gtsam/geometry/Point2.h>
 
 using namespace std;
 using namespace gtsam;