Localization: add other pyramid map files for pyramid_map module

modules/localization/msf/local_pyramid_map/pyramid_map/pyramid_map.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/pyramid_map/pyramid_map.h"
+
+#include "modules/localization/msf/local_map/pyramid_map/pyramid_map_node.h"
+
+namespace apollo {
+namespace localization {
+namespace msf {
+
+PyramidMap::PyramidMap(PyramidMapConfig* config) : BaseMap(config) {}
+
+PyramidMap::~PyramidMap() {}
+
+float PyramidMap::GetIntensitySafe(const Eigen::Vector3d& coordinate,
+                                   int zone_id, unsigned int resolution_id,
+                                   unsigned int level) {
+  MapNodeIndex index = MapNodeIndex::GetMapNodeIndex(GetMapConfig(), coordinate,
+                                                     resolution_id, zone_id);
+  PyramidMapNode* node = dynamic_cast<PyramidMapNode*>(GetMapNodeSafe(index));
+  return node->GetIntensitySafe(coordinate, level);
+}
+
+float PyramidMap::GetIntensityVarSafe(const Eigen::Vector3d& coordinate,
+                                      int zone_id, unsigned int resolution_id,
+                                      unsigned int level) {
+  MapNodeIndex index = MapNodeIndex::GetMapNodeIndex(GetMapConfig(), coordinate,
+                                                     resolution_id, zone_id);
+  PyramidMapNode* node = dynamic_cast<PyramidMapNode*>(GetMapNodeSafe(index));
+  return node->GetIntensityVarSafe(coordinate, level);
+}
+
+float PyramidMap::GetAltitudeSafe(const Eigen::Vector3d& coordinate,
+                                  int zone_id, unsigned int resolution_id,
+                                  unsigned int level) {
+  MapNodeIndex index = MapNodeIndex::GetMapNodeIndex(GetMapConfig(), coordinate,
+                                                     resolution_id, zone_id);
+  PyramidMapNode* node = dynamic_cast<PyramidMapNode*>(GetMapNodeSafe(index));
+  return node->GetAltitudeSafe(coordinate, level);
+}
+
+float PyramidMap::GetAltitudeVarSafe(const Eigen::Vector3d& coordinate,
+                                     int zone_id, unsigned int resolution_id,
+                                     unsigned int level) {
+  MapNodeIndex index = MapNodeIndex::GetMapNodeIndex(GetMapConfig(), coordinate,
+                                                     resolution_id, zone_id);
+  PyramidMapNode* node = dynamic_cast<PyramidMapNode*>(GetMapNodeSafe(index));
+  return node->GetAltitudeVarSafe(coordinate, level);
+}
+
+float PyramidMap::GetGroundAltitudeSafe(const Eigen::Vector3d& coordinate,
+                                        int zone_id, unsigned int resolution_id,
+                                        unsigned int level) {
+  MapNodeIndex index = MapNodeIndex::GetMapNodeIndex(GetMapConfig(), coordinate,
+                                                     resolution_id, zone_id);
+  PyramidMapNode* node = dynamic_cast<PyramidMapNode*>(GetMapNodeSafe(index));
+  return node->GetGroundAltitudeSafe(coordinate, level);
+}
+
+unsigned int PyramidMap::GetCountSafe(const Eigen::Vector3d& coordinate,
+                                      int zone_id, unsigned int resolution_id,
+                                      unsigned int level) {
+  MapNodeIndex index = MapNodeIndex::GetMapNodeIndex(GetMapConfig(), coordinate,
+                                                     resolution_id, zone_id);
+  PyramidMapNode* node = dynamic_cast<PyramidMapNode*>(GetMapNodeSafe(index));
+  return node->GetCountSafe(coordinate, level);
+}
+
+unsigned int PyramidMap::GetGroundCountSafe(const Eigen::Vector3d& coordinate,
+                                            int zone_id,
+                                            unsigned int resolution_id,
+                                            unsigned int level) {
+  MapNodeIndex index = MapNodeIndex::GetMapNodeIndex(GetMapConfig(), coordinate,
+                                                     resolution_id, zone_id);
+  PyramidMapNode* node = dynamic_cast<PyramidMapNode*>(GetMapNodeSafe(index));
+  return node->GetGroundCountSafe(coordinate, level);
+}
+
+}  // namespace msf
+}  // namespace localization
+}  // namespace apollo

modules/localization/msf/local_pyramid_map/pyramid_map/pyramid_map.h

+/******************************************************************************
+ * Copyright 2019 The Apollo Authors. All Rights Reserved.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *****************************************************************************/
+#pragma once
+
+#include "modules/localization/msf/local_map/base_map/base_map.h"
+#include "modules/localization/msf/local_map/pyramid_map/pyramid_map_config.h"
+
+namespace apollo {
+namespace localization {
+namespace msf {
+
+class PyramidMap : public BaseMap {
+ public:
+  explicit PyramidMap(PyramidMapConfig* config);
+  ~PyramidMap();
+
+ public:
+  /**@brief Given the 3D global coordinate, this function loads the
+   * corresponding map node in the cache thread safely and return intensity, if
+   *necessary.
+   **/
+  float GetIntensitySafe(const Eigen::Vector3d& coordinate, int zone_id,
+                         unsigned int resolution_id, unsigned int level = 0);
+  /**@brief Given the 3D global coordinate, this function loads the
+   * corresponding map node in the cache and return intensity var, if necessary.
+   **/
+  float GetIntensityVarSafe(const Vector3d& coordinate, int zone_id,
+                            unsigned int resolution_id, unsigned int level = 0);
+  /**@brief Given the 3D global coordinate, this function loads the
+   * corresponding map node in the cache thread safely and return altitude, if
+   *necessary.
+   **/
+  float GetAltitudeSafe(const Eigen::Vector3d& coordinate, int zone_id,
+                        unsigned int resolution_id, unsigned int level = 0);
+  /**@brief Given the 3D global coordinate, this function loads the
+   * corresponding map node in the cache thread safely and return altitude var,
+   *if necessary.
+   **/
+  float GetAltitudeVarSafe(const Eigen::Vector3d& coordinate, int zone_id,
+                           unsigned int resolution_id, unsigned int level = 0);
+  /**@brief Given the 3D global coordinate, this function loads the
+   * corresponding map node in the cache thread safely and return ground
+   *altitude, if necessary.
+   **/
+  float GetGroundAltitudeSafe(const Eigen::Vector3d& coordinate, int zone_id,
+                              unsigned int resolution_id,
+                              unsigned int level = 0);
+  /**@brief Get the number of samples in the map cell thread safely. */
+  unsigned int GetCountSafe(const Eigen::Vector3d& coordinate, int zone_id,
+                            unsigned int resolution_id, unsigned int level = 0);
+  /**@brief Get the number of ground samples in the map cell thread safely. */
+  unsigned int GetGroundCountSafe(const Eigen::Vector3d& coordinate,
+                                  int zone_id, unsigned int resolution_id,
+                                  unsigned int level = 0);
+};
+
+}  // namespace msf
+}  // namespace localization
+}  // namespace apollo

modules/localization/msf/local_pyramid_map/pyramid_map/pyramid_map_node.cc

