Merge pull request #1121 from borglab/fix/basis_tests
						commit
						a0206e210d
					
				|  | @ -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<Chebyshev2> factor(key, measured, model, 8, 0.5); | ||||
|  */ | ||||
| template <class BASIS> | ||||
| class EvaluationFactor : public FunctorizedFactor<double, Vector> { | ||||
|  | @ -47,7 +50,7 @@ class EvaluationFactor : public FunctorizedFactor<double, Vector> { | |||
|    * @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<double, Vector> { | |||
|    * @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)) {} | ||||
| 
 | ||||
|  |  | |||
|  | @ -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. | ||||
|  * | ||||
|  |  | |||
|  | @ -0,0 +1,230 @@ | |||
| /* ----------------------------------------------------------------------------
 | ||||
| 
 | ||||
|  * 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 <gtsam/basis/Basis.h> | ||||
| #include <gtsam/basis/BasisFactors.h> | ||||
| #include <gtsam/basis/Chebyshev2.h> | ||||
| #include <gtsam/geometry/Pose2.h> | ||||
| #include <gtsam/nonlinear/FunctorizedFactor.h> | ||||
| #include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h> | ||||
| #include <gtsam/nonlinear/factorTesting.h> | ||||
| #include <gtsam/inference/Symbol.h> | ||||
| #include <gtsam/base/Testable.h> | ||||
| #include <gtsam/base/TestableAssertions.h> | ||||
| #include <gtsam/base/Vector.h> | ||||
| 
 | ||||
| #include <CppUnitLite/TestHarness.h> | ||||
| 
 | ||||
| using gtsam::noiseModel::Isotropic; | ||||
| using gtsam::Pose2; | ||||
| using gtsam::Vector; | ||||
| using gtsam::Values; | ||||
| using gtsam::Chebyshev2; | ||||
| using gtsam::ParameterMatrix; | ||||
| using gtsam::LevenbergMarquardtParams; | ||||
| using gtsam::LevenbergMarquardtOptimizer; | ||||
| using gtsam::NonlinearFactorGraph; | ||||
| using gtsam::NonlinearOptimizerParams; | ||||
| 
 | ||||
| constexpr size_t N = 2; | ||||
| 
 | ||||
| // Key used in all tests
 | ||||
| const gtsam::Symbol key('X', 0); | ||||
| 
 | ||||
| //******************************************************************************
 | ||||
| TEST(BasisFactors, EvaluationFactor) { | ||||
|   using gtsam::EvaluationFactor; | ||||
| 
 | ||||
|   double measured = 0; | ||||
| 
 | ||||
|   auto model = Isotropic::Sigma(1, 1.0); | ||||
|   EvaluationFactor<Chebyshev2> factor(key, measured, model, N, 0); | ||||
| 
 | ||||
|   NonlinearFactorGraph graph; | ||||
|   graph.add(factor); | ||||
| 
 | ||||
|   Vector functionValues(N); | ||||
|   functionValues.setZero(); | ||||
| 
 | ||||
|   Values initial; | ||||
|   initial.insert<Vector>(key, functionValues); | ||||
| 
 | ||||
|   LevenbergMarquardtParams parameters; | ||||
|   parameters.setMaxIterations(20); | ||||
|   Values result = | ||||
|       LevenbergMarquardtOptimizer(graph, initial, parameters).optimize(); | ||||
| 
 | ||||
|   EXPECT_DOUBLES_EQUAL(0, graph.error(result), 1e-9); | ||||
| } | ||||
| 
 | ||||
| //******************************************************************************
 | ||||
| TEST(BasisFactors, VectorEvaluationFactor) { | ||||
|   using gtsam::VectorEvaluationFactor; | ||||
|   const size_t M = 4; | ||||
| 
 | ||||
|   const Vector measured = Vector::Zero(M); | ||||
| 
 | ||||
|   auto model = Isotropic::Sigma(M, 1.0); | ||||
|   VectorEvaluationFactor<Chebyshev2, M> factor(key, measured, model, N, 0); | ||||
| 
 | ||||
|   NonlinearFactorGraph graph; | ||||
|   graph.add(factor); | ||||
| 
 | ||||
|   ParameterMatrix<M> stateMatrix(N); | ||||
| 
 | ||||
|   Values initial; | ||||
|   initial.insert<ParameterMatrix<M>>(key, stateMatrix); | ||||
| 
 | ||||
|   LevenbergMarquardtParams parameters; | ||||
|   parameters.setMaxIterations(20); | ||||
|   Values result = | ||||
|       LevenbergMarquardtOptimizer(graph, initial, parameters).optimize(); | ||||
| 
 | ||||
|   EXPECT_DOUBLES_EQUAL(0, graph.error(result), 1e-9); | ||||
| } | ||||
| 
 | ||||
| //******************************************************************************
 | ||||
| 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<Chebyshev2, M> 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; | ||||
|   const double measured = 0.0, t = 3.0, a = 2.0, b = 4.0; | ||||
|   auto model = Isotropic::Sigma(1, 1.0); | ||||
|   VectorComponentFactor<Chebyshev2, P> factor(key, measured, model, N, i, | ||||
|                                                     t, a, b); | ||||
| 
 | ||||
|   NonlinearFactorGraph graph; | ||||
|   graph.add(factor); | ||||
| 
 | ||||
|   ParameterMatrix<P> stateMatrix(N); | ||||
| 
 | ||||
|   Values initial; | ||||
|   initial.insert<ParameterMatrix<P>>(key, stateMatrix); | ||||
| 
 | ||||
|   LevenbergMarquardtParams parameters; | ||||
|   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<Chebyshev2, Pose2> factor(key, measured, model, N, | ||||
|                                                      t, a, b); | ||||
| 
 | ||||
|   NonlinearFactorGraph graph; | ||||
|   graph.add(factor); | ||||
| 
 | ||||
|   ParameterMatrix<3> stateMatrix(N); | ||||
| 
 | ||||
|   Values initial; | ||||
|   initial.insert<ParameterMatrix<3>>(key, stateMatrix); | ||||
| 
 | ||||
|   LevenbergMarquardtParams parameters; | ||||
|   parameters.setMaxIterations(20); | ||||
|   Values result = | ||||
|       LevenbergMarquardtOptimizer(graph, initial, parameters).optimize(); | ||||
| 
 | ||||
|   EXPECT_DOUBLES_EQUAL(0, graph.error(result), 1e-9); | ||||
| } | ||||
| 
 | ||||
| //******************************************************************************
 | ||||
| TEST(BasisFactors, VecDerivativePrior) { | ||||
|   using gtsam::VectorDerivativeFactor; | ||||
|   const size_t M = 4; | ||||
| 
 | ||||
|   const Vector measured = Vector::Zero(M); | ||||
|   auto model = Isotropic::Sigma(M, 1.0); | ||||
|   VectorDerivativeFactor<Chebyshev2, M> vecDPrior(key, measured, model, N, 0); | ||||
| 
 | ||||
|   NonlinearFactorGraph graph; | ||||
|   graph.add(vecDPrior); | ||||
| 
 | ||||
|   ParameterMatrix<M> stateMatrix(N); | ||||
| 
 | ||||
|   Values initial; | ||||
|   initial.insert<ParameterMatrix<M>>(key, stateMatrix); | ||||
| 
 | ||||
|   LevenbergMarquardtParams parameters; | ||||
|   parameters.setMaxIterations(20); | ||||
|   Values result = | ||||
|       LevenbergMarquardtOptimizer(graph, initial, parameters).optimize(); | ||||
| 
 | ||||
|   EXPECT_DOUBLES_EQUAL(0, graph.error(result), 1e-9); | ||||
| } | ||||
| 
 | ||||
| //******************************************************************************
 | ||||
| TEST(BasisFactors, ComponentDerivativeFactor) { | ||||
|   using gtsam::ComponentDerivativeFactor; | ||||
|   const size_t M = 4; | ||||
| 
 | ||||
|   double measured = 0; | ||||
|   auto model = Isotropic::Sigma(1, 1.0); | ||||
|   ComponentDerivativeFactor<Chebyshev2, M> controlDPrior(key, measured, model, | ||||
|                                                          N, 0, 0); | ||||
| 
 | ||||
|   NonlinearFactorGraph graph; | ||||
|   graph.add(controlDPrior); | ||||
| 
 | ||||
|   Values initial; | ||||
|   ParameterMatrix<M> stateMatrix(N); | ||||
|   initial.insert<ParameterMatrix<M>>(key, stateMatrix); | ||||
| 
 | ||||
|   LevenbergMarquardtParams parameters; | ||||
|   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); | ||||
| } | ||||
| /* ************************************************************************* */ | ||||
|  | @ -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 <CppUnitLite/TestHarness.h> | ||||
| #include <gtsam/base/Testable.h> | ||||
| #include <gtsam/basis/Chebyshev2.h> | ||||
| #include <gtsam/basis/FitBasis.h> | ||||
| #include <gtsam/geometry/Pose2.h> | ||||
| #include <gtsam/nonlinear/factorTesting.h> | ||||
| #include <gtsam/base/Testable.h> | ||||
| 
 | ||||
| #include <CppUnitLite/TestHarness.h> | ||||
| 
 | ||||
| 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<Pose2> 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); | ||||
|  |  | |||
|  | @ -17,16 +17,14 @@ | |||
|  * @brief unit tests for FunctorizedFactor class | ||||
|  */ | ||||
| 
 | ||||
