diff --git a/grid_map_pcl/CHANGELOG.rst b/grid_map_pcl/CHANGELOG.rst index 533a43142..ab8d2914b 100644 --- a/grid_map_pcl/CHANGELOG.rst +++ b/grid_map_pcl/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package grid_map_pcl ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.7.0 (2022-03-17) +------------------ + 1.6.4 (2020-12-04) ------------------ diff --git a/grid_map_pcl/CMakeLists.txt b/grid_map_pcl/CMakeLists.txt index a8dc5573d..d82b918e7 100644 --- a/grid_map_pcl/CMakeLists.txt +++ b/grid_map_pcl/CMakeLists.txt @@ -99,7 +99,7 @@ target_link_libraries(${PROJECT_NAME} ) -# Node. +# Nodes. add_executable(grid_map_pcl_loader_node src/grid_map_pcl_loader_node.cpp ) @@ -118,6 +118,24 @@ target_link_libraries(grid_map_pcl_loader_node ${catkin_LIBRARIES} ) +add_executable(pointcloud_publisher_node + src/pointcloud_publisher_node.cpp +) +add_dependencies(pointcloud_publisher_node + ${PROJECT_NAME} +) +target_include_directories(pointcloud_publisher_node PRIVATE + include +) +target_include_directories(pointcloud_publisher_node SYSTEM PUBLIC + ${catkin_INCLUDE_DIRS} + ${EIGEN3_INCLUDE_DIR} +) +target_link_libraries(pointcloud_publisher_node + ${PROJECT_NAME} + ${catkin_LIBRARIES} +) + ############# ## Install ## ############# @@ -125,6 +143,7 @@ install( TARGETS ${PROJECT_NAME} grid_map_pcl_loader_node + pointcloud_publisher_node ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} @@ -162,6 +181,14 @@ install( if(CATKIN_ENABLE_TESTING) set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -pthread") + find_package(catkin REQUIRED + COMPONENTS + ${CATKIN_PACKAGE_DEPENDENCIES} + roslaunch + ) + + roslaunch_add_file_check(launch) + ## Add gtest based cpp test target and link libraries catkin_add_gtest(${PROJECT_NAME}-test test/test_grid_map_pcl.cpp diff --git a/grid_map_pcl/README.md b/grid_map_pcl/README.md index 51c30ed55..8947b5a56 100644 --- a/grid_map_pcl/README.md +++ b/grid_map_pcl/README.md @@ -14,6 +14,19 @@ The elevation is computed by slicing the point cloud in the x-y plane into colum **Authors: Edo Jelavic, Dominic Jud
Affiliation: [ETH Zurich, Robotics Systems Lab](https://rsl.ethz.ch/)
** +## Publications +The code for point cloud to grid map conversion has been developed as a part of research on autonomous precision harvesting. If you are using the point cloud to grid map conversion, please add the following citation to your publication: + +Jelavic, E., Jud, D., Egli, P. and Hutter, M., 2021. Towards Autonomous Robotic Precision Harvesting: Mapping, Localization, Planning and Control for a Legged Tree Harvester. + + @article{jelavic2021towards, + title={Towards Autonomous Robotic Precision Harvesting: Mapping, Localization, Planning and Control for a Legged Tree Harvester}, + author={Jelavic, Edo and Jud, Dominic and Egli, Pascal and Hutter, Marco}, + journal={Field Robotics}, + year={2021}, + publisher={Wiley} + } + ## Examples Examples of elevation maps computed from point clouds using this package: @@ -30,15 +43,15 @@ Examples of elevation maps computed from point clouds using this package: ## Usage -The algorithm will open the .pcd file, convert the point cloud to a grid map and save the grid map as a rosbag into the folder specified by the user. +The algorithm will open the `.pcd` file, convert the point cloud to a grid map and save the grid map as a rosbag into the folder specified by the user. -1. Place .pcd files in the package folder or anywhere on the system (e.g. grid_map_pcl/data/example.pcd). -2. Modify the *folder_path* inside the launch file such that the folder file points to the folder containing .pcd files (e.g. /*"path to the grid_map_pcl folder"*/data). -3. Change the *pcd_filename* to the point cloud file that you want to process -4. You can run the algorithm with: *roslaunch grid_map_pcl grid_map_pcl_loader_node.launch* -5. Once the algorithm is done you will see the output in the console, then you can run rviz in a separate terminal (make sure that you have sourced your workspace, **DO NOT CLOSE** the terminal where *grid_map_pcl_loader_node* is running ) and visualize the resulting grid map. Instructions on how to visualize a grid map are in the grid map [README](../README.md). +1. Place your `.pcd` file in the package folder or anywhere on the system (e.g. `/.../grid_map_pcl/data/example.pcd`). +2. Modify the `pcd_filename` inside the launch file such that it points to the `.pcd` file you would like to process (e.g. `/.../grid_map_pcl/data/example.pcd`). Set the `output_grid_map` variable to point to the location where you wish to save the resulting grid map (e.g. `/.../grid_map_pcl/data/example_grid_map.bag`) +3. Change the `configFilePath_` variable in the launch file to point to the `.yaml` file with configuration parameters (e.g. `/.../grid_map_pcl/config/parameters.yaml`) +4. You can run the algorithm with: `roslaunch grid_map_pcl grid_map_pcl_loader_node.launch` +5. Once the algorithm is done you will see the output in the console, and it should visualize the point cloud and the grid map in rviz. If that does not happen, then you can run rviz in a separate terminal (make sure that you have sourced your workspace, **DO NOT CLOSE** the terminal where `grid_map_pcl_loader_node` is running ) and visualize the resulting grid map. Instructions on how to visualize a grid map are in the grid map [README](../README.md). -The resulting grid map will be saved in the folder given by *folder_path* variable in the launch file and will be named with a string contained inside the *output_grid_map* variable. For large point clouds (100M-140M points) the algorithm takes about 30-60 min to finish (with 6 threads). For sizes that are in the range of 40M to 60M points, the runtime varies between 5 and 15 min, depending on the number of points. Point cloud with around 10M points or less can be processed in a minute or two. +The resulting grid map will be saved in to the location pointed by `output_grid_map` variable in the launch file. For large point clouds (100M-140M points) the algorithm takes about 30-60 min to finish (with 6 threads). For sizes that are in the range of 40M to 60M points, the runtime varies between 5 and 15 min, depending on the number of points. Point cloud with around 10M points or less can be processed in a minute or two. ## Parameters @@ -50,6 +63,7 @@ The resulting grid map will be saved in the folder given by *folder_path* variab ##### Grid map parameters Resulting grid map parameters. * **pcl_grid_map_extraction/grid_map/min_num_points_per_cell** Minimum number of points in the point cloud that have to fall within any of the grid map cells. Otherwise the cell elevation will be set to NaN. +* **pcl_grid_map_extraction/grid_map/max_num_points_per_cell** Maximum number of points in the point cloud that are allowed to fall within any of the grid map cells. If there is more points, the value will be set to NaN. This number can be used to ignore cells with a lot of points which speeds up the processing (but results with some holes in the map). * **pcl_grid_map_extraction/grid_map/resolution** Resolution of the grid map. Width and lengths are computed automatically. ### Point Cloud Pre-processing Parameters diff --git a/grid_map_pcl/config/parameters.yaml b/grid_map_pcl/config/parameters.yaml index ea4854ab9..4db5d418a 100644 --- a/grid_map_pcl/config/parameters.yaml +++ b/grid_map_pcl/config/parameters.yaml @@ -15,17 +15,18 @@ pcl_grid_map_extraction: max_num_points: 1000000 use_max_height_as_cell_elevation: false outlier_removal: - is_remove_outliers: true + is_remove_outliers: false mean_K: 10 stddev_threshold: 1.0 downsampling: - is_downsample_cloud: true + is_downsample_cloud: false voxel_size: x: 0.02 y: 0.02 z: 0.02 grid_map: min_num_points_per_cell: 4 + max_num_points_per_cell: 100000 resolution: 0.1 diff --git a/grid_map_pcl/include/grid_map_pcl/GridMapPclLoader.hpp b/grid_map_pcl/include/grid_map_pcl/GridMapPclLoader.hpp index 920be8e98..e7ba410ab 100644 --- a/grid_map_pcl/include/grid_map_pcl/GridMapPclLoader.hpp +++ b/grid_map_pcl/include/grid_map_pcl/GridMapPclLoader.hpp @@ -92,6 +92,12 @@ class GridMapPclLoader { */ void loadParameters(const std::string& filename); + /*! + * Set algorithm's parameters. + * @param[in] parameters of the algorithm + */ + void setParameters(const grid_map_pcl::PclLoaderParameters::Parameters parameters); + //! @return the parameters. const grid_map_pcl::PclLoaderParameters::Parameters& getParameters() const { return params_.get(); } diff --git a/grid_map_pcl/include/grid_map_pcl/PclLoaderParameters.hpp b/grid_map_pcl/include/grid_map_pcl/PclLoaderParameters.hpp index 18b2b2e7f..81f2172eb 100644 --- a/grid_map_pcl/include/grid_map_pcl/PclLoaderParameters.hpp +++ b/grid_map_pcl/include/grid_map_pcl/PclLoaderParameters.hpp @@ -46,6 +46,7 @@ class PclLoaderParameters { struct GridMapParameters { double resolution_ = 0.1; unsigned int minCloudPointsPerCell_ = 2; + unsigned int maxCloudPointsPerCell_ = 100000; }; struct Parameters { @@ -64,7 +65,6 @@ class PclLoaderParameters { PclLoaderParameters() = default; ~PclLoaderParameters() = default; - private: /*! * Load parameters for the GridMapPclLoader class. * @param[in] full path to the config file with parameters @@ -76,7 +76,7 @@ class PclLoaderParameters { * Invoke operator[] on the yaml node. Finds * the parameters in the yaml tree. */ - void handleYamlNode(const YAML::Node& yamlNode); + void loadParameters(const YAML::Node& yamlNode); /*! * Saves typing parameters_ in the owner class. The owner of this @@ -85,6 +85,7 @@ class PclLoaderParameters { */ const Parameters& get() const; + private: // Parameters for the GridMapPclLoader class. Parameters parameters_; }; diff --git a/grid_map_pcl/include/grid_map_pcl/helpers.hpp b/grid_map_pcl/include/grid_map_pcl/helpers.hpp index 824787076..d818e28bf 100644 --- a/grid_map_pcl/include/grid_map_pcl/helpers.hpp +++ b/grid_map_pcl/include/grid_map_pcl/helpers.hpp @@ -22,7 +22,7 @@ namespace grid_map_pcl { void setVerbosityLevelToDebugIfFlagSet(const ros::NodeHandle& nh); -std::string getParameterPath(); +std::string getParameterPath(const ros::NodeHandle& nh); std::string getOutputBagPath(const ros::NodeHandle& nh); diff --git a/grid_map_pcl/launch/grid_map_pcl_loader_node.launch b/grid_map_pcl/launch/grid_map_pcl_loader_node.launch index fde5fd36a..06fd69ed5 100644 --- a/grid_map_pcl/launch/grid_map_pcl_loader_node.launch +++ b/grid_map_pcl/launch/grid_map_pcl_loader_node.launch @@ -1,24 +1,44 @@ - - + + + - + + - - + + + - + - \ No newline at end of file + + + + + + + + + + diff --git a/grid_map_pcl/package.xml b/grid_map_pcl/package.xml index 557587061..e8e5b43aa 100644 --- a/grid_map_pcl/package.xml +++ b/grid_map_pcl/package.xml @@ -1,7 +1,7 @@ grid_map_pcl - 1.6.4 + 1.7.5 Conversions between grid maps and Point Cloud Library (PCL) types. Maximilian Wulf Yoshua Nava @@ -12,7 +12,9 @@ Edo Jelavic catkin + + grid_map_core grid_map_msgs grid_map_ros @@ -20,6 +22,9 @@ pcl_ros roscpp yaml-cpp + + rviz + gtest diff --git a/grid_map_pcl/rviz/grid_map_vis.rviz b/grid_map_pcl/rviz/grid_map_vis.rviz new file mode 100644 index 000000000..4e753d636 --- /dev/null +++ b/grid_map_pcl/rviz/grid_map_vis.rviz @@ -0,0 +1,162 @@ +Panels: + - Class: rviz/Displays + Help Height: 0 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /PointCloud21/Autocompute Value Bounds1 + Splitter Ratio: 0.6114649772644043 + Tree Height: 797 + - Class: rviz/Selection + Name: Selection + - Class: rviz/Tool Properties + Expanded: + - /2D Pose Estimate1 + - /2D Nav Goal1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: rviz/Time + Experimental: false + Name: Time + SyncMode: 0 + SyncSource: PointCloud2 +Preferences: + PromptSaveOnExit: true +Toolbars: + toolButtonStyle: 2 +Visualization Manager: + Class: "" + Displays: + - Alpha: 1 + Class: rviz/Axes + Enabled: true + Length: 2 + Name: Axes + Radius: 0.20000000298023224 + Reference Frame: + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Class: grid_map_rviz_plugin/GridMap + Color: 200; 200; 200 + Color Layer: elevation + Color Transformer: GridMapLayer + ColorMap: default + Enabled: false + Grid Cell Decimation: 1 + Grid Line Thickness: 0.10000000149011612 + Height Layer: elevation + Height Transformer: GridMapLayer + History Length: 1 + Invert ColorMap: false + Max Color: 255; 255; 255 + Max Intensity: 10 + Min Color: 85; 87; 83 + Min Intensity: 0 + Name: GridMap + Show Grid Lines: true + Topic: /grid_map_pcl_loader_node/grid_map_from_raw_pointcloud + Unreliable: false + Use ColorMap: true + Value: false + - Alpha: 0.9900000095367432 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 4.018562316894531 + Min Value: -2.210437059402466 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/PointCloud2 + Color: 164; 0; 0 + Color Transformer: AxisColor + Decay Time: 0 + Enabled: false + Invert Rainbow: false + Max Color: 255; 255; 255 + Min Color: 0; 0; 0 + Name: PointCloud2 + Position Transformer: XYZ + Queue Size: 1 + Selectable: true + Size (Pixels): 3 + Size (m): 0.009999999776482582 + Style: Flat Squares + Topic: /point_cloud_publisher_node/raw_pointcloud + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: false + Enabled: true + Global Options: + Background Color: 46; 52; 54 + Default Light: true + Fixed Frame: map + Frame Rate: 30 + Name: root + Tools: + - Class: rviz/Interact + Hide Inactive Objects: true + - Class: rviz/MoveCamera + - Class: rviz/Select + - Class: rviz/FocusCamera + - Class: rviz/Measure + - Class: rviz/SetInitialPose + Theta std deviation: 0.2617993950843811 + Topic: /initialpose + X std deviation: 0.5 + Y std deviation: 0.5 + - Class: rviz/SetGoal + Topic: /move_base_simple/goal + - Class: rviz/PublishPoint + Single click: true + Topic: /clicked_point + Value: true + Views: + Current: + Class: rviz/Orbit + Distance: 26.305757522583008 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Field of View: 0.7853981852531433 + Focal Point: + X: -3.3914899826049805 + Y: -4.434649467468262 + Z: -1.8720630407333374 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 0.5947977900505066 + Target Frame: + Yaw: 1.7004344463348389 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 1016 + Hide Left Dock: false + Hide Right Dock: true + QMainWindow State: 000000ff00000000fd00000004000000000000016a0000035afc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d0000035a000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f0000040efc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a0056006900650077007300000000280000040e000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007380000003efc0100000002fb0000000800540069006d0065010000000000000738000002eb00fffffffb0000000800540069006d00650100000000000004500000000000000000000005c80000035a00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: true + Width: 1848 + X: 72 + Y: 27 diff --git a/grid_map_pcl/src/GridMapPclLoader.cpp b/grid_map_pcl/src/GridMapPclLoader.cpp index 50ef8a8d4..281d39dd7 100644 --- a/grid_map_pcl/src/GridMapPclLoader.cpp +++ b/grid_map_pcl/src/GridMapPclLoader.cpp @@ -100,17 +100,18 @@ void GridMapPclLoader::addLayerFromInputCloud(const std::string& layer) { ROS_INFO_STREAM("Started adding layer: " << layer); // Preprocess: allocate memory in the internal data structure preprocessGridMapCells(); + ROS_INFO("Finished preprocessing"); workingGridMap_.add(layer); grid_map::Matrix& gridMapData = workingGridMap_.get(layer); unsigned int linearGridMapSize = workingGridMap_.getSize().prod(); + // Iterate through grid map and calculate the corresponding height based on the point cloud #ifndef GRID_MAP_PCL_OPENMP_FOUND ROS_WARN_STREAM("OpemMP not found, defaulting to single threaded implementation"); #else omp_set_num_threads(params_.get().numThreads_); #pragma omp parallel for schedule(dynamic, 10) #endif - // Iterate through grid map and calculate the corresponding height based on the point cloud for (unsigned int linearIndex = 0; linearIndex < linearGridMapSize; ++linearIndex) { processGridMapCell(linearIndex, &gridMapData); } @@ -125,7 +126,11 @@ void GridMapPclLoader::processGridMapCell(const unsigned int linearGridMapIndex, pointsInsideCellBorder = getPointcloudInsideGridMapCellBorder(index); const bool isTooFewPointsInCell = pointsInsideCellBorder->size() < params_.get().gridMap_.minCloudPointsPerCell_; if (isTooFewPointsInCell) { - ROS_WARN_STREAM_THROTTLE(10.0, "Less than " << params_.get().gridMap_.minCloudPointsPerCell_ << " points in a cell"); + ROS_WARN_STREAM_THROTTLE(10.0, "Less than " << params_.get().gridMap_.minCloudPointsPerCell_ << " points in a cell. Skipping."); + return; + } + if (pointsInsideCellBorder->size() > params_.get().gridMap_.maxCloudPointsPerCell_) { + ROS_WARN_STREAM_THROTTLE(10.0, "More than " << params_.get().gridMap_.maxCloudPointsPerCell_ << " points in a cell. Skipping."); return; } auto& clusterHeights = clusterHeightsWithingGridMapCell_[index(0)][index(1)]; @@ -165,6 +170,10 @@ void GridMapPclLoader::loadParameters(const std::string& filename) { pointcloudProcessor_.loadParameters(filename); } +void GridMapPclLoader::setParameters(grid_map_pcl::PclLoaderParameters::Parameters parameters) { + params_.parameters_ = std::move(parameters); +} + void GridMapPclLoader::savePointCloudAsPcdFile(const std::string& filename) const { pointcloudProcessor_.savePointCloudAsPcdFile(filename, *workingCloud_); } diff --git a/grid_map_pcl/src/PclLoaderParameters.cpp b/grid_map_pcl/src/PclLoaderParameters.cpp index 35c0fd817..29fc80380 100644 --- a/grid_map_pcl/src/PclLoaderParameters.cpp +++ b/grid_map_pcl/src/PclLoaderParameters.cpp @@ -14,36 +14,35 @@ namespace grid_map { namespace grid_map_pcl { -void PclLoaderParameters::handleYamlNode(const YAML::Node& yamlNode) { - const std::string prefix = "pcl_grid_map_extraction"; +void PclLoaderParameters::loadParameters(const YAML::Node& yamlNode) { + parameters_.numThreads_ = yamlNode["num_processing_threads"].as(); - parameters_.numThreads_ = yamlNode[prefix]["num_processing_threads"].as(); + parameters_.cloudTransformation_.translation_.x() = yamlNode["cloud_transform"]["translation"]["x"].as(); + parameters_.cloudTransformation_.translation_.y() = yamlNode["cloud_transform"]["translation"]["y"].as(); + parameters_.cloudTransformation_.translation_.z() = yamlNode["cloud_transform"]["translation"]["z"].as(); - parameters_.cloudTransformation_.translation_.x() = yamlNode[prefix]["cloud_transform"]["translation"]["x"].as(); - parameters_.cloudTransformation_.translation_.y() = yamlNode[prefix]["cloud_transform"]["translation"]["y"].as(); - parameters_.cloudTransformation_.translation_.z() = yamlNode[prefix]["cloud_transform"]["translation"]["z"].as(); - - parameters_.cloudTransformation_.rpyIntrinsic_.x() = yamlNode[prefix]["cloud_transform"]["rotation"]["r"].as(); - parameters_.cloudTransformation_.rpyIntrinsic_.y() = yamlNode[prefix]["cloud_transform"]["rotation"]["p"].as(); - parameters_.cloudTransformation_.rpyIntrinsic_.z() = yamlNode[prefix]["cloud_transform"]["rotation"]["y"].as(); + parameters_.cloudTransformation_.rpyIntrinsic_.x() = yamlNode["cloud_transform"]["rotation"]["r"].as(); + parameters_.cloudTransformation_.rpyIntrinsic_.y() = yamlNode["cloud_transform"]["rotation"]["p"].as(); + parameters_.cloudTransformation_.rpyIntrinsic_.z() = yamlNode["cloud_transform"]["rotation"]["y"].as(); parameters_.clusterExtraction_.useMaxHeightAsCellElevation_ = - yamlNode[prefix]["cluster_extraction"]["use_max_height_as_cell_elevation"].as(); - parameters_.clusterExtraction_.clusterTolerance_ = yamlNode[prefix]["cluster_extraction"]["cluster_tolerance"].as(); - parameters_.clusterExtraction_.minNumPoints_ = yamlNode[prefix]["cluster_extraction"]["min_num_points"].as(); - parameters_.clusterExtraction_.maxNumPoints_ = yamlNode[prefix]["cluster_extraction"]["max_num_points"].as(); - - parameters_.outlierRemoval_.isRemoveOutliers_ = yamlNode[prefix]["outlier_removal"]["is_remove_outliers"].as(); - parameters_.outlierRemoval_.meanK_ = yamlNode[prefix]["outlier_removal"]["mean_K"].as(); - parameters_.outlierRemoval_.stddevThreshold_ = yamlNode[prefix]["outlier_removal"]["stddev_threshold"].as(); - - parameters_.gridMap_.resolution_ = yamlNode[prefix]["grid_map"]["resolution"].as(); - parameters_.gridMap_.minCloudPointsPerCell_ = yamlNode[prefix]["grid_map"]["min_num_points_per_cell"].as(); - - parameters_.downsampling_.isDownsampleCloud_ = yamlNode[prefix]["downsampling"]["is_downsample_cloud"].as(); - parameters_.downsampling_.voxelSize_.x() = yamlNode[prefix]["downsampling"]["voxel_size"]["x"].as(); - parameters_.downsampling_.voxelSize_.y() = yamlNode[prefix]["downsampling"]["voxel_size"]["y"].as(); - parameters_.downsampling_.voxelSize_.z() = yamlNode[prefix]["downsampling"]["voxel_size"]["z"].as(); + yamlNode["cluster_extraction"]["use_max_height_as_cell_elevation"].as(); + parameters_.clusterExtraction_.clusterTolerance_ = yamlNode["cluster_extraction"]["cluster_tolerance"].as(); + parameters_.clusterExtraction_.minNumPoints_ = yamlNode["cluster_extraction"]["min_num_points"].as(); + parameters_.clusterExtraction_.maxNumPoints_ = yamlNode["cluster_extraction"]["max_num_points"].as(); + + parameters_.outlierRemoval_.isRemoveOutliers_ = yamlNode["outlier_removal"]["is_remove_outliers"].as(); + parameters_.outlierRemoval_.meanK_ = yamlNode["outlier_removal"]["mean_K"].as(); + parameters_.outlierRemoval_.stddevThreshold_ = yamlNode["outlier_removal"]["stddev_threshold"].as(); + + parameters_.gridMap_.resolution_ = yamlNode["grid_map"]["resolution"].as(); + parameters_.gridMap_.minCloudPointsPerCell_ = yamlNode["grid_map"]["min_num_points_per_cell"].as(); + parameters_.gridMap_.maxCloudPointsPerCell_ = yamlNode["grid_map"]["max_num_points_per_cell"].as(); + + parameters_.downsampling_.isDownsampleCloud_ = yamlNode["downsampling"]["is_downsample_cloud"].as(); + parameters_.downsampling_.voxelSize_.x() = yamlNode["downsampling"]["voxel_size"]["x"].as(); + parameters_.downsampling_.voxelSize_.y() = yamlNode["downsampling"]["voxel_size"]["y"].as(); + parameters_.downsampling_.voxelSize_.z() = yamlNode["downsampling"]["voxel_size"]["z"].as(); } bool PclLoaderParameters::loadParameters(const std::string& filename) { @@ -56,7 +55,8 @@ bool PclLoaderParameters::loadParameters(const std::string& filename) { } try { - handleYamlNode(yamlNode); + const std::string prefix{"pcl_grid_map_extraction"}; + loadParameters(yamlNode[prefix]); } catch (const std::runtime_error& exception) { ROS_ERROR_STREAM("PclLoaderParameters: Loading parameters failed: " << exception.what()); return false; diff --git a/grid_map_pcl/src/grid_map_pcl_loader_node.cpp b/grid_map_pcl/src/grid_map_pcl_loader_node.cpp index 7371a6e75..16531050c 100644 --- a/grid_map_pcl/src/grid_map_pcl_loader_node.cpp +++ b/grid_map_pcl/src/grid_map_pcl_loader_node.cpp @@ -26,7 +26,7 @@ int main(int argc, char** argv) { grid_map::GridMapPclLoader gridMapPclLoader; const std::string pathToCloud = gm::getPcdFilePath(nh); - gridMapPclLoader.loadParameters(gm::getParameterPath()); + gridMapPclLoader.loadParameters(gm::getParameterPath(nh)); gridMapPclLoader.loadCloudFromPcdFile(pathToCloud); gm::processPointcloud(&gridMapPclLoader, nh); @@ -37,7 +37,6 @@ int main(int argc, char** argv) { gm::saveGridMap(gridMap, nh, gm::getMapRosbagTopic(nh)); // publish grid map - grid_map_msgs::GridMap msg; grid_map::GridMapRosConverter::toMessage(gridMap, msg); gridMapPub.publish(msg); diff --git a/grid_map_pcl/src/helpers.cpp b/grid_map_pcl/src/helpers.cpp index bdcad43c2..4c016c26f 100644 --- a/grid_map_pcl/src/helpers.cpp +++ b/grid_map_pcl/src/helpers.cpp @@ -41,25 +41,27 @@ void setVerbosityLevelToDebugIfFlagSet(const ros::NodeHandle& nh) { } } -std::string getParameterPath() { - std::string filePath = ros::package::getPath("grid_map_pcl") + "/config/parameters.yaml"; - return filePath; +std::string getParameterPath(const ros::NodeHandle& nh) { + std::string defaultPath = ros::package::getPath("grid_map_pcl") + "/config/parameters.yaml"; + std::string pathToConfig; + nh.param("config_file_path", pathToConfig, defaultPath); + return pathToConfig; } std::string getOutputBagPath(const ros::NodeHandle& nh) { - std::string outputRosbagName, folderPath; - nh.param("folder_path", folderPath, ""); - nh.param("output_grid_map", outputRosbagName, "output_grid_map.bag"); - std::string pathToOutputBag = folderPath + "/" + outputRosbagName; + std::string pathToOutputBag; + const std::string defaultPath = ros::package::getPath("grid_map_pcl") + "/data/output_grid_map.bag"; + nh.param("output_grid_map", pathToOutputBag, defaultPath); return pathToOutputBag; } std::string getPcdFilePath(const ros::NodeHandle& nh) { - std::string inputCloudName, folderPath; - nh.param("folder_path", folderPath, ""); - nh.param("pcd_filename", inputCloudName, "input_cloud"); - std::string pathToCloud = folderPath + "/" + inputCloudName; - return pathToCloud; + std::string pathToCloud, folderPath; + const std::string defaultPathToCloud = "/data/input_cloud.pcd"; + const std::string defaultFolderPath = ros::package::getPath("grid_map_pcl"); + nh.param("pcd_filename", pathToCloud, defaultPathToCloud); + nh.param("folder_path", folderPath, defaultFolderPath); + return folderPath + "/" + pathToCloud; } std::string getMapFrame(const ros::NodeHandle& nh) { diff --git a/grid_map_pcl/src/pointcloud_publisher_node.cpp b/grid_map_pcl/src/pointcloud_publisher_node.cpp new file mode 100644 index 000000000..82e2b8e0f --- /dev/null +++ b/grid_map_pcl/src/pointcloud_publisher_node.cpp @@ -0,0 +1,42 @@ +/* + * pointcloud_publisher_node.cpp + * + * Created on: Aug 19, 2021 + * Author: Edo Jelavic + * Institute: ETH Zurich, Robotic Systems Lab + */ + +#include +#include +#include +#include "grid_map_pcl/helpers.hpp" + +namespace gm = ::grid_map::grid_map_pcl; +using Point = ::pcl::PointXYZ; +using PointCloud = ::pcl::PointCloud; + +void publishCloud(const std::string& filename, const ros::Publisher& pub, const std::string& frame) { + PointCloud::Ptr cloud(new pcl::PointCloud); + cloud = gm::loadPointcloudFromPcd(filename); + cloud->header.frame_id = frame; + sensor_msgs::PointCloud2 msg; + pcl::toROSMsg(*cloud, msg); + ROS_INFO_STREAM("Publishing loaded cloud, number of points: " << cloud->points.size()); + msg.header.stamp = ros::Time::now(); + pub.publish(msg); +} + +int main(int argc, char** argv) { + ros::init(argc, argv, "point_cloud_pub_node"); + ros::NodeHandle nh("~"); + + const std::string pathToCloud = gm::getPcdFilePath(nh); + const std::string cloudFrame = nh.param("cloud_frame", ""); + // publish cloud + ros::Publisher cloudPub = nh.advertise("raw_pointcloud", 1, true); + publishCloud(pathToCloud, cloudPub, cloudFrame); + + // run + ros::spin(); + return EXIT_SUCCESS; +} diff --git a/grid_map_pcl/test/GridMapPclLoaderTest.cpp b/grid_map_pcl/test/GridMapPclLoaderTest.cpp index b9e52e009..5b0f10913 100644 --- a/grid_map_pcl/test/GridMapPclLoaderTest.cpp +++ b/grid_map_pcl/test/GridMapPclLoaderTest.cpp @@ -37,7 +37,7 @@ TEST(GridMapPclLoaderTest, FlatGroundRealDataset) // NOLINT // allow for some difference (2cm) since the input cloud is noisy (real dataset) double referenceElevation = elevationValues.front(); for (const auto& elevation : elevationValues) { - EXPECT_NEAR(elevation, referenceElevation, 2e-2); + EXPECT_NEAR(elevation, referenceElevation, 3e-2); } } diff --git a/grid_map_pcl/test/test_data/parameters.yaml b/grid_map_pcl/test/test_data/parameters.yaml index ea4854ab9..772a8dadd 100644 --- a/grid_map_pcl/test/test_data/parameters.yaml +++ b/grid_map_pcl/test/test_data/parameters.yaml @@ -26,6 +26,7 @@ pcl_grid_map_extraction: z: 0.02 grid_map: min_num_points_per_cell: 4 + max_num_points_per_cell: 100000 resolution: 0.1 diff --git a/grid_map_pcl/test/test_helpers.cpp b/grid_map_pcl/test/test_helpers.cpp index 037fce436..44926b205 100644 --- a/grid_map_pcl/test/test_helpers.cpp +++ b/grid_map_pcl/test/test_helpers.cpp @@ -115,13 +115,9 @@ Pointcloud::Ptr concatenate(Pointcloud::Ptr cloud1, Pointcloud::Ptr cloud2) { Pointcloud::Ptr concatenatedCloud(new grid_map_pcl_test::Pointcloud()); concatenatedCloud->points.reserve(cloud1->points.size() + cloud2->points.size()); - for (const auto& point : cloud2->points) { - concatenatedCloud->push_back(point); - } + std::copy(cloud2->points.begin(), cloud2->points.end(), std::back_inserter(concatenatedCloud->points)); - for (const auto& point : cloud1->points) { - concatenatedCloud->push_back(point); - } + std::copy(cloud1->points.begin(), cloud1->points.end(), std::back_inserter(concatenatedCloud->points)); return concatenatedCloud; } diff --git a/grid_map_sdf/CHANGELOG.rst b/grid_map_sdf/CHANGELOG.rst index a685bd101..bd9e5fde1 100644 --- a/grid_map_sdf/CHANGELOG.rst +++ b/grid_map_sdf/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package grid_map_sdf ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.7.0 (2022-03-17) +------------------ +* Release of new implementation to convert grid maps to signed distance fields. +* Contributors: Ruben Grandia + 1.6.4 (2020-12-04) ------------------ diff --git a/grid_map_sdf/CMakeLists.txt b/grid_map_sdf/CMakeLists.txt index 34b8369f7..88eda8145 100644 --- a/grid_map_sdf/CMakeLists.txt +++ b/grid_map_sdf/CMakeLists.txt @@ -8,7 +8,6 @@ set(CMAKE_EXPORT_COMPILE_COMMANDS ON) ## Find catkin macros and libraries find_package(catkin REQUIRED COMPONENTS grid_map_core - pcl_ros ) ################################### @@ -27,7 +26,6 @@ catkin_package( ${PROJECT_NAME} CATKIN_DEPENDS grid_map_core - pcl_ros DEPENDS ) @@ -45,6 +43,7 @@ include_directories( ## Declare a cpp library add_library(${PROJECT_NAME} + src/SignedDistance2d.cpp src/SignedDistanceField.cpp ) @@ -82,11 +81,16 @@ if(CATKIN_ENABLE_TESTING) ## Add gtest based cpp test target and link libraries catkin_add_gtest(${PROJECT_NAME}-test - test/SignedDistanceFieldTest.cpp + test/test3dLookup.cpp test/test_grid_map_sdf.cpp + test/testDerivatives.cpp + test/testPixelBorderDistance.cpp + test/testSignedDistance2d.cpp + test/testSignedDistance3d.cpp ) target_include_directories(${PROJECT_NAME}-test PRIVATE include + test/include ) target_include_directories(${PROJECT_NAME}-test SYSTEM PUBLIC ${catkin_INCLUDE_DIRS} diff --git a/grid_map_sdf/doc/anymal_sdf_demo.gif b/grid_map_sdf/doc/anymal_sdf_demo.gif new file mode 100644 index 000000000..1e3fe5d84 Binary files /dev/null and b/grid_map_sdf/doc/anymal_sdf_demo.gif differ diff --git a/grid_map_sdf/include/grid_map_sdf/DistanceDerivatives.hpp b/grid_map_sdf/include/grid_map_sdf/DistanceDerivatives.hpp new file mode 100644 index 000000000..20fe2d166 --- /dev/null +++ b/grid_map_sdf/include/grid_map_sdf/DistanceDerivatives.hpp @@ -0,0 +1,62 @@ +/* + * DistanceDerivatives.h + * + * Created on: Aug 10, 2020 + * Author: Ruben Grandia + * Institute: ETH Zurich + */ + +#pragma once + +#include + +namespace grid_map { +namespace signed_distance_field { + +/** + * Takes the columnwise central difference of a matrix. Uses single sided difference at the boundaries. + * diff = (col(i+1) - col(i-1)) / (2 * resolution) + * + * @param data [in] : data to take the difference of. + * @param centralDifference [out] : matrix to store the result in. + * @param resolution [in] : scaling of the distance between two columns. + */ +inline void columnwiseCentralDifference(const Matrix& data, Matrix& centralDifference, float resolution) { + assert(data.cols() >= 2); // Minimum size to take finite differences. + + const Eigen::Index m{data.cols()}; + const float resInv{1.0F / resolution}; + const float doubleResInv{1.0F / (2.0F * resolution)}; + + // First column + centralDifference.col(0) = resInv * (data.col(1) - data.col(0)); + + // All the middle columns + for (Eigen::Index i = 1; i + 1 < m; ++i) { + centralDifference.col(i) = doubleResInv * (data.col(i + 1) - data.col(i - 1)); + } + + // Last column + centralDifference.col(m - 1) = resInv * (data.col(m - 1) - data.col(m - 2)); +} + +/** + * Takes the finite difference between layers + * result = (data_{k+1} - data{k}) / resolution + */ +inline void layerFiniteDifference(const Matrix& data_k, const Matrix& data_kp1, Matrix& result, float resolution) { + const float resInv{1.0F / resolution}; + result = resInv * (data_kp1 - data_k); +} + +/** + * Takes the central difference between layers + * result = (data_{k+1} - data{k-1}) / (2.0 * resolution) + */ +inline void layerCentralDifference(const Matrix& data_km1, const Matrix& data_kp1, Matrix& result, float resolution) { + const float doubleResInv{1.0F / (2.0F * resolution)}; + result = doubleResInv * (data_kp1 - data_km1); +} + +} // namespace signed_distance_field +} // namespace grid_map \ No newline at end of file diff --git a/grid_map_sdf/include/grid_map_sdf/Gridmap3dLookup.hpp b/grid_map_sdf/include/grid_map_sdf/Gridmap3dLookup.hpp new file mode 100644 index 000000000..1ef1f5427 --- /dev/null +++ b/grid_map_sdf/include/grid_map_sdf/Gridmap3dLookup.hpp @@ -0,0 +1,105 @@ +/* + * Gridmap3dLookup.h + * + * Created on: Aug 13, 2020 + * Author: Ruben Grandia + * Institute: ETH Zurich + */ + +#pragma once + +#include + +namespace grid_map { +namespace signed_distance_field { + +/** + * Stores 3 dimensional grid information and provides methods to convert between position - 3d Index - linear index. + * + * As with the 2D GridMap, the X-Y position is opposite to the row-col-index: (X,Y) is highest at (0,0) and lowest at (n, m). + * The z-position is increasing with the layer-index. + */ +struct Gridmap3dLookup { + public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + /// size_t in 3 dimensions + struct size_t_3d { + size_t x{0}; + size_t y{0}; + size_t z{0}; + + size_t_3d() = default; + size_t_3d(size_t x, size_t y, size_t z) : x(x), y(y), z(z) {} + }; + + //! 3D size of the grid + size_t_3d gridsize_{0, 0, 0}; + + //! Origin position of the grid + Position3 gridOrigin_{0.0, 0.0, 0.0}; + + //! Maximum index per dimension stored as double + Position3 gridMaxIndexAsDouble_{0.0, 0.0, 0.0}; + + //! Grid resolution + double resolution_{1.0}; + + /** Default constructor: creates an empty grid */ + Gridmap3dLookup() = default; + + /** + * Constructor + * @param gridsize : x, y, z size of the grid + * @param gridOrigin : position at x=y=z=0 + * @param resolution : (>0.0) size of 1 voxel + */ + Gridmap3dLookup(const size_t_3d& gridsize, const Position3& gridOrigin, double resolution) + : gridsize_(gridsize), + gridOrigin_(gridOrigin), + gridMaxIndexAsDouble_(static_cast(gridsize_.x - 1), static_cast(gridsize_.y - 1), + static_cast(gridsize_.z - 1)), + resolution_(resolution) { + assert(resolution_ > 0.0); + assert(gridsize_.x > 0); + assert(gridsize_.y > 0); + assert(gridsize_.z > 0); + }; + + /** Returns the 3d index of the grid node closest to the query position */ + size_t_3d nearestNode(const Position3& position) const noexcept { + const double resInv{1.0 / resolution_}; + Position3 subpixelVector{(gridOrigin_.x() - position.x()) * resInv, (gridOrigin_.y() - position.y()) * resInv, + (position.z() - gridOrigin_.z()) * resInv}; + return {getNearestPositiveInteger(subpixelVector.x(), gridMaxIndexAsDouble_.x()), + getNearestPositiveInteger(subpixelVector.y(), gridMaxIndexAsDouble_.y()), + getNearestPositiveInteger(subpixelVector.z(), gridMaxIndexAsDouble_.z())}; + } + + /** Returns the 3d node position from a 3d index */ + Position3 nodePosition(const size_t_3d& index) const noexcept { + assert(index.x < gridsize_.x); + assert(index.y < gridsize_.y); + assert(index.z < gridsize_.z); + return {gridOrigin_.x() - index.x * resolution_, gridOrigin_.y() - index.y * resolution_, gridOrigin_.z() + index.z * resolution_}; + } + + /** Returns the linear node index from a 3d node index */ + size_t linearIndex(const size_t_3d& index) const noexcept { + assert(index.x < gridsize_.x); + assert(index.y < gridsize_.y); + assert(index.z < gridsize_.z); + return (index.z * gridsize_.y + index.y) * gridsize_.x + index.x; + } + + /** Linear size */ + size_t linearSize() const noexcept { return gridsize_.x * gridsize_.y * gridsize_.z; } + + /** rounds subindex value and clamps it to [0, max] */ + static size_t getNearestPositiveInteger(double val, double max) noexcept { + // Comparing bounds as double prevents underflow/overflow of size_t + return static_cast(std::max(0.0, std::min(std::round(val), max))); + } +}; + +} // namespace signed_distance_field +} // namespace grid_map \ No newline at end of file diff --git a/grid_map_sdf/include/grid_map_sdf/PixelBorderDistance.hpp b/grid_map_sdf/include/grid_map_sdf/PixelBorderDistance.hpp new file mode 100644 index 000000000..5cd2baebf --- /dev/null +++ b/grid_map_sdf/include/grid_map_sdf/PixelBorderDistance.hpp @@ -0,0 +1,104 @@ +/* + * PixelBorderDistance.h + * + * Created on: Aug 7, 2020 + * Author: Ruben Grandia + * Institute: ETH Zurich + */ + +#pragma once + +#include +#include +#include + +namespace grid_map { +namespace signed_distance_field { + +/** + * Returns distance between the center of a pixel and the border of an other pixel. + * Returns zero if the center is inside the other pixel. + * Pixels are assumed to have size 1.0F + * @param i : location of pixel 1 + * @param j : location of pixel 2 + * @return : absolute distance between center of pixel 1 and the border of pixel 2 + */ +inline float pixelBorderDistance(float i, float j) { + return std::max(std::abs(i - j) - 0.5F, 0.0F); +} + +/** + * Returns square pixelBorderDistance, adding offset f. + */ +inline float squarePixelBorderDistance(float i, float j, float f) { + const float d{pixelBorderDistance(i, j)}; + return d * d + f; +} + +namespace internal { + +/** + * Return equidistancepoint between origin and pixel p (with p > 0) with offset fp + */ +inline float intersectionPointRightSideOfOrigin(float p, float fp) { + /* + * There are 5 different solutions + * In decreasing order: + * sol 1 in [p^2, inf] + * sol 2 in [bound, p^2] + * sol 3 in [-bound, bound] + * sol 4 in [-p^2, -bound] + * sol 5 in [-inf, -p^2] + */ + const float pSquared{p * p}; + if (fp > pSquared) { + return (pSquared + p + fp) / (2.0F * p); // sol 1 + } else if (fp < -pSquared) { + return (pSquared - p + fp) / (2.0F * p); // sol 5 + } else { + const float bound{pSquared - 2.0F * p + 1.0F}; // Always positive because (p > 0) + if (fp > bound) { + return 0.5F + std::sqrt(fp); // sol 2 + } else if (fp < -bound) { + return p - 0.5F - std::sqrt(-fp); // sol 4 + } else { + return (pSquared - p + fp) / (2.0F * p - 2.0F); // sol 3 + } + } +} + +/** + * Return equidistancepoint between origin and pixel p with offset fp + */ +inline float intersectionOffsetFromOrigin(float p, float fp) { + if (p > 0.0F) { + return intersectionPointRightSideOfOrigin(p, fp); + } else { + // call with negative p and flip the result + return -intersectionPointRightSideOfOrigin(-p, fp); + } +} + +} // namespace internal + +/** + * Return the point s in pixel space that is equally far from p and q (taking into account offsets fp, and fq) + * It is the solution to the following equation: + * squarePixelBorderDistance(s, q, fq) == squarePixelBorderDistance(s, p, fp) + */ +inline float equidistancePoint(float q, float fq, float p, float fp) { + assert(q == p || + std::abs(q - p) >= 1.0F); // Check that q and p are proper pixel locations: either the same pixel or non-overlapping pixels + assert((q == p) ? fp == fq : true); // Check when q and p are equal, the offsets are also equal + + if (fp == fq) { // quite common case when both pixels are of the same class (occupied / free) + return 0.5F * (p + q); + } else { + float df{fp - fq}; + float dr{p - q}; + return internal::intersectionOffsetFromOrigin(dr, df) + q; + } +} + +} // namespace signed_distance_field +} // namespace grid_map \ No newline at end of file diff --git a/grid_map_sdf/include/grid_map_sdf/SignedDistance2d.hpp b/grid_map_sdf/include/grid_map_sdf/SignedDistance2d.hpp new file mode 100644 index 000000000..1646d0bbb --- /dev/null +++ b/grid_map_sdf/include/grid_map_sdf/SignedDistance2d.hpp @@ -0,0 +1,59 @@ +/* + * SignedDistance2d.h + * + * Created on: Jul 10, 2020 + * Author: Ruben Grandia + * Institute: ETH Zurich + */ + +#pragma once + +#include + +#include + +#include "Utils.hpp" + +namespace grid_map { +namespace signed_distance_field { + +/** + * Computes the signed distance field at a specified height for a given elevation map. + * + * @param elevationMap : elevation data. + * @param height : height to generate the signed distance at. + * @param resolution : resolution of the elevation map. (The true distance [m] between cells in world frame) + * @param minHeight : the lowest height contained in elevationMap + * @param maxHeight : the maximum height contained in elevationMap + * @return The signed distance field at the query height. + */ +Matrix signedDistanceAtHeight(const Matrix& elevationMap, float height, float resolution, float minHeight, float maxHeight); + +/** + * Same as above, but returns the sdf in transposed form. + * Also takes temporary variables from outside to prevent memory allocation. + * + * @param elevationMap : elevation data. + * @param sdfTranspose : [output] resulting sdf in transposed form (automatically allocated if of wrong size) + * @param tmp : temporary of size elevationMap (automatically allocated if of wrong size) + * @param tmpTranspose : temporary of size elevationMap transpose (automatically allocated if of wrong size) + * @param height : height to generate the signed distance at. + * @param resolution : resolution of the elevation map. (The true distance [m] between cells in world frame) + * @param minHeight : the lowest height contained in elevationMap + * @param maxHeight : the maximum height contained in elevationMap + */ +void signedDistanceAtHeightTranspose(const Matrix& elevationMap, Matrix& sdfTranspose, Matrix& tmp, Matrix& tmpTranspose, float height, + float resolution, float minHeight, float maxHeight); + +/** + * Gets the 2D signed distance from an occupancy grid. + * Returns +INF if there are no obstacles, and -INF if there are only obstacles + * + * @param occupancyGrid : occupancy grid with true = obstacle, false = free space + * @param resolution : resolution of the grid. + * @return signed distance for each point in the grid to the occupancy border. + */ +Matrix signedDistanceFromOccupancy(const Eigen::Matrix& occupancyGrid, float resolution); + +} // namespace signed_distance_field +} // namespace grid_map \ No newline at end of file diff --git a/grid_map_sdf/include/grid_map_sdf/SignedDistanceField.hpp b/grid_map_sdf/include/grid_map_sdf/SignedDistanceField.hpp index f1105da55..cfcd8ef81 100644 --- a/grid_map_sdf/include/grid_map_sdf/SignedDistanceField.hpp +++ b/grid_map_sdf/include/grid_map_sdf/SignedDistanceField.hpp @@ -1,45 +1,141 @@ /* - * SignedDistanceField.hpp + * SignedDistanceField.h * - * Created on: Aug 16, 2017 - * Authors: Takahiro Miki, Peter Fankhauser - * Institute: ETH Zurich, ANYbotics + * Created on: Jul 10, 2020 + * Author: Ruben Grandia + * Institute: ETH Zurich */ #pragma once -#include +#include -#include -#include +#include -#include -#include +#include +#include + +#include "Gridmap3dLookup.hpp" namespace grid_map { -class SignedDistanceField -{ +/** + * This class creates a dense 3D signed distance field grid for a given elevation map. + * The 3D grid uses the same resolution as the 2D map. i.e. all voxels are of size (resolution x resolution x resolution). + * The size of the 3D grid is the same as the map in XY direction. The size in Z direction is specified in the constructor. + * + * During the creation of the class all values and derivatives are pre-computed in one go. This makes repeated lookups very cheap. + * Querying a point outside of the constructed grid will result in linear extrapolation. + * + * The distance value and derivatives (dx,dy,dz) per voxel are stored next to each other in memory to support fast lookup during + * interpolation, where we need all 4 values simultaneously. The entire dense grid is stored as a flat vector, with the indexing outsourced + * to the Gridmap3dLookup class. + */ +class SignedDistanceField { public: - SignedDistanceField(); - virtual ~SignedDistanceField(); + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + using Derivative3 = Eigen::Vector3d; + + /** + * Create a signed distance field and its derivative for an elevation layer in the grid map. + * + * @param gridMap : Input map to create the SDF for. + * @param elevationLayer : Name of the elevation layer. + * @param minHeight : Desired starting height of the 3D SDF grid. + * @param maxHeight : Desired ending height of the 3D SDF grid. (Will be rounded up to match the resolution) + */ + SignedDistanceField(const GridMap& gridMap, const std::string& elevationLayer, double minHeight, double maxHeight); + + /** + * Get the signed distance value at a 3D position. + * @param position : 3D position in the frame of the gridmap. + * @return signed distance to the elevation surface. + */ + double value(const Position3& position) const noexcept; + + /** + * Get the signed distance derivative at a 3D position. + * @param position : 3D position in the frame of the gridmap. + * @return derivative of the signed distance field. + */ + Derivative3 derivative(const Position3& position) const noexcept; + + /** + * Get both the signed distance value and derivative at a 3D position. + * @param position : 3D position in the frame of the gridmap. + * @return {value, derivative} of the signed distance field. + */ + std::pair valueAndDerivative(const Position3& position) const noexcept; + + size_t size() const noexcept; - void calculateSignedDistanceField(const GridMap& gridMap, const std::string& layer, const double heightClearance); - double getDistanceAt(const Position3& position) const; - Vector3 getDistanceGradientAt(const Position3& position) const; - double getInterpolatedDistanceAt(const Position3& position) const; - void convertToPointCloud(pcl::PointCloud& points) const; + const std::string& getFrameId() const noexcept; + + Time getTime() const noexcept; + + /** + * Calls a function on each point in the signed distance field. The points are processed in the order they are stored in memory. + * @param func : function taking the node position, signed distance value, and signed distance derivative. + * @param decimation : specifies how many points are returned. 1: all points, 2: every second point, etc. + */ + void filterPoints(std::function func, size_t decimation = 1) const; private: - Matrix getPlanarSignedDistanceField(Eigen::Matrix& data) const; - - double resolution_; - Size size_; - Position position_; - std::vector data_; - float zIndexStartHeight_; - float maxDistance_; - const float lowestHeight_; + /** + * Implementation of the signed distance field computation in this class. + * @param elevation + */ + void computeSignedDistance(const Matrix& elevation); + + /** + * Simultaneously compute the signed distance and derivative in x direction at a given height + * @param elevation [in] : elevation data + * @param currentLayer [out] : signed distance values + * @param dxTranspose [out] : derivative in x direction (the matrix is transposed) + * @param sdfTranspose [tmp] : temporary variable to reuse between calls (allocated on first use) + * @param tmp [tmp] : temporary variable (allocated on first use) + * @param tmpTranspose [tmp] : temporary variable (allocated on first use) + * @param height [in] : height the signed distance is created for. + * @param resolution [in] : resolution of the map. + * @param minHeight [in] : smallest height value in the elevation data. + * @param maxHeight [in] : largest height value in the elevation data. + */ + void computeLayerSdfandDeltaX(const Matrix& elevation, Matrix& currentLayer, Matrix& dxTranspose, Matrix& sdfTranspose, Matrix& tmp, + Matrix& tmpTranspose, float height, float resolution, float minHeight, float maxHeight) const; + + /** + * Add the computed signed distance values and derivatives at a particular height to the grid. + * Adds the layer to the back of data_ + * @param signedDistance : signed distance values. + * @param dxTranspose : x components of the derivative (matrix is transposed). + * @param dy : y components of the derivative. + * @param dz : z components of the derivative. + */ + void emplacebackLayerData(const Matrix& signedDistance, const Matrix& dxTranspose, const Matrix& dy, const Matrix& dz); + + //! Data structure to store together {signed distance value, derivative}. + using node_data_t = std::array; + + /** Helper function to extract the sdf value */ + static double distance(const node_data_t& nodeData) noexcept { return nodeData[0]; } + + /** Helper function to extract the sdf value as float */ + static float distanceFloat(const node_data_t& nodeData) noexcept { return nodeData[0]; } + + /** Helper function to extract the sdf derivative */ + static Derivative3 derivative(const node_data_t& nodeData) noexcept { return {nodeData[1], nodeData[2], nodeData[3]}; } + + //! Object encoding the 3D grid. + signed_distance_field::Gridmap3dLookup gridmap3DLookup_; + + //! Object encoding the signed distance value and derivative in the grid. + std::vector data_; + + //! Frame id of the grid map. + std::string frameId_; + + //! Timestamp of the grid map (nanoseconds). + Time timestamp_; }; -} /* namespace */ +} // namespace grid_map \ No newline at end of file diff --git a/grid_map_sdf/include/grid_map_sdf/Utils.hpp b/grid_map_sdf/include/grid_map_sdf/Utils.hpp new file mode 100644 index 000000000..32d03c452 --- /dev/null +++ b/grid_map_sdf/include/grid_map_sdf/Utils.hpp @@ -0,0 +1,21 @@ +/* + * Utils.h + * + * Created on: Jul 10, 2020 + * Author: Ruben Grandia + * Institute: ETH Zurich + */ + +#pragma once + +namespace grid_map { +namespace signed_distance_field { + +// Check existence of inf +static_assert(std::numeric_limits::has_infinity, "float does not support infinity"); + +//! Distance value that is considered infinite. +constexpr float INF = std::numeric_limits::infinity(); + +} // namespace signed_distance_field +} // namespace grid_map \ No newline at end of file diff --git a/grid_map_sdf/include/grid_map_sdf/distance_transform/COPYING b/grid_map_sdf/include/grid_map_sdf/distance_transform/COPYING deleted file mode 100755 index d511905c1..000000000 --- a/grid_map_sdf/include/grid_map_sdf/distance_transform/COPYING +++ /dev/null @@ -1,339 +0,0 @@ - GNU GENERAL PUBLIC LICENSE - Version 2, June 1991 - - Copyright (C) 1989, 1991 Free Software Foundation, Inc., - 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA - Everyone is permitted to copy and distribute verbatim copies - of this license document, but changing it is not allowed. - - Preamble - - The licenses for most software are designed to take away your -freedom to share and change it. By contrast, the GNU General Public -License is intended to guarantee your freedom to share and change free -software--to make sure the software is free for all its users. This -General Public License applies to most of the Free Software -Foundation's software and to any other program whose authors commit to -using it. (Some other Free Software Foundation software is covered by -the GNU Lesser General Public License instead.) You can apply it to -your programs, too. - - When we speak of free software, we are referring to freedom, not -price. Our General Public Licenses are designed to make sure that you -have the freedom to distribute copies of free software (and charge for -this service if you wish), that you receive source code or can get it -if you want it, that you can change the software or use pieces of it -in new free programs; and that you know you can do these things. - - To protect your rights, we need to make restrictions that forbid -anyone to deny you these rights or to ask you to surrender the rights. -These restrictions translate to certain responsibilities for you if you -distribute copies of the software, or if you modify it. - - For example, if you distribute copies of such a program, whether -gratis or for a fee, you must give the recipients all the rights that -you have. You must make sure that they, too, receive or can get the -source code. And you must show them these terms so they know their -rights. - - We protect your rights with two steps: (1) copyright the software, and -(2) offer you this license which gives you legal permission to copy, -distribute and/or modify the software. - - Also, for each author's protection and ours, we want to make certain -that everyone understands that there is no warranty for this free -software. If the software is modified by someone else and passed on, we -want its recipients to know that what they have is not the original, so -that any problems introduced by others will not reflect on the original -authors' reputations. - - Finally, any free program is threatened constantly by software -patents. We wish to avoid the danger that redistributors of a free -program will individually obtain patent licenses, in effect making the -program proprietary. To prevent this, we have made it clear that any -patent must be licensed for everyone's free use or not licensed at all. - - The precise terms and conditions for copying, distribution and -modification follow. - - GNU GENERAL PUBLIC LICENSE - TERMS AND CONDITIONS FOR COPYING, DISTRIBUTION AND MODIFICATION - - 0. This License applies to any program or other work which contains -a notice placed by the copyright holder saying it may be distributed -under the terms of this General Public License. The "Program", below, -refers to any such program or work, and a "work based on the Program" -means either the Program or any derivative work under copyright law: -that is to say, a work containing the Program or a portion of it, -either verbatim or with modifications and/or translated into another -language. (Hereinafter, translation is included without limitation in -the term "modification".) Each licensee is addressed as "you". - -Activities other than copying, distribution and modification are not -covered by this License; they are outside its scope. The act of -running the Program is not restricted, and the output from the Program -is covered only if its contents constitute a work based on the -Program (independent of having been made by running the Program). -Whether that is true depends on what the Program does. - - 1. You may copy and distribute verbatim copies of the Program's -source code as you receive it, in any medium, provided that you -conspicuously and appropriately publish on each copy an appropriate -copyright notice and disclaimer of warranty; keep intact all the -notices that refer to this License and to the absence of any warranty; -and give any other recipients of the Program a copy of this License -along with the Program. - -You may charge a fee for the physical act of transferring a copy, and -you may at your option offer warranty protection in exchange for a fee. - - 2. You may modify your copy or copies of the Program or any portion -of it, thus forming a work based on the Program, and copy and -distribute such modifications or work under the terms of Section 1 -above, provided that you also meet all of these conditions: - - a) You must cause the modified files to carry prominent notices - stating that you changed the files and the date of any change. - - b) You must cause any work that you distribute or publish, that in - whole or in part contains or is derived from the Program or any - part thereof, to be licensed as a whole at no charge to all third - parties under the terms of this License. - - c) If the modified program normally reads commands interactively - when run, you must cause it, when started running for such - interactive use in the most ordinary way, to print or display an - announcement including an appropriate copyright notice and a - notice that there is no warranty (or else, saying that you provide - a warranty) and that users may redistribute the program under - these conditions, and telling the user how to view a copy of this - License. (Exception: if the Program itself is interactive but - does not normally print such an announcement, your work based on - the Program is not required to print an announcement.) - -These requirements apply to the modified work as a whole. If -identifiable sections of that work are not derived from the Program, -and can be reasonably considered independent and separate works in -themselves, then this License, and its terms, do not apply to those -sections when you distribute them as separate works. But when you -distribute the same sections as part of a whole which is a work based -on the Program, the distribution of the whole must be on the terms of -this License, whose permissions for other licensees extend to the -entire whole, and thus to each and every part regardless of who wrote it. - -Thus, it is not the intent of this section to claim rights or contest -your rights to work written entirely by you; rather, the intent is to -exercise the right to control the distribution of derivative or -collective works based on the Program. - -In addition, mere aggregation of another work not based on the Program -with the Program (or with a work based on the Program) on a volume of -a storage or distribution medium does not bring the other work under -the scope of this License. - - 3. You may copy and distribute the Program (or a work based on it, -under Section 2) in object code or executable form under the terms of -Sections 1 and 2 above provided that you also do one of the following: - - a) Accompany it with the complete corresponding machine-readable - source code, which must be distributed under the terms of Sections - 1 and 2 above on a medium customarily used for software interchange; or, - - b) Accompany it with a written offer, valid for at least three - years, to give any third party, for a charge no more than your - cost of physically performing source distribution, a complete - machine-readable copy of the corresponding source code, to be - distributed under the terms of Sections 1 and 2 above on a medium - customarily used for software interchange; or, - - c) Accompany it with the information you received as to the offer - to distribute corresponding source code. (This alternative is - allowed only for noncommercial distribution and only if you - received the program in object code or executable form with such - an offer, in accord with Subsection b above.) - -The source code for a work means the preferred form of the work for -making modifications to it. For an executable work, complete source -code means all the source code for all modules it contains, plus any -associated interface definition files, plus the scripts used to -control compilation and installation of the executable. However, as a -special exception, the source code distributed need not include -anything that is normally distributed (in either source or binary -form) with the major components (compiler, kernel, and so on) of the -operating system on which the executable runs, unless that component -itself accompanies the executable. - -If distribution of executable or object code is made by offering -access to copy from a designated place, then offering equivalent -access to copy the source code from the same place counts as -distribution of the source code, even though third parties are not -compelled to copy the source along with the object code. - - 4. You may not copy, modify, sublicense, or distribute the Program -except as expressly provided under this License. Any attempt -otherwise to copy, modify, sublicense or distribute the Program is -void, and will automatically terminate your rights under this License. -However, parties who have received copies, or rights, from you under -this License will not have their licenses terminated so long as such -parties remain in full compliance. - - 5. You are not required to accept this License, since you have not -signed it. However, nothing else grants you permission to modify or -distribute the Program or its derivative works. These actions are -prohibited by law if you do not accept this License. Therefore, by -modifying or distributing the Program (or any work based on the -Program), you indicate your acceptance of this License to do so, and -all its terms and conditions for copying, distributing or modifying -the Program or works based on it. - - 6. Each time you redistribute the Program (or any work based on the -Program), the recipient automatically receives a license from the -original licensor to copy, distribute or modify the Program subject to -these terms and conditions. You may not impose any further -restrictions on the recipients' exercise of the rights granted herein. -You are not responsible for enforcing compliance by third parties to -this License. - - 7. If, as a consequence of a court judgment or allegation of patent -infringement or for any other reason (not limited to patent issues), -conditions are imposed on you (whether by court order, agreement or -otherwise) that contradict the conditions of this License, they do not -excuse you from the conditions of this License. If you cannot -distribute so as to satisfy simultaneously your obligations under this -License and any other pertinent obligations, then as a consequence you -may not distribute the Program at all. For example, if a patent -license would not permit royalty-free redistribution of the Program by -all those who receive copies directly or indirectly through you, then -the only way you could satisfy both it and this License would be to -refrain entirely from distribution of the Program. - -If any portion of this section is held invalid or unenforceable under -any particular circumstance, the balance of the section is intended to -apply and the section as a whole is intended to apply in other -circumstances. - -It is not the purpose of this section to induce you to infringe any -patents or other property right claims or to contest validity of any -such claims; this section has the sole purpose of protecting the -integrity of the free software distribution system, which is -implemented by public license practices. Many people have made -generous contributions to the wide range of software distributed -through that system in reliance on consistent application of that -system; it is up to the author/donor to decide if he or she is willing -to distribute software through any other system and a licensee cannot -impose that choice. - -This section is intended to make thoroughly clear what is believed to -be a consequence of the rest of this License. - - 8. If the distribution and/or use of the Program is restricted in -certain countries either by patents or by copyrighted interfaces, the -original copyright holder who places the Program under this License -may add an explicit geographical distribution limitation excluding -those countries, so that distribution is permitted only in or among -countries not thus excluded. In such case, this License incorporates -the limitation as if written in the body of this License. - - 9. The Free Software Foundation may publish revised and/or new versions -of the General Public License from time to time. Such new versions will -be similar in spirit to the present version, but may differ in detail to -address new problems or concerns. - -Each version is given a distinguishing version number. If the Program -specifies a version number of this License which applies to it and "any -later version", you have the option of following the terms and conditions -either of that version or of any later version published by the Free -Software Foundation. If the Program does not specify a version number of -this License, you may choose any version ever published by the Free Software -Foundation. - - 10. If you wish to incorporate parts of the Program into other free -programs whose distribution conditions are different, write to the author -to ask for permission. For software which is copyrighted by the Free -Software Foundation, write to the Free Software Foundation; we sometimes -make exceptions for this. Our decision will be guided by the two goals -of preserving the free status of all derivatives of our free software and -of promoting the sharing and reuse of software generally. - - NO WARRANTY - - 11. BECAUSE THE PROGRAM IS LICENSED FREE OF CHARGE, THERE IS NO WARRANTY -FOR THE PROGRAM, TO THE EXTENT PERMITTED BY APPLICABLE LAW. EXCEPT WHEN -OTHERWISE STATED IN WRITING THE COPYRIGHT HOLDERS AND/OR OTHER PARTIES -PROVIDE THE PROGRAM "AS IS" WITHOUT WARRANTY OF ANY KIND, EITHER EXPRESSED -OR IMPLIED, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF -MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE. THE ENTIRE RISK AS -TO THE QUALITY AND PERFORMANCE OF THE PROGRAM IS WITH YOU. SHOULD THE -PROGRAM PROVE DEFECTIVE, YOU ASSUME THE COST OF ALL NECESSARY SERVICING, -REPAIR OR CORRECTION. - - 12. IN NO EVENT UNLESS REQUIRED BY APPLICABLE LAW OR AGREED TO IN WRITING -WILL ANY COPYRIGHT HOLDER, OR ANY OTHER PARTY WHO MAY MODIFY AND/OR -REDISTRIBUTE THE PROGRAM AS PERMITTED ABOVE, BE LIABLE TO YOU FOR DAMAGES, -INCLUDING ANY GENERAL, SPECIAL, INCIDENTAL OR CONSEQUENTIAL DAMAGES ARISING -OUT OF THE USE OR INABILITY TO USE THE PROGRAM (INCLUDING BUT NOT LIMITED -TO LOSS OF DATA OR DATA BEING RENDERED INACCURATE OR LOSSES SUSTAINED BY -YOU OR THIRD PARTIES OR A FAILURE OF THE PROGRAM TO OPERATE WITH ANY OTHER -PROGRAMS), EVEN IF SUCH HOLDER OR OTHER PARTY HAS BEEN ADVISED OF THE -POSSIBILITY OF SUCH DAMAGES. - - END OF TERMS AND CONDITIONS - - How to Apply These Terms to Your New Programs - - If you develop a new program, and you want it to be of the greatest -possible use to the public, the best way to achieve this is to make it -free software which everyone can redistribute and change under these terms. - - To do so, attach the following notices to the program. It is safest -to attach them to the start of each source file to most effectively -convey the exclusion of warranty; and each file should have at least -the "copyright" line and a pointer to where the full notice is found. - - - Copyright (C) - - This program is free software; you can redistribute it and/or modify - it under the terms of the GNU General Public License as published by - the Free Software Foundation; either version 2 of the License, or - (at your option) any later version. - - This program is distributed in the hope that it will be useful, - but WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - GNU General Public License for more details. - - You should have received a copy of the GNU General Public License along - with this program; if not, write to the Free Software Foundation, Inc., - 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA. - -Also add information on how to contact you by electronic and paper mail. - -If the program is interactive, make it output a short notice like this -when it starts in an interactive mode: - - Gnomovision version 69, Copyright (C) year name of author - Gnomovision comes with ABSOLUTELY NO WARRANTY; for details type `show w'. - This is free software, and you are welcome to redistribute it - under certain conditions; type `show c' for details. - -The hypothetical commands `show w' and `show c' should show the appropriate -parts of the General Public License. Of course, the commands you use may -be called something other than `show w' and `show c'; they could even be -mouse-clicks or menu items--whatever suits your program. - -You should also get your employer (if you work as a programmer) or your -school, if any, to sign a "copyright disclaimer" for the program, if -necessary. Here is a sample; alter the names: - - Yoyodyne, Inc., hereby disclaims all copyright interest in the program - `Gnomovision' (which makes passes at compilers) written by James Hacker. - - , 1 April 1989 - Ty Coon, President of Vice - -This General Public License does not permit incorporating your program into -proprietary programs. If your program is a subroutine library, you may -consider it more useful to permit linking proprietary applications with the -library. If this is what you want to do, use the GNU Lesser General -Public License instead of this License. diff --git a/grid_map_sdf/include/grid_map_sdf/distance_transform/dt.h b/grid_map_sdf/include/grid_map_sdf/distance_transform/dt.h deleted file mode 100755 index 7a00eab49..000000000 --- a/grid_map_sdf/include/grid_map_sdf/distance_transform/dt.h +++ /dev/null @@ -1,117 +0,0 @@ -/* -Copyright (C) 2006 Pedro Felzenszwalb - -This program is free software; you can redistribute it and/or modify -it under the terms of the GNU General Public License as published by -the Free Software Foundation; either version 2 of the License, or -(at your option) any later version. - -This program is distributed in the hope that it will be useful, -but WITHOUT ANY WARRANTY; without even the implied warranty of -MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the -GNU General Public License for more details. - -You should have received a copy of the GNU General Public License -along with this program; if not, write to the Free Software -Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA -*/ - -/* distance transform */ - -#pragma once - -#include - -#include "image.h" -#include "imconv.h" - -namespace distance_transform -{ - const double INF = 1E20; - - /* dt of 1d function using squared distance */ - static float *dt(float *f, int n) { - float *d = new float[n]; - int *v = new int[n]; - float *z = new float[n+1]; - int k = 0; - v[0] = 0; - z[0] = -INF; - z[1] = +INF; - for (int q = 1; q <= n-1; q++) { - float s = ((f[q]+square(q))-(f[v[k]]+square(v[k])))/(2*q-2*v[k]); - while (s <= z[k]) { - k--; - s = ((f[q]+square(q))-(f[v[k]]+square(v[k])))/(2*q-2*v[k]); - } - k++; - v[k] = q; - z[k] = s; - z[k+1] = +INF; - } - - k = 0; - for (int q = 0; q <= n-1; q++) { - while (z[k+1] < q) - k++; - d[q] = square(q-v[k]) + f[v[k]]; - } - - delete [] v; - delete [] z; - return d; - } - - /* dt of 2d function using squared distance */ - static void dt(image *im) { - int width = im->width(); - int height = im->height(); - float *f = new float[std::max(width,height)]; - - // transform along columns - for (int x = 0; x < width; x++) { - for (int y = 0; y < height; y++) { - f[y] = imRef(im, x, y); - } - float *d = dt(f, height); - for (int y = 0; y < height; y++) { - imRef(im, x, y) = d[y]; - } - delete [] d; - } - - // transform along rows - for (int y = 0; y < height; y++) { - for (int x = 0; x < width; x++) { - f[x] = imRef(im, x, y); - } - float *d = dt(f, width); - for (int x = 0; x < width; x++) { - imRef(im, x, y) = d[x]; - } - delete [] d; - } - - delete [] f; - } - - /* dt of binary image using squared distance */ - static image *dt(image *im, uchar on = 1) { - int width = im->width(); - int height = im->height(); - - image *out = new image(width, height, false); - for (int y = 0; y < height; y++) { - for (int x = 0; x < width; x++) { - if (imRef(im, x, y) == on) - imRef(out, x, y) = 0; - else - imRef(out, x, y) = INF; - } - } - - dt(out); - return out; - } - -} // namespace diff --git a/grid_map_sdf/include/grid_map_sdf/distance_transform/image.h b/grid_map_sdf/include/grid_map_sdf/distance_transform/image.h deleted file mode 100755 index 47cada090..000000000 --- a/grid_map_sdf/include/grid_map_sdf/distance_transform/image.h +++ /dev/null @@ -1,101 +0,0 @@ -/* -Copyright (C) 2006 Pedro Felzenszwalb - -This program is free software; you can redistribute it and/or modify -it under the terms of the GNU General Public License as published by -the Free Software Foundation; either version 2 of the License, or -(at your option) any later version. - -This program is distributed in the hope that it will be useful, -but WITHOUT ANY WARRANTY; without even the implied warranty of -MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the -GNU General Public License for more details. - -You should have received a copy of the GNU General Public License -along with this program; if not, write to the Free Software -Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA -*/ - -/* a simple image class */ - -#pragma once - -#include - -namespace distance_transform -{ - - template - class image { - public: - /* create an image */ - image(const int width, const int height, const bool init = true); - - /* delete an image */ - ~image(); - - /* init an image */ - void init(const T &val); - - /* copy an image */ - image *copy() const; - - /* get the width of an image. */ - int width() const { return w; } - - /* get the height of an image. */ - int height() const { return h; } - - /* image data. */ - T *data; - - /* row pointers. */ - T **access; - - private: - int w, h; - }; - - /* use imRef to access image data. */ -#define imRef(im, x, y) (im->access[y][x]) - - /* use imPtr to get pointer to image data. */ -#define imPtr(im, x, y) &(im->access[y][x]) - - template - image::image(const int width, const int height, const bool init) { - w = width; - h = height; - data = new T[w * h]; // allocate space for image data - access = new T*[h]; // allocate space for row pointers - - // initialize row pointers - for (int i = 0; i < h; i++) - access[i] = data + (i * w); - - if (init) - memset(data, 0, w * h * sizeof(T)); - } - - template - image::~image() { - delete [] data; - delete [] access; - } - - template - void image::init(const T &val) { - T *ptr = imPtr(this, 0, 0); - T *end = imPtr(this, w-1, h-1); - while (ptr <= end) - *ptr++ = val; - } - - template - image *image::copy() const { - image *im = new image(w, h, false); - memcpy(im->data, data, w * h * sizeof(T)); - return im; - } - -} // namespace diff --git a/grid_map_sdf/include/grid_map_sdf/distance_transform/imconv.h b/grid_map_sdf/include/grid_map_sdf/distance_transform/imconv.h deleted file mode 100755 index b4f27ff36..000000000 --- a/grid_map_sdf/include/grid_map_sdf/distance_transform/imconv.h +++ /dev/null @@ -1,179 +0,0 @@ -/* -Copyright (C) 2006 Pedro Felzenszwalb - -This program is free software; you can redistribute it and/or modify -it under the terms of the GNU General Public License as published by -the Free Software Foundation; either version 2 of the License, or -(at your option) any later version. - -This program is distributed in the hope that it will be useful, -but WITHOUT ANY WARRANTY; without even the implied warranty of -MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the -GNU General Public License for more details. - -You should have received a copy of the GNU General Public License -along with this program; if not, write to the Free Software -Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA -*/ - -/* image conversion */ - -#pragma once - -#include - -#include "image.h" -#include "imutil.h" -#include "misc.h" - -namespace distance_transform -{ - const double RED_WEIGHT = 0.299; - const double GREEN_WEIGHT = 0.584; - const double BLUE_WEIGHT = 0.114; - - static image *imageRGBtoGRAY(image *input) { - int width = input->width(); - int height = input->height(); - image *output = new image(width, height, false); - - for (int y = 0; y < height; y++) { - for (int x = 0; x < width; x++) { - imRef(output, x, y) = (uchar) - (imRef(input, x, y).r * RED_WEIGHT + - imRef(input, x, y).g * GREEN_WEIGHT + - imRef(input, x, y).b * BLUE_WEIGHT); - } - } - return output; - } - - static image *imageGRAYtoRGB(image *input) { - int width = input->width(); - int height = input->height(); - image *output = new image(width, height, false); - - for (int y = 0; y < height; y++) { - for (int x = 0; x < width; x++) { - imRef(output, x, y).r = imRef(input, x, y); - imRef(output, x, y).g = imRef(input, x, y); - imRef(output, x, y).b = imRef(input, x, y); - } - } - return output; - } - - static image *imageUCHARtoFLOAT(image *input) { - int width = input->width(); - int height = input->height(); - image *output = new image(width, height, false); - - for (int y = 0; y < height; y++) { - for (int x = 0; x < width; x++) { - imRef(output, x, y) = imRef(input, x, y); - } - } - return output; - } - - static image *imageINTtoFLOAT(image *input) { - int width = input->width(); - int height = input->height(); - image *output = new image(width, height, false); - - for (int y = 0; y < height; y++) { - for (int x = 0; x < width; x++) { - imRef(output, x, y) = imRef(input, x, y); - } - } - return output; - } - - static image *imageFLOATtoUCHAR(image *input, - float min, float max) { - int width = input->width(); - int height = input->height(); - image *output = new image(width, height, false); - - if (max == min) - return output; - - float scale = UCHAR_MAX / (max - min); - for (int y = 0; y < height; y++) { - for (int x = 0; x < width; x++) { - uchar val = (uchar)((imRef(input, x, y) - min) * scale); - imRef(output, x, y) = bound(val, (uchar)0, (uchar)UCHAR_MAX); - } - } - return output; - } - - static image *imageFLOATtoUCHAR(image *input) { - float min, max; - min_max(input, &min, &max); - return imageFLOATtoUCHAR(input, min, max); - } - - static image *imageUCHARtoLONG(image *input) { - int width = input->width(); - int height = input->height(); - image *output = new image(width, height, false); - - for (int y = 0; y < height; y++) { - for (int x = 0; x < width; x++) { - imRef(output, x, y) = imRef(input, x, y); - } - } - return output; - } - - static image *imageLONGtoUCHAR(image *input, long min, long max) { - int width = input->width(); - int height = input->height(); - image *output = new image(width, height, false); - - if (max == min) - return output; - - float scale = UCHAR_MAX / (float)(max - min); - for (int y = 0; y < height; y++) { - for (int x = 0; x < width; x++) { - uchar val = (uchar)((imRef(input, x, y) - min) * scale); - imRef(output, x, y) = bound(val, (uchar)0, (uchar)UCHAR_MAX); - } - } - return output; - } - - static image *imageLONGtoUCHAR(image *input) { - long min, max; - min_max(input, &min, &max); - return imageLONGtoUCHAR(input, min, max); - } - - static image *imageSHORTtoUCHAR(image *input, - short min, short max) { - int width = input->width(); - int height = input->height(); - image *output = new image(width, height, false); - - if (max == min) - return output; - - float scale = UCHAR_MAX / (float)(max - min); - for (int y = 0; y < height; y++) { - for (int x = 0; x < width; x++) { - uchar val = (uchar)((imRef(input, x, y) - min) * scale); - imRef(output, x, y) = bound(val, (uchar)0, (uchar)UCHAR_MAX); - } - } - return output; - } - - static image *imageSHORTtoUCHAR(image *input) { - short min, max; - min_max(input, &min, &max); - return imageSHORTtoUCHAR(input, min, max); - } - -} // namespace diff --git a/grid_map_sdf/include/grid_map_sdf/distance_transform/imutil.h b/grid_map_sdf/include/grid_map_sdf/distance_transform/imutil.h deleted file mode 100755 index ad09d1f8e..000000000 --- a/grid_map_sdf/include/grid_map_sdf/distance_transform/imutil.h +++ /dev/null @@ -1,67 +0,0 @@ -/* -Copyright (C) 2006 Pedro Felzenszwalb - -This program is free software; you can redistribute it and/or modify -it under the terms of the GNU General Public License as published by -the Free Software Foundation; either version 2 of the License, or -(at your option) any later version. - -This program is distributed in the hope that it will be useful, -but WITHOUT ANY WARRANTY; without even the implied warranty of -MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the -GNU General Public License for more details. - -You should have received a copy of the GNU General Public License -along with this program; if not, write to the Free Software -Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA -*/ - -/* some image utilities */ - -#pragma once - -#include "image.h" -#include "misc.h" - -namespace distance_transform -{ - - /* compute minimum and maximum value in an image */ - template - void min_max(image *im, T *ret_min, T *ret_max) { - int width = im->width(); - int height = im->height(); - - T min = imRef(im, 0, 0); - T max = imRef(im, 0, 0); - for (int y = 0; y < height; y++) { - for (int x = 0; x < width; x++) { - T val = imRef(im, x, y); - if (min > val) - min = val; - if (max < val) - max = val; - } - } - - *ret_min = min; - *ret_max = max; - } - - /* threshold image */ - template - image *threshold(image *src, int t) { - int width = src->width(); - int height = src->height(); - image *dst = new image(width, height); - - for (int y = 0; y < height; y++) { - for (int x = 0; x < width; x++) { - imRef(dst, x, y) = (imRef(src, x, y) >= t); - } - } - - return dst; - } - -} // namespace diff --git a/grid_map_sdf/include/grid_map_sdf/distance_transform/misc.h b/grid_map_sdf/include/grid_map_sdf/distance_transform/misc.h deleted file mode 100755 index a9514d1f4..000000000 --- a/grid_map_sdf/include/grid_map_sdf/distance_transform/misc.h +++ /dev/null @@ -1,62 +0,0 @@ -/* -Copyright (C) 2006 Pedro Felzenszwalb - -This program is free software; you can redistribute it and/or modify -it under the terms of the GNU General Public License as published by -the Free Software Foundation; either version 2 of the License, or -(at your option) any later version. - -This program is distributed in the hope that it will be useful, -but WITHOUT ANY WARRANTY; without even the implied warranty of -MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the -GNU General Public License for more details. - -You should have received a copy of the GNU General Public License -along with this program; if not, write to the Free Software -Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA -*/ - -/* random stuff */ - -#pragma once - -#include - -namespace distance_transform -{ - typedef unsigned char uchar; - - typedef struct { uchar r, g, b; } rgb; - - inline bool operator==(const rgb &a, const rgb &b) { - return ((a.r == b.r) && (a.g == b.g) && (a.b == b.b)); - } - - template - inline T abs(const T &x) { return (x > 0 ? x : -x); }; - - template - inline int sign(const T &x) { return (x >= 0 ? 1 : -1); }; - - template - inline T square(const T &x) { return x*x; }; - - template - inline T bound(const T &x, const T &min, const T &max) { - return (x < min ? min : (x > max ? max : x)); - } - - template - inline bool check_bound(const T &x, const T&min, const T &max) { - return ((x < min) || (x > max)); - } - - inline int vlib_round(float x) { return (int)(x + 0.5F); } - - inline int vlib_round(double x) { return (int)(x + 0.5); } - - inline double gaussian(double val, double sigma) { - return exp(-square(val/sigma)/2)/(sqrt(2*M_PI)*sigma); - } - -} // namespace diff --git a/grid_map_sdf/include/grid_map_sdf/distance_transform/pnmfile.h b/grid_map_sdf/include/grid_map_sdf/distance_transform/pnmfile.h deleted file mode 100755 index 282843284..000000000 --- a/grid_map_sdf/include/grid_map_sdf/distance_transform/pnmfile.h +++ /dev/null @@ -1,214 +0,0 @@ -/* -Copyright (C) 2006 Pedro Felzenszwalb - -This program is free software; you can redistribute it and/or modify -it under the terms of the GNU General Public License as published by -the Free Software Foundation; either version 2 of the License, or -(at your option) any later version. - -This program is distributed in the hope that it will be useful, -but WITHOUT ANY WARRANTY; without even the implied warranty of -MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the -GNU General Public License for more details. - -You should have received a copy of the GNU General Public License -along with this program; if not, write to the Free Software -Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA -*/ - -/* basic image I/O */ - -#pragma once - -#include -#include -#include -#include - -#include "image.h" -#include "misc.h" - -namespace distance_transform -{ - - const short BUF_SIZE = 256; - - class pnm_error { }; - - static void read_packed(unsigned char *data, int size, std::ifstream &f) { - unsigned char c = 0; - - int bitshift = -1; - for (int pos = 0; pos < size; pos++) { - if (bitshift == -1) { - c = f.get(); - bitshift = 7; - } - data[pos] = (c >> bitshift) & 1; - bitshift--; - } - } - - static void write_packed(unsigned char *data, int size, std::ofstream &f) { - unsigned char c = 0; - - int bitshift = 7; - for (int pos = 0; pos < size; pos++) { - c = c | (data[pos] << bitshift); - bitshift--; - if ((bitshift == -1) || (pos == size-1)) { - f.put(c); - bitshift = 7; - c = 0; - } - } - } - - /* read PNM field, skipping comments */ - static void pnm_read(std::ifstream &file, char *buf) { - char doc[BUF_SIZE]; - char c; - - file >> c; - while (c == '#') { - file.getline(doc, BUF_SIZE); - file >> c; - } - file.putback(c); - - file.width(BUF_SIZE); - file >> buf; - file.ignore(); - } - - static image *loadPBM(const char *name) { - char buf[BUF_SIZE]; - - /* read header */ - std::ifstream file(name, std::ios::in | std::ios::binary); - pnm_read(file, buf); - if (strncmp(buf, "P4", 2)) - throw pnm_error(); - - pnm_read(file, buf); - int width = atoi(buf); - pnm_read(file, buf); - int height = atoi(buf); - - /* read data */ - image *im = new image(width, height); - for (int i = 0; i < height; i++) - read_packed(imPtr(im, 0, i), width, file); - - return im; - } - - static void savePBM(image *im, const char *name) { - int width = im->width(); - int height = im->height(); - std::ofstream file(name, std::ios::out | std::ios::binary); - - file << "P4\n" << width << " " << height << "\n"; - for (int i = 0; i < height; i++) - write_packed(imPtr(im, 0, i), width, file); - } - - static image *loadPGM(const char *name) { - char buf[BUF_SIZE]; - - /* read header */ - std::ifstream file(name, std::ios::in | std::ios::binary); - pnm_read(file, buf); - if (strncmp(buf, "P5", 2)) - throw pnm_error(); - - pnm_read(file, buf); - int width = atoi(buf); - pnm_read(file, buf); - int height = atoi(buf); - - pnm_read(file, buf); - if (atoi(buf) > UCHAR_MAX) - throw pnm_error(); - - /* read data */ - image *im = new image(width, height); - file.read((char *)imPtr(im, 0, 0), width * height * sizeof(uchar)); - - return im; - } - - static void savePGM(image *im, const char *name) { - int width = im->width(); - int height = im->height(); - std::ofstream file(name, std::ios::out | std::ios::binary); - - file << "P5\n" << width << " " << height << "\n" << UCHAR_MAX << "\n"; - file.write((char *)imPtr(im, 0, 0), width * height * sizeof(uchar)); - } - - static image *loadPPM(const char *name) { - char buf[BUF_SIZE], doc[BUF_SIZE]; - - /* read header */ - std::ifstream file(name, std::ios::in | std::ios::binary); - pnm_read(file, buf); - if (strncmp(buf, "P6", 2)) - throw pnm_error(); - - pnm_read(file, buf); - int width = atoi(buf); - pnm_read(file, buf); - int height = atoi(buf); - - pnm_read(file, buf); - if (atoi(buf) > UCHAR_MAX) - throw pnm_error(); - - /* read data */ - image *im = new image(width, height); - file.read((char *)imPtr(im, 0, 0), width * height * sizeof(rgb)); - - return im; - } - - static void savePPM(image *im, const char *name) { - int width = im->width(); - int height = im->height(); - std::ofstream file(name, std::ios::out | std::ios::binary); - - file << "P6\n" << width << " " << height << "\n" << UCHAR_MAX << "\n"; - file.write((char *)imPtr(im, 0, 0), width * height * sizeof(rgb)); - } - - template - void load_image(image **im, const char *name) { - char buf[BUF_SIZE]; - - /* read header */ - std::ifstream file(name, std::ios::in | std::ios::binary); - pnm_read(file, buf); - if (strncmp(buf, "VLIB", 9)) - throw pnm_error(); - - pnm_read(file, buf); - int width = atoi(buf); - pnm_read(file, buf); - int height = atoi(buf); - - /* read data */ - *im = new image(width, height); - file.read((char *)imPtr((*im), 0, 0), width * height * sizeof(T)); - } - - template - void save_image(image *im, const char *name) { - int width = im->width(); - int height = im->height(); - std::ofstream file(name, std::ios::out | std::ios::binary); - - file << "VLIB\n" << width << " " << height << "\n"; - file.write((char *)imPtr(im, 0, 0), width * height * sizeof(T)); - } - -} // namespace diff --git a/grid_map_sdf/package.xml b/grid_map_sdf/package.xml index 778413346..16be082ba 100644 --- a/grid_map_sdf/package.xml +++ b/grid_map_sdf/package.xml @@ -1,7 +1,7 @@ grid_map_sdf - 1.6.4 + 1.7.5 Generates signed distance fields from grid maps. Maximilian Wulf Yoshua Nava @@ -13,7 +13,6 @@ catkin grid_map_core - pcl_ros gtest diff --git a/grid_map_sdf/src/SignedDistance2d.cpp b/grid_map_sdf/src/SignedDistance2d.cpp new file mode 100644 index 000000000..c3ee519f5 --- /dev/null +++ b/grid_map_sdf/src/SignedDistance2d.cpp @@ -0,0 +1,290 @@ +/* +* SignedDistance2d.cpp +* +* Created on: Jul 10, 2020 +* Author: Ruben Grandia +* Institute: ETH Zurich +*/ + +#include "grid_map_sdf/SignedDistance2d.hpp" + +#include "grid_map_sdf/PixelBorderDistance.hpp" + +namespace grid_map { +namespace signed_distance_field { + +namespace internal { +struct DistanceLowerBound { + float v; // origin of bounding function + float f; // functional offset at the origin + float z_lhs; // lhs of interval where this bound holds + float z_rhs; // rhs of interval where this lower bound holds +}; + +/** +* 1D Euclidean Distance Transform based on: http://cs.brown.edu/people/pfelzens/dt/ +* Adapted to work on Eigen objects directly +* Optimized computation of s. +* Some optimization to not keep track of bounds that lie fully outside the grid. +*/ +std::vector::iterator fillLowerBounds(const Eigen::Ref& squareDistance1d, + std::vector& lowerBounds, Eigen::Index start) { + const auto n = squareDistance1d.size(); + const auto nFloat = static_cast(n); + const auto startFloat = static_cast(start); + + // Initialize + auto rhsBoundIt = lowerBounds.begin(); + *rhsBoundIt = DistanceLowerBound{startFloat, squareDistance1d[start], -INF, INF}; + auto firstBoundIt = lowerBounds.begin(); + + // Compute bounds to the right of minimum + float qFloat = rhsBoundIt->v + 1.0F; + for (Eigen::Index q = start + 1; q < n; ++q) { + // Storing this by value gives better performance (removed indirection?) + const float fq = squareDistance1d[q]; + + float s = equidistancePoint(qFloat, fq, rhsBoundIt->v, rhsBoundIt->f); + if (s < nFloat) { // Can ignore the lower bound derived from this point if it is only active outsize of [start, n] + // Search backwards in bounds until s is within [z_lhs, z_rhs] + while (s < rhsBoundIt->z_lhs) { + --rhsBoundIt; + s = equidistancePoint(qFloat, fq, rhsBoundIt->v, rhsBoundIt->f); + } + if (s > startFloat) { // Intersection is within [start, n]. Adjust current lowerbound and insert the new one after + rhsBoundIt->z_rhs = s; // Update the bound that comes before + ++rhsBoundIt; // insert new bound after. + *rhsBoundIt = DistanceLowerBound{qFloat, fq, s, INF}; // Valid from s till infinity + } else { // Intersection is outside [0, n]. This means that the new bound dominates all previous bounds + *rhsBoundIt = DistanceLowerBound{qFloat, fq, -INF, INF}; + firstBoundIt = rhsBoundIt; // No need to keep other bounds, so update the first bound iterator to this one. + } + } + + // Increment to follow loop counter as float + qFloat += 1.0F; + } + + return firstBoundIt; +} + +void extractSquareDistances(Eigen::Ref squareDistance1d, std::vector::const_iterator lowerBoundIt, + Eigen::Index start) { + const auto n = squareDistance1d.size(); + + // Store active bound by value to remove indirection + auto lastz = lowerBoundIt->z_rhs; + + auto qFloat = static_cast(start); + for (Eigen::Index q = start; q < n; ++q) { + if (squareDistance1d[q] > 0.0F) { // Observe that if squareDistance1d[q] == 0.0, this is already the minimum and it will stay 0.0 + // Find the new active lower bound if q no longer belongs to current interval + if (qFloat > lastz) { + do { + ++lowerBoundIt; + } while (lowerBoundIt->z_rhs < qFloat); + lastz = lowerBoundIt->z_rhs; + } + + squareDistance1d[q] = squarePixelBorderDistance(qFloat, lowerBoundIt->v, lowerBoundIt->f); + } + + qFloat += 1.0F; + } +} + +/** +* Same as extractSquareDistances, but takes the sqrt as a final step. +* Because several cells will have a value of 0.0 (obstacle / free space label), we can skip the sqrt computation for those. +*/ +void extractDistances(Eigen::Ref squareDistance1d, + std::vector::const_iterator lowerBoundIt, Eigen::Index start) { + const auto n = squareDistance1d.size(); + + // Store active bound by value to remove indirection + auto lastz = lowerBoundIt->z_rhs; + + auto qFloat = static_cast(start); + for (Eigen::Index q = start; q < n; ++q) { + if (squareDistance1d[q] > 0.0F) { // Observe that if squareDistance1d[q] == 0.0, this is already the minimum and it will stay 0.0 + // Find the new active lower bound if q no longer belongs to current interval + if (qFloat > lastz) { + do { + ++lowerBoundIt; + } while (lowerBoundIt->z_rhs < qFloat); + lastz = lowerBoundIt->z_rhs; + } + + squareDistance1d[q] = std::sqrt(squarePixelBorderDistance(qFloat, lowerBoundIt->v, lowerBoundIt->f)); + } + + qFloat += 1.0F; + } +} + +/** +* Find the location of the last zero value from the front +*/ +Eigen::Index lastZeroFromFront(const Eigen::Ref& squareDistance1d) { + const auto n = squareDistance1d.size(); + + for (Eigen::Index q = 0; q < n; ++q) { + if (squareDistance1d[q] > 0.0F) { + if (q > 0) { + return q - 1; + } else { + return 0; + } + } + } + return n; +} + +inline void squaredDistanceTransform_1d_inplace(Eigen::Ref squareDistance1d, + std::vector& lowerBounds) { + auto start = lastZeroFromFront(squareDistance1d); + + // Only need to process line if there are nonzero elements. Also the first zeros stay untouched. + if (start < squareDistance1d.size()) { + auto startIt = fillLowerBounds(squareDistance1d, lowerBounds, start); + extractSquareDistances(squareDistance1d, startIt, start); + } +} + +/** +* Same as above, but takes sqrt as final step (within the same loop) +* @param squareDistance1d : input as squared distance, output is the distance after sqrt. +* @param lowerBounds : work vector +*/ +inline void distanceTransform_1d_inplace(Eigen::Ref squareDistance1d, std::vector& lowerBounds) { + auto start = lastZeroFromFront(squareDistance1d); + + // Only need to process line if there are nonzero elements. Also the first zeros stay untouched. + if (start < squareDistance1d.size()) { + auto startIt = fillLowerBounds(squareDistance1d, lowerBounds, start); + extractDistances(squareDistance1d, startIt, start); + } +} + +void computePixelDistance2dTranspose(Matrix& input, Matrix& distanceTranspose) { + const auto n = input.rows(); + const auto m = input.cols(); + + // Allocate a buffer big enough for processing both rowise and columnwise + std::vector lowerBounds(std::max(n, m)); + + // Process columns + for (Eigen::Index i = 0; i < m; ++i) { + squaredDistanceTransform_1d_inplace(input.col(i), lowerBounds); + } + + // Process rows (= columns after transpose). + distanceTranspose = input.transpose(); + for (Eigen::Index i = 0; i < n; ++i) { + // Fuses square distance algorithm and taking sqrt. + distanceTransform_1d_inplace(distanceTranspose.col(i), lowerBounds); + } +} + +// Initialize with square distance in height direction in pixel units if above the surface +void initializeObstacleDistance(const Matrix& elevationMap, Matrix& result, float height, float resolution) { + /* Vectorized implementation of: + * if (height > elevation) { + * const auto diff = (height - elevation) / resolution; + * return diff * diff; + * } else { + * return 0.0F; + * } + */ + result = ((1.0F / resolution) * (height - elevationMap.array()).cwiseMax(0.0F)).square(); +} + +// Initialize with square distance in height direction in pixel units if below the surface +void initializeObstacleFreeDistance(const Matrix& elevationMap, Matrix& result, float height, float resolution) { + /* Vectorized implementation of: + * if (height < elevation) { + * const auto diff = (height - elevation) / resolution; + * return diff * diff; + * } else { + * return 0.0F; + * } + */ + result = ((1.0F / resolution) * (height - elevationMap.array()).cwiseMin(0.0F)).square(); +} + +void pixelDistanceToFreeSpaceTranspose(const Matrix& elevationMap, Matrix& sdfObstacleFree, Matrix& tmp, float height, float resolution) { + internal::initializeObstacleFreeDistance(elevationMap, tmp, height, resolution); + internal::computePixelDistance2dTranspose(tmp, sdfObstacleFree); +} + +void pixelDistanceToObstacleTranspose(const Matrix& elevationMap, Matrix& sdfObstacleTranspose, Matrix& tmp, float height, + float resolution) { + internal::initializeObstacleDistance(elevationMap, tmp, height, resolution); + internal::computePixelDistance2dTranspose(tmp, sdfObstacleTranspose); +} + +Matrix signedDistanceFromOccupancyTranspose(const Eigen::Matrix& occupancyGrid, float resolution) { + // Compute pixel distance to obstacles + Matrix sdfObstacle; + Matrix init = occupancyGrid.unaryExpr([=](bool val) { return (val) ? 0.0F : INF; }); + internal::computePixelDistance2dTranspose(init, sdfObstacle); + + // Compute pixel distance to obstacle free space + Matrix sdfObstacleFree; + init = occupancyGrid.unaryExpr([=](bool val) { return (val) ? INF : 0.0F; }); + internal::computePixelDistance2dTranspose(init, sdfObstacleFree); + + return resolution * (sdfObstacle - sdfObstacleFree); +} + +} // namespace internal + +void signedDistanceAtHeightTranspose(const Matrix& elevationMap, Matrix& sdfTranspose, Matrix& tmp, Matrix& tmpTranspose, float height, + float resolution, float minHeight, float maxHeight) { + const bool allPixelsAreObstacles = height < minHeight; + const bool allPixelsAreFreeSpace = height > maxHeight; + + if (allPixelsAreObstacles) { + internal::pixelDistanceToFreeSpaceTranspose(elevationMap, sdfTranspose, tmp, height, resolution); + + sdfTranspose *= -resolution; + } else if (allPixelsAreFreeSpace) { + internal::pixelDistanceToObstacleTranspose(elevationMap, sdfTranspose, tmp, height, resolution); + + sdfTranspose *= resolution; + } else { // This layer contains a mix of obstacles and free space + internal::pixelDistanceToObstacleTranspose(elevationMap, sdfTranspose, tmp, height, resolution); + internal::pixelDistanceToFreeSpaceTranspose(elevationMap, tmpTranspose, tmp, height, resolution); + + sdfTranspose = resolution * (sdfTranspose - tmpTranspose); + } +} + +Matrix signedDistanceAtHeight(const Matrix& elevationMap, float height, float resolution, float minHeight, float maxHeight) { + Matrix sdfTranspose; + Matrix tmp; + Matrix tmpTranspose; + + signedDistanceAtHeightTranspose(elevationMap, sdfTranspose, tmp, tmpTranspose, height, resolution, minHeight, maxHeight); + return sdfTranspose.transpose(); +} + +Matrix signedDistanceFromOccupancy(const Eigen::Matrix& occupancyGrid, float resolution) { + auto obstacleCount = occupancyGrid.count(); + bool hasObstacles = obstacleCount > 0; + if (hasObstacles) { + bool hasFreeSpace = obstacleCount < occupancyGrid.size(); + if (hasFreeSpace) { + return internal::signedDistanceFromOccupancyTranspose(occupancyGrid, resolution).transpose(); + } else { + // Only obstacles -> distance is minus infinity everywhere + return Matrix::Constant(occupancyGrid.rows(), occupancyGrid.cols(), -INF); + } + } else { + // No obstacles -> planar distance is infinite + return Matrix::Constant(occupancyGrid.rows(), occupancyGrid.cols(), INF); + } +} + +} // namespace signed_distance_field +} // namespace grid_map \ No newline at end of file diff --git a/grid_map_sdf/src/SignedDistanceField.cpp b/grid_map_sdf/src/SignedDistanceField.cpp index 163dc3a2b..16ecd3906 100644 --- a/grid_map_sdf/src/SignedDistanceField.cpp +++ b/grid_map_sdf/src/SignedDistanceField.cpp @@ -1,184 +1,198 @@ /* - * SignedDistanceField.hpp + * SignedDistanceField.cpp * - * Created on: Aug 16, 2017 - * Authors: Takahiro Miki, Peter Fankhauser - * Institute: ETH Zurich, ANYbotics + * Created on: Jul 10, 2020 + * Author: Ruben Grandia + * Institute: ETH Zurich */ -#include -#include +#include "grid_map_sdf/SignedDistanceField.hpp" -#include +#include -#include - -using namespace distance_transform; +#include "grid_map_sdf/DistanceDerivatives.hpp" +#include "grid_map_sdf/SignedDistance2d.hpp" namespace grid_map { -SignedDistanceField::SignedDistanceField() - : maxDistance_(std::numeric_limits::max()), - zIndexStartHeight_(0.0), - resolution_(0.0), - lowestHeight_(-1e5) // We need some precision. -{ -} +// Import from the signed_distance_field namespace +using signed_distance_field::columnwiseCentralDifference; +using signed_distance_field::Gridmap3dLookup; +using signed_distance_field::layerCentralDifference; +using signed_distance_field::layerFiniteDifference; +using signed_distance_field::signedDistanceAtHeightTranspose; + +SignedDistanceField::SignedDistanceField(const GridMap& gridMap, const std::string& elevationLayer, double minHeight, double maxHeight) + : frameId_(gridMap.getFrameId()), timestamp_(gridMap.getTimestamp()) { + assert(maxHeight >= minHeight); + + // Determine origin of the 3D grid + Position mapOriginXY; + gridMap.getPosition(Eigen::Vector2i(0, 0), mapOriginXY); + Position3 gridOrigin(mapOriginXY.x(), mapOriginXY.y(), minHeight); + + // Round up the Z-discretization. We need a minimum of two layers to enable finite difference in Z direction + const auto numZLayers = static_cast(std::max(std::ceil((maxHeight - minHeight) / gridMap.getResolution()), 2.0)); + const size_t numXrows = gridMap.getSize().x(); + const size_t numYrows = gridMap.getSize().y(); + Gridmap3dLookup::size_t_3d gridsize = {numXrows, numYrows, numZLayers}; + + // Initialize 3D lookup + gridmap3DLookup_ = Gridmap3dLookup(gridsize, gridOrigin, gridMap.getResolution()); + + // Allocate the internal data structure + data_.reserve(gridmap3DLookup_.linearSize()); + + // Check for NaN + const auto& elevationData = gridMap.get(elevationLayer); + if (elevationData.hasNaN()) { + std::cerr + << "[grid_map_sdf::SignedDistanceField] elevation data contains NaN. The generated SDF will be invalid! Apply inpainting first" + << std::endl; + } -SignedDistanceField::~SignedDistanceField() -{ + // Compute the SDF + computeSignedDistance(elevationData); } -void SignedDistanceField::calculateSignedDistanceField(const GridMap& gridMap, const std::string& layer, - const double heightClearance) -{ - data_.clear(); - resolution_ = gridMap.getResolution(); - position_ = gridMap.getPosition(); - size_ = gridMap.getSize(); - Matrix map = gridMap.get(layer); // Copy! - - float minHeight = map.minCoeffOfFinites(); - if (!std::isfinite(minHeight)) minHeight = lowestHeight_; - float maxHeight = map.maxCoeffOfFinites(); - if (!std::isfinite(maxHeight)) maxHeight = lowestHeight_; - - const float valueForEmptyCells = lowestHeight_; // maxHeight, minHeight (TODO Make this an option). - for (size_t i = 0; i < map.size(); ++i) { - if (std::isnan(map(i))) map(i) = valueForEmptyCells; - } +double SignedDistanceField::value(const Position3& position) const noexcept { + const auto nodeIndex = gridmap3DLookup_.nearestNode(position); + const auto nodePosition = gridmap3DLookup_.nodePosition(nodeIndex); + const auto nodeData = data_[gridmap3DLookup_.linearIndex(nodeIndex)]; + const auto jacobian = derivative(nodeData); + return distance(nodeData) + jacobian.dot(position - nodePosition); +} - // Height range of the signed distance field is higher than the max height. - maxHeight += heightClearance; - - Matrix sdfElevationAbove = Matrix::Ones(map.rows(), map.cols()) * maxDistance_; - Matrix sdfLayer = Matrix::Zero(map.rows(), map.cols()); - std::vector sdf; - zIndexStartHeight_ = minHeight; - - // Calculate signed distance field from bottom. - for (float h = minHeight; h < maxHeight; h += resolution_) { - Eigen::Matrix obstacleFreeField = map.array() < h; - Eigen::Matrix obstacleField = obstacleFreeField.array() < 1; - Matrix sdfObstacle = getPlanarSignedDistanceField(obstacleField); - Matrix sdfObstacleFree = getPlanarSignedDistanceField(obstacleFreeField); - Matrix sdf2d; - // If 2d sdfObstacleFree calculation failed, neglect this SDF - // to avoid extreme small distances (-INF). - if ((sdfObstacleFree.array() >= INF).any()) sdf2d = sdfObstacle; - else sdf2d = sdfObstacle - sdfObstacleFree; - sdf2d *= resolution_; - for (size_t i = 0; i < sdfElevationAbove.size(); ++i) { - if(sdfElevationAbove(i) == maxDistance_ && map(i) <= h) sdfElevationAbove(i) = h - map(i); - else if(sdfElevationAbove(i) != maxDistance_ && map(i) <= h) sdfElevationAbove(i) = sdfLayer(i) + resolution_; - if (sdf2d(i) == 0) sdfLayer(i) = h - map(i); - else if (sdf2d(i) < 0) sdfLayer(i) = -std::min(fabs(sdf2d(i)), fabs(map(i) - h)); - else sdfLayer(i) = std::min(sdf2d(i), sdfElevationAbove(i)); - } - data_.push_back(sdfLayer); - } +SignedDistanceField::Derivative3 SignedDistanceField::derivative(const Position3& position) const noexcept { + const auto nodeIndex = gridmap3DLookup_.nearestNode(position); + const auto nodeData = data_[gridmap3DLookup_.linearIndex(nodeIndex)]; + return derivative(nodeData); } -grid_map::Matrix SignedDistanceField::getPlanarSignedDistanceField(Eigen::Matrix& data) const -{ - image *input = new image(data.rows(), data.cols(), true); +std::pair SignedDistanceField::valueAndDerivative(const Position3& position) const noexcept { + const auto nodeIndex = gridmap3DLookup_.nearestNode(position); + const auto nodePosition = gridmap3DLookup_.nodePosition(nodeIndex); + const auto nodeData = data_[gridmap3DLookup_.linearIndex(nodeIndex)]; + const auto jacobian = derivative(nodeData); + return {distance(nodeData) + jacobian.dot(position - nodePosition), jacobian}; +} - for (int y = 0; y < input->height(); y++) { - for (int x = 0; x < input->width(); x++) { - imRef(input, x, y) = data(x, y); - } +void SignedDistanceField::computeSignedDistance(const Matrix& elevation) { + const auto gridOriginZ = static_cast(gridmap3DLookup_.gridOrigin_.z()); + const auto resolution = static_cast(gridmap3DLookup_.resolution_); + const auto minHeight = elevation.minCoeff(); + const auto maxHeight = elevation.maxCoeff(); + + /* + * General strategy to reduce the amount of transposing: + * - SDF at a height is in transposed form after computing it. + * - Take the finite difference in dx, now that this data is continuous in memory. + * - Transpose the SDF + * - Take other finite differences. Now dy is efficient. + * - When writing to the 3D structure, keep in mind that dx is still transposed. + */ + + // Memory needed to compute the SDF at a layer + Matrix tmp; // allocated on first use + Matrix tmpTranspose; // allocated on first use + Matrix sdfTranspose; // allocated on first use + + // Memory needed to keep a buffer of layers. We need 3 due to the central difference + Matrix currentLayer; // allocated on first use + Matrix nextLayer; // allocated on first use + Matrix previousLayer; // allocated on first use + + // Memory needed to compute finite differences + Matrix dxTranspose = Matrix::Zero(elevation.cols(), elevation.rows()); + Matrix dxNextTranspose = Matrix::Zero(elevation.cols(), elevation.rows()); + Matrix dy = Matrix::Zero(elevation.rows(), elevation.cols()); + Matrix dz = Matrix::Zero(elevation.rows(), elevation.cols()); + + // Compute SDF of first layer + computeLayerSdfandDeltaX(elevation, currentLayer, dxTranspose, sdfTranspose, tmp, tmpTranspose, gridOriginZ, resolution, minHeight, + maxHeight); + + // Compute SDF of second layer + computeLayerSdfandDeltaX(elevation, nextLayer, dxNextTranspose, sdfTranspose, tmp, tmpTranspose, gridOriginZ + resolution, resolution, + minHeight, maxHeight); + + // First layer: forward difference in z + layerFiniteDifference(currentLayer, nextLayer, dz, resolution); // dz / layer = +resolution + columnwiseCentralDifference(currentLayer, dy, -resolution); // dy / dcol = -resolution + + emplacebackLayerData(currentLayer, dxTranspose, dy, dz); + + // Middle layers: central difference in z + for (size_t layerZ = 1; layerZ + 1 < gridmap3DLookup_.gridsize_.z; ++layerZ) { + // Circulate layer buffers + previousLayer.swap(currentLayer); + currentLayer.swap(nextLayer); + dxTranspose.swap(dxNextTranspose); + + // Compute SDF of next layer + computeLayerSdfandDeltaX(elevation, nextLayer, dxNextTranspose, sdfTranspose, tmp, tmpTranspose, + gridOriginZ + (layerZ + 1) * resolution, resolution, minHeight, maxHeight); + + // Compute other finite differences + layerCentralDifference(previousLayer, nextLayer, dz, resolution); + columnwiseCentralDifference(currentLayer, dy, -resolution); + + // Add the data to the 3D structure + emplacebackLayerData(currentLayer, dxTranspose, dy, dz); } - // Compute dt. - image *out = dt(input); + // Circulate layer buffers one last time + previousLayer.swap(currentLayer); + currentLayer.swap(nextLayer); + dxTranspose.swap(dxNextTranspose); - Matrix result(data.rows(), data.cols()); + // Last layer: backward difference in z + layerFiniteDifference(previousLayer, currentLayer, dz, resolution); + columnwiseCentralDifference(currentLayer, dy, -resolution); + + // Add the data to the 3D structure + emplacebackLayerData(currentLayer, dxTranspose, dy, dz); +} +void SignedDistanceField::computeLayerSdfandDeltaX(const Matrix& elevation, Matrix& currentLayer, Matrix& dxTranspose, Matrix& sdfTranspose, + Matrix& tmp, Matrix& tmpTranspose, float height, float resolution, float minHeight, + float maxHeight) const { + // Compute SDF + dx of layer: compute sdfTranspose -> take dxTranspose -> transpose to get sdf + signedDistanceAtHeightTranspose(elevation, sdfTranspose, tmp, tmpTranspose, height, resolution, minHeight, maxHeight); + columnwiseCentralDifference(sdfTranspose, dxTranspose, -resolution); // dx / drow = -resolution + currentLayer = sdfTranspose.transpose(); +} - // Take square roots. - for (int y = 0; y < out->height(); y++) { - for (int x = 0; x < out->width(); x++) { - result(x, y) = sqrt(imRef(out, x, y)); +void SignedDistanceField::emplacebackLayerData(const Matrix& signedDistance, const Matrix& dxTranspose, const Matrix& dy, + const Matrix& dz) { + for (size_t colY = 0; colY < gridmap3DLookup_.gridsize_.y; ++colY) { + for (size_t rowX = 0; rowX < gridmap3DLookup_.gridsize_.x; ++rowX) { + data_.emplace_back(node_data_t{signedDistance(rowX, colY), dxTranspose(colY, rowX), dy(rowX, colY), dz(rowX, colY)}); } } - return result; } -double SignedDistanceField::getDistanceAt(const Position3& position) const -{ - double xCenter = size_.x() / 2.0; - double yCenter = size_.y() / 2.0; - int i = std::round(xCenter - (position.x() - position_.x()) / resolution_); - int j = std::round(yCenter - (position.y() - position_.y()) / resolution_); - int k = std::round((position.z() - zIndexStartHeight_) / resolution_); - i = std::max(i, 0); - i = std::min(i, size_.x() - 1); - j = std::max(j, 0); - j = std::min(j, size_.y() - 1); - k = std::max(k, 0); - k = std::min(k, (int)data_.size() - 1); - return data_[k](i, j); +size_t SignedDistanceField::size() const noexcept { + return gridmap3DLookup_.linearSize(); } -double SignedDistanceField::getInterpolatedDistanceAt(const Position3& position) const -{ - double xCenter = size_.x() / 2.0; - double yCenter = size_.y() / 2.0; - int i = std::round(xCenter - (position.x() - position_.x()) / resolution_); - int j = std::round(yCenter - (position.y() - position_.y()) / resolution_); - int k = std::round((position.z() - zIndexStartHeight_) / resolution_); - i = std::max(i, 0); - i = std::min(i, size_.x() - 1); - j = std::max(j, 0); - j = std::min(j, size_.y() - 1); - k = std::max(k, 0); - k = std::min(k, (int)data_.size() - 1); - Vector3 gradient = getDistanceGradientAt(position); - double xp = position_.x() + ((size_.x() - i) - xCenter) * resolution_; - double yp = position_.y() + ((size_.y() - j) - yCenter) * resolution_; - double zp = zIndexStartHeight_ + k * resolution_; - Vector3 error = position - Vector3(xp, yp, zp); - return data_[k](i, j) + gradient.dot(error); +const std::string& SignedDistanceField::getFrameId() const noexcept { + return frameId_; } -Vector3 SignedDistanceField::getDistanceGradientAt(const Position3& position) const -{ - double xCenter = size_.x() / 2.0; - double yCenter = size_.y() / 2.0; - int i = std::round(xCenter - (position.x() - position_.x()) / resolution_); - int j = std::round(yCenter - (position.y() - position_.y()) / resolution_); - int k = std::round((position.z() - zIndexStartHeight_) / resolution_); - i = std::max(i, 1); - i = std::min(i, size_.x() - 2); - j = std::max(j, 1); - j = std::min(j, size_.y() - 2); - k = std::max(k, 1); - k = std::min(k, (int)data_.size() - 2); - double dx = (data_[k](i - 1, j) - data_[k](i + 1, j)) / (2 * resolution_); - double dy = (data_[k](i, j - 1) - data_[k](i, j + 1)) / (2 * resolution_); - double dz = (data_[k + 1](i, j) - data_[k - 1](i, j)) / (2 * resolution_); - return Vector3(dx, dy, dz); +Time SignedDistanceField::getTime() const noexcept { + return timestamp_; } -void SignedDistanceField::convertToPointCloud(pcl::PointCloud& points) const -{ - double xCenter = size_.x() / 2.0; - double yCenter = size_.y() / 2.0; - for (int z = 0; z < data_.size(); z++){ - for (int y = 0; y < size_.y(); y++) { - for (int x = 0; x < size_.x(); x++) { - double xp = position_.x() + ((size_.x() - x) - xCenter) * resolution_; - double yp = position_.y() + ((size_.y() - y) - yCenter) * resolution_; - double zp = zIndexStartHeight_ + z * resolution_; - pcl::PointXYZI p; - p.x = xp; - p.y = yp; - p.z = zp; - p.intensity = data_[z](x, y); - points.push_back(p); +void SignedDistanceField::filterPoints(std::function func, size_t decimation) const { + for (size_t layerZ = 0; layerZ < gridmap3DLookup_.gridsize_.z; layerZ += decimation) { + for (size_t colY = 0; colY < gridmap3DLookup_.gridsize_.y; colY += decimation) { + for (size_t rowX = 0; rowX < gridmap3DLookup_.gridsize_.x; rowX += decimation) { + const Gridmap3dLookup::size_t_3d index3d = {rowX, colY, layerZ}; + const auto index = gridmap3DLookup_.linearIndex(index3d); + func(gridmap3DLookup_.nodePosition(index3d), distanceFloat(data_[index]), derivative(data_[index])); } } } - return; } -} /* namespace */ +} // namespace grid_map diff --git a/grid_map_sdf/test/SignedDistanceFieldTest.cpp b/grid_map_sdf/test/SignedDistanceFieldTest.cpp deleted file mode 100644 index c66adbcc0..000000000 --- a/grid_map_sdf/test/SignedDistanceFieldTest.cpp +++ /dev/null @@ -1,305 +0,0 @@ -/* - * SignedDistanceFieldTest.cpp - * - * Created on: Aug 25, 2017 - * Author: Takahiro Miki, Peter Fankhauser - * Institute: ETH Zurich, ANYbotics - */ - -#include "grid_map_core/GridMap.hpp" -#include "grid_map_sdf/SignedDistanceField.hpp" - -#include -#include - -using namespace std; -using namespace grid_map; - -TEST(SignedDistanceField, EmptyMap) -{ - GridMap map({"layer"}); - map.setGeometry(Length(1.0, 2.0), 0.1, Position(0.0, 0.0)); - - SignedDistanceField sdf; - sdf.calculateSignedDistanceField(map, "layer", 1.0); - Position3 position(0.0, 0.0, 0.0); - - EXPECT_NO_THROW(sdf.getDistanceAt(position)); - EXPECT_NO_THROW(sdf.getInterpolatedDistanceAt(position)); - EXPECT_NO_THROW(sdf.getDistanceGradientAt(position)); -} - -TEST(SignedDistanceField, GetDistanceFlat) -{ - GridMap map({"layer"}); - map.setGeometry(Length(1.0, 2.0), 0.1, Position(0.1, 0.2)); - map["layer"].setConstant(1.0); - map.at("layer", Index(0,0)) = -1.0; - - SignedDistanceField sdf; - sdf.calculateSignedDistanceField(map, "layer", 2.5); - Position pos; - map.getPosition(Index(9, 9), pos); - EXPECT_NEAR(sdf.getDistanceAt(Vector3(pos.x(), pos.y(), 0.0)), -1.0, 0.0001); - EXPECT_NEAR(sdf.getDistanceAt(Vector3(pos.x(), pos.y(), 0.1)), -0.9, 0.0001); - EXPECT_NEAR(sdf.getDistanceAt(Vector3(pos.x(), pos.y(), 0.2)), -0.8, 0.0001); - EXPECT_NEAR(sdf.getDistanceAt(Vector3(pos.x(), pos.y(), 0.3)), -0.7, 0.0001); - EXPECT_NEAR(sdf.getDistanceAt(Vector3(pos.x(), pos.y(), 0.4)), -0.6, 0.0001); - EXPECT_NEAR(sdf.getDistanceAt(Vector3(pos.x(), pos.y(), 0.5)), -0.5, 0.0001); - EXPECT_NEAR(sdf.getDistanceAt(Vector3(pos.x(), pos.y(), 0.6)), -0.4, 0.0001); - EXPECT_NEAR(sdf.getDistanceAt(Vector3(pos.x(), pos.y(), 0.7)), -0.3, 0.0001); - EXPECT_NEAR(sdf.getDistanceAt(Vector3(pos.x(), pos.y(), 0.8)), -0.2, 0.0001); - EXPECT_NEAR(sdf.getDistanceAt(Vector3(pos.x(), pos.y(), 0.9)), -0.1, 0.0001); - EXPECT_NEAR(sdf.getDistanceAt(Vector3(pos.x(), pos.y(), 1.0)), 0.0, 0.0001); - EXPECT_NEAR(sdf.getDistanceAt(Vector3(pos.x(), pos.y(), 1.1)), 0.1, 0.0001); - EXPECT_NEAR(sdf.getDistanceAt(Vector3(pos.x(), pos.y(), 1.2)), 0.2, 0.0001); - EXPECT_NEAR(sdf.getDistanceAt(Vector3(pos.x(), pos.y(), 1.3)), 0.3, 0.0001); - EXPECT_NEAR(sdf.getDistanceAt(Vector3(pos.x(), pos.y(), 1.4)), 0.4, 0.0001); - EXPECT_NEAR(sdf.getDistanceAt(Vector3(pos.x(), pos.y(), 1.5)), 0.5, 0.0001); - EXPECT_NEAR(sdf.getDistanceAt(Vector3(pos.x(), pos.y(), 1.6)), 0.6, 0.0001); - EXPECT_NEAR(sdf.getDistanceAt(Vector3(pos.x(), pos.y(), 1.7)), 0.7, 0.0001); - EXPECT_NEAR(sdf.getDistanceAt(Vector3(pos.x(), pos.y(), 1.8)), 0.8, 0.0001); - EXPECT_NEAR(sdf.getDistanceAt(Vector3(pos.x(), pos.y(), 1.9)), 0.9, 0.0001); - EXPECT_NEAR(sdf.getDistanceAt(Vector3(pos.x(), pos.y(), 2.0)), 1.0, 0.0001); - EXPECT_NEAR(sdf.getDistanceAt(Vector3(pos.x(), pos.y(), 2.1)), 1.1, 0.0001); - EXPECT_NEAR(sdf.getDistanceAt(Vector3(pos.x(), pos.y(), 2.2)), 1.2, 0.0001); - EXPECT_NEAR(sdf.getDistanceAt(Vector3(pos.x(), pos.y(), 2.3)), 1.3, 0.0001); - EXPECT_NEAR(sdf.getDistanceAt(Vector3(pos.x(), pos.y(), 2.4)), 1.4, 0.0001); - EXPECT_NEAR(sdf.getDistanceAt(Vector3(pos.x(), pos.y(), 2.5)), 1.5, 0.0001); - EXPECT_NEAR(sdf.getDistanceAt(Vector3(pos.x(), pos.y(), 10.0)), 2.5, 0.0001); -} - - -TEST(SignedDistanceField, GetDistance) -{ - GridMap map({"layer"}); - map.setGeometry(Length(1.0, 2.0), 0.1, Position(0.15, 0.25)); - map["layer"].setConstant(1.0); - - map.at("layer", Index(3,4)) = 2.0; - map.at("layer", Index(3,5)) = 2.0; - map.at("layer", Index(3,6)) = 2.0; - map.at("layer", Index(4,4)) = 2.0; - map.at("layer", Index(4,5)) = 2.0; - map.at("layer", Index(4,6)) = 2.0; - map.at("layer", Index(5,4)) = 2.0; - map.at("layer", Index(5,5)) = 2.0; - map.at("layer", Index(5,6)) = 2.0; - map.at("layer", Index(6,4)) = 2.0; - map.at("layer", Index(6,5)) = 2.0; - map.at("layer", Index(6,6)) = 2.0; - map.at("layer", Index(7,4)) = 2.0; - map.at("layer", Index(7,5)) = 2.0; - map.at("layer", Index(7,6)) = 2.0; - - SignedDistanceField sdf; - sdf.calculateSignedDistanceField(map, "layer", 1.5); - Position pos; - - map.getPosition(Index(5, 5), pos); - EXPECT_NEAR(sdf.getDistanceAt(Vector3(pos.x(), pos.y(), 0.0)), -1.0, 0.0001); - EXPECT_NEAR(sdf.getDistanceAt(Vector3(pos.x(), pos.y(), 0.5)), -1.0, 0.0001); - EXPECT_NEAR(sdf.getDistanceAt(Vector3(pos.x(), pos.y(), 1.0)), -1.0, 0.0001); - EXPECT_NEAR(sdf.getDistanceAt(Vector3(pos.x(), pos.y(), 1.1)), -0.1, 0.0001); - EXPECT_NEAR(sdf.getDistanceAt(Vector3(pos.x(), pos.y(), 1.2)), -0.1, 0.0001); - EXPECT_NEAR(sdf.getDistanceAt(Vector3(pos.x(), pos.y(), 1.3)), -0.1, 0.0001); - EXPECT_NEAR(sdf.getDistanceAt(Vector3(pos.x(), pos.y(), 1.4)), -0.1, 0.0001); - EXPECT_NEAR(sdf.getDistanceAt(Vector3(pos.x(), pos.y(), 1.5)), -0.1, 0.0001); - EXPECT_NEAR(sdf.getDistanceAt(Vector3(pos.x(), pos.y(), 1.6)), -0.1, 0.0001); - EXPECT_NEAR(sdf.getDistanceAt(Vector3(pos.x(), pos.y(), 1.7)), -0.1, 0.0001); - EXPECT_NEAR(sdf.getDistanceAt(Vector3(pos.x(), pos.y(), 1.8)), -0.1, 0.0001); - EXPECT_NEAR(sdf.getDistanceAt(Vector3(pos.x(), pos.y(), 1.8)), -0.1, 0.0001); - EXPECT_NEAR(sdf.getDistanceAt(Vector3(pos.x(), pos.y(), 2.0)), 0.0, 0.0001); - EXPECT_NEAR(sdf.getDistanceAt(Vector3(pos.x(), pos.y(), 2.1)), 0.1, 0.0001); - EXPECT_NEAR(sdf.getDistanceAt(Vector3(pos.x(), pos.y(), 2.2)), 0.2, 0.0001); - EXPECT_NEAR(sdf.getDistanceAt(Vector3(pos.x(), pos.y(), 2.3)), 0.3, 0.0001); - EXPECT_NEAR(sdf.getDistanceAt(Vector3(pos.x(), pos.y(), 2.4)), 0.4, 0.0001); - EXPECT_NEAR(sdf.getDistanceAt(Vector3(pos.x(), pos.y(), 2.5)), 0.5, 0.0001); - EXPECT_NEAR(sdf.getDistanceAt(Vector3(pos.x(), pos.y(), 10.0)), 1.5, 0.0001); - - map.getPosition(Index(5, 2), pos); - EXPECT_NEAR(sdf.getDistanceAt(Vector3(pos.x(), pos.y(), 0.0)), 0.0, 0.0001); - EXPECT_NEAR(sdf.getDistanceAt(Vector3(pos.x(), pos.y(), 0.5)), 0.0, 0.0001); - EXPECT_NEAR(sdf.getDistanceAt(Vector3(pos.x(), pos.y(), 1.0)), 0.0, 0.0001); - EXPECT_NEAR(sdf.getDistanceAt(Vector3(pos.x(), pos.y(), 1.1)), 0.1, 0.0001); - EXPECT_NEAR(sdf.getDistanceAt(Vector3(pos.x(), pos.y(), 1.2)), 0.1, 0.0001); - EXPECT_NEAR(sdf.getDistanceAt(Vector3(pos.x(), pos.y(), 1.3)), 0.1, 0.0001); - EXPECT_NEAR(sdf.getDistanceAt(Vector3(pos.x(), pos.y(), 1.4)), 0.1, 0.0001); - EXPECT_NEAR(sdf.getDistanceAt(Vector3(pos.x(), pos.y(), 1.5)), 0.1, 0.0001); - EXPECT_NEAR(sdf.getDistanceAt(Vector3(pos.x(), pos.y(), 1.6)), 0.1, 0.0001); - EXPECT_NEAR(sdf.getDistanceAt(Vector3(pos.x(), pos.y(), 1.7)), 0.1, 0.0001); - EXPECT_NEAR(sdf.getDistanceAt(Vector3(pos.x(), pos.y(), 1.8)), 0.1, 0.0001); - EXPECT_NEAR(sdf.getDistanceAt(Vector3(pos.x(), pos.y(), 1.9)), 0.1, 0.0001); - EXPECT_NEAR(sdf.getDistanceAt(Vector3(pos.x(), pos.y(), 2.0)), 0.2, 0.0001); - EXPECT_NEAR(sdf.getDistanceAt(Vector3(pos.x(), pos.y(), 2.1)), 0.3, 0.0001); - EXPECT_NEAR(sdf.getDistanceAt(Vector3(pos.x(), pos.y(), 2.2)), 0.4, 0.0001); - EXPECT_NEAR(sdf.getDistanceAt(Vector3(pos.x(), pos.y(), 2.3)), 0.5, 0.0001); - EXPECT_NEAR(sdf.getDistanceAt(Vector3(pos.x(), pos.y(), 2.4)), 0.6, 0.0001); - EXPECT_NEAR(sdf.getDistanceAt(Vector3(pos.x(), pos.y(), 2.5)), 0.7, 0.0001); - EXPECT_NEAR(sdf.getDistanceAt(Vector3(pos.x(), pos.y(), 10.0)), 1.7, 0.0001); -} - -TEST(SignedDistanceField, GetDistanceGradient) -{ - GridMap map({"layer"}); - map.setGeometry(Length(1.0, 2.0), 0.1, Position(0.15, 0.25)); - map["layer"].setConstant(1.0); - - map.at("layer", Index(3,4)) = 2.0; - map.at("layer", Index(3,5)) = 2.0; - map.at("layer", Index(3,6)) = 2.0; - map.at("layer", Index(4,4)) = 2.0; - map.at("layer", Index(4,5)) = 2.0; - map.at("layer", Index(4,6)) = 2.0; - map.at("layer", Index(5,4)) = 2.0; - map.at("layer", Index(5,5)) = 2.0; - map.at("layer", Index(5,6)) = 2.0; - map.at("layer", Index(6,4)) = 2.0; - map.at("layer", Index(6,5)) = 2.0; - map.at("layer", Index(6,6)) = 2.0; - map.at("layer", Index(7,4)) = 2.0; - map.at("layer", Index(7,5)) = 2.0; - map.at("layer", Index(7,6)) = 2.0; - - SignedDistanceField sdf; - sdf.calculateSignedDistanceField(map, "layer", 1.5); - Position pos; - Vector3 gradient; - - map.getPosition(Index(5, 6), pos); - gradient = sdf.getDistanceGradientAt(Vector3(pos.x(), pos.y(), 0.0)); - EXPECT_NEAR(gradient.x(), 0.0, 0.0001); - EXPECT_NEAR(gradient.y(), -1, 0.0001); - EXPECT_NEAR(gradient.z(), 0.5, 0.0001); - gradient = sdf.getDistanceGradientAt(Vector3(pos.x(), pos.y(), 1.0)); - EXPECT_NEAR(gradient.x(), 0.0, 0.0001); - EXPECT_NEAR(gradient.y(), -1, 0.0001); - EXPECT_NEAR(gradient.z(), 0.5, 0.0001); - gradient = sdf.getDistanceGradientAt(Vector3(pos.x(), pos.y(), 2.0)); - EXPECT_NEAR(gradient.x(), 0.0, 0.0001); - EXPECT_NEAR(gradient.y(), -1.5, 0.0001); - EXPECT_NEAR(gradient.z(), 1, 0.0001); - gradient = sdf.getDistanceGradientAt(Vector3(pos.x(), pos.y(), 2.2)); - EXPECT_NEAR(gradient.x(), 0.0, 0.0001); - EXPECT_NEAR(gradient.y(), -1.5, 0.0001); - EXPECT_NEAR(gradient.z(), 1.0, 0.0001); - gradient = sdf.getDistanceGradientAt(Vector3(pos.x(), pos.y(), 10.0)); - EXPECT_NEAR(gradient.x(), 0.0, 0.0001); - EXPECT_NEAR(gradient.y(), -1.5, 0.0001); - EXPECT_NEAR(gradient.z(), 1.0, 0.0001); - map.getPosition(Index(2, 2), pos); - gradient = sdf.getDistanceGradientAt(Vector3(pos.x(), pos.y(), 1.0)); - EXPECT_NEAR(gradient.x(), 0.0, 0.0001); - EXPECT_NEAR(gradient.y(), 1, 0.0001); - EXPECT_NEAR(gradient.z(), 0.5, 0.0001); - gradient = sdf.getDistanceGradientAt(Vector3(pos.x(), pos.y(), 2.0)); - EXPECT_NEAR(gradient.x(), 0.207107, 0.0001); - EXPECT_NEAR(gradient.y(), 1.5, 0.0001); - EXPECT_NEAR(gradient.z(), 1, 0.0001); - gradient = sdf.getDistanceGradientAt(Vector3(pos.x(), pos.y(), 2.2)); - EXPECT_NEAR(gradient.x(), 0.207107, 0.0001); - EXPECT_NEAR(gradient.y(), 1.5, 0.0001); - EXPECT_NEAR(gradient.z(), 1, 0.0001); - map.getPosition(Index(12, 22), pos); - gradient = sdf.getDistanceGradientAt(Vector3(pos.x(), pos.y(), 1.0)); - EXPECT_NEAR(gradient.x(), 0.0, 0.0001); - EXPECT_NEAR(gradient.y(), 1.0, 0.0001); - EXPECT_NEAR(gradient.z(), 0.5, 0.0001); -} - -TEST(SignedDistanceField, GetInterpolatedDistance) -{ - GridMap map({"layer"}); - map.setGeometry(Length(1.0, 2.0), 0.1, Position(0.15, 0.25)); - map["layer"].setConstant(1.0); - - map.at("layer", Index(3,3)) = 2.0; - map.at("layer", Index(3,4)) = 2.0; - map.at("layer", Index(3,5)) = 2.0; - map.at("layer", Index(3,6)) = 2.0; - map.at("layer", Index(3,7)) = 2.0; - map.at("layer", Index(4,3)) = 2.0; - map.at("layer", Index(4,4)) = 2.0; - map.at("layer", Index(4,5)) = 2.0; - map.at("layer", Index(4,6)) = 2.0; - map.at("layer", Index(4,7)) = 2.0; - map.at("layer", Index(5,3)) = 2.0; - map.at("layer", Index(5,4)) = 2.0; - map.at("layer", Index(5,5)) = 2.0; - map.at("layer", Index(5,6)) = 2.0; - map.at("layer", Index(5,7)) = 2.0; - map.at("layer", Index(6,3)) = 2.0; - map.at("layer", Index(6,4)) = 2.0; - map.at("layer", Index(6,5)) = 2.0; - map.at("layer", Index(6,6)) = 2.0; - map.at("layer", Index(6,7)) = 2.0; - map.at("layer", Index(7,3)) = 2.0; - map.at("layer", Index(7,4)) = 2.0; - map.at("layer", Index(7,5)) = 2.0; - map.at("layer", Index(7,6)) = 2.0; - map.at("layer", Index(7,7)) = 2.0; - - SignedDistanceField sdf; - sdf.calculateSignedDistanceField(map, "layer", 1.5); - Position pos; - - map.getPosition(Index(5, 5), pos); - EXPECT_NEAR(sdf.getInterpolatedDistanceAt(Vector3(pos.x(), pos.y(), 0.0)), -5.05, 0.0001); - EXPECT_NEAR(sdf.getInterpolatedDistanceAt(Vector3(pos.x(), pos.y(), 0.5)), -3.05, 0.0001); - EXPECT_NEAR(sdf.getInterpolatedDistanceAt(Vector3(pos.x(), pos.y(), 1.0)), -1.05, 0.0001); - EXPECT_NEAR(sdf.getInterpolatedDistanceAt(Vector3(pos.x(), pos.y(), 1.1)), -0.25, 0.0001); - EXPECT_NEAR(sdf.getInterpolatedDistanceAt(Vector3(pos.x(), pos.y(), 1.2)), -0.25, 0.0001); - EXPECT_NEAR(sdf.getInterpolatedDistanceAt(Vector3(pos.x(), pos.y(), 1.3)), -0.25, 0.0001); - EXPECT_NEAR(sdf.getInterpolatedDistanceAt(Vector3(pos.x(), pos.y(), 1.4)), -0.25, 0.0001); - EXPECT_NEAR(sdf.getInterpolatedDistanceAt(Vector3(pos.x(), pos.y(), 1.5)), -0.25, 0.0001); - EXPECT_NEAR(sdf.getInterpolatedDistanceAt(Vector3(pos.x(), pos.y(), 1.6)), -0.25, 0.0001); - EXPECT_NEAR(sdf.getInterpolatedDistanceAt(Vector3(pos.x(), pos.y(), 1.7)), -0.25, 0.0001); - EXPECT_NEAR(sdf.getInterpolatedDistanceAt(Vector3(pos.x(), pos.y(), 1.8)), -0.25, 0.0001); - EXPECT_NEAR(sdf.getInterpolatedDistanceAt(Vector3(pos.x(), pos.y(), 1.9)), -0.1, 0.0001); - EXPECT_NEAR(sdf.getInterpolatedDistanceAt(Vector3(pos.x(), pos.y(), 2.0)), 0.0, 0.0001); - EXPECT_NEAR(sdf.getInterpolatedDistanceAt(Vector3(pos.x(), pos.y(), 2.1)), 0.1, 0.0001); - EXPECT_NEAR(sdf.getInterpolatedDistanceAt(Vector3(pos.x(), pos.y(), 2.2)), 0.2, 0.0001); - EXPECT_NEAR(sdf.getInterpolatedDistanceAt(Vector3(pos.x(), pos.y(), 2.3)), 0.3, 0.0001); - EXPECT_NEAR(sdf.getInterpolatedDistanceAt(Vector3(pos.x(), pos.y(), 2.4)), 0.4, 0.0001); - EXPECT_NEAR(sdf.getInterpolatedDistanceAt(Vector3(pos.x(), pos.y(), 2.5)), 0.5, 0.0001); - EXPECT_NEAR(sdf.getInterpolatedDistanceAt(Vector3(pos.x(), pos.y(), 10.0)), 8, 0.0001); - - map.getPosition(Index(5, 10), pos); - EXPECT_NEAR(sdf.getInterpolatedDistanceAt(Vector3(pos.x(), pos.y(), 0.0)), -1.0, 0.0001); - EXPECT_NEAR(sdf.getInterpolatedDistanceAt(Vector3(pos.x(), pos.y(), 0.5)), -0.5, 0.0001); - EXPECT_NEAR(sdf.getInterpolatedDistanceAt(Vector3(pos.x(), pos.y(), 1.0)), 0.0, 0.0001); - EXPECT_NEAR(sdf.getInterpolatedDistanceAt(Vector3(pos.x(), pos.y(), 1.1)), 0.1, 0.0001); - EXPECT_NEAR(sdf.getInterpolatedDistanceAt(Vector3(pos.x(), pos.y(), 1.2)), 0.2, 0.0001); - EXPECT_NEAR(sdf.getInterpolatedDistanceAt(Vector3(pos.x(), pos.y(), 1.3)), 0.3, 0.0001); - EXPECT_NEAR(sdf.getInterpolatedDistanceAt(Vector3(pos.x(), pos.y(), 1.5)), 0.35, 0.0001); - EXPECT_NEAR(sdf.getInterpolatedDistanceAt(Vector3(pos.x(), pos.y(), 1.7)), 0.35, 0.0001); - EXPECT_NEAR(sdf.getInterpolatedDistanceAt(Vector3(pos.x(), pos.y(), 1.9)), 0.35, 0.0001); - EXPECT_NEAR(sdf.getInterpolatedDistanceAt(Vector3(pos.x(), pos.y(), 2.0)), 0.45, 0.0001); - EXPECT_NEAR(sdf.getInterpolatedDistanceAt(Vector3(pos.x(), pos.y(), 2.1)), 0.55, 0.0001); - EXPECT_NEAR(sdf.getInterpolatedDistanceAt(Vector3(pos.x(), pos.y(), 2.2)), 0.65, 0.0001); - EXPECT_NEAR(sdf.getInterpolatedDistanceAt(Vector3(pos.x(), pos.y(), 2.3)), 0.75, 0.0001); - EXPECT_NEAR(sdf.getInterpolatedDistanceAt(Vector3(pos.x(), pos.y(), 2.4)), 0.85, 0.0001); - EXPECT_NEAR(sdf.getInterpolatedDistanceAt(Vector3(pos.x(), pos.y(), 2.5)), 0.95, 0.0001); - EXPECT_NEAR(sdf.getInterpolatedDistanceAt(Vector3(pos.x(), pos.y(), 10.0)), 8.45, 0.0001); - - map.getPosition(Index(5, 0), pos); - EXPECT_NEAR(sdf.getInterpolatedDistanceAt(Vector3(pos.x(), pos.y(), 1.8)), 0.25, 0.0001); - map.getPosition(Index(5, 1), pos); - EXPECT_NEAR(sdf.getInterpolatedDistanceAt(Vector3(pos.x(), pos.y(), 1.8)), 0.175, 0.0001); - map.getPosition(Index(5, 2), pos); - EXPECT_NEAR(sdf.getInterpolatedDistanceAt(Vector3(pos.x(), pos.y(), 1.8)), -0.025, 0.0001); - map.getPosition(Index(5, 3), pos); - EXPECT_NEAR(sdf.getInterpolatedDistanceAt(Vector3(pos.x(), pos.y(), 1.8)), -0.2, 0.0001); - map.getPosition(Index(5, 4), pos); - EXPECT_NEAR(sdf.getInterpolatedDistanceAt(Vector3(pos.x(), pos.y(), 1.8)), -0.225, 0.0001); - map.getPosition(Index(5, 5), pos); - EXPECT_NEAR(sdf.getInterpolatedDistanceAt(Vector3(pos.x(), pos.y(), 1.8)), -0.25, 0.0001); - map.getPosition(Index(5, 6), pos); - EXPECT_NEAR(sdf.getInterpolatedDistanceAt(Vector3(pos.x(), pos.y(), 1.8)), -0.175, 0.0001); - map.getPosition(Index(5, 7), pos); - EXPECT_NEAR(sdf.getInterpolatedDistanceAt(Vector3(pos.x(), pos.y(), 1.8)), 0.025, 0.0001); - map.getPosition(Index(5, 8), pos); - EXPECT_NEAR(sdf.getInterpolatedDistanceAt(Vector3(pos.x(), pos.y(), 1.8)), 0.15, 0.0001); - map.getPosition(Index(5, 9), pos); - EXPECT_NEAR(sdf.getInterpolatedDistanceAt(Vector3(pos.x(), pos.y(), 1.8)), 0.25, 0.0001); -} diff --git a/grid_map_sdf/test/include/naiveSignedDistance.hpp b/grid_map_sdf/test/include/naiveSignedDistance.hpp new file mode 100644 index 000000000..c3957a0b2 --- /dev/null +++ b/grid_map_sdf/test/include/naiveSignedDistance.hpp @@ -0,0 +1,116 @@ +/* + * naiveSignedDistance.h + * + * Created on: Aug 10, 2020 + * Author: Ruben Grandia + * Institute: ETH Zurich + */ + +#pragma once + +namespace grid_map { +namespace signed_distance_field { + +inline Eigen::Matrix occupancyAtHeight(const Matrix& elevationMap, float height) { + Eigen::Matrix occupancy = elevationMap.unaryExpr([=](float val) { return val > height; }); + return occupancy; +} + +inline bool isEqualSdf(const Matrix& sdf0, const Matrix& sdf1, float tol) { + if (sdf0.cols() != sdf1.cols() || sdf0.rows() != sdf1.rows()) { + return false; + } + + for (Eigen::Index col = 0; col < sdf0.cols(); ++col) { + for (Eigen::Index row = 0; row < sdf0.rows(); ++row) { + if (sdf0(row, col) == sdf1(row, col) || std::abs(sdf0(row, col) - sdf1(row, col)) < tol) { + continue; + } else { + return false; + } + } + } + return true; +} + +// N^2 naive implementation, for testing purposes +inline Matrix naiveSignedDistanceAtHeight(const Matrix& elevationMap, float height, float resolution) { + Matrix signedDistance(elevationMap.rows(), elevationMap.cols()); + + // For each point + for (Eigen::Index col = 0; col < elevationMap.cols(); ++col) { + for (Eigen::Index row = 0; row < elevationMap.rows(); ++row) { + if (elevationMap(row, col) >= height) { // point in the SDF is below surface + signedDistance(row, col) = -INF; + // find closest open space over all other points + for (Eigen::Index j = 0; j < elevationMap.cols(); ++j) { + for (Eigen::Index i = 0; i < elevationMap.rows(); ++i) { + // Compute distance to free cube at location (i, j) + const float dx{resolution * pixelBorderDistance(i, row)}; + const float dy{resolution * pixelBorderDistance(j, col)}; + const float dz{std::max(elevationMap(i, j) - height, 0.0F)}; + const float currentSignedDistance{-std::sqrt(dx * dx + dy * dy + dz * dz)}; + signedDistance(row, col) = std::max(signedDistance(row, col), currentSignedDistance); + } + } + } else { // point in the SDF is above surface + signedDistance(row, col) = INF; + // find closest object over all other points + for (Eigen::Index j = 0; j < elevationMap.cols(); ++j) { + for (Eigen::Index i = 0; i < elevationMap.rows(); ++i) { + // Compute distance to occupied cube at location (i, j) + const float dx{resolution * pixelBorderDistance(i, row)}; + const float dy{resolution * pixelBorderDistance(j, col)}; + const float dz{std::max(height - elevationMap(i, j), 0.0F)}; + const float currentSignedDistance{std::sqrt(dx * dx + dy * dy + dz * dz)}; + signedDistance(row, col) = std::min(signedDistance(row, col), currentSignedDistance); + } + } + } + } + } + + return signedDistance; +} + +inline Matrix naiveSignedDistanceFromOccupancy(const Eigen::Matrix& occupancyGrid, float resolution) { + Matrix signedDistance(occupancyGrid.rows(), occupancyGrid.cols()); + + // For each point + for (Eigen::Index col = 0; col < occupancyGrid.cols(); ++col) { + for (Eigen::Index row = 0; row < occupancyGrid.rows(); ++row) { + if (occupancyGrid(row, col)) { // This point is an obstable + signedDistance(row, col) = -INF; + // find closest open space over all other points + for (Eigen::Index j = 0; j < occupancyGrid.cols(); ++j) { + for (Eigen::Index i = 0; i < occupancyGrid.rows(); ++i) { + if (!occupancyGrid(i, j)) { + const float dx{resolution * pixelBorderDistance(i, row)}; + const float dy{resolution * pixelBorderDistance(j, col)}; + const float currentSignedDistance{-std::sqrt(dx * dx + dy * dy)}; + signedDistance(row, col) = std::max(signedDistance(row, col), currentSignedDistance); + } + } + } + } else { // This point is in free space + signedDistance(row, col) = INF; + // find closest object over all other points + for (Eigen::Index j = 0; j < occupancyGrid.cols(); ++j) { + for (Eigen::Index i = 0; i < occupancyGrid.rows(); ++i) { + if (occupancyGrid(i, j)) { + const float dx{resolution * pixelBorderDistance(i, row)}; + const float dy{resolution * pixelBorderDistance(j, col)}; + const float currentSignedDistance{std::sqrt(dx * dx + dy * dy)}; + signedDistance(row, col) = std::min(signedDistance(row, col), currentSignedDistance); + } + } + } + } + } + } + + return signedDistance; +} + +} // namespace signed_distance_field +} // namespace grid_map \ No newline at end of file diff --git a/grid_map_sdf/test/test3dLookup.cpp b/grid_map_sdf/test/test3dLookup.cpp new file mode 100644 index 000000000..254a6cbd5 --- /dev/null +++ b/grid_map_sdf/test/test3dLookup.cpp @@ -0,0 +1,59 @@ +/* + * test3dLookup.cpp + * + * Created on: Aug 18, 2020 + * Author: Ruben Grandia + * Institute: ETH Zurich + */ + +#include + +#include "grid_map_sdf/Gridmap3dLookup.hpp" + +using namespace grid_map; +using namespace signed_distance_field; +using size_t_3d = Gridmap3dLookup::size_t_3d; + +TEST(testGridmap3dLookup, nearestNode) { + const size_t_3d gridsize{8, 9, 10}; + const Position3 gridOrigin{-0.1, -0.2, -0.4}; + const double resolution = 0.1; + + Gridmap3dLookup gridmap3DLookup(gridsize, gridOrigin, resolution); + + // Retrieve origin + const size_t_3d originNode = gridmap3DLookup.nearestNode(gridOrigin); + ASSERT_EQ(originNode.x, 0); + ASSERT_EQ(originNode.y, 0); + ASSERT_EQ(originNode.z, 0); + + // test underflow + const size_t_3d originNodeProjected = gridmap3DLookup.nearestNode(gridOrigin + Position3{1e20, 1e20, -1e20}); + ASSERT_EQ(originNodeProjected.x, 0); + ASSERT_EQ(originNodeProjected.y, 0); + ASSERT_EQ(originNodeProjected.z, 0); + + // test overflow + const size_t_3d maxNodeProjected = gridmap3DLookup.nearestNode(gridOrigin + Position3{-1e20, -1e20, +1e20}); + ASSERT_EQ(maxNodeProjected.x, gridsize.x - 1); + ASSERT_EQ(maxNodeProjected.y, gridsize.y - 1); + ASSERT_EQ(maxNodeProjected.z, gridsize.z - 1); + + // Nearest neighbour + const size_t_3d nodeIndex{3, 4, 5}; + const Position3 nodePosition = gridmap3DLookup.nodePosition(nodeIndex); + const size_t_3d closestNodeIndex = gridmap3DLookup.nearestNode(nodePosition + 0.49 * Position3::Constant(resolution)); + ASSERT_EQ(closestNodeIndex.x, nodeIndex.x); + ASSERT_EQ(closestNodeIndex.y, nodeIndex.y); + ASSERT_EQ(closestNodeIndex.z, nodeIndex.z); +} + +TEST(testGridmap3dLookup, linearIndex) { + const size_t_3d gridsize{8, 9, 10}; + const Position3 gridOrigin{-0.1, -0.2, -0.4}; + const double resolution = 0.1; + + Gridmap3dLookup gridmap3DLookup(gridsize, gridOrigin, resolution); + ASSERT_EQ(gridmap3DLookup.linearIndex({0, 0, 0}), 0); + ASSERT_EQ(gridmap3DLookup.linearIndex({gridsize.x - 1, gridsize.y - 1, gridsize.z - 1}), gridmap3DLookup.linearSize() - 1); +} \ No newline at end of file diff --git a/grid_map_sdf/test/testDerivatives.cpp b/grid_map_sdf/test/testDerivatives.cpp new file mode 100644 index 000000000..5fa890412 --- /dev/null +++ b/grid_map_sdf/test/testDerivatives.cpp @@ -0,0 +1,31 @@ +/* + * testDerivatives.cpp + * + * Created on: Aug 10, 2020 + * Author: Ruben Grandia + * Institute: ETH Zurich + */ + +#include + +#include "grid_map_sdf/DistanceDerivatives.hpp" + +using namespace grid_map; +using namespace signed_distance_field; + +TEST(testDerivatives, columnwise) { + Matrix data(2, 3); + data << 1.0, 2.0, 4.0, 2.0, 4.0, 6.0; + + float resolution{0.1F}; + float doubleResolution{2.0F * resolution}; + + Matrix manualDifference(2, 3); + manualDifference << 1.0 / resolution, 3.0 / doubleResolution, 2.0 / resolution, 2.0 / resolution, 4.0 / doubleResolution, + 2.0 / resolution; + + Matrix computedDifference = Matrix::Zero(data.rows(), data.cols()); + columnwiseCentralDifference(data, computedDifference, resolution); + + ASSERT_TRUE(manualDifference.isApprox(computedDifference)); +} diff --git a/grid_map_sdf/test/testPixelBorderDistance.cpp b/grid_map_sdf/test/testPixelBorderDistance.cpp new file mode 100644 index 000000000..38b1a441c --- /dev/null +++ b/grid_map_sdf/test/testPixelBorderDistance.cpp @@ -0,0 +1,76 @@ +/* + * testPixelBorderDistance.cpp + * + * Created on: Aug 7, 2020 + * Author: Ruben Grandia + * Institute: ETH Zurich + */ + +#include + +#include "grid_map_sdf/PixelBorderDistance.hpp" +#include "grid_map_sdf/Utils.hpp" + +using namespace grid_map; +using namespace signed_distance_field; + +TEST(testPixelBorderDistance, distanceFunction) { + // Basic properties of the distance function + ASSERT_TRUE(pixelBorderDistance(0, 0) == 0.0F); + ASSERT_FLOAT_EQ(pixelBorderDistance(0, 1), 0.5); + ASSERT_FLOAT_EQ(pixelBorderDistance(0, 2), 1.5); + ASSERT_TRUE(pixelBorderDistance(0, 1) == pixelBorderDistance(1, 0)); + ASSERT_TRUE(pixelBorderDistance(-10, 42) == pixelBorderDistance(42, -10)); +} + +TEST(testPixelBorderDistance, equidistantPoint) { + int pixelRange = 10; + float offsetRange = 20.0; + float offsetStep = 0.25; + float tol = 1e-4; + + for (int p = -pixelRange; p < pixelRange; ++p) { + for (float fp = -offsetRange; fp < offsetRange; fp += offsetStep) { + for (int q = -pixelRange; q < pixelRange; ++q) { + for (float fq = -offsetRange; fq < offsetRange; fq += offsetStep) { + // Fix that offset is the same if pixels are the same + if (p == q) { + fp = fq; + } + // Check symmetry of the equidistant point computation + float s0 = equidistancePoint(q, fq, p, fp); + float s1 = equidistancePoint(p, fp, q, fq); + ASSERT_LT(std::abs(s0 - s1), tol); + + // Check that the distance from s0 to p and q is indeed equal + float dp = squarePixelBorderDistance(s0, p, fp); + float dq = squarePixelBorderDistance(s0, q, fq); + ASSERT_LT(std::abs(dp - dq), tol) << "p: " << p << ", q: " << q << ", fp: " << fp << ", fq: " << fq; + } + } + } + } +} + +TEST(testPixelBorderDistance, equidistantPointInfCases) { + const float pixelTestDistance{1e6}; // Pick a very high pixel index + // With one of the cells at +INF, the intersection will always be +- INF on the side of that cell + // Here the intersection is at the left = -INF + EXPECT_FLOAT_EQ(equidistancePoint(-pixelTestDistance, INF, pixelTestDistance, std::numeric_limits::max()), -INF); + EXPECT_FLOAT_EQ(equidistancePoint(-pixelTestDistance, INF, pixelTestDistance, 0.0F), -INF); + EXPECT_FLOAT_EQ(equidistancePoint(-pixelTestDistance, INF, pixelTestDistance, std::numeric_limits::lowest()), -INF); + EXPECT_FLOAT_EQ(equidistancePoint(-pixelTestDistance, INF, pixelTestDistance, -INF), -INF); + // Here the intersection is at the right = +INF + EXPECT_FLOAT_EQ(equidistancePoint(-pixelTestDistance, std::numeric_limits::max(), pixelTestDistance, INF), INF); + EXPECT_FLOAT_EQ(equidistancePoint(-pixelTestDistance, 0.0F, pixelTestDistance, INF), INF); + EXPECT_FLOAT_EQ(equidistancePoint(-pixelTestDistance, std::numeric_limits::lowest(), pixelTestDistance, INF), INF); + EXPECT_FLOAT_EQ(equidistancePoint(-pixelTestDistance, -INF, pixelTestDistance, INF), INF); + // Except when both are infinite, then the intersection is in the middle + EXPECT_FLOAT_EQ(equidistancePoint(-pixelTestDistance, INF, pixelTestDistance, INF), 0.0F); + EXPECT_FLOAT_EQ( + equidistancePoint(-pixelTestDistance, std::numeric_limits::max(), pixelTestDistance, std::numeric_limits::max()), 0.0F); + EXPECT_FLOAT_EQ( + equidistancePoint(-pixelTestDistance, std::numeric_limits::lowest(), pixelTestDistance, std::numeric_limits::lowest()), + 0.0F); + EXPECT_FLOAT_EQ(equidistancePoint(-pixelTestDistance, -INF, pixelTestDistance, -INF), 0.0F); +} diff --git a/grid_map_sdf/test/testSignedDistance2d.cpp b/grid_map_sdf/test/testSignedDistance2d.cpp new file mode 100644 index 000000000..621f96c17 --- /dev/null +++ b/grid_map_sdf/test/testSignedDistance2d.cpp @@ -0,0 +1,112 @@ +/* + * testSignedDistance2d.cpp + * + * Created on: Jul 10, 2020 + * Author: Ruben Grandia + * Institute: ETH Zurich + */ + +#include + +#include "grid_map_sdf/PixelBorderDistance.hpp" +#include "grid_map_sdf/SignedDistance2d.hpp" + +#include "naiveSignedDistance.hpp" + +using namespace grid_map; +using namespace signed_distance_field; + +TEST(testSignedDistance2d, signedDistance2d_noObstacles) { + const int n = 3; + const int m = 4; + const float resolution = 0.1; + const Matrix map = Matrix::Ones(n, m); + + const auto occupancy = occupancyAtHeight(map, 2.0); + const auto signedDistance = signedDistanceFromOccupancy(occupancy, resolution); + + ASSERT_TRUE((signedDistance.array() == INF).all()); +} + +TEST(testSignedDistance2d, signedDistance2d_allObstacles) { + const int n = 3; + const int m = 4; + const float resolution = 0.1; + const Matrix map = Matrix::Ones(n, m); + + const auto occupancy = occupancyAtHeight(map, 0.0); + const auto signedDistance = signedDistanceFromOccupancy(occupancy, resolution); + + ASSERT_TRUE((signedDistance.array() == -INF).all()); +} + +TEST(testSignedDistance2d, signedDistance2d_mixed) { + const int n = 2; + const int m = 3; + const float resolution = 1.0; + Matrix map(n, m); + map << 0.0, 1.0, 1.0, 0.0, 0.0, 1.0; + const auto occupancy = occupancyAtHeight(map, 0.5); + + const auto naiveSignedDistance = naiveSignedDistanceFromOccupancy(occupancy, resolution); + const auto signedDistance = signedDistanceFromOccupancy(occupancy, resolution); + ASSERT_TRUE(isEqualSdf(signedDistance, naiveSignedDistance, 1e-4)); +} + +TEST(testSignedDistance2d, signedDistance2d_oneObstacle) { + const int n = 20; + const int m = 30; + const float resolution = 0.1; + Matrix map = Matrix::Zero(n, m); + map(n / 2, m / 2) = 1.0; + const auto occupancy = occupancyAtHeight(map, 0.5); + + const auto naiveSignedDistance = naiveSignedDistanceFromOccupancy(occupancy, resolution); + const auto signedDistance = signedDistanceFromOccupancy(occupancy, resolution); + ASSERT_TRUE(isEqualSdf(signedDistance, naiveSignedDistance, 1e-4)); +} + +TEST(testSignedDistance2d, signedDistance2d_oneFreeSpace) { + const int n = 20; + const int m = 30; + const float resolution = 0.1; + Matrix map = Matrix::Ones(n, m); + map(n / 2, m / 2) = 0.0; + + const auto occupancy = occupancyAtHeight(map, 0.5); + + const auto naiveSignedDistance = naiveSignedDistanceFromOccupancy(occupancy, resolution); + const auto signedDistance = signedDistanceFromOccupancy(occupancy, resolution); + ASSERT_TRUE(isEqualSdf(signedDistance, naiveSignedDistance, 1e-4)); +} + +TEST(testSignedDistance2d, signedDistance2d_debugcase) { + const int n = 3; + const int m = 3; + const float resolution = 1.0; + Matrix map(n, m); + map << 1.0, 1.0, 1.0, 0.0, 1.0, 1.0, 1.0, 1.0, 0.0; + + const auto occupancy = occupancyAtHeight(map, 0.5); + + const auto naiveSignedDistance = naiveSignedDistanceFromOccupancy(occupancy, resolution); + const auto signedDistance = signedDistanceFromOccupancy(occupancy, resolution); + ASSERT_TRUE(isEqualSdf(signedDistance, naiveSignedDistance, 1e-4)); +} + +TEST(testSignedDistance2d, signedDistance2d_random) { + const int n = 20; + const int m = 30; + const float resolution = 1.0; + Matrix map = Matrix::Random(n, m); // random [-1.0, 1.0] + + // Check at different heights, resulting in different levels of sparsity. + float heightStep = 0.1; + for (float height = -1.0 - heightStep; height < 1.0 + heightStep; height += heightStep) { + const auto occupancy = occupancyAtHeight(map, height); + + const auto naiveSignedDistance = naiveSignedDistanceFromOccupancy(occupancy, resolution); + const auto signedDistance = signedDistanceFromOccupancy(occupancy, resolution); + ASSERT_TRUE(isEqualSdf(signedDistance, naiveSignedDistance, 1e-4)) << "height: " << height; + } +} \ No newline at end of file diff --git a/grid_map_sdf/test/testSignedDistance3d.cpp b/grid_map_sdf/test/testSignedDistance3d.cpp new file mode 100644 index 000000000..49219e890 --- /dev/null +++ b/grid_map_sdf/test/testSignedDistance3d.cpp @@ -0,0 +1,193 @@ +/* + * testSignedDistance3d.cpp + * + * Created on: Aug 10, 2020 + * Author: Ruben Grandia + * Institute: ETH Zurich + */ + +#include + +#include "grid_map_sdf/PixelBorderDistance.hpp" +#include "grid_map_sdf/SignedDistance2d.hpp" +#include "grid_map_sdf/SignedDistanceField.hpp" + +#include "naiveSignedDistance.hpp" + +using namespace grid_map; +using namespace signed_distance_field; + +TEST(testSignedDistance3d, flatTerrain) { + const int n = 3; + const int m = 4; + const float resolution = 0.1; + const float terrainHeight = 0.5; + const Matrix map = Matrix::Constant(n, m, terrainHeight); + const float minHeight = map.minCoeff(); + const float maxHeight = map.maxCoeff(); + + const float testHeightAboveTerrain = 3.0; + const auto naiveSignedDistanceAbove = naiveSignedDistanceAtHeight(map, testHeightAboveTerrain, resolution); + const auto signedDistanceAbove = signedDistanceAtHeight(map, testHeightAboveTerrain, resolution, minHeight, maxHeight); + ASSERT_TRUE(isEqualSdf(signedDistanceAbove, naiveSignedDistanceAbove, 1e-4)); + + const float testHeightBelowTerrain = -3.0; + const auto naiveSignedDistanceBelow = naiveSignedDistanceAtHeight(map, testHeightBelowTerrain, resolution); + const auto signedDistanceBelow = signedDistanceAtHeight(map, testHeightBelowTerrain, resolution, minHeight, maxHeight); + ASSERT_TRUE(isEqualSdf(signedDistanceBelow, naiveSignedDistanceBelow, 1e-4)); +} + +TEST(testSignedDistance3d, randomTerrain) { + const int n = 20; + const int m = 30; + const float resolution = 0.1; + Matrix map = Matrix::Random(n, m); // random [-1.0, 1.0] + const float minHeight = map.minCoeff(); + const float maxHeight = map.maxCoeff(); + + // Check at different heights, resulting in different levels of sparsity. + float heightStep = 0.1; + for (float height = -1.0 - heightStep; height < 1.0 + heightStep; height += heightStep) { + const auto naiveSignedDistance = naiveSignedDistanceAtHeight(map, height, resolution); + const auto signedDistance = signedDistanceAtHeight(map, height, resolution, minHeight, maxHeight); + + ASSERT_TRUE(isEqualSdf(signedDistance, naiveSignedDistance, 1e-4)) << "height: " << height; + } +} + +TEST(testSignedDistance3d, randomTerrainInterpolation) { + const int n = 20; + const int m = 30; + const float resolution = 0.1; + GridMap map; + map.setGeometry({n * resolution, m * resolution}, resolution); + map.add("elevation"); + map.get("elevation").setRandom(); // random [-1.0, 1.0] + const Matrix mapData = map.get("elevation"); + const float minHeight = mapData.minCoeff(); + const float maxHeight = mapData.maxCoeff(); + + SignedDistanceField sdf(map, "elevation", minHeight, maxHeight); + + // Check at different heights/ + for (float height = minHeight; height < maxHeight; height += resolution) { + const auto naiveSignedDistance = naiveSignedDistanceAtHeight(mapData, height, resolution); + + for (int i = 0; i < mapData.rows(); ++i) { + for (int j = 0; j < mapData.rows(); ++j) { + Position position2d; + map.getPosition({i, j}, position2d); + + const auto sdfValue = sdf.value({position2d.x(), position2d.y(), height}); + const auto sdfCheck = naiveSignedDistance(i, j); + ASSERT_LT(std::abs(sdfValue - sdfCheck), 1e-4); + } + } + } +} + +TEST(testSignedDistance3d, randomTerrainDerivative) { + const int n = 10; + const int m = 20; + const float resolution = 0.1; + GridMap map; + map.setGeometry({n * resolution, m * resolution}, resolution); + map.add("elevation"); + map.get("elevation").setRandom(); // random [-1.0, 1.0] + const Matrix mapData = map.get("elevation"); + const float minHeight = mapData.minCoeff(); + const float maxHeight = mapData.maxCoeff(); + + SignedDistanceField sdf(map, "elevation", minHeight, maxHeight); + + // Check at different heights/ + int numLayers = (maxHeight - minHeight) / resolution; + for (int k = 0; k <= numLayers; ++k) { + const float height = minHeight + k * resolution; + const auto naiveSignedDistance = naiveSignedDistanceAtHeight(mapData, height, resolution); + const auto naiveSignedDistanceNext = naiveSignedDistanceAtHeight(mapData, height + resolution, resolution); + const auto naiveSignedDistancePrevious = naiveSignedDistanceAtHeight(mapData, height - resolution, resolution); + + for (int i = 0; i < mapData.rows(); ++i) { + for (int j = 0; j < mapData.rows(); ++j) { + Position position2d; + map.getPosition({i, j}, position2d); + const auto sdfderivative = sdf.valueAndDerivative(Position3{position2d.x(), position2d.y(), height}); + const auto sdfCheck = naiveSignedDistance(i, j); + ASSERT_LT(std::abs(sdfderivative.first - sdfCheck), 1e-4); + + // Check finite difference + float dx = 0.0; + if (i > 0) { + if (i + 1 < mapData.rows()) { + dx = (naiveSignedDistance(i + 1, j) - naiveSignedDistance(i - 1, j)) / (-2.0 * resolution); + } else { + dx = (naiveSignedDistance(i, j) - naiveSignedDistance(i - 1, j)) / (-resolution); + } + } else { + dx = (naiveSignedDistance(i + 1, j) - naiveSignedDistance(i, j)) / (-resolution); + } + ASSERT_LT(std::abs(dx - sdfderivative.second.x()), 1e-4); + + float dy = 0.0; + if (j > 0) { + if (j + 1 < mapData.cols()) { + dy = (naiveSignedDistance(i, j + 1) - naiveSignedDistance(i, j - 1)) / (-2.0 * resolution); + } else { + dy = (naiveSignedDistance(i, j) - naiveSignedDistance(i, j - 1)) / (-resolution); + } + } else { + dy = (naiveSignedDistance(i, j + 1) - naiveSignedDistance(i, j)) / (-resolution); + } + ASSERT_LT(std::abs(dy - sdfderivative.second.y()), 1e-4); + + float dz = 0.0; + if (k > 0) { + if (k < numLayers) { + dz = (naiveSignedDistanceNext(i, j) - naiveSignedDistancePrevious(i, j)) / (2.0 * resolution); + } else { + dz = (naiveSignedDistance(i, j) - naiveSignedDistancePrevious(i, j)) / (resolution); + } + } else { + dz = (naiveSignedDistanceNext(i, j) - naiveSignedDistance(i, j)) / (resolution); + } + ASSERT_LT(std::abs(dz - sdfderivative.second.z()), 1e-4); + } + } + } +} + +TEST(testSignedDistance3d, extrapolation) { + const int n = 20; + const int m = 30; + const float resolution = 0.1; + const float h = 0.5; + GridMap map; + map.setGeometry({n * resolution, m * resolution}, resolution); + map.add("elevation"); + map.get("elevation").setConstant(h); // random [-1.0, 1.0] + const Matrix mapData = map.get("elevation"); + const float minHeight = h - resolution; + const float maxHeight = h + resolution; + + SignedDistanceField sdf(map, "elevation", minHeight, maxHeight); + + // Check at different heights/ + for (float height = h - 1.0; height < h + 1.0; height += resolution) { + const auto naiveSignedDistance = naiveSignedDistanceAtHeight(mapData, height, resolution); + + for (int i = 0; i < mapData.rows(); ++i) { + for (int j = 0; j < mapData.rows(); ++j) { + Position position2d; + map.getPosition({i, j}, position2d); + + const auto sdfValueAndDerivative = sdf.valueAndDerivative({position2d.x(), position2d.y(), height}); + const auto sdfCheck = naiveSignedDistance(i, j); + ASSERT_LT(std::abs(sdfValueAndDerivative.first - sdfCheck), 1e-4); + + // Constant terrain, derivative should be up everywhere + ASSERT_LT((sdfValueAndDerivative.second - SignedDistanceField::Derivative3::UnitZ()).norm(), 1e-4); + } + } + } +} \ No newline at end of file diff --git a/jenkins-pipeline b/jenkins-pipeline new file mode 100644 index 000000000..59d9ff9be --- /dev/null +++ b/jenkins-pipeline @@ -0,0 +1,2 @@ +library 'continuous_integration_pipeline' +ciPipeline("")