From 5f9082b5367ce793c44b535ad4dc31f0de593157 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 26 Feb 2022 22:43:37 -0500 Subject: [PATCH 1/5] docs --- gtsam/basis/Chebyshev2.h | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/gtsam/basis/Chebyshev2.h b/gtsam/basis/Chebyshev2.h index 28590961d..e306c93d5 100644 --- a/gtsam/basis/Chebyshev2.h +++ b/gtsam/basis/Chebyshev2.h @@ -22,8 +22,7 @@ * * This is different from Chebyshev.h since it leverage ideas from * pseudo-spectral optimization, i.e. we don't decompose into basis functions, - * rather estimate function parameters that enforce function nodes at Chebyshev - * points. + * rather estimate function values at the Chebyshev points. * * Please refer to Agrawal21icra for more details. * From a16f588317d975c47732a7b157ac1d74d8dedbfe Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 26 Feb 2022 22:43:55 -0500 Subject: [PATCH 2/5] Add test for ManifoldEvaluationFunctor --- gtsam/basis/tests/testChebyshev2.cpp | 36 +++++++++++++++++++++------- 1 file changed, 28 insertions(+), 8 deletions(-) diff --git a/gtsam/basis/tests/testChebyshev2.cpp b/gtsam/basis/tests/testChebyshev2.cpp index 6fafc0ccf..9090757f4 100644 --- a/gtsam/basis/tests/testChebyshev2.cpp +++ b/gtsam/basis/tests/testChebyshev2.cpp @@ -10,18 +10,20 @@ * -------------------------------------------------------------------------- */ /** - * @file testChebyshev.cpp + * @file testChebyshev2.cpp * @date July 4, 2020 * @author Varun Agrawal * @brief Unit tests for Chebyshev Basis Decompositions via pseudo-spectral * methods */ -#include -#include #include #include +#include #include +#include + +#include using namespace std; using namespace gtsam; @@ -123,12 +125,30 @@ TEST(Chebyshev2, InterpolateVector) { EXPECT(assert_equal(numericalH, actualH, 1e-9)); } +//****************************************************************************** +// Interpolating poses using the exponential map +TEST(Chebyshev2, InterpolatePose2) { + double t = 30, a = 0, b = 100; + + ParameterMatrix<3> X(N); + X.row(0) = Chebyshev2::Points(N, a, b); // slope 1 ramp + X.row(1) = Vector::Zero(N); + X.row(2) = 0.1 * Vector::Ones(N); + + Vector xi(3); + xi << t, 0, 0.1; + Chebyshev2::ManifoldEvaluationFunctor fx(N, t, a, b); + // We use xi as canonical coordinates via exponential map + Pose2 expected = Pose2::ChartAtOrigin::Retract(xi); + EXPECT(assert_equal(expected, fx(X))); +} + //****************************************************************************** TEST(Chebyshev2, Decomposition) { // Create example sequence Sequence sequence; for (size_t i = 0; i < 16; i++) { - double x = (double)i / 16. - 0.99, y = x; + double x = (1.0/ 16)*i - 0.99, y = x; sequence[x] = y; } @@ -146,11 +166,11 @@ TEST(Chebyshev2, DifferentiationMatrix3) { // Trefethen00book, p.55 const size_t N = 3; Matrix expected(N, N); - // Differentiation matrix computed from Chebfun + // Differentiation matrix computed from chebfun expected << 1.5000, -2.0000, 0.5000, // 0.5000, -0.0000, -0.5000, // -0.5000, 2.0000, -1.5000; - // multiply by -1 since the cheb points have a phase shift wrt Trefethen + // multiply by -1 since the chebyshev points have a phase shift wrt Trefethen // This was verified with chebfun expected = -expected; @@ -169,7 +189,7 @@ TEST(Chebyshev2, DerivativeMatrix6) { 0.3820, -0.8944, 1.6180, 0.1708, -2.0000, 0.7236, // -0.2764, 0.6180, -0.8944, 2.0000, 1.1708, -2.6180, // 0.5000, -1.1056, 1.5279, -2.8944, 10.4721, -8.5000; - // multiply by -1 since the cheb points have a phase shift wrt Trefethen + // multiply by -1 since the chebyshev points have a phase shift wrt Trefethen // This was verified with chebfun expected = -expected; @@ -254,7 +274,7 @@ TEST(Chebyshev2, DerivativeWeights2) { Weights dWeights2 = Chebyshev2::DerivativeWeights(N, x2, a, b); EXPECT_DOUBLES_EQUAL(fprime(x2), dWeights2 * fvals, 1e-8); - // test if derivative calculation and cheb point is correct + // test if derivative calculation and Chebyshev point is correct double x3 = Chebyshev2::Point(N, 3, a, b); Weights dWeights3 = Chebyshev2::DerivativeWeights(N, x3, a, b); EXPECT_DOUBLES_EQUAL(fprime(x3), dWeights3 * fvals, 1e-8); From c85b6496855ca701c8969dac6f4c64359a5860a3 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 26 Feb 2022 22:44:17 -0500 Subject: [PATCH 3/5] Move BasisFactors tests to correct place --- gtsam/basis/tests/testBasisFactors.cpp | 185 ++++++++++++++++++ .../nonlinear/tests/testFunctorizedFactor.cpp | 141 +------------ 2 files changed, 190 insertions(+), 136 deletions(-) create mode 100644 gtsam/basis/tests/testBasisFactors.cpp diff --git a/gtsam/basis/tests/testBasisFactors.cpp b/gtsam/basis/tests/testBasisFactors.cpp new file mode 100644 index 000000000..b0e1e91cb --- /dev/null +++ b/gtsam/basis/tests/testBasisFactors.cpp @@ -0,0 +1,185 @@ +/* ---------------------------------------------------------------------------- + + * 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 testBasisFactors.cpp + * @date May 31, 2020 + * @author Varun Agrawal + * @brief unit tests for factors in BasisFactors.h + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +using gtsam::noiseModel::Isotropic; +using gtsam::Vector; +using gtsam::Values; +using gtsam::Chebyshev2; +using gtsam::ParameterMatrix; +using gtsam::LevenbergMarquardtParams; +using gtsam::LevenbergMarquardtOptimizer; +using gtsam::NonlinearFactorGraph; +using gtsam::NonlinearOptimizerParams; + +const size_t N = 2; + +// Key for FunctorizedFactor +gtsam::Key key = gtsam::Symbol('X', 0); + +//****************************************************************************** +TEST(FunctorizedFactor, Print2) { + using gtsam::VectorEvaluationFactor; + const size_t M = 1; + + Vector measured = Vector::Ones(M) * 42; + + auto model = Isotropic::Sigma(M, 1.0); + VectorEvaluationFactor priorFactor(key, measured, model, N, 0); + + std::string expected = + " keys = { X0 }\n" + " noise model: unit (1) \n" + "FunctorizedFactor(X0)\n" + " measurement: [\n" + " 42\n" + "]\n" + " noise model sigmas: 1\n"; + + EXPECT(assert_print_equal(expected, priorFactor)); +} + +//****************************************************************************** +TEST(FunctorizedFactor, VectorEvaluationFactor) { + using gtsam::VectorEvaluationFactor; + const size_t M = 4; + + Vector measured = Vector::Zero(M); + + auto model = Isotropic::Sigma(M, 1.0); + VectorEvaluationFactor priorFactor(key, measured, model, N, 0); + + NonlinearFactorGraph graph; + graph.add(priorFactor); + + ParameterMatrix stateMatrix(N); + + Values initial; + initial.insert>(key, stateMatrix); + + LevenbergMarquardtParams parameters; + parameters.verbosity = NonlinearOptimizerParams::SILENT; + parameters.verbosityLM = LevenbergMarquardtParams::SILENT; + parameters.setMaxIterations(20); + Values result = + LevenbergMarquardtOptimizer(graph, initial, parameters).optimize(); + + EXPECT_DOUBLES_EQUAL(0, graph.error(result), 1e-9); +} + +//****************************************************************************** +TEST(FunctorizedFactor, VectorComponentFactor) { + using gtsam::VectorComponentFactor; + const int P = 4; + const size_t i = 2; + const double measured = 0.0, t = 3.0, a = 2.0, b = 4.0; + auto model = Isotropic::Sigma(1, 1.0); + VectorComponentFactor controlPrior(key, measured, model, N, i, + t, a, b); + + NonlinearFactorGraph graph; + graph.add(controlPrior); + + ParameterMatrix

