/* ************************************************************************* */
TEST (Serialization, gaussian_factor_graph) {
GaussianFactorGraph graph;
{
Matrix A1 = (Matrix(2, 2) << 1., 2., 3., 4.);
Matrix A2 = (Matrix(2, 2) << 6., 0.2, 8., 0.4);
Matrix R = (Matrix(2, 2) << 0.1, 0.3, 0.0, 0.34);
Vector d(2); d << 0.2, 0.5;
GaussianConditional cg(0, d, R, 1, A1, 2, A2);
graph.push_back(cg);
}
{
Key i1 = 4, i2 = 7;
Matrix A1 = eye(3), A2 = -1.0 * eye(3);
Vector b = ones(3);
SharedDiagonal model = noiseModel::Diagonal::Sigmas((Vector(3) << 1.0, 2.0, 3.0));
JacobianFactor jacobianfactor(i1, A1, i2, A2, b, model);
HessianFactor hessianfactor(jacobianfactor);
graph.push_back(jacobianfactor);
graph.push_back(hessianfactor);
}
EXPECT(equalsObj(graph));
EXPECT(equalsXML(graph));
EXPECT(equalsBinary(graph));
}
/* ************************************************************************* */
TEST(GaussianFactorGraph, multiplyHessianAdd2) {
GaussianFactorGraph gfg = createGaussianFactorGraphWithHessianFactor();
// brute force
Matrix AtA;
Vector eta;
boost::tie(AtA, eta) = gfg.hessian();
Vector X(6);
X << 1, 2, 3, 4, 5, 6;
Vector Y(6);
Y << -450, -450, 300, 400, 2950, 3450;
EXPECT(assert_equal(Y, AtA * X));
VectorValues x = map_list_of<Key, Vector>(0, Vector2(1, 2))(1, Vector2(3, 4))(2, Vector2(5, 6));
VectorValues expected;
expected.insert(0, Vector2(-450, -450));
expected.insert(1, Vector2(300, 400));
expected.insert(2, Vector2(2950, 3450));
VectorValues actual;
gfg.multiplyHessianAdd(1.0, x, actual);
EXPECT(assert_equal(expected, actual));
// now, do it with non-zero y
gfg.multiplyHessianAdd(1.0, x, actual);
EXPECT(assert_equal(2 * expected, actual));
}
/* ************************************************************************* */
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( 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));
}
/* ************************************************************************* */
int main() {
gtsam::Key PoseKey1(11);
gtsam::Key PoseKey2(12);
gtsam::Key VelKey1(21);
gtsam::Key VelKey2(22);
gtsam::Key BiasKey1(31);
double measurement_dt(0.1);
Vector world_g(Vector3(0.0, 0.0, 9.81));
Vector world_rho(Vector3(0.0, -1.5724e-05, 0.0)); // NED system
gtsam::Vector ECEF_omega_earth(Vector3(0.0, 0.0, 7.292115e-5));
gtsam::Vector world_omega_earth(world_R_ECEF.matrix() * ECEF_omega_earth);
SharedGaussian model(noiseModel::Isotropic::Sigma(9, 0.1));
// Second test: zero angular motion, some acceleration - generated in matlab
Vector measurement_acc(Vector3(6.501390843381716, -6.763926150509185, -2.300389940090343));
Vector measurement_gyro(Vector3(0.1, 0.2, 0.3));
InertialNavFactor_GlobalVelocity<Pose3, Vector3, imuBias::ConstantBias> f(PoseKey1, VelKey1, BiasKey1, PoseKey2, VelKey2, measurement_acc, measurement_gyro, measurement_dt, world_g, world_rho, world_omega_earth, model);
Rot3 R1(0.487316618, 0.125253866, 0.86419557,
0.580273724, 0.693095498, -0.427669306,
-0.652537293, 0.709880342, 0.265075427);
Point3 t1(2.0,1.0,3.0);
Pose3 Pose1(R1, t1);
Vector3 Vel1 = Vector(Vector3(0.5,-0.5,0.4));
Rot3 R2(0.473618898, 0.119523052, 0.872582019,
0.609241153, 0.67099888, -0.422594037,
-0.636011287, 0.731761397, 0.244979388);
Point3 t2 = t1.compose( Point3(Vel1*measurement_dt) );
Pose3 Pose2(R2, t2);
Vector dv = measurement_dt * (R1.matrix() * measurement_acc + world_g);
Vector3 Vel2 = Vel1 + dv;
imuBias::ConstantBias Bias1;
Values values;
values.insert(PoseKey1, Pose1);
values.insert(PoseKey2, Pose2);
values.insert(VelKey1, Vel1);
values.insert(VelKey2, Vel2);
values.insert(BiasKey1, Bias1);
Ordering ordering;
ordering.push_back(PoseKey1);
ordering.push_back(VelKey1);
ordering.push_back(BiasKey1);
ordering.push_back(PoseKey2);
ordering.push_back(VelKey2);
GaussianFactorGraph graph;
gttic_(LinearizeTiming);
for(size_t i = 0; i < 100000; ++i) {
GaussianFactor::shared_ptr g = f.linearize(values);
graph.push_back(g);
}
gttoc_(LinearizeTiming);
tictoc_finishedIteration_();
tictoc_print_();
}
/* ************************************************************************* */
TEST(HessianFactor, CombineAndEliminate2) {
Matrix A01 = I_3x3;
Vector3 b0(1.5, 1.5, 1.5);
Vector3 s0(1.6, 1.6, 1.6);
Matrix A10 = 2.0 * I_3x3;
Matrix A11 = -2.0 * I_3x3;
Vector3 b1(2.5, 2.5, 2.5);
Vector3 s1(2.6, 2.6, 2.6);
Matrix A21 = 3.0 * I_3x3;
Vector3 b2(3.5, 3.5, 3.5);
Vector3 s2(3.6, 3.6, 3.6);
GaussianFactorGraph gfg;
gfg.add(1, A01, b0, noiseModel::Diagonal::Sigmas(s0, true));
gfg.add(0, A10, 1, A11, b1, noiseModel::Diagonal::Sigmas(s1, true));
gfg.add(1, A21, b2, noiseModel::Diagonal::Sigmas(s2, true));
Matrix93 A0, A1;
A0 << A10, Z_3x3, Z_3x3;
A1 << A11, A01, A21;
Vector9 b, sigmas;
b << b1, b0, b2;
sigmas << s1, s0, s2;
// create a full, uneliminated version of the factor
JacobianFactor jacobian(0, A0, 1, A1, b,
noiseModel::Diagonal::Sigmas(sigmas, true));
// Make sure combining works
HessianFactor hessian(gfg);
EXPECT(assert_equal(HessianFactor(jacobian), hessian, 1e-6));
EXPECT(
assert_equal(jacobian.augmentedInformation(),
hessian.augmentedInformation(), 1e-9));
// perform elimination on jacobian
Ordering ordering = list_of(0);
GaussianConditional::shared_ptr expectedConditional;
JacobianFactor::shared_ptr expectedFactor;
boost::tie(expectedConditional, expectedFactor) = //
jacobian.eliminate(ordering);
// Eliminate
GaussianConditional::shared_ptr actualConditional;
HessianFactor::shared_ptr actualHessian;
boost::tie(actualConditional, actualHessian) = //
EliminateCholesky(gfg, ordering);
EXPECT(assert_equal(*expectedConditional, *actualConditional, 1e-6));
VectorValues v;
v.insert(1, Vector3(1, 2, 3));
EXPECT_DOUBLES_EQUAL(expectedFactor->error(v), actualHessian->error(v), 1e-9);
EXPECT(
assert_equal(expectedFactor->augmentedInformation(),
actualHessian->augmentedInformation(), 1e-9));
EXPECT(assert_equal(HessianFactor(*expectedFactor), *actualHessian, 1e-6));
}
/* ************************************************************************* */
KalmanFilter::State KalmanFilter::init(const Vector& x0,
const SharedDiagonal& P0) {
// Create a factor graph f(x0), eliminate it into P(x0)
GaussianFactorGraph factorGraph;
factorGraph.add(0, I_, x0, P0); // |x-x0|^2_diagSigma
return solve(factorGraph, useQR());
}
// Create a planar factor graph and eliminate
double timePlanarSmootherEliminate(int N, bool old = true) {
GaussianFactorGraph fg = planarGraph(N).get<0>();
clock_t start = clock();
fg.eliminateMultifrontal();
clock_t end = clock ();
double dif = (double)(end - start) / CLOCKS_PER_SEC;
return dif;
}
/* ************************************************************************* */
TEST(GaussianFactorGraph, gradientAtZero) {
GaussianFactorGraph gfg = createGaussianFactorGraphWithHessianFactor();
VectorValues expected;
VectorValues actual = gfg.gradientAtZero();
expected.insert(0, Vector2(-25, 17.5));
expected.insert(1, Vector2(5, -13.5));
expected.insert(2, Vector2(29, 4));
EXPECT(assert_equal(expected, actual));
}
/* ************************************************************************* */
KalmanFilter::State KalmanFilter::init(const Vector& x, const Matrix& P0) {
// Create a factor graph f(x0), eliminate it into P(x0)
GaussianFactorGraph factorGraph;
// 0.5*(x-x0)'*inv(Sigma)*(x-x0)
HessianFactor::shared_ptr factor(new HessianFactor(0, x, P0));
factorGraph.push_back(factor);
return solve(factorGraph, useQR());
}
// Create a Kalman smoother for t=1:T and optimize
double timeKalmanSmoother(int T) {
GaussianFactorGraph smoother = createSmoother(T);
clock_t start = clock();
// Keys will come out sorted since keys() returns a set
smoother.optimize(Ordering(smoother.keys()));
clock_t end = clock ();
double dif = (double)(end - start) / CLOCKS_PER_SEC;
return dif;
}
/* ************************************************************************* */
GaussianFactorGraph GaussianFactorGraph::clone() const {
GaussianFactorGraph result;
BOOST_FOREACH(const sharedFactor& f, *this) {
if (f)
result.push_back(f->clone());
else
result.push_back(sharedFactor()); // Passes on null factors so indices remain valid
}
return result;
}
/* ************************************************************************* */
TEST( GaussianBayesTree, balanced_smoother_joint )
{
// Create smoother with 7 nodes
Ordering ordering;
ordering += X(1),X(3),X(5),X(7),X(2),X(6),X(4);
GaussianFactorGraph smoother = createSmoother(7);
// Create the Bayes tree, expected to look like:
// x5 x6 x4
// x3 x2 : x4
// x1 : x2
// x7 : x6
GaussianBayesTree bayesTree = *smoother.eliminateMultifrontal(ordering);
// Conditional density elements reused by both tests
const Matrix I = eye(2), A = -0.00429185*I;
// Check the joint density P(x1,x7) factored as P(x1|x7)P(x7)
GaussianBayesNet expected1 = list_of
// Why does the sign get flipped on the prior?
(GaussianConditional(X(1), zero(2), I/sigmax7, X(7), A/sigmax7))
(GaussianConditional(X(7), zero(2), -1*I/sigmax7));
GaussianBayesNet actual1 = *bayesTree.jointBayesNet(X(1),X(7));
EXPECT(assert_equal(expected1, actual1, tol));
// // Check the joint density P(x7,x1) factored as P(x7|x1)P(x1)
// GaussianBayesNet expected2;
// GaussianConditional::shared_ptr
// parent2(new GaussianConditional(X(1), zero(2), -1*I/sigmax1, ones(2)));
// expected2.push_front(parent2);
// push_front(expected2,X(7), zero(2), I/sigmax1, X(1), A/sigmax1, sigma);
// GaussianBayesNet actual2 = *bayesTree.jointBayesNet(X(7),X(1));
// EXPECT(assert_equal(expected2,actual2,tol));
// Check the joint density P(x1,x4), i.e. with a root variable
double sig14 = 0.784465;
Matrix A14 = -0.0769231*I;
GaussianBayesNet expected3 = list_of
(GaussianConditional(X(1), zero(2), I/sig14, X(4), A14/sig14))
(GaussianConditional(X(4), zero(2), I/sigmax4));
GaussianBayesNet actual3 = *bayesTree.jointBayesNet(X(1),X(4));
EXPECT(assert_equal(expected3,actual3,tol));
// // Check the joint density P(x4,x1), i.e. with a root variable, factored the other way
// GaussianBayesNet expected4;
// GaussianConditional::shared_ptr
// parent4(new GaussianConditional(X(1), zero(2), -1.0*I/sigmax1, ones(2)));
// expected4.push_front(parent4);
// double sig41 = 0.668096;
// Matrix A41 = -0.055794*I;
// push_front(expected4,X(4), zero(2), I/sig41, X(1), A41/sig41, sigma);
// GaussianBayesNet actual4 = *bayesTree.jointBayesNet(X(4),X(1));
// EXPECT(assert_equal(expected4,actual4,tol));
}
/* ************************************************************************* */
bool check_smoother(const NonlinearFactorGraph& fullgraph, const Values& fullinit, const IncrementalFixedLagSmoother& smoother, const Key& key) {
GaussianFactorGraph linearized = *fullgraph.linearize(fullinit);
VectorValues delta = linearized.optimize();
Values fullfinal = fullinit.retract(delta);
Point2 expected = fullfinal.at<Point2>(key);
Point2 actual = smoother.calculateEstimate<Point2>(key);
return assert_equal(expected, actual);
}
/* ************************************************************************* */
TEST(HessianFactor, CombineAndEliminate)
{
Matrix A01 = (Matrix(3,3) <<
1.0, 0.0, 0.0,
0.0, 1.0, 0.0,
0.0, 0.0, 1.0);
Vector b0 = (Vector(3) << 1.5, 1.5, 1.5);
Vector s0 = (Vector(3) << 1.6, 1.6, 1.6);
Matrix A10 = (Matrix(3,3) <<
2.0, 0.0, 0.0,
0.0, 2.0, 0.0,
0.0, 0.0, 2.0);
Matrix A11 = (Matrix(3,3) <<
-2.0, 0.0, 0.0,
0.0, -2.0, 0.0,
0.0, 0.0, -2.0);
Vector b1 = (Vector(3) << 2.5, 2.5, 2.5);
Vector s1 = (Vector(3) << 2.6, 2.6, 2.6);
Matrix A21 = (Matrix(3,3) <<
3.0, 0.0, 0.0,
0.0, 3.0, 0.0,
0.0, 0.0, 3.0);
Vector b2 = (Vector(3) << 3.5, 3.5, 3.5);
Vector s2 = (Vector(3) << 3.6, 3.6, 3.6);
GaussianFactorGraph gfg;
gfg.add(1, A01, b0, noiseModel::Diagonal::Sigmas(s0, true));
gfg.add(0, A10, 1, A11, b1, noiseModel::Diagonal::Sigmas(s1, true));
gfg.add(1, A21, b2, noiseModel::Diagonal::Sigmas(s2, true));
Matrix zero3x3 = zeros(3,3);
Matrix A0 = gtsam::stack(3, &A10, &zero3x3, &zero3x3);
Matrix A1 = gtsam::stack(3, &A11, &A01, &A21);
Vector b = gtsam::concatVectors(3, &b1, &b0, &b2);
Vector sigmas = gtsam::concatVectors(3, &s1, &s0, &s2);
// create a full, uneliminated version of the factor
JacobianFactor expectedFactor(0, A0, 1, A1, b, noiseModel::Diagonal::Sigmas(sigmas, true));
// perform elimination on jacobian
GaussianConditional::shared_ptr expectedConditional;
JacobianFactor::shared_ptr expectedRemainingFactor;
boost::tie(expectedConditional, expectedRemainingFactor) = expectedFactor.eliminate(Ordering(list_of(0)));
// Eliminate
GaussianConditional::shared_ptr actualConditional;
HessianFactor::shared_ptr actualCholeskyFactor;
boost::tie(actualConditional, actualCholeskyFactor) = EliminateCholesky(gfg, Ordering(list_of(0)));
EXPECT(assert_equal(*expectedConditional, *actualConditional, 1e-6));
EXPECT(assert_equal(HessianFactor(*expectedRemainingFactor), *actualCholeskyFactor, 1e-6));
}
/* ************************************************************************* */
TEST(GaussianFactorGraph, hessianDiagonal) {
GaussianFactorGraph gfg = createGaussianFactorGraphWithHessianFactor();
VectorValues expected;
Matrix infoMatrix = gfg.hessian().first;
Vector d = infoMatrix.diagonal();
VectorValues actual = gfg.hessianDiagonal();
expected.insert(0, d.segment<2>(0));
expected.insert(1, d.segment<2>(2));
expected.insert(2, d.segment<2>(4));
EXPECT(assert_equal(expected, actual));
}
/**
* TEST gtsam solver with an over-constrained system
* x + y = 1
* x - y = 5
* x + 2y = 6
*/
TEST(LPSolver, overConstrainedLinearSystem) {
GaussianFactorGraph graph;
Matrix A1 = Vector3(1, 1, 1);
Matrix A2 = Vector3(1, -1, 2);
Vector b = Vector3(1, 5, 6);
JacobianFactor factor(1, A1, 2, A2, b, noiseModel::Constrained::All(3));
graph.push_back(factor);
VectorValues x = graph.optimize();
// This check confirms that gtsam linear constraint solver can't handle
// over-constrained system
CHECK(factor.error(x) != 0.0);
}
/* ************************************************************************* */
KalmanFilter::State fuse(const KalmanFilter::State& p,
GaussianFactor* newFactor, bool useQR) {
// Create a factor graph
GaussianFactorGraph factorGraph;
// push back previous solution and new factor
factorGraph.push_back(p->toFactor());
factorGraph.push_back(GaussianFactor::shared_ptr(newFactor));
// Eliminate graph in order x0, x1, to get Bayes net P(x0|x1)P(x1)
return solve(factorGraph, useQR);
}
/* ************************************************************************* */
TEST(GaussianFactorGraph, matricesMixed) {
GaussianFactorGraph gfg = createGaussianFactorGraphWithHessianFactor();
Matrix A;
Vector b;
boost::tie(A, b) = gfg.jacobian(); // incorrect !
Matrix AtA;
Vector eta;
boost::tie(AtA, eta) = gfg.hessian(); // correct
EXPECT(assert_equal(A.transpose() * A, AtA));
Vector expected = -(Vector(6) << -25, 17.5, 5, -13.5, 29, 4).finished();
EXPECT(assert_equal(expected, eta));
EXPECT(assert_equal(A.transpose() * b, eta));
}
/* ************************************************************************* */
TEST(GaussianFactorGraph, negate) {
GaussianFactorGraph init_graph = createGaussianFactorGraphWithHessianFactor();
init_graph.push_back(GaussianFactor::shared_ptr()); /// Add null factor
GaussianFactorGraph actNegation = init_graph.negate();
GaussianFactorGraph expNegation;
expNegation.push_back(init_graph.at(0)->negate());
expNegation.push_back(init_graph.at(1)->negate());
expNegation.push_back(init_graph.at(2)->negate());
expNegation.push_back(init_graph.at(3)->negate());
expNegation.push_back(init_graph.at(4)->negate());
expNegation.push_back(GaussianFactor::shared_ptr());
EXPECT(assert_equal(expNegation, actNegation));
}
/* ************************************************************************* */
TEST(HessianFactor, CombineAndEliminate1) {
Matrix3 A01 = 3.0 * I_3x3;
Vector3 b0(1, 0, 0);
Matrix3 A21 = 4.0 * I_3x3;
Vector3 b2 = Vector3::Zero();
GaussianFactorGraph gfg;
gfg.add(1, A01, b0);
gfg.add(1, A21, b2);
Matrix63 A1;
A1 << A01, A21;
Vector6 b;
b << b0, b2;
// create a full, uneliminated version of the factor
JacobianFactor jacobian(1, A1, b);
// Make sure combining works
HessianFactor hessian(gfg);
VectorValues v;
v.insert(1, Vector3(1, 0, 0));
EXPECT_DOUBLES_EQUAL(jacobian.error(v), hessian.error(v), 1e-9);
EXPECT(assert_equal(HessianFactor(jacobian), hessian, 1e-6));
EXPECT(assert_equal(25.0 * I_3x3, hessian.information(), 1e-9));
EXPECT(
assert_equal(jacobian.augmentedInformation(),
hessian.augmentedInformation(), 1e-9));
// perform elimination on jacobian
Ordering ordering = list_of(1);
GaussianConditional::shared_ptr expectedConditional;
JacobianFactor::shared_ptr expectedFactor;
boost::tie(expectedConditional, expectedFactor) = //
jacobian.eliminate(ordering);
// Eliminate
GaussianConditional::shared_ptr actualConditional;
HessianFactor::shared_ptr actualHessian;
boost::tie(actualConditional, actualHessian) = //
EliminateCholesky(gfg, ordering);
EXPECT(assert_equal(*expectedConditional, *actualConditional, 1e-6));
EXPECT_DOUBLES_EQUAL(expectedFactor->error(v), actualHessian->error(v), 1e-9);
EXPECT(
assert_equal(expectedFactor->augmentedInformation(),
actualHessian->augmentedInformation(), 1e-9));
EXPECT(assert_equal(HessianFactor(*expectedFactor), *actualHessian, 1e-6));
}
/* ************************************************************************* */
TEST(GaussianFactorGraph, transposeMultiplication) {
GaussianFactorGraph A = createSimpleGaussianFactorGraph();
Errors e;
e += Vector2(0.0, 0.0), Vector2(15.0, 0.0), Vector2(0.0, -5.0), Vector2(-7.5, -5.0);
VectorValues expected;
expected.insert(1, Vector2(-37.5, -50.0));
expected.insert(2, Vector2(-150.0, 25.0));
expected.insert(0, Vector2(187.5, 25.0));
VectorValues actual = A.transposeMultiply(e);
EXPECT(assert_equal(expected, actual));
}
/* ************************************************************************* *
Bayes tree for smoother with "nested dissection" ordering:
Node[x1] P(x1 | x2)
Node[x3] P(x3 | x2 x4)
Node[x5] P(x5 | x4 x6)
Node[x7] P(x7 | x6)
Node[x2] P(x2 | x4)
Node[x6] P(x6 | x4)
Node[x4] P(x4)
becomes
C1 x5 x6 x4
C2 x3 x2 : x4
C3 x1 : x2
C4 x7 : x6
************************************************************************* */
TEST( GaussianBayesTree, balanced_smoother_marginals )
{
// 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);
VectorValues actualSolution = bayesTree.optimize();
VectorValues expectedSolution = VectorValues::Zero(actualSolution);
EXPECT(assert_equal(expectedSolution,actualSolution,tol));
LONGS_EQUAL(4, (long)bayesTree.size());
double tol=1e-5;
// Check marginal on x1
JacobianFactor expected1 = GaussianDensity::FromMeanAndStddev(X(1), zero(2), sigmax1);
JacobianFactor actual1 = *bayesTree.marginalFactor(X(1));
Matrix expectedCovarianceX1 = eye(2,2) * (sigmax1 * sigmax1);
Matrix actualCovarianceX1;
GaussianFactor::shared_ptr m = bayesTree.marginalFactor(X(1), EliminateCholesky);
actualCovarianceX1 = bayesTree.marginalFactor(X(1), EliminateCholesky)->information().inverse();
EXPECT(assert_equal(expectedCovarianceX1, actualCovarianceX1, tol));
EXPECT(assert_equal(expected1,actual1,tol));
// Check marginal on x2
double sigx2 = 0.68712938; // FIXME: this should be corrected analytically
JacobianFactor expected2 = GaussianDensity::FromMeanAndStddev(X(2), zero(2), sigx2);
JacobianFactor actual2 = *bayesTree.marginalFactor(X(2));
EXPECT(assert_equal(expected2,actual2,tol));
// Check marginal on x3
JacobianFactor expected3 = GaussianDensity::FromMeanAndStddev(X(3), zero(2), sigmax3);
JacobianFactor actual3 = *bayesTree.marginalFactor(X(3));
EXPECT(assert_equal(expected3,actual3,tol));
// Check marginal on x4
JacobianFactor expected4 = GaussianDensity::FromMeanAndStddev(X(4), zero(2), sigmax4);
JacobianFactor actual4 = *bayesTree.marginalFactor(X(4));
EXPECT(assert_equal(expected4,actual4,tol));
// Check marginal on x7 (should be equal to x1)
JacobianFactor expected7 = GaussianDensity::FromMeanAndStddev(X(7), zero(2), sigmax7);
JacobianFactor actual7 = *bayesTree.marginalFactor(X(7));
EXPECT(assert_equal(expected7,actual7,tol));
}
/* ************************************************************************* */
TEST(GaussianFactorGraph, matrices) {
// Create factor graph:
// x1 x2 x3 x4 x5 b
// 1 2 3 0 0 4
// 5 6 7 0 0 8
// 9 10 0 11 12 13
// 0 0 0 14 15 16
Matrix A00 = (Matrix(2, 3) << 1, 2, 3, 5, 6, 7).finished();
Matrix A10 = (Matrix(2, 3) << 9, 10, 0, 0, 0, 0).finished();
Matrix A11 = (Matrix(2, 2) << 11, 12, 14, 15).finished();
GaussianFactorGraph gfg;
SharedDiagonal model = noiseModel::Unit::Create(2);
gfg.add(0, A00, Vector2(4., 8.), model);
gfg.add(0, A10, 1, A11, Vector2(13., 16.), model);
Matrix Ab(4, 6);
Ab << 1, 2, 3, 0, 0, 4, 5, 6, 7, 0, 0, 8, 9, 10, 0, 11, 12, 13, 0, 0, 0, 14, 15, 16;
// augmented versions
EXPECT(assert_equal(Ab, gfg.augmentedJacobian()));
EXPECT(assert_equal(Ab.transpose() * Ab, gfg.augmentedHessian()));
// jacobian
Matrix A = Ab.leftCols(Ab.cols() - 1);
Vector b = Ab.col(Ab.cols() - 1);
Matrix actualA;
Vector actualb;
boost::tie(actualA, actualb) = gfg.jacobian();
EXPECT(assert_equal(A, actualA));
EXPECT(assert_equal(b, actualb));
// hessian
Matrix L = A.transpose() * A;
Vector eta = A.transpose() * b;
Matrix actualL;
Vector actualeta;
boost::tie(actualL, actualeta) = gfg.hessian();
EXPECT(assert_equal(L, actualL));
EXPECT(assert_equal(eta, actualeta));
// hessianBlockDiagonal
VectorValues expectLdiagonal; // Make explicit that diagonal is sum-squares of columns
expectLdiagonal.insert(0, Vector3(1 + 25 + 81, 4 + 36 + 100, 9 + 49));
expectLdiagonal.insert(1, Vector2(121 + 196, 144 + 225));
EXPECT(assert_equal(expectLdiagonal, gfg.hessianDiagonal()));
// hessianBlockDiagonal
map<Key, Matrix> actualBD = gfg.hessianBlockDiagonal();
LONGS_EQUAL(2, actualBD.size());
EXPECT(assert_equal(A00.transpose() * A00 + A10.transpose() * A10, actualBD[0]));
EXPECT(assert_equal(A11.transpose() * A11, actualBD[1]));
}
/* ************************************************************************* */
TEST( SubgraphPreconditioner, planarGraph )
{
// Check planar graph construction
GaussianFactorGraph A;
VectorValues xtrue;
boost::tie(A, xtrue) = planarGraph(3);
LONGS_EQUAL(13,A.size());
LONGS_EQUAL(9,xtrue.size());
DOUBLES_EQUAL(0,error(A,xtrue),1e-9); // check zero error for xtrue
// Check that xtrue is optimal
GaussianBayesNet::shared_ptr R1 = GaussianSequentialSolver(A).eliminate();
VectorValues actual = optimize(*R1);
CHECK(assert_equal(xtrue,actual));
}
/* ************************************************************************* */
TEST (Serialization, gaussian_bayes_tree) {
const Key x1=1, x2=2, x3=3, x4=4;
const Ordering chainOrdering = Ordering(list_of(x2)(x1)(x3)(x4));
const SharedDiagonal chainNoise = noiseModel::Isotropic::Sigma(1, 0.5);
const GaussianFactorGraph chain = list_of
(JacobianFactor(x2, (Matrix(1, 1) << 1.), x1, (Matrix(1, 1) << 1.), (Vector(1) << 1.), chainNoise))
(JacobianFactor(x2, (Matrix(1, 1) << 1.), x3, (Matrix(1, 1) << 1.), (Vector(1) << 1.), chainNoise))
(JacobianFactor(x3, (Matrix(1, 1) << 1.), x4, (Matrix(1, 1) << 1.), (Vector(1) << 1.), chainNoise))
(JacobianFactor(x4, (Matrix(1, 1) << 1.), (Vector(1) << 1.), chainNoise));
GaussianBayesTree init = *chain.eliminateMultifrontal(chainOrdering);
GaussianBayesTree expected = *chain.eliminateMultifrontal(chainOrdering);
GaussianBayesTree actual;
std::string serialized = serialize(init);
deserialize(serialized, actual);
EXPECT(assert_equal(expected, actual));
}
/* ************************************************************************* */
TEST(GaussianFactorGraph, multiplyHessianAdd) {
GaussianFactorGraph gfg = createSimpleGaussianFactorGraph();
VectorValues x = map_list_of<Key, Vector>(0, Vector2(1, 2))(1, Vector2(3, 4))(2, Vector2(5, 6));
VectorValues expected;
expected.insert(0, Vector2(-450, -450));
expected.insert(1, Vector2(0, 0));
expected.insert(2, Vector2(950, 1050));
VectorValues actual;
gfg.multiplyHessianAdd(1.0, x, actual);
EXPECT(assert_equal(expected, actual));
// now, do it with non-zero y
gfg.multiplyHessianAdd(1.0, x, actual);
EXPECT(assert_equal(2 * expected, actual));
}
/* ************************************************************************* */
TEST(GaussianFactorGraph, eliminate_empty) {
// eliminate an empty factor
GaussianFactorGraph gfg;
gfg.add(JacobianFactor());
GaussianBayesNet::shared_ptr actualBN;
GaussianFactorGraph::shared_ptr remainingGFG;
boost::tie(actualBN, remainingGFG) = gfg.eliminatePartialSequential(Ordering());
// expected Bayes net is empty
GaussianBayesNet expectedBN;
// expected remaining graph should be the same as the original, still containing the empty factor
GaussianFactorGraph expectedLF = gfg;
// check if the result matches
EXPECT(assert_equal(*actualBN, expectedBN));
EXPECT(assert_equal(*remainingGFG, expectedLF));
}
/* ************************************************************************* */
TEST(GaussianFactorGraph, matrices2) {
GaussianFactorGraph gfg = createSimpleGaussianFactorGraph();
Matrix A;
Vector b;
boost::tie(A, b) = gfg.jacobian();
Matrix AtA;
Vector eta;
boost::tie(AtA, eta) = gfg.hessian();
EXPECT(assert_equal(A.transpose() * A, AtA));
EXPECT(assert_equal(A.transpose() * b, eta));
Matrix expectedAtA(6, 6);
expectedAtA << 125, 0, -25, 0, -100, 0, //
0, 125, 0, -25, 0, -100, //
-25, 0, 50, 0, -25, 0, //
0, -25, 0, 50, 0, -25, //
-100, 0, -25, 0, 225, 0, //
0, -100, 0, -25, 0, 225;
EXPECT(assert_equal(expectedAtA, AtA));
}
/* ************************************************************************* */
TEST(GaussianFactorGraph, gradient) {
GaussianFactorGraph fg = createSimpleGaussianFactorGraph();
// Construct expected gradient
// 2*f(x) = 100*(x1+c[X(1)])^2 + 100*(x2-x1-[0.2;-0.1])^2 + 25*(l1-x1-[0.0;0.2])^2 +
// 25*(l1-x2-[-0.2;0.3])^2
// worked out: df/dx1 = 100*[0.1;0.1] + 100*[0.2;-0.1]) + 25*[0.0;0.2] = [10+20;10-10+5] = [30;5]
VectorValues expected = map_list_of<Key, Vector>(1, Vector2(5.0, -12.5))(2, Vector2(30.0, 5.0))(
0, Vector2(-25.0, 17.5));
// Check the gradient at delta=0
VectorValues zero = VectorValues::Zero(expected);
VectorValues actual = fg.gradient(zero);
EXPECT(assert_equal(expected, actual));
EXPECT(assert_equal(expected, fg.gradientAtZero()));
// Check the gradient at the solution (should be zero)
VectorValues solution = fg.optimize();
VectorValues actual2 = fg.gradient(solution);
EXPECT(assert_equal(VectorValues::Zero(solution), actual2));
}
