Apollo 10.0
自动驾驶开放平台
apollo::localization::msf::BaseMap类 参考

The data structure of the base map. 更多...

#include <base_map.h>

类 apollo::localization::msf::BaseMap 继承关系图:
apollo::localization::msf::BaseMap 的协作图:

Public 成员函数

 BaseMap (BaseMapConfig *map_config)
 The constructor.
 
virtual ~BaseMap ()
 The destructor.
 
virtual void InitMapNodeCaches (int cacheL1_size, int cahceL2_size)
 Init load threadpool and preload threadpool.
 
BaseMapNodeGetMapNode (const MapNodeIndex &index)
 Get the map node, if it's not in the cache, return false.
 
BaseMapNodeGetMapNodeSafe (const MapNodeIndex &index)
 Return the map node, if it's not in the cache, safely load it.
 
bool IsMapNodeExist (const MapNodeIndex &index) const
 Check if the map node in the cache.
 
bool SetMapFolderPath (const std::string folder_path)
 Set the directory of the map.
 
void AddDataset (const std::string dataset_path)
 Add a dataset path to the map config.
 
virtual void PreloadMapArea (const Eigen::Vector3d &location, const Eigen::Vector3d &trans_diff, unsigned int resolution_id, unsigned int zone_id)
 Preload map nodes for the next frame location calculation.
 
virtual bool LoadMapArea (const Eigen::Vector3d &seed_pt3d, unsigned int resolution_id, unsigned int zone_id, int filter_size_x, int filter_size_y)
 Load map nodes for the location calculate of this frame.
 
void AttachMapNodePool (BaseMapNodePool *p_map_node_pool)
 Attach map node pointer.
 
void WriteBinary (FILE *file)
 Write all the map nodes to a single binary file stream.
 
void LoadBinary (FILE *file, std::string map_folder_path="")
 Load all the map nodes from a single binary file stream.
 
const BaseMapConfigGetConfig () const
 Get the map config.
 
BaseMapConfigGetConfig ()
 Get the map config.
 

Protected 成员函数

void LoadMapNodes (std::set< MapNodeIndex > *map_ids)
 Load map node by index.
 
void PreloadMapNodes (std::set< MapNodeIndex > *map_ids)
 Load map node by index.
 
void LoadMapNodeThreadSafety (MapNodeIndex index, bool is_reserved=false)
 Load map node by index, thread_safety.
 

Protected 属性

BaseMapConfigmap_config_
 The map settings.
 
std::list< MapNodeIndexmap_nodes_disk_
 All the map nodes in the Map (in the disk).
 
MapNodeCache< MapNodeIndex, BaseMapNode >::DestroyFunc destroy_func_lvl1_
 
MapNodeCache< MapNodeIndex, BaseMapNode >::DestroyFunc destroy_func_lvl2_
 
MapNodeCache< MapNodeIndex, BaseMapNode > * map_node_cache_lvl1_
 The cache for map node preload.
 
MapNodeCache< MapNodeIndex, BaseMapNode > * map_node_cache_lvl2_
 brief The dynamic map node preloading thread pool pointer.
 
BaseMapNodePoolmap_node_pool_
 The map node memory pool pointer.
 
std::set< MapNodeIndexmap_preloading_task_index_
 @bried Keep the index of preloading nodes.
 
boost::recursive_mutex map_load_mutex_
 The mutex for preload map node.
 

详细描述

The data structure of the base map.

在文件 base_map.h35 行定义.

构造及析构函数说明

◆ BaseMap()

apollo::localization::msf::BaseMap::BaseMap ( BaseMapConfig map_config)
explicit

The constructor.

在文件 base_map.cc29 行定义.