stateMatrix(N); + + Values initial; + initial.insert>(key, stateMatrix); + + LevenbergMarquardtParams parameters; + parameters.verbosity = NonlinearOptimizerParams::SILENT; + parameters.verbosityLM = LevenbergMarquardtParams::SILENT; + parameters.setMaxIterations(20); + Values result = + LevenbergMarquardtOptimizer(graph, initial, parameters).optimize(); + + EXPECT_DOUBLES_EQUAL(0, graph.error(result), 1e-9); +} + +//****************************************************************************** +TEST(FunctorizedFactor, VecDerivativePrior) { + using gtsam::VectorDerivativeFactor; + const size_t M = 4; + + Vector measured = Vector::Zero(M); + auto model = Isotropic::Sigma(M, 1.0); + VectorDerivativeFactor vecDPrior(key, measured, model, N, 0); + + NonlinearFactorGraph graph; + graph.add(vecDPrior); + + ParameterMatrix stateMatrix(N); + + Values initial; + initial.insert>(key, stateMatrix); + + LevenbergMarquardtParams parameters; + parameters.verbosity = NonlinearOptimizerParams::SILENT; + parameters.verbosityLM = LevenbergMarquardtParams::SILENT; + parameters.setMaxIterations(20); + Values result = + LevenbergMarquardtOptimizer(graph, initial, parameters).optimize(); + + EXPECT_DOUBLES_EQUAL(0, graph.error(result), 1e-9); +} + +//****************************************************************************** +TEST(FunctorizedFactor, ComponentDerivativeFactor) { + using gtsam::ComponentDerivativeFactor; + const size_t M = 4; + + double measured = 0; + auto model = Isotropic::Sigma(1, 1.0); + ComponentDerivativeFactor controlDPrior(key, measured, model, + N, 0, 0); + + NonlinearFactorGraph graph; + graph.add(controlDPrior); + + Values initial; + ParameterMatrix stateMatrix(N); + initial.insert>(key, stateMatrix); + + LevenbergMarquardtParams parameters; + parameters.verbosity = NonlinearOptimizerParams::SILENT; + parameters.verbosityLM = LevenbergMarquardtParams::SILENT; + parameters.setMaxIterations(20); + Values result = + LevenbergMarquardtOptimizer(graph, initial, parameters).optimize(); + + EXPECT_DOUBLES_EQUAL(0, graph.error(result), 1e-9); +} + +/* ************************************************************************* */ +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +/* ************************************************************************* */ diff --git a/gtsam/nonlinear/tests/testFunctorizedFactor.cpp b/gtsam/nonlinear/tests/testFunctorizedFactor.cpp index 14a14fc19..214c5efa7 100644 --- a/gtsam/nonlinear/tests/testFunctorizedFactor.cpp +++ b/gtsam/nonlinear/tests/testFunctorizedFactor.cpp @@ -17,16 +17,14 @@ * @brief unit tests for FunctorizedFactor class */ -#include -#include -#include -#include -#include -#include -#include #include #include #include +#include +#include +#include + +#include using namespace std; using namespace gtsam; @@ -272,135 +270,6 @@ TEST(FunctorizedFactor, Lambda2) { EXPECT(assert_equal(Vector::Zero(3), error, 1e-9)); } -const size_t N = 2; - -//****************************************************************************** -TEST(FunctorizedFactor, Print2) { - const size_t M = 1; - - Vector measured = Vector::Ones(M) * 42; - - auto model = noiseModel::Isotropic::Sigma(M, 1.0); - VectorEvaluationFactor priorFactor(key, measured, model, N, 0); - - string expected = - " keys = { X0 }\n" - " noise model: unit (1) \n" - "FunctorizedFactor(X0)\n" - " measurement: [\n" - " 42\n" - "]\n" - " noise model sigmas: 1\n"; - - EXPECT(assert_print_equal(expected, priorFactor)); -} - -//****************************************************************************** -TEST(FunctorizedFactor, VectorEvaluationFactor) { - const size_t M = 4; - - Vector measured = Vector::Zero(M); - - auto model = noiseModel::Isotropic::Sigma(M, 1.0); - VectorEvaluationFactor priorFactor(key, measured, model, N, 0); - - NonlinearFactorGraph graph; - graph.add(priorFactor); - - ParameterMatrix stateMatrix(N); - - Values initial; - initial.insert>(key, stateMatrix); - - LevenbergMarquardtParams parameters; - parameters.verbosity = NonlinearOptimizerParams::SILENT; - parameters.verbosityLM = LevenbergMarquardtParams::SILENT; - parameters.setMaxIterations(20); - Values result = - LevenbergMarquardtOptimizer(graph, initial, parameters).optimize(); - - EXPECT_DOUBLES_EQUAL(0, graph.error(result), 1e-9); -} - -//****************************************************************************** -TEST(FunctorizedFactor, VectorComponentFactor) { - const int P = 4; - const size_t i = 2; - const double measured = 0.0, t = 3.0, a = 2.0, b = 4.0; - auto model = noiseModel::Isotropic::Sigma(1, 1.0); - VectorComponentFactor controlPrior(key, measured, model, N, i, - t, a, b); - - NonlinearFactorGraph graph; - graph.add(controlPrior); - - ParameterMatrix

