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

Removed LDL in favor of Cholesky

release/4.3a0
Richard Roberts 2012-05-15 15:49:14 +00:00
parent 2009357584
commit 1ce95c1d89
32 changed files with 98 additions and 568 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
examples/elaboratePoint2KalmanFilter.cpp
View File

@ -35,7 +35,7 @@ using namespace gtsam;
int main() { int main() {
// [code below basically does SRIF with LDL] // [code below basically does SRIF with Cholesky]
// Create a factor graph to perform the inference // Create a factor graph to perform the inference
GaussianFactorGraph::shared_ptr linearFactorGraph(new GaussianFactorGraph); GaussianFactorGraph::shared_ptr linearFactorGraph(new GaussianFactorGraph);

67
gtsam/base/cholesky.cpp
View File

@ -155,71 +155,4 @@ void choleskyPartial(Matrix& ABC, size_t nFrontal) {
toc(3, "compute L"); toc(3, "compute L");
} }
/* ************************************************************************* */
Eigen::LDLT<Matrix>::TranspositionType ldlPartial(Matrix& ABC, size_t nFrontal) {
const bool debug = ISDEBUG("ldlPartial");
assert(ABC.rows() == ABC.cols());
assert(ABC.rows() >= 0 && nFrontal <= size_t(ABC.rows()));
const size_t n = ABC.rows();
if(debug) {
cout << "Partial LDL with " << nFrontal << " frontal scalars, ";
print(ABC, "LDL ABC: ");
}
// Compute Cholesky factorization of A, overwrites A = sqrt(D)*R
// tic(1, "ldl");
Eigen::LDLT<Matrix> ldlt;
ldlt.compute(ABC.block(0,0,nFrontal,nFrontal).selfadjointView<Eigen::Upper>());
if (debug) ldlt.isNegative() ? cout << "Matrix is negative" << endl : cout << "Matrix is not negative" << endl;
Vector D = ldlt.vectorD();
if(dampUnderconstrained) {
D = D.array().max(Vector::Constant(D.rows(), D.cols(), underconstrainedPrior).array());
}
Vector sqrtD = D.cwiseSqrt(); // FIXME: we shouldn't do sqrt in LDL
if (debug) cout << "LDL Dsqrt:\n" << sqrtD << endl;
if(D.unaryExpr(boost::bind(less<double>(), _1, 0.0)).any()) {
if(ISDEBUG("detailed_exceptions"))
throw NegativeMatrixException(NegativeMatrixException::Detail(ABC, ldlt.matrixU(), ldlt.vectorD()));
else
throw NegativeMatrixException();
}
// U = sqrtD * L^
Matrix U = ldlt.matrixU();
if(debug) cout << "LDL U:\n" << U << endl;
// we store the permuted upper triangular matrix
ABC.block(0,0,nFrontal,nFrontal) = sqrtD.asDiagonal() * U; // FIXME: this isn't actually LDL', it's Cholesky
if(debug) cout << "LDL R:\n" << ABC.topLeftCorner(nFrontal,nFrontal) << endl;
// toc(1, "ldl");
// Compute S = inv(R') * B = inv(P'U')*B = inv(U')*P*B
// tic(2, "compute S");
if(n - nFrontal > 0) {
ABC.topRightCorner(nFrontal, n-nFrontal) = ldlt.transpositionsP() * ABC.topRightCorner(nFrontal, n-nFrontal);
ABC.block(0,0,nFrontal,nFrontal).triangularView<Eigen::Upper>().transpose().solveInPlace(ABC.topRightCorner(nFrontal, n-nFrontal));
}
if(debug) cout << "LDL S:\n" << ABC.topRightCorner(nFrontal, n-nFrontal) << endl;
// toc(2, "compute S");
// Compute L = C - S' * S
// tic(3, "compute L");
if(debug) cout << "LDL C:\n" << Eigen::MatrixXd(ABC.bottomRightCorner(n-nFrontal,n-nFrontal).selfadjointView<Eigen::Upper>()) << endl;
if(n - nFrontal > 0)
ABC.bottomRightCorner(n-nFrontal,n-nFrontal).selfadjointView<Eigen::Upper>().rankUpdate(
ABC.topRightCorner(nFrontal, n-nFrontal).transpose(), -1.0);
if(debug) cout << "LDL L:\n" << Eigen::MatrixXd(ABC.bottomRightCorner(n-nFrontal,n-nFrontal).selfadjointView<Eigen::Upper>()) << endl;
// toc(3, "compute L");
if(debug) cout << "LDL P: " << ldlt.transpositionsP().indices() << endl;
return ldlt.transpositionsP();
}
} }

19
gtsam/base/cholesky.h
View File

@ -90,24 +90,5 @@ std::pair<size_t,bool> choleskyCareful(Matrix& ATA, int order = -1);
*/ */
void choleskyPartial(Matrix& ABC, size_t nFrontal); void choleskyPartial(Matrix& ABC, size_t nFrontal);
/**
* Partial LDL computes a factor [R S such that [P'R' 0 [RP S = [A B
* 0 F] S' I] 0 F] B' C].
* The input to this function is the matrix ABC = [A B], and the parameter
* [B' C]
* nFrontal determines the split between A, B, and C, with A being of size
* nFrontal x nFrontal.
* P is a permutation matrix obtained from the pivoting process while doing LDL'.
*
* Specifically, if A = P'U'DUP is the LDL' factorization of A,
* then R = sqrt(D)*U is the permuted upper-triangular matrix.
* The permutation is returned so that it can be used in the backsubstitution
* process to return the correct order of variables, and in creating the
* Jacobian factor by permuting R correctly.
*
* Note that S and F are not permuted (in correct original ordering).
*/
Eigen::LDLT<Matrix>::TranspositionType ldlPartial(Matrix& ABC, size_t nFrontal);
} }

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

@ -53,113 +53,6 @@ TEST(cholesky, choleskyPartial) {
EXPECT(assert_equal(expected, actual, 1e-9)); EXPECT(assert_equal(expected, actual, 1e-9));
} }
/* ************************************************************************* */
TEST(cholesky, ldlPartial) {
// 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, 3);
// See the function comment for choleskyPartial, this decomposition should hold.
Matrix R1 = RSL.transpose();
Matrix R2 = RSL;
R1.block(3, 3, 4, 4).setIdentity();
R2.block(3, 3, 4, 4) = R2.block(3, 3, 4, 4).selfadjointView<Eigen::Upper>();
// un-permute the permuted upper triangular part
R1.topLeftCorner(3,3) = permutation.transpose() * R1.topLeftCorner(3,3);
R2.topLeftCorner(3,3) = R2.topLeftCorner(3,3)*permutation;
Matrix actual = R1 * R2;
Matrix expected = ABC.selfadjointView<Eigen::Upper>();
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));
}
/* ************************************************************************* */ /* ************************************************************************* */
TEST(cholesky, BadScalingCholesky) { TEST(cholesky, BadScalingCholesky) {
Matrix A = Matrix_(2,2, Matrix A = Matrix_(2,2,
@ -175,24 +68,6 @@ TEST(cholesky, BadScalingCholesky) {
DOUBLES_EQUAL(expectedSqrtCondition, actualSqrtCondition, 1e-41); DOUBLES_EQUAL(expectedSqrtCondition, actualSqrtCondition, 1e-41);
} }
/* ************************************************************************* */
TEST(cholesky, BadScalingLDL) {
Matrix A = Matrix_(2,2,
1.0, 0.0,
0.0, 1e-40);
Matrix R(A.transpose() * A);
Eigen::LDLT<Matrix>::TranspositionType permutation = ldlPartial(R, 2);
EXPECT(permutation.indices()(0) == 0);
EXPECT(permutation.indices()(1) == 1);
double expectedCondition = 1e40;
double actualCondition = R(0,0) / R(1,1);
DOUBLES_EQUAL(expectedCondition, actualCondition, 1e-41);
}
/* ************************************************************************* */ /* ************************************************************************* */
TEST(cholesky, BadScalingSVD) { TEST(cholesky, BadScalingSVD) {
Matrix A = Matrix_(2,2, Matrix A = Matrix_(2,2,

2
gtsam/linear/GaussianBayesNet.h
View File

@ -116,8 +116,6 @@ namespace gtsam {
/** /**
* Return (dense) upper-triangular matrix representation * Return (dense) upper-triangular matrix representation
* NOTE: if this is the result of elimination with LDL, the matrix will
* not necessarily be upper triangular due to column permutations
*/ */
std::pair<Matrix, Vector> matrix(const GaussianBayesNet&); std::pair<Matrix, Vector> matrix(const GaussianBayesNet&);

7
gtsam/linear/GaussianConditional.cpp
View File

@ -124,7 +124,6 @@ GaussianConditional& GaussianConditional::operator=(const GaussianConditional& r
this->Base::operator=(rhs); // Copy keys this->Base::operator=(rhs); // Copy keys
rsd_.assignNoalias(rhs.rsd_); // Copy matrix and block configuration rsd_.assignNoalias(rhs.rsd_); // Copy matrix and block configuration
sigmas_ = rhs.sigmas_; // Copy sigmas sigmas_ = rhs.sigmas_; // Copy sigmas
permutation_ = rhs.permutation_; // Copy permutation
} }
return *this; return *this;
} }
@ -143,7 +142,6 @@ void GaussianConditional::print(const string &s) const
} }
gtsam::print(Vector(get_d()),"d"); gtsam::print(Vector(get_d()),"d");
gtsam::print(sigmas_,"sigmas"); gtsam::print(sigmas_,"sigmas");
cout << "Permutation: " << permutation_.indices().transpose() << endl;
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -196,8 +194,7 @@ inline static void doSolveInPlace(const GaussianConditional& conditional, VALUES
if(debug) conditional.print("Solving conditional in place"); if(debug) conditional.print("Solving conditional in place");
Vector xS = internal::extractVectorValuesSlices(x, conditional.beginParents(), conditional.endParents()); Vector xS = internal::extractVectorValuesSlices(x, conditional.beginParents(), conditional.endParents());
xS = conditional.get_d() - conditional.get_S() * xS; xS = conditional.get_d() - conditional.get_S() * xS;
Vector soln = conditional.permutation().transpose() * Vector soln = conditional.get_R().triangularView<Eigen::Upper>().solve(xS);
conditional.get_R().triangularView<Eigen::Upper>().solve(xS);
if(debug) { if(debug) {
gtsam::print(Matrix(conditional.get_R()), "Calling backSubstituteUpper on "); gtsam::print(Matrix(conditional.get_R()), "Calling backSubstituteUpper on ");
gtsam::print(soln, "full back-substitution solution: "); gtsam::print(soln, "full back-substitution solution: ");
@ -219,7 +216,7 @@ void GaussianConditional::solveInPlace(Permuted<VectorValues>& x) const {
void GaussianConditional::solveTransposeInPlace(VectorValues& gy) const { void GaussianConditional::solveTransposeInPlace(VectorValues& gy) const {
Vector frontalVec = internal::extractVectorValuesSlices(gy, beginFrontals(), endFrontals()); Vector frontalVec = internal::extractVectorValuesSlices(gy, beginFrontals(), endFrontals());
// TODO: verify permutation // TODO: verify permutation
frontalVec = permutation_ * gtsam::backSubstituteUpper(frontalVec,Matrix(get_R())); frontalVec = gtsam::backSubstituteUpper(frontalVec,Matrix(get_R()));
GaussianConditional::const_iterator it; GaussianConditional::const_iterator it;
for (it = beginParents(); it!= endParents(); it++) { for (it = beginParents(); it!= endParents(); it++) {
const Index i = *it; const Index i = *it;

26
gtsam/linear/GaussianConditional.h
View File

@ -60,17 +60,10 @@ public:
typedef rsd_type::Column d_type; typedef rsd_type::Column d_type;
typedef rsd_type::constColumn const_d_type; typedef rsd_type::constColumn const_d_type;
typedef Eigen::LDLT<Matrix>::TranspositionType TranspositionType;
protected: protected:
/** Store the conditional as one big upper-triangular wide matrix, arranged /** Store the conditional as one big upper-triangular wide matrix, arranged
* as \f$ [ R S1 S2 ... d ] \f$. Access these blocks using a VerticalBlockView. * as \f$ [ R S1 S2 ... d ] \f$. Access these blocks using a VerticalBlockView.
*
* WARNING!!! When using with LDL, R is the permuted upper triangular matrix.
* Its columns/rows do not correspond to the correct components of the variables.
* Use R*permutation_ to get back the correct non-permuted order,
* for example when converting to the Jacobian
* */ * */
RdMatrix matrix_; RdMatrix matrix_;
rsd_type rsd_; rsd_type rsd_;
@ -78,10 +71,6 @@ protected:
/** vector of standard deviations */ /** vector of standard deviations */
Vector sigmas_; Vector sigmas_;
/** Store the permutation matrix, used by LDL' in the pivoting process
* This is used to get back the correct ordering of x after solving by backSubstitition */
TranspositionType permutation_;
/** typedef to base class */ /** typedef to base class */
typedef IndexConditional Base; typedef IndexConditional Base;
@ -131,8 +120,7 @@ public:
*/ */
template<typename ITERATOR, class MATRIX> template<typename ITERATOR, class MATRIX>
GaussianConditional(ITERATOR firstKey, ITERATOR lastKey, size_t nrFrontals, GaussianConditional(ITERATOR firstKey, ITERATOR lastKey, size_t nrFrontals,
const VerticalBlockView<MATRIX>& matrices, const Vector& sigmas, const VerticalBlockView<MATRIX>& matrices, const Vector& sigmas);
const TranspositionType& permutation = TranspositionType());
/** Copy constructor */ /** Copy constructor */
GaussianConditional(const GaussianConditional& rhs); GaussianConditional(const GaussianConditional& rhs);
@ -160,8 +148,7 @@ public:
/** Compute the information matrix */ /** Compute the information matrix */
Matrix computeInformation() const { Matrix computeInformation() const {
Matrix R = get_R() * permutation_.transpose(); return get_R().transpose() * get_R();
return R.transpose() * R;
} }
/** Return a view of the upper-triangular R block of the conditional */ /** Return a view of the upper-triangular R block of the conditional */
@ -180,11 +167,6 @@ public:
/** Get the Vector of sigmas */ /** Get the Vector of sigmas */
const Vector& get_sigmas() const {return sigmas_;} const Vector& get_sigmas() const {return sigmas_;}
/** Return the permutation of R made by LDL, to recover R in the correct
* order use R*permutation, see the GaussianConditional main class comment
*/
const TranspositionType& permutation() const { return permutation_; }
protected: protected:
const RdMatrix& matrix() const { return matrix_; } const RdMatrix& matrix() const { return matrix_; }
@ -269,9 +251,9 @@ private:
template<typename ITERATOR, class MATRIX> template<typename ITERATOR, class MATRIX>
GaussianConditional::GaussianConditional(ITERATOR firstKey, ITERATOR lastKey, GaussianConditional::GaussianConditional(ITERATOR firstKey, ITERATOR lastKey,
size_t nrFrontals, const VerticalBlockView<MATRIX>& matrices, size_t nrFrontals, const VerticalBlockView<MATRIX>& matrices,
const Vector& sigmas, const GaussianConditional::TranspositionType& permutation) : const Vector& sigmas) :
IndexConditional(std::vector<Index>(firstKey, lastKey), nrFrontals), rsd_( IndexConditional(std::vector<Index>(firstKey, lastKey), nrFrontals), rsd_(
matrix_), sigmas_(sigmas), permutation_(permutation) { matrix_), sigmas_(sigmas) {
rsd_.assignNoalias(matrices); rsd_.assignNoalias(matrices);
} }

1
gtsam/linear/GaussianDensity.cpp
View File

@ -32,7 +32,6 @@ namespace gtsam {
gtsam::print(Matrix(get_R()),"R"); gtsam::print(Matrix(get_R()),"R");
gtsam::print(Vector(get_d()),"d"); gtsam::print(Vector(get_d()),"d");
gtsam::print(sigmas_,"sigmas"); gtsam::print(sigmas_,"sigmas");
cout << "Permutation: " << permutation_.indices().transpose() << endl;
} }
/* ************************************************************************* */ /* ************************************************************************* */

141
gtsam/linear/GaussianFactorGraph.cpp
View File

@ -557,145 +557,4 @@ break;
} // \EliminatePreferCholesky } // \EliminatePreferCholesky
/* ************************************************************************* */
GaussianFactorGraph::EliminationResult EliminateLDL(
const FactorGraph<GaussianFactor>& factors, size_t nrFrontals) {
const bool debug = ISDEBUG("EliminateLDL");
// Find the scatter and variable dimensions
tic(1, "find scatter");
Scatter scatter(findScatterAndDims(factors));
toc(1, "find scatter");
// Pull out keys and dimensions
tic(2, "keys");
vector<size_t> dimensions(scatter.size() + 1);
BOOST_FOREACH(const Scatter::value_type& var_slot, scatter) {
dimensions[var_slot.second.slot] = var_slot.second.dimension;
}
// This is for the r.h.s. vector
dimensions.back() = 1;
toc(2, "keys");
// Form Ab' * Ab
tic(3, "combine");
if (debug) {
// print out everything before combine
factors.print("Factors to be combined into hessian");
cout << "Dimensions (" << dimensions.size() << "): ";
BOOST_FOREACH(size_t d, dimensions) cout << d << " ";
cout << "\nScatter:" << endl;
BOOST_FOREACH(const Scatter::value_type& p, scatter)
cout << " Index: " << p.first << ", " << p.second.toString() << endl;
}
HessianFactor::shared_ptr //
combinedFactor(new HessianFactor(factors, dimensions, scatter));
toc(3, "combine");
// Do LDL, note that after this, the lower triangle still contains
// some untouched non-zeros that should be zero. We zero them while
// extracting submatrices next.
tic(4, "partial LDL");
Eigen::LDLT<Matrix>::TranspositionType permutation = combinedFactor->partialLDL(nrFrontals);
toc(4, "partial LDL");
// Extract conditional and fill in details of the remaining factor
tic(5, "split");
GaussianConditional::shared_ptr conditional =
combinedFactor->splitEliminatedFactor(nrFrontals, permutation);
if (debug) {
conditional->print("Extracted conditional: ");
combinedFactor->print("Eliminated factor (L piece): ");
}
toc(5, "split");
combinedFactor->assertInvariants();
return make_pair(conditional, combinedFactor);
} // \EliminateLDL
/* ************************************************************************* */
GaussianFactorGraph::EliminationResult EliminatePreferLDL(
const FactorGraph<GaussianFactor>& factors, size_t nrFrontals) {
typedef JacobianFactor J;
typedef HessianFactor H;
// If any JacobianFactors have constrained noise models, we have to convert
// all factors to JacobianFactors. Otherwise, we can convert all factors
// to HessianFactors. This is because QR can handle constrained noise
// models but LDL cannot.
// Decide whether to use QR or LDL
// Check if any JacobianFactors have constrained noise models.
if (hasConstraints(factors))
return EliminateQR(factors, nrFrontals);
else {
GaussianFactorGraph::EliminationResult ret;
#ifdef NDEBUG
static const bool diag = false;
#else
static const bool diag = !ISDEBUG("NoLDLDiagnostics");
#endif
if(!diag) {
tic(2, "EliminateLDL");
ret = EliminateLDL(factors, nrFrontals);
toc(2, "EliminateLDL");
} else {
try {
tic(2, "EliminateLDL");
ret = EliminateLDL(factors, nrFrontals);
toc(2, "EliminateLDL");
} catch (const NegativeMatrixException& e) {
throw;
} catch (const exception& e) {
cout << "Exception in EliminateLDL: " << e.what() << endl;
SETDEBUG("EliminateLDL", true);
SETDEBUG("updateATA", true);
SETDEBUG("JacobianFactor::eliminate", true);
SETDEBUG("JacobianFactor::Combine", true);
SETDEBUG("ldlPartial", true);
SETDEBUG("findScatterAndDims", true);
factors.print("Combining factors: ");
EliminateLDL(factors, nrFrontals);
throw;
}
}
const bool checkLDL = ISDEBUG("EliminateGaussian Check LDL");
if (checkLDL) {
GaussianFactorGraph::EliminationResult expected;
FactorGraph<J> jacobians = convertToJacobians(factors);
try {
// Compare with QR
expected = EliminateJacobians(jacobians, nrFrontals);
} catch (...) {
cout << "Exception in QR" << endl;
throw;
}
H actual_factor(*ret.second);
H expected_factor(*expected.second);
if (!assert_equal(*expected.first, *ret.first, 100.0) || !assert_equal(
expected_factor, actual_factor, 1.0)) {
cout << "LDL and QR do not agree" << endl;
SETDEBUG("EliminateLDL", true);
SETDEBUG("updateATA", true);
SETDEBUG("JacobianFactor::eliminate", true);
SETDEBUG("JacobianFactor::Combine", true);
jacobians.print("Jacobian Factors: ");
EliminateJacobians(jacobians, nrFrontals);
EliminateLDL(factors, nrFrontals);
factors.print("Combining factors: ");
throw runtime_error("LDL and QR do not agree");
}
}
return ret;
}
} // \EliminatePreferLDL
} // namespace gtsam } // namespace gtsam

43
gtsam/linear/GaussianFactorGraph.h
View File

@ -286,47 +286,4 @@ namespace gtsam {
GaussianFactorGraph::EliminationResult EliminateCholesky(const FactorGraph< GaussianFactorGraph::EliminationResult EliminateCholesky(const FactorGraph<
GaussianFactor>& factors, size_t nrFrontals = 1); GaussianFactor>& factors, size_t nrFrontals = 1);
/**
* Densely partially eliminate with LDL factorization. JacobianFactors
* are left-multiplied with their transpose to form the Hessian using the
* conversion constructor HessianFactor(const JacobianFactor&).
*
* If any factors contain constrained noise models (any sigmas equal to
* zero), QR factorization will be performed instead, because our current
* implementation cannot handle constrained noise models in LDL
* factorization. EliminateLDL(), on the other hand, will fail if any
* factors contain constrained noise models.
*
* Variables are eliminated in the natural order of the variable indices of in
* the factors.
* @param factors Factors to combine and eliminate
* @param nrFrontals Number of frontal variables to eliminate.
* @return The conditional and remaining factor
* \ingroup LinearSolving
*/
GaussianFactorGraph::EliminationResult EliminatePreferLDL(const FactorGraph<
GaussianFactor>& factors, size_t nrFrontals = 1);
/**
* Densely partially eliminate with LDL factorization. JacobianFactors
* are left-multiplied with their transpose to form the Hessian using the
* conversion constructor HessianFactor(const JacobianFactor&).
*
* If any factors contain constrained noise models, this function will fail
* because our current implementation cannot handle constrained noise models
* in LDL factorization. The function EliminatePreferLDL()
* automatically does QR instead when this is the case.
*
* Variables are eliminated in the natural order of the variable indices of in
* the factors.
* @param factors Factors to combine and eliminate
* @param nrFrontals Number of frontal variables to eliminate.
* @return The conditional and remaining factor
* \ingroup LinearSolving
*/
GaussianFactorGraph::EliminationResult EliminateLDL(const FactorGraph<
GaussianFactor>& factors, size_t nrFrontals = 1);
} // namespace gtsam } // namespace gtsam

12
gtsam/linear/GaussianMultifrontalSolver.cpp
View File

@ -46,7 +46,7 @@ GaussianBayesTree::shared_ptr GaussianMultifrontalSolver::eliminate() const {
if (useQR_) if (useQR_)
return Base::eliminate(&EliminateQR); return Base::eliminate(&EliminateQR);
else else
return Base::eliminate(&EliminatePreferLDL); return Base::eliminate(&EliminatePreferCholesky);
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -56,7 +56,7 @@ VectorValues::shared_ptr GaussianMultifrontalSolver::optimize() const {
if (useQR_) if (useQR_)
values = VectorValues::shared_ptr(new VectorValues(junctionTree_->optimize(&EliminateQR))); values = VectorValues::shared_ptr(new VectorValues(junctionTree_->optimize(&EliminateQR)));
else else
values= VectorValues::shared_ptr(new VectorValues(junctionTree_->optimize(&EliminatePreferLDL))); values= VectorValues::shared_ptr(new VectorValues(junctionTree_->optimize(&EliminatePreferCholesky)));
toc(2,"optimize"); toc(2,"optimize");
return values; return values;
} }
@ -66,7 +66,7 @@ GaussianFactorGraph::shared_ptr GaussianMultifrontalSolver::jointFactorGraph(con
if (useQR_) if (useQR_)
return GaussianFactorGraph::shared_ptr(new GaussianFactorGraph(*Base::jointFactorGraph(js,&EliminateQR))); return GaussianFactorGraph::shared_ptr(new GaussianFactorGraph(*Base::jointFactorGraph(js,&EliminateQR)));
else else
return GaussianFactorGraph::shared_ptr(new GaussianFactorGraph(*Base::jointFactorGraph(js,&EliminatePreferLDL))); return GaussianFactorGraph::shared_ptr(new GaussianFactorGraph(*Base::jointFactorGraph(js,&EliminatePreferCholesky)));
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -74,7 +74,7 @@ GaussianFactor::shared_ptr GaussianMultifrontalSolver::marginalFactor(Index j) c
if (useQR_) if (useQR_)
return Base::marginalFactor(j,&EliminateQR); return Base::marginalFactor(j,&EliminateQR);
else else
return Base::marginalFactor(j,&EliminatePreferLDL); return Base::marginalFactor(j,&EliminatePreferCholesky);
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -85,8 +85,8 @@ Matrix GaussianMultifrontalSolver::marginalCovariance(Index j) const {
fg.push_back(Base::marginalFactor(j,&EliminateQR)); fg.push_back(Base::marginalFactor(j,&EliminateQR));
conditional = EliminateQR(fg,1).first; conditional = EliminateQR(fg,1).first;
} else { } else {
fg.push_back(Base::marginalFactor(j,&EliminatePreferLDL)); fg.push_back(Base::marginalFactor(j,&EliminatePreferCholesky));
conditional = EliminatePreferLDL(fg,1).first; conditional = EliminatePreferCholesky(fg,1).first;
} }
return conditional->computeInformation().inverse(); return conditional->computeInformation().inverse();
} }

2
gtsam/linear/GaussianMultifrontalSolver.h
View File

@ -48,7 +48,7 @@ protected:
typedef GenericMultifrontalSolver<GaussianFactor, GaussianJunctionTree> Base; typedef GenericMultifrontalSolver<GaussianFactor, GaussianJunctionTree> Base;
typedef boost::shared_ptr<const GaussianMultifrontalSolver> shared_ptr; typedef boost::shared_ptr<const GaussianMultifrontalSolver> shared_ptr;
/** flag to determine whether to use LDL or QR */ /** flag to determine whether to use Cholesky or QR */
bool useQR_; bool useQR_;
public: public:

10
gtsam/linear/GaussianSequentialSolver.cpp
View File

@ -53,7 +53,7 @@ GaussianBayesNet::shared_ptr GaussianSequentialSolver::eliminate() const {
if (useQR_) if (useQR_)
return Base::eliminate(&EliminateQR); return Base::eliminate(&EliminateQR);
else else
return Base::eliminate(&EliminatePreferLDL); return Base::eliminate(&EliminatePreferCholesky);
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -90,7 +90,7 @@ GaussianFactor::shared_ptr GaussianSequentialSolver::marginalFactor(Index j) con
if (useQR_) if (useQR_)
return Base::marginalFactor(j,&EliminateQR); return Base::marginalFactor(j,&EliminateQR);
else else
return Base::marginalFactor(j,&EliminatePreferLDL); return Base::marginalFactor(j,&EliminatePreferCholesky);
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -101,8 +101,8 @@ Matrix GaussianSequentialSolver::marginalCovariance(Index j) const {
fg.push_back(Base::marginalFactor(j, &EliminateQR)); fg.push_back(Base::marginalFactor(j, &EliminateQR));
conditional = EliminateQR(fg, 1).first; conditional = EliminateQR(fg, 1).first;
} else { } else {
fg.push_back(Base::marginalFactor(j, &EliminatePreferLDL)); fg.push_back(Base::marginalFactor(j, &EliminatePreferCholesky));
conditional = EliminatePreferLDL(fg, 1).first; conditional = EliminatePreferCholesky(fg, 1).first;
} }
return conditional->computeInformation().inverse(); return conditional->computeInformation().inverse();
} }
@ -115,7 +115,7 @@ GaussianSequentialSolver::jointFactorGraph(const std::vector<Index>& js) const {
*Base::jointFactorGraph(js, &EliminateQR))); *Base::jointFactorGraph(js, &EliminateQR)));
else else
return GaussianFactorGraph::shared_ptr(new GaussianFactorGraph( return GaussianFactorGraph::shared_ptr(new GaussianFactorGraph(
*Base::jointFactorGraph(js, &EliminatePreferLDL))); *Base::jointFactorGraph(js, &EliminatePreferCholesky)));
} }
} /// namespace gtsam } /// namespace gtsam

