diff --git a/.cproject b/.cproject index dd3c433c5..ca8b29567 100644 --- a/.cproject +++ b/.cproject @@ -258,6 +258,14 @@ true true + + make + -j2 + testGaussianFactor.run + true + true + true + make -j2 @@ -284,7 +292,6 @@ make - tests/testBayesTree.run true false @@ -292,7 +299,6 @@ make - testBinaryBayesNet.run true false @@ -340,7 +346,6 @@ make - testSymbolicBayesNet.run true false @@ -348,7 +353,6 @@ make - tests/testSymbolicFactor.run true false @@ -356,7 +360,6 @@ make - testSymbolicFactorGraph.run true false @@ -372,20 +375,11 @@ make - tests/testBayesTree true false true - - make - -j2 - testGaussianFactor.run - true - true - true - make -j2 @@ -412,6 +406,7 @@ make + testGraph.run true false @@ -483,6 +478,7 @@ make + testInference.run true false @@ -490,6 +486,7 @@ make + testGaussianFactor.run true false @@ -497,6 +494,7 @@ make + testJunctionTree.run true false @@ -504,6 +502,7 @@ make + testSymbolicBayesNet.run true false @@ -511,6 +510,7 @@ make + testSymbolicFactorGraph.run true false @@ -580,22 +580,6 @@ false true - - make - -j2 - tests/testPose2.run - true - true - true - - - make - -j2 - tests/testPose3.run - true - true - true - make -j2 @@ -612,6 +596,22 @@ true true + + make + -j2 + tests/testPose2.run + true + true + true + + + make + -j2 + tests/testPose3.run + true + true + true + make -j2 @@ -636,15 +636,7 @@ true true - - make - -j2 - all - true - true - true - - + make -j2 check @@ -652,14 +644,6 @@ true true - - make - -j2 - clean - true - true - true - make -j2 @@ -700,7 +684,15 @@ true true - + + make + -j2 + all + true + true + true + + make -j2 check @@ -708,6 +700,14 @@ true true + + make + -j2 + clean + true + true + true + make -j2 @@ -1038,7 +1038,6 @@ make - testErrors.run true false @@ -1494,6 +1493,7 @@ make + testSimulated2DOriented.run true false @@ -1533,6 +1533,7 @@ make + testSimulated2D.run true false @@ -1540,6 +1541,7 @@ make + testSimulated3D.run true false @@ -1800,6 +1802,14 @@ true true + + make + -j2 + tests/testGaussianDensity.run + true + true + true + make -j2 @@ -1890,6 +1900,7 @@ make + tests/testGaussianISAM2 true false @@ -1911,46 +1922,6 @@ true true - - make - -j2 - install - true - true - true - - - make - -j2 - clean - true - true - true - - - make - -j2 - check - true - true - true - - - make - -j2 - all - true - true - true - - - make - -j2 - dist - true - true - true - make -j2 @@ -2047,6 +2018,94 @@ true true + + make + -j2 + install + true + true + true + + + make + -j2 + clean + true + true + true + + + make + -j2 + check + true + true + true + + + make + -j2 + all + true + true + true + + + make + -j2 + dist + true + true + true + + + make + -j2 + check + true + true + true + + + make + -j2 + install + true + true + true + + + make + -j2 + tests/testSpirit.run + true + true + true + + + make + -j2 + tests/testWrap.run + true + true + true + + + make + -j2 + clean + true + true + true + + + make + -j2 + all + true + true + true + make -j2 @@ -2103,54 +2162,6 @@ false true - - make - -j2 - check - true - true - true - - - make - -j2 - install - true - true - true - - - make - -j2 - tests/testSpirit.run - true - true - true - - - make - -j2 - tests/testWrap.run - true - true - true - - - make - -j2 - clean - true - true - true - - - make - -j2 - all - true - true - true - diff --git a/gtsam/linear/GaussianDensity.cpp b/gtsam/linear/GaussianDensity.cpp new file mode 100644 index 000000000..8e37a677d --- /dev/null +++ b/gtsam/linear/GaussianDensity.cpp @@ -0,0 +1,47 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file GaussianDensity.cpp + * @brief A Gaussian Density + * @author Frank Dellaert + * @date Jan 21, 2012 + */ + +#include + +using namespace std; + +namespace gtsam { + + /* ************************************************************************* */ + Vector GaussianDensity::mean() const { + // Solve for mean + VectorValues x; + Index k = firstFrontalKey(); + // a VectorValues that only has a value for k: cannot be printed if k<>0 + x.insert(k, Vector(sigmas_.size())); + rhs(x); + solveInPlace(x); + return x[k]; + } + + /* ************************************************************************* */ + Matrix GaussianDensity::information() const { + return computeInformation(); + } + + /* ************************************************************************* */ + Matrix GaussianDensity::covariance() const { + return inverse(information()); + } + +} // gtsam diff --git a/gtsam/linear/GaussianDensity.h b/gtsam/linear/GaussianDensity.h new file mode 100644 index 000000000..9ef0007ac --- /dev/null +++ b/gtsam/linear/GaussianDensity.h @@ -0,0 +1,70 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file GaussianDensity.h + * @brief A Gaussian Density + * @author Frank Dellaert + * @date Jan 21, 2012 + */ + +// \callgraph +#pragma once + +#include + +namespace gtsam { + + /** + * A Gaussian density. + * + * It is implemented as a GaussianConditional without parents. + * The negative log-probability is given by \f$ |Rx - d|^2 \f$ + * with \f$ \Lambda = \Sigma^{-1} = R^T R \f$ and \f$ \mu = R^{-1} d \f$ + */ + class GaussianDensity: public GaussianConditional { + + public: + + protected: + + public: + + /// default constructor needed for serialization + GaussianDensity() : + GaussianConditional() { + } + + // Copy constructor from GaussianConditional + GaussianDensity(const GaussianConditional& conditional) : + GaussianConditional(conditional) { + assert(conditional.nrParents() == 0); + } + + /// constructor using d, R + GaussianDensity(Index key, const Vector& d, const Matrix& R, + const Vector& sigmas) : + GaussianConditional(key, d, R, sigmas) { + } + + // Mean \f$ \mu = R^{-1} d \f$ + Vector mean() const; + + /// Information matrix \f$ \Lambda = R^T R \f$ + Matrix information() const; + + /// Covariance matrix \f$ \Sigma = (R^T R)^{-1} \f$ + Matrix covariance() const; + + }; +// GaussianDensity + +}// gtsam diff --git a/gtsam/linear/Makefile.am b/gtsam/linear/Makefile.am index 9fa46b7f9..811ae7458 100644 --- a/gtsam/linear/Makefile.am +++ b/gtsam/linear/Makefile.am @@ -26,10 +26,10 @@ sources += GaussianSequentialSolver.cpp GaussianMultifrontalSolver.cpp sources += JacobianFactor.cpp HessianFactor.cpp sources += GaussianFactor.cpp GaussianFactorGraph.cpp sources += GaussianJunctionTree.cpp -sources += GaussianConditional.cpp GaussianBayesNet.cpp +sources += GaussianConditional.cpp GaussianDensity.cpp GaussianBayesNet.cpp sources += GaussianISAM.cpp check_PROGRAMS += tests/testHessianFactor tests/testJacobianFactor tests/testGaussianConditional -check_PROGRAMS += tests/testGaussianFactorGraph tests/testGaussianJunctionTree +check_PROGRAMS += tests/testGaussianDensity tests/testGaussianFactorGraph tests/testGaussianJunctionTree # Kalman Filter sources += KalmanFilter.cpp diff --git a/gtsam/linear/tests/testGaussianDensity.cpp b/gtsam/linear/tests/testGaussianDensity.cpp new file mode 100644 index 000000000..7e7ecf5f8 --- /dev/null +++ b/gtsam/linear/tests/testGaussianDensity.cpp @@ -0,0 +1,42 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file testGaussianDensity.cpp + * @brief Unit tests for Gaussian Density + * @author Frank Dellaert + * @date Jan 21, 2012 + **/ + +#include +#include + +using namespace gtsam; +using namespace std; + +/* ************************************************************************* */ +TEST(GaussianDensity, constructor) +{ + Matrix R = Matrix_(2,2, + -12.1244, -5.1962, + 0., 4.6904); + + Vector d = Vector_(2, 1.0, 2.0), s = Vector_(2, 3.0, 4.0); + GaussianConditional conditional(1, d, R, s); + + GaussianDensity copied(conditional); + EXPECT(assert_equal(d, copied.get_d())); + EXPECT(assert_equal(s, copied.get_sigmas())); +} + +/* ************************************************************************* */ +int main() { TestResult tr; return TestRegistry::runAllTests(tr);} +/* ************************************************************************* */