├── .gitignore ├── .resources ├── ROSCon2023.png ├── micp.gif └── rmcl_logo_landscape_small.png ├── CITATION.cff ├── LICENSE ├── README.md ├── rmcl ├── CMakeLists.txt ├── LICENSE ├── README.md ├── cmake │ ├── CompileOptixKernels.cmake │ ├── CompileOptixKernelsCudaToolkit.cmake │ ├── DownloadNanoflann.cmake │ ├── FileToString.h.in │ └── rmcl-config.cmake.in ├── include │ └── rmcl │ │ ├── clustering │ │ └── clustering.h │ │ ├── math │ │ ├── math_batched.cuh │ │ └── math_batched.h │ │ ├── registration │ │ ├── CPCEmbree.hpp │ │ ├── Correspondences.hpp │ │ ├── CorrespondencesCPU.hpp │ │ ├── CorrespondencesCUDA.hpp │ │ ├── RCCEmbree.hpp │ │ └── RCCOptix.hpp │ │ └── spatial │ │ └── KdTree.hpp ├── package.xml └── src │ └── rmcl │ ├── clustering │ └── clustering.cpp │ ├── math │ ├── math_batched.cpp │ └── math_batched.cu │ ├── registration │ ├── CPCEmbree.cpp │ ├── CorrespondencesCPU.cpp │ ├── CorrespondencesCUDA.cpp │ ├── RCCEmbree.cpp │ └── RCCOptix.cpp │ └── spatial │ └── KdTree.cpp ├── rmcl_msgs ├── CMakeLists.txt ├── LICENSE ├── README.md ├── msg │ ├── Depth.msg │ ├── DepthInfo.msg │ ├── DepthInfoStamped.msg │ ├── DepthStamped.msg │ ├── MICPSensorStats.msg │ ├── MICPStats.msg │ ├── O1Dn.msg │ ├── O1DnInfo.msg │ ├── O1DnInfoStamped.msg │ ├── O1DnStamped.msg │ ├── OnDn.msg │ ├── OnDnInfo.msg │ ├── OnDnInfoStamped.msg │ ├── OnDnStamped.msg │ ├── PolarCoord.msg │ ├── RangeData.msg │ ├── RangeDataStamped.msg │ ├── Scan.msg │ ├── ScanInfo.msg │ ├── ScanInfoStamped.msg │ └── ScanStamped.msg └── package.xml └── rmcl_ros ├── CMakeLists.txt ├── LICENSE ├── include └── rmcl_ros │ ├── micpl │ ├── MICPO1DnSensorCPU.hpp │ ├── MICPO1DnSensorCUDA.hpp │ ├── MICPOnDnSensorCPU.hpp │ ├── MICPOnDnSensorCUDA.hpp │ ├── MICPPinholeSensorCPU.hpp │ ├── MICPPinholeSensorCUDA.hpp │ ├── MICPSensor.hpp │ ├── MICPSensorCPU.hpp │ ├── MICPSensorCUDA.hpp │ ├── MICPSphericalSensorCPU.hpp │ └── MICPSphericalSensorCUDA.hpp │ ├── nodes │ ├── conversion │ │ ├── pc2_to_o1dn.hpp │ │ ├── pc2_to_scan.hpp │ │ └── scan_to_scan.hpp │ ├── filter │ │ ├── map_segmentation.hpp │ │ ├── o1dn_map_segmentation_embree.hpp │ │ └── scan_map_segmentation_embree.hpp │ └── micp_localization.hpp │ └── util │ ├── conversions.h │ ├── ros_defines.h │ ├── ros_helper.h │ ├── ros_helper.tcc │ ├── scan_operations.h │ └── text_colors.h ├── package.xml └── src ├── benchmarks ├── lidar_corrector_embree_benchmark.cpp └── lidar_corrector_optix_benchmark.cpp ├── micpl ├── MICPO1DnSensorCPU.cpp ├── MICPO1DnSensorCUDA.cpp ├── MICPOnDnSensorCPU.cpp ├── MICPOnDnSensorCUDA.cpp ├── MICPPinholeSensorCPU.cpp ├── MICPPinholeSensorCUDA.cpp ├── MICPSensor.cpp ├── MICPSensorCPU.cpp ├── MICPSensorCUDA.cpp ├── MICPSphericalSensorCPU.cpp └── MICPSphericalSensorCUDA.cpp ├── nodes ├── conversion │ ├── pc2_to_o1dn.cpp │ ├── pc2_to_scan.cpp │ └── scan_to_scan.cpp ├── filter │ ├── map_segmentation.cpp │ ├── o1dn_map_segmentation_embree.cpp │ └── scan_map_segmentation_embree.cpp └── micp_localization.cpp └── util ├── conversions.cpp ├── ros_helper.cpp └── scan_operations.cpp /.gitignore: -------------------------------------------------------------------------------- 1 | .vscode/ 2 | build 3 | doc 4 | ext 5 | -------------------------------------------------------------------------------- /.resources/ROSCon2023.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/uos/rmcl/95142e637091e6803485e4d0f63d31682961acde/.resources/ROSCon2023.png -------------------------------------------------------------------------------- /.resources/micp.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/uos/rmcl/95142e637091e6803485e4d0f63d31682961acde/.resources/micp.gif -------------------------------------------------------------------------------- /.resources/rmcl_logo_landscape_small.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/uos/rmcl/95142e637091e6803485e4d0f63d31682961acde/.resources/rmcl_logo_landscape_small.png -------------------------------------------------------------------------------- /CITATION.cff: -------------------------------------------------------------------------------- 1 | cff-version: 1.2.0 2 | preferred-citation: 3 | title: "MICP-L: Mesh-based ICP for Robot Localization Using Hardware-Accelerated Ray Casting" 4 | doi: "10.1109/IROS58592.2024.10802360" 5 | year: "2024" 6 | type: conference-paper 7 | collection-title: "2024 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS)" 8 | url: https://kbs.informatik.uos.de/files/pdfs/iros2024_amock_micp.pdf 9 | codeurl: https://github.com/uos/rmcl 10 | authors: 11 | - family-names: Mock 12 | given-names: Alexander 13 | orcid: 0009-0004-7800-9774 14 | - family-names: Wiemann 15 | given-names: Thomas 16 | orcid: 0000-0003-0710-872X 17 | - family-names: Pütz 18 | given-names: Sebastian 19 | orcid: 0000-0002-4210-4233 20 | - family-names: Hertzberg 21 | given-names: Joachim 22 | orcid: 0000-0002-6371-9624 23 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | BSD 3-Clause License 2 | 3 | Copyright (c) 2024, Alexander Mock 4 | All rights reserved. 5 | 6 | Redistribution and use in source and binary forms, with or without 7 | modification, are permitted provided that the following conditions are met: 8 | 9 | 1. Redistributions of source code must retain the above copyright notice, this 10 | list of conditions and the following disclaimer. 11 | 12 | 2. Redistributions in binary form must reproduce the above copyright notice, 13 | this list of conditions and the following disclaimer in the documentation 14 | and/or other materials provided with the distribution. 15 | 16 | 3. Neither the name of the copyright holder nor the names of its 17 | contributors may be used to endorse or promote products derived from 18 | this software without specific prior written permission. 19 | 20 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 21 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 22 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 23 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 24 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 25 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 26 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 27 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 28 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 29 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 30 | -------------------------------------------------------------------------------- /rmcl/LICENSE: -------------------------------------------------------------------------------- 1 | BSD 3-Clause License 2 | 3 | Copyright (c) 2024, Alexander Mock 4 | All rights reserved. 5 | 6 | Redistribution and use in source and binary forms, with or without 7 | modification, are permitted provided that the following conditions are met: 8 | 9 | 1. Redistributions of source code must retain the above copyright notice, this 10 | list of conditions and the following disclaimer. 11 | 12 | 2. Redistributions in binary form must reproduce the above copyright notice, 13 | this list of conditions and the following disclaimer in the documentation 14 | and/or other materials provided with the distribution. 15 | 16 | 3. Neither the name of the copyright holder nor the names of its 17 | contributors may be used to endorse or promote products derived from 18 | this software without specific prior written permission. 19 | 20 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 21 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 22 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 23 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 24 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 25 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 26 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 27 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 28 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 29 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 30 | -------------------------------------------------------------------------------- /rmcl/README.md: -------------------------------------------------------------------------------- 1 | # RMCL 2 | 3 | ROS-agnostic part of RMCL. 4 | -------------------------------------------------------------------------------- /rmcl/cmake/CompileOptixKernels.cmake: -------------------------------------------------------------------------------- 1 | ### COMPILE OPTIX KERNEL ### 2 | # Problem: nvcc cannot compile optix launches in kernels 3 | # but we can precompile to ptx files + include the strings in optix programs 4 | # Next problem: pathes to ptx files differ for installation and raw build 5 | # Solution: 6 | # 1. Compile to ptx 7 | # Here: 8 | # 2. Copy contents of ptx to temporary header file with R("...") 9 | # 3. Include temporary header file static into a char* variable 10 | 11 | # message(STATUS "Precomputing ${OPTIX_KERNEL_FILES}") 12 | 13 | 14 | string(REPLACE " " ";" OPTIX_KERNEL_FILES "${OPTIX_KERNEL_FILES}") 15 | 16 | # message(STATUS "Capsule: ${OPTIX_KERNEL_FILES}") 17 | 18 | # Capsule PTX-Strings in files 19 | foreach(OPTIX_KERNEL_FILE ${OPTIX_KERNEL_FILES}) 20 | # message(STATUS "Precomputing ${OPTIX_KERNEL_FILE}") 21 | # Get Name of Kernel 22 | get_filename_component(OPTIX_KERNEL_NAME ${OPTIX_KERNEL_FILE} NAME_WLE) 23 | # Read Compiled Kernel to String 24 | file(READ "${RMCL_OPTIX_PTX_DIR}/cuda_compile_ptx_1_generated_${OPTIX_KERNEL_NAME}.cu.ptx" INCLUDE_STRING) 25 | # Write to static readable file e.g. R("") 26 | configure_file(${RMCL_SOURCE_DIR}/cmake/FileToString.h.in "include/kernels/${OPTIX_KERNEL_NAME}String.h") 27 | message(STATUS "Preprocessed ${OPTIX_KERNEL_NAME}") 28 | endforeach() 29 | -------------------------------------------------------------------------------- /rmcl/cmake/CompileOptixKernelsCudaToolkit.cmake: -------------------------------------------------------------------------------- 1 | ### COMPILE OPTIX KERNEL ### 2 | # Problem: nvcc cannot compile optix launches in kernels 3 | # but we can precompile to ptx files + include the strings in optix programs 4 | # Next problem: pathes to ptx files differ for installation and raw build 5 | # Solution: 6 | # 1. Compile to ptx 7 | # Here: 8 | # 2. Copy contents of ptx to temporary header file with R("...") 9 | # 3. Include temporary header file static into a char* variable 10 | 11 | # message(STATUS "Precomputing ${OPTIX_KERNEL_FILES}") 12 | 13 | 14 | string(REPLACE " " ";" OPTIX_KERNEL_FILES "${OPTIX_KERNEL_FILES}") 15 | 16 | # message(STATUS "Capsule: ${OPTIX_KERNEL_FILES}") 17 | 18 | # Capsule PTX-Strings in files 19 | foreach(OPTIX_KERNEL_FILE ${OPTIX_KERNEL_FILES}) 20 | # message(STATUS "Precomputing ${OPTIX_KERNEL_FILE}") 21 | # Get Name of Kernel 22 | get_filename_component(OPTIX_KERNEL_NAME ${OPTIX_KERNEL_FILE} NAME_WLE) 23 | # Read Compiled Kernel to String 24 | file(READ "${CMAKE_CURRENT_BINARY_DIR}/CMakeFiles/rmcl_optix_cu_to_ptx.dir/src/rmcl/correction/optix/${OPTIX_KERNEL_NAME}.ptx" INCLUDE_STRING) 25 | # Write to static readable file e.g. R("") 26 | configure_file(${RMCL_SOURCE_DIR}/cmake/FileToString.h.in "include/kernels/${OPTIX_KERNEL_NAME}String.h") 27 | message(STATUS "Preprocessed ${OPTIX_KERNEL_NAME}") 28 | endforeach() -------------------------------------------------------------------------------- /rmcl/cmake/DownloadNanoflann.cmake: -------------------------------------------------------------------------------- 1 | set(NANOFLANN_DIR ${PROJECT_SOURCE_DIR}/ext/nanoflann) 2 | ExternalProject_Add(nanoflann 3 | PREFIX ${NANOFLANN_DIR} 4 | GIT_REPOSITORY "https://github.com/jlblancoc/nanoflann.git" 5 | GIT_TAG master 6 | SOURCE_DIR "" 7 | # Disable configure, build and install steps 8 | CONFIGURE_COMMAND "" 9 | UPDATE_COMMAND "" 10 | BUILD_COMMAND "" 11 | INSTALL_COMMAND "") 12 | 13 | set(nanoflann_INCLUDE_DIR "ext/nanoflann/src/nanoflann/include") 14 | set(nanoflann_INCLUDE_DIRS "ext/nanoflann/src/nanoflann/include") 15 | # include_directories(ext/nanoflann/include) -------------------------------------------------------------------------------- /rmcl/cmake/FileToString.h.in: -------------------------------------------------------------------------------- 1 | R"( 2 | @INCLUDE_STRING@ 3 | )" -------------------------------------------------------------------------------- /rmcl/cmake/rmcl-config.cmake.in: -------------------------------------------------------------------------------- 1 | # ----------------------------------------------------------------- 2 | # rmagine's cmake configuration 3 | # 4 | # provided interface variables: 5 | # - rmcl_INCLUDE_DIRS 6 | # - rmcl_LIBRARIES 7 | # - rmcl_DEFINITIONS 8 | # 9 | # 10 | # Do not forget to add_defintions(${rmcl_DEFINITIONS}) as they 11 | # describe the build configuration of rmcl. 12 | # 13 | # ----------------------------------------------------------------- 14 | @PACKAGE_INIT@ 15 | 16 | include(${CMAKE_CURRENT_LIST_DIR}/rmcl-config-version.cmake) 17 | include(${CMAKE_CURRENT_LIST_DIR}/rmcl-targets.cmake) 18 | 19 | set(RMCL_ROOT_DIR ${PACKAGE_PREFIX_DIR}) 20 | 21 | set(rmcl_LIB_DIR @CMAKE_INSTALL_PREFIX@/@CMAKE_INSTALL_LIBDIR@) 22 | set(rmcl_INCLUDE_DIR @CMAKE_INSTALL_PREFIX@/@CMAKE_INSTALL_INCLUDEDIR@/rmcl-@rmcl_VERSION@) 23 | set(rmcl_INCLUDE_DIRS ${rmcl_INCLUDE_DIR}) 24 | 25 | set(rmcl_FOUND TRUE) 26 | 27 | set(rmcl_LIBRARIES @RMCL_LIBRARIES@) -------------------------------------------------------------------------------- /rmcl/include/rmcl/clustering/clustering.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2022, University Osnabrück 3 | * All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * * Redistributions of source code must retain the above copyright 8 | * notice, this list of conditions and the following disclaimer. 9 | * * Redistributions in binary form must reproduce the above copyright 10 | * notice, this list of conditions and the following disclaimer in the 11 | * documentation and/or other materials provided with the distribution. 12 | * * Neither the name of the University Osnabrück nor the 13 | * names of its contributors may be used to endorse or promote products 14 | * derived from this software without specific prior written permission. 15 | * 16 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 17 | * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 18 | * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 19 | * DISCLAIMED. IN NO EVENT SHALL University Osnabrück BE LIABLE FOR ANY 20 | * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 21 | * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 22 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 23 | * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 24 | * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | */ 27 | 28 | /** 29 | * @file 30 | * 31 | * @brief Cluster Algorithms 32 | * 33 | * @date 03.10.2022 34 | * @author Alexander Mock 35 | * 36 | * @copyright Copyright (c) 2022, University Osnabrück. All rights reserved. 37 | * This project is released under the 3-Clause BSD License. 38 | * 39 | */ 40 | 41 | #ifndef RMCL_CLUSTERING_CLUSTERING_H 42 | #define RMCL_CLUSTERING_CLUSTERING_H 43 | 44 | #include 45 | #include 46 | #include 47 | 48 | namespace rmcl 49 | { 50 | 51 | void sort_clusters(std::vector >& clusters); 52 | 53 | std::vector > dbscan( 54 | KdTreePtr index, 55 | float search_dist, 56 | unsigned int min_pts_in_radius, 57 | unsigned int min_pts_per_cluster); 58 | 59 | } // namespace rmcl 60 | 61 | #endif // RMCL_CLUSTERING_CLUSTERING_H -------------------------------------------------------------------------------- /rmcl/include/rmcl/math/math_batched.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2022, University Osnabrück 3 | * All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * * Redistributions of source code must retain the above copyright 8 | * notice, this list of conditions and the following disclaimer. 9 | * * Redistributions in binary form must reproduce the above copyright 10 | * notice, this list of conditions and the following disclaimer in the 11 | * documentation and/or other materials provided with the distribution. 12 | * * Neither the name of the University Osnabrück nor the 13 | * names of its contributors may be used to endorse or promote products 14 | * derived from this software without specific prior written permission. 15 | * 16 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 17 | * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 18 | * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 19 | * DISCLAIMED. IN NO EVENT SHALL University Osnabrück BE LIABLE FOR ANY 20 | * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 21 | * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 22 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 23 | * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 24 | * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | */ 27 | 28 | /** 29 | * @file 30 | * 31 | * @brief Math functions for batched CPU memory 32 | * 33 | * @date 03.10.2022 34 | * @author Alexander Mock 35 | * 36 | * @copyright Copyright (c) 2022, University Osnabrück. All rights reserved. 37 | * This project is released under the 3-Clause BSD License. 38 | * 39 | */ 40 | 41 | #ifndef RMCL_MATH_BATCHED_H 42 | #define RMCL_MATH_BATCHED_H 43 | 44 | #include 45 | #include 46 | 47 | namespace rmcl 48 | { 49 | 50 | /** 51 | * @brief two-pass means and covariance computation 52 | * 53 | * @param dataset_points 54 | * @param model_points 55 | * @param mask 56 | * @param dataset_center 57 | * @param model_center 58 | * @param Cs 59 | * @param Ncorr 60 | */ 61 | void means_covs_batched( 62 | const rmagine::MemoryView& dataset_points, // from 63 | const rmagine::MemoryView& model_points, // to 64 | const rmagine::MemoryView& mask, 65 | rmagine::MemoryView& dataset_center, 66 | rmagine::MemoryView& model_center, 67 | rmagine::MemoryView& Cs, 68 | rmagine::MemoryView& Ncorr); 69 | 70 | /** 71 | * @brief one-pass means and covariance computation 72 | * 73 | * @param dataset_points 74 | * @param model_points 75 | * @param mask 76 | * @param dataset_center 77 | * @param model_center 78 | * @param Cs 79 | * @param Ncorr 80 | */ 81 | void means_covs_online_batched( 82 | const rmagine::MemoryView& dataset_points, // from 83 | const rmagine::MemoryView& model_points, // to 84 | const rmagine::MemoryView& mask, 85 | rmagine::MemoryView& dataset_center, 86 | rmagine::MemoryView& model_center, 87 | rmagine::MemoryView& Cs, 88 | rmagine::MemoryView& Ncorr); 89 | 90 | 91 | 92 | 93 | template 94 | struct PcdView 95 | { 96 | rmagine::MemoryView points; 97 | rmagine::MemoryView mask; 98 | rmagine::MemoryView normals; // optional: if memory view not set 99 | rmagine::MemoryView object_ids; 100 | 101 | // TODO: check if initializer list works: func(PcdView bla) -> func({points, mask, normals}) 102 | // PcdView inputs; 103 | // inputs.points = ; 104 | // func(inputs); // not good 105 | 106 | // func({}) 107 | }; 108 | 109 | struct UmeyamaReductionParams 110 | { 111 | float max_dist; 112 | }; 113 | 114 | 115 | 116 | void means_covs_p2l( 117 | const rmagine::MemoryView& pre_transforms, // N 118 | 119 | const PcdView& dataset, // from, M 120 | const PcdView& model, 121 | const UmeyamaReductionParams params, 122 | 123 | rmagine::MemoryView& dataset_center, 124 | rmagine::MemoryView& model_center, 125 | rmagine::MemoryView& Cs, 126 | rmagine::MemoryView& Ncorr, 127 | 128 | // this should not belong here, but this allows for defaults. 129 | int scene_id = -1, 130 | int object_id = -1, 131 | const rmagine::MemoryView& scene_mask = rmagine::Memory(0), // NxM 132 | const rmagine::MemoryView& object_mask = rmagine::Memory(0)// NxM 133 | ); 134 | 135 | 136 | // Poses: N 137 | // Scan size: M 138 | void means_covs_p2l_online_batched( 139 | const rmagine::MemoryView& pre_transforms, // N 140 | const rmagine::MemoryView& dataset_points, // from, M 141 | const rmagine::MemoryView& dataset_mask, // from, M 142 | const rmagine::MemoryView& model_points, // to, NxM 143 | const rmagine::MemoryView& model_normals, // to, NxM 144 | const rmagine::MemoryView& model_mask, // NxM 145 | const float max_corr_dist, 146 | rmagine::MemoryView& dataset_center, 147 | rmagine::MemoryView& model_center, 148 | rmagine::MemoryView& Cs, 149 | rmagine::MemoryView& Ncorr, 150 | 151 | // this should not belong here, but this allows for defaults. 152 | int scene_id = -1, 153 | int object_id = -1, 154 | const rmagine::MemoryView& scene_mask = rmagine::Memory(0), // NxM 155 | const rmagine::MemoryView& object_mask = rmagine::Memory(0)// NxM 156 | ); 157 | 158 | 159 | void incremental_covariance_object_wise( 160 | const rmagine::MemoryView& dataset_points, // from, M 161 | const rmagine::MemoryView& dataset_mask, // from, M 162 | const rmagine::MemoryView& model_points, // to, M 163 | const rmagine::MemoryView& model_normals, // to, M 164 | const rmagine::MemoryView& model_object_ids, // to, M 165 | const rmagine::MemoryView& model_mask, // to, M 166 | const float max_corr_dist, 167 | rmagine::MemoryView& dataset_centers, // per object id 168 | rmagine::MemoryView& model_centers, // per object 169 | rmagine::MemoryView& Cs, // per object 170 | rmagine::MemoryView& Ncorr // per object 171 | ); 172 | 173 | 174 | /** 175 | * @brief one-pass means and covariance computation 176 | * 177 | * @param dataset_points 178 | * @param model_points 179 | * @param mask 180 | * @param dataset_center 181 | * @param model_center 182 | * @param Cs 183 | * @param Ncorr 184 | */ 185 | void means_covs_online_approx_batched( 186 | const rmagine::MemoryView& dataset_points, // from 187 | const rmagine::MemoryView& model_points, // to 188 | const rmagine::MemoryView& mask, 189 | rmagine::MemoryView& dataset_center, 190 | rmagine::MemoryView& model_center, 191 | rmagine::MemoryView& Cs, 192 | rmagine::MemoryView& Ncorr); 193 | 194 | 195 | } // namespace rmcl 196 | 197 | #endif // RMCL_MATH_BATCHED_CUH 198 | -------------------------------------------------------------------------------- /rmcl/include/rmcl/registration/CPCEmbree.hpp: -------------------------------------------------------------------------------- 1 | #ifndef RMCL_CORRECTION_CORRESPONDENCES_CPC_EMBREE_HPP 2 | #define RMCL_CORRECTION_CORRESPONDENCES_CPC_EMBREE_HPP 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | #include 10 | 11 | 12 | namespace rmcl 13 | { 14 | 15 | class CPCEmbree 16 | : public CorrespondencesCPU 17 | { 18 | public: 19 | CPCEmbree(rmagine::EmbreeMapPtr map); 20 | 21 | void find( 22 | const rmagine::Transform& Tbm_est); 23 | 24 | protected: 25 | rmagine::EmbreeMapPtr map_; 26 | }; 27 | 28 | } // namespace rmcl 29 | 30 | #endif // RMCL_CORRECTION_CORRESPONDENCES_CPC_EMBREE_HPP -------------------------------------------------------------------------------- /rmcl/include/rmcl/registration/Correspondences.hpp: -------------------------------------------------------------------------------- 1 | #ifndef RMCL_CORRECTION_CORRESPONDENCES_HPP 2 | #define RMCL_CORRECTION_CORRESPONDENCES_HPP 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | 10 | #include 11 | 12 | 13 | namespace rmcl 14 | { 15 | 16 | template 17 | class Correspondences_ 18 | { 19 | public: 20 | 21 | // public attributes that have to be filled 22 | rmagine::UmeyamaReductionConstraints params; 23 | float adaptive_max_dist_min; 24 | rmagine::PointCloud_ dataset; 25 | 26 | /** 27 | * Mark dataset<->model correspondences as outdated: 28 | * - when a new dataset is added (new sensor data is acquired) 29 | * - model has substantially changed 30 | */ 31 | bool outdated = true; 32 | 33 | virtual void setTsb(const rmagine::Transform& Tsb) 34 | { 35 | Tsb_ = Tsb; 36 | }; 37 | 38 | /** 39 | * Finds and fill model buffers 40 | * 41 | */ 42 | virtual void find( 43 | const rmagine::Transform& Tbm_est 44 | ) = 0; 45 | 46 | 47 | inline rmagine::PointCloudView_ modelView() 48 | { 49 | return rmagine::PointCloudView_{ 50 | .points = model_buffers_.points, 51 | .mask = model_buffers_.hits, 52 | .normals = model_buffers_.normals 53 | }; 54 | } 55 | 56 | inline rmagine::PointCloudView_ datasetView() 57 | { 58 | return rmagine::PointCloudView_{ 59 | .points = dataset.points, 60 | .mask = dataset.mask 61 | }; 62 | } 63 | 64 | /** 65 | * 66 | * Computes cross statistics of correspondences that 67 | * can be used to solve the registration problem 68 | * via Kabsch/Umeyama. 69 | * 70 | * @param convergence_progress value (in [0,1]) describes an 71 | * estimate of convergence. (0: not converged at all, 1: fully 72 | * converged). It can be used, eg, to adapt a maximum search 73 | * distance. Otherwise this value can be ignored. 74 | */ 75 | virtual rmagine::CrossStatistics computeCrossStatistics( 76 | const rmagine::Transform& T_snew_sold, 77 | double convergence_progress = 0.0) const = 0; 78 | 79 | protected: 80 | 81 | rmagine::Bundle< 82 | rmagine::Points, // model points 83 | rmagine::Normals, // model normals 84 | rmagine::Hits // correspondence mask 85 | > model_buffers_; 86 | 87 | rmagine::Transform Tsb_; 88 | }; 89 | 90 | template 91 | Correspondences_ copy(const Correspondences_& corr_in) 92 | { 93 | Correspondences_ corr_out = corr_in; 94 | return corr_out; 95 | } 96 | 97 | } // namespace rmcl 98 | 99 | 100 | 101 | #endif // RMCL_CORRECTION_CORRESPONDENCES_HPP -------------------------------------------------------------------------------- /rmcl/include/rmcl/registration/CorrespondencesCPU.hpp: -------------------------------------------------------------------------------- 1 | #ifndef RMCL_CORRECTION_CORRESPONDENCES_CPU_HPP 2 | #define RMCL_CORRECTION_CORRESPONDENCES_CPU_HPP 3 | 4 | #include 5 | #include 6 | 7 | namespace rmcl 8 | { 9 | 10 | class CorrespondencesCPU 11 | : public Correspondences_ 12 | { 13 | public: 14 | rmagine::CrossStatistics computeCrossStatistics( 15 | const rmagine::Transform& T_snew_sold, 16 | double convergence_progress = 0.0 17 | ) const; 18 | }; 19 | 20 | } // namespace rmcl 21 | 22 | #endif // RMCL_CORRECTION_CORRESPONDENCES_CPU_HPP -------------------------------------------------------------------------------- /rmcl/include/rmcl/registration/CorrespondencesCUDA.hpp: -------------------------------------------------------------------------------- 1 | #ifndef RMCL_CORRECTION_CORRESPONDENCES_CUDA_HPP 2 | #define RMCL_CORRECTION_CORRESPONDENCES_CUDA_HPP 3 | 4 | #include 5 | #include 6 | 7 | namespace rmcl 8 | { 9 | 10 | class CorrespondencesCUDA 11 | : public Correspondences_ 12 | { 13 | public: 14 | rmagine::CrossStatistics computeCrossStatistics( 15 | const rmagine::Transform& T_snew_sold, 16 | double convergence_progress = 0.0) const; 17 | }; 18 | 19 | } // namespace rmcl 20 | 21 | #endif // RMCL_CORRECTION_CORRESPONDENCES_CUDA_HPP -------------------------------------------------------------------------------- /rmcl/include/rmcl/registration/RCCEmbree.hpp: -------------------------------------------------------------------------------- 1 | #ifndef RMCL_ROS_CORRECTION_RCC_EMBREE_HPP 2 | #define RMCL_ROS_CORRECTION_RCC_EMBREE_HPP 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | #include 9 | #include 10 | #include 11 | #include 12 | 13 | #include 14 | 15 | namespace rmcl 16 | { 17 | 18 | class RCCEmbreeSpherical 19 | : public CorrespondencesCPU 20 | , public rmagine::ModelSetter 21 | , protected rmagine::SphereSimulatorEmbree 22 | { 23 | public: 24 | 25 | RCCEmbreeSpherical( 26 | rmagine::EmbreeMapPtr map); 27 | 28 | void setModel(const rmagine::SphericalModel& sensor_model); 29 | 30 | virtual void setTsb(const rmagine::Transform& Tsb) override; 31 | 32 | virtual void find(const rmagine::Transform& Tbm_est); 33 | }; 34 | 35 | class RCCEmbreePinhole 36 | : public CorrespondencesCPU 37 | , public rmagine::ModelSetter 38 | , protected rmagine::PinholeSimulatorEmbree 39 | { 40 | public: 41 | RCCEmbreePinhole( 42 | rmagine::EmbreeMapPtr map); 43 | 44 | void setModel(const rmagine::PinholeModel& sensor_model); 45 | 46 | virtual void setTsb(const rmagine::Transform& Tsb) override; 47 | 48 | virtual void find(const rmagine::Transform& Tbm_est); 49 | }; 50 | 51 | class RCCEmbreeO1Dn 52 | : public CorrespondencesCPU 53 | , public rmagine::ModelSetter 54 | , protected rmagine::O1DnSimulatorEmbree 55 | { 56 | public: 57 | 58 | RCCEmbreeO1Dn( 59 | rmagine::EmbreeMapPtr map); 60 | 61 | void setModel(const rmagine::O1DnModel& sensor_model); 62 | 63 | virtual void setTsb(const rmagine::Transform& Tsb) override; 64 | 65 | virtual void find(const rmagine::Transform& Tbm_est); 66 | }; 67 | 68 | class RCCEmbreeOnDn 69 | : public CorrespondencesCPU 70 | , public rmagine::ModelSetter 71 | , protected rmagine::OnDnSimulatorEmbree 72 | { 73 | public: 74 | 75 | RCCEmbreeOnDn( 76 | rmagine::EmbreeMapPtr map); 77 | 78 | void setModel(const rmagine::OnDnModel& sensor_model); 79 | 80 | virtual void setTsb(const rmagine::Transform& Tsb) override; 81 | 82 | virtual void find(const rmagine::Transform& Tbm_est); 83 | }; 84 | 85 | } // namespace rmcl 86 | 87 | #endif // RMCL_ROS_CORRECTION_RCC_EMBREE_HPP -------------------------------------------------------------------------------- /rmcl/include/rmcl/registration/RCCOptix.hpp: -------------------------------------------------------------------------------- 1 | #ifndef RMCL_ROS_CORRECTION_RCC_OPTIX_HPP 2 | #define RMCL_ROS_CORRECTION_RCC_OPTIX_HPP 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | #include 9 | #include 10 | #include 11 | #include 12 | 13 | #include 14 | 15 | namespace rmcl 16 | { 17 | 18 | class RCCOptixSpherical 19 | : public CorrespondencesCUDA 20 | , public rmagine::ModelSetter 21 | , protected rmagine::SphereSimulatorOptix 22 | { 23 | public: 24 | 25 | RCCOptixSpherical( 26 | rmagine::OptixMapPtr map); 27 | 28 | void setModel(const rmagine::SphericalModel& sensor_model); 29 | 30 | virtual void setTsb(const rmagine::Transform& Tsb) override; 31 | 32 | virtual void find(const rmagine::Transform& Tbm_est); 33 | }; 34 | 35 | 36 | class RCCOptixPinhole 37 | : public CorrespondencesCUDA 38 | , public rmagine::ModelSetter 39 | , protected rmagine::PinholeSimulatorOptix 40 | { 41 | public: 42 | 43 | RCCOptixPinhole( 44 | rmagine::OptixMapPtr map); 45 | 46 | void setModel(const rmagine::PinholeModel& sensor_model); 47 | 48 | virtual void setTsb(const rmagine::Transform& Tsb) override; 49 | 50 | virtual void find(const rmagine::Transform& Tbm_est); 51 | }; 52 | 53 | 54 | class RCCOptixO1Dn 55 | : public CorrespondencesCUDA 56 | , public rmagine::ModelSetter 57 | , protected rmagine::O1DnSimulatorOptix 58 | { 59 | public: 60 | 61 | RCCOptixO1Dn( 62 | rmagine::OptixMapPtr map); 63 | 64 | void setModel(const rmagine::O1DnModel& sensor_model); 65 | 66 | virtual void setTsb(const rmagine::Transform& Tsb) override; 67 | 68 | virtual void find(const rmagine::Transform& Tbm_est); 69 | }; 70 | 71 | class RCCOptixOnDn 72 | : public CorrespondencesCUDA 73 | , public rmagine::ModelSetter 74 | , protected rmagine::OnDnSimulatorOptix 75 | { 76 | public: 77 | 78 | RCCOptixOnDn( 79 | rmagine::OptixMapPtr map); 80 | 81 | void setModel(const rmagine::OnDnModel& sensor_model); 82 | 83 | virtual void setTsb(const rmagine::Transform& Tsb) override; 84 | 85 | virtual void find(const rmagine::Transform& Tbm_est); 86 | }; 87 | 88 | 89 | } // namespace rmcl 90 | 91 | #endif // RMCL_ROS_CORRECTION_RCC_EMBREE_HPP -------------------------------------------------------------------------------- /rmcl/include/rmcl/spatial/KdTree.hpp: -------------------------------------------------------------------------------- 1 | /** 2 | * Copyright (c) 2021, University Osnabrück 3 | * All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * * Redistributions of source code must retain the above copyright 8 | * notice, this list of conditions and the following disclaimer. 9 | * * Redistributions in binary form must reproduce the above copyright 10 | * notice, this list of conditions and the following disclaimer in the 11 | * documentation and/or other materials provided with the distribution. 12 | * * Neither the name of the University Osnabrück nor the 13 | * names of its contributors may be used to endorse or promote products 14 | * derived from this software without specific prior written permission. 15 | * 16 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 17 | * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 18 | * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 19 | * DISCLAIMED. IN NO EVENT SHALL University Osnabrück BE LIABLE FOR ANY 20 | * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 21 | * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 22 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 23 | * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 24 | * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | */ 27 | 28 | /* 29 | * KdTree.hpp 30 | * 31 | * Created on: Jul 17, 2021 32 | * Author: Alexander Mock 33 | */ 34 | 35 | #ifndef MAMCL_SPATIAL_KDTREE_HPP 36 | #define MAMCL_SPATIAL_KDTREE_HPP 37 | 38 | 39 | #include 40 | #include 41 | #include 42 | #include 43 | 44 | #include 45 | 46 | namespace rmcl { 47 | 48 | class KdPoints 49 | { 50 | public: 51 | KdPoints(const rmagine::Memory& mem) 52 | :m_mem(mem) 53 | { 54 | 55 | } 56 | 57 | inline size_t kdtree_get_point_count() const { 58 | return m_mem.size(); 59 | } 60 | 61 | inline float kdtree_get_pt(const size_t idx, const size_t dim) const 62 | { 63 | if(dim == 0) { 64 | return m_mem[idx].x; 65 | } else if(dim == 1) { 66 | return m_mem[idx].y; 67 | } else if(dim == 2) { 68 | return m_mem[idx].z; 69 | } else { 70 | throw std::runtime_error("Tried to access dim 4 on vector"); 71 | return 0.0; 72 | } 73 | } 74 | 75 | // Optional bounding-box computation: return false to default to a standard bbox computation loop. 76 | // Return true if the BBOX was already computed by the class and returned in "bb" so it can be avoided to redo it again. 77 | // Look at bb.size() to find out the expected dimensionality (e.g. 2 or 3 for point clouds) 78 | template 79 | bool kdtree_get_bbox(BBOX& /* bb */) const { return false; } 80 | 81 | const rmagine::Memory& m_mem; 82 | }; 83 | 84 | using KdPointsPtr = std::shared_ptr; 85 | 86 | class KdTree : public nanoflann::KDTreeSingleIndexAdaptor< 87 | nanoflann::L2_Simple_Adaptor , 88 | KdPoints, 3 /* dim */> 89 | { 90 | public: 91 | using Super = nanoflann::KDTreeSingleIndexAdaptor< 92 | nanoflann::L2_Simple_Adaptor , 93 | KdPoints, 3 /* dim */ >; 94 | 95 | KdTree(KdPointsPtr points); 96 | 97 | rmagine::Vector nearest( 98 | const rmagine::Vector& query_point, 99 | bool query_in_index = false) const; 100 | 101 | float nearestDist( 102 | const rmagine::Vector& query_point, 103 | bool query_in_index = false) const; 104 | 105 | size_t nearestId( 106 | const rmagine::Vector& query_point, 107 | bool query_in_index = false) const; 108 | 109 | const KdPointsPtr dataset() const 110 | { 111 | return m_points; 112 | } 113 | 114 | protected: 115 | KdPointsPtr m_points; 116 | }; 117 | 118 | using KdTreePtr = std::shared_ptr; 119 | 120 | } // namespace rmcl 121 | 122 | #endif // RMCL_SPATIAL_KDTREE_HPP -------------------------------------------------------------------------------- /rmcl/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | rmcl 4 | 2.2.0 5 | The rmcl package 6 | 7 | Alexander Mock 8 | 9 | BSD 10 | Alexander Mock 11 | 12 | cmake 13 | 14 | rmagine 15 | 16 | 17 | cmake 18 | 19 | 20 | 21 | -------------------------------------------------------------------------------- /rmcl/src/rmcl/clustering/clustering.cpp: -------------------------------------------------------------------------------- 1 | #include "rmcl/clustering/clustering.h" 2 | 3 | namespace rmcl 4 | { 5 | 6 | void sort_clusters(std::vector >& clusters) 7 | { 8 | std::sort(clusters.begin(), clusters.end(), 9 | [](const std::vector& a, const std::vector& b) -> bool 10 | { 11 | return a.size() > b.size(); 12 | } 13 | ); 14 | 15 | // for(auto& cluster: clusters) 16 | // { 17 | // std::sort(cluster.begin(), cluster.end()); 18 | // } 19 | } 20 | 21 | std::vector > dbscan( 22 | KdTreePtr index, 23 | float search_dist, 24 | unsigned int min_pts_in_radius, 25 | unsigned int min_pts_per_cluster) 26 | { 27 | size_t Npoints = index->m_size; 28 | 29 | std::vector > clusters; 30 | 31 | std::vector visited(Npoints); 32 | std::vector > matches; 33 | std::vector > sub_matches; 34 | 35 | auto data = index->dataset(); 36 | 37 | for(size_t i = 0; i < Npoints; i++) 38 | { 39 | if (visited[i]) 40 | { 41 | continue; 42 | } 43 | 44 | index->radiusSearch( 45 | reinterpret_cast(&data->m_mem[i].x), 46 | search_dist, matches, 47 | nanoflann::SearchParams(32, 0.f, false)); 48 | 49 | if (matches.size() < static_cast(min_pts_in_radius)) 50 | { 51 | continue; 52 | } 53 | 54 | visited[i] = true; 55 | 56 | std::vector cluster = {i}; 57 | 58 | while (matches.empty() == false) 59 | { 60 | auto nb_idx = matches.back().first; 61 | matches.pop_back(); 62 | if (visited[nb_idx]) continue; 63 | visited[nb_idx] = true; 64 | 65 | index->radiusSearch( 66 | reinterpret_cast(&data->m_mem[nb_idx].x), 67 | search_dist, sub_matches, 68 | nanoflann::SearchParams(32, 0.f, false)); 69 | 70 | if (sub_matches.size() >= static_cast(min_pts_in_radius)) 71 | { 72 | std::copy(sub_matches.begin(), sub_matches.end(), std::back_inserter(matches)); 73 | } 74 | cluster.push_back(nb_idx); 75 | } 76 | 77 | if(cluster.size() >= min_pts_per_cluster) 78 | { 79 | clusters.emplace_back(std::move(cluster)); 80 | } 81 | } 82 | 83 | return clusters; 84 | } 85 | 86 | } // namespace rmcl -------------------------------------------------------------------------------- /rmcl/src/rmcl/registration/CPCEmbree.cpp: -------------------------------------------------------------------------------- 1 | #include "rmcl/registration/CPCEmbree.hpp" 2 | 3 | namespace rm = rmagine; 4 | 5 | namespace rmcl 6 | { 7 | 8 | CPCEmbree::CPCEmbree( 9 | rm::EmbreeMapPtr map) 10 | :map_(map) 11 | { 12 | 13 | } 14 | 15 | void CPCEmbree::find(const rm::Transform& Tbm_est) 16 | { 17 | size_t n_old_measurements = model_buffers_.points.size(); 18 | size_t n_new_measurements = dataset.points.size(); 19 | if(n_new_measurements > n_old_measurements) 20 | { 21 | rm::resize_memory_bundle(model_buffers_, dataset.points.size(), 1, 1); 22 | } 23 | 24 | const rm::Transform Tsm = Tbm_est * Tsb_; 25 | const rm::Transform Tms = ~Tsm; 26 | 27 | #pragma omp parallel for 28 | for(size_t i=0; iclosestPoint(Pm); 32 | 33 | model_buffers_.hits[i] = (cp.d <= params.max_dist); 34 | model_buffers_.points[i] = Tms * cp.p; 35 | model_buffers_.normals[i] = Tms.R * cp.n; 36 | } 37 | } 38 | 39 | 40 | } // namespace rmcl -------------------------------------------------------------------------------- /rmcl/src/rmcl/registration/CorrespondencesCPU.cpp: -------------------------------------------------------------------------------- 1 | #include "rmcl/registration/CorrespondencesCPU.hpp" 2 | #include 3 | #include 4 | 5 | namespace rm = rmagine; 6 | 7 | namespace rmcl 8 | { 9 | 10 | rmagine::CrossStatistics CorrespondencesCPU::computeCrossStatistics( 11 | const rmagine::Transform& T_snew_sold, 12 | double convergence_progress) const 13 | { 14 | const rm::PointCloudView_ cloud_dataset = rm::watch(dataset); 15 | const rm::PointCloudView_ cloud_model = { 16 | .points = model_buffers_.points, 17 | .mask = model_buffers_.hits, 18 | .normals = model_buffers_.normals 19 | }; 20 | 21 | rm::UmeyamaReductionConstraints params_local = params; 22 | params_local.max_dist = params.max_dist * (1.0 - convergence_progress) 23 | + adaptive_max_dist_min * convergence_progress; 24 | 25 | 26 | const rm::CrossStatistics stats_s = rm::statistics_p2l( 27 | T_snew_sold, 28 | cloud_dataset, 29 | cloud_model, 30 | params_local); 31 | 32 | // std::cout << "CrossStatistics:" << std::endl; 33 | // std::cout << stats_s.n_meas << std::endl; 34 | // std::cout << stats_s.dataset_mean << " -> " << stats_s.model_mean << std::endl; 35 | // std::cout << stats_s.covariance << std::endl; 36 | 37 | 38 | return stats_s; 39 | } 40 | 41 | } // namespace rmcl -------------------------------------------------------------------------------- /rmcl/src/rmcl/registration/CorrespondencesCUDA.cpp: -------------------------------------------------------------------------------- 1 | #include "rmcl/registration/CorrespondencesCUDA.hpp" 2 | #include 3 | 4 | namespace rm = rmagine; 5 | 6 | namespace rmcl 7 | { 8 | 9 | rm::CrossStatistics CorrespondencesCUDA::computeCrossStatistics( 10 | const rm::Transform& T_snew_sold, 11 | double convergence_progress) const 12 | { 13 | const rm::PointCloudView_ cloud_dataset = rm::watch(dataset); 14 | const rm::PointCloudView_ cloud_model = { 15 | .points = model_buffers_.points, 16 | .mask = model_buffers_.hits, 17 | .normals = model_buffers_.normals 18 | }; 19 | 20 | // linearly interpolate between params.max_dist and adaptive_max_dist_min 21 | // convergence_progress == 0.0 -> max_dist = params.max_dist 22 | // convergence_progress == 1.0 -> max_dist = adaptive_max_dist_min 23 | rm::UmeyamaReductionConstraints params_local = params; 24 | params_local.max_dist = params.max_dist * (1.0 - convergence_progress) 25 | + adaptive_max_dist_min * convergence_progress; 26 | 27 | 28 | const rm::CrossStatistics stats_s = rm::statistics_p2l(T_snew_sold, cloud_dataset, cloud_model, params_local); 29 | return stats_s; 30 | } 31 | 32 | } // namespace rmcl -------------------------------------------------------------------------------- /rmcl/src/rmcl/registration/RCCEmbree.cpp: -------------------------------------------------------------------------------- 1 | #include "rmcl/registration/RCCEmbree.hpp" 2 | 3 | namespace rm = rmagine; 4 | 5 | namespace rmcl 6 | { 7 | 8 | RCCEmbreeSpherical::RCCEmbreeSpherical( 9 | rm::EmbreeMapPtr map) 10 | : rm::SphereSimulatorEmbree(map) 11 | { 12 | 13 | } 14 | 15 | void RCCEmbreeSpherical::setTsb(const rm::Transform& Tsb) 16 | { 17 | CorrespondencesCPU::setTsb(Tsb); 18 | rm::SphereSimulatorEmbree::setTsb(Tsb); 19 | } 20 | 21 | void RCCEmbreeSpherical::setModel(const rm::SphericalModel& sensor_model) 22 | { 23 | rm::SphereSimulatorEmbree::setModel(sensor_model); 24 | } 25 | 26 | void RCCEmbreeSpherical::find(const rm::Transform& Tbm_est) 27 | { 28 | size_t n_old_measurements = model_buffers_.points.size(); 29 | size_t n_new_measurements = m_model->size(); 30 | if(n_new_measurements > n_old_measurements) 31 | { 32 | rm::resize_memory_bundle(model_buffers_, m_model->getHeight(), m_model->getWidth(), 1); 33 | } 34 | 35 | simulate(Tbm_est, model_buffers_); 36 | } 37 | 38 | 39 | 40 | RCCEmbreePinhole::RCCEmbreePinhole( 41 | rm::EmbreeMapPtr map) 42 | : rm::PinholeSimulatorEmbree(map) 43 | { 44 | 45 | } 46 | 47 | void RCCEmbreePinhole::setTsb(const rm::Transform& Tsb) 48 | { 49 | CorrespondencesCPU::setTsb(Tsb); 50 | rm::PinholeSimulatorEmbree::setTsb(Tsb); 51 | } 52 | 53 | void RCCEmbreePinhole::setModel(const rm::PinholeModel& sensor_model) 54 | { 55 | rm::PinholeSimulatorEmbree::setModel(sensor_model); 56 | } 57 | 58 | void RCCEmbreePinhole::find(const rm::Transform& Tbm_est) 59 | { 60 | size_t n_old_measurements = model_buffers_.points.size(); 61 | size_t n_new_measurements = m_model->size(); 62 | if(n_new_measurements > n_old_measurements) 63 | { 64 | rm::resize_memory_bundle(model_buffers_, m_model->getHeight(), m_model->getWidth(), 1); 65 | } 66 | 67 | simulate(Tbm_est, model_buffers_); 68 | } 69 | 70 | 71 | RCCEmbreeO1Dn::RCCEmbreeO1Dn( 72 | rm::EmbreeMapPtr map) 73 | : rm::O1DnSimulatorEmbree(map) 74 | { 75 | 76 | } 77 | 78 | void RCCEmbreeO1Dn::setTsb(const rm::Transform& Tsb) 79 | { 80 | CorrespondencesCPU::setTsb(Tsb); 81 | rm::O1DnSimulatorEmbree::setTsb(Tsb); 82 | } 83 | 84 | void RCCEmbreeO1Dn::setModel(const rm::O1DnModel& sensor_model) 85 | { 86 | rm::O1DnSimulatorEmbree::setModel(sensor_model); 87 | } 88 | 89 | void RCCEmbreeO1Dn::find(const rm::Transform& Tbm_est) 90 | { 91 | size_t n_old_measurements = model_buffers_.points.size(); 92 | size_t n_new_measurements = m_model->size(); 93 | if(n_new_measurements > n_old_measurements) 94 | { 95 | rm::resize_memory_bundle(model_buffers_, m_model->getHeight(), m_model->getWidth(), 1); 96 | } 97 | 98 | simulate(Tbm_est, model_buffers_); 99 | } 100 | 101 | 102 | 103 | RCCEmbreeOnDn::RCCEmbreeOnDn( 104 | rm::EmbreeMapPtr map) 105 | : rm::OnDnSimulatorEmbree(map) 106 | { 107 | 108 | } 109 | 110 | void RCCEmbreeOnDn::setTsb(const rm::Transform& Tsb) 111 | { 112 | CorrespondencesCPU::setTsb(Tsb); 113 | rm::OnDnSimulatorEmbree::setTsb(Tsb); 114 | } 115 | 116 | void RCCEmbreeOnDn::setModel(const rm::OnDnModel& sensor_model) 117 | { 118 | rm::OnDnSimulatorEmbree::setModel(sensor_model); 119 | } 120 | 121 | void RCCEmbreeOnDn::find(const rm::Transform& Tbm_est) 122 | { 123 | size_t n_old_measurements = model_buffers_.points.size(); 124 | size_t n_new_measurements = m_model->size(); 125 | if(n_new_measurements > n_old_measurements) 126 | { 127 | rm::resize_memory_bundle(model_buffers_, m_model->getHeight(), m_model->getWidth(), 1); 128 | } 129 | 130 | simulate(Tbm_est, model_buffers_); 131 | } 132 | 133 | 134 | 135 | } // namespace rmcl -------------------------------------------------------------------------------- /rmcl/src/rmcl/registration/RCCOptix.cpp: -------------------------------------------------------------------------------- 1 | #include "rmcl/registration/RCCOptix.hpp" 2 | 3 | namespace rm = rmagine; 4 | 5 | namespace rmcl 6 | { 7 | 8 | 9 | RCCOptixSpherical::RCCOptixSpherical( 10 | rm::OptixMapPtr map) 11 | : rm::SphereSimulatorOptix(map) 12 | { 13 | 14 | } 15 | 16 | void RCCOptixSpherical::setTsb(const rm::Transform& Tsb) 17 | { 18 | CorrespondencesCUDA::setTsb(Tsb); 19 | rm::SphereSimulatorOptix::setTsb(Tsb); 20 | } 21 | 22 | void RCCOptixSpherical::setModel(const rm::SphericalModel& sensor_model) 23 | { 24 | rm::SphereSimulatorOptix::setModel(sensor_model); 25 | } 26 | 27 | void RCCOptixSpherical::find(const rm::Transform& Tbm_est) 28 | { 29 | size_t n_old_measurements = model_buffers_.points.size(); 30 | size_t n_new_measurements = m_model->size(); 31 | if(n_new_measurements > n_old_measurements) 32 | { 33 | rm::resize_memory_bundle(model_buffers_, m_model->getHeight(), m_model->getWidth(), 1); 34 | } 35 | 36 | simulate(Tbm_est, model_buffers_); 37 | } 38 | 39 | 40 | RCCOptixPinhole::RCCOptixPinhole( 41 | rm::OptixMapPtr map) 42 | : rm::PinholeSimulatorOptix(map) 43 | { 44 | 45 | } 46 | 47 | void RCCOptixPinhole::setTsb(const rm::Transform& Tsb) 48 | { 49 | CorrespondencesCUDA::setTsb(Tsb); 50 | rm::PinholeSimulatorOptix::setTsb(Tsb); 51 | } 52 | 53 | void RCCOptixPinhole::setModel(const rm::PinholeModel& sensor_model) 54 | { 55 | rm::PinholeSimulatorOptix::setModel(sensor_model); 56 | } 57 | 58 | void RCCOptixPinhole::find(const rm::Transform& Tbm_est) 59 | { 60 | size_t n_old_measurements = model_buffers_.points.size(); 61 | size_t n_new_measurements = m_model->size(); 62 | if(n_new_measurements > n_old_measurements) 63 | { 64 | rm::resize_memory_bundle(model_buffers_, m_model->getHeight(), m_model->getWidth(), 1); 65 | } 66 | 67 | simulate(Tbm_est, model_buffers_); 68 | } 69 | 70 | 71 | RCCOptixO1Dn::RCCOptixO1Dn( 72 | rm::OptixMapPtr map) 73 | : rm::O1DnSimulatorOptix(map) 74 | { 75 | 76 | } 77 | 78 | void RCCOptixO1Dn::setTsb(const rm::Transform& Tsb) 79 | { 80 | CorrespondencesCUDA::setTsb(Tsb); 81 | rm::O1DnSimulatorOptix::setTsb(Tsb); 82 | } 83 | 84 | void RCCOptixO1Dn::setModel(const rm::O1DnModel& sensor_model) 85 | { 86 | rm::O1DnSimulatorOptix::setModel(sensor_model); 87 | } 88 | 89 | void RCCOptixO1Dn::find(const rm::Transform& Tbm_est) 90 | { 91 | size_t n_old_measurements = model_buffers_.points.size(); 92 | size_t n_new_measurements = m_model->size(); 93 | if(n_new_measurements > n_old_measurements) 94 | { 95 | rm::resize_memory_bundle(model_buffers_, m_model->getHeight(), m_model->getWidth(), 1); 96 | } 97 | 98 | simulate(Tbm_est, model_buffers_); 99 | } 100 | 101 | 102 | RCCOptixOnDn::RCCOptixOnDn( 103 | rm::OptixMapPtr map) 104 | : rm::OnDnSimulatorOptix(map) 105 | { 106 | 107 | } 108 | 109 | void RCCOptixOnDn::setTsb(const rm::Transform& Tsb) 110 | { 111 | CorrespondencesCUDA::setTsb(Tsb); 112 | rm::OnDnSimulatorOptix::setTsb(Tsb); 113 | } 114 | 115 | void RCCOptixOnDn::setModel(const rm::OnDnModel& sensor_model) 116 | { 117 | rm::OnDnSimulatorOptix::setModel(sensor_model); 118 | } 119 | 120 | void RCCOptixOnDn::find(const rm::Transform& Tbm_est) 121 | { 122 | size_t n_old_measurements = model_buffers_.points.size(); 123 | size_t n_new_measurements = m_model->size(); 124 | if(n_new_measurements > n_old_measurements) 125 | { 126 | rm::resize_memory_bundle(model_buffers_, m_model->getHeight(), m_model->getWidth(), 1); 127 | } 128 | 129 | simulate(Tbm_est, model_buffers_); 130 | } 131 | 132 | 133 | } // namespace rmcl -------------------------------------------------------------------------------- /rmcl/src/rmcl/spatial/KdTree.cpp: -------------------------------------------------------------------------------- 1 | #include "rmcl/spatial/KdTree.hpp" 2 | 3 | using namespace rmagine; 4 | 5 | namespace rmcl { 6 | 7 | KdTree::KdTree(KdPointsPtr points) 8 | :m_points(points) 9 | ,Super(3, *points, nanoflann::KDTreeSingleIndexAdaptorParams(10) ) 10 | { 11 | buildIndex(); 12 | } 13 | 14 | Vector KdTree::nearest( 15 | const Vector& query_point, 16 | bool query_in_index) const 17 | { 18 | if(query_in_index) 19 | { 20 | const size_t num_results = 2; 21 | nanoflann::KNNResultSet resultSet(num_results); 22 | size_t ret_index[2]; 23 | float out_dist_sqr[2]; 24 | resultSet.init(ret_index, out_dist_sqr); 25 | findNeighbors(resultSet, reinterpret_cast(&query_point), nanoflann::SearchParams(10)); 26 | return m_points->m_mem.at(ret_index[1]); 27 | } else { 28 | const size_t num_results = 1; 29 | nanoflann::KNNResultSet resultSet(num_results); 30 | size_t ret_index; 31 | float out_dist_sqr; 32 | resultSet.init(&ret_index, &out_dist_sqr); 33 | findNeighbors(resultSet, reinterpret_cast(&query_point), nanoflann::SearchParams(10)); 34 | return m_points->m_mem.at(ret_index); 35 | } 36 | } 37 | 38 | float KdTree::nearestDist( 39 | const Vector& query_point, 40 | bool query_in_index) const 41 | { 42 | if(query_in_index) 43 | { 44 | const size_t num_results = 2; 45 | nanoflann::KNNResultSet resultSet(num_results); 46 | size_t ret_index[2]; 47 | float out_dist_sqr[2]; 48 | resultSet.init(ret_index, out_dist_sqr); 49 | findNeighbors(resultSet, reinterpret_cast(&query_point), nanoflann::SearchParams(10)); 50 | return out_dist_sqr[1]; 51 | } else { 52 | const size_t num_results = 1; 53 | nanoflann::KNNResultSet resultSet(num_results); 54 | size_t ret_index; 55 | float out_dist_sqr; 56 | resultSet.init(&ret_index, &out_dist_sqr); 57 | findNeighbors(resultSet, reinterpret_cast(&query_point), nanoflann::SearchParams(10)); 58 | return out_dist_sqr; 59 | } 60 | } 61 | 62 | size_t KdTree::nearestId( 63 | const Vector& query_point, 64 | bool query_in_index) const 65 | { 66 | if(query_in_index) 67 | { 68 | const size_t num_results = 2; 69 | nanoflann::KNNResultSet resultSet(num_results); 70 | size_t ret_index[2]; 71 | float out_dist_sqr[2]; 72 | resultSet.init(ret_index, out_dist_sqr); 73 | findNeighbors(resultSet, reinterpret_cast(&query_point), nanoflann::SearchParams(10)); 74 | return ret_index[1]; 75 | } else { 76 | const size_t num_results = 1; 77 | nanoflann::KNNResultSet resultSet(num_results); 78 | size_t ret_index; 79 | float out_dist_sqr; 80 | resultSet.init(&ret_index, &out_dist_sqr); 81 | findNeighbors(resultSet, reinterpret_cast(&query_point), nanoflann::SearchParams(10)); 82 | return ret_index; 83 | } 84 | } 85 | 86 | } // namespace rmcl -------------------------------------------------------------------------------- /rmcl_msgs/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.8) 2 | project(rmcl_msgs VERSION 2.2.0) 3 | 4 | # Default to C++17 5 | if(NOT CMAKE_CXX_STANDARD) 6 | set(CMAKE_CXX_STANDARD 17) 7 | set(CMAKE_CXX_STANDARD_REQUIRED ON) 8 | endif() 9 | if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") 10 | add_compile_options(-Wall -Wextra -Wpedantic) 11 | endif() 12 | 13 | find_package(ament_cmake REQUIRED) 14 | find_package(rosidl_default_generators REQUIRED) 15 | find_package(geometry_msgs REQUIRED) 16 | find_package(sensor_msgs REQUIRED) 17 | find_package(std_msgs REQUIRED) 18 | 19 | rosidl_generate_interfaces(${PROJECT_NAME} 20 | "msg/PolarCoord.msg" 21 | "msg/RangeData.msg" 22 | "msg/RangeDataStamped.msg" 23 | # Spherical 24 | "msg/ScanInfo.msg" 25 | "msg/ScanInfoStamped.msg" 26 | "msg/Scan.msg" 27 | "msg/ScanStamped.msg" 28 | # Pinhole 29 | "msg/DepthInfo.msg" 30 | "msg/DepthInfoStamped.msg" 31 | "msg/Depth.msg" 32 | "msg/DepthStamped.msg" 33 | # O1Dn 34 | "msg/O1DnInfo.msg" 35 | "msg/O1DnInfoStamped.msg" 36 | "msg/O1Dn.msg" 37 | "msg/O1DnStamped.msg" 38 | # OnDn 39 | "msg/OnDnInfo.msg" 40 | "msg/OnDnInfoStamped.msg" 41 | "msg/OnDn.msg" 42 | "msg/OnDnStamped.msg" 43 | # MICP 44 | "msg/MICPSensorStats.msg" 45 | "msg/MICPStats.msg" 46 | DEPENDENCIES 47 | std_msgs 48 | geometry_msgs 49 | sensor_msgs 50 | ADD_LINTER_TESTS 51 | ) 52 | 53 | ament_export_dependencies(rosidl_default_runtime) 54 | ament_package() -------------------------------------------------------------------------------- /rmcl_msgs/LICENSE: -------------------------------------------------------------------------------- 1 | BSD 3-Clause License 2 | 3 | Copyright (c) 2024, Alexander Mock 4 | All rights reserved. 5 | 6 | Redistribution and use in source and binary forms, with or without 7 | modification, are permitted provided that the following conditions are met: 8 | 9 | 1. Redistributions of source code must retain the above copyright notice, this 10 | list of conditions and the following disclaimer. 11 | 12 | 2. Redistributions in binary form must reproduce the above copyright notice, 13 | this list of conditions and the following disclaimer in the documentation 14 | and/or other materials provided with the distribution. 15 | 16 | 3. Neither the name of the copyright holder nor the names of its 17 | contributors may be used to endorse or promote products derived from 18 | this software without specific prior written permission. 19 | 20 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 21 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 22 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 23 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 24 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 25 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 26 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 27 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 28 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 29 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 30 | -------------------------------------------------------------------------------- /rmcl_msgs/README.md: -------------------------------------------------------------------------------- 1 | # rmcl_msgs 2 | 3 | Messages used by [RMCL](https://github.com/uos/rmcl). -------------------------------------------------------------------------------- /rmcl_msgs/msg/Depth.msg: -------------------------------------------------------------------------------- 1 | DepthInfo info 2 | RangeData data -------------------------------------------------------------------------------- /rmcl_msgs/msg/DepthInfo.msg: -------------------------------------------------------------------------------- 1 | float32 fx 2 | float32 fy 3 | float32 cx 4 | float32 cy 5 | uint32 width 6 | uint32 height 7 | float32 range_min 8 | float32 range_max -------------------------------------------------------------------------------- /rmcl_msgs/msg/DepthInfoStamped.msg: -------------------------------------------------------------------------------- 1 | std_msgs/Header header 2 | DepthInfo info -------------------------------------------------------------------------------- /rmcl_msgs/msg/DepthStamped.msg: -------------------------------------------------------------------------------- 1 | std_msgs/Header header 2 | Depth depth -------------------------------------------------------------------------------- /rmcl_msgs/msg/MICPSensorStats.msg: -------------------------------------------------------------------------------- 1 | std_msgs/Header header 2 | builtin_interfaces/Time measurement_stamp 3 | uint32 total_measurements 4 | uint32 valid_measurements 5 | uint32 valid_matches 6 | float32 cov_trace -------------------------------------------------------------------------------- /rmcl_msgs/msg/MICPStats.msg: -------------------------------------------------------------------------------- 1 | MICPSensorStats[] sensors -------------------------------------------------------------------------------- /rmcl_msgs/msg/O1Dn.msg: -------------------------------------------------------------------------------- 1 | O1DnInfo info 2 | RangeData data -------------------------------------------------------------------------------- /rmcl_msgs/msg/O1DnInfo.msg: -------------------------------------------------------------------------------- 1 | uint32 width 2 | uint32 height 3 | float32 range_min 4 | float32 range_max 5 | geometry_msgs/Point32 orig 6 | geometry_msgs/Point32[] dirs -------------------------------------------------------------------------------- /rmcl_msgs/msg/O1DnInfoStamped.msg: -------------------------------------------------------------------------------- 1 | std_msgs/Header header 2 | O1DnInfo info -------------------------------------------------------------------------------- /rmcl_msgs/msg/O1DnStamped.msg: -------------------------------------------------------------------------------- 1 | std_msgs/Header header 2 | O1Dn o1dn -------------------------------------------------------------------------------- /rmcl_msgs/msg/OnDn.msg: -------------------------------------------------------------------------------- 1 | OnDnInfo info 2 | RangeData data -------------------------------------------------------------------------------- /rmcl_msgs/msg/OnDnInfo.msg: -------------------------------------------------------------------------------- 1 | uint32 width 2 | uint32 height 3 | float32 range_min 4 | float32 range_max 5 | geometry_msgs/Point32[] origs 6 | geometry_msgs/Point32[] dirs -------------------------------------------------------------------------------- /rmcl_msgs/msg/OnDnInfoStamped.msg: -------------------------------------------------------------------------------- 1 | std_msgs/Header header 2 | OnDnInfo info -------------------------------------------------------------------------------- /rmcl_msgs/msg/OnDnStamped.msg: -------------------------------------------------------------------------------- 1 | std_msgs/Header header 2 | OnDn ondn -------------------------------------------------------------------------------- /rmcl_msgs/msg/PolarCoord.msg: -------------------------------------------------------------------------------- 1 | # RANGE: range 2 | # PHI: vertical, y-rot, pitch, polar angle 3 | # THETA: horizontal, z-rot, yaw, azimuth 4 | float32 phi 5 | float32 theta 6 | float32 range -------------------------------------------------------------------------------- /rmcl_msgs/msg/RangeData.msg: -------------------------------------------------------------------------------- 1 | # Computed ranges of a model's ray buffer 2 | float32[] ranges # required 3 | bool[] mask 4 | # We can attach a list of attributes to the ray buffer 5 | # An zero length list marks a missing attribute 6 | geometry_msgs/Point32[] normals 7 | std_msgs/ColorRGBA[] colors 8 | float32[] stamps 9 | float32[] intensities 10 | uint32[] labels # eg, obj id -------------------------------------------------------------------------------- /rmcl_msgs/msg/RangeDataStamped.msg: -------------------------------------------------------------------------------- 1 | std_msgs/Header header 2 | RangeData data -------------------------------------------------------------------------------- /rmcl_msgs/msg/Scan.msg: -------------------------------------------------------------------------------- 1 | ScanInfo info 2 | RangeData data -------------------------------------------------------------------------------- /rmcl_msgs/msg/ScanInfo.msg: -------------------------------------------------------------------------------- 1 | # PHI: vertical, y-rot, pitch, polar angle, height 2 | float32 phi_min 3 | float32 phi_inc 4 | uint32 phi_n 5 | 6 | # THETA: horizontal, z-rot, yaw, azimuth, width 7 | float32 theta_min 8 | float32 theta_inc 9 | uint32 theta_n 10 | 11 | # RANGE: range 12 | # range is valid if <= range_max && >= range_min 13 | float32 range_min 14 | float32 range_max -------------------------------------------------------------------------------- /rmcl_msgs/msg/ScanInfoStamped.msg: -------------------------------------------------------------------------------- 1 | std_msgs/Header header 2 | ScanInfo info -------------------------------------------------------------------------------- /rmcl_msgs/msg/ScanStamped.msg: -------------------------------------------------------------------------------- 1 | std_msgs/Header header 2 | Scan scan -------------------------------------------------------------------------------- /rmcl_msgs/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | rmcl_msgs 5 | 2.2.0 6 | The rmcl_msgs package 7 | Alexander Mock 8 | Alexander Mock 9 | BSD 10 | 11 | ament_cmake 12 | rosidl_default_generators 13 | 14 | geometry_msgs 15 | sensor_msgs 16 | std_msgs 17 | 18 | rosidl_default_runtime 19 | 20 | ament_lint_common 21 | 22 | rosidl_interface_packages 23 | 24 | 25 | ament_cmake 26 | 27 | 28 | -------------------------------------------------------------------------------- /rmcl_ros/LICENSE: -------------------------------------------------------------------------------- 1 | BSD 3-Clause License 2 | 3 | Copyright (c) 2024, Alexander Mock 4 | All rights reserved. 5 | 6 | Redistribution and use in source and binary forms, with or without 7 | modification, are permitted provided that the following conditions are met: 8 | 9 | 1. Redistributions of source code must retain the above copyright notice, this 10 | list of conditions and the following disclaimer. 11 | 12 | 2. Redistributions in binary form must reproduce the above copyright notice, 13 | this list of conditions and the following disclaimer in the documentation 14 | and/or other materials provided with the distribution. 15 | 16 | 3. Neither the name of the copyright holder nor the names of its 17 | contributors may be used to endorse or promote products derived from 18 | this software without specific prior written permission. 19 | 20 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 21 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 22 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 23 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 24 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 25 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 26 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 27 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 28 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 29 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 30 | -------------------------------------------------------------------------------- /rmcl_ros/include/rmcl_ros/micpl/MICPO1DnSensorCPU.hpp: -------------------------------------------------------------------------------- 1 | #ifndef RMCL_MICPO1DN_SENSOR_HPP 2 | #define RMCL_MICPO1DN_SENSOR_HPP 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | #include 9 | 10 | #include 11 | 12 | #include 13 | 14 | #include 15 | #include 16 | #include "tf2_ros/transform_broadcaster.h" 17 | #include 18 | #include 19 | #include 20 | 21 | #include 22 | 23 | 24 | #include 25 | #include 26 | 27 | #include 28 | 29 | // Correspondences 30 | #include 31 | #include 32 | 33 | #include 34 | #include 35 | namespace rmcl 36 | { 37 | 38 | class MICPO1DnSensorCPU 39 | : public MICPSensorCPU 40 | { 41 | public: 42 | using Base = MICPSensorCPU; 43 | 44 | MICPO1DnSensorCPU( 45 | rclcpp::Node::SharedPtr nh); 46 | 47 | // Data Loaders 48 | // TODO: Can we move this to seperate data loader instances? 49 | void connectToTopic(const std::string& topic_name); 50 | void getDataFromParameters(); 51 | 52 | void updateMsg(const rmcl_msgs::msg::O1DnStamped::SharedPtr msg); 53 | 54 | protected: 55 | 56 | void unpackMessage(const rmcl_msgs::msg::O1DnStamped::SharedPtr msg); 57 | 58 | private: 59 | rmagine::O1DnModel sensor_model_; 60 | 61 | message_filters::Subscriber data_sub_; 62 | std::unique_ptr > tf_filter_; 63 | }; 64 | 65 | } // namespace rmcl 66 | 67 | #endif // RMCL_MICPO1DN_SENSOR_HPP -------------------------------------------------------------------------------- /rmcl_ros/include/rmcl_ros/micpl/MICPO1DnSensorCUDA.hpp: -------------------------------------------------------------------------------- 1 | #ifndef RMCL_MICPL_O1DN_SENSOR_CUDA_HPP 2 | #define RMCL_MICPL_O1DN_SENSOR_CUDA_HPP 3 | 4 | #include 5 | #include 6 | 7 | #include 8 | #include 9 | 10 | #include 11 | #include 12 | #include "tf2_ros/transform_broadcaster.h" 13 | #include 14 | #include 15 | #include 16 | 17 | #include 18 | 19 | 20 | #include 21 | #include 22 | 23 | #include 24 | #include 25 | 26 | namespace rmcl 27 | { 28 | 29 | class MICPO1DnSensorCUDA 30 | : public MICPSensorCUDA 31 | { 32 | public: 33 | using Base = MICPSensorCUDA; 34 | 35 | MICPO1DnSensorCUDA( 36 | rclcpp::Node::SharedPtr nh); 37 | 38 | // Data Loaders 39 | // TODO: Can we move this to seperate data loader instances? 40 | void connectToTopic(const std::string& topic_name); 41 | void getDataFromParameters(); 42 | 43 | void updateMsg(const rmcl_msgs::msg::O1DnStamped::SharedPtr msg); 44 | 45 | protected: 46 | 47 | void unpackMessage(const rmcl_msgs::msg::O1DnStamped::SharedPtr msg); 48 | 49 | private: 50 | 51 | rmagine::O1DnModel sensor_model_; 52 | 53 | message_filters::Subscriber data_sub_; 54 | std::unique_ptr > tf_filter_; 55 | }; 56 | 57 | } // namespace rmcl 58 | 59 | #endif // RMCL_MICPL_O1DN_SENSOR_CUDA_HPP -------------------------------------------------------------------------------- /rmcl_ros/include/rmcl_ros/micpl/MICPOnDnSensorCPU.hpp: -------------------------------------------------------------------------------- 1 | #ifndef RMCL_MICPL_ONDN_SENSOR_HPP 2 | #define RMCL_MICPL_ONDN_SENSOR_HPP 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | #include 9 | 10 | #include 11 | 12 | #include 13 | 14 | #include 15 | #include 16 | #include "tf2_ros/transform_broadcaster.h" 17 | #include 18 | #include 19 | #include 20 | 21 | #include 22 | 23 | 24 | #include 25 | #include 26 | 27 | #include 28 | 29 | // Correspondences 30 | #include 31 | #include 32 | 33 | 34 | #include 35 | #include 36 | 37 | 38 | 39 | namespace rmcl 40 | { 41 | 42 | class MICPOnDnSensorCPU 43 | : public MICPSensorCPU 44 | { 45 | public: 46 | 47 | using Base = MICPSensorCPU; 48 | 49 | MICPOnDnSensorCPU( 50 | rclcpp::Node::SharedPtr nh); 51 | 52 | // Data Loaders 53 | // TODO: Can we move this to seperate data loader instances? 54 | void connectToTopic(const std::string& topic_name); 55 | void getDataFromParameters(); 56 | 57 | void updateMsg(const rmcl_msgs::msg::OnDnStamped::SharedPtr msg); 58 | 59 | protected: 60 | 61 | void unpackMessage(const rmcl_msgs::msg::OnDnStamped::SharedPtr msg); 62 | 63 | private: 64 | rmagine::OnDnModel sensor_model_; 65 | 66 | message_filters::Subscriber data_sub_; 67 | std::unique_ptr > tf_filter_; 68 | }; 69 | 70 | } // namespace rmcl 71 | 72 | #endif // RMCL_MICP_ONDN_SENSOR_HPP -------------------------------------------------------------------------------- /rmcl_ros/include/rmcl_ros/micpl/MICPOnDnSensorCUDA.hpp: -------------------------------------------------------------------------------- 1 | #ifndef RMCL_MICPL_ONDN_SENSOR_CUDA_HPP 2 | #define RMCL_MICPL_ONDN_SENSOR_CUDA_HPP 3 | 4 | #include 5 | #include 6 | 7 | #include 8 | #include 9 | 10 | #include 11 | #include 12 | #include "tf2_ros/transform_broadcaster.h" 13 | #include 14 | #include 15 | #include 16 | 17 | #include 18 | 19 | 20 | #include 21 | #include 22 | 23 | #include 24 | #include 25 | 26 | 27 | namespace rmcl 28 | { 29 | 30 | class MICPOnDnSensorCUDA 31 | : public MICPSensorCUDA 32 | { 33 | public: 34 | using Base = MICPSensorCUDA; 35 | 36 | MICPOnDnSensorCUDA( 37 | rclcpp::Node::SharedPtr nh); 38 | 39 | // Data Loaders 40 | // TODO: Can we move this to seperate data loader instances? 41 | void connectToTopic(const std::string& topic_name); 42 | void getDataFromParameters(); 43 | 44 | void updateMsg(const rmcl_msgs::msg::OnDnStamped::SharedPtr msg); 45 | 46 | protected: 47 | 48 | void unpackMessage(const rmcl_msgs::msg::OnDnStamped::SharedPtr msg); 49 | 50 | private: 51 | 52 | rmagine::OnDnModel sensor_model_; 53 | 54 | message_filters::Subscriber data_sub_; 55 | std::unique_ptr > tf_filter_; 56 | }; 57 | 58 | } // namespace rmcl 59 | 60 | #endif // RMCL_MICPL_ONDN_SENSOR_CUDA_HPP -------------------------------------------------------------------------------- /rmcl_ros/include/rmcl_ros/micpl/MICPPinholeSensorCPU.hpp: -------------------------------------------------------------------------------- 1 | #ifndef RMCL_MICP_PINHOLE_SENSOR_HPP 2 | #define RMCL_MICP_PINHOLE_SENSOR_HPP 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | #include 9 | 10 | #include 11 | 12 | #include 13 | 14 | #include 15 | #include 16 | #include "tf2_ros/transform_broadcaster.h" 17 | #include 18 | #include 19 | #include 20 | 21 | #include 22 | 23 | 24 | #include 25 | #include 26 | 27 | #include 28 | 29 | // Correspondences 30 | #include 31 | #include 32 | 33 | 34 | #include 35 | #include 36 | 37 | namespace rmcl 38 | { 39 | 40 | class MICPPinholeSensorCPU 41 | : public MICPSensorCPU 42 | { 43 | public: 44 | using Base = MICPSensorCPU; 45 | 46 | MICPPinholeSensorCPU( 47 | rclcpp::Node::SharedPtr nh); 48 | 49 | // Data Loaders 50 | // TODO: Can we move this to seperate data loader instances? 51 | void connectToTopic(const std::string& topic_name); 52 | void getDataFromParameters(); 53 | 54 | void updateMsg(const rmcl_msgs::msg::DepthStamped::SharedPtr msg); 55 | 56 | protected: 57 | 58 | void unpackMessage(const rmcl_msgs::msg::DepthStamped::SharedPtr msg); 59 | 60 | private: 61 | rmagine::PinholeModel sensor_model_; 62 | 63 | message_filters::Subscriber data_sub_; 64 | std::unique_ptr > tf_filter_; 65 | }; 66 | 67 | } // namespace rmcl 68 | 69 | #endif // RMCL_MICP_PINHOLE_SENSOR_HPP -------------------------------------------------------------------------------- /rmcl_ros/include/rmcl_ros/micpl/MICPPinholeSensorCUDA.hpp: -------------------------------------------------------------------------------- 1 | #ifndef RMCL_MICPL_PINHOLE_SENSOR_CUDA_HPP 2 | #define RMCL_MICPL_PINHOLE_SENSOR_CUDA_HPP 3 | 4 | #include 5 | #include 6 | 7 | #include 8 | #include 9 | 10 | #include 11 | #include 12 | #include "tf2_ros/transform_broadcaster.h" 13 | #include 14 | #include 15 | #include 16 | 17 | #include 18 | 19 | 20 | #include 21 | #include 22 | 23 | 24 | #include 25 | 26 | #include 27 | #include 28 | 29 | namespace rmcl 30 | { 31 | 32 | class MICPPinholeSensorCUDA 33 | : public MICPSensorCUDA 34 | { 35 | public: 36 | using Base = MICPSensorCUDA; 37 | 38 | MICPPinholeSensorCUDA( 39 | rclcpp::Node::SharedPtr nh); 40 | 41 | // Data Loaders 42 | // TODO: Can we move this to seperate data loader instances? 43 | void connectToTopic(const std::string& topic_name); 44 | void getDataFromParameters(); 45 | 46 | void updateMsg(const rmcl_msgs::msg::DepthStamped::SharedPtr msg); 47 | 48 | protected: 49 | 50 | void unpackMessage(const rmcl_msgs::msg::DepthStamped::SharedPtr msg); 51 | 52 | private: 53 | 54 | rmagine::PinholeModel sensor_model_; 55 | 56 | message_filters::Subscriber data_sub_; 57 | std::unique_ptr > tf_filter_; 58 | }; 59 | 60 | } // namespace rmcl 61 | 62 | #endif // RMCL_MICPL_PINHOLE_SENSOR_CUDA_HPP -------------------------------------------------------------------------------- /rmcl_ros/include/rmcl_ros/micpl/MICPSensor.hpp: -------------------------------------------------------------------------------- 1 | #ifndef RMCL_CORRECTION_MICP_SENSOR_HPP 2 | #define RMCL_CORRECTION_MICP_SENSOR_HPP 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | 12 | #include 13 | #include 14 | #include 15 | #include 16 | 17 | #include 18 | #include 19 | 20 | #include 21 | 22 | #include 23 | 24 | 25 | namespace rmcl 26 | { 27 | 28 | class MICPSensorBase 29 | { 30 | public: 31 | MICPSensorBase(rclcpp::Node::SharedPtr nh); 32 | virtual ~MICPSensorBase(); 33 | 34 | // Pipeline: load data -> search for correspondences -> update statistics 35 | // (later from base, for each sensor: merge statistics and compute pose corrections) 36 | 37 | // void setTbm(const rmagine::Transform& Tbm); 38 | void setTom(const rmagine::Transform& Tom); 39 | 40 | /** 41 | * Fetch TF chain. except for odom->map (this is estimated or has to be provided from extern) 42 | */ 43 | bool fetchTF(const rclcpp::Time stamp); 44 | 45 | virtual void drawCorrespondences() = 0; 46 | 47 | inline std::mutex& mutex() 48 | { 49 | return data_correction_mutex_; 50 | } 51 | 52 | virtual void findCorrespondences() = 0; 53 | 54 | virtual bool correspondencesOutdated() const = 0; 55 | 56 | virtual rmagine::CrossStatistics computeCrossStatistics( 57 | const rmagine::Transform& Tpre, 58 | double convergence_progress = 0.0 59 | ) const = 0; 60 | 61 | virtual void connectToTopic(const std::string& topic_name) = 0; 62 | 63 | virtual void getDataFromParameters() = 0; 64 | 65 | inline void asyncSpin() 66 | { 67 | exec_thread_ = std::thread([this]() { 68 | // main function 69 | exec_ = std::make_shared(); 70 | exec_->add_callback_group(cb_group_, nh_->get_node_base_interface()); 71 | exec_->spin(); 72 | }); 73 | } 74 | 75 | // name of the sensor 76 | std::string name; 77 | bool static_dataset = false; 78 | 79 | /** 80 | * This drops performance! 81 | */ 82 | bool enable_visualizations = false; 83 | 84 | // transform chain from sensor -> base -> odom -> map 85 | 86 | // keep this up to date 87 | rmagine::Transform Tsb; 88 | rclcpp::Time Tsb_stamp; 89 | rmagine::Transform Tbo; 90 | rclcpp::Time Tbo_stamp; 91 | 92 | rmagine::Transform Tom; // this is set from externally 93 | rclcpp::Time Tom_stamp; 94 | 95 | std::string map_frame = "map"; 96 | std::string odom_frame = "odom"; 97 | std::string base_frame = "base_footprint"; 98 | std::string sensor_frame = "velodyne"; 99 | 100 | bool first_message_received = false; 101 | 102 | // weight multiplier 103 | double merge_weight_multiplier = 1.0; 104 | 105 | rclcpp::Time dataset_stamp_; 106 | std::mutex data_correction_mutex_; 107 | 108 | // stats 109 | size_t total_dataset_measurements; 110 | size_t valid_dataset_measurements; 111 | 112 | // This is called as soon as data was received and pre-processed 113 | std::function on_data_received; 114 | 115 | 116 | protected: 117 | 118 | // ROS 119 | rclcpp::Node::SharedPtr nh_; 120 | 121 | // TF 122 | std::shared_ptr tf_listener_{nullptr}; 123 | std::shared_ptr tf_buffer_; 124 | 125 | // Visualization 126 | rclcpp::Publisher::SharedPtr 127 | correspondence_viz_pub_; 128 | 129 | rclcpp::CallbackGroup::SharedPtr cb_group_; 130 | std::shared_ptr exec_; 131 | std::thread exec_thread_; 132 | }; 133 | 134 | using MICPSensorPtr = std::shared_ptr; 135 | 136 | template 137 | class MICPSensor_ 138 | : public MICPSensorBase 139 | { 140 | public: 141 | MICPSensor_(rclcpp::Node::SharedPtr nh) 142 | :MICPSensorBase(nh) 143 | { 144 | 145 | } 146 | 147 | virtual void findCorrespondences() 148 | { 149 | const rmagine::Transform Tbm = Tom * Tbo; 150 | correspondences_->find(Tbm); 151 | correspondences_->outdated = false; 152 | } 153 | 154 | virtual bool correspondencesOutdated() const 155 | { 156 | return correspondences_->outdated; 157 | } 158 | 159 | virtual rmagine::CrossStatistics computeCrossStatistics( 160 | const rmagine::Transform& T_bnew_bold, // Tpre_b 161 | double convergence_progress = 0.0 162 | ) const 163 | { 164 | // this is what we want to optimize: 165 | // find a transformation from a new base frame to the old base frame that optimizes the alignment 166 | 167 | // bnew --- T_bnew_bold --> bold : base frame (shared frame of all robot sensors) 168 | // ^ ^ 169 | // | | 170 | // Tsb Tsb 171 | // | | 172 | // snew --- T_snew_sold --> sold : sensor frames 173 | // 174 | // Fig 1: How to transform delta transforms 175 | 176 | namespace rm = rmagine; 177 | 178 | // reduce correspondences_ to C 179 | const rm::Transform T_snew_sold = ~Tsb * T_bnew_bold * Tsb; 180 | const rm::CrossStatistics stats_s = correspondences_->computeCrossStatistics( 181 | T_snew_sold, convergence_progress); 182 | // transform CrossStatistics of every sensor to base frame 183 | const rm::CrossStatistics stats_b = Tsb * stats_s; 184 | return stats_b; 185 | } 186 | 187 | std::shared_ptr > correspondences_; 188 | 189 | // this is a temporary storage for incoming topic data 190 | rmagine::PointCloud_ dataset_cpu_; 191 | }; 192 | 193 | } // namespace rmcl 194 | 195 | 196 | #endif // RMCL_CORRECTION_MICP_SENSOR_HPP -------------------------------------------------------------------------------- /rmcl_ros/include/rmcl_ros/micpl/MICPSensorCPU.hpp: -------------------------------------------------------------------------------- 1 | #ifndef RMCL_CORRECTION_MICP_SENSOR_CPU_HPP 2 | #define RMCL_CORRECTION_MICP_SENSOR_CPU_HPP 3 | 4 | #include 5 | #include 6 | 7 | namespace rmcl 8 | { 9 | 10 | class MICPSensorCPU 11 | : public MICPSensor_ 12 | { 13 | public: 14 | using Base = MICPSensor_; 15 | 16 | MICPSensorCPU(rclcpp::Node::SharedPtr nh); 17 | 18 | void drawCorrespondences(); 19 | }; 20 | 21 | } // namespace rmcl 22 | 23 | #endif // RMCL_CORRECTION_MICP_SENSOR_CPU_HPP -------------------------------------------------------------------------------- /rmcl_ros/include/rmcl_ros/micpl/MICPSensorCUDA.hpp: -------------------------------------------------------------------------------- 1 | #ifndef RMCL_MICPL_SENSOR_CUDA_HPP 2 | #define RMCL_MICPL_SENSOR_CUDA_HPP 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | namespace rmcl 9 | { 10 | 11 | class MICPSensorCUDA 12 | : public MICPSensor_ 13 | { 14 | public: 15 | using Base = MICPSensor_; 16 | 17 | MICPSensorCUDA(rclcpp::Node::SharedPtr nh); 18 | 19 | void drawCorrespondences(); 20 | }; 21 | 22 | } // namespace rmcl 23 | 24 | #endif // RMCL_MICPL_SENSOR_CUDA_HPP -------------------------------------------------------------------------------- /rmcl_ros/include/rmcl_ros/micpl/MICPSphericalSensorCPU.hpp: -------------------------------------------------------------------------------- 1 | #ifndef RMCL_MICP_SPHERICAL_SENSOR_HPP 2 | #define RMCL_MICP_SPHERICAL_SENSOR_HPP 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | #include 9 | 10 | #include 11 | 12 | #include 13 | 14 | #include 15 | #include 16 | #include "tf2_ros/transform_broadcaster.h" 17 | #include 18 | #include 19 | #include 20 | 21 | #include 22 | 23 | 24 | #include 25 | #include 26 | 27 | #include 28 | 29 | // Correspondences 30 | #include 31 | #include 32 | 33 | 34 | #include 35 | #include 36 | 37 | namespace rmcl 38 | { 39 | 40 | class MICPSphericalSensorCPU 41 | : public MICPSensorCPU 42 | { 43 | public: 44 | using Base = MICPSensorCPU; 45 | 46 | MICPSphericalSensorCPU( 47 | rclcpp::Node::SharedPtr nh); 48 | 49 | // Data Loaders 50 | // TODO: Can we move this to seperate data loader instances? 51 | void connectToTopic(const std::string& topic_name); 52 | void getDataFromParameters(); 53 | 54 | void updateMsg(const rmcl_msgs::msg::ScanStamped::SharedPtr msg); 55 | 56 | protected: 57 | 58 | void unpackMessage(const rmcl_msgs::msg::ScanStamped::SharedPtr msg); 59 | 60 | private: 61 | rmagine::SphericalModel sensor_model_; 62 | 63 | message_filters::Subscriber data_sub_; 64 | std::unique_ptr > tf_filter_; 65 | }; 66 | 67 | } // namespace rmcl 68 | 69 | #endif // RMCL_MICP_SPHERICAL_SENSOR_HPP -------------------------------------------------------------------------------- /rmcl_ros/include/rmcl_ros/micpl/MICPSphericalSensorCUDA.hpp: -------------------------------------------------------------------------------- 1 | #ifndef RMCL_MICPL_SPHERICAL_SENSOR_CUDA_HPP 2 | #define RMCL_MICPL_SPHERICAL_SENSOR_CUDA_HPP 3 | 4 | #include 5 | #include 6 | 7 | #include 8 | #include 9 | 10 | #include 11 | #include 12 | #include "tf2_ros/transform_broadcaster.h" 13 | #include 14 | #include 15 | #include 16 | 17 | #include 18 | 19 | 20 | #include 21 | #include 22 | 23 | 24 | #include 25 | 26 | #include 27 | #include 28 | 29 | namespace rmcl 30 | { 31 | 32 | class MICPSphericalSensorCUDA 33 | : public MICPSensorCUDA 34 | { 35 | public: 36 | using Base = MICPSensorCUDA; 37 | 38 | MICPSphericalSensorCUDA( 39 | rclcpp::Node::SharedPtr nh); 40 | 41 | // Data Loaders 42 | // TODO: Can we move this to seperate data loader instances? 43 | void connectToTopic(const std::string& topic_name); 44 | void getDataFromParameters(); 45 | 46 | void updateMsg(const rmcl_msgs::msg::ScanStamped::SharedPtr msg); 47 | 48 | protected: 49 | 50 | void unpackMessage(const rmcl_msgs::msg::ScanStamped::SharedPtr msg); 51 | 52 | private: 53 | 54 | rmagine::SphericalModel sensor_model_; 55 | 56 | message_filters::Subscriber data_sub_; 57 | std::unique_ptr > tf_filter_; 58 | }; 59 | 60 | } // namespace rmcl 61 | 62 | #endif // RMCL_MICPL_SPHERICAL_SENSOR_CUDA_HPP -------------------------------------------------------------------------------- /rmcl_ros/include/rmcl_ros/nodes/conversion/pc2_to_o1dn.hpp: -------------------------------------------------------------------------------- 1 | #ifndef RMCL_CONVERSION_PC2_TO_O1DN_HPP 2 | #define RMCL_CONVERSION_PC2_TO_O1DN_HPP 3 | 4 | #include 5 | 6 | #include 7 | #include 8 | #include 9 | #include 10 | 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | 17 | 18 | namespace rmcl 19 | { 20 | 21 | class Pc2ToO1DnNode : public rclcpp::Node 22 | { 23 | public: 24 | explicit Pc2ToO1DnNode( 25 | const rclcpp::NodeOptions& options = rclcpp::NodeOptions()); 26 | 27 | private: 28 | 29 | void declareParameters(); 30 | 31 | void fetchParameters(); 32 | 33 | rcl_interfaces::msg::SetParametersResult parametersCallback( 34 | const std::vector ¶meters); 35 | 36 | bool convert(const sensor_msgs::msg::PointCloud2::ConstSharedPtr& pcd, 37 | rmcl_msgs::msg::O1DnStamped& scan) const; 38 | 39 | void cloudCB(const sensor_msgs::msg::PointCloud2::ConstSharedPtr& msg); 40 | 41 | // parameters 42 | std::string sensor_frame_ = ""; 43 | bool debug_cloud_ = false; 44 | 45 | FilterOptions2D filter_options_; 46 | 47 | rclcpp::Subscription::SharedPtr sub_pcl_; 48 | 49 | rclcpp::Publisher::SharedPtr pub_debug_cloud_; 50 | rclcpp::Publisher::SharedPtr pub_scan_; 51 | rmcl_msgs::msg::O1DnStamped scan_; 52 | 53 | std::shared_ptr tf_listener_; 54 | std::unique_ptr tf_buffer_; 55 | 56 | OnSetParametersCallbackHandle::SharedPtr callback_handle_; 57 | }; 58 | 59 | } // namespace rmcl 60 | 61 | #endif // RMCL_CONVERSION_PC2_TO_O1DN_HPP -------------------------------------------------------------------------------- /rmcl_ros/include/rmcl_ros/nodes/conversion/pc2_to_scan.hpp: -------------------------------------------------------------------------------- 1 | #ifndef RMCL_CONVERSION_PC2_TO_SCAN_HPP 2 | #define RMCL_CONVERSION_PC2_TO_SCAN_HPP 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | #include 9 | #include 10 | 11 | #include 12 | #include 13 | 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | 20 | namespace rm = rmagine; 21 | 22 | namespace rmcl 23 | { 24 | 25 | class Pc2ToScanNode : public rclcpp::Node 26 | { 27 | public: 28 | explicit Pc2ToScanNode( 29 | const rclcpp::NodeOptions& options = rclcpp::NodeOptions()); 30 | 31 | private: 32 | 33 | void fetchParameters(); 34 | 35 | void initScanArray(); 36 | 37 | bool convert( 38 | const sensor_msgs::msg::PointCloud2::ConstSharedPtr& pcd, 39 | rmcl_msgs::msg::ScanStamped& scan) const; 40 | 41 | void cloudCB(const sensor_msgs::msg::PointCloud2::ConstSharedPtr& msg); 42 | 43 | std::string sensor_frame_ = ""; 44 | bool debug_cloud_ = false; 45 | 46 | rclcpp::Subscription::SharedPtr sub_pcd_; 47 | 48 | rmcl_msgs::msg::ScanStamped scan_; 49 | 50 | rclcpp::Publisher::SharedPtr pub_debug_cloud_; 51 | rclcpp::Publisher::SharedPtr pub_scan_; 52 | 53 | std::shared_ptr tf_listener_; 54 | std::unique_ptr tf_buffer_; 55 | }; 56 | 57 | } // namespace rmcl 58 | 59 | #endif // RMCL_CONVERSION_PC2_TO_SCAN_HPP -------------------------------------------------------------------------------- /rmcl_ros/include/rmcl_ros/nodes/conversion/scan_to_scan.hpp: -------------------------------------------------------------------------------- 1 | #ifndef RMCL_CONVERSION_SCAN_TO_SCAN_HPP 2 | #define RMCL_CONVERSION_SCAN_TO_SCAN_HPP 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | #include 9 | 10 | #include 11 | #include 12 | 13 | #include 14 | #include 15 | 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include 21 | 22 | 23 | namespace rm = rmagine; 24 | 25 | namespace rmcl 26 | { 27 | 28 | class ScanToScanNode : public rclcpp::Node 29 | { 30 | public: 31 | explicit ScanToScanNode( 32 | const rclcpp::NodeOptions& options = rclcpp::NodeOptions()); 33 | 34 | private: 35 | 36 | void declareParameters(); 37 | 38 | void fetchParameters(); 39 | 40 | rcl_interfaces::msg::SetParametersResult parametersCallback( 41 | const std::vector ¶meters); 42 | 43 | void initScanArray(); 44 | 45 | bool convert( 46 | const sensor_msgs::msg::LaserScan::ConstSharedPtr& scan_in, 47 | rmcl_msgs::msg::ScanStamped& scan_out) const; 48 | 49 | void scanCB(const sensor_msgs::msg::LaserScan::ConstSharedPtr& msg); 50 | 51 | size_t skip_begin_; 52 | size_t skip_end_; 53 | size_t increment_; 54 | bool debug_cloud_ = false; 55 | 56 | rclcpp::Subscription::SharedPtr sub_scan_; 57 | 58 | rmcl_msgs::msg::ScanStamped scan_; 59 | 60 | rclcpp::Publisher::SharedPtr pub_debug_cloud_; 61 | rclcpp::Publisher::SharedPtr pub_scan_; 62 | 63 | OnSetParametersCallbackHandle::SharedPtr callback_handle_; 64 | }; 65 | 66 | } // namespace rmcl 67 | 68 | #endif // RMCL_CONVERSION_PC2_TO_SCAN_HPP -------------------------------------------------------------------------------- /rmcl_ros/include/rmcl_ros/nodes/filter/map_segmentation.hpp: -------------------------------------------------------------------------------- 1 | #ifndef RMCL_FILTER_MAP_SEGMENTATION_HPP 2 | #define RMCL_FILTER_MAP_SEGMENTATION_HPP 3 | 4 | #include 5 | 6 | #include 7 | #include 8 | #include 9 | #include 10 | 11 | // RCML msgs 12 | #include 13 | 14 | #include 15 | 16 | namespace rmcl 17 | { 18 | 19 | class MapSegmentationNode 20 | : public rclcpp::Node 21 | { 22 | public: 23 | explicit MapSegmentationNode( 24 | std::string node_name, 25 | const rclcpp::NodeOptions& options = rclcpp::NodeOptions()); 26 | 27 | rcl_interfaces::msg::SetParametersResult reconfigureCallback( 28 | const std::vector& params); 29 | 30 | protected: 31 | 32 | float min_dist_outlier_scan_; 33 | float min_dist_outlier_map_; 34 | std::string map_frame_; 35 | std::string map_file_; 36 | 37 | std::shared_ptr tf_listener_; 38 | std::unique_ptr tf_buffer_; 39 | 40 | rclcpp::Publisher::SharedPtr pub_outlier_scan_; 41 | rclcpp::Publisher::SharedPtr pub_outlier_map_; 42 | rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr dyn_params_handler_; 43 | }; 44 | 45 | } // namespace rmcl 46 | 47 | #endif // RMCL_FILTER_MAP_SEGMENTATION_HPP -------------------------------------------------------------------------------- /rmcl_ros/include/rmcl_ros/nodes/filter/o1dn_map_segmentation_embree.hpp: -------------------------------------------------------------------------------- 1 | #ifndef RMCL_FILTER_O1DN_MAP_SEGMENTATION_EMBREE_HPP 2 | #define RMCL_FILTER_O1DN_MAP_SEGMENTATION_EMBREE_HPP 3 | 4 | #include 5 | 6 | #include 7 | 8 | #include 9 | #include 10 | 11 | // Rmagine deps 12 | #include 13 | 14 | // RCML msgs 15 | #include 16 | 17 | #include 18 | 19 | namespace rmcl 20 | { 21 | 22 | class O1DnMapSegmentationEmbreeNode 23 | : public MapSegmentationNode 24 | { 25 | public: 26 | explicit O1DnMapSegmentationEmbreeNode( 27 | const rclcpp::NodeOptions& options = rclcpp::NodeOptions()); 28 | 29 | void scanCB(const rmcl_msgs::msg::O1DnStamped::ConstSharedPtr& msg) const; 30 | private: 31 | rmagine::O1DnSimulatorEmbreePtr scan_sim_; 32 | 33 | rclcpp::Subscription::SharedPtr sub_scan_; 34 | }; 35 | 36 | } // namespace rmcl 37 | 38 | #endif // RMCL_FILTER_O1DN_MAP_SEGMENTATION_EMBREE_HPP -------------------------------------------------------------------------------- /rmcl_ros/include/rmcl_ros/nodes/filter/scan_map_segmentation_embree.hpp: -------------------------------------------------------------------------------- 1 | #ifndef RMCL_FILTER_SCAN_MAP_SEGMENTATION_EMBREE_HPP 2 | #define RMCL_FILTER_SCAN_MAP_SEGMENTATION_EMBREE_HPP 3 | 4 | #include 5 | 6 | #include 7 | 8 | #include 9 | 10 | // Rmagine deps 11 | #include 12 | 13 | // RCML msgs 14 | #include 15 | 16 | #include 17 | 18 | namespace rmcl 19 | { 20 | 21 | class ScanMapSegmentationEmbreeNode 22 | : public MapSegmentationNode 23 | { 24 | public: 25 | explicit ScanMapSegmentationEmbreeNode( 26 | const rclcpp::NodeOptions& options = rclcpp::NodeOptions()); 27 | 28 | void scanCB(const rmcl_msgs::msg::O1DnStamped::ConstSharedPtr& msg) const; 29 | 30 | private: 31 | 32 | rmagine::SphereSimulatorEmbreePtr scan_sim_; 33 | rclcpp::Subscription::SharedPtr sub_scan_; 34 | }; 35 | 36 | } // namespace rmcl 37 | 38 | #endif // RMCL_FILTER_SCAN_MAP_SEGMENTATION_EMBREE_HPP -------------------------------------------------------------------------------- /rmcl_ros/include/rmcl_ros/nodes/micp_localization.hpp: -------------------------------------------------------------------------------- 1 | #ifndef RMCL_ROS_NODES_MICP_LOCALIZATION_HPP 2 | #define RMCL_ROS_NODES_MICP_LOCALIZATION_HPP 3 | 4 | #include 5 | 6 | #include 7 | #include 8 | 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | 15 | #include 16 | 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | 24 | 25 | #include 26 | 27 | #ifdef RMCL_EMBREE 28 | #include 29 | #endif // RMCL_EMBREE 30 | 31 | #ifdef RMCL_OPTIX 32 | #include 33 | #endif // RMCL_OPTIX 34 | 35 | #include 36 | #include 37 | 38 | #include 39 | #include 40 | 41 | namespace rmcl 42 | { 43 | 44 | class MICPLocalizationNode : public rclcpp::Node 45 | { 46 | public: 47 | explicit MICPLocalizationNode( 48 | const rclcpp::NodeOptions& options = rclcpp::NodeOptions()); 49 | 50 | /** 51 | * Loads a single sensor config 52 | */ 53 | MICPSensorPtr loadSensor( 54 | ParamTree::SharedPtr sensor_params); 55 | 56 | void printSetup(); 57 | 58 | // I make this public if someone wants to write an application 59 | // for multiple robots 60 | rmagine::Transform Tom_; 61 | rclcpp::Time Tom_stamp_; 62 | 63 | private: 64 | 65 | void poseCB( 66 | const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg); 67 | 68 | void sensorDataReceived(const MICPSensorBase* sensor); 69 | 70 | void correct(); 71 | 72 | void correctOnce(); 73 | 74 | void correctionLoop(); 75 | 76 | void broadcastTransform(); 77 | 78 | void publishPose(); 79 | 80 | bool fetchTF(const rclcpp::Time stamp); 81 | 82 | void tfBroadcastLoop(); 83 | 84 | std::string map_frame_; 85 | std::string base_frame_; 86 | std::string odom_frame_; 87 | 88 | std::string map_filename_; 89 | std::unordered_map sensors_; 90 | std::vector sensors_vec_; 91 | 92 | // TODO: use this to become independent from the implementations 93 | // at this place in code 94 | // rmagine::MapMap map_server_; 95 | 96 | // TODO: can we avoid ifdefs here? 97 | #ifdef RMCL_EMBREE 98 | rmagine::EmbreeMapPtr map_embree_; 99 | #endif // RMCL_EMBREE 100 | 101 | #ifdef RMCL_OPTIX 102 | rmagine::OptixMapPtr map_optix_; 103 | #endif // RMCL_OPTIX 104 | 105 | std::thread correction_thread_; 106 | bool stop_correction_thread_ = false; 107 | 108 | std::thread tf_broadcaster_thread_; 109 | bool stop_tf_broadcaster_thread_ = false; 110 | 111 | // tf2 112 | std::shared_ptr tf_listener_{nullptr}; 113 | std::shared_ptr tf_buffer_; 114 | std::shared_ptr tf_broadcaster_; 115 | 116 | // pose wc stamped subscriber (eg, RViz) 117 | rclcpp::Subscription::SharedPtr 118 | pose_sub_; 119 | 120 | rclcpp::Publisher::SharedPtr 121 | stats_publisher_; 122 | 123 | rclcpp::Publisher::SharedPtr 124 | Tbm_publisher_; 125 | 126 | std::mutex mutex_; 127 | 128 | // High-Level stats about sensors 129 | rclcpp::Time data_stamp_latest_; 130 | rmagine::Transform Tbo_latest_; 131 | rclcpp::Time Tbo_stamp_latest_; 132 | rmcl_msgs::msg::MICPSensorStats correction_stats_latest_; 133 | double convergence_progress_ = 0.0; 134 | bool first_message_received_ = false; 135 | 136 | // IF there are at least 1 dynamic sensors (reading data from topic): 137 | // synchronize all static sensors with the latest dynamic message 138 | // to get small time errors. 139 | // ELSE 140 | // It is OK to synchronize with the current time. 141 | size_t num_dynamic_sensors_ = 0; 142 | 143 | bool disable_correction_ = false; 144 | size_t optimization_iterations_ = 10; 145 | int tf_time_source_ = 0; 146 | double pose_noise_ = 0.01; 147 | bool broadcast_tf_ = true; 148 | bool publish_pose_ = false; 149 | bool adaptive_max_dist_ = true; 150 | double correction_rate_max_ = 100.0; 151 | rmagine::Transform pose_guess_offset_; 152 | rmagine::Transform initial_pose_guess_; 153 | 154 | double tf_rate_ = 100.0; 155 | }; 156 | 157 | 158 | } // namespace rmcl 159 | 160 | #endif // RMCL_ROS_NODES_MICP_LOCALIZATION_HPP 161 | -------------------------------------------------------------------------------- /rmcl_ros/include/rmcl_ros/util/conversions.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2022, University Osnabrück 3 | * All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * * Redistributions of source code must retain the above copyright 8 | * notice, this list of conditions and the following disclaimer. 9 | * * Redistributions in binary form must reproduce the above copyright 10 | * notice, this list of conditions and the following disclaimer in the 11 | * documentation and/or other materials provided with the distribution. 12 | * * Neither the name of the University Osnabrück nor the 13 | * names of its contributors may be used to endorse or promote products 14 | * derived from this software without specific prior written permission. 15 | * 16 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 17 | * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 18 | * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 19 | * DISCLAIMED. IN NO EVENT SHALL University Osnabrück BE LIABLE FOR ANY 20 | * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 21 | * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 22 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 23 | * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 24 | * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | */ 27 | 28 | /** 29 | * @file 30 | * 31 | * @brief ROS Conversions 32 | * 33 | * @date 03.10.2022 34 | * @author Alexander Mock 35 | * 36 | * @copyright Copyright (c) 2022, University Osnabrück. All rights reserved. 37 | * This project is released under the 3-Clause BSD License. 38 | * 39 | */ 40 | 41 | #ifndef RMCL_CONVERSIONS_HPP 42 | #define RMCL_CONVERSIONS_HPP 43 | 44 | #include 45 | #include 46 | #include 47 | #include 48 | #include 49 | #include 50 | 51 | 52 | #include 53 | #include 54 | #include 55 | #include 56 | #include 57 | #include 58 | #include 59 | #include 60 | #include 61 | 62 | #include 63 | 64 | #include 65 | 66 | #include 67 | 68 | #include 69 | 70 | namespace rmcl { 71 | 72 | void convert( 73 | const sensor_msgs::msg::LaserScan& from, 74 | rmagine::SphericalModel& to); 75 | 76 | void convert( 77 | const rmcl_msgs::msg::ScanInfo& from, 78 | rmagine::SphericalModel& to); 79 | 80 | void convert( 81 | const sensor_msgs::msg::CameraInfo& from, 82 | rmagine::PinholeModel& to); 83 | 84 | void convert( 85 | const rmcl_msgs::msg::DepthInfo& from, 86 | rmagine::PinholeModel& to); 87 | 88 | void convert( 89 | const sensor_msgs::msg::CameraInfo& from, 90 | rmcl_msgs::msg::DepthInfo& to); 91 | 92 | void convert( 93 | const rmcl_msgs::msg::O1DnInfo& from, 94 | rmagine::O1DnModel& to); 95 | 96 | void convert( 97 | const rmcl_msgs::msg::OnDnInfo& from, 98 | rmagine::OnDnModel& to); 99 | 100 | void convert( 101 | const geometry_msgs::msg::Transform& Tros, 102 | rmagine::Transform& Trm); 103 | 104 | void convert( 105 | const rmagine::Transform& Trm, 106 | geometry_msgs::msg::Transform& Tros); 107 | 108 | void convert( 109 | const geometry_msgs::msg::Pose& Pros, 110 | rmagine::Transform& Trm); 111 | 112 | void convert( 113 | const rmagine::Transform& Trm, 114 | geometry_msgs::msg::Pose& Pros); 115 | 116 | geometry_msgs::msg::Point32 polar2cartesian( 117 | const rmcl_msgs::msg::PolarCoord& polar); 118 | 119 | void convert( 120 | const rmcl_msgs::msg::Scan& scan, 121 | std::vector& cloud); 122 | 123 | void convert( 124 | const rmcl_msgs::msg::ScanStamped& scan, 125 | sensor_msgs::msg::PointCloud& cloud); 126 | 127 | void convert( 128 | const sensor_msgs::msg::LaserScan& scan_in, 129 | rmcl_msgs::msg::ScanStamped& scan_out); 130 | 131 | void convert( 132 | const rmcl_msgs::msg::O1Dn& scan, 133 | std::vector& cloud); 134 | 135 | void convert( 136 | const rmcl_msgs::msg::O1DnStamped& scan, 137 | sensor_msgs::msg::PointCloud& cloud); 138 | 139 | // PointCloud2 140 | void convert( 141 | sensor_msgs::msg::PointCloud2& cloud, 142 | const std_msgs::msg::Header& header, 143 | const rmcl_msgs::msg::ScanInfo& info, 144 | const rmcl_msgs::msg::RangeData& data, 145 | bool dense = false); 146 | 147 | void convert( 148 | sensor_msgs::msg::PointCloud2& cloud, 149 | const std_msgs::msg::Header& header, 150 | const rmcl_msgs::msg::DepthInfo& info, 151 | const rmcl_msgs::msg::RangeData& data, 152 | bool dense = false); 153 | 154 | void convert( 155 | sensor_msgs::msg::PointCloud2& cloud, 156 | const std_msgs::msg::Header& header, 157 | const rmcl_msgs::msg::O1DnInfo& info, 158 | const rmcl_msgs::msg::RangeData& data, 159 | bool dense = false); 160 | 161 | void convert( 162 | sensor_msgs::msg::PointCloud2& cloud, 163 | const std_msgs::msg::Header& header, 164 | const rmcl_msgs::msg::OnDnInfo& info, 165 | const rmcl_msgs::msg::RangeData& data, 166 | bool dense = false); 167 | 168 | bool convert( 169 | const ParamTree::SharedPtr sensor_model_params, 170 | rmcl_msgs::msg::ScanInfo& scan_model); 171 | 172 | bool convert( 173 | const ParamTree::SharedPtr sensor_model_params, 174 | rmcl_msgs::msg::DepthInfo& depth_model); 175 | 176 | bool convert( 177 | const ParamTree::SharedPtr sensor_model_params, 178 | rmcl_msgs::msg::O1DnInfo& o1dn_model); 179 | 180 | bool convert( 181 | const ParamTree::SharedPtr sensor_model_params, 182 | rmcl_msgs::msg::OnDnInfo& ondn_model); 183 | 184 | bool convert( 185 | const ParamTree::SharedPtr data_params, 186 | rmcl_msgs::msg::RangeData& range_data); 187 | 188 | void estimateModelAndData( 189 | std_msgs::msg::Header& header, 190 | rmcl_msgs::msg::O1DnInfo& info, 191 | rmcl_msgs::msg::RangeData& data, 192 | const sensor_msgs::msg::PointCloud2& cloud); 193 | 194 | } // namespace rmcl 195 | 196 | #endif //RMCL_CONVERSIONS_HPP -------------------------------------------------------------------------------- /rmcl_ros/include/rmcl_ros/util/ros_defines.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2022, University Osnabrück 3 | * All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * * Redistributions of source code must retain the above copyright 8 | * notice, this list of conditions and the following disclaimer. 9 | * * Redistributions in binary form must reproduce the above copyright 10 | * notice, this list of conditions and the following disclaimer in the 11 | * documentation and/or other materials provided with the distribution. 12 | * * Neither the name of the University Osnabrück nor the 13 | * names of its contributors may be used to endorse or promote products 14 | * derived from this software without specific prior written permission. 15 | * 16 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 17 | * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 18 | * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 19 | * DISCLAIMED. IN NO EVENT SHALL University Osnabrück BE LIABLE FOR ANY 20 | * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 21 | * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 22 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 23 | * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 24 | * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | */ 27 | 28 | /** 29 | * @file 30 | * 31 | * @brief ROS defines 32 | * 33 | * @date 03.10.2022 34 | * @author Alexander Mock 35 | * 36 | * @copyright Copyright (c) 2022, University Osnabrück. All rights reserved. 37 | * This project is released under the 3-Clause BSD License. 38 | * 39 | */ 40 | 41 | #ifndef RMCL_UTIL_ROS_DEFINES_H 42 | #define RMCL_UTIL_ROS_DEFINES_H 43 | 44 | #include 45 | #include 46 | #include 47 | #include 48 | #include 49 | 50 | namespace rmcl 51 | { 52 | 53 | // using NodePtr = std::shared_ptr; 54 | // using SubscriberPtr = rclcpp::Subscription::SharedPtr; 55 | // using PublisherPtr = rclcpp::Publisher::SharedPtr; 56 | 57 | using ImageTransportPtr = std::shared_ptr; 58 | using ITSubscriberPtr = std::shared_ptr; 59 | using TFBufferPtr = std::shared_ptr; 60 | using TFListenerPtr = std::shared_ptr; 61 | 62 | } // namespace rmcl 63 | 64 | #endif // RMCL_UTIL_ROS_DEFINES_H -------------------------------------------------------------------------------- /rmcl_ros/include/rmcl_ros/util/ros_helper.h: -------------------------------------------------------------------------------- 1 | #ifndef RMCL_UTIL_ROS_HELPER_H 2 | #define RMCL_UTIL_ROS_HELPER_H 3 | 4 | #include 5 | #include 6 | 7 | namespace rmcl 8 | { 9 | 10 | std::string replace_char( 11 | const std::string& input, 12 | char old_char, 13 | char new_char); 14 | 15 | void replace_char_inplace( 16 | std::string& input, 17 | char old_char, 18 | char new_char); 19 | 20 | std::string make_sub_parameter( 21 | rclcpp::Node* node, 22 | const std::string& param_name); 23 | 24 | std::string make_sub_parameter( 25 | rclcpp::Node::SharedPtr node, 26 | const std::string& param_name); 27 | 28 | // special case for char arrays to be converted properly 29 | std::string get_parameter( 30 | rclcpp::Node* node, 31 | const std::string& param_name, 32 | const std::string& default_value); 33 | 34 | // special case for char arrays to be converted properly 35 | std::string get_parameter( 36 | rclcpp::Node::SharedPtr node, 37 | const std::string& param_name, 38 | const std::string& default_value); 39 | 40 | template 41 | T get_parameter( 42 | rclcpp::Node* node, 43 | const std::string& param_name, 44 | const T& default_value); 45 | 46 | template 47 | T get_parameter( 48 | rclcpp::Node::SharedPtr node, 49 | const std::string& param_name, 50 | const T& default_value); 51 | 52 | std::optional get_parameter( 53 | rclcpp::Node::SharedPtr node, 54 | const std::string& param_name); 55 | 56 | 57 | std::map get_parameters( 58 | rclcpp::Node* node, 59 | std::string prefix); 60 | 61 | std::map get_parameters( 62 | rclcpp::Node::SharedPtr node, 63 | std::string prefix); 64 | 65 | 66 | 67 | 68 | // Forward declarations 69 | template 70 | class ParamTree; 71 | 72 | template 73 | using ParamTreePtr = std::shared_ptr >; 74 | 75 | 76 | /** 77 | * This tree structure lets us simply parse dynamic parameter sets 78 | * It is required to have the correct node settings: 79 | * 80 | * @code 81 | * rclcpp::NodeOptions options = rclcpp::NodeOptions() 82 | * .allow_undeclared_parameters(true) 83 | * .automatically_declare_parameters_from_overrides(true); 84 | * @endcode 85 | */ 86 | template 87 | class ParamTree : public std::unordered_map > > 88 | { 89 | private: 90 | using Base = std::unordered_map > >; 91 | using ThisType = ParamTree; 92 | public: 93 | using SharedPtr = std::shared_ptr >; 94 | 95 | std::shared_ptr data; 96 | std::string name; // uid 97 | 98 | inline void insert(ParamTree::SharedPtr subtree) 99 | { 100 | if(Base::find(subtree->name) != Base::end()) 101 | { 102 | // exists, need to merge 103 | for(auto elem : *subtree) 104 | { 105 | Base::at(subtree->name)->insert(elem.second); 106 | } 107 | } else { 108 | Base::insert(std::make_pair(subtree->name, subtree)); 109 | } 110 | } 111 | 112 | inline void insert(std::string param_name, ParamT param) 113 | { 114 | size_t pos = param_name.find("."); 115 | if(pos != std::string::npos) 116 | { 117 | std::string first_name = param_name.substr(0, pos); 118 | std::string last_name = param_name.substr(pos + 1); 119 | 120 | ParamTree::SharedPtr child = std::make_shared(); 121 | child->name = first_name; 122 | child->insert(last_name, param); // recursion 123 | this->insert(child); 124 | } else { 125 | // leaf 126 | ParamTree::SharedPtr child = std::make_shared(); 127 | child->name = param_name; 128 | child->data = std::make_shared(param); 129 | this->insert(child); 130 | } 131 | } 132 | 133 | inline void print(size_t indent = 0) 134 | { 135 | for(size_t i=0; iprint(indent + 1); 144 | } 145 | } 146 | 147 | inline bool exists(std::string key) const{ 148 | return Base::find(key) != Base::end(); 149 | } 150 | }; 151 | 152 | inline ParamTree::SharedPtr get_parameter_tree( 153 | rclcpp::Node* node, 154 | std::string prefix) 155 | { 156 | ParamTree::SharedPtr ret; 157 | 158 | const std::string prefix_path = make_sub_parameter(node, prefix); 159 | const std::map param_map 160 | = get_parameters(node, prefix_path); 161 | 162 | ret = std::make_shared >(); 163 | for(auto elem : param_map) 164 | { 165 | ret->insert(elem.first, elem.second); 166 | } 167 | ret->name = prefix_path; 168 | return ret; 169 | } 170 | 171 | inline ParamTree::SharedPtr get_parameter_tree( 172 | rclcpp::Node::SharedPtr node, 173 | std::string prefix) 174 | { 175 | return get_parameter_tree(node.get(), prefix); 176 | } 177 | 178 | } // namespace rmcl 179 | 180 | #include "ros_helper.tcc" 181 | 182 | #endif // RMCL_UTIL_ROS_HELPER_H -------------------------------------------------------------------------------- /rmcl_ros/include/rmcl_ros/util/ros_helper.tcc: -------------------------------------------------------------------------------- 1 | #include "ros_helper.h" 2 | 3 | namespace rmcl 4 | { 5 | 6 | template 7 | T get_parameter( 8 | rclcpp::Node* node, 9 | const std::string& param_name, 10 | const T& default_value) 11 | { 12 | const std::string param_path = make_sub_parameter(node, param_name); 13 | T param_out; 14 | if(node->has_parameter(param_path)) 15 | { 16 | param_out = node->get_parameter(param_path).get_value(); 17 | } 18 | else 19 | { 20 | param_out = node->declare_parameter(param_path, default_value); 21 | } 22 | return param_out; 23 | } 24 | 25 | template 26 | T get_parameter( 27 | rclcpp::Node::SharedPtr node, 28 | const std::string& param_name, 29 | const T& default_value) 30 | { 31 | return get_parameter(node.get(), param_name, default_value); 32 | } 33 | 34 | } // namespace rmcl -------------------------------------------------------------------------------- /rmcl_ros/include/rmcl_ros/util/scan_operations.h: -------------------------------------------------------------------------------- 1 | /** 2 | * Copyright (c) 2021, University Osnabrück 3 | * All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * * Redistributions of source code must retain the above copyright 8 | * notice, this list of conditions and the following disclaimer. 9 | * * Redistributions in binary form must reproduce the above copyright 10 | * notice, this list of conditions and the following disclaimer in the 11 | * documentation and/or other materials provided with the distribution. 12 | * * Neither the name of the University Osnabrück nor the 13 | * names of its contributors may be used to endorse or promote products 14 | * derived from this software without specific prior written permission. 15 | * 16 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 17 | * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 18 | * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 19 | * DISCLAIMED. IN NO EVENT SHALL University Osnabrück BE LIABLE FOR ANY 20 | * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 21 | * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 22 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 23 | * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 24 | * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 25 | * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | */ 27 | 28 | /* 29 | * scan_operations.hpp 30 | * 31 | * Created on: Jul 17, 2021 32 | * Author: Alexander Mock 33 | */ 34 | 35 | #ifndef RMCL_UTIL_SCAN_OPERATIONS_H 36 | #define RMCL_UTIL_SCAN_OPERATIONS_H 37 | 38 | #include 39 | #include 40 | #include 41 | #include 42 | 43 | namespace rmcl { 44 | 45 | void fill( 46 | rmcl_msgs::msg::Scan& scan, 47 | const rmagine::Memory& ranges); 48 | 49 | void fillEmpty(rmcl_msgs::msg::Scan& scan); 50 | 51 | 52 | struct IterationOptions 53 | { 54 | size_t skip_begin = 0; 55 | size_t skip_end = 0; 56 | size_t increment = 0; 57 | }; 58 | 59 | struct FilterOptions2D 60 | { 61 | float range_min; 62 | float range_max; 63 | 64 | IterationOptions width; 65 | IterationOptions height; 66 | }; 67 | 68 | struct FilterOptions1D 69 | { 70 | float range_min; 71 | float range_max; 72 | 73 | IterationOptions skip; 74 | }; 75 | 76 | void filter( 77 | rmcl_msgs::msg::O1Dn& o1dn_out, 78 | const rmcl_msgs::msg::O1Dn& o1dn_in, 79 | const FilterOptions2D& options); 80 | 81 | 82 | } // namespace rmcl 83 | 84 | #endif // RMCL_UTIL_SCAN_OPERATIONS_H -------------------------------------------------------------------------------- /rmcl_ros/include/rmcl_ros/util/text_colors.h: -------------------------------------------------------------------------------- 1 | #ifndef RMCL_ROS_UTIL_TEXT_COLORS_H 2 | #define RMCL_ROS_UTIL_TEXT_COLORS_H 3 | 4 | // TEXT COLORS 5 | #define TC_BLACK "\033[1;30m" 6 | #define TC_RED "\033[1;31m" 7 | #define TC_GREEN "\033[1;32m" 8 | #define TC_YELLOW "\033[1;33m" 9 | #define TC_BLUE "\033[1;34m" 10 | #define TC_MAGENTA "\033[1;35m" 11 | #define TC_CYAN "\033[1;36m" 12 | #define TC_WHITE "\033[1;37m" 13 | 14 | #define TC_END "\033[0m" 15 | 16 | #define TC_SENSOR TC_YELLOW 17 | #define TC_TOPIC TC_CYAN 18 | #define TC_FRAME TC_MAGENTA 19 | #define TC_MSG TC_WHITE 20 | #define TC_BACKENDS TC_BLUE 21 | 22 | 23 | #endif // RMCL_ROS_UTIL_TEXT_COLORS_H -------------------------------------------------------------------------------- /rmcl_ros/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | rmcl_ros 5 | 2.2.0 6 | The RMCL package. Software Tools for Mobile Robot Localization in 3D Meshes. 7 | 8 | Alexander Mock 9 | 10 | BSD 11 | Alexander Mock 12 | 13 | ament_cmake 14 | 15 | rclcpp 16 | geometry_msgs 17 | sensor_msgs 18 | tf2_ros 19 | rmcl_msgs 20 | image_transport 21 | visualization_msgs 22 | rmcl 23 | rmagine 24 | 25 | 26 | ament_cmake 27 | 28 | 29 | -------------------------------------------------------------------------------- /rmcl_ros/src/benchmarks/lidar_corrector_embree_benchmark.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | #include 10 | 11 | #include 12 | 13 | 14 | using namespace rmagine; 15 | using namespace rmcl; 16 | 17 | bool find_abc(unsigned int& A, unsigned int& B, unsigned int C) 18 | { 19 | A = static_cast( sqrt(C) ); 20 | B = A; 21 | 22 | unsigned int res = A * B; 23 | 24 | while(res != C && B > 0 && A <= C) 25 | { 26 | if(res > C) 27 | { 28 | B--; 29 | } else { 30 | A++; 31 | } 32 | res = A * B; 33 | } 34 | 35 | return (B > 0 && A <= C && res == C); 36 | } 37 | 38 | EmbreeMapPtr make_sphere_map(unsigned int Nfaces) 39 | { 40 | // N = Nfaces / 2 41 | 42 | // A*B = N 43 | // 44 | // A = N / B 45 | // B = N / A 46 | // A, B must be integer. N / B must be possible 47 | 48 | if(Nfaces % 2 == 1) 49 | { 50 | throw std::runtime_error("Nfaces must be even"); 51 | } 52 | 53 | 54 | size_t N = Nfaces / 2; 55 | 56 | unsigned int A, B; 57 | if(!find_abc(A, B, N) ) 58 | { 59 | throw std::runtime_error("Could not find integers for A*B=N"); 60 | } 61 | 62 | 63 | 64 | EmbreeMeshPtr sphere_mesh = std::make_shared(A, B); 65 | sphere_mesh->commit(); 66 | std::cout << A << "x" << B << " * 2 = " << Nfaces << " = " << sphere_mesh->faces().size() << std::endl; 67 | EmbreeScenePtr sphere_scene = std::make_shared(); 68 | sphere_scene->add(sphere_mesh); 69 | sphere_scene->commit(); 70 | return std::make_shared(sphere_scene); 71 | } 72 | 73 | int main(int argc, char** argv) 74 | { 75 | // TODO: somewhere is memory that is not be freed when the scene is overwritten 76 | 77 | size_t Nfaces = atoi(argv[1]); 78 | size_t Nposes = atoi(argv[2]); 79 | size_t Nruns = 10; 80 | 81 | 82 | std::cout << "Running benchmark with " << Nfaces << " faces and " << Nposes << " poses" << std::endl; 83 | 84 | EmbreeMapPtr sphere_map_tmp = make_sphere_map(Nfaces); 85 | 86 | SphereCorrectorEmbree correct(sphere_map_tmp); 87 | correct.setTsb(Transform::Identity()); 88 | 89 | // return 0; 90 | 91 | auto model = vlp16_900(); 92 | model.range.min = 0.0; 93 | correct.setModel(model); 94 | 95 | using ResultT = Bundle< 96 | Ranges 97 | >; 98 | 99 | ResultT sim_res; 100 | sim_res.ranges.resize(model.size()); 101 | 102 | Memory T_dest(1); 103 | T_dest[0] = Transform::Identity(); 104 | 105 | 106 | StopWatch sw; 107 | double el; 108 | 109 | Memory T_curr(Nposes); 110 | for(size_t i=0; i(Nruns) << std::endl; 139 | 140 | // TIMINGS 0 141 | 142 | // DPC (AMOCKHOME) 143 | // #faces, #poses, correction runtime 144 | // 100000,1000,0.18301 145 | // 500000,1000,0.203063 146 | // 1000000,1000,0.201213 147 | // 2000000,1000,0.23414 148 | // 3000000,1000,0.341999 149 | // 4000000,1000,0.350003 150 | // 6000000,1000,0.419774 151 | // 8000000,1000,0.448595 152 | // 10000000,1000,0.455784 153 | 154 | // NUC 155 | // #faces, #poses, correction runtime 156 | // 100000,1000,0.377121 157 | // 500000,1000,0.429566 158 | // 1000000,1000,0.439094 159 | // 2000000,1000,0.555697 160 | // 3000000,1000,0.624029 161 | // 4000000,1000,0.652404 162 | // 6000000,1000,0.703334 163 | // 8000000,1000,0.740325 164 | // 10000000,1000,0.773015 165 | 166 | // LX1 167 | // #faces, #poses, correction runtime 168 | // 100000,1000,0.3775 169 | // 500000,1000,0.410651 170 | // 1000000,1000,0.421343 171 | // 2000000,1000,0.460799 172 | // 3000000,1000,0.508743 173 | // 4000000,1000,0.522019 174 | // 6000000,1000,0.563217 175 | // 8000000,1000,0.601782 176 | // 10000000,1000,0.611425 177 | 178 | 179 | // TIMINGS 1 180 | // args: 1000000 1000 181 | 182 | // DPC (AMOCKHOME) 183 | // 184 | // Absolute: 185 | // - Sim: 2.35949 186 | // - Red: 0.0771303 187 | // - SVD: 0.000365363 188 | // - Total: 2.43698 189 | // - Relative: 190 | // 0.9682,0.0316499,0.000149924 191 | // 192 | // NUC 193 | // 194 | // Absolute: 195 | // - Sim: 2.39404 196 | // - Red: 0.0890634 197 | // - SVD: 0.000354392 198 | // - Total: 2.48346 199 | // - Relative: 200 | // 0.963995,0.0358627,0.000142701 201 | 202 | // LX1 203 | // Absolute: 204 | // - Sim: 2.82588 205 | // - Red: 0.13325 206 | // - SVD: 0.00047981 207 | // - Total: 2.9596 208 | // - Relative: 209 | // 0.954815,0.0450229,0.000162119 210 | 211 | return 0; 212 | } -------------------------------------------------------------------------------- /rmcl_ros/src/benchmarks/lidar_corrector_optix_benchmark.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | #include 10 | 11 | #include 12 | 13 | 14 | using namespace rmagine; 15 | using namespace rmcl; 16 | 17 | bool find_abc(unsigned int& A, unsigned int& B, unsigned int C) 18 | { 19 | A = static_cast( sqrt(C) ); 20 | B = A; 21 | 22 | unsigned int res = A * B; 23 | 24 | while(res != C && B > 0 && A <= C) 25 | { 26 | if(res > C) 27 | { 28 | B--; 29 | } else { 30 | A++; 31 | } 32 | res = A * B; 33 | } 34 | 35 | return (B > 0 && A <= C && res == C); 36 | } 37 | 38 | OptixMapPtr make_sphere_map(unsigned int Nfaces) 39 | { 40 | // N = Nfaces / 2 41 | 42 | // A*B = N 43 | // 44 | // A = N / B 45 | // B = N / A 46 | // A, B must be integer. N / B must be possible 47 | 48 | if(Nfaces % 2 == 1) 49 | { 50 | throw std::runtime_error("Nfaces must be even"); 51 | } 52 | 53 | 54 | size_t N = Nfaces / 2; 55 | 56 | unsigned int A, B; 57 | if(!find_abc(A, B, N) ) 58 | { 59 | throw std::runtime_error("Could not find integers for A*B=N"); 60 | } 61 | 62 | std::cout << A << "x" << B << " * 2 = " << Nfaces << std::endl; 63 | 64 | OptixMeshPtr sphere_mesh = std::make_shared(A, B); 65 | sphere_mesh->commit(); 66 | OptixScenePtr sphere_scene = std::make_shared(); 67 | sphere_scene->add(sphere_mesh); 68 | sphere_scene->commit(); 69 | return std::make_shared(sphere_scene); 70 | } 71 | 72 | int main(int argc, char** argv) 73 | { 74 | // TODO: somewhere is memory that is not be freed when the scene is overwritten 75 | 76 | size_t Nfaces = atoi(argv[1]); 77 | size_t Nposes = atoi(argv[2]); 78 | size_t Nruns = 1000; 79 | 80 | 81 | std::cout << "Running benchmark with " << Nfaces << " faces and " << Nposes << " poses" << std::endl; 82 | 83 | OptixMapPtr sphere_map_tmp = make_sphere_map(Nfaces); 84 | 85 | SphereCorrectorOptix correct(sphere_map_tmp); 86 | correct.setTsb(Transform::Identity()); 87 | 88 | // return 0; 89 | 90 | auto model = vlp16_900(); 91 | model.range.min = 0.0; 92 | correct.setModel(model); 93 | 94 | using ResultT = Bundle< 95 | Ranges 96 | >; 97 | 98 | ResultT sim_res; 99 | sim_res.ranges.resize(model.size()); 100 | 101 | Memory T_dest(1); 102 | T_dest[0] = Transform::Identity(); 103 | 104 | 105 | StopWatch sw; 106 | double el; 107 | 108 | Memory T_curr(Nposes); 109 | for(size_t i=0; i T_dest_ = T_dest; 116 | Memory T_curr_ = T_curr; 117 | 118 | // simulate the data that would be recorded at destination 119 | correct.simulate(T_dest_, sim_res); 120 | correct.setInputData(sim_res.ranges); 121 | 122 | 123 | // timings: 124 | // - 0: measure outer timings 125 | // - 1: measure inner timings 126 | 127 | int timings = 0; 128 | 129 | if(timings == 0) 130 | { 131 | double el_total = 0.0; 132 | for(size_t i=0; i(Nruns) << std::endl; 143 | 144 | } else if(timings == 1) { 145 | auto bres = correct.benchmark(T_curr_, Nruns); 146 | std::cout << "Absolute:" << std::endl; 147 | std::cout << "- Sim: " << bres.sim << std::endl; 148 | std::cout << "- Red: " << bres.red << std::endl; 149 | std::cout << "- SVD: " << bres.svd << std::endl; 150 | 151 | double total = bres.sim + bres.red + bres.svd; 152 | std::cout << "- Total: " << total << std::endl; 153 | std::cout << "- Relative: " << std::endl; 154 | std::cout << bres.sim / total << "," << bres.red / total << "," << bres.svd / total << std::endl; 155 | } 156 | 157 | // TIMINGS 0 158 | 159 | // DPC (AMOCKHOME) 160 | // #faces, #poses, correction runtime 161 | // 100000,1000,0.0135733 162 | // 500000,1000,0.0141381 163 | // 1000000,1000,0.0169034 164 | // 2000000,1000,0.0212061 165 | // 3000000,1000,0.0242707 166 | // 4000000,1000,0.0256912 167 | // 6000000,1000,0.0278478 168 | // 8000000,1000,0.0291446 169 | // 10000000,1000,0.0311896 170 | 171 | // NUC 172 | // #faces, #poses, correction runtime 173 | // 100000,1000,0.0131995 174 | // 500000,1000,0.0192545 175 | // 1000000,1000,0.0231976 176 | // 2000000,1000,0.0307703 177 | // 3000000,1000,0.0365178 178 | // 4000000,1000,0.0360314 179 | // 6000000,1000,0.0400326 180 | // 8000000,1000,0.0433546 181 | // 10000000,1000,0.0449058 182 | 183 | // LX1 184 | // #faces, #poses, correction runtime 185 | // 100000,1000,0.133156 186 | // 500000,1000,0.167014 187 | // 1000000,1000,0.185449 188 | // 2000000,1000,0.208977 189 | // 3000000,1000,0.235557 190 | // 4000000,1000,0.241927 191 | // 6000000,1000,0.259021 192 | // 8000000,1000,0.270712 193 | // 10000000,1000,0.273286 194 | 195 | 196 | // TIMINGS 1 197 | // args 1000000 1000 198 | 199 | // DPC (AMOCKHOME) 200 | // 201 | // Absolute: 202 | // - Sim: 0.0119368 203 | // - Red: 1.32316e-05 204 | // - SVD: 0.0028495 205 | // - Total: 0.0147996 206 | // - Relative: 207 | // 0.806567,0.000894055,0.192539 208 | 209 | // NUC 210 | // 211 | // Absolute: 212 | // - Sim: 0.0181216 213 | // - Red: 8.49177e-05 214 | // - SVD: 0.00435061 215 | // - Total: 0.0225571 216 | // - Relative: 217 | // 0.803365,0.00376456,0.192871 218 | 219 | // LX1 220 | // 221 | // 222 | 223 | 224 | 225 | 226 | return 0; 227 | } -------------------------------------------------------------------------------- /rmcl_ros/src/micpl/MICPO1DnSensorCPU.cpp: -------------------------------------------------------------------------------- 1 | #include "rmcl_ros/micpl/MICPO1DnSensorCPU.hpp" 2 | 3 | #include 4 | #include 5 | 6 | #include 7 | 8 | #include 9 | #include 10 | 11 | #include 12 | 13 | using namespace std::chrono_literals; 14 | 15 | namespace rm = rmagine; 16 | 17 | namespace rmcl 18 | { 19 | 20 | MICPO1DnSensorCPU::MICPO1DnSensorCPU( 21 | rclcpp::Node::SharedPtr nh) 22 | :MICPSensorCPU(nh) 23 | { 24 | Tom.setIdentity(); 25 | correspondences_.reset(); 26 | } 27 | 28 | void MICPO1DnSensorCPU::connectToTopic(const std::string& topic_name) 29 | { 30 | static_dataset = false; 31 | 32 | std::chrono::duration buffer_timeout(1); 33 | 34 | tf_filter_ = std::make_unique >( 35 | data_sub_, *tf_buffer_, odom_frame, 10, nh_->get_node_logging_interface(), 36 | nh_->get_node_clock_interface(), buffer_timeout); 37 | 38 | rclcpp::SubscriptionOptions sub_options; 39 | sub_options.callback_group = cb_group_; 40 | rclcpp::QoS qos(10); // = rclcpp::SystemDefaultsQoS(); 41 | data_sub_.subscribe(nh_, topic_name, qos.get_rmw_qos_profile(), sub_options); // delete "get_rmw_..." for rolling 42 | 43 | tf_filter_->registerCallback(&MICPO1DnSensorCPU::updateMsg, this); 44 | 45 | RCLCPP_INFO_STREAM(nh_->get_logger(), "[" << name << "] [MICPO1DnSensorCPU] Waiting for message from topic '" << topic_name << "'..."); 46 | } 47 | 48 | void MICPO1DnSensorCPU::getDataFromParameters() 49 | { 50 | static_dataset = true; 51 | 52 | // fill this: 53 | rmcl_msgs::msg::O1DnStamped::SharedPtr o1dn_stamped 54 | = std::make_shared(); 55 | 56 | const ParamTree::SharedPtr sensor_param_tree 57 | = get_parameter_tree(nh_, "~"); 58 | 59 | // 1. Load Model 60 | const ParamTree::SharedPtr sensor_model_params 61 | = sensor_param_tree->at("model"); 62 | 63 | if(!convert(sensor_model_params, o1dn_stamped->o1dn.info)) 64 | { 65 | // could parse data from parameters 66 | throw std::runtime_error("Could not load O1Dn model from parameters!"); 67 | return; 68 | } 69 | 70 | // 2. Load Data 71 | const ParamTree::SharedPtr sensor_data_params 72 | = sensor_param_tree->at("data"); 73 | 74 | if(!convert(sensor_data_params, o1dn_stamped->o1dn.data)) 75 | { 76 | // could parse data from parameters 77 | throw std::runtime_error("Could not load O1Dn data from parameters!"); 78 | return; 79 | } 80 | 81 | if(sensor_data_params->exists("frame")) 82 | { 83 | sensor_frame = sensor_data_params->at("frame")->data->as_string(); 84 | } 85 | 86 | o1dn_stamped->header.frame_id = sensor_frame; 87 | o1dn_stamped->header.stamp = nh_->now(); 88 | 89 | updateMsg(o1dn_stamped); 90 | } 91 | 92 | void MICPO1DnSensorCPU::updateMsg( 93 | const rmcl_msgs::msg::O1DnStamped::SharedPtr msg) 94 | { 95 | rm::StopWatch sw; 96 | const rclcpp::Time now_time = nh_->get_clock()->now(); 97 | const rclcpp::Time msg_time = msg->header.stamp; 98 | 99 | if(now_time.get_clock_type() != msg_time.get_clock_type()) 100 | { 101 | RCLCPP_WARN_STREAM(nh_->get_logger(), "[" << name << "::topicCB] WARNING - STAMP MISMATCH: (now - input msg's stamp) have different clock types: " << now_time.get_clock_type()); 102 | return; 103 | } 104 | 105 | // const double diff_now_msg = (now_time - msg_time).seconds(); 106 | 107 | double diff_now_msg; 108 | 109 | try { 110 | diff_now_msg = (now_time - msg_time).seconds(); 111 | } catch(const std::runtime_error& ex) { 112 | RCLCPP_WARN_STREAM(nh_->get_logger(), "[" << name << "::topicCB] WARNING - STAMP MISMATCH: (now - input msg's stamp) have different clock type"); 113 | return; 114 | } 115 | 116 | if(fabs(diff_now_msg) > 0.5) 117 | { 118 | RCLCPP_WARN_STREAM(nh_->get_logger(), "[" << name << "::topicCB] WARNING - NETWORK DELAY: (now - input msg's stamp) is more far apart (" << diff_now_msg * 1000.0 << " ms). It is likely that control algorithms will not work as expected."); 119 | } 120 | 121 | sw(); 122 | data_correction_mutex_.lock(); 123 | const double el_mutex_lock = sw(); 124 | 125 | sw(); 126 | // Part 1: transfrom sensor and filter input data 127 | dataset_stamp_ = msg->header.stamp; 128 | sensor_frame = msg->header.frame_id; 129 | fetchTF(dataset_stamp_); 130 | correspondences_->setTsb(Tsb); 131 | const double el_fetch_tf = sw(); 132 | 133 | // const double diff_odom_msg = (Tbo_stamp - dataset_stamp_).seconds(); 134 | 135 | double diff_odom_msg; 136 | try { 137 | diff_odom_msg = (Tbo_stamp - dataset_stamp_).seconds(); 138 | } catch(const std::runtime_error& ex) { 139 | RCLCPP_WARN_STREAM(nh_->get_logger(), "[" << name << "::topicCB] WARNING - STAMP MISMATCH: (now - input msg's stamp) have different clock type"); 140 | data_correction_mutex_.unlock(); 141 | return; 142 | } 143 | 144 | // copy to internal representation 145 | // fill sensor_model_ and initialize copy data to dataset 146 | sw(); 147 | unpackMessage(msg); 148 | if(auto model_setter = std::dynamic_pointer_cast< 149 | rm::ModelSetter >(correspondences_)) 150 | { 151 | // RCC required sensor model 152 | model_setter->setModel(sensor_model_); 153 | } 154 | const double el_unpack_msg = sw(); 155 | 156 | correspondences_->outdated = true; 157 | first_message_received = true; 158 | 159 | data_correction_mutex_.unlock(); 160 | 161 | { // print stats 162 | RCLCPP_DEBUG_STREAM(nh_->get_logger(), "[" << name << "::topicCB] MICPO1DnSensorCPU Timings:"); 163 | RCLCPP_DEBUG_STREAM(nh_->get_logger(), "[" << name << "::topicCB] - (Now - msg stamp) = " << diff_now_msg * 1000.0 << " ms"); 164 | RCLCPP_DEBUG_STREAM(nh_->get_logger(), "[" << name << "::topicCB] - (Odom - msg stamp) = " << diff_odom_msg * 1000.0 << " ms"); 165 | RCLCPP_DEBUG_STREAM(nh_->get_logger(), "[" << name << "::topicCB] - Lock mutex: " << el_mutex_lock * 1000.0 << " ms"); 166 | RCLCPP_DEBUG_STREAM(nh_->get_logger(), "[" << name << "::topicCB] - Fetch TF: " << el_fetch_tf * 1000.0 << " ms"); 167 | RCLCPP_DEBUG_STREAM(nh_->get_logger(), "[" << name << "::topicCB] - Unpack message (" << correspondences_->dataset.points.size() << "): " << el_unpack_msg * 1000.0 << " ms"); 168 | } 169 | 170 | if(!static_dataset) 171 | { 172 | on_data_received(this); 173 | } 174 | } 175 | 176 | 177 | void MICPO1DnSensorCPU::unpackMessage( 178 | const rmcl_msgs::msg::O1DnStamped::SharedPtr msg) 179 | { 180 | ///// 181 | // sensor model 182 | rmcl::convert(msg->o1dn.info, sensor_model_); 183 | 184 | //// 185 | // data: TODOs: 186 | // - use input mask values 187 | // - use input normals 188 | size_t n_old_measurements = correspondences_->dataset.points.size(); 189 | size_t n_new_measurements = msg->o1dn.data.ranges.size(); 190 | if(n_new_measurements > n_old_measurements) 191 | { 192 | // need to resize buffers 193 | correspondences_->dataset.points.resize(n_new_measurements); 194 | correspondences_->dataset.mask.resize(n_new_measurements); 195 | for(size_t i=n_old_measurements; idataset.mask[i] = 1; 198 | } 199 | } 200 | 201 | total_dataset_measurements = n_new_measurements; 202 | valid_dataset_measurements = 0; 203 | 204 | // sw(); 205 | // fill data 206 | for(unsigned int vid = 0; vid < sensor_model_.getHeight(); vid++) 207 | { 208 | for(unsigned int hid = 0; hid < sensor_model_.getWidth(); hid++) 209 | { 210 | const unsigned int loc_id = sensor_model_.getBufferId(vid, hid); 211 | const float real_range = msg->o1dn.data.ranges[loc_id]; 212 | const rm::Vector3f real_point = sensor_model_.getDirection(vid, hid) * real_range 213 | + sensor_model_.getOrigin(vid, hid); 214 | correspondences_->dataset.points[loc_id] = real_point; 215 | 216 | if(real_range < sensor_model_.range.min || real_range > sensor_model_.range.max) 217 | { 218 | // out of range 219 | correspondences_->dataset.mask[loc_id] = 0; 220 | } else { 221 | correspondences_->dataset.mask[loc_id] = 1; 222 | valid_dataset_measurements++; 223 | } 224 | } 225 | } 226 | 227 | dataset_stamp_ = msg->header.stamp; 228 | } 229 | 230 | } // namespace rmcl -------------------------------------------------------------------------------- /rmcl_ros/src/micpl/MICPO1DnSensorCUDA.cpp: -------------------------------------------------------------------------------- 1 | #include "rmcl_ros/micpl/MICPO1DnSensorCUDA.hpp" 2 | 3 | #include 4 | #include 5 | 6 | #include 7 | 8 | #include 9 | #include 10 | 11 | #include 12 | 13 | using namespace std::chrono_literals; 14 | 15 | namespace rm = rmagine; 16 | 17 | namespace rmcl 18 | { 19 | 20 | MICPO1DnSensorCUDA::MICPO1DnSensorCUDA( 21 | rclcpp::Node::SharedPtr nh) 22 | :Base(nh) 23 | { 24 | Tom.setIdentity(); 25 | correspondences_.reset(); 26 | } 27 | 28 | void MICPO1DnSensorCUDA::connectToTopic(const std::string& topic_name) 29 | { 30 | static_dataset = false; 31 | 32 | std::chrono::duration buffer_timeout(1); 33 | 34 | tf_filter_ = std::make_unique >( 35 | data_sub_, *tf_buffer_, odom_frame, 10, nh_->get_node_logging_interface(), 36 | nh_->get_node_clock_interface(), buffer_timeout); 37 | 38 | rclcpp::SubscriptionOptions sub_options; 39 | sub_options.callback_group = cb_group_; 40 | rclcpp::QoS qos(10); // = rclcpp::SystemDefaultsQoS(); 41 | data_sub_.subscribe(nh_, topic_name, qos.get_rmw_qos_profile(), sub_options); // delete "get_rmw_..." for rolling 42 | 43 | tf_filter_->registerCallback(&MICPO1DnSensorCUDA::updateMsg, this); 44 | RCLCPP_INFO_STREAM(nh_->get_logger(), "[" << name << "] [MICPO1DnSensorCUDA] Waiting for message from topic '" << topic_name << "'..."); 45 | } 46 | 47 | void MICPO1DnSensorCUDA::getDataFromParameters() 48 | { 49 | static_dataset = true; 50 | 51 | // fill this: 52 | rmcl_msgs::msg::O1DnStamped::SharedPtr o1dn_stamped 53 | = std::make_shared(); 54 | 55 | const ParamTree::SharedPtr sensor_param_tree 56 | = get_parameter_tree(nh_, "~"); 57 | 58 | // 1. Load Model 59 | const ParamTree::SharedPtr sensor_model_params 60 | = sensor_param_tree->at("model"); 61 | 62 | if(!convert(sensor_model_params, o1dn_stamped->o1dn.info)) 63 | { 64 | // could parse data from parameters 65 | throw std::runtime_error("Could not load O1Dn model from parameters!"); 66 | return; 67 | } 68 | 69 | // 2. Load Data 70 | const ParamTree::SharedPtr sensor_data_params 71 | = sensor_param_tree->at("data"); 72 | 73 | if(!convert(sensor_data_params, o1dn_stamped->o1dn.data)) 74 | { 75 | // could parse data from parameters 76 | throw std::runtime_error("Could not load O1Dn data from parameters!"); 77 | return; 78 | } 79 | 80 | if(sensor_data_params->exists("frame")) 81 | { 82 | sensor_frame = sensor_data_params->at("frame")->data->as_string(); 83 | } 84 | 85 | o1dn_stamped->header.frame_id = sensor_frame; 86 | o1dn_stamped->header.stamp = nh_->now(); 87 | 88 | updateMsg(o1dn_stamped); 89 | } 90 | 91 | void MICPO1DnSensorCUDA::updateMsg( 92 | const rmcl_msgs::msg::O1DnStamped::SharedPtr msg) 93 | { 94 | rm::StopWatch sw; 95 | const rclcpp::Time now_time = nh_->get_clock()->now(); 96 | const rclcpp::Time msg_time = msg->header.stamp; 97 | 98 | if(now_time.get_clock_type() != msg_time.get_clock_type()) 99 | { 100 | RCLCPP_WARN_STREAM(nh_->get_logger(), "[" << name << "::topicCB] WARNING - STAMP MISMATCH: (now - input msg's stamp) have different clock types: " << now_time.get_clock_type()); 101 | return; 102 | } 103 | 104 | double diff_now_msg; 105 | 106 | try { 107 | diff_now_msg = (now_time - msg_time).seconds(); 108 | } catch(const std::runtime_error& ex) { 109 | RCLCPP_WARN_STREAM(nh_->get_logger(), "[" << name << "::topicCB] WARNING - STAMP MISMATCH: (now - input msg's stamp) have different clock type"); 110 | return; 111 | } 112 | 113 | if(fabs(diff_now_msg) > 0.5) 114 | { 115 | RCLCPP_WARN_STREAM(nh_->get_logger(), "[" << name << "::topicCB] WARNING - NETWORK DELAY: (now - input msg's stamp) is more far apart (" << diff_now_msg * 1000.0 << " ms). It is likely that control algorithms will not work as expected."); 116 | } 117 | 118 | sw(); 119 | data_correction_mutex_.lock(); 120 | const double el_mutex_lock = sw(); 121 | 122 | sw(); 123 | // Part 1: transfrom sensor and filter input data 124 | dataset_stamp_ = msg->header.stamp; 125 | sensor_frame = msg->header.frame_id; 126 | fetchTF(dataset_stamp_); 127 | correspondences_->setTsb(Tsb); 128 | const double el_fetch_tf = sw(); 129 | 130 | double diff_odom_msg; 131 | try { 132 | diff_odom_msg = (Tbo_stamp - dataset_stamp_).seconds(); 133 | } catch(const std::runtime_error& ex) { 134 | RCLCPP_WARN_STREAM(nh_->get_logger(), "[" << name << "::topicCB] WARNING - STAMP MISMATCH: (now - input msg's stamp) have different clock type"); 135 | data_correction_mutex_.unlock(); 136 | return; 137 | } 138 | 139 | // copy to internal representation 140 | // fill sensor_model_ and initialize copy data to dataset 141 | sw(); 142 | unpackMessage(msg); 143 | if(auto model_setter = std::dynamic_pointer_cast< 144 | rm::ModelSetter >(correspondences_)) 145 | { 146 | // RCC required sensor model 147 | model_setter->setModel(sensor_model_); 148 | } 149 | const double el_unpack_msg = sw(); 150 | 151 | correspondences_->outdated = true; 152 | first_message_received = true; 153 | 154 | data_correction_mutex_.unlock(); 155 | 156 | { // print conversion & sync delay 157 | RCLCPP_DEBUG_STREAM(nh_->get_logger(), "[" << name << "::topicCB] MICPO1DnSensorCUDA Timings:"); 158 | RCLCPP_DEBUG_STREAM(nh_->get_logger(), "[" << name << "::topicCB] - (Now - msg stamp) = " << diff_now_msg * 1000.0 << " ms"); 159 | RCLCPP_DEBUG_STREAM(nh_->get_logger(), "[" << name << "::topicCB] - (Odom - msg stamp) = " << diff_odom_msg * 1000.0 << " ms"); 160 | RCLCPP_DEBUG_STREAM(nh_->get_logger(), "[" << name << "::topicCB] - Lock mutex: " << el_mutex_lock * 1000.0 << " ms"); 161 | RCLCPP_DEBUG_STREAM(nh_->get_logger(), "[" << name << "::topicCB] - Fetch TF: " << el_fetch_tf * 1000.0 << " ms"); 162 | RCLCPP_DEBUG_STREAM(nh_->get_logger(), "[" << name << "::topicCB] - Unpack message (" << correspondences_->dataset.points.size() << "): " << el_unpack_msg * 1000.0 << " ms"); 163 | } 164 | 165 | if(!static_dataset) 166 | { 167 | on_data_received(this); 168 | } 169 | } 170 | 171 | void MICPO1DnSensorCUDA::unpackMessage( 172 | const rmcl_msgs::msg::O1DnStamped::SharedPtr msg) 173 | { 174 | ///// 175 | // sensor model 176 | // std::cout << "INFO: " << msg->o1dn.info.range_min << ", " << msg->o1dn.info.range_max << std::endl; 177 | rmcl::convert(msg->o1dn.info, sensor_model_); 178 | 179 | //// 180 | // data: TODOs: 181 | // - use input mask values 182 | // - use input normals 183 | size_t n_old_measurements = correspondences_->dataset.points.size(); 184 | size_t n_new_measurements = msg->o1dn.data.ranges.size(); 185 | if(n_new_measurements > n_old_measurements) 186 | { 187 | // need to resize buffers 188 | correspondences_->dataset.points.resize(n_new_measurements); 189 | correspondences_->dataset.mask.resize(n_new_measurements); 190 | dataset_cpu_.points.resize(n_new_measurements); 191 | dataset_cpu_.mask.resize(n_new_measurements); 192 | 193 | for(size_t i=n_old_measurements; io1dn.data.ranges[loc_id]; 210 | const rm::Vector3f real_point = sensor_model_.getDirection(vid, hid) * real_range 211 | + sensor_model_.getOrigin(vid, hid);; 212 | dataset_cpu_.points[loc_id] = real_point; 213 | 214 | if(real_range < sensor_model_.range.min || real_range > sensor_model_.range.max) 215 | { 216 | // out of range 217 | dataset_cpu_.mask[loc_id] = 0; 218 | } else { 219 | dataset_cpu_.mask[loc_id] = 1; 220 | valid_dataset_measurements++; 221 | } 222 | } 223 | } 224 | 225 | // upload 226 | correspondences_->dataset.points = dataset_cpu_.points; 227 | correspondences_->dataset.mask = dataset_cpu_.mask; 228 | 229 | dataset_stamp_ = msg->header.stamp; 230 | } 231 | 232 | } // namespace rmcl -------------------------------------------------------------------------------- /rmcl_ros/src/micpl/MICPOnDnSensorCUDA.cpp: -------------------------------------------------------------------------------- 1 | #include "rmcl_ros/micpl/MICPOnDnSensorCUDA.hpp" 2 | 3 | #include 4 | #include 5 | 6 | #include 7 | 8 | #include 9 | #include 10 | 11 | #include 12 | 13 | using namespace std::chrono_literals; 14 | 15 | namespace rm = rmagine; 16 | 17 | namespace rmcl 18 | { 19 | 20 | MICPOnDnSensorCUDA::MICPOnDnSensorCUDA( 21 | rclcpp::Node::SharedPtr nh) 22 | :Base(nh) 23 | { 24 | Tom.setIdentity(); 25 | correspondences_.reset(); 26 | } 27 | 28 | void MICPOnDnSensorCUDA::connectToTopic(const std::string& topic_name) 29 | { 30 | static_dataset = false; 31 | 32 | std::chrono::duration buffer_timeout(1); 33 | 34 | tf_filter_ = std::make_unique >( 35 | data_sub_, *tf_buffer_, odom_frame, 10, nh_->get_node_logging_interface(), 36 | nh_->get_node_clock_interface(), buffer_timeout); 37 | 38 | rclcpp::SubscriptionOptions sub_options; 39 | sub_options.callback_group = cb_group_; 40 | rclcpp::QoS qos(10); // = rclcpp::SystemDefaultsQoS(); 41 | data_sub_.subscribe(nh_, topic_name, qos.get_rmw_qos_profile(), sub_options); // delete "get_rmw_..." for rolling 42 | tf_filter_->registerCallback(&MICPOnDnSensorCUDA::updateMsg, this); 43 | 44 | 45 | 46 | RCLCPP_INFO_STREAM(nh_->get_logger(), "[" << name << "] [MICPOnDnSensorCUDA] Waiting for message from topic '" << topic_name << "'..."); 47 | } 48 | 49 | void MICPOnDnSensorCUDA::getDataFromParameters() 50 | { 51 | static_dataset = true; 52 | 53 | // fill this: 54 | rmcl_msgs::msg::OnDnStamped::SharedPtr ondn_stamped 55 | = std::make_shared(); 56 | 57 | const ParamTree::SharedPtr sensor_param_tree 58 | = get_parameter_tree(nh_, "~"); 59 | 60 | // 1. Load Model 61 | const ParamTree::SharedPtr sensor_model_params 62 | = sensor_param_tree->at("model"); 63 | 64 | if(!convert(sensor_model_params, ondn_stamped->ondn.info)) 65 | { 66 | // could parse data from parameters 67 | throw std::runtime_error("Could not load OnDn model from parameters!"); 68 | return; 69 | } 70 | 71 | // 2. Load Data 72 | const ParamTree::SharedPtr sensor_data_params 73 | = sensor_param_tree->at("data"); 74 | 75 | if(!convert(sensor_data_params, ondn_stamped->ondn.data)) 76 | { 77 | // could parse data from parameters 78 | throw std::runtime_error("Could not load O1Dn data from parameters!"); 79 | return; 80 | } 81 | 82 | if(sensor_data_params->exists("frame")) 83 | { 84 | sensor_frame = sensor_data_params->at("frame")->data->as_string(); 85 | } 86 | 87 | ondn_stamped->header.frame_id = sensor_frame; 88 | ondn_stamped->header.stamp = nh_->now(); 89 | 90 | updateMsg(ondn_stamped); 91 | } 92 | 93 | void MICPOnDnSensorCUDA::updateMsg( 94 | const rmcl_msgs::msg::OnDnStamped::SharedPtr msg) 95 | { 96 | rm::StopWatch sw; 97 | const rclcpp::Time now_time = nh_->get_clock()->now(); 98 | const rclcpp::Time msg_time = msg->header.stamp; 99 | 100 | if(now_time.get_clock_type() != msg_time.get_clock_type()) 101 | { 102 | RCLCPP_WARN_STREAM(nh_->get_logger(), "[" << name << "::topicCB] WARNING - STAMP MISMATCH: (now - input msg's stamp) have different clock types: " << now_time.get_clock_type()); 103 | return; 104 | } 105 | 106 | // const double diff_now_msg = (now_time - msg_time).seconds(); 107 | 108 | double diff_now_msg; 109 | 110 | try { 111 | diff_now_msg = (now_time - msg_time).seconds(); 112 | } catch(const std::runtime_error& ex) { 113 | RCLCPP_WARN_STREAM(nh_->get_logger(), "[" << name << "::topicCB] WARNING - STAMP MISMATCH: (now - input msg's stamp) have different clock type"); 114 | return; 115 | } 116 | 117 | if(fabs(diff_now_msg) > 0.5) 118 | { 119 | RCLCPP_WARN_STREAM(nh_->get_logger(), "[" << name << "::topicCB] WARNING - NETWORK DELAY: (now - input msg's stamp) is more far apart (" << diff_now_msg * 1000.0 << " ms). It is likely that control algorithms will not work as expected."); 120 | } 121 | 122 | sw(); 123 | data_correction_mutex_.lock(); 124 | const double el_mutex_lock = sw(); 125 | 126 | sw(); 127 | // Part 1: transfrom sensor and filter input data 128 | dataset_stamp_ = msg->header.stamp; 129 | sensor_frame = msg->header.frame_id; 130 | fetchTF(dataset_stamp_); 131 | correspondences_->setTsb(Tsb); 132 | const double el_fetch_tf = sw(); 133 | 134 | // const double diff_odom_msg = (Tbo_stamp - dataset_stamp_).seconds(); 135 | 136 | double diff_odom_msg; 137 | try { 138 | diff_odom_msg = (Tbo_stamp - dataset_stamp_).seconds(); 139 | } catch(const std::runtime_error& ex) { 140 | RCLCPP_WARN_STREAM(nh_->get_logger(), "[" << name << "::topicCB] WARNING - STAMP MISMATCH: (now - input msg's stamp) have different clock type"); 141 | data_correction_mutex_.unlock(); 142 | return; 143 | } 144 | 145 | // copy to internal representation 146 | // fill sensor_model_ and initialize copy data to dataset 147 | sw(); 148 | unpackMessage(msg); 149 | if(auto model_setter = std::dynamic_pointer_cast< 150 | rm::ModelSetter >(correspondences_)) 151 | { 152 | // RCC required sensor model 153 | model_setter->setModel(sensor_model_); 154 | } 155 | const double el_unpack_msg = sw(); 156 | 157 | correspondences_->outdated = true; 158 | first_message_received = true; 159 | 160 | data_correction_mutex_.unlock(); 161 | 162 | { // print conversion & sync delay 163 | RCLCPP_DEBUG_STREAM(nh_->get_logger(), "[" << name << "::topicCB] MICPOnDnSensorCUDA Timings:"); 164 | RCLCPP_DEBUG_STREAM(nh_->get_logger(), "[" << name << "::topicCB] - (Now - msg stamp) = " << diff_now_msg * 1000.0 << " ms"); 165 | RCLCPP_DEBUG_STREAM(nh_->get_logger(), "[" << name << "::topicCB] - (Odom - msg stamp) = " << diff_odom_msg * 1000.0 << " ms"); 166 | RCLCPP_DEBUG_STREAM(nh_->get_logger(), "[" << name << "::topicCB] - Lock mutex: " << el_mutex_lock * 1000.0 << " ms"); 167 | RCLCPP_DEBUG_STREAM(nh_->get_logger(), "[" << name << "::topicCB] - Fetch TF: " << el_fetch_tf * 1000.0 << " ms"); 168 | RCLCPP_DEBUG_STREAM(nh_->get_logger(), "[" << name << "::topicCB] - Unpack message (" << correspondences_->dataset.points.size() << "): " << el_unpack_msg * 1000.0 << " ms"); 169 | } 170 | 171 | if(!static_dataset) 172 | { 173 | on_data_received(this); 174 | } 175 | } 176 | 177 | void MICPOnDnSensorCUDA::unpackMessage( 178 | const rmcl_msgs::msg::OnDnStamped::SharedPtr msg) 179 | { 180 | ///// 181 | // sensor model 182 | // std::cout << "INFO: " << msg->ondn.info.range_min << ", " << msg->ondn.info.range_max << std::endl; 183 | rmcl::convert(msg->ondn.info, sensor_model_); 184 | 185 | //// 186 | // data: TODOs: 187 | // - use input mask values 188 | // - use input normals 189 | size_t n_old_measurements = correspondences_->dataset.points.size(); 190 | size_t n_new_measurements = msg->ondn.data.ranges.size(); 191 | if(n_new_measurements > n_old_measurements) 192 | { 193 | // need to resize buffers 194 | correspondences_->dataset.points.resize(n_new_measurements); 195 | correspondences_->dataset.mask.resize(n_new_measurements); 196 | dataset_cpu_.points.resize(n_new_measurements); 197 | dataset_cpu_.mask.resize(n_new_measurements); 198 | 199 | for(size_t i=n_old_measurements; iondn.data.ranges[loc_id]; 216 | const rm::Vector3f real_point = sensor_model_.getDirection(vid, hid) * real_range 217 | + sensor_model_.getOrigin(vid, hid);; 218 | dataset_cpu_.points[loc_id] = real_point; 219 | 220 | if(real_range < sensor_model_.range.min || real_range > sensor_model_.range.max) 221 | { 222 | // out of range 223 | dataset_cpu_.mask[loc_id] = 0; 224 | } else { 225 | dataset_cpu_.mask[loc_id] = 1; 226 | valid_dataset_measurements++; 227 | } 228 | } 229 | } 230 | 231 | // upload 232 | correspondences_->dataset.points = dataset_cpu_.points; 233 | correspondences_->dataset.mask = dataset_cpu_.mask; 234 | 235 | dataset_stamp_ = msg->header.stamp; 236 | } 237 | 238 | } // namespace rmcl -------------------------------------------------------------------------------- /rmcl_ros/src/micpl/MICPPinholeSensorCPU.cpp: -------------------------------------------------------------------------------- 1 | #include "rmcl_ros/micpl/MICPPinholeSensorCPU.hpp" 2 | 3 | #include 4 | #include 5 | #include 6 | 7 | #include 8 | 9 | #include 10 | #include 11 | 12 | #include 13 | #include 14 | 15 | #include 16 | 17 | 18 | using namespace std::chrono_literals; 19 | 20 | namespace rm = rmagine; 21 | 22 | namespace rmcl 23 | { 24 | 25 | MICPPinholeSensorCPU::MICPPinholeSensorCPU( 26 | rclcpp::Node::SharedPtr nh) 27 | :MICPSensorCPU(nh) 28 | { 29 | Tom.setIdentity(); 30 | correspondences_.reset(); 31 | } 32 | 33 | void MICPPinholeSensorCPU::connectToTopic(const std::string& topic_name) 34 | { 35 | static_dataset = false; 36 | 37 | std::chrono::duration buffer_timeout(1); 38 | 39 | tf_filter_ = std::make_unique >( 40 | data_sub_, *tf_buffer_, odom_frame, 10, nh_->get_node_logging_interface(), 41 | nh_->get_node_clock_interface(), buffer_timeout); 42 | 43 | rclcpp::SubscriptionOptions sub_options; 44 | sub_options.callback_group = cb_group_; 45 | rclcpp::QoS qos(10); 46 | data_sub_.subscribe(nh_, topic_name, qos.get_rmw_qos_profile(), sub_options); // delete "get_rmw_..." for rolling 47 | 48 | tf_filter_->registerCallback(&MICPPinholeSensorCPU::updateMsg, this); 49 | 50 | RCLCPP_INFO_STREAM(nh_->get_logger(), "[" << name << "] [MICPPinholeSensorCPU] " << "Waiting for message from topic '" << topic_name << "'..."); 51 | } 52 | 53 | void MICPPinholeSensorCPU::getDataFromParameters() 54 | { 55 | static_dataset = true; 56 | 57 | // fill this: 58 | rmcl_msgs::msg::DepthStamped::SharedPtr depth_stamped 59 | = std::make_shared(); 60 | 61 | const ParamTree::SharedPtr sensor_param_tree 62 | = get_parameter_tree(nh_, "~"); 63 | 64 | // 1. Load Model 65 | const ParamTree::SharedPtr sensor_model_params 66 | = sensor_param_tree->at("model"); 67 | 68 | if(!convert(sensor_model_params, depth_stamped->depth.info)) 69 | { 70 | // could parse data from parameters 71 | throw std::runtime_error("Could not load OnDn model from parameters!"); 72 | return; 73 | } 74 | 75 | // 2. Load Data 76 | const ParamTree::SharedPtr sensor_data_params 77 | = sensor_param_tree->at("data"); 78 | 79 | if(!convert(sensor_data_params, depth_stamped->depth.data)) 80 | { 81 | // could parse data from parameters 82 | throw std::runtime_error("Could not load O1Dn data from parameters!"); 83 | return; 84 | } 85 | 86 | if(sensor_data_params->exists("frame")) 87 | { 88 | sensor_frame = sensor_data_params->at("frame")->data->as_string(); 89 | } 90 | 91 | depth_stamped->header.frame_id = sensor_frame; 92 | depth_stamped->header.stamp = nh_->now(); 93 | 94 | updateMsg(depth_stamped); 95 | } 96 | 97 | void MICPPinholeSensorCPU::updateMsg( 98 | const rmcl_msgs::msg::DepthStamped::SharedPtr msg) 99 | { 100 | rm::StopWatch sw; 101 | const rclcpp::Time now_time = nh_->get_clock()->now(); 102 | const rclcpp::Time msg_time = msg->header.stamp; 103 | 104 | if(now_time.get_clock_type() != msg_time.get_clock_type()) 105 | { 106 | RCLCPP_WARN_STREAM(nh_->get_logger(), "[" << name << "::topicCB] WARNING - STAMP MISMATCH: (now - input msg's stamp) have different clock types: " << now_time.get_clock_type()); 107 | return; 108 | } 109 | 110 | // const double diff_now_msg = (now_time - msg_time).seconds(); 111 | 112 | double diff_now_msg; 113 | 114 | try { 115 | diff_now_msg = (now_time - msg_time).seconds(); 116 | } catch(const std::runtime_error& ex) { 117 | RCLCPP_WARN_STREAM(nh_->get_logger(), "[" << name << "::topicCB] WARNING - STAMP MISMATCH: (now - input msg's stamp) have different clock type"); 118 | return; 119 | } 120 | 121 | if(fabs(diff_now_msg) > 0.5) 122 | { 123 | RCLCPP_WARN_STREAM(nh_->get_logger(), "[" << name << "::topicCB] WARNING - NETWORK DELAY: (now - input msg's stamp) is more far apart (" << diff_now_msg * 1000.0 << " ms). It is likely that control algorithms will not work as expected."); 124 | } 125 | 126 | sw(); 127 | data_correction_mutex_.lock(); 128 | const double el_mutex_lock = sw(); 129 | 130 | sw(); 131 | // Part 1: transfrom sensor and filter input data 132 | dataset_stamp_ = msg->header.stamp; 133 | sensor_frame = msg->header.frame_id; 134 | fetchTF(dataset_stamp_); 135 | correspondences_->setTsb(Tsb); 136 | const double el_fetch_tf = sw(); 137 | 138 | // const double diff_odom_msg = (Tbo_stamp - dataset_stamp_).seconds(); 139 | 140 | double diff_odom_msg; 141 | try { 142 | diff_odom_msg = (Tbo_stamp - dataset_stamp_).seconds(); 143 | } catch(const std::runtime_error& ex) { 144 | RCLCPP_WARN_STREAM(nh_->get_logger(), "[" << name << "::topicCB] WARNING - STAMP MISMATCH: (now - input msg's stamp) have different clock type"); 145 | data_correction_mutex_.unlock(); 146 | return; 147 | } 148 | 149 | // copy to internal representation 150 | // fill sensor_model_ and initialize copy data to dataset 151 | sw(); 152 | unpackMessage(msg); 153 | 154 | // TODO: make some kind of PinholeSetter base class that doesnt depend on Embree 155 | if(auto model_setter = std::dynamic_pointer_cast< 156 | rm::ModelSetter >(correspondences_)) 157 | { 158 | // RCC required sensor model 159 | model_setter->setModel(sensor_model_); 160 | } 161 | 162 | const double el_unpack_msg = sw(); 163 | 164 | correspondences_->outdated = true; 165 | first_message_received = true; 166 | 167 | data_correction_mutex_.unlock(); 168 | 169 | { // print stats 170 | RCLCPP_DEBUG_STREAM(nh_->get_logger(), "[" << name << "::topicCB] MICPPinholeSensorCPU Timings:"); 171 | RCLCPP_DEBUG_STREAM(nh_->get_logger(), "[" << name << "::topicCB] - (Now - msg stamp) = " << diff_now_msg * 1000.0 << " ms"); 172 | RCLCPP_DEBUG_STREAM(nh_->get_logger(), "[" << name << "::topicCB] - (Odom - msg stamp) = " << diff_odom_msg * 1000.0 << " ms"); 173 | RCLCPP_DEBUG_STREAM(nh_->get_logger(), "[" << name << "::topicCB] - Lock mutex: " << el_mutex_lock * 1000.0 << " ms"); 174 | RCLCPP_DEBUG_STREAM(nh_->get_logger(), "[" << name << "::topicCB] - Fetch TF: " << el_fetch_tf * 1000.0 << " ms"); 175 | RCLCPP_DEBUG_STREAM(nh_->get_logger(), "[" << name << "::topicCB] - Unpack message (" << correspondences_->dataset.points.size() << "): " << el_unpack_msg * 1000.0 << " ms"); 176 | } 177 | 178 | on_data_received(this); 179 | } 180 | 181 | void MICPPinholeSensorCPU::unpackMessage( 182 | const rmcl_msgs::msg::DepthStamped::SharedPtr msg) 183 | { 184 | ///// 185 | // sensor model 186 | // std::cout << "INFO: " << msg->o1dn.info.range_min << ", " << msg->o1dn.info.range_max << std::endl; 187 | rmcl::convert(msg->depth.info, sensor_model_); 188 | 189 | //// 190 | // data: TODOs: 191 | // - use input mask values 192 | // - use input normals 193 | size_t n_old_measurements = correspondences_->dataset.points.size(); 194 | size_t n_new_measurements = msg->depth.data.ranges.size(); 195 | if(n_new_measurements > n_old_measurements) 196 | { 197 | // need to resize buffers 198 | // std::cout << "Need to resize buffers: " << n_old_measurements << " -> " << n_new_measurements << std::endl; 199 | correspondences_->dataset.points.resize(n_new_measurements); 200 | correspondences_->dataset.mask.resize(n_new_measurements); 201 | for(size_t i=n_old_measurements; idataset.mask[i] = 1; 204 | } 205 | } 206 | 207 | total_dataset_measurements = n_new_measurements; 208 | valid_dataset_measurements = 0; 209 | 210 | // sw(); 211 | // fill data 212 | for(unsigned int vid = 0; vid < sensor_model_.getHeight(); vid++) 213 | { 214 | for(unsigned int hid = 0; hid < sensor_model_.getWidth(); hid++) 215 | { 216 | const unsigned int loc_id = sensor_model_.getBufferId(vid, hid); 217 | const float real_range = msg->depth.data.ranges[loc_id]; 218 | const rm::Vector3f real_point = sensor_model_.getDirection(vid, hid) * real_range; 219 | correspondences_->dataset.points[loc_id] = real_point; 220 | 221 | if(real_range < sensor_model_.range.min || real_range > sensor_model_.range.max) 222 | { 223 | // out of range 224 | correspondences_->dataset.mask[loc_id] = 0; 225 | } else { 226 | valid_dataset_measurements++; 227 | correspondences_->dataset.mask[loc_id] = 1; 228 | } 229 | } 230 | } 231 | 232 | dataset_stamp_ = msg->header.stamp; 233 | } 234 | 235 | } // namespace rmcl -------------------------------------------------------------------------------- /rmcl_ros/src/micpl/MICPSensor.cpp: -------------------------------------------------------------------------------- 1 | #include "rmcl_ros/micpl/MICPSensor.hpp" 2 | 3 | #include 4 | #include 5 | 6 | #include 7 | 8 | #include 9 | 10 | 11 | namespace rm = rmagine; 12 | 13 | namespace rmcl 14 | { 15 | 16 | MICPSensorBase::MICPSensorBase( 17 | rclcpp::Node::SharedPtr nh) 18 | :nh_(nh) 19 | { 20 | cb_group_ = nh_->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); 21 | 22 | tf_buffer_ = 23 | std::make_shared(nh_->get_clock()); 24 | 25 | auto timer_interface = std::make_shared( 26 | nh_->get_node_base_interface(), 27 | nh_->get_node_timers_interface()); 28 | tf_buffer_->setCreateTimerInterface(timer_interface); 29 | 30 | rclcpp::SubscriptionOptions sub_options; 31 | sub_options.callback_group = cb_group_; 32 | 33 | tf_listener_ = 34 | std::make_shared(*tf_buffer_, 35 | nh_, 36 | true, 37 | tf2_ros::DynamicListenerQoS(), 38 | tf2_ros::StaticListenerQoS(), 39 | sub_options 40 | ); 41 | 42 | // load name 43 | ParamTree::SharedPtr sensor_param_tree 44 | = get_parameter_tree(nh, "~"); 45 | 46 | // Get name of sensor: 'sensors.velodyne' -> 'velodyne' 47 | name = sensor_param_tree->name.substr( 48 | sensor_param_tree->name.find_last_of(".") + 1); 49 | 50 | if(sensor_param_tree->at("correspondences")->exists("visualize")) 51 | { 52 | enable_visualizations = sensor_param_tree->at("correspondences")->at("visualize")->data->as_bool(); 53 | } 54 | 55 | map_frame = nh_->get_parameter("map_frame").as_string(); 56 | odom_frame = nh_->get_parameter("odom_frame").as_string(); 57 | base_frame = nh_->get_parameter("base_frame").as_string(); 58 | 59 | while(!tf_buffer_->_frameExists(base_frame)) 60 | { 61 | RCLCPP_INFO_STREAM_ONCE(nh_->get_logger(), "Waiting for '" << base_frame << "' frame to become available ..."); 62 | nh_->get_clock()->sleep_for(std::chrono::duration(0.2)); 63 | } 64 | 65 | while(!tf_buffer_->_frameExists(odom_frame)) 66 | { 67 | RCLCPP_INFO_STREAM_ONCE(nh_->get_logger(), "Waiting for '" << odom_frame << "' frame to become available ..."); 68 | nh_->get_clock()->sleep_for(std::chrono::duration(0.2)); 69 | } 70 | 71 | correspondence_viz_pub_ = nh_->create_publisher("~/sensors/" + name + "/correspondences", 10); 72 | 73 | on_data_received = [](MICPSensorBase*){ 74 | // default: dont use 75 | }; 76 | } 77 | 78 | MICPSensorBase::~MICPSensorBase() 79 | { 80 | 81 | } 82 | 83 | void MICPSensorBase::setTom(const rm::Transform& Tom_in) 84 | { 85 | // when Tom changes, we have to find correspondences again 86 | Tom = Tom_in; 87 | } 88 | 89 | bool MICPSensorBase::fetchTF(const rclcpp::Time stamp) 90 | { 91 | // figure out current transform chain. 92 | if(base_frame == sensor_frame) 93 | { 94 | Tsb_stamp = stamp; 95 | Tsb.setIdentity(); 96 | } else { 97 | try { 98 | geometry_msgs::msg::TransformStamped T_sensor_base; 99 | T_sensor_base = tf_buffer_->lookupTransform(base_frame, sensor_frame, stamp); 100 | Tsb_stamp = T_sensor_base.header.stamp; 101 | convert(T_sensor_base.transform, Tsb); 102 | } catch (tf2::TransformException &ex) { 103 | RCLCPP_WARN(nh_->get_logger(), "%s", ex.what()); 104 | RCLCPP_WARN_STREAM(nh_->get_logger(), "Source (Sensor): " << sensor_frame << ", Target (Base): " << base_frame); 105 | return false; 106 | } 107 | } 108 | 109 | try { 110 | if(tf_buffer_->canTransform( 111 | odom_frame, base_frame, stamp, rclcpp::Duration::from_seconds(1.0))) 112 | { 113 | geometry_msgs::msg::TransformStamped T_base_odom; 114 | T_base_odom = tf_buffer_->lookupTransform(odom_frame, base_frame, stamp); 115 | Tbo_stamp = T_base_odom.header.stamp; 116 | convert(T_base_odom.transform, Tbo); 117 | } 118 | else 119 | { 120 | RCLCPP_WARN(nh_->get_logger(), "Transform not available yet."); 121 | return false; 122 | } 123 | } catch (tf2::TransformException& ex) { 124 | // std::cout << "Range sensor data is newer than odom! This not too bad. Try to get latest stamp" << std::endl; 125 | RCLCPP_WARN(nh_->get_logger(), "%s", ex.what()); 126 | RCLCPP_WARN_STREAM(nh_->get_logger(), "Source (Base): " << base_frame << ", Target (Odom): " << odom_frame); 127 | return false; 128 | } 129 | 130 | return true; 131 | } 132 | 133 | } // namespace rmcl -------------------------------------------------------------------------------- /rmcl_ros/src/micpl/MICPSensorCPU.cpp: -------------------------------------------------------------------------------- 1 | #include "rmcl_ros/micpl/MICPSensorCPU.hpp" 2 | 3 | namespace rm = rmagine; 4 | 5 | namespace rmcl 6 | { 7 | 8 | MICPSensorCPU::MICPSensorCPU(rclcpp::Node::SharedPtr nh) 9 | : Base(nh) 10 | { 11 | 12 | } 13 | 14 | void MICPSensorCPU::drawCorrespondences() 15 | { 16 | if(!enable_visualizations) 17 | { 18 | return; 19 | } 20 | 21 | visualization_msgs::msg::Marker marker; 22 | marker.header.frame_id = sensor_frame; 23 | 24 | if(static_dataset) 25 | { 26 | marker.header.stamp = Tbo_stamp; 27 | } else { 28 | marker.header.stamp = dataset_stamp_; 29 | } 30 | 31 | marker.action = visualization_msgs::msg::Marker::ADD; 32 | marker.type = visualization_msgs::msg::Marker::LINE_LIST; 33 | 34 | marker.color.r = 1.0; 35 | marker.color.g = 0.0; 36 | marker.color.b = 0.0; 37 | marker.color.a = 1.0; 38 | 39 | std_msgs::msg::ColorRGBA color_start; 40 | color_start.r = 0.0; 41 | color_start.g = 0.0; 42 | color_start.b = 0.0; 43 | color_start.a = 0.8; 44 | 45 | std_msgs::msg::ColorRGBA color_end; 46 | color_end.r = 1.0; 47 | color_end.g = 1.0; 48 | color_end.b = 1.0; 49 | color_end.a = 0.8; 50 | 51 | marker.pose.position.x = 0.0; 52 | marker.pose.position.y = 0.0; 53 | marker.pose.position.z = 0.0; 54 | marker.pose.orientation.x = 0.0; 55 | marker.pose.orientation.y = 0.0; 56 | marker.pose.orientation.z = 0.0; 57 | marker.pose.orientation.w = 1.0; 58 | 59 | marker.scale.x = 0.02; 60 | marker.scale.y = 0.02; 61 | marker.scale.z = 0.02; 62 | 63 | // const auto dataset = corre 64 | const auto model = correspondences_->modelView(); 65 | const auto dataset = correspondences_->datasetView(); 66 | 67 | const rm::UmeyamaReductionConstraints corr_params 68 | = correspondences_->params; 69 | 70 | // P2L 71 | for(size_t i=0; i 0 && dataset.mask[i] > 0) 74 | { 75 | const rm::Vector Di = dataset.points[i]; // read 76 | const rm::Vector Ii = model.points[i]; // read 77 | const rm::Vector Ni = model.normals[i]; 78 | 79 | const float signed_plane_dist = (Ii - Di).dot(Ni); 80 | 81 | if(fabs(signed_plane_dist) < corr_params.max_dist) 82 | { 83 | const rm::Vector Mi = Di + Ni * signed_plane_dist; 84 | geometry_msgs::msg::Point p1, p2; 85 | 86 | p1.x = Di.x; 87 | p1.y = Di.y; 88 | p1.z = Di.z; 89 | p2.x = Mi.x; 90 | p2.y = Mi.y; 91 | p2.z = Mi.z; 92 | 93 | marker.points.push_back(p1); 94 | marker.points.push_back(p2); 95 | 96 | marker.colors.push_back(color_start); 97 | marker.colors.push_back(color_end); 98 | } 99 | } 100 | } 101 | 102 | correspondence_viz_pub_->publish(marker); 103 | } 104 | 105 | } // namespace rmcl -------------------------------------------------------------------------------- /rmcl_ros/src/micpl/MICPSensorCUDA.cpp: -------------------------------------------------------------------------------- 1 | #include "rmcl_ros/micpl/MICPSensorCUDA.hpp" 2 | 3 | 4 | namespace rm = rmagine; 5 | 6 | namespace rmcl 7 | { 8 | 9 | MICPSensorCUDA::MICPSensorCUDA(rclcpp::Node::SharedPtr nh) 10 | : Base(nh) 11 | { 12 | 13 | } 14 | 15 | void MICPSensorCUDA::drawCorrespondences() 16 | { 17 | if(!enable_visualizations) 18 | { 19 | return; 20 | } 21 | 22 | visualization_msgs::msg::Marker marker; 23 | marker.header.frame_id = sensor_frame; 24 | 25 | if(static_dataset) 26 | { 27 | marker.header.stamp = Tbo_stamp; 28 | } else { 29 | marker.header.stamp = dataset_stamp_; 30 | } 31 | 32 | marker.action = visualization_msgs::msg::Marker::ADD; 33 | marker.type = visualization_msgs::msg::Marker::LINE_LIST; 34 | 35 | marker.color.r = 1.0; 36 | marker.color.g = 0.0; 37 | marker.color.b = 0.0; 38 | marker.color.a = 1.0; 39 | 40 | std_msgs::msg::ColorRGBA color_start; 41 | color_start.r = 0.0; 42 | color_start.g = 0.0; 43 | color_start.b = 0.0; 44 | color_start.a = 0.8; 45 | 46 | std_msgs::msg::ColorRGBA color_end; 47 | color_end.r = 1.0; 48 | color_end.g = 1.0; 49 | color_end.b = 1.0; 50 | color_end.a = 0.8; 51 | 52 | marker.pose.position.x = 0.0; 53 | marker.pose.position.y = 0.0; 54 | marker.pose.position.z = 0.0; 55 | marker.pose.orientation.x = 0.0; 56 | marker.pose.orientation.y = 0.0; 57 | marker.pose.orientation.z = 0.0; 58 | marker.pose.orientation.w = 1.0; 59 | 60 | marker.scale.x = 0.02; 61 | marker.scale.y = 0.02; 62 | marker.scale.z = 0.02; 63 | 64 | // Download model 65 | // This is the reason why you should disable visualizations 66 | const auto model_cpu = rm::transfer(correspondences_->modelView()); 67 | 68 | const rm::UmeyamaReductionConstraints corr_params 69 | = correspondences_->params; 70 | 71 | // P2L 72 | for(size_t i=0; i 0 && dataset_cpu_.mask[i] > 0) 75 | { 76 | const rm::Vector Di = dataset_cpu_.points[i]; // read 77 | const rm::Vector Ii = model_cpu.points[i]; // read 78 | const rm::Vector Ni = model_cpu.normals[i]; 79 | 80 | const float signed_plane_dist = (Ii - Di).dot(Ni); 81 | 82 | if(fabs(signed_plane_dist) < corr_params.max_dist) 83 | { 84 | const rm::Vector Mi = Di + Ni * signed_plane_dist; 85 | geometry_msgs::msg::Point p1, p2; 86 | 87 | p1.x = Di.x; 88 | p1.y = Di.y; 89 | p1.z = Di.z; 90 | p2.x = Mi.x; 91 | p2.y = Mi.y; 92 | p2.z = Mi.z; 93 | 94 | marker.points.push_back(p1); 95 | marker.points.push_back(p2); 96 | 97 | marker.colors.push_back(color_start); 98 | marker.colors.push_back(color_end); 99 | } 100 | } 101 | } 102 | 103 | correspondence_viz_pub_->publish(marker); 104 | } 105 | 106 | } // namespace rmcl -------------------------------------------------------------------------------- /rmcl_ros/src/nodes/conversion/pc2_to_o1dn.cpp: -------------------------------------------------------------------------------- 1 | #include "rmcl_ros/nodes/conversion/pc2_to_o1dn.hpp" 2 | 3 | #include 4 | #include 5 | #include 6 | 7 | #include 8 | #include 9 | 10 | 11 | namespace rm = rmagine; 12 | 13 | namespace rmcl 14 | { 15 | 16 | Pc2ToO1DnNode::Pc2ToO1DnNode( 17 | const rclcpp::NodeOptions& options) 18 | :rclcpp::Node("pc2_to_o1dn_node", options) 19 | { 20 | declareParameters(); 21 | fetchParameters(); 22 | 23 | pub_scan_ = this->create_publisher( 24 | "output", 10); 25 | 26 | if(debug_cloud_) 27 | { 28 | pub_debug_cloud_ = this->create_publisher( 29 | "~/debug_cloud", 10); 30 | } 31 | 32 | tf_buffer_ = 33 | std::make_unique(this->get_clock()); 34 | tf_listener_ = 35 | std::make_shared(*tf_buffer_); 36 | 37 | sub_pcl_ = this->create_subscription( 38 | "input", 10, 39 | [=](const sensor_msgs::msg::PointCloud2::ConstSharedPtr& msg) -> void 40 | { 41 | cloudCB(msg); 42 | }); 43 | 44 | callback_handle_ = this->add_on_set_parameters_callback( 45 | std::bind(&Pc2ToO1DnNode::parametersCallback, this, std::placeholders::_1)); 46 | } 47 | 48 | void Pc2ToO1DnNode::declareParameters() 49 | { 50 | declare_parameter("sensor_frame", ""); 51 | declare_parameter("model.range_min", 0.3); 52 | declare_parameter("model.range_max", 50.0); 53 | declare_parameter("debug_cloud", false); 54 | declare_parameter("height.increment", 1); 55 | declare_parameter("height.skip_begin", 0); 56 | declare_parameter("height.skip_end", 0); 57 | declare_parameter("width.increment", 1); 58 | declare_parameter("width.skip_begin", 0); 59 | declare_parameter("width.skip_end", 0); 60 | } 61 | 62 | void Pc2ToO1DnNode::fetchParameters() 63 | { 64 | sensor_frame_ = get_parameter("sensor_frame").as_string(); 65 | scan_.o1dn.info.range_min = get_parameter("model.range_min").as_double(); 66 | scan_.o1dn.info.range_max = get_parameter("model.range_max").as_double(); 67 | debug_cloud_ = get_parameter("debug_cloud").as_bool(); 68 | filter_options_.range_min = scan_.o1dn.info.range_min; 69 | filter_options_.range_max = scan_.o1dn.info.range_max; 70 | 71 | filter_options_.height.increment = get_parameter("height.increment").as_int(); 72 | filter_options_.height.skip_begin = get_parameter("height.skip_begin").as_int(); 73 | filter_options_.height.skip_end = get_parameter("height.skip_end").as_int(); 74 | filter_options_.width.increment = get_parameter("width.increment").as_int(); 75 | filter_options_.width.skip_begin = get_parameter("width.skip_begin").as_int(); 76 | filter_options_.width.skip_end = get_parameter("width.skip_end").as_int(); 77 | } 78 | 79 | rcl_interfaces::msg::SetParametersResult Pc2ToO1DnNode::parametersCallback( 80 | const std::vector ¶meters) 81 | { 82 | rcl_interfaces::msg::SetParametersResult result; 83 | result.successful = true; 84 | result.reason = "success"; 85 | 86 | for(const auto& param: parameters) 87 | { 88 | if(param.get_name() == "height.increment") 89 | { 90 | filter_options_.height.increment = param.as_int(); 91 | } 92 | else if(param.get_name() == "height.skip_begin") 93 | { 94 | filter_options_.height.skip_begin = param.as_int(); 95 | } 96 | else if(param.get_name() == "height.skip_end") 97 | { 98 | filter_options_.height.skip_end = param.as_int(); 99 | } 100 | else if(param.get_name() == "width.increment") 101 | { 102 | filter_options_.width.increment = param.as_int(); 103 | } 104 | else if(param.get_name() == "width.skip_begin") 105 | { 106 | filter_options_.width.skip_begin = param.as_int(); 107 | } 108 | else if(param.get_name() == "width.skip_end") 109 | { 110 | filter_options_.width.skip_end = param.as_int(); 111 | } 112 | else if(param.get_name() == "debug_cloud") 113 | { 114 | debug_cloud_ = param.as_bool(); 115 | if(debug_cloud_ && !pub_debug_cloud_) 116 | { 117 | pub_debug_cloud_ = this->create_publisher( 118 | "debug_cloud", 10); 119 | } 120 | else if(!debug_cloud_ && pub_debug_cloud_) 121 | { 122 | pub_debug_cloud_.reset(); 123 | } 124 | } 125 | else if(param.get_name() == "model.range_min") 126 | { 127 | scan_.o1dn.info.range_min = param.as_double(); 128 | filter_options_.range_min = scan_.o1dn.info.range_min; 129 | } 130 | else if(param.get_name() == "model.range_max") 131 | { 132 | scan_.o1dn.info.range_max = param.as_double(); 133 | filter_options_.range_max = scan_.o1dn.info.range_max; 134 | } 135 | } 136 | 137 | // Here update class attributes, do some actions, etc. 138 | return result; 139 | } 140 | 141 | 142 | bool Pc2ToO1DnNode::convert( 143 | const sensor_msgs::msg::PointCloud2::ConstSharedPtr& pcd, 144 | rmcl_msgs::msg::O1DnStamped& scan) const 145 | { 146 | rmcl_msgs::msg::O1DnStamped scan_unfiltered; 147 | 148 | scan_unfiltered.o1dn.info.range_min = scan.o1dn.info.range_min; 149 | scan_unfiltered.o1dn.info.range_max = scan.o1dn.info.range_max; 150 | 151 | estimateModelAndData(scan_unfiltered.header, scan_unfiltered.o1dn.info, scan_unfiltered.o1dn.data, *pcd); 152 | 153 | filter(scan.o1dn, scan_unfiltered.o1dn, filter_options_); 154 | 155 | return true; 156 | } 157 | 158 | void Pc2ToO1DnNode::cloudCB(const sensor_msgs::msg::PointCloud2::ConstSharedPtr& msg) 159 | { 160 | const rclcpp::Time msg_time = msg->header.stamp; 161 | const rclcpp::Time ros_now = this->get_clock()->now(); 162 | 163 | const double diff_now_msg = (ros_now - msg_time).seconds(); 164 | if(fabs(diff_now_msg) > 0.5) 165 | { 166 | RCLCPP_WARN_STREAM(this->get_logger(), "[Pc2ToO1DnNode::cloudCB] WARNING - NETWORK DELAY: (now - input msg's stamp) is far apart (" << diff_now_msg * 1000.0 << " ms)."); 167 | } 168 | 169 | rm::StopWatch sw; 170 | sw(); 171 | 172 | if(sensor_frame_ == "") 173 | { 174 | sensor_frame_ = msg->header.frame_id; 175 | } 176 | 177 | scan_.header.stamp = msg->header.stamp; 178 | scan_.header.frame_id = sensor_frame_; 179 | if(!convert(msg, scan_)) 180 | { 181 | return; 182 | } 183 | 184 | const double el = sw(); 185 | // const rclcpp::Time phsical_time_2 = this->get_clock()->now(); 186 | // const double diff_now_msg_2 = (phsical_time_2 - phsical_time_1).seconds(); 187 | if(fabs(el) > 0.5) 188 | { 189 | RCLCPP_WARN_STREAM(this->get_logger(), "[Pc2ToO1DnNode::cloudCB] WARNING: Conversion takes too long (" << el * 1000.0 << " ms)."); 190 | } 191 | 192 | // this is send directly to RMCL sensors 193 | pub_scan_->publish(scan_); 194 | 195 | if(debug_cloud_) 196 | { 197 | if(!pub_debug_cloud_) 198 | { 199 | // SHOULD NEVER HAPPEN 200 | RCLCPP_ERROR(get_logger(), "ERROR: debug cloud is true but publisher does not exist!"); 201 | } 202 | sensor_msgs::msg::PointCloud2 cloud; 203 | rmcl::convert(cloud, scan_.header, scan_.o1dn.info, scan_.o1dn.data); 204 | pub_debug_cloud_->publish(cloud); 205 | } 206 | } 207 | 208 | } // namespace rmcl 209 | 210 | #include "rclcpp_components/register_node_macro.hpp" 211 | RCLCPP_COMPONENTS_REGISTER_NODE(rmcl::Pc2ToO1DnNode) 212 | -------------------------------------------------------------------------------- /rmcl_ros/src/nodes/conversion/pc2_to_scan.cpp: -------------------------------------------------------------------------------- 1 | #include "rmcl_ros/nodes/conversion/pc2_to_scan.hpp" 2 | 3 | 4 | 5 | namespace rmcl 6 | { 7 | 8 | Pc2ToScanNode::Pc2ToScanNode( 9 | const rclcpp::NodeOptions& options) 10 | :rclcpp::Node("pc2_to_scan_node", rclcpp::NodeOptions(options) 11 | .allow_undeclared_parameters(true) 12 | .automatically_declare_parameters_from_overrides(true)) 13 | { 14 | fetchParameters(); 15 | 16 | pub_scan_ = this->create_publisher( 17 | "output", 10); 18 | 19 | if(debug_cloud_) 20 | { 21 | pub_debug_cloud_ = this->create_publisher( 22 | "~/debug_cloud", 10); 23 | } 24 | 25 | tf_buffer_ = 26 | std::make_unique(this->get_clock()); 27 | tf_listener_ = 28 | std::make_shared(*tf_buffer_); 29 | 30 | sub_pcd_ = this->create_subscription( 31 | "input", 10, 32 | [=](const sensor_msgs::msg::PointCloud2::ConstSharedPtr& msg) -> void 33 | { 34 | cloudCB(msg); 35 | }); 36 | } 37 | 38 | void Pc2ToScanNode::fetchParameters() 39 | { 40 | if(!this->get_parameter("sensor_frame", sensor_frame_)) 41 | { 42 | sensor_frame_ = ""; 43 | } 44 | 45 | rmcl_msgs::msg::ScanInfo &scanner_model = scan_.scan.info; 46 | 47 | if (!this->get_parameter("model.phi_min", scanner_model.phi_min)) 48 | { 49 | RCLCPP_ERROR_STREAM(this->get_logger(), "When specifying auto_detect_phi to false you have to provide model.phi_min"); 50 | return; 51 | } 52 | if (!this->get_parameter("model.phi_inc", scanner_model.phi_inc)) 53 | { 54 | RCLCPP_ERROR_STREAM(this->get_logger(), "When specifying auto_detect_phi to false you have to provide model.phi_max"); 55 | return; 56 | } 57 | 58 | if (!this->get_parameter("model.theta_min", scanner_model.theta_min)) 59 | { 60 | RCLCPP_ERROR_STREAM(this->get_logger(), "When specifying auto_detect_phi to false you have to provide model.phi_min"); 61 | return; 62 | } 63 | if (!this->get_parameter("model.theta_inc", scanner_model.theta_inc)) 64 | { 65 | RCLCPP_ERROR_STREAM(this->get_logger(), "When specifying auto_detect_phi to false you have to provide model.phi_max"); 66 | return; 67 | } 68 | 69 | if (!this->get_parameter("model.range_min", scanner_model.range_min)) 70 | { 71 | RCLCPP_ERROR_STREAM(this->get_logger(), "When specifying auto_detect_phi to false you have to provide model.phi_min"); 72 | return; 73 | } 74 | if (!this->get_parameter("model.range_max", scanner_model.range_max)) 75 | { 76 | RCLCPP_ERROR_STREAM(this->get_logger(), "When specifying auto_detect_phi to false you have to provide model.phi_max"); 77 | return; 78 | } 79 | 80 | int phi_n_tmp, theta_n_tmp; 81 | if (!this->get_parameter("model.phi_n", phi_n_tmp)) 82 | { 83 | RCLCPP_ERROR_STREAM(this->get_logger(), "When specifying auto_detect_phi to false you have to provide model/phi_min"); 84 | return; 85 | } 86 | if (!this->get_parameter("model.theta_n", theta_n_tmp)) 87 | { 88 | RCLCPP_ERROR_STREAM(this->get_logger(), "When specifying auto_detect_phi to false you have to provide model/phi_max"); 89 | return; 90 | } 91 | scanner_model.phi_n = phi_n_tmp; 92 | scanner_model.theta_n = theta_n_tmp; 93 | 94 | if(!this->get_parameter("debug_cloud", debug_cloud_)) 95 | { 96 | debug_cloud_ = false; 97 | } 98 | } 99 | 100 | void Pc2ToScanNode::initScanArray() 101 | { 102 | fillEmpty(scan_.scan); 103 | } 104 | 105 | bool Pc2ToScanNode::convert( 106 | const sensor_msgs::msg::PointCloud2::ConstSharedPtr& pcd, 107 | rmcl_msgs::msg::ScanStamped& scan) const 108 | { 109 | rm::Transform T = rm::Transform::Identity(); 110 | 111 | if(pcd->header.frame_id != sensor_frame_) 112 | { 113 | // TODO: get transform 114 | geometry_msgs::msg::TransformStamped Tros; 115 | 116 | try 117 | { 118 | Tros = tf_buffer_->lookupTransform(sensor_frame_, pcd->header.frame_id, 119 | pcd->header.stamp); 120 | T.t.x = Tros.transform.translation.x; 121 | T.t.y = Tros.transform.translation.y; 122 | T.t.z = Tros.transform.translation.z; 123 | T.R.x = Tros.transform.rotation.x; 124 | T.R.y = Tros.transform.rotation.y; 125 | T.R.z = Tros.transform.rotation.z; 126 | T.R.w = Tros.transform.rotation.w; 127 | } 128 | catch (tf2::TransformException &ex) 129 | { 130 | RCLCPP_WARN(this->get_logger(), "%s", ex.what()); 131 | return false; 132 | } 133 | } 134 | 135 | fillEmpty(scan.scan); 136 | 137 | sensor_msgs::msg::PointField field_x; 138 | sensor_msgs::msg::PointField field_y; 139 | sensor_msgs::msg::PointField field_z; 140 | 141 | for (size_t i = 0; i < pcd->fields.size(); i++) 142 | { 143 | if (pcd->fields[i].name == "x") 144 | { 145 | field_x = pcd->fields[i]; 146 | } 147 | if (pcd->fields[i].name == "y") 148 | { 149 | field_y = pcd->fields[i]; 150 | } 151 | if (pcd->fields[i].name == "z") 152 | { 153 | field_z = pcd->fields[i]; 154 | } 155 | } 156 | 157 | rm::SphericalModel model; 158 | rmcl::convert(scan.scan.info, model); 159 | 160 | for (size_t i = 0; i < pcd->width * pcd->height; i++) 161 | { 162 | const uint8_t *data_ptr = &pcd->data[i * pcd->point_step]; 163 | 164 | // rmagine::Vector point; 165 | float x, y, z; 166 | 167 | if (field_x.datatype == sensor_msgs::msg::PointField::FLOAT32) 168 | { 169 | // Float 170 | x = *reinterpret_cast(data_ptr + field_x.offset); 171 | y = *reinterpret_cast(data_ptr + field_y.offset); 172 | z = *reinterpret_cast(data_ptr + field_z.offset); 173 | } 174 | else if (field_x.datatype == sensor_msgs::msg::PointField::FLOAT64) 175 | { 176 | // Double 177 | x = *reinterpret_cast(data_ptr + field_x.offset); 178 | y = *reinterpret_cast(data_ptr + field_y.offset); 179 | z = *reinterpret_cast(data_ptr + field_z.offset); 180 | } 181 | else 182 | { 183 | throw std::runtime_error("Field X has unknown DataType. Check Topic of pcd"); 184 | } 185 | 186 | if(std::isfinite(x) && std::isfinite(y) && std::isfinite(z)) 187 | { 188 | rm::Vector ps_s = rm::Vector{x, y, z}; 189 | rm::Vector ps = T * ps_s; 190 | 191 | float range_est = ps.l2norm(); 192 | float theta_est = atan2(ps.y, ps.x); // horizontal 193 | float phi_est = atan2(ps.z, range_est); // vertical 194 | 195 | int phi_id = ((phi_est - model.phi.min) / model.phi.inc) + 0.5; 196 | int theta_id = ((theta_est - model.theta.min) / model.theta.inc) + 0.5; 197 | 198 | if(phi_id >= 0 && phi_id < (int)model.phi.size 199 | && theta_id >= 0 && theta_id < (int)model.theta.size) 200 | { 201 | if(model.range.inside(range_est)) 202 | { 203 | unsigned int p_id = model.getBufferId(phi_id, theta_id); 204 | scan.scan.data.ranges[p_id] = range_est; 205 | } 206 | } else { 207 | // std::cout << "- out scanner matrix" << std::endl; 208 | } 209 | } 210 | } 211 | 212 | return true; 213 | } 214 | 215 | void Pc2ToScanNode::cloudCB( 216 | const sensor_msgs::msg::PointCloud2::ConstSharedPtr& msg) 217 | { 218 | if (sensor_frame_ == "") 219 | { 220 | sensor_frame_ = msg->header.frame_id; 221 | } 222 | 223 | scan_.header.stamp = msg->header.stamp; 224 | scan_.header.frame_id = sensor_frame_; 225 | if(!convert(msg, scan_)) 226 | { 227 | return; 228 | } 229 | 230 | pub_scan_->publish(scan_); 231 | 232 | if (debug_cloud_) 233 | { 234 | sensor_msgs::msg::PointCloud2 cloud; 235 | rmcl::convert(cloud, scan_.header, scan_.scan.info, scan_.scan.data); 236 | pub_debug_cloud_->publish(cloud); 237 | } 238 | } 239 | 240 | } // namespace rmcl 241 | 242 | 243 | #include "rclcpp_components/register_node_macro.hpp" 244 | RCLCPP_COMPONENTS_REGISTER_NODE(rmcl::Pc2ToScanNode) 245 | -------------------------------------------------------------------------------- /rmcl_ros/src/nodes/conversion/scan_to_scan.cpp: -------------------------------------------------------------------------------- 1 | #include "rmcl_ros/nodes/conversion/scan_to_scan.hpp" 2 | namespace rmcl 3 | { 4 | 5 | ScanToScanNode::ScanToScanNode( 6 | const rclcpp::NodeOptions& options) 7 | :rclcpp::Node("scan_to_scan_node", options) 8 | { 9 | declareParameters(); 10 | fetchParameters(); 11 | 12 | pub_scan_ = this->create_publisher( 13 | "output", 10); 14 | 15 | pub_debug_cloud_ = this->create_publisher( 16 | "~/debug_cloud", 10); 17 | 18 | sub_scan_ = this->create_subscription( 19 | "input", 10, 20 | [=](const sensor_msgs::msg::LaserScan::ConstSharedPtr& msg) -> void 21 | { 22 | scanCB(msg); 23 | }); 24 | 25 | callback_handle_ = this->add_on_set_parameters_callback( 26 | std::bind(&ScanToScanNode::parametersCallback, this, std::placeholders::_1)); 27 | } 28 | 29 | void ScanToScanNode::declareParameters() 30 | { 31 | declare_parameter("skip_begin", 0); 32 | declare_parameter("skip_end", 0); 33 | declare_parameter("increment", 1); 34 | declare_parameter("debug_cloud", false); 35 | } 36 | 37 | void ScanToScanNode::fetchParameters() 38 | { 39 | skip_begin_ = get_parameter("skip_begin").as_int(); 40 | skip_end_ = get_parameter("skip_end").as_int(); 41 | increment_ = get_parameter("increment").as_int(); 42 | debug_cloud_ = get_parameter("debug_cloud").as_bool(); 43 | } 44 | 45 | rcl_interfaces::msg::SetParametersResult ScanToScanNode::parametersCallback( 46 | const std::vector ¶meters) 47 | { 48 | rcl_interfaces::msg::SetParametersResult result; 49 | result.successful = true; 50 | result.reason = "success"; 51 | 52 | for(const auto& param: parameters) 53 | { 54 | if(param.get_name() == "increment") 55 | { 56 | increment_ = param.as_int(); 57 | } 58 | else if(param.get_name() == "skip_begin") 59 | { 60 | skip_begin_ = param.as_int(); 61 | } 62 | else if(param.get_name() == "skip_end") 63 | { 64 | skip_end_ = param.as_int(); 65 | } 66 | else if(param.get_name() == "debug_cloud") 67 | { 68 | debug_cloud_ = param.as_bool(); 69 | } 70 | } 71 | 72 | // Here update class attributes, do some actions, etc. 73 | return result; 74 | } 75 | 76 | void ScanToScanNode::initScanArray() 77 | { 78 | fillEmpty(scan_.scan); 79 | } 80 | 81 | bool ScanToScanNode::convert( 82 | const sensor_msgs::msg::LaserScan::ConstSharedPtr& scan_in, 83 | rmcl_msgs::msg::ScanStamped& scan) const 84 | { 85 | scan.header.frame_id = scan_in->header.frame_id; 86 | scan.header.stamp = scan_in->header.stamp; 87 | 88 | scan.scan.info.theta_min = scan_in->angle_min + skip_begin_ * scan_in->angle_increment; 89 | scan.scan.info.theta_inc = scan_in->angle_increment * increment_; 90 | scan.scan.info.theta_n = (scan_in->ranges.size() - skip_begin_ - skip_end_) / increment_; 91 | 92 | scan.scan.info.phi_min = 0.0; 93 | scan.scan.info.phi_inc = 0.0; 94 | scan.scan.info.phi_n = 1; 95 | scan.scan.info.range_max = scan_in->range_max; 96 | scan.scan.info.range_min = scan_in->range_min; 97 | 98 | scan.scan.data.ranges.resize(scan.scan.info.theta_n); 99 | 100 | for(size_t tgt_i=0; tgt_iranges[src_i]; 104 | } 105 | 106 | return true; 107 | } 108 | 109 | void ScanToScanNode::scanCB(const sensor_msgs::msg::LaserScan::ConstSharedPtr& msg) 110 | { 111 | scan_.header.stamp = msg->header.stamp; 112 | scan_.header.frame_id = msg->header.frame_id; 113 | if(!convert(msg, scan_)) 114 | { 115 | return; 116 | } 117 | pub_scan_->publish(scan_); 118 | 119 | if (debug_cloud_) 120 | { 121 | sensor_msgs::msg::PointCloud2 cloud; 122 | rmcl::convert(cloud, scan_.header, scan_.scan.info, scan_.scan.data); 123 | cloud.header.stamp = msg->header.stamp; 124 | pub_debug_cloud_->publish(cloud); 125 | } 126 | } 127 | 128 | } // namespace rmcl 129 | 130 | 131 | #include "rclcpp_components/register_node_macro.hpp" 132 | RCLCPP_COMPONENTS_REGISTER_NODE(rmcl::ScanToScanNode) 133 | -------------------------------------------------------------------------------- /rmcl_ros/src/nodes/filter/map_segmentation.cpp: -------------------------------------------------------------------------------- 1 | #include "rmcl_ros/nodes/filter/map_segmentation.hpp" 2 | 3 | namespace rmcl 4 | { 5 | 6 | MapSegmentationNode::MapSegmentationNode( 7 | std::string node_name, 8 | const rclcpp::NodeOptions& options) 9 | : rclcpp::Node(node_name, options) 10 | { 11 | declare_parameter("map_file", ""); 12 | declare_parameter("map_frame", "map"); 13 | 14 | map_file_ = get_parameter("map_file").as_string(); 15 | if(map_file_ == "") 16 | { 17 | RCLCPP_ERROR_STREAM(this->get_logger(), "'map_file' is required!"); 18 | throw std::runtime_error("O1DnMapSegmentationEmbreeNode - 'map_file' is required"); 19 | } 20 | 21 | map_frame_ = get_parameter("map_frame").as_string(); 22 | 23 | { 24 | rcl_interfaces::msg::ParameterDescriptor descriptor; 25 | descriptor.name = "min_dist_outlier_scan"; 26 | descriptor.floating_point_range.push_back( 27 | rcl_interfaces::msg::FloatingPointRange() 28 | .set__from_value(0.0) 29 | .set__to_value(1.0)); 30 | descriptor.description = "Minimum distance for point to be considered as outlier from scan"; 31 | min_dist_outlier_scan_ = this->declare_parameter(descriptor.name, 0.15, descriptor); 32 | } 33 | { 34 | rcl_interfaces::msg::ParameterDescriptor descriptor; 35 | descriptor.name = "min_dist_outlier_map"; 36 | descriptor.floating_point_range.push_back( 37 | rcl_interfaces::msg::FloatingPointRange() 38 | .set__from_value(0.0) 39 | .set__to_value(1.0)); 40 | descriptor.description = "Minimum distance for point to be considered as outlier from map"; 41 | min_dist_outlier_map_ = this->declare_parameter(descriptor.name, 0.15, descriptor); 42 | } 43 | 44 | dyn_params_handler_ = this->add_on_set_parameters_callback( 45 | std::bind(&MapSegmentationNode::reconfigureCallback, this, std::placeholders::_1) 46 | ); 47 | 48 | tf_buffer_ = 49 | std::make_unique(this->get_clock()); 50 | tf_listener_ = 51 | std::make_shared(*tf_buffer_); 52 | 53 | pub_outlier_scan_ = this->create_publisher( 54 | "outlier_scan", 10); 55 | pub_outlier_map_ = this->create_publisher( 56 | "outlier_map", 10); 57 | } 58 | 59 | rcl_interfaces::msg::SetParametersResult MapSegmentationNode::reconfigureCallback( 60 | const std::vector& params) 61 | { 62 | rcl_interfaces::msg::SetParametersResult result; 63 | result.successful = true; 64 | 65 | for (const auto& param: params) 66 | { 67 | if ("min_dist_outlier_map" == param.get_name()) 68 | { 69 | min_dist_outlier_map_ = param.as_double(); 70 | } 71 | else if ("min_dist_outlier_scan" == param.get_name()) 72 | { 73 | min_dist_outlier_scan_ = param.as_double(); 74 | } 75 | } 76 | 77 | return result; 78 | } 79 | 80 | } // namespace rmcl 81 | -------------------------------------------------------------------------------- /rmcl_ros/src/nodes/filter/o1dn_map_segmentation_embree.cpp: -------------------------------------------------------------------------------- 1 | #include "rmcl_ros/nodes/filter/o1dn_map_segmentation_embree.hpp" 2 | 3 | // Rmagine deps 4 | #include 5 | #include 6 | #include 7 | #include 8 | // Rmagine utils 9 | #include 10 | #include 11 | 12 | // RMCL code 13 | #include 14 | #include 15 | #include 16 | 17 | #include 18 | 19 | #include 20 | #include 21 | #include 22 | 23 | namespace rm = rmagine; 24 | 25 | namespace rmcl 26 | { 27 | 28 | O1DnMapSegmentationEmbreeNode::O1DnMapSegmentationEmbreeNode( 29 | const rclcpp::NodeOptions& options) 30 | :MapSegmentationNode("o1dn_map_segmentation_embree_node", options) 31 | { 32 | std::cout << "O1DnMapSegmentationEmbreeNode started" << std::endl; 33 | 34 | rm::EmbreeMapPtr map = rm::import_embree_map(map_file_); 35 | scan_sim_ = std::make_shared(map); 36 | scan_sim_->setTsb(rm::Transform::Identity()); 37 | 38 | sub_scan_ = this->create_subscription( 39 | "scan", 10, 40 | [=](const rmcl_msgs::msg::O1DnStamped::ConstSharedPtr& msg) -> void 41 | { 42 | scanCB(msg); 43 | }); 44 | } 45 | 46 | void O1DnMapSegmentationEmbreeNode::scanCB(const rmcl_msgs::msg::O1DnStamped::ConstSharedPtr& msg) const 47 | { 48 | geometry_msgs::msg::TransformStamped T_sensor_map; 49 | 50 | try{ 51 | T_sensor_map = tf_buffer_->lookupTransform( 52 | map_frame_, 53 | msg->header.frame_id, 54 | msg->header.stamp, 55 | rclcpp::Duration(1, 0) 56 | ); 57 | } 58 | catch (tf2::TransformException &ex) { 59 | rclcpp::Clock clock = *get_clock(); 60 | RCLCPP_WARN_THROTTLE( 61 | get_logger(), clock, 5000, 62 | "Could not transform from '%s' to '%s': %s", 63 | msg->header.frame_id.c_str(), map_frame_.c_str(), ex.what() 64 | ); 65 | return; 66 | } 67 | 68 | rm::Memory T(1); 69 | // T[0].t.x = T_sensor_map.transform.translation.x; 70 | convert(T_sensor_map.transform, T[0]); 71 | 72 | // Memory T_ = T; 73 | 74 | rm::O1DnModel model; 75 | convert(msg->o1dn.info, model); 76 | scan_sim_->setModel(model); 77 | 78 | using ResultT = rm::Bundle< 79 | rm::Ranges, 80 | rm::Normals 81 | >; 82 | 83 | ResultT res = scan_sim_->simulate(T); 84 | 85 | rm::MemoryView ranges = res.ranges; 86 | rm::MemoryView normals = res.normals; 87 | 88 | sensor_msgs::msg::PointCloud cloud_outlier_scan; 89 | cloud_outlier_scan.header.stamp = msg->header.stamp; 90 | cloud_outlier_scan.header.frame_id = msg->header.frame_id; 91 | 92 | sensor_msgs::msg::PointCloud cloud_outlier_map; 93 | cloud_outlier_map.header.stamp = msg->header.stamp; 94 | cloud_outlier_map.header.frame_id = msg->header.frame_id; 95 | 96 | 97 | // if this doesnt work: the ranges/dirs of the original scan must be ordered differently 98 | for(size_t vid = 0; vid < model.getHeight(); vid++) 99 | { 100 | for(size_t hid = 0; hid < model.getWidth(); hid++) 101 | { 102 | const size_t bid = model.getBufferId(vid, hid); 103 | 104 | const float range_real = msg->o1dn.data.ranges[bid]; 105 | const float range_sim = ranges[bid]; 106 | 107 | const bool range_real_valid = model.range.inside(range_real); 108 | const bool range_sim_valid = model.range.inside(range_sim); 109 | 110 | if(range_real_valid) 111 | { 112 | rm::Vector preal_s = model.getDirection(vid, hid) * range_real + model.getOrigin(vid, hid); 113 | 114 | if(range_sim_valid) 115 | { 116 | rm::Vector pint_s = model.getDirection(vid, hid) * range_sim; 117 | rm::Vector nint_s = normals[bid]; 118 | nint_s.normalizeInplace(); 119 | 120 | float signed_plane_dist = (preal_s - pint_s).dot(nint_s); 121 | const rm::Vector pmesh_s = preal_s + nint_s * signed_plane_dist; 122 | const float plane_distance = (pmesh_s - preal_s).l2norm(); 123 | 124 | if(range_real < range_sim) 125 | { 126 | // something is in front of surface 127 | if( plane_distance > min_dist_outlier_scan_ ) 128 | { 129 | geometry_msgs::msg::Point32 p_ros; 130 | p_ros.x = preal_s.x; 131 | p_ros.y = preal_s.y; 132 | p_ros.z = preal_s.z; 133 | cloud_outlier_scan.points.push_back(p_ros); 134 | } 135 | } else { 136 | // ray cutted the surface 137 | if( plane_distance > min_dist_outlier_map_ ) 138 | { 139 | geometry_msgs::msg::Point32 p_ros; 140 | p_ros.x = pint_s.x; 141 | p_ros.y = pint_s.y; 142 | p_ros.z = pint_s.z; 143 | cloud_outlier_map.points.push_back(p_ros); 144 | } 145 | } 146 | 147 | } else { 148 | // point in real scan but not in simulated 149 | geometry_msgs::msg::Point32 p_ros; 150 | p_ros.x = preal_s.x; 151 | p_ros.y = preal_s.y; 152 | p_ros.z = preal_s.z; 153 | cloud_outlier_scan.points.push_back(p_ros); 154 | } 155 | } else { 156 | if(range_sim_valid) 157 | { 158 | // sim hits surface but real not: map could be wrong 159 | rm::Vector pint_s = model.getDirection(vid, hid) * range_sim + model.getOrigin(vid, hid); 160 | geometry_msgs::msg::Point32 p_ros; 161 | p_ros.x = pint_s.x; 162 | p_ros.y = pint_s.y; 163 | p_ros.z = pint_s.z; 164 | cloud_outlier_map.points.push_back(p_ros); 165 | } else { 166 | // both sim and real does not hit the map 167 | } 168 | } 169 | } 170 | } 171 | 172 | // TODO: make just a PC2, since PC1 is deprecated 173 | sensor_msgs::msg::PointCloud2 cloud_outlier_scan2; 174 | sensor_msgs::msg::PointCloud2 cloud_outlier_map2; 175 | sensor_msgs::convertPointCloudToPointCloud2(cloud_outlier_scan, cloud_outlier_scan2); 176 | sensor_msgs::convertPointCloudToPointCloud2(cloud_outlier_map, cloud_outlier_map2); 177 | 178 | pub_outlier_scan_->publish(cloud_outlier_scan2); 179 | pub_outlier_map_->publish(cloud_outlier_map2); 180 | } 181 | 182 | } // namespace rmcl 183 | 184 | #include "rclcpp_components/register_node_macro.hpp" 185 | RCLCPP_COMPONENTS_REGISTER_NODE(rmcl::O1DnMapSegmentationEmbreeNode) 186 | -------------------------------------------------------------------------------- /rmcl_ros/src/nodes/filter/scan_map_segmentation_embree.cpp: -------------------------------------------------------------------------------- 1 | #include "rmcl_ros/nodes/filter/scan_map_segmentation_embree.hpp" 2 | 3 | 4 | // Rmagine deps 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | 12 | // RMCL code 13 | #include 14 | #include 15 | #include 16 | 17 | 18 | #include 19 | 20 | #include 21 | #include 22 | #include 23 | #include 24 | #include 25 | 26 | 27 | namespace rm = rmagine; 28 | 29 | namespace rmcl 30 | { 31 | 32 | ScanMapSegmentationEmbreeNode::ScanMapSegmentationEmbreeNode( 33 | const rclcpp::NodeOptions& options) 34 | :MapSegmentationNode("scan_map_segmentation_embree_node", options) 35 | { 36 | std::cout << "ScanMapSegmentationEmbreeNode started" << std::endl; 37 | 38 | rm::EmbreeMapPtr map = rm::import_embree_map(map_file_); 39 | scan_sim_ = std::make_shared(map); 40 | scan_sim_->setTsb(rm::Transform::Identity()); 41 | 42 | sub_scan_ = this->create_subscription( 43 | "scan", 10, 44 | [=](const rmcl_msgs::msg::ScanStamped::ConstSharedPtr& msg) -> void 45 | { 46 | scanCB(msg); 47 | }); 48 | } 49 | 50 | void ScanMapSegmentationEmbreeNode::scanCB( 51 | const rmcl_msgs::msg::ScanStamped::ConstSharedPtr& msg) const 52 | { 53 | geometry_msgs::msg::TransformStamped T_sensor_map; 54 | 55 | try{ 56 | T_sensor_map = tf_buffer_->lookupTransform( 57 | map_frame_, 58 | msg->header.frame_id, 59 | msg->header.stamp, 60 | rclcpp::Duration(1, 0) 61 | ); 62 | } 63 | catch (tf2::TransformException &ex) { 64 | rclcpp::Clock clock = *get_clock(); 65 | RCLCPP_WARN_THROTTLE( 66 | get_logger(), clock, 5000, 67 | "Could not transform from '%s' to '%s': %s", 68 | msg->header.frame_id.c_str(), map_frame_.c_str(), ex.what() 69 | ); 70 | return; 71 | } 72 | 73 | rm::Transform T; 74 | // T[0].t.x = T_sensor_map.transform.translation.x; 75 | convert(T_sensor_map.transform, T); 76 | 77 | // Memory T_ = T; 78 | 79 | rm::O1DnModel model; 80 | convert(msg->o1dn.info, model); 81 | scan_sim_->setModel(model); 82 | 83 | using ResultT = rm::Bundle< 84 | rm::Ranges, 85 | rm::Normals 86 | >; 87 | 88 | ResultT res = scan_sim_->simulate(T); 89 | 90 | const rm::MemoryView ranges = res.ranges; 91 | const rm::MemoryView normals = res.normals; 92 | 93 | sensor_msgs::msg::PointCloud cloud_outlier_scan; 94 | cloud_outlier_scan.header.stamp = msg->header.stamp; 95 | cloud_outlier_scan.header.frame_id = msg->header.frame_id; 96 | 97 | sensor_msgs::msg::PointCloud cloud_outlier_map; 98 | cloud_outlier_map.header.stamp = msg->header.stamp; 99 | cloud_outlier_map.header.frame_id = msg->header.frame_id; 100 | 101 | // if this doesnt work: the ranges/dirs of the original scan must be ordered differently 102 | for(size_t vid = 0; vid < model.getHeight(); vid++) 103 | { 104 | for(size_t hid = 0; hid < model.getWidth(); hid++) 105 | { 106 | const size_t bid = model.getBufferId(vid, hid); 107 | 108 | const float range_real = msg->scan.data.ranges[bid]; 109 | const float range_sim = ranges[bid]; 110 | 111 | const bool range_real_valid = model.range.inside(range_real); 112 | const bool range_sim_valid = model.range.inside(range_sim); 113 | 114 | if(range_real_valid) 115 | { 116 | rm::Vector preal_s = model.getDirection(vid, hid) * range_real + model.getOrigin(vid, hid); 117 | 118 | if(range_sim_valid) 119 | { 120 | rm::Vector pint_s = model.getDirection(vid, hid) * range_sim; 121 | rm::Vector nint_s = normals[bid]; 122 | nint_s.normalizeInplace(); 123 | 124 | float signed_plane_dist = (preal_s - pint_s).dot(nint_s); 125 | const rm::Vector pmesh_s = preal_s + nint_s * signed_plane_dist; 126 | const float plane_distance = (pmesh_s - preal_s).l2norm(); 127 | 128 | if(range_real < range_sim) 129 | { 130 | // something is in front of surface 131 | if( plane_distance > min_dist_outlier_scan_ ) 132 | { 133 | geometry_msgs::msg::Point32 p_ros; 134 | p_ros.x = preal_s.x; 135 | p_ros.y = preal_s.y; 136 | p_ros.z = preal_s.z; 137 | cloud_outlier_scan.points.push_back(p_ros); 138 | } 139 | } else { 140 | // ray cutted the surface 141 | if( plane_distance > min_dist_outlier_map_ ) 142 | { 143 | geometry_msgs::msg::Point32 p_ros; 144 | p_ros.x = pint_s.x; 145 | p_ros.y = pint_s.y; 146 | p_ros.z = pint_s.z; 147 | cloud_outlier_map.points.push_back(p_ros); 148 | } 149 | } 150 | 151 | } else { 152 | // point in real scan but not in simulated 153 | geometry_msgs::msg::Point32 p_ros; 154 | p_ros.x = preal_s.x; 155 | p_ros.y = preal_s.y; 156 | p_ros.z = preal_s.z; 157 | cloud_outlier_scan.points.push_back(p_ros); 158 | } 159 | } else { 160 | if(range_sim_valid) 161 | { 162 | // sim hits surface but real not: map could be wrong 163 | rm::Vector pint_s = model.getDirection(vid, hid) * range_sim + model.getOrigin(vid, hid); 164 | geometry_msgs::msg::Point32 p_ros; 165 | p_ros.x = pint_s.x; 166 | p_ros.y = pint_s.y; 167 | p_ros.z = pint_s.z; 168 | cloud_outlier_map.points.push_back(p_ros); 169 | } else { 170 | // both sim and real does not hit the map 171 | } 172 | } 173 | } 174 | } 175 | 176 | sensor_msgs::msg::PointCloud2 cloud_outlier_scan2; 177 | sensor_msgs::msg::PointCloud2 cloud_outlier_map2; 178 | sensor_msgs::convertPointCloudToPointCloud2(cloud_outlier_scan, cloud_outlier_scan2); 179 | sensor_msgs::convertPointCloudToPointCloud2(cloud_outlier_map, cloud_outlier_map2); 180 | 181 | pub_outlier_scan_->publish(cloud_outlier_scan2); 182 | pub_outlier_map_->publish(cloud_outlier_map2); 183 | } 184 | 185 | } // namespace rmcl 186 | 187 | #include "rclcpp_components/register_node_macro.hpp" 188 | RCLCPP_COMPONENTS_REGISTER_NODE(rmcl::ScanMapSegmentationEmbreeNode) 189 | -------------------------------------------------------------------------------- /rmcl_ros/src/util/ros_helper.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | 5 | namespace rmcl 6 | { 7 | 8 | std::string replace_char( 9 | const std::string& input, 10 | char old_char, 11 | char new_char) 12 | { 13 | std::string modified = input; 14 | for(char& c : modified) 15 | { 16 | if(c == old_char) 17 | { 18 | c = new_char; // Replace the character 19 | } 20 | } 21 | return modified; 22 | } 23 | 24 | void replace_char_inplace( 25 | std::string& input, 26 | char old_char, 27 | char new_char) 28 | { 29 | for(char& c : input) 30 | { 31 | if(c == old_char) 32 | { 33 | c = new_char; // Replace the character 34 | } 35 | } 36 | } 37 | 38 | std::string make_sub_parameter( 39 | rclcpp::Node* node, 40 | const std::string& param_name) 41 | { 42 | if(param_name[0] == '~') 43 | { 44 | std::string param_prefix = node->get_sub_namespace(); 45 | if(param_prefix != "") 46 | { 47 | replace_char_inplace(param_prefix, '/', '.'); 48 | std::string param_postfix = param_name.substr(1); 49 | if(param_postfix != "") 50 | { 51 | param_prefix += "." + param_postfix; 52 | } 53 | return param_prefix; 54 | } 55 | else 56 | { 57 | return param_name.substr(1); 58 | } 59 | } 60 | else 61 | { 62 | return param_name; 63 | } 64 | } 65 | 66 | std::string make_sub_parameter( 67 | rclcpp::Node::SharedPtr node, 68 | const std::string& param_name) 69 | { 70 | return make_sub_parameter(node.get(), param_name); 71 | } 72 | 73 | // special case for char arrays to be converted properly 74 | std::string get_parameter( 75 | rclcpp::Node* node, 76 | const std::string& param_name, 77 | const std::string& default_value) 78 | { 79 | const std::string param_path = make_sub_parameter(node, param_name); 80 | if(node->has_parameter(param_path)) 81 | { 82 | return node->get_parameter(param_path).as_string(); 83 | } 84 | else 85 | { 86 | return node->declare_parameter(param_path, default_value); 87 | } 88 | } 89 | 90 | // special case for char arrays to be converted properly 91 | std::string get_parameter( 92 | rclcpp::Node::SharedPtr node, 93 | const std::string& param_name, 94 | const std::string& default_value) 95 | { 96 | return get_parameter(node.get(), param_name, default_value); 97 | } 98 | 99 | std::optional get_parameter( 100 | rclcpp::Node::SharedPtr node, 101 | const std::string& param_name) 102 | { 103 | const std::string param_path = make_sub_parameter(node, param_name); 104 | if(node->has_parameter(param_path)) 105 | { 106 | return node->get_parameter(param_path); 107 | } 108 | return std::nullopt; 109 | } 110 | 111 | std::map get_parameters( 112 | rclcpp::Node* node, 113 | std::string param_name) 114 | { 115 | std::map param_map; 116 | 117 | const std::string param_path = make_sub_parameter(node, param_name); 118 | if(!node->get_parameters(param_path, param_map)) 119 | { 120 | param_map.clear(); 121 | } 122 | 123 | return param_map; 124 | } 125 | 126 | std::map get_parameters( 127 | rclcpp::Node::SharedPtr node, 128 | std::string prefix) 129 | { 130 | return get_parameters(node.get(), prefix); 131 | } 132 | 133 | } // namespace rmcl 134 | -------------------------------------------------------------------------------- /rmcl_ros/src/util/scan_operations.cpp: -------------------------------------------------------------------------------- 1 | #include "rmcl_ros/util/scan_operations.h" 2 | #include 3 | 4 | namespace rmcl 5 | { 6 | 7 | void fill( 8 | rmcl_msgs::msg::Scan& scan, 9 | const rmagine::Memory& ranges) 10 | { 11 | rmagine::SphericalModel model; 12 | convert(scan.info, model); 13 | 14 | scan.data.ranges.resize(ranges.size()); 15 | for(unsigned int vid = 0; vid < model.getHeight(); vid++) 16 | { 17 | for(unsigned int hid = 0; hid < model.getWidth(); hid++) 18 | { 19 | const unsigned int loc_id = model.getBufferId(vid, hid); 20 | scan.data.ranges[loc_id] = ranges[loc_id]; 21 | } 22 | } 23 | } 24 | 25 | void fillEmpty(rmcl_msgs::msg::Scan& scan) 26 | { 27 | rmagine::SphericalModel model; 28 | convert(scan.info, model); 29 | 30 | scan.data.ranges.resize(model.size()); 31 | for(unsigned int vid = 0; vid < model.getHeight(); vid++) 32 | { 33 | for(unsigned int hid = 0; hid < model.getWidth(); hid++) 34 | { 35 | const unsigned int loc_id = model.getBufferId(vid, hid); 36 | scan.data.ranges[loc_id] = scan.info.range_max + 1.0; 37 | } 38 | } 39 | } 40 | 41 | void filter( 42 | rmcl_msgs::msg::O1Dn& o1dn_out, 43 | const rmcl_msgs::msg::O1Dn& o1dn_in, 44 | const FilterOptions2D& options) 45 | { 46 | o1dn_out.info.width = (o1dn_in.info.width - options.width.skip_begin - options.width.skip_end) / options.width.increment; 47 | o1dn_out.info.height = (o1dn_in.info.height - options.height.skip_begin - options.height.skip_end) / options.height.increment; 48 | 49 | 50 | o1dn_out.info.orig = o1dn_in.info.orig; 51 | o1dn_out.info.dirs.resize(o1dn_in.info.width * o1dn_in.info.height); 52 | 53 | o1dn_out.info.range_min = std::max(o1dn_in.info.range_min, options.range_min); 54 | o1dn_out.info.range_max = std::min(o1dn_in.info.range_max, options.range_max); 55 | 56 | 57 | o1dn_out.data.ranges.resize(o1dn_out.info.width * o1dn_out.info.height); 58 | 59 | const bool has_mask = !o1dn_in.data.mask.empty(); 60 | const bool has_normals = !o1dn_in.data.normals.empty(); 61 | const bool has_colors = !o1dn_in.data.colors.empty(); 62 | const bool has_stamps = !o1dn_in.data.stamps.empty(); 63 | const bool has_intensities = !o1dn_in.data.intensities.empty(); 64 | const bool has_labels = !o1dn_in.data.labels.empty(); 65 | 66 | if(has_mask) 67 | { 68 | o1dn_out.data.mask.resize(o1dn_out.info.width * o1dn_out.info.height); 69 | } 70 | 71 | if(has_normals) 72 | { 73 | o1dn_out.data.normals.resize(o1dn_out.info.width * o1dn_out.info.height); 74 | } 75 | 76 | if(has_colors) 77 | { 78 | o1dn_out.data.colors.resize(o1dn_out.info.width * o1dn_out.info.height); 79 | } 80 | 81 | if(has_stamps) 82 | { 83 | o1dn_out.data.stamps.resize(o1dn_out.info.width * o1dn_out.info.height); 84 | } 85 | 86 | if(has_intensities) 87 | { 88 | o1dn_out.data.intensities.resize(o1dn_out.info.width * o1dn_out.info.height); 89 | } 90 | 91 | if(has_labels) 92 | { 93 | o1dn_out.data.labels.resize(o1dn_out.info.width * o1dn_out.info.height); 94 | } 95 | 96 | for(size_t tgt_i = 0; tgt_i < o1dn_out.info.height; tgt_i++) 97 | { 98 | const size_t src_i = tgt_i * options.height.increment + options.height.skip_begin; 99 | // const uint8_t* row = &pcd->data[src_i * pcd->row_step]; 100 | 101 | for(size_t tgt_j = 0; tgt_j < o1dn_out.info.width; tgt_j++) 102 | { 103 | const size_t src_j = tgt_j * options.width.increment + options.width.skip_begin; 104 | // const uint8_t* data_ptr = &row[src_j * pcd->point_step]; 105 | 106 | const size_t tgt_buf_id = tgt_i * o1dn_out.info.width + tgt_j; 107 | const size_t src_buf_id = src_i * o1dn_in.info.width + src_j; 108 | 109 | o1dn_out.info.dirs[tgt_buf_id] = o1dn_in.info.dirs[src_buf_id]; 110 | o1dn_out.data.ranges[tgt_buf_id] = o1dn_in.data.ranges[src_buf_id]; 111 | 112 | if(has_mask) 113 | { 114 | o1dn_out.data.mask[tgt_buf_id] = o1dn_in.data.mask[src_buf_id]; 115 | } 116 | 117 | if(has_normals) 118 | { 119 | o1dn_out.data.normals[tgt_buf_id] = o1dn_in.data.normals[src_buf_id]; 120 | } 121 | 122 | if(has_colors) 123 | { 124 | o1dn_out.data.colors[tgt_buf_id] = o1dn_in.data.colors[src_buf_id]; 125 | } 126 | 127 | if(has_stamps) 128 | { 129 | o1dn_out.data.stamps[tgt_buf_id] = o1dn_in.data.stamps[src_buf_id]; 130 | } 131 | 132 | if(has_intensities) 133 | { 134 | o1dn_out.data.intensities[tgt_buf_id] = o1dn_in.data.intensities[src_buf_id]; 135 | } 136 | 137 | if(has_labels) 138 | { 139 | o1dn_out.data.labels[tgt_buf_id] = o1dn_in.data.labels[src_buf_id]; 140 | } 141 | } 142 | } 143 | } 144 | 145 | } // namespace rmcl --------------------------------------------------------------------------------