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

The data structure of a Node in the map. 更多...

#include <base_map_node.h>

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

Public 成员函数

 BaseMapNode ()
 Construct a map node.
 
 BaseMapNode (BaseMapMatrix *matrix, CompressionStrategy *strategy)
 Construct a map node.
 
virtual ~BaseMapNode ()
 Destruct a map node.
 
virtual void Init (const BaseMapConfig *map_config)=0
 Initialize the map node.
 
virtual void Init (const BaseMapConfig *map_config, const MapNodeIndex &index, bool create_map_cells=true)=0
 
virtual void InitMapMatrix (const BaseMapConfig *map_config)
 Initialize the map matrix.
 
virtual void Finalize ()
 call before deconstruction or reset.
 
virtual void ResetMapNode ()
 Reset map cells data.
 
bool Save ()
 Save the map node to the disk.
 
bool SaveIntensityImage () const
 Save intensity image of node.
 
bool SaveAltitudeImage () const
 Save altitude image of node.
 
bool Load ()
 Load the map node from the disk.
 
bool Load (const char *filename)
 
virtual bool GetCoordinate (const Eigen::Vector2d &coordinate, unsigned int *x, unsigned int *y) const
 Given the global coordinate, get the local 2D coordinate of the map cell matrix.
 
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
 Given the local 2D coordinate, return the global coordinate.
 
void SetMapNodeIndex (const MapNodeIndex &index)
 Set the map node index.
 
bool SaveIntensityImage (const std::string &path) const
 Save intensity image of node.
 
bool SaveAltitudeImage (const std::string &path) const
 Save altitude image of node.
 
const BaseMapMatrixGetMapCellMatrix () const
 Get map cell matrix.
 
BaseMapMatrixGetMapCellMatrix ()
 
const BaseMapConfigGetMapConfig () const
 Get the map settings.
 
const BaseMapNodeConfigGetMapNodeConfig () const
 Get the map node config.
 
const MapNodeIndexGetMapNodeIndex () const
 Get the map node index.
 
void SetIsReserved (bool is_reserved)
 Set if the map node is reserved.
 
bool GetIsReserved () const
 Get if the map node is reserved.
 
bool GetIsChanged () const
 Get if the map data has changed.
 
void SetIsChanged (bool is)
 Set if the map node data has changed.
 
bool GetIsReady () const
 Get if the map node data is ready
 
const Eigen::Vector2d & GetLeftTopCorner () const
 
void SetLeftTopCorner (double x, double y)
 Set the left top corner of the map node.
 
float GetMapResolution () const
 Get the resolution of this map nodex.
 

静态 Public 成员函数

static Eigen::Vector2d ComputeLeftTopCorner (const BaseMapConfig &config, const MapNodeIndex &index)
 
static Eigen::Vector2d GetLeftTopCorner (const BaseMapConfig &option, const MapNodeIndex &index)
 

Protected 成员函数

bool CreateMapDirectory (const std::string &path) const
 Try to create the map directory.
 
bool CreateMapDirectoryRecursively (const std::vector< std::string > &paths) const
 Try to create the map directory recursively.
 
bool CheckMapDirectoryRecursively (const std::vector< std::string > &paths) const
 Try to check the map directory recursively.
 
virtual bool LoadBinary (FILE *file)
 Load the map cell from a binary chunk.
 
virtual bool CreateBinary (FILE *file) const
 Create the binary.
 
virtual size_t GetBinarySize () const
 Get the binary size of the object.
 
virtual size_t LoadHeaderBinary (const unsigned char *buf)
 Load the map node header from a binary chunk.
 
virtual size_t CreateHeaderBinary (unsigned char *buf, size_t buf_size) const
 Create the binary header.
 
virtual size_t GetHeaderBinarySize () const
 Get the size of the header in bytes.
 
virtual size_t LoadBodyBinary (std::vector< unsigned char > *buf)
 Load the map node body from a binary chunk.
 
virtual size_t CreateBodyBinary (std::vector< unsigned char > *buf) const
 Create the binary body.
 
virtual size_t GetBodyBinarySize () const
 Get the size of the body in bytes.
 

Protected 属性

const BaseMapConfigmap_config_ = nullptr
 The map settings.
 
MapNodeIndex index_
 The index of this node
 
Eigen::Vector2d left_top_corner_
 The left top corner of the map node in the global coordinate system.
 
std::shared_ptr< BaseMapNodeConfigmap_node_config_ = nullptr
 The map node config.
 
std::shared_ptr< BaseMapMatrixmap_matrix_ = nullptr
 The data structure of the map datas, which is a matrix.
 
std::shared_ptr< BaseMapMatrixHandlermap_matrix_handler_ = nullptr
 The class to load and create map matrix binary.
 
