Merged 'develop'.

.cproject

@@ -1027,6 +1027,14 @@
 				<useDefaultCommand>true</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>
 			</target>
+			<target name="testPinholePose.run" path="build/gtsam/geometry/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j4</buildArguments>
+				<buildTarget>testPinholePose.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
 			<target name="all" path="spqr_mini" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
 				<buildArguments>-j2</buildArguments>
@@ -1546,6 +1554,14 @@
 				<useDefaultCommand>true</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>
 			</target>
+			<target name="testSmartProjectionPoseFactor.run" path="build/gtsam/slam/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j4</buildArguments>
+				<buildTarget>testSmartProjectionPoseFactor.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
 			<target name="install" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
 				<buildArguments>-j3</buildArguments>

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_g2o.cpp

@@ -20,6 +20,7 @@
 
 #include <gtsam/slam/dataset.h>
 #include <gtsam/slam/PriorFactor.h>
+#include <gtsam/geometry/Pose2.h>
 #include <gtsam/nonlinear/GaussNewtonOptimizer.h>
 #include <fstream>
 

examples/Pose2SLAMExample_graph.cpp

@@ -16,11 +16,15 @@
  * @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/LevenbergMarquardtOptimizer.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/Pose2SLAMExample_lago.cpp

@@ -22,6 +22,7 @@
 #include <gtsam/slam/lago.h>
 #include <gtsam/slam/dataset.h>
 #include <gtsam/slam/PriorFactor.h>
+#include <gtsam/geometry/Pose2.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/discrete/DecisionTree-inl.h

