Localization: add base_map files for pyramid_map module

modules/localization/msf/local_pyramid_map/base_map/base_map.h

+/******************************************************************************
+ * 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

+/******************************************************************************
+ * 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

+/******************************************************************************
+ * 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

+/******************************************************************************
+ * 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

+/******************************************************************************
+ * 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