From d96b1146182eedbec66bac7feffa538353b76d01 Mon Sep 17 00:00:00 2001 From: Alex Cunningham Date: Thu, 3 Mar 2011 21:14:55 +0000 Subject: [PATCH] Changed diagonal noisemodel to use boost.optional for inverse sigmas to avoid ever storing inf --- .cproject | 276 +++++++++++++++++---------------- gtsam/linear/HessianFactor.cpp | 5 +- gtsam/linear/NoiseModel.cpp | 33 ++-- gtsam/linear/NoiseModel.h | 25 +-- 4 files changed, 184 insertions(+), 155 deletions(-) diff --git a/.cproject b/.cproject index 433391f3e..b086d14c5 100644 --- a/.cproject +++ b/.cproject @@ -322,14 +322,6 @@ true true - - make - -j2 - testGaussianFactor.run - true - true - true - make -j2 @@ -356,6 +348,7 @@ make + tests/testBayesTree.run true false @@ -363,6 +356,7 @@ make + testBinaryBayesNet.run true false @@ -410,6 +404,7 @@ make + testSymbolicBayesNet.run true false @@ -417,6 +412,7 @@ make + tests/testSymbolicFactor.run true false @@ -424,6 +420,7 @@ make + testSymbolicFactorGraph.run true false @@ -439,11 +436,20 @@ make + tests/testBayesTree true false true + + make + -j2 + testGaussianFactor.run + true + true + true + make -j2 @@ -478,7 +484,6 @@ make - testGraph.run true false @@ -574,7 +579,6 @@ make - testInference.run true false @@ -582,7 +586,6 @@ make - testGaussianBayesNet.run true false @@ -590,7 +593,6 @@ make - testGaussianFactor.run true false @@ -598,7 +600,6 @@ make - testJunctionTree.run true false @@ -606,7 +607,6 @@ make - testSymbolicBayesNet.run true false @@ -614,7 +614,6 @@ make - testSymbolicFactorGraph.run true false @@ -974,6 +973,7 @@ make + testErrors.run true false @@ -1333,7 +1333,6 @@ make - testSimulated2DOriented.run true false @@ -1373,7 +1372,6 @@ make - testSimulated2D.run true false @@ -1381,7 +1379,6 @@ make - testSimulated3D.run true false @@ -1427,6 +1424,14 @@ true true + + make + -j2 + tests/testHessianFactor.run + true + true + true + make -j2 @@ -1469,7 +1474,6 @@ make - tests/testGaussianISAM2 true false @@ -1491,6 +1495,46 @@ 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 @@ -1587,54 +1631,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 - check - true - true - true - make -j2 @@ -1667,6 +1663,14 @@ true true + + make + -j2 + check + true + true + true + @@ -1989,14 +1993,6 @@ true true - - make - -j2 - testGaussianFactor.run - true - true - true - make -j2 @@ -2023,6 +2019,7 @@ make + tests/testBayesTree.run true false @@ -2030,6 +2027,7 @@ make + testBinaryBayesNet.run true false @@ -2077,6 +2075,7 @@ make + testSymbolicBayesNet.run true false @@ -2084,6 +2083,7 @@ make + tests/testSymbolicFactor.run true false @@ -2091,6 +2091,7 @@ make + testSymbolicFactorGraph.run true false @@ -2106,11 +2107,20 @@ make + tests/testBayesTree true false true + + make + -j2 + testGaussianFactor.run + true + true + true + make -j2 @@ -2145,7 +2155,6 @@ make - testGraph.run true false @@ -2241,7 +2250,6 @@ make - testInference.run true false @@ -2249,7 +2257,6 @@ make - testGaussianBayesNet.run true false @@ -2257,7 +2264,6 @@ make - testGaussianFactor.run true false @@ -2265,7 +2271,6 @@ make - testJunctionTree.run true false @@ -2273,7 +2278,6 @@ make - testSymbolicBayesNet.run true false @@ -2281,7 +2285,6 @@ make - testSymbolicFactorGraph.run true false @@ -2641,6 +2644,7 @@ make + testErrors.run true false @@ -3000,7 +3004,6 @@ make - testSimulated2DOriented.run true false @@ -3040,7 +3043,6 @@ make - testSimulated2D.run true false @@ -3048,7 +3050,6 @@ make - testSimulated3D.run true false @@ -3094,6 +3095,14 @@ true true + + make + -j2 + tests/testHessianFactor.run + true + true + true + make -j2 @@ -3136,7 +3145,6 @@ make - tests/testGaussianISAM2 true false @@ -3158,6 +3166,46 @@ 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 @@ -3254,54 +3302,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 - check - true - true - true - make -j2 @@ -3334,6 +3334,14 @@ true true + + make + -j2 + check + true + true + true + diff --git a/gtsam/linear/HessianFactor.cpp b/gtsam/linear/HessianFactor.cpp index 0c1327bf1..d77bfcc87 100644 --- a/gtsam/linear/HessianFactor.cpp +++ b/gtsam/linear/HessianFactor.cpp @@ -153,12 +153,13 @@ namespace gtsam { if(jf.model_->isConstrained()) throw invalid_argument("Cannot construct HessianFactor from JacobianFactor with constrained noise model"); else { + Vector invsigmas = jf.model_->invsigmas(); typedef Eigen::Map EigenMap; typedef typeof(EigenMap(&jf.matrix_(0,0),0,0).block(0,0,0,0)) EigenBlock; EigenBlock A(EigenMap(&jf.matrix_(0,0),jf.matrix_.size1(),jf.matrix_.size2()).block( jf.Ab_.rowStart(),jf.Ab_.offset(0), jf.Ab_.full().size1(), jf.Ab_.full().size2())); - typedef typeof(Eigen::Map(&jf.model_->invsigmas()(0),0).asDiagonal()) EigenDiagonal; - EigenDiagonal R(Eigen::Map(&jf.model_->invsigmas()(0),jf.model_->dim()).asDiagonal()); + typedef typeof(Eigen::Map(&invsigmas(0),0).asDiagonal()) EigenDiagonal; + EigenDiagonal R(Eigen::Map(&invsigmas(0),jf.model_->dim()).asDiagonal()); info_.copyStructureFrom(jf.Ab_); EigenMap L(EigenMap(&matrix_(0,0), matrix_.size1(), matrix_.size2())); L.noalias() = A.transpose() * R * R * A; diff --git a/gtsam/linear/NoiseModel.cpp b/gtsam/linear/NoiseModel.cpp index b4d23f147..95ef00c4c 100644 --- a/gtsam/linear/NoiseModel.cpp +++ b/gtsam/linear/NoiseModel.cpp @@ -296,12 +296,16 @@ SharedDiagonal Gaussian::Cholesky(MatrixColMajor& Ab, size_t nFrontals) const { /* ************************************************************************* */ // Diagonal /* ************************************************************************* */ -Diagonal::Diagonal(const Vector& sigmas) : - Gaussian(sigmas.size()), sigmas_(sigmas), invsigmas_(reciprocal(sigmas)) { +Diagonal::Diagonal() : + Gaussian(1), sigmas_(ones(1)), invsigmas_(ones(1)) { } -Diagonal::Diagonal(const Vector& sigmas, const Vector& invsigmas): - Gaussian(sigmas.size()), sigmas_(sigmas), invsigmas_(invsigmas) { +Diagonal::Diagonal(const Vector& sigmas, bool initialize_invsigmas): + Gaussian(sigmas.size()), sigmas_(sigmas) { + if (initialize_invsigmas) + invsigmas_ = reciprocal(sigmas); + else + invsigmas_ = boost::none; } /* ************************************************************************* */ @@ -332,9 +336,21 @@ 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_); + return emul(v, invsigmas()); } /* ************************************************************************* */ @@ -344,18 +360,17 @@ Vector Diagonal::unwhiten(const Vector& v) const { /* ************************************************************************* */ Matrix Diagonal::Whiten(const Matrix& H) const { - return vector_scale(invsigmas_, H); + return vector_scale(invsigmas(), H); } /* ************************************************************************* */ void Diagonal::WhitenInPlace(Matrix& H) const { - vector_scale_inplace(invsigmas_, H); + vector_scale_inplace(invsigmas(), H); } /* ************************************************************************* */ void Diagonal::WhitenInPlace(MatrixColMajor& H) const { - - vector_scale_inplace(invsigmas_, H); + vector_scale_inplace(invsigmas(), H); } /* ************************************************************************* */ diff --git a/gtsam/linear/NoiseModel.h b/gtsam/linear/NoiseModel.h index 42b464e74..cc44f6155 100644 --- a/gtsam/linear/NoiseModel.h +++ b/gtsam/linear/NoiseModel.h @@ -100,12 +100,14 @@ namespace gtsam { */ struct Gaussian: public Base { - private: + protected: // TODO: store as boost upper-triangular or whatever is passed from above /* Matrix square root of information matrix (R) */ boost::optional sqrt_information_; + private: + /** * Return R itself, but note that Whiten(H) is cheaper than R*H */ @@ -222,13 +224,16 @@ namespace gtsam { protected: /** sigmas and reciprocal */ - Vector sigmas_, invsigmas_; - - /** protected constructor for constructing constraints */ - Diagonal(const Vector& sigmas, const Vector& invsigmas); + Vector sigmas_; + private: + boost::optional invsigmas_; /// optional to allow for constraints + protected: /** protected constructor takes sigmas */ - Diagonal(const Vector& sigmas = ones(1)); + Diagonal(); + + /** constructor to allow for disabling initializaion of invsigmas */ + Diagonal(const Vector& sigmas, bool initialize_invsigmas=true); public: @@ -273,8 +278,8 @@ namespace gtsam { /** * Return sqrt precisions */ - const Vector& invsigmas() const { return invsigmas_; } - double invsigma(size_t i) const { return invsigmas_(i); } + Vector invsigmas() const; + double invsigma(size_t i) const; /** * generate random variate @@ -285,7 +290,7 @@ namespace gtsam { * Return R itself, but note that Whiten(H) is cheaper than R*H */ virtual Matrix R() const { - return diag(invsigmas_); + return diag(invsigmas()); } private: @@ -316,7 +321,7 @@ namespace gtsam { /** protected constructor takes sigmas */ // Keeps only sigmas and calculates invsigmas when necessary Constrained(const Vector& sigmas = zero(1)) : - Diagonal(sigmas, sigmas) {} + Diagonal(sigmas, false) {} public: