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,72 @@ 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);
+  }
+
+  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 Derivative<X> &y) const {
+    const double distance = this->distance_metric_(x.value, y.value);
+    const double d_x = this->distance_metric_.derivative(x.value, y.value);
+    const double d_y = this->distance_metric_.derivative(y.value, x.value);
+    const double d_xy =
+        this->distance_metric_.second_derivative(x.value, y.value);
+
+    const double f_d = squared_exponential_covariance_derivative(
+        distance, squared_exponential_length_scale.value,
+        sigma_squared_exponential.value);
+
+    const double f_dd = squared_exponential_covariance_second_derivative(
+        distance, squared_exponential_length_scale.value,
+        sigma_squared_exponential.value);
+
+    std::cout << x.value << "  " << y.value << "  " << d_xy << ", " << f_d
+              << ", " << d_x << ", " << d_y << ", " << f_dd << std::endl;
+    return d_xy * f_d + d_x * d_y * f_dd;
+  }
+
+  // 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;
+  }
+
+  // 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 SecondDerivative<X> &y) const {
+    return NAN;
+  }
+
   DistanceMetricType distance_metric_;
 };
 

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

@@ -18,6 +18,7 @@ add_executable(albatross_unit_tests
   test_gp.cc
   test_group_by.cc
   test_indexing.cc
+  test_interpolate.cc
   test_linalg_utils.cc
   test_map_utils.cc
   test_model_adapter.cc

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