stateMatrix(N); - - Values initial; - initial.insert>(key, stateMatrix); - - LevenbergMarquardtParams parameters; - parameters.verbosity = NonlinearOptimizerParams::SILENT; - parameters.verbosityLM = LevenbergMarquardtParams::SILENT; - parameters.setMaxIterations(20); - Values result = - LevenbergMarquardtOptimizer(graph, initial, parameters).optimize(); - - EXPECT_DOUBLES_EQUAL(0, graph.error(result), 1e-9); -} - -//****************************************************************************** -TEST(FunctorizedFactor, VecDerivativePrior) { - const size_t M = 4; - - Vector measured = Vector::Zero(M); - auto model = noiseModel::Isotropic::Sigma(M, 1.0); - VectorDerivativeFactor vecDPrior(key, measured, model, N, 0); - - NonlinearFactorGraph graph; - graph.add(vecDPrior); - - ParameterMatrix stateMatrix(N); - - Values initial; - initial.insert>(key, stateMatrix); - - LevenbergMarquardtParams parameters; - parameters.verbosity = NonlinearOptimizerParams::SILENT; - parameters.verbosityLM = LevenbergMarquardtParams::SILENT; - parameters.setMaxIterations(20); - Values result = - LevenbergMarquardtOptimizer(graph, initial, parameters).optimize(); - - EXPECT_DOUBLES_EQUAL(0, graph.error(result), 1e-9); -} - -//****************************************************************************** -TEST(FunctorizedFactor, ComponentDerivativeFactor) { - const size_t M = 4; - - double measured = 0; - auto model = noiseModel::Isotropic::Sigma(1, 1.0); - ComponentDerivativeFactor controlDPrior(key, measured, model, - N, 0, 0); - - NonlinearFactorGraph graph; - graph.add(controlDPrior); - - Values initial; - ParameterMatrix stateMatrix(N); - initial.insert>(key, stateMatrix); - - LevenbergMarquardtParams parameters; - parameters.verbosity = NonlinearOptimizerParams::SILENT; - parameters.verbosityLM = LevenbergMarquardtParams::SILENT; - parameters.setMaxIterations(20); - Values result = - LevenbergMarquardtOptimizer(graph, initial, parameters).optimize(); - - EXPECT_DOUBLES_EQUAL(0, graph.error(result), 1e-9); -} - /* ************************************************************************* */ int main() { TestResult tr; From 3d8c7b9fdfed5c4bc5ced72d27ccedde48968693 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 26 Feb 2022 22:56:46 -0500 Subject: [PATCH 4/5] Add EvaluationFactor test --- gtsam/basis/BasisFactors.h | 7 ++- gtsam/basis/tests/testBasisFactors.cpp | 80 +++++++++++++++++--------- 2 files changed, 59 insertions(+), 28 deletions(-) diff --git a/gtsam/basis/BasisFactors.h b/gtsam/basis/BasisFactors.h index 3d1a12f19..648bcd510 100644 --- a/gtsam/basis/BasisFactors.h +++ b/gtsam/basis/BasisFactors.h @@ -29,6 +29,9 @@ namespace gtsam { * pseudo-spectral parameterization. * * @tparam BASIS The basis class to use e.g. Chebyshev2 + * + * Example, degree 8 Chebyshev polynomial measured at x=0.5: + * EvaluationFactor factor(key, measured, model, 8, 0.5); */ template class EvaluationFactor : public FunctorizedFactor { @@ -47,7 +50,7 @@ class EvaluationFactor : public FunctorizedFactor { * @param N The degree of the polynomial. * @param x The point at which to evaluate the polynomial. */ - EvaluationFactor(Key key, const double &z, const SharedNoiseModel &model, + EvaluationFactor(Key key, double z, const SharedNoiseModel &model, const size_t N, double x) : Base(key, z, model, typename BASIS::EvaluationFunctor(N, x)) {} @@ -62,7 +65,7 @@ class EvaluationFactor : public FunctorizedFactor { * @param a Lower bound for the polynomial. * @param b Upper bound for the polynomial. */ - EvaluationFactor(Key key, const double &z, const SharedNoiseModel &model, + EvaluationFactor(Key key, double z, const SharedNoiseModel &model, const size_t N, double x, double a, double b) : Base(key, z, model, typename BASIS::EvaluationFunctor(N, x, a, b)) {} diff --git a/gtsam/basis/tests/testBasisFactors.cpp b/gtsam/basis/tests/testBasisFactors.cpp index b0e1e91cb..ec3c53152 100644 --- a/gtsam/basis/tests/testBasisFactors.cpp +++ b/gtsam/basis/tests/testBasisFactors.cpp @@ -40,45 +40,51 @@ using gtsam::LevenbergMarquardtOptimizer; using gtsam::NonlinearFactorGraph; using gtsam::NonlinearOptimizerParams; -const size_t N = 2; +constexpr size_t N = 2; -// Key for FunctorizedFactor -gtsam::Key key = gtsam::Symbol('X', 0); +// Key used in all tests +const gtsam::Symbol key('X', 0); //****************************************************************************** -TEST(FunctorizedFactor, Print2) { - using gtsam::VectorEvaluationFactor; - const size_t M = 1; +TEST(BasisFactors, EvaluationFactor) { + using gtsam::EvaluationFactor; - Vector measured = Vector::Ones(M) * 42; + double measured = 0; - auto model = Isotropic::Sigma(M, 1.0); - VectorEvaluationFactor priorFactor(key, measured, model, N, 0); + auto model = Isotropic::Sigma(1, 1.0); + EvaluationFactor factor(key, measured, model, N, 0); - std::string expected = - " keys = { X0 }\n" - " noise model: unit (1) \n" - "FunctorizedFactor(X0)\n" - " measurement: [\n" - " 42\n" - "]\n" - " noise model sigmas: 1\n"; + NonlinearFactorGraph graph; + graph.add(factor); - EXPECT(assert_print_equal(expected, priorFactor)); + Vector functionValues(N); + functionValues.setZero(); + + Values initial; + initial.insert(key, functionValues); + + LevenbergMarquardtParams parameters; + parameters.verbosity = NonlinearOptimizerParams::SILENT; + parameters.verbosityLM = LevenbergMarquardtParams::SILENT; + parameters.setMaxIterations(20); + Values result = + LevenbergMarquardtOptimizer(graph, initial, parameters).optimize(); + + EXPECT_DOUBLES_EQUAL(0, graph.error(result), 1e-9); } //****************************************************************************** -TEST(FunctorizedFactor, VectorEvaluationFactor) { +TEST(BasisFactors, VectorEvaluationFactor) { using gtsam::VectorEvaluationFactor; const size_t M = 4; - Vector measured = Vector::Zero(M); + const Vector measured = Vector::Zero(M); auto model = Isotropic::Sigma(M, 1.0); - VectorEvaluationFactor priorFactor(key, measured, model, N, 0); + VectorEvaluationFactor factor(key, measured, model, N, 0); NonlinearFactorGraph graph; - graph.add(priorFactor); + graph.add(factor); ParameterMatrix stateMatrix(N); @@ -96,7 +102,29 @@ TEST(FunctorizedFactor, VectorEvaluationFactor) { } //****************************************************************************** -TEST(FunctorizedFactor, VectorComponentFactor) { +TEST(BasisFactors, Print) { + using gtsam::VectorEvaluationFactor; + const size_t M = 1; + + const Vector measured = Vector::Ones(M) * 42; + + auto model = Isotropic::Sigma(M, 1.0); + VectorEvaluationFactor factor(key, measured, model, N, 0); + + std::string expected = + " keys = { X0 }\n" + " noise model: unit (1) \n" + "FunctorizedFactor(X0)\n" + " measurement: [\n" + " 42\n" + "]\n" + " noise model sigmas: 1\n"; + + EXPECT(assert_print_equal(expected, factor)); +} + +//****************************************************************************** +TEST(BasisFactors, VectorComponentFactor) { using gtsam::VectorComponentFactor; const int P = 4; const size_t i = 2; @@ -124,11 +152,11 @@ TEST(FunctorizedFactor, VectorComponentFactor) { } //****************************************************************************** -TEST(FunctorizedFactor, VecDerivativePrior) { +TEST(BasisFactors, VecDerivativePrior) { using gtsam::VectorDerivativeFactor; const size_t M = 4; - Vector measured = Vector::Zero(M); + const Vector measured = Vector::Zero(M); auto model = Isotropic::Sigma(M, 1.0); VectorDerivativeFactor vecDPrior(key, measured, model, N, 0); @@ -151,7 +179,7 @@ TEST(FunctorizedFactor, VecDerivativePrior) { } //****************************************************************************** -TEST(FunctorizedFactor, ComponentDerivativeFactor) { +TEST(BasisFactors, ComponentDerivativeFactor) { using gtsam::ComponentDerivativeFactor; const size_t M = 4; From 6fb38aa8d7f009aefe42c88e5ca7b5eca6fb287d Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 26 Feb 2022 23:05:49 -0500 Subject: [PATCH 5/5] Add test for ManifoldEvaluationFactor --- gtsam/basis/tests/testBasisFactors.cpp | 41 ++++++++++++++++++-------- 1 file changed, 29 insertions(+), 12 deletions(-) diff --git a/gtsam/basis/tests/testBasisFactors.cpp b/gtsam/basis/tests/testBasisFactors.cpp index ec3c53152..18a389da5 100644 --- a/gtsam/basis/tests/testBasisFactors.cpp +++ b/gtsam/basis/tests/testBasisFactors.cpp @@ -20,6 +20,7 @@ #include #include #include +#include #include #include #include @@ -31,6 +32,7 @@ #include using gtsam::noiseModel::Isotropic; +using gtsam::Pose2; using gtsam::Vector; using gtsam::Values; using gtsam::Chebyshev2; @@ -64,8 +66,6 @@ TEST(BasisFactors, EvaluationFactor) { initial.insert(key, functionValues); LevenbergMarquardtParams parameters; - parameters.verbosity = NonlinearOptimizerParams::SILENT; - parameters.verbosityLM = LevenbergMarquardtParams::SILENT; parameters.setMaxIterations(20); Values result = LevenbergMarquardtOptimizer(graph, initial, parameters).optimize(); @@ -92,8 +92,6 @@ TEST(BasisFactors, VectorEvaluationFactor) { initial.insert>(key, stateMatrix); LevenbergMarquardtParams parameters; - parameters.verbosity = NonlinearOptimizerParams::SILENT; - parameters.verbosityLM = LevenbergMarquardtParams::SILENT; parameters.setMaxIterations(20); Values result = LevenbergMarquardtOptimizer(graph, initial, parameters).optimize(); @@ -130,11 +128,11 @@ TEST(BasisFactors, VectorComponentFactor) { const size_t i = 2; const double measured = 0.0, t = 3.0, a = 2.0, b = 4.0; auto model = Isotropic::Sigma(1, 1.0); - VectorComponentFactor controlPrior(key, measured, model, N, i, + VectorComponentFactor factor(key, measured, model, N, i, t, a, b); NonlinearFactorGraph graph; - graph.add(controlPrior); + graph.add(factor); ParameterMatrix

