Пример #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);
}
Пример #2
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));
}
Пример #4
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));
}
/* ************************************************************************* */
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));
}
Пример #6
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));
}
Пример #7
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));
}
/* ************************************************************************* */
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));
}
/* ************************************************************************* */
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));
}
Пример #12
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);
}
Пример #13
0
/* ************************************************************************* */
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)]);
}
Пример #14
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());
}
Пример #15
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);
}
Пример #16
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));
}
Пример #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 );
}
/* ************************************************************************* */
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));
}
Пример #20
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));
}
Пример #21
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));
}
Пример #22
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( ProjectionFactorPPPC, Constructor) {
  Point2 measurement(323.0, 240.0);
  TestProjectionFactor factor(measurement, model, X(1), T(1), L(1), K(1));
  // TODO: Actually check something
}
Пример #24
0
/* ************************************************************************* */
TEST( StereoFactor, ConstructorWithTransform) {
  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);
}
Пример #25
0
/* ************************************************************************* */
TEST( StereoFactor, Constructor) {
  StereoPoint2 measurement(323, 318-50, 241);

  TestStereoFactor factor(measurement, model, X(1), L(1), K);
}