2
gtsam/linear/GaussianSequentialSolver.h
View File

@ -54,7 +54,7 @@ protected:
typedef GenericSequentialSolver<GaussianFactor> Base; typedef GenericSequentialSolver<GaussianFactor> Base;
typedef boost::shared_ptr<const GaussianSequentialSolver> shared_ptr; typedef boost::shared_ptr<const GaussianSequentialSolver> shared_ptr;
/** flag to determine whether to use LDL or QR */ /** flag to determine whether to use Cholesky or QR */
bool useQR_; bool useQR_;
public: public:

19
gtsam/linear/HessianFactor.cpp
View File

@ -251,7 +251,7 @@ HessianFactor::HessianFactor(const FactorGraph<GaussianFactor>& factors,
const vector<size_t>& dimensions, const Scatter& scatter) : const vector<size_t>& dimensions, const Scatter& scatter) :
info_(matrix_) { info_(matrix_) {
const bool debug = ISDEBUG("EliminateCholesky") || ISDEBUG("EliminateLDL"); const bool debug = ISDEBUG("EliminateCholesky");
// Form Ab' * Ab // Form Ab' * Ab
tic(1, "allocate"); tic(1, "allocate");
info_.resize(dimensions.begin(), dimensions.end(), false); info_.resize(dimensions.begin(), dimensions.end(), false);
@ -472,19 +472,13 @@ void HessianFactor::partialCholesky(size_t nrFrontals) {
} }
/* ************************************************************************* */ /* ************************************************************************* */
Eigen::LDLT<Matrix>::TranspositionType HessianFactor::partialLDL(size_t nrFrontals) { GaussianConditional::shared_ptr HessianFactor::splitEliminatedFactor(size_t nrFrontals) {
return ldlPartial(matrix_, info_.offset(nrFrontals));
}
/* ************************************************************************* */
GaussianConditional::shared_ptr
HessianFactor::splitEliminatedFactor(size_t nrFrontals, const Eigen::LDLT<Matrix>::TranspositionType& permutation) {
static const bool debug = false; static const bool debug = false;
// Extract conditionals // Extract conditionals
tic(1, "extract conditionals"); tic(1, "extract conditionals");
GaussianConditional::shared_ptr conditionals(new GaussianConditional()); GaussianConditional::shared_ptr conditional(new GaussianConditional());
typedef VerticalBlockView<Matrix> BlockAb; typedef VerticalBlockView<Matrix> BlockAb;
BlockAb Ab(matrix_, info_); BlockAb Ab(matrix_, info_);
@ -492,12 +486,11 @@ HessianFactor::splitEliminatedFactor(size_t nrFrontals, const Eigen::LDLT<Matrix
Ab.rowEnd() = Ab.rowStart() + varDim; Ab.rowEnd() = Ab.rowStart() + varDim;
// Create one big conditionals with many frontal variables. // Create one big conditionals with many frontal variables.
// Because of the pivoting permutation when using LDL, treating each variable separately doesn't make sense.
tic(2, "construct cond"); tic(2, "construct cond");
Vector sigmas = Vector::Ones(varDim); Vector sigmas = Vector::Ones(varDim);
conditionals = boost::make_shared<ConditionalType>(keys_.begin(), keys_.end(), nrFrontals, Ab, sigmas, permutation); conditional = boost::make_shared<ConditionalType>(keys_.begin(), keys_.end(), nrFrontals, Ab, sigmas);
toc(2, "construct cond"); toc(2, "construct cond");
if(debug) conditionals->print("Extracted conditional: "); if(debug) conditional->print("Extracted conditional: ");
toc(1, "extract conditionals"); toc(1, "extract conditionals");
@ -510,7 +503,7 @@ HessianFactor::splitEliminatedFactor(size_t nrFrontals, const Eigen::LDLT<Matrix
keys_.swap(remainingKeys); keys_.swap(remainingKeys);
toc(2, "remaining factor"); toc(2, "remaining factor");
return conditionals; return conditional;
} }
} // gtsam } // gtsam

8
gtsam/linear/HessianFactor.h
View File

@ -288,14 +288,8 @@ namespace gtsam {
*/ */
void partialCholesky(size_t nrFrontals); void partialCholesky(size_t nrFrontals);
/**
* Do LDL.
*/
Eigen::LDLT<Matrix>::TranspositionType partialLDL(size_t nrFrontals);
/** split partially eliminated factor */ /** split partially eliminated factor */
boost::shared_ptr<GaussianConditional> splitEliminatedFactor( boost::shared_ptr<GaussianConditional> splitEliminatedFactor(size_t nrFrontals);
size_t nrFrontals, const Eigen::LDLT<Matrix>::TranspositionType& permutation = Eigen::LDLT<Matrix>::TranspositionType());
/** assert invariants */ /** assert invariants */
void assertInvariants() const; void assertInvariants() const;

1
gtsam/linear/JacobianFactor.cpp
View File

@ -152,7 +152,6 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
JacobianFactor::JacobianFactor(const GaussianConditional& cg) : GaussianFactor(cg), model_(noiseModel::Diagonal::Sigmas(cg.get_sigmas(), true)), Ab_(matrix_) { JacobianFactor::JacobianFactor(const GaussianConditional& cg) : GaussianFactor(cg), model_(noiseModel::Diagonal::Sigmas(cg.get_sigmas(), true)), Ab_(matrix_) {
Ab_.assignNoalias(cg.rsd_); Ab_.assignNoalias(cg.rsd_);
Ab_.range(0,cg.nrFrontals()) = Ab_.range(0,cg.nrFrontals()) * cg.permutation_.transpose();
firstNonzeroBlocks_.resize(cg.get_d().size(), 0); // set sigmas from precisions firstNonzeroBlocks_.resize(cg.get_d().size(), 0); // set sigmas from precisions
assertInvariants(); assertInvariants();
} }

2
gtsam/linear/KalmanFilter.cpp
View File

@ -13,7 +13,7 @@
* @file KalmanFilter.cpp * @file KalmanFilter.cpp
* *
* @brief Simple linear Kalman filter. * @brief Simple linear Kalman filter.
* Implemented using factor graphs, i.e., does LDL-based SRIF, really. * Implemented using factor graphs, i.e., does Cholesky-based SRIF, really.
* *
* @date Sep 3, 2011 * @date Sep 3, 2011
* @author Stephen Williams * @author Stephen Williams

4
gtsam/linear/KalmanFilter.h
View File

@ -13,7 +13,7 @@
* @file testKalmanFilter.cpp * @file testKalmanFilter.cpp
* *
* Simple linear Kalman filter. * Simple linear Kalman filter.
* Implemented using factor graphs, i.e., does LDL-based SRIF, really. * Implemented using factor graphs, i.e., does Cholesky-based SRIF, really.
* *
* @date Sep 3, 2011 * @date Sep 3, 2011
* @author Stephen Williams * @author Stephen Williams
@ -49,7 +49,7 @@ namespace gtsam {
* The type below allows you to specify the factorization variant. * The type below allows you to specify the factorization variant.
*/ */
enum Factorization { enum Factorization {
QR, LDL QR, CHOLESKY
}; };
/** /**

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

@ -191,7 +191,6 @@ TEST( GaussianConditional, solve )
/* ************************************************************************* */ /* ************************************************************************* */
TEST( GaussianConditional, solve_simple ) TEST( GaussianConditional, solve_simple )
{ {
// no pivoting from LDL, so R matrix is not permuted
Matrix full_matrix = Matrix_(4, 7, Matrix full_matrix = Matrix_(4, 7,
1.0, 0.0, 2.0, 0.0, 3.0, 0.0, 0.1, 1.0, 0.0, 2.0, 0.0, 3.0, 0.0, 0.1,
0.0, 1.0, 0.0, 2.0, 0.0, 3.0, 0.2, 0.0, 1.0, 0.0, 2.0, 0.0, 3.0, 0.2,
@ -231,7 +230,6 @@ TEST( GaussianConditional, solve_simple )
TEST( GaussianConditional, solve_multifrontal ) TEST( GaussianConditional, solve_multifrontal )
{ {
// create full system, 3 variables, 2 frontals, all 2 dim // create full system, 3 variables, 2 frontals, all 2 dim
// no pivoting from LDL, so R matrix is not permuted
Matrix full_matrix = Matrix_(4, 7, Matrix full_matrix = Matrix_(4, 7,
1.0, 0.0, 2.0, 0.0, 3.0, 0.0, 0.1, 1.0, 0.0, 2.0, 0.0, 3.0, 0.0, 0.1,
0.0, 1.0, 0.0, 2.0, 0.0, 3.0, 0.2, 0.0, 1.0, 0.0, 2.0, 0.0, 3.0, 0.2,
@ -273,7 +271,6 @@ TEST( GaussianConditional, solve_multifrontal )
TEST( GaussianConditional, solve_multifrontal_permuted ) TEST( GaussianConditional, solve_multifrontal_permuted )
{ {
// create full system, 3 variables, 2 frontals, all 2 dim // create full system, 3 variables, 2 frontals, all 2 dim
// no pivoting from LDL, so R matrix is not permuted
Matrix full_matrix = Matrix_(4, 7, Matrix full_matrix = Matrix_(4, 7,
1.0, 0.0, 2.0, 0.0, 3.0, 0.0, 0.1, 1.0, 0.0, 2.0, 0.0, 3.0, 0.0, 0.1,
0.0, 1.0, 0.0, 2.0, 0.0, 3.0, 0.2, 0.0, 1.0, 0.0, 2.0, 0.0, 3.0, 0.2,
@ -367,30 +364,11 @@ TEST( GaussianConditional, computeInformation ) {
0, 0, 8, 9, 0, 0, 8, 9,
0, 0, 0, 10).finished(); 0, 0, 0, 10).finished();
// Shuffle columns // Create conditional
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)); GaussianConditional conditional(0, Vector::Zero(4), R, Vector::Constant(4, 1.0));
conditional.permutation_ = p;
// Expected information matrix (using permuted R) // Expected information matrix (using permuted R)
Matrix IExpected = RpExpected.transpose() * RpExpected; Matrix IExpected = R.transpose() * R;
// Actual information matrix (conditional should permute R) // Actual information matrix (conditional should permute R)
Matrix IActual = conditional.computeInformation(); Matrix IActual = conditional.computeInformation();
@ -407,23 +385,8 @@ TEST( GaussianConditional, isGaussianFactor ) {
0, 0, 8, 9, 0, 0, 8, 9,
0, 0, 0, 10).finished(); 0, 0, 0, 10).finished();
// Shuffle columns // Create a conditional
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)); GaussianConditional conditional(0, Vector::Zero(4), R, Vector::Constant(4, 1.0));
conditional.permutation_ = p;
// Expected information matrix computed by conditional // Expected information matrix computed by conditional
Matrix IExpected = conditional.computeInformation(); Matrix IExpected = conditional.computeInformation();

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

@ -104,7 +104,7 @@ TEST( GaussianJunctionTree, eliminate )
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST_UNSAFE( GaussianJunctionTree, GBNConstructor ) TEST( GaussianJunctionTree, GBNConstructor )
{ {
GaussianFactorGraph fg = createChain(); GaussianFactorGraph fg = createChain();
GaussianJunctionTree jt(fg); GaussianJunctionTree jt(fg);
@ -167,6 +167,25 @@ TEST(GaussianJunctionTree, complicatedMarginal) {
0.7943, 0.1656, 0.7943, 0.1656,
0.3112, 0.6020).finished()), 0.3112, 0.6020).finished()),
2, (Vector(3) << 0.9619, 0.0046, 0.7749).finished(), ones(3))); 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.4733, 0.1966,
// 0.9022, 0.0979,
// 0.0, 0.2312).finished()) // Attempted to recreate without permutation
// (7, (Matrix(3,1) <<
// 0.5853,
// 1.0589,
// 0.1487).finished())
// (8, (Matrix(3,2) <<
// 0.2858, 0.3804,
// 0.9893, 0.2912,
// 0.4035, 0.4933).finished()),
// 2, (Vector(3) << 0.8173, 0.4164, 0.7671).finished(), ones(3)));
GaussianConditional::shared_ptr R_5_6(new GaussianConditional( GaussianConditional::shared_ptr R_5_6(new GaussianConditional(
pair_list_of pair_list_of
(5, (Matrix(3,1) << (5, (Matrix(3,1) <<
@ -174,9 +193,13 @@ TEST(GaussianJunctionTree, complicatedMarginal) {
0, 0,
0).finished()) 0).finished())
(6, (Matrix(3,2) << (6, (Matrix(3,2) <<
0.1966, 0.4733, 0.4733, 0.1966,
0.2511, 0.3517, 0.3517, 0.2511,
0, 0.8308).finished()) 0.8308, 0.0).finished()) // NOTE the non-upper-triangular form
// here since this test was written when we had column permutations
// from LDL. The code still works currently (does not enfore
// upper-triangularity in this case) but this test will need to be
// redone if this stops working in the future
(7, (Matrix(3,1) << (7, (Matrix(3,1) <<
0.5853, 0.5853,
0.5497, 0.5497,
@ -186,9 +209,6 @@ TEST(GaussianJunctionTree, complicatedMarginal) {
0.7572, 0.5678, 0.7572, 0.5678,
0.7537, 0.0759).finished()), 0.7537, 0.0759).finished()),
2, (Vector(3) << 0.8173, 0.8687, 0.0844).finished(), ones(3))); 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( GaussianConditional::shared_ptr R_7_8(new GaussianConditional(
pair_list_of pair_list_of
(7, (Matrix(3,1) << (7, (Matrix(3,1) <<
@ -251,15 +271,15 @@ TEST(GaussianJunctionTree, complicatedMarginal) {
// Marginal on 5 // Marginal on 5
Matrix expectedCov = (Matrix(1,1) << 236.5166).finished(); Matrix expectedCov = (Matrix(1,1) << 236.5166).finished();
JacobianFactor::shared_ptr actualJacobianLDL = boost::dynamic_pointer_cast<JacobianFactor>( JacobianFactor::shared_ptr actualJacobianChol= boost::dynamic_pointer_cast<JacobianFactor>(
bt.marginalFactor(5, EliminateLDL)); bt.marginalFactor(5, EliminateCholesky));
JacobianFactor::shared_ptr actualJacobianQR = boost::dynamic_pointer_cast<JacobianFactor>( JacobianFactor::shared_ptr actualJacobianQR = boost::dynamic_pointer_cast<JacobianFactor>(
bt.marginalFactor(5, EliminateQR)); bt.marginalFactor(5, EliminateQR));
CHECK(assert_equal(*actualJacobianLDL, *actualJacobianQR)); // Check that LDL and QR obtained marginals are the same CHECK(assert_equal(*actualJacobianChol, *actualJacobianQR)); // Check that Chol and QR obtained marginals are the same
LONGS_EQUAL(1, actualJacobianLDL->rows()); LONGS_EQUAL(1, actualJacobianChol->rows());
LONGS_EQUAL(1, actualJacobianLDL->size()); LONGS_EQUAL(1, actualJacobianChol->size());
LONGS_EQUAL(5, actualJacobianLDL->keys()[0]); LONGS_EQUAL(5, actualJacobianChol->keys()[0]);
Matrix actualA = actualJacobianLDL->getA(actualJacobianLDL->begin()); Matrix actualA = actualJacobianChol->getA(actualJacobianChol->begin());
Matrix actualCov = inverse(actualA.transpose() * actualA); Matrix actualCov = inverse(actualA.transpose() * actualA);
EXPECT(assert_equal(expectedCov, actualCov, 1e-1)); EXPECT(assert_equal(expectedCov, actualCov, 1e-1));
@ -270,15 +290,15 @@ TEST(GaussianJunctionTree, complicatedMarginal) {
expectedCov = (Matrix(2,2) << expectedCov = (Matrix(2,2) <<
1015.8, 2886.2, 1015.8, 2886.2,
2886.2, 8471.2).finished(); 2886.2, 8471.2).finished();
actualJacobianLDL = boost::dynamic_pointer_cast<JacobianFactor>( actualJacobianChol = boost::dynamic_pointer_cast<JacobianFactor>(
bt.marginalFactor(6, EliminateLDL)); bt.marginalFactor(6, EliminateCholesky));
actualJacobianQR = boost::dynamic_pointer_cast<JacobianFactor>( actualJacobianQR = boost::dynamic_pointer_cast<JacobianFactor>(
bt.marginalFactor(6, EliminateQR)); bt.marginalFactor(6, EliminateQR));
CHECK(assert_equal(*actualJacobianLDL, *actualJacobianQR)); // Check that LDL and QR obtained marginals are the same CHECK(assert_equal(*actualJacobianChol, *actualJacobianQR)); // Check that Chol and QR obtained marginals are the same
LONGS_EQUAL(2, actualJacobianLDL->rows()); LONGS_EQUAL(2, actualJacobianChol->rows());
LONGS_EQUAL(1, actualJacobianLDL->size()); LONGS_EQUAL(1, actualJacobianChol->size());
LONGS_EQUAL(6, actualJacobianLDL->keys()[0]); LONGS_EQUAL(6, actualJacobianChol->keys()[0]);
actualA = actualJacobianLDL->getA(actualJacobianLDL->begin()); actualA = actualJacobianChol->getA(actualJacobianChol->begin());
actualCov = inverse(actualA.transpose() * actualA); actualCov = inverse(actualA.transpose() * actualA);
EXPECT(assert_equal(expectedCov, actualCov, 1e1)); EXPECT(assert_equal(expectedCov, actualCov, 1e1));

14
gtsam/linear/tests/testHessianFactor.cpp
View File

@ -414,7 +414,7 @@ TEST_UNSAFE(HessianFactor, CombineAndEliminate)
// create expected Hessian after elimination // create expected Hessian after elimination
HessianFactor expectedCholeskyFactor(expectedFactor); HessianFactor expectedCholeskyFactor(expectedFactor);
GaussianFactorGraph::EliminationResult actualCholesky = EliminateLDL( GaussianFactorGraph::EliminationResult actualCholesky = EliminateCholesky(
*gfg.convertCastFactors<FactorGraph<HessianFactor> > (), 1); *gfg.convertCastFactors<FactorGraph<HessianFactor> > (), 1);
HessianFactor::shared_ptr actualFactor = boost::dynamic_pointer_cast< HessianFactor::shared_ptr actualFactor = boost::dynamic_pointer_cast<
HessianFactor>(actualCholesky.second); HessianFactor>(actualCholesky.second);
@ -465,7 +465,7 @@ TEST(HessianFactor, eliminate2 )
FactorGraph<HessianFactor> combinedLFG_Chol; FactorGraph<HessianFactor> combinedLFG_Chol;
combinedLFG_Chol.push_back(combinedLF_Chol); combinedLFG_Chol.push_back(combinedLF_Chol);
GaussianFactorGraph::EliminationResult actual_Chol = EliminateLDL( GaussianFactorGraph::EliminationResult actual_Chol = EliminateCholesky(
combinedLFG_Chol, 1); combinedLFG_Chol, 1);
HessianFactor::shared_ptr actualFactor = boost::dynamic_pointer_cast< HessianFactor::shared_ptr actualFactor = boost::dynamic_pointer_cast<
HessianFactor>(actual_Chol.second); HessianFactor>(actual_Chol.second);
@ -576,16 +576,6 @@ TEST(HessianFactor, eliminateUnsorted) {
EXPECT(assert_equal(*expected_bn, *actual_bn, 1e-10)); EXPECT(assert_equal(*expected_bn, *actual_bn, 1e-10));
EXPECT(assert_equal(*expected_factor, *actual_factor, 1e-10)); EXPECT(assert_equal(*expected_factor, *actual_factor, 1e-10));
// Test LDL
boost::tie(expected_bn, expected_factor) =
EliminatePreferLDL(sortedGraph, 1);
boost::tie(actual_bn, actual_factor) =
EliminatePreferLDL(unsortedGraph, 1);
EXPECT(assert_equal(*expected_bn, *actual_bn, 1e-10));
EXPECT(assert_equal(*expected_factor, *actual_factor, 1e-10));
} }
/* ************************************************************************* */ /* ************************************************************************* */

6
gtsam/linear/tests/testKalmanFilter.cpp
View File

@ -165,7 +165,7 @@ TEST( KalmanFilter, predict ) {
} }
/* ************************************************************************* */ /* ************************************************************************* */
// Test both QR and LDL versions in case of a realistic (AHRS) dynamics update // Test both QR and Cholesky versions in case of a realistic (AHRS) dynamics update
TEST( KalmanFilter, QRvsCholesky ) { TEST( KalmanFilter, QRvsCholesky ) {
Vector mean = ones(9); Vector mean = ones(9);
@ -180,8 +180,8 @@ TEST( KalmanFilter, QRvsCholesky ) {
63.8, -0.0, 0.1, 0.0, 0.0, 0.0, 0.0, 625.0, 0.0, 63.8, -0.0, 0.1, 0.0, 0.0, 0.0, 0.0, 625.0, 0.0,
-0.6, -0.1, -0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 625.0); -0.6, -0.1, -0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 625.0);
// Create two Kalman filter of dimension 9, one using QR the other LDL // Create two Kalman filter of dimension 9, one using QR the other Cholesky
KalmanFilter kfa(9, KalmanFilter::QR), kfb(9, KalmanFilter::LDL); KalmanFilter kfa(9, KalmanFilter::QR), kfb(9, KalmanFilter::CHOLESKY);
// create corresponding initial states // create corresponding initial states
KalmanFilter::State p0a = kfa.init(mean, covariance); KalmanFilter::State p0a = kfa.init(mean, covariance);

2
gtsam/nonlinear/ExtendedKalmanFilter.h
View File

@ -26,7 +26,7 @@ namespace gtsam {
/** /**
* This is a generic Extended Kalman Filter class implemented using nonlinear factors. GTSAM * This is a generic Extended Kalman Filter class implemented using nonlinear factors. GTSAM
* basically does SRIF with LDL to solve the filter problem, making this an efficient, numerically * basically does SRIF with Cholesky to solve the filter problem, making this an efficient, numerically
* stable Kalman Filter implementation. * stable Kalman Filter implementation.
* *
* The Kalman Filter relies on two models: a motion model that predicts the next state using * The Kalman Filter relies on two models: a motion model that predicts the next state using

4
gtsam/nonlinear/ISAM2-impl.cpp
View File

@ -246,7 +246,7 @@ ISAM2::Impl::PartialSolve(GaussianFactorGraph& factors,
tic(7,"eliminate"); tic(7,"eliminate");
JunctionTree<GaussianFactorGraph, ISAM2::Clique> jt(factors, affectedFactorsIndex); JunctionTree<GaussianFactorGraph, ISAM2::Clique> jt(factors, affectedFactorsIndex);
if(!useQR) if(!useQR)
result.bayesTree = jt.eliminate(EliminatePreferLDL); result.bayesTree = jt.eliminate(EliminatePreferCholesky);
else else
result.bayesTree = jt.eliminate(EliminateQR); result.bayesTree = jt.eliminate(EliminateQR);
toc(7,"eliminate"); toc(7,"eliminate");
@ -338,7 +338,7 @@ void updateDoglegDeltas(const boost::shared_ptr<ISAM2Clique>& clique, std::vecto
Vector gS = internal::extractVectorValuesSlices(grad, (*clique)->beginParents(), (*clique)->endParents()); Vector gS = internal::extractVectorValuesSlices(grad, (*clique)->beginParents(), (*clique)->endParents());
// Compute R*g and S*g for this clique // Compute R*g and S*g for this clique
Vector RSgProd = ((*clique)->get_R() * (*clique)->permutation().transpose()) * gR + (*clique)->get_S() * gS; Vector RSgProd = (*clique)->get_R() * gR + (*clique)->get_S() * gS;
// Write into RgProd vector // Write into RgProd vector
internal::writeVectorValuesSlices(RSgProd, RgProd, (*clique)->beginFrontals(), (*clique)->endFrontals()); internal::writeVectorValuesSlices(RSgProd, RgProd, (*clique)->beginFrontals(), (*clique)->endFrontals());

2
gtsam/nonlinear/ISAM2-impl.h
View File

@ -118,7 +118,7 @@ struct ISAM2::Impl {
* problem. This is permuted during use and so is cleared when this function * problem. This is permuted during use and so is cleared when this function
* returns in order to invalidate it. * returns in order to invalidate it.
* \param keys The set of indices used in \c factors. * \param keys The set of indices used in \c factors.
* \param useQR Whether to use QR (if true), or LDL (if false). * \param useQR Whether to use QR (if true), or Cholesky (if false).
* \return The eliminated BayesTree and the permutation to be applied to the * \return The eliminated BayesTree and the permutation to be applied to the
* rest of the ISAM2 data. * rest of the ISAM2 data.
*/ */

4
gtsam/nonlinear/ISAM2.cpp
View File

@ -326,8 +326,8 @@ boost::shared_ptr<FastSet<Index> > ISAM2::recalculate(
tic(5,"eliminate"); tic(5,"eliminate");
JunctionTree<GaussianFactorGraph, Base::Clique> jt(linearFactors_, variableIndex_); JunctionTree<GaussianFactorGraph, Base::Clique> jt(linearFactors_, variableIndex_);
sharedClique newRoot; sharedClique newRoot;
if(params_.factorization == ISAM2Params::LDL) if(params_.factorization == ISAM2Params::CHOLESKY)
newRoot = jt.eliminate(EliminatePreferLDL); newRoot = jt.eliminate(EliminatePreferCholesky);
else if(params_.factorization == ISAM2Params::QR) else if(params_.factorization == ISAM2Params::QR)
newRoot = jt.eliminate(EliminateQR); newRoot = jt.eliminate(EliminateQR);
else assert(false); else assert(false);

14
gtsam/nonlinear/ISAM2.h
View File

@ -107,14 +107,14 @@ struct ISAM2Params {
bool evaluateNonlinearError; ///< Whether to evaluate the nonlinear error before and after the update, to return in ISAM2Result from update() bool evaluateNonlinearError; ///< Whether to evaluate the nonlinear error before and after the update, to return in ISAM2Result from update()
enum Factorization { LDL, QR }; enum Factorization { CHOLESKY, QR };
/** Specifies whether to use QR or LDL numerical factorization (default: LDL). /** Specifies whether to use QR or CHOESKY numerical factorization (default: CHOLESKY).
* LDL is faster but potentially numerically unstable for poorly-conditioned problems, which can occur when * Cholesky is faster but potentially numerically unstable for poorly-conditioned problems, which can occur when
* uncertainty is very low in some variables (or dimensions of variables) and very high in others. QR is * uncertainty is very low in some variables (or dimensions of variables) and very high in others. QR is
* slower but more numerically stable in poorly-conditioned problems. We suggest using the default of LDL * slower but more numerically stable in poorly-conditioned problems. We suggest using the default of Cholesky
* unless gtsam sometimes throws NegativeMatrixException when your problem's Hessian is actually positive * unless gtsam sometimes throws NegativeMatrixException when your problem's Hessian is actually positive
* definite. For positive definite problems, numerical error accumulation can cause the problem to become * definite. For positive definite problems, numerical error accumulation can cause the problem to become
* numerically negative or indefinite as solving proceeds, especially when using LDL. * numerically negative or indefinite as solving proceeds, especially when using Cholesky.
*/ */
Factorization factorization; Factorization factorization;
@ -136,7 +136,7 @@ struct ISAM2Params {
int _relinearizeSkip = 10, ///< see ISAM2Params::relinearizeSkip int _relinearizeSkip = 10, ///< see ISAM2Params::relinearizeSkip
bool _enableRelinearization = true, ///< see ISAM2Params::enableRelinearization bool _enableRelinearization = true, ///< see ISAM2Params::enableRelinearization
bool _evaluateNonlinearError = false, ///< see ISAM2Params::evaluateNonlinearError bool _evaluateNonlinearError = false, ///< see ISAM2Params::evaluateNonlinearError
Factorization _factorization = ISAM2Params::LDL, ///< see ISAM2Params::factorization Factorization _factorization = ISAM2Params::CHOLESKY, ///< see ISAM2Params::factorization
bool _cacheLinearizedFactors = true, ///< see ISAM2Params::cacheLinearizedFactors bool _cacheLinearizedFactors = true, ///< see ISAM2Params::cacheLinearizedFactors
const KeyFormatter& _keyFormatter = DefaultKeyFormatter ///< see ISAM2::Params::keyFormatter const KeyFormatter& _keyFormatter = DefaultKeyFormatter ///< see ISAM2::Params::keyFormatter
) : optimizationParams(_optimizationParams), relinearizeThreshold(_relinearizeThreshold), ) : optimizationParams(_optimizationParams), relinearizeThreshold(_relinearizeThreshold),
@ -258,7 +258,7 @@ struct ISAM2Clique : public BayesTreeCliqueBase<ISAM2Clique, GaussianConditional
// Compute gradient contribution // Compute gradient contribution
const ConditionalType& conditional(*result.first); const ConditionalType& conditional(*result.first);
// Rewrite -(R * P')'*d as -(d' * R * P')' for computational speed reasons // Rewrite -(R * P')'*d as -(d' * R * P')' for computational speed reasons
gradientContribution_ << -(conditional.get_d().transpose() * conditional.get_R() * conditional.permutation().transpose()).transpose(), gradientContribution_ << -conditional.get_R().transpose() * conditional.get_d(),
-conditional.get_S().transpose() * conditional.get_d(); -conditional.get_S().transpose() * conditional.get_d();
} }

9
gtsam/nonlinear/SuccessiveLinearizationOptimizer.h
View File

@ -33,16 +33,15 @@ public:
/** See SuccessiveLinearizationParams::factorization */ /** See SuccessiveLinearizationParams::factorization */
enum Factorization { enum Factorization {
CHOLESKY, CHOLESKY,
LDL,
QR, QR,
}; };
Elimination elimination; ///< The elimination algorithm to use (default: MULTIFRONTAL) Elimination elimination; ///< The elimination algorithm to use (default: MULTIFRONTAL)
Factorization factorization; ///< The numerical factorization (default: LDL) Factorization factorization; ///< The numerical factorization (default: Cholesky)
boost::optional<Ordering> ordering; ///< The variable elimination ordering, or empty to use COLAMD (default: empty) boost::optional<Ordering> ordering; ///< The variable elimination ordering, or empty to use COLAMD (default: empty)
SuccessiveLinearizationParams() : SuccessiveLinearizationParams() :
elimination(MULTIFRONTAL), factorization(LDL) {} elimination(MULTIFRONTAL), factorization(CHOLESKY) {}
virtual ~SuccessiveLinearizationParams() {} virtual ~SuccessiveLinearizationParams() {}
@ -57,8 +56,6 @@ public:
if(factorization == CHOLESKY) if(factorization == CHOLESKY)
std::cout << " factorization method: CHOLESKY\n"; std::cout << " factorization method: CHOLESKY\n";
else if(factorization == LDL)
std::cout << " factorization method: LDL\n";
else if(factorization == QR) else if(factorization == QR)
std::cout << " factorization method: QR\n"; std::cout << " factorization method: QR\n";
else else
@ -75,8 +72,6 @@ public:
GaussianFactorGraph::Eliminate getEliminationFunction() const { GaussianFactorGraph::Eliminate getEliminationFunction() const {
if(factorization == SuccessiveLinearizationParams::CHOLESKY) if(factorization == SuccessiveLinearizationParams::CHOLESKY)
return EliminatePreferCholesky; return EliminatePreferCholesky;
else if(factorization == SuccessiveLinearizationParams::LDL)
return EliminatePreferLDL;
else if(factorization == SuccessiveLinearizationParams::QR) else if(factorization == SuccessiveLinearizationParams::QR)
return EliminateQR; return EliminateQR;
else else

2
tests/testGaussianFactorGraphB.cpp
View File

@ -476,7 +476,7 @@ TEST( GaussianFactorGraph, getOrdering)
//} //}
/* ************************************************************************* */ /* ************************************************************************* */
TEST( GaussianFactorGraph, optimize_LDL ) TEST( GaussianFactorGraph, optimize_Cholesky )
{ {
// create an ordering // create an ordering
Ordering ord; ord += kx(2),kl(1),kx(1); Ordering ord; ord += kx(2),kl(1),kx(1);

4
tests/testGaussianJunctionTreeB.cpp
View File

@ -219,8 +219,8 @@ TEST(GaussianJunctionTree, simpleMarginal) {
// Compute marginal directly from BayesTree // Compute marginal directly from BayesTree
GaussianBayesTree gbt; GaussianBayesTree gbt;
gbt.insert(GaussianJunctionTree(gfg).eliminate(EliminateLDL)); gbt.insert(GaussianJunctionTree(gfg).eliminate(EliminateCholesky));
marginalFactor = gbt.marginalFactor(1, EliminateLDL); marginalFactor = gbt.marginalFactor(1, EliminateCholesky);
marginalJacobian = boost::dynamic_pointer_cast<JacobianFactor>(marginalFactor); marginalJacobian = boost::dynamic_pointer_cast<JacobianFactor>(marginalFactor);
Matrix actual3 = inverse(marginalJacobian->getA(marginalJacobian->begin()).transpose() * marginalJacobian->getA(marginalJacobian->begin())); Matrix actual3 = inverse(marginalJacobian->getA(marginalJacobian->begin()).transpose() * marginalJacobian->getA(marginalJacobian->begin()));

7
tests/testNonlinearOptimizer.cpp
View File

@ -151,10 +151,8 @@ TEST( NonlinearOptimizer, optimization_method )
{ {
LevenbergMarquardtParams paramsQR; LevenbergMarquardtParams paramsQR;
paramsQR.factorization = LevenbergMarquardtParams::QR; paramsQR.factorization = LevenbergMarquardtParams::QR;
LevenbergMarquardtParams paramsLDL;
paramsLDL.factorization = LevenbergMarquardtParams::LDL;
LevenbergMarquardtParams paramsChol; LevenbergMarquardtParams paramsChol;
paramsLDL.factorization = LevenbergMarquardtParams::CHOLESKY; paramsChol.factorization = LevenbergMarquardtParams::CHOLESKY;
example::Graph fg = example::createReallyNonlinearFactorGraph(); example::Graph fg = example::createReallyNonlinearFactorGraph();
@ -165,9 +163,6 @@ TEST( NonlinearOptimizer, optimization_method )
Values actualMFQR = LevenbergMarquardtOptimizer(fg, c0, paramsQR).optimize(); Values actualMFQR = LevenbergMarquardtOptimizer(fg, c0, paramsQR).optimize();
DOUBLES_EQUAL(0,fg.error(actualMFQR),tol); DOUBLES_EQUAL(0,fg.error(actualMFQR),tol);
Values actualMFLDL = LevenbergMarquardtOptimizer(fg, c0, paramsLDL).optimize();
DOUBLES_EQUAL(0,fg.error(actualMFLDL),tol);
Values actualMFChol = LevenbergMarquardtOptimizer(fg, c0, paramsChol).optimize(); Values actualMFChol = LevenbergMarquardtOptimizer(fg, c0, paramsChol).optimize();
DOUBLES_EQUAL(0,fg.error(actualMFChol),tol); DOUBLES_EQUAL(0,fg.error(actualMFChol),tol);
} }