Merging in NoiseModel, not yet used though

cpp/Makefile.am

@@ -100,9 +100,9 @@ testBinaryBayesNet_SOURCES    = testBinaryBayesNet.cpp
 testBinaryBayesNet_LDADD      = libgtsam.la 
 
 # Gaussian inference 
-headers += GaussianFactorSet.h
-sources += Errors.cpp VectorConfig.cpp GaussianFactor.cpp GaussianFactorGraph.cpp GaussianConditional.cpp GaussianBayesNet.cpp
-check_PROGRAMS += testVectorConfig testGaussianFactor testGaussianFactorGraph testGaussianConditional testGaussianBayesNet
+headers += GaussianFactorSet.h NoiseModel.h
+sources += Errors.cpp VectorConfig.cpp GaussianFactor.cpp GaussianFactorGraph.cpp GaussianConditional.cpp GaussianBayesNet.cpp NoiseModel.cpp
+check_PROGRAMS += testVectorConfig testGaussianFactor testGaussianFactorGraph testGaussianConditional testGaussianBayesNet testNoiseModel
 testVectorConfig_SOURCES        = testVectorConfig.cpp
 testVectorConfig_LDADD          = libgtsam.la
 testGaussianFactor_SOURCES      = $(example) testGaussianFactor.cpp
@@ -113,6 +113,8 @@ testGaussianConditional_SOURCES = $(example) testGaussianConditional.cpp
 testGaussianConditional_LDADD   = libgtsam.la
 testGaussianBayesNet_SOURCES    = $(example) testGaussianBayesNet.cpp
 testGaussianBayesNet_LDADD      = libgtsam.la
+testNoiseModel_SOURCES          = testNoiseModel.cpp
+testNoiseModel_LDADD            = libgtsam.la
 
 # Iterative Methods
 headers += iterative-inl.h
@@ -278,11 +280,3 @@ CXXLINK = $(LIBTOOL) $(AM_LIBTOOLFLAGS) $(LIBTOOLFLAGS) --mode=link \
 # rule to run an executable
 %.run: % libgtsam.la
 	./$^
-
-# to compile colamd and cppunitlite first when make all and make install. 
-# The part after external_libs is copied from automatically generated Makefile when without the following line
-libgtsam.la: external_libs $(libgtsam_la_OBJECTS) $(libgtsam_la_DEPENDENCIES) 
-	$(libgtsam_la_LINK) -rpath $(libdir) $(libgtsam_la_OBJECTS) $(libgtsam_la_LIBADD) $(LIBS)
-
-
-

cpp/Matrix.cpp

@@ -624,6 +624,23 @@ Matrix inverse_square_root(const Matrix& A) {
   return vector_scale(S, V); // V*S;
 }
 
+/* ************************************************************************* */
+Matrix square_root_positive(const Matrix& A) {
+  size_t m = A.size2(), n = A.size1();
+  if (m!=n)
+    throw invalid_argument("inverse_square_root: A must be square");
+
+  // Perform SVD, TODO: symmetric SVD?
+  Matrix U,V;
+  Vector S;
+  svd(A,U,S,V);
+
+  // invert and sqrt diagonal of S
+  // We also arbitrarily choose sign to make result have positive signs
+  for(size_t i = 0; i<m; i++) S(i) = - pow(S(i),0.5);
+  return vector_scale(S, V); // V*S;
+}
+
 /* ************************************************************************* */
 
 } // namespace gtsam

cpp/Matrix.h

@@ -292,6 +292,9 @@ void svd(Matrix& A, Vector& S, Matrix& V);
 /** Use SVD to calculate inverse square root of a matrix */
 Matrix inverse_square_root(const Matrix& A);
 