30 : map_config_(map_config),
31 map_node_cache_lvl1_(nullptr),
32 map_node_cache_lvl2_(nullptr),
33 map_node_pool_(nullptr) {}
BaseMapConfig * map_config_
The map settings.
Definition base_map.h:105
MapNodeCache< MapNodeIndex, BaseMapNode > * map_node_cache_lvl2_
brief The dynamic map node preloading thread pool pointer.
Definition base_map.h:114
BaseMapNodePool * map_node_pool_
The map node memory pool pointer.
Definition base_map.h:116
MapNodeCache< MapNodeIndex, BaseMapNode > * map_node_cache_lvl1_
The cache for map node preload.
Definition base_map.h:112

◆ ~BaseMap()

apollo::localization::msf::BaseMap::~BaseMap ( )
virtual

The destructor.

在文件 base_map.cc35 行定义.

35 {
38 map_node_cache_lvl1_ = nullptr;
39 }
42 map_node_cache_lvl2_ = nullptr;
43 }
44}

成员函数说明

◆ AddDataset()

void apollo::localization::msf::BaseMap::AddDataset ( const std::string  dataset_path)

Add a dataset path to the map config.

在文件 base_map.cc114 行定义.

114 {
115 map_config_->map_datasets_.push_back(dataset_path);
116 std::string config_path = map_config_->map_folder_path_ + "/config.xml";
117 map_config_->Save(config_path);
118}
std::vector< std::string > map_datasets_
The datasets that contributed to the map.
std::string map_folder_path_
The map folder path.
bool Save(const std::string file_path)
Save the map option to a XML file.

◆ AttachMapNodePool()

void apollo::localization::msf::BaseMap::AttachMapNodePool ( BaseMapNodePool p_map_node_pool)

Attach map node pointer.

在文件 base_map.cc232 行定义.

232 {
233 map_node_pool_ = map_node_pool;
234}

◆ GetConfig() [1/2]

BaseMapConfig & apollo::localization::msf::BaseMap::GetConfig ( )
inline

Get the map config.

在文件 base_map.h94 行定义.

94{ return *map_config_; }

◆ GetConfig() [2/2]

const BaseMapConfig & apollo::localization::msf::BaseMap::GetConfig ( ) const
inline

Get the map config.

在文件 base_map.h92 行定义.

92{ return *map_config_; }

◆ GetMapNode()

BaseMapNode * apollo::localization::msf::BaseMap::GetMapNode ( const MapNodeIndex index)

Get the map node, if it's not in the cache, return false.

在文件 base_map.cc61 行定义.

61 {
62 BaseMapNode* node = nullptr;
63 map_node_cache_lvl1_->Get(index, &node);
64 return node;
65}

◆ GetMapNodeSafe()

BaseMapNode * apollo::localization::msf::BaseMap::GetMapNodeSafe ( const MapNodeIndex index)

Return the map node, if it's not in the cache, safely load it.

在文件 base_map.cc67 行定义.

67 {
68 BaseMapNode* node = nullptr;
69 // try get from cacheL1
70 if (map_node_cache_lvl1_->Get(index, &node)) {
71 return node;
72 }
73
74 // try get from cacheL2
75 boost::unique_lock<boost::recursive_mutex> lock(map_load_mutex_);
76 if (map_node_cache_lvl2_->Get(index, &node)) {
77 node->SetIsReserved(true);
78
79 map_node_cache_lvl1_->Put(index, node);
80 return node;
81 }
82 lock.unlock();
83
84 // load from disk
85 AERROR << "GetMapNodeSafe: This node don't exist in cache! ";
86 AERROR << "load this node from disk now! index = " << index;
87 LoadMapNodeThreadSafety(index, true);
88 boost::unique_lock<boost::recursive_mutex> lock2(map_load_mutex_);
89 map_node_cache_lvl2_->Get(index, &node);
90 lock2.unlock();
91
92 map_node_cache_lvl1_->Put(index, node);
93 return node;
94}
boost::recursive_mutex map_load_mutex_
The mutex for preload map node.
Definition base_map.h:120
void LoadMapNodeThreadSafety(MapNodeIndex index, bool is_reserved=false)
Load map node by index, thread_safety.
Definition base_map.cc:236
#define AERROR
Definition log.h:44

◆ InitMapNodeCaches()

void apollo::localization::msf::BaseMap::InitMapNodeCaches ( int  cacheL1_size,
int  cahceL2_size 
)
virtual

Init load threadpool and preload threadpool.

在文件 base_map.cc46 行定义.

46 {
49 std::placeholders::_1);
52 std::placeholders::_1);
53 ACHECK(map_node_cache_lvl1_ == nullptr);
54 ACHECK(map_node_cache_lvl2_ == nullptr);
55 map_node_cache_lvl1_ = new MapNodeCache<MapNodeIndex, BaseMapNode>(
56 cacheL1_size, destroy_func_lvl1_);
57 map_node_cache_lvl2_ = new MapNodeCache<MapNodeIndex, BaseMapNode>(
58 cacheL2_size, destroy_func_lvl2_);
59}
MapNodeCache< MapNodeIndex, BaseMapNode >::DestroyFunc destroy_func_lvl2_
Definition base_map.h:110
MapNodeCache< MapNodeIndex, BaseMapNode >::DestroyFunc destroy_func_lvl1_
Definition base_map.h:109
static bool CacheL2Destroy(Element *value)
static bool CacheL1Destroy(Element *value)
#define ACHECK(cond)
Definition log.h:80

◆ IsMapNodeExist()

bool apollo::localization::msf::BaseMap::IsMapNodeExist ( const MapNodeIndex index) const

Check if the map node in the cache.

在文件 base_map.cc97 行定义.

97 {
98 return map_node_cache_lvl1_->IsExist(index);
99}

◆ LoadBinary()

void apollo::localization::msf::BaseMap::LoadBinary ( FILE *  file,
std::string  map_folder_path = "" 
)

Load all the map nodes from a single binary file stream.

It's for binary streaming or packing.

参数
<map_folder_path>The new map folder path to save the map.
<return>The size read (the real size of body).

◆ LoadMapArea()

bool apollo::localization::msf::BaseMap::LoadMapArea ( const Eigen::Vector3d &  seed_pt3d,
unsigned int  resolution_id,
unsigned int  zone_id,
int  filter_size_x,
int  filter_size_y 
)
virtual

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.

top left

top center

top right

middle left

middle center

middle right

bottom left

bottom center

bottom right

apollo::localization::msf::LosslessMap, apollo::localization::msf::LossyMap2D , 以及 apollo::localization::msf::NdtMap 重载.

在文件 base_map.cc407 行定义.

