The precisions are used many many times in HessianFactor::updatATA, so I added them to the NoiseModel. The invsigmas are no longer optional and hence do not cause extra allocation at every invocation. Same with precision: returns const &

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<noiseModel::Diagonal>(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");
   }

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());

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<Vector> 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: