Standardized and corrected error handling on the linear size - underconstrained and negative systems always throw IndeterminantLinearSystemException, and all assertions checking for infinite values are removed. Also, we were not properly checking the result of Eigen's Cholesky, so sometimes elimination continued with incorrect matrices despite being underconstrained when Cholesky failed but did not produce NaN's.
Richard Roberts
parent
cf4a4b4285
commit
1dbda3f7ed
|
|
@ -24,6 +24,7 @@
|
|||
#include <gtsam/3rdparty/Eigen/Eigen/Dense>
|
||||
|
||||
#include <boost/format.hpp>
|
||||
#include <cmath>
|
||||
|
||||
using namespace std;
|
||||
|
||||
|
|
@ -32,18 +33,23 @@ namespace gtsam {
|
|||
static const double negativePivotThreshold = -1e-1;
|
||||
static const double zeroPivotThreshold = 1e-6;
|
||||
static const double underconstrainedPrior = 1e-5;
|
||||
static const int underconstrainedExponentDifference = 12;
|
||||
|
||||
/* ************************************************************************* */
|
||||
static inline bool choleskyStep(Matrix& ATA, size_t k, size_t order) {
|
||||
static inline int choleskyStep(Matrix& ATA, size_t k, size_t order) {
|
||||
|
||||
const bool debug = ISDEBUG("choleskyCareful");
|
||||
|
||||
// Get pivot value
|
||||
double alpha = ATA(k,k);
|
||||
|
||||
// Correct negative pivots from round-off error
|
||||
if(alpha < negativePivotThreshold) {
|
||||
cout << "pivot = " << alpha << endl;
|
||||
print(ATA, "Partially-factorized matrix: ");
|
||||
throw(CarefulCholeskyNegativeMatrixException());
|
||||
if(debug) {
|
||||
cout << "pivot = " << alpha << endl;
|
||||
print(ATA, "Partially-factorized matrix: ");
|
||||
}
|
||||
return -1;
|
||||
} else if(alpha < 0.0)
|
||||
alpha = 0.0;
|
||||
|
||||
|
|
@ -66,14 +72,14 @@ static inline bool choleskyStep(Matrix& ATA, size_t k, size_t order) {
|
|||
// ATA.bottomRightCorner(order-(k+1), order-(k+1)).selfadjointView<Eigen::Upper>()
|
||||
// .rankUpdate(V.adjoint(), -1);
|
||||
}
|
||||
|
||||
return true;
|
||||
return 1;
|
||||
} else {
|
||||
// For zero pivots, add the underconstrained variable prior
|
||||
ATA(k,k) = underconstrainedPrior;
|
||||
for(size_t j=k+1; j<order; ++j)
|
||||
ATA(k,j) = 0.0;
|
||||
return false;
|
||||
if(debug) cout << "choleskyCareful: Skipping " << k << endl;
|
||||
return 0;
|
||||
}
|
||||
}
|
||||
|
||||
|
|
@ -90,31 +96,32 @@ pair<size_t,bool> choleskyCareful(Matrix& ATA, int order) {
|
|||
|
||||
// Negative order means factor the entire matrix
|
||||
if(order < 0)
|
||||
order = n;
|
||||
order = int(n);
|
||||
|
||||
assert(size_t(order) <= n);
|
||||
|
||||
// The index of the row after the last non-zero row of the square-root factor
|
||||
size_t maxrank = 0;
|
||||
bool fullRank = true;
|
||||
bool success = true;
|
||||
|
||||
// Factor row-by-row
|
||||
for(size_t k = 0; k < size_t(order); ++k) {
|
||||
if(choleskyStep(ATA, k, size_t(order))) {
|
||||
int stepResult = choleskyStep(ATA, k, size_t(order));
|
||||
if(stepResult == 1) {
|
||||
if(debug) cout << "choleskyCareful: Factored through " << k << endl;
|
||||
if(debug) print(ATA, "ATA: ");
|
||||
maxrank = k+1;
|
||||
} else {
|
||||
fullRank = false;
|
||||
if(debug) cout << "choleskyCareful: Skipping " << k << endl;
|
||||
}
|
||||
} else if(stepResult == -1) {
|
||||
success = false;
|
||||
break;
|
||||
} /* else if(stepResult == 0) Found zero pivot */
|
||||
}
|
||||
|
||||
return make_pair(maxrank, fullRank);
|
||||
return make_pair(maxrank, success);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
void choleskyPartial(Matrix& ABC, size_t nFrontal) {
|
||||
bool choleskyPartial(Matrix& ABC, size_t nFrontal) {
|
||||
|
||||
const bool debug = ISDEBUG("choleskyPartial");
|
||||
|
||||
|
|
@ -125,9 +132,8 @@ void choleskyPartial(Matrix& ABC, size_t nFrontal) {
|
|||
|
||||
// Compute Cholesky factorization of A, overwrites A.
|
||||
tic(1, "lld");
|
||||
ABC.block(0,0,nFrontal,nFrontal).triangularView<Eigen::Upper>() =
|
||||
ABC.block(0,0,nFrontal,nFrontal).selfadjointView<Eigen::Upper>().llt().matrixU();
|
||||
assert(ABC.topLeftCorner(nFrontal,nFrontal).triangularView<Eigen::Upper>().toDenseMatrix().unaryExpr(ptr_fun(isfinite<double>)).all());
|
||||
Eigen::LLT<Matrix, Eigen::Upper> llt = ABC.block(0,0,nFrontal,nFrontal).selfadjointView<Eigen::Upper>().llt();
|
||||
ABC.block(0,0,nFrontal,nFrontal).triangularView<Eigen::Upper>() = llt.matrixU();
|
||||
toc(1, "lld");
|
||||
|
||||
if(debug) cout << "R:\n" << Eigen::MatrixXd(ABC.topLeftCorner(nFrontal,nFrontal).triangularView<Eigen::Upper>()) << endl;
|
||||
|
|
@ -138,7 +144,6 @@ void choleskyPartial(Matrix& ABC, size_t nFrontal) {
|
|||
ABC.topLeftCorner(nFrontal,nFrontal).triangularView<Eigen::Upper>().transpose().solveInPlace(
|
||||
ABC.topRightCorner(nFrontal, n-nFrontal));
|
||||
}
|
||||
assert(ABC.topRightCorner(nFrontal, n-nFrontal).unaryExpr(ptr_fun(isfinite<double>)).all());
|
||||
if(debug) cout << "S:\n" << ABC.topRightCorner(nFrontal, n-nFrontal) << endl;
|
||||
toc(2, "compute S");
|
||||
|
||||
|
|
@ -148,9 +153,27 @@ void choleskyPartial(Matrix& ABC, size_t nFrontal) {
|
|||
if(n - nFrontal > 0)
|
||||
ABC.bottomRightCorner(n-nFrontal,n-nFrontal).selfadjointView<Eigen::Upper>().rankUpdate(
|
||||
ABC.topRightCorner(nFrontal, n-nFrontal).transpose(), -1.0);
|
||||
assert(ABC.bottomRightCorner(n-nFrontal,n-nFrontal).selfadjointView<Eigen::Upper>().toDenseMatrix().unaryExpr(ptr_fun(isfinite<double>)).all());
|
||||
if(debug) cout << "L:\n" << Eigen::MatrixXd(ABC.bottomRightCorner(n-nFrontal,n-nFrontal).selfadjointView<Eigen::Upper>()) << endl;
|
||||
toc(3, "compute L");
|
||||
|
||||
// Check last diagonal element - Eigen does not check it
|
||||
bool ok;
|
||||
if(llt.info() == Eigen::Success) {
|
||||
if(nFrontal >= 2) {
|
||||
int exp2, exp1;
|
||||
(void)frexp(ABC(nFrontal-2, nFrontal-2), &exp2);
|
||||
(void)frexp(ABC(nFrontal-1, nFrontal-1), &exp1);
|
||||
ok = (exp2 - exp1 < underconstrainedExponentDifference);
|
||||
} else if(nFrontal == 1) {
|
||||
int exp1;
|
||||
(void)frexp(ABC(0,0), &exp1);
|
||||
ok = (exp1 > -underconstrainedExponentDifference);
|
||||
}
|
||||
} else {
|
||||
ok = false;
|
||||
}
|
||||
|
||||
return ok;
|
||||
}
|
||||
|
||||
}
|
||||
|
|
|
|||
|
|
@ -22,45 +22,6 @@
|
|||
|
||||
namespace gtsam {
|
||||
|
||||
/**
|
||||
* An exception indicating an attempt to factor a negative or indefinite matrix.
|
||||
* If detailed exceptions are enabled, then the \c detail member will contain
|
||||
* the matrices leading to the problem, see documentation for
|
||||
* NegativeMatrixException::Detail.
|
||||
*/
|
||||
struct NegativeMatrixException : public std::exception {
|
||||
/// Detail for NegativeMatrixException
|
||||
struct Detail {
|
||||
Matrix A; ///< The original matrix attempted to factor
|
||||
Matrix U; ///< The produced upper-triangular factor
|
||||
Matrix D; ///< The produced diagonal factor
|
||||
Detail(const Matrix& A, const Matrix& U, const Matrix& D) /**< Detail constructor */ : A(A), U(U), D(D) {}
|
||||
void print(const std::string& str = "") const {
|
||||
std::cout << str << "\n";
|
||||
gtsam::print(A, " A: ");
|
||||
gtsam::print(U, " U: ");
|
||||
gtsam::print(D, " D: ");
|
||||
}
|
||||
};
|
||||
const boost::shared_ptr<const Detail> detail; ///< Detail
|
||||
NegativeMatrixException() /**< Constructor with no detail */ {}
|
||||
NegativeMatrixException(const Detail& _detail) /**< Constructor with detail */ : detail(new Detail(_detail)) {}
|
||||
NegativeMatrixException(const boost::shared_ptr<Detail>& _detail) /**< Constructor with detail */ : detail(_detail) {}
|
||||
virtual ~NegativeMatrixException() throw() {}
|
||||
};
|
||||
|
||||
/**
|
||||
* An exception indicating an attempt to factor a negative or indefinite matrix.
|
||||
* If detailed exceptions are enabled, then the \c detail member will contain
|
||||
* the matrices leading to the problem, see documentation for
|
||||
* CarefulCholeskyNegativeMatrixException::Detail.
|
||||
*/
|
||||
struct CarefulCholeskyNegativeMatrixException : public std::exception {
|
||||
CarefulCholeskyNegativeMatrixException() throw() {}
|
||||
virtual ~CarefulCholeskyNegativeMatrixException() throw() {}
|
||||
const char* what() const throw() { return "The matrix was found to be non-positive-semidefinite when factoring with careful Cholesky."; }
|
||||
};
|
||||
|
||||
/**
|
||||
* "Careful" Cholesky computes the positive square-root of a positive symmetric
|
||||
* semi-definite matrix (i.e. that may be rank-deficient). Unlike standard
|
||||
|
|
@ -70,6 +31,10 @@ struct CarefulCholeskyNegativeMatrixException : public std::exception {
|
|||
* non-zero row in the computed factor, so that it may be truncated to an
|
||||
* upper-trapazoidal matrix.
|
||||
*
|
||||
* The second element of the return value is \c true if the matrix was factored
|
||||
* successfully, or \c false if it was non-positive-semidefinite (i.e.
|
||||
* indefinite or negative-(semi-)definite.
|
||||
*
|
||||
* Note that this returned index is the rank of the matrix if and only if all
|
||||
* of the zero-rows of the factor occur after any non-zero rows. This is
|
||||
* (always?) the case during elimination of a fully-constrained least-squares
|
||||
|
|
@ -77,6 +42,8 @@ struct CarefulCholeskyNegativeMatrixException : public std::exception {
|
|||
*
|
||||
* The optional order argument specifies the size of the square upper-left
|
||||
* submatrix to operate on, ignoring the rest of the matrix.
|
||||
*
|
||||
*
|
||||
*/
|
||||
std::pair<size_t,bool> choleskyCareful(Matrix& ATA, int order = -1);
|
||||
|
||||
|
|
@ -87,8 +54,11 @@ std::pair<size_t,bool> choleskyCareful(Matrix& ATA, int order = -1);
|
|||
* [B' C]
|
||||
* nFrontal determines the split between A, B, and C, with A being of size
|
||||
* nFrontal x nFrontal.
|
||||
*
|
||||
* @return \c true if the decomposition is successful, \c false if \c A was
|
||||
* not positive-definite.
|
||||
*/
|
||||
void choleskyPartial(Matrix& ABC, size_t nFrontal);
|
||||
bool choleskyPartial(Matrix& ABC, size_t nFrontal);
|
||||
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -84,6 +84,46 @@ TEST(cholesky, BadScalingSVD) {
|
|||
DOUBLES_EQUAL(expectedCondition, actualCondition, 1e-41);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(cholesky, underconstrained) {
|
||||
Matrix L(6,6); L <<
|
||||
1, 0, 0, 0, 0, 0,
|
||||
1.11177808157954, 1.06204809504665, 0.507342638873381, 1.34953401829486, 1, 0,
|
||||
0.155864888199928, 1.10933048588373, 0.501255576961674, 1, 0, 0,
|
||||
1.12108665967793, 1.01584408366945, 1, 0, 0, 0,
|
||||
0.776164062474843, 0.117617236580373, -0.0236628691347294, 0.814118199972143, 0.694309975328922, 1,
|
||||
0.1197220685104, 1, 0, 0, 0, 0;
|
||||
Matrix D1(6,6); D1 <<
|
||||
0.814723686393179, 0, 0, 0, 0, 0,
|
||||
0, 0.811780089277421, 0, 0, 0, 0,
|
||||
0, 0, 1.82596950680844, 0, 0, 0,
|
||||
0, 0, 0, 0.240287537694585, 0, 0,
|
||||
0, 0, 0, 0, 1.34342584865901, 0,
|
||||
0, 0, 0, 0, 0, 1e-12;
|
||||
Matrix D2(6,6); D2 <<
|
||||
0.814723686393179, 0, 0, 0, 0, 0,
|
||||
0, 0.811780089277421, 0, 0, 0, 0,
|
||||
0, 0, 1.82596950680844, 0, 0, 0,
|
||||
0, 0, 0, 0.240287537694585, 0, 0,
|
||||
0, 0, 0, 0, 0, 0,
|
||||
0, 0, 0, 0, 0, 0;
|
||||
Matrix D3(6,6); D3 <<
|
||||
0.814723686393179, 0, 0, 0, 0, 0,
|
||||
0, 0.811780089277421, 0, 0, 0, 0,
|
||||
0, 0, 1.82596950680844, 0, 0, 0,
|
||||
0, 0, 0, 0.240287537694585, 0, 0,
|
||||
0, 0, 0, 0, -0.5, 0,
|
||||
0, 0, 0, 0, 0, -0.6;
|
||||
|
||||
Matrix A1 = L * D1 * L.transpose();
|
||||
Matrix A2 = L * D2 * L.transpose();
|
||||
Matrix A3 = L * D3 * L.transpose();
|
||||
|
||||
LONGS_EQUAL(long(false), long(choleskyPartial(A1, 6)));
|
||||
LONGS_EQUAL(long(false), long(choleskyPartial(A2, 6)));
|
||||
LONGS_EQUAL(long(false), long(choleskyPartial(A3, 6)));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
int main() {
|
||||
TestResult tr;
|
||||
|
|
|
|||
|
|
@ -17,8 +17,10 @@
|
|||
|
||||
#include <string.h>
|
||||
#include <boost/format.hpp>
|
||||
#include <boost/lambda/lambda.hpp>
|
||||
#include <boost/lambda/bind.hpp>
|
||||
|
||||
#include <gtsam/linear/linearExceptions.h>
|
||||
#include <gtsam/linear/GaussianConditional.h>
|
||||
#include <gtsam/linear/GaussianFactor.h>
|
||||
#include <gtsam/linear/JacobianFactor.h>
|
||||
|
|
@ -189,6 +191,11 @@ void GaussianConditional::solveInPlace(VectorValues& x) const {
|
|||
Vector xS = internal::extractVectorValuesSlices(x, this->beginParents(), this->endParents());
|
||||
xS = this->get_d() - this->get_S() * xS;
|
||||
Vector soln = this->get_R().triangularView<Eigen::Upper>().solve(xS);
|
||||
|
||||
// Check for indeterminant solution
|
||||
if(soln.unaryExpr(!boost::lambda::bind(ptr_fun(isfinite<double>), boost::lambda::_1)).any())
|
||||
throw IndeterminantLinearSystemException(this->keys().front());
|
||||
|
||||
if(debug) {
|
||||
gtsam::print(Matrix(this->get_R()), "Calling backSubstituteUpper on ");
|
||||
gtsam::print(soln, "full back-substitution solution: ");
|
||||
|
|
@ -199,8 +206,12 @@ void GaussianConditional::solveInPlace(VectorValues& x) const {
|
|||
/* ************************************************************************* */
|
||||
void GaussianConditional::solveTransposeInPlace(VectorValues& gy) const {
|
||||
Vector frontalVec = internal::extractVectorValuesSlices(gy, beginFrontals(), endFrontals());
|
||||
// TODO: verify permutation
|
||||
frontalVec = gtsam::backSubstituteUpper(frontalVec,Matrix(get_R()));
|
||||
|
||||
// Check for indeterminant solution
|
||||
if(frontalVec.unaryExpr(!boost::lambda::bind(ptr_fun(isfinite<double>), boost::lambda::_1)).any())
|
||||
throw IndeterminantLinearSystemException(this->keys().front());
|
||||
|
||||
GaussianConditional::const_iterator it;
|
||||
for (it = beginParents(); it!= endParents(); it++) {
|
||||
const Index i = *it;
|
||||
|
|
|
|||
|
|
@ -393,14 +393,7 @@ break;
|
|||
// some untouched non-zeros that should be zero. We zero them while
|
||||
// extracting submatrices next.
|
||||
tic(4, "partial Cholesky");
|
||||
try {
|
||||
combinedFactor->partialCholesky(nrFrontals);
|
||||
} catch
|
||||
(std::exception &ex) { // catch exception from Cholesky
|
||||
combinedFactor->print("combinedFactor");
|
||||
string reason = "EliminateCholesky failed while trying to eliminate the combined factor";
|
||||
throw invalid_argument(reason);
|
||||
}
|
||||
combinedFactor->partialCholesky(nrFrontals);
|
||||
|
||||
toc(4, "partial Cholesky");
|
||||
|
||||
|
|
@ -502,64 +495,9 @@ break;
|
|||
return EliminateQR(factors, nrFrontals);
|
||||
else {
|
||||
GaussianFactorGraph::EliminationResult ret;
|
||||
#ifdef NDEBUG
|
||||
static const bool diag = false;
|
||||
#else
|
||||
static const bool diag = !ISDEBUG("NoCholeskyDiagnostics");
|
||||
#endif
|
||||
if (!diag) {
|
||||
tic(2, "EliminateCholesky");
|
||||
ret = EliminateCholesky(factors, nrFrontals);
|
||||
toc(2, "EliminateCholesky");
|
||||
} else {
|
||||
try {
|
||||
tic(2, "EliminateCholesky");
|
||||
ret = EliminateCholesky(factors, nrFrontals);
|
||||
toc(2, "EliminateCholesky");
|
||||
} catch (const exception& e) {
|
||||
cout << "Exception in EliminateCholesky: " << e.what() << endl;
|
||||
SETDEBUG("EliminateCholesky", true);
|
||||
SETDEBUG("updateATA", true);
|
||||
SETDEBUG("JacobianFactor::eliminate", true);
|
||||
SETDEBUG("JacobianFactor::Combine", true);
|
||||
SETDEBUG("choleskyPartial", true);
|
||||
factors.print("Combining factors: ");
|
||||
EliminateCholesky(factors, nrFrontals);
|
||||
throw;
|
||||
}
|
||||
}
|
||||
|
||||
const bool checkCholesky = ISDEBUG("EliminateGaussian Check Cholesky");
|
||||
if (checkCholesky) {
|
||||
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 << "Cholesky and QR do not agree" << endl;
|
||||
|
||||
SETDEBUG("EliminateCholesky", true);
|
||||
SETDEBUG("updateATA", true);
|
||||
SETDEBUG("JacobianFactor::eliminate", true);
|
||||
SETDEBUG("JacobianFactor::Combine", true);
|
||||
jacobians.print("Jacobian Factors: ");
|
||||
EliminateJacobians(jacobians, nrFrontals);
|
||||
EliminateCholesky(factors, nrFrontals);
|
||||
factors.print("Combining factors: ");
|
||||
|
||||
throw runtime_error("Cholesky and QR do not agree");
|
||||
}
|
||||
}
|
||||
|
||||
tic(2, "EliminateCholesky");
|
||||
ret = EliminateCholesky(factors, nrFrontals);
|
||||
toc(2, "EliminateCholesky");
|
||||
return ret;
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -28,6 +28,7 @@
|
|||
#include <gtsam/base/Matrix.h>
|
||||
#include <gtsam/base/FastMap.h>
|
||||
#include <gtsam/base/cholesky.h>
|
||||
#include <gtsam/linear/linearExceptions.h>
|
||||
#include <gtsam/linear/GaussianConditional.h>
|
||||
#include <gtsam/linear/HessianFactor.h>
|
||||
#include <gtsam/linear/JacobianFactor.h>
|
||||
|
|
@ -47,13 +48,7 @@ string SlotEntry::toString() const {
|
|||
|
||||
/* ************************************************************************* */
|
||||
void HessianFactor::assertInvariants() const {
|
||||
#ifndef NDEBUG
|
||||
// Check for non-finite values
|
||||
for(size_t i=0; i<(size_t) matrix_.rows(); ++i)
|
||||
for(size_t j=i; j<(size_t) matrix_.cols(); ++j)
|
||||
if(!isfinite(matrix_(i,j)))
|
||||
throw invalid_argument("HessianFactor contains non-finite matrix entries.");
|
||||
#endif
|
||||
GaussianFactor::assertInvariants(); // The base class checks for unique keys
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
@ -473,7 +468,8 @@ void HessianFactor::updateATA(const JacobianFactor& update, const Scatter& scatt
|
|||
|
||||
/* ************************************************************************* */
|
||||
void HessianFactor::partialCholesky(size_t nrFrontals) {
|
||||
choleskyPartial(matrix_, info_.offset(nrFrontals));
|
||||
if(!choleskyPartial(matrix_, info_.offset(nrFrontals)))
|
||||
throw IndeterminantLinearSystemException(this->keys().front());
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
|
|||
|
|
@ -15,6 +15,7 @@
|
|||
* @date Dec 8, 2010
|
||||
*/
|
||||
|
||||
#include <gtsam/linear/linearExceptions.h>
|
||||
#include <gtsam/linear/GaussianConditional.h>
|
||||
#include <gtsam/linear/JacobianFactor.h>
|
||||
#include <gtsam/linear/HessianFactor.h>
|
||||
|
|
@ -50,12 +51,6 @@ namespace gtsam {
|
|||
assert(firstNonzeroBlocks_.size() == Ab_.rows());
|
||||
for(size_t i=0; i<firstNonzeroBlocks_.size(); ++i)
|
||||
assert(firstNonzeroBlocks_[i] < Ab_.nBlocks());
|
||||
|
||||
// Check for non-finite values
|
||||
for(size_t i=0; i<Ab_.rows(); ++i)
|
||||
for(size_t j=0; j<Ab_.cols(); ++j)
|
||||
if(isnan(matrix_(i,j)))
|
||||
throw invalid_argument("JacobianFactor contains NaN matrix entries.");
|
||||
#endif
|
||||
}
|
||||
|
||||
|
|
@ -194,18 +189,16 @@ namespace gtsam {
|
|||
JacobianFactor::JacobianFactor(const HessianFactor& factor) : Ab_(matrix_) {
|
||||
keys_ = factor.keys_;
|
||||
Ab_.assignNoalias(factor.info_);
|
||||
|
||||
// Do Cholesky to get a Jacobian
|
||||
size_t maxrank;
|
||||
try {
|
||||
maxrank = choleskyCareful(matrix_).first;
|
||||
} catch(const CarefulCholeskyNegativeMatrixException&) {
|
||||
cout <<
|
||||
"Attempting to convert a HessianFactor to a JacobianFactor, but for this\n"
|
||||
"HessianFactor it is not possible because either the Hessian is negative or\n"
|
||||
"indefinite, or the quadratic error function it describes becomes negative for\n"
|
||||
"some values. Here is the HessianFactor on which this conversion was attempted:\n";
|
||||
factor.print("");
|
||||
throw;
|
||||
}
|
||||
bool success;
|
||||
boost::tie(maxrank, success) = choleskyCareful(matrix_);
|
||||
|
||||
// Check for indefinite system
|
||||
if(!success)
|
||||
throw IndeterminantLinearSystemException(factor.keys().front());
|
||||
|
||||
// Zero out lower triangle
|
||||
matrix_.topRows(maxrank).triangularView<Eigen::StrictlyLower>() =
|
||||
Matrix::Zero(maxrank, matrix_.cols());
|
||||
|
|
@ -324,7 +317,6 @@ namespace gtsam {
|
|||
return model_->whiten(Ax);
|
||||
}
|
||||
|
||||
|
||||
/* ************************************************************************* */
|
||||
void JacobianFactor::transposeMultiplyAdd(double alpha, const Vector& e,
|
||||
VectorValues& x) const {
|
||||
|
|
@ -410,11 +402,8 @@ namespace gtsam {
|
|||
size_t frontalDim = Ab_.range(0,nrFrontals).cols();
|
||||
|
||||
// Check for singular factor
|
||||
if(model_->dim() < frontalDim) {
|
||||
throw domain_error((boost::format(
|
||||
"JacobianFactor is singular in variable %1%, discovered while attempting\n"
|
||||
"to eliminate this variable.") % front()).str());
|
||||
}
|
||||
if(model_->dim() < frontalDim)
|
||||
throw IndeterminantLinearSystemException(this->keys().front());
|
||||
|
||||
// Extract conditional
|
||||
tic(3, "cond Rd");
|
||||
|
|
|
|||
|
|
@ -0,0 +1,180 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
|
||||
* Atlanta, Georgia 30332-0415
|
||||
* All Rights Reserved
|
||||
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
|
||||
|
||||
* See LICENSE for the license information
|
||||
|
||||
* -------------------------------------------------------------------------- */
|
||||
|
||||
/**
|
||||
* @file linearExceptions.h
|
||||
* @brief Exceptions that may be thrown by linear solver components
|
||||
* @author Richard Roberts
|
||||
* @date Aug 17, 2012
|
||||
*/
|
||||
#pragma once
|
||||
|
||||
#include <gtsam/base/types.h>
|
||||
#include <boost/lexical_cast.hpp>
|
||||
#include <exception>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
/**
|
||||
Thrown when a linear system is ill-posed. The most common cause for this
|
||||
error is having underconstrained variables. Mathematically, the system is
|
||||
either underdetermined, or its quadratic error function is concave in some
|
||||
directions.
|
||||
|
||||
Examples of situations causing this error are:
|
||||
- A landmark observed by two cameras with a very small baseline will have
|
||||
high uncertainty in its distance from the cameras but low uncertainty
|
||||
in its bearing, creating a poorly-conditioned system.
|
||||
- A landmark observed by only a single ProjectionFactor, RangeFactor, or
|
||||
BearingFactor (the landmark is not completely constrained).
|
||||
- An overall scale or rigid transformation ambiguity, for example missing
|
||||
a prior or hard constraint on the first pose, or missing a scale
|
||||
constraint between the first two cameras (in structure-from-motion).
|
||||
|
||||
Mathematically, the following conditions cause this problem:
|
||||
- Underdetermined system: This occurs when the variables are not
|
||||
completely constrained by factors. Even if all variables are involved
|
||||
in factors, the variables can still be numerically underconstrained,
|
||||
(for example, if a landmark is observed by only one ProjectionFactor).
|
||||
Mathematically in this case, the rank of the linear Jacobian or Hessian
|
||||
is less than the number of scalars in the system.
|
||||
- Indefinite system: This condition occurs when the system Hessian is
|
||||
indefinite, i.e. non-positive-semidefinite. Note that this condition
|
||||
can also indicate an underdetermined system, but when solving with
|
||||
Cholesky, accumulation of numerical errors can cause the system to
|
||||
become indefinite (have some negative eigenvalues) even if it is in
|
||||
reality positive semi-definite (has some near-zero positive
|
||||
eigenvalues). Alternatively, if the system contains some
|
||||
HessianFactor's with negative eigenvalues, these can create a truly
|
||||
indefinite system, whose quadratic error function has negative
|
||||
curvature in some directions.
|
||||
- Poorly-conditioned system: A system that is positive-definite (has a
|
||||
unique solution) but is numerically ill-conditioned can accumulate
|
||||
numerical errors during solving that cause the system to become
|
||||
indefinite later during the elimination process. Note that poorly-
|
||||
conditioned systems will usually have inaccurate solutions, even if
|
||||
they do not always trigger this error. Systems with almost-
|
||||
unconstrained variables or vastly different measurement uncertainties
|
||||
or variable units can be poorly-conditioned.
|
||||
|
||||
Resolving this problem:
|
||||
- This exception contains the variable at which the problem was
|
||||
discovered (IndeterminantLinearSystemException::nearbyVariable()).
|
||||
Note, however, that this is not necessarily the variable where the
|
||||
problem originates. For example, in the case that a prior on the
|
||||
first camera was forgotten, it may only be another camera or landmark
|
||||
where the problem can be detected. Where the problem is detected
|
||||
depends on the graph structure and variable elimination ordering.
|
||||
- MATLAB (or similar software) is a useful tool to diagnose this problem.
|
||||
Use GaussianFactorGraph::sparseJacobian_(),
|
||||
GaussianFactorGraph::sparseJacobian()
|
||||
GaussianFactorGraph::denseJacobian(), and
|
||||
GaussianFactorGraph::denseHessian() to output the linear graph in
|
||||
matrix format. If using the MATLAB wrapper, the returned matrices can
|
||||
be immediately inspected and analyzed using standard methods. If not
|
||||
using the MATLAB wrapper, the output of these functions may be written
|
||||
to text files and then loaded into MATLAB.
|
||||
- When outputting linear graphs as Jacobian matrices, rows are ordered in
|
||||
the same order as factors in the graph, which each factor occupying the
|
||||
number of rows indicated by its JacobianFactor::rows() function. Each
|
||||
column appears in elimination ordering, as in the Ordering class. Each
|
||||
variable occupies the number of columns indicated by the
|
||||
JacobianFactor::getDim() function.
|
||||
- When outputting linear graphs as Hessian matrices, rows and columns are
|
||||
ordered in elimination order and occupy scalars in the same way as
|
||||
described for Jacobian columns in the previous bullet.
|
||||
*/
|
||||
class IndeterminantLinearSystemException : public std::exception {
|
||||
Index j_;
|
||||
mutable std::string what_;
|
||||
public:
|
||||
IndeterminantLinearSystemException(Index j) throw() : j_(j) {}
|
||||
virtual ~IndeterminantLinearSystemException() throw() {}
|
||||
Index nearbyVariable() const { return j_; }
|
||||
virtual const char* what() const throw() {
|
||||
if(what_.empty())
|
||||
what_ =
|
||||
"\nIndeterminant linear system detected while working near variable with\n"
|
||||
"index " + boost::lexical_cast<std::string>(j_) + " in ordering.\n"
|
||||
"\n\
|
||||
Thrown when a linear system is ill-posed. The most common cause for this\n\
|
||||
error is having underconstrained variables. Mathematically, the system is\n\
|
||||
either underdetermined, or its quadratic error function is concave in some\n\
|
||||
directions.\n\
|
||||
\n\
|
||||
Examples of situations causing this error are:\n\
|
||||
- A landmark observed by two cameras with a very small baseline will have\n\
|
||||
high uncertainty in its distance from the cameras but low uncertainty\n\
|
||||
in its bearing, creating a poorly-conditioned system.\n\
|
||||
- A landmark observed by only a single ProjectionFactor, RangeFactor, or\n\
|
||||
BearingFactor (the landmark is not completely constrained).\n\
|
||||
- An overall scale or rigid transformation ambiguity, for example missing\n\
|
||||
a prior or hard constraint on the first pose, or missing a scale\n\
|
||||
constraint between the first two cameras (in structure-from-motion).\n\
|
||||
\n\
|
||||
Mathematically, the following conditions cause this problem:\n\
|
||||
- Underdetermined system: This occurs when the variables are not\n\
|
||||
completely constrained by factors. Even if all variables are involved\n\
|
||||
in factors, the variables can still be numerically underconstrained,\n\
|
||||
(for example, if a landmark is observed by only one ProjectionFactor).\n\
|
||||
Mathematically in this case, the rank of the linear Jacobian or Hessian\n\
|
||||
is less than the number of scalars in the system.\n\
|
||||
- Indefinite system: This condition occurs when the system Hessian is\n\
|
||||
indefinite, i.e. non-positive-semidefinite. Note that this condition\n\
|
||||
can also indicate an underdetermined system, but when solving with\n\
|
||||
Cholesky, accumulation of numerical errors can cause the system to\n\
|
||||
become indefinite (have some negative eigenvalues) even if it is in\n\
|
||||
reality positive semi-definite (has some near-zero positive\n\
|
||||
eigenvalues). Alternatively, if the system contains some\n\
|
||||
HessianFactor's with negative eigenvalues, these can create a truly\n\
|
||||
indefinite system, whose quadratic error function has negative\n\
|
||||
curvature in some directions.\n\
|
||||
- Poorly-conditioned system: A system that is positive-definite (has a\n\
|
||||
unique solution) but is numerically ill-conditioned can accumulate\n\
|
||||
numerical errors during solving that cause the system to become\n\
|
||||
indefinite later during the elimination process. Note that poorly-\n\
|
||||
conditioned systems will usually have inaccurate solutions, even if\n\
|
||||
they do not always trigger this error. Systems with almost-\n\
|
||||
unconstrained variables or vastly different measurement uncertainties\n\
|
||||
or variable units can be poorly-conditioned.\n\
|
||||
\n\
|
||||
Resolving this problem:\n\
|
||||
- This exception contains the variable at which the problem was\n\
|
||||
discovered (IndeterminantLinearSystemException::nearbyVariable()).\n\
|
||||
Note, however, that this is not necessarily the variable where the\n\
|
||||
problem originates. For example, in the case that a prior on the\n\
|
||||
first camera was forgotten, it may only be another camera or landmark\n\
|
||||
where the problem can be detected. Where the problem is detected\n\
|
||||
depends on the graph structure and variable elimination ordering.\n\
|
||||
- MATLAB (or similar software) is a useful tool to diagnose this problem.\n\
|
||||
Use GaussianFactorGraph::sparseJacobian_(),\n\
|
||||
GaussianFactorGraph::sparseJacobian()\n\
|
||||
GaussianFactorGraph::denseJacobian(), and\n\
|
||||
GaussianFactorGraph::denseHessian() to output the linear graph in\n\
|
||||
matrix format. If using the MATLAB wrapper, the returned matrices can\n\
|
||||
be immediately inspected and analyzed using standard methods. If not\n\
|
||||
using the MATLAB wrapper, the output of these functions may be written\n\
|
||||
to text files and then loaded into MATLAB.\n\
|
||||
- When outputting linear graphs as Jacobian matrices, rows are ordered in\n\
|
||||
the same order as factors in the graph, which each factor occupying the\n\
|
||||
number of rows indicated by its JacobianFactor::rows() function. Each\n\
|
||||
column appears in elimination ordering, as in the Ordering class. Each\n\
|
||||
variable occupies the number of columns indicated by the\n\
|
||||
JacobianFactor::getDim() function.\n\
|
||||
- When outputting linear graphs as Hessian matrices, rows and columns are\n\
|
||||
ordered in elimination order and occupy scalars in the same way as\n\
|
||||
described for Jacobian columns in the previous bullet.\
|
||||
";
|
||||
return what_.c_str();
|
||||
}
|
||||
};
|
||||
|
||||
}
|
||||
|
|
@ -140,7 +140,7 @@ struct ISAM2Params {
|
|||
* 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
|
||||
* 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 IndefiniteLinearSystemException when your problem's Hessian is actually positive
|
||||
* definite. For positive definite problems, numerical error accumulation can cause the problem to become
|
||||
* numerically negative or indefinite as solving proceeds, especially when using Cholesky.
|
||||
*/
|
||||
|
|
|
|||
|
|
@ -18,10 +18,10 @@
|
|||
|
||||
#include <cmath>
|
||||
|
||||
#include <gtsam/linear/linearExceptions.h>
|
||||
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
|
||||
#include <gtsam/nonlinear/SuccessiveLinearizationOptimizer.h>
|
||||
|
||||
#include <gtsam/base/cholesky.h> // For NegativeMatrixException
|
||||
#include <boost/algorithm/string.hpp>
|
||||
#include <string>
|
||||
|
||||
|
|
@ -134,7 +134,7 @@ void LevenbergMarquardtOptimizer::iterate() {
|
|||
state_.lambda *= params_.lambdaFactor;
|
||||
}
|
||||
}
|
||||
} catch(const NegativeMatrixException& e) {
|
||||
} catch(const IndeterminantLinearSystemException& e) {
|
||||
if(lmVerbosity >= LevenbergMarquardtParams::LAMBDA)
|
||||
cout << "Negative matrix, increasing lambda" << endl;
|
||||
// Either we're not cautious, or the same lambda was worse than the current error.
|
||||
|
|
|
|||
Loading…
Reference in New Issue