Add normal computation

include/grid_map_geo/grid_map_geo.h

@@ -168,6 +168,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(const std::string& layer_name, std::string reference_layer);
+
  protected:
   grid_map::GridMap grid_map_;
   double localorigin_e_{789823.93};  // duerrboden berghaus

src/grid_map_geo.cpp

@@ -324,6 +324,43 @@ bool GridMapGeo::AddLayerOffset(const double offset_distance, const std::string
   return true;
 }
 
+void GridMapGeo::AddLayerNormals(const std::string &layer_name, std::string reference_layer) {
+  grid_map_.add("elevation_normal_x");
+  grid_map_.add("elevation_normal_y");
+  grid_map_.add("elevation_normal_z");
+
+  grid_map::Matrix &layer_elevation = grid_map_[reference_layer];
+  grid_map::Matrix &layer_normal_x = grid_map_["elevation_normal_x"];
+  grid_map::Matrix &layer_normal_y = grid_map_["elevation_normal_y"];
+  grid_map::Matrix &layer_normal_z = grid_map_["elevation_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);
+  }
+}
+
 void GridMapGeo::setGlobalOrigin(ESPG src_coord, const Eigen::Vector3d origin) {
   // Transform global origin into CH1903 / LV03 coordinates
   maporigin_.espg = src_coord;

src/test_normals_node.cpp

@@ -0,0 +1,66 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2021 Jaeyoung Lim. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+/**
+ * @brief Node to test planner in the view utiltiy map
+ *
+ *
+ * @author Jaeyoung Lim <[email protected]>
+ */
+
+#include "adaptive_viewutility/adaptive_viewutility.h"
+#include "terrain_navigation/profiler.h"
+
+int main(int argc, char **argv) {
+  ros::init(argc, argv, "adaptive_viewutility");
+  ros::NodeHandle nh("");
+  ros::NodeHandle nh_private("~");
+
+  ros::Publisher normal_marker_pub = nh.advertise<visualization_msgs::MarkerArray>("surface_normal_marker", 1, true);
+
+  std::string file_path;
+  nh_private.param<std::string>("file_path", file_path, "");
+
+  std::shared_ptr<AdaptiveViewUtility> adaptive_viewutility = std::make_shared<AdaptiveViewUtility>(nh, nh_private);
+  adaptive_viewutility->LoadMap(file_path);
+
+  while (true) {
+    // Visualize results
+    adaptive_viewutility->MapPublishOnce();
+    adaptive_viewutility->NormalPublishOnce(normal_marker_pub);
+
+    ros::Duration(10.0).sleep();
+  }
+
+  ros::spin();
+  return 0;
+}