From a3ce3f31c8d0d31a7041347bc011c716504418ce Mon Sep 17 00:00:00 2001 From: Alex Cunningham Date: Mon, 30 Nov 2009 17:36:34 +0000 Subject: [PATCH] Cleanup with NonlinearConstraints to make the active() function do thresholding for inequality constraints, rather than constraint function itself. testSQP now has all tests active and passing. Added greaterThanOrEqual() for vector comparison. --- cpp/NonlinearConstraint-inl.h | 5 +--- cpp/NonlinearOptimizer-inl.h | 2 +- cpp/NonlinearOptimizer.h | 2 +- cpp/SQPOptimizer-inl.h | 10 +++---- cpp/Vector.cpp | 11 +++++++ cpp/Vector.h | 7 +++++ cpp/testSQP.cpp | 56 ++++++++++------------------------- cpp/testSQPOptimizer.cpp | 7 ++--- cpp/testVector.cpp | 9 ++++++ 9 files changed, 53 insertions(+), 56 deletions(-) diff --git a/cpp/NonlinearConstraint-inl.h b/cpp/NonlinearConstraint-inl.h index cbb06cf4b..82dd29be9 100644 --- a/cpp/NonlinearConstraint-inl.h +++ b/cpp/NonlinearConstraint-inl.h @@ -14,10 +14,7 @@ namespace gtsam { /* ************************************************************************* */ template bool NonlinearConstraint::active(const Config& config) const { - if (!isEquality_ && zero(error_vector(config))) - return false; - else - return true; + return !(!isEquality_ && greaterThanOrEqual(error_vector(config), zero(p_))); } /* ************************************************************************* */ diff --git a/cpp/NonlinearOptimizer-inl.h b/cpp/NonlinearOptimizer-inl.h index c601aa477..e76c104cf 100644 --- a/cpp/NonlinearOptimizer-inl.h +++ b/cpp/NonlinearOptimizer-inl.h @@ -134,7 +134,7 @@ namespace gtsam { delta.print("delta"); // update config - shared_config newConfig(new C(config_->exmap(delta))); + shared_config newConfig(new C(config_->exmap(delta))); // TODO: updateConfig if (verbosity >= TRYCONFIG) newConfig->print("config"); diff --git a/cpp/NonlinearOptimizer.h b/cpp/NonlinearOptimizer.h index c41d35b61..36a99b17a 100644 --- a/cpp/NonlinearOptimizer.h +++ b/cpp/NonlinearOptimizer.h @@ -38,10 +38,10 @@ namespace gtsam { LAMBDA, CONFIG, DELTA, - LINEAR, TRYLAMBDA, TRYCONFIG, TRYDELTA, + LINEAR, DAMPED }; diff --git a/cpp/SQPOptimizer-inl.h b/cpp/SQPOptimizer-inl.h index 6111ca310..a7d5fd086 100644 --- a/cpp/SQPOptimizer-inl.h +++ b/cpp/SQPOptimizer-inl.h @@ -95,7 +95,7 @@ SQPOptimizer SQPOptimizer::iterate(Verbosity v) const { GaussianFactorGraph fg; // prepare an ordering of lagrange multipliers to remove - Ordering rem; + Ordering keysToRemove; // iterate over all factors and linearize for (const_iterator factor = graph_->begin(); factor < graph_->end(); factor++) { @@ -115,22 +115,22 @@ SQPOptimizer SQPOptimizer::iterate(Verbosity v) const { fg.push_back(c); } else { if (verbose) constraint->print("Skipping..."); - rem += constraint->lagrangeKey(); + keysToRemove += constraint->lagrangeKey(); } } if (verbose) fg.print("Before Optimization"); // optimize linear graph to get full delta config - VectorConfig delta = fg.optimize(full_ordering_.subtract(rem)); + VectorConfig delta = fg.optimize(full_ordering_.subtract(keysToRemove)); if (verbose) delta.print("Delta Config"); // update both state variables shared_config newConfig(new C(config_->exmap(delta))); - shared_vconfig newLamConfig(new VectorConfig(lagrange_config_->exmap(delta))); + shared_vconfig newLambdas(new VectorConfig(lagrange_config_->exmap(delta))); // construct a new optimizer - return SQPOptimizer(*graph_, full_ordering_, newConfig, newLamConfig); + return SQPOptimizer(*graph_, full_ordering_, newConfig, newLambdas); } /* **************************************************************** */ diff --git a/cpp/Vector.cpp b/cpp/Vector.cpp index a414f2b27..ae6b99da4 100644 --- a/cpp/Vector.cpp +++ b/cpp/Vector.cpp @@ -111,6 +111,17 @@ namespace gtsam { return true; } + /* ************************************************************************* */ + bool greaterThanOrEqual(const Vector& vec1, const Vector& vec2) { + Vector::const_iterator it1 = vec1.begin(); + Vector::const_iterator it2 = vec2.begin(); + size_t m = vec1.size(); + for(size_t i=0; i= it2[i])) + return false; + return true; + } + /* ************************************************************************* */ bool equal_with_abs_tol(const Vector& vec1, const Vector& vec2, double tol) { Vector::const_iterator it1 = vec1.begin(); diff --git a/cpp/Vector.h b/cpp/Vector.h index 073480dd8..a7d37c744 100644 --- a/cpp/Vector.h +++ b/cpp/Vector.h @@ -87,6 +87,13 @@ void print(const Vector& v, const std::string& s = ""); */ bool operator==(const Vector& vec1,const Vector& vec2); +/** + * Greater than or equal to operation + * returns true if all elements in v1 + * are greater than corresponding elements in v2 + */ +bool greaterThanOrEqual(const Vector& v1, const Vector& v2); + /** * VecA == VecB up to tolerance */ diff --git a/cpp/testSQP.cpp b/cpp/testSQP.cpp index 9e9149b7f..3d57a072b 100644 --- a/cpp/testSQP.cpp +++ b/cpp/testSQP.cpp @@ -528,25 +528,16 @@ TEST (SQP, stereo_truth ) { } -// TEST DISABLED: optimization becomes unstable with one coordinate -// growing indefinitely and by the 4th iteration the system is -// underconstrained /** * Ground truth for a visual slam example with stereo vision * with some noise injected into the initial config */ TEST (SQP, stereo_truth_noisy ) { bool verbose = false; - int maxIt = 5; // setting to determine how far away the noisy landmark is, // given that the ground truth is 5m in front of the cameras - double noisyDist = 7.4; - /** RESULTS of optimization based on size of error: - * With a ground truth distance away of 5m, results based on initial input: - * >= 7.5m: the system diverges - * < 7.5m: system finds a solution with near zero error - */ + double noisyDist = 7.6; // create initial estimates Rot3 faceDownY(Matrix_(3,3, @@ -604,17 +595,9 @@ TEST (SQP, stereo_truth_noisy ) { if (verbose) cout << "Initial Error: " << optimizer.error() << endl; - // use Gauss-Newton optimization + // use Levenberg-Marquardt optimization double relThresh = 1e-5, absThresh = 1e-5; - optimizer = optimizer.gaussNewton(relThresh, absThresh); - -// // optimize -// for (int i = 0; iprint("Updated Config"); -// cout << "Error after iteration: " << optimizer.error() << endl; -// } + optimizer = optimizer.levenbergMarquardt(relThresh, absThresh, VOptimizer::SILENT); // verify DOUBLES_EQUAL(0.0, optimizer.error(), 1e-9); @@ -742,10 +725,6 @@ TEST (SQP, stereo_sqp ) { CHECK(assert_equal(*truthConfig,*(afterOneIteration.config()))); } - -// TEST DISABLED: There is a stability problem that sends the solution -// further away with every iteration, and eventually will result in the linearized -// system being underconstrained /** * SQP version of the above stereo example, * with noise in the initial estimate @@ -807,12 +786,9 @@ TEST (SQP, stereo_sqp_noisy ) { } // check if correct - //CHECK(assert_equal(*truthConfig,*(optimizer.config()))); + CHECK(assert_equal(*truthConfig,*(optimizer.config()))); } -// TEST DISABLED: There is a stability problem that sends the solution -// further away with every iteration, and eventually will result in the linearized -// system being underconstrained /** * SQP version of the above stereo example, * with noise in the initial estimate and manually specified @@ -855,17 +831,17 @@ TEST (SQP, stereo_sqp_noisy_manualLagrange ) { // optimize double start_error = optimizer.error(); int maxIt = 5; -// for (int i=0; iprint("Config Before Iteration"); -// optimizer.configLagrange()->print("Lagrange Before Iteration"); -// optimizer = optimizer.iterate(SOptimizer::FULL); -// } -// else -// optimizer = optimizer.iterate(SOptimizer::SILENT); -// } + for (int i=0; iprint("Config Before Iteration"); + optimizer.configLagrange()->print("Lagrange Before Iteration"); + optimizer = optimizer.iterate(SOptimizer::FULL); + } + else + optimizer = optimizer.iterate(SOptimizer::SILENT); + } if (verbose) cout << "Initial Error: " << start_error << "\n" << "Final Error: " << optimizer.error() << endl; @@ -880,7 +856,7 @@ TEST (SQP, stereo_sqp_noisy_manualLagrange ) { } // check if correct - //CHECK(assert_equal(*truthConfig,*(optimizer.config()))); + CHECK(assert_equal(*truthConfig,*(optimizer.config()))); } /* ************************************************************************* */ diff --git a/cpp/testSQPOptimizer.cpp b/cpp/testSQPOptimizer.cpp index 102348210..e6cedf6c0 100644 --- a/cpp/testSQPOptimizer.cpp +++ b/cpp/testSQPOptimizer.cpp @@ -224,15 +224,12 @@ namespace sqp_avoid1 { double radius = 1.0; // binary avoidance constraint -/** g(x) = ||x2-obs||^2 > radius^2 */ +/** g(x) = ||x2-obs||^2 - radius^2 > 0 */ Vector g_func(const VectorConfig& config, const list& keys) { Vector delta = config[keys.front()]-config[keys.back()]; double dist2 = sum(emul(delta, delta)); double thresh = radius*radius; - if (dist2 < thresh) - return Vector_(1, dist2-thresh); - else - return zero(1); + return Vector_(1, dist2-thresh); } /** gradient at pose */ diff --git a/cpp/testVector.cpp b/cpp/testVector.cpp index f6ac4c01a..7035d0f20 100644 --- a/cpp/testVector.cpp +++ b/cpp/testVector.cpp @@ -209,6 +209,15 @@ TEST( TestVector, equals ) CHECK(!equal_with_abs_tol(v1, v2, tol)); } +/* ************************************************************************* */ +TEST( TestVector, greater_than ) +{ + Vector v1 = Vector_(3, 1.0, 2.0, 3.0), + v2 = zero(3); + CHECK(greaterThanOrEqual(v1, v1)); // test basic greater than + CHECK(greaterThanOrEqual(v1, v2)); // test equals +} + /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr); } /* ************************************************************************* */