409 {
410 CHECK_NOTNULL(map_node_pool_);
411 std::set<MapNodeIndex> map_ids;
412 float map_pixel_resolution =
413 this->map_config_->map_resolutions_[resolution_id];
415 Eigen::Vector3d pt_top_left;
416 pt_top_left[0] = seed_pt3d[0] -
417 (static_cast<float>(this->map_config_->map_node_size_x_) *
418 map_pixel_resolution / 2.0f) -
419 static_cast<float>(filter_size_x / 2) * map_pixel_resolution;
420 pt_top_left[1] = seed_pt3d[1] -
421 (static_cast<float>(this->map_config_->map_node_size_y_) *
422 map_pixel_resolution / 2.0f) -
423 static_cast<float>(filter_size_y / 2) * map_pixel_resolution;
424 pt_top_left[2] = 0;
426 *(this->map_config_), pt_top_left, resolution_id, zone_id);
427 map_ids.insert(map_id);
428
430 Eigen::Vector3d pt_top_center;
431 pt_top_center[0] = seed_pt3d[0];
432 pt_top_center[1] = pt_top_left[1];
433 pt_top_center[2] = 0;
434 map_id = MapNodeIndex::GetMapNodeIndex(*(this->map_config_), pt_top_center,
435 resolution_id, zone_id);
436 map_ids.insert(map_id);
437
439 Eigen::Vector3d pt_top_right;
440 pt_top_right[0] =
441 seed_pt3d[0] +
442 (static_cast<float>(this->map_config_->map_node_size_x_) *
443 map_pixel_resolution / 2.0f) +
444 static_cast<float>(filter_size_x / 2) * map_pixel_resolution;
445 pt_top_right[1] = pt_top_left[1];
446 pt_top_left[2] = 0;
447 map_id = MapNodeIndex::GetMapNodeIndex(*(this->map_config_), pt_top_right,
448 resolution_id, zone_id);
449 map_ids.insert(map_id);
450
452 Eigen::Vector3d pt_middle_left;
453 pt_middle_left[0] = pt_top_left[0];
454 pt_middle_left[1] = seed_pt3d[1];
455 pt_middle_left[2] = 0;
456 map_id = MapNodeIndex::GetMapNodeIndex(*(this->map_config_), pt_middle_left,
457 resolution_id, zone_id);
458 map_ids.insert(map_id);
459
461 map_id = MapNodeIndex::GetMapNodeIndex(*(this->map_config_), seed_pt3d,
462 resolution_id, zone_id);
463 map_ids.insert(map_id);
464
466 Eigen::Vector3d pt_middle_right;
467 pt_middle_right[0] = pt_top_right[0];
468 pt_middle_right[1] = seed_pt3d[1];
469 pt_middle_right[2] = 0;
470 map_id = MapNodeIndex::GetMapNodeIndex(*(this->map_config_), pt_middle_right,
471 resolution_id, zone_id);
472 map_ids.insert(map_id);
473
475 Eigen::Vector3d pt_bottom_left;
476 pt_bottom_left[0] = pt_top_left[0];
477 pt_bottom_left[1] =
478 seed_pt3d[1] +
479 (static_cast<float>(this->map_config_->map_node_size_y_) *
480 map_pixel_resolution / 2.0f) +
481 static_cast<float>(filter_size_y / 2) * map_pixel_resolution;
482 pt_bottom_left[2] = 0;
483 map_id = MapNodeIndex::GetMapNodeIndex(*(this->map_config_), pt_bottom_left,
484 resolution_id, zone_id);
485 map_ids.insert(map_id);
486
488 Eigen::Vector3d pt_bottom_center;
489 pt_bottom_center[0] = seed_pt3d[0];
490 pt_bottom_center[1] = pt_bottom_left[1];
491 pt_bottom_center[2] = 0;
492 map_id = MapNodeIndex::GetMapNodeIndex(*(this->map_config_), pt_bottom_center,
493 resolution_id, zone_id);
494 map_ids.insert(map_id);
495
497 Eigen::Vector3d pt_bottom_right;
498 pt_bottom_right[0] = pt_top_right[0];
499 pt_bottom_right[1] = pt_bottom_left[1];
500 pt_bottom_right[2] = 0;
501 map_id = MapNodeIndex::GetMapNodeIndex(*(this->map_config_), pt_bottom_right,
502 resolution_id, zone_id);
503 map_ids.insert(map_id);
504
505 this->LoadMapNodes(&map_ids);
506 return true;
507}
std::vector< float > map_resolutions_
The pixel resolutions in the map in meters.
unsigned int map_node_size_y_
The map node size in pixels.
unsigned int map_node_size_x_
The map node size in pixels.
void LoadMapNodes(std::set< MapNodeIndex > *map_ids)
Load map node by index.
Definition base_map.cc:120
static MapNodeIndex GetMapNodeIndex(const BaseMapConfig &option, const Eigen::Vector3d &coordinate, unsigned int resolution_id, int zone_id)
Construct a map node index, given a global coordinate, eigen version.
apollo::localization::msf::pyramid_map::MapNodeIndex MapNodeIndex

◆ LoadMapNodes()

void apollo::localization::msf::BaseMap::LoadMapNodes ( std::set< MapNodeIndex > *  map_ids)
protected

Load map node by index.

