C++ symbol_shorthand::L方法代码示例

/* ************************************************************************* */
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( ProjectionFactorPPPC, Equals ) {
  // Create two identical factors and make sure they're equal
  Point2 measurement(323.0, 240.0);

  TestProjectionFactor factor1(measurement, model, X(1), T(1), L(1), K(1));
  TestProjectionFactor factor2(measurement, model, X(1), T(1), L(1), K(1));

  CHECK(assert_equal(factor1, factor2));
}

/* ************************************************************************* */
TEST( SymbolicFactorGraph, symbolicFactorGraph )
{
  Ordering o; o += X(1),L(1),X(2);
	// construct expected symbolic graph
	SymbolicFactorGraph expected;
	expected.push_factor(o[X(1)]);
	expected.push_factor(o[X(1)],o[X(2)]);
	expected.push_factor(o[X(1)],o[L(1)]);
	expected.push_factor(o[X(2)],o[L(1)]);

	// construct it from the factor graph
	GaussianFactorGraph factorGraph = example::createGaussianFactorGraph(o);
	SymbolicFactorGraph actual(factorGraph);

	CHECK(assert_equal(expected, actual));
}

/* ************************************************************************* */
TEST( SymbolicBayesNet, constructor )
{
  Ordering o; o += X(2),L(1),X(1);
  // Create manually
  IndexConditional::shared_ptr
    x2(new IndexConditional(o[X(2)],o[L(1)], o[X(1)])),
    l1(new IndexConditional(o[L(1)],o[X(1)])),
    x1(new IndexConditional(o[X(1)]));
  BayesNet<IndexConditional> expected;
  expected.push_back(x2);
  expected.push_back(l1);
  expected.push_back(x1);

  // Create from a factor graph
  GaussianFactorGraph factorGraph = createGaussianFactorGraph(o);
  SymbolicFactorGraph fg(factorGraph);

  // eliminate it
  SymbolicBayesNet actual = *SymbolicSequentialSolver(fg).eliminate();

  CHECK(assert_equal(expected, actual));
}

/* ************************************************************************* */
TEST( SymbolicFactorGraph, eliminate )
{
  Ordering o; o += X(2),L(1),X(1);

  // create expected Chordal bayes Net
  IndexConditional::shared_ptr x2(new IndexConditional(o[X(2)], o[L(1)], o[X(1)]));
  IndexConditional::shared_ptr l1(new IndexConditional(o[L(1)], o[X(1)]));
  IndexConditional::shared_ptr x1(new IndexConditional(o[X(1)]));

  SymbolicBayesNet expected;
  expected.push_back(x2);
  expected.push_back(l1);
  expected.push_back(x1);

  // create a test graph
	GaussianFactorGraph factorGraph = example::createGaussianFactorGraph(o);
	SymbolicFactorGraph fg(factorGraph);

	// eliminate it
  SymbolicBayesNet actual = *SymbolicSequentialSolver(fg).eliminate();

  CHECK(assert_equal(expected,actual));
}

/* ************************************************************************* */
TEST( GaussianFactor, getDim )
{
	const Key kx1 = X(1), kx2 = X(2), kl1 = L(1);
	// get a factor
  Ordering ordering; ordering += kx1,kx2,kl1;
  GaussianFactorGraph fg = example::createGaussianFactorGraph(ordering);
	GaussianFactor::shared_ptr factor = fg[0];

	// get the size of a variable
	size_t actual = factor->getDim(factor->find(ordering[kx1]));

	// verify
	size_t expected = 2;
	EXPECT_LONGS_EQUAL(expected, actual);
}

