Always place dem at center of the map (#21)

* Always place dem at center of the map F Do not align altitude F * remove local origins
ethz-asl · Feb 25, 2023 · 64161b8 · 64161b8
1 parent cd568e9
commit 64161b8
Show file tree

Hide file tree

Showing 2 changed files with 26 additions and 23 deletions.
diff --git a/include/grid_map_geo/grid_map_geo.h b/include/grid_map_geo/grid_map_geo.h
@@ -84,8 +84,8 @@ class GridMapGeo {
    * @param origin
    */
   void getGlobalOrigin(ESPG& src_coord, Eigen::Vector3d& origin) {
-    src_coord = localorigin_wgs84_.espg;
-    origin = localorigin_wgs84_.position;
+    src_coord = maporigin_.espg;
+    origin = maporigin_.position;
   };
 
   /**
@@ -173,6 +173,6 @@ class GridMapGeo {
   double localorigin_e_{789823.93};  // duerrboden berghaus
   double localorigin_n_{177416.56};
   double localorigin_altitude_{0.0};
-  Location localorigin_wgs84_;
+  Location maporigin_;
 };
 #endif
diff --git a/src/grid_map_geo.cpp b/src/grid_map_geo.cpp
@@ -94,25 +94,28 @@ bool GridMapGeo::initializeFromGeotiff(const std::string &path, bool align_terra
 
   double mapcenter_e = originX + pixelSizeX * width * 0.5;
   double mapcenter_n = originY + pixelSizeY * height * 0.5;
-
-  Eigen::Vector3d origin_lv03 =
-      transformCoordinates(ESPG::WGS84, std::string(pszProjection), localorigin_wgs84_.position);
-  localorigin_e_ = origin_lv03(0);
-  localorigin_n_ = origin_lv03(1);
-  localorigin_altitude_ = origin_lv03(2);
+  maporigin_.espg = ESPG::CH1903_LV03;
+  maporigin_.position = Eigen::Vector3d(mapcenter_e, mapcenter_n, 0.0);
 
   Eigen::Vector2d position{Eigen::Vector2d::Zero()};
 
-  if (align_terrain) {
-    std::cout << "[GridMapGeo] Aligning terrain!" << std::endl;
-    double map_position_x = mapcenter_e - localorigin_e_;
-    double map_position_y = mapcenter_n - localorigin_n_;
-    position = Eigen::Vector2d(map_position_x, map_position_y);
-  } else {
-    std::cout << "[GridMapGeo] Not aligning terrain!" << std::endl;
-  }
+  /// TODO: Generalize to set local origin as center of map position
+  // Eigen::Vector3d origin_lv03 =
+  //     transformCoordinates(ESPG::WGS84, std::string(pszProjection), localorigin_wgs84_.position);
+  // localorigin_e_ = origin_lv03(0);
+  // localorigin_n_ = origin_lv03(1);
+  // localorigin_altitude_ = origin_lv03(2);
+  // if (align_terrain) {
+  //   std::cout << "[GridMapGeo] Aligning terrain!" << std::endl;
+  //   double map_position_x = mapcenter_e - localorigin_e_;
+  //   double map_position_y = mapcenter_n - localorigin_n_;
+  //   position = Eigen::Vector2d(map_position_x, map_position_y);
+  // } else {
+  //   std::cout << "[GridMapGeo] Not aligning terrain!" << std::endl;
+  // }
 
   grid_map_.setGeometry(length, resolution, position);
+  /// TODO: Use TF for geocoordinates
   grid_map_.setFrameId("map");
   grid_map_.add("elevation");
   GDALRasterBand *elevationBand = dataset->GetRasterBand(1);
@@ -137,10 +140,10 @@ bool GridMapGeo::initializeFromGeotiff(const std::string &path, bool align_terra
     altitude = grid_map_.atPosition("elevation", Eigen::Vector2d(0.0, 0.0));
   }
 
-  Eigen::Translation3d meshlab_translation(0.0, 0.0, -altitude);
-  Eigen::AngleAxisd meshlab_rotation(Eigen::AngleAxisd::Identity());
-  Eigen::Isometry3d transform = meshlab_translation * meshlab_rotation;  // Apply affine transformation.
-  grid_map_ = grid_map_.getTransformedMap(transform, "elevation", grid_map_.getFrameId(), true);
+  // Eigen::Translation3d meshlab_translation(0.0, 0.0, -altitude);
+  // Eigen::AngleAxisd meshlab_rotation(Eigen::AngleAxisd::Identity());
+  // Eigen::Isometry3d transform = meshlab_translation * meshlab_rotation;  // Apply affine transformation.
+  // grid_map_ = grid_map_.getTransformedMap(transform, "elevation", grid_map_.getFrameId(), true);
   return true;
 }
 
@@ -323,6 +326,6 @@ bool GridMapGeo::AddLayerOffset(const double offset_distance, const std::string
 
 void GridMapGeo::setGlobalOrigin(ESPG src_coord, const Eigen::Vector3d origin) {
   // Transform global origin into CH1903 / LV03 coordinates
-  localorigin_wgs84_.espg = src_coord;
-  localorigin_wgs84_.position = origin;
+  maporigin_.espg = src_coord;
+  maporigin_.position = origin;
 }