Add an interpolation model which provides special methods for

computing first and second derivatives of the interpolated mean

include/albatross/CovarianceFunctions

@@ -23,6 +23,7 @@
 
 #include <albatross/src/covariance_functions/traits.hpp>
 #include <albatross/src/covariance_functions/callers.hpp>
+#include <albatross/src/covariance_functions/derivative.hpp>
 #include <albatross/src/covariance_functions/covariance_function.hpp>
 #include <albatross/src/covariance_functions/mean_function.hpp>
 #include <albatross/src/covariance_functions/call_trace.hpp>

include/albatross/Interpolate

@@ -0,0 +1,19 @@
+/*
+ * Copyright (C) 2020 Swift Navigation Inc.
+ * Contact: Swift Navigation <dev@swiftnav.com>
+ *
+ * This source is subject to the license found in the file 'LICENSE' which must
+ * be distributed together with this source. All other rights reserved.
+ *
+ * THIS CODE AND INFORMATION IS PROVIDED "AS IS" WITHOUT WARRANTY OF ANY KIND,
+ * EITHER EXPRESSED OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE IMPLIED
+ * WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A PARTICULAR PURPOSE.
+ */
+
+#ifndef ALBATROSS_INTERPOLATE_H
+#define ALBATROSS_INTERPOLATE_H
+
+#include "GP"
+#include <albatross/src/models/interpolate.hpp>
+
+#endif

include/albatross/src/covariance_functions/derivative.hpp

@@ -0,0 +1,38 @@
+/*
+ * Copyright (C) 2020 Swift Navigation Inc.
+ * Contact: Swift Navigation <dev@swiftnav.com>
+ *
+ * This source is subject to the license found in the file 'LICENSE' which must
+ * be distributed together with this source. All other rights reserved.
+ *
+ * THIS CODE AND INFORMATION IS PROVIDED "AS IS" WITHOUT WARRANTY OF ANY KIND,
+ * EITHER EXPRESSED OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE IMPLIED
+ * WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A PARTICULAR PURPOSE.
+ */
+
+#ifndef INCLUDE_ALBATROSS_SRC_COVARIANCE_FUNCTIONS_DERIVATIVE_HPP_
+#define INCLUDE_ALBATROSS_SRC_COVARIANCE_FUNCTIONS_DERIVATIVE_HPP_
+
+namespace albatross {
+
+template <typename T> struct Derivative {
+
+  Derivative() : value(){};
+
+  Derivative(const T &t) : value(t){};
+
+  T value;
+};
+
+template <typename T> struct SecondDerivative {
+
+  SecondDerivative() : value(){};
+
+  SecondDerivative(const T &t) : value(t){};
+
+  T value;
+};
+
+} // namespace albatross
+
+#endif /* INCLUDE_ALBATROSS_SRC_COVARIANCE_FUNCTIONS_DERIVATIVE_HPP_ */

include/albatross/src/covariance_functions/distance_metrics.hpp

@@ -37,6 +37,17 @@ class EuclideanDistance : public DistanceMetric {
     return fabs(x - y);
   }
 