+/******************************************************************************
+ * Copyright 2019 The Apollo Authors. All Rights Reserved.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *****************************************************************************/
+#include "modules/localization/msf/local_map/pyramid_map/pyramid_map_node.h"
+
+#include <vector>
+#include "modules/localization/msf/local_map/pyramid_map/pyramid_map_matrix.h"
+#include "modules/localization/msf/local_map/pyramid_map/pyramid_map_matrix_handler.h"
+#include "modules/localization/msf/local_map/pyramid_map/pyramid_map_node_config.h"
+
+namespace apollo {
+namespace localization {
+namespace msf {
+
+PyramidMapNode::PyramidMapNode() {}
+
+PyramidMapNode::~PyramidMapNode() {}
+
+void PyramidMapNode::Init(const BaseMapConfig* map_config) {
+  map_config_ = map_config;
+
+  map_node_config_ = new PyramidMapNodeConfig();
+  map_node_config_->map_version_ = map_config_->GetMapVersion();
+  if (map_node_config_->map_version_ == MapVersion::LOSSY_FULL_ALT_MAP ||
+      map_node_config_->map_version_ == MapVersion::LOSSLESS_MAP) {
+    map_node_config_->has_map_version_ = false;
+    map_node_config_->has_body_md5_ = false;
+  } else {
+    map_node_config_->has_map_version_ = true;
+    map_node_config_->has_body_md5_ = true;
+  }
+
+  is_reserved_ = false;
+  data_is_ready_ = false;
+  is_changed_ = false;
+
+  map_matrix_ = new PyramidMapMatrix();
+  map_matrix_handler_ =
+      PyramidMapMatrixHandlerSelector::AllocPyramidMapMatrixHandler(
+          map_node_config_->map_version_);
+  compression_strategy_ = new ZlibStrategy();
+
+  const PyramidMapConfig* pm_map_config =
+      dynamic_cast<const PyramidMapConfig*>(map_config_);
+  resolutions_mr_.resize(pm_map_config->resolution_num_, 0);
+  for (unsigned int i = 0; i < resolutions_mr_.size(); ++i) {
+    resolutions_mr_[i] = static_cast<float>(pm_map_config->map_resolutions_[0] *
+                         std::pow(pm_map_config->resolution_ratio_, i));
+  }
+
+  InitMapMatrix(map_config_);
+}
+
+void PyramidMapNode::Init(const BaseMapConfig* map_config,
+                          const MapNodeIndex& index, bool create_map_cells) {
+  map_config_ = map_config;
+
+  map_node_config_ = new PyramidMapNodeConfig();
+  map_node_config_->node_index_ = index;
+  map_node_config_->map_version_ = map_config_->GetMapVersion();
+  if (map_node_config_->map_version_ == MapVersion::LOSSY_FULL_ALT_MAP ||
+      map_node_config_->map_version_ == MapVersion::LOSSLESS_MAP) {
+    map_node_config_->has_map_version_ = false;
+    map_node_config_->has_body_md5_ = false;
+  } else {
+    map_node_config_->has_map_version_ = true;
+    map_node_config_->has_body_md5_ = true;
+  }
+
+  left_top_corner_ =
+      ComputeLeftTopCorner(*map_config_, map_node_config_->node_index_);
+  is_reserved_ = false;
+  data_is_ready_ = false;
+  is_changed_ = false;
+
+  map_matrix_ = new PyramidMapMatrix();
+  map_matrix_handler_ =
+      PyramidMapMatrixHandlerSelector::AllocPyramidMapMatrixHandler(
+          map_node_config_->map_version_);
+  compression_strategy_ = new ZlibStrategy();
+
+  if (create_map_cells) {
+    InitMapMatrix(map_config_);
+  }
+}
+
+void PyramidMapNode::BottomUpBase() {
+  PyramidMapMatrix* map_matrix = dynamic_cast<PyramidMapMatrix*>(map_matrix_);
+  map_matrix->BottomUpBase();
+}
+
+void PyramidMapNode::BottomUpSafe() {
+  PyramidMapMatrix* map_matrix = dynamic_cast<PyramidMapMatrix*>(map_matrix_);
+  map_matrix->BottomUpSafe();
+}
+
+bool PyramidMapNode::AddValueIfInBound(const Eigen::Vector3d& coordinate,
+                                       unsigned char intensity,
+                                       unsigned int level) {
+  Eigen::Vector2d coord2d(coordinate[0], coordinate[1]);
+  //    Eigen::Map<Eigen::Vector2d>(coordinate.data(), 2);
+  unsigned int x = 0;
+  unsigned int y = 0;
+  bool is_success = GetCoordinate(coord2d, level, &x, &y);
+  if (is_success) {
+    PyramidMapMatrix* map_matrix = dynamic_cast<PyramidMapMatrix*>(map_matrix_);
+    map_matrix->AddSampleBase(static_cast<float>(intensity),
+                              static_cast<float>(coordinate[2]),
+                              y, x, level);
+    return true;
+  } else {
+    return false;
+  }
+}
+
+void PyramidMapNode::AddValueIfInBound(
+    const std::vector<Eigen::Vector3d>& coordinates,
+    const std::vector<unsigned char>& intensity, unsigned int level) {
+  if (coordinates.size() != intensity.size()) {
+    return;
+  }
+
+  for (unsigned int i = 0; i < coordinates.size(); i++) {
+    AddValueIfInBound(coordinates[i], intensity[i], level);
+  }
+}
+
+bool PyramidMapNode::GetCoordinate(const Eigen::Vector2d& coordinate,
+                                   unsigned int level, unsigned int* x,
+                                   unsigned int* y) const {
+  const PyramidMapMatrix* map_matrix =
+      dynamic_cast<PyramidMapMatrix*>(map_matrix_);
+
+  const float& current_resolution = resolutions_mr_[level];
+
+  unsigned int off_x = static_cast<unsigned int>(
+      (coordinate[0] - left_top_corner_[0]) / current_resolution);
+  unsigned int off_y = static_cast<unsigned int>(
+      (coordinate[1] - left_top_corner_[1]) / current_resolution);
+  if (off_x >= 0 && off_x < map_matrix->GetCols(level) && off_y >= 0 &&
+      off_y < map_matrix->GetRows(level)) {
+    *x = static_cast<unsigned int>(off_x);
+    *y = static_cast<unsigned int>(off_y);
+    return true;
+  } else {
+    return false;
+  }
+}
+
+Eigen::Vector2d PyramidMapNode::GetCoordinate(unsigned int level,
+                                              unsigned int x,
+                                              unsigned int y) const {
+  const PyramidMapConfig* map_config =
+      dynamic_cast<const PyramidMapConfig*>(map_config_);
+
+  float current_resolution =
+    static_cast<float>(map_config->map_resolutions_[0] *
+    std::pow(map_config->resolution_ratio_, level));
+
+  Eigen::Vector2d coord;
+  coord[0] = left_top_corner_[0] + current_resolution * static_cast<float>(x);
+  coord[1] = left_top_corner_[1] + current_resolution * static_cast<float>(y);
+  return coord;
+}
+
+bool PyramidMapNode::GetCoordinate(const Eigen::Vector2d& coordinate,
+                                   unsigned int* x, unsigned int* y) const {
+  return BaseMapNode::GetCoordinate(coordinate, x, y);
+}
+bool PyramidMapNode::GetCoordinate(const Eigen::Vector3d& coordinate,
+                                   unsigned int* x, unsigned int* y) const {
+  return BaseMapNode::GetCoordinate(coordinate, x, y);
+}
+
+Eigen::Vector2d PyramidMapNode::GetCoordinate(unsigned int x,
+                                              unsigned int y) const {
+  return BaseMapNode::GetCoordinate(x, y);
+}
+
+float PyramidMapNode::GetIntensitySafe(const Eigen::Vector3d& coordinate,
+                                       unsigned int level) const {
+  Eigen::Vector2d coord2d(coordinate[0], coordinate[1]);
+  unsigned int x = 0;
+  unsigned int y = 0;
+  bool is_success = GetCoordinate(coord2d, level, &x, &y);
+
+  float intensity = 0.0;
+  if (is_success) {
+    PyramidMapMatrix* map_matrix = dynamic_cast<PyramidMapMatrix*>(map_matrix_);
+    const float* intensity_ptr = map_matrix->GetIntensitySafe(y, x, level);
+
+    if (intensity_ptr != NULL) {
+      intensity = *intensity_ptr;
+    }
+  }
+
+  return intensity;
+}
+
+float PyramidMapNode::GetIntensityVarSafe(const Eigen::Vector3d& coordinate,
+                                          unsigned int level) const {
+  Eigen::Vector2d coord2d(coordinate[0], coordinate[1]);
+  unsigned int x = 0;
+  unsigned int y = 0;
+  bool is_success = GetCoordinate(coord2d, level, &x, &y);
+
+  float intensity_var = 0.0;
+  if (is_success) {
+    PyramidMapMatrix* map_matrix = dynamic_cast<PyramidMapMatrix*>(map_matrix_);
+    const float* intensity_var_ptr =
+        map_matrix->GetIntensityVarSafe(y, x, level);
+
+    if (intensity_var_ptr != NULL) {
+      intensity_var = *intensity_var_ptr;
+    }
+  }
+
+  return intensity_var;
+}
+
+float PyramidMapNode::GetAltitudeSafe(const Eigen::Vector3d& coordinate,
+                                      unsigned int level) const {
+  Eigen::Vector2d coord2d(coordinate[0], coordinate[1]);
+  unsigned int x = 0;
+  unsigned int y = 0;
+  bool is_success = GetCoordinate(coord2d, level, &x, &y);
+
+  float altitude = 0.0;
+  if (is_success) {
+    PyramidMapMatrix* map_matrix = dynamic_cast<PyramidMapMatrix*>(map_matrix_);
+    const float* altitude_ptr = map_matrix->GetAltitudeSafe(y, x, level);
+
+    if (altitude_ptr != NULL) {
+      altitude = *altitude_ptr;
+    }
+  }
+
+  return altitude;
+}
+
+float PyramidMapNode::GetAltitudeVarSafe(const Eigen::Vector3d& coordinate,
+                                         unsigned int level) const {
+  Eigen::Vector2d coord2d(coordinate[0], coordinate[1]);
+  unsigned int x = 0;
+  unsigned int y = 0;
+  bool is_success = GetCoordinate(coord2d, level, &x, &y);
+
+  float altitude_var = 0.0;
+  if (is_success) {
+    PyramidMapMatrix* map_matrix = dynamic_cast<PyramidMapMatrix*>(map_matrix_);
+    const float* altitude_var_ptr = map_matrix->GetAltitudeVarSafe(y, x, level);
+
+    if (altitude_var_ptr != NULL) {
+      altitude_var = *altitude_var_ptr;
+    }
+  }
+
+  return altitude_var;
+}
+
+float PyramidMapNode::GetGroundAltitudeSafe(const Eigen::Vector3d& coordinate,
+                                            unsigned int level) const {
+  Eigen::Vector2d coord2d(coordinate[0], coordinate[1]);
+  unsigned int x = 0;
+  unsigned int y = 0;
+  bool is_success = GetCoordinate(coord2d, level, &x, &y);
+
+  float ground_altitude = 0.0;
+  if (is_success) {
+    PyramidMapMatrix* map_matrix = dynamic_cast<PyramidMapMatrix*>(map_matrix_);
+    const float* ground_altitude_ptr =
+        map_matrix->GetGroundAltitudeSafe(y, x, level);
+
+    if (ground_altitude_ptr != NULL) {
+      ground_altitude = *ground_altitude_ptr;
+    }
+  }
+
+  return ground_altitude;
+}
+
+unsigned int PyramidMapNode::GetCountSafe(const Eigen::Vector3d& coordinate,
+                                          unsigned int level) const {
+  Eigen::Vector2d coord2d(coordinate[0], coordinate[1]);
+  unsigned int x = 0;
+  unsigned int y = 0;
+  bool is_success = GetCoordinate(coord2d, level, &x, &y);
+
+  unsigned int count = 0;
+  if (is_success) {
+    PyramidMapMatrix* map_matrix = dynamic_cast<PyramidMapMatrix*>(map_matrix_);
+    const unsigned int* count_ptr = map_matrix->GetCountSafe(y, x, level);
+
+    if (count_ptr != NULL) {
+      count = *count_ptr;
+    }
+  }
+
+  return count;
+}
+
+unsigned int PyramidMapNode::GetGroundCountSafe(
+    const Eigen::Vector3d& coordinate, unsigned int level) const {
+  Eigen::Vector2d coord2d(coordinate[0], coordinate[1]);
+  unsigned int x = 0;
+  unsigned int y = 0;
+  bool is_success = GetCoordinate(coord2d, level, &x, &y);
+
+  unsigned int ground_count = 0;
+  if (is_success) {
+    PyramidMapMatrix* map_matrix = dynamic_cast<PyramidMapMatrix*>(map_matrix_);
+    const unsigned int* ground_count_ptr =
+        map_matrix->GetGroundCountSafe(y, x, level);
+
+    if (ground_count_ptr != NULL) {
+      ground_count = *ground_count_ptr;
+    }
+  }
+
+  return ground_count;
+}
+
+float PyramidMapNode::GetIntensity(const Eigen::Vector3d& coordinate,
+                                   unsigned int level) const {
+  Eigen::Vector2d coord2d(coordinate[0], coordinate[1]);
+  unsigned int x = 0;
+  unsigned int y = 0;
+  bool is_success = GetCoordinate(coord2d, level, &x, &y);
+
+  float intensity = 0.0;
+  if (is_success) {
+    PyramidMapMatrix* map_matrix = dynamic_cast<PyramidMapMatrix*>(map_matrix_);
+    const float* intensity_ptr = map_matrix->GetIntensity(y, x, level);
+
+    if (intensity_ptr != NULL) {
+      intensity = *intensity_ptr;
+    }
+  }
+
+  return intensity;
+}
+
+float PyramidMapNode::GetIntensityVar(const Eigen::Vector3d& coordinate,
+                                      unsigned int level) const {
+  Eigen::Vector2d coord2d(coordinate[0], coordinate[1]);
+  unsigned int x = 0;
+  unsigned int y = 0;
+  bool is_success = GetCoordinate(coord2d, level, &x, &y);
+
+  float intensity_var = 0.0;
+  if (is_success) {
+    PyramidMapMatrix* map_matrix = dynamic_cast<PyramidMapMatrix*>(map_matrix_);
+    const float* intensity_var_ptr = map_matrix->GetIntensityVar(y, x, level);
+
+    if (intensity_var_ptr != NULL) {
+      intensity_var = *intensity_var_ptr;
+    }
+  }
+
+  return intensity_var;
+}
+
+float PyramidMapNode::GetAltitude(const Eigen::Vector3d& coordinate,
+                                  unsigned int level) const {
+  Eigen::Vector2d coord2d(coordinate[0], coordinate[1]);
+  unsigned int x = 0;
+  unsigned int y = 0;
+  bool is_success = GetCoordinate(coord2d, level, &x, &y);
+
+  float altitude = 0.0;
+  if (is_success) {
+    PyramidMapMatrix* map_matrix = dynamic_cast<PyramidMapMatrix*>(map_matrix_);
+    const float* altitude_ptr = map_matrix->GetAltitude(y, x, level);
+
+    if (altitude_ptr != NULL) {
+      altitude = *altitude_ptr;
+    }
+  }
+
+  return altitude;
+}
+
+float PyramidMapNode::GetAltitudeVar(const Eigen::Vector3d& coordinate,
+                                     unsigned int level) const {
+  Eigen::Vector2d coord2d(coordinate[0], coordinate[1]);
+  unsigned int x = 0;
+  unsigned int y = 0;
+  bool is_success = GetCoordinate(coord2d, level, &x, &y);
+
+  float altitude_var = 0.0;
+  if (is_success) {
+    PyramidMapMatrix* map_matrix = dynamic_cast<PyramidMapMatrix*>(map_matrix_);
+    const float* altitude_var_ptr = map_matrix->GetAltitudeVar(y, x, level);
+
+    if (altitude_var_ptr != NULL) {
+      altitude_var = *altitude_var_ptr;
+    }
+  }
+
+  return altitude_var;
+}
+
+float PyramidMapNode::GetGroundAltitude(const Eigen::Vector3d& coordinate,
+                                        unsigned int level) const {
+  Eigen::Vector2d coord2d(coordinate[0], coordinate[1]);
+  unsigned int x = 0;
+  unsigned int y = 0;
+  bool is_success = GetCoordinate(coord2d, level, &x, &y);
+
+  float ground_altitude = 0.0;
+  if (is_success) {
+    PyramidMapMatrix* map_matrix = dynamic_cast<PyramidMapMatrix*>(map_matrix_);
+    const float* ground_altitude_ptr =
+        map_matrix->GetGroundAltitude(y, x, level);
+
+    if (ground_altitude_ptr != NULL) {
+      ground_altitude = *ground_altitude_ptr;
+    }
+  }
+
+  return ground_altitude;
+}
+
+unsigned int PyramidMapNode::GetCount(const Eigen::Vector3d& coordinate,
+                                      unsigned int level) const {
+  Eigen::Vector2d coord2d(coordinate[0], coordinate[1]);
+  unsigned int x = 0;
+  unsigned int y = 0;
+  bool is_success = GetCoordinate(coord2d, level, &x, &y);
+
+  unsigned int count = 0;
+  if (is_success) {
+    PyramidMapMatrix* map_matrix = dynamic_cast<PyramidMapMatrix*>(map_matrix_);
+    const unsigned int* count_ptr = map_matrix->GetCount(y, x, level);
+
+    if (count_ptr != NULL) {
+      count = *count_ptr;
+    }
+  }
+
+  return count;
+}
+
+unsigned int PyramidMapNode::GetGroundCount(const Eigen::Vector3d& coordinate,
+                                            unsigned int level) const {
+  Eigen::Vector2d coord2d(coordinate[0], coordinate[1]);
+  unsigned int x = 0;
+  unsigned int y = 0;
+  bool is_success = GetCoordinate(coord2d, level, &x, &y);
+
+  unsigned int ground_count = 0;
+  if (is_success) {
+    PyramidMapMatrix* map_matrix = dynamic_cast<PyramidMapMatrix*>(map_matrix_);
+    const unsigned int* ground_count_ptr =
+        map_matrix->GetGroundCount(y, x, level);
+
+    if (ground_count_ptr != NULL) {
+      ground_count = *ground_count_ptr;
+    }
+  }
+
+  return ground_count;
+}
+
+double PyramidMapNode::ComputeMeanIntensity(unsigned int level) {
+  PyramidMapMatrix* map_matrix = dynamic_cast<PyramidMapMatrix*>(map_matrix_);
+  return map_matrix->ComputeMeanIntensity(level);
+}
+
+}  // namespace msf
+}  // namespace localization
+}  // namespace apollo