@@ -462,15 +462,17 @@ namespace gtsam {
   // cannot just create a root Choice node on the label: if the label is not the
   // highest label, we need to do a complicated and expensive recursive call.
   template<typename L, typename Y> template<typename Iterator>
-  typename DecisionTree<L, Y>::NodePtr DecisionTree<L, Y>::compose(
+  typename DecisionTree<L, Y>::NodePtr DecisionTree<L, Y>::compose(Iterator begin,
-      Iterator begin, Iterator end, const L& label) const {
+      Iterator end, const L& label) const {
 
     // find highest label among branches
     boost::optional<L> highestLabel;
     boost::optional<size_t> nrChoices;
     for (Iterator it = begin; it != end; it++) {
-      if (it->root_->isLeaf()) continue;
+      if (it->root_->isLeaf())
-      boost::shared_ptr<const Choice> c = boost::dynamic_pointer_cast<const Choice> (it->root_);
+        continue;
+      boost::shared_ptr<const Choice> c =
+          boost::dynamic_pointer_cast<const Choice>(it->root_);
       if (!highestLabel || c->label() > *highestLabel) {
         highestLabel.reset(c->label());
         nrChoices.reset(c->nrChoices());
@@ -478,15 +480,15 @@ namespace gtsam {
     }
 
     // if label is already in correct order, just put together a choice on label
-    if (!highestLabel || label > *highestLabel) {
+    if (!highestLabel || !nrChoices || label > *highestLabel) {
       boost::shared_ptr<Choice> choiceOnLabel(new Choice(label, end - begin));
       for (Iterator it = begin; it != end; it++)
         choiceOnLabel->push_back(it->root_);
       return Choice::Unique(choiceOnLabel);
-    }
+    } else {
-
       // Set up a new choice on the highest label
-    boost::shared_ptr<Choice> choiceOnHighestLabel(new Choice(*highestLabel, *nrChoices));
+      boost::shared_ptr<Choice> choiceOnHighestLabel(
+          new Choice(*highestLabel, *nrChoices));
       // now, for all possible values of highestLabel
       for (size_t index = 0; index < *nrChoices; index++) {
         // make a new set of functions for composing by iterating over the given
@@ -503,6 +505,7 @@ namespace gtsam {
       }
       return Choice::Unique(choiceOnHighestLabel);
     }
+  }
 
   /*********************************************************************************/
   // "create" is a bit of a complicated thing, but very useful.
@@ -667,9 +670,10 @@ namespace gtsam {
   void DecisionTree<L, Y>::dot(const std::string& name, bool showZero) const {
     std::ofstream os((name + ".dot").c_str());
     dot(os, showZero);
-    system(
+    int result = system(
         ("dot -Tpdf " + name + ".dot -o " + name + ".pdf >& /dev/null").c_str());
-  }
+    if (result==-1) throw std::runtime_error("DecisionTree::dot system call failed");
+}
 
 /*********************************************************************************/
 

gtsam/geometry/CalibratedCamera.cpp

@@ -19,88 +19,134 @@
 #include <gtsam/geometry/Pose2.h>
 #include <gtsam/geometry/CalibratedCamera.h>
 
+using namespace std;
+
 namespace gtsam {
 
+#ifndef PINHOLEBASE_LINKING_FIX
 /* ************************************************************************* */
-CalibratedCamera::CalibratedCamera(const Pose3& pose) :
+Matrix26 PinholeBase::Dpose(const Point2& pn, double d) {
-    pose_(pose) {
+  // optimized version of derivatives, see CalibratedCamera.nb
+  const double u = pn.x(), v = pn.y();
+  double uv = u * v, uu = u * u, vv = v * v;
+  Matrix26 Dpn_pose;
+  Dpn_pose << uv, -1 - uu, v, -d, 0, d * u, 1 + vv, -uv, -u, 0, -d, d * v;
+  return Dpn_pose;
 }
 
 /* ************************************************************************* */
-CalibratedCamera::CalibratedCamera(const Vector &v) :
+Matrix23 PinholeBase::Dpoint(const Point2& pn, double d, const Matrix3& Rt) {
-    pose_(Pose3::Expmap(v)) {
+  // optimized version of derivatives, see CalibratedCamera.nb
+  const double u = pn.x(), v = pn.y();
+  Matrix23 Dpn_point;
+  Dpn_point << //
+      Rt(0, 0) - u * Rt(2, 0), Rt(0, 1) - u * Rt(2, 1), Rt(0, 2) - u * Rt(2, 2), //
+  /**/Rt(1, 0) - v * Rt(2, 0), Rt(1, 1) - v * Rt(2, 1), Rt(1, 2) - v * Rt(2, 2);
+  Dpn_point *= d;
+  return Dpn_point;
 }
 
 /* ************************************************************************* */
-Point2 CalibratedCamera::project_to_camera(const Point3& P,
+Pose3 PinholeBase::LevelPose(const Pose2& pose2, double height) {
-    OptionalJacobian<2,3> H1) {
+  const double st = sin(pose2.theta()), ct = cos(pose2.theta());
-  if (H1) {
+  const Point3 x(st, -ct, 0), y(0, 0, -1), z(ct, st, 0);
-    double d = 1.0 / P.z(), d2 = d * d;
+  const Rot3 wRc(x, y, z);
-    *H1 <<  d, 0.0, -P.x() * d2, 0.0, d, -P.y() * d2;
+  const Point3 t(pose2.x(), pose2.y(), height);
+  return Pose3(wRc, t);
+}
+
+/* ************************************************************************* */
+Pose3 PinholeBase::LookatPose(const Point3& eye, const Point3& target,
+    const Point3& upVector) {
+  Point3 zc = target - eye;
+  zc = zc / zc.norm();
+  Point3 xc = (-upVector).cross(zc); // minus upVector since yc is pointing down
+  xc = xc / xc.norm();
+  Point3 yc = zc.cross(xc);
+  return Pose3(Rot3(xc, yc, zc), eye);
+}
+
+/* ************************************************************************* */
+bool PinholeBase::equals(const PinholeBase &camera, double tol) const {
+  return pose_.equals(camera.pose(), tol);
+}
+
+/* ************************************************************************* */
+void PinholeBase::print(const string& s) const {
+  pose_.print(s + ".pose");
+}
+
+/* ************************************************************************* */
+const Pose3& PinholeBase::getPose(OptionalJacobian<6, 6> H) const {
+  if (H) {
+    H->setZero();
+    H->block(0, 0, 6, 6) = I_6x6;
   }
-  return Point2(P.x() / P.z(), P.y() / P.z());
+  return pose_;
 }
 
 /* ************************************************************************* */
-Point3 CalibratedCamera::backproject_from_camera(const Point2& p,
+Point2 PinholeBase::project_to_camera(const Point3& pc,
-    const double scale) {
+    OptionalJacobian<2, 3> Dpoint) {
-  return Point3(p.x() * scale, p.y() * scale, scale);
+  double d = 1.0 / pc.z();
+  const double u = pc.x() * d, v = pc.y() * d;
+  if (Dpoint)
+    *Dpoint << d, 0.0, -u * d, 0.0, d, -v * d;
+  return Point2(u, v);
 }
 
+/* ************************************************************************* */
+pair<Point2, bool> PinholeBase::projectSafe(const Point3& pw) const {
+  const Point3 pc = pose().transform_to(pw);
+  const Point2 pn = project_to_camera(pc);
+  return make_pair(pn, pc.z() > 0);
+}
+
+/* ************************************************************************* */
+Point2 PinholeBase::project2(const Point3& point, OptionalJacobian<2, 6> Dpose,
+    OptionalJacobian<2, 3> Dpoint) const {
+
+  Matrix3 Rt; // calculated by transform_to if needed
+  const Point3 q = pose().transform_to(point, boost::none, Dpoint ? &Rt : 0);
+#ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION
+  if (q.z() <= 0)
+  throw CheiralityException();
+#endif
+  const Point2 pn = project_to_camera(q);
+
+  if (Dpose || Dpoint) {
+    const double d = 1.0 / q.z();
+    if (Dpose)
+      *Dpose = PinholeBase::Dpose(pn, d);
+    if (Dpoint)
+      *Dpoint = PinholeBase::Dpoint(pn, d, Rt);
+  }
+  return pn;
+}
+
+/* ************************************************************************* */
+Point3 PinholeBase::backproject_from_camera(const Point2& p,
+    const double depth) {
+  return Point3(p.x() * depth, p.y() * depth, depth);
+}
+
+#endif
+
 /* ************************************************************************* */
 CalibratedCamera CalibratedCamera::Level(const Pose2& pose2, double height) {
-  double st = sin(pose2.theta()), ct = cos(pose2.theta());
+  return CalibratedCamera(LevelPose(pose2, height));
-  Point3 x(st, -ct, 0), y(0, 0, -1), z(ct, st, 0);
+}
-  Rot3 wRc(x, y, z);
+
-  Point3 t(pose2.x(), pose2.y(), height);
+/* ************************************************************************* */
-  Pose3 pose3(wRc, t);
+CalibratedCamera CalibratedCamera::Lookat(const Point3& eye,
-  return CalibratedCamera(pose3);
+    const Point3& target, const Point3& upVector) {
+  return CalibratedCamera(LookatPose(eye, target, upVector));
 }
 
 /* ************************************************************************* */
 Point2 CalibratedCamera::project(const Point3& point,
-    OptionalJacobian<2,6> Dpose, OptionalJacobian<2,3> Dpoint) const {
+    OptionalJacobian<2, 6> Dcamera, OptionalJacobian<2, 3> Dpoint) const {
-
+  return project2(point, Dcamera, Dpoint);
-#ifdef CALIBRATEDCAMERA_CHAIN_RULE
-  Matrix36 Dpose_;
-  Matrix3 Dpoint_;
-  Point3 q = pose_.transform_to(point, Dpose ? Dpose_ : 0, Dpoint ? Dpoint_ : 0);
-#else
-  Point3 q = pose_.transform_to(point);
-#endif
-  Point2 intrinsic = project_to_camera(q);
-
-  // Check if point is in front of camera
-  if (q.z() <= 0)
-    throw CheiralityException();
-
-  if (Dpose || Dpoint) {
-#ifdef CALIBRATEDCAMERA_CHAIN_RULE
-    // just implement chain rule
-    if(Dpose && Dpoint) {
-      Matrix23 H;
-      project_to_camera(q,H);
-      if (Dpose) *Dpose = H * (*Dpose_);
-      if (Dpoint) *Dpoint = H * (*Dpoint_);
-    }
-#else
-    // optimized version, see CalibratedCamera.nb
-    const double z = q.z(), d = 1.0 / z;
-    const double u = intrinsic.x(), v = intrinsic.y(), uv = u * v;
-    if (Dpose)
-      *Dpose <<  uv, -(1. + u * u), v, -d, 0., d * u, (1. + v * v),
-          -uv, -u, 0., -d, d * v;
-    if (Dpoint) {
-      const Matrix3 R(pose_.rotation().matrix());
-      Matrix23 Dpoint_;
-      Dpoint_ << R(0, 0) - u * R(0, 2), R(1, 0) - u * R(1, 2),
-              R(2, 0) - u * R(2, 2), R(0, 1) - v * R(0, 2),
-              R(1, 1) - v * R(1, 2), R(2, 1) - v * R(2, 2);
-      *Dpoint = d * Dpoint_;
-    }
-#endif
-  }
-  return intrinsic;
 }
 
 /* ************************************************************************* */

gtsam/geometry/CalibratedCamera.h

@@ -19,10 +19,14 @@
 #pragma once
 
 #include <gtsam/geometry/Pose3.h>
-#include <gtsam/geometry/Point2.h>
+#define PINHOLEBASE_LINKING_FIX
-
+#ifdef PINHOLEBASE_LINKING_FIX
+#include <gtsam/geometry/Pose2.h>
+#endif
 namespace gtsam {
 
+class Point2;
+
 class GTSAM_EXPORT CheiralityException: public ThreadsafeException<
     CheiralityException> {
 public:
@@ -31,6 +35,275 @@ public:
   }
 };
 
+/**
+ * A pinhole camera class that has a Pose3, functions as base class for all pinhole cameras
+ * @addtogroup geometry
+ * \nosubgrouping
+ */
+class GTSAM_EXPORT PinholeBase {
+
+private:
+
+  Pose3 pose_; ///< 3D pose of camera
+
+#ifndef PINHOLEBASE_LINKING_FIX
+
+protected:
+
+  /// @name Derivatives
+  /// @{
+
+  /**
+   * Calculate Jacobian with respect to pose
+   * @param pn projection in normalized coordinates
+   * @param d disparity (inverse depth)
+   */
+  static Matrix26 Dpose(const Point2& pn, double d);
+
+  /**
+   * Calculate Jacobian with respect to point
+   * @param pn projection in normalized coordinates
+   * @param d disparity (inverse depth)
+   * @param Rt transposed rotation matrix
+   */
+  static Matrix23 Dpoint(const Point2& pn, double d, const Matrix3& Rt);
+
+  /// @}
+
+public:
+
+  /// @name Static functions
+  /// @{
+
+  /**
+   * Create a level pose at the given 2D pose and height
+   * @param K the calibration
+   * @param pose2 specifies the location and viewing direction
+   * (theta 0 = looking in direction of positive X axis)
+   * @param height camera height
+   */
+  static Pose3 LevelPose(const Pose2& pose2, double height);
+
+  /**
+   * Create a camera pose at the given eye position looking at a target point in the scene
+   * with the specified up direction vector.
+   * @param eye specifies the camera position
+   * @param target the point to look at
+   * @param upVector specifies the camera up direction vector,
+   *        doesn't need to be on the image plane nor orthogonal to the viewing axis
+   */
+  static Pose3 LookatPose(const Point3& eye, const Point3& target,
+      const Point3& upVector);
+
+  /// @}
+  /// @name Standard Constructors
+  /// @{
+
+  /** default constructor */
+  PinholeBase() {
+  }
+
+  /** constructor with pose */
+  explicit PinholeBase(const Pose3& pose) :
+      pose_(pose) {
+  }
+
+  /// @}
+  /// @name Advanced Constructors
+  /// @{
+
+  explicit PinholeBase(const Vector &v) :
+      pose_(Pose3::Expmap(v)) {
+  }
+
+  /// @}
+  /// @name Testable
+  /// @{
+
+  /// assert equality up to a tolerance
+  bool equals(const PinholeBase &camera, double tol = 1e-9) const;
+
+  /// print
+  void print(const std::string& s = "PinholeBase") const;
+
+  /// @}
+  /// @name Standard Interface
+  /// @{
+
+  /// return pose, constant version
+  const Pose3& pose() const {
+    return pose_;
+  }
+
+  /// return pose, with derivative
+  const Pose3& getPose(OptionalJacobian<6, 6> H) const;
+
+  /// @}
+  /// @name Transformations and measurement functions
+  /// @{
+
+  /**
+   * Project from 3D point in camera coordinates into image
+   * Does *not* throw a CheiralityException, even if pc behind image plane
+   * @param pc point in camera coordinates
+   */
+  static Point2 project_to_camera(const Point3& pc, //
+      OptionalJacobian<2, 3> Dpoint = boost::none);
+
+  /// Project a point into the image and check depth
+  std::pair<Point2, bool> projectSafe(const Point3& pw) const;
+
+  /**
+   * Project point into the image
+   * Throws a CheiralityException if point behind image plane iff GTSAM_THROW_CHEIRALITY_EXCEPTION
+   * @param point 3D point in world coordinates
+   * @return the intrinsic coordinates of the projected point
+   */
+  Point2 project2(const Point3& point, OptionalJacobian<2, 6> Dpose =
+      boost::none, OptionalJacobian<2, 3> Dpoint = boost::none) const;
+
+  /// backproject a 2-dimensional point to a 3-dimensional point at given depth
+  static Point3 backproject_from_camera(const Point2& p, const double depth);
+
+#else
+
+public:
+
+  PinholeBase() {
+  }
+
+  explicit PinholeBase(const Pose3& pose) :
+      pose_(pose) {
+  }
+
+  explicit PinholeBase(const Vector &v) :
+      pose_(Pose3::Expmap(v)) {
+  }
+
+  const Pose3& pose() const {
+    return pose_;
+  }
+
+  /* ************************************************************************* */
+  static Matrix26 Dpose(const Point2& pn, double d) {
+    // optimized version of derivatives, see CalibratedCamera.nb
+    const double u = pn.x(), v = pn.y();
+    double uv = u * v, uu = u * u, vv = v * v;
+    Matrix26 Dpn_pose;
+    Dpn_pose << uv, -1 - uu, v, -d, 0, d * u, 1 + vv, -uv, -u, 0, -d, d * v;
+    return Dpn_pose;
+  }
+
+  /* ************************************************************************* */
+  static Matrix23 Dpoint(const Point2& pn, double d, const Matrix3& Rt) {
+    // optimized version of derivatives, see CalibratedCamera.nb
+    const double u = pn.x(), v = pn.y();
+    Matrix23 Dpn_point;
+    Dpn_point << //
+        Rt(0, 0) - u * Rt(2, 0), Rt(0, 1) - u * Rt(2, 1), Rt(0, 2) - u * Rt(2, 2), //
+    /**/Rt(1, 0) - v * Rt(2, 0), Rt(1, 1) - v * Rt(2, 1), Rt(1, 2) - v * Rt(2, 2);
+    Dpn_point *= d;
+    return Dpn_point;
+  }
+
+  /* ************************************************************************* */
+  static Pose3 LevelPose(const Pose2& pose2, double height) {
+    const double st = sin(pose2.theta()), ct = cos(pose2.theta());
+    const Point3 x(st, -ct, 0), y(0, 0, -1), z(ct, st, 0);
+    const Rot3 wRc(x, y, z);
+    const Point3 t(pose2.x(), pose2.y(), height);
+    return Pose3(wRc, t);
+  }
+
+  /* ************************************************************************* */
+  static Pose3 LookatPose(const Point3& eye, const Point3& target,
+      const Point3& upVector) {
+    Point3 zc = target - eye;
+    zc = zc / zc.norm();
+    Point3 xc = (-upVector).cross(zc); // minus upVector since yc is pointing down
+    xc = xc / xc.norm();
+    Point3 yc = zc.cross(xc);
+    return Pose3(Rot3(xc, yc, zc), eye);
+  }
+
+  /* ************************************************************************* */
+  bool equals(const PinholeBase &camera, double tol=1e-9) const {
+    return pose_.equals(camera.pose(), tol);
+  }
+
+  /* ************************************************************************* */
+  void print(const std::string& s) const {
+    pose_.print(s + ".pose");
+  }
+
+  /* ************************************************************************* */
+  const Pose3& getPose(OptionalJacobian<6, 6> H) const {
+    if (H) {
+      H->setZero();
+      H->block(0, 0, 6, 6) = I_6x6;
+    }
+    return pose_;
+  }
+
+  /* ************************************************************************* */
+  static Point2 project_to_camera(const Point3& pc,
+      OptionalJacobian<2, 3> Dpoint = boost::none) {
+    double d = 1.0 / pc.z();
+    const double u = pc.x() * d, v = pc.y() * d;
+    if (Dpoint)
+      *Dpoint << d, 0.0, -u * d, 0.0, d, -v * d;
+    return Point2(u, v);
+  }
+
+  /* ************************************************************************* */
+  std::pair<Point2, bool> projectSafe(const Point3& pw) const {
+    const Point3 pc = pose().transform_to(pw);
+    const Point2 pn = project_to_camera(pc);
+    return std::make_pair(pn, pc.z() > 0);
+  }
+
+  /* ************************************************************************* */
+  Point2 project2(const Point3& point, OptionalJacobian<2, 6> Dpose,
+      OptionalJacobian<2, 3> Dpoint) const {
+
+    Matrix3 Rt; // calculated by transform_to if needed
+    const Point3 q = pose().transform_to(point, boost::none, Dpoint ? &Rt : 0);
+  #ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION
+    if (q.z() <= 0)
+    throw CheiralityException();
+  #endif
+    const Point2 pn = project_to_camera(q);
+
+    if (Dpose || Dpoint) {
+      const double d = 1.0 / q.z();
+      if (Dpose)
+        *Dpose = PinholeBase::Dpose(pn, d);
+      if (Dpoint)
+        *Dpoint = PinholeBase::Dpoint(pn, d, Rt);
+    }
+    return pn;
+  }
+
+  /* ************************************************************************* */
+  static Point3 backproject_from_camera(const Point2& p,
+      const double depth) {
+    return Point3(p.x() * depth, p.y() * depth, depth);
+  }
+
+#endif
+
+private:
+
+  /** Serialization function */
+  friend class boost::serialization::access;
+  template<class Archive>
+  void serialize(Archive & ar, const unsigned int version) {
+    ar & BOOST_SERIALIZATION_NVP(pose_);
+  }
+
+};
+// end of class PinholeBase
+
 /**
  * A Calibrated camera class [R|-R't], calibration K=I.
  * If calibration is known, it is more computationally efficient
@@ -38,13 +311,13 @@ public:
  * @addtogroup geometry
  * \nosubgrouping
  */
-class GTSAM_EXPORT CalibratedCamera {
+class GTSAM_EXPORT CalibratedCamera: public PinholeBase {
-private:
-  Pose3 pose_; // 6DOF pose
 
 public:
 
-  enum { dimension = 6 };
+  enum {
+    dimension = 6
+  };
 
   /// @name Standard Constructors
   /// @{
@@ -54,26 +327,40 @@ public:
   }
 
   /// construct with pose
-  explicit CalibratedCamera(const Pose3& pose);
+  explicit CalibratedCamera(const Pose3& pose) :
+      PinholeBase(pose) {
+  }
+
+  /// @}
+  /// @name Named Constructors
+  /// @{
+
+  /**
+   * Create a level camera at the given 2D pose and height
+   * @param pose2 specifies the location and viewing direction
+   * @param height specifies the height of the camera (along the positive Z-axis)
+   * (theta 0 = looking in direction of positive X axis)
+   */
+  static CalibratedCamera Level(const Pose2& pose2, double height);
+
+  /**
+   * Create a camera at the given eye position looking at a target point in the scene
+   * with the specified up direction vector.
+   * @param eye specifies the camera position
+   * @param target the point to look at
+   * @param upVector specifies the camera up direction vector,
+   *        doesn't need to be on the image plane nor orthogonal to the viewing axis
+   */
+  static CalibratedCamera Lookat(const Point3& eye, const Point3& target,
+      const Point3& upVector);
 
   /// @}
   /// @name Advanced Constructors
   /// @{
 
   /// construct from vector
-  explicit CalibratedCamera(const Vector &v);
+  explicit CalibratedCamera(const Vector &v) :
-
+      PinholeBase(v) {
-  /// @}
-  /// @name Testable
-  /// @{
-
-  virtual void print(const std::string& s = "") const {
-    pose_.print(s);
-  }
-
-  /// check equality to another camera
-  bool equals(const CalibratedCamera &camera, double tol = 1e-9) const {
-    return pose_.equals(camera.pose(), tol);
   }
 
   /// @}
@@ -84,19 +371,6 @@ public:
   virtual ~CalibratedCamera() {
   }
 
-  /// return pose
-  inline const Pose3& pose() const {
-    return pose_;
-  }
-
-  /**
-   * Create a level camera at the given 2D pose and height
-   * @param pose2 specifies the location and viewing direction
-   * @param height specifies the height of the camera (along the positive Z-axis)
-   * (theta 0 = looking in direction of positive X axis)
-   */
-  static CalibratedCamera Level(const Pose2& pose2, double height);
-
   /// @}
   /// @name Manifold
   /// @{
@@ -107,87 +381,68 @@ public:
   /// Return canonical coordinate
   Vector localCoordinates(const CalibratedCamera& T2) const;
 
-  /// Lie group dimensionality
+  /// @deprecated
   inline size_t dim() const {
     return 6;
   }
 
-  /// Lie group dimensionality
+  /// @deprecated
   inline static size_t Dim() {
     return 6;
   }
 
-  /* ************************************************************************* */
-  // measurement functions and derivatives
-  /* ************************************************************************* */
-
   /// @}
   /// @name Transformations and mesaurement functions
   /// @{
-  /**
-   * This function receives the camera pose and the landmark location and
-   * returns the location the point is supposed to appear in the image
-   * @param point a 3D point to be projected
-   * @param Dpose the optionally computed Jacobian with respect to pose
-   * @param Dpoint the optionally computed Jacobian with respect to the 3D point
-   * @return the intrinsic coordinates of the projected point
-   */
-  Point2 project(const Point3& point,
-      OptionalJacobian<2, 6> Dpose = boost::none,
-      OptionalJacobian<2, 3> Dpoint = boost::none) const;
 
   /**
-   * projects a 3-dimensional point in camera coordinates into the
+   * @deprecated
-   * camera and returns a 2-dimensional point, no calibration applied
+   * Use project2, which is more consistently named across Pinhole cameras
-   * With optional 2by3 derivative
    */
-  static Point2 project_to_camera(const Point3& cameraPoint,
+  Point2 project(const Point3& point, OptionalJacobian<2, 6> Dcamera =
-      OptionalJacobian<2, 3> H1 = boost::none);
+      boost::none, OptionalJacobian<2, 3> Dpoint = boost::none) const;
 
-  /**
+  /// backproject a 2-dimensional point to a 3-dimensional point at given depth
-   * backproject a 2-dimensional point to a 3-dimension point
+  Point3 backproject(const Point2& pn, double depth) const {
-   */
+    return pose().transform_from(backproject_from_camera(pn, depth));
-  static Point3 backproject_from_camera(const Point2& p, const double scale);
+  }
 
   /**
    * Calculate range to a landmark
    * @param point 3D location of landmark
-   * @param H1 optionally computed Jacobian with respect to pose
-   * @param H2 optionally computed Jacobian with respect to the 3D point
    * @return range (double)
    */
-  double range(const Point3& point, OptionalJacobian<1, 6> H1 = boost::none,
+  double range(const Point3& point,
-      OptionalJacobian<1, 3> H2 = boost::none) const {
+      OptionalJacobian<1, 6> Dcamera = boost::none,
-    return pose_.range(point, H1, H2);
+      OptionalJacobian<1, 3> Dpoint = boost::none) const {
+    return pose().range(point, Dcamera, Dpoint);
   }
 
   /**
    * Calculate range to another pose
    * @param pose Other SO(3) pose
-   * @param H1 optionally computed Jacobian with respect to pose
-   * @param H2 optionally computed Jacobian with respect to the 3D point
    * @return range (double)
    */
-  double range(const Pose3& pose, OptionalJacobian<1, 6> H1 = boost::none,
+  double range(const Pose3& pose, OptionalJacobian<1, 6> Dcamera = boost::none,
-      OptionalJacobian<1, 6> H2 = boost::none) const {
+      OptionalJacobian<1, 6> Dpose = boost::none) const {
-    return pose_.range(pose, H1, H2);
+    return this->pose().range(pose, Dcamera, Dpose);
   }
 
   /**
    * Calculate range to another camera
    * @param camera Other camera
-   * @param H1 optionally computed Jacobian with respect to pose
-   * @param H2 optionally computed Jacobian with respect to the 3D point
    * @return range (double)
    */
-  double range(const CalibratedCamera& camera, OptionalJacobian<1, 6> H1 =
+  double range(const CalibratedCamera& camera, //
-      boost::none, OptionalJacobian<1, 6> H2 = boost::none) const {
+      OptionalJacobian<1, 6> H1 = boost::none, //
-    return pose_.range(camera.pose_, H1, H2);
+      OptionalJacobian<1, 6> H2 = boost::none) const {
+    return pose().range(camera.pose(), H1, H2);
   }
 
+  /// @}
+
 private:
 
-  /// @}
   /// @name Advanced Interface
   /// @{
 
@@ -195,17 +450,22 @@ private:
   friend class boost::serialization::access;
   template<class Archive>
   void serialize(Archive & ar, const unsigned int version) {
-    ar & BOOST_SERIALIZATION_NVP(pose_);
+    ar
+        & boost::serialization::make_nvp("PinholeBase",
+            boost::serialization::base_object<PinholeBase>(*this));
   }
 
   /// @}
 };
 
 template<>
-struct traits<CalibratedCamera> : public internal::Manifold<CalibratedCamera> {};
+struct traits<CalibratedCamera> : public internal::Manifold<CalibratedCamera> {
+};
 
 template<>
-struct traits<const CalibratedCamera> : public internal::Manifold<CalibratedCamera> {};
+struct traits<const CalibratedCamera> : public internal::Manifold<
+    CalibratedCamera> {
+};
 
 }
 

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

@@ -18,32 +18,40 @@
 
 #pragma once
 
-#include <gtsam/geometry/CalibratedCamera.h>
+#include <gtsam/geometry/PinholePose.h>
-#include <gtsam/geometry/Pose2.h>
-#include <cmath>
 
 namespace gtsam {
 
 /**
  * A pinhole camera class that has a Pose3 and a Calibration.
+ * Use PinholePose if you will not be optimizing for Calibration
  * @addtogroup geometry
  * \nosubgrouping
  */
 template<typename Calibration>
-class PinholeCamera {
+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:
-  Pose3 pose_;
-  Calibration K_;
 
-  GTSAM_CONCEPT_MANIFOLD_TYPE(Calibration)
+  typedef PinholeBaseK<Calibration> Base; ///< base class has 3D pose as private member
+  Calibration K_; ///< Calibration, part of class now
 
   // Get dimensions of calibration type at compile time
   static const int DimK = FixedDimension<Calibration>::value;
 
 public:
 
-  enum { dimension = 6 + DimK };
+  enum {
+    dimension = 6 + DimK
+  }; ///< Dimension depends on calibration
 
   /// @name Standard Constructors
   /// @{
@@ -54,12 +62,12 @@ public:
 
   /** constructor with pose */
   explicit PinholeCamera(const Pose3& pose) :
-      pose_(pose) {
+      Base(pose) {
   }
 
   /** constructor with pose and calibration */
   PinholeCamera(const Pose3& pose, const Calibration& K) :
-      pose_(pose), K_(K) {
+      Base(pose), K_(K) {
   }
 
   /// @}
@@ -75,12 +83,7 @@ public:
    */
   static PinholeCamera Level(const Calibration &K, const Pose2& pose2,
       double height) {
-    const double st = sin(pose2.theta()), ct = cos(pose2.theta());
+    return PinholeCamera(Base::LevelPose(pose2, height), K);
-    const Point3 x(st, -ct, 0), y(0, 0, -1), z(ct, st, 0);
-    const Rot3 wRc(x, y, z);
-    const Point3 t(pose2.x(), pose2.y(), height);
-    const Pose3 pose3(wRc, t);
-    return PinholeCamera(pose3, K);
   }
 
   /// PinholeCamera::level with default calibration
@@ -99,28 +102,23 @@ public:
    */
   static PinholeCamera Lookat(const Point3& eye, const Point3& target,
       const Point3& upVector, const Calibration& K = Calibration()) {
-    Point3 zc = target - eye;
+    return PinholeCamera(Base::LookatPose(eye, target, upVector), K);
-    zc = zc / zc.norm();
-    Point3 xc = (-upVector).cross(zc); // minus upVector since yc is pointing down
-    xc = xc / xc.norm();
-    Point3 yc = zc.cross(xc);
-    Pose3 pose3(Rot3(xc, yc, zc), eye);
-    return PinholeCamera(pose3, K);
   }
 
   /// @}
   /// @name Advanced Constructors
   /// @{
 
-  explicit PinholeCamera(const Vector &v) {
+  /// Init from vector, can be 6D (default calibration) or dim
-    pose_ = Pose3::Expmap(v.head(6));
+  explicit PinholeCamera(const Vector &v) :
-    if (v.size() > 6) {
+      Base(v.head<6>()) {
-      K_ = Calibration(v.tail(DimK));
+    if (v.size() > 6)
-    }
+      K_ = Calibration(v.tail<DimK>());
   }
 
+  /// Init from Vector and calibration
   PinholeCamera(const Vector &v, const Vector &K) :
-      pose_(Pose3::Expmap(v)), K_(K) {
+      Base(v), K_(K) {
   }
 
   /// @}
@@ -128,14 +126,14 @@ public:
   /// @{
 
   /// assert equality up to a tolerance
-  bool equals(const PinholeCamera &camera, double tol = 1e-9) const {
+  bool equals(const Base &camera, double tol = 1e-9) const {
-    return pose_.equals(camera.pose(), tol)
+    const PinholeCamera* e = dynamic_cast<const PinholeCamera*>(&camera);
-        && K_.equals(camera.calibration(), tol);
+    return Base::equals(camera, tol) && K_.equals(e->calibration(), tol);
   }
 
   /// print
   void print(const std::string& s = "PinholeCamera") const {
-    pose_.print(s + ".pose");
+    Base::print(s);
     K_.print(s + ".calibration");
   }
 
@@ -147,31 +145,21 @@ public:
   }
 
   /// return pose
-  inline Pose3& pose() {
+  const Pose3& pose() const {
-    return pose_;
+    return Base::pose();
-  }
-
-  /// return pose, constant version
-  inline const Pose3& pose() const {
-    return pose_;
   }
 
   /// return pose, with derivative
-  inline const Pose3& getPose(gtsam::OptionalJacobian<6, dimension> H) const {
+  const Pose3& getPose(OptionalJacobian<6, dimension> H) const {
     if (H) {
       H->setZero();
       H->block(0, 0, 6, 6) = I_6x6;
     }
-    return pose_;
+    return Base::pose();
   }
 
   /// return calibration
-  inline Calibration& calibration() {
+  const Calibration& calibration() const {
-    return K_;
-  }
-
-  /// return calibration
-  inline const Calibration& calibration() const {
     return K_;
   }
 
@@ -179,13 +167,13 @@ public:
   /// @name Manifold
   /// @{
 
-  /// Manifold dimension
+  /// @deprecated
-  inline size_t dim() const {
+  size_t dim() const {
     return dimension;
   }
 
-  /// Manifold dimension
+  /// @deprecated
-  inline static size_t Dim() {
+  static size_t Dim() {
     return dimension;
   }
 
@@ -194,9 +182,9 @@ public:
   /// move a cameras according to d
   PinholeCamera retract(const Vector& d) const {
     if ((size_t) d.size() == 6)
-      return PinholeCamera(pose().retract(d), calibration());
+      return PinholeCamera(this->pose().retract(d), calibration());
     else
-      return PinholeCamera(pose().retract(d.head(6)),
+      return PinholeCamera(this->pose().retract(d.head(6)),
           calibration().retract(d.tail(calibration().dim())));
   }
 
@@ -204,7 +192,7 @@ public:
   VectorK6 localCoordinates(const PinholeCamera& T2) const {
     VectorK6 d;
     // TODO: why does d.head<6>() not compile??
-    d.head(6) = pose().localCoordinates(T2.pose());
+    d.head(6) = this->pose().localCoordinates(T2.pose());
     d.tail(DimK) = calibration().localCoordinates(T2.calibration());
     return d;
   }
@@ -218,32 +206,6 @@ public:
   /// @name Transformations and measurement functions
   /// @{
 
-  /**
-   * projects a 3-dimensional point in camera coordinates into the
-   * camera and returns a 2-dimensional point, no calibration applied
-   * @param P A point in camera coordinates
-   * @param Dpoint is the 2*3 Jacobian w.r.t. P
-   */
-  static Point2 project_to_camera(const Point3& P, //
-      OptionalJacobian<2, 3> Dpoint = boost::none) {
-#ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION
-    if (P.z() <= 0)
-      throw CheiralityException();
-#endif
-    double d = 1.0 / P.z();
-    const double u = P.x() * d, v = P.y() * d;
-    if (Dpoint)
-      *Dpoint << d, 0.0, -u * d, 0.0, d, -v * d;
-    return Point2(u, v);
-  }
-
-  /// Project a point into the image and check depth
-  inline std::pair<Point2, bool> projectSafe(const Point3& pw) const {
-    const Point3 pc = pose_.transform_to(pw);
-    const Point2 pn = project_to_camera(pc);
-    return std::make_pair(K_.uncalibrate(pn), pc.z() > 0);
-  }
-
   typedef Eigen::Matrix<double, 2, DimK> Matrix2K;
 
   /** project a point from world coordinate to the image
@@ -252,31 +214,25 @@ public:
    *  @param Dpoint is the Jacobian w.r.t. point3
    *  @param Dcal is the Jacobian w.r.t. calibration
    */
-  inline Point2 project(const Point3& pw, OptionalJacobian<2, 6> Dpose =
+  Point2 project(const Point3& pw, OptionalJacobian<2, 6> Dpose = boost::none,
-      boost::none, OptionalJacobian<2, 3> Dpoint = boost::none,
+      OptionalJacobian<2, 3> Dpoint = boost::none,
       OptionalJacobian<2, DimK> Dcal = boost::none) const {
 
-    // Transform to camera coordinates and check cheirality
+    // project to normalized coordinates
-    const Point3 pc = pose_.transform_to(pw);
+    const Point2 pn = PinholeBase::project2(pw, Dpose, Dpoint);
 
-    // Project to normalized image coordinates
+    // uncalibrate to pixel coordinates
-    const Point2 pn = project_to_camera(pc);
-
-    if (Dpose || Dpoint) {
-      const double z = pc.z(), d = 1.0 / z;
-
-      // uncalibration
     Matrix2 Dpi_pn;
-      const Point2 pi = K_.uncalibrate(pn, Dcal, Dpi_pn);
+    const Point2 pi = calibration().uncalibrate(pn, Dcal,
+        Dpose || Dpoint ? &Dpi_pn : 0);
 
-      // chain the Jacobian matrices
+    // If needed, apply chain rule
     if (Dpose)
-        calculateDpose(pn, d, Dpi_pn, *Dpose);
+      *Dpose = Dpi_pn * *Dpose;
     if (Dpoint)
-        calculateDpoint(pn, d, pose_.rotation().matrix(), Dpi_pn, *Dpoint);
+      *Dpoint = Dpi_pn * *Dpoint;
+
     return pi;
-    } else
-      return K_.uncalibrate(pn, Dcal);
   }
 
   /** project a point at infinity from world coordinate to the image
@@ -285,20 +241,19 @@ public:
    *  @param Dpoint is the Jacobian w.r.t. point3
    *  @param Dcal is the Jacobian w.r.t. calibration
    */
-  inline Point2 projectPointAtInfinity(const Point3& pw,
+  Point2 projectPointAtInfinity(const Point3& pw, OptionalJacobian<2, 6> Dpose =
-      OptionalJacobian<2, 6> Dpose = boost::none,
+      boost::none, OptionalJacobian<2, 2> Dpoint = boost::none,
-      OptionalJacobian<2, 2> Dpoint = boost::none,
       OptionalJacobian<2, DimK> Dcal = boost::none) const {
 
     if (!Dpose && !Dpoint && !Dcal) {
-      const Point3 pc = pose_.rotation().unrotate(pw); // get direction in camera frame (translation does not matter)
+      const Point3 pc = this->pose().rotation().unrotate(pw); // get direction in camera frame (translation does not matter)
-      const Point2 pn = project_to_camera(pc); // project the point to the camera
+      const Point2 pn = Base::project_to_camera(pc); // project the point to the camera
       return K_.uncalibrate(pn);
     }
 
     // world to camera coordinate
     Matrix3 Dpc_rot, Dpc_point;
-    const Point3 pc = pose_.rotation().unrotate(pw, Dpc_rot, Dpc_point);
+    const Point3 pc = this->pose().rotation().unrotate(pw, Dpc_rot, Dpc_point);
 
     Matrix36 Dpc_pose;
     Dpc_pose.setZero();
@@ -306,7 +261,7 @@ public:
 
     // camera to normalized image coordinate
     Matrix23 Dpn_pc; // 2*3
-    const Point2 pn = project_to_camera(pc, Dpn_pc);
+    const Point2 pn = Base::project_to_camera(pc, Dpn_pc);
 
     // uncalibration
     Matrix2 Dpi_pn; // 2*2
@@ -323,65 +278,40 @@ public:
 
   /** project a point from world coordinate to the image, fixed Jacobians
    *  @param pw is a point in the world coordinate
-   *  @param Dcamera is the Jacobian w.r.t. [pose3 calibration]
-   *  @param Dpoint is the Jacobian w.r.t. point3
    */
   Point2 project2(
       const Point3& pw, //
       OptionalJacobian<2, dimension> Dcamera = boost::none,
       OptionalJacobian<2, 3> Dpoint = boost::none) const {
 
-    const Point3 pc = pose_.transform_to(pw);
+    // project to normalized coordinates
-    const Point2 pn = project_to_camera(pc);
+    Matrix26 Dpose;
+    const Point2 pn = PinholeBase::project2(pw, Dpose, Dpoint);
 
-    if (!Dcamera && !Dpoint) {
+    // uncalibrate to pixel coordinates
-      return K_.uncalibrate(pn);
-    } else {
-      const double z = pc.z(), d = 1.0 / z;
-
-      // uncalibration
     Matrix2K Dcal;
     Matrix2 Dpi_pn;
-      const Point2 pi = K_.uncalibrate(pn, Dcal, Dpi_pn);
+    const Point2 pi = calibration().uncalibrate(pn, Dcamera ? &Dcal : 0,
+        Dcamera || Dpoint ? &Dpi_pn : 0);
+
+    // If needed, calculate derivatives
+    if (Dcamera)
+      *Dcamera << Dpi_pn * Dpose, Dcal;
+    if (Dpoint)
+      *Dpoint = Dpi_pn * (*Dpoint);
 
-      if (Dcamera) { // TODO why does leftCols<6>() not compile ??
-        calculateDpose(pn, d, Dpi_pn, (*Dcamera).leftCols(6));
-        (*Dcamera).rightCols(DimK) = Dcal; // Jacobian wrt calib
-      }
-      if (Dpoint) {
-        calculateDpoint(pn, d, pose_.rotation().matrix(), Dpi_pn, *Dpoint);
-      }
     return pi;
   }
-  }
-
-  /// backproject a 2-dimensional point to a 3-dimensional point at given depth
-  inline Point3 backproject(const Point2& p, double depth) const {
-    const Point2 pn = K_.calibrate(p);
-    const Point3 pc(pn.x() * depth, pn.y() * depth, depth);
-    return pose_.transform_from(pc);
-  }
-
-  /// backproject a 2-dimensional point to a 3-dimensional point at infinity
-  inline Point3 backprojectPointAtInfinity(const Point2& p) const {
-    const Point2 pn = K_.calibrate(p);
-    const Point3 pc(pn.x(), pn.y(), 1.0); //by convention the last element is 1
-    return pose_.rotation().rotate(pc);
-  }
 
   /**
    * Calculate range to a landmark
    * @param point 3D location of landmark
-   * @param Dcamera the optionally computed Jacobian with respect to pose
-   * @param Dpoint the optionally computed Jacobian with respect to the landmark
    * @return range (double)
    */
-  double range(
+  double range(const Point3& point, OptionalJacobian<1, dimension> Dcamera =
-      const Point3& point, //
+      boost::none, OptionalJacobian<1, 3> Dpoint = boost::none) const {
-      OptionalJacobian<1, dimension> Dcamera = boost::none,
-      OptionalJacobian<1, 3> Dpoint = boost::none) const {
     Matrix16 Dpose_;
-    double result = pose_.range(point, Dcamera ? &Dpose_ : 0, Dpoint);
+    double result = this->pose().range(point, Dcamera ? &Dpose_ : 0, Dpoint);
     if (Dcamera)
       *Dcamera << Dpose_, Eigen::Matrix<double, 1, DimK>::Zero();
     return result;
@@ -390,16 +320,12 @@ public:
   /**
    * Calculate range to another pose
    * @param pose Other SO(3) pose
-   * @param Dcamera the optionally computed Jacobian with respect to pose
-   * @param Dpose2 the optionally computed Jacobian with respect to the other pose
    * @return range (double)
    */
-  double range(
+  double range(const Pose3& pose, OptionalJacobian<1, dimension> Dcamera =
-      const Pose3& pose, //
+      boost::none, OptionalJacobian<1, 6> Dpose = boost::none) const {
-      OptionalJacobian<1, dimension> Dcamera = boost::none,
-      OptionalJacobian<1, 6> Dpose = boost::none) const {
     Matrix16 Dpose_;
-    double result = pose_.range(pose, Dcamera ? &Dpose_ : 0, Dpose);
+    double result = this->pose().range(pose, Dcamera ? &Dpose_ : 0, Dpose);
     if (Dcamera)
       *Dcamera << Dpose_, Eigen::Matrix<double, 1, DimK>::Zero();
     return result;
@@ -408,26 +334,21 @@ public:
   /**
    * Calculate range to another camera
    * @param camera Other camera
-   * @param Dcamera the optionally computed Jacobian with respect to pose
-   * @param Dother the optionally computed Jacobian with respect to the other camera
    * @return range (double)
    */
   template<class CalibrationB>
-  double range(
+  double range(const PinholeCamera<CalibrationB>& camera,
-      const PinholeCamera<CalibrationB>& camera, //
       OptionalJacobian<1, dimension> Dcamera = boost::none,
-//      OptionalJacobian<1, 6 + traits::dimension<CalibrationB>::value> Dother =
+      boost::optional<Matrix&> Dother = boost::none) const {
-       boost::optional<Matrix&> Dother =
-          boost::none) const {
     Matrix16 Dcamera_, Dother_;
-    double result = pose_.range(camera.pose(), Dcamera ? &Dcamera_ : 0,
+    double result = this->pose().range(camera.pose(), Dcamera ? &Dcamera_ : 0,
         Dother ? &Dother_ : 0);
     if (Dcamera) {
       Dcamera->resize(1, 6 + DimK);
       *Dcamera << Dcamera_, Eigen::Matrix<double, 1, DimK>::Zero();
     }
     if (Dother) {
-      Dother->resize(1, 6+CalibrationB::dimension);
+      Dother->resize(1, 6 + CalibrationB::dimension);
       Dother->setZero();
       Dother->block(0, 0, 1, 6) = Dother_;
     }
@@ -437,12 +358,9 @@ public:
   /**
    * Calculate range to another camera
    * @param camera Other camera
-   * @param Dcamera the optionally computed Jacobian with respect to pose
-   * @param Dother the optionally computed Jacobian with respect to the other camera
    * @return range (double)
    */
-  double range(
+  double range(const CalibratedCamera& camera,
-      const CalibratedCamera& camera, //
       OptionalJacobian<1, dimension> Dcamera = boost::none,
       OptionalJacobian<1, 6> Dother = boost::none) const {
     return range(camera.pose(), Dcamera, Dother);
@@ -450,65 +368,26 @@ public:
 
 private:
 
-  /**
-   * Calculate Jacobian with respect to pose
-   * @param pn projection in normalized coordinates
-   * @param d disparity (inverse depth)
-   * @param Dpi_pn derivative of uncalibrate with respect to pn
-   * @param Dpose Output argument, can be matrix or block, assumed right size !
-   * See http://eigen.tuxfamily.org/dox/TopicFunctionTakingEigenTypes.html
-   */
-  template<typename Derived>
-  static void calculateDpose(const Point2& pn, double d, const Matrix2& Dpi_pn,
-      Eigen::MatrixBase<Derived> const & Dpose) {
-    // optimized version of derivatives, see CalibratedCamera.nb
-    const double u = pn.x(), v = pn.y();
-    double uv = u * v, uu = u * u, vv = v * v;
-    Matrix26 Dpn_pose;
-    Dpn_pose << uv, -1 - uu, v, -d, 0, d * u, 1 + vv, -uv, -u, 0, -d, d * v;
-    assert(Dpose.rows()==2 && Dpose.cols()==6);
-    const_cast<Eigen::MatrixBase<Derived>&>(Dpose) = //
-        Dpi_pn * Dpn_pose;
-  }
-
-  /**
-   * Calculate Jacobian with respect to point
-   * @param pn projection in normalized coordinates
-   * @param d disparity (inverse depth)
-   * @param Dpi_pn derivative of uncalibrate with respect to pn
-   * @param Dpoint Output argument, can be matrix or block, assumed right size !
-   * See http://eigen.tuxfamily.org/dox/TopicFunctionTakingEigenTypes.html
-   */
-  template<typename Derived>
-  static void calculateDpoint(const Point2& pn, double d, const Matrix3& R,
-      const Matrix2& Dpi_pn, Eigen::MatrixBase<Derived> const & Dpoint) {
-    // optimized version of derivatives, see CalibratedCamera.nb
-    const double u = pn.x(), v = pn.y();
-    Matrix23 Dpn_point;
-    Dpn_point << //
-        R(0, 0) - u * R(0, 2), R(1, 0) - u * R(1, 2), R(2, 0) - u * R(2, 2), //
-    /**/R(0, 1) - v * R(0, 2), R(1, 1) - v * R(1, 2), R(2, 1) - v * R(2, 2);
-    Dpn_point *= d;
-    assert(Dpoint.rows()==2 && Dpoint.cols()==3);
-    const_cast<Eigen::MatrixBase<Derived>&>(Dpoint) = //
-        Dpi_pn * Dpn_point;
-  }
-
   /** Serialization function */
   friend class boost::serialization::access;
   template<class Archive>
   void serialize(Archive & ar, const unsigned int version) {
-    ar & BOOST_SERIALIZATION_NVP(pose_);
+    ar
+        & boost::serialization::make_nvp("PinholeBaseK",
+            boost::serialization::base_object<Base>(*this));
     ar & BOOST_SERIALIZATION_NVP(K_);
   }
 
 };
 
+template<typename Calibration>
+struct traits<PinholeCamera<Calibration> > : public internal::Manifold<
+    PinholeCamera<Calibration> > {
+};
 
 template<typename Calibration>
-struct traits< PinholeCamera<Calibration> > : public internal::Manifold<PinholeCamera<Calibration> > {};
+struct traits<const PinholeCamera<Calibration> > : public internal::Manifold<
-
+    PinholeCamera<Calibration> > {
-template<typename Calibration>
+};
-struct traits< const PinholeCamera<Calibration> > : public internal::Manifold<PinholeCamera<Calibration> > {};
 
 } // \ gtsam

gtsam/geometry/PinholePose.h

@@ -0,0 +1,344 @@
+/* ----------------------------------------------------------------------------
+
+ * 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   PinholePose.h
+ * @brief  Pinhole camera with known calibration
+ * @author Yong-Dian Jian
+ * @author Frank Dellaert
+ * @date   Feb 20, 2015
+ */
+
+#pragma once
+
+#include <gtsam/geometry/CalibratedCamera.h>
+#include <gtsam/geometry/Point2.h>
+#include <boost/make_shared.hpp>
+
+namespace gtsam {
+
+/**
+ * A pinhole camera class that has a Pose3 and a *fixed* Calibration.
+ * @addtogroup geometry
+ * \nosubgrouping
+ */
+template<typename Calibration>
+class GTSAM_EXPORT PinholeBaseK: public PinholeBase {
+
+  GTSAM_CONCEPT_MANIFOLD_TYPE(Calibration)
+
+public  :
+
+  /// @name Standard Constructors
+  /// @{
+
+  /** default constructor */
+  PinholeBaseK() {
+  }
+
+  /** constructor with pose */
+  explicit PinholeBaseK(const Pose3& pose) :
+  PinholeBase(pose) {
+  }
+
+  /// @}
+  /// @name Advanced Constructors
+  /// @{
+
+  explicit PinholeBaseK(const Vector &v) :
+  PinholeBase(v) {
+  }
+
+  /// @}
+  /// @name Standard Interface
+  /// @{
+
+  virtual ~PinholeBaseK() {
+  }
+
+  /// return calibration
+  virtual const Calibration& calibration() const = 0;
+
+  /// @}
+  /// @name Transformations and measurement functions
+  /// @{
+
+  /// Project a point into the image and check depth
+  std::pair<Point2, bool> projectSafe(const Point3& pw) const {
+    std::pair<Point2, bool> pn = PinholeBase::projectSafe(pw);
+    pn.first = calibration().uncalibrate(pn.first);
+    return pn;
+  }
+
+  /** project a point from world coordinate to the image, fixed Jacobians
+   *  @param pw is a point in the world coordinates
+   */
+  Point2 project2(const Point3& pw, OptionalJacobian<2, 6> Dpose = boost::none,
+      OptionalJacobian<2, 3> Dpoint = boost::none) const {
+
+    // project to normalized coordinates
+    const Point2 pn = PinholeBase::project2(pw, Dpose, Dpoint);
+
+    // uncalibrate to pixel coordinates
+    Matrix2 Dpi_pn;
+    const Point2 pi = calibration().uncalibrate(pn, boost::none,
+        Dpose || Dpoint ? &Dpi_pn : 0);
+
+    // If needed, apply chain rule
+    if (Dpose) *Dpose = Dpi_pn * (*Dpose);
+    if (Dpoint) *Dpoint = Dpi_pn * (*Dpoint);
+
+    return pi;
+  }
+
+  /// backproject a 2-dimensional point to a 3-dimensional point at given depth
+  Point3 backproject(const Point2& p, double depth) const {
+    const Point2 pn = calibration().calibrate(p);
+    return pose().transform_from(backproject_from_camera(pn, depth));
+  }
+
+  /// backproject a 2-dimensional point to a 3-dimensional point at infinity
+  Point3 backprojectPointAtInfinity(const Point2& p) const {
+    const Point2 pn = calibration().calibrate(p);
+    const Point3 pc(pn.x(), pn.y(), 1.0); //by convention the last element is 1
+    return pose().rotation().rotate(pc);
+  }
+
+  /**
+   * Calculate range to a landmark
+   * @param point 3D location of landmark
+   * @return range (double)
+   */
+  double range(const Point3& point,
+      OptionalJacobian<1, 6> Dcamera = boost::none,
+      OptionalJacobian<1, 3> Dpoint = boost::none) const {
+    return pose().range(point, Dcamera, Dpoint);
+  }
+
+  /**
+   * Calculate range to another pose
+   * @param pose Other SO(3) pose
+   * @return range (double)
+   */
+  double range(const Pose3& pose, OptionalJacobian<1, 6> Dcamera = boost::none,
+      OptionalJacobian<1, 6> Dpose = boost::none) const {
+    return this->pose().range(pose, Dcamera, Dpose);
+  }
+
+  /**
+   * Calculate range to a CalibratedCamera
+   * @param camera Other camera
+   * @return range (double)
+   */
+  double range(const CalibratedCamera& camera, OptionalJacobian<1, 6> Dcamera =
+      boost::none, OptionalJacobian<1, 6> Dother = boost::none) const {
+    return pose().range(camera.pose(), Dcamera, Dother);
+  }
+
+  /**
+   * Calculate range to a PinholePoseK derived class
+   * @param camera Other camera
+   * @return range (double)
+   */
+  template<class CalibrationB>
+  double range(const PinholeBaseK<CalibrationB>& camera,
+      OptionalJacobian<1, 6> Dcamera = boost::none,
+      OptionalJacobian<1, 6> Dother = boost::none) const {
+    return pose().range(camera.pose(), Dcamera, Dother);
+  }
+
+private:
+
+  /** Serialization function */
+  friend class boost::serialization::access;
+  template<class Archive>
+  void serialize(Archive & ar, const unsigned int version) {
+    ar
+    & boost::serialization::make_nvp("PinholeBase",
+        boost::serialization::base_object<PinholeBase>(*this));
+  }
+
+};
+// end of class PinholeBaseK
+
+/**
+ * A pinhole camera class that has a Pose3 and a *fixed* Calibration.
+ * Instead of using this class, one might consider calibrating the measurements
+ * and using CalibratedCamera, which would then be faster.
+ * @addtogroup geometry
+ * \nosubgrouping
+ */
+template<typename Calibration>
+class GTSAM_EXPORT PinholePose: public PinholeBaseK<Calibration> {
+
+private:
+
+  typedef PinholeBaseK<Calibration> Base; ///< base class has 3D pose as private member
+  boost::shared_ptr<Calibration> K_; ///< shared pointer to fixed calibration
+
+public:
+
+  enum {
+    dimension = 6
+  }; ///< There are 6 DOF to optimize for
+
+  /// @name Standard Constructors
+  /// @{
+
+  /** default constructor */
+  PinholePose() {
+  }
+
+  /** constructor with pose, uses default calibration */
+  explicit PinholePose(const Pose3& pose) :
+      Base(pose), K_(new Calibration()) {
+  }
+
+  /** constructor with pose and calibration */
+  PinholePose(const Pose3& pose, const boost::shared_ptr<Calibration>& K) :
+      Base(pose), K_(K) {
+  }
+
+  /// @}
+  /// @name Named Constructors
+  /// @{
+
+  /**
+   * Create a level camera at the given 2D pose and height
+   * @param K the calibration
+   * @param pose2 specifies the location and viewing direction
+   * (theta 0 = looking in direction of positive X axis)
+   * @param height camera height
+   */
+  static PinholePose Level(const boost::shared_ptr<Calibration>& K,
+      const Pose2& pose2, double height) {
+    return PinholePose(Base::LevelPose(pose2, height), K);
+  }
+
+  /// PinholePose::level with default calibration
+  static PinholePose Level(const Pose2& pose2, double height) {
+    return PinholePose::Level(boost::make_shared<Calibration>(), pose2, height);
+  }
+
+  /**
+   * Create a camera at the given eye position looking at a target point in the scene
+   * with the specified up direction vector.
+   * @param eye specifies the camera position
+   * @param target the point to look at
+   * @param upVector specifies the camera up direction vector,
+   *        doesn't need to be on the image plane nor orthogonal to the viewing axis
+   * @param K optional calibration parameter
+   */
+  static PinholePose Lookat(const Point3& eye, const Point3& target,
+      const Point3& upVector, const boost::shared_ptr<Calibration>& K =
+          boost::make_shared<Calibration>()) {
+    return PinholePose(Base::LookatPose(eye, target, upVector), K);
+  }
+
+  /// @}
+  /// @name Advanced Constructors
+  /// @{
+
+  /// Init from 6D vector
+  explicit PinholePose(const Vector &v) :
+      Base(v), K_(new Calibration()) {
+  }
+
+  /// Init from Vector and calibration
+  PinholePose(const Vector &v, const Vector &K) :
+      Base(v), K_(new Calibration(K)) {
+  }
+
+  /// @}
+  /// @name Testable
+  /// @{
+
+  /// assert equality up to a tolerance
+  bool equals(const Base &camera, double tol = 1e-9) const {
+    const PinholePose* e = dynamic_cast<const PinholePose*>(&camera);
+    return Base::equals(camera, tol) && K_->equals(e->calibration(), tol);
+  }
+
+  /// print
+  void print(const std::string& s = "PinholePose") const {
+    Base::print(s);
+    K_->print(s + ".calibration");
+  }
+
+  /// @}
+  /// @name Standard Interface
+  /// @{
+
+  virtual ~PinholePose() {
+  }
+
+  /// return calibration
+  virtual const Calibration& calibration() const {
+    return *K_;
+  }
+
+  /// @}
+  /// @name Manifold
+  /// @{
+
+  /// @deprecated
+  size_t dim() const {
+    return 6;
+  }
+
+  /// @deprecated
+  static size_t Dim() {
+    return 6;
+  }
+
+  /// move a cameras according to d
+  PinholePose retract(const Vector6& d) const {
+    return PinholePose(Base::pose().retract(d), K_);
+  }
+
+  /// return canonical coordinate
+  Vector6 localCoordinates(const PinholePose& p) const {
+    return Base::pose().localCoordinates(p.Base::pose());
+  }
+
+  /// for Canonical
+  static PinholePose identity() {
+    return PinholePose(); // assumes that the default constructor is valid
+  }
+
+  /// @}
+
+private:
+
+  /** Serialization function */
+  friend class boost::serialization::access;
+  template<class Archive>
+  void serialize(Archive & ar, const unsigned int version) {
+    ar
+        & boost::serialization::make_nvp("PinholeBaseK",
+            boost::serialization::base_object<Base>(*this));
+    ar & BOOST_SERIALIZATION_NVP(K_);
+  }
+
+};
+// end of class PinholePose
+
+template<typename Calibration>
+struct traits<PinholePose<Calibration> > : public internal::Manifold<
+    PinholePose<Calibration> > {
+};
+
+template<typename Calibration>
+struct traits<const PinholePose<Calibration> > : public internal::Manifold<
+    PinholePose<Calibration> > {
+};
+
+} // \ gtsam

gtsam/geometry/StereoCamera.cpp

@@ -29,16 +29,15 @@ namespace gtsam {
   }
 
   /* ************************************************************************* */
-  StereoPoint2 StereoCamera::project(const Point3& point,
+  StereoPoint2 StereoCamera::project(const Point3& point) const {
-      OptionalJacobian<3,6> H1, OptionalJacobian<3,3> H2,
+    return project2(point);
-      OptionalJacobian<3,6> H3) const {
+  }
+
+  /* ************************************************************************* */
+  StereoPoint2 StereoCamera::project2(const Point3& point,
+      OptionalJacobian<3,6> H1, OptionalJacobian<3,3> H2) const {
 
-#ifdef STEREOCAMERA_CHAIN_RULE
-    const Point3 q = leftCamPose_.transform_to(point, H1, H2);
-#else
-    // omit derivatives
     const Point3 q = leftCamPose_.transform_to(point);
-#endif
 
     if ( q.z() <= 0 ) throw StereoCheiralityException();
 
@@ -56,12 +55,6 @@ namespace gtsam {
 
     // check if derivatives need to be computed
     if (H1 || H2) {
-#ifdef STEREOCAMERA_CHAIN_RULE
-      // just implement chain rule
-      Matrix3 D_project_point = Dproject_to_stereo_camera1(q); // 3x3 Jacobian
-      if (H1) *H1 = D_project_point*(*H1);
-      if (H2) *H2 = D_project_point*(*H2);
-#else
       // optimized version, see StereoCamera.nb
       if (H1) {
         const double v1 = v/fy, v2 = fx*v1, dx=d*x;
@@ -76,10 +69,6 @@ namespace gtsam {
                 fy*R(0, 1) - R(0, 2)*v , fy*R(1, 1) - R(1, 2)*v , fy*R(2, 1) - R(2, 2)*v;
         *H2 << d * (*H2);
       }
-#endif
-      if (H3)
-        // TODO, this is set to zero, as Cal3_S2Stereo cannot be optimized yet
-        H3->setZero();
     }
 
     // finally translate
@@ -87,15 +76,23 @@ namespace gtsam {
   }
 
   /* ************************************************************************* */
-  Matrix3 StereoCamera::Dproject_to_stereo_camera1(const Point3& P) const {
+  StereoPoint2 StereoCamera::project(const Point3& point,
-    double d = 1.0 / P.z(), d2 = d*d;
+      OptionalJacobian<3,6> H1, OptionalJacobian<3,3> H2,
-    const Cal3_S2Stereo& K = *K_;
+      OptionalJacobian<3,0> H3) const {
-    double f_x = K.fx(), f_y = K.fy(), b = K.baseline();
+    if (H3)
-    Matrix3 m;
+    throw std::runtime_error(
-    m << f_x*d,   0.0, -d2*f_x* P.x(),
+        "StereoCamera::project does not support third derivative - BTW use project2");
-         f_x*d,   0.0, -d2*f_x*(P.x() - b),
+    return project2(point,H1,H2);
-           0.0, f_y*d, -d2*f_y* P.y();
+  }
-    return m;
+
+  /* ************************************************************************* */
+  Point3 StereoCamera::backproject(const StereoPoint2& z) const {
+    Vector measured = z.vector();
+    double Z = K_->baseline() * K_->fx() / (measured[0] - measured[1]);
+    double X = Z * (measured[0] - K_->px()) / K_->fx();
+    double Y = Z * (measured[2] - K_->py()) / K_->fy();
+    Point3 world_point = leftCamPose_.transform_from(Point3(X, Y, Z));
+    return world_point;
   }
 
 }

gtsam/geometry/StereoCamera.h

@@ -17,9 +17,6 @@
 
 #pragma once
 
-#include <boost/tuple/tuple.hpp>
-
-#include <gtsam/base/DerivedValue.h>
 #include <gtsam/geometry/Cal3_S2Stereo.h>
 #include <gtsam/geometry/Pose3.h>
 #include <gtsam/geometry/StereoPoint2.h>
@@ -28,32 +25,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 +74,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,42 +114,46 @@ public:
   /// @name Standard Interface
   /// @{
 
+  /// pose
   const Pose3& pose() const {
     return leftCamPose_;
   }
 
+  /// baseline
   double baseline() const {
     return K_->baseline();
   }
 
-  /*
+  /// Project 3D point to StereoPoint2 (uL,uR,v)
-   * project 3D point and compute optional derivatives
+  StereoPoint2 project(const Point3& point) const;
+
+  /** Project 3D point and compute optional derivatives
+   * @param H1 derivative with respect to pose
+   * @param H2 derivative with respect to point
+   */
+  StereoPoint2 project2(const Point3& point, OptionalJacobian<3, 6> H1 =
+      boost::none, OptionalJacobian<3, 3> H2 = boost::none) const;
+
+  /// back-project a measurement
+  Point3 backproject(const StereoPoint2& z) const;
+
+  /// @}
+  /// @name Deprecated
+  /// @{
+
+  /** Project 3D point and compute optional derivatives
+   * @deprecated, use project2 - this class has fixed calibration
    * @param H1 derivative with respect to pose
    * @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,
+      OptionalJacobian<3, 3> H2 = boost::none, OptionalJacobian<3, 0> H3 =
-      OptionalJacobian<3, 3> H2 = boost::none,
+          boost::none) const;
-      OptionalJacobian<3, 6> H3 = boost::none) const;
-
-  /**
-   *
-   */
-  Point3 backproject(const StereoPoint2& z) const {
-    Vector measured = z.vector();
-    double Z = K_->baseline()*K_->fx()/(measured[0]-measured[1]);
-    double X = Z *(measured[0]- K_->px()) / K_->fx();
-    double Y = Z *(measured[2]- K_->py()) / K_->fy();
-    Point3 world_point = leftCamPose_.transform_from(Point3(X, Y, Z));
-    return world_point;
-  }
 
   /// @}
 
 private:
-  /** utility functions */
-  Matrix3 Dproject_to_stereo_camera1(const Point3& P) const;
 
   friend class boost::serialization::access;
   template<class Archive>
@@ -147,8 +165,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/testCalibratedCamera.cpp

@@ -29,6 +29,7 @@ using namespace gtsam;
 
 GTSAM_CONCEPT_MANIFOLD_INST(CalibratedCamera)
 
+// Camera situated at 0.5 meters high, looking down
 static const Pose3 pose1((Matrix)(Matrix(3,3) <<
               1., 0., 0.,
               0.,-1., 0.,
@@ -97,24 +98,37 @@ TEST( CalibratedCamera, Dproject_to_camera1) {
 }
 
 /* ************************************************************************* */
-static Point2 project2(const Pose3& pose, const Point3& point) {
+static Point2 project2(const CalibratedCamera& camera, const Point3& point) {
-  return CalibratedCamera(pose).project(point);
+  return camera.project(point);
 }
 
 TEST( CalibratedCamera, Dproject_point_pose)
 {
   Matrix Dpose, Dpoint;
   Point2 result = camera.project(point1, Dpose, Dpoint);
-  Matrix numerical_pose  = numericalDerivative21(project2, pose1, point1);
+  Matrix numerical_pose  = numericalDerivative21(project2, camera, point1);
-  Matrix numerical_point = numericalDerivative22(project2, pose1, point1);
+  Matrix numerical_point = numericalDerivative22(project2, camera, point1);
   CHECK(assert_equal(Point3(-0.08, 0.08, 0.5), camera.pose().transform_to(point1)));
   CHECK(assert_equal(Point2(-.16,  .16), result));
   CHECK(assert_equal(numerical_pose,  Dpose, 1e-7));
   CHECK(assert_equal(numerical_point, Dpoint, 1e-7));
 }
 
+/* ************************************************************************* */
+// Add a test with more arbitrary rotation
+TEST( CalibratedCamera, Dproject_point_pose2)
+{
+  static const Pose3 pose1(Rot3::ypr(0.1, -0.1, 0.4), Point3(0, 0, -10));
+  static const CalibratedCamera camera(pose1);
+  Matrix Dpose, Dpoint;
+  camera.project(point1, Dpose, Dpoint);
+  Matrix numerical_pose  = numericalDerivative21(project2, camera, point1);
+  Matrix numerical_point = numericalDerivative22(project2, camera, point1);
+  CHECK(assert_equal(numerical_pose,  Dpose, 1e-7));
+  CHECK(assert_equal(numerical_point, Dpoint, 1e-7));
+}
+
 /* ************************************************************************* */
 int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
 /* ************************************************************************* */
 
-

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/geometry/tests/testPinholeCamera.cpp

@@ -15,12 +15,14 @@
  * @brief test PinholeCamera class
  */
 
-#include <CppUnitLite/TestHarness.h>
-#include <gtsam/base/Testable.h>
-#include <gtsam/base/numericalDerivative.h>
 #include <gtsam/geometry/PinholeCamera.h>
 #include <gtsam/geometry/Cal3_S2.h>
 #include <gtsam/geometry/Cal3Bundler.h>
+#include <gtsam/geometry/Pose2.h>
+#include <gtsam/base/Testable.h>
+#include <gtsam/base/numericalDerivative.h>
+
+#include <CppUnitLite/TestHarness.h>
 
 #include <cmath>
 #include <iostream>
@@ -236,6 +238,20 @@ TEST( PinholeCamera, Dproject2)
   EXPECT(assert_equal(Hexpected2,  Dpoint,  1e-7));
 }
 
+/* ************************************************************************* */
+// Add a test with more arbitrary rotation
+TEST( PinholeCamera, Dproject3)
+{
+  static const Pose3 pose1(Rot3::ypr(0.1, -0.1, 0.4), Point3(0, 0, -10));
+  static const Camera camera(pose1);
+  Matrix Dpose, Dpoint;
+  camera.project2(point1, Dpose, Dpoint);
+  Matrix numerical_pose  = numericalDerivative21(project4, camera, point1);
+  Matrix numerical_point = numericalDerivative22(project4, camera, point1);
+  CHECK(assert_equal(numerical_pose,  Dpose, 1e-7));
+  CHECK(assert_equal(numerical_point, Dpoint, 1e-7));
+}
+
 /* ************************************************************************* */
 static double range0(const Camera& camera, const Point3& point) {
   return camera.range(point);

gtsam/geometry/tests/testPinholePose.cpp

@@ -0,0 +1,259 @@
+/* ----------------------------------------------------------------------------
+
+ * 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   testPinholePose.cpp
+ * @author Frank Dellaert
+ * @brief  test PinholePose class
+ * @date   Feb 20, 2015
+ */
+
+#include <gtsam/geometry/PinholePose.h>
+#include <gtsam/geometry/Cal3_S2.h>
+#include <gtsam/geometry/Pose2.h>
+#include <gtsam/geometry/Cal3Bundler.h>
+#include <gtsam/base/Testable.h>
+#include <gtsam/base/numericalDerivative.h>
+
+#include <CppUnitLite/TestHarness.h>
+
+#include <cmath>
+#include <iostream>
+
+using namespace std;
+using namespace gtsam;
+
+typedef PinholePose<Cal3_S2> Camera;
+
+static const Cal3_S2::shared_ptr K = boost::make_shared<Cal3_S2>(625, 625, 0, 0, 0);
+
+static const Pose3 pose(Matrix3(Vector3(1, -1, -1).asDiagonal()), Point3(0, 0, 0.5));
+static const Camera camera(pose, K);
+
+static const Pose3 pose1(Rot3(), Point3(0, 1, 0.5));
+static const Camera camera1(pose1, K);
+
+static const Point3 point1(-0.08,-0.08, 0.0);
+static const Point3 point2(-0.08, 0.08, 0.0);
+static const Point3 point3( 0.08, 0.08, 0.0);
+static const Point3 point4( 0.08,-0.08, 0.0);
+
+static const Point3 point1_inf(-0.16,-0.16, -1.0);
+static const Point3 point2_inf(-0.16, 0.16, -1.0);
+static const Point3 point3_inf( 0.16, 0.16, -1.0);
+static const Point3 point4_inf( 0.16,-0.16, -1.0);
+
+/* ************************************************************************* */
+TEST( PinholePose, constructor)
+{
+  EXPECT(assert_equal( pose, camera.pose()));
+}
+
+//******************************************************************************
+TEST(PinholeCamera, Pose) {
+
+  Matrix actualH;
+  EXPECT(assert_equal(pose, camera.getPose(actualH)));
+
+  // Check derivative
+  boost::function<Pose3(Camera)> f = //
+      boost::bind(&Camera::getPose,_1,boost::none);
+  Matrix numericalH = numericalDerivative11<Pose3,Camera>(f,camera);
+  EXPECT(assert_equal(numericalH, actualH, 1e-9));
+}
+
+/* ************************************************************************* */
+TEST( PinholePose, lookat)
+{
+  // Create a level camera, looking in Y-direction
+  Point3 C(10.0,0.0,0.0);
+  Camera camera = Camera::Lookat(C, Point3(), Point3(0.0,0.0,1.0));
+
+  // expected
+  Point3 xc(0,1,0),yc(0,0,-1),zc(-1,0,0);
+  Pose3 expected(Rot3(xc,yc,zc),C);
+  EXPECT(assert_equal( camera.pose(), expected));
+
+  Point3 C2(30.0,0.0,10.0);
+  Camera camera2 = Camera::Lookat(C2, Point3(), Point3(0.0,0.0,1.0));
+
+  Matrix R = camera2.pose().rotation().matrix();
+  Matrix I = trans(R)*R;
+  EXPECT(assert_equal(I, eye(3)));
+}
+
+/* ************************************************************************* */
+TEST( PinholePose, project)
+{
+  EXPECT(assert_equal( camera.project2(point1), Point2(-100,  100) ));
+  EXPECT(assert_equal( camera.project2(point2), Point2(-100, -100) ));
+  EXPECT(assert_equal( camera.project2(point3), Point2( 100, -100) ));
+  EXPECT(assert_equal( camera.project2(point4), Point2( 100,  100) ));
+}
+
+/* ************************************************************************* */
+TEST( PinholePose, backproject)
+{
+  EXPECT(assert_equal( camera.backproject(Point2(-100,  100), 0.5),  point1));
+  EXPECT(assert_equal( camera.backproject(Point2(-100, -100), 0.5),  point2));
+  EXPECT(assert_equal( camera.backproject(Point2( 100, -100), 0.5),  point3));
+  EXPECT(assert_equal( camera.backproject(Point2( 100,  100), 0.5),  point4));
+}
+
+/* ************************************************************************* */
+TEST( PinholePose, backprojectInfinity)
+{
+  EXPECT(assert_equal( camera.backprojectPointAtInfinity(Point2(-100,  100)),  point1_inf));
+  EXPECT(assert_equal( camera.backprojectPointAtInfinity(Point2(-100, -100)),  point2_inf));
+  EXPECT(assert_equal( camera.backprojectPointAtInfinity(Point2( 100, -100)),  point3_inf));
+  EXPECT(assert_equal( camera.backprojectPointAtInfinity(Point2( 100,  100)),  point4_inf));
+}
+
+/* ************************************************************************* */
+TEST( PinholePose, backproject2)
+{
+  Point3 origin;
+  Rot3 rot(1., 0., 0., 0., 0., 1., 0., -1., 0.); // a camera1 looking down
+  Camera camera(Pose3(rot, origin), K);
+
+  Point3 actual = camera.backproject(Point2(), 1.);
+  Point3 expected(0., 1., 0.);
+  pair<Point2, bool> x = camera.projectSafe(expected);
+
+  EXPECT(assert_equal(expected, actual));
+  EXPECT(assert_equal(Point2(), x.first));
+  EXPECT(x.second);
+}
+
+/* ************************************************************************* */
+static Point2 project3(const Pose3& pose, const Point3& point,
+    const Cal3_S2::shared_ptr& cal) {
+  return Camera(pose, cal).project2(point);
+}
+
+/* ************************************************************************* */
+TEST( PinholePose, Dproject)
+{
+  Matrix Dpose, Dpoint;
+  Point2 result = camera.project2(point1, Dpose, Dpoint);
+  Matrix numerical_pose  = numericalDerivative31(project3, pose, point1, K);
+  Matrix Hexpected2 = numericalDerivative32(project3, pose, point1, K);
+  EXPECT(assert_equal(Point2(-100,  100), result));
+  EXPECT(assert_equal(numerical_pose,  Dpose,  1e-7));
+  EXPECT(assert_equal(Hexpected2, Dpoint, 1e-7));
+}
+
+/* ************************************************************************* */
+static Point2 project4(const Camera& camera, const Point3& point) {
+  return camera.project2(point);
+}
+
+/* ************************************************************************* */
+TEST( PinholePose, Dproject2)
+{
+  Matrix Dcamera, Dpoint;
+  Point2 result = camera.project2(point1, Dcamera, Dpoint);
+  Matrix Hexpected1 = numericalDerivative21(project4, camera, point1);
+  Matrix Hexpected2  = numericalDerivative22(project4, camera, point1);
+  EXPECT(assert_equal(result, Point2(-100,  100) ));
+  EXPECT(assert_equal(Hexpected1, Dcamera, 1e-7));
+  EXPECT(assert_equal(Hexpected2,  Dpoint,  1e-7));
+}
+
+/* ************************************************************************* */
+// Add a test with more arbitrary rotation
+TEST( CalibratedCamera, Dproject3)
+{
+  static const Pose3 pose1(Rot3::ypr(0.1, -0.1, 0.4), Point3(0, 0, -10));
+  static const Camera camera(pose1);
+  Matrix Dpose, Dpoint;
+  camera.project2(point1, Dpose, Dpoint);
+  Matrix numerical_pose  = numericalDerivative21(project4, camera, point1);
+  Matrix numerical_point = numericalDerivative22(project4, camera, point1);
+  CHECK(assert_equal(numerical_pose,  Dpose, 1e-7));
+  CHECK(assert_equal(numerical_point, Dpoint, 1e-7));
+}
+
+/* ************************************************************************* */
+static double range0(const Camera& camera, const Point3& point) {
+  return camera.range(point);
+}
+
+/* ************************************************************************* */
+TEST( PinholePose, range0) {
+  Matrix D1; Matrix D2;
+  double result = camera.range(point1, D1, D2);
+  Matrix Hexpected1 = numericalDerivative21(range0, camera, point1);
+  Matrix Hexpected2 = numericalDerivative22(range0, camera, point1);
+  EXPECT_DOUBLES_EQUAL(point1.distance(camera.pose().translation()), result,
+      1e-9);
+  EXPECT(assert_equal(Hexpected1, D1, 1e-7));
+  EXPECT(assert_equal(Hexpected2, D2, 1e-7));
+}
+
+/* ************************************************************************* */
+static double range1(const Camera& camera, const Pose3& pose) {
+  return camera.range(pose);
+}
+
+/* ************************************************************************* */
+TEST( PinholePose, range1) {
+  Matrix D1; Matrix D2;
+  double result = camera.range(pose1, D1, D2);
+  Matrix Hexpected1 = numericalDerivative21(range1, camera, pose1);
+  Matrix Hexpected2 = numericalDerivative22(range1, camera, pose1);
+  EXPECT_DOUBLES_EQUAL(1, result, 1e-9);
+  EXPECT(assert_equal(Hexpected1, D1, 1e-7));
+  EXPECT(assert_equal(Hexpected2, D2, 1e-7));
+}
+
+/* ************************************************************************* */
+typedef PinholePose<Cal3Bundler> Camera2;
+static const boost::shared_ptr<Cal3Bundler> K2 =
+    boost::make_shared<Cal3Bundler>(625, 1e-3, 1e-3);
+static const Camera2 camera2(pose1, K2);
+static double range2(const Camera& camera, const Camera2& camera2) {
+  return camera.range<Cal3Bundler>(camera2);
+}
+
+/* ************************************************************************* */
+TEST( PinholePose, range2) {
+  Matrix D1; Matrix D2;
+  double result = camera.range<Cal3Bundler>(camera2, D1, D2);
+  Matrix Hexpected1 = numericalDerivative21(range2, camera, camera2);
+  Matrix Hexpected2 = numericalDerivative22(range2, camera, camera2);
+  EXPECT_DOUBLES_EQUAL(1, result, 1e-9);
+  EXPECT(assert_equal(Hexpected1, D1, 1e-7));
+  EXPECT(assert_equal(Hexpected2, D2, 1e-7));
+}
+
+/* ************************************************************************* */
+static const CalibratedCamera camera3(pose1);
+static double range3(const Camera& camera, const CalibratedCamera& camera3) {
+  return camera.range(camera3);
+}
+
+/* ************************************************************************* */
+TEST( PinholePose, range3) {
+  Matrix D1; Matrix D2;
+  double result = camera.range(camera3, D1, D2);
+  Matrix Hexpected1 = numericalDerivative21(range3, camera, camera3);
+  Matrix Hexpected2 = numericalDerivative22(range3, camera, camera3);
+  EXPECT_DOUBLES_EQUAL(1, result, 1e-9);
+  EXPECT(assert_equal(Hexpected1, D1, 1e-7));
+  EXPECT(assert_equal(Hexpected2, D2, 1e-7));
+}
+
+/* ************************************************************************* */
+int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
+/* ************************************************************************* */
+
+

gtsam/geometry/tests/testSerializationGeometry.cpp

@@ -16,8 +16,6 @@
  * @date Feb 7, 2012
  */
 
-#include <gtsam/geometry/Point2.h>
-#include <gtsam/geometry/Point3.h>
 #include <gtsam/geometry/Pose2.h>
 #include <gtsam/geometry/Unit3.h>
 #include <gtsam/geometry/EssentialMatrix.h>

gtsam/geometry/tests/testSimpleCamera.cpp

@@ -15,13 +15,13 @@
  * @brief test SimpleCamera class
  */
 
-#include <cmath>
+#include <gtsam/geometry/SimpleCamera.h>
-#include <iostream>
+#include <gtsam/geometry/Pose2.h>
-
-#include <CppUnitLite/TestHarness.h>
 #include <gtsam/base/Testable.h>
 #include <gtsam/base/numericalDerivative.h>
-#include <gtsam/geometry/SimpleCamera.h>
+#include <CppUnitLite/TestHarness.h>
+#include <cmath>
+#include <iostream>
 
 using namespace std;
 using namespace gtsam;

gtsam/geometry/tests/testStereoCamera.cpp

@@ -96,16 +96,12 @@ static StereoPoint2 project3(const Pose3& pose, const Point3& point, const Cal3_
 TEST( StereoCamera, Dproject)
 {
   Matrix expected1 = numericalDerivative31(project3, camPose, landmark, *K);
-  Matrix actual1; stereoCam.project(landmark, actual1, boost::none, boost::none);
+  Matrix actual1; stereoCam.project2(landmark, actual1, boost::none);
   CHECK(assert_equal(expected1,actual1,1e-7));
 
   Matrix expected2 = numericalDerivative32(project3,camPose, landmark, *K);
-  Matrix actual2; stereoCam.project(landmark, boost::none, actual2, boost::none);
+  Matrix actual2; stereoCam.project2(landmark, boost::none, actual2);
   CHECK(assert_equal(expected2,actual2,1e-7));
-
-  Matrix expected3 = numericalDerivative33(project3,camPose, landmark, *K);
-  Matrix actual3; stereoCam.project(landmark, boost::none, boost::none, actual3);
-//  CHECK(assert_equal(expected3,actual3,1e-8));
 }
 
 /* ************************************************************************* */

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/nonlinear/tests/testSerializationNonlinear.cpp

@@ -19,6 +19,7 @@
 #include <gtsam/nonlinear/Values.h>
 #include <gtsam/inference/Symbol.h>
 #include <gtsam/geometry/PinholeCamera.h>
+#include <gtsam/geometry/Pose2.h>
 #include <gtsam/geometry/Cal3_S2.h>
 #include <gtsam/geometry/Cal3DS2.h>
 #include <gtsam/geometry/Cal3Bundler.h>

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
@@ -6,16 +17,18 @@
 
 #pragma once
 
-#include "JacobianSchurFactor.h"
+#include "RegularJacobianFactor.h"
 
 namespace gtsam {
 /**
  * JacobianFactor for Schur complement that uses Q noise model
  */
 template<size_t D, size_t ZDim>
-class JacobianFactorQ: public JacobianSchurFactor<D, ZDim> {
+class JacobianFactorQ: public RegularJacobianFactor<D> {
 
-  typedef JacobianSchurFactor<D, ZDim> Base;
+  typedef RegularJacobianFactor<D> Base;
+  typedef Eigen::Matrix<double, ZDim, D> MatrixZD;
+  typedef std::pair<Key, MatrixZD> KeyMatrixZD;
 
 public:
 
@@ -24,9 +37,10 @@ public:
   }
 
   /// Empty constructor with keys
-  JacobianFactorQ(const FastVector<Key>& keys,
+  JacobianFactorQ(const FastVector<Key>& keys, //
-      const SharedDiagonal& model =  SharedDiagonal()) : JacobianSchurFactor<D, ZDim>() {
+      const SharedDiagonal& model = SharedDiagonal()) :
-    Matrix zeroMatrix = Matrix::Zero(0,D);
+      Base() {
+    Matrix zeroMatrix = Matrix::Zero(0, D);
     Vector zeroVector = Vector::Zero(0);
     typedef std::pair<Key, Matrix> KeyMatrix;
     std::vector<KeyMatrix> QF;
@@ -37,24 +51,31 @@ public:
   }
 
   /// Constructor
-  JacobianFactorQ(const std::vector<typename Base::KeyMatrix2D>& Fblocks,
+  JacobianFactorQ(const std::vector<KeyMatrixZD>& Fblocks, const Matrix& E,
-      const Matrix& E, const Matrix3& P, const Vector& b,
+      const Matrix3& P, const Vector& b, const SharedDiagonal& model =
-      const SharedDiagonal& model = SharedDiagonal()) :
+          SharedDiagonal()) :
-      JacobianSchurFactor<D, ZDim>() {
+      Base() {
     size_t j = 0, m2 = E.rows(), m = m2 / ZDim;
     // Calculate projector Q
     Matrix Q = eye(m2) - E * P * E.transpose();
     // Calculate pre-computed Jacobian matrices
     // TODO: can we do better ?
     typedef std::pair<Key, Matrix> KeyMatrix;
-    std::vector < KeyMatrix > QF;
+    std::vector<KeyMatrix> QF;
     QF.reserve(m);
     // Below, we compute each mZDim*D block A_j = Q_j * F_j = (mZDim*ZDim) * (Zdim*D)
-    BOOST_FOREACH(const typename Base::KeyMatrix2D& it, Fblocks)
+    BOOST_FOREACH(const KeyMatrixZD& it, Fblocks)
-      QF.push_back(KeyMatrix(it.first, Q.block(0, ZDim * j++, m2, ZDim) * it.second));
+      QF.push_back(
+          KeyMatrix(it.first, Q.block(0, ZDim * j++, m2, ZDim) * it.second));
     // Which is then passed to the normal JacobianFactor constructor
     JacobianFactor::fillTerms(QF, Q * b, model);
   }
 };
+// end class JacobianFactorQ
+
+// traits
+template<size_t D, size_t ZDim> struct traits<JacobianFactorQ<D, ZDim> > : public Testable<
+    JacobianFactorQ<D, ZDim> > {
+};
 
 }

gtsam/slam/JacobianFactorQR.h

@@ -6,31 +6,38 @@
  */
 
 #pragma once
-#include <gtsam/slam/JacobianSchurFactor.h>
+#include <gtsam/slam/RegularJacobianFactor.h>
+#include <gtsam/linear/GaussianFactorGraph.h>
+#include <gtsam/inference/Symbol.h>
 
 namespace gtsam {
+
+class GaussianBayesNet;
+
 /**
  * JacobianFactor for Schur complement that uses Q noise model
  */
 template<size_t D, size_t ZDim>
-class JacobianFactorQR: public JacobianSchurFactor<D, ZDim> {
+class JacobianFactorQR: public RegularJacobianFactor<D> {
 
-  typedef JacobianSchurFactor<D, ZDim> Base;
+  typedef RegularJacobianFactor<D> Base;
+  typedef Eigen::Matrix<double, ZDim, D> MatrixZD;
+  typedef std::pair<Key, MatrixZD> KeyMatrixZD;
 
 public:
 
   /**
    * Constructor
    */
-  JacobianFactorQR(const std::vector<typename Base::KeyMatrix2D>& Fblocks,
+  JacobianFactorQR(const std::vector<KeyMatrixZD>& Fblocks, const Matrix& E,
-      const Matrix& E, const Matrix3& P, const Vector& b,
+      const Matrix3& P, const Vector& b, //
       const SharedDiagonal& model = SharedDiagonal()) :
-      JacobianSchurFactor<D, ZDim>() {
+      Base() {
     // Create a number of Jacobian factors in a factor graph
     GaussianFactorGraph gfg;
     Symbol pointKey('p', 0);
     size_t i = 0;
-    BOOST_FOREACH(const typename Base::KeyMatrix2D& it, Fblocks) {
+    BOOST_FOREACH(const KeyMatrixZD& it, Fblocks) {
       gfg.add(pointKey, E.block<ZDim, 3>(ZDim * i, 0), it.first, it.second,
           b.segment<ZDim>(ZDim * i), model);
       i += 1;
@@ -38,9 +45,9 @@ public:
     //gfg.print("gfg");
 
     // eliminate the point
-    GaussianBayesNet::shared_ptr bn;
+    boost::shared_ptr<GaussianBayesNet> bn;
     GaussianFactorGraph::shared_ptr fg;
-    std::vector < Key > variables;
+    std::vector<Key> variables;
     variables.push_back(pointKey);
     boost::tie(bn, fg) = gfg.eliminatePartialSequential(variables, EliminateQR);
     //fg->print("fg");
@@ -48,6 +55,6 @@ public:
     JacobianFactor::operator=(JacobianFactor(*fg));
   }
 };
-// class
+// end class JacobianFactorQR
 
-}// gtsam
+}// end namespace gtsam

gtsam/slam/JacobianFactorSVD.h

@@ -5,28 +5,31 @@
  */
 
 #pragma once
-#include "gtsam/slam/JacobianSchurFactor.h"
+#include "gtsam/slam/RegularJacobianFactor.h"
 
 namespace gtsam {
 /**
  * JacobianFactor for Schur complement that uses Q noise model
  */
 template<size_t D, size_t ZDim>
-class JacobianFactorSVD: public JacobianSchurFactor<D, ZDim> {
+class JacobianFactorSVD: public RegularJacobianFactor<D> {
+
+  typedef RegularJacobianFactor<D> Base;
+  typedef Eigen::Matrix<double, ZDim, D> MatrixZD; // e.g 2 x 6 with Z=Point2
+  typedef std::pair<Key, MatrixZD> KeyMatrixZD;
+  typedef std::pair<Key, Matrix> KeyMatrix;
 
 public:
 
-  typedef Eigen::Matrix<double, ZDim, D> Matrix2D;   // e.g 2 x 6 with Z=Point2
-  typedef std::pair<Key, Matrix2D> KeyMatrix2D;
-  typedef std::pair<Key, Matrix> KeyMatrix;
-
   /// Default constructor
-  JacobianFactorSVD() {}
+  JacobianFactorSVD() {
+  }
 
   /// Empty constructor with keys
-  JacobianFactorSVD(const FastVector<Key>& keys,
+  JacobianFactorSVD(const FastVector<Key>& keys, //
-      const SharedDiagonal& model =  SharedDiagonal()) : JacobianSchurFactor<D, ZDim>() {
+      const SharedDiagonal& model = SharedDiagonal()) :
-    Matrix zeroMatrix = Matrix::Zero(0,D);
+      Base() {
+    Matrix zeroMatrix = Matrix::Zero(0, D);
     Vector zeroVector = Vector::Zero(0);
     std::vector<KeyMatrix> QF;
     QF.reserve(keys.size());
@@ -36,19 +39,23 @@ public:
   }
 
   /// Constructor
-  JacobianFactorSVD(const std::vector<KeyMatrix2D>& Fblocks, const Matrix& Enull, const Vector& b,
+  JacobianFactorSVD(const std::vector<KeyMatrixZD>& Fblocks,
-      const SharedDiagonal& model =  SharedDiagonal()) : JacobianSchurFactor<D, ZDim>() {
+      const Matrix& Enull, const Vector& b, //
+      const SharedDiagonal& model = SharedDiagonal()) :
+      Base() {
     size_t numKeys = Enull.rows() / ZDim;
-    size_t j = 0, m2 = ZDim*numKeys-3;
+    size_t j = 0, m2 = ZDim * numKeys - 3;
     // PLAIN NULL SPACE TRICK
     // Matrix Q = Enull * Enull.transpose();
-    // BOOST_FOREACH(const KeyMatrix2D& it, Fblocks)
+    // BOOST_FOREACH(const KeyMatrixZD& it, Fblocks)
     //   QF.push_back(KeyMatrix(it.first, Q.block(0, 2 * j++, m2, 2) * it.second));
     // JacobianFactor factor(QF, Q * b);
     std::vector<KeyMatrix> QF;
     QF.reserve(numKeys);
-    BOOST_FOREACH(const KeyMatrix2D& it, Fblocks)
+    BOOST_FOREACH(const KeyMatrixZD& it, Fblocks)
-    QF.push_back(KeyMatrix(it.first, (Enull.transpose()).block(0, ZDim * j++, m2, ZDim) * it.second));
+      QF.push_back(
+          KeyMatrix(it.first,
+              (Enull.transpose()).block(0, ZDim * j++, m2, ZDim) * it.second));
     JacobianFactor::fillTerms(QF, Enull.transpose() * b, model);
   }
 };

gtsam/slam/JacobianSchurFactor.h

@@ -1,93 +0,0 @@
-/*
- * @file  JacobianSchurFactor.h
- * @brief Jacobianfactor that combines and eliminates points
- * @date  Oct 27, 2013
- * @uthor Frank Dellaert
- */
-
-#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 {
-/**
- * JacobianFactor for Schur complement that uses Q noise model
- */
-template<size_t D, size_t ZDim>
-class JacobianSchurFactor: public JacobianFactor {
-
-public:
-
-  typedef Eigen::Matrix<double, ZDim, D> Matrix2D;
-  typedef std::pair<Key, Matrix2D> KeyMatrix2D;
-
-  // Use eigen magic to access raw memory
-  typedef Eigen::Matrix<double, D, 1> DVector;
-  typedef Eigen::Map<DVector> DMap;
-  typedef Eigen::Map<const DVector> ConstDMap;
-
-  /**
-   * @brief double* Matrix-vector multiply, i.e. y = A*x
-   * RAW memory access! Assumes keys start at 0 and go to M-1, and x is laid out that way
-   */
-  Vector operator*(const double* x) const {
-    Vector Ax = zero(Ab_.rows());
-    if (empty()) return Ax;
-
-    // Just iterate over all A matrices and multiply in correct config part
-    for(size_t pos=0; pos<size(); ++pos)
-      Ax += Ab_(pos) * ConstDMap(x + D * keys_[pos]);
-
-    return model_ ? model_->whiten(Ax) : Ax;
-  }
-
-  /**
-   * @brief double* Transpose Matrix-vector multiply, i.e. x += A'*e
-   * RAW memory access! Assumes keys start at 0 and go to M-1, and y is laid out that way
-   */
-  void transposeMultiplyAdd(double alpha, const Vector& e, double* x) const
-  {
-    Vector E = alpha * (model_ ? model_->whiten(e) : e);
-    // Just iterate over all A matrices and insert Ai^e into y
-    for(size_t pos=0; pos<size(); ++pos)
-      DMap(x + D * keys_[pos]) += Ab_(pos).transpose() * E;
-  }
-
-  /** y += alpha * A'*A*x */
-  void multiplyHessianAdd(double alpha, const VectorValues& x, VectorValues& y) const {
-    JacobianFactor::multiplyHessianAdd(alpha,x,y);
-  }
-
-  /**
-   * @brief double* Hessian-vector multiply, i.e. y += A'*(A*x)
-   * RAW memory access! Assumes keys start at 0 and go to M-1, and x and and y are laid out that way
-   */
-  void multiplyHessianAdd(double alpha, const double* x, double* y) const {
-//    Vector Ax = (*this)*x;
-//    this->transposeMultiplyAdd(alpha,Ax,y);
-    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)
-      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); }
-
-    // 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)
-      DMap(y + D * keys_[pos]) += Ab_(pos).transpose() * Ax;
-  }
-
-}; // class
-
-} // gtsam

gtsam/slam/RegularHessianFactor.h

@@ -11,9 +11,9 @@
 
 /**
  * @file   RegularHessianFactor.h
- * @brief   HessianFactor class with constant sized blcoks
+ * @brief  HessianFactor class with constant sized blocks
- * @author  Richard Roberts
+ * @author Sungtae An
- * @date    Dec 8, 2010
+ * @date   March 2014
  */
 
 #pragma once
@@ -29,8 +29,10 @@ class RegularHessianFactor: public HessianFactor {
 
 private:
 
-  typedef Eigen::Matrix<double, D, D> MatrixDD; // camera hessian block
+  // Use Eigen magic to access raw memory
-  typedef Eigen::Matrix<double, D, 1> VectorD;
+  typedef Eigen::Matrix<double, D, 1> DVector;
+  typedef Eigen::Map<DVector> DMap;
+  typedef Eigen::Map<const DVector> ConstDMap;
 
 public:
 
@@ -43,6 +45,15 @@ public:
       HessianFactor(js, Gs, gs, f) {
   }
 
+  /** Construct a binary factor.  Gxx are the upper-triangle blocks of the
+   * quadratic term (the Hessian matrix), gx the pieces of the linear vector
+   * term, and f the constant term.
+   */
+  RegularHessianFactor(Key j1, Key j2, const Matrix& G11, const Matrix& G12,
+      const Vector& g1, const Matrix& G22, const Vector& g2, double f) :
+      HessianFactor(j1, j2, G11, G12, g1, G22, g2, f) {
+  }
+
   /** Constructor with an arbitrary number of keys and with the augmented information matrix
    *   specified as a block matrix. */
   template<typename KEYS>
@@ -52,25 +63,22 @@ public:
   }
 
   // Scratch space for multiplyHessianAdd
-  typedef Eigen::Matrix<double, D, 1> DVector;
   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)
       yi.setZero();
 
-    typedef Eigen::Map<DVector> DMap;
-    typedef Eigen::Map<const DVector> ConstDMap;
-
     // Accessing the VectorValues one by one is expensive
     // So we will loop over columns to access x only once per column
     // And fill the above temporary y values, to be added into yvalues after
@@ -95,14 +103,10 @@ public:
     }
   }
 
+  /// Raw memory version, with offsets TODO document reasoning
   void multiplyHessianAdd(double alpha, const double* x, double* yvalues,
       std::vector<size_t> offsets) const {
 
-    // Use eigen magic to access raw memory
-    typedef Eigen::Matrix<double, Eigen::Dynamic, 1> DVector;
-    typedef Eigen::Map<DVector> DMap;
-    typedef Eigen::Map<const DVector> ConstDMap;
-
     // Create a vector of temporary y values, corresponding to rows i
     std::vector<Vector> y;
     y.reserve(size());
@@ -131,47 +135,42 @@ 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();
+      DVector dj = -info_(pos, size()).knownOffDiagonal();
-      //DMap(d + 9 * j) += dj;
       DMap(d + D * j) += dj;
     }
   }
 
+  /* ************************************************************************* */
+
+};
+// end class RegularHessianFactor
+
+// traits
+template<size_t D> struct traits<RegularHessianFactor<D> > : public Testable<
+    RegularHessianFactor<D> > {
 };
 
 }

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 {
 
@@ -89,18 +87,27 @@ public:
       const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
     std::cout << " RegularImplicitSchurFactor " << std::endl;
     Factor::print(s);
-    std::cout << " PointCovariance_ \n" << PointCovariance_ << std::endl;
+    for (size_t pos = 0; pos < size(); ++pos) {
-    std::cout << " E_ \n" << E_ << std::endl;
+      std::cout << "Fblock:\n" << Fblocks_[pos].second << std::endl;
-    std::cout << " b_ \n" << b_.transpose() << std::endl;
+    }
+    std::cout << "PointCovariance:\n" << PointCovariance_ << std::endl;
+    std::cout << "E:\n" << E_ << std::endl;
+    std::cout << "b:\n" << b_.transpose() << std::endl;
   }
 
   /// equals
   bool equals(const GaussianFactor& lf, double tol) const {
-    if (!dynamic_cast<const RegularImplicitSchurFactor*>(&lf))
+    const This* f = dynamic_cast<const This*>(&lf);
-      return false;
+    if (!f)
-    else {
       return false;
+    for (size_t pos = 0; pos < size(); ++pos) {
+      if (keys_[pos] != f->keys_[pos]) return false;
+      if (Fblocks_[pos].first != f->Fblocks_[pos].first) return false;
+      if (!equal_with_abs_tol(Fblocks_[pos].second,f->Fblocks_[pos].second,tol)) return false;
     }
+    return equal_with_abs_tol(PointCovariance_, f->PointCovariance_, tol)
+        && equal_with_abs_tol(E_, f->E_, tol)
+        && equal_with_abs_tol(b_, f->b_, tol);
   }
 
   /// Degrees of freedom of camera
@@ -460,7 +467,12 @@ public:
 
 
 };
-// RegularImplicitSchurFactor
+// end class RegularImplicitSchurFactor
+
+// traits
+template<size_t D> struct traits<RegularImplicitSchurFactor<D> > : public Testable<
+    RegularImplicitSchurFactor<D> > {
+};
 
 }
 

gtsam/slam/RegularJacobianFactor.h

@@ -21,25 +21,34 @@
 #include <gtsam/linear/JacobianFactor.h>
 #include <gtsam/linear/VectorValues.h>
 #include <boost/foreach.hpp>
-#include <vector>
 
 namespace gtsam {
 
+/**
+ * JacobianFactor with constant sized blocks
+ * Provides raw memory access versions of linear operator.
+ * Is base class for JacobianQFactor, JacobianFactorQR, and JacobianFactorSVD
+ */
 template<size_t D>
 class RegularJacobianFactor: public JacobianFactor {
 
 private:
 
-  /** Use eigen magic to access raw memory */
+  // Use eigen magic to access raw memory
   typedef Eigen::Matrix<double, D, 1> DVector;
   typedef Eigen::Map<DVector> DMap;
   typedef Eigen::Map<const DVector> ConstDMap;
 
 public:
 
+  /// Default constructor
+  RegularJacobianFactor() {}
+
   /** Construct an n-ary factor
    * @tparam TERMS A container whose value type is std::pair<Key, Matrix>, specifying the
-   *         collection of keys and matrices making up the factor. */
+   *         collection of keys and matrices making up the factor.
+   * TODO Verify terms are regular
+   */
   template<typename TERMS>
   RegularJacobianFactor(const TERMS& terms, const Vector& b,
       const SharedDiagonal& model = SharedDiagonal()) :
@@ -49,91 +58,66 @@ public:
   /** Constructor with arbitrary number keys, and where the augmented matrix is given all together
    *  instead of in block terms.  Note that only the active view of the provided augmented matrix
    *  is used, and that the matrix data is copied into a newly-allocated matrix in the constructed
-   *  factor. */
+   *  factor.
+   *  TODO Verify complies to regular
+   */
   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) {
   }
 
-  /** Raw memory access version of multiplyHessianAdd y += alpha * A'*A*x
+  /** y += alpha * A'*A*x */
-   * Note: this is not assuming a fixed dimension for the variables,
+  virtual void multiplyHessianAdd(double alpha, const VectorValues& x,
-   * but requires the vector accumulatedDims to tell the dimension of
+      VectorValues& y) const {
-   * each variable: e.g.: x0 has dim 3, x2 has dim 6, x3 has dim 2,
+    JacobianFactor::multiplyHessianAdd(alpha, x, y);
-   * then accumulatedDims is [0 3 9 11 13]
+  }
-   * NOTE: size of accumulatedDims is size of keys + 1!! */
-  void multiplyHessianAdd(double alpha, const double* x, double* y,
-      const std::vector<size_t>& accumulatedDims) const {
-
-    /// Use eigen magic to access raw memory
-    typedef Eigen::Matrix<double, Eigen::Dynamic, 1> VectorD;
-    typedef Eigen::Map<VectorD> MapD;
-    typedef Eigen::Map<const VectorD> ConstMapD;
 
+  /**
+   * @brief double* Hessian-vector multiply, i.e. y += A'*(A*x)
+   * RAW memory access! Assumes keys start at 0 and go to M-1, and x and and y are laid out that way
+   */
+  void multiplyHessianAdd(double alpha, const double* x, double* y) const {
     if (empty())
       return;
     Vector Ax = zero(Ab_.rows());
 
-    /// Just iterate over all A matrices and multiply in correct config part (looping over keys)
+    // Just iterate over all A matrices and multiply in correct config part
-    /// 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)
-    {
+      Ax += Ab_(pos) * ConstDMap(x + D * keys_[pos]);
-      Ax += Ab_(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
-    }
-    /// Deal with noise properly, need to Double* whiten as we are dividing by variance
     if (model_) {
       model_->whitenInPlace(Ax);
       model_->whitenInPlace(Ax);
     }
 
-    /// multiply with alpha
-    Ax *= alpha;
-
-    /// Again iterate over all A matrices and insert Ai^T into y
-    for (size_t pos = 0; pos < size(); ++pos){
-      MapD(y + accumulatedDims[keys_[pos]], accumulatedDims[keys_[pos] + 1] - accumulatedDims[keys_[pos]]) += Ab_(
-          pos).transpose() * Ax;
-    }
-  }
-
-  /** Raw memory access version of multiplyHessianAdd */
-  void multiplyHessianAdd(double alpha, const double* x, double* y) const {
-
-    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)
-      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); }
-
     // 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;
   }
 
-  /** Raw memory access version of hessianDiagonal
+  /// Expose base class hessianDiagonal
-   * TODO: currently assumes all variables of the same size D (templated) and keys arranged from 0 to n
+  virtual VectorValues hessianDiagonal() const {
-   */
+    return JacobianFactor::hessianDiagonal();
-  virtual void hessianDiagonal(double* d) const {
+  }
+
+  /// Raw memory access version of hessianDiagonal
+  void hessianDiagonal(double* d) const {
     // Loop over all variables in the factor
     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();
         }
       }
@@ -141,10 +125,13 @@ public:
     }
   }
 
-  /** Raw memory access version of gradientAtZero
+  /// Expose base class gradientAtZero
-   * TODO: currently assumes all variables of the same size D (templated) and keys arranged from 0 to n
+  virtual VectorValues gradientAtZero() const {
-   */
+    return JacobianFactor::gradientAtZero();
-  virtual void gradientAtZero(double* d) const {
+  }
+
+  /// Raw memory access version of gradientAtZero
+  void gradientAtZero(double* d) const {
 
     // Get vector b not weighted
     Vector b = getb();
@@ -156,19 +143,90 @@ 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;
     }
   }
 
+  /**
+   * @brief double* Transpose Matrix-vector multiply, i.e. x += A'*e
+   * RAW memory access! Assumes keys start at 0 and go to M-1, and y is laid out that way
+   */
+  void transposeMultiplyAdd(double alpha, const Vector& e, double* x) const {
+    Vector E = alpha * (model_ ? model_->whiten(e) : e);
+    // Just iterate over all A matrices and insert Ai^e into y
+    for (size_t pos = 0; pos < size(); ++pos)
+      DMap(x + D * keys_[pos]) += Ab_(pos).transpose() * E;
+  }
+
+  /**
+   * @brief double* Matrix-vector multiply, i.e. y = A*x
+   * RAW memory access! Assumes keys start at 0 and go to M-1, and x is laid out that way
+   */
+  Vector operator*(const double* x) const {
+    Vector Ax = zero(Ab_.rows());
+    if (empty())
+      return Ax;
+
+    // Just iterate over all A matrices and multiply in correct config part
+    for (size_t pos = 0; pos < size(); ++pos)
+      Ax += Ab_(pos) * ConstDMap(x + D * keys_[pos]);
+
+    return model_ ? model_->whiten(Ax) : Ax;
+  }
+
+  /** 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
+   * each variable: e.g.: x0 has dim 3, x2 has dim 6, x3 has dim 2,
+   * then accumulatedDims is [0 3 9 11 13]
+   * NOTE: size of accumulatedDims is size of keys + 1!!
+   * TODO Frank asks: why is this here if not regular ????
+   */
+  void multiplyHessianAdd(double alpha, const double* x, double* y,
+      const std::vector<size_t>& accumulatedDims) const {
+
+    /// Use Eigen magic to access raw memory
+    typedef Eigen::Map<Vector> VectorMap;
+    typedef Eigen::Map<const Vector> ConstVectorMap;
+
+    if (empty())
+      return;
+    Vector Ax = zero(Ab_.rows());
+
+    /// 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) {
+      size_t offset = accumulatedDims[keys_[pos]];
+      size_t dim = accumulatedDims[keys_[pos] + 1] - offset;
+      Ax += Ab_(pos) * ConstVectorMap(x + offset, dim);
+    }
+    /// Deal with noise properly, need to Double* whiten as we are dividing by variance
+    if (model_) {
+      model_->whitenInPlace(Ax);
+      model_->whitenInPlace(Ax);
+    }
+
+    /// multiply with alpha
+    Ax *= alpha;
+
+    /// Again iterate over all A matrices and insert Ai^T into y
+    for (size_t pos = 0; pos < size(); ++pos) {
+      size_t offset = accumulatedDims[keys_[pos]];
+      size_t dim = accumulatedDims[keys_[pos] + 1] - offset;
+      VectorMap(y + offset, dim) += Ab_(pos).transpose() * Ax;
+    }
+  }
+
 };
+// end class RegularJacobianFactor
 
-}
+}// end namespace gtsam
-

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/expressions.h

@@ -8,6 +8,7 @@
 #pragma once
 
 #include <gtsam/nonlinear/expressions.h>
+#include <gtsam/geometry/Pose2.h>
 #include <gtsam/geometry/Cal3_S2.h>
 #include <gtsam/geometry/Cal3Bundler.h>
 #include <gtsam/geometry/PinholeCamera.h>

gtsam/slam/tests/testDataset.cpp

@@ -15,16 +15,16 @@
  * @author  Richard Roberts, Luca Carlone
  */
 
-#include <CppUnitLite/TestHarness.h>
-
-#include <boost/algorithm/string.hpp>
-
-#include <gtsam/inference/Symbol.h>
-#include <gtsam/base/TestableAssertions.h>
-#include <gtsam/inference/Symbol.h>
 
 #include <gtsam/slam/BetweenFactor.h>
 #include <gtsam/slam/dataset.h>
+#include <gtsam/geometry/Pose2.h>
+#include <gtsam/inference/Symbol.h>
+#include <gtsam/base/TestableAssertions.h>
+
+#include <boost/algorithm/string.hpp>
+
+#include <CppUnitLite/TestHarness.h>
 
 using namespace gtsam::symbol_shorthand;
 using namespace std;

gtsam/slam/tests/testLago.cpp

@@ -23,6 +23,7 @@
 #include <gtsam/slam/dataset.h>
 #include <gtsam/slam/BetweenFactor.h>
 #include <gtsam/slam/PriorFactor.h>
+#include <gtsam/geometry/Pose2.h>
 #include <gtsam/inference/Symbol.h>
 
 #include <CppUnitLite/TestHarness.h>

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/testManifold.cpp

@@ -17,9 +17,8 @@
  * @brief unit tests for Block Automatic Differentiation
  */
 
-#include <gtsam/geometry/Point2.h>
 #include <gtsam/geometry/PinholeCamera.h>
-#include <gtsam/geometry/Pose3.h>
+#include <gtsam/geometry/Pose2.h>
 #include <gtsam/geometry/Cal3_S2.h>
 #include <gtsam/geometry/Cal3Bundler.h>
 #include <gtsam/base/VectorSpace.h>

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;