Skip to content

Commit cfa6bc7

Browse files
akleemanakleeman
committed
Add an interpolation model which provides special methods for
computing first and second derivatives of the interpolated mean
1 parent 81b8704 commit cfa6bc7

File tree

8 files changed

+303
-0
lines changed

8 files changed

+303
-0
lines changed

‎include/albatross/CovarianceFunctions‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -23,6 +23,7 @@
2323

2424
#include <albatross/src/covariance_functions/traits.hpp>
2525
#include <albatross/src/covariance_functions/callers.hpp>
26+
#include <albatross/src/covariance_functions/derivative.hpp>
2627
#include <albatross/src/covariance_functions/covariance_function.hpp>
2728
#include <albatross/src/covariance_functions/mean_function.hpp>
2829
#include <albatross/src/covariance_functions/call_trace.hpp>

‎include/albatross/Interpolate‎

Lines changed: 19 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,19 @@
1+
/*
2+
* Copyright (C) 2020 Swift Navigation Inc.
3+
* Contact: Swift Navigation <dev@swiftnav.com>
4+
*
5+
* This source is subject to the license found in the file 'LICENSE' which must
6+
* be distributed together with this source. All other rights reserved.
7+
*
8+
* THIS CODE AND INFORMATION IS PROVIDED "AS IS" WITHOUT WARRANTY OF ANY KIND,
9+
* EITHER EXPRESSED OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE IMPLIED
10+
* WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A PARTICULAR PURPOSE.
11+
*/
12+
13+
#ifndef ALBATROSS_INTERPOLATE_H
14+
#define ALBATROSS_INTERPOLATE_H
15+
16+
#include "GP"
17+
#include <albatross/src/models/interpolate.hpp>
18+
19+
#endif

‎include/albatross/src/covariance_functions/derivative.hpp‎

Lines changed: 38 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,38 @@
1+
/*
2+
* Copyright (C) 2020 Swift Navigation Inc.
3+
* Contact: Swift Navigation <dev@swiftnav.com>
4+
*
5+
* This source is subject to the license found in the file 'LICENSE' which must
6+
* be distributed together with this source. All other rights reserved.
7+
*
8+
* THIS CODE AND INFORMATION IS PROVIDED "AS IS" WITHOUT WARRANTY OF ANY KIND,
9+
* EITHER EXPRESSED OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE IMPLIED
10+
* WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A PARTICULAR PURPOSE.
11+
*/
12+
13+
#ifndef INCLUDE_ALBATROSS_SRC_COVARIANCE_FUNCTIONS_DERIVATIVE_HPP_
14+
#define INCLUDE_ALBATROSS_SRC_COVARIANCE_FUNCTIONS_DERIVATIVE_HPP_
15+
16+
namespace albatross {
17+
18+
template <typename T> struct Derivative {
19+
20+
Derivative() : value(){};
21+
22+
Derivative(const T &t) : value(t){};
23+
24+
T value;
25+
};
26+
27+
template <typename T> struct SecondDerivative {
28+
29+
SecondDerivative() : value(){};
30+
31+
SecondDerivative(const T &t) : value(t){};
32+
33+
T value;
34+
};
35+
36+
} // namespace albatross
37+
38+
#endif /* INCLUDE_ALBATROSS_SRC_COVARIANCE_FUNCTIONS_DERIVATIVE_HPP_ */

‎include/albatross/src/covariance_functions/distance_metrics.hpp‎

Lines changed: 11 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -37,6 +37,17 @@ class EuclideanDistance : public DistanceMetric {
3737
return fabs(x - y);
3838
}
3939

