diff --git a/gtsam/linear/HessianFactor.cpp b/gtsam/linear/HessianFactor.cpp index 385ed2394..72503b470 100644 --- a/gtsam/linear/HessianFactor.cpp +++ b/gtsam/linear/HessianFactor.cpp @@ -458,8 +458,8 @@ void HessianFactor::updateATA(const JacobianFactor& update, const Scatter& scatt } else { noiseModel::Diagonal::shared_ptr diagonal(boost::dynamic_pointer_cast(update.model_)); if(diagonal) { - Vector invsigmas2 = update.model_->invsigmas().cwiseProduct(update.model_->invsigmas()); - updateInform.noalias() = updateA.transpose() * invsigmas2.asDiagonal() * updateA; + const Vector& precisions = diagonal->precisions(); + updateInform.noalias() = updateA.transpose() * precisions.asDiagonal() * updateA; } else throw invalid_argument("In HessianFactor::updateATA, JacobianFactor noise model is neither Unit nor Diagonal"); } diff --git a/gtsam/linear/NoiseModel.cpp b/gtsam/linear/NoiseModel.cpp index 5c8d591b9..88b57046e 100644 --- a/gtsam/linear/NoiseModel.cpp +++ b/gtsam/linear/NoiseModel.cpp @@ -159,15 +159,12 @@ void Gaussian::WhitenSystem(Matrix& A1, Matrix& A2, Matrix& A3, Vector& b) const // Diagonal /* ************************************************************************* */ Diagonal::Diagonal() : - Gaussian(1), sigmas_(ones(1)), invsigmas_(ones(1)) { + Gaussian(1), sigmas_(ones(1)), invsigmas_(ones(1)), precisions_(ones(1)) { } -Diagonal::Diagonal(const Vector& sigmas, bool initialize_invsigmas): - Gaussian(sigmas.size()), sigmas_(sigmas) { - if (initialize_invsigmas) - invsigmas_ = reciprocal(sigmas); - else - invsigmas_ = boost::none; +Diagonal::Diagonal(const Vector& sigmas) : + Gaussian(sigmas.size()), sigmas_(sigmas), invsigmas_(reciprocal(sigmas)), precisions_( + emul(invsigmas_, invsigmas_)) { } /* ************************************************************************* */ @@ -198,18 +195,6 @@ void Diagonal::print(const string& name) const { gtsam::print(sigmas_, name + "diagonal sigmas"); } -/* ************************************************************************* */ -Vector Diagonal::invsigmas() const { - if (invsigmas_) return *invsigmas_; - else return reciprocal(sigmas_); -} - -/* ************************************************************************* */ -double Diagonal::invsigma(size_t i) const { - if (invsigmas_) return (*invsigmas_)(i); - else return 1.0/sigmas_(i); -} - /* ************************************************************************* */ Vector Diagonal::whiten(const Vector& v) const { return emul(v, invsigmas()); diff --git a/gtsam/linear/NoiseModel.h b/gtsam/linear/NoiseModel.h index 6dc5640e0..704106bc8 100644 --- a/gtsam/linear/NoiseModel.h +++ b/gtsam/linear/NoiseModel.h @@ -235,19 +235,19 @@ namespace gtsam { class GTSAM_EXPORT Diagonal : public Gaussian { protected: - /** sigmas and reciprocal */ - Vector sigmas_; - - private: - - boost::optional invsigmas_; ///< optional to allow for constraints + /** + * Standard deviations (sigmas), their inverse and inverse square (weights/precisions) + * These are all computed at construction: the idea is to use one shared model + * where computation is done only once, the common use case in many problems. + */ + Vector sigmas_, invsigmas_, precisions_; protected: /** protected constructor takes sigmas */ Diagonal(); /** constructor to allow for disabling initializaion of invsigmas */ - Diagonal(const Vector& sigmas, bool initialize_invsigmas=true); + Diagonal(const Vector& sigmas); public: @@ -292,8 +292,14 @@ namespace gtsam { /** * Return sqrt precisions */ - Vector invsigmas() const; - double invsigma(size_t i) const; + inline const Vector& invsigmas() const { return invsigmas_; } + inline double invsigma(size_t i) const {return invsigmas_(i);} + + /** + * Return precisions + */ + inline const Vector& precisions() const { return precisions_; } + inline double precision(size_t i) const {return precisions_(i);} /** * Return R itself, but note that Whiten(H) is cheaper than R*H @@ -338,12 +344,12 @@ namespace gtsam { /** protected constructor takes sigmas */ // Keeps only sigmas and calculates invsigmas when necessary Constrained(const Vector& sigmas = zero(1)) : - Diagonal(sigmas, false), mu_(repeat(sigmas.size(), 1000.0)) {} + 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, false), mu_(mu) {} + Diagonal(sigmas), mu_(mu) {} public: diff --git a/gtsam/linear/tests/testNoiseModel.cpp b/gtsam/linear/tests/testNoiseModel.cpp index 7a0e02722..2165038bf 100644 --- a/gtsam/linear/tests/testNoiseModel.cpp +++ b/gtsam/linear/tests/testNoiseModel.cpp @@ -17,18 +17,21 @@ */ -#include -#include -#include -using namespace boost::assign; +#include +#include #include -#include -#include + +#include +#include + +#include +#include using namespace std; using namespace gtsam; using namespace noiseModel; +using namespace boost::assign; static double sigma = 2, s_1=1.0/sigma, var = sigma*sigma, prc = 1.0/var; static Matrix R = Matrix_(3, 3, @@ -40,7 +43,7 @@ static Matrix Sigma = Matrix_(3, 3, 0.0, var, 0.0, 0.0, 0.0, var); -//static double inf = std::numeric_limits::infinity(); +//static double inf = numeric_limits::infinity(); /* ************************************************************************* */ TEST(NoiseModel, constructors) @@ -155,7 +158,12 @@ TEST(NoiseModel, ConstrainedConstructors ) Vector sigmas = Vector_(3, sigma, 0.0, 0.0); Vector mu = Vector_(3, 200.0, 300.0, 400.0); actual = Constrained::All(d); + // TODO: why should this be a thousand ??? Dummy variable? EXPECT(assert_equal(gtsam::repeat(d, 1000.0), actual->mu())); + EXPECT(assert_equal(gtsam::repeat(d, 0), actual->sigmas())); + double Inf = numeric_limits::infinity(); + EXPECT(assert_equal(gtsam::repeat(d, Inf), actual->invsigmas())); + EXPECT(assert_equal(gtsam::repeat(d, Inf), actual->precisions())); actual = Constrained::All(d, m); EXPECT(assert_equal(gtsam::repeat(d, m), actual->mu()));