Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Added more tests for basis #1121

Merged
merged 5 commits into from
Feb 28, 2022
Merged

Added more tests for basis #1121

Changes from 1 commit
Commits
Show all changes
5 commits
Select commit Hold shift + click to select a range
5f9082b
docs
dellaert Feb 27, 2022
a16f588
Add test for ManifoldEvaluationFunctor
dellaert Feb 27, 2022
c85b649
Move BasisFactors tests to correct place
dellaert Feb 27, 2022
3d8c7b9
Add EvaluationFactor test
dellaert Feb 27, 2022
6fb38aa
Add test for ManifoldEvaluationFactor
dellaert Feb 27, 2022
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
Prev Previous commit
Add test for ManifoldEvaluationFactor
  • Loading branch information
@dellaert
dellaert committed Feb 27, 2022
commit 6fb38aa8d7f009aefe42c88e5ca7b5eca6fb287d
41 changes: 29 additions & 12 deletions gtsam/basis/tests/testBasisFactors.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -20,6 +20,7 @@
#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>
Expand All @@ -31,6 +32,7 @@
#include <CppUnitLite/TestHarness.h>

using gtsam::noiseModel::Isotropic;
using gtsam::Pose2;
using gtsam::Vector;
using gtsam::Values;
using gtsam::Chebyshev2;
Expand Down Expand Up @@ -64,8 +66,6 @@ TEST(BasisFactors, EvaluationFactor) {
initial.insert<Vector>(key, functionValues);

LevenbergMarquardtParams parameters;
parameters.verbosity = NonlinearOptimizerParams::SILENT;
parameters.verbosityLM = LevenbergMarquardtParams::SILENT;
parameters.setMaxIterations(20);
Values result =
LevenbergMarquardtOptimizer(graph, initial, parameters).optimize();
Expand All @@ -92,8 +92,6 @@ TEST(BasisFactors, VectorEvaluationFactor) {
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();
Expand Down Expand Up @@ -130,20 +128,43 @@ 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<Chebyshev2, P> controlPrior(key, measured, model, N, i,
VectorComponentFactor<Chebyshev2, P> factor(key, measured, model, N, i,
t, a, b);

NonlinearFactorGraph graph;
graph.add(controlPrior);
graph.add(factor);

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(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();
Expand All @@ -169,8 +190,6 @@ TEST(BasisFactors, VecDerivativePrior) {
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();
Expand All @@ -196,8 +215,6 @@ TEST(BasisFactors, ComponentDerivativeFactor) {
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();
Expand Down