12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Gaussian Factor Graph unittests and linearization 

(with Python bindings)
release/4.3a0
Gerry Chen 2019-10-09 15:58:57 -04:00
parent 19315cc3f3
commit e20494324f
5 changed files with 238 additions and 75 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

91
cython/gtsam/tests/test_GaussianFactorGraph.py Normal file
View File

@ -0,0 +1,91 @@
"""
GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
Atlanta, Georgia 30332-0415
All Rights Reserved
See LICENSE for the license information
Unit tests for Linear Factor Graphs.
Author: Frank Dellaert & Gerry Chen
"""
# pylint: disable=invalid-name, no-name-in-module, no-member
from __future__ import print_function
import unittest
import gtsam
from gtsam.utils.test_case import GtsamTestCase
import numpy as np
def create_graph():
"""Create a basic linear factor graph for testing"""
graph = gtsam.GaussianFactorGraph()
x0 = gtsam.symbol(ord('x'), 0)
x1 = gtsam.symbol(ord('x'), 1)
x2 = gtsam.symbol(ord('x'), 2)
BETWEEN_NOISE = gtsam.noiseModel_Diagonal.Sigmas(np.ones(1))
PRIOR_NOISE = gtsam.noiseModel_Diagonal.Sigmas(np.ones(1))
graph.add(x1, np.eye(1), x0, -np.eye(1), np.ones(1), BETWEEN_NOISE)
graph.add(x2, np.eye(1), x1, -np.eye(1), 2*np.ones(1), BETWEEN_NOISE)
graph.add(x0, np.eye(1), np.zeros(1), PRIOR_NOISE)
return graph, (x0, x1, x2)
class TestGaussianFactorGraph(GtsamTestCase):
"""Tests for Gaussian Factor Graphs."""
def test_fg(self):
"""Test solving a linear factor graph"""
graph, X = create_graph()
result = graph.optimize()
EXPECTEDX = [0, 1, 3]
# check solutions
self.assertAlmostEqual(EXPECTEDX[0], result.at(X[0]), delta=1e-8)
self.assertAlmostEqual(EXPECTEDX[1], result.at(X[1]), delta=1e-8)
self.assertAlmostEqual(EXPECTEDX[2], result.at(X[2]), delta=1e-8)
def test_convertNonlinear(self):
"""Test converting a linear factor graph to a nonlinear one"""
graph, X = create_graph()
EXPECTEDM = [1, 2, 3]
# create nonlinear factor graph for marginalization
nfg = gtsam.LinearContainerFactor.ConvertLinearGraph(graph)
optimizer = gtsam.LevenbergMarquardtOptimizer(nfg, gtsam.Values())
nlresult = optimizer.optimizeSafely()
# marginalize
marginals = gtsam.Marginals(nfg, nlresult)
m = [marginals.marginalCovariance(x) for x in X]
# check linear marginalizations
self.assertAlmostEqual(EXPECTEDM[0], m[0], delta=1e-8)
self.assertAlmostEqual(EXPECTEDM[1], m[1], delta=1e-8)
self.assertAlmostEqual(EXPECTEDM[2], m[2], delta=1e-8)
def test_linearMarginalization(self):
"""Marginalize a linear factor graph"""
graph, X = create_graph()
result = graph.optimize()
EXPECTEDM = [1, 2, 3]
# linear factor graph marginalize
marginals = gtsam.Marginals(graph, result)
m = [marginals.marginalCovariance(x) for x in X]
# check linear marginalizations
self.assertAlmostEqual(EXPECTEDM[0], m[0], delta=1e-8)
self.assertAlmostEqual(EXPECTEDM[1], m[1], delta=1e-8)
self.assertAlmostEqual(EXPECTEDM[2], m[2], delta=1e-8)
if __name__ == '__main__':
unittest.main()

4
gtsam.h
View File

