Skip to content

A
apollo

Pinoxchio / apollo
与 Fork 源项目一致

从无法访问的项目Fork

通知 2
Star 0
Fork 0
A
apollo

体验新版 GitCode，发现更多精彩内容 >>

提交 77c5d112 编写于 作者: Z Zhang Liangliang 提交者： Jiangtao Hu
浏览文件
操作

used integral in common/math, removed integration from planning/math

上级 86e34f7e
隐藏空白更改
内联 并排
Showing with 100 addition and 208 deletion
modules/common/math/BUILD
浏览文件 @ 77c5d112
......@@ -248,6 +248,9 @@ cc_library(
hdrs = [
"integral.h",
],
deps = [
"//modules/common:log",
],
)
cc_library(
......
modules/common/math/integral.cc
浏览文件 @ 77c5d112
......@@ -16,12 +16,41 @@
#include "modules/common/math/integral.h"
#include <array>
#include "modules/common/log.h"
namespace apollo {
namespace common {
namespace math {
double GaussLegendre(const std::function<double(double)> &func,
const double lower_bound, const double upper_bound) {
double IntegrateBySimpson(const std::vector<double>& func, const double dx,
const std::size_t nsteps) {
CHECK_EQ(1, nsteps & 1);
double sum1 = 0.0;
double sum2 = 0.0;
for (std::size_t i = 1; i + 1 < nsteps; ++i) {
if ((i & 1) != 0) {
sum1 += func[i];
} else {
sum2 += func[i];
}
}
return dx / 3.0 * (4.0 * sum1 + 2.0 * sum2 + func[0] + func[nsteps - 1]);
}
double IntegrateByTrapezoidal(const std::vector<double>& func, const double dx,
const std::size_t nsteps) {
double sum = 0;
for (std::size_t i = 1; i + 1 < nsteps; ++i) {
sum += func[i];
}
return dx * sum + 0.5 * dx * (func[0] + func[nsteps - 1]);
}
double IntegrateByGaussLegendre(const std::function<double(double)>& func,
const double lower_bound,
const double upper_bound) {
double t = (upper_bound - lower_bound) * 0.5;
double m = (upper_bound + lower_bound) * 0.5;
......
modules/common/math/integral.h
浏览文件 @ 77c5d112
......@@ -23,6 +23,7 @@
#define MODULES_COMMON_MATH_INTEGRATION_H_
#include <functional>
#include <vector>
/**
* @namespace apollo::common::math
......@@ -32,6 +33,12 @@ namespace apollo {
namespace common {
namespace math {
double IntegrateBySimpson(const std::vector<double>& funv_vec, const double dx,
const std::size_t nsteps);
double IntegrateByTrapezoidal(const std::vector<double>& funv_vec,
const double dx, const std::size_t nsteps);
// Given a target function and integral lower and upper bound,
// compute the integral approximation using 5th order Gauss-Legendre
// integration.
......@@ -52,8 +59,9 @@ namespace math {
* @param upper_bound The upper bound of the integral
* @return The integral result
*/
double GaussLegendre(const std::function<double(double)> &func,
const double lower_bound, const double upper_bound);
double IntegrateByGaussLegendre(const std::function<double(double)>& func,
const double lower_bound,
const double upper_bound);
} // namespace math
} // namespace common
......
modules/common/math/integral_test.cc
浏览文件 @ 77c5d112
......@@ -37,13 +37,13 @@ double SinFunc(double x) { return std::sin(x); }
} // namespace
TEST(IntegralTest, Integration) {
double linear_integral = GaussLegendre(LinearFunc, 0.0, 1.0);
double linear_integral = IntegrateByGaussLegendre(LinearFunc, 0.0, 1.0);
EXPECT_NEAR(linear_integral, 1.0, 1e-5);
double square_integral = GaussLegendre(SquareFunc, 0.0, 1.0);
double square_integral = IntegrateByGaussLegendre(SquareFunc, 0.0, 1.0);
EXPECT_NEAR(square_integral, 1.0 / 3.0, 1e-5);
double cubic_integral = GaussLegendre(CubicFunc, 0.0, 1.0);
double cubic_integral = IntegrateByGaussLegendre(CubicFunc, 0.0, 1.0);
EXPECT_NEAR(cubic_integral, 1.0 / 4.0, 1e-5);
double sin_integral = GaussLegendre(SinFunc, 0.0, 0.5 * M_PI);
double sin_integral = IntegrateByGaussLegendre(SinFunc, 0.0, 0.5 * M_PI);
EXPECT_NEAR(sin_integral, 1.0, 1e-5);
}
......
modules/planning/common/path/BUILD
浏览文件 @ 77c5d112
......@@ -15,7 +15,6 @@ cc_library(
deps = [
"//modules/common/proto:path_point_proto",
"//modules/planning/math:hermite_spline",
"//modules/planning/math:integration",
"//modules/common/math:math",
"@eigen//:eigen",
],
......
modules/planning/common/path/path_point_util.cc
浏览文件 @ 77c5d112
......@@ -22,9 +22,9 @@
#include <utility>
#include "modules/common/math/integral.h"
#include "modules/common/math/linear_interpolation.h"
#include "modules/planning/math/hermite_spline.h"
#include "modules/planning/math/integration.h"
namespace apollo {
namespace planning {
......@@ -51,8 +51,10 @@ PathPoint interpolate(const PathPoint& p0, const PathPoint& p1,
return std::sin(theta);
};
double x = p0.x() + Integration::gauss_legendre(func_cos_theta, s0, s);
double y = p0.y() + Integration::gauss_legendre(func_sin_theta, s0, s);
double x = p0.x() + apollo::common::math::IntegrateByGaussLegendre(
func_cos_theta, s0, s);
double y = p0.y() + apollo::common::math::IntegrateByGaussLegendre(
func_sin_theta, s0, s);
double theta = geometry_spline.evaluate(0, s);
double kappa = geometry_spline.evaluate(1, s);
double dkappa = geometry_spline.evaluate(2, s);
......@@ -112,8 +114,8 @@ TrajectoryPoint interpolate(const TrajectoryPoint& tp0,
auto func_v = [&dynamic_spline](const double t) {
return dynamic_spline.evaluate(0, t);
};
double s1 = Integration::gauss_legendre(func_v, t0, t1);
double s = Integration::gauss_legendre(func_v, t0, t);
double s1 = apollo::common::math::IntegrateByGaussLegendre(func_v, t0, t1);
double s = apollo::common::math::IntegrateByGaussLegendre(func_v, t0, t);
double v = dynamic_spline.evaluate(0, t);
double a = dynamic_spline.evaluate(1, t);
......@@ -130,8 +132,10 @@ TrajectoryPoint interpolate(const TrajectoryPoint& tp0,
return std::sin(theta);
};
double x = pp0.x() + Integration::gauss_legendre(func_cos_theta, s0, s);
double y = pp0.y() + Integration::gauss_legendre(func_sin_theta, s0, s);
double x = pp0.x() + apollo::common::math::IntegrateByGaussLegendre(
func_cos_theta, s0, s);
double y = pp0.y() + apollo::common::math::IntegrateByGaussLegendre(
func_sin_theta, s0, s);
double theta = geometry_spline.evaluate(0, s);
double kappa = geometry_spline.evaluate(1, s);
double dkappa = geometry_spline.evaluate(2, s);
......
modules/planning/math/BUILD
浏览文件 @ 77c5d112
......@@ -25,19 +25,6 @@ cc_library(
],
)
cc_library(
name = "integration",
srcs = [
"integration.cc",
],
hdrs = [
"integration.h",
],
deps = [
"//modules/common:log",
],
)
cc_library(
name = "polynomial_xd",
srcs = [
......
modules/planning/math/integration.cc 已删除 100644 → 0
浏览文件 @ 86e34f7e
/******************************************************************************
* 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.
*****************************************************************************/
/**
* @file: integration.cc
**/
#include "modules/planning/math/integration.h"
#include <array>
#include "modules/common/log.h"
namespace apollo {
namespace planning {
double Integration::trapezoidal(const std::vector<double>& v, const double dx,
const std::size_t nsteps) {
double sum = 0;
for (std::size_t i = 1; i + 1 < nsteps; ++i) {
sum += v[i];
}
return dx * sum + 0.5 * dx * (v[0] + v[nsteps - 1]);
}
double Integration::simpson(const std::vector<double>& v, const double dx,
const std::size_t nsteps) {
CHECK_EQ(1, nsteps & 1);
double sum1 = 0.0;
double sum2 = 0.0;
for (std::size_t i = 1; i + 1 < nsteps; ++i) {
if ((i & 1) != 0) {
sum1 += v[i];
} else {
sum2 += v[i];
}
}
return dx / 3.0 * (4.0 * sum1 + 2.0 * sum2 + v[0] + v[nsteps - 1]);
}
double Integration::gauss_legendre(const std::function<double(double)>& func,
const double a, const double b) {
double t = (b - a) * 0.5;
double m = (b + a) * 0.5;
std::array<double, 5> w;
w[0] = 0.5688888889;
w[1] = 0.4786286705;
w[2] = 0.4786286705;
w[3] = 0.2369268851;
w[4] = 0.2369268851;
std::array<double, 5> x;
x[0] = 0.0;
x[1] = 0.5384693101;
x[2] = -0.5384693101;
x[3] = 0.9061798459;
x[4] = -0.9061798459;
double integral = 0.0;
for (size_t i = 0; i < 5; ++i) {
integral += w[i] * func(t * x[i] + m);
}
return integral * t;
}
} // namespace planning
} // namespace apollo
modules/planning/math/integration.h 已删除 100644 → 0
浏览文件 @ 86e34f7e
/******************************************************************************
* 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.
*****************************************************************************/
/**
* @file: integration.h
**/
#ifndef MODULES_PLANNING_MATH_INTEGRATION_H_
#define MODULES_PLANNING_MATH_INTEGRATION_H_
#include <functional>
#include <vector>
namespace apollo {
namespace planning {
class Integration {
public:
Integration() = default;
static double simpson(const std::vector<double>& funv_vec, const double dx,
const std::size_t nsteps);
static double trapezoidal(const std::vector<double>& funv_vec,
const double dx, const std::size_t nsteps);
// Given a target function and integral lower and upper bound,
// compute the integral approximation using 5th order Gauss-Legendre
// integration.
// The target function must be a smooth function.
// Example:
// target function: auto func = [](const double& x) {return x * x;}
// double integral = gauss_legendre(func, -2, 3);
// This gives you the approximated integral of function x^2 in bound [-2, 3]
static double gauss_legendre(const std::function<double(double)>& func,
const double lower_bound,
const double upper_bound);
};
} // namespace planning
} // namespace apollo
#endif // MODULES_PLANNING_MATH_INTEGRATION_H_
modules/planning/math/spiral_curve/BUILD
浏览文件 @ 77c5d112
......@@ -43,7 +43,7 @@ cc_library(
"//modules/common:log",
"//modules/common/proto:error_code_proto",
"//modules/common/proto:path_point_proto",
"//modules/planning/math:integration",
"//modules/common/math:math",
"@eigen//:eigen",
],
)
......@@ -61,7 +61,7 @@ cc_library(
":spiral_formula",
"//modules/common:log",
"//modules/common/proto:path_point_proto",
"//modules/planning/math:integration",
"//modules/common/math:math",
"@eigen//:eigen",
],
)
......
modules/planning/math/spiral_curve/cubic_spiral_curve.cc
浏览文件 @ 77c5d112
......@@ -24,7 +24,7 @@
#include "Eigen/LU"
#include "modules/common/log.h"
#include "modules/planning/math/integration.h"
#include "modules/common/math/integral.h"
#include "modules/planning/math/spiral_curve/spiral_formula.h"
namespace apollo {
......@@ -129,30 +129,30 @@ bool CubicSpiralCurve::calculate_path() {
}
// update Jacobian and delta q
jacobi(0, 0) =
-Integration::simpson(sin_ptp_p1, ds, spiral_config().simpson_size());
jacobi(0, 1) =
-Integration::simpson(sin_ptp_p2, ds, spiral_config().simpson_size());
jacobi(0, 2) =
cos_theta[spiral_config().simpson_size() - 1] -
Integration::simpson(sin_ptp_sg, ds, spiral_config().simpson_size());
jacobi(1, 0) =
Integration::simpson(cos_ptp_p1, ds, spiral_config().simpson_size());
jacobi(1, 1) =
Integration::simpson(cos_ptp_p2, ds, spiral_config().simpson_size());
jacobi(1, 2) =
sin_theta[spiral_config().simpson_size() - 1] +
Integration::simpson(cos_ptp_sg, ds, spiral_config().simpson_size());
jacobi(0, 0) = -apollo::common::math::IntegrateBySimpson(
sin_ptp_p1, ds, spiral_config().simpson_size());
jacobi(0, 1) = -apollo::common::math::IntegrateBySimpson(
sin_ptp_p2, ds, spiral_config().simpson_size());
jacobi(0, 2) = cos_theta[spiral_config().simpson_size() - 1] -
apollo::common::math::IntegrateBySimpson(
sin_ptp_sg, ds, spiral_config().simpson_size());
jacobi(1, 0) = apollo::common::math::IntegrateBySimpson(
cos_ptp_p1, ds, spiral_config().simpson_size());
jacobi(1, 1) = apollo::common::math::IntegrateBySimpson(
cos_ptp_p2, ds, spiral_config().simpson_size());
jacobi(1, 2) = sin_theta[spiral_config().simpson_size() - 1] +
apollo::common::math::IntegrateBySimpson(
cos_ptp_sg, ds, spiral_config().simpson_size());
jacobi(2, 0) = ptp_p1[spiral_config().simpson_size() - 1];
jacobi(2, 1) = ptp_p2[spiral_config().simpson_size() - 1];
jacobi(2, 2) = ptp_sg[spiral_config().simpson_size() - 1];
q_guess(0) =
Integration::simpson(cos_theta, ds, spiral_config().simpson_size());
q_guess(1) =
Integration::simpson(sin_theta, ds, spiral_config().simpson_size());
q_guess(0) = apollo::common::math::IntegrateBySimpson(
cos_theta, ds, spiral_config().simpson_size());
q_guess(1) = apollo::common::math::IntegrateBySimpson(
sin_theta, ds, spiral_config().simpson_size());
q_guess(2) = theta[spiral_config().simpson_size() - 1];
delta_q = q_g - q_guess;
......
modules/planning/math/spiral_curve/quintic_spiral_curve.cc
浏览文件 @ 77c5d112
......@@ -24,7 +24,7 @@
#include "Eigen/LU"
#include "modules/common/log.h"
#include "modules/planning/math/integration.h"
#include "modules/common/math/integral.h"
#include "modules/planning/math/spiral_curve/spiral_formula.h"
namespace apollo {
......@@ -130,25 +130,25 @@ bool QuinticSpiralCurve::calculate_path() {
}
// update Jacobian and delta q
jacobi(0, 0) =
-Integration::simpson(sin_ptp_p3, ds, spiral_config().simpson_size());
jacobi(0, 0) = -apollo::common::math::IntegrateBySimpson(
sin_ptp_p3, ds, spiral_config().simpson_size());
jacobi(0, 1) =
-Integration::simpson(sin_ptp_p4, ds, spiral_config().simpson_size());
jacobi(0, 1) = -apollo::common::math::IntegrateBySimpson(
sin_ptp_p4, ds, spiral_config().simpson_size());
jacobi(0, 2) =
cos_theta[spiral_config().simpson_size() - 1] -
Integration::simpson(sin_ptp_sg, ds, spiral_config().simpson_size());
jacobi(0, 2) = cos_theta[spiral_config().simpson_size() - 1] -
apollo::common::math::IntegrateBySimpson(
sin_ptp_sg, ds, spiral_config().simpson_size());
jacobi(1, 0) =
Integration::simpson(cos_ptp_p3, ds, spiral_config().simpson_size());
jacobi(1, 0) = apollo::common::math::IntegrateBySimpson(
cos_ptp_p3, ds, spiral_config().simpson_size());
jacobi(1, 1) =
Integration::simpson(cos_ptp_p4, ds, spiral_config().simpson_size());
jacobi(1, 1) = apollo::common::math::IntegrateBySimpson(
cos_ptp_p4, ds, spiral_config().simpson_size());
jacobi(1, 2) =
sin_theta[spiral_config().simpson_size() - 1] +
Integration::simpson(cos_ptp_sg, ds, spiral_config().simpson_size());
jacobi(1, 2) = sin_theta[spiral_config().simpson_size() - 1] +
apollo::common::math::IntegrateBySimpson(
cos_ptp_sg, ds, spiral_config().simpson_size());
jacobi(2, 0) = ptp_p3[spiral_config().simpson_size() - 1];
......@@ -156,11 +156,11 @@ bool QuinticSpiralCurve::calculate_path() {
jacobi(2, 2) = ptp_sg[spiral_config().simpson_size() - 1];
q_guess(0) =
Integration::simpson(cos_theta, ds, spiral_config().simpson_size());
q_guess(0) = apollo::common::math::IntegrateBySimpson(
cos_theta, ds, spiral_config().simpson_size());
q_guess(1) =
Integration::simpson(sin_theta, ds, spiral_config().simpson_size());
q_guess(1) = apollo::common::math::IntegrateBySimpson(
sin_theta, ds, spiral_config().simpson_size());
q_guess(2) = theta[spiral_config().simpson_size() - 1];
......
Markdown is supported
0% 或 .
You are about to add 0 people to the discussion. Proceed with caution.
先完成此消息的编辑！
想要评论请 注册 或