+  double derivative(const double &x, const double &y) const {
+    if (x - y == 0.) {
+      return 1.;
+    }
+    return (x - y) / fabs(x - y);
+  }
+
+  double second_derivative(const double &x, const double &y) const {
+    return 0.;
+  }
+
   template <typename _Scalar, int _Rows>
   double operator()(const Eigen::Matrix<_Scalar, _Rows, 1> &x,
                     const Eigen::Matrix<_Scalar, _Rows, 1> &y) const {

include/albatross/src/covariance_functions/radial.hpp

@@ -28,6 +28,26 @@ inline double squared_exponential_covariance(double distance,
   return sigma * sigma * exp(-pow(distance / length_scale, 2));
 }
 
+inline double squared_exponential_covariance_derivative(double distance,
+                                                        double length_scale,
+                                                        double sigma = 1.) {
+  if (length_scale <= 0.) {
+    return 0.;
+  }
+  return -2 * distance / (length_scale * length_scale) *
+         squared_exponential_covariance(distance, length_scale, sigma);
+}
+
+inline double squared_exponential_covariance_second_derivative(
+    double distance, double length_scale, double sigma = 1.) {
+  if (length_scale <= 0.) {
+    return 0.;
+  }
+  const auto ratio = distance / length_scale;
+  return (4. * ratio * ratio - 2.) / (length_scale * length_scale) *
+         squared_exponential_covariance(distance, length_scale, sigma);
+}
+
 /*
  * SquaredExponential distance
  *    covariance(d) = sigma^2 exp(-(d/length_scale)^2)
@@ -83,6 +103,38 @@ class SquaredExponential
         sigma_squared_exponential.value);
   }
 
+  // This operator is only defined when the distance metric is also defined.
+  template <typename X,
+            typename std::enable_if<
+                has_call_operator<DistanceMetricType, X &, X &>::value,
+                int>::type = 0>
+  double _call_impl(const Derivative<X> &x, const X &y) const {
+    double distance = this->distance_metric_(x.value, y);
+    double distance_derivative = this->distance_metric_.derivative(x.value, y);
+    return distance_derivative * squared_exponential_covariance_derivative(
+                                     distance,
+                                     squared_exponential_length_scale.value,
+                                     sigma_squared_exponential.value);
+  }
+
+  // This operator is only defined when the distance metric is also defined.
+  template <typename X,
+            typename std::enable_if<
+                has_call_operator<DistanceMetricType, X &, X &>::value,
+                int>::type = 0>
+  double _call_impl(const SecondDerivative<X> &x, const X &y) const {
+    double d = this->distance_metric_(x.value, y);
+    double d_1 = this->distance_metric_.derivative(x.value, y);
+    double d_2 = this->distance_metric_.second_derivative(x.value, y);
+    double f_1 = squared_exponential_covariance_derivative(
+        d, squared_exponential_length_scale.value,
+        sigma_squared_exponential.value);
+    double f_2 = squared_exponential_covariance_second_derivative(
+        d, squared_exponential_length_scale.value,
+        sigma_squared_exponential.value);
+    return d_2 * f_1 + d_1 * d_1 * f_2;
+  }
+
   DistanceMetricType distance_metric_;
 };
 

include/albatross/src/models/gp.hpp

@@ -350,13 +350,11 @@ class GaussianProcessBase : public ModelBase<ImplType> {
     return pred;
   }
 
-  template <
-      typename FeatureType, typename FitFeaturetype,
-      typename CovarianceRepresentation,
-      typename std::enable_if<
-          has_call_operator<CovFunc, FeatureType, FeatureType>::value &&
-              has_call_operator<CovFunc, FeatureType, FitFeaturetype>::value,
-          int>::type = 0>
+  template <typename FeatureType, typename FitFeaturetype,
+            typename CovarianceRepresentation,
+            typename std::enable_if<
+                has_call_operator<CovFunc, FeatureType, FitFeaturetype>::value,
+                int>::type = 0>
   Eigen::VectorXd _predict_impl(
       const std::vector<FeatureType> &features,
       const GPFitType<CovarianceRepresentation, FitFeaturetype> &gp_fit,

include/albatross/src/models/interpolate.hpp

@@ -0,0 +1,83 @@
+/*
+ * Copyright (C) 2020 Swift Navigation Inc.
+ * Contact: Swift Navigation <dev@swiftnav.com>
+ *
+ * This source is subject to the license found in the file 'LICENSE' which must
+ * be distributed together with this source. All other rights reserved.
+ *
+ * THIS CODE AND INFORMATION IS PROVIDED "AS IS" WITHOUT WARRANTY OF ANY KIND,
+ * EITHER EXPRESSED OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE IMPLIED
+ * WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A PARTICULAR PURPOSE.
+ */
+
+#ifndef INCLUDE_ALBATROSS_SRC_MODELS_INTERPOLATE_HPP_
+#define INCLUDE_ALBATROSS_SRC_MODELS_INTERPOLATE_HPP_
+
+namespace albatross {
+
+auto interpolation_cov_func() {
+  SquaredExponential<EuclideanDistance> sqr_exp;
+  IndependentNoise<double> noise;
+  return sqr_exp + measurement_only(noise);
+}
+
+using InterpolationFunction = decltype(interpolation_cov_func());
+
+/*
+ * Generic Gaussian Process Implementation.
+ */
+class GaussianProcessInterpolator
+    : public GaussianProcessBase<InterpolationFunction, ZeroMean,
+                                 GaussianProcessInterpolator> {
+public:
+  using Base = GaussianProcessBase<InterpolationFunction, ZeroMean,
+                                   GaussianProcessInterpolator>;
+};
+
+using GPInterpolatorFitType =
+    typename fit_type<GaussianProcessInterpolator, double>::type;
+
+template <>
+class Prediction<GaussianProcessInterpolator, double, GPInterpolatorFitType> {
+
+public:
+  Prediction(const GaussianProcessInterpolator &model,
+             const GPInterpolatorFitType &fit,
+             const std::vector<double> &features)
+      : model_(model), fit_(fit), features_(features) {}
+
+  Prediction(GaussianProcessInterpolator &&model, GPInterpolatorFitType &&fit,
+             const std::vector<double> &features)
+      : model_(std::move(model)), fit_(std::move(fit)), features_(features) {}
+
+  // Mean
+  Eigen::VectorXd mean() const {
+    return MeanPredictor()._mean(model_, fit_, features_);
+  }
+
+  Eigen::VectorXd derivative() const {
+
+    std::vector<Derivative<double>> derivative_features;
+    for (const auto &f : features_) {
+      derivative_features.emplace_back(f);
+    }
+
+    return MeanPredictor()._mean(model_, fit_, derivative_features);
+  }
+
+  Eigen::VectorXd second_derivative() const {
+
+    std::vector<SecondDerivative<double>> derivative_features;
+    for (const auto &f : features_) {
+      derivative_features.emplace_back(f);
+    }
+    return MeanPredictor()._mean(model_, fit_, derivative_features);
+  }
+
+  const GaussianProcessInterpolator model_;
+  const GPInterpolatorFitType fit_;
+  const std::vector<double> features_;
+};
+
+} // namespace albatross
+#endif /* INCLUDE_ALBATROSS_SRC_MODELS_INTERPOLATE_HPP_ */

tests/CMakeLists.txt

@@ -1,48 +1,5 @@
 add_executable(albatross_unit_tests 
-  test_apply.cc
-  test_async_utils.cc
-  test_block_utils.cc
-  test_call_trace.cc
-  test_callers.cc
-  test_concatenate.cc
-  test_core_dataset.cc
-  test_core_distribution.cc
-  test_core_model.cc
-  test_covariance_function.cc
-  test_covariance_functions.cc
-  test_cross_validation.cc
-  test_csv_utils.cc
-  test_distance_metrics.cc
-  test_eigen_utils.cc
-  test_evaluate.cc
-  test_gp.cc
-  test_group_by.cc
-  test_indexing.cc
-  test_linalg_utils.cc
-  test_map_utils.cc
-  test_model_adapter.cc
-  test_model_metrics.cc  
-  test_models.cc
-  test_parameter_handling_mixin.cc
-  test_patchwork_gp.cc
-  test_prediction.cc
-  test_radial.cc
-  test_random_utils.cc
-  test_ransac.cc
-  test_samplers.cc
-  test_scaling_function.cc
-  test_serializable_ldlt.cc
-  test_serialize.cc
-  test_sparse_gp.cc
-  test_stats.cc
-  test_traits_cereal.cc
-  test_traits_core.cc
-  test_traits_details.cc
-  test_traits_covariance_functions.cc
-  test_traits_evaluation.cc
-  test_traits_indexing.cc
-  test_tune.cc
-  test_variant_utils.cc
+  test_interpolate.cc
   )
 target_include_directories(albatross_unit_tests SYSTEM PRIVATE
   "${gtest_SOURCE_DIR}"

tests/test_interpolate.cc

@@ -0,0 +1,64 @@
+/*
+ * Copyright (C) 2020 Swift Navigation Inc.
+ * Contact: Swift Navigation <dev@swiftnav.com>
+ *
+ * This source is subject to the license found in the file 'LICENSE' which must
+ * be distributed together with this source. All other rights reserved.
+ *
+ * THIS CODE AND INFORMATION IS PROVIDED "AS IS" WITHOUT WARRANTY OF ANY KIND,
+ * EITHER EXPRESSED OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE IMPLIED
+ * WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A PARTICULAR PURPOSE.
+ */
+
+#include <albatross/Interpolate>
+
+#include <gtest/gtest.h>
+
+namespace albatross {
+
+std::vector<double> uniform_points_on_line(const std::size_t n,
+                                           const double low,
+                                           const double high) {
+  std::vector<double> xs;
+  for (std::size_t i = 0; i < n; i++) {
+    double ratio = (double)i / (double)(n - 1);
+    xs.push_back(low + ratio * (high - low));
+  }
+  return xs;
+};
+
+TEST(test_interpolator, test_interpolate) {
+
+  const auto xs = uniform_points_on_line(21, 0., 2 * 3.14159);
+
+  Eigen::VectorXd targets(xs.size());
+  for (std::size_t i = 0; i < xs.size(); ++i) {
+    targets[i] = std::sin(xs[i]);
+  }
+
+  const auto interp_xs = uniform_points_on_line(101, 0., 2 * 3.14159);
+
+  Eigen::VectorXd mean_truth(interp_xs.size());
+  Eigen::VectorXd derivative_truth(interp_xs.size());
+  Eigen::VectorXd second_derivative_truth(interp_xs.size());
+
+  for (std::size_t i = 0; i < interp_xs.size(); ++i) {
+    mean_truth[i] = std::sin(interp_xs[i]);
+    derivative_truth[i] = std::cos(interp_xs[i]);
+    second_derivative_truth[i] = -std::sin(interp_xs[i]);
+  }
+
+  GaussianProcessInterpolator interpolator;
+  interpolator.set_param("squared_exponential_length_scale", 10.);
+  interpolator.set_param("sigma_squared_exponential", 10.);
+  interpolator.set_param("sigma_independent_noise", 1e-6);
+
+  const auto predictor = interpolator.fit(xs, targets).predict(interp_xs);
+
+  EXPECT_LT((predictor.mean() - mean_truth).norm(), 1e-3);
+  EXPECT_LT((predictor.derivative() - derivative_truth).norm(), 1e-2);
+  EXPECT_LT((predictor.second_derivative() - second_derivative_truth).norm(),
+            1e-1);
+}
+
+} // namespace albatross