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/testChebyshev.cpp b/gtsam/basis/tests/testChebyshev.cpp
index 64c925886..7d7f9323d 100644
--- a/gtsam/basis/tests/testChebyshev.cpp
+++ b/gtsam/basis/tests/testChebyshev.cpp
@@ -25,9 +25,10 @@
using namespace std;
using namespace gtsam;
+namespace {
auto model = noiseModel::Unit::Create(1);
-
const size_t N = 3;
+} // namespace
//******************************************************************************
TEST(Chebyshev, Chebyshev1) {
diff --git a/gtsam/basis/tests/testChebyshev2.cpp b/gtsam/basis/tests/testChebyshev2.cpp
index 4cee70daf..9090757f4 100644
--- a/gtsam/basis/tests/testChebyshev2.cpp
+++ b/gtsam/basis/tests/testChebyshev2.cpp
@@ -10,26 +10,30 @@
* -------------------------------------------------------------------------- */
/**
- * @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;
using namespace boost::placeholders;
+namespace {
noiseModel::Diagonal::shared_ptr model = noiseModel::Unit::Create(1);
const size_t N = 32;
+} // namespace
//******************************************************************************
TEST(Chebyshev2, Point) {
@@ -121,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;
}
@@ -144,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;
@@ -167,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;
@@ -252,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/discrete/AlgebraicDecisionTree.cpp b/gtsam/discrete/AlgebraicDecisionTree.cpp
new file mode 100644
index 000000000..83ee4051a
--- /dev/null
+++ b/gtsam/discrete/AlgebraicDecisionTree.cpp
@@ -0,0 +1,28 @@
+/* ----------------------------------------------------------------------------
+
+ * 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
+
+ * -------------------------------------------------------------------------- */
+
+/**
+ * @file AlgebraicDecisionTree.cpp
+ * @date Feb 20, 2022
+ * @author Mike Sheffler
+ * @author Duy-Nguyen Ta
+ * @author Frank Dellaert
+ */
+
+#include "AlgebraicDecisionTree.h"
+
+#include
+
+namespace gtsam {
+
+ template class AlgebraicDecisionTree;
+
+} // namespace gtsam
diff --git a/gtsam/discrete/AlgebraicDecisionTree.h b/gtsam/discrete/AlgebraicDecisionTree.h
index d2e05927a..a2ceac834 100644
--- a/gtsam/discrete/AlgebraicDecisionTree.h
+++ b/gtsam/discrete/AlgebraicDecisionTree.h
@@ -18,8 +18,13 @@
#pragma once
+#include
#include
+#include
+#include