Esempio n. 1
0
/* ************************************************************************* */
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(NonlinearOptimizer, NullFactor) {

  example::Graph fg = example::createReallyNonlinearFactorGraph();

  // Add null factor
  fg.push_back(example::Graph::sharedFactor());

  // test error at minimum
  Point2 xstar(0,0);
  Values cstar;
  cstar.insert(X(1), xstar);
  DOUBLES_EQUAL(0.0,fg.error(cstar),0.0);

  // test error at initial = [(1-cos(3))^2 + (sin(3))^2]*50 =
  Point2 x0(3,3);
  Values c0;
  c0.insert(X(1), x0);
  DOUBLES_EQUAL(199.0,fg.error(c0),1e-3);

  // optimize parameters
  Ordering ord;
  ord.push_back(X(1));

  // Gauss-Newton
  Values actual1 = GaussNewtonOptimizer(fg, c0, ord).optimize();
  DOUBLES_EQUAL(0,fg.error(actual1),tol);

  // Levenberg-Marquardt
  Values actual2 = LevenbergMarquardtOptimizer(fg, c0, ord).optimize();
  DOUBLES_EQUAL(0,fg.error(actual2),tol);

  // Dogleg
  Values actual3 = DoglegOptimizer(fg, c0, ord).optimize();
  DOUBLES_EQUAL(0,fg.error(actual3),tol);
}
Esempio n. 3
0
/*******************************************************************************
 * 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;
}
Esempio n. 4
0
/* ************************************************************************* */
TEST( ISAM, iSAM_smoother )
{
  Ordering ordering;
  for (int t = 1; t <= 7; t++) ordering += X(t);

  // Create smoother with 7 nodes
  GaussianFactorGraph smoother = createSmoother(7);

  // run iSAM for every factor
  GaussianISAM actual;
  for(boost::shared_ptr<GaussianFactor> factor: smoother) {
    GaussianFactorGraph factorGraph;
    factorGraph.push_back(factor);
    actual.update(factorGraph);
  }

  // Create expected Bayes Tree by solving smoother with "natural" ordering
  GaussianBayesTree expected = *smoother.eliminateMultifrontal(ordering);

  // Verify sigmas in the bayes tree
  for(const GaussianBayesTree::sharedClique& clique: expected.nodes() | br::map_values) {
    GaussianConditional::shared_ptr conditional = clique->conditional();
    EXPECT(!conditional->get_model());
  }

  // Check whether BayesTree is correct
  EXPECT(assert_equal(GaussianFactorGraph(expected).augmentedHessian(), GaussianFactorGraph(actual).augmentedHessian()));

  // obtain solution
  VectorValues e; // expected solution
  for (int t = 1; t <= 7; t++) e.insert(X(t), Vector::Zero(2));
  VectorValues optimized = actual.optimize(); // actual solution
  EXPECT(assert_equal(e, optimized));
}
Esempio n. 5
0
/* ************************************************************************* */
TEST( StereoFactor, Equals ) {
  // Create two identical factors and make sure they're equal
  StereoPoint2 measurement(323, 318-50, 241);

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

  CHECK(assert_equal(factor1, factor2));
}
/* ************************************************************************* */
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));
}
Esempio n. 7
0
/* ************************************************************************* */
TEST( StereoFactor, EqualsWithTransform ) {
  // Create two identical factors and make sure they're equal
  StereoPoint2 measurement(323, 318-50, 241);
  Pose3 body_P_sensor(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2), Point3(0.25, -0.10, 1.0));

  TestStereoFactor factor1(measurement, model, X(1), L(1), K, body_P_sensor);
  TestStereoFactor factor2(measurement, model, X(1), L(1), K, body_P_sensor);

  CHECK(assert_equal(factor1, factor2));
}
Esempio n. 8
0
/* ************************************************************************* */
TEST( simulated2DOriented, constructor )
{
  Pose2 measurement(0.2, 0.3, 0.1);
  SharedDiagonal model = noiseModel::Diagonal::Sigmas((Vector(3) << 1., 1., 1.));
  simulated2DOriented::Odometry factor(measurement, model, X(1), X(2));

  simulated2DOriented::Values config;
  config.insert(X(1), Pose2(1., 0., 0.2));
  config.insert(X(2), Pose2(2., 0., 0.1));
  boost::shared_ptr<GaussianFactor> lf = factor.linearize(config);
}
/* ************************************************************************* */
TEST( GaussianBayesTree, balanced_smoother_shortcuts )
{
  // Create smoother with 7 nodes
  GaussianFactorGraph smoother = createSmoother(7);

  // Create the Bayes tree
  Ordering ordering;
  ordering += X(1),X(3),X(5),X(7),X(2),X(6),X(4);
  GaussianBayesTree bayesTree = *smoother.eliminateMultifrontal(ordering);

  // Check the conditional P(Root|Root)
  GaussianBayesNet empty;
  GaussianBayesTree::sharedClique R = bayesTree.roots().front();
  GaussianBayesNet actual1 = R->shortcut(R);
  EXPECT(assert_equal(empty,actual1,tol));

  // Check the conditional P(C2|Root)
  GaussianBayesTree::sharedClique C2 = bayesTree[X(3)];
  GaussianBayesNet actual2 = C2->shortcut(R);
  EXPECT(assert_equal(empty,actual2,tol));

  // Check the conditional P(C3|Root), which should be equal to P(x2|x4)
  /** TODO: Note for multifrontal conditional:
   * p_x2_x4 is now an element conditional of the multifrontal conditional bayesTree[ordering[X(2)]]->conditional()
   * We don't know yet how to take it out.
   */
//  GaussianConditional::shared_ptr p_x2_x4 = bayesTree[ordering[X(2)]]->conditional();
//  p_x2_x4->print("Conditional p_x2_x4: ");
//  GaussianBayesNet expected3(p_x2_x4);
//  GaussianISAM::sharedClique C3 = isamTree[ordering[X(1)]];
//  GaussianBayesNet actual3 = GaussianISAM::shortcut(C3,R);
//  EXPECT(assert_equal(expected3,actual3,tol));
}
Esempio n. 10
0
/* ************************************************************************* */
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);
}
Esempio n. 11
0
/* ************************************************************************* */
TEST( StereoFactor, JacobianWithTransform ) {
  // Create the factor with a measurement that is 3 pixels off in x
  StereoPoint2 measurement(323, 318-50, 241);
  Pose3 body_P_sensor(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2), Point3(0.25, -0.10, 1.0));
  TestStereoFactor factor(measurement, model, X(1), L(1), K, body_P_sensor);

  // Set the linearization point. The vehicle pose has been selected to put the camera at (-6, 0, 0)
  Pose3 pose(Rot3(), Point3(-6.50, 0.10 , -1.0));
  Point3 point(0.0, 0.0, 0.0);

  // Use the factor to calculate the Jacobians
  Matrix H1Actual, H2Actual;
  factor.evaluateError(pose, point, H1Actual, H2Actual);

  // The expected Jacobians
  Matrix H1Expected = Matrix_(3, 6, -100.0,    0.0,  650.0,   0.0,  100.0,    0.0,
                                    -100.0,   -8.0,  649.2,  -8.0,  100.0,    0.0,
                                     -10.0, -650.0,    0.0,   0.0,    0.0,  100.0);
  Matrix H2Expected = Matrix_(3, 3,    0.0, -100.0,    0.0,
                                       8.0, -100.0,    0.0,
                                       0.0,    0.0, -100.0);

  // Verify the Jacobians are correct
  CHECK(assert_equal(H1Expected, H1Actual, 1e-3));
  CHECK(assert_equal(H2Expected, H2Actual, 1e-3));
}
Esempio n. 12
0
/* ************************************************************************* */
TEST( StereoFactor, Jacobian ) {
  // Create the factor with a measurement that is 3 pixels off in x
  StereoPoint2 measurement(323, 318-50, 241);
  TestStereoFactor factor(measurement, model, X(1), L(1), K);

  // Set the linearization point
  Pose3 pose(Rot3(), Point3(0.0, 0.0, -6.25));
  Point3 point(0.0, 0.0, 0.0);

  // Use the factor to calculate the Jacobians
  Matrix H1Actual, H2Actual;
  factor.evaluateError(pose, point, H1Actual, H2Actual);

  // The expected Jacobians
  Matrix H1Expected = Matrix_(3, 6, 0.0,  -625.0, 0.0, -100.0,    0.0,  0.0,
                                    0.0,  -625.0, 0.0, -100.0,    0.0, -8.0,
                                    625.0,   0.0, 0.0,    0.0, -100.0,  0.0);
  Matrix H2Expected = Matrix_(3, 3, 100.0,   0.0, 0.0,
                                    100.0,   0.0, 8.0,
                                    0.0,   100.0, 0.0);

  // Verify the Jacobians are correct
  CHECK(assert_equal(H1Expected, H1Actual, 1e-3));
  CHECK(assert_equal(H2Expected, H2Actual, 1e-3));
}
/* ************************************************************************* */
TEST( ProjectionFactorPPPC, JacobianWithTransform ) {
  // Create the factor with a measurement that is 3 pixels off in x
  Point2 measurement(323.0, 240.0);
  Pose3 body_P_sensor(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2), Point3(0.25, -0.10, 1.0));
  TestProjectionFactor factor(measurement, model, X(1), T(1), L(1), K(1));

  // Set the linearization point. The vehicle pose has been selected to put the camera at (-6, 0, 0)
  Pose3 pose(Rot3(), Point3(-6.25, 0.10 , -1.0));
  Point3 point(0.0, 0.0, 0.0);

  // Use the factor to calculate the Jacobians
  Matrix H1Actual, H2Actual, H3Actual, H4Actual;
  factor.evaluateError(pose, body_P_sensor, point, *K1, H1Actual, H2Actual, H3Actual, H4Actual);

  // The expected Jacobians
  Matrix H1Expected = (Matrix(2, 6) << -92.376, 0., 577.350, 0., 92.376, 0., -9.2376, -577.350, 0., 0., 0., 92.376).finished();
  Matrix H3Expected = (Matrix(2, 3) << 0., -92.376, 0., 0., 0., -92.376).finished();

  // Verify the Jacobians are correct
  CHECK(assert_equal(H1Expected, H1Actual, 1e-3));
  CHECK(assert_equal(H3Expected, H3Actual, 1e-3));

  // Verify H2 and H4 with numerical derivatives
  Matrix H2Expected = numericalDerivative11<Vector, Pose3>(
      boost::bind(&TestProjectionFactor::evaluateError, &factor, pose, _1, point,
          *K1, boost::none, boost::none, boost::none, boost::none), body_P_sensor);

  Matrix H4Expected = numericalDerivative11<Vector, Cal3_S2>(
      boost::bind(&TestProjectionFactor::evaluateError, &factor, pose, body_P_sensor, point,
          _1, boost::none, boost::none, boost::none, boost::none), *K1);

  CHECK(assert_equal(H2Expected, H2Actual, 1e-5));
  CHECK(assert_equal(H4Expected, H4Actual, 1e-5));

}
/* ************************************************************************* */
TEST( ProjectionFactorPPPC, Jacobian ) {
  // Create the factor with a measurement that is 3 pixels off in x
  Point2 measurement(323.0, 240.0);
  TestProjectionFactor factor(measurement, model, X(1), T(1), L(1), K(1));

  // Set the linearization point
  Pose3 pose(Rot3(), Point3(0,0,-6));
  Point3 point(0.0, 0.0, 0.0);

  // Use the factor to calculate the Jacobians
  Matrix H1Actual, H2Actual, H3Actual, H4Actual;
  factor.evaluateError(pose, Pose3(), point, *K1, H1Actual, H2Actual, H3Actual, H4Actual);

  // The expected Jacobians
  Matrix H1Expected = (Matrix(2, 6) << 0., -554.256, 0., -92.376, 0., 0., 554.256, 0., 0., 0., -92.376, 0.).finished();
  Matrix H3Expected = (Matrix(2, 3) << 92.376, 0., 0., 0., 92.376, 0.).finished();

  // Verify the Jacobians are correct
  CHECK(assert_equal(H1Expected, H1Actual, 1e-3));
  CHECK(assert_equal(H3Expected, H3Actual, 1e-3));

  // Verify H2 and H4 with numerical derivatives
  Matrix H2Expected = numericalDerivative11<Vector, Pose3>(
      boost::bind(&TestProjectionFactor::evaluateError, &factor, pose, _1, point,
          *K1, boost::none, boost::none, boost::none, boost::none), Pose3());

  Matrix H4Expected = numericalDerivative11<Vector, Cal3_S2>(
      boost::bind(&TestProjectionFactor::evaluateError, &factor, pose, Pose3(), point,
          _1, boost::none, boost::none, boost::none, boost::none), *K1);

  CHECK(assert_equal(H2Expected, H2Actual, 1e-5));
  CHECK(assert_equal(H4Expected, H4Actual, 1e-5));
}
Esempio n. 15
0
/* ************************************************************************* */
TEST( GaussianFactor, size )
{
	// create a linear factor graph
	const Key kx1 = X(1), kx2 = X(2), kl1 = L(1);
  Ordering ordering; ordering += kx1,kx2,kl1;
  GaussianFactorGraph fg = example::createGaussianFactorGraph(ordering);

	// get some factors from the graph
	boost::shared_ptr<GaussianFactor> factor1 = fg[0];
	boost::shared_ptr<GaussianFactor> factor2 = fg[1];
	boost::shared_ptr<GaussianFactor> factor3 = fg[2];

	EXPECT_LONGS_EQUAL(1, factor1->size());
	EXPECT_LONGS_EQUAL(2, factor2->size());
	EXPECT_LONGS_EQUAL(2, factor3->size());
}
/* ************************************************************************* */
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
  }
}
Esempio n. 17
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);
}
Esempio n. 18
0
/* ************************************************************************* */
TEST( GaussianFactor, error )
{
	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 first factor from the factor graph
	GaussianFactor::shared_ptr lf = fg[0];

	// check the error of the first factor with noisy config
	VectorValues cfg = example::createZeroDelta(ordering);

	// calculate the error from the factor kf1
	// note the error is the same as in testNonlinearFactor
	double actual = lf->error(cfg);
	DOUBLES_EQUAL( 1.0, actual, 0.00000001 );
}
Esempio n. 19
0
/* ************************************************************************* */
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));
}
Esempio n. 20
0
/* ************************************************************************* */
TEST( GaussianFactor, matrix_aug )
{
	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];
	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 unwhitened version
	Matrix Ab_act1;
	Ab_act1 = lf->matrix_augmented(false);

	Matrix Ab1 = Matrix_(2,5,
			-1.0,  0.0, 1.0,  0.0,  0.2,
			00.0,- 1.0, 0.0,  1.0, -0.1 );

	EQUALITY(Ab_act1,Ab1);

	// Test whitened version
	Matrix Ab_act2;
	Ab_act2 = lf->matrix_augmented(true);

	Matrix Ab2 = Matrix_(2,5,
		   -10.0,  0.0, 10.0,  0.0,  2.0,
			00.0, -10.0,  0.0, 10.0, -1.0 );

	EQUALITY(Ab_act2,Ab2);

	// Ensure that whitening is consistent
	boost::shared_ptr<noiseModel::Gaussian> model = lf->get_model();
	model->WhitenInPlace(Ab_act1);
	EQUALITY(Ab_act1, Ab_act2);
}
/* ************************************************************************* */
TEST( NonlinearOptimizer, SimpleDLOptimizer )
{
  example::Graph fg(example::createReallyNonlinearFactorGraph());

  Point2 x0(3,3);
  Values c0;
  c0.insert(X(1), x0);

  Values actual = DoglegOptimizer(fg, c0).optimize();
  DOUBLES_EQUAL(0,fg.error(actual),tol);
}
/* ************************************************************************* */
TEST( NonlinearOptimizer, optimize )
{
  example::Graph fg(example::createReallyNonlinearFactorGraph());

	// test error at minimum
	Point2 xstar(0,0);
	Values cstar;
	cstar.insert(X(1), xstar);
	DOUBLES_EQUAL(0.0,fg.error(cstar),0.0);

	// test error at initial = [(1-cos(3))^2 + (sin(3))^2]*50 =
	Point2 x0(3,3);
	Values c0;
	c0.insert(X(1), x0);
	DOUBLES_EQUAL(199.0,fg.error(c0),1e-3);

	// optimize parameters
	Ordering ord;
	ord.push_back(X(1));

	// Gauss-Newton
	GaussNewtonParams gnParams;
	gnParams.ordering = ord;
	Values actual1 = GaussNewtonOptimizer(fg, c0, gnParams).optimize();
	DOUBLES_EQUAL(0,fg.error(actual1),tol);

	// Levenberg-Marquardt
	LevenbergMarquardtParams lmParams;
	lmParams.ordering = ord;
  Values actual2 = LevenbergMarquardtOptimizer(fg, c0, lmParams).optimize();
  DOUBLES_EQUAL(0,fg.error(actual2),tol);

  // Dogleg
  DoglegParams dlParams;
  dlParams.ordering = ord;
  Values actual3 = DoglegOptimizer(fg, c0, dlParams).optimize();
  DOUBLES_EQUAL(0,fg.error(actual3),tol);
}
/* ************************************************************************* */
TEST( NonlinearOptimizer, Factorization )
{
	Values config;
	config.insert(X(1), Pose2(0.,0.,0.));
	config.insert(X(2), Pose2(1.5,0.,0.));

	NonlinearFactorGraph graph;
	graph.add(PriorFactor<Pose2>(X(1), Pose2(0.,0.,0.), noiseModel::Isotropic::Sigma(3, 1e-10)));
	graph.add(BetweenFactor<Pose2>(X(1),X(2), Pose2(1.,0.,0.), noiseModel::Isotropic::Sigma(3, 1)));

	Ordering ordering;
	ordering.push_back(X(1));
	ordering.push_back(X(2));

	LevenbergMarquardtOptimizer optimizer(graph, config, ordering);
	optimizer.iterate();

	Values expected;
	expected.insert(X(1), Pose2(0.,0.,0.));
	expected.insert(X(2), Pose2(1.,0.,0.));
	CHECK(assert_equal(expected, optimizer.values(), 1e-5));
}
/* ************************************************************************* */
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));
}
Esempio n. 25
0
/* ************************************************************************* */
TEST( StereoFactor, Error ) {
  // Create the factor with a measurement that is 3 pixels off in x
  StereoPoint2 measurement(323, 318-50, 241);
  TestStereoFactor factor(measurement, model, X(1), L(1), K);

  // Set the linearization point
  Pose3 pose(Rot3(), Point3(0.0, 0.0, -6.25));
  Point3 point(0.0, 0.0, 0.0);

  // Use the factor to calculate the error
  Vector actualError(factor.evaluateError(pose, point));

  // The expected error is (-3.0, +2.0, -1.0) pixels / UnitCovariance
  Vector expectedError = Vector_(3, -3.0, +2.0, -1.0);

  // Verify we get the expected error
  CHECK(assert_equal(expectedError, actualError, 1e-9));
}
/* ************************************************************************* */
TEST( ProjectionFactorPPPC, Error ) {
  // Create the factor with a measurement that is 3 pixels off in x
  Point2 measurement(323.0, 240.0);
  TestProjectionFactor factor(measurement, model, X(1), T(1), L(1), K(1));

  // Set the linearization point
  Pose3 pose(Rot3(), Point3(0,0,-6));
  Point3 point(0.0, 0.0, 0.0);

  // Use the factor to calculate the error
  Vector actualError(factor.evaluateError(pose, Pose3(), point, *K1));

  // The expected error is (-3.0, 0.0) pixels / UnitCovariance
  Vector expectedError = Vector2(-3.0, 0.0);

  // Verify we get the expected error
  CHECK(assert_equal(expectedError, actualError, 1e-9));
}
Esempio n. 27
0
/* ************************************************************************* */
TEST( StereoFactor, ErrorWithTransform ) {
  // Create the factor with a measurement that is 3 pixels off in x
  StereoPoint2 measurement(323, 318-50, 241);
  Pose3 body_P_sensor(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2), Point3(0.25, -0.10, 1.0));
  TestStereoFactor factor(measurement, model, X(1), L(1), K, body_P_sensor);

  // Set the linearization point. The vehicle pose has been selected to put the camera at (-6, 0, 0)
  Pose3 pose(Rot3(), Point3(-6.50, 0.10 , -1.0));
  Point3 point(0.0, 0.0, 0.0);

  // Use the factor to calculate the error
  Vector actualError(factor.evaluateError(pose, point));

  // The expected error is (-3.0, +2.0, -1.0) pixels / UnitCovariance
  Vector expectedError = Vector_(3, -3.0, +2.0, -1.0);

  // Verify we get the expected error
  CHECK(assert_equal(expectedError, actualError, 1e-9));
}
/* ************************************************************************* */
TEST( NonlinearOptimizer, optimization_method )
{
  LevenbergMarquardtParams paramsQR;
  paramsQR.linearSolverType = LevenbergMarquardtParams::MULTIFRONTAL_QR;
  LevenbergMarquardtParams paramsChol;
  paramsChol.linearSolverType = LevenbergMarquardtParams::MULTIFRONTAL_CHOLESKY;

	example::Graph fg = example::createReallyNonlinearFactorGraph();

	Point2 x0(3,3);
	Values c0;
	c0.insert(X(1), x0);

	Values actualMFQR = LevenbergMarquardtOptimizer(fg, c0, paramsQR).optimize();
	DOUBLES_EQUAL(0,fg.error(actualMFQR),tol);

  Values actualMFChol = LevenbergMarquardtOptimizer(fg, c0, paramsChol).optimize();
  DOUBLES_EQUAL(0,fg.error(actualMFChol),tol);
}
/* ************************************************************************* */
TEST( ProjectionFactorPPPC, ErrorWithTransform ) {
  // Create the factor with a measurement that is 3 pixels off in x
  Point2 measurement(323.0, 240.0);
  Pose3 transform(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2), Point3(0.25, -0.10, 1.0));
  TestProjectionFactor factor(measurement, model, X(1),T(1), L(1), K(1));

  // Set the linearization point. The vehicle pose has been selected to put the camera at (-6, 0, 0)
  Pose3 pose(Rot3(), Point3(-6.25, 0.10 , -1.0));
  Point3 point(0.0, 0.0, 0.0);

  // Use the factor to calculate the error
  Vector actualError(factor.evaluateError(pose, transform, point, *K1));

  // The expected error is (-3.0, 0.0) pixels / UnitCovariance
  Vector expectedError = Vector2(-3.0, 0.0);

  // Verify we get the expected error
  CHECK(assert_equal(expectedError, actualError, 1e-9));
}
/* ************************************************************************* */
TEST( NonlinearOptimizer, iterateLM )
{
	// really non-linear factor graph
  example::Graph fg(example::createReallyNonlinearFactorGraph());

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

	// normal iterate
	GaussNewtonParams gnParams;
	GaussNewtonOptimizer gnOptimizer(fg, config, gnParams);
	gnOptimizer.iterate();

	// LM iterate with lambda 0 should be the same
	LevenbergMarquardtParams lmParams;
	lmParams.lambdaInitial = 0.0;
	LevenbergMarquardtOptimizer lmOptimizer(fg, config, lmParams);
	lmOptimizer.iterate();

	CHECK(assert_equal(gnOptimizer.values(), lmOptimizer.values(), 1e-9));
}