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

Fixed bug in multifrontal marginals caused by backwards permutations with LDL (seems to be an inconsistency in Eigen?). Added GaussianConditional constructor from any number of frontal and parent variables. Added several new unit tests on marginals. Fixed small bug in GaussianConditional non-const get_d_ and get_R_ functions that didn't account for multiple frontal variables.

release/4.3a0
Richard Roberts 2011-09-23 02:50:46 +00:00
parent 222d5073b9
commit 63ca74e492
7 changed files with 388 additions and 14 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

72
gtsam/base/tests/testCholesky.cpp
View File

@ -89,6 +89,78 @@ TEST(cholesky, ldlPartial) {
EXPECT(assert_equal(expected, actual, 1e-9));
}
/* ************************************************************************* */
TEST(cholesky, ldlPartial2) {
// choleskyPartial should only use the upper triangle, so this represents a
// symmetric matrix.
Matrix ABC = Matrix_(7,7,
4.0375, 3.4584, 3.5735, 2.4815, 2.1471, 2.7400, 2.2063,
0., 4.7267, 3.8423, 2.3624, 2.8091, 2.9579, 2.5914,
0., 0., 5.1600, 2.0797, 3.4690, 3.2419, 2.9992,
0., 0., 0., 1.8786, 1.0535, 1.4250, 1.3347,
0., 0., 0., 0., 3.0788, 2.6283, 2.3791,
0., 0., 0., 0., 0., 2.9227, 2.4056,
0., 0., 0., 0., 0., 0., 2.5776);
// Do partial Cholesky on 3 frontal scalar variables
Matrix RSL(ABC);
Eigen::LDLT<Matrix>::TranspositionType permutation = ldlPartial(RSL, 6);
// Compute permuted R (note transposed permutation - seems to be an inconsistency in Eigen!)
Matrix Rp = RSL.topLeftCorner(6,6) * permutation.transpose();
Matrix expected = Matrix(ABC.selfadjointView<Eigen::Upper>()).topLeftCorner(6,6);
Matrix actual = Rp.transpose() * Rp;
EXPECT(assert_equal(expected, actual));
// Directly call LDLT and reconstruct using left and right applications of permutations,
// the permutations need to be transposed between the two cases, which seems to be an
// inconsistency in Eigen!
Matrix A = ABC.topLeftCorner(6,6);
A = A.selfadjointView<Eigen::Upper>();
Eigen::LDLT<Matrix, Eigen::Lower> ldlt;
ldlt.compute(A);
Matrix R = ldlt.vectorD().cwiseSqrt().asDiagonal() * Matrix(ldlt.matrixU());
Rp = R * ldlt.transpositionsP();
Matrix actual1 = Matrix::Identity(6,6);
actual1 = Matrix(actual1 * ldlt.transpositionsP());
actual1 *= ldlt.matrixL();
actual1 *= ldlt.vectorD().asDiagonal();
actual1 *= ldlt.matrixU();
actual1 = Matrix(actual1 * ldlt.transpositionsP().transpose());
Matrix actual2 = Matrix::Identity(6,6);
actual2 = Matrix(ldlt.transpositionsP() * actual2);
actual2 = ldlt.matrixU() * actual2;
actual2 = ldlt.vectorD().asDiagonal() * actual2;
actual2 = ldlt.matrixL() * actual2;
actual2 = Matrix(ldlt.transpositionsP().transpose() * actual2);
Matrix actual3 = ldlt.reconstructedMatrix();
EXPECT(assert_equal(expected, actual1));
EXPECT(assert_equal(expected, actual2));
EXPECT(assert_equal(expected, actual3));
// Again checking the difference between left and right permutation application
EXPECT(assert_equal(Matrix(eye(6) * ldlt.transpositionsP().transpose()), Matrix(ldlt.transpositionsP() * eye(6))));
// Same but with a manually-constructed permutation
Matrix I = eye(3);
Eigen::Transpositions<Eigen::Dynamic> p(3);
p.indices()[0] = 1;
p.indices()[1] = 1;
p.indices()[2] = 0;
Matrix IexpectedR = (Matrix(3,3) <<
0, 1, 0,
0, 0, 1,
1, 0, 0).finished();
Matrix IexpectedL = (Matrix(3,3) <<
0, 0, 1,
1, 0, 0,
0, 1, 0).finished();
EXPECT(assert_equal(IexpectedR, I*p));
EXPECT(assert_equal(IexpectedL, p*I));
EXPECT(assert_equal(IexpectedR, p.transpose()*I));
}
/* ************************************************************************* */
int main() {
TestResult tr;

30
gtsam/linear/GaussianConditional.cpp
View File

@ -87,7 +87,29 @@ GaussianConditional::GaussianConditional(Index key, const Vector& d, const Matri
rsd_(j).noalias() = parent->second;
++ j;
}
get_d_().noalias() = d;
get_d_() = d;
}
/* ************************************************************************* */
GaussianConditional::GaussianConditional(const std::list<std::pair<Index, Matrix> >& terms,
const size_t nrFrontals, const Vector& d, const Vector& sigmas) :
IndexConditional(GetKeys(terms.size(), terms.begin(), terms.end()), nrFrontals),
rsd_(matrix_), sigmas_(sigmas) {
size_t dims[terms.size()+1];
size_t j=0;
typedef pair<Index, Matrix> Index_Matrix;
BOOST_FOREACH(const Index_Matrix& term, terms) {
dims[j] = term.second.cols();
++ j;
}
dims[j] = 1;
rsd_.copyStructureFrom(rsd_type(matrix_, dims, dims+terms.size()+1, d.size()));
j=0;
BOOST_FOREACH(const Index_Matrix& term, terms) {
rsd_(j) = term.second;
++ j;
}
get_d_() = d;
}
/* ************************************************************************* */
@ -104,6 +126,7 @@ void GaussianConditional::print(const string &s) const
}
gtsam::print(Vector(get_d()),"d");
gtsam::print(sigmas_,"sigmas");
cout << "Permutation: " << permutation_.indices() << endl;
}
/* ************************************************************************* */
@ -139,11 +162,6 @@ bool GaussianConditional::equals(const GaussianConditional &c, double tol) const
return true;
}
/* ************************************************************************* */
GaussianConditional::rsd_type::constBlock GaussianConditional::get_R() const {
return rsd_.range(0, nrFrontals());
}
/* ************************************************************************* */
JacobianFactor::shared_ptr GaussianConditional::toFactor() const {
return JacobianFactor::shared_ptr(new JacobianFactor(*this));

27
gtsam/linear/GaussianConditional.h
View File

@ -31,6 +31,9 @@
class eliminate2JacobianFactorTest;
class constructorGaussianConditionalTest;
class eliminationGaussianFactorGraphTest;
class complicatedMarginalGaussianJunctionTreeTest;
class computeInformationGaussianConditionalTest;
class isGaussianFactorGaussianConditionalTest;
namespace gtsam {
@ -105,12 +108,20 @@ public:
Index name1, const Matrix& S, Index name2, const Matrix& T, const Vector& sigmas);
/**
* constructor with number of arbitrary parents
* constructor with number of arbitrary parents (only used in unit tests,
* std::list is not efficient)
* |Rx+sum(Ai*xi)-d|
*/
GaussianConditional(Index key, const Vector& d,
const Matrix& R, const std::list<std::pair<Index, Matrix> >& parents, const Vector& sigmas);
/**
* Constructor with arbitrary number of frontals and parents (only used in unit tests,
* std::list is not efficient)
*/
GaussianConditional(const std::list<std::pair<Index, Matrix> >& terms,
size_t nrFrontals, const Vector& d, const Vector& sigmas);
/**
* Constructor when matrices are already stored in a combined matrix, allows
* for multiple frontal variables.
@ -131,13 +142,12 @@ public:
/** Compute the information matrix */
Matrix computeInformation() const {
Matrix R = get_R();
R = R*permutation_;
return R.transpose()*R;
Matrix R = get_R() * permutation_.transpose();
return R.transpose() * R;
}
/** return stuff contained in GaussianConditional */
rsd_type::constBlock get_R() const;
rsd_type::constBlock get_R() const { return rsd_.range(0, nrFrontals()); }
/** access the d vector */
const_d_type get_d() const { return rsd_.column(nrFrontals()+nrParents(), 0); }
@ -216,8 +226,8 @@ public:
void scaleFrontalsBySigma(VectorValues& gy) const;
protected:
rsd_type::Column get_d_() { return rsd_.column(1+nrParents(), 0); }
rsd_type::Block get_R_() { return rsd_(0); }
rsd_type::Column get_d_() { return rsd_.column(nrFrontals()+nrParents(), 0); }
rsd_type::Block get_R_() { return rsd_.range(0, nrFrontals()); }
rsd_type::Block get_S_(iterator variable) { return rsd_(variable - this->begin()); }
private:
@ -227,6 +237,9 @@ private:
friend class ::eliminate2JacobianFactorTest;
friend class ::constructorGaussianConditionalTest;
friend class ::eliminationGaussianFactorGraphTest;
friend class ::complicatedMarginalGaussianJunctionTreeTest;
friend class ::computeInformationGaussianConditionalTest;
friend class ::isGaussianFactorGaussianConditionalTest;
/** Serialization function */
friend class boost::serialization::access;

2
gtsam/linear/JacobianFactor.cpp
View File

@ -154,7 +154,7 @@ namespace gtsam {
JacobianFactor::JacobianFactor(const GaussianConditional& cg) : GaussianFactor(cg), model_(noiseModel::Diagonal::Sigmas(cg.get_sigmas(), true)), Ab_(matrix_) {
Ab_.assignNoalias(cg.rsd_);
// TODO: permutation for all frontal blocks
Ab_.range(0,cg.nrFrontals()) = Ab_.range(0,cg.nrFrontals()) * cg.permutation_;
Ab_.range(0,cg.nrFrontals()) = Ab_.range(0,cg.nrFrontals()) * cg.permutation_.transpose();
// todo SL: make firstNonzeroCols triangular?
firstNonzeroBlocks_.resize(cg.get_d().size(), 0); // set sigmas from precisions
assertInvariants();

78
gtsam/linear/tests/testGaussianConditional.cpp
View File

@ -382,6 +382,84 @@ TEST( GaussianConditional, solveTranspose ) {
CHECK(assert_equal(y,actual));
}
/* ************************************************************************* */
TEST( GaussianConditional, computeInformation ) {
// Create R matrix
Matrix R = (Matrix(4,4) <<
1, 2, 3, 4,
0, 5, 6, 7,
0, 0, 8, 9,
0, 0, 0, 10).finished();
// Shuffle columns
Eigen::Transpositions<Eigen::Dynamic> p(4);
p.indices()[0] = 1;
p.indices()[1] = 1;
p.indices()[2] = 2;
p.indices()[3] = 0;
// The expected result of permuting R
Matrix RpExpected = (Matrix(4,4) <<
2, 4, 3, 1,
5, 7, 6, 0,
0, 9, 8, 0,
0, 10,0, 0).finished();
// Check that the permutation does what we expect
Matrix RpActual = R * p.transpose();
EXPECT(assert_equal(RpExpected, RpActual));
// Create conditional with the permutation
GaussianConditional conditional(0, Vector::Zero(4), R, Vector::Constant(4, 1.0));
conditional.permutation_ = p;
// Expected information matrix (using permuted R)
Matrix IExpected = RpExpected.transpose() * RpExpected;
// Actual information matrix (conditional should permute R)
Matrix IActual = conditional.computeInformation();
EXPECT(assert_equal(IExpected, IActual));
}
/* ************************************************************************* */
TEST( GaussianConditional, isGaussianFactor ) {
// Create R matrix
Matrix R = (Matrix(4,4) <<
1, 2, 3, 4,
0, 5, 6, 7,
0, 0, 8, 9,
0, 0, 0, 10).finished();
// Shuffle columns
Eigen::Transpositions<Eigen::Dynamic> p(4);
p.indices()[0] = 1;
p.indices()[1] = 1;
p.indices()[2] = 2;
p.indices()[3] = 0;
// The expected result of the permutation
Matrix RpExpected = (Matrix(4,4) <<
4, 1, 3, 2,
7, 0, 6, 5,
9, 0, 8, 0,
10,0, 0, 0).finished();
// Create a conditional with this permutation
GaussianConditional conditional(0, Vector::Zero(4), R, Vector::Constant(4, 1.0));
conditional.permutation_ = p;
// Expected information matrix computed by conditional
Matrix IExpected = conditional.computeInformation();
// Expected information matrix computed by a factor
JacobianFactor jf = *conditional.toFactor();
Matrix IActual = jf.getA(jf.begin()).transpose() * jf.getA(jf.begin());
EXPECT(assert_equal(IExpected, IActual));
}
/* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr);}
/* ************************************************************************* */

148
gtsam/linear/tests/testGaussianJunctionTree.cpp
View File

@ -25,7 +25,10 @@
using namespace boost::assign;
#include <gtsam/base/debug.h>
#include <gtsam/geometry/Rot2.h>
#include <gtsam/linear/GaussianJunctionTree.h>
#include <gtsam/linear/GaussianSequentialSolver.h>
#include <gtsam/linear/GaussianMultifrontalSolver.h>
#include <gtsam/inference/BayesTree-inl.h>
using namespace std;
@ -116,6 +119,151 @@ TEST_UNSAFE( GaussianJunctionTree, optimizeMultiFrontal )
EXPECT(assert_equal(expected,actual));
}
/* ************************************************************************* */
TEST_UNSAFE(GaussianJunctionTree, complicatedMarginal) {
// Create the conditionals to go in the BayesTree
GaussianConditional::shared_ptr R_1_2(new GaussianConditional(
pair_list_of
(1, (Matrix(3,1) <<
0.2630,
0,
0).finished())
(2, (Matrix(3,2) <<
0.7482, 0.2290,
0.4505, 0.9133,
0, 0.1524).finished())
(5, (Matrix(3,1) <<
0.8258,
0.5383,
0.9961).finished()),
2, (Vector(3) << 0.0782, 0.4427, 0.1067).finished(), ones(3)));
GaussianConditional::shared_ptr R_3_4(new GaussianConditional(
pair_list_of
(3, (Matrix(3,1) <<
0.0540,
0,
0).finished())
(4, (Matrix(3,2) <<
0.9340, 0.4694,
0.1299, 0.0119,
0, 0.3371).finished())
(6, (Matrix(3,2) <<
0.1622, 0.5285,
0.7943, 0.1656,
0.3112, 0.6020).finished()),
2, (Vector(3) << 0.9619, 0.0046, 0.7749).finished(), ones(3)));
GaussianConditional::shared_ptr R_5_6(new GaussianConditional(
pair_list_of
(5, (Matrix(3,1) <<
0.2435,
0,
0).finished())
(6, (Matrix(3,2) <<
0.1966, 0.4733,
0.2511, 0.3517,
0, 0.8308).finished())
(7, (Matrix(3,1) <<
0.5853,
0.5497,
0.9172).finished())
(8, (Matrix(3,2) <<
0.2858, 0.3804,
0.7572, 0.5678,
0.7537, 0.0759).finished()),
2, (Vector(3) << 0.8173, 0.8687, 0.0844).finished(), ones(3)));
R_5_6->permutation_ = Eigen::Transpositions<Eigen::Dynamic>(2);
R_5_6->permutation_.indices()(0) = 0;
R_5_6->permutation_.indices()(1) = 2;
GaussianConditional::shared_ptr R_7_8(new GaussianConditional(
pair_list_of
(7, (Matrix(3,1) <<
0.2551,
0,
0).finished())
(8, (Matrix(3,2) <<
0.8909, 0.1386,
0.9593, 0.1493,
0, 0.2575).finished())
(11, (Matrix(3,1) <<
0.8407,
0.2543,
0.8143).finished()),
2, (Vector(3) << 0.3998, 0.2599, 0.8001).finished(), ones(3)));
GaussianConditional::shared_ptr R_9_10(new GaussianConditional(
pair_list_of
(9, (Matrix(3,1) <<
0.7952,
0,
0).finished())
(10, (Matrix(3,2) <<
0.4456, 0.7547,
0.6463, 0.2760,
0, 0.6797).finished())
(11, (Matrix(3,1) <<
0.6551,
0.1626,
0.1190).finished())
(12, (Matrix(3,2) <<
0.4984, 0.5853,
0.9597, 0.2238,
0.3404, 0.7513).finished()),
2, (Vector(3) << 0.4314, 0.9106, 0.1818).finished(), ones(3)));
GaussianConditional::shared_ptr R_11_12(new GaussianConditional(
pair_list_of
(11, (Matrix(3,1) <<
0.0971,
0,
0).finished())
(12, (Matrix(3,2) <<
0.3171, 0.4387,
0.9502, 0.3816,
0, 0.7655).finished()),
2, (Vector(3) << 0.2638, 0.1455, 0.1361).finished(), ones(3)));
// Gaussian Bayes Tree
typedef BayesTree<GaussianConditional> GaussianBayesTree;
typedef GaussianBayesTree::Clique Clique;
typedef GaussianBayesTree::sharedClique sharedClique;
// Create Bayes Tree
GaussianBayesTree bt;
bt.insert(sharedClique(new Clique(R_11_12)));
bt.insert(sharedClique(new Clique(R_9_10)));
bt.insert(sharedClique(new Clique(R_7_8)));
bt.insert(sharedClique(new Clique(R_5_6)));
bt.insert(sharedClique(new Clique(R_3_4)));
bt.insert(sharedClique(new Clique(R_1_2)));
// Marginal on 5
Matrix expectedCov = (Matrix(1,1) << 236.5166).finished();
JacobianFactor::shared_ptr actualJacobian = boost::dynamic_pointer_cast<JacobianFactor>(
bt.marginalFactor(5, EliminateLDL));
LONGS_EQUAL(1, actualJacobian->rows());
LONGS_EQUAL(1, actualJacobian->size());
LONGS_EQUAL(5, actualJacobian->keys()[0]);
Matrix actualA = actualJacobian->getA(actualJacobian->begin());
Matrix actualCov = inverse(actualA.transpose() * actualA);
EXPECT(assert_equal(expectedCov, actualCov, 1e-1));
// Marginal on 6
// expectedCov = (Matrix(2,2) <<
// 8471.2, 2886.2,
// 2886.2, 1015.8).finished();
expectedCov = (Matrix(2,2) <<
1015.8, 2886.2,
2886.2, 8471.2).finished();
actualJacobian = boost::dynamic_pointer_cast<JacobianFactor>(
bt.marginalFactor(6, EliminateLDL));
LONGS_EQUAL(2, actualJacobian->rows());
LONGS_EQUAL(1, actualJacobian->size());
LONGS_EQUAL(6, actualJacobian->keys()[0]);
actualA = actualJacobian->getA(actualJacobian->begin());
actualCov = inverse(actualA.transpose() * actualA);
EXPECT(assert_equal(expectedCov, actualCov, 1e1));
}
/* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr);}
/* ************************************************************************* */

45
tests/testGaussianJunctionTree.cpp
View File

@ -30,11 +30,16 @@ using namespace boost::assign;
// Magically casts strings like "x3" to a Symbol('x',3) key, see Key.h
#define GTSAM_MAGIC_KEY
#include <gtsam/base/debug.h>
#include <gtsam/base/cholesky.h>
#include <gtsam/inference/BayesTree-inl.h>
#include <gtsam/nonlinear/Ordering.h>
#include <gtsam/linear/GaussianJunctionTree.h>
#include <gtsam/linear/GaussianSequentialSolver.h>
#include <gtsam/linear/GaussianMultifrontalSolver.h>
#include <gtsam/slam/smallExample.h>
#include <gtsam/slam/planarSLAM.h>
#include <gtsam/slam/pose2SLAM.h>
using namespace std;
using namespace gtsam;
@ -183,7 +188,47 @@ TEST(GaussianJunctionTree, slamlike) {
planarSLAM::Values expected = init.expmap(delta, ordering);
EXPECT(assert_equal(expected, actual));
}
/* ************************************************************************* */
TEST(GaussianJunctionTree, simpleMarginal) {
typedef BayesTree<GaussianConditional> GaussianBayesTree;
// Create a simple graph
pose2SLAM::Graph fg;
fg.addPrior(pose2SLAM::Key(0), Pose2(), sharedSigma(3, 10.0));
fg.addConstraint(0, 1, Pose2(1.0, 0.0, 0.0), sharedSigmas(Vector_(3, 10.0, 1.0, 1.0)));
pose2SLAM::Values init;
init.insert(0, Pose2());
init.insert(1, Pose2(1.0, 0.0, 0.0));
Ordering ordering;
ordering += "x1", "x0";
GaussianFactorGraph gfg = *fg.linearize(init, ordering);
// Compute marginals with both sequential and multifrontal
Matrix expected = GaussianSequentialSolver(gfg).marginalCovariance(1);
Matrix actual1 = GaussianMultifrontalSolver(gfg).marginalCovariance(1);
// Compute marginal directly from marginal factor
GaussianFactor::shared_ptr marginalFactor = GaussianMultifrontalSolver(gfg).marginalFactor(1);
JacobianFactor::shared_ptr marginalJacobian = boost::dynamic_pointer_cast<JacobianFactor>(marginalFactor);
Matrix actual2 = inverse(marginalJacobian->getA(marginalJacobian->begin()).transpose() * marginalJacobian->getA(marginalJacobian->begin()));
// Compute marginal directly from BayesTree
GaussianBayesTree gbt;
gbt.insert(GaussianJunctionTree(gfg).eliminate(EliminateLDL));
marginalFactor = gbt.marginalFactor(1, EliminateLDL);
marginalJacobian = boost::dynamic_pointer_cast<JacobianFactor>(marginalFactor);
Matrix actual3 = inverse(marginalJacobian->getA(marginalJacobian->begin()).transpose() * marginalJacobian->getA(marginalJacobian->begin()));
EXPECT(assert_equal(expected, actual1));
EXPECT(assert_equal(expected, actual2));
EXPECT(assert_equal(expected, actual3));
}
/* ************************************************************************* */