C++ NonlinearFactorGraph类代码示例

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

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 (EssentialMatrixFactor2, extraMinimization) {
    // Additional test with camera moving in positive X direction

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

    // Check error at ground truth
    Values truth;
    truth.insert(100, trueE);
    for (size_t i = 0; i < data.number_tracks(); 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 < data.number_tracks(); 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( 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(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));
}

/* ************************************************************************* */
void NonlinearISAM::update(const NonlinearFactorGraph& newFactors,
		const Values& initialValues) {

  if(newFactors.size() > 0) {

    // Reorder and relinearize every reorderInterval updates
    if(reorderInterval_ > 0 && ++reorderCounter_ >= reorderInterval_) {
      reorder_relinearize();
      reorderCounter_ = 0;
    }

    factors_.push_back(newFactors);

    // Linearize new factors and insert them
    // TODO: optimize for whole config?
    linPoint_.insert(initialValues);

    // Augment ordering
    // TODO: allow for ordering constraints within the new variables
    // FIXME: should just loop over new values
    BOOST_FOREACH(const NonlinearFactorGraph::sharedFactor& factor, newFactors)
      BOOST_FOREACH(Key key, factor->keys())
        ordering_.tryInsert(key, ordering_.nVars()); // will do nothing if already present

    boost::shared_ptr<GaussianFactorGraph> linearizedNewFactors = newFactors.linearize(linPoint_, ordering_);

    // Update ISAM
    isam_.update(*linearizedNewFactors);
  }
}

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

/* ************************************************************************* */
TEST(DoglegOptimizer, Iterate) {
  // really non-linear factor graph
  NonlinearFactorGraph fg = example::createReallyNonlinearFactorGraph();

  // config far from minimum
  Point2 x0(3,0);
  Values config;
  config.insert(X(1), x0);

  double Delta = 1.0;
  for(size_t it=0; it<10; ++it) {
    GaussianBayesNet gbn = *fg.linearize(config)->eliminateSequential();
    // Iterate assumes that linear error = nonlinear error at the linearization point, and this should be true
    double nonlinearError = fg.error(config);
    double linearError = GaussianFactorGraph(gbn).error(config.zeroVectors());
    DOUBLES_EQUAL(nonlinearError, linearError, 1e-5);
//    cout << "it " << it << ", Delta = " << Delta << ", error = " << fg->error(*config) << endl;
    VectorValues dx_u = gbn.optimizeGradientSearch();
    VectorValues dx_n = gbn.optimize();
    DoglegOptimizerImpl::IterationResult result = DoglegOptimizerImpl::Iterate(Delta, DoglegOptimizerImpl::SEARCH_EACH_ITERATION, dx_u, dx_n, gbn, fg, config, fg.error(config));
    Delta = result.Delta;
    EXPECT(result.f_error < fg.error(config)); // Check that error decreases
    Values newConfig(config.retract(result.dx_d));
    config = newConfig;
    DOUBLES_EQUAL(fg.error(config), result.f_error, 1e-5); // Check that error is correctly filled in
  }
}

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

int main(int argc, char* argv[]) {
  // parse options and read BAL file
  SfM_data db = preamble(argc, argv);

  // Build graph using conventional GeneralSFMFactor
  NonlinearFactorGraph graph;
  for (size_t j = 0; j < db.number_tracks(); j++) {
    BOOST_FOREACH (const SfM_Measurement& m, db.tracks[j].measurements) {
      size_t i = m.first;
      Point2 z = m.second;
      Pose3_ camTnav_(C(i));
      Cal3Bundler_ calibration_(K(i));
      Point3_ nav_point_(P(j));
      graph.addExpressionFactor(
          gNoiseModel, z,
          uncalibrate(calibration_,  // now using transform_from !!!:
                      project(transform_from(camTnav_, nav_point_))));
    }
  }

  Values initial;
  size_t i = 0, j = 0;
  BOOST_FOREACH (const SfM_Camera& camera, db.cameras) {
    initial.insert(C(i), camera.pose().inverse());  // inverse !!!
    initial.insert(K(i), camera.calibration());
    i += 1;
  }
  BOOST_FOREACH (const SfM_Track& track, db.tracks)
    initial.insert(P(j++), track.p);

  bool separateCalibration = true;
  return optimize(db, graph, initial, separateCalibration);
}

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

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

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

/* ************************************************************************* */
TEST(Marginals, order) {
  NonlinearFactorGraph fg;
  fg += PriorFactor<Pose2>(0, Pose2(), noiseModel::Unit::Create(3));
  fg += BetweenFactor<Pose2>(0, 1, Pose2(1,0,0), noiseModel::Unit::Create(3));
  fg += BetweenFactor<Pose2>(1, 2, Pose2(1,0,0), noiseModel::Unit::Create(3));
  fg += BetweenFactor<Pose2>(2, 3, Pose2(1,0,0), noiseModel::Unit::Create(3));

  Values vals;
  vals.insert(0, Pose2());
  vals.insert(1, Pose2(1,0,0));
  vals.insert(2, Pose2(2,0,0));
  vals.insert(3, Pose2(3,0,0));

  vals.insert(100, Point2(0,1));
  vals.insert(101, Point2(1,1));

  fg += BearingRangeFactor<Pose2,Point2>(0, 100,
    vals.at<Pose2>(0).bearing(vals.at<Point2>(100)),
    vals.at<Pose2>(0).range(vals.at<Point2>(100)), noiseModel::Unit::Create(2));
  fg += BearingRangeFactor<Pose2,Point2>(0, 101,
    vals.at<Pose2>(0).bearing(vals.at<Point2>(101)),
    vals.at<Pose2>(0).range(vals.at<Point2>(101)), noiseModel::Unit::Create(2));

  fg += BearingRangeFactor<Pose2,Point2>(1, 100,
    vals.at<Pose2>(1).bearing(vals.at<Point2>(100)),
    vals.at<Pose2>(1).range(vals.at<Point2>(100)), noiseModel::Unit::Create(2));
  fg += BearingRangeFactor<Pose2,Point2>(1, 101,
    vals.at<Pose2>(1).bearing(vals.at<Point2>(101)),
    vals.at<Pose2>(1).range(vals.at<Point2>(101)), noiseModel::Unit::Create(2));

  fg += BearingRangeFactor<Pose2,Point2>(2, 100,
    vals.at<Pose2>(2).bearing(vals.at<Point2>(100)),
    vals.at<Pose2>(2).range(vals.at<Point2>(100)), noiseModel::Unit::Create(2));
  fg += BearingRangeFactor<Pose2,Point2>(2, 101,
    vals.at<Pose2>(2).bearing(vals.at<Point2>(101)),
    vals.at<Pose2>(2).range(vals.at<Point2>(101)), noiseModel::Unit::Create(2));

  fg += BearingRangeFactor<Pose2,Point2>(3, 100,
    vals.at<Pose2>(3).bearing(vals.at<Point2>(100)),
    vals.at<Pose2>(3).range(vals.at<Point2>(100)), noiseModel::Unit::Create(2));
  fg += BearingRangeFactor<Pose2,Point2>(3, 101,
    vals.at<Pose2>(3).bearing(vals.at<Point2>(101)),
    vals.at<Pose2>(3).range(vals.at<Point2>(101)), noiseModel::Unit::Create(2));

  Marginals marginals(fg, vals);
  KeySet set = fg.keys();
  FastVector<Key> keys(set.begin(), set.end());
  JointMarginal joint = marginals.jointMarginalCovariance(keys);

  LONGS_EQUAL(3, (long)joint(0,0).rows());
  LONGS_EQUAL(3, (long)joint(1,1).rows());
  LONGS_EQUAL(3, (long)joint(2,2).rows());
  LONGS_EQUAL(3, (long)joint(3,3).rows());
  LONGS_EQUAL(2, (long)joint(100,100).rows());
  LONGS_EQUAL(2, (long)joint(101,101).rows());
}