From 9ff407d9ec83e2e32e19fd8a0d4f8d6802c78c82 Mon Sep 17 00:00:00 2001
From: Jaeyoung-Lim <jalim@ethz.ch>
Date: Mon, 27 Nov 2023 13:38:27 +0100
Subject: [PATCH] Add normal computation

---
 include/grid_map_geo/grid_map_geo.hpp |  8 ++++++
 src/grid_map_geo.cpp                  | 39 ++++++++++++++++++++++++---
 2 files changed, 43 insertions(+), 4 deletions(-)

diff --git a/include/grid_map_geo/grid_map_geo.hpp b/include/grid_map_geo/grid_map_geo.hpp
index aef380a..7d293b6 100644
--- a/include/grid_map_geo/grid_map_geo.hpp
+++ b/include/grid_map_geo/grid_map_geo.hpp
@@ -177,6 +177,14 @@ class GridMapGeo {
    */
   bool AddLayerOffset(const double offset_distance, const std::string& layer_name);
 
+  /**
+   * @brief Compute normal vectors of the surface
+   *
+   * @param layer_name
+   * @param reference_layer
+   */
+  void AddLayerNormals(std::string reference_layer);
+
  protected:
   grid_map::GridMap grid_map_;
   double localorigin_e_{789823.93};  // duerrboden berghaus
diff --git a/src/grid_map_geo.cpp b/src/grid_map_geo.cpp
index 355908e..c5d486e 100644
--- a/src/grid_map_geo.cpp
+++ b/src/grid_map_geo.cpp
@@ -324,8 +324,39 @@ bool GridMapGeo::AddLayerOffset(const double offset_distance, const std::string
   return true;
 }
 
-void GridMapGeo::setGlobalOrigin(ESPG src_coord, const Eigen::Vector3d origin) {
-  // Transform global origin into CH1903 / LV03 coordinates
-  maporigin_.espg = src_coord;
-  maporigin_.position = origin;
+void GridMapGeo::AddLayerNormals(const std::string reference_layer) {
+  grid_map_.add(reference_layer + "_normal_x");
+  grid_map_.add(reference_layer + "_normal_y");
+  grid_map_.add(reference_layer + "_normal_z");
+
+  grid_map::Matrix &layer_elevation = grid_map_[reference_layer];
+  grid_map::Matrix &layer_normal_x = grid_map_[reference_layer + "_normal_x"];
+  grid_map::Matrix &layer_normal_y = grid_map_[reference_layer + "_normal_y"];
+  grid_map::Matrix &layer_normal_z = grid_map_[reference_layer + "_normal_z"];
+
+  unsigned width = grid_map_.getSize()(0);
+  unsigned height = grid_map_.getSize()(1);
+  double resolution = grid_map_.getResolution();
+  // Compute normals from elevation map
+  // Surface normal calculation from: https://www.flipcode.com/archives/Calculating_Vertex_Normals_for_Height_Maps.shtml
+  for (grid_map::GridMapIterator iterator(grid_map_); !iterator.isPastEnd(); ++iterator) {
+    const grid_map::Index gridMapIndex = *iterator;
+
+    /// TODO: Verify normal by visualization
+    int x = gridMapIndex(0);
+    int y = height - 1 - gridMapIndex(1);
+
+    float sx = layer_elevation(x < width - 1 ? x + 1 : x, y) - layer_elevation(x > 0 ? x - 1 : x, y);
+    if (x == 0 || x == width - 1) sx *= 2;
+
+    float sy = layer_elevation(x, y < height - 1 ? y + 1 : y) - layer_elevation(x, y > 0 ? y - 1 : y);
+    if (y == 0 || y == height - 1) sy *= 2;
+
+    Eigen::Vector3d normal(Eigen::Vector3d(sx, sy, 2 * resolution));
+    normal.normalize();
+
+    layer_normal_x(x, y) = normal(0);
+    layer_normal_y(x, y) = normal(1);
+    layer_normal_z(x, y) = normal(2);
+  }
 }