From ae681cebb15d1aad5f8d38dfa58cd66aa2b9f636 Mon Sep 17 00:00:00 2001 From: dellaert Date: Tue, 25 Nov 2014 08:47:25 +0100 Subject: [PATCH] Formatting --- gtsam/nonlinear/NonlinearEquality.h | 489 +++++++++++++++------------- tests/testNonlinearEquality.cpp | 175 +++++----- 2 files changed, 354 insertions(+), 310 deletions(-) diff --git a/gtsam/nonlinear/NonlinearEquality.h b/gtsam/nonlinear/NonlinearEquality.h index 5d3319d97..9e3230f34 100644 --- a/gtsam/nonlinear/NonlinearEquality.h +++ b/gtsam/nonlinear/NonlinearEquality.h @@ -26,280 +26,311 @@ namespace gtsam { +/** + * Template default compare function that assumes a testable T + */ +template +bool compare(const T& a, const T& b) { + GTSAM_CONCEPT_TESTABLE_TYPE(T); + return a.equals(b); +} + +/** + * An equality factor that forces either one variable to a constant, + * or a set of variables to be equal to each other. + * + * Depending on flag, throws an error at linearization if the constraints are not met. + * + * Switchable implementation: + * - ALLLOW_ERROR : if we allow that there can be nonzero error, does not throw, and uses gain + * - ONLY_EXACT : throws error at linearization if not at exact feasible point, and infinite error + * + * \nosubgrouping + */ +template +class NonlinearEquality: public NoiseModelFactor1 { + +public: + typedef VALUE T; + +private: + + // feasible value + T feasible_; + + // error handling flag + bool allow_error_; + + // error gain in allow error case + double error_gain_; + + // typedef to this class + typedef NonlinearEquality This; + + // typedef to base class + typedef NoiseModelFactor1 Base; + +public: + /** - * Template default compare function that assumes a testable T + * Function that compares two values */ - template - bool compare(const T& a, const T& b) { - GTSAM_CONCEPT_TESTABLE_TYPE(T); - return a.equals(b); + bool (*compare_)(const T& a, const T& b); + + /** default constructor - only for serialization */ + NonlinearEquality() { + } + + virtual ~NonlinearEquality() { + } + + /// @name Standard Constructors + /// @{ + + /** + * Constructor - forces exact evaluation + */ + NonlinearEquality(Key j, const T& feasible, + bool (*_compare)(const T&, const T&) = compare) : + Base(noiseModel::Constrained::All(feasible.dim()), j), feasible_( + feasible), allow_error_(false), error_gain_(0.0), compare_(_compare) { } /** - * An equality factor that forces either one variable to a constant, - * or a set of variables to be equal to each other. - * - * Depending on flag, throws an error at linearization if the constraints are not met. - * - * Switchable implementation: - * - ALLLOW_ERROR : if we allow that there can be nonzero error, does not throw, and uses gain - * - ONLY_EXACT : throws error at linearization if not at exact feasible point, and infinite error - * - * \nosubgrouping + * Constructor - allows inexact evaluation */ - template - class NonlinearEquality: public NoiseModelFactor1 { + NonlinearEquality(Key j, const T& feasible, double error_gain, + bool (*_compare)(const T&, const T&) = compare) : + Base(noiseModel::Constrained::All(feasible.dim()), j), feasible_( + feasible), allow_error_(true), error_gain_(error_gain), compare_( + _compare) { + } - public: - typedef VALUE T; + /// @} + /// @name Testable + /// @{ - private: + virtual void print(const std::string& s = "", + const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { + std::cout << s << "Constraint: on [" << keyFormatter(this->key()) << "]\n"; + gtsam::print(feasible_, "Feasible Point:\n"); + std::cout << "Variable Dimension: " << feasible_.dim() << std::endl; + } - // feasible value - T feasible_; + /** Check if two factors are equal */ + virtual bool equals(const NonlinearFactor& f, double tol = 1e-9) const { + const This* e = dynamic_cast(&f); + return e && Base::equals(f) && feasible_.equals(e->feasible_, tol) + && fabs(error_gain_ - e->error_gain_) < tol; + } - // error handling flag - bool allow_error_; + /// @} + /// @name Standard Interface + /// @{ - // error gain in allow error case - double error_gain_; - - // typedef to this class - typedef NonlinearEquality This; - - // typedef to base class - typedef NoiseModelFactor1 Base; - - public: - - /** - * Function that compares two values - */ - bool (*compare_)(const T& a, const T& b); - - - /** default constructor - only for serialization */ - NonlinearEquality() {} - - virtual ~NonlinearEquality() {} - - /// @name Standard Constructors - /// @{ - - /** - * Constructor - forces exact evaluation - */ - NonlinearEquality(Key j, const T& feasible, bool (*_compare)(const T&, const T&) = compare) : - Base(noiseModel::Constrained::All(feasible.dim()), j), feasible_(feasible), - allow_error_(false), error_gain_(0.0), - compare_(_compare) { + /** actual error function calculation */ + virtual double error(const Values& c) const { + const T& xj = c.at(this->key()); + Vector e = this->unwhitenedError(c); + if (allow_error_ || !compare_(xj, feasible_)) { + return error_gain_ * dot(e, e); + } else { + return 0.0; } + } - /** - * Constructor - allows inexact evaluation - */ - NonlinearEquality(Key j, const T& feasible, double error_gain, bool (*_compare)(const T&, const T&) = compare) : - Base(noiseModel::Constrained::All(feasible.dim()), j), feasible_(feasible), - allow_error_(true), error_gain_(error_gain), - compare_(_compare) { + /** error function */ + Vector evaluateError(const T& xj, + boost::optional H = boost::none) const { + size_t nj = feasible_.dim(); + if (allow_error_) { + if (H) + *H = eye(nj); // FIXME: this is not the right linearization for nonlinear compare + return xj.localCoordinates(feasible_); + } else if (compare_(feasible_, xj)) { + if (H) + *H = eye(nj); + return zero(nj); // set error to zero if equal + } else { + if (H) + throw std::invalid_argument( + "Linearization point not feasible for " + + DefaultKeyFormatter(this->key()) + "!"); + return repeat(nj, std::numeric_limits::infinity()); // set error to infinity if not equal } + } - /// @} - /// @name Testable - /// @{ + // Linearize is over-written, because base linearization tries to whiten + virtual GaussianFactor::shared_ptr linearize(const Values& x) const { + const T& xj = x.at(this->key()); + Matrix A; + Vector b = evaluateError(xj, A); + SharedDiagonal model = noiseModel::Constrained::All(b.size()); + return GaussianFactor::shared_ptr( + new JacobianFactor(this->key(), A, b, model)); + } - virtual void print(const std::string& s = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { - std::cout << s << "Constraint: on [" << keyFormatter(this->key()) << "]\n"; - gtsam::print(feasible_,"Feasible Point:\n"); - std::cout << "Variable Dimension: " << feasible_.dim() << std::endl; - } + /// @return a deep copy of this factor + virtual gtsam::NonlinearFactor::shared_ptr clone() const { + return boost::static_pointer_cast( + gtsam::NonlinearFactor::shared_ptr(new This(*this))); + } - /** Check if two factors are equal */ - virtual bool equals(const NonlinearFactor& f, double tol = 1e-9) const { - const This* e = dynamic_cast(&f); - return e && Base::equals(f) && feasible_.equals(e->feasible_, tol) && - fabs(error_gain_ - e->error_gain_) < tol; - } + /// @} - /// @} - /// @name Standard Interface - /// @{ +private: - /** actual error function calculation */ - virtual double error(const Values& c) const { - const T& xj = c.at(this->key()); - Vector e = this->unwhitenedError(c); - if (allow_error_ || !compare_(xj, feasible_)) { - return error_gain_ * dot(e,e); - } else { - return 0.0; - } - } + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE & ar, const unsigned int version) { + ar + & boost::serialization::make_nvp("NoiseModelFactor1", + boost::serialization::base_object(*this)); + ar & BOOST_SERIALIZATION_NVP(feasible_); + ar & BOOST_SERIALIZATION_NVP(allow_error_); + ar & BOOST_SERIALIZATION_NVP(error_gain_); + } - /** error function */ - Vector evaluateError(const T& xj, boost::optional H = boost::none) const { - size_t nj = feasible_.dim(); - if (allow_error_) { - if (H) *H = eye(nj); // FIXME: this is not the right linearization for nonlinear compare - return xj.localCoordinates(feasible_); - } else if (compare_(feasible_,xj)) { - if (H) *H = eye(nj); - return zero(nj); // set error to zero if equal - } else { - if (H) throw std::invalid_argument( - "Linearization point not feasible for " + DefaultKeyFormatter(this->key()) + "!"); - return repeat(nj, std::numeric_limits::infinity()); // set error to infinity if not equal - } - } +}; +// \class NonlinearEquality - // Linearize is over-written, because base linearization tries to whiten - virtual GaussianFactor::shared_ptr linearize(const Values& x) const { - const T& xj = x.at(this->key()); - Matrix A; - Vector b = evaluateError(xj, A); - SharedDiagonal model = noiseModel::Constrained::All(b.size()); - return GaussianFactor::shared_ptr(new JacobianFactor(this->key(), A, b, model)); - } +/* ************************************************************************* */ +/** + * Simple unary equality constraint - fixes a value for a variable + */ +template +class NonlinearEquality1: public NoiseModelFactor1 { - /// @return a deep copy of this factor - virtual gtsam::NonlinearFactor::shared_ptr clone() const { - return boost::static_pointer_cast( - gtsam::NonlinearFactor::shared_ptr(new This(*this))); } +public: + typedef VALUE X; - /// @} +protected: + typedef NoiseModelFactor1 Base; + typedef NonlinearEquality1 This; - private: + /** default constructor to allow for serialization */ + NonlinearEquality1() { + } - /** Serialization function */ - friend class boost::serialization::access; - template - void serialize(ARCHIVE & ar, const unsigned int version) { - ar & boost::serialization::make_nvp("NoiseModelFactor1", - boost::serialization::base_object(*this)); - ar & BOOST_SERIALIZATION_NVP(feasible_); - ar & BOOST_SERIALIZATION_NVP(allow_error_); - ar & BOOST_SERIALIZATION_NVP(error_gain_); - } + X value_; /// fixed value for variable - }; // \class NonlinearEquality + GTSAM_CONCEPT_MANIFOLD_TYPE(X) + ;GTSAM_CONCEPT_TESTABLE_TYPE(X) + ; - /* ************************************************************************* */ - /** - * Simple unary equality constraint - fixes a value for a variable - */ - template - class NonlinearEquality1 : public NoiseModelFactor1 { +public: - public: - typedef VALUE X; + typedef boost::shared_ptr > shared_ptr; - protected: - typedef NoiseModelFactor1 Base; - typedef NonlinearEquality1 This; + ///TODO: comment + NonlinearEquality1(const X& value, Key key1, double mu = 1000.0) : + Base(noiseModel::Constrained::All(value.dim(), fabs(mu)), key1), value_( + value) { + } - /** default constructor to allow for serialization */ - NonlinearEquality1() {} + virtual ~NonlinearEquality1() { + } - X value_; /// fixed value for variable + /// @return a deep copy of this factor + virtual gtsam::NonlinearFactor::shared_ptr clone() const { + return boost::static_pointer_cast( + gtsam::NonlinearFactor::shared_ptr(new This(*this))); + } - GTSAM_CONCEPT_MANIFOLD_TYPE(X); - GTSAM_CONCEPT_TESTABLE_TYPE(X); + /** g(x) with optional derivative */ + Vector evaluateError(const X& x1, + boost::optional H = boost::none) const { + if (H) + (*H) = eye(x1.dim()); + // manifold equivalent of h(x)-z -> log(z,h(x)) + return value_.localCoordinates(x1); + } - public: + /** Print */ + virtual void print(const std::string& s = "", + const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { + std::cout << s << ": NonlinearEquality1(" << keyFormatter(this->key()) + << ")," << "\n"; + this->noiseModel_->print(); + value_.print("Value"); + } - typedef boost::shared_ptr > shared_ptr; +private: - ///TODO: comment - NonlinearEquality1(const X& value, Key key1, double mu = 1000.0) - : Base(noiseModel::Constrained::All(value.dim(), fabs(mu)), key1), value_(value) {} + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE & ar, const unsigned int version) { + ar + & boost::serialization::make_nvp("NoiseModelFactor1", + boost::serialization::base_object(*this)); + ar & BOOST_SERIALIZATION_NVP(value_); + } +}; +// \NonlinearEquality1 - virtual ~NonlinearEquality1() {} +/* ************************************************************************* */ +/** + * Simple binary equality constraint - this constraint forces two factors to + * be the same. + */ +template +class NonlinearEquality2: public NoiseModelFactor2 { +public: + typedef VALUE X; - /// @return a deep copy of this factor - virtual gtsam::NonlinearFactor::shared_ptr clone() const { - return boost::static_pointer_cast( - gtsam::NonlinearFactor::shared_ptr(new This(*this))); } +protected: + typedef NoiseModelFactor2 Base; + typedef NonlinearEquality2 This; - /** g(x) with optional derivative */ - Vector evaluateError(const X& x1, boost::optional H = boost::none) const { - if (H) (*H) = eye(x1.dim()); - // manifold equivalent of h(x)-z -> log(z,h(x)) - return value_.localCoordinates(x1); - } + GTSAM_CONCEPT_MANIFOLD_TYPE(X) + ; - /** Print */ - virtual void print(const std::string& s = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { - std::cout << s << ": NonlinearEquality1(" - << keyFormatter(this->key()) << "),"<< "\n"; - this->noiseModel_->print(); - value_.print("Value"); - } + /** default constructor to allow for serialization */ + NonlinearEquality2() { + } - private: +public: - /** Serialization function */ - friend class boost::serialization::access; - template - void serialize(ARCHIVE & ar, const unsigned int version) { - ar & boost::serialization::make_nvp("NoiseModelFactor1", - boost::serialization::base_object(*this)); - ar & BOOST_SERIALIZATION_NVP(value_); - } - }; // \NonlinearEquality1 + typedef boost::shared_ptr > shared_ptr; - /* ************************************************************************* */ - /** - * Simple binary equality constraint - this constraint forces two factors to - * be the same. - */ - template - class NonlinearEquality2 : public NoiseModelFactor2 { - public: - typedef VALUE X; + ///TODO: comment + NonlinearEquality2(Key key1, Key key2, double mu = 1000.0) : + Base(noiseModel::Constrained::All(X::Dim(), fabs(mu)), key1, key2) { + } + virtual ~NonlinearEquality2() { + } - protected: - typedef NoiseModelFactor2 Base; - typedef NonlinearEquality2 This; + /// @return a deep copy of this factor + virtual gtsam::NonlinearFactor::shared_ptr clone() const { + return boost::static_pointer_cast( + gtsam::NonlinearFactor::shared_ptr(new This(*this))); + } - GTSAM_CONCEPT_MANIFOLD_TYPE(X); + /** g(x) with optional derivative2 */ + Vector evaluateError(const X& x1, const X& x2, boost::optional H1 = + boost::none, boost::optional H2 = boost::none) const { + const size_t p = X::Dim(); + if (H1) + *H1 = -eye(p); + if (H2) + *H2 = eye(p); + return x1.localCoordinates(x2); + } - /** default constructor to allow for serialization */ - NonlinearEquality2() {} +private: - public: + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE & ar, const unsigned int version) { + ar + & boost::serialization::make_nvp("NoiseModelFactor2", + boost::serialization::base_object(*this)); + } +}; +// \NonlinearEquality2 - typedef boost::shared_ptr > shared_ptr; - - ///TODO: comment - NonlinearEquality2(Key key1, Key key2, double mu = 1000.0) - : Base(noiseModel::Constrained::All(X::Dim(), fabs(mu)), key1, key2) {} - virtual ~NonlinearEquality2() {} - - /// @return a deep copy of this factor - virtual gtsam::NonlinearFactor::shared_ptr clone() const { - return boost::static_pointer_cast( - gtsam::NonlinearFactor::shared_ptr(new This(*this))); } - - /** g(x) with optional derivative2 */ - Vector evaluateError(const X& x1, const X& x2, - boost::optional H1 = boost::none, - boost::optional H2 = boost::none) const { - const size_t p = X::Dim(); - if (H1) *H1 = -eye(p); - if (H2) *H2 = eye(p); - return x1.localCoordinates(x2); - } - - private: - - /** Serialization function */ - friend class boost::serialization::access; - template - void serialize(ARCHIVE & ar, const unsigned int version) { - ar & boost::serialization::make_nvp("NoiseModelFactor2", - boost::serialization::base_object(*this)); - } - }; // \NonlinearEquality2 - -} // namespace gtsam +}// namespace gtsam diff --git a/tests/testNonlinearEquality.cpp b/tests/testNonlinearEquality.cpp index 6366d9fa5..96039a3cc 100644 --- a/tests/testNonlinearEquality.cpp +++ b/tests/testNonlinearEquality.cpp @@ -42,9 +42,9 @@ typedef PriorFactor PosePrior; typedef NonlinearEquality PoseNLE; typedef boost::shared_ptr shared_poseNLE; -static Symbol key('x',1); +static Symbol key('x', 1); -/* ************************************************************************* */ +//****************************************************************************** TEST ( NonlinearEquality, linearization ) { Pose2 value = Pose2(2.1, 1.0, 2.0); Values linearize; @@ -60,10 +60,10 @@ TEST ( NonlinearEquality, linearization ) { EXPECT(assert_equal((const GaussianFactor&)expLF, *actualLF)); } -/* ********************************************************************** */ +//****************************************************************************** TEST ( NonlinearEquality, linearization_pose ) { - Symbol key('x',1); + Symbol key('x', 1); Pose2 value; Values config; config.insert(key, value); @@ -75,7 +75,7 @@ TEST ( NonlinearEquality, linearization_pose ) { EXPECT(true); } -/* ********************************************************************** */ +//****************************************************************************** TEST ( NonlinearEquality, linearization_fail ) { Pose2 value = Pose2(2.1, 1.0, 2.0); Pose2 wrong = Pose2(2.1, 3.0, 4.0); @@ -89,12 +89,11 @@ TEST ( NonlinearEquality, linearization_fail ) { CHECK_EXCEPTION(nle->linearize(bad_linearize), std::invalid_argument); } -/* ********************************************************************** */ +//****************************************************************************** TEST ( NonlinearEquality, linearization_fail_pose ) { - Symbol key('x',1); - Pose2 value(2.0, 1.0, 2.0), - wrong(2.0, 3.0, 4.0); + Symbol key('x', 1); + Pose2 value(2.0, 1.0, 2.0), wrong(2.0, 3.0, 4.0); Values bad_linearize; bad_linearize.insert(key, wrong); @@ -105,12 +104,11 @@ TEST ( NonlinearEquality, linearization_fail_pose ) { CHECK_EXCEPTION(nle->linearize(bad_linearize), std::invalid_argument); } -/* ********************************************************************** */ +//****************************************************************************** TEST ( NonlinearEquality, linearization_fail_pose_origin ) { - Symbol key('x',1); - Pose2 value, - wrong(2.0, 3.0, 4.0); + Symbol key('x', 1); + Pose2 value, wrong(2.0, 3.0, 4.0); Values bad_linearize; bad_linearize.insert(key, wrong); @@ -121,7 +119,7 @@ TEST ( NonlinearEquality, linearization_fail_pose_origin ) { CHECK_EXCEPTION(nle->linearize(bad_linearize), std::invalid_argument); } -/* ************************************************************************* */ +//****************************************************************************** TEST ( NonlinearEquality, error ) { Pose2 value = Pose2(2.1, 1.0, 2.0); Pose2 wrong = Pose2(2.1, 3.0, 4.0); @@ -137,10 +135,11 @@ TEST ( NonlinearEquality, error ) { EXPECT(assert_equal(actual, zero(3))); actual = nle->unwhitenedError(bad_linearize); - EXPECT(assert_equal(actual, repeat(3, std::numeric_limits::infinity()))); + EXPECT( + assert_equal(actual, repeat(3, std::numeric_limits::infinity()))); } -/* ************************************************************************* */ +//****************************************************************************** TEST ( NonlinearEquality, equals ) { Pose2 value1 = Pose2(2.1, 1.0, 2.0); Pose2 value2 = Pose2(2.1, 3.0, 4.0); @@ -151,14 +150,17 @@ TEST ( NonlinearEquality, equals ) { shared_poseNLE nle3(new PoseNLE(key, value2)); // verify - EXPECT(nle1->equals(*nle2)); // basic equality = true - EXPECT(nle2->equals(*nle1)); // test symmetry of equals() - EXPECT(!nle1->equals(*nle3)); // test config + EXPECT(nle1->equals(*nle2)); + // basic equality = true + EXPECT(nle2->equals(*nle1)); + // test symmetry of equals() + EXPECT(!nle1->equals(*nle3)); + // test config } -/* ************************************************************************* */ +//****************************************************************************** TEST ( NonlinearEquality, allow_error_pose ) { - Symbol key1('x',1); + Symbol key1('x', 1); Pose2 feasible1(1.0, 2.0, 3.0); double error_gain = 500.0; PoseNLE nle(key1, feasible1, error_gain); @@ -177,16 +179,17 @@ TEST ( NonlinearEquality, allow_error_pose ) { // check linearization GaussianFactor::shared_ptr actLinFactor = nle.linearize(config); - Matrix A1 = eye(3,3); + Matrix A1 = eye(3, 3); Vector b = expVec; SharedDiagonal model = noiseModel::Constrained::All(3); - GaussianFactor::shared_ptr expLinFactor(new JacobianFactor(key1, A1, b, model)); + GaussianFactor::shared_ptr expLinFactor( + new JacobianFactor(key1, A1, b, model)); EXPECT(assert_equal(*expLinFactor, *actLinFactor, 1e-5)); } -/* ************************************************************************* */ +//****************************************************************************** TEST ( NonlinearEquality, allow_error_optimize ) { - Symbol key1('x',1); + Symbol key1('x', 1); Pose2 feasible1(1.0, 2.0, 3.0); double error_gain = 500.0; PoseNLE nle(key1, feasible1, error_gain); @@ -211,11 +214,11 @@ TEST ( NonlinearEquality, allow_error_optimize ) { EXPECT(assert_equal(expected, result)); } -/* ************************************************************************* */ +//****************************************************************************** TEST ( NonlinearEquality, allow_error_optimize_with_factors ) { // create a hard constraint - Symbol key1('x',1); + Symbol key1('x', 1); Pose2 feasible1(1.0, 2.0, 3.0); // initialize away from the ideal @@ -245,14 +248,14 @@ TEST ( NonlinearEquality, allow_error_optimize_with_factors ) { EXPECT(assert_equal(expected, actual)); } -/* ************************************************************************* */ +//****************************************************************************** static SharedDiagonal hard_model = noiseModel::Constrained::All(2); static SharedDiagonal soft_model = noiseModel::Isotropic::Sigma(2, 1.0); -/* ************************************************************************* */ +//****************************************************************************** TEST( testNonlinearEqualityConstraint, unary_basics ) { Point2 pt(1.0, 2.0); - Symbol key1('x',1); + Symbol key1('x', 1); double mu = 1000.0; eq2D::UnaryEqualityConstraint constraint(pt, key, mu); @@ -267,38 +270,42 @@ TEST( testNonlinearEqualityConstraint, unary_basics ) { Point2 ptBad1(2.0, 2.0); config2.insert(key, ptBad1); EXPECT(constraint.active(config2)); - EXPECT(assert_equal(Vector2(1.0, 0.0), constraint.evaluateError(ptBad1), tol)); - EXPECT(assert_equal(Vector2(1.0, 0.0), constraint.unwhitenedError(config2), tol)); + EXPECT( + assert_equal(Vector2(1.0, 0.0), constraint.evaluateError(ptBad1), tol)); + EXPECT( + assert_equal(Vector2(1.0, 0.0), constraint.unwhitenedError(config2), tol)); EXPECT_DOUBLES_EQUAL(500.0, constraint.error(config2), tol); } -/* ************************************************************************* */ +//****************************************************************************** TEST( testNonlinearEqualityConstraint, unary_linearization ) { Point2 pt(1.0, 2.0); - Symbol key1('x',1); + Symbol key1('x', 1); double mu = 1000.0; eq2D::UnaryEqualityConstraint constraint(pt, key, mu); Values config1; config1.insert(key, pt); GaussianFactor::shared_ptr actual1 = constraint.linearize(config1); - GaussianFactor::shared_ptr expected1(new JacobianFactor(key, eye(2,2), zero(2), hard_model)); + GaussianFactor::shared_ptr expected1( + new JacobianFactor(key, eye(2, 2), zero(2), hard_model)); EXPECT(assert_equal(*expected1, *actual1, tol)); Values config2; Point2 ptBad(2.0, 2.0); config2.insert(key, ptBad); GaussianFactor::shared_ptr actual2 = constraint.linearize(config2); - GaussianFactor::shared_ptr expected2(new JacobianFactor(key, eye(2,2), Vector2(-1.0,0.0), hard_model)); + GaussianFactor::shared_ptr expected2( + new JacobianFactor(key, eye(2, 2), Vector2(-1.0, 0.0), hard_model)); EXPECT(assert_equal(*expected2, *actual2, tol)); } -/* ************************************************************************* */ +//****************************************************************************** TEST( testNonlinearEqualityConstraint, unary_simple_optimization ) { // create a single-node graph with a soft and hard constraint to // ensure that the hard constraint overrides the soft constraint Point2 truth_pt(1.0, 2.0); - Symbol key('x',1); + Symbol key('x', 1); double mu = 10.0; eq2D::UnaryEqualityConstraint::shared_ptr constraint( new eq2D::UnaryEqualityConstraint(truth_pt, key, mu)); @@ -326,10 +333,10 @@ TEST( testNonlinearEqualityConstraint, unary_simple_optimization ) { EXPECT(assert_equal(expected, actual, tol)); } -/* ************************************************************************* */ +//****************************************************************************** TEST( testNonlinearEqualityConstraint, odo_basics ) { Point2 x1(1.0, 2.0), x2(2.0, 3.0), odom(1.0, 1.0); - Symbol key1('x',1), key2('x',2); + Symbol key1('x', 1), key2('x', 2); double mu = 1000.0; eq2D::OdoEqualityConstraint constraint(odom, key1, key2, mu); @@ -347,15 +354,17 @@ TEST( testNonlinearEqualityConstraint, odo_basics ) { config2.insert(key1, x1bad); config2.insert(key2, x2bad); EXPECT(constraint.active(config2)); - EXPECT(assert_equal(Vector2(-1.0, -1.0), constraint.evaluateError(x1bad, x2bad), tol)); - EXPECT(assert_equal(Vector2(-1.0, -1.0), constraint.unwhitenedError(config2), tol)); + EXPECT( + assert_equal(Vector2(-1.0, -1.0), constraint.evaluateError(x1bad, x2bad), tol)); + EXPECT( + assert_equal(Vector2(-1.0, -1.0), constraint.unwhitenedError(config2), tol)); EXPECT_DOUBLES_EQUAL(1000.0, constraint.error(config2), tol); } -/* ************************************************************************* */ +//****************************************************************************** TEST( testNonlinearEqualityConstraint, odo_linearization ) { Point2 x1(1.0, 2.0), x2(2.0, 3.0), odom(1.0, 1.0); - Symbol key1('x',1), key2('x',2); + Symbol key1('x', 1), key2('x', 2); double mu = 1000.0; eq2D::OdoEqualityConstraint constraint(odom, key1, key2, mu); @@ -364,8 +373,8 @@ TEST( testNonlinearEqualityConstraint, odo_linearization ) { config1.insert(key2, x2); GaussianFactor::shared_ptr actual1 = constraint.linearize(config1); GaussianFactor::shared_ptr expected1( - new JacobianFactor(key1, -eye(2,2), key2, - eye(2,2), zero(2), hard_model)); + new JacobianFactor(key1, -eye(2, 2), key2, eye(2, 2), zero(2), + hard_model)); EXPECT(assert_equal(*expected1, *actual1, tol)); Values config2; @@ -375,18 +384,18 @@ TEST( testNonlinearEqualityConstraint, odo_linearization ) { config2.insert(key2, x2bad); GaussianFactor::shared_ptr actual2 = constraint.linearize(config2); GaussianFactor::shared_ptr expected2( - new JacobianFactor(key1, -eye(2,2), key2, - eye(2,2), Vector2(1.0, 1.0), hard_model)); + new JacobianFactor(key1, -eye(2, 2), key2, eye(2, 2), Vector2(1.0, 1.0), + hard_model)); EXPECT(assert_equal(*expected2, *actual2, tol)); } -/* ************************************************************************* */ +//****************************************************************************** TEST( testNonlinearEqualityConstraint, odo_simple_optimize ) { // create a two-node graph, connected by an odometry constraint, with // a hard prior on one variable, and a conflicting soft prior // on the other variable - the constraints should override the soft constraint Point2 truth_pt1(1.0, 2.0), truth_pt2(3.0, 2.0); - Symbol key1('x',1), key2('x',2); + Symbol key1('x', 1), key2('x', 2); // hard prior on x1 eq2D::UnaryEqualityConstraint::shared_ptr constraint1( @@ -399,8 +408,8 @@ TEST( testNonlinearEqualityConstraint, odo_simple_optimize ) { // odometry constraint eq2D::OdoEqualityConstraint::shared_ptr constraint2( - new eq2D::OdoEqualityConstraint( - truth_pt1.between(truth_pt2), key1, key2)); + new eq2D::OdoEqualityConstraint(truth_pt1.between(truth_pt2), key1, + key2)); NonlinearFactorGraph graph; graph.push_back(constraint1); @@ -418,7 +427,7 @@ TEST( testNonlinearEqualityConstraint, odo_simple_optimize ) { CHECK(assert_equal(expected, actual, tol)); } -/* ********************************************************************* */ +//****************************************************************************** TEST (testNonlinearEqualityConstraint, two_pose ) { /* * Determining a ground truth linear system @@ -428,19 +437,18 @@ TEST (testNonlinearEqualityConstraint, two_pose ) { NonlinearFactorGraph graph; - Symbol x1('x',1), x2('x',2); - Symbol l1('l',1), l2('l',2); - Point2 pt_x1(1.0, 1.0), - pt_x2(5.0, 6.0); + Symbol x1('x', 1), x2('x', 2); + Symbol l1('l', 1), l2('l', 2); + Point2 pt_x1(1.0, 1.0), pt_x2(5.0, 6.0); graph += eq2D::UnaryEqualityConstraint(pt_x1, x1); graph += eq2D::UnaryEqualityConstraint(pt_x2, x2); Point2 z1(0.0, 5.0); SharedNoiseModel sigma(noiseModel::Isotropic::Sigma(2, 0.1)); - graph += simulated2D::Measurement(z1, sigma, x1,l1); + graph += simulated2D::Measurement(z1, sigma, x1, l1); Point2 z2(-4.0, 0.0); - graph += simulated2D::Measurement(z2, sigma, x2,l2); + graph += simulated2D::Measurement(z2, sigma, x2, l2); graph += eq2D::PointEqualityConstraint(l1, l2); @@ -450,7 +458,8 @@ TEST (testNonlinearEqualityConstraint, two_pose ) { initialEstimate.insert(l1, Point2(1.0, 6.0)); // ground truth initialEstimate.insert(l2, Point2(-4.0, 0.0)); // starting with a separate reference frame - Values actual = LevenbergMarquardtOptimizer(graph, initialEstimate).optimize(); + Values actual = + LevenbergMarquardtOptimizer(graph, initialEstimate).optimize(); Values expected; expected.insert(x1, pt_x1); @@ -460,14 +469,14 @@ TEST (testNonlinearEqualityConstraint, two_pose ) { CHECK(assert_equal(expected, actual, 1e-5)); } -/* ********************************************************************* */ +//****************************************************************************** TEST (testNonlinearEqualityConstraint, map_warp ) { // get a graph NonlinearFactorGraph graph; // keys - Symbol x1('x',1), x2('x',2); - Symbol l1('l',1), l2('l',2); + Symbol x1('x', 1), x2('x', 2); + Symbol l1('l', 1), l2('l', 2); // constant constraint on x1 Point2 pose1(1.0, 1.0); @@ -488,13 +497,14 @@ TEST (testNonlinearEqualityConstraint, map_warp ) { // create an initial estimate Values initialEstimate; - initialEstimate.insert(x1, Point2( 1.0, 1.0)); - initialEstimate.insert(l1, Point2( 1.0, 6.0)); + initialEstimate.insert(x1, Point2(1.0, 1.0)); + initialEstimate.insert(l1, Point2(1.0, 6.0)); initialEstimate.insert(l2, Point2(-4.0, 0.0)); // starting with a separate reference frame - initialEstimate.insert(x2, Point2( 0.0, 0.0)); // other pose starts at origin + initialEstimate.insert(x2, Point2(0.0, 0.0)); // other pose starts at origin // optimize - Values actual = LevenbergMarquardtOptimizer(graph, initialEstimate).optimize(); + Values actual = + LevenbergMarquardtOptimizer(graph, initialEstimate).optimize(); Values expected; expected.insert(x1, Point2(1.0, 1.0)); @@ -506,8 +516,8 @@ TEST (testNonlinearEqualityConstraint, map_warp ) { // make a realistic calibration matrix static double fov = 60; // degrees -static int w=640,h=480; -static Cal3_S2 K(fov,w,h); +static int w = 640, h = 480; +static Cal3_S2 K(fov, w, h); static boost::shared_ptr shK(new Cal3_S2(K)); // typedefs for visual SLAM example @@ -516,14 +526,12 @@ typedef NonlinearFactorGraph VGraph; // factors for visual slam typedef NonlinearEquality2 Point3Equality; -/* ********************************************************************* */ +//****************************************************************************** TEST (testNonlinearEqualityConstraint, stereo_constrained ) { // create initial estimates - Rot3 faceDownY((Matrix)(Matrix(3,3) << - 1.0, 0.0, 0.0, - 0.0, 0.0, 1.0, - 0.0, -1.0, 0.0).finished()); + Rot3 faceDownY( + (Matrix) (Matrix(3, 3) << 1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, -1.0, 0.0).finished()); Pose3 pose1(faceDownY, Point3()); // origin, left camera SimpleCamera camera1(pose1, K); Pose3 pose2(faceDownY, Point3(2.0, 0.0, 0.0)); // 2 units to the left @@ -531,8 +539,8 @@ TEST (testNonlinearEqualityConstraint, stereo_constrained ) { Point3 landmark(1.0, 5.0, 0.0); //centered between the cameras, 5 units away // keys - Symbol x1('x',1), x2('x',2); - Symbol l1('l',1), l2('l',2); + Symbol x1('x', 1), x2('x', 2); + Symbol l1('l', 1), l2('l', 2); // create graph VGraph graph; @@ -543,8 +551,10 @@ TEST (testNonlinearEqualityConstraint, stereo_constrained ) { // create factors SharedDiagonal vmodel = noiseModel::Unit::Create(2); - graph += GenericProjectionFactor(camera1.project(landmark), vmodel, x1, l1, shK); - graph += GenericProjectionFactor(camera2.project(landmark), vmodel, x2, l2, shK); + graph += GenericProjectionFactor( + camera1.project(landmark), vmodel, x1, l1, shK); + graph += GenericProjectionFactor( + camera2.project(landmark), vmodel, x2, l2, shK); // add equality constraint graph += Point3Equality(l1, l2); @@ -573,6 +583,9 @@ TEST (testNonlinearEqualityConstraint, stereo_constrained ) { CHECK(assert_equal(truthValues, actual, 1e-5)); } -/* ************************************************************************* */ -int main() { TestResult tr; return TestRegistry::runAllTests(tr); } -/* ************************************************************************* */ +//****************************************************************************** +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +//******************************************************************************