bool is_reserved_ = false
 If the node is reserved in map.
 
bool is_changed_ = false
 Has the map node been changed.
 
bool data_is_ready_ = false
 
size_t file_body_binary_size_ = 0
 The body binary size in file.
 
size_t uncompressed_file_body_size_ = 0
 
std::shared_ptr< CompressionStrategycompression_strategy_ = nullptr
 @bried The compression strategy.
 

详细描述

The data structure of a Node in the map.

在文件 base_map_node.h37 行定义.

构造及析构函数说明

◆ BaseMapNode() [1/2]

apollo::localization::msf::pyramid_map::BaseMapNode::BaseMapNode ( )

Construct a map node.

在文件 base_map_node.cc33 行定义.

34 : map_config_(NULL),
35 map_matrix_(NULL),
38 is_changed_ = false;
39 data_is_ready_ = false;
40 is_reserved_ = false;
43}
std::shared_ptr< CompressionStrategy > compression_strategy_
@bried The compression strategy.
const BaseMapConfig * map_config_
The map settings.
bool is_changed_
Has the map node been changed.
bool is_reserved_
If the node is reserved in map.
std::shared_ptr< BaseMapMatrixHandler > map_matrix_handler_
The class to load and create map matrix binary.
std::shared_ptr< BaseMapMatrix > map_matrix_
The data structure of the map datas, which is a matrix.
size_t file_body_binary_size_
The body binary size in file.

◆ BaseMapNode() [2/2]

apollo::localization::msf::pyramid_map::BaseMapNode::BaseMapNode ( BaseMapMatrix matrix,
CompressionStrategy strategy 
)

Construct a map node.

在文件 base_map_node.cc45 行定义.

46 : map_config_(NULL),
47 map_matrix_(matrix),
49 compression_strategy_(strategy) {
50 is_changed_ = false;
51 data_is_ready_ = false;
52 is_reserved_ = false;
54}

◆ ~BaseMapNode()

apollo::localization::msf::pyramid_map::BaseMapNode::~BaseMapNode ( )
virtual

Destruct a map node.

在文件 base_map_node.cc56 行定义.

56{}

成员函数说明

◆ CheckMapDirectoryRecursively()

bool apollo::localization::msf::pyramid_map::BaseMapNode::CheckMapDirectoryRecursively ( const std::vector< std::string > &  paths) const
protected

Try to check the map directory recursively.

在文件 base_map_node.cc414 行定义.

415 {
416 std::string path = "";
417
418 for (unsigned int i = 0; i < paths.size(); ++i) {
419 path = path + paths[i];
420 if (!(cyber::common::DirectoryExists(path))) {
421 return false;
422 }
423 }
424
425 return true;
426}
bool DirectoryExists(const std::string &directory_path)
Check if the directory specified by directory_path exists and is indeed a directory.
Definition file.cc:207

◆ ComputeLeftTopCorner()

Eigen::Vector2d apollo::localization::msf::pyramid_map::BaseMapNode::ComputeLeftTopCorner ( const BaseMapConfig config,
const MapNodeIndex index 
)
static

在文件 base_map_node.cc356 行定义.

357 {
358 Eigen::Vector2d coord;
359 coord[0] = config.map_range_.GetMinX() +
360 static_cast<float>(config.map_node_size_x_) *
361 config.map_resolutions_[index.resolution_id_] *
362 static_cast<float>(index.n_);
363 coord[1] = config.map_range_.GetMinY() +
364 static_cast<float>(config.map_node_size_y_) *
365 config.map_resolutions_[index.resolution_id_] *
366 static_cast<float>(index.m_);
367 if (coord[0] >= config.map_range_.GetMaxX()) {
368 throw "[BaseMapNode::ComputeLeftTopCorner] coord[0]"
369 " >= config.map_range_.GetMaxX()";
370 }
371 if (coord[1] >= config.map_range_.GetMaxY()) {
372 throw "[BaseMapNode::compute_left_top_corner] coord[1]"
373 " >= config.map_range_.GetMaxY()";
374 }
375 return coord;
376}

◆ CreateBinary()

bool apollo::localization::msf::pyramid_map::BaseMapNode::CreateBinary ( FILE *  file) const
protectedvirtual

Create the binary.

Serialization of the object.

在文件 base_map_node.cc202 行定义.

