Skip to content
体验新版
项目
组织
正在加载...
登录
切换导航
打开侧边栏
Pinoxchio
apollo
提交
2c29bbce
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,发现更多精彩内容 >>
提交
2c29bbce
编写于
6月 28, 2019
作者:
T
tthhee
提交者:
Calvin Miao
7月 09, 2019
浏览文件
操作
浏览文件
下载
电子邮件补丁
差异文件
Localization: add other pyramid map files for pyramid_map module
上级
a619f695
变更
8
隐藏空白更改
内联
并排
Showing
8 changed file
with
1441 addition
and
0 deletion
+1441
-0
modules/localization/msf/local_pyramid_map/pyramid_map/pyramid_map.cc
...lization/msf/local_pyramid_map/pyramid_map/pyramid_map.cc
+95
-0
modules/localization/msf/local_pyramid_map/pyramid_map/pyramid_map.h
...alization/msf/local_pyramid_map/pyramid_map/pyramid_map.h
+72
-0
modules/localization/msf/local_pyramid_map/pyramid_map/pyramid_map_node.cc
...ion/msf/local_pyramid_map/pyramid_map/pyramid_map_node.cc
+484
-0
modules/localization/msf/local_pyramid_map/pyramid_map/pyramid_map_node.h
...tion/msf/local_pyramid_map/pyramid_map/pyramid_map_node.h
+142
-0
modules/localization/msf/local_pyramid_map/pyramid_map/pyramid_map_node_config.cc
.../local_pyramid_map/pyramid_map/pyramid_map_node_config.cc
+40
-0
modules/localization/msf/local_pyramid_map/pyramid_map/pyramid_map_node_config.h
...f/local_pyramid_map/pyramid_map/pyramid_map_node_config.h
+34
-0
modules/localization/msf/local_pyramid_map/pyramid_map/pyramid_map_node_test.cc
...sf/local_pyramid_map/pyramid_map/pyramid_map_node_test.cc
+345
-0
modules/localization/msf/local_pyramid_map/pyramid_map/pyramid_map_test.cc
...ion/msf/local_pyramid_map/pyramid_map/pyramid_map_test.cc
+229
-0
未找到文件。
modules/localization/msf/local_pyramid_map/pyramid_map/pyramid_map.cc
0 → 100644
浏览文件 @
2c29bbce
/******************************************************************************
* Copyright 2019 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 "modules/localization/msf/local_map/pyramid_map/pyramid_map.h"
#include "modules/localization/msf/local_map/pyramid_map/pyramid_map_node.h"
namespace
apollo
{
namespace
localization
{
namespace
msf
{
PyramidMap
::
PyramidMap
(
PyramidMapConfig
*
config
)
:
BaseMap
(
config
)
{}
PyramidMap
::~
PyramidMap
()
{}
float
PyramidMap
::
GetIntensitySafe
(
const
Eigen
::
Vector3d
&
coordinate
,
int
zone_id
,
unsigned
int
resolution_id
,
unsigned
int
level
)
{
MapNodeIndex
index
=
MapNodeIndex
::
GetMapNodeIndex
(
GetMapConfig
(),
coordinate
,
resolution_id
,
zone_id
);
PyramidMapNode
*
node
=
dynamic_cast
<
PyramidMapNode
*>
(
GetMapNodeSafe
(
index
));
return
node
->
GetIntensitySafe
(
coordinate
,
level
);
}
float
PyramidMap
::
GetIntensityVarSafe
(
const
Eigen
::
Vector3d
&
coordinate
,
int
zone_id
,
unsigned
int
resolution_id
,
unsigned
int
level
)
{
MapNodeIndex
index
=
MapNodeIndex
::
GetMapNodeIndex
(
GetMapConfig
(),
coordinate
,
resolution_id
,
zone_id
);
PyramidMapNode
*
node
=
dynamic_cast
<
PyramidMapNode
*>
(
GetMapNodeSafe
(
index
));
return
node
->
GetIntensityVarSafe
(
coordinate
,
level
);
}
float
PyramidMap
::
GetAltitudeSafe
(
const
Eigen
::
Vector3d
&
coordinate
,
int
zone_id
,
unsigned
int
resolution_id
,
unsigned
int
level
)
{
MapNodeIndex
index
=
MapNodeIndex
::
GetMapNodeIndex
(
GetMapConfig
(),
coordinate
,
resolution_id
,
zone_id
);
PyramidMapNode
*
node
=
dynamic_cast
<
PyramidMapNode
*>
(
GetMapNodeSafe
(
index
));
return
node
->
GetAltitudeSafe
(
coordinate
,
level
);
}
float
PyramidMap
::
GetAltitudeVarSafe
(
const
Eigen
::
Vector3d
&
coordinate
,
int
zone_id
,
unsigned
int
resolution_id
,
unsigned
int
level
)
{
MapNodeIndex
index
=
MapNodeIndex
::
GetMapNodeIndex
(
GetMapConfig
(),
coordinate
,
resolution_id
,
zone_id
);
PyramidMapNode
*
node
=
dynamic_cast
<
PyramidMapNode
*>
(
GetMapNodeSafe
(
index
));
return
node
->
GetAltitudeVarSafe
(
coordinate
,
level
);
}
float
PyramidMap
::
GetGroundAltitudeSafe
(
const
Eigen
::
Vector3d
&
coordinate
,
int
zone_id
,
unsigned
int
resolution_id
,
unsigned
int
level
)
{
MapNodeIndex
index
=
MapNodeIndex
::
GetMapNodeIndex
(
GetMapConfig
(),
coordinate
,
resolution_id
,
zone_id
);
PyramidMapNode
*
node
=
dynamic_cast
<
PyramidMapNode
*>
(
GetMapNodeSafe
(
index
));
return
node
->
GetGroundAltitudeSafe
(
coordinate
,
level
);
}
unsigned
int
PyramidMap
::
GetCountSafe
(
const
Eigen
::
Vector3d
&
coordinate
,
int
zone_id
,
unsigned
int
resolution_id
,
unsigned
int
level
)
{
MapNodeIndex
index
=
MapNodeIndex
::
GetMapNodeIndex
(
GetMapConfig
(),
coordinate
,
resolution_id
,
zone_id
);
PyramidMapNode
*
node
=
dynamic_cast
<
PyramidMapNode
*>
(
GetMapNodeSafe
(
index
));
return
node
->
GetCountSafe
(
coordinate
,
level
);
}
unsigned
int
PyramidMap
::
GetGroundCountSafe
(
const
Eigen
::
Vector3d
&
coordinate
,
int
zone_id
,
unsigned
int
resolution_id
,
unsigned
int
level
)
{
MapNodeIndex
index
=
MapNodeIndex
::
GetMapNodeIndex
(
GetMapConfig
(),
coordinate
,
resolution_id
,
zone_id
);
PyramidMapNode
*
node
=
dynamic_cast
<
PyramidMapNode
*>
(
GetMapNodeSafe
(
index
));
return
node
->
GetGroundCountSafe
(
coordinate
,
level
);
}
}
// namespace msf
}
// namespace localization
}
// namespace apollo
modules/localization/msf/local_pyramid_map/pyramid_map/pyramid_map.h
0 → 100644
浏览文件 @
2c29bbce
/******************************************************************************
* Copyright 2019 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.
*****************************************************************************/
#pragma once
#include "modules/localization/msf/local_map/base_map/base_map.h"
#include "modules/localization/msf/local_map/pyramid_map/pyramid_map_config.h"
namespace
apollo
{
namespace
localization
{
namespace
msf
{
class
PyramidMap
:
public
BaseMap
{
public:
explicit
PyramidMap
(
PyramidMapConfig
*
config
);
~
PyramidMap
();
public:
/**@brief Given the 3D global coordinate, this function loads the
* corresponding map node in the cache thread safely and return intensity, if
*necessary.
**/
float
GetIntensitySafe
(
const
Eigen
::
Vector3d
&
coordinate
,
int
zone_id
,
unsigned
int
resolution_id
,
unsigned
int
level
=
0
);
/**@brief Given the 3D global coordinate, this function loads the
* corresponding map node in the cache and return intensity var, if necessary.
**/
float
GetIntensityVarSafe
(
const
Vector3d
&
coordinate
,
int
zone_id
,
unsigned
int
resolution_id
,
unsigned
int
level
=
0
);
/**@brief Given the 3D global coordinate, this function loads the
* corresponding map node in the cache thread safely and return altitude, if
*necessary.
**/
float
GetAltitudeSafe
(
const
Eigen
::
Vector3d
&
coordinate
,
int
zone_id
,
unsigned
int
resolution_id
,
unsigned
int
level
=
0
);
/**@brief Given the 3D global coordinate, this function loads the
* corresponding map node in the cache thread safely and return altitude var,
*if necessary.
**/
float
GetAltitudeVarSafe
(
const
Eigen
::
Vector3d
&
coordinate
,
int
zone_id
,
unsigned
int
resolution_id
,
unsigned
int
level
=
0
);
/**@brief Given the 3D global coordinate, this function loads the
* corresponding map node in the cache thread safely and return ground
*altitude, if necessary.
**/
float
GetGroundAltitudeSafe
(
const
Eigen
::
Vector3d
&
coordinate
,
int
zone_id
,
unsigned
int
resolution_id
,
unsigned
int
level
=
0
);
/**@brief Get the number of samples in the map cell thread safely. */
unsigned
int
GetCountSafe
(
const
Eigen
::
Vector3d
&
coordinate
,
int
zone_id
,
unsigned
int
resolution_id
,
unsigned
int
level
=
0
);
/**@brief Get the number of ground samples in the map cell thread safely. */
unsigned
int
GetGroundCountSafe
(
const
Eigen
::
Vector3d
&
coordinate
,
int
zone_id
,
unsigned
int
resolution_id
,
unsigned
int
level
=
0
);
};
}
// namespace msf
}
// namespace localization
}
// namespace apollo
modules/localization/msf/local_pyramid_map/pyramid_map/pyramid_map_node.cc
0 → 100644
浏览文件 @
2c29bbce
/******************************************************************************
* Copyright 2019 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 "modules/localization/msf/local_map/pyramid_map/pyramid_map_node.h"
#include <vector>
#include "modules/localization/msf/local_map/pyramid_map/pyramid_map_matrix.h"
#include "modules/localization/msf/local_map/pyramid_map/pyramid_map_matrix_handler.h"
#include "modules/localization/msf/local_map/pyramid_map/pyramid_map_node_config.h"
namespace
apollo
{
namespace
localization
{
namespace
msf
{
PyramidMapNode
::
PyramidMapNode
()
{}
PyramidMapNode
::~
PyramidMapNode
()
{}
void
PyramidMapNode
::
Init
(
const
BaseMapConfig
*
map_config
)
{
map_config_
=
map_config
;
map_node_config_
=
new
PyramidMapNodeConfig
();
map_node_config_
->
map_version_
=
map_config_
->
GetMapVersion
();
if
(
map_node_config_
->
map_version_
==
MapVersion
::
LOSSY_FULL_ALT_MAP
||
map_node_config_
->
map_version_
==
MapVersion
::
LOSSLESS_MAP
)
{
map_node_config_
->
has_map_version_
=
false
;
map_node_config_
->
has_body_md5_
=
false
;
}
else
{
map_node_config_
->
has_map_version_
=
true
;
map_node_config_
->
has_body_md5_
=
true
;
}
is_reserved_
=
false
;
data_is_ready_
=
false
;
is_changed_
=
false
;
map_matrix_
=
new
PyramidMapMatrix
();
map_matrix_handler_
=
PyramidMapMatrixHandlerSelector
::
AllocPyramidMapMatrixHandler
(
map_node_config_
->
map_version_
);
compression_strategy_
=
new
ZlibStrategy
();
const
PyramidMapConfig
*
pm_map_config
=
dynamic_cast
<
const
PyramidMapConfig
*>
(
map_config_
);
resolutions_mr_
.
resize
(
pm_map_config
->
resolution_num_
,
0
);
for
(
unsigned
int
i
=
0
;
i
<
resolutions_mr_
.
size
();
++
i
)
{
resolutions_mr_
[
i
]
=
static_cast
<
float
>
(
pm_map_config
->
map_resolutions_
[
0
]
*
std
::
pow
(
pm_map_config
->
resolution_ratio_
,
i
));
}
InitMapMatrix
(
map_config_
);
}
void
PyramidMapNode
::
Init
(
const
BaseMapConfig
*
map_config
,
const
MapNodeIndex
&
index
,
bool
create_map_cells
)
{
map_config_
=
map_config
;
map_node_config_
=
new
PyramidMapNodeConfig
();
map_node_config_
->
node_index_
=
index
;
map_node_config_
->
map_version_
=
map_config_
->
GetMapVersion
();
if
(
map_node_config_
->
map_version_
==
MapVersion
::
LOSSY_FULL_ALT_MAP
||
map_node_config_
->
map_version_
==
MapVersion
::
LOSSLESS_MAP
)
{
map_node_config_
->
has_map_version_
=
false
;
map_node_config_
->
has_body_md5_
=
false
;
}
else
{
map_node_config_
->
has_map_version_
=
true
;
map_node_config_
->
has_body_md5_
=
true
;
}
left_top_corner_
=
ComputeLeftTopCorner
(
*
map_config_
,
map_node_config_
->
node_index_
);
is_reserved_
=
false
;
data_is_ready_
=
false
;
is_changed_
=
false
;
map_matrix_
=
new
PyramidMapMatrix
();
map_matrix_handler_
=
PyramidMapMatrixHandlerSelector
::
AllocPyramidMapMatrixHandler
(
map_node_config_
->
map_version_
);
compression_strategy_
=
new
ZlibStrategy
();
if
(
create_map_cells
)
{
InitMapMatrix
(
map_config_
);
}
}
void
PyramidMapNode
::
BottomUpBase
()
{
PyramidMapMatrix
*
map_matrix
=
dynamic_cast
<
PyramidMapMatrix
*>
(
map_matrix_
);
map_matrix
->
BottomUpBase
();
}
void
PyramidMapNode
::
BottomUpSafe
()
{
PyramidMapMatrix
*
map_matrix
=
dynamic_cast
<
PyramidMapMatrix
*>
(
map_matrix_
);
map_matrix
->
BottomUpSafe
();
}
bool
PyramidMapNode
::
AddValueIfInBound
(
const
Eigen
::
Vector3d
&
coordinate
,
unsigned
char
intensity
,
unsigned
int
level
)
{
Eigen
::
Vector2d
coord2d
(
coordinate
[
0
],
coordinate
[
1
]);
// Eigen::Map<Eigen::Vector2d>(coordinate.data(), 2);
unsigned
int
x
=
0
;
unsigned
int
y
=
0
;
bool
is_success
=
GetCoordinate
(
coord2d
,
level
,
&
x
,
&
y
);
if
(
is_success
)
{
PyramidMapMatrix
*
map_matrix
=
dynamic_cast
<
PyramidMapMatrix
*>
(
map_matrix_
);
map_matrix
->
AddSampleBase
(
static_cast
<
float
>
(
intensity
),
static_cast
<
float
>
(
coordinate
[
2
]),
y
,
x
,
level
);
return
true
;
}
else
{
return
false
;
}
}
void
PyramidMapNode
::
AddValueIfInBound
(
const
std
::
vector
<
Eigen
::
Vector3d
>&
coordinates
,
const
std
::
vector
<
unsigned
char
>&
intensity
,
unsigned
int
level
)
{
if
(
coordinates
.
size
()
!=
intensity
.
size
())
{
return
;
}
for
(
unsigned
int
i
=
0
;
i
<
coordinates
.
size
();
i
++
)
{
AddValueIfInBound
(
coordinates
[
i
],
intensity
[
i
],
level
);
}
}
bool
PyramidMapNode
::
GetCoordinate
(
const
Eigen
::
Vector2d
&
coordinate
,
unsigned
int
level
,
unsigned
int
*
x
,
unsigned
int
*
y
)
const
{
const
PyramidMapMatrix
*
map_matrix
=
dynamic_cast
<
PyramidMapMatrix
*>
(
map_matrix_
);
const
float
&
current_resolution
=
resolutions_mr_
[
level
];
unsigned
int
off_x
=
static_cast
<
unsigned
int
>
(
(
coordinate
[
0
]
-
left_top_corner_
[
0
])
/
current_resolution
);
unsigned
int
off_y
=
static_cast
<
unsigned
int
>
(
(
coordinate
[
1
]
-
left_top_corner_
[
1
])
/
current_resolution
);
if
(
off_x
>=
0
&&
off_x
<
map_matrix
->
GetCols
(
level
)
&&
off_y
>=
0
&&
off_y
<
map_matrix
->
GetRows
(
level
))
{
*
x
=
static_cast
<
unsigned
int
>
(
off_x
);
*
y
=
static_cast
<
unsigned
int
>
(
off_y
);
return
true
;
}
else
{
return
false
;
}
}
Eigen
::
Vector2d
PyramidMapNode
::
GetCoordinate
(
unsigned
int
level
,
unsigned
int
x
,
unsigned
int
y
)
const
{
const
PyramidMapConfig
*
map_config
=
dynamic_cast
<
const
PyramidMapConfig
*>
(
map_config_
);
float
current_resolution
=
static_cast
<
float
>
(
map_config
->
map_resolutions_
[
0
]
*
std
::
pow
(
map_config
->
resolution_ratio_
,
level
));
Eigen
::
Vector2d
coord
;
coord
[
0
]
=
left_top_corner_
[
0
]
+
current_resolution
*
static_cast
<
float
>
(
x
);
coord
[
1
]
=
left_top_corner_
[
1
]
+
current_resolution
*
static_cast
<
float
>
(
y
);
return
coord
;
}
bool
PyramidMapNode
::
GetCoordinate
(
const
Eigen
::
Vector2d
&
coordinate
,
unsigned
int
*
x
,
unsigned
int
*
y
)
const
{
return
BaseMapNode
::
GetCoordinate
(
coordinate
,
x
,
y
);
}
bool
PyramidMapNode
::
GetCoordinate
(
const
Eigen
::
Vector3d
&
coordinate
,
unsigned
int
*
x
,
unsigned
int
*
y
)
const
{
return
BaseMapNode
::
GetCoordinate
(
coordinate
,
x
,
y
);
}
Eigen
::
Vector2d
PyramidMapNode
::
GetCoordinate
(
unsigned
int
x
,
unsigned
int
y
)
const
{
return
BaseMapNode
::
GetCoordinate
(
x
,
y
);
}
float
PyramidMapNode
::
GetIntensitySafe
(
const
Eigen
::
Vector3d
&
coordinate
,
unsigned
int
level
)
const
{
Eigen
::
Vector2d
coord2d
(
coordinate
[
0
],
coordinate
[
1
]);
unsigned
int
x
=
0
;
unsigned
int
y
=
0
;
bool
is_success
=
GetCoordinate
(
coord2d
,
level
,
&
x
,
&
y
);
float
intensity
=
0.0
;
if
(
is_success
)
{
PyramidMapMatrix
*
map_matrix
=
dynamic_cast
<
PyramidMapMatrix
*>
(
map_matrix_
);
const
float
*
intensity_ptr
=
map_matrix
->
GetIntensitySafe
(
y
,
x
,
level
);
if
(
intensity_ptr
!=
NULL
)
{
intensity
=
*
intensity_ptr
;
}
}
return
intensity
;
}
float
PyramidMapNode
::
GetIntensityVarSafe
(
const
Eigen
::
Vector3d
&
coordinate
,
unsigned
int
level
)
const
{
Eigen
::
Vector2d
coord2d
(
coordinate
[
0
],
coordinate
[
1
]);
unsigned
int
x
=
0
;
unsigned
int
y
=
0
;
bool
is_success
=
GetCoordinate
(
coord2d
,
level
,
&
x
,
&
y
);
float
intensity_var
=
0.0
;
if
(
is_success
)
{
PyramidMapMatrix
*
map_matrix
=
dynamic_cast
<
PyramidMapMatrix
*>
(
map_matrix_
);
const
float
*
intensity_var_ptr
=
map_matrix
->
GetIntensityVarSafe
(
y
,
x
,
level
);
if
(
intensity_var_ptr
!=
NULL
)
{
intensity_var
=
*
intensity_var_ptr
;
}
}
return
intensity_var
;
}
float
PyramidMapNode
::
GetAltitudeSafe
(
const
Eigen
::
Vector3d
&
coordinate
,
unsigned
int
level
)
const
{
Eigen
::
Vector2d
coord2d
(
coordinate
[
0
],
coordinate
[
1
]);
unsigned
int
x
=
0
;
unsigned
int
y
=
0
;
bool
is_success
=
GetCoordinate
(
coord2d
,
level
,
&
x
,
&
y
);
float
altitude
=
0.0
;
if
(
is_success
)
{
PyramidMapMatrix
*
map_matrix
=
dynamic_cast
<
PyramidMapMatrix
*>
(
map_matrix_
);
const
float
*
altitude_ptr
=
map_matrix
->
GetAltitudeSafe
(
y
,
x
,
level
);
if
(
altitude_ptr
!=
NULL
)
{
altitude
=
*
altitude_ptr
;
}
}
return
altitude
;
}
float
PyramidMapNode
::
GetAltitudeVarSafe
(
const
Eigen
::
Vector3d
&
coordinate
,
unsigned
int
level
)
const
{
Eigen
::
Vector2d
coord2d
(
coordinate
[
0
],
coordinate
[
1
]);
unsigned
int
x
=
0
;
unsigned
int
y
=
0
;
bool
is_success
=
GetCoordinate
(
coord2d
,
level
,
&
x
,
&
y
);
float
altitude_var
=
0.0
;
if
(
is_success
)
{
PyramidMapMatrix
*
map_matrix
=
dynamic_cast
<
PyramidMapMatrix
*>
(
map_matrix_
);
const
float
*
altitude_var_ptr
=
map_matrix
->
GetAltitudeVarSafe
(
y
,
x
,
level
);
if
(
altitude_var_ptr
!=
NULL
)
{
altitude_var
=
*
altitude_var_ptr
;
}
}
return
altitude_var
;
}
float
PyramidMapNode
::
GetGroundAltitudeSafe
(
const
Eigen
::
Vector3d
&
coordinate
,
unsigned
int
level
)
const
{
Eigen
::
Vector2d
coord2d
(
coordinate
[
0
],
coordinate
[
1
]);
unsigned
int
x
=
0
;
unsigned
int
y
=
0
;
bool
is_success
=
GetCoordinate
(
coord2d
,
level
,
&
x
,
&
y
);
float
ground_altitude
=
0.0
;
if
(
is_success
)
{
PyramidMapMatrix
*
map_matrix
=
dynamic_cast
<
PyramidMapMatrix
*>
(
map_matrix_
);
const
float
*
ground_altitude_ptr
=
map_matrix
->
GetGroundAltitudeSafe
(
y
,
x
,
level
);
if
(
ground_altitude_ptr
!=
NULL
)
{
ground_altitude
=
*
ground_altitude_ptr
;
}
}
return
ground_altitude
;
}
unsigned
int
PyramidMapNode
::
GetCountSafe
(
const
Eigen
::
Vector3d
&
coordinate
,
unsigned
int
level
)
const
{
Eigen
::
Vector2d
coord2d
(
coordinate
[
0
],
coordinate
[
1
]);
unsigned
int
x
=
0
;
unsigned
int
y
=
0
;
bool
is_success
=
GetCoordinate
(
coord2d
,
level
,
&
x
,
&
y
);
unsigned
int
count
=
0
;
if
(
is_success
)
{
PyramidMapMatrix
*
map_matrix
=
dynamic_cast
<
PyramidMapMatrix
*>
(
map_matrix_
);
const
unsigned
int
*
count_ptr
=
map_matrix
->
GetCountSafe
(
y
,
x
,
level
);
if
(
count_ptr
!=
NULL
)
{
count
=
*
count_ptr
;
}
}
return
count
;
}
unsigned
int
PyramidMapNode
::
GetGroundCountSafe
(
const
Eigen
::
Vector3d
&
coordinate
,
unsigned
int
level
)
const
{
Eigen
::
Vector2d
coord2d
(
coordinate
[
0
],
coordinate
[
1
]);
unsigned
int
x
=
0
;
unsigned
int
y
=
0
;
bool
is_success
=
GetCoordinate
(
coord2d
,
level
,
&
x
,
&
y
);
unsigned
int
ground_count
=
0
;
if
(
is_success
)
{
PyramidMapMatrix
*
map_matrix
=
dynamic_cast
<
PyramidMapMatrix
*>
(
map_matrix_
);
const
unsigned
int
*
ground_count_ptr
=
map_matrix
->
GetGroundCountSafe
(
y
,
x
,
level
);
if
(
ground_count_ptr
!=
NULL
)
{
ground_count
=
*
ground_count_ptr
;
}
}
return
ground_count
;
}
float
PyramidMapNode
::
GetIntensity
(
const
Eigen
::
Vector3d
&
coordinate
,
unsigned
int
level
)
const
{
Eigen
::
Vector2d
coord2d
(
coordinate
[
0
],
coordinate
[
1
]);
unsigned
int
x
=
0
;
unsigned
int
y
=
0
;
bool
is_success
=
GetCoordinate
(
coord2d
,
level
,
&
x
,
&
y
);
float
intensity
=
0.0
;
if
(
is_success
)
{
PyramidMapMatrix
*
map_matrix
=
dynamic_cast
<
PyramidMapMatrix
*>
(
map_matrix_
);
const
float
*
intensity_ptr
=
map_matrix
->
GetIntensity
(
y
,
x
,
level
);
if
(
intensity_ptr
!=
NULL
)
{
intensity
=
*
intensity_ptr
;
}
}
return
intensity
;
}
float
PyramidMapNode
::
GetIntensityVar
(
const
Eigen
::
Vector3d
&
coordinate
,
unsigned
int
level
)
const
{
Eigen
::
Vector2d
coord2d
(
coordinate
[
0
],
coordinate
[
1
]);
unsigned
int
x
=
0
;
unsigned
int
y
=
0
;
bool
is_success
=
GetCoordinate
(
coord2d
,
level
,
&
x
,
&
y
);
float
intensity_var
=
0.0
;
if
(
is_success
)
{
PyramidMapMatrix
*
map_matrix
=
dynamic_cast
<
PyramidMapMatrix
*>
(
map_matrix_
);
const
float
*
intensity_var_ptr
=
map_matrix
->
GetIntensityVar
(
y
,
x
,
level
);
if
(
intensity_var_ptr
!=
NULL
)
{
intensity_var
=
*
intensity_var_ptr
;
}
}
return
intensity_var
;
}
float
PyramidMapNode
::
GetAltitude
(
const
Eigen
::
Vector3d
&
coordinate
,
unsigned
int
level
)
const
{
Eigen
::
Vector2d
coord2d
(
coordinate
[
0
],
coordinate
[
1
]);
unsigned
int
x
=
0
;
unsigned
int
y
=
0
;
bool
is_success
=
GetCoordinate
(
coord2d
,
level
,
&
x
,
&
y
);
float
altitude
=
0.0
;
if
(
is_success
)
{
PyramidMapMatrix
*
map_matrix
=
dynamic_cast
<
PyramidMapMatrix
*>
(
map_matrix_
);
const
float
*
altitude_ptr
=
map_matrix
->
GetAltitude
(
y
,
x
,
level
);
if
(
altitude_ptr
!=
NULL
)
{
altitude
=
*
altitude_ptr
;
}
}
return
altitude
;
}
float
PyramidMapNode
::
GetAltitudeVar
(
const
Eigen
::
Vector3d
&
coordinate
,
unsigned
int
level
)
const
{
Eigen
::
Vector2d
coord2d
(
coordinate
[
0
],
coordinate
[
1
]);
unsigned
int
x
=
0
;
unsigned
int
y
=
0
;
bool
is_success
=
GetCoordinate
(
coord2d
,
level
,
&
x
,
&
y
);
float
altitude_var
=
0.0
;
if
(
is_success
)
{
PyramidMapMatrix
*
map_matrix
=
dynamic_cast
<
PyramidMapMatrix
*>
(
map_matrix_
);
const
float
*
altitude_var_ptr
=
map_matrix
->
GetAltitudeVar
(
y
,
x
,
level
);
if
(
altitude_var_ptr
!=
NULL
)
{
altitude_var
=
*
altitude_var_ptr
;
}
}
return
altitude_var
;
}
float
PyramidMapNode
::
GetGroundAltitude
(
const
Eigen
::
Vector3d
&
coordinate
,
unsigned
int
level
)
const
{
Eigen
::
Vector2d
coord2d
(
coordinate
[
0
],
coordinate
[
1
]);
unsigned
int
x
=
0
;
unsigned
int
y
=
0
;
bool
is_success
=
GetCoordinate
(
coord2d
,
level
,
&
x
,
&
y
);
float
ground_altitude
=
0.0
;
if
(
is_success
)
{
PyramidMapMatrix
*
map_matrix
=
dynamic_cast
<
PyramidMapMatrix
*>
(
map_matrix_
);
const
float
*
ground_altitude_ptr
=
map_matrix
->
GetGroundAltitude
(
y
,
x
,
level
);
if
(
ground_altitude_ptr
!=
NULL
)
{
ground_altitude
=
*
ground_altitude_ptr
;
}
}
return
ground_altitude
;
}
unsigned
int
PyramidMapNode
::
GetCount
(
const
Eigen
::
Vector3d
&
coordinate
,
unsigned
int
level
)
const
{
Eigen
::
Vector2d
coord2d
(
coordinate
[
0
],
coordinate
[
1
]);
unsigned
int
x
=
0
;
unsigned
int
y
=
0
;
bool
is_success
=
GetCoordinate
(
coord2d
,
level
,
&
x
,
&
y
);
unsigned
int
count
=
0
;
if
(
is_success
)
{
PyramidMapMatrix
*
map_matrix
=
dynamic_cast
<
PyramidMapMatrix
*>
(
map_matrix_
);
const
unsigned
int
*
count_ptr
=
map_matrix
->
GetCount
(
y
,
x
,
level
);
if
(
count_ptr
!=
NULL
)
{
count
=
*
count_ptr
;
}
}
return
count
;
}
unsigned
int
PyramidMapNode
::
GetGroundCount
(
const
Eigen
::
Vector3d
&
coordinate
,
unsigned
int
level
)
const
{
Eigen
::
Vector2d
coord2d
(
coordinate
[
0
],
coordinate
[
1
]);
unsigned
int
x
=
0
;
unsigned
int
y
=
0
;
bool
is_success
=
GetCoordinate
(
coord2d
,
level
,
&
x
,
&
y
);
unsigned
int
ground_count
=
0
;
if
(
is_success
)
{
PyramidMapMatrix
*
map_matrix
=
dynamic_cast
<
PyramidMapMatrix
*>
(
map_matrix_
);
const
unsigned
int
*
ground_count_ptr
=
map_matrix
->
GetGroundCount
(
y
,
x
,
level
);
if
(
ground_count_ptr
!=
NULL
)
{
ground_count
=
*
ground_count_ptr
;
}
}
return
ground_count
;
}
double
PyramidMapNode
::
ComputeMeanIntensity
(
unsigned
int
level
)
{
PyramidMapMatrix
*
map_matrix
=
dynamic_cast
<
PyramidMapMatrix
*>
(
map_matrix_
);
return
map_matrix
->
ComputeMeanIntensity
(
level
);
}
}
// namespace msf
}
// namespace localization
}
// namespace apollo
modules/localization/msf/local_pyramid_map/pyramid_map/pyramid_map_node.h
0 → 100644
浏览文件 @
2c29bbce
/******************************************************************************
* Copyright 2019 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.
*****************************************************************************/
#pragma once
#include <Eigen/Core>
#include <vector>
#include "modules/localization/msf/local_map/base_map/base_map_node.h"
#include "modules/localization/msf/local_map/pyramid_map/pyramid_map_matrix.h"
namespace
apollo
{
namespace
localization
{
namespace
msf
{
class
PyramidMapNode
:
public
BaseMapNode
{
public:
PyramidMapNode
();
~
PyramidMapNode
();
public:
virtual
void
Init
(
const
BaseMapConfig
*
map_config
);
virtual
void
Init
(
const
BaseMapConfig
*
map_config
,
const
MapNodeIndex
&
index
,
bool
create_map_cells
=
true
);
/**@brief Propagate the data to the coarse resolution by check. */
void
BottomUpSafe
();
/**@brief Propagate the data to the coarse resolution.
* only update count, intensity, intensity var and altitude
*/
void
BottomUpBase
();
/**@brief Add the value of a pixel in the map node if the pixel in the node.
* @param <coordinate> The 3D global coordinate.
* @param <intensity> The reflectance intensity.
* @param <return> True, if pixel in the bound of the node, else False.
* */
bool
AddValueIfInBound
(
const
Eigen
::
Vector3d
&
coordinate
,
unsigned
char
intensity
,
unsigned
int
level
=
0
);
/**@brief Add the value of a pixel in the map node if the pixel in the node.
* @param <coordinates> The 3D global coordinates.
* @param <intensities> The reflectance intensities.
* */
void
AddValueIfInBound
(
const
std
::
vector
<
Eigen
::
Vector3d
>&
coordinates
,
const
std
::
vector
<
unsigned
char
>&
intensity
,
unsigned
int
level
=
0
);
/**@brief Given the global coordinate, get the local 2D coordinate of the map
* cell matrix.
* <return> If global coordinate (x, y) belongs to this map node. */
bool
GetCoordinate
(
const
Eigen
::
Vector2d
&
coordinate
,
unsigned
int
level
,
unsigned
int
*
x
,
unsigned
int
*
y
)
const
;
/**@brief Given the local 2D coordinate, return the global coordinate. */
Eigen
::
Vector2d
GetCoordinate
(
unsigned
int
level
,
unsigned
int
x
,
unsigned
int
y
)
const
;
virtual
bool
GetCoordinate
(
const
Eigen
::
Vector2d
&
coordinate
,
unsigned
int
*
x
,
unsigned
int
*
y
)
const
;
virtual
bool
GetCoordinate
(
const
Eigen
::
Vector3d
&
coordinate
,
unsigned
int
*
x
,
unsigned
int
*
y
)
const
;
virtual
Eigen
::
Vector2d
GetCoordinate
(
unsigned
int
x
,
unsigned
int
y
)
const
;
/**@brief Given the 3D global coordinate,
* get the map cell intensity with check. */
float
GetIntensitySafe
(
const
Eigen
::
Vector3d
&
coordinate
,
unsigned
int
level
=
0
)
const
;
/**@brief Given the 3D global coordinate,
* get the map cell variance of the intensity with check. */
float
GetIntensityVarSafe
(
const
Eigen
::
Vector3d
&
coordinate
,
unsigned
int
level
=
0
)
const
;
/**@brief Given the 3D global coordinate,
* get the map cell's average altitude with check. */
float
GetAltitudeSafe
(
const
Eigen
::
Vector3d
&
coordinate
,
unsigned
int
level
=
0
)
const
;
/**@brief Given the 3D global coordinate,
* get the map cell's variance of the altitude with check. */
float
GetAltitudeVarSafe
(
const
Eigen
::
Vector3d
&
coordinate
,
unsigned
int
level
=
0
)
const
;
/**@brief Given the 3D global coordinate,
* get the map cell's average ground altitude with check. */
float
GetGroundAltitudeSafe
(
const
Eigen
::
Vector3d
&
coordinate
,
unsigned
int
level
=
0
)
const
;
/**@brief Given the 3D global coordinate,
* get the map cell's count of the samples with check. */
unsigned
int
GetCountSafe
(
const
Eigen
::
Vector3d
&
coordinate
,
unsigned
int
level
=
0
)
const
;
/**@brief Given the 3D global coordinate,
* get the map cell's count of the ground samples with check. */
unsigned
int
GetGroundCountSafe
(
const
Eigen
::
Vector3d
&
coordinate
,
unsigned
int
level
=
0
)
const
;
/**@brief Given the 3D global coordinate,
* get the map cell intensity without check. */
float
GetIntensity
(
const
Eigen
::
Vector3d
&
coordinate
,
unsigned
int
level
=
0
)
const
;
/**@brief Given the 3D global coordinate,
* get the map cell variance of the intensity without check. */
float
GetIntensityVar
(
const
Eigen
::
Vector3d
&
coordinate
,
unsigned
int
level
=
0
)
const
;
/**@brief Given the 3D global coordinate,
* get the map cell's average altitude without check. */
float
GetAltitude
(
const
Eigen
::
Vector3d
&
coordinate
,
unsigned
int
level
=
0
)
const
;
/**@brief Given the 3D global coordinate,
* get the map cell's variance of the altitude without check. */
float
GetAltitudeVar
(
const
Eigen
::
Vector3d
&
coordinate
,
unsigned
int
level
=
0
)
const
;
/**@brief Given the 3D global coordinate,
* get the map cell's average ground altitude without check. */
float
GetGroundAltitude
(
const
Eigen
::
Vector3d
&
coordinate
,
unsigned
int
level
=
0
)
const
;
/**@brief Given the 3D global coordinate,
* get the map cell's count of the samples without check. */
unsigned
int
GetCount
(
const
Eigen
::
Vector3d
&
coordinate
,
unsigned
int
level
=
0
)
const
;
/**@brief Given the 3D global coordinate,
* get the map cell's count of the ground samples without check. */
unsigned
int
GetGroundCount
(
const
Eigen
::
Vector3d
&
coordinate
,
unsigned
int
level
=
0
)
const
;
/**@brief Compute mean intensity. */
double
ComputeMeanIntensity
(
unsigned
int
level
=
0
);
private:
std
::
vector
<
float
>
resolutions_mr_
;
};
}
// namespace msf
}
// namespace localization
}
// namespace apollo
modules/localization/msf/local_pyramid_map/pyramid_map/pyramid_map_node_config.cc
0 → 100644
浏览文件 @
2c29bbce
/******************************************************************************
* Copyright 2019 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 "modules/localization/msf/local_map/pyramid_map/pyramid_map_node_config.h"
namespace
apollo
{
namespace
localization
{
namespace
msf
{
PyramidMapNodeConfig
::
PyramidMapNodeConfig
()
{}
PyramidMapNodeConfig
::~
PyramidMapNodeConfig
()
{}
BaseMapNodeConfig
*
PyramidMapNodeConfig
::
Clone
()
{
PyramidMapNodeConfig
*
map_node_config
=
new
PyramidMapNodeConfig
();
map_node_config
->
node_index_
=
node_index_
;
map_node_config
->
map_version_
=
map_version_
;
memcpy
(
map_node_config
->
body_md5_
,
body_md5_
,
sizeof
(
body_md5_
));
map_node_config
->
body_size_
=
body_size_
;
map_node_config
->
has_map_version_
=
has_map_version_
;
map_node_config
->
has_body_md5_
=
has_body_md5_
;
return
dynamic_cast
<
BaseMapNodeConfig
*>
(
map_node_config
);
}
}
// namespace msf
}
// namespace localization
}
// namespace apollo
modules/localization/msf/local_pyramid_map/pyramid_map/pyramid_map_node_config.h
0 → 100644
浏览文件 @
2c29bbce
/******************************************************************************
* Copyright 2019 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.
*****************************************************************************/
#pragma once
#include "modules/localization/msf/local_map/base_map/base_map_node_config.h"
namespace
apollo
{
namespace
localization
{
namespace
msf
{
class
PyramidMapNodeConfig
:
public
BaseMapNodeConfig
{
public:
PyramidMapNodeConfig
();
~
PyramidMapNodeConfig
();
virtual
BaseMapNodeConfig
*
Clone
();
};
}
// namespace msf
}
// namespace localization
}
// namespace apollo
modules/localization/msf/local_pyramid_map/pyramid_map/pyramid_map_node_test.cc
0 → 100644
浏览文件 @
2c29bbce
/******************************************************************************
* Copyright 2019 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 "modules/localization/msf/local_map/pyramid_map/pyramid_map_node.h"
#include <gtest/gtest.h>
#include "modules/localization/msf/local_map/base_map/base_map_node.h"
#include "modules/localization/msf/local_map/base_map/base_map_node_config.h"
#include "modules/localization/msf/local_map/pyramid_map/pyramid_map_config.h"
#include "modules/localization/msf/local_map/pyramid_map/pyramid_map_node_config.h"
int
main
(
int
argc
,
char
**
argv
)
{
testing
::
InitGoogleTest
(
&
argc
,
argv
);
return
RUN_ALL_TESTS
();
}
namespace
apollo
{
namespace
localization
{
namespace
msf
{
class
PyramidMapNodeTestSuite
:
public
::
testing
::
Test
{
protected:
PyramidMapNodeTestSuite
()
{}
virtual
~
PyramidMapNodeTestSuite
()
{}
virtual
void
SetUp
()
{}
virtual
void
TearDown
()
{}
};
TEST_F
(
PyramidMapNodeTestSuite
,
init
)
{
// create index
MapNodeIndex
index
;
index
.
resolution_id_
=
0
;
index
.
zone_id_
=
50
;
index
.
m_
=
0
;
index
.
n_
=
0
;
// init config
PyramidMapConfig
*
config
=
new
PyramidMapConfig
(
"lossy_full_alt"
);
config
->
map_version_
=
"pyramid_lossy_map"
;
config
->
SetMapNodeSize
(
2
,
2
);
config
->
resolution_num_
=
2
;
// create map node
PyramidMapNode
*
node
=
new
PyramidMapNode
();
node
->
Init
(
config
);
node
->
SetMapNodeIndex
(
index
);
node
->
Init
(
config
,
index
);
config
->
map_version_
=
"lossy_full_alt"
;
node
->
Init
(
config
,
index
);
if
(
node
!=
nullptr
)
{
delete
node
;
node
=
nullptr
;
}
if
(
config
!=
nullptr
)
{
delete
config
;
config
=
nullptr
;
}
}
TEST_F
(
PyramidMapNodeTestSuite
,
pyramid_map_node_function
)
{
// create index
MapNodeIndex
index
;
index
.
resolution_id_
=
0
;
index
.
zone_id_
=
50
;
index
.
m_
=
0
;
index
.
n_
=
0
;
// init config
PyramidMapConfig
*
config
=
new
PyramidMapConfig
(
"lossy_full_alt"
);
config
->
SetMapNodeSize
(
2
,
2
);
config
->
resolution_num_
=
2
;
// create map node
PyramidMapNode
*
node
=
new
PyramidMapNode
();
node
->
Init
(
config
);
node
->
SetMapNodeIndex
(
index
);
// check global coordinate to local coordinate transformation
double
data
[]
=
{
0.125
,
0.125
,
1.0
};
unsigned
int
x
=
0
;
unsigned
int
y
=
0
;
Eigen
::
Vector2d
vector_2d_data
(
data
);
node
->
GetCoordinate
(
vector_2d_data
,
0
,
&
x
,
&
y
);
EXPECT_EQ
(
x
,
1
);
EXPECT_EQ
(
y
,
1
);
// good case: set and get method
Eigen
::
Vector3d
vector_3d_data
(
data
);
EXPECT_TRUE
(
node
->
AddValueIfInBound
(
vector_3d_data
,
10
,
0
));
EXPECT_FLOAT_EQ
(
node
->
GetIntensitySafe
(
vector_3d_data
,
0
),
10.
f
);
EXPECT_FLOAT_EQ
(
node
->
GetIntensityVarSafe
(
vector_3d_data
,
0
),
0.
f
);
EXPECT_FLOAT_EQ
(
node
->
GetAltitudeSafe
(
vector_3d_data
,
0
),
1.
f
);
EXPECT_FLOAT_EQ
(
node
->
GetAltitudeVarSafe
(
vector_3d_data
,
0
),
0.
f
);
EXPECT_EQ
(
node
->
GetCountSafe
(
vector_3d_data
,
0
),
1
);
EXPECT_EQ
(
node
->
GetGroundCountSafe
(
vector_3d_data
,
0
),
0
);
EXPECT_FLOAT_EQ
(
node
->
GetGroundAltitudeSafe
(
vector_3d_data
,
0
),
0.
f
);
EXPECT_FLOAT_EQ
(
node
->
GetIntensity
(
vector_3d_data
,
0
),
10.
f
);
EXPECT_FLOAT_EQ
(
node
->
GetIntensityVar
(
vector_3d_data
,
0
),
0.
f
);
EXPECT_FLOAT_EQ
(
node
->
GetAltitude
(
vector_3d_data
,
0
),
1.
f
);
EXPECT_FLOAT_EQ
(
node
->
GetAltitudeVar
(
vector_3d_data
,
0
),
0.
f
);
EXPECT_EQ
(
node
->
GetCount
(
vector_3d_data
,
0
),
1
);
EXPECT_EQ
(
node
->
GetGroundCount
(
vector_3d_data
,
0
),
0
);
EXPECT_FLOAT_EQ
(
node
->
GetGroundAltitude
(
vector_3d_data
,
0
),
0.
f
);
// bottom up
node
->
BottomUpSafe
();
EXPECT_FLOAT_EQ
(
node
->
GetIntensitySafe
(
vector_3d_data
,
1
),
10.
f
);
EXPECT_FLOAT_EQ
(
node
->
GetIntensityVarSafe
(
vector_3d_data
,
1
),
0.
f
);
EXPECT_FLOAT_EQ
(
node
->
GetAltitudeSafe
(
vector_3d_data
,
1
),
1.
f
);
EXPECT_FLOAT_EQ
(
node
->
GetAltitudeVarSafe
(
vector_3d_data
,
1
),
0.
f
);
EXPECT_EQ
(
node
->
GetCountSafe
(
vector_3d_data
,
1
),
1
);
EXPECT_EQ
(
node
->
GetGroundCountSafe
(
vector_3d_data
,
1
),
0
);
// bad case 1: get coordinate failed
vector_3d_data
[
0
]
=
10.0
;
vector_3d_data
[
1
]
=
10.0
;
EXPECT_FLOAT_EQ
(
node
->
GetIntensitySafe
(
vector_3d_data
,
0
),
0.
f
);
EXPECT_FLOAT_EQ
(
node
->
GetIntensityVarSafe
(
vector_3d_data
,
0
),
0.
f
);
EXPECT_FLOAT_EQ
(
node
->
GetAltitudeSafe
(
vector_3d_data
,
0
),
0.
f
);
EXPECT_FLOAT_EQ
(
node
->
GetAltitudeVarSafe
(
vector_3d_data
,
0
),
0.
f
);
EXPECT_EQ
(
node
->
GetCountSafe
(
vector_3d_data
,
0
),
0
);
EXPECT_EQ
(
node
->
GetGroundCountSafe
(
vector_3d_data
,
0
),
0
);
EXPECT_FLOAT_EQ
(
node
->
GetGroundAltitudeSafe
(
vector_3d_data
,
0
),
0.
f
);
EXPECT_FLOAT_EQ
(
node
->
GetIntensity
(
vector_3d_data
,
0
),
0.
f
);
EXPECT_FLOAT_EQ
(
node
->
GetIntensityVar
(
vector_3d_data
,
0
),
0.
f
);
EXPECT_FLOAT_EQ
(
node
->
GetAltitude
(
vector_3d_data
,
0
),
0.
f
);
EXPECT_FLOAT_EQ
(
node
->
GetAltitudeVar
(
vector_3d_data
,
0
),
0.
f
);
EXPECT_EQ
(
node
->
GetCount
(
vector_3d_data
,
0
),
0
);
EXPECT_EQ
(
node
->
GetGroundCount
(
vector_3d_data
,
0
),
0
);
EXPECT_FLOAT_EQ
(
node
->
GetGroundAltitude
(
vector_3d_data
,
0
),
0.
f
);
// check branch when reset
node
->
ResetMapNode
();
vector_3d_data
[
0
]
=
0.125
;
vector_3d_data
[
1
]
=
0.125
;
EXPECT_FLOAT_EQ
(
node
->
GetIntensitySafe
(
vector_3d_data
,
0
),
0.
f
);
EXPECT_FLOAT_EQ
(
node
->
GetIntensityVarSafe
(
vector_3d_data
,
0
),
0.
f
);
EXPECT_FLOAT_EQ
(
node
->
GetAltitudeSafe
(
vector_3d_data
,
0
),
0.
f
);
EXPECT_FLOAT_EQ
(
node
->
GetAltitudeVarSafe
(
vector_3d_data
,
0
),
0.
f
);
EXPECT_EQ
(
node
->
GetCountSafe
(
vector_3d_data
,
0
),
0
);
EXPECT_EQ
(
node
->
GetGroundCountSafe
(
vector_3d_data
,
0
),
0
);
EXPECT_FLOAT_EQ
(
node
->
GetGroundAltitudeSafe
(
vector_3d_data
,
0
),
0.
f
);
EXPECT_FLOAT_EQ
(
node
->
GetIntensity
(
vector_3d_data
,
0
),
0.
f
);
EXPECT_FLOAT_EQ
(
node
->
GetIntensityVar
(
vector_3d_data
,
0
),
0.
f
);
EXPECT_FLOAT_EQ
(
node
->
GetAltitude
(
vector_3d_data
,
0
),
0.
f
);
EXPECT_FLOAT_EQ
(
node
->
GetAltitudeVar
(
vector_3d_data
,
0
),
0.
f
);
EXPECT_EQ
(
node
->
GetCount
(
vector_3d_data
,
0
),
0
);
EXPECT_EQ
(
node
->
GetGroundCount
(
vector_3d_data
,
0
),
0
);
EXPECT_FLOAT_EQ
(
node
->
GetGroundAltitude
(
vector_3d_data
,
0
),
0.
f
);
if
(
node
!=
nullptr
)
{
delete
node
;
node
=
nullptr
;
}
if
(
config
!=
nullptr
)
{
delete
config
;
config
=
nullptr
;
}
}
TEST_F
(
PyramidMapNodeTestSuite
,
test_get_set_bad_case
)
{
// create index
MapNodeIndex
index
;
index
.
resolution_id_
=
0
;
index
.
zone_id_
=
50
;
index
.
m_
=
0
;
index
.
n_
=
0
;
// init config
PyramidMapConfig
*
config
=
new
PyramidMapConfig
(
"lossy_full_alt"
);
config
->
SetMapNodeSize
(
2
,
2
);
config
->
resolution_num_
=
1
;
config
->
has_intensity_
=
false
;
config
->
has_intensity_var_
=
false
;
config
->
has_altitude_
=
false
;
config
->
has_altitude_var_
=
false
;
config
->
has_ground_altitude_
=
false
;
config
->
has_ground_count_
=
false
;
config
->
has_count_
=
false
;
// create map node
PyramidMapNode
*
node
=
new
PyramidMapNode
();
node
->
Init
(
config
);
node
->
SetMapNodeIndex
(
index
);
// check global coordinate to local coordinate transformation
double
data
[]
=
{
0.125
,
0.125
,
1.0
};
Eigen
::
Vector3d
vector_3d_data
(
data
);
EXPECT_FLOAT_EQ
(
node
->
GetIntensitySafe
(
vector_3d_data
,
0
),
0.
f
);
EXPECT_FLOAT_EQ
(
node
->
GetIntensityVarSafe
(
vector_3d_data
,
0
),
0.
f
);
EXPECT_FLOAT_EQ
(
node
->
GetAltitudeSafe
(
vector_3d_data
,
0
),
0.
f
);
EXPECT_FLOAT_EQ
(
node
->
GetAltitudeVarSafe
(
vector_3d_data
,
0
),
0.
f
);
EXPECT_EQ
(
node
->
GetCountSafe
(
vector_3d_data
,
0
),
0
);
EXPECT_EQ
(
node
->
GetGroundCountSafe
(
vector_3d_data
,
0
),
0
);
EXPECT_FLOAT_EQ
(
node
->
GetGroundAltitudeSafe
(
vector_3d_data
,
0
),
0.
f
);
}
TEST_F
(
PyramidMapNodeTestSuite
,
test_base_map_node_save_load
)
{
// create index
MapNodeIndex
index
;
index
.
resolution_id_
=
0
;
index
.
zone_id_
=
50
;
index
.
m_
=
0
;
index
.
n_
=
1
;
// init config
PyramidMapConfig
*
config
=
new
PyramidMapConfig
(
"lossy_full_alt"
);
config
->
SetMapNodeSize
(
2
,
2
);
config
->
resolution_num_
=
1
;
config
->
map_folder_path_
=
"wrong_path"
;
// create map node
PyramidMapNode
*
node
=
new
PyramidMapNode
();
node
->
Init
(
config
);
node
->
SetMapNodeIndex
(
index
);
EXPECT_EQ
(
node
->
GetMapNodeIndex
().
m_
,
0
);
EXPECT_EQ
(
node
->
GetMapNodeIndex
().
n_
,
1
);
// check Load
EXPECT_FALSE
(
node
->
Load
());
EXPECT_FALSE
(
node
->
Load
(
"map.bin"
));
if
(
node
!=
nullptr
)
{
delete
node
;
node
=
nullptr
;
}
if
(
config
!=
nullptr
)
{
delete
config
;
config
=
nullptr
;
}
}
TEST_F
(
PyramidMapNodeTestSuite
,
test_base_map_node
)
{
// create index
MapNodeIndex
index
;
index
.
resolution_id_
=
0
;
index
.
zone_id_
=
50
;
index
.
m_
=
0
;
index
.
n_
=
1
;
// init config
PyramidMapConfig
*
config
=
new
PyramidMapConfig
(
"lossy_full_alt"
);
config
->
SetMapNodeSize
(
2
,
2
);
config
->
resolution_num_
=
1
;
// create map node
PyramidMapNode
*
node
=
new
PyramidMapNode
();
node
->
Init
(
config
);
node
->
SetMapNodeIndex
(
index
);
EXPECT_EQ
(
node
->
GetMapNodeIndex
().
m_
,
0
);
EXPECT_EQ
(
node
->
GetMapNodeIndex
().
n_
,
1
);
// add value
double
data
[]
=
{
0.125
,
0.125
,
1.0
};
Eigen
::
Vector3d
vector_3d_data
(
data
);
EXPECT_FALSE
(
node
->
AddValueIfInBound
(
vector_3d_data
,
10
,
0
));
vector_3d_data
[
0
]
=
0.375
;
EXPECT_TRUE
(
node
->
AddValueIfInBound
(
vector_3d_data
,
10
,
0
));
// Save and Load
// Load -> load_binary -> get_header_binary_size -> load_header_binary ->
// load_body_binary
EXPECT_TRUE
(
node
->
Save
());
EXPECT_TRUE
(
node
->
Load
());
// check bad case
EXPECT_TRUE
(
node
->
GetIsReady
());
// check calculate left top corner
Eigen
::
Vector2d
left_top_corner
=
node
->
ComputeLeftTopCorner
(
*
config
,
index
);
EXPECT_FLOAT_EQ
(
left_top_corner
[
0
],
0.25
);
EXPECT_FLOAT_EQ
(
left_top_corner
[
1
],
0.0
);
// check Get map cell matrix
const
BaseMapMatrix
&
map_matrix_const
=
node
->
GetMapCellMatrix
();
const
PyramidMapMatrix
&
pyramid_matrix_const
=
dynamic_cast
<
const
PyramidMapMatrix
&>
(
map_matrix_const
);
EXPECT_FLOAT_EQ
(
*
pyramid_matrix_const
.
GetIntensitySafe
(
1
,
1
,
0
),
10.
f
);
BaseMapMatrix
&
map_matrix
=
node
->
GetMapCellMatrix
();
PyramidMapMatrix
&
pyramid_matrix
=
dynamic_cast
<
PyramidMapMatrix
&>
(
map_matrix
);
EXPECT_FLOAT_EQ
(
*
pyramid_matrix
.
GetIntensitySafe
(
1
,
1
,
0
),
10.
f
);
// check Get the map config
const
BaseMapConfig
&
bm_config
=
node
->
GetMapConfig
();
const
PyramidMapConfig
&
pm_config
=
dynamic_cast
<
const
PyramidMapConfig
&>
(
bm_config
);
EXPECT_EQ
(
pm_config
.
resolution_num_
,
1
);
// check Get the map node config
const
BaseMapNodeConfig
&
bmn_config
=
node
->
GetMapNodeConfig
();
EXPECT_EQ
(
bmn_config
.
map_version_
,
MapVersion
::
LOSSY_FULL_ALT_MAP
);
// check variables
EXPECT_FALSE
(
node
->
GetIsReserved
());
EXPECT_FALSE
(
node
->
GetIsChanged
());
node
->
SetIsReserved
(
true
);
node
->
SetIsChanged
(
true
);
node
->
ResetMapNode
();
EXPECT_FALSE
(
node
->
GetIsReserved
());
EXPECT_FALSE
(
node
->
GetIsChanged
());
// check resolution
EXPECT_FLOAT_EQ
(
node
->
GetMapResolution
(),
0.125
f
);
// add vector
std
::
vector
<
Eigen
::
Vector3d
>
coordinates
;
vector_3d_data
[
0
]
=
0.375
;
vector_3d_data
[
0
]
=
0.125
;
coordinates
.
push_back
(
vector_3d_data
);
std
::
vector
<
unsigned
char
>
intensities
;
intensities
.
push_back
(
0
);
node
->
AddValueIfInBound
(
coordinates
,
intensities
,
0
);
intensities
.
push_back
(
1
);
node
->
AddValueIfInBound
(
coordinates
,
intensities
,
0
);
if
(
node
!=
nullptr
)
{
delete
node
;
node
=
nullptr
;
}
if
(
config
!=
nullptr
)
{
delete
config
;
config
=
nullptr
;
}
}
}
// namespace msf
}
// namespace localization
}
// namespace apollo
modules/localization/msf/local_pyramid_map/pyramid_map/pyramid_map_test.cc
0 → 100644
浏览文件 @
2c29bbce
/******************************************************************************
* Copyright 2019 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 <gtest/gtest.h>
#include <boost/filesystem.hpp>
#include "modules/localization/msf/local_map/pyramid_map/pyramid_map.h"
#include "modules/localization/msf/local_map/pyramid_map/pyramid_map_config.h"
#include "modules/localization/msf/local_map/pyramid_map/pyramid_map_pool.h"
#include "modules/localization/msf/local_map/base_map/base_map.h"
#include "modules/localization/msf/local_map/base_map/base_map_config.h"
int
main
(
int
argc
,
char
**
argv
)
{
testing
::
InitGoogleTest
(
&
argc
,
argv
);
return
RUN_ALL_TESTS
();
}
namespace
apollo
{
namespace
localization
{
namespace
msf
{
class
PyramidMapTestSuite
:
public
::
testing
::
Test
{
protected:
PyramidMapTestSuite
()
{}
virtual
~
PyramidMapTestSuite
()
{}
virtual
void
SetUp
()
{}
virtual
void
TearDown
()
{}
};
void
CreateTestMapNode
(
unsigned
int
m
,
unsigned
int
n
,
const
MapNodeIndex
&
index
,
const
PyramidMapConfig
*
config
)
{
// create map node
PyramidMapNode
*
node
=
new
PyramidMapNode
();
node
->
Init
(
config
);
node
->
SetMapNodeIndex
(
index
);
// set map node data
double
data
[]
=
{
node
->
GetLeftTopCorner
()[
0
]
+
0.125
,
node
->
GetLeftTopCorner
()[
1
]
+
0.125
,
1.0
};
Eigen
::
Vector3d
vec3d
(
data
);
EXPECT_TRUE
(
node
->
AddValueIfInBound
(
vec3d
,
static_cast
<
unsigned
char
>
(
m
*
config
->
map_node_size_x_
+
n
),
0
));
EXPECT_TRUE
(
node
->
Save
());
if
(
node
!=
NULL
)
{
delete
node
;
node
=
NULL
;
}
}
TEST_F
(
PyramidMapTestSuite
,
pyramid_map_function
)
{
// init config
PyramidMapConfig
*
config
=
new
PyramidMapConfig
(
"lossy_full_alt"
);
config
->
SetMapNodeSize
(
2
,
2
);
config
->
resolution_num_
=
1
;
config
->
map_folder_path_
=
"test_map"
;
// create and save nodes
unsigned
int
M
=
4
;
unsigned
int
N
=
4
;
std
::
vector
<
MapNodeIndex
>
indexes
;
for
(
unsigned
int
m
=
0
;
m
<
M
;
++
m
)
{
for
(
unsigned
int
n
=
0
;
n
<
N
;
++
n
)
{
// create map node index
MapNodeIndex
index
;
index
.
resolution_id_
=
0
;
index
.
zone_id_
=
50
;
index
.
m_
=
m
;
index
.
n_
=
n
;
CreateTestMapNode
(
m
,
n
,
index
,
config
);
indexes
.
push_back
(
index
);
}
}
// save config file
config
->
Save
(
"test_map/config.xml"
);
// init pyramid map pool
PyramidMapNodePool
pm_node_pool
(
16
,
4
);
pm_node_pool
.
Initial
(
config
);
// init pyramid map
PyramidMap
pyramid_map
(
config
);
pyramid_map
.
InitMapNodeCaches
(
4
,
15
);
pyramid_map
.
AttachMapNodePool
(
&
pm_node_pool
);
EXPECT_TRUE
(
pyramid_map
.
SetMapFolderPath
(
config
->
map_folder_path_
));
// load map node safe from disk
PyramidMapNode
*
pm_node
=
dynamic_cast
<
PyramidMapNode
*>
(
pyramid_map
.
GetMapNodeSafe
(
*
indexes
.
begin
()));
double
data
[]
=
{
0.125
,
0.125
,
1.0
};
Eigen
::
Vector3d
loc
(
data
);
EXPECT_FLOAT_EQ
(
pm_node
->
GetIntensitySafe
(
loc
),
static_cast
<
float
>
(
indexes
.
begin
()
->
m_
*
config
->
map_node_size_x_
+
indexes
.
begin
()
->
n_
));
EXPECT_TRUE
(
pyramid_map
.
IsMapNodeExist
(
*
indexes
.
begin
()));
EXPECT_FALSE
(
pyramid_map
.
IsMapNodeExist
(
*
(
indexes
.
begin
()
+
1
)));
// load map area
loc
[
0
]
=
0.125
*
(
config
->
map_node_size_x_
+
1
);
loc
[
1
]
=
0.125
*
(
config
->
map_node_size_x_
+
1
);
loc
[
2
]
=
1.0
;
Vector3d
loc_eigen
;
loc_eigen
[
0
]
=
loc
[
0
];
loc_eigen
[
1
]
=
loc
[
1
];
EXPECT_TRUE
(
pyramid_map
.
LoadMapArea
(
loc
,
0
,
50
,
0
,
0
));
EXPECT_TRUE
(
pyramid_map
.
LoadMapArea
(
loc_eigen
,
0
,
50
,
0
,
0
));
EXPECT_TRUE
(
pyramid_map
.
IsMapNodeExist
(
*
(
indexes
.
begin
()
+
5
)));
EXPECT_TRUE
(
pyramid_map
.
IsMapNodeExist
(
*
(
indexes
.
begin
()
+
6
)));
EXPECT_TRUE
(
pyramid_map
.
IsMapNodeExist
(
*
(
indexes
.
begin
()
+
9
)));
EXPECT_TRUE
(
pyramid_map
.
IsMapNodeExist
(
*
(
indexes
.
begin
()
+
10
)));
EXPECT_FALSE
(
pyramid_map
.
IsMapNodeExist
(
*
(
indexes
.
begin
()
+
0
)));
// preload map area
Eigen
::
Vector3d
trans_diff
;
trans_diff
[
0
]
=
1.0
;
trans_diff
[
1
]
=
1.0
;
trans_diff
[
2
]
=
1.0
;
pyramid_map
.
PreloadMapArea
(
loc
,
trans_diff
,
0
,
50
);
loc_eigen
[
0
]
=
0.125
;
loc_eigen
[
1
]
=
0.125
;
Vector3d
trans_diff_eigen
;
trans_diff_eigen
[
0
]
=
trans_diff
[
0
];
trans_diff_eigen
[
1
]
=
trans_diff
[
1
];
trans_diff_eigen
[
2
]
=
trans_diff
[
2
];
pyramid_map
.
PreloadMapArea
(
loc_eigen
,
trans_diff_eigen
,
0
,
50
);
// get path
pyramid_map
.
ComputeMd5ForAllMapNodes
();
std
::
vector
<
std
::
string
>
paths
=
pyramid_map
.
GetAllMapNodePaths
();
EXPECT_EQ
(
paths
.
size
(),
M
*
N
);
std
::
vector
<
std
::
string
>
md5s
=
pyramid_map
.
GetAllMapNodeMd5s
();
EXPECT_EQ
(
md5s
.
size
(),
M
*
N
);
// set md5
EXPECT_FALSE
(
pyramid_map
.
CheckMap
());
std
::
map
<
std
::
string
,
std
::
string
>
node_md5_map
;
for
(
unsigned
int
i
=
0
;
i
<
paths
.
size
();
++
i
)
{
node_md5_map
[
paths
[
i
]]
=
md5s
[
i
];
}
config
->
SetNodeMd5Map
(
node_md5_map
);
EXPECT_TRUE
(
pyramid_map
.
CheckMap
());
// test PyramidMap function: access specific coordinate
EXPECT_FLOAT_EQ
(
pyramid_map
.
GetIntensitySafe
(
loc
,
50
,
0
),
1.
f
*
static_cast
<
float
>
(
config
->
map_node_size_x_
)
+
1.
f
);
EXPECT_FLOAT_EQ
(
pyramid_map
.
GetIntensityVarSafe
(
loc
,
50
,
0
),
0.
f
);
EXPECT_FLOAT_EQ
(
pyramid_map
.
GetAltitudeSafe
(
loc
,
50
,
0
),
1.
f
);
EXPECT_FLOAT_EQ
(
pyramid_map
.
GetAltitudeVarSafe
(
loc
,
50
,
0
),
0.
f
);
EXPECT_EQ
(
pyramid_map
.
GetCountSafe
(
loc
,
50
,
0
),
1
);
loc
[
0
]
=
1.0
;
loc
[
1
]
=
1.0
;
loc
[
2
]
=
1.0
;
EXPECT_FLOAT_EQ
(
pyramid_map
.
GetGroundAltitudeSafe
(
loc
,
50
,
0
),
0.
f
);
EXPECT_EQ
(
pyramid_map
.
GetGroundCountSafe
(
loc
,
50
,
0
),
0
);
// reset the caches and pool
pyramid_map
.
InitMapNodeCaches
(
6
,
16
);
if
(
config
!=
NULL
)
{
delete
config
;
config
=
NULL
;
}
}
TEST_F
(
PyramidMapTestSuite
,
test_init
)
{
// init config
PyramidMapConfig
*
config
=
new
PyramidMapConfig
(
"lossy_full_alt"
);
config
->
SetMapNodeSize
(
2
,
2
);
config
->
resolution_num_
=
1
;
config
->
map_folder_path_
=
"test_map"
;
// create and save nodes
unsigned
int
M
=
4
;
unsigned
int
N
=
4
;
std
::
vector
<
MapNodeIndex
>
indexes
;
for
(
unsigned
int
m
=
0
;
m
<
M
;
++
m
)
{
for
(
unsigned
int
n
=
0
;
n
<
N
;
++
n
)
{
// create map node index
MapNodeIndex
index
;
index
.
resolution_id_
=
0
;
index
.
zone_id_
=
50
;
index
.
m_
=
m
;
index
.
n_
=
n
;
CreateTestMapNode
(
m
,
n
,
index
,
config
);
indexes
.
push_back
(
index
);
}
}
// save config file
config
->
Save
(
"test_map/config.xml"
);
// init pyramid map pool
PyramidMapNodePool
pm_node_pool
(
16
,
4
);
pm_node_pool
.
Initial
(
config
);
// init pyramid map
PyramidMap
pyramid_map
(
config
);
pyramid_map
.
InitMapNodeCaches
(
4
,
15
);
pyramid_map
.
AttachMapNodePool
(
&
pm_node_pool
);
EXPECT_TRUE
(
pyramid_map
.
SetMapFolderPath
(
config
->
map_folder_path_
));
// check fail case
double
data
[]
=
{
0.125
*
(
config
->
map_node_size_x_
+
1
),
0.125
*
(
config
->
map_node_size_x_
+
1
),
1.0
};
Eigen
::
Vector3d
loc
(
data
);
EXPECT_TRUE
(
pyramid_map
.
LoadMapArea
(
loc
,
0
,
50
,
0
,
0
));
}
}
// namespace msf
}
// namespace localization
}
// namespace apollo
编辑
预览
Markdown
is supported
0%
请重试
或
添加新附件
.
添加附件
取消
You are about to add
0
people
to the discussion. Proceed with caution.
先完成此消息的编辑!
取消
想要评论请
注册
或
登录