40+
double derivative(const double &x, const double &y) const {
41+
if (x - y == 0.) {
42+
return 1.;
43+
}
44+
return (x - y) / fabs(x - y);
45+
}
46+
47+
double second_derivative(const double &x, const double &y) const {
48+
return 0.;
49+
}
50+
4051
template <typename _Scalar, int _Rows>
4152
double operator()(const Eigen::Matrix<_Scalar, _Rows, 1> &x,
4253
const Eigen::Matrix<_Scalar, _Rows, 1> &y) const {

‎include/albatross/src/covariance_functions/radial.hpp‎

Lines changed: 86 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -28,6 +28,26 @@ inline double squared_exponential_covariance(double distance,
2828
return sigma * sigma * exp(-pow(distance / length_scale, 2));
2929
}
3030

31+
inline double squared_exponential_covariance_derivative(double distance,
32+
double length_scale,
33+
double sigma = 1.) {
34+
if (length_scale <= 0.) {
35+
return 0.;
36+
}
37+
return -2 * distance / (length_scale * length_scale) *
38+
squared_exponential_covariance(distance, length_scale, sigma);
39+
}
40+
41+
inline double squared_exponential_covariance_second_derivative(
42+
double distance, double length_scale, double sigma = 1.) {
43+
if (length_scale <= 0.) {
44+
return 0.;
45+
}
46+
const auto ratio = distance / length_scale;
47+
return (4. * ratio * ratio - 2.) / (length_scale * length_scale) *
48+
squared_exponential_covariance(distance, length_scale, sigma);
49+
}
50+
3151
/*
3252
* SquaredExponential distance
3353
* covariance(d) = sigma^2 exp(-(d/length_scale)^2)
@@ -83,6 +103,72 @@ class SquaredExponential
83103
sigma_squared_exponential.value);
84104
}
85105

106+
// This operator is only defined when the distance metric is also defined.
107+
template <typename X,
108+
typename std::enable_if<
109+
has_call_operator<DistanceMetricType, X &, X &>::value,
110+
int>::type = 0>
111+
double _call_impl(const Derivative<X> &x, const X &y) const {
112+
double distance = this->distance_metric_(x.value, y);
113+
double distance_derivative = this->distance_metric_.derivative(x.value, y);
114+
return distance_derivative * squared_exponential_covariance_derivative(
115+
distance,
116+
squared_exponential_length_scale.value,
117+
sigma_squared_exponential.value);
118+
}
119+
120+
template <typename X,
121+
typename std::enable_if<
122+
has_call_operator<DistanceMetricType, X &, X &>::value,
123+
int>::type = 0>
124+
double _call_impl(const Derivative<X> &x, const Derivative<X> &y) const {
125+
const double distance = this->distance_metric_(x.value, y.value);
126+
const double d_x = this->distance_metric_.derivative(x.value, y.value);
127+
const double d_y = this->distance_metric_.derivative(y.value, x.value);
128+
const double d_xy =
129+
this->distance_metric_.second_derivative(x.value, y.value);
130+
131+
const double f_d = squared_exponential_covariance_derivative(
132+
distance, squared_exponential_length_scale.value,
133+
sigma_squared_exponential.value);
134+
135+
const double f_dd = squared_exponential_covariance_second_derivative(
136+
distance, squared_exponential_length_scale.value,
137+
sigma_squared_exponential.value);
138+
139+
std::cout << x.value << " " << y.value << " " << d_xy << ", " << f_d
140+
<< ", " << d_x << ", " << d_y << ", " << f_dd << std::endl;
141+
return d_xy * f_d + d_x * d_y * f_dd;
142+
}
143+
144+
// This operator is only defined when the distance metric is also defined.
145+
template <typename X,
146+
typename std::enable_if<
147+
has_call_operator<DistanceMetricType, X &, X &>::value,
148+
int>::type = 0>
149+
double _call_impl(const SecondDerivative<X> &x, const X &y) const {
150+
double d = this->distance_metric_(x.value, y);
151+
double d_1 = this->distance_metric_.derivative(x.value, y);
152+
double d_2 = this->distance_metric_.second_derivative(x.value, y);
153+
double f_1 = squared_exponential_covariance_derivative(
154+
d, squared_exponential_length_scale.value,
155+
sigma_squared_exponential.value);
156+
double f_2 = squared_exponential_covariance_second_derivative(
157+
d, squared_exponential_length_scale.value,
158+
sigma_squared_exponential.value);
159+
return d_2 * f_1 + d_1 * d_1 * f_2;
160+
}
161+
162+
// This operator is only defined when the distance metric is also defined.
163+
template <typename X,
164+
typename std::enable_if<
165+
has_call_operator<DistanceMetricType, X &, X &>::value,
166+
int>::type = 0>
167+
double _call_impl(const SecondDerivative<X> &x,
168+
const SecondDerivative<X> &y) const {
169+
return NAN;
170+
}
171+
86172
DistanceMetricType distance_metric_;
87173
};
88174

‎include/albatross/src/models/interpolate.hpp‎

Lines changed: 83 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,83 @@
1+
/*
2+
* Copyright (C) 2020 Swift Navigation Inc.
3+
* Contact: Swift Navigation <dev@swiftnav.com>
4+
*
5+
* This source is subject to the license found in the file 'LICENSE' which must
6+
* be distributed together with this source. All other rights reserved.
7+
*
8+
* THIS CODE AND INFORMATION IS PROVIDED "AS IS" WITHOUT WARRANTY OF ANY KIND,
9+
* EITHER EXPRESSED OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE IMPLIED
10+
* WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A PARTICULAR PURPOSE.
11+
*/
12+
13+
#ifndef INCLUDE_ALBATROSS_SRC_MODELS_INTERPOLATE_HPP_
14+
#define INCLUDE_ALBATROSS_SRC_MODELS_INTERPOLATE_HPP_
15+
16+
namespace albatross {
17+
18+
auto interpolation_cov_func() {
19+
SquaredExponential<EuclideanDistance> sqr_exp;
20+
IndependentNoise<double> noise;
21+
return sqr_exp + measurement_only(noise);
22+
}
23+
24+
using InterpolationFunction = decltype(interpolation_cov_func());
25+
26+
/*
27+
* Generic Gaussian Process Implementation.
28+
*/
29+
class GaussianProcessInterpolator
30+
: public GaussianProcessBase<InterpolationFunction, ZeroMean,
31+
GaussianProcessInterpolator> {
32+
public:
33+
using Base = GaussianProcessBase<InterpolationFunction, ZeroMean,
34+
GaussianProcessInterpolator>;
35+
};
36+
37+
using GPInterpolatorFitType =
38+
typename fit_type<GaussianProcessInterpolator, double>::type;
39+
40+
template <>
41+
class Prediction<GaussianProcessInterpolator, double, GPInterpolatorFitType> {
42+
43+
public:
44+
Prediction(const GaussianProcessInterpolator &model,
45+
const GPInterpolatorFitType &fit,
46+
const std::vector<double> &features)
47+
: model_(model), fit_(fit), features_(features) {}
48+
49+
Prediction(GaussianProcessInterpolator &&model, GPInterpolatorFitType &&fit,
50+
const std::vector<double> &features)
51+
: model_(std::move(model)), fit_(std::move(fit)), features_(features) {}
52+
53+
// Mean
54+
Eigen::VectorXd mean() const {
55+
return MeanPredictor()._mean(model_, fit_, features_);
56+
}
57+
58+
Eigen::VectorXd derivative() const {
59+
60+
std::vector<Derivative<double>> derivative_features;
61+
for (const auto &f : features_) {
62+
derivative_features.emplace_back(f);
63+
}
64+
65+
return MeanPredictor()._mean(model_, fit_, derivative_features);
66+
}
67+
68+
Eigen::VectorXd second_derivative() const {
69+
70+
std::vector<SecondDerivative<double>> derivative_features;
71+
for (const auto &f : features_) {
72+
derivative_features.emplace_back(f);
73+
}
74+
return MeanPredictor()._mean(model_, fit_, derivative_features);
75+
}
76+
77+
const GaussianProcessInterpolator model_;
78+
const GPInterpolatorFitType fit_;
79+
const std::vector<double> features_;
80+
};
81+
82+
} // namespace albatross
83+
#endif /* INCLUDE_ALBATROSS_SRC_MODELS_INTERPOLATE_HPP_ */

‎tests/CMakeLists.txt‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -18,6 +18,7 @@ add_executable(albatross_unit_tests
1818
test_gp.cc
1919
test_group_by.cc
2020
test_indexing.cc
21+
test_interpolate.cc
2122
test_linalg_utils.cc
2223
test_map_utils.cc
2324
test_model_adapter.cc

‎tests/test_interpolate.cc‎

Lines changed: 64 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,64 @@
1+
/*
2+
* Copyright (C) 2020 Swift Navigation Inc.
3+
* Contact: Swift Navigation <dev@swiftnav.com>
4+
*
5+
* This source is subject to the license found in the file 'LICENSE' which must
6+
* be distributed together with this source. All other rights reserved.
7+
*
8+
* THIS CODE AND INFORMATION IS PROVIDED "AS IS" WITHOUT WARRANTY OF ANY KIND,
9+
* EITHER EXPRESSED OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE IMPLIED
10+
* WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A PARTICULAR PURPOSE.
11+
*/
12+
13+
#include <albatross/Interpolate>
14+
15+
#include <gtest/gtest.h>
16+
17+
namespace albatross {
18+
19+
std::vector<double> uniform_points_on_line(const std::size_t n,
20+
const double low,
21+
const double high) {
22+
std::vector<double> xs;
23+
for (std::size_t i = 0; i < n; i++) {
24+
double ratio = (double)i / (double)(n - 1);
25+
xs.push_back(low + ratio * (high - low));
26+
}
27+
return xs;
28+
};
29+
30+
TEST(test_interpolator, test_interpolate) {
31+
32+
const auto xs = uniform_points_on_line(21, 0., 2 * 3.14159);
33+
34+
Eigen::VectorXd targets(xs.size());
35+
for (std::size_t i = 0; i < xs.size(); ++i) {
36+
targets[i] = std::sin(xs[i]);
37+
}
38+
39+
const auto interp_xs = uniform_points_on_line(101, 0., 2 * 3.14159);
40+
41+
Eigen::VectorXd mean_truth(interp_xs.size());
42+
Eigen::VectorXd derivative_truth(interp_xs.size());
43+
Eigen::VectorXd second_derivative_truth(interp_xs.size());
44+
45+
for (std::size_t i = 0; i < interp_xs.size(); ++i) {
46+
mean_truth[i] = std::sin(interp_xs[i]);
47+
derivative_truth[i] = std::cos(interp_xs[i]);
48+
second_derivative_truth[i] = -std::sin(interp_xs[i]);
49+
}
50+
51+
GaussianProcessInterpolator interpolator;
52+
interpolator.set_param("squared_exponential_length_scale", 10.);
53+
interpolator.set_param("sigma_squared_exponential", 10.);
54+
interpolator.set_param("sigma_independent_noise", 1e-6);
55+
56+
const auto predictor = interpolator.fit(xs, targets).predict(interp_xs);
57+
58+
EXPECT_LT((predictor.mean() - mean_truth).norm(), 1e-3);
59+
EXPECT_LT((predictor.derivative() - derivative_truth).norm(), 1e-2);
60+
EXPECT_LT((predictor.second_derivative() - second_derivative_truth).norm(),
61+
1e-1);
62+
}
63+
64+
} // namespace albatross

0 commit comments

Comments
 (0)