modules/localization/msf/local_pyramid_map/pyramid_map/pyramid_map_node.h

+/******************************************************************************
+ * Copyright 2019 The Apollo Authors. All Rights Reserved.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *****************************************************************************/
+#pragma once
+
+#include <Eigen/Core>
+#include <vector>
+#include "modules/localization/msf/local_map/base_map/base_map_node.h"
+#include "modules/localization/msf/local_map/pyramid_map/pyramid_map_matrix.h"
+
+namespace apollo {
+namespace localization {
+namespace msf {
+
+class PyramidMapNode : public BaseMapNode {
+ public:
+  PyramidMapNode();
+  ~PyramidMapNode();
+
+ public:
+  virtual void Init(const BaseMapConfig* map_config);
+  virtual void Init(const BaseMapConfig* map_config, const MapNodeIndex& index,
+                    bool create_map_cells = true);
+
+  /**@brief Propagate the data to the coarse resolution by check. */
+  void BottomUpSafe();
+
+  /**@brief Propagate the data to the coarse resolution.
+   * only update count, intensity, intensity var and altitude
+   */
+  void BottomUpBase();
+
+  /**@brief Add the value of a pixel in the map node if the pixel in the node.
+   * @param <coordinate> The 3D global coordinate.
+   * @param <intensity> The reflectance intensity.
+   * @param <return> True, if pixel in the bound of the node, else False.
+   * */
+  bool AddValueIfInBound(const Eigen::Vector3d& coordinate,
+                         unsigned char intensity, unsigned int level = 0);
+  /**@brief Add the value of a pixel in the map node if the pixel in the node.
+   * @param <coordinates> The 3D global coordinates.
+   * @param <intensities> The reflectance intensities.
+   * */
+  void AddValueIfInBound(const std::vector<Eigen::Vector3d>& coordinates,
+                         const std::vector<unsigned char>& intensity,
+                         unsigned int level = 0);
+  /**@brief Given the global coordinate, get the local 2D coordinate of the map
+   * cell matrix.
+   * <return> If global coordinate (x, y) belongs to this map node. */
+  bool GetCoordinate(const Eigen::Vector2d& coordinate, unsigned int level,
+                     unsigned int* x, unsigned int* y) const;
+  /**@brief Given the local 2D coordinate, return the global coordinate. */
+  Eigen::Vector2d GetCoordinate(unsigned int level, unsigned int x,
+                                unsigned int y) const;
+
+  virtual bool GetCoordinate(const Eigen::Vector2d& coordinate, unsigned int* x,
+                             unsigned int* y) const;
+  virtual bool GetCoordinate(const Eigen::Vector3d& coordinate, unsigned int* x,
+                             unsigned int* y) const;
+
+  virtual Eigen::Vector2d GetCoordinate(unsigned int x, unsigned int y) const;
+
+  /**@brief Given the 3D global coordinate,
+   * get the map cell intensity with check. */
+  float GetIntensitySafe(const Eigen::Vector3d& coordinate,
+                         unsigned int level = 0) const;
+  /**@brief Given the 3D global coordinate,
+   * get the map cell variance of the intensity with check. */
+  float GetIntensityVarSafe(const Eigen::Vector3d& coordinate,
+                            unsigned int level = 0) const;
+  /**@brief Given the 3D global coordinate,
+   * get the map cell's average altitude with check. */
+  float GetAltitudeSafe(const Eigen::Vector3d& coordinate,
+                        unsigned int level = 0) const;
+  /**@brief Given the 3D global coordinate,
+   * get the map cell's variance of the altitude with check. */
+  float GetAltitudeVarSafe(const Eigen::Vector3d& coordinate,
+                           unsigned int level = 0) const;
+  /**@brief Given the 3D global coordinate,
+   * get the map cell's average ground altitude with check. */
+  float GetGroundAltitudeSafe(const Eigen::Vector3d& coordinate,
+                              unsigned int level = 0) const;
+  /**@brief Given the 3D global coordinate,
+   * get the map cell's count of the samples with check. */
+  unsigned int GetCountSafe(const Eigen::Vector3d& coordinate,
+                            unsigned int level = 0) const;
+  /**@brief Given the 3D global coordinate,
+   * get the map cell's count of the ground samples with check. */
+  unsigned int GetGroundCountSafe(const Eigen::Vector3d& coordinate,
+                                  unsigned int level = 0) const;
+
+  /**@brief Given the 3D global coordinate,
+   * get the map cell intensity without check. */
+  float GetIntensity(const Eigen::Vector3d& coordinate,
+                     unsigned int level = 0) const;
+  /**@brief Given the 3D global coordinate,
+   * get the map cell variance of the intensity without check. */
+  float GetIntensityVar(const Eigen::Vector3d& coordinate,
+                        unsigned int level = 0) const;
+  /**@brief Given the 3D global coordinate,
+   * get the map cell's average altitude without check. */
+  float GetAltitude(const Eigen::Vector3d& coordinate,
+                    unsigned int level = 0) const;
+  /**@brief Given the 3D global coordinate,
+   * get the map cell's variance of the altitude without check. */
+  float GetAltitudeVar(const Eigen::Vector3d& coordinate,
+                       unsigned int level = 0) const;
+  /**@brief Given the 3D global coordinate,
+   * get the map cell's average ground altitude without check. */
+  float GetGroundAltitude(const Eigen::Vector3d& coordinate,
+                          unsigned int level = 0) const;
+  /**@brief Given the 3D global coordinate,
+   * get the map cell's count of the samples without check. */
+  unsigned int GetCount(const Eigen::Vector3d& coordinate,
+                        unsigned int level = 0) const;
+  /**@brief Given the 3D global coordinate,
+   * get the map cell's count of the ground samples without check. */
+  unsigned int GetGroundCount(const Eigen::Vector3d& coordinate,
+                              unsigned int level = 0) const;
+
+  /**@brief Compute mean intensity. */
+  double ComputeMeanIntensity(unsigned int level = 0);
+
+ private:
+  std::vector<float> resolutions_mr_;
+};
+
+}  // namespace msf
+}  // namespace localization
+}  // namespace apollo

