├── .gitignore ├── CMakeLists.txt ├── LICENSE ├── README.md ├── data ├── map_hdl.pcd ├── map_mcl_3dl.pcd └── map_pub.sh ├── doc ├── demo.gif └── mcl3d_ros_image.png ├── include ├── DopeVector │ ├── DopeVector.hpp │ ├── Grid.hpp │ ├── Index.hpp │ ├── README.md │ └── internal │ │ ├── Common.hpp │ │ ├── Expression.hpp │ │ ├── Iterator.hpp │ │ ├── eigen_support │ │ └── EigenExpression.hpp │ │ └── inlines │ │ ├── DopeVector.inl │ │ ├── Expression.inl │ │ ├── Grid.inl │ │ ├── Index.inl │ │ ├── Iterator.inl │ │ └── eigen_support │ │ └── EigenExpression.inl ├── distance_transform │ ├── README.md │ ├── distance_transform.hpp │ └── inlines │ │ └── distance_transform.inl └── mcl3d_ros │ ├── DistanceField.h │ ├── IMU.h │ ├── MCL.h │ ├── Particle.h │ ├── Point.h │ └── Pose.h ├── launch ├── mcl.launch └── pc_to_df.launch ├── package.xml ├── rviz └── mcl3d_ros.rviz └── src ├── distance_field.cpp ├── imu.cpp ├── mcl.cpp ├── mcl_node.cpp ├── pc_to_df.cpp └── sensor_points_merger.cpp /.gitignore: -------------------------------------------------------------------------------- 1 | # map files 2 | data/*bin 3 | data/*yaml 4 | 5 | # Prerequisites 6 | *.d 7 | 8 | # Compiled Object files 9 | *.slo 10 | *.lo 11 | *.o 12 | *.obj 13 | 14 | # Precompiled Headers 15 | *.gch 16 | *.pch 17 | 18 | # Compiled Dynamic libraries 19 | *.so 20 | *.dylib 21 | *.dll 22 | 23 | # Fortran module files 24 | *.mod 25 | *.smod 26 | 27 | # Compiled Static libraries 28 | *.lai 29 | *.la 30 | *.a 31 | *.lib 32 | 33 | # Executables 34 | *.exe 35 | *.out 36 | *.app 37 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(mcl3d_ros) 3 | 4 | find_package(catkin REQUIRED COMPONENTS 5 | geometry_msgs 6 | nav_msgs 7 | pcl_conversions 8 | pcl_ros 9 | roscpp 10 | rospy 11 | sensor_msgs 12 | std_msgs 13 | tf 14 | ) 15 | 16 | find_package(PCL REQUIRED) 17 | 18 | find_package(OpenMP REQUIRED) 19 | if(OpenMP_FOUND) 20 | set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${OpenMP_C_FLAGS}") 21 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS}") 22 | endif() 23 | 24 | set(CMAKE_CXX_STANDARD 17) 25 | 26 | find_package(PkgConfig REQUIRED) 27 | pkg_check_modules(YAMLCPP yaml-cpp QUIET) 28 | if(NOT YAMLCPP_FOUND) 29 | find_package(yaml-cpp 0.6 REQUIRED) 30 | set(YAMLCPP_INCLUDE_DIRS ${YAML_CPP_INCLUDE_DIR}) 31 | set(YAMLCPP_LIBRARIES ${YAML_CPP_LIBRARIES}) 32 | add_definitions(-DHAVE_YAMLCPP_GT_0_5_0) 33 | else() 34 | if(YAMLCPP_VERSION VERSION_GREATER "0.5.0") 35 | add_definitions(-DHAVE_YAMLCPP_GT_0_5_0) 36 | endif() 37 | link_directories(${YAMLCPP_LIBRARY_DIRS}) 38 | endif() 39 | 40 | catkin_package( 41 | # INCLUDE_DIRS include 42 | # LIBRARIES mcl3d_ros 43 | # CATKIN_DEPENDS geometry_msgs nav_msgs pcl_conversions pcl_ros roscpp rospy sensor_msgs std_msgs tf 44 | # DEPENDS system_lib 45 | ) 46 | 47 | include_directories( 48 | ${catkin_INCLUDE_DIRS} 49 | ${PCL_INCLUDE_DIRS} 50 | ${YAMLCPP_INCLUDE_DIRS} 51 | "${PROJECT_SOURCE_DIR}/include" 52 | ) 53 | 54 | add_executable(mcl src/mcl_node.cpp src/mcl.cpp src/distance_field.cpp src/imu.cpp) 55 | target_link_libraries(mcl ${catkin_LIBRARIES} ${YAMLCPP_LIBRARIES}) 56 | 57 | add_executable(pc_to_df src/pc_to_df.cpp src/distance_field.cpp) 58 | target_link_libraries(pc_to_df ${catkin_LIBRARIES} ${YAMLCPP_LIBRARIES}) 59 | 60 | add_executable(sensor_points_merger src/sensor_points_merger.cpp) 61 | target_link_libraries(sensor_points_merger ${catkin_LIBRARIES} ${YAMLCPP_LIBRARIES}) 62 | 63 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | Apache License 2 | Version 2.0, January 2004 3 | http://www.apache.org/licenses/ 4 | 5 | TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION 6 | 7 | 1. Definitions. 8 | 9 | "License" shall mean the terms and conditions for use, reproduction, 10 | and distribution as defined by Sections 1 through 9 of this document. 11 | 12 | "Licensor" shall mean the copyright owner or entity authorized by 13 | the copyright owner that is granting the License. 14 | 15 | "Legal Entity" shall mean the union of the acting entity and all 16 | other entities that control, are controlled by, or are under common 17 | control with that entity. For the purposes of this definition, 18 | "control" means (i) the power, direct or indirect, to cause the 19 | direction or management of such entity, whether by contract or 20 | otherwise, or (ii) ownership of fifty percent (50%) or more of the 21 | outstanding shares, or (iii) beneficial ownership of such entity. 22 | 23 | "You" (or "Your") shall mean an individual or Legal Entity 24 | exercising permissions granted by this License. 25 | 26 | "Source" form shall mean the preferred form for making modifications, 27 | including but not limited to software source code, documentation 28 | source, and configuration files. 29 | 30 | "Object" form shall mean any form resulting from mechanical 31 | transformation or translation of a Source form, including but 32 | not limited to compiled object code, generated documentation, 33 | and conversions to other media types. 34 | 35 | "Work" shall mean the work of authorship, whether in Source or 36 | Object form, made available under the License, as indicated by a 37 | copyright notice that is included in or attached to the work 38 | (an example is provided in the Appendix below). 39 | 40 | "Derivative Works" shall mean any work, whether in Source or Object 41 | form, that is based on (or derived from) the Work and for which the 42 | editorial revisions, annotations, elaborations, or other modifications 43 | represent, as a whole, an original work of authorship. For the purposes 44 | of this License, Derivative Works shall not include works that remain 45 | separable from, or merely link (or bind by name) to the interfaces of, 46 | the Work and Derivative Works thereof. 47 | 48 | "Contribution" shall mean any work of authorship, including 49 | the original version of the Work and any modifications or additions 50 | to that Work or Derivative Works thereof, that is intentionally 51 | submitted to Licensor for inclusion in the Work by the copyright owner 52 | or by an individual or Legal Entity authorized to submit on behalf of 53 | the copyright owner. For the purposes of this definition, "submitted" 54 | means any form of electronic, verbal, or written communication sent 55 | to the Licensor or its representatives, including but not limited to 56 | communication on electronic mailing lists, source code control systems, 57 | and issue tracking systems that are managed by, or on behalf of, the 58 | Licensor for the purpose of discussing and improving the Work, but 59 | excluding communication that is conspicuously marked or otherwise 60 | designated in writing by the copyright owner as "Not a Contribution." 61 | 62 | "Contributor" shall mean Licensor and any individual or Legal Entity 63 | on behalf of whom a Contribution has been received by Licensor and 64 | subsequently incorporated within the Work. 65 | 66 | 2. Grant of Copyright License. Subject to the terms and conditions of 67 | this License, each Contributor hereby grants to You a perpetual, 68 | worldwide, non-exclusive, no-charge, royalty-free, irrevocable 69 | copyright license to reproduce, prepare Derivative Works of, 70 | publicly display, publicly perform, sublicense, and distribute the 71 | Work and such Derivative Works in Source or Object form. 72 | 73 | 3. Grant of Patent License. Subject to the terms and conditions of 74 | this License, each Contributor hereby grants to You a perpetual, 75 | worldwide, non-exclusive, no-charge, royalty-free, irrevocable 76 | (except as stated in this section) patent license to make, have made, 77 | use, offer to sell, sell, import, and otherwise transfer the Work, 78 | where such license applies only to those patent claims licensable 79 | by such Contributor that are necessarily infringed by their 80 | Contribution(s) alone or by combination of their Contribution(s) 81 | with the Work to which such Contribution(s) was submitted. If You 82 | institute patent litigation against any entity (including a 83 | cross-claim or counterclaim in a lawsuit) alleging that the Work 84 | or a Contribution incorporated within the Work constitutes direct 85 | or contributory patent infringement, then any patent licenses 86 | granted to You under this License for that Work shall terminate 87 | as of the date such litigation is filed. 88 | 89 | 4. Redistribution. You may reproduce and distribute copies of the 90 | Work or Derivative Works thereof in any medium, with or without 91 | modifications, and in Source or Object form, provided that You 92 | meet the following conditions: 93 | 94 | (a) You must give any other recipients of the Work or 95 | Derivative Works a copy of this License; and 96 | 97 | (b) You must cause any modified files to carry prominent notices 98 | stating that You changed the files; and 99 | 100 | (c) You must retain, in the Source form of any Derivative Works 101 | that You distribute, all copyright, patent, trademark, and 102 | attribution notices from the Source form of the Work, 103 | excluding those notices that do not pertain to any part of 104 | the Derivative Works; and 105 | 106 | (d) If the Work includes a "NOTICE" text file as part of its 107 | distribution, then any Derivative Works that You distribute must 108 | include a readable copy of the attribution notices contained 109 | within such NOTICE file, excluding those notices that do not 110 | pertain to any part of the Derivative Works, in at least one 111 | of the following places: within a NOTICE text file distributed 112 | as part of the Derivative Works; within the Source form or 113 | documentation, if provided along with the Derivative Works; or, 114 | within a display generated by the Derivative Works, if and 115 | wherever such third-party notices normally appear. The contents 116 | of the NOTICE file are for informational purposes only and 117 | do not modify the License. You may add Your own attribution 118 | notices within Derivative Works that You distribute, alongside 119 | or as an addendum to the NOTICE text from the Work, provided 120 | that such additional attribution notices cannot be construed 121 | as modifying the License. 122 | 123 | You may add Your own copyright statement to Your modifications and 124 | may provide additional or different license terms and conditions 125 | for use, reproduction, or distribution of Your modifications, or 126 | for any such Derivative Works as a whole, provided Your use, 127 | reproduction, and distribution of the Work otherwise complies with 128 | the conditions stated in this License. 129 | 130 | 5. Submission of Contributions. Unless You explicitly state otherwise, 131 | any Contribution intentionally submitted for inclusion in the Work 132 | by You to the Licensor shall be under the terms and conditions of 133 | this License, without any additional terms or conditions. 134 | Notwithstanding the above, nothing herein shall supersede or modify 135 | the terms of any separate license agreement you may have executed 136 | with Licensor regarding such Contributions. 137 | 138 | 6. Trademarks. This License does not grant permission to use the trade 139 | names, trademarks, service marks, or product names of the Licensor, 140 | except as required for reasonable and customary use in describing the 141 | origin of the Work and reproducing the content of the NOTICE file. 142 | 143 | 7. Disclaimer of Warranty. Unless required by applicable law or 144 | agreed to in writing, Licensor provides the Work (and each 145 | Contributor provides its Contributions) on an "AS IS" BASIS, 146 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or 147 | implied, including, without limitation, any warranties or conditions 148 | of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A 149 | PARTICULAR PURPOSE. You are solely responsible for determining the 150 | appropriateness of using or redistributing the Work and assume any 151 | risks associated with Your exercise of permissions under this License. 152 | 153 | 8. Limitation of Liability. In no event and under no legal theory, 154 | whether in tort (including negligence), contract, or otherwise, 155 | unless required by applicable law (such as deliberate and grossly 156 | negligent acts) or agreed to in writing, shall any Contributor be 157 | liable to You for damages, including any direct, indirect, special, 158 | incidental, or consequential damages of any character arising as a 159 | result of this License or out of the use or inability to use the 160 | Work (including but not limited to damages for loss of goodwill, 161 | work stoppage, computer failure or malfunction, or any and all 162 | other commercial damages or losses), even if such Contributor 163 | has been advised of the possibility of such damages. 164 | 165 | 9. Accepting Warranty or Additional Liability. While redistributing 166 | the Work or Derivative Works thereof, You may choose to offer, 167 | and charge a fee for, acceptance of support, warranty, indemnity, 168 | or other liability obligations and/or rights consistent with this 169 | License. However, in accepting such obligations, You may act only 170 | on Your own behalf and on Your sole responsibility, not on behalf 171 | of any other Contributor, and only if You agree to indemnify, 172 | defend, and hold each Contributor harmless for any liability 173 | incurred by, or claims asserted against, such Contributor by reason 174 | of your accepting any such warranty or additional liability. 175 | 176 | END OF TERMS AND CONDITIONS 177 | 178 | APPENDIX: How to apply the Apache License to your work. 179 | 180 | To apply the Apache License to your work, attach the following 181 | boilerplate notice, with the fields enclosed by brackets "[]" 182 | replaced with your own identifying information. (Don't include 183 | the brackets!) The text should be enclosed in the appropriate 184 | comment syntax for the file format. We also recommend that a 185 | file or class name and description of purpose be included on the 186 | same "printed page" as the copyright notice for easier 187 | identification within third-party archives. 188 | 189 | Copyright [yyyy] [name of copyright owner] 190 | 191 | Licensed under the Apache License, Version 2.0 (the "License"); 192 | you may not use this file except in compliance with the License. 193 | You may obtain a copy of the License at 194 | 195 | http://www.apache.org/licenses/LICENSE-2.0 196 | 197 | Unless required by applicable law or agreed to in writing, software 198 | distributed under the License is distributed on an "AS IS" BASIS, 199 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 200 | See the License for the specific language governing permissions and 201 | limitations under the License. 202 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # mcl3d_ros 2 | mcl3d_ros is a ROS package of 3D-LiDAR-based Monte Carlo localization (MCL) and also contains localization methods using distance-field-based optimization and extended-Kalman-filter-based fusion. The localizers work without INS such as odometry. Of course, INS can be available and it improves localization performance (current version does not support IMU). 3 | 4 | ![](doc/demo.gif) 5 | 6 | 7 | 8 | # Main characteristics 9 | 10 | mcl3d_ros can efficiently perform 3D-LiDAR-based MCL. To achieve it, mcl3d_ros uses measurement model optimization using a distance field and fusion of it with MCL via importance sampling. In short, mcl3d_ros uses two probabilistic distributions to sample the particles and fuses them. For more details, please refer this [preprint](https://arxiv.org/abs/2303.00216). 11 | 12 | ``` 13 | @misc{akai_arxiv2023_mcl3d, 14 | url = {https://arxiv.org/abs/2303.00216}, 15 | author = {Akai, Naoki}, 16 | title = {Efficient Solution to {3D-LiDAR}-based {Monte Carlo} Localization with Fusion of Measurement Model Optimization via Importance Sampling}, 17 | publisher = {arXiv}, 18 | year = {2023} 19 | } 20 | ``` 21 | 22 | 23 | 24 | ![](doc/mcl3d_ros_image.png) 25 | 26 | 27 | 28 | 29 | 30 | # Install 31 | 32 | ``` 33 | $ cd /your/catkin_ws/src 34 | $ git clone https://github.com/NaokiAkai/mcl3d_ros.git 35 | $ cd /your/catkin_ws 36 | $ catkin_make 37 | $ source devel/setup.bash 38 | ``` 39 | 40 | We tested mcl3d_ros on Ubuntu 20.04 with Noetic. 41 | 42 | 43 | 44 | 45 | 46 | # How to use 47 | 48 | Please first prepare a pcd file of your target environment and build a distance field of the pcd file. 49 | 50 | ``` 51 | $ roslaunch mcl3d_ros pc_to_df.launch pcd_file:=/your/pcd/file.pcd yaml_file_path:=/your/yaml/file.yaml map_file_name:=your_map_name.bin 52 | ``` 53 | 54 | The distance field is saved as a binary file and its parameters are saved at the given yaml file. A distance field can also be built from a point cloud published as a ROS message. 55 | 56 | Then, localization can be executed. 57 | 58 | ``` 59 | $ roslaunch mcl3d_ros mcl.launch map_yaml_file:=/your/yaml/file.yaml 60 | ``` 61 | 62 | A configuration file for rviz is prepared in the rviz directory. 63 | 64 | ``` 65 | $ rviz -d rviz/mcl3d_ros.rviz 66 | ``` 67 | 68 | Note that other map formats are not supported currently. 69 | 70 | 71 | 72 | # Parameters 73 | 74 | There are launch files at the launch directory. Descriptions of all the parameters can be seen in the launch files. 75 | 76 | ### 77 | 78 | # License 79 | 80 | This software is subject to the [Apache 2.0](http://www.apache.org/licenses/LICENSE-2.0.html) License. 81 | -------------------------------------------------------------------------------- /data/map_hdl.pcd: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/NaokiAkai/mcl3d_ros/8a5cf9ebe682ac495f8720eae1edfd754ab9e2b5/data/map_hdl.pcd -------------------------------------------------------------------------------- /data/map_pub.sh: -------------------------------------------------------------------------------- 1 | #! /bin/bash 2 | 3 | pcd_file=$1 4 | publish_cycle=10.0 5 | frame_id="map" 6 | topic_name="/map_points" 7 | 8 | rosrun pcl_ros pcd_to_pointcloud $pcd_file $publish_cycle _frame_id:=$frame_id /cloud_pcd:=$topic_name 9 | -------------------------------------------------------------------------------- /doc/demo.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/NaokiAkai/mcl3d_ros/8a5cf9ebe682ac495f8720eae1edfd754ab9e2b5/doc/demo.gif -------------------------------------------------------------------------------- /doc/mcl3d_ros_image.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/NaokiAkai/mcl3d_ros/8a5cf9ebe682ac495f8720eae1edfd754ab9e2b5/doc/mcl3d_ros_image.png -------------------------------------------------------------------------------- /include/DopeVector/Grid.hpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2016 Giorgio Marcias & Maurizio Kovacic 2 | // 3 | // This source code is part of DopeVector header library 4 | // and it is subject to Apache 2.0 License. 5 | // 6 | // Author: Giorgio Marcias 7 | // email: marcias.giorgio@gmail.com 8 | // Author: Maurizio Kovacic 9 | // email: maurizio.kovacic@gmail.com 10 | 11 | #ifndef Grid_hpp 12 | #define Grid_hpp 13 | 14 | #include 15 | #include 16 | 17 | namespace dope { 18 | 19 | /** 20 | * @brief The Grid class describes a D-dimensional grid, containing elements 21 | * of a specified type. Each element could be accessed by its regualr 22 | * index as if it was inside a array-like container. Moreover they 23 | * can be accessed by their index and through iterator. 24 | * It is possible also to extract slices, windows and such from the 25 | * grid using its functions. 26 | * @param T Type of the data to be stored. 27 | * @param Dimension Dimension of the grid. 28 | * @param Allocator Allocator to be used to store the data. 29 | * @param Args Parameter allowing to make specialized grid for 30 | * known fixed sizes. 31 | */ 32 | template < typename T, SizeType Dimension, class Allocator = std::allocator< T > > 33 | class Grid : public DopeVector< T, Dimension > { 34 | public: 35 | 36 | //////////////////////////////////////////////////////////////////////// 37 | // TYPEDEFS 38 | //////////////////////////////////////////////////////////////////////// 39 | 40 | typedef typename DopeVector::IndexD IndexD; 41 | typedef std::vector Data; 42 | 43 | //////////////////////////////////////////////////////////////////////// 44 | 45 | 46 | 47 | //////////////////////////////////////////////////////////////////////// 48 | // CONSTRUCTORS 49 | //////////////////////////////////////////////////////////////////////// 50 | 51 | /** 52 | * @brief Default constructor. 53 | */ 54 | inline Grid() = default; 55 | 56 | /** 57 | * @brief Initializer contructor. 58 | * @param size Sizes of the D-dimensional grid. 59 | * @param default_value Default value assigned to the grid 60 | * elements. 61 | */ 62 | inline explicit Grid(const IndexD &size, const T &default_value = T()); 63 | 64 | /** 65 | * @brief Initializer contructor. 66 | * @param size Sizes of the D-dimensional grid. 67 | * @param order A permutation of the matrix indices that 68 | * allows the use of different access (e.g. 69 | * grid[x][y][z] instead of grid[z][y][x]). 70 | * @param default_value Default value assigned to the grid 71 | * elements. 72 | */ 73 | inline explicit Grid(const IndexD &size, const IndexD &order, const T &default_value = T()); 74 | 75 | /** 76 | * @brief Initializer contructor. 77 | * @param size Sizes of the D-dimensional grid. The 78 | * grid will be an hypercube. 79 | * @param default_value Default value assigned to the grid 80 | * elements. 81 | */ 82 | inline explicit Grid(const SizeType size, const T &default_value = T()); 83 | 84 | /** 85 | * @brief Initializer contructor. 86 | * @param size Sizes of the D-dimensional grid. The 87 | * grid will be an hypercube. 88 | * @param order A permutation of the matrix indices that 89 | * allows the use of different access (e.g. 90 | * grid[x][y][z] instead of grid[z][y][x]) 91 | * @param default_value Default value assigned to the grid 92 | * elements. 93 | */ 94 | inline explicit Grid(const SizeType size, const IndexD &order, const T &default_value = T()); 95 | 96 | /** 97 | * @brief Copy constructor. 98 | */ 99 | inline explicit Grid(const Grid &o); 100 | 101 | /** 102 | * @brief Move constructor. 103 | */ 104 | inline explicit Grid(Grid &&) = default; 105 | 106 | //////////////////////////////////////////////////////////////////////// 107 | 108 | 109 | 110 | //////////////////////////////////////////////////////////////////////// 111 | // DESTRUCTOR 112 | //////////////////////////////////////////////////////////////////////// 113 | 114 | /** 115 | * @brief Default destructor. 116 | */ 117 | virtual inline ~Grid(); 118 | 119 | //////////////////////////////////////////////////////////////////////// 120 | 121 | 122 | 123 | //////////////////////////////////////////////////////////////////////// 124 | // DATA 125 | //////////////////////////////////////////////////////////////////////// 126 | 127 | /** 128 | * @brief Give access to the first element of the grid. 129 | * @return The const pointer to the first element of the grid. 130 | */ 131 | inline const T * data() const; 132 | 133 | /** 134 | * @brief Give access to the first element of the grid. 135 | * @return The pointer to the first element of the grid. 136 | */ 137 | inline T * data(); 138 | 139 | //////////////////////////////////////////////////////////////////////// 140 | 141 | 142 | 143 | //////////////////////////////////////////////////////////////////////// 144 | // CONVERSIONS 145 | //////////////////////////////////////////////////////////////////////// 146 | 147 | /** 148 | * @brief Convert the grid to a std::vector< T, Allocator >. 149 | * @return A const reference to the underneath data of the grid. 150 | */ 151 | inline const Data & to_stdvector() const; 152 | 153 | /** 154 | * @brief Convert the grid to a std::vector< T, Allocator >. 155 | * @return A reference to the underneath data of the grid. 156 | */ 157 | inline Data & to_stdvector(); 158 | 159 | /** 160 | * @brief Cast the grid to a std::vectot< T, Allocator >. 161 | * @return A const reference to the underneath data of the grid. 162 | */ 163 | explicit inline operator const Data&() const; 164 | 165 | /** 166 | * @brief Cast the grid to a std::vectot< T, Allocator >. 167 | * @return A reference to the underneath data of the grid. 168 | */ 169 | explicit inline operator Data&(); 170 | 171 | 172 | 173 | //////////////////////////////////////////////////////////////////////// 174 | // INFORMATION 175 | //////////////////////////////////////////////////////////////////////// 176 | 177 | /** 178 | * @brief Check the number of elements in the grid. 179 | * @return true if the grid has no elements. false otherwise. 180 | */ 181 | inline bool empty() const; 182 | 183 | /** 184 | * @brief Check if a given grid is equal to this. 185 | * @return true if the grids are equal. false otherwise. 186 | * @note Grid sizes must match as well. 187 | */ 188 | inline bool operator==(const Grid &o) const; 189 | 190 | //////////////////////////////////////////////////////////////////////// 191 | 192 | 193 | 194 | //////////////////////////////////////////////////////////////////////// 195 | // RESET 196 | //////////////////////////////////////////////////////////////////////// 197 | 198 | /** 199 | * @brief Erase all the grid elements, setting it empty. 200 | */ 201 | inline void clear(); 202 | 203 | /** 204 | * @brief Set all the grid elements to a given value. 205 | * @param default_value The value all the elements are set to. 206 | */ 207 | inline void reset(const T &default_value = T()); 208 | 209 | /** 210 | * @brief Resize the container. 211 | * @param size Sizes of the D-dimensional grid. 212 | * @param default_value Default value assigned to the grid 213 | * elements. 214 | * @note For performance reasons, data is not kept. See 215 | * conservativeResize. 216 | */ 217 | inline void resize(const IndexD &size, const T &default_value = T()); 218 | 219 | /** 220 | * @brief Resize the container. 221 | * @param size Sizes of the D-dimensional grid. 222 | * @param order A permutation of the matrix indices that 223 | * allows the use of different access (e.g. 224 | * grid[x][y][z] instead of grid[z][y][x]). 225 | * @param default_value Default value assigned to the grid 226 | * elements. 227 | * @note For performance reasons, data is not kept. See 228 | * conservativeResize. 229 | */ 230 | inline void resize(const IndexD &size, const IndexD &order, const T &default_value = T()); 231 | 232 | /** 233 | * @brief Resize the container. 234 | * @param size Sizes of the D-dimensional grid. The 235 | * grid will be an hypercube. 236 | * @param default_value Default value assigned to the grid 237 | * elements. 238 | * @note For performance reasons, data is not kept. See 239 | * conservativeResize. 240 | */ 241 | inline void resize(const SizeType size, const T &default_value = T()); 242 | 243 | /** 244 | * @brief Resize the container. 245 | * @param size Sizes of the D-dimensional grid. The 246 | * grid will be an hypercube. 247 | * @param order A permutation of the matrix indices that 248 | * allows the use of different access (e.g. 249 | * grid[x][y][z] instead of grid[z][y][x]) 250 | * @param default_value Default value assigned to the grid 251 | * elements. 252 | * @note For performance reasons, data is not kept. See 253 | * conservativeResize. 254 | */ 255 | inline void resize(const SizeType size, const IndexD &order, const T & default_value = T()); 256 | 257 | /** 258 | * @brief Resize the container assigning values. 259 | * @param size Sizes of the D-dimensional grid. 260 | * @param default_value Default value assigned to the grid 261 | * elements. 262 | */ 263 | inline void assign(const IndexD &size, const T &default_value); 264 | 265 | /** 266 | * @brief Resize the container assigning values. 267 | * @param size Sizes of the D-dimensional grid. 268 | * @param order A permutation of the matrix indices that 269 | * allows the use of different access (e.g. 270 | * grid[x][y][z] instead of grid[z][y][x]). 271 | * @param default_value Default value assigned to the grid 272 | * elements. 273 | */ 274 | inline void assign(const IndexD &size, const IndexD &order, const T &default_value); 275 | 276 | /** 277 | * @brief Resize the container assigning values. 278 | * @param size Sizes of the D-dimensional grid. The 279 | * grid will be an hypercube. 280 | * @param default_value Default value assigned to the grid 281 | * elements. 282 | */ 283 | inline void assign(const SizeType size, const T &default_value); 284 | 285 | /** 286 | * @brief Resize the container assigning values. 287 | * @param size Sizes of the D-dimensional grid. The 288 | * grid will be an hypercube. 289 | * @param order A permutation of the matrix indices that 290 | * allows the use of different access (e.g. 291 | * grid[x][y][z] instead of grid[z][y][x]) 292 | * @param default_value Default value assigned to the grid 293 | * elements. 294 | */ 295 | inline void assign(const SizeType size, const IndexD &order, const T &default_value); 296 | 297 | /** 298 | * @brief Resize the container. 299 | * @param size Sizes of the D-dimensional grid. 300 | * @param default_value Default value assigned to the grid 301 | * elements. 302 | * @note Data is kept as long as possible. That is, the block at the 303 | * origin with minimum size that fits into the new grid is 304 | * copied. See resize for faster resizing. 305 | */ 306 | inline void conservativeResize(const IndexD &size, const T &default_value = T()); 307 | 308 | /** 309 | * @brief Resize the container. 310 | * @param size Sizes of the D-dimensional grid. 311 | * @param order A permutation of the matrix indices that 312 | * allows the use of different access (e.g. 313 | * grid[x][y][z] instead of grid[z][y][x]). 314 | * @param default_value Default value assigned to the grid 315 | * elements. 316 | * @note Data is kept as long as possible. That is, the block at the 317 | * origin with minimum size that fits into the new grid is 318 | * copied. See resize for faster resizing. 319 | */ 320 | inline void conservativeResize(const IndexD &size, const IndexD &order, const T &default_value = T()); 321 | 322 | /** 323 | * @brief Resize the container. 324 | * @param size Sizes of the D-dimensional grid. The 325 | * grid will be an hypercube. 326 | * @param default_value Default value assigned to the grid 327 | * elements. 328 | * @note Data is kept as long as possible. That is, the block at the 329 | * origin with minimum size that fits into the new grid is 330 | * copied. See resize for faster resizing. 331 | */ 332 | inline void conservativeResize(const SizeType size, const T &default_value = T()); 333 | 334 | /** 335 | * @brief Resize the container. 336 | * @param size Sizes of the D-dimensional grid. The 337 | * grid will be an hypercube. 338 | * @param order A permutation of the matrix indices that 339 | * allows the use of different access (e.g. 340 | * grid[x][y][z] instead of grid[z][y][x]) 341 | * @param default_value Default value assigned to the grid 342 | * elements. 343 | * @note Data is kept as long as possible. That is, the block at the 344 | * origin with minimum size that fits into the new grid is 345 | * copied. See resize for faster resizing. 346 | */ 347 | inline void conservativeResize(const SizeType size, const IndexD &order, const T &default_value = T()); 348 | 349 | //////////////////////////////////////////////////////////////////////// 350 | 351 | 352 | 353 | //////////////////////////////////////////////////////////////////////// 354 | // ASSIGNMENTS 355 | //////////////////////////////////////////////////////////////////////// 356 | 357 | /** 358 | * @brief Copy assignment operator. 359 | */ 360 | inline Grid & operator=(const Grid &o); 361 | 362 | /** 363 | * @brief Move assignment operator. 364 | */ 365 | inline Grid & operator=(Grid &&o) = default; 366 | 367 | #ifdef DOPE_USE_RTTI 368 | /** 369 | * @brief Copies all single elements from o to this matrix. 370 | * @param o The matrix to copy from. 371 | */ 372 | inline void import(const DopeVector &o) override; 373 | #endif 374 | 375 | /** 376 | * @brief Swap this with a given grid. 377 | * @note Swap operation is performend in O( 1 ). 378 | */ 379 | virtual inline void swap(Grid &o); 380 | 381 | //////////////////////////////////////////////////////////////////////// 382 | 383 | protected: 384 | Data _data; ///< Elements of the grid. 385 | 386 | private: 387 | // hyde some methods from DopeVector 388 | using DopeVector::reset; 389 | }; 390 | 391 | 392 | /** 393 | * @brief Alias for a grid using the default allocator. 394 | */ 395 | template < typename T, SizeType Dimension > 396 | using StandardGrid = Grid; 397 | 398 | /** 399 | * @brief Alias for a 2D grid using the default allocator. 400 | */ 401 | template < typename T > 402 | using StandardGrid2D = Grid(2)>; 403 | 404 | /** 405 | * @brief Alias for a 3D grid using the default allocator. 406 | */ 407 | template < typename T > 408 | using StandardGrid3D = Grid(3)>; 409 | 410 | } 411 | 412 | #include 413 | 414 | #endif // Grid_hpp 415 | -------------------------------------------------------------------------------- /include/DopeVector/Index.hpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2016 Giorgio Marcias & Maurizio Kovacic 2 | // 3 | // This source code is part of DopeVector header library 4 | // and it is subject to Apache 2.0 License. 5 | // 6 | // Author: Giorgio Marcias 7 | // email: marcias.giorgio@gmail.com 8 | // Author: Maurizio Kovacic 9 | // email: maurizio.kovacic@gmail.com 10 | 11 | #ifndef Index_hpp 12 | #define Index_hpp 13 | 14 | #include 15 | #include 16 | 17 | namespace dope { 18 | 19 | namespace internal { 20 | 21 | template < typename T, typename ... Rest > 22 | struct SizeTypesCheck { 23 | static constexpr bool value = (std::is_arithmetic::value) && (SizeTypesCheck::value); 24 | }; 25 | 26 | template < typename T > 27 | struct SizeTypesCheck { 28 | static constexpr bool value = std::is_arithmetic::value; 29 | }; 30 | 31 | } 32 | 33 | /** 34 | * @brief The Index class defines a D-dimensional index, used to refer to an 35 | * element contained in a DopeVector. 36 | */ 37 | template < SizeType Dimension > 38 | class Index : public std::array, public internal::StaticArrayExpression, SizeType, Dimension> { 39 | public: 40 | 41 | //////////////////////////////////////////////////////////////////////// 42 | // CONSTRUCTORS 43 | //////////////////////////////////////////////////////////////////////// 44 | 45 | /** 46 | * @brief Default constructor. 47 | */ 48 | explicit Index() = default; 49 | 50 | /** 51 | * @brief Copy constructor. 52 | */ 53 | explicit Index(const Index &) = default; 54 | 55 | /** 56 | * @brief Move constructor. 57 | */ 58 | explicit Index(Index &&) = default; 59 | 60 | /** 61 | * @brief Initializer constructor. 62 | * @param size0 The first component of the index. 63 | * @param sizes The remaining components of the index, 64 | * if its dimension is greater than one. 65 | */ 66 | template < typename ... Sizes, class = typename std::enable_if::value>::type > 67 | explicit inline Index(const SizeType size0, Sizes &&...sizes); 68 | 69 | /** 70 | * @brief Initializer constructor. 71 | * @param il A list of components. 72 | */ 73 | inline Index(const std::initializer_list &il); 74 | 75 | /** 76 | * @brief Initializer constructor. 77 | * @param e The result of an expression. 78 | */ 79 | template < class E > 80 | inline Index(const internal::StaticArrayExpression &e); 81 | 82 | #ifdef DOPE_USE_EIGEN 83 | /** 84 | * @brief Initializer constructor. 85 | * @param e The result of an Eigen expression. 86 | */ 87 | template < class Derived > 88 | inline Index(const Eigen::MatrixBase &e); 89 | #endif 90 | 91 | //////////////////////////////////////////////////////////////////////// 92 | 93 | 94 | 95 | //////////////////////////////////////////////////////////////////////// 96 | // ASSIGNMENTS 97 | //////////////////////////////////////////////////////////////////////// 98 | 99 | /** 100 | * @brief Copy assignment operator. 101 | */ 102 | Index & operator= (const Index &) = default; 103 | 104 | /** 105 | * @brief Move assignment operator. 106 | */ 107 | Index & operator= (Index &&) = default; 108 | 109 | /** 110 | * @brief List assignment operator. 111 | */ 112 | inline Index & operator= (const std::initializer_list &il); 113 | 114 | //////////////////////////////////////////////////////////////////////// 115 | 116 | 117 | 118 | //////////////////////////////////////////////////////////////////////// 119 | // RELATIONAL OPERATORS 120 | //////////////////////////////////////////////////////////////////////// 121 | 122 | /** 123 | * @brief Returns true if a given index is equal to \p this. 124 | * @param o A index. 125 | * @return \p true if the components of the given index are equal to 126 | * the components of \p this. \p false otherwise. 127 | * @note This function was made for maintaining compatibility between 128 | * Eigen indices and built-in indices. 129 | */ 130 | inline bool isApprox(const Index &o) const; 131 | 132 | //////////////////////////////////////////////////////////////////////// 133 | 134 | 135 | 136 | //////////////////////////////////////////////////////////////////////// 137 | // EXPRESSION OPERATORS 138 | //////////////////////////////////////////////////////////////////////// 139 | 140 | /** 141 | * @brief Expression assignment operator. 142 | */ 143 | template < class E > 144 | inline Index& operator=(const internal::StaticArrayExpression &e); 145 | 146 | /** 147 | * @brief Expression addition operator. 148 | */ 149 | template < class E > 150 | inline Index& operator+=(const internal::StaticArrayExpression &e); 151 | 152 | /** 153 | * @brief Expression difference operator. 154 | */ 155 | template < class E > 156 | inline Index& operator-=(const internal::StaticArrayExpression &e); 157 | 158 | /** 159 | * @brief Expression product operator. 160 | * @warning This operator is not Eigen compatible. In case of using Eigen, please use 161 | * cwiseProduct. 162 | */ 163 | template < class E > 164 | inline Index& operator*=(const internal::StaticArrayExpression &e); 165 | 166 | /** 167 | * @brief Expression quotient operator. 168 | * @warning This operator is not Eigen compatible. In case of using Eigen, please use 169 | * cwiseQuotient. 170 | */ 171 | template < class E > 172 | inline Index& operator/=(const internal::StaticArrayExpression &e); 173 | 174 | /** 175 | * @brief Expression modulus operator. 176 | * @warning This operator is not Eigen compatible. 177 | */ 178 | template < class E > 179 | inline Index& operator%=(const internal::StaticArrayExpression &e); 180 | 181 | #ifdef DOPE_USE_EIGEN 182 | /** 183 | * @brief Eigen expression assignment operator. 184 | */ 185 | template < class Derived > 186 | inline Index& operator=(const Eigen::MatrixBase &e); 187 | #endif 188 | 189 | //////////////////////////////////////////////////////////////////////// 190 | 191 | 192 | 193 | //////////////////////////////////////////////////////////////////////// 194 | // ASSIGNMENTS 195 | //////////////////////////////////////////////////////////////////////// 196 | 197 | /** 198 | * @brief Performs the sum of all the components of \p this. 199 | * @return The sum of the components of \p this. 200 | */ 201 | inline SizeType sum() const; 202 | 203 | /** 204 | * @brief Performs the product of all the components of \p this. 205 | * @return The product of the components of \p this. 206 | */ 207 | inline SizeType prod() const; 208 | 209 | //////////////////////////////////////////////////////////////////////// 210 | 211 | 212 | 213 | //////////////////////////////////////////////////////////////////////// 214 | // ASSIGNMENTS 215 | //////////////////////////////////////////////////////////////////////// 216 | 217 | /** 218 | * @brief Returns the index {0, ..., 0}. 219 | * @return A index contaning only zeros. 220 | * @note This function was made for maintaining compatibility between 221 | * Eigen indices and built-in indices. 222 | */ 223 | static inline constexpr Index Zero(); 224 | 225 | /** 226 | * @brief Returns the index {1, ..., 1}. 227 | * @return A index containing only ones. 228 | * @note This function was made for maintaining compatibility between 229 | * Eigen indices and built-in indices. 230 | */ 231 | static inline constexpr Index Ones(); 232 | 233 | /** 234 | * @brief Returns the index {value, ... , value}. 235 | * @param value The value each component is set to. 236 | * @return A index with all entries equal to the given value. 237 | * @note This function was made for maintaining compatibility between 238 | * Eigen indices and built-in indices. 239 | */ 240 | static inline constexpr Index Constant(const SizeType value); 241 | 242 | //////////////////////////////////////////////////////////////////////// 243 | 244 | }; 245 | 246 | //////////////////////////////////////////////////////////////////////// 247 | // TYPEDEFS 248 | //////////////////////////////////////////////////////////////////////// 249 | 250 | typedef Index<1> Index1; ///< Alias for 1-dimensional indices 251 | typedef Index<2> Index2; ///< Alias for 2-dimensional indices 252 | typedef Index<3> Index3; ///< Alias for 3-dimensional indices 253 | typedef Index<4> Index4; ///< Alias for 4-dimensional indices 254 | 255 | //////////////////////////////////////////////////////////////////////// 256 | 257 | 258 | 259 | //////////////////////////////////////////////////////////////////////// 260 | // UTILITY FUNCTIONS 261 | //////////////////////////////////////////////////////////////////////// 262 | 263 | /** 264 | * @brief Returns the linearized index value, according to the given 265 | * range. 266 | * @param index The index to be linearized. 267 | * @param range The range of the linearization. 268 | * @return An unsigned integer value defining the linear version of 269 | * the given index. 270 | * @note This function was kept outside the Index class for maintaining 271 | * compatibility between Eigen indices and built-in indices. 272 | */ 273 | template < SizeType Dimension > 274 | static inline SizeType to_position(const Index &index, const Index &range); 275 | 276 | /** 277 | * @brief Returns the linearized index value, according to the given offset. 278 | * @param index The index to be linearized. 279 | * @param offset The offset in each dimension. 280 | * @return An unsigned integer value defining the linear version of 281 | * the given index. 282 | * @note This function was kept outside the Index class for maintaining 283 | * compatibility between Eigen indices and built-in indices. 284 | */ 285 | template < SizeType Dimension > 286 | static inline SizeType to_positionFromOffset(const Index &index, const Index &offset); 287 | 288 | /** 289 | * @brief Returns the index form of a linear position, according to the given 290 | * range. 291 | * @param position The position to be put in index form. 292 | * @param range The range of the index. 293 | * @return An index built from the given position. 294 | * @note This function was kept outside the Index class for maintaining 295 | * compatibility between Eigen indices and built-in indices. 296 | */ 297 | template < SizeType Dimension > 298 | static inline Index to_index(const SizeType position, const Index &range); 299 | 300 | /** 301 | * @brief Returns the index form of a linear position, according to the given 302 | * offset. 303 | * @param position The position to be put in index form. 304 | * @param offset The offset in each dimension. 305 | * @return An index built from the given position. 306 | * @note This function was kept outside the Index class for maintaining 307 | * compatibility between Eigen indices and built-in indices. 308 | */ 309 | template < SizeType Dimension > 310 | static inline Index to_indexFromOffset(const SizeType position, const Index &offset); 311 | 312 | //////////////////////////////////////////////////////////////////////// 313 | 314 | } 315 | 316 | #include 317 | 318 | #endif // Index_hpp 319 | -------------------------------------------------------------------------------- /include/DopeVector/README.md: -------------------------------------------------------------------------------- 1 | This program was created by [giorgiomarcias](https://github.com/giorgiomarcias). 2 | 3 | https://github.com/giorgiomarcias/DopeVector 4 | 5 | -------------------------------------------------------------------------------- /include/DopeVector/internal/Common.hpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2016 Giorgio Marcias & Maurizio Kovacic 2 | // 3 | // This source code is part of DopeVector header library 4 | // and it is subject to Apache 2.0 License. 5 | // 6 | // Author: Giorgio Marcias 7 | // email: marcias.giorgio@gmail.com 8 | // Author: Maurizio Kovacic 9 | // email: maurizio.kovacic@gmail.com 10 | 11 | #ifndef Common_hpp 12 | #define Common_hpp 13 | 14 | #include 15 | 16 | #ifndef _SIZETYPE_ 17 | #define DOPE_SIZETYPE std::size_t 18 | #endif 19 | 20 | namespace dope { 21 | 22 | /** 23 | * @brief SizeType is a common type for indexes. It is possible to change 24 | * this by changing the define above: just define _SIZETYPE_ before 25 | * including this file. 26 | */ 27 | using SizeType = std::make_unsigned::type; 28 | 29 | } 30 | 31 | #endif // Common_hpp 32 | -------------------------------------------------------------------------------- /include/DopeVector/internal/Expression.hpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2016 Giorgio Marcias & Maurizio Kovacic 2 | // 3 | // This source code is part of DopeVector header library 4 | // and it is subject to Apache 2.0 License. 5 | // 6 | // Author: Giorgio Marcias 7 | // email: marcias.giorgio@gmail.com 8 | // Author: Maurizio Kovacic 9 | // email: maurizio.kovacic@gmail.com 10 | 11 | #ifndef Expression_hpp 12 | #define Expression_hpp 13 | 14 | #include 15 | #include 16 | #include 17 | 18 | namespace dope { 19 | 20 | namespace internal { 21 | 22 | template < class E, typename T, SizeType Dimension > 23 | class StaticArrayExpression { 24 | public: 25 | inline T getAt(const SizeType i) const; 26 | 27 | inline operator E const&() const; 28 | 29 | inline operator E&(); 30 | }; 31 | 32 | 33 | 34 | template < class E, typename T, SizeType Dimension, typename Op > 35 | class StaticArrayUnaryExpression : public StaticArrayExpression, T, Dimension> { 36 | private: 37 | const E &_e; 38 | mutable std::array, Dimension> _values; 39 | static const Op _op; 40 | 41 | public: 42 | inline StaticArrayUnaryExpression(const E &e); 43 | 44 | inline T operator[](const SizeType i) const; 45 | }; 46 | 47 | 48 | 49 | template < class El, class Er, typename T, SizeType Dimension, typename Op > 50 | class StaticArrayBinaryExpression : public StaticArrayExpression, T, Dimension> { 51 | private: 52 | const El &_el; 53 | const Er &_er; 54 | mutable std::array, Dimension> _values; 55 | static const Op _op; 56 | 57 | public: 58 | inline StaticArrayBinaryExpression(const El &el, const Er &er); 59 | 60 | inline T operator[](const SizeType i) const; 61 | }; 62 | 63 | 64 | 65 | template < class E, typename T, SizeType Dimension > 66 | inline StaticArrayUnaryExpression> operator- (const StaticArrayExpression &e); 67 | 68 | 69 | 70 | template < class El, class Er, typename T, SizeType Dimension > 71 | inline StaticArrayBinaryExpression> operator+ (const StaticArrayExpression &el, const StaticArrayExpression &er); 72 | 73 | 74 | 75 | template < class El, class Er, typename T, SizeType Dimension > 76 | inline StaticArrayBinaryExpression> operator- (const StaticArrayExpression &el, const StaticArrayExpression &er); 77 | 78 | 79 | template < class El, class Er, typename T, SizeType Dimension > 80 | inline StaticArrayBinaryExpression> operator* (const StaticArrayExpression &el, const StaticArrayExpression &er); 81 | 82 | 83 | 84 | template < class El, class Er, typename T, SizeType Dimension > 85 | inline StaticArrayBinaryExpression> operator/ (const StaticArrayExpression &el, const StaticArrayExpression &er); 86 | 87 | 88 | 89 | template < class El, class Er, typename T, SizeType Dimension > 90 | inline StaticArrayBinaryExpression> operator% (const StaticArrayExpression &el, const StaticArrayExpression &er); 91 | 92 | } 93 | 94 | } 95 | 96 | #include 97 | 98 | #ifdef DOPE_USE_EIGEN 99 | #include 100 | #endif 101 | 102 | #endif 103 | -------------------------------------------------------------------------------- /include/DopeVector/internal/Iterator.hpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2016 Giorgio Marcias & Maurizio Kovacic 2 | // 3 | // This source code is part of DopeVector header library 4 | // and it is subject to Apache 2.0 License. 5 | // 6 | // Author: Giorgio Marcias 7 | // email: marcias.giorgio@gmail.com 8 | // Author: Maurizio Kovacic 9 | // email: maurizio.kovacic@gmail.com 10 | 11 | #ifndef Iterator_hpp 12 | #define Iterator_hpp 13 | 14 | #include 15 | #include 16 | #include 17 | #include 18 | 19 | namespace dope { 20 | 21 | template < typename T, SizeType Dimension > class DopeVector; 22 | 23 | namespace internal { 24 | 25 | struct output_random_access_iterator_tag : public std:: output_iterator_tag, public std:: random_access_iterator_tag { }; 26 | 27 | /** 28 | * @brief The Iterator class 29 | */ 30 | template < typename T, SizeType Dimension, bool Const > 31 | class Iterator { 32 | public: 33 | 34 | //////////////////////////////////////////////////////////////////// 35 | // TYPEDEFS 36 | //////////////////////////////////////////////////////////////////// 37 | 38 | using difference_type = std::make_signed::type; 39 | using value_type = T; 40 | using pointer = typename std::conditional::type>::type, typename std::remove_const::type>::type>::type; 41 | using reference = typename std::add_lvalue_reference::type; 42 | using iterator_category = typename std::conditional::type; 43 | 44 | using DopeVectorType = typename std::conditional>::type, typename std::remove_const>::type>::type; 45 | using DopeVectorRef = std::reference_wrapper; 46 | using IndexD = Index< Dimension >; 47 | using self_type = Iterator; 48 | 49 | //////////////////////////////////////////////////////////////////// 50 | 51 | 52 | 53 | //////////////////////////////////////////////////////////////////// 54 | // CONSTRUCTORS 55 | //////////////////////////////////////////////////////////////////// 56 | 57 | /** 58 | * @brief Default constructor. 59 | */ 60 | inline Iterator(); 61 | 62 | /** 63 | * @brief Constructor. 64 | */ 65 | explicit inline Iterator(DopeVectorType &dope_vector, const SizeType i = static_cast(0), const bool valid = true); 66 | 67 | /** 68 | * @brief Constructor. 69 | */ 70 | explicit inline Iterator(DopeVectorType &dope_vector, const IndexD &index, const bool valid = true); 71 | 72 | 73 | /** 74 | * @brief Copy constructor. 75 | */ 76 | inline Iterator(const Iterator& other ) = default; 77 | 78 | /** 79 | * @brief Move constructor. 80 | */ 81 | inline Iterator(Iterator&& other ) = default; 82 | 83 | //////////////////////////////////////////////////////////////////// 84 | 85 | 86 | 87 | //////////////////////////////////////////////////////////////////// 88 | // INFORMATION 89 | //////////////////////////////////////////////////////////////////// 90 | 91 | inline SizeType to_original() const; 92 | inline SizeType to_position() const; 93 | inline const IndexD & to_index() const; 94 | inline bool valid() const; 95 | explicit inline operator bool () const; 96 | 97 | //////////////////////////////////////////////////////////////////// 98 | 99 | 100 | 101 | //////////////////////////////////////////////////////////////////// 102 | // ASSIGNMENT 103 | //////////////////////////////////////////////////////////////////// 104 | 105 | inline self_type& operator=(const self_type &o) = default; 106 | inline self_type& operator=(self_type &&o) = default; 107 | 108 | //////////////////////////////////////////////////////////////////// 109 | 110 | 111 | 112 | //////////////////////////////////////////////////////////////////// 113 | // DATA ACCESS METHODS 114 | //////////////////////////////////////////////////////////////////// 115 | 116 | inline reference operator* () const; 117 | inline pointer operator->() const; 118 | inline reference operator[](const SizeType n) const; 119 | inline reference operator[](const IndexD &n) const; 120 | 121 | //////////////////////////////////////////////////////////////////// 122 | 123 | 124 | 125 | //////////////////////////////////////////////////////////////////// 126 | // INCREMENT OPERATIONS 127 | //////////////////////////////////////////////////////////////////// 128 | 129 | inline self_type & operator++(); 130 | inline self_type operator++(int); 131 | inline self_type & operator+=(const SizeType n); 132 | inline self_type operator+ (const SizeType n) const; 133 | inline self_type & operator+=(const Index &n); 134 | inline self_type operator+ (const Index &n) const; 135 | 136 | //////////////////////////////////////////////////////////////////// 137 | 138 | 139 | 140 | //////////////////////////////////////////////////////////////////// 141 | // DECREMENT OPERATIONS 142 | //////////////////////////////////////////////////////////////////// 143 | 144 | inline self_type & operator--(); 145 | inline self_type operator--(int); 146 | inline self_type & operator-=(const SizeType n); 147 | inline self_type operator- (const SizeType n) const; 148 | inline self_type & operator-=(const Index &n); 149 | inline self_type operator- (const Index &n) const; 150 | inline difference_type operator- (const self_type &o) const; 151 | 152 | //////////////////////////////////////////////////////////////////// 153 | 154 | 155 | 156 | //////////////////////////////////////////////////////////////////// 157 | // BOOLEAN OPERATIONS 158 | //////////////////////////////////////////////////////////////////// 159 | 160 | inline bool operator==(const self_type &o) const; 161 | inline bool operator!=(const self_type &o) const; 162 | inline bool operator< (const self_type &o) const; 163 | inline bool operator<=(const self_type &o) const; 164 | inline bool operator> (const self_type &o) const; 165 | inline bool operator>=(const self_type &o) const; 166 | 167 | //////////////////////////////////////////////////////////////////// 168 | 169 | 170 | 171 | private: 172 | DopeVectorRef _data; ///< A reference to the pointed DopeVector. 173 | IndexD _currentIndex; ///< The current position. 174 | bool _valid; ///< Tells if this iterator is valid, e.g. it is not at the end. 175 | }; 176 | } 177 | 178 | } 179 | 180 | #include 181 | 182 | #endif // Iterator_hpp 183 | -------------------------------------------------------------------------------- /include/DopeVector/internal/eigen_support/EigenExpression.hpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2016 Giorgio Marcias & Maurizio Kovacic 2 | // 3 | // This source code is part of DopeVector header library 4 | // and it is subject to Apache 2.0 License. 5 | // 6 | // Author: Giorgio Marcias 7 | // email: marcias.giorgio@gmail.com 8 | // Author: Maurizio Kovacic 9 | // email: maurizio.kovacic@gmail.com 10 | 11 | #ifndef EigenExpression_hpp 12 | #define EigenExpression_hpp 13 | 14 | #ifdef DOPE_USE_EIGEN 15 | 16 | #include 17 | #include 18 | 19 | namespace dope { 20 | 21 | namespace internal { 22 | 23 | template < class Derived, class Er, typename T, SizeType Dimension, typename Op > 24 | class EigenStaticArrayBinaryExpression : public StaticArrayExpression, T, Dimension> { 25 | static_assert((Eigen::MatrixBase::RowsAtCompileTime == Dimension && Eigen::MatrixBase::ColsAtCompileTime == 1) || 26 | (Eigen::MatrixBase::RowsAtCompileTime == 1 && Eigen::MatrixBase::ColsAtCompileTime == Dimension), "Eigen object must be a vertical vector."); 27 | private: 28 | const Eigen::MatrixBase &_el; 29 | const Er &_er; 30 | mutable std::array, Dimension> _values; 31 | static const Op _op; 32 | 33 | public: 34 | inline EigenStaticArrayBinaryExpression(const Eigen::MatrixBase &el, const Er &er); 35 | 36 | inline T operator[](const SizeType i) const; 37 | }; 38 | 39 | 40 | 41 | template < class El, typename T, SizeType Dimension, class Derived, typename Op > 42 | class StaticArrayBinaryEigenExpression : public StaticArrayExpression, T, Dimension> { 43 | static_assert((Eigen::MatrixBase::RowsAtCompileTime == Dimension && Eigen::MatrixBase::ColsAtCompileTime == 1) || 44 | (Eigen::MatrixBase::RowsAtCompileTime == 1 && Eigen::MatrixBase::ColsAtCompileTime == Dimension), "Eigen object must be a vertical vector."); 45 | private: 46 | const El &_el; 47 | const Eigen::MatrixBase &_er; 48 | mutable std::array, Dimension> _values; 49 | static const Op _op; 50 | 51 | public: 52 | inline StaticArrayBinaryEigenExpression(const El &el, const Eigen::MatrixBase &er); 53 | 54 | inline T operator[](const SizeType i) const; 55 | }; 56 | 57 | 58 | 59 | template < class Derived, class Er, typename T, SizeType Dimension > 60 | inline EigenStaticArrayBinaryExpression> operator+ (const Eigen::MatrixBase &el, const StaticArrayExpression &er); 61 | 62 | 63 | 64 | template < class Derived, class Er, typename T, SizeType Dimension > 65 | inline EigenStaticArrayBinaryExpression> operator- (const Eigen::MatrixBase &el, const StaticArrayExpression &er); 66 | 67 | 68 | 69 | template < class Derived, class Er, typename T, SizeType Dimension > 70 | inline EigenStaticArrayBinaryExpression> operator* (const Eigen::MatrixBase &el, const StaticArrayExpression &er); 71 | 72 | 73 | 74 | template < class Derived, class Er, typename T, SizeType Dimension > 75 | inline EigenStaticArrayBinaryExpression> operator/ (const Eigen::MatrixBase &el, const StaticArrayExpression &er); 76 | 77 | 78 | 79 | template < class Derived, class Er, typename T, SizeType Dimension > 80 | inline EigenStaticArrayBinaryExpression> operator% (const Eigen::MatrixBase &el, const StaticArrayExpression &er); 81 | 82 | 83 | 84 | template < class El, typename T, SizeType Dimension, class Derived > 85 | inline StaticArrayBinaryEigenExpression> operator+ (const StaticArrayExpression &el, const Eigen::MatrixBase &er); 86 | 87 | 88 | 89 | template < class El, typename T, SizeType Dimension, class Derived > 90 | inline StaticArrayBinaryEigenExpression> operator- (const StaticArrayExpression &el, const Eigen::MatrixBase &er); 91 | 92 | 93 | 94 | template < class El, typename T, SizeType Dimension, class Derived > 95 | inline StaticArrayBinaryEigenExpression> operator* (const StaticArrayExpression &el, const Eigen::MatrixBase &er); 96 | 97 | 98 | 99 | template < class El, typename T, SizeType Dimension, class Derived > 100 | inline StaticArrayBinaryEigenExpression> operator/ (const StaticArrayExpression &el, const Eigen::MatrixBase &er); 101 | 102 | 103 | 104 | template < class El, typename T, SizeType Dimension, class Derived > 105 | inline StaticArrayBinaryEigenExpression> operator% (const StaticArrayExpression &el, const Eigen::MatrixBase &er); 106 | 107 | } 108 | 109 | } 110 | 111 | #include 112 | 113 | #endif 114 | 115 | #endif 116 | -------------------------------------------------------------------------------- /include/DopeVector/internal/inlines/Expression.inl: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2016 Giorgio Marcias & Maurizio Kovacic 2 | // 3 | // This source code is part of DopeVector header library 4 | // and it is subject to Apache 2.0 License. 5 | // 6 | // Author: Giorgio Marcias 7 | // email: marcias.giorgio@gmail.com 8 | // Author: Maurizio Kovacic 9 | // email: maurizio.kovacic@gmail.com 10 | 11 | #include 12 | 13 | namespace dope { 14 | 15 | namespace internal { 16 | 17 | template < class E, typename T, SizeType Dimension > 18 | inline T StaticArrayExpression::getAt(const SizeType i) const 19 | { 20 | return static_cast(*this)[i]; 21 | } 22 | 23 | template < class E, typename T, SizeType Dimension > 24 | inline StaticArrayExpression::operator E const&() const 25 | { 26 | return static_cast(*this); 27 | } 28 | 29 | template < class E, typename T, SizeType Dimension > 30 | inline StaticArrayExpression::operator E &() 31 | { 32 | return static_cast(*this); 33 | } 34 | 35 | 36 | 37 | template < class E, typename T, SizeType Dimension, typename Op > 38 | const Op StaticArrayUnaryExpression::_op = Op(); 39 | 40 | template < class E, typename T, SizeType Dimension, typename Op > 41 | inline StaticArrayUnaryExpression::StaticArrayUnaryExpression(const E &e) 42 | : _e(e) 43 | { } 44 | 45 | template < class E, typename T, SizeType Dimension, typename Op > 46 | inline T StaticArrayUnaryExpression::operator[](const SizeType i) const 47 | { 48 | if (!_values[i]) { 49 | T t = _op(_e.getAt(i)); 50 | _values[i] = [t]()->T{ return t; }; 51 | } 52 | return _values[i](); 53 | } 54 | 55 | 56 | 57 | template < class El, class Er, typename T, SizeType Dimension, typename Op > 58 | const Op StaticArrayBinaryExpression::_op = Op(); 59 | 60 | template < class El, class Er, typename T, SizeType Dimension, typename Op > 61 | inline StaticArrayBinaryExpression::StaticArrayBinaryExpression(const El &el, const Er &er) 62 | : _el(el), _er(er) 63 | { } 64 | 65 | template < class El, class Er, typename T, SizeType Dimension, typename Op > 66 | inline T StaticArrayBinaryExpression::operator[](const SizeType i) const 67 | { 68 | if (!_values[i]) { 69 | T t = _op(_el.getAt(i), _er.getAt(i)); 70 | _values[i] = [t]()->T{ return t; }; 71 | } 72 | return _values[i](); 73 | } 74 | 75 | 76 | template < class E, typename T, SizeType Dimension > 77 | inline StaticArrayUnaryExpression> operator- (const StaticArrayExpression &e) 78 | { 79 | return StaticArrayUnaryExpression>(e); 80 | } 81 | 82 | 83 | 84 | template < class El, class Er, typename T, SizeType Dimension > 85 | inline StaticArrayBinaryExpression> operator+ (const StaticArrayExpression &el, const StaticArrayExpression &er) 86 | { 87 | return StaticArrayBinaryExpression>(el, er); 88 | } 89 | 90 | 91 | 92 | template < class El, class Er, typename T, SizeType Dimension > 93 | inline StaticArrayBinaryExpression> operator- (const StaticArrayExpression &el, const StaticArrayExpression &er) 94 | { 95 | return StaticArrayBinaryExpression>(el, er); 96 | } 97 | 98 | 99 | 100 | template < class El, class Er, typename T, SizeType Dimension > 101 | inline StaticArrayBinaryExpression> operator* (const StaticArrayExpression &el, const StaticArrayExpression &er) 102 | { 103 | return StaticArrayBinaryExpression>(el, er); 104 | } 105 | 106 | 107 | 108 | template < class El, class Er, typename T, SizeType Dimension > 109 | inline StaticArrayBinaryExpression> operator/ (const StaticArrayExpression &el, const StaticArrayExpression &er) 110 | { 111 | return StaticArrayBinaryExpression>(el, er); 112 | } 113 | 114 | 115 | 116 | template < class El, class Er, typename T, SizeType Dimension > 117 | inline StaticArrayBinaryExpression> operator% (const StaticArrayExpression &el, const StaticArrayExpression &er) 118 | { 119 | return StaticArrayBinaryExpression>(el, er); 120 | } 121 | 122 | } 123 | 124 | } 125 | -------------------------------------------------------------------------------- /include/DopeVector/internal/inlines/Grid.inl: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2016 Giorgio Marcias & Maurizio Kovacic 2 | // 3 | // This source code is part of DopeVector header library 4 | // and it is subject to Apache 2.0 License. 5 | // 6 | // Author: Giorgio Marcias 7 | // email: marcias.giorgio@gmail.com 8 | // Author: Maurizio Kovacic 9 | // email: maurizio.kovacic@gmail.com 10 | 11 | #include 12 | 13 | namespace dope { 14 | 15 | //////////////////////////////////////////////////////////////////////////// 16 | // CONSTRUCTORS 17 | //////////////////////////////////////////////////////////////////////////// 18 | 19 | template < typename T, SizeType Dimension, class Allocator > 20 | inline Grid::Grid(const IndexD &size, const T &default_value) 21 | : _data(size.prod(), default_value) 22 | { 23 | DopeVector::reset(_data.data(), static_cast(0), size); 24 | } 25 | 26 | template < typename T, SizeType Dimension, class Allocator > 27 | inline Grid::Grid(const IndexD &size, const IndexD &order, const T &default_value) 28 | : _data(size.prod(), default_value) 29 | { 30 | IndexD original_offset; 31 | original_offset[Dimension-1] = 1; 32 | for (SizeType d = Dimension-1; d > static_cast(0); --d) 33 | original_offset[d-1] = size[d] * original_offset[d]; 34 | IndexD offset; 35 | for (SizeType d = static_cast(0); d < Dimension; ++d) { 36 | if (order[d] >= Dimension) { 37 | std::stringstream stream; 38 | stream << "Index " << order[d] << " is out of range [0, " << Dimension-1 << ']'; 39 | throw std::out_of_range(stream.str()); 40 | } 41 | offset[d] = original_offset[order[d]]; 42 | } 43 | DopeVector::reset(_data.data(), static_cast(0), size, offset); 44 | } 45 | 46 | template < typename T, SizeType Dimension, class Allocator > 47 | inline Grid::Grid(const SizeType size, const T &default_value) 48 | : _data(Index::Constant(size).prod(), default_value) 49 | { 50 | DopeVector::reset(_data.data(), static_cast(0), Index::Constant(size)); 51 | } 52 | 53 | template < typename T, SizeType Dimension, class Allocator > 54 | inline Grid::Grid(const SizeType size, const IndexD &order, const T &default_value) 55 | : _data(Index::Constant(size).prod(), default_value) 56 | { 57 | IndexD original_offset; 58 | original_offset[Dimension-1] = 1; 59 | for (SizeType d = Dimension-1; d > 0; --d) 60 | original_offset[d-1] = size * original_offset[d]; 61 | IndexD offset; 62 | for (SizeType d = static_cast(0); d < Dimension; ++d) { 63 | if (order[d] >= Dimension) { 64 | std::stringstream stream; 65 | stream << "Index " << order[d] << " is out of range [0, " << Dimension-1 << ']'; 66 | throw std::out_of_range(stream.str()); 67 | } 68 | offset[d] = original_offset[order[d]]; 69 | } 70 | DopeVector::reset(_data.data(), static_cast(0), Index::Constant(size), offset); 71 | } 72 | 73 | template < typename T, SizeType Dimension, class Allocator > 74 | inline Grid::Grid(const Grid &o) 75 | : _data(o._data) 76 | { 77 | DopeVector::reset(_data.data(), static_cast(0), o.allSizes()); 78 | } 79 | 80 | //////////////////////////////////////////////////////////////////////////// 81 | 82 | 83 | 84 | //////////////////////////////////////////////////////////////////////////// 85 | // DESTRUCTOR 86 | //////////////////////////////////////////////////////////////////////////// 87 | 88 | template < typename T, SizeType Dimension, class Allocator > 89 | inline Grid::~Grid() 90 | { } 91 | 92 | //////////////////////////////////////////////////////////////////////////// 93 | 94 | 95 | //////////////////////////////////////////////////////////////////////////// 96 | // DATA 97 | //////////////////////////////////////////////////////////////////////////// 98 | 99 | template < typename T, SizeType Dimension, class Allocator > 100 | inline const T * Grid::data() const 101 | { 102 | return _data.data(); 103 | } 104 | 105 | template < typename T, SizeType Dimension, class Allocator > 106 | inline T * Grid::data() 107 | { 108 | return _data.data(); 109 | } 110 | 111 | //////////////////////////////////////////////////////////////////////////// 112 | 113 | 114 | 115 | //////////////////////////////////////////////////////////////////////////// 116 | // CONVERSIONS 117 | //////////////////////////////////////////////////////////////////////////// 118 | 119 | template < typename T, SizeType Dimension, class Allocator > 120 | inline const typename Grid::Data& Grid::to_stdvector() const 121 | { 122 | return _data; 123 | } 124 | 125 | template < typename T, SizeType Dimension, class Allocator > 126 | inline typename Grid::Data& Grid::to_stdvector() 127 | { 128 | return _data; 129 | } 130 | 131 | template < typename T, SizeType Dimension, class Allocator > 132 | inline Grid::operator const Data&() const 133 | { 134 | return _data; 135 | } 136 | 137 | template < typename T, SizeType Dimension, class Allocator > 138 | inline Grid::operator Data&() 139 | { 140 | return _data; 141 | } 142 | 143 | //////////////////////////////////////////////////////////////////////////// 144 | 145 | 146 | 147 | //////////////////////////////////////////////////////////////////////////// 148 | // INFORMATION 149 | //////////////////////////////////////////////////////////////////////////// 150 | 151 | template < typename T, SizeType Dimension, class Allocator > 152 | inline bool Grid::empty() const 153 | { 154 | return _data.empty(); 155 | } 156 | 157 | template < typename T, SizeType Dimension, class Allocator > 158 | inline bool Grid::operator==(const Grid &o) const 159 | { 160 | if (&o == this) 161 | return true; 162 | for (SizeType i = static_cast(0); i < Dimension; ++i) 163 | if (DopeVector::sizeAt(i) != o.sizeAt(i)) 164 | return false; 165 | return _data == o._data; 166 | } 167 | 168 | //////////////////////////////////////////////////////////////////////////// 169 | 170 | 171 | 172 | //////////////////////////////////////////////////////////////////////////// 173 | // RESET 174 | //////////////////////////////////////////////////////////////////////////// 175 | 176 | template < typename T, SizeType Dimension, class Allocator > 177 | inline void Grid::clear() 178 | { 179 | _data.clear(); 180 | DopeVector::reset(nullptr, static_cast(0), IndexD::Zero()); 181 | } 182 | 183 | template < typename T, SizeType Dimension, class Allocator > 184 | inline void Grid::reset(const T &default_value) 185 | { 186 | IndexD size = DopeVector::allSizes(); 187 | _data.assign(size.prod(), default_value); 188 | DopeVector::reset(_data.data(), static_cast(0), size); 189 | } 190 | 191 | template < typename T, SizeType Dimension, class Allocator > 192 | inline void Grid::resize(const IndexD &size, const T &default_value) 193 | { 194 | _data.resize(size.prod(), default_value); 195 | DopeVector::reset(_data.data(), static_cast(0), size); 196 | } 197 | 198 | template < typename T, SizeType Dimension, class Allocator > 199 | inline void Grid::resize(const IndexD &size, const IndexD &order, const T &default_value) 200 | { 201 | _data.resize(size.prod(), default_value); 202 | IndexD original_offset; 203 | original_offset[Dimension-1] = 1; 204 | for (SizeType d = Dimension-1; d > static_cast(0); --d) 205 | original_offset[d-1] = size[d] * original_offset[d]; 206 | IndexD offset; 207 | for (SizeType d = static_cast(0); d < Dimension; ++d) { 208 | if (order[d] >= Dimension) { 209 | std::stringstream stream; 210 | stream << "Index " << order[d] << " is out of range [0, " << Dimension-1 << ']'; 211 | throw std::out_of_range(stream.str()); 212 | } 213 | offset[d] = original_offset[order[d]]; 214 | } 215 | DopeVector::reset(_data.data(), static_cast(0), size, offset); 216 | } 217 | 218 | template < typename T, SizeType Dimension, class Allocator > 219 | inline void Grid::resize(const SizeType size, const T &default_value) 220 | { 221 | IndexD newSize = IndexD::Constant(size); 222 | resize(newSize, default_value); 223 | } 224 | 225 | template < typename T, SizeType Dimension, class Allocator > 226 | inline void Grid::resize(const SizeType size, const IndexD &order, const T & default_value) 227 | { 228 | IndexD newSize = IndexD::Constant(size); 229 | resize(newSize, order, default_value); 230 | } 231 | 232 | template < typename T, SizeType Dimension, class Allocator > 233 | inline void Grid::assign(const IndexD &size, const T &default_value) 234 | { 235 | _data.assign(size.prod(), default_value); 236 | DopeVector::reset(_data.data(), static_cast(0), size); 237 | } 238 | 239 | template < typename T, SizeType Dimension, class Allocator > 240 | inline void Grid::assign(const IndexD &size, const IndexD &order, const T &default_value) 241 | { 242 | _data.assign(size.prod(), default_value); 243 | IndexD original_offset; 244 | original_offset[Dimension-1] = 1; 245 | for (SizeType d = Dimension-1; d > static_cast(0); --d) 246 | original_offset[d-1] = size[d] * original_offset[d]; 247 | IndexD offset; 248 | for (SizeType d = static_cast(0); d < Dimension; ++d) { 249 | if (order[d] >= Dimension) { 250 | std::stringstream stream; 251 | stream << "Index " << order[d] << " is out of range [0, " << Dimension-1 << ']'; 252 | throw std::out_of_range(stream.str()); 253 | } 254 | offset[d] = original_offset[order[d]]; 255 | } 256 | DopeVector::reset(_data.data(), static_cast(0), size, offset); 257 | } 258 | 259 | template < typename T, SizeType Dimension, class Allocator > 260 | inline void Grid::assign(const SizeType size, const T &default_value) 261 | { 262 | IndexD newSize = IndexD::Constant(size); 263 | assign(newSize, default_value); 264 | } 265 | 266 | template < typename T, SizeType Dimension, class Allocator > 267 | inline void Grid::assign(const SizeType size, const IndexD &order, const T &default_value) 268 | { 269 | IndexD newSize = IndexD::Constant(size); 270 | assign(newSize, order, default_value); 271 | } 272 | 273 | template < typename T, SizeType Dimension, class Allocator > 274 | inline void Grid::conservativeResize(const IndexD &size, const T &default_value) 275 | { 276 | Grid newGrid(size, default_value); 277 | IndexD oldSize = DopeVector::allSizes(); 278 | IndexD minSize; 279 | for (SizeType d = static_cast(0); d < Dimension; ++d) 280 | minSize[d] = std::min(oldSize[d], size[d]); 281 | IndexD start = IndexD::Zero(); 282 | DopeVector oldWindow = DopeVector::window(start, minSize); 283 | DopeVector newWindow = newGrid.window(start, minSize); 284 | newWindow.import(oldWindow); 285 | *this = std::move(newGrid); 286 | } 287 | 288 | template < typename T, SizeType Dimension, class Allocator > 289 | inline void Grid::conservativeResize(const IndexD &size, const IndexD &order, const T &default_value) 290 | { 291 | IndexD original_offset; 292 | original_offset[Dimension-1] = 1; 293 | for (SizeType d = Dimension-1; d > static_cast(0); --d) 294 | original_offset[d-1] = size[d] * original_offset[d]; 295 | IndexD offset; 296 | for (SizeType d = static_cast(0); d < Dimension; ++d) { 297 | if (order[d] >= Dimension) { 298 | std::stringstream stream; 299 | stream << "Index " << order[d] << " is out of range [0, " << Dimension-1 << ']'; 300 | throw std::out_of_range(stream.str()); 301 | } 302 | offset[d] = original_offset[order[d]]; 303 | } 304 | Grid newGrid(size, offset, default_value); 305 | IndexD oldSize = DopeVector::allSizes(); 306 | IndexD minSize; 307 | for (SizeType d = static_cast(0); d < Dimension; ++d) 308 | minSize[d] = std::min(oldSize[d], size[d]); 309 | IndexD start = IndexD::Zero(); 310 | DopeVector oldWindow = DopeVector::window(start, minSize); 311 | DopeVector newWindow = newGrid.window(start, minSize); 312 | newWindow.import(oldWindow); 313 | *this = std::move(newGrid); 314 | } 315 | 316 | template < typename T, SizeType Dimension, class Allocator > 317 | inline void Grid::conservativeResize(const SizeType size, const T &default_value) 318 | { 319 | IndexD newSize = IndexD::Constant(size); 320 | conservativeResize(newSize, default_value); 321 | } 322 | 323 | template < typename T, SizeType Dimension, class Allocator > 324 | inline void Grid::conservativeResize(const SizeType size, const IndexD &order, const T &default_value) 325 | { 326 | IndexD newSize = IndexD::Constant(size); 327 | conservativeResize(newSize, order, default_value); 328 | } 329 | 330 | //////////////////////////////////////////////////////////////////////////// 331 | 332 | 333 | 334 | //////////////////////////////////////////////////////////////////////////// 335 | // ASSIGNMENTS 336 | //////////////////////////////////////////////////////////////////////////// 337 | 338 | template < typename T, SizeType Dimension, class Allocator > 339 | inline Grid & Grid::operator=(const Grid &o) 340 | { 341 | if (&o != this) { 342 | _data = o._data; 343 | DopeVector::reset(_data.data(), static_cast(0), o.allSizes()); 344 | } 345 | return *this; 346 | } 347 | 348 | #ifdef DOPE_USE_RTTI 349 | template < typename T, SizeType Dimension, class Allocator > 350 | inline void Grid::import(const DopeVector &o) 351 | { 352 | if (&o == this) 353 | return; 354 | 355 | try { 356 | const Grid &oo = dynamic_cast &>(o); 357 | IndexD size = DopeVector::allSizes(); 358 | for (SizeType d = static_cast(0); d < Dimension; ++d) 359 | if (size[d] != oo.sizeAt(d)) 360 | throw std::out_of_range("Matrixes do not have same size."); 361 | _data = oo._data; 362 | DopeVector::reset(_data.data(), static_cast(0), size); // could be unuseful 363 | } catch(std::bad_cast &bc) { 364 | DopeVector::import(o); 365 | } 366 | } 367 | #endif 368 | 369 | template < typename T, SizeType Dimension, class Allocator > 370 | inline void Grid::swap(Grid &o) 371 | { 372 | _data.swap(o._data); 373 | IndexD size = DopeVector::allSizes(); 374 | DopeVector::reset(_data.data(), static_cast(0), size); 375 | } 376 | 377 | //////////////////////////////////////////////////////////////////////////// 378 | 379 | } 380 | -------------------------------------------------------------------------------- /include/DopeVector/internal/inlines/Index.inl: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2016 Giorgio Marcias & Maurizio Kovacic 2 | // 3 | // This source code is part of DopeVector header library 4 | // and it is subject to Apache 2.0 License. 5 | // 6 | // Author: Giorgio Marcias 7 | // email: marcias.giorgio@gmail.com 8 | // Author: Maurizio Kovacic 9 | // email: maurizio.kovacic@gmail.com 10 | 11 | #include 12 | #include 13 | #include 14 | 15 | namespace dope { 16 | 17 | template < SizeType Dimension > template < typename ... Sizes, class > 18 | inline Index::Index(const SizeType size0, Sizes &&...sizes) 19 | : std::array({{size0, std::forward(sizes)...}}) 20 | { } 21 | 22 | template < SizeType Dimension > 23 | inline Index::Index(const std::initializer_list &il) 24 | { 25 | std::copy(il.begin(), il.begin() + std::min(il.size(), Dimension), std::array::begin()); 26 | std::fill(std::array::begin() + std::min(il.size(), Dimension), std::array::end(), std::array::operator[](std::min(Dimension, std::min(il.size(), Dimension)) - 1)); 27 | } 28 | 29 | template < SizeType Dimension > template < class E > 30 | inline Index::Index(const internal::StaticArrayExpression &e) 31 | { 32 | for (SizeType i = 0; i < Dimension; ++i) 33 | std::array::operator[](i) = e.getAt(i); 34 | } 35 | 36 | #ifdef DOPE_USE_EIGEN 37 | template < SizeType Dimension > template < class Derived > 38 | inline Index::Index(const Eigen::MatrixBase &e) 39 | { 40 | static_assert((Eigen::MatrixBase::RowsAtCompileTime == Dimension && Eigen::MatrixBase::ColsAtCompileTime == 1) || 41 | (Eigen::MatrixBase::RowsAtCompileTime == 1 && Eigen::MatrixBase::ColsAtCompileTime == Dimension), "Eigen object must be a vertical vector."); 42 | for (SizeType i = 0; i < Dimension; ++i) 43 | std::array::operator[](i) = e[i]; 44 | } 45 | #endif 46 | 47 | template < SizeType Dimension > 48 | inline Index & Index::operator= (const std::initializer_list &il) 49 | { 50 | std::copy(il.begin(), il.begin() + std::min(il.size(), Dimension), std::array::begin()); 51 | std::fill(std::array::begin() + std::min(il.size(), Dimension), std::array::end(), std::array::operator[](std::min(Dimension, std::min(il.size(), Dimension)) - 1)); 52 | return *this; 53 | } 54 | 55 | template < SizeType Dimension > 56 | inline bool Index::isApprox(const Index &o) const 57 | { 58 | return *this == o; 59 | } 60 | 61 | template < SizeType Dimension > template < class E > 62 | inline Index& Index::operator=(const internal::StaticArrayExpression &e) 63 | { 64 | for (SizeType i = 0; i < Dimension; ++i) 65 | std::array::operator[](i) = e.getAt(i); 66 | return *this; 67 | } 68 | 69 | template < SizeType Dimension > template < class E > 70 | inline Index& Index::operator+=(const internal::StaticArrayExpression &e) 71 | { 72 | for (SizeType i = 0; i < Dimension; ++i) 73 | std::array::operator[](i) += e.getAt(i); 74 | return *this; 75 | } 76 | 77 | template < SizeType Dimension > template < class E > 78 | inline Index& Index::operator-=(const internal::StaticArrayExpression &e) 79 | { 80 | for (SizeType i = 0; i < Dimension; ++i) 81 | std::array::operator[](i) -= e.getAt(i); 82 | return *this; 83 | } 84 | 85 | template < SizeType Dimension > template < class E > 86 | inline Index& Index::operator*=(const internal::StaticArrayExpression &e) 87 | { 88 | for (SizeType i = 0; i < Dimension; ++i) 89 | std::array::operator[](i) *= e.getAt(i); 90 | return *this; 91 | } 92 | 93 | template < SizeType Dimension > template < class E > 94 | inline Index& Index::operator/=(const internal::StaticArrayExpression &e) 95 | { 96 | for (SizeType i = 0; i < Dimension; ++i) 97 | std::array::operator[](i) /= e.getAt(i); 98 | return *this; 99 | } 100 | 101 | template < SizeType Dimension > template < class E > 102 | inline Index& Index::operator%=(const internal::StaticArrayExpression &e) 103 | { 104 | for (SizeType i = 0; i < Dimension; ++i) 105 | std::array::operator[](i) %= e.getAt(i); 106 | return *this; 107 | } 108 | 109 | #ifdef DOPE_USE_EIGEN 110 | /** 111 | * @brief Eigen expression assignment operator. 112 | */ 113 | template < SizeType Dimension > template < class Derived > 114 | inline Index& Index::operator=(const Eigen::MatrixBase &e) 115 | { 116 | static_assert((Eigen::MatrixBase::RowsAtCompileTime == Dimension && Eigen::MatrixBase::ColsAtCompileTime == 1) || 117 | (Eigen::MatrixBase::RowsAtCompileTime == 1 && Eigen::MatrixBase::ColsAtCompileTime == Dimension), "Eigen object must be a vertical vector."); 118 | for (SizeType i = 0; i < Dimension; ++i) 119 | std::array::operator[](i) = e[i]; 120 | return *this; 121 | } 122 | #endif 123 | 124 | template < SizeType Dimension > 125 | inline SizeType Index::sum() const 126 | { 127 | SizeType sum = static_cast(0); 128 | for (SizeType i = static_cast(0); i < Dimension; ++i) 129 | sum += std::array::at(i); 130 | return sum; 131 | } 132 | 133 | template < SizeType Dimension > 134 | inline SizeType Index::prod() const 135 | { 136 | SizeType prod = static_cast(1); 137 | for (SizeType i = static_cast(0); i < Dimension; ++i) 138 | prod *= std::array::at(i); 139 | return prod; 140 | } 141 | 142 | template < SizeType Dimension > 143 | inline constexpr Index Index::Zero() 144 | { 145 | return Index({{0}}); 146 | } 147 | 148 | template < SizeType Dimension > 149 | inline constexpr Index Index::Ones() 150 | { 151 | return Index({{1}}); 152 | } 153 | 154 | template < SizeType Dimension > 155 | inline constexpr Index Index::Constant( const SizeType value ) 156 | { 157 | return Index({{value}}); 158 | } 159 | 160 | template < SizeType Dimension > 161 | inline SizeType to_position(const Index &index, const Index &range) 162 | { 163 | SizeType result = static_cast(0); 164 | SizeType dimProd = static_cast(1); 165 | for(SizeType D = Dimension; D > static_cast(0); --D) { 166 | SizeType d = D - static_cast(1); 167 | result += index[d] * dimProd; 168 | dimProd *= range[d]; 169 | } 170 | return result; 171 | } 172 | 173 | template < SizeType Dimension > 174 | inline SizeType to_positionFromOffset(const Index &index, const Index &offset) 175 | { 176 | SizeType position = static_cast(0); 177 | for(SizeType d = static_cast(0); d < Dimension; ++d) 178 | position += index[d] * offset[d]; 179 | return position; 180 | } 181 | 182 | template < SizeType Dimension > 183 | inline Index to_index(const SizeType position, const Index &range) 184 | { 185 | Index result = Index::Zero(); 186 | SizeType i = position; 187 | for(SizeType D = Dimension; D > static_cast(0); --D) { 188 | SizeType d = D - static_cast(1); 189 | if (range[d] == static_cast(0)) 190 | throw std::overflow_error("Divide by zero exception"); 191 | result[d] = i % range[d]; 192 | i = i / range[d]; 193 | } 194 | return result; 195 | } 196 | 197 | template < SizeType Dimension > 198 | inline Index to_indexFromOffset(const SizeType position, const Index &offset) 199 | { 200 | SizeType linear_position = position; 201 | Index order; 202 | for (SizeType i = static_cast(0); i < Dimension; ++i) 203 | order[i] = i; 204 | std::sort(order.begin(), order.end(), [&offset](const SizeType &l, const SizeType &r)->bool{ 205 | return offset[l] < offset[r]; 206 | }); 207 | Index result = Index::Zero(); 208 | for(SizeType D = Dimension; D > static_cast(0); --D) { 209 | SizeType d = D - static_cast(1); 210 | result[order[d]] = linear_position / offset[order[d]]; 211 | linear_position -= result[order[d]] * offset[order[d]]; 212 | } 213 | return result; 214 | } 215 | 216 | } 217 | -------------------------------------------------------------------------------- /include/DopeVector/internal/inlines/Iterator.inl: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2016 Giorgio Marcias & Maurizio Kovacic 2 | // 3 | // This source code is part of DopeVector header library 4 | // and it is subject to Apache 2.0 License. 5 | // 6 | // Author: Giorgio Marcias 7 | // email: marcias.giorgio@gmail.com 8 | // Author: Maurizio Kovacic 9 | // email: maurizio.kovacic@gmail.com 10 | 11 | #include 12 | #include 13 | 14 | namespace dope { 15 | 16 | namespace internal { 17 | 18 | //////////////////////////////////////////////////////////////////////// 19 | // CONSTRUCTORS 20 | //////////////////////////////////////////////////////////////////////// 21 | 22 | template < typename T, SizeType Dimension, bool Const > 23 | inline Iterator::Iterator() 24 | : _currentIndex(IndexD::Zero()) 25 | , _valid(false) 26 | { } 27 | 28 | template < typename T, SizeType Dimension, bool Const > 29 | inline Iterator::Iterator(DopeVectorType &dope_vector, const SizeType i, const bool valid) 30 | : _data(dope_vector) 31 | , _currentIndex(valid ? dope::to_index(i, dope_vector.allSizes()) : IndexD::Zero()) 32 | , _valid(valid) 33 | { 34 | if (_valid && i >= _data.get().size()) { 35 | _currentIndex = IndexD::Zero(); 36 | _valid = false; 37 | } 38 | } 39 | 40 | template < typename T, SizeType Dimension, bool Const > 41 | inline Iterator::Iterator(DopeVectorType &dope_vector, const IndexD &index, const bool valid) 42 | : _data(dope_vector) 43 | , _currentIndex(valid ? index : IndexD::Zero()) 44 | , _valid(valid) 45 | { 46 | if (_valid) { 47 | IndexD size = _data.get().allSizes(); 48 | for (SizeType d = 0; d < Dimension; ++d) 49 | if (_currentIndex[d] >= size[d]) { 50 | _currentIndex = IndexD::Zero(); 51 | _valid = false; 52 | break; 53 | } 54 | } 55 | } 56 | 57 | //////////////////////////////////////////////////////////////////////// 58 | 59 | 60 | 61 | //////////////////////////////////////////////////////////////////////// 62 | // INFORMATION 63 | //////////////////////////////////////////////////////////////////////// 64 | 65 | template < typename T, SizeType Dimension, bool Const > 66 | inline SizeType Iterator::to_original() const 67 | { 68 | if (!_valid) 69 | throw std::range_error("Iterator not valid."); 70 | return _data.get().accumulatedOffset(_currentIndex); 71 | } 72 | 73 | template < typename T, SizeType Dimension, bool Const > 74 | inline SizeType Iterator::to_position() const 75 | { 76 | if (!_valid) 77 | throw std::range_error("Iterator not valid."); 78 | return dope::to_position< Dimension >(_currentIndex, _data.get().allSizes()); 79 | } 80 | 81 | template < typename T, SizeType Dimension, bool Const > 82 | inline const typename Iterator::IndexD & Iterator::to_index() const 83 | { 84 | if (!_valid) 85 | throw std::range_error("Iterator not valid."); 86 | return _currentIndex; 87 | } 88 | 89 | template < typename T, SizeType Dimension, bool Const > 90 | inline bool Iterator::valid() const 91 | { 92 | return _valid; 93 | } 94 | 95 | template < typename T, SizeType Dimension, bool Const > 96 | inline Iterator::operator bool() const 97 | { 98 | return valid(); 99 | } 100 | 101 | //////////////////////////////////////////////////////////////////////// 102 | 103 | 104 | 105 | //////////////////////////////////////////////////////////////////////// 106 | // DATA ACCESS METHODS 107 | //////////////////////////////////////////////////////////////////////// 108 | 109 | template < typename T, SizeType Dimension, bool Const > 110 | inline typename Iterator::reference Iterator::operator*() const 111 | { 112 | if (!_valid) 113 | throw std::range_error("Iterator not valid."); 114 | return const_cast(_data.get().at(_currentIndex)); 115 | } 116 | 117 | template < typename T, SizeType Dimension, bool Const > 118 | inline typename Iterator::pointer Iterator::operator->() const 119 | { 120 | if (!_valid) 121 | throw std::range_error("Iterator not valid."); 122 | return const_cast(&_data.get().at(_currentIndex)); 123 | } 124 | template < typename T, SizeType Dimension, bool Const > 125 | inline typename Iterator::reference Iterator::operator[](const SizeType n) const 126 | { 127 | return *(*this + n); 128 | } 129 | 130 | template < typename T, SizeType Dimension, bool Const > 131 | inline typename Iterator::reference Iterator::operator[](const IndexD &n) const 132 | { 133 | return *(*this + n); 134 | } 135 | 136 | //////////////////////////////////////////////////////////////////////// 137 | 138 | 139 | 140 | //////////////////////////////////////////////////////////////////////// 141 | // INCREMENT OPERATIONS 142 | //////////////////////////////////////////////////////////////////////// 143 | 144 | template < typename T, SizeType Dimension, bool Const > 145 | inline typename Iterator::self_type& Iterator::operator++() 146 | { 147 | return *this += static_cast(1); 148 | } 149 | 150 | template < typename T, SizeType Dimension, bool Const > 151 | inline typename Iterator::self_type Iterator::operator++(int) 152 | { 153 | self_type copy(*this); 154 | ++*this; 155 | return copy; 156 | } 157 | 158 | template < typename T, SizeType Dimension, bool Const > 159 | inline typename Iterator::self_type & Iterator::operator+=(const SizeType n) 160 | { 161 | if (!_valid) 162 | return *this; 163 | Index size = _data.get().allSizes(); 164 | SizeType tmp_val, carry = n; 165 | for (SizeType D = Dimension; D > 0 && carry != static_cast(0); --D) { 166 | SizeType d = D - static_cast(1); 167 | tmp_val = _currentIndex[d] + carry; 168 | carry = tmp_val / size[d]; 169 | _currentIndex[d] = tmp_val % size[d]; 170 | } 171 | if (carry != static_cast(0)) { 172 | _currentIndex = IndexD::Zero(); 173 | _valid = false; 174 | } 175 | return *this; 176 | } 177 | 178 | template < typename T, SizeType Dimension, bool Const > 179 | inline typename Iterator::self_type Iterator::operator+ (const SizeType n) const { 180 | self_type copy(*this); 181 | copy += n; 182 | return copy; 183 | } 184 | 185 | template < typename T, SizeType Dimension, bool Const > 186 | inline typename Iterator::self_type & Iterator::operator+=(const Index &n) 187 | { 188 | if (!_valid) 189 | return *this; 190 | Index size = _data.get().allSizes(); 191 | SizeType tmp_val, carry = static_cast(0); 192 | for (SizeType D = Dimension; D > static_cast(0); --D) { 193 | SizeType d = D - static_cast(1); 194 | tmp_val = _currentIndex[d] + n[d] + carry; 195 | carry = tmp_val / size[d]; 196 | _currentIndex[d] = tmp_val % size[d]; 197 | } 198 | if (carry != static_cast(0)) { 199 | _currentIndex = IndexD::Zero(); 200 | _valid = false; 201 | } 202 | return *this; 203 | } 204 | 205 | template < typename T, SizeType Dimension, bool Const > 206 | inline typename Iterator::self_type Iterator::operator+ (const Index &n) const { 207 | self_type copy(*this); 208 | copy += n; 209 | return copy; 210 | } 211 | 212 | //////////////////////////////////////////////////////////////////////// 213 | 214 | 215 | 216 | //////////////////////////////////////////////////////////////////////// 217 | // DECREMENT OPERATIONS 218 | //////////////////////////////////////////////////////////////////////// 219 | 220 | template < typename T, SizeType Dimension, bool Const > 221 | inline typename Iterator::self_type & Iterator::operator--() { 222 | return *this -= static_cast(1); 223 | } 224 | 225 | template < typename T, SizeType Dimension, bool Const > 226 | inline typename Iterator::self_type Iterator::operator--(int) { 227 | self_type copy(*this); 228 | --*this; 229 | return copy; 230 | } 231 | 232 | template < typename T, SizeType Dimension, bool Const > 233 | inline typename Iterator::self_type & Iterator::operator-=(const SizeType n) 234 | { 235 | if (!_valid) 236 | return *this; 237 | Index size = _data.get().allSizes(); 238 | difference_type tmp_val, loan, carry = -static_cast(n); 239 | for (SizeType D = Dimension; D > static_cast(0) && carry != static_cast(0); --D) { 240 | SizeType d = D - static_cast(1); 241 | tmp_val = static_cast(_currentIndex[d]) + carry; 242 | loan = static_cast(0); 243 | if (tmp_val < static_cast(0)) 244 | loan = std::abs(tmp_val) / static_cast(size[d]) + (std::abs(tmp_val) % static_cast(size[d]) != static_cast(0) ? static_cast(1) : static_cast(0)); 245 | tmp_val += loan * static_cast(size[d]); 246 | carry = tmp_val / static_cast(size[d]) - loan; 247 | _currentIndex[d] = static_cast(tmp_val) % size[d]; 248 | } 249 | if (carry != static_cast(0)) { 250 | _currentIndex = IndexD::Zero(); 251 | _valid = false; 252 | } 253 | return *this; 254 | } 255 | 256 | template < typename T, SizeType Dimension, bool Const > 257 | inline typename Iterator::self_type Iterator::operator- (const SizeType n) const 258 | { 259 | self_type copy(*this); 260 | copy -= n; 261 | return copy; 262 | } 263 | 264 | template < typename T, SizeType Dimension, bool Const > 265 | inline typename Iterator::self_type & Iterator::operator-=(const Index &n) 266 | { 267 | if (!_valid) 268 | throw std::range_error("Iterator not valid."); 269 | Index size = _data.get().allSizes(); 270 | difference_type tmp_val, loan, carry = static_cast(0); 271 | for (SizeType D = Dimension; D > static_cast(0); --D) { 272 | SizeType d = D - static_cast(1); 273 | tmp_val = static_cast(_currentIndex[d]) + carry - static_cast(n[d]); 274 | loan = static_cast(0); 275 | if (tmp_val < static_cast(0)) 276 | loan = std::abs(tmp_val) / static_cast(size[d]) + (std::abs(tmp_val) % static_cast(size[d]) != static_cast(0) ? static_cast(1) : static_cast(0)); 277 | tmp_val += loan * static_cast(size[d]); 278 | carry = tmp_val / static_cast(size[d]) - loan; 279 | _currentIndex[d] = static_cast(tmp_val) % size[d]; 280 | } 281 | if (carry != static_cast(0)) { 282 | _currentIndex = IndexD::Zero(); 283 | _valid = false; 284 | } 285 | return *this; 286 | } 287 | 288 | template < typename T, SizeType Dimension, bool Const > 289 | inline typename Iterator::self_type Iterator::operator-(const Index &n) const 290 | { 291 | self_type copy(*this); 292 | copy -= n; 293 | return copy; 294 | } 295 | 296 | template < typename T, SizeType Dimension, bool Const > 297 | inline typename Iterator::difference_type Iterator::operator- (const self_type &o) const 298 | { 299 | if (!_valid || !o._valid) 300 | throw std::range_error("Iterator not valid."); 301 | if(&_data.get() == &o._data.get()) 302 | return static_cast(o.to_position()) - static_cast(to_position()); 303 | return std::numeric_limits::max(); 304 | } 305 | 306 | //////////////////////////////////////////////////////////////////////// 307 | 308 | 309 | 310 | //////////////////////////////////////////////////////////////////////// 311 | // BOOLEAN OPERATIONS 312 | //////////////////////////////////////////////////////////////////////// 313 | 314 | template < typename T, SizeType Dimension, bool Const > 315 | inline bool Iterator::operator==(const self_type &o) const 316 | { 317 | return &_data.get() == &o._data.get() && ((!_valid && !o._valid) || (_valid && o._valid && _currentIndex.isApprox(o._currentIndex))); 318 | } 319 | 320 | template < typename T, SizeType Dimension, bool Const > 321 | inline bool Iterator::operator!=(const self_type &o) const 322 | { 323 | return !(*this == o); 324 | } 325 | 326 | template < typename T, SizeType Dimension, bool Const > 327 | inline bool Iterator::operator< (const self_type &o) const 328 | { 329 | if (&_data.get() != &o._data.get()) 330 | throw std::logic_error("Iterators on different dope vectors is undefined."); 331 | return (_valid && !o._valid) || (_valid && o._valid && to_position() < o.to_position()); 332 | } 333 | 334 | template < typename T, SizeType Dimension, bool Const > 335 | inline bool Iterator::operator<=(const self_type &o) const 336 | { 337 | return !(o < *this); 338 | } 339 | 340 | template < typename T, SizeType Dimension, bool Const > 341 | inline bool Iterator::operator> (const self_type &o) const 342 | { 343 | return o < *this; 344 | } 345 | 346 | template < typename T, SizeType Dimension, bool Const > 347 | inline bool Iterator::operator>=(const self_type &o) const 348 | { 349 | return !(*this < o); 350 | } 351 | 352 | //////////////////////////////////////////////////////////////////////// 353 | 354 | } // namespace internal 355 | 356 | } // namespace dope 357 | -------------------------------------------------------------------------------- /include/DopeVector/internal/inlines/eigen_support/EigenExpression.inl: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2016 Giorgio Marcias & Maurizio Kovacic 2 | // 3 | // This source code is part of DopeVector header library 4 | // and it is subject to Apache 2.0 License. 5 | // 6 | // Author: Giorgio Marcias 7 | // email: marcias.giorgio@gmail.com 8 | // Author: Maurizio Kovacic 9 | // email: maurizio.kovacic@gmail.com 10 | 11 | #ifdef DOPE_USE_EIGEN 12 | 13 | #include 14 | 15 | namespace dope { 16 | 17 | namespace internal { 18 | 19 | template < class Derived, class Er, typename T, SizeType Dimension, typename Op > 20 | const Op EigenStaticArrayBinaryExpression::_op = Op(); 21 | 22 | template < class Derived, class Er, typename T, SizeType Dimension, typename Op > 23 | inline EigenStaticArrayBinaryExpression::EigenStaticArrayBinaryExpression(const Eigen::MatrixBase &el, const Er &er) 24 | : _el(el), _er(er) 25 | { } 26 | 27 | template < class Derived, class Er, typename T, SizeType Dimension, typename Op > 28 | inline T EigenStaticArrayBinaryExpression::operator[](const SizeType i) const 29 | { 30 | if (!_values[i]) { 31 | T t = _op(_el[i], _er.getAt(i)); 32 | _values[i] = [t]()->T{ return t; }; 33 | } 34 | return _values[i](); 35 | } 36 | 37 | 38 | 39 | template < class El, typename T, SizeType Dimension, class Derived, typename Op > 40 | const Op StaticArrayBinaryEigenExpression::_op = Op(); 41 | 42 | template < class El, typename T, SizeType Dimension, class Derived, typename Op > 43 | inline StaticArrayBinaryEigenExpression::StaticArrayBinaryEigenExpression(const El &el, const Eigen::MatrixBase &er) 44 | : _el(el), _er(er) 45 | { } 46 | 47 | template < class El, typename T, SizeType Dimension, class Derived, typename Op > 48 | inline T StaticArrayBinaryEigenExpression::operator[](const SizeType i) const 49 | { 50 | if (!_values[i]) { 51 | T t = _op(_el.getAt(i), _er[i]); 52 | _values[i] = [t]()->T{ return t; }; 53 | } 54 | return _values[i](); 55 | } 56 | 57 | 58 | 59 | template < class Derived, class Er, typename T, SizeType Dimension > 60 | inline EigenStaticArrayBinaryExpression> operator+ (const Eigen::MatrixBase &el, const StaticArrayExpression &er) 61 | { 62 | return EigenStaticArrayBinaryExpression>(el, er); 63 | } 64 | 65 | 66 | 67 | template < class Derived, class Er, typename T, SizeType Dimension > 68 | inline EigenStaticArrayBinaryExpression> operator- (const Eigen::MatrixBase &el, const StaticArrayExpression &er) 69 | { 70 | return EigenStaticArrayBinaryExpression>(el, er); 71 | } 72 | 73 | 74 | 75 | template < class Derived, class Er, typename T, SizeType Dimension > 76 | inline EigenStaticArrayBinaryExpression> operator* (const Eigen::MatrixBase &el, const StaticArrayExpression &er) 77 | { 78 | return EigenStaticArrayBinaryExpression>(el, er); 79 | } 80 | 81 | 82 | 83 | template < class Derived, class Er, typename T, SizeType Dimension > 84 | inline EigenStaticArrayBinaryExpression> operator/ (const Eigen::MatrixBase &el, const StaticArrayExpression &er) 85 | { 86 | return EigenStaticArrayBinaryExpression>(el, er); 87 | } 88 | 89 | 90 | 91 | template < class Derived, class Er, typename T, SizeType Dimension > 92 | inline EigenStaticArrayBinaryExpression> operator% (const Eigen::MatrixBase &el, const StaticArrayExpression &er) 93 | { 94 | return EigenStaticArrayBinaryExpression>(el, er); 95 | } 96 | 97 | 98 | 99 | template < class El, typename T, SizeType Dimension, class Derived > 100 | inline StaticArrayBinaryEigenExpression> operator+ (const StaticArrayExpression &el, const Eigen::MatrixBase &er) 101 | { 102 | return StaticArrayBinaryEigenExpression>(el, er); 103 | } 104 | 105 | 106 | 107 | template < class El, typename T, SizeType Dimension, class Derived > 108 | inline StaticArrayBinaryEigenExpression> operator- (const StaticArrayExpression &el, const Eigen::MatrixBase &er) 109 | { 110 | return StaticArrayBinaryEigenExpression>(el, er); 111 | } 112 | 113 | 114 | 115 | template < class El, typename T, SizeType Dimension, class Derived > 116 | inline StaticArrayBinaryEigenExpression> operator* (const StaticArrayExpression &el, const Eigen::MatrixBase &er) 117 | { 118 | return StaticArrayBinaryEigenExpression>(el, er); 119 | } 120 | 121 | 122 | 123 | template < class El, typename T, SizeType Dimension, class Derived > 124 | inline StaticArrayBinaryEigenExpression> operator/ (const StaticArrayExpression &el, const Eigen::MatrixBase &er) 125 | { 126 | return StaticArrayBinaryEigenExpression>(el, er); 127 | } 128 | 129 | 130 | 131 | template < class El, typename T, SizeType Dimension, class Derived > 132 | inline StaticArrayBinaryEigenExpression> operator% (const StaticArrayExpression &el, const Eigen::MatrixBase &er) 133 | { 134 | return StaticArrayBinaryEigenExpression>(el, er); 135 | } 136 | 137 | } 138 | 139 | } 140 | 141 | #endif 142 | -------------------------------------------------------------------------------- /include/distance_transform/README.md: -------------------------------------------------------------------------------- 1 | This program was created by [giorgiomarcias](https://github.com/giorgiomarcias). 2 | 3 | https://github.com/giorgiomarcias/distance_transform 4 | 5 | -------------------------------------------------------------------------------- /include/distance_transform/distance_transform.hpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2016 Giorgio Marcias 2 | // 3 | // This file is part of distance_transform, a C++11 implementation of the 4 | // algorithm in "Distance Transforms of Sampled Functions" 5 | // Pedro F. Felzenszwalb, Daniel P. Huttenlocher 6 | // Theory of Computing, Vol. 8, No. 19, September 2012 7 | // 8 | // This source code is subject to Apache 2.0 License. 9 | // 10 | // Author: Giorgio Marcias 11 | // email: marcias.giorgio@gmail.com 12 | 13 | #ifndef distance_transform_hpp 14 | #define distance_transform_hpp 15 | 16 | #include 17 | #include 18 | 19 | namespace dt { 20 | 21 | /// The DistanceTransform class provides static method to compute a distance field over any multi-dimensional regularly sampled function. 22 | /// The dimension is fixed at compile time. 23 | /// It is also possible to compute the index of the nearest minimum of each sample. 24 | class DistanceTransform { 25 | public: 26 | /** 27 | * @brief Compute the L2-norm distance field D of a DIM-dimensional sampled function f. D gets the distance from the local minima of f. 28 | * @param f A DIM-dimensional, regularly sampled function. 29 | * @param D The resulting distance field of f. 30 | * @param squared Compute squared distances (L2)^2 - avoiding to compute square roots - (true) or keep them normal (false - default). 31 | * @param nThreads The number of threads for parallel computation. If <= 1, the computation will be sequential. 32 | * @note Arrays f and D can also be the same. 33 | */ 34 | template < typename Scalar, dope::SizeType DIM > 35 | inline static void distanceTransformL2(const dope::DopeVector &f, dope::DopeVector &D, const bool squared = false, const std::size_t nThreads = std::thread::hardware_concurrency()); 36 | 37 | /** 38 | * @brief Compute the L2-norm distance field D of a 1-dimensional sampled function f. D gets the distance from the local minima of f. 39 | * @param f A 1-dimensional, regularly sampled function. 40 | * @param D The resulting distance field of f. 41 | * @param squared Compute squared distances (L2)^2 - avoiding to compute square roots - (true) or keep them normal (false - default). 42 | * @param nThreads The number of threads for parallel computation. Actually NOT used, since it's not easy to run a single row computation in parallel. 43 | * @note Arrays f and D can also be the same. 44 | */ 45 | template < typename Scalar > 46 | inline static void distanceTransformL2(const dope::DopeVector &f, dope::DopeVector &D, const bool squared = false, const std::size_t nThreads = std::thread::hardware_concurrency()); 47 | 48 | /** 49 | * @brief Compute the L2-norm distance field D of a DIM-dimensional sampled function f. D gets the distance from the local minima of f. 50 | * Compute also the (index of the) nearest local minimum of each sample. 51 | * @param f A DIM-dimensional, regularly sampled function. 52 | * @param D The resulting distance field of f. 53 | * @param I Resulting array containing the (index of the) local minimum for each sample. 54 | * @param squared Compute squared distances (L2)^2 - avoiding to compute square roots - (true) or keep them normal (false - default). 55 | * @param nThreads The number of threads for parallel computation. If <= 1, the computation will be sequential. 56 | * @note Arrays f and D can also be the same. I should be first initialized. 57 | */ 58 | template < typename Scalar, dope::SizeType DIM > 59 | inline static void distanceTransformL2(const dope::DopeVector &f, dope::DopeVector &D, dope::DopeVector &I, const bool squared = false, const std::size_t nThreads = std::thread::hardware_concurrency()); 60 | 61 | /** 62 | * @brief Compute the L2-norm distance field D of a 1-dimensional sampled function f. D gets the distance from the local minima of f. 63 | * Compute also the (index of the) nearest local minimum of each sample. 64 | * @param f A 1-dimensional, regularly sampled function. 65 | * @param D The resulting distance field of f. 66 | * @param I Resulting array containing the (index of the) local minimum for each sample. 67 | * @param squared Compute squared distances (L2)^2 - avoiding to compute square roots - (true) or keep them normal (false - default). 68 | * @param nThreads The number of threads for parallel computation. Actually NOT used, since it's not easy to run a single row computation in parallel. 69 | * @note Arrays f and D can also be the same. I sould be first initialized. 70 | */ 71 | template < typename Scalar > 72 | inline static void distanceTransformL2(const dope::DopeVector &f, dope::DopeVector &D, dope::DopeVector &I, const bool squared = false, const std::size_t nThreads = std::thread::hardware_concurrency()); 73 | 74 | /** 75 | * @brief Set up the initial indices of a DIM-dimensional sampled function. 76 | * @param I Resulting array containing the initial index for each sample. 77 | */ 78 | template < dope::SizeType DIM > 79 | inline static void initializeIndices(dope::DopeVector &I); 80 | 81 | /** 82 | * @brief Set up the initial indices of a 1-dimensional sampled function. 83 | * @param I Resulting array containing the initial index for each sample. 84 | */ 85 | inline static void initializeIndices(dope::DopeVector &I); 86 | 87 | 88 | 89 | private: 90 | /** 91 | * @brief The loop iteration process that can be executed sequentially and concurrently as well. 92 | * @param f A DIM-dimensional, regularly sampled function (a window, in multi-threading). 93 | * @param D The resulting distance field of f (a window, in multi-threading). 94 | * @param d The dimension where to slice. 95 | * @param order The order in which to permute the slices. 96 | */ 97 | template < typename Scalar, dope::SizeType DIM > 98 | inline static void distanceL2Helper(const dope::DopeVector &f, dope::DopeVector &D); 99 | 100 | /** 101 | * @brief The actual distance field computation is done by recursive calls of this method, in lower dimenional sub-functions. 102 | * @param f A DIM-dimensional, regularly sampled function. 103 | * @param D The resulting distance field of f. 104 | */ 105 | template < typename Scalar, dope::SizeType DIM > 106 | inline static void distanceL2(const dope::DopeVector &f, dope::DopeVector &D); 107 | 108 | /** 109 | * @brief The actual distance field computation as in the "Distance Transforms of Sampled Functions" paper, performed in a mono-dimensional function. 110 | * @param f A 1-dimensional, regularly sampled function. 111 | * @param D The resulting distance field of f. 112 | */ 113 | template < typename Scalar > 114 | inline static void distanceL2(const dope::DopeVector &f, dope::DopeVector &D); 115 | 116 | /** 117 | * @brief The loop iteration process that can be executed sequentially and concurrently as well. 118 | * @param f A DIM-dimensional, regularly sampled function (a window, in multi-threading). 119 | * @param D The resulting distance field of f (a window, in multi-threading). 120 | * @param Ipre Array containing the initial inedx of local minimum for each sample. 121 | * @param Ipost Array containing the resulting index of local minimum for each sample. 122 | */ 123 | template < typename Scalar, dope::SizeType DIM > 124 | inline static void distanceL2Helper(const dope::DopeVector &f, dope::DopeVector &D, const dope::DopeVector &Ipre, dope::DopeVector &Ipost); 125 | 126 | /** 127 | * @brief The actual distance field computation is done by recursive calls of this method, in lower dimenional sub-functions. 128 | * @param f A DIM-dimensional, regularly sampled function. 129 | * @param D The resulting distance field of f. 130 | * @param I Resulting array containing the index of the local minimum for each sample. 131 | */ 132 | template < typename Scalar, dope::SizeType DIM > 133 | inline static void distanceL2(const dope::DopeVector &f, dope::DopeVector &D, const dope::DopeVector &Ipre, dope::DopeVector &Ipost); 134 | 135 | /** 136 | * @brief The actual distance field computation as in the "Distance Transforms of Sampled Functions" paper, performed in a mono-dimensional function. 137 | * @param f A 1-dimensional, regularly sampled function. 138 | * @param D The resulting distance field of f. 139 | * @param I Resulting array containing the (index of the) local minimum for each sample. 140 | */ 141 | template < typename Scalar > 142 | inline static void distanceL2(const dope::DopeVector &f, dope::DopeVector &D, const dope::DopeVector &Ipre, dope::DopeVector &Ipost); 143 | 144 | public: 145 | /** 146 | * @brief Compute the square root of each individual element of a DIM-dimensional array. 147 | * @param m A DIM-dimensioanl array whose element have to be square rooted. 148 | */ 149 | template < typename Scalar, dope::SizeType DIM > 150 | inline static void element_wiseSquareRoot(dope::DopeVector &m); 151 | 152 | /** 153 | * @brief Compute the square root of each individual element of a 1-dimensional array. 154 | * @param m A 1-dimensioanl array whose element have to be square rooted. 155 | */ 156 | template < typename Scalar > 157 | inline static void element_wiseSquareRoot(dope::DopeVector &m); 158 | 159 | }; 160 | 161 | } 162 | 163 | #include 164 | 165 | #endif /* distance_transform_hpp */ 166 | -------------------------------------------------------------------------------- /include/distance_transform/inlines/distance_transform.inl: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2016 Giorgio Marcias 2 | // 3 | // This file is part of distance_transform, a C++11 implementation of the 4 | // algorithm in "Distance Transforms of Sampled Functions" 5 | // Pedro F. Felzenszwalb, Daniel P. Huttenlocher 6 | // Theory of Computing, Vol. 8, No. 19, September 2012 7 | // 8 | // This source code is subject to Apache 2.0 License. 9 | // 10 | // Author: Giorgio Marcias 11 | // email: marcias.giorgio@gmail.com 12 | 13 | #include 14 | #include 15 | 16 | namespace dt { 17 | 18 | template < typename Scalar, dope::SizeType DIM > 19 | inline void DistanceTransform::distanceTransformL2(const dope::DopeVector &f, dope::DopeVector &D, const bool squared, const std::size_t nThreads) 20 | { 21 | dope::Index fSize, DSize; 22 | fSize = f.allSizes(); 23 | DSize = D.allSizes(); 24 | if (DSize != fSize) 25 | throw std::out_of_range("Matrixes do not have same size."); 26 | 27 | dope::Grid fCopy(fSize); 28 | fCopy.import(f); 29 | dope::Grid DCopy(DSize); 30 | 31 | dope::DopeVector tmpF(fCopy); 32 | dope::DopeVector tmpD(DCopy); 33 | 34 | dope::Index order; 35 | 36 | for (dope::SizeType d = static_cast(0); d < DIM; ++d) { 37 | // permute rotate 38 | for (dope::SizeType o = static_cast(0); o < DIM; ++o) 39 | order[o] = (d + o) % DIM; 40 | dope::DopeVector tmpF_rotated = tmpF.permute(order); 41 | dope::DopeVector tmpD_rotated = tmpD.permute(order); 42 | 43 | 44 | dope::Index winStart = dope::Index::Zero(), winSize; 45 | tmpF_rotated.allSizes(winSize); 46 | 47 | dope::SizeType range = winSize[0]; 48 | if (nThreads < range) { 49 | range += range % nThreads; 50 | range /= nThreads; 51 | } 52 | std::size_t nWindows = winSize[0] / range + (winSize[0] % range != 0 ? 1 : 0); 53 | 54 | if (nWindows > 1) { 55 | std::vector> tmpWindowsF(nWindows); 56 | std::vector> tmpWindowsD(nWindows); 57 | std::vector threads(nWindows); 58 | 59 | for (std::size_t i = 0; i < nWindows; ++i) { 60 | winStart[0] = i * range; 61 | winSize[0] = std::min(range, tmpF_rotated.sizeAt(0) - winStart[0]); 62 | tmpWindowsF.at(i) = tmpF_rotated.window(winStart, winSize); 63 | tmpWindowsD.at(i) = tmpD_rotated.window(winStart, winSize); 64 | winStart[0] = 0; 65 | winSize[0] = tmpF_rotated.sizeAt(0); 66 | threads.at(i) = std::thread(static_cast &, dope::DopeVector &)>(&distanceL2Helper), std::cref(tmpWindowsF.at(i)), std::ref(tmpWindowsD.at(i))); 67 | } 68 | for (std::size_t i = 0; i < nWindows; ++i) 69 | threads.at(i).join(); 70 | } else { 71 | distanceL2Helper(tmpF_rotated, tmpD_rotated); 72 | } 73 | 74 | std::swap(tmpD, tmpF); 75 | } 76 | 77 | if (DIM % 2 == 0) 78 | DCopy = std::move(fCopy); 79 | 80 | D.import(DCopy); 81 | 82 | if (!squared) 83 | element_wiseSquareRoot(D); 84 | } 85 | 86 | 87 | 88 | template < typename Scalar > 89 | inline void DistanceTransform::distanceTransformL2(const dope::DopeVector &f, dope::DopeVector &D, const bool squared, const std::size_t) 90 | { 91 | dope::Index1 fSize, DSize; 92 | fSize = f.allSizes(); 93 | DSize = D.allSizes(); 94 | if (DSize != fSize) 95 | throw std::out_of_range("Matrixes do not have same size."); 96 | 97 | distanceL2(f, D); 98 | 99 | if (!squared) 100 | element_wiseSquareRoot(D); 101 | } 102 | 103 | 104 | 105 | template < typename Scalar, dope::SizeType DIM > 106 | inline void DistanceTransform::distanceTransformL2(const dope::DopeVector &f, dope::DopeVector &D, dope::DopeVector &I, const bool squared, const std::size_t nThreads) 107 | { 108 | dope::Index fSize, DSize, ISize; 109 | fSize = f.allSizes(); 110 | DSize = D.allSizes(); 111 | ISize = I.allSizes(); 112 | if (DSize != fSize || ISize != fSize) 113 | throw std::out_of_range("Matrixes do not have same size."); 114 | 115 | dope::Grid fCopy(fSize); 116 | fCopy.import(f); 117 | dope::Grid DCopy(DSize); 118 | dope::Grid ICopyPre(ISize), ICopyPost(ISize); 119 | ICopyPre.import(I); 120 | 121 | dope::DopeVector tmpF(fCopy); 122 | dope::DopeVector tmpD(D); 123 | dope::DopeVector Ipre(ICopyPre); 124 | dope::DopeVector Ipost(ICopyPost); 125 | 126 | dope::Index order; 127 | 128 | for (dope::SizeType d = static_cast(0); d < DIM; ++d) { 129 | // permute rotate 130 | for (dope::SizeType o = static_cast(0); o < DIM; ++o) 131 | order[o] = (d + o) % DIM; 132 | dope::DopeVector tmpF_rotated = tmpF.permute(order); 133 | dope::DopeVector tmpD_rotated = tmpD.permute(order); 134 | dope::DopeVector Ipre_rotated = Ipre.permute(order); 135 | dope::DopeVector Ipost_rotated = Ipost.permute(order); 136 | 137 | dope::Index winStart = dope::Index::Zero(), winSize; 138 | tmpF_rotated.allSizes(winSize); 139 | 140 | std::size_t range = winSize[0]; 141 | if (nThreads < range) { 142 | range += range % nThreads; 143 | range /= nThreads; 144 | } 145 | std::size_t nWindows = winSize[0] / range + (winSize[0] % range != 0 ? 1 : 0); 146 | 147 | if (nWindows > 1) { 148 | std::vector> tmpWindowsF(nWindows); 149 | std::vector> tmpWindowsD(nWindows); 150 | std::vector> tmpWindowsIPre(nWindows); 151 | std::vector> tmpWindowsIPost(nWindows); 152 | std::vector threads(nWindows); 153 | 154 | for (std::size_t i = 0; i < nWindows; ++i) { 155 | winStart[0] = i * range; 156 | winSize[0] = std::min(range, tmpF_rotated.sizeAt(0) - winStart[0]); 157 | tmpWindowsF.at(i) = tmpF_rotated.window(winStart, winSize); 158 | tmpWindowsD.at(i) = tmpD_rotated.window(winStart, winSize); 159 | tmpWindowsIPre.at(i) = Ipre_rotated.window(winStart, winSize); 160 | tmpWindowsIPost.at(i) = Ipost_rotated.window(winStart, winSize); 161 | winStart[0] = 0; 162 | winSize[0] = tmpF_rotated.sizeAt(0); 163 | threads.at(i) = std::thread(static_cast &, dope::DopeVector &, const dope::DopeVector &, dope::DopeVector &)>(&distanceL2Helper), std::cref(tmpWindowsF.at(i)), std::ref(tmpWindowsD.at(i)), std::cref(tmpWindowsIPre.at(i)), std::ref(tmpWindowsIPost.at(i))); 164 | } 165 | for (std::size_t i = 0; i < nWindows; ++i) 166 | threads.at(i).join(); 167 | } else { 168 | distanceL2Helper(tmpF_rotated, tmpD_rotated, Ipre_rotated, Ipost_rotated); 169 | } 170 | 171 | std::swap(tmpD, tmpF); 172 | std::swap(Ipost, Ipre); 173 | } 174 | 175 | if (DIM % 2 == 0) { 176 | DCopy = std::move(fCopy); 177 | ICopyPost = std::move(ICopyPre); 178 | } 179 | 180 | D.import(DCopy); 181 | I.import(ICopyPost); 182 | 183 | if (!squared) 184 | element_wiseSquareRoot(D); 185 | } 186 | 187 | 188 | 189 | 190 | template < typename Scalar > 191 | inline void DistanceTransform::distanceTransformL2(const dope::DopeVector &f, dope::DopeVector &D, dope::DopeVector &I, const bool squared, const std::size_t) 192 | { 193 | dope::Index1 fSize, DSize, ISize; 194 | fSize = f.allSizes(); 195 | DSize = D.allSizes(); 196 | ISize = I.allSizes(); 197 | if (DSize != fSize || ISize != fSize) 198 | throw std::out_of_range("Matrixes do not have same size."); 199 | 200 | distanceL2(f, D, I); 201 | 202 | if (!squared) 203 | element_wiseSquareRoot(D); 204 | } 205 | 206 | 207 | 208 | template < dope::SizeType DIM > 209 | inline void DistanceTransform::initializeIndices(dope::DopeVector &I) 210 | { 211 | dope::DopeVector I_q; 212 | for (dope::SizeType q = static_cast(0); q < I.sizeAt(0); ++q) { 213 | I.at(q, I_q); 214 | initializeIndices(I_q); 215 | } 216 | } 217 | 218 | 219 | 220 | inline void DistanceTransform::initializeIndices(dope::DopeVector &I) 221 | { 222 | for (dope::SizeType q = static_cast(0); q < I.sizeAt(0); ++q) 223 | I[q] = I.accumulatedOffset(q); 224 | } 225 | 226 | 227 | 228 | template < typename Scalar, dope::SizeType DIM > 229 | inline void DistanceTransform::distanceL2Helper(const dope::DopeVector &f, dope::DopeVector &D) 230 | { 231 | dope::DopeVector f_dq; 232 | dope::DopeVector D_dq; 233 | 234 | for (dope::SizeType q = static_cast(0); q < f.sizeAt(0); ++q) { 235 | f.slice(0, q, f_dq); 236 | D.slice(0, q, D_dq); 237 | distanceL2(f_dq, D_dq); 238 | } 239 | } 240 | 241 | 242 | 243 | template < typename Scalar, dope::SizeType DIM > 244 | inline void DistanceTransform::distanceL2(const dope::DopeVector &f, dope::DopeVector &D) 245 | { 246 | dope::DopeVector f_q, D_q; 247 | // compute distance at lower dimensions for each hyperplane 248 | for (dope::SizeType q = static_cast(0); q < f.sizeAt(0); ++q) { 249 | f.at(q, f_q); 250 | D.at(q, D_q); 251 | distanceL2(f_q, D_q); 252 | } 253 | } 254 | 255 | 256 | 257 | template < typename Scalar > 258 | inline void DistanceTransform::distanceL2(const dope::DopeVector &f, dope::DopeVector &D) 259 | { 260 | if (f.sizeAt(0) == static_cast(0) || f.sizeAt(0) > D.sizeAt(0)) 261 | return; 262 | if (f.sizeAt(0) == static_cast(1)) { 263 | D[0] = f[0]; 264 | return; 265 | } 266 | dope::SizeType k = static_cast(0); // index of rightmost parabola in lower envelope 267 | dope::SizeType *v = new dope::SizeType[f.sizeAt(0)]; // locations of parabolas in lower envelope 268 | double *z = new double[f.sizeAt(0) + 1]; // locations of boundaries between parabolas 269 | double s = double(0); 270 | // initialization 271 | v[0] = static_cast(0); 272 | z[0] = -std::numeric_limits::max(); 273 | z[1] = std::numeric_limits::max(); 274 | // compute lowest envelope: 275 | for (dope::SizeType q = static_cast(1); q < f.sizeAt(0); ++q) { 276 | ++k; // this compensates for first line of next do-while block 277 | do { 278 | --k; 279 | // compute horizontal position of intersection between the parabola from q and the current lowest parabola 280 | s = ((f[q] + q*q) - static_cast(f[v[k]] + v[k]*v[k])) / (2*q - static_cast(2*v[k])); 281 | } while (s <= z[k]); 282 | ++k; 283 | v[k] = q; 284 | z[k] = s; 285 | z[k+1] = std::numeric_limits::max(); 286 | } 287 | // fill in values of distance transform 288 | k = static_cast(0); 289 | for (dope::SizeType q = static_cast(0); q < f.sizeAt(0); ++q) { 290 | while(z[k+1] < static_cast(q)) 291 | ++k; 292 | D[q] = f[v[k]] + (q - static_cast(v[k]))*(q - static_cast(v[k])); 293 | } 294 | // delete allocated memory 295 | delete[] z; 296 | delete[] v; 297 | } 298 | 299 | 300 | 301 | template < typename Scalar, dope::SizeType DIM > 302 | inline void DistanceTransform::distanceL2Helper(const dope::DopeVector &f, dope::DopeVector &D, const dope::DopeVector &Ipre, dope::DopeVector &Ipost) 303 | { 304 | dope::DopeVector f_dq; 305 | dope::DopeVector D_dq; 306 | dope::DopeVector Ipre_dq; 307 | dope::DopeVector Ipost_dq; 308 | 309 | for (dope::SizeType q = static_cast(0); q < f.sizeAt(0); ++q) { 310 | f.slice(0, q, f_dq); 311 | D.slice(0, q, D_dq); 312 | Ipre.slice(0, q, Ipre_dq); 313 | Ipost.slice(0, q, Ipost_dq); 314 | distanceL2(f_dq, D_dq, Ipre_dq, Ipost_dq); 315 | } 316 | } 317 | 318 | 319 | 320 | template < typename Scalar, dope::SizeType DIM > 321 | inline void DistanceTransform::distanceL2(const dope::DopeVector &f, dope::DopeVector &D, const dope::DopeVector &Ipre, dope::DopeVector &Ipost) 322 | { 323 | dope::DopeVector f_q, D_q; 324 | dope::DopeVector Ipre_q, Ipost_q; 325 | // compute distance at lower dimensions for each hyperplane 326 | for (dope::SizeType q = static_cast(0); q < f.sizeAt(0); ++q) { 327 | f.at(q, f_q); 328 | D.at(q, D_q); 329 | Ipre.at(q, Ipre_q); 330 | Ipost.at(q, Ipost_q); 331 | distanceL2(f_q, D_q, Ipre_q, Ipost_q); 332 | } 333 | } 334 | 335 | 336 | 337 | template < typename Scalar > 338 | inline void DistanceTransform::distanceL2(const dope::DopeVector &f, dope::DopeVector &D, const dope::DopeVector &Ipre, dope::DopeVector &Ipost) 339 | { 340 | if (f.sizeAt(0) == static_cast(0) || f.sizeAt(0) > D.sizeAt(0)) 341 | return; 342 | if (f.sizeAt(0) == static_cast(1)) { 343 | D[0] = f[0]; 344 | Ipost[0] = Ipre[0]; 345 | return; 346 | } 347 | dope::SizeType k = static_cast(0); // index of rightmost parabola in lower envelope 348 | dope::SizeType *v = new dope::SizeType[f.sizeAt(0)]; // locations of parabolas in lower envelope 349 | double *z = new double[f.sizeAt(0) + 1]; // locations of boundaries between parabolas 350 | double s = double(0); 351 | // initialization 352 | v[0] = static_cast(0); 353 | z[0] = -std::numeric_limits::max(); 354 | z[1] = std::numeric_limits::max(); 355 | // compute lowest envelope: 356 | for (dope::SizeType q = static_cast(1); q < f.sizeAt(0); ++q) { 357 | ++k; // this compensates for first line of next do-while block 358 | do { 359 | --k; 360 | // compute horizontal position of intersection between the parabola from q and the current lowest parabola 361 | s = ((f[q] + q*q) - static_cast(f[v[k]] + v[k]*v[k])) / (2*q - static_cast(2*v[k])); 362 | } while (s <= z[k]); 363 | ++k; 364 | v[k] = q; 365 | z[k] = s; 366 | z[k+1] = std::numeric_limits::max(); 367 | } 368 | // fill in values of distance transform 369 | k = static_cast(0); 370 | for (dope::SizeType q = static_cast(0); q < f.sizeAt(0); ++q) { 371 | while(z[k+1] < static_cast(q)) 372 | ++k; 373 | D[q] = f[v[k]] + (q - static_cast(v[k]))*(q - static_cast(v[k])); 374 | Ipost[q] = Ipre[v[k]]; 375 | } 376 | // delete allocated memory 377 | delete[] z; 378 | delete[] v; 379 | } 380 | 381 | 382 | 383 | template < typename Scalar, dope::SizeType DIM > 384 | inline void DistanceTransform::element_wiseSquareRoot(dope::DopeVector &m) 385 | { 386 | dope::DopeVector mm; 387 | for (dope::SizeType q = static_cast(0); q < m.sizeAt(0); ++q) { 388 | m.at(q, mm); 389 | element_wiseSquareRoot(mm); 390 | } 391 | } 392 | 393 | 394 | 395 | template < typename Scalar > 396 | inline void DistanceTransform::element_wiseSquareRoot(dope::DopeVector &m) 397 | { 398 | for (dope::SizeType q = static_cast(0); q < m.sizeAt(0); ++q) 399 | m[q] = static_cast(std::sqrt(m[q])); 400 | } 401 | 402 | } 403 | -------------------------------------------------------------------------------- /include/mcl3d_ros/DistanceField.h: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * mcl3d_ros: 3D Monte Carlo localization for ROS use 3 | * Copyright (C) 2023 Naoki Akai 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 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an “AS IS” BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * 14 | * See the License for the specific language governing permissions and 15 | * limitations under the License. 16 | * 17 | * @author Naoki Akai 18 | ****************************************************************************/ 19 | 20 | #ifndef __DISTANCE_FIELD_H__ 21 | #define __DISTANCE_FIELD_H__ 22 | 23 | #include 24 | #include 25 | #include 26 | #include 27 | #include 28 | #include 29 | #include 30 | #include 31 | 32 | namespace mcl3d { 33 | 34 | #define MAP_VAL_EXIST_POINT (0) 35 | #define MAP_VAL_INVALID (255) 36 | 37 | class DistanceField { 38 | public: 39 | std::string rootDirName_, mapFileName_; 40 | float resolution_, subMapResolution_; 41 | std::vector origin_; 42 | int width_, depth_, height_, subMapWidth_, subMapDepth_, subMapHeight_; 43 | unsigned int totalMapPointsNum_; 44 | std::vector>> subMapPointsNums_; 45 | std::vector>>> subMapOrigins_; 46 | std::vector>>>>> subMaps_; 47 | std::vector>>> distanceLists_; 48 | bool isAvailable_, debugFlag_; 49 | int dfComputationThreadsNum_; 50 | 51 | DistanceField(void) {} 52 | DistanceField(std::string yamlFilePath); 53 | DistanceField(std::string mapFileName, float resolution, float subMapResolution, float mapMargin, 54 | Point minPoint, Point maxPoint, std::string yamlFilePath); 55 | ~DistanceField() {}; 56 | void initializeSubMaps(void); 57 | void saveSubDistanceMap(int uo, int vo, int wo, FILE *fp); 58 | bool saveDistanceMap(void); 59 | bool loadDistanceMap(void); 60 | void writeMapPoints(std::string fname, float minX, float maxX, float minY, float maxY, float minZ, float maxZ); 61 | std::vector getMapPoints(float minX, float maxX, float minY, float maxY, float minZ, float maxZ); 62 | 63 | // functions to get parameters 64 | inline int getWidth(void) { return width_; } 65 | inline int getDepth(void) { return depth_; } 66 | inline int getHeight(void) { return height_; } 67 | inline float getResolution(void) { return resolution_; } 68 | inline float getSubMapResolution(void) { return subMapResolution_; } 69 | 70 | // functions to set parameters 71 | inline void setDebugFlag(bool debugFlag) { debugFlag_ = debugFlag; } 72 | inline void setDFComputationThreadsNum(int dfComputationThreadsNum) { dfComputationThreadsNum_ = dfComputationThreadsNum; } 73 | 74 | inline bool xyz2uvw(std::vector origin, float resolution, int width, int depth, int height, float x, float y, float z, int *u, int *v, int *w) { 75 | *u = (int)((x - origin[0]) / resolution); 76 | *v = (int)((y - origin[1]) / resolution); 77 | *w = (int)((z - origin[2]) / resolution); 78 | if (0 <= *u && *u < width && 0 <= *v && *v origin, float resolution, int u, int v, int w, float *x, float *y, float *z) { 85 | *x = (float)u * resolution + origin[0]; 86 | *y = (float)v * resolution + origin[1]; 87 | *z = (float)w * resolution + origin[2]; 88 | } 89 | 90 | void addPoint(float x, float y, float z); 91 | 92 | inline void addPoint(Point p) { 93 | addPoint(p.getX(), p.getY(), p.getZ()); 94 | } 95 | 96 | inline float getDistance(float x, float y, float z) { 97 | if (!isAvailable_) 98 | return -1000.0f; 99 | int u, v, w; 100 | if (!xyz2uvw(origin_, resolution_, width_, depth_, height_, x, y, z, &u, &v, &w)) 101 | return -1000.0f; 102 | if (subMapPointsNums_[u][v][w] == 0) 103 | return -1000.0f; 104 | int uu, vv, ww; 105 | if (xyz2uvw(subMapOrigins_[u][v][w], subMapResolution_, subMapWidth_, subMapDepth_, subMapHeight_, x, y, z, &uu, &vv, &ww)) 106 | return distanceLists_[u][v][w][subMaps_[u][v][w][uu][vv][ww]]; 107 | else 108 | return -1000.0f; 109 | } 110 | 111 | inline float getDistance(Point point) { 112 | return getDistance(point.getX(), point.getY(), point.getZ()); 113 | } 114 | 115 | inline std::vector getDistances(std::vector points) { 116 | std::vector distances((int)points.size()); 117 | for (int i = 0; i < (int)points.size(); i++) 118 | distances[i] = getDistance(points[i].getX(), points[i].getY(), points[i].getZ()); 119 | return distances; 120 | } 121 | 122 | inline unsigned int getTotalMapPointsNum(void) { 123 | return totalMapPointsNum_; 124 | } 125 | }; // class DistanceField 126 | 127 | } // namespace mcl3d 128 | 129 | #endif // __DISTANCE_FIELD_H__ -------------------------------------------------------------------------------- /include/mcl3d_ros/IMU.h: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * mcl3d_ros: 3D Monte Carlo localization for ROS use 3 | * Copyright (C) 2023 Naoki Akai 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 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an “AS IS” BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * 14 | * See the License for the specific language governing permissions and 15 | * limitations under the License. 16 | * 17 | * @author Naoki Akai 18 | ****************************************************************************/ 19 | 20 | #ifndef __IMU_H__ 21 | #define __IMU_H__ 22 | 23 | #include 24 | #include 25 | 26 | namespace mcl3d { 27 | 28 | class IMU { 29 | private: 30 | double ax_, ay_, az_, gx_, gy_, gz_; 31 | 32 | double q0_, q1_, q2_, q3_; 33 | double roll_, pitch_, yaw_; 34 | 35 | double sampleFreq_; 36 | double twoKp_, twoKi_; 37 | double integralFBx_, integralFBy_, integralFBz_; 38 | 39 | public: 40 | IMU(void): roll_(0.0), pitch_(0.0), yaw_(0.0), ax_(0.0), ay_(0.0), az_(0.0), gx_(0.0), gy_(0.0), gz_(0.0) {} 41 | IMU(double roll, double pitch, double yaw, double ax, double ay, double az, double gx, double gy, double gz): 42 | roll_(roll), pitch_(pitch), yaw_(yaw), ax_(ax), ay_(ay), az_(az), gx_(gx), gy_(gy), gz_(gz) {} 43 | 44 | void init(void); 45 | void printIMUResult(void); 46 | void updateOrientation(void); 47 | 48 | // inline functions 49 | inline double getRoll(void) { return roll_; } 50 | inline double getPitch(void) { return pitch_; } 51 | inline double getYaw(void) { return yaw_; } 52 | inline double getAx(void) { return ax_; } 53 | inline double getAy(void) { return ay_; } 54 | inline double getAz(void) { return az_; } 55 | inline double getGx(void) { return gx_; } 56 | inline double getGy(void) { return gy_; } 57 | inline double getGz(void) { return gz_; } 58 | inline void getRPY(double *roll, double *pitch, double *yaw) { *roll = roll_, *pitch = pitch_, *yaw = yaw_; } 59 | inline void getAcceleration(double *ax, double *ay, double *az) { *ax = ax_, *ay = ay_, *az = az_; } 60 | inline void getAngularVelocities(double *gx, double *gy, double *gz) { *gx = gx_, *gy = gy_, *gz = gz_; } 61 | 62 | inline void setRoll(double roll) { roll_ = roll; } 63 | inline void setPitch(double pitch) { pitch_ = pitch; } 64 | inline void setYaw(double yaw) { yaw_ = yaw; } 65 | inline void setAx(double ax) { ax_ = ax; } 66 | inline void setAy(double ay) { ay_ = ay; } 67 | inline void setAz(double az) { az_ = az; } 68 | inline void setGx(double gx) { gx_ = gx; } 69 | inline void setGy(double gy) { gy_ = gy; } 70 | inline void setGz(double gz) { gz_ = gz; } 71 | inline void setRPY(double roll, double pitch, double yaw) { roll_ = roll, pitch_ = pitch, yaw_ = yaw; } 72 | inline void setAcceleration(double ax, double ay, double az) { ax_ = ax, ay_ = ay, az_ = az; } 73 | inline void setAngularVelocities(double gx, double gy, double gz) { gx_ = gx, gy_ = gy, gz_ = gz; } 74 | inline void setSampleFreq(double sampleFreq) { sampleFreq_ = sampleFreq; } 75 | inline void setAHRSFilterGains(double kp, double ki) { twoKp_ = kp, twoKi_ = ki; } 76 | 77 | inline double invSqrt(double x) { return 1.0 / sqrt(x); } 78 | }; // class IMU 79 | 80 | } // namespace mcl3d 81 | 82 | #endif // __IMU_H__ -------------------------------------------------------------------------------- /include/mcl3d_ros/MCL.h: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * mcl3d_ros: 3D Monte Carlo localization for ROS use 3 | * Copyright (C) 2023 Naoki Akai 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 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an “AS IS” BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * 14 | * See the License for the specific language governing permissions and 15 | * limitations under the License. 16 | * 17 | * @author Naoki Akai 18 | ****************************************************************************/ 19 | 20 | #ifndef __MCL_H__ 21 | #define __MCL_H__ 22 | 23 | #include 24 | #include 25 | #include 26 | #include 27 | #include 28 | #include 29 | #include 30 | #include 31 | #include 32 | #include 33 | #include 34 | 35 | namespace mcl3d { 36 | 37 | class MCL { 38 | private: 39 | DistanceField distMap_; 40 | 41 | int localizationMode_; 42 | // 0: measurement model optimization (similar to ICP scan matching) 43 | // 1: normal particle filter (PF) 44 | // 2: fusion of the optimization- and PF-based localizations (proposed method) 45 | // 3: extended kalman filter (EKF) 46 | 47 | int measurementModelType_; 48 | // 0: use distance (only available for the measurement-model-optimization-based localization) 49 | // 1: normal distribution (only available in particle-filter-based localization) 50 | // 2: likelihood field model 51 | // 3: class conditional measurement model 52 | 53 | int particleNum_, optParticleNum_; 54 | std::vector particles_, optParticles_; 55 | 56 | Pose mclPose_, prevMCLPose_, odomPose_, odomVel_, baseLink2Laser_, optPose_; 57 | double optPoseCovCoef_, gmmPosVar_, gmmAngVar_; 58 | std::vector> approximateHessian_, optPoseCov_; 59 | std::vector> poseCov_, ekfPoseJacob_; 60 | bool odomAvailable_; 61 | 62 | IMU imu_; 63 | bool imuAvailable_; 64 | 65 | std::vector odomNoise_; 66 | bool useOmniModel_; 67 | bool useLinearInterpolation_; 68 | 69 | pcl::PointCloud::Ptr filteredPoints_; 70 | int sensorPointsNum_; 71 | double voxelLeafSize_; 72 | double zHit_, zRand_, zMax_; 73 | double rangeReso_, varHit_, rangeMax_, pRand_, pMax_, unknownLambda_; 74 | double hitNormConst_, ndMax_, lfmMax_, ccmmMax_; 75 | double totalLikelihood_, averageLikelihood_, maxLikelihood_; 76 | int maxLikelihoodParticleIdx_; 77 | 78 | int optMaxIterNum_; 79 | double optMaxError_, convergenceThreshold_; 80 | bool optHasConverged_; 81 | 82 | double randomParticleRate_, resampleThreshold_; 83 | Pose resampleNoise_; 84 | 85 | double effectiveSampleSize_; 86 | 87 | public: 88 | MCL(void); 89 | bool loadDistanceMap(std::string mapYamlFile); 90 | std::vector getMapPoints(void); 91 | void initializeParticles(Pose initialPose, Pose initialNoise); 92 | void initializeOptParticls(int optParticleNum); 93 | void setMeasurementModelParameters(double zHit, double zRand, double zMax, 94 | double varHit, double unknownLambda, double rangeReso, double rangeMax); 95 | bool checkParameters(void); 96 | void printMCLResult(void); 97 | void updatePoses(void); 98 | void calculateLikelihoodsByMeasurementModel(pcl::PointCloud::Ptr sensorPoints); 99 | void optimizeMeasurementModel(pcl::PointCloud::Ptr sensorPoints); 100 | void resampleParticles1(void); 101 | void resampleParticles2(void); 102 | 103 | // public inline functions 104 | // getter 105 | inline Pose getMCLPose(void) { return mclPose_; } 106 | inline Pose getOdomPose(void) { return odomPose_; } 107 | inline Pose getOptPose(void) { return optPose_; } 108 | inline std::vector getParticles(void) { return particles_; } 109 | inline std::vector getOptParticles(void) { return optParticles_; } 110 | inline std::vector> getPoseCovariance(void) { return poseCov_; } 111 | inline std::vector> getOptPoseCovariance(void) { return optPoseCov_; } 112 | inline pcl::PointCloud getAlignedPointsOpt(void) { return *filteredPoints_; } 113 | 114 | // setter 115 | inline void setParticleNum(int particleNum) { particleNum_ = particleNum, particles_.resize(particleNum_); } 116 | inline void setOptParticlsNum(int optParticleNum) { optParticleNum_ = optParticleNum; optParticles_.resize(optParticleNum_); } 117 | inline void setInitialPose(Pose initialPose) { mclPose_ = prevMCLPose_ = initialPose; } 118 | inline void setLocalizationMode(int localizationMode) { localizationMode_ = localizationMode; } 119 | inline void setMeasurementModelType(int measurementModelType) { measurementModelType_ = measurementModelType; } 120 | inline void setBaseLink2Laser(Pose baseLink2Laser) { baseLink2Laser_ = baseLink2Laser; } 121 | inline void setOdomNoise(std::vector odomNoise) { odomNoise_ = odomNoise; } 122 | inline void setUseLinearInterpolation(bool useLinearInterpolation) { useLinearInterpolation_ = useLinearInterpolation; } 123 | inline void setSensorPointsNum(int sensorPointsNum) { sensorPointsNum_ = sensorPointsNum; } 124 | inline void setVoxelLeafSize(double voxelLeafSize) { voxelLeafSize_ = voxelLeafSize; } 125 | inline void setRandomParticleRate(double randomParticleRate) { randomParticleRate_ = randomParticleRate; } 126 | inline void setResampleThreshold(double resampleThreshold) { resampleThreshold_ = resampleThreshold; } 127 | inline void setResampleNoise(Pose resampleNoise) { resampleNoise_ = resampleNoise; } 128 | inline void setOptMaxIterNum(int optMaxIterNum) { optMaxIterNum_ = optMaxIterNum; } 129 | inline void setOptMaxError(double optMaxError) { optMaxError_ = optMaxError; } 130 | inline void setConvergenceThreshold(double convergenceThreshold) { convergenceThreshold_ = convergenceThreshold; } 131 | inline void setIMU(IMU imu) { imu_ = imu, imuAvailable_ = true; } 132 | inline void setOdomPose(Pose odomPose) { odomPose_ = odomPose; } 133 | inline void setOdomVelocities(Pose odomVel) { odomVel_ = odomVel; odomAvailable_ = true; } 134 | inline void setOptPoseCovCoef(double optPoseCovCoef) { optPoseCovCoef_ = optPoseCovCoef; } 135 | inline void setGMMPosVar(double gmmPosVar) { gmmPosVar_ = gmmPosVar; } 136 | inline void setGMMAngVar(double gmmAngVar) { gmmAngVar_ = gmmAngVar; } 137 | 138 | private: 139 | inline double nrand(double n) { 140 | return (n * sqrt(-2.0 * log((double)rand() / RAND_MAX)) * cos(2.0 * M_PI * rand() / RAND_MAX)); 141 | } 142 | 143 | inline double getTime(void) { 144 | struct timeval timeNow{}; 145 | gettimeofday(&timeNow, nullptr); 146 | return (double)((timeNow.tv_sec) + (timeNow.tv_usec / 1000000.0)); 147 | } 148 | 149 | inline double modAngle(double a) { 150 | while (a < -M_PI) 151 | a += 2.0 * M_PI; 152 | while (a > M_PI) 153 | a -= 2.0 * M_PI; 154 | return a; 155 | } 156 | 157 | inline void getCosSin(double roll, double pitch, double yaw, 158 | double *cr, double *sr, double *cp, double *sp, double *cy, double *sy) 159 | { 160 | *cr = cos(roll); 161 | *sr = sin(roll); 162 | *cp = cos(pitch); 163 | *sp = sin(pitch); 164 | *cy = cos(yaw); 165 | *sy = sin(yaw); 166 | } 167 | 168 | inline std::vector getRotMat(double cr, double sr, double cp, double sp, double cy, double sy) { 169 | std::vector rm(9); 170 | rm[0] = cy * cp; 171 | rm[1] = cy * sp * sr - sy * cr; 172 | rm[2] = sy * sr + cy * sp * cr; 173 | rm[3] = sy * cp; 174 | rm[4] = cy * cr + sy * sp * sr; 175 | rm[5] = sy * sp * cr - cy * sr; 176 | rm[6] = -sp; 177 | rm[7] = cp * sr; 178 | rm[8] = cp * cr; 179 | return rm; 180 | } 181 | 182 | inline void transformPoint(double transX, double transY, double transZ, std::vector rotMat, 183 | double x, double y, double z, double *xx, double *yy, double *zz) 184 | { 185 | *xx = x * rotMat[0] + y * rotMat[1] + z * rotMat[2] + transX; 186 | *yy = x * rotMat[3] + y * rotMat[4] + z * rotMat[5] + transY; 187 | *zz = x * rotMat[6] + y * rotMat[7] + z * rotMat[8] + transZ; 188 | } 189 | 190 | std::vector> getInverseMatrix(std::vector> mat); 191 | std::vector> addMatrix(std::vector> m1, std::vector> m2, int row, int col); 192 | std::vector> multiplyMatrix(std::vector> m1, int row1, int col1, std::vector> m2, int row2, int col2); 193 | std::vector> transposeMatrix(std::vector> m, int row, int col); 194 | void printMatrixd(std::vector> m, int row, int col, std::string name); 195 | std::vector> CholeskyDecomposition(std::vector> mat); 196 | double getDeterminant(std::vector> mat, int n); 197 | double getError(float x, float y, float z, float r); 198 | }; // class MCL 199 | 200 | } // namespace mcl3d 201 | 202 | #endif // __MCL_H__ -------------------------------------------------------------------------------- /include/mcl3d_ros/Particle.h: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * mcl3d_ros: 3D Monte Carlo localization for ROS use 3 | * Copyright (C) 2023 Naoki Akai 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 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an “AS IS” BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * 14 | * See the License for the specific language governing permissions and 15 | * limitations under the License. 16 | * 17 | * @author Naoki Akai 18 | ****************************************************************************/ 19 | 20 | #ifndef __PARTICLE_H__ 21 | #define __PARTICLE_H__ 22 | 23 | #include 24 | 25 | namespace mcl3d { 26 | 27 | class Particle { 28 | Pose pose_; 29 | double w_; 30 | 31 | public: 32 | Particle(void): pose_(0.0, 0.0, 0.0, 0.0, 0.0, 0.0), w_(0.0) {} 33 | Particle(double x, double y, double z, double roll, double pitch, double yaw, double w): 34 | pose_(x, y, z, roll, pitch, yaw), w_(w) {} 35 | 36 | inline double getX(void) { return pose_.getX(); } 37 | inline double getY(void) { return pose_.getY(); } 38 | inline double getZ(void) { return pose_.getZ(); } 39 | inline double getRoll(void) { return pose_.getRoll(); } 40 | inline double getPitch(void) { return pose_.getPitch(); } 41 | inline double getYaw(void) { return pose_.getYaw(); } 42 | inline Pose getPose(void) { return pose_; } 43 | inline double getW(void) { return w_; } 44 | inline Particle getParticle(void) { 45 | return Particle(pose_.getX(), pose_.getY(), pose_.getZ(), pose_.getRoll(), pose_.getPitch(), pose_.getYaw(), w_); 46 | } 47 | 48 | inline void setX(double x) { pose_.setX(x); } 49 | inline void setY(double y) { pose_.setY(y); } 50 | inline void setZ(double z) { pose_.setZ(z); } 51 | inline void setRoll(double roll) { pose_.setRoll(roll); } 52 | inline void setPitch(double pitch) { pose_.setX(pitch); } 53 | inline void setYaw(double yaw) { pose_.setYaw(yaw); } 54 | inline void setPose(double x, double y, double z, double roll, double pitch, double yaw) { 55 | pose_.setPose(x, y, z, roll, pitch, yaw); 56 | } 57 | inline void setW(double w) { w_ = w; } 58 | inline void setParticle(double x, double y, double z, double roll, double pitch, double yaw, double w) { 59 | pose_.setPose(x, y, z, roll, pitch, yaw); 60 | w_ = w; 61 | } 62 | }; // class Particle 63 | 64 | } // namespace mcl3d 65 | 66 | #endif // __PARTICLE_H__ -------------------------------------------------------------------------------- /include/mcl3d_ros/Point.h: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * mcl3d_ros: 3D Monte Carlo localization for ROS use 3 | * Copyright (C) 2023 Naoki Akai 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 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an “AS IS” BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * 14 | * See the License for the specific language governing permissions and 15 | * limitations under the License. 16 | * 17 | * @author Naoki Akai 18 | ****************************************************************************/ 19 | 20 | #ifndef __POINT_H__ 21 | #define __POINT_H__ 22 | 23 | namespace mcl3d { 24 | 25 | class Point { 26 | private: 27 | double x_, y_, z_; 28 | 29 | public: 30 | Point(void): x_(0.0), y_(0.0), z_(0.0) {} 31 | Point(double x, double y, double z): x_(x), y_(y), z_(z) {} 32 | 33 | inline double getX(void) { return x_; } 34 | inline double getY(void) { return y_; } 35 | inline double getZ(void) { return z_; } 36 | inline Point getPoint(void) { return Point(x_, y_, z_); } 37 | 38 | inline void setX(double x) { x_ = x; } 39 | inline void setY(double y) { y_ = y; } 40 | inline void setZ(double z) { z_ = z; } 41 | inline void setPoint(double x, double y, double z) { x_ = x, y_ = y, z_ = z; } 42 | }; // class Point 43 | 44 | } // namespace mcl3d 45 | 46 | #endif // __POINT_H__ -------------------------------------------------------------------------------- /include/mcl3d_ros/Pose.h: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * mcl3d_ros: 3D Monte Carlo localization for ROS use 3 | * Copyright (C) 2023 Naoki Akai 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 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an “AS IS” BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * 14 | * See the License for the specific language governing permissions and 15 | * limitations under the License. 16 | * 17 | * @author Naoki Akai 18 | ****************************************************************************/ 19 | 20 | #ifndef __POSE_H__ 21 | #define __POSE_H__ 22 | 23 | namespace mcl3d { 24 | 25 | class Pose { 26 | private: 27 | double x_, y_, z_, roll_, pitch_, yaw_; 28 | 29 | public: 30 | Pose(void): x_(0.0), y_(0.0), z_(0.0), roll_(0.0), pitch_(0.0), yaw_(0.0) {} 31 | Pose(double x, double y, double z, double roll, double pitch, double yaw): 32 | x_(x), y_(y), z_(z), roll_(roll), pitch_(pitch), yaw_(yaw) {} 33 | 34 | inline double getX(void) { return x_; } 35 | inline double getY(void) { return y_; } 36 | inline double getZ(void) { return z_; } 37 | inline double getRoll(void) { return roll_; } 38 | inline double getPitch(void) { return pitch_; } 39 | inline double getYaw(void) { return yaw_; } 40 | inline Pose getPose(void) { return Pose(x_, y_, z_, roll_, pitch_, yaw_); } 41 | 42 | inline void setX(double x) { x_ = x; } 43 | inline void setY(double y) { y_ = y; } 44 | inline void setZ(double z) { z_ = z; } 45 | inline void setRoll(double roll) { roll_ = roll; } 46 | inline void setPitch(double pitch) { pitch_ = pitch; } 47 | inline void setYaw(double yaw) { yaw_ = yaw; } 48 | inline void setPose(double x, double y, double z, double roll, double pitch, double yaw) { 49 | x_ = x, y_ = y, z_ = z, roll_ = roll, pitch_ = pitch, yaw_ = yaw; 50 | } 51 | }; // class Pose 52 | 53 | } // namespace mcl3d 54 | 55 | #endif // __POSE_H__ -------------------------------------------------------------------------------- /launch/mcl.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | [0.0, 0.0, 0.0, 0.0, 0.0, 90.0] 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 | [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | [0.1, 0.1, 0.1, 0.05, 0.05, 0.05] 84 | 85 | 86 | 87 | [0.5, 0.01, 0.01, 0.01, 0.01, 0.01, 88 | 0.01, 0.5, 0.01, 0.01, 0.01, 0.01, 89 | 0.01, 0.01, 0.5, 0.01, 0.01, 0.01, 90 | 0.01, 0.01, 0.01, 0.5, 0.01, 0.01, 91 | 0.01, 0.01, 0.01, 0.01, 0.5, 0.01, 92 | 0.01, 0.01, 0.01, 0.01, 0.01, 0.5] 93 | 94 | 95 | 96 | 97 | 98 | 99 | 100 | 101 | 102 | 103 | [0.1, 0.1, 0.1, 0.05, 0.05, 0.05] 104 | 105 | 106 | 107 | 108 | 109 | 110 | 111 | 112 | 113 | 114 | 115 | 116 | 117 | 118 | 119 | 120 | 121 | 122 | 123 | 124 | 125 | 126 | 127 | 128 | 129 | 130 | 131 | 132 | 133 | 134 | 135 | 136 | 137 | 138 | 139 | 140 | 141 | 142 | 143 | 144 | 145 | 146 | 147 | 148 | 149 | 150 | 151 | 152 | 153 | 154 | 155 | 156 | 157 | 158 | 159 | 160 | 161 | 162 | 163 | 164 | 165 | 166 | 167 | 168 | 169 | 170 | 171 | 172 | 173 | 174 | 175 | 176 | 177 | 178 | 179 | 180 | 181 | 182 | 183 | 184 | 185 | 186 | 187 | 188 | 189 | 190 | 191 | 192 | 193 | 194 | 195 | 196 | 197 | 198 | 199 | 200 | 201 | 202 | 203 | 204 | 205 | 206 | 207 | 208 | 209 | 210 | 211 | 212 | 213 | 214 | 215 | 216 | 217 | -------------------------------------------------------------------------------- /launch/pc_to_df.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | mcl3d_ros 4 | 0.0.0 5 | The mcl3d_ros package 6 | 7 | 8 | 9 | 10 | akai 11 | 12 | 13 | 14 | 15 | 16 | TODO 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 | catkin 52 | geometry_msgs 53 | nav_msgs 54 | pcl_conversions 55 | pcl_ros 56 | roscpp 57 | rospy 58 | sensor_msgs 59 | std_msgs 60 | tf 61 | geometry_msgs 62 | nav_msgs 63 | pcl_conversions 64 | pcl_ros 65 | roscpp 66 | rospy 67 | sensor_msgs 68 | std_msgs 69 | tf 70 | geometry_msgs 71 | nav_msgs 72 | pcl_conversions 73 | pcl_ros 74 | roscpp 75 | rospy 76 | sensor_msgs 77 | std_msgs 78 | tf 79 | 80 | 81 | 82 | 83 | 84 | 85 | 86 | 87 | -------------------------------------------------------------------------------- /rviz/mcl3d_ros.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 | - /TF1/Frames1 10 | - /Sensor points1/Autocompute Value Bounds1 11 | Splitter Ratio: 0.5 12 | Tree Height: 1079 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.5886790156364441 22 | - Class: rviz/Views 23 | Expanded: 24 | - /Current View1 25 | Name: Views 26 | Splitter Ratio: 0.5 27 | - Class: rviz/Time 28 | Experimental: false 29 | Name: Time 30 | SyncMode: 0 31 | SyncSource: Sensor points 32 | Preferences: 33 | PromptSaveOnExit: true 34 | Toolbars: 35 | toolButtonStyle: 2 36 | Visualization Manager: 37 | Class: "" 38 | Displays: 39 | - Alpha: 0.5 40 | Cell Size: 1 41 | Class: rviz/Grid 42 | Color: 160; 160; 164 43 | Enabled: true 44 | Line Style: 45 | Line Width: 0.029999999329447746 46 | Value: Lines 47 | Name: Grid 48 | Normal Cell Count: 0 49 | Offset: 50 | X: 0 51 | Y: 0 52 | Z: 0 53 | Plane: XY 54 | Plane Cell Count: 10 55 | Reference Frame: 56 | Value: true 57 | - Alpha: 1 58 | Class: rviz/Axes 59 | Enabled: false 60 | Length: 1 61 | Name: Axes 62 | Radius: 0.10000000149011612 63 | Reference Frame: 64 | Value: false 65 | - Alpha: 0.20000000298023224 66 | Autocompute Intensity Bounds: true 67 | Autocompute Value Bounds: 68 | Max Value: 10 69 | Min Value: -10 70 | Value: true 71 | Axis: Z 72 | Channel Name: intensity 73 | Class: rviz/PointCloud 74 | Color: 255; 255; 255 75 | Color Transformer: FlatColor 76 | Decay Time: 0 77 | Enabled: true 78 | Invert Rainbow: false 79 | Max Color: 255; 255; 255 80 | Min Color: 0; 0; 0 81 | Name: DF map points 82 | Position Transformer: XYZ 83 | Queue Size: 10 84 | Selectable: true 85 | Size (Pixels): 1 86 | Size (m): 0.009999999776482582 87 | Style: Points 88 | Topic: /df_map_points 89 | Unreliable: false 90 | Use Fixed Frame: true 91 | Use rainbow: true 92 | Value: true 93 | - Class: rviz/TF 94 | Enabled: true 95 | Frame Timeout: 15 96 | Frames: 97 | All Enabled: false 98 | base_link: 99 | Value: false 100 | ground_truth: 101 | Value: false 102 | map: 103 | Value: false 104 | odom: 105 | Value: false 106 | opt_pose: 107 | Value: false 108 | velodyne: 109 | Value: true 110 | Marker Alpha: 1 111 | Marker Scale: 20 112 | Name: TF 113 | Show Arrows: false 114 | Show Axes: true 115 | Show Names: true 116 | Tree: 117 | map: 118 | base_link: 119 | velodyne: 120 | {} 121 | ground_truth: 122 | {} 123 | odom: 124 | {} 125 | opt_pose: 126 | {} 127 | Update Interval: 0 128 | Value: true 129 | - Alpha: 1 130 | Autocompute Intensity Bounds: true 131 | Autocompute Value Bounds: 132 | Max Value: -0.7486982345581055 133 | Min Value: -23.02642059326172 134 | Value: true 135 | Axis: Z 136 | Channel Name: intensity 137 | Class: rviz/PointCloud2 138 | Color: 252; 233; 79 139 | Color Transformer: AxisColor 140 | Decay Time: 0 141 | Enabled: true 142 | Invert Rainbow: false 143 | Max Color: 255; 255; 255 144 | Min Color: 0; 0; 0 145 | Name: Sensor points 146 | Position Transformer: XYZ 147 | Queue Size: 10 148 | Selectable: true 149 | Size (Pixels): 3 150 | Size (m): 0.009999999776482582 151 | Style: Points 152 | Topic: /velodyne_points 153 | Unreliable: false 154 | Use Fixed Frame: true 155 | Use rainbow: true 156 | Value: true 157 | - Alpha: 1 158 | Arrow Length: 2 159 | Axes Length: 1 160 | Axes Radius: 0.009999999776482582 161 | Class: rviz/PoseArray 162 | Color: 255; 25; 0 163 | Enabled: true 164 | Head Length: 1 165 | Head Radius: 1 166 | Name: Particles 167 | Queue Size: 10 168 | Shaft Length: 3 169 | Shaft Radius: 0.5 170 | Shape: Arrow (Flat) 171 | Topic: /particles 172 | Unreliable: false 173 | Value: true 174 | - Alpha: 1 175 | Arrow Length: 2 176 | Axes Length: 0.30000001192092896 177 | Axes Radius: 0.009999999776482582 178 | Class: rviz/PoseArray 179 | Color: 0; 0; 255 180 | Enabled: true 181 | Head Length: 0.07000000029802322 182 | Head Radius: 0.029999999329447746 183 | Name: Optimized particles 184 | Queue Size: 10 185 | Shaft Length: 0.23000000417232513 186 | Shaft Radius: 0.009999999776482582 187 | Shape: Arrow (Flat) 188 | Topic: /optimized_particles 189 | Unreliable: false 190 | Value: true 191 | - Alpha: 1 192 | Axes Length: 1 193 | Axes Radius: 0.10000000149011612 194 | Class: rviz/PoseWithCovariance 195 | Color: 255; 25; 0 196 | Covariance: 197 | Orientation: 198 | Alpha: 0.5 199 | Color: 255; 255; 127 200 | Color Style: Unique 201 | Frame: Local 202 | Offset: 1 203 | Scale: 1 204 | Value: true 205 | Position: 206 | Alpha: 0.30000001192092896 207 | Color: 204; 51; 204 208 | Scale: 1 209 | Value: true 210 | Value: true 211 | Enabled: true 212 | Head Length: 0.30000001192092896 213 | Head Radius: 0.5 214 | Name: MCL pose 215 | Queue Size: 10 216 | Shaft Length: 1 217 | Shaft Radius: 0.10000000149011612 218 | Shape: Arrow 219 | Topic: /mcl_pose 220 | Unreliable: false 221 | Value: true 222 | - Alpha: 1 223 | Axes Length: 1 224 | Axes Radius: 0.10000000149011612 225 | Class: rviz/PoseWithCovariance 226 | Color: 0; 0; 255 227 | Covariance: 228 | Orientation: 229 | Alpha: 0.5 230 | Color: 255; 255; 127 231 | Color Style: Unique 232 | Frame: Local 233 | Offset: 1 234 | Scale: 1 235 | Value: true 236 | Position: 237 | Alpha: 0.30000001192092896 238 | Color: 204; 51; 204 239 | Scale: 1 240 | Value: true 241 | Value: true 242 | Enabled: true 243 | Head Length: 0.30000001192092896 244 | Head Radius: 0.5 245 | Name: Optimized pose 246 | Queue Size: 10 247 | Shaft Length: 1 248 | Shaft Radius: 0.10000000149011612 249 | Shape: Arrow 250 | Topic: /opt_pose 251 | Unreliable: false 252 | Value: true 253 | - Alpha: 1 254 | Autocompute Intensity Bounds: true 255 | Autocompute Value Bounds: 256 | Max Value: 10 257 | Min Value: -10 258 | Value: true 259 | Axis: Z 260 | Channel Name: intensity 261 | Class: rviz/PointCloud 262 | Color: 252; 233; 79 263 | Color Transformer: FlatColor 264 | Decay Time: 0 265 | Enabled: true 266 | Invert Rainbow: false 267 | Max Color: 255; 255; 255 268 | Min Color: 0; 0; 0 269 | Name: Aligned points in optimization 270 | Position Transformer: XYZ 271 | Queue Size: 10 272 | Selectable: true 273 | Size (Pixels): 5 274 | Size (m): 0.009999999776482582 275 | Style: Points 276 | Topic: /aligned_points_opt 277 | Unreliable: false 278 | Use Fixed Frame: true 279 | Use rainbow: true 280 | Value: true 281 | Enabled: true 282 | Global Options: 283 | Background Color: 48; 48; 48 284 | Default Light: true 285 | Fixed Frame: map 286 | Frame Rate: 30 287 | Name: root 288 | Tools: 289 | - Class: rviz/Interact 290 | Hide Inactive Objects: true 291 | - Class: rviz/MoveCamera 292 | - Class: rviz/Select 293 | - Class: rviz/FocusCamera 294 | - Class: rviz/Measure 295 | - Class: rviz/SetInitialPose 296 | Theta std deviation: 0.2617993950843811 297 | Topic: /initialpose 298 | X std deviation: 0.5 299 | Y std deviation: 0.5 300 | - Class: rviz/SetGoal 301 | Topic: /move_base_simple/goal 302 | - Class: rviz/PublishPoint 303 | Single click: true 304 | Topic: /clicked_point 305 | Value: true 306 | Views: 307 | Current: 308 | Class: rviz/ThirdPersonFollower 309 | Distance: 69.08515167236328 310 | Enable Stereo Rendering: 311 | Stereo Eye Separation: 0.05999999865889549 312 | Stereo Focal Distance: 1 313 | Swap Stereo Eyes: false 314 | Value: false 315 | Field of View: 0.7853981852531433 316 | Focal Point: 317 | X: 0 318 | Y: 0 319 | Z: 0 320 | Focal Shape Fixed Size: true 321 | Focal Shape Size: 0.05000000074505806 322 | Invert Z Axis: false 323 | Name: Current View 324 | Near Clip Distance: 0.009999999776482582 325 | Pitch: 0.5853983759880066 326 | Target Frame: base_link 327 | Yaw: 2.5703914165496826 328 | Saved: ~ 329 | Window Geometry: 330 | Displays: 331 | collapsed: false 332 | Height: 1376 333 | Hide Left Dock: false 334 | Hide Right Dock: false 335 | QMainWindow State: 000000ff00000000fd000000040000000000000156000004c2fc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000004c2000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d0061006700650000000364000000d50000000000000000000000010000010f000004c2fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d000004c2000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000009b80000003efc0100000002fb0000000800540069006d00650100000000000009b8000002eb00fffffffb0000000800540069006d0065010000000000000450000000000000000000000747000004c200000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 336 | Selection: 337 | collapsed: false 338 | Time: 339 | collapsed: false 340 | Tool Properties: 341 | collapsed: false 342 | Views: 343 | collapsed: false 344 | Width: 2488 345 | X: 2632 346 | Y: 27 347 | -------------------------------------------------------------------------------- /src/distance_field.cpp: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * mcl3d_ros: 3D Monte Carlo localization for ROS use 3 | * Copyright (C) 2023 Naoki Akai 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 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an “AS IS” BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * 14 | * See the License for the specific language governing permissions and 15 | * limitations under the License. 16 | * 17 | * @author Naoki Akai 18 | ****************************************************************************/ 19 | 20 | #include 21 | 22 | namespace mcl3d { 23 | 24 | DistanceField::DistanceField(std::string yamlFilePath): 25 | isAvailable_(false), 26 | debugFlag_(false), 27 | dfComputationThreadsNum_(4) 28 | { 29 | // get root directory name from the given yaml file path path 30 | rootDirName_ = yamlFilePath.substr(0, yamlFilePath.find_last_of("/") + 1); 31 | FILE *fp = fopen(yamlFilePath.c_str(), "r"); 32 | if (fp == NULL) { 33 | fprintf(stderr, "The yaml file does not exist -> %s\n", yamlFilePath.c_str()); 34 | return; 35 | } 36 | fclose(fp); 37 | 38 | // load main map parameters 39 | YAML::Node lconf = YAML::LoadFile(yamlFilePath); 40 | mapFileName_ = lconf["map_file_name"].as(); 41 | resolution_ = lconf["resolution"].as(); 42 | origin_ = lconf["origin"].as >(); 43 | width_ = lconf["width"].as(); 44 | depth_ = lconf["depth"].as(); 45 | height_ = lconf["height"].as(); 46 | subMapResolution_ = lconf["sub_map_resolution"].as(); 47 | subMapWidth_ = lconf["sub_map_width"].as(); 48 | subMapDepth_ = lconf["sub_map_depth"].as(); 49 | subMapHeight_ = lconf["sub_map_height"].as(); 50 | 51 | // initialization 52 | initializeSubMaps(); 53 | isAvailable_ = true; 54 | } 55 | 56 | DistanceField::DistanceField(std::string mapFileName, float resolution, float subMapResolution, float mapMargin, 57 | Point minPoint, Point maxPoint, std::string yamlFilePath): 58 | mapFileName_(mapFileName), 59 | resolution_(resolution), 60 | subMapResolution_(subMapResolution), 61 | isAvailable_(false), 62 | debugFlag_(false), 63 | dfComputationThreadsNum_(4) 64 | { 65 | // get root directory name from the given yaml file path path 66 | rootDirName_ = yamlFilePath.substr(0, yamlFilePath.find_last_of("/") + 1); 67 | 68 | // calculate map parameters 69 | float minX = minPoint.getX() - mapMargin; 70 | float minY = minPoint.getY() - mapMargin; 71 | float minZ = minPoint.getZ() - mapMargin; 72 | float maxX = maxPoint.getX() + mapMargin; 73 | float maxY = maxPoint.getY() + mapMargin; 74 | float maxZ = maxPoint.getZ() + mapMargin; 75 | float dx = maxX - minX; 76 | float dy = maxY - minY; 77 | float dz = maxZ - minZ; 78 | float mapRangeX = 0.0f, mapRangeY = 0.0f, mapRangeZ = 0.0; 79 | width_ = depth_ = height_ = 0; 80 | while (mapRangeX < dx) { 81 | mapRangeX += resolution_; 82 | width_++; 83 | } 84 | while (mapRangeY < dy) { 85 | mapRangeY += resolution_; 86 | depth_++; 87 | } 88 | while (mapRangeZ < dz) { 89 | mapRangeZ += resolution_; 90 | height_++; 91 | } 92 | origin_.resize(3); 93 | origin_[0] = minX; 94 | origin_[1] = minY; 95 | origin_[2] = minZ; 96 | subMapWidth_ = (int)(resolution_ / subMapResolution_); 97 | subMapDepth_ = (int)(resolution_ / subMapResolution_); 98 | subMapHeight_ = (int)(resolution_ / subMapResolution_); 99 | 100 | // make an yaml file 101 | FILE *fp = fopen(yamlFilePath.c_str(), "w"); 102 | fprintf(fp, "map_file_name: %s\n", mapFileName_.c_str()); 103 | fprintf(fp, "resolution: %f\n", resolution_); 104 | fprintf(fp, "origin: [%f, %f, %f]\n", origin_[0], origin_[1], origin_[2]); 105 | fprintf(fp, "width: %d\n", width_); 106 | fprintf(fp, "depth: %d\n", depth_); 107 | fprintf(fp, "height: %d\n", height_); 108 | fprintf(fp, "sub_map_resolution: %f\n", subMapResolution_); 109 | fprintf(fp, "sub_map_width: %d\n", subMapWidth_); 110 | fprintf(fp, "sub_map_depth: %d\n", subMapDepth_); 111 | fprintf(fp, "sub_map_height: %d\n", subMapHeight_); 112 | fclose(fp); 113 | 114 | // initialization 115 | initializeSubMaps(); 116 | isAvailable_ = true; 117 | } 118 | 119 | void DistanceField::initializeSubMaps(void) { 120 | totalMapPointsNum_ = 0; 121 | subMapPointsNums_.resize(width_); 122 | subMapOrigins_.resize(width_); 123 | subMaps_.resize(width_); 124 | distanceLists_.resize(width_); 125 | for (int i = 0; i < width_; i++) { 126 | subMapPointsNums_[i].resize(depth_); 127 | subMapOrigins_[i].resize(depth_); 128 | subMaps_[i].resize(depth_); 129 | distanceLists_[i].resize(depth_); 130 | for (int j = 0; j < depth_; j++) { 131 | subMapPointsNums_[i][j].resize(height_, 0); 132 | subMapOrigins_[i][j].resize(height_); 133 | subMaps_[i][j].resize(height_); 134 | distanceLists_[i][j].resize(height_); 135 | } 136 | } 137 | } 138 | 139 | void DistanceField::addPoint(float x, float y, float z) { 140 | int u, v, w; 141 | if (!xyz2uvw(origin_, resolution_, width_, depth_, height_, x, y, z, &u, &v, &w)) 142 | return; 143 | 144 | if (subMapPointsNums_[u][v][w] == 0) { 145 | distanceLists_[u][v][w].resize(256); 146 | subMapOrigins_[u][v][w].resize(3); 147 | subMapOrigins_[u][v][w][0] = origin_[0] + (float)u * resolution_; 148 | subMapOrigins_[u][v][w][1] = origin_[1] + (float)v * resolution_; 149 | subMapOrigins_[u][v][w][2] = origin_[2] + (float)w * resolution_; 150 | int subMapSize = (int)(resolution_ / subMapResolution_); 151 | subMaps_[u][v][w].resize(subMapSize); 152 | for (int i = 0; i < subMapSize; i++) { 153 | subMaps_[u][v][w][i].resize(subMapSize); 154 | for (int j = 0; j < subMapSize; j++) 155 | subMaps_[u][v][w][i][j].resize(subMapSize, MAP_VAL_INVALID); 156 | } 157 | } 158 | 159 | int uu, vv, ww; 160 | if (xyz2uvw(subMapOrigins_[u][v][w], subMapResolution_, subMapWidth_, subMapDepth_, subMapHeight_, x, y, z, &uu, &vv, &ww)) { 161 | subMapPointsNums_[u][v][w]++; 162 | subMaps_[u][v][w][uu][vv][ww] = MAP_VAL_EXIST_POINT; 163 | totalMapPointsNum_++; 164 | } 165 | } 166 | 167 | void DistanceField::saveSubDistanceMap(int uo, int vo, int wo, FILE *fp) { 168 | // initialize distance map 169 | dope::Index3 mapSize({(long unsigned int)subMapWidth_, (long unsigned int)subMapDepth_, (long unsigned int)subMapHeight_}); 170 | dope::Grid distMap(mapSize); 171 | float maxValue = std::numeric_limits::max(); 172 | for (int u = 0; u < subMapWidth_; u++) { 173 | for (int v = 0; v < subMapDepth_; v++) { 174 | for (int w = 0; w < subMapHeight_; w++) { 175 | if (subMaps_[uo][vo][wo][u][v][w] == MAP_VAL_EXIST_POINT) 176 | distMap[u][v][w] = 0.0f; 177 | else 178 | distMap[u][v][w] = maxValue; 179 | } 180 | } 181 | } 182 | 183 | // perform distance transformation 184 | bool returnSquaredNorm = false; 185 | dt::DistanceTransform::distanceTransformL2(distMap, distMap, returnSquaredNorm, dfComputationThreadsNum_); 186 | 187 | // create distance list in centimeter 188 | std::vector distanceListCM(256); 189 | for (int i = 0; i < mapSize[0]; i++) { 190 | for (int j = 0; j < mapSize[1]; j++) { 191 | for (int k = 0; k < mapSize[2]; k++) { 192 | int dist = (int)(distMap[i][j][k] * subMapResolution_ * 100.0); // cm 193 | // printf("%d %d %d %d\n", i, j, k, dist); 194 | if (dist <= 0) { 195 | continue; 196 | } else { 197 | for (int idx = 1; idx < 256; idx++) { 198 | if (dist == distanceListCM[idx]) { 199 | break; 200 | } else if (dist < distanceListCM[idx] || distanceListCM[idx] == 0) { 201 | distanceListCM.insert(distanceListCM.begin() + idx, dist); 202 | distanceListCM.resize(256); 203 | break; 204 | } 205 | } 206 | } 207 | } 208 | } 209 | } 210 | 211 | // create distance list in meter and its inverse list 212 | std::vector distanceList(256); 213 | for (int i = 0; i < (int)distanceListCM.size(); i++) { 214 | distanceList[i] = (float)distanceListCM[i] / 100.0f; // cm -> m 215 | // printf("%d, %f [m]\n", i, distanceList[i]); 216 | } 217 | distanceList[255] = -1.0; 218 | std::map inverseDistanceList; 219 | for (int i = 0; i < 256; i++) 220 | inverseDistanceList.insert(std::make_pair(distanceListCM[i], i)); 221 | 222 | // save the results 223 | int maxDist = distanceListCM[254]; 224 | for (int i = 0; i < 256; i++) 225 | fwrite(&distanceList[i], sizeof(float), 1, fp); 226 | for (int i = 0; i < mapSize[0]; i++) { 227 | for (int j = 0; j < mapSize[1]; j++) { 228 | for (int k = 0; k < mapSize[2]; k++) { 229 | int dist = (int)(distMap[i][j][k] * subMapResolution_ * 100.0); 230 | if (dist <= maxDist) { 231 | int datai[4] = {i, j, k, inverseDistanceList.at(dist)}; 232 | // printf("%d %d %d %d\n", i, j, k, dist); 233 | fwrite(datai, sizeof(int), 4, fp); 234 | } 235 | } 236 | } 237 | } 238 | } 239 | 240 | bool DistanceField::saveDistanceMap(void) { 241 | std::string mapFilePath = rootDirName_ + mapFileName_; 242 | FILE *fp = fopen(mapFilePath.c_str(), "w"); 243 | if (fp == NULL) { 244 | fprintf(stderr, "a map file could not be open -> %s\n", mapFilePath.c_str()); 245 | return false; 246 | } 247 | 248 | if (debugFlag_) 249 | printf("save the 3D distance map.\n"); 250 | for (int u = 0; u < width_; u++) { 251 | printf("%.2f [%%] process done\n", (float)(u + 1) / (float)width_ * 100.0f); 252 | for (int v = 0; v < depth_; v++) { 253 | for (int w = 0; w < height_; w++) { 254 | if (subMapPointsNums_[u][v][w] == 0) 255 | continue; 256 | int datai[4] = {u, v, w, (int)subMapPointsNums_[u][v][w]}; 257 | fwrite(datai, sizeof(int), 4, fp); 258 | float dataf[3] = {subMapOrigins_[u][v][w][0], subMapOrigins_[u][v][w][1], subMapOrigins_[u][v][w][2]}; 259 | fwrite(dataf, sizeof(float), 3, fp); 260 | saveSubDistanceMap(u, v, w, fp); 261 | datai[0] = datai[1] = datai[2] = -1, datai[3] = 0; // this means end of the submap 262 | fwrite(datai, sizeof(int), 4, fp); 263 | } 264 | } 265 | } 266 | 267 | fclose(fp); 268 | return true; 269 | } 270 | 271 | bool DistanceField::loadDistanceMap(void) { 272 | std::string mapFilePath = rootDirName_ + mapFileName_; 273 | FILE *fp = fopen(mapFilePath.c_str(), "r"); 274 | if (fp == NULL) { 275 | fprintf(stderr, "a map file could not be open -> %s\n", mapFilePath.c_str()); 276 | return false; 277 | } 278 | 279 | if (debugFlag_) 280 | printf("start loading the distance map -> %s\n", mapFilePath.c_str()); 281 | 282 | int datai[4]; 283 | float dataf[3]; 284 | size_t size; 285 | for (;;) { 286 | size = fread(datai, sizeof(int), 4, fp); 287 | if (size <= 0) 288 | break; 289 | int uo = datai[0]; 290 | int vo = datai[1]; 291 | int wo = datai[2]; 292 | int pointsNum = datai[3]; 293 | subMapPointsNums_[uo][vo][wo] = pointsNum; 294 | totalMapPointsNum_ += pointsNum; 295 | 296 | size = fread(dataf, sizeof(float), 3, fp); 297 | subMapOrigins_[uo][vo][wo].resize(3); 298 | subMapOrigins_[uo][vo][wo][0] = dataf[0]; 299 | subMapOrigins_[uo][vo][wo][1] = dataf[1]; 300 | subMapOrigins_[uo][vo][wo][2] = dataf[2]; 301 | 302 | subMaps_[uo][vo][wo].resize(subMapWidth_); 303 | for (int i = 0; i < subMapWidth_; i++) { 304 | subMaps_[uo][vo][wo][i].resize(subMapDepth_); 305 | for (int j = 0; j < subMapDepth_; j++) 306 | subMaps_[uo][vo][wo][i][j].resize(subMapHeight_, MAP_VAL_INVALID); 307 | } 308 | 309 | float dist; 310 | distanceLists_[uo][vo][wo].resize(256); 311 | for (int i = 0; i < 256; i++) { 312 | size = fread(&dist, sizeof(float), 1, fp); 313 | distanceLists_[uo][vo][wo][i] = dist; 314 | } 315 | int num = 0; 316 | for (;;) { 317 | size = fread(datai, sizeof(int), 4, fp); 318 | int u = datai[0]; 319 | int v = datai[1]; 320 | int w = datai[2]; 321 | unsigned int val = (unsigned int)datai[3]; 322 | if (u < 0 || v < 0 || w < 0 || size != 4) 323 | break; 324 | subMaps_[uo][vo][wo][u][v][w] = val; 325 | } 326 | } 327 | fclose(fp); 328 | 329 | if (debugFlag_) 330 | printf("finish loading the distance map -> %s\n", mapFilePath.c_str()); 331 | return true; 332 | } 333 | 334 | void DistanceField::writeMapPoints(std::string fname, 335 | float minX, float maxX, float minY, float maxY, float minZ, float maxZ) 336 | { 337 | FILE *fp = fopen(fname.c_str(), "w"); 338 | if (fp == NULL) { 339 | fprintf(stderr, "Could not open file in writeMapPoints() -> %s\n", fname.c_str()); 340 | return; 341 | } 342 | 343 | for (int u = 0; u < width_; u++) { 344 | if (debugFlag_) 345 | printf("%.3f [%%] process done\n", (float)(u + 1) / (float)width_ * 100.0f); 346 | for (int v = 0; v < depth_; v++) { 347 | for (int w = 0; w < height_; w++) { 348 | if (subMapPointsNums_[u][v][w] == 0) 349 | continue; 350 | for (int uu = 0; uu < subMapWidth_; uu++) { 351 | for (int vv = 0; vv < subMapDepth_; vv++) { 352 | for (int ww = 0; ww < subMapHeight_; ww++) { 353 | if (subMaps_[u][v][w][uu][vv][ww] == MAP_VAL_EXIST_POINT) { 354 | float x, y, z; 355 | uvw2xyz(subMapOrigins_[u][v][w], subMapResolution_, uu, vv, ww, &x, &y, &z); 356 | if (minX <= x && x <= maxX && minY <= y && y <= maxY && minZ <= z && z <= maxZ) 357 | fprintf(fp, "%f %f %f\n", x, y, z); 358 | } 359 | } 360 | } 361 | } 362 | } 363 | } 364 | } 365 | fclose(fp); 366 | } 367 | 368 | std::vector DistanceField::getMapPoints(float minX, float maxX, float minY, float maxY, float minZ, float maxZ) { 369 | std::vector points; 370 | for (int u = 0; u < width_; u++) { 371 | for (int v = 0; v < depth_; v++) { 372 | for (int w = 0; w < height_; w++) { 373 | if (subMapPointsNums_[u][v][w] == 0) 374 | continue; 375 | for (int uu = 0; uu < subMapWidth_; uu++) { 376 | for (int vv = 0; vv < subMapDepth_; vv++) { 377 | for (int ww = 0; ww < subMapHeight_; ww++) { 378 | if (subMaps_[u][v][w][uu][vv][ww] == MAP_VAL_EXIST_POINT) { 379 | float x, y, z; 380 | uvw2xyz(subMapOrigins_[u][v][w], subMapResolution_, uu, vv, ww, &x, &y, &z); 381 | if (minX <= x && x <= maxX && minY <= y && y <= maxY && minZ <= z && z <= maxZ) { 382 | Point p(x, y, z); 383 | points.push_back(p); 384 | } 385 | } 386 | } 387 | } 388 | } 389 | } 390 | } 391 | } 392 | return points; 393 | } 394 | 395 | } // namespace mcl3d -------------------------------------------------------------------------------- /src/imu.cpp: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * mcl3d_ros: 3D Monte Carlo localization for ROS use 3 | * Copyright (C) 2023 Naoki Akai 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 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an “AS IS” BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * 14 | * See the License for the specific language governing permissions and 15 | * limitations under the License. 16 | * 17 | * @author Naoki Akai 18 | ****************************************************************************/ 19 | 20 | #include 21 | 22 | namespace mcl3d { 23 | 24 | void IMU::init(void) { 25 | q0_ = 1.0; 26 | q1_ = 0.0; 27 | q2_ = 0.0; 28 | q3_ = 0.0; 29 | 30 | roll_ = 0.0; 31 | pitch_ = 0.0; 32 | yaw_ = 0.0; 33 | 34 | twoKp_ = 0.0; 35 | twoKi_ = 0.0; 36 | 37 | integralFBx_ = 0.0; 38 | integralFBy_ = 0.0; 39 | integralFBz_ = 0.0; 40 | 41 | sampleFreq_ = 100.0; 42 | } 43 | 44 | void IMU::printIMUResult(void) { 45 | printf("sampleFreq_ = %lf\n", sampleFreq_); 46 | printf("ax = %lf, ay = %lf, az = %lf\n", ax_, ay_, az_); 47 | printf("gx = %lf, gy = %lf, gz = %lf\n", gx_, gy_, gz_); 48 | printf("roll = %lf [deg], pitch = %lf [deg], yaw = %lf [deg]\n", 49 | roll_ * 180.0 / M_PI, pitch_ * 180.0 / M_PI, yaw_ * 180.0 / M_PI); 50 | printf("q0 = %lf, q1 = %lf, q2 = %lf, q3 = %lf\n", q0_, q1_, q2_, q3_); 51 | printf("\n"); 52 | } 53 | 54 | void IMU::updateOrientation(void) { 55 | if (ax_ != 0.0 || ay_ != 0.0 || az_ != 0.0) { 56 | double recipNorm = invSqrt(ax_ * ax_ + ay_ * ay_ + az_ * az_); 57 | ax_ *= recipNorm; 58 | ay_ *= recipNorm; 59 | az_ *= recipNorm; 60 | 61 | double halfvx = q1_ * q3_ - q0_ * q2_; 62 | double halfvy = q0_ * q1_ + q2_ * q3_; 63 | double halfvz = q0_ * q0_ - 0.5 + q3_ * q3_; 64 | 65 | double halfex = (ay_ * halfvz - az_ * halfvy); 66 | double halfey = (az_ * halfvx - ax_ * halfvz); 67 | double halfez = (ax_ * halfvy - ay_ * halfvx); 68 | 69 | if (twoKi_ > 0.0) { 70 | integralFBx_ += twoKi_ * halfex * (1.0 / sampleFreq_); 71 | integralFBy_ += twoKi_ * halfey * (1.0 / sampleFreq_); 72 | integralFBz_ += twoKi_ * halfez * (1.0 / sampleFreq_); 73 | gx_ += integralFBx_; 74 | gy_ += integralFBy_; 75 | gz_ += integralFBz_; 76 | } else { 77 | integralFBx_ = 0.0; 78 | integralFBy_ = 0.0; 79 | integralFBz_ = 0.0; 80 | } 81 | 82 | gx_ += twoKp_ * halfex; 83 | gy_ += twoKp_ * halfey; 84 | gz_ += twoKp_ * halfez; 85 | } 86 | 87 | gx_ *= (0.5 * (1.0 / sampleFreq_)); 88 | gy_ *= (0.5 * (1.0 / sampleFreq_)); 89 | gz_ *= (0.5 * (1.0 / sampleFreq_)); 90 | double qa = q0_; 91 | double qb = q1_; 92 | double qc = q2_; 93 | q0_ += (-qb * gx_ - qc * gy_ - q3_ * gz_); 94 | q1_ += (qa * gx_ + qc * gz_ - q3_ * gy_); 95 | q2_ += (qa * gy_ - qb * gz_ + q3_ * gx_); 96 | q3_ += (qa * gz_ + qb * gy_ - qc * gx_); 97 | 98 | double recipNorm = invSqrt(q0_ * q0_ + q1_ * q1_ + q2_ * q2_ + q3_ * q3_); 99 | q0_ *= recipNorm; 100 | q1_ *= recipNorm; 101 | q2_ *= recipNorm; 102 | q3_ *= recipNorm; 103 | 104 | double q0q0 = q0_ * q0_; 105 | double q1q1 = q1_ * q1_; 106 | double q2q2 = q2_ * q2_; 107 | double q3q3 = q3_ * q3_; 108 | double q0q1 = q0_ * q1_; 109 | double q0q2 = q0_ * q2_; 110 | double q0q3 = q0_ * q3_; 111 | double q1q2 = q1_ * q2_; 112 | double q1q3 = q1_ * q3_; 113 | double q2q3 = q2_ * q3_; 114 | roll_ = atan2((2.0 * (q2q3 + q0q1)), (q0q0 - q1q1 - q2q2 + q3q3)); 115 | pitch_ = -asin((2.0 * (q1q3 - q0q2))); 116 | yaw_ = atan2((2.0 * (q1q2 + q0q3)), (q0q0 + q1q1 - q2q2 - q3q3)); 117 | } 118 | 119 | } // namespace mcl3d -------------------------------------------------------------------------------- /src/pc_to_df.cpp: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * mcl3d_ros: 3D Monte Carlo localization for ROS use 3 | * Copyright (C) 2023 Naoki Akai 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 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an “AS IS” BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * 14 | * See the License for the specific language governing permissions and 15 | * limitations under the License. 16 | * 17 | * @author Naoki Akai 18 | ****************************************************************************/ 19 | 20 | #include 21 | #include 22 | #include 23 | #include 24 | #include 25 | #include 26 | #include 27 | #include 28 | #include 29 | 30 | mcl3d::DistanceField distMap; 31 | bool doneMapBuild = false; 32 | 33 | std::string mapFileName, yamlFilePath; 34 | float resolution, subMapResolution, mapMargin; 35 | 36 | bool buildDistanceField(pcl::PointCloud mapPoints, std::string mapFileName, 37 | float resolution, float subMapResolution, float mapMargin, std::string yamlFilePath) 38 | { 39 | mcl3d::Point minPoint(mapPoints.points[0].x, mapPoints.points[0].y, mapPoints.points[0].z); 40 | mcl3d::Point maxPoint(mapPoints.points[0].x, mapPoints.points[0].y, mapPoints.points[0].z); 41 | for (int i = 1; i < (int)mapPoints.size(); ++i) { 42 | float x = mapPoints[i].x; 43 | float y = mapPoints[i].y; 44 | float z = mapPoints[i].z; 45 | if (minPoint.getX() > x) 46 | minPoint.setX(x); 47 | if (minPoint.getY() > y) 48 | minPoint.setY(y); 49 | if (minPoint.getZ() > z) 50 | minPoint.setZ(z); 51 | if (maxPoint.getX() < x) 52 | maxPoint.setX(x); 53 | if (maxPoint.getY() < y) 54 | maxPoint.setY(y); 55 | if (maxPoint.getZ() < z) 56 | maxPoint.setZ(z); 57 | } 58 | printf("Min map point: %f %f %f\n", minPoint.getX(), minPoint.getY(), minPoint.getZ()); 59 | printf("Max map point: %f %f %f\n", maxPoint.getX(), maxPoint.getY(), maxPoint.getZ()); 60 | 61 | distMap = mcl3d::DistanceField(mapFileName, resolution, subMapResolution, mapMargin, minPoint, maxPoint, yamlFilePath); 62 | // FILE *fp = fopen("/tmp/map_points_by_pc_to_df.txt", "w"); 63 | for (int i = 0; i < (int)mapPoints.size(); ++i) { 64 | float x = mapPoints.points[i].x; 65 | float y = mapPoints.points[i].y; 66 | float z = mapPoints.points[i].z; 67 | // fprintf(fp, "%f %f %f\n", x, y, z); 68 | distMap.addPoint(x, y, z); 69 | } 70 | // fclose(fp); 71 | 72 | // distMap.writeMapPoints("/tmp/df_map_points.txt", minPoint.getX(), maxPoint.getX(), 73 | // minPoint.getY(), maxPoint.getY(), minPoint.getZ(), maxPoint.getZ()); 74 | // exit(0); 75 | 76 | if (!distMap.saveDistanceMap()) { 77 | fprintf(stderr, "Error occurred during the distance field building.\n"); 78 | exit(1); 79 | } 80 | 81 | /* 82 | distMap.loadDistanceMap(); 83 | FILE *fp = fopen("/tmp/dist_map_2d.txt", "w"); 84 | for (float x = minPoint.getX(); x < maxPoint.getX(); x += distMap.getSubMapResolution()) { 85 | for (float y = minPoint.getY(); y < maxPoint.getY(); y += distMap.getSubMapResolution()) { 86 | float d = distMap.getDistance(x, y, 0.0f); 87 | if (d >= 0.0f) 88 | fprintf(fp, "%f %f %f\n", x, y, d); 89 | } 90 | } 91 | fclose(fp); 92 | */ 93 | 94 | return true; 95 | } 96 | 97 | void mapPointsCB(const sensor_msgs::PointCloud2::ConstPtr &msg) { 98 | pcl::PointCloud mapPoints; 99 | pcl::fromROSMsg(*msg, mapPoints); 100 | doneMapBuild = buildDistanceField(mapPoints, mapFileName, resolution, subMapResolution, mapMargin, yamlFilePath); 101 | } 102 | 103 | int main(int argc, char **argv) { 104 | if (argv[4] != NULL && argv[6] != NULL) { 105 | // build a distance field map from a PCD file 106 | // do not use any ROS functions 107 | std::string pcdFile = argv[1]; 108 | mapFileName = argv[2]; 109 | resolution = std::atof(argv[3]); 110 | subMapResolution = std::stof(argv[4]); 111 | mapMargin = std::atof(argv[5]); 112 | yamlFilePath = argv[6]; 113 | 114 | printf("pcdFile: %s\n", pcdFile.c_str()); 115 | printf("mapFileName: %s\n", mapFileName.c_str()); 116 | printf("resolution: %f [m]\n", resolution); 117 | printf("subMapResolution: %f [m]\n", subMapResolution); 118 | printf("mapMargin: %f [m]\n", mapMargin); 119 | printf("yamlFilePath: %s\n", yamlFilePath.c_str()); 120 | 121 | pcl::PointCloud mapPoints; 122 | pcl::io::loadPCDFile(pcdFile, mapPoints); 123 | doneMapBuild = buildDistanceField(mapPoints, mapFileName, resolution, subMapResolution, mapMargin, yamlFilePath); 124 | } else { 125 | // get point cloud from a ROS message and build a distance field map 126 | ros::init(argc, argv, "pc_to_df"); 127 | ros::NodeHandle nh("~"); 128 | std::string mapPointsName = "/map_points"; 129 | 130 | mapFileName = "dist_map.bin"; 131 | resolution = 5.0f; 132 | subMapResolution = 0.1f; 133 | mapMargin = 1.0f; 134 | yamlFilePath = "/tmp/dist_map.yaml"; 135 | 136 | nh.param("map_points_name", mapPointsName, mapPointsName); 137 | nh.param("map_file_name", mapFileName, mapFileName); 138 | nh.param("resolution", resolution, resolution); 139 | nh.param("sub_map_resolution", subMapResolution, subMapResolution); 140 | nh.param("map_margin", mapMargin, mapMargin); 141 | nh.param("yaml_file_path", yamlFilePath, yamlFilePath); 142 | 143 | printf("mapPointsName: %s\n", mapPointsName.c_str()); 144 | printf("mapFileName: %s\n", mapFileName.c_str()); 145 | printf("resolution: %f [m]\n", resolution); 146 | printf("subMapResolution: %f [m]\n", subMapResolution); 147 | printf("mapMargin: %f [m]\n", mapMargin); 148 | printf("yamlFilePath: %s\n", yamlFilePath.c_str()); 149 | 150 | ros::Subscriber mapPointsSub = nh.subscribe(mapPointsName, 1, mapPointsCB); 151 | 152 | ros::Rate loopRate(1.0); 153 | while (ros::ok()) { 154 | ros::spinOnce(); 155 | if (doneMapBuild) 156 | break; 157 | loopRate.sleep(); 158 | } 159 | } 160 | 161 | return 0; 162 | } -------------------------------------------------------------------------------- /src/sensor_points_merger.cpp: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * mcl3d_ros: 3D Monte Carlo localization for ROS use 3 | * Copyright (C) 2023 Naoki Akai 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 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an “AS IS” BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * 14 | * See the License for the specific language governing permissions and 15 | * limitations under the License. 16 | * 17 | * @author Naoki Akai 18 | ****************************************************************************/ 19 | 20 | #include 21 | #include 22 | #include 23 | #include 24 | #include 25 | #include 26 | #include 27 | #include 28 | #include 29 | #include 30 | #include 31 | #include 32 | 33 | class SensorPointsMerger { 34 | private: 35 | ros::NodeHandle nh_; 36 | std::string baseFrame_; 37 | std::vector sensorFrames_, sensorTopicNames_; 38 | std::vector pointsSubs_; 39 | std::vector displacements_; 40 | std::vector gotPoints_; 41 | 42 | std::string mergedPointsName_, mergedPointsFrame_; 43 | ros::Publisher pointsPub_; 44 | sensor_msgs::PointCloud mergedPoints_; 45 | 46 | tf::TransformListener tfListener_; 47 | 48 | public: 49 | SensorPointsMerger(void): 50 | nh_("~"), 51 | baseFrame_("base_link"), 52 | sensorTopicNames_({"/cloud", "/cloud"}), 53 | sensorFrames_({"hokuyo3d_front", "hokuyo3d_rear"}), 54 | gotPoints_({false, false}), 55 | mergedPointsName_("/velodyne_points"), 56 | mergedPointsFrame_("velodyne") 57 | { 58 | nh_.param("base_frame", baseFrame_, baseFrame_); 59 | nh_.param("sensor_frames", sensorFrames_, sensorFrames_); 60 | nh_.param("sensor_topic_names", sensorTopicNames_, sensorTopicNames_); 61 | 62 | nh_.param("merged_points_name", mergedPointsName_, mergedPointsName_); 63 | nh_.param("merged_points_frame", mergedPointsFrame_, mergedPointsFrame_); 64 | 65 | // read transformations from TF tree 66 | // printf("read transformations from TF tree\n"); 67 | for (int i = 0; i < (int)sensorFrames_.size(); ++i) { 68 | tf::StampedTransform trans; 69 | int tfFailedCnt = 0; 70 | ros::Rate loopRate(10.0); 71 | while (ros::ok()) { 72 | ros::spinOnce(); 73 | try { 74 | ros::Time now = ros::Time::now(); 75 | tfListener_.waitForTransform(baseFrame_, sensorFrames_[i], now, ros::Duration(1.0)); 76 | tfListener_.lookupTransform(baseFrame_, sensorFrames_[i], now, trans); 77 | break; 78 | } catch (tf::TransformException ex) { 79 | tfFailedCnt++; 80 | if (tfFailedCnt >= 100) { 81 | ROS_ERROR("Cannot get the relative pose from the base link to the laser from the tf tree." 82 | " Did you set the static transform publisher between %s to %s?", 83 | baseFrame_.c_str(), sensorFrames_[i].c_str()); 84 | exit(1); 85 | } 86 | loopRate.sleep(); 87 | } 88 | } 89 | tf::Quaternion quat(trans.getRotation().x(), trans.getRotation().y(), 90 | trans.getRotation().z(), trans.getRotation().w()); 91 | double roll, pitch, yaw; 92 | tf::Matrix3x3 rotMat(quat); 93 | rotMat.getRPY(roll, pitch, yaw); 94 | mcl3d::Pose poseTrans(trans.getOrigin().x(), trans.getOrigin().y(), trans.getOrigin().z(), roll, pitch, yaw); 95 | displacements_.push_back(poseTrans); 96 | } 97 | // printf("All the transformations have been obtained.\n"); 98 | 99 | // set subscribers 100 | for (int i = 0; i < (int)sensorTopicNames_.size(); ++i) { 101 | ros::Subscriber sub = nh_.subscribe(sensorTopicNames_[i], 1, &SensorPointsMerger::pointsCB, this); 102 | pointsSubs_.push_back(sub); 103 | } 104 | 105 | // set publisher 106 | pointsPub_ = nh_.advertise(mergedPointsName_, 5); 107 | 108 | // make empty 109 | mergedPoints_.points.clear(); 110 | printf("Initialization has been done.\n"); 111 | } 112 | 113 | void pointsCB(const ros::MessageEvent &event) { 114 | // check number of the sensor data 115 | std::string topicName = event.getConnectionHeader().at("topic"); 116 | std::string frame = event.getMessage()->header.frame_id; 117 | // printf("topicName = %s, frame = %s\n", topicName.c_str(), frame.c_str()); 118 | mcl3d::Pose poseTrans; 119 | for (int i = 0; i < (int)sensorTopicNames_.size(); ++i) { 120 | if (topicName == sensorTopicNames_[i] && frame == sensorFrames_[i]) { 121 | // return return if already have 122 | if (gotPoints_[i]) 123 | return; 124 | // add obtained points 125 | poseTrans = displacements_[i]; 126 | gotPoints_[i] = true; 127 | break; 128 | } 129 | } 130 | 131 | // transform and merge 132 | double cr = cos(poseTrans.getRoll()); 133 | double sr = sin(poseTrans.getRoll()); 134 | double cp = cos(poseTrans.getPitch()); 135 | double sp = sin(poseTrans.getPitch()); 136 | double cy = cos(poseTrans.getYaw()); 137 | double sy = sin(poseTrans.getYaw()); 138 | 139 | double m11 = cy * cp; 140 | double m12 = cy * sp * sr - sy * cr; 141 | double m13 = sy * sr + cy * sp * cr; 142 | double m21 = sy * cp; 143 | double m22 = cy * cr + sy * sp * sr; 144 | double m23 = sy * sp * cr - cy * sr; 145 | double m31 = -sp; 146 | double m32 = cp * sr; 147 | double m33 = cp * cr; 148 | 149 | pcl::PointCloud points; 150 | pcl::fromROSMsg(*event.getMessage(), points); 151 | 152 | for (int i = 0; i < (int)points.size(); ++i) { 153 | float x = points.points[i].x; 154 | float y = points.points[i].y; 155 | float z = points.points[i].z; 156 | geometry_msgs::Point32 p; 157 | p.x = x * m11 + y * m12 + z * m13 + poseTrans.getX(); 158 | p.y = x * m21 + y * m22 + z * m23 + poseTrans.getY(); 159 | p.z = x * m31 + y * m32 + z * m33 + poseTrans.getZ(); 160 | mergedPoints_.points.push_back(p); 161 | } 162 | 163 | // check whether all the sensor data has been obtained 164 | for (int i = 0; i < (int)gotPoints_.size(); ++i) { 165 | if (!gotPoints_[i]) 166 | return; 167 | } 168 | 169 | // publish the merged point cloud and make it empty for next publish 170 | sensor_msgs::PointCloud2 sensorPoints; 171 | sensor_msgs::convertPointCloudToPointCloud2(mergedPoints_, sensorPoints); 172 | sensorPoints.header.stamp = ros::Time::now(); 173 | sensorPoints.header.frame_id = mergedPointsFrame_; 174 | pointsPub_.publish(sensorPoints); 175 | 176 | // printf("publish\n"); 177 | mergedPoints_.points.clear(); 178 | for (int i = 0; i < (int)gotPoints_.size(); ++i) 179 | gotPoints_[i] = false; 180 | } 181 | 182 | void spin(void) { 183 | ros::spin(); 184 | } 185 | }; // class SensorPointsMerger 186 | 187 | int main(int argc, char **argv) { 188 | ros::init(argc, argv, "sensor_points_merger"); 189 | SensorPointsMerger node; 190 | node.spin(); 191 | return 0; 192 | } --------------------------------------------------------------------------------