├── .gitignore ├── CMakeLists.txt ├── LICENSE ├── README.md ├── demo.rviz ├── figure └── shortIntroduction.png ├── include ├── ESDFMap.h ├── Fiesta.h ├── parameters.h ├── raycast.h └── timing.h ├── launch ├── cow_and_lady.launch └── demo.launch ├── package.xml ├── src ├── ESDFMap.cpp ├── parameters.cpp ├── raycast.cpp └── timing.cc └── test ├── test_ESDF_Map.cpp └── test_fiesta.cpp /.gitignore: -------------------------------------------------------------------------------- 1 | cmake-build-debug 2 | cmake-build-releasei 3 | .idea 4 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(fiesta) 3 | 4 | find_package(catkin REQUIRED COMPONENTS 5 | roscpp 6 | std_msgs 7 | nav_msgs 8 | visualization_msgs 9 | tf 10 | cv_bridge 11 | ) 12 | 13 | find_package(Eigen3 REQUIRED) 14 | find_package(PCL 1.7 REQUIRED) 15 | find_package(OpenCV REQUIRED) 16 | set(Eigen3_INCLUDE_DIRS ${EIGEN3_INCLUDE_DIR}) 17 | 18 | catkin_package( 19 | INCLUDE_DIRS include 20 | ) 21 | 22 | 23 | include_directories(${PROJECT_SOURCE_DIR}/include) 24 | 25 | include_directories( 26 | include 27 | SYSTEM 28 | ${catkin_INCLUDE_DIRS} 29 | ${Eigen3_INCLUDE_DIRS} 30 | ${PCL_INCLUDE_DIRS} 31 | ${OpenCV_INCLUDE_DIRS} 32 | ) 33 | 34 | link_directories(${PCL_LIBRARY_DIRS}) 35 | 36 | set(CMAKE_CXX_FLAGS "-std=c++17 ${CMAKE_CXX_FLAGS} -O3 -Wall -pthread") # -Wextra -Werror 37 | set(CMAKE_BUILD_TYPE "RELEASE") 38 | #add_executable(test_esdf_map 39 | # include/ESDFMap.h 40 | # src/ESDFMap.cpp 41 | # include/timing.h 42 | # src/timing.cc 43 | # test/test_ESDF_Map.cpp 44 | # ) 45 | #target_link_libraries(test_esdf_map 46 | # ${catkin_LIBRARIES} 47 | # ) 48 | #FIND_PACKAGE( OpenMP REQUIRED) 49 | #if(OPENMP_FOUND) 50 | #message("OPENMP FOUND") 51 | #set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${OpenMP_C_FLAGS}") 52 | #set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS}") 53 | #set(CMAKE_EXE_LINKER_FLAGS "${CMAKE_EXE_LINKER_FLAGS} ${OpenMP_EXE_LINKER_FLAGS}") 54 | #endif() 55 | 56 | add_executable(test_fiesta 57 | include/parameters.h 58 | src/parameters.cpp 59 | include/timing.h 60 | src/timing.cc 61 | include/raycast.h 62 | src/raycast.cpp 63 | include/ESDFMap.h 64 | src/ESDFMap.cpp 65 | include/Fiesta.h 66 | test/test_fiesta.cpp 67 | ) 68 | target_link_libraries(test_fiesta 69 | ${catkin_LIBRARIES} 70 | ${PCL_LIBRARIES} 71 | ${OpenCV_LIBS} 72 | ) 73 | 74 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2019 hlx1996 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Fiesta 2 | > Incremental ESDF Map for motion planning 3 | 4 | Euclidean Signed Distance Field (ESDF) is useful for online motion planning of aerial robots 5 | since it can easily query the distance_ and gradient information against obstacles. 6 | Fast incrementally built ESDF map is the bottleneck for conducting real-time motion planning. 7 | In this paper, we investigate this problem and propose a mapping system called *Fiesta* to build 8 | global ESDF map incrementally. By introducing two independent updating queues for inserting and 9 | deleting obstacles separately, and using Indexing Data Structures and Doubly Linked Lists for 10 | map maintenance, our algorithm updates as few as possible nodes using a BFS framework. Our ESDF 11 | map has high computational performance and produces near-optimal results. 12 | We show our method outperforms other up-to-date methods in term of performance and accuracy 13 | by both theory and experiments. We integrate Fiesta into a completed quadrotor system and validate 14 | it by both simulation and onboard experiments. We release our method as open-source software for the community. 15 | 16 | The paper of this method is submitted to the 2019 IEEE/RSJ International Conference on 17 | Intelligent Robots and Systems (IROS 2019). The draft is shown on arxiv 18 | [here](https://arxiv.org/abs/1903.02144). 19 | 20 |

21 | Fiesta short introduction video 24 |

