From 07044137ebbd054822612eabe60d8fb0ae7154b0 Mon Sep 17 00:00:00 2001 From: Paul Furgale Date: Mon, 24 Nov 2014 21:14:59 +0100 Subject: [PATCH 1/7] A failing unit test for a custom chart --- gtsam/nonlinear/Values-inl.h | 8 +- gtsam/nonlinear/Values.h | 8 +- gtsam_unstable/nonlinear/ExpressionFactor.h | 15 ++- .../tests/testCustomChartExpression.cpp | 119 ++++++++++++++++++ 4 files changed, 139 insertions(+), 11 deletions(-) create mode 100644 gtsam_unstable/nonlinear/tests/testCustomChartExpression.cpp diff --git a/gtsam/nonlinear/Values-inl.h b/gtsam/nonlinear/Values-inl.h index 4595a70ed..e983ccb17 100644 --- a/gtsam/nonlinear/Values-inl.h +++ b/gtsam/nonlinear/Values-inl.h @@ -302,8 +302,8 @@ namespace gtsam { } // overloaded insert with chart initializer - template - void Values::insert(Key j, const ValueType& val, ChartInit chart) { + template + void Values::insert(Key j, const ValueType& val, Chart chart) { insert(j, static_cast(ChartValue(val, chart))); } @@ -320,8 +320,8 @@ namespace gtsam { } // update with chart initializer, /todo: perhaps there is a way to init chart from old value... - template - void Values::update(Key j, const ValueType& val, ChartInit chart) { + template + void Values::update(Key j, const ValueType& val, Chart chart) { update(j, static_cast(ChartValue(val, chart))); } diff --git a/gtsam/nonlinear/Values.h b/gtsam/nonlinear/Values.h index e4a27849d..d00baa0d9 100644 --- a/gtsam/nonlinear/Values.h +++ b/gtsam/nonlinear/Values.h @@ -263,8 +263,8 @@ namespace gtsam { void insert(Key j, const ValueType& val); /// overloaded insert version that also specifies a chart initializer - template - void insert(Key j, const ValueType& val, ChartInit chart); + template + void insert(Key j, const ValueType& val, Chart chart); /** insert that mimics the STL map insert - if the value already exists, the map is not modified @@ -288,8 +288,8 @@ namespace gtsam { void update(Key j, const T& val); /// overloaded insert version that also specifies a chart initializer - template - void update(Key j, const T& val, ChartInit chart); + template + void update(Key j, const T& val, Chart chart); /** update the current available values without adding new ones */ void update(const Values& values); diff --git a/gtsam_unstable/nonlinear/ExpressionFactor.h b/gtsam_unstable/nonlinear/ExpressionFactor.h index 22a2aefeb..273fb4c12 100644 --- a/gtsam_unstable/nonlinear/ExpressionFactor.h +++ b/gtsam_unstable/nonlinear/ExpressionFactor.h @@ -55,7 +55,7 @@ public: noiseModel_ = noiseModel; // Get dimensions of Jacobian matrices - // An Expression is assumed unmutable, so we do this now + // An Expression is assumed immutable, so we do this now std::map map; expression_.dims(map); size_t n = map.size(); @@ -83,12 +83,12 @@ public: */ virtual Vector unwhitenedError(const Values& x, boost::optional&> H = boost::none) const { - // TODO(PTF) Is this a place for custom charts? DefaultChart chart; if (H) { // H should be pre-allocated assert(H->size()==size()); + // todo...fix for custom charts. VerticalBlockMatrix Ab(dimensions_, Dim); // Wrap keys and VerticalBlockMatrix into structure passed to expression_ @@ -116,11 +116,20 @@ public: if (!active(x)) return boost::shared_ptr(); + // For custom charts, we would have to update the dimensions here + // based on the values that have been passed in. + FastVector::const_iterator kit = keys_.begin(); + std::vector dims; + dims.reserve(keys_.size()); + for(; kit != keys_.end(); ++kit) { + dims.push_back(x.at(*kit).dim()); + } + // Create a writeable JacobianFactor in advance // In case noise model is constrained, we need to provide a noise model bool constrained = noiseModel_->is_constrained(); boost::shared_ptr factor( - constrained ? new JacobianFactor(keys_, dimensions_, Dim, + constrained ? new JacobianFactor(keys_, dims, Dim, boost::static_pointer_cast(noiseModel_)->unit()) : new JacobianFactor(keys_, dimensions_, Dim)); diff --git a/gtsam_unstable/nonlinear/tests/testCustomChartExpression.cpp b/gtsam_unstable/nonlinear/tests/testCustomChartExpression.cpp new file mode 100644 index 000000000..59df89b32 --- /dev/null +++ b/gtsam_unstable/nonlinear/tests/testCustomChartExpression.cpp @@ -0,0 +1,119 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------1------------------------------------------- */ + +/** + * @file testExpression.cpp + * @date September 18, 2014 + * @author Frank Dellaert + * @author Paul Furgale + * @brief unit tests for Block Automatic Differentiation + */ + +#include +#include +#include +#include +#include + +#include + +#include +using boost::assign::list_of; +using boost::assign::map_list_of; + +using namespace std; +using namespace gtsam; + +// Dynamically sized Vector + +struct ProjectionChart { + typedef Eigen::Vector3d type; + typedef Eigen::Vector2d vector; + enum { dimension = 2 }; + static vector local(const type& origin, const type& other) { + return (other - origin).head<2>(); + } + static type retract(const type& origin, const vector& d) { + return origin + Eigen::Vector3d(2.0 * d[0], 3.0 * d[1], 0.0); + } + static int getDimension(const type& origin) { + return 2; + } +}; + +namespace gtsam { +namespace traits { +template<> struct is_chart : public boost::true_type {}; +template<> struct dimension : public boost::integral_constant {}; +} // namespace traits +} // namespace gtsam + +TEST(ExpressionCustomChart, projection) { + std::cout << "Hello!\n"; + // Create expression + Expression point(1); + + Eigen::Vector3d pval(1.0, 2.0, 3.0); + Values standard; + standard.insert(1, pval); + + Values custom; + custom.insert(1, pval, ProjectionChart()); + + Eigen::Vector3d pstandard = point.value(standard); + Eigen::Vector3d pcustom = point.value(custom); + + // The values should be the same. + EXPECT(assert_equal(Matrix(pstandard), Matrix(pcustom), 1e-10)); + + + EXPECT_LONGS_EQUAL(3, standard.at(1).dim()); + VectorValues vstandard = standard.zeroVectors(); + EXPECT_LONGS_EQUAL(3, vstandard.at(1).size()); + + + EXPECT_LONGS_EQUAL(2, custom.at(1).dim()); + VectorValues vcustom = custom.zeroVectors(); + EXPECT_LONGS_EQUAL(2, vcustom.at(1).size()); + + ExpressionFactor f(noiseModel::Unit::Create(pval.size()), pval, point); + + boost::shared_ptr gfstandard = f.linearize(standard); + boost::shared_ptr jfstandard = // + boost::dynamic_pointer_cast(gfstandard); + + typedef std::pair Jacobian; + Jacobian Jstandard = jfstandard->jacobianUnweighted(); + EXPECT(assert_equal(Eigen::Matrix3d::Identity(), Jstandard.first, 1e-10)); + + boost::shared_ptr gfcustom = f.linearize(custom); + boost::shared_ptr jfcustom = // + boost::dynamic_pointer_cast(gfcustom); + + Eigen::MatrixXd expectedJacobian = Eigen::MatrixXd::Zero(3,2); + expectedJacobian(0,0) = 2.0; + expectedJacobian(1,1) = 3.0; + Jacobian Jcustom = jfcustom->jacobianUnweighted(); + EXPECT(assert_equal(expectedJacobian, Jcustom.first, 1e-10)); + + // Amazingly, the finite differences get the expected Jacobian right. + const double fd_step = 1e-5; + const double tolerance = 1e-5; + EXPECT_CORRECT_EXPRESSION_JACOBIANS(point, custom, fd_step, tolerance); +} + +/* ************************************************************************* */ +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +/* ************************************************************************* */ + From 5b5122d15b0cbc04b30757f725a3b9d8e13ecb22 Mon Sep 17 00:00:00 2001 From: Paul Furgale Date: Mon, 24 Nov 2014 21:23:53 +0100 Subject: [PATCH 2/7] Fixed a copy/paste error --- gtsam_unstable/nonlinear/tests/testCustomChartExpression.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam_unstable/nonlinear/tests/testCustomChartExpression.cpp b/gtsam_unstable/nonlinear/tests/testCustomChartExpression.cpp index 59df89b32..bfd2574ea 100644 --- a/gtsam_unstable/nonlinear/tests/testCustomChartExpression.cpp +++ b/gtsam_unstable/nonlinear/tests/testCustomChartExpression.cpp @@ -10,7 +10,7 @@ * -------------------------------1------------------------------------------- */ /** - * @file testExpression.cpp + * @file testCustomChartExpression.cpp * @date September 18, 2014 * @author Frank Dellaert * @author Paul Furgale From e413fb3d68c700e60538b954be0889b5facd1d9e Mon Sep 17 00:00:00 2001 From: dellaert Date: Tue, 25 Nov 2014 00:18:42 +0100 Subject: [PATCH 3/7] target --- .cproject | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/.cproject b/.cproject index 1dcc51dfe..d42d2b0e0 100644 --- a/.cproject +++ b/.cproject @@ -2777,6 +2777,14 @@ true true + + make + -j4 + testCustomChartExpression.run + true + true + true + make -j2 From 4790bade85e14f609235264a59776d8587beecc0 Mon Sep 17 00:00:00 2001 From: dellaert Date: Tue, 25 Nov 2014 00:19:01 +0100 Subject: [PATCH 4/7] Moved and refactored testing --- gtsam/base/numericalDerivative.h | 66 --------------- gtsam_unstable/nonlinear/expressionTesting.h | 87 ++++++++++++++++++-- 2 files changed, 78 insertions(+), 75 deletions(-) diff --git a/gtsam/base/numericalDerivative.h b/gtsam/base/numericalDerivative.h index 30136d9f2..f2c4566bb 100644 --- a/gtsam/base/numericalDerivative.h +++ b/gtsam/base/numericalDerivative.h @@ -524,71 +524,5 @@ inline Matrix numericalHessian323(double (*f)(const X1&, const X2&, const X3&), delta); } -// The benefit of this method is that it does not need to know what types are involved -// to evaluate the factor. If all the machinery of gtsam is working correctly, we should -// get the correct numerical derivatives out the other side. -template -JacobianFactor computeNumericalDerivativeJacobianFactor(const FactorType& factor, - const Values& values, - double fd_step) { - Eigen::VectorXd e = factor.unwhitenedError(values); - const size_t rows = e.size(); - - std::map jacobians; - typename FactorType::const_iterator key_it = factor.begin(); - VectorValues dX = values.zeroVectors(); - for(; key_it != factor.end(); ++key_it) { - size_t key = *key_it; - // Compute central differences using the values struct. - const size_t cols = dX.dim(key); - Matrix J = Matrix::Zero(rows, cols); - for(size_t col = 0; col < cols; ++col) { - Eigen::VectorXd dx = Eigen::VectorXd::Zero(cols); - dx[col] = fd_step; - dX[key] = dx; - Values eval_values = values.retract(dX); - Eigen::VectorXd left = factor.unwhitenedError(eval_values); - dx[col] = -fd_step; - dX[key] = dx; - eval_values = values.retract(dX); - Eigen::VectorXd right = factor.unwhitenedError(eval_values); - J.col(col) = (left - right) * (1.0/(2.0 * fd_step)); - } - jacobians[key] = J; - } - - // Next step...return JacobianFactor - return JacobianFactor(jacobians, -e); -} - -template -void testFactorJacobians(TestResult& result_, - const std::string& name_, - const FactorType& f, - const gtsam::Values& values, - double fd_step, - double tolerance) { - // Check linearization - JacobianFactor expected = computeNumericalDerivativeJacobianFactor(f, values, fd_step); - boost::shared_ptr gf = f.linearize(values); - boost::shared_ptr jf = // - boost::dynamic_pointer_cast(gf); - - typedef std::pair Jacobian; - Jacobian evalJ = jf->jacobianUnweighted(); - Jacobian estJ = expected.jacobianUnweighted(); - EXPECT(assert_equal(evalJ.first, estJ.first, tolerance)); - EXPECT(assert_equal(evalJ.second, Eigen::VectorXd::Zero(evalJ.second.size()), tolerance)); - EXPECT(assert_equal(estJ.second, Eigen::VectorXd::Zero(evalJ.second.size()), tolerance)); -} - } // namespace gtsam -/// \brief Check the Jacobians produced by a factor against finite differences. -/// \param factor The factor to test. -/// \param values Values filled in for testing the Jacobians. -/// \param numerical_derivative_step The step to use when computing the numerical derivative Jacobians -/// \param tolerance The numerical tolerance to use when comparing Jacobians. -#define EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, numerical_derivative_step, tolerance) \ - { gtsam::testFactorJacobians(result_, name_, factor, values, numerical_derivative_step, tolerance); } - diff --git a/gtsam_unstable/nonlinear/expressionTesting.h b/gtsam_unstable/nonlinear/expressionTesting.h index ae2c1e7e5..92c8f71e8 100644 --- a/gtsam_unstable/nonlinear/expressionTesting.h +++ b/gtsam_unstable/nonlinear/expressionTesting.h @@ -30,19 +30,88 @@ namespace gtsam { +/** + * Linearize a nonlinear factor using numerical differentiation + * The benefit of this method is that it does not need to know what types are + * involved to evaluate the factor. If all the machinery of gtsam is working + * correctly, we should get the correct numerical derivatives out the other side. + */ +JacobianFactor linearizeNumerically(const NoiseModelFactor& factor, + const Values& values, double delta = 1e-5) { + + // We will fill a map of Jacobians + std::map jacobians; + + // Get size + Eigen::VectorXd e = factor.whitenedError(values); + const size_t rows = e.size(); + + // Loop over all variables + VectorValues dX = values.zeroVectors(); + BOOST_FOREACH(Key key, factor) { + // Compute central differences using the values struct. + const size_t cols = dX.dim(key); + Matrix J = Matrix::Zero(rows, cols); + for (size_t col = 0; col < cols; ++col) { + Eigen::VectorXd dx = Eigen::VectorXd::Zero(cols); + dx[col] = delta; + dX[key] = dx; + Values eval_values = values.retract(dX); + Eigen::VectorXd left = factor.whitenedError(eval_values); + dx[col] = -delta; + dX[key] = dx; + eval_values = values.retract(dX); + Eigen::VectorXd right = factor.whitenedError(eval_values); + J.col(col) = (left - right) * (1.0 / (2.0 * delta)); + } + jacobians[key] = J; + } + + // Next step...return JacobianFactor + return JacobianFactor(jacobians, -e); +} + +namespace internal { +// CPPUnitLite-style test for linearization of a factor +void testFactorJacobians(TestResult& result_, const std::string& name_, + const NoiseModelFactor& factor, const gtsam::Values& values, double delta, + double tolerance) { + + // Create expected value by numerical differentiation + JacobianFactor expected = linearizeNumerically(factor, values, delta); + + // Create actual value by linearize + boost::shared_ptr actual = // + boost::dynamic_pointer_cast(factor.linearize(values)); + + // Check cast result and then equality + CHECK(actual); + EXPECT( assert_equal(expected, *actual, tolerance)); +} +} + +/// \brief Check the Jacobians produced by a factor against finite differences. +/// \param factor The factor to test. +/// \param values Values filled in for testing the Jacobians. +/// \param numerical_derivative_step The step to use when computing the numerical derivative Jacobians +/// \param tolerance The numerical tolerance to use when comparing Jacobians. +#define EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, numerical_derivative_step, tolerance) \ + { gtsam::internal::testFactorJacobians(result_, name_, factor, values, numerical_derivative_step, tolerance); } + +namespace internal { +// CPPUnitLite-style test for linearization of an ExpressionFactor template -void testExpressionJacobians(TestResult& result_, - const std::string& name_, - const gtsam::Expression& expression, - const gtsam::Values& values, - double nd_step, - double tolerance) { +void testExpressionJacobians(TestResult& result_, const std::string& name_, + const gtsam::Expression& expression, const gtsam::Values& values, + double nd_step, double tolerance) { // Create factor size_t size = traits::dimension::value; - ExpressionFactor f(noiseModel::Unit::Create(size), expression.value(values), expression); + ExpressionFactor f(noiseModel::Unit::Create(size), + expression.value(values), expression); testFactorJacobians(result_, name_, f, values, nd_step, tolerance); } -} // namespace gtsam +} +} // namespace gtsam /// \brief Check the Jacobians produced by an expression against finite differences. /// \param expression The expression to test. @@ -50,4 +119,4 @@ void testExpressionJacobians(TestResult& result_, /// \param numerical_derivative_step The step to use when computing the finite difference Jacobians /// \param tolerance The numerical tolerance to use when comparing Jacobians. #define EXPECT_CORRECT_EXPRESSION_JACOBIANS(expression, values, numerical_derivative_step, tolerance) \ - { gtsam::testExpressionJacobians(result_, name_, expression, values, numerical_derivative_step, tolerance); } + { gtsam::internal::testExpressionJacobians(result_, name_, expression, values, numerical_derivative_step, tolerance); } From 14b582d268bc45661824b04c8978deac597ee0e4 Mon Sep 17 00:00:00 2001 From: Paul Furgale Date: Tue, 25 Nov 2014 06:42:36 +0100 Subject: [PATCH 5/7] cleaned up the code a bit --- .../nonlinear/tests/testCustomChartExpression.cpp | 12 ++++++++---- 1 file changed, 8 insertions(+), 4 deletions(-) diff --git a/gtsam_unstable/nonlinear/tests/testCustomChartExpression.cpp b/gtsam_unstable/nonlinear/tests/testCustomChartExpression.cpp index bfd2574ea..02c1803b6 100644 --- a/gtsam_unstable/nonlinear/tests/testCustomChartExpression.cpp +++ b/gtsam_unstable/nonlinear/tests/testCustomChartExpression.cpp @@ -32,12 +32,17 @@ using boost::assign::map_list_of; using namespace std; using namespace gtsam; -// Dynamically sized Vector - +// A simple prototype custom chart that does two things: +// 1. it reduces the dimension of the variable being estimated +// 2. it scales the input to retract. +// +// The Jacobian of this chart is: +// [ 2 0 ] +// [ 0 3 ] +// [ 0 0 ] struct ProjectionChart { typedef Eigen::Vector3d type; typedef Eigen::Vector2d vector; - enum { dimension = 2 }; static vector local(const type& origin, const type& other) { return (other - origin).head<2>(); } @@ -57,7 +62,6 @@ template<> struct dimension : public boost::integral_constant point(1); From 915c7605240a122b29591cf003cd8b3d03908a72 Mon Sep 17 00:00:00 2001 From: Paul Furgale Date: Tue, 25 Nov 2014 06:43:55 +0100 Subject: [PATCH 6/7] cleaned up the code a bit --- gtsam_unstable/nonlinear/tests/testCustomChartExpression.cpp | 5 ----- 1 file changed, 5 deletions(-) diff --git a/gtsam_unstable/nonlinear/tests/testCustomChartExpression.cpp b/gtsam_unstable/nonlinear/tests/testCustomChartExpression.cpp index 02c1803b6..bc2055c55 100644 --- a/gtsam_unstable/nonlinear/tests/testCustomChartExpression.cpp +++ b/gtsam_unstable/nonlinear/tests/testCustomChartExpression.cpp @@ -25,11 +25,6 @@ #include -#include -using boost::assign::list_of; -using boost::assign::map_list_of; - -using namespace std; using namespace gtsam; // A simple prototype custom chart that does two things: From 084de3350e3925684d590293b19fcab96e9791fd Mon Sep 17 00:00:00 2001 From: dellaert Date: Tue, 25 Nov 2014 08:47:25 +0100 Subject: [PATCH 7/7] 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); +} +//******************************************************************************