202 {
203 size_t buf_size = GetBinarySize();
204 std::vector<unsigned char> buffer;
205 buffer.resize(buf_size);
206
207 size_t binary_size = 0;
208 std::vector<unsigned char> body_buffer;
209 size_t processed_size = CreateBodyBinary(&body_buffer);
210
211 if (map_node_config_->has_body_md5_) {
212 FileUtility::ComputeBinaryMd5(&body_buffer[0], body_buffer.size(),
213 map_node_config_->body_md5_);
214 }
215
216 if (processed_size == 0) {
217 return false;
218 }
219
220 // Create header
221 size_t header_size = GetHeaderBinarySize();
222 std::vector<unsigned char> header_buf(header_size);
223 processed_size = CreateHeaderBinary(&buffer[0], buf_size);
224
225 if (header_size != processed_size) {
226 return false;
227 }
228
229 size_t buffer_bias = processed_size;
230 buf_size -= processed_size;
231 binary_size += processed_size;
232 // Create body
233 memcpy(&buffer[buffer_bias], &body_buffer[0], body_buffer.size());
234 // write binary
235 binary_size += static_cast<unsigned int>(body_buffer.size());
236 fwrite(&buffer[0], 1, binary_size, file);
237
238 return true;
239}
static void ComputeBinaryMd5(const unsigned char *binary, size_t size, unsigned char res[kUcharMd5Length])
Compute file md5 given a binary chunk.
virtual size_t GetBinarySize() const
Get the binary size of the object.
std::shared_ptr< BaseMapNodeConfig > map_node_config_
The map node config.
virtual size_t GetHeaderBinarySize() const
Get the size of the header in bytes.
virtual size_t CreateHeaderBinary(unsigned char *buf, size_t buf_size) const
Create the binary header.
virtual size_t CreateBodyBinary(std::vector< unsigned char > *buf) const
Create the binary body.

◆ CreateBodyBinary()

size_t apollo::localization::msf::pyramid_map::BaseMapNode::CreateBodyBinary ( std::vector< unsigned char > *  buf) const
protectedvirtual

Create the binary body.

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

在文件 base_map_node.cc291 行定义.

291 {
292 if (compression_strategy_ == nullptr) {
293 size_t body_size = GetBodyBinarySize();
294 buf->resize(body_size);
296 map_matrix_handler_->CreateBinary(map_matrix_, &(*buf)[0], body_size);
298 }
299 std::vector<unsigned char> buf_uncompressed;
300 // Compute the uncompression binary body size
301 size_t body_size = GetBodyBinarySize();
302 buf_uncompressed.resize(body_size);
303 size_t binary_size = map_matrix_handler_->CreateBinary(
304 map_matrix_, &buf_uncompressed[0], body_size);
305 if (binary_size == 0) {
306 return 0;
307 }
308
309 compression_strategy_->Encode(&buf_uncompressed, buf);
310 file_body_binary_size_ = buf->size();
311
312 return buf->size();
313}
virtual size_t GetBodyBinarySize() const
Get the size of the body in bytes.

◆ CreateHeaderBinary()

size_t apollo::localization::msf::pyramid_map::BaseMapNode::CreateHeaderBinary ( unsigned char *  buf,
size_t  buf_size 
) const
protectedvirtual

Create the binary header.

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

在文件 base_map_node.cc263 行定义.

264 {
266 return map_node_config_->CreateBinary(buf, buf_size);
267}

◆ CreateMapDirectory()

bool apollo::localization::msf::pyramid_map::BaseMapNode::CreateMapDirectory ( const std::string &  path) const
protected

Try to create the map directory.

在文件 base_map_node.cc394 行定义.

394 {
397 }
398 return true;
399}
bool EnsureDirectory(const std::string &directory_path)
Check if a specified directory specified by directory_path exists.
Definition file.cc:299

◆ CreateMapDirectoryRecursively()

bool apollo::localization::msf::pyramid_map::BaseMapNode::CreateMapDirectoryRecursively ( const std::vector< std::string > &  paths) const
protected

Try to create the map directory recursively.

在文件 base_map_node.cc401 行定义.

402 {
403 std::string path = "";
404
405 for (unsigned int i = 0; i < paths.size(); ++i) {
406 path = path + paths[i];
407 if (!CreateMapDirectory(path)) {
408 return false;
409 }
410 }
411 return true;
412}
bool CreateMapDirectory(const std::string &path) const
Try to create the map directory.

◆ Finalize()

void apollo::localization::msf::pyramid_map::BaseMapNode::Finalize ( )
virtual

call before deconstruction or reset.

在文件 base_map_node.cc63 行定义.

63 {
64 if (is_changed_) {
65 Save();
66 AINFO << "Save Map Node to disk: " << map_node_config_->node_index_ << ".";
67 }
68}
bool Save()
Save the map node to the disk.
#define AINFO
Definition log.h:42

◆ GetBinarySize()

size_t apollo::localization::msf::pyramid_map::BaseMapNode::GetBinarySize ( ) const
protectedvirtual

Get the binary size of the object.

在文件 base_map_node.cc241 行定义.

241 {
242 // It is uncompressed binary size.
244}

