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: