diff --git a/cpp/NonlinearOptimizer-inl.h b/cpp/NonlinearOptimizer-inl.h index ab1e4786e..e12497b8f 100644 --- a/cpp/NonlinearOptimizer-inl.h +++ b/cpp/NonlinearOptimizer-inl.h @@ -54,9 +54,17 @@ namespace gtsam { // linearize and optimize /* ************************************************************************* */ template - VectorConfig NonlinearOptimizer::delta() const { + VectorConfig NonlinearOptimizer::linearizeAndOptimizeForDelta() const { + + // linearize the non-linear graph around the current config + // which gives a linear optimization problem in the tangent space LinearFactorGraph linear = graph_->linearize(*config_); - return linear.optimize(*ordering_); + + // solve for the optimal displacement in the tangent space + VectorConfig delta = linear.optimize(*ordering_); + + // return + return delta; } /* ************************************************************************* */ @@ -67,7 +75,7 @@ namespace gtsam { verbosityLevel verbosity) const { // linearize and optimize - VectorConfig delta = this->delta(); + VectorConfig delta = linearizeAndOptimizeForDelta(); // maybe show output if (verbosity >= DELTA) diff --git a/cpp/NonlinearOptimizer.h b/cpp/NonlinearOptimizer.h index 21d003645..d900b9ef7 100644 --- a/cpp/NonlinearOptimizer.h +++ b/cpp/NonlinearOptimizer.h @@ -20,9 +20,9 @@ namespace gtsam { * and then one of the optimization routines is called. These recursively iterate * until convergence. All methods are functional and return a new state. * - * The class is parameterized by the Config class type, in order to optimize - * over non-vector configurations as well. - * To use in code, include in your cpp file + * The class is parameterized by the Graph type and Config class type, the latter + * in order to be able to optimize over non-vector configurations as well. + * To use in code, include in your cpp file * (the trick in http://www.ddj.com/cpp/184403420 did not work). */ template @@ -96,9 +96,9 @@ namespace gtsam { /** * linearize and optimize - * Thi returns an VectorConfig, i.e., vectors in tangent space of Config + * This returns an VectorConfig, i.e., vectors in tangent space of Config */ - VectorConfig delta() const; + VectorConfig linearizeAndOptimizeForDelta() const; /** * Do one Gauss-Newton iteration and return next state diff --git a/cpp/testNonlinearOptimizer.cpp b/cpp/testNonlinearOptimizer.cpp index 87730cada..d9024985c 100644 --- a/cpp/testNonlinearOptimizer.cpp +++ b/cpp/testNonlinearOptimizer.cpp @@ -47,7 +47,7 @@ TEST( NonlinearOptimizer, delta ) ord1.push_back("l1"); ord1.push_back("x1"); Optimizer optimizer1(fg, ord1, initial); - VectorConfig actual1 = optimizer1.delta(); + VectorConfig actual1 = optimizer1.linearizeAndOptimizeForDelta(); CHECK(assert_equal(actual1,expected)); // Check another @@ -56,7 +56,7 @@ TEST( NonlinearOptimizer, delta ) ord2.push_back("x2"); ord2.push_back("l1"); Optimizer optimizer2(fg, ord2, initial); - VectorConfig actual2 = optimizer2.delta(); + VectorConfig actual2 = optimizer2.linearizeAndOptimizeForDelta(); CHECK(assert_equal(actual2,expected)); // And yet another... @@ -65,7 +65,7 @@ TEST( NonlinearOptimizer, delta ) ord3.push_back("x1"); ord3.push_back("x2"); Optimizer optimizer3(fg, ord3, initial); - VectorConfig actual3 = optimizer3.delta(); + VectorConfig actual3 = optimizer3.linearizeAndOptimizeForDelta(); CHECK(assert_equal(actual3,expected)); }