◆ GetBodyBinarySize()

size_t apollo::localization::msf::pyramid_map::BaseMapNode::GetBodyBinarySize ( ) const
protectedvirtual

Get the size of the body in bytes.

在文件 base_map_node.cc315 行定义.

315 {
316 return map_matrix_handler_->GetBinarySize(map_matrix_);
317}

◆ GetCoordinate() [1/3]

bool apollo::localization::msf::pyramid_map::BaseMapNode::GetCoordinate ( const Eigen::Vector2d &  coordinate,
unsigned int *  x,
unsigned int *  y 
) const
virtual

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.

apollo::localization::msf::pyramid_map::PyramidMapNode 重载.

在文件 base_map_node.cc319 行定义.

320 {
321 const Eigen::Vector2d& left_top_corner = GetLeftTopCorner();
322 unsigned int off_x = static_cast<unsigned int>(
323 (coordinate[0] - left_top_corner[0]) / GetMapResolution());
324 unsigned int off_y = static_cast<unsigned int>(
325 (coordinate[1] - left_top_corner[1]) / GetMapResolution());
326 if (off_x < this->map_config_->map_node_size_x_ &&
327 off_y < this->map_config_->map_node_size_y_) {
328 *x = off_x;
329 *y = off_y;
330 return true;
331 }
332 return false;
333}
unsigned int map_node_size_y_
The map node size in pixels.
unsigned int map_node_size_x_
The map node size in pixels.
const Eigen::Vector2d & GetLeftTopCorner() const
float GetMapResolution() const
Get the resolution of this map nodex.

◆ GetCoordinate() [2/3]

bool apollo::localization::msf::pyramid_map::BaseMapNode::GetCoordinate ( const Eigen::Vector3d &  coordinate,
unsigned int *  x,
unsigned int *  y 
) const
virtual

apollo::localization::msf::pyramid_map::PyramidMapNode 重载.

在文件 base_map_node.cc335 行定义.

336 {
337 Eigen::Vector2d coord2d(coordinate[0], coordinate[1]);
338 return GetCoordinate(coord2d, x, y);
339}
virtual bool GetCoordinate(const Eigen::Vector2d &coordinate, unsigned int *x, unsigned int *y) const
Given the global coordinate, get the local 2D coordinate of the map cell matrix.

◆ GetCoordinate() [3/3]

Eigen::Vector2d apollo::localization::msf::pyramid_map::BaseMapNode::GetCoordinate ( unsigned int  x,
unsigned int  y 
) const
virtual

Given the local 2D coordinate, return the global coordinate.

apollo::localization::msf::pyramid_map::PyramidMapNode 重载.

在文件 base_map_node.cc341 行定义.

342 {
343 const Eigen::Vector2d& left_top_corner = GetLeftTopCorner();
344 Eigen::Vector2d coord(
345 left_top_corner[0] + static_cast<float>(x) * GetMapResolution(),
346 left_top_corner[1] + static_cast<float>(y) * GetMapResolution());
347 return coord;
348}

◆ GetHeaderBinarySize()

size_t apollo::localization::msf::pyramid_map::BaseMapNode::GetHeaderBinarySize ( ) const
protectedvirtual

Get the size of the header in bytes.

在文件 base_map_node.cc269 行定义.

269 {
270 return map_node_config_->GetBinarySize();
271}

◆ GetIsChanged()

bool apollo::localization::msf::pyramid_map::BaseMapNode::GetIsChanged ( ) const
inline

Get if the map data has changed.

在文件 base_map_node.h113 行定义.

113{ return is_changed_; }

◆ GetIsReady()

bool apollo::localization::msf::pyramid_map::BaseMapNode::GetIsReady ( ) const
inline

Get if the map node data is ready

在文件 base_map_node.h119 行定义.

119{ return data_is_ready_; }

◆ GetIsReserved()

bool apollo::localization::msf::pyramid_map::BaseMapNode::GetIsReserved ( ) const
inline

Get if the map node is reserved.

在文件 base_map_node.h110 行定义.

110{ return is_reserved_; }

◆ GetLeftTopCorner() [1/2]

const Eigen::Vector2d & apollo::localization::msf::pyramid_map::BaseMapNode::GetLeftTopCorner ( ) const
inline

在文件 base_map_node.h121 行定义.

121 {
122 return left_top_corner_;
123 }
Eigen::Vector2d left_top_corner_
The left top corner of the map node in the global coordinate system.

◆ GetLeftTopCorner() [2/2]

Eigen::Vector2d apollo::localization::msf::pyramid_map::BaseMapNode::GetLeftTopCorner ( const BaseMapConfig option,
const MapNodeIndex index 
)
static

在文件 base_map_node.cc378 行定义.

