├── FindGMock.cmake ├── README.md ├── amcl3d ├── CMakeLists.txt ├── LICENSE ├── launch │ ├── amcl3d.launch │ ├── amcl3d_floam.launch │ ├── amcl3d_rosin.launch │ └── map.launch ├── package.xml ├── rosdoc.yaml ├── rviz │ ├── amcl3d.rviz │ └── amcl3d_floam.rviz ├── src │ ├── Grid3d.cpp │ ├── Grid3d.h │ ├── Node.cpp │ ├── Node.h │ ├── Parameters.cpp │ ├── Parameters.h │ ├── ParticleFilter.cpp │ ├── ParticleFilter.h │ ├── PointCloudTools.cpp │ ├── PointCloudTools.h │ ├── amcl3d.cpp │ ├── amcl3d.h │ ├── floam │ │ ├── CMakeLists.txt │ │ ├── include │ │ │ ├── laserProcessingClass.h │ │ │ ├── lidar.h │ │ │ ├── lidarOptimization.h │ │ │ └── odomEstimationClass.h │ │ └── src │ │ │ ├── laserProcessingClass.cpp │ │ │ ├── laserProcessingNode.cpp │ │ │ ├── lidar.cpp │ │ │ ├── lidarOptimization.cpp │ │ │ ├── odomEstimationClass.cpp │ │ │ └── odomEstimationNode.cpp │ └── main.cpp └── tests │ ├── Grid3dTest.cpp │ ├── ParticleFilterTest.cpp │ ├── PointCloudToolsTest.cpp │ ├── data │ ├── grid_info.bin │ ├── mapfile_null.ot │ ├── mapfile_unknown_extension.unk │ ├── mapfile_wrong.bt │ ├── mapfile_wrong.ot │ ├── mappointcloud_msg.bin │ ├── nav_msg.bin │ └── particle_info.bin │ └── main.cpp ├── catkin.options ├── dependencies.rosinstall ├── doc ├── flow.png └── pf_demo.png └── rosinrange_msg ├── CMakeLists.txt ├── LICENSE ├── msg └── RangePose.msg └── package.xml /FindGMock.cmake: -------------------------------------------------------------------------------- 1 | # Locate the Google C++ Mocking Framework. 2 | # (This file is almost an identical copy of the original FindGTest.cmake file, 3 | # feel free to use it as it is or modify it for your own needs.) 4 | # 5 | # 6 | # Defines the following variables: 7 | # 8 | # GMOCK_FOUND - Found the Google Testing framework 9 | # GMOCK_INCLUDE_DIRS - Include directories 10 | # 11 | # Also defines the library variables below as normal 12 | # variables. These contain debug/optimized keywords when 13 | # a debugging library is found. 14 | # 15 | # GMOCK_BOTH_LIBRARIES - Both libgmock & libgmock-main 16 | # GMOCK_LIBRARIES - libgmock 17 | # GMOCK_MAIN_LIBRARIES - libgmock-main 18 | # 19 | # Accepts the following variables as input: 20 | # 21 | # GMOCK_ROOT - (as a CMake or environment variable) 22 | # The root directory of the gmock install prefix 23 | # 24 | # GMOCK_MSVC_SEARCH - If compiling with MSVC, this variable can be set to 25 | # "MD" or "MT" to enable searching a gmock build tree 26 | # (defaults: "MD") 27 | # 28 | #----------------------- 29 | # Example Usage: 30 | # 31 | # find_package(GMock REQUIRED) 32 | # include_directories(${GMOCK_INCLUDE_DIRS}) 33 | # 34 | # add_executable(foo foo.cc) 35 | # target_link_libraries(foo ${GMOCK_BOTH_LIBRARIES}) 36 | # 37 | #============================================================================= 38 | # This file is released under the MIT licence: 39 | # 40 | # Copyright (c) 2011 Matej Svec 41 | # 42 | # Permission is hereby granted, free of charge, to any person obtaining a copy 43 | # of this software and associated documentation files (the "Software"), to 44 | # deal in the Software without restriction, including without limitation the 45 | # rights to use, copy, modify, merge, publish, distribute, sublicense, and/or 46 | # sell copies of the Software, and to permit persons to whom the Software is 47 | # furnished to do so, subject to the following conditions: 48 | # 49 | # The above copyright notice and this permission notice shall be included in 50 | # all copies or substantial portions of the Software. 51 | # 52 | # THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 53 | # IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 54 | # FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 55 | # AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 56 | # LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING 57 | # FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS 58 | # IN THE SOFTWARE. 59 | #============================================================================= 60 | 61 | 62 | function(_gmock_append_debugs _endvar _library) 63 | if(${_library} AND ${_library}_DEBUG) 64 | set(_output optimized ${${_library}} debug ${${_library}_DEBUG}) 65 | else() 66 | set(_output ${${_library}}) 67 | endif() 68 | set(${_endvar} ${_output} PARENT_SCOPE) 69 | endfunction() 70 | 71 | function(_gmock_find_library _name) 72 | find_library(${_name} 73 | NAMES ${ARGN} 74 | HINTS 75 | $ENV{GMOCK_ROOT} 76 | ${GMOCK_ROOT} 77 | PATH_SUFFIXES ${_gmock_libpath_suffixes} 78 | ) 79 | mark_as_advanced(${_name}) 80 | endfunction() 81 | 82 | 83 | if(NOT DEFINED GMOCK_MSVC_SEARCH) 84 | set(GMOCK_MSVC_SEARCH MD) 85 | endif() 86 | 87 | set(_gmock_libpath_suffixes lib) 88 | if(MSVC) 89 | if(GMOCK_MSVC_SEARCH STREQUAL "MD") 90 | list(APPEND _gmock_libpath_suffixes 91 | msvc/gmock-md/Debug 92 | msvc/gmock-md/Release) 93 | elseif(GMOCK_MSVC_SEARCH STREQUAL "MT") 94 | list(APPEND _gmock_libpath_suffixes 95 | msvc/gmock/Debug 96 | msvc/gmock/Release) 97 | endif() 98 | endif() 99 | 100 | find_path(GMOCK_INCLUDE_DIR gmock/gmock.h 101 | HINTS 102 | $ENV{GMOCK_ROOT}/include 103 | ${GMOCK_ROOT}/include 104 | ) 105 | mark_as_advanced(GMOCK_INCLUDE_DIR) 106 | 107 | if(MSVC AND GMOCK_MSVC_SEARCH STREQUAL "MD") 108 | # The provided /MD project files for Google Mock add -md suffixes to the 109 | # library names. 110 | _gmock_find_library(GMOCK_LIBRARY gmock-md gmock) 111 | _gmock_find_library(GMOCK_LIBRARY_DEBUG gmock-mdd gmockd) 112 | _gmock_find_library(GMOCK_MAIN_LIBRARY gmock_main-md gmock_main) 113 | _gmock_find_library(GMOCK_MAIN_LIBRARY_DEBUG gmock_main-mdd gmock_maind) 114 | else() 115 | _gmock_find_library(GMOCK_LIBRARY gmock) 116 | _gmock_find_library(GMOCK_LIBRARY_DEBUG gmockd) 117 | _gmock_find_library(GMOCK_MAIN_LIBRARY gmock_main) 118 | _gmock_find_library(GMOCK_MAIN_LIBRARY_DEBUG gmock_maind) 119 | endif() 120 | 121 | include(${CMAKE_CURRENT_LIST_DIR}/FindPackageHandleStandardArgs.cmake) 122 | FIND_PACKAGE_HANDLE_STANDARD_ARGS(GMock DEFAULT_MSG GMOCK_LIBRARY GMOCK_INCLUDE_DIR GMOCK_MAIN_LIBRARY) 123 | 124 | if(GMOCK_FOUND) 125 | set(GMOCK_INCLUDE_DIRS ${GMOCK_INCLUDE_DIR}) 126 | _gmock_append_debugs(GMOCK_LIBRARIES GMOCK_LIBRARY) 127 | _gmock_append_debugs(GMOCK_MAIN_LIBRARIES GMOCK_MAIN_LIBRARY) 128 | set(GMOCK_BOTH_LIBRARIES ${GMOCK_LIBRARIES} ${GMOCK_MAIN_LIBRARIES}) 129 | endif() 130 | 131 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # amcl3d_test 2 | 3 | ### Acknowledgements 4 | 5 | This just a test demo. And maintenance may not be updated. 6 | 7 | See the [blog](https://blog.csdn.net/u012700322/article/details/120032339) for the algorithm flow. 8 | 9 | ### Overview 10 | 11 | ![image](https://github.com/chengwei0427/amcl3d_test/blob/master/doc/flow.png) 12 | 13 | This is a package is a **"Adaptive Monte-Carlo Localization in 3D"**. 14 | 15 | It is a particle filter that estimates the localization of a robot moving in a 3D environment. 16 | 17 | It takes information from an odometry, 3D laser point-clouds. 18 | 19 | 20 | The amcl3d package has been tested under [ROS] Kinetic and Ubuntu 16.04. 21 | 22 | #### Detailed Description 23 | 24 | 25 | 26 | 27 | ### Demostration Image 28 | 29 | ![image](https://github.com/chengwei0427/amcl3d_test/blob/master/doc/pf_demo.png) 30 | 31 | ### Installation 32 | 33 | #### Building from Source 34 | 35 | ##### Building 36 | 37 | To build from source, clone the latest version from this repository into your catkin workspace and compile the package using 38 | 39 | cd catkin_workspace/src 40 | git clone https://github.com/chengwei0427/amcl3d_test.git 41 | cd ../ 42 | catkin build 43 | 44 | 45 | -------------------------------------------------------------------------------- /amcl3d/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | project(amcl3d) 2 | cmake_minimum_required(VERSION 3.5.1) 3 | 4 | # add_compile_options(-std=c++11) 5 | set (CMAKE_BUILD_TYPE "Release") 6 | set( CMAKE_CXX_FLAGS_RELEASE "$ENV{CXXFLAGS} -o3 -w -fpermissive -fPIC") 7 | set( CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -fPIC") 8 | set( CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -fPIC") 9 | SET( CMAKE_CXX_FLAGS "-std=c++11 -O3") 10 | ################################## 11 | ## Configure CATKIN dependecies ## 12 | ################################## 13 | 14 | find_package(catkin REQUIRED 15 | COMPONENTS 16 | visualization_msgs 17 | nav_msgs 18 | # octomap_ros 19 | pcl_ros 20 | rosinrange_msg 21 | ) 22 | 23 | ################################### 24 | ## CATKIN specific configuration ## 25 | ################################### 26 | 27 | catkin_package( 28 | CATKIN_DEPENDS 29 | visualization_msgs 30 | nav_msgs 31 | # octomap_ros 32 | pcl_ros 33 | rosinrange_msg 34 | ) 35 | 36 | ####################### 37 | ## Configuring GMock ## 38 | ####################### 39 | 40 | find_package(GMock) 41 | 42 | ####################### 43 | ## Configuring 3d party ## 44 | ####################### 45 | set(3RDPARTY_DIR ${PROJECT_SOURCE_DIR}/../../../../workspace/3rdparty) 46 | #algo inc & lib 47 | set(ALGO_HEADER_DIR ${3RDPARTY_DIR}/../vscommon/inc) 48 | set(ALGO_LIB_DIR ${3RDPARTY_DIR}/../vscommon/lib) 49 | #eigen 50 | set(EIGEN_HEADER_DIR ${3RDPARTY_DIR}/Eigen/include/eigen3) 51 | #boost 52 | set(Boost_USE_STATIC_LIBS ON) 53 | find_package(Boost COMPONENTS log thread system regex date_time serialization filesystem program_options timer chrono REQUIRED) 54 | set(BOOST_HEADER_DIR ${Boost_INCLUDE_DIRS}) 55 | #flann 56 | set(FLANN_HEADER_DIR ${3RDPARTY_DIR}/flann-1.8.4/include) 57 | set(FLANN_LIB_DIR ${3RDPARTY_DIR}/flann-1.8.4/lib) 58 | #log4cpp 59 | set(LOG4CPP_HEADER_DIR ${3RDPARTY_DIR}/log4cpp/include) 60 | set(LOG4CPP_LIB_DIR ${3RDPARTY_DIR}/log4cpp/lib) 61 | #tinyxml2 62 | set(TINYXML2_HEADER_DIR ${3RDPARTY_DIR}/tinyxml2/include) 63 | set(TINYXML2_LIB_DIR ${3RDPARTY_DIR}/tinyxml2/lib) 64 | #include opencv 65 | find_package( OpenCV REQUIRED ) 66 | set(OPENCV_HEADER_DIR ${OpenCV_INCLUDE_DIRS}) 67 | ########### 68 | ## Build ## 69 | ########### 70 | 71 | include_directories(SYSTEM 72 | ${catkin_INCLUDE_DIRS} 73 | ${ALGO_HEADER_DIR} 74 | ${ALGO_HEADER_DIR}/robcommon 75 | ${EIGEN_HEADER_DIR} 76 | ${BOOST_HEADER_DIR} 77 | ${FLANN_HEADER_DIR} 78 | ${LOG4CPP_HEADER_DIR} 79 | # ${GTEST_HEADER_DIR} 80 | ${TINYXML2_HEADER_DIR} 81 | ${OPENCV_HEADER_DIR} 82 | ) 83 | 84 | link_directories( 85 | ${Boost_LIBRARIES} 86 | ${OpenCV_LIBRARIES} 87 | ${ALGO_LIB_DIR} 88 | ${FLANN_LIB_DIR} 89 | ${LOG4CPP_LIB_DIR} 90 | # ${GTEST_LIB_DIR} 91 | ${TINYXML2_LIB_DIR} 92 | ) 93 | 94 | link_libraries( 95 | libtinyxml2.a 96 | liblog4cpp.a 97 | # libgtest.a 98 | libflann_s.a 99 | libflann_cpp_s.a 100 | ${Boost_LIBRARIES} 101 | ${OpenCV_LIBRARIES} 102 | libvscommon.a) 103 | 104 | add_library(${PROJECT_NAME} STATIC 105 | src/Grid3d.cpp 106 | src/Grid3d.h 107 | src/amcl3d.cpp 108 | src/amcl3d.h 109 | src/ParticleFilter.cpp 110 | src/ParticleFilter.h 111 | src/PointCloudTools.cpp 112 | src/PointCloudTools.h) 113 | add_dependencies(${PROJECT_NAME} ${catkin_EXPORTED_TARGETS}) 114 | target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES}) 115 | 116 | add_executable(${PROJECT_NAME}_node 117 | src/main.cpp 118 | src/Node.cpp 119 | src/Node.h 120 | # src/Parameters.cpp 121 | src/Parameters.h) 122 | add_dependencies(${PROJECT_NAME}_node ${catkin_EXPORTED_TARGETS}) 123 | target_link_libraries(${PROJECT_NAME}_node 124 | ${catkin_LIBRARIES} ${PROJECT_NAME} 125 | ${Boost_LIBRARIES} 126 | ${OpenCV_LIBRARIES} 127 | libtinyxml2.a 128 | liblog4cpp.a 129 | # libgtest.a 130 | libflann_s.a 131 | libflann_cpp_s.a 132 | libvscommon.a 133 | liblocation.a 134 | ) 135 | 136 | ############# 137 | ## floam ## 138 | ############# 139 | add_subdirectory(${PROJECT_SOURCE_DIR}/src/floam) 140 | ############# 141 | ## Testing ## 142 | ############# 143 | 144 | if(GMOCK_FOUND) 145 | include_directories(src) 146 | include_directories(SYSTEM ${GMOCK_INCLUDE_DIRS}) 147 | 148 | file(GLOB_RECURSE TESTS_SRCS "tests/*.c*") 149 | file(GLOB_RECURSE TESTS_HDRS "tests/*.h*") 150 | 151 | catkin_add_gtest(${PROJECT_NAME}_tests 152 | ${TESTS_SRCS} ${TESTS_HDRS}) 153 | if(TARGET ${PROJECT_NAME}_tests) 154 | target_compile_definitions(${PROJECT_NAME}_tests PRIVATE 155 | PROJECT_SOURCE_DIR="${PROJECT_SOURCE_DIR}") 156 | target_link_libraries(${PROJECT_NAME}_tests 157 | ${GMOCK_BOTH_LIBRARIES} ${PROJECT_NAME}) 158 | endif() 159 | endif() 160 | -------------------------------------------------------------------------------- /amcl3d/LICENSE: -------------------------------------------------------------------------------- 1 | 2 | Apache License 3 | Version 2.0, January 2004 4 | http://www.apache.org/licenses/ 5 | 6 | TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION 7 | 8 | 1. Definitions. 9 | 10 | "License" shall mean the terms and conditions for use, reproduction, 11 | and distribution as defined by Sections 1 through 9 of this document. 12 | 13 | "Licensor" shall mean the copyright owner or entity authorized by 14 | the copyright owner that is granting the License. 15 | 16 | "Legal Entity" shall mean the union of the acting entity and all 17 | other entities that control, are controlled by, or are under common 18 | control with that entity. For the purposes of this definition, 19 | "control" means (i) the power, direct or indirect, to cause the 20 | direction or management of such entity, whether by contract or 21 | otherwise, or (ii) ownership of fifty percent (50%) or more of the 22 | outstanding shares, or (iii) beneficial ownership of such entity. 23 | 24 | "You" (or "Your") shall mean an individual or Legal Entity 25 | exercising permissions granted by this License. 26 | 27 | "Source" form shall mean the preferred form for making modifications, 28 | including but not limited to software source code, documentation 29 | source, and configuration files. 30 | 31 | "Object" form shall mean any form resulting from mechanical 32 | transformation or translation of a Source form, including but 33 | not limited to compiled object code, generated documentation, 34 | and conversions to other media types. 35 | 36 | "Work" shall mean the work of authorship, whether in Source or 37 | Object form, made available under the License, as indicated by a 38 | copyright notice that is included in or attached to the work 39 | (an example is provided in the Appendix below). 40 | 41 | "Derivative Works" shall mean any work, whether in Source or Object 42 | form, that is based on (or derived from) the Work and for which the 43 | editorial revisions, annotations, elaborations, or other modifications 44 | represent, as a whole, an original work of authorship. For the purposes 45 | of this License, Derivative Works shall not include works that remain 46 | separable from, or merely link (or bind by name) to the interfaces of, 47 | the Work and Derivative Works thereof. 48 | 49 | "Contribution" shall mean any work of authorship, including 50 | the original version of the Work and any modifications or additions 51 | to that Work or Derivative Works thereof, that is intentionally 52 | submitted to Licensor for inclusion in the Work by the copyright owner 53 | or by an individual or Legal Entity authorized to submit on behalf of 54 | the copyright owner. For the purposes of this definition, "submitted" 55 | means any form of electronic, verbal, or written communication sent 56 | to the Licensor or its representatives, including but not limited to 57 | communication on electronic mailing lists, source code control systems, 58 | and issue tracking systems that are managed by, or on behalf of, the 59 | Licensor for the purpose of discussing and improving the Work, but 60 | excluding communication that is conspicuously marked or otherwise 61 | designated in writing by the copyright owner as "Not a Contribution." 62 | 63 | "Contributor" shall mean Licensor and any individual or Legal Entity 64 | on behalf of whom a Contribution has been received by Licensor and 65 | subsequently incorporated within the Work. 66 | 67 | 2. Grant of Copyright License. Subject to the terms and conditions of 68 | this License, each Contributor hereby grants to You a perpetual, 69 | worldwide, non-exclusive, no-charge, royalty-free, irrevocable 70 | copyright license to reproduce, prepare Derivative Works of, 71 | publicly display, publicly perform, sublicense, and distribute the 72 | Work and such Derivative Works in Source or Object form. 73 | 74 | 3. Grant of Patent License. Subject to the terms and conditions of 75 | this License, each Contributor hereby grants to You a perpetual, 76 | worldwide, non-exclusive, no-charge, royalty-free, irrevocable 77 | (except as stated in this section) patent license to make, have made, 78 | use, offer to sell, sell, import, and otherwise transfer the Work, 79 | where such license applies only to those patent claims licensable 80 | by such Contributor that are necessarily infringed by their 81 | Contribution(s) alone or by combination of their Contribution(s) 82 | with the Work to which such Contribution(s) was submitted. If You 83 | institute patent litigation against any entity (including a 84 | cross-claim or counterclaim in a lawsuit) alleging that the Work 85 | or a Contribution incorporated within the Work constitutes direct 86 | or contributory patent infringement, then any patent licenses 87 | granted to You under this License for that Work shall terminate 88 | as of the date such litigation is filed. 89 | 90 | 4. Redistribution. You may reproduce and distribute copies of the 91 | Work or Derivative Works thereof in any medium, with or without 92 | modifications, and in Source or Object form, provided that You 93 | meet the following conditions: 94 | 95 | (a) You must give any other recipients of the Work or 96 | Derivative Works a copy of this License; and 97 | 98 | (b) You must cause any modified files to carry prominent notices 99 | stating that You changed the files; and 100 | 101 | (c) You must retain, in the Source form of any Derivative Works 102 | that You distribute, all copyright, patent, trademark, and 103 | attribution notices from the Source form of the Work, 104 | excluding those notices that do not pertain to any part of 105 | the Derivative Works; and 106 | 107 | (d) If the Work includes a "NOTICE" text file as part of its 108 | distribution, then any Derivative Works that You distribute must 109 | include a readable copy of the attribution notices contained 110 | within such NOTICE file, excluding those notices that do not 111 | pertain to any part of the Derivative Works, in at least one 112 | of the following places: within a NOTICE text file distributed 113 | as part of the Derivative Works; within the Source form or 114 | documentation, if provided along with the Derivative Works; or, 115 | within a display generated by the Derivative Works, if and 116 | wherever such third-party notices normally appear. The contents 117 | of the NOTICE file are for informational purposes only and 118 | do not modify the License. You may add Your own attribution 119 | notices within Derivative Works that You distribute, alongside 120 | or as an addendum to the NOTICE text from the Work, provided 121 | that such additional attribution notices cannot be construed 122 | as modifying the License. 123 | 124 | You may add Your own copyright statement to Your modifications and 125 | may provide additional or different license terms and conditions 126 | for use, reproduction, or distribution of Your modifications, or 127 | for any such Derivative Works as a whole, provided Your use, 128 | reproduction, and distribution of the Work otherwise complies with 129 | the conditions stated in this License. 130 | 131 | 5. Submission of Contributions. Unless You explicitly state otherwise, 132 | any Contribution intentionally submitted for inclusion in the Work 133 | by You to the Licensor shall be under the terms and conditions of 134 | this License, without any additional terms or conditions. 135 | Notwithstanding the above, nothing herein shall supersede or modify 136 | the terms of any separate license agreement you may have executed 137 | with Licensor regarding such Contributions. 138 | 139 | 6. Trademarks. This License does not grant permission to use the trade 140 | names, trademarks, service marks, or product names of the Licensor, 141 | except as required for reasonable and customary use in describing the 142 | origin of the Work and reproducing the content of the NOTICE file. 143 | 144 | 7. Disclaimer of Warranty. Unless required by applicable law or 145 | agreed to in writing, Licensor provides the Work (and each 146 | Contributor provides its Contributions) on an "AS IS" BASIS, 147 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or 148 | implied, including, without limitation, any warranties or conditions 149 | of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A 150 | PARTICULAR PURPOSE. You are solely responsible for determining the 151 | appropriateness of using or redistributing the Work and assume any 152 | risks associated with Your exercise of permissions under this License. 153 | 154 | 8. Limitation of Liability. In no event and under no legal theory, 155 | whether in tort (including negligence), contract, or otherwise, 156 | unless required by applicable law (such as deliberate and grossly 157 | negligent acts) or agreed to in writing, shall any Contributor be 158 | liable to You for damages, including any direct, indirect, special, 159 | incidental, or consequential damages of any character arising as a 160 | result of this License or out of the use or inability to use the 161 | Work (including but not limited to damages for loss of goodwill, 162 | work stoppage, computer failure or malfunction, or any and all 163 | other commercial damages or losses), even if such Contributor 164 | has been advised of the possibility of such damages. 165 | 166 | 9. Accepting Warranty or Additional Liability. While redistributing 167 | the Work or Derivative Works thereof, You may choose to offer, 168 | and charge a fee for, acceptance of support, warranty, indemnity, 169 | or other liability obligations and/or rights consistent with this 170 | License. However, in accepting such obligations, You may act only 171 | on Your own behalf and on Your sole responsibility, not on behalf 172 | of any other Contributor, and only if You agree to indemnify, 173 | defend, and hold each Contributor harmless for any liability 174 | incurred by, or claims asserted against, such Contributor by reason 175 | of your accepting any such warranty or additional liability. 176 | 177 | END OF TERMS AND CONDITIONS 178 | 179 | APPENDIX: How to apply the Apache License to your work. 180 | 181 | To apply the Apache License to your work, attach the following 182 | boilerplate notice, with the fields enclosed by brackets "[]" 183 | replaced with your own identifying information. (Don't include 184 | the brackets!) The text should be enclosed in the appropriate 185 | comment syntax for the file format. We also recommend that a 186 | file or class name and description of purpose be included on the 187 | same "printed page" as the copyright notice for easier 188 | identification within third-party archives. 189 | 190 | Copyright [yyyy] [name of copyright owner] 191 | 192 | Licensed under the Apache License, Version 2.0 (the "License"); 193 | you may not use this file except in compliance with the License. 194 | You may obtain a copy of the License at 195 | 196 | http://www.apache.org/licenses/LICENSE-2.0 197 | 198 | Unless required by applicable law or agreed to in writing, software 199 | distributed under the License is distributed on an "AS IS" BASIS, 200 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 201 | See the License for the specific language governing permissions and 202 | limitations under the License. 203 | -------------------------------------------------------------------------------- /amcl3d/launch/amcl3d.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | 86 | 87 | 88 | 89 | 90 | 91 | 92 | 93 | 94 | 95 | 96 | 97 | 98 | 99 | 100 | 101 | 102 | 103 | 104 | 105 | 106 | 107 | 108 | 109 | 110 | 111 | 112 | 113 | 114 | 115 | 116 | 117 | 118 | 119 | 120 | 121 | 122 | 123 | 124 | 125 | -------------------------------------------------------------------------------- /amcl3d/launch/amcl3d_floam.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | -------------------------------------------------------------------------------- /amcl3d/launch/amcl3d_rosin.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | -------------------------------------------------------------------------------- /amcl3d/launch/map.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | -------------------------------------------------------------------------------- /amcl3d/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | amcl3d 5 | 1.0.0 6 | 7 |

8 | Adaptive Monte Carlo Localization (AMCL) in 3D. 9 |

10 |

11 | amcl3d is a probabilistic algorithm to localizate a robot moving in 3D. 12 | It uses Monte-Carlo Localization, i.e. a particle filter. 13 | This package use a laser sensor and radio-range sensors to localizate a UAV within a known map. 14 |

