From 1c0dcbc438e2d4337a041ea6afabff1ff3ee0c47 Mon Sep 17 00:00:00 2001 From: Richard Roberts Date: Sun, 16 May 2010 17:08:14 +0000 Subject: [PATCH] Added smart=true for the noise model when constructing GaussianFactor from GaussianConditional (and unit test). If a factor with a constrained noise model was eliminated, and then a new factor was constructed from the conditional (this happens in iSAM), the new factor would have an unconstrained noise model with zero sigmas. --- cpp/GaussianFactor.cpp | 2 +- cpp/testGaussianFactor.cpp | 17 +++++++++++++++++ 2 files changed, 18 insertions(+), 1 deletion(-) diff --git a/cpp/GaussianFactor.cpp b/cpp/GaussianFactor.cpp index 0b6291b42..f77f93412 100644 --- a/cpp/GaussianFactor.cpp +++ b/cpp/GaussianFactor.cpp @@ -78,7 +78,7 @@ GaussianFactor::GaussianFactor(const boost::shared_ptr& cg) As_.insert(*it); // set sigmas from precisions size_t n = b_.size(); - model_ = noiseModel::Diagonal::Sigmas(cg->get_sigmas()); + model_ = noiseModel::Diagonal::Sigmas(cg->get_sigmas(), true); } /* ************************************************************************* */ diff --git a/cpp/testGaussianFactor.cpp b/cpp/testGaussianFactor.cpp index f484841ef..a18c77c83 100644 --- a/cpp/testGaussianFactor.cpp +++ b/cpp/testGaussianFactor.cpp @@ -20,6 +20,7 @@ using namespace boost::assign; #include "Matrix.h" #include "Ordering.h" #include "GaussianConditional.h" +#include "inference-inl.h" #include "smallExample.h" using namespace std; @@ -683,6 +684,22 @@ TEST( GaussianFactor, CONSTRUCTOR_GaussianConditional ) CHECK(assert_equal(expectedLF,actualLF,1e-5)); } +/* ************************************************************************* */ +TEST( GaussianFactor, CONSTRUCTOR_GaussianConditionalConstrained ) +{ + Matrix Ax = eye(2); + Vector b = Vector_(2, 3.0, 5.0); + SharedDiagonal noisemodel = noiseModel::Constrained::All(2); + GaussianFactor::shared_ptr expected(new GaussianFactor("x0", Ax, b, noisemodel)); + GaussianFactorGraph graph; + graph.push_back(expected); + + GaussianConditional::shared_ptr conditional = graph.eliminateOne("x0"); + GaussianFactor actual(conditional); + + CHECK(assert_equal(*expected, actual)); +} + /* ************************************************************************* */ TEST ( GaussianFactor, constraint_eliminate1 ) {