379 {
380 Eigen::Vector2d coord;
381 coord[0] = config.map_range_.GetMinX() +
382 static_cast<float>(config.map_node_size_x_) *
383 config.map_resolutions_[index.resolution_id_] *
384 static_cast<float>(index.n_);
385 coord[1] = config.map_range_.GetMinY() +
386 static_cast<float>(config.map_node_size_y_) *
387 config.map_resolutions_[index.resolution_id_] *
388 static_cast<float>(index.m_);
389 DCHECK_LT(coord[0], config.map_range_.GetMaxX());
390 DCHECK_LT(coord[1], config.map_range_.GetMaxY());
391 return coord;
392}

◆ GetMapCellMatrix() [1/2]

BaseMapMatrix & apollo::localization::msf::pyramid_map::BaseMapNode::GetMapCellMatrix ( )
inline

在文件 base_map_node.h91 行定义.

91{ return *map_matrix_; }

◆ GetMapCellMatrix() [2/2]

const BaseMapMatrix & apollo::localization::msf::pyramid_map::BaseMapNode::GetMapCellMatrix ( ) const
inline

Get map cell matrix.

在文件 base_map_node.h90 行定义.

90{ return *map_matrix_; }

◆ GetMapConfig()

const BaseMapConfig & apollo::localization::msf::pyramid_map::BaseMapNode::GetMapConfig ( ) const
inline

Get the map settings.

在文件 base_map_node.h94 行定义.

94{ return *map_config_; }

◆ GetMapNodeConfig()

const BaseMapNodeConfig & apollo::localization::msf::pyramid_map::BaseMapNode::GetMapNodeConfig ( ) const
inline

Get the map node config.

在文件 base_map_node.h97 行定义.

97 {
98 return *map_node_config_;
99 }

◆ GetMapNodeIndex()

const MapNodeIndex & apollo::localization::msf::pyramid_map::BaseMapNode::GetMapNodeIndex ( ) const
inline

Get the map node index.

在文件 base_map_node.h102 行定义.

102 {
103 return map_node_config_->node_index_;
104 }

◆ GetMapResolution()

float apollo::localization::msf::pyramid_map::BaseMapNode::GetMapResolution ( ) const
inline

Get the resolution of this map nodex.

在文件 base_map_node.h132 行定义.

132 {
133 return this->map_config_
134 ->map_resolutions_[map_node_config_->node_index_.resolution_id_];
135 }
std::vector< float > map_resolutions_
The pixel resolutions in the map in meters.

◆ Init() [1/2]

virtual void apollo::localization::msf::pyramid_map::BaseMapNode::Init ( const BaseMapConfig map_config)
pure virtual

Initialize the map node.

Call this function first before use it!

apollo::localization::msf::pyramid_map::NdtMapNode , 以及 apollo::localization::msf::pyramid_map::PyramidMapNode 内被实现.

◆ Init() [2/2]

virtual void apollo::localization::msf::pyramid_map::BaseMapNode::Init ( const BaseMapConfig map_config,
const MapNodeIndex index,
bool  create_map_cells = true 
)
pure virtual

◆ InitMapMatrix()

void apollo::localization::msf::pyramid_map::BaseMapNode::InitMapMatrix ( const BaseMapConfig map_config)
virtual

Initialize the map matrix.

在文件 base_map_node.cc58 行定义.

58 {
59 map_config_ = map_config;
60 map_matrix_->Init(*map_config);
61}

◆ Load() [1/2]

bool apollo::localization::msf::pyramid_map::BaseMapNode::Load ( )

Load the map node from the disk.

在文件 base_map_node.cc131 行定义.

131 {
132 std::string path = map_config_->map_folder_path_;
133 char buf[1024];
134 std::vector<std::string> paths;
135 paths.push_back(path);
136 snprintf(buf, sizeof(buf), "/map");
137 paths.push_back(buf);
138 path = path + buf;
139 snprintf(buf, sizeof(buf), "/%03u",
140 map_node_config_->node_index_.resolution_id_);
141 paths.push_back(buf);
142 path = path + buf;
143 paths.push_back(map_node_config_->node_index_.zone_id_ > 0 ? "/north"
144 : "/south");
145 path = path + paths.back();
146 snprintf(buf, sizeof(buf), "/%02d",
147 abs(map_node_config_->node_index_.zone_id_));
148 paths.push_back(buf);
149 path = path + buf;
150 snprintf(buf, sizeof(buf), "/%08d", map_node_config_->node_index_.m_);
151 paths.push_back(buf);
152 path = path + buf;
153 if (!CheckMapDirectoryRecursively(paths)) {
154 return false;
155 }
156 snprintf(buf, sizeof(buf), "/%08d", map_node_config_->node_index_.n_);
157 path = path + buf;
158 return Load(path.c_str());
159}
bool Load()
Load the map node from the disk.
bool CheckMapDirectoryRecursively(const std::vector< std::string > &paths) const
Try to check the map directory recursively.

