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

undo sigma checks, and use explicit triangular view to perform inverse of R matrix

release/4.3a0
Varun Agrawal 2020-03-27 07:54:50 -04:00
parent de170d5f3a
commit a59786c809
1 changed files with 8 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
gtsam/linear/NoiseModel.cpp
View File

@ -143,7 +143,10 @@ bool Gaussian::equals(const Base& expected, double tol) const {
/* ************************************************************************* */ /* ************************************************************************* */
Vector Gaussian::sigmas() const { Vector Gaussian::sigmas() const {
Matrix Rinv = thisR().inverse(); // inverse of triangular matrix is fast Matrix R = thisR();
// fast inverse of upper-triangular matrix
// Alternate for Matrix Rinv = R.inverse();
Matrix Rinv = R.triangularView<Eigen::Upper>().solve(Matrix::Identity(R.rows(), R.cols()));
return Vector((Rinv * Rinv.transpose()).diagonal()).cwiseSqrt(); return Vector((Rinv * Rinv.transpose()).diagonal()).cwiseSqrt();
} }
@ -312,9 +315,10 @@ void Diagonal::WhitenInPlace(Eigen::Block<Matrix> H) const {
namespace internal { namespace internal {
// switch precisions and invsigmas to finite value // switch precisions and invsigmas to finite value
// TODO: why?? And, why not just ask s==0.0 below ?
static void fix(const Vector& sigmas, Vector& precisions, Vector& invsigmas) { static void fix(const Vector& sigmas, Vector& precisions, Vector& invsigmas) {
for (Vector::Index i = 0; i < sigmas.size(); ++i) for (Vector::Index i = 0; i < sigmas.size(); ++i)
if (sigmas[i] <= 1e-12) { if (!std::isfinite(1. / sigmas[i])) {
precisions[i] = 0.0; precisions[i] = 0.0;
invsigmas[i] = 0.0; invsigmas[i] = 0.0;
} }
@ -341,8 +345,8 @@ Constrained::shared_ptr Constrained::MixedSigmas(const Vector& mu,
/* ************************************************************************* */ /* ************************************************************************* */
bool Constrained::constrained(size_t i) const { bool Constrained::constrained(size_t i) const {
// numerically stable way, rather than comparing to 0.0 directly. // TODO why not just check sigmas_[i]==0.0 ?
return sigmas_[i] <= 1e-12; return !std::isfinite(1./sigmas_[i]);
} }
/* ************************************************************************* */ /* ************************************************************************* */