├── .gitignore ├── LICENSE ├── README.md ├── elevation_mapping ├── .gitignore ├── CMakeLists.txt ├── config │ └── sensor_processors │ │ ├── aslam.yaml │ │ ├── fotonic_g45.yaml │ │ ├── hokuyo_utm-30lx.yaml │ │ ├── kinect_nguyen_et_al.yaml │ │ ├── perfect.yaml │ │ ├── primesense_carmine_109_short_range_datasheet.yaml │ │ ├── primesense_carmine_109_short_range_uncalibrated.yaml │ │ ├── realsense_ZR300.yaml │ │ ├── realsense_d435.yaml │ │ └── velodyne_HDL-32E.yaml ├── include │ └── elevation_mapping │ │ ├── ElevationMap.hpp │ │ ├── ElevationMapFunctors.hpp │ │ ├── ElevationMapping.hpp │ │ ├── PointXYZRGBConfidenceRatio.hpp │ │ ├── RobotMotionMapUpdater.hpp │ │ ├── WeightedEmpiricalCumulativeDistributionFunction.hpp │ │ ├── input_sources │ │ ├── Input.hpp │ │ └── InputSourceManager.hpp │ │ ├── postprocessing │ │ ├── PostprocessingPipelineFunctor.hpp │ │ ├── PostprocessingWorker.hpp │ │ └── PostprocessorPool.hpp │ │ └── sensor_processors │ │ ├── LaserSensorProcessor.hpp │ │ ├── PerfectSensorProcessor.hpp │ │ ├── SensorProcessorBase.hpp │ │ ├── StereoSensorProcessor.hpp │ │ └── StructuredLightSensorProcessor.hpp ├── package.xml ├── src │ ├── ElevationMap.cpp │ ├── ElevationMapping.cpp │ ├── PointXYZRGBConfidenceRatio.cpp │ ├── RobotMotionMapUpdater.cpp │ ├── elevation_mapping_node.cpp │ ├── input_sources │ │ ├── Input.cpp │ │ └── InputSourceManager.cpp │ ├── postprocessing │ │ ├── PostprocessingPipelineFunctor.cpp │ │ ├── PostprocessingWorker.cpp │ │ └── PostprocessorPool.cpp │ └── sensor_processors │ │ ├── LaserSensorProcessor.cpp │ │ ├── PerfectSensorProcessor.cpp │ │ ├── SensorProcessorBase.cpp │ │ ├── StereoSensorProcessor.cpp │ │ └── StructuredLightSensorProcessor.cpp └── test │ ├── ElevationMapTest.cpp │ ├── WeightedEmpiricalCumulativeDistributionFunctionTest.cpp │ ├── elevation_mapping.test │ ├── input_sources │ ├── InputSourcesTest.cpp │ ├── config │ │ ├── TestConfigurations.yaml │ │ └── rosconsole.config │ ├── input_sources.test │ └── test_input_sources.cpp │ ├── postprocessing │ ├── PostprocessorTest.cpp │ ├── main.cpp │ ├── postprocessor.test │ └── postprocessor_pipeline.yaml │ └── test_elevation_mapping.cpp ├── elevation_mapping_demos ├── .pydevproject ├── CMakeLists.txt ├── config │ ├── elevation_maps │ │ ├── long_range.yaml │ │ ├── remove_object.yaml │ │ ├── simple_demo_map.yaml │ │ └── starleth_map.yaml │ ├── postprocessing │ │ └── postprocessor_pipeline.yaml │ ├── robots │ │ ├── ground_truth_demo.yaml │ │ ├── remove_object.yaml │ │ ├── simple_demo_robot.yaml │ │ ├── starleth.yaml │ │ └── waffle_robot.yaml │ ├── rosconsole.conf │ └── visualization │ │ ├── fused.yaml │ │ └── raw.yaml ├── doc │ ├── anymal_forrest.jpg │ ├── anymal_locomotion_planner.jpg │ ├── anymal_outdoor_stairs.jpg │ ├── elevation_map.jpg │ └── starleth_kinect.jpg ├── launch │ ├── ground_truth_demo.launch │ ├── realsense_demo.launch │ ├── simple_demo.launch │ ├── starleth_kinect_demo.launch │ ├── turtlesim3_waffle_demo.launch │ └── visualization.launch ├── package.xml ├── rviz │ ├── elevation_map_visualization.rviz │ └── turtlebot3_waffle_demo.rviz ├── scripts │ └── tf_to_pose_publisher.py └── test │ └── empty_test.cpp └── jenkins-pipeline /.gitignore: -------------------------------------------------------------------------------- 1 | # Compiled Object files 2 | *.slo 3 | *.lo 4 | *.o 5 | *.obj 6 | 7 | # Compiled Dynamic libraries 8 | *.so 9 | *.dylib 10 | *.dll 11 | 12 | # Compiled Static libraries 13 | *.lai 14 | *.la 15 | *.a 16 | *.lib 17 | 18 | # Executables 19 | *.exe 20 | *.out 21 | *.app 22 | 23 | # Eclipse 24 | .cproject 25 | .project 26 | .settings 27 | 28 | # Sublime 29 | *.sublime* 30 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | Copyright 2019, ANYbotics AG. 2 | 3 | Redistribution and use in source and binary forms, with or without 4 | modification, are permitted provided that the following conditions 5 | are met: 6 | 7 | 1. Redistributions of source code must retain the above copyright 8 | notice, this list of conditions and the following disclaimer. 9 | 10 | 2. Redistributions in binary form must reproduce the above copyright 11 | notice, this list of conditions and the following disclaimer in 12 | the documentation and/or other materials provided with the 13 | distribution. 14 | 15 | 3. Neither the name of the copyright holder nor the names of its 16 | contributors may be used to endorse or promote products derived 17 | from this software without specific prior written permission. 18 | 19 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 20 | "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 21 | LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR 22 | A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 23 | HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 24 | SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT 25 | LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, 26 | DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY 27 | THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 28 | (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 29 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 30 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Robot-Centric Elevation Mapping 2 | 3 | ## Overview 4 | 5 | This is a [ROS] package developed for elevation mapping with a mobile robot. The software is designed for (local) navigation tasks with robots which are equipped with a pose estimation (e.g. IMU & odometry) and a distance sensor (e.g. structured light (Kinect, RealSense), laser range sensor, stereo camera). The provided elevation map is limited around the robot and reflects the pose uncertainty that is aggregated through the motion of the robot (robot-centric mapping). This method is developed to explicitly handle drift of the robot pose estimation. 6 | 7 | This is research code, expect that it changes often and any fitness for a particular purpose is disclaimed. 8 | 9 | The source code is released under a [BSD 3-Clause license](LICENSE). 10 | 11 | **Author: Péter Fankhauser
12 | Co-Author: Maximilian Wulf
13 | Affiliation: [ANYbotics](https://www.anybotics.com/)
14 | Maintainer: Maximilian Wulf, mwulf@anybotics.com
** 15 | 16 | This projected was initially developed at ETH Zurich (Autonomous Systems Lab & Robotic Systems Lab). 17 | 18 | [This work is conducted as part of ANYmal Research, a community to advance legged robotics.](https://www.anymal-research.org/) 19 | 20 | Elevation Map Example 21 | 22 | 23 | Videos of the elevation mapping software in use: 24 | 25 | 26 | 27 | 28 | 29 | 30 | ## Citing 31 | 32 | The robot-centric elevation mapping methods used in this software are described in the following paper (available [here](https://doi.org/10.3929/ethz-b-000272110)). If you use this work in an academic context, please cite the following publication(s): 33 | 34 | * > P. Fankhauser, M. Bloesch, and M. Hutter, 35 | > **"Probabilistic Terrain Mapping for Mobile Robots with Uncertain Localization"**, 36 | > in IEEE Robotics and Automation Letters (RA-L), vol. 3, no. 4, pp. 3019–3026, 2018. ([PDF](http://dx.doi.org/10.1109/LRA.2018.2849506)) 37 | 38 | @article{Fankhauser2018ProbabilisticTerrainMapping, 39 | author = {Fankhauser, P{\'{e}}ter and Bloesch, Michael and Hutter, Marco}, 40 | doi = {10.1109/LRA.2018.2849506}, 41 | title = {Probabilistic Terrain Mapping for Mobile Robots with Uncertain Localization}, 42 | journal = {IEEE Robotics and Automation Letters (RA-L)}, 43 | volume = {3}, 44 | number = {4}, 45 | pages = {3019--3026}, 46 | year = {2018} 47 | } 48 | 49 | * > P. Fankhauser, M. Bloesch, C. Gehring, M. Hutter, and R. Siegwart, 50 | > **"Robot-Centric Elevation Mapping with Uncertainty Estimates"**, 51 | > in International Conference on Climbing and Walking Robots (CLAWAR), 2014. ([PDF](http://dx.doi.org/10.3929/ethz-a-010173654)) 52 | 53 | @inproceedings{Fankhauser2014RobotCentricElevationMapping, 54 | author = {Fankhauser, P\'{e}ter and Bloesch, Michael and Gehring, Christian and Hutter, Marco and Siegwart, Roland}, 55 | title = {Robot-Centric Elevation Mapping with Uncertainty Estimates}, 56 | booktitle = {International Conference on Climbing and Walking Robots (CLAWAR)}, 57 | year = {2014} 58 | } 59 | 60 | ## Installation 61 | 62 | ### Dependencies 63 | 64 | This software is built on the Robotic Operating System ([ROS]), which needs to be [installed](http://wiki.ros.org) first. Additionally, the Robot-Centric Elevation Mapping depends on following software: 65 | 66 | - [Grid Map](https://github.com/anybotics/grid_map) (grid map library for mobile robots) 67 | - [kindr](http://github.com/anybotics/kindr) (kinematics and dynamics library for robotics), 68 | - [kindr_ros](https://github.com/anybotics/kindr_ros) (ROS wrapper for kindr), 69 | - [Point Cloud Library (PCL)](http://pointclouds.org/) (point cloud processing), 70 | - [Eigen](http://eigen.tuxfamily.org) (linear algebra library). 71 | 72 | 73 | ### Building 74 | 75 | In order to install the Robot-Centric Elevation Mapping, clone the latest version from this repository into your catkin workspace and compile the package using ROS. 76 | 77 | cd catkin_workspace/src 78 | git clone https://github.com/anybotics/elevation_mapping.git 79 | cd ../ 80 | catkin config --cmake-args -DCMAKE_BUILD_TYPE=Release 81 | catkin build 82 | 83 | 84 | ### Unit Tests 85 | 86 | Build tests with 87 | 88 | roscd elevation_mapping 89 | catkin build --catkin-make-args run_tests -- --this 90 | 91 | Run the tests with 92 | 93 | rostest elevation_mapping elevation_mapping.test -t 94 | 95 | 96 | ## Basic Usage 97 | 98 | In order to get the Robot-Centric Elevation Mapping to run with your robot, you will need to adapt a few parameters. It is the easiest if duplicate and adapt all the parameter files that you need to change from the `elevation_mapping_demos` package (e.g. the `simple_demo` example). These are specifically the parameter files in `config` and the launch file from the `launch` folder. 99 | 100 | ### TurtleBot3 Waffle Simulation 101 | 102 | A running example is provided, making use of the Turtlebot3 simulation environment. This example can be used to test elevation mapping, as a starting point for further integration. 103 | 104 | To start with, the Turtlebot3 simulation dependencies need to be installed: 105 | 106 | sudo apt install ros-melodic-turtlebot3* 107 | 108 | The elevation mapping demo together with the turtlebot3 simulation can be started with 109 | 110 | roslaunch elevation_mapping_demos turtlesim3_waffle_demo.launch 111 | 112 | To control the robot with a keyboard, a new terminal window needs to be opened (remember to source your ROS environment). Then run 113 | 114 | export TURTLEBOT3_MODEL=waffle 115 | roslaunch turtlebot3_teleop turtlebot3_teleop_key.launch 116 | 117 | Velocity inputs can be sent to the robot by pressing the keys `a`, `w`,`d`, `x`. To stop the robot completely, press `s`. 118 | 119 | ## Nodes 120 | 121 | ### Node: elevation_mapping 122 | 123 | This is the main Robot-Centric Elevation Mapping node. It uses the distance sensor measurements and the pose and covariance of the robot to generate an elevation map with variance estimates. 124 | 125 | 126 | #### Subscribed Topics 127 | 128 | * **`/points`** ([sensor_msgs/PointCloud2]) 129 | 130 | The distance measurements. 131 | 132 | * **`/pose`** ([geometry_msgs/PoseWithCovarianceStamped]) 133 | 134 | The robot pose and covariance. 135 | 136 | * **`/tf`** ([tf/tfMessage]) 137 | 138 | The transformation tree. 139 | 140 | 141 | #### Published Topics 142 | 143 | * **`elevation_map`** ([grid_map_msgs/GridMap]) 144 | 145 | The entire (fused) elevation map. It is published periodically (see `fused_map_publishing_rate` parameter) or after the `trigger_fusion` service is called. 146 | 147 | * **`elevation_map_raw`** ([grid_map_msgs/GridMap]) 148 | 149 | The entire (raw) elevation map before the fusion step. 150 | 151 | 152 | #### Services 153 | 154 | * **`trigger_fusion`** ([std_srvs/Empty]) 155 | 156 | Trigger the fusing process for the entire elevation map and publish it. For example, you can trigger the map fusion step from the console with 157 | 158 | rosservice call /elevation_mapping/trigger_fusion 159 | 160 | * **`get_submap`** ([grid_map_msgs/GetGridMap]) 161 | 162 | Get a fused elevation submap for a requested position and size. For example, you can get the fused elevation submap at position (-0.5, 0.0) and size (0.5, 1.2) described in the odom frame and save it to a text file form the console with 163 | 164 | rosservice call -- /elevation_mapping/get_submap odom -0.5 0.0 0.5 1.2 [] 165 | 166 | * **`get_raw_submap`** ([grid_map_msgs/GetGridMap]) 167 | 168 | Get a raw elevation submap for a requested position and size. For example, you can get the raw elevation submap at position (-0.5, 0.0) and size (0.5, 1.2) described in the odom frame and save it to a text file form the console with 169 | 170 | rosservice call -- /elevation_mapping/get_raw_submap odom -0.5 0.0 0.5 1.2 [] 171 | 172 | * **`clear_map`** ([std_srvs/Empty]) 173 | 174 | Initiates clearing of the entire map for resetting purposes. Trigger the map clearing with 175 | 176 | rosservice call /elevation_mapping/clear_map 177 | 178 | * **`masked_replace`** ([grid_map_msgs/SetGridMap]) 179 | 180 | Allows for setting the individual layers of the elevation map through a service call. The layer mask can be used to only set certain cells and not the entire map. Cells containing NAN in the mask are not set, all the others are set. If the layer mask is not supplied, the entire map will be set in the intersection of both maps. The provided map can be of different size and position than the map that will be altered. An example service call to set some cells marked with a mask in the elevation layer to 0.5 is 181 | 182 | rosservice call /elevation_mapping/masked_replace "map: 183 | info: 184 | header: 185 | seq: 3 186 | stamp: {secs: 3, nsecs: 80000000} 187 | frame_id: 'odom' 188 | resolution: 0.1 189 | length_x: 0.3 190 | length_y: 0.3 191 | pose: 192 | position: {x: 5.0, y: 0.0, z: 0.0} 193 | orientation: {x: 0.0, y: 0.0, z: 0.0, w: 0.0} 194 | layers: [elevation,mask] 195 | basic_layers: [elevation] 196 | data: 197 | - layout: 198 | dim: 199 | - {label: 'column_index', size: 3, stride: 9} 200 | - {label: 'row_index', size: 3, stride: 3} 201 | data_offset: 0 202 | data: [0.5, 0.5, 0.5, 0.5, 0.5, 0.5, 0.5, 0.5, 0.5] 203 | - layout: 204 | dim: 205 | - {label: 'column_index', size: 3, stride: 9} 206 | - {label: 'row_index', size: 3, stride: 3} 207 | data_offset: 0 208 | data: [0, 0, 0, .NAN, .NAN, .NAN, 0, 0, 0] 209 | outer_start_index: 0 210 | inner_start_index: 0" 211 | 212 | * **`save_map`** ([grid_map_msgs/ProcessFile]) 213 | 214 | Saves the current fused grid map and raw grid map to rosbag files. Field `topic_name` must be a base name, i.e. no leading slash character (/). If field `topic_name` is empty, then `elevation_map` is used per default. Example with default topic name 215 | 216 | rosservice call /elevation_mapping/save_map "file_path: '/home/integration/elevation_map.bag' topic_name: ''" 217 | 218 | * **`load_map`** ([grid_map_msgs/ProcessFile]) 219 | 220 | Loads the fused grid map and raw grid map from rosbag files. Field `topic_name` must be a base name, i.e. no leading slash character (/). If field `topic_name` is empty, then `elevation_map` is used per default. Example with default topic name 221 | 222 | rosservice call /elevation_mapping/load_map "file_path: '/home/integration/elevation_map.bag' topic_name: ''" 223 | 224 | * **`disable_updates`** ([std_srvs/Empty]) 225 | 226 | Stops updating the elevation map with sensor input. Trigger the update stopping with 227 | 228 | rosservice call /elevation_mapping/disable_updates {} 229 | 230 | * **`enable_updates`** ([std_srvs/Empty]) 231 | 232 | Start updating the elevation map with sensor input. Trigger the update starting with 233 | 234 | rosservice call /elevation_mapping/enable_updates {} 235 | 236 | #### Parameters 237 | 238 | * **`DEPRECATED point_cloud_topic`** (string, default: "/points") 239 | 240 | The name of the distance measurements topic. Use input_sources instead. 241 | 242 | * **`input_sources`** (list of input sources, default: none) 243 | 244 | Here you specify your inputs to elevation mapping, currently "pointcloud" inputs are supported. 245 | 246 | Example configuration: 247 | ```yaml 248 | input_sources: 249 | front: # A name to identify the input source 250 | type: pointcloud # Supported types: pointcloud 251 | topic: /lidar_front/depth/points 252 | queue_size: 1 253 | publish_on_update: true # Wheter to publish the elevation map after a callback from this source. 254 | rear: 255 | type: pointcloud 256 | topic: /lidar_rear/depth/points 257 | queue_size: 5 258 | publish_on_update: false 259 | ``` 260 | No input sources can be configured with an empty array: 261 | ```yaml 262 | input_sources: [] 263 | ``` 264 | * **`robot_pose_topic`** (string, default: "/robot_state/pose") 265 | 266 | The name of the robot pose and covariance topic. 267 | 268 | * **`base_frame_id`** (string, default: "/robot") 269 | 270 | The id of the robot base tf frame. 271 | 272 | * **`map_frame_id`** (string, default: "/map") 273 | 274 | The id of the tf frame of the elevation map. 275 | 276 | * **`track_point_frame_id`** (string, default: "/robot") 277 | 278 | The elevation map is moved along with the robot following a *track point*. This is the id of the tf frame in which the track point is defined. 279 | 280 | * **`track_point_x`**, **`track_point_y`**, **`track_point_z`** (double, default: 0.0, 0.0, 0.0) 281 | 282 | The elevation map is moved along with the robot following a *track point*. This is the position of the track point in the `track_point_frame_id`. 283 | 284 | * **`robot_pose_cache_size`** (int, default: 200, min: 0) 285 | 286 | The size of the robot pose cache. 287 | 288 | * **`min_update_rate`** (double, default: 2.0) 289 | 290 | The mininum update rate (in Hz) at which the elevation map is updated either from new measurements or the robot pose estimates. 291 | 292 | * **`fused_map_publishing_rate`** (double, default: 1.0) 293 | 294 | The rate for publishing the entire (fused) elevation map. 295 | 296 | * **`relocate_rate`** (double, default: 3.0) 297 | 298 | The rate (in Hz) at which the elevation map is checked for relocation following the tracking point. 299 | 300 | * **`length_in_x`**, **`length_in_y`** (double, default: 1.5, min: 0.0) 301 | 302 | The size (in m) of the elevation map. 303 | 304 | * **`position_x`**, **`position_y`** (double, default: 0.0) 305 | 306 | The position of the elevation map center, in the elevation map frame. This parameter sets the planar position offsets between the generated elevation map and the frame in which it is published (`map_frame_id`). It is only useful if no `track_point_frame_id` parameter is used. 307 | 308 | * **`resolution`** (double, default: 0.01, min: 0.0) 309 | 310 | The resolution (cell size in m/cell) of the elevation map. 311 | 312 | * **`min_variance`**, **`max_variance`** (double, default: 9.0e-6, 0.01) 313 | 314 | The minimum and maximum values for the elevation map variance data. 315 | 316 | * **`mahalanobis_distance_threshold`** (double, default: 2.5) 317 | 318 | Each cell in the elevation map has an uncertainty for its height value. Depending on the Mahalonobis distance of the existing height distribution and the new measurements, the incoming data is fused with the existing estimate, overwritten, or ignored. This parameter determines the threshold on the Mahalanobis distance which determines how the incoming measurements are processed. 319 | 320 | * **`sensor_processor/ignore_points_above`** (double, default: inf) 321 | A hard threshold on the height of points introduced by the depth sensor. Points with a height over this threshold will not be considered valid during the data collection step. 322 | 323 | * **`sensor_processor/ignore_points_below`** (double, default: -inf) 324 | A hard threshold on the height of points introduced by the depth sensor. Points with a height below this threshold will not be considered valid during the data collection step. 325 | 326 | * **`multi_height_noise`** (double, default: 9.0e-7) 327 | 328 | Noise added to measurements that are higher than the current elevation map at that particular position. This noise-adding process is only performed if a point falls over the Mahalanobis distance threshold. A higher value is useful to adapt faster to dynamic environments (e.g., moving objects), but might cause more noise in the height estimation. 329 | 330 | * **`min_horizontal_variance`**, **`max_horizontal_variance`** (double, default: pow(resolution / 2.0, 2), 0.5) 331 | 332 | The minimum and maximum values for the elevation map horizontal variance data. 333 | 334 | * **`enable_visibility_cleanup`** (bool, default: true) 335 | 336 | Enable/disable a separate thread that removes elements from the map which are not visible anymore, by means of ray-tracing, originating from the sensor frame. 337 | 338 | * **`visibility_cleanup_rate`** (double, default: 1.0) 339 | 340 | The rate (in Hz) at which the visibility clean-up is performed. 341 | 342 | * **`enable_continuous_cleanup`** (bool, default: false) 343 | 344 | Enable/disable a continuous clean-up of the elevation map. If enabled, on arrival of each new sensor data the elevation map will be cleared and filled up only with the latest data from the sensor. When continuous clean-up is enabled, visibility clean-up will automatically be disabled since it is not needed in this case. 345 | 346 | * **`num_callback_threads`** (int, default: 1, min: 1) 347 | The number of threads to use for processing callbacks. More threads results in higher throughput, at cost of more resource usage. 348 | 349 | * **`postprocessor_pipeline_name`** (string, default: postprocessor_pipeline) 350 | 351 | The name of the pipeline to execute for postprocessing. It expects a pipeline configuration to be loaded in the private namespace of the node under this name. 352 | E.g.: 353 | ``` 354 | 355 | ... 356 | 357 | 358 | ``` 359 | A pipeline is a grid_map_filter chain, see grid_map_demos/filters_demo.yaml and [ros / filters](http://wiki.ros.org/filters) for more information. 360 | 361 | * **`postprocessor_num_threads`** (int, default: 1, min: 1) 362 | 363 | The number of threads to use for asynchronous postprocessing. More threads results in higher throughput, at cost of more resource usage. 364 | 365 | * **`scanning_duration`** (double, default: 1.0) 366 | 367 | The sensor's scanning duration (in s) which is used for the visibility cleanup. Set this roughly to the duration it takes between two consecutive full scans (e.g. 0.033 for a ToF camera with 30 Hz, or 3 s for a rotating laser scanner). Depending on how dense or sparse your scans are, increase or reduce the scanning duration. Smaller values lead to faster dynamic object removal and bigger values help to reduce faulty map cleanups. 368 | 369 | * **`sensor_cutoff_min_depth`**, **`sensor_cutoff_max_depth`** (double, default: 0.2, 2.0) 370 | 371 | The minimum and maximum values for the length of the distance sensor measurements. Measurements outside this interval are ignored. 372 | 373 | * **`sensor_model_normal_factor_a`**, **`sensor_model_normal_factor_b`**, **`sensor_model_normal_factor_c`**, **`sensor_model_lateral_factor`** (double) 374 | 375 | The data for the sensor noise model. 376 | 377 | ## Changelog 378 | 379 | See [Changelog] 380 | 381 | ## Bugs & Feature Requests 382 | 383 | Please report bugs and request features using the [Issue Tracker](https://github.com/anybotics/elevation_mapping/issues). 384 | 385 | [Changelog]: CHANGELOG.rst 386 | [ROS]: http://www.ros.org 387 | [rviz]: http://wiki.ros.org/rviz 388 | [grid_map_msgs/GridMap]: https://github.com/anybotics/grid_map/blob/master/grid_map_msgs/msg/GridMap.msg 389 | [sensor_msgs/PointCloud2]: http://docs.ros.org/api/sensor_msgs/html/msg/PointCloud2.html 390 | [geometry_msgs/PoseWithCovarianceStamped]: http://docs.ros.org/api/geometry_msgs/html/msg/PoseWithCovarianceStamped.html 391 | [tf/tfMessage]: http://docs.ros.org/kinetic/api/tf/html/msg/tfMessage.html 392 | [std_srvs/Empty]: http://docs.ros.org/api/std_srvs/html/srv/Empty.html 393 | [grid_map_msgs/GetGridMap]: https://github.com/anybotics/grid_map/blob/master/grid_map_msgs/srv/GetGridMap.srv 394 | [grid_map_msgs/ProcessFile]: https://github.com/ANYbotics/grid_map/blob/master/grid_map_msgs/srv/ProcessFile.srv 395 | -------------------------------------------------------------------------------- /elevation_mapping/.gitignore: -------------------------------------------------------------------------------- 1 | *.pydevproject 2 | -------------------------------------------------------------------------------- /elevation_mapping/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5.1) 2 | project(elevation_mapping) 3 | 4 | set(CMAKE_CXX_STANDARD 14) 5 | add_compile_options(-Wall -Wextra -Wpedantic) 6 | 7 | set(CMAKE_EXPORT_COMPILE_COMMANDS ON) 8 | 9 | set( 10 | CATKIN_PACKAGE_DEPENDENCIES 11 | eigen_conversions 12 | grid_map_core 13 | grid_map_ros 14 | grid_map_msgs 15 | kindr 16 | kindr_ros 17 | message_filters 18 | pcl_ros 19 | roscpp 20 | sensor_msgs 21 | std_msgs 22 | tf 23 | tf_conversions 24 | ) 25 | 26 | find_package(catkin REQUIRED 27 | COMPONENTS 28 | ${CATKIN_PACKAGE_DEPENDENCIES} 29 | ) 30 | 31 | find_package(Boost REQUIRED COMPONENTS system) 32 | find_package(Eigen3 REQUIRED) 33 | 34 | catkin_package( 35 | INCLUDE_DIRS 36 | include 37 | ${Eigen_INCLUDE_DIRS} 38 | LIBRARIES 39 | ${PROJECT_NAME}_library 40 | CATKIN_DEPENDS 41 | ${CATKIN_PACKAGE_DEPENDENCIES} 42 | DEPENDS 43 | Boost 44 | ) 45 | 46 | include_directories( 47 | include 48 | SYSTEM 49 | ${Boost_INCLUDE_DIRS} 50 | ${catkin_INCLUDE_DIRS} 51 | ${Eigen_INCLUDE_DIRS} 52 | ) 53 | 54 | ########### 55 | # Library # 56 | ########### 57 | 58 | # Our custom pcl type with precompiled template instantiations. 59 | add_library(${PROJECT_NAME}_pcl_types 60 | src/PointXYZRGBConfidenceRatio.cpp 61 | ) 62 | target_link_libraries(${PROJECT_NAME}_pcl_types 63 | ${catkin_LIBRARIES} 64 | ) 65 | 66 | add_library(${PROJECT_NAME}_library 67 | src/ElevationMapping.cpp 68 | src/ElevationMap.cpp 69 | src/input_sources/Input.cpp 70 | src/input_sources/InputSourceManager.cpp 71 | src/postprocessing/PostprocessorPool.cpp 72 | src/postprocessing/PostprocessingWorker.cpp 73 | src/postprocessing/PostprocessingPipelineFunctor.cpp 74 | src/RobotMotionMapUpdater.cpp 75 | src/sensor_processors/SensorProcessorBase.cpp 76 | src/sensor_processors/StructuredLightSensorProcessor.cpp 77 | src/sensor_processors/StereoSensorProcessor.cpp 78 | src/sensor_processors/LaserSensorProcessor.cpp 79 | src/sensor_processors/PerfectSensorProcessor.cpp 80 | ) 81 | 82 | target_link_libraries(${PROJECT_NAME}_library 83 | ${catkin_LIBRARIES} 84 | ${PROJECT_NAME}_pcl_types 85 | pthread 86 | ) 87 | 88 | ############## 89 | # Executable # 90 | ############## 91 | 92 | add_executable(${PROJECT_NAME} 93 | src/elevation_mapping_node.cpp 94 | ) 95 | 96 | target_link_libraries(${PROJECT_NAME} 97 | ${PROJECT_NAME}_library 98 | ) 99 | 100 | ############# 101 | ## Install ## 102 | ############# 103 | 104 | install( 105 | TARGETS 106 | ${PROJECT_NAME} 107 | ${PROJECT_NAME}_pcl_types 108 | ${PROJECT_NAME}_library 109 | ARCHIVE DESTINATION 110 | ${CATKIN_PACKAGE_LIB_DESTINATION} 111 | LIBRARY DESTINATION 112 | ${CATKIN_PACKAGE_LIB_DESTINATION} 113 | RUNTIME DESTINATION 114 | ${CATKIN_PACKAGE_BIN_DESTINATION} 115 | ) 116 | 117 | install( 118 | DIRECTORY 119 | include/${PROJECT_NAME}/ 120 | DESTINATION 121 | ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 122 | ) 123 | 124 | install( 125 | DIRECTORY 126 | config 127 | DESTINATION 128 | ${CATKIN_PACKAGE_SHARE_DESTINATION} 129 | ) 130 | 131 | ########## 132 | ## Test ## 133 | ########## 134 | 135 | if(CATKIN_ENABLE_TESTING) 136 | find_package(catkin REQUIRED 137 | COMPONENTS 138 | ${CATKIN_PACKAGE_DEPENDENCIES} 139 | grid_map_filters 140 | roslaunch 141 | rostest 142 | ) 143 | 144 | # Cummulative distribution 145 | catkin_add_gtest(test_${PROJECT_NAME}_cumulative_distribution 146 | test/ElevationMapTest.cpp 147 | test/test_elevation_mapping.cpp 148 | test/WeightedEmpiricalCumulativeDistributionFunctionTest.cpp 149 | ) 150 | 151 | target_link_libraries(test_${PROJECT_NAME}_cumulative_distribution 152 | ${PROJECT_NAME}_library 153 | ) 154 | 155 | target_include_directories(test_${PROJECT_NAME}_cumulative_distribution 156 | PRIVATE 157 | include 158 | ) 159 | 160 | target_include_directories(test_${PROJECT_NAME}_cumulative_distribution 161 | SYSTEM PUBLIC 162 | ${catkin_INCLUDE_DIRS} 163 | ) 164 | 165 | # Input sources 166 | add_rostest_gtest(test_${PROJECT_NAME}_input_sources 167 | test/input_sources/input_sources.test 168 | test/input_sources/test_input_sources.cpp 169 | test/input_sources/InputSourcesTest.cpp 170 | ) 171 | 172 | target_link_libraries(test_${PROJECT_NAME}_input_sources 173 | ${PROJECT_NAME}_library 174 | ) 175 | 176 | target_include_directories(test_${PROJECT_NAME}_input_sources 177 | PRIVATE 178 | include 179 | ) 180 | 181 | target_include_directories(test_${PROJECT_NAME}_input_sources SYSTEM PUBLIC 182 | ${Boost_INCLUDE_DIRS} 183 | ${catkin_INCLUDE_DIRS} 184 | ${Eigen_INCLUDE_DIRS} 185 | ) 186 | 187 | # Postprocessor 188 | add_rostest_gtest(test_${PROJECT_NAME}_postprocessor 189 | test/postprocessing/postprocessor.test 190 | test/postprocessing/main.cpp 191 | test/postprocessing/PostprocessorTest.cpp 192 | ) 193 | 194 | target_include_directories(test_${PROJECT_NAME}_postprocessor PRIVATE 195 | include 196 | ) 197 | 198 | target_include_directories(test_${PROJECT_NAME}_postprocessor SYSTEM PUBLIC 199 | ${catkin_INCLUDE_DIRS} 200 | ) 201 | 202 | target_link_libraries(test_${PROJECT_NAME}_postprocessor 203 | ${GTEST_LIBRARIES} 204 | ${PROJECT_NAME}_library 205 | ${catkin_LIBRARIES} 206 | ) 207 | 208 | find_package(cmake_code_coverage QUIET) 209 | if(cmake_code_coverage_FOUND) 210 | add_gtest_coverage(TEST_BUILD_TARGETS 211 | test_${PROJECT_NAME}_cumulative_distribution 212 | ) 213 | add_rostest_coverage(TEST_BUILD_TARGETS 214 | test_${PROJECT_NAME}_input_sources 215 | ) 216 | add_rostest_coverage(TEST_BUILD_TARGETS 217 | test_${PROJECT_NAME}_postprocessor 218 | ) 219 | endif() 220 | endif() 221 | 222 | ########### 223 | ## Clang ## 224 | ########### 225 | 226 | find_package(cmake_clang_tools QUIET) 227 | if(cmake_clang_tools_FOUND) 228 | add_default_clang_tooling() 229 | endif(cmake_clang_tools_FOUND) 230 | -------------------------------------------------------------------------------- /elevation_mapping/config/sensor_processors/aslam.yaml: -------------------------------------------------------------------------------- 1 | # ASLAM Stereo camera: 2 | # "Localization and Path Planning of a Climbing Robot for Corrosion Monitoring", 3 | # Hannes Keller, Semester Project, ETH Zurich, 2014. 4 | 5 | sensor_processor/type: stereo 6 | sensor_processor/p_1: 0.03287 7 | sensor_processor/p_2: -0.0001276 8 | sensor_processor/p_3: 0.4850 9 | sensor_processor/p_4: 399.1046 10 | sensor_processor/p_5: 0.000006735 11 | sensor_processor/lateral_factor: 0.001376915 12 | sensor_processor/depth_to_disparity_factor: 47.3 13 | -------------------------------------------------------------------------------- /elevation_mapping/config/sensor_processors/fotonic_g45.yaml: -------------------------------------------------------------------------------- 1 | # Written by Lefteris kotsonis. 2 | # Modeling Fotonic G45 Sensor Noise for Improved 3D Reconstruction and Tracking. 3 | sensor_processor/type: structured_light 4 | sensor_processor/cutoff_min_depth: 0.5 5 | sensor_processor/cutoff_max_depth: 3.25 6 | sensor_processor/normal_factor_a: 0.0263 7 | sensor_processor/normal_factor_b: 0 8 | sensor_processor/normal_factor_c: 0 9 | sensor_processor/normal_factor_d: 0 10 | sensor_processor/normal_factor_e: 1 11 | sensor_processor/lateral_factor: 0.003163 12 | -------------------------------------------------------------------------------- /elevation_mapping/config/sensor_processors/hokuyo_utm-30lx.yaml: -------------------------------------------------------------------------------- 1 | # Hokuyo UTM-30LX laser scanner 2 | # Pomerleau, F.; Breitenmoser, A; Ming Liu; Colas, F.; Siegwart, R., 3 | # "Noise characterization of depth sensors for surface inspections," 4 | # International Conference on Applied Robotics for the Power Industry (CARPI), 2012. 5 | 6 | sensor_processor/type: laser 7 | sensor_processor/min_radius: 0.018 8 | sensor_processor/beam_angle: 0.0006 9 | sensor_processor/beam_constant: 0.0015 10 | -------------------------------------------------------------------------------- /elevation_mapping/config/sensor_processors/kinect_nguyen_et_al.yaml: -------------------------------------------------------------------------------- 1 | # Kinect (short-range mode) model taken from: 2 | # Nguyen, C. V., Izadi, S., & Lovell, D. (2012). 3 | # Modeling Kinect Sensor Noise for Improved 3D Reconstruction and Tracking. 4 | sensor_processor/type: structured_light 5 | sensor_processor/cutoff_min_depth: 0.35 6 | sensor_processor/cutoff_max_depth: 3.0 7 | sensor_processor/normal_factor_a: 0.0012 8 | sensor_processor/normal_factor_b: 0.0019 9 | sensor_processor/normal_factor_c: 0.4 10 | sensor_processor/normal_factor_d: 0 11 | sensor_processor/normal_factor_e: 1 12 | sensor_processor/lateral_factor: 0.001376915 # for theta = 45°: 0.835 * 0.00780 [mm/px] / 4.73 [mm] 13 | -------------------------------------------------------------------------------- /elevation_mapping/config/sensor_processors/perfect.yaml: -------------------------------------------------------------------------------- 1 | # Noiseless ground truth measurements 2 | sensor_processor/type: perfect 3 | -------------------------------------------------------------------------------- /elevation_mapping/config/sensor_processors/primesense_carmine_109_short_range_datasheet.yaml: -------------------------------------------------------------------------------- 1 | # Approximation of datasheet for Carmine 1.09 from: 2 | # http://www.openni.org/rd1-09-specifications/ 3 | # Coefficients for normal model computed with least squares regression. 4 | # Coefficients for lateral model taken from kinect_nguyen_et_al.yaml 5 | sensor_processor/type: structured_light 6 | sensor_processor/cutoff_min_depth: 0.2 7 | sensor_processor/cutoff_max_depth: 2.0 8 | sensor_processor/factor_a: 0.000181 9 | sensor_processor/factor_b: 0.00166 10 | sensor_processor/factor_c: 0.1 11 | sensor_processor/lateral_factor: 0.001376915 12 | -------------------------------------------------------------------------------- /elevation_mapping/config/sensor_processors/primesense_carmine_109_short_range_uncalibrated.yaml: -------------------------------------------------------------------------------- 1 | # Manual tuned for uncalibrated PrimeSense Carmine 1.09. 2 | # Also includes uncertainties with non perpedicular surfaces and 3 | # biases/warp at further distances. 4 | # Coefficients for lateral model taken from kinect_nguyen_et_al.yaml 5 | sensor_processor/type: structured_light 6 | sensor_processor/cutoff_min_depth: 0.2 7 | sensor_processor/cutoff_max_depth: 2.0 8 | sensor_processor/normal_factor_a: 0.003 9 | sensor_processor/normal_factor_b: 0.015 10 | sensor_processor/normal_factor_c: 0.25 11 | sensor_processor/lateral_factor: 0.04 12 | -------------------------------------------------------------------------------- /elevation_mapping/config/sensor_processors/realsense_ZR300.yaml: -------------------------------------------------------------------------------- 1 | # Written by Takahiro Miki. 2 | # Modeling Realsense ZR300 Sensor Noise for Improved 3D Reconstruction and Tracking. 3 | sensor_processor/type: structured_light 4 | sensor_processor/cutoff_min_depth: 0.35 5 | sensor_processor/cutoff_max_depth: 3.0 6 | sensor_processor/normal_factor_a: 0.00241809 7 | sensor_processor/normal_factor_b: 0.00662547 8 | sensor_processor/normal_factor_c: 0.77199589 9 | sensor_processor/normal_factor_d: 0 10 | sensor_processor/normal_factor_e: 1 11 | sensor_processor/lateral_factor: 0.00220941 12 | -------------------------------------------------------------------------------- /elevation_mapping/config/sensor_processors/realsense_d435.yaml: -------------------------------------------------------------------------------- 1 | # Written by Lefteris kotsonis. 2 | # Modeling Realsense D435 Sensor Noise for Improved 3D Reconstruction and Tracking. 3 | sensor_processor/type: structured_light 4 | sensor_processor/cutoff_min_depth: 0.2 5 | sensor_processor/cutoff_max_depth: 3.25 6 | sensor_processor/normal_factor_a: 0.000611 7 | sensor_processor/normal_factor_b: 0.003587 8 | sensor_processor/normal_factor_c: 0.3515 9 | sensor_processor/normal_factor_d: 0 10 | sensor_processor/normal_factor_e: 1 11 | sensor_processor/lateral_factor: 0.01576 12 | -------------------------------------------------------------------------------- /elevation_mapping/config/sensor_processors/velodyne_HDL-32E.yaml: -------------------------------------------------------------------------------- 1 | # Velodyne_HDL-32E 2 | # TODO 3 | 4 | sensor_processor/type: laser 5 | sensor_processor/min_radius: 0.018 6 | sensor_processor/beam_angle: 0.0006 7 | sensor_processor/beam_constant: 0.0015 8 | -------------------------------------------------------------------------------- /elevation_mapping/include/elevation_mapping/ElevationMap.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * ElevationMap.hpp 3 | * 4 | * Created on: Feb 5, 2014 5 | * Author: Péter Fankhauser 6 | * Institute: ETH Zurich, ANYbotics 7 | */ 8 | 9 | #pragma once 10 | 11 | // Grid Map 12 | #include 13 | 14 | // Eigen 15 | #include 16 | #include 17 | 18 | // Kindr 19 | #include 20 | 21 | // Boost 22 | #include 23 | 24 | // ROS 25 | #include 26 | 27 | // Elevation Mapping 28 | #include "elevation_mapping/PointXYZRGBConfidenceRatio.hpp" 29 | #include "elevation_mapping/postprocessing/PostprocessorPool.hpp" 30 | 31 | namespace elevation_mapping { 32 | 33 | /*! 34 | * Elevation map stored as grid map handling elevation height, variance, color etc. 35 | */ 36 | class ElevationMap { 37 | public: 38 | /*! 39 | * Constructor. 40 | */ 41 | explicit ElevationMap(ros::NodeHandle nodeHandle); 42 | 43 | /*! 44 | * Destructor. 45 | */ 46 | virtual ~ElevationMap(); 47 | 48 | /*! 49 | * Set the geometry of the elevation map. Clears all the data. 50 | * @param length the side lengths in x, and y-direction of the elevation map [m]. 51 | * @param resolution the cell size in [m/cell]. 52 | * @param position the 2d position of the elevation map in the elevation map frame [m]. 53 | * @return true if successful. 54 | */ 55 | void setGeometry(const grid_map::Length& length, const double& resolution, const grid_map::Position& position); 56 | 57 | /*! 58 | * Add new measurements to the elevation map. 59 | * @param pointCloud the point cloud data. 60 | * @param pointCloudVariances the corresponding variances of the point cloud data. 61 | * @param timeStamp the time of the input point cloud. 62 | * @param transformationSensorToMap 63 | * @return true if successful. 64 | */ 65 | bool add(const PointCloudType::Ptr pointCloud, Eigen::VectorXf& pointCloudVariances, const ros::Time& timeStamp, 66 | const Eigen::Affine3d& transformationSensorToMap); 67 | 68 | /*! 69 | * Update the elevation map with variance update data. 70 | * @param varianceUpdate the variance update in vertical direction. 71 | * @param horizontalVarianceUpdateX the variance update in horizontal x-direction. 72 | * @param horizontalVarianceUpdateY the variance update in horizontal y-direction. 73 | * @param horizontalVarianceUpdateXY the correlated variance update in horizontal xy-direction. 74 | * @param time the time of the update. 75 | * @return true if successful. 76 | */ 77 | bool update(const grid_map::Matrix& varianceUpdate, const grid_map::Matrix& horizontalVarianceUpdateX, 78 | const grid_map::Matrix& horizontalVarianceUpdateY, const grid_map::Matrix& horizontalVarianceUpdateXY, const ros::Time& time); 79 | 80 | /*! 81 | * Triggers the fusion of the entire elevation map. 82 | * @return true if successful. 83 | */ 84 | bool fuseAll(); 85 | 86 | /*! 87 | * Fuses the elevation map for a certain rectangular area. 88 | * @param position the center position of the area to fuse. 89 | * @param length the sides lengths of the area to fuse. 90 | * @return true if successful. 91 | */ 92 | bool fuseArea(const Eigen::Vector2d& position, const Eigen::Array2d& length); 93 | 94 | /*! 95 | * Clears all data of the elevation map (data and time). 96 | * @return true if successful. 97 | */ 98 | bool clear(); 99 | 100 | /*! 101 | * Removes parts of the map based on visibility criterion with ray tracing. 102 | * @param transformationSensorToMap 103 | * @param updatedTime 104 | */ 105 | void visibilityCleanup(const ros::Time& updatedTime); 106 | 107 | /*! 108 | * Move the grid map w.r.t. to the grid map frame. 109 | * @param position the new location of the elevation map in the map frame. 110 | */ 111 | void move(const Eigen::Vector2d& position); 112 | 113 | /*! 114 | * Publishes the (latest) raw elevation map. Optionally, if a postprocessing pipeline was configured, 115 | * the map is postprocessed before publishing. 116 | * @return true if successful. 117 | */ 118 | // TODO (magnus) rename? E.g. adapt for postprocessing structure. 119 | bool publishRawElevationMap(); 120 | 121 | /*! 122 | * Publishes the fused elevation map. Takes the latest available fused elevation 123 | * map, does not trigger the fusion process. 124 | * @return true if successful. 125 | */ 126 | bool publishFusedElevationMap(); 127 | 128 | /*! 129 | * Publishes the (latest) visibility cleanup map. 130 | * @return true if successful. 131 | */ 132 | bool publishVisibilityCleanupMap(); 133 | 134 | /*! 135 | * Gets a reference to the raw grid map. 136 | * @return the raw grid map. 137 | */ 138 | grid_map::GridMap& getRawGridMap(); 139 | 140 | /*! 141 | * Sets a raw grid map. 142 | * @param map The input raw grid map to set. 143 | */ 144 | void setRawGridMap(const grid_map::GridMap& map); 145 | 146 | /*! 147 | * Gets a reference to the fused grid map. 148 | * @return the fused grid map. 149 | */ 150 | grid_map::GridMap& getFusedGridMap(); 151 | 152 | /*! 153 | * Sets a fused grid map. 154 | * @param map The input fused grid map to set. 155 | */ 156 | void setFusedGridMap(const grid_map::GridMap& map); 157 | 158 | /*! 159 | * Gets the time of last map update. 160 | * @return time of the last map update. 161 | */ 162 | ros::Time getTimeOfLastUpdate(); 163 | 164 | /*! 165 | * Gets the time of last map fusion. 166 | * @return time of the last map fusion. 167 | */ 168 | ros::Time getTimeOfLastFusion(); 169 | 170 | /*! 171 | * Get the pose of the elevation map frame w.r.t. the inertial parent frame of the robot (e.g. world, map etc.). 172 | * @return pose of the elevation map frame w.r.t. the parent frame of the robot. 173 | */ 174 | const kindr::HomTransformQuatD& getPose(); 175 | 176 | /*! 177 | * Gets the position of a raw data point (x, y of cell position & height of cell value) in 178 | * the parent frame of the robot. 179 | * @param index the index of the requested cell. 180 | * @param position the position of the data point in the parent frame of the robot. 181 | * @return true if successful, false if no valid data available. 182 | */ 183 | bool getPosition3dInRobotParentFrame(const Eigen::Array2i& index, kindr::Position3D& position); 184 | 185 | /*! 186 | * Gets the fused data mutex. 187 | * @return reference to the fused data mutex. 188 | */ 189 | boost::recursive_mutex& getFusedDataMutex(); 190 | 191 | /*! 192 | * Gets the raw data mutex. 193 | * @return reference to the raw data mutex. 194 | */ 195 | boost::recursive_mutex& getRawDataMutex(); 196 | 197 | /*! 198 | * Set the frame id. 199 | * @param frameId the frame id. 200 | */ 201 | void setFrameId(const std::string& frameId); 202 | 203 | /*! 204 | * Get the frame id. 205 | * @return the frameId. 206 | */ 207 | const std::string& getFrameId(); 208 | 209 | /*! 210 | * Set the timestamp of the raw and fused elevation map. 211 | * @param timestmap to set. 212 | */ 213 | void setTimestamp(ros::Time timestamp); 214 | 215 | /*! 216 | * If the raw elevation map has subscribers. 217 | * @return true if number of subscribers bigger then 0. 218 | */ 219 | bool hasRawMapSubscribers() const; 220 | 221 | /*! 222 | * If the fused elevation map has subscribers. 223 | * @return true if number of subscribers bigger then 0. 224 | */ 225 | bool hasFusedMapSubscribers() const; 226 | 227 | /*! 228 | * Callback method for the updates of the underlying map. 229 | * Updates the internal underlying map. 230 | * @param underlyingMap the underlying map. 231 | */ 232 | void underlyingMapCallback(const grid_map_msgs::GridMap& underlyingMap); 233 | 234 | /*! 235 | * Method to set the height value around the center of the robot, can be used for initialization. 236 | * @param initPosition Position to calculate inner rectangle. 237 | * @param mapHeight The height that gets set uniformly. 238 | * @param lengthInXSubmap Length of the submap in X direction. 239 | * @param lengthInYSubmap Length of the submap in Y direction. 240 | * @param margin Extra margin that gets added to the submap boundaries. 241 | */ 242 | void setRawSubmapHeight(const grid_map::Position& initPosition, float mapHeight, double lengthInXSubmap, double lengthInYSubmap, 243 | double margin); 244 | 245 | friend class ElevationMapping; 246 | 247 | private: 248 | /*! 249 | * Fuses a region of the map. 250 | * @param topLeftIndex the top left index of the region. 251 | * @param size the size (in number of cells) of the region. 252 | * @return true if successful. 253 | */ 254 | bool fuse(const grid_map::Index& topLeftIndex, const grid_map::Index& size); 255 | 256 | /*! 257 | * Cleans the elevation map data to stay within the specified bounds. 258 | * @return true if successful. 259 | */ 260 | bool clean(); 261 | 262 | /*! 263 | * Resets the fused map data. 264 | * @return true if successful. 265 | */ 266 | void resetFusedData(); 267 | 268 | /*! 269 | * Cumulative distribution function. 270 | * @param x the argument value. 271 | * @param mean the mean of the distribution. 272 | * @param standardDeviation the standardDeviation of the distribution. 273 | * @return the function value. 274 | */ 275 | float cumulativeDistributionFunction(float x, float mean, float standardDeviation); 276 | 277 | //! ROS nodehandle. 278 | ros::NodeHandle nodeHandle_; 279 | 280 | //! Raw elevation map as grid map. 281 | grid_map::GridMap rawMap_; 282 | 283 | //! Fused elevation map as grid map. 284 | grid_map::GridMap fusedMap_; 285 | 286 | //! Visibility cleanup debug map. 287 | grid_map::GridMap visibilityCleanupMap_; 288 | 289 | //! Underlying map, used for ground truth maps, multi-robot mapping etc. 290 | grid_map::GridMap underlyingMap_; 291 | 292 | //! Thread Pool to handle raw map postprocessing filter pipelines. 293 | PostprocessorPool postprocessorPool_; 294 | 295 | //! True if underlying map has been set, false otherwise. 296 | bool hasUnderlyingMap_; 297 | 298 | //! Pose of the elevation map frame w.r.t. the inertial parent frame of the robot (e.g. world, map etc.). 299 | kindr::HomTransformQuatD pose_; 300 | 301 | //! ROS publishers. Publishing of the raw elevation map is handled by the postprocessing pool. 302 | ros::Publisher elevationMapFusedPublisher_; 303 | ros::Publisher visibilityCleanupMapPublisher_; 304 | 305 | //! Mutex lock for fused map. 306 | boost::recursive_mutex fusedMapMutex_; 307 | 308 | //! Mutex lock for raw map. 309 | boost::recursive_mutex rawMapMutex_; 310 | 311 | //! Mutex lock for visibility cleanup map. 312 | boost::recursive_mutex visibilityCleanupMapMutex_; 313 | 314 | //! Underlying map subscriber. 315 | ros::Subscriber underlyingMapSubscriber_; 316 | 317 | //! Initial ros time 318 | ros::Time initialTime_; 319 | 320 | //! Parameters. Are set through the ElevationMapping class. 321 | double minVariance_; 322 | double maxVariance_; 323 | double mahalanobisDistanceThreshold_; 324 | double multiHeightNoise_; 325 | double minHorizontalVariance_; 326 | double maxHorizontalVariance_; 327 | std::string underlyingMapTopic_; 328 | bool enableVisibilityCleanup_; 329 | bool enableContinuousCleanup_; 330 | double visibilityCleanupDuration_; 331 | double scanningDuration_; 332 | }; 333 | 334 | } // namespace elevation_mapping 335 | -------------------------------------------------------------------------------- /elevation_mapping/include/elevation_mapping/ElevationMapFunctors.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * ElevationMapFunctors.hpp 3 | * 4 | * Created on: Jan 4, 2014 5 | * Author: Péter Fankhauser 6 | * Institute: ETH Zurich, ANYbotics 7 | */ 8 | 9 | #pragma once 10 | 11 | namespace elevation_mapping { 12 | 13 | template 14 | struct VarianceClampOperator { 15 | VarianceClampOperator(const Scalar& minVariance, const Scalar& maxVariance) : minVariance_(minVariance), maxVariance_(maxVariance) {} 16 | const Scalar operator()(const Scalar& x) const { 17 | return x < minVariance_ ? minVariance_ : (x > maxVariance_ ? std::numeric_limits::infinity() : x); 18 | } 19 | Scalar minVariance_, maxVariance_; 20 | }; 21 | 22 | } // namespace elevation_mapping 23 | -------------------------------------------------------------------------------- /elevation_mapping/include/elevation_mapping/ElevationMapping.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * ElevationMap.hpp 3 | * 4 | * Created on: Nov 12, 2013 5 | * Author: Péter Fankhauser 6 | * Institute: ETH Zurich, ANYbotics 7 | */ 8 | 9 | #pragma once 10 | 11 | // Grid Map 12 | #include 13 | #include 14 | #include 15 | 16 | // ROS 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | #include 24 | 25 | // Eigen 26 | #include 27 | #include 28 | 29 | // Boost 30 | #include 31 | 32 | // Elevation Mapping 33 | #include "elevation_mapping/ElevationMap.hpp" 34 | #include "elevation_mapping/PointXYZRGBConfidenceRatio.hpp" 35 | #include "elevation_mapping/RobotMotionMapUpdater.hpp" 36 | #include "elevation_mapping/WeightedEmpiricalCumulativeDistributionFunction.hpp" 37 | #include "elevation_mapping/input_sources/InputSourceManager.hpp" 38 | #include "elevation_mapping/sensor_processors/SensorProcessorBase.hpp" 39 | 40 | namespace elevation_mapping { 41 | 42 | enum class InitializationMethods { PlanarFloorInitializer }; 43 | 44 | /*! 45 | * The elevation mapping main class. Coordinates the ROS interfaces, the timing, 46 | * and the data handling between the other classes. 47 | */ 48 | class ElevationMapping { 49 | public: 50 | /*! 51 | * Constructor. 52 | * 53 | * @param nodeHandle the ROS node handle. 54 | */ 55 | explicit ElevationMapping(ros::NodeHandle& nodeHandle); 56 | 57 | /*! 58 | * Destructor. 59 | */ 60 | virtual ~ElevationMapping(); 61 | 62 | /*! 63 | * Callback function for new data to be added to the elevation map. 64 | * 65 | * @param pointCloudMsg The point cloud to be fused with the existing data. 66 | * @param publishPointCloud If true, publishes the pointcloud after updating the map. 67 | * @param sensorProcessor_ The sensorProcessor to use in this callback. 68 | */ 69 | void pointCloudCallback(const sensor_msgs::PointCloud2ConstPtr& pointCloudMsg, bool publishPointCloud, 70 | const SensorProcessorBase::Ptr& sensorProcessor_); 71 | 72 | /*! 73 | * Callback function for the update timer. Forces an update of the map from 74 | * the robot's motion if no new measurements are received for a certain time 75 | * period. 76 | * 77 | * @param timerEvent The timer event. 78 | */ 79 | void mapUpdateTimerCallback(const ros::TimerEvent& timerEvent); 80 | 81 | /*! 82 | * Callback function for the fused map publish timer. Publishes the fused map 83 | * based on configurable duration. 84 | * 85 | * @param timerEvent The timer event. 86 | */ 87 | void publishFusedMapCallback(const ros::TimerEvent& timerEvent); 88 | 89 | /*! 90 | * Callback function for cleaning map based on visibility ray tracing. 91 | * 92 | * @param timerEvent The timer event. 93 | */ 94 | void visibilityCleanupCallback(const ros::TimerEvent& timerEvent); 95 | 96 | /*! 97 | * ROS service callback function to trigger the fusion of the entire 98 | * elevation map. 99 | * 100 | * @param request The ROS service request. 101 | * @param response The ROS service response. 102 | * @return true if successful. 103 | */ 104 | bool fuseEntireMapServiceCallback(std_srvs::Empty::Request& request, std_srvs::Empty::Response& response); 105 | 106 | /*! 107 | * ROS service callback function to return a submap of the fused elevation map. 108 | * 109 | * @param request The ROS service request defining the location and size of the fused submap. 110 | * @param response The ROS service response containing the requested fused submap. 111 | * @return true if successful. 112 | */ 113 | bool getFusedSubmapServiceCallback(grid_map_msgs::GetGridMap::Request& request, grid_map_msgs::GetGridMap::Response& response); 114 | 115 | /*! 116 | * ROS service callback function to return a submap of the raw elevation map. 117 | * 118 | * @param request The ROS service request defining the location and size of the raw submap. 119 | * @param response The ROS service response containing the requested raw submap. 120 | * @return true if successful. 121 | */ 122 | bool getRawSubmapServiceCallback(grid_map_msgs::GetGridMap::Request& request, grid_map_msgs::GetGridMap::Response& response); 123 | 124 | /*! 125 | * ROS service callback function to enable updates of the elevation map. 126 | * 127 | * @param request The ROS service request. 128 | * @param response The ROS service response. 129 | * @return true if successful. 130 | */ 131 | bool enableUpdatesServiceCallback(std_srvs::Empty::Request& request, std_srvs::Empty::Response& response); 132 | 133 | /*! 134 | * ROS service callback function to disable updates of the elevation map. 135 | * 136 | * @param request The ROS service request. 137 | * @param response The ROS service response. 138 | * @return true if successful. 139 | */ 140 | bool disableUpdatesServiceCallback(std_srvs::Empty::Request& request, std_srvs::Empty::Response& response); 141 | 142 | /*! 143 | * ROS service callback function to clear all data of the elevation map. 144 | * 145 | * @param request The ROS service request. 146 | * @param response The ROS service response. 147 | * @return true if successful. 148 | */ 149 | bool clearMapServiceCallback(std_srvs::Empty::Request& request, std_srvs::Empty::Response& response); 150 | 151 | /*! 152 | * ROS service callback function to allow for setting the individual layers of the elevation map through a service call. 153 | * The layer mask can be used to only set certain cells and not the entire map. Cells 154 | * containing NAN in the mask are not set, all the others are set. If the layer mask is 155 | * not supplied, the entire map will be set in the intersection of both maps. The 156 | * provided map can be of different size and position than the map that will be altered. 157 | * 158 | * @param request The ROS service request. 159 | * @param response The ROS service response. 160 | * @return true if successful. 161 | */ 162 | bool maskedReplaceServiceCallback(grid_map_msgs::SetGridMap::Request& request, grid_map_msgs::SetGridMap::Response& response); 163 | 164 | /*! 165 | * ROS service callback function to save the grid map with all layers to a ROS bag file. 166 | * 167 | * @param request The ROS service request. 168 | * @param response The ROS service response. 169 | * @return true if successful. 170 | */ 171 | bool saveMapServiceCallback(grid_map_msgs::ProcessFile::Request& request, grid_map_msgs::ProcessFile::Response& response); 172 | 173 | /*! 174 | * ROS service callback function to load the grid map with all layers from a ROS bag file. 175 | * 176 | * @param request The ROS service request. 177 | * @param response The ROS service response. 178 | * @return true if successful. 179 | */ 180 | bool loadMapServiceCallback(grid_map_msgs::ProcessFile::Request& request, grid_map_msgs::ProcessFile::Response& response); 181 | 182 | private: 183 | /*! 184 | * Reads and verifies the ROS parameters. 185 | * 186 | * @return true if successful. 187 | */ 188 | bool readParameters(); 189 | 190 | /*! 191 | * Performs the initialization procedure. 192 | * 193 | * @return true if successful. 194 | */ 195 | bool initialize(); 196 | 197 | /** 198 | * Sets up the subscribers for both robot poses and input data. 199 | */ 200 | void setupSubscribers(); 201 | 202 | /*! 203 | * Separate thread for all fusion service calls. 204 | */ 205 | void runFusionServiceThread(); 206 | 207 | /*! 208 | * Separate thread for visibility cleanup. 209 | */ 210 | void visibilityCleanupThread(); 211 | 212 | /*! 213 | * Update the elevation map from the robot motion up to a certain time. 214 | * 215 | * @param time Time to which the map is updated to. 216 | * @return true if successful. 217 | */ 218 | bool updatePrediction(const ros::Time& time); 219 | 220 | /*! 221 | * Updates the location of the map to follow the tracking point. Takes care 222 | * of the data handling the goes along with the relocalization. 223 | * 224 | * @return true if successful. 225 | */ 226 | bool updateMapLocation(); 227 | 228 | /*! 229 | * Reset and start the map update timer. 230 | */ 231 | void resetMapUpdateTimer(); 232 | 233 | /*! 234 | * Stop the map update timer. 235 | */ 236 | void stopMapUpdateTimer(); 237 | 238 | /*! 239 | * Initializes a submap around the robot of the elevation map with a constant height 240 | */ 241 | bool initializeElevationMap(); 242 | 243 | //! ROS nodehandle. 244 | ros::NodeHandle nodeHandle_; 245 | 246 | protected: 247 | //! Input sources. 248 | InputSourceManager inputSources_; 249 | //! ROS subscribers. 250 | ros::Subscriber pointCloudSubscriber_; //!< Deprecated, use input_source instead. 251 | message_filters::Subscriber robotPoseSubscriber_; 252 | 253 | //! ROS service servers. 254 | ros::ServiceServer fusionTriggerService_; 255 | ros::ServiceServer fusedSubmapService_; 256 | ros::ServiceServer rawSubmapService_; 257 | ros::ServiceServer enableUpdatesService_; 258 | ros::ServiceServer disableUpdatesService_; 259 | ros::ServiceServer clearMapService_; 260 | ros::ServiceServer maskedReplaceService_; 261 | ros::ServiceServer saveMapService_; 262 | ros::ServiceServer loadMapService_; 263 | 264 | //! Callback thread for the fusion services. 265 | boost::thread fusionServiceThread_; 266 | 267 | //! Callback queue for fusion service thread. 268 | ros::CallbackQueue fusionServiceQueue_; 269 | 270 | //! Cache for the robot pose messages. 271 | message_filters::Cache robotPoseCache_; 272 | 273 | //! Size of the cache for the robot pose messages. 274 | int robotPoseCacheSize_; 275 | 276 | //! Frame ID of the elevation map 277 | std::string mapFrameId_; 278 | 279 | //! TF listener and broadcaster. 280 | tf::TransformListener transformListener_; 281 | 282 | //! Point which the elevation map follows. 283 | kindr::Position3D trackPoint_; 284 | std::string trackPointFrameId_; 285 | 286 | //! ROS topics for subscriptions. 287 | std::string pointCloudTopic_; //!< Deprecated, use input_source instead. 288 | std::string robotPoseTopic_; 289 | 290 | //! Elevation map. 291 | ElevationMap map_; 292 | 293 | //! Sensor processors. Deprecated use the one from input sources instead. 294 | SensorProcessorBase::Ptr sensorProcessor_; 295 | 296 | //! Robot motion elevation map updater. 297 | RobotMotionMapUpdater robotMotionMapUpdater_; 298 | 299 | //! If true, robot motion updates are ignored. 300 | bool ignoreRobotMotionUpdates_; 301 | 302 | //! If false, elevation mapping stops updating 303 | bool updatesEnabled_; 304 | 305 | //! Time of the last point cloud update. 306 | ros::Time lastPointCloudUpdateTime_; 307 | 308 | //! Timer for the robot motion update. 309 | ros::Timer mapUpdateTimer_; 310 | 311 | //! Maximum time that the map will not be updated. 312 | ros::Duration maxNoUpdateDuration_; 313 | 314 | //! Time tolerance for updating the map with data before the last update. 315 | //! This is useful when having multiple sensors adding data to the map. 316 | ros::Duration timeTolerance_; 317 | 318 | //! Timer for publishing the fused map. 319 | ros::Timer fusedMapPublishTimer_; 320 | 321 | //! Duration for the publishing the fusing map. 322 | ros::Duration fusedMapPublishTimerDuration_; 323 | 324 | //! If map is fused after every change for debugging/analysis purposes. 325 | bool isContinuouslyFusing_; 326 | 327 | //! Timer for the raytracing cleanup. 328 | ros::Timer visibilityCleanupTimer_; 329 | 330 | //! Duration for the raytracing cleanup timer. 331 | ros::Duration visibilityCleanupTimerDuration_; 332 | 333 | //! Callback queue for raytracing cleanup thread. 334 | ros::CallbackQueue visibilityCleanupQueue_; 335 | 336 | //! Callback thread for raytracing cleanup. 337 | boost::thread visibilityCleanupThread_; 338 | 339 | //! Becomes true when corresponding poses and point clouds can be found 340 | bool receivedFirstMatchingPointcloudAndPose_; 341 | 342 | //! Name of the mask layer used in the masked replace service 343 | std::string maskedReplaceServiceMaskLayerName_; 344 | 345 | //! Enables initialization of the elevation map 346 | bool initializeElevationMap_; 347 | 348 | //! Enum to choose the initialization method 349 | int initializationMethod_; 350 | 351 | //! Width of submap of the elevation map with a constant height 352 | double lengthInXInitSubmap_; 353 | 354 | //! Height of submap of the elevation map with a constant height 355 | double lengthInYInitSubmap_; 356 | 357 | //! Margin of submap of the elevation map with a constant height 358 | double marginInitSubmap_; 359 | 360 | //! Target frame to get the init height of the elevation map 361 | std::string targetFrameInitSubmap_; 362 | 363 | //! Additional offset of the height value 364 | double initSubmapHeightOffset_; 365 | }; 366 | 367 | } // namespace elevation_mapping 368 | -------------------------------------------------------------------------------- /elevation_mapping/include/elevation_mapping/PointXYZRGBConfidenceRatio.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * PointXYZRGBConfidenceRatio.hpp 3 | * 4 | * Created on: Nov 26, 2020 5 | * Author: Magnus Gärtner 6 | * Institute: ETH Zurich, ANYbotics 7 | * 8 | * This file defines our custom pcl type, ie including a confidence_ratio. 9 | * Adapted from https://github.com/PointCloudLibrary/pcl/blob/master/common/include/pcl/impl/point_types.hpp 10 | */ 11 | 12 | #pragma once 13 | 14 | #include 15 | #include 16 | #include 17 | 18 | namespace pcl { 19 | 20 | #pragma GCC diagnostic push 21 | #pragma GCC diagnostic ignored "-Wpedantic" 22 | struct _PointXYZRGBConfidenceRatio { 23 | PCL_ADD_POINT4D; // This adds the members x,y,z which can also be accessed using the point (which is float[4]) 24 | PCL_ADD_RGB; 25 | union { 26 | struct { 27 | float confidence_ratio; 28 | }; 29 | float data_c[4]; 30 | }; 31 | PCL_MAKE_ALIGNED_OPERATOR_NEW 32 | } EIGEN_ALIGN16; // enforce SSE padding for correct memory alignment 33 | #pragma GCC diagnostic pop 34 | 35 | struct PointXYZRGBConfidenceRatio : public _PointXYZRGBConfidenceRatio { 36 | inline PointXYZRGBConfidenceRatio(const _PointXYZRGBConfidenceRatio& p) { 37 | // XZY 38 | x = p.x; 39 | y = p.y; 40 | z = p.z; 41 | data[3] = 1.0f; 42 | 43 | // RGB 44 | rgba = p.rgba; 45 | 46 | // Confidence 47 | confidence_ratio = p.confidence_ratio; 48 | } 49 | 50 | inline PointXYZRGBConfidenceRatio(float _confidence_ratio = 1.f) 51 | : PointXYZRGBConfidenceRatio(0.f, 0.f, 0.f, 0, 0, 0, _confidence_ratio) {} 52 | 53 | inline PointXYZRGBConfidenceRatio(std::uint8_t _r, std::uint8_t _g, std::uint8_t _b) 54 | : PointXYZRGBConfidenceRatio(0.f, 0.f, 0.f, _r, _g, _b) {} 55 | 56 | inline PointXYZRGBConfidenceRatio(float _x, float _y, float _z) : PointXYZRGBConfidenceRatio(_x, _y, _z, 0, 0, 0) {} 57 | 58 | inline PointXYZRGBConfidenceRatio(float _x, float _y, float _z, std::uint8_t _r, std::uint8_t _g, std::uint8_t _b, 59 | float _confidence_ratio = 1.f) { 60 | x = _x; 61 | y = _y; 62 | z = _z; 63 | data[3] = 1.0f; 64 | 65 | r = _r; 66 | g = _g; 67 | b = _b; 68 | a = 255; 69 | 70 | confidence_ratio = _confidence_ratio; 71 | } 72 | 73 | friend std::ostream& operator<<(std::ostream& os, const PointXYZRGBConfidenceRatio& p); 74 | }; 75 | 76 | PCL_EXPORTS std::ostream& operator<<(std::ostream& os, const PointXYZRGBConfidenceRatio& p); 77 | 78 | } // namespace pcl 79 | 80 | namespace elevation_mapping { 81 | using PointCloudType = pcl::PointCloud; 82 | } // namespace elevation_mapping 83 | 84 | POINT_CLOUD_REGISTER_POINT_STRUCT(pcl::_PointXYZRGBConfidenceRatio, // here we assume a XYZ + "confidence_ratio" (as fields) 85 | (float, x, x) // NOLINT 86 | (float, y, y) // NOLINT 87 | (float, z, z) // NOLINT 88 | (std::uint32_t, rgba, rgba) // NOLINT 89 | (float, confidence_ratio, confidence_ratio)) // NOLINT 90 | 91 | POINT_CLOUD_REGISTER_POINT_WRAPPER(pcl::PointXYZRGBConfidenceRatio, pcl::_PointXYZRGBConfidenceRatio) 92 | -------------------------------------------------------------------------------- /elevation_mapping/include/elevation_mapping/RobotMotionMapUpdater.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * RobotMotionMapUpdater.hpp 3 | * 4 | * Created on: Feb 5, 2014 5 | * Author: Péter Fankhauser 6 | * Institute: ETH Zurich, ANYbotics 7 | */ 8 | 9 | #pragma once 10 | 11 | // Elevation Mapping 12 | #include "elevation_mapping/ElevationMap.hpp" 13 | 14 | // Eigen 15 | #include 16 | 17 | // Kindr 18 | #include 19 | 20 | // ROS 21 | #include 22 | 23 | namespace elevation_mapping { 24 | 25 | /*! 26 | * Computes the map variance update from the pose covariance of the robot. 27 | */ 28 | class RobotMotionMapUpdater { 29 | public: 30 | using Pose = kindr::HomogeneousTransformationPosition3RotationQuaternionD; 31 | using Covariance = Eigen::Matrix; 32 | using PoseCovariance = Eigen::Matrix; 33 | using ReducedCovariance = Eigen::Matrix; 34 | using Jacobian = Eigen::Matrix; 35 | 36 | /*! 37 | * Constructor. 38 | */ 39 | explicit RobotMotionMapUpdater(ros::NodeHandle nodeHandle); 40 | 41 | /*! 42 | * Destructor. 43 | */ 44 | virtual ~RobotMotionMapUpdater(); 45 | 46 | /*! 47 | * Reads and verifies the ROS parameters. 48 | * @return true if successful. 49 | */ 50 | bool readParameters(); 51 | 52 | /*! 53 | * Computes the model update for the elevation map based on the pose covariance and 54 | * adds the update to the map. 55 | * @param[in] map the elevation map to be updated. 56 | * @param[in] robotPose the current pose. 57 | * @param[in] robotPoseCovariance the current pose covariance matrix. 58 | * @param[in] time the time of the current update. 59 | * @return true if successful. 60 | */ 61 | bool update(ElevationMap& map, const Pose& robotPose, const PoseCovariance& robotPoseCovariance, const ros::Time& time); 62 | 63 | private: 64 | /*! 65 | * Computes the reduced covariance (4x4: x, y, z, yaw) from the full pose covariance (6x6: x, y, z, roll, pitch, yaw). 66 | * @param[in] robotPose the robot pose. 67 | * @param[in] robotPoseCovariance the full pose covariance matrix (6x6). 68 | * @param[out] reducedCovariance the reduced covariance matrix (4x4); 69 | * @return true if successful. 70 | */ 71 | bool computeReducedCovariance(const Pose& robotPose, const PoseCovariance& robotPoseCovariance, ReducedCovariance& reducedCovariance); 72 | 73 | /*! 74 | * Computes the covariance between the new and the previous pose. 75 | * @param[in] robotPose the current robot pose. 76 | * @param[in] reducedCovariance the current robot pose covariance matrix (reduced). 77 | * @param[out] relativeRobotPoseCovariance the relative covariance between the current and the previous robot pose (reduced form). 78 | * @return true if successful. 79 | */ 80 | bool computeRelativeCovariance(const Pose& robotPose, const ReducedCovariance& reducedCovariance, ReducedCovariance& relativeCovariance); 81 | 82 | //! ROS nodehandle. 83 | ros::NodeHandle nodeHandle_; 84 | 85 | //! Time of the previous update. 86 | ros::Time previousUpdateTime_; 87 | 88 | //! Previous robot pose. 89 | Pose previousRobotPose_; 90 | 91 | //! Robot pose covariance (reduced) from the previous update. 92 | ReducedCovariance previousReducedCovariance_; 93 | 94 | //! Scaling factor for the covariance matrix (default 1). 95 | double covarianceScale_; 96 | }; 97 | 98 | } // namespace elevation_mapping 99 | -------------------------------------------------------------------------------- /elevation_mapping/include/elevation_mapping/WeightedEmpiricalCumulativeDistributionFunction.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * WeightedEmpiricalCumulativeDistributionFunction.hpp 3 | * 4 | * Created on: Dec 7, 2015 5 | * Author: Péter Fankhauser 6 | * Institute: ETH Zurich, ANYbotics 7 | */ 8 | 9 | #pragma once 10 | 11 | #include 12 | #include 13 | #include 14 | 15 | namespace elevation_mapping { 16 | 17 | template 18 | class WeightedEmpiricalCumulativeDistributionFunction { 19 | public: 20 | WeightedEmpiricalCumulativeDistributionFunction() : totalWeight_(0.0), isComputed_(false) {} 21 | 22 | virtual ~WeightedEmpiricalCumulativeDistributionFunction() = default; 23 | 24 | void add(const Type value, const double weight = 1.0) { 25 | isComputed_ = false; 26 | if (data_.find(value) != data_.end()) { 27 | data_[value] += weight; 28 | } else { 29 | data_.insert(std::pair(value, weight)); 30 | } 31 | totalWeight_ += weight; 32 | } 33 | 34 | void clear() { 35 | isComputed_ = false; 36 | totalWeight_ = 0.0; 37 | data_.clear(); 38 | distribution_.clear(); 39 | } 40 | 41 | bool compute() { 42 | if (data_.empty()) { 43 | return false; 44 | } 45 | distribution_.clear(); 46 | inverseDistribution_.clear(); 47 | 48 | if (data_.size() == 1) { 49 | // Special treatment for size 1. 50 | inverseDistribution_.insert(std::pair(0.0, data_.begin()->first)); 51 | inverseDistribution_.insert(std::pair(1.0, data_.begin()->first)); 52 | return isComputed_ = true; 53 | } 54 | 55 | // double cumulativeWeight = 0.0; 56 | // inverseDistribution_.insert(std::pair(0.0, data_.begin()->first)); 57 | // for (const auto& point : data_) { 58 | // cumulativeWeight += point.second; 59 | // inverseDistribution_.insert(inverseDistribution_.end(), 60 | // std::pair(cumulativeWeight / totalWeight_, point.first)); 61 | // } 62 | 63 | double cumulativeWeight = -data_.begin()->second; // Smallest observation corresponds to a probability of 0. 64 | const double adaptedTotalWeight = totalWeight_ - data_.begin()->second; 65 | for (const auto& point : data_) { 66 | cumulativeWeight += point.second; 67 | inverseDistribution_.insert(inverseDistribution_.end(), std::pair(cumulativeWeight / adaptedTotalWeight, point.first)); 68 | } 69 | 70 | return isComputed_ = true; 71 | } 72 | 73 | /*! 74 | * Returns the quantile corresponding to the given probability (inverse distribution function). 75 | * The smallest observation corresponds to a probability of 0 and the largest to a probability of 1. 76 | * Uses linear interpolation, see https://stat.ethz.ch/R-manual/R-devel/library/stats/html/quantile.html 77 | * and "Sampling Quantiles in Statistical Packages", Hyndman et. al., 1996. 78 | * @param probability the order of the quantile. 79 | * @return the quantile for the given probability. 80 | */ 81 | const Type quantile(const double probability) const { 82 | if (!isComputed_) { 83 | throw std::runtime_error( 84 | "WeightedEmpiricalCumulativeDistributionFunction::quantile(...): The distribution functions needs to be computed (compute()) " 85 | "first."); 86 | } 87 | if (probability <= 0.0) { 88 | return inverseDistribution_.begin()->second; 89 | } 90 | if (probability >= 1.0) { 91 | return inverseDistribution_.rbegin()->second; 92 | } 93 | const auto& up = inverseDistribution_.lower_bound(probability); // First element that is not less than key. 94 | auto low = up; // Copy. 95 | --low; 96 | return low->second + (probability - low->first) * (up->second - low->second) / (up->first - low->first); 97 | } 98 | 99 | friend std::ostream& operator<<(std::ostream& out, const WeightedEmpiricalCumulativeDistributionFunction& wecdf) { 100 | unsigned int i = 0; 101 | out << "Data points:" << std::endl; 102 | for (const auto& point : wecdf.data_) { 103 | out << "[" << i << "] Value: " << point.first << " Weight: " << point.second << std::endl; 104 | ++i; 105 | } 106 | 107 | i = 0; 108 | out << "Cumulative distribution function:" << std::endl; 109 | for (const auto& point : wecdf.distribution_) { 110 | out << "[" << i << "] Value: " << point.first << " Prob.: " << point.second << std::endl; 111 | ++i; 112 | } 113 | 114 | i = 0; 115 | out << "Inverse distribution function:" << std::endl; 116 | for (const auto& point : wecdf.inverseDistribution_) { 117 | out << "[" << i << "] Prob.: " << point.first << " Value: " << point.second << std::endl; 118 | ++i; 119 | } 120 | 121 | return out; 122 | } 123 | 124 | private: 125 | //! Data points stored as value/weight pair (histogram). 126 | std::map data_; 127 | 128 | //! Cumulative distribution function (and its inverse) stored as value pair/cumulative probability. 129 | std::map distribution_; 130 | std::map inverseDistribution_; 131 | 132 | //! Total weight. 133 | double totalWeight_; 134 | 135 | //! True of computed. 136 | bool isComputed_; 137 | }; 138 | 139 | } /* namespace elevation_mapping */ 140 | -------------------------------------------------------------------------------- /elevation_mapping/include/elevation_mapping/input_sources/Input.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Input.hpp 3 | * 4 | * Created on: Oct 06, 2020 5 | * Author: Magnus Gärtner 6 | * Institute: ETH Zurich, ANYbotics 7 | */ 8 | 9 | #pragma once 10 | 11 | #include 12 | #include 13 | #include 14 | 15 | #include "elevation_mapping/sensor_processors/SensorProcessorBase.hpp" 16 | 17 | namespace elevation_mapping { 18 | class ElevationMapping; // Forward declare to avoid cyclic import dependency. 19 | 20 | /** 21 | * @brief An Input feeds data to ElevationMapping callbacks. E.g it holds a subscription to an sensor source and registered an appropriate 22 | * ElevationMapping callback. 23 | */ 24 | class Input { 25 | public: 26 | template 27 | using CallbackT = void (ElevationMapping::*)(const boost::shared_ptr&, bool, const SensorProcessorBase::Ptr&); 28 | 29 | /** 30 | * @brief Constructor. 31 | * @param nh Reference to the nodeHandle of the manager. Used to subscribe 32 | * to inputs. 33 | */ 34 | explicit Input(ros::NodeHandle nh); 35 | 36 | /** 37 | * @brief Configure the input source. 38 | * @param name Name of this input source. 39 | * @param parameters The configuration parameters. 40 | * @param generalSensorProcessorParameters Parameters shared by all sensor processors. 41 | * @return True if configuring was successful. 42 | */ 43 | bool configure(std::string name, const XmlRpc::XmlRpcValue& parameters, 44 | const SensorProcessorBase::GeneralParameters& generalSensorProcessorParameters); 45 | 46 | /** 47 | * @brief Registers the corresponding callback in the elevationMap. 48 | * @param map The map we want to link this input source to. 49 | * @param callback The callback to use for incoming data. 50 | * @tparam MsgT The message types of the callback. 51 | */ 52 | template 53 | void registerCallback(ElevationMapping& map, CallbackT callback); 54 | 55 | /** 56 | * @return The topic (as absolute path, with renames) that this input 57 | * subscribes to. 58 | */ 59 | std::string getSubscribedTopic() const; 60 | 61 | /** 62 | * @return The type of this input source. 63 | */ 64 | std::string getType() { return type_; } 65 | 66 | private: 67 | /** 68 | * @brief Configures the used sensor processor from the given parameters. 69 | * @param name The name of this input source 70 | * @param parameters The parameters of this input source 71 | * @param generalSensorProcessorParameters General parameters needed for the sensor processor that are not specific to this sensor 72 | * processor. 73 | * @return True if successful. 74 | */ 75 | bool configureSensorProcessor(std::string name, const XmlRpc::XmlRpcValue& parameters, 76 | const SensorProcessorBase::GeneralParameters& generalSensorProcessorParameters); 77 | 78 | // ROS connection. 79 | ros::Subscriber subscriber_; 80 | ros::NodeHandle nodeHandle_; 81 | 82 | //! Sensor processor 83 | SensorProcessorBase::Ptr sensorProcessor_; 84 | 85 | // Parameters. 86 | std::string name_; 87 | std::string type_; 88 | uint32_t queueSize_; 89 | std::string topic_; 90 | bool publishOnUpdate_; 91 | }; 92 | 93 | template 94 | void Input::registerCallback(ElevationMapping& map, CallbackT callback) { 95 | subscriber_ = nodeHandle_.subscribe( 96 | topic_, queueSize_, std::bind(callback, std::ref(map), std::placeholders::_1, publishOnUpdate_, std::ref(sensorProcessor_))); 97 | ROS_INFO("Subscribing to %s: %s, queue_size: %i.", type_.c_str(), topic_.c_str(), queueSize_); 98 | } 99 | 100 | } // namespace elevation_mapping -------------------------------------------------------------------------------- /elevation_mapping/include/elevation_mapping/input_sources/InputSourceManager.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * InputSourceManager.hpp 3 | * 4 | * Created on: Oct 02, 2020 5 | * Author: Magnus Gärtner 6 | * Institute: ETH Zurich, ANYbotics 7 | */ 8 | 9 | #pragma once 10 | 11 | #include "elevation_mapping/input_sources/Input.hpp" 12 | 13 | #include 14 | #include 15 | 16 | namespace elevation_mapping { 17 | class ElevationMapping; // Forward declare to avoid cyclic import dependency. 18 | 19 | /** 20 | * @brief An input source manager reads a list of input sources from the configuration and connects them to the appropriate callback of 21 | * elevation mapping. 22 | */ 23 | class InputSourceManager { 24 | public: 25 | /** 26 | * @brief Constructor. 27 | * @param nodeHandle Used to resolve the namespace and setup the subscribers. 28 | */ 29 | explicit InputSourceManager(const ros::NodeHandle& nodeHandle); 30 | 31 | /** 32 | * @brief Configure the input sources from a configuration stored on the 33 | * parameter server under inputSourcesNamespace. 34 | * @param inputSourcesNamespace The namespace of the subscribers list to load. 35 | * @return True if configuring was successful. 36 | */ 37 | bool configureFromRos(const std::string& inputSourcesNamespace); 38 | 39 | /** 40 | * @brief Configure the input sources. 41 | * This will configure all managed input sources. 42 | * @param parameters The list of input source parameters. 43 | * @param sourceConfigurationName The name of the input source configuration. 44 | * @return True if configuring was successful. 45 | */ 46 | bool configure(const XmlRpc::XmlRpcValue& parameters, const std::string& sourceConfigurationName); 47 | 48 | /** 49 | * @brief Registers the corresponding callback in the elevationMap. 50 | * @param map The map we want to link the input sources to. 51 | * @param callbacks pairs of callback type strings and their corresponding 52 | * callback. E.g: std::make_pair("pointcloud", 53 | * &ElevationMap::pointCloudCallback), std::make_pair("depthimage", 54 | * &ElevationMap::depthImageCallback) 55 | * @tparam MsgT The message types of the callbacks 56 | * @return True if registering was successful. 57 | */ 58 | template 59 | bool registerCallbacks(ElevationMapping& map, std::pair>... callbacks); 60 | 61 | /** 62 | * @return The number of successfully configured input sources. 63 | */ 64 | int getNumberOfSources(); 65 | 66 | protected: 67 | //! A list of input sources. 68 | std::vector sources_; 69 | 70 | //! Node handle to load. 71 | ros::NodeHandle nodeHandle_; 72 | }; 73 | 74 | // Template definitions 75 | 76 | template 77 | bool InputSourceManager::registerCallbacks(ElevationMapping& map, std::pair>... callbacks) { 78 | if (sources_.empty()) { 79 | ROS_WARN("Not registering any callbacks, no input sources given. Did you configure the InputSourceManager?"); 80 | return true; 81 | } 82 | for (Input& source : sources_) { 83 | bool callbackRegistered = false; 84 | for (auto& callback : {callbacks...}) { 85 | if (source.getType() == callback.first) { 86 | source.registerCallback(map, callback.second); 87 | callbackRegistered = true; 88 | } 89 | } 90 | if (not callbackRegistered) { 91 | ROS_WARN("The configuration contains input sources of an unknown type: %s", source.getType().c_str()); 92 | ROS_WARN("Available types are:"); 93 | for (auto& callback : {callbacks...}) { 94 | ROS_WARN("- %s", callback.first); 95 | } 96 | return false; 97 | } 98 | } 99 | return true; 100 | } 101 | 102 | } // namespace elevation_mapping 103 | -------------------------------------------------------------------------------- /elevation_mapping/include/elevation_mapping/postprocessing/PostprocessingPipelineFunctor.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * PostprocessingPipelineFunctor.cpp 3 | * 4 | * Created on: Sep. 14, 2020 5 | * Author: Magnus Gärtner 6 | * Institute: ETH Zurich, ANYbotics 7 | */ 8 | 9 | #pragma once 10 | 11 | #include 12 | #include 13 | #include 14 | 15 | namespace elevation_mapping { 16 | 17 | /** 18 | * @brief A configurable postprocessing functor, it applies the configured filter pipeline to the input. 19 | * 20 | * Usage: 21 | * ======== 22 | * 23 | * // Create the functor, it will configure the postprocessing pipeline from the ros parameters. 24 | * PostprocessingPipelineFunctor postprocessor(nodeHandle); 25 | * 26 | * // Call the functor by feeding it some input data. It will postprocess and publish the processed data. 27 | * postprocessor(gridMap); 28 | * 29 | */ 30 | class PostprocessingPipelineFunctor { 31 | public: 32 | using GridMap = grid_map::GridMap; 33 | 34 | /** 35 | * @brief Explicit Constructor. 36 | * @param nodeHandle The node handle to read parameters from and to publish output data. 37 | */ 38 | explicit PostprocessingPipelineFunctor(ros::NodeHandle& nodeHandle); 39 | 40 | /** 41 | * @brief Destructor. 42 | */ 43 | ~PostprocessingPipelineFunctor(); 44 | 45 | /** 46 | * @brief The operator. It applies this object, e.g a postprocessing pipeline to the given GridMap. 47 | * @param inputMap The gridMap on which the postprocessing is applied (not inplace). 48 | * @return The postprocessed gridMap. 49 | */ 50 | GridMap operator()(GridMap& inputMap); 51 | 52 | /** 53 | * Publishes a given grid map. 54 | * @param gridMap The Grid Map that this functor will publish. 55 | */ 56 | void publish(const GridMap& gridMap) const; 57 | 58 | /** 59 | * Checks whether there are any subscribers to the result of this functor. 60 | * 61 | * @return True if someone listens to the topic this functor publishes to. 62 | */ 63 | bool hasSubscribers() const; 64 | 65 | private: 66 | /** 67 | * @brief Reads in the parameters from the ROS parameter server. 68 | * @return A flag whether the parameters were read successfully. 69 | */ 70 | void readParameters(); 71 | 72 | //! ROS nodehandle. 73 | ros::NodeHandle& nodeHandle_; 74 | 75 | //! Name of the output grid map topic. 76 | std::string outputTopic_; 77 | 78 | //! Grid map publisher. 79 | ros::Publisher publisher_; 80 | 81 | //! Filter chain. 82 | filters::FilterChain filterChain_; 83 | 84 | //! Filter chain parameters name. 85 | std::string filterChainParametersName_; 86 | 87 | //! Flag indicating if the filter chain was successfully configured. 88 | bool filterChainConfigured_; 89 | }; 90 | 91 | } // namespace elevation_mapping -------------------------------------------------------------------------------- /elevation_mapping/include/elevation_mapping/postprocessing/PostprocessingWorker.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * PostprocessingWorker.hpp 3 | * 4 | * Created on: Dec. 21, 2020 5 | * Author: Yoshua Nava 6 | * Institute: ANYbotics 7 | */ 8 | 9 | #pragma once 10 | 11 | #include 12 | #include 13 | #include 14 | 15 | #include 16 | 17 | #include "elevation_mapping/postprocessing/PostprocessingPipelineFunctor.hpp" 18 | 19 | namespace elevation_mapping { 20 | 21 | /** 22 | * @brief A wrapper around the postprocessing pipelines functor. 23 | * 24 | * @remark It stores together the functor, grid map data buffer, and thread tooling. 25 | * It is assumed that the members of this function are guarded by an external mutex, 26 | * handled by the owner of this class. 27 | */ 28 | class PostprocessingWorker { 29 | public: 30 | using GridMap = grid_map::GridMap; 31 | 32 | explicit PostprocessingWorker(ros::NodeHandle nodeHandle); 33 | 34 | /*! @name Accessors */ 35 | ///@{ 36 | boost::asio::io_service& ioService() { return ioService_; } 37 | std::thread& thread() { return thread_; } 38 | const GridMap& dataBuffer() { return dataBuffer_; } 39 | void setDataBuffer(GridMap data) { dataBuffer_ = std::move(data); } 40 | ///@} 41 | 42 | /*! @name Methods */ 43 | ///@{ 44 | /** 45 | * @brief Process the data in the buffer. 46 | * 47 | * @return GridMap Processed grid map. 48 | */ 49 | GridMap processBuffer(); 50 | 51 | /** 52 | * @brief Publish a given grid map. 53 | * 54 | * @param gridMap The grid map to publish. 55 | */ 56 | void publish(const GridMap& gridMap) const; 57 | 58 | /** 59 | * @brief Checks whether the worker publisher has any active subscribers. 60 | * 61 | * @return true If there are subscribers to the worker publisher, false otherwise. 62 | */ 63 | bool hasSubscribers() const; 64 | ///@} 65 | 66 | protected: 67 | //! The functor to execute on a given GridMap. 68 | PostprocessingPipelineFunctor functor_; 69 | 70 | //! BOOST Service Worker Infrastructure. 71 | //! The io_service objects provide the interface to post an asynchronous task. 72 | //! The work object ensures that the io_service run() method keeps spinning and accepts tasks. 73 | //! A thread executes the io_service::run() method, which does io_service::work, ie accepting and executing new tasks. 74 | //! IO service for asynchronous operation. 75 | boost::asio::io_service ioService_; 76 | //! IO service work notifier 77 | boost::asio::io_service::work work_; 78 | //! The thread on which this worker runs. 79 | std::thread thread_; 80 | 81 | //! Data container for the worker. 82 | GridMap dataBuffer_; 83 | }; 84 | 85 | } // namespace elevation_mapping 86 | -------------------------------------------------------------------------------- /elevation_mapping/include/elevation_mapping/postprocessing/PostprocessorPool.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * PostprocessorPool.hpp 3 | * 4 | * Created on: Sep. 14, 2020 5 | * Author: Magnus Gärtner 6 | * Institute: ETH Zurich, ANYbotics 7 | */ 8 | 9 | #pragma once 10 | 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | 18 | #include 19 | 20 | #include "elevation_mapping/postprocessing/PostprocessingPipelineFunctor.hpp" 21 | #include "elevation_mapping/postprocessing/PostprocessingWorker.hpp" 22 | 23 | namespace elevation_mapping { 24 | 25 | /** 26 | * @brief A handler for executing postprocessing pipelines in parallel. 27 | * 28 | * @remark This class starts poolSize threads. 29 | * It is intentional that if raw maps come in faster than the post processing works some raw maps are skipped. 30 | * There is a minimal critical section updating the availableServices_ structure. 31 | * Workers operate on copies of the raw elevation map instead of working on the same data. 32 | */ 33 | class PostprocessorPool { 34 | public: 35 | using GridMap = grid_map::GridMap; 36 | 37 | /** 38 | * @brief Constructor. 39 | * @param poolSize The number of worker threads to allocate. 40 | * @param nodeHandle The node handle used to configure the task to run and to publish the finished tasks. 41 | */ 42 | PostprocessorPool(std::size_t poolSize, ros::NodeHandle nodeHandle); 43 | 44 | /** 45 | * @brief Destructor. 46 | * 47 | * Requests all postprocessing pipelines to stop and joins their threads until they're done. 48 | */ 49 | ~PostprocessorPool(); 50 | 51 | /** 52 | * @brief Starts a task on a thread from the thread pool if there are some available. 53 | * @param gridMap The data to be processed by this task. 54 | * @return True if the PostprocessorPool accepted the task. If false the PostprocessorPool had no available threads and discarded the 55 | * task. 56 | */ 57 | bool runTask(const GridMap& gridMap); 58 | 59 | /** 60 | * @brief Performs a check on the number of subscribers. 61 | * @return True if someone listens to the topic that the managed postprocessor_ publishes to. 62 | */ 63 | bool pipelineHasSubscribers() const; 64 | 65 | private: 66 | /** 67 | * @brief Wrap a task so that the postprocessor pool gets notified on completion of the task. 68 | * @param serviceIndex The index of the thread / service that will process this task. 69 | */ 70 | void wrapTask(size_t serviceIndex); 71 | 72 | // Post-processing workers. 73 | std::vector> workers_; 74 | 75 | //! Container holding the service ids which have corresponding threads. The only object that is used in a mutual exclusive manner and must 76 | //! be protected by availableServicesMutex_. 77 | boost::mutex availableServicesMutex_; 78 | std::deque availableServices_; 79 | }; 80 | 81 | } // namespace elevation_mapping 82 | -------------------------------------------------------------------------------- /elevation_mapping/include/elevation_mapping/sensor_processors/LaserSensorProcessor.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * LaserSensorProcessor.hpp 3 | * 4 | * Created on: Sep 15, 2014 5 | * Author: Péter Fankhauser 6 | * Institute: ETH Zurich, ANYbotics 7 | */ 8 | 9 | #pragma once 10 | 11 | #include "elevation_mapping/PointXYZRGBConfidenceRatio.hpp" 12 | #include "elevation_mapping/sensor_processors/SensorProcessorBase.hpp" 13 | 14 | namespace elevation_mapping { 15 | 16 | /*! 17 | * Sensor processor for laser range sensors. 18 | */ 19 | class LaserSensorProcessor : public SensorProcessorBase { 20 | public: 21 | /*! 22 | * Constructor. 23 | * @param nodeHandle the ROS node handle. 24 | */ 25 | LaserSensorProcessor(ros::NodeHandle& nodeHandle, const SensorProcessorBase::GeneralParameters& generalParameters); 26 | 27 | /*! 28 | * Destructor. 29 | */ 30 | ~LaserSensorProcessor() override; 31 | 32 | private: 33 | /*! 34 | * Reads and verifies the parameters. 35 | * @return true if successful. 36 | */ 37 | bool readParameters() override; 38 | 39 | /*! 40 | * Computes the elevation map height variances for each point in a point cloud with the 41 | * sensor model and the robot pose covariance. 42 | * @param[in] pointCloud the point cloud for which the variances are computed. 43 | * @param[in] robotPoseCovariance the robot pose covariance matrix. 44 | * @param[out] variances the elevation map height variances. 45 | * @return true if successful. 46 | */ 47 | bool computeVariances(const PointCloudType::ConstPtr pointCloud, const Eigen::Matrix& robotPoseCovariance, 48 | Eigen::VectorXf& variances) override; 49 | }; 50 | 51 | } /* namespace elevation_mapping */ 52 | -------------------------------------------------------------------------------- /elevation_mapping/include/elevation_mapping/sensor_processors/PerfectSensorProcessor.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * PerfectSensorProcessor.hpp 3 | * 4 | * Created on: Sep 28, 2014 5 | * Author: Péter Fankhauser 6 | * Institute: ETH Zurich, ANYbotics 7 | */ 8 | 9 | #pragma once 10 | 11 | #include "elevation_mapping/PointXYZRGBConfidenceRatio.hpp" 12 | #include "elevation_mapping/sensor_processors/SensorProcessorBase.hpp" 13 | 14 | namespace elevation_mapping { 15 | 16 | /*! 17 | * Sensor processor for laser range sensors. 18 | */ 19 | class PerfectSensorProcessor : public SensorProcessorBase { 20 | public: 21 | /*! 22 | * Constructor. 23 | * @param nodeHandle the ROS node handle. 24 | */ 25 | PerfectSensorProcessor(ros::NodeHandle& nodeHandle, const SensorProcessorBase::GeneralParameters& generalParameters); 26 | 27 | /*! 28 | * Destructor. 29 | */ 30 | ~PerfectSensorProcessor() override; 31 | 32 | private: 33 | /*! 34 | * Reads and verifies the parameters. 35 | * @return true if successful. 36 | */ 37 | bool readParameters() override; 38 | 39 | /*! 40 | * Computes the elevation map height variances for each point in a point cloud with the 41 | * sensor model and the robot pose covariance. 42 | * @param[in] pointCloud the point cloud for which the variances are computed. 43 | * @param[in] robotPoseCovariance the robot pose covariance matrix. 44 | * @param[out] variances the elevation map height variances. 45 | * @return true if successful. 46 | */ 47 | bool computeVariances(const PointCloudType::ConstPtr pointCloud, const Eigen::Matrix& robotPoseCovariance, 48 | Eigen::VectorXf& variances) override; 49 | }; 50 | 51 | } /* namespace elevation_mapping */ 52 | -------------------------------------------------------------------------------- /elevation_mapping/include/elevation_mapping/sensor_processors/SensorProcessorBase.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * SensorProcessorBase.hpp 3 | * 4 | * Created on: Jun 6, 2014 5 | * Author: Péter Fankhauser, Hannes Keller 6 | * Institute: ETH Zurich, ANYbotics 7 | */ 8 | 9 | #pragma once 10 | 11 | // ROS 12 | #include 13 | #include 14 | 15 | // Eigen 16 | #include 17 | 18 | // Kindr 19 | #include 20 | 21 | // STL 22 | #include 23 | #include 24 | #include 25 | 26 | // PCL 27 | #include "elevation_mapping/PointXYZRGBConfidenceRatio.hpp" 28 | 29 | namespace elevation_mapping { 30 | 31 | /*! 32 | * Generic Sensor processor base class. Provides functionalities 33 | * common to all sensors and defines the interface for specialized 34 | * sensor processor classes. 35 | * Cleans the point cloud, transforms it to a desired frame, and 36 | * computes the measurement variances based on a sensor model in 37 | * the desired frame. 38 | */ 39 | class SensorProcessorBase { 40 | public: 41 | using Ptr = std::unique_ptr; 42 | friend class ElevationMapping; 43 | friend class Input; 44 | 45 | struct GeneralParameters { 46 | std::string robotBaseFrameId_; 47 | std::string mapFrameId_; 48 | 49 | GeneralParameters(std::string robotBaseFrameId = "robot", std::string mapFrameId = "map") 50 | : robotBaseFrameId_(std::move(robotBaseFrameId)), mapFrameId_(std::move(mapFrameId)) {} 51 | }; 52 | 53 | /*! 54 | * Constructor. 55 | * @param nodeHandle the ROS node handle. 56 | * @param generalConfig General parameters that the sensor processor must know in order to work. // TODO (magnus) improve documentation. 57 | */ 58 | SensorProcessorBase(ros::NodeHandle& nodeHandle, const GeneralParameters& generalConfig); 59 | 60 | /*! 61 | * Destructor. 62 | */ 63 | virtual ~SensorProcessorBase(); 64 | 65 | /*! 66 | * Processes the point cloud. 67 | * @param[in] pointCloudInput the input point cloud. 68 | * @param[in] targetFrame the frame to which the point cloud should be transformed. // TODO Update. 69 | * @param[out] pointCloudOutput the processed point cloud. 70 | * @param[out] variances the measurement variances expressed in the target frame. 71 | * @return true if successful. 72 | */ 73 | bool process(const PointCloudType::ConstPtr pointCloudInput, const Eigen::Matrix& robotPoseCovariance, 74 | const PointCloudType::Ptr pointCloudMapFrame, Eigen::VectorXf& variances, std::string sensorFrame); 75 | 76 | /*! 77 | * Checks if a valid tf transformation was received since startup. 78 | * @return True if there was one valid tf transformation. 79 | */ 80 | bool isTfAvailableInBuffer() { return firstTfAvailable_; } 81 | 82 | protected: 83 | /*! 84 | * Reads and verifies the parameters. 85 | * @return true if successful. 86 | */ 87 | virtual bool readParameters(); 88 | 89 | /*! 90 | * Filters the point cloud regardless of the sensor type. Removes NaN values. 91 | * Optionally, applies voxelGridFilter to reduce number of points in 92 | * the point cloud. 93 | * @param pointCloud the point cloud to clean. 94 | * @return true if successful. 95 | */ 96 | bool filterPointCloud(const PointCloudType::Ptr pointCloud); 97 | 98 | /*! 99 | * Sensor specific point cloud cleaning. 100 | * @param pointCloud the point cloud to clean. 101 | * @return true if successful. 102 | */ 103 | virtual bool filterPointCloudSensorType(const PointCloudType::Ptr pointCloud); 104 | 105 | /*! 106 | * Computes the elevation map height variances for each point in a point cloud with the 107 | * sensor model and the robot pose covariance. 108 | * @param[in] pointCloud the point cloud for which the variances are computed. 109 | * @param[in] robotPoseCovariance the robot pose covariance matrix. 110 | * @param[out] variances the elevation map height variances. 111 | * @return true if successful. 112 | */ 113 | virtual bool computeVariances(const PointCloudType::ConstPtr pointCloud, const Eigen::Matrix& robotPoseCovariance, 114 | Eigen::VectorXf& variances) = 0; 115 | 116 | /*! 117 | * Update the transformations for a given time stamp. 118 | * @param timeStamp the time stamp for the transformation. 119 | * @return true if successful. 120 | */ 121 | bool updateTransformations(const ros::Time& timeStamp); 122 | 123 | /*! 124 | * Transforms the point cloud the a target frame. 125 | * @param[in] pointCloud the point cloud to be transformed. 126 | * @param[out] pointCloudTransformed the resulting point cloud after transformation. 127 | * @param[in] targetFrame the desired target frame. 128 | * @return true if successful. 129 | */ 130 | bool transformPointCloud(PointCloudType::ConstPtr pointCloud, PointCloudType::Ptr pointCloudTransformed, const std::string& targetFrame); 131 | 132 | /*! 133 | * Removes points with z-coordinate above a limit in map frame. 134 | * @param[in/out] pointCloud the point cloud to be cropped. 135 | */ 136 | void removePointsOutsideLimits(PointCloudType::ConstPtr reference, std::vector& pointClouds); 137 | 138 | //! ROS nodehandle. 139 | ros::NodeHandle& nodeHandle_; 140 | 141 | //! TF transform listener. 142 | tf::TransformListener transformListener_; 143 | 144 | //! Rotation from Base to Sensor frame (C_SB) 145 | kindr::RotationMatrixD rotationBaseToSensor_; 146 | 147 | //! Translation from Base to Sensor in Base frame (B_r_BS) 148 | kindr::Position3D translationBaseToSensorInBaseFrame_; 149 | 150 | //! Rotation from (elevation) Map to Base frame (C_BM) 151 | kindr::RotationMatrixD rotationMapToBase_; 152 | 153 | //! Translation from Map to Base in Map frame (M_r_MB) 154 | kindr::Position3D translationMapToBaseInMapFrame_; 155 | 156 | //! Transformation from Sensor to Map frame 157 | Eigen::Affine3d transformationSensorToMap_; 158 | 159 | GeneralParameters generalParameters_; 160 | 161 | //! TF frame id of the range sensor for the point clouds. 162 | std::string sensorFrameId_; 163 | 164 | //! Ignore points above this height in map frame. 165 | double ignorePointsUpperThreshold_; 166 | 167 | //! Ignore points below this height in map frame. 168 | double ignorePointsLowerThreshold_; 169 | 170 | //! Sensor parameters. 171 | std::unordered_map sensorParameters_; 172 | 173 | //! Use VoxelGrid filter to cleanup pointcloud if true. 174 | bool applyVoxelGridFilter_; 175 | 176 | //! Indicates if the requested tf transformation was available. 177 | bool firstTfAvailable_; 178 | }; 179 | 180 | } /* namespace elevation_mapping */ 181 | -------------------------------------------------------------------------------- /elevation_mapping/include/elevation_mapping/sensor_processors/StereoSensorProcessor.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * StereoSensorProcessor.cpp 3 | * 4 | * Created on: Jun 6, 2014 5 | * Author: Hannes Keller 6 | */ 7 | 8 | #pragma once 9 | 10 | #include "elevation_mapping/PointXYZRGBConfidenceRatio.hpp" 11 | #include "elevation_mapping/sensor_processors/SensorProcessorBase.hpp" 12 | 13 | namespace elevation_mapping { 14 | 15 | /*! 16 | * Sensor processor for stereo camera sensors. 17 | * Cleans the point cloud, transforms it to a desired frame, and 18 | * computes the measurement variances based on a sensor model in 19 | * the desired frame. 20 | * 21 | * "Localization and Path Planning of a Climbing Robot for Corrosion Monitoring", Hannes Keller, Semester Project, ETH Zurich, 2014. 22 | */ 23 | 24 | class StereoSensorProcessor : public SensorProcessorBase { 25 | public: 26 | /*! 27 | * Constructor. 28 | * @param nodeHandle the ROS node handle. 29 | */ 30 | StereoSensorProcessor(ros::NodeHandle& nodeHandle, const SensorProcessorBase::GeneralParameters& generalParameters); 31 | 32 | /*! 33 | * Destructor. 34 | */ 35 | ~StereoSensorProcessor() override; 36 | 37 | private: 38 | /*! 39 | * Reads and verifies the parameters. 40 | * @return true if successful. 41 | */ 42 | bool readParameters() override; 43 | 44 | /*! 45 | * Computes the elevation map height variances for each point in a point cloud with the 46 | * sensor model and the robot pose covariance. 47 | * @param[in] pointCloud the point cloud for which the variances are computed. 48 | * @param[in] robotPoseCovariance the robot pose covariance matrix. 49 | * @param[out] variances the elevation map height variances. 50 | * @return true if successful. 51 | */ 52 | bool computeVariances(const PointCloudType::ConstPtr pointCloud, const Eigen::Matrix& robotPoseCovariance, 53 | Eigen::VectorXf& variances) override; 54 | 55 | /*! 56 | * Cuts off points that are not within the cutoff interval 57 | * @param pointCloud the point cloud to filter. 58 | * @return true if successful. 59 | */ 60 | bool filterPointCloudSensorType(const PointCloudType::Ptr pointCloud) override; 61 | 62 | //! Helper functions to get i-j indices out of a single index. 63 | int getI(int index); 64 | int getJ(int index); 65 | 66 | //! Stores 'original' point cloud indices of the cleaned point cloud. 67 | std::vector indices_; 68 | int originalWidth_; 69 | }; 70 | 71 | } // namespace elevation_mapping 72 | -------------------------------------------------------------------------------- /elevation_mapping/include/elevation_mapping/sensor_processors/StructuredLightSensorProcessor.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * StructuredLightSensorProcessor.hpp 3 | * 4 | * Created on: Feb 5, 2014 5 | * Author: Péter Fankhauser 6 | * Institute: ETH Zurich, ANYbotics 7 | */ 8 | 9 | #pragma once 10 | 11 | #include 12 | #include "elevation_mapping/PointXYZRGBConfidenceRatio.hpp" 13 | 14 | namespace elevation_mapping { 15 | 16 | /*! 17 | * Sensor processor for StructuredLight-type (PrimeSense) structured light sensors. 18 | */ 19 | class StructuredLightSensorProcessor : public SensorProcessorBase { 20 | public: 21 | /*! 22 | * Constructor. 23 | * @param nodeHandle the ROS node handle. 24 | */ 25 | StructuredLightSensorProcessor(ros::NodeHandle& nodeHandle, const SensorProcessorBase::GeneralParameters& generalParameters); 26 | 27 | /*! 28 | * Destructor. 29 | */ 30 | ~StructuredLightSensorProcessor() override; 31 | 32 | private: 33 | /*! 34 | * Reads and verifies the parameters. 35 | * @return true if successful. 36 | */ 37 | bool readParameters() override; 38 | 39 | /*! 40 | * Computes the elevation map height variances for each point in a point cloud with the 41 | * sensor model and the robot pose covariance. 42 | * @param[in] pointCloud the point cloud for which the variances are computed. 43 | * @param[in] robotPoseCovariance the robot pose covariance matrix. 44 | * @param[out] variances the elevation map height variances. 45 | * @return true if successful. 46 | */ 47 | bool computeVariances(const PointCloudType::ConstPtr pointCloud, const Eigen::Matrix& robotPoseCovariance, 48 | Eigen::VectorXf& variances) override; 49 | 50 | /*! 51 | * Cuts off points that are not within the cutoff interval 52 | * @param pointCloud the point cloud to filter. 53 | * @return true if successful. 54 | */ 55 | bool filterPointCloudSensorType(const PointCloudType::Ptr pointCloud) override; 56 | }; 57 | } /* namespace elevation_mapping */ 58 | -------------------------------------------------------------------------------- /elevation_mapping/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | elevation_mapping 4 | 0.7.2 5 | Generates an elevation map from distance measurements and robot pose information. 6 | Maximilian Wulf 7 | Yoshua Nava 8 | BSD 9 | http://github.com/anybotics/elevation_mapping 10 | http://github.com/anybotics/elevation_mapping/issues 11 | Péter Fankhauser 12 | 13 | catkin 14 | 15 | 16 | 17 | boost 18 | eigen 19 | eigen_conversions 20 | grid_map_core 21 | grid_map_ros 22 | grid_map_msgs 23 | kindr 24 | kindr_ros 25 | message_filters 26 | pcl_ros 27 | roscpp 28 | sensor_msgs 29 | std_msgs 30 | tf 31 | tf_conversions 32 | 33 | 34 | grid_map_filters 35 | gtest 36 | roslaunch 37 | rostest 38 | 39 | 40 | -------------------------------------------------------------------------------- /elevation_mapping/src/PointXYZRGBConfidenceRatio.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * PointXYZRGBConfidenceRatio.cpp 3 | * 4 | * Created on: Nov 26, 2020 5 | * Author: Magnus Gärtner 6 | * Institute: ETH Zurich, ANYbotics 7 | * 8 | * This file contains explicit definitions of algorithms used with our custom pcl type. 9 | */ 10 | 11 | #define PCL_NO_PRECOMPILE 12 | #include "elevation_mapping/PointXYZRGBConfidenceRatio.hpp" 13 | 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | 21 | template class pcl::PointCloud; 22 | template class pcl::PCLBase; 23 | template class pcl::VoxelGrid; 24 | template void pcl::removeNaNFromPointCloud( 25 | const pcl::PointCloud& cloud_in, pcl::PointCloud& cloud_out, 26 | std::vector >& index); 27 | template class pcl::ExtractIndices; 28 | template class pcl::PassThrough; 29 | 30 | std::ostream& operator<<(std::ostream& os, const pcl::PointXYZRGBConfidenceRatio& p) { 31 | os << "(" << p.x << "," << p.y << "," << p.z << " - " << static_cast(p.r) << "," << static_cast(p.g) << "," 32 | << static_cast(p.b) << " - " << p.confidence_ratio << ")"; 33 | return (os); 34 | } -------------------------------------------------------------------------------- /elevation_mapping/src/RobotMotionMapUpdater.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * RobotMotionMapUpdater.cpp 3 | * 4 | * Created on: Feb 5, 2014 5 | * Author: Péter Fankhauser 6 | * Institute: ETH Zurich, ANYbotics 7 | */ 8 | #include "elevation_mapping/RobotMotionMapUpdater.hpp" 9 | 10 | // Kindr 11 | #include 12 | 13 | // using namespace kindr; 14 | 15 | namespace elevation_mapping { 16 | 17 | RobotMotionMapUpdater::RobotMotionMapUpdater(ros::NodeHandle nodeHandle) : nodeHandle_(nodeHandle), covarianceScale_(1.0) { 18 | previousReducedCovariance_.setZero(); 19 | previousUpdateTime_ = ros::Time::now(); 20 | // TODO(max): How to initialize previousRobotPose_? 21 | } 22 | 23 | RobotMotionMapUpdater::~RobotMotionMapUpdater() = default; 24 | 25 | bool RobotMotionMapUpdater::readParameters() { 26 | nodeHandle_.param("robot_motion_map_update/covariance_scale", covarianceScale_, 1.0); 27 | return true; 28 | } 29 | 30 | bool RobotMotionMapUpdater::update(ElevationMap& map, const Pose& robotPose, const PoseCovariance& robotPoseCovariance, 31 | const ros::Time& time) { 32 | const PoseCovariance robotPoseCovarianceScaled = covarianceScale_ * robotPoseCovariance; 33 | 34 | // Check if update necessary. 35 | if (previousUpdateTime_ == time) { 36 | return false; 37 | } 38 | 39 | // Initialize update data. 40 | grid_map::Size size = map.getRawGridMap().getSize(); 41 | grid_map::Matrix varianceUpdate(size(0), size(1)); // TODO(max): Make as grid map? 42 | grid_map::Matrix horizontalVarianceUpdateX(size(0), size(1)); 43 | grid_map::Matrix horizontalVarianceUpdateY(size(0), size(1)); 44 | grid_map::Matrix horizontalVarianceUpdateXY(size(0), size(1)); 45 | 46 | // Relative convariance matrix between two robot poses. 47 | ReducedCovariance reducedCovariance; 48 | computeReducedCovariance(robotPose, robotPoseCovarianceScaled, reducedCovariance); 49 | ReducedCovariance relativeCovariance; 50 | computeRelativeCovariance(robotPose, reducedCovariance, relativeCovariance); 51 | 52 | // Retrieve covariances for (24). 53 | Covariance positionCovariance = relativeCovariance.topLeftCorner<3, 3>(); 54 | Covariance rotationCovariance(Covariance::Zero()); 55 | rotationCovariance(2, 2) = relativeCovariance(3, 3); 56 | 57 | // Map to robot pose rotation (R_B_M = R_I_B^T * R_I_M). 58 | kindr::RotationMatrixPD mapToRobotRotation = kindr::RotationMatrixPD(robotPose.getRotation().inverted() * map.getPose().getRotation()); 59 | kindr::RotationMatrixPD mapToPreviousRobotRotationInverted = 60 | kindr::RotationMatrixPD(previousRobotPose_.getRotation().inverted() * map.getPose().getRotation()).inverted(); 61 | 62 | // Translation Jacobian (J_r) (25). 63 | Eigen::Matrix3d translationJacobian = -mapToRobotRotation.matrix().transpose(); 64 | 65 | // Translation variance update (for all points the same). 66 | Eigen::Vector3f translationVarianceUpdate = 67 | (translationJacobian * positionCovariance * translationJacobian.transpose()).diagonal().cast(); 68 | 69 | // Map-robot relative position (M_r_Bk_M, for all points the same). 70 | // Preparation for (25): M_r_BP = R_I_M^T (I_r_I_M - I_r_I_B) + M_r_M_P 71 | // R_I_M^T (I_r_I_M - I_r_I_B): 72 | const kindr::Position3D positionRobotToMap = 73 | map.getPose().getRotation().inverseRotate(map.getPose().getPosition() - previousRobotPose_.getPosition()); 74 | 75 | auto& heightLayer = map.getRawGridMap()["elevation"]; 76 | 77 | // For each cell in map. // TODO(max): Change to new iterator. 78 | for (unsigned int i = 0; i < static_cast(size(0)); ++i) { 79 | for (unsigned int j = 0; j < static_cast(size(1)); ++j) { 80 | kindr::Position3D cellPosition; // M_r_MP 81 | 82 | const auto height = heightLayer(i, j); 83 | if (std::isfinite(height)) { 84 | grid_map::Position position; 85 | map.getRawGridMap().getPosition({i, j}, position); 86 | cellPosition = {position.x(), position.y(), height}; 87 | 88 | // Rotation Jacobian J_R (25) 89 | const Eigen::Matrix3d rotationJacobian = 90 | -kindr::getSkewMatrixFromVector((positionRobotToMap + cellPosition).vector()) * mapToPreviousRobotRotationInverted.matrix(); 91 | 92 | // Rotation variance update. 93 | const Eigen::Matrix2f rotationVarianceUpdate = 94 | (rotationJacobian * rotationCovariance * rotationJacobian.transpose()).topLeftCorner<2, 2>().cast(); 95 | 96 | // Variance update. 97 | varianceUpdate(i, j) = translationVarianceUpdate.z(); 98 | horizontalVarianceUpdateX(i, j) = translationVarianceUpdate.x() + rotationVarianceUpdate(0, 0); 99 | horizontalVarianceUpdateY(i, j) = translationVarianceUpdate.y() + rotationVarianceUpdate(1, 1); 100 | horizontalVarianceUpdateXY(i, j) = rotationVarianceUpdate(0, 1); 101 | } else { 102 | // Cell invalid. // TODO(max): Change to new functions 103 | varianceUpdate(i, j) = std::numeric_limits::infinity(); 104 | horizontalVarianceUpdateX(i, j) = std::numeric_limits::infinity(); 105 | horizontalVarianceUpdateY(i, j) = std::numeric_limits::infinity(); 106 | horizontalVarianceUpdateXY(i, j) = std::numeric_limits::infinity(); 107 | } 108 | } 109 | } 110 | 111 | map.update(varianceUpdate, horizontalVarianceUpdateX, horizontalVarianceUpdateY, horizontalVarianceUpdateXY, time); 112 | previousReducedCovariance_ = reducedCovariance; 113 | previousRobotPose_ = robotPose; 114 | return true; 115 | } 116 | 117 | bool RobotMotionMapUpdater::computeReducedCovariance(const Pose& robotPose, const PoseCovariance& robotPoseCovariance, 118 | ReducedCovariance& reducedCovariance) { 119 | // Get augmented Jacobian (A.4). 120 | kindr::EulerAnglesZyxPD eulerAngles(robotPose.getRotation()); 121 | double tanOfPitch = tan(eulerAngles.pitch()); 122 | // (A.5) 123 | Eigen::Matrix yawJacobian(cos(eulerAngles.yaw()) * tanOfPitch, sin(eulerAngles.yaw()) * tanOfPitch, 1.0); 124 | Eigen::Matrix jacobian; 125 | jacobian.setZero(); 126 | jacobian.topLeftCorner(3, 3).setIdentity(); 127 | jacobian.bottomRightCorner(1, 3) = yawJacobian; 128 | 129 | // (A.3) 130 | reducedCovariance = jacobian * robotPoseCovariance * jacobian.transpose(); 131 | return true; 132 | } 133 | 134 | bool RobotMotionMapUpdater::computeRelativeCovariance(const Pose& robotPose, const ReducedCovariance& reducedCovariance, 135 | ReducedCovariance& relativeCovariance) { 136 | // Rotation matrix of z-align frame R_I_tilde_B. 137 | const kindr::RotationVectorPD rotationVector_I_B(robotPose.getRotation()); 138 | const kindr::RotationVectorPD rotationVector_I_tilde_B(0.0, 0.0, rotationVector_I_B.vector().z()); 139 | const kindr::RotationMatrixPD R_I_tilde_B(rotationVector_I_tilde_B); 140 | 141 | // Compute translational velocity from finite differences. 142 | kindr::Position3D positionInRobotFrame = 143 | previousRobotPose_.getRotation().inverseRotate(robotPose.getPosition() - previousRobotPose_.getPosition()); 144 | kindr::Velocity3D v_Delta_t(positionInRobotFrame); // (A.8) 145 | 146 | // Jacobian F (A.8). 147 | Jacobian F; 148 | F.setIdentity(); 149 | // TODO(max): Why does Eigen::Vector3d::UnitZ() not work? 150 | F.topRightCorner(3, 1) = kindr::getSkewMatrixFromVector(Eigen::Vector3d(0.0, 0.0, 1.0)) * R_I_tilde_B.matrix() * v_Delta_t.vector(); 151 | 152 | // Jacobian inv(G) * Delta t (A.14). 153 | Jacobian inv_G_Delta_t; 154 | inv_G_Delta_t.setZero(); 155 | inv_G_Delta_t(3, 3) = 1.0; 156 | Jacobian inv_G_transpose_Delta_t(inv_G_Delta_t); 157 | inv_G_Delta_t.topLeftCorner(3, 3) = R_I_tilde_B.matrix().transpose(); 158 | inv_G_transpose_Delta_t.topLeftCorner(3, 3) = R_I_tilde_B.matrix(); 159 | 160 | // Relative (reduced) robot covariance (A.13). 161 | relativeCovariance = inv_G_Delta_t * (reducedCovariance - F * previousReducedCovariance_ * F.transpose()) * inv_G_transpose_Delta_t; 162 | 163 | return true; 164 | } 165 | 166 | } // namespace elevation_mapping 167 | -------------------------------------------------------------------------------- /elevation_mapping/src/elevation_mapping_node.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * elevation_map_node.cpp 3 | * 4 | * Created on: Oct 3, 2013 5 | * Author: Péter Fankhauser 6 | * Institute: ETH Zurich, ANYbotics 7 | */ 8 | 9 | #include 10 | #include "elevation_mapping/ElevationMapping.hpp" 11 | 12 | int main(int argc, char** argv) { 13 | ros::init(argc, argv, "elevation_mapping"); 14 | ros::NodeHandle nodeHandle("~"); 15 | elevation_mapping::ElevationMapping elevationMap(nodeHandle); 16 | 17 | // Spin 18 | ros::AsyncSpinner spinner(nodeHandle.param("num_callback_threads", 1)); // Use n threads 19 | spinner.start(); 20 | ros::waitForShutdown(); 21 | return 0; 22 | } 23 | -------------------------------------------------------------------------------- /elevation_mapping/src/input_sources/Input.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Input.cpp 3 | * 4 | * Created on: Oct 06, 2020 5 | * Author: Magnus Gärtner 6 | * Institute: ETH Zurich, ANYbotics 7 | */ 8 | 9 | #include "elevation_mapping/input_sources/Input.hpp" 10 | 11 | #include "elevation_mapping/sensor_processors/LaserSensorProcessor.hpp" 12 | #include "elevation_mapping/sensor_processors/PerfectSensorProcessor.hpp" 13 | #include "elevation_mapping/sensor_processors/StereoSensorProcessor.hpp" 14 | #include "elevation_mapping/sensor_processors/StructuredLightSensorProcessor.hpp" 15 | 16 | namespace elevation_mapping { 17 | 18 | Input::Input(ros::NodeHandle nh) : nodeHandle_(nh), queueSize_(0), publishOnUpdate_(true) {} 19 | 20 | bool Input::configure(std::string name, const XmlRpc::XmlRpcValue& parameters, 21 | const SensorProcessorBase::GeneralParameters& generalSensorProcessorParameters) { 22 | // Configuration Guards. 23 | if (parameters.getType() != XmlRpc::XmlRpcValue::TypeStruct) { 24 | ROS_ERROR( 25 | "Input source must be specified as map, but is " 26 | "XmlRpcType:%d.", 27 | parameters.getType()); 28 | return false; 29 | } 30 | 31 | // Check that parameters exist and has an appropriate type. 32 | using nameAndType = std::pair; 33 | for (const nameAndType& member : std::vector{{"type", XmlRpc::XmlRpcValue::TypeString}, 34 | {"topic", XmlRpc::XmlRpcValue::TypeString}, 35 | {"queue_size", XmlRpc::XmlRpcValue::TypeInt}, 36 | {"publish_on_update", XmlRpc::XmlRpcValue::TypeBoolean}, 37 | {"sensor_processor", XmlRpc::XmlRpcValue::TypeStruct}}) { 38 | if (!parameters.hasMember(member.first)) { 39 | ROS_ERROR("Could not configure input source %s because no %s was given.", name.c_str(), member.first.c_str()); 40 | return false; 41 | } 42 | if (parameters[member.first].getType() != member.second) { 43 | ROS_ERROR( 44 | "Could not configure input source %s because member %s has the " 45 | "wrong type.", 46 | name.c_str(), member.first.c_str()); 47 | return false; 48 | } 49 | } 50 | 51 | name_ = name; 52 | type_ = static_cast(parameters["type"]); 53 | topic_ = static_cast(parameters["topic"]); 54 | const int& queueSize = static_cast(parameters["queue_size"]); 55 | if (queueSize >= 0) { 56 | queueSize_ = static_cast(queueSize); 57 | } else { 58 | ROS_ERROR("The specified queue_size is negative."); 59 | return false; 60 | } 61 | publishOnUpdate_ = static_cast(parameters["publish_on_update"]); 62 | 63 | // SensorProcessor 64 | if (!configureSensorProcessor(name, parameters, generalSensorProcessorParameters)) { 65 | return false; 66 | } 67 | 68 | ROS_DEBUG("Configured %s:%s @ %s (publishing_on_update: %s), using %s to process data.\n", type_.c_str(), name_.c_str(), 69 | nodeHandle_.resolveName(topic_).c_str(), publishOnUpdate_ ? "true" : "false", 70 | static_cast(parameters["sensor_processor"]["type"]).c_str()); 71 | return true; 72 | } 73 | 74 | std::string Input::getSubscribedTopic() const { 75 | return nodeHandle_.resolveName(topic_); 76 | } 77 | 78 | bool Input::configureSensorProcessor(std::string name, const XmlRpc::XmlRpcValue& parameters, 79 | const SensorProcessorBase::GeneralParameters& generalSensorProcessorParameters) { 80 | if (!parameters["sensor_processor"].hasMember("type")) { 81 | ROS_ERROR("Could not configure sensor processor of input source %s because no type was given.", name.c_str()); 82 | return false; 83 | } 84 | if (parameters["sensor_processor"]["type"].getType() != XmlRpc::XmlRpcValue::TypeString) { 85 | ROS_ERROR( 86 | "Could not configure sensor processor of input source %s because the member 'type' has the " 87 | "wrong type.", 88 | name.c_str()); 89 | return false; 90 | } 91 | std::string sensorType = static_cast(parameters["sensor_processor"]["type"]); 92 | if (sensorType == "structured_light") { 93 | sensorProcessor_.reset(new StructuredLightSensorProcessor(nodeHandle_, generalSensorProcessorParameters)); 94 | } else if (sensorType == "stereo") { 95 | sensorProcessor_.reset(new StereoSensorProcessor(nodeHandle_, generalSensorProcessorParameters)); 96 | } else if (sensorType == "laser") { 97 | sensorProcessor_.reset(new LaserSensorProcessor(nodeHandle_, generalSensorProcessorParameters)); 98 | } else if (sensorType == "perfect") { 99 | sensorProcessor_.reset(new PerfectSensorProcessor(nodeHandle_, generalSensorProcessorParameters)); 100 | } else { 101 | ROS_ERROR("The sensor type %s is not available.", sensorType.c_str()); 102 | return false; 103 | } 104 | 105 | if (!sensorProcessor_->readParameters()) { 106 | return false; 107 | } 108 | 109 | return true; 110 | } 111 | 112 | } // namespace elevation_mapping -------------------------------------------------------------------------------- /elevation_mapping/src/input_sources/InputSourceManager.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * InputSourceManager.cpp 3 | * 4 | * Created on: Oct 02, 2020 5 | * Author: Magnus Gärtner 6 | * Institute: ETH Zurich, ANYbotics 7 | */ 8 | 9 | #include "elevation_mapping/input_sources/InputSourceManager.hpp" 10 | #include "elevation_mapping/ElevationMapping.hpp" 11 | 12 | namespace elevation_mapping { 13 | 14 | InputSourceManager::InputSourceManager(const ros::NodeHandle& nodeHandle) : nodeHandle_(nodeHandle) {} 15 | 16 | bool InputSourceManager::configureFromRos(const std::string& inputSourcesNamespace) { 17 | XmlRpc::XmlRpcValue inputSourcesConfiguration; 18 | if (!nodeHandle_.getParam(inputSourcesNamespace, inputSourcesConfiguration)) { 19 | ROS_WARN( 20 | "Could not load the input sources configuration from parameter\n " 21 | "%s, are you sure it was pushed to the parameter server? Assuming\n " 22 | "that you meant to leave it empty. Not subscribing to any inputs!\n", 23 | nodeHandle_.resolveName(inputSourcesNamespace).c_str()); 24 | return false; 25 | } 26 | return configure(inputSourcesConfiguration, inputSourcesNamespace); 27 | } 28 | 29 | bool InputSourceManager::configure(const XmlRpc::XmlRpcValue& config, const std::string& sourceConfigurationName) { 30 | if (config.getType() == XmlRpc::XmlRpcValue::TypeArray && 31 | config.size() == 0) { // Use Empty array as special case to explicitly configure no inputs. 32 | return true; 33 | } 34 | 35 | if (config.getType() != XmlRpc::XmlRpcValue::TypeStruct) { 36 | ROS_ERROR( 37 | "%s: The input sources specification must be a struct. but is of " 38 | "of XmlRpcType %d", 39 | sourceConfigurationName.c_str(), config.getType()); 40 | ROS_ERROR("The xml passed in is formatted as follows:\n %s", config.toXml().c_str()); 41 | return false; 42 | } 43 | 44 | bool successfulConfiguration = true; 45 | std::set subscribedTopics; 46 | SensorProcessorBase::GeneralParameters generalSensorProcessorConfig{nodeHandle_.param("robot_base_frame_id", std::string("/robot")), 47 | nodeHandle_.param("map_frame_id", std::string("/map"))}; 48 | // Configure all input sources in the list. 49 | for (auto& inputConfig : config) { 50 | Input source = Input(ros::NodeHandle(nodeHandle_.resolveName(sourceConfigurationName + "/" + inputConfig.first))); 51 | 52 | bool configured = source.configure(inputConfig.first, inputConfig.second, generalSensorProcessorConfig); 53 | if (!configured) { 54 | successfulConfiguration = false; 55 | continue; 56 | } 57 | 58 | std::string subscribedTopic = source.getSubscribedTopic(); 59 | bool topicIsUnique = subscribedTopics.insert(subscribedTopic).second; 60 | 61 | if (topicIsUnique) { 62 | sources_.push_back(std::move(source)); 63 | } else { 64 | ROS_WARN( 65 | "The input sources specification tried to subscribe to %s " 66 | "multiple times. Only subscribing once.", 67 | subscribedTopic.c_str()); 68 | successfulConfiguration = false; 69 | } 70 | } 71 | 72 | return successfulConfiguration; 73 | } 74 | 75 | int InputSourceManager::getNumberOfSources() { 76 | return static_cast(sources_.size()); 77 | } 78 | 79 | } // namespace elevation_mapping -------------------------------------------------------------------------------- /elevation_mapping/src/postprocessing/PostprocessingPipelineFunctor.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * PostprocessingPipelineFunctor.cpp 3 | * 4 | * Created on: Sep. 14, 2020 5 | * Author: Magnus Gärtner 6 | * Institute: ETH Zurich, ANYbotics 7 | * 8 | * Note. Large parts are adopted from grid_map_demos/FiltersDemo.cpp. 9 | */ 10 | 11 | #include 12 | 13 | #include "elevation_mapping/postprocessing/PostprocessingPipelineFunctor.hpp" 14 | 15 | namespace elevation_mapping { 16 | 17 | PostprocessingPipelineFunctor::PostprocessingPipelineFunctor(ros::NodeHandle& nodeHandle) 18 | : nodeHandle_(nodeHandle), filterChain_("grid_map::GridMap"), filterChainConfigured_(false) { 19 | // TODO (magnus) Add logic when setting up failed. What happens actually if it is not configured? 20 | readParameters(); 21 | 22 | publisher_ = nodeHandle_.advertise(outputTopic_, 1, true); 23 | 24 | // Setup filter chain. 25 | if (!nodeHandle.hasParam(filterChainParametersName_) || !filterChain_.configure(filterChainParametersName_, nodeHandle)) { 26 | ROS_WARN("Could not configure the filter chain. Will publish the raw elevation map without postprocessing!"); 27 | return; 28 | } 29 | 30 | filterChainConfigured_ = true; 31 | } 32 | 33 | PostprocessingPipelineFunctor::~PostprocessingPipelineFunctor() = default; 34 | 35 | void PostprocessingPipelineFunctor::readParameters() { 36 | nodeHandle_.param("output_topic", outputTopic_, std::string("elevation_map_raw")); 37 | nodeHandle_.param("postprocessor_pipeline_name", filterChainParametersName_, std::string("postprocessor_pipeline")); 38 | } 39 | 40 | grid_map::GridMap PostprocessingPipelineFunctor::operator()(GridMap& inputMap) { 41 | if (not filterChainConfigured_) { 42 | ROS_WARN_ONCE("No postprocessing pipeline was configured. Forwarding the raw elevation map!"); 43 | return inputMap; 44 | } 45 | 46 | grid_map::GridMap outputMap; 47 | if (not filterChain_.update(inputMap, outputMap)) { 48 | ROS_ERROR("Could not perform the grid map filter chain! Forwarding the raw elevation map!"); 49 | return inputMap; 50 | } 51 | 52 | return outputMap; 53 | } 54 | 55 | void PostprocessingPipelineFunctor::publish(const GridMap& gridMap) const { 56 | // Publish filtered output grid map. 57 | grid_map_msgs::GridMap outputMessage; 58 | grid_map::GridMapRosConverter::toMessage(gridMap, outputMessage); 59 | publisher_.publish(outputMessage); 60 | ROS_DEBUG("Elevation map raw has been published."); 61 | } 62 | 63 | bool PostprocessingPipelineFunctor::hasSubscribers() const { 64 | return publisher_.getNumSubscribers() > 0; 65 | } 66 | 67 | } // namespace elevation_mapping 68 | -------------------------------------------------------------------------------- /elevation_mapping/src/postprocessing/PostprocessingWorker.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * PostprocessingWorker.cpp 3 | * 4 | * Created on: Dec. 21, 2020 5 | * Author: Yoshua Nava 6 | * Institute: ANYbotics 7 | */ 8 | 9 | #include "elevation_mapping/postprocessing/PostprocessingWorker.hpp" 10 | 11 | namespace elevation_mapping { 12 | 13 | PostprocessingWorker::PostprocessingWorker(ros::NodeHandle nodeHandle) 14 | : functor_(nodeHandle), work_(ioService_), thread_(boost::bind(&boost::asio::io_service::run, &ioService_)) {} 15 | 16 | PostprocessingWorker::GridMap PostprocessingWorker::processBuffer() { 17 | return functor_(dataBuffer_); 18 | } 19 | 20 | void PostprocessingWorker::publish(const GridMap& gridMap) const { 21 | functor_.publish(gridMap); 22 | } 23 | 24 | bool PostprocessingWorker::hasSubscribers() const { 25 | return functor_.hasSubscribers(); 26 | } 27 | 28 | } // namespace elevation_mapping 29 | -------------------------------------------------------------------------------- /elevation_mapping/src/postprocessing/PostprocessorPool.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * PostprocessorPool.cpp 3 | * 4 | * Created on: Sep. 14, 2020 5 | * Author: Magnus Gärtner 6 | * Institute: ETH Zurich, ANYbotics 7 | */ 8 | 9 | #include "elevation_mapping/postprocessing/PostprocessorPool.hpp" 10 | 11 | namespace elevation_mapping { 12 | 13 | PostprocessorPool::PostprocessorPool(std::size_t poolSize, ros::NodeHandle nodeHandle) { 14 | for (std::size_t i = 0; i < poolSize; ++i) { 15 | // Add worker to the collection. 16 | workers_.emplace_back(std::make_unique(nodeHandle)); 17 | // Create one service per thread 18 | availableServices_.push_back(i); 19 | } 20 | } 21 | 22 | PostprocessorPool::~PostprocessorPool() { 23 | // Force all threads to return from io_service::run(). 24 | for (auto& worker : workers_) { 25 | worker->ioService().stop(); 26 | } 27 | 28 | // Suppress all exceptions. Try to join every worker thread. 29 | for (auto& worker : workers_) { 30 | try { 31 | if (worker->thread().joinable()) { 32 | worker->thread().join(); 33 | } 34 | } catch (const std::exception&) { 35 | } 36 | } 37 | } 38 | 39 | bool PostprocessorPool::runTask(const GridMap& gridMap) { 40 | // Get an available service id from the shared services pool in a mutually exclusive manner. 41 | size_t serviceIndex; 42 | { 43 | boost::lock_guard lock(availableServicesMutex_); 44 | if (availableServices_.empty()) { 45 | return false; 46 | } 47 | serviceIndex = availableServices_.back(); 48 | availableServices_.pop_back(); 49 | } 50 | 51 | // Copy data to the buffer for the worker. 52 | workers_.at(serviceIndex)->setDataBuffer(gridMap); 53 | 54 | // Create a task with the post-processor and dispatch it. 55 | auto task = std::bind(&PostprocessorPool::wrapTask, this, serviceIndex); 56 | workers_.at(serviceIndex)->ioService().post(task); 57 | return true; 58 | } 59 | 60 | void PostprocessorPool::wrapTask(size_t serviceIndex) { 61 | // Run the user supplied task. 62 | try { 63 | GridMap postprocessedMap = workers_.at(serviceIndex)->processBuffer(); 64 | workers_.at(serviceIndex)->publish(postprocessedMap); 65 | } 66 | // Suppress all exceptions. 67 | catch (const std::exception& exception) { 68 | ROS_ERROR_STREAM("Postprocessor pipeline, thread " << serviceIndex << " experienced an error: " << exception.what()); 69 | } 70 | 71 | // Task has finished, so increment count of available threads. 72 | boost::unique_lock lock(availableServicesMutex_); 73 | availableServices_.push_back(serviceIndex); 74 | } 75 | 76 | bool PostprocessorPool::pipelineHasSubscribers() const { 77 | return std::all_of(workers_.cbegin(), workers_.cend(), 78 | [](const std::unique_ptr& worker) { return worker->hasSubscribers(); }); 79 | } 80 | 81 | } // namespace elevation_mapping -------------------------------------------------------------------------------- /elevation_mapping/src/sensor_processors/LaserSensorProcessor.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * KinectSensorProcessor.cpp 3 | * 4 | * Created on: Feb 5, 2014 5 | * Author: Péter Fankhauser 6 | * Institute: ETH Zurich, ANYbotics 7 | */ 8 | 9 | #include "elevation_mapping/sensor_processors/LaserSensorProcessor.hpp" 10 | 11 | #include 12 | #include 13 | #include 14 | #include 15 | 16 | #include "elevation_mapping/PointXYZRGBConfidenceRatio.hpp" 17 | 18 | namespace elevation_mapping { 19 | 20 | /*! 21 | * Anisotropic laser range sensor model: 22 | * standardDeviationInBeamDirection = minRadius 23 | * standardDeviationOfBeamRadius = beamConstant + beamAngle * measurementDistance 24 | * 25 | * Taken from: Pomerleau, F.; Breitenmoser, A; Ming Liu; Colas, F.; Siegwart, R., 26 | * "Noise characterization of depth sensors for surface inspections," 27 | * International Conference on Applied Robotics for the Power Industry (CARPI), 2012. 28 | */ 29 | 30 | LaserSensorProcessor::LaserSensorProcessor(ros::NodeHandle& nodeHandle, const SensorProcessorBase::GeneralParameters& generalParameters) 31 | : SensorProcessorBase(nodeHandle, generalParameters) {} 32 | 33 | LaserSensorProcessor::~LaserSensorProcessor() = default; 34 | 35 | bool LaserSensorProcessor::readParameters() { 36 | SensorProcessorBase::readParameters(); 37 | nodeHandle_.param("sensor_processor/min_radius", sensorParameters_["min_radius"], 0.0); 38 | nodeHandle_.param("sensor_processor/beam_angle", sensorParameters_["beam_angle"], 0.0); 39 | nodeHandle_.param("sensor_processor/beam_constant", sensorParameters_["beam_constant"], 0.0); 40 | return true; 41 | } 42 | 43 | bool LaserSensorProcessor::computeVariances(const PointCloudType::ConstPtr pointCloud, 44 | const Eigen::Matrix& robotPoseCovariance, Eigen::VectorXf& variances) { 45 | variances.resize(pointCloud->size()); 46 | 47 | // Projection vector (P). 48 | const Eigen::RowVector3f projectionVector = Eigen::RowVector3f::UnitZ(); 49 | 50 | // Sensor Jacobian (J_s). 51 | const Eigen::RowVector3f sensorJacobian = 52 | projectionVector * (rotationMapToBase_.transposed() * rotationBaseToSensor_.transposed()).toImplementation().cast(); 53 | 54 | // Robot rotation covariance matrix (Sigma_q). 55 | Eigen::Matrix3f rotationVariance = robotPoseCovariance.bottomRightCorner(3, 3).cast(); 56 | 57 | // Preparations for robot rotation Jacobian (J_q) to minimize computation for every point in point cloud. 58 | const Eigen::Matrix3f C_BM_transpose = rotationMapToBase_.transposed().toImplementation().cast(); 59 | const Eigen::RowVector3f P_mul_C_BM_transpose = projectionVector * C_BM_transpose; 60 | const Eigen::Matrix3f C_SB_transpose = rotationBaseToSensor_.transposed().toImplementation().cast(); 61 | const Eigen::Matrix3f B_r_BS_skew = 62 | kindr::getSkewMatrixFromVector(Eigen::Vector3f(translationBaseToSensorInBaseFrame_.toImplementation().cast())); 63 | 64 | const float varianceNormal = sensorParameters_.at("min_radius") * sensorParameters_.at("min_radius"); 65 | const float beamConstant = sensorParameters_.at("beam_constant"); 66 | const float beamAngle = sensorParameters_.at("beam_angle"); 67 | for (size_t i = 0; i < pointCloud->size(); ++i) { 68 | // TODO(needs assignment): Move this loop body into a function for better unit testing. 69 | // For every point in point cloud. 70 | 71 | // Preparation. 72 | const auto& point = pointCloud->points[i]; 73 | const Eigen::Vector3f pointVector(point.x, point.y, point.z); // S_r_SP // NOLINT(cppcoreguidelines-pro-type-union-access) 74 | 75 | // Measurement distance. 76 | const float measurementDistance = pointVector.norm(); 77 | 78 | // Compute sensor covariance matrix (Sigma_S) with sensor model. 79 | float varianceLateral = beamConstant + beamAngle * measurementDistance; 80 | varianceLateral *= varianceLateral; 81 | 82 | Eigen::Matrix3f sensorVariance = Eigen::Matrix3f::Zero(); 83 | sensorVariance.diagonal() << varianceLateral, varianceLateral, varianceNormal; 84 | 85 | // Robot rotation Jacobian (J_q). 86 | const Eigen::Matrix3f C_SB_transpose_times_S_r_SP_skew = kindr::getSkewMatrixFromVector(Eigen::Vector3f(C_SB_transpose * pointVector)); 87 | const Eigen::RowVector3f rotationJacobian = P_mul_C_BM_transpose * (C_SB_transpose_times_S_r_SP_skew + B_r_BS_skew); 88 | 89 | // Measurement variance for map (error propagation law). 90 | float heightVariance = 0.0; // sigma_p 91 | heightVariance = rotationJacobian * rotationVariance * rotationJacobian.transpose(); 92 | heightVariance += sensorJacobian * sensorVariance * sensorJacobian.transpose(); 93 | 94 | // Copy to list. 95 | variances(i) = heightVariance; 96 | } 97 | 98 | return true; 99 | } 100 | 101 | } // namespace elevation_mapping 102 | -------------------------------------------------------------------------------- /elevation_mapping/src/sensor_processors/PerfectSensorProcessor.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * PerfectSensorProcessor.cpp 3 | * 4 | * Created on: Sep 28, 2014 5 | * Author: Péter Fankhauser 6 | * Institute: ETH Zurich, ANYbotics 7 | */ 8 | 9 | #include "elevation_mapping/sensor_processors/PerfectSensorProcessor.hpp" 10 | 11 | // PCL 12 | #include 13 | 14 | // STD 15 | #include 16 | #include 17 | #include 18 | 19 | #include "elevation_mapping/PointXYZRGBConfidenceRatio.hpp" 20 | 21 | namespace elevation_mapping { 22 | 23 | /*! 24 | * Noiseless, perfect sensor. 25 | */ 26 | 27 | PerfectSensorProcessor::PerfectSensorProcessor(ros::NodeHandle& nodeHandle, const SensorProcessorBase::GeneralParameters& generalParameters) 28 | : SensorProcessorBase(nodeHandle, generalParameters) {} 29 | 30 | PerfectSensorProcessor::~PerfectSensorProcessor() = default; 31 | 32 | bool PerfectSensorProcessor::readParameters() { 33 | return SensorProcessorBase::readParameters(); 34 | } 35 | 36 | bool PerfectSensorProcessor::computeVariances(const PointCloudType::ConstPtr pointCloud, 37 | const Eigen::Matrix& robotPoseCovariance, Eigen::VectorXf& variances) { 38 | variances.resize(pointCloud->size()); 39 | 40 | // Projection vector (P). 41 | const Eigen::RowVector3f projectionVector = Eigen::RowVector3f::UnitZ(); 42 | 43 | // Sensor Jacobian (J_s). 44 | const Eigen::RowVector3f sensorJacobian = 45 | projectionVector * (rotationMapToBase_.transposed() * rotationBaseToSensor_.transposed()).toImplementation().cast(); 46 | 47 | // Robot rotation covariance matrix (Sigma_q). 48 | const Eigen::Matrix3f rotationVariance = robotPoseCovariance.bottomRightCorner(3, 3).cast(); 49 | 50 | // Preparations for robot rotation Jacobian (J_q) to minimize computation for every point in point cloud. 51 | const Eigen::Matrix3f C_BM_transpose = rotationMapToBase_.transposed().toImplementation().cast(); 52 | const Eigen::RowVector3f P_mul_C_BM_transpose = projectionVector * C_BM_transpose; 53 | const Eigen::Matrix3f C_SB_transpose = rotationBaseToSensor_.transposed().toImplementation().cast(); 54 | const Eigen::Matrix3f B_r_BS_skew = 55 | kindr::getSkewMatrixFromVector(Eigen::Vector3f(translationBaseToSensorInBaseFrame_.toImplementation().cast())); 56 | 57 | for (unsigned int i = 0; i < pointCloud->size(); ++i) { 58 | // For every point in point cloud. 59 | 60 | // Preparation. 61 | auto& point = pointCloud->points[i]; 62 | Eigen::Vector3f pointVector(point.x, point.y, point.z); // S_r_SP // NOLINT(cppcoreguidelines-pro-type-union-access) 63 | float heightVariance = 0.0; // sigma_p 64 | 65 | // Compute sensor covariance matrix (Sigma_S) with sensor model. 66 | float varianceNormal = 0.0; 67 | float varianceLateral = 0.0; 68 | Eigen::Matrix3f sensorVariance = Eigen::Matrix3f::Zero(); 69 | sensorVariance.diagonal() << varianceLateral, varianceLateral, varianceNormal; 70 | 71 | // Robot rotation Jacobian (J_q). 72 | const Eigen::Matrix3f C_SB_transpose_times_S_r_SP_skew = kindr::getSkewMatrixFromVector(Eigen::Vector3f(C_SB_transpose * pointVector)); 73 | const Eigen::RowVector3f rotationJacobian = P_mul_C_BM_transpose * (C_SB_transpose_times_S_r_SP_skew + B_r_BS_skew); 74 | 75 | // Measurement variance for map (error propagation law). 76 | heightVariance = rotationJacobian * rotationVariance * rotationJacobian.transpose(); 77 | heightVariance += sensorJacobian * sensorVariance * sensorJacobian.transpose(); 78 | 79 | // Copy to list. 80 | variances(i) = heightVariance; 81 | } 82 | 83 | return true; 84 | } 85 | 86 | } // namespace elevation_mapping 87 | -------------------------------------------------------------------------------- /elevation_mapping/src/sensor_processors/SensorProcessorBase.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * SensorProcessorBase.cpp 3 | * 4 | * Created on: Jun 6, 2014 5 | * Author: Péter Fankhauser, Hannes Keller 6 | * Institute: ETH Zurich, ANYbotics 7 | */ 8 | 9 | #include "elevation_mapping/sensor_processors/SensorProcessorBase.hpp" 10 | 11 | // PCL 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | 19 | // TF 20 | #include 21 | 22 | // STL 23 | #include 24 | #include 25 | #include 26 | 27 | #include "elevation_mapping/PointXYZRGBConfidenceRatio.hpp" 28 | 29 | namespace elevation_mapping { 30 | 31 | SensorProcessorBase::SensorProcessorBase(ros::NodeHandle& nodeHandle, const GeneralParameters& generalConfig) 32 | : nodeHandle_(nodeHandle), 33 | ignorePointsUpperThreshold_(std::numeric_limits::infinity()), 34 | ignorePointsLowerThreshold_(-std::numeric_limits::infinity()), 35 | applyVoxelGridFilter_(false), 36 | firstTfAvailable_(false) { 37 | pcl::console::setVerbosityLevel(pcl::console::L_ERROR); 38 | transformationSensorToMap_.setIdentity(); 39 | generalParameters_ = generalConfig; 40 | ROS_DEBUG( 41 | "Sensor processor general parameters are:" 42 | "\n\t- robot_base_frame_id: %s" 43 | "\n\t- map_frame_id: %s", 44 | generalConfig.robotBaseFrameId_.c_str(), generalConfig.mapFrameId_.c_str()); 45 | } 46 | 47 | SensorProcessorBase::~SensorProcessorBase() = default; 48 | 49 | bool SensorProcessorBase::readParameters() { 50 | nodeHandle_.param("sensor_processor/ignore_points_above", ignorePointsUpperThreshold_, std::numeric_limits::infinity()); 51 | nodeHandle_.param("sensor_processor/ignore_points_below", ignorePointsLowerThreshold_, -std::numeric_limits::infinity()); 52 | 53 | nodeHandle_.param("sensor_processor/apply_voxelgrid_filter", applyVoxelGridFilter_, false); 54 | nodeHandle_.param("sensor_processor/voxelgrid_filter_size", sensorParameters_["voxelgrid_filter_size"], 0.0); 55 | return true; 56 | } 57 | 58 | bool SensorProcessorBase::process(const PointCloudType::ConstPtr pointCloudInput, const Eigen::Matrix& robotPoseCovariance, 59 | const PointCloudType::Ptr pointCloudMapFrame, Eigen::VectorXf& variances, std::string sensorFrame) { 60 | sensorFrameId_ = sensorFrame; 61 | ROS_DEBUG("Sensor Processor processing for frame %s", sensorFrameId_.c_str()); 62 | 63 | // Update transformation at timestamp of pointcloud 64 | ros::Time timeStamp; 65 | timeStamp.fromNSec(1000 * pointCloudInput->header.stamp); 66 | if (!updateTransformations(timeStamp)) { 67 | return false; 68 | } 69 | 70 | // Transform into sensor frame. 71 | PointCloudType::Ptr pointCloudSensorFrame(new PointCloudType); 72 | transformPointCloud(pointCloudInput, pointCloudSensorFrame, sensorFrameId_); 73 | 74 | // Remove Nans (optional voxel grid filter) 75 | filterPointCloud(pointCloudSensorFrame); 76 | 77 | // Specific filtering per sensor type 78 | filterPointCloudSensorType(pointCloudSensorFrame); 79 | 80 | // Remove outside limits in map frame 81 | if (!transformPointCloud(pointCloudSensorFrame, pointCloudMapFrame, generalParameters_.mapFrameId_)) { 82 | return false; 83 | } 84 | std::vector pointClouds({pointCloudMapFrame, pointCloudSensorFrame}); 85 | removePointsOutsideLimits(pointCloudMapFrame, pointClouds); 86 | 87 | // Compute variances 88 | return computeVariances(pointCloudSensorFrame, robotPoseCovariance, variances); 89 | } 90 | 91 | bool SensorProcessorBase::updateTransformations(const ros::Time& timeStamp) { 92 | try { 93 | transformListener_.waitForTransform(sensorFrameId_, generalParameters_.mapFrameId_, timeStamp, ros::Duration(1.0)); 94 | 95 | tf::StampedTransform transformTf; 96 | transformListener_.lookupTransform(generalParameters_.mapFrameId_, sensorFrameId_, timeStamp, transformTf); 97 | poseTFToEigen(transformTf, transformationSensorToMap_); 98 | 99 | transformListener_.lookupTransform(generalParameters_.robotBaseFrameId_, sensorFrameId_, timeStamp, 100 | transformTf); // TODO(max): Why wrong direction? 101 | Eigen::Affine3d transform; 102 | poseTFToEigen(transformTf, transform); 103 | rotationBaseToSensor_.setMatrix(transform.rotation().matrix()); 104 | translationBaseToSensorInBaseFrame_.toImplementation() = transform.translation(); 105 | 106 | transformListener_.lookupTransform(generalParameters_.mapFrameId_, generalParameters_.robotBaseFrameId_, timeStamp, 107 | transformTf); // TODO(max): Why wrong direction? 108 | poseTFToEigen(transformTf, transform); 109 | rotationMapToBase_.setMatrix(transform.rotation().matrix()); 110 | translationMapToBaseInMapFrame_.toImplementation() = transform.translation(); 111 | 112 | if (!firstTfAvailable_) { 113 | firstTfAvailable_ = true; 114 | } 115 | 116 | return true; 117 | } catch (tf::TransformException& ex) { 118 | if (!firstTfAvailable_) { 119 | return false; 120 | } 121 | ROS_ERROR("%s", ex.what()); 122 | return false; 123 | } 124 | } 125 | 126 | bool SensorProcessorBase::transformPointCloud(PointCloudType::ConstPtr pointCloud, PointCloudType::Ptr pointCloudTransformed, 127 | const std::string& targetFrame) { 128 | ros::Time timeStamp; 129 | timeStamp.fromNSec(1000 * pointCloud->header.stamp); 130 | const std::string inputFrameId(pointCloud->header.frame_id); 131 | 132 | tf::StampedTransform transformTf; 133 | try { 134 | transformListener_.waitForTransform(targetFrame, inputFrameId, timeStamp, ros::Duration(1.0), ros::Duration(0.001)); 135 | transformListener_.lookupTransform(targetFrame, inputFrameId, timeStamp, transformTf); 136 | } catch (tf::TransformException& ex) { 137 | ROS_ERROR("%s", ex.what()); 138 | return false; 139 | } 140 | 141 | Eigen::Affine3d transform; 142 | poseTFToEigen(transformTf, transform); 143 | pcl::transformPointCloud(*pointCloud, *pointCloudTransformed, transform.cast()); 144 | pointCloudTransformed->header.frame_id = targetFrame; 145 | 146 | ROS_DEBUG_THROTTLE(5, "Point cloud transformed to frame %s for time stamp %f.", targetFrame.c_str(), 147 | pointCloudTransformed->header.stamp / 1000.0); 148 | return true; 149 | } 150 | 151 | void SensorProcessorBase::removePointsOutsideLimits(PointCloudType::ConstPtr reference, std::vector& pointClouds) { 152 | if (!std::isfinite(ignorePointsLowerThreshold_) && !std::isfinite(ignorePointsUpperThreshold_)) { 153 | return; 154 | } 155 | ROS_DEBUG("Limiting point cloud to the height interval of [%f, %f] relative to the robot base.", ignorePointsLowerThreshold_, 156 | ignorePointsUpperThreshold_); 157 | 158 | pcl::PassThrough passThroughFilter(true); 159 | passThroughFilter.setInputCloud(reference); 160 | passThroughFilter.setFilterFieldName("z"); // TODO(max): Should this be configurable? 161 | double relativeLowerThreshold = translationMapToBaseInMapFrame_.z() + ignorePointsLowerThreshold_; 162 | double relativeUpperThreshold = translationMapToBaseInMapFrame_.z() + ignorePointsUpperThreshold_; 163 | passThroughFilter.setFilterLimits(relativeLowerThreshold, relativeUpperThreshold); 164 | pcl::IndicesPtr insideIndeces(new std::vector); 165 | passThroughFilter.filter(*insideIndeces); 166 | 167 | for (auto& pointCloud : pointClouds) { 168 | pcl::ExtractIndices extractIndicesFilter; 169 | extractIndicesFilter.setInputCloud(pointCloud); 170 | extractIndicesFilter.setIndices(insideIndeces); 171 | PointCloudType tempPointCloud; 172 | extractIndicesFilter.filter(tempPointCloud); 173 | pointCloud->swap(tempPointCloud); 174 | } 175 | 176 | ROS_DEBUG("removePointsOutsideLimits() reduced point cloud to %i points.", (int)pointClouds[0]->size()); 177 | } 178 | 179 | bool SensorProcessorBase::filterPointCloud(const PointCloudType::Ptr pointCloud) { 180 | PointCloudType tempPointCloud; 181 | 182 | // Remove nan points. 183 | std::vector indices; 184 | if (!pointCloud->is_dense) { 185 | pcl::removeNaNFromPointCloud(*pointCloud, tempPointCloud, indices); 186 | tempPointCloud.is_dense = true; 187 | pointCloud->swap(tempPointCloud); 188 | } 189 | 190 | // Reduce points using VoxelGrid filter. 191 | if (applyVoxelGridFilter_) { 192 | pcl::VoxelGrid voxelGridFilter; 193 | voxelGridFilter.setInputCloud(pointCloud); 194 | double filter_size = sensorParameters_.at("voxelgrid_filter_size"); 195 | voxelGridFilter.setLeafSize(filter_size, filter_size, filter_size); 196 | voxelGridFilter.filter(tempPointCloud); 197 | pointCloud->swap(tempPointCloud); 198 | } 199 | ROS_DEBUG_THROTTLE(2, "cleanPointCloud() reduced point cloud to %i points.", static_cast(pointCloud->size())); 200 | return true; 201 | } 202 | 203 | bool SensorProcessorBase::filterPointCloudSensorType(const PointCloudType::Ptr /*pointCloud*/) { 204 | return true; 205 | } 206 | 207 | } /* namespace elevation_mapping */ 208 | -------------------------------------------------------------------------------- /elevation_mapping/src/sensor_processors/StereoSensorProcessor.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * StereoSensorProcessor.cpp 3 | * 4 | * Created on: Jun 6, 2014 5 | * Author: Hannes Keller 6 | */ 7 | 8 | #include "elevation_mapping/sensor_processors/StereoSensorProcessor.hpp" 9 | 10 | // PCL 11 | #include 12 | #include 13 | 14 | // STD 15 | #include 16 | 17 | #include "elevation_mapping/PointXYZRGBConfidenceRatio.hpp" 18 | 19 | namespace elevation_mapping { 20 | 21 | StereoSensorProcessor::StereoSensorProcessor(ros::NodeHandle& nodeHandle, const SensorProcessorBase::GeneralParameters& generalParameters) 22 | : SensorProcessorBase(nodeHandle, generalParameters), originalWidth_(1) {} 23 | 24 | StereoSensorProcessor::~StereoSensorProcessor() = default; 25 | 26 | bool StereoSensorProcessor::readParameters() { 27 | SensorProcessorBase::readParameters(); 28 | nodeHandle_.param("sensor_processor/p_1", sensorParameters_["p_1"], 0.0); 29 | nodeHandle_.param("sensor_processor/p_2", sensorParameters_["p_2"], 0.0); 30 | nodeHandle_.param("sensor_processor/p_3", sensorParameters_["p_3"], 0.0); 31 | nodeHandle_.param("sensor_processor/p_4", sensorParameters_["p_4"], 0.0); 32 | nodeHandle_.param("sensor_processor/p_5", sensorParameters_["p_5"], 0.0); 33 | nodeHandle_.param("sensor_processor/lateral_factor", sensorParameters_["lateral_factor"], 0.0); 34 | nodeHandle_.param("sensor_processor/depth_to_disparity_factor", sensorParameters_["depth_to_disparity_factor"], 0.0); 35 | nodeHandle_.param("sensor_processor/cutoff_min_depth", sensorParameters_["cutoff_min_depth"], std::numeric_limits::min()); 36 | nodeHandle_.param("sensor_processor/cutoff_max_depth", sensorParameters_["cutoff_max_depth"], std::numeric_limits::max()); 37 | return true; 38 | } 39 | 40 | bool StereoSensorProcessor::computeVariances(const PointCloudType::ConstPtr pointCloud, 41 | const Eigen::Matrix& robotPoseCovariance, Eigen::VectorXf& variances) { 42 | variances.resize(pointCloud->size()); 43 | 44 | // Projection vector (P). 45 | const Eigen::RowVector3f projectionVector = Eigen::RowVector3f::UnitZ(); 46 | 47 | // Sensor Jacobian (J_s). 48 | const Eigen::RowVector3f sensorJacobian = 49 | projectionVector * (rotationMapToBase_.transposed() * rotationBaseToSensor_.transposed()).toImplementation().cast(); 50 | 51 | // Robot rotation covariance matrix (Sigma_q). 52 | Eigen::Matrix3f rotationVariance = robotPoseCovariance.bottomRightCorner(3, 3).cast(); 53 | 54 | // Preparations for#include robot rotation Jacobian (J_q) to minimize computation for every point in point 55 | // cloud. 56 | const Eigen::Matrix3f C_BM_transpose = rotationMapToBase_.transposed().toImplementation().cast(); 57 | const Eigen::RowVector3f P_mul_C_BM_transpose = projectionVector * C_BM_transpose; 58 | const Eigen::Matrix3f C_SB_transpose = rotationBaseToSensor_.transposed().toImplementation().cast(); 59 | const Eigen::Matrix3f B_r_BS_skew = 60 | kindr::getSkewMatrixFromVector(Eigen::Vector3f(translationBaseToSensorInBaseFrame_.toImplementation().cast())); 61 | 62 | for (unsigned int i = 0; i < pointCloud->size(); ++i) { 63 | // For every point in point cloud. 64 | 65 | // Preparation. 66 | pcl::PointXYZRGBConfidenceRatio point = pointCloud->points[i]; 67 | double disparity = sensorParameters_.at("depth_to_disparity_factor") / point.z; // NOLINT(cppcoreguidelines-pro-type-union-access) 68 | Eigen::Vector3f pointVector(point.x, point.y, point.z); // S_r_SP // NOLINT(cppcoreguidelines-pro-type-union-access) 69 | float heightVariance = 0.0; // sigma_p 70 | 71 | // Measurement distance. 72 | float measurementDistance = pointVector.norm(); 73 | 74 | // Compute sensor covariance matrix (Sigma_S) with sensor model. 75 | float varianceNormal = 76 | pow(sensorParameters_.at("depth_to_disparity_factor") / pow(disparity, 2), 2) * 77 | ((sensorParameters_.at("p_5") * disparity + sensorParameters_.at("p_2")) * 78 | sqrt(pow(sensorParameters_.at("p_3") * disparity + sensorParameters_.at("p_4") - getJ(i), 2) + pow(240 - getI(i), 2)) + 79 | sensorParameters_.at("p_1")); 80 | float varianceLateral = pow(sensorParameters_.at("lateral_factor") * measurementDistance, 2); 81 | Eigen::Matrix3f sensorVariance = Eigen::Matrix3f::Zero(); 82 | sensorVariance.diagonal() << varianceLateral, varianceLateral, varianceNormal; 83 | 84 | // Robot rotation Jacobian (J_q). 85 | const Eigen::Matrix3f C_SB_transpose_times_S_r_SP_skew = kindr::getSkewMatrixFromVector(Eigen::Vector3f(C_SB_transpose * pointVector)); 86 | Eigen::RowVector3f rotationJacobian = P_mul_C_BM_transpose * (C_SB_transpose_times_S_r_SP_skew + B_r_BS_skew); 87 | 88 | // Measurement variance for map (error propagation law). 89 | heightVariance = rotationJacobian * rotationVariance * rotationJacobian.transpose(); 90 | heightVariance += sensorJacobian * sensorVariance * sensorJacobian.transpose(); 91 | 92 | // Copy to list. 93 | variances(i) = heightVariance; 94 | } 95 | 96 | return true; 97 | } 98 | 99 | bool StereoSensorProcessor::filterPointCloudSensorType(const PointCloudType::Ptr pointCloud) { 100 | pcl::PassThrough passThroughFilter; 101 | PointCloudType tempPointCloud; 102 | 103 | // cutoff points with z values 104 | passThroughFilter.setInputCloud(pointCloud); 105 | passThroughFilter.setFilterFieldName("z"); 106 | passThroughFilter.setFilterLimits(sensorParameters_.at("cutoff_min_depth"), sensorParameters_.at("cutoff_max_depth")); 107 | passThroughFilter.filter(tempPointCloud); 108 | pointCloud->swap(tempPointCloud); 109 | 110 | return true; 111 | } 112 | 113 | int StereoSensorProcessor::getI(int index) { 114 | // TODO(max): Figure out originalWidth_ value. 115 | return indices_[index] / originalWidth_; 116 | } 117 | 118 | int StereoSensorProcessor::getJ(int index) { 119 | return indices_[index] % originalWidth_; 120 | } 121 | 122 | } // namespace elevation_mapping 123 | -------------------------------------------------------------------------------- /elevation_mapping/src/sensor_processors/StructuredLightSensorProcessor.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * StructuredLightSensorProcessor.cpp 3 | * 4 | * Created on: Feb 5, 2014 5 | * Author: Péter Fankhauser 6 | * Institute: ETH Zurich, ANYbotics 7 | */ 8 | #include 9 | #include 10 | #include 11 | 12 | #include 13 | #include 14 | 15 | #include "elevation_mapping/PointXYZRGBConfidenceRatio.hpp" 16 | #include "elevation_mapping/sensor_processors/StructuredLightSensorProcessor.hpp" 17 | 18 | namespace elevation_mapping { 19 | 20 | /*! StructuredLight-type (structured light) sensor model: 21 | * standardDeviationInNormalDirection = sensorModelNormalFactorA_ + sensorModelNormalFactorB_ * (measurementDistance - 22 | * sensorModelNormalFactorC_)^2; standardDeviationInLateralDirection = sensorModelLateralFactor_ * measurementDistance Taken from: Nguyen, 23 | * C. V., Izadi, S., & Lovell, D., Modeling Kinect Sensor Noise for Improved 3D Reconstruction and Tracking, 2012. 24 | */ 25 | 26 | StructuredLightSensorProcessor::StructuredLightSensorProcessor(ros::NodeHandle& nodeHandle, 27 | const SensorProcessorBase::GeneralParameters& generalParameters) 28 | : SensorProcessorBase(nodeHandle, generalParameters) {} 29 | 30 | StructuredLightSensorProcessor::~StructuredLightSensorProcessor() = default; 31 | 32 | bool StructuredLightSensorProcessor::readParameters() { 33 | SensorProcessorBase::readParameters(); 34 | nodeHandle_.param("sensor_processor/normal_factor_a", sensorParameters_["normal_factor_a"], 0.0); 35 | nodeHandle_.param("sensor_processor/normal_factor_b", sensorParameters_["normal_factor_b"], 0.0); 36 | nodeHandle_.param("sensor_processor/normal_factor_c", sensorParameters_["normal_factor_c"], 0.0); 37 | nodeHandle_.param("sensor_processor/normal_factor_d", sensorParameters_["normal_factor_d"], 0.0); 38 | nodeHandle_.param("sensor_processor/normal_factor_e", sensorParameters_["normal_factor_e"], 0.0); 39 | nodeHandle_.param("sensor_processor/lateral_factor", sensorParameters_["lateral_factor"], 0.0); 40 | nodeHandle_.param("sensor_processor/cutoff_min_depth", sensorParameters_["cutoff_min_depth"], std::numeric_limits::min()); 41 | nodeHandle_.param("sensor_processor/cutoff_max_depth", sensorParameters_["cutoff_max_depth"], std::numeric_limits::max()); 42 | return true; 43 | } 44 | 45 | bool StructuredLightSensorProcessor::computeVariances(const PointCloudType::ConstPtr pointCloud, 46 | const Eigen::Matrix& robotPoseCovariance, Eigen::VectorXf& variances) { 47 | variances.resize(pointCloud->size()); 48 | 49 | // Projection vector (P). 50 | const Eigen::RowVector3f projectionVector = Eigen::RowVector3f::UnitZ(); 51 | 52 | // Sensor Jacobian (J_s). 53 | const Eigen::RowVector3f sensorJacobian = 54 | projectionVector * (rotationMapToBase_.transposed() * rotationBaseToSensor_.transposed()).toImplementation().cast(); 55 | 56 | // Robot rotation covariance matrix (Sigma_q). 57 | const Eigen::Matrix3f rotationVariance = robotPoseCovariance.bottomRightCorner(3, 3).cast(); 58 | 59 | // Preparations for robot rotation Jacobian (J_q) to minimize computation for every point in point cloud. 60 | const Eigen::Matrix3f C_BM_transpose = rotationMapToBase_.transposed().toImplementation().cast(); 61 | const Eigen::RowVector3f P_mul_C_BM_transpose = projectionVector * C_BM_transpose; 62 | const Eigen::Matrix3f C_SB_transpose = rotationBaseToSensor_.transposed().toImplementation().cast(); 63 | const Eigen::Matrix3f B_r_BS_skew = 64 | kindr::getSkewMatrixFromVector(Eigen::Vector3f(translationBaseToSensorInBaseFrame_.toImplementation().cast())); 65 | const float epsilon = std::numeric_limits::min(); 66 | 67 | for (unsigned int i = 0; i < pointCloud->size(); ++i) { 68 | // For every point in point cloud. 69 | 70 | // Preparation. 71 | auto& point = pointCloud->points[i]; 72 | const float& confidenceRatio = point.confidence_ratio; 73 | Eigen::Vector3f pointVector(point.x, point.y, point.z); // S_r_SP // NOLINT(cppcoreguidelines-pro-type-union-access) 74 | 75 | // Measurement distance. 76 | const float measurementDistance = pointVector.z(); 77 | 78 | // Compute sensor covariance matrix (Sigma_S) with sensor model. 79 | const float deviationNormal = 80 | sensorParameters_.at("normal_factor_a") + 81 | sensorParameters_.at("normal_factor_b") * (measurementDistance - sensorParameters_.at("normal_factor_c")) * 82 | (measurementDistance - sensorParameters_.at("normal_factor_c")) + 83 | sensorParameters_.at("normal_factor_d") * pow(measurementDistance, sensorParameters_.at("normal_factor_e")); 84 | const float varianceNormal = deviationNormal * deviationNormal; 85 | const float deviationLateral = sensorParameters_.at("lateral_factor") * measurementDistance; 86 | const float varianceLateral = deviationLateral * deviationLateral; 87 | Eigen::Matrix3f sensorVariance = Eigen::Matrix3f::Zero(); 88 | sensorVariance.diagonal() << varianceLateral, varianceLateral, varianceNormal; 89 | 90 | // Robot rotation Jacobian (J_q). 91 | const Eigen::Matrix3f C_SB_transpose_times_S_r_SP_skew = kindr::getSkewMatrixFromVector(Eigen::Vector3f(C_SB_transpose * pointVector)); 92 | const Eigen::RowVector3f rotationJacobian = P_mul_C_BM_transpose * (C_SB_transpose_times_S_r_SP_skew + B_r_BS_skew); 93 | 94 | // Measurement variance for map (error propagation law). 95 | float heightVariance = 0.0; // sigma_p 96 | heightVariance = rotationJacobian * rotationVariance * rotationJacobian.transpose(); 97 | // Scale the sensor variance by the inverse, squared confidence ratio 98 | heightVariance += 99 | static_cast(sensorJacobian * sensorVariance * sensorJacobian.transpose()) / (epsilon + confidenceRatio * confidenceRatio); 100 | 101 | // Copy to list. 102 | variances(i) = heightVariance; 103 | } 104 | 105 | return true; 106 | } 107 | 108 | bool StructuredLightSensorProcessor::filterPointCloudSensorType(const PointCloudType::Ptr pointCloud) { 109 | pcl::PassThrough passThroughFilter; 110 | PointCloudType tempPointCloud; 111 | 112 | // cutoff points with z values 113 | passThroughFilter.setInputCloud(pointCloud); 114 | passThroughFilter.setFilterFieldName("z"); 115 | passThroughFilter.setFilterLimits(sensorParameters_.at("cutoff_min_depth"), sensorParameters_.at("cutoff_max_depth")); 116 | passThroughFilter.filter(tempPointCloud); 117 | pointCloud->swap(tempPointCloud); 118 | 119 | return true; 120 | } 121 | 122 | } // namespace elevation_mapping 123 | -------------------------------------------------------------------------------- /elevation_mapping/test/ElevationMapTest.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * ElevationMapTest.cpp 3 | * 4 | * Created on: Nov 27, 2015 5 | * Author: Péter Fankhauser 6 | * Institute: ETH Zurich, ANYbotics 7 | */ 8 | 9 | #include "elevation_mapping/ElevationMap.hpp" 10 | #include "grid_map_core/GridMap.hpp" 11 | 12 | #include 13 | 14 | // gtest 15 | #include 16 | 17 | TEST(ElevationMap, Test) { // NOLINT 18 | // ros::M_string remappings; 19 | // ros::init(remappings, "test_elevation_mapping"); 20 | // ros::NodeHandle nodeHandle("~"); 21 | // ElevationMap map(nodeHandle); 22 | // map.setGeometry(Length(1.0, 1.0), 0.01, Position(0.0, 0.0)); 23 | } 24 | -------------------------------------------------------------------------------- /elevation_mapping/test/WeightedEmpiricalCumulativeDistributionFunctionTest.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * WeightedEmpiricalCumulativeDistributionFunctionTest.cpp 3 | * 4 | * Created on: Dec 7, 2015 5 | * Author: Péter Fankhauser 6 | * Institute: ETH Zurich, ANYbotics 7 | */ 8 | 9 | #include "elevation_mapping/WeightedEmpiricalCumulativeDistributionFunction.hpp" 10 | 11 | // gtest 12 | #include 13 | 14 | TEST(WeightedEmpiricalCumulativeDistributionFunction, Initialization) { // NOLINT 15 | elevation_mapping::WeightedEmpiricalCumulativeDistributionFunction wecdf; 16 | EXPECT_FALSE(wecdf.compute()); 17 | wecdf.clear(); 18 | EXPECT_FALSE(wecdf.compute()); 19 | } 20 | 21 | TEST(WeightedEmpiricalCumulativeDistributionFunction, Trivial) { // NOLINT 22 | elevation_mapping::WeightedEmpiricalCumulativeDistributionFunction wecdf; 23 | wecdf.add(0.0); 24 | wecdf.add(1.0); 25 | EXPECT_TRUE(wecdf.compute()); 26 | EXPECT_DOUBLE_EQ(0.0, wecdf.quantile(-0.1)); 27 | EXPECT_DOUBLE_EQ(0.0, wecdf.quantile(0.0)); 28 | EXPECT_DOUBLE_EQ(0.25, wecdf.quantile(0.25)); 29 | EXPECT_DOUBLE_EQ(0.5, wecdf.quantile(0.5)); 30 | EXPECT_DOUBLE_EQ(2.0 / 3.0, wecdf.quantile(2.0 / 3.0)); 31 | EXPECT_DOUBLE_EQ(0.95, wecdf.quantile(0.95)); 32 | EXPECT_DOUBLE_EQ(1.0, wecdf.quantile(1.0)); 33 | EXPECT_DOUBLE_EQ(1.0, wecdf.quantile(1.1)); 34 | } 35 | 36 | TEST(WeightedEmpiricalCumulativeDistributionFunction, LinearEquallySpaced) { // NOLINT 37 | elevation_mapping::WeightedEmpiricalCumulativeDistributionFunction wecdf; 38 | wecdf.add(0.0); 39 | wecdf.add(10.0 / 3.0); 40 | wecdf.add(20.0 / 3.0); 41 | wecdf.add(10.0); 42 | EXPECT_TRUE(wecdf.compute()); 43 | EXPECT_DOUBLE_EQ(0.0, wecdf.quantile(0.0)); 44 | EXPECT_DOUBLE_EQ(2.5, wecdf.quantile(0.25)); 45 | EXPECT_DOUBLE_EQ(5.0, wecdf.quantile(0.5)); 46 | EXPECT_DOUBLE_EQ(20.0 / 3.0, wecdf.quantile(2.0 / 3.0)); 47 | EXPECT_DOUBLE_EQ(9.5, wecdf.quantile(0.95)); 48 | EXPECT_DOUBLE_EQ(10.0, wecdf.quantile(1.1)); 49 | } 50 | 51 | TEST(WeightedEmpiricalCumulativeDistributionFunction, SingleValue) { // NOLINT 52 | elevation_mapping::WeightedEmpiricalCumulativeDistributionFunction wecdf; 53 | wecdf.add(3.0); 54 | wecdf.add(3.0); 55 | wecdf.add(3.0); 56 | EXPECT_TRUE(wecdf.compute()); 57 | EXPECT_DOUBLE_EQ(3.0, wecdf.quantile(0.0)); 58 | EXPECT_DOUBLE_EQ(3.0, wecdf.quantile(0.25)); 59 | EXPECT_DOUBLE_EQ(3.0, wecdf.quantile(0.5)); 60 | EXPECT_DOUBLE_EQ(3.0, wecdf.quantile(1.0)); 61 | EXPECT_DOUBLE_EQ(3.0, wecdf.quantile(2.0)); 62 | } 63 | 64 | TEST(WeightedEmpiricalCumulativeDistributionFunction, SyntheticDataDebug) { // NOLINT 65 | elevation_mapping::WeightedEmpiricalCumulativeDistributionFunction wecdf; 66 | for (unsigned int i = 0; i < 10; ++i) { 67 | wecdf.add(1.0); 68 | } 69 | wecdf.add(2.0); 70 | EXPECT_TRUE(wecdf.compute()); 71 | EXPECT_DOUBLE_EQ(1.05, wecdf.quantile(0.05)); 72 | EXPECT_DOUBLE_EQ(1.95, wecdf.quantile(0.95)); 73 | } 74 | -------------------------------------------------------------------------------- /elevation_mapping/test/elevation_mapping.test: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | -------------------------------------------------------------------------------- /elevation_mapping/test/input_sources/InputSourcesTest.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * InputSourceTest.cpp 3 | * 4 | * Created on: Oct 02, 2020 5 | * Author: Magnus Gärtner 6 | * Institute: ETH Zurich, ANYbotics 7 | */ 8 | 9 | #include "elevation_mapping/ElevationMapping.hpp" 10 | 11 | #include 12 | 13 | #include 14 | 15 | static void assertSuccessAndNumberOfSources(const std::string& inputConfiguration, bool successExpected, 16 | uint32_t numberOfExpectedInputSources) { 17 | elevation_mapping::InputSourceManager inputSourceManager(ros::NodeHandle("~")); 18 | bool success = inputSourceManager.configureFromRos(inputConfiguration); 19 | ASSERT_EQ(success, successExpected) << "Configuration was:\n" 20 | << ros::NodeHandle("~").param(inputConfiguration, "not set").toXml() << "\n"; 21 | ASSERT_EQ(inputSourceManager.getNumberOfSources(), numberOfExpectedInputSources); 22 | } 23 | 24 | TEST(InputSources, SingleInputValid) { // NOLINT 25 | assertSuccessAndNumberOfSources("single_valid", true, 1); 26 | } 27 | 28 | TEST(InputSources, MultipleInputsValid) { // NOLINT 29 | assertSuccessAndNumberOfSources("multiple_valid", true, 3); 30 | } 31 | 32 | TEST(InputSources, NoType) { // NOLINT 33 | assertSuccessAndNumberOfSources("no_type", false, 0); 34 | } 35 | 36 | TEST(InputSources, NoTopic) { // NOLINT 37 | assertSuccessAndNumberOfSources("no_topic", false, 0); 38 | } 39 | 40 | TEST(InputSources, NoQueueSize) { // NOLINT 41 | assertSuccessAndNumberOfSources("no_queue_size", false, 0); 42 | } 43 | 44 | TEST(InputSources, NoPublishOnUpdate) { // NOLINT 45 | assertSuccessAndNumberOfSources("no_publish_on_update", false, 0); 46 | } 47 | 48 | TEST(InputSources, SubscribingSameTwice) { // NOLINT 49 | assertSuccessAndNumberOfSources("subscribing_same_topic_twice", false, 1); 50 | } 51 | 52 | TEST(InputSources, ConfigurationNotGiven) { // NOLINT 53 | assertSuccessAndNumberOfSources("unset_namespace", false, 0); 54 | } 55 | 56 | TEST(InputSources, ConfigurationEmptySources) { // NOLINT 57 | assertSuccessAndNumberOfSources("empty_sources_list", true, 0); 58 | } 59 | 60 | TEST(InputSources, ConfigurationWrongType) { // NOLINT 61 | assertSuccessAndNumberOfSources("wrong_type_configuration", false, 0); 62 | } 63 | 64 | TEST(InputSources, ConfigurationNotAStruct) { // NOLINT 65 | assertSuccessAndNumberOfSources("not_a_struct", false, 0); 66 | } 67 | 68 | TEST(InputSources, ConfigurationQueueSizeIsString) { // NOLINT 69 | assertSuccessAndNumberOfSources("queue_size_is_string", false, 0); 70 | } 71 | 72 | TEST(InputSources, ConfigurationQueueSizeIsNegative) { // NOLINT 73 | assertSuccessAndNumberOfSources("negative_queue_size", false, 0); 74 | } 75 | 76 | TEST(InputSources, UnknownType) { // NOLINT 77 | ros::NodeHandle nodeHandle("~"); 78 | elevation_mapping::InputSourceManager inputSourceManager(nodeHandle); 79 | inputSourceManager.configureFromRos("unknown_type"); 80 | 81 | elevation_mapping::ElevationMapping map{nodeHandle}; 82 | 83 | // Trying to register this misconfigured InputSourceManager to our map should fail. 84 | bool success = 85 | inputSourceManager.registerCallbacks(map, make_pair("pointcloud", &elevation_mapping::ElevationMapping::pointCloudCallback)); 86 | ASSERT_FALSE(success); 87 | } 88 | 89 | TEST(ElevationMap, Constructor) { // NOLINT 90 | ros::NodeHandle nodeHandle("~"); 91 | elevation_mapping::ElevationMapping map(nodeHandle); 92 | } 93 | 94 | TEST(InputSources, ListeningToTopicsAfterRegistration) { // NOLINT 95 | // subscribe to the default parameter "input_sources" 96 | ros::NodeHandle nodeHandle("~"); 97 | class ElevationMappingWithInputSourcesAccessor : public elevation_mapping::ElevationMapping { 98 | public: 99 | ElevationMappingWithInputSourcesAccessor(ros::NodeHandle nodeHandle) : elevation_mapping::ElevationMapping(nodeHandle) {} 100 | virtual ~ElevationMappingWithInputSourcesAccessor() = default; 101 | int getNumberOfSources() { return inputSources_.getNumberOfSources(); } 102 | } map{nodeHandle}; 103 | 104 | // Wait a bit. 105 | ros::spinOnce(); 106 | ros::Duration(1.0).sleep(); 107 | ros::spinOnce(); 108 | 109 | // Publish to the topics we expect map to subscribe. 110 | ros::NodeHandle nh(""); 111 | ros::Publisher firstLidarPublisher = nh.advertise("/lidar_1/depth/points", 1, false); 112 | ros::Publisher secondLidarPublisher = nh.advertise("/lidar_2/depth/points", 1, false); 113 | 114 | // Check if we have exactly one subscriber per topic. 115 | ASSERT_EQ(firstLidarPublisher.getNumSubscribers(), 1); 116 | ASSERT_EQ(secondLidarPublisher.getNumSubscribers(), 1); 117 | // ASSERT_EQ(firstDepthImagePublisher.getNumSubscribers(), 1); 118 | ASSERT_EQ(map.getNumberOfSources(), 2); 119 | } 120 | -------------------------------------------------------------------------------- /elevation_mapping/test/input_sources/config/TestConfigurations.yaml: -------------------------------------------------------------------------------- 1 | # This file defines multiple input source configurations for testing purposes. 2 | # Note: many of them are not valid. 3 | 4 | input_sources: 5 | input_1: 6 | type: pointcloud 7 | topic: /lidar_1/depth/points 8 | queue_size: 1 9 | publish_on_update: true 10 | sensor_processor: 11 | type: perfect 12 | input_3: 13 | type: pointcloud 14 | topic: /lidar_2/depth/points 15 | queue_size: 5 16 | publish_on_update: true 17 | sensor_processor: 18 | type: perfect 19 | single_valid: 20 | standard_single_input: 21 | type: pointcloud 22 | topic: /lidar/depth/points 23 | queue_size: 1 24 | publish_on_update: true 25 | sensor_processor: 26 | type: perfect 27 | multiple_valid: 28 | input_1: 29 | type: pointcloud 30 | topic: /lidar_1/depth/points 31 | queue_size: 1 32 | publish_on_update: true 33 | sensor_processor: 34 | type: perfect 35 | input_2: 36 | type: depthimage 37 | topic: /image/depth/image_rect_raw 38 | queue_size: 1 39 | publish_on_update: false 40 | sensor_processor: 41 | type: perfect 42 | input_3: 43 | type: pointcloud 44 | topic: /lidar_2/depth/points 45 | queue_size: 5 46 | publish_on_update: false 47 | sensor_processor: 48 | type: perfect 49 | no_type: 50 | standard_single_input: 51 | topic: /lidar/depth/points 52 | queue_size: 1 53 | publish_on_update: true 54 | sensor_processor: 55 | type: perfect 56 | queue_size_is_string: 57 | standard_single_input: 58 | type: pointcloud 59 | topic: /lidar/depth/points 60 | queue_size: "1" 61 | publish_on_update: true 62 | sensor_processor: 63 | type: perfect 64 | negative_queue_size: 65 | standard_single_input: 66 | type: pointcloud 67 | topic: /lidar/depth/points 68 | queue_size: -1 69 | publish_on_update: true 70 | sensor_processor: 71 | type: perfect 72 | no_topic: 73 | standard_single_input: 74 | type: pointcloud 75 | queue_size: 1 76 | publish_on_update: true 77 | sensor_processor: 78 | type: perfect 79 | no_queue_size: 80 | standard_single_input: 81 | type: pointcloud 82 | topic: /lidar/depth/points 83 | publish_on_update: true 84 | sensor_processor: 85 | type: perfect 86 | no_publish_on_update: 87 | standard_single_input: 88 | type: pointcloud 89 | topic: /lidar/depth/points 90 | queue_size: 1 91 | sensor_processor: 92 | type: perfect 93 | unknown_type: 94 | unknown_input: 95 | type: sonar 96 | topic: /sonar/ranges 97 | queue_size: 1 98 | publish_on_update: true 99 | sensor_processor: 100 | type: perfect 101 | wrong_type_configuration: [name: test, type: pointcloud, topic: /yet/another/topic, queue_size: 3, publish_on_update: true, sensor_processor/type: perfect] 102 | not_a_struct: 103 | - name: input_1 104 | type: pointcloud 105 | topic: /lidar_1/depth/points 106 | queue_size: 1 107 | publish_on_update: true 108 | sensor_processor: 109 | type: perfect 110 | subscribing_same_topic_twice: 111 | input_1: 112 | type: pointcloud 113 | topic: /lidar_1/depth/points 114 | queue_size: 1 115 | publish_on_update: true 116 | sensor_processor: 117 | type: perfect 118 | input_2: 119 | type: pointcloud 120 | topic: /lidar_1/depth/points 121 | queue_size: 1 122 | publish_on_update: false 123 | sensor_processor: 124 | type: perfect 125 | empty_sources_list: [] -------------------------------------------------------------------------------- /elevation_mapping/test/input_sources/config/rosconsole.config: -------------------------------------------------------------------------------- 1 | # Override any of the available loggers with your desired level. 2 | # However, you should override the logger level only locally and revert 3 | # your changes before pushing, unless you have a proper motivation for 4 | # different default settings. 5 | 6 | # Set the level of the ROS logger and all its children to DEBUG. 7 | # log4j.logger.ros=WARN 8 | # Set the level of the logger in the elevation_mapping_benchmark_ros package to DEBUG. 9 | log4j.logger.ros.elevation_mapping=DEBUG 10 | -------------------------------------------------------------------------------- /elevation_mapping/test/input_sources/input_sources.test: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | -------------------------------------------------------------------------------- /elevation_mapping/test/input_sources/test_input_sources.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * test_elevation_mapping.cpp 3 | * 4 | * Created on: Okt 02, 2020 5 | * Author: Magnus Gärtner 6 | * Institute: ETH Zurich, ANYbotics 7 | */ 8 | 9 | #include 10 | 11 | // gtest 12 | #include 13 | #include 14 | 15 | // Run all the tests that were declared with TEST() 16 | int main(int argc, char** argv) { 17 | testing::InitGoogleTest(&argc, argv); 18 | ros::init(argc, argv, "Tests"); 19 | int initValue = static_cast(time(nullptr)); 20 | ::testing::Test::RecordProperty("Init value for random number generator:", initValue); 21 | srand(initValue); 22 | return RUN_ALL_TESTS(); 23 | } 24 | -------------------------------------------------------------------------------- /elevation_mapping/test/postprocessing/PostprocessorTest.cpp: -------------------------------------------------------------------------------- 1 | /*! 2 | * @file PostprocessorTest.cpp 3 | * @authors Magnus Gärtner (ANYbotics) 4 | * @brief Tests for the PostprocessorPool and the PostprocessingPipeline. 5 | */ 6 | 7 | #include 8 | #include 9 | 10 | #include "elevation_mapping/postprocessing/PostprocessingPipelineFunctor.hpp" 11 | #include "elevation_mapping/postprocessing/PostprocessorPool.hpp" 12 | 13 | /** 14 | * We read in a postprocessing (mock) configuration that takes 150ms to execute. We test whether the postprocessorPool accepts/discards the 15 | * tasks for various configurations of time between tasks and number of threads in the pool. 16 | */ 17 | class RosFixture : public ::testing::Test { 18 | void SetUp() override { 19 | const std::map remappings{}; 20 | ros::init(remappings, "post_processor_ros_test"); 21 | ros::start(); 22 | } 23 | 24 | void TearDown() override { ros::shutdown(); } 25 | 26 | public: 27 | static void checkAcceptedTasks(uint poolSize, uint timeBetweenConsecutiveTasks, std::vector expectedAcceptanceOutcomes) { 28 | // Set up ROS node handle. 29 | ros::NodeHandle nodeHandle("~"); 30 | 31 | elevation_mapping::PostprocessorPool pool{poolSize, nodeHandle}; 32 | int taskNumber = 0; 33 | for (auto expectedOutcome : expectedAcceptanceOutcomes) { 34 | bool accepted = pool.runTask(grid_map::GridMap()); 35 | if (expectedOutcome) { 36 | ASSERT_TRUE(accepted) << "Postprocessor pool accepted task number: " << taskNumber << " although it should."; 37 | } else { 38 | ASSERT_FALSE(accepted) << "Postprocessor pool accepted task number: " << taskNumber << " although it should not. "; 39 | } 40 | taskNumber++; 41 | std::this_thread::sleep_for(std::chrono::milliseconds(timeBetweenConsecutiveTasks)); 42 | } 43 | } 44 | }; 45 | 46 | TEST_F(RosFixture, FiveTasksOneThreadSimultaneously) { // NOLINT 47 | checkAcceptedTasks(1, 0, {true, false, false, false, false}); 48 | } 49 | 50 | TEST_F(RosFixture, FiveTasksTwoThreadSimultaneously) { // NOLINT 51 | checkAcceptedTasks(2, 0, {true, true, false, false, false}); 52 | } 53 | 54 | TEST_F(RosFixture, EnoughTimeToProcess) { // NOLINT 55 | checkAcceptedTasks(1, 200, {true, true, true, true, true}); 56 | } 57 | 58 | TEST_F(RosFixture, EnoughTimeToProcessWithTwoThreads) { // NOLINT 59 | checkAcceptedTasks(2, 100, {true, true, true, true, true}); 60 | } 61 | 62 | TEST_F(RosFixture, ProcessEverySecond) { // NOLINT 63 | checkAcceptedTasks(1, 100, {true, false, true, false, true}); 64 | } 65 | 66 | TEST_F(RosFixture, TwoThreadsWithMiss) { // NOLINT 67 | checkAcceptedTasks(2, 60, {true, true, false, true, true, false}); 68 | } 69 | -------------------------------------------------------------------------------- /elevation_mapping/test/postprocessing/main.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | int main(int argc, char** argv) { 5 | ros::init(argc, argv, "elevation_mapping"); 6 | ros::start(); // To make use of ROS time in output macros. 7 | testing::InitGoogleTest(&argc, argv); 8 | int res = RUN_ALL_TESTS(); 9 | ros::shutdown(); 10 | return res; 11 | } 12 | -------------------------------------------------------------------------------- /elevation_mapping/test/postprocessing/postprocessor.test: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | -------------------------------------------------------------------------------- /elevation_mapping/test/postprocessing/postprocessor_pipeline.yaml: -------------------------------------------------------------------------------- 1 | postprocessor_pipeline_name: test_pipeline 2 | 3 | test_pipeline: 4 | - name: first 5 | type: gridMapFilters/MockFilter 6 | params: 7 | processing_time: 100 # [ms] 8 | print_name: true 9 | - name: second 10 | type: gridMapFilters/MockFilter 11 | params: 12 | processing_time: 50 # [ms] 13 | print_name: true -------------------------------------------------------------------------------- /elevation_mapping/test/test_elevation_mapping.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * test_elevation_mapping.cpp 3 | * 4 | * Created on: Nov 27, 2015 5 | * Author: Péter Fankhauser 6 | * Institute: ETH Zurich, ANYbotics 7 | */ 8 | 9 | #include 10 | 11 | // gtest 12 | #include 13 | 14 | // Run all the tests that were declared with TEST() 15 | int main(int argc, char** argv) { 16 | testing::InitGoogleTest(&argc, argv); 17 | int initValue = static_cast(time(nullptr)); 18 | std::cout << "Init value for random number generator: " << initValue << std::endl; 19 | srand(initValue); 20 | return RUN_ALL_TESTS(); 21 | } 22 | -------------------------------------------------------------------------------- /elevation_mapping_demos/.pydevproject: -------------------------------------------------------------------------------- 1 | 2 | 3 | Default 4 | python 2.7 5 | 6 | -------------------------------------------------------------------------------- /elevation_mapping_demos/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5.1) 2 | project(elevation_mapping_demos) 3 | 4 | ## Find catkin macros and libraries 5 | find_package(catkin REQUIRED COMPONENTS 6 | elevation_mapping 7 | grid_map_rviz_plugin 8 | rviz 9 | ) 10 | 11 | ################################### 12 | ## catkin specific configuration ## 13 | ################################### 14 | ## The catkin_package macro generates cmake config files for your package 15 | ## Declare things to be passed to dependent projects 16 | ## INCLUDE_DIRS: uncomment this if you package contains header files 17 | ## LIBRARIES: libraries you create in this project that dependent projects also need 18 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 19 | ## DEPENDS: system dependencies of this project that dependent projects also need 20 | 21 | catkin_package( 22 | #INCLUDE_DIRS include 23 | #LIBRARIES 24 | CATKIN_DEPENDS 25 | elevation_mapping 26 | grid_map_rviz_plugin 27 | rviz 28 | #DEPENDS 29 | ) 30 | 31 | ############# 32 | ## Install ## 33 | ############# 34 | 35 | install(DIRECTORY config doc launch rviz 36 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 37 | ) 38 | 39 | install(PROGRAMS scripts/tf_to_pose_publisher.py 40 | DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 41 | ) 42 | 43 | ############## 44 | ## Test ## 45 | ############## 46 | if(CATKIN_ENABLE_TESTING) 47 | catkin_add_gtest(test_${PROJECT_NAME} 48 | test/empty_test.cpp 49 | ) 50 | 51 | target_include_directories(test_${PROJECT_NAME} 52 | PRIVATE 53 | include 54 | SYSTEM PUBLIC 55 | ${catkin_INCLUDE_DIRS} 56 | ) 57 | 58 | target_link_libraries(test_${PROJECT_NAME} 59 | ${catkin_LIBRARIES} 60 | gtest_main 61 | ) 62 | 63 | ################### 64 | ## Code_coverage ## 65 | ################### 66 | find_package(cmake_code_coverage QUIET) 67 | if(cmake_code_coverage_FOUND) 68 | add_gtest_coverage( 69 | TEST_BUILD_TARGETS 70 | test_${PROJECT_NAME} 71 | ) 72 | endif() 73 | endif() -------------------------------------------------------------------------------- /elevation_mapping_demos/config/elevation_maps/long_range.yaml: -------------------------------------------------------------------------------- 1 | length_in_x: 6.0 2 | length_in_y: 6.0 3 | position_x: 0.0 4 | position_y: 0.0 5 | resolution: 0.06 6 | min_variance: 0.0001 7 | max_variance: 10000.0 8 | mahalanobis_distance_threshold: 2.5 9 | multi_height_noise: 0.00002 10 | -------------------------------------------------------------------------------- /elevation_mapping_demos/config/elevation_maps/remove_object.yaml: -------------------------------------------------------------------------------- 1 | point_cloud_topic: "/points" 2 | map_frame_id: "map" 3 | robot_base_frame_id: "base" 4 | robot_pose_with_covariance_topic: "/pose" 5 | robot_pose_cache_size: 500 6 | track_point_frame_id: "base" 7 | track_point_x: 0.0 8 | track_point_y: 0.0 9 | track_point_z: 0.0 10 | min_update_rate: 2.0 11 | time_tolerance: 0.5 12 | sensor_processor/ignore_points_above: 0.4 13 | robot_motion_map_update/covariance_scale_translation_x: 0.0 14 | robot_motion_map_update/covariance_scale_translation_y: 0.0 15 | robot_motion_map_update/covariance_scale_translation_z: 0.0 16 | robot_motion_map_update/covariance_scale_rotation_x: 0.0 17 | robot_motion_map_update/covariance_scale_rotation_y: 0.0 18 | robot_motion_map_update/covariance_scale_rotation_z: 0.0 19 | 20 | length_in_x: 6.0 21 | length_in_y: 6.0 22 | position_x: 0.0 23 | position_y: 0.0 24 | resolution: 0.01 25 | min_variance: 0.0001 26 | max_variance: 0.05 27 | mahalanobis_distance_threshold: 2.5 28 | multi_height_noise: 0.01 29 | fused_map_publishing_rate: 2.0 30 | enable_skip_lower_points: true 31 | scanning_duration: 0.01 32 | remove_penetrated_points_rate: 5.0 33 | -------------------------------------------------------------------------------- /elevation_mapping_demos/config/elevation_maps/simple_demo_map.yaml: -------------------------------------------------------------------------------- 1 | length_in_x: 2.5 2 | length_in_y: 2.5 3 | position_x: 0.0 4 | position_y: 0.0 5 | resolution: 0.01 6 | min_variance: 0.000009 7 | max_variance: 0.01 8 | mahalanobis_distance_threshold: 2.5 9 | multi_height_noise: 0.0000009 10 | -------------------------------------------------------------------------------- /elevation_mapping_demos/config/elevation_maps/starleth_map.yaml: -------------------------------------------------------------------------------- 1 | length_in_x: 2.5 2 | length_in_y: 2.5 3 | position_x: 0.0 4 | position_y: 0.0 5 | resolution: 0.01 6 | min_variance: 0.000009 7 | max_variance: 0.01 8 | mahalanobis_distance_threshold: 2.5 9 | multi_height_noise: 0.0000009 10 | -------------------------------------------------------------------------------- /elevation_mapping_demos/config/postprocessing/postprocessor_pipeline.yaml: -------------------------------------------------------------------------------- 1 | postprocessor_pipeline: # set by postprocessor_pipeline_name 2 | 3 | # Delete layers that are not needed. 4 | # - name: delete_original_layers 5 | # type: gridMapFilters/DeletionFilter 6 | # params: 7 | # layers: [lowest_scan_point,sensor_x_at_lowest_scan, sensor_y_at_lowest_scan, sensor_z_at_lowest_scan] # List of layers. 8 | 9 | # Fill holes in the map with inpainting. 10 | - name: inpaint 11 | type: gridMapCv/InpaintFilter 12 | params: 13 | input_layer: elevation 14 | output_layer: elevation_inpainted 15 | radius: 0.05 16 | 17 | # Compute Surface normals 18 | - name: surface_normals 19 | type: gridMapFilters/NormalVectorsFilter 20 | params: 21 | input_layer: elevation_inpainted 22 | output_layers_prefix: normal_vectors_ 23 | radius: 0.05 24 | normal_vector_positive_axis: z -------------------------------------------------------------------------------- /elevation_mapping_demos/config/robots/ground_truth_demo.yaml: -------------------------------------------------------------------------------- 1 | point_cloud_topic: "/ground_truth" 2 | map_frame_id: "map" 3 | robot_base_frame_id: "map" 4 | robot_pose_with_covariance_topic: "/ground_truth_pose" 5 | robot_pose_cache_size: 500 6 | track_point_frame_id: "map" 7 | track_point_x: 0.0 8 | track_point_y: 0.0 9 | track_point_z: 0.0 10 | -------------------------------------------------------------------------------- /elevation_mapping_demos/config/robots/remove_object.yaml: -------------------------------------------------------------------------------- 1 | point_cloud_topic: "/points" 2 | map_frame_id: "map" 3 | robot_base_frame_id: "base" 4 | robot_pose_with_covariance_topic: "/pose" 5 | robot_pose_cache_size: 200 6 | track_point_frame_id: "base" 7 | track_point_x: 0.0 8 | track_point_y: 0.0 9 | track_point_z: 0.0 10 | -------------------------------------------------------------------------------- /elevation_mapping_demos/config/robots/simple_demo_robot.yaml: -------------------------------------------------------------------------------- 1 | point_cloud_topic: "/points" 2 | map_frame_id: "map" 3 | robot_base_frame_id: "base" 4 | robot_pose_with_covariance_topic: "/pose" 5 | robot_pose_cache_size: 200 6 | track_point_frame_id: "base" 7 | track_point_x: 0.0 8 | track_point_y: 0.0 9 | track_point_z: 0.0 10 | -------------------------------------------------------------------------------- /elevation_mapping_demos/config/robots/starleth.yaml: -------------------------------------------------------------------------------- 1 | point_cloud_topic: "/depth_registered/points" 2 | map_frame_id: "starleth/odometry" 3 | robot_base_frame_id: "starleth/BASE" 4 | robot_pose_with_covariance_topic: "/starleth/robot_state/pose" 5 | robot_pose_cache_size: 200 6 | track_point_frame_id: "starleth/BASE" 7 | track_point_x: 0.0 8 | track_point_y: 0.0 9 | track_point_z: 0.0 10 | -------------------------------------------------------------------------------- /elevation_mapping_demos/config/robots/waffle_robot.yaml: -------------------------------------------------------------------------------- 1 | # Robot. 2 | map_frame_id: odom 3 | robot_base_frame_id: base_footprint 4 | robot_pose_with_covariance_topic: /base_footprint_pose 5 | robot_pose_cache_size: 200 6 | point_cloud_topic: /camera/depth/points 7 | track_point_frame_id: base_footprint 8 | track_point_x: 0.0 9 | track_point_y: 0.0 10 | track_point_z: 0.0 11 | min_update_rate: 2.0 12 | time_tolerance: 1.0 13 | time_offset_for_point_cloud: 0.0 14 | sensor_processor/ignore_points_above: 0.4 15 | robot_motion_map_update/covariance_scale: 0.01 16 | 17 | # Map. 18 | length_in_x: 6.0 19 | length_in_y: 6.0 20 | position_x: 0.0 21 | position_y: 0.0 22 | resolution: 0.1 23 | min_variance: 0.0001 24 | max_variance: 0.05 25 | mahalanobis_distance_threshold: 2.5 26 | multi_height_noise: 0.001 27 | surface_normal_positive_axis: z 28 | fused_map_publishing_rate: 0.5 29 | enable_visibility_cleanup: false 30 | visibility_cleanup_rate: 1.0 31 | scanning_duration: 1.0 32 | 33 | # Init submap 34 | initialize_elevation_map: false 35 | initialization_method: 0 36 | length_in_x_init_submap: 1.0 37 | length_in_y_init_submap: 1.0 38 | margin_init_submap: 0.3 39 | init_submap_height_offset: 0.01 40 | target_frame_init_submap: base_footprint -------------------------------------------------------------------------------- /elevation_mapping_demos/config/rosconsole.conf: -------------------------------------------------------------------------------- 1 | log4j.logger.ros.elevation_mapping=INFO -------------------------------------------------------------------------------- /elevation_mapping_demos/config/visualization/fused.yaml: -------------------------------------------------------------------------------- 1 | grid_map_visualizations: 2 | 3 | - name: elevation_cloud 4 | type: point_cloud 5 | params: 6 | layer: elevation 7 | 8 | - name: upper_bound_cloud 9 | type: point_cloud 10 | params: 11 | layer: upper_bound 12 | 13 | - name: lower_bound_cloud 14 | type: point_cloud 15 | params: 16 | layer: lower_bound 17 | 18 | - name: map_region 19 | type: map_region 20 | params: 21 | color: 16777215 22 | line_width: 0.003 23 | -------------------------------------------------------------------------------- /elevation_mapping_demos/config/visualization/raw.yaml: -------------------------------------------------------------------------------- 1 | grid_map_visualizations: 2 | 3 | - name: elevation_cloud 4 | type: point_cloud 5 | params: 6 | layer: elevation 7 | 8 | - name: sigma_cloud 9 | type: point_cloud 10 | params: 11 | layer: two_sigma_bound 12 | 13 | - name: map_region 14 | type: map_region 15 | params: 16 | color: 16777215 17 | line_width: 0.003 18 | -------------------------------------------------------------------------------- /elevation_mapping_demos/doc/anymal_forrest.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/leggedrobotics/elevation_mapping/32843c3098ab527ce6c88fac945fce61c38bce00/elevation_mapping_demos/doc/anymal_forrest.jpg -------------------------------------------------------------------------------- /elevation_mapping_demos/doc/anymal_locomotion_planner.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/leggedrobotics/elevation_mapping/32843c3098ab527ce6c88fac945fce61c38bce00/elevation_mapping_demos/doc/anymal_locomotion_planner.jpg -------------------------------------------------------------------------------- /elevation_mapping_demos/doc/anymal_outdoor_stairs.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/leggedrobotics/elevation_mapping/32843c3098ab527ce6c88fac945fce61c38bce00/elevation_mapping_demos/doc/anymal_outdoor_stairs.jpg -------------------------------------------------------------------------------- /elevation_mapping_demos/doc/elevation_map.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/leggedrobotics/elevation_mapping/32843c3098ab527ce6c88fac945fce61c38bce00/elevation_mapping_demos/doc/elevation_map.jpg -------------------------------------------------------------------------------- /elevation_mapping_demos/doc/starleth_kinect.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/leggedrobotics/elevation_mapping/32843c3098ab527ce6c88fac945fce61c38bce00/elevation_mapping_demos/doc/starleth_kinect.jpg -------------------------------------------------------------------------------- /elevation_mapping_demos/launch/ground_truth_demo.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | -------------------------------------------------------------------------------- /elevation_mapping_demos/launch/realsense_demo.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | -------------------------------------------------------------------------------- /elevation_mapping_demos/launch/simple_demo.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | -------------------------------------------------------------------------------- /elevation_mapping_demos/launch/starleth_kinect_demo.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | -------------------------------------------------------------------------------- /elevation_mapping_demos/launch/turtlesim3_waffle_demo.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | -------------------------------------------------------------------------------- /elevation_mapping_demos/launch/visualization.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | -------------------------------------------------------------------------------- /elevation_mapping_demos/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | elevation_mapping_demos 4 | 0.7.0 5 | Demonstrations for the elevation mapping framework. 6 | Maximilian Wulf 7 | Yoshua Nava 8 | BSD 9 | http://github.com/anybotics/elevation_mapping 10 | http://github.com/anybotics/elevation_mapping/issues 11 | Péter Fankhauser 12 | catkin 13 | elevation_mapping 14 | grid_map_rviz_plugin 15 | rviz 16 | 17 | gtest 18 | 19 | -------------------------------------------------------------------------------- /elevation_mapping_demos/rviz/elevation_map_visualization.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 78 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: ~ 7 | Splitter Ratio: 0.663889 8 | Tree Height: 341 9 | - Class: rviz/Selection 10 | Name: Selection 11 | - Class: rviz/Tool Properties 12 | Expanded: 13 | - /2D Pose Estimate1 14 | - /2D Nav Goal1 15 | - /Publish Point1 16 | Name: Tool Properties 17 | Splitter Ratio: 0.588679 18 | - Class: rviz/Views 19 | Expanded: 20 | - /Current View1 21 | Name: Views 22 | Splitter Ratio: 0.5 23 | - Class: rviz/Time 24 | Experimental: false 25 | Name: Time 26 | SyncMode: 0 27 | SyncSource: Elevation Map Raw 28 | Visualization Manager: 29 | Class: "" 30 | Displays: 31 | - Alpha: 0.5 32 | Cell Size: 1 33 | Class: rviz/Grid 34 | Color: 160; 160; 164 35 | Enabled: true 36 | Line Style: 37 | Line Width: 0.03 38 | Value: Lines 39 | Name: Grid 40 | Normal Cell Count: 0 41 | Offset: 42 | X: 0 43 | Y: 0 44 | Z: 0 45 | Plane: XY 46 | Plane Cell Count: 10 47 | Reference Frame: 48 | Value: true 49 | - Class: rviz/TF 50 | Enabled: true 51 | Frame Timeout: 15 52 | Frames: 53 | All Enabled: true 54 | starleth/BASE: 55 | Value: true 56 | starleth/LF_FOOT: 57 | Value: true 58 | starleth/LF_HIP: 59 | Value: true 60 | starleth/LF_SHANK: 61 | Value: true 62 | starleth/LF_THIGH: 63 | Value: true 64 | starleth/LH_FOOT: 65 | Value: true 66 | starleth/LH_HIP: 67 | Value: true 68 | starleth/LH_SHANK: 69 | Value: true 70 | starleth/LH_THIGH: 71 | Value: true 72 | starleth/RF_FOOT: 73 | Value: true 74 | starleth/RF_HIP: 75 | Value: true 76 | starleth/RF_SHANK: 77 | Value: true 78 | starleth/RF_THIGH: 79 | Value: true 80 | starleth/RH_FOOT: 81 | Value: true 82 | starleth/RH_HIP: 83 | Value: true 84 | starleth/RH_SHANK: 85 | Value: true 86 | starleth/RH_THIGH: 87 | Value: true 88 | starleth/odometry: 89 | Value: true 90 | starleth/openni_rgb_optical_frame: 91 | Value: true 92 | starleth/sensor_base: 93 | Value: true 94 | Marker Scale: 0.3 95 | Name: TF 96 | Show Arrows: true 97 | Show Axes: true 98 | Show Names: false 99 | Tree: 100 | starleth/odometry: 101 | starleth/BASE: 102 | starleth/LF_HIP: 103 | starleth/LF_THIGH: 104 | starleth/LF_SHANK: 105 | starleth/LF_FOOT: 106 | {} 107 | starleth/LH_HIP: 108 | starleth/LH_THIGH: 109 | starleth/LH_SHANK: 110 | starleth/LH_FOOT: 111 | {} 112 | starleth/RF_HIP: 113 | starleth/RF_THIGH: 114 | starleth/RF_SHANK: 115 | starleth/RF_FOOT: 116 | {} 117 | starleth/RH_HIP: 118 | starleth/RH_THIGH: 119 | starleth/RH_SHANK: 120 | starleth/RH_FOOT: 121 | {} 122 | starleth/sensor_base: 123 | starleth/openni_rgb_optical_frame: 124 | {} 125 | Update Interval: 0 126 | Value: true 127 | - Alpha: 1 128 | Autocompute Intensity Bounds: false 129 | Class: grid_map_rviz_plugin/GridMap 130 | Color: 200; 200; 200 131 | Color Layer: uncertainty_range 132 | Color Transformer: GridMapLayer 133 | Enabled: true 134 | Height Layer: elevation 135 | Height Transformer: GridMapLayer 136 | History Length: 1 137 | Invert Rainbow: false 138 | Max Color: 255; 255; 255 139 | Max Intensity: 0.1 140 | Min Color: 0; 0; 0 141 | Min Intensity: 0 142 | Name: Elevation Map 143 | Show Grid Lines: true 144 | Topic: /elevation_mapping/elevation_map 145 | Unreliable: false 146 | Use Rainbow: true 147 | Value: true 148 | - Alpha: 1 149 | Autocompute Intensity Bounds: false 150 | Autocompute Value Bounds: 151 | Max Value: 0.223659 152 | Min Value: -1.45534 153 | Value: true 154 | Axis: Z 155 | Channel Name: rgb 156 | Class: rviz/PointCloud2 157 | Color: 255; 255; 255 158 | Color Transformer: RGB8 159 | Decay Time: 0 160 | Enabled: false 161 | Invert Rainbow: true 162 | Max Color: 255; 0; 0 163 | Max Intensity: 0.0001 164 | Min Color: 0; 0; 255 165 | Min Intensity: -2e-05 166 | Name: Elevation Map Raw 167 | Position Transformer: XYZ 168 | Queue Size: 0 169 | Selectable: false 170 | Size (Pixels): 3 171 | Size (m): 0.01 172 | Style: Boxes 173 | Topic: /elevation_map_raw_visualization/elevation_cloud 174 | Unreliable: false 175 | Use Fixed Frame: true 176 | Use rainbow: true 177 | Value: false 178 | - Class: rviz/Marker 179 | Enabled: false 180 | Marker Topic: /elevation_map_raw_visualization/map_region 181 | Name: Elevation Map Region 182 | Namespaces: 183 | {} 184 | Queue Size: 100 185 | Value: false 186 | Enabled: true 187 | Global Options: 188 | Background Color: 48; 48; 48 189 | Fixed Frame: starleth/odometry 190 | Frame Rate: 30 191 | Name: root 192 | Tools: 193 | - Class: rviz/Interact 194 | Hide Inactive Objects: true 195 | - Class: rviz/MoveCamera 196 | - Class: rviz/Select 197 | - Class: rviz/FocusCamera 198 | - Class: rviz/Measure 199 | - Class: rviz/SetInitialPose 200 | Topic: /initialpose 201 | - Class: rviz/SetGoal 202 | Topic: /move_base_simple/goal 203 | - Class: rviz/PublishPoint 204 | Single click: true 205 | Topic: /clicked_point 206 | Value: true 207 | Views: 208 | Current: 209 | Class: rviz/Orbit 210 | Distance: 2.18666 211 | Enable Stereo Rendering: 212 | Stereo Eye Separation: 0.06 213 | Stereo Focal Distance: 1 214 | Swap Stereo Eyes: false 215 | Value: false 216 | Focal Point: 217 | X: 2.2389 218 | Y: 0.0532341 219 | Z: -0.0239365 220 | Name: Current View 221 | Near Clip Distance: 0.01 222 | Pitch: 0.57733 223 | Target Frame: 224 | Value: Orbit (rviz) 225 | Yaw: 5.37742 226 | Saved: ~ 227 | Window Geometry: 228 | Displays: 229 | collapsed: false 230 | Height: 622 231 | Hide Left Dock: false 232 | Hide Right Dock: true 233 | QMainWindow State: 000000ff00000000fd00000004000000000000016a000001e4fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006400fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000028000001e4000000dd00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f0000040efc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a0056006900650077007300000000280000040e000000b000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004450000003efc0100000002fb0000000800540069006d0065010000000000000445000002f600fffffffb0000000800540069006d00650100000000000004500000000000000000000002d5000001e400000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 234 | Selection: 235 | collapsed: false 236 | Time: 237 | collapsed: false 238 | Tool Properties: 239 | collapsed: false 240 | Views: 241 | collapsed: true 242 | Width: 1093 243 | -------------------------------------------------------------------------------- /elevation_mapping_demos/rviz/turtlebot3_waffle_demo.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 78 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /TF1/Frames1 8 | Splitter Ratio: 0.6088235378265381 9 | Tree Height: 462 10 | - Class: rviz/Selection 11 | Name: Selection 12 | - Class: rviz/Tool Properties 13 | Expanded: 14 | - /2D Pose Estimate1 15 | - /2D Nav Goal1 16 | - /Publish Point1 17 | Name: Tool Properties 18 | Splitter Ratio: 0.5886790156364441 19 | - Class: rviz/Views 20 | Expanded: 21 | - /Current View1 22 | Name: Views 23 | Splitter Ratio: 0.5 24 | - Class: rviz/Time 25 | Experimental: false 26 | Name: Time 27 | SyncMode: 0 28 | SyncSource: RealSense Image 29 | Preferences: 30 | PromptSaveOnExit: true 31 | Toolbars: 32 | toolButtonStyle: 2 33 | Visualization Manager: 34 | Class: "" 35 | Displays: 36 | - Alpha: 0.5 37 | Cell Size: 1 38 | Class: rviz/Grid 39 | Color: 160; 160; 164 40 | Enabled: true 41 | Line Style: 42 | Line Width: 0.029999999329447746 43 | Value: Lines 44 | Name: Grid 45 | Normal Cell Count: 0 46 | Offset: 47 | X: 0 48 | Y: 0 49 | Z: 0 50 | Plane: XY 51 | Plane Cell Count: 10 52 | Reference Frame: 53 | Value: true 54 | - Alpha: 1 55 | Autocompute Intensity Bounds: true 56 | Autocompute Value Bounds: 57 | Max Value: 10 58 | Min Value: -10 59 | Value: true 60 | Axis: Z 61 | Channel Name: intensity 62 | Class: rviz/PointCloud2 63 | Color: 255; 255; 255 64 | Color Transformer: RGB8 65 | Decay Time: 0 66 | Enabled: false 67 | Invert Rainbow: false 68 | Max Color: 255; 255; 255 69 | Max Intensity: 4096 70 | Min Color: 0; 0; 0 71 | Min Intensity: 0 72 | Name: Realsense point cloud 73 | Position Transformer: XYZ 74 | Queue Size: 10 75 | Selectable: true 76 | Size (Pixels): 3 77 | Size (m): 0.009999999776482582 78 | Style: Flat Squares 79 | Topic: /camera/depth/points 80 | Unreliable: false 81 | Use Fixed Frame: true 82 | Use rainbow: true 83 | Value: false 84 | - Class: rviz/TF 85 | Enabled: false 86 | Frame Timeout: 15 87 | Frames: 88 | All Enabled: false 89 | Marker Scale: 1 90 | Name: TF 91 | Show Arrows: true 92 | Show Axes: true 93 | Show Names: true 94 | Tree: 95 | {} 96 | Update Interval: 0 97 | Value: false 98 | - Alpha: 1 99 | Class: rviz/RobotModel 100 | Collision Enabled: false 101 | Enabled: true 102 | Links: 103 | All Links Enabled: true 104 | Expand Joint Details: false 105 | Expand Link Details: false 106 | Expand Tree: false 107 | Link Tree Style: Links in Alphabetic Order 108 | base_footprint: 109 | Alpha: 1 110 | Show Axes: false 111 | Show Trail: false 112 | base_link: 113 | Alpha: 1 114 | Show Axes: false 115 | Show Trail: false 116 | Value: true 117 | base_scan: 118 | Alpha: 1 119 | Show Axes: false 120 | Show Trail: false 121 | Value: true 122 | camera_depth_frame: 123 | Alpha: 1 124 | Show Axes: false 125 | Show Trail: false 126 | camera_depth_optical_frame: 127 | Alpha: 1 128 | Show Axes: false 129 | Show Trail: false 130 | camera_link: 131 | Alpha: 1 132 | Show Axes: false 133 | Show Trail: false 134 | Value: true 135 | camera_rgb_frame: 136 | Alpha: 1 137 | Show Axes: false 138 | Show Trail: false 139 | camera_rgb_optical_frame: 140 | Alpha: 1 141 | Show Axes: false 142 | Show Trail: false 143 | caster_back_left_link: 144 | Alpha: 1 145 | Show Axes: false 146 | Show Trail: false 147 | Value: true 148 | caster_back_right_link: 149 | Alpha: 1 150 | Show Axes: false 151 | Show Trail: false 152 | Value: true 153 | imu_link: 154 | Alpha: 1 155 | Show Axes: false 156 | Show Trail: false 157 | wheel_left_link: 158 | Alpha: 1 159 | Show Axes: false 160 | Show Trail: false 161 | Value: true 162 | wheel_right_link: 163 | Alpha: 1 164 | Show Axes: false 165 | Show Trail: false 166 | Value: true 167 | Name: RobotModel 168 | Robot Description: robot_description 169 | TF Prefix: "" 170 | Update Interval: 0 171 | Value: true 172 | Visual Enabled: true 173 | - Alpha: 1 174 | Axes Length: 1 175 | Axes Radius: 0.10000000149011612 176 | Class: rviz/PoseWithCovariance 177 | Color: 255; 25; 0 178 | Covariance: 179 | Orientation: 180 | Alpha: 0.5 181 | Color: 255; 255; 127 182 | Color Style: Unique 183 | Frame: Local 184 | Offset: 1 185 | Scale: 1 186 | Value: true 187 | Position: 188 | Alpha: 0.30000001192092896 189 | Color: 204; 51; 204 190 | Scale: 1 191 | Value: true 192 | Value: true 193 | Enabled: false 194 | Head Length: 0.30000001192092896 195 | Head Radius: 0.10000000149011612 196 | Name: Pose robot 197 | Shaft Length: 1 198 | Shaft Radius: 0.05000000074505806 199 | Shape: Arrow 200 | Topic: /base_footprint_pose 201 | Unreliable: false 202 | Value: false 203 | - Alpha: 1 204 | Autocompute Intensity Bounds: true 205 | Class: grid_map_rviz_plugin/GridMap 206 | Color: 200; 200; 200 207 | Color Layer: elevation 208 | Color Transformer: GridMapLayer 209 | Enabled: true 210 | Height Layer: elevation 211 | Height Transformer: GridMapLayer 212 | History Length: 1 213 | Invert Rainbow: false 214 | Max Color: 255; 255; 255 215 | Max Intensity: 10 216 | Min Color: 0; 0; 0 217 | Min Intensity: 0 218 | Name: Elevation Map 219 | Show Grid Lines: true 220 | Topic: /elevation_mapping/elevation_map_raw 221 | Unreliable: false 222 | Use Rainbow: true 223 | Value: true 224 | - Class: rviz/Image 225 | Enabled: true 226 | Image Topic: /camera/rgb/image_raw 227 | Max Value: 1 228 | Median window: 5 229 | Min Value: 0 230 | Name: RealSense Image 231 | Normalize Range: true 232 | Queue Size: 2 233 | Transport Hint: raw 234 | Unreliable: false 235 | Value: true 236 | Enabled: true 237 | Global Options: 238 | Background Color: 48; 48; 48 239 | Default Light: true 240 | Fixed Frame: odom 241 | Frame Rate: 30 242 | Name: root 243 | Tools: 244 | - Class: rviz/Interact 245 | Hide Inactive Objects: true 246 | - Class: rviz/MoveCamera 247 | - Class: rviz/Select 248 | - Class: rviz/FocusCamera 249 | - Class: rviz/Measure 250 | - Class: rviz/SetInitialPose 251 | Theta std deviation: 0.2617993950843811 252 | Topic: /initialpose 253 | X std deviation: 0.5 254 | Y std deviation: 0.5 255 | - Class: rviz/SetGoal 256 | Topic: /move_base_simple/goal 257 | - Class: rviz/PublishPoint 258 | Single click: true 259 | Topic: /clicked_point 260 | Value: true 261 | Views: 262 | Current: 263 | Class: rviz/ThirdPersonFollower 264 | Distance: 2.3616132736206055 265 | Enable Stereo Rendering: 266 | Stereo Eye Separation: 0.05999999865889549 267 | Stereo Focal Distance: 1 268 | Swap Stereo Eyes: false 269 | Value: false 270 | Focal Point: 271 | X: 0 272 | Y: 0 273 | Z: 0 274 | Focal Shape Fixed Size: false 275 | Focal Shape Size: 0.05000000074505806 276 | Invert Z Axis: false 277 | Name: Current View 278 | Near Clip Distance: 0.009999999776482582 279 | Pitch: 0.2697971761226654 280 | Target Frame: base_link 281 | Value: ThirdPersonFollower (rviz) 282 | Yaw: 3.1353981494903564 283 | Saved: ~ 284 | Window Geometry: 285 | Displays: 286 | collapsed: false 287 | Height: 1110 288 | Hide Left Dock: false 289 | Hide Right Dock: true 290 | QMainWindow State: 000000ff00000000fd0000000400000000000001f4000003b8fc020000000bfb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d00000259000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000c00430061006d006500720061000000020d0000021e0000000000000000fb0000000c00430061006d0065007200610000000333000001ee0000000000000000fb0000001e005200650061006c00530065006e0073006500200049006d006100670065010000029c000001590000001600ffffff000000010000010f0000030ffc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d0000030f000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000006180000003efc0100000002fb0000000800540069006d0065010000000000000618000002eb00fffffffb0000000800540069006d006501000000000000045000000000000000000000041e000003b800000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 291 | RealSense Image: 292 | collapsed: false 293 | Selection: 294 | collapsed: false 295 | Time: 296 | collapsed: false 297 | Tool Properties: 298 | collapsed: false 299 | Views: 300 | collapsed: true -------------------------------------------------------------------------------- /elevation_mapping_demos/scripts/tf_to_pose_publisher.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | 3 | import rospy 4 | import geometry_msgs.msg 5 | import tf 6 | 7 | def callback(newPose): 8 | """Listens to a transform between from_frame and to_frame and publishes it 9 | as a pose with a zero covariance.""" 10 | global publisher, tf_listener, from_frame, to_frame 11 | 12 | # Listen to transform and throw exception if the transform is not 13 | # available. 14 | try: 15 | (trans, rot) = tf_listener.lookupTransform( 16 | from_frame, to_frame, rospy.Time(0)) 17 | except (tf.LookupException, tf.ConnectivityException, 18 | tf.ExtrapolationException): 19 | return 20 | 21 | # Create and fill pose message for publishing 22 | pose = geometry_msgs.msg.PoseWithCovarianceStamped() 23 | pose.header.stamp = rospy.Time(0) 24 | pose.header.frame_id = from_frame 25 | pose.pose.pose.position.x = trans[0] 26 | pose.pose.pose.position.y = trans[1] 27 | pose.pose.pose.position.z = trans[2] 28 | pose.pose.pose.orientation.x = rot[0] 29 | pose.pose.pose.orientation.y = rot[1] 30 | pose.pose.pose.orientation.z = rot[2] 31 | pose.pose.pose.orientation.w = rot[3] 32 | 33 | # Since tf transforms do not have a covariance, pose is filled with 34 | # a zero covariance. 35 | pose.pose.covariance = [0, 0, 0, 0, 0, 0, 36 | 0, 0, 0, 0, 0, 0, 37 | 0, 0, 0, 0, 0, 0, 38 | 0, 0, 0, 0, 0, 0, 39 | 0, 0, 0, 0, 0, 0, 40 | 0, 0, 0, 0, 0, 0] 41 | 42 | publisher.publish(pose) 43 | 44 | 45 | def main_program(): 46 | """ Main function initializes node and subscribers and starts 47 | the ROS loop. """ 48 | global publisher, tf_listener, from_frame, to_frame 49 | rospy.init_node('tf_to_pose_publisher') 50 | # Read frame id's for tf listener 51 | from_frame = rospy.get_param("~from_frame") 52 | to_frame = rospy.get_param("~to_frame") 53 | pose_name = str(to_frame) + "_pose" 54 | 55 | tf_listener = tf.TransformListener() 56 | publisher = rospy.Publisher( 57 | pose_name, geometry_msgs.msg.PoseWithCovarianceStamped, queue_size=10) 58 | 59 | # Set callback and start spinning 60 | rospy.Timer(rospy.Duration(0.05), callback) 61 | rospy.spin() 62 | 63 | 64 | if __name__ == '__main__': 65 | try: 66 | main_program() 67 | except rospy.ROSInterruptException: 68 | pass 69 | -------------------------------------------------------------------------------- /elevation_mapping_demos/test/empty_test.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | TEST(Placeholder, emptyTest) { // NOLINT 4 | ASSERT_TRUE(true); 5 | } 6 | -------------------------------------------------------------------------------- /jenkins-pipeline: -------------------------------------------------------------------------------- 1 | library 'continuous_integration_pipeline' 2 | ciPipeline("--ignore elevation_mapping_demos") 3 | --------------------------------------------------------------------------------