Skip to content
体验新版
项目
组织
正在加载...
登录
切换导航
打开侧边栏
Pinoxchio
apollo
提交
62e54d7c
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,发现更多精彩内容 >>
提交
62e54d7c
编写于
6月 28, 2019
作者:
T
tthhee
提交者:
Calvin Miao
7月 09, 2019
浏览文件
操作
浏览文件
下载
电子邮件补丁
差异文件
Localization: add base_map files for pyramid_map module
上级
ddf31735
变更
6
展开全部
隐藏空白更改
内联
并排
Showing
6 changed file
with
1696 addition
and
0 deletion
+1696
-0
modules/localization/msf/local_pyramid_map/base_map/base_map.cc
...s/localization/msf/local_pyramid_map/base_map/base_map.cc
+584
-0
modules/localization/msf/local_pyramid_map/base_map/base_map.h
...es/localization/msf/local_pyramid_map/base_map/base_map.h
+143
-0
modules/localization/msf/local_pyramid_map/base_map/base_map_node.cc
...alization/msf/local_pyramid_map/base_map/base_map_node.cc
+544
-0
modules/localization/msf/local_pyramid_map/base_map/base_map_node.h
...calization/msf/local_pyramid_map/base_map/base_map_node.h
+212
-0
modules/localization/msf/local_pyramid_map/base_map/base_map_pool.cc
...alization/msf/local_pyramid_map/base_map/base_map_pool.cc
+121
-0
modules/localization/msf/local_pyramid_map/base_map/base_map_pool.h
...calization/msf/local_pyramid_map/base_map/base_map_pool.h
+92
-0
未找到文件。
modules/localization/msf/local_pyramid_map/base_map/base_map.cc
0 → 100644
浏览文件 @
62e54d7c
此差异已折叠。
点击以展开。
modules/localization/msf/local_pyramid_map/base_map/base_map.h
0 → 100644
浏览文件 @
62e54d7c
/******************************************************************************
* 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 <map>
#include <set>
#include <string>
#include <vector>
#include "cyber/task/task.h"
#include "modules/localization/msf/local_map/base_map/base_map_cache.h"
#include "modules/localization/msf/local_map/base_map/base_map_config.h"
#include "modules/localization/msf/local_map/base_map/base_map_fwd.h"
#include "modules/localization/msf/local_map/base_map/base_map_node.h"
#include "modules/localization/msf/local_map/base_map/base_map_node_index.h"
#include "modules/localization/msf/local_map/base_map/base_map_pool.h"
namespace
apollo
{
namespace
localization
{
namespace
msf
{
/**@brief The data structure of the base map. */
class
BaseMap
{
public:
/**@brief The constructor. */
explicit
BaseMap
(
BaseMapConfig
*
config
);
/**@brief The destructor. */
virtual
~
BaseMap
();
// @brief Init level 1 and level 2 map node caches. */
virtual
void
InitMapNodeCaches
(
int
cacheL1_size
,
int
cahceL2_size
);
/**@brief Attach map node pointer. */
void
AttachMapNodePool
(
BaseMapNodePool
*
p_map_node_pool
);
/**@brief Return the map node, if it's not in the cache, return false. */
BaseMapNode
*
GetMapNode
(
const
MapNodeIndex
&
index
);
/**@brief Return the map node, if it's not in the cache, safely load it. */
BaseMapNode
*
GetMapNodeSafe
(
const
MapNodeIndex
&
index
);
/**@brief Check if the map node in the cache. */
bool
IsMapNodeExist
(
const
MapNodeIndex
&
index
);
/**@brief Set the directory of the map. */
bool
SetMapFolderPath
(
const
std
::
string
folder_path
);
/**@brief Add a dataset path to the map config. */
void
AddDataset
(
const
std
::
string
dataset_path
);
/**@brief Release resources. */
void
Release
();
/**@brief Preload map nodes for the next frame location calculation.
* It will forecasts the nodes by the direction of the car moving.
* Because the progress of loading will cost a long time (over 100ms),
* it must do this for a period of time in advance.
* After the index of nodes calculate finished, it will create loading tasks,
* but will not wait for the loading finished, eigen version. */
virtual
void
PreloadMapArea
(
const
Eigen
::
Vector3d
&
location
,
const
Eigen
::
Vector3d
&
trans_diff
,
unsigned
int
resolution_id
,
unsigned
int
zone_id
);
/**@brief Load map nodes for the location calculate of this frame.
* If the forecasts are correct in last frame, these nodes will be all in
* cache, if not, then need to create loading tasks, and wait for the loading
* finish,
* in order to the nodes which the following calculate needed are all in the
* memory, eigen version. */
virtual
bool
LoadMapArea
(
const
Eigen
::
Vector3d
&
seed_pt3d
,
unsigned
int
resolution_id
,
unsigned
int
zone_id
,
int
filter_size_x
,
int
filter_size_y
);
/**@brief Compute md5 for all map node file in map. */
void
ComputeMd5ForAllMapNodes
();
/**@brief Check if map is normal. */
bool
CheckMap
();
/**@brief Check if map is normal(with map node checking). */
bool
CheckMapStrictly
();
/**@brief Get the map config. */
inline
const
BaseMapConfig
&
GetMapConfig
()
const
{
return
*
map_config_
;
}
/**@brief Get the map config. */
inline
BaseMapConfig
&
GetMapConfig
()
{
return
*
map_config_
;
}
/**@brief Get all map node paths. */
inline
const
std
::
vector
<
std
::
string
>&
GetAllMapNodePaths
()
const
{
return
all_map_node_paths_
;
}
/**@brief Get all map node md5s. */
inline
const
std
::
vector
<
std
::
string
>&
GetAllMapNodeMd5s
()
const
{
return
all_map_node_md5s_
;
}
protected:
void
GetAllMapIndexAndPath
();
MapNodeIndex
GetMapIndexFromMapPath
(
const
std
::
string
&
map_path
);
protected:
/**@brief Load map node by index.*/
void
LoadMapNodes
(
std
::
set
<
MapNodeIndex
>*
map_ids
);
/**@brief Load map node by index.*/
void
PreloadMapNodes
(
std
::
set
<
MapNodeIndex
>*
map_ids
);
/**@brief Load map node by index, thread_safety. */
void
LoadMapNodeThreadSafety
(
const
MapNodeIndex
&
index
,
bool
is_reserved
=
false
);
/**@brief The map settings. */
BaseMapConfig
*
map_config_
=
nullptr
;
/**@brief The cache for map node preload. */
MapNodeCacheL1
<
MapNodeIndex
,
BaseMapNode
>*
map_node_cache_lvl1_
=
nullptr
;
/**brief The dynamic map node preloading thread pool pointer. */
MapNodeCacheL2
<
MapNodeIndex
,
BaseMapNode
>*
map_node_cache_lvl2_
=
nullptr
;
/**@brief The map node memory pool pointer. */
BaseMapNodePool
*
map_node_pool_
=
nullptr
;
/**@bried Keep the index of preloading nodes. */
std
::
set
<
MapNodeIndex
>
map_preloading_task_index_
;
/**@brief The mutex for preload map node. **/
boost
::
recursive_mutex
map_load_mutex_
;
/**@brief All the map nodes in the Map (in the disk). */
std
::
vector
<
MapNodeIndex
>
all_map_node_indices_
;
std
::
vector
<
std
::
string
>
all_map_node_paths_
;
/**@brief All the map nodes' md5 in the Map (in the disk). */
std
::
vector
<
std
::
string
>
all_map_node_md5s_
;
};
}
// namespace msf
}
// namespace localization
}
// namespace apollo
modules/localization/msf/local_pyramid_map/base_map/base_map_node.cc
0 → 100644
浏览文件 @
62e54d7c
/******************************************************************************
* 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/base_map/base_map_node.h"
#include <cstdio>
#include <string>
#include <vector>
#include "cyber/common/file.h"
#include "modules/localization/msf/common/util/file_utility.h"
#include "modules/localization/msf/local_map/base_map/base_map_matrix.h"
namespace
apollo
{
namespace
localization
{
namespace
msf
{
BaseMapNode
::
BaseMapNode
()
:
map_config_
(
NULL
),
map_matrix_
(
NULL
),
map_matrix_handler_
(
NULL
),
compression_strategy_
(
NULL
)
{
is_changed_
=
false
;
data_is_ready_
=
false
;
is_reserved_
=
false
;
file_body_binary_size_
=
0
;
uncompressed_file_body_size_
=
0
;
}
BaseMapNode
::
BaseMapNode
(
BaseMapMatrix
*
matrix
,
CompressionStrategy
*
strategy
)
:
map_config_
(
NULL
),
map_matrix_
(
matrix
),
map_matrix_handler_
(
NULL
),
compression_strategy_
(
strategy
)
{
is_changed_
=
false
;
data_is_ready_
=
false
;
is_reserved_
=
false
;
file_body_binary_size_
=
0
;
}
BaseMapNode
::~
BaseMapNode
()
{
if
(
map_node_config_
!=
NULL
)
{
delete
map_node_config_
;
}
if
(
map_matrix_
!=
NULL
)
{
delete
map_matrix_
;
}
if
(
map_matrix_handler_
!=
NULL
)
{
delete
map_matrix_handler_
;
}
if
(
compression_strategy_
!=
NULL
)
{
delete
compression_strategy_
;
}
}
void
BaseMapNode
::
InitMapMatrix
(
const
BaseMapConfig
*
map_config
)
{
map_config_
=
map_config
;
map_matrix_
->
Init
(
*
map_config
);
}
void
BaseMapNode
::
Finalize
()
{
if
(
is_changed_
)
{
Save
();
AINFO
<<
"Save Map Node to disk: "
<<
map_node_config_
->
node_index_
<<
"."
;
}
}
void
BaseMapNode
::
ResetMapNode
()
{
is_changed_
=
false
;
data_is_ready_
=
false
;
is_reserved_
=
false
;
file_body_binary_size_
=
0
;
uncompressed_file_body_size_
=
0
;
map_matrix_
->
Reset
();
}
bool
BaseMapNode
::
Save
()
{
SaveIntensityImage
();
SaveAltitudeImage
();
std
::
string
path
=
map_config_
->
map_folder_path_
;
char
buf
[
1024
];
std
::
vector
<
std
::
string
>
paths
;
paths
.
push_back
(
path
);
snprintf
(
buf
,
sizeof
(
buf
),
"/map"
);
paths
.
push_back
(
buf
);
path
=
path
+
buf
;
snprintf
(
buf
,
sizeof
(
buf
),
"/%03u"
,
map_node_config_
->
node_index_
.
resolution_id_
);
paths
.
push_back
(
buf
);
path
=
path
+
buf
;
paths
.
push_back
(
map_node_config_
->
node_index_
.
zone_id_
>
0
?
"/north"
:
"/south"
);
path
=
path
+
paths
.
back
();
snprintf
(
buf
,
sizeof
(
buf
),
"/%02d"
,
abs
(
map_node_config_
->
node_index_
.
zone_id_
));
paths
.
push_back
(
buf
);
path
=
path
+
buf
;
snprintf
(
buf
,
sizeof
(
buf
),
"/%08u"
,
abs
(
map_node_config_
->
node_index_
.
m_
));
paths
.
push_back
(
buf
);
path
=
path
+
buf
;
if
(
!
CreateMapDirectoryRecursively
(
paths
))
{
return
false
;
}
snprintf
(
buf
,
sizeof
(
buf
),
"/%08u"
,
abs
(
map_node_config_
->
node_index_
.
n_
));
path
=
path
+
buf
;
FILE
*
file
=
fopen
(
path
.
c_str
(),
"wb"
);
if
(
file
)
{
bool
success
=
CreateBinary
(
file
);
fclose
(
file
);
is_changed_
=
false
;
return
success
;
}
else
{
AERROR
<<
"Can't write to file: "
<<
path
<<
"."
;
return
false
;
}
}
bool
BaseMapNode
::
Load
()
{
std
::
string
path
=
map_config_
->
map_folder_path_
;
char
buf
[
1024
];
std
::
vector
<
std
::
string
>
paths
;
paths
.
push_back
(
path
);
snprintf
(
buf
,
sizeof
(
buf
),
"/map"
);
paths
.
push_back
(
buf
);
path
=
path
+
buf
;
snprintf
(
buf
,
sizeof
(
buf
),
"/%03u"
,
map_node_config_
->
node_index_
.
resolution_id_
);
paths
.
push_back
(
buf
);
path
=
path
+
buf
;
paths
.
push_back
(
map_node_config_
->
node_index_
.
zone_id_
>
0
?
"/north"
:
"/south"
);
path
=
path
+
paths
.
back
();
snprintf
(
buf
,
sizeof
(
buf
),
"/%02d"
,
abs
(
map_node_config_
->
node_index_
.
zone_id_
));
paths
.
push_back
(
buf
);
path
=
path
+
buf
;
snprintf
(
buf
,
sizeof
(
buf
),
"/%08u"
,
abs
(
map_node_config_
->
node_index_
.
m_
));
paths
.
push_back
(
buf
);
path
=
path
+
buf
;
if
(
!
CheckMapDirectoryRecursively
(
paths
))
{
return
false
;
}
snprintf
(
buf
,
sizeof
(
buf
),
"/%08u"
,
abs
(
map_node_config_
->
node_index_
.
n_
));
path
=
path
+
buf
;
return
Load
(
path
.
c_str
());
}
bool
BaseMapNode
::
Load
(
const
char
*
filename
)
{
data_is_ready_
=
false
;
FILE
*
file
=
fopen
(
filename
,
"rb"
);
if
(
file
)
{
bool
success
=
LoadBinary
(
file
);
fclose
(
file
);
is_changed_
=
false
;
data_is_ready_
=
success
;
return
success
;
}
else
{
AERROR
<<
"Can't find the file: "
<<
filename
;
return
false
;
}
}
bool
BaseMapNode
::
LoadBinary
(
FILE
*
file
)
{
// Load the header
size_t
header_size
=
GetHeaderBinarySize
();
std
::
vector
<
unsigned
char
>
buf
(
header_size
);
size_t
read_size
=
fread
(
&
buf
[
0
],
1
,
header_size
,
file
);
if
(
read_size
!=
header_size
)
{
return
false
;
}
size_t
processed_size
=
LoadHeaderBinary
(
&
buf
[
0
]);
if
(
processed_size
!=
header_size
)
{
return
false
;
}
// Load the body
buf
.
resize
(
file_body_binary_size_
);
read_size
=
fread
(
&
buf
[
0
],
1
,
file_body_binary_size_
,
file
);
if
(
read_size
!=
file_body_binary_size_
)
{
return
false
;
}
processed_size
=
LoadBodyBinary
(
&
buf
);
if
(
processed_size
!=
uncompressed_file_body_size_
)
{
return
false
;
}
return
true
;
}
bool
BaseMapNode
::
CreateBinary
(
FILE
*
file
)
const
{
size_t
buf_size
=
GetBinarySize
();
std
::
vector
<
unsigned
char
>
buffer
;
buffer
.
resize
(
buf_size
);
size_t
binary_size
=
0
;
std
::
vector
<
unsigned
char
>
body_buffer
;
size_t
processed_size
=
CreateBodyBinary
(
&
body_buffer
);
if
(
map_node_config_
->
has_body_md5_
)
{
FileUtility
::
ComputeBinaryMd5
(
&
body_buffer
[
0
],
body_buffer
.
size
(),
map_node_config_
->
body_md5_
);
}
if
(
processed_size
==
0
)
{
return
false
;
}
// Create header
size_t
header_size
=
GetHeaderBinarySize
();
std
::
vector
<
unsigned
char
>
header_buf
(
header_size
);
processed_size
=
CreateHeaderBinary
(
&
buffer
[
0
],
buf_size
);
if
(
header_size
!=
processed_size
)
{
return
false
;
}
size_t
buffer_bias
=
processed_size
;
buf_size
-=
processed_size
;
binary_size
+=
processed_size
;
// Create body
memcpy
(
&
buffer
[
buffer_bias
],
&
body_buffer
[
0
],
body_buffer
.
size
());
// write binary
binary_size
+=
static_cast
<
unsigned
int
>
(
body_buffer
.
size
());
fwrite
(
&
buffer
[
0
],
1
,
binary_size
,
file
);
return
true
;
}
size_t
BaseMapNode
::
GetBinarySize
()
const
{
// It is uncompressed binary size.
return
GetBodyBinarySize
()
+
GetHeaderBinarySize
();
}
size_t
BaseMapNode
::
LoadHeaderBinary
(
const
unsigned
char
*
buf
)
{
BaseMapNodeConfig
*
node_config_tem
=
map_node_config_
->
Clone
();
size_t
target_size
=
map_node_config_
->
LoadBinary
(
buf
);
// check if header is valid
if
(
node_config_tem
->
map_version_
!=
map_node_config_
->
map_version_
||
node_config_tem
->
node_index_
!=
map_node_config_
->
node_index_
)
{
// release node_config_tem
delete
node_config_tem
;
return
0
;
}
// release node_config_tem
delete
node_config_tem
;
file_body_binary_size_
=
map_node_config_
->
body_size_
;
return
target_size
;
}
size_t
BaseMapNode
::
CreateHeaderBinary
(
unsigned
char
*
buf
,
size_t
buf_size
)
const
{
map_node_config_
->
body_size_
=
file_body_binary_size_
;
return
map_node_config_
->
CreateBinary
(
buf
,
buf_size
);
}
size_t
BaseMapNode
::
GetHeaderBinarySize
()
const
{
return
map_node_config_
->
GetBinarySize
();
}
size_t
BaseMapNode
::
LoadBodyBinary
(
std
::
vector
<
unsigned
char
>*
buf
)
{
if
(
compression_strategy_
==
NULL
)
{
return
map_matrix_handler_
->
LoadBinary
(
&
(
*
buf
)[
0
],
map_matrix_
);
}
std
::
vector
<
unsigned
char
>
buf_uncompressed
;
compression_strategy_
->
Decode
(
buf
,
&
buf_uncompressed
);
uncompressed_file_body_size_
=
buf_uncompressed
.
size
();
AINFO
<<
"map node compress ratio: "
<<
static_cast
<
float
>
(
buf
->
size
())
/
static_cast
<
float
>
(
uncompressed_file_body_size_
);
return
map_matrix_handler_
->
LoadBinary
(
&
buf_uncompressed
[
0
],
map_matrix_
);
}
size_t
BaseMapNode
::
CreateBodyBinary
(
std
::
vector
<
unsigned
char
>*
buf
)
const
{
if
(
compression_strategy_
==
NULL
)
{
size_t
body_size
=
GetBodyBinarySize
();
buf
->
resize
(
body_size
);
file_body_binary_size_
=
map_matrix_handler_
->
CreateBinary
(
map_matrix_
,
&
(
*
buf
)[
0
],
body_size
);
return
file_body_binary_size_
;
}
std
::
vector
<
unsigned
char
>
buf_uncompressed
;
// Compute the uncompression binary body size
size_t
body_size
=
GetBodyBinarySize
();
buf_uncompressed
.
resize
(
body_size
);
size_t
binary_size
=
map_matrix_handler_
->
CreateBinary
(
map_matrix_
,
&
buf_uncompressed
[
0
],
body_size
);
if
(
binary_size
==
0
)
{
return
0
;
}
compression_strategy_
->
Encode
(
&
buf_uncompressed
,
buf
);
file_body_binary_size_
=
buf
->
size
();
return
buf
->
size
();
}
size_t
BaseMapNode
::
GetBodyBinarySize
()
const
{
return
map_matrix_handler_
->
GetBinarySize
(
map_matrix_
);
}
bool
BaseMapNode
::
GetCoordinate
(
const
Eigen
::
Vector2d
&
coordinate
,
unsigned
int
*
x
,
unsigned
int
*
y
)
const
{
const
Eigen
::
Vector2d
&
left_top_corner
=
GetLeftTopCorner
();
unsigned
int
off_x
=
static_cast
<
unsigned
int
>
(
(
coordinate
[
0
]
-
left_top_corner
[
0
])
/
GetMapResolution
());
unsigned
int
off_y
=
static_cast
<
unsigned
int
>
(
(
coordinate
[
1
]
-
left_top_corner
[
1
])
/
GetMapResolution
());
if
(
off_x
>=
0
&&
off_x
<
this
->
map_config_
->
map_node_size_x_
&&
off_y
>=
0
&&
off_y
<
this
->
map_config_
->
map_node_size_y_
)
{
*
x
=
static_cast
<
unsigned
int
>
(
off_x
);
*
y
=
static_cast
<
unsigned
int
>
(
off_y
);
return
true
;
}
else
{
return
false
;
}
}
bool
BaseMapNode
::
GetCoordinate
(
const
Eigen
::
Vector3d
&
coordinate
,
unsigned
int
*
x
,
unsigned
int
*
y
)
const
{
Eigen
::
Vector2d
coord2d
(
coordinate
[
0
],
coordinate
[
1
]);
return
GetCoordinate
(
coord2d
,
x
,
y
);
}
Eigen
::
Vector2d
BaseMapNode
::
GetCoordinate
(
unsigned
int
x
,
unsigned
int
y
)
const
{
const
Eigen
::
Vector2d
&
left_top_corner
=
GetLeftTopCorner
();
Eigen
::
Vector2d
coord
(
left_top_corner
[
0
]
+
static_cast
<
float
>
(
x
)
*
GetMapResolution
(),
left_top_corner
[
1
]
+
static_cast
<
float
>
(
y
)
*
GetMapResolution
());
return
coord
;
}
void
BaseMapNode
::
SetMapNodeIndex
(
const
MapNodeIndex
&
index
)
{
map_node_config_
->
node_index_
=
index
;
left_top_corner_
=
ComputeLeftTopCorner
(
*
map_config_
,
map_node_config_
->
node_index_
);
}
Eigen
::
Vector2d
BaseMapNode
::
ComputeLeftTopCorner
(
const
BaseMapConfig
&
config
,
const
MapNodeIndex
&
index
)
{
Eigen
::
Vector2d
coord
;
coord
[
0
]
=
config
.
map_range_
.
GetMinX
()
+
static_cast
<
float
>
(
config
.
map_node_size_x_
)
*
config
.
map_resolutions_
[
index
.
resolution_id_
]
*
static_cast
<
float
>
(
index
.
n_
);
coord
[
1
]
=
config
.
map_range_
.
GetMinY
()
+
static_cast
<
float
>
(
config
.
map_node_size_y_
)
*
config
.
map_resolutions_
[
index
.
resolution_id_
]
*
static_cast
<
float
>
(
index
.
m_
);
if
(
coord
[
0
]
>=
config
.
map_range_
.
GetMaxX
())
{
throw
"[BaseMapNode::ComputeLeftTopCorner] coord[0]"
" >= config.map_range_.GetMaxX()"
;
}
if
(
coord
[
1
]
>=
config
.
map_range_
.
GetMaxY
())
{
throw
"[BaseMapNode::compute_left_top_corner] coord[1]"
" >= config.map_range_.GetMaxY()"
;
}
return
coord
;
}
Eigen
::
Vector2d
BaseMapNode
::
GetLeftTopCorner
(
const
BaseMapConfig
&
config
,
const
MapNodeIndex
&
index
)
{
Eigen
::
Vector2d
coord
;
coord
[
0
]
=
config
.
map_range_
.
GetMinX
()
+
static_cast
<
float
>
(
config
.
map_node_size_x_
)
*
config
.
map_resolutions_
[
index
.
resolution_id_
]
*
static_cast
<
float
>
(
index
.
n_
);
coord
[
1
]
=
config
.
map_range_
.
GetMinY
()
+
static_cast
<
float
>
(
config
.
map_node_size_y_
)
*
config
.
map_resolutions_
[
index
.
resolution_id_
]
*
static_cast
<
float
>
(
index
.
m_
);
DCHECK_LT
(
coord
[
0
],
config
.
map_range_
.
GetMaxX
());
DCHECK_LT
(
coord
[
1
],
config
.
map_range_
.
GetMaxY
());
return
coord
;
}
bool
BaseMapNode
::
CreateMapDirectory
(
const
std
::
string
&
path
)
const
{
if
(
!
cyber
::
common
::
DirectoryExists
(
path
))
{
return
cyber
::
common
::
EnsureDirectory
(
path
);
}
return
true
;
}
bool
BaseMapNode
::
CreateMapDirectoryRecursively
(
const
std
::
vector
<
std
::
string
>&
paths
)
const
{
std
::
string
path
=
""
;
for
(
unsigned
int
i
=
0
;
i
<
paths
.
size
();
++
i
)
{
path
=
path
+
paths
[
i
];
if
(
!
CreateMapDirectory
(
path
))
{
return
false
;
}
}
return
true
;
}
bool
BaseMapNode
::
CheckMapDirectoryRecursively
(
const
std
::
vector
<
std
::
string
>&
paths
)
const
{
std
::
string
path
=
""
;
for
(
unsigned
int
i
=
0
;
i
<
paths
.
size
();
++
i
)
{
path
=
path
+
paths
[
i
];
if
(
!
(
cyber
::
common
::
DirectoryExists
(
path
)))
{
return
false
;
}
}
return
true
;
}
bool
BaseMapNode
::
SaveIntensityImage
()
const
{
std
::
string
path
=
map_config_
->
map_folder_path_
;
char
buf
[
1024
];
std
::
vector
<
std
::
string
>
paths
;
paths
.
push_back
(
path
);
snprintf
(
buf
,
sizeof
(
buf
),
"/image"
);
paths
.
push_back
(
buf
);
path
=
path
+
buf
;
snprintf
(
buf
,
sizeof
(
buf
),
"/%03u"
,
map_node_config_
->
node_index_
.
resolution_id_
);
paths
.
push_back
(
buf
);
path
=
path
+
buf
;
paths
.
push_back
(
map_node_config_
->
node_index_
.
zone_id_
>
0
?
"/north"
:
"/south"
);
path
=
path
+
paths
.
back
();
snprintf
(
buf
,
sizeof
(
buf
),
"/%02d"
,
abs
(
map_node_config_
->
node_index_
.
zone_id_
));
paths
.
push_back
(
buf
);
path
=
path
+
buf
;
snprintf
(
buf
,
sizeof
(
buf
),
"/%08u"
,
abs
(
map_node_config_
->
node_index_
.
m_
));
paths
.
push_back
(
buf
);
path
=
path
+
buf
;
if
(
!
CreateMapDirectoryRecursively
(
paths
))
{
return
false
;
}
snprintf
(
buf
,
sizeof
(
buf
),
"/%08u.png"
,
abs
(
map_node_config_
->
node_index_
.
n_
));
path
=
path
+
buf
;
bool
success
=
SaveIntensityImage
(
path
);
return
success
;
}
bool
BaseMapNode
::
SaveIntensityImage
(
const
std
::
string
&
path
)
const
{
cv
::
Mat
image
;
map_matrix_
->
GetIntensityImg
(
&
image
);
bool
success
=
cv
::
imwrite
(
path
,
image
);
return
success
;
}
bool
BaseMapNode
::
SaveAltitudeImage
()
const
{
std
::
string
path
=
map_config_
->
map_folder_path_
;
char
buf
[
1024
];
std
::
vector
<
std
::
string
>
paths
;
paths
.
push_back
(
path
);
snprintf
(
buf
,
sizeof
(
buf
),
"/image_alt"
);
paths
.
push_back
(
buf
);
path
=
path
+
buf
;
snprintf
(
buf
,
sizeof
(
buf
),
"/%03u"
,
map_node_config_
->
node_index_
.
resolution_id_
);
paths
.
push_back
(
buf
);
path
=
path
+
buf
;
paths
.
push_back
(
map_node_config_
->
node_index_
.
zone_id_
>
0
?
"/north"
:
"/south"
);
path
=
path
+
paths
.
back
();
snprintf
(
buf
,
sizeof
(
buf
),
"/%02d"
,
abs
(
map_node_config_
->
node_index_
.
zone_id_
));
paths
.
push_back
(
buf
);
path
=
path
+
buf
;
snprintf
(
buf
,
sizeof
(
buf
),
"/%08u"
,
abs
(
map_node_config_
->
node_index_
.
m_
));
paths
.
push_back
(
buf
);
path
=
path
+
buf
;
if
(
!
CreateMapDirectoryRecursively
(
paths
))
{
return
false
;
}
snprintf
(
buf
,
sizeof
(
buf
),
"/%08u.png"
,
abs
(
map_node_config_
->
node_index_
.
n_
));
path
=
path
+
buf
;
bool
success
=
SaveAltitudeImage
(
path
);
return
success
;
}
bool
BaseMapNode
::
SaveAltitudeImage
(
const
std
::
string
&
path
)
const
{
cv
::
Mat
image
;
map_matrix_
->
GetAltitudeImg
(
&
image
);
bool
success
=
cv
::
imwrite
(
path
,
image
);
return
success
;
}
}
// namespace msf
}
// namespace localization
}
// namespace apollo
modules/localization/msf/local_pyramid_map/base_map/base_map_node.h
0 → 100644
浏览文件 @
62e54d7c
/******************************************************************************
* 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 <string>
#include <vector>
#include "modules/localization/msf/common/util/compression.h"
#include "modules/localization/msf/local_map/base_map/base_map_config.h"
#include "modules/localization/msf/local_map/base_map/base_map_fwd.h"
#include "modules/localization/msf/local_map/base_map/base_map_matrix_handler.h"
#include "modules/localization/msf/local_map/base_map/base_map_node_config.h"
#include "modules/localization/msf/local_map/base_map/base_map_node_index.h"
namespace
apollo
{
namespace
localization
{
namespace
msf
{
/**@brief The data structure of a Node in the map. */
class
BaseMapNode
{
public:
/**@brief Construct a map node. */
BaseMapNode
();
/**@brief Construct a map node. */
explicit
BaseMapNode
(
BaseMapMatrix
*
matrix
,
CompressionStrategy
*
strategy
);
/**@brief Destruct a map node. */
virtual
~
BaseMapNode
();
/**@brief Initialize the map node. Call this function first before use it! */
virtual
void
Init
(
const
BaseMapConfig
*
map_config
)
=
0
;
virtual
void
Init
(
const
BaseMapConfig
*
map_config
,
const
MapNodeIndex
&
index
,
bool
create_map_cells
=
true
)
=
0
;
/**@brief Initialize the map matrix. */
virtual
void
InitMapMatrix
(
const
BaseMapConfig
*
map_config
);
/**@brief call before deconstruction or reset. */
virtual
void
Finalize
();
/**@brief Reset map cells data. */
virtual
void
ResetMapNode
();
/**@brief Save the map node to the disk. */
bool
Save
();
/**@brief Save intensity image of node. */
bool
SaveIntensityImage
()
const
;
/**@brief Save altitude image of node. */
bool
SaveAltitudeImage
()
const
;
/**@brief Load the map node from the disk. */
bool
Load
();
bool
Load
(
const
char
*
filename
);
/**@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. */
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
;
/**@brief Given the local 2D coordinate, return the global coordinate. */
virtual
Eigen
::
Vector2d
GetCoordinate
(
unsigned
int
x
,
unsigned
int
y
)
const
;
/**@brief Set the map node index. */
void
SetMapNodeIndex
(
const
MapNodeIndex
&
index
);
/**@brief Save intensity image of node. */
bool
SaveIntensityImage
(
const
std
::
string
&
path
)
const
;
/**@brief Save altitude image of node. */
bool
SaveAltitudeImage
(
const
std
::
string
&
path
)
const
;
static
Eigen
::
Vector2d
ComputeLeftTopCorner
(
const
BaseMapConfig
&
config
,
const
MapNodeIndex
&
index
);
/**@brief Get map cell matrix. */
inline
const
BaseMapMatrix
&
GetMapCellMatrix
()
const
{
return
*
map_matrix_
;
}
inline
BaseMapMatrix
&
GetMapCellMatrix
()
{
return
*
map_matrix_
;
}
/**@brief Get the map settings. */
inline
const
BaseMapConfig
&
GetMapConfig
()
const
{
return
*
map_config_
;
}
/**@brief Get the map node config. */
inline
const
BaseMapNodeConfig
&
GetMapNodeConfig
()
const
{
return
*
map_node_config_
;
}
/**@brief Get the map node index. */
inline
const
MapNodeIndex
&
GetMapNodeIndex
()
const
{
return
map_node_config_
->
node_index_
;
}
/**@brief Set if the map node is reserved. */
inline
void
SetIsReserved
(
bool
is_reserved
)
{
is_reserved_
=
is_reserved
;
}
/**@brief Get if the map node is reserved. */
inline
bool
GetIsReserved
()
const
{
return
is_reserved_
;
}
/**@brief Get if the map data has changed. */
inline
bool
GetIsChanged
()
const
{
return
is_changed_
;
}
/**@brief Set if the map node data has changed. */
inline
void
SetIsChanged
(
bool
is
)
{
is_changed_
=
is
;
}
/**@brief Get if the map node data is ready*/
inline
bool
GetIsReady
()
const
{
return
data_is_ready_
;
}
inline
const
Eigen
::
Vector2d
&
GetLeftTopCorner
()
const
{
return
left_top_corner_
;
}
/**@brief Set the left top corner of the map node. */
inline
void
SetLeftTopCorner
(
double
x
,
double
y
)
{
left_top_corner_
[
0
]
=
x
;
left_top_corner_
[
1
]
=
y
;
}
/**@brief Get the resolution of this map nodex. */
inline
float
GetMapResolution
()
const
{
return
this
->
map_config_
->
map_resolutions_
[
map_node_config_
->
node_index_
.
resolution_id_
];
}
static
Eigen
::
Vector2d
GetLeftTopCorner
(
const
BaseMapConfig
&
option
,
const
MapNodeIndex
&
index
);
protected:
/**@brief Try to create the map directory. */
bool
CreateMapDirectory
(
const
std
::
string
&
path
)
const
;
/**@brief Try to create the map directory recursively. */
bool
CreateMapDirectoryRecursively
(
const
std
::
vector
<
std
::
string
>&
paths
)
const
;
/**@brief Try to check the map directory recursively. */
bool
CheckMapDirectoryRecursively
(
const
std
::
vector
<
std
::
string
>&
paths
)
const
;
/**@brief Load the map cell from a binary chunk.
*/
virtual
bool
LoadBinary
(
FILE
*
file
);
/**@brief Create the binary. Serialization of the object.
*/
virtual
bool
CreateBinary
(
FILE
*
file
)
const
;
/**@brief Get the binary size of the object. */
virtual
size_t
GetBinarySize
()
const
;
/**@brief Load the map node header from a binary chunk.
* @param <return> The size read (the real size of header).
*/
virtual
size_t
LoadHeaderBinary
(
const
unsigned
char
*
buf
);
/**@brief Create the binary header.
* @param <buf, buf_size> The buffer and its size.
* @param <return> The required or the used size of is returned.
*/
virtual
size_t
CreateHeaderBinary
(
unsigned
char
*
buf
,
size_t
buf_size
)
const
;
/**@brief Get the size of the header in bytes. */
virtual
size_t
GetHeaderBinarySize
()
const
;
/**@brief Load the map node body from a binary chunk.
* @param <return> The size read (the real size of body).
*/
virtual
size_t
LoadBodyBinary
(
std
::
vector
<
unsigned
char
>*
buf
);
/**@brief Create the binary body.
* @param <buf, buf_size> The buffer and its size.
* @param <return> The required or the used size of is returned.
*/
virtual
size_t
CreateBodyBinary
(
std
::
vector
<
unsigned
char
>*
buf
)
const
;
/**@brief Get the size of the body in bytes. */
virtual
size_t
GetBodyBinarySize
()
const
;
/**@brief The map settings. */
const
BaseMapConfig
*
map_config_
=
nullptr
;
/**@brief The index of this node*/
MapNodeIndex
index_
;
/**@brief The left top corner of the map node in the global coordinate system.
*/
Eigen
::
Vector2d
left_top_corner_
;
/**@brief The map node config. */
BaseMapNodeConfig
*
map_node_config_
=
nullptr
;
/**@brief The data structure of the map datas, which is a matrix. */
BaseMapMatrix
*
map_matrix_
=
nullptr
;
/**@brief The class to load and create map matrix binary. */
BaseMapMatrixHandler
*
map_matrix_handler_
=
nullptr
;
/**@brief If the node is reserved in map. */
bool
is_reserved_
=
false
;
/**@brief Has the map node been changed. */
bool
is_changed_
=
false
;
/* *@brief Indicate map node data is ready*/
bool
data_is_ready_
=
false
;
/**@brief The body binary size in file. */
mutable
size_t
file_body_binary_size_
=
0
;
mutable
size_t
uncompressed_file_body_size_
=
0
;
/**@bried The compression strategy. */
CompressionStrategy
*
compression_strategy_
=
nullptr
;
};
}
// namespace msf
}
// namespace localization
}
// namespace apollo
modules/localization/msf/local_pyramid_map/base_map/base_map_pool.cc
0 → 100644
浏览文件 @
62e54d7c
/******************************************************************************
* 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/base_map/base_map_pool.h"
#include "modules/localization/msf/local_map/base_map/base_map_config.h"
#include "modules/localization/msf/local_map/base_map/base_map_node.h"
#include "modules/localization/msf/local_map/base_map/base_map_node_index.h"
namespace
apollo
{
namespace
localization
{
namespace
msf
{
BaseMapNodePool
::
BaseMapNodePool
(
unsigned
int
pool_size
,
unsigned
int
thread_size
)
:
pool_size_
(
pool_size
)
{}
BaseMapNodePool
::~
BaseMapNodePool
()
{
Release
();
}
void
BaseMapNodePool
::
Initial
(
const
BaseMapConfig
*
map_config
,
bool
is_fixed_size
)
{
is_fixed_size_
=
is_fixed_size
;
map_config_
=
map_config
;
for
(
unsigned
int
i
=
0
;
i
<
pool_size_
;
++
i
)
{
BaseMapNode
*
node
=
AllocNewMapNode
();
InitNewMapNode
(
node
);
free_list_
.
push_back
(
node
);
}
}
void
BaseMapNodePool
::
Release
()
{
if
(
node_reset_workers_
.
valid
())
{
node_reset_workers_
.
get
();
}
typename
std
::
list
<
BaseMapNode
*>::
iterator
i
=
free_list_
.
begin
();
while
(
i
!=
free_list_
.
end
())
{
FinalizeMapNode
(
*
i
);
DellocMapNode
(
*
i
);
i
++
;
}
free_list_
.
clear
();
typename
std
::
set
<
BaseMapNode
*>::
iterator
j
=
busy_nodes_
.
begin
();
while
(
j
!=
busy_nodes_
.
end
())
{
FinalizeMapNode
(
*
j
);
DellocMapNode
(
*
j
);
j
++
;
}
busy_nodes_
.
clear
();
pool_size_
=
0
;
}
BaseMapNode
*
BaseMapNodePool
::
AllocMapNode
()
{
if
(
free_list_
.
empty
())
{
if
(
node_reset_workers_
.
valid
())
{
node_reset_workers_
.
wait
();
}
}
boost
::
unique_lock
<
boost
::
mutex
>
lock
(
mutex_
);
if
(
free_list_
.
empty
())
{
if
(
is_fixed_size_
)
{
return
NULL
;
}
BaseMapNode
*
node
=
AllocNewMapNode
();
InitNewMapNode
(
node
);
pool_size_
++
;
busy_nodes_
.
insert
(
node
);
return
node
;
}
else
{
BaseMapNode
*
node
=
free_list_
.
front
();
free_list_
.
pop_front
();
busy_nodes_
.
insert
(
node
);
return
node
;
}
}
void
BaseMapNodePool
::
FreeMapNode
(
BaseMapNode
*
map_node
)
{
node_reset_workers_
=
cyber
::
Async
(
&
BaseMapNodePool
::
FreeMapNodeTask
,
this
,
map_node
);
}
void
BaseMapNodePool
::
FreeMapNodeTask
(
BaseMapNode
*
map_node
)
{
FinalizeMapNode
(
map_node
);
ResetMapNode
(
map_node
);
{
boost
::
unique_lock
<
boost
::
mutex
>
lock
(
mutex_
);
typename
std
::
set
<
BaseMapNode
*>::
iterator
f
=
busy_nodes_
.
find
(
map_node
);
if
(
f
==
busy_nodes_
.
end
())
{
throw
"[BaseMapNodePool::free_map_node_task] f == busy_nodes_.end()"
;
}
free_list_
.
push_back
(
*
f
);
busy_nodes_
.
erase
(
f
);
}
}
void
BaseMapNodePool
::
InitNewMapNode
(
BaseMapNode
*
node
)
{
node
->
Init
(
map_config_
);
return
;
}
void
BaseMapNodePool
::
FinalizeMapNode
(
BaseMapNode
*
node
)
{
node
->
Finalize
();
}
void
BaseMapNodePool
::
DellocMapNode
(
BaseMapNode
*
node
)
{
delete
node
;
}
void
BaseMapNodePool
::
ResetMapNode
(
BaseMapNode
*
node
)
{
node
->
ResetMapNode
();
}
}
// namespace msf
}
// namespace localization
}
// namespace apollo
modules/localization/msf/local_pyramid_map/base_map/base_map_pool.h
0 → 100644
浏览文件 @
62e54d7c
/******************************************************************************
* 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 <list>
#include <set>
#include "boost/thread.hpp"
#include "cyber/task/task.h"
#include "modules/localization/msf/local_map/base_map/base_map_fwd.h"
namespace
apollo
{
namespace
localization
{
namespace
msf
{
/**@brief The memory pool for the data structure of BaseMapNode. */
class
BaseMapNodePool
{
public:
/**@brief Constructor
* @param <pool_size> The memory pool size.
* @param <thread_size> The thread pool size.
*/
BaseMapNodePool
(
unsigned
int
pool_size
,
unsigned
int
thread_size
);
/**@brief Destructor */
virtual
~
BaseMapNodePool
();
/**@brief Initialize the pool.
* @param <map_config> The map option.
* @param <is_fixed_size> The flag of pool auto expand.
* */
void
Initial
(
const
BaseMapConfig
*
map_config
,
bool
is_fixed_size
=
true
);
/**@brief Release the pool. */
void
Release
();
/**@brief Get a MapNode object from memory pool.
* @param <return> The MapNode object.
* */
BaseMapNode
*
AllocMapNode
();
/**@brief Release MapNode object to memory pool.
* @param <map_node> The released MapNode object.
* */
void
FreeMapNode
(
BaseMapNode
*
map_node
);
/**@brief Get the size of pool. */
unsigned
int
GetPoolSize
()
{
return
pool_size_
;
}
private:
/**@brief The task function of the thread pool for release node.
* @param <map_node> The released MapNode object.
* */
void
FreeMapNodeTask
(
BaseMapNode
*
map_node
);
/**@brief new a map node. */
virtual
BaseMapNode
*
AllocNewMapNode
()
=
0
;
/**@brief init a map node. */
virtual
void
InitNewMapNode
(
BaseMapNode
*
node
);
/**@brief finalize a map node, before reset or delloc the map node. */
virtual
void
FinalizeMapNode
(
BaseMapNode
*
node
);
/**@brief delloc a map node. */
virtual
void
DellocMapNode
(
BaseMapNode
*
node
);
/**@brief reset a map node. */
virtual
void
ResetMapNode
(
BaseMapNode
*
node
);
protected:
/**@brief The flag of pool auto expand. */
bool
is_fixed_size_
=
0
;
/**@brief The list for free node. */
std
::
list
<
BaseMapNode
*>
free_list_
;
/**@brief The set for used node. */
std
::
set
<
BaseMapNode
*>
busy_nodes_
;
/**@brief The size of memory pool. */
unsigned
int
pool_size_
=
0
;
/**@brief The thread pool for release node. */
std
::
future
<
void
>
node_reset_workers_
;
/**@brief The mutex for release thread.*/
boost
::
mutex
mutex_
;
/**@brief The mutex for release thread.*/
const
BaseMapConfig
*
map_config_
=
nullptr
;
};
}
// namespace msf
}
// namespace localization
}
// namespace apollo
编辑
预览
Markdown
is supported
0%
请重试
或
添加新附件
.
添加附件
取消
You are about to add
0
people
to the discussion. Proceed with caution.
先完成此消息的编辑!
取消
想要评论请
注册
或
登录