| #include <CppUnitLite/TestHarness.h> | ||||
| #include <gtsam/base/Testable.h> | ||||
| #include <gtsam/base/TestableAssertions.h> | ||||
| #include <gtsam/basis/Basis.h> | ||||
| #include <gtsam/basis/BasisFactors.h> | ||||
| #include <gtsam/basis/Chebyshev2.h> | ||||
| #include <gtsam/inference/Symbol.h> | ||||
| #include <gtsam/nonlinear/FunctorizedFactor.h> | ||||
| #include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h> | ||||
| #include <gtsam/nonlinear/factorTesting.h> | ||||
| #include <gtsam/inference/Symbol.h> | ||||
| #include <gtsam/base/Testable.h> | ||||
| #include <gtsam/base/TestableAssertions.h> | ||||
| 
 | ||||
| #include <CppUnitLite/TestHarness.h> | ||||
| 
 | ||||
| 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<Chebyshev2, M> 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<Chebyshev2, M> priorFactor(key, measured, model, N, 0); | ||||
| 
 | ||||
|   NonlinearFactorGraph graph; | ||||
|   graph.add(priorFactor); | ||||
| 
 | ||||
|   ParameterMatrix<M> stateMatrix(N); | ||||
| 
 | ||||
|   Values initial; | ||||
|   initial.insert<ParameterMatrix<M>>(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<Chebyshev2, P> controlPrior(key, measured, model, N, i, | ||||
|                                                     t, a, b); | ||||
| 
 | ||||
|   NonlinearFactorGraph graph; | ||||
|   graph.add(controlPrior); | ||||
| 
 | ||||
|   ParameterMatrix<P> stateMatrix(N); | ||||
| 
 | ||||
|   Values initial; | ||||
|   initial.insert<ParameterMatrix<P>>(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<Chebyshev2, M> vecDPrior(key, measured, model, N, 0); | ||||
| 
 | ||||
|   NonlinearFactorGraph graph; | ||||
|   graph.add(vecDPrior); | ||||
| 
 | ||||
|   ParameterMatrix<M> stateMatrix(N); | ||||
| 
 | ||||
|   Values initial; | ||||
|   initial.insert<ParameterMatrix<M>>(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<Chebyshev2, M> controlDPrior(key, measured, model, | ||||
|                                                          N, 0, 0); | ||||
| 
 | ||||
|   NonlinearFactorGraph graph; | ||||
|   graph.add(controlDPrior); | ||||
| 
 | ||||
|   Values initial; | ||||
|   ParameterMatrix<M> stateMatrix(N); | ||||
|   initial.insert<ParameterMatrix<M>>(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; | ||||
|  |  | |||
		Loading…
	
		Reference in New Issue