interpolation_1d.h
#pragma once
#include <memory>
#include <utility>
#include <vector>
#include "Eigen/Core"
#include "unsupported/Eigen/Splines"
namespace apollo {
namespace control {
class Interpolation1D {
public:
typedef std::vector<std::pair<double, double>> DataType;
//構造函數,C++11中,當類中自定義了帶參數的構造函數,那麼編譯器
//就不會再自動生成預設構造函數,但有時候需要建立一個預設的對象但是類中編譯器又沒有自動生成一個
//預設構造函數,那麼為了讓編譯器生成這個預設構造函數就需要default這個屬性
Interpolation1D() = default;
// Return true if init is ok.
bool Init(const DataType& xy);
// Only interpolate x between [x_min, x_max]
// For x out of range, start or end y value is returned.
double Interpolate(double x) const;
private:
// Helpers to scale X values down to [0, 1] 不明白為什麼非要弄到0-1,Eigen中spline的特性嗎
double ScaledValue(double x) const;
Eigen::RowVectorXd ScaledValues(Eigen::VectorXd const& x_vec) const;
double x_min_ = 0.0;
double x_max_ = 0.0;
double y_start_ = 0.0;
double y_end_ = 0.0;
// Spline of one-dimensional "points."
//Eigen為c++線性代數庫
//unique_ptr為智能指針,可以解決記憶體洩漏,具體見收藏檔案夾
std::unique_ptr<Eigen::Spline<double, 1>> spline_;
};
} // namespace control
} // namespace apollo
interpolation_1d.cc
/******************************************************************************
* Copyright 2017 The Apollo Authors. All Rights Reserved.
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*****************************************************************************/
#include "modules/control/common/interpolation_1d.h"
#include <algorithm>
#include "cyber/common/log.h"
namespace apollo {
namespace control {
const double kDoubleEpsilon = 1e-6;
bool Interpolation1D::Init(const DataType& xy) {
if (xy.empty()) {
AERROR << "empty input.";
return false;
}
//拷貝構造
auto data(xy);
std::sort(data.begin(), data.end());
//Eigen::VectorXd:動态長度double型列向量
Eigen::VectorXd x(data.size());
Eigen::VectorXd y(data.size());
//擷取插值的上下限
for (unsigned i = 0; i < data.size(); ++i) {
x(i) = data[i].first;
y(i) = data[i].second;
}
x_min_ = data.front().first;
x_max_ = data.back().first;
y_start_ = data.front().second;
y_end_ = data.back().second;
//通過輸入插值點集合和指定插值曲線次數,進行曲線拟合,有點長 看不懂
// Spline fitting here. X values are scaled down to [0, 1] for this.
spline_.reset(new Eigen::Spline<double, 1>(
Eigen::SplineFitting<Eigen::Spline<double, 1>>::Interpolate(
y.transpose(),
// No more than cubic spline, but accept short vectors.
static_cast<Eigen::DenseIndex>(std::min<size_t>(x.size() - 1, 3)),
ScaledValues(x))));
return true;
}
double Interpolation1D::Interpolate(double x) const {
if (x < x_min_) {
return y_start_;
}
if (x > x_max_) {
return y_end_;
}
// x values need to be scaled down in extraction as well.
return (*spline_)(ScaledValue(x))(0);
}
double Interpolation1D::ScaledValue(double x) const {
if (std::fabs(x_max_ - x_min_) < kDoubleEpsilon) {
return x_min_;
}
return (x - x_min_) / (x_max_ - x_min_);
}
//看不懂
Eigen::RowVectorXd Interpolation1D::ScaledValues(
Eigen::VectorXd const& x_vec) const {
return x_vec.unaryExpr([this](double x) { return ScaledValue(x); })
.transpose();
}
} // namespace control
} // namespace apollo