diff --git a/.gitignore b/.gitignore index bf4e674..dbe9c82 100644 --- a/.gitignore +++ b/.gitignore @@ -1,48 +1 @@ -.codes/* -.cmake/* - -build/* - -cmake-build-release/* -cmake-build-debug/* - -materials/*.bin -materials/*.pcd -*bin -*pcd - -.idea/ -.vscode/ - -# Prerequisites -*.d - -# Compiled Object files -*.slo -*.lo -*.o -*.obj - -# Precompiled Headers -*.gch -*.pch - -# Compiled Dynamic libraries -*.so -*.dylib -*.dll - -# Fortran module files -*.mod -*.smod - -# Compiled Static libraries -*.lai -*.la -*.a -*.lib - -# Executables -*.exe -*.out -*.app +.vscode/ \ No newline at end of file diff --git a/3rdparty/find_dependencies.cmake b/3rdparty/find_dependencies.cmake deleted file mode 100644 index 4d874c8..0000000 --- a/3rdparty/find_dependencies.cmake +++ /dev/null @@ -1,26 +0,0 @@ -# MIT License -# -# Copyright (c) 2022 Ignacio Vizzo, Tiziano Guadagnino, Benedikt Mersch, Cyrill -# Stachniss. -# -# Permission is hereby granted, free of charge, to any person obtaining a copy -# of this software and associated documentation files (the "Software"), to deal -# in the Software without restriction, including without limitation the rights -# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -# copies of the Software, and to permit persons to whom the Software is -# furnished to do so, subject to the following conditions: -# -# The above copyright notice and this permission notice shall be included in all -# copies or substantial portions of the Software. -# -# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -# AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -# SOFTWARE. - -include(${CMAKE_CURRENT_LIST_DIR}/pmc/pmc.cmake) - - diff --git a/3rdparty/pmc/LICENSE b/3rdparty/pmc/LICENSE deleted file mode 100644 index de5b632..0000000 --- a/3rdparty/pmc/LICENSE +++ /dev/null @@ -1,18 +0,0 @@ -Eigen is primarily MPL2 licensed. See COPYING.MPL2 and these links: - http://www.mozilla.org/MPL/2.0/ - http://www.mozilla.org/MPL/2.0/FAQ.html - -Some files contain third-party code under BSD or LGPL licenses, whence the other -COPYING.* files here. - -All the LGPL code is either LGPL 2.1-only, or LGPL 2.1-or-later. -For this reason, the COPYING.LGPL file contains the LGPL 2.1 text. - -If you want to guarantee that the Eigen code that you are #including is licensed -under the MPL2 and possibly more permissive licenses (like BSD), #define this -preprocessor symbol: - EIGEN_MPL2_ONLY -For example, with most compilers, you could add this to your project CXXFLAGS: - -DEIGEN_MPL2_ONLY -This will cause a compilation error to be generated if you #include any code that is -LGPL licensed. diff --git a/3rdparty/pmc/pmc.cmake b/3rdparty/pmc/pmc.cmake deleted file mode 100644 index 0e3d8ee..0000000 --- a/3rdparty/pmc/pmc.cmake +++ /dev/null @@ -1,44 +0,0 @@ -# MIT License -# -# Copyright (c) 2022 Ignacio Vizzo, Tiziano Guadagnino, Benedikt Mersch, Cyrill -# Stachniss. -# -# Permission is hereby granted, free of charge, to any person obtaining a copy -# of this software and associated documentation files (the "Software"), to deal -# in the Software without restriction, including without limitation the rights -# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -# copies of the Software, and to permit persons to whom the Software is -# furnished to do so, subject to the following conditions: -# -# The above copyright notice and this permission notice shall be included in all -# copies or substantial portions of the Software. -# -# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -# AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -# SOFTWARE. -include(ExternalProject) - -ExternalProject_Add( - external_pmc - PREFIX pmc - URL https://github.com/LimHyungTae/pmc/archive/refs/tags/libpmc.tar.gz - URL_HASH SHA256=64ea6e628fe0920df771d21a243df6a771ebde9221042fe203390ff1520e969d - UPDATE_COMMAND "" - CMAKE_ARGS -DCMAKE_INSTALL_PREFIX= -) - -ExternalProject_Get_Property(external_pmc INSTALL_DIR) -add_library(PMCHelper INTERFACE) -add_dependencies(PMCHelper external_pmc) -# `target_include_directories`: where to look for headers -target_include_directories(PMCHelper INTERFACE ${INSTALL_DIR}/include) -# `target_link_directories`: where to look for binary files -target_link_directories(PMCHelper INTERFACE ${INSTALL_DIR}/lib) -# link PMChelper to pmc, which should be equal to PREFIX (in line 27) -target_link_libraries(PMCHelper INTERFACE pmc) -set_property(TARGET PMCHelper PROPERTY EXPORT_NAME pmc::pmc) -add_library(pmc::pmc ALIAS PMCHelper) diff --git a/CMakeLists.txt b/CMakeLists.txt old mode 100755 new mode 100644 index 6156adb..77ee580 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,82 +1,77 @@ -cmake_minimum_required(VERSION 3.13) +cmake_minimum_required(VERSION 3.10) project(quatro) -set(CMAKE_CXX_STANDARD 17) - -if (NOT CMAKE_BUILD_TYPE) - set(CMAKE_BUILD_TYPE "Release") -endif () - -find_package(catkin REQUIRED COMPONENTS - roscpp - rospy - std_msgs - geometry_msgs - message_generation - sensor_msgs - nav_msgs - pcl_ros - cv_bridge -) - -find_package(OpenCV REQUIRED) -find_package(OpenCV REQUIRED QUIET) -find_package(OpenMP) -find_package(PCL 1.8 REQUIRED) -find_package(Boost 1.54 REQUIRED) -find_package(Eigen3 3.2 QUIET REQUIRED NO_MODULE) -find_package(catkin QUIET) - -add_message_files( - FILES - cloud_info.msg -) - -generate_messages( - DEPENDENCIES - geometry_msgs - std_msgs - nav_msgs -) - -catkin_package( - INCLUDE_DIRS - LIBRARIES - CATKIN_DEPENDS roscpp rospy std_msgs +### set compiler +set(CMAKE_BUILD_TYPE "Release") +set(CMAKE_CXX_FLAGS "-std=c++17 -fexceptions -g -ggdb") +include(FindOpenMP) #The best way to set proper compiler settings for using OpenMP in all platforms +if(OPENMP_FOUND) #The best way to set proper compiler settings for using OpenMP in all platforms + set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${OpenMP_C_FLAGS}") + set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS}") + set(CMAKE_EXE_LINKER_FLAGS "${CMAKE_EXE_LINKER_FLAGS} ${OpenMP_EXE_LINKER_FLAGS}") +else(OPENMP_FOUND) + message("ERROR: OpenMP could not be found.") +endif(OPENMP_FOUND) +set(CMAKE_CXX_FLAGS_RELEASE "-O3 -Wall ${CMAKE_CXX_FLAGS}") + +### get packages +find_package(Eigen3 REQUIRED) +find_package(PCL REQUIRED) +find_package(teaserpp REQUIRED) +if(PCL_FOUND) + message(WARNING "PCL_VER: ${PCL_VERSION}, and PCL_DIR: ${PCL_INCLUDE_DIRS}") +endif() +if(Eigen3_FOUND) + message(WARNING "EIGEN_VER: ${EIGEN3_VERSION_STRING}, and Eigen DIR: ${EIGEN3_INCLUDE_DIR}") +endif() +if(teaserpp_FOUND) + message(WARNING "teaserpp found") +endif() + +#### for tbb +option(QUATRO_TBB "Enable TBB support" ON) +if(QUATRO_TBB) + message(WARNING "Quatro with TBB") + list(APPEND CMAKE_MODULE_PATH /usr/local/include /usr/include) + find_library(TBB_LIBRARY tbb HINTS /usr/lib /usr/local/lib) + message(WARNING "TBB FOUND, ${TBB_LIBRARY}") + add_definitions(-DTBB_EN) + include(ProcessorCount) + ProcessorCount(N) + message(WARNING "Processer number: ${N}") + if(N GREATER 4) + add_definitions(-DTBB_PROC_NUM=${N}) + else() + add_definitions(-DTBB_PROC_NUM=1) + endif() +endif() + +#### for debug output +if(QUATRO_DEBUG) + add_definitions(-DQUATRO_DEBUG) +endif() + +### output as catkin package +find_package(catkin REQUIRED) +catkin_package( #this automatically adds/installs the results of build + INCLUDE_DIRS include + LIBRARIES ${PROJECT_NAME} ) +### get packages' headers include_directories( - ${catkin_INCLUDE_DIRS} - ${OpenCV_INCLUDE_DIRS} - ${PCL_INCLUDE_DIRS} - include + include + ${EIGEN3_INCLUDE_DIR} + ${PCL_INCLUDE_DIRS} ) -include(3rdparty/find_dependencies.cmake) - -file(DOWNLOAD https://urserver.kaist.ac.kr/publicdata/quatro/000540.bin ${CMAKE_CURRENT_LIST_DIR}/materials/000540.bin) -file(DOWNLOAD https://urserver.kaist.ac.kr/publicdata/quatro/001319.bin ${CMAKE_CURRENT_LIST_DIR}/materials/001319.bin) - -if (OPENMP_FOUND) - set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${OpenMP_C_FLAGS}") - set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS}") -endif () - - - -####### Executable ####### -set(teaser_src - src/graph.cc - src/teaser_utils/feature_matcher.cc - src/teaser_utils/fpfh.cc - ) - -add_executable(run_example examples/run_global_registration.cpp ${teaser_src}) - -target_link_libraries(run_example - PUBLIC - ${PCL_LIBRARY_DIRS} - ${catkin_LIBRARIES} - stdc++fs - pmc::pmc - ) +########### +## Build ## +########### +### main +add_library(${PROJECT_NAME} src/fpfh.cc src/matcher.cc src/quatro_module.cc) +if (QUATRO_TBB) + target_link_libraries(${PROJECT_NAME} ${OpenMP_LIBS} ${EIGEN3_LIBS} ${PCL_LIBRARIES} teaserpp::teaser_registration teaserpp::teaser_io ${TBB_LIBRARY}) +else() + target_link_libraries(${PROJECT_NAME} ${OpenMP_LIBS} ${EIGEN3_LIBS} ${PCL_LIBRARIES} teaserpp::teaser_registration teaserpp::teaser_io) +endif() \ No newline at end of file diff --git a/README.md b/README.md index 62e02db..3ec86bd 100644 --- a/README.md +++ b/README.md @@ -1,140 +1,100 @@ # Quatro - -Official page of *"A Single Correspondence Is Enough: Robust Global Registration to Avoid Degeneracy in Urban Environments"*, which is accepted @ ICRA'22. **NOTE that this repository is the re-implmenation, so it is not exactly same with the original version**. - -#### [[Video]](https://www.youtube.com/results?search_query=A+single+correpsondence+is+enough) [[Priprint Paper]](https://arxiv.org/abs/2203.06612) - -## Demo - -![](materials/README_demo_v2.gif) - -## NEWS (23.1.27) -- An improved version is under review in the Interntaitonal Journal of Robotics Research~(IJRR) -- The codes would be refactored and then updated soon! - -## Characteristics - -* Single hpp file (`include/quatro.hpp`). It requires other hpp files, though :sweat_smile: -* Intuitive usage based on the hierarchical inheritance of the `Registration` class in [Point Cloud Library (PCL)](https://pointclouds.org/). Quatro can be used as follows: - -```c++ -// After the declaration of Quatro, -quatro.setInputSource(srcMatched); -quatro.setInputTarget(tgtMatched); -Eigen::Matrix4d output; -quatro.computeTransformation(output); -``` - -* Robust global registration performance - - - As shown in the below figures, our method shows the most promising performance compared with other state-of-the-art methods. - - It could be over-claim, yet our method would be more appropriate for terrestrial mobile platforms. Reducing the degree of freedom (DoF) usually makes algorithms be more robust against errors by projecting them into a lower dimension. - - E.g. 3D-3D correspondences -> 3D-2D correspondences, LOAM -> LeGO-LOAM - - In this context, our method is the dimension-reduced version of [TEASER++](https://github.com/MIT-SPARK/TEASER-plusplus)! - - -KITTI dataset | NAVER LABS Loc. dataset -:-------------------------:|:-------------------------: -![](materials/kitti_for_readme.PNG) | ![](materials/labs_for_readme.PNG) - - -#### Contributors - -* Beomsoo Kim (as a research intern): `qjatn1208@naver.com` -* Daebeom Kim (as a research intern): `ted97k@naver.com` -* Hyungtae Lim: `shapelim@kaist.ac.kr` - -#### ToDo - -- [x] Add ROS support -- [x] Add demo videos -- [ ] Add preprint paper -- [ ] Add diverse examples for other sensor configuration - ---- - -## Contents -1. [Test Env.](#Test-Env.) -0. [How to Build](#How-to-Build) -0. [How to Run Quatro](#How-to-Run-Quatro) -0. [Citation](#citation) - -### Test Env. - -The code is tested successfully at -* Linux 18.04 LTS -* ROS Melodic - -## How to Build - -### ROS Setting -1. Install the following dependencies - -``` -sudo apt install cmake libeigen3-dev libboost-all-dev -``` - -2. Install [ROS](http://torch.ch/docs/getting-started.html) on a machine. -3. Then, build Quatro package and enjoy! :) We use [catkin tools](https://catkin-tools.readthedocs.io/en/latest/) - -``` -mkdir -p ~/catkin_ws/src -cd ~/catkin_ws/src -git clone git@github.com:url-kaist/Quatro.git -cd ~/catkin_ws -catkin build quatro -``` - -**Note** Quatro requires `pmc` library, which is automatically installed via `3rdparty/find_dependencies.cmake`. - -## How to Run Quatro - -### Prerequisites - -In this study, fast point feature histogram (FPFH) is utilized, which is widely used as a conventional descriptor for the registration. However, the original FPFH for a 3D point cloud captured by a 64-channel LiDAR sensor takes **tens of seconds**, which is too slow. In summary, still, feature extraction & matching is the bottleneck for global registration :worried: (in fact, the accuracy is not very important because Quatro is extremely robust against outliers!). - -For this reason, we employ voxel-sampled FPFH, which is preceded by voxel-sampling. This is followed by the correspondence test. In addition, we employ [Patchwork](https://arxiv.org/abs/2108.05560), which is the state-of-the-art method for ground segmentation, and image projection to reject some subclusters, which is proposed in [Le-GO-LOAM](https://github.com/RobustFieldAutonomyLab/LeGO-LOAM). These modules are not presented in our paper! - -Finally, we can reduce the computational time of feature extraction & matching, i.e. the front-end of global registration, from tens of seconds to almost 0.2 sec. The overall pipeline is as follows: - -![](materials/quatro_overview.PNG) - -#### Note - -For fine-tuning of the parameters to use this code in your own situations, please refer to `config` folder. In particular, for fine-tuning of Patchwork, please refer to [this Wiki](https://github.com/LimHyungTae/patchwork/wiki/4.-IMPORTANT:-Setting-Parameters-of-Patchwork-in-Your-Own-Env.) - -### TL;DR -1. ~Download toy pcd bins files~ - -~The point clouds are from the KITTI dataset, so these are captured by Velodyne-64-HDE~ - -Toy pcds are automatically downloaded. If there is a problem, follow the below commands: - ++ This branch is for being used as a third party module in other packages ++ `Quatro` is from [here, official repo](https://github.com/url-kaist/Quatro) and [here, author's module repository](https://github.com/LimHyungTae/quatro-cpp-fpfh/) + + `Quatro` is for the robust and global registration of pointclouds to avoid degeneracy in urban environments + + +### Dependencies ++ `PCL` >= 1.8 ++ `C++` >= 17 ++ `Boost` >= 1.54 ++ `Eigen` >= 3.2 ++ `OpenMP` >= 4.5 ++ `Teaser++` (tested with commit ver `e415c0d`, May 22, 2023) + ```shell + git clone https://github.com/MIT-SPARK/TEASER-plusplus.git + cd TEASER-plusplus && mkdir build && cd build + cmake .. -DENABLE_DIAGNOSTIC_PRINT=OFF + sudo make install -j16 + sudo ldconfig + ``` ++ (Optional, but recommended) `tbb` for parallelization and ***x10 faster computation in matching*** + ```shell + sudo apt install libtbb-dev + ``` + +### How to build ++ Git clone and `catkin build` this repository +```shell +cd ~/your_workspace/src +git clone https://github.com/engcang/Quatro +cd .. +catkin build ``` -roscd quatro -cd materials -wget https://urserver.kaist.ac.kr/publicdata/quatro/000540.bin -wget https://urserver.kaist.ac.kr/publicdata/quatro/001319.bin ++ (Optional, but recommended) with `tbb` +```shell +catkin build -DQUATRO_TBB=ON #which is default +catkin build -DQUATRO_TBB=OFF #to turn of TBB ``` - -2. Launch the roslaunch file as follows: - ++ (Optional) to enable `std::cout` for debugging, +```shell +catkin build -DQUATRO_DEBUG=ON ``` -OMP_NUM_THREADS=4 roslaunch quatro quatro.launch -``` - -(Unfortunately, for the first run, it shows rather slow and imprecise performance. It may be due to multi-thread issues.) -Visualized inner pipelines | Source (red), target (green), and the estimated output (blue) -:-------------------------:|:-------------------------: -![](materials/quatro_inner.png) | ![](materials/quatro_output.png) +### Use case ++ Refer - [here](https://github.com/engcang/FAST-LIO-SAM-QN) ++ Or, refer the example as follows: + 1. Make sure that you have all dependencies and built the `quatro` properly with `catkin build` as above + 2. In the `CMakeLists.txt` of your wanted package, import `quatro` as a component of `catkin` + ```CMake + find_package(catkin REQUIRED COMPONENTS + ... + quatro #Include here + ... + ) + include_directories( + ... + ${catkin_INCLUDE_DIRS} #Header files are included in catkin_INCLUDE_DIRS + ... + ) + add_library(some_library src/some_src.cpp) + target_link_libraries(some_library ${catkin_LIBRARIES}) #Libraries are included in catkin_LIBRARIES + ``` + 4. Use in the source file of your wanted package as: + ```c++ + #include + using QuatroPointType = pcl::PointXYZI; //can be changed + + shared_ptr> m_quatro_handler = nullptr; + m_quatro_handler = std::make_shared>(fpfh_normal_radius_, + fpfh_radius_, noise_bound_, rot_gnc_factor_, rot_cost_diff_thr_, + quatro_max_iter_, estimat_scale_, optimize_matching, + distance_threshold, max_correspondences); //refer https://github.com/engcang/FAST-LIO-SAM-QN/blob/master/fast_lio_sam_qn/config/config.yaml#L28 + + ////// use + pcl::PointCloud src_; //this should be not empty but the real data + pcl::PointCloud dst_; //this should be not empty but the real data + bool if_valid_; + Eigen::Matrix4d output_tf_ = m_quatro_handler->align(src_, dst_, if_valid_); + if (if_valid_) + { + //do something wiht output_tf_ + } + ``` + +### License +Creative Commons License +- This work is licensed under a [Creative Commons Attribution-NonCommercial-ShareAlike 4.0 International License](http://creativecommons.org/licenses/by-nc-sa/4.0/) + +### Acknowledgement +- This work was supported by the Industry Core Technology Development Project, 20005062, Development of Artificial Intelligence Robot Autonomous Navigation Technology for Agile Movement in Crowded Space, funded by the Ministry of Trade, Industry & Energy (MOTIE, Republic of Korea) and by the research project “Development of A.I. based recognition, judgement and control solution for autonomous vehicle corresponding to atypical driving environment,” which is financed from the Ministry of Science and ICT (Republic of Korea) Contract No. 2019-0-00399. The student was supported by the BK21 FOUR from the Ministry of Education (Republic of Korea). +### Copyright +- All codes on this page are copyrighted by KAIST published under the Creative Commons Attribution-NonCommercial-ShareAlike 4.0 License. You must attribute the work in the manner specified by the author. You may not use the work for commercial purposes, and you may only distribute the resulting work under the same license if you alter, transform, or create the work. -## Citation - -If our research has been helpful, please cite the below papers: - -``` +### Citation +- If our research has been helpful, please cite the below paper: +```tex @article{lim2022quatro, title={A Single Correspondence Is Enough: Robust Global Registration to Avoid Degeneracy in Urban Environments}, author={Lim, Hyungtae and Yeon, Suyong and Ryu, Suyong and Lee, Yonghan and Kim, Youngji and Yun, Jaeseong and Jung, Euigon and Lee, Donghwan and Myung, Hyun}, @@ -143,25 +103,11 @@ If our research has been helpful, please cite the below papers: pages={Accepted. To appear} } ``` - -``` -@article{lim2021patchwork, - title={Patchwork: Concentric Zone-based Region-wise Ground Segmentation with Ground Likelihood Estimation Using a 3D LiDAR Sensor}, - author={Lim, Hyungtae and Minho, Oh and Myung, Hyun}, - journal={IEEE Robot. Autom. Lett.}, - volume={6}, - number={4}, - pages={6458--6465}, - year={2021}, - } -``` - -## Acknowledgment -This work was supported by the Industry Core Technology Development Project, 20005062, Development of Artificial Intelligence Robot Autonomous Navigation Technology for Agile Movement in Crowded Space, funded by the Ministry of Trade, Industry & Energy (MOTIE, Republic of Korea) and by the research project “Development of A.I. based recognition, judgement and control solution for autonomous vehicle corresponding to atypical driving environment,” which is financed from the Ministry of Science and ICT (Republic of Korea) Contract No. 2019-0-00399. The student was supported by the BK21 FOUR from the Ministry of Education (Republic of Korea). - -## License -Creative Commons License
This work is licensed under a Creative Commons Attribution-NonCommercial-ShareAlike 4.0 International License. - - -### Copyright -- All codes on this page are copyrighted by KAIST published under the Creative Commons Attribution-NonCommercial-ShareAlike 4.0 License. You must attribute the work in the manner specified by the author. You may not use the work for commercial purposes, and you may only distribute the resulting work under the same license if you alter, transform, or create the work. +```tex +@article{lim2023quatro-plusplus, + author = {Lim, Hyungtae and Kim, Beomsoo and Kim, Daebeom and Lee, Eungchang and Myung, Hyun}, + title = {Quatro++: Robust Global Registration Exploiting Ground Segmentation for Loop Closing in LiDAR SLAM}, + booktitle = {International Journal of Robotics Research}, + year = {2023}, +} +``` \ No newline at end of file diff --git a/config/params.yaml b/config/params.yaml deleted file mode 100755 index 6838fa2..0000000 --- a/config/params.yaml +++ /dev/null @@ -1,49 +0,0 @@ -# Velodyne-64-HDE, VLP-16, HDL-32E, Ouster-OS1-16, Ouster-OS1-64 -Lidar_type: Velodyne-64-HDE - -ground_segmentation_mode: Patchwork # LeGO-LOAM , Patchwork -# 4Neighbor, 8Neighbor, 4CrossNeighbor -# 4Neighbor is the original sub-clustering method, but we empirically found that -# comparing 4 pixels located on the diagonal way is effective when Patchwork is employed as preprocessing -neigbor_mode: 4CrossNeighbor - -# Extrinsics (Raw lidar coordinate -> Coordinate that is parallel to the X-Y plane of ground) -# But, not in use -extrinsic_trans: [0.0, 0.0, 0.0] -extrinsic_rot: [1, 0, 0, - 0, 1, 0, - 0, 0, 1] - -# We empirically found that w/o voxelization rather degrades the matching performance! -# For Velodyne 16 Puck (NAVER LABS Loc dataset), -# voxel_size - 0.1, normal_radius - 0.3, fpfh_radius - 0.45 -# For Velodyne 64 HDE (KITTI dataset), -# voxel_size - 0.3, normal_radius - 0.5, fpfh_radius - 0.75 -voxel_size: 0.3 -FPFH: - normal_radius: 0.5 - fpfh_radius: 0.75 # `fpfh_radius` should satisfy the following condition: `fpfh_radius` >= 1.5 * `normal_radius` - -Quatro: - estimating_scale: false - # The magnitude of uncertainty of measurements - # Let v be the voxel size, we empirically found that the best `noise_bound` is within the range over v / 2 ~ v for a 3D point cloud - noise_bound: 0.3 - # `noise_bound_coeff` plays a role as an uncertainty multiplier and is used when estimating COTE. - # I.e. final noise bound is set to `noise_bound` * `noise_bound_coeff` - noise_bound_coeff: 1.0 - rotation: - # Num. max iter for the rotation estimation. - # Usually, rotation estimation converges within < 20 iterations - num_max_iter: 50 - # Control the magnitue of the increase in non-linearity. In case of TLS, usually `gnc_factor` is set to 1.4 - # The larger the value, the steeper the increase in nonlinearity. - gnc_factor: 1.4 - # The cost threshold is compared with the difference between costs of consecutive iterations. - # Once the diff. of cost < `rot_cost_diff_thr`, then the optimization is finished. - rot_cost_diff_thr: 0.00011 - - - - - diff --git a/config/patchwork_params.yaml b/config/patchwork_params.yaml deleted file mode 100755 index 0979dec..0000000 --- a/config/patchwork_params.yaml +++ /dev/null @@ -1,48 +0,0 @@ -sensor_height: 1.723 -save_flag: true - -patchwork: - mode: "czm" - verbose: true # To check effect of uprightness/elevation/flatness - visualize: true # Ground Likelihood Estimation is visualized - # Ground Plane Fitting parameters - num_iter: 3 - num_lpr: 20 - num_min_pts: 80 # 10 - th_seeds: 0.25 - th_dist: 0.125 # 0.125 - max_r: 80.0 - min_r: 2.7 # to consider vicinity of mobile plot form. - uprightness_thr: 0.707 # For uprightness. 45: 0.707 / 60: 0.866. The larger, the more conservative - - # The points below the adaptive_seed_selection_margin * sensor_height are filtered - # For reject points caused by reflection or multipath problems. - # it should be lower than -1.0 - adaptive_seed_selection_margin: -1.1 - - # It is not in the paper - # It is also not matched our philosophy, but it is employed to reject some FPs easily & intuitively. - # For patchwork, the global elevation threshold is only applied on Z3 and Z4 - using_global_elevation: false - # W.r.t sensor frame (That is, if it is 0.0, then the candidates whose z is higher than z height of 3D LiDAR sensor are rejected - global_elevation_threshold: -0.5 - - uniform: # deprecated - num_rings: 16 - num_sectors: 54 - czm: - # Note that `num_zones` == size of `num_sectors_each_zone` == size of `num_rings_each_zone` == size of `min_ranges` - 1 - # To divide zones, max_r, min_r, and min_ranges are utilized - num_zones: 4 - num_sectors_each_zone: [16, 32 ,54, 32] - num_rings_each_zone: [2, 4, 4, 4] - # Note that `min_r` == `min_ranges_each_zone`[0]! - min_ranges_each_zone: [2.7, 12.3625, 22.025, 41.35] - # The elevation_thresholds are with respect to the sensor frame - # Thus, actually (sensor_height - threshold_value) is the maximum ground height. - # For instance, for the first ring, 1.723 - 1.2 = 0.523 is acceptable, maximum ground height - # Note that the size of below parameters are indepentent to `num_zones`! - elevation_thresholds: [-1.2, -0.9984, -0.851, -0.605] # For elevation. The size should be equal to flatness_thresholds vector - flatness_thresholds: [0.0001, 0.000125, 0.000185, 0.000185] # For flatness. The size should be equal to elevation_thresholds vector - - diff --git a/examples/run_global_registration.cpp b/examples/run_global_registration.cpp deleted file mode 100644 index 6309e4c..0000000 --- a/examples/run_global_registration.cpp +++ /dev/null @@ -1,402 +0,0 @@ -#include - -// Our modules -#include "fpfh_manager.hpp" -#include "quatro.hpp" -#include "imageProjection.hpp" -#include "patchwork.hpp" - -namespace fs = std::experimental::filesystem; - -boost::shared_ptr > patchwork; - -pcl::PointCloud::ConstPtr getCloud(std::string filename); - -void setParams( - double noise_bound_of_each_measurement, double square_of_the_ratio_btw_noise_and_noise_bound, - double estimating_scale, int num_max_iter, double control_parameter_for_gnc, - double rot_cost_thr, const string& reg_type_name, Quatro::Params ¶ms); - -std::string lidarType; -std::string groundSegMode; -std::string neighborSelectionMode; - -// Just for printing out the results -const char separator = ' '; -const int nameWidth = 22; -const int numWidth = 8; -string dashedLine = std::string(nameWidth + numWidth * 2 + 7, '-'); - -using namespace std; - -int main(int argc, char **argv) { - ros::init(argc, argv, "run_example"); - ros::NodeHandle nh; - - // Feature extraction parameters - double voxel_size, normal_radius, fpfh_radius; - nh.param("/voxel_size", voxel_size, 0.3); - nh.param("/FPFH/normal_radius", normal_radius, 0.5); - nh.param("/FPFH/fpfh_radius", fpfh_radius, 0.75); - - // Quatro parameters - bool estimating_scale; - int num_max_iter; - double noise_bound, noise_bound_coeff, gnc_factor, rot_cost_diff_thr; - nh.param("/Quatro/estimating_scale", estimating_scale, false); - nh.param("/Quatro/noise_bound", noise_bound, 0.25); - nh.param("/Quatro/noise_bound_coeff", noise_bound_coeff, 0.99); - nh.param("/Quatro/rotation/gnc_factor", gnc_factor, 1.39); - nh.param("/Quatro/rotation/rot_cost_diff_thr", rot_cost_diff_thr, 0.0001); - nh.param("/Quatro/rotation/num_max_iter", num_max_iter, 50); - - nh.param("/Lidar_type", lidarType, "Velodyne-64-HDE"); - nh.param("/ground_segmentation_mode", groundSegMode, "Patchwork"); - nh.param("/neigbor_mode", neighborSelectionMode, "4CrossNeighbor"); - - ros::Publisher SrcPublisher = nh.advertise("/source", 100); - ros::Publisher TgtPublisher = nh.advertise("/target", 100); - ros::Publisher AlignPublisher = nh.advertise("/align", 100); - - ros::Publisher SrcMatchingPublisher = nh.advertise("/matched_src", 100); - ros::Publisher TgtMatchingPublisher = nh.advertise("/matched_tgt", 100); - - ros::Publisher SrcMCPublisher = nh.advertise("/max_clique_src", 100); - ros::Publisher TgtMCPublisher = nh.advertise("/max_clique_tgt", 100); - - ros::Publisher SrcGroundPublisher = nh.advertise("/ground_src", 100); - ros::Publisher SrcNongroundPublisher = nh.advertise("/nonground_src", 100); - - ros::Publisher TgtGroundPublisher = nh.advertise("/ground_tgt", 100); - ros::Publisher TgtNongroundPublisher = nh.advertise("/nonground_tgt", 100); - - ros::Publisher SrcTrueKpsPublisher = nh.advertise("/true_kps_src", 100); - ros::Publisher SrcTransformedPublisher = nh.advertise("/Transformed_src", 100); - - ros::Publisher SrcValidSegPublisher = nh.advertise("/valid_seg_src", 100); - ros::Publisher TgtValidSegPublisher = nh.advertise("/valid_seg_tgt", 100); - ros::Publisher SrcInvalidSegPublisher = nh.advertise("/invalid_seg_src", 100); - ros::Publisher TgtInvalidSegPublisher = nh.advertise("/invalid_seg_tgt", 100); - - ros::Publisher CorrespondencePublisher = nh.advertise("/correspondence", 100); - ros::Publisher CorrMCPublisher = nh.advertise("/corrMC", 100); - - /*** - * Load toy dataset - */ - pcl::PointCloud::Ptr srcRaw(new pcl::PointCloud); - pcl::PointCloud::Ptr tgtRaw(new pcl::PointCloud); - - std::string quatro_path = ros::package::getPath("quatro"); - cout << "Quatro path: " << quatro_path << endl; - string src_path = quatro_path + "/materials/000540.bin"; - string tgt_path = quatro_path + "/materials/001319.bin"; - *srcRaw = *getCloud(src_path); - *tgtRaw = *getCloud(tgt_path); - - /*** - * STEP 1. Initialize Quatro - * If you employ quatro in your own SLAM method, - * then note that `quatro.reset(params)` is necessary after the optimization for the next global registration - * Or, it causes munmap error :( (IMPORTANT) - */ - Quatro quatro; - Quatro::Params params; - - setParams(noise_bound, noise_bound_coeff, - estimating_scale, num_max_iter, gnc_factor, rot_cost_diff_thr, "Quatro", params); - quatro.reset(params); - - static double tTotal, tSrc, tTgt; - - // Outputs of ground segmentation - pcl::PointCloud srcGround; - pcl::PointCloud tgtGround; - pcl::PointCloud::Ptr ptrSrcNonground(new pcl::PointCloud); - pcl::PointCloud::Ptr ptrTgtNonground(new pcl::PointCloud); - - - pcl::PointCloud srcInvalidSegments; - pcl::PointCloud tgtInvalidSegments; - pcl::PointCloud::Ptr srcValidSegments(new pcl::PointCloud); - pcl::PointCloud::Ptr tgtValidSegments(new pcl::PointCloud); - - ImageProjection IPSrc(lidarType, neighborSelectionMode, groundSegMode); - ImageProjection IPTgt(lidarType, neighborSelectionMode, groundSegMode); - - std::chrono::system_clock::time_point start = std::chrono::system_clock::now(); - if (groundSegMode == "LeGO-LOAM") { - std::cout << "Ground Segmentation Mode: LeGO-LOAM" << std::endl; - IPSrc.segmentCloud(srcRaw); - IPTgt.segmentCloud(tgtRaw); - - IPSrc.getGround(srcGround); - IPTgt.getGround(tgtGround); - - } else if (groundSegMode == "Patchwork") { - std::cout << "Ground Segmentation Mode: Patchwork" << std::endl; - /*** - * STEP 2. Preprocessing by extracting non-ground points - * for speeding up the STEP 3 (feature extraction & matching), - * redundant points, i.e. ground or bushes, are rejected - */ - patchwork.reset(new PatchWork(&nh)); - patchwork->estimate_ground(*(srcRaw), srcGround, *ptrSrcNonground, tSrc); - patchwork->estimate_ground(*(tgtRaw), tgtGround, *ptrTgtNonground, tTgt); - - /*** - * STEP 3. Extract valid segment points by using image projection - * This process outputs valid segments by image projection, - * i.e. `srcValidSegments` and `tgtValidSegments`. - * Next, feature extraction takes these valid segments as input. - * The original code of image projection is from LeGO-LOAM: - * https://github.com/RobustFieldAutonomyLab/LeGO-LOAM - */ - IPSrc.segmentCloud(ptrSrcNonground); - IPTgt.segmentCloud(ptrTgtNonground); - } - IPSrc.getValidSegments(*srcValidSegments); - IPTgt.getValidSegments(*tgtValidSegments); - - IPSrc.getOutliers(srcInvalidSegments); - IPTgt.getOutliers(tgtInvalidSegments); - - /* --------------------------------------------------------------------- */ - /*** - * cout output of preprocessing - */ - std::cout.imbue(std::locale("")); - cout << dashedLine << endl; - cout << left << setw(nameWidth) << setfill(separator) << "# of raw cloud" << " | "; - cout << right << setw(numWidth) << setfill(separator) << srcRaw->size() << " | "; - cout << right << setw(numWidth) << setfill(separator) << tgtRaw->size() << endl; - - cout << left << setw(nameWidth) << setfill(separator) << "# of ground" << " | "; - cout << right << setw(numWidth) << setfill(separator) << srcGround.size() << " | "; - cout << right << setw(numWidth) << setfill(separator) << tgtGround.size() << endl; - - if (groundSegMode == "Patchwork") { - cout << left << setw(nameWidth) << setfill(separator) << "# of nonground" << " | "; - cout << right << setw(numWidth) << setfill(separator) << ptrSrcNonground->points.size() << " | "; - cout << right << setw(numWidth) << setfill(separator) << ptrTgtNonground->points.size() << endl; - } - - cout << left << setw(nameWidth) << setfill(separator) << "# of subcluster" << " | "; - cout << right << setw(numWidth) << setfill(separator) << srcInvalidSegments.size() << " | "; - cout << right << setw(numWidth) << setfill(separator) << tgtInvalidSegments.size() << endl; - - cout << dashedLine << endl; - cout << left << setw(nameWidth) << setfill(separator) << "# of segments (Input)" << " | "; - cout << right << setw(numWidth) << setfill(separator) << srcValidSegments->points.size() << " | "; - cout << right << setw(numWidth) << setfill(separator) << tgtValidSegments->points.size() << endl; - cout << dashedLine << endl; - /* --------------------------------------------------------------------- */ - - /*** - * STEP 4. Extract feature matching pairs - * We employed FPFH and empirically found that w/o voxelization rather degrades the matching performance! - * Note that size of the matched pair should be identical to each other, i.e. 3 X N. - * This part can also be changed with other feature extraction & matching methods. - */ - - // voxelize - pcl::PointCloud::Ptr srcFeat(new pcl::PointCloud); - pcl::PointCloud::Ptr tgtFeat(new pcl::PointCloud); - - voxelize(srcValidSegments, srcFeat, voxel_size); - voxelize(tgtValidSegments, tgtFeat, voxel_size); - - FPFHManager fpfhmanager(normal_radius, fpfh_radius); - fpfhmanager.flushAllFeatures(); - fpfhmanager.setFeaturePair(srcFeat, tgtFeat); - - // Eigen type -// Eigen::Matrix src_matched = fpfhmanager.getSrcMatched(); -// Eigen::Matrix tgt_matched = fpfhmanager.getTgtMatched(); - - // PCL type - pcl::PointCloud::Ptr srcMatched(new pcl::PointCloud); - pcl::PointCloud::Ptr tgtMatched(new pcl::PointCloud); - *srcMatched = fpfhmanager.getSrcKps(); - *tgtMatched = fpfhmanager.getTgtKps(); - - /* --------------------------------------------------------------------- */ - /*** - * cout output of Feature extraction & matching - */ - cout << dashedLine << endl; - cout << left << setw(nameWidth) << setfill(separator) << "# after voxelization" << " | "; - cout << right << setw(numWidth) << setfill(separator) << srcFeat->size() << " | "; - cout << right << setw(numWidth) << setfill(separator) << tgtFeat->size() << endl; - - cout << left << setw(nameWidth) << setfill(separator) << "# after matching" << " | "; - cout << right << setw(numWidth) << setfill(separator) << srcMatched->size() << " | "; - cout << right << setw(numWidth) << setfill(separator) << tgtMatched->size() << endl; - cout << dashedLine << endl; - /* --------------------------------------------------------------------- */ - - /*** - * STEP 5. Calculate relative pose between source and target - * Our method is PCL-friendly!! - */ - std::chrono::system_clock::time_point before_optim = std::chrono::system_clock::now(); - quatro.setInputSource(srcMatched); - quatro.setInputTarget(tgtMatched); - Eigen::Matrix4d output; - quatro.computeTransformation(output); - - std::chrono::duration sec = std::chrono::system_clock::now() - start; - std::chrono::duration optim_sec = std::chrono::system_clock::now() - before_optim; - std::cout << setprecision(4) << "\033[1;32mTotal takes: " << sec.count() << " sec. "; - std::cout << "(Setting matching pairs: " << sec.count() - optim_sec.count() << " sec. + Quatro: " << optim_sec.count() << " sec.)\033[0m" << std::endl; - - /** - * Below codes are for visualization - */ - // matched feature src - pcl::PointCloud aligned; - pcl::transformPointCloud(*srcRaw, aligned, output); - - // Good matched visualize - pcl::PointCloud transformed_srcMatched; - pcl::transformPointCloud(*srcMatched, transformed_srcMatched, output); - - pcl::PointCloud transformed_src; - pcl::PointCloud good_matched_pcl; - pcl::transformPointCloud(*srcFeat, transformed_src, output); - - std::vector> corr = fpfhmanager.getCorrespondences(); - for (size_t i = 0; i < corr.size(); ++i) { - auto src_idx = std::get<0>(corr[i]); - auto dst_idx = std::get<1>(corr[i]); - - double distance = sqrt( - pow(double(transformed_src[src_idx].x) - double(tgtFeat->points[dst_idx].x), 2) + - pow(double(transformed_src[src_idx].y) - double(tgtFeat->points[dst_idx].y), 2) + - pow(double(transformed_src[src_idx].z) - double(tgtFeat->points[dst_idx].z), 2) - ); - - if (distance < voxel_size * 3.0) - { - pcl::PointXYZ point_xyz; - point_xyz.x = double(transformed_src[src_idx].x); - point_xyz.y = double(transformed_src[src_idx].y); - point_xyz.z = double(transformed_src[src_idx].z); - - good_matched_pcl.push_back(point_xyz); - } - } - - pcl::PointCloud srcMaxCliques; - pcl::PointCloud tgtMaxCliques; - quatro.getMaxCliques(srcMaxCliques, tgtMaxCliques); - - visualization_msgs::Marker corrMarker, corrMCMarker; - setCorrespondenceMarker(*srcMatched, *tgtMatched, corrMarker); - setCorrespondenceMarker(srcMaxCliques, srcMaxCliques, corrMCMarker, 0.2, {0.0, 1.0, 0.0}, 5143); - - sensor_msgs::PointCloud2 SrcMsg = cloud2msg(*srcRaw); - sensor_msgs::PointCloud2 TgtMsg = cloud2msg(*tgtRaw); - sensor_msgs::PointCloud2 AlignMsg = cloud2msg(aligned); - - // Results of preprocessing - sensor_msgs::PointCloud2 SrcGndMsg = cloud2msg(srcGround); - sensor_msgs::PointCloud2 TgtGndMsg = cloud2msg(tgtGround); - sensor_msgs::PointCloud2 SrcNGMsg = cloud2msg(*ptrSrcNonground); - sensor_msgs::PointCloud2 TgtNGMsg = cloud2msg(*ptrTgtNonground); - - sensor_msgs::PointCloud2 SrcValidSegMsg = cloud2msg(*srcValidSegments); - sensor_msgs::PointCloud2 TgtValidSegMsg = cloud2msg(*tgtValidSegments); - sensor_msgs::PointCloud2 SrcInvalidSegMsg = cloud2msg(srcInvalidSegments); - sensor_msgs::PointCloud2 TgtInvalidSegMsg = cloud2msg(tgtInvalidSegments); - - sensor_msgs::PointCloud2 SrcTrueKPsMsg = cloud2msg(good_matched_pcl); - sensor_msgs::PointCloud2 SrcTfMsg = cloud2msg(transformed_srcMatched); - - int cnt = 0; - cout << "Done" << endl; - - ros::Rate loop_rate(10); - while (ros::ok()) { - /*** - * Source, target, and estimate - */ - SrcPublisher.publish(SrcMsg); - TgtPublisher.publish(TgtMsg); - AlignPublisher.publish(AlignMsg); - - /*** - * Visualization of preprocessing and segmentations - */ - SrcGroundPublisher.publish(SrcGndMsg); - SrcNongroundPublisher.publish(SrcNGMsg); - SrcValidSegPublisher.publish(SrcValidSegMsg); - SrcInvalidSegPublisher.publish(SrcInvalidSegMsg); - - TgtGroundPublisher.publish(TgtGndMsg); - TgtNongroundPublisher.publish(TgtNGMsg); - TgtValidSegPublisher.publish(TgtValidSegMsg); - TgtInvalidSegPublisher.publish(TgtInvalidSegMsg); - - SrcMatchingPublisher.publish(cloud2msg(*srcMatched)); - TgtMatchingPublisher.publish(cloud2msg(*tgtMatched)); - - SrcMCPublisher.publish(cloud2msg(srcMaxCliques)); - TgtMCPublisher.publish(cloud2msg(tgtMaxCliques)); - - SrcTrueKpsPublisher.publish(SrcTrueKPsMsg); - SrcTransformedPublisher.publish(SrcTfMsg); - - CorrespondencePublisher.publish(corrMarker); - CorrMCPublisher.publish(corrMCMarker); - - loop_rate.sleep(); - } -} - -void setParams( - double noise_bound_of_each_measurement, double square_of_the_ratio_btw_noise_and_noise_bound, - double estimating_scale, int num_max_iter, double control_parameter_for_gnc, - double rot_cost_thr, const string& reg_type_name, Quatro::Params ¶ms) { - //Quatro::Params - - params.noise_bound = noise_bound_of_each_measurement; - params.cbar2 = square_of_the_ratio_btw_noise_and_noise_bound; - params.estimate_scaling = estimating_scale; - params.rotation_max_iterations = num_max_iter; - params.rotation_gnc_factor = control_parameter_for_gnc; - params.rotation_estimation_algorithm = Quatro::ROTATION_ESTIMATION_ALGORITHM::GNC_TLS; - params.rotation_cost_threshold = rot_cost_thr; - - params.reg_name = reg_type_name; - if (reg_type_name == "Quatro") { - params.inlier_selection_mode = Quatro::INLIER_SELECTION_MODE::PMC_HEU; - } else { params.inlier_selection_mode = Quatro::INLIER_SELECTION_MODE::PMC_EXACT; } -} - -pcl::PointCloud::ConstPtr getCloud(std::string filename) { - FILE *file = fopen(filename.c_str(), "rb"); - if (!file) { - std::cerr << "error: failed to load " << filename << std::endl; - return nullptr; - } - - std::vector buffer(1000000); - size_t num_points = - fread(reinterpret_cast(buffer.data()), sizeof(float), buffer.size(), file) / 4; - fclose(file); - - pcl::PointCloud::Ptr cloud(new pcl::PointCloud()); - cloud->resize(num_points); - - for (int i = 0; i < num_points; i++) { - auto &pt = cloud->at(i); - pt.x = buffer[i * 4]; - pt.y = buffer[i * 4 + 1]; - pt.z = buffer[i * 4 + 2]; - // Intensity is not in use -// pt.intensity = buffer[i * 4 + 3]; - } - - return cloud; -} diff --git a/include/conversion.hpp b/include/conversion.hpp deleted file mode 100644 index 0ed2531..0000000 --- a/include/conversion.hpp +++ /dev/null @@ -1,59 +0,0 @@ -#ifndef CONVERSION_HPP -#define CONVERSION_HPP - -#include -#include -#include -#include -#include -#include -#include - -template -void pcl2teaser(const pcl::PointCloud &pcl_raw, teaser::PointCloud &cloud) { - cloud.clear(); - for (const auto &pt: pcl_raw.points) { - cloud.push_back({pt.x, pt.y, pt.z}); - } -} - -template -sensor_msgs::PointCloud2 cloud2msg(pcl::PointCloud cloud, std::string frame_id = "map") { - sensor_msgs::PointCloud2 cloud_ROS; - pcl::toROSMsg(cloud, cloud_ROS); - cloud_ROS.header.frame_id = frame_id; - return cloud_ROS; -} - -void xyzi2xyz(pcl::PointCloud::Ptr XYZI, pcl::PointCloud::Ptr XYZ) { - (*XYZ).points.resize((*XYZI).size()); - for (size_t i = 0; i < (*XYZI).points.size(); i++) { - (*XYZ).points[i].x = (*XYZI).points[i].x; - (*XYZ).points[i].y = (*XYZI).points[i].y; - (*XYZ).points[i].z = (*XYZI).points[i].z; - } -} - -template -void pcl2eigen(const pcl::PointCloud &pcl_raw, Eigen::Matrix &cloud) { - int N = pcl_raw.points.size(); - cloud.resize(3, N); - for (int i = 0; i < N; ++i) { - cloud.col(i) << pcl_raw.points[i].x, pcl_raw.points[i].y, pcl_raw.points[i].z; - } -} - -template -void eigen2pcl(const Eigen::Matrix &src, pcl::PointCloud &cloud) { - int num_pc = src.cols(); - T pt_tmp; - if (!cloud.empty()) cloud.clear(); - for (int i = 0; i < num_pc; ++i) { - pt_tmp.x = src(0, i); - pt_tmp.y = src(1, i); - pt_tmp.z = src(2, i); - cloud.points.emplace_back(pt_tmp); - } -} - -#endif // CONVERSION_HPP diff --git a/include/fpfh_manager.hpp b/include/fpfh_manager.hpp deleted file mode 100644 index ed39b26..0000000 --- a/include/fpfh_manager.hpp +++ /dev/null @@ -1,240 +0,0 @@ -// -// Created by Hyungtae Lim on 1/24/22. -// - -#ifndef FPFH_MANAGER_H -#define FPFH_MANAGER_H - - -#include -#include -#include -#include -#include -#include -#include "conversion.hpp" -#include "teaser_utils/fpfh.h" -#include "teaser_utils/feature_matcher.h" -#include "boost/format.hpp" -#include -#include -#include "utility.h" - -using namespace std; - -class FPFHManager -{ -private: - teaser::FPFHCloudPtr obj_descriptors; - teaser::FPFHCloudPtr scene_descriptors; - -public: - teaser::PointCloud src_cloud, tgt_cloud; - double normal_radius_; - double fpfh_radius_; - bool is_initial_ = true; - bool is_odometry_test_ = false; - int interval_; - std::string savedir_; - std::string loaddir_; - std::vector> corr; // Correspondence - - Eigen::Matrix src_matched; - Eigen::Matrix tgt_matched; - - Eigen::Matrix src_normals; // Not in use - Eigen::Matrix tgt_normals; - pcl::PointCloud src_matched_pcl; - pcl::PointCloud tgt_matched_pcl; - - FPFHManager(double normal_radius, double fpfh_radius, int interval=1): normal_radius_(normal_radius), fpfh_radius_(fpfh_radius), interval_(interval) { - obj_descriptors.reset(new pcl::PointCloud()); - scene_descriptors.reset(new pcl::PointCloud()); - is_initial_ = true; - } - - FPFHManager(){ - obj_descriptors.reset(new pcl::PointCloud()); - scene_descriptors.reset(new pcl::PointCloud()); - is_initial_ = true; - } - - ~FPFHManager(){ - normal_radius_ = 0.5; - fpfh_radius_ = 0.6; - interval_ = 1; - obj_descriptors.reset(new pcl::PointCloud()); - scene_descriptors.reset(new pcl::PointCloud()); - is_initial_ = true; - } - void flushAllFeatures(){ - is_initial_ = true; - } - - void swapTgt2Src(){ - src_cloud = tgt_cloud; - *obj_descriptors = *scene_descriptors; - } - void setParams(float normal_radius, float fpfh_radius, int interval){ - normal_radius_ = normal_radius; - fpfh_radius_ = fpfh_radius; - interval_ = interval; - } - - void clearInputs(){ - is_initial_ = true; - src_cloud.clear(); - tgt_cloud.clear(); - obj_descriptors->clear(); - scene_descriptors->clear(); - } - void setLoadDir(std::string loaddir){ - loaddir_ = loaddir; - } - void setSaveDir(std::string savedir){ - savedir_ = savedir; - } - - void setFeaturePair(pcl::PointCloud::Ptr src, pcl::PointCloud::Ptr target){ - if (normal_radius_ > fpfh_radius_){ - std::cout< "<< fpfh_radius_<::Ptr src_normals_raw(new pcl::PointCloud); - pcl::PointCloud::Ptr tgt_normals_raw(new pcl::PointCloud); - - std::chrono::system_clock::time_point start = std::chrono::system_clock::now(); - - // Set FPFH - static teaser::FPFHEstimation fpfh; - if (is_initial_ && !is_odometry_test_) { - pcl2teaser(*src, src_cloud); - obj_descriptors = fpfh.computeFPFHFeatures(src_cloud, *src_normals_raw, normal_radius_, fpfh_radius_); - is_initial_ = false; - }else{ - // To reduce computational cost on odometry test - swapTgt2Src(); - } - pcl2teaser(*target, tgt_cloud); - - scene_descriptors = fpfh.computeFPFHFeatures(tgt_cloud, *tgt_normals_raw, normal_radius_, fpfh_radius_); - - std::chrono::system_clock::time_point end_extraction = std::chrono::system_clock::now(); - - teaser::Matcher matcher; - corr = matcher.calculateCorrespondences( - src_cloud, tgt_cloud, *obj_descriptors, *scene_descriptors, true, true, true, 0.95); - - std::chrono::system_clock::time_point end_matching = std::chrono::system_clock::now(); - - // --------------------------- - src_matched.resize(3, corr.size()); - tgt_matched.resize(3, corr.size()); - tgt_normals.resize(3, corr.size()); - - for (size_t i = 0; i < corr.size(); ++i) { - auto src_idx = std::get<0>(corr[i]); - auto dst_idx = std::get<1>(corr[i]); - src_matched.col(i) << src_cloud[src_idx].x, src_cloud[src_idx].y, src_cloud[src_idx].z; - tgt_matched.col(i) << tgt_cloud[dst_idx].x, tgt_cloud[dst_idx].y, tgt_cloud[dst_idx].z; - - tgt_normals.col(i) << double(tgt_normals_raw->points[dst_idx].normal_x), double(tgt_normals_raw->points[dst_idx].normal_y), double(tgt_normals_raw->points[dst_idx].normal_z); - } - - std::chrono::duration sec = std::chrono::system_clock::now() - start; - std::chrono::duration sec_extraction = end_extraction - start; - std::chrono::duration sec_matching = end_matching - end_extraction; - std::cout << setprecision(4) << "FPFH takes: " << sec.count() << " sec. "; - std::cout << "(Extraction: " << sec_extraction.count() << " sec. + Matching: " << sec_matching.count() << " sec.)" << std::endl; - - eigen2pcl(src_matched, src_matched_pcl); - eigen2pcl(tgt_matched, tgt_matched_pcl); - } - Eigen::Matrix getSrcMatched(){ - return src_matched; - } - Eigen::Matrix getTgtMatched(){ - return tgt_matched; - } - - Eigen::Matrix getTgtNormals(){ - return tgt_normals; - } - - pcl::PointCloud getObjDescriptor(){ - return *obj_descriptors; - } - pcl::PointCloud getSceneDescriptor(){ - return *scene_descriptors; - } - - pcl::PointCloud getSrcKps(){ - return src_matched_pcl; - } - pcl::PointCloud getTgtKps(){ - return tgt_matched_pcl; - } - - void saveFeaturePair(int src_idx, int tgt_idx, bool verbose=false){ - if (savedir_.empty()){ - throw invalid_argument("Save dir. is not set"); - } - std::string pcdname = (boost::format("%s/%06d_to_%06d.pcd") % savedir_ % src_idx %tgt_idx).str(); -// std::string txtname = (boost::format("%s/%06d_to_%06d.txt") % savedir_ % src_idx %tgt_idx).str(); - if (verbose){ - std::cout<<"[SAVER]: "< merge = src_matched_pcl + tgt_matched_pcl; - pcl::io::savePCDFile(pcdname, merge); - -// ofstream num_check; -// num_check.open (txtname); -// num_check << src_matched_pcl.points.size() << " " << tgt_matched_pcl.points.size()< merge; - - if (pcl::io::loadPCDFile(pcdname, merge) == -1){ - throw invalid_argument("[FPFHManager]: Load feature set failed."); - } - - if (verbose){ - std::cout<<"[LOADER]: Loaded data from "<"<< src_matched_pcl.points.size()<<" " <> getCorrespondences(){ - return corr; - } - -}; - -#endif //FPFH_MANAGER_H diff --git a/include/imageProjection.hpp b/include/imageProjection.hpp deleted file mode 100755 index fd674d5..0000000 --- a/include/imageProjection.hpp +++ /dev/null @@ -1,582 +0,0 @@ - -#include "utility.h" -#include "conversion.hpp" - -#define NOT_ASSIGNED -1 -#define USE_FOUR_NEIGHBORS_PIXELS 0 -#define USE_EIGHT_NEIGHBORS_PIXELS 1 -#define USE_FOUR_CROSS_NEIGHBORS_PIXELS 2 - -class ImageProjection { -private: - pcl::PointCloud::Ptr laserCloudIn; - pcl::PointCloud::Ptr fullCloud; // projected velodyne raw cloud, but saved in the form of 1-D matrix - pcl::PointCloud::Ptr fullInfoCloud; // same as fullCloud, but with intensity is replaced with range - - pcl::PointCloud::Ptr groundCloud; - pcl::PointCloud::Ptr segmentedCloud; - pcl::PointCloud::Ptr segmentedCloudPure; - pcl::PointCloud::Ptr outlierCloud; - - PointTypeIP nanPoint; // fill in fullCloud at each iteration - - cv::Mat rangeMat; // range matrix for range image - cv::Mat labelMat; // label matrix for segmentaiton marking - cv::Mat groundMat; // ground matrix for ground cloud marking - - int labelCount; - int numMinPtsForSubclustering = 30; - - float startOrientation; - float endOrientation; - - quatro::cloud_info segMsg; // info of segmented cloud - std_msgs::Header cloudHeader; - - std::vector > neighborIterator; // neighbor iterator for segmentaiton process - - uint16_t *allPushedIndX; // array for tracking points of a segmented object - uint16_t *allPushedIndY; - - uint16_t *queueIndX; // array for breadth-first search process of segmentation - uint16_t *queueIndY; - -public: - int neighborPixelsSelectionMode = NOT_ASSIGNED; - string groundSegMode; - - int N_SCAN; - int Horizon_SCAN; //1028~4500 - float ang_res_x; - float ang_res_y; //28.0/float(N_SCAN-1); - float ang_bottom; - int groundScanInd; - - float segmentAlphaX; - float segmentAlphaY; - - const float sensorMountAngle = 0.0; - //segmentation threshold - const float segmentTheta = 60.0/180.0*M_PI; // decrese this value may improve accuracy - //If number of segment is below than 30, check line number. this for minimum number of point for it - const int segmentValidPointNum = 5; - //if number of segment is small, number of line is checked, this is threshold for it. - const int segmentValidLineNum = 3; - - - ImageProjection() { - nanPoint.x = std::numeric_limits::quiet_NaN(); - nanPoint.y = std::numeric_limits::quiet_NaN(); - nanPoint.z = std::numeric_limits::quiet_NaN(); - nanPoint.intensity = -1; - - allocateMemory(); - resetParameters(); - } - - ImageProjection( - string lidarType, string neighborSelectionMode, string groundSegmentationMode, int numSubclusteringCriteria = 30) { - nanPoint.x = std::numeric_limits::quiet_NaN(); - nanPoint.y = std::numeric_limits::quiet_NaN(); - nanPoint.z = std::numeric_limits::quiet_NaN(); - nanPoint.intensity = -1; - - std::cout << "[Lidar Type]: " << lidarType << std::endl; - if(lidarType == "Velodyne-64-HDE"){ - N_SCAN = 64; - Horizon_SCAN = 1800; //1028~4500 - ang_res_x = 360.0/float(Horizon_SCAN); - ang_res_y = 26.9/float(N_SCAN-1); //28.0/float(N_SCAN-1); - ang_bottom = 25.0; - groundScanInd = 60; - } - else if(lidarType == "VLP-16"){ - N_SCAN = 16; - Horizon_SCAN = 1800; - ang_res_x = 0.2; - ang_res_y = 2.0; - ang_bottom = 15.0+0.1; - groundScanInd = 7; - } - else if(lidarType == "HDL-32E"){ - N_SCAN = 32; - Horizon_SCAN = 1800; - ang_res_x = 360.0/float(Horizon_SCAN); - ang_res_y = 41.33/float(N_SCAN-1); - ang_bottom = 30.67; - groundScanInd = 20; - } - else if(lidarType == "Ouster-OS1-16"){ - N_SCAN = 16; - Horizon_SCAN = 1024; - ang_res_x = 360.0/float(Horizon_SCAN); - ang_res_y = 33.2/float(N_SCAN-1); - ang_bottom = 16.6+0.1; - groundScanInd = 7; - } - else if(lidarType == "Ouster-OS1-64"){ - N_SCAN = 64; - Horizon_SCAN = 1024; - ang_res_x = 360.0/float(Horizon_SCAN); - ang_res_y = 33.2/float(N_SCAN-1); - ang_bottom = 16.6+0.1; - groundScanInd = 15; - } - else throw invalid_argument("[ImageProjection]:Check your paramter. Lidar Type is wrong!"); - segmentAlphaX = ang_res_x / 180.0 * M_PI; - segmentAlphaY = ang_res_y / 180.0 * M_PI; - - std::cout << "[NEIGHBOR Mode]: " << neighborSelectionMode << std::endl; - if (neighborSelectionMode == "4Neighbor") neighborPixelsSelectionMode = USE_FOUR_NEIGHBORS_PIXELS; - else if (neighborSelectionMode == "8Neighbor") neighborPixelsSelectionMode = USE_EIGHT_NEIGHBORS_PIXELS; - else if (neighborSelectionMode == "4CrossNeighbor") neighborPixelsSelectionMode = USE_FOUR_CROSS_NEIGHBORS_PIXELS; - else throw invalid_argument("[ImageProjection]:Check your paramter. Neighbor selection mode is wrong!"); - - groundSegMode = groundSegmentationMode; - if (!(groundSegMode == "LeGO-LOAM") && !(groundSegMode == "Patchwork") ) { - throw invalid_argument("[ImageProjection]: Check your paramter. Ground Segmentation mode is wrong!"); - } - - numMinPtsForSubclustering = numSubclusteringCriteria; - - - allocateMemory(); - resetParameters(); - } - - void allocateMemory() { - - laserCloudIn.reset(new pcl::PointCloud()); - - fullCloud.reset(new pcl::PointCloud()); - fullInfoCloud.reset(new pcl::PointCloud()); - - groundCloud.reset(new pcl::PointCloud()); - segmentedCloud.reset(new pcl::PointCloud()); - segmentedCloudPure.reset(new pcl::PointCloud()); - outlierCloud.reset(new pcl::PointCloud()); - - fullCloud->points.resize(N_SCAN * Horizon_SCAN); - fullInfoCloud->points.resize(N_SCAN * Horizon_SCAN); - - segMsg.startRingIndex.assign(N_SCAN, 0); - segMsg.endRingIndex.assign(N_SCAN, 0); - - segMsg.segmentedCloudGroundFlag.assign(N_SCAN * Horizon_SCAN, false); - segMsg.segmentedCloudColInd.assign(N_SCAN * Horizon_SCAN, 0); - segMsg.segmentedCloudRange.assign(N_SCAN * Horizon_SCAN, 0); - - // 4 neigbor labeling - cout << "=> neighborPixelsSelectionMode: "; - if (neighborPixelsSelectionMode == USE_FOUR_NEIGHBORS_PIXELS) { - cout << "USE_FOUR_NEIGHBORS_PIXELS" << endl; - neighborIterator.push_back({-1, 0}); - neighborIterator.push_back({0, 1}); - neighborIterator.push_back({0, -1}); - neighborIterator.push_back({1, 0}); - - } else if (neighborPixelsSelectionMode == USE_EIGHT_NEIGHBORS_PIXELS) { - cout << "USE_EIGHT_NEIGHBORS_PIXELS" << endl; - neighborIterator.push_back({-1, 0}); - neighborIterator.push_back({0, 1}); - neighborIterator.push_back({0, -1}); - neighborIterator.push_back({1, 0}); - neighborIterator.push_back({-1, -1}); - neighborIterator.push_back({-1, 1}); - neighborIterator.push_back({1, 1}); - neighborIterator.push_back({1, -1}); - - } else if (neighborPixelsSelectionMode == USE_FOUR_CROSS_NEIGHBORS_PIXELS) { - cout << "USE_FOUR_CROSS_NEIGHBORS_PIXELS" << endl; - neighborIterator.push_back({-1, -1}); - neighborIterator.push_back({-1, 1}); - neighborIterator.push_back({1, 1}); - neighborIterator.push_back({1, -1}); - } else { cout << endl; } - - allPushedIndX = new uint16_t[N_SCAN * Horizon_SCAN]; - allPushedIndY = new uint16_t[N_SCAN * Horizon_SCAN]; - - queueIndX = new uint16_t[N_SCAN * Horizon_SCAN]; - queueIndY = new uint16_t[N_SCAN * Horizon_SCAN]; - } - - void resetParameters() { - laserCloudIn->clear(); - groundCloud->clear(); - segmentedCloud->clear(); - segmentedCloudPure->clear(); - outlierCloud->clear(); - - rangeMat = cv::Mat(N_SCAN, Horizon_SCAN, CV_32F, cv::Scalar::all(FLT_MAX)); - groundMat = cv::Mat(N_SCAN, Horizon_SCAN, CV_8S, cv::Scalar::all(0)); - labelMat = cv::Mat(N_SCAN, Horizon_SCAN, CV_32S, cv::Scalar::all(0)); - labelCount = 1; - - std::fill(fullCloud->points.begin(), fullCloud->points.end(), nanPoint); - std::fill(fullInfoCloud->points.begin(), fullInfoCloud->points.end(), nanPoint); - } - - void getValidSegments(pcl::PointCloud& output) { - pcl::copyPointCloud(*segmentedCloudPure, output); - } - - void getValidSegments(pcl::PointCloud& output) { - output = *segmentedCloudPure; - } - - void getGround(pcl::PointCloud& output) { - pcl::copyPointCloud(*groundCloud, output); - } - - void getGround(pcl::PointCloud& output) { - output = *groundCloud; - } - - void getOutliers(pcl::PointCloud& output) { - pcl::copyPointCloud(*outlierCloud, output); - } - - void getOutliers(pcl::PointCloud& output) { - output = *outlierCloud; - } - - cv::Mat getrangeMat() { - return rangeMat; - } - - - ~ImageProjection() {} - - void setPointCloud(const pcl::PointCloud::Ptr srcPtr) { - (*laserCloudIn).points.resize((*srcPtr).size()); - for (size_t i = 0; i < (*srcPtr).size(); i++) { - (*laserCloudIn).points[i].x = (*srcPtr).points[i].x; - (*laserCloudIn).points[i].y = (*srcPtr).points[i].y; - (*laserCloudIn).points[i].z = (*srcPtr).points[i].z; - } - } - - void copyPointCloud(const pcl::PointCloud::Ptr srcPtr) { - setPointCloud(srcPtr); - - // Remove Nan points - std::vector indices; - pcl::removeNaNFromPointCloud(*laserCloudIn, *laserCloudIn, indices); - } - - // (Ground Removal + Sub Clustering) - /*** - * Ground Removal + Subclustering - * @param pcPtr - */ - void segmentCloud(const pcl::PointCloud::Ptr pcPtr) { - // 1. Copy pcl point cloud - copyPointCloud(pcPtr); - // 2. Start and end angle of a scan - findStartEndAngle(); - // 3. Range image projection - projectPointCloud(); - // 4. Mark ground points - /*** - * If groundSegMode == "Patchwork", - * then the ground points are already rejected by Patchwork - * i.e. srcPtr does not contain ground points - * Thus, only masking labelMat is performed - */ - if (groundSegMode == "LeGO-LOAM") { - groundRemoval(); - } else if (groundSegMode == "Patchwork") { - maskGround(); - } - // 5. Point cloud segmentation - cloudSegmentation(); - } - - void findStartEndAngle() { - // start and end orientation of this cloud - segMsg.startOrientation = -atan2(laserCloudIn->points[0].y, laserCloudIn->points[0].x); - segMsg.endOrientation = -atan2(laserCloudIn->points[laserCloudIn->points.size() - 1].y, - laserCloudIn->points[laserCloudIn->points.size() - 1].x) + 2 * M_PI; - if (segMsg.endOrientation - segMsg.startOrientation > 3 * M_PI) { - segMsg.endOrientation -= 2 * M_PI; - } else if (segMsg.endOrientation - segMsg.startOrientation < M_PI) - segMsg.endOrientation += 2 * M_PI; - segMsg.orientationDiff = segMsg.endOrientation - segMsg.startOrientation; - } - - void projectPointCloud() { - // range image projection - float verticalAngle, horizonAngle, range; - size_t rowIdn, columnIdn, index, cloudSize; - PointTypeIP thisPoint; - - cloudSize = laserCloudIn->points.size(); - - for (size_t i = 0; i < cloudSize; ++i) { - - thisPoint.x = laserCloudIn->points[i].x; - thisPoint.y = laserCloudIn->points[i].y; - thisPoint.z = laserCloudIn->points[i].z; - - // find the row and column index in the iamge for this point - verticalAngle = atan2(thisPoint.z, sqrt(thisPoint.x * thisPoint.x + thisPoint.y * thisPoint.y)) * 180 / M_PI; - rowIdn = (verticalAngle + ang_bottom) / ang_res_y; - - if (rowIdn < 0 || rowIdn >= N_SCAN) - continue; - - horizonAngle = atan2(thisPoint.x, thisPoint.y) * 180 / M_PI; - columnIdn = -round((horizonAngle - 90.0) / ang_res_x) + Horizon_SCAN / 2; - if (columnIdn >= Horizon_SCAN) - columnIdn -= Horizon_SCAN; - - if (columnIdn < 0 || columnIdn >= Horizon_SCAN) - continue; - - range = sqrt(thisPoint.x * thisPoint.x + thisPoint.y * thisPoint.y + thisPoint.z * thisPoint.z); - if (range < 0.1) - continue; - - rangeMat.at(rowIdn, columnIdn) = range; - - // ??? - thisPoint.intensity = (float) rowIdn + (float) columnIdn / 10000.0; - - index = columnIdn + rowIdn * Horizon_SCAN; - fullCloud->points[index] = thisPoint; - fullInfoCloud->points[index] = thisPoint; - fullInfoCloud->points[index].intensity = range; // the corresponding range of a point is saved as "intensity" - } - - } - - void maskGround() { - // The pixels where the points are not projected are masked (i.e. rangeMat.at(i, j) == FLT_MAX) - for (size_t i = 0; i < N_SCAN; ++i) { - for (size_t j = 0; j < Horizon_SCAN; ++j) { - if (groundMat.at(i, j) == 1 || rangeMat.at(i, j) == FLT_MAX) { - labelMat.at(i, j) = -1; - } - } - } - } - - void groundRemoval() { - size_t lowerInd, upperInd; - float diffX, diffY, diffZ, angle; - // groundMat - // -1, no valid info to check if ground of not - // 0, initial value, after validation, means not ground - // 1, ground - for (size_t j = 0; j < Horizon_SCAN; ++j) { - for (size_t i = 0; i < groundScanInd; ++i) { - - // lowerInd : 현재 채널에서, 수평한 방향으로 포인트 인덱스 - // upperInd : lowerInd에서 z방향 바로 위 포인트 인덱스 - lowerInd = j + (i) * Horizon_SCAN; - upperInd = j + (i + 1) * Horizon_SCAN; - - if (fullCloud->points[lowerInd].intensity == -1 || - fullCloud->points[upperInd].intensity == -1) { - // no info to check, invalid points - groundMat.at(i, j) = -1; - continue; - } - - // lowerInd에서 upperInd까지 이루는 각도 : angle - diffX = fullCloud->points[upperInd].x - fullCloud->points[lowerInd].x; - diffY = fullCloud->points[upperInd].y - fullCloud->points[lowerInd].y; - diffZ = fullCloud->points[upperInd].z - fullCloud->points[lowerInd].z; - angle = atan2(diffZ, sqrt(diffX * diffX + diffY * diffY)) * 180 / M_PI; - - // angle이 theshold를 넘지 못하면 ground로 판단. - if (abs(angle - sensorMountAngle) <= 10) { - groundMat.at(i, j) = 1; - groundMat.at(i + 1, j) = 1; - } - } - } - - // extract ground cloud (groundMat == 1) - // mark entry that doesn't need to label (ground and invalid point) for segmentation - // note that ground remove is from 0~N_SCAN-1, need rangeMat for mark label matrix for the 16th scan - - // 거리가 매우 먼 포인트와 ground 판단되는 포인트들은 invalid로 설정 - for (size_t i = 0; i < N_SCAN; ++i) { - for (size_t j = 0; j < Horizon_SCAN; ++j) { - if (groundMat.at(i, j) == 1 || rangeMat.at(i, j) == FLT_MAX) { - labelMat.at(i, j) = -1; - } - } - } - - // if (pubGroundCloud.getNumSubscribers() != 0){ - for (size_t i = 0; i <= groundScanInd; ++i) { - for (size_t j = 0; j < Horizon_SCAN; ++j) { - if (groundMat.at(i, j) == 1) - groundCloud->push_back(fullCloud->points[j + i * Horizon_SCAN]); - } - } - // } - } - - void cloudSegmentation() { - // segmentation process - for (size_t i = 0; i < N_SCAN; ++i) - for (size_t j = 0; j < Horizon_SCAN; ++j) - if (labelMat.at(i, j) == 0) - labelComponents(i, j); - - // label: 999999 -> invalid - // label: over 0 -> valid - - int sizeOfSegCloud = 0; - // extract segmented cloud for lidar odometry - for (size_t i = 0; i < N_SCAN; ++i) { - - segMsg.startRingIndex[i] = sizeOfSegCloud - 1 + 5; - - for (size_t j = 0; j < Horizon_SCAN; ++j) { - if (labelMat.at(i, j) > 0 || groundMat.at(i, j) == 1) { - // outliers that will not be used for optimization (always continue) - if (labelMat.at(i, j) == 999999) { - if (j % 1 == 0) { // i > groundScanInd && j % 5 == 0 - outlierCloud->push_back(fullCloud->points[j + i * Horizon_SCAN]); - continue; - } else { - continue; - } - } - // majority of ground points are skipped - if (groundMat.at(i, j) == 1) { - if (j % 5 != 0 && j > 5 && j < Horizon_SCAN - 5) - continue; - } - // mark ground points so they will not be considered as edge features later - segMsg.segmentedCloudGroundFlag[sizeOfSegCloud] = (groundMat.at(i, j) == 1); - // mark the points' column index for marking occlusion later - segMsg.segmentedCloudColInd[sizeOfSegCloud] = j; - // save range info - segMsg.segmentedCloudRange[sizeOfSegCloud] = rangeMat.at(i, j); - // save seg cloud - segmentedCloud->push_back(fullCloud->points[j + i * Horizon_SCAN]); - // size of seg cloud - ++sizeOfSegCloud; - } - } - segMsg.endRingIndex[i] = sizeOfSegCloud - 1 - 5; - } - - // extract segmented cloud for visualization - // 해당토픽을 subscribe하지 않으면, 처리하지 않고 publish안함(아래코드) - // if (pubSegmentedCloudPure.getNumSubscribers() != 0){ - for (size_t i = 0; i < N_SCAN; ++i) { - for (size_t j = 0; j < Horizon_SCAN; ++j) { - if (labelMat.at(i, j) > 0 && labelMat.at(i, j) != 999999) { // - segmentedCloudPure->push_back(fullCloud->points[j + i * Horizon_SCAN]); - segmentedCloudPure->points.back().intensity = labelMat.at(i, j); - } - } - } - // } - } - - void labelComponents(int row, int col) { - // use std::queue std::vector std::deque will slow the program down greatly - float d1, d2, alpha, angle; - int fromIndX, fromIndY, thisIndX, thisIndY; - bool lineCountFlag[N_SCAN] = {false}; - - queueIndX[0] = row; - queueIndY[0] = col; - int queueSize = 1; - int queueStartInd = 0; - int queueEndInd = 1; - - allPushedIndX[0] = row; - allPushedIndY[0] = col; - int allPushedIndSize = 1; - - while (queueSize > 0) { - // Pop point - fromIndX = queueIndX[queueStartInd]; - fromIndY = queueIndY[queueStartInd]; - --queueSize; - ++queueStartInd; - // Mark popped point - labelMat.at(fromIndX, fromIndY) = labelCount; - // Loop through all the neighboring grids of popped grid - for (auto iter = neighborIterator.begin(); iter != neighborIterator.end(); ++iter) { - // new index - thisIndX = fromIndX + (*iter).first; - thisIndY = fromIndY + (*iter).second; - // index should be within the boundary - if (thisIndX < 0 || thisIndX >= N_SCAN) - continue; - // at range image margin (left or right side) - if (thisIndY < 0) - thisIndY = Horizon_SCAN - 1; - if (thisIndY >= Horizon_SCAN) - thisIndY = 0; - // prevent infinite loop (caused by put already examined point back) - if (labelMat.at(thisIndX, thisIndY) != 0) - continue; - - - //두포인트의 range 값 - d1 = std::max(rangeMat.at(fromIndX, fromIndY), - rangeMat.at(thisIndX, thisIndY)); - d2 = std::min(rangeMat.at(fromIndX, fromIndY), - rangeMat.at(thisIndX, thisIndY)); - - if ((*iter).first == 0) - alpha = segmentAlphaX; // velodyne64 : segmentAlphaX = 0.2deg - else - alpha = segmentAlphaY; - - // beta값 - angle = atan2(d2 * sin(alpha), (d1 - d2 * cos(alpha))); - - if (angle > segmentTheta) { - - queueIndX[queueEndInd] = thisIndX; - queueIndY[queueEndInd] = thisIndY; - ++queueSize; - ++queueEndInd; - - labelMat.at(thisIndX, thisIndY) = labelCount; - lineCountFlag[thisIndX] = true; - - allPushedIndX[allPushedIndSize] = thisIndX; - allPushedIndY[allPushedIndSize] = thisIndY; - ++allPushedIndSize; - } - } - } - // check if this segment is valid - bool feasibleSegment = false; - /*** - * once the # of allPushedIndSize is smaller than numMinPtsForSubclustering, - * it is considered as outliers (rejected) - */ - if (allPushedIndSize >= numMinPtsForSubclustering) - feasibleSegment = true; - else if (allPushedIndSize >= segmentValidPointNum) { - int lineCount = 0; - for (size_t i = 0; i < N_SCAN; ++i) - if (lineCountFlag[i] == true) - ++lineCount; - if (lineCount >= segmentValidLineNum) - feasibleSegment = true; - } - // segment is valid, mark these points - if (feasibleSegment == true) { - ++labelCount; - } else { // segment is invalid, mark these points - for (size_t i = 0; i < allPushedIndSize; ++i) { - labelMat.at(allPushedIndX[i], allPushedIndY[i]) = 999999; - } - } - } -}; \ No newline at end of file diff --git a/include/patchwork.hpp b/include/patchwork.hpp deleted file mode 100755 index b09ad89..0000000 --- a/include/patchwork.hpp +++ /dev/null @@ -1,634 +0,0 @@ -#ifndef PATCHWORK_H -#define PATCHWORK_H - -#include -#include -#include -#include -#include -#include -#include -#include - -#define MARKER_Z_VALUE -2.2 -#define UPRIGHT_ENOUGH 0.55 // cyan -#define FLAT_ENOUGH 0.2 // green -#define TOO_HIGH_ELEVATION 0.0 // blue -#define TOO_TILTED 1.0 // red -#define GLOBALLLY_TOO_HIGH_ELEVATION_THR 0.8 - -#define NUM_HEURISTIC_MAX_PTS_IN_PATCH 3000 - -using Eigen::MatrixXf; -using Eigen::JacobiSVD; -using Eigen::VectorXf; - -using namespace std; - -/* - @brief PathWork ROS Node. -*/ -template -bool point_z_cmp(PointT a, PointT b) { - return a.z < b.z; -} - -template -class PatchWork { - -public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW - - typedef std::vector > Ring; - typedef std::vector Zone; - - PatchWork() {}; - - PatchWork(ros::NodeHandle *nh) : node_handle_(*nh) { - // Init ROS related - ROS_INFO("Inititalizing PatchWork..."); - - node_handle_.param("/sensor_height", sensor_height_, 1.723); - node_handle_.param("/patchwork/verbose", verbose_, false); - - node_handle_.param("/patchwork/num_iter", num_iter_, 3); - node_handle_.param("/patchwork/num_lpr", num_lpr_, 20); - node_handle_.param("/patchwork/num_min_pts", num_min_pts_, 10); - node_handle_.param("/patchwork/th_seeds", th_seeds_, 0.4); - node_handle_.param("/patchwork/th_dist", th_dist_, 0.3); - node_handle_.param("/patchwork/max_r", max_range_, 80.0); - node_handle_.param("/patchwork/min_r", min_range_, 2.7); // It indicates bodysize of the car. - node_handle_.param("/patchwork/uniform/num_rings", num_rings_, 30); - node_handle_.param("/patchwork/uniform/num_sectors", num_sectors_, 108); - node_handle_.param("/patchwork/uprightness_thr", uprightness_thr_, 0.5); // The larger, the more strict - node_handle_.param("/patchwork/adaptive_seed_selection_margin", adaptive_seed_selection_margin_, - -1.1); // The more larger, the more soft - - // It is not in the paper - // It is also not matched our philosophy, but it is employed to reject some FPs easily & intuitively. - // For patchwork, it is only applied on Z3 and Z4 - node_handle_.param("/patchwork/using_global_elevation", using_global_thr_, true); - node_handle_.param("/patchwork/global_elevation_threshold", global_elevation_thr_, 0.0); - - if (using_global_thr_) { - cout << "\033[1;33m[Warning] Global elevation threshold is turned on :" << global_elevation_thr_ << "\033[0m" << endl; - } else { cout << "Global thr. is not in use" << endl; } - - ROS_INFO("Sensor Height: %f", sensor_height_); - ROS_INFO("Num of Iteration: %d", num_iter_); - ROS_INFO("Num of LPR: %d", num_lpr_); - ROS_INFO("Num of min. points: %d", num_min_pts_); - ROS_INFO("Seeds Threshold: %f", th_seeds_); - ROS_INFO("Distance Threshold: %f", th_dist_); - ROS_INFO("Max. range:: %f", max_range_); - ROS_INFO("Min. range:: %f", min_range_); - ROS_INFO("Num. rings: %d", num_rings_); - ROS_INFO("Num. sectors: %d", num_sectors_); - ROS_INFO("adaptive_seed_selection_margin: %f", adaptive_seed_selection_margin_); - - // CZM denotes 'Concentric Zone Model'. Please refer to our paper - node_handle_.getParam("/patchwork/czm/num_zones", num_zones_); - node_handle_.getParam("/patchwork/czm/num_sectors_each_zone", num_sectors_each_zone_); - node_handle_.getParam("/patchwork/czm/num_rings_each_zone", num_rings_each_zone_); - node_handle_.getParam("/patchwork/czm/min_ranges_each_zone", min_ranges_); - node_handle_.getParam("/patchwork/czm/elevation_thresholds", elevation_thr_); - node_handle_.getParam("/patchwork/czm/flatness_thresholds", flatness_thr_); - - - ROS_INFO("\033[1;32mUprightness\33[0m threshold: %f", uprightness_thr_); - ROS_INFO("\033[1;32mElevation\33[0m thresholds: %f %f %f %f", elevation_thr_[0],elevation_thr_[1], elevation_thr_[2], elevation_thr_[3]); - ROS_INFO("\033[1;32mFlatness\033[0m thresholds: %f %f %f %f", flatness_thr_[0], flatness_thr_[1], flatness_thr_[2], flatness_thr_[3]); - - ROS_INFO("Num. zones: %d", num_zones_); - - check_input_parameters_are_correct(); -// cout_params(); - - // It equals to elevation_thr_.size()/flatness_thr_.size(); - num_rings_of_interest_ = elevation_thr_.size(); - - node_handle_.param("/patchwork/visualize", visualize_, true); - - revert_pc.reserve(NUM_HEURISTIC_MAX_PTS_IN_PATCH); - ground_pc_.reserve(NUM_HEURISTIC_MAX_PTS_IN_PATCH); - non_ground_pc_.reserve(NUM_HEURISTIC_MAX_PTS_IN_PATCH); - regionwise_ground_.reserve(NUM_HEURISTIC_MAX_PTS_IN_PATCH); - regionwise_nonground_.reserve(NUM_HEURISTIC_MAX_PTS_IN_PATCH); - - revert_pc_pub = node_handle_.advertise("/revert_pc", 100); - reject_pc_pub = node_handle_.advertise("/reject_pc", 100); - - min_range_z2_ = min_ranges_[1]; - min_range_z3_ = min_ranges_[2]; - min_range_z4_ = min_ranges_[3]; - - min_ranges_ = {min_range_, min_range_z2_, min_range_z3_, min_range_z4_}; - ring_sizes_ = {(min_range_z2_ - min_range_) / num_rings_each_zone_.at(0), - (min_range_z3_ - min_range_z2_) / num_rings_each_zone_.at(1), - (min_range_z4_ - min_range_z3_) / num_rings_each_zone_.at(2), - (max_range_ - min_range_z4_) / num_rings_each_zone_.at(3)}; - sector_sizes_ = {2 * M_PI / num_sectors_each_zone_.at(0), 2 * M_PI / num_sectors_each_zone_.at(1), - 2 * M_PI / num_sectors_each_zone_.at(2), - 2 * M_PI / num_sectors_each_zone_.at(3)}; - cout << "INITIALIZATION COMPLETE" << endl; - - for (int iter = 0; iter < num_zones_; ++iter) { - Zone z; - initialize_zone(z, num_sectors_each_zone_.at(iter), num_rings_each_zone_.at(iter)); - ConcentricZoneModel_.push_back(z); - } - } - - void estimate_ground( - const pcl::PointCloud &cloudIn, - pcl::PointCloud &cloudOut, - pcl::PointCloud &cloudNonground, - double &time_taken); - -private: - ros::NodeHandle node_handle_; - - int num_iter_; - int num_lpr_; - int num_min_pts_; - int num_rings_; - int num_sectors_; - int num_zones_; - int num_rings_of_interest_; - - double sensor_height_; - double th_seeds_; - double th_dist_; - double max_range_; - double min_range_; - double uprightness_thr_; - double adaptive_seed_selection_margin_; - double min_range_z2_; // 12.3625 - double min_range_z3_; // 22.025 - double min_range_z4_; // 41.35 - - bool verbose_; - - // For global threshold - bool using_global_thr_; - double global_elevation_thr_; - - float d_; - MatrixXf normal_; - VectorXf singular_values_; - float th_dist_d_; - Eigen::Matrix3f cov_; - Eigen::Vector4f pc_mean_; - double ring_size; - double sector_size; - // For visualization - bool visualize_; - - vector num_sectors_each_zone_; - vector num_rings_each_zone_; - - vector sector_sizes_; - vector ring_sizes_; - vector min_ranges_; - vector elevation_thr_; - vector flatness_thr_; - - vector ConcentricZoneModel_; - - ros::Publisher PlaneViz, revert_pc_pub, reject_pc_pub; - pcl::PointCloud revert_pc, reject_pc; - pcl::PointCloud ground_pc_; - pcl::PointCloud non_ground_pc_; - - pcl::PointCloud regionwise_ground_; - pcl::PointCloud regionwise_nonground_; - - void check_input_parameters_are_correct(); - - void cout_params(); - - void initialize_zone(Zone &z, int num_sectors, int num_rings); - - void flush_patches_in_zone(Zone &patches, int num_sectors, int num_rings); - - double calc_principal_variance(const Eigen::Matrix3f &cov, const Eigen::Vector4f ¢roid); - - double xy2theta(const double &x, const double &y); - - double xy2radius(const double &x, const double &y); - - void pc2czm(const pcl::PointCloud &src, std::vector &czm); - - void estimate_plane_(const pcl::PointCloud &ground); - - void extract_piecewiseground( - const int zone_idx, const pcl::PointCloud &src, - pcl::PointCloud &dst, - pcl::PointCloud &non_ground_dst); - - void estimate_plane_(const int zone_idx, const pcl::PointCloud &ground); - - void extract_initial_seeds_( - const int zone_idx, const pcl::PointCloud &p_sorted, - pcl::PointCloud &init_seeds); -}; - - -template -inline -void PatchWork::initialize_zone(Zone &z, int num_sectors, int num_rings) { - z.clear(); - pcl::PointCloud cloud; - cloud.reserve(1000); - Ring ring; - for (int i = 0; i < num_sectors; i++) { - ring.emplace_back(cloud); - } - for (int j = 0; j < num_rings; j++) { - z.emplace_back(ring); - } -} - -template -inline -void PatchWork::flush_patches_in_zone(Zone &patches, int num_sectors, int num_rings) { - for (int i = 0; i < num_sectors; i++) { - for (int j = 0; j < num_rings; j++) { - if (!patches[j][i].points.empty()) patches[j][i].points.clear(); - } - } -} - -template -inline -void PatchWork::estimate_plane_(const pcl::PointCloud &ground) { - pcl::computeMeanAndCovarianceMatrix(ground, cov_, pc_mean_); - // Singular Value Decomposition: SVD - Eigen::JacobiSVD svd(cov_, Eigen::DecompositionOptions::ComputeFullU); - singular_values_ = svd.singularValues(); - - // use the least singular vector as normal - normal_ = (svd.matrixU().col(2)); - // mean ground seeds value - Eigen::Vector3f seeds_mean = pc_mean_.head<3>(); - - // according to normal.T*[x,y,z] = -d - d_ = -(normal_.transpose() * seeds_mean)(0, 0); - // set distance threhold to `th_dist - d` - th_dist_d_ = th_dist_ - d_; -} - -template -inline -void PatchWork::extract_initial_seeds_( - const int zone_idx, const pcl::PointCloud &p_sorted, - pcl::PointCloud &init_seeds) { - init_seeds.points.clear(); - - // LPR is the mean of low point representative - double sum = 0; - int cnt = 0; - - int init_idx = 0; - // Empirically, adaptive seed selection applying to Z1 is fine - static double lowest_h_margin_in_close_zone = (sensor_height_ == 0.0)? -0.1 : adaptive_seed_selection_margin_ * sensor_height_; - if (zone_idx == 0) { - for (int i = 0; i < p_sorted.points.size(); i++) { - if (p_sorted.points[i].z < lowest_h_margin_in_close_zone) { - ++init_idx; - } else { - break; - } - } - } - - // Calculate the mean height value. - for (int i = init_idx; i < p_sorted.points.size() && cnt < num_lpr_; i++) { - sum += p_sorted.points[i].z; - cnt++; - } - double lpr_height = cnt != 0 ? sum / cnt : 0;// in case divide by 0 - - // iterate pointcloud, filter those height is less than lpr.height+th_seeds_ - for (int i = 0; i < p_sorted.points.size(); i++) { - if (p_sorted.points[i].z < lpr_height + th_seeds_) { - init_seeds.points.push_back(p_sorted.points[i]); - } - } -} - - -/* - @brief Velodyne pointcloud callback function. The main GPF pipeline is here. - PointCloud SensorMsg -> Pointcloud -> z-value sorted Pointcloud - ->error points removal -> extract ground seeds -> ground plane fit mainloop -*/ - -template -inline -void PatchWork::estimate_ground( - const pcl::PointCloud &cloud_in, - pcl::PointCloud &cloud_out, - pcl::PointCloud &cloud_nonground, - double &time_taken) { - - static double start, t0, t1, t2, end; - - double t_total_ground = 0.0; - double t_total_estimate = 0.0; - // 1.Msg to pointcloud - pcl::PointCloud laserCloudIn; - laserCloudIn = cloud_in; - - start = ros::Time::now().toSec(); - - // 2.Sort on Z-axis value. 오름차순 - sort(laserCloudIn.points.begin(), laserCloudIn.end(), point_z_cmp); - - t0 = ros::Time::now().toSec(); - // 3.Error point removal - // As there are some error mirror reflection under the ground, - // here regardless point under 1.8* sensor_height - // Sort point according to height, here uses z-axis in default - - // 원하는 높이를 찾는다. - auto it = laserCloudIn.points.begin(); - for (int i = 0; i < laserCloudIn.points.size(); i++) { - if (laserCloudIn.points[i].z < -1.8 * sensor_height_) { - it++; - } else { - break; - } - } - - // 낮은곳 부터 원하는 높이까지 포인트를 제거한다. - laserCloudIn.points.erase(laserCloudIn.points.begin(), it); - - t1 = ros::Time::now().toSec(); - // 4. pointcloud -> regionwise setting - for (int k = 0; k < num_zones_; ++k) { - flush_patches_in_zone(ConcentricZoneModel_[k], num_sectors_each_zone_[k], num_rings_each_zone_[k]); - } - pc2czm(laserCloudIn, ConcentricZoneModel_); - - t2 = ros::Time::now().toSec(); - - cloud_out.clear(); - cloud_nonground.clear(); - revert_pc.clear(); - reject_pc.clear(); - - int concentric_idx = 0; - for (int k = 0; k < num_zones_; ++k) { - auto zone = ConcentricZoneModel_[k]; - for (uint16_t ring_idx = 0; ring_idx < num_rings_each_zone_[k]; ++ring_idx) { - for (uint16_t sector_idx = 0; sector_idx < num_sectors_each_zone_[k]; ++sector_idx) { - if (zone[ring_idx][sector_idx].points.size() > num_min_pts_) { - double t_tmp0 = ros::Time::now().toSec(); - extract_piecewiseground(k, zone[ring_idx][sector_idx], regionwise_ground_, regionwise_nonground_); - double t_tmp1 = ros::Time::now().toSec(); - t_total_ground += t_tmp1 - t_tmp0; - - // Status of each patch - // used in checking uprightness, elevation, and flatness, respectively - const double ground_z_vec = abs(normal_(2, 0)); - const double ground_z_elevation = pc_mean_(2, 0); - const double surface_variable = - singular_values_.minCoeff() / - (singular_values_(0) + singular_values_(1) + singular_values_(2)); - - double t_tmp2 = ros::Time::now().toSec(); - if (ground_z_vec < uprightness_thr_) { - // All points are rejected - cloud_nonground += regionwise_ground_; - cloud_nonground += regionwise_nonground_; - } else { // satisfy uprightness - if (concentric_idx < num_rings_of_interest_) { - if (ground_z_elevation > elevation_thr_[ring_idx + 2 * k]) { - if (flatness_thr_[ring_idx + 2 * k] > surface_variable) { - if (verbose_) { - std::cout << "\033[1;36m[Flatness] Recovery operated. Check " - << ring_idx + 2 * k - << "th param. flatness_thr_: " << flatness_thr_[ring_idx + 2 * k] - << " > " - << surface_variable << "\033[0m" << std::endl; - revert_pc += regionwise_ground_; - } - cloud_out += regionwise_ground_; - cloud_nonground += regionwise_nonground_; - } else { - if (verbose_) { - std::cout << "\033[1;34m[Elevation] Rejection operated. Check " - << ring_idx + 2 * k - << "th param. of elevation_thr_: " << elevation_thr_[ring_idx + 2 * k] - << " < " - << ground_z_elevation << "\033[0m" << std::endl; - reject_pc += regionwise_ground_; - } - cloud_nonground += regionwise_ground_; - cloud_nonground += regionwise_nonground_; - } - } else { - cloud_out += regionwise_ground_; - cloud_nonground += regionwise_nonground_; - } - } else { - // 근처의 땅만 제거 - // cloud_nonground += regionwise_ground_; - // cloud_nonground += regionwise_nonground_; - - if (using_global_thr_ && (ground_z_elevation > global_elevation_thr_)) { - cout << "\033[1;33m[Global elevation] " << ground_z_elevation << " > " << global_elevation_thr_ - << "\033[0m" << endl; - //멀면 그냥제거 - cloud_nonground += regionwise_ground_; - cloud_nonground += regionwise_nonground_; - } else { - cloud_out += regionwise_ground_; - cloud_nonground += regionwise_nonground_; - } - } - } - double t_tmp3 = ros::Time::now().toSec(); - t_total_estimate += t_tmp3 - t_tmp2; - } - } - ++concentric_idx; - } - } - end = ros::Time::now().toSec(); - time_taken = end - start; -// ofstream time_txt("/home/shapelim/patchwork_time_anal.txt", std::ios::app); -// time_txt< -inline -double PatchWork::calc_principal_variance(const Eigen::Matrix3f &cov, const Eigen::Vector4f ¢roid) { - double angle = atan2(centroid(1, 0), centroid(0, 0)); // y, x - double c = cos(angle); - double s = sin(angle); - double var_x_prime = c * c * cov(0, 0) + s * s * cov(1, 1) + 2 * c * s * cov(0, 1); - double var_y_prime = s * s * cov(0, 0) + c * c * cov(1, 1) - 2 * c * s * cov(0, 1); - return max(var_x_prime, var_y_prime); -} - - -template -inline -double PatchWork::xy2theta(const double &x, const double &y) { // 0 ~ 2 * PI - /* - if (y >= 0) { - return atan2(y, x); // 1, 2 quadrant - } else { - return 2 * M_PI + atan2(y, x);// 3, 4 quadrant - } - */ - auto atan_value = atan2(y,x); // EDITED! - return atan_value > 0 ? atan_value : atan_value + 2*M_PI; // EDITED! -} - -template -inline -double PatchWork::xy2radius(const double &x, const double &y) { - return sqrt(pow(x, 2) + pow(y, 2)); -} - -template -inline -void PatchWork::pc2czm(const pcl::PointCloud &src, std::vector &czm) { - - for (auto const &pt : src.points) { - int ring_idx, sector_idx; - double r = xy2radius(pt.x, pt.y); - if ((r <= max_range_) && (r > min_range_)) { - double theta = xy2theta(pt.x, pt.y); - - if (r < min_range_z2_) { // In First rings - ring_idx = min(static_cast(((r - min_range_) / ring_sizes_[0])), num_rings_each_zone_[0] - 1); - sector_idx = min(static_cast((theta / sector_sizes_[0])), num_sectors_each_zone_[0] - 1); - czm[0][ring_idx][sector_idx].points.emplace_back(pt); - } else if (r < min_range_z3_) { - ring_idx = min(static_cast(((r - min_range_z2_) / ring_sizes_[1])), num_rings_each_zone_[1] - 1); - sector_idx = min(static_cast((theta / sector_sizes_[1])), num_sectors_each_zone_[1] - 1); - czm[1][ring_idx][sector_idx].points.emplace_back(pt); - } else if (r < min_range_z4_) { - ring_idx = min(static_cast(((r - min_range_z3_) / ring_sizes_[2])), num_rings_each_zone_[2] - 1); - sector_idx = min(static_cast((theta / sector_sizes_[2])), num_sectors_each_zone_[2] - 1); - czm[2][ring_idx][sector_idx].points.emplace_back(pt); - } else { // Far! - ring_idx = min(static_cast(((r - min_range_z4_) / ring_sizes_[3])), num_rings_each_zone_[3] - 1); - sector_idx = min(static_cast((theta / sector_sizes_[3])), num_sectors_each_zone_[3] - 1); - czm[3][ring_idx][sector_idx].points.emplace_back(pt); - } - } - - } -} - -// For adaptive -template -inline -void PatchWork::extract_piecewiseground( - const int zone_idx, const pcl::PointCloud &src, - pcl::PointCloud &dst, - pcl::PointCloud &non_ground_dst) { - // 0. Initialization - if (!ground_pc_.empty()) ground_pc_.clear(); - if (!dst.empty()) dst.clear(); - if (!non_ground_dst.empty()) non_ground_dst.clear(); - // 1. set seeds! - - extract_initial_seeds_(zone_idx, src, ground_pc_); - // 2. Extract ground - for (int i = 0; i < num_iter_; i++) { - estimate_plane_(ground_pc_); - ground_pc_.clear(); - - //pointcloud to matrix - Eigen::MatrixXf points(src.points.size(), 3); - int j = 0; - for (auto &p:src.points) { - points.row(j++) << p.x, p.y, p.z; - } - // ground plane model - Eigen::VectorXf result = points * normal_; - // threshold filter - for (int r = 0; r < result.rows(); r++) { - if (i < num_iter_ - 1) { - if (result[r] < th_dist_d_) { - ground_pc_.points.push_back(src[r]); - } - } else { // Final stage - if (result[r] < th_dist_d_) { - dst.points.push_back(src[r]); - } else { - if (i == num_iter_ - 1) { - non_ground_dst.push_back(src[r]); - } - } - } - } - } -} - -template -inline -void PatchWork::check_input_parameters_are_correct() { - string SET_SAME_SIZES_OF_PARAMETERS = "Some parameters are wrong! the size of parameters should be same"; - - int n_z = num_zones_; - int n_r = num_rings_each_zone_.size(); - int n_s = num_sectors_each_zone_.size(); - int n_m = min_ranges_.size(); - - if ((n_z != n_r) || (n_z != n_s) || (n_z != n_m)) { - throw invalid_argument(SET_SAME_SIZES_OF_PARAMETERS); - } - - if ((n_r != n_s) || (n_r != n_m) || (n_s != n_m)) { - throw invalid_argument(SET_SAME_SIZES_OF_PARAMETERS); - } - - if (min_range_ != min_ranges_[0]) { - throw invalid_argument("Setting min. ranges are weired! The first term should be eqaul to min_range_"); - } - - if (elevation_thr_.size() != flatness_thr_.size()) { - throw invalid_argument("Some parameters are wrong! Check the elevation/flatness_thresholds"); - } - -} - -template -inline -void PatchWork::cout_params() { - cout << (boost::format("Num. sectors: %d, %d, %d, %d") % num_sectors_each_zone_[0] % num_sectors_each_zone_[1] % - num_sectors_each_zone_[2] % - num_sectors_each_zone_[3]).str() << endl; - cout << (boost::format("Num. rings: %01d, %01d, %01d, %01d") % num_rings_each_zone_[0] % - num_rings_each_zone_[1] % - num_rings_each_zone_[2] % - num_rings_each_zone_[3]).str() << endl; - cout << (boost::format("elevation_thr_: %0.4f, %0.4f, %0.4f, %0.4f ") % elevation_thr_[0] % elevation_thr_[1] % - elevation_thr_[2] % - elevation_thr_[3]).str() << endl; - cout << (boost::format("flatness_thr_: %0.4f, %0.4f, %0.4f, %0.4f ") % flatness_thr_[0] % flatness_thr_[1] % - flatness_thr_[2] % - flatness_thr_[3]).str() << endl; -} - -#endif diff --git a/include/quatro.hpp b/include/quatro.hpp deleted file mode 100644 index 631eb33..0000000 --- a/include/quatro.hpp +++ /dev/null @@ -1,1063 +0,0 @@ -// -// Created by Hyungtae Lim on 1/24/22. -// Our code is based on TEASER++. We really appreciate Prof. Luca Carlone group! :) -// - -#ifndef QUATRO_H -#define QUATRO_H - - -#include -#include -#include - -#include -#include -#include -#include -#include - -//fpfh.h -#include -#include - -//pcl voxelgrid -#include - -#include -#include - -#include "teaser/graph.h" - -#include -#include -#include - -#include -#include - -#include "teaser/utils.h" - -#include -#include "conversion.hpp" - - -using namespace std; -using namespace pcl; - - -template -void voxelize( - const boost::shared_ptr > srcPtr, boost::shared_ptr > dstPtr, - double voxelSize) { - static pcl::VoxelGrid voxel_filter; - voxel_filter.setInputCloud(srcPtr); - voxel_filter.setLeafSize(voxelSize, voxelSize, voxelSize); - voxel_filter.filter(*dstPtr); -} - - -template -void voxelize( - pcl::PointCloud &src, boost::shared_ptr > dstPtr, - double voxelSize) { - static pcl::VoxelGrid voxel_filter; - voxel_filter.setInputCloud(src); - voxel_filter.setLeafSize(voxelSize, voxelSize, voxelSize); - voxel_filter.filter(*dstPtr); -} - -template -class Quatro : public Registration { - -public: - using PointCloudSource = typename Registration::PointCloudSource; - using PointCloudSourceConstPtr = typename PointCloudSource::ConstPtr; - - using PointCloudTarget = typename Registration::PointCloudTarget; - using PointCloudTargetConstPtr = typename PointCloudTarget::ConstPtr; - - using Registration::reg_name_; - using Registration::getClassName; - using Registration::input_; //PCLBase::input_ - using Registration::indices_; - using Registration::target_; - using Registration::nr_iterations_; - using Registration::max_iterations_; ////set - using Registration::previous_transformation_; - using Registration::final_transformation_; - using Registration::transformation_; - using Registration::transformation_epsilon_; //set - using Registration::converged_; - using Registration::corr_dist_threshold_; - using Registration::inlier_threshold_; - using Registration::min_number_correspondences_; - using Registration::update_visualizer_; - using Registration::euclidean_fitness_epsilon_; - using Registration::correspondences_; - using Registration::transformation_estimation_; - using Registration::correspondence_estimation_; - using Registration::correspondence_rejectors_; - - using Registration::setInputSource; - using Registration::setInputTarget; - using Registration::computeTransformation; - - typename pcl::registration::DefaultConvergenceCriteria::Ptr - convergence_criteria_; - using Matrix4 = typename Registration::Matrix4; - - /** \brief Empty constructor. */ - Quatro() - : x_idx_offset_(0), y_idx_offset_(0), z_idx_offset_(0), nx_idx_offset_(0), ny_idx_offset_(0), nz_idx_offset_(0) - // , use_reciprocal_correspondence_(false) // use_reciprocal_correspondence_없음 - , source_has_normals_(false), target_has_normals_(false), - noise_bound_(0.3) { - reg_name_ = "Quatro"; - transformation_estimation_.reset( - new pcl::registration:: - TransformationEstimationSVD()); - correspondence_estimation_.reset( - new pcl::registration:: - CorrespondenceEstimation); - convergence_criteria_.reset( - new pcl::registration::DefaultConvergenceCriteria( - nr_iterations_, transformation_, *correspondences_)); - }; - - /** - * \brief Due to `convergence_criteria_` holding references to the class members, - * it is tricky to correctly implement its copy and move operations correctly. This - * can result in subtle bugs and to prevent them, these operations for ICP have - * been disabled. - * - * \todo: remove deleted ctors and assignments operations after resolving the issue - */ - Quatro(const Quatro &) = delete; - - Quatro(Quatro &&) = delete; - - Quatro & - operator=(const Quatro &) = delete; - - Quatro & - operator=(Quatro &&) = delete; - - /** \brief Empty destructor */ - ~Quatro() {} - - /** \brief Returns a pointer to the DefaultConvergenceCriteria used by the - * IterativeClosestPoint class. This allows to check the convergence state after the - * align() method as well as to configure DefaultConvergenceCriteria's parameters not - * available through the ICP API before the align() method is called. Please note that - * the align method sets max_iterations_, euclidean_fitness_epsilon_ and - * transformation_epsilon_ and therefore overrides the default / set values of the - * DefaultConvergenceCriteria instance. \return Pointer to the IterativeClosestPoint's - * DefaultConvergenceCriteria. - */ - bool using_pre_estimated_RyRx_ = false; - Eigen::Matrix3d estimated_RyRx_ = Eigen::Matrix3d::Identity(); - - struct RegistrationSolution { - bool valid = true; - double scale; - Eigen::Vector3d translation; - Eigen::Matrix3d rotation; - - EIGEN_MAKE_ALIGNED_OPERATOR_NEW - }; - - RegistrationSolution solution_; - - enum class ROTATION_ESTIMATION_ALGORITHM { - GNC_TLS = 0, - FGR = 1, - }; - /** - * Enum representing the type of graph-based inlier selection algorithm to use - * - * PMC_EXACT: Use PMC to find exact clique from the inlier graph - * PMC_HEU: Use PMC's heuristic finder to find approximate max clique - * KCORE_HEU: Use k-core heuristic to select inliers - * NONE: No inlier selection - */ - enum class INLIER_SELECTION_MODE { - PMC_EXACT = 0, - PMC_HEU = 1, - KCORE_HEU = 2, - NONE = 3, - }; - - /** - * Enum representing the formulation of the TIM graph after maximum clique filtering. - * - * CHAIN: formulate TIMs by only calculating ttable(run_pmc_pruninghe TIMs for consecutive measurements - * COMPLETE: formulate a fully connected TIM graph - */ - enum class INLIER_GRAPH_FORMULATION { - CHAIN = 0, - COMPLETE = 1, - }; - - struct Params { - std::string reg_name = "Quatro"; // "TEASER" or "Quatro" - /** - * Component-wise Translation Estimate (COTE)의 모드 설정 - * "weighted_mean" (original in TEASER++)과 - * "median" (Ours) 모드가 있음 - */ - std::string cote_mode = "median"; - bool using_rot_inliers_when_estimating_cote = false; - // A bound on the noise of each provided measurement. - double noise_bound = 0.3; - /** - * Square of the ratio between acceptable noise and noise bound. Usually set to 1. - */ - double cbar2 = 1; - /** - * Whether the scale is known. If set to False, the solver assumes no scale differences - * between the src and dst points. If set to True, the solver will first solve for scale. - * - * When the solver does not estimate scale, it solves the registration problem much faster. - */ - bool estimate_scaling = true; - // Which algorithm to use to estimate rotations. - ROTATION_ESTIMATION_ALGORITHM rotation_estimation_algorithm = ROTATION_ESTIMATION_ALGORITHM::GNC_TLS; - /** - * Factor to multiple/divide the control parameter in the GNC algorithm. - * - * For FGR: the algorithm divides the control parameter by the factor every iteration. - * For GNC-TLS: the algorithm multiples the control parameter by the factor every iteration. - */ - double rotation_gnc_factor = 1.4; - // Maximum iterations allowed for the GNC rotation estimators. - size_t rotation_max_iterations = 100; - /** - * Cost threshold for the GNC rotation estimators. - * - * For FGR / GNC-TLS algorithm, the cost thresholds represent different meanings. - * For FGR: the cost threshold compares with the computed cost at each iteration - * For GNC-TLS: the cost threshold compares with the difference between costs of consecutive - * iterations. - */ - double rotation_cost_threshold = 1e-6; - - // Type of TIM graph given to GNC rotation solver - INLIER_GRAPH_FORMULATION rotation_tim_graph = INLIER_GRAPH_FORMULATION::CHAIN; - // brief Type of the inlier selection - INLIER_SELECTION_MODE inlier_selection_mode = INLIER_SELECTION_MODE::PMC_HEU; //PMC_EXACT -> PMC_HEU - /** - * \brief The threshold ratio for determining whether to skip max clique and go straightly to - * GNC rotation estimation. Set this to 1 to always use exact max clique selection, 0 to always - * skip exact max clique selection. - * \attention Note that the use_max_clique option takes precedence. In other words, if - * use_max_clique is set to false, then kcore_heuristic_threshold will be ignored. If - * use_max_clique is set to true, then the following will happen: if the max core number of the - * inlier graph is lower than the kcore_heuristic_threshold as a percentage of the total nodes - * in the inlier graph, then the code will preceed to call the max clique finder. Otherwise, the - * graph will be directly fed to the GNC rotation solver. - * - */ - double kcore_heuristic_threshold = 0.5; - // \deprecated Use inlier_selection_mode instead. Set this to true to enable max clique inlier selection, false to skip it. - bool use_max_clique = true; - // Use inlier_selection_mode instead * Set this to false to enable heuristic only max clique finding. - bool max_clique_exact_solution = true; - //Timelimit on running the max clique algorithm (in seconds). - double max_clique_time_limit = 3600; - }; - double noise_bound_; - - Params getParams() { return params_; } - - void setParams(Params params) { params_ = params; } - - // For leveraging IMU data - void setPreEstaimatedRyRx(Eigen::Matrix4d &estimated_RyRx) { - estimated_RyRx_ = estimated_RyRx.block<3, 3>(0, 0); - using_pre_estimated_RyRx_ = true; - } - - /** \brief Provide a pointer to the input source - * (e.g., the point cloud that we want to align to the target) - * - * \param[in] cloud the input point cloud source - */ - void setInputSource(const PointCloudSourceConstPtr &cloud) override { - Registration::setInputSource(cloud); //setInputSource <- >PCLBase::setInputCloud <- - source_has_normals_ = false; - } - - /** \brief Provide a pointer to the input target - * (e.g., the point cloud that we want to align to the target) - * - * \param[in] cloud the input point cloud target - */ - inline void - setInputTarget(const PointCloudTargetConstPtr &cloud) override { - if (cloud->points.empty()) { - PCL_ERROR("[pcl::%s::setInputSource] Invalid or empty point cloud dataset given!\n", - getClassName().c_str()); - return; - } - Registration::setInputTarget(cloud); - target_has_normals_ = false; - } - - Eigen::Matrix computeTIMs( - const Eigen::Matrix &v, - Eigen::Matrix *map) { - - auto N = v.cols(); - // std::cout<<"\033[1;32m"<< "N:"< vtilde(3, N * (N - 1) / 2); - map->resize(2, N * (N - 1) / 2); - // std::cout<<"\033[1;32m"<< "Debug 1-3"<<"\033[0m"< m = v.col(i); - Eigen::Matrix temp = v - m * Eigen::MatrixXd::Ones(1, N); - - // concatenate to the end of the tilde vector - vtilde.middleCols(segment_start_idx, segment_cols) = temp.rightCols(segment_cols); - - // populate the index map - Eigen::Matrix map_addition(2, N); - for (size_t j = 0; j < N; ++j) { - map_addition(0, j) = i; - map_addition(1, j) = j; - } - map->middleCols(segment_start_idx, segment_cols) = map_addition.rightCols(segment_cols); - } - return vtilde; - } - - double solveForScale( - const Eigen::Matrix &v1, - const Eigen::Matrix &v2) { - scale_inliers_mask_.resize(1, v1.cols()); - solveForScale(v1, v2, &(solution_.scale), &scale_inliers_mask_); //ScaleInliersSelector의 solveForScale임 //scale_solver_-> 뺌 - std::cout << "Scale: " << solution_.scale << std::endl; - return solution_.scale; - } - - void solveForScale( - const Eigen::Matrix &src, - const Eigen::Matrix &dst, - double *scale, - Eigen::Matrix *inliers) { - // We assume no scale difference between the two vectors of points. - *scale = 1; - - Eigen::Matrix v1_dist = - src.array().square().colwise().sum().array().sqrt(); - Eigen::Matrix v2_dist = - dst.array().square().colwise().sum().array().sqrt(); - double beta = 2 * params_.noise_bound * - sqrt(params_.cbar2); ////cbar2_, noise_bound_ - // A pair-wise correspondence is an inlier if it passes the following two tests: - // 1. dst / src is within maximum allowed error - // 2. src / dst is within maximum allowed error - Eigen::Matrix alphas_forward = beta * v1_dist.cwiseInverse(); - Eigen::Matrix raw_scales_forward = v2_dist.array() / v1_dist.array(); - Eigen::Matrix inliers_forward = - (raw_scales_forward.array() - *scale).array().abs() <= - alphas_forward.array(); - - Eigen::Matrix alphas_reverse = beta * v2_dist.cwiseInverse(); - Eigen::Matrix raw_scales_reverse = v1_dist.array() / v2_dist.array(); - Eigen::Matrix inliers_reverse = - (raw_scales_reverse.array() - *scale).array().abs() <= - alphas_reverse.array(); - - // element-wise AND using component-wise product (Eigen 3.2 compatible) - *inliers = inliers_forward.cwiseProduct(inliers_reverse); - } - - Eigen::Matrix3d solveForRotation( - const Eigen::Matrix &v1, - const Eigen::Matrix &v2) { - - rotation_inliers_mask_.resize(1, v1.cols()); - - if (reg_name_ == "Quatro") { - Eigen::Matrix2d rotation_2d; - Eigen::Matrix src_2d; - Eigen::Matrix dst_2d; - // XY Coordinates for calculate yaw - src_2d.resize(2, v1.cols()); - dst_2d.resize(2, v2.cols()); - src_2d = v1.topRows(2); - dst_2d = v2.topRows(2); - - Eigen::Matrix3d rot_yaw = Eigen::Matrix3d::Identity(); - rotation_2d = Eigen::Matrix2d::Identity(); - solveForRotation2D(src_2d, dst_2d, &rotation_2d, &rotation_inliers_mask_); //rotation_solver_-> 뺌 - rot_yaw.block<2, 2>(0, 0) = rotation_2d; - solution_.rotation = rot_yaw; - } else - throw std::invalid_argument( - "[solveForRotation] The param is wrong! It should be 'TEASER' or 'Quatro'"); - - /*** - * Once the prior pose is given by INS (by the function `setPreEstimatedRyRx()` ), - * RyRx is multiplied by estimated Rz, i.e. Rz * RyRx - * Note that one calls the function setPreEstimatedRyRx(), - * then `using_pre_estimated_RyRx_` automatically turns into true - */ - if (using_pre_estimated_RyRx_) { - if (reg_name_ == "Quatro") { - std::cout << "Quatro: Pre-estimated RyRx is updated" << std::endl; - solution_.rotation = solution_.rotation * estimated_RyRx_; - } else { - throw std::invalid_argument("Wrong reg type name is coming!"); - } - } - return solution_.rotation; - } - - void solveForRotation2D( - const Eigen::Matrix &src, - const Eigen::Matrix &dst, Eigen::Matrix2d *rotation, - Eigen::Matrix *inliers) { - assert(rotation); // make sure R is not a nullptr - assert(src.cols() == dst.cols()); // check dimensions of input data - assert(params_.rotation_gnc_factor > 1); // make sure mu will increase gnc_factor -> rotation_gnc_factor - assert(params_.noise_bound != 0); // make sure noise sigma is not zero - - std::cout << "\033[1;32m=>Conducting 2D Rot solver w/ " << src.cols() << " \033[0m" << std::endl; - - if (inliers) { - assert(inliers->cols() == src.cols()); - } - - std::string costPath, estWeightPath, estRotPath; - std::ofstream costTxt, weightTxt, rotTxt; - - // If you want check the each weight of correspondence, - // Utilize the below code: - // if (SAVE_RESULTS) { - // costPath = (boost::format("%s/%04d_SONNY_cost.txt") % SAVE_ABS_PATH % COUNT_EXECUTION).str(); - // costTxt.open(costPath); - // estWeightPath = - // (boost::format("%s/%04d_SONNY_weights.txt") % SAVE_ABS_PATH % COUNT_EXECUTION).str(); - // weightTxt.open(estWeightPath); - // estRotPath = (boost::format("%s/%04d_SONNY_rot.txt") % SAVE_ABS_PATH % COUNT_EXECUTION).str(); - // rotTxt.open(estRotPath); - // } - // - - - - - - - - - - - - - - - - - - - - - - // Prepare some variables - size_t match_size = src.cols(); // number of correspondences - - double mu = 1; // arbitrary starting mu - - double prev_cost = std::numeric_limits::infinity(); - cost_ = std::numeric_limits::infinity(); - // double noise_bound_sq = std::pow(params_.noise_bound, 2); - static double rot_noise_bound = params_.noise_bound; - static double noise_bound_sq = std::pow(rot_noise_bound, 2); - if (noise_bound_sq < 1e-16) { - noise_bound_sq = 1e-2; - } - // TEASER_DEBUG_INFO_MSG("GNC rotation estimation noise bound:" << rot_noise_bound); - // TEASER_DEBUG_INFO_MSG("GNC rotation estimation noise bound:" << rot_noise_bound); - // TEASER_DEBUG_INFO_MSG("GNC rotation estimation noise bound squared:" << noise_bound_sq); - - - Eigen::Matrix diffs(2, match_size); - Eigen::Matrix weights(1, match_size); - weights.setOnes(1, match_size); - Eigen::Matrix residuals_sq(1, match_size); - - // Loop for performing GNC-TLS - for (size_t i = 0; i < params_.rotation_max_iterations; ++i) { //max_iterations = >rotation_max_iterations - - // Fix weights and perform SVD rotation estimation - *rotation = teaser::utils::svdRot2d(src, dst, weights); - - // Calculate residuals squared - diffs = (dst - (*rotation) * src).array().square(); - residuals_sq = diffs.colwise().sum(); - if (i == 0) { - // Initialize rule for mu - double max_residual = residuals_sq.maxCoeff(); - mu = 1 / (2 * max_residual / noise_bound_sq - 1); - // Degenerate case: mu = -1 because max_residual is very small - // i.e., little to none noise - if (mu <= 0) { - TEASER_DEBUG_INFO_MSG( - "GNC-TLS terminated because maximum residual at initialization is very small."); - break; - } - } - // Fix R and solve for weights in closed form - double th1 = (mu + 1) / mu * noise_bound_sq; - double th2 = mu / (mu + 1) * noise_bound_sq; - cost_ = 0; - for (size_t j = 0; j < match_size; ++j) { - // Also calculate cost in this loop - // Note: the cost calculated is using the previously solved weights - cost_ += weights(j) * residuals_sq(j); - - if (residuals_sq(j) >= th1) { - weights(j) = 0; - } else if (residuals_sq(j) <= th2) { - weights(j) = 1; - } else { - weights(j) = sqrt(noise_bound_sq * mu * (mu + 1) / residuals_sq(j)) - mu; - assert(weights(j) >= 0 && weights(j) <= 1); - } - // - - - - - - - - - - - - - - - - - - - - - // if (SAVE_RESULTS) { - // if (j == match_size - 1) { // Last - // weightTxt << weights(j) << "\n"; - // } else { - // weightTxt << weights(j) << " "; - // } - // } - // - - - - - - - - - - - - - - - - - - - - - } - // Calculate cost - double cost_diff = std::abs(cost_ - prev_cost); - // - - - - - - - - - - - - - - - - - - - - - // if (SAVE_RESULTS) { - // for (int ii = 0; ii < 2; ++ii) { - // for (int jj = 0; jj < 2; ++jj) { - // if ((ii == 1) && (jj == 1)) { - // rotTxt << (*rotation)(ii, jj) << "\n"; - // } else { - // rotTxt << (*rotation)(ii, jj) << " "; - // } - // } - // } - // costTxt << cost_ << "\n"; - // } - // - - - - - - - - - - - - - - - - - - - - - // Increase mu - mu = mu * params_.rotation_gnc_factor; //gnc_factor -> rotation_gnc_factor - prev_cost = cost_; - - if (cost_diff < params_.rotation_cost_threshold) { - TEASER_DEBUG_INFO_MSG("GNC-TLS solver terminated due to cost convergence."); - TEASER_DEBUG_INFO_MSG("Cost diff: " << cost_diff); - TEASER_DEBUG_INFO_MSG("Iterations: " << i); - break; - } - } - // - - - - - - - - - - - - - - - - - - - - - // if (SAVE_RESULTS) { - // weightTxt.close(); - // rotTxt.close(); - // costTxt.close(); - // } - // - - - - - - - - - - - - - - - - - - - - - - if (inliers) { - for (size_t i = 0; i < weights.cols(); ++i) { - (*inliers)(0, i) = weights(0, i) >= 0.4; - } - } - } - - Eigen::Vector3d solveForTranslation( - const Eigen::Matrix &v1, - const Eigen::Matrix &v2, - bool using_median_selection = false) { - - translation_inliers_mask_.resize(1, v1.cols()); - solveForTranslation(v1, v2, &(solution_.translation), &translation_inliers_mask_, using_median_selection); - - return solution_.translation; - } - - void solveForTranslation( - const Eigen::Matrix &src, - const Eigen::Matrix &dst, - Eigen::Vector3d *translation, - Eigen::Matrix *inliers, - bool using_median_selection) { - - assert(src.cols() == dst.cols()); - if (inliers) { - assert(inliers->cols() == src.cols()); - } - // Raw translation - Eigen::Matrix raw_translation = dst - src; - // Error bounds for each measurements - int N = src.cols(); - double beta = - noise_bound_ * sqrt(params_.cbar2); //params_.noise_bound -> noise_bound_ - - Eigen::Matrix alphas = beta * Eigen::MatrixXd::Ones(1, N); - - // Estimate x, y, and z component of translation: perform TLS on each row - *inliers = Eigen::Matrix::Ones(1, N); - Eigen::Matrix inliers_temp(1, N); - - for (size_t i = 0; i < raw_translation.rows(); ++i) { - estimate(raw_translation.row(i), alphas, &((*translation)(i)), &inliers_temp, using_median_selection); - // element-wise AND using component-wise product (Eigen 3.2 compatible) - // a point is an inlier iff. x,y,z are all inliers - *inliers = (*inliers).cwiseProduct(inliers_temp); - } - } - - - void estimate( - const Eigen::RowVectorXd &X, - const Eigen::RowVectorXd &ranges, - double *estimate, - Eigen::Matrix *inliers, - bool using_median_selection = false) { - // check input parameters - bool dimension_inconsistent = (X.rows() != ranges.rows()) || (X.cols() != ranges.cols()); - if (inliers) { - dimension_inconsistent |= ((inliers->rows() != 1) || (inliers->cols() != ranges.cols())); - } - bool only_one_element = (X.rows() == 1) && (X.cols() == 1); - assert(!dimension_inconsistent); - assert(!only_one_element); // TODO: admit a trivial solution - - int N = X.cols(); - std::vector> h; - for (size_t i = 0; i < N; ++i) { - h.push_back(std::make_pair(X(i) - ranges(i), i + 1)); - h.push_back(std::make_pair(X(i) + ranges(i), -i - 1)); - } - - // ascending order - std::sort(h.begin(), h.end(), - [](std::pair a, std::pair b) { return a.first < b.first; }); - - // calculate weights - Eigen::RowVectorXd weights = ranges.array().square(); - weights = weights.array().inverse(); - int nr_centers = 2 * N; - Eigen::RowVectorXd x_hat = Eigen::MatrixXd::Zero(1, nr_centers); - Eigen::RowVectorXd x_cost = Eigen::MatrixXd::Zero(1, nr_centers); - Eigen::RowVectorXi set_cardinality = Eigen::MatrixXi::Zero(1, nr_centers); - - double ranges_inverse_sum = ranges.sum(); - double dot_X_weights = 0; - double dot_weights_consensus = 0; - int consensus_set_cardinal = 0; - double sum_xi = 0; - double sum_xi_square = 0; - - // To save outputs - static int component_idx = 0; - component_idx = component_idx % 3; - - std::string h_path, estimate_path, idx_path; - std::ofstream h_txt, estimate_txt, idx_txt; - // if (SAVE_RESULTS) { - // h_path = - // (boost::format("%s/%04d_h_%d.txt") % SAVE_ABS_PATH % COUNT_EXECUTION % component_idx).str(); - - // estimate_path = (boost::format("%s/%04d_estimates_%d.txt") % SAVE_ABS_PATH % COUNT_EXECUTION % - // component_idx) - // .str(); - - // idx_path = (boost::format("%s/%04d_final_idx_%d.txt") % SAVE_ABS_PATH % COUNT_EXECUTION % - // component_idx) - // .str(); - // estimate_txt.open(estimate_path); - // estimate_txt.close(); - - // idx_txt.open(estimate_path); - // idx_txt.close(); - - // h_txt.open(h_path); - // for (int ijk = 0; ijk < h.size(); ++ijk) { - // h_txt << h[ijk].first << " " << h[ijk].second << "\n"; - // } - // h_txt.close(); - // } - for (size_t i = 0; i < nr_centers; ++i) { - - int idx = int(std::abs(h.at(i).second)) - 1; // Indices starting at 1 - int epsilon = (h.at(i).second > 0) ? 1 : -1; - - consensus_set_cardinal += epsilon; - dot_weights_consensus += epsilon * weights(idx); - dot_X_weights += epsilon * weights(idx) * X(idx); // X(idx): v_ij in my paper - ranges_inverse_sum -= epsilon * ranges(idx); // handling truncated loss! - sum_xi += epsilon * X(idx); - sum_xi_square += epsilon * X(idx) * X(idx); - - if (using_median_selection) { - // To set median values - set_cardinality(i) = consensus_set_cardinal; - } - x_hat(i) = dot_X_weights / dot_weights_consensus; - // sum_xi_square: already includes consensus_set_cardinal - double residual = - consensus_set_cardinal * x_hat(i) * x_hat(i) + sum_xi_square - 2 * sum_xi * x_hat(i); - x_cost(i) = residual + ranges_inverse_sum; - } - - size_t min_idx; - x_cost.minCoeff(&min_idx); - - double median; - if (using_median_selection) { - int n_card = set_cardinality(min_idx); - std::cout << "\033[1;32m[Median Selection] num: " << n_card << "\033[0m" << std::endl; - if (n_card > 0) { - std::vector candidates; - candidates.reserve(set_cardinality(min_idx)); - for (int j = 0; j < n_card; ++j) { - int idx = int(std::abs(h.at(min_idx - j).second)) - 1; - assert(idx >= 0); - candidates.push_back(X(idx)); - } - sort(candidates.begin(), candidates.end()); - median = static_cast(candidates[candidates.size() / 2 - 1] + - candidates[candidates.size() / 2]) / 2.0; - } - } - double estimate_temp; - if (using_median_selection) { - estimate_temp = median; - } else { - estimate_temp = x_hat(min_idx); - } - - if (estimate) { - // update estimate output if it's not nullptr - *estimate = estimate_temp; - } - if (inliers) { - // update inlier output if it's not nullptr - *inliers = (X.array() - estimate_temp).array().abs() <= ranges.array(); - } - component_idx++; - } - - double cost_; - - inline void setMaximumIterations(int nr_iterations) { - max_iterations_ = nr_iterations; - } - - void reset(const Params ¶ms) { - - reg_name_ = params.reg_name; - params_ = params; - - // Clear member variables - max_clique_.clear(); - rotation_inliers_.clear(); - translation_inliers_.clear(); - inlier_graph_.clear(); - } - - void computeTransformation(PointCloudSource &output, const Matrix4 &guess) override {}; - - void computeTransformation(Eigen::Matrix4d &output) { - - pcl::PointCloud::Ptr src(new pcl::PointCloud); - pcl::PointCloud::Ptr tgt(new pcl::PointCloud); - - pcl2eigen(*input_, src_matched); - pcl2eigen(*target_, tgt_matched); - - src_tims_ = computeTIMs(src_matched, &src_tims_map_); - dst_tims_ = computeTIMs(tgt_matched, &dst_tims_map_); - solveForScale(src_tims_, dst_tims_); - - // Calculate Maximum Clique - if (params_.inlier_selection_mode != INLIER_SELECTION_MODE::NONE) { //0,1,2 -> default 1 - - inlier_graph_.populateVertices(src_matched.cols()); - for (size_t i = 0; i < scale_inliers_mask_.cols(); ++i) { - if (scale_inliers_mask_(0, i)) { - inlier_graph_.addEdge(src_tims_map_(0, i), src_tims_map_(1, i)); - } - } - - teaser::MaxCliqueSolver::Params clique_params; - if (params_.inlier_selection_mode == INLIER_SELECTION_MODE::PMC_EXACT) { - clique_params.solver_mode = teaser::MaxCliqueSolver::CLIQUE_SOLVER_MODE::PMC_EXACT; - } else if (params_.inlier_selection_mode == INLIER_SELECTION_MODE::PMC_HEU) { - clique_params.solver_mode = teaser::MaxCliqueSolver::CLIQUE_SOLVER_MODE::PMC_HEU; - } else { - clique_params.solver_mode = teaser::MaxCliqueSolver::CLIQUE_SOLVER_MODE::KCORE_HEU; - } - - clique_params.time_limit = params_.max_clique_time_limit; - clique_params.kcore_heuristic_threshold = params_.kcore_heuristic_threshold; - - teaser::MaxCliqueSolver clique_solver(clique_params); - max_clique_ = clique_solver.findMaxClique(inlier_graph_); - - std::sort(max_clique_.begin(), max_clique_.end()); - std::copy(max_clique_.begin(), max_clique_.end(), std::ostream_iterator(std::cout, " ")); - std::cout << std::endl; - if (max_clique_.size() <= 1) { - TEASER_DEBUG_INFO_MSG("Clique size too small. Abort."); - solution_.valid = false; - return; //return solution_ - } - } - - // Calculate new measurements & TIMs based on max clique inliers - if (params_.rotation_tim_graph == INLIER_GRAPH_FORMULATION::CHAIN) { - // ==============> - // chain graph - // TEASER_DEBUG_INFO_MSG("Using chain graph for GNC rotation."); - std::cout << "\033[1;32mNum. of maximum cliques: " << max_clique_.size() << "\033[0m" - << std::endl; - num_maxclique_ = max_clique_.size(); - pruned_src_tims_.resize(3, max_clique_.size()); - pruned_dst_tims_.resize(3, max_clique_.size()); - src_tims_map_rotation_.resize(2, max_clique_.size()); - dst_tims_map_rotation_.resize(2, max_clique_.size()); - for (size_t i = 0; i < max_clique_.size(); ++i) { - const auto &root = max_clique_[i]; - int leaf; - if (i != max_clique_.size() - 1) { - leaf = max_clique_[i + 1]; - } else { - leaf = max_clique_[0]; - } - pruned_src_tims_.col(i) = src_matched.col(leaf) - src_matched.col(root); - pruned_dst_tims_.col(i) = tgt_matched.col(leaf) - tgt_matched.col(root); - // populate the TIMs map - dst_tims_map_rotation_(0, i) = leaf; - dst_tims_map_rotation_(1, i) = root; - src_tims_map_rotation_(0, i) = leaf; - src_tims_map_rotation_(1, i) = root; - } - } - // Remove scaling for rotation estimation - pruned_dst_tims_ *= (1 / solution_.scale); - // Update GNC rotation solver's noise bound with the new information - // Note: this implicitly assumes that rotation_solver_'s noise bound - // is set to the original noise bound of the measurements. - auto params = getParams(); //rotation_solver_-> 뺌 - params.noise_bound *= (2 / solution_.scale); - setParams(params); //rotation_solver_-> 뺌 - - // Solve for rotation - solveForRotation(pruned_src_tims_, pruned_dst_tims_); - - rotation_inliers_.clear(); - - // Save indices of inlier TIMs from GNC rotation estimation - for (size_t i = 0; i < rotation_inliers_mask_.cols(); ++i) { - if (i == 0) { - // Check (N-1)-th and 0-th - if (rotation_inliers_mask_(0, rotation_inliers_mask_.cols() - 1) && - rotation_inliers_mask_(0, i)) { - rotation_inliers_.emplace_back(i); - } - } else { - // Check i-1th and i-th - if (rotation_inliers_mask_(0, i - 1) && rotation_inliers_mask_(0, i)) { - rotation_inliers_.emplace_back(i); - } - } - } - num_rot_inliers_ = rotation_inliers_.size(); - int N_R = rotation_inliers_.size(); - Eigen::Matrix rotation_pruned_src; - Eigen::Matrix rotation_pruned_dst; - - if (params_.using_rot_inliers_when_estimating_cote && (N_R > 0)) { - for (size_t i = 0; i < N_R; ++i) { - rotation_pruned_src.resize(3, N_R); - rotation_pruned_dst.resize(3, N_R); - rotation_pruned_src.col(i) = src_matched.col(max_clique_[rotation_inliers_[i]]); - rotation_pruned_dst.col(i) = tgt_matched.col(max_clique_[rotation_inliers_[i]]); - } - } else { - int N_MC = max_clique_.size(); - for (size_t i = 0; i < N_MC; ++i) { - rotation_pruned_src.resize(3, N_MC); - rotation_pruned_dst.resize(3, N_MC); - if (reg_name_ == "Quatro") { - rotation_pruned_src.col(i) = estimated_RyRx_ * src_matched.col(max_clique_[i]); - } - // else if (reg_name_ == "TEASER") { - // rotation_pruned_src.col(i) = src.col(max_clique_[i]); - // } - rotation_pruned_dst.col(i) = tgt_matched.col(max_clique_[i]); - } - } - /** - * Modes of COTE: - * "weighted_mean" (TEASER++) and "median" (Ours) - * Median selection might be better because the weights of pairs are actually constant with same values. - */ - if (params_.cote_mode == "median") { - solveForTranslation(solution_.scale * solution_.rotation * rotation_pruned_src, - rotation_pruned_dst, true); - } else if (params_.cote_mode == "weighted_mean") { - solveForTranslation(solution_.scale * solution_.rotation * rotation_pruned_src, - rotation_pruned_dst); - } else { throw invalid_argument("[COTE]: Wrong parameter comes!"); } - - // Find the final inliers - translation_inliers_ = teaser::utils::findNonzero(translation_inliers_mask_); - - // final_inliers: Indices order of matched pairs - // which corresponds to `input_` and `target_` - // I.e. indices of src and dst - final_inliers_.clear(); - final_inliers_.reserve(translation_inliers_.size()); - // Note that `params_.using_rot_inliers_when_estimating_cote` == false exhibits better results - if (params_.using_rot_inliers_when_estimating_cote && (N_R > 0)) { - for (const auto &idx : translation_inliers_) { - final_inliers_.push_back(max_clique_[rotation_inliers_[idx]]); - } - } else { - for (const auto &idx : translation_inliers_) { - final_inliers_.push_back(max_clique_[idx]); - } - } - solution_.valid = true; - - output = Eigen::Matrix4d::Identity(); - output.block<3, 3>(0, 0) = solution_.rotation; - output.topRightCorner(3, 1) = solution_.translation; - } - - void setInliers( - const Eigen::Matrix &raw, pcl::PointCloud &inliers, - const vector &idx_inliers) { - inliers.clear(); - inliers.reserve(idx_inliers.size()); - - for (const int idx: idx_inliers) { - inliers.push_back(PointType(raw(0, idx), raw(1, idx), raw(2, idx))); - } - } - - void getMaxCliques(pcl::PointCloud &source_max_clique, - pcl::PointCloud &target_max_clique){ - setInliers(src_matched, source_max_clique, max_clique_); - setInliers(tgt_matched, target_max_clique, max_clique_); - } - - void getFinalInliers( - pcl::PointCloud &source_inliers, - pcl::PointCloud &target_inliers) { - setInliers(src_matched, source_inliers, final_inliers_); - setInliers(tgt_matched, target_inliers, final_inliers_); - } - - vector getFinalInliersIndices() { - return final_inliers_; - } - - int getNumRotaionInliers() { - return num_rot_inliers_; - } - - int getNumMaxCliqueInliers() { - return num_maxclique_; - } - -protected: - /** \brief Apply a rigid transform to a given dataset. Here we check whether - * the dataset has surface normals in addition to XYZ, and rotate normals as well. - * \param[in] input the input point cloud - * \param[out] output the resultant output point cloud - * \param[in] transform a 4x4 rigid transformation - * \note Can be used with cloud_in equal to cloud_out - */ - - // virtual - void - transformCloud( - const PointCloudSource &input, - PointCloudSource &output, - const Matrix4 &transform); - - /** \brief Rigid transformation computation method with initial guess. - * \param output the transformed input point cloud dataset using the rigid - * transformation found \param guess the initial guess of the transformation to - * compute - */ - - /** \brief Looks at the Estimators and Rejectors and determines whether their - * blob-setter methods need to be called */ - // virtual - void - determineRequiredBlobData(); - - //Params. - Params params_; - - int num_rot_inliers_; - int num_maxclique_; - - // Inlier graph - teaser::Graph inlier_graph_; - // Inlier Binary Vectors - Eigen::Matrix rotation_inliers_mask_; - Eigen::Matrix translation_inliers_mask_; - Eigen::Matrix scale_inliers_mask_; - // TIMs - // TIMs used for scale estimation/pruning - Eigen::Matrix src_tims_; - Eigen::Matrix dst_tims_; - // TIMs used for rotation estimation - Eigen::Matrix pruned_src_tims_; - Eigen::Matrix pruned_dst_tims_; - // Pruned data Eigen 2 Pcl - pcl::PointCloud pcl_pruned_src_; - pcl::PointCloud pcl_pruned_dst_; - - // TIM maps - // for scale estimation - Eigen::Matrix src_tims_map_; - Eigen::Matrix dst_tims_map_; - // for rotation estimation - Eigen::Matrix src_tims_map_rotation_; - Eigen::Matrix dst_tims_map_rotation_; - // Max clique vector - std::vector max_clique_; - // Inliers after rotation estimation - std::vector rotation_inliers_; - // Inliers after translation estimation (final inliers) - std::vector translation_inliers_; - std::vector final_inliers_; - - /** \brief XYZ fields offset. */ - std::size_t x_idx_offset_, y_idx_offset_, z_idx_offset_; - - /** \brief Normal fields offset. */ - std::size_t nx_idx_offset_, ny_idx_offset_, nz_idx_offset_; - - /** \brief Internal check whether source dataset has normals or not. */ - bool source_has_normals_; - /** \brief Internal check whether target dataset has normals or not. */ - bool target_has_normals_; - - /** \brief Checks for whether estimators and rejectors need various data */ - bool need_source_blob_, need_target_blob_; - - Eigen::Matrix src_matched; - Eigen::Matrix tgt_matched; - - typedef std::vector Feature; //public - -private: - -}; - -#endif //QUATRO_H \ No newline at end of file diff --git a/include/teaser_utils/fpfh.h b/include/quatro/fpfh.h similarity index 82% rename from include/teaser_utils/fpfh.h rename to include/quatro/fpfh.h index 6869a13..aa0672b 100644 --- a/include/teaser_utils/fpfh.h +++ b/include/quatro/fpfh.h @@ -9,12 +9,9 @@ #pragma once #include -//#include -#include +#include #include -//#include "./geometry.h" - #include namespace teaser { @@ -36,14 +33,10 @@ class FPFHEstimation { * @param normal_search_radius Radius for estimating normals * @param fpfh_search_radius Radius for calculating FPFH (needs to be at least normalSearchRadius) */ - FPFHCloudPtr computeFPFHFeatures(const PointCloud& input_cloud, double normal_search_radius = 0.03, - double fpfh_search_radius = 0.05); - - - + FPFHCloudPtr computeFPFHFeatures(const PointCloud& input_cloud, + double normal_search_radius = 0.03, + double fpfh_search_radius = 0.05); - FPFHCloudPtr computeFPFHFeatures(const teaser::PointCloud& input_cloud, pcl::PointCloud& normals - , double normal_search_radius = 0.03 , double fpfh_search_radius = 0.05); /** * Return the pointer to the underlying pcl::FPFHEstimation object * @return @@ -54,6 +47,7 @@ class FPFHEstimation { } private: + // pcl::FPFHEstimation::Ptr fpfh_estimation_; pcl::FPFHEstimationOMP::Ptr fpfh_estimation_; /** diff --git a/include/quatro/matcher.h b/include/quatro/matcher.h new file mode 100644 index 0000000..b967571 --- /dev/null +++ b/include/quatro/matcher.h @@ -0,0 +1,70 @@ +/** + * Copyright 2020, Massachusetts Institute of Technology, + * Cambridge, MA 02139 + * All Rights Reserved + * Authors: Jingnan Shi, et al. (see THANKS for the full author list) + * See LICENSE for the license information + */ +#pragma once + +#include +#include +#include +#include + +#include +#include + +#include +#include +#include + +#ifdef TBB_EN +#include +#endif + +namespace teaser { + +class Matcher { +public: + typedef std::vector Feature; + typedef flann::Index> KDTree; + + Matcher() = default; + /** + * Calculate correspondences based on given features and point clouds. + * input: source point cloud, target point cloud + * output: correspondences + */ + std::vector> + calculateCorrespondences(const teaser::PointCloud& source_points, const teaser::PointCloud& target_points, + const teaser::FPFHCloud& source_features, const teaser::FPFHCloud& target_features, + const bool& use_absolute_scale = false, const bool& use_crosscheck = true, + const bool& use_tuple_test = true, const float& tuple_scale = 0.95, + const bool& use_optimized_matching = true, const float& thr_dist = 30.0, const int& num_max_corres = 600); + +private: + // variables + std::vector> corres_; + std::vector pointcloud_; + std::vector features_; + std::vector > means_; // for normalization + float global_scale_; + + // methods + template void buildKDTree(const std::vector& data, KDTree* tree); +#ifdef TBB_EN + template void buildKDTreeWithTBB(const std::vector& data, KDTree* tree); +#endif + template + void searchKDTree(KDTree* tree, const T& input, std::vector& indices, + std::vector& dists, int nn); + template + void searchKDTreeAll(Matcher::KDTree* tree, const std::vector& inputs, + std::vector& indices, std::vector& dists, int nn); + void advancedMatching(const bool& use_crosscheck, const bool& use_tuple_test, const float& tuple_scale); + void optimizedMatching(const float& thr_dist, const int& num_max_corres, const float& tuple_scale); + void normalizePoints(const bool& use_absolute_scale); +}; + +} // namespace teaser diff --git a/include/quatro/quatro_module.h b/include/quatro/quatro_module.h new file mode 100644 index 0000000..9408e13 --- /dev/null +++ b/include/quatro/quatro_module.h @@ -0,0 +1,37 @@ +#pragma once + + +///// Eigen +#include +///// PCL +#include +#include +///// imported headers - teaser +#include +#include +#include +///// this package header +#include + +using namespace std; + +////////////////////////////////////////////////////// +template +class quatro +{ + private: + int m_rotation_max_iter = 100, m_num_max_corres = 200; + double m_normal_radius = 0.02, m_fpfh_radius = 0.04, m_distance_threshold = 30.0; + double m_noise_bound = 0.25, m_rotation_gnc_factor = 1.39, m_rotation_cost_thr = 0.0001; + bool m_estimate_scale = false, m_use_optimized_matching = true; + teaser::RobustRegistrationSolver::Params m_quatro_params; + public: + quatro(){}; + quatro(const double &fpfh_normal_radi, const double &fpfh_radi, const double noise_bound, const double &rot_gnc_fact, + const double &rot_cost_thr, const int &rot_max_iter, const bool &estimat_scale, + const bool& use_optimized_matching = true, const double& distance_threshold = 30.0, const int& num_max_corres = 200); + Eigen::Matrix4d align(const pcl::PointCloud &src, const pcl::PointCloud &dst, bool &if_valid); + private: + void set_params(); + teaser::PointCloud pcl_to_teaser_pcl(const pcl::PointCloud &cloud_in); +}; \ No newline at end of file diff --git a/include/teaser/geometry.h b/include/teaser/geometry.h deleted file mode 100644 index 42ed50f..0000000 --- a/include/teaser/geometry.h +++ /dev/null @@ -1,73 +0,0 @@ -/** - * Copyright 2020, Massachusetts Institute of Technology, - * Cambridge, MA 02139 - * All Rights Reserved - * Authors: Jingnan Shi, et al. (see THANKS for the full author list) - * See LICENSE for the license information - */ - -#pragma once - -#include - -namespace teaser { - -struct PointXYZ { - float x; - float y; - float z; - - friend inline bool operator==(const PointXYZ& lhs, const PointXYZ& rhs) { - return (lhs.x == rhs.x) && (lhs.y == rhs.y) && (lhs.z == rhs.z); - } - friend inline bool operator!=(const PointXYZ& lhs, const PointXYZ& rhs) { return !(lhs == rhs); } -}; - -class PointCloud { -public: - /** - * @brief Default constructor for PointCloud - */ - PointCloud() = default; - - // c++ container named requirements - using value_type = PointXYZ; - using reference = PointXYZ&; - using const_reference = const PointXYZ&; - using difference_type = std::vector::difference_type; - using size_type = std::vector::size_type; - - // iterators - using iterator = std::vector::iterator; - using const_iterator = std::vector::const_iterator; - inline iterator begin() { return points_.begin(); } - inline iterator end() { return points_.end(); } - inline const_iterator begin() const { return points_.begin(); } - inline const_iterator end() const { return points_.end(); } - - // capacity - inline size_t size() const { return points_.size(); } - inline void reserve(size_t n) { points_.reserve(n); } - inline bool empty() { return points_.empty(); } - - // element access - inline PointXYZ& operator[](size_t i) { return points_[i]; } - inline const PointXYZ& operator[](size_t i) const { return points_[i]; } - inline PointXYZ& at(size_t n) { return points_.at(n); } - inline const PointXYZ& at(size_t n) const { return points_.at(n); } - inline PointXYZ& front() { return points_.front(); } - inline const PointXYZ& front() const { return points_.front(); } - inline PointXYZ& back() { return points_.back(); } - inline const PointXYZ& back() const { return points_.back(); } - - inline void push_back(const PointXYZ& pt) { points_.push_back(pt); } - inline void push_back(PointXYZ& pt) { points_.push_back(pt); } - - inline void clear() { points_.clear(); } - -private: - std::vector points_; -}; - - -} // namespace teaser diff --git a/include/teaser/graph.h b/include/teaser/graph.h deleted file mode 100644 index ee80b95..0000000 --- a/include/teaser/graph.h +++ /dev/null @@ -1,276 +0,0 @@ -/** - * Copyright 2020, Massachusetts Institute of Technology, - * Cambridge, MA 02139 - * All Rights Reserved - * Authors: Jingnan Shi, et al. (see THANKS for the full author list) - * See LICENSE for the license information - */ - -#pragma once - -#include -#include -#include - -#include - -#include "teaser/macros.h" - -namespace teaser { - -/** - * A simple undirected graph class - * - * This graph assumes that vertices are numbered. In addition, the vertices numbers have to be - * consecutive starting from 0. - * - * For examples, if the graph have 3 vertices, they have to be named 0, 1, and 2. - */ -class Graph { -public: - Graph() : num_edges_(0){}; - - /** - * Constructor that takes in an adjacency list. Notice that for an edge connecting two arbitrary - * vertices v1 & v2, we assume that v2 exists in v1's list, and v1 also exists in v2's list. This - * condition is not enforced. If violated, removeEdge() function might exhibit undefined - * behaviors. - * @param [in] adj_list an map representing an adjacency list - */ - explicit Graph(const std::map>& adj_list) { - adj_list_.resize(adj_list.size()); - num_edges_ = 0; - for (const auto& e_list : adj_list) { - const auto& v = e_list.first; - adj_list_[e_list.first] = e_list.second; - num_edges_ += e_list.second.size(); - } - num_edges_ /= 2; - }; - - /** - * Add a vertex with no edges. - * @param [in] id the id of vertex to be added - */ - void addVertex(const int& id) { - if (id < adj_list_.size()) { - TEASER_DEBUG_ERROR_MSG("Vertex already exists."); - } else { - adj_list_.resize(id + 1); - } - } - - /** - * Populate the graph with the provided number of vertices without any edges. - * @param num_vertices - */ - void populateVertices(const int& num_vertices) { adj_list_.resize(num_vertices); } - - /** - * Return true if said edge exists - * @param [in] vertex_1 - * @param [in] vertex_2 - */ - bool hasEdge(const int& vertex_1, const int& vertex_2) { - if (vertex_1 >= adj_list_.size() || vertex_2 >= adj_list_.size()) { - return false; - } - auto& connected_vs = adj_list_[vertex_1]; - bool exists = - std::find(connected_vs.begin(), connected_vs.end(), vertex_2) != connected_vs.end(); - return exists; - } - - /** - * Return true if the vertex exists. - * @param vertex - * @return - */ - bool hasVertex(const int& vertex) { return vertex < adj_list_.size(); } - - /** - * Add an edge between two vertices - * @param [in] vertex_1 one vertex of the edge - * @param [in] vertex_2 another vertex of the edge - */ - void addEdge(const int& vertex_1, const int& vertex_2) { - if (hasEdge(vertex_1, vertex_2)) { - TEASER_DEBUG_ERROR_MSG("Edge exists."); - return; - } - adj_list_[vertex_1].push_back(vertex_2); - adj_list_[vertex_2].push_back(vertex_1); - num_edges_++; - } - - /** - * Remove the edge between two vertices. - * @param [in] vertex_1 one vertex of the edge - * @param [in] vertex_2 another vertex of the edge - */ - void removeEdge(const int& vertex_1, const int& vertex_2) { - if (vertex_1 >= adj_list_.size() || vertex_2 >= adj_list_.size()) { - TEASER_DEBUG_ERROR_MSG("Trying to remove non-existent edge."); - return; - } - - adj_list_[vertex_1].erase( - std::remove(adj_list_[vertex_1].begin(), adj_list_[vertex_1].end(), vertex_2), - adj_list_[vertex_1].end()); - adj_list_[vertex_2].erase( - std::remove(adj_list_[vertex_2].begin(), adj_list_[vertex_2].end(), vertex_1), - adj_list_[vertex_2].end()); - - num_edges_--; - } - - /** - * Get the number of vertices - * @return total number of vertices - */ - [[nodiscard]] int numVertices() const { return adj_list_.size(); } - - /** - * Get the number of edges - * @return total number of edges - */ - [[nodiscard]] int numEdges() const { return num_edges_; } - - /** - * Get edges originated from a specific vertex - * @param [in] id - * @return an unordered set of edges - */ - [[nodiscard]] const std::vector& getEdges(int id) const { return adj_list_[id]; } - - /** - * Get all vertices - * @return a vector of all vertices - */ - [[nodiscard]] std::vector getVertices() const { - std::vector v; - for (int i = 0; i < adj_list_.size(); ++i) { - v.push_back(i); - } - return v; - } - - [[nodiscard]] Eigen::MatrixXi getAdjMatrix() const { - const int num_v = numVertices(); - Eigen::MatrixXi adj_matrix(num_v, num_v); - for (size_t i = 0; i < num_v; ++i) { - const auto& c_edges = getEdges(i); - for (size_t j = 0; j < num_v; ++j) { - if (std::find(c_edges.begin(), c_edges.end(), j) != c_edges.end()) { - adj_matrix(i, j) = 1; - } else { - adj_matrix(i, j) = 0; - } - } - } - return adj_matrix; - } - - [[nodiscard]] std::vector> getAdjList() const { return adj_list_; } - - /** - * Preallocate spaces for vertices - * @param num_vertices - */ - void reserve(const int& num_vertices) { adj_list_.reserve(num_vertices); } - - /** - * Clear the contents of the graph - */ - void clear() { - adj_list_.clear(); - num_edges_ = 0; - } - - /** - * Reserve space for complete graph. A complete undirected graph should have N*(N-1)/2 edges - * @param num_vertices - */ - void reserveForCompleteGraph(const int& num_vertices) { - adj_list_.reserve(num_vertices); - for (int i = 0; i < num_vertices - 1; ++i) { - std::vector c_edges; - c_edges.reserve(num_vertices - 1); - adj_list_.push_back(c_edges); - } - adj_list_.emplace_back(std::initializer_list{}); - } - -private: - std::vector> adj_list_; - size_t num_edges_; -}; - -/** - * A facade to the Parallel Maximum Clique (PMC) library. - * - * For details about PMC, please refer to: - * https://github.com/ryanrossi/pmc - * and - * Ryan A. Rossi, David F. Gleich, Assefaw H. Gebremedhin, Md. Mostofa Patwary, A Fast Parallel - * Maximum Clique Algorithm for Large Sparse Graphs and Temporal Strong Components, arXiv preprint - * 1302.6256, 2013. - */ -class MaxCliqueSolver { -public: - /** - * Enum representing the solver algorithm to use - */ - enum class CLIQUE_SOLVER_MODE { - PMC_EXACT = 0, - PMC_HEU = 1, - KCORE_HEU = 2, - }; - - /** - * Parameter struct for MaxCliqueSolver - */ - struct Params { - - /** - * Algorithm used for finding max clique. - */ - CLIQUE_SOLVER_MODE solver_mode = CLIQUE_SOLVER_MODE::PMC_EXACT; - - /** - * \deprecated Use solver_mode instead - * Set this to false to enable heuristic-only max clique finding. - */ - bool solve_exactly = true; - - /** - * The threshold ratio for determining whether to skip max clique and go straightly to - * GNC rotation estimation. Set this to 1 to always use exact max clique selection, 0 to always - * skip exact max clique selection. - */ - double kcore_heuristic_threshold = 1; - - /** - * Time limit on running the solver. - */ - double time_limit = 3600; - }; - - MaxCliqueSolver() = default; - - MaxCliqueSolver(Params params) : params_(params){}; - - /** - * Find the maximum clique within the graph provided. By maximum clique, it means the clique of - * the largest size in an undirected graph. - * @param graph - * @return a vector of indices of cliques - */ - std::vector findMaxClique(Graph graph); - -private: - Graph graph_; - Params params_; -}; - -} // namespace teaser diff --git a/include/teaser/linalg.h b/include/teaser/linalg.h deleted file mode 100644 index c920ecf..0000000 --- a/include/teaser/linalg.h +++ /dev/null @@ -1,101 +0,0 @@ -/** - * Copyright (c) 2020, Massachusetts Institute of Technology, - * Cambridge, MA 02139 - * All Rights Reserved - * Authors: Jingnan Shi, et al. (see THANKS for the full author list) - * See LICENSE for the license information - */ - -#pragma once - -#include - -#include -#include -#include - -namespace teaser { - -/** - * Return the hat map of the provided vector (a skew symmetric matrix). - * @param u 3-by-1 vector - * @param x 3-by-3 skew symmetric matrix - */ -Eigen::Matrix hatmap(const Eigen::Matrix& u) { - Eigen::Matrix x; - // clang-format off - x << 0, -u(2), u(1), - u(2), 0, -u(0), - -u(1), u(0), 0; - // clang-format on - return x; -} - -/** - * Vector-vector kronecker product function with fixed-size output - * @tparam NumT - * @tparam N size of the first vector - * @tparam M size of the second vector - * @param v1 [in] first vector - * @param v2 [in] second vector - * @param output [out] output vector - */ -template -void vectorKron(const Eigen::Matrix& v1, const Eigen::Matrix& v2, - Eigen::Matrix* output) { -#pragma omp parallel for collapse(2) shared(v1, v2, output) default(none) - for (size_t i = 0; i < N; ++i) { - for (size_t j = 0; j < M; ++j) { - (*output)[i * M + j] = v1[i] * v2[j]; - } - } -} - -/** - * Vector-vector kronecker product function with dynamic-size output - * @tparam NumT numerical type for Eigen matrices (double, float, etc.) - * @param v1 [in] first vector - * @param v2 [in] second vector - * @return Result of kronecker product - */ -template -Eigen::Matrix vectorKron(const Eigen::Matrix& v1, - const Eigen::Matrix& v2) { - Eigen::Matrix output(v1.rows() * v2.rows(), 1); -#pragma omp parallel for collapse(2) shared(v1, v2, output) default(none) - for (size_t i = 0; i < v1.rows(); ++i) { - for (size_t j = 0; j < v2.rows(); ++j) { - output[i * v2.rows() + j] = v1[i] * v2[j]; - } - } - return output; -} - -/** - * Find the nearest (in Frobenius norm) Symmetric Positive Definite matrix to A - * - * See: https://www.sciencedirect.com/science/article/pii/0024379588902236 - * - * @tparam NumT numerical type for Eigen matrices (double, float, etc.) - * @param A [in] input matrix - * @param nearestPSD [out] output neaest positive semi-definite matrix - * @param eig_threshold [in] optional threshold of determining the smallest eigen values - */ -template -void getNearestPSD(const Eigen::Matrix& A, - Eigen::Matrix* nearestPSD) { - assert(A.rows() == A.cols()); - nearestPSD->resize(A.rows(), A.cols()); - - // symmetrize A into B - Eigen::MatrixXd B = (A + A.transpose()) / 2; - - // eigendecomposition of B - Eigen::SelfAdjointEigenSolver eig_B(B); - Eigen::VectorXd De = eig_B.eigenvalues(); - Eigen::MatrixXd De_positive = (De.array() < 0).select(0, De).asDiagonal(); - Eigen::MatrixXd Ve = eig_B.eigenvectors(); - *nearestPSD = Ve * De_positive * Ve.transpose(); -} - -} // namespace teaser \ No newline at end of file diff --git a/include/teaser/macros.h b/include/teaser/macros.h deleted file mode 100644 index b6be4c9..0000000 --- a/include/teaser/macros.h +++ /dev/null @@ -1,69 +0,0 @@ -/** - * Copyright 2020, Massachusetts Institute of Technology, - * Cambridge, MA 02139 - * All Rights Reserved - * Authors: Jingnan Shi, et al. (see THANKS for the full author list) - * See LICENSE for the license information - */ - -#pragma once - -#include - -#define TEASER_DEBUG_ERROR_VAR(x) \ - do { \ - std::cerr << #x << " = " << x << std::endl; \ - } while (0) - -#define TEASER_INFO_MSG(x) \ - do { \ - std::cout << x; \ - } while (0) - -#define TEASER_INFO_MSG_THROTTLE(x, i, N) \ - do { \ - if (i % N == 0) { \ - std::cout << x; \ - } \ - } while (0) - -#if defined(NDEBUG) && !defined(TEASER_DIAG_PRINT) -// Use NOOP to turn off the defined debug macros -#define TEASER_DEBUG_ERROR_MSG(x) \ - do { \ - } while (0) -#define TEASER_DEBUG_INFO_MSG(x) \ - do { \ - } while (0) -// Timing macros -#define TEASER_DEBUG_DECLARE_TIMING(s) \ - do { \ - } while (0) -#define TEASER_DEBUG_START_TIMING(s) \ - do { \ - } while (0) -#define TEASER_DEBUG_STOP_TIMING(s) \ - do { \ - } while (0) -#define TEASER_DEBUG_GET_TIMING(s) \ - do { \ - } while (0) -#else -// Debug messages -#define TEASER_DEBUG_ERROR_MSG(x) \ - do { \ - std::cerr << x << std::endl; \ - } while (0) -#define TEASER_DEBUG_INFO_MSG(x) \ - do { \ - std::cout << x << std::endl; \ - } while (0) -// Timing macros -#define TEASER_DEBUG_DECLARE_TIMING(s) std::chrono::steady_clock clock_##s; -#define TEASER_DEBUG_START_TIMING(s) auto t_start_##s = clock_##s.now(); -#define TEASER_DEBUG_STOP_TIMING(s) \ - auto t_end_##s = clock_##s.now(); \ - std::chrono::duration diff_dur_##s = t_end_##s - t_start_##s; \ - double diff_##s = diff_dur_##s.count(); -#define TEASER_DEBUG_GET_TIMING(s)(double)(diff_##s / 1000.0) -#endif \ No newline at end of file diff --git a/include/teaser/utils.h b/include/teaser/utils.h deleted file mode 100644 index 16800f7..0000000 --- a/include/teaser/utils.h +++ /dev/null @@ -1,203 +0,0 @@ -/** - * Copyright 2020, Massachusetts Institute of Technology, - * Cambridge, MA 02139 - * All Rights Reserved - * Authors: Jingnan Shi, et al. (see THANKS for the full author list) - * See LICENSE for the license information - */ - -#pragma once - -#include -#include - -#include -#include -#include -#include - -namespace teaser { -namespace utils { - -/** - * A templated random sample function (w/o replacement). Based on MATLAB implementation of - * randsample() - * @tparam T A number type - * @tparam URBG A UniformRandomBitGenerator type - * @param input A input vector containing the whole population - * @param num_samples Number of samples we want - * @param g - * @return - */ -template -std::vector randomSample(std::vector input, size_t num_samples, URBG&& g) { - - std::vector output; - output.reserve(num_samples); - if (4 * num_samples > input.size()) { - // if the sample is a sizeable fraction of the whole population, - // just randomly shuffle the entire population and return the - // first num_samples - std::shuffle(input.begin(), input.end(), g); - for (size_t i = 0; i < num_samples; ++i) { - output.push_back(input[i]); - } - } else { - // if the sample is small, repeatedly sample with replacement until num_samples - // unique values - std::unordered_set sample_indices; - std::uniform_int_distribution<> dis(0, input.size()); - while (sample_indices.size() < num_samples) { - sample_indices.insert(dis(std::forward(g))); - } - for (auto&& i : sample_indices) { - output.push_back(input[i]); - } - } - return output; -} - -/** - * Remove one row from matrix. - * Credit to: https://stackoverflow.com/questions/13290395 - * @param matrix an Eigen::Matrix. - * @param rowToRemove index of row to remove. If >= matrix.rows(), no operation will be taken - */ -template -void removeRow(Eigen::Matrix& matrix, unsigned int rowToRemove) { - if (rowToRemove >= matrix.rows()) { - return; - } - unsigned int numRows = matrix.rows() - 1; - unsigned int numCols = matrix.cols(); - - if (rowToRemove < numRows) { - matrix.block(rowToRemove, 0, numRows - rowToRemove, numCols) = - matrix.bottomRows(numRows - rowToRemove); - } - - matrix.conservativeResize(numRows, numCols); -} - -/** - * Remove one column from matrix. - * Credit to: https://stackoverflow.com/questions/13290395 - * @param matrix - * @param colToRemove index of col to remove. If >= matrix.cols(), no operation will be taken - */ -template -void removeColumn(Eigen::Matrix& matrix, unsigned int colToRemove) { - if (colToRemove >= matrix.cols()) { - return; - } - unsigned int numRows = matrix.rows(); - unsigned int numCols = matrix.cols() - 1; - - if (colToRemove < numCols) { - matrix.block(0, colToRemove, numRows, numCols - colToRemove) = - matrix.rightCols(numCols - colToRemove); - } - - matrix.conservativeResize(numRows, numCols); -} - -/** - * Helper function to calculate the diameter of a row vector of points - * @param X - * @return the diameter of the set of points given - */ -template float calculateDiameter(const Eigen::Matrix& X) { - Eigen::Matrix cog = X.rowwise().sum() / X.cols(); - Eigen::Matrix P = X.colwise() - cog; - Eigen::Matrix temp = P.array().square().colwise().sum(); - return 2 * std::sqrt(temp.maxCoeff()); -} - -/** - * Helper function to use svd to estimate rotation. - * Method described here: http://igl.ethz.ch/projects/ARAP/svd_rot.pdf - * @param X - * @param Y - * @return a rotation matrix R - */ -inline Eigen::Matrix3d svdRot(const Eigen::Matrix& X, - const Eigen::Matrix& Y, - const Eigen::Matrix& W, - int static_count) { - // Assemble the correlation matrix H = X * Y' - Eigen::Matrix3d H = X * W.asDiagonal() * Y.transpose(); - - Eigen::JacobiSVD svd(H, Eigen::ComputeFullU | Eigen::ComputeFullV); - Eigen::Matrix3d U = svd.matrixU(); - Eigen::Matrix3d V = svd.matrixV(); - -// std::string SAVE_PATH = "/home/shapelim/kitti_output/weights"; -// std::string svdPath; -// std::ofstream svdTxt; -// svdPath = (boost::format("%s/TEASER_%04d_singular_values.txt") % SAVE_PATH % static_count).str(); -// // save singular values -// svdTxt.open(svdPath, std::ios::app); -// const auto& sgValues = svd.singularValues(); -// svdTxt<& X, - const Eigen::Matrix& Y, - const Eigen::Matrix& W) { - // Assemble the correlation matrix H = X * Y' - Eigen::Matrix2d H = X * W.asDiagonal() * Y.transpose(); - - Eigen::JacobiSVD svd(H, Eigen::ComputeFullU | Eigen::ComputeFullV); - Eigen::Matrix2d U = svd.matrixU(); - Eigen::Matrix2d V = svd.matrixV(); - - if (U.determinant() * V.determinant() < 0) { - V.col(1) *= -1; - } - - return V * U.transpose(); -} - -/** - * Use an boolean Eigen matrix to mask a vector - * @param mask a 1-by-N boolean Eigen matrix - * @param elements vector to be masked - * @return - */ -template -inline std::vector maskVector(Eigen::Matrix mask, - const std::vector& elements) { - assert(mask.cols() == elements.size()); - std::vector result; - for (size_t i = 0; i < mask.cols(); ++i) { - if (mask(i)) { - result.emplace_back(elements[i]); - } - } - return result; -} - -/** - * Get indices of non-zero elements in an Eigen row vector - * @param mask a 1-by-N boolean Eigen matrix - * @return A vector containing indices of the true elements in the row vector - */ -template inline std::vector findNonzero(Eigen::Matrix mask) { - std::vector result; - for (size_t i = 0; i < mask.cols(); ++i) { - if (mask(i)) { - result.emplace_back(i); - } - } - return result; -} - -} // namespace utils -} // namespace teaser diff --git a/include/teaser_utils/feature_matcher.h b/include/teaser_utils/feature_matcher.h deleted file mode 100644 index f1b8d8c..0000000 --- a/include/teaser_utils/feature_matcher.h +++ /dev/null @@ -1,94 +0,0 @@ -/** - * Copyright 2020, Massachusetts Institute of Technology, - * Cambridge, MA 02139 - * All Rights Reserved - * Authors: Jingnan Shi, et al. (see THANKS for the full author list) - * See LICENSE for the license information - */ - -#pragma once - -#include -#include -#include -#include "teaser/geometry.h" -#include "./fpfh.h" - -namespace teaser { - -class Matcher { -public: - typedef std::vector Feature; - typedef flann::Index> KDTree; - - // New methods - // Public methods: - // 1. calculateCorrespondences - // input: source point cloud, target point cloud - // output: correspondences - Matcher() = default; - - /** - * Calculate correspondences based on given features and point clouds. - * @param source_points - * @param target_points - * @param use_absolute_scaleS - * @param use_crosscheck - * @param use_tuple_test - * @return - */ - -// template - std::vector> calculateCorrespondences(teaser::PointCloud& source_points, teaser::PointCloud& target_points, - FPFHCloud & source_features, FPFHCloud& target_features, - bool use_absolute_scale = true, bool use_crosscheck = true, - bool use_tuple_test = true, float tuple_scale = 0){ - Feature cloud_features; - pointcloud_.push_back(source_points); - pointcloud_.push_back(target_points); - - // It compute the global_scale_ required to set correctly the search radius - normalizePoints(use_absolute_scale); - int size_descriptor = source_features[0].descriptorSize(); - - for (auto& f : source_features) { - Eigen::VectorXf fpfh(size_descriptor); - for (int i = 0; i < size_descriptor; i++) - fpfh(i) = f.histogram[i]; - cloud_features.push_back(fpfh); - } - features_.push_back(cloud_features); - - cloud_features.clear(); - for (auto& f : target_features) { - Eigen::VectorXf fpfh(size_descriptor); - for (int i = 0; i < size_descriptor; i++) - fpfh(i) = f.histogram[i]; - cloud_features.push_back(fpfh); - } - features_.push_back(cloud_features); - - advancedMatching(use_crosscheck, use_tuple_test, tuple_scale); - - return corres_; - } - -private: - template void buildKDTree(const std::vector& data, KDTree* tree); - - template - void searchKDTree(KDTree* tree, const T& input, std::vector& indices, - std::vector& dists, int nn); - - void advancedMatching(bool use_crosscheck, bool use_tuple_test, float tuple_scale); - - void normalizePoints(bool use_absolute_scale); - - std::vector> corres_; - std::vector pointcloud_; - std::vector features_; - std::vector > means_; // for normalization - float global_scale_; -}; - -} // namespace teaser diff --git a/include/teaser_utils/geometry.h b/include/teaser_utils/geometry.h deleted file mode 100644 index 42ed50f..0000000 --- a/include/teaser_utils/geometry.h +++ /dev/null @@ -1,73 +0,0 @@ -/** - * Copyright 2020, Massachusetts Institute of Technology, - * Cambridge, MA 02139 - * All Rights Reserved - * Authors: Jingnan Shi, et al. (see THANKS for the full author list) - * See LICENSE for the license information - */ - -#pragma once - -#include - -namespace teaser { - -struct PointXYZ { - float x; - float y; - float z; - - friend inline bool operator==(const PointXYZ& lhs, const PointXYZ& rhs) { - return (lhs.x == rhs.x) && (lhs.y == rhs.y) && (lhs.z == rhs.z); - } - friend inline bool operator!=(const PointXYZ& lhs, const PointXYZ& rhs) { return !(lhs == rhs); } -}; - -class PointCloud { -public: - /** - * @brief Default constructor for PointCloud - */ - PointCloud() = default; - - // c++ container named requirements - using value_type = PointXYZ; - using reference = PointXYZ&; - using const_reference = const PointXYZ&; - using difference_type = std::vector::difference_type; - using size_type = std::vector::size_type; - - // iterators - using iterator = std::vector::iterator; - using const_iterator = std::vector::const_iterator; - inline iterator begin() { return points_.begin(); } - inline iterator end() { return points_.end(); } - inline const_iterator begin() const { return points_.begin(); } - inline const_iterator end() const { return points_.end(); } - - // capacity - inline size_t size() const { return points_.size(); } - inline void reserve(size_t n) { points_.reserve(n); } - inline bool empty() { return points_.empty(); } - - // element access - inline PointXYZ& operator[](size_t i) { return points_[i]; } - inline const PointXYZ& operator[](size_t i) const { return points_[i]; } - inline PointXYZ& at(size_t n) { return points_.at(n); } - inline const PointXYZ& at(size_t n) const { return points_.at(n); } - inline PointXYZ& front() { return points_.front(); } - inline const PointXYZ& front() const { return points_.front(); } - inline PointXYZ& back() { return points_.back(); } - inline const PointXYZ& back() const { return points_.back(); } - - inline void push_back(const PointXYZ& pt) { points_.push_back(pt); } - inline void push_back(PointXYZ& pt) { points_.push_back(pt); } - - inline void clear() { points_.clear(); } - -private: - std::vector points_; -}; - - -} // namespace teaser diff --git a/include/utility.h b/include/utility.h deleted file mode 100755 index 36133e9..0000000 --- a/include/utility.h +++ /dev/null @@ -1,201 +0,0 @@ -#ifndef _UTILITY_LIDAR_ODOMETRY_H_ -#define _UTILITY_LIDAR_ODOMETRY_H_ - - -#include -#include - -#include -#include -#include - -#include "quatro/cloud_info.h" - -#include - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -using namespace std; -using PointType = pcl::PointXYZ; - -typedef pcl::PointXYZI PointTypeIP; - -//Velodyne 64 HDE -// extern const int N_SCAN = 64; -// extern const int Horizon_SCAN = 1800; //1028~4500 -// extern const float ang_res_x = 360.0/float(Horizon_SCAN); -// extern const float ang_res_y = 26.9/float(N_SCAN-1);//28.0/float(N_SCAN-1); -// extern const float ang_bottom = 25.0; -// extern const int groundScanInd = 60; // 60 ; - -// VLP-16 -// extern const int N_SCAN = 16; -// extern const int Horizon_SCAN = 1800; -// extern const float ang_res_x = 0.2; -// extern const float ang_res_y = 2.0; -// extern const float ang_bottom = 15.0+0.1; -// extern const int groundScanInd = 7; - -// HDL-32E -// extern const int N_SCAN = 32; -// extern const int Horizon_SCAN = 1800; -// extern const float ang_res_x = 360.0/float(Horizon_SCAN); -// extern const float ang_res_y = 41.33/float(N_SCAN-1); -// extern const float ang_bottom = 30.67; -// extern const int groundScanInd = 20; - -// Ouster users may need to uncomment line 159 in imageProjection.cpp -// Usage of Ouster imu data is not fully supported yet, please just publish point cloud data -// Ouster OS1-16 -// extern const int N_SCAN = 16; -// extern const int Horizon_SCAN = 1024; -// extern const float ang_res_x = 360.0/float(Horizon_SCAN); -// extern const float ang_res_y = 33.2/float(N_SCAN-1); -// extern const float ang_bottom = 16.6+0.1; -// extern const int groundScanInd = 7; - -// Ouster OS1-64 -// extern const int N_SCAN = 64; -// extern const int Horizon_SCAN = 1024; -// extern const float ang_res_x = 360.0/float(Horizon_SCAN); -// extern const float ang_res_y = 33.2/float(N_SCAN-1); -// extern const float ang_bottom = 16.6+0.1; -// extern const int groundScanInd = 15; - -extern const bool loopClosureEnableFlag = true; -extern const double mappingProcessInterval = 0.3; - -extern const float scanPeriod = 0.1; -extern const int systemDelay = 0; -extern const int imuQueLength = 200; - -// extern const float sensorMountAngle = 0.0; - -// //segmentation threshold -// extern const float segmentTheta = 60.0/180.0*M_PI; // decrese this value may improve accuracy //60.0/180.0*M_PI - -// //If number of segment is below than 30, check line number. this for minimum number of point for it -// extern const int segmentValidPointNum = 5; - -// //if number of segment is small, number of line is checked, this is threshold for it. -// extern const int segmentValidLineNum = 3; - -// extern const float segmentAlphaX = ang_res_x / 180.0 * M_PI; -// extern const float segmentAlphaY = ang_res_y / 180.0 * M_PI; - - -//extern const int edgeFeatureNum = 2; -//extern const int surfFeatureNum = 4; -//extern const int sectionsTotal = 6; -//extern const float edgeThreshold = 0.1; -//extern const float surfThreshold = 0.1; -//extern const float nearestFeatureSearchSqDist = 25; -// -// -//// Mapping Params -//extern const float surroundingKeyframeSearchRadius = 50.0; // key frame that is within n meters from current pose will be considerd for scan-to-map optimization (when loop closure disabled) -//extern const int surroundingKeyframeSearchNum = 50; // submap size (when loop closure enabled) -//// history key frames (history submap for loop closure) -//extern const float historyKeyframeSearchRadius = 7.0; // key frame that is within n meters from current pose will be considerd for loop closure -//extern const int historyKeyframeSearchNum = 25; // 2n+1 number of hostory key frames will be fused into a submap for loop closure -//extern const float historyKeyframeFitnessScore = 0.3; // the smaller the better alignment -// -//extern const float globalMapVisualizationSearchRadius = 500.0; // key frames with in n meters will be visualized - - -struct smoothness_t{ - float value; - size_t ind; -}; - -struct by_value{ - bool operator()(smoothness_t const &left, smoothness_t const &right) { - return left.value < right.value; - } -}; - -/* - * A point cloud type that has 6D pose info ([x,y,z,roll,pitch,yaw] intensity is time stamp) - */ -struct PointXYZIRPYT -{ - PCL_ADD_POINT4D - PCL_ADD_INTENSITY; - float roll; - float pitch; - float yaw; - double time; - EIGEN_MAKE_ALIGNED_OPERATOR_NEW -} EIGEN_ALIGN16; - -POINT_CLOUD_REGISTER_POINT_STRUCT (PointXYZIRPYT, - (float, x, x) (float, y, y) - (float, z, z) (float, intensity, intensity) - (float, roll, roll) (float, pitch, pitch) (float, yaw, yaw) - (double, time, time) -) - -typedef PointXYZIRPYT PointTypePose; - -void setCorrespondenceMarker(const pcl::PointCloud& src_matched, const pcl::PointCloud& tgt_matched, - visualization_msgs::Marker& marker, float thickness=0.1, std::vector rgb_color={0.0, 0.0, 0.0}, int id=0){ - if (!marker.points.empty()) marker.points.clear(); - marker.header.frame_id = "map"; - marker.header.stamp = ros::Time(); - marker.ns = "my_namespace"; - marker.id = id; // To avoid overlap - marker.type = visualization_msgs::Marker::LINE_LIST; - marker.action = visualization_msgs::Marker::ADD; - - marker.scale.x = thickness; // thickness - marker.color.r = rgb_color[0]; - marker.color.g = rgb_color[1]; - marker.color.b = rgb_color[2]; - marker.color.a = 1.0; // Don't forget to set the alpha! - - geometry_msgs::Point srcP; - geometry_msgs::Point tgtP; - assert(src_matched.size() == tgt_matched.size()); - for (int idx = 0; idx < src_matched.size(); ++idx){ - PointType sP = src_matched[idx]; - PointType sT = tgt_matched[idx]; - srcP.x = sP.x; srcP.y = sP.y; srcP.z = sP.z; - tgtP.x = sT.x; tgtP.y = sT.y; tgtP.z = sT.z; - - marker.points.emplace_back(srcP); - marker.points.emplace_back(tgtP); - } -} - -#endif diff --git a/launch/quatro.launch b/launch/quatro.launch deleted file mode 100755 index e28ba5a..0000000 --- a/launch/quatro.launch +++ /dev/null @@ -1,15 +0,0 @@ - - - - - - -"quatro" - - - - - - - - diff --git a/materials/README_demo_v2.gif b/materials/README_demo_v2.gif deleted file mode 100644 index e70921b..0000000 Binary files a/materials/README_demo_v2.gif and /dev/null differ diff --git a/materials/kitti_for_readme.PNG b/materials/kitti_for_readme.PNG deleted file mode 100644 index 3189ce4..0000000 Binary files a/materials/kitti_for_readme.PNG and /dev/null differ diff --git a/materials/labs_for_readme.PNG b/materials/labs_for_readme.PNG deleted file mode 100644 index 24f36f6..0000000 Binary files a/materials/labs_for_readme.PNG and /dev/null differ diff --git a/materials/quatro_inner.png b/materials/quatro_inner.png deleted file mode 100644 index 27ec7f2..0000000 Binary files a/materials/quatro_inner.png and /dev/null differ diff --git a/materials/quatro_output.png b/materials/quatro_output.png deleted file mode 100644 index b018cd6..0000000 Binary files a/materials/quatro_output.png and /dev/null differ diff --git a/materials/quatro_overview.PNG b/materials/quatro_overview.PNG deleted file mode 100644 index bf8526e..0000000 Binary files a/materials/quatro_overview.PNG and /dev/null differ diff --git a/materials/seg_fault.png b/materials/seg_fault.png deleted file mode 100644 index 26dc62e..0000000 Binary files a/materials/seg_fault.png and /dev/null differ diff --git a/materials/title_img.png b/materials/title_img.png deleted file mode 100644 index 13a110f..0000000 Binary files a/materials/title_img.png and /dev/null differ diff --git a/msg/cloud_info.msg b/msg/cloud_info.msg deleted file mode 100755 index 5bd5104..0000000 --- a/msg/cloud_info.msg +++ /dev/null @@ -1,12 +0,0 @@ -Header header - -int32[] startRingIndex -int32[] endRingIndex - -float32 startOrientation -float32 endOrientation -float32 orientationDiff - -bool[] segmentedCloudGroundFlag # true - ground point, false - other points -uint32[] segmentedCloudColInd # point column index in range image -float32[] segmentedCloudRange # point range \ No newline at end of file diff --git a/package.xml b/package.xml old mode 100755 new mode 100644 index d672810..4869263 --- a/package.xml +++ b/package.xml @@ -1,33 +1,19 @@ + + quatro - 0.0.0 - Robust global registration + 1.0.0 + quatro + Eungchang Mason Lee Hyungtae Lim + GPL3 - - - - Creative Commons Attribution-NonCommercial-ShareAlike 4.0 License catkin - message_generation - libpcl-all - roscpp - rospy - std_msgs - nav_msgs - sensor_msgs - roscpp - rospy - std_msgs - nav_msgs - roslaunch - libpcl-all-dev - message_generation - sensor_msgs - + - + + \ No newline at end of file diff --git a/rviz/reg_output.rviz b/rviz/reg_output.rviz deleted file mode 100644 index 4f84345..0000000 --- a/rviz/reg_output.rviz +++ /dev/null @@ -1,212 +0,0 @@ -Panels: - - Class: rviz/Displays - Help Height: 78 - Name: Displays - Property Tree Widget: - Expanded: - - /Global Options1 - - /Status1 - - /tgt-green1/Autocompute Value Bounds1 - Splitter Ratio: 0.7464788556098938 - Tree Height: 462 - - Class: rviz/Selection - Name: Selection - - Class: rviz/Tool Properties - Expanded: - - /2D Pose Estimate1 - - /2D Nav Goal1 - - /Publish Point1 - Name: Tool Properties - Splitter Ratio: 0.5886790156364441 - - Class: rviz/Views - Expanded: - - /Current View1 - Name: Views - Splitter Ratio: 0.5 - - Class: rviz/Time - Experimental: true - Name: Time - SyncMode: 0 - SyncSource: src-red -Preferences: - PromptSaveOnExit: true -Toolbars: - toolButtonStyle: 2 -Visualization Manager: - Class: "" - Displays: - - Alpha: 0.5 - Cell Size: 1 - Class: rviz/Grid - Color: 160; 160; 164 - Enabled: false - Line Style: - Line Width: 0.029999999329447746 - Value: Lines - Name: Grid - Normal Cell Count: 0 - Offset: - X: 0 - Y: 0 - Z: 0 - Plane: XY - Plane Cell Count: 100 - Reference Frame: - Value: false - - Class: rviz/Axes - Enabled: false - Length: 3 - Name: Axes - Radius: 0.5 - Reference Frame: - Value: false - - Alpha: 1 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 10 - Min Value: -10 - Value: true - Axis: Z - Channel Name: intensity - Class: rviz/PointCloud2 - Color: 255; 25; 25 - Color Transformer: FlatColor - Decay Time: 0 - Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Min Color: 0; 0; 0 - Name: src-red - Position Transformer: XYZ - Queue Size: 10 - Selectable: true - Size (Pixels): 3 - Size (m): 0.15000000596046448 - Style: Flat Squares - Topic: /source - Unreliable: false - Use Fixed Frame: true - Use rainbow: true - Value: true - - Alpha: 1 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 2.2932701110839844 - Min Value: -0.07719100266695023 - Value: false - Axis: Z - Channel Name: intensity - Class: rviz/PointCloud2 - Color: 25; 255; 25 - Color Transformer: FlatColor - Decay Time: 0 - Enabled: true - Invert Rainbow: false - Max Color: 25; 255; 25 - Min Color: 0; 0; 0 - Name: tgt-green - Position Transformer: XYZ - Queue Size: 10 - Selectable: true - Size (Pixels): 3 - Size (m): 0.15000000596046448 - Style: Flat Squares - Topic: /target - Unreliable: false - Use Fixed Frame: true - Use rainbow: true - Value: true - - Alpha: 1 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 10 - Min Value: -10 - Value: true - Axis: Z - Channel Name: intensity - Class: rviz/PointCloud2 - Color: 25; 25; 255 - Color Transformer: FlatColor - Decay Time: 0 - Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Min Color: 0; 0; 0 - Name: align-blue - Position Transformer: XYZ - Queue Size: 10 - Selectable: true - Size (Pixels): 3 - Size (m): 0.15000000596046448 - Style: Flat Squares - Topic: /align - Unreliable: false - Use Fixed Frame: true - Use rainbow: true - Value: true - Enabled: true - Global Options: - Background Color: 255; 255; 255 - Default Light: true - Fixed Frame: map - Frame Rate: 30 - Name: root - Tools: - - Class: rviz/Interact - Hide Inactive Objects: true - - Class: rviz/MoveCamera - - Class: rviz/Select - - Class: rviz/FocusCamera - - Class: rviz/Measure - - Class: rviz/SetInitialPose - Theta std deviation: 0.2617993950843811 - Topic: /initialpose - X std deviation: 0.5 - Y std deviation: 0.5 - - Class: rviz/SetGoal - Topic: /move_base_simple/goal - - Class: rviz/PublishPoint - Single click: true - Topic: /clicked_point - Value: true - Views: - Current: - Class: rviz/ThirdPersonFollower - Distance: 81.53340148925781 - Enable Stereo Rendering: - Stereo Eye Separation: 0.05999999865889549 - Stereo Focal Distance: 1 - Swap Stereo Eyes: false - Value: false - Focal Point: - X: 7.51470422744751 - Y: -10.406728744506836 - Z: 5.616979251499288e-6 - Focal Shape Fixed Size: false - Focal Shape Size: 0.05000000074505806 - Invert Z Axis: false - Name: Current View - Near Clip Distance: 0.009999999776482582 - Pitch: 1.51979660987854 - Target Frame: base_link - Value: ThirdPersonFollower (rviz) - Yaw: 4.742389678955078 - Saved: ~ -Window Geometry: - Displays: - collapsed: false - Height: 759 - Hide Left Dock: false - Hide Right Dock: true - QMainWindow State: 000000ff00000000fd0000000400000000000001ac00000259fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d00000259000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000363fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d00000363000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000056d0000003efc0100000002fb0000000800540069006d006501000000000000056d0000031b00fffffffb0000000800540069006d00650100000000000004500000000000000000000003bb0000025900000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 - Selection: - collapsed: false - Time: - collapsed: false - Tool Properties: - collapsed: false - Views: - collapsed: true - Width: 1389 - X: 2317 - Y: 67 diff --git a/rviz/registration.rviz b/rviz/registration.rviz deleted file mode 100644 index 09898cc..0000000 --- a/rviz/registration.rviz +++ /dev/null @@ -1,524 +0,0 @@ -Panels: - - Class: rviz/Displays - Help Height: 78 - Name: Displays - Property Tree Widget: - Expanded: - - /Global Options1 - - /Status1 - - /src1 - - /src1/preprocessing1/est_ground1 - - /src1/preprocessing1/est_nonground1 - - /src1/segmentation1/src_invalid_seg1 - - /src1/segmentation1/src_seg1 - - /src1/features1 - - /tgt1 - - /tgt1/preprocessing1/est_ground1 - - /tgt1/preprocessing1/est_nonground1 - - /tgt1/segmentation1/tgt_invalid_seg1 - - /tgt1/segmentation1/tgt_seg1 - - /tgt1/features1/tgt_match1 - - /tgt1/features1/tgt_max_cliques1 - Splitter Ratio: 0.545258641242981 - Tree Height: 462 - - Class: rviz/Selection - Name: Selection - - Class: rviz/Tool Properties - Expanded: - - /2D Pose Estimate1 - - /2D Nav Goal1 - - /Publish Point1 - Name: Tool Properties - Splitter Ratio: 0.5886790156364441 - - Class: rviz/Views - Expanded: - - /Current View1 - Name: Views - Splitter Ratio: 0.5 - - Class: rviz/Time - Experimental: true - Name: Time - SyncMode: 0 - SyncSource: src_seg -Preferences: - PromptSaveOnExit: true -Toolbars: - toolButtonStyle: 2 -Visualization Manager: - Class: "" - Displays: - - Alpha: 0.5 - Cell Size: 1 - Class: rviz/Grid - Color: 160; 160; 164 - Enabled: false - Line Style: - Line Width: 0.029999999329447746 - Value: Lines - Name: Grid - Normal Cell Count: 0 - Offset: - X: 0 - Y: 0 - Z: 0 - Plane: XY - Plane Cell Count: 100 - Reference Frame: - Value: false - - Class: rviz/Axes - Enabled: false - Length: 3 - Name: Axes - Radius: 0.5 - Reference Frame: - Value: false - - Class: rviz/Group - Displays: - - Class: rviz/Group - Displays: - - Alpha: 1 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 10 - Min Value: -10 - Value: true - Axis: Z - Channel Name: intensity - Class: rviz/PointCloud2 - Color: 252; 233; 79 - Color Transformer: FlatColor - Decay Time: 0 - Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Min Color: 0; 0; 0 - Name: est_ground - Position Transformer: XYZ - Queue Size: 10 - Selectable: true - Size (Pixels): 3 - Size (m): 0.10000000149011612 - Style: Flat Squares - Topic: /ground_src - Unreliable: false - Use Fixed Frame: true - Use rainbow: true - Value: true - - Alpha: 1 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 10 - Min Value: -10 - Value: true - Axis: Z - Channel Name: intensity - Class: rviz/PointCloud2 - Color: 252; 13; 0 - Color Transformer: FlatColor - Decay Time: 0 - Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Min Color: 0; 0; 0 - Name: est_nonground - Position Transformer: XYZ - Queue Size: 10 - Selectable: true - Size (Pixels): 3 - Size (m): 0.10000000149011612 - Style: Points - Topic: /nonground_src - Unreliable: false - Use Fixed Frame: true - Use rainbow: true - Value: true - Enabled: false - Name: preprocessing - - Class: rviz/Group - Displays: - - Alpha: 1 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 10 - Min Value: -10 - Value: true - Axis: Z - Channel Name: intensity - Class: rviz/PointCloud2 - Color: 255; 0; 0 - Color Transformer: FlatColor - Decay Time: 0 - Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Min Color: 0; 0; 0 - Name: src_invalid_seg - Position Transformer: XYZ - Queue Size: 10 - Selectable: true - Size (Pixels): 3 - Size (m): 0.10000000149011612 - Style: Flat Squares - Topic: /invalid_seg_src - Unreliable: false - Use Fixed Frame: true - Use rainbow: true - Value: true - - Alpha: 1 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 10 - Min Value: -10 - Value: true - Axis: Z - Channel Name: intensity - Class: rviz/PointCloud2 - Color: 255; 0; 255 - Color Transformer: FlatColor - Decay Time: 0 - Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Min Color: 0; 0; 0 - Name: src_seg - Position Transformer: XYZ - Queue Size: 10 - Selectable: true - Size (Pixels): 3 - Size (m): 0.10000000149011612 - Style: Points - Topic: /valid_seg_src - Unreliable: false - Use Fixed Frame: true - Use rainbow: true - Value: true - Enabled: true - Name: segmentation - - Class: rviz/Group - Displays: - - Alpha: 1 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 10 - Min Value: -10 - Value: true - Axis: Z - Channel Name: intensity - Class: rviz/PointCloud2 - Color: 255; 128; 0 - Color Transformer: FlatColor - Decay Time: 0 - Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Min Color: 0; 0; 0 - Name: src_match - Position Transformer: XYZ - Queue Size: 10 - Selectable: false - Size (Pixels): 3 - Size (m): 1.2000000476837158 - Style: Squares - Topic: /matched_src - Unreliable: false - Use Fixed Frame: true - Use rainbow: true - Value: true - - Alpha: 1 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 10 - Min Value: -10 - Value: true - Axis: Z - Channel Name: intensity - Class: rviz/PointCloud2 - Color: 255; 128; 0 - Color Transformer: FlatColor - Decay Time: 0 - Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Min Color: 0; 0; 0 - Name: src_max_cliques - Position Transformer: XYZ - Queue Size: 10 - Selectable: true - Size (Pixels): 3 - Size (m): 2.5 - Style: Spheres - Topic: /max_clique_src - Unreliable: false - Use Fixed Frame: true - Use rainbow: true - Value: true - Enabled: true - Name: features - Enabled: true - Name: src - - Class: rviz/Group - Displays: - - Class: rviz/Group - Displays: - - Alpha: 1 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 10 - Min Value: -10 - Value: true - Axis: Z - Channel Name: intensity - Class: rviz/PointCloud2 - Color: 245; 121; 0 - Color Transformer: FlatColor - Decay Time: 0 - Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Min Color: 0; 0; 0 - Name: est_ground - Position Transformer: XYZ - Queue Size: 10 - Selectable: true - Size (Pixels): 3 - Size (m): 0.10000000149011612 - Style: Flat Squares - Topic: /ground_tgt - Unreliable: false - Use Fixed Frame: true - Use rainbow: true - Value: true - - Alpha: 1 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 10 - Min Value: -10 - Value: true - Axis: Z - Channel Name: intensity - Class: rviz/PointCloud2 - Color: 0; 255; 0 - Color Transformer: FlatColor - Decay Time: 0 - Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Min Color: 0; 0; 0 - Name: est_nonground - Position Transformer: XYZ - Queue Size: 10 - Selectable: true - Size (Pixels): 3 - Size (m): 0.10000000149011612 - Style: Points - Topic: /nonground_tgt - Unreliable: false - Use Fixed Frame: true - Use rainbow: true - Value: true - Enabled: false - Name: preprocessing - - Class: rviz/Group - Displays: - - Alpha: 1 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 10 - Min Value: -10 - Value: true - Axis: Z - Channel Name: intensity - Class: rviz/PointCloud2 - Color: 0; 255; 0 - Color Transformer: FlatColor - Decay Time: 0 - Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Min Color: 0; 0; 0 - Name: tgt_invalid_seg - Position Transformer: XYZ - Queue Size: 10 - Selectable: true - Size (Pixels): 3 - Size (m): 0.10000000149011612 - Style: Flat Squares - Topic: /invalid_seg_tgt - Unreliable: false - Use Fixed Frame: true - Use rainbow: true - Value: true - - Alpha: 1 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 10 - Min Value: -10 - Value: true - Axis: Z - Channel Name: intensity - Class: rviz/PointCloud2 - Color: 0; 255; 255 - Color Transformer: FlatColor - Decay Time: 0 - Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Min Color: 0; 0; 0 - Name: tgt_seg - Position Transformer: XYZ - Queue Size: 10 - Selectable: true - Size (Pixels): 3 - Size (m): 0.10000000149011612 - Style: Points - Topic: /valid_seg_tgt - Unreliable: false - Use Fixed Frame: true - Use rainbow: true - Value: true - Enabled: true - Name: segmentation - - Class: rviz/Group - Displays: - - Alpha: 1 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 10 - Min Value: -10 - Value: true - Axis: Z - Channel Name: intensity - Class: rviz/PointCloud2 - Color: 0; 0; 255 - Color Transformer: FlatColor - Decay Time: 0 - Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Min Color: 0; 0; 0 - Name: tgt_match - Position Transformer: XYZ - Queue Size: 10 - Selectable: true - Size (Pixels): 3 - Size (m): 1.2000000476837158 - Style: Squares - Topic: /matched_tgt - Unreliable: false - Use Fixed Frame: true - Use rainbow: true - Value: true - - Alpha: 1 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 10 - Min Value: -10 - Value: true - Axis: Z - Channel Name: intensity - Class: rviz/PointCloud2 - Color: 0; 0; 255 - Color Transformer: FlatColor - Decay Time: 0 - Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Min Color: 0; 0; 0 - Name: tgt_max_cliques - Position Transformer: XYZ - Queue Size: 10 - Selectable: true - Size (Pixels): 3 - Size (m): 2.5 - Style: Spheres - Topic: /max_clique_tgt - Unreliable: false - Use Fixed Frame: true - Use rainbow: true - Value: true - Enabled: true - Name: features - Enabled: true - Name: tgt - - Class: rviz/Marker - Enabled: true - Marker Topic: /correspondence - Name: corr. - Namespaces: - my_namespace: true - Queue Size: 100 - Value: true - - Class: rviz/Marker - Enabled: false - Marker Topic: /corrMC - Name: corrMC - Namespaces: - {} - Queue Size: 100 - Value: false - Enabled: true - Global Options: - Background Color: 255; 255; 255 - Default Light: true - Fixed Frame: map - Frame Rate: 30 - Name: root - Tools: - - Class: rviz/Interact - Hide Inactive Objects: true - - Class: rviz/MoveCamera - - Class: rviz/Select - - Class: rviz/FocusCamera - - Class: rviz/Measure - - Class: rviz/SetInitialPose - Theta std deviation: 0.2617993950843811 - Topic: /initialpose - X std deviation: 0.5 - Y std deviation: 0.5 - - Class: rviz/SetGoal - Topic: /move_base_simple/goal - - Class: rviz/PublishPoint - Single click: true - Topic: /clicked_point - Value: true - Views: - Current: - Class: rviz/ThirdPersonFollower - Distance: 80.4365234375 - Enable Stereo Rendering: - Stereo Eye Separation: 0.05999999865889549 - Stereo Focal Distance: 1 - Swap Stereo Eyes: false - Value: false - Focal Point: - X: 3.369354248046875 - Y: -8.598184585571289 - Z: -1.8921531591331586e-5 - Focal Shape Fixed Size: false - Focal Shape Size: 0.05000000074505806 - Invert Z Axis: false - Name: Current View - Near Clip Distance: 0.009999999776482582 - Pitch: 1.5697963237762451 - Target Frame: base_link - Value: ThirdPersonFollower (rviz) - Yaw: 4.797357559204102 - Saved: ~ -Window Geometry: - Displays: - collapsed: false - Height: 759 - Hide Left Dock: false - Hide Right Dock: true - QMainWindow State: 000000ff00000000fd0000000400000000000001d200000259fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d00000259000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000363fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d00000363000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000053e0000003efc0100000002fb0000000800540069006d006501000000000000053e0000031b00fffffffb0000000800540069006d00650100000000000004500000000000000000000003660000025900000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 - Selection: - collapsed: false - Time: - collapsed: false - Tool Properties: - collapsed: false - Views: - collapsed: true - Width: 1342 - X: 292 - Y: 27 diff --git a/src/teaser_utils/fpfh.cc b/src/fpfh.cc similarity index 58% rename from src/teaser_utils/fpfh.cc rename to src/fpfh.cc index ad77320..dcf6d96 100644 --- a/src/teaser_utils/fpfh.cc +++ b/src/fpfh.cc @@ -6,9 +6,9 @@ * See LICENSE for the license information */ -#include "teaser_utils/fpfh.h" #include -#include "teaser/utils.h" +#include +#include teaser::FPFHCloudPtr teaser::FPFHEstimation::computeFPFHFeatures( @@ -24,7 +24,7 @@ teaser::FPFHCloudPtr teaser::FPFHEstimation::computeFPFHFeatures( } // Estimate normals - pcl::NormalEstimationOMP normalEstimation; + pcl::NormalEstimation normalEstimation; normalEstimation.setInputCloud(pcl_input_cloud); normalEstimation.setRadiusSearch(normal_search_radius); pcl::search::KdTree::Ptr kdtree(new pcl::search::KdTree); @@ -41,41 +41,6 @@ teaser::FPFHCloudPtr teaser::FPFHEstimation::computeFPFHFeatures( return descriptors; } -teaser::FPFHCloudPtr teaser::FPFHEstimation::computeFPFHFeatures( // setFeaturePair에 쓰임 - const teaser::PointCloud& input_cloud, pcl::PointCloud& normals, - double normal_search_radius, double fpfh_search_radius) { - - // Intermediate variables - pcl::PointCloud::Ptr ptr_normals(new pcl::PointCloud); - teaser::FPFHCloudPtr descriptors(new pcl::PointCloud()); - pcl::PointCloud::Ptr pcl_input_cloud(new pcl::PointCloud); - for (auto& i : input_cloud) { - pcl::PointXYZ p(i.x, i.y, i.z); - pcl_input_cloud->push_back(p); - } - - // Estimate normals - pcl::NormalEstimation normalEstimation; - normalEstimation.setInputCloud(pcl_input_cloud); - normalEstimation.setRadiusSearch(normal_search_radius); - pcl::search::KdTree::Ptr kdtree(new pcl::search::KdTree); - normalEstimation.setSearchMethod(kdtree); - normalEstimation.compute(*ptr_normals); - - normals = * ptr_normals; - - // Estimate FPFH - setInputCloud(pcl_input_cloud); - setInputNormals(ptr_normals); - setSearchMethod(kdtree); - setRadiusSearch(fpfh_search_radius); - compute(*descriptors); - - return descriptors; -} - - - void teaser::FPFHEstimation::setInputCloud(pcl::PointCloud::Ptr input_cloud) { fpfh_estimation_->setInputCloud(input_cloud); } diff --git a/src/graph.cc b/src/graph.cc deleted file mode 100644 index efd7c73..0000000 --- a/src/graph.cc +++ /dev/null @@ -1,130 +0,0 @@ -/** - * Copyright 2020, Massachusetts Institute of Technology, - * Cambridge, MA 02139 - * All Rights Reserved - * Authors: Jingnan Shi, et al. (see THANKS for the full author list) - * See LICENSE for the license information - */ - -#include "teaser/graph.h" -#include "pmc/pmc.h" - -vector teaser::MaxCliqueSolver::findMaxClique(teaser::Graph graph) { - - // Handle deprecated field - if (!params_.solve_exactly) { - params_.solver_mode = CLIQUE_SOLVER_MODE::PMC_HEU; - } - - // Create a PMC graph from the TEASER graph - vector edges; - vector vertices; - vertices.push_back(edges.size()); - - const auto all_vertices = graph.getVertices(); - for (const auto& i : all_vertices) { - const auto& c_edges = graph.getEdges(i); - edges.insert(edges.end(), c_edges.begin(), c_edges.end()); - vertices.push_back(edges.size()); - } - - // Use PMC to calculate - pmc::pmc_graph G(vertices, edges); - - // Prepare PMC input - // TODO: Incorporate this to the constructor - - pmc::input in; - in.algorithm = 0; - in.threads = 12; // Originally, threads are 12 - in.experiment = 0; - in.lb = 0; - in.ub = 0; - in.param_ub = 0; - in.adj_limit = 20000; - in.time_limit = params_.time_limit; - in.remove_time = 4; - in.graph_stats = false; - in.verbose = false; - in.help = false; - in.MCE = false; - in.decreasing_order = false; - in.heu_strat = "kcore"; - in.vertex_search_order = "deg"; - - // vector to represent max clique - vector C; - - // upper-bound of max clique - G.compute_cores(); - auto max_core = G.get_max_core(); - - TEASER_DEBUG_INFO_MSG("Max core number: " << max_core); - TEASER_DEBUG_INFO_MSG("Num vertices: " << vertices.size()); - - // check for k-core heuristic threshold - // check whether threshold equals 1 to short circuit the comparison - if (params_.solver_mode == CLIQUE_SOLVER_MODE::KCORE_HEU && - params_.kcore_heuristic_threshold != 1 && - max_core > static_cast(params_.kcore_heuristic_threshold * - static_cast(all_vertices.size()))) { - TEASER_DEBUG_INFO_MSG("Using K-core heuristic finder."); - // remove all nodes with core number less than max core number - // k_cores is a vector saving the core number of each vertex - auto k_cores = G.get_kcores(); - for (int i = 1; i < k_cores->size(); ++i) { - // Note: k_core has size equals to num vertices + 1 - if ((*k_cores)[i] >= max_core) { - C.push_back(i-1); - } - } - return C; - } - - if (in.ub == 0) { - in.ub = max_core + 1; - } - - // lower-bound of max clique - if (in.lb == 0 && in.heu_strat != "0") { // skip if given as input - pmc::pmc_heu maxclique(G, in); - in.lb = maxclique.search(G, C); - } - - assert(in.lb != 0); - if (in.lb == 0) { - // This means that max clique has a size of one - TEASER_DEBUG_ERROR_MSG("Max clique lower bound equals to zero. Abort."); - return C; - } - - if (in.lb == in.ub) { - return C; - } - - // Optional exact max clique finding - if (params_.solver_mode == CLIQUE_SOLVER_MODE::PMC_EXACT) { - TEASER_DEBUG_INFO_MSG("Using exact finder."); - // The following methods are used: - // 1. k-core pruning - // 2. neigh-core pruning/ordering - // 3. dynamic coloring bounds/sort - // see the original PMC paper and implementation for details: - // R. A. Rossi, D. F. Gleich, and A. H. Gebremedhin, “Parallel Maximum Clique Algorithms with - // Applications to Network Analysis,” SIAM J. Sci. Comput., vol. 37, no. 5, pp. C589–C616, Jan. - // 2015. - if (G.num_vertices() < in.adj_limit) { - TEASER_DEBUG_INFO_MSG("Using exact search_dense finder."); - G.create_adj(); - pmc::pmcx_maxclique finder(G, in); -// finder.search_dense(G, C); - finder.search_dense(G, C); - } else { - TEASER_DEBUG_INFO_MSG("Using exact search finder."); - pmc::pmcx_maxclique finder(G, in); - finder.search(G, C); - } - } - - return C; -} diff --git a/src/matcher.cc b/src/matcher.cc new file mode 100644 index 0000000..054e394 --- /dev/null +++ b/src/matcher.cc @@ -0,0 +1,638 @@ +/** + * Copyright 2020, Massachusetts Institute of Technology, + * Cambridge, MA 02139 + * All Rights Reserved + * Authors: Jingnan Shi, et al. (see THANKS for the full author list) + * See LICENSE for the license information + */ + +#include +#ifdef QUATRO_DEBUG +#include +#endif + +namespace teaser { + +std::vector> Matcher::calculateCorrespondences( + const teaser::PointCloud& source_points, const teaser::PointCloud& target_points, + const teaser::FPFHCloud& source_features, const teaser::FPFHCloud& target_features, + const bool& use_absolute_scale, const bool& use_crosscheck, + const bool& use_tuple_test, const float& tuple_scale, + const bool& use_optimized_matching, const float& thr_dist, const int& num_max_corres) { + + Feature cloud_features; + pointcloud_.push_back(source_points); + pointcloud_.push_back(target_points); + + // It compute the global_scale_ required to set correctly the search radius + normalizePoints(use_absolute_scale); + + for (auto& f : source_features) { + Eigen::VectorXf fpfh(33); + for (int i = 0; i < 33; i++) + fpfh(i) = f.histogram[i]; + cloud_features.push_back(fpfh); + } + features_.push_back(cloud_features); + + cloud_features.clear(); + for (auto& f : target_features) { + Eigen::VectorXf fpfh(33); + for (int i = 0; i < 33; i++) + fpfh(i) = f.histogram[i]; + cloud_features.push_back(fpfh); + } + features_.push_back(cloud_features); + + if (use_optimized_matching) { +#ifdef QUATRO_DEBUG + std::cout << "\033[1;32mUse optimized matching!\033[0m\n"; +#endif + optimizedMatching(thr_dist, num_max_corres, tuple_scale); + } else { + advancedMatching(use_crosscheck, use_tuple_test, tuple_scale); + } + return corres_; +} + +void Matcher::normalizePoints(const bool& use_absolute_scale) { + int num = 2; + float scale = 0; + + means_.clear(); + + for (int i = 0; i < num; ++i) { + float max_scale = 0; + + // compute mean + Eigen::Vector3f mean; + mean.setZero(); + + int npti = pointcloud_[i].size(); + for (int ii = 0; ii < npti; ++ii) { + Eigen::Vector3f p(pointcloud_[i][ii].x, pointcloud_[i][ii].y, pointcloud_[i][ii].z); + mean = mean + p; + } + mean = mean / npti; + means_.push_back(mean); + + for (int ii = 0; ii < npti; ++ii) { + pointcloud_[i][ii].x -= mean(0); + pointcloud_[i][ii].y -= mean(1); + pointcloud_[i][ii].z -= mean(2); + } + + // compute scale + for (int ii = 0; ii < npti; ++ii) { + Eigen::Vector3f p(pointcloud_[i][ii].x, pointcloud_[i][ii].y, pointcloud_[i][ii].z); + float temp = p.norm(); // because we extract mean in the previous stage. + if (temp > max_scale) { + max_scale = temp; + } + } + + if (max_scale > scale) { + scale = max_scale; + } + } + + // mean of the scale variation + if (use_absolute_scale) { + global_scale_ = 1.0f; + } else { + global_scale_ = scale; // second choice: we keep the maximum scale. + } + + if (global_scale_ != 1.0f) { + for (int i = 0; i < num; ++i) { + int npti = pointcloud_[i].size(); + for (int ii = 0; ii < npti; ++ii) { + pointcloud_[i][ii].x /= global_scale_; + pointcloud_[i][ii].y /= global_scale_; + pointcloud_[i][ii].z /= global_scale_; + } + } + } +} + +void Matcher::advancedMatching(const bool& use_crosscheck, const bool& use_tuple_test, const float& tuple_scale) { + + int fi = 0; // source idx + int fj = 1; // destination idx + + bool swapped = false; + + if (pointcloud_[fj].size() > pointcloud_[fi].size()) { + int temp = fi; + fi = fj; + fj = temp; + swapped = true; + } + + int nPti = pointcloud_[fi].size(); + int nPtj = pointcloud_[fj].size(); + + /////////////////////////// + /// Build FLANNTREE + /////////////////////////// + KDTree feature_tree_i(flann::KDTreeSingleIndexParams(15)); +#ifdef TBB_EN + buildKDTreeWithTBB(features_[fi], &feature_tree_i); +#else + buildKDTree(features_[fi], &feature_tree_i); +#endif + + KDTree feature_tree_j(flann::KDTreeSingleIndexParams(15)); +#ifdef TBB_EN + buildKDTreeWithTBB(features_[fj], &feature_tree_j); +#else + buildKDTree(features_[fj], &feature_tree_j); +#endif + + std::vector corres_K(1, 0); + std::vector dis(1, 0.0); + + std::vector> corres; + std::vector> corres_cross; + std::vector> corres_ij; + std::vector> corres_ji; + +#ifdef TBB_EN + //////////////// Multi-Threading - flann + std::vector i_to_j_multi_flann(nPti, -1); + std::vector j_to_i_multi_flann(nPtj, -1); + + std::vector> corres_multi_flann; + + std::vector j_idx(nPtj); + std::iota(j_idx.begin(), j_idx.end(), 0); + + tbb::parallel_for_each(j_idx.begin(), j_idx.end(), [&](int j){ + searchKDTree(&feature_tree_i, features_[fj][j], corres_K, dis, 1); + int ji = corres_K[0]; + + if(i_to_j_multi_flann[ji] == -1){ + searchKDTree(&feature_tree_j, features_[fi][ji], corres_K, dis, 1); + int jij = corres_K[0]; + i_to_j_multi_flann[ji] = jij; + } + + j_to_i_multi_flann[j] = ji; + + }); + + for(int j=0; j(ji, j)); + } + } + corres = corres_multi_flann; + corres_cross = corres_multi_flann; +#else + ///////////// Single-threading - flann + ///////////////////////// + // INITIAL MATCHING + ///////////////////////// + std::vector i_to_j(nPti, -1); + for (int j = 0; j < nPtj; j++) { + searchKDTree(&feature_tree_i, features_[fj][j], corres_K, dis, 1); + int i = corres_K[0]; + if (i_to_j[i] == -1) { + searchKDTree(&feature_tree_j, features_[fi][i], corres_K, dis, 1); + int ij = corres_K[0]; + i_to_j[i] = ij; + } + corres_ji.push_back(std::pair(i, j)); + } + + for (int i = 0; i < nPti; i++) { + if (i_to_j[i] != -1) + corres_ij.push_back(std::pair(i, i_to_j[i])); + } + + int ncorres_ij = corres_ij.size(); + int ncorres_ji = corres_ji.size(); + + // corres = corres_ij + corres_ji; + for (int i = 0; i < ncorres_ij; ++i) + corres.push_back(std::pair(corres_ij[i].first, corres_ij[i].second)); + for (int j = 0; j < ncorres_ji; ++j) + corres.push_back(std::pair(corres_ji[j].first, corres_ji[j].second)); + + /////////////////////////// + /// CROSS CHECK + /// input : corres_ij, corres_ji + /// output : corres + /////////////////////////// + if (use_crosscheck) { +#ifdef QUATRO_DEBUG + std::cout << "CROSS CHECK" << std::endl; +#endif + // build data structure for cross check + corres.clear(); + corres_cross.clear(); + std::vector> Mi(nPti); + std::vector> Mj(nPtj); + + int ci, cj; + for (int i = 0; i < ncorres_ij; ++i) { + ci = corres_ij[i].first; + cj = corres_ij[i].second; + Mi[ci].push_back(cj); + } + for (int j = 0; j < ncorres_ji; ++j) { + ci = corres_ji[j].first; + cj = corres_ji[j].second; + Mj[cj].push_back(ci); + } + + // cross check + for (int i = 0; i < nPti; ++i) { + for (int ii = 0; ii < Mi[i].size(); ++ii) { + int j = Mi[i][ii]; + for (int jj = 0; jj < Mj[j].size(); ++jj) { + if (Mj[j][jj] == i) { + corres.push_back(std::pair(i, j)); + corres_cross.push_back(std::pair(i, j)); + } + } + } + } + } else { +#ifdef QUATRO_DEBUG + std::cout << "Skipping Cross Check." << std::endl; +#endif + } +#endif + + /////////////////////////// + /// TUPLE CONSTRAINT + /// input : corres + /// output : corres + /////////////////////////// + if (use_tuple_test && tuple_scale != 0) { +#ifdef QUATRO_DEBUG + std::cout << "TUPLE CONSTRAINT" << std::endl; +#endif + srand(time(NULL)); + int rand0, rand1, rand2; + int idi0, idi1, idi2; + int idj0, idj1, idj2; + float scale = tuple_scale; + int ncorr = corres.size(); + int number_of_trial = ncorr * 100; + std::vector> corres_tuple; + + for (int i = 0; i < number_of_trial; i++) { + rand0 = rand() % ncorr; + rand1 = rand() % ncorr; + rand2 = rand() % ncorr; + + idi0 = corres[rand0].first; + idj0 = corres[rand0].second; + idi1 = corres[rand1].first; + idj1 = corres[rand1].second; + idi2 = corres[rand2].first; + idj2 = corres[rand2].second; + + // collect 3 points from i-th fragment + Eigen::Vector3f pti0 = {pointcloud_[fi][idi0].x, pointcloud_[fi][idi0].y, + pointcloud_[fi][idi0].z}; + Eigen::Vector3f pti1 = {pointcloud_[fi][idi1].x, pointcloud_[fi][idi1].y, + pointcloud_[fi][idi1].z}; + Eigen::Vector3f pti2 = {pointcloud_[fi][idi2].x, pointcloud_[fi][idi2].y, + pointcloud_[fi][idi2].z}; + + float li0 = (pti0 - pti1).norm(); + float li1 = (pti1 - pti2).norm(); + float li2 = (pti2 - pti0).norm(); + + // collect 3 points from j-th fragment + Eigen::Vector3f ptj0 = {pointcloud_[fj][idj0].x, pointcloud_[fj][idj0].y, + pointcloud_[fj][idj0].z}; + Eigen::Vector3f ptj1 = {pointcloud_[fj][idj1].x, pointcloud_[fj][idj1].y, + pointcloud_[fj][idj1].z}; + Eigen::Vector3f ptj2 = {pointcloud_[fj][idj2].x, pointcloud_[fj][idj2].y, + pointcloud_[fj][idj2].z}; + + float lj0 = (ptj0 - ptj1).norm(); + float lj1 = (ptj1 - ptj2).norm(); + float lj2 = (ptj2 - ptj0).norm(); + + if ((li0 * scale < lj0) && (lj0 < li0 / scale) && (li1 * scale < lj1) && + (lj1 < li1 / scale) && (li2 * scale < lj2) && (lj2 < li2 / scale)) { + corres_tuple.push_back(std::pair(idi0, idj0)); + corres_tuple.push_back(std::pair(idi1, idj1)); + corres_tuple.push_back(std::pair(idi2, idj2)); + } + } + corres.clear(); + + for (size_t i = 0; i < corres_tuple.size(); ++i) + corres.push_back(std::pair(corres_tuple[i].first, corres_tuple[i].second)); + } else { +#ifdef QUATRO_DEBUG + std::cout << "Skipping Tuple Constraint." << std::endl; +#endif + } + + if (swapped) { + std::vector> temp; + for (size_t i = 0; i < corres.size(); i++) + temp.push_back(std::pair(corres[i].second, corres[i].first)); + corres.clear(); + corres = temp; + } + corres_ = corres; + + /////////////////////////// + /// ERASE DUPLICATES + /// input : corres_ + /// output : corres_ + /////////////////////////// + std::sort(corres_.begin(), corres_.end()); + corres_.erase(std::unique(corres_.begin(), corres_.end()), corres_.end()); +} + +void Matcher::optimizedMatching(const float& thr_dist, const int& num_max_corres, const float& tuple_scale) { + int fi = 0; // source idx + int fj = 1; // destination idx + + bool swapped = false; + + if (pointcloud_[fj].size() > pointcloud_[fi].size()) { + int temp = fi; + fi = fj; + fj = temp; + swapped = true; + } + + int nPti = pointcloud_[fi].size(); + int nPtj = pointcloud_[fj].size(); + + /////////////////////////// + /// Build FLANNTREE + /////////////////////////// + std::chrono::steady_clock::time_point begin_build = std::chrono::steady_clock::now(); + KDTree feature_tree_i(flann::KDTreeSingleIndexParams(15)); +#ifdef TBB_EN + buildKDTreeWithTBB(features_[fi], &feature_tree_i); +#else + buildKDTree(features_[fi], &feature_tree_i); +#endif + + KDTree feature_tree_j(flann::KDTreeSingleIndexParams(15)); +#ifdef TBB_EN + buildKDTreeWithTBB(features_[fj], &feature_tree_j); +#else + buildKDTree(features_[fj], &feature_tree_j); +#endif + std::chrono::steady_clock::time_point end_build = std::chrono::steady_clock::now(); + + std::vector corres_K(1, 0); + std::vector dis(1, 0.0); + + /////////////////////////// + /// INITIAL MATCHING + /////////////////////////// + searchKDTreeAll(&feature_tree_i, features_[fj], corres_K, dis, 1); + std::chrono::steady_clock::time_point end_search = std::chrono::steady_clock::now(); + + std::vector i_to_j(nPti, -1); + std::vector> empty_vector; + empty_vector.reserve(corres_K.size()); + std::vector corres_K_for_i(1, 0); + std::vector dis_for_i(1, 0.0); + + std::chrono::steady_clock::time_point begin_corr = std::chrono::steady_clock::now(); + std::vector> corres; + +#ifdef TBB_EN + corres = tbb::parallel_reduce( + // Range + // tbb::blocked_range(0, corres_K.size(), corres_K.size()/TBB_PROC_NUM*2), + tbb::blocked_range(0, corres_K.size()), + // Identity + empty_vector, + // 1st lambda: Parallel computation + [&](const tbb::blocked_range &r, std::vector> local_corres) -> std::vector> { + local_corres.reserve(r.size()); + for (size_t j = r.begin(); j != r.end(); ++j) { + if (dis[j] > thr_dist * thr_dist) { continue; } + + const int& i = corres_K[j]; + if (i_to_j[i] == -1) { + searchKDTree(&feature_tree_j, features_[fi][i], corres_K_for_i, dis_for_i, 1); + i_to_j[i] = corres_K_for_i[0]; + if (corres_K_for_i[0] == j) { + local_corres.emplace_back(i, j); + } + } + } + return local_corres; + }, + // 2nd lambda: Parallel reduction + [](std::vector> a, const std::vector> &b) -> std::vector> { + a.insert(a.end(), // + std::make_move_iterator(b.begin()), std::make_move_iterator(b.end())); + return a; + }); +#else + corres.reserve(corres_K.size()); + for (size_t j = 0; j < corres_K.size(); ++j) { + if (dis[j] > thr_dist * thr_dist) { continue; } + + const int& i = corres_K[j]; + if (i_to_j[i] == -1) { + searchKDTree(&feature_tree_j, features_[fi][i], corres_K_for_i, dis_for_i, 1); + i_to_j[i] = corres_K_for_i[0]; + if (corres_K_for_i[0] == j) { + corres.emplace_back(i, j); + } + } + } +#endif + std::chrono::steady_clock::time_point end_corr = std::chrono::steady_clock::now(); + + /////////////////////////// + /// TUPLE TEST + /////////////////////////// + if (tuple_scale != 0) { +#ifdef QUATRO_DEBUG + std::cout << "TUPLE CONSTRAINT" << std::endl; +#endif + srand(time(NULL)); + int rand0, rand1, rand2; + int idi0, idi1, idi2; + int idj0, idj1, idj2; + float scale = tuple_scale; + int ncorr = corres.size(); + int number_of_trial = ncorr * 100; + + std::vector is_already_included(ncorr, false); + corres_.clear(); + corres_.reserve(num_max_corres); + + auto addUniqueCorrespondence = [&](const int randIndex, const int id1, const int id2) { + if (!is_already_included[randIndex]) { + corres_.emplace_back(id1, id2); + is_already_included[randIndex] = true; + } + }; + + for (int i = 0; i < number_of_trial; i++) { + rand0 = rand() % ncorr; + rand1 = rand() % ncorr; + + idi0 = corres[rand0].first; + idj0 = corres[rand0].second; + idi1 = corres[rand1].first; + idj1 = corres[rand1].second; + + // The order has been changed to reduce the redundant computation + Eigen::Vector3f pti0 = {pointcloud_[fi][idi0].x, pointcloud_[fi][idi0].y, + pointcloud_[fi][idi0].z}; + Eigen::Vector3f pti1 = {pointcloud_[fi][idi1].x, pointcloud_[fi][idi1].y, + pointcloud_[fi][idi1].z}; + + Eigen::Vector3f ptj0 = {pointcloud_[fj][idj0].x, pointcloud_[fj][idj0].y, + pointcloud_[fj][idj0].z}; + Eigen::Vector3f ptj1 = {pointcloud_[fj][idj1].x, pointcloud_[fj][idj1].y, + pointcloud_[fj][idj1].z}; + + float li0 = (pti0 - pti1).norm(); + float lj0 = (ptj0 - ptj1).norm(); + + if ((li0 * scale > lj0) || (lj0 > li0 / scale)) { + continue; + } + + rand2 = rand() % ncorr; + idi2 = corres[rand2].first; + idj2 = corres[rand2].second; + + Eigen::Vector3f pti2 = {pointcloud_[fi][idi2].x, pointcloud_[fi][idi2].y, + pointcloud_[fi][idi2].z}; + Eigen::Vector3f ptj2 = {pointcloud_[fj][idj2].x, pointcloud_[fj][idj2].y, + pointcloud_[fj][idj2].z}; + + float li1 = (pti1 - pti2).norm(); + float li2 = (pti2 - pti0).norm(); + + float lj1 = (ptj1 - ptj2).norm(); + float lj2 = (ptj2 - ptj0).norm(); + + if ((li1 * scale < lj1) && (lj1 < li1 / scale) && (li2 * scale < lj2) && (lj2 < li2 / scale)) { + if (swapped) { + addUniqueCorrespondence(rand0, idj0, idi0); + addUniqueCorrespondence(rand1, idj1, idi1); + addUniqueCorrespondence(rand2, idj2, idi2); + } else { + addUniqueCorrespondence(rand0, idi0, idj0); + addUniqueCorrespondence(rand1, idi1, idj1); + addUniqueCorrespondence(rand2, idi2, idj2); + } + } + if (corres_.size() > num_max_corres) { break; } + } + } else { +#ifdef QUATRO_DEBUG + std::cout << "Skipping Tuple Constraint." << std::endl; +#endif + } + + std::chrono::steady_clock::time_point end_tuple_test = std::chrono::steady_clock::now(); + const int width = 25; +#ifdef QUATRO_DEBUG + std::cout << std::setw(width) << "[Build KdTree]: " + << std::chrono::duration_cast(end_build - begin_build).count() / + 1000000.0 << " sec" << std::endl; + std::cout << std::setw(width) << "[Search using FLANN]: " + << std::chrono::duration_cast(end_search - end_build).count() / + 1000000.0 << " sec" << std::endl; + std::cout << std::setw(width) << "[Cross checking]: " + << std::chrono::duration_cast(end_corr - end_search).count() / + 1000000.0 << " sec" << std::endl; + std::cout << std::setw(width) << "[Tuple test]: " + << std::chrono::duration_cast(end_tuple_test - end_corr).count() / + 1000000.0 << " sec" << std::endl; +#endif +} + +template void Matcher::buildKDTree(const std::vector& data, Matcher::KDTree* tree) { + int rows, dim; + rows = (int)data.size(); + dim = (int)data[0].size(); + std::vector dataset(rows * dim); + flann::Matrix dataset_mat(&dataset[0], rows, dim); + for (int i = 0; i < rows; i++) + for (int j = 0; j < dim; j++) + dataset[i * dim + j] = data[i][j]; + KDTree temp_tree(dataset_mat, flann::KDTreeSingleIndexParams(15)); + temp_tree.buildIndex(); + *tree = temp_tree; +} + +#ifdef TBB_EN +template void Matcher::buildKDTreeWithTBB(const std::vector& data, Matcher::KDTree* tree) { + // Note that it is not much faster than `buildKDTrees`, which is without TBB + // I guess the reason is that the data is too small and the overhead of TBB is a bit larger than the speedup. + int rows, dim; + rows = (int)data.size(); + dim = (int)data[0].size(); + std::vector dataset(rows * dim); + flann::Matrix dataset_mat(&dataset[0], rows, dim); + tbb::parallel_for(0, rows, 1, [&](int i) { + for (int j = 0; j < dim; j++) { + dataset[i * dim + j] = data[i][j]; + } + }); + KDTree temp_tree(dataset_mat, flann::KDTreeSingleIndexParams(15)); + temp_tree.buildIndex(); + *tree = temp_tree; +} +#endif + +template +void Matcher::searchKDTree(Matcher::KDTree* tree, const T& input, std::vector& indices, + std::vector& dists, int nn) { + int rows_t = 1; + int dim = input.size(); + + std::vector query; + query.resize(rows_t * dim); + for (int i = 0; i < dim; i++) + query[i] = input(i); + flann::Matrix query_mat(&query[0], rows_t, dim); + + flann::Matrix indices_mat(&indices[0], rows_t, nn); + flann::Matrix dists_mat(&dists[0], rows_t, nn); + + tree->knnSearch(query_mat, indices_mat, dists_mat, nn, flann::SearchParams(128)); +} + +template +void Matcher::searchKDTreeAll(Matcher::KDTree* tree, const std::vector& inputs, + std::vector& indices, std::vector& dists, int nn) { + int dim = inputs[0].size(); + + std::vector query(inputs.size() * dim); + for (size_t i = 0; i < inputs.size(); ++i) { + for (int j = 0; j < dim; ++j) { + query[i * dim + j] = inputs[i](j); + } + } + flann::Matrix query_mat(&query[0], inputs.size(), dim); + + indices.resize(inputs.size() * nn); + dists.resize(inputs.size() * nn); + flann::Matrix indices_mat(&indices[0], inputs.size(), nn); + flann::Matrix dists_mat(&dists[0], inputs.size(), nn); + + auto flann_params = flann::SearchParams(128); + flann_params.cores = 12; + tree->knnSearch(query_mat, indices_mat, dists_mat, nn, flann_params); +} + +} // namespace teaser diff --git a/src/quatro_module.cc b/src/quatro_module.cc new file mode 100644 index 0000000..0e726b6 --- /dev/null +++ b/src/quatro_module.cc @@ -0,0 +1,84 @@ +#include + +template +quatro::quatro(const double &fpfh_normal_radi, const double &fpfh_radi, const double noise_bound, const double &rot_gnc_fact, + const double &rot_cost_thr, const int &rot_max_iter, const bool &estimat_scale, + const bool& use_optimized_matching, const double& distance_threshold, const int& num_max_corres) +{ + m_normal_radius = fpfh_normal_radi; + m_fpfh_radius = fpfh_radi; + m_noise_bound = noise_bound; + m_rotation_gnc_factor = rot_gnc_fact; + m_rotation_cost_thr = rot_cost_thr; + m_rotation_max_iter = rot_max_iter; + m_estimate_scale = estimat_scale; + m_use_optimized_matching = use_optimized_matching; + m_distance_threshold = distance_threshold; + m_num_max_corres = num_max_corres; + set_params(); +} + +template +teaser::PointCloud quatro::pcl_to_teaser_pcl(const pcl::PointCloud &cloud_in) +{ + teaser::PointCloud t_pcl_out_; + if (cloud_in.size() > 0 ) t_pcl_out_.reserve(cloud_in.size()); + for (size_t i = 0; i < cloud_in.size(); ++i) + { + t_pcl_out_.push_back({cloud_in.points[i].x, cloud_in.points[i].y, cloud_in.points[i].z}); + } + return t_pcl_out_; +} + +template +void quatro::set_params() +{ + m_quatro_params.noise_bound = m_noise_bound; + m_quatro_params.estimate_scaling = m_estimate_scale; + m_quatro_params.rotation_max_iterations = m_rotation_max_iter; + m_quatro_params.rotation_gnc_factor = m_rotation_gnc_factor; + m_quatro_params.rotation_cost_threshold = m_rotation_cost_thr; + m_quatro_params.cbar2 = 1; // cbar2: 'noise_bound_coeff' plays a role as an uncertainty multiplier and is used when estimating COTE + //I.e. final noise bound is set to `noise_bound` * `noise_bound_coeff` + m_quatro_params.rotation_estimation_algorithm = teaser::RobustRegistrationSolver::ROTATION_ESTIMATION_ALGORITHM::QUATRO; + m_quatro_params.inlier_selection_mode = teaser::RobustRegistrationSolver::INLIER_SELECTION_MODE::PMC_HEU; + return; +} + +template +Eigen::Matrix4d quatro::align(const pcl::PointCloud &src, const pcl::PointCloud &dst, bool &if_valid) +{ + Eigen::Matrix4d out_tf_ = Eigen::Matrix4d::Identity(); + + teaser::PointCloud src_cloud_ = pcl_to_teaser_pcl(src); + teaser::PointCloud tgt_cloud_ = pcl_to_teaser_pcl(dst); + teaser::FPFHEstimation fpfh_; + teaser::FPFHCloudPtr obj_descriptors_ = fpfh_.computeFPFHFeatures(src_cloud_, m_normal_radius, m_fpfh_radius); + teaser::FPFHCloudPtr scene_descriptors_ = fpfh_.computeFPFHFeatures(tgt_cloud_, m_normal_radius, m_fpfh_radius); + + teaser::Matcher matcher_; + std::vector> correspondences_ = matcher_.calculateCorrespondences(src_cloud_, tgt_cloud_, *obj_descriptors_, *scene_descriptors_, + false, true, true, 0.95, m_use_optimized_matching, m_distance_threshold, m_num_max_corres); + + if (correspondences_.empty()) // no correspondences!!! + { + if_valid = false; + } + else + { + teaser::RobustRegistrationSolver Quatro_(m_quatro_params); + + Quatro_.solve(src_cloud_, tgt_cloud_, correspondences_); + teaser::RegistrationSolution solution_by_quatro_ = Quatro_.getSolution(); + if_valid = solution_by_quatro_.valid; //if the result is valid or not + + out_tf_.block<3, 3>(0, 0) = solution_by_quatro_.rotation; + out_tf_.block<3, 1>(0, 3) = solution_by_quatro_.translation; + } + return out_tf_; +} + + +//manual instatiations +template class quatro; +template class quatro; diff --git a/src/teaser_utils/feature_matcher.cc b/src/teaser_utils/feature_matcher.cc deleted file mode 100644 index c394b9b..0000000 --- a/src/teaser_utils/feature_matcher.cc +++ /dev/null @@ -1,301 +0,0 @@ -/** - * Copyright 2020, Massachusetts Institute of Technology, - * Cambridge, MA 02139 - * All Rights Reserved - * Authors: Jingnan Shi, et al. (see THANKS for the full author list) - * See LICENSE for the license information - */ - - -#include -#include -#include -#include "teaser_utils/feature_matcher.h" - -namespace teaser { - - -void Matcher::normalizePoints(bool use_absolute_scale) { - int num = 2; - float scale = 0; - - means_.clear(); - - for (int i = 0; i < num; ++i) { - float max_scale = 0; - - // compute mean - Eigen::Vector3f mean; - mean.setZero(); - - int npti = pointcloud_[i].size(); - for (int ii = 0; ii < npti; ++ii) { - Eigen::Vector3f p(pointcloud_[i][ii].x, pointcloud_[i][ii].y, pointcloud_[i][ii].z); - mean = mean + p; - } - mean = mean / npti; - means_.push_back(mean); - - for (int ii = 0; ii < npti; ++ii) { - pointcloud_[i][ii].x -= mean(0); - pointcloud_[i][ii].y -= mean(1); - pointcloud_[i][ii].z -= mean(2); - } - - // compute scale - for (int ii = 0; ii < npti; ++ii) { - Eigen::Vector3f p(pointcloud_[i][ii].x, pointcloud_[i][ii].y, pointcloud_[i][ii].z); - float temp = p.norm(); // because we extract mean in the previous stage. - if (temp > max_scale) { - max_scale = temp; - } - } - - if (max_scale > scale) { - scale = max_scale; - } - } - - // mean of the scale variation - if (use_absolute_scale) { - global_scale_ = 1.0f; - } else { - global_scale_ = scale; // second choice: we keep the maximum scale. - } - - if (global_scale_ != 1.0f) { - for (int i = 0; i < num; ++i) { - int npti = pointcloud_[i].size(); - for (int ii = 0; ii < npti; ++ii) { - pointcloud_[i][ii].x /= global_scale_; - pointcloud_[i][ii].y /= global_scale_; - pointcloud_[i][ii].z /= global_scale_; - } - } - } -} -void Matcher::advancedMatching(bool use_crosscheck, bool use_tuple_test, float tuple_scale) { - - int fi = 0; // source idx - int fj = 1; // destination idx - - bool swapped = false; - - if (pointcloud_[fj].size() > pointcloud_[fi].size()) { - int temp = fi; - fi = fj; - fj = temp; - swapped = true; - } - - int nPti = pointcloud_[fi].size(); - int nPtj = pointcloud_[fj].size(); - - /////////////////////////// - /// Build FLANNTREE - /////////////////////////// - KDTree feature_tree_i(flann::KDTreeSingleIndexParams(15)); - buildKDTree(features_[fi], &feature_tree_i); - - KDTree feature_tree_j(flann::KDTreeSingleIndexParams(15)); - buildKDTree(features_[fj], &feature_tree_j); - - std::vector corres_K, corres_K2; - std::vector dis; - std::vector ind; - - std::vector> corres; - std::vector> corres_cross; - std::vector> corres_ij; - std::vector> corres_ji; - - /////////////////////////// - /// INITIAL MATCHING - /////////////////////////// - std::vector i_to_j(nPti, -1); - for (int j = 0; j < nPtj; j++) { - searchKDTree(&feature_tree_i, features_[fj][j], corres_K, dis, 1); - int i = corres_K[0]; - if (i_to_j[i] == -1) { - searchKDTree(&feature_tree_j, features_[fi][i], corres_K, dis, 1); - int ij = corres_K[0]; - i_to_j[i] = ij; - } - corres_ji.push_back(std::pair(i, j)); - } - - for (int i = 0; i < nPti; i++) { - if (i_to_j[i] != -1) - corres_ij.push_back(std::pair(i, i_to_j[i])); - } - - int ncorres_ij = corres_ij.size(); - int ncorres_ji = corres_ji.size(); - - // corres = corres_ij + corres_ji; - for (int i = 0; i < ncorres_ij; ++i) - corres.push_back(std::pair(corres_ij[i].first, corres_ij[i].second)); - for (int j = 0; j < ncorres_ji; ++j) - corres.push_back(std::pair(corres_ji[j].first, corres_ji[j].second)); - - /////////////////////////// - /// CROSS CHECK - /// input : corres_ij, corres_ji - /// output : corres - /////////////////////////// - if (use_crosscheck) { - std::cout << "CROSS CHECK" << std::endl; - // build data structure for cross check - corres.clear(); - corres_cross.clear(); - std::vector> Mi(nPti); - std::vector> Mj(nPtj); - - int ci, cj; - for (int i = 0; i < ncorres_ij; ++i) { - ci = corres_ij[i].first; - cj = corres_ij[i].second; - Mi[ci].push_back(cj); - } - for (int j = 0; j < ncorres_ji; ++j) { - ci = corres_ji[j].first; - cj = corres_ji[j].second; - Mj[cj].push_back(ci); - } - - // cross check - for (int i = 0; i < nPti; ++i) { - for (int ii = 0; ii < Mi[i].size(); ++ii) { - int j = Mi[i][ii]; - for (int jj = 0; jj < Mj[j].size(); ++jj) { - if (Mj[j][jj] == i) { - corres.push_back(std::pair(i, j)); - corres_cross.push_back(std::pair(i, j)); - } - } - } - } - } else { - std::cout << "Skipping Cross Check." << std::endl; - } - - /////////////////////////// - /// TUPLE CONSTRAINT - /// input : corres - /// output : corres - /////////////////////////// - if (use_tuple_test && tuple_scale != 0) { - std::cout << "TUPLE CONSTRAINT" << std::endl; - srand(time(NULL)); - int rand0, rand1, rand2; - int idi0, idi1, idi2; - int idj0, idj1, idj2; - float scale = tuple_scale; - int ncorr = corres.size(); - int number_of_trial = ncorr * 100; - std::vector> corres_tuple; - - for (int i = 0; i < number_of_trial; i++) { - rand0 = rand() % ncorr; - rand1 = rand() % ncorr; - rand2 = rand() % ncorr; - - idi0 = corres[rand0].first; - idj0 = corres[rand0].second; - idi1 = corres[rand1].first; - idj1 = corres[rand1].second; - idi2 = corres[rand2].first; - idj2 = corres[rand2].second; - - // collect 3 points from i-th fragment - Eigen::Vector3f pti0 = {pointcloud_[fi][idi0].x, pointcloud_[fi][idi0].y, - pointcloud_[fi][idi0].z}; - Eigen::Vector3f pti1 = {pointcloud_[fi][idi1].x, pointcloud_[fi][idi1].y, - pointcloud_[fi][idi1].z}; - Eigen::Vector3f pti2 = {pointcloud_[fi][idi2].x, pointcloud_[fi][idi2].y, - pointcloud_[fi][idi2].z}; - - float li0 = (pti0 - pti1).norm(); - float li1 = (pti1 - pti2).norm(); - float li2 = (pti2 - pti0).norm(); - - // collect 3 points from j-th fragment - Eigen::Vector3f ptj0 = {pointcloud_[fj][idj0].x, pointcloud_[fj][idj0].y, - pointcloud_[fj][idj0].z}; - Eigen::Vector3f ptj1 = {pointcloud_[fj][idj1].x, pointcloud_[fj][idj1].y, - pointcloud_[fj][idj1].z}; - Eigen::Vector3f ptj2 = {pointcloud_[fj][idj2].x, pointcloud_[fj][idj2].y, - pointcloud_[fj][idj2].z}; - - float lj0 = (ptj0 - ptj1).norm(); - float lj1 = (ptj1 - ptj2).norm(); - float lj2 = (ptj2 - ptj0).norm(); - - if ((li0 * scale < lj0) && (lj0 < li0 / scale) && (li1 * scale < lj1) && - (lj1 < li1 / scale) && (li2 * scale < lj2) && (lj2 < li2 / scale)) { - corres_tuple.push_back(std::pair(idi0, idj0)); - corres_tuple.push_back(std::pair(idi1, idj1)); - corres_tuple.push_back(std::pair(idi2, idj2)); - } - } - corres.clear(); - - for (size_t i = 0; i < corres_tuple.size(); ++i) - corres.push_back(std::pair(corres_tuple[i].first, corres_tuple[i].second)); - } else { - std::cout << "Skipping Tuple Constraint." << std::endl; - } - - if (swapped) { - std::vector> temp; - for (size_t i = 0; i < corres.size(); i++) - temp.push_back(std::pair(corres[i].second, corres[i].first)); - corres.clear(); - corres = temp; - } - corres_ = corres; - - /////////////////////////// - /// ERASE DUPLICATES - /// input : corres_ - /// output : corres_ - /////////////////////////// - std::sort(corres_.begin(), corres_.end()); - corres_.erase(std::unique(corres_.begin(), corres_.end()), corres_.end()); -} - -template void Matcher::buildKDTree(const std::vector& data, Matcher::KDTree* tree) { - int rows, dim; - rows = (int)data.size(); - dim = (int)data[0].size(); - std::vector dataset(rows * dim); - flann::Matrix dataset_mat(&dataset[0], rows, dim); - for (int i = 0; i < rows; i++) - for (int j = 0; j < dim; j++) - dataset[i * dim + j] = data[i][j]; - KDTree temp_tree(dataset_mat, flann::KDTreeSingleIndexParams(15)); - temp_tree.buildIndex(); - *tree = temp_tree; -} - -template -void Matcher::searchKDTree(Matcher::KDTree* tree, const T& input, std::vector& indices, - std::vector& dists, int nn) { - int rows_t = 1; - int dim = input.size(); - - std::vector query; - query.resize(rows_t * dim); - for (int i = 0; i < dim; i++) - query[i] = input(i); - flann::Matrix query_mat(&query[0], rows_t, dim); - - indices.resize(rows_t * nn); - dists.resize(rows_t * nn); - flann::Matrix indices_mat(&indices[0], rows_t, nn); - flann::Matrix dists_mat(&dists[0], rows_t, nn); - - tree->knnSearch(query_mat, indices_mat, dists_mat, nn, flann::SearchParams(128)); -} - -} // namespace teaser