Apollo 10.0
自动驾驶开放平台
interpolation_1d.cc
浏览该文件的文档.
1/******************************************************************************
2 * Copyright 2017 The Apollo Authors. All Rights Reserved.
3 *
4 * Licensed under the Apache License, Version 2.0 (the "License");
5 * you may not use this file except in compliance with the License.
6 * You may obtain a copy of the License at
7 *
8 * http://www.apache.org/licenses/LICENSE-2.0
9 *
10 * Unless required by applicable law or agreed to in writing, software
11 * distributed under the License is distributed on an "AS IS" BASIS,
12 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13 * See the License for the specific language governing permissions and
14 * limitations under the License.
15 *****************************************************************************/
17
18#include <algorithm>
19
20#include "cyber/common/log.h"
21
22namespace apollo {
23namespace control {
24
25const double kDoubleEpsilon = 1e-6;
26
28 if (xy.empty()) {
29 AERROR << "empty input.";
30 return false;
31 }
32 auto data(xy);
33 std::sort(data.begin(), data.end());
34 Eigen::VectorXd x(data.size());
35 Eigen::VectorXd y(data.size());
36 for (unsigned i = 0; i < data.size(); ++i) {
37 x(i) = data[i].first;
38 y(i) = data[i].second;
39 }
40 x_min_ = data.front().first;
41 x_max_ = data.back().first;
42 y_start_ = data.front().second;
43 y_end_ = data.back().second;
44
45 // Spline fitting here. X values are scaled down to [0, 1] for this.
46 spline_.reset(new Eigen::Spline<double, 1>(
47 Eigen::SplineFitting<Eigen::Spline<double, 1>>::Interpolate(
48 y.transpose(),
49 // No more than cubic spline, but accept short vectors.
50 static_cast<Eigen::DenseIndex>(std::min<size_t>(x.size() - 1, 3)),
51 ScaledValues(x))));
52 return true;
53}
54
55double Interpolation1D::Interpolate(double x) const {
56 if (x < x_min_) {
57 return y_start_;
58 }
59 if (x > x_max_) {
60 return y_end_;
61 }
62 // x values need to be scaled down in extraction as well.
63 return (*spline_)(ScaledValue(x))(0);
64}
65
66double Interpolation1D::ScaledValue(double x) const {
67 if (std::fabs(x_max_ - x_min_) < kDoubleEpsilon) {
68 return x_min_;
69 }
70 return (x - x_min_) / (x_max_ - x_min_);
71}
72
73Eigen::RowVectorXd Interpolation1D::ScaledValues(
74 Eigen::VectorXd const& x_vec) const {
75 return x_vec.unaryExpr([this](double x) { return ScaledValue(x); })
76 .transpose();
77}
78
79} // namespace control
80} // namespace apollo
double Interpolate(double x) const
bool Init(const DataType &xy)
std::vector< std::pair< double, double > > DataType
#define AERROR
Definition log.h:44
const double kDoubleEpsilon
class register implement
Definition arena_queue.h:37