◆ Load() [2/2]

bool apollo::localization::msf::pyramid_map::BaseMapNode::Load ( const char *  filename)

在文件 base_map_node.cc161 行定义.

161 {
162 data_is_ready_ = false;
163
164 FILE* file = fopen(filename, "rb");
165 if (file) {
166 bool success = LoadBinary(file);
167 fclose(file);
168 is_changed_ = false;
169 data_is_ready_ = success;
170 return success;
171 } else {
172 AERROR << "Can't find the file: " << filename;
173 return false;
174 }
175}
virtual bool LoadBinary(FILE *file)
Load the map cell from a binary chunk.
#define AERROR
Definition log.h:44

◆ LoadBinary()

bool apollo::localization::msf::pyramid_map::BaseMapNode::LoadBinary ( FILE *  file)
protectedvirtual

Load the map cell from a binary chunk.

在文件 base_map_node.cc177 行定义.

177 {
178 // Load the header
179 size_t header_size = GetHeaderBinarySize();
180 std::vector<unsigned char> buf(header_size);
181 size_t read_size = fread(&buf[0], 1, header_size, file);
182 if (read_size != header_size) {
183 return false;
184 }
185 size_t processed_size = LoadHeaderBinary(&buf[0]);
186 if (processed_size != header_size) {
187 return false;
188 }
189 // Load the body
190 buf.resize(file_body_binary_size_);
191 read_size = fread(&buf[0], 1, file_body_binary_size_, file);
192 if (read_size != file_body_binary_size_) {
193 return false;
194 }
195 processed_size = LoadBodyBinary(&buf);
196 if (processed_size != uncompressed_file_body_size_) {
197 return false;
198 }
199 return true;
200}
virtual size_t LoadBodyBinary(std::vector< unsigned char > *buf)
Load the map node body from a binary chunk.
virtual size_t LoadHeaderBinary(const unsigned char *buf)
Load the map node header from a binary chunk.

◆ LoadBodyBinary()

size_t apollo::localization::msf::pyramid_map::BaseMapNode::LoadBodyBinary ( std::vector< unsigned char > *  buf)
protectedvirtual

Load the map node body from a binary chunk.

参数
<return>The size read (the real size of body).

在文件 base_map_node.cc273 行定义.

273 {
274 if (compression_strategy_ == nullptr) {
275 return map_matrix_handler_->LoadBinary(&(*buf)[0], map_matrix_);
276 }
277 std::vector<unsigned char> buf_uncompressed;
278 int ret = compression_strategy_->Decode(buf, &buf_uncompressed);
279 if (ret < 0) {
280 AERROR << "compression Decode error: " << ret;
281 return 0;
282 }
283 uncompressed_file_body_size_ = buf_uncompressed.size();
284 AINFO << "map node compress ratio: "
285 << static_cast<float>(buf->size()) /
286 static_cast<float>(uncompressed_file_body_size_);
287
288 return map_matrix_handler_->LoadBinary(&buf_uncompressed[0], map_matrix_);
289}

◆ LoadHeaderBinary()

size_t apollo::localization::msf::pyramid_map::BaseMapNode::LoadHeaderBinary ( const unsigned char *  buf)
protectedvirtual

Load the map node header from a binary chunk.

参数
<return>The size read (the real size of header).

在文件 base_map_node.cc246 行定义.

246 {
247 std::shared_ptr<BaseMapNodeConfig> node_config_tem =
248 map_node_config_->Clone();
249
250 size_t target_size = map_node_config_->LoadBinary(buf);
251
252 // check if header is valid
253 if (node_config_tem->map_version_ != map_node_config_->map_version_ ||
254 node_config_tem->node_index_ != map_node_config_->node_index_) {
255 return 0;
256 }
257
259
260 return target_size;
261}

◆ ResetMapNode()

void apollo::localization::msf::pyramid_map::BaseMapNode::ResetMapNode ( )
virtual

Reset map cells data.

在文件 base_map_node.cc70 行定义.

70 {
71 is_changed_ = false;
72 data_is_ready_ = false;
73 is_reserved_ = false;
76 map_matrix_->Reset();
77}

◆ Save()

bool apollo::localization::msf::pyramid_map::BaseMapNode::Save ( )

Save the map node to the disk.

在文件 base_map_node.cc79 行定义.

