12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

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.

release/4.3a0
Richard Roberts 2010-05-16 17:08:14 +00:00
parent d5143347d9
commit 1c0dcbc438
2 changed files with 18 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
cpp/GaussianFactor.cpp
View File

@ -78,7 +78,7 @@ GaussianFactor::GaussianFactor(const boost::shared_ptr<GaussianConditional>& 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);
}
/* ************************************************************************* */

17
cpp/testGaussianFactor.cpp
View File

@ -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 )
{