/* ************************************************************************* */
TEST( inference, marginals2)
{
  NonlinearFactorGraph fg;
  SharedDiagonal poseModel(noiseModel::Isotropic::Sigma(3, 0.1));
  SharedDiagonal pointModel(noiseModel::Isotropic::Sigma(2, 0.1));

  fg.add(PriorFactor<Pose2>(X(0), Pose2(), poseModel));
  fg.add(BetweenFactor<Pose2>(X(0), X(1), Pose2(1.0,0.0,0.0), poseModel));
  fg.add(BetweenFactor<Pose2>(X(1), X(2), Pose2(1.0,0.0,0.0), poseModel));
  fg.add(BearingRangeFactor<Pose2, Point2>(X(0), L(0), Rot2(), 1.0, pointModel));
  fg.add(BearingRangeFactor<Pose2, Point2>(X(1), L(0), Rot2(), 1.0, pointModel));
  fg.add(BearingRangeFactor<Pose2, Point2>(X(2), L(0), Rot2(), 1.0, pointModel));

  Values init;
  init.insert(X(0), Pose2(0.0,0.0,0.0));
  init.insert(X(1), Pose2(1.0,0.0,0.0));
  init.insert(X(2), Pose2(2.0,0.0,0.0));
  init.insert(L(0), Point2(1.0,1.0));

  Ordering ordering(*fg.orderingCOLAMD(init));
  FactorGraph<GaussianFactor>::shared_ptr gfg(fg.linearize(init, ordering));
  GaussianMultifrontalSolver solver(*gfg);
  solver.marginalFactor(ordering[L(0)]);
}

/* ************************************************************************* */
TEST( GaussianFactor, matrix )
{
	const Key kx1 = X(1), kx2 = X(2), kl1 = L(1);
	// create a small linear factor graph
  Ordering ordering; ordering += kx1,kx2,kl1;
  GaussianFactorGraph fg = example::createGaussianFactorGraph(ordering);

	// get the factor kf2 from the factor graph
	//GaussianFactor::shared_ptr lf = fg[1]; // NOTE: using the older version
	Vector b2 = Vector_(2, 0.2, -0.1);
	Matrix I = eye(2);
  // render with a given ordering
  Ordering ord;
  ord += kx1,kx2;
	JacobianFactor::shared_ptr lf(new JacobianFactor(ord[kx1], -I, ord[kx2], I, b2, sigma0_1));

	// Test whitened version
	Matrix A_act1; Vector b_act1;
	boost::tie(A_act1,b_act1) = lf->matrix(true);

	Matrix A1 = Matrix_(2,4,
			-10.0,  0.0, 10.0,  0.0,
			000.0,-10.0,  0.0, 10.0 );
	Vector b1 = Vector_(2, 2.0, -1.0);

	EQUALITY(A_act1,A1);
	EQUALITY(b_act1,b1);

	// Test unwhitened version
	Matrix A_act2; Vector b_act2;
	boost::tie(A_act2,b_act2) = lf->matrix(false);


	Matrix A2 = Matrix_(2,4,
			-1.0,  0.0, 1.0,  0.0,
			000.0,-1.0,  0.0, 1.0 );
	//Vector b2 = Vector_(2, 2.0, -1.0);

	EQUALITY(A_act2,A2);
	EQUALITY(b_act2,b2);

	// Ensure that whitening is consistent
	boost::shared_ptr<noiseModel::Gaussian> model = lf->get_model();
	model->WhitenSystem(A_act2, b_act2);
	EQUALITY(A_act1, A_act2);
	EQUALITY(b_act1, b_act2);
}

/* ************************************************************************* */
TEST( GaussianFactor, linearFactor )
{
	const Key kx1 = X(1), kx2 = X(2), kl1 = L(1);
  Ordering ordering; ordering += kx1,kx2,kl1;

  Matrix I = eye(2);
	Vector b = Vector_(2, 2.0, -1.0);
	JacobianFactor expected(ordering[kx1], -10*I,ordering[kx2], 10*I, b, noiseModel::Unit::Create(2));

	// create a small linear factor graph
	GaussianFactorGraph fg = example::createGaussianFactorGraph(ordering);

	// get the factor kf2 from the factor graph
	JacobianFactor::shared_ptr lf =
	    boost::dynamic_pointer_cast<JacobianFactor>(fg[1]);

	// check if the two factors are the same
	EXPECT(assert_equal(expected,*lf));
}