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

Fixed error calculation with NonlinearConstraints - now use Unit noisemodels in NonlinearConstraint to prevent having the error forced to infinity

release/4.3a0
Alex Cunningham 2010-02-22 21:11:22 +00:00
parent ee9205f0fa
commit fbbdd0be3e
2 changed files with 8 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
cpp/NonlinearConstraint-inl.h
View File

@ -25,7 +25,7 @@ template <class Config>
NonlinearConstraint<Config>::NonlinearConstraint(const LagrangeKey& lagrange_key, NonlinearConstraint<Config>::NonlinearConstraint(const LagrangeKey& lagrange_key,
size_t dim_lagrange, size_t dim_lagrange,
bool isEquality) : bool isEquality) :
NonlinearFactor<Config>(noiseModel::Constrained::All(dim_lagrange)), NonlinearFactor<Config>(noiseModel::Unit::Create(dim_lagrange)),
lagrange_key_(lagrange_key), p_(dim_lagrange), isEquality_(isEquality) { lagrange_key_(lagrange_key), p_(dim_lagrange), isEquality_(isEquality) {
this->keys_.push_back(lagrange_key_); this->keys_.push_back(lagrange_key_);
} }

10
cpp/testNonlinearConstraint.cpp
View File

@ -63,9 +63,13 @@ TEST( NonlinearConstraint1, unary_scalar_construction ) {
config.insert(x1, Vector_(1, 1.0)); config.insert(x1, Vector_(1, 1.0));
// calculate the error // calculate the error
Vector actual = c1.unwhitenedError(config); Vector actualVec = c1.unwhitenedError(config);
Vector expected = Vector_(1, -4.0); Vector expectedVec = Vector_(1, -4.0);
CHECK(assert_equal(actual, expected, 1e-5)); CHECK(assert_equal(actualVec, expectedVec, 1e-5));
double actError = c1.error(config);
double expError = 8.0;
DOUBLES_EQUAL(expError, actError, 1e-5);
} }
/* ************************************************************************* */ /* ************************************************************************* */