modules/localization/msf/local_pyramid_map/pyramid_map/pyramid_map_node_config.cc

+/******************************************************************************
+ * Copyright 2019 The Apollo Authors. All Rights Reserved.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *****************************************************************************/
+#include "modules/localization/msf/local_map/pyramid_map/pyramid_map_node_config.h"
+
+namespace apollo {
+namespace localization {
+namespace msf {
+
+PyramidMapNodeConfig::PyramidMapNodeConfig() {}
+
+PyramidMapNodeConfig::~PyramidMapNodeConfig() {}
+
+BaseMapNodeConfig* PyramidMapNodeConfig::Clone() {
+  PyramidMapNodeConfig* map_node_config = new PyramidMapNodeConfig();
+  map_node_config->node_index_ = node_index_;
+  map_node_config->map_version_ = map_version_;
+  memcpy(map_node_config->body_md5_, body_md5_, sizeof(body_md5_));
+  map_node_config->body_size_ = body_size_;
+  map_node_config->has_map_version_ = has_map_version_;
+  map_node_config->has_body_md5_ = has_body_md5_;
+
+  return dynamic_cast<BaseMapNodeConfig*>(map_node_config);
+}
+
+}  // namespace msf
+}  // namespace localization
+}  // namespace apollo

modules/localization/msf/local_pyramid_map/pyramid_map/pyramid_map_node_config.h

