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/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. * diff --git a/gtsam/basis/tests/testBasisFactors.cpp b/gtsam/basis/tests/testBasisFactors.cpp new file mode 100644 index 000000000..18a389da5 --- /dev/null +++ b/gtsam/basis/tests/testBasisFactors.cpp @@ -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 +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +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 factor(key, measured, model, N, 0); + + NonlinearFactorGraph graph; + graph.add(factor); + + Vector functionValues(N); + functionValues.setZero(); + + Values initial; + initial.insert(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 factor(key, measured, model, N, 0); + + NonlinearFactorGraph graph; + graph.add(factor); + + ParameterMatrix stateMatrix(N); + + Values initial; + initial.insert>(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 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 factor(key, measured, model, N, i, + t, a, b); + + NonlinearFactorGraph graph; + graph.add(factor); + + ParameterMatrix

stateMatrix(N); + + Values initial; + initial.insert>(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 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(); + + 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 vecDPrior(key, measured, model, N, 0); + + NonlinearFactorGraph graph; + graph.add(vecDPrior); + + ParameterMatrix stateMatrix(N); + + Values initial; + initial.insert>(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 controlDPrior(key, measured, model, + N, 0, 0); + + NonlinearFactorGraph graph; + graph.add(controlDPrior); + + Values initial; + ParameterMatrix stateMatrix(N); + initial.insert>(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); +} +/* ************************************************************************* */ 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); 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;