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

/* ************************************************************************** */
TEST(JointLimitFactorPose2Vector, optimization_2) {
  // over down limit

  // settings
  noiseModel::Gaussian::shared_ptr cost_model = noiseModel::Isotropic::Sigma(5, 0.001);
  noiseModel::Gaussian::shared_ptr prior_model = noiseModel::Isotropic::Sigma(5, 1000);
  Key qkey = Symbol('x', 0);
  Vector5 dlimit = (Vector5() << 0, 0, 0, -5.0, -10.0).finished();
  Vector5 ulimit = (Vector5() << 0, 0, 0, 5, 10.0).finished();
  Vector5 thresh = (Vector5() << 0, 0, 0, 2.0, 2.0).finished();

  Pose2Vector conf(Pose2(1, -2, 3), Vector2(-10.0, -10.0));

  NonlinearFactorGraph graph;
  graph.add(JointLimitFactorPose2Vector(qkey, cost_model, dlimit, ulimit, thresh));
  graph.add(PriorFactor<Pose2Vector>(qkey, conf, prior_model));
  Values init_values;
  init_values.insert(qkey, conf);

  GaussNewtonParams parameters;
  parameters.setVerbosity("ERROR");
  parameters.setAbsoluteErrorTol(1e-12);
  GaussNewtonOptimizer optimizer(graph, init_values, parameters);
  optimizer.optimize();
  Values results = optimizer.values();

  Vector conf_limit = (Vector(2) << -3.0, -8.0).finished();
  EXPECT(assert_equal(conf_limit, results.at<Pose2Vector>(qkey).configuration(), 1e-6));
}

/* ************************************************************************* */
NonlinearFactorGraph planarSLAMGraph() {
  NonlinearFactorGraph graph;

  // Prior on pose x1 at the origin.
  Pose2 prior(0.0, 0.0, 0.0);
  auto priorNoise = noiseModel::Diagonal::Sigmas(Vector3(0.3, 0.3, 0.1));
  graph.add(PriorFactor<Pose2>(x1, prior, priorNoise));

  // Two odometry factors
  Pose2 odometry(2.0, 0.0, 0.0);
  auto odometryNoise = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1));
  graph.add(BetweenFactor<Pose2>(x1, x2, odometry, odometryNoise));
  graph.add(BetweenFactor<Pose2>(x2, x3, odometry, odometryNoise));

  // Add Range-Bearing measurements to two different landmarks
  auto measurementNoise = noiseModel::Diagonal::Sigmas(Vector2(0.1, 0.2));
  Rot2 bearing11 = Rot2::fromDegrees(45), bearing21 = Rot2::fromDegrees(90),
       bearing32 = Rot2::fromDegrees(90);
  double range11 = std::sqrt(4.0 + 4.0), range21 = 2.0, range32 = 2.0;
  graph.add(BearingRangeFactor<Pose2, Point2>(x1, l1, bearing11, range11, measurementNoise));
  graph.add(BearingRangeFactor<Pose2, Point2>(x2, l1, bearing21, range21, measurementNoise));
  graph.add(BearingRangeFactor<Pose2, Point2>(x3, l2, bearing32, range32, measurementNoise));

  return graph;
}

/* ************************************************************************* */
TEST( StereoFactor, singlePoint)
{
  NonlinearFactorGraph graph;

  graph.add(NonlinearEquality<Pose3>(X(1), camera1));

  StereoPoint2 measurement(320, 320.0-50, 240);
  // arguments: measurement, sigma, cam#, measurement #, K, baseline (m)
  graph.add(GenericStereoFactor<Pose3, Point3>(measurement, model, X(1), L(1), K));

  // Create a configuration corresponding to the ground truth
  Values values;
  values.insert(X(1), camera1); // add camera at z=6.25m looking towards origin

  Point3 l1(0, 0, 0);
  values.insert(L(1), l1);   // add point at origin;

  GaussNewtonOptimizer optimizer(graph, values);

  // We expect the initial to be zero because config is the ground truth
  DOUBLES_EQUAL(0.0, optimizer.error(), 1e-9);

  // Iterate once, and the config should not have changed
  optimizer.iterate();
  DOUBLES_EQUAL(0.0, optimizer.error(), 1e-9);

  // Complete solution
  optimizer.optimize();

  DOUBLES_EQUAL(0.0, optimizer.error(), 1e-6);
}

