diff --git a/cpp/NonlinearConstraint-inl.h b/cpp/NonlinearConstraint-inl.h index 0b59f6c19..b237081b9 100644 --- a/cpp/NonlinearConstraint-inl.h +++ b/cpp/NonlinearConstraint-inl.h @@ -25,7 +25,7 @@ template NonlinearConstraint::NonlinearConstraint(const LagrangeKey& lagrange_key, size_t dim_lagrange, bool isEquality) : - NonlinearFactor(noiseModel::Constrained::All(dim_lagrange)), + NonlinearFactor(noiseModel::Unit::Create(dim_lagrange)), lagrange_key_(lagrange_key), p_(dim_lagrange), isEquality_(isEquality) { this->keys_.push_back(lagrange_key_); } diff --git a/cpp/testNonlinearConstraint.cpp b/cpp/testNonlinearConstraint.cpp index 86edf7809..5c59795e1 100644 --- a/cpp/testNonlinearConstraint.cpp +++ b/cpp/testNonlinearConstraint.cpp @@ -63,9 +63,13 @@ TEST( NonlinearConstraint1, unary_scalar_construction ) { config.insert(x1, Vector_(1, 1.0)); // calculate the error - Vector actual = c1.unwhitenedError(config); - Vector expected = Vector_(1, -4.0); - CHECK(assert_equal(actual, expected, 1e-5)); + Vector actualVec = c1.unwhitenedError(config); + Vector expectedVec = Vector_(1, -4.0); + CHECK(assert_equal(actualVec, expectedVec, 1e-5)); + + double actError = c1.error(config); + double expError = 8.0; + DOUBLES_EQUAL(expError, actError, 1e-5); } /* ************************************************************************* */