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

/* ************************************************************************* */
TEST( testNonlinearEqualityConstraint, unary_simple_optimization ) {
	// create a single-node graph with a soft and hard constraint to
	// ensure that the hard constraint overrides the soft constraint
	Point2 truth_pt(1.0, 2.0);
  Symbol key('x',1);
	double mu = 10.0;
	eq2D::UnaryEqualityConstraint::shared_ptr constraint(
			new eq2D::UnaryEqualityConstraint(truth_pt, key, mu));

	Point2 badPt(100.0, -200.0);
	simulated2D::Prior::shared_ptr factor(
			new simulated2D::Prior(badPt, soft_model, key));

	NonlinearFactorGraph graph;
	graph.push_back(constraint);
	graph.push_back(factor);

	Values initValues;
	initValues.insert(key, badPt);

	// verify error values
	EXPECT(constraint->active(initValues));

	Values expected;
	expected.insert(key, truth_pt);
	EXPECT(constraint->active(expected));
	EXPECT_DOUBLES_EQUAL(0.0, constraint->error(expected), tol);

	Values actual = LevenbergMarquardtOptimizer(graph, initValues).optimize();
	EXPECT(assert_equal(expected, actual, tol));
}

/*******************************************************************************
 * Camera: f = 1, Image: 100x100, center: 50, 50.0
 * Pose (ground truth): (Xw, -Yw, -Zw, [0,0,2.0]')
 * Known landmarks:
 *    3D Points: (10,10,0) (-10,10,0) (-10,-10,0) (10,-10,0)
 * Perfect measurements:
 *    2D Point:  (55,45)   (45,45)    (45,55)     (55,55)
 *******************************************************************************/
int main(int argc, char* argv[]) {
  /* read camera intrinsic parameters */
  Cal3_S2::shared_ptr calib(new Cal3_S2(1, 1, 0, 50, 50));

  /* 1. create graph */
  NonlinearFactorGraph graph;

  /* 2. add factors to the graph */
  // add measurement factors
  SharedDiagonal measurementNoise = Diagonal::Sigmas((Vector(2) << 0.5, 0.5));
  boost::shared_ptr<ResectioningFactor> factor;
  graph.push_back(
      boost::make_shared<ResectioningFactor>(measurementNoise, X(1), calib,
          Point2(55, 45), Point3(10, 10, 0)));
  graph.push_back(
      boost::make_shared<ResectioningFactor>(measurementNoise, X(1), calib,
          Point2(45, 45), Point3(-10, 10, 0)));
  graph.push_back(
      boost::make_shared<ResectioningFactor>(measurementNoise, X(1), calib,
          Point2(45, 55), Point3(-10, -10, 0)));
  graph.push_back(
      boost::make_shared<ResectioningFactor>(measurementNoise, X(1), calib,
          Point2(55, 55), Point3(10, -10, 0)));

  /* 3. Create an initial estimate for the camera pose */
  Values initial;
  initial.insert(X(1),
      Pose3(Rot3(1, 0, 0, 0, -1, 0, 0, 0, -1), Point3(0, 0, 2)));

  /* 4. Optimize the graph using Levenberg-Marquardt*/
  Values result = LevenbergMarquardtOptimizer(graph, initial).optimize();
  result.print("Final result:\n");

  return 0;
}

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

  // Find default file, but if an argument is given, try loading a file
  string filename = findExampleDataFile("dubrovnik-3-7-pre");
  if (argc>1) filename = string(argv[1]);

  // Load the SfM data from file
  SfM_data mydata;
  assert(readBAL(filename, mydata));
  cout << boost::format("read %1% tracks on %2% cameras\n") % mydata.number_tracks() % mydata.number_cameras();

  // Create a factor graph
  NonlinearFactorGraph graph;

  // We share *one* noiseModel between all projection factors
  noiseModel::Isotropic::shared_ptr noise =
      noiseModel::Isotropic::Sigma(2, 1.0); // one pixel in u and v

  // Add measurements to the factor graph
  size_t j = 0;
  BOOST_FOREACH(const SfM_Track& track, mydata.tracks) {
    BOOST_FOREACH(const SfM_Measurement& m, track.measurements) {
      size_t i = m.first;
      Point2 uv = m.second;
      graph.push_back(MyFactor(uv, noise, C(i), P(j))); // note use of shorthand symbols C and P
    }
    j += 1;
  }

  // Add a prior on pose x1. This indirectly specifies where the origin is.
  // and a prior on the position of the first landmark to fix the scale
  graph.push_back(PriorFactor<SfM_Camera>(C(0), mydata.cameras[0],  noiseModel::Isotropic::Sigma(9, 0.1)));
  graph.push_back(PriorFactor<Point3>    (P(0), mydata.tracks[0].p, noiseModel::Isotropic::Sigma(3, 0.1)));

  // Create initial estimate
  Values initial;
  size_t i = 0; j = 0;
  BOOST_FOREACH(const SfM_Camera& camera, mydata.cameras) initial.insert(C(i++), camera);
  BOOST_FOREACH(const SfM_Track& track, mydata.tracks)    initial.insert(P(j++), track.p);

  /* Optimize the graph and print results */
  Values result;
  try {
    LevenbergMarquardtParams params;
    params.setVerbosity("ERROR");
    LevenbergMarquardtOptimizer lm(graph, initial, params);
    result = lm.optimize();
  } catch (exception& e) {
    cout << e.what();
  }
  cout << "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[]) {

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

  // Create the factor graph
  NonlinearFactorGraph graph;

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

  // Simulated measurements from each camera pose, adding them to the factor graph
  Cal3_S2 K(50.0, 50.0, 0.0, 50.0, 50.0);
  noiseModel::Isotropic::shared_ptr measurementNoise = noiseModel::Isotropic::Sigma(2, 1.0);
  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]);
      // The only real difference with the Visual SLAM example is that here we use a
      // different factor type, that also calculates the Jacobian with respect to calibration
      graph.push_back(GeneralSFMFactor2<Cal3_S2>(measurement, measurementNoise, Symbol('x', i), Symbol('l', j), Symbol('K', 0)));
    }
  }

  // Add a prior on the position of the first landmark.
  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

  // Add a prior on the calibration.
  noiseModel::Diagonal::shared_ptr calNoise = noiseModel::Diagonal::Sigmas((Vector(5) << 500, 500, 0.1, 100, 100).finished());
  graph.push_back(PriorFactor<Cal3_S2>(Symbol('K', 0), K, calNoise));

  // Create the initial estimate to the solution
  // now including an estimate on the camera calibration parameters
  Values initialEstimate;
  initialEstimate.insert(Symbol('K', 0), Cal3_S2(60.0, 60.0, 0.0, 45.0, 45.0));
  for (size_t i = 0; i < poses.size(); ++i)
    initialEstimate.insert(Symbol('x', i), poses[i].compose(Pose3(Rot3::Rodrigues(-0.1, 0.2, 0.25), Point3(0.05, -0.10, 0.20))));
  for (size_t j = 0; j < points.size(); ++j)
    initialEstimate.insert<Point3>(Symbol('l', j), points[j] + Point3(-0.25, 0.20, 0.15));

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

  return 0;
}