@ -2011,6 +2011,10 @@ class Values {
class Marginals {
Marginals(const gtsam::NonlinearFactorGraph& graph,
const gtsam::Values& solution);
Marginals(const gtsam::GaussianFactorGraph& gfgraph,
const gtsam::Values& solution);
Marginals(const gtsam::GaussianFactorGraph& gfgraph,
const gtsam::VectorValues& solutionvec);
void print(string s) const;
Matrix marginalCovariance(size_t variable) const;

30
gtsam/nonlinear/Marginals.cpp
View File

@ -28,15 +28,35 @@ namespace gtsam {
/* ************************************************************************* */
Marginals::Marginals(const NonlinearFactorGraph& graph, const Values& solution, Factorization factorization,
EliminateableFactorGraph<GaussianFactorGraph>::OptionalOrdering ordering)
{
: values_(solution), factorization_(factorization) {
gttic(MarginalsConstructor);
// Linearize graph
graph_ = *graph.linearize(solution);
computeBayesTree(ordering);
}
// Store values
values_ = solution;
/* ************************************************************************* */
Marginals::Marginals(const GaussianFactorGraph& graph, const VectorValues& solution, Factorization factorization,
EliminateableFactorGraph<GaussianFactorGraph>::OptionalOrdering ordering)
: graph_(graph), factorization_(factorization) {
gttic(MarginalsConstructor);
Values vals;
for (const VectorValues::KeyValuePair& v: solution) {
vals.insert(v.first, v.second);
}
values_ = vals;
computeBayesTree(ordering);
}
/* ************************************************************************* */
Marginals::Marginals(const GaussianFactorGraph& graph, const Values& solution, Factorization factorization,
EliminateableFactorGraph<GaussianFactorGraph>::OptionalOrdering ordering)
: graph_(graph), values_(solution), factorization_(factorization) {
gttic(MarginalsConstructor);
computeBayesTree(ordering);
}
/* ************************************************************************* */
void Marginals::computeBayesTree(EliminateableFactorGraph<GaussianFactorGraph>::OptionalOrdering ordering) {
// Compute BayesTree
factorization_ = factorization;
if(factorization_ == CHOLESKY)

26
gtsam/nonlinear/Marginals.h
View File

@ -51,7 +51,7 @@ public:
/// Default constructor only for Cython wrapper
Marginals(){}
/** Construct a marginals class.
/** Construct a marginals class from a nonlinear factor graph.
* @param graph The factor graph defining the full joint density on all variables.
* @param solution The linearization point about which to compute Gaussian marginals (usually the MLE as obtained from a NonlinearOptimizer).
* @param factorization The linear decomposition mode - either Marginals::CHOLESKY (faster and suitable for most problems) or Marginals::QR (slower but more numerically stable for poorly-conditioned problems).
@ -60,6 +60,24 @@ public:
Marginals(const NonlinearFactorGraph& graph, const Values& solution, Factorization factorization = CHOLESKY,
EliminateableFactorGraph<GaussianFactorGraph>::OptionalOrdering ordering = boost::none);
/** Construct a marginals class from a linear factor graph.
* @param graph The factor graph defining the full joint density on all variables.
* @param solution The solution point to compute Gaussian marginals.
* @param factorization The linear decomposition mode - either Marginals::CHOLESKY (faster and suitable for most problems) or Marginals::QR (slower but more numerically stable for poorly-conditioned problems).
* @param ordering An optional variable ordering for elimination.
*/
Marginals(const GaussianFactorGraph& graph, const Values& solution, Factorization factorization = CHOLESKY,
EliminateableFactorGraph<GaussianFactorGraph>::OptionalOrdering ordering = boost::none);
/** Construct a marginals class from a linear factor graph.
* @param graph The factor graph defining the full joint density on all variables.
* @param solution The solution point to compute Gaussian marginals.
* @param factorization The linear decomposition mode - either Marginals::CHOLESKY (faster and suitable for most problems) or Marginals::QR (slower but more numerically stable for poorly-conditioned problems).
* @param ordering An optional variable ordering for elimination.
*/
Marginals(const GaussianFactorGraph& graph, const VectorValues& solution, Factorization factorization = CHOLESKY,
EliminateableFactorGraph<GaussianFactorGraph>::OptionalOrdering ordering = boost::none);
/** print */
void print(const std::string& str = "Marginals: ", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const;
@ -81,6 +99,12 @@ public:
/** Optimize the bayes tree */
VectorValues optimize() const;
protected:
/** Compute the Bayes Tree as a helper function to the constructor */
void computeBayesTree(EliminateableFactorGraph<GaussianFactorGraph>::OptionalOrdering ordering);
};
/**

48
tests/testMarginals.cpp
View File

@ -87,6 +87,12 @@ TEST(Marginals, planarSLAMmarginals) {
soln.insert(x3, Pose2(4.0, 0.0, 0.0));
soln.insert(l1, Point2(2.0, 2.0));
soln.insert(l2, Point2(4.0, 2.0));
VectorValues soln_lin;
soln_lin.insert(x1, Vector3(0.0, 0.0, 0.0));
soln_lin.insert(x2, Vector3(2.0, 0.0, 0.0));
soln_lin.insert(x3, Vector3(4.0, 0.0, 0.0));
soln_lin.insert(l1, Vector2(2.0, 2.0));
soln_lin.insert(l2, Vector2(4.0, 2.0));
Matrix expectedx1(3,3);
expectedx1 <<
@ -112,22 +118,15 @@ TEST(Marginals, planarSLAMmarginals) {
0.293870968, -0.104516129,
-0.104516129, 0.391935484;
// Check marginals covariances for all variables (Cholesky mode)
Marginals marginals(graph, soln, Marginals::CHOLESKY);
EXPECT(assert_equal(expectedx1, marginals.marginalCovariance(x1), 1e-8));
EXPECT(assert_equal(expectedx2, marginals.marginalCovariance(x2), 1e-8));
EXPECT(assert_equal(expectedx3, marginals.marginalCovariance(x3), 1e-8));
EXPECT(assert_equal(expectedl1, marginals.marginalCovariance(l1), 1e-8));
EXPECT(assert_equal(expectedl2, marginals.marginalCovariance(l2), 1e-8));
// Check marginals covariances for all variables (QR mode)
marginals = Marginals(graph, soln, Marginals::QR);
auto testMarginals = [&] (Marginals marginals) {
EXPECT(assert_equal(expectedx1, marginals.marginalCovariance(x1), 1e-8));
EXPECT(assert_equal(expectedx2, marginals.marginalCovariance(x2), 1e-8));
EXPECT(assert_equal(expectedx3, marginals.marginalCovariance(x3), 1e-8));
EXPECT(assert_equal(expectedl1, marginals.marginalCovariance(l1), 1e-8));
EXPECT(assert_equal(expectedl2, marginals.marginalCovariance(l2), 1e-8));
};
auto testJointMarginals = [&] (Marginals marginals) {
// Check joint marginals for 3 variables
Matrix expected_l2x1x3(8,8);
expected_l2x1x3 <<
@ -175,6 +174,22 @@ TEST(Marginals, planarSLAMmarginals) {
variables[0] = x1;
JointMarginal joint_x1 = marginals.jointMarginalCovariance(variables);
EXPECT(assert_equal(expectedx1, Matrix(joint_l2x1(x1,x1)), 1e-6));
};
Marginals marginals;
marginals = Marginals(graph, soln, Marginals::CHOLESKY);
testMarginals(marginals);
marginals = Marginals(graph, soln, Marginals::QR);
testMarginals(marginals);
testJointMarginals(marginals);
GaussianFactorGraph gfg = *graph.linearize(soln);
marginals = Marginals(gfg, soln_lin, Marginals::CHOLESKY);
testMarginals(marginals);
marginals = Marginals(gfg, soln_lin, Marginals::QR);
testMarginals(marginals);
testJointMarginals(marginals);
}
/* ************************************************************************* */
@ -222,8 +237,7 @@ TEST(Marginals, order) {
vals.at<Pose2>(3).bearing(vals.at<Point2>(101)),
vals.at<Pose2>(3).range(vals.at<Point2>(101)), noiseModel::Unit::Create(2));
Marginals marginals(fg, vals);
KeySet set = fg.keys();
auto testMarginals = [&] (Marginals marginals, KeySet set) {
KeyVector keys(set.begin(), set.end());
JointMarginal joint = marginals.jointMarginalCovariance(keys);
@ -233,6 +247,16 @@ TEST(Marginals, order) {
LONGS_EQUAL(3, (long)joint(3,3).rows());
LONGS_EQUAL(2, (long)joint(100,100).rows());
LONGS_EQUAL(2, (long)joint(101,101).rows());
};
Marginals marginals(fg, vals);
KeySet set = fg.keys();
testMarginals(marginals, set);
GaussianFactorGraph gfg = *fg.linearize(vals);
marginals = Marginals(gfg, vals);
set = gfg.keys();
testMarginals(marginals, set);
}
/* ************************************************************************* */