Skip to content
体验新版
项目
组织
正在加载...
登录
切换导航
打开侧边栏
Pinoxchio
apollo
提交
5e36ebe7
A
apollo
项目概览
Pinoxchio
/
apollo
与 Fork 源项目一致
从无法访问的项目Fork
通知
2
Star
0
Fork
0
代码
文件
提交
分支
Tags
贡献者
分支图
Diff
Issue
0
列表
看板
标记
里程碑
合并请求
0
Wiki
0
Wiki
分析
仓库
DevOps
项目成员
Pages
A
apollo
项目概览
项目概览
详情
发布
仓库
仓库
文件
提交
分支
标签
贡献者
分支图
比较
Issue
0
Issue
0
列表
看板
标记
里程碑
合并请求
0
合并请求
0
Pages
分析
分析
仓库分析
DevOps
Wiki
0
Wiki
成员
成员
收起侧边栏
关闭侧边栏
动态
分支图
创建新Issue
提交
Issue看板
体验新版 GitCode,发现更多精彩内容 >>
提交
5e36ebe7
编写于
8月 08, 2017
作者:
Z
Zhang Liangliang
提交者:
Calvin Miao
8月 09, 2017
浏览文件
操作
浏览文件
下载
电子邮件补丁
差异文件
Planning: cleaned up function names in planning/math
上级
60075c0a
变更
23
显示空白变更内容
内联
并排
Showing
23 changed file
with
92 addition
and
99 deletion
+92
-99
modules/common/math/mpc_solver.cc
modules/common/math/mpc_solver.cc
+25
-32
modules/common/math/qp_solver/active_set_qp_solver.cc
modules/common/math/qp_solver/active_set_qp_solver.cc
+1
-1
modules/common/math/qp_solver/active_set_qp_solver.h
modules/common/math/qp_solver/active_set_qp_solver.h
+1
-1
modules/common/math/qp_solver/qp_solver.h
modules/common/math/qp_solver/qp_solver.h
+1
-1
modules/planning/common/path/frenet_frame_path.h
modules/planning/common/path/frenet_frame_path.h
+1
-1
modules/planning/math/smoothing_spline/spline_1d_generator.cc
...les/planning/math/smoothing_spline/spline_1d_generator.cc
+6
-6
modules/planning/math/smoothing_spline/spline_1d_generator.h
modules/planning/math/smoothing_spline/spline_1d_generator.h
+3
-3
modules/planning/math/smoothing_spline/spline_1d_kernel.cc
modules/planning/math/smoothing_spline/spline_1d_kernel.cc
+4
-4
modules/planning/math/smoothing_spline/spline_1d_kernel.h
modules/planning/math/smoothing_spline/spline_1d_kernel.h
+3
-3
modules/planning/math/smoothing_spline/spline_2d.cc
modules/planning/math/smoothing_spline/spline_2d.cc
+6
-6
modules/planning/math/smoothing_spline/spline_2d.h
modules/planning/math/smoothing_spline/spline_2d.h
+3
-3
modules/planning/math/smoothing_spline/spline_2d_constraint.cc
...es/planning/math/smoothing_spline/spline_2d_constraint.cc
+1
-1
modules/planning/math/smoothing_spline/spline_2d_constraint.h
...les/planning/math/smoothing_spline/spline_2d_constraint.h
+1
-1
modules/planning/math/smoothing_spline/spline_2d_kernel.cc
modules/planning/math/smoothing_spline/spline_2d_kernel.cc
+4
-4
modules/planning/math/smoothing_spline/spline_2d_kernel.h
modules/planning/math/smoothing_spline/spline_2d_kernel.h
+3
-3
modules/planning/math/smoothing_spline/spline_2d_seg.cc
modules/planning/math/smoothing_spline/spline_2d_seg.cc
+6
-6
modules/planning/math/smoothing_spline/spline_2d_seg.h
modules/planning/math/smoothing_spline/spline_2d_seg.h
+6
-6
modules/planning/math/smoothing_spline/spline_2d_solver.cc
modules/planning/math/smoothing_spline/spline_2d_solver.cc
+2
-2
modules/planning/math/smoothing_spline/spline_2d_solver.h
modules/planning/math/smoothing_spline/spline_2d_solver.h
+1
-1
modules/planning/optimizer/qp_spline_path/qp_spline_path_generator.cc
...ning/optimizer/qp_spline_path/qp_spline_path_generator.cc
+2
-2
modules/planning/optimizer/qp_spline_st_speed/qp_spline_st_graph.cc
...anning/optimizer/qp_spline_st_speed/qp_spline_st_graph.cc
+1
-1
modules/planning/reference_line/reference_line_smoother.cc
modules/planning/reference_line/reference_line_smoother.cc
+10
-10
modules/planning/reference_line/reference_line_smoother.h
modules/planning/reference_line/reference_line_smoother.h
+1
-1
未找到文件。
modules/common/math/mpc_solver.cc
浏览文件 @
5e36ebe7
...
...
@@ -29,18 +29,13 @@ using ::apollo::common::math::ActiveSetQpSolver;
// discrete linear predictive control solver, with control format
// x(i + 1) = A * x(i) + B * u (i) + C
void
SolveLinearMPC
(
const
Matrix
&
matrix_a
,
const
Matrix
&
matrix_b
,
const
Matrix
&
matrix_c
,
const
Matrix
&
matrix_q
,
const
Matrix
&
matrix_r
,
const
Matrix
&
matrix_lower
,
void
SolveLinearMPC
(
const
Matrix
&
matrix_a
,
const
Matrix
&
matrix_b
,
const
Matrix
&
matrix_c
,
const
Matrix
&
matrix_q
,
const
Matrix
&
matrix_r
,
const
Matrix
&
matrix_lower
,
const
Matrix
&
matrix_upper
,
const
Matrix
&
matrix_initial_state
,
const
std
::
vector
<
Matrix
>
&
reference
,
const
double
eps
,
const
int
max_iter
,
std
::
vector
<
Matrix
>
*
control
)
{
const
std
::
vector
<
Matrix
>
&
reference
,
const
double
eps
,
const
int
max_iter
,
std
::
vector
<
Matrix
>
*
control
)
{
if
(
matrix_a
.
rows
()
!=
matrix_a
.
cols
()
||
matrix_b
.
rows
()
!=
matrix_a
.
rows
()
||
matrix_lower
.
rows
()
!=
matrix_upper
.
rows
())
{
...
...
@@ -67,7 +62,7 @@ void SolveLinearMPC(const Matrix &matrix_a,
std
::
vector
<
Matrix
>
matrix_a_power
(
horizon
);
matrix_a_power
[
0
]
=
matrix_a
;
for
(
unsigned
int
i
=
1
;
i
<
matrix_a_power
.
size
();
++
i
)
{
matrix_a_power
[
i
]
=
matrix_a
*
matrix_a_power
[
i
-
1
];
matrix_a_power
[
i
]
=
matrix_a
*
matrix_a_power
[
i
-
1
];
}
Matrix
matrix_k
=
Matrix
::
Zero
(
matrix_b
.
rows
()
*
horizon
,
...
...
@@ -75,7 +70,7 @@ void SolveLinearMPC(const Matrix &matrix_a,
for
(
unsigned
int
r
=
0
;
r
<
horizon
;
++
r
)
{
for
(
unsigned
int
c
=
0
;
c
<=
r
;
++
c
)
{
matrix_k
.
block
(
r
*
matrix_b
.
rows
(),
c
*
matrix_b
.
cols
(),
matrix_b
.
rows
(),
matrix_b
.
cols
())
=
matrix_a_power
[
r
-
c
]
*
matrix_b
;
matrix_b
.
cols
())
=
matrix_a_power
[
r
-
c
]
*
matrix_b
;
}
}
...
...
@@ -93,7 +88,7 @@ void SolveLinearMPC(const Matrix &matrix_a,
for
(
unsigned
int
i
=
1
;
i
<
horizon
;
++
i
)
{
matrix_m
.
block
(
i
*
matrix_a
.
rows
(),
0
,
matrix_a
.
rows
(),
1
)
=
matrix_a
*
matrix_m
.
block
((
i
-
1
)
*
matrix_a
.
rows
(),
0
,
matrix_a
.
rows
(),
1
)
+
matrix_m
.
block
((
i
-
1
)
*
matrix_a
.
rows
(),
0
,
matrix_a
.
rows
(),
1
)
+
matrix_c
;
}
...
...
@@ -103,10 +98,10 @@ void SolveLinearMPC(const Matrix &matrix_a,
matrix_lower
;
matrix_uu
.
block
(
i
*
(
*
control
)[
0
].
rows
(),
0
,
(
*
control
)[
0
].
rows
(),
1
)
=
matrix_upper
;
matrix_qq
.
block
(
i
*
matrix_q
.
rows
(),
i
*
matrix_q
.
rows
(),
matrix_q
.
rows
()
,
matrix_q
.
rows
()
)
=
matrix_q
;
matrix_rr
.
block
(
i
*
matrix_r
.
rows
(),
i
*
matrix_r
.
rows
(),
matrix_r
.
rows
()
,
matrix_r
.
rows
()
)
=
matrix_r
;
matrix_qq
.
block
(
i
*
matrix_q
.
rows
(),
i
*
matrix_q
.
rows
(),
matrix_q
.
rows
(),
matrix_q
.
rows
())
=
matrix_q
;
matrix_rr
.
block
(
i
*
matrix_r
.
rows
(),
i
*
matrix_r
.
rows
(),
matrix_r
.
rows
(),
matrix_r
.
rows
())
=
matrix_r
;
}
// Update matrix_m1, matrix_m2, convert MPC problem to QP problem done
...
...
@@ -114,29 +109,27 @@ void SolveLinearMPC(const Matrix &matrix_a,
Matrix
matrix_m2
=
matrix_k
.
transpose
()
*
matrix_qq
*
(
matrix_m
-
matrix_t
);
// Method 1: QPOASES
Eigen
::
MatrixXd
matrix_inequality_constrain_ll
=
-
Eigen
::
MatrixXd
::
Identity
(
matrix_ll
.
rows
(),
matrix_ll
.
rows
());
Eigen
::
MatrixXd
matrix_inequality_constrain_uu
=
Eigen
::
MatrixXd
::
Identity
(
matrix_uu
.
rows
(),
matrix_uu
.
rows
());
Eigen
::
MatrixXd
matrix_inequality_constrain_ll
=
-
Eigen
::
MatrixXd
::
Identity
(
matrix_ll
.
rows
(),
matrix_ll
.
rows
());
Eigen
::
MatrixXd
matrix_inequality_constrain_uu
=
Eigen
::
MatrixXd
::
Identity
(
matrix_uu
.
rows
(),
matrix_uu
.
rows
());
Eigen
::
MatrixXd
matrix_inequality_constrain
=
Eigen
::
MatrixXd
::
Zero
(
matrix_ll
.
rows
()
+
matrix_uu
.
rows
(),
matrix_ll
.
rows
());
matrix_inequality_constrain
<<
-
matrix_inequality_constrain_ll
,
-
matrix_inequality_constrain_uu
;
matrix_inequality_constrain
<<
-
matrix_inequality_constrain_ll
,
-
matrix_inequality_constrain_uu
;
Eigen
::
MatrixXd
matrix_inequality_boundary
=
Eigen
::
MatrixXd
::
Zero
(
matrix_ll
.
rows
()
+
matrix_uu
.
rows
(),
matrix_ll
.
cols
());
matrix_inequality_boundary
<<
matrix_ll
,
-
matrix_uu
;
matrix_inequality_boundary
<<
matrix_ll
,
-
matrix_uu
;
Eigen
::
MatrixXd
matrix_equality_constrain
=
Eigen
::
MatrixXd
::
Zero
(
matrix_ll
.
rows
()
+
matrix_uu
.
rows
(),
matrix_ll
.
rows
());
Eigen
::
MatrixXd
matrix_equality_boundary
=
Eigen
::
MatrixXd
::
Zero
(
matrix_ll
.
rows
()
+
matrix_uu
.
rows
(),
matrix_ll
.
cols
());
std
::
unique_ptr
<
QpSolver
>
qp_solver
(
new
ActiveSetQpSolver
(
matrix_m1
,
matrix_m2
,
matrix_inequality_constrain
,
matrix_inequality_boundary
,
matrix_equality_constrain
,
std
::
unique_ptr
<
QpSolver
>
qp_solver
(
new
ActiveSetQpSolver
(
matrix_m1
,
matrix_m2
,
matrix_inequality_constrain
,
matrix_inequality_boundary
,
matrix_equality_constrain
,
matrix_equality_boundary
));
qp_solver
->
s
olve
();
qp_solver
->
S
olve
();
matrix_v
=
qp_solver
->
params
();
for
(
unsigned
int
i
=
0
;
i
<
horizon
;
++
i
)
{
...
...
modules/common/math/qp_solver/active_set_qp_solver.cc
浏览文件 @
5e36ebe7
...
...
@@ -48,7 +48,7 @@ ActiveSetQpSolver::ActiveSetQpSolver(
debug_info_
=
FLAGS_default_enable_active_set_debug_info
;
}
bool
ActiveSetQpSolver
::
s
olve
()
{
bool
ActiveSetQpSolver
::
S
olve
()
{
::
qpOASES
::
QProblem
qp_problem
(
num_param_
,
num_constraint_
);
::
qpOASES
::
Options
my_options
;
my_options
.
epsNum
=
qp_eps_num_
;
...
...
modules/common/math/qp_solver/active_set_qp_solver.h
浏览文件 @
5e36ebe7
...
...
@@ -38,7 +38,7 @@ class ActiveSetQpSolver : public QpSolver {
const
Eigen
::
MatrixXd
&
affine_equality_matrix
,
const
Eigen
::
MatrixXd
&
affine_equality_boundary
);
bool
s
olve
()
override
;
bool
S
olve
()
override
;
void
set_qp_eps_num
(
const
double
eps
);
void
set_qp_eps_den
(
const
double
eps
);
...
...
modules/common/math/qp_solver/qp_solver.h
浏览文件 @
5e36ebe7
...
...
@@ -41,7 +41,7 @@ class QpSolver {
const
Eigen
::
MatrixXd
&
affine_equality_matrix
,
const
Eigen
::
MatrixXd
&
affine_equality_boundary
);
virtual
bool
s
olve
()
=
0
;
virtual
bool
S
olve
()
=
0
;
const
Eigen
::
MatrixXd
&
params
()
const
;
const
Eigen
::
MatrixXd
&
kernel_matrix
()
const
;
const
Eigen
::
MatrixXd
&
offset
()
const
;
...
...
modules/planning/common/path/frenet_frame_path.h
浏览文件 @
5e36ebe7
...
...
@@ -40,10 +40,10 @@ class FrenetFramePath {
double
Length
()
const
;
const
common
::
FrenetFramePoint
&
PointAt
(
const
std
::
uint32_t
index
)
const
;
common
::
FrenetFramePoint
&
PointAt
(
const
std
::
uint32_t
index
);
common
::
FrenetFramePoint
EvaluateByS
(
const
double
s
)
const
;
virtual
void
Clear
();
private:
std
::
vector
<
common
::
FrenetFramePoint
>
points_
;
};
...
...
modules/planning/math/smoothing_spline/spline_1d_generator.cc
浏览文件 @
5e36ebe7
...
...
@@ -42,7 +42,7 @@ Spline1dKernel* Spline1dGenerator::mutable_spline_kernel() {
return
&
spline_kernel_
;
}
void
Spline1dGenerator
::
setup_init_qp_p
oint
(
const
Eigen
::
MatrixXd
&
x
,
void
Spline1dGenerator
::
SetupInitQpP
oint
(
const
Eigen
::
MatrixXd
&
x
,
const
Eigen
::
MatrixXd
&
y
,
const
Eigen
::
MatrixXd
&
z
,
const
Eigen
::
MatrixXd
&
s
)
{
...
...
@@ -52,7 +52,7 @@ void Spline1dGenerator::setup_init_qp_point(const Eigen::MatrixXd& x,
init_s_
=
s
;
}
bool
Spline1dGenerator
::
s
olve
()
{
bool
Spline1dGenerator
::
S
olve
()
{
const
Eigen
::
MatrixXd
&
kernel_matrix
=
spline_kernel_
.
kernel_matrix
();
const
Eigen
::
MatrixXd
&
offset
=
spline_kernel_
.
offset
();
const
Eigen
::
MatrixXd
&
inequality_constraint_matrix
=
...
...
@@ -69,7 +69,7 @@ bool Spline1dGenerator::solve() {
inequality_constraint_boundary
,
equality_constraint_matrix
,
equality_constraint_boundary
));
if
(
!
qp_solver_
->
s
olve
())
{
if
(
!
qp_solver_
->
S
olve
())
{
return
false
;
}
...
...
modules/planning/math/smoothing_spline/spline_1d_generator.h
浏览文件 @
5e36ebe7
...
...
@@ -74,11 +74,11 @@ class Spline1dGenerator {
// setup initialize point, if not,
// will use the default primal-dual parameter
void
setup_init_qp_p
oint
(
const
Eigen
::
MatrixXd
&
x
,
const
Eigen
::
MatrixXd
&
y
,
void
SetupInitQpP
oint
(
const
Eigen
::
MatrixXd
&
x
,
const
Eigen
::
MatrixXd
&
y
,
const
Eigen
::
MatrixXd
&
z
,
const
Eigen
::
MatrixXd
&
s
);
// solve
bool
s
olve
();
bool
S
olve
();
// output
const
Spline1d
&
spline
()
const
;
...
...
modules/planning/math/smoothing_spline/spline_1d_kernel.cc
浏览文件 @
5e36ebe7
...
...
@@ -46,13 +46,13 @@ Spline1dKernel::Spline1dKernel(const std::vector<double>& x_knots,
offset_
=
Eigen
::
MatrixXd
::
Zero
(
total_params_
,
1
);
}
void
Spline1dKernel
::
add_r
egularization
(
const
double
regularized_param
)
{
void
Spline1dKernel
::
AddR
egularization
(
const
double
regularized_param
)
{
Eigen
::
MatrixXd
id_matrix
=
Eigen
::
MatrixXd
::
Identity
(
kernel_matrix_
.
rows
(),
kernel_matrix_
.
cols
());
kernel_matrix_
+=
id_matrix
*
regularized_param
;
}
bool
Spline1dKernel
::
add_k
ernel
(
const
Eigen
::
MatrixXd
&
kernel
,
bool
Spline1dKernel
::
AddK
ernel
(
const
Eigen
::
MatrixXd
&
kernel
,
const
Eigen
::
MatrixXd
&
offset
,
const
double
weight
)
{
if
(
kernel
.
rows
()
!=
kernel
.
cols
()
||
...
...
@@ -65,10 +65,10 @@ bool Spline1dKernel::add_kernel(const Eigen::MatrixXd& kernel,
return
true
;
}
bool
Spline1dKernel
::
add_k
ernel
(
const
Eigen
::
MatrixXd
&
kernel
,
bool
Spline1dKernel
::
AddK
ernel
(
const
Eigen
::
MatrixXd
&
kernel
,
const
double
weight
)
{
Eigen
::
MatrixXd
offset
=
Eigen
::
MatrixXd
::
Zero
(
kernel
.
rows
(),
1
);
return
add_k
ernel
(
kernel
,
offset
,
weight
);
return
AddK
ernel
(
kernel
,
offset
,
weight
);
}
Eigen
::
MatrixXd
*
Spline1dKernel
::
mutable_kernel_matrix
()
{
...
...
modules/planning/math/smoothing_spline/spline_1d_kernel.h
浏览文件 @
5e36ebe7
...
...
@@ -38,10 +38,10 @@ class Spline1dKernel {
const
std
::
uint32_t
spline_order
);
// customized input / output method
void
add_r
egularization
(
const
double
regularized_param
);
bool
add_k
ernel
(
const
Eigen
::
MatrixXd
&
kernel
,
const
Eigen
::
MatrixXd
&
offset
,
void
AddR
egularization
(
const
double
regularized_param
);
bool
AddK
ernel
(
const
Eigen
::
MatrixXd
&
kernel
,
const
Eigen
::
MatrixXd
&
offset
,
const
double
weight
);
bool
add_k
ernel
(
const
Eigen
::
MatrixXd
&
kernel
,
const
double
weight
);
bool
AddK
ernel
(
const
Eigen
::
MatrixXd
&
kernel
,
const
double
weight
);
Eigen
::
MatrixXd
*
mutable_kernel_matrix
();
Eigen
::
MatrixXd
*
mutable_offset
();
...
...
modules/planning/math/smoothing_spline/spline_2d.cc
浏览文件 @
5e36ebe7
...
...
@@ -60,13 +60,13 @@ double Spline2d::y(const double t) const {
return
splines_
[
index
].
y
(
t
-
t_knots_
[
index
]);
}
double
Spline2d
::
derivative_x
(
const
double
t
)
const
{
double
Spline2d
::
DerivativeX
(
const
double
t
)
const
{
// zero order spline
if
(
splines_
.
size
()
==
0
)
{
return
0.0
;
}
std
::
uint32_t
index
=
find_index
(
t
);
return
splines_
[
index
].
derivative_x
(
t
-
t_knots_
[
index
]);
return
splines_
[
index
].
DerivativeX
(
t
-
t_knots_
[
index
]);
}
double
Spline2d
::
derivative_y
(
const
double
t
)
const
{
...
...
@@ -78,12 +78,12 @@ double Spline2d::derivative_y(const double t) const {
return
splines_
[
index
].
derivative_y
(
t
-
t_knots_
[
index
]);
}
double
Spline2d
::
second_derivative_x
(
const
double
t
)
const
{
double
Spline2d
::
SecondDerivativeX
(
const
double
t
)
const
{
if
(
splines_
.
size
()
==
0
)
{
return
0.0
;
}
std
::
uint32_t
index
=
find_index
(
t
);
return
splines_
[
index
].
second_derivative_x
(
t
-
t_knots_
[
index
]);
return
splines_
[
index
].
SecondDerivativeX
(
t
-
t_knots_
[
index
]);
}
double
Spline2d
::
second_derivative_y
(
const
double
t
)
const
{
...
...
@@ -94,12 +94,12 @@ double Spline2d::second_derivative_y(const double t) const {
return
splines_
[
index
].
second_derivative_y
(
t
-
t_knots_
[
index
]);
}
double
Spline2d
::
third_derivative_x
(
const
double
t
)
const
{
double
Spline2d
::
ThirdDerivativeX
(
const
double
t
)
const
{
if
(
splines_
.
size
()
==
0
)
{
return
0.0
;
}
std
::
uint32_t
index
=
find_index
(
t
);
return
splines_
[
index
].
third_derivative_x
(
t
-
t_knots_
[
index
]);
return
splines_
[
index
].
ThirdDerivativeX
(
t
-
t_knots_
[
index
]);
}
double
Spline2d
::
third_derivative_y
(
const
double
t
)
const
{
...
...
modules/planning/math/smoothing_spline/spline_2d.h
浏览文件 @
5e36ebe7
...
...
@@ -39,11 +39,11 @@ class Spline2d {
std
::
pair
<
double
,
double
>
operator
()(
const
double
t
)
const
;
double
x
(
const
double
t
)
const
;
double
y
(
const
double
t
)
const
;
double
derivative_x
(
const
double
t
)
const
;
double
DerivativeX
(
const
double
t
)
const
;
double
derivative_y
(
const
double
t
)
const
;
double
second_derivative_x
(
const
double
t
)
const
;
double
SecondDerivativeX
(
const
double
t
)
const
;
double
second_derivative_y
(
const
double
t
)
const
;
double
third_derivative_x
(
const
double
t
)
const
;
double
ThirdDerivativeX
(
const
double
t
)
const
;
double
third_derivative_y
(
const
double
t
)
const
;
bool
set_splines
(
const
Eigen
::
MatrixXd
&
params
,
const
std
::
uint32_t
order
);
Spline2dSeg
*
mutable_smoothing_spline
(
const
std
::
uint32_t
index
);
...
...
modules/planning/math/smoothing_spline/spline_2d_constraint.cc
浏览文件 @
5e36ebe7
...
...
@@ -56,7 +56,7 @@ bool Spline2dConstraint::AddEqualityConstraint(
* @brief: inequality boundary constraints
* if no boundary, do specify either by std::infinity or let vector.size() = 0
**/
bool
Spline2dConstraint
::
add_2d_b
oundary
(
bool
Spline2dConstraint
::
Add2dB
oundary
(
const
std
::
vector
<
double
>&
t_coord
,
const
std
::
vector
<
double
>&
angle
,
const
std
::
vector
<
Vec2d
>&
ref_point
,
const
std
::
vector
<
double
>&
longitidinal_bound
,
...
...
modules/planning/math/smoothing_spline/spline_2d_constraint.h
浏览文件 @
5e36ebe7
...
...
@@ -48,7 +48,7 @@ class Spline2dConstraint {
* if no boundary, do specify either by std::infinity or let vector.size() =
*0
**/
bool
add_2d_b
oundary
(
bool
Add2dB
oundary
(
const
std
::
vector
<
double
>&
t_coord
,
const
std
::
vector
<
double
>&
angle
,
const
std
::
vector
<
apollo
::
common
::
math
::
Vec2d
>&
ref_point
,
const
std
::
vector
<
double
>&
longitidinal_bound
,
...
...
modules/planning/math/smoothing_spline/spline_2d_kernel.cc
浏览文件 @
5e36ebe7
...
...
@@ -39,13 +39,13 @@ Spline2dKernel::Spline2dKernel(const std::vector<double>& t_knots,
}
// customized input output
void
Spline2dKernel
::
add_r
egularization
(
const
double
regularization_param
)
{
void
Spline2dKernel
::
AddR
egularization
(
const
double
regularization_param
)
{
Eigen
::
MatrixXd
id_matrix
=
Eigen
::
MatrixXd
::
Identity
(
kernel_matrix_
.
rows
(),
kernel_matrix_
.
cols
());
kernel_matrix_
+=
id_matrix
*
regularization_param
;
}
bool
Spline2dKernel
::
add_k
ernel
(
const
Eigen
::
MatrixXd
&
kernel
,
bool
Spline2dKernel
::
AddK
ernel
(
const
Eigen
::
MatrixXd
&
kernel
,
const
Eigen
::
MatrixXd
&
offset
,
const
double
weight
)
{
if
(
kernel
.
rows
()
!=
kernel
.
cols
()
||
...
...
@@ -58,10 +58,10 @@ bool Spline2dKernel::add_kernel(const Eigen::MatrixXd& kernel,
return
true
;
}
bool
Spline2dKernel
::
add_k
ernel
(
const
Eigen
::
MatrixXd
&
kernel
,
bool
Spline2dKernel
::
AddK
ernel
(
const
Eigen
::
MatrixXd
&
kernel
,
const
double
weight
)
{
Eigen
::
MatrixXd
offset
=
Eigen
::
MatrixXd
::
Zero
(
kernel
.
rows
(),
1
);
return
add_k
ernel
(
kernel
,
offset
,
weight
);
return
AddK
ernel
(
kernel
,
offset
,
weight
);
}
Eigen
::
MatrixXd
*
Spline2dKernel
::
mutable_kernel_matrix
()
{
...
...
modules/planning/math/smoothing_spline/spline_2d_kernel.h
浏览文件 @
5e36ebe7
...
...
@@ -36,10 +36,10 @@ class Spline2dKernel {
const
std
::
uint32_t
spline_order
);
// customized input output
void
add_r
egularization
(
const
double
regularization_param
);
bool
add_k
ernel
(
const
Eigen
::
MatrixXd
&
kernel
,
const
Eigen
::
MatrixXd
&
offset
,
void
AddR
egularization
(
const
double
regularization_param
);
bool
AddK
ernel
(
const
Eigen
::
MatrixXd
&
kernel
,
const
Eigen
::
MatrixXd
&
offset
,
const
double
weight
);
bool
add_k
ernel
(
const
Eigen
::
MatrixXd
&
kernel
,
const
double
weight
);
bool
AddK
ernel
(
const
Eigen
::
MatrixXd
&
kernel
,
const
double
weight
);
Eigen
::
MatrixXd
*
mutable_kernel_matrix
();
Eigen
::
MatrixXd
*
mutable_offset
();
...
...
modules/planning/math/smoothing_spline/spline_2d_seg.cc
浏览文件 @
5e36ebe7
...
...
@@ -70,7 +70,7 @@ double Spline2dSeg::x(const double t) const { return spline_func_x_(t); }
double
Spline2dSeg
::
y
(
const
double
t
)
const
{
return
spline_func_y_
(
t
);
}
double
Spline2dSeg
::
derivative_x
(
const
double
t
)
const
{
double
Spline2dSeg
::
DerivativeX
(
const
double
t
)
const
{
return
derivative_x_
(
t
);
}
...
...
@@ -78,7 +78,7 @@ double Spline2dSeg::derivative_y(const double t) const {
return
derivative_y_
(
t
);
}
double
Spline2dSeg
::
second_derivative_x
(
const
double
t
)
const
{
double
Spline2dSeg
::
SecondDerivativeX
(
const
double
t
)
const
{
return
second_derivative_x_
(
t
);
}
...
...
@@ -86,7 +86,7 @@ double Spline2dSeg::second_derivative_y(const double t) const {
return
second_derivative_y_
(
t
);
}
double
Spline2dSeg
::
third_derivative_x
(
const
double
t
)
const
{
double
Spline2dSeg
::
ThirdDerivativeX
(
const
double
t
)
const
{
return
third_derivative_x_
(
t
);
}
...
...
@@ -102,11 +102,11 @@ const PolynomialXd& Spline2dSeg::spline_func_y() const {
return
spline_func_y_
;
}
const
PolynomialXd
&
Spline2dSeg
::
derivative_x
()
const
{
return
derivative_x_
;
}
const
PolynomialXd
&
Spline2dSeg
::
DerivativeX
()
const
{
return
derivative_x_
;
}
const
PolynomialXd
&
Spline2dSeg
::
derivative_y
()
const
{
return
derivative_y_
;
}
const
PolynomialXd
&
Spline2dSeg
::
second_derivative_x
()
const
{
const
PolynomialXd
&
Spline2dSeg
::
SecondDerivativeX
()
const
{
return
second_derivative_x_
;
}
...
...
@@ -114,7 +114,7 @@ const PolynomialXd& Spline2dSeg::second_derivative_y() const {
return
second_derivative_y_
;
}
const
PolynomialXd
&
Spline2dSeg
::
third_derivative_x
()
const
{
const
PolynomialXd
&
Spline2dSeg
::
ThirdDerivativeX
()
const
{
return
third_derivative_x_
;
}
...
...
modules/planning/math/smoothing_spline/spline_2d_seg.h
浏览文件 @
5e36ebe7
...
...
@@ -43,20 +43,20 @@ class Spline2dSeg {
std
::
pair
<
double
,
double
>
operator
()(
const
double
t
)
const
;
double
x
(
const
double
t
)
const
;
double
y
(
const
double
t
)
const
;
double
derivative_x
(
const
double
t
)
const
;
double
DerivativeX
(
const
double
t
)
const
;
double
derivative_y
(
const
double
t
)
const
;
double
second_derivative_x
(
const
double
t
)
const
;
double
SecondDerivativeX
(
const
double
t
)
const
;
double
second_derivative_y
(
const
double
t
)
const
;
double
third_derivative_x
(
const
double
t
)
const
;
double
ThirdDerivativeX
(
const
double
t
)
const
;
double
third_derivative_y
(
const
double
t
)
const
;
const
PolynomialXd
&
spline_func_x
()
const
;
const
PolynomialXd
&
spline_func_y
()
const
;
const
PolynomialXd
&
derivative_x
()
const
;
const
PolynomialXd
&
DerivativeX
()
const
;
const
PolynomialXd
&
derivative_y
()
const
;
const
PolynomialXd
&
second_derivative_x
()
const
;
const
PolynomialXd
&
SecondDerivativeX
()
const
;
const
PolynomialXd
&
second_derivative_y
()
const
;
const
PolynomialXd
&
third_derivative_x
()
const
;
const
PolynomialXd
&
ThirdDerivativeX
()
const
;
const
PolynomialXd
&
third_derivative_y
()
const
;
private:
...
...
modules/planning/math/smoothing_spline/spline_2d_solver.cc
浏览文件 @
5e36ebe7
...
...
@@ -40,7 +40,7 @@ Spline2dKernel* Spline2dSolver::mutable_kernel() { return &kernel_; }
Spline2d
*
Spline2dSolver
::
mutable_spline
()
{
return
&
spline_
;
}
// solve
bool
Spline2dSolver
::
s
olve
()
{
bool
Spline2dSolver
::
S
olve
()
{
const
Eigen
::
MatrixXd
&
kernel_matrix
=
kernel_
.
kernel_matrix
();
const
Eigen
::
MatrixXd
&
offset
=
kernel_
.
offset
();
const
Eigen
::
MatrixXd
&
inequality_constraint_matrix
=
...
...
@@ -56,7 +56,7 @@ bool Spline2dSolver::solve() {
kernel_matrix
,
offset
,
inequality_constraint_matrix
,
inequality_constraint_boundary
,
equality_constraint_matrix
,
equality_constraint_boundary
));
if
(
!
qp_solver_
->
s
olve
())
{
if
(
!
qp_solver_
->
S
olve
())
{
return
false
;
}
...
...
modules/planning/math/smoothing_spline/spline_2d_solver.h
浏览文件 @
5e36ebe7
...
...
@@ -42,7 +42,7 @@ class Spline2dSolver {
Spline2d
*
mutable_spline
();
// solve
bool
s
olve
();
bool
S
olve
();
// extract
const
Spline2d
&
spline
()
const
;
...
...
modules/planning/optimizer/qp_spline_path/qp_spline_path_generator.cc
浏览文件 @
5e36ebe7
...
...
@@ -282,7 +282,7 @@ bool QpSplinePathGenerator::AddKernel() {
Spline1dKernel
*
spline_kernel
=
spline_generator_
->
mutable_spline_kernel
();
if
(
qp_spline_path_config_
.
regularization_weight
()
>
0
)
{
spline_kernel
->
add_r
egularization
(
spline_kernel
->
AddR
egularization
(
qp_spline_path_config_
.
regularization_weight
());
}
...
...
@@ -311,7 +311,7 @@ bool QpSplinePathGenerator::AddKernel() {
}
bool
QpSplinePathGenerator
::
Solve
()
{
if
(
!
spline_generator_
->
s
olve
())
{
if
(
!
spline_generator_
->
S
olve
())
{
AERROR
<<
"Could not solve the qp problem in spline path generator."
;
return
false
;
}
...
...
modules/planning/optimizer/qp_spline_st_speed/qp_spline_st_graph.cc
浏览文件 @
5e36ebe7
...
...
@@ -245,7 +245,7 @@ Status QpSplineStGraph::ApplyKernel(
}
Status
QpSplineStGraph
::
Solve
()
{
return
spline_generator_
->
s
olve
()
return
spline_generator_
->
S
olve
()
?
Status
::
OK
()
:
Status
(
ErrorCode
::
PLANNING_ERROR
,
"QpSplineStGraph::solve"
);
}
...
...
modules/planning/reference_line/reference_line_smoother.cc
浏览文件 @
5e36ebe7
...
...
@@ -75,7 +75,7 @@ bool ReferenceLineSmoother::smooth(
return
false
;
}
if
(
!
s
olve
())
{
if
(
!
S
olve
())
{
AERROR
<<
"Solve spline smoother problem failed"
;
}
...
...
@@ -90,16 +90,16 @@ bool ReferenceLineSmoother::smooth(
i
<
smoother_config_
.
num_of_total_points
()
&&
t
<
end_t
;
++
i
)
{
std
::
pair
<
double
,
double
>
xy
=
spline_solver_
->
spline
()(
t
);
const
double
heading
=
std
::
atan2
(
spline_solver_
->
spline
().
derivative_y
(
t
),
spline_solver_
->
spline
().
derivative_x
(
t
));
spline_solver_
->
spline
().
DerivativeX
(
t
));
const
double
kappa
=
CurveMath
::
ComputeCurvature
(
spline_solver_
->
spline
().
derivative_x
(
t
),
spline_solver_
->
spline
().
second_derivative_x
(
t
),
spline_solver_
->
spline
().
DerivativeX
(
t
),
spline_solver_
->
spline
().
SecondDerivativeX
(
t
),
spline_solver_
->
spline
().
derivative_y
(
t
),
spline_solver_
->
spline
().
second_derivative_y
(
t
));
const
double
dkappa
=
CurveMath
::
ComputeCurvatureDerivative
(
spline_solver_
->
spline
().
derivative_x
(
t
),
spline_solver_
->
spline
().
second_derivative_x
(
t
),
spline_solver_
->
spline
().
third_derivative_x
(
t
),
spline_solver_
->
spline
().
DerivativeX
(
t
),
spline_solver_
->
spline
().
SecondDerivativeX
(
t
),
spline_solver_
->
spline
().
ThirdDerivativeX
(
t
),
spline_solver_
->
spline
().
derivative_y
(
t
),
spline_solver_
->
spline
().
second_derivative_y
(
t
),
spline_solver_
->
spline
().
third_derivative_y
(
t
));
...
...
@@ -162,7 +162,7 @@ bool ReferenceLineSmoother::apply_constraint(
xy_points
.
emplace_back
(
path_points
[
i
].
x
(),
path_points
[
i
].
y
());
}
if
(
!
spline_solver_
->
mutable_constraint
()
->
add_2d_b
oundary
(
if
(
!
spline_solver_
->
mutable_constraint
()
->
Add2dB
oundary
(
evaluated_t
,
angles
,
xy_points
,
longitidinal_bound
,
lateral_bound
))
{
AERROR
<<
"Add 2d boundary constraint failed"
;
return
false
;
...
...
@@ -196,11 +196,11 @@ bool ReferenceLineSmoother::ApplyKernel() {
}
// TODO(fanhaoyang): change to a configurable param
kernel
->
add_r
egularization
(
0.01
);
kernel
->
AddR
egularization
(
0.01
);
return
true
;
}
bool
ReferenceLineSmoother
::
solve
()
{
return
spline_solver_
->
s
olve
();
}
bool
ReferenceLineSmoother
::
Solve
()
{
return
spline_solver_
->
S
olve
();
}
bool
ReferenceLineSmoother
::
extract_evaluated_points
(
const
ReferenceLine
&
raw_reference_line
,
const
std
::
vector
<
double
>&
vec_t
,
...
...
modules/planning/reference_line/reference_line_smoother.h
浏览文件 @
5e36ebe7
...
...
@@ -51,7 +51,7 @@ class ReferenceLineSmoother {
bool
ApplyKernel
();
bool
s
olve
();
bool
S
olve
();
bool
extract_evaluated_points
(
const
ReferenceLine
&
raw_reference_line
,
const
std
::
vector
<
double
>&
vec_t
,
...
...
编辑
预览
Markdown
is supported
0%
请重试
或
添加新附件
.
添加附件
取消
You are about to add
0
people
to the discussion. Proceed with caution.
先完成此消息的编辑!
取消
想要评论请
注册
或
登录