在文件 base_map.cc120 行定义.

120 {
121 CHECK_LE(map_ids->size(), map_node_cache_lvl1_->Capacity());
122 // check in cacheL1
123 typename std::set<MapNodeIndex>::iterator itr = map_ids->begin();
124 while (itr != map_ids->end()) {
125 if (map_node_cache_lvl1_->IsExist(*itr)) {
126 // std::cout << "LoadMapNodes find in L1 cache" << std::endl;
127 boost::unique_lock<boost::recursive_mutex> lock(map_load_mutex_);
128 map_node_cache_lvl2_->IsExist(*itr); // fresh lru list
129 lock.unlock();
130 itr = map_ids->erase(itr);
131 } else {
132 ++itr;
133 }
134 }
135
136 // check in cacheL2
137 itr = map_ids->begin();
138 BaseMapNode* node = nullptr;
139 boost::unique_lock<boost::recursive_mutex> lock(map_load_mutex_);
140 while (itr != map_ids->end()) {
141 if (map_node_cache_lvl2_->Get(*itr, &node)) {
142 // std::cout << "LoadMapNodes find in L2 cache" << std::endl;
143 node->SetIsReserved(true);
144 map_node_cache_lvl1_->Put(*itr, node);
145 itr = map_ids->erase(itr);
146 } else {
147 ++itr;
148 }
149 }
150 lock.unlock();
151
152 // load from disk sync
153 std::vector<std::future<void>> load_futures;
154 itr = map_ids->begin();
155 while (itr != map_ids->end()) {
156 AERROR << "Preload map node failed!";
157 load_futures.emplace_back(
158 cyber::Async(&BaseMap::LoadMapNodeThreadSafety, this, *itr, true));
159 ++itr;
160 }
161
162 for (auto& future : load_futures) {
163 if (future.valid()) {
164 future.get();
165 }
166 }
167 // check in cacheL2 again
168 itr = map_ids->begin();
169 node = nullptr;
170 boost::unique_lock<boost::recursive_mutex> lock2(map_load_mutex_);
171 while (itr != map_ids->end()) {
172 if (map_node_cache_lvl2_->Get(*itr, &node)) {
173 AINFO << "LoadMapNodes: preload missed, load this node in main thread.\n"
174 << *itr;
175 node->SetIsReserved(true);
176 map_node_cache_lvl1_->Put(*itr, node);
177 itr = map_ids->erase(itr);
178 } else {
179 ++itr;
180 }
181 }
182 lock2.unlock();
183
184 ACHECK(map_ids->empty());
185}
#define AINFO
Definition log.h:42

◆ LoadMapNodeThreadSafety()

void apollo::localization::msf::BaseMap::LoadMapNodeThreadSafety ( MapNodeIndex  index,
bool  is_reserved = false 
)
protected

Load map node by index, thread_safety.

在文件 base_map.cc236 行定义.

236 {
237 BaseMapNode* map_node = nullptr;
238 while (map_node == nullptr) {
239 map_node = map_node_pool_->AllocMapNode();
240 if (map_node == nullptr) {
241 boost::unique_lock<boost::recursive_mutex> lock(map_load_mutex_);
242 BaseMapNode* node_remove = map_node_cache_lvl2_->ClearOne();
243 if (node_remove) {
244 map_node_pool_->FreeMapNode(node_remove);
245 }
246 }
247 }
248 map_node->Init(map_config_, index, false);
249 if (!map_node->Load()) {
250 AERROR << "Created map node: " << index;
251 } else {
252 AERROR << " Loaded map node: " << index;
253 }
254 map_node->SetIsReserved(is_reserved);
255
256 boost::unique_lock<boost::recursive_mutex> lock(map_load_mutex_);
257 BaseMapNode* node_remove = map_node_cache_lvl2_->Put(index, map_node);
258 // if the node already added into cacheL2, erase it from preloading set
259 auto itr = map_preloading_task_index_.find(index);
260 if (itr != map_preloading_task_index_.end()) {
262 }
263 if (node_remove) {
264 map_node_pool_->FreeMapNode(node_remove);
265 }
266}
BaseMapNode * AllocMapNode()
Get a MapNode object from memory pool.
void FreeMapNode(BaseMapNode *map_node)
Release MapNode object to memory pool.
std::set< MapNodeIndex > map_preloading_task_index_
@bried Keep the index of preloading nodes.
Definition base_map.h:118

