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;