stateMatrix(N); @@ -142,8 +140,31 @@ TEST(BasisFactors, VectorComponentFactor) { initial.insert>(key, stateMatrix); LevenbergMarquardtParams parameters; - parameters.verbosity = NonlinearOptimizerParams::SILENT; - parameters.verbosityLM = LevenbergMarquardtParams::SILENT; + parameters.setMaxIterations(20); + Values result = + LevenbergMarquardtOptimizer(graph, initial, parameters).optimize(); + + EXPECT_DOUBLES_EQUAL(0, graph.error(result), 1e-9); +} + +//****************************************************************************** +TEST(BasisFactors, ManifoldEvaluationFactor) { + using gtsam::ManifoldEvaluationFactor; + const Pose2 measured; + const double t = 3.0, a = 2.0, b = 4.0; + auto model = Isotropic::Sigma(3, 1.0); + ManifoldEvaluationFactor factor(key, measured, model, N, + t, a, b); + + NonlinearFactorGraph graph; + graph.add(factor); + + ParameterMatrix<3> stateMatrix(N); + + Values initial; + initial.insert>(key, stateMatrix); + + LevenbergMarquardtParams parameters; parameters.setMaxIterations(20); Values result = LevenbergMarquardtOptimizer(graph, initial, parameters).optimize(); @@ -169,8 +190,6 @@ TEST(BasisFactors, VecDerivativePrior) { initial.insert>(key, stateMatrix); LevenbergMarquardtParams parameters; - parameters.verbosity = NonlinearOptimizerParams::SILENT; - parameters.verbosityLM = LevenbergMarquardtParams::SILENT; parameters.setMaxIterations(20); Values result = LevenbergMarquardtOptimizer(graph, initial, parameters).optimize(); @@ -196,8 +215,6 @@ TEST(BasisFactors, ComponentDerivativeFactor) { initial.insert>(key, stateMatrix); LevenbergMarquardtParams parameters; - parameters.verbosity = NonlinearOptimizerParams::SILENT; - parameters.verbosityLM = LevenbergMarquardtParams::SILENT; parameters.setMaxIterations(20); Values result = LevenbergMarquardtOptimizer(graph, initial, parameters).optimize();