diff --git a/.cproject b/.cproject
index d2323e19c..f3f62e42d 100644
--- a/.cproject
+++ b/.cproject
@@ -2303,14 +2303,6 @@
true
true
-
- make
- -j2
- Pose2SLAMwSPCG_easy.run
- true
- true
- true
-
make
-j5
@@ -2447,6 +2439,22 @@
true
true
+
+ make
+ -j4
+ Pose2SLAMwSPCG.run
+ true
+ true
+ true
+
+
+ make
+ -j4
+ SFMExample_SmartFactorPCG.run
+ true
+ true
+ true
+
make
-j2
diff --git a/examples/SFMExample_SmartFactorPCG.cpp b/examples/SFMExample_SmartFactorPCG.cpp
new file mode 100644
index 000000000..82d7f62c3
--- /dev/null
+++ b/examples/SFMExample_SmartFactorPCG.cpp
@@ -0,0 +1,118 @@
+/* ----------------------------------------------------------------------------
+
+ * 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
+
+// These extra headers allow us a LM outer loop with PCG linear solver (inner loop)
+#include
+#include
+#include
+
+using namespace std;
+using namespace gtsam;
+
+// Make the typename short so it looks much cleaner
+typedef gtsam::SmartProjectionPoseFactor 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 points = createPoints();
+ vector 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(0, poses[0], poseNoise));
+
+ // Fix the scale ambiguity by adding a prior
+ graph.push_back(PriorFactor(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;
+ PCGSolverParameters::shared_ptr pcg =
+ boost::make_shared();
+ pcg->preconditioner_ =
+ boost::make_shared();
+ 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(graph[j]);
+ if (smart) {
+ boost::optional point = smart->point(result);
+ if (point) // ignore if boost::optional return NULL
+ landmark_result.insert(j, *point);
+ }
+ }
+
+ landmark_result.print("Landmark results:\n");
+
+ return 0;
+}
+/* ************************************************************************* */
+