├── .gitignore ├── README.md ├── octomap_world ├── CMakeLists.txt ├── include │ └── octomap_world │ │ ├── octomap_manager.h │ │ └── octomap_world.h ├── package.xml └── src │ ├── octomap_manager.cc │ ├── octomap_manager_node.cc │ └── octomap_world.cc ├── volumetric_map_base ├── CMakeLists.txt ├── include │ └── volumetric_map_base │ │ ├── point_weighing.h │ │ └── world_base.h ├── package.xml └── src │ └── world_base.cc ├── volumetric_mapping ├── CMakeLists.txt └── package.xml └── volumetric_msgs ├── CMakeLists.txt ├── package.xml └── srv ├── GetChangedPoints.srv ├── LoadMap.srv ├── SaveMap.srv ├── SetBoxOccupancy.srv └── SetDisplayBounds.srv /.gitignore: -------------------------------------------------------------------------------- 1 | # Compiled Object files 2 | *.slo 3 | *.lo 4 | *.o 5 | *.obj 6 | 7 | # Precompiled Headers 8 | *.gch 9 | *.pch 10 | 11 | # Compiled Dynamic libraries 12 | *.so 13 | *.dylib 14 | *.dll 15 | 16 | # Fortran module files 17 | *.mod 18 | 19 | # Compiled Static libraries 20 | *.lai 21 | *.la 22 | *.a 23 | *.lib 24 | 25 | # Executables 26 | *.exe 27 | *.out 28 | *.app 29 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # volumetric_mapping 2 | A repository for 3D volumetric (occupancy) maps, providing a generic interface for disparity map and pointcloud insertion, and support for custom sensor error models. 3 | 4 | ## Packages 5 | **volumetric_map_base** - base class/package that all volumetric maps should inherit from, contains methods to handle ROS disparity maps and pointclouds. 6 | 7 | **volumetric_msgs** - collection of messages for interacting with various maps. 8 | 9 | **octomap_world** - an octomap-based volumetric representation, both with a library and a stand-alone ROS node. 10 | 11 | ## Dependencies 12 | In addition to `ros-indigo-desktop-full`, please install: 13 | ``` 14 | sudo apt-get install ros-indigo-octomap-mapping 15 | ``` 16 | 17 | And the following packages, which can be done all at once with the script below: 18 | [minkindr](https://github.com/ethz-asl/minkindr) 19 | [minkindr_ros](https://github.com/ethz-asl/minkindr_ros) 20 | [eigen_catkin](https://github.com/ethz-asl/eigen_catkin) 21 | [eigen_checks](https://github.com/ethz-asl/eigen_checks) 22 | [glog_catkin](https://github.com/ethz-asl/glog_catkin) 23 | [gflags_catkin](https://github.com/ethz-asl/gflags_catkin) 24 | 25 | ``` 26 | cd ~/catkin_ws/src/ 27 | wstool init 28 | wstool set catkin_simple --git https://github.com/catkin/catkin_simple.git 29 | wstool set eigen_catkin --git https://github.com/ethz-asl/eigen_catkin.git 30 | wstool set eigen_checks --git https://github.com/ethz-asl/eigen_checks.git 31 | wstool set gflags_catkin --git https://github.com/ethz-asl/gflags_catkin.git 32 | wstool set glog_catkin --git https://github.com/ethz-asl/glog_catkin.git 33 | wstool set minkindr --git https://github.com/ethz-asl/minkindr.git 34 | wstool set minkindr_ros --git https://github.com/ethz-asl/minkindr_ros.git 35 | wstool set volumetric_mapping --git https://github.com/ethz-asl/volumetric_mapping.git 36 | wstool update 37 | ``` 38 | 39 | On Mac OS X, see the [mav_tools Wiki instructions](https://github.com/ethz-asl/mav_tools/wiki/Install-the-ASL-MAV-framework#install-extra-stock-ros-packages-octomap-ompl-etc). 40 | 41 | ## Libraries 42 | **[OctomapWorld](https://github.com/ethz-asl/volumetric_mapping/blob/master/octomap_world/include/octomap_world/octomap_world.h)** - general library for handling insertion of pointclouds, can be run outside of a ROS node, and takes parameters as a struct. 43 | 44 | **[OctomapManager](https://github.com/ethz-asl/volumetric_mapping/blob/master/octomap_world/include/octomap_world/octomap_manager.h)** - inherits from OctomapWorld, essentially a ROS wrapper for it. Reads parameters in from the ROS parameter server. 45 | 46 | ## Nodes 47 | ### octomap_manager 48 | Listens to disparity and pointcloud messages and adds them to an octomap. 49 | 50 | #### Parameters 51 | * `tf_frame` (string, default: "/world") - tf frame name to use for the world. 52 | * `resolution` (double, default: 0.15) - resolution each grid cell in meters. 53 | * `Q` (vector of doubles (representing 4x4 matrix, row-major)) - Q projection matrix for disparity projection, in case camera info topics are not available. 54 | * `map_publish_frequency` (double, default: 0.0) - Frequency at which the Octomap is published for visualization purposes. If set to < 0.0, the Octomap is not regularly published (use service call instead). 55 | * `octomap_file` (string, default: "") - Loads an octomap from this path on startup. Use `load_map` service below to load a map from file after startup. 56 | 57 | For other parameters, see [octomap_world.h](https://github.com/ethz-asl/volumetric_mapping/blob/master/octomap_world/include/octomap_world/octomap_world.h#L16-L24). 58 | 59 | #### Subscribed Topics 60 | * `disparity` ([stereo_msgs/DisparityImage]) - disparity image to subscribe to. 61 | * `pointcloud` ([sensor_msgs/PointCloud2]) - pointcloud to subscribe to. 62 | * `cam0/camera_info` ([sensor_msgs/CameraInfo]) - left camera info. 63 | * `cam1/camera_info` ([sensor_msgs/CameraInfo]) - right camera info. 64 | 65 | #### Published Topics 66 | * `octomap_occupied` ([visualization_msgs/MarkerArray]) - marker array showing occupied octomap cells, colored by z. 67 | * `octomap_free` ([visualization_msgs/MarkerArray]) - marker array showing free octomap cells, colored by z. 68 | * `octomap_full` ([octomap_msgs/Octomap]) - octomap with full probabilities. 69 | * `octomap_binary` ([octomap_msgs/Octomap]) - octomap with binary occupancy - free or occupied, taken by max likelihood of each node. 70 | 71 | #### Services 72 | * `reset_map` ([std_srvs/Empty]) - clear the map. 73 | * `publish_all` ([std_srvs/Empty]) - publish all the topics in the above section. 74 | * `get_map` ([octomap_msgs/GetOctomap]) - returns binary octomap message. 75 | * `save_map` ([volumetric_msgs/SaveMap]) - save map to the specified `file_path`. 76 | * `load_map` ([volumetric_msgs/LoadMap]) - load map from the specified `file_path`. 77 | 78 | ## Running 79 | Run an octomap manager, and load a map from disk, then publish it in the `map` tf frame: 80 | 81 | ``` 82 | rosrun octomap_world octomap_manager _tf_frame:=map 83 | rosservice call /octomap_manager/load_map /home/helen/data/my_awesome_octomap.bt 84 | rosservice call /octomap_manager/publish_all 85 | ``` 86 | 87 | [std_srvs/Empty]: http://docs.ros.org/indigo/api/std_srvs/html/srv/Empty.html 88 | [sensor_msgs/PointCloud2]: http://docs.ros.org/api/sensor_msgs/html/msg/PointCloud2.html 89 | [stereo_msgs/DisparityImage]: http://docs.ros.org/api/stereo_msgs/html/msg/DisparityImage.html 90 | [sensor_msgs/CameraInfo]: http://docs.ros.org/api/sensor_msgs/html/msg/CameraInfo.html 91 | [octomap_msgs/Octomap]: http://docs.ros.org/indigo/api/octomap_msgs/html/msg/Octomap.html 92 | [octomap_msgs/GetOctomap]: http://docs.ros.org/indigo/api/octomap_msgs/html/srv/GetOctomap.html 93 | [visualization_msgs/MarkerArray]: http://docs.ros.org/api/visualization_msgs/html/msg/MarkerArray.html 94 | [volumetric_msgs/LoadMap]: https://github.com/ethz-asl/volumetric_mapping/blob/master/volumetric_msgs/srv/LoadMap.srv 95 | [volumetric_msgs/SaveMap]: https://github.com/ethz-asl/volumetric_mapping/blob/master/volumetric_msgs/srv/SaveMap.srv 96 | -------------------------------------------------------------------------------- /octomap_world/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(octomap_world) 3 | 4 | find_package(catkin_simple REQUIRED) 5 | catkin_simple(ALL_DEPS_REQUIRED) 6 | 7 | add_definitions(-std=c++11) 8 | 9 | ############# 10 | # LIBRARIES # 11 | ############# 12 | cs_add_library(${PROJECT_NAME} 13 | src/octomap_world.cc 14 | src/octomap_manager.cc 15 | ) 16 | 17 | ############ 18 | # BINARIES # 19 | ############ 20 | cs_add_executable(octomap_manager 21 | src/octomap_manager_node.cc 22 | ) 23 | target_link_libraries(octomap_manager ${PROJECT_NAME}) 24 | 25 | ########## 26 | # EXPORT # 27 | ########## 28 | cs_install() 29 | cs_export() 30 | -------------------------------------------------------------------------------- /octomap_world/include/octomap_world/octomap_manager.h: -------------------------------------------------------------------------------- 1 | /* 2 | Copyright (c) 2015, Helen Oleynikova, ETH Zurich, Switzerland 3 | You can contact the author at 4 | 5 | All rights reserved. 6 | 7 | Redistribution and use in source and binary forms, with or without 8 | modification, are permitted provided that the following conditions are met: 9 | * Redistributions of source code must retain the above copyright 10 | notice, this list of conditions and the following disclaimer. 11 | * Redistributions in binary form must reproduce the above copyright 12 | notice, this list of conditions and the following disclaimer in the 13 | documentation and/or other materials provided with the distribution. 14 | * Neither the name of ETHZ-ASL nor the 15 | names of its contributors may be used to endorse or promote products 16 | derived from this software without specific prior written permission. 17 | 18 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 19 | ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 20 | WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 21 | DISCLAIMED. IN NO EVENT SHALL ETHZ-ASL BE LIABLE FOR ANY 22 | DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 23 | (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 24 | LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 25 | ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 26 | (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 27 | SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 28 | */ 29 | 30 | #ifndef OCTOMAP_WORLD_OCTOMAP_MANAGER_H_ 31 | #define OCTOMAP_WORLD_OCTOMAP_MANAGER_H_ 32 | 33 | #include 34 | #include 35 | 36 | #include "octomap_world/octomap_world.h" 37 | 38 | #include 39 | #include 40 | #include 41 | #include 42 | #include 43 | #include 44 | #include 45 | #include 46 | 47 | #include 48 | 49 | namespace volumetric_mapping { 50 | 51 | // An inherited class from OctomapWorld, which also handles the connection to 52 | // ROS via publishers, subscribers, service calls, etc. 53 | class OctomapManager : public OctomapWorld { 54 | public: 55 | typedef std::shared_ptr Ptr; 56 | 57 | // By default, loads octomap parameters from the ROS parameter server. 58 | OctomapManager(const ros::NodeHandle& nh, const ros::NodeHandle& nh_private); 59 | 60 | void publishAll(); 61 | void publishAllEvent(const ros::TimerEvent& e); 62 | 63 | // Data insertion callbacks with TF frame resolution through the listener. 64 | void insertDisparityImageWithTf( 65 | const stereo_msgs::DisparityImageConstPtr& disparity); 66 | void insertPointcloudWithTf( 67 | const sensor_msgs::PointCloud2::ConstPtr& pointcloud); 68 | 69 | // Input Octomap callback. 70 | void octomapCallback(const octomap_msgs::Octomap& msg); 71 | 72 | // Camera info callbacks. 73 | void leftCameraInfoCallback(const sensor_msgs::CameraInfoPtr& left_info); 74 | void rightCameraInfoCallback(const sensor_msgs::CameraInfoPtr& right_info); 75 | 76 | // Service callbacks. 77 | bool resetMapCallback(std_srvs::Empty::Request& request, 78 | std_srvs::Empty::Response& response); 79 | bool publishAllCallback(std_srvs::Empty::Request& request, 80 | std_srvs::Empty::Response& response); 81 | bool getOctomapCallback(octomap_msgs::GetOctomap::Request& request, 82 | octomap_msgs::GetOctomap::Response& response); 83 | 84 | bool loadOctomapCallback(volumetric_msgs::LoadMap::Request& request, 85 | volumetric_msgs::LoadMap::Response& response); 86 | bool saveOctomapCallback(volumetric_msgs::SaveMap::Request& request, 87 | volumetric_msgs::SaveMap::Response& response); 88 | bool savePointCloudCallback(volumetric_msgs::SaveMap::Request& request, 89 | volumetric_msgs::SaveMap::Response& response); 90 | 91 | bool setBoxOccupancyCallback( 92 | volumetric_msgs::SetBoxOccupancy::Request& request, 93 | volumetric_msgs::SetBoxOccupancy::Response& response); 94 | bool setDisplayBoundsCallback( 95 | volumetric_msgs::SetDisplayBounds::Request& request, 96 | volumetric_msgs::SetDisplayBounds::Response& response); 97 | 98 | bool getChangedPointsCallback( 99 | volumetric_msgs::GetChangedPoints::Request& request, 100 | volumetric_msgs::GetChangedPoints::Response& response); 101 | 102 | void transformCallback(const geometry_msgs::TransformStamped& transform_msg); 103 | 104 | private: 105 | // Sets up subscriptions based on ROS node parameters. 106 | void setParametersFromROS(); 107 | void subscribe(); 108 | void advertiseServices(); 109 | void advertisePublishers(); 110 | 111 | bool setQFromParams(std::vector* Q_vec); 112 | void calculateQ(); 113 | bool lookupTransform(const std::string& from_frame, 114 | const std::string& to_frame, const ros::Time& timestamp, 115 | Transformation* transform); 116 | bool lookupTransformTf(const std::string& from_frame, 117 | const std::string& to_frame, 118 | const ros::Time& timestamp, Transformation* transform); 119 | bool lookupTransformQueue(const std::string& from_frame, 120 | const std::string& to_frame, 121 | const ros::Time& timestamp, 122 | Transformation* transform); 123 | 124 | ros::NodeHandle nh_; 125 | ros::NodeHandle nh_private_; 126 | 127 | tf::TransformListener tf_listener_; 128 | 129 | // Global/map coordinate frame. Will always look up TF transforms to this 130 | // frame. 131 | std::string world_frame_; 132 | std::string robot_frame_; 133 | // Whether to use TF transform resolution (true) or fixed transforms from 134 | // parameters and transform topics (false). 135 | bool use_tf_transforms_; 136 | int64_t timestamp_tolerance_ns_; 137 | // B is the body frame of the robot, C is the camera/sensor frame creating 138 | // the pointclouds, and D is the 'dynamic' frame; i.e., incoming messages 139 | // are assumed to be T_G_D. 140 | Transformation T_B_C_; 141 | Transformation T_B_D_; 142 | 143 | bool latch_topics_; 144 | // Subscriptions for input sensor data. 145 | ros::Subscriber disparity_sub_; 146 | ros::Subscriber left_info_sub_; 147 | ros::Subscriber right_info_sub_; 148 | ros::Subscriber pointcloud_sub_; 149 | ros::Subscriber octomap_sub_; 150 | 151 | // Only used if use_tf_transforms_ set to false. 152 | ros::Subscriber transform_sub_; 153 | 154 | // Publish full state of octomap. 155 | ros::Publisher binary_map_pub_; 156 | ros::Publisher full_map_pub_; 157 | 158 | // Publish voxel centroids as pcl. 159 | ros::Publisher nearest_obstacle_pub_; 160 | ros::Publisher pcl_pub_; 161 | 162 | // Publish markers for visualization. 163 | ros::Publisher occupied_nodes_pub_; 164 | ros::Publisher free_nodes_pub_; 165 | 166 | // Services! 167 | ros::ServiceServer reset_map_service_; 168 | ros::ServiceServer publish_all_service_; 169 | ros::ServiceServer get_map_service_; 170 | ros::ServiceServer save_octree_service_; 171 | ros::ServiceServer load_octree_service_; 172 | ros::ServiceServer save_point_cloud_service_; 173 | ros::ServiceServer set_box_occupancy_service_; 174 | ros::ServiceServer set_display_bounds_service_; 175 | // IMPORTANT NOTE: change_detection MUST be enabled in order for this to work! 176 | // Otherwise it just gives 0 changed points. 177 | ros::ServiceServer get_changed_points_service_; 178 | 179 | // Keep state of the cameras. 180 | sensor_msgs::CameraInfoPtr left_info_; 181 | sensor_msgs::CameraInfoPtr right_info_; 182 | 183 | // Only calculate Q matrix for disparity once. 184 | bool Q_initialized_; 185 | Eigen::Matrix4d Q_; 186 | Eigen::Vector2d full_image_size_; 187 | double map_publish_frequency_; 188 | ros::Timer map_publish_timer_; 189 | 190 | // Transform queue, used only when use_tf_transforms is false. 191 | std::deque transform_queue_; 192 | }; 193 | 194 | } // namespace volumetric_mapping 195 | 196 | #endif // OCTOMAP_WORLD_OCTOMAP_MANAGER_H_ 197 | -------------------------------------------------------------------------------- /octomap_world/include/octomap_world/octomap_world.h: -------------------------------------------------------------------------------- 1 | /* 2 | Copyright (c) 2015, Helen Oleynikova, ETH Zurich, Switzerland 3 | You can contact the author at 4 | 5 | All rights reserved. 6 | 7 | Redistribution and use in source and binary forms, with or without 8 | modification, are permitted provided that the following conditions are met: 9 | * Redistributions of source code must retain the above copyright 10 | notice, this list of conditions and the following disclaimer. 11 | * Redistributions in binary form must reproduce the above copyright 12 | notice, this list of conditions and the following disclaimer in the 13 | documentation and/or other materials provided with the distribution. 14 | * Neither the name of ETHZ-ASL nor the 15 | names of its contributors may be used to endorse or promote products 16 | derived from this software without specific prior written permission. 17 | 18 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 19 | ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 20 | WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 21 | DISCLAIMED. IN NO EVENT SHALL ETHZ-ASL BE LIABLE FOR ANY 22 | DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 23 | (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 24 | LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 25 | ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 26 | (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 27 | SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 28 | */ 29 | 30 | #ifndef OCTOMAP_WORLD_OCTOMAP_WORLD_H_ 31 | #define OCTOMAP_WORLD_OCTOMAP_WORLD_H_ 32 | 33 | #include 34 | 35 | #include 36 | #include 37 | #include 38 | #include 39 | #include 40 | 41 | namespace volumetric_mapping { 42 | 43 | // Different behaviours for setting log_odds_value in a bounding box 44 | enum BoundHandling { 45 | kDefault, 46 | // kIncludePartialBoxes: consider also boxes that are only partially included 47 | // in bounding box. 48 | kIncludePartialBoxes, 49 | // kIgnorePartialBoxes: consider only boxes that are fully included in 50 | // bounding box. 51 | kIgnorePartialBoxes 52 | }; 53 | 54 | struct OctomapParameters { 55 | OctomapParameters() 56 | : resolution(0.15), 57 | probability_hit(0.65), 58 | probability_miss(0.4), 59 | threshold_min(0.12), 60 | threshold_max(0.97), 61 | threshold_occupancy(0.7), 62 | filter_speckles(true), 63 | max_free_space(0.0), 64 | min_height_free_space(0.0), 65 | sensor_max_range(5.0), 66 | visualize_min_z(-std::numeric_limits::max()), 67 | visualize_max_z(std::numeric_limits::max()), 68 | treat_unknown_as_occupied(true), 69 | change_detection_enabled(false) { 70 | // Set reasonable defaults here... 71 | } 72 | 73 | // Resolution for the Octree. It is not possible to change this without 74 | // creating a new Octree. 75 | double resolution; 76 | // Hit probabilities for pointcloud data. 77 | double probability_hit; 78 | double probability_miss; 79 | // Clamping thresholds for pruning: above and below these thresholds, all 80 | // values are treated the same. 81 | double threshold_min; 82 | double threshold_max; 83 | // Threshold considered for a cell to be occupied. 84 | double threshold_occupancy; 85 | 86 | // Filter neighbor-less nodes as 'speckles'. 87 | bool filter_speckles; 88 | 89 | // Maximum range to allow a free space update. 90 | double max_free_space; 91 | 92 | // Minimum height below sensor to allow a free space update. 93 | double min_height_free_space; 94 | 95 | // Maximum range to allow a sensor measurement. Negative values to not 96 | // filter. 97 | double sensor_max_range; 98 | 99 | // Minimum and maximum z to visualize. Only used for marker, not full 100 | // octomap, visualization. 101 | double visualize_min_z; 102 | double visualize_max_z; 103 | 104 | // Collision checking. 105 | bool treat_unknown_as_occupied; 106 | 107 | // Whether to track changes -- must be set to true to use getChangedPoints(). 108 | bool change_detection_enabled; 109 | }; 110 | 111 | // A wrapper around octomap that allows insertion from various ROS message 112 | // data sources, given their transforms from sensor frame to world frame. 113 | // Does not need to run within a ROS node, does not do any TF look-ups, and 114 | // does not publish/subscribe to anything (though provides serialization 115 | // and deserialization functions to and from ROS messages). 116 | class OctomapWorld : public WorldBase { 117 | typedef std::shared_ptr Ptr; 118 | 119 | public: 120 | // Default constructor - creates a valid octree using parameter defaults. 121 | OctomapWorld(); 122 | 123 | // Creates an octomap with the correct parameters. 124 | OctomapWorld(const OctomapParameters& params); 125 | 126 | // Deep copy of OctomapWorld rhs 127 | OctomapWorld(const OctomapWorld& rhs); 128 | 129 | virtual ~OctomapWorld() {} 130 | 131 | // General map management. 132 | void resetMap(); 133 | void prune(); 134 | // Creates an octomap if one is not yet created or if the resolution of the 135 | // current varies from the parameters requested. 136 | void setOctomapParameters(const OctomapParameters& params); 137 | void getOctomapParameters(OctomapParameters* params) const; 138 | 139 | // Virtual functions for manually manipulating map probabilities. 140 | virtual void setFree( 141 | const Eigen::Vector3d& position, const Eigen::Vector3d& bounding_box_size, 142 | const BoundHandling& insertion_method = BoundHandling::kDefault); 143 | virtual void setFree( 144 | const std::vector& positions, 145 | const Eigen::Vector3d& bounding_box_size, 146 | const BoundHandling& insertion_method = BoundHandling::kDefault); 147 | virtual void setOccupied( 148 | const Eigen::Vector3d& position, const Eigen::Vector3d& bounding_box_size, 149 | const BoundHandling& insertion_method = BoundHandling::kDefault); 150 | virtual void setOccupied( 151 | const std::vector& positions, 152 | const Eigen::Vector3d& bounding_box_size, 153 | const BoundHandling& insertion_method = BoundHandling::kDefault); 154 | virtual void setBordersOccupied(const Eigen::Vector3d& cropping_size); 155 | 156 | // Virtual functions for outputting map status. If treat_unknown_as_occupied 157 | // is set to true, those functions return kOccupied instead of kUnknown. 158 | void enableTreatUnknownAsOccupied(); 159 | void disableTreatUnknownAsOccupied(); 160 | virtual CellStatus getCellStatusBoundingBox( 161 | const Eigen::Vector3d& point, 162 | const Eigen::Vector3d& bounding_box_size) const; 163 | virtual CellStatus getCellStatusPoint(const Eigen::Vector3d& point) const; 164 | virtual CellStatus getCellTrueStatusPoint(const Eigen::Vector3d& point) const; 165 | virtual CellStatus getCellProbabilityPoint(const Eigen::Vector3d& point, 166 | double* probability) const; 167 | virtual CellStatus getLineStatus(const Eigen::Vector3d& start, 168 | const Eigen::Vector3d& end) const; 169 | virtual CellStatus getVisibility(const Eigen::Vector3d& view_point, 170 | const Eigen::Vector3d& voxel_to_test, 171 | bool stop_at_unknown_cell) const; 172 | virtual CellStatus getLineStatusBoundingBox( 173 | const Eigen::Vector3d& start, const Eigen::Vector3d& end, 174 | const Eigen::Vector3d& bounding_box_size) const; 175 | virtual void getOccupiedPointCloud( 176 | pcl::PointCloud* output_cloud) const; 177 | virtual void getOccupiedPointcloudInBoundingBox( 178 | const Eigen::Vector3d& center, const Eigen::Vector3d& bounding_box_size, 179 | pcl::PointCloud* output_cloud, 180 | const BoundHandling& insertion_method = BoundHandling::kDefault) const; 181 | 182 | // Structure: vector of pairs, key is the cube center and double is the 183 | // dimension of each side. 184 | void getAllFreeBoxes( 185 | std::vector >* free_box_vector) const; 186 | void getAllOccupiedBoxes(std::vector >* 187 | occupied_box_vector) const; 188 | void getBox(const octomap::OcTreeKey& key, 189 | std::pair* box) const; 190 | void getFreeBoxesBoundingBox( 191 | const Eigen::Vector3d& position, const Eigen::Vector3d& bounding_box_size, 192 | std::vector >* free_box_vector) const; 193 | void getOccupiedBoxesBoundingBox( 194 | const Eigen::Vector3d& position, const Eigen::Vector3d& bounding_box_size, 195 | std::vector >* occupied_box_vector) 196 | const; 197 | 198 | virtual double getResolution() const; 199 | virtual Eigen::Vector3d getMapCenter() const; 200 | virtual Eigen::Vector3d getMapSize() const; 201 | virtual void getMapBounds(Eigen::Vector3d* min_bound, 202 | Eigen::Vector3d* max_bound) const; 203 | bool getNearestFreePoint(const Eigen::Vector3d& position, 204 | Eigen::Vector3d* free_position) const; 205 | 206 | // Collision checking with robot model. Implemented as a box with our own 207 | // implementation. 208 | virtual void setRobotSize(const Eigen::Vector3d& robot_size); 209 | virtual Eigen::Vector3d getRobotSize() const; 210 | virtual bool checkCollisionWithRobot(const Eigen::Vector3d& robot_position); 211 | // Checks a path (assumed to be time-ordered) for collision. 212 | // Sets the second input to the index at which the collision occurred. 213 | virtual bool checkPathForCollisionsWithRobot( 214 | const std::vector& robot_positions, 215 | size_t* collision_index); 216 | 217 | // Serialization and deserialization from ROS messages. 218 | bool getOctomapBinaryMsg(octomap_msgs::Octomap* msg) const; 219 | bool getOctomapFullMsg(octomap_msgs::Octomap* msg) const; 220 | // Clears the current octomap and replaces it with one from the message. 221 | void setOctomapFromMsg(const octomap_msgs::Octomap& msg); 222 | 223 | // Loading and writing to disk. 224 | bool loadOctomapFromFile(const std::string& filename); 225 | bool writeOctomapToFile(const std::string& filename); 226 | 227 | // Writing binary octomap to stream 228 | bool writeOctomapToBinaryConst(std::ostream& s) const; 229 | 230 | // Helpers for publishing. 231 | void generateMarkerArray(const std::string& tf_frame, 232 | visualization_msgs::MarkerArray* occupied_nodes, 233 | visualization_msgs::MarkerArray* free_nodes); 234 | 235 | // Convert all unknown space into free space. 236 | void convertUnknownToFree(); 237 | // Convert unknown space between min_bound and max_bound into free space. 238 | void convertUnknownToFree(const Eigen::Vector3d& min_bound, 239 | const Eigen::Vector3d& max_bound); 240 | // Inflation of all obstacles by safety_space. 241 | void inflateOccupied(const Eigen::Vector3d& safety_space); 242 | 243 | // Change detection -- when this is called, this resets the change detection 244 | // tracking within the map. So 2 consecutive calls will produce first the 245 | // change set, then nothing. 246 | // If not NULL, changed_states contains the new state of the node -- 1 is 247 | // occupied, 0 is free. 248 | // IMPORTANT NOTE: change_detection MUST be set to true in the parameters in 249 | // order for this to work! 250 | void getChangedPoints(std::vector* changed_points, 251 | std::vector* changed_states); 252 | void enableChangeDetection() { octree_->enableChangeDetection(true); } 253 | void disableChangeDetection() { octree_->enableChangeDetection(false); } 254 | 255 | void coordToKey(const Eigen::Vector3d& coord, octomap::OcTreeKey* key) const; 256 | void keyToCoord(const octomap::OcTreeKey& key, Eigen::Vector3d* coord) const; 257 | 258 | protected: 259 | // Actual implementation for inserting disparity data. 260 | virtual void insertProjectedDisparityIntoMapImpl( 261 | const Transformation& sensor_to_world, const cv::Mat& projected_points); 262 | 263 | // Actual implementation for inserting pointclouds. 264 | virtual void insertPointcloudIntoMapImpl( 265 | const Transformation& T_G_sensor, 266 | const pcl::PointCloud::Ptr& pointcloud); 267 | 268 | // Check if the node at the specified key has neighbors or not. 269 | bool isSpeckleNode(const octomap::OcTreeKey& key) const; 270 | 271 | // Manually affect the probabilities of areas within a bounding box. 272 | void setLogOddsBoundingBox( 273 | const Eigen::Vector3d& position, const Eigen::Vector3d& bounding_box_size, 274 | double log_odds_value, 275 | const BoundHandling& insertion_method = BoundHandling::kDefault); 276 | void setLogOddsBoundingBox( 277 | const std::vector& positions, 278 | const Eigen::Vector3d& bounding_box_size, double log_odds_value, 279 | const BoundHandling& insertion_method = BoundHandling::kDefault); 280 | 281 | void getAllBoxes( 282 | bool occupied_boxes, 283 | std::vector >* box_vector) const; 284 | void getBoxesBoundingBox(bool occupied_boxes, const Eigen::Vector3d& position, 285 | const Eigen::Vector3d& bounding_box_size, 286 | std::vector >* 287 | occupied_box_vector) const; 288 | 289 | void getKeysBoundingBox( 290 | const Eigen::Vector3d& position, const Eigen::Vector3d& bounding_box_size, 291 | octomap::KeySet* keys, 292 | const BoundHandling& insertion_method = BoundHandling::kDefault) const; 293 | 294 | // Helper function to align bounding_box 295 | void adjustBoundingBox(const Eigen::Vector3d& position, 296 | const Eigen::Vector3d& bounding_box_size, 297 | const BoundHandling& insertion_method, 298 | Eigen::Vector3d* bbx_min, 299 | Eigen::Vector3d* bbx_max) const; 300 | 301 | // Helper functions for building up a map from sensor data. 302 | void castRay(const octomap::point3d& sensor_origin, 303 | const octomap::point3d& point, octomap::KeySet* free_cells, 304 | octomap::KeySet* occupied_cells); 305 | void updateOccupancy(octomap::KeySet* free_cells, 306 | octomap::KeySet* occupied_cells); 307 | bool isValidPoint(const cv::Vec3f& point) const; 308 | 309 | void setOctomapFromBinaryMsg(const octomap_msgs::Octomap& msg); 310 | void setOctomapFromFullMsg(const octomap_msgs::Octomap& msg); 311 | 312 | double colorizeMapByHeight(double z, double min_z, double max_z) const; 313 | 314 | // Collision checking methods. 315 | bool checkSinglePoseCollision(const Eigen::Vector3d& robot_position) const; 316 | 317 | std_msgs::ColorRGBA percentToColor(double h) const; 318 | 319 | std::shared_ptr octree_; 320 | 321 | OctomapParameters params_; 322 | 323 | // For collision checking. 324 | Eigen::Vector3d robot_size_; 325 | 326 | // Temporary variable for KeyRay since it resizes it to a HUGE value by 327 | // default. Thanks a lot to @xiaopenghuang for catching this. 328 | octomap::KeyRay key_ray_; 329 | }; 330 | 331 | } // namespace volumetric_mapping 332 | 333 | #endif // OCTOMAP_WORLD_OCTOMAP_WORLD_H_ 334 | -------------------------------------------------------------------------------- /octomap_world/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | octomap_world 4 | 0.0.0 5 | 6 | A wrapper for Octomap, allowing insertions of pointclouds and disparity maps. 7 | Inherits from WorldBase in volumetric_map_base package. 8 | 9 | 10 | Helen Oleynikova 11 | Markus Achtelik 12 | 13 | Helen Oleynikova 14 | 15 | BSD 16 | 17 | catkin 18 | catkin_simple 19 | 20 | minkindr_conversions 21 | octomap 22 | octomap_msgs 23 | octomap_ros 24 | pcl_conversions 25 | pcl_ros 26 | volumetric_map_base 27 | volumetric_msgs 28 | 29 | -------------------------------------------------------------------------------- /octomap_world/src/octomap_manager.cc: -------------------------------------------------------------------------------- 1 | /* 2 | Copyright (c) 2015, Helen Oleynikova, ETH Zurich, Switzerland 3 | You can contact the author at 4 | 5 | All rights reserved. 6 | 7 | Redistribution and use in source and binary forms, with or without 8 | modification, are permitted provided that the following conditions are met: 9 | * Redistributions of source code must retain the above copyright 10 | notice, this list of conditions and the following disclaimer. 11 | * Redistributions in binary form must reproduce the above copyright 12 | notice, this list of conditions and the following disclaimer in the 13 | documentation and/or other materials provided with the distribution. 14 | * Neither the name of ETHZ-ASL nor the 15 | names of its contributors may be used to endorse or promote products 16 | derived from this software without specific prior written permission. 17 | 18 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 19 | ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 20 | WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 21 | DISCLAIMED. IN NO EVENT SHALL ETHZ-ASL BE LIABLE FOR ANY 22 | DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 23 | (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 24 | LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 25 | ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 26 | (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 27 | SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 28 | */ 29 | 30 | #include "octomap_world/octomap_manager.h" 31 | 32 | #include 33 | #include 34 | #include 35 | #include 36 | #include 37 | #include 38 | 39 | namespace volumetric_mapping { 40 | 41 | OctomapManager::OctomapManager(const ros::NodeHandle& nh, 42 | const ros::NodeHandle& nh_private) 43 | : nh_(nh), 44 | nh_private_(nh_private), 45 | world_frame_("world"), 46 | robot_frame_("state"), 47 | use_tf_transforms_(true), 48 | latch_topics_(true), 49 | timestamp_tolerance_ns_(10000000), 50 | Q_initialized_(false), 51 | Q_(Eigen::Matrix4d::Identity()), 52 | full_image_size_(752, 480), 53 | map_publish_frequency_(0.0) { 54 | setParametersFromROS(); 55 | subscribe(); 56 | advertiseServices(); 57 | advertisePublishers(); 58 | 59 | // After creating the manager, if the octomap_file parameter is set, 60 | // load the octomap at that path and publish it. 61 | std::string octomap_file; 62 | if (nh_private_.getParam("octomap_file", octomap_file)) { 63 | if (loadOctomapFromFile(octomap_file)) { 64 | ROS_INFO_STREAM( 65 | "Successfully loaded octomap from path: " << octomap_file); 66 | publishAll(); 67 | } else { 68 | ROS_ERROR_STREAM("Could not load octomap from path: " << octomap_file); 69 | } 70 | } 71 | } 72 | 73 | void OctomapManager::setParametersFromROS() { 74 | OctomapParameters params; 75 | nh_private_.param("tf_frame", world_frame_, world_frame_); 76 | nh_private_.param("robot_frame", robot_frame_, robot_frame_); 77 | nh_private_.param("resolution", params.resolution, params.resolution); 78 | nh_private_.param("probability_hit", params.probability_hit, 79 | params.probability_hit); 80 | nh_private_.param("probability_miss", params.probability_miss, 81 | params.probability_miss); 82 | nh_private_.param("threshold_min", params.threshold_min, 83 | params.threshold_min); 84 | nh_private_.param("threshold_max", params.threshold_max, 85 | params.threshold_max); 86 | nh_private_.param("threshold_occupancy", params.threshold_occupancy, 87 | params.threshold_occupancy); 88 | nh_private_.param("filter_speckles", params.filter_speckles, 89 | params.filter_speckles); 90 | nh_private_.param("max_free_space", params.max_free_space, 91 | params.max_free_space); 92 | nh_private_.param("min_height_free_space", params.min_height_free_space, 93 | params.min_height_free_space); 94 | nh_private_.param("sensor_max_range", params.sensor_max_range, 95 | params.sensor_max_range); 96 | nh_private_.param("visualize_min_z", params.visualize_min_z, 97 | params.visualize_min_z); 98 | nh_private_.param("visualize_max_z", params.visualize_max_z, 99 | params.visualize_max_z); 100 | nh_private_.param("full_image_width", full_image_size_.x(), 101 | full_image_size_.x()); 102 | nh_private_.param("full_image_height", full_image_size_.y(), 103 | full_image_size_.y()); 104 | nh_private_.param("map_publish_frequency", map_publish_frequency_, 105 | map_publish_frequency_); 106 | nh_private_.param("treat_unknown_as_occupied", 107 | params.treat_unknown_as_occupied, 108 | params.treat_unknown_as_occupied); 109 | nh_private_.param("change_detection_enabled", params.change_detection_enabled, 110 | params.change_detection_enabled); 111 | 112 | // Try to initialize Q matrix from parameters, if available. 113 | std::vector Q_vec; 114 | if (nh_private_.getParam("Q", Q_vec)) { 115 | Q_initialized_ = setQFromParams(&Q_vec); 116 | } 117 | 118 | // Publisher/subscriber settings. 119 | nh_private_.param("latch_topics", latch_topics_, latch_topics_); 120 | 121 | // Transform settings. 122 | nh_private_.param("use_tf_transforms", use_tf_transforms_, 123 | use_tf_transforms_); 124 | // If we use topic transforms, we have 2 parts: a dynamic transform from a 125 | // topic and a static transform from parameters. 126 | // Static transform should be T_G_D (where D is whatever sensor the 127 | // dynamic coordinate frame is in) and the static should be T_D_C (where 128 | // C is the sensor frame that produces the depth data). It is possible to 129 | // specific T_C_D and set invert_static_tranform to true. 130 | if (!use_tf_transforms_) { 131 | transform_sub_ = nh_.subscribe("transform", 40, 132 | &OctomapManager::transformCallback, this); 133 | // Retrieve T_D_C from params. 134 | XmlRpc::XmlRpcValue T_B_D_xml; 135 | // TODO(helenol): split out into a function to avoid duplication. 136 | if (nh_private_.getParam("T_B_D", T_B_D_xml)) { 137 | kindr::minimal::xmlRpcToKindr(T_B_D_xml, &T_B_D_); 138 | 139 | // See if we need to invert it. 140 | bool invert_static_tranform = false; 141 | nh_private_.param("invert_T_B_D", invert_static_tranform, 142 | invert_static_tranform); 143 | if (invert_static_tranform) { 144 | T_B_D_ = T_B_D_.inverse(); 145 | } 146 | } 147 | XmlRpc::XmlRpcValue T_B_C_xml; 148 | if (nh_private_.getParam("T_B_C", T_B_C_xml)) { 149 | kindr::minimal::xmlRpcToKindr(T_B_C_xml, &T_B_C_); 150 | 151 | // See if we need to invert it. 152 | bool invert_static_tranform = false; 153 | nh_private_.param("invert_T_B_C", invert_static_tranform, 154 | invert_static_tranform); 155 | if (invert_static_tranform) { 156 | T_B_C_ = T_B_C_.inverse(); 157 | } 158 | } 159 | } 160 | 161 | // Set the parent class parameters. 162 | setOctomapParameters(params); 163 | } 164 | 165 | bool OctomapManager::setQFromParams(std::vector* Q_vec) { 166 | if (Q_vec->size() != 16) { 167 | ROS_ERROR_STREAM("Invalid Q matrix size, expected size: 16, actual size: " 168 | << Q_vec->size()); 169 | return false; 170 | } 171 | 172 | // Try to map the vector as coefficients. 173 | Eigen::Map > Q_vec_map( 174 | Q_vec->data()); 175 | // Copy over to the Q member. 176 | Q_ = Q_vec_map; 177 | 178 | return true; 179 | } 180 | 181 | void OctomapManager::subscribe() { 182 | left_info_sub_ = nh_.subscribe("cam0/camera_info", 1, 183 | &OctomapManager::leftCameraInfoCallback, this); 184 | right_info_sub_ = nh_.subscribe( 185 | "cam1/camera_info", 1, &OctomapManager::rightCameraInfoCallback, this); 186 | disparity_sub_ = nh_.subscribe( 187 | "disparity", 40, &OctomapManager::insertDisparityImageWithTf, this); 188 | pointcloud_sub_ = nh_.subscribe( 189 | "pointcloud", 40, &OctomapManager::insertPointcloudWithTf, this); 190 | octomap_sub_ = 191 | nh_.subscribe("input_octomap", 1, &OctomapManager::octomapCallback, this); 192 | } 193 | 194 | void OctomapManager::octomapCallback(const octomap_msgs::Octomap& msg) { 195 | setOctomapFromMsg(msg); 196 | publishAll(); 197 | ROS_INFO_ONCE("Got octomap from message."); 198 | } 199 | 200 | void OctomapManager::advertiseServices() { 201 | reset_map_service_ = nh_private_.advertiseService( 202 | "reset_map", &OctomapManager::resetMapCallback, this); 203 | publish_all_service_ = nh_private_.advertiseService( 204 | "publish_all", &OctomapManager::publishAllCallback, this); 205 | get_map_service_ = nh_private_.advertiseService( 206 | "get_map", &OctomapManager::getOctomapCallback, this); 207 | save_octree_service_ = nh_private_.advertiseService( 208 | "save_map", &OctomapManager::saveOctomapCallback, this); 209 | load_octree_service_ = nh_private_.advertiseService( 210 | "load_map", &OctomapManager::loadOctomapCallback, this); 211 | save_point_cloud_service_ = nh_private_.advertiseService( 212 | "save_point_cloud", &OctomapManager::savePointCloudCallback, this); 213 | set_box_occupancy_service_ = nh_private_.advertiseService( 214 | "set_box_occupancy", &OctomapManager::setBoxOccupancyCallback, this); 215 | set_display_bounds_service_ = nh_private_.advertiseService( 216 | "set_display_bounds", &OctomapManager::setDisplayBoundsCallback, this); 217 | get_changed_points_service_ = nh_private_.advertiseService( 218 | "get_changed_points", &OctomapManager::getChangedPointsCallback, this); 219 | } 220 | 221 | void OctomapManager::advertisePublishers() { 222 | occupied_nodes_pub_ = nh_private_.advertise( 223 | "octomap_occupied", 1, latch_topics_); 224 | free_nodes_pub_ = nh_private_.advertise( 225 | "octomap_free", 1, latch_topics_); 226 | 227 | binary_map_pub_ = nh_private_.advertise( 228 | "octomap_binary", 1, latch_topics_); 229 | full_map_pub_ = nh_private_.advertise( 230 | "octomap_full", 1, latch_topics_); 231 | 232 | pcl_pub_ = nh_private_.advertise("octomap_pcl", 1, 233 | latch_topics_); 234 | nearest_obstacle_pub_ = nh_private_.advertise( 235 | "nearest_obstacle", 1, false); 236 | 237 | if (map_publish_frequency_ > 0.0) { 238 | map_publish_timer_ = 239 | nh_private_.createTimer(ros::Duration(1.0 / map_publish_frequency_), 240 | &OctomapManager::publishAllEvent, this); 241 | } 242 | } 243 | 244 | void OctomapManager::publishAll() { 245 | if (latch_topics_ || occupied_nodes_pub_.getNumSubscribers() > 0 || 246 | free_nodes_pub_.getNumSubscribers() > 0) { 247 | visualization_msgs::MarkerArray occupied_nodes, free_nodes; 248 | generateMarkerArray(world_frame_, &occupied_nodes, &free_nodes); 249 | occupied_nodes_pub_.publish(occupied_nodes); 250 | free_nodes_pub_.publish(free_nodes); 251 | } 252 | 253 | if (latch_topics_ || binary_map_pub_.getNumSubscribers() > 0) { 254 | octomap_msgs::Octomap binary_map; 255 | getOctomapBinaryMsg(&binary_map); 256 | binary_map.header.frame_id = world_frame_; 257 | binary_map_pub_.publish(binary_map); 258 | } 259 | 260 | if (latch_topics_ || full_map_pub_.getNumSubscribers() > 0) { 261 | octomap_msgs::Octomap full_map; 262 | getOctomapBinaryMsg(&full_map); 263 | full_map.header.frame_id = world_frame_; 264 | full_map_pub_.publish(full_map); 265 | } 266 | 267 | if (latch_topics_ || pcl_pub_.getNumSubscribers() > 0) { 268 | pcl::PointCloud point_cloud; 269 | getOccupiedPointCloud(&point_cloud); 270 | sensor_msgs::PointCloud2 cloud; 271 | pcl::toROSMsg(point_cloud, cloud); 272 | cloud.header.frame_id = world_frame_; 273 | pcl_pub_.publish(cloud); 274 | } 275 | 276 | if (use_tf_transforms_ && nearest_obstacle_pub_.getNumSubscribers() > 0) { 277 | Transformation robot_to_world; 278 | if (lookupTransformTf(robot_frame_, world_frame_, ros::Time::now(), 279 | &robot_to_world)) { 280 | Eigen::Vector3d robot_center = robot_to_world.getPosition(); 281 | pcl::PointCloud point_cloud; 282 | getOccupiedPointcloudInBoundingBox(robot_center, robot_size_, 283 | &point_cloud); 284 | sensor_msgs::PointCloud2 cloud; 285 | pcl::toROSMsg(point_cloud, cloud); 286 | cloud.header.frame_id = world_frame_; 287 | cloud.header.stamp = ros::Time::now(); 288 | nearest_obstacle_pub_.publish(cloud); 289 | } 290 | } 291 | } 292 | 293 | void OctomapManager::publishAllEvent(const ros::TimerEvent& e) { publishAll(); } 294 | 295 | bool OctomapManager::resetMapCallback(std_srvs::Empty::Request& request, 296 | std_srvs::Empty::Response& response) { 297 | resetMap(); 298 | return true; 299 | } 300 | 301 | bool OctomapManager::publishAllCallback(std_srvs::Empty::Request& request, 302 | std_srvs::Empty::Response& response) { 303 | publishAll(); 304 | return true; 305 | } 306 | 307 | bool OctomapManager::getOctomapCallback( 308 | octomap_msgs::GetOctomap::Request& request, 309 | octomap_msgs::GetOctomap::Response& response) { 310 | return getOctomapFullMsg(&response.map); 311 | } 312 | 313 | bool OctomapManager::loadOctomapCallback( 314 | volumetric_msgs::LoadMap::Request& request, 315 | volumetric_msgs::LoadMap::Response& response) { 316 | std::string extension = 317 | request.file_path.substr(request.file_path.find_last_of(".") + 1); 318 | if (extension == "bt") { 319 | return loadOctomapFromFile(request.file_path); 320 | } else { 321 | pcl::PointCloud::Ptr cloud( 322 | new pcl::PointCloud); 323 | if (extension == "pcd") { 324 | pcl::io::loadPCDFile(request.file_path, *cloud); 325 | } else if (extension == "ply") { 326 | pcl::io::loadPLYFile(request.file_path, *cloud); 327 | } else { 328 | ROS_ERROR_STREAM( 329 | "No known file extension (.bt, .pcd, .ply): " << request.file_path); 330 | return false; 331 | } 332 | octomap::KeySet free_cells, occupied_cells; 333 | for (size_t i = 0u; i < cloud->size(); ++i) { 334 | const octomap::point3d p_G_point((*cloud)[i].x, (*cloud)[i].y, 335 | (*cloud)[i].z); 336 | octomap::OcTreeKey key; 337 | if (octree_->coordToKeyChecked(p_G_point, key)) { 338 | occupied_cells.insert(key); 339 | } 340 | } 341 | updateOccupancy(&free_cells, &occupied_cells); 342 | return true; 343 | } 344 | } 345 | 346 | bool OctomapManager::saveOctomapCallback( 347 | volumetric_msgs::SaveMap::Request& request, 348 | volumetric_msgs::SaveMap::Response& response) { 349 | return writeOctomapToFile(request.file_path); 350 | } 351 | 352 | bool OctomapManager::savePointCloudCallback( 353 | volumetric_msgs::SaveMap::Request& request, 354 | volumetric_msgs::SaveMap::Response& response) { 355 | pcl::PointCloud point_cloud; 356 | getOccupiedPointCloud(&point_cloud); 357 | pcl::io::savePLYFileASCII(request.file_path, point_cloud); 358 | return true; 359 | } 360 | 361 | bool OctomapManager::setBoxOccupancyCallback( 362 | volumetric_msgs::SetBoxOccupancy::Request& request, 363 | volumetric_msgs::SetBoxOccupancy::Response& response) { 364 | Eigen::Vector3d bounding_box_center; 365 | Eigen::Vector3d bounding_box_size; 366 | 367 | tf::vectorMsgToKindr(request.box_center, &bounding_box_center); 368 | tf::vectorMsgToKindr(request.box_size, &bounding_box_size); 369 | bool set_occupied = request.set_occupied; 370 | 371 | if (set_occupied) { 372 | setOccupied(bounding_box_center, bounding_box_size); 373 | } else { 374 | setFree(bounding_box_center, bounding_box_size); 375 | } 376 | publishAll(); 377 | return true; 378 | } 379 | 380 | bool OctomapManager::setDisplayBoundsCallback( 381 | volumetric_msgs::SetDisplayBounds::Request& request, 382 | volumetric_msgs::SetDisplayBounds::Response& response) { 383 | params_.visualize_min_z = request.min_z; 384 | params_.visualize_max_z = request.max_z; 385 | publishAll(); 386 | return true; 387 | } 388 | 389 | bool OctomapManager::getChangedPointsCallback( 390 | volumetric_msgs::GetChangedPoints::Request& request, 391 | volumetric_msgs::GetChangedPoints::Response& response) { 392 | std::vector changed_points; 393 | std::vector changed_states; 394 | getChangedPoints(&changed_points, &changed_states); 395 | if (changed_points.size() != changed_states.size()) { 396 | std::cerr << "In getChangedPointsCallback changed_points and " 397 | "changed_states have different size!\n"; 398 | return false; 399 | } 400 | int size = changed_points.size(); 401 | response.size = size; 402 | response.changed_points.resize(size); 403 | response.changed_states.resize(size); 404 | for (int i = 0; i < size; ++i) { 405 | tf::vectorKindrToMsg(changed_points[i], &(response.changed_points[i])); 406 | response.changed_states[i] = changed_states[i]; 407 | } 408 | return true; 409 | } 410 | 411 | void OctomapManager::leftCameraInfoCallback( 412 | const sensor_msgs::CameraInfoPtr& left_info) { 413 | left_info_ = left_info; 414 | if (left_info_ && right_info_ && !Q_initialized_) { 415 | calculateQ(); 416 | } 417 | } 418 | void OctomapManager::rightCameraInfoCallback( 419 | const sensor_msgs::CameraInfoPtr& right_info) { 420 | right_info_ = right_info; 421 | if (left_info_ && right_info_ && !Q_initialized_) { 422 | calculateQ(); 423 | } 424 | } 425 | 426 | void OctomapManager::calculateQ() { 427 | Q_ = getQForROSCameras(*left_info_, *right_info_); 428 | full_image_size_.x() = left_info_->width; 429 | full_image_size_.y() = left_info_->height; 430 | Q_initialized_ = true; 431 | } 432 | 433 | void OctomapManager::insertDisparityImageWithTf( 434 | const stereo_msgs::DisparityImageConstPtr& disparity) { 435 | if (!Q_initialized_) { 436 | ROS_WARN_THROTTLE( 437 | 1, "No camera info available yet, skipping adding disparity."); 438 | return; 439 | } 440 | 441 | // Look up transform from sensor frame to world frame. 442 | Transformation sensor_to_world; 443 | if (lookupTransform(disparity->header.frame_id, world_frame_, 444 | disparity->header.stamp, &sensor_to_world)) { 445 | insertDisparityImage(sensor_to_world, disparity, Q_, full_image_size_); 446 | } 447 | } 448 | 449 | void OctomapManager::insertPointcloudWithTf( 450 | const sensor_msgs::PointCloud2::ConstPtr& pointcloud) { 451 | // Look up transform from sensor frame to world frame. 452 | Transformation sensor_to_world; 453 | if (lookupTransform(pointcloud->header.frame_id, world_frame_, 454 | pointcloud->header.stamp, &sensor_to_world)) { 455 | insertPointcloud(sensor_to_world, pointcloud); 456 | } 457 | } 458 | 459 | bool OctomapManager::lookupTransform(const std::string& from_frame, 460 | const std::string& to_frame, 461 | const ros::Time& timestamp, 462 | Transformation* transform) { 463 | if (use_tf_transforms_) { 464 | return lookupTransformTf(from_frame, to_frame, timestamp, transform); 465 | } else { 466 | return lookupTransformQueue(from_frame, to_frame, timestamp, transform); 467 | } 468 | } 469 | 470 | bool OctomapManager::lookupTransformTf(const std::string& from_frame, 471 | const std::string& to_frame, 472 | const ros::Time& timestamp, 473 | Transformation* transform) { 474 | tf::StampedTransform tf_transform; 475 | 476 | ros::Time time_to_lookup = timestamp; 477 | 478 | // If this transform isn't possible at the time, then try to just look up 479 | // the latest (this is to work with bag files and static transform publisher, 480 | // etc). 481 | if (!tf_listener_.canTransform(to_frame, from_frame, time_to_lookup)) { 482 | ros::Duration timestamp_age = ros::Time::now() - time_to_lookup; 483 | if (timestamp_age < tf_listener_.getCacheLength()) { 484 | time_to_lookup = ros::Time(0); 485 | ROS_WARN("Using latest TF transform instead of timestamp match."); 486 | } else { 487 | ROS_ERROR("Requested transform time older than cache limit."); 488 | return false; 489 | } 490 | } 491 | 492 | try { 493 | tf_listener_.lookupTransform(to_frame, from_frame, time_to_lookup, 494 | tf_transform); 495 | } catch (tf::TransformException& ex) { 496 | ROS_ERROR_STREAM( 497 | "Error getting TF transform from sensor data: " << ex.what()); 498 | return false; 499 | } 500 | 501 | tf::transformTFToKindr(tf_transform, transform); 502 | return true; 503 | } 504 | 505 | void OctomapManager::transformCallback( 506 | const geometry_msgs::TransformStamped& transform_msg) { 507 | transform_queue_.push_back(transform_msg); 508 | } 509 | 510 | bool OctomapManager::lookupTransformQueue(const std::string& from_frame, 511 | const std::string& to_frame, 512 | const ros::Time& timestamp, 513 | Transformation* transform) { 514 | // Try to match the transforms in the queue. 515 | bool match_found = false; 516 | std::deque::iterator it = 517 | transform_queue_.begin(); 518 | for (; it != transform_queue_.end(); ++it) { 519 | // If the current transform is newer than the requested timestamp, we need 520 | // to break. 521 | if (it->header.stamp > timestamp) { 522 | if ((it->header.stamp - timestamp).toNSec() < timestamp_tolerance_ns_) { 523 | match_found = true; 524 | } 525 | break; 526 | } 527 | 528 | if ((timestamp - it->header.stamp).toNSec() < timestamp_tolerance_ns_) { 529 | match_found = true; 530 | break; 531 | } 532 | } 533 | 534 | if (match_found) { 535 | Transformation T_G_D; 536 | tf::transformMsgToKindr(it->transform, &T_G_D); 537 | 538 | // If we have a static transform, apply it too. 539 | // Transform should actually be T_G_C. So need to take it through the full 540 | // chain. 541 | *transform = T_G_D * T_B_D_.inverse() * T_B_C_; 542 | 543 | // And also clear the queue up to this point. This leaves the current 544 | // message in place. 545 | transform_queue_.erase(transform_queue_.begin(), it); 546 | } else { 547 | ROS_WARN_STREAM_THROTTLE( 548 | 30, "No match found for transform timestamp: " << timestamp); 549 | if (!transform_queue_.empty()) { 550 | ROS_WARN_STREAM_THROTTLE( 551 | 30, 552 | "Queue front: " << transform_queue_.front().header.stamp 553 | << " back: " << transform_queue_.back().header.stamp); 554 | } 555 | } 556 | return match_found; 557 | } 558 | 559 | } // namespace volumetric_mapping 560 | -------------------------------------------------------------------------------- /octomap_world/src/octomap_manager_node.cc: -------------------------------------------------------------------------------- 1 | /* 2 | Copyright (c) 2015, Helen Oleynikova, ETH Zurich, Switzerland 3 | You can contact the author at 4 | 5 | All rights reserved. 6 | 7 | Redistribution and use in source and binary forms, with or without 8 | modification, are permitted provided that the following conditions are met: 9 | * Redistributions of source code must retain the above copyright 10 | notice, this list of conditions and the following disclaimer. 11 | * Redistributions in binary form must reproduce the above copyright 12 | notice, this list of conditions and the following disclaimer in the 13 | documentation and/or other materials provided with the distribution. 14 | * Neither the name of ETHZ-ASL nor the 15 | names of its contributors may be used to endorse or promote products 16 | derived from this software without specific prior written permission. 17 | 18 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 19 | ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 20 | WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 21 | DISCLAIMED. IN NO EVENT SHALL ETHZ-ASL BE LIABLE FOR ANY 22 | DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 23 | (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 24 | LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 25 | ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 26 | (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 27 | SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 28 | */ 29 | 30 | #include "octomap_world/octomap_manager.h" 31 | 32 | int main(int argc, char** argv) { 33 | ros::init(argc, argv, "octomap_manager"); 34 | google::InitGoogleLogging(argv[0]); 35 | google::ParseCommandLineFlags(&argc, &argv, false); 36 | google::InstallFailureSignalHandler(); 37 | ros::NodeHandle nh; 38 | ros::NodeHandle nh_private("~"); 39 | 40 | volumetric_mapping::OctomapManager manager(nh, nh_private); 41 | 42 | ros::spin(); 43 | return 0; 44 | } 45 | -------------------------------------------------------------------------------- /octomap_world/src/octomap_world.cc: -------------------------------------------------------------------------------- 1 | /* 2 | Copyright (c) 2015, Helen Oleynikova, ETH Zurich, Switzerland 3 | You can contact the author at 4 | 5 | All rights reserved. 6 | 7 | Redistribution and use in source and binary forms, with or without 8 | modification, are permitted provided that the following conditions are met: 9 | * Redistributions of source code must retain the above copyright 10 | notice, this list of conditions and the following disclaimer. 11 | * Redistributions in binary form must reproduce the above copyright 12 | notice, this list of conditions and the following disclaimer in the 13 | documentation and/or other materials provided with the distribution. 14 | * Neither the name of ETHZ-ASL nor the 15 | names of its contributors may be used to endorse or promote products 16 | derived from this software without specific prior written permission. 17 | 18 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 19 | ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 20 | WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 21 | DISCLAIMED. IN NO EVENT SHALL ETHZ-ASL BE LIABLE FOR ANY 22 | DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 23 | (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 24 | LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 25 | ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 26 | (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 27 | SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 28 | */ 29 | 30 | #include "octomap_world/octomap_world.h" 31 | 32 | #include 33 | #include 34 | #include 35 | #include 36 | #include 37 | #include 38 | 39 | namespace volumetric_mapping { 40 | 41 | // Convenience functions for octomap point <-> eigen conversions. 42 | octomap::point3d pointEigenToOctomap(const Eigen::Vector3d& point) { 43 | return octomap::point3d(point.x(), point.y(), point.z()); 44 | } 45 | Eigen::Vector3d pointOctomapToEigen(const octomap::point3d& point) { 46 | return Eigen::Vector3d(point.x(), point.y(), point.z()); 47 | } 48 | 49 | // Create a default parameters object and call the other constructor with it. 50 | OctomapWorld::OctomapWorld() : OctomapWorld(OctomapParameters()) {} 51 | 52 | // Creates an octomap with the correct parameters. 53 | OctomapWorld::OctomapWorld(const OctomapParameters& params) 54 | : robot_size_(Eigen::Vector3d::Ones()) { 55 | setOctomapParameters(params); 56 | } 57 | 58 | // Creates deepcopy of OctomapWorld 59 | OctomapWorld::OctomapWorld(const OctomapWorld& rhs) { 60 | OctomapParameters params; 61 | rhs.getOctomapParameters(¶ms); 62 | setOctomapParameters(params); 63 | robot_size_ = rhs.getRobotSize(); 64 | 65 | // Get rhs octomap binary 66 | std::stringstream datastream; 67 | rhs.writeOctomapToBinaryConst(datastream); 68 | // Write octomap binary 69 | if (!octree_->readBinary(datastream)) { 70 | std::cerr << "Could not copy octree!\n"; 71 | } 72 | octree_->prune(); 73 | } 74 | 75 | void OctomapWorld::resetMap() { 76 | if (!octree_) { 77 | octree_.reset(new octomap::OcTree(params_.resolution)); 78 | } 79 | octree_->clear(); 80 | } 81 | 82 | void OctomapWorld::prune() { octree_->prune(); } 83 | 84 | void OctomapWorld::setOctomapParameters(const OctomapParameters& params) { 85 | if (octree_) { 86 | if (octree_->getResolution() != params.resolution) { 87 | LOG(WARNING) << "Octomap resolution has changed! Resetting tree!"; 88 | octree_.reset(new octomap::OcTree(params.resolution)); 89 | } 90 | } else { 91 | octree_.reset(new octomap::OcTree(params.resolution)); 92 | } 93 | 94 | octree_->setProbHit(params.probability_hit); 95 | octree_->setProbMiss(params.probability_miss); 96 | octree_->setClampingThresMin(params.threshold_min); 97 | octree_->setClampingThresMax(params.threshold_max); 98 | octree_->setOccupancyThres(params.threshold_occupancy); 99 | octree_->enableChangeDetection(params.change_detection_enabled); 100 | 101 | // Copy over all the parameters for future use (some are not used just for 102 | // creating the octree). 103 | params_ = params; 104 | } 105 | 106 | void OctomapWorld::getOctomapParameters(OctomapParameters* params) const { 107 | *params = params_; 108 | } 109 | 110 | void OctomapWorld::insertPointcloudIntoMapImpl( 111 | const Transformation& T_G_sensor, 112 | const pcl::PointCloud::Ptr& cloud) { 113 | // Remove NaN values, if any. 114 | std::vector indices; 115 | pcl::removeNaNFromPointCloud(*cloud, *cloud, indices); 116 | 117 | // First, rotate the pointcloud into the world frame. 118 | pcl::transformPointCloud(*cloud, *cloud, 119 | T_G_sensor.getTransformationMatrix()); 120 | const octomap::point3d p_G_sensor = 121 | pointEigenToOctomap(T_G_sensor.getPosition()); 122 | 123 | // Then add all the rays from this pointcloud. 124 | // We do this as a batch operation - so first get all the keys in a set, then 125 | // do the update in batch. 126 | octomap::KeySet free_cells, occupied_cells; 127 | for (pcl::PointCloud::const_iterator it = cloud->begin(); 128 | it != cloud->end(); ++it) { 129 | const octomap::point3d p_G_point(it->x, it->y, it->z); 130 | // First, check if we've already checked this. 131 | octomap::OcTreeKey key = octree_->coordToKey(p_G_point); 132 | 133 | if (occupied_cells.find(key) == occupied_cells.end()) { 134 | // Check if this is within the allowed sensor range. 135 | castRay(p_G_sensor, p_G_point, &free_cells, &occupied_cells); 136 | } 137 | } 138 | 139 | // Apply the new free cells and occupied cells from 140 | updateOccupancy(&free_cells, &occupied_cells); 141 | } 142 | 143 | void OctomapWorld::insertProjectedDisparityIntoMapImpl( 144 | const Transformation& sensor_to_world, const cv::Mat& projected_points) { 145 | // Get the sensor origin in the world frame. 146 | Eigen::Vector3d sensor_origin_eigen = Eigen::Vector3d::Zero(); 147 | sensor_origin_eigen = sensor_to_world * sensor_origin_eigen; 148 | octomap::point3d sensor_origin = pointEigenToOctomap(sensor_origin_eigen); 149 | 150 | octomap::KeySet free_cells, occupied_cells; 151 | for (int v = 0; v < projected_points.rows; ++v) { 152 | const cv::Vec3f* row_pointer = projected_points.ptr(v); 153 | 154 | for (int u = 0; u < projected_points.cols; ++u) { 155 | // Check whether we're within the correct range for disparity. 156 | if (!isValidPoint(row_pointer[u]) || row_pointer[u][2] < 0) { 157 | continue; 158 | } 159 | Eigen::Vector3d point_eigen(row_pointer[u][0], row_pointer[u][1], 160 | row_pointer[u][2]); 161 | 162 | point_eigen = sensor_to_world * point_eigen; 163 | octomap::point3d point_octomap = pointEigenToOctomap(point_eigen); 164 | 165 | // First, check if we've already checked this. 166 | octomap::OcTreeKey key = octree_->coordToKey(point_octomap); 167 | 168 | if (occupied_cells.find(key) == occupied_cells.end()) { 169 | // Check if this is within the allowed sensor range. 170 | castRay(sensor_origin, point_octomap, &free_cells, &occupied_cells); 171 | } 172 | } 173 | } 174 | updateOccupancy(&free_cells, &occupied_cells); 175 | } 176 | 177 | void OctomapWorld::castRay(const octomap::point3d& sensor_origin, 178 | const octomap::point3d& point, 179 | octomap::KeySet* free_cells, 180 | octomap::KeySet* occupied_cells) { 181 | CHECK_NOTNULL(free_cells); 182 | CHECK_NOTNULL(occupied_cells); 183 | 184 | if (params_.sensor_max_range < 0.0 || 185 | (point - sensor_origin).norm() <= params_.sensor_max_range) { 186 | // Cast a ray to compute all the free cells. 187 | key_ray_.reset(); 188 | if (octree_->computeRayKeys(sensor_origin, point, key_ray_)) { 189 | if (params_.max_free_space == 0.0) { 190 | free_cells->insert(key_ray_.begin(), key_ray_.end()); 191 | } else { 192 | for (const auto& key : key_ray_) { 193 | octomap::point3d voxel_coordinate = octree_->keyToCoord(key); 194 | if ((voxel_coordinate - sensor_origin).norm() < 195 | params_.max_free_space || 196 | voxel_coordinate.z() > 197 | (sensor_origin.z() - params_.min_height_free_space)) { 198 | free_cells->insert(key); 199 | } 200 | } 201 | } 202 | } 203 | // Mark endpoing as occupied. 204 | octomap::OcTreeKey key; 205 | if (octree_->coordToKeyChecked(point, key)) { 206 | occupied_cells->insert(key); 207 | } 208 | } else { 209 | // If the ray is longer than the max range, just update free space. 210 | octomap::point3d new_end = 211 | sensor_origin + 212 | (point - sensor_origin).normalized() * params_.sensor_max_range; 213 | key_ray_.reset(); 214 | if (octree_->computeRayKeys(sensor_origin, new_end, key_ray_)) { 215 | if (params_.max_free_space == 0.0) { 216 | free_cells->insert(key_ray_.begin(), key_ray_.end()); 217 | } else { 218 | for (const auto& key : key_ray_) { 219 | octomap::point3d voxel_coordinate = octree_->keyToCoord(key); 220 | if ((voxel_coordinate - sensor_origin).norm() < 221 | params_.max_free_space || 222 | voxel_coordinate.z() > 223 | (sensor_origin.z() - params_.min_height_free_space)) { 224 | free_cells->insert(key); 225 | } 226 | } 227 | } 228 | } 229 | } 230 | } 231 | 232 | bool OctomapWorld::isValidPoint(const cv::Vec3f& point) const { 233 | // Check both for disparities explicitly marked as invalid (where OpenCV maps 234 | // pt.z to MISSING_Z) and zero disparities (point mapped to infinity). 235 | return point[2] != 10000.0f && !std::isinf(point[2]); 236 | } 237 | 238 | void OctomapWorld::updateOccupancy(octomap::KeySet* free_cells, 239 | octomap::KeySet* occupied_cells) { 240 | CHECK_NOTNULL(free_cells); 241 | CHECK_NOTNULL(occupied_cells); 242 | 243 | // Mark occupied cells. 244 | for (octomap::KeySet::iterator it = occupied_cells->begin(), 245 | end = occupied_cells->end(); 246 | it != end; it++) { 247 | octree_->updateNode(*it, true); 248 | 249 | // Remove any occupied cells from free cells - assume there are far fewer 250 | // occupied cells than free cells, so this is much faster than checking on 251 | // every free cell. 252 | if (free_cells->find(*it) != free_cells->end()) { 253 | free_cells->erase(*it); 254 | } 255 | } 256 | 257 | // Mark free cells. 258 | for (octomap::KeySet::iterator it = free_cells->begin(), 259 | end = free_cells->end(); 260 | it != end; ++it) { 261 | octree_->updateNode(*it, false); 262 | } 263 | octree_->updateInnerOccupancy(); 264 | } 265 | 266 | void OctomapWorld::enableTreatUnknownAsOccupied() { 267 | params_.treat_unknown_as_occupied = true; 268 | } 269 | 270 | void OctomapWorld::disableTreatUnknownAsOccupied() { 271 | params_.treat_unknown_as_occupied = false; 272 | } 273 | 274 | OctomapWorld::CellStatus OctomapWorld::getCellStatusBoundingBox( 275 | const Eigen::Vector3d& point, 276 | const Eigen::Vector3d& bounding_box_size) const { 277 | // First case: center point is unknown or occupied. Can just quit. 278 | CellStatus center_status = getCellStatusPoint(point); 279 | if (center_status != CellStatus::kFree) { 280 | return center_status; 281 | } 282 | 283 | // Also if center is outside of the bounds. 284 | octomap::OcTreeKey key; 285 | if (!octree_->coordToKeyChecked(pointEigenToOctomap(point), key)) { 286 | if (params_.treat_unknown_as_occupied) { 287 | return CellStatus::kOccupied; 288 | } else { 289 | return CellStatus::kUnknown; 290 | } 291 | } 292 | 293 | // Now we have to iterate over everything in the bounding box. 294 | Eigen::Vector3d bbx_min_eigen = point - bounding_box_size / 2; 295 | Eigen::Vector3d bbx_max_eigen = point + bounding_box_size / 2; 296 | 297 | octomap::point3d bbx_min = pointEigenToOctomap(bbx_min_eigen); 298 | octomap::point3d bbx_max = pointEigenToOctomap(bbx_max_eigen); 299 | 300 | for (octomap::OcTree::leaf_bbx_iterator 301 | iter = octree_->begin_leafs_bbx(bbx_min, bbx_max), 302 | end = octree_->end_leafs_bbx(); 303 | iter != end; ++iter) { 304 | Eigen::Vector3d cube_center(iter.getX(), iter.getY(), iter.getZ()); 305 | int depth_level = iter.getDepth(); 306 | double cube_size = octree_->getNodeSize(depth_level); 307 | 308 | // Check if it is really inside bounding box, since leaf_bbx_iterator begins 309 | // "too early" 310 | Eigen::Vector3d cube_lower_bound = 311 | cube_center - (cube_size / 2) * Eigen::Vector3d::Ones(); 312 | Eigen::Vector3d cube_upper_bound = 313 | cube_center + (cube_size / 2) * Eigen::Vector3d::Ones(); 314 | if (cube_upper_bound.x() < bbx_min.x() || 315 | cube_lower_bound.x() > bbx_max.x() || 316 | cube_upper_bound.y() < bbx_min.y() || 317 | cube_lower_bound.y() > bbx_max.y() || 318 | cube_upper_bound.z() < bbx_min.z() || 319 | cube_lower_bound.z() > bbx_max.z()) { 320 | continue; 321 | } 322 | 323 | if (octree_->isNodeOccupied(*iter)) { 324 | if (params_.filter_speckles && isSpeckleNode(iter.getKey())) { 325 | continue; 326 | } else { 327 | return CellStatus::kOccupied; 328 | } 329 | } 330 | } 331 | 332 | // The above only returns valid nodes - we should check for unknown nodes as 333 | // well. 334 | octomap::point3d_list unknown_centers; 335 | octree_->getUnknownLeafCenters(unknown_centers, bbx_min, bbx_max); 336 | if (unknown_centers.size() > 0) { 337 | if (params_.treat_unknown_as_occupied) { 338 | return CellStatus::kOccupied; 339 | } else { 340 | return CellStatus::kUnknown; 341 | } 342 | } 343 | return CellStatus::kFree; 344 | } 345 | 346 | OctomapWorld::CellStatus OctomapWorld::getCellStatusPoint( 347 | const Eigen::Vector3d& point) const { 348 | octomap::OcTreeNode* node = octree_->search(point.x(), point.y(), point.z()); 349 | if (node == NULL) { 350 | if (params_.treat_unknown_as_occupied) { 351 | return CellStatus::kOccupied; 352 | } else { 353 | return CellStatus::kUnknown; 354 | } 355 | } else if (octree_->isNodeOccupied(node)) { 356 | return CellStatus::kOccupied; 357 | } else { 358 | return CellStatus::kFree; 359 | } 360 | } 361 | 362 | // Returns kUnknown even if treat_unknown_as_occupied is true. 363 | OctomapWorld::CellStatus OctomapWorld::getCellTrueStatusPoint( 364 | const Eigen::Vector3d& point) const { 365 | octomap::OcTreeNode* node = octree_->search(point.x(), point.y(), point.z()); 366 | if (node == NULL) { 367 | return CellStatus::kUnknown; 368 | } else if (octree_->isNodeOccupied(node)) { 369 | return CellStatus::kOccupied; 370 | } else { 371 | return CellStatus::kFree; 372 | } 373 | } 374 | 375 | OctomapWorld::CellStatus OctomapWorld::getCellProbabilityPoint( 376 | const Eigen::Vector3d& point, double* probability) const { 377 | octomap::OcTreeNode* node = octree_->search(point.x(), point.y(), point.z()); 378 | if (node == NULL) { 379 | if (probability) { 380 | *probability = -1.0; 381 | } 382 | return CellStatus::kUnknown; 383 | } else { 384 | if (probability) { 385 | *probability = node->getOccupancy(); 386 | } 387 | if (octree_->isNodeOccupied(node)) { 388 | return CellStatus::kOccupied; 389 | } else { 390 | return CellStatus::kFree; 391 | } 392 | } 393 | } 394 | 395 | OctomapWorld::CellStatus OctomapWorld::getLineStatus( 396 | const Eigen::Vector3d& start, const Eigen::Vector3d& end) const { 397 | // Get all node keys for this line. 398 | // This is actually a typedef for a vector of OcTreeKeys. 399 | // Can't use the key_ray_ temp member here because this is a const function. 400 | octomap::KeyRay key_ray; 401 | octree_->computeRayKeys(pointEigenToOctomap(start), pointEigenToOctomap(end), 402 | key_ray); 403 | 404 | // Now check if there are any unknown or occupied nodes in the ray. 405 | for (octomap::OcTreeKey key : key_ray) { 406 | octomap::OcTreeNode* node = octree_->search(key); 407 | if (node == NULL) { 408 | if (params_.treat_unknown_as_occupied) { 409 | return CellStatus::kOccupied; 410 | } else { 411 | return CellStatus::kUnknown; 412 | } 413 | } else if (octree_->isNodeOccupied(node)) { 414 | return CellStatus::kOccupied; 415 | } 416 | } 417 | return CellStatus::kFree; 418 | } 419 | 420 | OctomapWorld::CellStatus OctomapWorld::getVisibility( 421 | const Eigen::Vector3d& view_point, const Eigen::Vector3d& voxel_to_test, 422 | bool stop_at_unknown_cell) const { 423 | // Get all node keys for this line. 424 | // This is actually a typedef for a vector of OcTreeKeys. 425 | // Can't use the key_ray_ temp member here because this is a const function. 426 | octomap::KeyRay key_ray; 427 | 428 | octree_->computeRayKeys(pointEigenToOctomap(view_point), 429 | pointEigenToOctomap(voxel_to_test), key_ray); 430 | 431 | const octomap::OcTreeKey& voxel_to_test_key = 432 | octree_->coordToKey(pointEigenToOctomap(voxel_to_test)); 433 | 434 | // Now check if there are any unknown or occupied nodes in the ray, 435 | // except for the voxel_to_test key. 436 | for (octomap::OcTreeKey key : key_ray) { 437 | if (key != voxel_to_test_key) { 438 | octomap::OcTreeNode* node = octree_->search(key); 439 | if (node == NULL) { 440 | if (stop_at_unknown_cell) { 441 | return CellStatus::kUnknown; 442 | } 443 | } else if (octree_->isNodeOccupied(node)) { 444 | return CellStatus::kOccupied; 445 | } 446 | } 447 | } 448 | return CellStatus::kFree; 449 | } 450 | 451 | OctomapWorld::CellStatus OctomapWorld::getLineStatusBoundingBox( 452 | const Eigen::Vector3d& start, const Eigen::Vector3d& end, 453 | const Eigen::Vector3d& bounding_box_size) const { 454 | // TODO(helenol): Probably best way would be to get all the coordinates along 455 | // the line, then make a set of all the OcTreeKeys in all the bounding boxes 456 | // around the nodes... and then just go through and query once. 457 | const double epsilon = 0.001; // Small offset 458 | CellStatus ret = CellStatus::kFree; 459 | const double& resolution = getResolution(); 460 | 461 | // Check corner connections and depending on resolution also interior: 462 | // Discretization step is smaller than the octomap resolution, as this way 463 | // no cell can possibly be missed 464 | double x_disc = bounding_box_size.x() / 465 | ceil((bounding_box_size.x() + epsilon) / resolution); 466 | double y_disc = bounding_box_size.y() / 467 | ceil((bounding_box_size.y() + epsilon) / resolution); 468 | double z_disc = bounding_box_size.z() / 469 | ceil((bounding_box_size.z() + epsilon) / resolution); 470 | 471 | // Ensure that resolution is not infinit 472 | if (x_disc <= 0.0) x_disc = 1.0; 473 | if (y_disc <= 0.0) y_disc = 1.0; 474 | if (z_disc <= 0.0) z_disc = 1.0; 475 | 476 | const Eigen::Vector3d bounding_box_half_size = bounding_box_size * 0.5; 477 | 478 | for (double x = -bounding_box_half_size.x(); x <= bounding_box_half_size.x(); 479 | x += x_disc) { 480 | for (double y = -bounding_box_half_size.y(); 481 | y <= bounding_box_half_size.y(); y += y_disc) { 482 | for (double z = -bounding_box_half_size.z(); 483 | z <= bounding_box_half_size.z(); z += z_disc) { 484 | Eigen::Vector3d offset(x, y, z); 485 | ret = getLineStatus(start + offset, end + offset); 486 | if (ret != CellStatus::kFree) { 487 | return ret; 488 | } 489 | } 490 | } 491 | } 492 | return CellStatus::kFree; 493 | } 494 | 495 | double OctomapWorld::getResolution() const { return octree_->getResolution(); } 496 | 497 | void OctomapWorld::setFree(const Eigen::Vector3d& position, 498 | const Eigen::Vector3d& bounding_box_size, 499 | const BoundHandling& insertion_method) { 500 | setLogOddsBoundingBox(position, bounding_box_size, 501 | octree_->getClampingThresMinLog(), insertion_method); 502 | } 503 | 504 | void OctomapWorld::setFree(const std::vector& positions, 505 | const Eigen::Vector3d& bounding_box_size, 506 | const BoundHandling& insertion_method) { 507 | setLogOddsBoundingBox(positions, bounding_box_size, 508 | octree_->getClampingThresMinLog(), insertion_method); 509 | } 510 | 511 | void OctomapWorld::setOccupied(const Eigen::Vector3d& position, 512 | const Eigen::Vector3d& bounding_box_size, 513 | const BoundHandling& insertion_method) { 514 | setLogOddsBoundingBox(position, bounding_box_size, 515 | octree_->getClampingThresMaxLog(), insertion_method); 516 | } 517 | 518 | void OctomapWorld::setOccupied(const std::vector& positions, 519 | const Eigen::Vector3d& bounding_box_size, 520 | const BoundHandling& insertion_method) { 521 | setLogOddsBoundingBox(positions, bounding_box_size, 522 | octree_->getClampingThresMaxLog(), insertion_method); 523 | } 524 | 525 | void OctomapWorld::setBordersOccupied(const Eigen::Vector3d& cropping_size) { 526 | // Crop map size by setting borders occupied 527 | const bool lazy_eval = true; 528 | const double log_odds_value = octree_->getClampingThresMaxLog(); 529 | octomap::KeySet occupied_keys; 530 | 531 | Eigen::Vector3d map_center = getMapCenter(); 532 | Eigen::Vector3d map_size = getMapSize(); 533 | Eigen::Vector3d bbx_center; 534 | Eigen::Vector3d bbx_size; 535 | 536 | bbx_size = map_size; 537 | bbx_size.x() = cropping_size.x() / 2; 538 | bbx_center = map_center; 539 | bbx_center.x() = map_center.x() - map_size.x() / 2 + cropping_size.x() / 4; 540 | getKeysBoundingBox(bbx_center, bbx_size, &occupied_keys, 541 | BoundHandling::kIncludePartialBoxes); 542 | bbx_center.x() = map_center.x() + map_size.x() / 2 - cropping_size.x() / 4; 543 | getKeysBoundingBox(bbx_center, bbx_size, &occupied_keys, 544 | BoundHandling::kIncludePartialBoxes); 545 | 546 | bbx_size = map_size; 547 | bbx_size.y() = cropping_size.y() / 2; 548 | bbx_center = map_center; 549 | bbx_center.y() = map_center.y() - map_size.y() / 2 + cropping_size.y() / 4; 550 | getKeysBoundingBox(bbx_center, bbx_size, &occupied_keys, 551 | BoundHandling::kIncludePartialBoxes); 552 | bbx_center.y() = map_center.y() + map_size.y() / 2 - cropping_size.y() / 4; 553 | getKeysBoundingBox(bbx_center, bbx_size, &occupied_keys, 554 | BoundHandling::kIncludePartialBoxes); 555 | 556 | bbx_size = map_size; 557 | bbx_size.z() = cropping_size.z() / 2; 558 | bbx_center = map_center; 559 | bbx_center.z() = map_center.z() - map_size.z() / 2 + cropping_size.z() / 4; 560 | getKeysBoundingBox(bbx_center, bbx_size, &occupied_keys, 561 | BoundHandling::kIncludePartialBoxes); 562 | bbx_center.z() = map_center.z() + map_size.z() / 2 - cropping_size.z() / 4; 563 | getKeysBoundingBox(bbx_center, bbx_size, &occupied_keys, 564 | BoundHandling::kIncludePartialBoxes); 565 | 566 | // Set all infeasible points occupied 567 | for (octomap::OcTreeKey key : occupied_keys) { 568 | octree_->setNodeValue(octree_->keyToCoord(key), log_odds_value, lazy_eval); 569 | } 570 | 571 | if (lazy_eval) { 572 | octree_->updateInnerOccupancy(); 573 | } 574 | octree_->prune(); 575 | } 576 | 577 | void OctomapWorld::getOccupiedPointCloud( 578 | pcl::PointCloud* output_cloud) const { 579 | CHECK_NOTNULL(output_cloud)->clear(); 580 | unsigned int max_tree_depth = octree_->getTreeDepth(); 581 | double resolution = octree_->getResolution(); 582 | for (octomap::OcTree::leaf_iterator it = octree_->begin_leafs(); 583 | it != octree_->end_leafs(); ++it) { 584 | if (octree_->isNodeOccupied(*it)) { 585 | // If leaf is max depth add coordinates. 586 | if (max_tree_depth == it.getDepth()) { 587 | pcl::PointXYZ point(it.getX(), it.getY(), it.getZ()); 588 | output_cloud->push_back(point); 589 | } 590 | // If leaf is not max depth it represents an occupied voxel with edge 591 | // length of 2^(max_tree_depth - leaf_depth) * resolution. 592 | // We use multiple points to visualize this filled volume. 593 | else { 594 | const unsigned int box_edge_length = 595 | pow(2, max_tree_depth - it.getDepth() - 1); 596 | const double bbx_offset = box_edge_length * resolution - resolution / 2; 597 | Eigen::Vector3d bbx_offset_vec(bbx_offset, bbx_offset, bbx_offset); 598 | Eigen::Vector3d center(it.getX(), it.getY(), it.getZ()); 599 | Eigen::Vector3d bbx_min = center - bbx_offset_vec; 600 | Eigen::Vector3d bbx_max = center + bbx_offset_vec; 601 | // Add small offset to avoid overshooting bbx_max. 602 | bbx_max += Eigen::Vector3d(0.001, 0.001, 0.001); 603 | for (double x_position = bbx_min.x(); x_position <= bbx_max.x(); 604 | x_position += resolution) { 605 | for (double y_position = bbx_min.y(); y_position <= bbx_max.y(); 606 | y_position += resolution) { 607 | for (double z_position = bbx_min.z(); z_position <= bbx_max.z(); 608 | z_position += resolution) { 609 | output_cloud->push_back( 610 | pcl::PointXYZ(x_position, y_position, z_position)); 611 | } 612 | } 613 | } 614 | } 615 | } 616 | } 617 | } 618 | 619 | void OctomapWorld::getOccupiedPointcloudInBoundingBox( 620 | const Eigen::Vector3d& center, const Eigen::Vector3d& bounding_box_size, 621 | pcl::PointCloud* output_cloud, 622 | const BoundHandling& insertion_method) const { 623 | CHECK_NOTNULL(output_cloud); 624 | output_cloud->clear(); 625 | 626 | const double resolution = octree_->getResolution(); 627 | 628 | // Determine correct center of voxel. 629 | const Eigen::Vector3d center_corrected( 630 | resolution * std::floor(center.x() / resolution) + resolution / 2.0, 631 | resolution * std::floor(center.y() / resolution) + resolution / 2.0, 632 | resolution * std::floor(center.z() / resolution) + resolution / 2.0); 633 | 634 | Eigen::Vector3d bbx_min, bbx_max; 635 | 636 | if (insertion_method == BoundHandling::kDefault) { 637 | adjustBoundingBox(center_corrected, bounding_box_size, insertion_method, 638 | &bbx_min, &bbx_max); 639 | } else { 640 | adjustBoundingBox(center, bounding_box_size, insertion_method, &bbx_min, 641 | &bbx_max); 642 | } 643 | 644 | for (double x_position = bbx_min.x(); x_position <= bbx_max.x(); 645 | x_position += resolution) { 646 | for (double y_position = bbx_min.y(); y_position <= bbx_max.y(); 647 | y_position += resolution) { 648 | for (double z_position = bbx_min.z(); z_position <= bbx_max.z(); 649 | z_position += resolution) { 650 | octomap::point3d point = 651 | octomap::point3d(x_position, y_position, z_position); 652 | octomap::OcTreeKey key = octree_->coordToKey(point); 653 | octomap::OcTreeNode* node = octree_->search(key); 654 | if (node != NULL && octree_->isNodeOccupied(node)) { 655 | output_cloud->push_back( 656 | pcl::PointXYZ(point.x(), point.y(), point.z())); 657 | } 658 | } 659 | } 660 | } 661 | } 662 | 663 | void OctomapWorld::getAllFreeBoxes( 664 | std::vector>* free_box_vector) const { 665 | const bool occupied_boxes = false; 666 | getAllBoxes(occupied_boxes, free_box_vector); 667 | } 668 | 669 | void OctomapWorld::getAllOccupiedBoxes( 670 | std::vector>* occupied_box_vector) 671 | const { 672 | const bool occupied_boxes = true; 673 | getAllBoxes(occupied_boxes, occupied_box_vector); 674 | } 675 | 676 | void OctomapWorld::getAllBoxes( 677 | bool occupied_boxes, 678 | std::vector>* box_vector) const { 679 | box_vector->clear(); 680 | box_vector->reserve(octree_->size()); 681 | for (octomap::OcTree::leaf_iterator it = octree_->begin_leafs(), 682 | end = octree_->end_leafs(); 683 | it != end; ++it) { 684 | Eigen::Vector3d cube_center(it.getX(), it.getY(), it.getZ()); 685 | int depth_level = it.getDepth(); 686 | double cube_size = octree_->getNodeSize(depth_level); 687 | 688 | if (octree_->isNodeOccupied(*it) && occupied_boxes) { 689 | box_vector->emplace_back(cube_center, cube_size); 690 | } else if (!octree_->isNodeOccupied(*it) && !occupied_boxes) { 691 | box_vector->emplace_back(cube_center, cube_size); 692 | } 693 | } 694 | } 695 | 696 | void OctomapWorld::getBox(const octomap::OcTreeKey& key, 697 | std::pair* box) const { 698 | // bbx_iterator begins "too early", and the last leaf is the expected one 699 | for (octomap::OcTree::leaf_bbx_iterator 700 | it = octree_->begin_leafs_bbx(key, key), 701 | end = octree_->end_leafs_bbx(); 702 | it != end; ++it) { 703 | box->first = Eigen::Vector3d(it.getX(), it.getY(), it.getZ()); 704 | box->second = it.getSize(); 705 | } 706 | } 707 | 708 | void OctomapWorld::getFreeBoxesBoundingBox( 709 | const Eigen::Vector3d& position, const Eigen::Vector3d& bounding_box_size, 710 | std::vector>* free_box_vector) const { 711 | const bool occupied_boxes = false; 712 | getBoxesBoundingBox(occupied_boxes, position, bounding_box_size, 713 | free_box_vector); 714 | } 715 | 716 | void OctomapWorld::getOccupiedBoxesBoundingBox( 717 | const Eigen::Vector3d& position, const Eigen::Vector3d& bounding_box_size, 718 | std::vector>* occupied_box_vector) 719 | const { 720 | const bool occupied_boxes = true; 721 | getBoxesBoundingBox(occupied_boxes, position, bounding_box_size, 722 | occupied_box_vector); 723 | } 724 | 725 | void OctomapWorld::getBoxesBoundingBox( 726 | bool occupied_boxes, const Eigen::Vector3d& position, 727 | const Eigen::Vector3d& bounding_box_size, 728 | std::vector>* box_vector) const { 729 | box_vector->clear(); 730 | if (bounding_box_size.maxCoeff() <= 0.0 || octree_->size() == 0) { 731 | return; 732 | } 733 | const Eigen::Vector3d max_boxes = 734 | bounding_box_size / octree_->getResolution(); 735 | const int max_vector_size = std::ceil(max_boxes.x()) * 736 | std::ceil(max_boxes.y()) * 737 | std::ceil(max_boxes.z()); 738 | box_vector->reserve(max_vector_size); 739 | 740 | const double epsilon = 0.001; // Small offset to not hit boundary of nodes. 741 | Eigen::Vector3d epsilon_3d; 742 | epsilon_3d.setConstant(epsilon); 743 | 744 | Eigen::Vector3d bbx_min_eigen = position - bounding_box_size / 2 + epsilon_3d; 745 | Eigen::Vector3d bbx_max_eigen = position + bounding_box_size / 2 - epsilon_3d; 746 | 747 | octomap::point3d bbx_min = pointEigenToOctomap(bbx_min_eigen); 748 | octomap::point3d bbx_max = pointEigenToOctomap(bbx_max_eigen); 749 | 750 | for (octomap::OcTree::leaf_bbx_iterator 751 | it = octree_->begin_leafs_bbx(bbx_min, bbx_max), 752 | end = octree_->end_leafs_bbx(); 753 | it != end; ++it) { 754 | Eigen::Vector3d cube_center(it.getX(), it.getY(), it.getZ()); 755 | int depth_level = it.getDepth(); 756 | double cube_size = octree_->getNodeSize(depth_level); 757 | 758 | // Check if it is really inside bounding box, since leaf_bbx_iterator begins 759 | // "too early" 760 | Eigen::Vector3d cube_lower_bound = 761 | cube_center - (cube_size / 2) * Eigen::Vector3d::Ones(); 762 | Eigen::Vector3d cube_upper_bound = 763 | cube_center + (cube_size / 2) * Eigen::Vector3d::Ones(); 764 | if (cube_upper_bound.x() < bbx_min.x() || 765 | cube_lower_bound.x() > bbx_max.x() || 766 | cube_upper_bound.y() < bbx_min.y() || 767 | cube_lower_bound.y() > bbx_max.y() || 768 | cube_upper_bound.z() < bbx_min.z() || 769 | cube_lower_bound.z() > bbx_max.z()) { 770 | continue; 771 | } 772 | 773 | if (octree_->isNodeOccupied(*it) && occupied_boxes) { 774 | box_vector->emplace_back(cube_center, cube_size); 775 | } else if (!octree_->isNodeOccupied(*it) && !occupied_boxes) { 776 | box_vector->emplace_back(cube_center, cube_size); 777 | } 778 | } 779 | } 780 | 781 | void OctomapWorld::setLogOddsBoundingBox( 782 | const Eigen::Vector3d& position, const Eigen::Vector3d& bounding_box_size, 783 | double log_odds_value, const BoundHandling& insertion_method) { 784 | std::vector positions; 785 | positions.push_back(position); 786 | setLogOddsBoundingBox(positions, bounding_box_size, log_odds_value, 787 | insertion_method); 788 | } 789 | 790 | void OctomapWorld::setLogOddsBoundingBox( 791 | const std::vector& positions, 792 | const Eigen::Vector3d& bounding_box_size, double log_odds_value, 793 | const BoundHandling& insertion_method) { 794 | const bool lazy_eval = true; 795 | const double resolution = octree_->getResolution(); 796 | Eigen::Vector3d bbx_min, bbx_max; 797 | 798 | for (const Eigen::Vector3d& position : positions) { 799 | adjustBoundingBox(position, bounding_box_size, insertion_method, &bbx_min, 800 | &bbx_max); 801 | 802 | for (double x_position = bbx_min.x(); x_position <= bbx_max.x(); 803 | x_position += resolution) { 804 | for (double y_position = bbx_min.y(); y_position <= bbx_max.y(); 805 | y_position += resolution) { 806 | for (double z_position = bbx_min.z(); z_position <= bbx_max.z(); 807 | z_position += resolution) { 808 | octomap::point3d point = 809 | octomap::point3d(x_position, y_position, z_position); 810 | octree_->setNodeValue(point, log_odds_value, lazy_eval); 811 | } 812 | } 813 | } 814 | } 815 | 816 | // This is necessary since lazy_eval is set to true. 817 | octree_->updateInnerOccupancy(); 818 | } 819 | 820 | bool OctomapWorld::getOctomapBinaryMsg(octomap_msgs::Octomap* msg) const { 821 | return octomap_msgs::binaryMapToMsg(*octree_, *msg); 822 | } 823 | 824 | bool OctomapWorld::getOctomapFullMsg(octomap_msgs::Octomap* msg) const { 825 | return octomap_msgs::fullMapToMsg(*octree_, *msg); 826 | } 827 | 828 | void OctomapWorld::setOctomapFromMsg(const octomap_msgs::Octomap& msg) { 829 | if (msg.binary) { 830 | setOctomapFromBinaryMsg(msg); 831 | } else { 832 | setOctomapFromFullMsg(msg); 833 | } 834 | } 835 | 836 | void OctomapWorld::setOctomapFromBinaryMsg(const octomap_msgs::Octomap& msg) { 837 | octree_.reset( 838 | dynamic_cast(octomap_msgs::binaryMsgToMap(msg))); 839 | } 840 | 841 | void OctomapWorld::setOctomapFromFullMsg(const octomap_msgs::Octomap& msg) { 842 | octree_.reset( 843 | dynamic_cast(octomap_msgs::fullMsgToMap(msg))); 844 | } 845 | 846 | bool OctomapWorld::loadOctomapFromFile(const std::string& filename) { 847 | return octree_->readBinary(filename); 848 | } 849 | 850 | bool OctomapWorld::writeOctomapToFile(const std::string& filename) { 851 | return octree_->writeBinary(filename); 852 | } 853 | 854 | bool OctomapWorld::writeOctomapToBinaryConst(std::ostream& s) const { 855 | return octree_->writeBinaryConst(s); 856 | } 857 | 858 | bool OctomapWorld::isSpeckleNode(const octomap::OcTreeKey& key) const { 859 | octomap::OcTreeKey current_key; 860 | // Search neighbors in a +/-1 key range cube. If there are neighbors, it's 861 | // not a speckle. 862 | bool neighbor_found = false; 863 | for (current_key[2] = key[2] - 1; current_key[2] <= key[2] + 1; 864 | ++current_key[2]) { 865 | for (current_key[1] = key[1] - 1; current_key[1] <= key[1] + 1; 866 | ++current_key[1]) { 867 | for (current_key[0] = key[0] - 1; current_key[0] <= key[0] + 1; 868 | ++current_key[0]) { 869 | if (current_key != key) { 870 | octomap::OcTreeNode* node = octree_->search(key); 871 | if (node && octree_->isNodeOccupied(node)) { 872 | // We have a neighbor => not a speckle! 873 | return false; 874 | } 875 | } 876 | } 877 | } 878 | } 879 | return true; 880 | } 881 | 882 | void OctomapWorld::generateMarkerArray( 883 | const std::string& tf_frame, 884 | visualization_msgs::MarkerArray* occupied_nodes, 885 | visualization_msgs::MarkerArray* free_nodes) { 886 | CHECK_NOTNULL(occupied_nodes); 887 | CHECK_NOTNULL(free_nodes); 888 | 889 | // Prune the octree first. 890 | octree_->prune(); 891 | int tree_depth = octree_->getTreeDepth() + 1; 892 | 893 | // In the marker array, assign each node to its respective depth level, since 894 | // all markers in a CUBE_LIST must have the same scale. 895 | occupied_nodes->markers.resize(tree_depth); 896 | free_nodes->markers.resize(tree_depth); 897 | 898 | // Metric min and max z of the map: 899 | double min_x, min_y, min_z, max_x, max_y, max_z; 900 | octree_->getMetricMin(min_x, min_y, min_z); 901 | octree_->getMetricMax(max_x, max_y, max_z); 902 | 903 | // Update values from params if necessary. 904 | if (params_.visualize_min_z > min_z) { 905 | min_z = params_.visualize_min_z; 906 | } 907 | if (params_.visualize_max_z < max_z) { 908 | max_z = params_.visualize_max_z; 909 | } 910 | 911 | for (int i = 0; i < tree_depth; ++i) { 912 | double size = octree_->getNodeSize(i); 913 | 914 | occupied_nodes->markers[i].header.frame_id = tf_frame; 915 | occupied_nodes->markers[i].ns = "map"; 916 | occupied_nodes->markers[i].id = i; 917 | occupied_nodes->markers[i].type = visualization_msgs::Marker::CUBE_LIST; 918 | occupied_nodes->markers[i].scale.x = size; 919 | occupied_nodes->markers[i].scale.y = size; 920 | occupied_nodes->markers[i].scale.z = size; 921 | 922 | free_nodes->markers[i] = occupied_nodes->markers[i]; 923 | } 924 | 925 | for (octomap::OcTree::leaf_iterator it = octree_->begin_leafs(), 926 | end = octree_->end_leafs(); 927 | it != end; ++it) { 928 | geometry_msgs::Point cube_center; 929 | cube_center.x = it.getX(); 930 | cube_center.y = it.getY(); 931 | cube_center.z = it.getZ(); 932 | 933 | if (cube_center.z > max_z || cube_center.z < min_z) { 934 | continue; 935 | } 936 | 937 | int depth_level = it.getDepth(); 938 | 939 | if (octree_->isNodeOccupied(*it)) { 940 | occupied_nodes->markers[depth_level].points.push_back(cube_center); 941 | occupied_nodes->markers[depth_level].colors.push_back( 942 | percentToColor(colorizeMapByHeight(it.getZ(), min_z, max_z))); 943 | } else { 944 | free_nodes->markers[depth_level].points.push_back(cube_center); 945 | free_nodes->markers[depth_level].colors.push_back( 946 | percentToColor(colorizeMapByHeight(it.getZ(), min_z, max_z))); 947 | } 948 | } 949 | 950 | for (int i = 0; i < tree_depth; ++i) { 951 | if (occupied_nodes->markers[i].points.size() > 0) { 952 | occupied_nodes->markers[i].action = visualization_msgs::Marker::ADD; 953 | } else { 954 | occupied_nodes->markers[i].action = visualization_msgs::Marker::DELETE; 955 | } 956 | 957 | if (free_nodes->markers[i].points.size() > 0) { 958 | free_nodes->markers[i].action = visualization_msgs::Marker::ADD; 959 | } else { 960 | free_nodes->markers[i].action = visualization_msgs::Marker::DELETE; 961 | } 962 | } 963 | } 964 | 965 | void OctomapWorld::convertUnknownToFree() { 966 | Eigen::Vector3d min_bound, max_bound; 967 | getMapBounds(&min_bound, &max_bound); 968 | convertUnknownToFree(min_bound, max_bound); 969 | } 970 | 971 | void OctomapWorld::convertUnknownToFree(const Eigen::Vector3d& min_bound, 972 | const Eigen::Vector3d& max_bound) { 973 | const bool lazy_eval = true; 974 | const double log_odds_value = octree_->getClampingThresMinLog(); 975 | const double resolution = octree_->getResolution(); 976 | const double epsilon = 0.001; // Small offset to not hit boundary of nodes. 977 | Eigen::Vector3d epsilon_3d; 978 | epsilon_3d.setConstant(epsilon); 979 | octomap::point3d pmin = pointEigenToOctomap(min_bound + epsilon_3d); 980 | octomap::point3d pmax = pointEigenToOctomap(max_bound - epsilon_3d); 981 | // octree_->getUnknownLeafCenters would have been easier, but it doesn't get 982 | // all the unknown points for some reason 983 | for (float x = pmin.x() + epsilon; x < pmax.x(); x += resolution) { 984 | for (float y = pmin.y() + epsilon; y < pmax.y(); y += resolution) { 985 | for (float z = pmin.z() + epsilon; z < pmax.z(); z += resolution) { 986 | octomap::OcTree::NodeType* res = octree_->search(x, y, z); 987 | if (res == NULL) { 988 | // Point is unknown, set it free 989 | octree_->setNodeValue(x, y, z, log_odds_value, lazy_eval); 990 | } 991 | } 992 | } 993 | } 994 | if (lazy_eval) { 995 | octree_->updateInnerOccupancy(); 996 | } 997 | octree_->prune(); 998 | } 999 | 1000 | void OctomapWorld::inflateOccupied(const Eigen::Vector3d& safety_space) { 1001 | // Inflate all obstacles by safety_space, such that if a collision free 1002 | // trajectory is generated in this new space, it is guaranteed that 1003 | // safety_space around this trajectory is collision free in the original space 1004 | const bool lazy_eval = true; 1005 | const double log_odds_value = octree_->getClampingThresMaxLog(); 1006 | const double resolution = octree_->getResolution(); 1007 | const double epsilon = 0.001; // Small offset to not hit boundary of nodes. 1008 | Eigen::Vector3d epsilon_3d; 1009 | epsilon_3d.setConstant(epsilon); 1010 | 1011 | // Compute the maximal distance from the map's bounds where a small box has to 1012 | // be checked explicitly for its feasibility. 1013 | double bound_threshold = 0; 1014 | double resolution_multiple = resolution; 1015 | while (resolution_multiple < safety_space.minCoeff() / 4.0 - epsilon) { 1016 | bound_threshold += resolution_multiple; 1017 | resolution_multiple *= 2; 1018 | } 1019 | Eigen::Vector3d map_min_bound, map_max_bound; 1020 | getMapBounds(&map_min_bound, &map_max_bound); 1021 | 1022 | std::vector> free_boxes_vector; 1023 | getAllFreeBoxes(&free_boxes_vector); 1024 | 1025 | Eigen::Vector3d actual_position; 1026 | octomap::KeySet occupied_keys; 1027 | octomap::OcTreeKey actual_key; 1028 | 1029 | for (const std::pair& free_box : free_boxes_vector) { 1030 | // In case box size implicates that the whole box is infeasible (an obstacle 1031 | // is at a distance of at most 2 * box_size (from the center of the 1032 | // safety_space), otherwise the pruned free box would have been bigger) 1033 | if (free_box.second < safety_space.minCoeff() / 4.0 - epsilon) { 1034 | Eigen::Vector3d min_bound_distance = free_box.first - map_min_bound; 1035 | Eigen::Vector3d max_bound_distance = map_max_bound - free_box.first; 1036 | Eigen::Vector3d bound_distance = 1037 | min_bound_distance.cwiseMin(max_bound_distance); 1038 | if (bound_distance.minCoeff() > bound_threshold) { 1039 | // It's not at the map's bounds, therefore its small size depends on a 1040 | // near obstacle. 1041 | getKeysBoundingBox(free_box.first, 1042 | Eigen::Vector3d::Constant(free_box.second), 1043 | &occupied_keys, BoundHandling::kIncludePartialBoxes); 1044 | continue; 1045 | } 1046 | } 1047 | 1048 | // In case the whole box is feasible, nothing has to be done with this box 1049 | if (getCellStatusBoundingBox( 1050 | free_box.first, safety_space + Eigen::Vector3d::Constant( 1051 | free_box.second)) != kOccupied) { 1052 | continue; 1053 | } 1054 | 1055 | // In case the whole box can't be feasible (bounding box of safety_space 1056 | // around a point on one bound of the box would hit obstacle on the other 1057 | // side) 1058 | if (getCellStatusBoundingBox( 1059 | free_box.first, 1060 | Eigen::Vector3d::Constant(resolution - epsilon) 1061 | .cwiseMax(safety_space - Eigen::Vector3d::Constant( 1062 | free_box.second))) == kOccupied) { 1063 | getKeysBoundingBox(free_box.first, 1064 | Eigen::Vector3d::Constant(free_box.second), 1065 | &occupied_keys, BoundHandling::kIncludePartialBoxes); 1066 | continue; 1067 | } 1068 | 1069 | // Otherwise find which obstacles cause some parts of the box to be 1070 | // infeasible, and find all those points through inflating those obstacles 1071 | std::vector> occupied_boxes_vector; 1072 | getOccupiedBoxesBoundingBox( 1073 | free_box.first, 1074 | safety_space + Eigen::Vector3d::Constant(free_box.second), 1075 | &occupied_boxes_vector); 1076 | for (const std::pair& box_occupied : 1077 | occupied_boxes_vector) { 1078 | // Infeasible volume caused by box_occupied 1079 | Eigen::Vector3d infeasible_box_min = 1080 | box_occupied.first - 1081 | (Eigen::Vector3d::Constant(box_occupied.second) + safety_space) / 2; 1082 | Eigen::Vector3d infeasible_box_max = 1083 | box_occupied.first + 1084 | (Eigen::Vector3d::Constant(box_occupied.second) + safety_space) / 2; 1085 | // Volume of free_box 1086 | Eigen::Vector3d actual_box_min = 1087 | free_box.first - Eigen::Vector3d::Constant(free_box.second) / 2; 1088 | Eigen::Vector3d actual_box_max = 1089 | free_box.first + Eigen::Vector3d::Constant(free_box.second) / 2; 1090 | // Overlapping volume of box_infeasible and free_box 1091 | Eigen::Vector3d bbx_min = actual_box_min.cwiseMax(infeasible_box_min); 1092 | Eigen::Vector3d bbx_max = actual_box_max.cwiseMin(infeasible_box_max); 1093 | Eigen::Vector3d bbx_center = (bbx_min + bbx_max) / 2; 1094 | Eigen::Vector3d bbx_size = bbx_max - bbx_min; 1095 | getKeysBoundingBox(bbx_center, bbx_size, &occupied_keys, 1096 | BoundHandling::kIncludePartialBoxes); 1097 | } 1098 | } 1099 | 1100 | // Inflate all obstacles at the map's borders, since the obstacles that are 1101 | // less than safety_space / 2 away from the boundary have to be inflated 1102 | // beyond the boundary. 1103 | Eigen::Vector3d direction, bounding_box_center, bounding_box_size; 1104 | std::vector> occupied_boxes_vector; 1105 | for (unsigned i = 0u; i < 3u; i++) { 1106 | for (int sign = -1; sign <= 1; sign += 2) { 1107 | direction = Eigen::Vector3d::Zero(); 1108 | direction[i] = sign; 1109 | bounding_box_center = getMapCenter(); 1110 | bounding_box_center[i] += 1111 | sign * (getMapSize()[i] - safety_space[i] / 2) / 2; 1112 | bounding_box_size = getMapSize(); 1113 | bounding_box_size[i] = safety_space[i] / 2; 1114 | getOccupiedBoxesBoundingBox(bounding_box_center, 1115 | bounding_box_size - epsilon_3d, 1116 | &occupied_boxes_vector); 1117 | for (const std::pair& occupied_box : 1118 | occupied_boxes_vector) { 1119 | // Set just the inflated volume that is outside the original map bounds. 1120 | Eigen::Vector3d outer_bbx_min = 1121 | occupied_box.first - 1122 | (Eigen::Vector3d::Constant(occupied_box.second) + safety_space) / 2; 1123 | Eigen::Vector3d outer_bbx_max = 1124 | occupied_box.first + 1125 | (Eigen::Vector3d::Constant(occupied_box.second) + safety_space) / 2; 1126 | if (sign == -1) { 1127 | outer_bbx_max[i] = map_min_bound[i]; 1128 | } else { 1129 | outer_bbx_min[i] = map_max_bound[i]; 1130 | } 1131 | Eigen::Vector3d outer_bbx_center = (outer_bbx_min + outer_bbx_max) / 2; 1132 | Eigen::Vector3d outer_bbx_size = 1133 | outer_bbx_max - outer_bbx_min - epsilon_3d; 1134 | getKeysBoundingBox(outer_bbx_center, outer_bbx_size, &occupied_keys, 1135 | BoundHandling::kIncludePartialBoxes); 1136 | } 1137 | } 1138 | } 1139 | 1140 | // Set all infeasible points occupied 1141 | for (octomap::OcTreeKey key : occupied_keys) { 1142 | octree_->setNodeValue(octree_->keyToCoord(key), log_odds_value, lazy_eval); 1143 | } 1144 | 1145 | if (lazy_eval) { 1146 | octree_->updateInnerOccupancy(); 1147 | } 1148 | octree_->prune(); 1149 | } 1150 | 1151 | void OctomapWorld::getKeysBoundingBox( 1152 | const Eigen::Vector3d& position, const Eigen::Vector3d& bounding_box_size, 1153 | octomap::KeySet* keys, const BoundHandling& insertion_method) const { 1154 | const double resolution = octree_->getResolution(); 1155 | Eigen::Vector3d bbx_min, bbx_max; 1156 | adjustBoundingBox(position, bounding_box_size, insertion_method, &bbx_min, 1157 | &bbx_max); 1158 | Eigen::Vector3d actual_position; 1159 | octomap::OcTreeKey actual_key; 1160 | for (double x_position = bbx_min.x(); x_position <= bbx_max.x(); 1161 | x_position += resolution) { 1162 | for (double y_position = bbx_min.y(); y_position <= bbx_max.y(); 1163 | y_position += resolution) { 1164 | for (double z_position = bbx_min.z(); z_position <= bbx_max.z(); 1165 | z_position += resolution) { 1166 | actual_position << x_position, y_position, z_position; 1167 | coordToKey(actual_position, &actual_key); 1168 | keys->insert(actual_key); 1169 | } 1170 | } 1171 | } 1172 | } 1173 | 1174 | void OctomapWorld::adjustBoundingBox(const Eigen::Vector3d& position, 1175 | const Eigen::Vector3d& bounding_box_size, 1176 | const BoundHandling& insertion_method, 1177 | Eigen::Vector3d* bbx_min, 1178 | Eigen::Vector3d* bbx_max) const { 1179 | const double resolution = octree_->getResolution(); 1180 | Eigen::Vector3d resolution_3d; 1181 | resolution_3d.setConstant(resolution); 1182 | const double epsilon = 0.001; // Small offset to not hit boundary of nodes. 1183 | Eigen::Vector3d epsilon_3d; 1184 | epsilon_3d.setConstant(epsilon); 1185 | 1186 | if (insertion_method == BoundHandling::kDefault) { 1187 | *bbx_min = position - bounding_box_size / 2 - epsilon_3d; 1188 | *bbx_max = position + bounding_box_size / 2 + epsilon_3d; 1189 | 1190 | } else if (insertion_method == BoundHandling::kIncludePartialBoxes) { 1191 | // If the bbx is exactly on boundary of a node, reducing the bbx by 1192 | // epsilon_3d will avoid to include the adjacent nodes as well. 1193 | *bbx_min = position - bounding_box_size / 2 + epsilon_3d; 1194 | *bbx_max = position + bounding_box_size / 2 - epsilon_3d; 1195 | // Align positions to the center of the octree boxes 1196 | octomap::OcTreeKey bbx_min_key = 1197 | octree_->coordToKey(pointEigenToOctomap(*bbx_min)); 1198 | octomap::OcTreeKey bbx_max_key = 1199 | octree_->coordToKey(pointEigenToOctomap(*bbx_max)); 1200 | *bbx_min = pointOctomapToEigen(octree_->keyToCoord(bbx_min_key)); 1201 | *bbx_max = pointOctomapToEigen(octree_->keyToCoord(bbx_max_key)); 1202 | // Add small offset so that all boxes are considered in the for-loops 1203 | *bbx_min -= epsilon_3d; 1204 | *bbx_max += epsilon_3d; 1205 | } else if (insertion_method == BoundHandling::kIgnorePartialBoxes) { 1206 | // If the bbx is exactly on boundary of a node, incrementing the bbx by 1207 | // epsilon_3d will include that node as well. 1208 | *bbx_min = position - bounding_box_size / 2 - epsilon_3d; 1209 | *bbx_max = position + bounding_box_size / 2 + epsilon_3d; 1210 | // Align positions to the center of the octree boxes 1211 | octomap::OcTreeKey bbx_min_key = 1212 | octree_->coordToKey(pointEigenToOctomap(*bbx_min)); 1213 | octomap::OcTreeKey bbx_max_key = 1214 | octree_->coordToKey(pointEigenToOctomap(*bbx_max)); 1215 | *bbx_min = pointOctomapToEigen(octree_->keyToCoord(bbx_min_key)); 1216 | *bbx_max = pointOctomapToEigen(octree_->keyToCoord(bbx_max_key)); 1217 | // Crop bounding box so that it excludes the partially included boxes 1218 | *bbx_min += resolution_3d; 1219 | *bbx_max -= resolution_3d; 1220 | // Add small offset so that all boxes are considered in the for-loops 1221 | *bbx_min -= epsilon_3d; 1222 | *bbx_max += epsilon_3d; 1223 | } 1224 | } 1225 | 1226 | double OctomapWorld::colorizeMapByHeight(double z, double min_z, 1227 | double max_z) const { 1228 | return (1.0 - std::min(std::max((z - min_z) / (max_z - min_z), 0.0), 1.0)); 1229 | } 1230 | 1231 | std_msgs::ColorRGBA OctomapWorld::percentToColor(double h) const { 1232 | // Helen's note: direct copy from OctomapProvider. 1233 | std_msgs::ColorRGBA color; 1234 | color.a = 1.0; 1235 | // blend over HSV-values (more colors) 1236 | 1237 | double s = 1.0; 1238 | double v = 1.0; 1239 | 1240 | h -= floor(h); 1241 | h *= 6; 1242 | int i; 1243 | double m, n, f; 1244 | 1245 | i = floor(h); 1246 | f = h - i; 1247 | if (!(i & 1)) f = 1 - f; // if i is even 1248 | m = v * (1 - s); 1249 | n = v * (1 - s * f); 1250 | 1251 | switch (i) { 1252 | case 6: 1253 | case 0: 1254 | color.r = v; 1255 | color.g = n; 1256 | color.b = m; 1257 | break; 1258 | case 1: 1259 | color.r = n; 1260 | color.g = v; 1261 | color.b = m; 1262 | break; 1263 | case 2: 1264 | color.r = m; 1265 | color.g = v; 1266 | color.b = n; 1267 | break; 1268 | case 3: 1269 | color.r = m; 1270 | color.g = n; 1271 | color.b = v; 1272 | break; 1273 | case 4: 1274 | color.r = n; 1275 | color.g = m; 1276 | color.b = v; 1277 | break; 1278 | case 5: 1279 | color.r = v; 1280 | color.g = m; 1281 | color.b = n; 1282 | break; 1283 | default: 1284 | color.r = 1; 1285 | color.g = 0.5; 1286 | color.b = 0.5; 1287 | break; 1288 | } 1289 | 1290 | return color; 1291 | } 1292 | 1293 | Eigen::Vector3d OctomapWorld::getMapCenter() const { 1294 | // Metric min and max z of the map: 1295 | double min_x, min_y, min_z, max_x, max_y, max_z; 1296 | octree_->getMetricMin(min_x, min_y, min_z); 1297 | octree_->getMetricMax(max_x, max_y, max_z); 1298 | 1299 | Eigen::Vector3d min_3d(min_x, min_y, min_z); 1300 | Eigen::Vector3d max_3d(max_x, max_y, max_z); 1301 | 1302 | return min_3d + (max_3d - min_3d) / 2; 1303 | } 1304 | 1305 | Eigen::Vector3d OctomapWorld::getMapSize() const { 1306 | // Metric min and max z of the map: 1307 | double min_x, min_y, min_z, max_x, max_y, max_z; 1308 | octree_->getMetricMin(min_x, min_y, min_z); 1309 | octree_->getMetricMax(max_x, max_y, max_z); 1310 | 1311 | return Eigen::Vector3d(max_x - min_x, max_y - min_y, max_z - min_z); 1312 | } 1313 | 1314 | void OctomapWorld::getMapBounds(Eigen::Vector3d* min_bound, 1315 | Eigen::Vector3d* max_bound) const { 1316 | CHECK_NOTNULL(min_bound); 1317 | CHECK_NOTNULL(max_bound); 1318 | // Metric min and max z of the map: 1319 | double min_x, min_y, min_z, max_x, max_y, max_z; 1320 | octree_->getMetricMin(min_x, min_y, min_z); 1321 | octree_->getMetricMax(max_x, max_y, max_z); 1322 | 1323 | *min_bound = Eigen::Vector3d(min_x, min_y, min_z); 1324 | *max_bound = Eigen::Vector3d(max_x, max_y, max_z); 1325 | } 1326 | 1327 | bool OctomapWorld::getNearestFreePoint(const Eigen::Vector3d& position, 1328 | Eigen::Vector3d* free_position) const { 1329 | const double epsilon = 1e-3; // Small offset to not hit boundaries 1330 | // Check if the given position is already unoccupied 1331 | if (getCellStatusPoint(position) == CellStatus::kFree) { 1332 | *free_position = position; 1333 | return true; 1334 | } 1335 | 1336 | const double resolution = octree_->getResolution(); 1337 | double bbx_size = resolution; 1338 | std::vector> free_box_vector; 1339 | // Find the nearest free boxes, enlarge searching volume around position until 1340 | // there is something unoccupied 1341 | while (free_box_vector.empty() && bbx_size < getMapSize().maxCoeff()) { 1342 | getFreeBoxesBoundingBox(position, Eigen::Vector3d::Constant(bbx_size), 1343 | &free_box_vector); 1344 | bbx_size += resolution; 1345 | } 1346 | if (free_box_vector.empty()) { 1347 | return false; // There are no free boxes in the octomap 1348 | } 1349 | 1350 | // Overestimate minimum distance between desired position and free position 1351 | double min_distance = bbx_size; 1352 | Eigen::Vector3d actual_distance; 1353 | Eigen::Vector3d actual_nearest_position; 1354 | for (const std::pair& free_box : free_box_vector) { 1355 | // Distance between center of box and position 1356 | actual_distance = position - free_box.first; 1357 | // Limit the distance such that it is still in the box 1358 | actual_distance = actual_distance.cwiseMin( 1359 | Eigen::Vector3d::Constant(free_box.second / 2 - epsilon)); 1360 | actual_distance = actual_distance.cwiseMax( 1361 | Eigen::Vector3d::Constant(-free_box.second / 2 + epsilon)); 1362 | // Nearest position to the desired position 1363 | actual_nearest_position = free_box.first + actual_distance; 1364 | // Check if this is the best position found so far 1365 | if ((position - actual_nearest_position).norm() < min_distance) { 1366 | min_distance = (position - actual_nearest_position).norm(); 1367 | *free_position = actual_nearest_position; 1368 | } 1369 | } 1370 | return true; 1371 | } 1372 | 1373 | void OctomapWorld::setRobotSize(const Eigen::Vector3d& robot_size) { 1374 | robot_size_ = robot_size; 1375 | } 1376 | 1377 | Eigen::Vector3d OctomapWorld::getRobotSize() const { return robot_size_; } 1378 | 1379 | bool OctomapWorld::checkCollisionWithRobot( 1380 | const Eigen::Vector3d& robot_position) { 1381 | return checkSinglePoseCollision(robot_position); 1382 | } 1383 | 1384 | bool OctomapWorld::checkPathForCollisionsWithRobot( 1385 | const std::vector& robot_positions, 1386 | size_t* collision_index) { 1387 | // Iterate over vector of poses. 1388 | // Check each one. 1389 | // Return when a collision is found, and return the index of the earliest 1390 | // collision. 1391 | for (size_t i = 0; i < robot_positions.size(); ++i) { 1392 | if (checkSinglePoseCollision(robot_positions[i])) { 1393 | if (collision_index != nullptr) { 1394 | *collision_index = i; 1395 | } 1396 | return true; 1397 | } 1398 | } 1399 | return false; 1400 | } 1401 | 1402 | bool OctomapWorld::checkSinglePoseCollision( 1403 | const Eigen::Vector3d& robot_position) const { 1404 | if (params_.treat_unknown_as_occupied) { 1405 | return (CellStatus::kFree != 1406 | getCellStatusBoundingBox(robot_position, robot_size_)); 1407 | } else { 1408 | return (CellStatus::kOccupied == 1409 | getCellStatusBoundingBox(robot_position, robot_size_)); 1410 | } 1411 | } 1412 | 1413 | void OctomapWorld::getChangedPoints( 1414 | std::vector* changed_points, 1415 | std::vector* changed_states) { 1416 | CHECK_NOTNULL(changed_points); 1417 | // These keys are always *leaf node* keys, even if the actual change was in 1418 | // a larger cube (see Octomap docs). 1419 | octomap::KeyBoolMap::const_iterator start_key = octree_->changedKeysBegin(); 1420 | octomap::KeyBoolMap::const_iterator end_key = octree_->changedKeysEnd(); 1421 | 1422 | changed_points->clear(); 1423 | if (changed_states != NULL) { 1424 | changed_states->clear(); 1425 | } 1426 | 1427 | for (octomap::KeyBoolMap::const_iterator iter = start_key; iter != end_key; 1428 | ++iter) { 1429 | octomap::OcTreeNode* node = octree_->search(iter->first); 1430 | bool occupied = octree_->isNodeOccupied(node); 1431 | Eigen::Vector3d center = 1432 | pointOctomapToEigen(octree_->keyToCoord(iter->first)); 1433 | 1434 | changed_points->push_back(center); 1435 | if (changed_states != NULL) { 1436 | changed_states->push_back(occupied); 1437 | } 1438 | } 1439 | octree_->resetChangeDetection(); 1440 | } 1441 | 1442 | void OctomapWorld::coordToKey(const Eigen::Vector3d& coord, 1443 | octomap::OcTreeKey* key) const { 1444 | octomap::point3d position(coord.x(), coord.y(), coord.z()); 1445 | *key = octree_->coordToKey(position); 1446 | } 1447 | 1448 | void OctomapWorld::keyToCoord(const octomap::OcTreeKey& key, 1449 | Eigen::Vector3d* coord) const { 1450 | octomap::point3d position; 1451 | position = octree_->keyToCoord(key); 1452 | *coord << position.x(), position.y(), position.z(); 1453 | } 1454 | 1455 | } // namespace volumetric_mapping 1456 | -------------------------------------------------------------------------------- /volumetric_map_base/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(volumetric_map_base) 3 | 4 | find_package(catkin_simple REQUIRED) 5 | catkin_simple(ALL_DEPS_REQUIRED) 6 | 7 | add_definitions(-std=c++11) 8 | 9 | ############# 10 | # LIBRARIES # 11 | ############# 12 | cs_add_library(${PROJECT_NAME} 13 | src/world_base.cc 14 | ) 15 | 16 | ########## 17 | # EXPORT # 18 | ########## 19 | cs_install() 20 | cs_export() 21 | -------------------------------------------------------------------------------- /volumetric_map_base/include/volumetric_map_base/point_weighing.h: -------------------------------------------------------------------------------- 1 | /* 2 | Copyright (c) 2015, Helen Oleynikova, ETH Zurich, Switzerland 3 | You can contact the author at 4 | 5 | All rights reserved. 6 | 7 | Redistribution and use in source and binary forms, with or without 8 | modification, are permitted provided that the following conditions are met: 9 | * Redistributions of source code must retain the above copyright 10 | notice, this list of conditions and the following disclaimer. 11 | * Redistributions in binary form must reproduce the above copyright 12 | notice, this list of conditions and the following disclaimer in the 13 | documentation and/or other materials provided with the distribution. 14 | * Neither the name of ETHZ-ASL nor the 15 | names of its contributors may be used to endorse or promote products 16 | derived from this software without specific prior written permission. 17 | 18 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 19 | ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 20 | WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 21 | DISCLAIMED. IN NO EVENT SHALL ETHZ-ASL BE LIABLE FOR ANY 22 | DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 23 | (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 24 | LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 25 | ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 26 | (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 27 | SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 28 | */ 29 | 30 | #ifndef VOLUMETRIC_MAP_BASE_WEIGHING_FUNCTION_H_ 31 | #define VOLUMETRIC_MAP_BASE_WEIGHING_FUNCTION_H_ 32 | 33 | #include 34 | 35 | namespace volumetric_mapping { 36 | 37 | // A base class for weighing functions: each weighing function should take in 38 | // either a point in 3D (PCL), UVD (disparity), or an Eigen point (or 39 | // preferably all 3) and return a weight between 0.0 and 1.0 as confidence 40 | // value for the measurement. 41 | // For example, default behavior would happen with a value of 1.0 for all points 42 | // (and this is the base class implementation). 43 | // In the case of raycasting-built maps, such as octomap, all occupied 44 | // probabilities in the ray of the point will be scaled by this amount. 45 | class PointWeighing { 46 | public: 47 | PointWeighing() {} 48 | virtual ~PointWeighing() {} 49 | 50 | virtual double computeWeightForPoint(double x, double y, double z) const { 51 | return 1.0; 52 | } 53 | virtual double computeWeightForDisparity(unsigned int u, unsigned int v, 54 | double d) const { 55 | return 1.0; 56 | } 57 | }; 58 | 59 | } // namespace volumetric_mapping 60 | 61 | #endif // VOLUMETRIC_MAP_BASE_WEIGHING_FUNCTION_H_ 62 | -------------------------------------------------------------------------------- /volumetric_map_base/include/volumetric_map_base/world_base.h: -------------------------------------------------------------------------------- 1 | /* 2 | Copyright (c) 2015, Helen Oleynikova, ETH Zurich, Switzerland 3 | You can contact the author at 4 | 5 | All rights reserved. 6 | 7 | Redistribution and use in source and binary forms, with or without 8 | modification, are permitted provided that the following conditions are met: 9 | * Redistributions of source code must retain the above copyright 10 | notice, this list of conditions and the following disclaimer. 11 | * Redistributions in binary form must reproduce the above copyright 12 | notice, this list of conditions and the following disclaimer in the 13 | documentation and/or other materials provided with the distribution. 14 | * Neither the name of ETHZ-ASL nor the 15 | names of its contributors may be used to endorse or promote products 16 | derived from this software without specific prior written permission. 17 | 18 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 19 | ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 20 | WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 21 | DISCLAIMED. IN NO EVENT SHALL ETHZ-ASL BE LIABLE FOR ANY 22 | DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 23 | (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 24 | LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 25 | ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 26 | (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 27 | SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 28 | */ 29 | 30 | #ifndef VOLUMETRIC_MAP_BASE_WORLD_BASE_H_ 31 | #define VOLUMETRIC_MAP_BASE_WORLD_BASE_H_ 32 | 33 | #include 34 | #include 35 | #include 36 | #include 37 | #include 38 | #include 39 | #include 40 | #include 41 | 42 | #include "volumetric_map_base/point_weighing.h" 43 | 44 | namespace volumetric_mapping { 45 | 46 | typedef kindr::minimal::QuatTransformation Transformation; 47 | 48 | // Base class for all 3D volumetric representations of the environment. 49 | // By default, implements a valid completely empty world. 50 | class WorldBase { 51 | public: 52 | typedef std::shared_ptr Ptr; 53 | 54 | enum CellStatus { kFree = 0, kOccupied = 1, kUnknown = 2 }; 55 | 56 | WorldBase() {} 57 | virtual ~WorldBase() {} 58 | 59 | // Data insertion functions. 60 | // Project the given disparity map to 3D and insert it into the map. 61 | // Q is a 4x4 perspective disparity-to-depth mapping matrix for the full-size 62 | // image. This takes care of adjusting Q for downsampled disparity maps as 63 | // well. 64 | // See: http://docs.opencv.org/modules/calib3d/doc/ 65 | // camera_calibration_and_3d_reconstruction.html#reprojectimageto3d 66 | void insertDisparityImage( 67 | const Transformation& sensor_to_world, 68 | const stereo_msgs::DisparityImageConstPtr& disparity, 69 | const Eigen::Matrix4d& Q_full, const Eigen::Vector2d& full_image_size); 70 | void insertDisparityImage(const Transformation& sensor_to_world, 71 | const cv::Mat& disparity, 72 | const Eigen::Matrix4d& Q_full, 73 | const Eigen::Vector2d& full_image_size); 74 | 75 | // Helper functions to compute the Q matrix for given camera parameters. 76 | // Assumes UNRECTIFIED camera matrices. 77 | // Downsampling is handled in insertDisparityImage. 78 | Eigen::Matrix4d getQForCameras(const Transformation& T_C1_C0, 79 | const Eigen::Matrix3d& left_cam_matrix, 80 | const Eigen::Matrix3d& right_cam_matrix, 81 | const Eigen::Vector2d& full_image_size) const; 82 | Eigen::Matrix4d getQForROSCameras( 83 | const sensor_msgs::CameraInfo& left_camera, 84 | const sensor_msgs::CameraInfo& right_camera) const; 85 | 86 | // Calls insertPointcloudImpl() or insertPointcloudIntoMapWithWeightsImpl(), 87 | // depending if points are to be weighted. 88 | void insertPointcloud( 89 | const Transformation& T_G_sensor, 90 | const sensor_msgs::PointCloud2::ConstPtr& pointcloud_sensor); 91 | void insertPointcloud(const Transformation& T_G_sensor, 92 | const Eigen::Matrix3Xd& pointcloud_sensor); 93 | void insertPointcloud( 94 | const Transformation& T_G_sensor, 95 | const pcl::PointCloud::Ptr& pointcloud_sensor); 96 | 97 | // Manually affect the state of a bounding box. For the WorldBase class, 98 | // setting to occupied is a no-op. 99 | virtual void setFree(const Eigen::Vector3d& position, 100 | const Eigen::Vector3d& bounding_box_size) {} 101 | virtual void setOccupied(const Eigen::Vector3d& position, 102 | const Eigen::Vector3d& bounding_box_size) {} 103 | 104 | // Methods to query the current map state. 105 | virtual CellStatus getCellStatusBoundingBox( 106 | const Eigen::Vector3d& point, 107 | const Eigen::Vector3d& bounding_box_size) const { 108 | return CellStatus::kFree; 109 | } 110 | virtual CellStatus getCellStatusPoint(const Eigen::Vector3d& point) const { 111 | return CellStatus::kFree; 112 | } 113 | 114 | virtual CellStatus getLineStatus(const Eigen::Vector3d& start, 115 | const Eigen::Vector3d& end) const { 116 | return CellStatus::kFree; 117 | } 118 | virtual CellStatus getLineStatusBoundingBox( 119 | const Eigen::Vector3d& start, const Eigen::Vector3d& end, 120 | const Eigen::Vector3d& bounding_box_size) const { 121 | return CellStatus::kFree; 122 | } 123 | 124 | virtual void getOccupiedPointCloud( 125 | pcl::PointCloud *output_cloud) const { 126 | // Blank world by default, so don't fill the pointcloud. 127 | return; 128 | } 129 | 130 | virtual void getOccupiedPointcloudInBoundingBox( 131 | const Eigen::Vector3d& center, const Eigen::Vector3d& bounding_box_size, 132 | pcl::PointCloud* output_cloud) const { 133 | // Blank world by default, so don't fill the pointcloud. 134 | return; 135 | } 136 | 137 | // Collision checking with a robot model. 138 | virtual void setRobotSize(double x, double y, double z) { 139 | setRobotSize(Eigen::Vector3d(x, y, z)); 140 | } 141 | virtual void setRobotSize(const Eigen::Vector3d& robot_size) { return; } 142 | virtual Eigen::Vector3d getRobotSize() const { 143 | return Eigen::Vector3d::Zero(); 144 | } 145 | 146 | virtual bool checkCollisionWithRobot(const Eigen::Vector3d& robot_position) { 147 | return false; 148 | } 149 | // Checks a path (assumed to be time-ordered) for collision. 150 | // Sets the second input to the index at which the collision occurred. 151 | virtual bool checkPathForCollisionsWithRobot( 152 | const std::vector& robot_positions, 153 | size_t* collision_index) { 154 | return false; 155 | } 156 | 157 | virtual Eigen::Vector3d getMapCenter() const { 158 | return Eigen::Vector3d::Zero(); 159 | } 160 | virtual Eigen::Vector3d getMapSize() const { 161 | return Eigen::Vector3d(std::numeric_limits::max(), 162 | std::numeric_limits::max(), 163 | std::numeric_limits::max()); 164 | } 165 | virtual void getMapBounds(Eigen::Vector3d* min_bound, 166 | Eigen::Vector3d* max_bound) const { 167 | CHECK_NOTNULL(min_bound); 168 | CHECK_NOTNULL(max_bound); 169 | *min_bound = Eigen::Vector3d(std::numeric_limits::min(), 170 | std::numeric_limits::min(), 171 | std::numeric_limits::min()); 172 | *max_bound = Eigen::Vector3d(std::numeric_limits::max(), 173 | std::numeric_limits::max(), 174 | std::numeric_limits::max()); 175 | } 176 | 177 | // Weighing class for points -> affect the weight of each point inserted 178 | // into the map. 179 | // If the weighing class is set, the "with weights" version if used for 180 | // all insertion functions. 181 | void setPointWeighing(const std::shared_ptr& point_weighing) { 182 | point_weighing_ = point_weighing; 183 | } 184 | 185 | void clearPointWeighing() { point_weighing_.reset(); } 186 | 187 | bool isPointWeighingSet() const { 188 | if (point_weighing_) { 189 | return true; 190 | } 191 | return false; 192 | } 193 | 194 | protected: 195 | // Called by insertDisparityImage(). Inheriting classes need to implement 196 | // this. 197 | // Input is the sensor to world transform and projected points in 3D in 198 | // the sensor coordinate frame, of type CV_32FC3. 199 | virtual void insertProjectedDisparityIntoMapImpl( 200 | const Transformation& sensor_to_world, const cv::Mat& projected_points) { 201 | LOG(ERROR) << "Calling unimplemented disparity insertion!"; 202 | } 203 | virtual void insertProjectedDisparityIntoMapWithWeightsImpl( 204 | const Transformation& sensor_to_world, const cv::Mat& projected_points, 205 | const cv::Mat& weights) { 206 | LOG(ERROR) << "Calling unimplemented disparity insertion!"; 207 | } 208 | 209 | virtual void insertPointcloudIntoMapImpl( 210 | const Transformation& T_G_sensor, 211 | const pcl::PointCloud::Ptr& pointcloud_sensor) { 212 | LOG(ERROR) << "Calling unimplemented pointcloud insertion!"; 213 | } 214 | virtual void insertPointcloudIntoMapWithWeightsImpl( 215 | const Transformation& sensor_to_world, 216 | const pcl::PointCloud::Ptr& pointcloud, 217 | const std::vector& weights) { 218 | LOG(ERROR) << "Calling unimplemented disparity insertion!"; 219 | } 220 | 221 | // Generate Q matrix from parameters. 222 | Eigen::Matrix4d generateQ(double Tx, double left_cx, double left_cy, 223 | double left_fx, double left_fy, double right_cx, 224 | double right_cy, double right_fx, 225 | double right_fy) const; 226 | 227 | // Compute weights from disparities. 228 | void computeWeights(const cv::Mat& disparity, cv::Mat* weights) const; 229 | 230 | // Compute weights from pointcloud data. 231 | void computeWeights(const pcl::PointCloud::Ptr& cloud, 232 | std::vector* weights) const; 233 | 234 | std::shared_ptr point_weighing_; 235 | }; 236 | 237 | } // namespace volumetric_mapping 238 | #endif // VOLUMETRIC_MAP_BASE_WORLD_BASE_H_ 239 | -------------------------------------------------------------------------------- /volumetric_map_base/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | volumetric_map_base 4 | 0.0.0 5 | Base class for volumentric map representations for planning. 6 | 7 | Helen Oleynikova 8 | Markus Achtelik 9 | 10 | Helen Oleynikova 11 | 12 | BSD 13 | 14 | catkin 15 | catkin_simple 16 | 17 | cv_bridge 18 | eigen_catkin 19 | gflags_catkin 20 | glog_catkin 21 | image_geometry 22 | minkindr 23 | pcl_conversions 24 | pcl_ros 25 | sensor_msgs 26 | stereo_msgs 27 | 28 | -------------------------------------------------------------------------------- /volumetric_map_base/src/world_base.cc: -------------------------------------------------------------------------------- 1 | /* 2 | Copyright (c) 2015, Helen Oleynikova, ETH Zurich, Switzerland 3 | You can contact the author at 4 | 5 | All rights reserved. 6 | 7 | Redistribution and use in source and binary forms, with or without 8 | modification, are permitted provided that the following conditions are met: 9 | * Redistributions of source code must retain the above copyright 10 | notice, this list of conditions and the following disclaimer. 11 | * Redistributions in binary form must reproduce the above copyright 12 | notice, this list of conditions and the following disclaimer in the 13 | documentation and/or other materials provided with the distribution. 14 | * Neither the name of ETHZ-ASL nor the 15 | names of its contributors may be used to endorse or promote products 16 | derived from this software without specific prior written permission. 17 | 18 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 19 | ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 20 | WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 21 | DISCLAIMED. IN NO EVENT SHALL ETHZ-ASL BE LIABLE FOR ANY 22 | DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 23 | (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 24 | LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 25 | ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 26 | (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 27 | SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 28 | */ 29 | 30 | #include "volumetric_map_base/world_base.h" 31 | 32 | #include 33 | #include 34 | #include 35 | #include 36 | #include 37 | #include 38 | 39 | namespace volumetric_mapping { 40 | 41 | void WorldBase::insertDisparityImage( 42 | const Transformation& sensor_to_world, 43 | const stereo_msgs::DisparityImageConstPtr& disparity, 44 | const Eigen::Matrix4d& Q_full, const Eigen::Vector2d& full_image_size) { 45 | cv_bridge::CvImageConstPtr cv_img_ptr = 46 | cv_bridge::toCvShare(disparity->image, disparity); 47 | insertDisparityImage(sensor_to_world, cv_img_ptr->image, Q_full, 48 | full_image_size); 49 | } 50 | 51 | void WorldBase::insertDisparityImage(const Transformation& sensor_to_world, 52 | const cv::Mat& disparity, 53 | const Eigen::Matrix4d& Q_full, 54 | const Eigen::Vector2d& full_image_size) { 55 | // Figure out the downsampling of the image. 56 | double downsampling_factor = full_image_size.x() / disparity.cols; 57 | Eigen::Matrix4d Q = Q_full; 58 | if (fabs(downsampling_factor - 1.0) > 1e-6) { 59 | // c{x,y} and f{x,y} are scaled by the downsampling factor then. 60 | const double downsampling_squared = 61 | downsampling_factor * downsampling_factor; 62 | Q(0, 0) /= downsampling_factor; 63 | Q(0, 3) /= downsampling_squared; 64 | Q(1, 1) /= downsampling_factor; 65 | Q(1, 3) /= downsampling_squared; 66 | Q(2, 3) /= downsampling_squared; 67 | Q(3, 2) /= downsampling_factor; 68 | Q(3, 3) /= downsampling_factor; 69 | } 70 | 71 | cv::Mat reprojected_disparities(disparity.size(), CV_32FC3); 72 | cv::Mat Q_cv; 73 | cv::eigen2cv(Q, Q_cv); 74 | 75 | cv::reprojectImageTo3D(disparity, reprojected_disparities, Q_cv, true); 76 | 77 | // Call the implementation function of the inheriting class. 78 | if (!isPointWeighingSet()) { 79 | insertProjectedDisparityIntoMapImpl(sensor_to_world, 80 | reprojected_disparities); 81 | } else { 82 | cv::Mat weights; 83 | computeWeights(disparity, &weights); 84 | insertProjectedDisparityIntoMapWithWeightsImpl( 85 | sensor_to_world, reprojected_disparities, weights); 86 | } 87 | } 88 | 89 | // Helper functions to compute the Q matrix for given UNRECTIFIED camera 90 | // parameters. 91 | // Assumes 0 distortion. 92 | Eigen::Matrix4d WorldBase::getQForCameras( 93 | const Transformation& T_C1_C0, const Eigen::Matrix3d& left_cam_matrix, 94 | const Eigen::Matrix3d& right_cam_matrix, 95 | const Eigen::Vector2d& full_image_size) const { 96 | // So unfortunately... Have to actually stereo rectify this pair. 97 | // Input matrices: C = camera matrix (3x3), D = disortion coefficients (5x1), 98 | // R = rotation matrix between cameras (3x3), T = translation between cameras 99 | // (3x1). 100 | cv::Mat C0, D0, C1, D1, R, T; 101 | // For now, ignore distortion coefficients. 102 | Eigen::Matrix distortion = Eigen::Matrix::Zero(); 103 | 104 | // Output parameters. 105 | cv::Mat R0, R1, P0, P1, Q_cv; 106 | 107 | cv::eigen2cv(left_cam_matrix, C0); 108 | cv::eigen2cv(right_cam_matrix, C1); 109 | cv::eigen2cv(distortion, D0); 110 | cv::eigen2cv(distortion, D1); 111 | cv::eigen2cv(T_C1_C0.getRotationMatrix(), R); 112 | cv::eigen2cv(T_C1_C0.getPosition(), T); 113 | cv::Size img_size(full_image_size.x(), full_image_size.y()); 114 | 115 | cv::Rect roi1, roi2; 116 | // Stereo rectify: 117 | cv::stereoRectify(C0, D0, C1, D1, img_size, R, T, R0, R1, P0, P1, Q_cv, 118 | cv::CALIB_ZERO_DISPARITY, 0, img_size, &roi1, &roi2); 119 | 120 | Eigen::Matrix4d Q; 121 | cv::cv2eigen(Q_cv, Q); 122 | return Q; 123 | } 124 | 125 | Eigen::Matrix4d WorldBase::getQForROSCameras( 126 | const sensor_msgs::CameraInfo& left_camera, 127 | const sensor_msgs::CameraInfo& right_camera) const { 128 | // Unfortunately updateQ is protected in StereoCameraModel. 129 | image_geometry::StereoCameraModel stereo_model; 130 | stereo_model.fromCameraInfo(left_camera, right_camera); 131 | 132 | double Tx = -stereo_model.baseline(); 133 | double left_cx = stereo_model.left().cx(); 134 | double left_cy = stereo_model.left().cy(); 135 | double left_fx = stereo_model.left().fx(); 136 | double left_fy = stereo_model.left().fy(); 137 | double right_cx = stereo_model.right().cx(); 138 | double right_cy = stereo_model.right().cy(); 139 | double right_fx = stereo_model.right().fx(); 140 | double right_fy = stereo_model.right().fy(); 141 | 142 | return generateQ(Tx, left_cx, left_cy, left_fx, left_fy, right_cx, right_cy, 143 | right_fx, right_fy); 144 | } 145 | 146 | Eigen::Matrix4d WorldBase::generateQ(double Tx, double left_cx, double left_cy, 147 | double left_fx, double left_fy, 148 | double right_cx, double right_cy, 149 | double right_fx, double right_fy) const { 150 | Eigen::Matrix4d Q = Eigen::Matrix4d::Zero(); 151 | 152 | // Basically do the same that the stereo model does by hand: 153 | // See: https://github.com/ros-perception/vision_opencv/blob/indigo/ 154 | // image_geometry/src/stereo_camera_model.cpp#L53 155 | // 156 | // From Springer Handbook of Robotics, p. 524: 157 | // [x y z 1]^T = Q * [u v d 1]^T 158 | // Where Q is defined as 159 | // Q = [ FyTx 0 0 -FyCxTx ] 160 | // [ 0 FxTx 0 -FxCyTx ] 161 | // [ 0 0 0 FxFyTx ] 162 | // [ 0 0 -Fy Fy(Cx-Cx') ] 163 | // where primed parameters are from the left projection matrix, 164 | // unprimed from the right. 165 | // Helen's note: their actual implementation uses the left cam as unprimed, 166 | // which should make more sense since we always have left disparities anyway. 167 | // Disparity = x_left - x_right 168 | 169 | Q(0, 0) = left_fy * Tx; 170 | Q(0, 3) = -left_fy * left_cx * Tx; 171 | Q(1, 1) = left_fx * Tx; 172 | Q(1, 3) = -left_fx * left_cy * Tx; 173 | Q(2, 3) = left_fx * left_fy * Tx; 174 | Q(3, 2) = -left_fy; 175 | // Zero when disparities are pre-adjusted for the difference in projection 176 | // centers of the 2 cameras. 177 | Q(3, 3) = left_fy * (left_cx - right_cx); 178 | 179 | return Q; 180 | } 181 | 182 | void WorldBase::insertPointcloud( 183 | const Transformation& T_G_sensor, 184 | const sensor_msgs::PointCloud2::ConstPtr& pointcloud_sensor) { 185 | pcl::PointCloud::Ptr pointcloud_sensor_pcl( 186 | new pcl::PointCloud); 187 | pcl::fromROSMsg(*pointcloud_sensor, *pointcloud_sensor_pcl); 188 | insertPointcloud(T_G_sensor, pointcloud_sensor_pcl); 189 | } 190 | 191 | // TODO(tcies) Make the virtual function insertPointcloudIntoMapImpl take a 192 | // signature like this so that the transformation logic doesn't need to be 193 | // repeated in all derived classes. 194 | void WorldBase::insertPointcloud(const Transformation& T_G_sensor, 195 | const Eigen::Matrix3Xd& pointcloud_sensor) { 196 | pcl::PointCloud::Ptr pointcloud_sensor_pcl( 197 | new pcl::PointCloud(pointcloud_sensor.cols(), 1)); 198 | pointcloud_sensor_pcl->getMatrixXfMap() = pointcloud_sensor.cast(); 199 | insertPointcloud(T_G_sensor, pointcloud_sensor_pcl); 200 | } 201 | 202 | void WorldBase::insertPointcloud( 203 | const Transformation& T_G_sensor, 204 | const pcl::PointCloud::Ptr& pointcloud_sensor) { 205 | if (!isPointWeighingSet()) { 206 | insertPointcloudIntoMapImpl(T_G_sensor, pointcloud_sensor); 207 | } else { 208 | std::vector weights; 209 | computeWeights(pointcloud_sensor, &weights); 210 | insertPointcloudIntoMapWithWeightsImpl(T_G_sensor, pointcloud_sensor, 211 | weights); 212 | } 213 | } 214 | 215 | void WorldBase::computeWeights(const cv::Mat& disparity, 216 | cv::Mat* weights) const { 217 | *weights = cv::Mat::ones(disparity.rows, disparity.cols, CV_32F); 218 | 219 | if (!point_weighing_) { 220 | return; 221 | } 222 | 223 | for (int v = 0; v < disparity.rows; ++v) { 224 | // Disparity is 32f. 225 | const float* row_pointer = disparity.ptr(v); 226 | for (int u = 0; u < disparity.cols; ++u) { 227 | weights->at(v, u) = 228 | point_weighing_->computeWeightForDisparity(u, v, row_pointer[u]); 229 | } 230 | } 231 | } 232 | 233 | void WorldBase::computeWeights(const pcl::PointCloud::Ptr& cloud, 234 | std::vector* weights) const { 235 | weights->resize(cloud->size(), 1.0); 236 | 237 | if (!point_weighing_) { 238 | return; 239 | } 240 | 241 | unsigned int index = 0; 242 | for (pcl::PointCloud::const_iterator it = cloud->begin(); 243 | it != cloud->end(); ++it) { 244 | (*weights)[index] = 245 | point_weighing_->computeWeightForPoint(it->x, it->y, it->z); 246 | index++; 247 | } 248 | } 249 | 250 | } // namespace volumetric_mapping 251 | -------------------------------------------------------------------------------- /volumetric_mapping/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(volumetric_mapping) 3 | find_package(catkin REQUIRED) 4 | catkin_metapackage() 5 | -------------------------------------------------------------------------------- /volumetric_mapping/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | volumetric_mapping 4 | 0.0.0 5 | Stack for volumetric mapping implementations to use with path planning. 6 | Helen Oleynikova 7 | 8 | BSD 9 | 10 | catkin 11 | 12 | octomap_world 13 | volumetric_map_base 14 | volumetric_msgs 15 | minkindr 16 | 17 | 18 | 19 | 20 | 21 | -------------------------------------------------------------------------------- /volumetric_msgs/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(volumetric_msgs) 3 | 4 | find_package(catkin_simple REQUIRED) 5 | # Catkin simple also generates messages! 6 | catkin_simple(ALL_DEPS_REQUIRED) 7 | 8 | ########## 9 | # EXPORT # 10 | ########## 11 | cs_install() 12 | cs_export() 13 | -------------------------------------------------------------------------------- /volumetric_msgs/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | volumetric_msgs 4 | 0.0.0 5 | Messages and services for volumetric mapping. 6 | 7 | Helen Oleynikova 8 | Markus Achtelik 9 | 10 | Helen Oleynikova 11 | 12 | BSD 13 | 14 | catkin 15 | catkin_simple 16 | 17 | message_generation 18 | message_runtime 19 | std_msgs 20 | geometry_msgs 21 | 22 | -------------------------------------------------------------------------------- /volumetric_msgs/srv/GetChangedPoints.srv: -------------------------------------------------------------------------------- 1 | --- 2 | int32 size 3 | geometry_msgs/Vector3[] changed_points 4 | bool[] changed_states 5 | -------------------------------------------------------------------------------- /volumetric_msgs/srv/LoadMap.srv: -------------------------------------------------------------------------------- 1 | string file_path 2 | --- 3 | -------------------------------------------------------------------------------- /volumetric_msgs/srv/SaveMap.srv: -------------------------------------------------------------------------------- 1 | string file_path 2 | --- 3 | -------------------------------------------------------------------------------- /volumetric_msgs/srv/SetBoxOccupancy.srv: -------------------------------------------------------------------------------- 1 | # Message to make it easy to clear or set the occupancy of a bounding box. 2 | geometry_msgs/Vector3 box_center 3 | geometry_msgs/Vector3 box_size 4 | bool set_occupied # Whether to set box free (false) or set occupied (true). 5 | --- 6 | -------------------------------------------------------------------------------- /volumetric_msgs/srv/SetDisplayBounds.srv: -------------------------------------------------------------------------------- 1 | float64 min_z 2 | float64 max_z 3 | --- 4 | --------------------------------------------------------------------------------