25 | 26 | ## Installation 27 | ### Required Library 28 | - Eigen3 29 | - PCL 1.7 30 | - OpenCV 31 | - ROS 32 | 33 | Note: C++ 17 is required to run FIESTA. Please install and configure C++ 17. 34 | ```bash 35 | sudo add-apt-repository ppa:ubuntu-toolchain-r/test 36 | sudo apt-get update 37 | sudo apt-get install gcc-7 g++-7 38 | ``` 39 | 40 | use the following command to check your gcc compilers 41 | 42 | ``` 43 | $ ls -lh /usr/bin/g++* 44 | ``` 45 | 46 | for **ubuntu 14.04**: 47 | 48 | ``` 49 | $ sudo update-alternatives --install /usr/bin/gcc gcc /usr/bin/gcc-4.8 60 --slave /usr/bin/g++ g++ /usr/bin/g++-4.8 50 | ``` 51 | 52 | for **ubuntu 16.04**: 53 | ``` 54 | $ sudo update-alternatives --install /usr/bin/gcc gcc /usr/bin/gcc-5 60 --slave /usr/bin/g++ g++ /usr/bin/g++-5 55 | ``` 56 | 57 | then 58 | 59 | ``` 60 | $ sudo update-alternatives --install /usr/bin/gcc gcc /usr/bin/gcc-7 70 --slave /usr/bin/g++ g++ /usr/bin/g++-7 61 | ``` 62 | 63 | use the folling command to change the default compiler (choose 0 as default) 64 | 65 | ``` 66 | $ sudo update-alternatives --config gcc 67 | ``` 68 | 69 | check your current gcc compiler 70 | 71 | ``` 72 | $ gcc --version 73 | ``` 74 | 75 | 76 | 77 | ### Download & Compile 78 | ```sh 79 | cd ~/catkin_ws/src 80 | git clone https://github.com/hlx1996/Fiesta.git 81 | cd ../ 82 | catkin_make 83 | source ~/catkin_ws/devel/setup.bash 84 | ``` 85 | 86 | 87 | 88 | ## Usage example 89 | 90 | ```sh 91 | roslaunch fiesta cow_and_lady.launch 92 | rosbag play data.bag 93 | ``` 94 | 95 | Cow and lady data set can be downloaded [here](http://robotics.ethz.ch/~asl-datasets/iros_2017_voxblox/data.bag). 96 | A `rviz` will be opened with the visualization of occupancy grid map and a slice of esdf map. 97 | 98 | _For more examples, usage and FAQ, please refer to the [Wiki][wiki]. 99 | 100 | 101 | 102 | ## Release History 103 | Prediction: 1.0.0 will be our first elegant version. 104 | * 0.6.0 105 | * CHANGE: Refactor code according Google C++ Code Style 106 | * 0.5.3 107 | * CHANGE: Improve the performance of the depth conversion process 108 | * 0.5.2 109 | * ADD: Support local map 110 | * ADD: Support raw depth image as input, and depth filter of consistency 111 | * 0.5.1 112 | * CHANGE: Improve performance of raycasting a lot 113 | * 0.5.0 114 | * ADD: Give Local / global update / visualization options 115 | * ADD: Show Performance visualization directly on rviz 116 | * CHANGE: Deprecate older visualization, will be deleted in next release 117 | * 0.4.1 118 | * CHANGE: Give better Visualization based on a slice of ESDF and pointcloud of obstacles 119 | * 0.4.0 120 | * ADD: Support LIDAR Input 121 | * ADD: Support Deterministic Occupancy Grid Map 122 | * 0.3.2 123 | * ADD: Implement Multi-thread Raycasting for array implementation 124 | * 0.3.1 125 | * ADD: Implement Hash table with blocks 126 | * 0.3.0 127 | * ADD: Implement Hash table 128 | * 0.2.0 129 | * ADD: Support Raycasting from point cloud_ 130 | * ADD: Support Probabilistic Occupancy Grid Map Fusion 131 | * CHANGE: Patch code for limited observations 132 | * 0.1.0 133 | * The first proper release, support ESDF for fully dynamic case and implemented in array 134 | * 0.0.1 135 | * Support ESDF for insert-only case 136 | 137 | ## Meta 138 | 139 | [hlx1996](https://github.com/hlx1996/) – hlx1996@example.com 140 | 141 | Distributed under the MIT license. See ``LICENSE`` for more information. 142 | 143 | ## Contributing 144 | 145 | 1. Fork it () 146 | 2. Create your feature branch (`git checkout -b feature/fooBar`) 147 | 3. Commit your changes (`git commit -am 'Add some fooBar'`) 148 | 4. Push to the branch (`git push origin feature/fooBar`) 149 | 5. Create a new Pull Request 150 | 151 | 152 | [wiki]: https://github.com/hlx1996/Fiesta/wiki 153 | -------------------------------------------------------------------------------- /demo.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 78 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Global Options1 8 | - /Status1 9 | - /PointCloud1 10 | - /Marker1 11 | Splitter Ratio: 0.5 12 | Tree Height: 774 13 | - Class: rviz/Selection 14 | Name: Selection 15 | - Class: rviz/Tool Properties 16 | Expanded: 17 | - /2D Pose Estimate1 18 | - /2D Nav Goal1 19 | - /Publish Point1 20 | Name: Tool Properties 21 | Splitter Ratio: 0.588679016 22 | - Class: rviz/Views 23 | Expanded: 24 | - /Current View1 25 | - /Current View1/Focal Point1 26 | Name: Views 27 | Splitter Ratio: 0.5 28 | - Class: rviz/Time 29 | Experimental: false 30 | Name: Time 31 | SyncMode: 0 32 | SyncSource: PointCloud 33 | Toolbars: 34 | toolButtonStyle: 2 35 | Visualization Manager: 36 | Class: "" 37 | Displays: 38 | - Alpha: 0.5 39 | Cell Size: 1 40 | Class: rviz/Grid 41 | Color: 160; 160; 164 42 | Enabled: true 43 | Line Style: 44 | Line Width: 0.0299999993 45 | Value: Lines 46 | Name: Grid 47 | Normal Cell Count: 0 48 | Offset: 49 | X: 0 50 | Y: 0 51 | Z: 0 52 | Plane: XY 53 | Plane Cell Count: 10 54 | Reference Frame: 55 | Value: true 56 | - Alpha: 1 57 | Autocompute Intensity Bounds: true 58 | Autocompute Value Bounds: 59 | Max Value: 2.9749999 60 | Min Value: -0.275000006 61 | Value: true 62 | Axis: Z 63 | Channel Name: intensity 64 | Class: rviz/PointCloud 65 | Color: 255; 255; 255 66 | Color Transformer: AxisColor 67 | Decay Time: 0 68 | Enabled: true 69 | Invert Rainbow: false 70 | Max Color: 255; 255; 255 71 | Max Intensity: 4096 72 | Min Color: 0; 0; 0 73 | Min Intensity: 0 74 | Name: PointCloud 75 | Position Transformer: XYZ 76 | Queue Size: 10 77 | Selectable: true 78 | Size (Pixels): 3 79 | Size (m): 0.0500000007 80 | Style: Squares 81 | Topic: /fiesta/ESDFMap/occ_pc 82 | Unreliable: false 83 | Use Fixed Frame: true 84 | Use rainbow: true 85 | Value: true 86 | - Class: rviz/Marker 87 | Enabled: true 88 | Marker Topic: /fiesta/ESDFMap/slice 89 | Name: Marker 90 | Namespaces: 91 | "": true 92 | Queue Size: 100 93 | Value: true 94 | Enabled: true 95 | Global Options: 96 | Background Color: 255; 255; 127 97 | Default Light: true 98 | Fixed Frame: world 99 | Frame Rate: 30 100 | Name: root 101 | Tools: 102 | - Class: rviz/Interact 103 | Hide Inactive Objects: true 104 | - Class: rviz/MoveCamera 105 | - Class: rviz/Select 106 | - Class: rviz/FocusCamera 107 | - Class: rviz/Measure 108 | - Class: rviz/SetInitialPose 109 | Topic: /initialpose 110 | - Class: rviz/SetGoal 111 | Topic: /move_base_simple/goal 112 | - Class: rviz/PublishPoint 113 | Single click: true 114 | Topic: /clicked_point 115 | Value: true 116 | Views: 117 | Current: 118 | Class: rviz/Orbit 119 | Distance: 10.7248745 120 | Enable Stereo Rendering: 121 | Stereo Eye Separation: 0.0599999987 122 | Stereo Focal Distance: 1 123 | Swap Stereo Eyes: false 124 | Value: false 125 | Focal Point: 126 | X: 1.16496587 127 | Y: 0.201870233 128 | Z: 0.447906345 129 | Focal Shape Fixed Size: true 130 | Focal Shape Size: 0.0500000007 131 | Invert Z Axis: false 132 | Name: Current View 133 | Near Clip Distance: 0.00999999978 134 | Pitch: 0.274794996 135 | Target Frame: 136 | Value: Orbit (rviz) 137 | Yaw: 3.01372385 138 | Saved: ~ 139 | Window Geometry: 140 | Displays: 141 | collapsed: false 142 | Height: 1056 143 | Hide Left Dock: false 144 | Hide Right Dock: true 145 | QMainWindow State: 000000ff00000000fd00000004000000000000016a00000395fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006100fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000002800000395000000d700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000396fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000002800000396000000ad00fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007800000003ffc0100000002fb0000000800540069006d00650100000000000007800000030000fffffffb0000000800540069006d00650100000000000004500000000000000000000006100000039500000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 146 | Selection: 147 | collapsed: false 148 | Time: 149 | collapsed: false 150 | Tool Properties: 151 | collapsed: false 152 | Views: 153 | collapsed: true 154 | Width: 1920 155 | X: 0 156 | Y: 24 157 | -------------------------------------------------------------------------------- /figure/shortIntroduction.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/HKUST-Aerial-Robotics/FIESTA/d01ce1b4602340a417a68ec7bb5f6b5a6790207e/figure/shortIntroduction.png -------------------------------------------------------------------------------- /include/ESDFMap.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by tommy on 12/17/18. 3 | // 4 | 5 | #ifndef ESDF_MAP_H 6 | #define ESDF_MAP_H 7 | 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include "parameters.h" 18 | 19 | namespace fiesta { 20 | #ifdef HASH_TABLE 21 | // Eigen::Matrix hashing function 22 | template 23 | struct MatrixHash : std::unary_function { 24 | std::size_t operator()(transform_ const &matrix) const { 25 | // Note that it is oblivious to the storage order of Eigen matrix (column- or 26 | // row-major). It will give you the same hash value for two different matrices if they 27 | // are the transpose of each other in different storage order. 28 | size_t seed = 0; 29 | for (size_t i = 0; i < matrix.size(); ++i) { 30 | auto elem = *(matrix.data() + i); 31 | seed ^= std::hash()(elem) + 0x9e3779b9 + (seed << 6) + (seed >> 2); 32 | } 33 | return seed; 34 | } 35 | }; 36 | #endif 37 | class ESDFMap { 38 | // Type of queue element to be used in priority queue 39 | struct QueueElement { 40 | Eigen::Vector3i point_; 41 | double distance_; 42 | bool operator<(const QueueElement &element) const { 43 | return distance_ > element.distance_; 44 | } 45 | }; 46 | 47 | 48 | private: 49 | // parameters & method for occupancy information updating 50 | double prob_hit_log_, prob_miss_log_, clamp_min_log_, clamp_max_log_, min_occupancy_log_; 51 | const double Logit(const double &x) const; 52 | bool Exist(const int &idx); 53 | double Dist(Eigen::Vector3i a, Eigen::Vector3i b); 54 | 55 | // parameters & methods for conversion between Pos, Vox & Idx 56 | bool PosInMap(Eigen::Vector3d pos); 57 | bool VoxInRange(Eigen::Vector3i vox, bool current_vec = true); 58 | void Vox2Pos(Eigen::Vector3i vox, Eigen::Vector3d &pos); 59 | int Vox2Idx(Eigen::Vector3i vox); 60 | int Vox2Idx(Eigen::Vector3i vox, int sub_sampling_factor); 61 | void Pos2Vox(Eigen::Vector3d pos, Eigen::Vector3i &vox); 62 | Eigen::Vector3i Idx2Vox(int idx); 63 | 64 | // HASH TABLE related 65 | #ifdef HASH_TABLE 66 | // Increase the capacity from old_size to new_size, and change the old_size to new_size. 67 | void IncreaseCapacity(int &old_size, int new_size); 68 | int FindAndInsert(Eigen::Vector3i hash_key); 69 | std::unordered_map> hash_table_; 70 | int count, reserve_size_; 71 | #ifdef BLOCK 72 | int block_, block_size_, block_bit_; 73 | #endif 74 | std::vector vox_buffer_; 75 | #else // #ifndef HASH_TABLE 76 | Eigen::Vector3d map_size_; 77 | Eigen::Vector3d min_range_, max_range_; // map range in pos 78 | Eigen::Vector3i grid_size_; // map range in index 79 | int grid_size_yz_; 80 | #endif 81 | 82 | // data are saved in vector 83 | #ifdef PROBABILISTIC 84 | std::vector occupancy_buffer_; // 0 is free, 1 is occupied 85 | #else 86 | std::vector occupancy_buffer_; // 0 is free, 1 is occupied 87 | #endif 88 | std::vector distance_buffer_; 89 | std::vector num_hit_, num_miss_; 90 | std::vector closest_obstacle_; 91 | std::vector head_, prev_, next_; 92 | 93 | std::queue insert_queue_; 94 | std::queue delete_queue_; 95 | std::queue update_queue_; 96 | std::queue occupancy_queue_; 97 | 98 | // Map Properties 99 | Eigen::Vector3d origin_; 100 | int reserved_idx_4_undefined_; 101 | int total_time_ = 0; 102 | int infinity_, undefined_; 103 | double resolution_, resolution_inv_; 104 | Eigen::Vector3i max_vec_, min_vec_, last_max_vec_, last_min_vec_; 105 | 106 | 107 | // DLL Operations 108 | void DeleteFromList(int link, int idx); 109 | void InsertIntoList(int link, int idx); 110 | 111 | public: 112 | #ifdef HASH_TABLE 113 | ESDFMap(Eigen::Vector3d origin, double resolution, int reserve_size = 0); 114 | #else 115 | int grid_total_size_; 116 | ESDFMap(Eigen::Vector3d origin, double resolution, Eigen::Vector3d map_size); 117 | #endif 118 | 119 | ~ESDFMap() { 120 | //TODO: implement this 121 | } 122 | 123 | #ifdef PROBABILISTIC 124 | void SetParameters(double p_hit, double p_miss, double p_min, double p_max, double p_occ); 125 | #endif 126 | 127 | 128 | bool CheckUpdate(); 129 | bool UpdateOccupancy(bool global_map); 130 | void UpdateESDF(); 131 | 132 | // Occupancy Management 133 | int SetOccupancy(Eigen::Vector3d pos, int occ); 134 | int SetOccupancy(Eigen::Vector3i vox, int occ); 135 | int GetOccupancy(Eigen::Vector3d pos); 136 | int GetOccupancy(Eigen::Vector3i pos_id); 137 | 138 | // Distance Field Management 139 | double GetDistance(Eigen::Vector3d pos); 140 | double GetDistance(Eigen::Vector3i vox); 141 | double GetDistWithGradTrilinear(Eigen::Vector3d pos, Eigen::Vector3d &grad); 142 | 143 | // Visualization 144 | void GetPointCloud(sensor_msgs::PointCloud &m, int vis_lower_bound, int vis_upper_bound); 145 | void GetSliceMarker(visualization_msgs::Marker &m, int slice, int id, Eigen::Vector4d color, double max_dist); 146 | 147 | // Local Range 148 | void SetUpdateRange(Eigen::Vector3d min_pos, Eigen::Vector3d max_pos, bool new_vec = true); 149 | void SetOriginalRange(); 150 | 151 | #ifndef PROBABILISTIC 152 | // For Deterministic Occupancy Grid 153 | void SetAway(); 154 | void SetAway(Eigen::Vector3i left, Eigen::Vector3i right); 155 | void SetBack(); 156 | void SetBack(Eigen::Vector3i left, Eigen::Vector3i right); 157 | #endif 158 | 159 | #ifdef DEBUG 160 | // only for test, check whether consistent 161 | bool CheckConsistency(); 162 | // only for test, check between Ground Truth calculated by k-d tree 163 | bool CheckWithGroundTruth(); 164 | #endif 165 | 166 | }; 167 | } 168 | 169 | #endif //ESDF_MAP_H 170 | -------------------------------------------------------------------------------- /include/Fiesta.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by tommy on 4/25/19. 3 | // 4 | 5 | #ifndef ESDF_TOOLS_INCLUDE_FIESTA_H_ 6 | #define ESDF_TOOLS_INCLUDE_FIESTA_H_ 7 | #include "ESDFMap.h" 8 | #include "raycast.h" 9 | #include "timing.h" 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | #include 24 | #include 25 | #include 26 | namespace fiesta { 27 | 28 | // sensor_msgs::PointCloud2::ConstPtr 29 | // sensor_msgs::Image::ConstPtr 30 | 31 | // geometry_msgs::PoseStamped::ConstPtr 32 | // nav_msgs::Odometry::ConstPtr 33 | // geometry_msgs::TransformStamped::ConstPtr 34 | template 35 | class Fiesta { 36 | private: 37 | Parameters parameters_; 38 | ESDFMap *esdf_map_; 39 | #ifdef SIGNED_NEEDED 40 | ESDFMap *inv_esdf_map_; 41 | #endif 42 | bool new_msg_ = false; 43 | pcl::PointCloud cloud_; 44 | #ifndef PROBABILISTIC 45 | sensor_msgs::PointCloud2::ConstPtr sync_pc_; 46 | #endif 47 | ros::Publisher slice_pub_, occupancy_pub_, text_pub_; 48 | ros::Subscriber transform_sub_, depth_sub_; 49 | ros::Timer update_mesh_timer_; 50 | Eigen::Vector3d sync_pos_, raycast_origin_, cur_pos_; 51 | Eigen::Quaterniond sync_q_; 52 | 53 | std::queue> transform_queue_; 54 | std::queue depth_queue_; 55 | DepthMsgType sync_depth_; 56 | 57 | cv::Mat img_[2]; 58 | Eigen::Matrix4d transform_, last_transform_; 59 | uint image_cnt_ = 0, esdf_cnt_ = 0, tot_ = 0; 60 | #ifdef HASH_TABLE 61 | std::unordered_set set_free_, set_occ_; 62 | #else 63 | std::vector set_free_, set_occ_; 64 | #endif 65 | void Visualization(ESDFMap *esdf_map, bool global_vis, const std::string &text); 66 | #ifdef PROBABILISTIC 67 | void RaycastProcess(int i, int part, int tt); 68 | void RaycastMultithread(); 69 | #endif 70 | 71 | double GetInterpolation(const cv::Mat &img, double u, double v); 72 | 73 | void DepthConversion(); 74 | 75 | void SynchronizationAndProcess(); 76 | 77 | void DepthCallback(const DepthMsgType &depth_map); 78 | 79 | void PoseCallback(const PoseMsgType &msg); 80 | 81 | void UpdateEsdfEvent(const ros::TimerEvent & /*event*/); 82 | public: 83 | Fiesta(ros::NodeHandle node); 84 | ~Fiesta(); 85 | }; 86 | 87 | template 88 | Fiesta::Fiesta(ros::NodeHandle node) { 89 | parameters_.SetParameters(node); 90 | #ifdef HASH_TABLE 91 | esdf_map_ = new ESDFMap(Eigen::Vector3d(0, 0, 0), parameters_.resolution_, parameters_.reserved_size_); 92 | #ifdef SIGNED_NEEDED 93 | inv_esdf_map_ = new ESDFMap(Eigen::Vector3d(0, 0, 0), parameters_.resolution_, parameters_.reserved_size_); 94 | #endif 95 | #else 96 | esdf_map_ = new ESDFMap(parameters_.l_cornor_, parameters_.resolution_, parameters_.map_size_); 97 | #ifdef SIGNED_NEEDED 98 | inv_esdf_map_ = new ESDFMap(parameters_.l_cornor_, parameters_.resolution_, parameters_.map_size_); 99 | #endif 100 | #endif 101 | 102 | #ifdef PROBABILISTIC 103 | esdf_map_->SetParameters(parameters_.p_hit_, parameters_.p_miss_, 104 | parameters_.p_min_, parameters_.p_max_, parameters_.p_occ_); 105 | #endif 106 | #ifndef HASH_TABLE 107 | set_free_.resize(esdf_map_->grid_total_size_); 108 | set_occ_.resize(esdf_map_->grid_total_size_); 109 | std::fill(set_free_.begin(), set_free_.end(), 0); 110 | std::fill(set_occ_.begin(), set_occ_.end(), 0); 111 | #endif 112 | // For Jie Bao 113 | // transform_sub_ = node.subscribe("/vins_estimator/camera_pose", 10, &Fiesta::PoseCallback, this); 114 | // depth_sub_ = node.subscribe("/camera/depth/image_rect_raw", 10, &Fiesta::DepthCallback, this); 115 | transform_sub_ = node.subscribe("transform", 10, &Fiesta::PoseCallback, this); 116 | depth_sub_ = node.subscribe("depth", 10, &Fiesta::DepthCallback, this); 117 | 118 | // Cow_and_Lady 119 | // depth_sub_ = node.subscribe("/camera/depth_registered/points", 1000, PointcloudCallback); 120 | // transform_sub_ = node.subscribe("/kinect/vrpn_client/estimated_transform", 1000, PoseCallback); 121 | 122 | //EuRoC 123 | // depth_sub_ = node.subscribe("/dense_stereo/pointcloud", 1000, PointcloudCallback); 124 | // transform_sub_ = node.subscribe("/vicon/firefly_sbx/firefly_sbx", 1000, PoseCallback); 125 | 126 | slice_pub_ = node.advertise("ESDFMap/slice", 1, true); 127 | occupancy_pub_ = node.advertise("ESDFMap/occ_pc", 1, true); 128 | text_pub_ = node.advertise("ESDFMap/text", 1, true); 129 | 130 | update_mesh_timer_ = 131 | node.createTimer(ros::Duration(parameters_.update_esdf_every_n_sec_), 132 | &Fiesta::UpdateEsdfEvent, this); 133 | } 134 | 135 | template 136 | Fiesta::~Fiesta() { 137 | delete esdf_map_; 138 | #ifdef SIGNED_NEEDED 139 | delete inv_esdf_map_; 140 | #endif 141 | } 142 | 143 | template 144 | void Fiesta::Visualization(ESDFMap *esdf_map, bool global_vis, const std::string &text) { 145 | if (esdf_map!=nullptr) { 146 | std::cout << "Visualization" << std::endl; 147 | if (global_vis) 148 | esdf_map->SetOriginalRange(); 149 | else 150 | esdf_map->SetUpdateRange(cur_pos_ - parameters_.radius_, cur_pos_ + parameters_.radius_, false); 151 | 152 | sensor_msgs::PointCloud pc; 153 | esdf_map->GetPointCloud(pc, parameters_.vis_lower_bound_, parameters_.vis_upper_bound_); 154 | occupancy_pub_.publish(pc); 155 | 156 | visualization_msgs::Marker slice_marker; 157 | esdf_map->GetSliceMarker(slice_marker, parameters_.slice_vis_level_, 100, 158 | Eigen::Vector4d(0, 1.0, 0, 1), parameters_.slice_vis_max_dist_); 159 | slice_pub_.publish(slice_marker); 160 | } 161 | if (!text.empty()) { 162 | visualization_msgs::Marker marker; 163 | marker.header.frame_id = "world"; 164 | marker.header.stamp = ros::Time::now(); 165 | marker.id = 3456; 166 | marker.type = visualization_msgs::Marker::TEXT_VIEW_FACING; 167 | marker.action = visualization_msgs::Marker::MODIFY; 168 | 169 | marker.pose.position.x = 8.0; 170 | marker.pose.position.y = 2.0; 171 | marker.pose.position.z = 3.0; 172 | marker.pose.orientation.x = 0.0; 173 | marker.pose.orientation.y = 0.0; 174 | marker.pose.orientation.z = 0.0; 175 | marker.pose.orientation.w = 1.0; 176 | 177 | marker.text = text; 178 | 179 | marker.scale.x = 0.3; 180 | marker.scale.y = 0.3; 181 | marker.scale.z = 0.6; 182 | 183 | marker.color.r = 0.0f; 184 | marker.color.g = 0.0f; 185 | marker.color.b = 1.0f; 186 | marker.color.a = 1.0f; 187 | text_pub_.publish(marker); 188 | } 189 | } 190 | 191 | #ifdef PROBABILISTIC 192 | 193 | template 194 | void Fiesta::RaycastProcess(int i, int part, int tt) { 195 | Eigen::Vector3d half = Eigen::Vector3d(0.5, 0.5, 0.5); 196 | for (int idx = part*i; idx < part*(i + 1); idx++) { 197 | std::vector output; 198 | if (idx > cloud_.points.size()) 199 | break; 200 | pcl::PointXYZ pt = cloud_.points[idx]; 201 | int cnt = 0; 202 | if (std::isnan(pt.x) || std::isnan(pt.y) || std::isnan(pt.z)) 203 | continue; 204 | Eigen::Vector4d tmp = transform_*Eigen::Vector4d(pt.x, pt.y, pt.z, 1); 205 | Eigen::Vector3d point = Eigen::Vector3d(tmp(0), tmp(1), tmp(2))/tmp(3); 206 | 207 | int tmp_idx; 208 | double length = (point - raycast_origin_).norm(); 209 | if (length < parameters_.min_ray_length_) 210 | continue; 211 | else if (length > parameters_.max_ray_length_) { 212 | point = (point - raycast_origin_)/length*parameters_.max_ray_length_ + raycast_origin_; 213 | tmp_idx = esdf_map_->SetOccupancy((Eigen::Vector3d) point, 0); 214 | } else 215 | tmp_idx = esdf_map_->SetOccupancy((Eigen::Vector3d) point, 1); 216 | #ifdef SIGNED_NEEDED 217 | tmp_idx = inv_esdf_map_->SetOccupancy((Eigen::Vector3d) point, 0); 218 | #endif 219 | // //TODO: -10000 ? 220 | 221 | if (tmp_idx!=-10000) { 222 | #ifdef HASH_TABLE 223 | if (set_occ_.find(tmp_idx) != set_occ_.end()) 224 | continue; 225 | else set_occ_.insert(tmp_idx); 226 | #else 227 | if (set_occ_[tmp_idx]==tt) 228 | continue; 229 | else 230 | set_occ_[tmp_idx] = tt; 231 | #endif 232 | } 233 | Raycast(raycast_origin_/parameters_.resolution_, 234 | point/parameters_.resolution_, 235 | parameters_.l_cornor_/parameters_.resolution_, 236 | parameters_.r_cornor_/parameters_.resolution_, 237 | &output); 238 | 239 | for (int i = output.size() - 2; i >= 0; i--) { 240 | Eigen::Vector3d tmp = (output[i] + half)*parameters_.resolution_; 241 | 242 | length = (tmp - raycast_origin_).norm(); 243 | if (length < parameters_.min_ray_length_) 244 | break; 245 | if (length > parameters_.max_ray_length_) 246 | continue; 247 | int tmp_idx; 248 | tmp_idx = esdf_map_->SetOccupancy(tmp, 0); 249 | #ifdef SIGNED_NEEDED 250 | tmp_idx = inv_esdf_map_->SetOccupancy(tmp, 1); 251 | #endif 252 | //TODO: -10000 ? 253 | if (tmp_idx!=-10000) { 254 | #ifdef HASH_TABLE 255 | if (set_free_.find(tmp_idx) != set_free_.end()) { 256 | if (++cnt >= 1) { 257 | cnt = 0; 258 | break; 259 | } 260 | } else { 261 | set_free_.insert(tmp_idx); 262 | cnt = 0; 263 | } 264 | #else 265 | if (set_free_[tmp_idx]==tt) { 266 | if (++cnt >= 1) { 267 | cnt = 0; 268 | break; 269 | } 270 | } else { 271 | set_free_[tmp_idx] = tt; 272 | cnt = 0; 273 | } 274 | #endif 275 | } 276 | } 277 | } 278 | } 279 | 280 | template 281 | void Fiesta::RaycastMultithread() { 282 | // TODO: when using vector, this is not needed 283 | #ifdef HASH_TABLE 284 | set_free_.clear(); 285 | set_occ_.clear(); 286 | #endif 287 | int tt = ++tot_; 288 | timing::Timer raycastingTimer("raycasting"); 289 | 290 | if (parameters_.ray_cast_num_thread_==0) { 291 | RaycastProcess(0, cloud_.points.size(), tt); 292 | } else { 293 | int part = cloud_.points.size()/parameters_.ray_cast_num_thread_; 294 | std::list integration_threads = std::list(); 295 | for (size_t i = 0; i < parameters_.ray_cast_num_thread_; ++i) { 296 | integration_threads.emplace_back(&Fiesta::RaycastProcess, this, i, part, tt); 297 | } 298 | for (std::thread &thread : integration_threads) { 299 | thread.join(); 300 | } 301 | } 302 | raycastingTimer.Stop(); 303 | } 304 | 305 | #endif // PROBABILISTIC 306 | 307 | template 308 | double Fiesta::GetInterpolation(const cv::Mat &img, double u, double v) { 309 | int vu = img.at(v, u); 310 | int v1u = img.at(v + 1, u); 311 | int vu1 = img.at(v, u + 1); 312 | int v1u1 = img.at(v + 1, u + 1); 313 | float a = u - (float) u; 314 | float c = v - (float) v; 315 | return (vu*(1.f - a) + vu1*a)*(1.f - c) + (v1u*(1.f - a) + v1u1*a)*c; 316 | } 317 | 318 | template 319 | void Fiesta::DepthConversion() { 320 | timing::Timer depth_timer("depth"); 321 | ++image_cnt_; 322 | cv::Mat ¤t_img = img_[image_cnt_ & 1]; 323 | cv::Mat &last_img = img_[!(image_cnt_ & 1)]; 324 | 325 | cv_bridge::CvImagePtr cv_ptr; 326 | cv_ptr = cv_bridge::toCvCopy(depth_queue_.front(), depth_queue_.front()->encoding); 327 | // TODO: make it a parameter 328 | constexpr double k_depth_scaling_factor = 1000.0; 329 | if (depth_queue_.front()->encoding==sensor_msgs::image_encodings::TYPE_32FC1) { 330 | (cv_ptr->image).convertTo(cv_ptr->image, CV_16UC1, k_depth_scaling_factor); 331 | } 332 | cv_ptr->image.copyTo(current_img); 333 | 334 | double depth; 335 | cloud_.clear(); 336 | double uu, vv; 337 | 338 | uint16_t *row_ptr; 339 | int cols = current_img.cols, rows = current_img.rows; 340 | if (!parameters_.use_depth_filter_) { 341 | for (int v = 0; v < rows; v++) { 342 | row_ptr = current_img.ptr(v); 343 | for (int u = 0; u < cols; u++) { 344 | depth = (*row_ptr++)/k_depth_scaling_factor; 345 | pcl::PointXYZ point; 346 | point.x = (u - parameters_.center_x_)*depth/parameters_.focal_x_; 347 | point.y = (v - parameters_.center_y_)*depth/parameters_.focal_y_; 348 | point.z = depth; 349 | cloud_.push_back(point); 350 | } 351 | } 352 | } else { 353 | if (image_cnt_!=1) { 354 | Eigen::Vector4d coord_h; 355 | Eigen::Vector3d coord; 356 | for (int v = parameters_.depth_filter_margin_; v < rows - parameters_.depth_filter_margin_; v++) { 357 | row_ptr = current_img.ptr(v) + parameters_.depth_filter_margin_; 358 | for (int u = parameters_.depth_filter_margin_; u < cols - parameters_.depth_filter_margin_; u++) { 359 | depth = (*row_ptr++)/k_depth_scaling_factor; 360 | pcl::PointXYZ point; 361 | point.x = (u - parameters_.center_x_)*depth/parameters_.focal_x_; 362 | point.y = (v - parameters_.center_y_)*depth/parameters_.focal_y_; 363 | point.z = depth; 364 | if (depth > parameters_.depth_filter_max_dist_ || depth < parameters_.depth_filter_min_dist_) 365 | continue; 366 | coord_h = last_transform_.inverse()*transform_*Eigen::Vector4d(point.x, point.y, point.z, 1); 367 | coord = Eigen::Vector3d(coord_h(0), coord_h(1), coord_h(2))/coord_h(3); 368 | uu = coord.x()*parameters_.focal_x_/coord.z() + parameters_.center_x_; 369 | vv = coord.y()*parameters_.focal_y_/coord.z() + parameters_.center_y_; 370 | if (uu >= 0 && uu < cols && vv >= 0 && vv < rows) { 371 | // getInterpolation(last_img, uu, vv) 372 | if (fabs(last_img.at((int) vv, (int) uu)/k_depth_scaling_factor - coord.z()) 373 | < parameters_.depth_filter_tolerance_) { 374 | cloud_.push_back(point); 375 | } 376 | } //else cloud_.push_back(point_); 377 | } 378 | } 379 | } 380 | } 381 | depth_timer.Stop(); 382 | } 383 | 384 | template 385 | void Fiesta::SynchronizationAndProcess() { 386 | ros::Time depth_time; 387 | double time_delay = 3e-3; 388 | while (!depth_queue_.empty()) { 389 | bool new_pos = false; 390 | depth_time = depth_queue_.front()->header.stamp; 391 | while (transform_queue_.size() > 1 && 392 | std::get<0>(transform_queue_.front()) <= depth_time + ros::Duration(time_delay)) { 393 | sync_pos_ = std::get<1>(transform_queue_.front()); 394 | sync_q_ = std::get<2>(transform_queue_.front()); 395 | transform_queue_.pop(); 396 | new_pos = true; 397 | } 398 | if (transform_queue_.empty() 399 | || std::get<0>(transform_queue_.front()) <= depth_time + ros::Duration(time_delay)) { 400 | break; 401 | } 402 | if (!new_pos) { 403 | depth_queue_.pop(); 404 | continue; 405 | } 406 | 407 | new_msg_ = true; 408 | #ifndef PROBABILISTIC 409 | // TODO: sync_depth_ must be PointCloud2 410 | sync_depth_ = depth_queue_.front(); 411 | return; 412 | #else 413 | if (parameters_.use_depth_filter_) 414 | last_transform_ = transform_; 415 | transform_.block<3, 3>(0, 0) = sync_q_.toRotationMatrix(); 416 | transform_.block<3, 1>(0, 3) = sync_pos_; 417 | transform_(3, 0) = transform_(3, 1) = transform_(3, 2) = 0; 418 | transform_(3, 3) = 1; 419 | transform_ = transform_*parameters_.T_D_B_*parameters_.T_B_C_; 420 | raycast_origin_ = Eigen::Vector3d(transform_(0, 3), transform_(1, 3), transform_(2, 3))/transform_(3, 3); 421 | 422 | if constexpr(std::is_same::value) { 423 | DepthConversion(); 424 | } else if constexpr(std::is_same::value) { 425 | sensor_msgs::PointCloud2::ConstPtr tmp = depth_queue_.front(); 426 | pcl::fromROSMsg(*tmp, cloud_); 427 | } 428 | 429 | std::cout << "Pointcloud Size:\t" << cloud_.points.size() << std::endl; 430 | if ((int) cloud_.points.size()==0) { 431 | depth_queue_.pop(); 432 | continue; 433 | } 434 | 435 | RaycastMultithread(); 436 | depth_queue_.pop(); 437 | #endif 438 | } 439 | } 440 | 441 | template 442 | void Fiesta::PoseCallback(const PoseMsgType &msg) { 443 | Eigen::Vector3d pos; 444 | Eigen::Quaterniond q; 445 | 446 | if constexpr(std::is_same::value) { 447 | pos = Eigen::Vector3d(msg->pose.position.x, 448 | msg->pose.position.y, 449 | msg->pose.position.z); 450 | q = Eigen::Quaterniond(msg->pose.orientation.w, 451 | msg->pose.orientation.x, 452 | msg->pose.orientation.y, 453 | msg->pose.orientation.z); 454 | } else if constexpr(std::is_same::value) { 455 | pos = Eigen::Vector3d(msg->pose.pose.position.x, 456 | msg->pose.pose.position.y, 457 | msg->pose.pose.position.z); 458 | q = Eigen::Quaterniond(msg->pose.pose.orientation.w, 459 | msg->pose.pose.orientation.x, 460 | msg->pose.pose.orientation.y, 461 | msg->pose.pose.orientation.z); 462 | } else if constexpr(std::is_same::value) { 463 | pos = Eigen::Vector3d(msg->transform.translation.x, 464 | msg->transform.translation.y, 465 | msg->transform.translation.z); 466 | q = Eigen::Quaterniond(msg->transform.rotation.w, 467 | msg->transform.rotation.x, 468 | msg->transform.rotation.y, 469 | msg->transform.rotation.z); 470 | } 471 | 472 | transform_queue_.push(std::make_tuple(msg->header.stamp, pos, q)); 473 | } 474 | 475 | template 476 | void Fiesta::DepthCallback(const DepthMsgType &depth_map) { 477 | depth_queue_.push(depth_map); 478 | SynchronizationAndProcess(); 479 | } 480 | 481 | template 482 | void Fiesta::UpdateEsdfEvent(const ros::TimerEvent & /*event*/) { 483 | if (!new_msg_) 484 | return; 485 | new_msg_ = false; 486 | cur_pos_ = sync_pos_; 487 | 488 | #ifndef PROBABILISTIC 489 | timing::Timer handlePCTimer("handlePointCloud"); 490 | pcl::fromROSMsg(*sync_pc_, cloud_); 491 | 492 | esdf_map_->SetUpdateRange(cur_pos_ - parameters_.radius_, cur_pos_ + parameters_.radius_, false); 493 | esdf_map_->SetAway(); 494 | 495 | Eigen::Vector3i tmp_vox; 496 | Eigen::Vector3d tmp_pos; 497 | for (int i = 0; i < cloud_.size(); i++) { 498 | tmp_pos = Eigen::Vector3d(cloud_[i].x, cloud_[i].y, cloud_[i].z); 499 | esdf_map_->SetOccupancy(tmp_pos, 1); 500 | } 501 | esdf_map_->SetBack(); 502 | handlePCTimer.Stop(); 503 | #endif 504 | esdf_cnt_++; 505 | std::cout << "Running " << esdf_cnt_ << " updates." << std::endl; 506 | // ros::Time t1 = ros::Time::now(); 507 | if (esdf_map_->CheckUpdate()) { 508 | timing::Timer update_esdf_timer("UpdateESDF"); 509 | if (parameters_.global_update_) 510 | esdf_map_->SetOriginalRange(); 511 | else 512 | esdf_map_->SetUpdateRange(cur_pos_ - parameters_.radius_, cur_pos_ + parameters_.radius_); 513 | esdf_map_->UpdateOccupancy(parameters_.global_update_); 514 | esdf_map_->UpdateESDF(); 515 | #ifdef SIGNED_NEEDED 516 | // TODO: Complete this SIGNED_NEEDED 517 | inv_esdf_map_->UpdateOccupancy(); 518 | inv_esdf_map_->UpdateESDF(); 519 | #endif 520 | update_esdf_timer.Stop(); 521 | timing::Timing::Print(std::cout); 522 | } 523 | // ros::Time t2 = ros::Time::now(); 524 | 525 | // std::string text = "Fiesta\nCurrent update Time\n" 526 | // + timing::Timing::SecondsToTimeString((t2 - t1).toSec() * 1000) 527 | // + " ms\n" + "Average update Time\n" + 528 | // timing::Timing::SecondsToTimeString(timing::Timing::GetMeanSeconds("UpdateESDF") * 1000) 529 | // + " ms"; 530 | 531 | if (parameters_.visualize_every_n_updates_!=0 && esdf_cnt_%parameters_.visualize_every_n_updates_==0) { 532 | // std::thread(Visualization, esdf_map_, text).detach(); 533 | Visualization(esdf_map_, parameters_.global_vis_, ""); 534 | } 535 | // else { 536 | // std::thread(Visualization, nullptr, text).detach(); 537 | // Visualization(nullptr, globalVis, ""); 538 | // } 539 | } 540 | } 541 | #endif //ESDF_TOOLS_INCLUDE_FIESTA_H_ 542 | -------------------------------------------------------------------------------- /include/parameters.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by tommy on 4/25/19. 3 | // 4 | 5 | #ifndef ESDF_TOOLS_INCLUDE_PARAMETERS_H_ 6 | #define ESDF_TOOLS_INCLUDE_PARAMETERS_H_ 7 | #include 8 | #include 9 | #define PROBABILISTIC 10 | //#define HASH_TABLE 11 | #define BLOCK 12 | #define BITWISE 13 | #define DEBUG 14 | //#define SIGNED_NEEDED 15 | 16 | namespace fiesta { 17 | 18 | // Connectivity used in BFS 19 | // region DIRECTION 20 | // const static int num_dirs_ = 6; // only faces 21 | // const Eigen::Vector3i dirs_[num_dirs_] = {Eigen::Vector3i(-1, 0, 0), Eigen::Vector3i(1, 0, 0), 22 | // Eigen::Vector3i(0, -1, 0), Eigen::Vector3i(0, 1, 0), 23 | // Eigen::Vector3i(0, 0, -1), Eigen::Vector3i(0, 0, 1)}; 24 | 25 | // const static int num_dirs_ = 18; // faces & enges 26 | // const Eigen::Vector3i dirs_[num_dirs_] = {Eigen::Vector3i(-1, 0, 0), Eigen::Vector3i(1, 0, 0), 27 | // Eigen::Vector3i(0, -1, 0), Eigen::Vector3i(0, 1, 0), 28 | // Eigen::Vector3i(0, 0, -1), Eigen::Vector3i(0, 0, 1), 29 | // 30 | // Eigen::Vector3i(-1, -1, 0), Eigen::Vector3i(1, 1, 0), 31 | // Eigen::Vector3i(0, -1, -1), Eigen::Vector3i(0, 1, 1), 32 | // Eigen::Vector3i(-1, 0, -1), Eigen::Vector3i(1, 0, 1), 33 | // Eigen::Vector3i(-1, 1, 0), Eigen::Vector3i(1, -1, 0), 34 | // Eigen::Vector3i(0, -1, 1), Eigen::Vector3i(0, 1, -1), 35 | // Eigen::Vector3i(1, 0, -1), Eigen::Vector3i(-1, 0, 1)}; 36 | 37 | // const static int num_dirs_ = 26; // faces & edges & vertices 38 | // const Eigen::Vector3i dirs_[num_dirs_] = {Eigen::Vector3i(-1, 0, 0), Eigen::Vector3i(1, 0, 0), 39 | // Eigen::Vector3i(0, -1, 0), Eigen::Vector3i(0, 1, 0), 40 | // Eigen::Vector3i(0, 0, -1), Eigen::Vector3i(0, 0, 1), 41 | // 42 | // Eigen::Vector3i(-1, -1, 0), Eigen::Vector3i(1, 1, 0), 43 | // Eigen::Vector3i(0, -1, -1), Eigen::Vector3i(0, 1, 1), 44 | // Eigen::Vector3i(-1, 0, -1), Eigen::Vector3i(1, 0, 1), 45 | // Eigen::Vector3i(-1, 1, 0), Eigen::Vector3i(1, -1, 0), 46 | // Eigen::Vector3i(0, -1, 1), Eigen::Vector3i(0, 1, -1), 47 | // Eigen::Vector3i(1, 0, -1), Eigen::Vector3i(-1, 0, 1), 48 | // 49 | // Eigen::Vector3i(-1, -1, -1), Eigen::Vector3i(1, 1, 1), 50 | // Eigen::Vector3i(-1, -1, 1), Eigen::Vector3i(1, 1, -1), 51 | // Eigen::Vector3i(-1, 1, -1), Eigen::Vector3i(1, -1, 1), 52 | // Eigen::Vector3i(1, -1, -1), Eigen::Vector3i(-1, 1, 1)}; 53 | 54 | const static int num_dirs_ = 24; // faces 2 steps 55 | const Eigen::Vector3i dirs_[num_dirs_] = {Eigen::Vector3i(-1, 0, 0), Eigen::Vector3i(1, 0, 0), 56 | Eigen::Vector3i(0, -1, 0), Eigen::Vector3i(0, 1, 0), 57 | Eigen::Vector3i(0, 0, -1), Eigen::Vector3i(0, 0, 1), 58 | 59 | Eigen::Vector3i(-1, -1, 0), Eigen::Vector3i(1, 1, 0), 60 | Eigen::Vector3i(0, -1, -1), Eigen::Vector3i(0, 1, 1), 61 | Eigen::Vector3i(-1, 0, -1), Eigen::Vector3i(1, 0, 1), 62 | Eigen::Vector3i(-1, 1, 0), Eigen::Vector3i(1, -1, 0), 63 | Eigen::Vector3i(0, -1, 1), Eigen::Vector3i(0, 1, -1), 64 | Eigen::Vector3i(1, 0, -1), Eigen::Vector3i(-1, 0, 1), 65 | 66 | Eigen::Vector3i(-2, 0, 0), Eigen::Vector3i(2, 0, 0), 67 | Eigen::Vector3i(0, -2, 0), Eigen::Vector3i(0, 2, 0), 68 | Eigen::Vector3i(0, 0, -2), Eigen::Vector3i(0, 0, 2)}; 69 | 70 | // const static int num_dirs_ = 32; // faces 2 steps & cornor 71 | // const Eigen::Vector3i dirs_[num_dirs_] = {Eigen::Vector3i(-1, 0, 0), Eigen::Vector3i(1, 0, 0), 72 | // Eigen::Vector3i(0, -1, 0), Eigen::Vector3i(0, 1, 0), 73 | // Eigen::Vector3i(0, 0, -1), Eigen::Vector3i(0, 0, 1), 74 | // 75 | // Eigen::Vector3i(-1, -1, 0), Eigen::Vector3i(1, 1, 0), 76 | // Eigen::Vector3i(0, -1, -1), Eigen::Vector3i(0, 1, 1), 77 | // Eigen::Vector3i(-1, 0, -1), Eigen::Vector3i(1, 0, 1), 78 | // Eigen::Vector3i(-1, 1, 0), Eigen::Vector3i(1, -1, 0), 79 | // Eigen::Vector3i(0, -1, 1), Eigen::Vector3i(0, 1, -1), 80 | // Eigen::Vector3i(1, 0, -1), Eigen::Vector3i(-1, 0, 1), 81 | // 82 | // Eigen::Vector3i(-2, 0, 0), Eigen::Vector3i(2, 0, 0), 83 | // Eigen::Vector3i(0, -2, 0), Eigen::Vector3i(0, 2, 0), 84 | // Eigen::Vector3i(0, 0, -2), Eigen::Vector3i(0, 0, 2), 85 | // 86 | // Eigen::Vector3i(-1, -1, -1), Eigen::Vector3i(1, 1, 1), 87 | // Eigen::Vector3i(-1, -1, 1), Eigen::Vector3i(1, 1, -1), 88 | // Eigen::Vector3i(-1, 1, -1), Eigen::Vector3i(1, -1, 1), 89 | // Eigen::Vector3i(1, -1, -1), Eigen::Vector3i(-1, 1, 1) 90 | // }; 91 | 92 | // const static int num_dirs_ = 62; // faces 3 steps 93 | // const Eigen::Vector3i dirs_[num_dirs_] = {Eigen::Vector3i(-1, 0, 0), Eigen::Vector3i(1, 0, 0), 94 | // Eigen::Vector3i(0, -1, 0), Eigen::Vector3i(0, 1, 0), 95 | // Eigen::Vector3i(0, 0, -1), Eigen::Vector3i(0, 0, 1), 96 | // 97 | // Eigen::Vector3i(-1, -1, 0), Eigen::Vector3i(1, 1, 0), 98 | // Eigen::Vector3i(0, -1, -1), Eigen::Vector3i(0, 1, 1), 99 | // Eigen::Vector3i(-1, 0, -1), Eigen::Vector3i(1, 0, 1), 100 | // Eigen::Vector3i(-1, 1, 0), Eigen::Vector3i(1, -1, 0), 101 | // Eigen::Vector3i(0, -1, 1), Eigen::Vector3i(0, 1, -1), 102 | // Eigen::Vector3i(1, 0, -1), Eigen::Vector3i(-1, 0, 1), 103 | // 104 | // Eigen::Vector3i(-1, -1, -1), Eigen::Vector3i(1, 1, 1), 105 | // Eigen::Vector3i(-1, -1, 1), Eigen::Vector3i(1, 1, -1), 106 | // Eigen::Vector3i(-1, 1, -1), Eigen::Vector3i(1, -1, 1), 107 | // Eigen::Vector3i(1, -1, -1), Eigen::Vector3i(-1, 1, 1), 108 | // 109 | // Eigen::Vector3i(-2, 0, 0), Eigen::Vector3i(2, 0, 0), 110 | // Eigen::Vector3i(0, -2, 0), Eigen::Vector3i(0, 2, 0), 111 | // Eigen::Vector3i(0, 0, -2), Eigen::Vector3i(0, 0, 2), 112 | // 113 | // Eigen::Vector3i(-2, -1, 0), Eigen::Vector3i(2, 1, 0), 114 | // Eigen::Vector3i(0, -2, -1), Eigen::Vector3i(0, 2, 1), 115 | // Eigen::Vector3i(-2, 0, -1), Eigen::Vector3i(2, 0, 1), 116 | // Eigen::Vector3i(-2, 1, 0), Eigen::Vector3i(2, -1, 0), 117 | // Eigen::Vector3i(0, -2, 1), Eigen::Vector3i(0, 2, -1), 118 | // Eigen::Vector3i(2, 0, -1), Eigen::Vector3i(-2, 0, 1), 119 | // 120 | // Eigen::Vector3i(-1, -2, 0), Eigen::Vector3i(1, 2, 0), 121 | // Eigen::Vector3i(0, -1, -2), Eigen::Vector3i(0, 1, 2), 122 | // Eigen::Vector3i(-1, 0, -2), Eigen::Vector3i(1, 0, 2), 123 | // Eigen::Vector3i(-1, 2, 0), Eigen::Vector3i(1, -2, 0), 124 | // Eigen::Vector3i(0, -1, 2), Eigen::Vector3i(0, 1, -2), 125 | // Eigen::Vector3i(1, 0, -2), Eigen::Vector3i(-1, 0, 2), 126 | // 127 | // Eigen::Vector3i(-3, 0, 0), Eigen::Vector3i(3, 0, 0), 128 | // Eigen::Vector3i(0, -3, 0), Eigen::Vector3i(0, 3, 0), 129 | // Eigen::Vector3i(0, 0, -3), Eigen::Vector3i(0, 0, 3)}; 130 | 131 | // endregion 132 | 133 | struct Parameters { 134 | // resolution 135 | double resolution_; 136 | // hash table implementation only 137 | int reserved_size_; 138 | // array implementation only 139 | Eigen::Vector3d l_cornor_, r_cornor_, map_size_; 140 | // intrinsic parameters of camera, only used if your input is depth image 141 | double center_x_, center_y_, focal_x_, focal_y_; 142 | // parameters of probabilistic occupancy updating 143 | double p_hit_, p_miss_, p_min_, p_max_, p_occ_; 144 | //depth filter 145 | bool use_depth_filter_; 146 | double depth_filter_max_dist_, depth_filter_min_dist_, depth_filter_tolerance_; 147 | int depth_filter_margin_; 148 | // ray cast parameters 149 | double min_ray_length_, max_ray_length_; 150 | // visualization 151 | double slice_vis_max_dist_; 152 | int slice_vis_level_, vis_lower_bound_, vis_upper_bound_; 153 | // frequency of updating 154 | double update_esdf_every_n_sec_; 155 | // frequency of visualization 156 | int visualize_every_n_updates_; 157 | // number of thread used 158 | int ray_cast_num_thread_; 159 | // local map 160 | bool global_vis_, global_update_, global_map_; 161 | Eigen::Vector3d radius_; 162 | // transforms 163 | Eigen::Matrix4d T_B_C_, T_D_B_; 164 | 165 | 166 | void SetParameters(const ros::NodeHandle &node); 167 | }; 168 | } 169 | #endif //ESDF_TOOLS_INCLUDE_PARAMETERS_H_ 170 | -------------------------------------------------------------------------------- /include/raycast.h: -------------------------------------------------------------------------------- 1 | #ifndef RAYCAST_H_ 2 | #define RAYCAST_H_ 3 | 4 | #include 5 | #include 6 | 7 | double signum(double x); 8 | 9 | double mod(double value, double modulus); 10 | 11 | double intbound(double s, double ds); 12 | 13 | bool RayIntersectsAABB(const Eigen::Vector3d &start, const Eigen::Vector3d &end, 14 | const Eigen::Vector3d &lb, const Eigen::Vector3d &rt); 15 | 16 | void Raycast(const Eigen::Vector3d &start, const Eigen::Vector3d &end, 17 | const Eigen::Vector3d &min, const Eigen::Vector3d &max, 18 | std::vector *output); 19 | 20 | #endif // RAYCAST_H_ -------------------------------------------------------------------------------- /include/timing.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2012-2013 Simon Lynen, ASL, ETH Zurich, Switzerland 3 | * You can contact the author at 4 | * 5 | * Licensed under the Apache License, Version 2.0 (the "License"); 6 | * you may not use this file except in compliance with the License. 7 | * You may obtain a copy of the License at 8 | * 9 | * http://www.apache.org/licenses/LICENSE-2.0 10 | * 11 | * Unless required by applicable law or agreed to in writing, software 12 | * distributed under the License is distributed on an "AS IS" BASIS, 13 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 14 | * See the License for the specific language governing permissions and 15 | * limitations under the License. 16 | */ 17 | 18 | /* Adapted from Paul Furgale Schweizer Messer sm_timing */ 19 | 20 | #ifndef VOXBLOX_UTILS_TIMING_H_ 21 | #define VOXBLOX_UTILS_TIMING_H_ 22 | 23 | #include 24 | #include 25 | #include 26 | #include 27 | #include 28 | #include 29 | #include 30 | #include 31 | 32 | template 33 | using AlignedVector = std::vector>; 34 | 35 | namespace timing { 36 | 37 | template 38 | class Accumulator { 39 | public: 40 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 41 | 42 | Accumulator() 43 | : window_samples_(0), 44 | totalsamples_(0), 45 | window_sum_(0), 46 | sum_(0), 47 | min_(std::numeric_limits::max()), 48 | max_(std::numeric_limits::min()) {} 49 | 50 | void Add(T sample) { 51 | if (window_samples_ < N) { 52 | samples_[window_samples_++] = sample; 53 | window_sum_ += sample; 54 | } else { 55 | T &oldest = samples_[window_samples_++ % N]; 56 | window_sum_ += sample - oldest; 57 | oldest = sample; 58 | } 59 | sum_ += sample; 60 | ++totalsamples_; 61 | if (sample > max_) { 62 | max_ = sample; 63 | } 64 | if (sample < min_) { 65 | min_ = sample; 66 | } 67 | } 68 | 69 | int TotalSamples() const { return totalsamples_; } 70 | 71 | double Sum() const { return sum_; } 72 | 73 | double Mean() const { return sum_ / totalsamples_; } 74 | 75 | double RollingMean() const { 76 | return window_sum_ / std::min(window_samples_, N); 77 | } 78 | 79 | double Max() const { return max_; } 80 | 81 | double Min() const { return min_; } 82 | 83 | double LazyVariance() const { 84 | if (window_samples_ == 0) { 85 | return 0.0; 86 | } 87 | double var = 0; 88 | double mean = RollingMean(); 89 | for (int i = 0; i < std::min(window_samples_, N); ++i) { 90 | var += (samples_[i] - mean) * (samples_[i] - mean); 91 | } 92 | var /= std::min(window_samples_, N); 93 | return var; 94 | } 95 | 96 | private: 97 | int window_samples_; 98 | int totalsamples_; 99 | Total window_sum_; 100 | Total sum_; 101 | T min_; 102 | T max_; 103 | T samples_[N]; 104 | }; 105 | 106 | struct TimerMapValue { 107 | TimerMapValue() {} 108 | 109 | /// Create an accumulator with specified window size. 110 | Accumulator acc_; 111 | }; 112 | 113 | /** 114 | * A class that has the timer interface but does nothing. Swapping this in in 115 | * place of the Timer class (say with a typedef) should allow one to disable 116 | * timing. Because all of the functions are inline, they should just disappear. 117 | */ 118 | class DummyTimer { 119 | public: 120 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 121 | 122 | explicit DummyTimer(size_t /*handle*/, bool /*constructStopped*/ = false) {} 123 | 124 | explicit DummyTimer(std::string const & /*tag*/, 125 | bool /*constructStopped*/ = false) {} 126 | 127 | ~DummyTimer() {} 128 | 129 | void Start() {} 130 | 131 | void Stop() {} 132 | 133 | bool IsTiming() { return false; } 134 | }; 135 | 136 | class Timer { 137 | public: 138 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 139 | 140 | explicit Timer(size_t handle, bool constructStopped = false); 141 | 142 | explicit Timer(std::string const &tag, bool constructStopped = false); 143 | 144 | ~Timer(); 145 | 146 | void Start(); 147 | 148 | void Stop(); 149 | 150 | bool IsTiming() const; 151 | 152 | private: 153 | std::chrono::time_point time_; 154 | 155 | bool timing_; 156 | size_t handle_; 157 | }; 158 | 159 | class Timing { 160 | public: 161 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 162 | 163 | typedef std::map map_t; 164 | 165 | friend class Timer; 166 | 167 | // Definition of static functions to query the timers. 168 | static size_t GetHandle(std::string const &tag); 169 | 170 | static std::string GetTag(size_t handle); 171 | 172 | static double GetTotalSeconds(size_t handle); 173 | 174 | static double GetTotalSeconds(std::string const &tag); 175 | 176 | static double GetMeanSeconds(size_t handle); 177 | 178 | static double GetMeanSeconds(std::string const &tag); 179 | 180 | static size_t GetNumSamples(size_t handle); 181 | 182 | static size_t GetNumSamples(std::string const &tag); 183 | 184 | static double GetVarianceSeconds(size_t handle); 185 | 186 | static double GetVarianceSeconds(std::string const &tag); 187 | 188 | static double GetMinSeconds(size_t handle); 189 | 190 | static double GetMinSeconds(std::string const &tag); 191 | 192 | static double GetMaxSeconds(size_t handle); 193 | 194 | static double GetMaxSeconds(std::string const &tag); 195 | 196 | static double GetHz(size_t handle); 197 | 198 | static double GetHz(std::string const &tag); 199 | 200 | static void Print(std::ostream &out); 201 | 202 | static std::string Print(); 203 | 204 | static std::string SecondsToTimeString(double seconds); 205 | 206 | static void Reset(); 207 | 208 | static const map_t &GetTimers() { return Instance().tagMap_; } 209 | 210 | private: 211 | void AddTime(size_t handle, double seconds); 212 | 213 | static Timing &Instance(); 214 | 215 | Timing(); 216 | 217 | ~Timing(); 218 | 219 | typedef AlignedVector list_t; 220 | 221 | list_t timers_; 222 | map_t tagMap_; 223 | size_t maxTagLength_; 224 | std::mutex mutex_; 225 | }; 226 | 227 | #if ENABLE_MSF_TIMING 228 | typedef Timer DebugTimer; 229 | #else 230 | typedef DummyTimer DebugTimer; 231 | #endif 232 | 233 | } // namespace timing 234 | 235 | #endif // VOXBLOX_UTILS_TIMING_H_ 236 | -------------------------------------------------------------------------------- /launch/cow_and_lady.launch: -------------------------------------------------------------------------------- 1 | 2 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | -------------------------------------------------------------------------------- /launch/demo.launch: -------------------------------------------------------------------------------- 1 | 2 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | fiesta 4 | 0.0.1 5 | ESDF Tools 6 | hlx1996 7 | MIT 8 | 9 | catkin 10 | 11 | roscpp 12 | rospy 13 | sensor_msgs 14 | std_msgs 15 | visualization_msgs 16 | message_generation 17 | 18 | roscpp 19 | rospy 20 | sensor_msgs 21 | std_msgs 22 | visualization_msgs 23 | message_runtime 24 | 25 | 26 | 27 | 28 | 29 | -------------------------------------------------------------------------------- /src/ESDFMap.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Created by tommy on 12/17/18. 3 | // 4 | 5 | #include "ESDFMap.h" 6 | #include 7 | #include 8 | 9 | using std::cout; 10 | using std::endl; 11 | 12 | const double fiesta::ESDFMap::Logit(const double &x) const { 13 | return log(x / (1 - x)); 14 | } 15 | 16 | bool fiesta::ESDFMap::Exist(const int &idx) { 17 | #ifdef PROBABILISTIC 18 | return occupancy_buffer_[idx] > min_occupancy_log_; 19 | #else 20 | return occupancy_buffer_[idx] == 1; 21 | #endif 22 | } 23 | 24 | void fiesta::ESDFMap::DeleteFromList(int link, int idx) { 25 | if (prev_[idx] != undefined_) 26 | next_[prev_[idx]] = next_[idx]; 27 | else 28 | head_[link] = next_[idx]; 29 | if (next_[idx] != undefined_) 30 | prev_[next_[idx]] = prev_[idx]; 31 | prev_[idx] = next_[idx] = undefined_; 32 | } 33 | 34 | void fiesta::ESDFMap::InsertIntoList(int link, int idx) { 35 | if (head_[link] == undefined_) 36 | head_[link] = idx; 37 | else { 38 | prev_[head_[link]] = idx; 39 | next_[idx] = head_[link]; 40 | head_[link] = idx; 41 | } 42 | } 43 | 44 | // region CONVERSION 45 | 46 | bool fiesta::ESDFMap::PosInMap(Eigen::Vector3d pos) { 47 | #ifdef HASH_TABLE 48 | return true; 49 | #else 50 | if (pos(0) < min_range_(0) || pos(1) < min_range_(1) || pos(2) < min_range_(2)) { 51 | // cout << "less than min range!\t" << pos(0) << ' ' << pos(1) << ' ' << pos(2) << endl; 52 | return false; 53 | } 54 | 55 | if (pos(0) > max_range_(0) || pos(1) > max_range_(1) || pos(2) > max_range_(2)) { 56 | // cout << "larger than min range!\t" << pos(0) << ' ' << pos(1) << ' ' << pos(2) << endl; 57 | return false; 58 | } 59 | return true; 60 | #endif 61 | } 62 | 63 | bool fiesta::ESDFMap::VoxInRange(Eigen::Vector3i vox, bool current_vec) { 64 | if (current_vec) 65 | return (vox(0) >= min_vec_(0) && vox(0) <= max_vec_(0) 66 | && vox(1) >= min_vec_(1) && vox(1) <= max_vec_(1) 67 | && vox(2) >= min_vec_(2) && vox(2) <= max_vec_(2)); 68 | else 69 | return (vox(0) >= last_min_vec_(0) && vox(0) <= last_max_vec_(0) 70 | && vox(1) >= last_min_vec_(1) && vox(1) <= last_max_vec_(1) 71 | && vox(2) >= last_min_vec_(2) && vox(2) <= last_max_vec_(2)); 72 | } 73 | 74 | void fiesta::ESDFMap::Pos2Vox(Eigen::Vector3d pos, Eigen::Vector3i &vox) { 75 | for (int i = 0; i < 3; ++i) 76 | vox(i) = floor((pos(i) - origin_(i)) / resolution_); 77 | } 78 | 79 | void fiesta::ESDFMap::Vox2Pos(Eigen::Vector3i vox, Eigen::Vector3d &pos) { 80 | for (int i = 0; i < 3; ++i) 81 | pos(i) = (vox(i) + 0.5) * resolution_ + origin_(i); 82 | } 83 | 84 | int fiesta::ESDFMap::Vox2Idx(Eigen::Vector3i vox) { 85 | #ifdef HASH_TABLE 86 | if (vox(0) == undefined_) return reserved_idx_4_undefined_; 87 | return FindAndInsert(Eigen::Vector3i(vox(0), vox(1), vox(2))); 88 | #else 89 | if (vox(0) == undefined_) 90 | return reserved_idx_4_undefined_; 91 | return vox(0) * grid_size_yz_ + vox(1) * grid_size_(2) + vox(2); 92 | #endif 93 | } 94 | 95 | int fiesta::ESDFMap::Vox2Idx(Eigen::Vector3i vox, int sub_sampling_factor) { 96 | #ifdef HASH_TABLE 97 | // TODO: ignore sub_sampling_factor 98 | if (vox(0) == undefined_) return reserved_idx_4_undefined_; 99 | return FindAndInsert(Eigen::Vector3i(vox(0), vox(1), vox(2))); 100 | #else 101 | if (vox(0) == undefined_) 102 | return reserved_idx_4_undefined_; 103 | return vox(0) * grid_size_yz_ / sub_sampling_factor / sub_sampling_factor / sub_sampling_factor 104 | + vox(1) * grid_size_(2) / sub_sampling_factor / sub_sampling_factor 105 | + vox(2) / sub_sampling_factor; 106 | #endif 107 | } 108 | 109 | Eigen::Vector3i fiesta::ESDFMap::Idx2Vox(int idx) { 110 | #ifdef HASH_TABLE 111 | return vox_buffer_[idx]; 112 | #else 113 | return Eigen::Vector3i(idx / grid_size_yz_, 114 | idx % (grid_size_yz_) / grid_size_(2), 115 | idx % grid_size_(2)); 116 | #endif 117 | 118 | } 119 | 120 | // endregion 121 | 122 | double fiesta::ESDFMap::Dist(Eigen::Vector3i a, Eigen::Vector3i b) { 123 | return (b - a).cast().norm() * resolution_; 124 | // return (b - a).squaredNorm(); 125 | // TODO: may use square root & * resolution_ at last together to speed up 126 | } 127 | 128 | #ifdef HASH_TABLE 129 | 130 | fiesta::ESDFMap::ESDFMap(Eigen::Vector3d origin, double resolution_, int reserve_size) 131 | : origin_(origin), resolution_(resolution_) { 132 | resolution_inv_ = 1 / resolution_; 133 | infinity_ = 10000; 134 | undefined_ = -10000; 135 | reserved_idx_4_undefined_ = 0; 136 | #ifdef BLOCK 137 | block_bit_ = 3; 138 | block_ = (1 << block_bit_); 139 | 140 | block_size_ = block_ * block_ * block_; 141 | if (block_size_ > reserve_size) reserve_size = block_size_; 142 | #endif 143 | SetOriginalRange(); 144 | count = 1; // 0 is used for special use 145 | reserve_size_ = reserve_size + 1; 146 | occupancy_buffer_.resize(reserve_size_); 147 | distance_buffer_.resize(reserve_size_); 148 | closest_obstacle_.resize(reserve_size_); 149 | vox_buffer_.resize(reserve_size_); 150 | num_hit_.resize(reserve_size_); 151 | num_miss_.resize(reserve_size_); 152 | 153 | std::fill(distance_buffer_.begin(), distance_buffer_.end(), (double) undefined_); 154 | std::fill(occupancy_buffer_.begin(), occupancy_buffer_.end(), 0); 155 | std::fill(closest_obstacle_.begin(), closest_obstacle_.end(), Eigen::Vector3i(undefined_, undefined_, undefined_)); 156 | std::fill(vox_buffer_.begin(), vox_buffer_.end(), Eigen::Vector3i(undefined_, undefined_, undefined_)); 157 | std::fill(num_hit_.begin(), num_hit_.end(), 0); 158 | std::fill(num_miss_.begin(), num_miss_.end(), 0); 159 | 160 | head_.resize(reserve_size_); 161 | prev_.resize(reserve_size_); 162 | next_.resize(reserve_size_); 163 | std::fill(head_.begin(), head_.end(), undefined_); 164 | std::fill(prev_.begin(), prev_.end(), undefined_); 165 | std::fill(next_.begin(), next_.end(), undefined_); 166 | 167 | } 168 | 169 | #else 170 | 171 | fiesta::ESDFMap::ESDFMap(Eigen::Vector3d origin, double resolution_, Eigen::Vector3d map_size) 172 | : origin_(origin), resolution_(resolution_), map_size_(map_size) { 173 | resolution_inv_ = 1 / resolution_; 174 | 175 | for (int i = 0; i < 3; ++i) 176 | grid_size_(i) = ceil(map_size(i) / resolution_); 177 | 178 | min_range_ = origin; 179 | max_range_ = origin + map_size; 180 | grid_size_yz_ = grid_size_(1) * grid_size_(2); 181 | infinity_ = 10000; 182 | undefined_ = -10000; 183 | 184 | grid_total_size_ = grid_size_(0) * grid_size_yz_; 185 | reserved_idx_4_undefined_ = grid_total_size_; 186 | SetOriginalRange(); 187 | 188 | cout << grid_total_size_ << endl; 189 | occupancy_buffer_.resize(grid_total_size_); 190 | 191 | distance_buffer_.resize(grid_total_size_); 192 | // distanceBufferNegative.resize(grid_total_size_); 193 | 194 | closest_obstacle_.resize(grid_total_size_); 195 | num_hit_.resize(grid_total_size_); 196 | num_miss_.resize(grid_total_size_); 197 | 198 | std::fill(distance_buffer_.begin(), distance_buffer_.end(), (double) undefined_); 199 | // std::fill(distanceBufferNegative.begin(), distanceBufferNegative.end(), (double) undefined_); 200 | 201 | std::fill(occupancy_buffer_.begin(), occupancy_buffer_.end(), 0); 202 | std::fill(closest_obstacle_.begin(), closest_obstacle_.end(), Eigen::Vector3i(undefined_, undefined_, undefined_)); 203 | std::fill(num_hit_.begin(), num_hit_.end(), 0); 204 | std::fill(num_miss_.begin(), num_miss_.end(), 0); 205 | 206 | head_.resize(grid_total_size_ + 1); 207 | prev_.resize(grid_total_size_); 208 | next_.resize(grid_total_size_); 209 | std::fill(head_.begin(), head_.end(), undefined_); 210 | std::fill(prev_.begin(), prev_.end(), undefined_); 211 | std::fill(next_.begin(), next_.end(), undefined_); 212 | 213 | } 214 | 215 | #endif 216 | 217 | #ifdef PROBABILISTIC 218 | void fiesta::ESDFMap::SetParameters(double p_hit, double p_miss, double p_min, double p_max, double p_occ) { 219 | prob_hit_log_ = Logit(p_hit); 220 | prob_miss_log_ = Logit(p_miss); 221 | clamp_min_log_ = Logit(p_min); 222 | clamp_max_log_ = Logit(p_max); 223 | min_occupancy_log_ = Logit(p_occ); 224 | } 225 | #endif 226 | 227 | bool fiesta::ESDFMap::CheckUpdate() { 228 | #ifdef PROBABILISTIC 229 | return !occupancy_queue_.empty(); 230 | #else 231 | return true; 232 | #endif 233 | } 234 | 235 | bool fiesta::ESDFMap::UpdateOccupancy(bool global_map) { 236 | #ifdef PROBABILISTIC 237 | cout << "Occupancy Update" << ' ' << occupancy_queue_.size() << '\t'; 238 | while (!occupancy_queue_.empty()) { 239 | QueueElement xx = occupancy_queue_.front(); 240 | occupancy_queue_.pop(); 241 | int idx = Vox2Idx(xx.point_); 242 | int occupy = Exist(idx); 243 | double log_odds_update = (num_hit_[idx] >= num_miss_[idx] - num_hit_[idx] ? prob_hit_log_ : prob_miss_log_); 244 | 245 | num_hit_[idx] = num_miss_[idx] = 0; 246 | if (distance_buffer_[idx] < 0) { 247 | distance_buffer_[idx] = infinity_; 248 | InsertIntoList(reserved_idx_4_undefined_, idx); 249 | } 250 | if ((log_odds_update >= 0 && 251 | occupancy_buffer_[idx] >= clamp_max_log_) || 252 | (log_odds_update <= 0 && 253 | occupancy_buffer_[idx] <= clamp_min_log_)) { 254 | continue; 255 | } 256 | if (!global_map && !VoxInRange(xx.point_, false)) { 257 | occupancy_buffer_[idx] = 0; 258 | distance_buffer_[idx] = infinity_; 259 | } 260 | occupancy_buffer_[idx] = std::min( 261 | std::max(occupancy_buffer_[idx] + log_odds_update, clamp_min_log_), 262 | clamp_max_log_); 263 | if (Exist(idx) && !occupy) { 264 | insert_queue_.push(QueueElement{xx.point_, 0.0}); 265 | } else if (!Exist(idx) && occupy) { 266 | delete_queue_.push(QueueElement{xx.point_, (double) infinity_}); 267 | } 268 | } 269 | #endif 270 | return !insert_queue_.empty() || !delete_queue_.empty(); 271 | } 272 | 273 | void fiesta::ESDFMap::UpdateESDF() { 274 | // clock_t startTime,endTime; 275 | // startTime = clock(); 276 | // UpdateOccupancy(); 277 | cout << "Insert " << insert_queue_.size() << "\tDelete " << delete_queue_.size() << endl; 278 | while (!insert_queue_.empty()) { 279 | QueueElement xx = insert_queue_.front(); 280 | insert_queue_.pop(); 281 | int idx = Vox2Idx(xx.point_); 282 | if (Exist(idx)) { 283 | // Exist after a whole brunch of updates 284 | // delete previous link & create a new linked-list 285 | DeleteFromList(Vox2Idx(closest_obstacle_[idx]), idx); 286 | closest_obstacle_[idx] = xx.point_; 287 | distance_buffer_[idx] = 0.0; 288 | InsertIntoList(idx, idx); 289 | update_queue_.push(xx); 290 | } 291 | } 292 | while (!delete_queue_.empty()) { 293 | QueueElement xx = delete_queue_.front(); 294 | 295 | delete_queue_.pop(); 296 | int idx = Vox2Idx(xx.point_); 297 | if (!Exist(idx)) { 298 | // doesn't Exist after a whole brunch of updates 299 | 300 | int next_obs_idx; 301 | for (int obs_idx = head_[idx]; obs_idx != undefined_; obs_idx = next_obs_idx) { 302 | 303 | closest_obstacle_[obs_idx] = Eigen::Vector3i(undefined_, undefined_, undefined_); 304 | Eigen::Vector3i obs_vox = Idx2Vox(obs_idx); 305 | 306 | double distance = infinity_; 307 | // find neighborhood whose closest obstacles Exist 308 | for (const auto &dir : dirs_) { 309 | Eigen::Vector3i new_pos = obs_vox + dir; 310 | int new_pos_idx = Vox2Idx(new_pos); 311 | if (VoxInRange(new_pos) && closest_obstacle_[new_pos_idx](0) != undefined_ 312 | && Exist(Vox2Idx(closest_obstacle_[new_pos_idx]))) { 313 | // if in range and closest obstacles Exist 314 | double tmp = Dist(obs_vox, closest_obstacle_[new_pos_idx]); 315 | if (tmp < distance) { 316 | distance = tmp; 317 | closest_obstacle_[obs_idx] = closest_obstacle_[new_pos_idx]; 318 | } 319 | break; 320 | } // if 321 | } // for neighborhood 322 | 323 | // destroy the linked-list 324 | prev_[obs_idx] = undefined_; 325 | next_obs_idx = next_[obs_idx]; 326 | next_[obs_idx] = undefined_; 327 | 328 | distance_buffer_[obs_idx] = distance; 329 | if (distance < infinity_) { 330 | update_queue_.push(QueueElement{obs_vox, distance}); 331 | } 332 | int new_obs_idx = Vox2Idx(closest_obstacle_[obs_idx]); 333 | InsertIntoList(new_obs_idx, obs_idx); 334 | } // for obs_idx 335 | head_[idx] = undefined_; 336 | } // if 337 | } // delete_queue_ 338 | int times = 0, change_num = 0; 339 | while (!update_queue_.empty()) { 340 | QueueElement xx = update_queue_.front(); 341 | // QueueElement xx = update_queue_.top(); 342 | 343 | update_queue_.pop(); 344 | int idx = Vox2Idx(xx.point_); 345 | if (xx.distance_ != distance_buffer_[idx]) 346 | continue; 347 | times++; 348 | bool change = false; 349 | for (int i = 0; i < num_dirs_; i++) { 350 | Eigen::Vector3i new_pos = xx.point_ + dirs_[i]; 351 | if (VoxInRange(new_pos)) { 352 | int new_pos_idx = Vox2Idx(new_pos); 353 | if (closest_obstacle_[new_pos_idx](0) == undefined_) 354 | continue; 355 | double tmp = Dist(xx.point_, closest_obstacle_[new_pos_idx]); 356 | 357 | if (distance_buffer_[idx] > tmp) { 358 | distance_buffer_[idx] = tmp; 359 | change = true; 360 | DeleteFromList(Vox2Idx(closest_obstacle_[idx]), idx); 361 | 362 | int new_obs_idx = Vox2Idx(closest_obstacle_[new_pos_idx]); 363 | InsertIntoList(new_obs_idx, idx); 364 | closest_obstacle_[idx] = closest_obstacle_[new_pos_idx]; 365 | } 366 | } 367 | } 368 | 369 | if (change) { 370 | change_num++; 371 | update_queue_.push(QueueElement{xx.point_, distance_buffer_[idx]}); 372 | continue; 373 | } 374 | 375 | int new_obs_idx = Vox2Idx(closest_obstacle_[idx]); 376 | for (const auto &dir : dirs_) { 377 | Eigen::Vector3i new_pos = xx.point_ + dir; 378 | if (VoxInRange(new_pos)) { 379 | int new_pos_id = Vox2Idx(new_pos); 380 | 381 | double tmp = Dist(new_pos, closest_obstacle_[idx]); 382 | if (distance_buffer_[new_pos_id] > tmp) { 383 | distance_buffer_[new_pos_id] = tmp; 384 | DeleteFromList(Vox2Idx(closest_obstacle_[new_pos_id]), new_pos_id); 385 | 386 | InsertIntoList(new_obs_idx, new_pos_id); 387 | closest_obstacle_[new_pos_id] = closest_obstacle_[idx]; 388 | update_queue_.push(QueueElement{new_pos, tmp}); 389 | } 390 | } 391 | } 392 | } 393 | total_time_ += times; 394 | cout << "Expanding " << times << " nodes, with change_num = " << change_num << ", accumulator = " << total_time_ 395 | << endl; 396 | // endTime = clock(); 397 | // cout << "Totle Time : " <<(double)(endTime - startTime) / CLOCKS_PER_SEC << "s" << endl; 398 | } 399 | 400 | 401 | int fiesta::ESDFMap::SetOccupancy(Eigen::Vector3d pos, int occ) { 402 | if (occ != 1 && occ != 0) { 403 | cout << "occ value error!" << endl; 404 | return undefined_; 405 | } 406 | 407 | if (!PosInMap(pos)) { 408 | // cout << "Not in map" << endl; 409 | return undefined_; 410 | } 411 | 412 | Eigen::Vector3i vox; 413 | Pos2Vox(pos, vox); 414 | return SetOccupancy(vox, occ); 415 | } 416 | 417 | int fiesta::ESDFMap::SetOccupancy(Eigen::Vector3i vox, int occ) { 418 | int idx = Vox2Idx(vox);//, idx2 = Vox2Idx(vox, 2); 419 | 420 | if (!VoxInRange(vox)) 421 | return idx; 422 | 423 | #ifdef PROBABILISTIC 424 | num_miss_[idx]++; 425 | num_hit_[idx] += occ; 426 | if (num_miss_[idx] == 1) { 427 | #ifdef HASH_TABLE 428 | occupancy_queue_.push(QueueElement{vox, 0.0}); 429 | #else 430 | // multi-thread 431 | // mtx.lock(); 432 | occupancy_queue_.push(QueueElement{vox, 0.0}); 433 | // mtx.unlock(); 434 | #endif 435 | } 436 | 437 | return idx; 438 | #else 439 | if (occupancy_buffer_[idx] != occ && occupancy_buffer_[idx] != (occ | 2)) { 440 | // cout << occupancy_buffer_[idx] << "\t" << occ << endl; 441 | if (occ == 1) insert_queue_.push(QueueElement{vox, 0.0}); 442 | else delete_queue_.push(QueueElement{vox, (double)infinity_}); 443 | } 444 | occupancy_buffer_[idx] = occ; 445 | if (distance_buffer_[idx] < 0) { 446 | distance_buffer_[idx] = infinity_; 447 | InsertIntoList(reserved_idx_4_undefined_, idx); 448 | } 449 | #endif 450 | } 451 | 452 | int fiesta::ESDFMap::GetOccupancy(Eigen::Vector3d pos) { 453 | if (!PosInMap(pos)) 454 | return undefined_; 455 | 456 | Eigen::Vector3i vox; 457 | Pos2Vox(pos, vox); 458 | 459 | return Exist(Vox2Idx(vox)); 460 | } 461 | 462 | int fiesta::ESDFMap::GetOccupancy(Eigen::Vector3i pos_id) { 463 | // TODO: no boundary check 464 | return Exist(Vox2Idx(std::move(pos_id))); 465 | } 466 | 467 | double fiesta::ESDFMap::GetDistance(Eigen::Vector3d pos) { 468 | if (!PosInMap(pos)) 469 | return undefined_; 470 | 471 | Eigen::Vector3i vox; 472 | Pos2Vox(pos, vox); 473 | 474 | return GetDistance(vox); 475 | } 476 | 477 | double fiesta::ESDFMap::GetDistance(Eigen::Vector3i vox) { 478 | return distance_buffer_[Vox2Idx(vox)] < 0 ? infinity_ : distance_buffer_[Vox2Idx(vox)]; 479 | } 480 | 481 | double fiesta::ESDFMap::GetDistWithGradTrilinear(Eigen::Vector3d pos, 482 | Eigen::Vector3d &grad) { 483 | if (!PosInMap(pos)) 484 | return -1; 485 | 486 | /* this is too young too simple */ 487 | // gradient(0) = (GetDistance(id(0) + 1, id(1), id(2)) - 488 | // GetDistance(id(0) - 1, id(1), id(2))) * 489 | // 0.5 * resolution_inv_; 490 | // gradient(1) = (GetDistance(id(0), id(1) + 1, id(2)) - 491 | // GetDistance(id(0), id(1) - 1, id(2))) * 492 | // 0.5 * resolution_inv_; 493 | // gradient(2) = (GetDistance(id(0), id(1), id(2) + 1) - 494 | // GetDistance(id(0), id(1), id(2) - 1)) * 495 | // 0.5 * resolution_inv_; 496 | 497 | /* use trilinear interpolation */ 498 | Eigen::Vector3d pos_m = pos - 0.5 * resolution_ * Eigen::Vector3d::Ones(); 499 | 500 | Eigen::Vector3i idx; 501 | Pos2Vox(pos_m, idx); 502 | // cout << voxInMap(idx) << endl; 503 | 504 | Eigen::Vector3d idx_pos, diff; 505 | Vox2Pos(idx, idx_pos); 506 | 507 | diff = (pos - idx_pos) * resolution_inv_; 508 | 509 | double values[2][2][2]; 510 | for (int x = 0; x < 2; x++) { 511 | for (int y = 0; y < 2; y++) { 512 | for (int z = 0; z < 2; z++) { 513 | Eigen::Vector3i current_idx = idx + Eigen::Vector3i(x, y, z); 514 | values[x][y][z] = GetDistance(current_idx); 515 | } 516 | } 517 | } 518 | 519 | double v00 = (1 - diff[0]) * values[0][0][0] + diff[0] * values[1][0][0]; 520 | double v01 = (1 - diff[0]) * values[0][0][1] + diff[0] * values[1][0][1]; 521 | double v10 = (1 - diff[0]) * values[0][1][0] + diff[0] * values[1][1][0]; 522 | double v11 = (1 - diff[0]) * values[0][1][1] + diff[0] * values[1][1][1]; 523 | 524 | double v0 = (1 - diff[1]) * v00 + diff[1] * v10; 525 | double v1 = (1 - diff[1]) * v01 + diff[1] * v11; 526 | 527 | double dist = (1 - diff[2]) * v0 + diff[2] * v1; 528 | 529 | grad[2] = (v1 - v0) * resolution_inv_; 530 | grad[1] = 531 | ((1 - diff[2]) * (v10 - v00) + diff[2] * (v11 - v01)) * resolution_inv_; 532 | grad[0] = (1 - diff[2]) * (1 - diff[1]) * (values[1][0][0] - values[0][0][0]); 533 | grad[0] += (1 - diff[2]) * diff[1] * (values[1][1][0] - values[0][1][0]); 534 | grad[0] += diff[2] * (1 - diff[1]) * (values[1][0][1] - values[0][0][1]); 535 | grad[0] += diff[2] * diff[1] * (values[1][1][1] - values[0][1][1]); 536 | 537 | grad[0] *= resolution_inv_; 538 | 539 | return dist; 540 | } 541 | 542 | // region VISUALIZATION 543 | 544 | void fiesta::ESDFMap::GetPointCloud(sensor_msgs::PointCloud &m, int vis_lower_bound, int vis_upper_bound) { 545 | m.header.frame_id = "world"; 546 | m.points.clear(); 547 | #ifdef HASH_TABLE 548 | for (int i = 1; i < count; i++) { 549 | if (!Exist(Vox2Idx(vox_buffer_[i])) || vox_buffer_[i].z() < vis_lower_bound || vox_buffer_[i].z() > vis_upper_bound 550 | || vox_buffer_[i].x() < min_vec_(0) || vox_buffer_[i].x() > max_vec_(0) 551 | || vox_buffer_[i].y() < min_vec_(1) || vox_buffer_[i].y() > max_vec_(1)) 552 | continue; 553 | 554 | Eigen::Vector3d pos; 555 | Vox2Pos(Eigen::Vector3i(vox_buffer_[i]), pos); 556 | 557 | geometry_msgs::Point32 p; 558 | p.x = pos(0); 559 | p.y = pos(1); 560 | p.z = pos(2); 561 | m.points.push_back(p); 562 | // cnt++; 563 | } 564 | #else 565 | for (int x = min_vec_(0); x <= max_vec_(0); ++x) 566 | for (int y = min_vec_(1); y <= max_vec_(1); ++y) 567 | for (int z = min_vec_(2); z <= max_vec_(2); ++z) { 568 | if (!Exist(Vox2Idx(Eigen::Vector3i(x, y, z))) || z < vis_lower_bound || z > vis_upper_bound) 569 | continue; 570 | 571 | Eigen::Vector3d pos; 572 | Vox2Pos(Eigen::Vector3i(x, y, z), pos); 573 | 574 | geometry_msgs::Point32 p; 575 | p.x = pos(0); 576 | p.y = pos(1); 577 | p.z = pos(2); 578 | m.points.push_back(p); 579 | } 580 | // cout << m.points.size() << endl; 581 | #endif 582 | } 583 | 584 | inline std_msgs::ColorRGBA RainbowColorMap(double h) { 585 | std_msgs::ColorRGBA color; 586 | color.a = 1; 587 | // blend over HSV-values (more colors) 588 | 589 | double s = 1.0; 590 | double v = 1.0; 591 | 592 | h -= floor(h); 593 | h *= 6; 594 | int i; 595 | double m, n, f; 596 | 597 | i = floor(h); 598 | f = h - i; 599 | if (!(i & 1)) 600 | f = 1 - f; // if i is even 601 | m = v * (1 - s); 602 | n = v * (1 - s * f); 603 | 604 | switch (i) { 605 | case 6: 606 | case 0:color.r = v; 607 | color.g = n; 608 | color.b = m; 609 | break; 610 | case 1:color.r = n; 611 | color.g = v; 612 | color.b = m; 613 | break; 614 | case 2:color.r = m; 615 | color.g = v; 616 | color.b = n; 617 | break; 618 | case 3:color.r = m; 619 | color.g = n; 620 | color.b = v; 621 | break; 622 | case 4:color.r = n; 623 | color.g = m; 624 | color.b = v; 625 | break; 626 | case 5:color.r = v; 627 | color.g = m; 628 | color.b = n; 629 | break; 630 | default:color.r = 1; 631 | color.g = 0.5; 632 | color.b = 0.5; 633 | break; 634 | } 635 | 636 | return color; 637 | } 638 | 639 | void fiesta::ESDFMap::GetSliceMarker(visualization_msgs::Marker &m, int slice, int id, 640 | Eigen::Vector4d color, double max_dist) { 641 | m.header.frame_id = "world"; 642 | m.id = id; 643 | m.type = visualization_msgs::Marker::POINTS; 644 | m.action = visualization_msgs::Marker::MODIFY; 645 | m.scale.x = resolution_; 646 | m.scale.y = resolution_; 647 | m.scale.z = resolution_; 648 | m.pose.orientation.w = 1; 649 | m.pose.orientation.x = 0; 650 | m.pose.orientation.y = 0; 651 | m.pose.orientation.z = 0; 652 | 653 | m.points.clear(); 654 | m.colors.clear(); 655 | // iterate the map 656 | std_msgs::ColorRGBA c; 657 | #ifdef HASH_TABLE 658 | // TODO: low performance, need to be modified, which is also easy 659 | for (int i = 1; i < count; i++) { 660 | int idx = Vox2Idx(vox_buffer_[i]); 661 | if (vox_buffer_[i].z() != slice || distance_buffer_[idx] < 0 || distance_buffer_[idx] >= infinity_ 662 | || vox_buffer_[i].x() < min_vec_(0) || vox_buffer_[i].x() > max_vec_(0) 663 | || vox_buffer_[i].y() < min_vec_(1) || vox_buffer_[i].y() > max_vec_(1)) 664 | continue; 665 | 666 | Eigen::Vector3d pos; 667 | Vox2Pos(Eigen::Vector3i(vox_buffer_[i]), pos); 668 | 669 | geometry_msgs::Point p; 670 | p.x = pos(0); 671 | p.y = pos(1); 672 | p.z = pos(2); 673 | c = RainbowColorMap(distance_buffer_[idx] <= max_dist ? distance_buffer_[idx] / max_dist : 1); 674 | m.points.push_back(p); 675 | m.colors.push_back(c); 676 | } 677 | #else 678 | for (int x = min_vec_(0); x <= max_vec_(0); ++x) 679 | for (int y = min_vec_(1); y <= max_vec_(1); ++y) { 680 | int z = slice; 681 | Eigen::Vector3i vox = Eigen::Vector3i(x, y, z); 682 | if (distance_buffer_[Vox2Idx(vox)] < 0 || distance_buffer_[Vox2Idx(vox)] >= infinity_) 683 | continue; 684 | 685 | Eigen::Vector3d pos; 686 | Vox2Pos(vox, pos); 687 | 688 | geometry_msgs::Point p; 689 | p.x = pos(0); 690 | p.y = pos(1); 691 | p.z = pos(2); 692 | 693 | c = RainbowColorMap( 694 | distance_buffer_[Vox2Idx(vox)] <= max_dist ? distance_buffer_[Vox2Idx(vox)] / max_dist : 1); 695 | m.points.push_back(p); 696 | m.colors.push_back(c); 697 | } 698 | #endif 699 | } 700 | 701 | // endregion 702 | 703 | // region HASH_TABLE 704 | #ifdef HASH_TABLE 705 | void fiesta::ESDFMap::IncreaseCapacity(int &old_size, int new_size) { 706 | occupancy_buffer_.resize(new_size); 707 | distance_buffer_.resize(new_size); 708 | closest_obstacle_.resize(new_size); 709 | vox_buffer_.resize(new_size); 710 | num_hit_.resize(new_size); 711 | num_miss_.resize(new_size); 712 | 713 | std::fill(distance_buffer_.begin() + old_size, distance_buffer_.end(), (double) undefined_); 714 | std::fill(occupancy_buffer_.begin() + old_size, occupancy_buffer_.end(), 0); 715 | std::fill(closest_obstacle_.begin() + old_size, closest_obstacle_.end(), 716 | Eigen::Vector3i(undefined_, undefined_, undefined_)); 717 | std::fill(vox_buffer_.begin() + old_size, vox_buffer_.end(), Eigen::Vector3i(undefined_, undefined_, undefined_)); 718 | std::fill(num_hit_.begin() + old_size, num_hit_.end(), 0); 719 | std::fill(num_miss_.begin() + old_size, num_miss_.end(), 0); 720 | 721 | head_.resize(new_size); 722 | prev_.resize(new_size); 723 | next_.resize(new_size); 724 | std::fill(head_.begin() + old_size, head_.end(), undefined_); 725 | std::fill(prev_.begin() + old_size, prev_.end(), undefined_); 726 | std::fill(next_.begin() + old_size, next_.end(), undefined_); 727 | 728 | printf("\tnew_size:\t%d\n", new_size); 729 | old_size = new_size; 730 | } 731 | 732 | int fiesta::ESDFMap::FindAndInsert(Eigen::Vector3i hash_key) { 733 | #ifdef BLOCK 734 | if (count + block_size_ > reserve_size_) 735 | IncreaseCapacity(reserve_size_, reserve_size_ * 2); 736 | #ifdef BITWISE 737 | int idx_in_block = ((hash_key.x() & (block_ - 1)) << block_bit_ << block_bit_) 738 | + ((hash_key.y() & (block_ - 1)) << block_bit_) 739 | + (hash_key.z() & (block_ - 1)); 740 | Eigen::Vector3i block_id = hash_key.unaryExpr([&](const int x) { return x >> block_bit_; }); 741 | #else 742 | int idx_in_block = (hash_key.x() % block_ + block_) % block_ * block_ * block_ 743 | + (hash_key.y() % block_ + block_) % block_ * block_ 744 | + (hash_key.z() % block_ + block_) % block_; 745 | Eigen::Vector3i block_id = hash_key.unaryExpr([&](const int x) { return x / block_ - (x % block_ < 0); }); 746 | #endif 747 | auto tmp = hash_table_.find(block_id); 748 | 749 | if (tmp == hash_table_.end()) { 750 | hash_table_.insert(std::make_pair(block_id, count)); 751 | #ifdef BITWISE 752 | block_id = block_id.unaryExpr([&](const int x) { return x << block_bit_; }); 753 | #else 754 | block_id = block_id * block_; 755 | #endif 756 | for (int i = 0; i < block_; i++) 757 | for (int j = 0; j < block_; j++) 758 | for (int k = 0; k < block_; k++) { 759 | vox_buffer_[count++] = block_id + Eigen::Vector3i(i, j, k); 760 | } 761 | return count - block_size_ + idx_in_block; 762 | } else { 763 | return tmp->second + idx_in_block; 764 | } 765 | 766 | #else 767 | 768 | if (count >= reserve_size_) 769 | IncreaseCapacity(reserve_size_, reserve_size_ * 2); 770 | auto tmp = hash_table_.find(hashKey); 771 | 772 | if (tmp == hash_table_.end()) { 773 | hash_table_.insert(std::make_pair(hashKey, count)); 774 | vox_buffer_[count] = hashKey; 775 | return count++; 776 | } else { 777 | return tmp->second; 778 | } 779 | 780 | 781 | } 782 | #endif 783 | } 784 | 785 | #endif 786 | 787 | // endregion 788 | 789 | 790 | //region LOCAL vs GLOBAL 791 | 792 | void fiesta::ESDFMap::SetUpdateRange(Eigen::Vector3d min_pos, Eigen::Vector3d max_pos, bool new_vec) { 793 | #ifndef HASH_TABLE 794 | min_pos(0) = std::max(min_pos(0), min_range_(0)); 795 | min_pos(1) = std::max(min_pos(1), min_range_(1)); 796 | min_pos(2) = std::max(min_pos(2), min_range_(2)); 797 | 798 | max_pos(0) = std::min(max_pos(0), max_range_(0)); 799 | max_pos(1) = std::min(max_pos(1), max_range_(1)); 800 | max_pos(2) = std::min(max_pos(2), max_range_(2)); 801 | #endif 802 | if (new_vec) { 803 | last_min_vec_ = min_vec_; 804 | last_max_vec_ = max_vec_; 805 | } 806 | Pos2Vox(min_pos, min_vec_); 807 | Pos2Vox( 808 | max_pos - Eigen::Vector3d(resolution_ / 2, resolution_ / 2, resolution_ / 2), 809 | max_vec_); 810 | } 811 | 812 | void fiesta::ESDFMap::SetOriginalRange() { 813 | #ifdef HASH_TABLE 814 | min_vec_ << -infinity_, -infinity_, -infinity_; 815 | max_vec_ << +infinity_, +infinity_, +infinity_; 816 | last_min_vec_ = min_vec_; 817 | last_max_vec_ = max_vec_; 818 | #else 819 | min_vec_ << 0, 0, 0; 820 | max_vec_ << grid_size_(0) - 1, grid_size_(1) - 1, grid_size_(2) - 1; 821 | last_min_vec_ = min_vec_; 822 | last_max_vec_ = max_vec_; 823 | #endif 824 | } 825 | 826 | #ifndef PROBABILISTIC 827 | void SetAway(){ 828 | SetAway(min_vec_, max_vec_); 829 | } 830 | 831 | void SetAway(Eigen::Vector3i left, Eigen::Vector3i right) { 832 | for (int i = left(0); i <= right(0); i++) 833 | for (int j = left(1); j <= right(1); j++) 834 | for (int k = left(2); k <= right(2); k++) 835 | occupancy_buffer_[Vox2Idx(Eigen::Vector3i(i, j, k))] |= 2; 836 | } 837 | void SetBack(){ 838 | SetBack(min_vec_, max_vec_); 839 | } 840 | void SetBack(Eigen::Vector3i left, Eigen::Vector3i right) { 841 | for (int i = left(0); i <= right(0); i++) 842 | for (int j = left(1); j <= right(1); j++) 843 | for (int k = left(2); k <= right(2); k++) 844 | if (occupancy_buffer_[Vox2Idx(Eigen::Vector3i(i, j, k))] >= 2) 845 | SetOccupancy(Eigen::Vector3i(i, j, k), 0); 846 | } 847 | #endif 848 | 849 | //endregion 850 | 851 | #ifdef DEBUG 852 | 853 | // region DEBUG 854 | 855 | // only for test, check whether consistent 856 | bool fiesta::ESDFMap::CheckConsistency() { 857 | #ifdef HASH_TABLE 858 | for (int ii = 1; ii < count; ii++) { 859 | int vox = Vox2Idx(vox_buffer_[ii]); 860 | if (prev_[vox] != undefined_ && next_[prev_[vox]] != vox 861 | || next_[vox] != undefined_ && prev_[next_[vox]] != vox) { 862 | std::cout << vox_buffer_[ii](0) << ' ' << vox_buffer_[ii](1) << ' ' << vox_buffer_[ii](2) << ' ' << vox 863 | << std::endl; 864 | std::cout << prev_[vox] << ' ' << next_[prev_[vox]] << ' ' << next_[vox] << ' ' << prev_[next_[vox]] 865 | << std::endl; 866 | std::cout << Vox2Idx(closest_obstacle_[next_[prev_[vox]]]) << ' ' << Vox2Idx(closest_obstacle_[vox]) 867 | << std::endl; 868 | std::cout << Vox2Idx(closest_obstacle_[prev_[vox]]) << ' ' << Vox2Idx(closest_obstacle_[next_[vox]]) 869 | << std::endl; 870 | 871 | return false; 872 | } 873 | if (prev_[vox] == undefined_ && distance_buffer_[vox] >= 0 874 | && head_[Vox2Idx(closest_obstacle_[vox])] != vox) 875 | return false; 876 | } 877 | 878 | #else 879 | 880 | for (int x = 0; x < grid_size_(0); ++x) 881 | for (int y = 0; y < grid_size_(1); ++y) 882 | for (int z = 0; z < grid_size_(2); ++z) { 883 | int vox = Vox2Idx(Eigen::Vector3i(x, y, z)); 884 | if (prev_[vox] != undefined_ && next_[prev_[vox]] != vox || 885 | next_[vox] != undefined_ && prev_[next_[vox]] != vox) { 886 | std::cout << x << ' ' << y << ' ' << z << ' ' << vox << std::endl; 887 | std::cout << prev_[vox] << ' ' << next_[prev_[vox]] << ' ' << next_[vox] << ' ' << prev_[next_[vox]] 888 | << std::endl; 889 | std::cout << Vox2Idx(closest_obstacle_[next_[prev_[vox]]]) << ' ' << Vox2Idx(closest_obstacle_[vox]) 890 | << std::endl; 891 | std::cout << Vox2Idx(closest_obstacle_[prev_[vox]]) << ' ' << Vox2Idx(closest_obstacle_[next_[vox]]) 892 | << std::endl; 893 | 894 | return false; 895 | } 896 | if (prev_[vox] == undefined_ && distance_buffer_[vox] >= 0 897 | && head_[Vox2Idx(closest_obstacle_[vox])] != vox) 898 | return false; 899 | } 900 | #endif 901 | return true; 902 | } 903 | 904 | // only for test, check between Ground Truth calculated by k-d tree 905 | bool fiesta::ESDFMap::CheckWithGroundTruth() { 906 | #ifdef HASH_TABLE 907 | Eigen::Vector3i ma = Eigen::Vector3i(0, 0, 0), mi = Eigen::Vector3i(0, 0, 0); 908 | // ESDFMap *esdf_map_ = new ESDFMap(Eigen::Vector3d(0, 0, 0), resolution, 10000000); 909 | pcl::PointCloud::Ptr cloud_(new pcl::PointCloud); 910 | cloud_->width = 0; 911 | cloud_->height = 1; 912 | for (int ii = 1; ii < count; ii++) 913 | if (Exist(ii)) 914 | cloud_->width++; 915 | cloud_->points.resize(cloud_->width * cloud_->height); 916 | int tot_ = 0; 917 | for (int ii = 1; ii < count; ii++) 918 | if (Exist(ii)) { 919 | cloud_->points[tot_].x = vox_buffer_[ii](0); 920 | cloud_->points[tot_].y = vox_buffer_[ii](1); 921 | cloud_->points[tot_].z = vox_buffer_[ii](2); 922 | tot_++; 923 | if (vox_buffer_[ii](0) > ma(0)) ma(0) = vox_buffer_[ii](0); 924 | if (vox_buffer_[ii](1) > ma(1)) ma(1) = vox_buffer_[ii](1); 925 | if (vox_buffer_[ii](2) > ma(2)) ma(2) = vox_buffer_[ii](2); 926 | if (vox_buffer_[ii](0) < mi(0)) mi(0) = vox_buffer_[ii](0); 927 | if (vox_buffer_[ii](1) < mi(1)) mi(1) = vox_buffer_[ii](1); 928 | if (vox_buffer_[ii](2) < mi(2)) mi(2) = vox_buffer_[ii](2); 929 | 930 | } 931 | std::cout << ma(0) << ' ' << ma(1) << ' ' << ma(2) << ' ' << mi(0) << ' ' << mi(1) << ' ' << mi(2) << std::endl; 932 | pcl::KdTreeFLANN kdtree; 933 | kdtree.setInputCloud(cloud_); 934 | std::vector pointIdxNKNSearch(1); 935 | std::vector pointNKNSquaredDistance(1); 936 | int cnt1 = 0, cnt2 = 0, cnt3 = 0; 937 | double ems1 = 0, ems2 = 0, max1 = 0, max2 = 0; 938 | int a[32]; 939 | std::fill(a, a + 32, 0); 940 | for (int ii = 1; ii < count; ii++) { 941 | if (distance_buffer_[ii] >= 0 && distance_buffer_[ii] < infinity_) { 942 | kdtree.nearestKSearch(pcl::PointXYZ(vox_buffer_[ii](0), vox_buffer_[ii](1), vox_buffer_[ii](2)), 1, 943 | pointIdxNKNSearch, pointNKNSquaredDistance); 944 | double tmp = sqrt(pointNKNSquaredDistance[0]) * resolution_; 945 | // if (fabs(distance_buffer_[ii] - tmp) > 1e-3) { 946 | // std::cout << vox_buffer_[ii](0) << '\t' << vox_buffer_[ii](1) << '\t' << vox_buffer_[ii](2) << '\n'; 947 | // std::cout << cloud_->points[pointIdxNKNSearch[0]].x << '\t' 948 | // << cloud_->points[pointIdxNKNSearch[0]].y << '\t' 949 | // << cloud_->points[pointIdxNKNSearch[0]].z << '\n'; 950 | // 951 | // std::cout << distance_buffer_[ii] << '\t' << tmp << '\n'; 952 | // 953 | // return false; 954 | double error = distance_buffer_[ii] - tmp; 955 | if (error > 1e-3) { 956 | cnt1++; 957 | a[(int) (error / 0.1)]++; 958 | } 959 | if (error < -1e-3) { 960 | cnt3++; 961 | } 962 | ems1 += distance_buffer_[ii] - tmp; 963 | ems2 += (distance_buffer_[ii] - tmp) * (distance_buffer_[ii] - tmp); 964 | cnt2++; 965 | max1 = std::max(distance_buffer_[ii] - tmp, max1); 966 | // } 967 | } 968 | } 969 | 970 | std::cout << count << std::endl; 971 | std::cout << ems1 << " / " << cnt1 << " = " << ems1 / cnt1 << std::endl; 972 | std::cout << ems1 << " / " << cnt2 << " = " << ems1 / cnt2 << std::endl; 973 | std::cout << ems2 << " / " << cnt1 << " = " << ems2 / cnt1 << std::endl; 974 | std::cout << ems2 << " / " << cnt2 << " = " << ems2 / cnt2 << std::endl; 975 | std::cout << "max = " << max1 << "\tcnt3 = " << cnt3 << std::endl; 976 | for (int i = 0; i < 32; i++) { 977 | std::cout << " [ " << i * 0.1 << ", " << i * 0.1 + 0.1 << " ]\t" << a[i] << std::endl; 978 | } 979 | 980 | #else 981 | // ESDFMap *esdf_map_ = new ESDFMap(Eigen::Vector3d(0, 0, 0), resolution, 10000000); 982 | pcl::PointCloud::Ptr cloud(new pcl::PointCloud); 983 | cloud->width = 0; 984 | cloud->height = 1; 985 | for (int x = 0; x < grid_size_(0); ++x) 986 | for (int y = 0; y < grid_size_(1); ++y) 987 | for (int z = 0; z < grid_size_(2); ++z) 988 | if (Exist(Vox2Idx(Eigen::Vector3i(x, y, z)))) 989 | cloud->width++; 990 | cloud->points.resize(cloud->width * cloud->height); 991 | int tot = 0; 992 | for (int x = 0; x < grid_size_(0); ++x) 993 | for (int y = 0; y < grid_size_(1); ++y) 994 | for (int z = 0; z < grid_size_(2); ++z) 995 | if (Exist(Vox2Idx(Eigen::Vector3i(x, y, z)))) { 996 | cloud->points[tot].x = x; 997 | cloud->points[tot].y = y; 998 | cloud->points[tot].z = z; 999 | tot++; 1000 | } 1001 | pcl::KdTreeFLANN kdtree; 1002 | kdtree.setInputCloud(cloud); 1003 | std::vector pointIdxNKNSearch(1); 1004 | std::vector pointNKNSquaredDistance(1); 1005 | int cnt1 = 0, cnt2 = 0, cnt3 = 0; 1006 | double ems1 = 0, ems2 = 0, max1 = 0, max2 = 0; 1007 | int a[32]; 1008 | std::fill(a, a + 32, 0); 1009 | for (int x = 0; x < grid_size_(0); ++x) 1010 | for (int y = 0; y < grid_size_(1); ++y) 1011 | for (int z = 0; z < grid_size_(2); ++z) { 1012 | int ii = Vox2Idx(Eigen::Vector3i(x, y, z)); 1013 | if (distance_buffer_[ii] >= 0 && distance_buffer_[ii] < infinity_) { 1014 | kdtree.nearestKSearch(pcl::PointXYZ(x, y, z), 1, 1015 | pointIdxNKNSearch, pointNKNSquaredDistance); 1016 | double tmp = sqrt(pointNKNSquaredDistance[0]) * resolution_; 1017 | // if (fabs(distance_buffer_[ii] - tmp) > 1e-3) { 1018 | // std::cout << vox_buffer_[ii](0) << '\t' << vox_buffer_[ii](1) << '\t' << vox_buffer_[ii](2) << '\n'; 1019 | // std::cout << cloud_->points[pointIdxNKNSearch[0]].x << '\t' 1020 | // << cloud_->points[pointIdxNKNSearch[0]].y << '\t' 1021 | // << cloud_->points[pointIdxNKNSearch[0]].z << '\n'; 1022 | // 1023 | // std::cout << distance_buffer_[ii] << '\t' << tmp << '\n'; 1024 | // 1025 | // return false; 1026 | double error = distance_buffer_[ii] - tmp; 1027 | if (error > 1e-3) { 1028 | cnt1++; 1029 | a[(int) (error / 0.1)]++; 1030 | } 1031 | if (error < -1e-3) { 1032 | cnt3++; 1033 | } 1034 | ems1 += distance_buffer_[ii] - tmp; 1035 | ems2 += (distance_buffer_[ii] - tmp) * (distance_buffer_[ii] - tmp); 1036 | cnt2++; 1037 | max1 = std::max(distance_buffer_[ii] - tmp, max1); 1038 | // } 1039 | } 1040 | } 1041 | 1042 | std::cout << grid_total_size_ << std::endl; 1043 | std::cout << ems1 << " / " << cnt1 << " = " << ems1 / cnt1 << std::endl; 1044 | std::cout << ems1 << " / " << cnt2 << " = " << ems1 / cnt2 << std::endl; 1045 | std::cout << ems2 << " / " << cnt1 << " = " << ems2 / cnt1 << std::endl; 1046 | std::cout << ems2 << " / " << cnt2 << " = " << ems2 / cnt2 << std::endl; 1047 | std::cout << "max = " << max1 << "\tcnt3 = " << cnt3 << std::endl; 1048 | for (int i = 0; i < 32; i++) { 1049 | std::cout << " [ " << i * 0.1 << ", " << i * 0.1 + 0.1 << " ]\t" << a[i] << std::endl; 1050 | } 1051 | 1052 | #endif 1053 | return true; 1054 | } 1055 | 1056 | // endregion 1057 | 1058 | #endif -------------------------------------------------------------------------------- /src/parameters.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Created by tommy on 5/2/19. 3 | // 4 | 5 | #include "parameters.h" 6 | void fiesta::Parameters::SetParameters(const ros::NodeHandle &node) { 7 | node.param("resolution", resolution_, 0.1); 8 | node.param("visualize_every_n_updates", visualize_every_n_updates_, 1); 9 | node.param("min_ray_length", min_ray_length_, 0.5); 10 | node.param("max_ray_length", max_ray_length_, 5.0); 11 | double slice_vis_level_tmp, vis_lower_bound_tmp, vis_upper_bound_tmp; 12 | 13 | node.param("slice_vis_max_dist", slice_vis_max_dist_, 2.0); 14 | node.param("slice_vis_level", slice_vis_level_tmp, 5); 15 | node.param("vis_lower_bound", vis_lower_bound_tmp, -5); 16 | node.param("vis_upper_bound", vis_upper_bound_tmp, 10); 17 | slice_vis_level_ = (int) (slice_vis_level_tmp / resolution_); 18 | vis_lower_bound_ = (int) (vis_lower_bound_tmp / resolution_); 19 | vis_upper_bound_ = (int) (vis_upper_bound_tmp / resolution_); 20 | 21 | node.param("center_x", center_x_, 322.477357419); 22 | node.param("center_y", center_y_, 237.076346481); 23 | node.param("focal_x", focal_x_, 384.458089392); 24 | node.param("focal_y", focal_y_, 383.982755697); 25 | node.param("ray_cast_num_thread", ray_cast_num_thread_, 0); 26 | double radius_x, radius_y, radius_z; 27 | node.param("radius_x", radius_x, 3.f); 28 | node.param("radius_y", radius_y, 3.f); 29 | node.param("radius_z", radius_z, 1.5f); 30 | radius_ = Eigen::Vector3d(radius_x, radius_y, radius_z); 31 | 32 | node.param("global_map", global_map_, true); 33 | node.param("global_update", global_update_, true); 34 | node.param("global_vis", global_vis_, true); 35 | if (!global_map_) 36 | global_vis_ = global_update_ = false; 37 | 38 | node.param("use_depth_filter", use_depth_filter_, true); 39 | node.param("depth_filter_tolerance", depth_filter_tolerance_, 0.1f); 40 | node.param("depth_filter_max_dist", depth_filter_max_dist_, 10.f); 41 | node.param("depth_filter_min_dist", depth_filter_min_dist_, 0.1f); 42 | node.param("depth_filter_margin", depth_filter_margin_, 0); 43 | 44 | #ifdef HASH_TABLE 45 | l_cornor_ << -100.f, -100.f, -100.f; 46 | r_cornor_ << 100.f, 100.f, 100.f; 47 | node.param("reserved_size", reserved_size_, 1000000); 48 | #else 49 | double lx, ly, lz; 50 | double rx, ry, rz; 51 | node.param("lx", lx, -20.f); 52 | node.param("ly", ly, -20.f); 53 | node.param("lz", lz, -5.f); 54 | node.param("rx", rx, 20.f); 55 | node.param("ry", ry, 20.f); 56 | node.param("rz", rz, 5.f); 57 | 58 | l_cornor_ << lx, ly, lz; 59 | r_cornor_ << rx, ry, rz; 60 | map_size_ = r_cornor_ - l_cornor_; 61 | #endif 62 | 63 | node.param("update_esdf_every_n_sec", update_esdf_every_n_sec_, 0.1f); 64 | // T_B_C_ << 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1; 65 | // T_D_B_ << 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1; 66 | 67 | // LADY_AND_COW 68 | T_B_C_ << 1, 0, 0, 0, 69 | 0, 1, 0, 0, 70 | 0, 0, 1, 0, 71 | 0, 0, 0, 1; 72 | T_D_B_ << 0.971048, -0.120915, 0.206023, 0.00114049, 73 | 0.15701, 0.973037, -0.168959, 0.0450936, 74 | -0.180038, 0.196415, 0.96385, 0.0430765, 75 | 0.0, 0.0, 0.0, 1.0; 76 | 77 | // EuRoC 78 | // T_B_C << 0.0148655429818, -0.999880929698, 0.00414029679422, -0.0216401454975, 79 | // 0.999557249008, 0.0149672133247, 0.025715529948, -0.064676986768, 80 | // -0.0257744366974, 0.00375618835797, 0.999660727178, 0.00981073058949, 81 | // 0.0, 0.0, 0.0, 1.0; 82 | // T_D_B << 0.33638, -0.01749, 0.94156, 0.06901, 83 | // -0.02078, -0.99972, -0.01114, -0.02781, 84 | // 0.94150, -0.01582, -0.33665, -0.12395, 85 | // 0.0, 0.0, 0.0, 1.0; 86 | // T_D_B = T_D_B.inverse(); 87 | 88 | #ifdef PROBABILISTIC 89 | node.param("p_hit", p_hit_, 0.70); 90 | node.param("p_miss", p_miss_, 0.35); 91 | node.param("p_min", p_min_, 0.12); 92 | node.param("p_max", p_max_, 0.97); 93 | node.param("p_occ", p_occ_, 0.80); 94 | // esdf_map_->SetParameters(p_hit_, p_miss_, p_min_, p_max_, p_occ_); 95 | #endif 96 | } -------------------------------------------------------------------------------- /src/raycast.cpp: -------------------------------------------------------------------------------- 1 | #include "raycast.h" 2 | #include 3 | #include 4 | #include 5 | 6 | int signum(int x) { 7 | return x == 0 ? 0 : x < 0 ? -1 : 1; 8 | } 9 | 10 | double mod(double value, double modulus) { 11 | return fmod(fmod(value, modulus) + modulus, modulus); 12 | } 13 | 14 | double intbound(double s, double ds) { 15 | // Find the smallest positive t such that s+t*ds is an integer. 16 | if (ds < 0) { 17 | return intbound(-s, -ds); 18 | } else { 19 | s = mod(s, 1); 20 | // problem is now s+t*ds = 1 21 | return (1 - s) / ds; 22 | } 23 | } 24 | 25 | bool RayIntersectsAABB(const Eigen::Vector3d &start, const Eigen::Vector3d &end, const Eigen::Vector3d &lb, 26 | const Eigen::Vector3d &rt) { 27 | Eigen::Vector3d dir = (end - start).normalized(); 28 | Eigen::Vector3d dirfrac(1.0f / dir.x(), 1.0f / dir.y(), 1.0f / dir.z()); 29 | 30 | // r.dirs_ is unit dirs_ vector of ray 31 | // lb is the corner of AABB with minimal coordinates - left bottom, rt is maximal corner 32 | // start is origin of ray 33 | double t1 = (lb.x() - start.x()) * dirfrac.x(); 34 | double t2 = (rt.x() - start.x()) * dirfrac.x(); 35 | double t3 = (lb.y() - start.y()) * dirfrac.y(); 36 | double t4 = (rt.y() - start.y()) * dirfrac.y(); 37 | double t5 = (lb.z() - start.z()) * dirfrac.z(); 38 | double t6 = (rt.z() - start.z()) * dirfrac.z(); 39 | 40 | double tmin = fmax(fmax(fmin(t1, t2), fmin(t3, t4)), fmin(t5, t6)); 41 | double tmax = fmin(fmin(fmax(t1, t2), fmax(t3, t4)), fmax(t5, t6)); 42 | 43 | // if tmax < 0, ray (line) is intersecting AABB, but whole AABB is behing us 44 | if (tmax < 0) { 45 | return false; 46 | } 47 | 48 | // if tmin > tmax, ray doesn't intersect AABB 49 | if (tmin > tmax) { 50 | return false; 51 | } 52 | 53 | return true; 54 | } 55 | 56 | void Raycast(const Eigen::Vector3d &start, const Eigen::Vector3d &end, 57 | const Eigen::Vector3d &min, const Eigen::Vector3d &max, 58 | std::vector *output) { 59 | // std::cout << start << ' ' << end << std::endl; 60 | // From "A Fast Voxel Traversal Algorithm for Ray Tracing" 61 | // by John Amanatides and Andrew Woo, 1987 62 | // 63 | // 64 | // Extensions to the described algorithm: 65 | // • Imposed a distance_ limit. 66 | // • The face passed through to reach the current cube is provided to 67 | // the callback. 68 | 69 | // The foundation of this algorithm is a parameterized representation of 70 | // the provided ray, 71 | // origin + t * dirs_, 72 | // except that t is not actually stored; rather, at any given point_ in the 73 | // traversal, we keep track of the *greater* t values which we would have 74 | // if we took a step sufficient to cross a cube boundary along that axis 75 | // (i.e. change the integer part of the coordinate) in the variables 76 | // tMaxX, tMaxY, and tMaxZ. 77 | 78 | // Cube containing origin point_. 79 | int x = (int) std::floor(start.x()); 80 | int y = (int) std::floor(start.y()); 81 | int z = (int) std::floor(start.z()); 82 | int endX = (int) std::floor(end.x()); 83 | int endY = (int) std::floor(end.y()); 84 | int endZ = (int) std::floor(end.z()); 85 | Eigen::Vector3d direction = (end - start); 86 | double maxDist = direction.squaredNorm(); 87 | 88 | // Break out dirs_ vector. 89 | double dx = endX - x; 90 | double dy = endY - y; 91 | double dz = endZ - z; 92 | 93 | // Direction to increment x,y,z when stepping. 94 | int stepX = (int) signum((int) dx); 95 | int stepY = (int) signum((int) dy); 96 | int stepZ = (int) signum((int) dz); 97 | 98 | // See description above. The initial values depend on the fractional 99 | // part of the origin. 100 | double tMaxX = intbound(start.x(), dx); 101 | double tMaxY = intbound(start.y(), dy); 102 | double tMaxZ = intbound(start.z(), dz); 103 | 104 | // The change in t when taking a step (always positive). 105 | double tDeltaX = ((double) stepX) / dx; 106 | double tDeltaY = ((double) stepY) / dy; 107 | double tDeltaZ = ((double) stepZ) / dz; 108 | 109 | output->clear(); 110 | 111 | // Avoids an infinite loop. 112 | if (stepX == 0 && stepY == 0 && stepZ == 0) 113 | return; 114 | 115 | double dist = 0; 116 | while (true) { 117 | 118 | if (x >= min.x() && x < max.x() && 119 | y >= min.y() && y < max.y() && 120 | z >= min.z() && z < max.z()) { 121 | output->push_back(Eigen::Vector3d(x, y, z)); 122 | 123 | dist = (Eigen::Vector3d(x, y, z) - start).squaredNorm(); 124 | 125 | if (dist > maxDist) return; 126 | 127 | if (output->size() > 1500) { 128 | std::cerr << "Error, too many racyast voxels." << std::endl; 129 | throw std::out_of_range("Too many RaycasMultithread voxels"); 130 | } 131 | } 132 | 133 | if (x == endX && y == endY && z == endZ) break; 134 | 135 | // tMaxX stores the t-value at which we cross a cube boundary along the 136 | // X axis, and similarly for Y and Z. Therefore, choosing the least tMax 137 | // chooses the closest cube boundary. Only the first case of the four 138 | // has been commented in detail. 139 | if (tMaxX < tMaxY) { 140 | if (tMaxX < tMaxZ) { 141 | // Update which cube we are now in. 142 | x += stepX; 143 | // Adjust tMaxX to the next X-oriented boundary crossing. 144 | tMaxX += tDeltaX; 145 | } else { 146 | z += stepZ; 147 | tMaxZ += tDeltaZ; 148 | } 149 | } else { 150 | if (tMaxY < tMaxZ) { 151 | y += stepY; 152 | tMaxY += tDeltaY; 153 | } else { 154 | z += stepZ; 155 | tMaxZ += tDeltaZ; 156 | } 157 | } 158 | } 159 | } -------------------------------------------------------------------------------- /src/timing.cc: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2012-2013 Simon Lynen, ASL, ETH Zurich, Switzerland 3 | * You can contact the author at 4 | * 5 | * Licensed under the Apache License, Version 2.0 (the "License"); 6 | * you may not use this file except in compliance with the License. 7 | * You may obtain a copy of the License at 8 | * 9 | * http://www.apache.org/licenses/LICENSE-2.0 10 | * 11 | * Unless required by applicable law or agreed to in writing, software 12 | * distributed under the License is distributed on an "AS IS" BASIS, 13 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 14 | * See the License for the specific language governing permissions and 15 | * limitations under the License. 16 | */ 17 | 18 | /* Adapted from Paul Furgale Schweizer Messer sm_timing*/ 19 | 20 | #include 21 | #include 22 | #include 23 | #include 24 | #include 25 | #include 26 | 27 | //#include 28 | 29 | #include "timing.h" 30 | 31 | namespace timing { 32 | 33 | const double kNumSecondsPerNanosecond = 1.e-9; 34 | 35 | Timing& Timing::Instance() { 36 | static Timing t; 37 | return t; 38 | } 39 | 40 | Timing::Timing() : maxTagLength_(0) {} 41 | 42 | Timing::~Timing() {} 43 | 44 | // Static functions to query the timers: 45 | size_t Timing::GetHandle(std::string const& tag) { 46 | std::lock_guard lock(Instance().mutex_); 47 | // Search for an existing tag. 48 | map_t::iterator i = Instance().tagMap_.find(tag); 49 | if (i == Instance().tagMap_.end()) { 50 | // If it is not there, create a tag. 51 | size_t handle = Instance().timers_.size(); 52 | Instance().tagMap_[tag] = handle; 53 | Instance().timers_.push_back(TimerMapValue()); 54 | // Track the maximum tag length to help printing a table of timing values 55 | // later. 56 | Instance().maxTagLength_ = std::max(Instance().maxTagLength_, tag.size()); 57 | return handle; 58 | } else { 59 | return i->second; 60 | } 61 | } 62 | 63 | std::string Timing::GetTag(size_t handle) { 64 | std::lock_guard lock(Instance().mutex_); 65 | std::string tag; 66 | 67 | // Perform a linear search for the tag. 68 | for (typename map_t::value_type current_tag : Instance().tagMap_) { 69 | if (current_tag.second == handle) { 70 | return current_tag.first; 71 | } 72 | } 73 | return tag; 74 | } 75 | 76 | // Class functions used for timing. 77 | Timer::Timer(size_t handle, bool constructStopped) 78 | : timing_(false), handle_(handle) { 79 | if (!constructStopped) Start(); 80 | } 81 | 82 | Timer::Timer(std::string const& tag, bool constructStopped) 83 | : timing_(false), handle_(Timing::GetHandle(tag)) { 84 | if (!constructStopped) Start(); 85 | } 86 | 87 | Timer::~Timer() { 88 | if (IsTiming()) Stop(); 89 | } 90 | 91 | void Timer::Start() { 92 | timing_ = true; 93 | time_ = std::chrono::system_clock::now(); 94 | } 95 | 96 | void Timer::Stop() { 97 | std::chrono::time_point now = 98 | std::chrono::system_clock::now(); 99 | double dt = 100 | static_cast( 101 | std::chrono::duration_cast(now - time_) 102 | .count()) * 103 | kNumSecondsPerNanosecond; 104 | 105 | Timing::Instance().AddTime(handle_, dt); 106 | timing_ = false; 107 | } 108 | 109 | bool Timer::IsTiming() const { return timing_; } 110 | 111 | void Timing::AddTime(size_t handle, double seconds) { 112 | std::lock_guard lock(Instance().mutex_); 113 | timers_[handle].acc_.Add(seconds); 114 | } 115 | 116 | double Timing::GetTotalSeconds(size_t handle) { 117 | std::lock_guard lock(Instance().mutex_); 118 | return Instance().timers_[handle].acc_.Sum(); 119 | } 120 | double Timing::GetTotalSeconds(std::string const& tag) { 121 | return GetTotalSeconds(GetHandle(tag)); 122 | } 123 | double Timing::GetMeanSeconds(size_t handle) { 124 | std::lock_guard lock(Instance().mutex_); 125 | return Instance().timers_[handle].acc_.Mean(); 126 | } 127 | double Timing::GetMeanSeconds(std::string const& tag) { 128 | return GetMeanSeconds(GetHandle(tag)); 129 | } 130 | size_t Timing::GetNumSamples(size_t handle) { 131 | return Instance().timers_[handle].acc_.TotalSamples(); 132 | } 133 | size_t Timing::GetNumSamples(std::string const& tag) { 134 | return GetNumSamples(GetHandle(tag)); 135 | } 136 | double Timing::GetVarianceSeconds(size_t handle) { 137 | std::lock_guard lock(Instance().mutex_); 138 | return Instance().timers_[handle].acc_.LazyVariance(); 139 | } 140 | double Timing::GetVarianceSeconds(std::string const& tag) { 141 | return GetVarianceSeconds(GetHandle(tag)); 142 | } 143 | double Timing::GetMinSeconds(size_t handle) { 144 | std::lock_guard lock(Instance().mutex_); 145 | return Instance().timers_[handle].acc_.Min(); 146 | } 147 | double Timing::GetMinSeconds(std::string const& tag) { 148 | return GetMinSeconds(GetHandle(tag)); 149 | } 150 | double Timing::GetMaxSeconds(size_t handle) { 151 | std::lock_guard lock(Instance().mutex_); 152 | return Instance().timers_[handle].acc_.Max(); 153 | } 154 | double Timing::GetMaxSeconds(std::string const& tag) { 155 | return GetMaxSeconds(GetHandle(tag)); 156 | } 157 | 158 | double Timing::GetHz(size_t handle) { 159 | std::lock_guard lock(Instance().mutex_); 160 | const double rolling_mean = Instance().timers_[handle].acc_.RollingMean(); 161 | // CHECK_GT(rolling_mean, 0.0); 162 | return 1.0 / rolling_mean; 163 | } 164 | 165 | double Timing::GetHz(std::string const& tag) { return GetHz(GetHandle(tag)); } 166 | 167 | std::string Timing::SecondsToTimeString(double seconds) { 168 | char buffer[256]; 169 | snprintf(buffer, sizeof(buffer), "%09.6f", seconds); 170 | return buffer; 171 | } 172 | 173 | void Timing::Print(std::ostream& out) { 174 | map_t& tagMap = Instance().tagMap_; 175 | 176 | if (tagMap.empty()) { 177 | return; 178 | } 179 | 180 | out << "SM Timing\n"; 181 | out << "-----------\n"; 182 | for (typename map_t::value_type t : tagMap) { 183 | size_t i = t.second; 184 | out.width((std::streamsize)Instance().maxTagLength_); 185 | out.setf(std::ios::left, std::ios::adjustfield); 186 | out << t.first << "\t"; 187 | out.width(7); 188 | 189 | out.setf(std::ios::right, std::ios::adjustfield); 190 | out << GetNumSamples(i) << "\t"; 191 | if (GetNumSamples(i) > 0) { 192 | out << SecondsToTimeString(GetTotalSeconds(i)) << "\t"; 193 | double meansec = GetMeanSeconds(i); 194 | double stddev = sqrt(GetVarianceSeconds(i)); 195 | out << "(" << SecondsToTimeString(meansec) << " +- "; 196 | out << SecondsToTimeString(stddev) << ")\t"; 197 | 198 | double minsec = GetMinSeconds(i); 199 | double maxsec = GetMaxSeconds(i); 200 | 201 | // The min or max are out of bounds. 202 | out << "[" << SecondsToTimeString(minsec) << "," 203 | << SecondsToTimeString(maxsec) << "]"; 204 | } 205 | out << std::endl; 206 | } 207 | } 208 | std::string Timing::Print() { 209 | std::stringstream ss; 210 | Print(ss); 211 | return ss.str(); 212 | } 213 | 214 | void Timing::Reset() { 215 | std::lock_guard lock(Instance().mutex_); 216 | Instance().tagMap_.clear(); 217 | } 218 | 219 | } // namespace timing -------------------------------------------------------------------------------- /test/test_ESDF_Map.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include "ESDF_Map.h" 3 | #include 4 | #include 5 | #include 6 | 7 | using std::cout; 8 | using std::endl; 9 | using std::vector; 10 | ros::Publisher occ_pub; 11 | ros::Publisher dist_pub; 12 | 13 | 14 | void visulization(ESDF_Map esdf_map) { 15 | visualization_msgs::Marker occ_marker; 16 | esdf_map.getOccupancyMarker(occ_marker, 0, Eigen::Vector4d(0, 0, 1.0, 0.8)); 17 | occ_pub.publish(occ_marker); 18 | // visualize distance field 19 | vector dis_markers; 20 | esdf_map.getESDFMarker(dis_markers, 0, Eigen::Vector3d(1.0, 0, 0)); 21 | for (int i = 0; i < int(dis_markers.size()); ++i) { 22 | dist_pub.publish(dis_markers[i]); 23 | ros::Duration(0.1).sleep(); 24 | } 25 | 26 | ros::Duration(1.0).sleep(); 27 | } 28 | 29 | int main(int argc, char **argv) { 30 | 31 | /* 32 | ros::init(argc, argv, "EDFS_Map_Test"); 33 | ros::NodeHandle node; 34 | 35 | occ_pub = node.advertise("EDFS_Map/occ", 1, true); 36 | dist_pub = node.advertise("EDFS_Map/dist", 1, true); 37 | ros::Duration(1.0).sleep(); 38 | 39 | ros::Time t1, t2; 40 | 41 | // create a map 42 | Eigen::Vector3d origin, map_size; 43 | origin << -5.0, -5.0, 0.0; 44 | map_size << 10.0, 10.0, 5.0; 45 | double resolution = 0.2; 46 | ESDF_Map esdf_map = ESDF_Map(origin, resolution); 47 | 48 | // set occupancy for some positions 49 | Eigen::Vector3d pos; 50 | 51 | std::vector> vp; 52 | for (int x = -4; x <= 4; x += 2) 53 | for (int y = -4; y <= 4; y += 2) 54 | vp.push_back(std::make_pair(x, y)); 55 | 56 | // insert 57 | double tt = 0; 58 | std::random_shuffle(vp.begin(), vp.end()); 59 | for (auto iter = vp.begin(); iter != vp.end(); iter++) { 60 | // a cuboid 61 | for (double z = 0.0; z < 5.0; z += 0.1) { 62 | pos << iter->first, iter->second, z; 63 | esdf_map.setOccupancy(pos, 1); 64 | } // z 65 | 66 | 67 | t1 = ros::Time::now(); 68 | esdf_map.updateESDF(); 69 | t2 = ros::Time::now(); 70 | tt += (t2 - t1).toSec(); 71 | if (!esdf_map.check()) { 72 | cout << "Fail!!!" << endl; 73 | exit(-1); 74 | } 75 | if (std::distance(vp.begin(), iter) % 25 == 24) 76 | visulization(esdf_map); 77 | } //iter 78 | 79 | cout << "Total Time Used is " << tt << "s." << endl; 80 | // visulization(esdf_map); 81 | 82 | // delete 83 | tt = 0; 84 | std::random_shuffle(vp.begin(), vp.end()); 85 | for (auto iter = vp.begin(); iter != vp.end(); iter++) { 86 | // a cuboid 87 | for (double z = 0.0; z < 5.0; z += 0.1) { 88 | pos << iter->first, iter->second, z; 89 | esdf_map.setOccupancy(pos, 0); 90 | } // z 91 | 92 | 93 | t1 = ros::Time::now(); 94 | esdf_map.updateESDF(); 95 | t2 = ros::Time::now(); 96 | tt += (t2 - t1).toSec(); 97 | if (!esdf_map.check()) { 98 | cout << "Fail!!!" << endl; 99 | exit(-1); 100 | } 101 | if (std::distance(vp.begin(), iter) % 5 == 4) 102 | visulization(esdf_map); 103 | } //iter 104 | cout << "Total Time Used is " << tt << "s." << endl; 105 | 106 | 107 | return 0; 108 | */ 109 | } -------------------------------------------------------------------------------- /test/test_fiesta.cpp: -------------------------------------------------------------------------------- 1 | #include "Fiesta.h" 2 | 3 | int main(int argc, char **argv) { 4 | ros::init(argc, argv, "FIESTA"); 5 | ros::NodeHandle node("~"); 6 | fiesta::Fiesta esdf_map(node); 7 | ros::spin(); 8 | return 0; 9 | } 10 | --------------------------------------------------------------------------------