79 {
82
83 std::string path = map_config_->map_folder_path_;
84
85 char buf[1024];
86 std::vector<std::string> paths;
87
88 paths.push_back(path);
89
90 snprintf(buf, sizeof(buf), "/map");
91 paths.push_back(buf);
92 path = path + buf;
93
94 snprintf(buf, sizeof(buf), "/%03u",
95 map_node_config_->node_index_.resolution_id_);
96 paths.push_back(buf);
97 path = path + buf;
98
99 paths.push_back(map_node_config_->node_index_.zone_id_ > 0 ? "/north"
100 : "/south");
101 path = path + paths.back();
102
103 snprintf(buf, sizeof(buf), "/%02d",
104 abs(map_node_config_->node_index_.zone_id_));
105 paths.push_back(buf);
106 path = path + buf;
107
108 snprintf(buf, sizeof(buf), "/%08d", map_node_config_->node_index_.m_);
109 paths.push_back(buf);
110 path = path + buf;
111
112 if (!CreateMapDirectoryRecursively(paths)) {
113 return false;
114 }
115
116 snprintf(buf, sizeof(buf), "/%08d", map_node_config_->node_index_.n_);
117 path = path + buf;
118
119 FILE* file = fopen(path.c_str(), "wb");
120 if (file) {
121 bool success = CreateBinary(file);
122 fclose(file);
123 is_changed_ = false;
124 return success;
125 } else {
126 AERROR << "Can't write to file: " << path << ".";
127 return false;
128 }
129}
bool SaveIntensityImage() const
Save intensity image of node.
bool SaveAltitudeImage() const
Save altitude image of node.
virtual bool CreateBinary(FILE *file) const
Create the binary.
bool CreateMapDirectoryRecursively(const std::vector< std::string > &paths) const
Try to create the map directory recursively.

◆ SaveAltitudeImage() [1/2]

bool apollo::localization::msf::pyramid_map::BaseMapNode::SaveAltitudeImage ( ) const

Save altitude image of node.

在文件 base_map_node.cc476 行定义.

476 {
477 std::string path = map_config_->map_folder_path_;
478
479 char buf[1024];
480 std::vector<std::string> paths;
481
482 paths.push_back(path);
483
484 snprintf(buf, sizeof(buf), "/image_alt");
485 paths.push_back(buf);
486 path = path + buf;
487
488 snprintf(buf, sizeof(buf), "/%03u",
489 map_node_config_->node_index_.resolution_id_);
490 paths.push_back(buf);
491 path = path + buf;
492
493 paths.push_back(map_node_config_->node_index_.zone_id_ > 0 ? "/north"
494 : "/south");
495 path = path + paths.back();
496
497 snprintf(buf, sizeof(buf), "/%02d",
498 abs(map_node_config_->node_index_.zone_id_));
499 paths.push_back(buf);
500 path = path + buf;
501
502 snprintf(buf, sizeof(buf), "/%08d", map_node_config_->node_index_.m_);
503 paths.push_back(buf);
504 path = path + buf;
505
506 if (!CreateMapDirectoryRecursively(paths)) {
507 return false;
508 }
509
510 snprintf(buf, sizeof(buf), "/%08d.png", map_node_config_->node_index_.n_);
511 path = path + buf;
512
513 bool success = SaveAltitudeImage(path);
514 return success;
515}

◆ SaveAltitudeImage() [2/2]

bool apollo::localization::msf::pyramid_map::BaseMapNode::SaveAltitudeImage ( const std::string &  path) const

Save altitude image of node.

在文件 base_map_node.cc517 行定义.

517 {
518 cv::Mat image;
519 map_matrix_->GetAltitudeImg(&image);
520 if (image.empty()) {
521 return false;
522 }
523
524 bool success = cv::imwrite(path, image);
525 return success;
526}

◆ SaveIntensityImage() [1/2]

bool apollo::localization::msf::pyramid_map::BaseMapNode::SaveIntensityImage ( ) const

Save intensity image of node.

在文件 base_map_node.cc428 行定义.

428 {
429 std::string path = map_config_->map_folder_path_;
430
431 char buf[1024];
432 std::vector<std::string> paths;
433
434 paths.push_back(path);
435
436 snprintf(buf, sizeof(buf), "/image");
437 paths.push_back(buf);
438 path = path + buf;
439
440 snprintf(buf, sizeof(buf), "/%03u",
441 map_node_config_->node_index_.resolution_id_);
442 paths.push_back(buf);
443 path = path + buf;
444
445 paths.push_back(map_node_config_->node_index_.zone_id_ > 0 ? "/north"
446 : "/south");
447 path = path + paths.back();
448
449 snprintf(buf, sizeof(buf), "/%02d",
450 abs(map_node_config_->node_index_.zone_id_));
451 paths.push_back(buf);
452 path = path + buf;
453
454 snprintf(buf, sizeof(buf), "/%08d", map_node_config_->node_index_.m_);
455 paths.push_back(buf);
456 path = path + buf;
457
458 if (!CreateMapDirectoryRecursively(paths)) {
459 return false;
460 }
461
462 snprintf(buf, sizeof(buf), "/%08d.png", map_node_config_->node_index_.n_);
463 path = path + buf;
464
465 bool success = SaveIntensityImage(path);
466 return success;
467}

