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