Skip to content
体验新版
项目
组织
正在加载...
登录
切换导航
打开侧边栏
Pinoxchio
apollo
提交
f68010fa
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,发现更多精彩内容 >>
提交
f68010fa
编写于
9月 29, 2018
作者:
okkoc
提交者:
Jiangtao Hu
9月 29, 2018
浏览文件
操作
浏览文件
下载
电子邮件补丁
差异文件
perception:leave hdmap
上级
392f0132
变更
2
隐藏空白更改
内联
并排
Showing
2 changed file
with
360 addition
and
358 deletion
+360
-358
modules/perception/map/hdmap/hdmap_input.cc
modules/perception/map/hdmap/hdmap_input.cc
+345
-344
modules/perception/map/hdmap/hdmap_input.h
modules/perception/map/hdmap/hdmap_input.h
+15
-14
未找到文件。
modules/perception/map/hdmap/hdmap_input.cc
浏览文件 @
f68010fa
...
@@ -24,9 +24,9 @@
...
@@ -24,9 +24,9 @@
// #include "hdmap_definitions.h" // NOLINT
// #include "hdmap_definitions.h" // NOLINT
// #include "hdmap_trajectory.h" // NOLINT
// #include "hdmap_trajectory.h" // NOLINT
//
#include "modules/perception/base/object_pool_types.h"
#include "modules/perception/base/object_pool_types.h"
//
#include "modules/perception/lib/config_manager/config_manager.h"
#include "modules/perception/lib/config_manager/config_manager.h"
//
#include "modules/perception/lib/io/file_util.h"
#include "modules/perception/lib/io/file_util.h"
namespace
apollo
{
namespace
apollo
{
namespace
perception
{
namespace
perception
{
...
@@ -34,232 +34,233 @@ namespace map {
...
@@ -34,232 +34,233 @@ namespace map {
// using adu::common::math::Polygon2d;
// using adu::common::math::Polygon2d;
// using adu::common::math::Vec2d;
// using adu::common::math::Vec2d;
// using adu::hdmap::JunctionInfo;
using
apollo
::
hdmap
::
JunctionInfo
;
// using adu::hdmap::JunctionInfoConstPtr;
using
apollo
::
hdmap
::
JunctionInfoConstPtr
;
// using adu::hdmap::RoiRoadBoundaryPtr;
using
apollo
::
hdmap
::
RoadROIBoundaryPtr
;
using
apollo
::
hdmap
::
JunctionBoundaryPtr
;
// using adu::hdmap::BoundaryLine;
// using adu::hdmap::BoundaryLine;
// using adu::hdmap::SignalInfoConstPtr;
// using adu::hdmap::SignalInfoConstPtr;
// using adu
::hdmap::LaneInfoConstPtr;
using
apollo
::
hdmap
::
LaneInfoConstPtr
;
//
using lib::ConfigManager;
using
lib
::
ConfigManager
;
//
using base::PolygonDType;
using
base
::
PolygonDType
;
using
base
::
PointD
;
using
base
::
PointD
;
using
base
::
RoadBoundary
;
using
base
::
RoadBoundary
;
//
using base::PointDCloud;
using
base
::
PointDCloud
;
//
using base::PointDCloudPtr;
using
base
::
PointDCloudPtr
;
// HDMapInput
// HDMapInput
bool
HDMapInput
::
Init
()
{
bool
HDMapInput
::
Init
()
{
//
lib::MutexLock lock(&mutex_);
lib
::
MutexLock
lock
(
&
mutex_
);
return
InitInternal
();
return
InitInternal
();
}
}
bool
HDMapInput
::
InitInternal
()
{
bool
HDMapInput
::
InitInternal
()
{
//
if (inited_) {
if
(
inited_
)
{
//
return true;
return
true
;
//
}
}
//
if (!InitHDMap()) {
if
(
!
InitHDMap
())
{
//
return false;
return
false
;
//
}
}
//
inited_ = true;
inited_
=
true
;
return
true
;
return
true
;
}
}
bool
HDMapInput
::
Reset
()
{
bool
HDMapInput
::
Reset
()
{
//
lib::MutexLock lock(&mutex_);
lib
::
MutexLock
lock
(
&
mutex_
);
//
inited_ = false;
inited_
=
false
;
return
InitInternal
();
return
InitInternal
();
}
}
bool
HDMapInput
::
InitHDMap
()
{
bool
HDMapInput
::
InitHDMap
()
{
// hdmap_.reset(new adu
::hdmap::HDMap());
hdmap_
.
reset
(
new
apollo
::
hdmap
::
HDMap
());
//
ConfigManager* config_manager =
ConfigManager
*
config_manager
=
//
lib::Singleton<ConfigManager>::get_instance();
lib
::
Singleton
<
ConfigManager
>::
get_instance
();
//
CHECK_NOTNULL(config_manager);
CHECK_NOTNULL
(
config_manager
);
//
std::string model_name = "HDMapInput";
std
::
string
model_name
=
"HDMapInput"
;
//
const lib::ModelConfig* model_config = NULL;
const
lib
::
ModelConfig
*
model_config
=
NULL
;
//
if (!config_manager->GetModelConfig(model_name, &model_config)) {
if
(
!
config_manager
->
GetModelConfig
(
model_name
,
&
model_config
))
{
//
AERROR << "not found model: " << model_name;
AERROR
<<
"not found model: "
<<
model_name
;
//
return false;
return
false
;
//
}
}
//
if (!model_config->get_value("hdmap_sample_step", &hdmap_sample_step_)) {
if
(
!
model_config
->
get_value
(
"hdmap_sample_step"
,
&
hdmap_sample_step_
))
{
//
AERROR << "hdmap_sample_step not found.";
AERROR
<<
"hdmap_sample_step not found."
;
//
hdmap_sample_step_ = 5;
hdmap_sample_step_
=
5
;
//
}
}
//
if (!model_config->get_value("hdmap_file", &hdmap_file_)) {
if
(
!
model_config
->
get_value
(
"hdmap_file"
,
&
hdmap_file_
))
{
//
AERROR << "hdmap_file not found.";
AERROR
<<
"hdmap_file not found."
;
//
return false;
return
false
;
//
}
}
//
if (!lib::FileUtil::Exists(hdmap_file_)) {
if
(
!
lib
::
FileUtil
::
Exists
(
hdmap_file_
))
{
//
AERROR << "Failed to find hadmap file: " << hdmap_file_;
AERROR
<<
"Failed to find hadmap file: "
<<
hdmap_file_
;
//
return false;
return
false
;
//
}
}
// if (hdmap_->load_map_from_f
ile(hdmap_file_) != 0) {
if
(
hdmap_
->
LoadMapFromF
ile
(
hdmap_file_
)
!=
0
)
{
//
AERROR << "Failed to load hadmap file: " << hdmap_file_;
AERROR
<<
"Failed to load hadmap file: "
<<
hdmap_file_
;
//
return false;
return
false
;
//
}
}
//
AINFO << "load hdmap file: " << hdmap_file_;
AINFO
<<
"load hdmap file: "
<<
hdmap_file_
;
return
true
;
return
true
;
}
}
bool
HDMapInput
::
GetRoiHDMapStruct
(
bool
HDMapInput
::
GetRoiHDMapStruct
(
const
base
::
PointD
&
pointd
,
const
double
distance
,
const
base
::
PointD
&
pointd
,
const
double
distance
,
std
::
shared_ptr
<
base
::
HdmapStruct
>
hdmap_struct_prt
)
{
std
::
shared_ptr
<
base
::
HdmapStruct
>
hdmap_struct_prt
)
{
//
lib::MutexLock lock(&mutex_);
lib
::
MutexLock
lock
(
&
mutex_
);
//
CHECK_NOTNULL(hdmap_.get());
CHECK_NOTNULL
(
hdmap_
.
get
());
//
//
get original road boundary and junction
// get original road boundary and junction
// std::vector<RoiRoad
BoundaryPtr> road_boundary_vec;
std
::
vector
<
RoadROI
BoundaryPtr
>
road_boundary_vec
;
// std::vector<JunctionInfoConst
Ptr> junctions_vec;
std
::
vector
<
JunctionBoundary
Ptr
>
junctions_vec
;
// adu::common::hdmap::Point
point;
apollo
::
common
::
PointENU
point
;
//
point.set_x(pointd.x);
point
.
set_x
(
pointd
.
x
);
//
point.set_y(pointd.y);
point
.
set_y
(
pointd
.
y
);
//
point.set_z(pointd.z);
point
.
set_z
(
pointd
.
z
);
// int status = hdmap_->get_road_b
oundaries(point, distance,
int
status
=
hdmap_
->
GetRoadB
oundaries
(
point
,
distance
,
//
&road_boundary_vec,
&
road_boundary_vec
,
//
&junctions_vec);
&
junctions_vec
);
//
if (status != 0) {
if
(
status
!=
0
)
{
//
AERROR << "Failed to get road boundary, point: "
AERROR
<<
"Failed to get road boundary, point: "
//
<< point.DebugString();
<<
point
.
DebugString
();
//
return false;
return
false
;
//
}
}
//
if (hdmap_struct_prt != nullptr) {
if
(
hdmap_struct_prt
!=
nullptr
)
{
//
hdmap_struct_prt->hole_polygons.clear();
hdmap_struct_prt
->
hole_polygons
.
clear
();
//
hdmap_struct_prt->junction_polygons.clear();
hdmap_struct_prt
->
junction_polygons
.
clear
();
//
hdmap_struct_prt->road_boundary.clear();
hdmap_struct_prt
->
road_boundary
.
clear
();
//
hdmap_struct_prt->road_polygons.clear();
hdmap_struct_prt
->
road_polygons
.
clear
();
//
} else {
}
else
{
//
return false;
return
false
;
//
}
}
//
//
merge boundary and junction
// merge boundary and junction
//
std::vector<RoadBoundary> road_boundaries;
std
::
vector
<
RoadBoundary
>
road_boundaries
;
//
MergeBoundaryJunction(
MergeBoundaryJunction
(
//
road_boundary_vec,
road_boundary_vec
,
//
junctions_vec,
junctions_vec
,
//
&road_boundaries,
&
road_boundaries
,
//
&(hdmap_struct_prt->road_polygons),
&
(
hdmap_struct_prt
->
road_polygons
),
//
&(hdmap_struct_prt->junction_polygons));
&
(
hdmap_struct_prt
->
junction_polygons
));
//
//
filter road boundary by junction
// filter road boundary by junction
//
GetRoadBoundaryFilteredByJunctions(
GetRoadBoundaryFilteredByJunctions
(
//
road_boundaries,
road_boundaries
,
//
hdmap_struct_prt->junction_polygons,
hdmap_struct_prt
->
junction_polygons
,
//
&(hdmap_struct_prt->road_boundary));
&
(
hdmap_struct_prt
->
road_boundary
));
return
true
;
return
true
;
}
}
//
void HDMapInput::MergeBoundaryJunction(
void
HDMapInput
::
MergeBoundaryJunction
(
// const std::vector<adu::hdmap::RoiRoad
BoundaryPtr>& boundary,
const
std
::
vector
<
apollo
::
hdmap
::
RoadROI
BoundaryPtr
>&
boundary
,
// const std::vector<adu::hdmap::JunctionInfoConst
Ptr>& junctions,
const
std
::
vector
<
apollo
::
hdmap
::
JunctionBoundary
Ptr
>&
junctions
,
//
std::vector<base::RoadBoundary>* road_boundaries_ptr,
std
::
vector
<
base
::
RoadBoundary
>*
road_boundaries_ptr
,
//
std::vector<base::PolygonDType>* road_polygons_ptr,
std
::
vector
<
base
::
PolygonDType
>*
road_polygons_ptr
,
//
std::vector<base::PolygonDType>* junction_polygons_ptr) {
std
::
vector
<
base
::
PolygonDType
>*
junction_polygons_ptr
)
{
//
const int boundary_size = static_cast<int>(boundary.size());
const
int
boundary_size
=
static_cast
<
int
>
(
boundary
.
size
());
//
const int junctions_size = static_cast<int>(junctions.size());
const
int
junctions_size
=
static_cast
<
int
>
(
junctions
.
size
());
//
const int polygon_size = boundary_size;
const
int
polygon_size
=
boundary_size
;
//
road_boundaries_ptr->clear();
road_boundaries_ptr
->
clear
();
//
road_polygons_ptr->clear();
road_polygons_ptr
->
clear
();
//
junction_polygons_ptr->clear();
junction_polygons_ptr
->
clear
();
//
road_polygons_ptr->resize(polygon_size);
road_polygons_ptr
->
resize
(
polygon_size
);
//
junction_polygons_ptr->resize(junctions_size);
junction_polygons_ptr
->
resize
(
junctions_size
);
//
road_boundaries_ptr->resize(polygon_size);
road_boundaries_ptr
->
resize
(
polygon_size
);
//
int polygons_index = 0;
int
polygons_index
=
0
;
//
// merege boundary
// merege boundary
//
int step = hdmap_sample_step_;
int
step
=
hdmap_sample_step_
;
//
PointDCloudPtr temp_cloud = base::PointDCloudPool::Instance().Get();
PointDCloudPtr
temp_cloud
=
base
::
PointDCloudPool
::
Instance
().
Get
();
//
for (int i = 0; i < polygon_size; i++) {
for
(
int
i
=
0
;
i
<
polygon_size
;
i
++
)
{
//
temp_cloud->clear();
temp_cloud
->
clear
();
//
const BoundaryLine& left_boundary = boundary[i]->left_boundary;
const
BoundaryLine
&
left_boundary
=
boundary
[
i
]
->
left_boundary
;
// const std::vector<adu::common::hdmap::Point
>& left_line_points =
const
std
::
vector
<
apollo
::
common
::
PointENU
>&
left_line_points
=
//
left_boundary.line_points;
left_boundary
.
line_points
;
//
ADEBUG << "input left road_boundary size = " <<
ADEBUG
<<
"input left road_boundary size = "
<<
//
left_line_points.size();
left_line_points
.
size
();
//
step = (left_line_points.size() > 2) ? hdmap_sample_step_ : 1;
step
=
(
left_line_points
.
size
()
>
2
)
?
hdmap_sample_step_
:
1
;
//
for (int idx = 0; idx < left_line_points.size(); idx += step) {
for
(
int
idx
=
0
;
idx
<
left_line_points
.
size
();
idx
+=
step
)
{
//
PointD pointd;
PointD
pointd
;
//
pointd.x = left_line_points.at(idx).x();
pointd
.
x
=
left_line_points
.
at
(
idx
).
x
();
//
pointd.y = left_line_points.at(idx).y();
pointd
.
y
=
left_line_points
.
at
(
idx
).
y
();
//
pointd.z = left_line_points.at(idx).z();
pointd
.
z
=
left_line_points
.
at
(
idx
).
z
();
//
temp_cloud->push_back(pointd);
temp_cloud
->
push_back
(
pointd
);
//
}
}
//
DownsamplePoints(temp_cloud,
DownsamplePoints
(
temp_cloud
,
//
&(road_boundaries_ptr->at(polygons_index).left_boundary));
&
(
road_boundaries_ptr
->
at
(
polygons_index
).
left_boundary
));
//
for (int index = 0;
for
(
int
index
=
0
;
//
index < road_boundaries_ptr->at(polygons_index).left_boundary.size();
index
<
road_boundaries_ptr
->
at
(
polygons_index
).
left_boundary
.
size
();
//
++index) {
++
index
)
{
//
road_polygons_ptr->at(polygons_index).push_back(
road_polygons_ptr
->
at
(
polygons_index
).
push_back
(
//
road_boundaries_ptr->at(polygons_index).left_boundary[index]);
road_boundaries_ptr
->
at
(
polygons_index
).
left_boundary
[
index
]);
//
}
}
//
ADEBUG << "left road_boundary downsample size = "
ADEBUG
<<
"left road_boundary downsample size = "
//
<< road_polygons_ptr->at(polygons_index).size();
<<
road_polygons_ptr
->
at
(
polygons_index
).
size
();
//
temp_cloud->clear();
temp_cloud
->
clear
();
//
const BoundaryLine& right_boundary = boundary[i]->right_boundary;
const
BoundaryLine
&
right_boundary
=
boundary
[
i
]
->
right_boundary
;
// const std::vector<adu::common::hdmap::Point
>& right_line_points
const
std
::
vector
<
apollo
::
common
::
PointENU
>&
right_line_points
//
= right_boundary.line_points;
=
right_boundary
.
line_points
;
//
ADEBUG << "input right road_boundary size = "
ADEBUG
<<
"input right road_boundary size = "
//
<< right_line_points.size();
<<
right_line_points
.
size
();
//
step = (right_line_points.size() > 2) ? hdmap_sample_step_ : 1;
step
=
(
right_line_points
.
size
()
>
2
)
?
hdmap_sample_step_
:
1
;
//
for (int idx = 0; idx < right_line_points.size(); idx += step) {
for
(
int
idx
=
0
;
idx
<
right_line_points
.
size
();
idx
+=
step
)
{
//
PointD pointd;
PointD
pointd
;
//
pointd.x = right_line_points.at(idx).x();
pointd
.
x
=
right_line_points
.
at
(
idx
).
x
();
//
pointd.y = right_line_points.at(idx).y();
pointd
.
y
=
right_line_points
.
at
(
idx
).
y
();
//
pointd.z = right_line_points.at(idx).z();
pointd
.
z
=
right_line_points
.
at
(
idx
).
z
();
//
temp_cloud->push_back(pointd);
temp_cloud
->
push_back
(
pointd
);
//
}
}
//
DownsamplePoints(temp_cloud,
DownsamplePoints
(
temp_cloud
,
//
&(road_boundaries_ptr->at(polygons_index).right_boundary));
&
(
road_boundaries_ptr
->
at
(
polygons_index
).
right_boundary
));
//
for (int index = 0;
for
(
int
index
=
0
;
//
index <
index
<
//
road_boundaries_ptr->at(polygons_index).right_boundary.size();
road_boundaries_ptr
->
at
(
polygons_index
).
right_boundary
.
size
();
//
++index) {
++
index
)
{
//
road_polygons_ptr->at(polygons_index).push_back(
road_polygons_ptr
->
at
(
polygons_index
).
push_back
(
//
road_boundaries_ptr->at(polygons_index).right_boundary[
road_boundaries_ptr
->
at
(
polygons_index
).
right_boundary
[
//
road_boundaries_ptr->at(
road_boundaries_ptr
->
at
(
//
polygons_index).right_boundary.size() - 1 - index]);
polygons_index
).
right_boundary
.
size
()
-
1
-
index
]);
//
}
}
//
ADEBUG << "right road_boundary downsample size = "
ADEBUG
<<
"right road_boundary downsample size = "
//
<< road_polygons_ptr->at(polygons_index).size();
<<
road_polygons_ptr
->
at
(
polygons_index
).
size
();
//
polygons_index++;
polygons_index
++
;
//
}
}
//
// merge junctions
// merge junctions
//
for (int idx = 0; idx < junctions_size; idx++) {
for
(
int
idx
=
0
;
idx
<
junctions_size
;
idx
++
)
{
//
const Polygon2d& polygon = junctions[idx]->polygon();
const
Polygon2d
&
polygon
=
junctions
[
idx
]
->
polygon
();
//
const std::vector<Vec2d>& points = polygon.points();
const
std
::
vector
<
Vec2d
>&
points
=
polygon
.
points
();
//
for (size_t idj = 0; idj < points.size(); ++idj) {
for
(
size_t
idj
=
0
;
idj
<
points
.
size
();
++
idj
)
{
//
PointD pointd;
PointD
pointd
;
//
pointd.x = points[idj].x();
pointd
.
x
=
points
[
idj
].
x
();
//
pointd.y = points[idj].y();
pointd
.
y
=
points
[
idj
].
y
();
//
pointd.z = 0.0;
pointd
.
z
=
0.0
;
//
junction_polygons_ptr->at(idx).push_back(pointd);
junction_polygons_ptr
->
at
(
idx
).
push_back
(
pointd
);
//
}
}
//
}
}
//
}
}
bool
HDMapInput
::
GetRoadBoundaryFilteredByJunctions
(
bool
HDMapInput
::
GetRoadBoundaryFilteredByJunctions
(
const
std
::
vector
<
base
::
RoadBoundary
>&
road_boundaries
,
const
std
::
vector
<
base
::
RoadBoundary
>&
road_boundaries
,
const
std
::
vector
<
base
::
PointCloud
<
PointD
>>&
junctions
,
const
std
::
vector
<
base
::
PointCloud
<
PointD
>>&
junctions
,
std
::
vector
<
base
::
RoadBoundary
>*
flt_road_boundaries_ptr
)
{
std
::
vector
<
base
::
RoadBoundary
>*
flt_road_boundaries_ptr
)
{
//
for (size_t n_rd = 0; n_rd < road_boundaries.size(); ++n_rd) {
for
(
size_t
n_rd
=
0
;
n_rd
<
road_boundaries
.
size
();
++
n_rd
)
{
//
const base::RoadBoundary& temp_road_boundary = road_boundaries[n_rd];
const
base
::
RoadBoundary
&
temp_road_boundary
=
road_boundaries
[
n_rd
];
//
std::vector<base::PolygonDType> temp_left_boundary_vec;
std
::
vector
<
base
::
PolygonDType
>
temp_left_boundary_vec
;
//
std::vector<base::PolygonDType> temp_right_boundary_vec;
std
::
vector
<
base
::
PolygonDType
>
temp_right_boundary_vec
;
//
// filter left boundary points
// filter left boundary points
//
this->SplitBoundary(temp_road_boundary.left_boundary,
this
->
SplitBoundary
(
temp_road_boundary
.
left_boundary
,
//
junctions, &temp_left_boundary_vec);
junctions
,
&
temp_left_boundary_vec
);
//
// filter right boundary points
// filter right boundary points
//
this->SplitBoundary(temp_road_boundary.right_boundary,
this
->
SplitBoundary
(
temp_road_boundary
.
right_boundary
,
//
junctions, &temp_right_boundary_vec);
junctions
,
&
temp_right_boundary_vec
);
//
int n_temp_road_boundary = std::max(temp_left_boundary_vec.size(),
int
n_temp_road_boundary
=
std
::
max
(
temp_left_boundary_vec
.
size
(),
//
temp_right_boundary_vec.size());
temp_right_boundary_vec
.
size
());
//
for (int i = 0; i < n_temp_road_boundary; ++i) {
for
(
int
i
=
0
;
i
<
n_temp_road_boundary
;
++
i
)
{
//
base::RoadBoundary temp_road_boundary;
base
::
RoadBoundary
temp_road_boundary
;
//
if (i < temp_left_boundary_vec.size()) {
if
(
i
<
temp_left_boundary_vec
.
size
())
{
//
temp_road_boundary.left_boundary = temp_left_boundary_vec[i];
temp_road_boundary
.
left_boundary
=
temp_left_boundary_vec
[
i
];
//
}
}
//
if (i < temp_right_boundary_vec.size()) {
if
(
i
<
temp_right_boundary_vec
.
size
())
{
//
temp_road_boundary.right_boundary = temp_right_boundary_vec[i];
temp_road_boundary
.
right_boundary
=
temp_right_boundary_vec
[
i
];
//
}
}
//
flt_road_boundaries_ptr->push_back(temp_road_boundary);
flt_road_boundaries_ptr
->
push_back
(
temp_road_boundary
);
//
}
}
//
}
}
return
true
;
return
true
;
}
}
...
@@ -267,168 +268,168 @@ void HDMapInput::DownsamplePoints(
...
@@ -267,168 +268,168 @@ void HDMapInput::DownsamplePoints(
const
std
::
shared_ptr
<
base
::
PointCloud
<
base
::
PointD
>>&
raw_cloud_ptr
,
const
std
::
shared_ptr
<
base
::
PointCloud
<
base
::
PointD
>>&
raw_cloud_ptr
,
base
::
PointCloud
<
base
::
PointD
>*
polygon_ptr
,
base
::
PointCloud
<
base
::
PointD
>*
polygon_ptr
,
int
min_points_num_for_sample
)
const
{
int
min_points_num_for_sample
)
const
{
//
const PointDCloud& raw_cloud = *raw_cloud_ptr;
const
PointDCloud
&
raw_cloud
=
*
raw_cloud_ptr
;
//
unsigned int spt = 0;
unsigned
int
spt
=
0
;
//
double acos_theta = 0.0;
double
acos_theta
=
0.0
;
//
const double radian_to_degree = 57.29577951308232;
const
double
radian_to_degree
=
57.29577951308232
;
//
const size_t raw_cloud_size = raw_cloud.size();
const
size_t
raw_cloud_size
=
raw_cloud
.
size
();
//
if (raw_cloud_size <= min_points_num_for_sample) {
if
(
raw_cloud_size
<=
min_points_num_for_sample
)
{
//
for (size_t i = 0; i < raw_cloud_size; i++) {
for
(
size_t
i
=
0
;
i
<
raw_cloud_size
;
i
++
)
{
//
polygon_ptr->push_back(raw_cloud[i]);
polygon_ptr
->
push_back
(
raw_cloud
[
i
]);
//
}
}
//
return;
return
;
//
}
}
//
//
the first point
// the first point
//
polygon_ptr->push_back(raw_cloud[0]);
polygon_ptr
->
push_back
(
raw_cloud
[
0
]);
//
for (size_t idx = 2; idx < raw_cloud_size; ++idx) {
for
(
size_t
idx
=
2
;
idx
<
raw_cloud_size
;
++
idx
)
{
//
const PointD& point_0 = raw_cloud[spt];
const
PointD
&
point_0
=
raw_cloud
[
spt
];
//
const PointD& point_1 = raw_cloud[idx - 1];
const
PointD
&
point_1
=
raw_cloud
[
idx
-
1
];
//
const PointD& point_2 = raw_cloud[idx];
const
PointD
&
point_2
=
raw_cloud
[
idx
];
//
Eigen::Vector2d v1(point_1.x - point_0.x, point_1.y - point_0.y);
Eigen
::
Vector2d
v1
(
point_1
.
x
-
point_0
.
x
,
point_1
.
y
-
point_0
.
y
);
//
Eigen::Vector2d v2(point_2.x - point_1.x, point_2.y - point_1.y);
Eigen
::
Vector2d
v2
(
point_2
.
x
-
point_1
.
x
,
point_2
.
y
-
point_1
.
y
);
//
double vector_dist = sqrt(v1.cwiseProduct(v1).sum())
double
vector_dist
=
sqrt
(
v1
.
cwiseProduct
(
v1
).
sum
())
//
* sqrt(v2.cwiseProduct(v2).sum());
*
sqrt
(
v2
.
cwiseProduct
(
v2
).
sum
());
//
// judge duplicate points
// judge duplicate points
//
if (vector_dist < DBL_EPSILON) {
if
(
vector_dist
<
DBL_EPSILON
)
{
//
continue;
continue
;
//
}
}
//
double cos_theta = (v1.cwiseProduct(v2)).sum() / vector_dist;
double
cos_theta
=
(
v1
.
cwiseProduct
(
v2
)).
sum
()
/
vector_dist
;
//
if (cos_theta > 1.0) {
if
(
cos_theta
>
1.0
)
{
//
cos_theta = 1.0;
cos_theta
=
1.0
;
//
} else if (cos_theta < -1.0) {
}
else
if
(
cos_theta
<
-
1.0
)
{
//
cos_theta = -1.0;
cos_theta
=
-
1.0
;
//
}
}
//
double angle = (acos(cos_theta) * radian_to_degree);
double
angle
=
(
acos
(
cos_theta
)
*
radian_to_degree
);
//
acos_theta += angle;
acos_theta
+=
angle
;
//
if ((acos_theta - 1.0) > DBL_EPSILON) {
if
((
acos_theta
-
1.0
)
>
DBL_EPSILON
)
{
//
polygon_ptr->push_back(point_1);
polygon_ptr
->
push_back
(
point_1
);
//
spt = idx - 1;
spt
=
idx
-
1
;
//
acos_theta = 0.0;
acos_theta
=
0.0
;
//
}
}
//
}
}
//
//
the last point
// the last point
//
polygon_ptr->push_back(raw_cloud[raw_cloud_size - 1]);
polygon_ptr
->
push_back
(
raw_cloud
[
raw_cloud_size
-
1
]);
//
AINFO << "Downsample road boundary points from "
AINFO
<<
"Downsample road boundary points from "
//
<< raw_cloud_size << " to " << polygon_ptr->size();
<<
raw_cloud_size
<<
" to "
<<
polygon_ptr
->
size
();
}
}
void
HDMapInput
::
SplitBoundary
(
void
HDMapInput
::
SplitBoundary
(
const
base
::
PointCloud
<
base
::
PointD
>&
boundary_line
,
const
base
::
PointCloud
<
base
::
PointD
>&
boundary_line
,
const
std
::
vector
<
base
::
PointCloud
<
base
::
PointD
>>&
junctions
,
const
std
::
vector
<
base
::
PointCloud
<
base
::
PointD
>>&
junctions
,
std
::
vector
<
base
::
PointCloud
<
base
::
PointD
>>*
boundary_line_vec_ptr
)
{
std
::
vector
<
base
::
PointCloud
<
base
::
PointD
>>*
boundary_line_vec_ptr
)
{
//
std::vector<bool> boundary_flag(boundary_line.size());
std
::
vector
<
bool
>
boundary_flag
(
boundary_line
.
size
());
//
for (size_t npt = 0; npt < boundary_line.size(); ++npt) {
for
(
size_t
npt
=
0
;
npt
<
boundary_line
.
size
();
++
npt
)
{
//
const PointD& pointd = boundary_line[npt];
const
PointD
&
pointd
=
boundary_line
[
npt
];
//
boundary_flag[npt] = false;
boundary_flag
[
npt
]
=
false
;
//
for (size_t n_rj = 0; n_rj < junctions.size(); ++n_rj) {
for
(
size_t
n_rj
=
0
;
n_rj
<
junctions
.
size
();
++
n_rj
)
{
//
if (common::IsPointXYInPolygon2DXY(pointd, junctions[n_rj])) {
if
(
common
::
IsPointXYInPolygon2DXY
(
pointd
,
junctions
[
n_rj
]))
{
//
boundary_flag[npt] = true;
boundary_flag
[
npt
]
=
true
;
//
break;
break
;
//
}
}
//
}
}
//
}
}
//
std::vector<int> line_index;
std
::
vector
<
int
>
line_index
;
//
base::PolygonDType temp_line;
base
::
PolygonDType
temp_line
;
//
for (size_t i = 1; i < boundary_flag.size(); ++i) {
for
(
size_t
i
=
1
;
i
<
boundary_flag
.
size
();
++
i
)
{
//
if (!boundary_flag[i - 1] || !boundary_flag[i]) {
if
(
!
boundary_flag
[
i
-
1
]
||
!
boundary_flag
[
i
])
{
//
line_index.push_back(i - 1);
line_index
.
push_back
(
i
-
1
);
//
line_index.push_back(i);
line_index
.
push_back
(
i
);
//
} else if (line_index.size() > 1) {
}
else
if
(
line_index
.
size
()
>
1
)
{
//
std::vector<int>::iterator pos;
std
::
vector
<
int
>::
iterator
pos
;
//
pos = std::unique(line_index.begin(), line_index.end());
pos
=
std
::
unique
(
line_index
.
begin
(),
line_index
.
end
());
//
line_index.erase(pos, line_index.end());
line_index
.
erase
(
pos
,
line_index
.
end
());
//
for (size_t j = 0; j < line_index.size(); ++j) {
for
(
size_t
j
=
0
;
j
<
line_index
.
size
();
++
j
)
{
//
const PointD& pointd = boundary_line[line_index[j]];
const
PointD
&
pointd
=
boundary_line
[
line_index
[
j
]];
//
temp_line.push_back(pointd);
temp_line
.
push_back
(
pointd
);
//
}
}
//
boundary_line_vec_ptr->push_back(temp_line);
boundary_line_vec_ptr
->
push_back
(
temp_line
);
//
line_index.clear();
line_index
.
clear
();
//
temp_line.clear();
temp_line
.
clear
();
//
}
}
//
}
}
//
//
in case the last several unjunctioned points in the "boundary_line"
// in case the last several unjunctioned points in the "boundary_line"
//
if (line_index.size() > 1) {
if
(
line_index
.
size
()
>
1
)
{
//
std::vector<int>::iterator pos;
std
::
vector
<
int
>::
iterator
pos
;
//
pos = std::unique(line_index.begin(), line_index.end());
pos
=
std
::
unique
(
line_index
.
begin
(),
line_index
.
end
());
//
line_index.erase(pos, line_index.end());
line_index
.
erase
(
pos
,
line_index
.
end
());
//
for (size_t j = 0; j < line_index.size(); ++j) {
for
(
size_t
j
=
0
;
j
<
line_index
.
size
();
++
j
)
{
//
const PointD& pointd = boundary_line[line_index[j]];
const
PointD
&
pointd
=
boundary_line
[
line_index
[
j
]];
//
temp_line.push_back(pointd);
temp_line
.
push_back
(
pointd
);
//
}
}
//
boundary_line_vec_ptr->push_back(temp_line);
boundary_line_vec_ptr
->
push_back
(
temp_line
);
//
}
}
}
}
bool
HDMapInput
::
GetNearestLaneDirection
(
const
base
::
PointD
&
pointd
,
bool
HDMapInput
::
GetNearestLaneDirection
(
const
base
::
PointD
&
pointd
,
Eigen
::
Vector3d
*
lane_direction
)
{
Eigen
::
Vector3d
*
lane_direction
)
{
//
if (hdmap_ == nullptr) {
if
(
hdmap_
==
nullptr
)
{
//
return false;
return
false
;
//
}
}
// adu::common::hdmap::Point
point;
apollo
::
common
::
PointENU
point
;
//
point.set_x(pointd.x);
point
.
set_x
(
pointd
.
x
);
//
point.set_y(pointd.y);
point
.
set_y
(
pointd
.
y
);
//
point.set_z(pointd.z);
point
.
set_z
(
pointd
.
z
);
// adu
::hdmap::LaneInfoConstPtr nearest_lane;
apollo
::
hdmap
::
LaneInfoConstPtr
nearest_lane
;
//
double nearest_s = 0.0;
double
nearest_s
=
0.0
;
//
double nearest_l = 0.0;
double
nearest_l
=
0.0
;
//
//
get nearest lane of query point
// get nearest lane of query point
// int status = hdmap_->get_nearest_l
ane(point, &nearest_lane,
int
status
=
hdmap_
->
GetNearestL
ane
(
point
,
&
nearest_lane
,
//
&nearest_s, &nearest_l);
&
nearest_s
,
&
nearest_l
);
//
if (status != 0) {
if
(
status
!=
0
)
{
//
AINFO << "Failed to get nearest lane for point " <<
AINFO
<<
"Failed to get nearest lane for point "
<<
//
point.DebugString();
point
.
DebugString
();
//
return false;
return
false
;
//
}
}
//
//
get lane heading of nearest s
// get lane heading of nearest s
//
const adu::hdmap::Trajectory& nearest_lane_trajectory
const
adu
::
hdmap
::
Trajectory
&
nearest_lane_trajectory
//
= nearest_lane->trajectory();
=
nearest_lane
->
trajectory
();
//
double lane_heading
double
lane_heading
//
= nearest_lane_trajectory.get_smooth_point(nearest_s).heading();
=
nearest_lane_trajectory
.
get_smooth_point
(
nearest_s
).
heading
();
//
*lane_direction = Eigen::Vector3d(cos(lane_heading), sin(lane_heading), 0);
*
lane_direction
=
Eigen
::
Vector3d
(
cos
(
lane_heading
),
sin
(
lane_heading
),
0
);
return
true
;
return
true
;
}
}
//
bool HDMapInput::GetSignalsFromHDMap(const Eigen::Vector3d& pointd,
bool
HDMapInput
::
GetSignalsFromHDMap
(
const
Eigen
::
Vector3d
&
pointd
,
//
double forward_distance,
double
forward_distance
,
// std::vector<adu::common
::hdmap::Signal>* signals) {
std
::
vector
<
apollo
::
hdmap
::
Signal
>*
signals
)
{
// adu::common::hdmap::Point
point;
apollo
::
common
::
PointENU
point
;
//
point.set_x(pointd(0));
point
.
set_x
(
pointd
(
0
));
//
point.set_y(pointd(1));
point
.
set_y
(
pointd
(
1
));
//
point.set_z(pointd(2));
point
.
set_z
(
pointd
(
2
));
//
std::vector<SignalInfoConstPtr> forward_signals;
std
::
vector
<
SignalInfoConstPtr
>
forward_signals
;
//
// int result = _hdmap->get_forward_signal(
// int result = _hdmap->get_forward_signal(
//
// point,
// point,
//
// forward_distance,
// forward_distance,
//
// &forward_signals);
// &forward_signals);
//
// int result = _hdmap->get_forward_nearest_signals_on_road(
// int result = _hdmap->get_forward_nearest_signals_on_road(
//
// point,
// point,
//
// forward_distance,
// forward_distance,
//
// &forward_signals);
// &forward_signals);
//
int result = hdmap_->get_forward_nearest_signals_on_lane(
int
result
=
hdmap_
->
get_forward_nearest_signals_on_lane
(
//
point,
point
,
//
forward_distance,
forward_distance
,
//
&forward_signals);
&
forward_signals
);
//
if (result != 0) {
if
(
result
!=
0
)
{
//
AERROR << "Failed to call HDMap::get_signal. point: "
AERROR
<<
"Failed to call HDMap::get_signal. point: "
//
<< point.ShortDebugString();
<<
point
.
ShortDebugString
();
//
return false;
return
false
;
//
}
}
//
signals->reserve(forward_signals.size());
signals
->
reserve
(
forward_signals
.
size
());
//
for (auto& signal_info : forward_signals) {
for
(
auto
&
signal_info
:
forward_signals
)
{
//
signals->push_back(signal_info->signal());
signals
->
push_back
(
signal_info
->
signal
());
//
ADEBUG << "Signal: " << signals->back().DebugString();
ADEBUG
<<
"Signal: "
<<
signals
->
back
().
DebugString
();
//
}
}
//
ADEBUG << "get_signal success. num_signals: " << signals->size()
ADEBUG
<<
"get_signal success. num_signals: "
<<
signals
->
size
()
//
<< " point: " << point.ShortDebugString();
<<
" point: "
<<
point
.
ShortDebugString
();
//
return true;
return
true
;
//
}
}
//
bool HDMapInput::GetSignals(const Eigen::Vector3d& pointd,
bool
HDMapInput
::
GetSignals
(
const
Eigen
::
Vector3d
&
pointd
,
//
double forward_distance,
double
forward_distance
,
// std::vector<adu::common
::hdmap::Signal>* signals) {
std
::
vector
<
apollo
::
hdmap
::
Signal
>*
signals
)
{
//
lib::MutexLock lock(&mutex_);
lib
::
MutexLock
lock
(
&
mutex_
);
//
CHECK_NOTNULL(hdmap_.get());
CHECK_NOTNULL
(
hdmap_
.
get
());
//
return GetSignalsFromHDMap(pointd, forward_distance, signals);
return
GetSignalsFromHDMap
(
pointd
,
forward_distance
,
signals
);
//
}
}
// add end
// add end
}
// namespace map
}
// namespace map
...
...
modules/perception/map/hdmap/hdmap_input.h
浏览文件 @
f68010fa
...
@@ -19,10 +19,11 @@
...
@@ -19,10 +19,11 @@
#include <gflags/gflags.h>
#include <gflags/gflags.h>
#include <memory>
#include <memory>
// #include <hdmap.h>
#include <string>
#include <string>
#include <vector>
#include <vector>
#include "modules/perception/base/hdmap_struct.h"
#include "modules/perception/base/hdmap_struct.h"
#include "modules/map/hdmap/hdmap.h"
#include "modules/map/hdmap/hdmap_common.h"
// #include "modules/perception/base/point_cloud_types.h"
// #include "modules/perception/base/point_cloud_types.h"
#include "modules/perception/lib/singleton/singleton.h"
#include "modules/perception/lib/singleton/singleton.h"
// #include "modules/perception/lib/thread/mutex.h"
// #include "modules/perception/lib/thread/mutex.h"
...
@@ -50,12 +51,12 @@ class HDMapInput {
...
@@ -50,12 +51,12 @@ class HDMapInput {
HDMapInput
&
operator
=
(
const
HDMapInput
&
)
=
delete
;
HDMapInput
&
operator
=
(
const
HDMapInput
&
)
=
delete
;
bool
InitHDMap
();
bool
InitHDMap
();
bool
InitInternal
();
bool
InitInternal
();
//
void MergeBoundaryJunction(
void
MergeBoundaryJunction
(
// const std::vector<adu::hdmap::RoiRoad
BoundaryPtr>& boundary,
const
std
::
vector
<
apollo
::
hdmap
::
RoadROI
BoundaryPtr
>&
boundary
,
// const std::vector<adu::hdmap::JunctionInfoConst
Ptr>& junctions,
const
std
::
vector
<
apollo
::
hdmap
::
JunctionBoundary
Ptr
>&
junctions
,
//
std::vector<base::RoadBoundary>* road_boundaries_ptr,
std
::
vector
<
base
::
RoadBoundary
>*
road_boundaries_ptr
,
//
std::vector<base::PolygonDType>* road_polygons_ptr,
std
::
vector
<
base
::
PolygonDType
>*
road_polygons_ptr
,
//
std::vector<base::PolygonDType>* junction_polygons_ptr);
std
::
vector
<
base
::
PolygonDType
>*
junction_polygons_ptr
);
bool
GetRoadBoundaryFilteredByJunctions
(
bool
GetRoadBoundaryFilteredByJunctions
(
const
std
::
vector
<
base
::
RoadBoundary
>&
road_boundaries
,
const
std
::
vector
<
base
::
RoadBoundary
>&
road_boundaries
,
...
@@ -71,14 +72,14 @@ class HDMapInput {
...
@@ -71,14 +72,14 @@ class HDMapInput {
const
base
::
PointCloud
<
base
::
PointD
>&
boundary_line
,
const
base
::
PointCloud
<
base
::
PointD
>&
boundary_line
,
const
std
::
vector
<
base
::
PointCloud
<
base
::
PointD
>>&
junctions
,
const
std
::
vector
<
base
::
PointCloud
<
base
::
PointD
>>&
junctions
,
std
::
vector
<
base
::
PointCloud
<
base
::
PointD
>>*
boundary_line_vec_ptr
);
std
::
vector
<
base
::
PointCloud
<
base
::
PointD
>>*
boundary_line_vec_ptr
);
//
bool GetSignalsFromHDMap(const Eigen::Vector3d& pointd,
bool
GetSignalsFromHDMap
(
const
Eigen
::
Vector3d
&
pointd
,
//
double forward_distance,
double
forward_distance
,
// std::vector<adu::common
::hdmap::Signal>* signals);
std
::
vector
<
apollo
::
hdmap
::
Signal
>*
signals
);
bool
inited_
=
false
;
bool
inited_
=
false
;
//
lib::Mutex mutex_;
lib
::
Mutex
mutex_
;
// std::unique_ptr<adu
::hdmap::HDMap> hdmap_;
std
::
unique_ptr
<
apollo
::
hdmap
::
HDMap
>
hdmap_
;
//
int hdmap_sample_step_ = 5;
int
hdmap_sample_step_
=
5
;
//
std::string hdmap_file_;
std
::
string
hdmap_file_
;
friend
class
lib
::
Singleton
<
HDMapInput
>
;
friend
class
lib
::
Singleton
<
HDMapInput
>
;
};
};
...
...
编辑
预览
Markdown
is supported
0%
请重试
或
添加新附件
.
添加附件
取消
You are about to add
0
people
to the discussion. Proceed with caution.
先完成此消息的编辑!
取消
想要评论请
注册
或
登录