C++ NonlinearFactorGraph::print方法代码示例

int main(const int argc, const char *argv[]) {

  // Read graph from file
  string g2oFile;
  if (argc < 2)
    g2oFile = findExampleDataFile("noisyToyGraph.txt");
  else
    g2oFile = argv[1];

  NonlinearFactorGraph::shared_ptr graph;
  Values::shared_ptr initial;
  boost::tie(graph, initial) = readG2o(g2oFile);

  // Add prior on the pose having index (key) = 0
  NonlinearFactorGraph graphWithPrior = *graph;
  noiseModel::Diagonal::shared_ptr priorModel = //
      noiseModel::Diagonal::Variances((Vector(3) << 1e-6, 1e-6, 1e-8));
  graphWithPrior.add(PriorFactor<Pose2>(0, Pose2(), priorModel));
  graphWithPrior.print();

  std::cout << "Computing LAGO estimate" << std::endl;
  Values estimateLago = lago::initialize(graphWithPrior);
  std::cout << "done!" << std::endl;

  if (argc < 3) {
    estimateLago.print("estimateLago");
  } else {
    const string outputFile = argv[2];
    std::cout << "Writing results to file: " << outputFile << std::endl;
    writeG2o(*graph, estimateLago, outputFile);
    std::cout << "done! " << std::endl;
  }

  return 0;
}

int main(int argc, char** argv) {

  // 1. Create a factor graph container and add factors to it
  NonlinearFactorGraph graph;

  // 2a. Add a prior on the first pose, setting it to the origin
  // A prior factor consists of a mean and a noise model (covariance matrix)
  Pose2 prior(0.0, 0.0, 0.0); // prior at origin
  noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas(Vector_(3, 0.3, 0.3, 0.1));
  graph.add(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(Vector_(3, 0.2, 0.2, 0.1));
  // Create odometry (Between) factors between consecutive poses
  graph.add(BetweenFactor<Pose2>(1, 2, Pose2(2.0, 0.0, M_PI_2),    odometryNoise));
  graph.add(BetweenFactor<Pose2>(2, 3, Pose2(2.0, 0.0, M_PI_2), odometryNoise));
  graph.add(BetweenFactor<Pose2>(3, 4, Pose2(2.0, 0.0, M_PI_2), odometryNoise));
  graph.add(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(Vector_(3, 0.2, 0.2, 0.1));
  graph.add(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));
  initialEstimate.insert(3, Pose2(2.1, 1.9, 2.8));
  initialEstimate.insert(4, Pose2(-.3, 2.5, 4.2));
  initialEstimate.insert(5, Pose2(0.1,-0.7, 5.8));
  initialEstimate.print("\nInitial Estimate:\n"); // print

  // 4. Single Step Optimization using Levenberg-Marquardt
  LevenbergMarquardtParams parameters;
  parameters.verbosity = NonlinearOptimizerParams::ERROR;
  parameters.verbosityLM = LevenbergMarquardtParams::LAMBDA;
  parameters.linearSolverType = SuccessiveLinearizationParams::CONJUGATE_GRADIENT;

  {
    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;
}

/* ************************************************************************* */
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
  vector<Point3> points = createPoints();

  // Create the set of ground-truth poses
  vector<Pose3> poses = createPoses();

  // Create a factor graph
  NonlinearFactorGraph graph;

  // Add a prior on pose x1. This indirectly specifies where the origin is.
  noiseModel::Diagonal::shared_ptr poseNoise = noiseModel::Diagonal::Sigmas((Vector(6) << Vector3::Constant(0.3), Vector3::Constant(0.1))); // 30cm std on x,y,z 0.1 rad on roll,pitch,yaw
  graph.push_back(PriorFactor<Pose3>(Symbol('x', 0), poses[0], poseNoise)); // add directly to graph

  // Simulated measurements from each camera pose, adding them to the factor graph
  for (size_t i = 0; i < poses.size(); ++i) {
    for (size_t j = 0; j < points.size(); ++j) {
      SimpleCamera camera(poses[i], *K);
      Point2 measurement = camera.project(points[j]);
      graph.push_back(GenericProjectionFactor<Pose3, Point3, Cal3_S2>(measurement, measurementNoise, Symbol('x', i), Symbol('l', j), K));
    }
  }

  // Because the structure-from-motion problem has a scale ambiguity, the problem is still under-constrained
  // Here we add a prior on the position of the first landmark. This fixes the scale by indicating the distance
  // between the first camera and the first landmark. All other landmark positions are interpreted using this scale.
  noiseModel::Isotropic::shared_ptr pointNoise = noiseModel::Isotropic::Sigma(3, 0.1);
  graph.push_back(PriorFactor<Point3>(Symbol('l', 0), points[0], pointNoise)); // add directly to graph
  graph.print("Factor Graph:\n");

  // Create the data structure to hold the initial estimate to the solution
  // Intentionally initialize the variables off from the ground truth
  Values initialEstimate;
  for (size_t i = 0; i < poses.size(); ++i)
    initialEstimate.insert(Symbol('x', i), poses[i].compose(Pose3(Rot3::rodriguez(-0.1, 0.2, 0.25), Point3(0.05, -0.10, 0.20))));
  for (size_t j = 0; j < points.size(); ++j)
    initialEstimate.insert(Symbol('l', j), points[j].compose(Point3(-0.25, 0.20, 0.15)));
  initialEstimate.print("Initial Estimates:\n");

  /* Optimize the graph and print results */
  Values result = DoglegOptimizer(graph, initialEstimate).optimize();
  result.print("Final results:\n");

  return 0;
}

int main(int argc, char** argv) {

  // 1. Create a factor graph container and add factors to it
  NonlinearFactorGraph graph;

  // 2a. Add a prior on the first pose, setting it to the origin
  Pose2 prior(0.0, 0.0, 0.0); // prior at origin
  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
  noiseModel::Diagonal::shared_ptr odometryNoise = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1));
  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
  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
  Values initialEstimate;
  initialEstimate.insert(1, Pose2(0.5, 0.0, 0.2));
  initialEstimate.insert(2, Pose2(2.3, 0.1, 1.1));
  initialEstimate.insert(3, Pose2(2.1, 1.9, 2.8));
  initialEstimate.insert(4, Pose2(-.3, 2.5, 4.2));
  initialEstimate.insert(5, Pose2(0.1,-0.7, 5.8));
  initialEstimate.print("\nInitial Estimate:\n"); // print

  // 4. Single Step Optimization using Levenberg-Marquardt
  LevenbergMarquardtParams parameters;
  parameters.verbosity = NonlinearOptimizerParams::ERROR;
  parameters.verbosityLM = LevenbergMarquardtParams::LAMBDA;

  // LM is still the outer optimization loop, but by specifying "Iterative" below
  // 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;
}