/* ************************************************************************* */
TEST( ConcurrentIncrementalSmootherDL, synchronize_1 )
{
    // Create a set of optimizer parameters
    ISAM2Params parameters;
    parameters.optimizationParams = ISAM2DoglegParams();
//  parameters.maxIterations = 1;

    // Create a Concurrent Batch Smoother
    ConcurrentIncrementalSmoother smoother(parameters);

    // Create a simple separator *from* the filter
    NonlinearFactorGraph smootherFactors, filterSumarization;
    Values smootherValues, filterSeparatorValues;
    filterSeparatorValues.insert(1, Pose3().compose(poseError));
    filterSeparatorValues.insert(2, filterSeparatorValues.at<Pose3>(1).compose(poseOdometry).compose(poseError));

    filterSumarization.push_back(LinearContainerFactor(PriorFactor<Pose3>(1, poseInitial, noisePrior).linearize(filterSeparatorValues), filterSeparatorValues));
    filterSumarization.push_back(LinearContainerFactor(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery).linearize(filterSeparatorValues), filterSeparatorValues));

    // Create expected values: the smoother output will be empty for this case
    NonlinearFactorGraph expectedSmootherSummarization;
    Values expectedSmootherSeparatorValues;

    NonlinearFactorGraph actualSmootherSummarization;
    Values actualSmootherSeparatorValues;
    smoother.presync();
    smoother.getSummarizedFactors(actualSmootherSummarization, actualSmootherSeparatorValues);
    smoother.synchronize(smootherFactors, smootherValues, filterSumarization, filterSeparatorValues);
    smoother.postsync();

    // Check
    CHECK(assert_equal(expectedSmootherSummarization, actualSmootherSummarization, 1e-6));
    CHECK(assert_equal(expectedSmootherSeparatorValues, actualSmootherSeparatorValues, 1e-6));


    // Update the smoother
    smoother.update();

    // Check the factor graph. It should contain only the filter-provided factors
    NonlinearFactorGraph expectedFactorGraph = filterSumarization;
    NonlinearFactorGraph actualFactorGraph = smoother.getFactors();
    CHECK(assert_equal(expectedFactorGraph, actualFactorGraph, 1e-6));

    // Check the optimized value of smoother state
    NonlinearFactorGraph allFactors;
    allFactors.push_back(filterSumarization);
    Values allValues;
    allValues.insert(filterSeparatorValues);
    Values expectedValues = BatchOptimize(allFactors, allValues,1);
    Values actualValues = smoother.calculateEstimate();
    CHECK(assert_equal(expectedValues, actualValues, 1e-6));

    // Check the linearization point. The separator should remain identical to the filter provided values
    Values expectedLinearizationPoint = filterSeparatorValues;
    Values actualLinearizationPoint = smoother.getLinearizationPoint();
    CHECK(assert_equal(expectedLinearizationPoint, actualLinearizationPoint, 1e-6));
}

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
  noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas((Vector(3) << 0.3, 0.3, 0.1));
  graph.push_back(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
  graph.push_back(BetweenFactor<Pose2>(1, 2, Pose2(2, 0, 0     ), model));
  graph.push_back(BetweenFactor<Pose2>(2, 3, Pose2(2, 0, M_PI_2), model));
  graph.push_back(BetweenFactor<Pose2>(3, 4, Pose2(2, 0, M_PI_2), model));
  graph.push_back(BetweenFactor<Pose2>(4, 5, Pose2(2, 0, M_PI_2), model));

  // 2c. Add the loop closure constraint
  graph.push_back(BetweenFactor<Pose2>(5, 2, Pose2(2, 0, M_PI_2), model));

  // 3. Create the data structure to hold the initial 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,  M_PI_2));
  initial.insert(4, Pose2(4.0, 2.0,  M_PI  ));
  initial.insert(5, Pose2(2.1, 2.1, -M_PI_2));

  // Single Step Optimization using Levenberg-Marquardt
  Values result = LevenbergMarquardtOptimizer(graph, initial).optimize();

  // save factor graph as graphviz dot file
  // Render to PDF using "fdp Pose2SLAMExample.dot -Tpdf > graph.pdf"
  ofstream os("Pose2SLAMExample.dot");
  graph.saveGraph(os, result);

  // Also print out to console
  graph.saveGraph(cout, result);

  return 0;
}

/* *************************************************************************/
TEST( SmartProjectionCameraFactor, comparisonGeneralSfMFactor1 ) {

  using namespace bundler;
  Values values;
  values.insert(c1, level_camera);
  values.insert(c2, level_camera_right);

  NonlinearFactorGraph smartGraph;
  SmartFactor::shared_ptr factor1(new SmartFactor(unit2));
  factor1->add(level_uv, c1);
  factor1->add(level_uv_right, c2);
  smartGraph.push_back(factor1);
  Matrix expectedHessian = smartGraph.linearize(values)->hessian().first;
  Vector expectedInfoVector = smartGraph.linearize(values)->hessian().second;
  Point3 expectedPoint;
  if (factor1->point())
    expectedPoint = *(factor1->point());

  // COMMENTS:
  // 1) triangulation introduces small errors, then for a fair comparison we use expectedPoint as
  // value in the generalGrap
  NonlinearFactorGraph generalGraph;
  SFMFactor sfm1(level_uv, unit2, c1, l1);
  SFMFactor sfm2(level_uv_right, unit2, c2, l1);
  generalGraph.push_back(sfm1);
  generalGraph.push_back(sfm2);
  values.insert(l1, expectedPoint); // note: we get rid of possible errors in the triangulation
  Matrix actualFullHessian = generalGraph.linearize(values)->hessian().first;
  Matrix actualFullInfoVector = generalGraph.linearize(values)->hessian().second;
  Matrix actualHessian = actualFullHessian.block(0, 0, 18, 18)
      - actualFullHessian.block(0, 18, 18, 3)
          * (actualFullHessian.block(18, 18, 3, 3)).inverse()
          * actualFullHessian.block(18, 0, 3, 18);
  Vector actualInfoVector = actualFullInfoVector.block(0, 0, 18, 1)
      - actualFullHessian.block(0, 18, 18, 3)
          * (actualFullHessian.block(18, 18, 3, 3)).inverse()
          * actualFullInfoVector.block(18, 0, 3, 1);

  EXPECT(assert_equal(expectedHessian, actualHessian, 1e-7));
  EXPECT(assert_equal(expectedInfoVector, actualInfoVector, 1e-7));
}

