From d340e556b58698facf9aeaf28007dc43134205d7 Mon Sep 17 00:00:00 2001 From: Alex Cunningham Date: Mon, 7 Nov 2011 21:16:26 +0000 Subject: [PATCH] Fixes due to bad merge --- gtsam.h | 2 +- gtsam/geometry/tests/testPose3.cpp | 6 +++--- gtsam/nonlinear/DoglegOptimizer.h | 2 +- gtsam/nonlinear/DoglegOptimizerImpl.h | 6 +++--- gtsam/nonlinear/Makefile.am | 4 ++-- gtsam/nonlinear/NonlinearOptimizer-inl.h | 2 +- tests/testDoglegOptimizer.cpp | 2 +- 7 files changed, 12 insertions(+), 12 deletions(-) diff --git a/gtsam.h b/gtsam.h index 3ff7d77e6..52422cf3c 100644 --- a/gtsam.h +++ b/gtsam.h @@ -40,7 +40,7 @@ class Pose2 { int dim() const; Pose2* compose_(const Pose2& p2); Pose2* between_(const Pose2& p2); - Vector logmap(const Pose2& p); + Vector localCoordinates(const Pose2& p); }; class SharedGaussian { diff --git a/gtsam/geometry/tests/testPose3.cpp b/gtsam/geometry/tests/testPose3.cpp index bbe69bc9e..afaa25015 100644 --- a/gtsam/geometry/tests/testPose3.cpp +++ b/gtsam/geometry/tests/testPose3.cpp @@ -148,7 +148,7 @@ TEST(Pose3, expmaps_galore) actual = Pose3::Retract(xi); EXPECT(assert_equal(expm(xi), actual,1e-6)); EXPECT(assert_equal(Agrawal06iros(xi), actual,1e-6)); - EXPECT(assert_equal(xi, logmap(actual),1e-6)); + EXPECT(assert_equal(xi, localCoordinates(actual),1e-6)); xi = Vector_(6,0.1,-0.2,0.3,-0.4,0.5,-0.6); for (double theta=1.0;0.3*theta<=M_PI;theta*=2) { @@ -156,7 +156,7 @@ TEST(Pose3, expmaps_galore) actual = Pose3::Retract(txi); EXPECT(assert_equal(expm(txi,30), actual,1e-6)); EXPECT(assert_equal(Agrawal06iros(txi), actual,1e-6)); - Vector log = logmap(actual); + Vector log = localCoordinates(actual); EXPECT(assert_equal(actual, Pose3::Retract(log),1e-6)); EXPECT(assert_equal(txi,log,1e-6)); // not true once wraps } @@ -166,7 +166,7 @@ TEST(Pose3, expmaps_galore) actual = Pose3::Retract(xi); EXPECT(assert_equal(expm(xi,10), actual,1e-5)); EXPECT(assert_equal(Agrawal06iros(xi), actual,1e-6)); - EXPECT(assert_equal(xi, logmap(actual),1e-6)); + EXPECT(assert_equal(xi, localCoordinates(actual),1e-6)); } /* ************************************************************************* */ diff --git a/gtsam/nonlinear/DoglegOptimizer.h b/gtsam/nonlinear/DoglegOptimizer.h index b4ea90f3d..62ff72b05 100644 --- a/gtsam/nonlinear/DoglegOptimizer.h +++ b/gtsam/nonlinear/DoglegOptimizer.h @@ -15,7 +15,7 @@ namespace gtsam { * This class is functional, meaning every method is \c const, and returns a new * copy of the class. * - * \tparam VALUES The LieValues or TupleValues type to hold the values to be + * \tparam VALUES The Values or TupleValues type to hold the values to be * estimated. * * \tparam GAUSSIAN_SOLVER The linear solver to use at each iteration, diff --git a/gtsam/nonlinear/DoglegOptimizerImpl.h b/gtsam/nonlinear/DoglegOptimizerImpl.h index edd3c75ac..eefa76df1 100644 --- a/gtsam/nonlinear/DoglegOptimizerImpl.h +++ b/gtsam/nonlinear/DoglegOptimizerImpl.h @@ -18,7 +18,7 @@ namespace gtsam { * here matches that in "trustregion.pdf" in gtsam_experimental/doc, see this * file for further explanation of the computations performed by this class. * - * \tparam VALUES The LieValues or TupleValues type to hold the values to be + * \tparam VALUES The Values or TupleValues type to hold the values to be * estimated. * * \tparam GAUSSIAN_SOLVER The linear solver to use at each iteration, @@ -60,7 +60,7 @@ struct DoglegOptimizerImpl { * @tparam M The type of the Bayes' net or tree, currently * either BayesNet (or GaussianBayesNet) or BayesTree. * @tparam F For normal usage this will be NonlinearFactorGraph. - * @tparam VALUES The LieValues or TupleValues to pass to F::error() to evaluate + * @tparam VALUES The Values or TupleValues to pass to F::error() to evaluate * the error function. * @param initialDelta The initial trust region radius. * @param Rd The Bayes' net or tree as described above. @@ -159,7 +159,7 @@ typename DoglegOptimizerImpl::IterationResult DoglegOptimizerImpl::Iterate( if(verbose) cout << "Delta = " << Delta << ", dx_d_norm = " << result.dx_d.vector().norm() << endl; // Compute expmapped solution - const VALUES x_d(x0.expmap(result.dx_d, ordering)); + const VALUES x_d(x0.retract(result.dx_d, ordering)); // Compute decrease in f result.f_error = f.error(x_d); diff --git a/gtsam/nonlinear/Makefile.am b/gtsam/nonlinear/Makefile.am index c187fd6f8..8629379fc 100644 --- a/gtsam/nonlinear/Makefile.am +++ b/gtsam/nonlinear/Makefile.am @@ -16,8 +16,8 @@ check_PROGRAMS = #---------------------------------------------------------------------------------------------------- # Lie Groups -headers += LieValues.h LieValues-inl.h TupleValues.h TupleValues-inl.h -check_PROGRAMS += tests/testLieValues tests/testKey tests/testOrdering +headers += Values.h Values-inl.h TupleValues.h TupleValues-inl.h +check_PROGRAMS += tests/testValues tests/testKey tests/testOrdering # Nonlinear nonlinear headers += Key.h diff --git a/gtsam/nonlinear/NonlinearOptimizer-inl.h b/gtsam/nonlinear/NonlinearOptimizer-inl.h index 52437aa61..5a7d929f6 100644 --- a/gtsam/nonlinear/NonlinearOptimizer-inl.h +++ b/gtsam/nonlinear/NonlinearOptimizer-inl.h @@ -306,7 +306,7 @@ namespace gtsam { DoglegOptimizerImpl::IterationResult result = DoglegOptimizerImpl::Iterate( parameters_->lambda_, DoglegOptimizerImpl::ONE_STEP_PER_ITERATION, *solver.eliminate(), *graph_, *values_, *ordering_, error_); - shared_values newValues(new T(values_->expmap(result.dx_d, *ordering_))); + shared_values newValues(new T(values_->retract(result.dx_d, *ordering_))); cout << "newValues: " << newValues.get() << endl; return newValuesErrorLambda_(newValues, result.f_error, result.Delta); } diff --git a/tests/testDoglegOptimizer.cpp b/tests/testDoglegOptimizer.cpp index a7f2ab001..85044009f 100644 --- a/tests/testDoglegOptimizer.cpp +++ b/tests/testDoglegOptimizer.cpp @@ -405,7 +405,7 @@ TEST(DoglegOptimizer, Iterate) { DoglegOptimizerImpl::IterationResult result = DoglegOptimizerImpl::Iterate(Delta, DoglegOptimizerImpl::SEARCH_EACH_ITERATION, gbn, *fg, *config, *ord, fg->error(*config)); Delta = result.Delta; EXPECT(result.f_error < fg->error(*config)); // Check that error decreases - simulated2D::Values newConfig(config->expmap(result.dx_d, *ord)); + simulated2D::Values newConfig(config->retract(result.dx_d, *ord)); (*config) = newConfig; DOUBLES_EQUAL(fg->error(*config), result.f_error, 1e-5); // Check that error is correctly filled in }