+/******************************************************************************
+ * Copyright 2019 The Apollo Authors. All Rights Reserved.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *****************************************************************************/
+#pragma once
+
+#include "modules/localization/msf/local_map/base_map/base_map_node_config.h"
+
+namespace apollo {
+namespace localization {
+namespace msf {
+
+class PyramidMapNodeConfig : public BaseMapNodeConfig {
+ public:
+  PyramidMapNodeConfig();
+  ~PyramidMapNodeConfig();
+
+  virtual BaseMapNodeConfig* Clone();
+};
+
+}  // namespace msf
+}  // namespace localization
+}  // namespace apollo

modules/localization/msf/local_pyramid_map/pyramid_map/pyramid_map_node_test.cc

+/******************************************************************************
+ * Copyright 2019 The Apollo Authors. All Rights Reserved.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *****************************************************************************/
+#include "modules/localization/msf/local_map/pyramid_map/pyramid_map_node.h"
+#include <gtest/gtest.h>
+#include "modules/localization/msf/local_map/base_map/base_map_node.h"
+#include "modules/localization/msf/local_map/base_map/base_map_node_config.h"
+#include "modules/localization/msf/local_map/pyramid_map/pyramid_map_config.h"
+#include "modules/localization/msf/local_map/pyramid_map/pyramid_map_node_config.h"
+
+int main(int argc, char** argv) {
+  testing::InitGoogleTest(&argc, argv);
+  return RUN_ALL_TESTS();
+}
+
+namespace apollo {
+namespace localization {
+namespace msf {
+
+class PyramidMapNodeTestSuite : public ::testing::Test {
+ protected:
+  PyramidMapNodeTestSuite() {}
+  virtual ~PyramidMapNodeTestSuite() {}
+  virtual void SetUp() {}
+  virtual void TearDown() {}
+};
+
+TEST_F(PyramidMapNodeTestSuite, init) {
+  // create index
+  MapNodeIndex index;
+  index.resolution_id_ = 0;
+  index.zone_id_ = 50;
+  index.m_ = 0;
+  index.n_ = 0;
+
+  // init config
+  PyramidMapConfig* config = new PyramidMapConfig("lossy_full_alt");
+  config->map_version_ = "pyramid_lossy_map";
+  config->SetMapNodeSize(2, 2);
+  config->resolution_num_ = 2;
+
+  // create map node
+  PyramidMapNode* node = new PyramidMapNode();
+  node->Init(config);
+  node->SetMapNodeIndex(index);
+
+  node->Init(config, index);
+  config->map_version_ = "lossy_full_alt";
+  node->Init(config, index);
+
+  if (node != nullptr) {
+    delete node;
+    node = nullptr;
+  }
+  if (config != nullptr) {
+    delete config;
+    config = nullptr;
+  }
+}
+
+TEST_F(PyramidMapNodeTestSuite, pyramid_map_node_function) {
+  // create index
+  MapNodeIndex index;
+  index.resolution_id_ = 0;
+  index.zone_id_ = 50;
+  index.m_ = 0;
+  index.n_ = 0;
+
+  // init config
+  PyramidMapConfig* config = new PyramidMapConfig("lossy_full_alt");
+  config->SetMapNodeSize(2, 2);
+  config->resolution_num_ = 2;
+
+  // create map node
+  PyramidMapNode* node = new PyramidMapNode();
+  node->Init(config);
+  node->SetMapNodeIndex(index);
+
+  // check global coordinate to local coordinate transformation
+  double data[] = {0.125, 0.125, 1.0};
+  unsigned int x = 0;
+  unsigned int y = 0;
+  Eigen::Vector2d vector_2d_data(data);
+  node->GetCoordinate(vector_2d_data, 0, &x, &y);
+  EXPECT_EQ(x, 1);
+  EXPECT_EQ(y, 1);
+
+  // good case: set and get method
+  Eigen::Vector3d vector_3d_data(data);
+  EXPECT_TRUE(node->AddValueIfInBound(vector_3d_data, 10, 0));
+  EXPECT_FLOAT_EQ(node->GetIntensitySafe(vector_3d_data, 0), 10.f);
+  EXPECT_FLOAT_EQ(node->GetIntensityVarSafe(vector_3d_data, 0), 0.f);
+  EXPECT_FLOAT_EQ(node->GetAltitudeSafe(vector_3d_data, 0), 1.f);
+  EXPECT_FLOAT_EQ(node->GetAltitudeVarSafe(vector_3d_data, 0), 0.f);
+  EXPECT_EQ(node->GetCountSafe(vector_3d_data, 0), 1);
+  EXPECT_EQ(node->GetGroundCountSafe(vector_3d_data, 0), 0);
+  EXPECT_FLOAT_EQ(node->GetGroundAltitudeSafe(vector_3d_data, 0), 0.f);
+
+  EXPECT_FLOAT_EQ(node->GetIntensity(vector_3d_data, 0), 10.f);
+  EXPECT_FLOAT_EQ(node->GetIntensityVar(vector_3d_data, 0), 0.f);
+  EXPECT_FLOAT_EQ(node->GetAltitude(vector_3d_data, 0), 1.f);
+  EXPECT_FLOAT_EQ(node->GetAltitudeVar(vector_3d_data, 0), 0.f);
+  EXPECT_EQ(node->GetCount(vector_3d_data, 0), 1);
+  EXPECT_EQ(node->GetGroundCount(vector_3d_data, 0), 0);
+  EXPECT_FLOAT_EQ(node->GetGroundAltitude(vector_3d_data, 0), 0.f);
+
+  // bottom up
+  node->BottomUpSafe();
+  EXPECT_FLOAT_EQ(node->GetIntensitySafe(vector_3d_data, 1), 10.f);
+  EXPECT_FLOAT_EQ(node->GetIntensityVarSafe(vector_3d_data, 1), 0.f);
+  EXPECT_FLOAT_EQ(node->GetAltitudeSafe(vector_3d_data, 1), 1.f);
+  EXPECT_FLOAT_EQ(node->GetAltitudeVarSafe(vector_3d_data, 1), 0.f);
+  EXPECT_EQ(node->GetCountSafe(vector_3d_data, 1), 1);
+  EXPECT_EQ(node->GetGroundCountSafe(vector_3d_data, 1), 0);
+
+  // bad case 1: get coordinate failed
+  vector_3d_data[0] = 10.0;
+  vector_3d_data[1] = 10.0;
+  EXPECT_FLOAT_EQ(node->GetIntensitySafe(vector_3d_data, 0), 0.f);
+  EXPECT_FLOAT_EQ(node->GetIntensityVarSafe(vector_3d_data, 0), 0.f);
+  EXPECT_FLOAT_EQ(node->GetAltitudeSafe(vector_3d_data, 0), 0.f);
+  EXPECT_FLOAT_EQ(node->GetAltitudeVarSafe(vector_3d_data, 0), 0.f);
+  EXPECT_EQ(node->GetCountSafe(vector_3d_data, 0), 0);
+  EXPECT_EQ(node->GetGroundCountSafe(vector_3d_data, 0), 0);
+  EXPECT_FLOAT_EQ(node->GetGroundAltitudeSafe(vector_3d_data, 0), 0.f);
+
+  EXPECT_FLOAT_EQ(node->GetIntensity(vector_3d_data, 0), 0.f);
+  EXPECT_FLOAT_EQ(node->GetIntensityVar(vector_3d_data, 0), 0.f);
+  EXPECT_FLOAT_EQ(node->GetAltitude(vector_3d_data, 0), 0.f);
+  EXPECT_FLOAT_EQ(node->GetAltitudeVar(vector_3d_data, 0), 0.f);
+  EXPECT_EQ(node->GetCount(vector_3d_data, 0), 0);
+  EXPECT_EQ(node->GetGroundCount(vector_3d_data, 0), 0);
+  EXPECT_FLOAT_EQ(node->GetGroundAltitude(vector_3d_data, 0), 0.f);
+
+  // check branch when reset
+  node->ResetMapNode();
+  vector_3d_data[0] = 0.125;
+  vector_3d_data[1] = 0.125;
+  EXPECT_FLOAT_EQ(node->GetIntensitySafe(vector_3d_data, 0), 0.f);
+  EXPECT_FLOAT_EQ(node->GetIntensityVarSafe(vector_3d_data, 0), 0.f);
+  EXPECT_FLOAT_EQ(node->GetAltitudeSafe(vector_3d_data, 0), 0.f);
+  EXPECT_FLOAT_EQ(node->GetAltitudeVarSafe(vector_3d_data, 0), 0.f);
+  EXPECT_EQ(node->GetCountSafe(vector_3d_data, 0), 0);
+  EXPECT_EQ(node->GetGroundCountSafe(vector_3d_data, 0), 0);
+  EXPECT_FLOAT_EQ(node->GetGroundAltitudeSafe(vector_3d_data, 0), 0.f);
+
+  EXPECT_FLOAT_EQ(node->GetIntensity(vector_3d_data, 0), 0.f);
+  EXPECT_FLOAT_EQ(node->GetIntensityVar(vector_3d_data, 0), 0.f);
+  EXPECT_FLOAT_EQ(node->GetAltitude(vector_3d_data, 0), 0.f);
+  EXPECT_FLOAT_EQ(node->GetAltitudeVar(vector_3d_data, 0), 0.f);
+  EXPECT_EQ(node->GetCount(vector_3d_data, 0), 0);
+  EXPECT_EQ(node->GetGroundCount(vector_3d_data, 0), 0);
+  EXPECT_FLOAT_EQ(node->GetGroundAltitude(vector_3d_data, 0), 0.f);
+
+  if (node != nullptr) {
+    delete node;
+    node = nullptr;
+  }
+  if (config != nullptr) {
+    delete config;
+    config = nullptr;
+  }
+}
+
+TEST_F(PyramidMapNodeTestSuite, test_get_set_bad_case) {
+  // create index
+  MapNodeIndex index;
+  index.resolution_id_ = 0;
+  index.zone_id_ = 50;
+  index.m_ = 0;
+  index.n_ = 0;
+
+  // init config
+  PyramidMapConfig* config = new PyramidMapConfig("lossy_full_alt");
+  config->SetMapNodeSize(2, 2);
+  config->resolution_num_ = 1;
+  config->has_intensity_ = false;
+  config->has_intensity_var_ = false;
+  config->has_altitude_ = false;
+  config->has_altitude_var_ = false;
+  config->has_ground_altitude_ = false;
+  config->has_ground_count_ = false;
+  config->has_count_ = false;
+
+  // create map node
+  PyramidMapNode* node = new PyramidMapNode();
+  node->Init(config);
+  node->SetMapNodeIndex(index);
+
+  // check global coordinate to local coordinate transformation
+  double data[] = {0.125, 0.125, 1.0};
+  Eigen::Vector3d vector_3d_data(data);
+  EXPECT_FLOAT_EQ(node->GetIntensitySafe(vector_3d_data, 0), 0.f);
+  EXPECT_FLOAT_EQ(node->GetIntensityVarSafe(vector_3d_data, 0), 0.f);
+  EXPECT_FLOAT_EQ(node->GetAltitudeSafe(vector_3d_data, 0), 0.f);
+  EXPECT_FLOAT_EQ(node->GetAltitudeVarSafe(vector_3d_data, 0), 0.f);
+  EXPECT_EQ(node->GetCountSafe(vector_3d_data, 0), 0);
+  EXPECT_EQ(node->GetGroundCountSafe(vector_3d_data, 0), 0);
+  EXPECT_FLOAT_EQ(node->GetGroundAltitudeSafe(vector_3d_data, 0), 0.f);
+}
+
+TEST_F(PyramidMapNodeTestSuite, test_base_map_node_save_load) {
+  // create index
+  MapNodeIndex index;
+  index.resolution_id_ = 0;
+  index.zone_id_ = 50;
+  index.m_ = 0;
+  index.n_ = 1;
+
+  // init config
+  PyramidMapConfig* config = new PyramidMapConfig("lossy_full_alt");
+  config->SetMapNodeSize(2, 2);
+  config->resolution_num_ = 1;
+  config->map_folder_path_ = "wrong_path";
+
+  // create map node
+  PyramidMapNode* node = new PyramidMapNode();
+  node->Init(config);
+  node->SetMapNodeIndex(index);
+  EXPECT_EQ(node->GetMapNodeIndex().m_, 0);
+  EXPECT_EQ(node->GetMapNodeIndex().n_, 1);
+
+  // check Load
+  EXPECT_FALSE(node->Load());
+  EXPECT_FALSE(node->Load("map.bin"));
+
+  if (node != nullptr) {
+    delete node;
+    node = nullptr;
+  }
+  if (config != nullptr) {
+    delete config;
+    config = nullptr;
+  }
+}
+
+TEST_F(PyramidMapNodeTestSuite, test_base_map_node) {
+  // create index
+  MapNodeIndex index;
+  index.resolution_id_ = 0;
+  index.zone_id_ = 50;
+  index.m_ = 0;
+  index.n_ = 1;
+
+  // init config
+  PyramidMapConfig* config = new PyramidMapConfig("lossy_full_alt");
+  config->SetMapNodeSize(2, 2);
+  config->resolution_num_ = 1;
+
+  // create map node
+  PyramidMapNode* node = new PyramidMapNode();
+  node->Init(config);
+  node->SetMapNodeIndex(index);
+  EXPECT_EQ(node->GetMapNodeIndex().m_, 0);
+  EXPECT_EQ(node->GetMapNodeIndex().n_, 1);
+
+  // add value
+  double data[] = {0.125, 0.125, 1.0};
+  Eigen::Vector3d vector_3d_data(data);
+  EXPECT_FALSE(node->AddValueIfInBound(vector_3d_data, 10, 0));
+  vector_3d_data[0] = 0.375;
+  EXPECT_TRUE(node->AddValueIfInBound(vector_3d_data, 10, 0));
+
+  // Save and Load
+  // Load -> load_binary -> get_header_binary_size -> load_header_binary ->
+  // load_body_binary
+  EXPECT_TRUE(node->Save());
+  EXPECT_TRUE(node->Load());
+
+  // check bad case
+  EXPECT_TRUE(node->GetIsReady());
+
+  // check calculate left top corner
+  Eigen::Vector2d left_top_corner = node->ComputeLeftTopCorner(*config, index);
+  EXPECT_FLOAT_EQ(left_top_corner[0], 0.25);
+  EXPECT_FLOAT_EQ(left_top_corner[1], 0.0);
+
+  // check Get map cell matrix
+  const BaseMapMatrix& map_matrix_const = node->GetMapCellMatrix();
+  const PyramidMapMatrix& pyramid_matrix_const =
+      dynamic_cast<const PyramidMapMatrix&>(map_matrix_const);
+  EXPECT_FLOAT_EQ(*pyramid_matrix_const.GetIntensitySafe(1, 1, 0), 10.f);
+  BaseMapMatrix& map_matrix = node->GetMapCellMatrix();
+  PyramidMapMatrix& pyramid_matrix =
+      dynamic_cast<PyramidMapMatrix&>(map_matrix);
+  EXPECT_FLOAT_EQ(*pyramid_matrix.GetIntensitySafe(1, 1, 0), 10.f);
+
+  // check Get the map config
+  const BaseMapConfig& bm_config = node->GetMapConfig();
+  const PyramidMapConfig& pm_config =
+      dynamic_cast<const PyramidMapConfig&>(bm_config);
+  EXPECT_EQ(pm_config.resolution_num_, 1);
+
+  // check Get the map node config
+  const BaseMapNodeConfig& bmn_config = node->GetMapNodeConfig();
+  EXPECT_EQ(bmn_config.map_version_, MapVersion::LOSSY_FULL_ALT_MAP);
+
+  // check variables
+  EXPECT_FALSE(node->GetIsReserved());
+  EXPECT_FALSE(node->GetIsChanged());
+  node->SetIsReserved(true);
+  node->SetIsChanged(true);
+  node->ResetMapNode();
+  EXPECT_FALSE(node->GetIsReserved());
+  EXPECT_FALSE(node->GetIsChanged());
+
+  // check resolution
+  EXPECT_FLOAT_EQ(node->GetMapResolution(), 0.125f);
+
+  // add vector
+  std::vector<Eigen::Vector3d> coordinates;
+  vector_3d_data[0] = 0.375;
+  vector_3d_data[0] = 0.125;
+  coordinates.push_back(vector_3d_data);
+  std::vector<unsigned char> intensities;
+  intensities.push_back(0);
+  node->AddValueIfInBound(coordinates, intensities, 0);
+  intensities.push_back(1);
+  node->AddValueIfInBound(coordinates, intensities, 0);
+
+  if (node != nullptr) {
+    delete node;
+    node = nullptr;
+  }
+  if (config != nullptr) {
+    delete config;
+    config = nullptr;
+  }
+}
+
+}  // namespace msf
+}  // namespace localization
+}  // namespace apollo

