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