diff --git a/gtsam/linear/HessianFactor.cpp b/gtsam/linear/HessianFactor.cpp index 72503b470..5baee8712 100644 --- a/gtsam/linear/HessianFactor.cpp +++ b/gtsam/linear/HessianFactor.cpp @@ -254,10 +254,10 @@ HessianFactor::HessianFactor(const GaussianFactor& gf) : info_(matrix_) { if(jf.model_->isConstrained()) throw invalid_argument("Cannot construct HessianFactor from JacobianFactor with constrained noise model"); else { - Vector invsigmas = jf.model_->invsigmas().cwiseProduct(jf.model_->invsigmas()); + const Vector& precisions = jf.model_->precisions(); info_.copyStructureFrom(jf.Ab_); BlockInfo::constBlock A = jf.Ab_.full(); - matrix_.noalias() = A.transpose() * invsigmas.asDiagonal() * A; + matrix_.noalias() = A.transpose() * precisions.asDiagonal() * A; } } else if(dynamic_cast(&gf)) { const HessianFactor& hf(static_cast(gf)); diff --git a/gtsam/linear/NoiseModel.cpp b/gtsam/linear/NoiseModel.cpp index 88b57046e..16b0a5bc1 100644 --- a/gtsam/linear/NoiseModel.cpp +++ b/gtsam/linear/NoiseModel.cpp @@ -308,8 +308,6 @@ SharedDiagonal Constrained::QR(Matrix& Ab) const { list Rd; Vector pseudo(m); // allocate storage for pseudo-inverse - Vector invsigmas = reciprocal(sigmas_); - Vector weights = emul(invsigmas,invsigmas); // calculate weights once // We loop over all columns, because the columns that can be eliminated // are not necessarily contiguous. For each one, estimate the corresponding @@ -321,7 +319,7 @@ SharedDiagonal Constrained::QR(Matrix& Ab) const { // Calculate weighted pseudo-inverse and corresponding precision gttic(constrained_QR_weightedPseudoinverse); - double precision = weightedPseudoinverse(a, weights, pseudo); + double precision = weightedPseudoinverse(a, precisions_, pseudo); gttoc(constrained_QR_weightedPseudoinverse); // If precision is zero, no information on this column diff --git a/gtsam/linear/NoiseModel.h b/gtsam/linear/NoiseModel.h index 704106bc8..37e5f0090 100644 --- a/gtsam/linear/NoiseModel.h +++ b/gtsam/linear/NoiseModel.h @@ -342,11 +342,9 @@ namespace gtsam { Vector mu_; /** protected constructor takes sigmas */ - // Keeps only sigmas and calculates invsigmas when necessary Constrained(const Vector& sigmas = zero(1)) : Diagonal(sigmas), mu_(repeat(sigmas.size(), 1000.0)) {} - // Keeps only sigmas and calculates invsigmas when necessary // allows for specifying mu Constrained(const Vector& mu, const Vector& sigmas) : Diagonal(sigmas), mu_(mu) {}