text: /* ----------------------------------------------------------------------------
  
   * 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;
  
  // create a typedef to the camera type
  typedef PinholePose<Cal3_S2> Camera;
  
  /* ************************************************************************* */
  int main(int argc, char* argv[]) {
  
    // Define the camera calibration parameters
    Cal3_S2::shared_ptr K(new Cal3_S2(50.0, 50.0, 0.0, 50.0, 50.0));
  
    // Define the camera observation noise model
    noiseModel::Isotropic::shared_ptr measurementNoise =
        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(measurementNoise, K));
  
      for (size_t i = 0; i < poses.size(); ++i) {
  
        // generate the 2D measurement
        Camera camera(poses[i], K);
        Point2 measurement = camera.project(points[j]);
  
        // call add() function to add measurement into a single factor, here we need to add:
        smartfactor->add(measurement, i);
      }
  
      // 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 noise = noiseModel::Diagonal::Sigmas(
        (Vector(6) << Vector3::Constant(0.1), Vector3::Constant(0.3)).finished());
    graph.addPrior(0, poses[0], noise);
  
    // Fix the scale ambiguity by adding a prior
    graph.addPrior(1, poses[0], noise);
  
    // Create the initial estimate to the solution
    Values initialEstimate;
    Pose3 delta(Rot3::Rodrigues(-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 nullptr
          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;
  }
  /* ************************************************************************* */
  
  