+/** Use SVD to calculate the positive square root of a matrix */
+Matrix square_root_positive(const Matrix& A);
+
 // macro for unit tests
 #define EQUALITY(expected,actual)\
   { if (!assert_equal(expected,actual)) \

cpp/NoiseModel.cpp

@@ -0,0 +1,8 @@
+/*
+ * NoiseModel.cpp
+ *
+ *  Created on: Jan 13, 2010
+ *      Author: richard
+ */
+
+#include "NoiseModel.h"

cpp/NoiseModel.h

@@ -0,0 +1,275 @@
+/*
+ * NoiseModel.h
+ *
+ *  Created on: Jan 13, 2010
+ *      Author: richard
+ */
+
+#pragma once
+
+#include <boost/shared_ptr.hpp>
+#include <boost/numeric/ublas/traits.hpp>
+//#include <iostream>
+//using namespace std;
+
+#include "Vector.h"
+#include "Matrix.h"
+
+namespace gtsam {
+
+  // Forward declaration
+  class NoiseModel;
+
+  /*****************************************************************************
+   * NoiseModelBase is the abstract base class for all noise models.  NoiseModels
+   * must implement a 'whiten' function to normalize an error vector, and an
+   * 'unwhiten' function to unnormalize an error vector.
+   */
+  class NoiseModelBase {
+  public:
+    /**
+     * Whiten an error vector.
+     */
+    virtual Vector whiten(const Vector& v) const = 0;
+
+    /**
+     * Unwhiten an error vector.
+     */
+    virtual Vector unwhiten(const Vector& v) const = 0;
+
+    friend class NoiseModel;
+
+  private:
+    /**
+     * Used internally to duplicate the object while retaining the type.
+     */
+    virtual boost::shared_ptr<NoiseModelBase> clone() const = 0;
+  };
+
+
+
+  /*****************************************************************************
+   * NoiseModel is a container for NoiseModelBase, which internally stores
+   * a shared_ptr to a NoiseModelBase as to support fast and compact storage and
+   * copies.  Copying this class simply assigns the internal shared_ptr.
+   */
+  class NoiseModel {
+  private:
+    const boost::shared_ptr<const NoiseModelBase> base_;
+
+    /**
+     * Fast constructor, simply assigns shared_ptr.
+     */
+    NoiseModel(boost::shared_ptr<NoiseModelBase> noiseModel): base_(noiseModel) {
+      /*std::cout << "Assigning pointer" << std::endl;*/
+    }
+
+  public:
+    /**
+     * Fast copy constructor, simply assigns shared_ptr.
+     */
+    NoiseModel(const NoiseModel& noiseModel): base_(noiseModel.base_) { /*std::cout << "Assigning pointer" << std::endl;*/ }
+
+    /**
+     * Constructor that creates a fast-copyable NoiseModel class by cloning
+     * a non-pointer NoiseModelBase.  The type is retained and can be retrieved
+     * using a dynamic_cast.
+     */
+    template<class T>
+    NoiseModel(const T& noiseModel): base_(noiseModel.clone()) {}
+
+    /**
+     * Cast to boost::shared_ptr<NoiseModelBase> to retrieve a pointer to the
+     * NoiseModelBase type.  Can be used with dynamic_pointer_cast to retrieve
+     * the type at runtime.
+     * E.g.:  dynamic_pointer_cast<const Isotropic>(noiseModel).
+     */
+    operator const boost::shared_ptr<const NoiseModelBase> () const {
+      return base_; }
+
+    /**
+     * Call the NoiseModelBase virtual whiten function
+     */
+    Vector whiten(const Vector& v) const { return base_->whiten(v); }
+
+    /**
+     * Call the NoiseModelBase virtual unwhiten function
+     */
+    Vector unwhiten(const Vector& v) const { return base_->unwhiten(v); }
+
+    template<class T> friend boost::shared_ptr<const T> dynamic_pointer_cast(const NoiseModel& p);
+  };
+
+  template<class T>
+  boost::shared_ptr<const T> dynamic_pointer_cast(const NoiseModel& p) {
+    return boost::dynamic_pointer_cast<const T>(p.base_); }
+
+
+
+  /*****************************************************************************
+   * An isotropic noise model assigns the same sigma to each vector element.
+   * This class has no public constructors.  Instead, use either either the
+   * Sigma or Variance class.
+   */
+  class Isotropic : public NoiseModelBase {
+  protected:
+    double sigma_;
+    double invsigma_;
+
+    Isotropic(double sigma): sigma_(sigma), invsigma_(1.0/sigma) {}
+    Isotropic(const Isotropic& isotropic):
+      sigma_(isotropic.sigma_), invsigma_(isotropic.invsigma_) {}
+
+  public:
+    /**
+     * Whiten error vector by dividing by sigma
+     */
+    virtual Vector whiten(const Vector& v) const { return v * invsigma_; }
+
+    /**
+     * Unwhiten error vector by multiplying by sigma
+     */
+    virtual Vector unwhiten(const Vector& v) const { return v * sigma_; }
+
+    /**
+     * Clone is used to duplicate object while retaining type
+     */
+    boost::shared_ptr<NoiseModelBase> clone() const {
+      /*cout << "Cloning Isotropic" << endl;*/
+      return boost::shared_ptr<NoiseModelBase>(new Isotropic(*this)); }
+  };
+
+
+  /*****************************************************************************
+   * A diagonal noise model implements a diagonal covariance matrix, with the
+   * elements of the diagonal specified in a Vector.  This class has no public
+   * constructors, instead, use either the Sigmas or Variances class.
+   */
+  class Diagonal : public NoiseModelBase {
+  protected:
+    Vector sigmas_;
+    Vector invsigmas_;
+
+    Diagonal() {}
+    Diagonal(const Vector& sigmas): sigmas_(sigmas), invsigmas_(1.0 / sigmas) {}
+    Diagonal(const Diagonal& d): sigmas_(d.sigmas_), invsigmas_(d.invsigmas_) {}
+
+  public:
+    /**
+     * Whiten error vector by dividing by sigmas
+     */
+    virtual Vector whiten(const Vector& v) const { return emul(v, invsigmas_); }
+
+    /**
+     * Unwhiten error vector by multiplying by sigmas
+     */
+    virtual Vector unwhiten(const Vector& v) const { return emul(v, sigmas_); }
+
+    /**
+     * Clone is used to duplicate object while retaining type
+     */
+    boost::shared_ptr<NoiseModelBase> clone() const {
+      /*cout << "Cloning Isotropic" << endl;*/
+      return boost::shared_ptr<NoiseModelBase>(new Diagonal(*this)); }
+  };
+
+
+  /*****************************************************************************
+   * A full covariance noise model.
+   */
+  class FullCovariance : public NoiseModelBase {
+  protected:
+    Matrix sqrt_covariance_;
+    Matrix sqrt_inv_covariance_;
+
+  public:
+    FullCovariance(const Matrix& covariance):
+        sqrt_covariance_(square_root_positive(covariance)),
+        sqrt_inv_covariance_(inverse_square_root(covariance)) {}
+
+    FullCovariance(const FullCovariance& c):
+        sqrt_covariance_(c.sqrt_covariance_), sqrt_inv_covariance_(c.sqrt_inv_covariance_) {}
+
+    /**
+     * Whiten error vector by dividing by sigmas
+     */
+    virtual Vector whiten(const Vector& v) const { return sqrt_inv_covariance_ * v; }
+
+    /**
+     * Unwhiten error vector by multiplying by sigmas
+     */
+    virtual Vector unwhiten(const Vector& v) const { return sqrt_covariance_ * v; }
+
+    /**
+     * Clone is used to duplicate object while retaining type
+     */
+    boost::shared_ptr<NoiseModelBase> clone() const {
+      /*cout << "Cloning Isotropic" << endl;*/
+      return boost::shared_ptr<NoiseModelBase>(new FullCovariance(*this)); }
+  };
+
+
+
+  /*****************************************************************************
+   * An isotropic noise model using sigma, the noise standard
+   * deviation.
+   */
+  class Sigma : public Isotropic {
+  public:
+    Sigma(const Sigma& isotropic): Isotropic(isotropic) { /*cout << "Constructing Sigma from Sigma" << endl;*/ }
+
+    Sigma(double sigma): Isotropic(sigma) { /*cout << "Constructing Sigma from double" << endl;*/ }
+
+    boost::shared_ptr<NoiseModelBase> clone() const {
+      return boost::shared_ptr<NoiseModelBase>(new Sigma(*this)); }
+  };
+
+
+  /*****************************************************************************
+   * An isotropic noise model using the noise variance = sigma^2.
+   */
+  class Variance : public Isotropic {
+  public:
+    Variance(const Variance& v): Isotropic(v) {}
+
+    Variance(double variance): Isotropic(sqrt(variance)) {}
+
+    boost::shared_ptr<NoiseModelBase> clone() const {
+      return boost::shared_ptr<NoiseModelBase>(new Variance(*this)); }
+  };
+
+
+  /*****************************************************************************
+   * A diagonal noise model created by specifying a Vector of sigmas, i.e.
+   * standard devations, i.e. the diagonal of the square root covariance
+   * matrix.
+   */
+  class Sigmas : public Diagonal {
+  public:
+    Sigmas(const Sigmas& s): Diagonal(s) {}
+
+    Sigmas(const Vector& sigmas): Diagonal(sigmas) {}
+
+    boost::shared_ptr<NoiseModelBase> clone() const {
+      return boost::shared_ptr<NoiseModelBase>(new Sigmas(*this)); }
+  };
+
+
+  /*****************************************************************************
+   * A diagonal noise model created by specifying a Vector of variances, i.e.
+   * i.e. the diagonal of the covariance matrix.
+   */
+  class Variances : public Diagonal {
+  public:
+    Variances(const Variances& s): Diagonal(s) {}
+
+    Variances(const Vector& variances) {
+      std::transform(variances.begin(), variances.end(), sigmas_.begin(), sqrt);
+      invsigmas_ = 1.0 / sigmas_;
+    }
+
+    boost::shared_ptr<NoiseModelBase> clone() const {
+      return boost::shared_ptr<NoiseModelBase>(new Variances(*this)); }
+  };
+
+}

cpp/Vector.cpp

@@ -209,6 +209,14 @@ namespace gtsam {
   	return *(std::max_element(a.begin(), a.end()));
 	}
 
+  /* ************************************************************************* */
+  Vector operator/(double s, const Vector& v) {
+    Vector result(v.size());
+    for(size_t i = 0; i < v.size(); i++)
+      result[i] = s / v[i];
+    return result;
+  }
+
   /* ************************************************************************* */
   pair<double, Vector > house(Vector &x)
   {

cpp/Vector.h

@@ -187,6 +187,11 @@ double max(const Vector &a);
 /** Dot product */
 inline double dot(const Vector &a, const Vector& b) { return sum(emul(a,b)); }
 
+/**
+ * Divide every element of a Vector into a scalar
+ */
+Vector operator/(double s, const Vector& v);
+
 /**
  * house(x,j) computes HouseHolder vector v and scaling factor beta
  *  from x, such that the corresponding Householder reflection zeroes out

cpp/testMatrix.cpp

@@ -603,6 +603,26 @@ TEST( matrix, inverse_square_root )
 	EQUALITY(measurement_covariance,inverse(actual*actual));
 }
 
+/* ************************************************************************* */
+TEST( matrix, square_root_positive )
+{
+  Matrix cov = Matrix_(3,3,
+      4.25, 1.5, 0.0,
+      1.5,  2.0, 0.0,
+      0.0,  0.0, 1.0);
+
+  Matrix expected = Matrix_(3,3,
+      2.01246117974981,  0.447213595499958,  0.0,
+      0.447213595499958, 1.34164078649987,  0.0,
+      0.0, 0.0, 1.0);
+
+  Matrix actual = square_root_positive(cov);
+
+  CHECK(assert_equal(expected, actual));
+  CHECK(assert_equal(cov, trans(actual)*actual));
+}
+
+
 /* ************************************************************************* */
 int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
 /* ************************************************************************* */

cpp/testNoiseModel.cpp

@@ -0,0 +1,57 @@
+/*
+ * testNoiseModel.cpp
+ *
+ *  Created on: Jan 13, 2010
+ *      Author: richard
+ */
+
+#include <CppUnitLite/TestHarness.h>
+
+#include <boost/shared_ptr.hpp>
+#include <iostream>
+#include "NoiseModel.h"
+
+using namespace std;
+using namespace boost;
+using namespace gtsam;
+
+class TakesNoiseModel {
+public:
+  NoiseModel noiseModel_;
+public:
+  //template<class N>
+  TakesNoiseModel(const NoiseModel& noiseModel): noiseModel_(noiseModel) {}
+};
+
+
+TEST(NoiseModel, sharedptr)
+{
+  TakesNoiseModel tnm1(Sigma(1.0));
+  cout << endl;
+  TakesNoiseModel tnm2(tnm1.noiseModel_);
+
+  if(dynamic_pointer_cast<Sigma>(tnm1.noiseModel_))
+    cout << "tnm1 has a Sigma!" << endl;
+  if(dynamic_pointer_cast<Variance>(tnm1.noiseModel_))
+    cout << "tnm1 has a Variance!" << endl;
+  if(dynamic_pointer_cast<Isotropic>(tnm1.noiseModel_))
+    cout << "tnm1 has an Isotropic!" << endl;
+  if(dynamic_pointer_cast<NoiseModelBase>(tnm1.noiseModel_))
+    cout << "tnm1 has a NoiseModelBase!" << endl;
+
+  if(dynamic_pointer_cast<Sigma>(tnm2.noiseModel_))
+    cout << "tnm2 has a Sigma!" << endl;
+  if(dynamic_pointer_cast<Variance>(tnm2.noiseModel_))
+    cout << "tnm2 has a Variance!" << endl;
+  if(dynamic_pointer_cast<Isotropic>(tnm2.noiseModel_))
+    cout << "tnm2 has an Isotropic!" << endl;
+  if(dynamic_pointer_cast<NoiseModelBase>(tnm2.noiseModel_))
+    cout << "tnm2 has a NoiseModelBase!" << endl;
+}
+
+/* ************************************************************************* */
+int main() {
+  TestResult tr;
+  return TestRegistry::runAllTests(tr);
+}
+/* ************************************************************************* */