291 lines
9.4 KiB
C++
291 lines
9.4 KiB
C++
/* ----------------------------------------------------------------------------
|
|
|
|
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
|
|
* Atlanta, Georgia 30332-0415
|
|
* All Rights Reserved
|
|
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
|
|
|
|
* See LICENSE for the license information
|
|
|
|
* -------------------------------------------------------------------------- */
|
|
|
|
/**
|
|
* @file testNonlinearFactorGraph.cpp
|
|
* @brief Unit tests for Non-Linear Factor NonlinearFactorGraph
|
|
* @brief testNonlinearFactorGraph
|
|
* @author Carlos Nieto
|
|
* @author Christian Potthast
|
|
*/
|
|
|
|
#include <gtsam/base/Testable.h>
|
|
#include <gtsam/base/Matrix.h>
|
|
#include <tests/smallExample.h>
|
|
#include <gtsam/inference/FactorGraph.h>
|
|
#include <gtsam/inference/Symbol.h>
|
|
#include <gtsam/symbolic/SymbolicFactorGraph.h>
|
|
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
|
|
#include <gtsam/geometry/Pose2.h>
|
|
#include <gtsam/geometry/Pose3.h>
|
|
#include <gtsam/sam/RangeFactor.h>
|
|
#include <gtsam/slam/BetweenFactor.h>
|
|
|
|
#include <CppUnitLite/TestHarness.h>
|
|
|
|
#include <boost/assign/std/list.hpp>
|
|
#include <boost/assign/std/set.hpp>
|
|
using namespace boost::assign;
|
|
|
|
/*STL/C++*/
|
|
#include <iostream>
|
|
|
|
using namespace std;
|
|
using namespace gtsam;
|
|
using namespace example;
|
|
|
|
using symbol_shorthand::X;
|
|
using symbol_shorthand::L;
|
|
|
|
/* ************************************************************************* */
|
|
TEST( NonlinearFactorGraph, equals )
|
|
{
|
|
NonlinearFactorGraph fg = createNonlinearFactorGraph();
|
|
NonlinearFactorGraph fg2 = createNonlinearFactorGraph();
|
|
CHECK( fg.equals(fg2) );
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
TEST( NonlinearFactorGraph, error )
|
|
{
|
|
NonlinearFactorGraph fg = createNonlinearFactorGraph();
|
|
Values c1 = createValues();
|
|
double actual1 = fg.error(c1);
|
|
DOUBLES_EQUAL( 0.0, actual1, 1e-9 );
|
|
|
|
Values c2 = createNoisyValues();
|
|
double actual2 = fg.error(c2);
|
|
DOUBLES_EQUAL( 5.625, actual2, 1e-9 );
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
TEST( NonlinearFactorGraph, keys )
|
|
{
|
|
NonlinearFactorGraph fg = createNonlinearFactorGraph();
|
|
KeySet actual = fg.keys();
|
|
LONGS_EQUAL(3, (long)actual.size());
|
|
KeySet::const_iterator it = actual.begin();
|
|
LONGS_EQUAL((long)L(1), (long)*(it++));
|
|
LONGS_EQUAL((long)X(1), (long)*(it++));
|
|
LONGS_EQUAL((long)X(2), (long)*(it++));
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
TEST( NonlinearFactorGraph, GET_ORDERING)
|
|
{
|
|
Ordering expected; expected += L(1), X(2), X(1); // For starting with l1,x1,x2
|
|
NonlinearFactorGraph nlfg = createNonlinearFactorGraph();
|
|
Ordering actual = Ordering::Colamd(nlfg);
|
|
EXPECT(assert_equal(expected,actual));
|
|
|
|
// Constrained ordering - put x2 at the end
|
|
Ordering expectedConstrained; expectedConstrained += L(1), X(1), X(2);
|
|
FastMap<Key, int> constraints;
|
|
constraints[X(2)] = 1;
|
|
Ordering actualConstrained = Ordering::ColamdConstrained(nlfg, constraints);
|
|
EXPECT(assert_equal(expectedConstrained, actualConstrained));
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
TEST( NonlinearFactorGraph, probPrime )
|
|
{
|
|
NonlinearFactorGraph fg = createNonlinearFactorGraph();
|
|
Values cfg = createValues();
|
|
|
|
// evaluate the probability of the factor graph
|
|
double actual = fg.probPrime(cfg);
|
|
double expected = 1.0;
|
|
DOUBLES_EQUAL(expected,actual,0);
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
TEST( NonlinearFactorGraph, linearize )
|
|
{
|
|
NonlinearFactorGraph fg = createNonlinearFactorGraph();
|
|
Values initial = createNoisyValues();
|
|
GaussianFactorGraph linearFG = *fg.linearize(initial);
|
|
GaussianFactorGraph expected = createGaussianFactorGraph();
|
|
CHECK(assert_equal(expected,linearFG)); // Needs correct linearizations
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
TEST( NonlinearFactorGraph, clone )
|
|
{
|
|
NonlinearFactorGraph fg = createNonlinearFactorGraph();
|
|
NonlinearFactorGraph actClone = fg.clone();
|
|
EXPECT(assert_equal(fg, actClone));
|
|
for (size_t i=0; i<fg.size(); ++i)
|
|
EXPECT(fg[i] != actClone[i]);
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
TEST( NonlinearFactorGraph, rekey )
|
|
{
|
|
NonlinearFactorGraph init = createNonlinearFactorGraph();
|
|
map<Key,Key> rekey_mapping;
|
|
rekey_mapping.insert(make_pair(L(1), L(4)));
|
|
NonlinearFactorGraph actRekey = init.rekey(rekey_mapping);
|
|
|
|
// ensure deep clone
|
|
LONGS_EQUAL((long)init.size(), (long)actRekey.size());
|
|
for (size_t i=0; i<init.size(); ++i)
|
|
EXPECT(init[i] != actRekey[i]);
|
|
|
|
NonlinearFactorGraph expRekey;
|
|
// original measurements
|
|
expRekey.push_back(init[0]);
|
|
expRekey.push_back(init[1]);
|
|
|
|
// updated measurements
|
|
Point2 z3(0, -1), z4(-1.5, -1.);
|
|
SharedDiagonal sigma0_2 = noiseModel::Isotropic::Sigma(2,0.2);
|
|
expRekey += simulated2D::Measurement(z3, sigma0_2, X(1), L(4));
|
|
expRekey += simulated2D::Measurement(z4, sigma0_2, X(2), L(4));
|
|
|
|
EXPECT(assert_equal(expRekey, actRekey));
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
TEST( NonlinearFactorGraph, symbolic )
|
|
{
|
|
NonlinearFactorGraph graph = createNonlinearFactorGraph();
|
|
|
|
SymbolicFactorGraph expected;
|
|
expected.push_factor(X(1));
|
|
expected.push_factor(X(1), X(2));
|
|
expected.push_factor(X(1), L(1));
|
|
expected.push_factor(X(2), L(1));
|
|
|
|
SymbolicFactorGraph actual = *graph.symbolic();
|
|
|
|
EXPECT(assert_equal(expected, actual));
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
TEST(NonlinearFactorGraph, UpdateCholesky) {
|
|
NonlinearFactorGraph fg = createNonlinearFactorGraph();
|
|
Values initial = createNoisyValues();
|
|
|
|
// solve conventionally
|
|
GaussianFactorGraph linearFG = *fg.linearize(initial);
|
|
auto delta = linearFG.optimizeDensely();
|
|
auto expected = initial.retract(delta);
|
|
|
|
// solve with new method
|
|
EXPECT(assert_equal(expected, fg.updateCholesky(initial)));
|
|
|
|
// solve with Ordering
|
|
Ordering ordering;
|
|
ordering += L(1), X(2), X(1);
|
|
EXPECT(assert_equal(expected, fg.updateCholesky(initial, ordering)));
|
|
|
|
// solve with new method, heavily damped
|
|
auto dampen = [](const HessianFactor::shared_ptr& hessianFactor) {
|
|
auto iterator = hessianFactor->begin();
|
|
for (; iterator != hessianFactor->end(); iterator++) {
|
|
const auto index = std::distance(hessianFactor->begin(), iterator);
|
|
auto block = hessianFactor->info().diagonalBlock(index);
|
|
for (int j = 0; j < block.rows(); j++) {
|
|
block(j, j) += 1e9;
|
|
}
|
|
}
|
|
};
|
|
EXPECT(assert_equal(initial, fg.updateCholesky(initial, dampen), 1e-6));
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
// Example from issue #452 which threw an ILS error. The reason was a very
|
|
// weak prior on heading, which was tightened, and the ILS disappeared.
|
|
TEST(testNonlinearFactorGraph, eliminate) {
|
|
// Linearization point
|
|
Pose2 T11(0, 0, 0);
|
|
Pose2 T12(1, 0, 0);
|
|
Pose2 T21(0, 1, 0);
|
|
Pose2 T22(1, 1, 0);
|
|
|
|
// Factor graph
|
|
auto graph = NonlinearFactorGraph();
|
|
|
|
// Priors
|
|
auto prior = noiseModel::Isotropic::Sigma(3, 1);
|
|
graph.addPrior(11, T11, prior);
|
|
graph.addPrior(21, T21, prior);
|
|
|
|
// Odometry
|
|
auto model = noiseModel::Diagonal::Sigmas(Vector3(0.01, 0.01, 0.3));
|
|
graph.add(BetweenFactor<Pose2>(11, 12, T11.between(T12), model));
|
|
graph.add(BetweenFactor<Pose2>(21, 22, T21.between(T22), model));
|
|
|
|
// Range factor
|
|
auto model_rho = noiseModel::Isotropic::Sigma(1, 0.01);
|
|
graph.add(RangeFactor<Pose2>(12, 22, 1.0, model_rho));
|
|
|
|
Values values;
|
|
values.insert(11, T11.retract(Vector3(0.1,0.2,0.3)));
|
|
values.insert(12, T12);
|
|
values.insert(21, T21);
|
|
values.insert(22, T22);
|
|
auto linearized = graph.linearize(values);
|
|
|
|
// Eliminate
|
|
Ordering ordering;
|
|
ordering += 11, 21, 12, 22;
|
|
auto bn = linearized->eliminateSequential(ordering);
|
|
EXPECT_LONGS_EQUAL(4, bn->size());
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
TEST(testNonlinearFactorGraph, addPrior) {
|
|
Key k(0);
|
|
|
|
// Factor graph.
|
|
auto graph = NonlinearFactorGraph();
|
|
|
|
// Add a prior factor for key k.
|
|
auto model_double = noiseModel::Isotropic::Sigma(1, 1);
|
|
graph.addPrior<double>(k, 10, model_double);
|
|
|
|
// Assert the graph has 0 error with the correct values.
|
|
Values values;
|
|
values.insert(k, 10.0);
|
|
EXPECT_DOUBLES_EQUAL(0, graph.error(values), 1e-16);
|
|
|
|
// Assert the graph has some error with incorrect values.
|
|
values.clear();
|
|
values.insert(k, 11.0);
|
|
EXPECT(0 != graph.error(values));
|
|
|
|
// Clear the factor graph and values.
|
|
values.clear();
|
|
graph.erase(graph.begin(), graph.end());
|
|
|
|
// Add a Pose3 prior to the factor graph. Use a gaussian noise model by
|
|
// providing the covariance matrix.
|
|
Eigen::DiagonalMatrix<double, 6, 6> covariance_pose3;
|
|
covariance_pose3.setIdentity();
|
|
Pose3 pose{Rot3(), Point3(0, 0, 0)};
|
|
graph.addPrior(k, pose, covariance_pose3);
|
|
|
|
// Assert the graph has 0 error with the correct values.
|
|
values.insert(k, pose);
|
|
EXPECT_DOUBLES_EQUAL(0, graph.error(values), 1e-16);
|
|
|
|
// Assert the graph has some error with incorrect values.
|
|
values.clear();
|
|
Pose3 pose_incorrect{Rot3::RzRyRx(-M_PI, M_PI, -M_PI / 8), Point3(1, 2, 3)};
|
|
values.insert(k, pose_incorrect);
|
|
EXPECT(0 != graph.error(values));
|
|
}
|
|
|
|
/* ************************************************************************* */
|
|
int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
|
|
/* ************************************************************************* */
|