Added more tests for basis

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<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)) {}
 

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.
  *

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 <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);
+}
+/* ************************************************************************* */

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 <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);