/* ************************************************************************* */
TEST ( NonlinearEquality, allow_error_optimize_with_factors ) {

	// create a hard constraint
  Symbol key1('x',1);
	Pose2 feasible1(1.0, 2.0, 3.0);

	// initialize away from the ideal
	Values init;
	Pose2 initPose(0.0, 2.0, 3.0);
	init.insert(key1, initPose);

	double error_gain = 500.0;
	PoseNLE nle(key1, feasible1, error_gain);

	// create a soft prior that conflicts
	PosePrior prior(key1, initPose, noiseModel::Isotropic::Sigma(3, 0.1));

	// add to a graph
	NonlinearFactorGraph graph;
	graph.add(nle);
	graph.add(prior);

	// optimize
	Ordering ordering;
	ordering.push_back(key1);
  Values actual = LevenbergMarquardtOptimizer(graph, init, ordering).optimize();

	// verify
	Values expected;
	expected.insert(key1, feasible1);
	EXPECT(assert_equal(expected, actual));
}

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

/* ************************************************************************* */
TEST( dataSet, readG2oTukey)
{
  const string g2oFile = findExampleDataFile("pose2example");
  NonlinearFactorGraph::shared_ptr actualGraph;
  Values::shared_ptr actualValues;
  bool is3D = false;
  boost::tie(actualGraph, actualValues) = readG2o(g2oFile, is3D, KernelFunctionTypeTUKEY);

  noiseModel::Diagonal::shared_ptr baseModel = noiseModel::Diagonal::Precisions((Vector(3) << 44.721360, 44.721360, 30.901699));
  SharedNoiseModel model = noiseModel::Robust::Create(noiseModel::mEstimator::Tukey::Create(4.6851), baseModel);

  NonlinearFactorGraph expectedGraph;
  expectedGraph.add(BetweenFactor<Pose2>(0, 1, Pose2(1.030390, 0.011350, -0.081596), model));
  expectedGraph.add(BetweenFactor<Pose2>(1, 2, Pose2(1.013900, -0.058639, -0.220291), model));
  expectedGraph.add(BetweenFactor<Pose2>(2, 3, Pose2(1.027650, -0.007456, -0.043627), model));
  expectedGraph.add(BetweenFactor<Pose2>(3, 4, Pose2(-0.012016, 1.004360, 1.560229), model));
  expectedGraph.add(BetweenFactor<Pose2>(4, 5, Pose2(1.016030, 0.014565, -0.030930), model));
  expectedGraph.add(BetweenFactor<Pose2>(5, 6, Pose2(1.023890, 0.006808, -0.007452), model));
  expectedGraph.add(BetweenFactor<Pose2>(6, 7, Pose2(0.957734, 0.003159, 0.082836), model));
  expectedGraph.add(BetweenFactor<Pose2>(7, 8, Pose2(-1.023820, -0.013668, -3.084560), model));
  expectedGraph.add(BetweenFactor<Pose2>(8, 9, Pose2(1.023440, 0.013984, -0.127624), model));
  expectedGraph.add(BetweenFactor<Pose2>(9,10, Pose2(1.003350, 0.022250, -0.195918), model));
  expectedGraph.add(BetweenFactor<Pose2>(5, 9, Pose2(0.033943, 0.032439, 3.073637), model));
  expectedGraph.add(BetweenFactor<Pose2>(3,10, Pose2(0.044020, 0.988477, -1.553511), model));
  EXPECT(assert_equal(expectedGraph,*actualGraph,1e-5));
}