◆ SaveIntensityImage() [2/2]

bool apollo::localization::msf::pyramid_map::BaseMapNode::SaveIntensityImage ( const std::string &  path) const

Save intensity image of node.

在文件 base_map_node.cc469 行定义.

469 {
470 cv::Mat image;
471 map_matrix_->GetIntensityImg(&image);
472 bool success = cv::imwrite(path, image);
473 return success;
474}

◆ SetIsChanged()

void apollo::localization::msf::pyramid_map::BaseMapNode::SetIsChanged ( bool  is)
inline

Set if the map node data has changed.

在文件 base_map_node.h116 行定义.

116{ is_changed_ = is; }

◆ SetIsReserved()

void apollo::localization::msf::pyramid_map::BaseMapNode::SetIsReserved ( bool  is_reserved)
inline

Set if the map node is reserved.

在文件 base_map_node.h107 行定义.

107{ is_reserved_ = is_reserved; }

◆ SetLeftTopCorner()

void apollo::localization::msf::pyramid_map::BaseMapNode::SetLeftTopCorner ( double  x,
double  y 
)
inline

Set the left top corner of the map node.

在文件 base_map_node.h126 行定义.

126 {
127 left_top_corner_[0] = x;
128 left_top_corner_[1] = y;
129 }

◆ SetMapNodeIndex()

void apollo::localization::msf::pyramid_map::BaseMapNode::SetMapNodeIndex ( const MapNodeIndex index)

Set the map node index.

在文件 base_map_node.cc350 行定义.

350 {
351 map_node_config_->node_index_ = index;
354}
static Eigen::Vector2d ComputeLeftTopCorner(const BaseMapConfig &config, const MapNodeIndex &index)

类成员变量说明

◆ compression_strategy_

std::shared_ptr<CompressionStrategy> apollo::localization::msf::pyramid_map::BaseMapNode::compression_strategy_ = nullptr
protected

@bried The compression strategy.

在文件 base_map_node.h212 行定义.

◆ data_is_ready_

bool apollo::localization::msf::pyramid_map::BaseMapNode::data_is_ready_ = false
protected

在文件 base_map_node.h206 行定义.

◆ file_body_binary_size_

size_t apollo::localization::msf::pyramid_map::BaseMapNode::file_body_binary_size_ = 0
mutableprotected

The body binary size in file.

在文件 base_map_node.h208 行定义.

◆ index_

MapNodeIndex apollo::localization::msf::pyramid_map::BaseMapNode::index_
protected

The index of this node

在文件 base_map_node.h189 行定义.

◆ is_changed_

bool apollo::localization::msf::pyramid_map::BaseMapNode::is_changed_ = false
protected

Has the map node been changed.

在文件 base_map_node.h204 行定义.

◆ is_reserved_

bool apollo::localization::msf::pyramid_map::BaseMapNode::is_reserved_ = false
protected

If the node is reserved in map.

在文件 base_map_node.h202 行定义.

◆ left_top_corner_

Eigen::Vector2d apollo::localization::msf::pyramid_map::BaseMapNode::left_top_corner_
protected

The left top corner of the map node in the global coordinate system.

在文件 base_map_node.h193 行定义.

◆ map_config_

const BaseMapConfig* apollo::localization::msf::pyramid_map::BaseMapNode::map_config_ = nullptr
protected

The map settings.

在文件 base_map_node.h186 行定义.

◆ map_matrix_

std::shared_ptr<BaseMapMatrix> apollo::localization::msf::pyramid_map::BaseMapNode::map_matrix_ = nullptr
protected

The data structure of the map datas, which is a matrix.

在文件 base_map_node.h198 行定义.

◆ map_matrix_handler_

std::shared_ptr<BaseMapMatrixHandler> apollo::localization::msf::pyramid_map::BaseMapNode::map_matrix_handler_ = nullptr
protected

The class to load and create map matrix binary.

在文件 base_map_node.h200 行定义.

◆ map_node_config_

std::shared_ptr<BaseMapNodeConfig> apollo::localization::msf::pyramid_map::BaseMapNode::map_node_config_ = nullptr
protected

The map node config.

在文件 base_map_node.h196 行定义.

◆ uncompressed_file_body_size_

size_t apollo::localization::msf::pyramid_map::BaseMapNode::uncompressed_file_body_size_ = 0
mutableprotected

在文件 base_map_node.h210 行定义.


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