/* ************************************************************************* */
TEST( testNonlinearEqualityConstraint, odo_simple_optimize ) {
	// create a two-node graph, connected by an odometry constraint, with
	// a hard prior on one variable, and a conflicting soft prior
	// on the other variable - the constraints should override the soft constraint
	Point2 truth_pt1(1.0, 2.0), truth_pt2(3.0, 2.0);
	Symbol key1('x',1), key2('x',2);

	// hard prior on x1
	eq2D::UnaryEqualityConstraint::shared_ptr constraint1(
			new eq2D::UnaryEqualityConstraint(truth_pt1, key1));

	// soft prior on x2
	Point2 badPt(100.0, -200.0);
	simulated2D::Prior::shared_ptr factor(
			new simulated2D::Prior(badPt, soft_model, key2));

	// odometry constraint
	eq2D::OdoEqualityConstraint::shared_ptr constraint2(
			new eq2D::OdoEqualityConstraint(
					truth_pt1.between(truth_pt2), key1, key2));

	NonlinearFactorGraph graph;
	graph.push_back(constraint1);
	graph.push_back(constraint2);
	graph.push_back(factor);

	Values initValues;
	initValues.insert(key1, Point2());
	initValues.insert(key2, badPt);

	Values actual = LevenbergMarquardtOptimizer(graph, initValues).optimize();
	Values expected;
	expected.insert(key1, truth_pt1);
	expected.insert(key2, truth_pt2);
	CHECK(assert_equal(expected, actual, tol));
}

gtsam::NonlinearFactorGraph Matcher2D::findLocalLoopClosure(
  const PoseD slam_pose, LaserScan2D& scan) {

  NonlinearFactorGraph graph;
#if 0
  // get looped index
  vector<LoopResult2d> loop_result;
  loop_result = this->findLoopClosure(scan);

  // perform small EM only after init
  if (local_smallEM_.flag_init) {
    for (size_t i = 0; i < loop_result.size(); i++) {
      Pose2 relpose = loop_result[i].delta_pose;
      pair<size_t, size_t> relidx = make_pair(loop_result[i].loop_idx, pose_count_);
      // inlier
      if (pose_count_ - loop_result[i].loop_idx > setting_.local_loop_interval &&
          local_smallEM_.perform(relpose, relidx, curr_values_, isam_.getFactorsUnsafe())) {

        cout << "local loop detected! " << endl;
        cout << "robot_" << ID_ << ": [" << loop_result[i].loop_idx << ", " << pose_count_ << "]" << endl;
        cout << "Press Enter to continue ... " << endl;
        cin.ignore(1);

        // matched: insert between robot factor
        graph.push_back(BetweenFactor<Pose2>(Symbol(ID_, loop_result[i].loop_idx), Symbol(ID_, pose_count_),
            relpose, setting_.loop_default_model));
      }
    }

  } else {
    // only init small EM after certain count
    if (local_measure_poses_.size() >= setting_.local_loop_count_smallEM) {
      local_smallEM_.init(local_measure_poses_, local_measure_index_, Pose2());
      local_measure_poses_.clear();
      local_measure_index_.clear();

    // insert in local cache
    } else {
      for (size_t i = 0; i < loop_result.size(); i++) {
        local_measure_poses_.push_back(loop_result[i].delta_pose);
        local_measure_index_.push_back(make_pair(loop_result[i].loop_idx, pose_count_));
      }
    }
  }
#endif
  return graph;
}

