Localization: update pyramid_map module

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

@@ -22,6 +22,7 @@
 namespace apollo {
 namespace localization {
 namespace msf {
+namespace pyramid_map {
 
 template <typename Scalar, int aligned_len = alignof(max_align_t)>
 class AlignedMatrix {
@@ -226,6 +227,7 @@ operator=(const AlignedMatrix<Scalar, aligned_len>& matrix) {
   return *this;
 }
 
+}  // namespace pyramid_map
 }  // namespace msf
 }  // namespace localization
 }  // namespace apollo

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

@@ -13,13 +13,14 @@
  * See the License for the specific language governing permissions and
  * limitations under the License.
  *****************************************************************************/
-#include "modules/localization/msf/local_map/pyramid_map/pyramid_map_config.h"
+#include "modules/localization/msf/local_pyramid_map/pyramid_map/pyramid_map_config.h"
 
 #include <string>
 
 namespace apollo {
 namespace localization {
 namespace msf {
+namespace pyramid_map {
 
 PyramidMapConfig::PyramidMapConfig(const std::string& map_version)
     : BaseMapConfig(map_version) {}
@@ -129,6 +130,7 @@ bool PyramidMapConfig::LoadXml(const boost::property_tree::ptree& config) {
   return false;
 }
 
+}  // namespace pyramid_map
 }  // namespace msf
 }  // namespace localization
 }  // namespace apollo

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

@@ -16,11 +16,12 @@
 #pragma once
 
 #include <string>
-#include "modules/localization/msf/local_map/base_map/base_map_config.h"
+#include "modules/localization/msf/local_pyramid_map/base_map/base_map_config.h"
 
 namespace apollo {
 namespace localization {
 namespace msf {
+namespace pyramid_map {
 
 class PyramidMapConfig : public BaseMapConfig {
  public:
@@ -53,6 +54,7 @@ class PyramidMapConfig : public BaseMapConfig {
   virtual bool LoadXml(const boost::property_tree::ptree& config);
 };
 
+}  // namespace pyramid_map
 }  // namespace msf
 }  // namespace localization
 }  // namespace apollo

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

@@ -14,9 +14,9 @@
  * limitations under the License.
  *****************************************************************************/
 
-#include "modules/localization/msf/local_map/pyramid_map/pyramid_map_config.h"
+#include "modules/localization/msf/local_pyramid_map/pyramid_map/pyramid_map_config.h"
 #include <gtest/gtest.h>
-#include "modules/localization/msf/local_map/base_map/base_map_config.h"
+#include "modules/localization/msf/local_pyramid_map/base_map/base_map_config.h"
 
 int main(int argc, char** argv) {
   testing::InitGoogleTest(&argc, argv);
@@ -26,6 +26,7 @@ int main(int argc, char** argv) {
 namespace apollo {
 namespace localization {
 namespace msf {
+namespace pyramid_map {
 
 TEST(PyramidMapConfigTestSuite, base_config_method) {
   PyramidMapConfig config("lossy_full_alt");
@@ -61,6 +62,7 @@ TEST(PyramidMapConfigTestSuite, base_config_method) {
   config2.Load(config_file);
 }
 
+}  // namespace pyramid_map
 }  // namespace msf
 }  // namespace localization
 }  // namespace apollo

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

@@ -13,13 +13,16 @@
  * See the License for the specific language governing permissions and
  * limitations under the License.
  *****************************************************************************/
-#include "modules/localization/msf/local_map/pyramid_map/pyramid_map_matrix.h"
+#include "modules/localization/msf/local_pyramid_map/pyramid_map/pyramid_map_matrix.h"
 
 #include <algorithm>
+#include <memory>
+#include "cyber/common/log.h"
 
 namespace apollo {
 namespace localization {
 namespace msf {
+namespace pyramid_map {
 
 PyramidMapMatrix::PyramidMapMatrix() { Clear(); }
 
@@ -1478,7 +1481,7 @@ double PyramidMapMatrix::ComputeMeanIntensity(unsigned int level) {
   return avg;
 }
 
-void PyramidMapMatrix::Reduce(PyramidMapMatrix* cells,
+void PyramidMapMatrix::Reduce(std::shared_ptr<PyramidMapMatrix> cells,
                               const PyramidMapMatrix& new_cells,
                               unsigned int level, unsigned int new_level) {
   if (level >= cells->resolution_num_) {
@@ -1515,6 +1518,7 @@ void PyramidMapMatrix::Reduce(PyramidMapMatrix* cells,
   }
 }
 
+}  // namespace pyramid_map
 }  // namespace msf
 }  // namespace localization
 }  // namespace apollo

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

@@ -15,15 +15,17 @@
  *****************************************************************************/
 #pragma once
 
+#include <memory>
 #include <vector>
 #include "modules/localization/msf/common/util/rect2d.h"
-#include "modules/localization/msf/local_map/base_map/base_map_matrix.h"
-#include "modules/localization/msf/local_map/pyramid_map/aligned_matrix.h"
-#include "modules/localization/msf/local_map/pyramid_map/pyramid_map_config.h"
+#include "modules/localization/msf/local_pyramid_map/base_map/base_map_matrix.h"
+#include "modules/localization/msf/local_pyramid_map/pyramid_map/aligned_matrix.h"
+#include "modules/localization/msf/local_pyramid_map/pyramid_map/pyramid_map_config.h"
 
 namespace apollo {
 namespace localization {
 namespace msf {
+namespace pyramid_map {
 
 typedef AlignedMatrix<float> FloatMatrix;
 typedef AlignedMatrix<unsigned int> UIntMatrix;
@@ -199,8 +201,9 @@ class PyramidMapMatrix : public BaseMapMatrix {
   double ComputeMeanIntensity(unsigned int level = 0);
 
   /**@brief Combine two PyramidMapMatrix instances (Reduce). */
-  static void Reduce(PyramidMapMatrix* cells, const PyramidMapMatrix& new_cells,
-                     unsigned int level = 0, unsigned int new_level = 0);
+  static void Reduce(std::shared_ptr<PyramidMapMatrix> cells,
+                     const PyramidMapMatrix& new_cells, unsigned int level = 0,
+                     unsigned int new_level = 0);
 
   inline bool HasIntensity() const;
   inline bool HasIntensityVar() const;
@@ -562,6 +565,7 @@ inline const UIntMatrix* PyramidMapMatrix::GetGroundCountMatrix(
   return &ground_count_matrixes_[level];
 }
 
+}  // namespace pyramid_map
 }  // namespace msf
 }  // namespace localization
 }  // namespace apollo

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

@@ -14,9 +14,9 @@
  * limitations under the License.
  *****************************************************************************/
 
-#include "modules/localization/msf/local_map/pyramid_map/pyramid_map_matrix.h"
+#include "modules/localization/msf/local_pyramid_map/pyramid_map/pyramid_map_matrix.h"
 #include <gtest/gtest.h>
-#include "modules/localization/msf/local_map/pyramid_map/pyramid_map_config.h"
+#include "modules/localization/msf/local_pyramid_map/pyramid_map/pyramid_map_config.h"
 
 int main(int argc, char** argv) {
   testing::InitGoogleTest(&argc, argv);
@@ -26,6 +26,7 @@ int main(int argc, char** argv) {
 namespace apollo {
 namespace localization {
 namespace msf {
+namespace pyramid_map {
 
 class PyramidMapMatrixTestSuite : public ::testing::Test {
  protected:
@@ -40,7 +41,7 @@ TEST_F(PyramidMapMatrixTestSuite, constructor_and_init) {
   std::unique_ptr<PyramidMapConfig> config(
       new PyramidMapConfig("lossy_full_alt"));
   config->SetMapNodeSize(1024, 1024);
-  std::unique_ptr<PyramidMapMatrix> pm_matrix(new PyramidMapMatrix());
+  std::shared_ptr<PyramidMapMatrix> pm_matrix(new PyramidMapMatrix());
   pm_matrix->Init(*config);
 
   EXPECT_EQ(pm_matrix->rows_mr_[0], 1024);
@@ -56,7 +57,7 @@ TEST_F(PyramidMapMatrixTestSuite, constructor_and_init) {
   EXPECT_EQ(pm_matrix->ratio_, 2);
 
   // check operator =
-  std::unique_ptr<PyramidMapMatrix> pm_matrix2(new PyramidMapMatrix());
+  std::shared_ptr<PyramidMapMatrix> pm_matrix2(new PyramidMapMatrix());
   config->SetMapNodeSize(2, 2);
   pm_matrix2->Init(*config);
   EXPECT_EQ(pm_matrix2->rows_mr_[0], 2);
@@ -79,7 +80,7 @@ TEST_F(PyramidMapMatrixTestSuite, get_and_set_intensity) {
   std::unique_ptr<PyramidMapConfig> config(
       new PyramidMapConfig("lossy_full_alt"));
   config->SetMapNodeSize(2, 2);
-  std::unique_ptr<PyramidMapMatrix> pm_matrix(new PyramidMapMatrix());
+  std::shared_ptr<PyramidMapMatrix> pm_matrix(new PyramidMapMatrix());
 
   // check getter safe before initializtion
   EXPECT_EQ(pm_matrix->GetIntensityMatrixSafe(), nullptr);
@@ -132,7 +133,7 @@ TEST_F(PyramidMapMatrixTestSuite, get_and_set_intensity_var) {
   std::unique_ptr<PyramidMapConfig> config(
       new PyramidMapConfig("lossy_full_alt"));
   config->SetMapNodeSize(2, 2);
-  std::unique_ptr<PyramidMapMatrix> pm_matrix(new PyramidMapMatrix());
+  std::shared_ptr<PyramidMapMatrix> pm_matrix(new PyramidMapMatrix());
 
   // check getter safe before initializtion
   EXPECT_EQ(pm_matrix->GetIntensityVarMatrixSafe(), nullptr);
@@ -178,7 +179,7 @@ TEST_F(PyramidMapMatrixTestSuite, get_and_set_altitude) {
   std::unique_ptr<PyramidMapConfig> config(
       new PyramidMapConfig("lossy_full_alt"));
   config->SetMapNodeSize(2, 2);
-  std::unique_ptr<PyramidMapMatrix> pm_matrix(new PyramidMapMatrix());
+  std::shared_ptr<PyramidMapMatrix> pm_matrix(new PyramidMapMatrix());
 
   // check getter safe before initializtion
   EXPECT_EQ(pm_matrix->GetAltitudeMatrixSafe(), nullptr);
@@ -244,7 +245,7 @@ TEST_F(PyramidMapMatrixTestSuite, get_and_set_altitude_var) {
   std::unique_ptr<PyramidMapConfig> config(
       new PyramidMapConfig("lossy_full_alt"));
   config->SetMapNodeSize(2, 2);
-  std::unique_ptr<PyramidMapMatrix> pm_matrix(new PyramidMapMatrix());
+  std::shared_ptr<PyramidMapMatrix> pm_matrix(new PyramidMapMatrix());
 
   // check getter safe before initializtion
   EXPECT_EQ(pm_matrix->GetAltitudeVarMatrixSafe(), nullptr);
@@ -290,7 +291,7 @@ TEST_F(PyramidMapMatrixTestSuite, get_and_set_ground_altitude) {
   std::unique_ptr<PyramidMapConfig> config(
       new PyramidMapConfig("lossy_full_alt"));
   config->SetMapNodeSize(2, 2);
-  std::unique_ptr<PyramidMapMatrix> pm_matrix(new PyramidMapMatrix());
+  std::shared_ptr<PyramidMapMatrix> pm_matrix(new PyramidMapMatrix());
 
   // check getter safe before initializtion
   EXPECT_EQ(pm_matrix->GetGroundAltitudeMatrixSafe(), nullptr);
@@ -337,7 +338,7 @@ TEST_F(PyramidMapMatrixTestSuite, get_and_set_count) {
   std::unique_ptr<PyramidMapConfig> config(
       new PyramidMapConfig("lossy_full_alt"));
   config->SetMapNodeSize(2, 2);
-  std::unique_ptr<PyramidMapMatrix> pm_matrix(new PyramidMapMatrix());
+  std::shared_ptr<PyramidMapMatrix> pm_matrix(new PyramidMapMatrix());
 
   // check getter safe before initializtion
   EXPECT_EQ(pm_matrix->GetCountMatrixSafe(), nullptr);
@@ -382,7 +383,7 @@ TEST_F(PyramidMapMatrixTestSuite, get_and_set_ground_count) {
   std::unique_ptr<PyramidMapConfig> config(
       new PyramidMapConfig("lossy_full_alt"));
   config->SetMapNodeSize(2, 2);
-  std::unique_ptr<PyramidMapMatrix> pm_matrix(new PyramidMapMatrix());
+  std::shared_ptr<PyramidMapMatrix> pm_matrix(new PyramidMapMatrix());
 
   // check getter safe before initializtion
   EXPECT_EQ(pm_matrix->GetGroundCountMatrixSafe(), nullptr);
@@ -428,7 +429,7 @@ TEST_F(PyramidMapMatrixTestSuite, set_and_reset_and_roi) {
   std::unique_ptr<PyramidMapConfig> config(
       new PyramidMapConfig("lossy_full_alt"));
   config->SetMapNodeSize(3, 3);
-  std::unique_ptr<PyramidMapMatrix> pm_matrix(new PyramidMapMatrix());
+  std::shared_ptr<PyramidMapMatrix> pm_matrix(new PyramidMapMatrix());
   pm_matrix->Init(*config);
 
   EXPECT_EQ(pm_matrix->rows_mr_[0], 3);
@@ -533,7 +534,7 @@ TEST_F(PyramidMapMatrixTestSuite, merge_and_add_and_reduce) {
   std::unique_ptr<PyramidMapConfig> config(
       new PyramidMapConfig("lossy_full_alt"));
   config->SetMapNodeSize(3, 3);
-  std::unique_ptr<PyramidMapMatrix> pm_matrix(new PyramidMapMatrix());
+  std::shared_ptr<PyramidMapMatrix> pm_matrix(new PyramidMapMatrix());
   pm_matrix->Init(*config);
 
   EXPECT_EQ(pm_matrix->rows_mr_[0], 3);
@@ -612,7 +613,7 @@ TEST_F(PyramidMapMatrixTestSuite, merge_and_add_and_reduce) {
   pm_matrix->SetGroundCountMatrix(uint_data, 9, 0);
   pm_matrix->SetCountMatrix(uint_data, 9, 0);
 
-  std::unique_ptr<PyramidMapMatrix> pm_matrix2(new PyramidMapMatrix());
+  std::shared_ptr<PyramidMapMatrix> pm_matrix2(new PyramidMapMatrix());
   pm_matrix2->Init(*config);
   pm_matrix2->SetIntensityMatrix(float_data, 9, 0);
   pm_matrix2->SetIntensityVarMatrix(float_data, 9, 0);
@@ -640,7 +641,7 @@ TEST_F(PyramidMapMatrixTestSuite, add_merge_get_base) {
   std::unique_ptr<PyramidMapConfig> config(
       new PyramidMapConfig("lossy_full_alt"));
   config->SetMapNodeSize(3, 3);
-  std::unique_ptr<PyramidMapMatrix> pm_matrix(new PyramidMapMatrix());
+  std::shared_ptr<PyramidMapMatrix> pm_matrix(new PyramidMapMatrix());
   pm_matrix->Init(*config);
 
   EXPECT_EQ(pm_matrix->rows_mr_[0], 3);
@@ -684,7 +685,7 @@ TEST_F(PyramidMapMatrixTestSuite, BottomUp) {
       new PyramidMapConfig("lossy_full_alt"));
   config->SetMapNodeSize(4, 4);
   config->resolution_num_ = 2;
-  std::unique_ptr<PyramidMapMatrix> pm_matrix(new PyramidMapMatrix());
+  std::shared_ptr<PyramidMapMatrix> pm_matrix(new PyramidMapMatrix());
   pm_matrix->Init(*config);
 
   EXPECT_EQ(pm_matrix->rows_mr_[0], 4);
@@ -743,7 +744,7 @@ TEST_F(PyramidMapMatrixTestSuite, flags_are_false_and_resolution_are_two) {
   config->has_ground_altitude_ = false;
   config->has_count_ = false;
   config->has_ground_count_ = false;
-  std::unique_ptr<PyramidMapMatrix> pm_matrix(new PyramidMapMatrix());
+  std::shared_ptr<PyramidMapMatrix> pm_matrix(new PyramidMapMatrix());
   pm_matrix->Init(*config);
 
   // bottom up safe
@@ -760,7 +761,7 @@ TEST_F(PyramidMapMatrixTestSuite, check_const_matrix) {
       new PyramidMapConfig("lossy_full_alt"));
   config->SetMapNodeSize(2, 2);
   config->resolution_num_ = 1;
-  std::unique_ptr<PyramidMapMatrix> pm_matrix(new PyramidMapMatrix());
+  std::shared_ptr<PyramidMapMatrix> pm_matrix(new PyramidMapMatrix());
   pm_matrix->Init(*config);
 
   const PyramidMapMatrix const_matrix(*pm_matrix);
@@ -781,7 +782,7 @@ TEST_F(PyramidMapMatrixTestSuite, check_const_matrix) {
   EXPECT_EQ((*const_matrix.GetGroundCountMatrix(0))[0][0], 0);
 
   // bad case 1: resolution is 1024
-  std::unique_ptr<PyramidMapMatrix> pm_matrix2(new PyramidMapMatrix());
+  std::shared_ptr<PyramidMapMatrix> pm_matrix2(new PyramidMapMatrix());
   pm_matrix2->Init(*config);
   const PyramidMapMatrix const_matrix2(*pm_matrix2);
   EXPECT_EQ(const_matrix2.GetIntensityMatrixSafe(1024), nullptr);
@@ -801,7 +802,7 @@ TEST_F(PyramidMapMatrixTestSuite, check_const_matrix) {
   config->has_ground_altitude_ = false;
   config->has_count_ = false;
   config->has_ground_count_ = false;
-  std::unique_ptr<PyramidMapMatrix> pm_matrix3(new PyramidMapMatrix());
+  std::shared_ptr<PyramidMapMatrix> pm_matrix3(new PyramidMapMatrix());
   pm_matrix3->Init(*config);
   const PyramidMapMatrix const_matrix3(*pm_matrix3);
   EXPECT_EQ(const_matrix3.GetIntensityMatrixSafe(0), nullptr);
@@ -813,6 +814,7 @@ TEST_F(PyramidMapMatrixTestSuite, check_const_matrix) {
   EXPECT_EQ(const_matrix3.GetGroundCountMatrixSafe(0), nullptr);
 }
 
+}  // namespace pyramid_map
 }  // namespace msf
 }  // namespace localization
 }  // namespace apollo