diff --git a/include/albatross/CovarianceFunctions b/include/albatross/CovarianceFunctions index 79d2b8c6..fae8c37f 100644 --- a/include/albatross/CovarianceFunctions +++ b/include/albatross/CovarianceFunctions @@ -23,6 +23,7 @@ #include #include +#include #include #include #include diff --git a/include/albatross/Interpolate b/include/albatross/Interpolate new file mode 100644 index 00000000..ab5b5dc4 --- /dev/null +++ b/include/albatross/Interpolate @@ -0,0 +1,19 @@ +/* + * Copyright (C) 2020 Swift Navigation Inc. + * Contact: Swift Navigation + * + * 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 + +#endif diff --git a/include/albatross/src/covariance_functions/derivative.hpp b/include/albatross/src/covariance_functions/derivative.hpp new file mode 100644 index 00000000..c7d87224 --- /dev/null +++ b/include/albatross/src/covariance_functions/derivative.hpp @@ -0,0 +1,38 @@ +/* + * Copyright (C) 2020 Swift Navigation Inc. + * Contact: Swift Navigation + * + * 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 struct Derivative { + + Derivative() : value(){}; + + Derivative(const T &t) : value(t){}; + + T value; +}; + +template struct SecondDerivative { + + SecondDerivative() : value(){}; + + SecondDerivative(const T &t) : value(t){}; + + T value; +}; + +} // namespace albatross + +#endif /* INCLUDE_ALBATROSS_SRC_COVARIANCE_FUNCTIONS_DERIVATIVE_HPP_ */ diff --git a/include/albatross/src/covariance_functions/distance_metrics.hpp b/include/albatross/src/covariance_functions/distance_metrics.hpp index df4db4c7..c3ec0e4d 100644 --- a/include/albatross/src/covariance_functions/distance_metrics.hpp +++ b/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 double operator()(const Eigen::Matrix<_Scalar, _Rows, 1> &x, const Eigen::Matrix<_Scalar, _Rows, 1> &y) const { diff --git a/include/albatross/src/covariance_functions/radial.hpp b/include/albatross/src/covariance_functions/radial.hpp index deda4a1c..e7c23fb4 100644 --- a/include/albatross/src/covariance_functions/radial.hpp +++ b/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 ::value, + int>::type = 0> + double _call_impl(const Derivative &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 ::value, + int>::type = 0> + double _call_impl(const SecondDerivative &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_; }; diff --git a/include/albatross/src/models/gp.hpp b/include/albatross/src/models/gp.hpp index b1647d38..af94c085 100644 --- a/include/albatross/src/models/gp.hpp +++ b/include/albatross/src/models/gp.hpp @@ -350,13 +350,11 @@ class GaussianProcessBase : public ModelBase { return pred; } - template < - typename FeatureType, typename FitFeaturetype, - typename CovarianceRepresentation, - typename std::enable_if< - has_call_operator::value && - has_call_operator::value, - int>::type = 0> + template ::value, + int>::type = 0> Eigen::VectorXd _predict_impl( const std::vector &features, const GPFitType &gp_fit, diff --git a/include/albatross/src/models/interpolate.hpp b/include/albatross/src/models/interpolate.hpp new file mode 100644 index 00000000..a0889b87 --- /dev/null +++ b/include/albatross/src/models/interpolate.hpp @@ -0,0 +1,83 @@ +/* + * Copyright (C) 2020 Swift Navigation Inc. + * Contact: Swift Navigation + * + * 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 sqr_exp; + IndependentNoise noise; + return sqr_exp + measurement_only(noise); +} + +using InterpolationFunction = decltype(interpolation_cov_func()); + +/* + * Generic Gaussian Process Implementation. + */ +class GaussianProcessInterpolator + : public GaussianProcessBase { +public: + using Base = GaussianProcessBase; +}; + +using GPInterpolatorFitType = + typename fit_type::type; + +template <> +class Prediction { + +public: + Prediction(const GaussianProcessInterpolator &model, + const GPInterpolatorFitType &fit, + const std::vector &features) + : model_(model), fit_(fit), features_(features) {} + + Prediction(GaussianProcessInterpolator &&model, GPInterpolatorFitType &&fit, + const std::vector &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_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> 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 features_; +}; + +} // namespace albatross +#endif /* INCLUDE_ALBATROSS_SRC_MODELS_INTERPOLATE_HPP_ */ diff --git a/tests/CMakeLists.txt b/tests/CMakeLists.txt index 53e9fade..8494a40d 100644 --- a/tests/CMakeLists.txt +++ b/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}" diff --git a/tests/test_interpolate.cc b/tests/test_interpolate.cc new file mode 100644 index 00000000..486ba934 --- /dev/null +++ b/tests/test_interpolate.cc @@ -0,0 +1,64 @@ +/* + * Copyright (C) 2020 Swift Navigation Inc. + * Contact: Swift Navigation + * + * 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 + +#include + +namespace albatross { + +std::vector uniform_points_on_line(const std::size_t n, + const double low, + const double high) { + std::vector 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