int main(int argc, char** argv) {

    // 1. Create a factor graph container and add factors to it
    NonlinearFactorGraph graph;

    // 2a. Add odometry factors
    // For simplicity, we will use the same noise model for each odometry factor
    noiseModel::Diagonal::shared_ptr odometryNoise = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1));
    // Create odometry (Between) factors between consecutive poses
    graph.add(BetweenFactor<Pose2>(1, 2, Pose2(2.0, 0.0, 0.0), odometryNoise));
    graph.add(BetweenFactor<Pose2>(2, 3, Pose2(2.0, 0.0, 0.0), odometryNoise));

    // 2b. Add "GPS-like" measurements
    // We will use our custom UnaryFactor for this.
    noiseModel::Diagonal::shared_ptr unaryNoise = noiseModel::Diagonal::Sigmas(Vector2(0.1, 0.1)); // 10cm std on x,y
    graph.add(boost::make_shared<UnaryFactor>(1, 0.0, 0.0, unaryNoise));
    graph.add(boost::make_shared<UnaryFactor>(2, 2.0, 0.0, unaryNoise));
    graph.add(boost::make_shared<UnaryFactor>(3, 4.0, 0.0, unaryNoise));
    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, -0.2));
    initialEstimate.insert(3, Pose2(4.1, 0.1, 0.1));
    initialEstimate.print("\nInitial Estimate:\n"); // print

    // 4. Optimize using Levenberg-Marquardt optimization. The optimizer
    // accepts an optional set of configuration parameters, controlling
    // things like convergence criteria, the type of linear system solver
    // to use, and the amount of information displayed during optimization.
    // Here we will use the default set of parameters.  See the
    // documentation for the full set of parameters.
    LevenbergMarquardtOptimizer optimizer(graph, initialEstimate);
    Values result = optimizer.optimize();
    result.print("Final Result:\n");

    // 5. Calculate and print marginal covariances for all variables
    Marginals marginals(graph, result);
    cout << "x1 covariance:\n" << marginals.marginalCovariance(1) << endl;
    cout << "x2 covariance:\n" << marginals.marginalCovariance(2) << endl;
    cout << "x3 covariance:\n" << marginals.marginalCovariance(3) << endl;

    return 0;
}