◆ PreloadMapArea()

void apollo::localization::msf::BaseMap::PreloadMapArea ( const Eigen::Vector3d &  location,
const Eigen::Vector3d &  trans_diff,
unsigned int  resolution_id,
unsigned int  zone_id 
)
virtual

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.

top left

top center

top right

middle left

middle center

middle right

bottom left

bottom center

bottom right

apollo::localization::msf::LosslessMap, apollo::localization::msf::LossyMap2D , 以及 apollo::localization::msf::NdtMap 重载.

在文件 base_map.cc268 行定义.

270 {
271 CHECK_NOTNULL(map_node_pool_);
272
273 int x_direction = trans_diff[0] > 0 ? 1 : -1;
274 int y_direction = trans_diff[1] > 0 ? 1 : -1;
275
276 std::set<MapNodeIndex> map_ids;
277 float map_pixel_resolution =
278 this->map_config_->map_resolutions_[resolution_id];
280 Eigen::Vector3d pt_top_left;
281 pt_top_left[0] =
282 location[0] - (static_cast<float>(this->map_config_->map_node_size_x_) *
283 map_pixel_resolution / 2.0f);
284 pt_top_left[1] =
285 location[1] - (static_cast<float>(this->map_config_->map_node_size_y_) *
286 map_pixel_resolution / 2.0f);
287 pt_top_left[2] = 0;
289 *(this->map_config_), pt_top_left, resolution_id, zone_id);
290 map_ids.insert(map_id);
291
293 Eigen::Vector3d pt_top_center;
294 pt_top_center[0] = location[0];
295 pt_top_center[1] = pt_top_left[1];
296 pt_top_center[2] = 0;
297 map_id = MapNodeIndex::GetMapNodeIndex(*(this->map_config_), pt_top_center,
298 resolution_id, zone_id);
299 map_ids.insert(map_id);
300
302 Eigen::Vector3d pt_top_right;
303 pt_top_right[0] =
304 location[0] + (static_cast<float>(this->map_config_->map_node_size_x_) *
305 map_pixel_resolution / 2.0);
306 pt_top_right[1] = pt_top_left[1];
307 pt_top_right[2] = 0;
308 map_id = MapNodeIndex::GetMapNodeIndex(*(this->map_config_), pt_top_right,
309 resolution_id, zone_id);
310 map_ids.insert(map_id);
311
313 Eigen::Vector3d pt_middle_left;
314 pt_middle_left[0] = pt_top_left[0];
315 pt_middle_left[1] = location[1];
316 pt_middle_left[2] = 0;
317 map_id = MapNodeIndex::GetMapNodeIndex(*(this->map_config_), pt_middle_left,
318 resolution_id, zone_id);
319 map_ids.insert(map_id);
320
322 map_id = MapNodeIndex::GetMapNodeIndex(*(this->map_config_), location,
323 resolution_id, zone_id);
324 map_ids.insert(map_id);
325
327 Eigen::Vector3d pt_middle_right;
328 pt_middle_right[0] = pt_top_right[0];
329 pt_middle_right[1] = pt_middle_left[1];
330 pt_middle_right[2] = 0;
331 map_id = MapNodeIndex::GetMapNodeIndex(*(this->map_config_), pt_middle_right,
332 resolution_id, zone_id);
333 map_ids.insert(map_id);
334
336 Eigen::Vector3d pt_bottom_left;
337 pt_bottom_left[0] = pt_top_left[0];
338 pt_bottom_left[1] =
339 location[1] + (static_cast<float>(this->map_config_->map_node_size_y_) *
340 map_pixel_resolution / 2.0);
341 pt_bottom_left[2] = 0;
342 map_id = MapNodeIndex::GetMapNodeIndex(*(this->map_config_), pt_bottom_left,
343 resolution_id, zone_id);
344 map_ids.insert(map_id);
345
347 Eigen::Vector3d pt_bottom_center;
348 pt_bottom_center[0] = pt_top_center[0];
349 pt_bottom_center[1] = pt_bottom_left[1];
350 pt_bottom_center[2] = 0;
351 map_id = MapNodeIndex::GetMapNodeIndex(*(this->map_config_), pt_bottom_center,
352 resolution_id, zone_id);
353 map_ids.insert(map_id);
354
356 Eigen::Vector3d pt_bottom_right;
357 pt_bottom_right[0] = pt_top_right[0];
358 pt_bottom_right[1] = pt_bottom_left[1];
359 pt_bottom_right[2] = 0;
360 map_id = MapNodeIndex::GetMapNodeIndex(*(this->map_config_), pt_bottom_right,
361 resolution_id, zone_id);
362 map_ids.insert(map_id);
363
364 for (int i = -1; i < 2; ++i) {
365 Eigen::Vector3d pt;
366 pt[0] = location[0] + x_direction * 1.5 *
368 map_pixel_resolution;
369 pt[1] = location[1] + static_cast<double>(i) *
371 map_pixel_resolution;
372 pt[2] = 0;
373 map_id = MapNodeIndex::GetMapNodeIndex(*(this->map_config_), pt,
374 resolution_id, zone_id);
375 map_ids.insert(map_id);
376 }
377 for (int i = -1; i < 2; ++i) {
378 Eigen::Vector3d pt;
379 pt[0] = location[0] + static_cast<double>(i) *
381 map_pixel_resolution;
382 pt[1] = location[1] + y_direction * 1.5 *
384 map_pixel_resolution;
385 pt[2] = 0;
386 map_id = MapNodeIndex::GetMapNodeIndex(*(this->map_config_), pt,
387 resolution_id, zone_id);
388 map_ids.insert(map_id);
389 }
390 {
391 Eigen::Vector3d pt;
392 pt[0] = location[0] + x_direction * 1.5 *
394 map_pixel_resolution;
395 pt[1] = location[1] + y_direction * 1.5 *
397 map_pixel_resolution;
398 pt[2] = 0;
399 map_id = MapNodeIndex::GetMapNodeIndex(*(this->map_config_), pt,
400 resolution_id, zone_id);
401 map_ids.insert(map_id);
402 }
403
404 this->PreloadMapNodes(&map_ids);
405}
void PreloadMapNodes(std::set< MapNodeIndex > *map_ids)
Load map node by index.
Definition base_map.cc:187