/* ************************************************************************* */
TEST(PinholeCamera, BAL) {
  string filename = findExampleDataFile("dubrovnik-3-7-pre");
  SfM_data db;
  bool success = readBAL(filename, db);
  if (!success) throw runtime_error("Could not access file!");

  SharedNoiseModel unit2 = noiseModel::Unit::Create(2);
  NonlinearFactorGraph graph;

  for (size_t j = 0; j < db.number_tracks(); j++) {
    for (const SfM_Measurement& m: db.tracks[j].measurements)
      graph.push_back(sfmFactor(m.second, unit2, m.first, P(j)));
  }

  Values initial = initialCamerasAndPointsEstimate(db);

  LevenbergMarquardtOptimizer lm(graph, initial);

  Values actual = lm.optimize();
  double actualError = graph.error(actual);
  EXPECT_DOUBLES_EQUAL(0.0199833, actualError, 1e-5);
}

/* *************************************************************************/
TEST( SmartProjectionPoseFactor, smartFactorWithSensorBodyTransform ){
  // make a realistic calibration matrix
  double fov = 60; // degrees
  size_t w=640,h=480;

  Cal3_S2::shared_ptr K(new Cal3_S2(fov,w,h));

  // create first camera. Looking along X-axis, 1 meter above ground plane (x-y)
  Pose3 cameraPose1 = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1)); // body poses
  Pose3 cameraPose2 = cameraPose1 * Pose3(Rot3(), Point3(1,0,0));
  Pose3 cameraPose3 = cameraPose1 * Pose3(Rot3(), Point3(0,-1,0));

  SimpleCamera cam1(cameraPose1, *K); // with camera poses
  SimpleCamera cam2(cameraPose2, *K);
  SimpleCamera cam3(cameraPose3, *K);

  // create arbitrary body_Pose_sensor (transforms from sensor to body)
  Pose3 sensor_to_body =  Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(1, 1, 1)); // Pose3(); //

  // These are the poses we want to estimate, from camera measurements
  Pose3 bodyPose1 = cameraPose1.compose(sensor_to_body.inverse());
  Pose3 bodyPose2 = cameraPose2.compose(sensor_to_body.inverse());
  Pose3 bodyPose3 = cameraPose3.compose(sensor_to_body.inverse());

  // three landmarks ~5 meters infront of camera
  Point3 landmark1(5, 0.5, 1.2);
  Point3 landmark2(5, -0.5, 1.2);
  Point3 landmark3(5, 0, 3.0);

  vector<Point2> measurements_cam1, measurements_cam2, measurements_cam3;

  // Project three landmarks into three cameras
  projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1);
  projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2);
  projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3);

  // Create smart factors
  std::vector<Key> views;
  views.push_back(x1);
  views.push_back(x2);
  views.push_back(x3);

  SmartProjectionParams params;
  params.setRankTolerance(1.0);
  params.setDegeneracyMode(gtsam::IGNORE_DEGENERACY);
  params.setEnableEPI(false);

  SmartProjectionPoseFactor<Cal3_S2> smartFactor1(model, K, sensor_to_body, params);
  smartFactor1.add(measurements_cam1, views);

  SmartProjectionPoseFactor<Cal3_S2> smartFactor2(model, K, sensor_to_body, params);
  smartFactor2.add(measurements_cam2, views);

  SmartProjectionPoseFactor<Cal3_S2> smartFactor3(model, K, sensor_to_body, params);
  smartFactor3.add(measurements_cam3, views);

  const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);

  // Put all factors in factor graph, adding priors
  NonlinearFactorGraph graph;
  graph.push_back(smartFactor1);
  graph.push_back(smartFactor2);
  graph.push_back(smartFactor3);
  graph.push_back(PriorFactor<Pose3>(x1, bodyPose1, noisePrior));
  graph.push_back(PriorFactor<Pose3>(x2, bodyPose2, noisePrior));

  // Check errors at ground truth poses
  Values gtValues;
  gtValues.insert(x1, bodyPose1);
  gtValues.insert(x2, bodyPose2);
  gtValues.insert(x3, bodyPose3);
  double actualError = graph.error(gtValues);
  double expectedError = 0.0;
  DOUBLES_EQUAL(expectedError, actualError, 1e-7)

  Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/100, 0., -M_PI/100), gtsam::Point3(0.1,0.1,0.1));
  Values values;
  values.insert(x1, bodyPose1);
  values.insert(x2, bodyPose2);
  // initialize third pose with some noise, we expect it to move back to original pose3
  values.insert(x3, bodyPose3*noise_pose);

  LevenbergMarquardtParams lmParams;
  Values result;
  LevenbergMarquardtOptimizer optimizer(graph, values, lmParams);
  result = optimizer.optimize();
  EXPECT(assert_equal(bodyPose3,result.at<Pose3>(x3)));
}

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

  typedef SmartStereoProjectionPoseFactor SmartFactor;

  bool output_poses = true;
  string poseOutput("../../../examples/data/optimized_poses.txt");
  string init_poseOutput("../../../examples/data/initial_poses.txt");
  Values initial_estimate;
  NonlinearFactorGraph graph;
  const noiseModel::Isotropic::shared_ptr model = noiseModel::Isotropic::Sigma(3,1);
  ofstream pose3Out, init_pose3Out;

  bool add_initial_noise = true;

  string calibration_loc = findExampleDataFile("VO_calibration.txt");
  string pose_loc = findExampleDataFile("VO_camera_poses_large.txt");
  string factor_loc = findExampleDataFile("VO_stereo_factors_large.txt");
  
  //read camera calibration info from file
  // focal lengths fx, fy, skew s, principal point u0, v0, baseline b
  cout << "Reading calibration info" << endl;
  ifstream calibration_file(calibration_loc.c_str());

  double fx, fy, s, u0, v0, b;
  calibration_file >> fx >> fy >> s >> u0 >> v0 >> b;
  const Cal3_S2Stereo::shared_ptr K(new Cal3_S2Stereo(fx, fy, s, u0, v0,b));

  cout << "Reading camera poses" << endl;
  ifstream pose_file(pose_loc.c_str());

  int pose_id;
  MatrixRowMajor m(4,4);
  //read camera pose parameters and use to make initial estimates of camera poses
  while (pose_file >> pose_id) {
    for (int i = 0; i < 16; i++) {
      pose_file >> m.data()[i];
    }
    if(add_initial_noise){
      m(1,3) += (pose_id % 10)/10.0;
    }
    initial_estimate.insert(Symbol('x', pose_id), Pose3(m));
  }

  Values initial_pose_values = initial_estimate.filter<Pose3>();
  if (output_poses) {
    init_pose3Out.open(init_poseOutput.c_str(), ios::out);
    for (size_t i = 1; i <= initial_pose_values.size(); i++) {
      init_pose3Out
          << i << " "
          << initial_pose_values.at<Pose3>(Symbol('x', i)).matrix().format(
              Eigen::IOFormat(Eigen::StreamPrecision, 0, " ", " ")) << endl;
    }
  }
  
  // camera and landmark keys
  size_t x, l;

  // pixel coordinates uL, uR, v (same for left/right images due to rectification)
  // landmark coordinates X, Y, Z in camera frame, resulting from triangulation
  double uL, uR, v, X, Y, Z;
  ifstream factor_file(factor_loc.c_str());
  cout << "Reading stereo factors" << endl;

  //read stereo measurements and construct smart factors

  SmartFactor::shared_ptr factor(new SmartFactor(model));
  size_t current_l = 3;   // hardcoded landmark ID from first measurement

  while (factor_file >> x >> l >> uL >> uR >> v >> X >> Y >> Z) {

    if(current_l != l) {
      graph.push_back(factor);
      factor = SmartFactor::shared_ptr(new SmartFactor(model));
      current_l = l;
    }
    factor->add(StereoPoint2(uL,uR,v), Symbol('x',x), K);
  }

  Pose3 first_pose = initial_estimate.at<Pose3>(Symbol('x',1));
  //constrain the first pose such that it cannot change from its original value during optimization
  // NOTE: NonlinearEquality forces the optimizer to use QR rather than Cholesky
  // QR is much slower than Cholesky, but numerically more stable
  graph.push_back(NonlinearEquality<Pose3>(Symbol('x',1),first_pose));

  LevenbergMarquardtParams params;
  params.verbosityLM = LevenbergMarquardtParams::TRYLAMBDA;
  params.verbosity = NonlinearOptimizerParams::ERROR;

  cout << "Optimizing" << endl;
  //create Levenberg-Marquardt optimizer to optimize the factor graph
  LevenbergMarquardtOptimizer optimizer(graph, initial_estimate, params);
  Values result = optimizer.optimize();

  cout << "Final result sample:" << endl;
  Values pose_values = result.filter<Pose3>();
  pose_values.print("Final camera poses:\n");

  if(output_poses){
    pose3Out.open(poseOutput.c_str(),ios::out);
    for(size_t i = 1; i<=pose_values.size(); i++){
//.........这里部分代码省略.........

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

  // load Plaza2 data
  list<TimedOdometry> odometry = readOdometry();
//  size_t M = odometry.size();

  vector<RangeTriple> triples = readTriples();
  size_t K = triples.size();

  // parameters
  size_t minK = 150; // minimum number of range measurements to process initially
  size_t incK = 25; // minimum number of range measurements to process after
  bool groundTruth = false;
  bool robust = true;

  // Set Noise parameters
  Vector priorSigmas = Vector3(1,1,M_PI);
  Vector odoSigmas = Vector3(0.05, 0.01, 0.2);
  double sigmaR = 100; // range standard deviation
  const NM::Base::shared_ptr // all same type
  priorNoise = NM::Diagonal::Sigmas(priorSigmas), //prior
  odoNoise = NM::Diagonal::Sigmas(odoSigmas), // odometry
  gaussian = NM::Isotropic::Sigma(1, sigmaR), // non-robust
  tukey = NM::Robust::Create(NM::mEstimator::Tukey::Create(15), gaussian), //robust
  rangeNoise = robust ? tukey : gaussian;

  // Initialize iSAM
  ISAM2 isam;

  // Add prior on first pose
  Pose2 pose0 = Pose2(-34.2086489999201, 45.3007639991120,
      M_PI - 2.02108900000000);
  NonlinearFactorGraph newFactors;
  newFactors.push_back(PriorFactor<Pose2>(0, pose0, priorNoise));
  Values initial;
  initial.insert(0, pose0);

  //  initialize points
  if (groundTruth) { // from TL file
    initial.insert(symbol('L', 1), Point2(-68.9265, 18.3778));
    initial.insert(symbol('L', 6), Point2(-37.5805, 69.2278));
    initial.insert(symbol('L', 0), Point2(-33.6205, 26.9678));
    initial.insert(symbol('L', 5), Point2(1.7095, -5.8122));
  } else { // drawn from sigma=1 Gaussian in matlab version
    initial.insert(symbol('L', 1), Point2(3.5784, 2.76944));
    initial.insert(symbol('L', 6), Point2(-1.34989, 3.03492));
    initial.insert(symbol('L', 0), Point2(0.725404, -0.0630549));
    initial.insert(symbol('L', 5), Point2(0.714743, -0.204966));
  }

  // set some loop variables
  size_t i = 1; // step counter
  size_t k = 0; // range measurement counter
  bool initialized = false;
  Pose2 lastPose = pose0;
  size_t countK = 0;

  // Loop over odometry
  gttic_(iSAM);
  BOOST_FOREACH(const TimedOdometry& timedOdometry, odometry) {
    //--------------------------------- odometry loop -----------------------------------------
    double t;
    Pose2 odometry;
    boost::tie(t, odometry) = timedOdometry;

    // add odometry factor
    newFactors.push_back(BetweenFactor<Pose2>(i-1, i, odometry,NM::Diagonal::Sigmas(odoSigmas)));

    // predict pose and add as initial estimate
    Pose2 predictedPose = lastPose.compose(odometry);
    lastPose = predictedPose;
    initial.insert(i, predictedPose);

    // Check if there are range factors to be added
    while (k < K && t >= boost::get<0>(triples[k])) {
      size_t j = boost::get<1>(triples[k]);
      double range = boost::get<2>(triples[k]);
      newFactors.push_back(RangeFactor<Pose2, Point2>(i, symbol('L', j), range,rangeNoise));
      k = k + 1;
      countK = countK + 1;
    }

    // Check whether to update iSAM 2
    if ((k > minK) && (countK > incK)) {
      if (!initialized) { // Do a full optimize for first minK ranges
        gttic_(batchInitialization);
        LevenbergMarquardtOptimizer batchOptimizer(newFactors, initial);
        initial = batchOptimizer.optimize();
        gttoc_(batchInitialization);
        initialized = true;
      }
      gttic_(update);
      isam.update(newFactors, initial);
      gttoc_(update);
      gttic_(calculateEstimate);
      Values result = isam.calculateEstimate();
      gttoc_(calculateEstimate);
      lastPose = result.at<Pose2>(i);
      newFactors = NonlinearFactorGraph();
//.........这里部分代码省略.........