int main(int argc, char** argv) {

  // Create an empty nonlinear factor graph
  NonlinearFactorGraph graph;

  // Add a prior on the first pose, setting it to the origin
  // A prior factor consists of a mean and a noise model (covariance matrix)
  Pose2 priorMean(0.0, 0.0, 0.0); // prior at origin
  noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas(Vector3(0.3, 0.3, 0.1));
  graph.add(PriorFactor<Pose2>(1, priorMean, priorNoise));

  // Add odometry factors
  Pose2 odometry(2.0, 0.0, 0.0);
  // For simplicity, we will use the same noise model for each odometry factor
  noiseModel::Diagonal::shared_ptr odometryNoise = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1));
  // Create odometry (Between) factors between consecutive poses
  graph.add(BetweenFactor<Pose2>(1, 2, odometry, odometryNoise));
  graph.add(BetweenFactor<Pose2>(2, 3, odometry, odometryNoise));
  graph.print("\nFactor Graph:\n"); // print

  // Create the data structure to hold the initialEstimate estimate to the solution
  // For illustrative purposes, these have been deliberately set to incorrect values
  Values initial;
  initial.insert(1, Pose2(0.5, 0.0, 0.2));
  initial.insert(2, Pose2(2.3, 0.1, -0.2));
  initial.insert(3, Pose2(4.1, 0.1, 0.1));
  initial.print("\nInitial Estimate:\n"); // print

  // optimize using Levenberg-Marquardt optimization
  Values result = LevenbergMarquardtOptimizer(graph, initial).optimize();
  result.print("Final Result:\n");

  // Calculate and print marginal covariances for all variables
  cout.precision(2);
  Marginals marginals(graph, result);
  cout << "x1 covariance:\n" << marginals.marginalCovariance(1) << endl;
  cout << "x2 covariance:\n" << marginals.marginalCovariance(2) << endl;
  cout << "x3 covariance:\n" << marginals.marginalCovariance(3) << endl;

  return 0;
}

int main() {

	/**
	 *    Step 1: Create a factor to express a unary constraint
	 * The "prior" in this case is the measurement from a sensor,
	 * with a model of the noise on the measurement.
	 *
	 * The "Key" created here is a label used to associate parts of the
	 * state (stored in "RotValues") with particular factors.  They require
	 * an index to allow for lookup, and should be unique.
	 *
	 * In general, creating a factor requires:
	 *  - A key or set of keys labeling the variables that are acted upon
	 *  - A measurement value
	 *  - A measurement model with the correct dimensionality for the factor
	 */
	Rot2 prior = Rot2::fromAngle(30 * degree);
	prior.print("goal angle");
	noiseModel::Isotropic::shared_ptr model = noiseModel::Isotropic::Sigma(1, 1 * degree);
	Symbol key('x',1);
	PriorFactor<Rot2> factor(key, prior, model);

	/**
	 *    Step 2: Create a graph container and add the factor to it
	 * Before optimizing, all factors need to be added to a Graph container,
	 * which provides the necessary top-level functionality for defining a
	 * system of constraints.
	 *
	 * In this case, there is only one factor, but in a practical scenario,
	 * many more factors would be added.
	 */
	NonlinearFactorGraph graph;
	graph.add(factor);
	graph.print("full graph");

	/**
	 *    Step 3: Create an initial estimate
	 * An initial estimate of the solution for the system is necessary to
	 * start optimization.  This system state is the "RotValues" structure,
	 * which is similar in structure to a STL map, in that it maps
	 * keys (the label created in step 1) to specific values.
	 *
	 * The initial estimate provided to optimization will be used as
	 * a linearization point for optimization, so it is important that
	 * all of the variables in the graph have a corresponding value in
	 * this structure.
	 *
	 * The interface to all RotValues types is the same, it only depends
	 * on the type of key used to find the appropriate value map if there
	 * are multiple types of variables.
	 */
	Values initial;
	initial.insert(key, Rot2::fromAngle(20 * degree));
	initial.print("initial estimate");

	/**
	 *    Step 4: Optimize
	 * After formulating the problem with a graph of constraints
	 * and an initial estimate, executing optimization is as simple
	 * as calling a general optimization function with the graph and
	 * initial estimate.  This will yield a new RotValues structure
	 * with the final state of the optimization.
	 */
	Values result = LevenbergMarquardtOptimizer(graph, initial).optimize();
	result.print("final result");

	return 0;
}