15 | 16 | http://wiki.ros.org/amcl3d 17 | Francisco J.Perez-Grau 18 | Francisco Cuesta Rodriguez 19 | Paloma Carrasco Fernandez 20 | Apache 2.0 21 | 22 | catkin 23 | 24 | visualization_msgs 25 | nav_msgs 26 | 27 | pcl_ros 28 | rosinrange_msg 29 | 30 | 31 | -------------------------------------------------------------------------------- /amcl3d/rosdoc.yaml: -------------------------------------------------------------------------------- 1 | - builder: doxygen 2 | name: amcl3d 3 | output_dir: doc 4 | file_patterns: '*.c *.cpp *.h *.hpp' 5 | exclude_patterns: '*/rosinrange_msg/* */amcl3d/demo* */amcl3d/tests*' -------------------------------------------------------------------------------- /amcl3d/rviz/amcl3d.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 78 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /TF1/Frames1 8 | Splitter Ratio: 0.554795027 9 | Tree Height: 552 10 | - Class: rviz/Selection 11 | Name: Selection 12 | - Class: rviz/Tool Properties 13 | Expanded: 14 | - /Publish Point1 15 | - /2D Pose Estimate1 16 | Name: Tool Properties 17 | Splitter Ratio: 0.588679016 18 | - Class: rviz/Views 19 | Expanded: 20 | - /Current View1 21 | Name: Views 22 | Splitter Ratio: 0.5 23 | - Class: rviz/Time 24 | Experimental: false 25 | Name: Time 26 | SyncMode: 0 27 | SyncSource: PointCloud2 28 | - Class: rviz/Tool Properties 29 | Expanded: 30 | - /2D Pose Estimate1 31 | Name: Tool Properties 32 | Splitter Ratio: 0.5 33 | Toolbars: 34 | toolButtonStyle: 2 35 | Visualization Manager: 36 | Class: "" 37 | Displays: 38 | - Alpha: 0.5 39 | Cell Size: 1 40 | Class: rviz/Grid 41 | Color: 160; 160; 164 42 | Enabled: true 43 | Line Style: 44 | Line Width: 0.0299999993 45 | Value: Lines 46 | Name: Grid 47 | Normal Cell Count: 0 48 | Offset: 49 | X: 0 50 | Y: 0 51 | Z: 0 52 | Plane: XY 53 | Plane Cell Count: 100 54 | Reference Frame: 55 | Value: true 56 | - Class: rviz/TF 57 | Enabled: true 58 | Frame Timeout: 15 59 | Frames: 60 | All Enabled: false 61 | base_link: 62 | Value: true 63 | odom: 64 | Value: true 65 | velodyne: 66 | Value: true 67 | world: 68 | Value: true 69 | Marker Scale: 10 70 | Name: TF 71 | Show Arrows: true 72 | Show Axes: true 73 | Show Names: true 74 | Tree: 75 | world: 76 | odom: 77 | base_link: 78 | velodyne: 79 | {} 80 | Update Interval: 0 81 | Value: true 82 | - Alpha: 1 83 | Arrow Length: 0.300000012 84 | Axes Length: 0.300000012 85 | Axes Radius: 0.00999999978 86 | Class: rviz/PoseArray 87 | Color: 255; 25; 0 88 | Enabled: true 89 | Head Length: 0.0700000003 90 | Head Radius: 0.0299999993 91 | Name: Particles 92 | Shaft Length: 0.230000004 93 | Shaft Radius: 0.00999999978 94 | Shape: Arrow (Flat) 95 | Topic: /amcl3d_node/particle_cloud 96 | Unreliable: false 97 | Value: true 98 | - Class: rviz/Marker 99 | Enabled: false 100 | Marker Topic: /amcl3d_node/range 101 | Name: Ranges 102 | Namespaces: 103 | {} 104 | Queue Size: 100 105 | Value: false 106 | - Alpha: 1 107 | Autocompute Intensity Bounds: true 108 | Autocompute Value Bounds: 109 | Max Value: 144.778641 110 | Min Value: -91.8346329 111 | Value: true 112 | Axis: X 113 | Channel Name: intensity 114 | Class: rviz/PointCloud2 115 | Color: 255; 255; 255 116 | Color Transformer: AxisColor 117 | Decay Time: 0 118 | Enabled: true 119 | Invert Rainbow: false 120 | Max Color: 255; 255; 255 121 | Max Intensity: 4096 122 | Min Color: 0; 0; 0 123 | Min Intensity: 0 124 | Name: PointCloud2 125 | Position Transformer: XYZ 126 | Queue Size: 30 127 | Selectable: true 128 | Size (Pixels): 1 129 | Size (m): 0.00999999978 130 | Style: Points 131 | Topic: /amcl3d_node/map_point_cloud 132 | Unreliable: false 133 | Use Fixed Frame: true 134 | Use rainbow: true 135 | Value: true 136 | - Alpha: 1 137 | Autocompute Intensity Bounds: true 138 | Autocompute Value Bounds: 139 | Max Value: 10 140 | Min Value: -10 141 | Value: true 142 | Axis: Z 143 | Channel Name: intensity 144 | Class: rviz/PointCloud2 145 | Color: 255; 255; 255 146 | Color Transformer: Intensity 147 | Decay Time: 0 148 | Enabled: true 149 | Invert Rainbow: false 150 | Max Color: 255; 255; 255 151 | Max Intensity: 215 152 | Min Color: 0; 0; 0 153 | Min Intensity: 0 154 | Name: PointCloud2 155 | Position Transformer: XYZ 156 | Queue Size: 10 157 | Selectable: true 158 | Size (Pixels): 3 159 | Size (m): 0.0500000007 160 | Style: Points 161 | Topic: /velodyne_points 162 | Unreliable: false 163 | Use Fixed Frame: true 164 | Use rainbow: true 165 | Value: true 166 | - Alpha: 1 167 | Autocompute Intensity Bounds: true 168 | Autocompute Value Bounds: 169 | Max Value: 17.6393681 170 | Min Value: -1.59433985 171 | Value: true 172 | Axis: Z 173 | Channel Name: intensity 174 | Class: rviz/PointCloud2 175 | Color: 255; 255; 255 176 | Color Transformer: AxisColor 177 | Decay Time: 0 178 | Enabled: false 179 | Invert Rainbow: false 180 | Max Color: 255; 255; 255 181 | Max Intensity: 4096 182 | Min Color: 0; 0; 0 183 | Min Intensity: 0 184 | Name: PointCloud2 185 | Position Transformer: XYZ 186 | Queue Size: 10 187 | Selectable: true 188 | Size (Pixels): 3 189 | Size (m): 0.100000001 190 | Style: Points 191 | Topic: /amcl3d_node/pointcloud_filtered 192 | Unreliable: false 193 | Use Fixed Frame: true 194 | Use rainbow: true 195 | Value: false 196 | - Alpha: 1 197 | Autocompute Intensity Bounds: true 198 | Autocompute Value Bounds: 199 | Max Value: 10 200 | Min Value: -10 201 | Value: true 202 | Axis: Z 203 | Channel Name: intensity 204 | Class: rviz/PointCloud2 205 | Color: 255; 255; 255 206 | Color Transformer: Intensity 207 | Decay Time: 0 208 | Enabled: false 209 | Invert Rainbow: false 210 | Max Color: 255; 255; 255 211 | Max Intensity: 4096 212 | Min Color: 0; 0; 0 213 | Min Intensity: 0 214 | Name: PointCloud2 215 | Position Transformer: XYZ 216 | Queue Size: 10 217 | Selectable: true 218 | Size (Pixels): 3 219 | Size (m): 0.00999999978 220 | Style: Flat Squares 221 | Topic: /amcl3d_node/pointcloud_passfiltered 222 | Unreliable: false 223 | Use Fixed Frame: true 224 | Use rainbow: true 225 | Value: false 226 | - Alpha: 0.699999988 227 | Class: rviz/Map 228 | Color Scheme: map 229 | Draw Behind: false 230 | Enabled: false 231 | Name: Map 232 | Topic: /amcl3d_node/grid_slice 233 | Unreliable: false 234 | Use Timestamp: false 235 | Value: false 236 | Enabled: true 237 | Global Options: 238 | Background Color: 48; 48; 48 239 | Default Light: true 240 | Fixed Frame: world 241 | Frame Rate: 30 242 | Name: root 243 | Tools: 244 | - Class: rviz/Interact 245 | Hide Inactive Objects: true 246 | - Class: rviz/MoveCamera 247 | - Class: rviz/Select 248 | - Class: rviz/FocusCamera 249 | - Class: rviz/Measure 250 | - Class: rviz/PublishPoint 251 | Single click: true 252 | Topic: /clicked_point 253 | - Class: rviz/SetInitialPose 254 | Topic: initialpose 255 | Value: true 256 | Views: 257 | Current: 258 | Class: rviz/Orbit 259 | Distance: 69.5626678 260 | Enable Stereo Rendering: 261 | Stereo Eye Separation: 0.0599999987 262 | Stereo Focal Distance: 1 263 | Swap Stereo Eyes: false 264 | Value: false 265 | Focal Point: 266 | X: 10.6786909 267 | Y: -1.90463603 268 | Z: 47.697937 269 | Focal Shape Fixed Size: true 270 | Focal Shape Size: 0.0500000007 271 | Invert Z Axis: false 272 | Name: Current View 273 | Near Clip Distance: 0.00999999978 274 | Pitch: 1.56979632 275 | Target Frame: 276 | Value: Orbit (rviz) 277 | Yaw: 4.86725426 278 | Saved: ~ 279 | Window Geometry: 280 | Displays: 281 | collapsed: false 282 | Height: 1056 283 | Hide Left Dock: false 284 | Hide Right Dock: true 285 | QMainWindow State: 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 286 | Selection: 287 | collapsed: false 288 | Time: 289 | collapsed: false 290 | Tool Properties: 291 | collapsed: false 292 | Views: 293 | collapsed: true 294 | Width: 1855 295 | X: 65 296 | Y: 24 297 | -------------------------------------------------------------------------------- /amcl3d/rviz/amcl3d_floam.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 78 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /TF1/Frames1 8 | Splitter Ratio: 0.554795027 9 | Tree Height: 552 10 | - Class: rviz/Selection 11 | Name: Selection 12 | - Class: rviz/Tool Properties 13 | Expanded: 14 | - /Publish Point1 15 | - /2D Pose Estimate1 16 | Name: Tool Properties 17 | Splitter Ratio: 0.588679016 18 | - Class: rviz/Views 19 | Expanded: 20 | - /Current View1 21 | Name: Views 22 | Splitter Ratio: 0.5 23 | - Class: rviz/Time 24 | Experimental: false 25 | Name: Time 26 | SyncMode: 0 27 | SyncSource: map 28 | - Class: rviz/Tool Properties 29 | Expanded: 30 | - /2D Pose Estimate1 31 | Name: Tool Properties 32 | Splitter Ratio: 0.5 33 | Toolbars: 34 | toolButtonStyle: 2 35 | Visualization Manager: 36 | Class: "" 37 | Displays: 38 | - Alpha: 0.5 39 | Cell Size: 1 40 | Class: rviz/Grid 41 | Color: 160; 160; 164 42 | Enabled: true 43 | Line Style: 44 | Line Width: 0.0299999993 45 | Value: Lines 46 | Name: Grid 47 | Normal Cell Count: 0 48 | Offset: 49 | X: 0 50 | Y: 0 51 | Z: 0 52 | Plane: XY 53 | Plane Cell Count: 100 54 | Reference Frame: 55 | Value: true 56 | - Class: rviz/TF 57 | Enabled: true 58 | Frame Timeout: 15 59 | Frames: 60 | All Enabled: false 61 | Marker Scale: 10 62 | Name: TF 63 | Show Arrows: true 64 | Show Axes: true 65 | Show Names: true 66 | Tree: 67 | {} 68 | Update Interval: 0 69 | Value: true 70 | - Alpha: 1 71 | Arrow Length: 0.300000012 72 | Axes Length: 0.300000012 73 | Axes Radius: 0.00999999978 74 | Class: rviz/PoseArray 75 | Color: 255; 25; 0 76 | Enabled: true 77 | Head Length: 0.0700000003 78 | Head Radius: 0.0299999993 79 | Name: Particles 80 | Shaft Length: 0.230000004 81 | Shaft Radius: 0.00999999978 82 | Shape: Arrow (Flat) 83 | Topic: /amcl3d_node/particle_cloud 84 | Unreliable: false 85 | Value: true 86 | - Class: rviz/Marker 87 | Enabled: false 88 | Marker Topic: /amcl3d_node/range 89 | Name: Ranges 90 | Namespaces: 91 | {} 92 | Queue Size: 100 93 | Value: false 94 | - Alpha: 1 95 | Autocompute Intensity Bounds: true 96 | Autocompute Value Bounds: 97 | Max Value: 128.554733 98 | Min Value: -125.507805 99 | Value: true 100 | Axis: X 101 | Channel Name: intensity 102 | Class: rviz/PointCloud2 103 | Color: 255; 255; 255 104 | Color Transformer: AxisColor 105 | Decay Time: 0 106 | Enabled: true 107 | Invert Rainbow: false 108 | Max Color: 255; 255; 255 109 | Max Intensity: 4096 110 | Min Color: 0; 0; 0 111 | Min Intensity: 0 112 | Name: map 113 | Position Transformer: XYZ 114 | Queue Size: 30 115 | Selectable: true 116 | Size (Pixels): 1 117 | Size (m): 0.00999999978 118 | Style: Points 119 | Topic: /amcl3d_node/map_point_cloud 120 | Unreliable: false 121 | Use Fixed Frame: true 122 | Use rainbow: true 123 | Value: true 124 | - Alpha: 1 125 | Autocompute Intensity Bounds: true 126 | Autocompute Value Bounds: 127 | Max Value: 10 128 | Min Value: -10 129 | Value: true 130 | Axis: Z 131 | Channel Name: intensity 132 | Class: rviz/PointCloud2 133 | Color: 255; 255; 255 134 | Color Transformer: Intensity 135 | Decay Time: 0 136 | Enabled: true 137 | Invert Rainbow: false 138 | Max Color: 255; 255; 255 139 | Max Intensity: 215 140 | Min Color: 0; 0; 0 141 | Min Intensity: 0 142 | Name: PointCloud2 143 | Position Transformer: XYZ 144 | Queue Size: 10 145 | Selectable: true 146 | Size (Pixels): 3 147 | Size (m): 0.0500000007 148 | Style: Points 149 | Topic: /velodyne_points 150 | Unreliable: false 151 | Use Fixed Frame: true 152 | Use rainbow: true 153 | Value: true 154 | - Alpha: 1 155 | Autocompute Intensity Bounds: true 156 | Autocompute Value Bounds: 157 | Max Value: 17.6393681 158 | Min Value: -1.59433985 159 | Value: true 160 | Axis: Z 161 | Channel Name: intensity 162 | Class: rviz/PointCloud2 163 | Color: 255; 255; 255 164 | Color Transformer: AxisColor 165 | Decay Time: 0 166 | Enabled: false 167 | Invert Rainbow: false 168 | Max Color: 255; 255; 255 169 | Max Intensity: 4096 170 | Min Color: 0; 0; 0 171 | Min Intensity: 0 172 | Name: PointCloud2 173 | Position Transformer: XYZ 174 | Queue Size: 10 175 | Selectable: true 176 | Size (Pixels): 3 177 | Size (m): 0.100000001 178 | Style: Points 179 | Topic: /amcl3d_node/pointcloud_filtered 180 | Unreliable: false 181 | Use Fixed Frame: true 182 | Use rainbow: true 183 | Value: false 184 | - Alpha: 1 185 | Autocompute Intensity Bounds: true 186 | Autocompute Value Bounds: 187 | Max Value: 10 188 | Min Value: -10 189 | Value: true 190 | Axis: Z 191 | Channel Name: intensity 192 | Class: rviz/PointCloud2 193 | Color: 255; 255; 255 194 | Color Transformer: Intensity 195 | Decay Time: 0 196 | Enabled: false 197 | Invert Rainbow: false 198 | Max Color: 255; 255; 255 199 | Max Intensity: 4096 200 | Min Color: 0; 0; 0 201 | Min Intensity: 0 202 | Name: PointCloud2 203 | Position Transformer: XYZ 204 | Queue Size: 10 205 | Selectable: true 206 | Size (Pixels): 3 207 | Size (m): 0.00999999978 208 | Style: Flat Squares 209 | Topic: /amcl3d_node/pointcloud_passfiltered 210 | Unreliable: false 211 | Use Fixed Frame: true 212 | Use rainbow: true 213 | Value: false 214 | - Alpha: 0.699999988 215 | Class: rviz/Map 216 | Color Scheme: map 217 | Draw Behind: false 218 | Enabled: true 219 | Name: Map 220 | Topic: /map 221 | Unreliable: false 222 | Use Timestamp: false 223 | Value: true 224 | Enabled: true 225 | Global Options: 226 | Background Color: 48; 48; 48 227 | Default Light: true 228 | Fixed Frame: world 229 | Frame Rate: 30 230 | Name: root 231 | Tools: 232 | - Class: rviz/Interact 233 | Hide Inactive Objects: true 234 | - Class: rviz/MoveCamera 235 | - Class: rviz/Select 236 | - Class: rviz/FocusCamera 237 | - Class: rviz/Measure 238 | - Class: rviz/PublishPoint 239 | Single click: true 240 | Topic: /clicked_point 241 | - Class: rviz/SetInitialPose 242 | Topic: initialpose 243 | Value: true 244 | Views: 245 | Current: 246 | Class: rviz/Orbit 247 | Distance: 69.5626678 248 | Enable Stereo Rendering: 249 | Stereo Eye Separation: 0.0599999987 250 | Stereo Focal Distance: 1 251 | Swap Stereo Eyes: false 252 | Value: false 253 | Focal Point: 254 | X: 10.6786909 255 | Y: -1.90463603 256 | Z: 47.697937 257 | Focal Shape Fixed Size: true 258 | Focal Shape Size: 0.0500000007 259 | Invert Z Axis: false 260 | Name: Current View 261 | Near Clip Distance: 0.00999999978 262 | Pitch: 1.56979632 263 | Target Frame: 264 | Value: Orbit (rviz) 265 | Yaw: 4.86725426 266 | Saved: ~ 267 | Window Geometry: 268 | Displays: 269 | collapsed: false 270 | Height: 1056 271 | Hide Left Dock: false 272 | Hide Right Dock: true 273 | QMainWindow State: 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 274 | Selection: 275 | collapsed: false 276 | Time: 277 | collapsed: false 278 | Tool Properties: 279 | collapsed: false 280 | Views: 281 | collapsed: true 282 | Width: 1855 283 | X: 65 284 | Y: 24 285 | -------------------------------------------------------------------------------- /amcl3d/src/Grid3d.h: -------------------------------------------------------------------------------- 1 | /*! 2 | * @file Grid3d.h 3 | * @copyright Copyright (c) 2019, FADA-CATEC 4 | * 5 | * Licensed under the Apache License, Version 2.0 (the "License"); 6 | * you may not use this file except in compliance with the License. 7 | * You may obtain a copy of the License at 8 | * 9 | * http://www.apache.org/licenses/LICENSE-2.0 10 | * 11 | * Unless required by applicable law or agreed to in writing, software 12 | * distributed under the License is distributed on an "AS IS" BASIS, 13 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 14 | * See the License for the specific language governing permissions and 15 | * limitations under the License. 16 | */ 17 | 18 | #pragma once 19 | 20 | #include 21 | #include 22 | #include 23 | 24 | #include "PointCloudTools.h" 25 | 26 | #include 27 | #include 28 | 29 | namespace amcl3d 30 | { 31 | /*! \brief Class that contains the stages of the grid construction. 32 | */ 33 | class Grid3d 34 | { 35 | public: 36 | /*! \brief Grid3d class constructor. 37 | */ 38 | explicit Grid3d(VSCOMMON::LoggerPtr&); 39 | /*! \brief Grid3d class destructor. 40 | */ 41 | virtual ~Grid3d() 42 | { 43 | } 44 | 45 | /*! \brief To inicialite the grid map. 46 | * 47 | * \param map_path Route of the map environment. 48 | * \param sensor_dev Deviation of the point cloud sensor. 49 | * \return bool=False - If there are problems openning the octotree. 50 | * \return bool=True - If the grid map has been loaded and saved. 51 | * 52 | * It tries to load the octomap with the input route, showing that the loading was correct and the size that occupies 53 | * the octomap. If it can not load the octomap, it shuttles an error to indicate it and the program finish. But it 54 | * there is not any problem, the associated grid-map from the input route is looked up by loadGrid method. When it is 55 | * found, it is loaded. If there are not any grid-map associated, computeGrid method is called to 56 | * create it. Finally, the grid map is saved. 57 | */ 58 | bool open(const std::string& map_path, const double sensor_dev); 59 | 60 | 61 | /*! \brief To calculate the particle weight. 62 | * 63 | * \param cloud Point cloud message. 64 | * \param tx Particle x-axis position. 65 | * \param ty Particle y-axis position. 66 | * \param tz Particle z-axis position. 67 | * \param a Particle yaw orientation. 68 | * \return float - Particle weight. 69 | * 70 | * It checks possible errors with the information of the grid and point cloud of the environment map. It uses the 71 | * information of this enviroment map and the particle to create a loop that records the likeness between the sensor 72 | * point cloud and the map. Depending on the resemblance the particle acquires the weight. 73 | */ 74 | float computeCloudWeight(const pcl::PointCloud::Ptr& cloud, const float tx, const float ty, 75 | const float tz, const float roll, const float pitch, const float yaw) const; 76 | 77 | float computeCloudWeight(const pcl::PointCloud::Ptr& cloud, const float tx, const float ty, 78 | const float tz, const float roll, const float pitch, const float yaw, const int sampleNun) const; 79 | 80 | /*! \brief To check if the particle is into the map. 81 | * 82 | * \param x Particle x-axis position. 83 | * \param y Particle y-axis position. 84 | * \param z Particle z-axis position. 85 | * \return bool=False - If the particle is not into the map. 86 | * \return bool=True - If the particle is into the map. 87 | * 88 | * It looks up if there are problems with the information of the point cloud of the map to return false if this 89 | * happens. Then, it checks if the position of the particle is into the map using the extreme values of the octomap. 90 | */ 91 | bool isIntoMap(const float x, const float y, const float z) const; 92 | 93 | bool loadPCD(std::string file_path,pcl::PointCloud < pcl::PointXYZI>::Ptr& input); 94 | 95 | bool updateMap(const float x, const float y, const float z,const float radius = 2,const float voxel_size = 0.5); 96 | 97 | /*! \brief To create a vector that contains the grid data. 98 | * 99 | * \param x Point x-axis position. 100 | * \param y Point y-axis position. 101 | * \param z Point z-axis position. 102 | * \return uint32_t - Vector with the information of the grid map. 103 | * 104 | * It needs the point of point cloud to do the relation with the octomap and the grid. 105 | * It turns the matrix into a vector so you can move through it. It was used in the "Grid3d: buildGridSliceMsg" to 106 | * extract the maximum probability touring said vector. 107 | */ 108 | uint32_t point2grid(const float x, const float y, const float z) const; 109 | 110 | PointCloudInfo::Ptr pc_info_; /*!< 3D point cloud representation of the map. */ 111 | 112 | Grid3dInfo::Ptr grid_info_; /*!< 3D probabilistic grid cell */ 113 | 114 | std::vector::Ptr> corner_keyframes; 115 | std::vector::Ptr> surf_keyframes; 116 | std::vector::Ptr> outlier_keyframes; 117 | pcl::PointCloud::Ptr keyposes_3d; 118 | 119 | pcl::KdTreeFLANN::Ptr kd_map_ptr_; 120 | pcl::KdTreeFLANN::Ptr kd_pose_ptr_; 121 | private: 122 | /*! \brief To save the file of map like grid. 123 | * 124 | * \param grid_path Route where the grid map will be saved. 125 | * \return bool=False - If the map has not been save correctly. 126 | * \return bool=True - If the map has been save correctly. 127 | * 128 | * It checks if the grid information has error to return false. If there are not problems, it creates a file checking 129 | * the procees of creation. When the file is created, the information is added. 130 | */ 131 | bool saveGrid(const std::string& grid_path); 132 | 133 | /*! \brief To load the grid file. 134 | * 135 | * \param grid_path Route where the grid map will be saved. 136 | * \param sensor_dev Deviation of the point cloud sensor. 137 | * \return bool=False - If there are problems with the grid map information or writting the file. 138 | * \return bool=True - If the grid map file is saved without problems. 139 | * 140 | * It checks if there are problems reading the file and if the desviation of the sensor is diferent between the grid 141 | * and the variable that user set. Then, the method reads the grid cells and close the file. 142 | */ 143 | bool loadGrid(const std::string& grid_path, const double sensor_dev); 144 | 145 | 146 | VSCOMMON::LoggerPtr g_log; 147 | }; 148 | 149 | } // namespace amcl3d 150 | -------------------------------------------------------------------------------- /amcl3d/src/Node.h: -------------------------------------------------------------------------------- 1 | /*! 2 | * @file Node.h 3 | * @copyright Copyright (c) 2019, FADA-CATEC 4 | * 5 | * Licensed under the Apache License, Version 2.0 (the "License"); 6 | * you may not use this file except in compliance with the License. 7 | * You may obtain a copy of the License at 8 | * 9 | * http://www.apache.org/licenses/LICENSE-2.0 10 | * 11 | * Unless required by applicable law or agreed to in writing, software 12 | * distributed under the License is distributed on an "AS IS" BASIS, 13 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 14 | * See the License for the specific language governing permissions and 15 | * limitations under the License. 16 | */ 17 | 18 | #pragma once 19 | 20 | #include "Parameters.h" 21 | #include "amcl3d.h" //! Include Grid.hpp 22 | 23 | #include 24 | #include 25 | #include 26 | #include 27 | #include 28 | #include 29 | #include 30 | #include 31 | #include 32 | 33 | #include 34 | #include 35 | #include 36 | #include 37 | 38 | #include 39 | #include 40 | 41 | #define TOOL_VERSION ("amcl3d") 42 | /*! \brief Namespace of the algorithm. 43 | */ 44 | namespace amcl3d 45 | { 46 | /*! \brief Class that contains all the intances of the rest of the classes. 47 | * 48 | * It is the main node of the algorithm that is responsible for its operation. 49 | */ 50 | class Node 51 | { 52 | public: 53 | /*! \brief Node class constructor. 54 | */ 55 | explicit Node(const std::string& str = "./"); 56 | /*! \brief Node class destructor. 57 | */ 58 | virtual ~Node(); 59 | 60 | /*! \brief To run the grid3d methods and subscribers/publishers. 61 | * 62 | * It runs the Grid3d::open, returning if this process has been successful. In adicction, it runs 63 | * Grid3d::buildGridSliceMsg and Grid3d::buildMapPointCloudMsg. Apart from what is related to Dgrid3d, it is 64 | * responsible for starting the subscribers and publishers, to get the information of odometry, 3d point cloud sensor 65 | * and radio-range sensor and to send the information of the particles, the representation of the range of radio-range 66 | * sensor, the algorithm result (tranformation) and the pointcloud filtered for the algorithm. 67 | */ 68 | void spin(); 69 | 70 | void readParamFromXML(); 71 | 72 | 73 | private: 74 | /*! \brief To publish the environment map point cloud. 75 | * 76 | * It is the method responsible for publishing the point cloud when the timer provides the event. The timer is set by 77 | * the user. 78 | */ 79 | void publishMapPointCloud(const ros::TimerEvent&); 80 | void publishMapPointCloud(); 81 | 82 | /*! \brief To publish the grid slice. 83 | * 84 | * It is the method responsible for publishing the grid slice when the timer provides the event. The timer is set by 85 | * the user. 86 | */ 87 | void publishGridSlice(const ros::TimerEvent&); 88 | 89 | /*! \brief To publish the grid slice. 90 | * 91 | * It uses the methods of the ParticleFilter class, ParticleFilter::isInitialized and 92 | * ParticleFilter::buildParticlesPoseMsg, to publish the position of the particles. 93 | */ 94 | void publishParticles(); 95 | 96 | 97 | /*! \brief To process the point cloud that arrives from the UAV view. 98 | * 99 | * \param msg Point cloud from the UAV view. 100 | * 101 | * It makes a prediction based on odometry and updates the particles according to the previous prediction, the 102 | * cloud message of entry points and the range measurement of radio-range sensor. Finally, it obtains the average of 103 | * the particles as a result and performs the updates of the transformations and resampling of particles. To do this, 104 | * it uses the methods of the ParticleFilter class, ParticleFilter::predict, ParticleFilter::update, class 105 | * ParticleFilter::getMean and ParticleFilter::resample. 106 | */ 107 | void pointcloudCallback(const sensor_msgs::PointCloud2ConstPtr& msg); 108 | 109 | /*! \brief To process the odometry. 110 | * 111 | * \param msg Odometry message. 112 | * 113 | * It converts odometry into a transformed form, to work with it. It checks if the filter has been initialized with 114 | * the initial position that is provided by the user. Subsequently, it updates the odometry transform with the 115 | * orientation that arrives through the message and sends it establishing a relationship between the position where 116 | * the UAV starts and what moves. It is also responsible for initializing the odometry and checking if it has taken 117 | * off to update the transforms based on this takeoff and if there is any jump in the algorithm. 118 | */ 119 | //void odomCallback(const geometry_msgs::TransformStampedConstPtr& msg); 120 | void odomCallback(const nav_msgs::OdometryConstPtr& msg); 121 | 122 | void initialPoseCallback(const geometry_msgs::PoseWithCovarianceStampedConstPtr& msg); 123 | 124 | /*! \brief To check motion and time thresholds for AMCL update. 125 | * 126 | * \return bool=False - If there are problems with the checks. 127 | * \return bool=True - If the checks has been correct. 128 | * 129 | * The algorithm has to be updated in a shorter time than a timer indicates. If something happens that makes this 130 | * impossible, it goes back to the previous odometry. The same happens with the estimated translation and rotation of 131 | * the UAV. It has delimited up to how much distance and rotation can change at most in the updates. This node is 132 | * responsible for performing these checks. 133 | */ 134 | bool checkUpdateThresholds(); 135 | 136 | /*! \brief To initialize the algorithm. 137 | * 138 | * \param init_pose Range message of radio-range sensors. 139 | * \param x_dev Thresholds of x-axis in initial pose. 140 | * \param y_dev Thresholds of y-axis in initial pose. 141 | * \param z_dev Thresholds of z-axis in initial pose. 142 | * \param a_dev Thresholds of yaw in initial pose. 143 | * 144 | * Starting from the starting pose of the UAV provided by the user. It is in charge of initializing the particles 145 | * taking into account the given pose and the marked deviations, to later publish them. In this way, the particles are 146 | * initialized around the initial pose, the threshold is marked by deviations. 147 | */ 148 | void setInitialPose(const Eigen::Affine3f& init_pose); 149 | 150 | Eigen::Vector3f quaternion2eulerAngle(float x,float y,float z,float w); 151 | 152 | Eigen::Quaternionf eulerAngle2quaternion(float roll,float pitch,float yaw); 153 | 154 | tf::Transform getTransformFromAffine3f(const Eigen::Affine3f& tf_pose); 155 | 156 | 157 | /*! \brief To create the message of the grid slice. 158 | * 159 | * \param z Build height. 160 | * \param msg Occupancy grid message. 161 | * \return bool=False - If there are problems with the point cloud map information, grid map information nor 162 | * the 163 | * selected height. 164 | * \return bool=True - If the construction has been done without problem. 165 | * 166 | * It checks variables that shows information of the point cloud and information of the grid, to realize if there are 167 | * errors that can oblige to end the algorithm. Besides, it also checks if the height selected like input comply 168 | * with the size of the octomap. Subsequently, it extracts the probability of each grid point to find the maximum 169 | * probability and be able to rescale it in the occupancy message. 170 | */ 171 | //bool buildGridSliceMsg(const double z, nav_msgs::OccupancyGrid& msg) const; 172 | 173 | /*! \brief To transform the information of point cloud into the message of ROS. 174 | * 175 | * \param msg Point cloud message. 176 | * \return bool=False - If there are problems with the point cloud map information. 177 | * \return bool=True - If the construction has been done without problem. 178 | * 179 | * It uses the pcl library to do the conversion but first, it checks for problems with the point cloud information. 180 | * The output is true if the conversion has been done, or false if there are errors with the information. 181 | */ 182 | bool buildMapPointCloudMsg(sensor_msgs::PointCloud2& msg) const; 183 | 184 | 185 | std::string WORKING_DIR = "./"; 186 | std::string bin_dir = WORKING_DIR; 187 | std::string path_param = WORKING_DIR + "params/config/amcl3d_param.xml"; 188 | VSCOMMON::LoggerPtr g_log; 189 | 190 | 191 | Parameters parameters_; /*!< Instance of the Parameters class */ 192 | LocalizationParam amcl_params_; 193 | MonteCarloLocalization* mcl_; /*!< Instance of the ParticleFilter class */ 194 | 195 | ros::NodeHandle nh_; /*!< ROS Node Handle */ 196 | 197 | sensor_msgs::PointCloud2 map_point_cloud_msg_; /*!< Map point cloud message */ 198 | ros::Publisher map_point_cloud_pub_; /*!< Publisher of map point cloud message */ 199 | ros::Timer map_point_cloud_pub_timer_; /*!< Timer for publish map point cloud message */ 200 | 201 | nav_msgs::OccupancyGrid grid_slice_msg_; 202 | ros::Publisher grid_slice_pub_; /*!< Publisher of map grid slice message */ 203 | ros::Timer grid_slice_pub_timer_; /*!< Timer for publish map grid slice message */ 204 | 205 | bool is_odom_arrive_{ false }; /*!< Flag to know the initialize of odometry */ 206 | bool amcl_out_{ false }; /*!< Flag to know jumps in algorithm */ 207 | double roll_{ 0 }; /*!< Roll angle */ 208 | double pitch_{ 0 }; /*!< Pitch angle */ 209 | 210 | Particle mean_p_; /*!< Instance of the Particle struct for particles of filter */ 211 | Particle lastmean_p_; /*!< Instance of the Particle struct for previous update particles of filter */ 212 | 213 | ros::Subscriber point_sub_; /*!< UAV point cloud subscriber */ 214 | ros::Subscriber odom_sub_; /*!< Odometry subscriber */ 215 | ros::Subscriber initialPose_sub_; 216 | 217 | ros::Publisher particles_pose_pub_; /*!< Particles publisher */ 218 | ros::Publisher cloud_filter_pub_; /*!< Filtered point cloud publisher */ 219 | 220 | 221 | Eigen::Affine3f lastbase_2_world_eigen_; /*!< Base-world last transformation */ 222 | Eigen::Affine3f initodom_2_world_eigen_; /*!< Odom-world init transformation */ 223 | Eigen::Affine3f lastodom_2_world_eigen_; /*!< Odom-world last transformation */ 224 | Eigen::Affine3f amcl_out_lastbase_2_odom_eigen_; /*!< Base-odom Transformation for the appearance of jumps */ 225 | Eigen::Affine3f lastupdatebase_2_odom_eigen_; /*!< Base-odom last update transformation */ 226 | Eigen::Affine3f base_2_odom_eigen_; /*!< Base-odom Transformation */ 227 | Eigen::Affine3f odom_increment_eigen_; /*!< Odom increase transformation */ 228 | 229 | std::shared_ptr tf_broadcaster; 230 | 231 | ros::Time nextupdate_time_; /*!< Timer for the next update */ 232 | }; 233 | 234 | } // namespace amcl3d 235 | -------------------------------------------------------------------------------- /amcl3d/src/Parameters.cpp: -------------------------------------------------------------------------------- 1 | /*! 2 | * @file Parameters.cpp 3 | * @copyright Copyright (c) 2019, FADA-CATEC 4 | * 5 | * Licensed under the Apache License, Version 2.0 (the "License"); 6 | * you may not use this file except in compliance with the License. 7 | * You may obtain a copy of the License at 8 | * 9 | * http://www.apache.org/licenses/LICENSE-2.0 10 | * 11 | * Unless required by applicable law or agreed to in writing, software 12 | * distributed under the License is distributed on an "AS IS" BASIS, 13 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 14 | * See the License for the specific language governing permissions and 15 | * limitations under the License. 16 | */ 17 | 18 | #include "Parameters.h" 19 | 20 | #include 21 | 22 | namespace amcl3d 23 | { 24 | Parameters::Parameters() 25 | { 26 | if (!ros::param::get("~base_frame_id", base_frame_id_)) 27 | { 28 | exitWithParameterError("base_frame_id"); 29 | } 30 | 31 | if (!ros::param::get("~odom_frame_id", odom_frame_id_)) 32 | { 33 | exitWithParameterError("odom_frame_id"); 34 | } 35 | 36 | if (!ros::param::get("~global_frame_id", global_frame_id_)) 37 | { 38 | exitWithParameterError("global_frame_id"); 39 | } 40 | 41 | if (!ros::param::get("~map_path", map_path_)) 42 | { 43 | exitWithParameterError("map_path"); 44 | } 45 | 46 | if (!ros::param::get("~set_initial_pose", set_initial_pose_)) 47 | { 48 | exitWithParameterError("set_initial_pose"); 49 | } 50 | 51 | if (!ros::param::get("~init_x", init_x_)) 52 | { 53 | exitWithParameterError("init_x"); 54 | } 55 | 56 | if (!ros::param::get("~init_y", init_y_)) 57 | { 58 | exitWithParameterError("init_y"); 59 | } 60 | 61 | if (!ros::param::get("~init_z", init_z_)) 62 | { 63 | exitWithParameterError("init_z"); 64 | } 65 | 66 | if (!ros::param::get("~init_a", init_a_)) 67 | { 68 | exitWithParameterError("init_a"); 69 | } 70 | 71 | if (!ros::param::get("~init_x_dev", init_x_dev_)) 72 | { 73 | exitWithParameterError("init_x_dev"); 74 | } 75 | 76 | if (!ros::param::get("~init_y_dev", init_y_dev_)) 77 | { 78 | exitWithParameterError("init_y_dev"); 79 | } 80 | 81 | if (!ros::param::get("~init_z_dev", init_z_dev_)) 82 | { 83 | exitWithParameterError("init_z_dev"); 84 | } 85 | 86 | if (!ros::param::get("~init_a_dev", init_a_dev_)) 87 | { 88 | exitWithParameterError("init_a_dev"); 89 | } 90 | 91 | if (!ros::param::get("~publish_point_cloud_rate", publish_point_cloud_rate_)) 92 | { 93 | exitWithParameterError("publish_point_cloud_rate"); 94 | } 95 | 96 | if (!ros::param::get("~grid_slice_z", grid_slice_z_)) 97 | { 98 | exitWithParameterError("grid_slice_z"); 99 | } 100 | 101 | if (!ros::param::get("~publish_grid_slice_rate", publish_grid_slice_rate_)) 102 | { 103 | exitWithParameterError("publish_grid_slice_rate"); 104 | } 105 | 106 | if (!ros::param::get("~sensor_dev", sensor_dev_)) 107 | { 108 | exitWithParameterError("sensor_dev"); 109 | } 110 | 111 | if (!ros::param::get("~sensor_range", sensor_range_)) 112 | { 113 | exitWithParameterError("sensor_range"); 114 | } 115 | 116 | if (!ros::param::get("~voxel_size", voxel_size_)) 117 | { 118 | exitWithParameterError("voxel_size"); 119 | } 120 | 121 | if (!ros::param::get("~num_particles", num_particles_)) 122 | { 123 | exitWithParameterError("num_particles"); 124 | } 125 | 126 | if (!ros::param::get("~odom_x_mod", odom_x_mod_)) 127 | { 128 | exitWithParameterError("odom_x_mod"); 129 | } 130 | 131 | if (!ros::param::get("~odom_y_mod", odom_y_mod_)) 132 | { 133 | exitWithParameterError("odom_y_mod"); 134 | } 135 | 136 | if (!ros::param::get("~odom_z_mod", odom_z_mod_)) 137 | { 138 | exitWithParameterError("odom_z_mod"); 139 | } 140 | 141 | if (!ros::param::get("~odom_a_mod", odom_a_mod_)) 142 | { 143 | exitWithParameterError("odom_a_mod"); 144 | } 145 | 146 | if (!ros::param::get("~resample_interval", resample_interval_)) 147 | { 148 | exitWithParameterError("resample_interval"); 149 | } 150 | 151 | if (!ros::param::get("~update_rate", update_rate_)) 152 | { 153 | exitWithParameterError("update_rate"); 154 | } 155 | 156 | if (!ros::param::get("~d_th", d_th_)) 157 | { 158 | exitWithParameterError("d_th"); 159 | } 160 | 161 | if (!ros::param::get("~a_th", a_th_)) 162 | { 163 | exitWithParameterError("a_th"); 164 | } 165 | 166 | if (!ros::param::get("~take_off_height", take_off_height_)) 167 | { 168 | exitWithParameterError("take_off_height"); 169 | } 170 | 171 | if (!ros::param::get("~alpha", alpha_)) 172 | { 173 | exitWithParameterError("alpha"); 174 | } 175 | 176 | ROS_INFO("[%s]" 177 | "\n Parameters:" 178 | "\n base_frame_id=%s" 179 | "\n odom_frame_id=%s" 180 | "\n global_frame_id=%s" 181 | "\n map_path=%s" 182 | "\n set_initial_pose=%d" 183 | "\n init_x=%lf" 184 | "\n init_y=%lf" 185 | "\n init_z=%lf" 186 | "\n init_a=%lf" 187 | "\n init_x_dev=%lf" 188 | "\n init_y_dev=%lf" 189 | "\n init_z_dev=%lf" 190 | "\n init_a_dev=%lf" 191 | "\n publish_point_cloud_rate=%lf" 192 | "\n grid_slice_z=%f" 193 | "\n publish_grid_slice_rate=%lf" 194 | "\n sensor_dev=%lf" 195 | "\n sensor_range=%lf" 196 | "\n voxel_size=%lf" 197 | "\n num_particles=%d" 198 | "\n odom_x_mod=%lf" 199 | "\n odom_y_mod=%lf" 200 | "\n odom_z_mod=%lf" 201 | "\n odom_a_mod=%lf" 202 | "\n resample_interval=%d" 203 | "\n update_rate=%lf" 204 | "\n d_th=%lf" 205 | "\n a_th=%lf" 206 | "\n take_off_height=%lf" 207 | "\n alpha=%lf", 208 | ros::this_node::getName().data(), base_frame_id_.c_str(), odom_frame_id_.c_str(), global_frame_id_.c_str(), 209 | map_path_.c_str(), (int)set_initial_pose_, init_x_, init_y_, init_z_, init_a_, init_x_dev_, init_y_dev_, 210 | init_z_dev_, init_a_dev_, publish_point_cloud_rate_, grid_slice_z_, publish_grid_slice_rate_, sensor_dev_, 211 | sensor_range_, voxel_size_, num_particles_, odom_x_mod_, odom_y_mod_, odom_z_mod_, odom_a_mod_, 212 | resample_interval_, update_rate_, d_th_, a_th_, take_off_height_, alpha_); 213 | } 214 | 215 | void Parameters::exitWithParameterError(const char* parameter_str) 216 | { 217 | ROS_ERROR("[%s] `%s` parameter not set!", ros::this_node::getName().data(), parameter_str); 218 | exit(EXIT_FAILURE); 219 | } 220 | 221 | } // namespace amcl3d 222 | -------------------------------------------------------------------------------- /amcl3d/src/Parameters.h: -------------------------------------------------------------------------------- 1 | /*! 2 | * @file Parameters.h 3 | * @copyright Copyright (c) 2019, FADA-CATEC 4 | * 5 | * Licensed under the Apache License, Version 2.0 (the "License"); 6 | * you may not use this file except in compliance with the License. 7 | * You may obtain a copy of the License at 8 | * 9 | * http://www.apache.org/licenses/LICENSE-2.0 10 | * 11 | * Unless required by applicable law or agreed to in writing, software 12 | * distributed under the License is distributed on an "AS IS" BASIS, 13 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 14 | * See the License for the specific language governing permissions and 15 | * limitations under the License. 16 | */ 17 | 18 | #pragma once 19 | 20 | #include 21 | 22 | namespace amcl3d 23 | { 24 | /*! \brief Class that contains the global parameters of the algorithm. 25 | */ 26 | struct Parameters 27 | { 28 | public: 29 | /*! \brief Parameters class constructor. 30 | */ 31 | //explicit Parameters(); 32 | 33 | std::string base_frame_id_; /*!< Name of the robot TF */ 34 | std::string odom_frame_id_; /*!< Name of the flight origin of the robot TF */ 35 | std::string global_frame_id_; /*!< Name of the test-bed origin TF */ 36 | std::string map_path_; /*!< Route to the localization of the environment map */ 37 | 38 | bool set_initial_pose_; /*!< Flag to indicate if t he initial pose has been received */ 39 | 40 | double init_x_; /*!< Start x-axis position */ 41 | double init_y_; /*!< Start y-axis position */ 42 | double init_z_; /*!< Start z-axis position */ 43 | double init_roll_; 44 | double init_pitch_; 45 | double init_yaw_; /*!< Start yaw angle */ 46 | 47 | 48 | 49 | double grid_slice_z_; /*!< Height of grid slice */ 50 | double publish_point_cloud_rate_; /*!< Map point cloud publishing rate */ 51 | double publish_grid_slice_rate_; /*!< map grid slice publishing rate */ 52 | 53 | double sensor_dev_; /*!< Desviation of 3D point cloud sensor */ 54 | double voxel_size_; /*!< Size of voxel grid filter */ 55 | 56 | int num_particles_; /*!< Particle number in the filter */ 57 | 58 | int resample_interval_; /*!< Resampling control */ 59 | 60 | double update_rate_; /*!< Filter updating frequency */ 61 | double d_th_; /*!< Threshold in the distance for the update */ 62 | double a_th_; /*!< Threshold in yaw angle for the update */ 63 | 64 | private: 65 | /*! \brief To check parameter errors. 66 | * 67 | * \param parameter_str Name of parameter. 68 | * 69 | * It give a error if the input parameter was not set. 70 | */ 71 | void exitWithParameterError(const char* parameter_str); 72 | }; 73 | 74 | } // namespace amcl3d 75 | -------------------------------------------------------------------------------- /amcl3d/src/ParticleFilter.cpp: -------------------------------------------------------------------------------- 1 | /*! 2 | * @file ParticleFilter.cpp 3 | * @copyright Copyright (c) 2019, FADA-CATEC 4 | * 5 | * Licensed under the Apache License, Version 2.0 (the "License"); 6 | * you may not use this file except in compliance with the License. 7 | * You may obtain a copy of the License at 8 | * 9 | * http://www.apache.org/licenses/LICENSE-2.0 10 | * 11 | * Unless required by applicable law or agreed to in writing, software 12 | * distributed under the License is distributed on an "AS IS" BASIS, 13 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 14 | * See the License for the specific language governing permissions and 15 | * limitations under the License. 16 | */ 17 | 18 | #include "ParticleFilter.h" 19 | 20 | namespace amcl3d 21 | { 22 | ParticleFilter::ParticleFilter(VSCOMMON::LoggerPtr& t_log) 23 | : generator_(rd_()),g_log(t_log),grid3d_(new Grid3d(t_log)) 24 | { 25 | } 26 | 27 | ParticleFilter::~ParticleFilter() 28 | { 29 | } 30 | 31 | void ParticleFilter::relocPose(const int num_particles, const float x_init, const float y_init, const float z_init, 32 | const float roll_init, const float pitch_init, const float yaw_init, 33 | const float x_dev, const float y_dev, const float z_dev, 34 | const float roll_dev, const float pitch_dev, const float yaw_dev) 35 | { 36 | /* Resize particle set */ 37 | p_.resize(abs(num_particles)); 38 | 39 | /* Sample the given pose */ 40 | const float dev = std::max(std::max(x_dev, y_dev), z_dev); 41 | const float gauss_const_1 = 1. / (dev * sqrt(2 * M_PI)); 42 | const float gauss_const_2 = 1. / (2 * dev * dev); 43 | 44 | p_[0].x = x_init; 45 | p_[0].y = y_init; 46 | p_[0].z = z_init; 47 | p_[0].roll = roll_init; 48 | p_[0].pitch = pitch_init; 49 | p_[0].yaw = yaw_init; 50 | p_[0].w = gauss_const_1; 51 | 52 | float wt = p_[0].w; 53 | float dist; 54 | 55 | for (uint32_t i = 1; i < p_.size(); ++i) 56 | { 57 | p_[i].x = p_[0].x + ranGaussian(0, x_dev); 58 | p_[i].y = p_[0].y + ranGaussian(0, y_dev); 59 | p_[i].z = p_[0].z + ranGaussian(0, z_dev); 60 | p_[i].roll = p_[0].roll + ranGaussian(0, roll_dev); 61 | p_[i].pitch = p_[0].pitch + ranGaussian(0, pitch_dev); 62 | p_[i].yaw = p_[0].yaw + ranGaussian(0, yaw_dev); 63 | 64 | dist = sqrt((p_[i].x - p_[0].x) * (p_[i].x - p_[0].x) + (p_[i].y - p_[0].y) * (p_[i].y - p_[0].y) + 65 | (p_[i].z - p_[0].z) * (p_[i].z - p_[0].z)); 66 | 67 | p_[i].w = gauss_const_1 * exp(-dist * dist * gauss_const_2); 68 | 69 | wt += p_[i].w; 70 | } 71 | 72 | Particle mean_p; 73 | for (uint32_t i = 0; i < p_.size(); ++i) 74 | { 75 | p_[i].w /= wt; 76 | 77 | mean_p.x += p_[i].w * p_[i].x; 78 | mean_p.y += p_[i].w * p_[i].y; 79 | mean_p.z += p_[i].w * p_[i].z; 80 | mean_p.roll += p_[i].w * p_[i].roll; 81 | mean_p.pitch += p_[i].w * p_[i].pitch; 82 | mean_p.yaw += p_[i].w * p_[i].yaw; 83 | } 84 | mean_ = mean_p; 85 | 86 | initialized_ = true; 87 | using namespace VSCOMMON; 88 | LOG_COUT_INFO(g_log,"mcl initialized."); 89 | } 90 | 91 | void ParticleFilter::predict(const double delta_x, const double delta_y, const double delta_z, 92 | const double delta_roll,const double delta_pitch,const double delta_yaw, 93 | const double odom_x_noise, const double odom_y_noise, const double odom_z_noise, 94 | const double odom_roll_noise, const double odom_pitch_noise,const double odom_yaw_noise) 95 | { 96 | /* Make a prediction for all particles according to the odometry */ 97 | float sa, ca, rand_x, rand_y; 98 | for (uint32_t i = 0; i < p_.size(); ++i) 99 | { 100 | // TODO: 角度变换，roll pitch对坐标的影响 101 | sa = sin(p_[i].yaw); 102 | ca = cos(p_[i].yaw); 103 | rand_x = delta_x + ranGaussian(0, odom_x_noise); 104 | rand_y = delta_y + ranGaussian(0, odom_y_noise); 105 | p_[i].x += ca * rand_x - sa * rand_y; 106 | p_[i].y += sa * rand_x + ca * rand_y; 107 | p_[i].z += delta_z + ranGaussian(0, odom_z_noise); 108 | p_[i].roll += delta_roll + ranGaussian(0,odom_roll_noise); 109 | p_[i].pitch += delta_pitch + ranGaussian(0,odom_pitch_noise); 110 | p_[i].yaw += delta_yaw + ranGaussian(0, odom_yaw_noise); 111 | } 112 | } 113 | 114 | void ParticleFilter::update(const pcl::PointCloud::Ptr& cloud) 115 | { 116 | /* Incorporate measurements */ 117 | VSCOMMON::tic("Update"); 118 | for (uint32_t i = 0; i < p_.size(); ++i) 119 | { 120 | /* Get particle information */ 121 | float tx = p_[i].x; 122 | float ty = p_[i].y; 123 | float tz = p_[i].z; 124 | 125 | /* Check the particle is into the map */ 126 | if (!grid3d_->isIntoMap(tx, ty, tz)) 127 | { 128 | // std::cout << "Not into map: " << grid3d_.isIntoMap(tx, ty, tz-1.0) << std::endl; 129 | p_[i].w = 0; 130 | continue; 131 | } 132 | 133 | // Evaluate the weight of the point cloud 134 | // TODO: ground and non ground points compute weight respectively 135 | 136 | p_[i].w = grid3d_->computeCloudWeight(cloud, tx, ty, tz, p_[i].roll, p_[i].pitch, p_[i].yaw); 137 | } 138 | LOG_INFO(g_log,"Update time:"<::Ptr& cloud,const int& sampleNum) 142 | { 143 | /* Incorporate measurements */ 144 | VSCOMMON::tic("Update"); 145 | for (uint32_t i = 0; i < p_.size(); ++i) 146 | { 147 | /* Get particle information */ 148 | float tx = p_[i].x; 149 | float ty = p_[i].y; 150 | float tz = p_[i].z; 151 | 152 | // Evaluate the weight of the point cloud 153 | // TODO: ground and non ground points compute weight respectively 154 | 155 | p_[i].w = grid3d_->computeCloudWeight(cloud, tx, ty, tz, p_[i].roll, p_[i].pitch, p_[i].yaw, sampleNum); 156 | } 157 | double max_log_likelihood = p_[0].w; 158 | for(uint32_t i = 1;i < p_.size();++i) 159 | if(p_[i].w > max_log_likelihood) 160 | max_log_likelihood = p_[i].w; 161 | for (uint32_t i = 0; i < p_.size(); ++i) 162 | { 163 | p_[i].w = std::exp(p_[i].w - max_log_likelihood); 164 | } 165 | LOG_INFO(g_log,"Update time:"< 0) 175 | wtp += p_[i].w; 176 | if(p_[i].w > w_max) 177 | w_max = p_[i].w; 178 | } 179 | float wt = 0; 180 | for (uint32_t i = 0; i < p_.size(); ++i) 181 | { 182 | if (wtp > 0) 183 | p_[i].w /= wtp; 184 | else 185 | p_[i].w = 0; 186 | 187 | if (!grid3d_->isIntoMap(p_[i].x, p_[i].y, p_[i].z)) 188 | { 189 | /* std::cout << "Not into map: " << grid3d_.isIntoMap(tx, ty, tz-1.0) << std::endl; */ 190 | p_[i].w = 0; 191 | } 192 | else 193 | p_[i].w = p_[i].w; 194 | wt += p_[i].w; 195 | } 196 | } 197 | 198 | Particle ParticleFilter::getMean() 199 | { 200 | Particle mean_p; 201 | float w_max = 0; 202 | for (uint32_t i = 0; i < p_.size(); ++i) 203 | { 204 | mean_p.x += p_[i].w * p_[i].x; 205 | mean_p.y += p_[i].w * p_[i].y; 206 | mean_p.z += p_[i].w * p_[i].z; 207 | mean_p.roll += p_[i].w * p_[i].roll; 208 | mean_p.pitch += p_[i].w * p_[i].pitch; 209 | mean_p.yaw += p_[i].w * p_[i].yaw; 210 | if(p_[i].w > w_max) 211 | w_max = p_[i].w; 212 | } 213 | mean_p.w = w_max; // out put max pf weight 214 | mean_ = mean_p; 215 | return mean_p; 216 | } 217 | 218 | Particle ParticleFilter::getBestParticle() 219 | { 220 | Particle best_p; 221 | best_p.w = 0; 222 | for (uint32_t i = 0; i < p_.size(); ++i) 223 | { 224 | if(p_[i].w > best_p.w) 225 | best_p = p_[i]; 226 | } 227 | return best_p; 228 | } 229 | 230 | void ParticleFilter::resample() 231 | { 232 | std::vector new_p(p_.size()); 233 | const float factor = 1.f / p_.size(); 234 | const float r = factor * rngUniform(0, 1); 235 | float c = p_[0].w; 236 | float u; 237 | 238 | //! Do resamplig 239 | for (uint32_t m = 0, i = 0; m < p_.size(); ++m) 240 | { 241 | u = r + factor * m; 242 | while (u > c) 243 | { 244 | if (++i >= p_.size()) 245 | break; 246 | c += p_[i].w; 247 | } 248 | new_p[m] = p_[i]; 249 | new_p[m].w = factor; 250 | } 251 | 252 | //! Asign the new particles set 253 | p_ = new_p; 254 | } 255 | 256 | void ParticleFilter::uniformSample(int& sample_num) 257 | { 258 | particleNormalize(); 259 | std::vector left_sum(p_.size()); 260 | left_sum[0] = p_[0].w; 261 | for(int i=1;i new_p(sample_num); 267 | const float inteval = 1.0f / sample_num; 268 | float samp_value = 0.5f / sample_num; 269 | int idx=0; 270 | for (int i = 0; i samp_value) { 272 | new_p[idx] = p_[i]; 273 | new_p[idx].w = inteval; 274 | idx++; 275 | samp_value += inteval; 276 | } 277 | } 278 | p_ = new_p; 279 | // std::cout<<"sample_num: "<< sample_num<<" --> "<< p_.size()< distribution(mean, sigma); 285 | return distribution(generator_); 286 | } 287 | 288 | float ParticleFilter::rngUniform(const float range_from, const float range_to) 289 | { 290 | std::uniform_real_distribution distribution(range_from, range_to); 291 | return distribution(generator_); 292 | } 293 | 294 | void ParticleFilter::setParticleNum(int max_sample ,int min_sample) 295 | { 296 | max_resamp_num_ = max_sample; 297 | min_resamp_num_ = min_sample; 298 | } 299 | 300 | } // namespace amcl3d 301 | -------------------------------------------------------------------------------- /amcl3d/src/ParticleFilter.h: -------------------------------------------------------------------------------- 1 | /*! 2 | * @file ParticleFilter.h 3 | * @copyright Copyright (c) 2019, FADA-CATEC 4 | * 5 | * Licensed under the Apache License, Version 2.0 (the "License"); 6 | * you may not use this file except in compliance with the License. 7 | * You may obtain a copy of the License at 8 | * 9 | * http://www.apache.org/licenses/LICENSE-2.0 10 | * 11 | * Unless required by applicable law or agreed to in writing, software 12 | * distributed under the License is distributed on an "AS IS" BASIS, 13 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 14 | * See the License for the specific language governing permissions and 15 | * limitations under the License. 16 | */ 17 | 18 | #pragma once 19 | 20 | #include "Grid3d.h" 21 | #include "Parameters.h" 22 | 23 | #include 24 | 25 | #include 26 | #include 27 | 28 | namespace amcl3d 29 | { 30 | /*! \brief Struct that contains the data concerning one particle. 31 | */ 32 | struct Particle 33 | { 34 | float x; /*!< Position x */ 35 | float y; /*!< Position y */ 36 | float z; /*!< Position z */ 37 | float roll; 38 | float pitch; 39 | float yaw; /*!< Yaw angle */ 40 | 41 | float w; /*!< Total weight */ 42 | 43 | Particle() : x(0), y(0), z(0),roll(0),pitch(0), yaw(0), w(0) 44 | { 45 | } 46 | Particle(float _x,float _y,float _z,float _roll,float _pitch,float _yaw) 47 | :x(_x),y(_y),z(_z),roll(_roll),pitch(_pitch),yaw(_yaw),w(-1){} 48 | }; 49 | 50 | struct LocalizationParam{ 51 | LocalizationParam() 52 | :min_particle_num_local(50) 53 | ,min_particle_num_global(150) 54 | ,max_particle_num_local(200) 55 | ,max_particle_num_global(600) 56 | ,init_x_dev_(0.5) 57 | ,init_y_dev_(0.5) 58 | ,init_z_dev_(0.4) 59 | ,init_roll_dev_(0.2) 60 | ,init_pitch_dev_(0,2) 61 | ,init_yaw_dev_(0.4) 62 | ,odom_x_mod_(0.4) 63 | ,odom_y_mod_(0.4) 64 | ,odom_z_mod_(0.05) 65 | ,odom_roll_mod_(0.05) 66 | ,odom_pitch_mod_(0.05) 67 | ,odom_yaw_mod_(0.2) 68 | ,min_xy_noise_(0.1) 69 | ,min_z_noise_(0.05) 70 | ,min_rp_noise_(0.05) 71 | ,min_yaw_noise_(0.05) 72 | ,weight_global_(4.0) 73 | {} 74 | 75 | int min_particle_num_global; 76 | int min_particle_num_local; 77 | int max_particle_num_global; 78 | int max_particle_num_local; 79 | double init_x_dev_; /*!< Thresholds x-axis position in initialization*/ 80 | double init_y_dev_; /*!< Thresholds y-axis position in initialization*/ 81 | double init_z_dev_; /*!< Thresholds z-axis position in initialization*/ 82 | double init_roll_dev_; 83 | double init_pitch_dev_; 84 | double init_yaw_dev_; /*!< Thresholds yaw angle in initialization*/ 85 | 86 | double odom_x_mod_; /*!< Thresholds x-axis position in the prediction */ 87 | double odom_y_mod_; /*!< Thresholds y-axis position in the prediction */ 88 | double odom_z_mod_; /*!< Thresholds z-axis position in the prediction */ 89 | double odom_roll_mod_; 90 | double odom_pitch_mod_; 91 | double odom_yaw_mod_; /*!< Thresholds yaw angle in the prediction */ 92 | 93 | double min_xy_noise_; 94 | double min_z_noise_; 95 | double min_rp_noise_; 96 | double min_yaw_noise_; 97 | 98 | double weight_global_; 99 | }; 100 | 101 | 102 | /*! \brief Class that contains the stages of the Particle Filter. 103 | */ 104 | class ParticleFilter 105 | { 106 | public: 107 | /*! \brief ParticleFilter class constructor. 108 | */ 109 | explicit ParticleFilter(VSCOMMON::LoggerPtr&); 110 | /*! \brief ParticleFilter class destructor. 111 | */ 112 | virtual ~ParticleFilter(); 113 | 114 | /*! \brief To inicialite the grid map. 115 | * 116 | * \return bool=False - If it has not been initialized. 117 | * \return bool=True - If it has been initialized. 118 | * 119 | * It only return the variable initialized_, and this is modified in the code when the filter does the 120 | * ParticleFilter::init method. 121 | */ 122 | bool isInitialized() const 123 | { 124 | return initialized_; 125 | } 126 | 127 | /*! \brief To get the information from the Particle struct. 128 | * 129 | * \return Particle - Particle struct. 130 | */ 131 | Particle getMean(); 132 | 133 | Particle getBestParticle(); 134 | 135 | /*! \brief To build the particles pose message. 136 | * 137 | * \param msg Type of message that it is wanted to build. 138 | */ 139 | //void buildParticlesPoseMsg(geometry_msgs::PoseArray& msg) const; 140 | 141 | /*! \brief This function implements the PF init stage. 142 | * 143 | * \param num_particles Particle number in the filter. 144 | * \param x_init Init x-axis position. 145 | * \param y_init Init x-axis position. 146 | * \param z_init Init x-axis position. 147 | * \param a_init Init yaw angle orientation. 148 | * \param x_dev Init thresholds of x-axis position. 149 | * \param y_dev Init thresholds of y-axis position. 150 | * \param z_dev Init thresholds of z-axis position. 151 | * \param a_dev Init thresholds of yaw angle orientation. 152 | * 153 | * It restructures the particle vector to adapt it to the number of selected particles. Subsequently, it initializes 154 | * it using a Gaussian distribution and the deviation introduced. Subsequently, it calculates what would be the 155 | * average particle that would simulate the estimated position of the UAV. 156 | */ 157 | void relocPose(const int num_particles, const float x_init, const float y_init, const float z_init, 158 | const float roll_init, const float pitch_init, const float yaw_init, 159 | const float x_dev, const float y_dev, const float z_dev, 160 | const float roll_dev, const float pitch_dev, const float yaw_dev); 161 | 162 | /*! \brief This function implements the PF prediction stage. 163 | * (Translation in X, Y and Z in meters and yaw angle incremenet in rad.) 164 | * 165 | * \param odom_x_mod Increased odometry in the position of the x-axis. 166 | * \param odom_y_mod Increased odometry in the position of the x-axis. 167 | * \param odom_z_mod Increased odometry in the position of the x-axis. 168 | * \param odom_a_mod Increased odometry in the position of the x-axis. 169 | * \param delta_x Thresholds of x-axis position in prediction. 170 | * \param delta_y Thresholds of y-axis position in prediction. 171 | * \param delta_z Thresholds of z-axis position in prediction. 172 | * \param delta_a Thresholds of yaw angle orientation in prediction. 173 | * 174 | * It calculates the increase that has occurred in the odometry and makes predictions of where it is possible that the 175 | * UAV is, taking into account selected thresholds. 176 | */ 177 | void predict(const double delta_x, const double delta_y, const double delta_z, 178 | const double delta_roll,const double delta_pitch,const double delta_yaw, 179 | const double odom_x_noise, const double odom_y_noise, const double odom_z_noise, 180 | const double odom_roll_noise, const double odom_pitch_noise,const double odom_yaw_noise); 181 | 182 | /*! \brief This function implements the PF update stage. 183 | * 184 | * \param grid3d Instance of the Grid3d class. 185 | * \param cloud Point cloud from the UAV view. 186 | * \param range_data Information of the radio-range sensor. 187 | * \param alpha Percentage weight between point cloud and range sensor. 188 | * \param sigma Desviation in the measurement of the radio-range sensor. 189 | * 190 | * It takes the positions of the particles to change if they are on the map. Then, it evaluates the weight of the 191 | * particle according to the point cloud and the measurement of the radio sensors. Finally, it normalizes the weights 192 | * for all particles and finds the average for the composition of the UAV pose. 193 | */ 194 | void update(const pcl::PointCloud::Ptr& cloud); 195 | void update(const pcl::PointCloud::Ptr& cloud,const int& sampleNum); 196 | 197 | void particleNormalize(); 198 | 199 | /*! \brief This function implements the PF resample stage. 200 | * Translation in X, Y and Z in meters and yaw angle incremenet in rad. 201 | * 202 | * \param num_particles Particle number in the filter. 203 | * \param x_dev Init thresholds of x-axis position. 204 | * \param y_dev Init thresholds of y-axis position. 205 | * \param z_dev Init thresholds of z-axis position. 206 | * \param a_dev Init thresholds of yaw angle orientation. 207 | * 208 | * Sample the particle set again using low variance samples. So that the particles with less weights are discarded. To 209 | * complete the number of particles that the filter must have, new ones are introduced taking the average of those 210 | * that passed the resampling and applying the same variance thresholds that is applied in the prediction. 211 | */ 212 | void resample(); 213 | 214 | void uniformSample(int& sample_num); 215 | 216 | void setParticleNum(int max_sample = 800,int min_sample = 150); 217 | 218 | 219 | public: 220 | std::vector p_; /*!< Vector of particles */ 221 | Particle mean_; /*!< Particle to show the mean of all the particles */ 222 | Grid3d* grid3d_; /*!< Instance of the Grid3d class */ 223 | 224 | LocalizationParam localization_params_; 225 | 226 | int max_resamp_num_,min_resamp_num_; 227 | 228 | private: 229 | 230 | /*! \brief To generate the random value by the Gaussian distribution. 231 | * 232 | * \param mean Average of the distribution. 233 | * \param sigma Desviation of the distribution. 234 | * \return float - Random value. 235 | */ 236 | float ranGaussian(const double mean, const double sigma); 237 | 238 | /*! \brief To generate the random between two values. 239 | * 240 | * \param range_from Lower end of range. 241 | * \param range_to Upper end of range. 242 | * \return float - Random value. 243 | */ 244 | float rngUniform(const float range_from, const float range_to); 245 | 246 | bool initialized_{ false }; /*!< To indicate the initialition of the filter */ 247 | 248 | std::random_device rd_; /*!< Random device */ 249 | std::mt19937 generator_; /*!< Generator of random values */ 250 | 251 | VSCOMMON::LoggerPtr g_log; 252 | }; 253 | 254 | } // namespace amcl3d 255 | -------------------------------------------------------------------------------- /amcl3d/src/PointCloudTools.cpp: -------------------------------------------------------------------------------- 1 | /*! 2 | * @file PointCloudTools.cpp 3 | * @copyright Copyright (c) 2019, FADA-CATEC 4 | * 5 | * Licensed under the Apache License, Version 2.0 (the "License"); 6 | * you may not use this file except in compliance with the License. 7 | * You may obtain a copy of the License at 8 | * 9 | * http://www.apache.org/licenses/LICENSE-2.0 10 | * 11 | * Unless required by applicable law or agreed to in writing, software 12 | * distributed under the License is distributed on an "AS IS" BASIS, 13 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 14 | * See the License for the specific language governing permissions and 15 | * limitations under the License. 16 | */ 17 | 18 | #include 19 | 20 | #include 21 | 22 | #include "PointCloudTools.h" 23 | 24 | namespace amcl3d 25 | { 26 | /*boost::shared_ptr openOcTree(const std::string& file_path) 27 | { 28 | if (!boost::filesystem::exists(file_path)) 29 | throw std::runtime_error(std::string("Cannot find file ") + file_path); 30 | 31 | boost::shared_ptr octo_tree; 32 | 33 | if (file_path.compare(file_path.length() - 3, 3, ".bt") == 0) 34 | { 35 | octo_tree.reset(new octomap::OcTree(0.1)); 36 | 37 | if (!octo_tree->readBinary(file_path)) 38 | throw std::runtime_error("OcTree cannot be read"); 39 | } 40 | else if (file_path.compare(file_path.length() - 3, 3, ".ot") == 0) 41 | { 42 | octo_tree.reset(dynamic_cast(octomap::AbstractOcTree::read(file_path))); 43 | } 44 | 45 | if (!octo_tree) 46 | throw std::runtime_error(std::string("OcTree cannot be created from file ") + file_path); 47 | 48 | return octo_tree; 49 | }*/ 50 | 51 | /*PointCloudInfo::Ptr computePointCloud(boost::shared_ptr octo_tree) 52 | { 53 | if (!octo_tree) 54 | throw std::runtime_error("OcTree is NULL"); 55 | 56 | const uint32_t octo_size = octo_tree->size(); 57 | if (octo_size <= 1) 58 | throw std::runtime_error("OcTree is empty"); 59 | 60 | PointCloudInfo::Ptr pc_info(new PointCloudInfo()); 61 | 62 | octo_tree->getMetricMin(pc_info->octo_min_x, pc_info->octo_min_y, pc_info->octo_min_z); 63 | octo_tree->getMetricMax(pc_info->octo_max_x, pc_info->octo_max_y, pc_info->octo_max_z); 64 | pc_info->octo_resol = octo_tree->getResolution(); 65 | 66 | pc_info->cloud.reset(new pcl::PointCloud()); 67 | 68 | pcl::PointXYZ point; 69 | for (octomap::OcTree::leaf_iterator it = octo_tree->begin_leafs(); it != octo_tree->end_leafs(); ++it) 70 | { 71 | if (it != nullptr && octo_tree->isNodeOccupied(*it)) 72 | { 73 | point.x = static_cast(it.getX()); 74 | point.y = static_cast(it.getY()); 75 | point.z = static_cast(it.getZ()); 76 | 77 | pc_info->cloud->push_back(point); 78 | } 79 | } 80 | 81 | return pc_info; 82 | }*/ 83 | 84 | PointCloudInfo::Ptr computePointCloud(pcl::PointCloud::Ptr inputCloud,double resolution) 85 | { 86 | if (!inputCloud) 87 | throw std::runtime_error("OcTree is NULL"); 88 | 89 | const uint32_t octo_size = inputCloud->size(); 90 | if (octo_size <= 1) 91 | throw std::runtime_error("OcTree is empty"); 92 | 93 | PointCloudInfo::Ptr pc_info(new PointCloudInfo()); 94 | PointType minPt, maxPt; 95 | pcl::getMinMax3D(*inputCloud, minPt, maxPt); 96 | pc_info->octo_min_x = minPt.x; 97 | pc_info->octo_min_y = minPt.y; 98 | pc_info->octo_min_z = minPt.z; 99 | pc_info->octo_max_x = maxPt.x; 100 | pc_info->octo_max_y = maxPt.y; 101 | pc_info->octo_max_z = maxPt.z; 102 | 103 | pc_info->octo_resol = resolution; 104 | 105 | //pc_info->cloud.reset(new pcl::PointCloud()); 106 | pc_info->cloud = inputCloud; 107 | 108 | return pc_info; 109 | } 110 | 111 | Grid3dInfo::Ptr computeGrid(PointCloudInfo::Ptr pc_info, const double sensor_dev) 112 | { 113 | if (!pc_info) 114 | throw std::runtime_error("PointCloudInfo is NULL"); 115 | 116 | Grid3dInfo::Ptr grid_info(new Grid3dInfo()); 117 | grid_info->sensor_dev = sensor_dev; 118 | 119 | /* Alloc the 3D grid */ 120 | const auto octo_size_x = pc_info->octo_max_x - pc_info->octo_min_x; 121 | const auto octo_size_y = pc_info->octo_max_y - pc_info->octo_min_y; 122 | const auto octo_size_z = pc_info->octo_max_z - pc_info->octo_min_z; 123 | grid_info->size_x = static_cast(ceil(octo_size_x / pc_info->octo_resol)); 124 | grid_info->size_y = static_cast(ceil(octo_size_y / pc_info->octo_resol)); 125 | grid_info->size_z = static_cast(ceil(octo_size_z / pc_info->octo_resol)); 126 | 127 | grid_info->step_y = grid_info->size_x; 128 | grid_info->step_z = grid_info->size_x * grid_info->size_y; 129 | 130 | const auto grid_size = grid_info->size_x * grid_info->size_y * grid_info->size_z; 131 | grid_info->grid.resize(grid_size); 132 | std::cout<<"grid size: "<< grid_size< kdtree; 136 | kdtree.setInputCloud(pc_info->cloud); 137 | 138 | /* Compute the distance to the closest point of the grid */ 139 | const float gauss_const1 = static_cast(1. / (grid_info->sensor_dev * sqrt(2 * M_PI))); 140 | const float gauss_const2 = static_cast(1. / (2. * grid_info->sensor_dev * grid_info->sensor_dev)); 141 | uint32_t index; 142 | float dist; 143 | PointType search_point; 144 | std::vector point_idx_nkn_search(1); 145 | std::vector point_nkn_squared_distance(1); 146 | for (uint32_t iz = 0; iz < grid_info->size_z; ++iz) 147 | { 148 | for (uint32_t iy = 0; iy < grid_info->size_y; ++iy) 149 | { 150 | for (uint32_t ix = 0; ix < grid_info->size_x; ++ix) 151 | { 152 | search_point.x = pc_info->octo_min_x + (ix * pc_info->octo_resol); 153 | search_point.y = pc_info->octo_min_y + (iy * pc_info->octo_resol); 154 | search_point.z = pc_info->octo_min_z + (iz * pc_info->octo_resol); 155 | 156 | index = ix + iy * grid_info->step_y + iz * grid_info->step_z; 157 | 158 | if (kdtree.nearestKSearch(search_point, 1, point_idx_nkn_search, point_nkn_squared_distance) > 0) 159 | { 160 | dist = point_nkn_squared_distance[0]; 161 | //grid_info->grid[index].dist = dist; 162 | grid_info->grid[index].prob = gauss_const1 * expf(-dist * dist * gauss_const2); 163 | } 164 | else 165 | { 166 | //grid_info->grid[index].dist = -1.0; 167 | grid_info->grid[index].prob = 0.0; 168 | } 169 | } 170 | } 171 | } 172 | 173 | return grid_info; 174 | } 175 | 176 | Grid3dInfo::Ptr computeGrid2(PointCloudInfo::Ptr pc_info, const double sensor_dev) 177 | { 178 | if (!pc_info) 179 | throw std::runtime_error("PointCloudInfo is NULL"); 180 | 181 | Grid3dInfo::Ptr grid_info(new Grid3dInfo()); 182 | grid_info->sensor_dev = sensor_dev; 183 | 184 | /* Alloc the 3D grid */ 185 | const auto octo_size_x = pc_info->octo_max_x - pc_info->octo_min_x; 186 | const auto octo_size_y = pc_info->octo_max_y - pc_info->octo_min_y; 187 | const auto octo_size_z = pc_info->octo_max_z - pc_info->octo_min_z; 188 | grid_info->size_x = static_cast(ceil(octo_size_x / pc_info->octo_resol)); 189 | grid_info->size_y = static_cast(ceil(octo_size_y / pc_info->octo_resol)); 190 | grid_info->size_z = static_cast(ceil(octo_size_z / pc_info->octo_resol)); 191 | 192 | grid_info->step_y = grid_info->size_x; 193 | grid_info->step_z = grid_info->size_x * grid_info->size_y; 194 | 195 | const auto grid_size = grid_info->size_x * grid_info->size_y * grid_info->size_z; 196 | grid_info->grid.resize(grid_size); 197 | std::cout<<"grid size: "<< grid_size<(1. / (grid_info->sensor_dev * sqrt(2 * M_PI))); 201 | const float gauss_const2 = static_cast(1. / (2. * grid_info->sensor_dev * grid_info->sensor_dev)); 202 | uint32_t index; 203 | float dist; 204 | PointType search_point; 205 | 206 | for(auto pt:*pc_info->cloud) 207 | { 208 | uint32_t ix = static_cast((pt.x - pc_info->octo_min_x)/pc_info->octo_resol); 209 | uint32_t iy = static_cast((pt.y - pc_info->octo_min_y)/pc_info->octo_resol); 210 | uint32_t iz = static_cast((pt.z - pc_info->octo_min_z)/pc_info->octo_resol); 211 | #ifdef TTTTT 212 | index = ix + iy * grid_info->step_y + iz * grid_info->step_z; 213 | 214 | search_point.x = pc_info->octo_min_x + ix*pc_info->octo_resol; 215 | search_point.y = pc_info->octo_min_y + iy*pc_info->octo_resol; 216 | search_point.z = pc_info->octo_min_z + iz*pc_info->octo_resol; 217 | 218 | dist = std::sqrt((search_point.x - pt.x)*(search_point.x - pt.x) 219 | + (search_point.y - pt.y)*(search_point.y - pt.y) 220 | + (search_point.z - pt.z)*(search_point.z - pt.z)); 221 | 222 | grid_info->grid[index].prob = gauss_const1 * expf(-dist * dist * gauss_const2); 223 | #else 224 | int xx,yy,zz; 225 | for(int i = -1;i <=1;i++) 226 | { 227 | xx = ix +i; 228 | if(xx < 0 || xx >= grid_info->size_x) continue; 229 | for(int j = -1;j <=1;j++) 230 | { 231 | yy = iy+j; 232 | if(yy < 0|| yy >= grid_info->size_y) continue; 233 | for(int k = -1;k <= 1;k++) 234 | { 235 | zz = iz + k; 236 | if(zz < 0|| zz > grid_info->size_z) continue; 237 | index = xx + yy * grid_info->step_y + zz * grid_info->step_z; 238 | if(index < 0 || index >= grid_size) continue; 239 | search_point.x = pc_info->octo_min_x + xx*pc_info->octo_resol; 240 | search_point.y = pc_info->octo_min_y + yy*pc_info->octo_resol; 241 | search_point.z = pc_info->octo_min_z + zz*pc_info->octo_resol; 242 | dist = std::sqrt((search_point.x - pt.x)*(search_point.x - pt.x) 243 | + (search_point.y - pt.y)*(search_point.y - pt.y) 244 | + (search_point.z - pt.z)*(search_point.z - pt.z)); 245 | 246 | grid_info->grid[index].prob = (gauss_const1 * expf(-dist * dist * gauss_const2) > grid_info->grid[index].prob)?(gauss_const1 * expf(-dist * dist * gauss_const2)):grid_info->grid[index].prob; 247 | } 248 | } 249 | } 250 | #endif 251 | } 252 | 253 | return grid_info; 254 | } 255 | 256 | } // namespace amcl3d 257 | -------------------------------------------------------------------------------- /amcl3d/src/PointCloudTools.h: -------------------------------------------------------------------------------- 1 | /*! 2 | * @file PointCloudTools.h 3 | * @copyright Copyright (c) 2019, FADA-CATEC 4 | * 5 | * Licensed under the Apache License, Version 2.0 (the "License"); 6 | * you may not use this file except in compliance with the License. 7 | * You may obtain a copy of the License at 8 | * 9 | * http://www.apache.org/licenses/LICENSE-2.0 10 | * 11 | * Unless required by applicable law or agreed to in writing, software 12 | * distributed under the License is distributed on an "AS IS" BASIS, 13 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 14 | * See the License for the specific language governing permissions and 15 | * limitations under the License. 16 | */ 17 | 18 | #pragma once 19 | 20 | // #include 21 | #include 22 | #include 23 | #include 24 | 25 | namespace amcl3d 26 | { 27 | using PointType = pcl::PointXYZI; 28 | /*! \brief Class that contains the size of the cell grid. 29 | */ 30 | class Grid3dCell 31 | { 32 | public: 33 | //float dist{ -1 }; // Distance ,may not used 34 | float prob{ 0 }; /*!< Probability */ 35 | }; 36 | 37 | /*! \brief Class that contains the information of the grid. 38 | */ 39 | class Grid3dInfo 40 | { 41 | public: 42 | typedef boost::shared_ptr Ptr; /*!< Grid information shared point */ 43 | typedef boost::shared_ptr ConstPtr; /*!< Grid information shared point (const)*/ 44 | 45 | std::vector grid; /*!< Vector of cells */ 46 | double sensor_dev{ 0 }; /*!< Desviation of the 3d point cloud sensor */ 47 | uint32_t size_x{ 0 }; /*!< Grid size of x-axis */ 48 | uint32_t size_y{ 0 }; /*!< Grid size of y-axis */ 49 | uint32_t size_z{ 0 }; /*!< Grid size of z-axis */ 50 | uint32_t step_y{ 0 }; /*!< Step to convert from matrix to vector in y-axis */ 51 | uint32_t step_z{ 0 }; /*!< Step to convert from matrix to vector in z-axis */ 52 | }; 53 | 54 | /*! \brief Class that contains the information of the point cloud. 55 | */ 56 | class PointCloudInfo 57 | { 58 | public: 59 | typedef boost::shared_ptr Ptr; /*!< Point cloud information shared point */ 60 | typedef boost::shared_ptr ConstPtr; /*!< Point cloud information shared point (const)*/ 61 | 62 | pcl::PointCloud::Ptr cloud; /*!< Pcl point cloud */ 63 | double octo_min_x{ 0 }; /*!< Minimum x-axis value of octomap */ 64 | double octo_min_y{ 0 }; /*!< Minimum y-axis value of octomap */ 65 | double octo_min_z{ 0 }; /*!< Minimum z-axis value of octomap */ 66 | double octo_max_x{ 0 }; /*!< Maximum x-axis value of octomap */ 67 | double octo_max_y{ 0 }; /*!< Maximum y-axis value of octomap */ 68 | double octo_max_z{ 0 }; /*!< Maximum z-axis value of octomap */ 69 | double octo_resol{ 0 }; /*!< Octomap Resolution */ 70 | }; 71 | 72 | /*! \brief To open the octomap. 73 | * 74 | * \param file_path Route of the octomap environment. 75 | * \return OcTree Octomap information shared point 76 | * 77 | * It is responsible for opening the octomapa using the route that the user provides. It check the route and the 78 | * correctly creation of the octree. 79 | */ 80 | //boost::shared_ptr openOcTree(const std::string& file_path); 81 | 82 | /*! \brief Load the octomap in PCL for easy nearest neighborhood computation. 83 | * 84 | * \param octo_tree Shared point that contains the information of the environment map. 85 | * \return PointCloudInfo Point cloud information associated with the map. 86 | * 87 | * It is responsible for opening the octomapa using the route that the user provides. It check the route and the 88 | * correctly creation of the octree. The point-cloud is shifted to have (0,0,0) as min values 89 | */ 90 | //PointCloudInfo::Ptr computePointCloud(boost::shared_ptr octo_tree); 91 | PointCloudInfo::Ptr computePointCloud(pcl::PointCloud::Ptr inputCloud,double resolution); 92 | 93 | /*! \brief Load the octomap in PCL for easy nearest neighborhood computation. 94 | * 95 | * \param pc_info Point cloud information associated with the map. 96 | * \param sensor_dev Desviation of the 3d point cloud sensor. 97 | * \return Grid3dInfo Grid information associated with the map. 98 | * 99 | * It verifies the information in the point cloud to check if it is not null. Use the sensor deviation to calculate and 100 | * save the grid size as well as the distance information between the cloud points to the nearest point on the grid. 101 | */ 102 | Grid3dInfo::Ptr computeGrid(PointCloudInfo::Ptr pc_info, const double sensor_dev); 103 | 104 | Grid3dInfo::Ptr computeGrid2(PointCloudInfo::Ptr pc_info, const double sensor_dev); 105 | 106 | } // namespace amcl3d 107 | -------------------------------------------------------------------------------- /amcl3d/src/amcl3d.cpp: -------------------------------------------------------------------------------- 1 | /*! 2 | * @file ParticleFilter.cpp 3 | * @copyright Copyright (c) 2019, FADA-CATEC 4 | * 5 | * Licensed under the Apache License, Version 2.0 (the "License"); 6 | * you may not use this file except in compliance with the License. 7 | * You may obtain a copy of the License at 8 | * 9 | * http://www.apache.org/licenses/LICENSE-2.0 10 | * 11 | * Unless required by applicable law or agreed to in writing, software 12 | * distributed under the License is distributed on an "AS IS" BASIS, 13 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 14 | * See the License for the specific language governing permissions and 15 | * limitations under the License. 16 | */ 17 | 18 | #include "amcl3d.h" 19 | 20 | namespace amcl3d 21 | { 22 | MonteCarloLocalization::MonteCarloLocalization(VSCOMMON::LoggerPtr& t_log) 23 | : g_log(t_log) 24 | ,ParticleFilter(t_log) 25 | ,loc_loop_(0) 26 | ,cur_loc_status_(GLOBAL_LOC) 27 | ,global_loc_done_(false) 28 | ,cur_loc_status_cnt_(0) 29 | { } 30 | 31 | MonteCarloLocalization::~MonteCarloLocalization() 32 | { } 33 | 34 | bool MonteCarloLocalization::init() 35 | { 36 | cloud_ds.reset(new pcl::PointCloud()); 37 | ds_.setLeafSize(params_.voxel_size_, params_.voxel_size_, params_.voxel_size_); 38 | if (!grid3d_->open(params_.map_path_, params_.sensor_dev_)) 39 | return false; 40 | kdtree_keypose_3d_.reset(new pcl::KdTreeFLANN()); 41 | kdtree_keypose_3d_->setInputCloud(grid3d_->keyposes_3d); 42 | return true; 43 | } 44 | 45 | void MonteCarloLocalization::processFrame(pcl::PointCloud::Ptr& input_cloud,Eigen::Affine3f& odom_increment) 46 | { 47 | VSCOMMON::tic("ProcessFrame"); 48 | LOG_INFO(g_log,"Begin process frame. loop:" << loc_loop_++); 49 | 50 | //pcl::PointCloud::Ptr temp_cloud(new pcl::PointCloud()); 51 | ds_.setInputCloud(input_cloud); 52 | ds_.filter(*cloud_ds); 53 | 54 | static bool first_process = true; 55 | // 注释掉另一种likelyhood计算方式 56 | /*if(!first_process) 57 | { 58 | current_pose = mean_; 59 | if((current_pose.x-Last_pose.x)*(current_pose.x-Last_pose.x) + (current_pose.y-Last_pose.y)*(current_pose.y-Last_pose.y) > 25) 60 | { 61 | if(grid3d_->updateMap(current_pose.x,current_pose.y,current_pose.z)) 62 | Last_pose = current_pose; 63 | } 64 | }*/ 65 | 66 | float _t_x, _t_y, _t_z, _t_roll, _t_pitch, _t_yaw; 67 | pcl::getTranslationAndEulerAngles(odom_increment, _t_x, _t_y, _t_z, _t_roll, _t_pitch, _t_yaw); 68 | odomIncre_ = Particle(_t_x, _t_y, _t_z, _t_roll, _t_pitch, _t_yaw); 69 | 70 | if(!global_loc_done_) // do global localization here 71 | { 72 | caculateGlobalNoise(); 73 | globalLocalization(); 74 | } 75 | else // do local localization for times 76 | { 77 | caculateLocalNoise(); 78 | localLocalization(); 79 | } 80 | 81 | getMean(); 82 | if(first_process) 83 | { 84 | first_process = false; 85 | Last_pose = mean_; 86 | } 87 | LOG_INFO(g_log,"Finish process frame. cost: " << VSCOMMON::toc("ProcessFrame") * 1000 << " ms." 88 | <<" robot pose:"<computeCloudWeight(cloud_ds, mean_pf.x, mean_pf.y, mean_pf.z, mean_pf.roll, mean_pf.pitch, mean_pf.yaw); 135 | 136 | LOG_COUT_INFO(g_log,"Global localization." << " cnt: " << cur_loc_status_cnt_ 137 | <<",particle num: "<< ptc_num<<",w_pf: "<< weight_pf<<",cloud_ds: "<< cloud_ds->points.size()); 138 | 139 | if(ptc_num < 550 && weight_pf>localization_params_.weight_global_) 140 | { 141 | consist_cnt++; 142 | if(consist_cnt > 10/*consist_cnt_thres_*/) 143 | { 144 | consist_cnt = 0; 145 | LOG_COUT_INFO(g_log,"Global localization done. Current pose is " <updateMap(kd_nearest_pt.x,kd_nearest_pt.y,kd_nearest_pt.z); 171 | 172 | LOG_INFO(g_log,"find nearest pt: "<< kd_nearest_pt.x<<" "<< kd_nearest_pt.y<<" "<< kd_nearest_pt.z <<" from: "<< x_init<<" "<< y_init<<" "<< z_init); 173 | set_pose_ = Particle(x_init, y_init, kd_nearest_pt.z, roll_init,pitch_init,yaw_init); 174 | ParticleFilter::relocPose(num_particles, x_init, y_init, kd_nearest_pt.z, roll_init,pitch_init,yaw_init, 175 | localization_params_.init_x_dev_,localization_params_.init_y_dev_,localization_params_.init_z_dev_, 176 | localization_params_.init_roll_dev_,localization_params_.init_pitch_dev_,localization_params_.init_yaw_dev_); 177 | 178 | setParticleNum(localization_params_.max_particle_num_global,localization_params_.min_particle_num_global); 179 | cur_loc_status_cnt_ = 1; 180 | global_loc_done_ = false; 181 | } 182 | 183 | void MonteCarloLocalization::PFMove(const Particle& odom_increment, const Particle& odom_increment_noise) 184 | { 185 | VSCOMMON::tic("PFMove"); 186 | ParticleFilter::predict(odom_increment.x,odom_increment.y,odom_increment.z,odom_increment.roll,odom_increment.pitch, 187 | odom_increment.yaw,odom_increment_noise.x,odom_increment_noise.y,odom_increment_noise.z, 188 | odom_increment_noise.roll,odom_increment_noise.pitch,odom_increment_noise.yaw); 189 | LOG_INFO(g_log,"PFMove time:"< max_w) 211 | max_w = p_[i].w; 212 | 213 | float k = 1; 214 | if(max_w>0) k = 1.0/max_w; 215 | for(uint32_t i = 0;i < p_.size();++i) 216 | p_[i].w = k*p_[i].w; 217 | } 218 | 219 | void MonteCarloLocalization::setParticleWeight() 220 | { } 221 | 222 | void computeBoundary(const std::vector& p_set,float& xmin 223 | ,float& ymin,float& zmin,float& xmax,float& ymax,float& zmax) 224 | { 225 | xmin = xmax = ymin = ymax = zmin = zmax = 0.0; 226 | if(p_set.empty()) 227 | return; 228 | xmin = xmax = p_set[0].x; 229 | ymin = ymax = p_set[0].y; 230 | zmin = zmax = p_set[0].z; 231 | for(uint32_t i = 0;i < p_set.size();++i) 232 | { 233 | Particle _p = p_set[i]; 234 | xmax = xmax>_p.x ? xmax:_p.x; 235 | xmin = xmin>_p.x ? _p.x:xmin; 236 | ymax = ymax>_p.y ? ymax:_p.y; 237 | ymin = ymin>_p.y ? _p.y:ymin; 238 | zmax = zmax>_p.z ? zmax:_p.z; 239 | zmin = zmin>_p.z ? _p.z:zmin; 240 | } 241 | } 242 | int MonteCarloLocalization::computeSampleNum(const int cnt_per_grid /*= 9*/) 243 | { 244 | float xyz_step = 0.2; 245 | float theta_step = 8.0; 246 | float xmin, xmax, ymin, ymax ,zmin, zmax; 247 | computeBoundary(p_,xmin,ymin,zmin,xmax,ymax,zmax); 248 | 249 | int xwidth = std::ceil((xmax - xmin)/xyz_step)+1; 250 | int ywidth = std::ceil((ymax - ymin)/xyz_step)+1; 251 | int zwidth = std::ceil((zmax - zmin)/xyz_step)+1; 252 | int theta = std::ceil((M_PI*2/theta_step*180))+1; 253 | LOG_INFO(g_log,"bdx: "<< xmin<<","<< xmax<<", bdy: "<< ymin<<","<< ymax<<", bdz: " 254 | << zmin<<","<< zmax <<", xwidth: "<< xwidth<<", "<< ywidth<<", "<< zwidth<<", "<< theta); 255 | if(xwidth*ywidth*zwidth == 0) 256 | return min_resamp_num_; 257 | else if(xwidth*ywidth*zwidth > 1e3) 258 | return max_resamp_num_; 259 | 260 | std::vector>>> grid(xwidth,std::vector>>(ywidth,std::vector>(zwidth,std::vector(theta,0)))); 261 | for(uint32_t i = 0;i < p_.size();++i) 262 | { 263 | Particle _p = p_[i]; 264 | int x = (_p.x - xmin)/xyz_step; 265 | int y = (_p.y - ymin)/xyz_step; 266 | int z = (_p.z - zmin)/xyz_step; 267 | int t = (_p.yaw - (-M_PI))/theta_step; 268 | if(x>=0 && x < xwidth && y>=0 && y < ywidth && z >= 0 && z < zwidth && t >=0 && y < theta) 269 | grid[x][y][z][t]++; 270 | else 271 | std::cout<<"error: pf out of boundary"<0?1:0); 280 | 281 | int sample_num = cnt * cnt_per_grid; 282 | LOG_INFO(g_log,"cnt: "<< cnt<<", sample_num: "<< sample_num); 283 | if(sample_num < min_resamp_num_) 284 | return min_resamp_num_; 285 | if(sample_num > max_resamp_num_) 286 | return max_resamp_num_; 287 | return sample_num; 288 | } 289 | 290 | pcl::PointCloud MonteCarloLocalization::getMapCloud() 291 | { 292 | return (*(grid3d_->pc_info_->cloud)); 293 | } 294 | 295 | PointType MonteCarloLocalization::getMapHeight(const float& x,const float& y) 296 | { 297 | PointType kd_pt; 298 | kd_pt.x = x,kd_pt.y = y,kd_pt.z = 0.; 299 | std::vector point_idx_(1); 300 | std::vector point_nkn_squared_(1); 301 | kdtree_keypose_3d_->nearestKSearch(kd_pt,1,point_idx_,point_nkn_squared_); 302 | 303 | PointType kd_nearest_pt; 304 | kd_nearest_pt.x = grid3d_->keyposes_3d->points[point_idx_[0]].x, 305 | kd_nearest_pt.y = grid3d_->keyposes_3d->points[point_idx_[0]].y, 306 | kd_nearest_pt.z = grid3d_->keyposes_3d->points[point_idx_[0]].z; 307 | return kd_nearest_pt; 308 | } 309 | 310 | void MonteCarloLocalization::updateSampleHeight() 311 | { 312 | VSCOMMON::tic("updateSampleHeight"); 313 | Particle pt = getMean(); 314 | PointType pt_pcl = getMapHeight(pt.x,pt.y); 315 | float delta_h = pt_pcl.z - pt.z; 316 | for (uint32_t i = 0; i < p_.size(); ++i) 317 | { 318 | p_[i].z += delta_h; 319 | } 320 | LOG_INFO(g_log,"update Sample Height takes:"< 22 | #include 23 | 24 | namespace amcl3d 25 | { 26 | 27 | /*! \brief Class that contains the stages of the Particle Filter. 28 | */ 29 | class MonteCarloLocalization : public ParticleFilter 30 | { 31 | public: 32 | /*! \brief MonteCarloLocalization class constructor. 33 | */ 34 | explicit MonteCarloLocalization(VSCOMMON::LoggerPtr&); 35 | /*! \brief MonteCarloLocalization class destructor. 36 | */ 37 | virtual ~MonteCarloLocalization(); 38 | 39 | void processFrame(pcl::PointCloud::Ptr& input,Eigen::Affine3f& odom_increment); 40 | 41 | void RelocPose(const int num_particles, const float x_init, const float y_init, const float z_init, 42 | const float roll_init, const float pitch_init,const float yaw_init); 43 | 44 | void PFMove(const Particle& odom_increment, const Particle& odom_increment_noise); 45 | 46 | void PFResample(const bool weight_multiply = false); 47 | 48 | std::vector getParticle() 49 | { 50 | return p_; 51 | } 52 | 53 | void setParams(const LocalizationParam& option,const Parameters& param) 54 | { 55 | localization_params_ = option; 56 | params_ = param; 57 | } 58 | 59 | bool init(); 60 | 61 | pcl::PointCloud getMapCloud(); 62 | 63 | const bool globalLocalizationDone() const 64 | { 65 | return global_loc_done_; 66 | } 67 | 68 | private: 69 | void caculateGlobalNoise(const float f = 1); 70 | 71 | void caculateLocalNoise(); 72 | 73 | void globalLocalization(); 74 | 75 | void localLocalization(); 76 | 77 | void addParticleWeight(); 78 | 79 | void setParticleWeight(); 80 | 81 | int computeSampleNum(const int cnt_per_grid = 3); 82 | 83 | PointType getMapHeight(const float& x,const float& y); 84 | 85 | void updateSampleHeight(); 86 | 87 | Parameters params_; 88 | 89 | Particle Last_pose,current_pose; 90 | 91 | enum{GLOBAL_LOC, LASER_ODOM, POSE_TRACKING, BLIND_TRACKING}; 92 | int cur_loc_status_; 93 | 94 | pcl::KdTreeFLANN::Ptr kdtree_keypose_3d_; 95 | 96 | VSCOMMON::LoggerPtr g_log; 97 | 98 | pcl::VoxelGrid ds_; 99 | pcl::PointCloud::Ptr cloud_ds; 100 | int loc_loop_; /*process pointcloud loop num*/ 101 | int cur_loc_status_cnt_; 102 | 103 | 104 | bool global_loc_done_; 105 | Particle set_pose_; 106 | Particle odomIncre_; 107 | Particle cur_odom_noise_; 108 | }; 109 | 110 | } // namespace amcl3d 111 | -------------------------------------------------------------------------------- /amcl3d/src/floam/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # csparse 2 | LIST(APPEND CMAKE_MODULE_PATH ${3RDPARTY_DIR}/g2o/g2o2017_CSparse/cmake_modules) 3 | find_package( CSparse REQUIRED) 4 | find_package(Ceres REQUIRED) 5 | include_directories( 6 | include 7 | ${CSPARSE_INCLUDE_DIR} 8 | ${CERES_INCLUDE_DIRS}) 9 | 10 | link_directories( 11 | include 12 | ${CERES_LIBRARY_DIRS} ) 13 | 14 | 15 | add_executable(floam_laserProcessing_amcl3d src/laserProcessingNode.cpp src/laserProcessingClass.cpp src/lidar.cpp) 16 | target_link_libraries(floam_laserProcessing_amcl3d ${EIGEN3_LIBRARIES} ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${CERES_LIBRARIES}) 17 | 18 | add_executable(floam_odomEstimation_amcl3d src/odomEstimationNode.cpp src/lidarOptimization.cpp src/lidar.cpp src/odomEstimationClass.cpp) 19 | target_link_libraries(floam_odomEstimation_amcl3d ${EIGEN3_LIBRARIES} ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${CERES_LIBRARIES}) 20 | -------------------------------------------------------------------------------- /amcl3d/src/floam/include/laserProcessingClass.h: -------------------------------------------------------------------------------- 1 | // Author of FLOAM: Wang Han 2 | // Email wh200720041@gmail.com 3 | // Homepage https://wanghan.pro 4 | #ifndef _LASER_PROCESSING_CLASS_H_ 5 | #define _LASER_PROCESSING_CLASS_H_ 6 | 7 | #include 8 | #include 9 | 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | 16 | #include "lidar.h" 17 | 18 | //points covariance class 19 | class Double2d{ 20 | public: 21 | int id; 22 | double value; 23 | Double2d(int id_in, double value_in); 24 | }; 25 | //points info class 26 | class PointsInfo{ 27 | public: 28 | int layer; 29 | double time; 30 | PointsInfo(int layer_in, double time_in); 31 | }; 32 | 33 | 34 | //这里所有的数据传递全部采用指针或引用的形式来提高传递效率 35 | class LaserProcessingClass 36 | { 37 | public: 38 | LaserProcessingClass(); 39 | void init(lidar::Lidar lidar_param_in); 40 | void preFiltering(const pcl::PointCloud::Ptr pc_in, pcl::PointCloud::Ptr& pc_out); 41 | void featureExtraction(const pcl::PointCloud::Ptr& pc_in, pcl::PointCloud::Ptr& pc_out_edge, pcl::PointCloud::Ptr& pc_out_surf); 42 | //void featureExtraction(const pcl::PointCloud::Ptr& pc_in, pcl::PointCloud::Ptr& pc_out_sharp, pcl::PointCloud::Ptr& pc_out_lessSharp, pcl::PointCloud::Ptr& pc_out_flat, pcl::PointCloud::Ptr& pc_out_lessFlat); 43 | void featureExtractionFromSector(const pcl::PointCloud::Ptr& pc_in, std::vector& cloudCurvature, pcl::PointCloud::Ptr& pc_out_edge, pcl::PointCloud::Ptr& pc_out_surf); 44 | //void featureExtractionFromSector(const pcl::PointCloud::Ptr& pc_in, std::vector& cloudCurvature, pcl::PointCloud::Ptr& pc_edge_sharp, pcl::PointCloud::Ptr& pc_edge_lessSharp, pcl::PointCloud::Ptr& pc_surf_flat, pcl::PointCloud::Ptr& pc_surf_lessFlat); 45 | private: 46 | lidar::Lidar lidar_param; 47 | 48 | pcl::CropBox closePointFilter; 49 | pcl::CropBox farPointFilter; 50 | pcl::VoxelGrid downSizeFilter; 51 | }; 52 | 53 | 54 | 55 | #endif // _LASER_PROCESSING_CLASS_H_ 56 | 57 | -------------------------------------------------------------------------------- /amcl3d/src/floam/include/lidar.h: -------------------------------------------------------------------------------- 1 | // Author of FLOAM: Wang Han 2 | // Email wh200720041@gmail.com 3 | // Homepage https://wanghan.pro 4 | #ifndef _LIDAR_H_ 5 | #define _LIDAR_H_ 6 | 7 | //define lidar parameter 8 | 9 | namespace lidar{ 10 | 11 | class Lidar 12 | { 13 | public: 14 | Lidar(); 15 | 16 | void setScanPeriod(double scan_period_in); 17 | void setLines(double num_lines_in); 18 | void setVerticalAngle(double vertical_angle_in); 19 | void setVerticalResolution(double vertical_angle_resolution_in); 20 | //by default is 100. pls do not change 21 | void setMaxDistance(double max_distance_in); 22 | void setMinDistance(double min_distance_in); 23 | 24 | double max_distance; 25 | double min_distance; 26 | int num_lines; 27 | double scan_period; 28 | int points_per_line; 29 | double horizontal_angle_resolution; 30 | double horizontal_angle; 31 | double vertical_angle_resolution; 32 | double vertical_angle; 33 | }; 34 | 35 | 36 | } 37 | 38 | 39 | #endif // _LIDAR_H_ 40 | 41 | -------------------------------------------------------------------------------- /amcl3d/src/floam/include/lidarOptimization.h: -------------------------------------------------------------------------------- 1 | // Author of FLOAM: Wang Han 2 | // Email wh200720041@gmail.com 3 | // Homepage https://wanghan.pro 4 | #ifndef _LIDAR_OPTIMIZATION_ANALYTIC_H_ 5 | #define _LIDAR_OPTIMIZATION_ANALYTIC_H_ 6 | 7 | #include 8 | #include 9 | #include 10 | #include 11 | 12 | void getTransformFromSe3(const Eigen::Matrix& se3, Eigen::Quaterniond& q, Eigen::Vector3d& t); 13 | 14 | Eigen::Matrix3d skew(Eigen::Vector3d& mat_in); 15 | 16 | class EdgeAnalyticCostFunction : public ceres::SizedCostFunction<3, 7> { 17 | public: 18 | 19 | EdgeAnalyticCostFunction(Eigen::Vector3d curr_point_, Eigen::Vector3d last_point_a_, Eigen::Vector3d last_point_b_); 20 | virtual ~EdgeAnalyticCostFunction() {} 21 | virtual bool Evaluate(double const *const *parameters, double *residuals, double **jacobians) const; 22 | 23 | Eigen::Vector3d curr_point; 24 | Eigen::Vector3d last_point_a; 25 | Eigen::Vector3d last_point_b; 26 | }; 27 | 28 | class SurfNormAnalyticCostFunction : public ceres::SizedCostFunction<1, 7> { 29 | public: 30 | SurfNormAnalyticCostFunction(Eigen::Vector3d curr_point_, Eigen::Vector3d plane_unit_norm_, double negative_OA_dot_norm_); 31 | virtual ~SurfNormAnalyticCostFunction() {} 32 | virtual bool Evaluate(double const *const *parameters, double *residuals, double **jacobians) const; 33 | 34 | Eigen::Vector3d curr_point; 35 | Eigen::Vector3d plane_unit_norm; 36 | double negative_OA_dot_norm; 37 | }; 38 | 39 | class PoseSE3Parameterization : public ceres::LocalParameterization { 40 | public: 41 | 42 | PoseSE3Parameterization() {} 43 | virtual ~PoseSE3Parameterization() {} 44 | virtual bool Plus(const double* x, const double* delta, double* x_plus_delta) const; 45 | virtual bool ComputeJacobian(const double* x, double* jacobian) const; 46 | virtual int GlobalSize() const { return 7; } 47 | virtual int LocalSize() const { return 6; } 48 | }; 49 | 50 | 51 | 52 | #endif // _LIDAR_OPTIMIZATION_ANALYTIC_H_ 53 | 54 | -------------------------------------------------------------------------------- /amcl3d/src/floam/include/odomEstimationClass.h: -------------------------------------------------------------------------------- 1 | // Author of FLOAM: Wang Han 2 | // Email wh200720041@gmail.com 3 | // Homepage https://wanghan.pro 4 | #ifndef _ODOM_ESTIMATION_CLASS_H_ 5 | #define _ODOM_ESTIMATION_CLASS_H_ 6 | 7 | //std lib 8 | #include 9 | #include 10 | #include 11 | 12 | //PCL 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | 23 | //ceres 24 | #include 25 | #include 26 | 27 | //eigen 28 | #include 29 | #include 30 | 31 | //LOCAL LIB 32 | #include "lidar.h" 33 | #include "lidarOptimization.h" 34 | #include 35 | 36 | class OdomEstimationClass 37 | { 38 | 39 | public: 40 | OdomEstimationClass(); 41 | 42 | void init(lidar::Lidar lidar_param); 43 | void initMapWithPoints(const pcl::PointCloud::Ptr& edge_in, const pcl::PointCloud::Ptr& surf_in); 44 | void updatePointsToMap(const pcl::PointCloud::Ptr& edge_in, const pcl::PointCloud::Ptr& surf_in); 45 | void getMap(pcl::PointCloud::Ptr& laserCloudMap); 46 | 47 | Eigen::Isometry3d odom; 48 | pcl::PointCloud::Ptr laserCloudCornerMap; 49 | pcl::PointCloud::Ptr laserCloudSurfMap; 50 | std::queue> pointCloudCornerQueue; 51 | std::queue> pointCloudSurfQueue; 52 | int queueSize; 53 | private: 54 | //optimization variable 55 | double parameters[7] = {0, 0, 0, 1, 0, 0, 0}; 56 | Eigen::Map q_w_curr = Eigen::Map(parameters); 57 | Eigen::Map t_w_curr = Eigen::Map(parameters + 4); 58 | 59 | Eigen::Isometry3d last_odom; 60 | 61 | //kd-tree 62 | pcl::KdTreeFLANN::Ptr kdtreeEdgeMap; 63 | pcl::KdTreeFLANN::Ptr kdtreeSurfMap; 64 | 65 | //points downsampling before add to map 66 | pcl::VoxelGrid downSizeFilterEdge; 67 | pcl::VoxelGrid downSizeFilterSurf; 68 | 69 | //local map 70 | pcl::CropBox cropBoxFilter; 71 | 72 | //optimization count 73 | int optimization_count; 74 | 75 | //function 76 | void addEdgeCostFactor(const pcl::PointCloud::Ptr& pc_in, const pcl::PointCloud::Ptr& map_in, ceres::Problem& problem, ceres::LossFunction *loss_function); 77 | void addSurfCostFactor(const pcl::PointCloud::Ptr& pc_in, const pcl::PointCloud::Ptr& map_in, ceres::Problem& problem, ceres::LossFunction *loss_function); 78 | void addPointsToMap(const pcl::PointCloud::Ptr& downsampledEdgeCloud, const pcl::PointCloud::Ptr& downsampledSurfCloud); 79 | void pointAssociateToMap(pcl::PointXYZI const *const pi, pcl::PointXYZI *const po); 80 | void downSamplingToMap(const pcl::PointCloud::Ptr& edge_pc_in, pcl::PointCloud::Ptr& edge_pc_out, const pcl::PointCloud::Ptr& surf_pc_in, pcl::PointCloud::Ptr& surf_pc_out); 81 | }; 82 | 83 | #endif // _ODOM_ESTIMATION_CLASS_H_ 84 | 85 | -------------------------------------------------------------------------------- /amcl3d/src/floam/src/laserProcessingClass.cpp: -------------------------------------------------------------------------------- 1 | // Author of FLOAM: Wang Han 2 | // Email wh200720041@gmail.com 3 | // Homepage https://wanghan.pro 4 | #include "laserProcessingClass.h" 5 | 6 | void LaserProcessingClass::init(lidar::Lidar lidar_param_in){ 7 | 8 | lidar_param = lidar_param_in; 9 | 10 | } 11 | 12 | void LaserProcessingClass::featureExtraction(const pcl::PointCloud::Ptr& pc_in, pcl::PointCloud::Ptr& pc_out_edge, pcl::PointCloud::Ptr& pc_out_surf){ 13 | 14 | std::vector indices; 15 | pcl::removeNaNFromPointCloud(*pc_in, indices); 16 | 17 | 18 | int N_SCANS = lidar_param.num_lines; 19 | std::vector::Ptr> laserCloudScans; 20 | for(int i=0;i::Ptr(new pcl::PointCloud())); 22 | } 23 | 24 | for (int i = 0; i < (int) pc_in->points.size(); i++) 25 | { 26 | int scanID=0; 27 | double distance = sqrt(pc_in->points[i].x * pc_in->points[i].x + pc_in->points[i].y * pc_in->points[i].y); 28 | if(distancelidar_param.max_distance) 29 | continue; 30 | double angle = atan(pc_in->points[i].z / distance) * 180 / M_PI; 31 | 32 | if (N_SCANS == 16) 33 | { 34 | scanID = int((angle + 15) / 2 + 0.5); 35 | if (scanID > (N_SCANS - 1) || scanID < 0) 36 | { 37 | continue; 38 | } 39 | } 40 | else if (N_SCANS == 32) 41 | { 42 | scanID = int((angle + 92.0/3.0) * 3.0 / 4.0); 43 | if (scanID > (N_SCANS - 1) || scanID < 0) 44 | { 45 | continue; 46 | } 47 | } 48 | else if (N_SCANS == 64) 49 | { 50 | if (angle >= -8.83) 51 | scanID = int((2 - angle) * 3.0 + 0.5); 52 | else 53 | scanID = N_SCANS / 2 + int((-8.83 - angle) * 2.0 + 0.5); 54 | 55 | if (angle > 2 || angle < -24.33 || scanID > 63 || scanID < 0) 56 | { 57 | continue; 58 | } 59 | } 60 | else 61 | { 62 | printf("wrong scan number\n"); 63 | } 64 | laserCloudScans[scanID]->push_back(pc_in->points[i]); 65 | 66 | } 67 | 68 | for(int i = 0; i < N_SCANS; i++){ 69 | if(laserCloudScans[i]->points.size()<131){ 70 | continue; 71 | } 72 | 73 | std::vector cloudCurvature; 74 | int total_points = laserCloudScans[i]->points.size()-10; 75 | for(int j = 5; j < (int)laserCloudScans[i]->points.size() - 5; j++){ 76 | double diffX = laserCloudScans[i]->points[j - 5].x + laserCloudScans[i]->points[j - 4].x + laserCloudScans[i]->points[j - 3].x + laserCloudScans[i]->points[j - 2].x + laserCloudScans[i]->points[j - 1].x - 10 * laserCloudScans[i]->points[j].x + laserCloudScans[i]->points[j + 1].x + laserCloudScans[i]->points[j + 2].x + laserCloudScans[i]->points[j + 3].x + laserCloudScans[i]->points[j + 4].x + laserCloudScans[i]->points[j + 5].x; 77 | double diffY = laserCloudScans[i]->points[j - 5].y + laserCloudScans[i]->points[j - 4].y + laserCloudScans[i]->points[j - 3].y + laserCloudScans[i]->points[j - 2].y + laserCloudScans[i]->points[j - 1].y - 10 * laserCloudScans[i]->points[j].y + laserCloudScans[i]->points[j + 1].y + laserCloudScans[i]->points[j + 2].y + laserCloudScans[i]->points[j + 3].y + laserCloudScans[i]->points[j + 4].y + laserCloudScans[i]->points[j + 5].y; 78 | double diffZ = laserCloudScans[i]->points[j - 5].z + laserCloudScans[i]->points[j - 4].z + laserCloudScans[i]->points[j - 3].z + laserCloudScans[i]->points[j - 2].z + laserCloudScans[i]->points[j - 1].z - 10 * laserCloudScans[i]->points[j].z + laserCloudScans[i]->points[j + 1].z + laserCloudScans[i]->points[j + 2].z + laserCloudScans[i]->points[j + 3].z + laserCloudScans[i]->points[j + 4].z + laserCloudScans[i]->points[j + 5].z; 79 | Double2d distance(j,diffX * diffX + diffY * diffY + diffZ * diffZ); 80 | cloudCurvature.push_back(distance); 81 | 82 | } 83 | for(int j=0;j<6;j++){ 84 | int sector_length = (int)(total_points/6); 85 | int sector_start = sector_length *j; 86 | int sector_end = sector_length *(j+1)-1; 87 | if (j==5){ 88 | sector_end = total_points - 1; 89 | } 90 | std::vector subCloudCurvature(cloudCurvature.begin()+sector_start,cloudCurvature.begin()+sector_end); 91 | 92 | featureExtractionFromSector(laserCloudScans[i],subCloudCurvature, pc_out_edge, pc_out_surf); 93 | 94 | } 95 | 96 | } 97 | 98 | } 99 | 100 | 101 | void LaserProcessingClass::featureExtractionFromSector(const pcl::PointCloud::Ptr& pc_in, std::vector& cloudCurvature, pcl::PointCloud::Ptr& pc_out_edge, pcl::PointCloud::Ptr& pc_out_surf){ 102 | 103 | std::sort(cloudCurvature.begin(), cloudCurvature.end(), [](const Double2d & a, const Double2d & b) 104 | { 105 | return a.value < b.value; 106 | }); 107 | 108 | 109 | int largestPickedNum = 0; 110 | std::vector picked_points; 111 | int point_info_count =0; 112 | for (int i = cloudCurvature.size()-1; i >= 0; i--) 113 | { 114 | int ind = cloudCurvature[i].id; 115 | if(std::find(picked_points.begin(), picked_points.end(), ind)==picked_points.end()){ 116 | if(cloudCurvature[i].value <= 0.1){ 117 | break; 118 | } 119 | 120 | largestPickedNum++; 121 | picked_points.push_back(ind); 122 | 123 | if (largestPickedNum <= 20){ 124 | pc_out_edge->push_back(pc_in->points[ind]); 125 | point_info_count++; 126 | }else{ 127 | break; 128 | } 129 | 130 | for(int k=1;k<=5;k++){ 131 | double diffX = pc_in->points[ind + k].x - pc_in->points[ind + k - 1].x; 132 | double diffY = pc_in->points[ind + k].y - pc_in->points[ind + k - 1].y; 133 | double diffZ = pc_in->points[ind + k].z - pc_in->points[ind + k - 1].z; 134 | if (diffX * diffX + diffY * diffY + diffZ * diffZ > 0.05){ 135 | break; 136 | } 137 | picked_points.push_back(ind+k); 138 | } 139 | for(int k=-1;k>=-5;k--){ 140 | double diffX = pc_in->points[ind + k].x - pc_in->points[ind + k + 1].x; 141 | double diffY = pc_in->points[ind + k].y - pc_in->points[ind + k + 1].y; 142 | double diffZ = pc_in->points[ind + k].z - pc_in->points[ind + k + 1].z; 143 | if (diffX * diffX + diffY * diffY + diffZ * diffZ > 0.05){ 144 | break; 145 | } 146 | picked_points.push_back(ind+k); 147 | } 148 | 149 | } 150 | } 151 | 152 | //find flat points 153 | // point_info_count =0; 154 | // int smallestPickedNum = 0; 155 | 156 | // for (int i = 0; i <= (int)cloudCurvature.size()-1; i++) 157 | // { 158 | // int ind = cloudCurvature[i].id; 159 | 160 | // if( std::find(picked_points.begin(), picked_points.end(), ind)==picked_points.end()){ 161 | // if(cloudCurvature[i].value > 0.1){ 162 | // //ROS_WARN("extracted feature not qualified, please check lidar"); 163 | // break; 164 | // } 165 | // smallestPickedNum++; 166 | // picked_points.push_back(ind); 167 | 168 | // if(smallestPickedNum <= 4){ 169 | // //find all points 170 | // pc_surf_flat->push_back(pc_in->points[ind]); 171 | // pc_surf_lessFlat->push_back(pc_in->points[ind]); 172 | // point_info_count++; 173 | // } 174 | // else{ 175 | // break; 176 | // } 177 | 178 | // for(int k=1;k<=5;k++){ 179 | // double diffX = pc_in->points[ind + k].x - pc_in->points[ind + k - 1].x; 180 | // double diffY = pc_in->points[ind + k].y - pc_in->points[ind + k - 1].y; 181 | // double diffZ = pc_in->points[ind + k].z - pc_in->points[ind + k - 1].z; 182 | // if (diffX * diffX + diffY * diffY + diffZ * diffZ > 0.05){ 183 | // break; 184 | // } 185 | // picked_points.push_back(ind+k); 186 | // } 187 | // for(int k=-1;k>=-5;k--){ 188 | // double diffX = pc_in->points[ind + k].x - pc_in->points[ind + k + 1].x; 189 | // double diffY = pc_in->points[ind + k].y - pc_in->points[ind + k + 1].y; 190 | // double diffZ = pc_in->points[ind + k].z - pc_in->points[ind + k + 1].z; 191 | // if (diffX * diffX + diffY * diffY + diffZ * diffZ > 0.05){ 192 | // break; 193 | // } 194 | // picked_points.push_back(ind+k); 195 | // } 196 | 197 | // } 198 | // } 199 | 200 | for (int i = 0; i <= (int)cloudCurvature.size()-1; i++) 201 | { 202 | int ind = cloudCurvature[i].id; 203 | if( std::find(picked_points.begin(), picked_points.end(), ind)==picked_points.end()) 204 | { 205 | pc_out_surf->push_back(pc_in->points[ind]); 206 | } 207 | } 208 | 209 | 210 | 211 | } 212 | LaserProcessingClass::LaserProcessingClass(){ 213 | 214 | } 215 | 216 | Double2d::Double2d(int id_in, double value_in){ 217 | id = id_in; 218 | value =value_in; 219 | }; 220 | 221 | PointsInfo::PointsInfo(int layer_in, double time_in){ 222 | layer = layer_in; 223 | time = time_in; 224 | }; 225 | -------------------------------------------------------------------------------- /amcl3d/src/floam/src/laserProcessingNode.cpp: -------------------------------------------------------------------------------- 1 | // Author of FLOAM: Wang Han 2 | // Email wh200720041@gmail.com 3 | // Homepage https://wanghan.pro 4 | 5 | //c++ lib 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | 13 | //ros lib 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | 21 | //pcl lib 22 | #include 23 | #include 24 | #include 25 | 26 | //local lib 27 | #include "lidar.h" 28 | #include "laserProcessingClass.h" 29 | 30 | 31 | LaserProcessingClass laserProcessing; 32 | std::mutex mutex_lock; 33 | std::queue pointCloudBuf; 34 | lidar::Lidar lidar_param; 35 | 36 | ros::Publisher pubEdgePoints; 37 | ros::Publisher pubSurfPoints; 38 | ros::Publisher pubLaserCloudFiltered; 39 | 40 | void velodyneHandler(const sensor_msgs::PointCloud2ConstPtr &laserCloudMsg) 41 | { 42 | mutex_lock.lock(); 43 | pointCloudBuf.push(laserCloudMsg); 44 | mutex_lock.unlock(); 45 | 46 | } 47 | 48 | double total_time =0; 49 | int total_frame=0; 50 | 51 | void laser_processing(){ 52 | while(1){ 53 | if(!pointCloudBuf.empty()){ 54 | //read data 55 | mutex_lock.lock(); 56 | pcl::PointCloud::Ptr pointcloud_in(new pcl::PointCloud()); 57 | pcl::fromROSMsg(*pointCloudBuf.front(), *pointcloud_in); 58 | ros::Time pointcloud_time = (pointCloudBuf.front())->header.stamp; 59 | pointCloudBuf.pop(); 60 | mutex_lock.unlock(); 61 | 62 | pcl::PointCloud::Ptr pointcloud_edge(new pcl::PointCloud()); 63 | pcl::PointCloud::Ptr pointcloud_surf(new pcl::PointCloud()); 64 | 65 | std::chrono::time_point start, end; 66 | start = std::chrono::system_clock::now(); 67 | laserProcessing.featureExtraction(pointcloud_in,pointcloud_edge,pointcloud_surf); 68 | end = std::chrono::system_clock::now(); 69 | std::chrono::duration elapsed_seconds = end - start; 70 | total_frame++; 71 | float time_temp = elapsed_seconds.count() * 1000; 72 | total_time+=time_temp; 73 | //ROS_INFO("average laser processing time %f ms \n \n", total_time/total_frame); 74 | 75 | sensor_msgs::PointCloud2 laserCloudFilteredMsg; 76 | pcl::PointCloud::Ptr pointcloud_filtered(new pcl::PointCloud()); 77 | *pointcloud_filtered+=*pointcloud_edge; 78 | *pointcloud_filtered+=*pointcloud_surf; 79 | pcl::toROSMsg(*pointcloud_filtered, laserCloudFilteredMsg); 80 | laserCloudFilteredMsg.header.stamp = pointcloud_time; 81 | laserCloudFilteredMsg.header.frame_id = "/velodyne"; 82 | pubLaserCloudFiltered.publish(laserCloudFilteredMsg); 83 | 84 | sensor_msgs::PointCloud2 edgePointsMsg; 85 | pcl::toROSMsg(*pointcloud_edge, edgePointsMsg); 86 | edgePointsMsg.header.stamp = pointcloud_time; 87 | edgePointsMsg.header.frame_id = "/velodyne"; 88 | pubEdgePoints.publish(edgePointsMsg); 89 | 90 | 91 | sensor_msgs::PointCloud2 surfPointsMsg; 92 | pcl::toROSMsg(*pointcloud_surf, surfPointsMsg); 93 | surfPointsMsg.header.stamp = pointcloud_time; 94 | surfPointsMsg.header.frame_id = "/velodyne"; 95 | pubSurfPoints.publish(surfPointsMsg); 96 | 97 | } 98 | //sleep 2 ms every time 99 | std::chrono::milliseconds dura(2); 100 | std::this_thread::sleep_for(dura); 101 | } 102 | } 103 | 104 | int main(int argc, char **argv) 105 | { 106 | ros::init(argc, argv, "amcl3d_node"); 107 | ros::NodeHandle nh; 108 | ROS_INFO("\033[1;32m---->\033[0m floam_laserProcessing_amcl3d initialization."); 109 | int scan_line = 64; 110 | double vertical_angle = 2.0; 111 | double scan_period= 0.1; 112 | double max_dis = 60.0; 113 | double min_dis = 2.0; 114 | 115 | nh.getParam("/scan_period", scan_period); 116 | nh.getParam("/vertical_angle", vertical_angle); 117 | nh.getParam("/max_dis", max_dis); 118 | nh.getParam("/min_dis", min_dis); 119 | nh.getParam("/scan_line", scan_line); 120 | 121 | lidar_param.setScanPeriod(scan_period); 122 | lidar_param.setVerticalAngle(vertical_angle); 123 | lidar_param.setLines(scan_line); 124 | lidar_param.setMaxDistance(max_dis); 125 | lidar_param.setMinDistance(min_dis); 126 | 127 | laserProcessing.init(lidar_param); 128 | 129 | ros::Subscriber subLaserCloud = nh.subscribe("/velodyne_points", 100, velodyneHandler); 130 | 131 | pubLaserCloudFiltered = nh.advertise("/velodyne_points_filtered", 100); 132 | 133 | pubEdgePoints = nh.advertise("/laser_cloud_edge", 100); 134 | 135 | pubSurfPoints = nh.advertise("/laser_cloud_surf", 100); 136 | 137 | std::thread laser_processing_process{laser_processing}; 138 | 139 | ros::spin(); 140 | 141 | return 0; 142 | } 143 | -------------------------------------------------------------------------------- /amcl3d/src/floam/src/lidar.cpp: -------------------------------------------------------------------------------- 1 | // Author of FLOAM: Wang Han 2 | // Email wh200720041@gmail.com 3 | // Homepage https://wanghan.pro 4 | 5 | #include "lidar.h" 6 | 7 | 8 | lidar::Lidar::Lidar(){ 9 | 10 | } 11 | 12 | 13 | void lidar::Lidar::setLines(double num_lines_in){ 14 | num_lines=num_lines_in; 15 | } 16 | 17 | 18 | void lidar::Lidar::setVerticalAngle(double vertical_angle_in){ 19 | vertical_angle = vertical_angle_in; 20 | } 21 | 22 | 23 | void lidar::Lidar::setVerticalResolution(double vertical_angle_resolution_in){ 24 | vertical_angle_resolution = vertical_angle_resolution_in; 25 | } 26 | 27 | 28 | void lidar::Lidar::setScanPeriod(double scan_period_in){ 29 | scan_period = scan_period_in; 30 | } 31 | 32 | 33 | void lidar::Lidar::setMaxDistance(double max_distance_in){ 34 | max_distance = max_distance_in; 35 | } 36 | 37 | void lidar::Lidar::setMinDistance(double min_distance_in){ 38 | min_distance = min_distance_in; 39 | } -------------------------------------------------------------------------------- /amcl3d/src/floam/src/lidarOptimization.cpp: -------------------------------------------------------------------------------- 1 | // Author of FLOAM: Wang Han 2 | // Email wh200720041@gmail.com 3 | // Homepage https://wanghan.pro 4 | 5 | #include "lidarOptimization.h" 6 | 7 | EdgeAnalyticCostFunction::EdgeAnalyticCostFunction(Eigen::Vector3d curr_point_, Eigen::Vector3d last_point_a_, Eigen::Vector3d last_point_b_) 8 | : curr_point(curr_point_), last_point_a(last_point_a_), last_point_b(last_point_b_){ 9 | 10 | } 11 | 12 | bool EdgeAnalyticCostFunction::Evaluate(double const *const *parameters, double *residuals, double **jacobians) const 13 | { 14 | 15 | Eigen::Map q_last_curr(parameters[0]); 16 | Eigen::Map t_last_curr(parameters[0] + 4); 17 | Eigen::Vector3d lp; 18 | lp = q_last_curr * curr_point + t_last_curr; //new point 19 | Eigen::Vector3d nu = (lp - last_point_a).cross(lp - last_point_b); 20 | Eigen::Vector3d de = last_point_a - last_point_b; 21 | 22 | residuals[0] = nu.x() / de.norm(); 23 | residuals[1] = nu.y() / de.norm(); 24 | residuals[2] = nu.z() / de.norm(); 25 | 26 | if(jacobians != NULL) 27 | { 28 | if(jacobians[0] != NULL) 29 | { 30 | Eigen::Matrix3d skew_lp = skew(lp); 31 | Eigen::Matrix dp_by_so3; 32 | dp_by_so3.block<3,3>(0,0) = -skew_lp; 33 | (dp_by_so3.block<3,3>(0, 3)).setIdentity(); 34 | Eigen::Map > J_se3(jacobians[0]); 35 | J_se3.setZero(); 36 | Eigen::Vector3d re = last_point_b - last_point_a; 37 | Eigen::Matrix3d skew_re = skew(re); 38 | 39 | J_se3.block<3,6>(0,0) = skew_re * dp_by_so3/de.norm(); 40 | 41 | } 42 | } 43 | 44 | return true; 45 | 46 | } 47 | 48 | //surf norm cost 49 | 50 | SurfNormAnalyticCostFunction::SurfNormAnalyticCostFunction(Eigen::Vector3d curr_point_, Eigen::Vector3d plane_unit_norm_, double negative_OA_dot_norm_) 51 | : curr_point(curr_point_), plane_unit_norm(plane_unit_norm_), negative_OA_dot_norm(negative_OA_dot_norm_) { 52 | 53 | } 54 | 55 | bool SurfNormAnalyticCostFunction::Evaluate(double const *const *parameters, double *residuals, double **jacobians) const 56 | { 57 | Eigen::Map q_w_curr(parameters[0]); 58 | Eigen::Map t_w_curr(parameters[0] + 4); 59 | Eigen::Vector3d point_w = q_w_curr * curr_point + t_w_curr; 60 | 61 | residuals[0] = plane_unit_norm.dot(point_w) + negative_OA_dot_norm; 62 | 63 | if(jacobians != NULL) 64 | { 65 | if(jacobians[0] != NULL) 66 | { 67 | Eigen::Matrix3d skew_point_w = skew(point_w); 68 | 69 | Eigen::Matrix dp_by_so3; 70 | dp_by_so3.block<3,3>(0,0) = -skew_point_w; 71 | (dp_by_so3.block<3,3>(0, 3)).setIdentity(); 72 | Eigen::Map > J_se3(jacobians[0]); 73 | J_se3.setZero(); 74 | J_se3.block<1,6>(0,0) = plane_unit_norm.transpose() * dp_by_so3; 75 | 76 | } 77 | } 78 | return true; 79 | 80 | } 81 | 82 | 83 | bool PoseSE3Parameterization::Plus(const double *x, const double *delta, double *x_plus_delta) const 84 | { 85 | Eigen::Map trans(x + 4); 86 | 87 | Eigen::Quaterniond delta_q; 88 | Eigen::Vector3d delta_t; 89 | getTransformFromSe3(Eigen::Map>(delta), delta_q, delta_t); 90 | Eigen::Map quater(x); 91 | Eigen::Map quater_plus(x_plus_delta); 92 | Eigen::Map trans_plus(x_plus_delta + 4); 93 | 94 | quater_plus = delta_q * quater; 95 | trans_plus = delta_q * trans + delta_t; 96 | 97 | return true; 98 | } 99 | 100 | 101 | bool PoseSE3Parameterization::ComputeJacobian(const double *x, double *jacobian) const 102 | { 103 | Eigen::Map> j(jacobian); 104 | (j.topRows(6)).setIdentity(); 105 | (j.bottomRows(1)).setZero(); 106 | 107 | return true; 108 | } 109 | 110 | 111 | void getTransformFromSe3(const Eigen::Matrix& se3, Eigen::Quaterniond& q, Eigen::Vector3d& t){ 112 | Eigen::Vector3d omega(se3.data()); 113 | Eigen::Vector3d upsilon(se3.data()+3); 114 | Eigen::Matrix3d Omega = skew(omega); 115 | 116 | double theta = omega.norm(); 117 | double half_theta = 0.5*theta; 118 | 119 | double imag_factor; 120 | double real_factor = cos(half_theta); 121 | if(theta<1e-10) 122 | { 123 | double theta_sq = theta*theta; 124 | double theta_po4 = theta_sq*theta_sq; 125 | imag_factor = 0.5-0.0208333*theta_sq+0.000260417*theta_po4; 126 | } 127 | else 128 | { 129 | double sin_half_theta = sin(half_theta); 130 | imag_factor = sin_half_theta/theta; 131 | } 132 | 133 | q = Eigen::Quaterniond(real_factor, imag_factor*omega.x(), imag_factor*omega.y(), imag_factor*omega.z()); 134 | 135 | 136 | Eigen::Matrix3d J; 137 | if (theta<1e-10) 138 | { 139 | J = q.matrix(); 140 | } 141 | else 142 | { 143 | Eigen::Matrix3d Omega2 = Omega*Omega; 144 | J = (Eigen::Matrix3d::Identity() + (1-cos(theta))/(theta*theta)*Omega + (theta-sin(theta))/(pow(theta,3))*Omega2); 145 | } 146 | 147 | t = J*upsilon; 148 | } 149 | 150 | Eigen::Matrix skew(Eigen::Matrix& mat_in){ 151 | Eigen::Matrix skew_mat; 152 | skew_mat.setZero(); 153 | skew_mat(0,1) = -mat_in(2); 154 | skew_mat(0,2) = mat_in(1); 155 | skew_mat(1,2) = -mat_in(0); 156 | skew_mat(1,0) = mat_in(2); 157 | skew_mat(2,0) = -mat_in(1); 158 | skew_mat(2,1) = mat_in(0); 159 | return skew_mat; 160 | } 161 | -------------------------------------------------------------------------------- /amcl3d/src/floam/src/odomEstimationClass.cpp: -------------------------------------------------------------------------------- 1 | // Author of FLOAM: Wang Han 2 | // Email wh200720041@gmail.com 3 | // Homepage https://wanghan.pro 4 | 5 | #include "odomEstimationClass.h" 6 | 7 | void OdomEstimationClass::init(lidar::Lidar lidar_param){ 8 | //init local map 9 | laserCloudCornerMap = pcl::PointCloud::Ptr(new pcl::PointCloud()); 10 | laserCloudSurfMap = pcl::PointCloud::Ptr(new pcl::PointCloud()); 11 | 12 | //downsampling size 13 | downSizeFilterEdge.setLeafSize(0.4, 0.4, 0.4); 14 | downSizeFilterSurf.setLeafSize(0.8, 0.8, 0.8); 15 | 16 | //kd-tree 17 | kdtreeEdgeMap = pcl::KdTreeFLANN::Ptr(new pcl::KdTreeFLANN()); 18 | kdtreeSurfMap = pcl::KdTreeFLANN::Ptr(new pcl::KdTreeFLANN()); 19 | 20 | odom = Eigen::Isometry3d::Identity(); 21 | last_odom = Eigen::Isometry3d::Identity(); 22 | optimization_count=2; 23 | queueSize = 20; 24 | } 25 | 26 | void OdomEstimationClass::initMapWithPoints(const pcl::PointCloud::Ptr& edge_in, const pcl::PointCloud::Ptr& surf_in){ 27 | *laserCloudCornerMap += *edge_in; 28 | *laserCloudSurfMap += *surf_in; 29 | pointCloudCornerQueue.push(*edge_in); 30 | pointCloudSurfQueue.push(*surf_in); 31 | optimization_count=12; 32 | } 33 | 34 | 35 | void OdomEstimationClass::updatePointsToMap(const pcl::PointCloud::Ptr& edge_in, const pcl::PointCloud::Ptr& surf_in){ 36 | 37 | if(optimization_count>2) 38 | optimization_count--; 39 | 40 | Eigen::Isometry3d odom_prediction = odom * (last_odom.inverse() * odom); 41 | last_odom = odom; 42 | odom = odom_prediction; 43 | 44 | q_w_curr = Eigen::Quaterniond(odom.rotation()); 45 | t_w_curr = odom.translation(); 46 | 47 | pcl::PointCloud::Ptr downsampledEdgeCloud(new pcl::PointCloud()); 48 | pcl::PointCloud::Ptr downsampledSurfCloud(new pcl::PointCloud()); 49 | downSamplingToMap(edge_in,downsampledEdgeCloud,surf_in,downsampledSurfCloud); 50 | //ROS_WARN("point nyum%d,%d",(int)downsampledEdgeCloud->points.size(), (int)downsampledSurfCloud->points.size()); 51 | if(laserCloudCornerMap->points.size()>10 && laserCloudSurfMap->points.size()>50){ 52 | kdtreeEdgeMap->setInputCloud(laserCloudCornerMap); 53 | kdtreeSurfMap->setInputCloud(laserCloudSurfMap); 54 | 55 | for (int iterCount = 0; iterCount < optimization_count; iterCount++){ 56 | ceres::LossFunction *loss_function = new ceres::HuberLoss(0.1); 57 | ceres::Problem::Options problem_options; 58 | ceres::Problem problem(problem_options); 59 | 60 | problem.AddParameterBlock(parameters, 7, new PoseSE3Parameterization()); 61 | 62 | addEdgeCostFactor(downsampledEdgeCloud,laserCloudCornerMap,problem,loss_function); 63 | addSurfCostFactor(downsampledSurfCloud,laserCloudSurfMap,problem,loss_function); 64 | 65 | ceres::Solver::Options options; 66 | options.linear_solver_type = ceres::DENSE_QR; 67 | options.max_num_iterations = 4; 68 | options.minimizer_progress_to_stdout = false; 69 | options.check_gradients = false; 70 | options.gradient_check_relative_precision = 1e-4; 71 | ceres::Solver::Summary summary; 72 | 73 | ceres::Solve(options, &problem, &summary); 74 | 75 | } 76 | }else{ 77 | printf("not enough points in map to associate, map error"); 78 | } 79 | odom = Eigen::Isometry3d::Identity(); 80 | odom.linear() = q_w_curr.toRotationMatrix(); 81 | odom.translation() = t_w_curr; 82 | addPointsToMap(downsampledEdgeCloud,downsampledSurfCloud); 83 | 84 | } 85 | 86 | void OdomEstimationClass::pointAssociateToMap(pcl::PointXYZI const *const pi, pcl::PointXYZI *const po) 87 | { 88 | Eigen::Vector3d point_curr(pi->x, pi->y, pi->z); 89 | Eigen::Vector3d point_w = q_w_curr * point_curr + t_w_curr; 90 | po->x = point_w.x(); 91 | po->y = point_w.y(); 92 | po->z = point_w.z(); 93 | po->intensity = pi->intensity; 94 | //po->intensity = 1.0; 95 | } 96 | 97 | void OdomEstimationClass::downSamplingToMap(const pcl::PointCloud::Ptr& edge_pc_in, pcl::PointCloud::Ptr& edge_pc_out, const pcl::PointCloud::Ptr& surf_pc_in, pcl::PointCloud::Ptr& surf_pc_out){ 98 | downSizeFilterEdge.setInputCloud(edge_pc_in); 99 | downSizeFilterEdge.filter(*edge_pc_out); 100 | downSizeFilterSurf.setInputCloud(surf_pc_in); 101 | downSizeFilterSurf.filter(*surf_pc_out); 102 | } 103 | 104 | void OdomEstimationClass::addEdgeCostFactor(const pcl::PointCloud::Ptr& pc_in, const pcl::PointCloud::Ptr& map_in, ceres::Problem& problem, ceres::LossFunction *loss_function){ 105 | int corner_num=0; 106 | for (int i = 0; i < (int)pc_in->points.size(); i++) 107 | { 108 | pcl::PointXYZI point_temp; 109 | pointAssociateToMap(&(pc_in->points[i]), &point_temp); 110 | 111 | std::vector pointSearchInd; 112 | std::vector pointSearchSqDis; 113 | kdtreeEdgeMap->nearestKSearch(point_temp, 5, pointSearchInd, pointSearchSqDis); 114 | if (pointSearchSqDis[4] < 1.0) 115 | { 116 | std::vector nearCorners; 117 | Eigen::Vector3d center(0, 0, 0); 118 | for (int j = 0; j < 5; j++) 119 | { 120 | Eigen::Vector3d tmp(map_in->points[pointSearchInd[j]].x, 121 | map_in->points[pointSearchInd[j]].y, 122 | map_in->points[pointSearchInd[j]].z); 123 | center = center + tmp; 124 | nearCorners.push_back(tmp); 125 | } 126 | center = center / 5.0; 127 | 128 | Eigen::Matrix3d covMat = Eigen::Matrix3d::Zero(); 129 | for (int j = 0; j < 5; j++) 130 | { 131 | Eigen::Matrix tmpZeroMean = nearCorners[j] - center; 132 | covMat = covMat + tmpZeroMean * tmpZeroMean.transpose(); 133 | } 134 | 135 | Eigen::SelfAdjointEigenSolver saes(covMat); 136 | 137 | Eigen::Vector3d unit_direction = saes.eigenvectors().col(2); 138 | Eigen::Vector3d curr_point(pc_in->points[i].x, pc_in->points[i].y, pc_in->points[i].z); 139 | if (saes.eigenvalues()[2] > 3 * saes.eigenvalues()[1]) 140 | { 141 | Eigen::Vector3d point_on_line = center; 142 | Eigen::Vector3d point_a, point_b; 143 | point_a = 0.1 * unit_direction + point_on_line; 144 | point_b = -0.1 * unit_direction + point_on_line; 145 | 146 | ceres::CostFunction *cost_function = new EdgeAnalyticCostFunction(curr_point, point_a, point_b); 147 | problem.AddResidualBlock(cost_function, loss_function, parameters); 148 | corner_num++; 149 | } 150 | } 151 | } 152 | if(corner_num<20){ 153 | printf("not enough correct points"); 154 | } 155 | 156 | } 157 | 158 | void OdomEstimationClass::addSurfCostFactor(const pcl::PointCloud::Ptr& pc_in, const pcl::PointCloud::Ptr& map_in, ceres::Problem& problem, ceres::LossFunction *loss_function){ 159 | int surf_num=0; 160 | for (int i = 0; i < (int)pc_in->points.size(); i++) 161 | { 162 | pcl::PointXYZI point_temp; 163 | pointAssociateToMap(&(pc_in->points[i]), &point_temp); 164 | std::vector pointSearchInd; 165 | std::vector pointSearchSqDis; 166 | kdtreeSurfMap->nearestKSearch(point_temp, 5, pointSearchInd, pointSearchSqDis); 167 | 168 | Eigen::Matrix matA0; 169 | Eigen::Matrix matB0 = -1 * Eigen::Matrix::Ones(); 170 | if (pointSearchSqDis[4] < 1.0) 171 | { 172 | 173 | for (int j = 0; j < 5; j++) 174 | { 175 | matA0(j, 0) = map_in->points[pointSearchInd[j]].x; 176 | matA0(j, 1) = map_in->points[pointSearchInd[j]].y; 177 | matA0(j, 2) = map_in->points[pointSearchInd[j]].z; 178 | } 179 | // find the norm of plane 180 | Eigen::Vector3d norm = matA0.colPivHouseholderQr().solve(matB0); 181 | double negative_OA_dot_norm = 1 / norm.norm(); 182 | norm.normalize(); 183 | 184 | bool planeValid = true; 185 | for (int j = 0; j < 5; j++) 186 | { 187 | // if OX * n > 0.2, then plane is not fit well 188 | if (fabs(norm(0) * map_in->points[pointSearchInd[j]].x + 189 | norm(1) * map_in->points[pointSearchInd[j]].y + 190 | norm(2) * map_in->points[pointSearchInd[j]].z + negative_OA_dot_norm) > 0.2) 191 | { 192 | planeValid = false; 193 | break; 194 | } 195 | } 196 | Eigen::Vector3d curr_point(pc_in->points[i].x, pc_in->points[i].y, pc_in->points[i].z); 197 | if (planeValid) 198 | { 199 | ceres::CostFunction *cost_function = new SurfNormAnalyticCostFunction(curr_point, norm, negative_OA_dot_norm); 200 | problem.AddResidualBlock(cost_function, loss_function, parameters); 201 | 202 | surf_num++; 203 | } 204 | } 205 | 206 | } 207 | if(surf_num<20){ 208 | printf("not enough correct points"); 209 | } 210 | 211 | } 212 | 213 | void OdomEstimationClass::addPointsToMap(const pcl::PointCloud::Ptr& downsampledEdgeCloud, const pcl::PointCloud::Ptr& downsampledSurfCloud){ 214 | //储存点 215 | pcl::PointCloud::Ptr tempEdgeCloud(new pcl::PointCloud); 216 | pcl::PointCloud::Ptr tempSurfCloud(new pcl::PointCloud); 217 | for (int i = 0; i < (int)downsampledEdgeCloud->points.size(); i++) 218 | { 219 | pcl::PointXYZI point_temp; 220 | pointAssociateToMap(&downsampledEdgeCloud->points[i], &point_temp); 221 | //laserCloudCornerMap->push_back(point_temp); 222 | tempEdgeCloud->push_back(point_temp); 223 | } 224 | 225 | for (int i = 0; i < (int)downsampledSurfCloud->points.size(); i++) 226 | { 227 | pcl::PointXYZI point_temp; 228 | pointAssociateToMap(&downsampledSurfCloud->points[i], &point_temp); 229 | //laserCloudSurfMap->push_back(point_temp); 230 | tempSurfCloud->push_back(point_temp); 231 | } 232 | if(pointCloudCornerQueue.size()>queueSize) 233 | { 234 | pointCloudCornerQueue.pop(); 235 | pointCloudSurfQueue.pop(); 236 | } 237 | pointCloudSurfQueue.push(*tempSurfCloud); 238 | pointCloudCornerQueue.push(*tempEdgeCloud); 239 | 240 | 241 | double x_min = +odom.translation().x()-100; 242 | double y_min = +odom.translation().y()-100; 243 | double z_min = +odom.translation().z()-100; 244 | double x_max = +odom.translation().x()+100; 245 | double y_max = +odom.translation().y()+100; 246 | double z_max = +odom.translation().z()+100; 247 | 248 | //ROS_INFO("size : %f,%f,%f,%f,%f,%f", x_min, y_min, z_min,x_max, y_max, z_max); 249 | cropBoxFilter.setMin(Eigen::Vector4f(x_min, y_min, z_min, 1.0)); 250 | cropBoxFilter.setMax(Eigen::Vector4f(x_max, y_max, z_max, 1.0)); 251 | cropBoxFilter.setNegative(false); 252 | 253 | laserCloudSurfMap->clear(); 254 | laserCloudCornerMap->clear(); 255 | { 256 | int qsize = pointCloudCornerQueue.size(); 257 | for(int i = 0;i < qsize;i++) 258 | { 259 | *laserCloudCornerMap += pointCloudCornerQueue.front(); 260 | *laserCloudSurfMap += pointCloudSurfQueue.front(); 261 | pointCloudCornerQueue.push(pointCloudCornerQueue.front()); 262 | pointCloudSurfQueue.push(pointCloudSurfQueue.front()); 263 | pointCloudCornerQueue.pop(); 264 | pointCloudSurfQueue.pop(); 265 | } 266 | } 267 | 268 | pcl::PointCloud::Ptr tmpCorner(new pcl::PointCloud()); 269 | pcl::PointCloud::Ptr tmpSurf(new pcl::PointCloud()); 270 | cropBoxFilter.setInputCloud(laserCloudSurfMap); 271 | cropBoxFilter.filter(*tmpSurf); 272 | cropBoxFilter.setInputCloud(laserCloudCornerMap); 273 | cropBoxFilter.filter(*tmpCorner); 274 | 275 | downSizeFilterEdge.setInputCloud(tmpSurf); 276 | downSizeFilterEdge.filter(*laserCloudSurfMap); 277 | downSizeFilterSurf.setInputCloud(tmpCorner); 278 | downSizeFilterSurf.filter(*laserCloudCornerMap); 279 | 280 | } 281 | 282 | void OdomEstimationClass::getMap(pcl::PointCloud::Ptr& laserCloudMap){ 283 | 284 | *laserCloudMap += *laserCloudSurfMap; 285 | *laserCloudMap += *laserCloudCornerMap; 286 | } 287 | 288 | OdomEstimationClass::OdomEstimationClass(){ 289 | 290 | } -------------------------------------------------------------------------------- /amcl3d/src/floam/src/odomEstimationNode.cpp: -------------------------------------------------------------------------------- 1 | // Author of FLOAM: Wang Han 2 | // Email wh200720041@gmail.com 3 | // Homepage https://wanghan.pro 4 | 5 | //c++ lib 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | 13 | //ros lib 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | 20 | //pcl lib 21 | #include 22 | #include 23 | #include 24 | 25 | //local lib 26 | #include "lidar.h" 27 | #include "odomEstimationClass.h" 28 | 29 | OdomEstimationClass odomEstimation; 30 | std::mutex mutex_lock; 31 | std::queue pointCloudEdgeBuf; 32 | std::queue pointCloudSurfBuf; 33 | std::queue pointCloudBuf; 34 | lidar::Lidar lidar_param; 35 | 36 | ros::Publisher pubLaserOdometry; 37 | ros::Publisher pubLaserSensor; 38 | ros::Publisher map_cloud_pub_; 39 | void velodyneSurfHandler(const sensor_msgs::PointCloud2ConstPtr &laserCloudMsg) 40 | { 41 | mutex_lock.lock(); 42 | pointCloudSurfBuf.push(laserCloudMsg); 43 | mutex_lock.unlock(); 44 | } 45 | void velodyneEdgeHandler(const sensor_msgs::PointCloud2ConstPtr &laserCloudMsg) 46 | { 47 | mutex_lock.lock(); 48 | pointCloudEdgeBuf.push(laserCloudMsg); 49 | mutex_lock.unlock(); 50 | } 51 | void velodyneHandler(const sensor_msgs::PointCloud2ConstPtr &laserCloudMsg) 52 | { 53 | mutex_lock.lock(); 54 | pointCloudBuf.push(laserCloudMsg); 55 | mutex_lock.unlock(); 56 | } 57 | 58 | bool is_odom_inited = false; 59 | double total_time =0; 60 | int total_frame=0; 61 | void odom_estimation(){ 62 | while(1){ 63 | if(!pointCloudEdgeBuf.empty() && !pointCloudSurfBuf.empty()&& !pointCloudBuf.empty()){ 64 | 65 | //read data 66 | mutex_lock.lock(); 67 | if(!pointCloudBuf.empty() && (pointCloudBuf.front()->header.stamp.toSec()header.stamp.toSec()-0.5*lidar_param.scan_period || pointCloudBuf.front()->header.stamp.toSec()header.stamp.toSec()-0.5*lidar_param.scan_period)){ 68 | ROS_WARN("time stamp unaligned error and odom discarded, pls check your data --> odom correction"); 69 | pointCloudBuf.pop(); 70 | mutex_lock.unlock(); 71 | continue; 72 | } 73 | 74 | if(!pointCloudSurfBuf.empty() && (pointCloudSurfBuf.front()->header.stamp.toSec()header.stamp.toSec()-0.5*lidar_param.scan_period || pointCloudSurfBuf.front()->header.stamp.toSec()header.stamp.toSec()-0.5*lidar_param.scan_period)){ 75 | pointCloudSurfBuf.pop(); 76 | ROS_INFO("time stamp unaligned with extra point cloud, pls check your data --> odom correction"); 77 | mutex_lock.unlock(); 78 | continue; 79 | } 80 | 81 | if(!pointCloudEdgeBuf.empty() && (pointCloudEdgeBuf.front()->header.stamp.toSec()header.stamp.toSec()-0.5*lidar_param.scan_period || pointCloudEdgeBuf.front()->header.stamp.toSec()header.stamp.toSec()-0.5*lidar_param.scan_period)){ 82 | pointCloudEdgeBuf.pop(); 83 | ROS_INFO("time stamp unaligned with extra point cloud, pls check your data --> odom correction"); 84 | mutex_lock.unlock(); 85 | continue; 86 | } 87 | //if time aligned 88 | 89 | pcl::PointCloud::Ptr pointcloud_surf_in(new pcl::PointCloud()); 90 | pcl::PointCloud::Ptr pointcloud_edge_in(new pcl::PointCloud()); 91 | pcl::PointCloud::Ptr pointcloud_in(new pcl::PointCloud()); 92 | pcl::fromROSMsg(*pointCloudEdgeBuf.front(), *pointcloud_edge_in); 93 | pcl::fromROSMsg(*pointCloudSurfBuf.front(), *pointcloud_surf_in); 94 | pcl::fromROSMsg(*pointCloudBuf.front(), *pointcloud_in); 95 | ros::Time pointcloud_time = (pointCloudBuf.front())->header.stamp; 96 | pointCloudEdgeBuf.pop(); 97 | pointCloudSurfBuf.pop(); 98 | pointCloudBuf.pop(); 99 | mutex_lock.unlock(); 100 | 101 | if(is_odom_inited == false){ 102 | odomEstimation.initMapWithPoints(pointcloud_edge_in, pointcloud_surf_in); 103 | is_odom_inited = true; 104 | ROS_INFO("odom inited"); 105 | }else{ 106 | std::chrono::time_point start, end; 107 | start = std::chrono::system_clock::now(); 108 | odomEstimation.updatePointsToMap(pointcloud_edge_in, pointcloud_surf_in); 109 | end = std::chrono::system_clock::now(); 110 | std::chrono::duration elapsed_seconds = end - start; 111 | total_frame++; 112 | float time_temp = elapsed_seconds.count() * 1000; 113 | total_time+=time_temp; 114 | //ROS_INFO("takes: %f ms, average odom estimation time %f ms \n \n",time_temp, total_time/total_frame); 115 | } 116 | 117 | 118 | 119 | Eigen::Quaterniond q_current(odomEstimation.odom.rotation()); 120 | //q_current.normalize(); 121 | Eigen::Vector3d t_current = odomEstimation.odom.translation(); 122 | 123 | static tf::TransformBroadcaster br; 124 | tf::Transform transform; 125 | transform.setOrigin( tf::Vector3(t_current.x(), t_current.y(), t_current.z()) ); 126 | tf::Quaternion q(q_current.x(),q_current.y(),q_current.z(),q_current.w()); 127 | transform.setRotation(q); 128 | br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "map", "aft_mapped")); 129 | 130 | sensor_msgs::PointCloud2 laserSensorMsg; 131 | pcl::toROSMsg(*pointcloud_in, laserSensorMsg); 132 | laserSensorMsg.header.stamp = pointcloud_time; 133 | laserSensorMsg.header.frame_id = "/velodyne"; 134 | pubLaserSensor.publish(laserSensorMsg); 135 | 136 | // publish odometry 137 | nav_msgs::Odometry laserOdometry; 138 | laserOdometry.header.frame_id = "/world"; 139 | laserOdometry.child_frame_id = "/aft_mapped"; 140 | laserOdometry.header.stamp = pointcloud_time; 141 | laserOdometry.pose.pose.orientation.x = q_current.x(); 142 | laserOdometry.pose.pose.orientation.y = q_current.y(); 143 | laserOdometry.pose.pose.orientation.z = q_current.z(); 144 | laserOdometry.pose.pose.orientation.w = q_current.w(); 145 | laserOdometry.pose.pose.position.x = t_current.x(); 146 | laserOdometry.pose.pose.position.y = t_current.y(); 147 | laserOdometry.pose.pose.position.z = t_current.z(); 148 | pubLaserOdometry.publish(laserOdometry); 149 | 150 | } 151 | //sleep 2 ms every time 152 | std::chrono::milliseconds dura(2); 153 | std::this_thread::sleep_for(dura); 154 | } 155 | } 156 | 157 | void map_pub() 158 | { 159 | while(1) 160 | { 161 | pcl::PointCloud::Ptr laserCloudMap(new pcl::PointCloud); 162 | odomEstimation.getMap(laserCloudMap); 163 | sensor_msgs::PointCloud2 msg; 164 | pcl::toROSMsg(*laserCloudMap,msg); 165 | msg.header.frame_id = "/world"; 166 | msg.header.stamp = ros::Time::now(); 167 | map_cloud_pub_.publish(msg); 168 | std::chrono::milliseconds dura(100); 169 | std::this_thread::sleep_for(dura); 170 | } 171 | } 172 | 173 | int main(int argc, char **argv) 174 | { 175 | ros::init(argc, argv, "amcl3d_node"); 176 | ros::NodeHandle nh; 177 | ROS_INFO("\033[1;32m---->\033[0m floam_odomEstimation_amcl3d initialization."); 178 | 179 | int scan_line = 64; 180 | double vertical_angle = 2.0; 181 | double scan_period= 0.1; 182 | double max_dis = 60.0; 183 | double min_dis = 2.0; 184 | 185 | nh.getParam("/scan_period", scan_period); 186 | nh.getParam("/vertical_angle", vertical_angle); 187 | nh.getParam("/max_dis", max_dis); 188 | nh.getParam("/min_dis", min_dis); 189 | nh.getParam("/scan_line", scan_line); 190 | 191 | lidar_param.setScanPeriod(scan_period); 192 | lidar_param.setVerticalAngle(vertical_angle); 193 | lidar_param.setLines(scan_line); 194 | lidar_param.setMaxDistance(max_dis); 195 | lidar_param.setMinDistance(min_dis); 196 | 197 | odomEstimation.init(lidar_param); 198 | ros::Subscriber subLaserCloud = nh.subscribe("/velodyne_points_filtered", 100, velodyneHandler); 199 | ros::Subscriber subEdgeLaserCloud = nh.subscribe("/laser_cloud_edge", 100, velodyneEdgeHandler); 200 | ros::Subscriber subSurfLaserCloud = nh.subscribe("/laser_cloud_surf", 100, velodyneSurfHandler); 201 | 202 | pubLaserOdometry = nh.advertise("/odometry", 100); 203 | pubLaserSensor = nh.advertise("/laser_sensor", 100); 204 | map_cloud_pub_ = nh.advertise("/odom_cloud",1); 205 | std::thread odom_estimation_process{odom_estimation}; 206 | std::thread match_map_thread{map_pub}; 207 | 208 | ros::spin(); 209 | 210 | return 0; 211 | } 212 | -------------------------------------------------------------------------------- /amcl3d/src/main.cpp: -------------------------------------------------------------------------------- 1 | /*! 2 | * @file main.cpp 3 | * @copyright Copyright (c) 2019, FADA-CATEC 4 | * 5 | * Licensed under the Apache License, Version 2.0 (the "License"); 6 | * you may not use this file except in compliance with the License. 7 | * You may obtain a copy of the License at 8 | * 9 | * http://www.apache.org/licenses/LICENSE-2.0 10 | * 11 | * Unless required by applicable law or agreed to in writing, software 12 | * distributed under the License is distributed on an "AS IS" BASIS, 13 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 14 | * See the License for the specific language governing permissions and 15 | * limitations under the License. 16 | */ 17 | 18 | #include "Node.h" 19 | 20 | int main(int argc, char** argv) 21 | { 22 | ros::init(argc, argv, "amcl3d_node"); 23 | 24 | // ROS_INFO("[%s] Node initialization.", ros::this_node::getName().data()); 25 | ROS_INFO("\033[1;32m---->\033[0m amcl3d Node initialization."); 26 | 27 | if (!ros::master::check()) 28 | { 29 | ROS_ERROR("[%s] Roscore is not running.", ros::this_node::getName().data()); 30 | return EXIT_FAILURE; 31 | } 32 | const std::string workingDir = "../catkin_ws/"; 33 | amcl3d::Node node(workingDir); 34 | node.spin(); 35 | 36 | // ROS_INFO("[%s] Node finished.", ros::this_node::getName().data()); 37 | ROS_INFO("\033[1;32m---->\033[0m amcl3d Node finished."); 38 | 39 | return EXIT_SUCCESS; 40 | } 41 | -------------------------------------------------------------------------------- /amcl3d/tests/Grid3dTest.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include 4 | 5 | using namespace amcl3d; 6 | 7 | static const double DEFAULT_SENSOR_DEV = 0.05; 8 | 9 | class Grid3dTest : public ::testing::Test 10 | { 11 | protected: 12 | Grid3d _sut; 13 | }; 14 | 15 | TEST_F(Grid3dTest, openTest) 16 | { 17 | std::string source = PROJECT_SOURCE_DIR; 18 | 19 | // Open .bt 20 | std::string map_path_bt = source + "/data/map/mapfile_complete.bt"; 21 | bool open_true_bt = _sut.open(map_path_bt, DEFAULT_SENSOR_DEV); 22 | EXPECT_EQ(open_true_bt, true); 23 | 24 | // Open .ot 25 | std::string map_path_ot = source + "/data/map/mapfile_complete_ot.ot"; 26 | bool open_true_ot = _sut.open(map_path_ot, DEFAULT_SENSOR_DEV); 27 | EXPECT_EQ(open_true_ot, true); 28 | 29 | // Open null .bt 30 | std::string map_path_nullbt = source + "/tests/data/mapfile_null.bt"; 31 | bool open_false_nullbt = _sut.open(map_path_nullbt, DEFAULT_SENSOR_DEV); 32 | EXPECT_EQ(open_false_nullbt, false); 33 | 34 | // Open null .ot 35 | std::string map_path_nullot = source + "/tests/data/mapfile_null.ot"; 36 | bool open_false_nullot = _sut.open(map_path_nullot, DEFAULT_SENSOR_DEV); 37 | EXPECT_EQ(open_false_nullot, false); 38 | 39 | // Open map_path.length() <= 3 40 | std::string map_path_false = ".bt"; 41 | bool open_false = _sut.open(map_path_false, DEFAULT_SENSOR_DEV); 42 | EXPECT_EQ(open_false, false); 43 | } 44 | 45 | TEST_F(Grid3dTest, buildGridSliceMsgTest) 46 | { 47 | ros::Time::init(); 48 | std::string source = PROJECT_SOURCE_DIR; 49 | std::string map_path = source + "/data/map/mapfile_complete.bt"; 50 | 51 | nav_msgs::OccupancyGrid msg; 52 | float z_negative = -100; 53 | float z_positive = 0; 54 | 55 | // Deserialize msg 56 | std::ifstream ifs(source + "/tests/data/nav_msg.bin", std::ios::in | std::ios::binary); 57 | ifs.seekg(0, std::ios::end); 58 | std::streampos end = ifs.tellg(); 59 | ifs.seekg(0, std::ios::beg); 60 | std::streampos begin = ifs.tellg(); 61 | 62 | uint32_t file_size = end - begin; 63 | boost::shared_array ibuffer(new uint8_t[file_size]); 64 | ifs.read((char*)ibuffer.get(), file_size); 65 | ros::serialization::IStream istream(ibuffer.get(), file_size); 66 | ros::serialization::deserialize(istream, msg); 67 | ifs.close(); 68 | 69 | // Without Open 70 | bool result_open = _sut.buildGridSliceMsg(z_negative, msg); 71 | EXPECT_EQ(result_open, false); 72 | 73 | // Open 74 | bool open_true = _sut.open(map_path, DEFAULT_SENSOR_DEV); 75 | EXPECT_EQ(open_true, true); 76 | 77 | // Z negative 78 | bool result_negative = _sut.buildGridSliceMsg(z_negative, msg); 79 | EXPECT_EQ(result_negative, false); 80 | 81 | // Z positive 82 | bool result_positive = _sut.buildGridSliceMsg(z_positive, msg); 83 | EXPECT_EQ(result_positive, true); 84 | } 85 | 86 | TEST_F(Grid3dTest, buildMapPointCloudMsgTest) 87 | { 88 | std::string source = PROJECT_SOURCE_DIR; 89 | std::string map_path = source + "/data/map/mapfile_complete.bt"; 90 | 91 | sensor_msgs::PointCloud2 msg; 92 | 93 | // Deserialize msg 94 | std::ifstream ifs(source + "/tests/data/mappointcloud_msg.bin", std::ios::in | std::ios::binary); 95 | ifs.seekg(0, std::ios::end); 96 | std::streampos end = ifs.tellg(); 97 | ifs.seekg(0, std::ios::beg); 98 | std::streampos begin = ifs.tellg(); 99 | 100 | uint32_t file_size = end - begin; 101 | boost::shared_array ibuffer(new uint8_t[file_size]); 102 | ifs.read((char*)ibuffer.get(), file_size); 103 | ros::serialization::IStream istream(ibuffer.get(), file_size); 104 | ros::serialization::deserialize(istream, msg); 105 | ifs.close(); 106 | 107 | // cloud_ = false 108 | bool result_false = _sut.buildMapPointCloudMsg(msg); 109 | EXPECT_EQ(result_false, false); 110 | 111 | // Open 112 | bool open_true = _sut.open(map_path, DEFAULT_SENSOR_DEV); 113 | EXPECT_EQ(open_true, true); 114 | 115 | // cloud_ = true 116 | bool result_true = _sut.buildMapPointCloudMsg(msg); 117 | EXPECT_EQ(result_true, true); 118 | } 119 | 120 | TEST_F(Grid3dTest, computeCloudWeightTest) 121 | { 122 | std::string source = PROJECT_SOURCE_DIR; 123 | std::string map_path = source + "/data/map/mapfile_complete.bt"; 124 | 125 | geometry_msgs::PoseArray msg; 126 | 127 | // Expect 128 | float tx = 20.017967; 129 | float ty = 10.140815; 130 | float tz = 3.372801; 131 | float a = 0.166781; 132 | float wp_real = 3.8109049797058105; 133 | 134 | // Deserialize msg 135 | std::ifstream ifs(source + "/tests/data/grid_info.bin", std::ios::in | std::ios::binary); 136 | ifs.seekg(0, std::ios::end); 137 | std::streampos end = ifs.tellg(); 138 | ifs.seekg(0, std::ios::beg); 139 | std::streampos begin = ifs.tellg(); 140 | 141 | uint32_t file_size = end - begin; 142 | boost::shared_array ibuffer(new uint8_t[file_size]); 143 | ifs.read((char*)ibuffer.get(), file_size); 144 | ros::serialization::IStream istream(ibuffer.get(), file_size); 145 | ros::serialization::deserialize(istream, msg); 146 | ifs.close(); 147 | 148 | pcl::PointCloud::Ptr points_grid(new pcl::PointCloud()); 149 | pcl::PointXYZ point_grid; 150 | for (int i = 0; i < msg.poses.size(); i++) 151 | { 152 | point_grid.x = msg.poses[i].position.x; 153 | point_grid.y = msg.poses[i].position.y; 154 | point_grid.z = msg.poses[i].position.z; 155 | 156 | points_grid->push_back(point_grid); 157 | } 158 | 159 | // computeCloudWeigh without Open 160 | float result_0 = _sut.computeCloudWeight(points_grid, 20.017967, 10.140815, 3.372801, 0, 0, 0.166781); 161 | EXPECT_EQ(result_0, 0); 162 | 163 | // Open 164 | bool open = _sut.open(map_path, DEFAULT_SENSOR_DEV); 165 | 166 | // computeCloudWeight in known point 167 | float result = _sut.computeCloudWeight(points_grid, 20.017967, 10.140815, 3.372801, 0, 0, 0.166781); 168 | EXPECT_NEAR(result, wp_real, 0.0001); 169 | } 170 | 171 | TEST_F(Grid3dTest, computeCloudWeightParticlesTest) 172 | { 173 | std::vector wp_test; 174 | float wtp = 3301.69; 175 | 176 | std::string source = PROJECT_SOURCE_DIR; 177 | std::string map_path = source + "/data/map/mapfile_complete.bt"; 178 | 179 | bool open = _sut.open(map_path, DEFAULT_SENSOR_DEV); 180 | 181 | geometry_msgs::PoseArray msg_pos; 182 | geometry_msgs::PoseArray msg; 183 | std::vector tx_vector; 184 | std::vector ty_vector; 185 | std::vector tz_vector; 186 | std::vector ta_vector; 187 | std::vector wp_vector; 188 | 189 | // Deserialize the points 190 | std::ifstream ifs(source + "/tests/data/grid_info.bin", std::ios::in | std::ios::binary); 191 | ifs.seekg(0, std::ios::end); 192 | std::streampos end = ifs.tellg(); 193 | ifs.seekg(0, std::ios::beg); 194 | std::streampos begin = ifs.tellg(); 195 | 196 | uint32_t file_size = end - begin; 197 | boost::shared_array ibuffer(new uint8_t[file_size]); 198 | ifs.read((char*)ibuffer.get(), file_size); 199 | ros::serialization::IStream istream(ibuffer.get(), file_size); 200 | ros::serialization::deserialize(istream, msg); 201 | ifs.close(); 202 | 203 | pcl::PointCloud::Ptr points_grid(new pcl::PointCloud()); 204 | pcl::PointXYZ point_grid; 205 | for (int i = 0; i < msg.poses.size(); i++) 206 | { 207 | point_grid.x = msg.poses[i].position.x; 208 | point_grid.y = msg.poses[i].position.y; 209 | point_grid.z = msg.poses[i].position.z; 210 | 211 | points_grid->push_back(point_grid); 212 | } 213 | 214 | // Deserialize information about particles 215 | std::ifstream ifs_pos(source + "/tests/data/particle_info.bin", std::ios::in | std::ios::binary); 216 | ifs_pos.seekg(0, std::ios::end); 217 | std::streampos end_pos = ifs_pos.tellg(); 218 | ifs_pos.seekg(0, std::ios::beg); 219 | std::streampos begin_pos = ifs_pos.tellg(); 220 | 221 | uint32_t file_size_pos = end_pos - begin_pos; 222 | boost::shared_array ibuffer_pos(new uint8_t[file_size_pos]); 223 | ifs_pos.read((char*)ibuffer_pos.get(), file_size_pos); 224 | ros::serialization::IStream istream_pos(ibuffer_pos.get(), file_size_pos); 225 | ros::serialization::deserialize(istream_pos, msg_pos); 226 | ifs_pos.close(); 227 | 228 | for (int i = 0; i < msg_pos.poses.size(); i++) 229 | { 230 | tx_vector.push_back(msg_pos.poses[i].position.x); 231 | ty_vector.push_back(msg_pos.poses[i].position.y); 232 | tz_vector.push_back(msg_pos.poses[i].position.z); 233 | ta_vector.push_back(msg_pos.poses[i].orientation.x); 234 | wp_vector.push_back(msg_pos.poses[i].orientation.y); 235 | } 236 | 237 | for (uint32_t i = 0; i < wp_vector.size(); ++i) 238 | { 239 | wp_test.push_back(_sut.computeCloudWeight(points_grid, tx_vector[i], ty_vector[i], tz_vector[i], 0, 0, ta_vector[i])); 240 | wp_test[i] /= wtp; 241 | EXPECT_NEAR(wp_vector[i], wp_test[i], 0.0001); 242 | } 243 | } 244 | 245 | TEST_F(Grid3dTest, isIntoMapTest) 246 | { 247 | // In mapfile_complete: x_max_ = 26.1 y_max_ = 19.2 z_max_ = 7.65 248 | float x_into = 1; 249 | float y_into = 1; 250 | float z_into = 1; 251 | float x_ninto = -100; 252 | float y_ninto = -100; 253 | float z_ninto = -100; 254 | 255 | std::string source = PROJECT_SOURCE_DIR; 256 | std::string map_path = source + "/data/map/mapfile_complete.bt"; 257 | 258 | bool open = _sut.open(map_path, DEFAULT_SENSOR_DEV); 259 | 260 | bool isInto = _sut.isIntoMap(x_into, y_into, z_into); 261 | bool isNotInto = _sut.isIntoMap(x_ninto, y_ninto, z_ninto); 262 | 263 | EXPECT_EQ(open, true); 264 | EXPECT_EQ(isInto, true); 265 | EXPECT_EQ(isNotInto, false); 266 | } 267 | -------------------------------------------------------------------------------- /amcl3d/tests/ParticleFilterTest.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include 4 | 5 | using namespace amcl3d; 6 | 7 | class ParticleFilterTest : public ::testing::Test 8 | { 9 | protected: 10 | ParticleFilterTest() 11 | { 12 | } 13 | 14 | ParticleFilter _sut; 15 | }; 16 | 17 | TEST_F(ParticleFilterTest, initTest) 18 | { 19 | // int num_particles = 600; 20 | // float x_init = 2.87080164308; 21 | // float y_init = 1.63820032984; 22 | // float z_init = 0.21460881682; 23 | // float a_init = 0.144; 24 | // float x_dev = 0.05; 25 | // float y_dev = 0.05; 26 | // float z_dev = 0.05; 27 | // float a_dev = 0.1; 28 | 29 | // _sut.init(num_particles, x_init, y_init, z_init, a_init, x_dev, y_dev, z_dev, a_dev); 30 | 31 | // Particle mean_ = _sut.getMean(); 32 | 33 | // EXPECT_NEAR(mean_.x, x_init, 0.01); 34 | // EXPECT_NEAR(mean_.y, y_init, 0.01); 35 | // EXPECT_NEAR(mean_.z, z_init, 0.01); 36 | // EXPECT_NEAR(mean_.a, a_init, 0.01); 37 | } 38 | 39 | TEST_F(ParticleFilterTest, predictTest) 40 | { 41 | int num_particles = 600; 42 | float x_init = 2.87080164308; 43 | float y_init = 1.63820032984; 44 | float z_init = 0.21460881682; 45 | float a_init = 0.144; 46 | float x_dev = 0.05; 47 | float y_dev = 0.05; 48 | float z_dev = 0.05; 49 | float a_dev = 0.1; 50 | 51 | double odom_x_mod = 0.1; 52 | double odom_y_mod = 0.1; 53 | double odom_z_mod = 0.1; 54 | double odom_a_mod = 0.3; 55 | double delta_x = -0.067421; 56 | double delta_y = -0.006161; 57 | double delta_z = 0.130909; 58 | double delta_a = 0.052421; 59 | 60 | _sut.init(num_particles, x_init, y_init, z_init, a_init, x_dev, y_dev, z_dev, a_dev); 61 | 62 | Particle mean_ = _sut.getMean(); 63 | 64 | _sut.predict(odom_x_mod, odom_y_mod, odom_z_mod, odom_a_mod, delta_x, delta_y, delta_z, delta_a); 65 | 66 | Particle predict_mean_ = _sut.getMean(); 67 | 68 | EXPECT_NEAR(mean_.x, predict_mean_.x, 0.0001); 69 | EXPECT_NEAR(mean_.y, predict_mean_.y, 0.0001); 70 | EXPECT_NEAR(mean_.z, predict_mean_.z, 0.0001); 71 | EXPECT_NEAR(mean_.a, predict_mean_.a, 0.0001); 72 | } 73 | -------------------------------------------------------------------------------- /amcl3d/tests/PointCloudToolsTest.cpp: -------------------------------------------------------------------------------- 1 | /*! 2 | * @file PointCloudToolsTest.h 3 | * @copyright Copyright (c) 2019, FADA-CATEC 4 | * 5 | * Licensed under the Apache License, Version 2.0 (the "License"); 6 | * you may not use this file except in compliance with the License. 7 | * You may obtain a copy of the License at 8 | * 9 | * http://www.apache.org/licenses/LICENSE-2.0 10 | * 11 | * Unless required by applicable law or agreed to in writing, software 12 | * distributed under the License is distributed on an "AS IS" BASIS, 13 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 14 | * See the License for the specific language governing permissions and 15 | * limitations under the License. 16 | */ 17 | 18 | #include 19 | 20 | #include 21 | 22 | using namespace amcl3d; 23 | 24 | static const std::string DATA_DIR = std::string(PROJECT_SOURCE_DIR) + std::string("/data"); 25 | static const std::string TESTSDATA_DIR = std::string(PROJECT_SOURCE_DIR) + std::string("/tests/data"); 26 | 27 | class PointCloudToolsTest : public ::testing::Test 28 | { 29 | protected: 30 | void openOcTreeFromBtFile() 31 | { 32 | boost::shared_ptr octo_tree; 33 | 34 | const std::string file_path = DATA_DIR + "/map/mapfile_complete.bt"; 35 | 36 | ASSERT_NO_THROW(octo_tree = openOcTree(file_path)); 37 | ASSERT_NE(octo_tree.get(), nullptr); 38 | ASSERT_GT(octo_tree->size(), 1); 39 | 40 | double min_x, min_y, min_z; 41 | octo_tree->getMetricMin(min_x, min_y, min_z); 42 | ASSERT_DOUBLE_EQ(min_x, -17.35); 43 | ASSERT_DOUBLE_EQ(min_y, -9.5); 44 | ASSERT_DOUBLE_EQ(min_z, -1.4); 45 | 46 | double max_x, max_y, max_z; 47 | octo_tree->getMetricMax(max_x, max_y, max_z); 48 | ASSERT_DOUBLE_EQ(max_x, 8.75); 49 | ASSERT_DOUBLE_EQ(max_y, 9.7); 50 | ASSERT_DOUBLE_EQ(max_z, 6.25); 51 | 52 | double resolution = octo_tree->getResolution(); 53 | ASSERT_DOUBLE_EQ(resolution, 0.05); 54 | 55 | _octo_tree = octo_tree; 56 | } 57 | 58 | boost::shared_ptr _octo_tree; 59 | }; 60 | 61 | TEST_F(PointCloudToolsTest, shouldNotOpenOcTreeFromNonExistentFile) 62 | { 63 | boost::shared_ptr octo_tree; 64 | 65 | ASSERT_THROW(octo_tree = openOcTree("unknown_file.bt"), std::runtime_error); 66 | ASSERT_EQ(octo_tree.get(), nullptr); 67 | 68 | ASSERT_THROW(octo_tree = openOcTree("unknown_file.ot"), std::runtime_error); 69 | ASSERT_EQ(octo_tree.get(), nullptr); 70 | 71 | ASSERT_THROW(octo_tree = openOcTree("unknown_file.unk"), std::runtime_error); 72 | ASSERT_EQ(octo_tree.get(), nullptr); 73 | } 74 | 75 | TEST_F(PointCloudToolsTest, shouldNotOpenOcTreeFromWrongBtFile) 76 | { 77 | boost::shared_ptr octo_tree; 78 | 79 | const std::string file_path = TESTSDATA_DIR + "/mapfile_wrong.bt"; 80 | 81 | ASSERT_THROW(octo_tree = openOcTree(file_path), std::runtime_error); 82 | ASSERT_EQ(octo_tree.get(), nullptr); 83 | } 84 | 85 | TEST_F(PointCloudToolsTest, shouldNotOpenOcTreeFromWrongOtFile) 86 | { 87 | boost::shared_ptr octo_tree; 88 | 89 | const std::string file_path = TESTSDATA_DIR + "/mapfile_wrong.ot"; 90 | 91 | ASSERT_THROW(octo_tree = openOcTree(file_path), std::runtime_error); 92 | ASSERT_EQ(octo_tree.get(), nullptr); 93 | } 94 | 95 | TEST_F(PointCloudToolsTest, shouldNotOpenOcTreeFromUnknownExtension) 96 | { 97 | boost::shared_ptr octo_tree; 98 | 99 | const std::string file_path = TESTSDATA_DIR + "/mapfile_unknown_extension.unk"; 100 | 101 | ASSERT_THROW(octo_tree = openOcTree(file_path), std::runtime_error); 102 | ASSERT_EQ(octo_tree.get(), nullptr); 103 | } 104 | 105 | TEST_F(PointCloudToolsTest, shouldOpenOcTreeFromBtFile) 106 | { 107 | openOcTreeFromBtFile(); 108 | } 109 | 110 | TEST_F(PointCloudToolsTest, shouldOpenOcTreeFromOtFile) 111 | { 112 | boost::shared_ptr octo_tree; 113 | 114 | const std::string file_path = DATA_DIR + "/map/mapfile_complete_ot.ot"; 115 | 116 | ASSERT_NO_THROW(octo_tree = openOcTree(file_path)); 117 | ASSERT_NE(octo_tree.get(), nullptr); 118 | ASSERT_GT(octo_tree->size(), 1); 119 | 120 | double min_x, min_y, min_z; 121 | octo_tree->getMetricMin(min_x, min_y, min_z); 122 | ASSERT_DOUBLE_EQ(min_x, -17.35); 123 | ASSERT_DOUBLE_EQ(min_y, -9.5); 124 | ASSERT_DOUBLE_EQ(min_z, -1.45); 125 | 126 | double max_x, max_y, max_z; 127 | octo_tree->getMetricMax(max_x, max_y, max_z); 128 | ASSERT_DOUBLE_EQ(max_x, 8.8); 129 | ASSERT_DOUBLE_EQ(max_y, 9.7); 130 | ASSERT_DOUBLE_EQ(max_z, 6.25); 131 | 132 | double resolution = octo_tree->getResolution(); 133 | ASSERT_DOUBLE_EQ(resolution, 0.05); 134 | } 135 | 136 | TEST_F(PointCloudToolsTest, shouldNotComputePointCloudWithNullOctoTree) 137 | { 138 | PointCloudInfo::Ptr pc_info; 139 | 140 | ASSERT_THROW(pc_info = computePointCloud(nullptr), std::runtime_error); 141 | 142 | ASSERT_EQ(pc_info, nullptr); 143 | } 144 | 145 | TEST_F(PointCloudToolsTest, shouldNotComputePointCloudWithEmptyOctoTree) 146 | { 147 | PointCloudInfo::Ptr pc_info; 148 | 149 | boost::shared_ptr octo_tree(new octomap::OcTree(0.1)); 150 | 151 | ASSERT_THROW(pc_info = computePointCloud(octo_tree), std::runtime_error); 152 | 153 | ASSERT_EQ(pc_info, nullptr); 154 | } 155 | 156 | TEST_F(PointCloudToolsTest, shouldComputePointCloud) 157 | { 158 | PointCloudInfo::Ptr pc_info; 159 | 160 | openOcTreeFromBtFile(); 161 | 162 | ASSERT_NO_THROW(pc_info = computePointCloud(_octo_tree)); 163 | 164 | ASSERT_NE(pc_info, nullptr); 165 | ASSERT_NE(pc_info->cloud, nullptr); 166 | ASSERT_EQ(pc_info->cloud->size(), 179551); 167 | ASSERT_DOUBLE_EQ(pc_info->octo_min_x, -17.35); 168 | ASSERT_DOUBLE_EQ(pc_info->octo_min_y, -9.5); 169 | ASSERT_DOUBLE_EQ(pc_info->octo_min_z, -1.4); 170 | ASSERT_DOUBLE_EQ(pc_info->octo_max_x, 8.75); 171 | ASSERT_DOUBLE_EQ(pc_info->octo_max_y, 9.7); 172 | ASSERT_DOUBLE_EQ(pc_info->octo_max_z, 6.25); 173 | ASSERT_DOUBLE_EQ(pc_info->octo_resol, 0.05); 174 | } 175 | -------------------------------------------------------------------------------- /amcl3d/tests/data/grid_info.bin: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/chengwei0427/amcl3d_test/278d4679a2ef0ed438d7dd387279c932928bf7fe/amcl3d/tests/data/grid_info.bin -------------------------------------------------------------------------------- /amcl3d/tests/data/mapfile_null.ot: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/chengwei0427/amcl3d_test/278d4679a2ef0ed438d7dd387279c932928bf7fe/amcl3d/tests/data/mapfile_null.ot -------------------------------------------------------------------------------- /amcl3d/tests/data/mapfile_unknown_extension.unk: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/chengwei0427/amcl3d_test/278d4679a2ef0ed438d7dd387279c932928bf7fe/amcl3d/tests/data/mapfile_unknown_extension.unk -------------------------------------------------------------------------------- /amcl3d/tests/data/mapfile_wrong.bt: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/chengwei0427/amcl3d_test/278d4679a2ef0ed438d7dd387279c932928bf7fe/amcl3d/tests/data/mapfile_wrong.bt -------------------------------------------------------------------------------- /amcl3d/tests/data/mapfile_wrong.ot: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/chengwei0427/amcl3d_test/278d4679a2ef0ed438d7dd387279c932928bf7fe/amcl3d/tests/data/mapfile_wrong.ot -------------------------------------------------------------------------------- /amcl3d/tests/data/mappointcloud_msg.bin: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/chengwei0427/amcl3d_test/278d4679a2ef0ed438d7dd387279c932928bf7fe/amcl3d/tests/data/mappointcloud_msg.bin -------------------------------------------------------------------------------- /amcl3d/tests/data/nav_msg.bin: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/chengwei0427/amcl3d_test/278d4679a2ef0ed438d7dd387279c932928bf7fe/amcl3d/tests/data/nav_msg.bin -------------------------------------------------------------------------------- /amcl3d/tests/data/particle_info.bin: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /amcl3d/tests/main.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | int main(int argc, char** argv) 4 | { 5 | ::testing::InitGoogleMock(&argc, argv); 6 | 7 | int ret = RUN_ALL_TESTS(); 8 | (void)ret; 9 | 10 | return EXIT_SUCCESS; 11 | } 12 | -------------------------------------------------------------------------------- /catkin.options: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/chengwei0427/amcl3d_test/278d4679a2ef0ed438d7dd387279c932928bf7fe/catkin.options -------------------------------------------------------------------------------- /dependencies.rosinstall: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/chengwei0427/amcl3d_test/278d4679a2ef0ed438d7dd387279c932928bf7fe/dependencies.rosinstall -------------------------------------------------------------------------------- /doc/flow.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/chengwei0427/amcl3d_test/278d4679a2ef0ed438d7dd387279c932928bf7fe/doc/flow.png -------------------------------------------------------------------------------- /doc/pf_demo.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/chengwei0427/amcl3d_test/278d4679a2ef0ed438d7dd387279c932928bf7fe/doc/pf_demo.png -------------------------------------------------------------------------------- /rosinrange_msg/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | project(rosinrange_msg) 2 | cmake_minimum_required(VERSION 2.8.3) 3 | 4 | ################################## 5 | ## Configure CATKIN dependecies ## 6 | ################################## 7 | 8 | find_package(catkin REQUIRED 9 | COMPONENTS 10 | geometry_msgs 11 | message_generation 12 | ) 13 | 14 | ####################### 15 | ## Generate messages ## 16 | ####################### 17 | 18 | add_message_files(DIRECTORY msg) 19 | 20 | ########################################## 21 | ## Generate added messages and services ## 22 | ########################################## 23 | 24 | generate_messages( 25 | DEPENDENCIES 26 | geometry_msgs 27 | ) 28 | 29 | ################################### 30 | ## CATKIN specific configuration ## 31 | ################################### 32 | 33 | catkin_package( 34 | CATKIN_DEPENDS 35 | geometry_msgs 36 | message_runtime 37 | ) 38 | -------------------------------------------------------------------------------- /rosinrange_msg/LICENSE: -------------------------------------------------------------------------------- 1 | 2 | Apache License 3 | Version 2.0, January 2004 4 | http://www.apache.org/licenses/ 5 | 6 | TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION 7 | 8 | 1. Definitions. 9 | 10 | "License" shall mean the terms and conditions for use, reproduction, 11 | and distribution as defined by Sections 1 through 9 of this document. 12 | 13 | "Licensor" shall mean the copyright owner or entity authorized by 14 | the copyright owner that is granting the License. 15 | 16 | "Legal Entity" shall mean the union of the acting entity and all 17 | other entities that control, are controlled by, or are under common 18 | control with that entity. For the purposes of this definition, 19 | "control" means (i) the power, direct or indirect, to cause the 20 | direction or management of such entity, whether by contract or 21 | otherwise, or (ii) ownership of fifty percent (50%) or more of the 22 | outstanding shares, or (iii) beneficial ownership of such entity. 23 | 24 | "You" (or "Your") shall mean an individual or Legal Entity 25 | exercising permissions granted by this License. 26 | 27 | "Source" form shall mean the preferred form for making modifications, 28 | including but not limited to software source code, documentation 29 | source, and configuration files. 30 | 31 | "Object" form shall mean any form resulting from mechanical 32 | transformation or translation of a Source form, including but 33 | not limited to compiled object code, generated documentation, 34 | and conversions to other media types. 35 | 36 | "Work" shall mean the work of authorship, whether in Source or 37 | Object form, made available under the License, as indicated by a 38 | copyright notice that is included in or attached to the work 39 | (an example is provided in the Appendix below). 40 | 41 | "Derivative Works" shall mean any work, whether in Source or Object 42 | form, that is based on (or derived from) the Work and for which the 43 | editorial revisions, annotations, elaborations, or other modifications 44 | represent, as a whole, an original work of authorship. For the purposes 45 | of this License, Derivative Works shall not include works that remain 46 | separable from, or merely link (or bind by name) to the interfaces of, 47 | the Work and Derivative Works thereof. 48 | 49 | "Contribution" shall mean any work of authorship, including 50 | the original version of the Work and any modifications or additions 51 | to that Work or Derivative Works thereof, that is intentionally 52 | submitted to Licensor for inclusion in the Work by the copyright owner 53 | or by an individual or Legal Entity authorized to submit on behalf of 54 | the copyright owner. For the purposes of this definition, "submitted" 55 | means any form of electronic, verbal, or written communication sent 56 | to the Licensor or its representatives, including but not limited to 57 | communication on electronic mailing lists, source code control systems, 58 | and issue tracking systems that are managed by, or on behalf of, the 59 | Licensor for the purpose of discussing and improving the Work, but 60 | excluding communication that is conspicuously marked or otherwise 61 | designated in writing by the copyright owner as "Not a Contribution." 62 | 63 | "Contributor" shall mean Licensor and any individual or Legal Entity 64 | on behalf of whom a Contribution has been received by Licensor and 65 | subsequently incorporated within the Work. 66 | 67 | 2. Grant of Copyright License. Subject to the terms and conditions of 68 | this License, each Contributor hereby grants to You a perpetual, 69 | worldwide, non-exclusive, no-charge, royalty-free, irrevocable 70 | copyright license to reproduce, prepare Derivative Works of, 71 | publicly display, publicly perform, sublicense, and distribute the 72 | Work and such Derivative Works in Source or Object form. 73 | 74 | 3. Grant of Patent License. Subject to the terms and conditions of 75 | this License, each Contributor hereby grants to You a perpetual, 76 | worldwide, non-exclusive, no-charge, royalty-free, irrevocable 77 | (except as stated in this section) patent license to make, have made, 78 | use, offer to sell, sell, import, and otherwise transfer the Work, 79 | where such license applies only to those patent claims licensable 80 | by such Contributor that are necessarily infringed by their 81 | Contribution(s) alone or by combination of their Contribution(s) 82 | with the Work to which such Contribution(s) was submitted. If You 83 | institute patent litigation against any entity (including a 84 | cross-claim or counterclaim in a lawsuit) alleging that the Work 85 | or a Contribution incorporated within the Work constitutes direct 86 | or contributory patent infringement, then any patent licenses 87 | granted to You under this License for that Work shall terminate 88 | as of the date such litigation is filed. 89 | 90 | 4. Redistribution. You may reproduce and distribute copies of the 91 | Work or Derivative Works thereof in any medium, with or without 92 | modifications, and in Source or Object form, provided that You 93 | meet the following conditions: 94 | 95 | (a) You must give any other recipients of the Work or 96 | Derivative Works a copy of this License; and 97 | 98 | (b) You must cause any modified files to carry prominent notices 99 | stating that You changed the files; and 100 | 101 | (c) You must retain, in the Source form of any Derivative Works 102 | that You distribute, all copyright, patent, trademark, and 103 | attribution notices from the Source form of the Work, 104 | excluding those notices that do not pertain to any part of 105 | the Derivative Works; and 106 | 107 | (d) If the Work includes a "NOTICE" text file as part of its 108 | distribution, then any Derivative Works that You distribute must 109 | include a readable copy of the attribution notices contained 110 | within such NOTICE file, excluding those notices that do not 111 | pertain to any part of the Derivative Works, in at least one 112 | of the following places: within a NOTICE text file distributed 113 | as part of the Derivative Works; within the Source form or 114 | documentation, if provided along with the Derivative Works; or, 115 | within a display generated by the Derivative Works, if and 116 | wherever such third-party notices normally appear. The contents 117 | of the NOTICE file are for informational purposes only and 118 | do not modify the License. You may add Your own attribution 119 | notices within Derivative Works that You distribute, alongside 120 | or as an addendum to the NOTICE text from the Work, provided 121 | that such additional attribution notices cannot be construed 122 | as modifying the License. 123 | 124 | You may add Your own copyright statement to Your modifications and 125 | may provide additional or different license terms and conditions 126 | for use, reproduction, or distribution of Your modifications, or 127 | for any such Derivative Works as a whole, provided Your use, 128 | reproduction, and distribution of the Work otherwise complies with 129 | the conditions stated in this License. 130 | 131 | 5. Submission of Contributions. Unless You explicitly state otherwise, 132 | any Contribution intentionally submitted for inclusion in the Work 133 | by You to the Licensor shall be under the terms and conditions of 134 | this License, without any additional terms or conditions. 135 | Notwithstanding the above, nothing herein shall supersede or modify 136 | the terms of any separate license agreement you may have executed 137 | with Licensor regarding such Contributions. 138 | 139 | 6. Trademarks. This License does not grant permission to use the trade 140 | names, trademarks, service marks, or product names of the Licensor, 141 | except as required for reasonable and customary use in describing the 142 | origin of the Work and reproducing the content of the NOTICE file. 143 | 144 | 7. Disclaimer of Warranty. Unless required by applicable law or 145 | agreed to in writing, Licensor provides the Work (and each 146 | Contributor provides its Contributions) on an "AS IS" BASIS, 147 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or 148 | implied, including, without limitation, any warranties or conditions 149 | of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A 150 | PARTICULAR PURPOSE. You are solely responsible for determining the 151 | appropriateness of using or redistributing the Work and assume any 152 | risks associated with Your exercise of permissions under this License. 153 | 154 | 8. Limitation of Liability. In no event and under no legal theory, 155 | whether in tort (including negligence), contract, or otherwise, 156 | unless required by applicable law (such as deliberate and grossly 157 | negligent acts) or agreed to in writing, shall any Contributor be 158 | liable to You for damages, including any direct, indirect, special, 159 | incidental, or consequential damages of any character arising as a 160 | result of this License or out of the use or inability to use the 161 | Work (including but not limited to damages for loss of goodwill, 162 | work stoppage, computer failure or malfunction, or any and all 163 | other commercial damages or losses), even if such Contributor 164 | has been advised of the possibility of such damages. 165 | 166 | 9. Accepting Warranty or Additional Liability. While redistributing 167 | the Work or Derivative Works thereof, You may choose to offer, 168 | and charge a fee for, acceptance of support, warranty, indemnity, 169 | or other liability obligations and/or rights consistent with this 170 | License. However, in accepting such obligations, You may act only 171 | on Your own behalf and on Your sole responsibility, not on behalf 172 | of any other Contributor, and only if You agree to indemnify, 173 | defend, and hold each Contributor harmless for any liability 174 | incurred by, or claims asserted against, such Contributor by reason 175 | of your accepting any such warranty or additional liability. 176 | 177 | END OF TERMS AND CONDITIONS 178 | 179 | APPENDIX: How to apply the Apache License to your work. 180 | 181 | To apply the Apache License to your work, attach the following 182 | boilerplate notice, with the fields enclosed by brackets "[]" 183 | replaced with your own identifying information. (Don't include 184 | the brackets!) The text should be enclosed in the appropriate 185 | comment syntax for the file format. We also recommend that a 186 | file or class name and description of purpose be included on the 187 | same "printed page" as the copyright notice for easier 188 | identification within third-party archives. 189 | 190 | Copyright [yyyy] [name of copyright owner] 191 | 192 | Licensed under the Apache License, Version 2.0 (the "License"); 193 | you may not use this file except in compliance with the License. 194 | You may obtain a copy of the License at 195 | 196 | http://www.apache.org/licenses/LICENSE-2.0 197 | 198 | Unless required by applicable law or agreed to in writing, software 199 | distributed under the License is distributed on an "AS IS" BASIS, 200 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 201 | See the License for the specific language governing permissions and 202 | limitations under the License. 203 | -------------------------------------------------------------------------------- /rosinrange_msg/msg/RangePose.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | 3 | uint64 source_id 4 | uint64 destination_id 5 | float64 range 6 | geometry_msgs/Point position -------------------------------------------------------------------------------- /rosinrange_msg/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | rosinrange_msg 5 | 1.0.0 6 | 7 |

8 | ROSIN radio-range sensor message. 9 |

10 |

11 | It is the message that amcl3d uses to receive distance measures and positions of radio-range sensors. 12 |

13 | 14 | http://wiki.ros.org/amcl3d 15 | Paloma Carrasco Fernandez 16 | Francisco J.Perez-Grau 17 | Francisco Cuesta Rodriguez 18 | Apache 2.0 19 | 20 | catkin 21 | 22 | geometry_msgs 23 | message_generation 24 | message_runtime 25 | 26 | 27 | --------------------------------------------------------------------------------