◆ PreloadMapNodes()

void apollo::localization::msf::BaseMap::PreloadMapNodes ( std::set< MapNodeIndex > *  map_ids)
protected

Load map node by index.

在文件 base_map.cc187 行定义.

187 {
188 DCHECK_LE(map_ids->size(), map_node_cache_lvl2_->Capacity());
189 // check in cacheL2
190 typename std::set<MapNodeIndex>::iterator itr = map_ids->begin();
191 while (itr != map_ids->end()) {
192 boost::unique_lock<boost::recursive_mutex> lock(map_load_mutex_);
193 bool is_exist = map_node_cache_lvl2_->IsExist(*itr);
194 lock.unlock();
195 if (is_exist) {
196 itr = map_ids->erase(itr);
197 } else {
198 ++itr;
199 }
200 }
201
202 // check whether in already preloading index set
203 itr = map_ids->begin();
204 auto preloading_itr = map_preloading_task_index_.end();
205 while (itr != map_ids->end()) {
206 boost::unique_lock<boost::recursive_mutex> lock(map_load_mutex_);
207 preloading_itr = map_preloading_task_index_.find(*itr);
208 lock.unlock();
209 if (preloading_itr !=
210 map_preloading_task_index_.end()) { // already preloading
211 itr = map_ids->erase(itr);
212 } else {
213 ++itr;
214 }
215 }
216
217 // load form disk sync
218 std::vector<std::future<void>> preload_futures;
219 itr = map_ids->begin();
220 AINFO << "Preload map node size: " << map_ids->size();
221 while (itr != map_ids->end()) {
222 AINFO << "Preload map node: " << *itr << std::endl;
223 boost::unique_lock<boost::recursive_mutex> lock3(map_load_mutex_);
224 map_preloading_task_index_.insert(*itr);
225 lock3.unlock();
226 preload_futures.emplace_back(
227 cyber::Async(&BaseMap::LoadMapNodeThreadSafety, this, *itr, false));
228 ++itr;
229 }
230}

