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); } /* ************************************************************************* */