//*************************************************************************
TEST (EssentialMatrixFactor2, minimization) {
    // Here we want to optimize for E and inverse depths at the same time

    // We start with a factor graph and add constraints to it
    // Noise sigma is 1cm, assuming metric measurements
    NonlinearFactorGraph graph;
    for (size_t i = 0; i < 5; i++)
        graph.add(EssentialMatrixFactor2(100, i, pA(i), pB(i), model2));

    // Check error at ground truth
    Values truth;
    truth.insert(100, trueE);
    for (size_t i = 0; i < 5; i++) {
        Point3 P1 = data.tracks[i].p;
        truth.insert(i, double(baseline / P1.z()));
    }
    EXPECT_DOUBLES_EQUAL(0, graph.error(truth), 1e-8);

    // Optimize
    LevenbergMarquardtParams parameters;
    // parameters.setVerbosity("ERROR");
    LevenbergMarquardtOptimizer optimizer(graph, truth, parameters);
    Values result = optimizer.optimize();

    // Check result
    EssentialMatrix actual = result.at<EssentialMatrix>(100);
    EXPECT(assert_equal(trueE, actual, 1e-1));
    for (size_t i = 0; i < 5; i++)
        EXPECT_DOUBLES_EQUAL(truth.at<double>(i), result.at<double>(i), 1e-1);

    // Check error at result
    EXPECT_DOUBLES_EQUAL(0, graph.error(result), 1e-4);
}

/* ************************************************************************** */
TEST(GaussianPriorWorkspacePoseArm, optimization) {

  noiseModel::Gaussian::shared_ptr cost_model = noiseModel::Isotropic::Sigma(6, 0.1);

  Vector a = (Vector(2) << 1, 1).finished();
  Vector alpha = (Vector(2) << 0, 0).finished();
  Vector d = (Vector(2) << 0, 0).finished();
  ArmModel arm = ArmModel(Arm(2, a, alpha, d), BodySphereVector());
  Pose3 des = Pose3(Rot3(), Point3(2, 0, 0));

  Key qkey = Symbol('x', 0);
  Vector q = (Vector(2) << 0, 0).finished();
  Vector qinit = (Vector(2) << M_PI/2, M_PI/2).finished();

  NonlinearFactorGraph graph;
  graph.add(GaussianPriorWorkspacePoseArm(qkey, arm, 1, des, cost_model));
  Values init_values;
  init_values.insert(qkey, qinit);

  LevenbergMarquardtParams parameters;
  parameters.setVerbosity("ERROR");
  parameters.setAbsoluteErrorTol(1e-12);
  LevenbergMarquardtOptimizer optimizer(graph, init_values, parameters);
  optimizer.optimize();
  Values results = optimizer.values();

  EXPECT_DOUBLES_EQUAL(0, graph.error(results), 1e-3);
  EXPECT(assert_equal(q, results.at<Vector>(qkey), 1e-3));
}

/* ************************************************************************** */
TEST(GoalFactorArm, optimization_1) {

  // use optimization to solve inverse kinematics
  noiseModel::Gaussian::shared_ptr cost_model = noiseModel::Isotropic::Sigma(3, 0.1);

  // 2 link simple example
  Vector a = (Vector(2) << 1, 1).finished();
  Vector alpha = (Vector(2) << 0, 0).finished();
  Vector d = (Vector(2) << 0, 0).finished();
  Arm arm(2, a, alpha, d);
  Point3 goal(1.414213562373095, 1.414213562373095, 0);

  Key qkey = Symbol('x', 0);
  Vector q = (Vector(2) << M_PI/4.0, 0).finished();
  Vector qinit = (Vector(2) << 0, 0).finished();

  NonlinearFactorGraph graph;
  graph.add(GoalFactorArm(qkey, cost_model, arm, goal));
  Values init_values;
  init_values.insert(qkey, qinit);

  LevenbergMarquardtParams parameters;
  parameters.setVerbosity("ERROR");
  parameters.setAbsoluteErrorTol(1e-12);
  LevenbergMarquardtOptimizer optimizer(graph, init_values, parameters);
  optimizer.optimize();
  Values results = optimizer.values();

  EXPECT_DOUBLES_EQUAL(0, graph.error(results), 1e-3);
  EXPECT(assert_equal(q, results.at<Vector>(qkey), 1e-3));
}

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