/* ************************************************************************* */
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:
      //    1. the 2D measurement
      //    2. the corresponding camera's key
      //    3. camera noise model
      //    4. camera calibration
      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.3), Vector3::Constant(0.1)).finished());
  graph.push_back(PriorFactor<Pose3>(0, poses[0], noise));

  // Because the structure-from-motion problem has a scale ambiguity, the problem is
  // still under-constrained. Here we add a prior on the second pose x1, so this will
  // fix the scale by indicating the distance between x0 and x1.
  // Because these two are fixed, the rest of the poses will be also be fixed.
  graph.push_back(PriorFactor<Pose3>(1, poses[1], noise)); // add directly to graph

  graph.print("Factor Graph:\n");

  // Create the initial estimate to the solution
  // Intentionally initialize the variables off from the ground truth
  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));
  initialEstimate.print("Initial Estimates:\n");

  // Optimize the graph and print results
  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.
  // The 3D position of the landmark is not explicitly calculated by the optimizer.
  // To obtain the landmark's 3D position, we use the "point" method of the smart factor.
  Values landmark_result;
  for (size_t j = 0; j < points.size(); ++j) {

    // 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) {
      // 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;
}

int main(int argc, char** argv) {

	// 1. Create a factor graph container and add factors to it
	NonlinearFactorGraph graph;

	// 2a. Add a prior on the first pose, setting it to the origin
	// A prior factor consists of a mean and a noise model (covariance matrix)
	noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas(Vector_(3, 0.3, 0.3, 0.1));
	graph.add(PriorFactor<Pose2>(1, Pose2(0, 0, 0), priorNoise));

	// For simplicity, we will use the same noise model for odometry and loop closures
	noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Sigmas(Vector_(3, 0.2, 0.2, 0.1));

	// 2b. Add odometry factors
	// Create odometry (Between) factors between consecutive poses
	graph.add(BetweenFactor<Pose2>(1, 2, Pose2(2, 0, 0     ), model));
	graph.add(BetweenFactor<Pose2>(2, 3, Pose2(2, 0, M_PI_2), model));
	graph.add(BetweenFactor<Pose2>(3, 4, Pose2(2, 0, M_PI_2), model));
	graph.add(BetweenFactor<Pose2>(4, 5, Pose2(2, 0, M_PI_2), model));

	// 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:
	graph.add(BetweenFactor<Pose2>(5, 2, Pose2(2, 0, M_PI_2), 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, -0.2   ));
	initialEstimate.insert(3, Pose2(4.1, 0.1,  M_PI_2));
	initialEstimate.insert(4, Pose2(4.0, 2.0,  M_PI  ));
	initialEstimate.insert(5, Pose2(2.1, 2.1, -M_PI_2));
	initialEstimate.print("\nInitial Estimate:\n"); // print

  // 4. Optimize the initial values using a Gauss-Newton nonlinear optimizer
  // The optimizer accepts an optional set of configuration parameters,
	// controlling things like convergence criteria, the type of linear
	// system solver to use, and the amount of information displayed during
	// optimization. We will set a few parameters as a demonstration.
	GaussNewtonParams parameters;
	// Stop iterating once the change in error between steps is less than this value
	parameters.relativeErrorTol = 1e-5;
	// Do not perform more than N iteration steps
	parameters.maxIterations = 100;
	// Create the optimizer ...
	GaussNewtonOptimizer optimizer(graph, initialEstimate, parameters);
	// ... and optimize
  Values result = optimizer.optimize();
  result.print("Final Result:\n");

  // 5. Calculate and print marginal covariances for all variables
  cout.precision(3);
  Marginals marginals(graph, result);
  cout << "x1 covariance:\n" << marginals.marginalCovariance(1) << endl;
  cout << "x2 covariance:\n" << marginals.marginalCovariance(2) << endl;
  cout << "x3 covariance:\n" << marginals.marginalCovariance(3) << endl;
  cout << "x4 covariance:\n" << marginals.marginalCovariance(4) << endl;
  cout << "x5 covariance:\n" << marginals.marginalCovariance(5) << endl;

	return 0;
}

int main(int argc, char** argv) {

  // Create a factor graph container
  NonlinearFactorGraph graph;

  // first state prior noise model (covariance matrix)
  noiseModel::Diagonal::shared_ptr priorModel = noiseModel::Diagonal::Sigmas(Vector2(0.2, 0.2));
  
  // add prior factor on first state (at origin)
  graph.add(PriorFactor<Point2c>(Symbol('x', 1), Point2c(0, 0), priorModel));

  // odometry measurement noise model (covariance matrix)
  noiseModel::Diagonal::shared_ptr odomModel = noiseModel::Diagonal::Sigmas(Vector2(0.5, 0.5));

  // Add odometry factors
  // Create odometry (Between) factors between consecutive point2c
  graph.add(BetweenFactor<Point2c>(Symbol('x', 1), Symbol('x', 2), Point2c(2, 0), odomModel));
  graph.add(BetweenFactor<Point2c>(Symbol('x', 2), Symbol('x', 3), Point2c(2, 0), odomModel));
  graph.add(BetweenFactor<Point2c>(Symbol('x', 3), Symbol('x', 4), Point2c(2, 0), odomModel));
  graph.add(BetweenFactor<Point2c>(Symbol('x', 4), Symbol('x', 5), Point2c(2, 0), odomModel));
  
  // print factor graph
  graph.print("\nFactor Graph:\n"); 


  // initial varible values for the optimization
  // add random noise from ground truth values
  Values initials;
  initials.insert(Symbol('x', 1), Point2c(0.2, -0.3));
  initials.insert(Symbol('x', 2), Point2c(2.1, 0.3));
  initials.insert(Symbol('x', 3), Point2c(3.9, -0.1));
  initials.insert(Symbol('x', 4), Point2c(5.9, -0.3));
  initials.insert(Symbol('x', 5), Point2c(8.2, 0.1));
  
  // print initial values
  initials.print("\nInitial Values:\n"); 


  // Use Gauss-Newton method optimizes the initial values
  GaussNewtonParams parameters;
  
  // print per iteration
  parameters.setVerbosity("ERROR");
  
  // optimize!
  GaussNewtonOptimizer optimizer(graph, initials, parameters);
  Values results = optimizer.optimize();
  
  // print final values
  results.print("Final Result:\n");


  // Calculate marginal covariances for all poses
  Marginals marginals(graph, results);
  
  // print marginal covariances
  cout << "x1 covariance:\n" << marginals.marginalCovariance(Symbol('x', 1)) << endl;
  cout << "x2 covariance:\n" << marginals.marginalCovariance(Symbol('x', 2)) << endl;
  cout << "x3 covariance:\n" << marginals.marginalCovariance(Symbol('x', 3)) << endl;
  cout << "x4 covariance:\n" << marginals.marginalCovariance(Symbol('x', 4)) << endl;
  cout << "x5 covariance:\n" << marginals.marginalCovariance(Symbol('x', 5)) << endl;

  return 0;
}