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