Fixed memory hog problem with Alex's help. SAM marginally faster (3-4% with 1000 poses, might be bigger effect for full dataset)

cpp/NoiseModel.cpp

@@ -85,26 +85,27 @@ namespace gtsam {
 				for (j = 0; j < n; j++) variances(j) = covariance(j,j);
 				return Diagonal::Variances(variances,true);
 			}
-			full: return shared_ptr(new Gaussian(inverse_square_root(covariance)));
+			full: return shared_ptr(new Gaussian(n, inverse_square_root(covariance)));
 		}
 
     void Gaussian::print(const string& name) const {
-			gtsam::print(sqrt_information_, "Gaussian");
+			gtsam::print(thisR(), "Gaussian");
 		}
 
 		bool Gaussian::equals(const Base& expected, double tol) const {
 			const Gaussian* p = dynamic_cast<const Gaussian*> (&expected);
 			if (p == NULL) return false;
 			if (typeid(*this) != typeid(*p)) return false;
-			return equal_with_abs_tol(sqrt_information_, p->sqrt_information_, sqrt(tol));
+			//if (!sqrt_information_) return true; // ALEX todo;
+			return equal_with_abs_tol(R(), p->R(), sqrt(tol));
 		}
 
 		Vector Gaussian::whiten(const Vector& v) const {
-			return sqrt_information_ * v;
+			return thisR() * v;
 		}
 
 		Vector Gaussian::unwhiten(const Vector& v) const {
-			return backSubstituteUpper(sqrt_information_, v);
+			return backSubstituteUpper(thisR(), v);
 		}
 
 		double Gaussian::Mahalanobis(const Vector& v) const {
@@ -114,11 +115,11 @@ namespace gtsam {
 		}
 
 		Matrix Gaussian::Whiten(const Matrix& H) const {
-		  return sqrt_information_ * H;
+		  return thisR() * H;
 		}
 
 		void Gaussian::WhitenInPlace(Matrix& H) const {
-		  H = sqrt_information_ * H;
+		  H = thisR() * H;
 		}
 
 		// General QR, see also special version in Constrained
@@ -140,8 +141,8 @@ namespace gtsam {
 		/* ************************************************************************* */
 		// TODO: can we avoid calling reciprocal twice ?
 		Diagonal::Diagonal(const Vector& sigmas) :
-					Gaussian(diag(reciprocal(sigmas))), invsigmas_(reciprocal(sigmas)),
+					Gaussian(sigmas.size()), invsigmas_(reciprocal(sigmas)), sigmas_(
-							sigmas_(sigmas) {
+							sigmas) {
 				}
 
     Diagonal::shared_ptr Diagonal::Variances(const Vector& variances, bool smart) {

cpp/NoiseModel.h

@@ -74,15 +74,26 @@ namespace gtsam {
 	 */
 	struct Gaussian: public Base {
 
-	protected:
+	private:
 
 		// TODO: store as boost upper-triangular or whatever is passed from above
 		/* Matrix square root of information matrix (R) */
-		Matrix sqrt_information_;
+		boost::optional<Matrix> sqrt_information_;
+
+		/**
+		 * Return R itself, but note that Whiten(H) is cheaper than R*H
+		 */
+   const Matrix& thisR() const {
+			// should never happen
+			if (!sqrt_information_) throw std::runtime_error("Gaussian: has no R matrix");
+			return *sqrt_information_;
+		}
+
+	protected:
 
 		/** protected constructor takes square root information matrix */
-		Gaussian(const Matrix& sqrt_information) :
+		Gaussian(size_t dim, const boost::optional<Matrix>& sqrt_information = boost::none) :
-			Base(sqrt_information.size1()), sqrt_information_(sqrt_information) {
+			Base(dim), sqrt_information_(sqrt_information) {
 		}
 
 	public:
@@ -94,7 +105,7 @@ namespace gtsam {
      * @param smart, check if can be simplified to derived class
      */
     static shared_ptr SqrtInformation(const Matrix& R) {
-    	return shared_ptr(new Gaussian(R));
+    	return shared_ptr(new Gaussian(R.size1(),R));
     }
 
     /**
@@ -107,7 +118,7 @@ namespace gtsam {
      * A Gaussian noise model created by specifying an information matrix.
      */
     static shared_ptr Information(const Matrix& Q)  {
-    	return shared_ptr(new Gaussian(square_root_positive(Q)));
+    	return shared_ptr(new Gaussian(Q.size1(),square_root_positive(Q)));
     }
 
 		virtual void print(const std::string& name) const;
@@ -151,9 +162,7 @@ namespace gtsam {
 		/**
 		 * Return R itself, but note that Whiten(H) is cheaper than R*H
 		 */
-		const Matrix& R() const {
+    virtual Matrix R() const { return thisR();}
-			return sqrt_information_;
-		}
 
 	}; // Gaussian
 
@@ -223,6 +232,12 @@ namespace gtsam {
      */
     virtual Vector sample() const;
 
+		/**
+		 * Return R itself, but note that Whiten(H) is cheaper than R*H
+		 */
+		virtual Matrix R() const {
+			return diag(invsigmas_);
+		}
   }; // Diagonal