/* ************************************************************************* */
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( 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));
}
/* ************************************************************************* */
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));
}
/* ************************************************************************* */
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( 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));
}
/* ************************************************************************* */
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, 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( 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( 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));
}
/* ************************************************************************* */
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));
}
/* ************************************************************************* */
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( 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));
}
/* ************************************************************************* */
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
}
/* ************************************************************************* */
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);
}
/* ************************************************************************* */
TEST( StereoFactor, Constructor) {
StereoPoint2 measurement(323, 318-50, 241);
TestStereoFactor factor(measurement, model, X(1), L(1), K);
}