/* ************************************************************************* */
TEST ( NonlinearEquality, allow_error_optimize ) {
  Symbol key1('x',1);
	Pose2 feasible1(1.0, 2.0, 3.0);
	double error_gain = 500.0;
	PoseNLE nle(key1, feasible1, error_gain);

	// add to a graph
	NonlinearFactorGraph graph;
	graph.add(nle);

	// initialize away from the ideal
	Pose2 initPose(0.0, 2.0, 3.0);
	Values init;
	init.insert(key1, initPose);

	// optimize
	Ordering ordering;
	ordering.push_back(key1);
	Values result = LevenbergMarquardtOptimizer(graph, init, ordering).optimize();

	// verify
	Values expected;
	expected.insert(key1, feasible1);
	EXPECT(assert_equal(expected, result));
}

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

/* ********************************************************************* */
TEST (testNonlinearEqualityConstraint, map_warp ) {
	// get a graph
  NonlinearFactorGraph graph;

	// keys
  Symbol x1('x',1), x2('x',2);
  Symbol l1('l',1), l2('l',2);

	// constant constraint on x1
	Point2 pose1(1.0, 1.0);
	graph.add(eq2D::UnaryEqualityConstraint(pose1, x1));

	SharedDiagonal sigma = noiseModel::Isotropic::Sigma(1,0.1);

	// measurement from x1 to l1
	Point2 z1(0.0, 5.0);
	graph.add(simulated2D::Measurement(z1, sigma, x1, l1));

	// measurement from x2 to l2
	Point2 z2(-4.0, 0.0);
	graph.add(simulated2D::Measurement(z2, sigma, x2, l2));

	// equality constraint between l1 and l2
	graph.add(eq2D::PointEqualityConstraint(l1, l2));

	// create an initial estimate
	Values initialEstimate;
	initialEstimate.insert(x1, Point2( 1.0, 1.0));
	initialEstimate.insert(l1, Point2( 1.0, 6.0));
	initialEstimate.insert(l2, Point2(-4.0, 0.0)); // starting with a separate reference frame
	initialEstimate.insert(x2, Point2( 0.0, 0.0)); // other pose starts at origin

	// optimize
	Values actual = LevenbergMarquardtOptimizer(graph, initialEstimate).optimize();

	Values expected;
	expected.insert(x1, Point2(1.0, 1.0));
	expected.insert(l1, Point2(1.0, 6.0));
	expected.insert(l2, Point2(1.0, 6.0));
	expected.insert(x2, Point2(5.0, 6.0));
	CHECK(assert_equal(expected, actual, tol));
}

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

/* ************************************************************************* */
TEST(NonlinearOptimizer, MoreOptimization) {

  NonlinearFactorGraph fg;
  fg.add(PriorFactor<Pose2>(0, Pose2(0,0,0), noiseModel::Isotropic::Sigma(3,1)));
  fg.add(BetweenFactor<Pose2>(0, 1, Pose2(1,0,M_PI/2), noiseModel::Isotropic::Sigma(3,1)));
  fg.add(BetweenFactor<Pose2>(1, 2, Pose2(1,0,M_PI/2), noiseModel::Isotropic::Sigma(3,1)));

  Values init;
  init.insert(0, Pose2(3,4,-M_PI));
  init.insert(1, Pose2(10,2,-M_PI));
  init.insert(2, Pose2(11,7,-M_PI));

  Values expected;
  expected.insert(0, Pose2(0,0,0));
  expected.insert(1, Pose2(1,0,M_PI/2));
  expected.insert(2, Pose2(1,1,M_PI));

  // Try LM and Dogleg
  EXPECT(assert_equal(expected, LevenbergMarquardtOptimizer(fg, init).optimize()));
  EXPECT(assert_equal(expected, DoglegOptimizer(fg, init).optimize()));
}