From 6af8e6ac9a933b168af4aa2b81e9b74bfacc908b Mon Sep 17 00:00:00 2001 From: Jaeyoung Lim Date: Tue, 3 Jan 2023 11:31:08 +0100 Subject: [PATCH] Add normal computation --- include/grid_map_geo/grid_map_geo.h | 8 ++++ src/grid_map_geo.cpp | 37 ++++++++++++++++ src/test_normals_node.cpp | 66 +++++++++++++++++++++++++++++ 3 files changed, 111 insertions(+) create mode 100644 src/test_normals_node.cpp diff --git a/include/grid_map_geo/grid_map_geo.h b/include/grid_map_geo/grid_map_geo.h index 16cc1dc..263a51b 100644 --- a/include/grid_map_geo/grid_map_geo.h +++ b/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 diff --git a/src/grid_map_geo.cpp b/src/grid_map_geo.cpp index 0da28ff..b3bcd43 100644 --- a/src/grid_map_geo.cpp +++ b/src/grid_map_geo.cpp @@ -321,6 +321,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 localorigin_wgs84_.espg = src_coord; diff --git a/src/test_normals_node.cpp b/src/test_normals_node.cpp new file mode 100644 index 0000000..8b630c3 --- /dev/null +++ b/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 + */ + +#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("surface_normal_marker", 1, true); + + std::string file_path; + nh_private.param("file_path", file_path, ""); + + std::shared_ptr adaptive_viewutility = std::make_shared(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; +}