Skip to content
体验新版
项目
组织
正在加载...
登录
切换导航
打开侧边栏
Pinoxchio
apollo
提交
75594e82
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,发现更多精彩内容 >>
提交
75594e82
编写于
3月 02, 2018
作者:
L
Liangliang Zhang
提交者:
Qi Luo
3月 02, 2018
浏览文件
操作
浏览文件
下载
电子邮件补丁
差异文件
Perception: code changes.
上级
f827e5b7
变更
5
隐藏空白更改
内联
并排
Showing
5 changed file
with
80 addition
and
81 deletion
+80
-81
modules/common/util/util.h
modules/common/util/util.h
+10
-1
modules/perception/obstacle/camera/cipv/cipv.cc
modules/perception/obstacle/camera/cipv/cipv.cc
+36
-40
modules/perception/obstacle/camera/cipv/cipv.h
modules/perception/obstacle/camera/cipv/cipv.h
+16
-14
modules/perception/obstacle/camera/visualizer/frame_content.cc
...es/perception/obstacle/camera/visualizer/frame_content.cc
+13
-13
modules/perception/obstacle/camera/visualizer/frame_content.h
...les/perception/obstacle/camera/visualizer/frame_content.h
+5
-13
未找到文件。
modules/common/util/util.h
浏览文件 @
75594e82
...
...
@@ -32,11 +32,12 @@
#include "google/protobuf/util/message_differencer.h"
#include "modules/common/math/vec2d.h"
#include "modules/common/proto/geometry.pb.h"
#include "modules/common/proto/pnc_point.pb.h"
#include "modules/perception/proto/perception_obstacle.pb.h"
#include "modules/common/math/vec2d.h"
/**
* @namespace apollo::common::util
* @brief apollo::common::util
...
...
@@ -161,6 +162,14 @@ PathPoint GetWeightedAverageOfTwoPathPoints(const PathPoint& p1,
const
PathPoint
&
p2
,
const
double
w1
,
const
double
w2
);
// a wrapper template function for remove_if (notice that remove_if cannot
// change the Container size)
template
<
class
Container
,
class
F
>
auto
erase_where
(
Container
&
c
,
F
&&
f
)
{
return
c
.
erase
(
std
::
remove_if
(
c
.
begin
(),
c
.
end
(),
std
::
forward
<
F
>
(
f
)),
c
.
end
());
}
}
// namespace util
}
// namespace common
}
// namespace apollo
...
...
modules/perception/obstacle/camera/cipv/cipv.cc
浏览文件 @
75594e82
...
...
@@ -14,8 +14,10 @@
* limitations under the License.
*****************************************************************************/
#include <math.h>
#include "modules/perception/obstacle/camera/cipv/cipv.h"
#include <math.h>
#include "modules/common/log.h"
namespace
apollo
{
...
...
@@ -80,9 +82,9 @@ bool Cipv::distance_from_point_to_line_segment(
*
distance
=
sqrt
(
dx
*
dx
+
dy
*
dy
);
if
(
_debug_level
>=
2
)
{
AINFO
<<
"distance_from_point ("
<<
point
[
0
]
<<
", "
<<
point
[
1
]
<<
") _to_line_segment ("
<<
line_seg_start_point
[
0
]
<<
", "
<<
line_seg_start_point
[
1
]
<<
")->("
<<
line_seg_end_point
[
0
]
<<
", "
<<
line_seg_end_point
[
1
]
<<
"): "
<<
*
distance
<<
"m"
;
<<
") _to_line_segment ("
<<
line_seg_start_point
[
0
]
<<
", "
<<
line_seg_start_point
[
1
]
<<
")->("
<<
line_seg_end_point
[
0
]
<<
", "
<<
line_seg_end_point
[
1
]
<<
"): "
<<
*
distance
<<
"m"
;
}
return
true
;
}
...
...
@@ -95,7 +97,7 @@ bool Cipv::get_egolane(const LaneObjectsPtr lane_objects,
if
((
*
lane_objects
)[
i
].
spatial
==
L_0
)
{
if
(
_debug_level
>=
2
)
{
AINFO
<<
"[get_egolane]LEFT(*lane_objects)[i].image_pos.size(): "
<<
(
*
lane_objects
)[
i
].
image_pos
.
size
();
<<
(
*
lane_objects
)[
i
].
image_pos
.
size
();
}
if
((
*
lane_objects
)[
i
].
image_pos
.
size
()
<
MIN_LANE_LINE_LENGTH_FOR_CIPV_DETERMINATION
)
{
...
...
@@ -116,7 +118,7 @@ bool Cipv::get_egolane(const LaneObjectsPtr lane_objects,
}
else
if
((
*
lane_objects
)[
i
].
spatial
==
R_0
)
{
if
(
_debug_level
>=
2
)
{
AINFO
<<
"[get_egolane]RIGHT(*lane_objects)[i].image_pos.size(): "
<<
(
*
lane_objects
)[
i
].
image_pos
.
size
();
<<
(
*
lane_objects
)[
i
].
image_pos
.
size
();
}
if
((
*
lane_objects
)[
i
].
image_pos
.
size
()
<
MIN_LANE_LINE_LENGTH_FOR_CIPV_DETERMINATION
)
{
...
...
@@ -210,11 +212,10 @@ bool Cipv::make_virtual_ego_lane_from_yaw_rate(const float yaw_rate,
}
// Elongate lane line
bool
Cipv
::
elongate_egolane
(
const
LaneObjectsPtr
lane_objects
,
const
bool
b_left_valid
,
const
bool
b_right_valid
,
const
float
yaw_rate
,
const
float
velocity
,
EgoLane
*
egolane_image
,
EgoLane
*
egolane_ground
)
{
bool
Cipv
::
elongate_egolane
(
const
LaneObjectsPtr
lane_objects
,
const
bool
b_left_valid
,
const
bool
b_right_valid
,
const
float
yaw_rate
,
const
float
velocity
,
EgoLane
*
egolane_image
,
EgoLane
*
egolane_ground
)
{
float
offset_distance
=
EGO_CAR_HALF_VIRTUAL_LANE
;
// When left lane line is available
if
(
b_left_valid
&&
b_right_valid
)
{
...
...
@@ -427,9 +428,9 @@ bool Cipv::find_closest_edge_of_object_ground(
if
(
_debug_level
>=
2
)
{
AINFO
<<
"start("
<<
closted_object_edge
->
start_point
[
0
]
<<
", "
<<
closted_object_edge
->
start_point
[
1
]
<<
")->"
;
<<
closted_object_edge
->
start_point
[
1
]
<<
")->"
;
AINFO
<<
"end("
<<
closted_object_edge
->
end_point
[
0
]
<<
", "
<<
closted_object_edge
->
end_point
[
1
]
<<
")"
;
<<
closted_object_edge
->
end_point
[
1
]
<<
")"
;
}
return
true
;
}
...
...
@@ -443,28 +444,28 @@ bool Cipv::are_distances_sane(const float distance_start_point_to_right_lane,
if
(
distance_start_point_to_right_lane
>
MAX_DIST_OBJECT_TO_LANE_METER
)
{
if
(
_debug_level
>=
1
)
{
AINFO
<<
"distance from start to right lane("
<<
distance
<<
" m) is too long"
;
<<
" m) is too long"
;
}
return
false
;
}
if
(
distance_start_point_to_left_lane
>
MAX_DIST_OBJECT_TO_LANE_METER
)
{
if
(
_debug_level
>=
1
)
{
AINFO
<<
"distance from start to left lane("
<<
distance
<<
" m) is too long"
;
<<
" m) is too long"
;
}
return
false
;
}
if
(
distance_end_point_to_right_lane
>
MAX_DIST_OBJECT_TO_LANE_METER
)
{
if
(
_debug_level
>=
1
)
{
AINFO
<<
"distance from end to right lane("
<<
distance
<<
" m) is too long"
;
<<
" m) is too long"
;
}
return
false
;
}
if
(
distance_end_point_to_left_lane
>
MAX_DIST_OBJECT_TO_LANE_METER
)
{
if
(
_debug_level
>=
1
)
{
AINFO
<<
"distance from end to left lane("
<<
distance
<<
" m) is too long"
;
<<
" m) is too long"
;
}
return
false
;
}
...
...
@@ -504,19 +505,19 @@ bool Cipv::is_point_left_of_line(const Point2Df &point,
if
(
cross_product
>
0.0
f
)
{
if
(
_debug_level
>=
2
)
{
AINFO
<<
"point ("
<<
point
[
0
]
<<
", "
<<
point
[
1
]
<<
") is left of line_segment ("
<<
line_seg_start_point
[
0
]
<<
", "
<<
line_seg_start_point
[
1
]
<<
")->("
<<
line_seg_end_point
[
0
]
<<
", "
<<
line_seg_end_point
[
1
]
<<
"), cross_product: "
<<
cross_product
;
<<
") is left of line_segment ("
<<
line_seg_start_point
[
0
]
<<
", "
<<
line_seg_start_point
[
1
]
<<
")->("
<<
line_seg_end_point
[
0
]
<<
", "
<<
line_seg_end_point
[
1
]
<<
"), cross_product: "
<<
cross_product
;
}
return
true
;
}
else
{
if
(
_debug_level
>=
2
)
{
AINFO
<<
"point ("
<<
point
[
0
]
<<
", "
<<
point
[
1
]
<<
") is right of line_segment ("
<<
line_seg_start_point
[
0
]
<<
", "
<<
line_seg_start_point
[
1
]
<<
")->("
<<
line_seg_end_point
[
0
]
<<
", "
<<
line_seg_end_point
[
1
]
<<
"), cross_product: "
<<
cross_product
;
<<
") is right of line_segment ("
<<
line_seg_start_point
[
0
]
<<
", "
<<
line_seg_start_point
[
1
]
<<
")->("
<<
line_seg_end_point
[
0
]
<<
", "
<<
line_seg_end_point
[
1
]
<<
"), cross_product: "
<<
cross_product
;
}
return
false
;
}
...
...
@@ -561,7 +562,7 @@ bool Cipv::is_object_in_the_lane_ground(const ObjectPtr &object,
if
(
_debug_level
>=
3
)
{
AINFO
<<
"egolane_ground.left_line.line_point.size(): "
<<
egolane_ground
.
left_line
.
line_point
.
size
();
<<
egolane_ground
.
left_line
.
line_point
.
size
();
}
if
(
egolane_ground
.
left_line
.
line_point
.
size
()
<=
1
)
{
if
(
_debug_level
>=
1
)
{
...
...
@@ -591,7 +592,7 @@ bool Cipv::is_object_in_the_lane_ground(const ObjectPtr &object,
// Check if the end point is on the right of the line segment
if
(
_debug_level
>=
3
)
{
AINFO
<<
"[Left] closest_index: "
<<
closest_index
<<
", shortest_distance: "
<<
shortest_distance
;
<<
", shortest_distance: "
<<
shortest_distance
;
}
if
(
is_point_left_of_line
(
closted_object_edge
.
end_point
,
...
...
@@ -603,7 +604,7 @@ bool Cipv::is_object_in_the_lane_ground(const ObjectPtr &object,
if
(
_debug_level
>=
3
)
{
AINFO
<<
"egolane_ground.right_line.line_point.size(): "
<<
egolane_ground
.
right_line
.
line_point
.
size
();
<<
egolane_ground
.
right_line
.
line_point
.
size
();
}
// Check start_point and right lane
if
(
egolane_ground
.
right_line
.
line_point
.
size
()
<=
1
)
{
...
...
@@ -631,7 +632,7 @@ bool Cipv::is_object_in_the_lane_ground(const ObjectPtr &object,
if
(
closest_index
>=
0
)
{
if
(
_debug_level
>=
3
)
{
AINFO
<<
"[right] closest_index: "
<<
closest_index
<<
", shortest_distance: "
<<
shortest_distance
;
<<
", shortest_distance: "
<<
shortest_distance
;
}
// Check if the end point is on the right of the line segment
if
(
is_point_left_of_line
(
...
...
@@ -672,10 +673,9 @@ bool Cipv::is_object_in_the_lane(const ObjectPtr &object,
bool
Cipv
::
determine_cipv
(
std
::
shared_ptr
<
SensorObjects
>
sensor_objects
,
CipvOptions
*
options
)
{
if
(
_debug_level
>=
3
)
{
AINFO
<<
"Cipv Got SensorObjects size "
<<
sensor_objects
->
objects
.
size
();
AINFO
<<
"Cipv Got SensorObjects size "
<<
sensor_objects
->
objects
.
size
();
AINFO
<<
"Cipv Got lane object size "
<<
sensor_objects
->
lane_objects
->
size
();
<<
sensor_objects
->
lane_objects
->
size
();
}
float
yaw_rate
=
options
->
yaw_rate
;
float
velocity
=
options
->
yaw_rate
;
...
...
@@ -693,16 +693,14 @@ bool Cipv::determine_cipv(std::shared_ptr<SensorObjects> sensor_objects,
// Get ego lanes (in both image and ground coordinate)
get_egolane
(
sensor_objects
->
lane_objects
,
&
egolane_image
,
&
egolane_ground
,
&
b_left_valid
,
&
b_right_valid
);
elongate_egolane
(
sensor_objects
->
lane_objects
,
b_left_valid
,
b_right_valid
,
yaw_rate
,
velocity
,
&
egolane_image
,
&
egolane_ground
);
elongate_egolane
(
sensor_objects
->
lane_objects
,
b_left_valid
,
b_right_valid
,
yaw_rate
,
velocity
,
&
egolane_image
,
&
egolane_ground
);
for
(
int32_t
i
=
0
;
i
<
static_cast
<
int32_t
>
(
sensor_objects
->
objects
.
size
());
i
++
)
{
if
(
_debug_level
>=
2
)
{
AINFO
<<
"sensor_objects->objects[i]->track_id: "
<<
sensor_objects
->
objects
[
i
]
->
track_id
;
<<
sensor_objects
->
objects
[
i
]
->
track_id
;
}
if
(
is_object_in_the_lane
(
sensor_objects
->
objects
[
i
],
egolane_image
,
egolane_ground
)
==
true
)
{
...
...
@@ -761,9 +759,7 @@ bool Cipv::determine_cipv(std::shared_ptr<SensorObjects> sensor_objects,
return
true
;
}
std
::
string
Cipv
::
name
()
const
{
return
"Cipv"
;
}
std
::
string
Cipv
::
name
()
const
{
return
"Cipv"
;
}
// Register plugin.
// REGISTER_CIPV(Cipv);
...
...
modules/perception/obstacle/camera/cipv/cipv.h
浏览文件 @
75594e82
...
...
@@ -14,15 +14,17 @@
* limitations under the License.
*****************************************************************************/
#ifndef MODULES_PERCEPTION_OBSTACLE_CAMERA_CIPV_H
#define MODULES_PERCEPTION_OBSTACLE_CAMERA_CIPV_H
#ifndef MODULES_PERCEPTION_OBSTACLE_CAMERA_CIPV_H
_
#define MODULES_PERCEPTION_OBSTACLE_CAMERA_CIPV_H
_
#include <Eigen/Dense>
#include <Eigen/Eigen>
#include <Eigen/Geometry>
#include <array>
#include <memory>
#include <string>
#include "Eigen/Dense"
#include "Eigen/Eigen"
#include "Eigen/Geometry"
#include "modules/perception/obstacle/base/object.h"
#include "modules/perception/obstacle/camera/common/lane_object.h"
#include "modules/perception/obstacle/camera/lane_post_process/common/type.h"
...
...
@@ -75,9 +77,9 @@ class Cipv {
float
*
distance
);
// Determine CIPV among multiple objects
bool
get_egolane
(
const
LaneObjectsPtr
lane_objects
,
EgoLane
*
egolane_
image
,
EgoLane
*
egolane_groun
d
,
bool
*
b_
left_valid
,
bool
*
b_
right_valid
);
bool
get_egolane
(
const
LaneObjectsPtr
lane_objects
,
EgoLane
*
egolane_image
,
EgoLane
*
egolane_
ground
,
bool
*
b_left_vali
d
,
bool
*
b_right_valid
);
// Elongate lane line
bool
elongate_egolane
(
const
LaneObjectsPtr
lane_objects
,
...
...
@@ -132,8 +134,8 @@ class Cipv {
LaneLine
*
virtual_lane_line
);
float
vehicle_dynamics
(
const
uint32_t
tick
,
const
float
yaw_rate
,
const
float
velocity
,
const
float
time_unit
,
float
*
x
,
float
*
y
);
const
float
velocity
,
const
float
time_unit
,
float
*
x
,
float
*
y
);
// Make a virtual lane line using a yaw_rate
bool
make_virtual_ego_lane_from_yaw_rate
(
const
float
yaw_rate
,
const
float
velocity
,
...
...
@@ -141,13 +143,13 @@ class Cipv {
LaneLine
*
left_lane_line
,
LaneLine
*
right_lane_line
);
// Member variables
bool
_b_image_based_cipv
;
int32_t
_debug_level
;
float
_time_unit
;
bool
_b_image_based_cipv
=
false
;
int32_t
_debug_level
=
0
;
float
_time_unit
=
0.0
f
;
};
}
// namespace obstacle
}
// namespace perception
}
// namespace apollo
#endif // MODULES_PERCEPTION_OBSTACLE_CAMERA_CIPV_H
#endif // MODULES_PERCEPTION_OBSTACLE_CAMERA_CIPV_H
_
modules/perception/obstacle/camera/visualizer/frame_content.cc
浏览文件 @
75594e82
...
...
@@ -14,10 +14,10 @@
* limitations under the License.
*****************************************************************************/
#include "modules/perception/obstacle/camera/visualizer/frame_content.h"
#include <Eigen/LU>
#include <map>
#include "modules/common/log.h"
#include "modules/perception/obstacle/camera/visualizer/frame_content.h"
namespace
apollo
{
namespace
perception
{
...
...
@@ -143,7 +143,7 @@ void FrameContent::update_timestamp(double ref) {
}
best_ts
=
FLT_MAX
;
for
(
std
::
map
<
double
,
ImageContent
>::
iterator
it
=
_image_caches
.
begin
();
it
!=
_image_caches
.
end
();
it
++
)
{
it
!=
_image_caches
.
end
();
++
it
)
{
double
it_ts
=
it
->
first
;
if
(
it_ts
<
best_ts
&&
_current_image_timestamp
!=
it_ts
)
{
best_ts
=
it_ts
;
...
...
@@ -157,7 +157,7 @@ void FrameContent::update_timestamp(double ref) {
if
(
it
->
first
<
_current_image_timestamp
)
{
_image_caches
.
erase
(
it
++
);
}
else
{
it
++
;
++
it
;
}
}
...
...
@@ -171,7 +171,7 @@ void FrameContent::update_timestamp(double ref) {
best_ts
=
-
1
;
for
(
std
::
map
<
double
,
RadarContent
>::
iterator
it
=
_radar_caches
.
begin
();
it
!=
_radar_caches
.
end
();
it
++
)
{
it
!=
_radar_caches
.
end
();
++
it
)
{
double
it_ts
=
it
->
first
;
double
delta
=
fabs
(
it_ts
-
ref
);
...
...
@@ -186,13 +186,13 @@ void FrameContent::update_timestamp(double ref) {
if
(
it
->
first
<
best_ts
)
{
_radar_caches
.
erase
(
it
++
);
}
else
{
it
++
;
++
it
;
}
}
best_delta
=
FLT_MAX
;
best_ts
=
-
1
;
for
(
std
::
map
<
double
,
FusionContent
>::
iterator
it
=
_fusion_caches
.
begin
();
it
!=
_fusion_caches
.
end
();
it
++
)
{
it
!=
_fusion_caches
.
end
();
++
it
)
{
double
it_ts
=
it
->
first
;
double
delta
=
fabs
(
it_ts
-
ref
);
...
...
@@ -207,7 +207,7 @@ void FrameContent::update_timestamp(double ref) {
if
(
it
->
first
<
best_ts
)
{
_fusion_caches
.
erase
(
it
++
);
}
else
{
it
++
;
++
it
;
}
}
...
...
@@ -215,7 +215,7 @@ void FrameContent::update_timestamp(double ref) {
best_delta
=
FLT_MAX
;
best_ts
=
-
1
;
for
(
std
::
map
<
double
,
CameraContent
>::
iterator
it
=
_camera_caches
.
begin
();
it
!=
_camera_caches
.
end
();
it
++
)
{
it
!=
_camera_caches
.
end
();
++
it
)
{
double
it_ts
=
it
->
first
;
double
delta
=
fabs
(
it_ts
-
ref
);
...
...
@@ -230,14 +230,14 @@ void FrameContent::update_timestamp(double ref) {
if
(
it
->
first
<
best_ts
)
{
_camera_caches
.
erase
(
it
++
);
}
else
{
it
++
;
++
it
;
}
}
best_delta
=
FLT_MAX
;
best_ts
=
-
1
;
for
(
std
::
map
<
double
,
GroundTruthContent
>::
iterator
it
=
_gt_caches
.
begin
();
it
!=
_gt_caches
.
end
();
it
++
)
{
it
!=
_gt_caches
.
end
();
++
it
)
{
double
it_ts
=
it
->
first
;
double
delta
=
fabs
(
it_ts
-
ref
);
if
(
delta
<
best_delta
)
{
...
...
@@ -251,7 +251,7 @@ void FrameContent::update_timestamp(double ref) {
if
(
it
->
first
<
best_ts
)
{
_gt_caches
.
erase
(
it
++
);
}
else
{
it
++
;
++
it
;
}
}
...
...
@@ -259,7 +259,7 @@ void FrameContent::update_timestamp(double ref) {
best_delta
=
FLT_MAX
;
best_ts
=
-
1
;
for
(
std
::
map
<
double
,
MotionContent
>::
iterator
it
=
_motion_caches
.
begin
();
it
!=
_motion_caches
.
end
();
it
++
)
{
it
!=
_motion_caches
.
end
();
++
it
)
{
double
it_ts
=
it
->
first
;
double
delta
=
fabs
(
it_ts
-
ref
);
...
...
@@ -274,7 +274,7 @@ void FrameContent::update_timestamp(double ref) {
if
(
it
->
first
<
best_ts
)
{
_motion_caches
.
erase
(
it
++
);
}
else
{
it
++
;
++
it
;
}
}
}
...
...
modules/perception/obstacle/camera/visualizer/frame_content.h
浏览文件 @
75594e82
...
...
@@ -20,11 +20,11 @@
#include <boost/shared_ptr.hpp>
#include <deque>
#include <iomanip>
#include <map>
#include <sstream>
#include <string>
#include <thread>
#include <vector>
#include <map>
#include "modules/perception/obstacle/base/object.h"
#include "modules/perception/obstacle/base/object_supplement.h"
...
...
@@ -129,27 +129,19 @@ class FrameContent {
Eigen
::
Matrix4d
get_pose_v2w
();
cv
::
Mat
get_camera_image
();
int
get_pose_type
()
{
return
_continuous_type
;
}
int
get_pose_type
()
{
return
_continuous_type
;
}
void
set_pose_type
(
int
type
)
{
_continuous_type
=
type
;
}
void
set_pose_type
(
int
type
)
{
_continuous_type
=
type
;
}
std
::
vector
<
ObjectPtr
>
get_camera_objects
();
std
::
vector
<
ObjectPtr
>
get_radar_objects
();
double
get_visualization_timestamp
();
inline
bool
has_radar_data
()
{
return
_radar_caches
.
size
();
}
inline
bool
has_radar_data
()
{
return
_radar_caches
.
size
();
}
CameraFrameSupplementPtr
get_camera_frame_supplement
();
inline
bool
has_camera_data
()
{
return
_camera_caches
.
size
();
}
inline
bool
has_camera_data
()
{
return
_camera_caches
.
size
();
}
/* inline void set_camera2velo_pose(const Eigen::Matrix4d& pose) {
_pose_camera2velo = pose;
}*/
...
...
编辑
预览
Markdown
is supported
0%
请重试
或
添加新附件
.
添加附件
取消
You are about to add
0
people
to the discussion. Proceed with caution.
先完成此消息的编辑!
取消
想要评论请
注册
或
登录