modules/localization/msf/local_pyramid_map/pyramid_map/pyramid_map_test.cc

+/******************************************************************************
+ * Copyright 2019 The Apollo Authors. All Rights Reserved.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *****************************************************************************/
+#include <gtest/gtest.h>
+#include <boost/filesystem.hpp>
+
+#include "modules/localization/msf/local_map/pyramid_map/pyramid_map.h"
+#include "modules/localization/msf/local_map/pyramid_map/pyramid_map_config.h"
+#include "modules/localization/msf/local_map/pyramid_map/pyramid_map_pool.h"
+
+#include "modules/localization/msf/local_map/base_map/base_map.h"
+#include "modules/localization/msf/local_map/base_map/base_map_config.h"
+
+int main(int argc, char** argv) {
+  testing::InitGoogleTest(&argc, argv);
+  return RUN_ALL_TESTS();
+}
+
+namespace apollo {
+namespace localization {
+namespace msf {
+
+class PyramidMapTestSuite : public ::testing::Test {
+ protected:
+  PyramidMapTestSuite() {}
+  virtual ~PyramidMapTestSuite() {}
+  virtual void SetUp() {}
+  virtual void TearDown() {}
+};
+
+void CreateTestMapNode(unsigned int m, unsigned int n,
+                       const MapNodeIndex& index,
+                       const PyramidMapConfig* config) {
+  // create map node
+  PyramidMapNode* node = new PyramidMapNode();
+  node->Init(config);
+  node->SetMapNodeIndex(index);
+
+  // set map node data
+  double data[] = {node->GetLeftTopCorner()[0] + 0.125,
+                   node->GetLeftTopCorner()[1] + 0.125, 1.0};
+  Eigen::Vector3d vec3d(data);
+  EXPECT_TRUE(
+      node->AddValueIfInBound(vec3d,
+      static_cast<unsigned char>(m * config->map_node_size_x_ + n), 0));
+  EXPECT_TRUE(node->Save());
+
+  if (node != NULL) {
+    delete node;
+    node = NULL;
+  }
+}
+
+TEST_F(PyramidMapTestSuite, pyramid_map_function) {
+  // init config
+  PyramidMapConfig* config = new PyramidMapConfig("lossy_full_alt");
+  config->SetMapNodeSize(2, 2);
+  config->resolution_num_ = 1;
+  config->map_folder_path_ = "test_map";
+
+  // create and save nodes
+  unsigned int M = 4;
+  unsigned int N = 4;
+  std::vector<MapNodeIndex> indexes;
+  for (unsigned int m = 0; m < M; ++m) {
+    for (unsigned int n = 0; n < N; ++n) {
+      // create map node index
+      MapNodeIndex index;
+      index.resolution_id_ = 0;
+      index.zone_id_ = 50;
+      index.m_ = m;
+      index.n_ = n;
+      CreateTestMapNode(m, n, index, config);
+      indexes.push_back(index);
+    }
+  }
+  // save config file
+  config->Save("test_map/config.xml");
+
+  // init pyramid map pool
+  PyramidMapNodePool pm_node_pool(16, 4);
+  pm_node_pool.Initial(config);
+
+  // init pyramid map
+  PyramidMap pyramid_map(config);
+  pyramid_map.InitMapNodeCaches(4, 15);
+  pyramid_map.AttachMapNodePool(&pm_node_pool);
+  EXPECT_TRUE(pyramid_map.SetMapFolderPath(config->map_folder_path_));
+
+  // load map node safe from disk
+  PyramidMapNode* pm_node = dynamic_cast<PyramidMapNode*>(
+      pyramid_map.GetMapNodeSafe(*indexes.begin()));
+  double data[] = {0.125, 0.125, 1.0};
+  Eigen::Vector3d loc(data);
+  EXPECT_FLOAT_EQ(
+      pm_node->GetIntensitySafe(loc),
+      static_cast<float>(indexes.begin()->m_ *
+        config->map_node_size_x_ +
+        indexes.begin()->n_));
+  EXPECT_TRUE(pyramid_map.IsMapNodeExist(*indexes.begin()));
+  EXPECT_FALSE(pyramid_map.IsMapNodeExist(*(indexes.begin() + 1)));
+
+  // load map area
+  loc[0] = 0.125 * (config->map_node_size_x_ + 1);
+  loc[1] = 0.125 * (config->map_node_size_x_ + 1);
+  loc[2] = 1.0;
+  Vector3d loc_eigen;
+  loc_eigen[0] = loc[0];
+  loc_eigen[1] = loc[1];
+  EXPECT_TRUE(pyramid_map.LoadMapArea(loc, 0, 50, 0, 0));
+  EXPECT_TRUE(pyramid_map.LoadMapArea(loc_eigen, 0, 50, 0, 0));
+  EXPECT_TRUE(pyramid_map.IsMapNodeExist(*(indexes.begin() + 5)));
+  EXPECT_TRUE(pyramid_map.IsMapNodeExist(*(indexes.begin() + 6)));
+  EXPECT_TRUE(pyramid_map.IsMapNodeExist(*(indexes.begin() + 9)));
+  EXPECT_TRUE(pyramid_map.IsMapNodeExist(*(indexes.begin() + 10)));
+  EXPECT_FALSE(pyramid_map.IsMapNodeExist(*(indexes.begin() + 0)));
+
+  // preload map area
+  Eigen::Vector3d trans_diff;
+  trans_diff[0] = 1.0;
+  trans_diff[1] = 1.0;
+  trans_diff[2] = 1.0;
+  pyramid_map.PreloadMapArea(loc, trans_diff, 0, 50);
+
+  loc_eigen[0] = 0.125;
+  loc_eigen[1] = 0.125;
+  Vector3d trans_diff_eigen;
+  trans_diff_eigen[0] = trans_diff[0];
+  trans_diff_eigen[1] = trans_diff[1];
+  trans_diff_eigen[2] = trans_diff[2];
+  pyramid_map.PreloadMapArea(loc_eigen, trans_diff_eigen, 0, 50);
+
+  // get path
+  pyramid_map.ComputeMd5ForAllMapNodes();
+  std::vector<std::string> paths = pyramid_map.GetAllMapNodePaths();
+  EXPECT_EQ(paths.size(), M * N);
+  std::vector<std::string> md5s = pyramid_map.GetAllMapNodeMd5s();
+  EXPECT_EQ(md5s.size(), M * N);
+
+  // set md5
+  EXPECT_FALSE(pyramid_map.CheckMap());
+  std::map<std::string, std::string> node_md5_map;
+  for (unsigned int i = 0; i < paths.size(); ++i) {
+    node_md5_map[paths[i]] = md5s[i];
+  }
+  config->SetNodeMd5Map(node_md5_map);
+  EXPECT_TRUE(pyramid_map.CheckMap());
+
+  // test PyramidMap function: access specific coordinate
+  EXPECT_FLOAT_EQ(pyramid_map.GetIntensitySafe(loc, 50, 0),
+      1.f * static_cast<float>(config->map_node_size_x_) + 1.f);
+  EXPECT_FLOAT_EQ(pyramid_map.GetIntensityVarSafe(loc, 50, 0), 0.f);
+  EXPECT_FLOAT_EQ(pyramid_map.GetAltitudeSafe(loc, 50, 0), 1.f);
+  EXPECT_FLOAT_EQ(pyramid_map.GetAltitudeVarSafe(loc, 50, 0), 0.f);
+  EXPECT_EQ(pyramid_map.GetCountSafe(loc, 50, 0), 1);
+
+  loc[0] = 1.0;
+  loc[1] = 1.0;
+  loc[2] = 1.0;
+  EXPECT_FLOAT_EQ(pyramid_map.GetGroundAltitudeSafe(loc, 50, 0), 0.f);
+  EXPECT_EQ(pyramid_map.GetGroundCountSafe(loc, 50, 0), 0);
+
+  // reset the caches and pool
+  pyramid_map.InitMapNodeCaches(6, 16);
+
+  if (config != NULL) {
+    delete config;
+    config = NULL;
+  }
+}
+
+TEST_F(PyramidMapTestSuite, test_init) {
+  // init config
+  PyramidMapConfig* config = new PyramidMapConfig("lossy_full_alt");
+  config->SetMapNodeSize(2, 2);
+  config->resolution_num_ = 1;
+  config->map_folder_path_ = "test_map";
+
+  // create and save nodes
+  unsigned int M = 4;
+  unsigned int N = 4;
+  std::vector<MapNodeIndex> indexes;
+  for (unsigned int m = 0; m < M; ++m) {
+    for (unsigned int n = 0; n < N; ++n) {
+      // create map node index
+      MapNodeIndex index;
+      index.resolution_id_ = 0;
+      index.zone_id_ = 50;
+      index.m_ = m;
+      index.n_ = n;
+      CreateTestMapNode(m, n, index, config);
+      indexes.push_back(index);
+    }
+  }
+  // save config file
+  config->Save("test_map/config.xml");
+
+  // init pyramid map pool
+  PyramidMapNodePool pm_node_pool(16, 4);
+  pm_node_pool.Initial(config);
+
+  // init pyramid map
+  PyramidMap pyramid_map(config);
+  pyramid_map.InitMapNodeCaches(4, 15);
+  pyramid_map.AttachMapNodePool(&pm_node_pool);
+  EXPECT_TRUE(pyramid_map.SetMapFolderPath(config->map_folder_path_));
+
+  // check fail case
+  double data[] = {0.125 * (config->map_node_size_x_ + 1),
+                   0.125 * (config->map_node_size_x_ + 1), 1.0};
+  Eigen::Vector3d loc(data);
+  EXPECT_TRUE(pyramid_map.LoadMapArea(loc, 0, 50, 0, 0));
+}
+
+}  // namespace msf
+}  // namespace localization
+}  // namespace apollo