◆ SetMapFolderPath()

bool apollo::localization::msf::BaseMap::SetMapFolderPath ( const std::string  folder_path)

Set the directory of the map.

在文件 base_map.cc101 行定义.

101 {
102 map_config_->map_folder_path_ = folder_path;
103
104 // Try to load the config
105 std::string config_path = map_config_->map_folder_path_ + "/config.xml";
106 if (system::IsExists(config_path)) {
107 map_config_->Load(config_path);
108 return true;
109 } else {
110 return false;
111 }
112}
bool Load(const std::string file_path)
Load the map option from a XML file.
static bool IsExists(const std::string &path)
Determine if the file or directory exists.

◆ WriteBinary()

void apollo::localization::msf::BaseMap::WriteBinary ( FILE *  file)

Write all the map nodes to a single binary file stream.

It's for binary streaming or packing.

参数
<buf,buf_size>The buffer and its size.
<return>The required or the used size of is returned.

类成员变量说明

◆ destroy_func_lvl1_

MapNodeCache<MapNodeIndex,BaseMapNode>::DestroyFunc apollo::localization::msf::BaseMap::destroy_func_lvl1_
protected

在文件 base_map.h109 行定义.

◆ destroy_func_lvl2_

MapNodeCache<MapNodeIndex,BaseMapNode>::DestroyFunc apollo::localization::msf::BaseMap::destroy_func_lvl2_
protected

在文件 base_map.h110 行定义.

◆ map_config_

BaseMapConfig* apollo::localization::msf::BaseMap::map_config_
protected

The map settings.

在文件 base_map.h105 行定义.

◆ map_load_mutex_

boost::recursive_mutex apollo::localization::msf::BaseMap::map_load_mutex_
protected

The mutex for preload map node.

在文件 base_map.h120 行定义.

◆ map_node_cache_lvl1_

MapNodeCache<MapNodeIndex, BaseMapNode>* apollo::localization::msf::BaseMap::map_node_cache_lvl1_
protected

The cache for map node preload.

在文件 base_map.h112 行定义.

◆ map_node_cache_lvl2_

MapNodeCache<MapNodeIndex, BaseMapNode>* apollo::localization::msf::BaseMap::map_node_cache_lvl2_
protected

brief The dynamic map node preloading thread pool pointer.

在文件 base_map.h114 行定义.

◆ map_node_pool_

BaseMapNodePool* apollo::localization::msf::BaseMap::map_node_pool_
protected

The map node memory pool pointer.

在文件 base_map.h116 行定义.

◆ map_nodes_disk_

std::list<MapNodeIndex> apollo::localization::msf::BaseMap::map_nodes_disk_
protected

All the map nodes in the Map (in the disk).

在文件 base_map.h107 行定义.

◆ map_preloading_task_index_

std::set<MapNodeIndex> apollo::localization::msf::BaseMap::map_preloading_task_index_
protected

@bried Keep the index of preloading nodes.

在文件 base_map.h118 行定义.


该类的文档由以下文件生成: