Skip to content
体验新版
项目
组织
正在加载...
登录
切换导航
打开侧边栏
Pinoxchio
apollo
提交
8f509aea
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,发现更多精彩内容 >>
提交
8f509aea
编写于
7月 18, 2017
作者:
K
kechxu
提交者:
Calvin Miao
7月 18, 2017
浏览文件
操作
浏览文件
下载
电子邮件补丁
差异文件
implement SetMotionStatus() in prediction/container/obstacles/obstacle.cc
上级
61877230
变更
3
隐藏空白更改
内联
并排
Showing
3 changed file
with
82 addition
and
1 deletion
+82
-1
modules/prediction/common/prediction_gflags.cc
modules/prediction/common/prediction_gflags.cc
+7
-0
modules/prediction/common/prediction_gflags.h
modules/prediction/common/prediction_gflags.h
+5
-0
modules/prediction/container/obstacles/obstacle.cc
modules/prediction/container/obstacles/obstacle.cc
+70
-1
未找到文件。
modules/prediction/common/prediction_gflags.cc
浏览文件 @
8f509aea
...
...
@@ -34,3 +34,10 @@ DEFINE_double(go_approach_rate, 0.995,
"The rate to approach to the reference line of going straight"
);
DEFINE_double
(
cutin_approach_rate
,
0.9
,
"The rate to approach to the reference line of lane change"
);
DEFINE_int32
(
still_obstacle_history_length
,
10
,
"Min # historical frames for still obstacles"
);
DEFINE_bool
(
enable_kf_tracking
,
true
,
"Use measurements with KF tracking"
);
DEFINE_double
(
still_obstacle_speed_threshold
,
1.0
,
"Speed threshold for still obstacles"
);
DEFINE_double
(
still_obstacle_position_std
,
1.0
,
"Position standard deviation for still obstacles"
);
modules/prediction/common/prediction_gflags.h
浏览文件 @
8f509aea
...
...
@@ -31,5 +31,10 @@ DECLARE_double(r_var);
DECLARE_double
(
p_var
);
DECLARE_double
(
go_approach_rate
);
DECLARE_double
(
cutin_approach_rate
);
DECLARE_int32
(
still_obstacle_history_length
);
DECLARE_bool
(
enable_kf_tracking
);
DECLARE_double
(
still_obstacle_speed_threshold
);
DECLARE_double
(
still_obstacle_position_std
);
#endif // MODULES_PREDICTION_COMMON_PREDICTION_GFLAGS_H_
modules/prediction/container/obstacles/obstacle.cc
浏览文件 @
8f509aea
...
...
@@ -609,9 +609,78 @@ void Obstacle::SetNearbyLanes(Feature* feature) {
}
void
Obstacle
::
SetMotionStatus
()
{
// TODO(kechxu) implement
bool
is_still
=
true
;
int
history_size
=
static_cast
<
int
>
(
feature_history_
.
size
());
if
(
history_size
<
2
)
{
ADEBUG
<<
"Obstacle ["
<<
id_
<<
"] has no history and "
<<
"is considered still [default = true]."
;
if
(
history_size
>
0
)
{
feature_history_
.
front
().
set_is_still
(
is_still
);
ADEBUG
<<
"Obstacle ["
<<
id_
<<
"] has stillness status ["
<<
is_still
<<
"]."
;
}
return
;
}
double
start_x
=
0.0
;
double
start_y
=
0.0
;
double
avg_drift_x
=
0.0
;
double
avg_drift_y
=
0.0
;
int
len
=
std
::
min
(
history_size
,
FLAGS_still_obstacle_history_length
);
CHECK
(
len
>
1
);
auto
feature_riter
=
feature_history_
.
rbegin
();
if
(
FLAGS_enable_kf_tracking
)
{
start_x
=
feature_riter
->
t_position
().
x
();
start_y
=
feature_riter
->
t_position
().
y
();
}
else
{
start_x
=
feature_riter
->
position
().
x
();
start_y
=
feature_riter
->
position
().
y
();
}
++
feature_riter
;
while
(
feature_riter
!=
feature_history_
.
rend
())
{
if
(
FLAGS_enable_kf_tracking
)
{
avg_drift_x
+=
(
feature_riter
->
t_position
().
x
()
-
start_x
)
/
(
len
-
1
);
avg_drift_y
+=
(
feature_riter
->
t_position
().
y
()
-
start_y
)
/
(
len
-
1
);
}
else
{
avg_drift_x
+=
(
feature_riter
->
position
().
x
()
-
start_x
)
/
(
len
-
1
);
avg_drift_y
+=
(
feature_riter
->
position
().
y
()
-
start_y
)
/
(
len
-
1
);
}
++
feature_riter
;
}
double
delta_ts
=
feature_history_
.
front
().
timestamp
()
-
feature_history_
.
back
().
timestamp
();
double
std
=
FLAGS_still_obstacle_position_std
;
double
speed_sensibility
=
std
::
sqrt
(
2
*
history_size
)
*
4
*
std
/
((
history_size
+
1
)
*
delta_ts
);
double
speed
=
(
FLAGS_enable_kf_tracking
?
feature_history_
.
front
().
t_speed
()
:
feature_history_
.
front
().
speed
());
double
speed_threshold
=
FLAGS_still_obstacle_speed_threshold
;
if
(
apollo
::
common
::
math
::
DoubleCompare
(
speed
,
speed_threshold
)
<
0
)
{
is_still
=
true
;
ADEBUG
<<
"Obstacle ["
<<
id_
<<
"] has a small speed and is considered still."
;
}
else
if
(
apollo
::
common
::
math
::
DoubleCompare
(
speed_sensibility
,
speed_threshold
)
<
0
)
{
is_still
=
false
;
ADEBUG
<<
"Obstacle ["
<<
id_
<<
"] has a too short history ["
<<
history_size
<<
"] and is considered moving [sensibility = "
<<
speed_sensibility
<<
"]"
;
}
else
{
double
distance
=
std
::
hypot
(
avg_drift_x
,
avg_drift_y
);
double
distance_std
=
std
::
sqrt
(
2.0
/
len
)
*
std
;
if
(
apollo
::
common
::
math
::
DoubleCompare
(
distance
,
2.0
*
distance_std
)
>
0
)
{
is_still
=
false
;
ADEBUG
<<
"Obstacle ["
<<
id_
<<
"] is moving."
;
}
else
{
is_still
=
true
;
ADEBUG
<<
"Obstacle ["
<<
id_
<<
"] is still."
;
}
}
}
void
Obstacle
::
InsertFeatureToHistory
(
Feature
*
feature
)
{
// TODO(kechxu) implement
}
...
...
编辑
预览
Markdown
is supported
0%
请重试
或
添加新附件
.
添加附件
取消
You are about to add
0
people
to the discussion. Proceed with caution.
先完成此消息的编辑!
取消
想要评论请
注册
或
登录