Skip to content
体验新版
项目
组织
正在加载...
登录
切换导航
打开侧边栏
Pinoxchio
apollo
提交
1d6b9273
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,发现更多精彩内容 >>
提交
1d6b9273
编写于
2月 28, 2018
作者:
C
cheni-kuei
提交者:
Qi Luo
3月 01, 2018
浏览文件
操作
浏览文件
下载
电子邮件补丁
差异文件
Perception: Add the original cascaded tracker
上级
64ae2c6c
变更
3
隐藏空白更改
内联
并排
Showing
3 changed file
with
479 addition
and
4 deletion
+479
-4
modules/perception/obstacle/camera/tracker/BUILD
modules/perception/obstacle/camera/tracker/BUILD
+13
-4
modules/perception/obstacle/camera/tracker/cascaded_camera_tracker.cc
...eption/obstacle/camera/tracker/cascaded_camera_tracker.cc
+328
-0
modules/perception/obstacle/camera/tracker/cascaded_camera_tracker.h
...ception/obstacle/camera/tracker/cascaded_camera_tracker.h
+138
-0
未找到文件。
modules/perception/obstacle/camera/tracker/BUILD
浏览文件 @
1d6b9273
...
...
@@ -3,10 +3,19 @@ load("//tools:cpplint.bzl", "cpplint")
package
(
default_visibility
=
[
"//visibility:public"
])
cc_library
(
name
=
"camera_tracker_cascaded"
,
srcs
=
[
"cascaded_camera_tracker_util.cc"
],
hdrs
=
glob
([
"*.h"
]),
deps
=
name
=
"camera_tracker_cascaded"
,
srcs
=
[
"cascaded_camera_tracker.cc"
,
"cascaded_camera_tracker_util.cc"
],
hdrs
=
[
"base_affinity_tracker.h"
,
"cascaded_camera_tracker.h"
,
"cascaded_camera_tracker_util.h"
],
deps
=
[
"//modules/perception/lib/base"
,
"//modules/perception/obstacle/base:perception_obstacle_base"
,
...
...
modules/perception/obstacle/camera/tracker/cascaded_camera_tracker.cc
0 → 100644
浏览文件 @
1d6b9273
/******************************************************************************
* Copyright 2018 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 "mix_camera_tracker.h"
namespace
adu
{
namespace
perception
{
namespace
obstacle
{
bool
MixCameraTracker
::
init
()
{
// TODO Config here for turing trackers on or off
bool
init_flag
=
true
;
init_flag
&=
_cs2d_tracker
.
init
();
if
(
_with_dl_feature
)
{
init_flag
&=
_dlf_tracker
.
init
();
}
// // TODO Disable DAT for now
// init_flag &= _dat_tracker.init();
init_flag
&=
_kcf_tracker
.
init
();
return
init_flag
;
}
bool
MixCameraTracker
::
associate
(
const
cv
::
Mat
&
frame
,
const
std
::
vector
<
VisualObjectPtr
>
&
objects
,
double
timestamp
,
const
CameraTrackerOptions
&
options
,
std
::
vector
<
VisualObjectPtr
>
*
tracked_objects
)
{
_frame_cnt
++
;
// Profiling
Timer
timer
;
*
tracked_objects
=
objects
;
// TODO: What kind of API design is this? Who defined this
std
::
map
<
int
,
int
>
id_mapping
;
// detect to track
float
scale
=
1.0
f
;
// TODO put scale adjustment in config
cv
::
Mat
img
;
cv
::
resize
(
frame
,
img
,
cv
::
Size
(
static_cast
<
int
>
(
frame
.
cols
*
scale
),
static_cast
<
int
>
(
frame
.
rows
*
scale
)));
std
::
vector
<
Detected
>
detected
;
get_detected_from_vo
(
frame
.
size
(),
objects
,
scale
,
detected
);
for
(
auto
&
dets
:
detected
)
{
dets
.
_last_seen_frame_cnt
=
_frame_cnt
;
dets
.
_last_seen_timestamp
=
timestamp
;
}
if
(
_verbose
)
{
XLOG
(
INFO
)
<<
"#T:"
<<
_tracked
.
size
()
<<
" #D:"
<<
detected
.
size
();
}
// (From all trackers)
// 1. Get affinity matrix
std
::
vector
<
std
::
vector
<
float
>
>
affinity_matrix
;
affinity_matrix
=
std
::
vector
<
std
::
vector
<
float
>>
(
_tracked
.
size
(),
std
::
vector
<
float
>
(
detected
.
size
(),
1.0
f
));
// (0) cs2d
timer
.
tic
();
std
::
vector
<
std
::
vector
<
float
>
>
cs2d_affinity_matrix
;
_cs2d_tracker
.
set_full_selection
(
_tracked
.
size
(),
detected
.
size
());
_cs2d_tracker
.
get_affinity_matrix
(
img
,
_tracked
,
detected
,
cs2d_affinity_matrix
);
if
(
_verbose
)
{
XLOG
(
INFO
)
<<
"CS2D Affinity time: "
<<
timer
.
toc
()
<<
"ms"
;
}
// Merge
merge_affinity_matrix
(
cs2d_affinity_matrix
,
affinity_matrix
);
// (1) dlf
if
(
_with_dl_feature
)
{
timer
.
tic
();
std
::
vector
<
std
::
vector
<
float
>
>
dlf_affinity_matrix
;
_dlf_tracker
.
set_full_selection
(
_tracked
.
size
(),
detected
.
size
());
_dlf_tracker
.
get_affinity_matrix
(
img
,
_tracked
,
detected
,
dlf_affinity_matrix
);
// Merge
merge_affinity_matrix
(
dlf_affinity_matrix
,
affinity_matrix
);
// High confidence selection filtering. Thresholds are tuned
// 1st pass high value as 0.95. No competing high value in the cross
filter_affinity_matrix
(
0.95
f
,
0.95
f
,
affinity_matrix
);
// 2nd pass value above 0.7. The others for 0.3
filter_affinity_matrix
(
0.7
f
,
0.3
f
,
affinity_matrix
);
// 3rd pass value above 0.5. The others for 0.2
filter_affinity_matrix
(
0.5
f
,
0.2
f
,
affinity_matrix
);
if
(
_verbose
)
{
XLOG
(
INFO
)
<<
"DLF Affinity time: "
<<
timer
.
toc
()
<<
"ms"
;
}
}
// // TODO Disable DAT for now. Upgrade "update_tracked" logic in DAT
// // (2) dat
// timer.tic();
// std::vector<std::vector<float > > dat_affinity_matrix;
// _dat_tracker.set_selected_entries(affinity_matrix);
// _dat_tracker.get_affinity_matrix(img, _tracked, detected, dat_affinity_matrix);
// if (_verbose) {
// XLOG(INFO) << "DAT Affinity time: " << timer.toc() << "ms";
// }
//
// // Merge
// merge_affinity_matrix(dat_affinity_matrix, affinity_matrix);
// (3) kcf
timer
.
tic
();
std
::
vector
<
std
::
vector
<
float
>
>
kcf_affinity_matrix
;
_kcf_tracker
.
set_selected_entries
(
affinity_matrix
);
_kcf_tracker
.
get_affinity_matrix
(
img
,
_tracked
,
detected
,
kcf_affinity_matrix
);
if
(
_verbose
)
{
XLOG
(
INFO
)
<<
"KCF Affinity time: "
<<
timer
.
toc
()
<<
"ms"
;
}
// Merge
merge_affinity_matrix
(
kcf_affinity_matrix
,
affinity_matrix
);
// 2. Matching
std
::
unordered_map
<
int
,
int
>
local_matching
;
std
::
unordered_set
<
int
>
local_matched_detected
;
matrix_matching
(
affinity_matrix
,
local_matching
,
local_matched_detected
);
// 3. Tracker and ID management
tracker_and_id_management
(
local_matching
,
local_matched_detected
,
detected
,
_tracked
,
_next_tracked_id
,
id_mapping
,
_frame_cnt
);
// (From all trackers)
// 4. Update information used in tracks for the next frame
timer
.
tic
();
_cs2d_tracker
.
update_tracked
(
img
,
detected
,
_tracked
);
if
(
_verbose
)
{
XLOG
(
INFO
)
<<
"CS2D Update time: "
<<
timer
.
toc
()
<<
"ms"
;
}
if
(
_with_dl_feature
)
{
timer
.
tic
();
_dlf_tracker
.
update_tracked
(
img
,
detected
,
_tracked
);
if
(
_verbose
)
{
XLOG
(
INFO
)
<<
"DLF Update time: "
<<
timer
.
toc
()
<<
"ms"
;
}
}
// // TODO Disable DAT for now
// timer.tic();
// _dat_tracker.update_tracked(img, detected, _tracked);
// if (_verbose) {
// XLOG(INFO) << "DAT Update time: " << timer.toc() << "ms";
// }
timer
.
tic
();
_kcf_tracker
.
update_tracked
(
img
,
detected
,
_tracked
);
if
(
_verbose
)
{
XLOG
(
INFO
)
<<
"KCF Update time: "
<<
timer
.
toc
()
<<
"ms"
;
}
// Output: Id mapping from detected id to tracked id
for
(
auto
obj_ptr
:
*
tracked_objects
)
{
obj_ptr
->
latest_tracked_time
=
timestamp
;
if
(
id_mapping
.
find
(
obj_ptr
->
id
)
!=
id_mapping
.
end
())
{
obj_ptr
->
track_id
=
id_mapping
[
obj_ptr
->
id
];
}
else
{
// Should not happen since every detected object will have a tracked ID
XLOG
(
WARN
)
<<
"WARN: Detection"
<<
obj_ptr
->
id
<<
" has no tracking ID"
;
obj_ptr
->
track_id
=
-
1
;
}
}
return
true
;
}
bool
MixCameraTracker
::
predict_shape
(
const
cv
::
Mat
&
frame
,
const
std
::
vector
<
VisualObjectPtr
>
&
objects
,
double
timestamp
,
const
CameraTrackerOptions
&
options
,
std
::
vector
<
VisualObjectPtr
>
*
tracked_objects
)
{
return
true
;
}
bool
MixCameraTracker
::
predict_velocity
(
const
cv
::
Mat
&
frame
,
const
std
::
vector
<
VisualObjectPtr
>
&
objects
,
double
timestamp
,
const
CameraTrackerOptions
&
options
,
std
::
vector
<
VisualObjectPtr
>
*
tracked_objects
)
{
*
tracked_objects
=
objects
;
// update lost_frame_count
for
(
auto
iter
:
_tracked_filters
)
{
iter
.
second
.
lost_frame_count
+=
1
;
}
// create and reduce lost_frame_count to 0 if matched. predict here
for
(
auto
obj_ptr
:
*
tracked_objects
)
{
int
tracked_id
=
obj_ptr
->
track_id
;
double
now_timestamp
=
obj_ptr
->
latest_tracked_time
;
if
(
tracked_id
!=
-
1
)
{
if
(
_tracked_filters
.
find
(
tracked_id
)
!=
_tracked_filters
.
end
())
{
double
last_timestamp
=
_tracked_filters
[
tracked_id
].
last_seen_timestamp
;
_tracked_filters
[
tracked_id
].
lost_frame_count
=
0
;
_tracked_filters
[
tracked_id
].
last_seen_timestamp
=
obj_ptr
->
latest_tracked_time
;
_tracked_filters
[
tracked_id
].
position
.
predict
(
now_timestamp
-
last_timestamp
);
_tracked_filters
[
tracked_id
].
orientation
.
predict
(
now_timestamp
-
last_timestamp
);
_tracked_filters
[
tracked_id
].
size_3d_1
.
predict
(
now_timestamp
-
last_timestamp
);
_tracked_filters
[
tracked_id
].
size_3d_2
.
predict
(
now_timestamp
-
last_timestamp
);
}
else
{
_tracked_filters
[
tracked_id
]
=
Filter
();
_tracked_filters
[
tracked_id
].
track_id
=
tracked_id
;
_tracked_filters
[
tracked_id
].
last_seen_timestamp
=
obj_ptr
->
latest_tracked_time
;
}
}
}
// destroy old filters
std
::
vector
<
int
>
id_to_destroy
;
for
(
const
auto
iter
:
_tracked_filters
)
{
if
(
iter
.
second
.
lost_frame_count
>
_max_kep_frame_cnt
)
{
id_to_destroy
.
push_back
(
iter
.
first
);
}
}
for
(
const
auto
id
:
id_to_destroy
)
{
_tracked_filters
.
erase
(
id
);
}
// filter orientation and 3d size for tracked_objects in camera space
for
(
auto
obj_ptr
:
*
tracked_objects
)
{
int
tracked_id
=
obj_ptr
->
track_id
;
if
(
tracked_id
!=
-
1
&&
_tracked_filters
.
find
(
tracked_id
)
!=
_tracked_filters
.
end
())
{
// Regenerate theta...
// This definition is correct for OpenGL viewer...
double
beta
=
std
::
atan2
(
obj_ptr
->
center
.
x
(),
obj_ptr
->
center
.
z
());
double
theta
=
obj_ptr
->
alpha
+
beta
;
if
(
theta
>
M_PI
)
{
theta
-=
2
*
M_PI
;
}
else
if
(
theta
<
-
M_PI
)
{
theta
+=
2
*
M_PI
;
}
obj_ptr
->
theta
=
theta
;
// Orientation
Eigen
::
Vector2d
z
;
z
<<
obj_ptr
->
alpha
,
obj_ptr
->
theta
;
_tracked_filters
[
tracked_id
].
orientation
.
correct
(
z
);
Eigen
::
Vector4d
x
=
_tracked_filters
[
tracked_id
].
orientation
.
get_state
();
obj_ptr
->
alpha
=
x
(
0
);
obj_ptr
->
theta
=
x
(
1
);
// 3D size 1
z
<<
obj_ptr
->
height
,
obj_ptr
->
width
;
_tracked_filters
[
tracked_id
].
size_3d_1
.
correct
(
z
);
x
=
_tracked_filters
[
tracked_id
].
size_3d_1
.
get_state
();
obj_ptr
->
height
=
x
(
0
);
obj_ptr
->
width
=
x
(
1
);
// 3D size 2
z
<<
obj_ptr
->
length
,
0.0
;
_tracked_filters
[
tracked_id
].
size_3d_2
.
correct
(
z
);
x
=
_tracked_filters
[
tracked_id
].
size_3d_2
.
get_state
();
obj_ptr
->
length
=
x
(
0
);
}
}
// do orientation filter before this, cause it update everything about orientation and update the original
this
->
trans_object_to_world
(
options
,
tracked_objects
);
// filter position for tracked_objects in world space
for
(
auto
obj_ptr
:
*
tracked_objects
)
{
int
tracked_id
=
obj_ptr
->
track_id
;
if
(
tracked_id
!=
-
1
&&
_tracked_filters
.
find
(
tracked_id
)
!=
_tracked_filters
.
end
())
{
Eigen
::
Vector2d
z
;
z
<<
obj_ptr
->
center
(
0
),
obj_ptr
->
center
(
1
);
_tracked_filters
[
tracked_id
].
position
.
correct
(
z
);
Eigen
::
Vector4d
x
=
_tracked_filters
[
tracked_id
].
position
.
get_state
();
obj_ptr
->
center
(
0
)
=
x
[
0
];
obj_ptr
->
center
(
1
)
=
x
[
1
];
obj_ptr
->
velocity
(
0
)
=
x
[
2
];
obj_ptr
->
velocity
(
1
)
=
x
[
3
];
obj_ptr
->
velocity
(
2
)
=
0
;
}
}
return
true
;
}
std
::
string
MixCameraTracker
::
name
()
const
{
return
"MixCameraTracker"
;
}
REGISTER_CAMERA_TRACKER
(
MixCameraTracker
);
}
// namespace obstacle
}
// namespace perception
}
// namespace adu
modules/perception/obstacle/camera/tracker/cascaded_camera_tracker.h
0 → 100644
浏览文件 @
1d6b9273
/******************************************************************************
* Copyright 2018 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.
*****************************************************************************/
#ifndef ADU_PERCEPTION_OBSTACLE_MIX_CAMERA_TRACKER_H
#define ADU_PERCEPTION_OBSTACLE_MIX_CAMERA_TRACKER_H
#include <map>
#include <vector>
#include <algorithm>
#include "obstacle/camera/interface/base_camera_tracker.h"
#include "obstacle/camera/tracker/common/kalman_filter.h"
#include "obstacle/camera/tracker/mix_camera_tracker/mix_camera_tracker_util.h"
#include "obstacle/camera/tracker/mix_camera_tracker/base_affinity_tracker.h"
#include "obstacle/camera/tracker/mix_camera_tracker/cs2d/cs2d_affinity_tracker.h"
#include "obstacle/camera/tracker/mix_camera_tracker/dlf/dlf_affinity_tracker.h"
#include "obstacle/camera/tracker/mix_camera_tracker/dat/dat_affinity_tracker.h"
#include "obstacle/camera/tracker/mix_camera_tracker/kcf/kcf_affinity_tracker.h"
namespace
adu
{
namespace
perception
{
namespace
obstacle
{
class
MixCameraTracker
:
public
BaseCameraTracker
{
public:
MixCameraTracker
()
:
BaseCameraTracker
()
{}
virtual
~
MixCameraTracker
()
{}
virtual
bool
init
()
override
;
virtual
bool
associate
(
const
cv
::
Mat
&
frame
,
const
std
::
vector
<
VisualObjectPtr
>
&
objects
,
double
timestamp
,
const
CameraTrackerOptions
&
options
,
std
::
vector
<
VisualObjectPtr
>
*
tracked_objects
)
override
;
virtual
bool
predict_shape
(
const
cv
::
Mat
&
frame
,
const
std
::
vector
<
VisualObjectPtr
>
&
objects
,
double
timestamp
,
const
CameraTrackerOptions
&
options
,
std
::
vector
<
VisualObjectPtr
>
*
tracked_objects
)
override
;
virtual
bool
predict_velocity
(
const
cv
::
Mat
&
frame
,
const
std
::
vector
<
VisualObjectPtr
>
&
objects
,
double
timestamp
,
const
CameraTrackerOptions
&
options
,
std
::
vector
<
VisualObjectPtr
>
*
tracked_objects
)
override
;
virtual
std
::
string
name
()
const
override
;
private:
DISALLOW_COPY_AND_ASSIGN
(
MixCameraTracker
);
// Verbose to XLOG(INFO)
bool
_verbose
=
false
;
bool
_with_dl_feature
=
true
;
// TODO Put pipeline into config and organize it
// Trackers from different stages
CS2DAffinityTracker
_cs2d_tracker
;
DLFAffinityTracker
_dlf_tracker
;
DATAffinityTracker
_dat_tracker
;
KCFAffinityTracker
_kcf_tracker
;
// Tracking and ID management
std
::
vector
<
Tracked
>
_tracked
;
// All tracked objects, extended up to max frame latent space
int
_next_tracked_id
=
0
;
int
_frame_cnt
=
0
;
int
_max_kep_frame_cnt
=
10
;
class
Filter
{
public:
int
track_id
;
int
lost_frame_count
;
double
last_seen_timestamp
;
// TODO Make Kalman filter 3 dims
KalmanFilterConstVelocity
position
;
KalmanFilterConstVelocity
orientation
;
// TODO Temporary filtering for 3D size
KalmanFilterConstVelocity
size_3d_1
;
KalmanFilterConstVelocity
size_3d_2
;
Filter
()
:
track_id
(
-
1
),
lost_frame_count
(
0
),
last_seen_timestamp
(
0.0
)
{
// TODO more reasonable noises and put in config
position
.
init
();
position
.
_p
*=
50.0
;
position
.
_q
*=
50.0
;
position
.
_r
*=
50.0
;
orientation
.
init
();
orientation
.
_p
*=
10.0
;
orientation
.
_q
*=
10.0
;
orientation
.
_r
*=
10.0
;
size_3d_1
.
init
();
size_3d_1
.
_p
*=
10.0
;
size_3d_1
.
_q
*=
10.0
;
size_3d_1
.
_r
*=
10.0
;
size_3d_2
.
init
();
size_3d_2
.
_p
*=
10.0
;
size_3d_2
.
_q
*=
10.0
;
size_3d_2
.
_r
*=
10.0
;
}
};
std
::
map
<
int
,
Filter
>
_tracked_filters
;
};
}
// namespace obstacle
}
// namespace perception
}
// namespace adu
#endif //ADU_PERCEPTION_OBSTACLE_MIX_CAMERA_TRACKER_H
编辑
预览
Markdown
is supported
0%
请重试
或
添加新附件
.
添加附件
取消
You are about to add
0
people
to the discussion. Proceed with caution.
先完成此消息的编辑!
取消
想要评论请
注册
或
登录