Merged in feature/2.4.0/precisions (pull request #1)

Better noise model for updateAtA using precisions

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:
 

gtsam/linear/tests/testNoiseModel.cpp

@@ -17,18 +17,21 @@
  */
 
 
-#include <iostream>
-#include <boost/foreach.hpp>
-#include <boost/assign/std/vector.hpp>
-using namespace boost::assign;
+#include <gtsam/linear/NoiseModel.h>
+#include <gtsam/base/TestableAssertions.h>
 
 #include <CppUnitLite/TestHarness.h>
-#include <gtsam/base/TestableAssertions.h>
-#include <gtsam/linear/NoiseModel.h>
+
+#include <boost/foreach.hpp>
+#include <boost/assign/std/vector.hpp>
+
+#include <iostream>
+#include <limits>
 
 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<double>::infinity();
+//static double inf = numeric_limits<double>::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<double>::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()));