From 5d730aeaf367db1468542e8211eb293eff59496a Mon Sep 17 00:00:00 2001 From: Robofish <1683502971@qq.com> Date: Sat, 5 Oct 2024 19:29:28 +0800 Subject: [PATCH] =?UTF-8?q?=E6=B7=BB=E5=8A=A0fastlio=E5=92=8Cpointlio?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .catkin_workspace | 1 + .gitignore | 2 + build.sh | 1 + src/CMakeLists.txt | 1 + src/FAST_LIO/.github/stale.yml | 17 + src/FAST_LIO/.gitignore | 8 + src/FAST_LIO/.gitmodules | 4 + src/FAST_LIO/CMakeLists.txt | 89 + src/FAST_LIO/LICENSE | 339 +++ src/FAST_LIO/Log/fast_lio_time_log_analysis.m | 135 + src/FAST_LIO/Log/guide.md | 1 + src/FAST_LIO/Log/plot.py | 94 + src/FAST_LIO/PCD/1 | 1 + src/FAST_LIO/README.md | 259 ++ src/FAST_LIO/config/avia.yaml | 35 + src/FAST_LIO/config/horizon.yaml | 35 + src/FAST_LIO/config/mid360.yaml | 35 + src/FAST_LIO/config/mid360_down.yaml | 35 + src/FAST_LIO/config/ouster64.yaml | 36 + src/FAST_LIO/config/velodyne.yaml | 37 + src/FAST_LIO/include/Exp_mat.h | 103 + .../include/IKFoM_toolkit/esekfom/esekfom.hpp | 2008 +++++++++++++ .../include/IKFoM_toolkit/esekfom/util.hpp | 82 + .../IKFoM_toolkit/mtk/build_manifold.hpp | 229 ++ .../IKFoM_toolkit/mtk/src/SubManifold.hpp | 123 + .../include/IKFoM_toolkit/mtk/src/mtkmath.hpp | 294 ++ .../IKFoM_toolkit/mtk/src/vectview.hpp | 168 ++ .../include/IKFoM_toolkit/mtk/startIdx.hpp | 328 +++ .../include/IKFoM_toolkit/mtk/types/S2.hpp | 316 +++ .../include/IKFoM_toolkit/mtk/types/SOn.hpp | 317 +++ .../include/IKFoM_toolkit/mtk/types/vect.hpp | 461 +++ .../mtk/types/wrapped_cv_mat.hpp | 113 + src/FAST_LIO/include/common_lib.h | 259 ++ src/FAST_LIO/include/ikd-Tree/README.md | 2 + src/FAST_LIO/include/ikd-Tree/ikd_Tree.cpp | 1728 ++++++++++++ src/FAST_LIO/include/ikd-Tree/ikd_Tree.h | 344 +++ src/FAST_LIO/include/matplotlibcpp.h | 2499 +++++++++++++++++ src/FAST_LIO/include/so3_math.h | 111 + src/FAST_LIO/include/use-ikfom.hpp | 126 + src/FAST_LIO/launch/Pointcloud2Map.launch | 13 + src/FAST_LIO/launch/gdb_debug_example.launch | 22 + src/FAST_LIO/launch/mapping_avia.launch | 21 + src/FAST_LIO/launch/mapping_horizon.launch | 21 + src/FAST_LIO/launch/mapping_mid360.launch | 26 + src/FAST_LIO/launch/mapping_ouster64.launch | 21 + src/FAST_LIO/launch/mapping_velodyne.launch | 21 + src/FAST_LIO/msg/Pose6D.msg | 7 + src/FAST_LIO/package.xml | 47 + src/FAST_LIO/rviz_cfg/.gitignore | 0 src/FAST_LIO/rviz_cfg/loam_livox.rviz | 363 +++ src/FAST_LIO/src/IMU_Processing.hpp | 377 +++ src/FAST_LIO/src/laserMapping.cpp | 1048 +++++++ src/FAST_LIO/src/preprocess.cpp | 928 ++++++ src/FAST_LIO/src/preprocess.h | 124 + src/Point-LIO/.gitmodules | 8 + src/Point-LIO/CMakeLists.txt | 83 + src/Point-LIO/LICENSE | 339 +++ src/Point-LIO/Log/guide.md | 1 + src/Point-LIO/Log/imu.txt | 0 src/Point-LIO/Log/imu_pbp.txt | 0 src/Point-LIO/Log/mat_out.txt | 0 src/Point-LIO/Log/plot.py | 46 + src/Point-LIO/Log/plot_imu.py | 125 + src/Point-LIO/Log/plot_out.py | 178 ++ src/Point-LIO/Log/pos_log.txt | 0 src/Point-LIO/PCD/temp.txt | 1 + src/Point-LIO/README.md | 183 ++ src/Point-LIO/config/avia.yaml | 57 + src/Point-LIO/config/horizon.yaml | 57 + src/Point-LIO/config/mid360.yaml | 57 + src/Point-LIO/config/ouster64.yaml | 57 + src/Point-LIO/config/velody16.yaml | 63 + .../include/.vscode/c_cpp_properties.json | 18 + src/Point-LIO/include/.vscode/settings.json | 6 + .../include/FOV_Checker/FOV_Checker.cpp | 472 ++++ .../include/FOV_Checker/FOV_Checker.h | 33 + src/Point-LIO/include/IKFoM/.gitignore | 32 + .../include/IKFoM/.vscode/settings.json | 5 + .../IKFoM/IKFoM_toolkit/.vscode/settings.json | 62 + .../esekfom/.vscode/settings.json | 5 + .../IKFoM/IKFoM_toolkit/esekfom/esekfom.hpp | 390 +++ .../IKFoM/IKFoM_toolkit/esekfom/util.hpp | 82 + .../IKFoM_toolkit/mtk/build_manifold.hpp | 248 ++ .../IKFoM_toolkit/mtk/src/SubManifold.hpp | 123 + .../IKFoM/IKFoM_toolkit/mtk/src/mtkmath.hpp | 294 ++ .../IKFoM/IKFoM_toolkit/mtk/src/vectview.hpp | 168 ++ .../IKFoM/IKFoM_toolkit/mtk/startIdx.hpp | 328 +++ .../IKFoM/IKFoM_toolkit/mtk/types/S2.hpp | 326 +++ .../IKFoM/IKFoM_toolkit/mtk/types/SEn.hpp | 334 +++ .../IKFoM/IKFoM_toolkit/mtk/types/SOn.hpp | 360 +++ .../IKFoM/IKFoM_toolkit/mtk/types/vect.hpp | 511 ++++ .../mtk/types/wrapped_cv_mat.hpp | 113 + src/Point-LIO/include/IKFoM/LICENSE | 339 +++ src/Point-LIO/include/IKFoM/README.md | 489 ++++ src/Point-LIO/include/common_lib.h | 176 ++ src/Point-LIO/include/ikd-Tree/README.md | 2 + src/Point-LIO/include/ikd-Tree/ikd_Tree.cpp | 1728 ++++++++++++ src/Point-LIO/include/ikd-Tree/ikd_Tree.h | 344 +++ src/Point-LIO/include/so3_math.h | 113 + src/Point-LIO/launch/gdb_debug_example.launch | 25 + src/Point-LIO/launch/mapping_avia.launch | 25 + src/Point-LIO/launch/mapping_horizon.launch | 25 + src/Point-LIO/launch/mapping_mid360.launch | 25 + src/Point-LIO/launch/mapping_ouster64.launch | 25 + src/Point-LIO/launch/mapping_velody16.launch | 25 + src/Point-LIO/package.xml | 47 + src/Point-LIO/rviz_cfg/.gitignore | 0 src/Point-LIO/rviz_cfg/loam_livox.rviz | 364 +++ src/Point-LIO/src/Estimator.cpp | 452 +++ src/Point-LIO/src/Estimator.h | 118 + src/Point-LIO/src/IMU_Processing.hpp | 181 ++ src/Point-LIO/src/laserMapping.cpp | 1378 +++++++++ src/Point-LIO/src/parameters.cpp | 90 + src/Point-LIO/src/parameters.h | 40 + src/Point-LIO/src/preprocess.cpp | 795 ++++++ src/Point-LIO/src/preprocess.h | 145 + src/livox_ros_driver2 | 1 + 117 files changed, 25932 insertions(+) create mode 100644 .catkin_workspace create mode 100644 .gitignore create mode 100644 build.sh create mode 120000 src/CMakeLists.txt create mode 100644 src/FAST_LIO/.github/stale.yml create mode 100644 src/FAST_LIO/.gitignore create mode 100644 src/FAST_LIO/.gitmodules create mode 100644 src/FAST_LIO/CMakeLists.txt create mode 100644 src/FAST_LIO/LICENSE create mode 100644 src/FAST_LIO/Log/fast_lio_time_log_analysis.m create mode 100644 src/FAST_LIO/Log/guide.md create mode 100644 src/FAST_LIO/Log/plot.py create mode 100644 src/FAST_LIO/PCD/1 create mode 100644 src/FAST_LIO/README.md create mode 100644 src/FAST_LIO/config/avia.yaml create mode 100644 src/FAST_LIO/config/horizon.yaml create mode 100644 src/FAST_LIO/config/mid360.yaml create mode 100644 src/FAST_LIO/config/mid360_down.yaml create mode 100644 src/FAST_LIO/config/ouster64.yaml create mode 100644 src/FAST_LIO/config/velodyne.yaml create mode 100644 src/FAST_LIO/include/Exp_mat.h create mode 100755 src/FAST_LIO/include/IKFoM_toolkit/esekfom/esekfom.hpp create mode 100755 src/FAST_LIO/include/IKFoM_toolkit/esekfom/util.hpp create mode 100755 src/FAST_LIO/include/IKFoM_toolkit/mtk/build_manifold.hpp create mode 100755 src/FAST_LIO/include/IKFoM_toolkit/mtk/src/SubManifold.hpp create mode 100755 src/FAST_LIO/include/IKFoM_toolkit/mtk/src/mtkmath.hpp create mode 100755 src/FAST_LIO/include/IKFoM_toolkit/mtk/src/vectview.hpp create mode 100755 src/FAST_LIO/include/IKFoM_toolkit/mtk/startIdx.hpp create mode 100755 src/FAST_LIO/include/IKFoM_toolkit/mtk/types/S2.hpp create mode 100755 src/FAST_LIO/include/IKFoM_toolkit/mtk/types/SOn.hpp create mode 100755 src/FAST_LIO/include/IKFoM_toolkit/mtk/types/vect.hpp create mode 100755 src/FAST_LIO/include/IKFoM_toolkit/mtk/types/wrapped_cv_mat.hpp create mode 100644 src/FAST_LIO/include/common_lib.h create mode 100644 src/FAST_LIO/include/ikd-Tree/README.md create mode 100644 src/FAST_LIO/include/ikd-Tree/ikd_Tree.cpp create mode 100644 src/FAST_LIO/include/ikd-Tree/ikd_Tree.h create mode 100644 src/FAST_LIO/include/matplotlibcpp.h create mode 100644 src/FAST_LIO/include/so3_math.h create mode 100644 src/FAST_LIO/include/use-ikfom.hpp create mode 100644 src/FAST_LIO/launch/Pointcloud2Map.launch create mode 100644 src/FAST_LIO/launch/gdb_debug_example.launch create mode 100644 src/FAST_LIO/launch/mapping_avia.launch create mode 100644 src/FAST_LIO/launch/mapping_horizon.launch create mode 100644 src/FAST_LIO/launch/mapping_mid360.launch create mode 100644 src/FAST_LIO/launch/mapping_ouster64.launch create mode 100644 src/FAST_LIO/launch/mapping_velodyne.launch create mode 100644 src/FAST_LIO/msg/Pose6D.msg create mode 100644 src/FAST_LIO/package.xml create mode 100644 src/FAST_LIO/rviz_cfg/.gitignore create mode 100644 src/FAST_LIO/rviz_cfg/loam_livox.rviz create mode 100644 src/FAST_LIO/src/IMU_Processing.hpp create mode 100644 src/FAST_LIO/src/laserMapping.cpp create mode 100644 src/FAST_LIO/src/preprocess.cpp create mode 100644 src/FAST_LIO/src/preprocess.h create mode 100644 src/Point-LIO/.gitmodules create mode 100755 src/Point-LIO/CMakeLists.txt create mode 100644 src/Point-LIO/LICENSE create mode 100755 src/Point-LIO/Log/guide.md create mode 100755 src/Point-LIO/Log/imu.txt create mode 100644 src/Point-LIO/Log/imu_pbp.txt create mode 100644 src/Point-LIO/Log/mat_out.txt create mode 100755 src/Point-LIO/Log/plot.py create mode 100755 src/Point-LIO/Log/plot_imu.py create mode 100755 src/Point-LIO/Log/plot_out.py create mode 100644 src/Point-LIO/Log/pos_log.txt create mode 100644 src/Point-LIO/PCD/temp.txt create mode 100644 src/Point-LIO/README.md create mode 100755 src/Point-LIO/config/avia.yaml create mode 100755 src/Point-LIO/config/horizon.yaml create mode 100644 src/Point-LIO/config/mid360.yaml create mode 100755 src/Point-LIO/config/ouster64.yaml create mode 100755 src/Point-LIO/config/velody16.yaml create mode 100755 src/Point-LIO/include/.vscode/c_cpp_properties.json create mode 100755 src/Point-LIO/include/.vscode/settings.json create mode 100755 src/Point-LIO/include/FOV_Checker/FOV_Checker.cpp create mode 100755 src/Point-LIO/include/FOV_Checker/FOV_Checker.h create mode 100644 src/Point-LIO/include/IKFoM/.gitignore create mode 100644 src/Point-LIO/include/IKFoM/.vscode/settings.json create mode 100755 src/Point-LIO/include/IKFoM/IKFoM_toolkit/.vscode/settings.json create mode 100755 src/Point-LIO/include/IKFoM/IKFoM_toolkit/esekfom/.vscode/settings.json create mode 100755 src/Point-LIO/include/IKFoM/IKFoM_toolkit/esekfom/esekfom.hpp create mode 100755 src/Point-LIO/include/IKFoM/IKFoM_toolkit/esekfom/util.hpp create mode 100755 src/Point-LIO/include/IKFoM/IKFoM_toolkit/mtk/build_manifold.hpp create mode 100755 src/Point-LIO/include/IKFoM/IKFoM_toolkit/mtk/src/SubManifold.hpp create mode 100755 src/Point-LIO/include/IKFoM/IKFoM_toolkit/mtk/src/mtkmath.hpp create mode 100755 src/Point-LIO/include/IKFoM/IKFoM_toolkit/mtk/src/vectview.hpp create mode 100755 src/Point-LIO/include/IKFoM/IKFoM_toolkit/mtk/startIdx.hpp create mode 100755 src/Point-LIO/include/IKFoM/IKFoM_toolkit/mtk/types/S2.hpp create mode 100755 src/Point-LIO/include/IKFoM/IKFoM_toolkit/mtk/types/SEn.hpp create mode 100755 src/Point-LIO/include/IKFoM/IKFoM_toolkit/mtk/types/SOn.hpp create mode 100755 src/Point-LIO/include/IKFoM/IKFoM_toolkit/mtk/types/vect.hpp create mode 100755 src/Point-LIO/include/IKFoM/IKFoM_toolkit/mtk/types/wrapped_cv_mat.hpp create mode 100644 src/Point-LIO/include/IKFoM/LICENSE create mode 100644 src/Point-LIO/include/IKFoM/README.md create mode 100755 src/Point-LIO/include/common_lib.h create mode 100644 src/Point-LIO/include/ikd-Tree/README.md create mode 100644 src/Point-LIO/include/ikd-Tree/ikd_Tree.cpp create mode 100644 src/Point-LIO/include/ikd-Tree/ikd_Tree.h create mode 100755 src/Point-LIO/include/so3_math.h create mode 100755 src/Point-LIO/launch/gdb_debug_example.launch create mode 100755 src/Point-LIO/launch/mapping_avia.launch create mode 100755 src/Point-LIO/launch/mapping_horizon.launch create mode 100644 src/Point-LIO/launch/mapping_mid360.launch create mode 100755 src/Point-LIO/launch/mapping_ouster64.launch create mode 100755 src/Point-LIO/launch/mapping_velody16.launch create mode 100755 src/Point-LIO/package.xml create mode 100755 src/Point-LIO/rviz_cfg/.gitignore create mode 100755 src/Point-LIO/rviz_cfg/loam_livox.rviz create mode 100755 src/Point-LIO/src/Estimator.cpp create mode 100755 src/Point-LIO/src/Estimator.h create mode 100755 src/Point-LIO/src/IMU_Processing.hpp create mode 100755 src/Point-LIO/src/laserMapping.cpp create mode 100755 src/Point-LIO/src/parameters.cpp create mode 100755 src/Point-LIO/src/parameters.h create mode 100755 src/Point-LIO/src/preprocess.cpp create mode 100755 src/Point-LIO/src/preprocess.h create mode 160000 src/livox_ros_driver2 diff --git a/.catkin_workspace b/.catkin_workspace new file mode 100644 index 0000000..52fd97e --- /dev/null +++ b/.catkin_workspace @@ -0,0 +1 @@ +# This file currently only serves to mark the location of a catkin workspace for tool integration diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..f936c19 --- /dev/null +++ b/.gitignore @@ -0,0 +1,2 @@ +build/ +devel/ \ No newline at end of file diff --git a/build.sh b/build.sh new file mode 100644 index 0000000..2e212dd --- /dev/null +++ b/build.sh @@ -0,0 +1 @@ +catkin_make \ No newline at end of file diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt new file mode 120000 index 0000000..2016816 --- /dev/null +++ b/src/CMakeLists.txt @@ -0,0 +1 @@ +/opt/ros/noetic/share/catkin/cmake/toplevel.cmake \ No newline at end of file diff --git a/src/FAST_LIO/.github/stale.yml b/src/FAST_LIO/.github/stale.yml new file mode 100644 index 0000000..4d545fe --- /dev/null +++ b/src/FAST_LIO/.github/stale.yml @@ -0,0 +1,17 @@ +# Number of days of inactivity before an issue becomes stale +daysUntilStale: 21 +# Number of days of inactivity before a stale issue is closed +daysUntilClose: 1 +# Issues with these labels will never be considered stale +exemptLabels: + - pinned + - security +# Label to use when marking an issue as stale +staleLabel: stale +# Comment to post when marking an issue as stale. Set to `false` to disable +markComment: > + This issue has been automatically marked as stale because it has not had + recent activity. It will be closed if no further activity occurs. Thank you + for your contributions. +# Comment to post when closing a stale issue. Set to `false` to disable +closeComment: false diff --git a/src/FAST_LIO/.gitignore b/src/FAST_LIO/.gitignore new file mode 100644 index 0000000..eb7be62 --- /dev/null +++ b/src/FAST_LIO/.gitignore @@ -0,0 +1,8 @@ +build +Log/*.png +Log/*.txt +Log/*.csv +Log/*.pdf +.vscode/c_cpp_properties.json +.vscode/settings.json +PCD/*.pcd diff --git a/src/FAST_LIO/.gitmodules b/src/FAST_LIO/.gitmodules new file mode 100644 index 0000000..b8a0efa --- /dev/null +++ b/src/FAST_LIO/.gitmodules @@ -0,0 +1,4 @@ +[submodule "include/ikd-Tree"] + path = include/ikd-Tree + url = https://github.com/hku-mars/ikd-Tree.git + branch = fast_lio diff --git a/src/FAST_LIO/CMakeLists.txt b/src/FAST_LIO/CMakeLists.txt new file mode 100644 index 0000000..3cd8269 --- /dev/null +++ b/src/FAST_LIO/CMakeLists.txt @@ -0,0 +1,89 @@ +cmake_minimum_required(VERSION 2.8.3) +project(fast_lio) + +SET(CMAKE_BUILD_TYPE "Debug") + +ADD_COMPILE_OPTIONS(-std=c++14 ) +ADD_COMPILE_OPTIONS(-std=c++14 ) +set( CMAKE_CXX_FLAGS "-std=c++14 -O3" ) + +add_definitions(-DROOT_DIR=\"${CMAKE_CURRENT_SOURCE_DIR}/\") + +set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -fexceptions" ) +set(CMAKE_CXX_STANDARD 14) +set(CMAKE_CXX_STANDARD_REQUIRED ON) +set(CMAKE_CXX_EXTENSIONS OFF) +set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++14 -pthread -std=c++0x -std=c++14 -fexceptions") + +message("Current CPU archtecture: ${CMAKE_SYSTEM_PROCESSOR}") +if(CMAKE_SYSTEM_PROCESSOR MATCHES "(x86)|(X86)|(amd64)|(AMD64)" ) + include(ProcessorCount) + ProcessorCount(N) + message("Processer number: ${N}") + if(N GREATER 4) + add_definitions(-DMP_EN) + add_definitions(-DMP_PROC_NUM=3) + message("core for MP: 3") + elseif(N GREATER 3) + add_definitions(-DMP_EN) + add_definitions(-DMP_PROC_NUM=2) + message("core for MP: 2") + else() + add_definitions(-DMP_PROC_NUM=1) + endif() +else() + add_definitions(-DMP_PROC_NUM=1) +endif() + +find_package(OpenMP QUIET) +set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS}") +set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${OpenMP_C_FLAGS}") + +find_package(PythonLibs REQUIRED) +find_path(MATPLOTLIB_CPP_INCLUDE_DIRS "matplotlibcpp.h") + +find_package(catkin REQUIRED COMPONENTS + geometry_msgs + nav_msgs + sensor_msgs + roscpp + rospy + std_msgs + pcl_ros + tf + livox_ros_driver2 + message_generation + eigen_conversions +) + +find_package(Eigen3 REQUIRED) +find_package(PCL 1.10 REQUIRED) + +message(Eigen: ${EIGEN3_INCLUDE_DIR}) + +include_directories( + ${catkin_INCLUDE_DIRS} + ${EIGEN3_INCLUDE_DIR} + ${PCL_INCLUDE_DIRS} + ${PYTHON_INCLUDE_DIRS} + include) + +add_message_files( + FILES + Pose6D.msg +) + +generate_messages( + DEPENDENCIES + geometry_msgs +) + +catkin_package( + CATKIN_DEPENDS geometry_msgs nav_msgs roscpp rospy std_msgs message_runtime + DEPENDS EIGEN3 PCL + INCLUDE_DIRS +) + +add_executable(fastlio_mapping src/laserMapping.cpp include/ikd-Tree/ikd_Tree.cpp src/preprocess.cpp) +target_link_libraries(fastlio_mapping ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${PYTHON_LIBRARIES}) +target_include_directories(fastlio_mapping PRIVATE ${PYTHON_INCLUDE_DIRS}) \ No newline at end of file diff --git a/src/FAST_LIO/LICENSE b/src/FAST_LIO/LICENSE new file mode 100644 index 0000000..d159169 --- /dev/null +++ b/src/FAST_LIO/LICENSE @@ -0,0 +1,339 @@ + GNU GENERAL PUBLIC LICENSE + Version 2, June 1991 + + Copyright (C) 1989, 1991 Free Software Foundation, Inc., + 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA + Everyone is permitted to copy and distribute verbatim copies + of this license document, but changing it is not allowed. + + Preamble + + The licenses for most software are designed to take away your +freedom to share and change it. By contrast, the GNU General Public +License is intended to guarantee your freedom to share and change free +software--to make sure the software is free for all its users. This +General Public License applies to most of the Free Software +Foundation's software and to any other program whose authors commit to +using it. (Some other Free Software Foundation software is covered by +the GNU Lesser General Public License instead.) You can apply it to +your programs, too. + + When we speak of free software, we are referring to freedom, not +price. Our General Public Licenses are designed to make sure that you +have the freedom to distribute copies of free software (and charge for +this service if you wish), that you receive source code or can get it +if you want it, that you can change the software or use pieces of it +in new free programs; and that you know you can do these things. + + To protect your rights, we need to make restrictions that forbid +anyone to deny you these rights or to ask you to surrender the rights. +These restrictions translate to certain responsibilities for you if you +distribute copies of the software, or if you modify it. + + For example, if you distribute copies of such a program, whether +gratis or for a fee, you must give the recipients all the rights that +you have. You must make sure that they, too, receive or can get the +source code. And you must show them these terms so they know their +rights. + + We protect your rights with two steps: (1) copyright the software, and +(2) offer you this license which gives you legal permission to copy, +distribute and/or modify the software. + + Also, for each author's protection and ours, we want to make certain +that everyone understands that there is no warranty for this free +software. If the software is modified by someone else and passed on, we +want its recipients to know that what they have is not the original, so +that any problems introduced by others will not reflect on the original +authors' reputations. + + Finally, any free program is threatened constantly by software +patents. We wish to avoid the danger that redistributors of a free +program will individually obtain patent licenses, in effect making the +program proprietary. To prevent this, we have made it clear that any +patent must be licensed for everyone's free use or not licensed at all. + + The precise terms and conditions for copying, distribution and +modification follow. + + GNU GENERAL PUBLIC LICENSE + TERMS AND CONDITIONS FOR COPYING, DISTRIBUTION AND MODIFICATION + + 0. This License applies to any program or other work which contains +a notice placed by the copyright holder saying it may be distributed +under the terms of this General Public License. The "Program", below, +refers to any such program or work, and a "work based on the Program" +means either the Program or any derivative work under copyright law: +that is to say, a work containing the Program or a portion of it, +either verbatim or with modifications and/or translated into another +language. (Hereinafter, translation is included without limitation in +the term "modification".) Each licensee is addressed as "you". + +Activities other than copying, distribution and modification are not +covered by this License; they are outside its scope. The act of +running the Program is not restricted, and the output from the Program +is covered only if its contents constitute a work based on the +Program (independent of having been made by running the Program). +Whether that is true depends on what the Program does. + + 1. You may copy and distribute verbatim copies of the Program's +source code as you receive it, in any medium, provided that you +conspicuously and appropriately publish on each copy an appropriate +copyright notice and disclaimer of warranty; keep intact all the +notices that refer to this License and to the absence of any warranty; +and give any other recipients of the Program a copy of this License +along with the Program. + +You may charge a fee for the physical act of transferring a copy, and +you may at your option offer warranty protection in exchange for a fee. + + 2. You may modify your copy or copies of the Program or any portion +of it, thus forming a work based on the Program, and copy and +distribute such modifications or work under the terms of Section 1 +above, provided that you also meet all of these conditions: + + a) You must cause the modified files to carry prominent notices + stating that you changed the files and the date of any change. + + b) You must cause any work that you distribute or publish, that in + whole or in part contains or is derived from the Program or any + part thereof, to be licensed as a whole at no charge to all third + parties under the terms of this License. + + c) If the modified program normally reads commands interactively + when run, you must cause it, when started running for such + interactive use in the most ordinary way, to print or display an + announcement including an appropriate copyright notice and a + notice that there is no warranty (or else, saying that you provide + a warranty) and that users may redistribute the program under + these conditions, and telling the user how to view a copy of this + License. (Exception: if the Program itself is interactive but + does not normally print such an announcement, your work based on + the Program is not required to print an announcement.) + +These requirements apply to the modified work as a whole. If +identifiable sections of that work are not derived from the Program, +and can be reasonably considered independent and separate works in +themselves, then this License, and its terms, do not apply to those +sections when you distribute them as separate works. But when you +distribute the same sections as part of a whole which is a work based +on the Program, the distribution of the whole must be on the terms of +this License, whose permissions for other licensees extend to the +entire whole, and thus to each and every part regardless of who wrote it. + +Thus, it is not the intent of this section to claim rights or contest +your rights to work written entirely by you; rather, the intent is to +exercise the right to control the distribution of derivative or +collective works based on the Program. + +In addition, mere aggregation of another work not based on the Program +with the Program (or with a work based on the Program) on a volume of +a storage or distribution medium does not bring the other work under +the scope of this License. + + 3. You may copy and distribute the Program (or a work based on it, +under Section 2) in object code or executable form under the terms of +Sections 1 and 2 above provided that you also do one of the following: + + a) Accompany it with the complete corresponding machine-readable + source code, which must be distributed under the terms of Sections + 1 and 2 above on a medium customarily used for software interchange; or, + + b) Accompany it with a written offer, valid for at least three + years, to give any third party, for a charge no more than your + cost of physically performing source distribution, a complete + machine-readable copy of the corresponding source code, to be + distributed under the terms of Sections 1 and 2 above on a medium + customarily used for software interchange; or, + + c) Accompany it with the information you received as to the offer + to distribute corresponding source code. (This alternative is + allowed only for noncommercial distribution and only if you + received the program in object code or executable form with such + an offer, in accord with Subsection b above.) + +The source code for a work means the preferred form of the work for +making modifications to it. For an executable work, complete source +code means all the source code for all modules it contains, plus any +associated interface definition files, plus the scripts used to +control compilation and installation of the executable. However, as a +special exception, the source code distributed need not include +anything that is normally distributed (in either source or binary +form) with the major components (compiler, kernel, and so on) of the +operating system on which the executable runs, unless that component +itself accompanies the executable. + +If distribution of executable or object code is made by offering +access to copy from a designated place, then offering equivalent +access to copy the source code from the same place counts as +distribution of the source code, even though third parties are not +compelled to copy the source along with the object code. + + 4. You may not copy, modify, sublicense, or distribute the Program +except as expressly provided under this License. Any attempt +otherwise to copy, modify, sublicense or distribute the Program is +void, and will automatically terminate your rights under this License. +However, parties who have received copies, or rights, from you under +this License will not have their licenses terminated so long as such +parties remain in full compliance. + + 5. You are not required to accept this License, since you have not +signed it. However, nothing else grants you permission to modify or +distribute the Program or its derivative works. These actions are +prohibited by law if you do not accept this License. Therefore, by +modifying or distributing the Program (or any work based on the +Program), you indicate your acceptance of this License to do so, and +all its terms and conditions for copying, distributing or modifying +the Program or works based on it. + + 6. Each time you redistribute the Program (or any work based on the +Program), the recipient automatically receives a license from the +original licensor to copy, distribute or modify the Program subject to +these terms and conditions. You may not impose any further +restrictions on the recipients' exercise of the rights granted herein. +You are not responsible for enforcing compliance by third parties to +this License. + + 7. If, as a consequence of a court judgment or allegation of patent +infringement or for any other reason (not limited to patent issues), +conditions are imposed on you (whether by court order, agreement or +otherwise) that contradict the conditions of this License, they do not +excuse you from the conditions of this License. If you cannot +distribute so as to satisfy simultaneously your obligations under this +License and any other pertinent obligations, then as a consequence you +may not distribute the Program at all. For example, if a patent +license would not permit royalty-free redistribution of the Program by +all those who receive copies directly or indirectly through you, then +the only way you could satisfy both it and this License would be to +refrain entirely from distribution of the Program. + +If any portion of this section is held invalid or unenforceable under +any particular circumstance, the balance of the section is intended to +apply and the section as a whole is intended to apply in other +circumstances. + +It is not the purpose of this section to induce you to infringe any +patents or other property right claims or to contest validity of any +such claims; this section has the sole purpose of protecting the +integrity of the free software distribution system, which is +implemented by public license practices. Many people have made +generous contributions to the wide range of software distributed +through that system in reliance on consistent application of that +system; it is up to the author/donor to decide if he or she is willing +to distribute software through any other system and a licensee cannot +impose that choice. + +This section is intended to make thoroughly clear what is believed to +be a consequence of the rest of this License. + + 8. If the distribution and/or use of the Program is restricted in +certain countries either by patents or by copyrighted interfaces, the +original copyright holder who places the Program under this License +may add an explicit geographical distribution limitation excluding +those countries, so that distribution is permitted only in or among +countries not thus excluded. In such case, this License incorporates +the limitation as if written in the body of this License. + + 9. The Free Software Foundation may publish revised and/or new versions +of the General Public License from time to time. Such new versions will +be similar in spirit to the present version, but may differ in detail to +address new problems or concerns. + +Each version is given a distinguishing version number. If the Program +specifies a version number of this License which applies to it and "any +later version", you have the option of following the terms and conditions +either of that version or of any later version published by the Free +Software Foundation. If the Program does not specify a version number of +this License, you may choose any version ever published by the Free Software +Foundation. + + 10. If you wish to incorporate parts of the Program into other free +programs whose distribution conditions are different, write to the author +to ask for permission. For software which is copyrighted by the Free +Software Foundation, write to the Free Software Foundation; we sometimes +make exceptions for this. Our decision will be guided by the two goals +of preserving the free status of all derivatives of our free software and +of promoting the sharing and reuse of software generally. + + NO WARRANTY + + 11. BECAUSE THE PROGRAM IS LICENSED FREE OF CHARGE, THERE IS NO WARRANTY +FOR THE PROGRAM, TO THE EXTENT PERMITTED BY APPLICABLE LAW. EXCEPT WHEN +OTHERWISE STATED IN WRITING THE COPYRIGHT HOLDERS AND/OR OTHER PARTIES +PROVIDE THE PROGRAM "AS IS" WITHOUT WARRANTY OF ANY KIND, EITHER EXPRESSED +OR IMPLIED, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF +MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE. THE ENTIRE RISK AS +TO THE QUALITY AND PERFORMANCE OF THE PROGRAM IS WITH YOU. SHOULD THE +PROGRAM PROVE DEFECTIVE, YOU ASSUME THE COST OF ALL NECESSARY SERVICING, +REPAIR OR CORRECTION. + + 12. IN NO EVENT UNLESS REQUIRED BY APPLICABLE LAW OR AGREED TO IN WRITING +WILL ANY COPYRIGHT HOLDER, OR ANY OTHER PARTY WHO MAY MODIFY AND/OR +REDISTRIBUTE THE PROGRAM AS PERMITTED ABOVE, BE LIABLE TO YOU FOR DAMAGES, +INCLUDING ANY GENERAL, SPECIAL, INCIDENTAL OR CONSEQUENTIAL DAMAGES ARISING +OUT OF THE USE OR INABILITY TO USE THE PROGRAM (INCLUDING BUT NOT LIMITED +TO LOSS OF DATA OR DATA BEING RENDERED INACCURATE OR LOSSES SUSTAINED BY +YOU OR THIRD PARTIES OR A FAILURE OF THE PROGRAM TO OPERATE WITH ANY OTHER +PROGRAMS), EVEN IF SUCH HOLDER OR OTHER PARTY HAS BEEN ADVISED OF THE +POSSIBILITY OF SUCH DAMAGES. + + END OF TERMS AND CONDITIONS + + How to Apply These Terms to Your New Programs + + If you develop a new program, and you want it to be of the greatest +possible use to the public, the best way to achieve this is to make it +free software which everyone can redistribute and change under these terms. + + To do so, attach the following notices to the program. It is safest +to attach them to the start of each source file to most effectively +convey the exclusion of warranty; and each file should have at least +the "copyright" line and a pointer to where the full notice is found. + + + Copyright (C) + + This program is free software; you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation; either version 2 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License along + with this program; if not, write to the Free Software Foundation, Inc., + 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA. + +Also add information on how to contact you by electronic and paper mail. + +If the program is interactive, make it output a short notice like this +when it starts in an interactive mode: + + Gnomovision version 69, Copyright (C) year name of author + Gnomovision comes with ABSOLUTELY NO WARRANTY; for details type `show w'. + This is free software, and you are welcome to redistribute it + under certain conditions; type `show c' for details. + +The hypothetical commands `show w' and `show c' should show the appropriate +parts of the General Public License. Of course, the commands you use may +be called something other than `show w' and `show c'; they could even be +mouse-clicks or menu items--whatever suits your program. + +You should also get your employer (if you work as a programmer) or your +school, if any, to sign a "copyright disclaimer" for the program, if +necessary. Here is a sample; alter the names: + + Yoyodyne, Inc., hereby disclaims all copyright interest in the program + `Gnomovision' (which makes passes at compilers) written by James Hacker. + + , 1 April 1989 + Ty Coon, President of Vice + +This General Public License does not permit incorporating your program into +proprietary programs. If your program is a subroutine library, you may +consider it more useful to permit linking proprietary applications with the +library. If this is what you want to do, use the GNU Lesser General +Public License instead of this License. diff --git a/src/FAST_LIO/Log/fast_lio_time_log_analysis.m b/src/FAST_LIO/Log/fast_lio_time_log_analysis.m new file mode 100644 index 0000000..8a0b679 --- /dev/null +++ b/src/FAST_LIO/Log/fast_lio_time_log_analysis.m @@ -0,0 +1,135 @@ +clear +close all + +Color_red = [0.6350 0.0780 0.1840]; +Color_blue = [0 0.4470 0.7410]; +Color_orange = [0.8500 0.3250 0.0980]; +Color_green = [0.4660 0.6740 0.1880]; +Color_lightblue = [0.3010 0.7450 0.9330]; +Color_purple = [0.4940 0.1840 0.5560]; +Color_yellow = [0.9290 0.6940 0.1250]; + +fast_lio_ikdtree = csvread("./fast_lio_time_log.csv",1,0); +timestamp_ikd = fast_lio_ikdtree(:,1); +timestamp_ikd = timestamp_ikd - min(timestamp_ikd); +total_time_ikd = fast_lio_ikdtree(:,2)*1e3; +scan_num = fast_lio_ikdtree(:,3); +incremental_time_ikd = fast_lio_ikdtree(:,4)*1e3; +search_time_ikd = fast_lio_ikdtree(:,5)*1e3; +delete_size_ikd = fast_lio_ikdtree(:,6); +delete_time_ikd = fast_lio_ikdtree(:,7) * 1e3; +tree_size_ikd_st = fast_lio_ikdtree(:,8); +tree_size_ikd = fast_lio_ikdtree(:,9); +add_points = fast_lio_ikdtree(:,10); + +fast_lio_forest = csvread("fast_lio_time_log.csv",1,0); +fov_check_time_forest = fast_lio_forest(:,5)*1e3; +average_time_forest = fast_lio_forest(:,2)*1e3; +total_time_forest = fast_lio_forest(:,6)*1e3; +incremental_time_forest = fast_lio_forest(:,3)*1e3; +search_time_forest = fast_lio_forest(:,4)*1e3; +timestamp_forest = fast_lio_forest(:,1); + +% Use slide window to calculate average +L = 1; % Length of slide window +for i = 1:length(timestamp_ikd) + if (i 0); +search_time_ikd = search_time_ikd(index_ikd); +index_forest = find(search_time_forest > 0); +search_time_forest = search_time_forest(index_forest); + +t = nexttile; +hold on; +boxplot_data_ikd = [incremental_time_ikd,total_time_ikd]; +boxplot_data_forest = [incremental_time_forest,total_time_forest]; +Colors_ikd = [Color_blue;Color_blue;Color_blue]; +Colors_forest = [Color_orange;Color_orange;Color_orange]; +% xticks([3,8,13]) +h_search_ikd = boxplot(search_time_ikd,'Whisker',50,'Positions',1,'Colors',Color_blue,'Widths',0.3); +h_search_forest = boxplot(search_time_forest,'Whisker',50,'Positions',1.5,'Colors',Color_orange,'Widths',0.3); +h_ikd = boxplot(boxplot_data_ikd,'Whisker',50,'Positions',[3,5],'Colors',Color_blue,'Widths',0.3); +h_forest = boxplot(boxplot_data_forest,'Whisker',50,'Positions',[3.5,5.5],'Colors',Color_orange,'Widths',0.3); +ax2 = gca; +ax2.YAxis.Scale = 'log'; +xlim([0.5,6.0]) +ylim([0.0008,100]) +xticks([1.25 3.25 5.25]) +xticklabels({'Nearest Search',' Incremental Updates','Total Time'}); +yticks([1e-3,1e-2,1e-1,1e0,1e1,1e2]) +ax2.YAxis.FontSize = 12; +ax2.XAxis.FontSize = 14.5; +% ax.XAxis.FontWeight = 'bold'; +ylabel('Run Time/ms','FontSize',14,'FontName','Times New Roman') +box_vars = [findall(h_search_ikd,'Tag','Box');findall(h_ikd,'Tag','Box');findall(h_search_forest,'Tag','Box');findall(h_forest,'Tag','Box')]; +for j=1:length(box_vars) + if (j<=3) + Color = Color_blue; + else + Color = Color_orange; + end + patch(get(box_vars(j),'XData'),get(box_vars(j),'YData'),Color,'FaceAlpha',0.25,'EdgeColor',Color); +end +Lg = legend(box_vars([1,4]), {'ikd-Tree','ikd-Forest'},'Location',[0.6707 0.4305 0.265 0.07891],'fontsize',14,'fontname','Times New Roman'); +grid on +set(gca,'YMinorGrid','off') +nexttile; +hold on; +grid on; +box on; +set(gca,'FontSize',12,'FontName','Times New Roman') +plot(timestamp_ikd, alpha_bal_ikd,'-','Color',Color_blue,'LineWidth',1.2); +plot(timestamp_ikd, alpha_del_ikd,'--','Color',Color_orange, 'LineWidth', 1.2); +plot(timestamp_ikd, 0.6*ones(size(alpha_bal_ikd)), ':','Color','black','LineWidth',1.2); +lg = legend("\alpha_{bal}", "\alpha_{del}",'location',[0.7871 0.1131 0.1433 0.069],'fontsize',14,'fontname','Times New Roman') +title("Re-balancing Criterion",'FontSize',16,'FontName','Times New Roman') +xlabel("time/s",'FontSize',16,'FontName','Times New Roman') +yl = ylabel("\alpha",'FontSize',15, 'Position',[285.7 0.4250 -1]) +xlim([32,390]); +ylim([0,0.85]); +ax3 = gca; +ax3.YAxis.FontSize = 12; +ax3.XAxis.FontSize = 12; +% print('./Figures/fastlio_exp_combine','-depsc','-r1200') +% exportgraphics(f,'./Figures/fastlio_exp_combine_1.pdf','ContentType','vector') + diff --git a/src/FAST_LIO/Log/guide.md b/src/FAST_LIO/Log/guide.md new file mode 100644 index 0000000..8ff3fc1 --- /dev/null +++ b/src/FAST_LIO/Log/guide.md @@ -0,0 +1 @@ +Here saved the debug records which can be drew by the ../Log/plot.py. The record function can be found frm the MACRO: DEBUG_FILE_DIR(name) in common_lib.h. diff --git a/src/FAST_LIO/Log/plot.py b/src/FAST_LIO/Log/plot.py new file mode 100644 index 0000000..e4aad85 --- /dev/null +++ b/src/FAST_LIO/Log/plot.py @@ -0,0 +1,94 @@ +# import matplotlib +# matplotlib.use('Agg') +import numpy as np +import matplotlib.pyplot as plt + + +#######for ikfom +fig, axs = plt.subplots(4,2) +lab_pre = ['', 'pre-x', 'pre-y', 'pre-z'] +lab_out = ['', 'out-x', 'out-y', 'out-z'] +plot_ind = range(7,10) +a_pre=np.loadtxt('mat_pre.txt') +a_out=np.loadtxt('mat_out.txt') +time=a_pre[:,0] +axs[0,0].set_title('Attitude') +axs[1,0].set_title('Translation') +axs[2,0].set_title('Extrins-R') +axs[3,0].set_title('Extrins-T') +axs[0,1].set_title('Velocity') +axs[1,1].set_title('bg') +axs[2,1].set_title('ba') +axs[3,1].set_title('Gravity') +for i in range(1,4): + for j in range(8): + axs[j%4, j/4].plot(time, a_pre[:,i+j*3],'.-', label=lab_pre[i]) + axs[j%4, j/4].plot(time, a_out[:,i+j*3],'.-', label=lab_out[i]) +for j in range(8): + # axs[j].set_xlim(386,389) + axs[j%4, j/4].grid() + axs[j%4, j/4].legend() +plt.grid() +#######for ikfom####### + + +#### Draw IMU data +# fig, axs = plt.subplots(2) +# imu=np.loadtxt('imu.txt') +# time=imu[:,0] +# axs[0].set_title('Gyroscope') +# axs[1].set_title('Accelerameter') +# lab_1 = ['gyr-x', 'gyr-y', 'gyr-z'] +# lab_2 = ['acc-x', 'acc-y', 'acc-z'] +# for i in range(3): +# # if i==1: +# axs[0].plot(time, imu[:,i+1],'.-', label=lab_1[i]) +# axs[1].plot(time, imu[:,i+4],'.-', label=lab_2[i]) +# for i in range(2): +# # axs[i].set_xlim(386,389) +# axs[i].grid() +# axs[i].legend() +# plt.grid() + +# #### Draw time calculation +# plt.figure(3) +# fig = plt.figure() +# font1 = {'family' : 'Times New Roman', +# 'weight' : 'normal', +# 'size' : 12, +# } +# c="red" +# a_out1=np.loadtxt('Log/mat_out_time_indoor1.txt') +# a_out2=np.loadtxt('Log/mat_out_time_indoor2.txt') +# a_out3=np.loadtxt('Log/mat_out_time_outdoor.txt') +# # n = a_out[:,1].size +# # time_mean = a_out[:,1].mean() +# # time_se = a_out[:,1].std() / np.sqrt(n) +# # time_err = a_out[:,1] - time_mean +# # feat_mean = a_out[:,2].mean() +# # feat_err = a_out[:,2] - feat_mean +# # feat_se = a_out[:,2].std() / np.sqrt(n) +# ax1 = fig.add_subplot(111) +# ax1.set_ylabel('Effective Feature Numbers',font1) +# ax1.boxplot(a_out1[:,2], showfliers=False, positions=[0.9]) +# ax1.boxplot(a_out2[:,2], showfliers=False, positions=[1.9]) +# ax1.boxplot(a_out3[:,2], showfliers=False, positions=[2.9]) +# ax1.set_ylim([0, 3000]) + +# ax2 = ax1.twinx() +# ax2.spines['right'].set_color('red') +# ax2.set_ylabel('Compute Time (ms)',font1) +# ax2.yaxis.label.set_color('red') +# ax2.tick_params(axis='y', colors='red') +# ax2.boxplot(a_out1[:,1]*1000, showfliers=False, positions=[1.1],boxprops=dict(color=c),capprops=dict(color=c),whiskerprops=dict(color=c)) +# ax2.boxplot(a_out2[:,1]*1000, showfliers=False, positions=[2.1],boxprops=dict(color=c),capprops=dict(color=c),whiskerprops=dict(color=c)) +# ax2.boxplot(a_out3[:,1]*1000, showfliers=False, positions=[3.1],boxprops=dict(color=c),capprops=dict(color=c),whiskerprops=dict(color=c)) +# ax2.set_xlim([0.5, 3.5]) +# ax2.set_ylim([0, 100]) + +# plt.xticks([1,2,3], ('Outdoor Scene', 'Indoor Scene 1', 'Indoor Scene 2')) +# # # print(time_se) +# # # print(a_out3[:,2]) +# plt.grid() +# plt.savefig("time.pdf", dpi=1200) +plt.show() diff --git a/src/FAST_LIO/PCD/1 b/src/FAST_LIO/PCD/1 new file mode 100644 index 0000000..d00491f --- /dev/null +++ b/src/FAST_LIO/PCD/1 @@ -0,0 +1 @@ +1 diff --git a/src/FAST_LIO/README.md b/src/FAST_LIO/README.md new file mode 100644 index 0000000..b4a360c --- /dev/null +++ b/src/FAST_LIO/README.md @@ -0,0 +1,259 @@ +## Related Works and Extended Application + +**SLAM:** + +1. [ikd-Tree](https://github.com/hku-mars/ikd-Tree): A state-of-art dynamic KD-Tree for 3D kNN search. +2. [R2LIVE](https://github.com/hku-mars/r2live): A high-precision LiDAR-inertial-Vision fusion work using FAST-LIO as LiDAR-inertial front-end. +3. [LI_Init](https://github.com/hku-mars/LiDAR_IMU_Init): A robust, real-time LiDAR-IMU extrinsic initialization and synchronization package.. +4. [FAST-LIO-LOCALIZATION](https://github.com/HViktorTsoi/FAST_LIO_LOCALIZATION): The integration of FAST-LIO with **Re-localization** function module. + +**Control and Plan:** + +1. [IKFOM](https://github.com/hku-mars/IKFoM): A Toolbox for fast and high-precision on-manifold Kalman filter. +2. [UAV Avoiding Dynamic Obstacles](https://github.com/hku-mars/dyn_small_obs_avoidance): One of the implementation of FAST-LIO in robot's planning. +3. [UGV Demo](https://www.youtube.com/watch?v=wikgrQbE6Cs): Model Predictive Control for Trajectory Tracking on Differentiable Manifolds. +4. [Bubble Planner](https://arxiv.org/abs/2202.12177): Planning High-speed Smooth Quadrotor Trajectories using Receding Corridors. + + + +## FAST-LIO +**FAST-LIO** (Fast LiDAR-Inertial Odometry) is a computationally efficient and robust LiDAR-inertial odometry package. It fuses LiDAR feature points with IMU data using a tightly-coupled iterated extended Kalman filter to allow robust navigation in fast-motion, noisy or cluttered environments where degeneration occurs. Our package address many key issues: +1. Fast iterated Kalman filter for odometry optimization; +2. Automaticaly initialized at most steady environments; +3. Parallel KD-Tree Search to decrease the computation; + +## FAST-LIO 2.0 (2021-07-05 Update) + + +
+ + +
+ +**Related video:** [FAST-LIO2](https://youtu.be/2OvjGnxszf8), [FAST-LIO1](https://youtu.be/iYCY6T79oNU) + +**Pipeline:** +
+ +
+ +**New Features:** +1. Incremental mapping using [ikd-Tree](https://github.com/hku-mars/ikd-Tree), achieve faster speed and over 100Hz LiDAR rate. +2. Direct odometry (scan to map) on Raw LiDAR points (feature extraction can be disabled), achieving better accuracy. +3. Since no requirements for feature extraction, FAST-LIO2 can support many types of LiDAR including spinning (Velodyne, Ouster) and solid-state (Livox Avia, Horizon, MID-70) LiDARs, and can be easily extended to support more LiDARs. +4. Support external IMU. +5. Support ARM-based platforms including Khadas VIM3, Nivida TX2, Raspberry Pi 4B(8G RAM). + +**Related papers**: + +[FAST-LIO2: Fast Direct LiDAR-inertial Odometry](doc/Fast_LIO_2.pdf) + +[FAST-LIO: A Fast, Robust LiDAR-inertial Odometry Package by Tightly-Coupled Iterated Kalman Filter](https://arxiv.org/abs/2010.08196) + +**Contributors** + +[Wei Xu 徐威](https://github.com/XW-HKU)，[Yixi Cai 蔡逸熙](https://github.com/Ecstasy-EC)，[Dongjiao He 贺东娇](https://github.com/Joanna-HE)，[Fangcheng Zhu 朱方程](https://github.com/zfc-zfc)，[Jiarong Lin 林家荣](https://github.com/ziv-lin)，[Zheng Liu 刘政](https://github.com/Zale-Liu), [Borong Yuan](https://github.com/borongyuan) + + + +## 1. Prerequisites +### 1.1 **Ubuntu** and **ROS** +**Ubuntu >= 16.04** + +For **Ubuntu 18.04 or higher**, the **default** PCL and Eigen is enough for FAST-LIO to work normally. + +ROS >= Melodic. [ROS Installation](http://wiki.ros.org/ROS/Installation) + +### 1.2. **PCL && Eigen** +PCL >= 1.8, Follow [PCL Installation](http://www.pointclouds.org/downloads/linux.html). + +Eigen >= 3.3.4, Follow [Eigen Installation](http://eigen.tuxfamily.org/index.php?title=Main_Page). + +### 1.3. **livox_ros_driver** +Follow [livox_ros_driver Installation](https://github.com/Livox-SDK/livox_ros_driver). + +*Remarks:* +- Since the FAST-LIO must support Livox serials LiDAR firstly, so the **livox_ros_driver** must be installed and **sourced** before run any FAST-LIO luanch file. +- How to source? The easiest way is add the line ``` source $Livox_ros_driver_dir$/devel/setup.bash ``` to the end of file ``` ~/.bashrc ```, where ``` $Livox_ros_driver_dir$ ``` is the directory of the livox ros driver workspace (should be the ``` ws_livox ``` directory if you completely followed the livox official document). + + +## 2. Build +If you want to use docker conatiner to run fastlio2, please install the docker on you machine. +Follow [Docker Installation](https://docs.docker.com/engine/install/ubuntu/). +### 2.1 Docker Container +User can create a new script with anyname by the following command in linux: +``` +touch .sh +``` +Place the following code inside the ``` .sh ``` script. +``` +#!/bin/bash +mkdir docker_ws +# Script to run ROS Kinetic with GUI support in Docker + +# Allow X server to be accessed from the local machine +xhost +local: + +# Container name +CONTAINER_NAME="fastlio2" + +# Run the Docker container +docker run -itd \ + --name=$CONTAINER_NAME \ + --user mars_ugv \ + --network host \ + --ipc=host \ + -v /home/$USER/docker_ws:/home/mars_ugv/docker_ws \ + --privileged \ + --env="QT_X11_NO_MITSHM=1" \ + --volume="/etc/localtime:/etc/localtime:ro" \ + -v /dev/bus/usb:/dev/bus/usb \ + --device=/dev/dri \ + --group-add video \ + -v /tmp/.X11-unix:/tmp/.X11-unix \ + --env="DISPLAY=$DISPLAY" \ + kenny0407/marslab_fastlio2:latest \ + /bin/bash +``` +execute the following command to grant execute permissions to the script, making it runnable: +``` +sudo chmod +x .sh +``` +execute the following command to download the image and create the container. +``` +./.sh +``` + +*Script explanation:* +- The docker run command provided below creates a container with a tag, using an image from Docker Hub. The download duration for this image can differ depending on the user's network speed. +- This command also establishes a new workspace called ``` docker_ws ```, which serves as a shared folder between the Docker container and the host machine. This means that if users wish to run the rosbag example, they need to download the rosbag file and place it in the ``` docker_ws ``` directory on their host machine. +- Subsequently, a folder with the same name inside the Docker container will receive this file. Users can then easily play the file within Docker. +- In this example, we've shared the network of the host machine with the Docker container. Consequently, if users execute the ``` rostopic list ``` command, they will observe identical output whether they run it on the host machine or inside the Docker container." +### 2.2 Build from source +Clone the repository and catkin_make: + +``` + cd ~/$A_ROS_DIR$/src + git clone https://github.com/hku-mars/FAST_LIO.git + cd FAST_LIO + git submodule update --init + cd ../.. + catkin_make + source devel/setup.bash +``` +- Remember to source the livox_ros_driver before build (follow 1.3 **livox_ros_driver**) +- If you want to use a custom build of PCL, add the following line to ~/.bashrc +```export PCL_ROOT={CUSTOM_PCL_PATH}``` +## 3. Directly run +Noted: + +A. Please make sure the IMU and LiDAR are **Synchronized**, that's important. + +B. The warning message "Failed to find match for field 'time'." means the timestamps of each LiDAR points are missed in the rosbag file. That is important for the forward propagation and backwark propagation. + +C. We recommend to set the **extrinsic_est_en** to false if the extrinsic is give. As for the extrinsic initiallization, please refer to our recent work: [**Robust Real-time LiDAR-inertial Initialization**](https://github.com/hku-mars/LiDAR_IMU_Init). + +### 3.1 For Avia +Connect to your PC to Livox Avia LiDAR by following [Livox-ros-driver installation](https://github.com/Livox-SDK/livox_ros_driver), then +``` + cd ~/$FAST_LIO_ROS_DIR$ + source devel/setup.bash + roslaunch fast_lio mapping_avia.launch + roslaunch livox_ros_driver livox_lidar_msg.launch +``` +- For livox serials, FAST-LIO only support the data collected by the ``` livox_lidar_msg.launch ``` since only its ``` livox_ros_driver/CustomMsg ``` data structure produces the timestamp of each LiDAR point which is very important for the motion undistortion. ``` livox_lidar.launch ``` can not produce it right now. +- If you want to change the frame rate, please modify the **publish_freq** parameter in the [livox_lidar_msg.launch](https://github.com/Livox-SDK/livox_ros_driver/blob/master/livox_ros_driver/launch/livox_lidar_msg.launch) of [Livox-ros-driver](https://github.com/Livox-SDK/livox_ros_driver) before make the livox_ros_driver pakage. + +### 3.2 For Livox serials with external IMU + +mapping_avia.launch theratically supports mid-70, mid-40 or other livox serial LiDAR, but need to setup some parameters befor run: + +Edit ``` config/avia.yaml ``` to set the below parameters: + +1. LiDAR point cloud topic name: ``` lid_topic ``` +2. IMU topic name: ``` imu_topic ``` +3. Translational extrinsic: ``` extrinsic_T ``` +4. Rotational extrinsic: ``` extrinsic_R ``` (only support rotation matrix) +- The extrinsic parameters in FAST-LIO is defined as the LiDAR's pose (position and rotation matrix) in IMU body frame (i.e. the IMU is the base frame). They can be found in the official manual. +- FAST-LIO produces a very simple software time sync for livox LiDAR, set parameter ```time_sync_en``` to ture to turn on. But turn on **ONLY IF external time synchronization is really not possible**, since the software time sync cannot make sure accuracy. + +### 3.3 For Velodyne or Ouster (Velodyne as an example) + +Step A: Setup before run + +Edit ``` config/velodyne.yaml ``` to set the below parameters: + +1. LiDAR point cloud topic name: ``` lid_topic ``` +2. IMU topic name: ``` imu_topic ``` (both internal and external, 6-aixes or 9-axies are fine) +3. Set the parameter ```timestamp_unit``` based on the unit of **time** (Velodyne) or **t** (Ouster) field in PoindCloud2 rostopic +4. Line number (we tested 16, 32 and 64 line, but not tested 128 or above): ``` scan_line ``` +5. Translational extrinsic: ``` extrinsic_T ``` +6. Rotational extrinsic: ``` extrinsic_R ``` (only support rotation matrix) +- The extrinsic parameters in FAST-LIO is defined as the LiDAR's pose (position and rotation matrix) in IMU body frame (i.e. the IMU is the base frame). + +Step B: Run below +``` + cd ~/$FAST_LIO_ROS_DIR$ + source devel/setup.bash + roslaunch fast_lio mapping_velodyne.launch +``` + +Step C: Run LiDAR's ros driver or play rosbag. + +### 3.4 PCD file save + +Set ``` pcd_save_enable ``` in launchfile to ``` 1 ```. All the scans (in global frame) will be accumulated and saved to the file ``` FAST_LIO/PCD/scans.pcd ``` after the FAST-LIO is terminated. ```pcl_viewer scans.pcd``` can visualize the point clouds. + +*Tips for pcl_viewer:* +- change what to visualize/color by pressing keyboard 1,2,3,4,5 when pcl_viewer is running. +``` + 1 is all random + 2 is X values + 3 is Y values + 4 is Z values + 5 is intensity +``` + +## 4. Rosbag Example +### 4.1 Livox Avia Rosbag +
+ + + +Files: Can be downloaded from [google drive](https://drive.google.com/drive/folders/1CGYEJ9-wWjr8INyan6q1BZz_5VtGB-fP?usp=sharing) + +Run: +``` +roslaunch fast_lio mapping_avia.launch +rosbag play YOUR_DOWNLOADED.bag + +``` + +### 4.2 Velodyne HDL-32E Rosbag + +**NCLT Dataset**: Original bin file can be found [here](http://robots.engin.umich.edu/nclt/). + +We produce [Rosbag Files](https://drive.google.com/drive/folders/1blQJuAB4S80NwZmpM6oALyHWvBljPSOE?usp=sharing) and [a python script](https://drive.google.com/file/d/1QC9IRBv2_-cgo_AEvL62E1ml1IL9ht6J/view?usp=sharing) to generate Rosbag files: ```python3 sensordata_to_rosbag_fastlio.py bin_file_dir bag_name.bag``` + +Run: +``` +roslaunch fast_lio mapping_velodyne.launch +rosbag play YOUR_DOWNLOADED.bag +``` + +## 5.Implementation on UAV +In order to validate the robustness and computational efficiency of FAST-LIO in actual mobile robots, we build a small-scale quadrotor which can carry a Livox Avia LiDAR with 70 degree FoV and a DJI Manifold 2-C onboard computer with a 1.8 GHz Intel i7-8550U CPU and 8 G RAM, as shown in below. + +The main structure of this UAV is 3d printed (Aluminum or PLA), the .stl file will be open-sourced in the future. + +
+ + +
+ +## 6.Acknowledgments + +Thanks for LOAM(J. Zhang and S. Singh. LOAM: Lidar Odometry and Mapping in Real-time), [Livox_Mapping](https://github.com/Livox-SDK/livox_mapping), [LINS](https://github.com/ChaoqinRobotics/LINS---LiDAR-inertial-SLAM) and [Loam_Livox](https://github.com/hku-mars/loam_livox). diff --git a/src/FAST_LIO/config/avia.yaml b/src/FAST_LIO/config/avia.yaml new file mode 100644 index 0000000..adcc03b --- /dev/null +++ b/src/FAST_LIO/config/avia.yaml @@ -0,0 +1,35 @@ +common: + lid_topic: "/livox/lidar" + imu_topic: "/livox/imu" + time_sync_en: false # ONLY turn on when external time synchronization is really not possible + time_offset_lidar_to_imu: 0.0 # Time offset between lidar and IMU calibrated by other algorithms, e.g. LI-Init (can be found in README). + # This param will take effect no matter what time_sync_en is. So if the time offset is not known exactly, please set as 0.0 + +preprocess: + lidar_type: 1 # 1 for Livox serials LiDAR, 2 for Velodyne LiDAR, 3 for ouster LiDAR, + scan_line: 6 + blind: 4 + +mapping: + acc_cov: 0.1 + gyr_cov: 0.1 + b_acc_cov: 0.0001 + b_gyr_cov: 0.0001 + fov_degree: 90 + det_range: 450.0 + extrinsic_est_en: false # true: enable the online estimation of IMU-LiDAR extrinsic + extrinsic_T: [ 0.04165, 0.02326, -0.0284 ] + extrinsic_R: [ 1, 0, 0, + 0, 1, 0, + 0, 0, 1] + +publish: + path_en: false + scan_publish_en: true # false: close all the point cloud output + dense_publish_en: true # false: low down the points number in a global-frame point clouds scan. + scan_bodyframe_pub_en: true # true: output the point cloud scans in IMU-body-frame + +pcd_save: + pcd_save_en: true + interval: -1 # how many LiDAR frames saved in each pcd file; + # -1 : all frames will be saved in ONE pcd file, may lead to memory crash when having too much frames. diff --git a/src/FAST_LIO/config/horizon.yaml b/src/FAST_LIO/config/horizon.yaml new file mode 100644 index 0000000..1c489c5 --- /dev/null +++ b/src/FAST_LIO/config/horizon.yaml @@ -0,0 +1,35 @@ +common: + lid_topic: "/livox/lidar" + imu_topic: "/livox/imu" + time_sync_en: false # ONLY turn on when external time synchronization is really not possible + time_offset_lidar_to_imu: 0.0 # Time offset between lidar and IMU calibrated by other algorithms, e.g. LI-Init (can be found in README). + # This param will take effect no matter what time_sync_en is. So if the time offset is not known exactly, please set as 0.0 + +preprocess: + lidar_type: 1 # 1 for Livox serials LiDAR, 2 for Velodyne LiDAR, 3 for ouster LiDAR, + scan_line: 6 + blind: 4 + +mapping: + acc_cov: 0.1 + gyr_cov: 0.1 + b_acc_cov: 0.0001 + b_gyr_cov: 0.0001 + fov_degree: 100 + det_range: 260.0 + extrinsic_est_en: true # true: enable the online estimation of IMU-LiDAR extrinsic + extrinsic_T: [ 0.05512, 0.02226, -0.0297 ] + extrinsic_R: [ 1, 0, 0, + 0, 1, 0, + 0, 0, 1] + +publish: + path_en: false + scan_publish_en: true # false: close all the point cloud output + dense_publish_en: true # false: low down the points number in a global-frame point clouds scan. + scan_bodyframe_pub_en: true # true: output the point cloud scans in IMU-body-frame + +pcd_save: + pcd_save_en: true + interval: -1 # how many LiDAR frames saved in each pcd file; + # -1 : all frames will be saved in ONE pcd file, may lead to memory crash when having too much frames. diff --git a/src/FAST_LIO/config/mid360.yaml b/src/FAST_LIO/config/mid360.yaml new file mode 100644 index 0000000..debe7cb --- /dev/null +++ b/src/FAST_LIO/config/mid360.yaml @@ -0,0 +1,35 @@ +common: + lid_topic: "/livox/lidar" + imu_topic: "/livox/imu" + time_sync_en: false # ONLY turn on when external time synchronization is really not possible + time_offset_lidar_to_imu: 0.0 # Time offset between lidar and IMU calibrated by other algorithms, e.g. LI-Init (can be found in README). + # This param will take effect no matter what time_sync_en is. So if the time offset is not known exactly, please set as 0.0 + +preprocess: + lidar_type: 1 # 1 for Livox serials LiDAR, 2 for Velodyne LiDAR, 3 for ouster LiDAR, + scan_line: 4 + blind: 0.3 + +mapping: + acc_cov: 0.1 + gyr_cov: 0.1 + b_acc_cov: 0.0001 + b_gyr_cov: 0.0001 + fov_degree: 360 + det_range: 100.0 + extrinsic_est_en: false # true: enable the online estimation of IMU-LiDAR extrinsic + extrinsic_T: [ -0.011, -0.02329, 0.04412 ] + extrinsic_R: [ 1, 0, 0, + 0, 1, 0, + 0, 0, 1] + +publish: + path_en: false + scan_publish_en: true # false: close all the point cloud output + dense_publish_en: true # false: low down the points number in a global-frame point clouds scan. + scan_bodyframe_pub_en: true # true: output the point cloud scans in IMU-body-frame + +pcd_save: + pcd_save_en: true + interval: -1 # how many LiDAR frames saved in each pcd file; + # -1 : all frames will be saved in ONE pcd file, may lead to memory crash when having too much frames. diff --git a/src/FAST_LIO/config/mid360_down.yaml b/src/FAST_LIO/config/mid360_down.yaml new file mode 100644 index 0000000..1d9c228 --- /dev/null +++ b/src/FAST_LIO/config/mid360_down.yaml @@ -0,0 +1,35 @@ +common: + lid_topic: "/livox/lidar" + imu_topic: "/livox/imu" + time_sync_en: false # ONLY turn on when external time synchronization is really not possible + time_offset_lidar_to_imu: 0.0 # Time offset between lidar and IMU calibrated by other algorithms, e.g. LI-Init (can be found in README). + # This param will take effect no matter what time_sync_en is. So if the time offset is not known exactly, please set as 0.0 + +preprocess: + lidar_type: 1 # 1 for Livox serials LiDAR, 2 for Velodyne LiDAR, 3 for ouster LiDAR, + scan_line: 4 + blind: 0.1 + +mapping: + acc_cov: 0.1 + gyr_cov: 0.1 + b_acc_cov: 0.0001 + b_gyr_cov: 0.0001 + fov_degree: 360 + det_range: 100.0 + extrinsic_est_en: true # true: enable the online estimation of IMU-LiDAR extrinsic + extrinsic_T: [ -0.011, -0.02329, 0.04412 ] + extrinsic_R: [ 1, 0, 0, + 0, -1, 0, + 0, 0, -1] + +publish: + path_en: false + scan_publish_en: true # false: close all the point cloud output + dense_publish_en: true # false: low down the points number in a global-frame point clouds scan. + scan_bodyframe_pub_en: true # true: output the point cloud scans in IMU-body-frame + +pcd_save: + pcd_save_en: true + interval: -1 # how many LiDAR frames saved in each pcd file; + # -1 : all frames will be saved in ONE pcd file, may lead to memory crash when having too much frames. diff --git a/src/FAST_LIO/config/ouster64.yaml b/src/FAST_LIO/config/ouster64.yaml new file mode 100644 index 0000000..11d16ae --- /dev/null +++ b/src/FAST_LIO/config/ouster64.yaml @@ -0,0 +1,36 @@ +common: + lid_topic: "/os_cloud_node/points" + imu_topic: "/os_cloud_node/imu" + time_sync_en: false # ONLY turn on when external time synchronization is really not possible + time_offset_lidar_to_imu: 0.0 # Time offset between lidar and IMU calibrated by other algorithms, e.g. LI-Init (can be found in README). + # This param will take effect no matter what time_sync_en is. So if the time offset is not known exactly, please set as 0.0 + +preprocess: + lidar_type: 3 # 1 for Livox serials LiDAR, 2 for Velodyne LiDAR, 3 for ouster LiDAR, + scan_line: 64 + timestamp_unit: 3 # 0-second, 1-milisecond, 2-microsecond, 3-nanosecond. + blind: 4 + +mapping: + acc_cov: 0.1 + gyr_cov: 0.1 + b_acc_cov: 0.0001 + b_gyr_cov: 0.0001 + fov_degree: 180 + det_range: 150.0 + extrinsic_est_en: false # true: enable the online estimation of IMU-LiDAR extrinsic + extrinsic_T: [ 0.0, 0.0, 0.0 ] + extrinsic_R: [1, 0, 0, + 0, 1, 0, + 0, 0, 1] + +publish: + path_en: false + scan_publish_en: true # false: close all the point cloud output + dense_publish_en: true # false: low down the points number in a global-frame point clouds scan. + scan_bodyframe_pub_en: true # true: output the point cloud scans in IMU-body-frame + +pcd_save: + pcd_save_en: true + interval: -1 # how many LiDAR frames saved in each pcd file; + # -1 : all frames will be saved in ONE pcd file, may lead to memory crash when having too much frames. diff --git a/src/FAST_LIO/config/velodyne.yaml b/src/FAST_LIO/config/velodyne.yaml new file mode 100644 index 0000000..453777b --- /dev/null +++ b/src/FAST_LIO/config/velodyne.yaml @@ -0,0 +1,37 @@ +common: + lid_topic: "/velodyne_points" + imu_topic: "/imu/data" + time_sync_en: false # ONLY turn on when external time synchronization is really not possible + time_offset_lidar_to_imu: 0.0 # Time offset between lidar and IMU calibrated by other algorithms, e.g. LI-Init (can be found in README). + # This param will take effect no matter what time_sync_en is. So if the time offset is not known exactly, please set as 0.0 + +preprocess: + lidar_type: 2 # 1 for Livox serials LiDAR, 2 for Velodyne LiDAR, 3 for ouster LiDAR, + scan_line: 32 + scan_rate: 10 # only need to be set for velodyne, unit: Hz, + timestamp_unit: 2 # the unit of time/t field in the PointCloud2 rostopic: 0-second, 1-milisecond, 2-microsecond, 3-nanosecond. + blind: 2 + +mapping: + acc_cov: 0.1 + gyr_cov: 0.1 + b_acc_cov: 0.0001 + b_gyr_cov: 0.0001 + fov_degree: 180 + det_range: 100.0 + extrinsic_est_en: false # true: enable the online estimation of IMU-LiDAR extrinsic, + extrinsic_T: [ 0, 0, 0.28] + extrinsic_R: [ 1, 0, 0, + 0, 1, 0, + 0, 0, 1] + +publish: + path_en: false + scan_publish_en: true # false: close all the point cloud output + dense_publish_en: true # false: low down the points number in a global-frame point clouds scan. + scan_bodyframe_pub_en: true # true: output the point cloud scans in IMU-body-frame + +pcd_save: + pcd_save_en: true + interval: -1 # how many LiDAR frames saved in each pcd file; + # -1 : all frames will be saved in ONE pcd file, may lead to memory crash when having too much frames. diff --git a/src/FAST_LIO/include/Exp_mat.h b/src/FAST_LIO/include/Exp_mat.h new file mode 100644 index 0000000..7ab2897 --- /dev/null +++ b/src/FAST_LIO/include/Exp_mat.h @@ -0,0 +1,103 @@ +#ifndef EXP_MAT_H +#define EXP_MAT_H + +#include +#include +#include +// #include + +#define SKEW_SYM_MATRX(v) 0.0,-v[2],v[1],v[2],0.0,-v[0],-v[1],v[0],0.0 + +template +Eigen::Matrix Exp(const Eigen::Matrix &&ang) +{ + T ang_norm = ang.norm(); + Eigen::Matrix Eye3 = Eigen::Matrix::Identity(); + if (ang_norm > 0.0000001) + { + Eigen::Matrix r_axis = ang / ang_norm; + Eigen::Matrix K; + K << SKEW_SYM_MATRX(r_axis); + /// Roderigous Tranformation + return Eye3 + std::sin(ang_norm) * K + (1.0 - std::cos(ang_norm)) * K * K; + } + else + { + return Eye3; + } +} + +template +Eigen::Matrix Exp(const Eigen::Matrix &ang_vel, const Ts &dt) +{ + T ang_vel_norm = ang_vel.norm(); + Eigen::Matrix Eye3 = Eigen::Matrix::Identity(); + + if (ang_vel_norm > 0.0000001) + { + Eigen::Matrix r_axis = ang_vel / ang_vel_norm; + Eigen::Matrix K; + + K << SKEW_SYM_MATRX(r_axis); + + T r_ang = ang_vel_norm * dt; + + /// Roderigous Tranformation + return Eye3 + std::sin(r_ang) * K + (1.0 - std::cos(r_ang)) * K * K; + } + else + { + return Eye3; + } +} + +template +Eigen::Matrix Exp(const T &v1, const T &v2, const T &v3) +{ + T &&norm = sqrt(v1 * v1 + v2 * v2 + v3 * v3); + Eigen::Matrix Eye3 = Eigen::Matrix::Identity(); + if (norm > 0.00001) + { + T r_ang[3] = {v1 / norm, v2 / norm, v3 / norm}; + Eigen::Matrix K; + K << SKEW_SYM_MATRX(r_ang); + + /// Roderigous Tranformation + return Eye3 + std::sin(norm) * K + (1.0 - std::cos(norm)) * K * K; + } + else + { + return Eye3; + } +} + +/* Logrithm of a Rotation Matrix */ +template +Eigen::Matrix Log(const Eigen::Matrix &R) +{ + T &&theta = std::acos(0.5 * (R.trace() - 1)); + Eigen::Matrix K(R(2,1) - R(1,2), R(0,2) - R(2,0), R(1,0) - R(0,1)); + return (std::abs(theta) < 0.001) ? (0.5 * K) : (0.5 * theta / std::sin(theta) * K); +} + +// template +// cv::Mat Exp(const T &v1, const T &v2, const T &v3) +// { + +// T norm = sqrt(v1 * v1 + v2 * v2 + v3 * v3); +// cv::Mat Eye3 = cv::Mat::eye(3, 3, CV_32F); +// if (norm > 0.0000001) +// { +// T r_ang[3] = {v1 / norm, v2 / norm, v3 / norm}; +// cv::Mat K = (cv::Mat_(3,3) << SKEW_SYM_MATRX(r_ang)); + +// /// Roderigous Tranformation +// return Eye3 + std::sin(norm) * K + (1.0 - std::cos(norm)) * K * K; +// } +// else +// { +// return Eye3; +// } +// } + +#endif diff --git a/src/FAST_LIO/include/IKFoM_toolkit/esekfom/esekfom.hpp b/src/FAST_LIO/include/IKFoM_toolkit/esekfom/esekfom.hpp new file mode 100755 index 0000000..7547584 --- /dev/null +++ b/src/FAST_LIO/include/IKFoM_toolkit/esekfom/esekfom.hpp @@ -0,0 +1,2008 @@ +/* + * Copyright (c) 2019--2023, The University of Hong Kong + * All rights reserved. + * + * Author: Dongjiao HE + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the Universitaet Bremen nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#ifndef ESEKFOM_EKF_HPP +#define ESEKFOM_EKF_HPP + + +#include +#include + +#include +#include +#include +#include +#include + +#include "../mtk/types/vect.hpp" +#include "../mtk/types/SOn.hpp" +#include "../mtk/types/S2.hpp" +#include "../mtk/startIdx.hpp" +#include "../mtk/build_manifold.hpp" +#include "util.hpp" + +//#define USE_sparse + + +namespace esekfom { + +using namespace Eigen; + +//used for iterated error state EKF update +//for the aim to calculate measurement (z), estimate measurement (h), partial differention matrices (h_x, h_v) and the noise covariance (R) at the same time, by only one function. +//applied for measurement as a manifold. +template +struct share_datastruct +{ + bool valid; + bool converge; + M z; + Eigen::Matrix h_v; + Eigen::Matrix h_x; + Eigen::Matrix R; +}; + +//used for iterated error state EKF update +//for the aim to calculate measurement (z), estimate measurement (h), partial differention matrices (h_x, h_v) and the noise covariance (R) at the same time, by only one function. +//applied for measurement as an Eigen matrix whose dimension is changing +template +struct dyn_share_datastruct +{ + bool valid; + bool converge; + Eigen::Matrix z; + Eigen::Matrix h; + Eigen::Matrix h_v; + Eigen::Matrix h_x; + Eigen::Matrix R; +}; + +//used for iterated error state EKF update +//for the aim to calculate measurement (z), estimate measurement (h), partial differention matrices (h_x, h_v) and the noise covariance (R) at the same time, by only one function. +//applied for measurement as a dynamic manifold whose dimension or type is changing +template +struct dyn_runtime_share_datastruct +{ + bool valid; + bool converge; + //Z z; + Eigen::Matrix h_v; + Eigen::Matrix h_x; + Eigen::Matrix R; +}; + +template +class esekf{ + + typedef esekf self; + enum{ + n = state::DOF, m = state::DIM, l = measurement::DOF + }; + +public: + + typedef typename state::scalar scalar_type; + typedef Matrix cov; + typedef Matrix cov_; + typedef SparseMatrix spMt; + typedef Matrix vectorized_state; + typedef Matrix flatted_state; + typedef flatted_state processModel(state &, const input &); + typedef Eigen::Matrix processMatrix1(state &, const input &); + typedef Eigen::Matrix processMatrix2(state &, const input &); + typedef Eigen::Matrix processnoisecovariance; + typedef measurement measurementModel(state &, bool &); + typedef measurement measurementModel_share(state &, share_datastruct &); + typedef Eigen::Matrix measurementModel_dyn(state &, bool &); + //typedef Eigen::Matrix measurementModel_dyn_share(state &, dyn_share_datastruct &); + typedef void measurementModel_dyn_share(state &, dyn_share_datastruct &); + typedef Eigen::Matrix measurementMatrix1(state &, bool&); + typedef Eigen::Matrix measurementMatrix1_dyn(state &, bool&); + typedef Eigen::Matrix measurementMatrix2(state &, bool&); + typedef Eigen::Matrix measurementMatrix2_dyn(state &, bool&); + typedef Eigen::Matrix measurementnoisecovariance; + typedef Eigen::Matrix measurementnoisecovariance_dyn; + + esekf(const state &x = state(), + const cov &P = cov::Identity()): x_(x), P_(P){ + #ifdef USE_sparse + SparseMatrix ref(n, n); + ref.setIdentity(); + l_ = ref; + f_x_2 = ref; + f_x_1 = ref; + #endif + }; + + //receive system-specific models and their differentions. + //for measurement as a manifold. + void init(processModel f_in, processMatrix1 f_x_in, processMatrix2 f_w_in, measurementModel h_in, measurementMatrix1 h_x_in, measurementMatrix2 h_v_in, int maximum_iteration, scalar_type limit_vector[n]) + { + f = f_in; + f_x = f_x_in; + f_w = f_w_in; + h = h_in; + h_x = h_x_in; + h_v = h_v_in; + + maximum_iter = maximum_iteration; + for(int i=0; i f_w_ = f_w(x_, i_in); + Matrix f_w_final; + state x_before = x_; + x_.oplus(f_, dt); + + F_x1 = cov::Identity(); + for (std::vector, int> >::iterator it = x_.vect_state.begin(); it != x_.vect_state.end(); it++) { + int idx = (*it).first.first; + int dim = (*it).first.second; + int dof = (*it).second; + for(int i = 0; i < n; i++){ + for(int j=0; j res_temp_SO3; + MTK::vect<3, scalar_type> seg_SO3; + for (std::vector >::iterator it = x_.SO3_state.begin(); it != x_.SO3_state.end(); it++) { + int idx = (*it).first; + int dim = (*it).second; + for(int i = 0; i < 3; i++){ + seg_SO3(i) = -1 * f_(dim + i) * dt; + } + MTK::SO3 res; + res.w() = MTK::exp(res.vec(), seg_SO3, scalar_type(1/2)); + #ifdef USE_sparse + res_temp_SO3 = res.toRotationMatrix(); + for(int i = 0; i < 3; i++){ + for(int j = 0; j < 3; j++){ + f_x_1.coeffRef(idx + i, idx + j) = res_temp_SO3(i, j); + } + } + #else + F_x1.template block<3, 3>(idx, idx) = res.toRotationMatrix(); + #endif + res_temp_SO3 = MTK::A_matrix(seg_SO3); + for(int i = 0; i < n; i++){ + f_x_final. template block<3, 1>(idx, i) = res_temp_SO3 * (f_x_. template block<3, 1>(dim, i)); + } + for(int i = 0; i < process_noise_dof; i++){ + f_w_final. template block<3, 1>(idx, i) = res_temp_SO3 * (f_w_. template block<3, 1>(dim, i)); + } + } + + + Matrix res_temp_S2; + Matrix res_temp_S2_; + MTK::vect<3, scalar_type> seg_S2; + for (std::vector >::iterator it = x_.S2_state.begin(); it != x_.S2_state.end(); it++) { + int idx = (*it).first; + int dim = (*it).second; + for(int i = 0; i < 3; i++){ + seg_S2(i) = f_(dim + i) * dt; + } + MTK::vect<2, scalar_type> vec = MTK::vect<2, scalar_type>::Zero(); + MTK::SO3 res; + res.w() = MTK::exp(res.vec(), seg_S2, scalar_type(1/2)); + Eigen::Matrix Nx; + Eigen::Matrix Mx; + x_.S2_Nx_yy(Nx, idx); + x_before.S2_Mx(Mx, vec, idx); + #ifdef USE_sparse + res_temp_S2_ = Nx * res.toRotationMatrix() * Mx; + for(int i = 0; i < 2; i++){ + for(int j = 0; j < 2; j++){ + f_x_1.coeffRef(idx + i, idx + j) = res_temp_S2_(i, j); + } + } + #else + F_x1.template block<2, 2>(idx, idx) = Nx * res.toRotationMatrix() * Mx; + #endif + + Eigen::Matrix x_before_hat; + x_before.S2_hat(x_before_hat, idx); + res_temp_S2 = -Nx * res.toRotationMatrix() * x_before_hat*MTK::A_matrix(seg_S2).transpose(); + + for(int i = 0; i < n; i++){ + f_x_final. template block<2, 1>(idx, i) = res_temp_S2 * (f_x_. template block<3, 1>(dim, i)); + + } + for(int i = 0; i < process_noise_dof; i++){ + f_w_final. template block<2, 1>(idx, i) = res_temp_S2 * (f_w_. template block<3, 1>(dim, i)); + } + } + + #ifdef USE_sparse + f_x_1.makeCompressed(); + spMt f_x2 = f_x_final.sparseView(); + spMt f_w1 = f_w_final.sparseView(); + spMt xp = f_x_1 + f_x2 * dt; + P_ = xp * P_ * xp.transpose() + (f_w1 * dt) * Q * (f_w1 * dt).transpose(); + #else + F_x1 += f_x_final * dt; + P_ = (F_x1) * P_ * (F_x1).transpose() + (dt * f_w_final) * Q * (dt * f_w_final).transpose(); + #endif + } + + //iterated error state EKF update for measurement as a manifold. + void update_iterated(measurement& z, measurementnoisecovariance &R) { + + if(!(is_same())){ + std::cerr << "the scalar type of measurment must be the same as the state" << std::endl; + std::exit(100); + } + int t = 0; + bool converg = true; + bool valid = true; + state x_propagated = x_; + cov P_propagated = P_; + + for(int i=-1; i h_x_ = h_x(x_, valid); + Matrix h_v_ = h_v(x_, valid); + #endif + if(! valid) + { + continue; + } + + P_ = P_propagated; + + Matrix res_temp_SO3; + MTK::vect<3, scalar_type> seg_SO3; + for (std::vector >::iterator it = x_.SO3_state.begin(); it != x_.SO3_state.end(); it++) { + int idx = (*it).first; + int dim = (*it).second; + for(int i = 0; i < 3; i++){ + seg_SO3(i) = dx(idx+i); + } + + res_temp_SO3 = A_matrix(seg_SO3).transpose(); + dx_new.template block<3, 1>(idx, 0) = res_temp_SO3 * dx.template block<3, 1>(idx, 0); + for(int i = 0; i < n; i++){ + P_. template block<3, 1>(idx, i) = res_temp_SO3 * (P_. template block<3, 1>(idx, i)); + } + for(int i = 0; i < n; i++){ + P_. template block<1, 3>(i, idx) =(P_. template block<1, 3>(i, idx)) * res_temp_SO3.transpose(); + } + } + + + Matrix res_temp_S2; + MTK::vect<2, scalar_type> seg_S2; + for (std::vector >::iterator it = x_.S2_state.begin(); it != x_.S2_state.end(); it++) { + int idx = (*it).first; + int dim = (*it).second; + for(int i = 0; i < 2; i++){ + seg_S2(i) = dx(idx + i); + } + + Eigen::Matrix Nx; + Eigen::Matrix Mx; + x_.S2_Nx_yy(Nx, idx); + x_propagated.S2_Mx(Mx, seg_S2, idx); + res_temp_S2 = Nx * Mx; + dx_new.template block<2, 1>(idx, 0) = res_temp_S2 * dx.template block<2, 1>(idx, 0); + for(int i = 0; i < n; i++){ + P_. template block<2, 1>(idx, i) = res_temp_S2 * (P_. template block<2, 1>(idx, i)); + } + for(int i = 0; i < n; i++){ + P_. template block<1, 2>(i, idx) = (P_. template block<1, 2>(i, idx)) * res_temp_S2.transpose(); + } + } + + Matrix K_; + if(n > l) + { + #ifdef USE_sparse + Matrix K_temp = h_x_ * P_ * h_x_.transpose(); + spMt R_temp = h_v_ * R_ * h_v_.transpose(); + K_temp += R_temp; + K_ = P_ * h_x_.transpose() * K_temp.inverse(); + #else + K_= P_ * h_x_.transpose() * (h_x_ * P_ * h_x_.transpose() + h_v_ * R * h_v_.transpose()).inverse(); + #endif + } + else + { + #ifdef USE_sparse + measurementnoisecovariance b = measurementnoisecovariance::Identity(); + Eigen::SparseQR, Eigen::COLAMDOrdering> solver; + solver.compute(R_); + measurementnoisecovariance R_in_temp = solver.solve(b); + spMt R_in = R_in_temp.sparseView(); + spMt K_temp = h_x_.transpose() * R_in * h_x_; + cov P_temp = P_.inverse(); + P_temp += K_temp; + K_ = P_temp.inverse() * h_x_.transpose() * R_in; + #else + measurementnoisecovariance R_in = (h_v_*R*h_v_.transpose()).inverse(); + K_ = (h_x_.transpose() * R_in * h_x_ + P_.inverse()).inverse() * h_x_.transpose() * R_in; + #endif + } + Matrix innovation; + z.boxminus(innovation, h(x_, valid)); + cov K_x = K_ * h_x_; + Matrix dx_ = K_ * innovation + (K_x - Matrix::Identity()) * dx_new; + state x_before = x_; + x_.boxplus(dx_); + + converg = true; + for(int i = 0; i < n ; i++) + { + if(std::fabs(dx_[i]) > limit[i]) + { + converg = false; + break; + } + } + + if(converg) t++; + + if(t > 1 || i == maximum_iter - 1) + { + L_ = P_; + + Matrix res_temp_SO3; + MTK::vect<3, scalar_type> seg_SO3; + for(typename std::vector >::iterator it = x_.SO3_state.begin(); it != x_.SO3_state.end(); it++) { + int idx = (*it).first; + for(int i = 0; i < 3; i++){ + seg_SO3(i) = dx_(i + idx); + } + res_temp_SO3 = A_matrix(seg_SO3).transpose(); + for(int i = 0; i < n; i++){ + L_. template block<3, 1>(idx, i) = res_temp_SO3 * (P_. template block<3, 1>(idx, i)); + } + if(n > l) + { + for(int i = 0; i < l; i++){ + K_. template block<3, 1>(idx, i) = res_temp_SO3 * (K_. template block<3, 1>(idx, i)); + } + } + else + { + for(int i = 0; i < n; i++){ + K_x. template block<3, 1>(idx, i) = res_temp_SO3 * (K_x. template block<3, 1>(idx, i)); + } + } + for(int i = 0; i < n; i++){ + L_. template block<1, 3>(i, idx) = (L_. template block<1, 3>(i, idx)) * res_temp_SO3.transpose(); + P_. template block<1, 3>(i, idx) = (P_. template block<1, 3>(i, idx)) * res_temp_SO3.transpose(); + } + } + + Matrix res_temp_S2; + MTK::vect<2, scalar_type> seg_S2; + for(typename std::vector >::iterator it = x_.S2_state.begin(); it != x_.S2_state.end(); it++) { + int idx = (*it).first; + + for(int i = 0; i < 2; i++){ + seg_S2(i) = dx_(i + idx); + } + + Eigen::Matrix Nx; + Eigen::Matrix Mx; + x_.S2_Nx_yy(Nx, idx); + x_propagated.S2_Mx(Mx, seg_S2, idx); + res_temp_S2 = Nx * Mx; + + for(int i = 0; i < n; i++){ + L_. template block<2, 1>(idx, i) = res_temp_S2 * (P_. template block<2, 1>(idx, i)); + } + if(n > l) + { + for(int i = 0; i < l; i++){ + K_. template block<2, 1>(idx, i) = res_temp_S2 * (K_. template block<2, 1>(idx, i)); + } + } + else + { + for(int i = 0; i < n; i++){ + K_x. template block<2, 1>(idx, i) = res_temp_S2 * (K_x. template block<2, 1>(idx, i)); + } + } + for(int i = 0; i < n; i++){ + L_. template block<1, 2>(i, idx) = (L_. template block<1, 2>(i, idx)) * res_temp_S2.transpose(); + P_. template block<1, 2>(i, idx) = (P_. template block<1, 2>(i, idx)) * res_temp_S2.transpose(); + } + } + if(n > l) + { + P_ = L_ - K_ * h_x_ * P_; + } + else + { + P_ = L_ - K_x * P_; + } + return; + } + } + } + + //iterated error state EKF update for measurement as a manifold. + //calculate measurement (z), estimate measurement (h), partial differention matrices (h_x, h_v) and the noise covariance (R) at the same time, by only one function. + void update_iterated_share() { + + if(!(is_same())){ + std::cerr << "the scalar type of measurment must be the same as the state" << std::endl; + std::exit(100); + } + + int t = 0; + share_datastruct _share; + _share.valid = true; + _share.converge = true; + state x_propagated = x_; + cov P_propagated = P_; + + for(int i=-1; i h_x_ = _share.h_x; + Matrix h_v_ = _share.h_v; + #endif + if(! _share.valid) + { + continue; + } + + P_ = P_propagated; + + Matrix res_temp_SO3; + MTK::vect<3, scalar_type> seg_SO3; + for (std::vector >::iterator it = x_.SO3_state.begin(); it != x_.SO3_state.end(); it++) { + int idx = (*it).first; + int dim = (*it).second; + for(int i = 0; i < 3; i++){ + seg_SO3(i) = dx(idx+i); + } + + res_temp_SO3 = A_matrix(seg_SO3).transpose(); + dx_new.template block<3, 1>(idx, 0) = res_temp_SO3 * dx.template block<3, 1>(idx, 0); + for(int i = 0; i < n; i++){ + P_. template block<3, 1>(idx, i) = res_temp_SO3 * (P_. template block<3, 1>(idx, i)); + } + for(int i = 0; i < n; i++){ + P_. template block<1, 3>(i, idx) =(P_. template block<1, 3>(i, idx)) * res_temp_SO3.transpose(); + } + } + + + Matrix res_temp_S2; + MTK::vect<2, scalar_type> seg_S2; + for (std::vector >::iterator it = x_.S2_state.begin(); it != x_.S2_state.end(); it++) { + int idx = (*it).first; + int dim = (*it).second; + for(int i = 0; i < 2; i++){ + seg_S2(i) = dx(idx + i); + } + + Eigen::Matrix Nx; + Eigen::Matrix Mx; + x_.S2_Nx_yy(Nx, idx); + x_propagated.S2_Mx(Mx, seg_S2, idx); + res_temp_S2 = Nx * Mx; + dx_new.template block<2, 1>(idx, 0) = res_temp_S2 * dx.template block<2, 1>(idx, 0); + for(int i = 0; i < n; i++){ + P_. template block<2, 1>(idx, i) = res_temp_S2 * (P_. template block<2, 1>(idx, i)); + } + for(int i = 0; i < n; i++){ + P_. template block<1, 2>(i, idx) = (P_. template block<1, 2>(i, idx)) * res_temp_S2.transpose(); + } + } + + Matrix K_; + if(n > l) + { + #ifdef USE_sparse + Matrix K_temp = h_x_ * P_ * h_x_.transpose(); + spMt R_temp = h_v_ * R_ * h_v_.transpose(); + K_temp += R_temp; + K_ = P_ * h_x_.transpose() * K_temp.inverse(); + #else + K_= P_ * h_x_.transpose() * (h_x_ * P_ * h_x_.transpose() + h_v_ * R * h_v_.transpose()).inverse(); + #endif + } + else + { + #ifdef USE_sparse + measurementnoisecovariance b = measurementnoisecovariance::Identity(); + Eigen::SparseQR, Eigen::COLAMDOrdering> solver; + solver.compute(R_); + measurementnoisecovariance R_in_temp = solver.solve(b); + spMt R_in = R_in_temp.sparseView(); + spMt K_temp = h_x_.transpose() * R_in * h_x_; + cov P_temp = P_.inverse(); + P_temp += K_temp; + K_ = P_temp.inverse() * h_x_.transpose() * R_in; + #else + measurementnoisecovariance R_in = (h_v_*R*h_v_.transpose()).inverse(); + K_ = (h_x_.transpose() * R_in * h_x_ + P_.inverse()).inverse() * h_x_.transpose() * R_in; + #endif + } + Matrix innovation; + z.boxminus(innovation, h); + cov K_x = K_ * h_x_; + Matrix dx_ = K_ * innovation + (K_x - Matrix::Identity()) * dx_new; + state x_before = x_; + x_.boxplus(dx_); + + _share.converge = true; + for(int i = 0; i < n ; i++) + { + if(std::fabs(dx_[i]) > limit[i]) + { + _share.converge = false; + break; + } + } + + if(_share.converge) t++; + + if(t > 1 || i == maximum_iter - 1) + { + L_ = P_; + + Matrix res_temp_SO3; + MTK::vect<3, scalar_type> seg_SO3; + for(typename std::vector >::iterator it = x_.SO3_state.begin(); it != x_.SO3_state.end(); it++) { + int idx = (*it).first; + for(int i = 0; i < 3; i++){ + seg_SO3(i) = dx_(i + idx); + } + res_temp_SO3 = A_matrix(seg_SO3).transpose(); + for(int i = 0; i < n; i++){ + L_. template block<3, 1>(idx, i) = res_temp_SO3 * (P_. template block<3, 1>(idx, i)); + } + if(n > l) + { + for(int i = 0; i < l; i++){ + K_. template block<3, 1>(idx, i) = res_temp_SO3 * (K_. template block<3, 1>(idx, i)); + } + } + else + { + for(int i = 0; i < n; i++){ + K_x. template block<3, 1>(idx, i) = res_temp_SO3 * (K_x. template block<3, 1>(idx, i)); + } + } + for(int i = 0; i < n; i++){ + L_. template block<1, 3>(i, idx) = (L_. template block<1, 3>(i, idx)) * res_temp_SO3.transpose(); + P_. template block<1, 3>(i, idx) = (P_. template block<1, 3>(i, idx)) * res_temp_SO3.transpose(); + } + } + + Matrix res_temp_S2; + MTK::vect<2, scalar_type> seg_S2; + for(typename std::vector >::iterator it = x_.S2_state.begin(); it != x_.S2_state.end(); it++) { + int idx = (*it).first; + + for(int i = 0; i < 2; i++){ + seg_S2(i) = dx_(i + idx); + } + + Eigen::Matrix Nx; + Eigen::Matrix Mx; + x_.S2_Nx_yy(Nx, idx); + x_propagated.S2_Mx(Mx, seg_S2, idx); + res_temp_S2 = Nx * Mx; + + for(int i = 0; i < n; i++){ + L_. template block<2, 1>(idx, i) = res_temp_S2 * (P_. template block<2, 1>(idx, i)); + } + if(n > l) + { + for(int i = 0; i < l; i++){ + K_. template block<2, 1>(idx, i) = res_temp_S2 * (K_. template block<2, 1>(idx, i)); + } + } + else + { + for(int i = 0; i < n; i++){ + K_x. template block<2, 1>(idx, i) = res_temp_S2 * (K_x. template block<2, 1>(idx, i)); + } + } + for(int i = 0; i < n; i++){ + L_. template block<1, 2>(i, idx) = (L_. template block<1, 2>(i, idx)) * res_temp_S2.transpose(); + P_. template block<1, 2>(i, idx) = (P_. template block<1, 2>(i, idx)) * res_temp_S2.transpose(); + } + } + if(n > l) + { + P_ = L_ - K_ * h_x_ * P_; + } + else + { + P_ = L_ - K_x * P_; + } + return; + } + } + } + + //iterated error state EKF update for measurement as an Eigen matrix whose dimension is changing. + void update_iterated_dyn(Eigen::Matrix z, measurementnoisecovariance_dyn R) { + + int t = 0; + bool valid = true; + bool converg = true; + state x_propagated = x_; + cov P_propagated = P_; + int dof_Measurement; + int dof_Measurement_noise = R.rows(); + for(int i=-1; i h_x_ = h_x_dyn(x_, valid); + Matrix h_v_ = h_v_dyn(x_, valid); + #endif + Matrix h_ = h_dyn(x_, valid); + dof_Measurement = h_.rows(); + vectorized_state dx, dx_new; + x_.boxminus(dx, x_propagated); + dx_new = dx; + if(! valid) + { + continue; + } + + P_ = P_propagated; + Matrix res_temp_SO3; + MTK::vect<3, scalar_type> seg_SO3; + for (std::vector >::iterator it = x_.SO3_state.begin(); it != x_.SO3_state.end(); it++) { + int idx = (*it).first; + int dim = (*it).second; + for(int i = 0; i < 3; i++){ + seg_SO3(i) = dx(idx+i); + } + + res_temp_SO3 = MTK::A_matrix(seg_SO3).transpose(); + dx_new.template block<3, 1>(idx, 0) = res_temp_SO3 * dx_new.template block<3, 1>(idx, 0); + for(int i = 0; i < n; i++){ + P_. template block<3, 1>(idx, i) = res_temp_SO3 * (P_. template block<3, 1>(idx, i)); + } + for(int i = 0; i < n; i++){ + P_. template block<1, 3>(i, idx) =(P_. template block<1, 3>(i, idx)) * res_temp_SO3.transpose(); + } + } + + Matrix res_temp_S2; + MTK::vect<2, scalar_type> seg_S2; + for (std::vector >::iterator it = x_.S2_state.begin(); it != x_.S2_state.end(); it++) { + int idx = (*it).first; + int dim = (*it).second; + for(int i = 0; i < 2; i++){ + seg_S2(i) = dx(idx + i); + } + + Eigen::Matrix Nx; + Eigen::Matrix Mx; + x_.S2_Nx_yy(Nx, idx); + x_propagated.S2_Mx(Mx, seg_S2, idx); + res_temp_S2 = Nx * Mx; + dx_new.template block<2, 1>(idx, 0) = res_temp_S2 * dx_new.template block<2, 1>(idx, 0); + for(int i = 0; i < n; i++){ + P_. template block<2, 1>(idx, i) = res_temp_S2 * (P_. template block<2, 1>(idx, i)); + } + for(int i = 0; i < n; i++){ + P_. template block<1, 2>(i, idx) = (P_. template block<1, 2>(i, idx)) * res_temp_S2.transpose(); + } + } + + Matrix K_; + if(n > dof_Measurement) + { + #ifdef USE_sparse + Matrix K_temp = h_x_ * P_ * h_x_.transpose(); + spMt R_temp = h_v_ * R_ * h_v_.transpose(); + K_temp += R_temp; + K_ = P_ * h_x_.transpose() * K_temp.inverse(); + #else + K_= P_ * h_x_.transpose() * (h_x_ * P_ * h_x_.transpose() + h_v_ * R * h_v_.transpose()).inverse(); + #endif + } + else + { + #ifdef USE_sparse + Eigen::Matrix b = Eigen::Matrix::Identity(dof_Measurement_noise, dof_Measurement_noise); + Eigen::SparseQR, Eigen::COLAMDOrdering> solver; + solver.compute(R_); + Eigen::Matrix R_in_temp = solver.solve(b); + spMt R_in = R_in_temp.sparseView(); + spMt K_temp = h_x_.transpose() * R_in * h_x_; + cov P_temp = P_.inverse(); + P_temp += K_temp; + K_ = P_temp.inverse() * h_x_.transpose() * R_in; + #else + Eigen::Matrix R_in = (h_v_*R*h_v_.transpose()).inverse(); + K_ = (h_x_.transpose() * R_in * h_x_ + P_.inverse()).inverse() * h_x_.transpose() * R_in; + #endif + } + cov K_x = K_ * h_x_; + Matrix dx_ = K_ * (z - h_) + (K_x - Matrix::Identity()) * dx_new; + state x_before = x_; + x_.boxplus(dx_); + converg = true; + for(int i = 0; i < n ; i++) + { + if(std::fabs(dx_[i]) > limit[i]) + { + converg = false; + break; + } + } + if(converg) t++; + if(t > 1 || i == maximum_iter - 1) + { + L_ = P_; + std::cout << "iteration time:" << t << "," << i << std::endl; + + Matrix res_temp_SO3; + MTK::vect<3, scalar_type> seg_SO3; + for(typename std::vector >::iterator it = x_.SO3_state.begin(); it != x_.SO3_state.end(); it++) { + int idx = (*it).first; + for(int i = 0; i < 3; i++){ + seg_SO3(i) = dx_(i + idx); + } + res_temp_SO3 = MTK::A_matrix(seg_SO3).transpose(); + for(int i = 0; i < n; i++){ + L_. template block<3, 1>(idx, i) = res_temp_SO3 * (P_. template block<3, 1>(idx, i)); + } + if(n > dof_Measurement) + { + for(int i = 0; i < dof_Measurement; i++){ + K_. template block<3, 1>(idx, i) = res_temp_SO3 * (K_. template block<3, 1>(idx, i)); + } + } + else + { + for(int i = 0; i < n; i++){ + K_x. template block<3, 1>(idx, i) = res_temp_SO3 * (K_x. template block<3, 1>(idx, i)); + } + } + for(int i = 0; i < n; i++){ + L_. template block<1, 3>(i, idx) = (L_. template block<1, 3>(i, idx)) * res_temp_SO3.transpose(); + P_. template block<1, 3>(i, idx) = (P_. template block<1, 3>(i, idx)) * res_temp_SO3.transpose(); + } + } + + Matrix res_temp_S2; + MTK::vect<2, scalar_type> seg_S2; + for(typename std::vector >::iterator it = x_.S2_state.begin(); it != x_.S2_state.end(); it++) { + int idx = (*it).first; + + for(int i = 0; i < 2; i++){ + seg_S2(i) = dx_(i + idx); + } + + Eigen::Matrix Nx; + Eigen::Matrix Mx; + x_.S2_Nx_yy(Nx, idx); + x_propagated.S2_Mx(Mx, seg_S2, idx); + res_temp_S2 = Nx * Mx; + + for(int i = 0; i < n; i++){ + L_. template block<2, 1>(idx, i) = res_temp_S2 * (P_. template block<2, 1>(idx, i)); + } + if(n > dof_Measurement) + { + for(int i = 0; i < dof_Measurement; i++){ + K_. template block<2, 1>(idx, i) = res_temp_S2 * (K_. template block<2, 1>(idx, i)); + } + } + else + { + for(int i = 0; i < n; i++){ + K_x. template block<2, 1>(idx, i) = res_temp_S2 * (K_x. template block<2, 1>(idx, i)); + } + } + for(int i = 0; i < n; i++){ + L_. template block<1, 2>(i, idx) = (L_. template block<1, 2>(i, idx)) * res_temp_S2.transpose(); + P_. template block<1, 2>(i, idx) = (P_. template block<1, 2>(i, idx)) * res_temp_S2.transpose(); + } + } + if(n > dof_Measurement) + { + P_ = L_ - K_*h_x_*P_; + } + else + { + P_ = L_ - K_x * P_; + } + return; + } + } + } + //iterated error state EKF update for measurement as an Eigen matrix whose dimension is changing. + //calculate measurement (z), estimate measurement (h), partial differention matrices (h_x, h_v) and the noise covariance (R) at the same time, by only one function. + void update_iterated_dyn_share() { + + int t = 0; + dyn_share_datastruct dyn_share; + dyn_share.valid = true; + dyn_share.converge = true; + state x_propagated = x_; + cov P_propagated = P_; + int dof_Measurement; + int dof_Measurement_noise; + for(int i=-1; i h = h_dyn_share (x_, dyn_share); + Matrix z = dyn_share.z; + Matrix h = dyn_share.h; + #ifdef USE_sparse + spMt h_x = dyn_share.h_x.sparseView(); + spMt h_v = dyn_share.h_v.sparseView(); + spMt R_ = dyn_share.R.sparseView(); + #else + Matrix R = dyn_share.R; + Matrix h_x = dyn_share.h_x; + Matrix h_v = dyn_share.h_v; + #endif + dof_Measurement = h_x.rows(); + dof_Measurement_noise = dyn_share.R.rows(); + vectorized_state dx, dx_new; + x_.boxminus(dx, x_propagated); + dx_new = dx; + if(! (dyn_share.valid)) + { + continue; + } + + P_ = P_propagated; + Matrix res_temp_SO3; + MTK::vect<3, scalar_type> seg_SO3; + for (std::vector >::iterator it = x_.SO3_state.begin(); it != x_.SO3_state.end(); it++) { + int idx = (*it).first; + int dim = (*it).second; + for(int i = 0; i < 3; i++){ + seg_SO3(i) = dx(idx+i); + } + + res_temp_SO3 = MTK::A_matrix(seg_SO3).transpose(); + dx_new.template block<3, 1>(idx, 0) = res_temp_SO3 * dx_new.template block<3, 1>(idx, 0); + for(int i = 0; i < n; i++){ + P_. template block<3, 1>(idx, i) = res_temp_SO3 * (P_. template block<3, 1>(idx, i)); + } + for(int i = 0; i < n; i++){ + P_. template block<1, 3>(i, idx) =(P_. template block<1, 3>(i, idx)) * res_temp_SO3.transpose(); + } + } + + Matrix res_temp_S2; + MTK::vect<2, scalar_type> seg_S2; + for (std::vector >::iterator it = x_.S2_state.begin(); it != x_.S2_state.end(); it++) { + int idx = (*it).first; + int dim = (*it).second; + for(int i = 0; i < 2; i++){ + seg_S2(i) = dx(idx + i); + } + + Eigen::Matrix Nx; + Eigen::Matrix Mx; + x_.S2_Nx_yy(Nx, idx); + x_propagated.S2_Mx(Mx, seg_S2, idx); + res_temp_S2 = Nx * Mx; + dx_new.template block<2, 1>(idx, 0) = res_temp_S2 * dx_new.template block<2, 1>(idx, 0); + for(int i = 0; i < n; i++){ + P_. template block<2, 1>(idx, i) = res_temp_S2 * (P_. template block<2, 1>(idx, i)); + } + for(int i = 0; i < n; i++){ + P_. template block<1, 2>(i, idx) = (P_. template block<1, 2>(i, idx)) * res_temp_S2.transpose(); + } + } + + Matrix K_; + if(n > dof_Measurement) + { + #ifdef USE_sparse + Matrix K_temp = h_x * P_ * h_x.transpose(); + spMt R_temp = h_v * R_ * h_v.transpose(); + K_temp += R_temp; + K_ = P_ * h_x.transpose() * K_temp.inverse(); + #else + K_= P_ * h_x.transpose() * (h_x * P_ * h_x.transpose() + h_v * R * h_v.transpose()).inverse(); + #endif + } + else + { + #ifdef USE_sparse + Eigen::Matrix b = Eigen::Matrix::Identity(dof_Measurement_noise, dof_Measurement_noise); + Eigen::SparseQR, Eigen::COLAMDOrdering> solver; + solver.compute(R_); + Eigen::Matrix R_in_temp = solver.solve(b); + spMt R_in = R_in_temp.sparseView(); + spMt K_temp = h_x.transpose() * R_in * h_x; + cov P_temp = P_.inverse(); + P_temp += K_temp; + K_ = P_temp.inverse() * h_x.transpose() * R_in; + #else + Eigen::Matrix R_in = (h_v*R*h_v.transpose()).inverse(); + K_ = (h_x.transpose() * R_in * h_x + P_.inverse()).inverse() * h_x.transpose() * R_in; + #endif + } + + cov K_x = K_ * h_x; + Matrix dx_ = K_ * (z - h) + (K_x - Matrix::Identity()) * dx_new; + state x_before = x_; + x_.boxplus(dx_); + dyn_share.converge = true; + for(int i = 0; i < n ; i++) + { + if(std::fabs(dx_[i]) > limit[i]) + { + dyn_share.converge = false; + break; + } + } + if(dyn_share.converge) t++; + if(t > 1 || i == maximum_iter - 1) + { + L_ = P_; + std::cout << "iteration time:" << t << "," << i << std::endl; + + Matrix res_temp_SO3; + MTK::vect<3, scalar_type> seg_SO3; + for(typename std::vector >::iterator it = x_.SO3_state.begin(); it != x_.SO3_state.end(); it++) { + int idx = (*it).first; + for(int i = 0; i < 3; i++){ + seg_SO3(i) = dx_(i + idx); + } + res_temp_SO3 = MTK::A_matrix(seg_SO3).transpose(); + for(int i = 0; i < int(n); i++){ + L_. template block<3, 1>(idx, i) = res_temp_SO3 * (P_. template block<3, 1>(idx, i)); + } + if(n > dof_Measurement) + { + for(int i = 0; i < dof_Measurement; i++){ + K_. template block<3, 1>(idx, i) = res_temp_SO3 * (K_. template block<3, 1>(idx, i)); + } + } + else + { + for(int i = 0; i < n; i++){ + K_x. template block<3, 1>(idx, i) = res_temp_SO3 * (K_x. template block<3, 1>(idx, i)); + } + } + for(int i = 0; i < n; i++){ + L_. template block<1, 3>(i, idx) = (L_. template block<1, 3>(i, idx)) * res_temp_SO3.transpose(); + P_. template block<1, 3>(i, idx) = (P_. template block<1, 3>(i, idx)) * res_temp_SO3.transpose(); + } + } + + Matrix res_temp_S2; + MTK::vect<2, scalar_type> seg_S2; + for(typename std::vector >::iterator it = x_.S2_state.begin(); it != x_.S2_state.end(); it++) { + int idx = (*it).first; + + for(int i = 0; i < 2; i++){ + seg_S2(i) = dx_(i + idx); + } + + Eigen::Matrix Nx; + Eigen::Matrix Mx; + x_.S2_Nx_yy(Nx, idx); + x_propagated.S2_Mx(Mx, seg_S2, idx); + res_temp_S2 = Nx * Mx; + + for(int i = 0; i < n; i++){ + L_. template block<2, 1>(idx, i) = res_temp_S2 * (P_. template block<2, 1>(idx, i)); + } + if(n > dof_Measurement) + { + for(int i = 0; i < dof_Measurement; i++){ + K_. template block<2, 1>(idx, i) = res_temp_S2 * (K_. template block<2, 1>(idx, i)); + } + } + else + { + for(int i = 0; i < n; i++){ + K_x. template block<2, 1>(idx, i) = res_temp_S2 * (K_x. template block<2, 1>(idx, i)); + } + } + for(int i = 0; i < n; i++){ + L_. template block<1, 2>(i, idx) = (L_. template block<1, 2>(i, idx)) * res_temp_S2.transpose(); + P_. template block<1, 2>(i, idx) = (P_. template block<1, 2>(i, idx)) * res_temp_S2.transpose(); + } + } + if(n > dof_Measurement) + { + P_ = L_ - K_*h_x*P_; + } + else + { + P_ = L_ - K_x * P_; + } + return; + } + } + } + + //iterated error state EKF update for measurement as a dynamic manifold, whose dimension or type is changing. + //the measurement and the measurement model are received in a dynamic manner. + template + void update_iterated_dyn_runtime(measurement_runtime z, measurementnoisecovariance_dyn R, measurementModel_runtime h_runtime) { + + int t = 0; + bool valid = true; + bool converg = true; + state x_propagated = x_; + cov P_propagated = P_; + int dof_Measurement; + int dof_Measurement_noise; + for(int i=-1; i h_x_ = h_x_dyn(x_, valid); + Matrix h_v_ = h_v_dyn(x_, valid); + #endif + measurement_runtime h_ = h_runtime(x_, valid); + dof_Measurement = measurement_runtime::DOF; + dof_Measurement_noise = R.rows(); + vectorized_state dx, dx_new; + x_.boxminus(dx, x_propagated); + dx_new = dx; + if(! valid) + { + continue; + } + + P_ = P_propagated; + Matrix res_temp_SO3; + MTK::vect<3, scalar_type> seg_SO3; + for (std::vector >::iterator it = x_.SO3_state.begin(); it != x_.SO3_state.end(); it++) { + int idx = (*it).first; + int dim = (*it).second; + for(int i = 0; i < 3; i++){ + seg_SO3(i) = dx(idx+i); + } + + res_temp_SO3 = MTK::A_matrix(seg_SO3).transpose(); + dx_new.template block<3, 1>(idx, 0) = res_temp_SO3 * dx_new.template block<3, 1>(idx, 0); + for(int i = 0; i < n; i++){ + P_. template block<3, 1>(idx, i) = res_temp_SO3 * (P_. template block<3, 1>(idx, i)); + } + for(int i = 0; i < n; i++){ + P_. template block<1, 3>(i, idx) =(P_. template block<1, 3>(i, idx)) * res_temp_SO3.transpose(); + } + } + + Matrix res_temp_S2; + MTK::vect<2, scalar_type> seg_S2; + for (std::vector >::iterator it = x_.S2_state.begin(); it != x_.S2_state.end(); it++) { + int idx = (*it).first; + int dim = (*it).second; + for(int i = 0; i < 2; i++){ + seg_S2(i) = dx(idx + i); + } + + Eigen::Matrix Nx; + Eigen::Matrix Mx; + x_.S2_Nx_yy(Nx, idx); + x_propagated.S2_Mx(Mx, seg_S2, idx); + res_temp_S2 = Nx * Mx; + dx_new.template block<2, 1>(idx, 0) = res_temp_S2 * dx_new.template block<2, 1>(idx, 0); + for(int i = 0; i < n; i++){ + P_. template block<2, 1>(idx, i) = res_temp_S2 * (P_. template block<2, 1>(idx, i)); + } + for(int i = 0; i < n; i++){ + P_. template block<1, 2>(i, idx) = (P_. template block<1, 2>(i, idx)) * res_temp_S2.transpose(); + } + } + + Matrix K_; + if(n > dof_Measurement) + { + #ifdef USE_sparse + Matrix K_temp = h_x_ * P_ * h_x_.transpose(); + spMt R_temp = h_v_ * R_ * h_v_.transpose(); + K_temp += R_temp; + K_ = P_ * h_x_.transpose() * K_temp.inverse(); + #else + K_= P_ * h_x_.transpose() * (h_x_ * P_ * h_x_.transpose() + h_v_ * R * h_v_.transpose()).inverse(); + #endif + } + else + { + #ifdef USE_sparse + Eigen::Matrix b = Eigen::Matrix::Identity(dof_Measurement_noise, dof_Measurement_noise); + Eigen::SparseQR, Eigen::COLAMDOrdering> solver; + solver.compute(R_); + Eigen::Matrix R_in_temp = solver.solve(b); + spMt R_in = R_in_temp.sparseView(); + spMt K_temp = h_x_.transpose() * R_in * h_x_; + cov P_temp = P_.inverse(); + P_temp += K_temp; + K_ = P_temp.inverse() * h_x_.transpose() * R_in; + #else + Eigen::Matrix R_in = (h_v_*R*h_v_.transpose()).inverse(); + K_ = (h_x_.transpose() * R_in * h_x_ + P_.inverse()).inverse() * h_x_.transpose() * R_in; + #endif + } + cov K_x = K_ * h_x_; + Eigen::Matrix innovation; + z.boxminus(innovation, h_); + Matrix dx_ = K_ * innovation + (K_x - Matrix::Identity()) * dx_new; + state x_before = x_; + x_.boxplus(dx_); + converg = true; + for(int i = 0; i < n ; i++) + { + if(std::fabs(dx_[i]) > limit[i]) + { + converg = false; + break; + } + } + if(converg) t++; + if(t > 1 || i == maximum_iter - 1) + { + L_ = P_; + std::cout << "iteration time:" << t << "," << i << std::endl; + + Matrix res_temp_SO3; + MTK::vect<3, scalar_type> seg_SO3; + for(typename std::vector >::iterator it = x_.SO3_state.begin(); it != x_.SO3_state.end(); it++) { + int idx = (*it).first; + for(int i = 0; i < 3; i++){ + seg_SO3(i) = dx_(i + idx); + } + res_temp_SO3 = MTK::A_matrix(seg_SO3).transpose(); + for(int i = 0; i < n; i++){ + L_. template block<3, 1>(idx, i) = res_temp_SO3 * (P_. template block<3, 1>(idx, i)); + } + if(n > dof_Measurement) + { + for(int i = 0; i < dof_Measurement; i++){ + K_. template block<3, 1>(idx, i) = res_temp_SO3 * (K_. template block<3, 1>(idx, i)); + } + } + else + { + for(int i = 0; i < n; i++){ + K_x. template block<3, 1>(idx, i) = res_temp_SO3 * (K_x. template block<3, 1>(idx, i)); + } + } + for(int i = 0; i < n; i++){ + L_. template block<1, 3>(i, idx) = (L_. template block<1, 3>(i, idx)) * res_temp_SO3.transpose(); + P_. template block<1, 3>(i, idx) = (P_. template block<1, 3>(i, idx)) * res_temp_SO3.transpose(); + } + } + + Matrix res_temp_S2; + MTK::vect<2, scalar_type> seg_S2; + for(typename std::vector >::iterator it = x_.S2_state.begin(); it != x_.S2_state.end(); it++) { + int idx = (*it).first; + + for(int i = 0; i < 2; i++){ + seg_S2(i) = dx_(i + idx); + } + + Eigen::Matrix Nx; + Eigen::Matrix Mx; + x_.S2_Nx_yy(Nx, idx); + x_propagated.S2_Mx(Mx, seg_S2, idx); + res_temp_S2 = Nx * Mx; + + for(int i = 0; i < n; i++){ + L_. template block<2, 1>(idx, i) = res_temp_S2 * (P_. template block<2, 1>(idx, i)); + } + if(n > dof_Measurement) + { + for(int i = 0; i < dof_Measurement; i++){ + K_. template block<2, 1>(idx, i) = res_temp_S2 * (K_. template block<2, 1>(idx, i)); + } + } + else + { + for(int i = 0; i < n; i++){ + K_x. template block<2, 1>(idx, i) = res_temp_S2 * (K_x. template block<2, 1>(idx, i)); + } + } + for(int i = 0; i < n; i++){ + L_. template block<1, 2>(i, idx) = (L_. template block<1, 2>(i, idx)) * res_temp_S2.transpose(); + P_. template block<1, 2>(i, idx) = (P_. template block<1, 2>(i, idx)) * res_temp_S2.transpose(); + } + } + if(n > dof_Measurement) + { + P_ = L_ - K_*h_x_*P_; + } + else + { + P_ = L_ - K_x * P_; + } + return; + } + } + } + + //iterated error state EKF update for measurement as a dynamic manifold, whose dimension or type is changing. + //the measurement and the measurement model are received in a dynamic manner. + //calculate measurement (z), estimate measurement (h), partial differention matrices (h_x, h_v) and the noise covariance (R) at the same time, by only one function. + template + void update_iterated_dyn_runtime_share(measurement_runtime z, measurementModel_dyn_runtime_share h) { + + int t = 0; + dyn_runtime_share_datastruct dyn_share; + dyn_share.valid = true; + dyn_share.converge = true; + state x_propagated = x_; + cov P_propagated = P_; + int dof_Measurement; + int dof_Measurement_noise; + for(int i=-1; i R = dyn_share.R; + Matrix h_x = dyn_share.h_x; + Matrix h_v = dyn_share.h_v; + #endif + dof_Measurement = measurement_runtime::DOF; + dof_Measurement_noise = dyn_share.R.rows(); + vectorized_state dx, dx_new; + x_.boxminus(dx, x_propagated); + dx_new = dx; + if(! (dyn_share.valid)) + { + continue; + } + + P_ = P_propagated; + Matrix res_temp_SO3; + MTK::vect<3, scalar_type> seg_SO3; + for (std::vector >::iterator it = x_.SO3_state.begin(); it != x_.SO3_state.end(); it++) { + int idx = (*it).first; + int dim = (*it).second; + for(int i = 0; i < 3; i++){ + seg_SO3(i) = dx(idx+i); + } + + res_temp_SO3 = MTK::A_matrix(seg_SO3).transpose(); + dx_new.template block<3, 1>(idx, 0) = res_temp_SO3 * dx_new.template block<3, 1>(idx, 0); + for(int i = 0; i < n; i++){ + P_. template block<3, 1>(idx, i) = res_temp_SO3 * (P_. template block<3, 1>(idx, i)); + } + for(int i = 0; i < n; i++){ + P_. template block<1, 3>(i, idx) =(P_. template block<1, 3>(i, idx)) * res_temp_SO3.transpose(); + } + } + + Matrix res_temp_S2; + MTK::vect<2, scalar_type> seg_S2; + for (std::vector >::iterator it = x_.S2_state.begin(); it != x_.S2_state.end(); it++) { + int idx = (*it).first; + int dim = (*it).second; + for(int i = 0; i < 2; i++){ + seg_S2(i) = dx(idx + i); + } + + Eigen::Matrix Nx; + Eigen::Matrix Mx; + x_.S2_Nx_yy(Nx, idx); + x_propagated.S2_Mx(Mx, seg_S2, idx); + res_temp_S2 = Nx * Mx; + dx_new.template block<2, 1>(idx, 0) = res_temp_S2 * dx_new.template block<2, 1>(idx, 0); + for(int i = 0; i < n; i++){ + P_. template block<2, 1>(idx, i) = res_temp_S2 * (P_. template block<2, 1>(idx, i)); + } + for(int i = 0; i < n; i++){ + P_. template block<1, 2>(i, idx) = (P_. template block<1, 2>(i, idx)) * res_temp_S2.transpose(); + } + } + + Matrix K_; + if(n > dof_Measurement) + { + #ifdef USE_sparse + Matrix K_temp = h_x * P_ * h_x.transpose(); + spMt R_temp = h_v * R_ * h_v.transpose(); + K_temp += R_temp; + K_ = P_ * h_x.transpose() * K_temp.inverse(); + #else + K_= P_ * h_x.transpose() * (h_x * P_ * h_x.transpose() + h_v * R * h_v.transpose()).inverse(); + #endif + } + else + { + #ifdef USE_sparse + Eigen::Matrix b = Eigen::Matrix::Identity(dof_Measurement_noise, dof_Measurement_noise); + Eigen::SparseQR, Eigen::COLAMDOrdering> solver; + solver.compute(R_); + Eigen::Matrix R_in_temp = solver.solve(b); + spMt R_in =R_in_temp.sparseView(); + spMt K_temp = h_x.transpose() * R_in * h_x; + cov P_temp = P_.inverse(); + P_temp += K_temp; + K_ = P_temp.inverse() * h_x.transpose() * R_in; + #else + Eigen::Matrix R_in = (h_v*R*h_v.transpose()).inverse(); + K_ = (h_x.transpose() * R_in * h_x + P_.inverse()).inverse() * h_x.transpose() * R_in; + #endif + } + cov K_x = K_ * h_x; + Eigen::Matrix innovation; + z.boxminus(innovation, h_); + Matrix dx_ = K_ * innovation + (K_x - Matrix::Identity()) * dx_new; + state x_before = x_; + x_.boxplus(dx_); + dyn_share.converge = true; + for(int i = 0; i < n ; i++) + { + if(std::fabs(dx_[i]) > limit[i]) + { + dyn_share.converge = false; + break; + } + } + if(dyn_share.converge) t++; + if(t > 1 || i == maximum_iter - 1) + { + L_ = P_; + std::cout << "iteration time:" << t << "," << i << std::endl; + + Matrix res_temp_SO3; + MTK::vect<3, scalar_type> seg_SO3; + for(typename std::vector >::iterator it = x_.SO3_state.begin(); it != x_.SO3_state.end(); it++) { + int idx = (*it).first; + for(int i = 0; i < 3; i++){ + seg_SO3(i) = dx_(i + idx); + } + res_temp_SO3 = MTK::A_matrix(seg_SO3).transpose(); + for(int i = 0; i < int(n); i++){ + L_. template block<3, 1>(idx, i) = res_temp_SO3 * (P_. template block<3, 1>(idx, i)); + } + if(n > dof_Measurement) + { + for(int i = 0; i < dof_Measurement; i++){ + K_. template block<3, 1>(idx, i) = res_temp_SO3 * (K_. template block<3, 1>(idx, i)); + } + } + else + { + for(int i = 0; i < n; i++){ + K_x. template block<3, 1>(idx, i) = res_temp_SO3 * (K_x. template block<3, 1>(idx, i)); + } + } + for(int i = 0; i < n; i++){ + L_. template block<1, 3>(i, idx) = (L_. template block<1, 3>(i, idx)) * res_temp_SO3.transpose(); + P_. template block<1, 3>(i, idx) = (P_. template block<1, 3>(i, idx)) * res_temp_SO3.transpose(); + } + } + + Matrix res_temp_S2; + MTK::vect<2, scalar_type> seg_S2; + for(typename std::vector >::iterator it = x_.S2_state.begin(); it != x_.S2_state.end(); it++) { + int idx = (*it).first; + + for(int i = 0; i < 2; i++){ + seg_S2(i) = dx_(i + idx); + } + + Eigen::Matrix Nx; + Eigen::Matrix Mx; + x_.S2_Nx_yy(Nx, idx); + x_propagated.S2_Mx(Mx, seg_S2, idx); + res_temp_S2 = Nx * Mx; + + for(int i = 0; i < n; i++){ + L_. template block<2, 1>(idx, i) = res_temp_S2 * (P_. template block<2, 1>(idx, i)); + } + if(n > dof_Measurement) + { + for(int i = 0; i < dof_Measurement; i++){ + K_. template block<2, 1>(idx, i) = res_temp_S2 * (K_. template block<2, 1>(idx, i)); + } + } + else + { + for(int i = 0; i < n; i++){ + K_x. template block<2, 1>(idx, i) = res_temp_S2 * (K_x. template block<2, 1>(idx, i)); + } + } + for(int i = 0; i < n; i++){ + L_. template block<1, 2>(i, idx) = (L_. template block<1, 2>(i, idx)) * res_temp_S2.transpose(); + P_. template block<1, 2>(i, idx) = (P_. template block<1, 2>(i, idx)) * res_temp_S2.transpose(); + } + } + if(n > dof_Measurement) + { + P_ = L_ - K_*h_x * P_; + } + else + { + P_ = L_ - K_x * P_; + } + return; + } + } + } + + //iterated error state EKF update modified for one specific system. + void update_iterated_dyn_share_modified(double R, double &solve_time) { + + dyn_share_datastruct dyn_share; + dyn_share.valid = true; + dyn_share.converge = true; + int t = 0; + state x_propagated = x_; + cov P_propagated = P_; + int dof_Measurement; + + Matrix K_h; + Matrix K_x; + + vectorized_state dx_new = vectorized_state::Zero(); + for(int i=-1; i h = h_dyn_share(x_, dyn_share); + #ifdef USE_sparse + spMt h_x_ = dyn_share.h_x.sparseView(); + #else + Eigen::Matrix h_x_ = dyn_share.h_x; + #endif + double solve_start = omp_get_wtime(); + dof_Measurement = h_x_.rows(); + vectorized_state dx; + x_.boxminus(dx, x_propagated); + dx_new = dx; + + + + P_ = P_propagated; + + Matrix res_temp_SO3; + MTK::vect<3, scalar_type> seg_SO3; + for (std::vector >::iterator it = x_.SO3_state.begin(); it != x_.SO3_state.end(); it++) { + int idx = (*it).first; + int dim = (*it).second; + for(int i = 0; i < 3; i++){ + seg_SO3(i) = dx(idx+i); + } + + res_temp_SO3 = MTK::A_matrix(seg_SO3).transpose(); + dx_new.template block<3, 1>(idx, 0) = res_temp_SO3 * dx_new.template block<3, 1>(idx, 0); + for(int i = 0; i < n; i++){ + P_. template block<3, 1>(idx, i) = res_temp_SO3 * (P_. template block<3, 1>(idx, i)); + } + for(int i = 0; i < n; i++){ + P_. template block<1, 3>(i, idx) =(P_. template block<1, 3>(i, idx)) * res_temp_SO3.transpose(); + } + } + + Matrix res_temp_S2; + MTK::vect<2, scalar_type> seg_S2; + for (std::vector >::iterator it = x_.S2_state.begin(); it != x_.S2_state.end(); it++) { + int idx = (*it).first; + int dim = (*it).second; + for(int i = 0; i < 2; i++){ + seg_S2(i) = dx(idx + i); + } + + Eigen::Matrix Nx; + Eigen::Matrix Mx; + x_.S2_Nx_yy(Nx, idx); + x_propagated.S2_Mx(Mx, seg_S2, idx); + res_temp_S2 = Nx * Mx; + dx_new.template block<2, 1>(idx, 0) = res_temp_S2 * dx_new.template block<2, 1>(idx, 0); + for(int i = 0; i < n; i++){ + P_. template block<2, 1>(idx, i) = res_temp_S2 * (P_. template block<2, 1>(idx, i)); + } + for(int i = 0; i < n; i++){ + P_. template block<1, 2>(i, idx) = (P_. template block<1, 2>(i, idx)) * res_temp_S2.transpose(); + } + } + //Matrix K_; + //Matrix K_h; + //Matrix K_x; + + /* + if(n > dof_Measurement) + { + K_= P_ * h_x_.transpose() * (h_x_ * P_ * h_x_.transpose()/R + Eigen::Matrix::Identity(dof_Measurement, dof_Measurement)).inverse()/R; + } + else + { + K_= (h_x_.transpose() * h_x_ + (P_/R).inverse()).inverse()*h_x_.transpose(); + } + */ + + if(n > dof_Measurement) + { + //#ifdef USE_sparse + //Matrix K_temp = h_x * P_ * h_x.transpose(); + //spMt R_temp = h_v * R_ * h_v.transpose(); + //K_temp += R_temp; + Eigen::Matrix h_x_cur = Eigen::Matrix::Zero(dof_Measurement, n); + h_x_cur.topLeftCorner(dof_Measurement, 12) = h_x_; + /* + h_x_cur.col(0) = h_x_.col(0); + h_x_cur.col(1) = h_x_.col(1); + h_x_cur.col(2) = h_x_.col(2); + h_x_cur.col(3) = h_x_.col(3); + h_x_cur.col(4) = h_x_.col(4); + h_x_cur.col(5) = h_x_.col(5); + h_x_cur.col(6) = h_x_.col(6); + h_x_cur.col(7) = h_x_.col(7); + h_x_cur.col(8) = h_x_.col(8); + h_x_cur.col(9) = h_x_.col(9); + h_x_cur.col(10) = h_x_.col(10); + h_x_cur.col(11) = h_x_.col(11); + */ + + Matrix K_ = P_ * h_x_cur.transpose() * (h_x_cur * P_ * h_x_cur.transpose()/R + Eigen::Matrix::Identity(dof_Measurement, dof_Measurement)).inverse()/R; + K_h = K_ * dyn_share.h; + K_x = K_ * h_x_cur; + //#else + // K_= P_ * h_x.transpose() * (h_x * P_ * h_x.transpose() + h_v * R * h_v.transpose()).inverse(); + //#endif + } + else + { + #ifdef USE_sparse + //Eigen::Matrix b = Eigen::Matrix::Identity(); + //Eigen::SparseQR, Eigen::COLAMDOrdering> solver; + spMt A = h_x_.transpose() * h_x_; + cov P_temp = (P_/R).inverse(); + P_temp. template block<12, 12>(0, 0) += A; + P_temp = P_temp.inverse(); + /* + Eigen::Matrix h_x_cur = Eigen::Matrix::Zero(dof_Measurement, n); + h_x_cur.col(0) = h_x_.col(0); + h_x_cur.col(1) = h_x_.col(1); + h_x_cur.col(2) = h_x_.col(2); + h_x_cur.col(3) = h_x_.col(3); + h_x_cur.col(4) = h_x_.col(4); + h_x_cur.col(5) = h_x_.col(5); + h_x_cur.col(6) = h_x_.col(6); + h_x_cur.col(7) = h_x_.col(7); + h_x_cur.col(8) = h_x_.col(8); + h_x_cur.col(9) = h_x_.col(9); + h_x_cur.col(10) = h_x_.col(10); + h_x_cur.col(11) = h_x_.col(11); + */ + K_ = P_temp. template block(0, 0) * h_x_.transpose(); + K_x = cov::Zero(); + K_x. template block(0, 0) = P_inv. template block(0, 0) * HTH; + /* + solver.compute(R_); + Eigen::Matrix R_in_temp = solver.solve(b); + spMt R_in =R_in_temp.sparseView(); + spMt K_temp = h_x.transpose() * R_in * h_x; + cov P_temp = P_.inverse(); + P_temp += K_temp; + K_ = P_temp.inverse() * h_x.transpose() * R_in; + */ + #else + cov P_temp = (P_/R).inverse(); + //Eigen::Matrix h_T = h_x_.transpose(); + Eigen::Matrix HTH = h_x_.transpose() * h_x_; + P_temp. template block<12, 12>(0, 0) += HTH; + /* + Eigen::Matrix h_x_cur = Eigen::Matrix::Zero(dof_Measurement, n); + //std::cout << "line 1767" << std::endl; + h_x_cur.col(0) = h_x_.col(0); + h_x_cur.col(1) = h_x_.col(1); + h_x_cur.col(2) = h_x_.col(2); + h_x_cur.col(3) = h_x_.col(3); + h_x_cur.col(4) = h_x_.col(4); + h_x_cur.col(5) = h_x_.col(5); + h_x_cur.col(6) = h_x_.col(6); + h_x_cur.col(7) = h_x_.col(7); + h_x_cur.col(8) = h_x_.col(8); + h_x_cur.col(9) = h_x_.col(9); + h_x_cur.col(10) = h_x_.col(10); + h_x_cur.col(11) = h_x_.col(11); + */ + cov P_inv = P_temp.inverse(); + //std::cout << "line 1781" << std::endl; + K_h = P_inv. template block(0, 0) * h_x_.transpose() * dyn_share.h; + //std::cout << "line 1780" << std::endl; + //cov HTH_cur = cov::Zero(); + //HTH_cur. template block<12, 12>(0, 0) = HTH; + K_x.setZero(); // = cov::Zero(); + K_x. template block(0, 0) = P_inv. template block(0, 0) * HTH; + //K_= (h_x_.transpose() * h_x_ + (P_/R).inverse()).inverse()*h_x_.transpose(); + #endif + } + + //K_x = K_ * h_x_; + Matrix dx_ = K_h + (K_x - Matrix::Identity()) * dx_new; + state x_before = x_; + x_.boxplus(dx_); + dyn_share.converge = true; + for(int i = 0; i < n ; i++) + { + if(std::fabs(dx_[i]) > limit[i]) + { + dyn_share.converge = false; + break; + } + } + if(dyn_share.converge) t++; + + if(!t && i == maximum_iter - 2) + { + dyn_share.converge = true; + } + + if(t > 1 || i == maximum_iter - 1) + { + L_ = P_; + //std::cout << "iteration time" << t << "," << i << std::endl; + Matrix res_temp_SO3; + MTK::vect<3, scalar_type> seg_SO3; + for(typename std::vector >::iterator it = x_.SO3_state.begin(); it != x_.SO3_state.end(); it++) { + int idx = (*it).first; + for(int i = 0; i < 3; i++){ + seg_SO3(i) = dx_(i + idx); + } + res_temp_SO3 = MTK::A_matrix(seg_SO3).transpose(); + for(int i = 0; i < n; i++){ + L_. template block<3, 1>(idx, i) = res_temp_SO3 * (P_. template block<3, 1>(idx, i)); + } + // if(n > dof_Measurement) + // { + // for(int i = 0; i < dof_Measurement; i++){ + // K_.template block<3, 1>(idx, i) = res_temp_SO3 * (K_. template block<3, 1>(idx, i)); + // } + // } + // else + // { + for(int i = 0; i < 12; i++){ + K_x. template block<3, 1>(idx, i) = res_temp_SO3 * (K_x. template block<3, 1>(idx, i)); + } + //} + for(int i = 0; i < n; i++){ + L_. template block<1, 3>(i, idx) = (L_. template block<1, 3>(i, idx)) * res_temp_SO3.transpose(); + P_. template block<1, 3>(i, idx) = (P_. template block<1, 3>(i, idx)) * res_temp_SO3.transpose(); + } + } + + Matrix res_temp_S2; + MTK::vect<2, scalar_type> seg_S2; + for(typename std::vector >::iterator it = x_.S2_state.begin(); it != x_.S2_state.end(); it++) { + int idx = (*it).first; + + for(int i = 0; i < 2; i++){ + seg_S2(i) = dx_(i + idx); + } + + Eigen::Matrix Nx; + Eigen::Matrix Mx; + x_.S2_Nx_yy(Nx, idx); + x_propagated.S2_Mx(Mx, seg_S2, idx); + res_temp_S2 = Nx * Mx; + for(int i = 0; i < n; i++){ + L_. template block<2, 1>(idx, i) = res_temp_S2 * (P_. template block<2, 1>(idx, i)); + } + // if(n > dof_Measurement) + // { + // for(int i = 0; i < dof_Measurement; i++){ + // K_. template block<2, 1>(idx, i) = res_temp_S2 * (K_. template block<2, 1>(idx, i)); + // } + // } + // else + // { + for(int i = 0; i < 12; i++){ + K_x. template block<2, 1>(idx, i) = res_temp_S2 * (K_x. template block<2, 1>(idx, i)); + } + //} + for(int i = 0; i < n; i++){ + L_. template block<1, 2>(i, idx) = (L_. template block<1, 2>(i, idx)) * res_temp_S2.transpose(); + P_. template block<1, 2>(i, idx) = (P_. template block<1, 2>(i, idx)) * res_temp_S2.transpose(); + } + } + + // if(n > dof_Measurement) + // { + // Eigen::Matrix h_x_cur = Eigen::Matrix::Zero(dof_Measurement, n); + // h_x_cur.topLeftCorner(dof_Measurement, 12) = h_x_; + // /* + // h_x_cur.col(0) = h_x_.col(0); + // h_x_cur.col(1) = h_x_.col(1); + // h_x_cur.col(2) = h_x_.col(2); + // h_x_cur.col(3) = h_x_.col(3); + // h_x_cur.col(4) = h_x_.col(4); + // h_x_cur.col(5) = h_x_.col(5); + // h_x_cur.col(6) = h_x_.col(6); + // h_x_cur.col(7) = h_x_.col(7); + // h_x_cur.col(8) = h_x_.col(8); + // h_x_cur.col(9) = h_x_.col(9); + // h_x_cur.col(10) = h_x_.col(10); + // h_x_cur.col(11) = h_x_.col(11); + // */ + // P_ = L_ - K_*h_x_cur * P_; + // } + // else + //{ + P_ = L_ - K_x.template block(0, 0) * P_.template block<12, n>(0, 0); + //} + solve_time += omp_get_wtime() - solve_start; + return; + } + solve_time += omp_get_wtime() - solve_start; + } + } + + void change_x(state &input_state) + { + x_ = input_state; + if((!x_.vect_state.size())&&(!x_.SO3_state.size())&&(!x_.S2_state.size())) + { + x_.build_S2_state(); + x_.build_SO3_state(); + x_.build_vect_state(); + } + } + + void change_P(cov &input_cov) + { + P_ = input_cov; + } + + const state& get_x() const { + return x_; + } + const cov& get_P() const { + return P_; + } +private: + state x_; + measurement m_; + cov P_; + spMt l_; + spMt f_x_1; + spMt f_x_2; + cov F_x1 = cov::Identity(); + cov F_x2 = cov::Identity(); + cov L_ = cov::Identity(); + + processModel *f; + processMatrix1 *f_x; + processMatrix2 *f_w; + + measurementModel *h; + measurementMatrix1 *h_x; + measurementMatrix2 *h_v; + + measurementModel_dyn *h_dyn; + measurementMatrix1_dyn *h_x_dyn; + measurementMatrix2_dyn *h_v_dyn; + + measurementModel_share *h_share; + measurementModel_dyn_share *h_dyn_share; + + int maximum_iter = 0; + scalar_type limit[n]; + + template + T check_safe_update( T _temp_vec ) + { + T temp_vec = _temp_vec; + if ( std::isnan( temp_vec(0, 0) ) ) + { + temp_vec.setZero(); + return temp_vec; + } + double angular_dis = temp_vec.block( 0, 0, 3, 1 ).norm() * 57.3; + double pos_dis = temp_vec.block( 3, 0, 3, 1 ).norm(); + if ( angular_dis >= 20 || pos_dis > 1 ) + { + printf( "Angular dis = %.2f, pos dis = %.2f\r\n", angular_dis, pos_dis ); + temp_vec.setZero(); + } + return temp_vec; + } +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW +}; + +} // namespace esekfom + +#endif // ESEKFOM_EKF_HPP diff --git a/src/FAST_LIO/include/IKFoM_toolkit/esekfom/util.hpp b/src/FAST_LIO/include/IKFoM_toolkit/esekfom/util.hpp new file mode 100755 index 0000000..ab39fc4 --- /dev/null +++ b/src/FAST_LIO/include/IKFoM_toolkit/esekfom/util.hpp @@ -0,0 +1,82 @@ +/* + * Copyright (c) 2019--2023, The University of Hong Kong + * All rights reserved. + * + * Author: Dongjiao HE + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the Universitaet Bremen nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#ifndef __MEKFOM_UTIL_HPP__ +#define __MEKFOM_UTIL_HPP__ + +#include +#include "../mtk/src/mtkmath.hpp" +namespace esekfom { + +template +class is_same { +public: + operator bool() { + return false; + } +}; +template +class is_same { +public: + operator bool() { + return true; + } +}; + +template +class is_double { +public: + operator bool() { + return false; + } +}; + +template<> +class is_double { +public: + operator bool() { + return true; + } +}; + +template +static T +id(const T &x) +{ + return x; +} + +} // namespace esekfom + +#endif // __MEKFOM_UTIL_HPP__ diff --git a/src/FAST_LIO/include/IKFoM_toolkit/mtk/build_manifold.hpp b/src/FAST_LIO/include/IKFoM_toolkit/mtk/build_manifold.hpp new file mode 100755 index 0000000..2cdb106 --- /dev/null +++ b/src/FAST_LIO/include/IKFoM_toolkit/mtk/build_manifold.hpp @@ -0,0 +1,229 @@ +// This is an advanced implementation of the algorithm described in the +// following paper: +// C. Hertzberg, R. Wagner, U. Frese, and L. Schroder. Integratinggeneric sensor fusion algorithms with sound state representationsthrough encapsulation of manifolds. +// CoRR, vol. abs/1107.1119, 2011.[Online]. Available: http://arxiv.org/abs/1107.1119 + +/* + * Copyright (c) 2019--2023, The University of Hong Kong + * All rights reserved. + * + * Modifier: Dongjiao HE + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the Universitaet Bremen nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/* + * Copyright (c) 2008--2011, Universitaet Bremen + * All rights reserved. + * + * Author: Christoph Hertzberg + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the Universitaet Bremen nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ +/** + * @file mtk/build_manifold.hpp + * @brief Macro to automatically construct compound manifolds. + * + */ +#ifndef MTK_AUTOCONSTRUCT_HPP_ +#define MTK_AUTOCONSTRUCT_HPP_ + +#include + +#include +#include +#include + +#include "src/SubManifold.hpp" +#include "startIdx.hpp" + +#ifndef PARSED_BY_DOXYGEN +//////// internals ////// + +#define MTK_APPLY_MACRO_ON_TUPLE(r, macro, tuple) macro tuple + +#define MTK_TRANSFORM_COMMA(macro, entries) BOOST_PP_SEQ_ENUM(BOOST_PP_SEQ_TRANSFORM_S(1, MTK_APPLY_MACRO_ON_TUPLE, macro, entries)) + +#define MTK_TRANSFORM(macro, entries) BOOST_PP_SEQ_FOR_EACH_R(1, MTK_APPLY_MACRO_ON_TUPLE, macro, entries) + +#define MTK_CONSTRUCTOR_ARG( type, id) const type& id = type() +#define MTK_CONSTRUCTOR_COPY( type, id) id(id) +#define MTK_BOXPLUS( type, id) id.boxplus(MTK::subvector(__vec, &self::id), __scale); +#define MTK_OPLUS( type, id) id.oplus(MTK::subvector_(__vec, &self::id), __scale); +#define MTK_BOXMINUS( type, id) id.boxminus(MTK::subvector(__res, &self::id), __oth.id); +#define MTK_S2_hat( type, id) if(id.IDX == idx){id.S2_hat(res);} +#define MTK_S2_Nx_yy( type, id) if(id.IDX == idx){id.S2_Nx_yy(res);} +#define MTK_S2_Mx( type, id) if(id.IDX == idx){id.S2_Mx(res, dx);} +#define MTK_OSTREAM( type, id) << __var.id << " " +#define MTK_ISTREAM( type, id) >> __var.id +#define MTK_S2_state( type, id) if(id.TYP == 1){S2_state.push_back(std::make_pair(id.IDX, id.DIM));} +#define MTK_SO3_state( type, id) if(id.TYP == 2){(SO3_state).push_back(std::make_pair(id.IDX, id.DIM));} +#define MTK_vect_state( type, id) if(id.TYP == 0){(vect_state).push_back(std::make_pair(std::make_pair(id.IDX, id.DIM), type::DOF));} + +#define MTK_SUBVARLIST(seq, S2state, SO3state) \ +BOOST_PP_FOR_1( \ + ( \ + BOOST_PP_SEQ_SIZE(seq), \ + BOOST_PP_SEQ_HEAD(seq), \ + BOOST_PP_SEQ_TAIL(seq) (~), \ + 0,\ + 0,\ + S2state,\ + SO3state ),\ + MTK_ENTRIES_TEST, MTK_ENTRIES_NEXT, MTK_ENTRIES_OUTPUT) + +#define MTK_PUT_TYPE(type, id, dof, dim, S2state, SO3state) \ + MTK::SubManifold id; +#define MTK_PUT_TYPE_AND_ENUM(type, id, dof, dim, S2state, SO3state) \ + MTK_PUT_TYPE(type, id, dof, dim, S2state, SO3state) \ + enum {DOF = type::DOF + dof}; \ + enum {DIM = type::DIM+dim}; \ + typedef type::scalar scalar; + +#define MTK_ENTRIES_OUTPUT(r, state) MTK_ENTRIES_OUTPUT_I state +#define MTK_ENTRIES_OUTPUT_I(s, head, seq, dof, dim, S2state, SO3state) \ + MTK_APPLY_MACRO_ON_TUPLE(~, \ + BOOST_PP_IF(BOOST_PP_DEC(s), MTK_PUT_TYPE, MTK_PUT_TYPE_AND_ENUM), \ + ( BOOST_PP_TUPLE_REM_2 head, dof, dim, S2state, SO3state)) + +#define MTK_ENTRIES_TEST(r, state) MTK_TUPLE_ELEM_4_0 state + +//! this used to be BOOST_PP_TUPLE_ELEM_4_0: +#define MTK_TUPLE_ELEM_4_0(a,b,c,d,e,f, g) a + +#define MTK_ENTRIES_NEXT(r, state) MTK_ENTRIES_NEXT_I state +#define MTK_ENTRIES_NEXT_I(len, head, seq, dof, dim, S2state, SO3state) ( \ + BOOST_PP_DEC(len), \ + BOOST_PP_SEQ_HEAD(seq), \ + BOOST_PP_SEQ_TAIL(seq), \ + dof + BOOST_PP_TUPLE_ELEM_2_0 head::DOF,\ + dim + BOOST_PP_TUPLE_ELEM_2_0 head::DIM,\ + S2state,\ + SO3state) + +#endif /* not PARSED_BY_DOXYGEN */ + + +/** + * Construct a manifold. + * @param name is the class-name of the manifold, + * @param entries is the list of sub manifolds + * + * Entries must be given in a list like this: + * @code + * typedef MTK::trafo > Pose; + * typedef MTK::vect Vec3; + * MTK_BUILD_MANIFOLD(imu_state, + * ((Pose, pose)) + * ((Vec3, vel)) + * ((Vec3, acc_bias)) + * ) + * @endcode + * Whitespace is optional, but the double parentheses are necessary. + * Construction is done entirely in preprocessor. + * After construction @a name is also a manifold. Its members can be + * accessed by names given in @a entries. + * + * @note Variable types are not allowed to have commas, thus types like + * @c vect need to be typedef'ed ahead. + */ +#define MTK_BUILD_MANIFOLD(name, entries) \ +struct name { \ + typedef name self; \ + std::vector > S2_state;\ + std::vector > SO3_state;\ + std::vector, int> > vect_state;\ + MTK_SUBVARLIST(entries, S2_state, SO3_state) \ + name ( \ + MTK_TRANSFORM_COMMA(MTK_CONSTRUCTOR_ARG, entries) \ + ) : \ + MTK_TRANSFORM_COMMA(MTK_CONSTRUCTOR_COPY, entries) {}\ + int getDOF() const { return DOF; } \ + void boxplus(const MTK::vectview & __vec, scalar __scale = 1 ) { \ + MTK_TRANSFORM(MTK_BOXPLUS, entries)\ + } \ + void oplus(const MTK::vectview & __vec, scalar __scale = 1 ) { \ + MTK_TRANSFORM(MTK_OPLUS, entries)\ + } \ + void boxminus(MTK::vectview __res, const name& __oth) const { \ + MTK_TRANSFORM(MTK_BOXMINUS, entries)\ + } \ + friend std::ostream& operator<<(std::ostream& __os, const name& __var){ \ + return __os MTK_TRANSFORM(MTK_OSTREAM, entries); \ + } \ + void build_S2_state(){\ + MTK_TRANSFORM(MTK_S2_state, entries)\ + }\ + void build_vect_state(){\ + MTK_TRANSFORM(MTK_vect_state, entries)\ + }\ + void build_SO3_state(){\ + MTK_TRANSFORM(MTK_SO3_state, entries)\ + }\ + void S2_hat(Eigen::Matrix &res, int idx) {\ + MTK_TRANSFORM(MTK_S2_hat, entries)\ + }\ + void S2_Nx_yy(Eigen::Matrix &res, int idx) {\ + MTK_TRANSFORM(MTK_S2_Nx_yy, entries)\ + }\ + void S2_Mx(Eigen::Matrix &res, Eigen::Matrix dx, int idx) {\ + MTK_TRANSFORM(MTK_S2_Mx, entries)\ + }\ + friend std::istream& operator>>(std::istream& __is, name& __var){ \ + return __is MTK_TRANSFORM(MTK_ISTREAM, entries); \ + } \ +}; + + + +#endif /*MTK_AUTOCONSTRUCT_HPP_*/ diff --git a/src/FAST_LIO/include/IKFoM_toolkit/mtk/src/SubManifold.hpp b/src/FAST_LIO/include/IKFoM_toolkit/mtk/src/SubManifold.hpp new file mode 100755 index 0000000..a1b13de --- /dev/null +++ b/src/FAST_LIO/include/IKFoM_toolkit/mtk/src/SubManifold.hpp @@ -0,0 +1,123 @@ +// This is an advanced implementation of the algorithm described in the +// following paper: +// C. Hertzberg, R. Wagner, U. Frese, and L. Schroder. Integratinggeneric sensor fusion algorithms with sound state representationsthrough encapsulation of manifolds. +// CoRR, vol. abs/1107.1119, 2011.[Online]. Available: http://arxiv.org/abs/1107.1119 + +/* + * Copyright (c) 2019--2023, The University of Hong Kong + * All rights reserved. + * + * Modifier: Dongjiao HE + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the Universitaet Bremen nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/* + * Copyright (c) 2008--2011, Universitaet Bremen + * All rights reserved. + * + * Author: Christoph Hertzberg + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the Universitaet Bremen nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ +/** + * @file mtk/src/SubManifold.hpp + * @brief Defines the SubManifold class + */ + + +#ifndef SUBMANIFOLD_HPP_ +#define SUBMANIFOLD_HPP_ + + +#include "vectview.hpp" + + +namespace MTK { + +/** + * @ingroup SubManifolds + * Helper class for compound manifolds. + * This class wraps a manifold T and provides an enum IDX refering to the + * index of the SubManifold within the compound manifold. + * + * Memberpointers to a submanifold can be used for @ref SubManifolds "functions accessing submanifolds". + * + * @tparam T The manifold type of the sub-type + * @tparam idx The index of the sub-type within the compound manifold + */ +template +struct SubManifold : public T +{ + enum {IDX = idx, DIM = dim /*!< index of the sub-type within the compound manifold */ }; + //! manifold type + typedef T type; + + //! Construct from derived type + template + explicit + SubManifold(const X& t) : T(t) {}; + + //! Construct from internal type + //explicit + SubManifold(const T& t) : T(t) {}; + + //! inherit assignment operator + using T::operator=; + +}; + +} // namespace MTK + + +#endif /* SUBMANIFOLD_HPP_ */ diff --git a/src/FAST_LIO/include/IKFoM_toolkit/mtk/src/mtkmath.hpp b/src/FAST_LIO/include/IKFoM_toolkit/mtk/src/mtkmath.hpp new file mode 100755 index 0000000..e3420d1 --- /dev/null +++ b/src/FAST_LIO/include/IKFoM_toolkit/mtk/src/mtkmath.hpp @@ -0,0 +1,294 @@ +// This is an advanced implementation of the algorithm described in the +// following paper: +// C. Hertzberg, R. Wagner, U. Frese, and L. Schroder. Integratinggeneric sensor fusion algorithms with sound state representationsthrough encapsulation of manifolds. +// CoRR, vol. abs/1107.1119, 2011.[Online]. Available: http://arxiv.org/abs/1107.1119 + +/* + * Copyright (c) 2019--2023, The University of Hong Kong + * All rights reserved. + * + * Modifier: Dongjiao HE + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the Universitaet Bremen nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/* + * Copyright (c) 2008--2011, Universitaet Bremen + * All rights reserved. + * + * Author: Christoph Hertzberg + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the Universitaet Bremen nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ +/** + * @file mtk/src/mtkmath.hpp + * @brief several math utility functions. + */ + +#ifndef MTKMATH_H_ +#define MTKMATH_H_ + +#include + +#include + +#include "../types/vect.hpp" + +#ifndef M_PI +#define M_PI 3.1415926535897932384626433832795 +#endif + + +namespace MTK { + +namespace internal { + +template +struct traits { + typedef typename Manifold::scalar scalar; + enum {DOF = Manifold::DOF}; + typedef vect vectorized_type; + typedef Eigen::Matrix matrix_type; +}; + +template<> +struct traits : traits > {}; +template<> +struct traits : traits > {}; + +} // namespace internal + +/** + * \defgroup MTKMath Mathematical helper functions + */ +//@{ + +//! constant @f$ \pi @f$ +const double pi = M_PI; + +template inline scalar tolerance(); + +template<> inline float tolerance() { return 1e-5f; } +template<> inline double tolerance() { return 1e-11; } + + +/** + * normalize @a x to @f$[-bound, bound] @f$. + * + * result for @f$ x = bound + 2\cdot n\cdot bound @f$ is arbitrary @f$\pm bound @f$. + */ +template +inline scalar normalize(scalar x, scalar bound){ //not used + if(std::fabs(x) <= bound) return x; + int r = (int)(x *(scalar(1.0)/ bound)); + return x - ((r + (r>>31) + 1) & ~1)*bound; +} + +/** + * Calculate cosine and sinc of sqrt(x2). + * @param x2 the squared angle must be non-negative + * @return a pair containing cos and sinc of sqrt(x2) + */ +template +std::pair cos_sinc_sqrt(const scalar &x2){ + using std::sqrt; + using std::cos; + using std::sin; + static scalar const taylor_0_bound = boost::math::tools::epsilon(); + static scalar const taylor_2_bound = sqrt(taylor_0_bound); + static scalar const taylor_n_bound = sqrt(taylor_2_bound); + + assert(x2>=0 && "argument must be non-negative"); + + // FIXME check if bigger bounds are possible + if(x2>=taylor_n_bound) { + // slow fall-back solution + scalar x = sqrt(x2); + return std::make_pair(cos(x), sin(x)/x); // x is greater than 0. + } + + // FIXME Replace by Horner-Scheme (4 instead of 5 FLOP/term, numerically more stable, theoretically cos and sinc can be calculated in parallel using SSE2 mulpd/addpd) + // TODO Find optimal coefficients using Remez algorithm + static scalar const inv[] = {1/3., 1/4., 1/5., 1/6., 1/7., 1/8., 1/9.}; + scalar cosi = 1., sinc=1; + scalar term = -1/2. * x2; + for(int i=0; i<3; ++i) { + cosi += term; + term *= inv[2*i]; + sinc += term; + term *= -inv[2*i+1] * x2; + } + + return std::make_pair(cosi, sinc); + +} + +template +Eigen::Matrix hat(const Base& v) { + Eigen::Matrix res; + res << 0, -v[2], v[1], + v[2], 0, -v[0], + -v[1], v[0], 0; + return res; +} + +template +Eigen::Matrix A_inv_trans(const Base& v){ + Eigen::Matrix res; + if(v.norm() > MTK::tolerance()) + { + res = Eigen::Matrix::Identity() + 0.5 * hat(v) + (1 - v.norm() * std::cos(v.norm() / 2) / 2 / std::sin(v.norm() / 2)) * hat(v) * hat(v) / v.squaredNorm(); + + } + else + { + res = Eigen::Matrix::Identity(); + } + + return res; +} + +template +Eigen::Matrix A_inv(const Base& v){ + Eigen::Matrix res; + if(v.norm() > MTK::tolerance()) + { + res = Eigen::Matrix::Identity() - 0.5 * hat(v) + (1 - v.norm() * std::cos(v.norm() / 2) / 2 / std::sin(v.norm() / 2)) * hat(v) * hat(v) / v.squaredNorm(); + + } + else + { + res = Eigen::Matrix::Identity(); + } + + return res; +} + +template +Eigen::Matrix S2_w_expw_( Eigen::Matrix v, scalar length) + { + Eigen::Matrix res; + scalar norm = std::sqrt(v[0]*v[0] + v[1]*v[1]); + if(norm < MTK::tolerance()){ + res = Eigen::Matrix::Zero(); + res(0, 1) = 1; + res(1, 2) = 1; + res /= length; + } + else{ + res << -v[0]*(1/norm-1/std::tan(norm))/std::sin(norm), norm/std::sin(norm), 0, + -v[1]*(1/norm-1/std::tan(norm))/std::sin(norm), 0, norm/std::sin(norm); + res /= length; + } + } + +template +Eigen::Matrix A_matrix(const Base & v){ + Eigen::Matrix res; + double squaredNorm = v[0] * v[0] + v[1] * v[1] + v[2] * v[2]; + double norm = std::sqrt(squaredNorm); + if(norm < MTK::tolerance()){ + res = Eigen::Matrix::Identity(); + } + else{ + res = Eigen::Matrix::Identity() + (1 - std::cos(norm)) / squaredNorm * hat(v) + (1 - std::sin(norm) / norm) / squaredNorm * hat(v) * hat(v); + } + return res; +} + +template +scalar exp(vectview result, vectview vec, const scalar& scale = 1) { + scalar norm2 = vec.squaredNorm(); + std::pair cos_sinc = cos_sinc_sqrt(scale*scale * norm2); + scalar mult = cos_sinc.second * scale; + result = mult * vec; + return cos_sinc.first; +} + + +/** + * Inverse function to @c exp. + * + * @param result @c vectview to the result + * @param w scalar part of input + * @param vec vector part of input + * @param scale scale result by this value + * @param plus_minus_periodicity if true values @f$[w, vec]@f$ and @f$[-w, -vec]@f$ give the same result + */ +template +void log(vectview result, + const scalar &w, const vectview vec, + const scalar &scale, bool plus_minus_periodicity) +{ + // FIXME implement optimized case for vec.squaredNorm() <= tolerance() * (w*w) via Rational Remez approximation ~> only one division + scalar nv = vec.norm(); + if(nv < tolerance()) { + if(!plus_minus_periodicity && w < 0) { + // find the maximal entry: + int i; + nv = vec.cwiseAbs().maxCoeff(&i); + result = scale * std::atan2(nv, w) * vect::Unit(i); + return; + } + nv = tolerance(); + } + scalar s = scale / nv * (plus_minus_periodicity ? std::atan(nv / w) : std::atan2(nv, w) ); + + result = s * vec; +} + + +} // namespace MTK + + +#endif /* MTKMATH_H_ */ diff --git a/src/FAST_LIO/include/IKFoM_toolkit/mtk/src/vectview.hpp b/src/FAST_LIO/include/IKFoM_toolkit/mtk/src/vectview.hpp new file mode 100755 index 0000000..5025071 --- /dev/null +++ b/src/FAST_LIO/include/IKFoM_toolkit/mtk/src/vectview.hpp @@ -0,0 +1,168 @@ + +/* + * Copyright (c) 2008--2011, Universitaet Bremen + * All rights reserved. + * + * Author: Christoph Hertzberg + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the Universitaet Bremen nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ +/** + * @file mtk/src/vectview.hpp + * @brief Wrapper class around a pointer used as interface for plain vectors. + */ + +#ifndef VECTVIEW_HPP_ +#define VECTVIEW_HPP_ + +#include + +namespace MTK { + +/** + * A view to a vector. + * Essentially, @c vectview is only a pointer to @c scalar but can be used directly in @c Eigen expressions. + * The dimension of the vector is given as template parameter and type-checked when used in expressions. + * Data has to be modifiable. + * + * @tparam scalar Scalar type of the vector. + * @tparam dim Dimension of the vector. + * + * @todo @c vectview can be replaced by simple inheritance of @c Eigen::Map, as soon as they get const-correct + */ +namespace internal { + template + struct CovBlock { + typedef typename Eigen::Block, T1::DOF, T2::DOF> Type; + typedef typename Eigen::Block, T1::DOF, T2::DOF> ConstType; + }; + + template + struct CovBlock_ { + typedef typename Eigen::Block, T1::DIM, T2::DIM> Type; + typedef typename Eigen::Block, T1::DIM, T2::DIM> ConstType; + }; + + template + struct CrossCovBlock { + typedef typename Eigen::Block, T1::DOF, T2::DOF> Type; + typedef typename Eigen::Block, T1::DOF, T2::DOF> ConstType; + }; + + template + struct CrossCovBlock_ { + typedef typename Eigen::Block, T1::DIM, T2::DIM> Type; + typedef typename Eigen::Block, T1::DIM, T2::DIM> ConstType; + }; + + template + struct VectviewBase { + typedef Eigen::Matrix matrix_type; + typedef typename matrix_type::MapType Type; + typedef typename matrix_type::ConstMapType ConstType; + }; + + template + struct UnalignedType { + typedef T type; + }; +} + +template +class vectview : public internal::VectviewBase::Type { + typedef internal::VectviewBase VectviewBase; +public: + //! plain matrix type + typedef typename VectviewBase::matrix_type matrix_type; + //! base type + typedef typename VectviewBase::Type base; + //! construct from pointer + explicit + vectview(scalar* data, int dim_=dim) : base(data, dim_) {} + //! construct from plain matrix + vectview(matrix_type& m) : base(m.data(), m.size()) {} + //! construct from another @c vectview + vectview(const vectview &v) : base(v) {} + //! construct from Eigen::Block: + template + vectview(Eigen::VectorBlock block) : base(&block.coeffRef(0), block.size()) {} + template + vectview(Eigen::Block block) : base(&block.coeffRef(0), block.size()) {} + + //! inherit assignment operator + using base::operator=; + //! data pointer + scalar* data() {return const_cast(base::data());} +}; + +/** + * @c const version of @c vectview. + * Compared to @c Eigen::Map this implementation is const correct, i.e., + * data will not be modifiable using this view. + * + * @tparam scalar Scalar type of the vector. + * @tparam dim Dimension of the vector. + * + * @sa vectview + */ +template +class vectview : public internal::VectviewBase::ConstType { + typedef internal::VectviewBase VectviewBase; +public: + //! plain matrix type + typedef typename VectviewBase::matrix_type matrix_type; + //! base type + typedef typename VectviewBase::ConstType base; + //! construct from const pointer + explicit + vectview(const scalar* data, int dim_ = dim) : base(data, dim_) {} + //! construct from column vector + template + vectview(const Eigen::Matrix& m) : base(m.data()) {} + //! construct from row vector + template + vectview(const Eigen::Matrix& m) : base(m.data()) {} + //! construct from another @c vectview + vectview(vectview x) : base(x.data()) {} + //! construct from base + vectview(const base &x) : base(x) {} + /** + * Construct from Block + * @todo adapt this, when Block gets const-correct + */ + template + vectview(Eigen::VectorBlock block) : base(&block.coeffRef(0)) {} + template + vectview(Eigen::Block block) : base(&block.coeffRef(0)) {} + +}; + + +} // namespace MTK + +#endif /* VECTVIEW_HPP_ */ diff --git a/src/FAST_LIO/include/IKFoM_toolkit/mtk/startIdx.hpp b/src/FAST_LIO/include/IKFoM_toolkit/mtk/startIdx.hpp new file mode 100755 index 0000000..4dc2958 --- /dev/null +++ b/src/FAST_LIO/include/IKFoM_toolkit/mtk/startIdx.hpp @@ -0,0 +1,328 @@ +// This is an advanced implementation of the algorithm described in the +// following paper: +// C. Hertzberg, R. Wagner, U. Frese, and L. Schroder. Integratinggeneric sensor fusion algorithms with sound state representationsthrough encapsulation of manifolds. +// CoRR, vol. abs/1107.1119, 2011.[Online]. Available: http://arxiv.org/abs/1107.1119 + +/* + * Copyright (c) 2019--2023, The University of Hong Kong + * All rights reserved. + * + * Modifier: Dongjiao HE + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the Universitaet Bremen nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/* + * Copyright (c) 2008--2011, Universitaet Bremen + * All rights reserved. + * + * Author: Christoph Hertzberg + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the Universitaet Bremen nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ +/** + * @file mtk/startIdx.hpp + * @brief Tools to access sub-elements of compound manifolds. + */ +#ifndef GET_START_INDEX_H_ +#define GET_START_INDEX_H_ + +#include + +#include "src/SubManifold.hpp" +#include "src/vectview.hpp" + +namespace MTK { + + +/** + * \defgroup SubManifolds Accessing Submanifolds + * For compound manifolds constructed using MTK_BUILD_MANIFOLD, member pointers + * can be used to get sub-vectors or matrix-blocks of a corresponding big matrix. + * E.g. for a type @a pose consisting of @a orient and @a trans the member pointers + * @c &pose::orient and @c &pose::trans give all required information and are still + * valid if the base type gets extended or the actual types of @a orient and @a trans + * change (e.g. from 2D to 3D). + * + * @todo Maybe require manifolds to typedef MatrixType and VectorType, etc. + */ +//@{ + +/** + * Determine the index of a sub-variable within a compound variable. + */ +template +int getStartIdx( MTK::SubManifold Base::*) +{ + return idx; +} + +template +int getStartIdx_( MTK::SubManifold Base::*) +{ + return dim; +} + +/** + * Determine the degrees of freedom of a sub-variable within a compound variable. + */ +template +int getDof( MTK::SubManifold Base::*) +{ + return T::DOF; +} +template +int getDim( MTK::SubManifold Base::*) +{ + return T::DIM; +} + +/** + * set the diagonal elements of a covariance matrix corresponding to a sub-variable + */ +template +void setDiagonal(Eigen::Matrix &cov, + MTK::SubManifold Base::*, const typename Base::scalar &val) +{ + cov.diagonal().template segment(idx).setConstant(val); +} + +template +void setDiagonal_(Eigen::Matrix &cov, + MTK::SubManifold Base::*, const typename Base::scalar &val) +{ + cov.diagonal().template segment(dim).setConstant(val); +} + +/** + * Get the subblock of corresponding to two members, i.e. + * \code + * Eigen::Matrix m; + * MTK::subblock(m, &Pose::orient, &Pose::trans) = some_expression; + * MTK::subblock(m, &Pose::trans, &Pose::orient) = some_expression.trans(); + * \endcode + * lets you modify mixed covariance entries in a bigger covariance matrix. + */ +template +typename MTK::internal::CovBlock::Type +subblock(Eigen::Matrix &cov, + MTK::SubManifold Base::*, MTK::SubManifold Base::*) +{ + return cov.template block(idx1, idx2); +} + +template +typename MTK::internal::CovBlock_::Type +subblock_(Eigen::Matrix &cov, + MTK::SubManifold Base::*, MTK::SubManifold Base::*) +{ + return cov.template block(dim1, dim2); +} + +template +typename MTK::internal::CrossCovBlock::Type +subblock(Eigen::Matrix &cov, MTK::SubManifold Base1::*, MTK::SubManifold Base2::*) +{ + return cov.template block(idx1, idx2); +} + +template +typename MTK::internal::CrossCovBlock_::Type +subblock_(Eigen::Matrix &cov, MTK::SubManifold Base1::*, MTK::SubManifold Base2::*) +{ + return cov.template block(dim1, dim2); +} +/** + * Get the subblock of corresponding to a member, i.e. + * \code + * Eigen::Matrix m; + * MTK::subblock(m, &Pose::orient) = some_expression; + * \endcode + * lets you modify covariance entries in a bigger covariance matrix. + */ +template +typename MTK::internal::CovBlock_::Type +subblock_(Eigen::Matrix &cov, + MTK::SubManifold Base::*) +{ + return cov.template block(dim, dim); +} + +template +typename MTK::internal::CovBlock::Type +subblock(Eigen::Matrix &cov, + MTK::SubManifold Base::*) +{ + return cov.template block(idx, idx); +} + +template +class get_cov { +public: + typedef Eigen::Matrix type; + typedef const Eigen::Matrix const_type; +}; + +template +class get_cov_ { +public: + typedef Eigen::Matrix type; + typedef const Eigen::Matrix const_type; +}; + +template +class get_cross_cov { +public: + typedef Eigen::Matrix type; + typedef const type const_type; +}; + +template +class get_cross_cov_ { +public: + typedef Eigen::Matrix type; + typedef const type const_type; +}; + + +template +vectview +subvector_impl_(vectview vec, SubManifold Base::*) +{ + return vec.template segment(dim); +} + +template +vectview +subvector_impl(vectview vec, SubManifold Base::*) +{ + return vec.template segment(idx); +} + +/** + * Get the subvector corresponding to a sub-manifold from a bigger vector. + */ + template +vectview +subvector_(vectview vec, SubManifold Base::* ptr) +{ + return subvector_impl_(vec, ptr); +} + +template +vectview +subvector(vectview vec, SubManifold Base::* ptr) +{ + return subvector_impl(vec, ptr); +} + +/** + * @todo This should be covered already by subvector(vectview vec,SubManifold Base::*) + */ +template +vectview +subvector(Eigen::Matrix& vec, SubManifold Base::* ptr) +{ + return subvector_impl(vectview(vec), ptr); +} + +template +vectview +subvector_(Eigen::Matrix& vec, SubManifold Base::* ptr) +{ + return subvector_impl_(vectview(vec), ptr); +} + +template +vectview +subvector_(const Eigen::Matrix& vec, SubManifold Base::* ptr) +{ + return subvector_impl_(vectview(vec), ptr); +} + +template +vectview +subvector(const Eigen::Matrix& vec, SubManifold Base::* ptr) +{ + return subvector_impl(vectview(vec), ptr); +} + + +/** + * const version of subvector(vectview vec,SubManifold Base::*) + */ +template +vectview +subvector_impl(const vectview cvec, SubManifold Base::*) +{ + return cvec.template segment(idx); +} + +template +vectview +subvector_impl_(const vectview cvec, SubManifold Base::*) +{ + return cvec.template segment(dim); +} + +template +vectview +subvector(const vectview cvec, SubManifold Base::* ptr) +{ + return subvector_impl(cvec, ptr); +} + + +} // namespace MTK + +#endif // GET_START_INDEX_H_ diff --git a/src/FAST_LIO/include/IKFoM_toolkit/mtk/types/S2.hpp b/src/FAST_LIO/include/IKFoM_toolkit/mtk/types/S2.hpp new file mode 100755 index 0000000..789ebd9 --- /dev/null +++ b/src/FAST_LIO/include/IKFoM_toolkit/mtk/types/S2.hpp @@ -0,0 +1,316 @@ +// This is a NEW implementation of the algorithm described in the +// following paper: +// C. Hertzberg, R. Wagner, U. Frese, and L. Schroder. Integratinggeneric sensor fusion algorithms with sound state representationsthrough encapsulation of manifolds. +// CoRR, vol. abs/1107.1119, 2011.[Online]. Available: http://arxiv.org/abs/1107.1119 + +/* + * Copyright (c) 2019--2023, The University of Hong Kong + * All rights reserved. + * + * Modifier: Dongjiao HE + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the Universitaet Bremen nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/* + * Copyright (c) 2008--2011, Universitaet Bremen + * All rights reserved. + * + * Author: Christoph Hertzberg + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the Universitaet Bremen nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ +/** + * @file mtk/types/S2.hpp + * @brief Unit vectors on the sphere, or directions in 3D. + */ +#ifndef S2_H_ +#define S2_H_ + + +#include "vect.hpp" + +#include "SOn.hpp" +#include "../src/mtkmath.hpp" + + + + +namespace MTK { + +/** + * Manifold representation of @f$ S^2 @f$. + * Used for unit vectors on the sphere or directions in 3D. + * + * @todo add conversions from/to polar angles? + */ +template +struct S2 { + + typedef _scalar scalar; + typedef vect<3, scalar> vect_type; + typedef SO3 SO3_type; + typedef typename vect_type::base vec3; + scalar length = scalar(den)/scalar(num); + enum {DOF=2, TYP = 1, DIM = 3}; + +//private: + /** + * Unit vector on the sphere, or vector pointing in a direction + */ + vect_type vec; + +public: + S2() { + if(S2_typ == 3) vec=length * vec3(0, 0, std::sqrt(1)); + if(S2_typ == 2) vec=length * vec3(0, std::sqrt(1), 0); + if(S2_typ == 1) vec=length * vec3(std::sqrt(1), 0, 0); + } + S2(const scalar &x, const scalar &y, const scalar &z) : vec(vec3(x, y, z)) { + vec.normalize(); + vec = vec * length; + } + + S2(const vect_type &_vec) : vec(_vec) { + vec.normalize(); + vec = vec * length; + } + + void oplus(MTK::vectview delta, scalar scale = 1) + { + SO3_type res; + res.w() = MTK::exp(res.vec(), delta, scalar(scale/2)); + vec = res.toRotationMatrix() * vec; + } + + void boxplus(MTK::vectview delta, scalar scale=1) { + Eigen::Matrix Bx; + S2_Bx(Bx); + vect_type Bu = Bx*delta;SO3_type res; + res.w() = MTK::exp(res.vec(), Bu, scalar(scale/2)); + vec = res.toRotationMatrix() * vec; + } + + void boxminus(MTK::vectview res, const S2& other) const { + scalar v_sin = (MTK::hat(vec)*other.vec).norm(); + scalar v_cos = vec.transpose() * other.vec; + scalar theta = std::atan2(v_sin, v_cos); + if(v_sin < MTK::tolerance()) + { + if(std::fabs(theta) > MTK::tolerance() ) + { + res[0] = 3.1415926; + res[1] = 0; + } + else{ + res[0] = 0; + res[1] = 0; + } + } + else + { + S2 other_copy = other; + Eigen::MatrixBx; + other_copy.S2_Bx(Bx); + res = theta/v_sin * Bx.transpose() * MTK::hat(other.vec)*vec; + } + } + + void S2_hat(Eigen::Matrix &res) + { + Eigen::Matrix skew_vec; + skew_vec << scalar(0), -vec[2], vec[1], + vec[2], scalar(0), -vec[0], + -vec[1], vec[0], scalar(0); + res = skew_vec; + } + + + void S2_Bx(Eigen::Matrix &res) + { + if(S2_typ == 3) + { + if(vec[2] + length > tolerance()) + { + + res << length - vec[0]*vec[0]/(length+vec[2]), -vec[0]*vec[1]/(length+vec[2]), + -vec[0]*vec[1]/(length+vec[2]), length-vec[1]*vec[1]/(length+vec[2]), + -vec[0], -vec[1]; + res /= length; + } + else + { + res = Eigen::Matrix::Zero(); + res(1, 1) = -1; + res(2, 0) = 1; + } + } + else if(S2_typ == 2) + { + if(vec[1] + length > tolerance()) + { + + res << length - vec[0]*vec[0]/(length+vec[1]), -vec[0]*vec[2]/(length+vec[1]), + -vec[0], -vec[2], + -vec[0]*vec[2]/(length+vec[1]), length-vec[2]*vec[2]/(length+vec[1]); + res /= length; + } + else + { + res = Eigen::Matrix::Zero(); + res(1, 1) = -1; + res(2, 0) = 1; + } + } + else + { + if(vec[0] + length > tolerance()) + { + + res << -vec[1], -vec[2], + length - vec[1]*vec[1]/(length+vec[0]), -vec[2]*vec[1]/(length+vec[0]), + -vec[2]*vec[1]/(length+vec[0]), length-vec[2]*vec[2]/(length+vec[0]); + res /= length; + } + else + { + res = Eigen::Matrix::Zero(); + res(1, 1) = -1; + res(2, 0) = 1; + } + } + } + + void S2_Nx(Eigen::Matrix &res, S2& subtrahend) + { + if((vec+subtrahend.vec).norm() > tolerance()) + { + Eigen::Matrix Bx; + S2_Bx(Bx); + if((vec-subtrahend.vec).norm() > tolerance()) + { + scalar v_sin = (MTK::hat(vec)*subtrahend.vec).norm(); + scalar v_cos = vec.transpose() * subtrahend.vec; + + res = Bx.transpose() * (std::atan2(v_sin, v_cos)/v_sin*MTK::hat(vec)+MTK::hat(vec)*subtrahend.vec*((-v_cos/v_sin/v_sin/length/length/length/length+std::atan2(v_sin, v_cos)/v_sin/v_sin/v_sin)*subtrahend.vec.transpose()*MTK::hat(vec)*MTK::hat(vec)-vec.transpose()/length/length/length/length)); + } + else + { + res = 1/length/length*Bx.transpose()*MTK::hat(vec); + } + } + else + { + std::cerr << "No N(x, y) for x=-y" << std::endl; + std::exit(100); + } + } + + void S2_Nx_yy(Eigen::Matrix &res) + { + Eigen::Matrix Bx; + S2_Bx(Bx); + res = 1/length/length*Bx.transpose()*MTK::hat(vec); + } + + void S2_Mx(Eigen::Matrix &res, MTK::vectview delta) + { + Eigen::Matrix Bx; + S2_Bx(Bx); + if(delta.norm() < tolerance()) + { + res = -MTK::hat(vec)*Bx; + } + else{ + vect_type Bu = Bx*delta; + SO3_type exp_delta; + exp_delta.w() = MTK::exp(exp_delta.vec(), Bu, scalar(1/2)); + res = -exp_delta.toRotationMatrix()*MTK::hat(vec)*MTK::A_matrix(Bu).transpose()*Bx; + } + } + + operator const vect_type&() const{ + return vec; + } + + const vect_type& get_vect() const { + return vec; + } + + friend S2 operator*(const SO3& rot, const S2& dir) + { + S2 ret; + ret.vec = rot * dir.vec; + return ret; + } + + scalar operator[](int idx) const {return vec[idx]; } + + friend std::ostream& operator<<(std::ostream &os, const S2& vec){ + return os << vec.vec.transpose() << " "; + } + friend std::istream& operator>>(std::istream &is, S2& vec){ + for(int i=0; i<3; ++i) + is >> vec.vec[i]; + vec.vec.normalize(); + vec.vec = vec.vec * vec.length; + return is; + + } +}; + + +} // namespace MTK + + +#endif /*S2_H_*/ diff --git a/src/FAST_LIO/include/IKFoM_toolkit/mtk/types/SOn.hpp b/src/FAST_LIO/include/IKFoM_toolkit/mtk/types/SOn.hpp new file mode 100755 index 0000000..31005e6 --- /dev/null +++ b/src/FAST_LIO/include/IKFoM_toolkit/mtk/types/SOn.hpp @@ -0,0 +1,317 @@ +// This is an advanced implementation of the algorithm described in the +// following paper: +// C. Hertzberg, R. Wagner, U. Frese, and L. Schroder. Integratinggeneric sensor fusion algorithms with sound state representationsthrough encapsulation of manifolds. +// CoRR, vol. abs/1107.1119, 2011.[Online]. Available: http://arxiv.org/abs/1107.1119 + +/* + * Copyright (c) 2019--2023, The University of Hong Kong + * All rights reserved. + * + * Modifier: Dongjiao HE + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the Universitaet Bremen nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/* + * Copyright (c) 2008--2011, Universitaet Bremen + * All rights reserved. + * + * Author: Christoph Hertzberg + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the Universitaet Bremen nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ +/** + * @file mtk/types/SOn.hpp + * @brief Standard Orthogonal Groups i.e.\ rotatation groups. + */ +#ifndef SON_H_ +#define SON_H_ + +#include + +#include "vect.hpp" +#include "../src/mtkmath.hpp" + + +namespace MTK { + + +/** + * Two-dimensional orientations represented as scalar. + * There is no guarantee that the representing scalar is within any interval, + * but the result of boxminus will always have magnitude @f$\le\pi @f$. + */ +template +struct SO2 : public Eigen::Rotation2D<_scalar> { + enum {DOF = 1, DIM = 2, TYP = 3}; + + typedef _scalar scalar; + typedef Eigen::Rotation2D base; + typedef vect vect_type; + + //! Construct from angle + SO2(const scalar& angle = 0) : base(angle) { } + + //! Construct from Eigen::Rotation2D + SO2(const base& src) : base(src) {} + + /** + * Construct from 2D vector. + * Resulting orientation will rotate the first unit vector to point to vec. + */ + SO2(const vect_type &vec) : base(atan2(vec[1], vec[0])) {}; + + + //! Calculate @c this->inverse() * @c r + SO2 operator%(const base &r) const { + return base::inverse() * r; + } + + //! Calculate @c this->inverse() * @c r + template + vect_type operator%(const Eigen::MatrixBase &vec) const { + return base::inverse() * vec; + } + + //! Calculate @c *this * @c r.inverse() + SO2 operator/(const SO2 &r) const { + return *this * r.inverse(); + } + + //! Gets the angle as scalar. + operator scalar() const { + return base::angle(); + } + void S2_hat(Eigen::Matrix &res) + { + res = Eigen::Matrix::Zero(); + } + //! @name Manifold requirements + void S2_Nx_yy(Eigen::Matrix &res) + { + std::cerr << "wrong idx for S2" << std::endl; + std::exit(100); + res = Eigen::Matrix::Zero(); + } + + void S2_Mx(Eigen::Matrix &res, MTK::vectview delta) + { + std::cerr << "wrong idx for S2" << std::endl; + std::exit(100); + res = Eigen::Matrix::Zero(); + } + + void oplus(MTK::vectview vec, scalar scale = 1) { + base::angle() += scale * vec[0]; + } + + void boxplus(MTK::vectview vec, scalar scale = 1) { + base::angle() += scale * vec[0]; + } + void boxminus(MTK::vectview res, const SO2& other) const { + res[0] = MTK::normalize(base::angle() - other.angle(), scalar(MTK::pi)); + } + + friend std::istream& operator>>(std::istream &is, SO2& ang){ + return is >> ang.angle(); + } + +}; + + +/** + * Three-dimensional orientations represented as Quaternion. + * It is assumed that the internal Quaternion always stays normalized, + * should this not be the case, call inherited member function @c normalize(). + */ +template +struct SO3 : public Eigen::Quaternion<_scalar, Options> { + enum {DOF = 3, DIM = 3, TYP = 2}; + typedef _scalar scalar; + typedef Eigen::Quaternion base; + typedef Eigen::Quaternion Quaternion; + typedef vect vect_type; + + //! Calculate @c this->inverse() * @c r + template EIGEN_STRONG_INLINE + Quaternion operator%(const Eigen::QuaternionBase &r) const { + return base::conjugate() * r; + } + + //! Calculate @c this->inverse() * @c r + template + vect_type operator%(const Eigen::MatrixBase &vec) const { + return base::conjugate() * vec; + } + + //! Calculate @c this * @c r.conjugate() + template EIGEN_STRONG_INLINE + Quaternion operator/(const Eigen::QuaternionBase &r) const { + return *this * r.conjugate(); + } + + /** + * Construct from real part and three imaginary parts. + * Quaternion is normalized after construction. + */ + SO3(const scalar& w, const scalar& x, const scalar& y, const scalar& z) : base(w, x, y, z) { + base::normalize(); + } + + /** + * Construct from Eigen::Quaternion. + * @note Non-normalized input may result result in spurious behavior. + */ + SO3(const base& src = base::Identity()) : base(src) {} + + /** + * Construct from rotation matrix. + * @note Invalid rotation matrices may lead to spurious behavior. + */ + template + SO3(const Eigen::MatrixBase& matrix) : base(matrix) {} + + /** + * Construct from arbitrary rotation type. + * @note Invalid rotation matrices may lead to spurious behavior. + */ + template + SO3(const Eigen::RotationBase& rotation) : base(rotation.derived()) {} + + //! @name Manifold requirements + + void boxplus(MTK::vectview vec, scalar scale=1) { + SO3 delta = exp(vec, scale); + *this = *this * delta; + } + void boxminus(MTK::vectview res, const SO3& other) const { + res = SO3::log(other.conjugate() * *this); + } + //} + + void oplus(MTK::vectview vec, scalar scale=1) { + SO3 delta = exp(vec, scale); + *this = *this * delta; + } + + void S2_hat(Eigen::Matrix &res) + { + res = Eigen::Matrix::Zero(); + } + void S2_Nx_yy(Eigen::Matrix &res) + { + std::cerr << "wrong idx for S2" << std::endl; + std::exit(100); + res = Eigen::Matrix::Zero(); + } + + void S2_Mx(Eigen::Matrix &res, MTK::vectview delta) + { + std::cerr << "wrong idx for S2" << std::endl; + std::exit(100); + res = Eigen::Matrix::Zero(); + } + + friend std::ostream& operator<<(std::ostream &os, const SO3& q){ + return os << q.coeffs().transpose() << " "; + } + + friend std::istream& operator>>(std::istream &is, SO3& q){ + vect<4,scalar> coeffs; + is >> coeffs; + q.coeffs() = coeffs.normalized(); + return is; + } + + //! @name Helper functions + //{ + /** + * Calculate the exponential map. In matrix terms this would correspond + * to the Rodrigues formula. + */ + // FIXME vectview<> can't be constructed from every MatrixBase<>, use const Vector3x& as workaround +// static SO3 exp(MTK::vectview dvec, scalar scale = 1){ + static SO3 exp(const Eigen::Matrix& dvec, scalar scale = 1){ + SO3 res; + res.w() = MTK::exp(res.vec(), dvec, scalar(scale/2)); + return res; + } + /** + * Calculate the inverse of @c exp. + * Only guarantees that exp(log(x)) == x + */ + static typename base::Vector3 log(const SO3 &orient){ + typename base::Vector3 res; + MTK::log(res, orient.w(), orient.vec(), scalar(2), true); + return res; + } +}; + +namespace internal { +template +struct UnalignedType >{ + typedef SO2 type; +}; + +template +struct UnalignedType >{ + typedef SO3 type; +}; + +} // namespace internal + + +} // namespace MTK + +#endif /*SON_H_*/ + diff --git a/src/FAST_LIO/include/IKFoM_toolkit/mtk/types/vect.hpp b/src/FAST_LIO/include/IKFoM_toolkit/mtk/types/vect.hpp new file mode 100755 index 0000000..0e5b9ce --- /dev/null +++ b/src/FAST_LIO/include/IKFoM_toolkit/mtk/types/vect.hpp @@ -0,0 +1,461 @@ +// This is an advanced implementation of the algorithm described in the +// following paper: +// C. Hertzberg, R. Wagner, U. Frese, and L. Schroder. Integratinggeneric sensor fusion algorithms with sound state representationsthrough encapsulation of manifolds. +// CoRR, vol. abs/1107.1119, 2011.[Online]. Available: http://arxiv.org/abs/1107.1119 + +/* + * Copyright (c) 2019--2023, The University of Hong Kong + * All rights reserved. + * + * Modifier: Dongjiao HE + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the Universitaet Bremen nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/* + * Copyright (c) 2008--2011, Universitaet Bremen + * All rights reserved. + * + * Author: Christoph Hertzberg + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the Universitaet Bremen nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ +/** + * @file mtk/types/vect.hpp + * @brief Basic vectors interpreted as manifolds. + * + * This file also implements a simple wrapper for matrices, for arbitrary scalars + * and for positive scalars. + */ +#ifndef VECT_H_ +#define VECT_H_ + +#include +#include +#include + +#include "../src/vectview.hpp" + +namespace MTK { + +static const Eigen::IOFormat IO_no_spaces(Eigen::StreamPrecision, Eigen::DontAlignCols, ",", ",", "", "", "[", "]"); + + +/** + * A simple vector class. + * Implementation is basically a wrapper around Eigen::Matrix with manifold + * requirements added. + */ +template +struct vect : public Eigen::Matrix<_scalar, D, 1, _Options> { + typedef Eigen::Matrix<_scalar, D, 1, _Options> base; + enum {DOF = D, DIM = D, TYP = 0}; + typedef _scalar scalar; + + //using base::operator=; + + /** Standard constructor. Sets all values to zero. */ + vect(const base &src = base::Zero()) : base(src) {} + + /** Constructor copying the value of the expression \a other */ + template + EIGEN_STRONG_INLINE vect(const Eigen::DenseBase& other) : base(other) {} + + /** Construct from memory. */ + vect(const scalar* src, int size = DOF) : base(base::Map(src, size)) { } + + void boxplus(MTK::vectview vec, scalar scale=1) { + *this += scale * vec; + } + void boxminus(MTK::vectview res, const vect& other) const { + res = *this - other; + } + + void oplus(MTK::vectview vec, scalar scale=1) { + *this += scale * vec; + } + + void S2_hat(Eigen::Matrix &res) + { + res = Eigen::Matrix::Zero(); + } + + void S2_Nx_yy(Eigen::Matrix &res) + { + std::cerr << "wrong idx for S2" << std::endl; + std::exit(100); + res = Eigen::Matrix::Zero(); + } + + void S2_Mx(Eigen::Matrix &res, MTK::vectview delta) + { + std::cerr << "wrong idx for S2" << std::endl; + std::exit(100); + res = Eigen::Matrix::Zero(); + } + + friend std::ostream& operator<<(std::ostream &os, const vect& v){ + // Eigen sometimes messes with the streams flags, so output manually: + for(int i=0; i>(std::istream &is, vect& v){ + char term=0; + is >> std::ws; // skip whitespace + switch(is.peek()) { + case '(': term=')'; is.ignore(1); break; + case '[': term=']'; is.ignore(1); break; + case '{': term='}'; is.ignore(1); break; + default: break; + } + if(D==Eigen::Dynamic) { + assert(term !=0 && "Dynamic vectors must be embraced"); + std::vector temp; + while(is.good() && is.peek() != term) { + scalar x; + is >> x; + temp.push_back(x); + if(is.peek()==',') is.ignore(1); + } + v = vect::Map(temp.data(), temp.size()); + } else + for(int i=0; i> v[i]; + if(is.peek()==',') { // ignore commas between values + is.ignore(1); + } + } + if(term!=0) { + char x; + is >> x; + if(x!=term) { + is.setstate(is.badbit); +// assert(x==term && "start and end bracket do not match!"); + } + } + return is; + } + + template + vectview tail(){ + BOOST_STATIC_ASSERT(0< dim && dim <= DOF); + return base::template tail(); + } + template + vectview tail() const{ + BOOST_STATIC_ASSERT(0< dim && dim <= DOF); + return base::template tail(); + } + template + vectview head(){ + BOOST_STATIC_ASSERT(0< dim && dim <= DOF); + return base::template head(); + } + template + vectview head() const{ + BOOST_STATIC_ASSERT(0< dim && dim <= DOF); + return base::template head(); + } +}; + + +/** + * A simple matrix class. + * Implementation is basically a wrapper around Eigen::Matrix with manifold + * requirements added, i.e., matrix is viewed as a plain vector for that. + */ +template::Options> +struct matrix : public Eigen::Matrix<_scalar, M, N, _Options> { + typedef Eigen::Matrix<_scalar, M, N, _Options> base; + enum {DOF = M * N, TYP = 4, DIM=0}; + typedef _scalar scalar; + + using base::operator=; + + /** Standard constructor. Sets all values to zero. */ + matrix() { + base::setZero(); + } + + /** Constructor copying the value of the expression \a other */ + template + EIGEN_STRONG_INLINE matrix(const Eigen::MatrixBase& other) : base(other) {} + + /** Construct from memory. */ + matrix(const scalar* src) : base(src) { } + + void boxplus(MTK::vectview vec, scalar scale = 1) { + *this += scale * base::Map(vec.data()); + } + void boxminus(MTK::vectview res, const matrix& other) const { + base::Map(res.data()) = *this - other; + } + + void S2_hat(Eigen::Matrix &res) + { + res = Eigen::Matrix::Zero(); + } + + void oplus(MTK::vectview vec, scalar scale = 1) { + *this += scale * base::Map(vec.data()); + } + + void S2_Nx_yy(Eigen::Matrix &res) + { + std::cerr << "wrong idx for S2" << std::endl; + std::exit(100); + res = Eigen::Matrix::Zero(); + } + + void S2_Mx(Eigen::Matrix &res, MTK::vectview delta) + { + std::cerr << "wrong idx for S2" << std::endl; + std::exit(100); + res = Eigen::Matrix::Zero(); + } + + friend std::ostream& operator<<(std::ostream &os, const matrix& mat){ + for(int i=0; i>(std::istream &is, matrix& mat){ + for(int i=0; i> mat.data()[i]; + } + return is; + } +};// @todo What if M / N = Eigen::Dynamic? + + + +/** + * A simple scalar type. + */ +template +struct Scalar { + enum {DOF = 1, TYP = 5, DIM=0}; + typedef _scalar scalar; + + scalar value; + + Scalar(const scalar& value = scalar(0)) : value(value) {} + operator const scalar&() const { return value; } + operator scalar&() { return value; } + Scalar& operator=(const scalar& val) { value = val; return *this; } + + void S2_hat(Eigen::Matrix &res) + { + res = Eigen::Matrix::Zero(); + } + + void S2_Nx_yy(Eigen::Matrix &res) + { + std::cerr << "wrong idx for S2" << std::endl; + std::exit(100); + res = Eigen::Matrix::Zero(); + } + + void S2_Mx(Eigen::Matrix &res, MTK::vectview delta) + { + std::cerr << "wrong idx for S2" << std::endl; + std::exit(100); + res = Eigen::Matrix::Zero(); + } + + void oplus(MTK::vectview vec, scalar scale=1) { + value += scale * vec[0]; + } + + void boxplus(MTK::vectview vec, scalar scale=1) { + value += scale * vec[0]; + } + void boxminus(MTK::vectview res, const Scalar& other) const { + res[0] = *this - other; + } +}; + +/** + * Positive scalars. + * Boxplus is implemented using multiplication by @f$x\boxplus\delta = x\cdot\exp(\delta) @f$. + */ +template +struct PositiveScalar { + enum {DOF = 1, TYP = 6, DIM=0}; + typedef _scalar scalar; + + scalar value; + + PositiveScalar(const scalar& value = scalar(1)) : value(value) { + assert(value > scalar(0)); + } + operator const scalar&() const { return value; } + PositiveScalar& operator=(const scalar& val) { assert(val>0); value = val; return *this; } + + void boxplus(MTK::vectview vec, scalar scale = 1) { + value *= std::exp(scale * vec[0]); + } + void boxminus(MTK::vectview res, const PositiveScalar& other) const { + res[0] = std::log(*this / other); + } + + void oplus(MTK::vectview vec, scalar scale = 1) { + value *= std::exp(scale * vec[0]); + } + + void S2_hat(Eigen::Matrix &res) + { + res = Eigen::Matrix::Zero(); + } + + void S2_Nx_yy(Eigen::Matrix &res) + { + std::cerr << "wrong idx for S2" << std::endl; + std::exit(100); + res = Eigen::Matrix::Zero(); + } + + void S2_Mx(Eigen::Matrix &res, MTK::vectview delta) + { + std::cerr << "wrong idx for S2" << std::endl; + std::exit(100); + res = Eigen::Matrix::Zero(); + } + + + friend std::istream& operator>>(std::istream &is, PositiveScalar& s){ + is >> s.value; + assert(s.value > 0); + return is; + } +}; + +template +struct Complex : public std::complex<_scalar>{ + enum {DOF = 2, TYP = 7, DIM=0}; + typedef _scalar scalar; + + typedef std::complex Base; + + Complex(const Base& value) : Base(value) {} + Complex(const scalar& re = 0.0, const scalar& im = 0.0) : Base(re, im) {} + Complex(const MTK::vectview &in) : Base(in[0], in[1]) {} + template + Complex(const Eigen::DenseBase &in) : Base(in[0], in[1]) {} + + void boxplus(MTK::vectview vec, scalar scale = 1) { + Base::real() += scale * vec[0]; + Base::imag() += scale * vec[1]; + }; + void boxminus(MTK::vectview res, const Complex& other) const { + Complex diff = *this - other; + res << diff.real(), diff.imag(); + } + + void S2_hat(Eigen::Matrix &res) + { + res = Eigen::Matrix::Zero(); + } + + void oplus(MTK::vectview vec, scalar scale = 1) { + Base::real() += scale * vec[0]; + Base::imag() += scale * vec[1]; + }; + + void S2_Nx_yy(Eigen::Matrix &res) + { + std::cerr << "wrong idx for S2" << std::endl; + std::exit(100); + res = Eigen::Matrix::Zero(); + } + + void S2_Mx(Eigen::Matrix &res, MTK::vectview delta) + { + std::cerr << "wrong idx for S2" << std::endl; + std::exit(100); + res = Eigen::Matrix::Zero(); + } + + scalar squaredNorm() const { + return std::pow(Base::real(),2) + std::pow(Base::imag(),2); + } + + const scalar& operator()(int i) const { + assert(0<=i && i<2 && "Index out of range"); + return i==0 ? Base::real() : Base::imag(); + } + scalar& operator()(int i){ + assert(0<=i && i<2 && "Index out of range"); + return i==0 ? Base::real() : Base::imag(); + } +}; + + +namespace internal { + +template +struct UnalignedType >{ + typedef vect type; +}; + +} // namespace internal + + +} // namespace MTK + + + + +#endif /*VECT_H_*/ diff --git a/src/FAST_LIO/include/IKFoM_toolkit/mtk/types/wrapped_cv_mat.hpp b/src/FAST_LIO/include/IKFoM_toolkit/mtk/types/wrapped_cv_mat.hpp new file mode 100755 index 0000000..b6643f1 --- /dev/null +++ b/src/FAST_LIO/include/IKFoM_toolkit/mtk/types/wrapped_cv_mat.hpp @@ -0,0 +1,113 @@ +/* + * Copyright (c) 2010--2011, Universitaet Bremen and DFKI GmbH + * All rights reserved. + * + * Author: Rene Wagner + * Christoph Hertzberg + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the Universitaet Bremen nor the DFKI GmbH + * nor the names of its contributors may be used to endorse or + * promote products derived from this software without specific + * prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#ifndef WRAPPED_CV_MAT_HPP_ +#define WRAPPED_CV_MAT_HPP_ + +#include +#include + +namespace MTK { + +template +struct cv_f_type; + +template<> +struct cv_f_type +{ + enum {value = CV_64F}; +}; + +template<> +struct cv_f_type +{ + enum {value = CV_32F}; +}; + +/** + * cv_mat wraps a CvMat around an Eigen Matrix + */ +template +class cv_mat : public matrix +{ + typedef matrix base_type; + enum {type_ = cv_f_type::value}; + CvMat cv_mat_; + +public: + cv_mat() + { + cv_mat_ = cvMat(rows, cols, type_, base_type::data()); + } + + cv_mat(const cv_mat& oth) : base_type(oth) + { + cv_mat_ = cvMat(rows, cols, type_, base_type::data()); + } + + template + cv_mat(const Eigen::MatrixBase &value) : base_type(value) + { + cv_mat_ = cvMat(rows, cols, type_, base_type::data()); + } + + template + cv_mat& operator=(const Eigen::MatrixBase &value) + { + base_type::operator=(value); + return *this; + } + + cv_mat& operator=(const cv_mat& value) + { + base_type::operator=(value); + return *this; + } + + // FIXME: Maybe overloading operator& is not a good idea ... + CvMat* operator&() + { + return &cv_mat_; + } + const CvMat* operator&() const + { + return &cv_mat_; + } +}; + +} // namespace MTK + +#endif /* WRAPPED_CV_MAT_HPP_ */ diff --git a/src/FAST_LIO/include/common_lib.h b/src/FAST_LIO/include/common_lib.h new file mode 100644 index 0000000..82a57cb --- /dev/null +++ b/src/FAST_LIO/include/common_lib.h @@ -0,0 +1,259 @@ +#ifndef COMMON_LIB_H +#define COMMON_LIB_H + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +using namespace std; +using namespace Eigen; + +#define USE_IKFOM + +#define PI_M (3.14159265358) +#define G_m_s2 (9.81) // Gravaty const in GuangDong/China +#define DIM_STATE (18) // Dimension of states (Let Dim(SO(3)) = 3) +#define DIM_PROC_N (12) // Dimension of process noise (Let Dim(SO(3)) = 3) +#define CUBE_LEN (6.0) +#define LIDAR_SP_LEN (2) +#define INIT_COV (1) +#define NUM_MATCH_POINTS (5) +#define MAX_MEAS_DIM (10000) + +#define VEC_FROM_ARRAY(v) v[0],v[1],v[2] +#define MAT_FROM_ARRAY(v) v[0],v[1],v[2],v[3],v[4],v[5],v[6],v[7],v[8] +#define CONSTRAIN(v,min,max) ((v>min)?((v (mat.data(), mat.data() + mat.rows() * mat.cols()) +#define DEBUG_FILE_DIR(name) (string(string(ROOT_DIR) + "Log/"+ name)) + +typedef fast_lio::Pose6D Pose6D; +typedef pcl::PointXYZINormal PointType; +typedef pcl::PointCloud PointCloudXYZI; +typedef vector> PointVector; +typedef Vector3d V3D; +typedef Matrix3d M3D; +typedef Vector3f V3F; +typedef Matrix3f M3F; + +#define MD(a,b) Matrix +#define VD(a) Matrix +#define MF(a,b) Matrix +#define VF(a) Matrix + +M3D Eye3d(M3D::Identity()); +M3F Eye3f(M3F::Identity()); +V3D Zero3d(0, 0, 0); +V3F Zero3f(0, 0, 0); + +struct MeasureGroup // Lidar data and imu dates for the curent process +{ + MeasureGroup() + { + lidar_beg_time = 0.0; + this->lidar.reset(new PointCloudXYZI()); + }; + double lidar_beg_time; + double lidar_end_time; + PointCloudXYZI::Ptr lidar; + deque imu; +}; + +struct StatesGroup +{ + StatesGroup() { + this->rot_end = M3D::Identity(); + this->pos_end = Zero3d; + this->vel_end = Zero3d; + this->bias_g = Zero3d; + this->bias_a = Zero3d; + this->gravity = Zero3d; + this->cov = MD(DIM_STATE,DIM_STATE)::Identity() * INIT_COV; + this->cov.block<9,9>(9,9) = MD(9,9)::Identity() * 0.00001; + }; + + StatesGroup(const StatesGroup& b) { + this->rot_end = b.rot_end; + this->pos_end = b.pos_end; + this->vel_end = b.vel_end; + this->bias_g = b.bias_g; + this->bias_a = b.bias_a; + this->gravity = b.gravity; + this->cov = b.cov; + }; + + StatesGroup& operator=(const StatesGroup& b) + { + this->rot_end = b.rot_end; + this->pos_end = b.pos_end; + this->vel_end = b.vel_end; + this->bias_g = b.bias_g; + this->bias_a = b.bias_a; + this->gravity = b.gravity; + this->cov = b.cov; + return *this; + }; + + StatesGroup operator+(const Matrix &state_add) + { + StatesGroup a; + a.rot_end = this->rot_end * Exp(state_add(0,0), state_add(1,0), state_add(2,0)); + a.pos_end = this->pos_end + state_add.block<3,1>(3,0); + a.vel_end = this->vel_end + state_add.block<3,1>(6,0); + a.bias_g = this->bias_g + state_add.block<3,1>(9,0); + a.bias_a = this->bias_a + state_add.block<3,1>(12,0); + a.gravity = this->gravity + state_add.block<3,1>(15,0); + a.cov = this->cov; + return a; + }; + + StatesGroup& operator+=(const Matrix &state_add) + { + this->rot_end = this->rot_end * Exp(state_add(0,0), state_add(1,0), state_add(2,0)); + this->pos_end += state_add.block<3,1>(3,0); + this->vel_end += state_add.block<3,1>(6,0); + this->bias_g += state_add.block<3,1>(9,0); + this->bias_a += state_add.block<3,1>(12,0); + this->gravity += state_add.block<3,1>(15,0); + return *this; + }; + + Matrix operator-(const StatesGroup& b) + { + Matrix a; + M3D rotd(b.rot_end.transpose() * this->rot_end); + a.block<3,1>(0,0) = Log(rotd); + a.block<3,1>(3,0) = this->pos_end - b.pos_end; + a.block<3,1>(6,0) = this->vel_end - b.vel_end; + a.block<3,1>(9,0) = this->bias_g - b.bias_g; + a.block<3,1>(12,0) = this->bias_a - b.bias_a; + a.block<3,1>(15,0) = this->gravity - b.gravity; + return a; + }; + + void resetpose() + { + this->rot_end = M3D::Identity(); + this->pos_end = Zero3d; + this->vel_end = Zero3d; + } + + M3D rot_end; // the estimated attitude (rotation matrix) at the end lidar point + V3D pos_end; // the estimated position at the end lidar point (world frame) + V3D vel_end; // the estimated velocity at the end lidar point (world frame) + V3D bias_g; // gyroscope bias + V3D bias_a; // accelerator bias + V3D gravity; // the estimated gravity acceleration + Matrix cov; // states covariance +}; + +template +T rad2deg(T radians) +{ + return radians * 180.0 / PI_M; +} + +template +T deg2rad(T degrees) +{ + return degrees * PI_M / 180.0; +} + +template +auto set_pose6d(const double t, const Matrix &a, const Matrix &g, \ + const Matrix &v, const Matrix &p, const Matrix &R) +{ + Pose6D rot_kp; + rot_kp.offset_time = t; + for (int i = 0; i < 3; i++) + { + rot_kp.acc[i] = a(i); + rot_kp.gyr[i] = g(i); + rot_kp.vel[i] = v(i); + rot_kp.pos[i] = p(i); + for (int j = 0; j < 3; j++) rot_kp.rot[i*3+j] = R(i,j); + } + return move(rot_kp); +} + +/* comment +plane equation: Ax + By + Cz + D = 0 +convert to: A/D*x + B/D*y + C/D*z = -1 +solve: A0*x0 = b0 +where A0_i = [x_i, y_i, z_i], x0 = [A/D, B/D, C/D]^T, b0 = [-1, ..., -1]^T +normvec: normalized x0 +*/ +template +bool esti_normvector(Matrix &normvec, const PointVector &point, const T &threshold, const int &point_num) +{ + MatrixXf A(point_num, 3); + MatrixXf b(point_num, 1); + b.setOnes(); + b *= -1.0f; + + for (int j = 0; j < point_num; j++) + { + A(j,0) = point[j].x; + A(j,1) = point[j].y; + A(j,2) = point[j].z; + } + normvec = A.colPivHouseholderQr().solve(b); + + for (int j = 0; j < point_num; j++) + { + if (fabs(normvec(0) * point[j].x + normvec(1) * point[j].y + normvec(2) * point[j].z + 1.0f) > threshold) + { + return false; + } + } + + normvec.normalize(); + return true; +} + +float calc_dist(PointType p1, PointType p2){ + float d = (p1.x - p2.x) * (p1.x - p2.x) + (p1.y - p2.y) * (p1.y - p2.y) + (p1.z - p2.z) * (p1.z - p2.z); + return d; +} + +template +bool esti_plane(Matrix &pca_result, const PointVector &point, const T &threshold) +{ + Matrix A; + Matrix b; + A.setZero(); + b.setOnes(); + b *= -1.0f; + + for (int j = 0; j < NUM_MATCH_POINTS; j++) + { + A(j,0) = point[j].x; + A(j,1) = point[j].y; + A(j,2) = point[j].z; + } + + Matrix normvec = A.colPivHouseholderQr().solve(b); + + T n = normvec.norm(); + pca_result(0) = normvec(0) / n; + pca_result(1) = normvec(1) / n; + pca_result(2) = normvec(2) / n; + pca_result(3) = 1.0 / n; + + for (int j = 0; j < NUM_MATCH_POINTS; j++) + { + if (fabs(pca_result(0) * point[j].x + pca_result(1) * point[j].y + pca_result(2) * point[j].z + pca_result(3)) > threshold) + { + return false; + } + } + return true; +} + +#endif \ No newline at end of file diff --git a/src/FAST_LIO/include/ikd-Tree/README.md b/src/FAST_LIO/include/ikd-Tree/README.md new file mode 100644 index 0000000..e113a91 --- /dev/null +++ b/src/FAST_LIO/include/ikd-Tree/README.md @@ -0,0 +1,2 @@ +# ikd-Tree +ikd-Tree is an incremental k-d tree for robotic applications. diff --git a/src/FAST_LIO/include/ikd-Tree/ikd_Tree.cpp b/src/FAST_LIO/include/ikd-Tree/ikd_Tree.cpp new file mode 100644 index 0000000..e8c4e86 --- /dev/null +++ b/src/FAST_LIO/include/ikd-Tree/ikd_Tree.cpp @@ -0,0 +1,1728 @@ +#include "ikd_Tree.h" + +/* +Description: ikd-Tree: an incremental k-d tree for robotic applications +Author: Yixi Cai +email: yixicai@connect.hku.hk +*/ + +template +KD_TREE::KD_TREE(float delete_param, float balance_param, float box_length) +{ + delete_criterion_param = delete_param; + balance_criterion_param = balance_param; + downsample_size = box_length; + Rebuild_Logger.clear(); + termination_flag = false; + start_thread(); +} + +template +KD_TREE::~KD_TREE() +{ + stop_thread(); + Delete_Storage_Disabled = true; + delete_tree_nodes(&Root_Node); + PointVector().swap(PCL_Storage); + Rebuild_Logger.clear(); +} + + + +template +void KD_TREE::InitializeKDTree(float delete_param, float balance_param, float box_length) +{ + Set_delete_criterion_param(delete_param); + Set_balance_criterion_param(balance_param); + set_downsample_param(box_length); +} + +template +void KD_TREE::InitTreeNode(KD_TREE_NODE *root) +{ + root->point.x = 0.0f; + root->point.y = 0.0f; + root->point.z = 0.0f; + root->node_range_x[0] = 0.0f; + root->node_range_x[1] = 0.0f; + root->node_range_y[0] = 0.0f; + root->node_range_y[1] = 0.0f; + root->node_range_z[0] = 0.0f; + root->node_range_z[1] = 0.0f; + root->radius_sq = 0.0f; + root->division_axis = 0; + root->father_ptr = nullptr; + root->left_son_ptr = nullptr; + root->right_son_ptr = nullptr; + root->TreeSize = 0; + root->invalid_point_num = 0; + root->down_del_num = 0; + root->point_deleted = false; + root->tree_deleted = false; + root->need_push_down_to_left = false; + root->need_push_down_to_right = false; + root->point_downsample_deleted = false; + root->working_flag = false; + pthread_mutex_init(&(root->push_down_mutex_lock), NULL); +} + +template +int KD_TREE::size() +{ + int s = 0; + if (Rebuild_Ptr == nullptr || *Rebuild_Ptr != Root_Node) + { + if (Root_Node != nullptr) + { + return Root_Node->TreeSize; + } + else + { + return 0; + } + } + else + { + if (!pthread_mutex_trylock(&working_flag_mutex)) + { + s = Root_Node->TreeSize; + pthread_mutex_unlock(&working_flag_mutex); + return s; + } + else + { + return Treesize_tmp; + } + } +} + +template +BoxPointType KD_TREE::tree_range() +{ + BoxPointType range; + if (Rebuild_Ptr == nullptr || *Rebuild_Ptr != Root_Node) + { + if (Root_Node != nullptr) + { + range.vertex_min[0] = Root_Node->node_range_x[0]; + range.vertex_min[1] = Root_Node->node_range_y[0]; + range.vertex_min[2] = Root_Node->node_range_z[0]; + range.vertex_max[0] = Root_Node->node_range_x[1]; + range.vertex_max[1] = Root_Node->node_range_y[1]; + range.vertex_max[2] = Root_Node->node_range_z[1]; + } + else + { + memset(&range, 0, sizeof(range)); + } + } + else + { + if (!pthread_mutex_trylock(&working_flag_mutex)) + { + range.vertex_min[0] = Root_Node->node_range_x[0]; + range.vertex_min[1] = Root_Node->node_range_y[0]; + range.vertex_min[2] = Root_Node->node_range_z[0]; + range.vertex_max[0] = Root_Node->node_range_x[1]; + range.vertex_max[1] = Root_Node->node_range_y[1]; + range.vertex_max[2] = Root_Node->node_range_z[1]; + pthread_mutex_unlock(&working_flag_mutex); + } + else + { + memset(&range, 0, sizeof(range)); + } + } + return range; +} + +template +int KD_TREE::validnum() +{ + int s = 0; + if (Rebuild_Ptr == nullptr || *Rebuild_Ptr != Root_Node) + { + if (Root_Node != nullptr) + return (Root_Node->TreeSize - Root_Node->invalid_point_num); + else + return 0; + } + else + { + if (!pthread_mutex_trylock(&working_flag_mutex)) + { + s = Root_Node->TreeSize - Root_Node->invalid_point_num; + pthread_mutex_unlock(&working_flag_mutex); + return s; + } + else + { + return -1; + } + } +} + +template +void KD_TREE::root_alpha(float &alpha_bal, float &alpha_del) +{ + if (Rebuild_Ptr == nullptr || *Rebuild_Ptr != Root_Node) + { + alpha_bal = Root_Node->alpha_bal; + alpha_del = Root_Node->alpha_del; + return; + } + else + { + if (!pthread_mutex_trylock(&working_flag_mutex)) + { + alpha_bal = Root_Node->alpha_bal; + alpha_del = Root_Node->alpha_del; + pthread_mutex_unlock(&working_flag_mutex); + return; + } + else + { + alpha_bal = alpha_bal_tmp; + alpha_del = alpha_del_tmp; + return; + } + } +} + +template +void KD_TREE::start_thread() +{ + pthread_mutex_init(&termination_flag_mutex_lock, NULL); + pthread_mutex_init(&rebuild_ptr_mutex_lock, NULL); + pthread_mutex_init(&rebuild_logger_mutex_lock, NULL); + pthread_mutex_init(&points_deleted_rebuild_mutex_lock, NULL); + pthread_mutex_init(&working_flag_mutex, NULL); + pthread_mutex_init(&search_flag_mutex, NULL); + pthread_create(&rebuild_thread, NULL, multi_thread_ptr, (void *)this); + printf("Multi thread started \n"); +} + +template +void KD_TREE::stop_thread() +{ + pthread_mutex_lock(&termination_flag_mutex_lock); + termination_flag = true; + pthread_mutex_unlock(&termination_flag_mutex_lock); + if (rebuild_thread) + pthread_join(rebuild_thread, NULL); + pthread_mutex_destroy(&termination_flag_mutex_lock); + pthread_mutex_destroy(&rebuild_logger_mutex_lock); + pthread_mutex_destroy(&rebuild_ptr_mutex_lock); + pthread_mutex_destroy(&points_deleted_rebuild_mutex_lock); + pthread_mutex_destroy(&working_flag_mutex); + pthread_mutex_destroy(&search_flag_mutex); +} + +template +void *KD_TREE::multi_thread_ptr(void *arg) +{ + KD_TREE *handle = (KD_TREE *)arg; + handle->multi_thread_rebuild(); + return nullptr; +} + +template +void KD_TREE::multi_thread_rebuild() +{ + bool terminated = false; + KD_TREE_NODE *father_ptr, **new_node_ptr; + pthread_mutex_lock(&termination_flag_mutex_lock); + terminated = termination_flag; + pthread_mutex_unlock(&termination_flag_mutex_lock); + while (!terminated) + { + pthread_mutex_lock(&rebuild_ptr_mutex_lock); + pthread_mutex_lock(&working_flag_mutex); + if (Rebuild_Ptr != nullptr) + { + /* Traverse and copy */ + if (!Rebuild_Logger.empty()) + { + printf("\n\n\n\n\n\n\n\n\n\n\n ERROR!!! \n\n\n\n\n\n\n\n\n"); + } + rebuild_flag = true; + if (*Rebuild_Ptr == Root_Node) + { + Treesize_tmp = Root_Node->TreeSize; + Validnum_tmp = Root_Node->TreeSize - Root_Node->invalid_point_num; + alpha_bal_tmp = Root_Node->alpha_bal; + alpha_del_tmp = Root_Node->alpha_del; + } + KD_TREE_NODE *old_root_node = (*Rebuild_Ptr); + father_ptr = (*Rebuild_Ptr)->father_ptr; + PointVector().swap(Rebuild_PCL_Storage); + // Lock Search + pthread_mutex_lock(&search_flag_mutex); + while (search_mutex_counter != 0) + { + pthread_mutex_unlock(&search_flag_mutex); + usleep(1); + pthread_mutex_lock(&search_flag_mutex); + } + search_mutex_counter = -1; + pthread_mutex_unlock(&search_flag_mutex); + // Lock deleted points cache + pthread_mutex_lock(&points_deleted_rebuild_mutex_lock); + flatten(*Rebuild_Ptr, Rebuild_PCL_Storage, MULTI_THREAD_REC); + // Unlock deleted points cache + pthread_mutex_unlock(&points_deleted_rebuild_mutex_lock); + // Unlock Search + pthread_mutex_lock(&search_flag_mutex); + search_mutex_counter = 0; + pthread_mutex_unlock(&search_flag_mutex); + pthread_mutex_unlock(&working_flag_mutex); + /* Rebuild and update missed operations*/ + Operation_Logger_Type Operation; + KD_TREE_NODE *new_root_node = nullptr; + if (int(Rebuild_PCL_Storage.size()) > 0) + { + BuildTree(&new_root_node, 0, Rebuild_PCL_Storage.size() - 1, Rebuild_PCL_Storage); + // Rebuild has been done. Updates the blocked operations into the new tree + pthread_mutex_lock(&working_flag_mutex); + pthread_mutex_lock(&rebuild_logger_mutex_lock); + int tmp_counter = 0; + while (!Rebuild_Logger.empty()) + { + Operation = Rebuild_Logger.front(); + max_queue_size = max(max_queue_size, Rebuild_Logger.size()); + Rebuild_Logger.pop(); + pthread_mutex_unlock(&rebuild_logger_mutex_lock); + pthread_mutex_unlock(&working_flag_mutex); + run_operation(&new_root_node, Operation); + tmp_counter++; + if (tmp_counter % 10 == 0) + usleep(1); + pthread_mutex_lock(&working_flag_mutex); + pthread_mutex_lock(&rebuild_logger_mutex_lock); + } + pthread_mutex_unlock(&rebuild_logger_mutex_lock); + } + /* Replace to original tree*/ + // pthread_mutex_lock(&working_flag_mutex); + pthread_mutex_lock(&search_flag_mutex); + while (search_mutex_counter != 0) + { + pthread_mutex_unlock(&search_flag_mutex); + usleep(1); + pthread_mutex_lock(&search_flag_mutex); + } + search_mutex_counter = -1; + pthread_mutex_unlock(&search_flag_mutex); + if (father_ptr->left_son_ptr == *Rebuild_Ptr) + { + father_ptr->left_son_ptr = new_root_node; + } + else if (father_ptr->right_son_ptr == *Rebuild_Ptr) + { + father_ptr->right_son_ptr = new_root_node; + } + else + { + throw "Error: Father ptr incompatible with current node\n"; + } + if (new_root_node != nullptr) + new_root_node->father_ptr = father_ptr; + (*Rebuild_Ptr) = new_root_node; + int valid_old = old_root_node->TreeSize - old_root_node->invalid_point_num; + int valid_new = new_root_node->TreeSize - new_root_node->invalid_point_num; + if (father_ptr == STATIC_ROOT_NODE) + Root_Node = STATIC_ROOT_NODE->left_son_ptr; + KD_TREE_NODE *update_root = *Rebuild_Ptr; + while (update_root != nullptr && update_root != Root_Node) + { + update_root = update_root->father_ptr; + if (update_root->working_flag) + break; + if (update_root == update_root->father_ptr->left_son_ptr && update_root->father_ptr->need_push_down_to_left) + break; + if (update_root == update_root->father_ptr->right_son_ptr && update_root->father_ptr->need_push_down_to_right) + break; + Update(update_root); + } + pthread_mutex_lock(&search_flag_mutex); + search_mutex_counter = 0; + pthread_mutex_unlock(&search_flag_mutex); + Rebuild_Ptr = nullptr; + pthread_mutex_unlock(&working_flag_mutex); + rebuild_flag = false; + /* Delete discarded tree nodes */ + delete_tree_nodes(&old_root_node); + } + else + { + pthread_mutex_unlock(&working_flag_mutex); + } + pthread_mutex_unlock(&rebuild_ptr_mutex_lock); + pthread_mutex_lock(&termination_flag_mutex_lock); + terminated = termination_flag; + pthread_mutex_unlock(&termination_flag_mutex_lock); + usleep(100); + } + printf("Rebuild thread terminated normally\n"); +} + +template +void KD_TREE::run_operation(KD_TREE_NODE **root, Operation_Logger_Type operation) +{ + switch (operation.op) + { + case ADD_POINT: + Add_by_point(root, operation.point, false, (*root)->division_axis); + break; + case ADD_BOX: + Add_by_range(root, operation.boxpoint, false); + break; + case DELETE_POINT: + Delete_by_point(root, operation.point, false); + break; + case DELETE_BOX: + Delete_by_range(root, operation.boxpoint, false, false); + break; + case DOWNSAMPLE_DELETE: + Delete_by_range(root, operation.boxpoint, false, true); + break; + case PUSH_DOWN: + (*root)->tree_downsample_deleted |= operation.tree_downsample_deleted; + (*root)->point_downsample_deleted |= operation.tree_downsample_deleted; + (*root)->tree_deleted = operation.tree_deleted || (*root)->tree_downsample_deleted; + (*root)->point_deleted = (*root)->tree_deleted || (*root)->point_downsample_deleted; + if (operation.tree_downsample_deleted) + (*root)->down_del_num = (*root)->TreeSize; + if (operation.tree_deleted) + (*root)->invalid_point_num = (*root)->TreeSize; + else + (*root)->invalid_point_num = (*root)->down_del_num; + (*root)->need_push_down_to_left = true; + (*root)->need_push_down_to_right = true; + break; + default: + break; + } +} + +template +void KD_TREE::Build(PointVector point_cloud) +{ + if (Root_Node != nullptr) + { + delete_tree_nodes(&Root_Node); + } + if (point_cloud.size() == 0) + return; + STATIC_ROOT_NODE = new KD_TREE_NODE; + InitTreeNode(STATIC_ROOT_NODE); + BuildTree(&STATIC_ROOT_NODE->left_son_ptr, 0, point_cloud.size() - 1, point_cloud); + Update(STATIC_ROOT_NODE); + STATIC_ROOT_NODE->TreeSize = 0; + Root_Node = STATIC_ROOT_NODE->left_son_ptr; +} + +template +void KD_TREE::Nearest_Search(PointType point, int k_nearest, PointVector &Nearest_Points, vector &Point_Distance, float max_dist) +{ + MANUAL_HEAP q(2 * k_nearest); + q.clear(); + vector().swap(Point_Distance); + if (Rebuild_Ptr == nullptr || *Rebuild_Ptr != Root_Node) + { + Search(Root_Node, k_nearest, point, q, max_dist); + } + else + { + pthread_mutex_lock(&search_flag_mutex); + while (search_mutex_counter == -1) + { + pthread_mutex_unlock(&search_flag_mutex); + usleep(1); + pthread_mutex_lock(&search_flag_mutex); + } + search_mutex_counter += 1; + pthread_mutex_unlock(&search_flag_mutex); + Search(Root_Node, k_nearest, point, q, max_dist); + pthread_mutex_lock(&search_flag_mutex); + search_mutex_counter -= 1; + pthread_mutex_unlock(&search_flag_mutex); + } + int k_found = min(k_nearest, int(q.size())); + PointVector().swap(Nearest_Points); + vector().swap(Point_Distance); + for (int i = 0; i < k_found; i++) + { + Nearest_Points.insert(Nearest_Points.begin(), q.top().point); + Point_Distance.insert(Point_Distance.begin(), q.top().dist); + q.pop(); + } + return; +} + +template +void KD_TREE::Box_Search(const BoxPointType &Box_of_Point, PointVector &Storage) +{ + Storage.clear(); + Search_by_range(Root_Node, Box_of_Point, Storage); +} + +template +void KD_TREE::Radius_Search(PointType point, const float radius, PointVector &Storage) +{ + Storage.clear(); + Search_by_radius(Root_Node, point, radius, Storage); +} + +template +int KD_TREE::Add_Points(PointVector &PointToAdd, bool downsample_on) +{ + int NewPointSize = PointToAdd.size(); + int tree_size = size(); + BoxPointType Box_of_Point; + PointType downsample_result, mid_point; + bool downsample_switch = downsample_on && DOWNSAMPLE_SWITCH; + float min_dist, tmp_dist; + int tmp_counter = 0; + for (int i = 0; i < PointToAdd.size(); i++) + { + if (downsample_switch) + { + Box_of_Point.vertex_min[0] = floor(PointToAdd[i].x / downsample_size) * downsample_size; + Box_of_Point.vertex_max[0] = Box_of_Point.vertex_min[0] + downsample_size; + Box_of_Point.vertex_min[1] = floor(PointToAdd[i].y / downsample_size) * downsample_size; + Box_of_Point.vertex_max[1] = Box_of_Point.vertex_min[1] + downsample_size; + Box_of_Point.vertex_min[2] = floor(PointToAdd[i].z / downsample_size) * downsample_size; + Box_of_Point.vertex_max[2] = Box_of_Point.vertex_min[2] + downsample_size; + mid_point.x = Box_of_Point.vertex_min[0] + (Box_of_Point.vertex_max[0] - Box_of_Point.vertex_min[0]) / 2.0; + mid_point.y = Box_of_Point.vertex_min[1] + (Box_of_Point.vertex_max[1] - Box_of_Point.vertex_min[1]) / 2.0; + mid_point.z = Box_of_Point.vertex_min[2] + (Box_of_Point.vertex_max[2] - Box_of_Point.vertex_min[2]) / 2.0; + PointVector().swap(Downsample_Storage); + Search_by_range(Root_Node, Box_of_Point, Downsample_Storage); + min_dist = calc_dist(PointToAdd[i], mid_point); + downsample_result = PointToAdd[i]; + for (int index = 0; index < Downsample_Storage.size(); index++) + { + tmp_dist = calc_dist(Downsample_Storage[index], mid_point); + if (tmp_dist < min_dist) + { + min_dist = tmp_dist; + downsample_result = Downsample_Storage[index]; + } + } + if (Rebuild_Ptr == nullptr || *Rebuild_Ptr != Root_Node) + { + if (Downsample_Storage.size() > 1 || same_point(PointToAdd[i], downsample_result)) + { + if (Downsample_Storage.size() > 0) + Delete_by_range(&Root_Node, Box_of_Point, true, true); + Add_by_point(&Root_Node, downsample_result, true, Root_Node->division_axis); + tmp_counter++; + } + } + else + { + if (Downsample_Storage.size() > 1 || same_point(PointToAdd[i], downsample_result)) + { + Operation_Logger_Type operation_delete, operation; + operation_delete.boxpoint = Box_of_Point; + operation_delete.op = DOWNSAMPLE_DELETE; + operation.point = downsample_result; + operation.op = ADD_POINT; + pthread_mutex_lock(&working_flag_mutex); + if (Downsample_Storage.size() > 0) + Delete_by_range(&Root_Node, Box_of_Point, false, true); + Add_by_point(&Root_Node, downsample_result, false, Root_Node->division_axis); + tmp_counter++; + if (rebuild_flag) + { + pthread_mutex_lock(&rebuild_logger_mutex_lock); + if (Downsample_Storage.size() > 0) + Rebuild_Logger.push(operation_delete); + Rebuild_Logger.push(operation); + pthread_mutex_unlock(&rebuild_logger_mutex_lock); + } + pthread_mutex_unlock(&working_flag_mutex); + }; + } + } + else + { + if (Rebuild_Ptr == nullptr || *Rebuild_Ptr != Root_Node) + { + Add_by_point(&Root_Node, PointToAdd[i], true, Root_Node->division_axis); + } + else + { + Operation_Logger_Type operation; + operation.point = PointToAdd[i]; + operation.op = ADD_POINT; + pthread_mutex_lock(&working_flag_mutex); + Add_by_point(&Root_Node, PointToAdd[i], false, Root_Node->division_axis); + if (rebuild_flag) + { + pthread_mutex_lock(&rebuild_logger_mutex_lock); + Rebuild_Logger.push(operation); + pthread_mutex_unlock(&rebuild_logger_mutex_lock); + } + pthread_mutex_unlock(&working_flag_mutex); + } + } + } + return tmp_counter; +} + +template +void KD_TREE::Add_Point_Boxes(vector &BoxPoints) +{ + for (int i = 0; i < BoxPoints.size(); i++) + { + if (Rebuild_Ptr == nullptr || *Rebuild_Ptr != Root_Node) + { + Add_by_range(&Root_Node, BoxPoints[i], true); + } + else + { + Operation_Logger_Type operation; + operation.boxpoint = BoxPoints[i]; + operation.op = ADD_BOX; + pthread_mutex_lock(&working_flag_mutex); + Add_by_range(&Root_Node, BoxPoints[i], false); + if (rebuild_flag) + { + pthread_mutex_lock(&rebuild_logger_mutex_lock); + Rebuild_Logger.push(operation); + pthread_mutex_unlock(&rebuild_logger_mutex_lock); + } + pthread_mutex_unlock(&working_flag_mutex); + } + } + return; +} + +template +void KD_TREE::Delete_Points(PointVector &PointToDel) +{ + for (int i = 0; i < PointToDel.size(); i++) + { + if (Rebuild_Ptr == nullptr || *Rebuild_Ptr != Root_Node) + { + Delete_by_point(&Root_Node, PointToDel[i], true); + } + else + { + Operation_Logger_Type operation; + operation.point = PointToDel[i]; + operation.op = DELETE_POINT; + pthread_mutex_lock(&working_flag_mutex); + Delete_by_point(&Root_Node, PointToDel[i], false); + if (rebuild_flag) + { + pthread_mutex_lock(&rebuild_logger_mutex_lock); + Rebuild_Logger.push(operation); + pthread_mutex_unlock(&rebuild_logger_mutex_lock); + } + pthread_mutex_unlock(&working_flag_mutex); + } + } + return; +} + +template +int KD_TREE::Delete_Point_Boxes(vector &BoxPoints) +{ + int tmp_counter = 0; + for (int i = 0; i < BoxPoints.size(); i++) + { + if (Rebuild_Ptr == nullptr || *Rebuild_Ptr != Root_Node) + { + tmp_counter += Delete_by_range(&Root_Node, BoxPoints[i], true, false); + } + else + { + Operation_Logger_Type operation; + operation.boxpoint = BoxPoints[i]; + operation.op = DELETE_BOX; + pthread_mutex_lock(&working_flag_mutex); + tmp_counter += Delete_by_range(&Root_Node, BoxPoints[i], false, false); + if (rebuild_flag) + { + pthread_mutex_lock(&rebuild_logger_mutex_lock); + Rebuild_Logger.push(operation); + pthread_mutex_unlock(&rebuild_logger_mutex_lock); + } + pthread_mutex_unlock(&working_flag_mutex); + } + } + return tmp_counter; +} + +template +void KD_TREE::acquire_removed_points(PointVector &removed_points) +{ + pthread_mutex_lock(&points_deleted_rebuild_mutex_lock); + for (int i = 0; i < Points_deleted.size(); i++) + { + removed_points.push_back(Points_deleted[i]); + } + for (int i = 0; i < Multithread_Points_deleted.size(); i++) + { + removed_points.push_back(Multithread_Points_deleted[i]); + } + Points_deleted.clear(); + Multithread_Points_deleted.clear(); + pthread_mutex_unlock(&points_deleted_rebuild_mutex_lock); + return; +} + +template +void KD_TREE::BuildTree(KD_TREE_NODE **root, int l, int r, PointVector &Storage) +{ + if (l > r) + return; + *root = new KD_TREE_NODE; + InitTreeNode(*root); + int mid = (l + r) >> 1; + int div_axis = 0; + int i; + // Find the best division Axis + float min_value[3] = {INFINITY, INFINITY, INFINITY}; + float max_value[3] = {-INFINITY, -INFINITY, -INFINITY}; + float dim_range[3] = {0, 0, 0}; + for (i = l; i <= r; i++) + { + min_value[0] = min(min_value[0], Storage[i].x); + min_value[1] = min(min_value[1], Storage[i].y); + min_value[2] = min(min_value[2], Storage[i].z); + max_value[0] = max(max_value[0], Storage[i].x); + max_value[1] = max(max_value[1], Storage[i].y); + max_value[2] = max(max_value[2], Storage[i].z); + } + // Select the longest dimension as division axis + for (i = 0; i < 3; i++) + dim_range[i] = max_value[i] - min_value[i]; + for (i = 1; i < 3; i++) + if (dim_range[i] > dim_range[div_axis]) + div_axis = i; + // Divide by the division axis and recursively build. + + (*root)->division_axis = div_axis; + switch (div_axis) + { + case 0: + nth_element(begin(Storage) + l, begin(Storage) + mid, begin(Storage) + r + 1, point_cmp_x); + break; + case 1: + nth_element(begin(Storage) + l, begin(Storage) + mid, begin(Storage) + r + 1, point_cmp_y); + break; + case 2: + nth_element(begin(Storage) + l, begin(Storage) + mid, begin(Storage) + r + 1, point_cmp_z); + break; + default: + nth_element(begin(Storage) + l, begin(Storage) + mid, begin(Storage) + r + 1, point_cmp_x); + break; + } + (*root)->point = Storage[mid]; + KD_TREE_NODE *left_son = nullptr, *right_son = nullptr; + BuildTree(&left_son, l, mid - 1, Storage); + BuildTree(&right_son, mid + 1, r, Storage); + (*root)->left_son_ptr = left_son; + (*root)->right_son_ptr = right_son; + Update((*root)); + return; +} + +template +void KD_TREE::Rebuild(KD_TREE_NODE **root) +{ + KD_TREE_NODE *father_ptr; + if ((*root)->TreeSize >= Multi_Thread_Rebuild_Point_Num) + { + if (!pthread_mutex_trylock(&rebuild_ptr_mutex_lock)) + { + if (Rebuild_Ptr == nullptr || ((*root)->TreeSize > (*Rebuild_Ptr)->TreeSize)) + { + Rebuild_Ptr = root; + } + pthread_mutex_unlock(&rebuild_ptr_mutex_lock); + } + } + else + { + father_ptr = (*root)->father_ptr; + int size_rec = (*root)->TreeSize; + PCL_Storage.clear(); + flatten(*root, PCL_Storage, DELETE_POINTS_REC); + delete_tree_nodes(root); + BuildTree(root, 0, PCL_Storage.size() - 1, PCL_Storage); + if (*root != nullptr) + (*root)->father_ptr = father_ptr; + if (*root == Root_Node) + STATIC_ROOT_NODE->left_son_ptr = *root; + } + return; +} + +template +int KD_TREE::Delete_by_range(KD_TREE_NODE **root, BoxPointType boxpoint, bool allow_rebuild, bool is_downsample) +{ + if ((*root) == nullptr || (*root)->tree_deleted) + return 0; + (*root)->working_flag = true; + Push_Down(*root); + int tmp_counter = 0; + if (boxpoint.vertex_max[0] <= (*root)->node_range_x[0] || boxpoint.vertex_min[0] > (*root)->node_range_x[1]) + return 0; + if (boxpoint.vertex_max[1] <= (*root)->node_range_y[0] || boxpoint.vertex_min[1] > (*root)->node_range_y[1]) + return 0; + if (boxpoint.vertex_max[2] <= (*root)->node_range_z[0] || boxpoint.vertex_min[2] > (*root)->node_range_z[1]) + return 0; + if (boxpoint.vertex_min[0] <= (*root)->node_range_x[0] && boxpoint.vertex_max[0] > (*root)->node_range_x[1] && boxpoint.vertex_min[1] <= (*root)->node_range_y[0] && boxpoint.vertex_max[1] > (*root)->node_range_y[1] && boxpoint.vertex_min[2] <= (*root)->node_range_z[0] && boxpoint.vertex_max[2] > (*root)->node_range_z[1]) + { + (*root)->tree_deleted = true; + (*root)->point_deleted = true; + (*root)->need_push_down_to_left = true; + (*root)->need_push_down_to_right = true; + tmp_counter = (*root)->TreeSize - (*root)->invalid_point_num; + (*root)->invalid_point_num = (*root)->TreeSize; + if (is_downsample) + { + (*root)->tree_downsample_deleted = true; + (*root)->point_downsample_deleted = true; + (*root)->down_del_num = (*root)->TreeSize; + } + return tmp_counter; + } + if (!(*root)->point_deleted && boxpoint.vertex_min[0] <= (*root)->point.x && boxpoint.vertex_max[0] > (*root)->point.x && boxpoint.vertex_min[1] <= (*root)->point.y && boxpoint.vertex_max[1] > (*root)->point.y && boxpoint.vertex_min[2] <= (*root)->point.z && boxpoint.vertex_max[2] > (*root)->point.z) + { + (*root)->point_deleted = true; + tmp_counter += 1; + if (is_downsample) + (*root)->point_downsample_deleted = true; + } + Operation_Logger_Type delete_box_log; + struct timespec Timeout; + if (is_downsample) + delete_box_log.op = DOWNSAMPLE_DELETE; + else + delete_box_log.op = DELETE_BOX; + delete_box_log.boxpoint = boxpoint; + if ((Rebuild_Ptr == nullptr) || (*root)->left_son_ptr != *Rebuild_Ptr) + { + tmp_counter += Delete_by_range(&((*root)->left_son_ptr), boxpoint, allow_rebuild, is_downsample); + } + else + { + pthread_mutex_lock(&working_flag_mutex); + tmp_counter += Delete_by_range(&((*root)->left_son_ptr), boxpoint, false, is_downsample); + if (rebuild_flag) + { + pthread_mutex_lock(&rebuild_logger_mutex_lock); + Rebuild_Logger.push(delete_box_log); + pthread_mutex_unlock(&rebuild_logger_mutex_lock); + } + pthread_mutex_unlock(&working_flag_mutex); + } + if ((Rebuild_Ptr == nullptr) || (*root)->right_son_ptr != *Rebuild_Ptr) + { + tmp_counter += Delete_by_range(&((*root)->right_son_ptr), boxpoint, allow_rebuild, is_downsample); + } + else + { + pthread_mutex_lock(&working_flag_mutex); + tmp_counter += Delete_by_range(&((*root)->right_son_ptr), boxpoint, false, is_downsample); + if (rebuild_flag) + { + pthread_mutex_lock(&rebuild_logger_mutex_lock); + Rebuild_Logger.push(delete_box_log); + pthread_mutex_unlock(&rebuild_logger_mutex_lock); + } + pthread_mutex_unlock(&working_flag_mutex); + } + Update(*root); + if (Rebuild_Ptr != nullptr && *Rebuild_Ptr == *root && (*root)->TreeSize < Multi_Thread_Rebuild_Point_Num) + Rebuild_Ptr = nullptr; + bool need_rebuild = allow_rebuild & Criterion_Check((*root)); + if (need_rebuild) + Rebuild(root); + if ((*root) != nullptr) + (*root)->working_flag = false; + return tmp_counter; +} + +template +void KD_TREE::Delete_by_point(KD_TREE_NODE **root, PointType point, bool allow_rebuild) +{ + if ((*root) == nullptr || (*root)->tree_deleted) + return; + (*root)->working_flag = true; + Push_Down(*root); + if (same_point((*root)->point, point) && !(*root)->point_deleted) + { + (*root)->point_deleted = true; + (*root)->invalid_point_num += 1; + if ((*root)->invalid_point_num == (*root)->TreeSize) + (*root)->tree_deleted = true; + return; + } + Operation_Logger_Type delete_log; + struct timespec Timeout; + delete_log.op = DELETE_POINT; + delete_log.point = point; + if (((*root)->division_axis == 0 && point.x < (*root)->point.x) || ((*root)->division_axis == 1 && point.y < (*root)->point.y) || ((*root)->division_axis == 2 && point.z < (*root)->point.z)) + { + if ((Rebuild_Ptr == nullptr) || (*root)->left_son_ptr != *Rebuild_Ptr) + { + Delete_by_point(&(*root)->left_son_ptr, point, allow_rebuild); + } + else + { + pthread_mutex_lock(&working_flag_mutex); + Delete_by_point(&(*root)->left_son_ptr, point, false); + if (rebuild_flag) + { + pthread_mutex_lock(&rebuild_logger_mutex_lock); + Rebuild_Logger.push(delete_log); + pthread_mutex_unlock(&rebuild_logger_mutex_lock); + } + pthread_mutex_unlock(&working_flag_mutex); + } + } + else + { + if ((Rebuild_Ptr == nullptr) || (*root)->right_son_ptr != *Rebuild_Ptr) + { + Delete_by_point(&(*root)->right_son_ptr, point, allow_rebuild); + } + else + { + pthread_mutex_lock(&working_flag_mutex); + Delete_by_point(&(*root)->right_son_ptr, point, false); + if (rebuild_flag) + { + pthread_mutex_lock(&rebuild_logger_mutex_lock); + Rebuild_Logger.push(delete_log); + pthread_mutex_unlock(&rebuild_logger_mutex_lock); + } + pthread_mutex_unlock(&working_flag_mutex); + } + } + Update(*root); + if (Rebuild_Ptr != nullptr && *Rebuild_Ptr == *root && (*root)->TreeSize < Multi_Thread_Rebuild_Point_Num) + Rebuild_Ptr = nullptr; + bool need_rebuild = allow_rebuild & Criterion_Check((*root)); + if (need_rebuild) + Rebuild(root); + if ((*root) != nullptr) + (*root)->working_flag = false; + return; +} + +template +void KD_TREE::Add_by_range(KD_TREE_NODE **root, BoxPointType boxpoint, bool allow_rebuild) +{ + if ((*root) == nullptr) + return; + (*root)->working_flag = true; + Push_Down(*root); + if (boxpoint.vertex_max[0] <= (*root)->node_range_x[0] || boxpoint.vertex_min[0] > (*root)->node_range_x[1]) + return; + if (boxpoint.vertex_max[1] <= (*root)->node_range_y[0] || boxpoint.vertex_min[1] > (*root)->node_range_y[1]) + return; + if (boxpoint.vertex_max[2] <= (*root)->node_range_z[0] || boxpoint.vertex_min[2] > (*root)->node_range_z[1]) + return; + if (boxpoint.vertex_min[0] <= (*root)->node_range_x[0] && boxpoint.vertex_max[0] > (*root)->node_range_x[1] && boxpoint.vertex_min[1] <= (*root)->node_range_y[0] && boxpoint.vertex_max[1] > (*root)->node_range_y[1] && boxpoint.vertex_min[2] <= (*root)->node_range_z[0] && boxpoint.vertex_max[2] > (*root)->node_range_z[1]) + { + (*root)->tree_deleted = false || (*root)->tree_downsample_deleted; + (*root)->point_deleted = false || (*root)->point_downsample_deleted; + (*root)->need_push_down_to_left = true; + (*root)->need_push_down_to_right = true; + (*root)->invalid_point_num = (*root)->down_del_num; + return; + } + if (boxpoint.vertex_min[0] <= (*root)->point.x && boxpoint.vertex_max[0] > (*root)->point.x && boxpoint.vertex_min[1] <= (*root)->point.y && boxpoint.vertex_max[1] > (*root)->point.y && boxpoint.vertex_min[2] <= (*root)->point.z && boxpoint.vertex_max[2] > (*root)->point.z) + { + (*root)->point_deleted = (*root)->point_downsample_deleted; + } + Operation_Logger_Type add_box_log; + struct timespec Timeout; + add_box_log.op = ADD_BOX; + add_box_log.boxpoint = boxpoint; + if ((Rebuild_Ptr == nullptr) || (*root)->left_son_ptr != *Rebuild_Ptr) + { + Add_by_range(&((*root)->left_son_ptr), boxpoint, allow_rebuild); + } + else + { + pthread_mutex_lock(&working_flag_mutex); + Add_by_range(&((*root)->left_son_ptr), boxpoint, false); + if (rebuild_flag) + { + pthread_mutex_lock(&rebuild_logger_mutex_lock); + Rebuild_Logger.push(add_box_log); + pthread_mutex_unlock(&rebuild_logger_mutex_lock); + } + pthread_mutex_unlock(&working_flag_mutex); + } + if ((Rebuild_Ptr == nullptr) || (*root)->right_son_ptr != *Rebuild_Ptr) + { + Add_by_range(&((*root)->right_son_ptr), boxpoint, allow_rebuild); + } + else + { + pthread_mutex_lock(&working_flag_mutex); + Add_by_range(&((*root)->right_son_ptr), boxpoint, false); + if (rebuild_flag) + { + pthread_mutex_lock(&rebuild_logger_mutex_lock); + Rebuild_Logger.push(add_box_log); + pthread_mutex_unlock(&rebuild_logger_mutex_lock); + } + pthread_mutex_unlock(&working_flag_mutex); + } + Update(*root); + if (Rebuild_Ptr != nullptr && *Rebuild_Ptr == *root && (*root)->TreeSize < Multi_Thread_Rebuild_Point_Num) + Rebuild_Ptr = nullptr; + bool need_rebuild = allow_rebuild & Criterion_Check((*root)); + if (need_rebuild) + Rebuild(root); + if ((*root) != nullptr) + (*root)->working_flag = false; + return; +} + +template +void KD_TREE::Add_by_point(KD_TREE_NODE **root, PointType point, bool allow_rebuild, int father_axis) +{ + if (*root == nullptr) + { + *root = new KD_TREE_NODE; + InitTreeNode(*root); + (*root)->point = point; + (*root)->division_axis = (father_axis + 1) % 3; + Update(*root); + return; + } + (*root)->working_flag = true; + Operation_Logger_Type add_log; + struct timespec Timeout; + add_log.op = ADD_POINT; + add_log.point = point; + Push_Down(*root); + if (((*root)->division_axis == 0 && point.x < (*root)->point.x) || ((*root)->division_axis == 1 && point.y < (*root)->point.y) || ((*root)->division_axis == 2 && point.z < (*root)->point.z)) + { + if ((Rebuild_Ptr == nullptr) || (*root)->left_son_ptr != *Rebuild_Ptr) + { + Add_by_point(&(*root)->left_son_ptr, point, allow_rebuild, (*root)->division_axis); + } + else + { + pthread_mutex_lock(&working_flag_mutex); + Add_by_point(&(*root)->left_son_ptr, point, false, (*root)->division_axis); + if (rebuild_flag) + { + pthread_mutex_lock(&rebuild_logger_mutex_lock); + Rebuild_Logger.push(add_log); + pthread_mutex_unlock(&rebuild_logger_mutex_lock); + } + pthread_mutex_unlock(&working_flag_mutex); + } + } + else + { + if ((Rebuild_Ptr == nullptr) || (*root)->right_son_ptr != *Rebuild_Ptr) + { + Add_by_point(&(*root)->right_son_ptr, point, allow_rebuild, (*root)->division_axis); + } + else + { + pthread_mutex_lock(&working_flag_mutex); + Add_by_point(&(*root)->right_son_ptr, point, false, (*root)->division_axis); + if (rebuild_flag) + { + pthread_mutex_lock(&rebuild_logger_mutex_lock); + Rebuild_Logger.push(add_log); + pthread_mutex_unlock(&rebuild_logger_mutex_lock); + } + pthread_mutex_unlock(&working_flag_mutex); + } + } + Update(*root); + if (Rebuild_Ptr != nullptr && *Rebuild_Ptr == *root && (*root)->TreeSize < Multi_Thread_Rebuild_Point_Num) + Rebuild_Ptr = nullptr; + bool need_rebuild = allow_rebuild & Criterion_Check((*root)); + if (need_rebuild) + Rebuild(root); + if ((*root) != nullptr) + (*root)->working_flag = false; + return; +} + +template +void KD_TREE::Search(KD_TREE_NODE *root, int k_nearest, PointType point, MANUAL_HEAP &q, float max_dist) +{ + if (root == nullptr || root->tree_deleted) + return; + float cur_dist = calc_box_dist(root, point); + float max_dist_sqr = max_dist * max_dist; + if (cur_dist > max_dist_sqr) + return; + int retval; + if (root->need_push_down_to_left || root->need_push_down_to_right) + { + retval = pthread_mutex_trylock(&(root->push_down_mutex_lock)); + if (retval == 0) + { + Push_Down(root); + pthread_mutex_unlock(&(root->push_down_mutex_lock)); + } + else + { + pthread_mutex_lock(&(root->push_down_mutex_lock)); + pthread_mutex_unlock(&(root->push_down_mutex_lock)); + } + } + if (!root->point_deleted) + { + float dist = calc_dist(point, root->point); + if (dist <= max_dist_sqr && (q.size() < k_nearest || dist < q.top().dist)) + { + if (q.size() >= k_nearest) + q.pop(); + PointType_CMP current_point{root->point, dist}; + q.push(current_point); + } + } + int cur_search_counter; + float dist_left_node = calc_box_dist(root->left_son_ptr, point); + float dist_right_node = calc_box_dist(root->right_son_ptr, point); + if (q.size() < k_nearest || dist_left_node < q.top().dist && dist_right_node < q.top().dist) + { + if (dist_left_node <= dist_right_node) + { + if (Rebuild_Ptr == nullptr || *Rebuild_Ptr != root->left_son_ptr) + { + Search(root->left_son_ptr, k_nearest, point, q, max_dist); + } + else + { + pthread_mutex_lock(&search_flag_mutex); + while (search_mutex_counter == -1) + { + pthread_mutex_unlock(&search_flag_mutex); + usleep(1); + pthread_mutex_lock(&search_flag_mutex); + } + search_mutex_counter += 1; + pthread_mutex_unlock(&search_flag_mutex); + Search(root->left_son_ptr, k_nearest, point, q, max_dist); + pthread_mutex_lock(&search_flag_mutex); + search_mutex_counter -= 1; + pthread_mutex_unlock(&search_flag_mutex); + } + if (q.size() < k_nearest || dist_right_node < q.top().dist) + { + if (Rebuild_Ptr == nullptr || *Rebuild_Ptr != root->right_son_ptr) + { + Search(root->right_son_ptr, k_nearest, point, q, max_dist); + } + else + { + pthread_mutex_lock(&search_flag_mutex); + while (search_mutex_counter == -1) + { + pthread_mutex_unlock(&search_flag_mutex); + usleep(1); + pthread_mutex_lock(&search_flag_mutex); + } + search_mutex_counter += 1; + pthread_mutex_unlock(&search_flag_mutex); + Search(root->right_son_ptr, k_nearest, point, q, max_dist); + pthread_mutex_lock(&search_flag_mutex); + search_mutex_counter -= 1; + pthread_mutex_unlock(&search_flag_mutex); + } + } + } + else + { + if (Rebuild_Ptr == nullptr || *Rebuild_Ptr != root->right_son_ptr) + { + Search(root->right_son_ptr, k_nearest, point, q, max_dist); + } + else + { + pthread_mutex_lock(&search_flag_mutex); + while (search_mutex_counter == -1) + { + pthread_mutex_unlock(&search_flag_mutex); + usleep(1); + pthread_mutex_lock(&search_flag_mutex); + } + search_mutex_counter += 1; + pthread_mutex_unlock(&search_flag_mutex); + Search(root->right_son_ptr, k_nearest, point, q, max_dist); + pthread_mutex_lock(&search_flag_mutex); + search_mutex_counter -= 1; + pthread_mutex_unlock(&search_flag_mutex); + } + if (q.size() < k_nearest || dist_left_node < q.top().dist) + { + if (Rebuild_Ptr == nullptr || *Rebuild_Ptr != root->left_son_ptr) + { + Search(root->left_son_ptr, k_nearest, point, q, max_dist); + } + else + { + pthread_mutex_lock(&search_flag_mutex); + while (search_mutex_counter == -1) + { + pthread_mutex_unlock(&search_flag_mutex); + usleep(1); + pthread_mutex_lock(&search_flag_mutex); + } + search_mutex_counter += 1; + pthread_mutex_unlock(&search_flag_mutex); + Search(root->left_son_ptr, k_nearest, point, q, max_dist); + pthread_mutex_lock(&search_flag_mutex); + search_mutex_counter -= 1; + pthread_mutex_unlock(&search_flag_mutex); + } + } + } + } + else + { + if (dist_left_node < q.top().dist) + { + if (Rebuild_Ptr == nullptr || *Rebuild_Ptr != root->left_son_ptr) + { + Search(root->left_son_ptr, k_nearest, point, q, max_dist); + } + else + { + pthread_mutex_lock(&search_flag_mutex); + while (search_mutex_counter == -1) + { + pthread_mutex_unlock(&search_flag_mutex); + usleep(1); + pthread_mutex_lock(&search_flag_mutex); + } + search_mutex_counter += 1; + pthread_mutex_unlock(&search_flag_mutex); + Search(root->left_son_ptr, k_nearest, point, q, max_dist); + pthread_mutex_lock(&search_flag_mutex); + search_mutex_counter -= 1; + pthread_mutex_unlock(&search_flag_mutex); + } + } + if (dist_right_node < q.top().dist) + { + if (Rebuild_Ptr == nullptr || *Rebuild_Ptr != root->right_son_ptr) + { + Search(root->right_son_ptr, k_nearest, point, q, max_dist); + } + else + { + pthread_mutex_lock(&search_flag_mutex); + while (search_mutex_counter == -1) + { + pthread_mutex_unlock(&search_flag_mutex); + usleep(1); + pthread_mutex_lock(&search_flag_mutex); + } + search_mutex_counter += 1; + pthread_mutex_unlock(&search_flag_mutex); + Search(root->right_son_ptr, k_nearest, point, q, max_dist); + pthread_mutex_lock(&search_flag_mutex); + search_mutex_counter -= 1; + pthread_mutex_unlock(&search_flag_mutex); + } + } + } + return; +} + +template +void KD_TREE::Search_by_range(KD_TREE_NODE *root, BoxPointType boxpoint, PointVector &Storage) +{ + if (root == nullptr) + return; + Push_Down(root); + if (boxpoint.vertex_max[0] <= root->node_range_x[0] || boxpoint.vertex_min[0] > root->node_range_x[1]) + return; + if (boxpoint.vertex_max[1] <= root->node_range_y[0] || boxpoint.vertex_min[1] > root->node_range_y[1]) + return; + if (boxpoint.vertex_max[2] <= root->node_range_z[0] || boxpoint.vertex_min[2] > root->node_range_z[1]) + return; + if (boxpoint.vertex_min[0] <= root->node_range_x[0] && boxpoint.vertex_max[0] > root->node_range_x[1] && boxpoint.vertex_min[1] <= root->node_range_y[0] && boxpoint.vertex_max[1] > root->node_range_y[1] && boxpoint.vertex_min[2] <= root->node_range_z[0] && boxpoint.vertex_max[2] > root->node_range_z[1]) + { + flatten(root, Storage, NOT_RECORD); + return; + } + if (boxpoint.vertex_min[0] <= root->point.x && boxpoint.vertex_max[0] > root->point.x && boxpoint.vertex_min[1] <= root->point.y && boxpoint.vertex_max[1] > root->point.y && boxpoint.vertex_min[2] <= root->point.z && boxpoint.vertex_max[2] > root->point.z) + { + if (!root->point_deleted) + Storage.push_back(root->point); + } + if ((Rebuild_Ptr == nullptr) || root->left_son_ptr != *Rebuild_Ptr) + { + Search_by_range(root->left_son_ptr, boxpoint, Storage); + } + else + { + pthread_mutex_lock(&search_flag_mutex); + Search_by_range(root->left_son_ptr, boxpoint, Storage); + pthread_mutex_unlock(&search_flag_mutex); + } + if ((Rebuild_Ptr == nullptr) || root->right_son_ptr != *Rebuild_Ptr) + { + Search_by_range(root->right_son_ptr, boxpoint, Storage); + } + else + { + pthread_mutex_lock(&search_flag_mutex); + Search_by_range(root->right_son_ptr, boxpoint, Storage); + pthread_mutex_unlock(&search_flag_mutex); + } + return; +} + +template +void KD_TREE::Search_by_radius(KD_TREE_NODE *root, PointType point, float radius, PointVector &Storage) +{ + if (root == nullptr) + return; + Push_Down(root); + PointType range_center; + range_center.x = (root->node_range_x[0] + root->node_range_x[1]) * 0.5; + range_center.y = (root->node_range_y[0] + root->node_range_y[1]) * 0.5; + range_center.z = (root->node_range_z[0] + root->node_range_z[1]) * 0.5; + float dist = sqrt(calc_dist(range_center, point)); + if (dist > radius + sqrt(root->radius_sq)) return; + if (dist <= radius - sqrt(root->radius_sq)) + { + flatten(root, Storage, NOT_RECORD); + return; + } + if (!root->point_deleted && calc_dist(root->point, point) <= radius * radius){ + Storage.push_back(root->point); + } + if ((Rebuild_Ptr == nullptr) || root->left_son_ptr != *Rebuild_Ptr) + { + Search_by_radius(root->left_son_ptr, point, radius, Storage); + } + else + { + pthread_mutex_lock(&search_flag_mutex); + Search_by_radius(root->left_son_ptr, point, radius, Storage); + pthread_mutex_unlock(&search_flag_mutex); + } + if ((Rebuild_Ptr == nullptr) || root->right_son_ptr != *Rebuild_Ptr) + { + Search_by_radius(root->right_son_ptr, point, radius, Storage); + } + else + { + pthread_mutex_lock(&search_flag_mutex); + Search_by_radius(root->right_son_ptr, point, radius, Storage); + pthread_mutex_unlock(&search_flag_mutex); + } + return; +} + +template +bool KD_TREE::Criterion_Check(KD_TREE_NODE *root) +{ + if (root->TreeSize <= Minimal_Unbalanced_Tree_Size) + { + return false; + } + float balance_evaluation = 0.0f; + float delete_evaluation = 0.0f; + KD_TREE_NODE *son_ptr = root->left_son_ptr; + if (son_ptr == nullptr) + son_ptr = root->right_son_ptr; + delete_evaluation = float(root->invalid_point_num) / root->TreeSize; + balance_evaluation = float(son_ptr->TreeSize) / (root->TreeSize - 1); + if (delete_evaluation > delete_criterion_param) + { + return true; + } + if (balance_evaluation > balance_criterion_param || balance_evaluation < 1 - balance_criterion_param) + { + return true; + } + return false; +} + +template +void KD_TREE::Push_Down(KD_TREE_NODE *root) +{ + if (root == nullptr) + return; + Operation_Logger_Type operation; + operation.op = PUSH_DOWN; + operation.tree_deleted = root->tree_deleted; + operation.tree_downsample_deleted = root->tree_downsample_deleted; + if (root->need_push_down_to_left && root->left_son_ptr != nullptr) + { + if (Rebuild_Ptr == nullptr || *Rebuild_Ptr != root->left_son_ptr) + { + root->left_son_ptr->tree_downsample_deleted |= root->tree_downsample_deleted; + root->left_son_ptr->point_downsample_deleted |= root->tree_downsample_deleted; + root->left_son_ptr->tree_deleted = root->tree_deleted || root->left_son_ptr->tree_downsample_deleted; + root->left_son_ptr->point_deleted = root->left_son_ptr->tree_deleted || root->left_son_ptr->point_downsample_deleted; + if (root->tree_downsample_deleted) + root->left_son_ptr->down_del_num = root->left_son_ptr->TreeSize; + if (root->tree_deleted) + root->left_son_ptr->invalid_point_num = root->left_son_ptr->TreeSize; + else + root->left_son_ptr->invalid_point_num = root->left_son_ptr->down_del_num; + root->left_son_ptr->need_push_down_to_left = true; + root->left_son_ptr->need_push_down_to_right = true; + root->need_push_down_to_left = false; + } + else + { + pthread_mutex_lock(&working_flag_mutex); + root->left_son_ptr->tree_downsample_deleted |= root->tree_downsample_deleted; + root->left_son_ptr->point_downsample_deleted |= root->tree_downsample_deleted; + root->left_son_ptr->tree_deleted = root->tree_deleted || root->left_son_ptr->tree_downsample_deleted; + root->left_son_ptr->point_deleted = root->left_son_ptr->tree_deleted || root->left_son_ptr->point_downsample_deleted; + if (root->tree_downsample_deleted) + root->left_son_ptr->down_del_num = root->left_son_ptr->TreeSize; + if (root->tree_deleted) + root->left_son_ptr->invalid_point_num = root->left_son_ptr->TreeSize; + else + root->left_son_ptr->invalid_point_num = root->left_son_ptr->down_del_num; + root->left_son_ptr->need_push_down_to_left = true; + root->left_son_ptr->need_push_down_to_right = true; + if (rebuild_flag) + { + pthread_mutex_lock(&rebuild_logger_mutex_lock); + Rebuild_Logger.push(operation); + pthread_mutex_unlock(&rebuild_logger_mutex_lock); + } + root->need_push_down_to_left = false; + pthread_mutex_unlock(&working_flag_mutex); + } + } + if (root->need_push_down_to_right && root->right_son_ptr != nullptr) + { + if (Rebuild_Ptr == nullptr || *Rebuild_Ptr != root->right_son_ptr) + { + root->right_son_ptr->tree_downsample_deleted |= root->tree_downsample_deleted; + root->right_son_ptr->point_downsample_deleted |= root->tree_downsample_deleted; + root->right_son_ptr->tree_deleted = root->tree_deleted || root->right_son_ptr->tree_downsample_deleted; + root->right_son_ptr->point_deleted = root->right_son_ptr->tree_deleted || root->right_son_ptr->point_downsample_deleted; + if (root->tree_downsample_deleted) + root->right_son_ptr->down_del_num = root->right_son_ptr->TreeSize; + if (root->tree_deleted) + root->right_son_ptr->invalid_point_num = root->right_son_ptr->TreeSize; + else + root->right_son_ptr->invalid_point_num = root->right_son_ptr->down_del_num; + root->right_son_ptr->need_push_down_to_left = true; + root->right_son_ptr->need_push_down_to_right = true; + root->need_push_down_to_right = false; + } + else + { + pthread_mutex_lock(&working_flag_mutex); + root->right_son_ptr->tree_downsample_deleted |= root->tree_downsample_deleted; + root->right_son_ptr->point_downsample_deleted |= root->tree_downsample_deleted; + root->right_son_ptr->tree_deleted = root->tree_deleted || root->right_son_ptr->tree_downsample_deleted; + root->right_son_ptr->point_deleted = root->right_son_ptr->tree_deleted || root->right_son_ptr->point_downsample_deleted; + if (root->tree_downsample_deleted) + root->right_son_ptr->down_del_num = root->right_son_ptr->TreeSize; + if (root->tree_deleted) + root->right_son_ptr->invalid_point_num = root->right_son_ptr->TreeSize; + else + root->right_son_ptr->invalid_point_num = root->right_son_ptr->down_del_num; + root->right_son_ptr->need_push_down_to_left = true; + root->right_son_ptr->need_push_down_to_right = true; + if (rebuild_flag) + { + pthread_mutex_lock(&rebuild_logger_mutex_lock); + Rebuild_Logger.push(operation); + pthread_mutex_unlock(&rebuild_logger_mutex_lock); + } + root->need_push_down_to_right = false; + pthread_mutex_unlock(&working_flag_mutex); + } + } + return; +} + +template +void KD_TREE::Update(KD_TREE_NODE *root) +{ + KD_TREE_NODE *left_son_ptr = root->left_son_ptr; + KD_TREE_NODE *right_son_ptr = root->right_son_ptr; + float tmp_range_x[2] = {INFINITY, -INFINITY}; + float tmp_range_y[2] = {INFINITY, -INFINITY}; + float tmp_range_z[2] = {INFINITY, -INFINITY}; + // Update Tree Size + if (left_son_ptr != nullptr && right_son_ptr != nullptr) + { + root->TreeSize = left_son_ptr->TreeSize + right_son_ptr->TreeSize + 1; + root->invalid_point_num = left_son_ptr->invalid_point_num + right_son_ptr->invalid_point_num + (root->point_deleted ? 1 : 0); + root->down_del_num = left_son_ptr->down_del_num + right_son_ptr->down_del_num + (root->point_downsample_deleted ? 1 : 0); + root->tree_downsample_deleted = left_son_ptr->tree_downsample_deleted & right_son_ptr->tree_downsample_deleted & root->point_downsample_deleted; + root->tree_deleted = left_son_ptr->tree_deleted && right_son_ptr->tree_deleted && root->point_deleted; + if (root->tree_deleted || (!left_son_ptr->tree_deleted && !right_son_ptr->tree_deleted && !root->point_deleted)) + { + tmp_range_x[0] = min(min(left_son_ptr->node_range_x[0], right_son_ptr->node_range_x[0]), root->point.x); + tmp_range_x[1] = max(max(left_son_ptr->node_range_x[1], right_son_ptr->node_range_x[1]), root->point.x); + tmp_range_y[0] = min(min(left_son_ptr->node_range_y[0], right_son_ptr->node_range_y[0]), root->point.y); + tmp_range_y[1] = max(max(left_son_ptr->node_range_y[1], right_son_ptr->node_range_y[1]), root->point.y); + tmp_range_z[0] = min(min(left_son_ptr->node_range_z[0], right_son_ptr->node_range_z[0]), root->point.z); + tmp_range_z[1] = max(max(left_son_ptr->node_range_z[1], right_son_ptr->node_range_z[1]), root->point.z); + } + else + { + if (!left_son_ptr->tree_deleted) + { + tmp_range_x[0] = min(tmp_range_x[0], left_son_ptr->node_range_x[0]); + tmp_range_x[1] = max(tmp_range_x[1], left_son_ptr->node_range_x[1]); + tmp_range_y[0] = min(tmp_range_y[0], left_son_ptr->node_range_y[0]); + tmp_range_y[1] = max(tmp_range_y[1], left_son_ptr->node_range_y[1]); + tmp_range_z[0] = min(tmp_range_z[0], left_son_ptr->node_range_z[0]); + tmp_range_z[1] = max(tmp_range_z[1], left_son_ptr->node_range_z[1]); + } + if (!right_son_ptr->tree_deleted) + { + tmp_range_x[0] = min(tmp_range_x[0], right_son_ptr->node_range_x[0]); + tmp_range_x[1] = max(tmp_range_x[1], right_son_ptr->node_range_x[1]); + tmp_range_y[0] = min(tmp_range_y[0], right_son_ptr->node_range_y[0]); + tmp_range_y[1] = max(tmp_range_y[1], right_son_ptr->node_range_y[1]); + tmp_range_z[0] = min(tmp_range_z[0], right_son_ptr->node_range_z[0]); + tmp_range_z[1] = max(tmp_range_z[1], right_son_ptr->node_range_z[1]); + } + if (!root->point_deleted) + { + tmp_range_x[0] = min(tmp_range_x[0], root->point.x); + tmp_range_x[1] = max(tmp_range_x[1], root->point.x); + tmp_range_y[0] = min(tmp_range_y[0], root->point.y); + tmp_range_y[1] = max(tmp_range_y[1], root->point.y); + tmp_range_z[0] = min(tmp_range_z[0], root->point.z); + tmp_range_z[1] = max(tmp_range_z[1], root->point.z); + } + } + } + else if (left_son_ptr != nullptr) + { + root->TreeSize = left_son_ptr->TreeSize + 1; + root->invalid_point_num = left_son_ptr->invalid_point_num + (root->point_deleted ? 1 : 0); + root->down_del_num = left_son_ptr->down_del_num + (root->point_downsample_deleted ? 1 : 0); + root->tree_downsample_deleted = left_son_ptr->tree_downsample_deleted & root->point_downsample_deleted; + root->tree_deleted = left_son_ptr->tree_deleted && root->point_deleted; + if (root->tree_deleted || (!left_son_ptr->tree_deleted && !root->point_deleted)) + { + tmp_range_x[0] = min(left_son_ptr->node_range_x[0], root->point.x); + tmp_range_x[1] = max(left_son_ptr->node_range_x[1], root->point.x); + tmp_range_y[0] = min(left_son_ptr->node_range_y[0], root->point.y); + tmp_range_y[1] = max(left_son_ptr->node_range_y[1], root->point.y); + tmp_range_z[0] = min(left_son_ptr->node_range_z[0], root->point.z); + tmp_range_z[1] = max(left_son_ptr->node_range_z[1], root->point.z); + } + else + { + if (!left_son_ptr->tree_deleted) + { + tmp_range_x[0] = min(tmp_range_x[0], left_son_ptr->node_range_x[0]); + tmp_range_x[1] = max(tmp_range_x[1], left_son_ptr->node_range_x[1]); + tmp_range_y[0] = min(tmp_range_y[0], left_son_ptr->node_range_y[0]); + tmp_range_y[1] = max(tmp_range_y[1], left_son_ptr->node_range_y[1]); + tmp_range_z[0] = min(tmp_range_z[0], left_son_ptr->node_range_z[0]); + tmp_range_z[1] = max(tmp_range_z[1], left_son_ptr->node_range_z[1]); + } + if (!root->point_deleted) + { + tmp_range_x[0] = min(tmp_range_x[0], root->point.x); + tmp_range_x[1] = max(tmp_range_x[1], root->point.x); + tmp_range_y[0] = min(tmp_range_y[0], root->point.y); + tmp_range_y[1] = max(tmp_range_y[1], root->point.y); + tmp_range_z[0] = min(tmp_range_z[0], root->point.z); + tmp_range_z[1] = max(tmp_range_z[1], root->point.z); + } + } + } + else if (right_son_ptr != nullptr) + { + root->TreeSize = right_son_ptr->TreeSize + 1; + root->invalid_point_num = right_son_ptr->invalid_point_num + (root->point_deleted ? 1 : 0); + root->down_del_num = right_son_ptr->down_del_num + (root->point_downsample_deleted ? 1 : 0); + root->tree_downsample_deleted = right_son_ptr->tree_downsample_deleted & root->point_downsample_deleted; + root->tree_deleted = right_son_ptr->tree_deleted && root->point_deleted; + if (root->tree_deleted || (!right_son_ptr->tree_deleted && !root->point_deleted)) + { + tmp_range_x[0] = min(right_son_ptr->node_range_x[0], root->point.x); + tmp_range_x[1] = max(right_son_ptr->node_range_x[1], root->point.x); + tmp_range_y[0] = min(right_son_ptr->node_range_y[0], root->point.y); + tmp_range_y[1] = max(right_son_ptr->node_range_y[1], root->point.y); + tmp_range_z[0] = min(right_son_ptr->node_range_z[0], root->point.z); + tmp_range_z[1] = max(right_son_ptr->node_range_z[1], root->point.z); + } + else + { + if (!right_son_ptr->tree_deleted) + { + tmp_range_x[0] = min(tmp_range_x[0], right_son_ptr->node_range_x[0]); + tmp_range_x[1] = max(tmp_range_x[1], right_son_ptr->node_range_x[1]); + tmp_range_y[0] = min(tmp_range_y[0], right_son_ptr->node_range_y[0]); + tmp_range_y[1] = max(tmp_range_y[1], right_son_ptr->node_range_y[1]); + tmp_range_z[0] = min(tmp_range_z[0], right_son_ptr->node_range_z[0]); + tmp_range_z[1] = max(tmp_range_z[1], right_son_ptr->node_range_z[1]); + } + if (!root->point_deleted) + { + tmp_range_x[0] = min(tmp_range_x[0], root->point.x); + tmp_range_x[1] = max(tmp_range_x[1], root->point.x); + tmp_range_y[0] = min(tmp_range_y[0], root->point.y); + tmp_range_y[1] = max(tmp_range_y[1], root->point.y); + tmp_range_z[0] = min(tmp_range_z[0], root->point.z); + tmp_range_z[1] = max(tmp_range_z[1], root->point.z); + } + } + } + else + { + root->TreeSize = 1; + root->invalid_point_num = (root->point_deleted ? 1 : 0); + root->down_del_num = (root->point_downsample_deleted ? 1 : 0); + root->tree_downsample_deleted = root->point_downsample_deleted; + root->tree_deleted = root->point_deleted; + tmp_range_x[0] = root->point.x; + tmp_range_x[1] = root->point.x; + tmp_range_y[0] = root->point.y; + tmp_range_y[1] = root->point.y; + tmp_range_z[0] = root->point.z; + tmp_range_z[1] = root->point.z; + } + memcpy(root->node_range_x, tmp_range_x, sizeof(tmp_range_x)); + memcpy(root->node_range_y, tmp_range_y, sizeof(tmp_range_y)); + memcpy(root->node_range_z, tmp_range_z, sizeof(tmp_range_z)); + float x_L = (root->node_range_x[1] - root->node_range_x[0]) * 0.5; + float y_L = (root->node_range_y[1] - root->node_range_y[0]) * 0.5; + float z_L = (root->node_range_z[1] - root->node_range_z[0]) * 0.5; + root->radius_sq = x_L*x_L + y_L * y_L + z_L * z_L; + if (left_son_ptr != nullptr) + left_son_ptr->father_ptr = root; + if (right_son_ptr != nullptr) + right_son_ptr->father_ptr = root; + if (root == Root_Node && root->TreeSize > 3) + { + KD_TREE_NODE *son_ptr = root->left_son_ptr; + if (son_ptr == nullptr) + son_ptr = root->right_son_ptr; + float tmp_bal = float(son_ptr->TreeSize) / (root->TreeSize - 1); + root->alpha_del = float(root->invalid_point_num) / root->TreeSize; + root->alpha_bal = (tmp_bal >= 0.5 - EPSS) ? tmp_bal : 1 - tmp_bal; + } + return; +} + +template +void KD_TREE::flatten(KD_TREE_NODE *root, PointVector &Storage, delete_point_storage_set storage_type) +{ + if (root == nullptr) + return; + Push_Down(root); + if (!root->point_deleted) + { + Storage.push_back(root->point); + } + flatten(root->left_son_ptr, Storage, storage_type); + flatten(root->right_son_ptr, Storage, storage_type); + switch (storage_type) + { + case NOT_RECORD: + break; + case DELETE_POINTS_REC: + if (root->point_deleted && !root->point_downsample_deleted) + { + Points_deleted.push_back(root->point); + } + break; + case MULTI_THREAD_REC: + if (root->point_deleted && !root->point_downsample_deleted) + { + Multithread_Points_deleted.push_back(root->point); + } + break; + default: + break; + } + return; +} + +template +void KD_TREE::delete_tree_nodes(KD_TREE_NODE **root) +{ + if (*root == nullptr) + return; + Push_Down(*root); + delete_tree_nodes(&(*root)->left_son_ptr); + delete_tree_nodes(&(*root)->right_son_ptr); + + pthread_mutex_destroy(&(*root)->push_down_mutex_lock); + delete *root; + *root = nullptr; + + return; +} + +template +bool KD_TREE::same_point(PointType a, PointType b) +{ + return (fabs(a.x - b.x) < EPSS && fabs(a.y - b.y) < EPSS && fabs(a.z - b.z) < EPSS); +} + +template +float KD_TREE::calc_dist(PointType a, PointType b) +{ + float dist = 0.0f; + dist = (a.x - b.x) * (a.x - b.x) + (a.y - b.y) * (a.y - b.y) + (a.z - b.z) * (a.z - b.z); + return dist; +} + +template +float KD_TREE::calc_box_dist(KD_TREE_NODE *node, PointType point) +{ + if (node == nullptr) + return INFINITY; + float min_dist = 0.0; + if (point.x < node->node_range_x[0]) + min_dist += (point.x - node->node_range_x[0]) * (point.x - node->node_range_x[0]); + if (point.x > node->node_range_x[1]) + min_dist += (point.x - node->node_range_x[1]) * (point.x - node->node_range_x[1]); + if (point.y < node->node_range_y[0]) + min_dist += (point.y - node->node_range_y[0]) * (point.y - node->node_range_y[0]); + if (point.y > node->node_range_y[1]) + min_dist += (point.y - node->node_range_y[1]) * (point.y - node->node_range_y[1]); + if (point.z < node->node_range_z[0]) + min_dist += (point.z - node->node_range_z[0]) * (point.z - node->node_range_z[0]); + if (point.z > node->node_range_z[1]) + min_dist += (point.z - node->node_range_z[1]) * (point.z - node->node_range_z[1]); + return min_dist; +} +template +bool KD_TREE::point_cmp_x(PointType a, PointType b) { return a.x < b.x; } +template +bool KD_TREE::point_cmp_y(PointType a, PointType b) { return a.y < b.y; } +template +bool KD_TREE::point_cmp_z(PointType a, PointType b) { return a.z < b.z; } + +// Manual heap + + + +// manual queue + + +// Manual Instatiations +template class KD_TREE; +template class KD_TREE; +template class KD_TREE; + diff --git a/src/FAST_LIO/include/ikd-Tree/ikd_Tree.h b/src/FAST_LIO/include/ikd-Tree/ikd_Tree.h new file mode 100644 index 0000000..d4b302e --- /dev/null +++ b/src/FAST_LIO/include/ikd-Tree/ikd_Tree.h @@ -0,0 +1,344 @@ +#pragma once +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#define EPSS 1e-6 +#define Minimal_Unbalanced_Tree_Size 10 +#define Multi_Thread_Rebuild_Point_Num 1500 +#define DOWNSAMPLE_SWITCH true +#define ForceRebuildPercentage 0.2 +#define Q_LEN 1000000 + +using namespace std; + +// typedef pcl::PointXYZINormal PointType; +// typedef vector> PointVector; + +struct BoxPointType +{ + float vertex_min[3]; + float vertex_max[3]; +}; + +enum operation_set +{ + ADD_POINT, + DELETE_POINT, + DELETE_BOX, + ADD_BOX, + DOWNSAMPLE_DELETE, + PUSH_DOWN +}; + +enum delete_point_storage_set +{ + NOT_RECORD, + DELETE_POINTS_REC, + MULTI_THREAD_REC +}; + +template +class KD_TREE +{ + // using MANUAL_Q_ = MANUAL_Q; + // using PointVector = std::vector; + + // using MANUAL_Q_ = MANUAL_Q; +public: + using PointVector = std::vector>; + using Ptr = std::shared_ptr>; + + struct KD_TREE_NODE + { + PointType point; + int division_axis; + int TreeSize = 1; + int invalid_point_num = 0; + int down_del_num = 0; + bool point_deleted = false; + bool tree_deleted = false; + bool point_downsample_deleted = false; + bool tree_downsample_deleted = false; + bool need_push_down_to_left = false; + bool need_push_down_to_right = false; + bool working_flag = false; + pthread_mutex_t push_down_mutex_lock; + float node_range_x[2], node_range_y[2], node_range_z[2]; + float radius_sq; + KD_TREE_NODE *left_son_ptr = nullptr; + KD_TREE_NODE *right_son_ptr = nullptr; + KD_TREE_NODE *father_ptr = nullptr; + // For paper data record + float alpha_del; + float alpha_bal; + }; + + struct Operation_Logger_Type + { + PointType point; + BoxPointType boxpoint; + bool tree_deleted, tree_downsample_deleted; + operation_set op; + }; + // static const PointType zeroP; + + struct PointType_CMP + { + PointType point; + float dist = 0.0; + PointType_CMP(PointType p = PointType(), float d = INFINITY) + { + this->point = p; + this->dist = d; + }; + bool operator<(const PointType_CMP &a) const + { + if (fabs(dist - a.dist) < 1e-10) + return point.x < a.point.x; + else + return dist < a.dist; + } + }; + + class MANUAL_HEAP + { + + public: + MANUAL_HEAP(int max_capacity = 100) + + { + cap = max_capacity; + heap = new PointType_CMP[max_capacity]; + heap_size = 0; + } + + ~MANUAL_HEAP() + { + delete[] heap; + } + void pop() + { + if (heap_size == 0) + return; + heap[0] = heap[heap_size - 1]; + heap_size--; + MoveDown(0); + return; + } + PointType_CMP top() + { + return heap[0]; + } + void push(PointType_CMP point) + { + if (heap_size >= cap) + return; + heap[heap_size] = point; + FloatUp(heap_size); + heap_size++; + return; + } + int size() + { + return heap_size; + } + void clear() + { + heap_size = 0; + return; + } + + private: + PointType_CMP *heap; + void MoveDown(int heap_index) + { + int l = heap_index * 2 + 1; + PointType_CMP tmp = heap[heap_index]; + while (l < heap_size) + { + if (l + 1 < heap_size && heap[l] < heap[l + 1]) + l++; + if (tmp < heap[l]) + { + heap[heap_index] = heap[l]; + heap_index = l; + l = heap_index * 2 + 1; + } + else + break; + } + heap[heap_index] = tmp; + return; + } + void FloatUp(int heap_index) + { + int ancestor = (heap_index - 1) / 2; + PointType_CMP tmp = heap[heap_index]; + while (heap_index > 0) + { + if (heap[ancestor] < tmp) + { + heap[heap_index] = heap[ancestor]; + heap_index = ancestor; + ancestor = (heap_index - 1) / 2; + } + else + break; + } + heap[heap_index] = tmp; + return; + } + int heap_size = 0; + int cap = 0; + }; + + class MANUAL_Q + { + private: + int head = 0, tail = 0, counter = 0; + Operation_Logger_Type q[Q_LEN]; + bool is_empty; + + public: + void pop() + { + if (counter == 0) + return; + head++; + head %= Q_LEN; + counter--; + if (counter == 0) + is_empty = true; + return; + } + Operation_Logger_Type front() + { + return q[head]; + } + Operation_Logger_Type back() + { + return q[tail]; + } + void clear() + { + head = 0; + tail = 0; + counter = 0; + is_empty = true; + return; + } + void push(Operation_Logger_Type op) + { + q[tail] = op; + counter++; + if (is_empty) + is_empty = false; + tail++; + tail %= Q_LEN; + } + bool empty() + { + return is_empty; + } + int size() + { + return counter; + } + }; + +private: + // Multi-thread Tree Rebuild + bool termination_flag = false; + bool rebuild_flag = false; + pthread_t rebuild_thread; + pthread_mutex_t termination_flag_mutex_lock, rebuild_ptr_mutex_lock, working_flag_mutex, search_flag_mutex; + pthread_mutex_t rebuild_logger_mutex_lock, points_deleted_rebuild_mutex_lock; + // queue Rebuild_Logger; + MANUAL_Q Rebuild_Logger; + PointVector Rebuild_PCL_Storage; + KD_TREE_NODE **Rebuild_Ptr = nullptr; + int search_mutex_counter = 0; + static void *multi_thread_ptr(void *arg); + void multi_thread_rebuild(); + void start_thread(); + void stop_thread(); + void run_operation(KD_TREE_NODE **root, Operation_Logger_Type operation); + // KD Tree Functions and augmented variables + int Treesize_tmp = 0, Validnum_tmp = 0; + float alpha_bal_tmp = 0.5, alpha_del_tmp = 0.0; + float delete_criterion_param = 0.5f; + float balance_criterion_param = 0.7f; + float downsample_size = 0.2f; + bool Delete_Storage_Disabled = false; + KD_TREE_NODE *STATIC_ROOT_NODE = nullptr; + PointVector Points_deleted; + PointVector Downsample_Storage; + PointVector Multithread_Points_deleted; + void InitTreeNode(KD_TREE_NODE *root); + void Test_Lock_States(KD_TREE_NODE *root); + void BuildTree(KD_TREE_NODE **root, int l, int r, PointVector &Storage); + void Rebuild(KD_TREE_NODE **root); + int Delete_by_range(KD_TREE_NODE **root, BoxPointType boxpoint, bool allow_rebuild, bool is_downsample); + void Delete_by_point(KD_TREE_NODE **root, PointType point, bool allow_rebuild); + void Add_by_point(KD_TREE_NODE **root, PointType point, bool allow_rebuild, int father_axis); + void Add_by_range(KD_TREE_NODE **root, BoxPointType boxpoint, bool allow_rebuild); + void Search(KD_TREE_NODE *root, int k_nearest, PointType point, MANUAL_HEAP &q, float max_dist); //priority_queue + void Search_by_range(KD_TREE_NODE *root, BoxPointType boxpoint, PointVector &Storage); + void Search_by_radius(KD_TREE_NODE *root, PointType point, float radius, PointVector &Storage); + bool Criterion_Check(KD_TREE_NODE *root); + void Push_Down(KD_TREE_NODE *root); + void Update(KD_TREE_NODE *root); + void delete_tree_nodes(KD_TREE_NODE **root); + void downsample(KD_TREE_NODE **root); + bool same_point(PointType a, PointType b); + float calc_dist(PointType a, PointType b); + float calc_box_dist(KD_TREE_NODE *node, PointType point); + static bool point_cmp_x(PointType a, PointType b); + static bool point_cmp_y(PointType a, PointType b); + static bool point_cmp_z(PointType a, PointType b); + +public: + KD_TREE(float delete_param = 0.5, float balance_param = 0.6, float box_length = 0.2); + ~KD_TREE(); + void Set_delete_criterion_param(float delete_param) + { + delete_criterion_param = delete_param; + } + void Set_balance_criterion_param(float balance_param) + { + balance_criterion_param = balance_param; + } + void set_downsample_param(float downsample_param) + { + downsample_size = downsample_param; + } + void InitializeKDTree(float delete_param = 0.5, float balance_param = 0.7, float box_length = 0.2); + int size(); + int validnum(); + void root_alpha(float &alpha_bal, float &alpha_del); + void Build(PointVector point_cloud); + void Nearest_Search(PointType point, int k_nearest, PointVector &Nearest_Points, vector &Point_Distance, float max_dist = INFINITY); + void Box_Search(const BoxPointType &Box_of_Point, PointVector &Storage); + void Radius_Search(PointType point, const float radius, PointVector &Storage); + int Add_Points(PointVector &PointToAdd, bool downsample_on); + void Add_Point_Boxes(vector &BoxPoints); + void Delete_Points(PointVector &PointToDel); + int Delete_Point_Boxes(vector &BoxPoints); + void flatten(KD_TREE_NODE *root, PointVector &Storage, delete_point_storage_set storage_type); + void acquire_removed_points(PointVector &removed_points); + BoxPointType tree_range(); + PointVector PCL_Storage; + KD_TREE_NODE *Root_Node = nullptr; + int max_queue_size = 0; +}; + +// template +// PointType KD_TREE::zeroP = PointType(0,0,0); diff --git a/src/FAST_LIO/include/matplotlibcpp.h b/src/FAST_LIO/include/matplotlibcpp.h new file mode 100644 index 0000000..6855445 --- /dev/null +++ b/src/FAST_LIO/include/matplotlibcpp.h @@ -0,0 +1,2499 @@ +#pragma once + +// Python headers must be included before any system headers, since +// they define _POSIX_C_SOURCE +#include + +#include +#include +#include +#include +#include +#include +#include +#include // requires c++11 support +#include + +#ifndef WITHOUT_NUMPY +# define NPY_NO_DEPRECATED_API NPY_1_7_API_VERSION +# include + +# ifdef WITH_OPENCV +# include +# endif // WITH_OPENCV + +/* + * A bunch of constants were removed in OpenCV 4 in favour of enum classes, so + * define the ones we need here. + */ +# if CV_MAJOR_VERSION > 3 +# define CV_BGR2RGB cv::COLOR_BGR2RGB +# define CV_BGRA2RGBA cv::COLOR_BGRA2RGBA +# endif +#endif // WITHOUT_NUMPY + +#if PY_MAJOR_VERSION >= 3 +# define PyString_FromString PyUnicode_FromString +# define PyInt_FromLong PyLong_FromLong +# define PyString_FromString PyUnicode_FromString +#endif + + +namespace matplotlibcpp { +namespace detail { + +static std::string s_backend; + +struct _interpreter { + PyObject* s_python_function_arrow; + PyObject *s_python_function_show; + PyObject *s_python_function_close; + PyObject *s_python_function_draw; + PyObject *s_python_function_pause; + PyObject *s_python_function_save; + PyObject *s_python_function_figure; + PyObject *s_python_function_fignum_exists; + PyObject *s_python_function_plot; + PyObject *s_python_function_quiver; + PyObject* s_python_function_contour; + PyObject *s_python_function_semilogx; + PyObject *s_python_function_semilogy; + PyObject *s_python_function_loglog; + PyObject *s_python_function_fill; + PyObject *s_python_function_fill_between; + PyObject *s_python_function_hist; + PyObject *s_python_function_imshow; + PyObject *s_python_function_scatter; + PyObject *s_python_function_boxplot; + PyObject *s_python_function_subplot; + PyObject *s_python_function_subplot2grid; + PyObject *s_python_function_legend; + PyObject *s_python_function_xlim; + PyObject *s_python_function_ion; + PyObject *s_python_function_ginput; + PyObject *s_python_function_ylim; + PyObject *s_python_function_title; + PyObject *s_python_function_axis; + PyObject *s_python_function_axvline; + PyObject *s_python_function_axvspan; + PyObject *s_python_function_xlabel; + PyObject *s_python_function_ylabel; + PyObject *s_python_function_gca; + PyObject *s_python_function_xticks; + PyObject *s_python_function_yticks; + PyObject* s_python_function_margins; + PyObject *s_python_function_tick_params; + PyObject *s_python_function_grid; + PyObject* s_python_function_cla; + PyObject *s_python_function_clf; + PyObject *s_python_function_errorbar; + PyObject *s_python_function_annotate; + PyObject *s_python_function_tight_layout; + PyObject *s_python_colormap; + PyObject *s_python_empty_tuple; + PyObject *s_python_function_stem; + PyObject *s_python_function_xkcd; + PyObject *s_python_function_text; + PyObject *s_python_function_suptitle; + PyObject *s_python_function_bar; + PyObject *s_python_function_colorbar; + PyObject *s_python_function_subplots_adjust; + + + /* For now, _interpreter is implemented as a singleton since its currently not possible to have + multiple independent embedded python interpreters without patching the python source code + or starting a separate process for each. + http://bytes.com/topic/python/answers/793370-multiple-independent-python-interpreters-c-c-program + */ + + static _interpreter& get() { + static _interpreter ctx; + return ctx; + } + + PyObject* safe_import(PyObject* module, std::string fname) { + PyObject* fn = PyObject_GetAttrString(module, fname.c_str()); + + if (!fn) + throw std::runtime_error(std::string("Couldn't find required function: ") + fname); + + if (!PyFunction_Check(fn)) + throw std::runtime_error(fname + std::string(" is unexpectedly not a PyFunction.")); + + return fn; + } + +private: + +#ifndef WITHOUT_NUMPY +# if PY_MAJOR_VERSION >= 3 + + void *import_numpy() { + import_array(); // initialize C-API + return NULL; + } + +# else + + void import_numpy() { + import_array(); // initialize C-API + } + +# endif +#endif + + _interpreter() { + + // optional but recommended +#if PY_MAJOR_VERSION >= 3 + wchar_t name[] = L"plotting"; +#else + char name[] = "plotting"; +#endif + Py_SetProgramName(name); + Py_Initialize(); + +#ifndef WITHOUT_NUMPY + import_numpy(); // initialize numpy C-API +#endif + + PyObject* matplotlibname = PyString_FromString("matplotlib"); + PyObject* pyplotname = PyString_FromString("matplotlib.pyplot"); + PyObject* cmname = PyString_FromString("matplotlib.cm"); + PyObject* pylabname = PyString_FromString("pylab"); + if (!pyplotname || !pylabname || !matplotlibname || !cmname) { + throw std::runtime_error("couldnt create string"); + } + + PyObject* matplotlib = PyImport_Import(matplotlibname); + Py_DECREF(matplotlibname); + if (!matplotlib) { + PyErr_Print(); + throw std::runtime_error("Error loading module matplotlib!"); + } + + // matplotlib.use() must be called *before* pylab, matplotlib.pyplot, + // or matplotlib.backends is imported for the first time + if (!s_backend.empty()) { + PyObject_CallMethod(matplotlib, const_cast("use"), const_cast("s"), s_backend.c_str()); + } + + PyObject* pymod = PyImport_Import(pyplotname); + Py_DECREF(pyplotname); + if (!pymod) { throw std::runtime_error("Error loading module matplotlib.pyplot!"); } + + s_python_colormap = PyImport_Import(cmname); + Py_DECREF(cmname); + if (!s_python_colormap) { throw std::runtime_error("Error loading module matplotlib.cm!"); } + + PyObject* pylabmod = PyImport_Import(pylabname); + Py_DECREF(pylabname); + if (!pylabmod) { throw std::runtime_error("Error loading module pylab!"); } + + s_python_function_arrow = safe_import(pymod, "arrow"); + s_python_function_show = safe_import(pymod, "show"); + s_python_function_close = safe_import(pymod, "close"); + s_python_function_draw = safe_import(pymod, "draw"); + s_python_function_pause = safe_import(pymod, "pause"); + s_python_function_figure = safe_import(pymod, "figure"); + s_python_function_fignum_exists = safe_import(pymod, "fignum_exists"); + s_python_function_plot = safe_import(pymod, "plot"); + s_python_function_quiver = safe_import(pymod, "quiver"); + s_python_function_contour = safe_import(pymod, "contour"); + s_python_function_semilogx = safe_import(pymod, "semilogx"); + s_python_function_semilogy = safe_import(pymod, "semilogy"); + s_python_function_loglog = safe_import(pymod, "loglog"); + s_python_function_fill = safe_import(pymod, "fill"); + s_python_function_fill_between = safe_import(pymod, "fill_between"); + s_python_function_hist = safe_import(pymod,"hist"); + s_python_function_scatter = safe_import(pymod,"scatter"); + s_python_function_boxplot = safe_import(pymod,"boxplot"); + s_python_function_subplot = safe_import(pymod, "subplot"); + s_python_function_subplot2grid = safe_import(pymod, "subplot2grid"); + s_python_function_legend = safe_import(pymod, "legend"); + s_python_function_ylim = safe_import(pymod, "ylim"); + s_python_function_title = safe_import(pymod, "title"); + s_python_function_axis = safe_import(pymod, "axis"); + s_python_function_axvline = safe_import(pymod, "axvline"); + s_python_function_axvspan = safe_import(pymod, "axvspan"); + s_python_function_xlabel = safe_import(pymod, "xlabel"); + s_python_function_ylabel = safe_import(pymod, "ylabel"); + s_python_function_gca = safe_import(pymod, "gca"); + s_python_function_xticks = safe_import(pymod, "xticks"); + s_python_function_yticks = safe_import(pymod, "yticks"); + s_python_function_margins = safe_import(pymod, "margins"); + s_python_function_tick_params = safe_import(pymod, "tick_params"); + s_python_function_grid = safe_import(pymod, "grid"); + s_python_function_xlim = safe_import(pymod, "xlim"); + s_python_function_ion = safe_import(pymod, "ion"); + s_python_function_ginput = safe_import(pymod, "ginput"); + s_python_function_save = safe_import(pylabmod, "savefig"); + s_python_function_annotate = safe_import(pymod,"annotate"); + s_python_function_cla = safe_import(pymod, "cla"); + s_python_function_clf = safe_import(pymod, "clf"); + s_python_function_errorbar = safe_import(pymod, "errorbar"); + s_python_function_tight_layout = safe_import(pymod, "tight_layout"); + s_python_function_stem = safe_import(pymod, "stem"); + s_python_function_xkcd = safe_import(pymod, "xkcd"); + s_python_function_text = safe_import(pymod, "text"); + s_python_function_suptitle = safe_import(pymod, "suptitle"); + s_python_function_bar = safe_import(pymod,"bar"); + s_python_function_colorbar = PyObject_GetAttrString(pymod, "colorbar"); + s_python_function_subplots_adjust = safe_import(pymod,"subplots_adjust"); +#ifndef WITHOUT_NUMPY + s_python_function_imshow = safe_import(pymod, "imshow"); +#endif + s_python_empty_tuple = PyTuple_New(0); + } + + ~_interpreter() { + Py_Finalize(); + } +}; + +} // end namespace detail + +/// Select the backend +/// +/// **NOTE:** This must be called before the first plot command to have +/// any effect. +/// +/// Mainly useful to select the non-interactive 'Agg' backend when running +/// matplotlibcpp in headless mode, for example on a machine with no display. +/// +/// See also: https://matplotlib.org/2.0.2/api/matplotlib_configuration_api.html#matplotlib.use +inline void backend(const std::string& name) +{ + detail::s_backend = name; +} + +inline bool annotate(std::string annotation, double x, double y) +{ + detail::_interpreter::get(); + + PyObject * xy = PyTuple_New(2); + PyObject * str = PyString_FromString(annotation.c_str()); + + PyTuple_SetItem(xy,0,PyFloat_FromDouble(x)); + PyTuple_SetItem(xy,1,PyFloat_FromDouble(y)); + + PyObject* kwargs = PyDict_New(); + PyDict_SetItemString(kwargs, "xy", xy); + + PyObject* args = PyTuple_New(1); + PyTuple_SetItem(args, 0, str); + + PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_annotate, args, kwargs); + + Py_DECREF(args); + Py_DECREF(kwargs); + + if(res) Py_DECREF(res); + + return res; +} + +namespace detail { + +#ifndef WITHOUT_NUMPY +// Type selector for numpy array conversion +template struct select_npy_type { const static NPY_TYPES type = NPY_NOTYPE; }; //Default +template <> struct select_npy_type { const static NPY_TYPES type = NPY_DOUBLE; }; +template <> struct select_npy_type { const static NPY_TYPES type = NPY_FLOAT; }; +template <> struct select_npy_type { const static NPY_TYPES type = NPY_BOOL; }; +template <> struct select_npy_type { const static NPY_TYPES type = NPY_INT8; }; +template <> struct select_npy_type { const static NPY_TYPES type = NPY_SHORT; }; +template <> struct select_npy_type { const static NPY_TYPES type = NPY_INT; }; +template <> struct select_npy_type { const static NPY_TYPES type = NPY_INT64; }; +template <> struct select_npy_type { const static NPY_TYPES type = NPY_UINT8; }; +template <> struct select_npy_type { const static NPY_TYPES type = NPY_USHORT; }; +template <> struct select_npy_type { const static NPY_TYPES type = NPY_ULONG; }; +template <> struct select_npy_type { const static NPY_TYPES type = NPY_UINT64; }; + +// Sanity checks; comment them out or change the numpy type below if you're compiling on +// a platform where they don't apply +static_assert(sizeof(long long) == 8); +template <> struct select_npy_type { const static NPY_TYPES type = NPY_INT64; }; +static_assert(sizeof(unsigned long long) == 8); +template <> struct select_npy_type { const static NPY_TYPES type = NPY_UINT64; }; +// TODO: add int, long, etc. + +template +PyObject* get_array(const std::vector& v) +{ + npy_intp vsize = v.size(); + NPY_TYPES type = select_npy_type::type; + if (type == NPY_NOTYPE) { + size_t memsize = v.size()*sizeof(double); + double* dp = static_cast(::malloc(memsize)); + for (size_t i=0; i(varray), NPY_ARRAY_OWNDATA); + return varray; + } + + PyObject* varray = PyArray_SimpleNewFromData(1, &vsize, type, (void*)(v.data())); + return varray; +} + + +template +PyObject* get_2darray(const std::vector<::std::vector>& v) +{ + if (v.size() < 1) throw std::runtime_error("get_2d_array v too small"); + + npy_intp vsize[2] = {static_cast(v.size()), + static_cast(v[0].size())}; + + PyArrayObject *varray = + (PyArrayObject *)PyArray_SimpleNew(2, vsize, NPY_DOUBLE); + + double *vd_begin = static_cast(PyArray_DATA(varray)); + + for (const ::std::vector &v_row : v) { + if (v_row.size() != static_cast(vsize[1])) + throw std::runtime_error("Missmatched array size"); + std::copy(v_row.begin(), v_row.end(), vd_begin); + vd_begin += vsize[1]; + } + + return reinterpret_cast(varray); +} + +#else // fallback if we don't have numpy: copy every element of the given vector + +template +PyObject* get_array(const std::vector& v) +{ + PyObject* list = PyList_New(v.size()); + for(size_t i = 0; i < v.size(); ++i) { + PyList_SetItem(list, i, PyFloat_FromDouble(v.at(i))); + } + return list; +} + +#endif // WITHOUT_NUMPY + +// sometimes, for labels and such, we need string arrays +inline PyObject * get_array(const std::vector& strings) +{ + PyObject* list = PyList_New(strings.size()); + for (std::size_t i = 0; i < strings.size(); ++i) { + PyList_SetItem(list, i, PyString_FromString(strings[i].c_str())); + } + return list; +} + +// not all matplotlib need 2d arrays, some prefer lists of lists +template +PyObject* get_listlist(const std::vector>& ll) +{ + PyObject* listlist = PyList_New(ll.size()); + for (std::size_t i = 0; i < ll.size(); ++i) { + PyList_SetItem(listlist, i, get_array(ll[i])); + } + return listlist; +} + +} // namespace detail + +/// Plot a line through the given x and y data points.. +/// +/// See: https://matplotlib.org/3.2.1/api/_as_gen/matplotlib.pyplot.plot.html +template +bool plot(const std::vector &x, const std::vector &y, const std::map& keywords) +{ + assert(x.size() == y.size()); + + detail::_interpreter::get(); + + // using numpy arrays + PyObject* xarray = detail::get_array(x); + PyObject* yarray = detail::get_array(y); + + // construct positional args + PyObject* args = PyTuple_New(2); + PyTuple_SetItem(args, 0, xarray); + PyTuple_SetItem(args, 1, yarray); + + // construct keyword args + PyObject* kwargs = PyDict_New(); + for(std::map::const_iterator it = keywords.begin(); it != keywords.end(); ++it) + { + PyDict_SetItemString(kwargs, it->first.c_str(), PyString_FromString(it->second.c_str())); + } + + PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_plot, args, kwargs); + + Py_DECREF(args); + Py_DECREF(kwargs); + if(res) Py_DECREF(res); + + return res; +} + +// TODO - it should be possible to make this work by implementing +// a non-numpy alternative for `detail::get_2darray()`. +#ifndef WITHOUT_NUMPY +template +void plot_surface(const std::vector<::std::vector> &x, + const std::vector<::std::vector> &y, + const std::vector<::std::vector> &z, + const std::map &keywords = + std::map()) +{ + detail::_interpreter::get(); + + // We lazily load the modules here the first time this function is called + // because I'm not sure that we can assume "matplotlib installed" implies + // "mpl_toolkits installed" on all platforms, and we don't want to require + // it for people who don't need 3d plots. + static PyObject *mpl_toolkitsmod = nullptr, *axis3dmod = nullptr; + if (!mpl_toolkitsmod) { + detail::_interpreter::get(); + + PyObject* mpl_toolkits = PyString_FromString("mpl_toolkits"); + PyObject* axis3d = PyString_FromString("mpl_toolkits.mplot3d"); + if (!mpl_toolkits || !axis3d) { throw std::runtime_error("couldnt create string"); } + + mpl_toolkitsmod = PyImport_Import(mpl_toolkits); + Py_DECREF(mpl_toolkits); + if (!mpl_toolkitsmod) { throw std::runtime_error("Error loading module mpl_toolkits!"); } + + axis3dmod = PyImport_Import(axis3d); + Py_DECREF(axis3d); + if (!axis3dmod) { throw std::runtime_error("Error loading module mpl_toolkits.mplot3d!"); } + } + + assert(x.size() == y.size()); + assert(y.size() == z.size()); + + // using numpy arrays + PyObject *xarray = detail::get_2darray(x); + PyObject *yarray = detail::get_2darray(y); + PyObject *zarray = detail::get_2darray(z); + + // construct positional args + PyObject *args = PyTuple_New(3); + PyTuple_SetItem(args, 0, xarray); + PyTuple_SetItem(args, 1, yarray); + PyTuple_SetItem(args, 2, zarray); + + // Build up the kw args. + PyObject *kwargs = PyDict_New(); + PyDict_SetItemString(kwargs, "rstride", PyInt_FromLong(1)); + PyDict_SetItemString(kwargs, "cstride", PyInt_FromLong(1)); + + PyObject *python_colormap_coolwarm = PyObject_GetAttrString( + detail::_interpreter::get().s_python_colormap, "coolwarm"); + + PyDict_SetItemString(kwargs, "cmap", python_colormap_coolwarm); + + for (std::map::const_iterator it = keywords.begin(); + it != keywords.end(); ++it) { + PyDict_SetItemString(kwargs, it->first.c_str(), + PyString_FromString(it->second.c_str())); + } + + + PyObject *fig = + PyObject_CallObject(detail::_interpreter::get().s_python_function_figure, + detail::_interpreter::get().s_python_empty_tuple); + if (!fig) throw std::runtime_error("Call to figure() failed."); + + PyObject *gca_kwargs = PyDict_New(); + PyDict_SetItemString(gca_kwargs, "projection", PyString_FromString("3d")); + + PyObject *gca = PyObject_GetAttrString(fig, "gca"); + if (!gca) throw std::runtime_error("No gca"); + Py_INCREF(gca); + PyObject *axis = PyObject_Call( + gca, detail::_interpreter::get().s_python_empty_tuple, gca_kwargs); + + if (!axis) throw std::runtime_error("No axis"); + Py_INCREF(axis); + + Py_DECREF(gca); + Py_DECREF(gca_kwargs); + + PyObject *plot_surface = PyObject_GetAttrString(axis, "plot_surface"); + if (!plot_surface) throw std::runtime_error("No surface"); + Py_INCREF(plot_surface); + PyObject *res = PyObject_Call(plot_surface, args, kwargs); + if (!res) throw std::runtime_error("failed surface"); + Py_DECREF(plot_surface); + + Py_DECREF(axis); + Py_DECREF(args); + Py_DECREF(kwargs); + if (res) Py_DECREF(res); +} +#endif // WITHOUT_NUMPY + +template +void plot3(const std::vector &x, + const std::vector &y, + const std::vector &z, + const std::map &keywords = + std::map()) +{ + detail::_interpreter::get(); + + // Same as with plot_surface: We lazily load the modules here the first time + // this function is called because I'm not sure that we can assume "matplotlib + // installed" implies "mpl_toolkits installed" on all platforms, and we don't + // want to require it for people who don't need 3d plots. + static PyObject *mpl_toolkitsmod = nullptr, *axis3dmod = nullptr; + if (!mpl_toolkitsmod) { + detail::_interpreter::get(); + + PyObject* mpl_toolkits = PyString_FromString("mpl_toolkits"); + PyObject* axis3d = PyString_FromString("mpl_toolkits.mplot3d"); + if (!mpl_toolkits || !axis3d) { throw std::runtime_error("couldnt create string"); } + + mpl_toolkitsmod = PyImport_Import(mpl_toolkits); + Py_DECREF(mpl_toolkits); + if (!mpl_toolkitsmod) { throw std::runtime_error("Error loading module mpl_toolkits!"); } + + axis3dmod = PyImport_Import(axis3d); + Py_DECREF(axis3d); + if (!axis3dmod) { throw std::runtime_error("Error loading module mpl_toolkits.mplot3d!"); } + } + + assert(x.size() == y.size()); + assert(y.size() == z.size()); + + PyObject *xarray = detail::get_array(x); + PyObject *yarray = detail::get_array(y); + PyObject *zarray = detail::get_array(z); + + // construct positional args + PyObject *args = PyTuple_New(3); + PyTuple_SetItem(args, 0, xarray); + PyTuple_SetItem(args, 1, yarray); + PyTuple_SetItem(args, 2, zarray); + + // Build up the kw args. + PyObject *kwargs = PyDict_New(); + + for (std::map::const_iterator it = keywords.begin(); + it != keywords.end(); ++it) { + PyDict_SetItemString(kwargs, it->first.c_str(), + PyString_FromString(it->second.c_str())); + } + + PyObject *fig = + PyObject_CallObject(detail::_interpreter::get().s_python_function_figure, + detail::_interpreter::get().s_python_empty_tuple); + if (!fig) throw std::runtime_error("Call to figure() failed."); + + PyObject *gca_kwargs = PyDict_New(); + PyDict_SetItemString(gca_kwargs, "projection", PyString_FromString("3d")); + + PyObject *gca = PyObject_GetAttrString(fig, "gca"); + if (!gca) throw std::runtime_error("No gca"); + Py_INCREF(gca); + PyObject *axis = PyObject_Call( + gca, detail::_interpreter::get().s_python_empty_tuple, gca_kwargs); + + if (!axis) throw std::runtime_error("No axis"); + Py_INCREF(axis); + + Py_DECREF(gca); + Py_DECREF(gca_kwargs); + + PyObject *plot3 = PyObject_GetAttrString(axis, "plot"); + if (!plot3) throw std::runtime_error("No 3D line plot"); + Py_INCREF(plot3); + PyObject *res = PyObject_Call(plot3, args, kwargs); + if (!res) throw std::runtime_error("Failed 3D line plot"); + Py_DECREF(plot3); + + Py_DECREF(axis); + Py_DECREF(args); + Py_DECREF(kwargs); + if (res) Py_DECREF(res); +} + +template +bool stem(const std::vector &x, const std::vector &y, const std::map& keywords) +{ + assert(x.size() == y.size()); + + detail::_interpreter::get(); + + // using numpy arrays + PyObject* xarray = detail::get_array(x); + PyObject* yarray = detail::get_array(y); + + // construct positional args + PyObject* args = PyTuple_New(2); + PyTuple_SetItem(args, 0, xarray); + PyTuple_SetItem(args, 1, yarray); + + // construct keyword args + PyObject* kwargs = PyDict_New(); + for (std::map::const_iterator it = + keywords.begin(); it != keywords.end(); ++it) { + PyDict_SetItemString(kwargs, it->first.c_str(), + PyString_FromString(it->second.c_str())); + } + + PyObject* res = PyObject_Call( + detail::_interpreter::get().s_python_function_stem, args, kwargs); + + Py_DECREF(args); + Py_DECREF(kwargs); + if (res) + Py_DECREF(res); + + return res; +} + +template< typename Numeric > +bool fill(const std::vector& x, const std::vector& y, const std::map& keywords) +{ + assert(x.size() == y.size()); + + detail::_interpreter::get(); + + // using numpy arrays + PyObject* xarray = detail::get_array(x); + PyObject* yarray = detail::get_array(y); + + // construct positional args + PyObject* args = PyTuple_New(2); + PyTuple_SetItem(args, 0, xarray); + PyTuple_SetItem(args, 1, yarray); + + // construct keyword args + PyObject* kwargs = PyDict_New(); + for (auto it = keywords.begin(); it != keywords.end(); ++it) { + PyDict_SetItemString(kwargs, it->first.c_str(), PyUnicode_FromString(it->second.c_str())); + } + + PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_fill, args, kwargs); + + Py_DECREF(args); + Py_DECREF(kwargs); + + if (res) Py_DECREF(res); + + return res; +} + +template< typename Numeric > +bool fill_between(const std::vector& x, const std::vector& y1, const std::vector& y2, const std::map& keywords) +{ + assert(x.size() == y1.size()); + assert(x.size() == y2.size()); + + detail::_interpreter::get(); + + // using numpy arrays + PyObject* xarray = detail::get_array(x); + PyObject* y1array = detail::get_array(y1); + PyObject* y2array = detail::get_array(y2); + + // construct positional args + PyObject* args = PyTuple_New(3); + PyTuple_SetItem(args, 0, xarray); + PyTuple_SetItem(args, 1, y1array); + PyTuple_SetItem(args, 2, y2array); + + // construct keyword args + PyObject* kwargs = PyDict_New(); + for(std::map::const_iterator it = keywords.begin(); it != keywords.end(); ++it) { + PyDict_SetItemString(kwargs, it->first.c_str(), PyUnicode_FromString(it->second.c_str())); + } + + PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_fill_between, args, kwargs); + + Py_DECREF(args); + Py_DECREF(kwargs); + if(res) Py_DECREF(res); + + return res; +} + +template +bool arrow(Numeric x, Numeric y, Numeric end_x, Numeric end_y, const std::string& fc = "r", + const std::string ec = "k", Numeric head_length = 0.25, Numeric head_width = 0.1625) { + PyObject* obj_x = PyFloat_FromDouble(x); + PyObject* obj_y = PyFloat_FromDouble(y); + PyObject* obj_end_x = PyFloat_FromDouble(end_x); + PyObject* obj_end_y = PyFloat_FromDouble(end_y); + + PyObject* kwargs = PyDict_New(); + PyDict_SetItemString(kwargs, "fc", PyString_FromString(fc.c_str())); + PyDict_SetItemString(kwargs, "ec", PyString_FromString(ec.c_str())); + PyDict_SetItemString(kwargs, "head_width", PyFloat_FromDouble(head_width)); + PyDict_SetItemString(kwargs, "head_length", PyFloat_FromDouble(head_length)); + + PyObject* plot_args = PyTuple_New(4); + PyTuple_SetItem(plot_args, 0, obj_x); + PyTuple_SetItem(plot_args, 1, obj_y); + PyTuple_SetItem(plot_args, 2, obj_end_x); + PyTuple_SetItem(plot_args, 3, obj_end_y); + + PyObject* res = + PyObject_Call(detail::_interpreter::get().s_python_function_arrow, plot_args, kwargs); + + Py_DECREF(plot_args); + Py_DECREF(kwargs); + if (res) + Py_DECREF(res); + + return res; +} + +template< typename Numeric> +bool hist(const std::vector& y, long bins=10,std::string color="b", + double alpha=1.0, bool cumulative=false) +{ + detail::_interpreter::get(); + + PyObject* yarray = detail::get_array(y); + + PyObject* kwargs = PyDict_New(); + PyDict_SetItemString(kwargs, "bins", PyLong_FromLong(bins)); + PyDict_SetItemString(kwargs, "color", PyString_FromString(color.c_str())); + PyDict_SetItemString(kwargs, "alpha", PyFloat_FromDouble(alpha)); + PyDict_SetItemString(kwargs, "cumulative", cumulative ? Py_True : Py_False); + + PyObject* plot_args = PyTuple_New(1); + + PyTuple_SetItem(plot_args, 0, yarray); + + + PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_hist, plot_args, kwargs); + + + Py_DECREF(plot_args); + Py_DECREF(kwargs); + if(res) Py_DECREF(res); + + return res; +} + +#ifndef WITHOUT_NUMPY +namespace detail { + +inline void imshow(void *ptr, const NPY_TYPES type, const int rows, const int columns, const int colors, const std::map &keywords, PyObject** out) +{ + assert(type == NPY_UINT8 || type == NPY_FLOAT); + assert(colors == 1 || colors == 3 || colors == 4); + + detail::_interpreter::get(); + + // construct args + npy_intp dims[3] = { rows, columns, colors }; + PyObject *args = PyTuple_New(1); + PyTuple_SetItem(args, 0, PyArray_SimpleNewFromData(colors == 1 ? 2 : 3, dims, type, ptr)); + + // construct keyword args + PyObject* kwargs = PyDict_New(); + for(std::map::const_iterator it = keywords.begin(); it != keywords.end(); ++it) + { + PyDict_SetItemString(kwargs, it->first.c_str(), PyUnicode_FromString(it->second.c_str())); + } + + PyObject *res = PyObject_Call(detail::_interpreter::get().s_python_function_imshow, args, kwargs); + Py_DECREF(args); + Py_DECREF(kwargs); + if (!res) + throw std::runtime_error("Call to imshow() failed"); + if (out) + *out = res; + else + Py_DECREF(res); +} + +} // namespace detail + +inline void imshow(const unsigned char *ptr, const int rows, const int columns, const int colors, const std::map &keywords = {}, PyObject** out = nullptr) +{ + detail::imshow((void *) ptr, NPY_UINT8, rows, columns, colors, keywords, out); +} + +inline void imshow(const float *ptr, const int rows, const int columns, const int colors, const std::map &keywords = {}, PyObject** out = nullptr) +{ + detail::imshow((void *) ptr, NPY_FLOAT, rows, columns, colors, keywords, out); +} + +#ifdef WITH_OPENCV +void imshow(const cv::Mat &image, const std::map &keywords = {}) +{ + // Convert underlying type of matrix, if needed + cv::Mat image2; + NPY_TYPES npy_type = NPY_UINT8; + switch (image.type() & CV_MAT_DEPTH_MASK) { + case CV_8U: + image2 = image; + break; + case CV_32F: + image2 = image; + npy_type = NPY_FLOAT; + break; + default: + image.convertTo(image2, CV_MAKETYPE(CV_8U, image.channels())); + } + + // If color image, convert from BGR to RGB + switch (image2.channels()) { + case 3: + cv::cvtColor(image2, image2, CV_BGR2RGB); + break; + case 4: + cv::cvtColor(image2, image2, CV_BGRA2RGBA); + } + + detail::imshow(image2.data, npy_type, image2.rows, image2.cols, image2.channels(), keywords); +} +#endif // WITH_OPENCV +#endif // WITHOUT_NUMPY + +template +bool scatter(const std::vector& x, + const std::vector& y, + const double s=1.0, // The marker size in points**2 + const std::map & keywords = {}) +{ + detail::_interpreter::get(); + + assert(x.size() == y.size()); + + PyObject* xarray = detail::get_array(x); + PyObject* yarray = detail::get_array(y); + + PyObject* kwargs = PyDict_New(); + PyDict_SetItemString(kwargs, "s", PyLong_FromLong(s)); + for (const auto& it : keywords) + { + PyDict_SetItemString(kwargs, it.first.c_str(), PyString_FromString(it.second.c_str())); + } + + PyObject* plot_args = PyTuple_New(2); + PyTuple_SetItem(plot_args, 0, xarray); + PyTuple_SetItem(plot_args, 1, yarray); + + PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_scatter, plot_args, kwargs); + + Py_DECREF(plot_args); + Py_DECREF(kwargs); + if(res) Py_DECREF(res); + + return res; +} + +template +bool boxplot(const std::vector>& data, + const std::vector& labels = {}, + const std::map & keywords = {}) +{ + detail::_interpreter::get(); + + PyObject* listlist = detail::get_listlist(data); + PyObject* args = PyTuple_New(1); + PyTuple_SetItem(args, 0, listlist); + + PyObject* kwargs = PyDict_New(); + + // kwargs needs the labels, if there are (the correct number of) labels + if (!labels.empty() && labels.size() == data.size()) { + PyDict_SetItemString(kwargs, "labels", detail::get_array(labels)); + } + + // take care of the remaining keywords + for (const auto& it : keywords) + { + PyDict_SetItemString(kwargs, it.first.c_str(), PyString_FromString(it.second.c_str())); + } + + PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_boxplot, args, kwargs); + + Py_DECREF(args); + Py_DECREF(kwargs); + + if(res) Py_DECREF(res); + + return res; +} + +template +bool boxplot(const std::vector& data, + const std::map & keywords = {}) +{ + detail::_interpreter::get(); + + PyObject* vector = detail::get_array(data); + PyObject* args = PyTuple_New(1); + PyTuple_SetItem(args, 0, vector); + + PyObject* kwargs = PyDict_New(); + for (const auto& it : keywords) + { + PyDict_SetItemString(kwargs, it.first.c_str(), PyString_FromString(it.second.c_str())); + } + + PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_boxplot, args, kwargs); + + Py_DECREF(args); + Py_DECREF(kwargs); + + if(res) Py_DECREF(res); + + return res; +} + +template +bool bar(const std::vector & x, + const std::vector & y, + std::string ec = "black", + std::string ls = "-", + double lw = 1.0, + const std::map & keywords = {}) +{ + detail::_interpreter::get(); + + PyObject * xarray = detail::get_array(x); + PyObject * yarray = detail::get_array(y); + + PyObject * kwargs = PyDict_New(); + + PyDict_SetItemString(kwargs, "ec", PyString_FromString(ec.c_str())); + PyDict_SetItemString(kwargs, "ls", PyString_FromString(ls.c_str())); + PyDict_SetItemString(kwargs, "lw", PyFloat_FromDouble(lw)); + + for (std::map::const_iterator it = + keywords.begin(); + it != keywords.end(); + ++it) { + PyDict_SetItemString( + kwargs, it->first.c_str(), PyUnicode_FromString(it->second.c_str())); + } + + PyObject * plot_args = PyTuple_New(2); + PyTuple_SetItem(plot_args, 0, xarray); + PyTuple_SetItem(plot_args, 1, yarray); + + PyObject * res = PyObject_Call( + detail::_interpreter::get().s_python_function_bar, plot_args, kwargs); + + Py_DECREF(plot_args); + Py_DECREF(kwargs); + if (res) Py_DECREF(res); + + return res; +} + +template +bool bar(const std::vector & y, + std::string ec = "black", + std::string ls = "-", + double lw = 1.0, + const std::map & keywords = {}) +{ + using T = typename std::remove_reference::type::value_type; + + detail::_interpreter::get(); + + std::vector x; + for (std::size_t i = 0; i < y.size(); i++) { x.push_back(i); } + + return bar(x, y, ec, ls, lw, keywords); +} + +inline bool subplots_adjust(const std::map& keywords = {}) +{ + detail::_interpreter::get(); + + PyObject* kwargs = PyDict_New(); + for (std::map::const_iterator it = + keywords.begin(); it != keywords.end(); ++it) { + PyDict_SetItemString(kwargs, it->first.c_str(), + PyFloat_FromDouble(it->second)); + } + + + PyObject* plot_args = PyTuple_New(0); + + PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_subplots_adjust, plot_args, kwargs); + + Py_DECREF(plot_args); + Py_DECREF(kwargs); + if(res) Py_DECREF(res); + + return res; +} + +template< typename Numeric> +bool named_hist(std::string label,const std::vector& y, long bins=10, std::string color="b", double alpha=1.0) +{ + detail::_interpreter::get(); + + PyObject* yarray = detail::get_array(y); + + PyObject* kwargs = PyDict_New(); + PyDict_SetItemString(kwargs, "label", PyString_FromString(label.c_str())); + PyDict_SetItemString(kwargs, "bins", PyLong_FromLong(bins)); + PyDict_SetItemString(kwargs, "color", PyString_FromString(color.c_str())); + PyDict_SetItemString(kwargs, "alpha", PyFloat_FromDouble(alpha)); + + + PyObject* plot_args = PyTuple_New(1); + PyTuple_SetItem(plot_args, 0, yarray); + + PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_hist, plot_args, kwargs); + + Py_DECREF(plot_args); + Py_DECREF(kwargs); + if(res) Py_DECREF(res); + + return res; +} + +template +bool plot(const std::vector& x, const std::vector& y, const std::string& s = "") +{ + assert(x.size() == y.size()); + + detail::_interpreter::get(); + + PyObject* xarray = detail::get_array(x); + PyObject* yarray = detail::get_array(y); + + PyObject* pystring = PyString_FromString(s.c_str()); + + PyObject* plot_args = PyTuple_New(3); + PyTuple_SetItem(plot_args, 0, xarray); + PyTuple_SetItem(plot_args, 1, yarray); + PyTuple_SetItem(plot_args, 2, pystring); + + PyObject* res = PyObject_CallObject(detail::_interpreter::get().s_python_function_plot, plot_args); + + Py_DECREF(plot_args); + if(res) Py_DECREF(res); + + return res; +} + +template +bool contour(const std::vector& x, const std::vector& y, + const std::vector& z, + const std::map& keywords = {}) { + assert(x.size() == y.size() && x.size() == z.size()); + + PyObject* xarray = get_array(x); + PyObject* yarray = get_array(y); + PyObject* zarray = get_array(z); + + PyObject* plot_args = PyTuple_New(3); + PyTuple_SetItem(plot_args, 0, xarray); + PyTuple_SetItem(plot_args, 1, yarray); + PyTuple_SetItem(plot_args, 2, zarray); + + // construct keyword args + PyObject* kwargs = PyDict_New(); + for (std::map::const_iterator it = keywords.begin(); + it != keywords.end(); ++it) { + PyDict_SetItemString(kwargs, it->first.c_str(), PyUnicode_FromString(it->second.c_str())); + } + + PyObject* res = + PyObject_Call(detail::_interpreter::get().s_python_function_contour, plot_args, kwargs); + + Py_DECREF(kwargs); + Py_DECREF(plot_args); + if (res) + Py_DECREF(res); + + return res; +} + +template +bool quiver(const std::vector& x, const std::vector& y, const std::vector& u, const std::vector& w, const std::map& keywords = {}) +{ + assert(x.size() == y.size() && x.size() == u.size() && u.size() == w.size()); + + detail::_interpreter::get(); + + PyObject* xarray = detail::get_array(x); + PyObject* yarray = detail::get_array(y); + PyObject* uarray = detail::get_array(u); + PyObject* warray = detail::get_array(w); + + PyObject* plot_args = PyTuple_New(4); + PyTuple_SetItem(plot_args, 0, xarray); + PyTuple_SetItem(plot_args, 1, yarray); + PyTuple_SetItem(plot_args, 2, uarray); + PyTuple_SetItem(plot_args, 3, warray); + + // construct keyword args + PyObject* kwargs = PyDict_New(); + for(std::map::const_iterator it = keywords.begin(); it != keywords.end(); ++it) + { + PyDict_SetItemString(kwargs, it->first.c_str(), PyUnicode_FromString(it->second.c_str())); + } + + PyObject* res = PyObject_Call( + detail::_interpreter::get().s_python_function_quiver, plot_args, kwargs); + + Py_DECREF(kwargs); + Py_DECREF(plot_args); + if (res) + Py_DECREF(res); + + return res; +} + +template +bool stem(const std::vector& x, const std::vector& y, const std::string& s = "") +{ + assert(x.size() == y.size()); + + detail::_interpreter::get(); + + PyObject* xarray = detail::get_array(x); + PyObject* yarray = detail::get_array(y); + + PyObject* pystring = PyString_FromString(s.c_str()); + + PyObject* plot_args = PyTuple_New(3); + PyTuple_SetItem(plot_args, 0, xarray); + PyTuple_SetItem(plot_args, 1, yarray); + PyTuple_SetItem(plot_args, 2, pystring); + + PyObject* res = PyObject_CallObject( + detail::_interpreter::get().s_python_function_stem, plot_args); + + Py_DECREF(plot_args); + if (res) + Py_DECREF(res); + + return res; +} + +template +bool semilogx(const std::vector& x, const std::vector& y, const std::string& s = "") +{ + assert(x.size() == y.size()); + + detail::_interpreter::get(); + + PyObject* xarray = detail::get_array(x); + PyObject* yarray = detail::get_array(y); + + PyObject* pystring = PyString_FromString(s.c_str()); + + PyObject* plot_args = PyTuple_New(3); + PyTuple_SetItem(plot_args, 0, xarray); + PyTuple_SetItem(plot_args, 1, yarray); + PyTuple_SetItem(plot_args, 2, pystring); + + PyObject* res = PyObject_CallObject(detail::_interpreter::get().s_python_function_semilogx, plot_args); + + Py_DECREF(plot_args); + if(res) Py_DECREF(res); + + return res; +} + +template +bool semilogy(const std::vector& x, const std::vector& y, const std::string& s = "") +{ + assert(x.size() == y.size()); + + detail::_interpreter::get(); + + PyObject* xarray = detail::get_array(x); + PyObject* yarray = detail::get_array(y); + + PyObject* pystring = PyString_FromString(s.c_str()); + + PyObject* plot_args = PyTuple_New(3); + PyTuple_SetItem(plot_args, 0, xarray); + PyTuple_SetItem(plot_args, 1, yarray); + PyTuple_SetItem(plot_args, 2, pystring); + + PyObject* res = PyObject_CallObject(detail::_interpreter::get().s_python_function_semilogy, plot_args); + + Py_DECREF(plot_args); + if(res) Py_DECREF(res); + + return res; +} + +template +bool loglog(const std::vector& x, const std::vector& y, const std::string& s = "") +{ + assert(x.size() == y.size()); + + detail::_interpreter::get(); + + PyObject* xarray = detail::get_array(x); + PyObject* yarray = detail::get_array(y); + + PyObject* pystring = PyString_FromString(s.c_str()); + + PyObject* plot_args = PyTuple_New(3); + PyTuple_SetItem(plot_args, 0, xarray); + PyTuple_SetItem(plot_args, 1, yarray); + PyTuple_SetItem(plot_args, 2, pystring); + + PyObject* res = PyObject_CallObject(detail::_interpreter::get().s_python_function_loglog, plot_args); + + Py_DECREF(plot_args); + if(res) Py_DECREF(res); + + return res; +} + +template +bool errorbar(const std::vector &x, const std::vector &y, const std::vector &yerr, const std::map &keywords = {}) +{ + assert(x.size() == y.size()); + + detail::_interpreter::get(); + + PyObject* xarray = detail::get_array(x); + PyObject* yarray = detail::get_array(y); + PyObject* yerrarray = detail::get_array(yerr); + + // construct keyword args + PyObject* kwargs = PyDict_New(); + for(std::map::const_iterator it = keywords.begin(); it != keywords.end(); ++it) + { + PyDict_SetItemString(kwargs, it->first.c_str(), PyString_FromString(it->second.c_str())); + } + + PyDict_SetItemString(kwargs, "yerr", yerrarray); + + PyObject *plot_args = PyTuple_New(2); + PyTuple_SetItem(plot_args, 0, xarray); + PyTuple_SetItem(plot_args, 1, yarray); + + PyObject *res = PyObject_Call(detail::_interpreter::get().s_python_function_errorbar, plot_args, kwargs); + + Py_DECREF(kwargs); + Py_DECREF(plot_args); + + if (res) + Py_DECREF(res); + else + throw std::runtime_error("Call to errorbar() failed."); + + return res; +} + +template +bool named_plot(const std::string& name, const std::vector& y, const std::string& format = "") +{ + detail::_interpreter::get(); + + PyObject* kwargs = PyDict_New(); + PyDict_SetItemString(kwargs, "label", PyString_FromString(name.c_str())); + + PyObject* yarray = detail::get_array(y); + + PyObject* pystring = PyString_FromString(format.c_str()); + + PyObject* plot_args = PyTuple_New(2); + + PyTuple_SetItem(plot_args, 0, yarray); + PyTuple_SetItem(plot_args, 1, pystring); + + PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_plot, plot_args, kwargs); + + Py_DECREF(kwargs); + Py_DECREF(plot_args); + if (res) Py_DECREF(res); + + return res; +} + +template +bool named_plot(const std::string& name, const std::vector& x, const std::vector& y, const std::string& format = "") +{ + detail::_interpreter::get(); + + PyObject* kwargs = PyDict_New(); + PyDict_SetItemString(kwargs, "label", PyString_FromString(name.c_str())); + + PyObject* xarray = detail::get_array(x); + PyObject* yarray = detail::get_array(y); + + PyObject* pystring = PyString_FromString(format.c_str()); + + PyObject* plot_args = PyTuple_New(3); + PyTuple_SetItem(plot_args, 0, xarray); + PyTuple_SetItem(plot_args, 1, yarray); + PyTuple_SetItem(plot_args, 2, pystring); + + PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_plot, plot_args, kwargs); + + Py_DECREF(kwargs); + Py_DECREF(plot_args); + if (res) Py_DECREF(res); + + return res; +} + +template +bool named_semilogx(const std::string& name, const std::vector& x, const std::vector& y, const std::string& format = "") +{ + detail::_interpreter::get(); + + PyObject* kwargs = PyDict_New(); + PyDict_SetItemString(kwargs, "label", PyString_FromString(name.c_str())); + + PyObject* xarray = detail::get_array(x); + PyObject* yarray = detail::get_array(y); + + PyObject* pystring = PyString_FromString(format.c_str()); + + PyObject* plot_args = PyTuple_New(3); + PyTuple_SetItem(plot_args, 0, xarray); + PyTuple_SetItem(plot_args, 1, yarray); + PyTuple_SetItem(plot_args, 2, pystring); + + PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_semilogx, plot_args, kwargs); + + Py_DECREF(kwargs); + Py_DECREF(plot_args); + if (res) Py_DECREF(res); + + return res; +} + +template +bool named_semilogy(const std::string& name, const std::vector& x, const std::vector& y, const std::string& format = "") +{ + detail::_interpreter::get(); + + PyObject* kwargs = PyDict_New(); + PyDict_SetItemString(kwargs, "label", PyString_FromString(name.c_str())); + + PyObject* xarray = detail::get_array(x); + PyObject* yarray = detail::get_array(y); + + PyObject* pystring = PyString_FromString(format.c_str()); + + PyObject* plot_args = PyTuple_New(3); + PyTuple_SetItem(plot_args, 0, xarray); + PyTuple_SetItem(plot_args, 1, yarray); + PyTuple_SetItem(plot_args, 2, pystring); + + PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_semilogy, plot_args, kwargs); + + Py_DECREF(kwargs); + Py_DECREF(plot_args); + if (res) Py_DECREF(res); + + return res; +} + +template +bool named_loglog(const std::string& name, const std::vector& x, const std::vector& y, const std::string& format = "") +{ + detail::_interpreter::get(); + + PyObject* kwargs = PyDict_New(); + PyDict_SetItemString(kwargs, "label", PyString_FromString(name.c_str())); + + PyObject* xarray = detail::get_array(x); + PyObject* yarray = detail::get_array(y); + + PyObject* pystring = PyString_FromString(format.c_str()); + + PyObject* plot_args = PyTuple_New(3); + PyTuple_SetItem(plot_args, 0, xarray); + PyTuple_SetItem(plot_args, 1, yarray); + PyTuple_SetItem(plot_args, 2, pystring); + PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_loglog, plot_args, kwargs); + + Py_DECREF(kwargs); + Py_DECREF(plot_args); + if (res) Py_DECREF(res); + + return res; +} + +template +bool plot(const std::vector& y, const std::string& format = "") +{ + std::vector x(y.size()); + for(size_t i=0; i +bool plot(const std::vector& y, const std::map& keywords) +{ + std::vector x(y.size()); + for(size_t i=0; i +bool stem(const std::vector& y, const std::string& format = "") +{ + std::vector x(y.size()); + for (size_t i = 0; i < x.size(); ++i) x.at(i) = i; + return stem(x, y, format); +} + +template +void text(Numeric x, Numeric y, const std::string& s = "") +{ + detail::_interpreter::get(); + + PyObject* args = PyTuple_New(3); + PyTuple_SetItem(args, 0, PyFloat_FromDouble(x)); + PyTuple_SetItem(args, 1, PyFloat_FromDouble(y)); + PyTuple_SetItem(args, 2, PyString_FromString(s.c_str())); + + PyObject* res = PyObject_CallObject(detail::_interpreter::get().s_python_function_text, args); + if(!res) throw std::runtime_error("Call to text() failed."); + + Py_DECREF(args); + Py_DECREF(res); +} + +inline void colorbar(PyObject* mappable = NULL, const std::map& keywords = {}) +{ + if (mappable == NULL) + throw std::runtime_error("Must call colorbar with PyObject* returned from an image, contour, surface, etc."); + + detail::_interpreter::get(); + + PyObject* args = PyTuple_New(1); + PyTuple_SetItem(args, 0, mappable); + + PyObject* kwargs = PyDict_New(); + for(std::map::const_iterator it = keywords.begin(); it != keywords.end(); ++it) + { + PyDict_SetItemString(kwargs, it->first.c_str(), PyFloat_FromDouble(it->second)); + } + + PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_colorbar, args, kwargs); + if(!res) throw std::runtime_error("Call to colorbar() failed."); + + Py_DECREF(args); + Py_DECREF(kwargs); + Py_DECREF(res); +} + + +inline long figure(long number = -1) +{ + detail::_interpreter::get(); + + PyObject *res; + if (number == -1) + res = PyObject_CallObject(detail::_interpreter::get().s_python_function_figure, detail::_interpreter::get().s_python_empty_tuple); + else { + assert(number > 0); + + // Make sure interpreter is initialised + detail::_interpreter::get(); + + PyObject *args = PyTuple_New(1); + PyTuple_SetItem(args, 0, PyLong_FromLong(number)); + res = PyObject_CallObject(detail::_interpreter::get().s_python_function_figure, args); + Py_DECREF(args); + } + + if(!res) throw std::runtime_error("Call to figure() failed."); + + PyObject* num = PyObject_GetAttrString(res, "number"); + if (!num) throw std::runtime_error("Could not get number attribute of figure object"); + const long figureNumber = PyLong_AsLong(num); + + Py_DECREF(num); + Py_DECREF(res); + + return figureNumber; +} + +inline bool fignum_exists(long number) +{ + detail::_interpreter::get(); + + PyObject *args = PyTuple_New(1); + PyTuple_SetItem(args, 0, PyLong_FromLong(number)); + PyObject *res = PyObject_CallObject(detail::_interpreter::get().s_python_function_fignum_exists, args); + if(!res) throw std::runtime_error("Call to fignum_exists() failed."); + + bool ret = PyObject_IsTrue(res); + Py_DECREF(res); + Py_DECREF(args); + + return ret; +} + +inline void figure_size(size_t w, size_t h) +{ + detail::_interpreter::get(); + + const size_t dpi = 100; + PyObject* size = PyTuple_New(2); + PyTuple_SetItem(size, 0, PyFloat_FromDouble((double)w / dpi)); + PyTuple_SetItem(size, 1, PyFloat_FromDouble((double)h / dpi)); + + PyObject* kwargs = PyDict_New(); + PyDict_SetItemString(kwargs, "figsize", size); + PyDict_SetItemString(kwargs, "dpi", PyLong_FromSize_t(dpi)); + + PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_figure, + detail::_interpreter::get().s_python_empty_tuple, kwargs); + + Py_DECREF(kwargs); + + if(!res) throw std::runtime_error("Call to figure_size() failed."); + Py_DECREF(res); +} + +inline void legend() +{ + detail::_interpreter::get(); + + PyObject* res = PyObject_CallObject(detail::_interpreter::get().s_python_function_legend, detail::_interpreter::get().s_python_empty_tuple); + if(!res) throw std::runtime_error("Call to legend() failed."); + + Py_DECREF(res); +} + +inline void legend(const std::map& keywords) +{ + detail::_interpreter::get(); + + // construct keyword args + PyObject* kwargs = PyDict_New(); + for(std::map::const_iterator it = keywords.begin(); it != keywords.end(); ++it) + { + PyDict_SetItemString(kwargs, it->first.c_str(), PyString_FromString(it->second.c_str())); + } + + PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_legend, detail::_interpreter::get().s_python_empty_tuple, kwargs); + if(!res) throw std::runtime_error("Call to legend() failed."); + + Py_DECREF(kwargs); + Py_DECREF(res); +} + +template +void ylim(Numeric left, Numeric right) +{ + detail::_interpreter::get(); + + PyObject* list = PyList_New(2); + PyList_SetItem(list, 0, PyFloat_FromDouble(left)); + PyList_SetItem(list, 1, PyFloat_FromDouble(right)); + + PyObject* args = PyTuple_New(1); + PyTuple_SetItem(args, 0, list); + + PyObject* res = PyObject_CallObject(detail::_interpreter::get().s_python_function_ylim, args); + if(!res) throw std::runtime_error("Call to ylim() failed."); + + Py_DECREF(args); + Py_DECREF(res); +} + +template +void xlim(Numeric left, Numeric right) +{ + detail::_interpreter::get(); + + PyObject* list = PyList_New(2); + PyList_SetItem(list, 0, PyFloat_FromDouble(left)); + PyList_SetItem(list, 1, PyFloat_FromDouble(right)); + + PyObject* args = PyTuple_New(1); + PyTuple_SetItem(args, 0, list); + + PyObject* res = PyObject_CallObject(detail::_interpreter::get().s_python_function_xlim, args); + if(!res) throw std::runtime_error("Call to xlim() failed."); + + Py_DECREF(args); + Py_DECREF(res); +} + + +inline double* xlim() +{ + detail::_interpreter::get(); + + PyObject* args = PyTuple_New(0); + PyObject* res = PyObject_CallObject(detail::_interpreter::get().s_python_function_xlim, args); + PyObject* left = PyTuple_GetItem(res,0); + PyObject* right = PyTuple_GetItem(res,1); + + double* arr = new double[2]; + arr[0] = PyFloat_AsDouble(left); + arr[1] = PyFloat_AsDouble(right); + + if(!res) throw std::runtime_error("Call to xlim() failed."); + + Py_DECREF(res); + return arr; +} + + +inline double* ylim() +{ + detail::_interpreter::get(); + + PyObject* args = PyTuple_New(0); + PyObject* res = PyObject_CallObject(detail::_interpreter::get().s_python_function_ylim, args); + PyObject* left = PyTuple_GetItem(res,0); + PyObject* right = PyTuple_GetItem(res,1); + + double* arr = new double[2]; + arr[0] = PyFloat_AsDouble(left); + arr[1] = PyFloat_AsDouble(right); + + if(!res) throw std::runtime_error("Call to ylim() failed."); + + Py_DECREF(res); + return arr; +} + +template +inline void xticks(const std::vector &ticks, const std::vector &labels = {}, const std::map& keywords = {}) +{ + assert(labels.size() == 0 || ticks.size() == labels.size()); + + detail::_interpreter::get(); + + // using numpy array + PyObject* ticksarray = detail::get_array(ticks); + + PyObject* args; + if(labels.size() == 0) { + // construct positional args + args = PyTuple_New(1); + PyTuple_SetItem(args, 0, ticksarray); + } else { + // make tuple of tick labels + PyObject* labelstuple = PyTuple_New(labels.size()); + for (size_t i = 0; i < labels.size(); i++) + PyTuple_SetItem(labelstuple, i, PyUnicode_FromString(labels[i].c_str())); + + // construct positional args + args = PyTuple_New(2); + PyTuple_SetItem(args, 0, ticksarray); + PyTuple_SetItem(args, 1, labelstuple); + } + + // construct keyword args + PyObject* kwargs = PyDict_New(); + for(std::map::const_iterator it = keywords.begin(); it != keywords.end(); ++it) + { + PyDict_SetItemString(kwargs, it->first.c_str(), PyString_FromString(it->second.c_str())); + } + + PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_xticks, args, kwargs); + + Py_DECREF(args); + Py_DECREF(kwargs); + if(!res) throw std::runtime_error("Call to xticks() failed"); + + Py_DECREF(res); +} + +template +inline void xticks(const std::vector &ticks, const std::map& keywords) +{ + xticks(ticks, {}, keywords); +} + +template +inline void yticks(const std::vector &ticks, const std::vector &labels = {}, const std::map& keywords = {}) +{ + assert(labels.size() == 0 || ticks.size() == labels.size()); + + detail::_interpreter::get(); + + // using numpy array + PyObject* ticksarray = detail::get_array(ticks); + + PyObject* args; + if(labels.size() == 0) { + // construct positional args + args = PyTuple_New(1); + PyTuple_SetItem(args, 0, ticksarray); + } else { + // make tuple of tick labels + PyObject* labelstuple = PyTuple_New(labels.size()); + for (size_t i = 0; i < labels.size(); i++) + PyTuple_SetItem(labelstuple, i, PyUnicode_FromString(labels[i].c_str())); + + // construct positional args + args = PyTuple_New(2); + PyTuple_SetItem(args, 0, ticksarray); + PyTuple_SetItem(args, 1, labelstuple); + } + + // construct keyword args + PyObject* kwargs = PyDict_New(); + for(std::map::const_iterator it = keywords.begin(); it != keywords.end(); ++it) + { + PyDict_SetItemString(kwargs, it->first.c_str(), PyString_FromString(it->second.c_str())); + } + + PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_yticks, args, kwargs); + + Py_DECREF(args); + Py_DECREF(kwargs); + if(!res) throw std::runtime_error("Call to yticks() failed"); + + Py_DECREF(res); +} + +template +inline void yticks(const std::vector &ticks, const std::map& keywords) +{ + yticks(ticks, {}, keywords); +} + +template inline void margins(Numeric margin) +{ + // construct positional args + PyObject* args = PyTuple_New(1); + PyTuple_SetItem(args, 0, PyFloat_FromDouble(margin)); + + PyObject* res = + PyObject_CallObject(detail::_interpreter::get().s_python_function_margins, args); + if (!res) + throw std::runtime_error("Call to margins() failed."); + + Py_DECREF(args); + Py_DECREF(res); +} + +template inline void margins(Numeric margin_x, Numeric margin_y) +{ + // construct positional args + PyObject* args = PyTuple_New(2); + PyTuple_SetItem(args, 0, PyFloat_FromDouble(margin_x)); + PyTuple_SetItem(args, 1, PyFloat_FromDouble(margin_y)); + + PyObject* res = + PyObject_CallObject(detail::_interpreter::get().s_python_function_margins, args); + if (!res) + throw std::runtime_error("Call to margins() failed."); + + Py_DECREF(args); + Py_DECREF(res); +} + + +inline void tick_params(const std::map& keywords, const std::string axis = "both") +{ + detail::_interpreter::get(); + + // construct positional args + PyObject* args; + args = PyTuple_New(1); + PyTuple_SetItem(args, 0, PyString_FromString(axis.c_str())); + + // construct keyword args + PyObject* kwargs = PyDict_New(); + for (std::map::const_iterator it = keywords.begin(); it != keywords.end(); ++it) + { + PyDict_SetItemString(kwargs, it->first.c_str(), PyString_FromString(it->second.c_str())); + } + + + PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_tick_params, args, kwargs); + + Py_DECREF(args); + Py_DECREF(kwargs); + if (!res) throw std::runtime_error("Call to tick_params() failed"); + + Py_DECREF(res); +} + +inline void subplot(long nrows, long ncols, long plot_number) +{ + detail::_interpreter::get(); + + // construct positional args + PyObject* args = PyTuple_New(3); + PyTuple_SetItem(args, 0, PyFloat_FromDouble(nrows)); + PyTuple_SetItem(args, 1, PyFloat_FromDouble(ncols)); + PyTuple_SetItem(args, 2, PyFloat_FromDouble(plot_number)); + + PyObject* res = PyObject_CallObject(detail::_interpreter::get().s_python_function_subplot, args); + if(!res) throw std::runtime_error("Call to subplot() failed."); + + Py_DECREF(args); + Py_DECREF(res); +} + +inline void subplot2grid(long nrows, long ncols, long rowid=0, long colid=0, long rowspan=1, long colspan=1) +{ + detail::_interpreter::get(); + + PyObject* shape = PyTuple_New(2); + PyTuple_SetItem(shape, 0, PyLong_FromLong(nrows)); + PyTuple_SetItem(shape, 1, PyLong_FromLong(ncols)); + + PyObject* loc = PyTuple_New(2); + PyTuple_SetItem(loc, 0, PyLong_FromLong(rowid)); + PyTuple_SetItem(loc, 1, PyLong_FromLong(colid)); + + PyObject* args = PyTuple_New(4); + PyTuple_SetItem(args, 0, shape); + PyTuple_SetItem(args, 1, loc); + PyTuple_SetItem(args, 2, PyLong_FromLong(rowspan)); + PyTuple_SetItem(args, 3, PyLong_FromLong(colspan)); + + PyObject* res = PyObject_CallObject(detail::_interpreter::get().s_python_function_subplot2grid, args); + if(!res) throw std::runtime_error("Call to subplot2grid() failed."); + + Py_DECREF(shape); + Py_DECREF(loc); + Py_DECREF(args); + Py_DECREF(res); +} + +inline void title(const std::string &titlestr, const std::map &keywords = {}) +{ + detail::_interpreter::get(); + + PyObject* pytitlestr = PyString_FromString(titlestr.c_str()); + PyObject* args = PyTuple_New(1); + PyTuple_SetItem(args, 0, pytitlestr); + + PyObject* kwargs = PyDict_New(); + for (auto it = keywords.begin(); it != keywords.end(); ++it) { + PyDict_SetItemString(kwargs, it->first.c_str(), PyUnicode_FromString(it->second.c_str())); + } + + PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_title, args, kwargs); + if(!res) throw std::runtime_error("Call to title() failed."); + + Py_DECREF(args); + Py_DECREF(kwargs); + Py_DECREF(res); +} + +inline void suptitle(const std::string &suptitlestr, const std::map &keywords = {}) +{ + detail::_interpreter::get(); + + PyObject* pysuptitlestr = PyString_FromString(suptitlestr.c_str()); + PyObject* args = PyTuple_New(1); + PyTuple_SetItem(args, 0, pysuptitlestr); + + PyObject* kwargs = PyDict_New(); + for (auto it = keywords.begin(); it != keywords.end(); ++it) { + PyDict_SetItemString(kwargs, it->first.c_str(), PyUnicode_FromString(it->second.c_str())); + } + + PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_suptitle, args, kwargs); + if(!res) throw std::runtime_error("Call to suptitle() failed."); + + Py_DECREF(args); + Py_DECREF(kwargs); + Py_DECREF(res); +} + +inline void axis(const std::string &axisstr) +{ + detail::_interpreter::get(); + + PyObject* str = PyString_FromString(axisstr.c_str()); + PyObject* args = PyTuple_New(1); + PyTuple_SetItem(args, 0, str); + + PyObject* res = PyObject_CallObject(detail::_interpreter::get().s_python_function_axis, args); + if(!res) throw std::runtime_error("Call to title() failed."); + + Py_DECREF(args); + Py_DECREF(res); +} + +inline void axvline(double x, double ymin = 0., double ymax = 1., const std::map& keywords = std::map()) +{ + detail::_interpreter::get(); + + // construct positional args + PyObject* args = PyTuple_New(3); + PyTuple_SetItem(args, 0, PyFloat_FromDouble(x)); + PyTuple_SetItem(args, 1, PyFloat_FromDouble(ymin)); + PyTuple_SetItem(args, 2, PyFloat_FromDouble(ymax)); + + // construct keyword args + PyObject* kwargs = PyDict_New(); + for(std::map::const_iterator it = keywords.begin(); it != keywords.end(); ++it) + { + PyDict_SetItemString(kwargs, it->first.c_str(), PyString_FromString(it->second.c_str())); + } + + PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_axvline, args, kwargs); + + Py_DECREF(args); + Py_DECREF(kwargs); + + if(res) Py_DECREF(res); +} + +inline void axvspan(double xmin, double xmax, double ymin = 0., double ymax = 1., const std::map& keywords = std::map()) +{ + // construct positional args + PyObject* args = PyTuple_New(4); + PyTuple_SetItem(args, 0, PyFloat_FromDouble(xmin)); + PyTuple_SetItem(args, 1, PyFloat_FromDouble(xmax)); + PyTuple_SetItem(args, 2, PyFloat_FromDouble(ymin)); + PyTuple_SetItem(args, 3, PyFloat_FromDouble(ymax)); + + // construct keyword args + PyObject* kwargs = PyDict_New(); + for(std::map::const_iterator it = keywords.begin(); it != keywords.end(); ++it) + { + if (it->first == "linewidth" || it->first == "alpha") + PyDict_SetItemString(kwargs, it->first.c_str(), PyFloat_FromDouble(std::stod(it->second))); + else + PyDict_SetItemString(kwargs, it->first.c_str(), PyString_FromString(it->second.c_str())); + } + + PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_axvspan, args, kwargs); + Py_DECREF(args); + Py_DECREF(kwargs); + + if(res) Py_DECREF(res); +} + +inline void xlabel(const std::string &str, const std::map &keywords = {}) +{ + detail::_interpreter::get(); + + PyObject* pystr = PyString_FromString(str.c_str()); + PyObject* args = PyTuple_New(1); + PyTuple_SetItem(args, 0, pystr); + + PyObject* kwargs = PyDict_New(); + for (auto it = keywords.begin(); it != keywords.end(); ++it) { + PyDict_SetItemString(kwargs, it->first.c_str(), PyUnicode_FromString(it->second.c_str())); + } + + PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_xlabel, args, kwargs); + if(!res) throw std::runtime_error("Call to xlabel() failed."); + + Py_DECREF(args); + Py_DECREF(kwargs); + Py_DECREF(res); +} + +inline void ylabel(const std::string &str, const std::map& keywords = {}) +{ + detail::_interpreter::get(); + + PyObject* pystr = PyString_FromString(str.c_str()); + PyObject* args = PyTuple_New(1); + PyTuple_SetItem(args, 0, pystr); + + PyObject* kwargs = PyDict_New(); + for (auto it = keywords.begin(); it != keywords.end(); ++it) { + PyDict_SetItemString(kwargs, it->first.c_str(), PyUnicode_FromString(it->second.c_str())); + } + + PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_ylabel, args, kwargs); + if(!res) throw std::runtime_error("Call to ylabel() failed."); + + Py_DECREF(args); + Py_DECREF(kwargs); + Py_DECREF(res); +} + +inline void set_zlabel(const std::string &str, const std::map& keywords = {}) +{ + detail::_interpreter::get(); + + // Same as with plot_surface: We lazily load the modules here the first time + // this function is called because I'm not sure that we can assume "matplotlib + // installed" implies "mpl_toolkits installed" on all platforms, and we don't + // want to require it for people who don't need 3d plots. + static PyObject *mpl_toolkitsmod = nullptr, *axis3dmod = nullptr; + if (!mpl_toolkitsmod) { + PyObject* mpl_toolkits = PyString_FromString("mpl_toolkits"); + PyObject* axis3d = PyString_FromString("mpl_toolkits.mplot3d"); + if (!mpl_toolkits || !axis3d) { throw std::runtime_error("couldnt create string"); } + + mpl_toolkitsmod = PyImport_Import(mpl_toolkits); + Py_DECREF(mpl_toolkits); + if (!mpl_toolkitsmod) { throw std::runtime_error("Error loading module mpl_toolkits!"); } + + axis3dmod = PyImport_Import(axis3d); + Py_DECREF(axis3d); + if (!axis3dmod) { throw std::runtime_error("Error loading module mpl_toolkits.mplot3d!"); } + } + + PyObject* pystr = PyString_FromString(str.c_str()); + PyObject* args = PyTuple_New(1); + PyTuple_SetItem(args, 0, pystr); + + PyObject* kwargs = PyDict_New(); + for (auto it = keywords.begin(); it != keywords.end(); ++it) { + PyDict_SetItemString(kwargs, it->first.c_str(), PyUnicode_FromString(it->second.c_str())); + } + + PyObject *ax = + PyObject_CallObject(detail::_interpreter::get().s_python_function_gca, + detail::_interpreter::get().s_python_empty_tuple); + if (!ax) throw std::runtime_error("Call to gca() failed."); + Py_INCREF(ax); + + PyObject *zlabel = PyObject_GetAttrString(ax, "set_zlabel"); + if (!zlabel) throw std::runtime_error("Attribute set_zlabel not found."); + Py_INCREF(zlabel); + + PyObject *res = PyObject_Call(zlabel, args, kwargs); + if (!res) throw std::runtime_error("Call to set_zlabel() failed."); + Py_DECREF(zlabel); + + Py_DECREF(ax); + Py_DECREF(args); + Py_DECREF(kwargs); + if (res) Py_DECREF(res); +} + +inline void grid(bool flag) +{ + detail::_interpreter::get(); + + PyObject* pyflag = flag ? Py_True : Py_False; + Py_INCREF(pyflag); + + PyObject* args = PyTuple_New(1); + PyTuple_SetItem(args, 0, pyflag); + + PyObject* res = PyObject_CallObject(detail::_interpreter::get().s_python_function_grid, args); + if(!res) throw std::runtime_error("Call to grid() failed."); + + Py_DECREF(args); + Py_DECREF(res); +} + +inline void show(const bool block = true) +{ + detail::_interpreter::get(); + + PyObject* res; + if(block) + { + res = PyObject_CallObject( + detail::_interpreter::get().s_python_function_show, + detail::_interpreter::get().s_python_empty_tuple); + } + else + { + PyObject *kwargs = PyDict_New(); + PyDict_SetItemString(kwargs, "block", Py_False); + res = PyObject_Call( detail::_interpreter::get().s_python_function_show, detail::_interpreter::get().s_python_empty_tuple, kwargs); + Py_DECREF(kwargs); + } + + + if (!res) throw std::runtime_error("Call to show() failed."); + + Py_DECREF(res); +} + +inline void close() +{ + detail::_interpreter::get(); + + PyObject* res = PyObject_CallObject( + detail::_interpreter::get().s_python_function_close, + detail::_interpreter::get().s_python_empty_tuple); + + if (!res) throw std::runtime_error("Call to close() failed."); + + Py_DECREF(res); +} + +inline void xkcd() { + detail::_interpreter::get(); + + PyObject* res; + PyObject *kwargs = PyDict_New(); + + res = PyObject_Call(detail::_interpreter::get().s_python_function_xkcd, + detail::_interpreter::get().s_python_empty_tuple, kwargs); + + Py_DECREF(kwargs); + + if (!res) + throw std::runtime_error("Call to show() failed."); + + Py_DECREF(res); +} + +inline void draw() +{ + detail::_interpreter::get(); + + PyObject* res = PyObject_CallObject( + detail::_interpreter::get().s_python_function_draw, + detail::_interpreter::get().s_python_empty_tuple); + + if (!res) throw std::runtime_error("Call to draw() failed."); + + Py_DECREF(res); +} + +template +inline void pause(Numeric interval) +{ + detail::_interpreter::get(); + + PyObject* args = PyTuple_New(1); + PyTuple_SetItem(args, 0, PyFloat_FromDouble(interval)); + + PyObject* res = PyObject_CallObject(detail::_interpreter::get().s_python_function_pause, args); + if(!res) throw std::runtime_error("Call to pause() failed."); + + Py_DECREF(args); + Py_DECREF(res); +} + +inline void save(const std::string& filename) +{ + detail::_interpreter::get(); + + PyObject* pyfilename = PyString_FromString(filename.c_str()); + + PyObject* args = PyTuple_New(1); + PyTuple_SetItem(args, 0, pyfilename); + std::cout<<"args:"<> ginput(const int numClicks = 1, const std::map& keywords = {}) +{ + detail::_interpreter::get(); + + PyObject *args = PyTuple_New(1); + PyTuple_SetItem(args, 0, PyLong_FromLong(numClicks)); + + // construct keyword args + PyObject* kwargs = PyDict_New(); + for(std::map::const_iterator it = keywords.begin(); it != keywords.end(); ++it) + { + PyDict_SetItemString(kwargs, it->first.c_str(), PyUnicode_FromString(it->second.c_str())); + } + + PyObject* res = PyObject_Call( + detail::_interpreter::get().s_python_function_ginput, args, kwargs); + + Py_DECREF(kwargs); + Py_DECREF(args); + if (!res) throw std::runtime_error("Call to ginput() failed."); + + const size_t len = PyList_Size(res); + std::vector> out; + out.reserve(len); + for (size_t i = 0; i < len; i++) { + PyObject *current = PyList_GetItem(res, i); + std::array position; + position[0] = PyFloat_AsDouble(PyTuple_GetItem(current, 0)); + position[1] = PyFloat_AsDouble(PyTuple_GetItem(current, 1)); + out.push_back(position); + } + Py_DECREF(res); + + return out; +} + +// Actually, is there any reason not to call this automatically for every plot? +inline void tight_layout() { + detail::_interpreter::get(); + + PyObject *res = PyObject_CallObject( + detail::_interpreter::get().s_python_function_tight_layout, + detail::_interpreter::get().s_python_empty_tuple); + + if (!res) throw std::runtime_error("Call to tight_layout() failed."); + + Py_DECREF(res); +} + +// Support for variadic plot() and initializer lists: + +namespace detail { + +template +using is_function = typename std::is_function>>::type; + +template +struct is_callable_impl; + +template +struct is_callable_impl +{ + typedef is_function type; +}; // a non-object is callable iff it is a function + +template +struct is_callable_impl +{ + struct Fallback { void operator()(); }; + struct Derived : T, Fallback { }; + + template struct Check; + + template + static std::true_type test( ... ); // use a variadic function to make sure (1) it accepts everything and (2) its always the worst match + + template + static std::false_type test( Check* ); + +public: + typedef decltype(test(nullptr)) type; + typedef decltype(&Fallback::operator()) dtype; + static constexpr bool value = type::value; +}; // an object is callable iff it defines operator() + +template +struct is_callable +{ + // dispatch to is_callable_impl or is_callable_impl depending on whether T is of class type or not + typedef typename is_callable_impl::value, T>::type type; +}; + +template +struct plot_impl { }; + +template<> +struct plot_impl +{ + template + bool operator()(const IterableX& x, const IterableY& y, const std::string& format) + { + // 2-phase lookup for distance, begin, end + using std::distance; + using std::begin; + using std::end; + + auto xs = distance(begin(x), end(x)); + auto ys = distance(begin(y), end(y)); + assert(xs == ys && "x and y data must have the same number of elements!"); + + PyObject* xlist = PyList_New(xs); + PyObject* ylist = PyList_New(ys); + PyObject* pystring = PyString_FromString(format.c_str()); + + auto itx = begin(x), ity = begin(y); + for(size_t i = 0; i < xs; ++i) { + PyList_SetItem(xlist, i, PyFloat_FromDouble(*itx++)); + PyList_SetItem(ylist, i, PyFloat_FromDouble(*ity++)); + } + + PyObject* plot_args = PyTuple_New(3); + PyTuple_SetItem(plot_args, 0, xlist); + PyTuple_SetItem(plot_args, 1, ylist); + PyTuple_SetItem(plot_args, 2, pystring); + + PyObject* res = PyObject_CallObject(detail::_interpreter::get().s_python_function_plot, plot_args); + + Py_DECREF(plot_args); + if(res) Py_DECREF(res); + + return res; + } +}; + +template<> +struct plot_impl +{ + template + bool operator()(const Iterable& ticks, const Callable& f, const std::string& format) + { + if(begin(ticks) == end(ticks)) return true; + + // We could use additional meta-programming to deduce the correct element type of y, + // but all values have to be convertible to double anyways + std::vector y; + for(auto x : ticks) y.push_back(f(x)); + return plot_impl()(ticks,y,format); + } +}; + +} // end namespace detail + +// recursion stop for the above +template +bool plot() { return true; } + +template +bool plot(const A& a, const B& b, const std::string& format, Args... args) +{ + return detail::plot_impl::type>()(a,b,format) && plot(args...); +} + +/* + * This group of plot() functions is needed to support initializer lists, i.e. calling + * plot( {1,2,3,4} ) + */ +inline bool plot(const std::vector& x, const std::vector& y, const std::string& format = "") { + return plot(x,y,format); +} + +inline bool plot(const std::vector& y, const std::string& format = "") { + return plot(y,format); +} + +inline bool plot(const std::vector& x, const std::vector& y, const std::map& keywords) { + return plot(x,y,keywords); +} + +/* + * This class allows dynamic plots, ie changing the plotted data without clearing and re-plotting + */ +class Plot +{ +public: + // default initialization with plot label, some data and format + template + Plot(const std::string& name, const std::vector& x, const std::vector& y, const std::string& format = "") { + detail::_interpreter::get(); + + assert(x.size() == y.size()); + + PyObject* kwargs = PyDict_New(); + if(name != "") + PyDict_SetItemString(kwargs, "label", PyString_FromString(name.c_str())); + + PyObject* xarray = detail::get_array(x); + PyObject* yarray = detail::get_array(y); + + PyObject* pystring = PyString_FromString(format.c_str()); + + PyObject* plot_args = PyTuple_New(3); + PyTuple_SetItem(plot_args, 0, xarray); + PyTuple_SetItem(plot_args, 1, yarray); + PyTuple_SetItem(plot_args, 2, pystring); + + PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_plot, plot_args, kwargs); + + Py_DECREF(kwargs); + Py_DECREF(plot_args); + + if(res) + { + line= PyList_GetItem(res, 0); + + if(line) + set_data_fct = PyObject_GetAttrString(line,"set_data"); + else + Py_DECREF(line); + Py_DECREF(res); + } + } + + // shorter initialization with name or format only + // basically calls line, = plot([], []) + Plot(const std::string& name = "", const std::string& format = "") + : Plot(name, std::vector(), std::vector(), format) {} + + template + bool update(const std::vector& x, const std::vector& y) { + assert(x.size() == y.size()); + if(set_data_fct) + { + PyObject* xarray = detail::get_array(x); + PyObject* yarray = detail::get_array(y); + + PyObject* plot_args = PyTuple_New(2); + PyTuple_SetItem(plot_args, 0, xarray); + PyTuple_SetItem(plot_args, 1, yarray); + + PyObject* res = PyObject_CallObject(set_data_fct, plot_args); + if (res) Py_DECREF(res); + return res; + } + return false; + } + + // clears the plot but keep it available + bool clear() { + return update(std::vector(), std::vector()); + } + + // definitely remove this line + void remove() { + if(line) + { + auto remove_fct = PyObject_GetAttrString(line,"remove"); + PyObject* args = PyTuple_New(0); + PyObject* res = PyObject_CallObject(remove_fct, args); + if (res) Py_DECREF(res); + } + decref(); + } + + ~Plot() { + decref(); + } +private: + + void decref() { + if(line) + Py_DECREF(line); + if(set_data_fct) + Py_DECREF(set_data_fct); + } + + + PyObject* line = nullptr; + PyObject* set_data_fct = nullptr; +}; + +} // end namespace matplotlibcpp diff --git a/src/FAST_LIO/include/so3_math.h b/src/FAST_LIO/include/so3_math.h new file mode 100644 index 0000000..8530c3c --- /dev/null +++ b/src/FAST_LIO/include/so3_math.h @@ -0,0 +1,111 @@ +#ifndef SO3_MATH_H +#define SO3_MATH_H + +#include +#include + +#define SKEW_SYM_MATRX(v) 0.0,-v[2],v[1],v[2],0.0,-v[0],-v[1],v[0],0.0 + +template +Eigen::Matrix skew_sym_mat(const Eigen::Matrix &v) +{ + Eigen::Matrix skew_sym_mat; + skew_sym_mat<<0.0,-v[2],v[1],v[2],0.0,-v[0],-v[1],v[0],0.0; + return skew_sym_mat; +} + +template +Eigen::Matrix Exp(const Eigen::Matrix &&ang) +{ + T ang_norm = ang.norm(); + Eigen::Matrix Eye3 = Eigen::Matrix::Identity(); + if (ang_norm > 0.0000001) + { + Eigen::Matrix r_axis = ang / ang_norm; + Eigen::Matrix K; + K << SKEW_SYM_MATRX(r_axis); + /// Roderigous Tranformation + return Eye3 + std::sin(ang_norm) * K + (1.0 - std::cos(ang_norm)) * K * K; + } + else + { + return Eye3; + } +} + +template +Eigen::Matrix Exp(const Eigen::Matrix &ang_vel, const Ts &dt) +{ + T ang_vel_norm = ang_vel.norm(); + Eigen::Matrix Eye3 = Eigen::Matrix::Identity(); + + if (ang_vel_norm > 0.0000001) + { + Eigen::Matrix r_axis = ang_vel / ang_vel_norm; + Eigen::Matrix K; + + K << SKEW_SYM_MATRX(r_axis); + + T r_ang = ang_vel_norm * dt; + + /// Roderigous Tranformation + return Eye3 + std::sin(r_ang) * K + (1.0 - std::cos(r_ang)) * K * K; + } + else + { + return Eye3; + } +} + +template +Eigen::Matrix Exp(const T &v1, const T &v2, const T &v3) +{ + T &&norm = sqrt(v1 * v1 + v2 * v2 + v3 * v3); + Eigen::Matrix Eye3 = Eigen::Matrix::Identity(); + if (norm > 0.00001) + { + T r_ang[3] = {v1 / norm, v2 / norm, v3 / norm}; + Eigen::Matrix K; + K << SKEW_SYM_MATRX(r_ang); + + /// Roderigous Tranformation + return Eye3 + std::sin(norm) * K + (1.0 - std::cos(norm)) * K * K; + } + else + { + return Eye3; + } +} + +/* Logrithm of a Rotation Matrix */ +template +Eigen::Matrix Log(const Eigen::Matrix &R) +{ + T theta = (R.trace() > 3.0 - 1e-6) ? 0.0 : std::acos(0.5 * (R.trace() - 1)); + Eigen::Matrix K(R(2,1) - R(1,2), R(0,2) - R(2,0), R(1,0) - R(0,1)); + return (std::abs(theta) < 0.001) ? (0.5 * K) : (0.5 * theta / std::sin(theta) * K); +} + +template +Eigen::Matrix RotMtoEuler(const Eigen::Matrix &rot) +{ + T sy = sqrt(rot(0,0)*rot(0,0) + rot(1,0)*rot(1,0)); + bool singular = sy < 1e-6; + T x, y, z; + if(!singular) + { + x = atan2(rot(2, 1), rot(2, 2)); + y = atan2(-rot(2, 0), sy); + z = atan2(rot(1, 0), rot(0, 0)); + } + else + { + x = atan2(-rot(1, 2), rot(1, 1)); + y = atan2(-rot(2, 0), sy); + z = 0; + } + Eigen::Matrix ang(x, y, z); + return ang; +} + +#endif diff --git a/src/FAST_LIO/include/use-ikfom.hpp b/src/FAST_LIO/include/use-ikfom.hpp new file mode 100644 index 0000000..fdd8f87 --- /dev/null +++ b/src/FAST_LIO/include/use-ikfom.hpp @@ -0,0 +1,126 @@ +#ifndef USE_IKFOM_H +#define USE_IKFOM_H + +#include + +typedef MTK::vect<3, double> vect3; +typedef MTK::SO3 SO3; +typedef MTK::S2 S2; +typedef MTK::vect<1, double> vect1; +typedef MTK::vect<2, double> vect2; + +MTK_BUILD_MANIFOLD(state_ikfom, +((vect3, pos)) +((SO3, rot)) +((SO3, offset_R_L_I)) +((vect3, offset_T_L_I)) +((vect3, vel)) +((vect3, bg)) +((vect3, ba)) +((S2, grav)) +); + +MTK_BUILD_MANIFOLD(input_ikfom, +((vect3, acc)) +((vect3, gyro)) +); + +MTK_BUILD_MANIFOLD(process_noise_ikfom, +((vect3, ng)) +((vect3, na)) +((vect3, nbg)) +((vect3, nba)) +); + +MTK::get_cov::type process_noise_cov() +{ + MTK::get_cov::type cov = MTK::get_cov::type::Zero(); + MTK::setDiagonal(cov, &process_noise_ikfom::ng, 0.0001);// 0.03 + MTK::setDiagonal(cov, &process_noise_ikfom::na, 0.0001); // *dt 0.01 0.01 * dt * dt 0.05 + MTK::setDiagonal(cov, &process_noise_ikfom::nbg, 0.00001); // *dt 0.00001 0.00001 * dt *dt 0.3 //0.001 0.0001 0.01 + MTK::setDiagonal(cov, &process_noise_ikfom::nba, 0.00001); //0.001 0.05 0.0001/out 0.01 + return cov; +} + +//double L_offset_to_I[3] = {0.04165, 0.02326, -0.0284}; // Avia +//vect3 Lidar_offset_to_IMU(L_offset_to_I, 3); +Eigen::Matrix get_f(state_ikfom &s, const input_ikfom &in) +{ + Eigen::Matrix res = Eigen::Matrix::Zero(); + vect3 omega; + in.gyro.boxminus(omega, s.bg); + vect3 a_inertial = s.rot * (in.acc-s.ba); + for(int i = 0; i < 3; i++ ){ + res(i) = s.vel[i]; + res(i + 3) = omega[i]; + res(i + 12) = a_inertial[i] + s.grav[i]; + } + return res; +} + +Eigen::Matrix df_dx(state_ikfom &s, const input_ikfom &in) +{ + Eigen::Matrix cov = Eigen::Matrix::Zero(); + cov.template block<3, 3>(0, 12) = Eigen::Matrix3d::Identity(); + vect3 acc_; + in.acc.boxminus(acc_, s.ba); + vect3 omega; + in.gyro.boxminus(omega, s.bg); + cov.template block<3, 3>(12, 3) = -s.rot.toRotationMatrix()*MTK::hat(acc_); + cov.template block<3, 3>(12, 18) = -s.rot.toRotationMatrix(); + Eigen::Matrix vec = Eigen::Matrix::Zero(); + Eigen::Matrix grav_matrix; + s.S2_Mx(grav_matrix, vec, 21); + cov.template block<3, 2>(12, 21) = grav_matrix; + cov.template block<3, 3>(3, 15) = -Eigen::Matrix3d::Identity(); + return cov; +} + + +Eigen::Matrix df_dw(state_ikfom &s, const input_ikfom &in) +{ + Eigen::Matrix cov = Eigen::Matrix::Zero(); + cov.template block<3, 3>(12, 3) = -s.rot.toRotationMatrix(); + cov.template block<3, 3>(3, 0) = -Eigen::Matrix3d::Identity(); + cov.template block<3, 3>(15, 6) = Eigen::Matrix3d::Identity(); + cov.template block<3, 3>(18, 9) = Eigen::Matrix3d::Identity(); + return cov; +} + +vect3 SO3ToEuler(const SO3 &orient) +{ + Eigen::Matrix _ang; + Eigen::Vector4d q_data = orient.coeffs().transpose(); + //scalar w=orient.coeffs[3], x=orient.coeffs[0], y=orient.coeffs[1], z=orient.coeffs[2]; + double sqw = q_data[3]*q_data[3]; + double sqx = q_data[0]*q_data[0]; + double sqy = q_data[1]*q_data[1]; + double sqz = q_data[2]*q_data[2]; + double unit = sqx + sqy + sqz + sqw; // if normalized is one, otherwise is correction factor + double test = q_data[3]*q_data[1] - q_data[2]*q_data[0]; + + if (test > 0.49999*unit) { // singularity at north pole + + _ang << 2 * std::atan2(q_data[0], q_data[3]), M_PI/2, 0; + double temp[3] = {_ang[0] * 57.3, _ang[1] * 57.3, _ang[2] * 57.3}; + vect3 euler_ang(temp, 3); + return euler_ang; + } + if (test < -0.49999*unit) { // singularity at south pole + _ang << -2 * std::atan2(q_data[0], q_data[3]), -M_PI/2, 0; + double temp[3] = {_ang[0] * 57.3, _ang[1] * 57.3, _ang[2] * 57.3}; + vect3 euler_ang(temp, 3); + return euler_ang; + } + + _ang << + std::atan2(2*q_data[0]*q_data[3]+2*q_data[1]*q_data[2] , -sqx - sqy + sqz + sqw), + std::asin (2*test/unit), + std::atan2(2*q_data[2]*q_data[3]+2*q_data[1]*q_data[0] , sqx - sqy - sqz + sqw); + double temp[3] = {_ang[0] * 57.3, _ang[1] * 57.3, _ang[2] * 57.3}; + vect3 euler_ang(temp, 3); + // euler_ang[0] = roll, euler_ang[1] = pitch, euler_ang[2] = yaw + return euler_ang; +} + +#endif \ No newline at end of file diff --git a/src/FAST_LIO/launch/Pointcloud2Map.launch b/src/FAST_LIO/launch/Pointcloud2Map.launch new file mode 100644 index 0000000..6ad1b5c --- /dev/null +++ b/src/FAST_LIO/launch/Pointcloud2Map.launch @@ -0,0 +1,13 @@ + + + + + + + + + + + + + \ No newline at end of file diff --git a/src/FAST_LIO/launch/gdb_debug_example.launch b/src/FAST_LIO/launch/gdb_debug_example.launch new file mode 100644 index 0000000..d1f86ad --- /dev/null +++ b/src/FAST_LIO/launch/gdb_debug_example.launch @@ -0,0 +1,22 @@ + + + + + + + + + + + + + + + + + + + + diff --git a/src/FAST_LIO/launch/mapping_avia.launch b/src/FAST_LIO/launch/mapping_avia.launch new file mode 100644 index 0000000..9c5ea00 --- /dev/null +++ b/src/FAST_LIO/launch/mapping_avia.launch @@ -0,0 +1,21 @@ + + + + + + + + + + + + + + + + + + + + + diff --git a/src/FAST_LIO/launch/mapping_horizon.launch b/src/FAST_LIO/launch/mapping_horizon.launch new file mode 100644 index 0000000..17b6d63 --- /dev/null +++ b/src/FAST_LIO/launch/mapping_horizon.launch @@ -0,0 +1,21 @@ + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/src/FAST_LIO/launch/mapping_mid360.launch b/src/FAST_LIO/launch/mapping_mid360.launch new file mode 100644 index 0000000..b4d27e4 --- /dev/null +++ b/src/FAST_LIO/launch/mapping_mid360.launch @@ -0,0 +1,26 @@ + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/FAST_LIO/launch/mapping_ouster64.launch b/src/FAST_LIO/launch/mapping_ouster64.launch new file mode 100644 index 0000000..1f5a2a5 --- /dev/null +++ b/src/FAST_LIO/launch/mapping_ouster64.launch @@ -0,0 +1,21 @@ + + + + + + + + + + + + + + + + + + + + + diff --git a/src/FAST_LIO/launch/mapping_velodyne.launch b/src/FAST_LIO/launch/mapping_velodyne.launch new file mode 100644 index 0000000..25d449c --- /dev/null +++ b/src/FAST_LIO/launch/mapping_velodyne.launch @@ -0,0 +1,21 @@ + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/src/FAST_LIO/msg/Pose6D.msg b/src/FAST_LIO/msg/Pose6D.msg new file mode 100644 index 0000000..1ebd1fd --- /dev/null +++ b/src/FAST_LIO/msg/Pose6D.msg @@ -0,0 +1,7 @@ +# the preintegrated Lidar states at the time of IMU measurements in a frame +float64 offset_time # the offset time of IMU measurement w.r.t the first lidar point +float64[3] acc # the preintegrated total acceleration (global frame) at the Lidar origin +float64[3] gyr # the unbiased angular velocity (body frame) at the Lidar origin +float64[3] vel # the preintegrated velocity (global frame) at the Lidar origin +float64[3] pos # the preintegrated position (global frame) at the Lidar origin +float64[9] rot # the preintegrated rotation (global frame) at the Lidar origin \ No newline at end of file diff --git a/src/FAST_LIO/package.xml b/src/FAST_LIO/package.xml new file mode 100644 index 0000000..1e8c4d1 --- /dev/null +++ b/src/FAST_LIO/package.xml @@ -0,0 +1,47 @@ + + + fast_lio + 0.0.0 + + + This is a modified version of LOAM which is original algorithm + is described in the following paper: + J. Zhang and S. Singh. LOAM: Lidar Odometry and Mapping in Real-time. + Robotics: Science and Systems Conference (RSS). Berkeley, CA, July 2014. + + + claydergc + + BSD + + Ji Zhang + + catkin + geometry_msgs + nav_msgs + roscpp + rospy + std_msgs + sensor_msgs + tf + pcl_ros + livox_ros_driver2 + message_generation + + geometry_msgs + nav_msgs + sensor_msgs + roscpp + rospy + std_msgs + tf + pcl_ros + livox_ros_driver2 + message_runtime + + rostest + rosbag + + + + diff --git a/src/FAST_LIO/rviz_cfg/.gitignore b/src/FAST_LIO/rviz_cfg/.gitignore new file mode 100644 index 0000000..e69de29 diff --git a/src/FAST_LIO/rviz_cfg/loam_livox.rviz b/src/FAST_LIO/rviz_cfg/loam_livox.rviz new file mode 100644 index 0000000..195a39a --- /dev/null +++ b/src/FAST_LIO/rviz_cfg/loam_livox.rviz @@ -0,0 +1,363 @@ +Panels: + - Class: rviz/Displays + Help Height: 0 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /mapping1 + - /mapping1/surround1 + - /mapping1/currPoints1 + - /mapping1/currPoints1/Autocompute Value Bounds1 + - /Odometry1/Odometry1 + - /Odometry1/Odometry1/Shape1 + - /Odometry1/Odometry1/Covariance1 + - /Odometry1/Odometry1/Covariance1/Position1 + - /Odometry1/Odometry1/Covariance1/Orientation1 + - /MarkerArray1/Namespaces1 + Splitter Ratio: 0.6432291865348816 + Tree Height: 451 + - 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 + Name: Time + SyncMode: 0 + SyncSource: "" +Preferences: + PromptSaveOnExit: true +Toolbars: + toolButtonStyle: 2 +Visualization Manager: + Class: "" + Displays: + - Alpha: 1 + Cell Size: 1000 + 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: 40 + Reference Frame: + Value: false + - Alpha: 1 + Class: rviz/Axes + Enabled: false + Length: 0.699999988079071 + Name: Axes + Radius: 0.05999999865889549 + Reference Frame: + Show Trail: false + Value: false + - 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: 238; 238; 236 + Color Transformer: Intensity + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Min Color: 238; 238; 236 + Name: surround + Position Transformer: XYZ + Queue Size: 1 + Selectable: false + Size (Pixels): 1 + Size (m): 0.05000000074505806 + Style: Points + Topic: /cloud_registered + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: true + - Alpha: 0.10000000149011612 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 15 + Min Value: -5 + Value: false + Axis: Z + Channel Name: intensity + Class: rviz/PointCloud2 + Color: 255; 255; 255 + Color Transformer: Intensity + Decay Time: 1000 + Enabled: true + Invert Rainbow: true + Max Color: 255; 255; 255 + Min Color: 0; 0; 0 + Name: currPoints + Position Transformer: XYZ + Queue Size: 100000 + Selectable: true + Size (Pixels): 1 + Size (m): 0.009999999776482582 + Style: Points + Topic: /cloud_registered + 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; 0 + Color Transformer: FlatColor + Decay Time: 0 + Enabled: false + Invert Rainbow: false + Max Color: 255; 255; 255 + Min Color: 0; 0; 0 + Name: PointCloud2 + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.10000000149011612 + Style: Flat Squares + Topic: /Laser_map + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: false + Enabled: true + Name: mapping + - Class: rviz/Group + Displays: + - Angle Tolerance: 0.009999999776482582 + Class: rviz/Odometry + Covariance: + Orientation: + Alpha: 0.5 + Color: 255; 255; 127 + Color Style: Unique + Frame: Local + Offset: 1 + Scale: 1 + Value: true + Position: + Alpha: 0.30000001192092896 + Color: 204; 51; 204 + Scale: 1 + Value: true + Value: true + Enabled: true + Keep: 1 + Name: Odometry + Position Tolerance: 0.0010000000474974513 + Queue Size: 10 + Shape: + Alpha: 1 + Axes Length: 1 + Axes Radius: 0.20000000298023224 + Color: 255; 85; 0 + Head Length: 0 + Head Radius: 0 + Shaft Length: 0.05000000074505806 + Shaft Radius: 0.05000000074505806 + Value: Axes + Topic: /Odometry + Unreliable: false + Value: true + Enabled: true + Name: Odometry + - Alpha: 1 + Class: rviz/Axes + Enabled: true + Length: 0.699999988079071 + Name: Axes + Radius: 0.10000000149011612 + Reference Frame: + Show Trail: false + Value: true + - Alpha: 0 + Buffer Length: 2 + Class: rviz/Path + Color: 25; 255; 255 + Enabled: true + Head Diameter: 0 + Head Length: 0 + Length: 0.30000001192092896 + Line Style: Billboards + Line Width: 0.20000000298023224 + Name: Path + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 25; 255; 255 + Pose Style: None + Queue Size: 10 + Radius: 0.029999999329447746 + Shaft Diameter: 0.4000000059604645 + Shaft Length: 0.4000000059604645 + Topic: /path + Unreliable: false + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: false + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/PointCloud2 + Color: 255; 255; 255 + Color Transformer: Intensity + Decay Time: 0 + Enabled: false + Invert Rainbow: false + Max Color: 239; 41; 41 + Max Intensity: 0 + Min Color: 239; 41; 41 + Min Intensity: 0 + Name: PointCloud2 + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 4 + Size (m): 0.30000001192092896 + Style: Spheres + Topic: /cloud_effected + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: false + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 13.139549255371094 + Min Value: -32.08251953125 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/PointCloud2 + Color: 138; 226; 52 + Color Transformer: FlatColor + Decay Time: 0 + Enabled: false + Invert Rainbow: false + Max Color: 138; 226; 52 + Min Color: 138; 226; 52 + Name: PointCloud2 + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.10000000149011612 + Style: Flat Squares + Topic: /Laser_map + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: false + - Class: rviz/MarkerArray + Enabled: false + Marker Topic: /MarkerArray + Name: MarkerArray + Namespaces: + {} + Queue Size: 100 + Value: false + Enabled: true + Global Options: + Background Color: 0; 0; 0 + Default Light: true + Fixed Frame: camera_init + Frame Rate: 10 + 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/Orbit + Distance: 38.82821273803711 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Field of View: 0.7853981852531433 + Focal Point: + X: -4.982542037963867 + Y: -15.83572006225586 + Z: -3.063523054122925 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 0.35479673743247986 + Target Frame: global + Yaw: 1.2271827459335327 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 684 + Hide Left Dock: false + Hide Right Dock: true + QMainWindow State: 000000ff00000000fd0000000400000000000001c8000001fefc020000000dfb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b000001fe000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d0061006700650000000297000001dc0000000000000000fb0000000a0049006d0061006700650000000394000001600000000000000000fb0000000a0049006d00610067006501000002c5000000c70000000000000000fb0000000a0049006d00610067006501000002c5000000c70000000000000000fb0000000a0049006d00610067006501000002c5000000c700000000000000000000000100000152000004b7fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d000004b7000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004ac00000052fc0100000002fb0000000800540069006d00650100000000000004ac0000030700fffffffb0000000800540069006d00650100000000000004500000000000000000000002de000001fe00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: true + Width: 1196 + X: 35 + Y: 23 diff --git a/src/FAST_LIO/src/IMU_Processing.hpp b/src/FAST_LIO/src/IMU_Processing.hpp new file mode 100644 index 0000000..0774911 --- /dev/null +++ b/src/FAST_LIO/src/IMU_Processing.hpp @@ -0,0 +1,377 @@ +#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 "use-ikfom.hpp" + +/// *************Preconfiguration + +#define MAX_INI_COUNT (10) + +const bool time_list(PointType &x, PointType &y) {return (x.curvature < y.curvature);}; + +/// *************IMU Process and undistortion +class ImuProcess +{ + public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + ImuProcess(); + ~ImuProcess(); + + void Reset(); + void Reset(double start_timestamp, const sensor_msgs::ImuConstPtr &lastimu); + void set_extrinsic(const V3D &transl, const M3D &rot); + void set_extrinsic(const V3D &transl); + void set_extrinsic(const MD(4,4) &T); + void set_gyr_cov(const V3D &scaler); + void set_acc_cov(const V3D &scaler); + void set_gyr_bias_cov(const V3D &b_g); + void set_acc_bias_cov(const V3D &b_a); + Eigen::Matrix Q; + void Process(const MeasureGroup &meas, esekfom::esekf &kf_state, PointCloudXYZI::Ptr pcl_un_); + + ofstream fout_imu; + V3D cov_acc; + V3D cov_gyr; + V3D cov_acc_scale; + V3D cov_gyr_scale; + V3D cov_bias_gyr; + V3D cov_bias_acc; + double first_lidar_time; + + private: + void IMU_init(const MeasureGroup &meas, esekfom::esekf &kf_state, int &N); + void UndistortPcl(const MeasureGroup &meas, esekfom::esekf &kf_state, PointCloudXYZI &pcl_in_out); + + PointCloudXYZI::Ptr cur_pcl_un_; + sensor_msgs::ImuConstPtr last_imu_; + deque v_imu_; + vector IMUpose; + vector v_rot_pcl_; + M3D Lidar_R_wrt_IMU; + V3D Lidar_T_wrt_IMU; + V3D mean_acc; + V3D mean_gyr; + V3D angvel_last; + V3D acc_s_last; + double start_timestamp_; + double last_lidar_end_time_; + int init_iter_num = 1; + bool b_first_frame_ = true; + bool imu_need_init_ = true; +}; + +ImuProcess::ImuProcess() + : b_first_frame_(true), imu_need_init_(true), start_timestamp_(-1) +{ + init_iter_num = 1; + Q = process_noise_cov(); + cov_acc = V3D(0.1, 0.1, 0.1); + cov_gyr = V3D(0.1, 0.1, 0.1); + cov_bias_gyr = V3D(0.0001, 0.0001, 0.0001); + cov_bias_acc = V3D(0.0001, 0.0001, 0.0001); + mean_acc = V3D(0, 0, -1.0); + mean_gyr = V3D(0, 0, 0); + angvel_last = Zero3d; + Lidar_T_wrt_IMU = Zero3d; + Lidar_R_wrt_IMU = Eye3d; + last_imu_.reset(new sensor_msgs::Imu()); +} + +ImuProcess::~ImuProcess() {} + +void ImuProcess::Reset() +{ + // ROS_WARN("Reset ImuProcess"); + mean_acc = V3D(0, 0, -1.0); + mean_gyr = V3D(0, 0, 0); + angvel_last = Zero3d; + imu_need_init_ = true; + start_timestamp_ = -1; + init_iter_num = 1; + v_imu_.clear(); + IMUpose.clear(); + last_imu_.reset(new sensor_msgs::Imu()); + cur_pcl_un_.reset(new PointCloudXYZI()); +} + +void ImuProcess::set_extrinsic(const MD(4,4) &T) +{ + Lidar_T_wrt_IMU = T.block<3,1>(0,3); + Lidar_R_wrt_IMU = T.block<3,3>(0,0); +} + +void ImuProcess::set_extrinsic(const V3D &transl) +{ + Lidar_T_wrt_IMU = transl; + Lidar_R_wrt_IMU.setIdentity(); +} + +void ImuProcess::set_extrinsic(const V3D &transl, const M3D &rot) +{ + Lidar_T_wrt_IMU = transl; + Lidar_R_wrt_IMU = rot; +} + +void ImuProcess::set_gyr_cov(const V3D &scaler) +{ + cov_gyr_scale = scaler; +} + +void ImuProcess::set_acc_cov(const V3D &scaler) +{ + cov_acc_scale = scaler; +} + +void ImuProcess::set_gyr_bias_cov(const V3D &b_g) +{ + cov_bias_gyr = b_g; +} + +void ImuProcess::set_acc_bias_cov(const V3D &b_a) +{ + cov_bias_acc = b_a; +} + +void ImuProcess::IMU_init(const MeasureGroup &meas, esekfom::esekf &kf_state, int &N) +{ + /** 1. initializing the gravity, gyro bias, acc and gyro covariance + ** 2. normalize the acceleration measurenments to unit gravity **/ + + V3D cur_acc, cur_gyr; + + if (b_first_frame_) + { + Reset(); + N = 1; + b_first_frame_ = false; + const auto &imu_acc = meas.imu.front()->linear_acceleration; + const auto &gyr_acc = meas.imu.front()->angular_velocity; + mean_acc << imu_acc.x, imu_acc.y, imu_acc.z; + mean_gyr << gyr_acc.x, gyr_acc.y, gyr_acc.z; + first_lidar_time = meas.lidar_beg_time; + } + + for (const auto &imu : meas.imu) + { + const auto &imu_acc = imu->linear_acceleration; + const auto &gyr_acc = imu->angular_velocity; + cur_acc << imu_acc.x, imu_acc.y, imu_acc.z; + cur_gyr << gyr_acc.x, gyr_acc.y, gyr_acc.z; + + mean_acc += (cur_acc - mean_acc) / N; + mean_gyr += (cur_gyr - mean_gyr) / N; + + cov_acc = cov_acc * (N - 1.0) / N + (cur_acc - mean_acc).cwiseProduct(cur_acc - mean_acc) * (N - 1.0) / (N * N); + cov_gyr = cov_gyr * (N - 1.0) / N + (cur_gyr - mean_gyr).cwiseProduct(cur_gyr - mean_gyr) * (N - 1.0) / (N * N); + + // cout<<"acc norm: "<::cov init_P = kf_state.get_P(); + init_P.setIdentity(); + init_P(6,6) = init_P(7,7) = init_P(8,8) = 0.00001; + init_P(9,9) = init_P(10,10) = init_P(11,11) = 0.00001; + init_P(15,15) = init_P(16,16) = init_P(17,17) = 0.0001; + init_P(18,18) = init_P(19,19) = init_P(20,20) = 0.001; + init_P(21,21) = init_P(22,22) = 0.00001; + kf_state.change_P(init_P); + last_imu_ = meas.imu.back(); + +} + +void ImuProcess::UndistortPcl(const MeasureGroup &meas, esekfom::esekf &kf_state, PointCloudXYZI &pcl_out) +{ + /*** add the imu of the last frame-tail to the of current frame-head ***/ + auto v_imu = meas.imu; + v_imu.push_front(last_imu_); + const double &imu_beg_time = v_imu.front()->header.stamp.toSec(); + const double &imu_end_time = v_imu.back()->header.stamp.toSec(); + const double &pcl_beg_time = meas.lidar_beg_time; + const double &pcl_end_time = meas.lidar_end_time; + + /*** sort point clouds by offset time ***/ + pcl_out = *(meas.lidar); + sort(pcl_out.points.begin(), pcl_out.points.end(), time_list); + // cout<<"[ IMU Process ]: Process lidar from "<header.stamp.toSec() < last_lidar_end_time_) continue; + + angvel_avr<<0.5 * (head->angular_velocity.x + tail->angular_velocity.x), + 0.5 * (head->angular_velocity.y + tail->angular_velocity.y), + 0.5 * (head->angular_velocity.z + tail->angular_velocity.z); + acc_avr <<0.5 * (head->linear_acceleration.x + tail->linear_acceleration.x), + 0.5 * (head->linear_acceleration.y + tail->linear_acceleration.y), + 0.5 * (head->linear_acceleration.z + tail->linear_acceleration.z); + + // fout_imu << setw(10) << head->header.stamp.toSec() - first_lidar_time << " " << angvel_avr.transpose() << " " << acc_avr.transpose() << endl; + + acc_avr = acc_avr * G_m_s2 / mean_acc.norm(); // - state_inout.ba; + + if(head->header.stamp.toSec() < last_lidar_end_time_) + { + dt = tail->header.stamp.toSec() - last_lidar_end_time_; + // dt = tail->header.stamp.toSec() - pcl_beg_time; + } + else + { + dt = tail->header.stamp.toSec() - head->header.stamp.toSec(); + } + + in.acc = acc_avr; + in.gyro = angvel_avr; + Q.block<3, 3>(0, 0).diagonal() = cov_gyr; + Q.block<3, 3>(3, 3).diagonal() = cov_acc; + Q.block<3, 3>(6, 6).diagonal() = cov_bias_gyr; + Q.block<3, 3>(9, 9).diagonal() = cov_bias_acc; + kf_state.predict(dt, Q, in); + + /* save the poses at each IMU measurements */ + imu_state = kf_state.get_x(); + angvel_last = angvel_avr - imu_state.bg; + acc_s_last = imu_state.rot * (acc_avr - imu_state.ba); + for(int i=0; i<3; i++) + { + acc_s_last[i] += imu_state.grav[i]; + } + double &&offs_t = tail->header.stamp.toSec() - pcl_beg_time; + IMUpose.push_back(set_pose6d(offs_t, acc_s_last, angvel_last, imu_state.vel, imu_state.pos, imu_state.rot.toRotationMatrix())); + } + + /*** calculated the pos and attitude prediction at the frame-end ***/ + double note = pcl_end_time > imu_end_time ? 1.0 : -1.0; + dt = note * (pcl_end_time - imu_end_time); + kf_state.predict(dt, Q, in); + + imu_state = kf_state.get_x(); + last_imu_ = meas.imu.back(); + last_lidar_end_time_ = pcl_end_time; + + /*** undistort each lidar point (backward propagation) ***/ + if (pcl_out.points.begin() == pcl_out.points.end()) return; + auto it_pcl = pcl_out.points.end() - 1; + for (auto it_kp = IMUpose.end() - 1; it_kp != IMUpose.begin(); it_kp--) + { + auto head = it_kp - 1; + auto tail = it_kp; + R_imu<rot); + // cout<<"head imu acc: "<vel); + pos_imu<pos); + acc_imu<acc); + angvel_avr<gyr); + + for(; it_pcl->curvature / double(1000) > head->offset_time; it_pcl --) + { + dt = it_pcl->curvature / double(1000) - head->offset_time; + + /* Transform to the 'end' frame, using only the rotation + * Note: Compensation direction is INVERSE of Frame's moving direction + * So if we want to compensate a point at timestamp-i to the frame-e + * P_compensate = R_imu_e ^ T * (R_i * P_i + T_ei) where T_ei is represented in global frame */ + M3D R_i(R_imu * Exp(angvel_avr, dt)); + + V3D P_i(it_pcl->x, it_pcl->y, it_pcl->z); + V3D T_ei(pos_imu + vel_imu * dt + 0.5 * acc_imu * dt * dt - imu_state.pos); + V3D P_compensate = imu_state.offset_R_L_I.conjugate() * (imu_state.rot.conjugate() * (R_i * (imu_state.offset_R_L_I * P_i + imu_state.offset_T_L_I) + T_ei) - imu_state.offset_T_L_I);// not accurate! + + // save Undistorted points and their rotation + it_pcl->x = P_compensate(0); + it_pcl->y = P_compensate(1); + it_pcl->z = P_compensate(2); + + if (it_pcl == pcl_out.points.begin()) break; + } + } +} + +void ImuProcess::Process(const MeasureGroup &meas, esekfom::esekf &kf_state, PointCloudXYZI::Ptr cur_pcl_un_) +{ + double t1,t2,t3; + t1 = omp_get_wtime(); + + if(meas.imu.empty()) {return;}; + ROS_ASSERT(meas.lidar != nullptr); + + if (imu_need_init_) + { + /// The very first lidar frame + IMU_init(meas, kf_state, init_iter_num); + + imu_need_init_ = true; + + last_imu_ = meas.imu.back(); + + state_ikfom imu_state = kf_state.get_x(); + if (init_iter_num > MAX_INI_COUNT) + { + cov_acc *= pow(G_m_s2 / mean_acc.norm(), 2); + imu_need_init_ = false; + + cov_acc = cov_acc_scale; + cov_gyr = cov_gyr_scale; + ROS_INFO("IMU Initial Done"); + // ROS_INFO("IMU Initial Done: Gravity: %.4f %.4f %.4f %.4f; state.bias_g: %.4f %.4f %.4f; acc covarience: %.8f %.8f %.8f; gry covarience: %.8f %.8f %.8f",\ + // imu_state.grav[0], imu_state.grav[1], imu_state.grav[2], mean_acc.norm(), cov_bias_gyr[0], cov_bias_gyr[1], cov_bias_gyr[2], cov_acc[0], cov_acc[1], cov_acc[2], cov_gyr[0], cov_gyr[1], cov_gyr[2]); + fout_imu.open(DEBUG_FILE_DIR("imu.txt"),ios::out); + } + + return; + } + + UndistortPcl(meas, kf_state, *cur_pcl_un_); + + t2 = omp_get_wtime(); + t3 = omp_get_wtime(); + + // cout<<"[ IMU Process ]: Time: "< +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include "IMU_Processing.hpp" +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include "preprocess.h" +#include + +#define INIT_TIME (0.1) +#define LASER_POINT_COV (0.001) +#define MAXN (720000) +#define PUBFRAME_PERIOD (20) + +/*** Time Log Variables ***/ +double kdtree_incremental_time = 0.0, kdtree_search_time = 0.0, kdtree_delete_time = 0.0; +double T1[MAXN], s_plot[MAXN], s_plot2[MAXN], s_plot3[MAXN], s_plot4[MAXN], s_plot5[MAXN], s_plot6[MAXN], s_plot7[MAXN], s_plot8[MAXN], s_plot9[MAXN], s_plot10[MAXN], s_plot11[MAXN]; +double match_time = 0, solve_time = 0, solve_const_H_time = 0; +int kdtree_size_st = 0, kdtree_size_end = 0, add_point_size = 0, kdtree_delete_counter = 0; +bool runtime_pos_log = false, pcd_save_en = false, time_sync_en = false, extrinsic_est_en = true, path_en = true; +/**************************/ + +float res_last[100000] = {0.0}; +float DET_RANGE = 300.0f; +const float MOV_THRESHOLD = 1.5f; +double time_diff_lidar_to_imu = 0.0; + +mutex mtx_buffer; +condition_variable sig_buffer; + +string root_dir = ROOT_DIR; +string map_file_path, lid_topic, imu_topic; + +double res_mean_last = 0.05, total_residual = 0.0; +double last_timestamp_lidar = 0, last_timestamp_imu = -1.0; +double gyr_cov = 0.1, acc_cov = 0.1, b_gyr_cov = 0.0001, b_acc_cov = 0.0001; +double filter_size_corner_min = 0, filter_size_surf_min = 0, filter_size_map_min = 0, fov_deg = 0; +double cube_len = 0, HALF_FOV_COS = 0, FOV_DEG = 0, total_distance = 0, lidar_end_time = 0, first_lidar_time = 0.0; +int effct_feat_num = 0, time_log_counter = 0, scan_count = 0, publish_count = 0; +int iterCount = 0, feats_down_size = 0, NUM_MAX_ITERATIONS = 0, laserCloudValidNum = 0, pcd_save_interval = -1, pcd_index = 0; +bool point_selected_surf[100000] = {0}; +bool lidar_pushed, flg_first_scan = true, flg_exit = false, flg_EKF_inited; +bool scan_pub_en = false, dense_pub_en = false, scan_body_pub_en = false; + +vector> pointSearchInd_surf; +vector cub_needrm; +vector Nearest_Points; +vector extrinT(3, 0.0); +vector extrinR(9, 0.0); +deque time_buffer; +deque lidar_buffer; +deque imu_buffer; + +PointCloudXYZI::Ptr featsFromMap(new PointCloudXYZI()); +PointCloudXYZI::Ptr feats_undistort(new PointCloudXYZI()); +PointCloudXYZI::Ptr feats_down_body(new PointCloudXYZI()); +PointCloudXYZI::Ptr feats_down_world(new PointCloudXYZI()); +PointCloudXYZI::Ptr normvec(new PointCloudXYZI(100000, 1)); +PointCloudXYZI::Ptr laserCloudOri(new PointCloudXYZI(100000, 1)); +PointCloudXYZI::Ptr corr_normvect(new PointCloudXYZI(100000, 1)); +PointCloudXYZI::Ptr _featsArray; + +pcl::VoxelGrid downSizeFilterSurf; +pcl::VoxelGrid downSizeFilterMap; + +KD_TREE ikdtree; + +V3F XAxisPoint_body(LIDAR_SP_LEN, 0.0, 0.0); +V3F XAxisPoint_world(LIDAR_SP_LEN, 0.0, 0.0); +V3D euler_cur; +V3D position_last(Zero3d); +V3D Lidar_T_wrt_IMU(Zero3d); +M3D Lidar_R_wrt_IMU(Eye3d); + +/*** EKF inputs and output ***/ +MeasureGroup Measures; +esekfom::esekf kf; +state_ikfom state_point; +vect3 pos_lid; + +nav_msgs::Path path; +nav_msgs::Odometry odomAftMapped; +geometry_msgs::Quaternion geoQuat; +geometry_msgs::PoseStamped msg_body_pose; + +shared_ptr p_pre(new Preprocess()); +shared_ptr p_imu(new ImuProcess()); + +void SigHandle(int sig) +{ + flg_exit = true; + ROS_WARN("catch sig %d", sig); + sig_buffer.notify_all(); +} + +inline void dump_lio_state_to_log(FILE *fp) +{ + V3D rot_ang(Log(state_point.rot.toRotationMatrix())); + fprintf(fp, "%lf ", Measures.lidar_beg_time - first_lidar_time); + fprintf(fp, "%lf %lf %lf ", rot_ang(0), rot_ang(1), rot_ang(2)); // Angle + fprintf(fp, "%lf %lf %lf ", state_point.pos(0), state_point.pos(1), state_point.pos(2)); // Pos + fprintf(fp, "%lf %lf %lf ", 0.0, 0.0, 0.0); // omega + fprintf(fp, "%lf %lf %lf ", state_point.vel(0), state_point.vel(1), state_point.vel(2)); // Vel + fprintf(fp, "%lf %lf %lf ", 0.0, 0.0, 0.0); // Acc + fprintf(fp, "%lf %lf %lf ", state_point.bg(0), state_point.bg(1), state_point.bg(2)); // Bias_g + fprintf(fp, "%lf %lf %lf ", state_point.ba(0), state_point.ba(1), state_point.ba(2)); // Bias_a + fprintf(fp, "%lf %lf %lf ", state_point.grav[0], state_point.grav[1], state_point.grav[2]); // Bias_a + fprintf(fp, "\r\n"); + fflush(fp); +} + +void pointBodyToWorld_ikfom(PointType const * const pi, PointType * const po, state_ikfom &s) +{ + V3D p_body(pi->x, pi->y, pi->z); + V3D p_global(s.rot * (s.offset_R_L_I*p_body + s.offset_T_L_I) + s.pos); + + po->x = p_global(0); + po->y = p_global(1); + po->z = p_global(2); + po->intensity = pi->intensity; +} + + +void pointBodyToWorld(PointType const * const pi, PointType * const po) +{ + V3D p_body(pi->x, pi->y, pi->z); + V3D p_global(state_point.rot * (state_point.offset_R_L_I*p_body + state_point.offset_T_L_I) + state_point.pos); + + po->x = p_global(0); + po->y = p_global(1); + po->z = p_global(2); + po->intensity = pi->intensity; +} + +template +void pointBodyToWorld(const Matrix &pi, Matrix &po) +{ + V3D p_body(pi[0], pi[1], pi[2]); + V3D p_global(state_point.rot * (state_point.offset_R_L_I*p_body + state_point.offset_T_L_I) + state_point.pos); + + po[0] = p_global(0); + po[1] = p_global(1); + po[2] = p_global(2); +} + +void RGBpointBodyToWorld(PointType const * const pi, PointType * const po) +{ + V3D p_body(pi->x, pi->y, pi->z); + V3D p_global(state_point.rot * (state_point.offset_R_L_I*p_body + state_point.offset_T_L_I) + state_point.pos); + + po->x = p_global(0); + po->y = p_global(1); + po->z = p_global(2); + po->intensity = pi->intensity; +} + +void RGBpointBodyLidarToIMU(PointType const * const pi, PointType * const po) +{ + V3D p_body_lidar(pi->x, pi->y, pi->z); + V3D p_body_imu(state_point.offset_R_L_I*p_body_lidar + state_point.offset_T_L_I); + + po->x = p_body_imu(0); + po->y = p_body_imu(1); + po->z = p_body_imu(2); + po->intensity = pi->intensity; +} + +void points_cache_collect() +{ + PointVector points_history; + ikdtree.acquire_removed_points(points_history); + // for (int i = 0; i < points_history.size(); i++) _featsArray->push_back(points_history[i]); +} + +BoxPointType LocalMap_Points; +bool Localmap_Initialized = false; +void lasermap_fov_segment() +{ + cub_needrm.clear(); + kdtree_delete_counter = 0; + kdtree_delete_time = 0.0; + pointBodyToWorld(XAxisPoint_body, XAxisPoint_world); + V3D pos_LiD = pos_lid; + if (!Localmap_Initialized){ + for (int i = 0; i < 3; i++){ + LocalMap_Points.vertex_min[i] = pos_LiD(i) - cube_len / 2.0; + LocalMap_Points.vertex_max[i] = pos_LiD(i) + cube_len / 2.0; + } + Localmap_Initialized = true; + return; + } + float dist_to_map_edge[3][2]; + bool need_move = false; + for (int i = 0; i < 3; i++){ + dist_to_map_edge[i][0] = fabs(pos_LiD(i) - LocalMap_Points.vertex_min[i]); + dist_to_map_edge[i][1] = fabs(pos_LiD(i) - LocalMap_Points.vertex_max[i]); + if (dist_to_map_edge[i][0] <= MOV_THRESHOLD * DET_RANGE || dist_to_map_edge[i][1] <= MOV_THRESHOLD * DET_RANGE) need_move = true; + } + if (!need_move) return; + BoxPointType New_LocalMap_Points, tmp_boxpoints; + New_LocalMap_Points = LocalMap_Points; + float mov_dist = max((cube_len - 2.0 * MOV_THRESHOLD * DET_RANGE) * 0.5 * 0.9, double(DET_RANGE * (MOV_THRESHOLD -1))); + for (int i = 0; i < 3; i++){ + tmp_boxpoints = LocalMap_Points; + if (dist_to_map_edge[i][0] <= MOV_THRESHOLD * DET_RANGE){ + New_LocalMap_Points.vertex_max[i] -= mov_dist; + New_LocalMap_Points.vertex_min[i] -= mov_dist; + tmp_boxpoints.vertex_min[i] = LocalMap_Points.vertex_max[i] - mov_dist; + cub_needrm.push_back(tmp_boxpoints); + } else if (dist_to_map_edge[i][1] <= MOV_THRESHOLD * DET_RANGE){ + New_LocalMap_Points.vertex_max[i] += mov_dist; + New_LocalMap_Points.vertex_min[i] += mov_dist; + tmp_boxpoints.vertex_max[i] = LocalMap_Points.vertex_min[i] + mov_dist; + cub_needrm.push_back(tmp_boxpoints); + } + } + LocalMap_Points = New_LocalMap_Points; + + points_cache_collect(); + double delete_begin = omp_get_wtime(); + if(cub_needrm.size() > 0) kdtree_delete_counter = ikdtree.Delete_Point_Boxes(cub_needrm); + kdtree_delete_time = omp_get_wtime() - delete_begin; +} + +void standard_pcl_cbk(const sensor_msgs::PointCloud2::ConstPtr &msg) +{ + mtx_buffer.lock(); + scan_count ++; + double preprocess_start_time = omp_get_wtime(); + if (msg->header.stamp.toSec() < last_timestamp_lidar) + { + ROS_ERROR("lidar loop back, clear buffer"); + lidar_buffer.clear(); + } + + PointCloudXYZI::Ptr ptr(new PointCloudXYZI()); + p_pre->process(msg, ptr); + lidar_buffer.push_back(ptr); + time_buffer.push_back(msg->header.stamp.toSec()); + last_timestamp_lidar = msg->header.stamp.toSec(); + s_plot11[scan_count] = omp_get_wtime() - preprocess_start_time; + mtx_buffer.unlock(); + sig_buffer.notify_all(); +} + +double timediff_lidar_wrt_imu = 0.0; +bool timediff_set_flg = false; +void livox_pcl_cbk(const livox_ros_driver2::CustomMsg::ConstPtr &msg) +{ + mtx_buffer.lock(); + double preprocess_start_time = omp_get_wtime(); + scan_count ++; + if (msg->header.stamp.toSec() < last_timestamp_lidar) + { + ROS_ERROR("lidar loop back, clear buffer"); + lidar_buffer.clear(); + } + last_timestamp_lidar = msg->header.stamp.toSec(); + + if (!time_sync_en && abs(last_timestamp_imu - last_timestamp_lidar) > 10.0 && !imu_buffer.empty() && !lidar_buffer.empty() ) + { + printf("IMU and LiDAR not Synced, IMU time: %lf, lidar header time: %lf \n",last_timestamp_imu, last_timestamp_lidar); + } + + if (time_sync_en && !timediff_set_flg && abs(last_timestamp_lidar - last_timestamp_imu) > 1 && !imu_buffer.empty()) + { + timediff_set_flg = true; + timediff_lidar_wrt_imu = last_timestamp_lidar + 0.1 - last_timestamp_imu; + printf("Self sync IMU and LiDAR, time diff is %.10lf \n", timediff_lidar_wrt_imu); + } + + PointCloudXYZI::Ptr ptr(new PointCloudXYZI()); + p_pre->process(msg, ptr); + lidar_buffer.push_back(ptr); + time_buffer.push_back(last_timestamp_lidar); + + s_plot11[scan_count] = omp_get_wtime() - preprocess_start_time; + mtx_buffer.unlock(); + sig_buffer.notify_all(); +} + +void imu_cbk(const sensor_msgs::Imu::ConstPtr &msg_in) +{ + publish_count ++; + // cout<<"IMU got at: "<header.stamp.toSec()<header.stamp = ros::Time().fromSec(msg_in->header.stamp.toSec() - time_diff_lidar_to_imu); + if (abs(timediff_lidar_wrt_imu) > 0.1 && time_sync_en) + { + msg->header.stamp = \ + ros::Time().fromSec(timediff_lidar_wrt_imu + msg_in->header.stamp.toSec()); + } + + double timestamp = msg->header.stamp.toSec(); + + mtx_buffer.lock(); + + if (timestamp < last_timestamp_imu) + { + ROS_WARN("imu loop back, clear buffer"); + imu_buffer.clear(); + } + + last_timestamp_imu = timestamp; + + imu_buffer.push_back(msg); + mtx_buffer.unlock(); + sig_buffer.notify_all(); +} + +double lidar_mean_scantime = 0.0; +int scan_num = 0; +bool sync_packages(MeasureGroup &meas) +{ + if (lidar_buffer.empty() || imu_buffer.empty()) { + return false; + } + + /*** push a lidar scan ***/ + if(!lidar_pushed) + { + meas.lidar = lidar_buffer.front(); + meas.lidar_beg_time = time_buffer.front(); + if (meas.lidar->points.size() <= 1) // time too little + { + lidar_end_time = meas.lidar_beg_time + lidar_mean_scantime; + ROS_WARN("Too few input point cloud!\n"); + } + else if (meas.lidar->points.back().curvature / double(1000) < 0.5 * lidar_mean_scantime) + { + lidar_end_time = meas.lidar_beg_time + lidar_mean_scantime; + } + else + { + scan_num ++; + lidar_end_time = meas.lidar_beg_time + meas.lidar->points.back().curvature / double(1000); + lidar_mean_scantime += (meas.lidar->points.back().curvature / double(1000) - lidar_mean_scantime) / scan_num; + } + + meas.lidar_end_time = lidar_end_time; + + lidar_pushed = true; + } + + if (last_timestamp_imu < lidar_end_time) + { + return false; + } + + /*** push imu data, and pop from imu buffer ***/ + double imu_time = imu_buffer.front()->header.stamp.toSec(); + meas.imu.clear(); + while ((!imu_buffer.empty()) && (imu_time < lidar_end_time)) + { + imu_time = imu_buffer.front()->header.stamp.toSec(); + if(imu_time > lidar_end_time) break; + meas.imu.push_back(imu_buffer.front()); + imu_buffer.pop_front(); + } + + lidar_buffer.pop_front(); + time_buffer.pop_front(); + lidar_pushed = false; + return true; +} + +int process_increments = 0; +void map_incremental() +{ + PointVector PointToAdd; + PointVector PointNoNeedDownsample; + PointToAdd.reserve(feats_down_size); + PointNoNeedDownsample.reserve(feats_down_size); + for (int i = 0; i < feats_down_size; i++) + { + /* transform to world frame */ + pointBodyToWorld(&(feats_down_body->points[i]), &(feats_down_world->points[i])); + /* decide if need add to map */ + if (!Nearest_Points[i].empty() && flg_EKF_inited) + { + const PointVector &points_near = Nearest_Points[i]; + bool need_add = true; + BoxPointType Box_of_Point; + PointType downsample_result, mid_point; + mid_point.x = floor(feats_down_world->points[i].x/filter_size_map_min)*filter_size_map_min + 0.5 * filter_size_map_min; + mid_point.y = floor(feats_down_world->points[i].y/filter_size_map_min)*filter_size_map_min + 0.5 * filter_size_map_min; + mid_point.z = floor(feats_down_world->points[i].z/filter_size_map_min)*filter_size_map_min + 0.5 * filter_size_map_min; + float dist = calc_dist(feats_down_world->points[i],mid_point); + if (fabs(points_near[0].x - mid_point.x) > 0.5 * filter_size_map_min && fabs(points_near[0].y - mid_point.y) > 0.5 * filter_size_map_min && fabs(points_near[0].z - mid_point.z) > 0.5 * filter_size_map_min){ + PointNoNeedDownsample.push_back(feats_down_world->points[i]); + continue; + } + for (int readd_i = 0; readd_i < NUM_MATCH_POINTS; readd_i ++) + { + if (points_near.size() < NUM_MATCH_POINTS) break; + if (calc_dist(points_near[readd_i], mid_point) < dist) + { + need_add = false; + break; + } + } + if (need_add) PointToAdd.push_back(feats_down_world->points[i]); + } + else + { + PointToAdd.push_back(feats_down_world->points[i]); + } + } + + double st_time = omp_get_wtime(); + add_point_size = ikdtree.Add_Points(PointToAdd, true); + ikdtree.Add_Points(PointNoNeedDownsample, false); + add_point_size = PointToAdd.size() + PointNoNeedDownsample.size(); + kdtree_incremental_time = omp_get_wtime() - st_time; +} + +PointCloudXYZI::Ptr pcl_wait_pub(new PointCloudXYZI(500000, 1)); +PointCloudXYZI::Ptr pcl_wait_save(new PointCloudXYZI()); +void publish_frame_world(const ros::Publisher & pubLaserCloudFull) +{ + if(scan_pub_en) + { + PointCloudXYZI::Ptr laserCloudFullRes(dense_pub_en ? feats_undistort : feats_down_body); + int size = laserCloudFullRes->points.size(); + PointCloudXYZI::Ptr laserCloudWorld( \ + new PointCloudXYZI(size, 1)); + + for (int i = 0; i < size; i++) + { + RGBpointBodyToWorld(&laserCloudFullRes->points[i], \ + &laserCloudWorld->points[i]); + } + + sensor_msgs::PointCloud2 laserCloudmsg; + pcl::toROSMsg(*laserCloudWorld, laserCloudmsg); + laserCloudmsg.header.stamp = ros::Time().fromSec(lidar_end_time); + laserCloudmsg.header.frame_id = "camera_init"; + pubLaserCloudFull.publish(laserCloudmsg); + publish_count -= PUBFRAME_PERIOD; + } + + /**************** save map ****************/ + /* 1. make sure you have enough memories + /* 2. noted that pcd save will influence the real-time performences **/ + if (pcd_save_en) + { + int size = feats_undistort->points.size(); + PointCloudXYZI::Ptr laserCloudWorld( \ + new PointCloudXYZI(size, 1)); + + for (int i = 0; i < size; i++) + { + RGBpointBodyToWorld(&feats_undistort->points[i], \ + &laserCloudWorld->points[i]); + } + *pcl_wait_save += *laserCloudWorld; + + static int scan_wait_num = 0; + scan_wait_num ++; + if (pcl_wait_save->size() > 0 && pcd_save_interval > 0 && scan_wait_num >= pcd_save_interval) + { + pcd_index ++; + string all_points_dir(string(string(ROOT_DIR) + "PCD/scans_") + to_string(pcd_index) + string(".pcd")); + pcl::PCDWriter pcd_writer; + cout << "current scan saved to /PCD/" << all_points_dir << endl; + pcd_writer.writeBinary(all_points_dir, *pcl_wait_save); + pcl_wait_save->clear(); + scan_wait_num = 0; + } + } +} + +void publish_frame_body(const ros::Publisher & pubLaserCloudFull_body) +{ + int size = feats_undistort->points.size(); + PointCloudXYZI::Ptr laserCloudIMUBody(new PointCloudXYZI(size, 1)); + + for (int i = 0; i < size; i++) + { + RGBpointBodyLidarToIMU(&feats_undistort->points[i], \ + &laserCloudIMUBody->points[i]); + } + + sensor_msgs::PointCloud2 laserCloudmsg; + pcl::toROSMsg(*laserCloudIMUBody, laserCloudmsg); + laserCloudmsg.header.stamp = ros::Time().fromSec(lidar_end_time); + laserCloudmsg.header.frame_id = "body"; + pubLaserCloudFull_body.publish(laserCloudmsg); + publish_count -= PUBFRAME_PERIOD; +} + +void publish_effect_world(const ros::Publisher & pubLaserCloudEffect) +{ + PointCloudXYZI::Ptr laserCloudWorld( \ + new PointCloudXYZI(effct_feat_num, 1)); + for (int i = 0; i < effct_feat_num; i++) + { + RGBpointBodyToWorld(&laserCloudOri->points[i], \ + &laserCloudWorld->points[i]); + } + sensor_msgs::PointCloud2 laserCloudFullRes3; + pcl::toROSMsg(*laserCloudWorld, laserCloudFullRes3); + laserCloudFullRes3.header.stamp = ros::Time().fromSec(lidar_end_time); + laserCloudFullRes3.header.frame_id = "camera_init"; + pubLaserCloudEffect.publish(laserCloudFullRes3); +} + +void publish_map(const ros::Publisher & pubLaserCloudMap) +{ + sensor_msgs::PointCloud2 laserCloudMap; + pcl::toROSMsg(*featsFromMap, laserCloudMap); + laserCloudMap.header.stamp = ros::Time().fromSec(lidar_end_time); + laserCloudMap.header.frame_id = "camera_init"; + pubLaserCloudMap.publish(laserCloudMap); +} + +template +void set_posestamp(T & out) +{ + out.pose.position.x = state_point.pos(0); + out.pose.position.y = state_point.pos(1); + out.pose.position.z = state_point.pos(2); + out.pose.orientation.x = geoQuat.x; + out.pose.orientation.y = geoQuat.y; + out.pose.orientation.z = geoQuat.z; + out.pose.orientation.w = geoQuat.w; + +} + +void publish_odometry(const ros::Publisher & pubOdomAftMapped) +{ + odomAftMapped.header.frame_id = "camera_init"; + odomAftMapped.child_frame_id = "body"; + odomAftMapped.header.stamp = ros::Time().fromSec(lidar_end_time);// ros::Time().fromSec(lidar_end_time); + set_posestamp(odomAftMapped.pose); + pubOdomAftMapped.publish(odomAftMapped); + auto P = kf.get_P(); + for (int i = 0; i < 6; i ++) + { + int k = i < 3 ? i + 3 : i - 3; + odomAftMapped.pose.covariance[i*6 + 0] = P(k, 3); + odomAftMapped.pose.covariance[i*6 + 1] = P(k, 4); + odomAftMapped.pose.covariance[i*6 + 2] = P(k, 5); + odomAftMapped.pose.covariance[i*6 + 3] = P(k, 0); + odomAftMapped.pose.covariance[i*6 + 4] = P(k, 1); + odomAftMapped.pose.covariance[i*6 + 5] = P(k, 2); + } + + static tf::TransformBroadcaster br; + tf::Transform transform; + tf::Quaternion q; + transform.setOrigin(tf::Vector3(odomAftMapped.pose.pose.position.x, \ + odomAftMapped.pose.pose.position.y, \ + odomAftMapped.pose.pose.position.z)); + q.setW(odomAftMapped.pose.pose.orientation.w); + q.setX(odomAftMapped.pose.pose.orientation.x); + q.setY(odomAftMapped.pose.pose.orientation.y); + q.setZ(odomAftMapped.pose.pose.orientation.z); + transform.setRotation( q ); + br.sendTransform( tf::StampedTransform( transform, odomAftMapped.header.stamp, "camera_init", "body" ) ); +} + +void publish_path(const ros::Publisher pubPath) +{ + set_posestamp(msg_body_pose); + msg_body_pose.header.stamp = ros::Time().fromSec(lidar_end_time); + msg_body_pose.header.frame_id = "camera_init"; + + /*** if path is too large, the rvis will crash ***/ + static int jjj = 0; + jjj++; + if (jjj % 10 == 0) + { + path.poses.push_back(msg_body_pose); + pubPath.publish(path); + } +} + +void h_share_model(state_ikfom &s, esekfom::dyn_share_datastruct &ekfom_data) +{ + double match_start = omp_get_wtime(); + laserCloudOri->clear(); + corr_normvect->clear(); + total_residual = 0.0; + + /** closest surface search and residual computation **/ + #ifdef MP_EN + omp_set_num_threads(MP_PROC_NUM); + #pragma omp parallel for + #endif + for (int i = 0; i < feats_down_size; i++) + { + PointType &point_body = feats_down_body->points[i]; + PointType &point_world = feats_down_world->points[i]; + + /* transform to world frame */ + V3D p_body(point_body.x, point_body.y, point_body.z); + V3D p_global(s.rot * (s.offset_R_L_I*p_body + s.offset_T_L_I) + s.pos); + point_world.x = p_global(0); + point_world.y = p_global(1); + point_world.z = p_global(2); + point_world.intensity = point_body.intensity; + + vector pointSearchSqDis(NUM_MATCH_POINTS); + + auto &points_near = Nearest_Points[i]; + + if (ekfom_data.converge) + { + /** Find the closest surfaces in the map **/ + ikdtree.Nearest_Search(point_world, NUM_MATCH_POINTS, points_near, pointSearchSqDis); + point_selected_surf[i] = points_near.size() < NUM_MATCH_POINTS ? false : pointSearchSqDis[NUM_MATCH_POINTS - 1] > 5 ? false : true; + } + + if (!point_selected_surf[i]) continue; + + VF(4) pabcd; + point_selected_surf[i] = false; + if (esti_plane(pabcd, points_near, 0.1f)) + { + float pd2 = pabcd(0) * point_world.x + pabcd(1) * point_world.y + pabcd(2) * point_world.z + pabcd(3); + float s = 1 - 0.9 * fabs(pd2) / sqrt(p_body.norm()); + + if (s > 0.9) + { + point_selected_surf[i] = true; + normvec->points[i].x = pabcd(0); + normvec->points[i].y = pabcd(1); + normvec->points[i].z = pabcd(2); + normvec->points[i].intensity = pd2; + res_last[i] = abs(pd2); + } + } + } + + effct_feat_num = 0; + + for (int i = 0; i < feats_down_size; i++) + { + if (point_selected_surf[i]) + { + laserCloudOri->points[effct_feat_num] = feats_down_body->points[i]; + corr_normvect->points[effct_feat_num] = normvec->points[i]; + total_residual += res_last[i]; + effct_feat_num ++; + } + } + + if (effct_feat_num < 1) + { + ekfom_data.valid = false; + ROS_WARN("No Effective Points! \n"); + return; + } + + res_mean_last = total_residual / effct_feat_num; + match_time += omp_get_wtime() - match_start; + double solve_start_ = omp_get_wtime(); + + /*** Computation of Measuremnt Jacobian matrix H and measurents vector ***/ + ekfom_data.h_x = MatrixXd::Zero(effct_feat_num, 12); //23 + ekfom_data.h.resize(effct_feat_num); + + for (int i = 0; i < effct_feat_num; i++) + { + const PointType &laser_p = laserCloudOri->points[i]; + V3D point_this_be(laser_p.x, laser_p.y, laser_p.z); + M3D point_be_crossmat; + point_be_crossmat << SKEW_SYM_MATRX(point_this_be); + V3D point_this = s.offset_R_L_I * point_this_be + s.offset_T_L_I; + M3D point_crossmat; + point_crossmat<points[i]; + V3D norm_vec(norm_p.x, norm_p.y, norm_p.z); + + /*** calculate the Measuremnt Jacobian matrix H ***/ + V3D C(s.rot.conjugate() *norm_vec); + V3D A(point_crossmat * C); + if (extrinsic_est_en) + { + V3D B(point_be_crossmat * s.offset_R_L_I.conjugate() * C); //s.rot.conjugate()*norm_vec); + ekfom_data.h_x.block<1, 12>(i,0) << norm_p.x, norm_p.y, norm_p.z, VEC_FROM_ARRAY(A), VEC_FROM_ARRAY(B), VEC_FROM_ARRAY(C); + } + else + { + ekfom_data.h_x.block<1, 12>(i,0) << norm_p.x, norm_p.y, norm_p.z, VEC_FROM_ARRAY(A), 0.0, 0.0, 0.0, 0.0, 0.0, 0.0; + } + + /*** Measuremnt: distance to the closest surface/corner ***/ + ekfom_data.h(i) = -norm_p.intensity; + } + solve_time += omp_get_wtime() - solve_start_; +} + +int main(int argc, char** argv) +{ + ros::init(argc, argv, "laserMapping"); + ros::NodeHandle nh; + + nh.param("publish/path_en",path_en, true); + nh.param("publish/scan_publish_en",scan_pub_en, true); + nh.param("publish/dense_publish_en",dense_pub_en, true); + nh.param("publish/scan_bodyframe_pub_en",scan_body_pub_en, true); + nh.param("max_iteration",NUM_MAX_ITERATIONS,4); + nh.param("map_file_path",map_file_path,""); + nh.param("common/lid_topic",lid_topic,"/livox/lidar"); + nh.param("common/imu_topic", imu_topic,"/livox/imu"); + nh.param("common/time_sync_en", time_sync_en, false); + nh.param("common/time_offset_lidar_to_imu", time_diff_lidar_to_imu, 0.0); + nh.param("filter_size_corner",filter_size_corner_min,0.5); + nh.param("filter_size_surf",filter_size_surf_min,0.5); + nh.param("filter_size_map",filter_size_map_min,0.5); + nh.param("cube_side_length",cube_len,200); + nh.param("mapping/det_range",DET_RANGE,300.f); + nh.param("mapping/fov_degree",fov_deg,180); + nh.param("mapping/gyr_cov",gyr_cov,0.1); + nh.param("mapping/acc_cov",acc_cov,0.1); + nh.param("mapping/b_gyr_cov",b_gyr_cov,0.0001); + nh.param("mapping/b_acc_cov",b_acc_cov,0.0001); + nh.param("preprocess/blind", p_pre->blind, 0.01); + nh.param("preprocess/lidar_type", p_pre->lidar_type, AVIA); + nh.param("preprocess/scan_line", p_pre->N_SCANS, 16); + nh.param("preprocess/timestamp_unit", p_pre->time_unit, US); + nh.param("preprocess/scan_rate", p_pre->SCAN_RATE, 10); + nh.param("point_filter_num", p_pre->point_filter_num, 2); + nh.param("feature_extract_enable", p_pre->feature_enabled, false); + nh.param("runtime_pos_log_enable", runtime_pos_log, 0); + nh.param("mapping/extrinsic_est_en", extrinsic_est_en, true); + nh.param("pcd_save/pcd_save_en", pcd_save_en, false); + nh.param("pcd_save/interval", pcd_save_interval, -1); + nh.param>("mapping/extrinsic_T", extrinT, vector()); + nh.param>("mapping/extrinsic_R", extrinR, vector()); + cout<<"p_pre->lidar_type "<lidar_type< 179.9 ? 179.9 : (fov_deg + 10.0); + HALF_FOV_COS = cos((FOV_DEG) * 0.5 * PI_M / 180.0); + + _featsArray.reset(new PointCloudXYZI()); + + memset(point_selected_surf, true, sizeof(point_selected_surf)); + memset(res_last, -1000.0f, sizeof(res_last)); + downSizeFilterSurf.setLeafSize(filter_size_surf_min, filter_size_surf_min, filter_size_surf_min); + downSizeFilterMap.setLeafSize(filter_size_map_min, filter_size_map_min, filter_size_map_min); + memset(point_selected_surf, true, sizeof(point_selected_surf)); + memset(res_last, -1000.0f, sizeof(res_last)); + + Lidar_T_wrt_IMU<set_extrinsic(Lidar_T_wrt_IMU, Lidar_R_wrt_IMU); + p_imu->set_gyr_cov(V3D(gyr_cov, gyr_cov, gyr_cov)); + p_imu->set_acc_cov(V3D(acc_cov, acc_cov, acc_cov)); + p_imu->set_gyr_bias_cov(V3D(b_gyr_cov, b_gyr_cov, b_gyr_cov)); + p_imu->set_acc_bias_cov(V3D(b_acc_cov, b_acc_cov, b_acc_cov)); + + double epsi[23] = {0.001}; + fill(epsi, epsi+23, 0.001); + kf.init_dyn_share(get_f, df_dx, df_dw, h_share_model, NUM_MAX_ITERATIONS, epsi); + + /*** debug record ***/ + FILE *fp; + string pos_log_dir = root_dir + "/Log/pos_log.txt"; + fp = fopen(pos_log_dir.c_str(),"w"); + + ofstream fout_pre, fout_out, fout_dbg; + fout_pre.open(DEBUG_FILE_DIR("mat_pre.txt"),ios::out); + fout_out.open(DEBUG_FILE_DIR("mat_out.txt"),ios::out); + fout_dbg.open(DEBUG_FILE_DIR("dbg.txt"),ios::out); + if (fout_pre && fout_out) + cout << "~~~~"<lidar_type == AVIA ? \ + nh.subscribe(lid_topic, 200000, livox_pcl_cbk) : \ + nh.subscribe(lid_topic, 200000, standard_pcl_cbk); + ros::Subscriber sub_imu = nh.subscribe(imu_topic, 200000, imu_cbk); + ros::Publisher pubLaserCloudFull = nh.advertise + ("/cloud_registered", 100000); + ros::Publisher pubLaserCloudFull_body = nh.advertise + ("/cloud_registered_body", 100000); + ros::Publisher pubLaserCloudEffect = nh.advertise + ("/cloud_effected", 100000); + ros::Publisher pubLaserCloudMap = nh.advertise + ("/Laser_map", 100000); + ros::Publisher pubOdomAftMapped = nh.advertise + ("/Odometry", 100000); + ros::Publisher pubPath = nh.advertise + ("/path", 100000); +//------------------------------------------------------------------------------------------------------ + signal(SIGINT, SigHandle); + ros::Rate rate(5000); + bool status = ros::ok(); + while (status) + { + if (flg_exit) break; + ros::spinOnce(); + if(sync_packages(Measures)) + { + if (flg_first_scan) + { + first_lidar_time = Measures.lidar_beg_time; + p_imu->first_lidar_time = first_lidar_time; + flg_first_scan = false; + continue; + } + + double t0,t1,t2,t3,t4,t5,match_start, solve_start, svd_time; + + match_time = 0; + kdtree_search_time = 0.0; + solve_time = 0; + solve_const_H_time = 0; + svd_time = 0; + t0 = omp_get_wtime(); + + p_imu->Process(Measures, kf, feats_undistort); + state_point = kf.get_x(); + pos_lid = state_point.pos + state_point.rot * state_point.offset_T_L_I; + + if (feats_undistort->empty() || (feats_undistort == NULL)) + { + ROS_WARN("No point, skip this scan!\n"); + continue; + } + + flg_EKF_inited = (Measures.lidar_beg_time - first_lidar_time) < INIT_TIME ? \ + false : true; + /*** Segment the map in lidar FOV ***/ + lasermap_fov_segment(); + + /*** downsample the feature points in a scan ***/ + downSizeFilterSurf.setInputCloud(feats_undistort); + downSizeFilterSurf.filter(*feats_down_body); + t1 = omp_get_wtime(); + feats_down_size = feats_down_body->points.size(); + /*** initialize the map kdtree ***/ + if(ikdtree.Root_Node == nullptr) + { + if(feats_down_size > 5) + { + ikdtree.set_downsample_param(filter_size_map_min); + feats_down_world->resize(feats_down_size); + for(int i = 0; i < feats_down_size; i++) + { + pointBodyToWorld(&(feats_down_body->points[i]), &(feats_down_world->points[i])); + } + ikdtree.Build(feats_down_world->points); + } + continue; + } + int featsFromMapNum = ikdtree.validnum(); + kdtree_size_st = ikdtree.size(); + + // cout<<"[ mapping ]: In num: "<points.size()<<" downsamp "<resize(feats_down_size); + feats_down_world->resize(feats_down_size); + + V3D ext_euler = SO3ToEuler(state_point.offset_R_L_I); + fout_pre<clear(); + featsFromMap->points = ikdtree.PCL_Storage; + } + + pointSearchInd_surf.resize(feats_down_size); + Nearest_Points.resize(feats_down_size); + int rematch_num = 0; + bool nearest_search_en = true; // + + t2 = omp_get_wtime(); + + /*** iterated state estimation ***/ + double t_update_start = omp_get_wtime(); + double solve_H_time = 0; + kf.update_iterated_dyn_share_modified(LASER_POINT_COV, solve_H_time); + state_point = kf.get_x(); + euler_cur = SO3ToEuler(state_point.rot); + pos_lid = state_point.pos + state_point.rot * state_point.offset_T_L_I; + geoQuat.x = state_point.rot.coeffs()[0]; + geoQuat.y = state_point.rot.coeffs()[1]; + geoQuat.z = state_point.rot.coeffs()[2]; + geoQuat.w = state_point.rot.coeffs()[3]; + + double t_update_end = omp_get_wtime(); + + /******* Publish odometry *******/ + publish_odometry(pubOdomAftMapped); + + /*** add the feature points to map kdtree ***/ + t3 = omp_get_wtime(); + map_incremental(); + t5 = omp_get_wtime(); + + /******* Publish points *******/ + if (path_en) publish_path(pubPath); + if (scan_pub_en || pcd_save_en) publish_frame_world(pubLaserCloudFull); + if (scan_pub_en && scan_body_pub_en) publish_frame_body(pubLaserCloudFull_body); + // publish_effect_world(pubLaserCloudEffect); + // publish_map(pubLaserCloudMap); + + /*** Debug variables ***/ + if (runtime_pos_log) + { + frame_num ++; + kdtree_size_end = ikdtree.size(); + aver_time_consu = aver_time_consu * (frame_num - 1) / frame_num + (t5 - t0) / frame_num; + aver_time_icp = aver_time_icp * (frame_num - 1)/frame_num + (t_update_end - t_update_start) / frame_num; + aver_time_match = aver_time_match * (frame_num - 1)/frame_num + (match_time)/frame_num; + aver_time_incre = aver_time_incre * (frame_num - 1)/frame_num + (kdtree_incremental_time)/frame_num; + aver_time_solve = aver_time_solve * (frame_num - 1)/frame_num + (solve_time + solve_H_time)/frame_num; + aver_time_const_H_time = aver_time_const_H_time * (frame_num - 1)/frame_num + solve_time / frame_num; + T1[time_log_counter] = Measures.lidar_beg_time; + s_plot[time_log_counter] = t5 - t0; + s_plot2[time_log_counter] = feats_undistort->points.size(); + s_plot3[time_log_counter] = kdtree_incremental_time; + s_plot4[time_log_counter] = kdtree_search_time; + s_plot5[time_log_counter] = kdtree_delete_counter; + s_plot6[time_log_counter] = kdtree_delete_time; + s_plot7[time_log_counter] = kdtree_size_st; + s_plot8[time_log_counter] = kdtree_size_end; + s_plot9[time_log_counter] = aver_time_consu; + s_plot10[time_log_counter] = add_point_size; + time_log_counter ++; + printf("[ mapping ]: time: IMU + Map + Input Downsample: %0.6f ave match: %0.6f ave solve: %0.6f ave ICP: %0.6f map incre: %0.6f ave total: %0.6f icp: %0.6f construct H: %0.6f \n",t1-t0,aver_time_match,aver_time_solve,t3-t1,t5-t3,aver_time_consu,aver_time_icp, aver_time_const_H_time); + ext_euler = SO3ToEuler(state_point.offset_R_L_I); + fout_out << setw(20) << Measures.lidar_beg_time - first_lidar_time << " " << euler_cur.transpose() << " " << state_point.pos.transpose()<< " " << ext_euler.transpose() << " "<points.size()<size() > 0 && pcd_save_en) + { + string file_name = string("scans.pcd"); + string all_points_dir(string(string(ROOT_DIR) + "PCD/") + file_name); + pcl::PCDWriter pcd_writer; + cout << "current scan saved to /PCD/" << file_name< t, s_vec, s_vec2, s_vec3, s_vec4, s_vec5, s_vec6, s_vec7; + FILE *fp2; + string log_dir = root_dir + "/Log/fast_lio_time_log.csv"; + fp2 = fopen(log_dir.c_str(),"w"); + fprintf(fp2,"time_stamp, total time, scan point size, incremental time, search time, delete size, delete time, tree size st, tree size end, add point size, preprocess time\n"); + for (int i = 0;ipoint_num; + // cout<<"plsie: "<points[i].line < N_SCANS) && ((msg->points[i].tag & 0x30) == 0x10 || (msg->points[i].tag & 0x30) == 0x00)) + { + pl_full[i].x = msg->points[i].x; + pl_full[i].y = msg->points[i].y; + pl_full[i].z = msg->points[i].z; + pl_full[i].intensity = msg->points[i].reflectivity; + pl_full[i].curvature = msg->points[i].offset_time / float(1000000); //use curvature as time of each laser points + + bool is_new = false; + if((abs(pl_full[i].x - pl_full[i-1].x) > 1e-7) + || (abs(pl_full[i].y - pl_full[i-1].y) > 1e-7) + || (abs(pl_full[i].z - pl_full[i-1].z) > 1e-7)) + { + pl_buff[msg->points[i].line].push_back(pl_full[i]); + } + } + } + static int count = 0; + static double time = 0.0; + count ++; + double t0 = omp_get_wtime(); + for(int j=0; j &pl = pl_buff[j]; + plsize = pl.size(); + vector &types = typess[j]; + types.clear(); + types.resize(plsize); + plsize--; + for(uint i=0; ipoints[i].line < N_SCANS) && ((msg->points[i].tag & 0x30) == 0x10 || (msg->points[i].tag & 0x30) == 0x00)) + { + valid_num ++; + if (valid_num % point_filter_num == 0) + { + pl_full[i].x = msg->points[i].x; + pl_full[i].y = msg->points[i].y; + pl_full[i].z = msg->points[i].z; + pl_full[i].intensity = msg->points[i].reflectivity; + pl_full[i].curvature = msg->points[i].offset_time / float(1000000); // use curvature as time of each laser points, curvature unit: ms + + if(((abs(pl_full[i].x - pl_full[i-1].x) > 1e-7) + || (abs(pl_full[i].y - pl_full[i-1].y) > 1e-7) + || (abs(pl_full[i].z - pl_full[i-1].z) > 1e-7)) + && (pl_full[i].x * pl_full[i].x + pl_full[i].y * pl_full[i].y + pl_full[i].z * pl_full[i].z > (blind * blind))) + { + pl_surf.push_back(pl_full[i]); + } + } + } + } + } +} + +void Preprocess::oust64_handler(const sensor_msgs::PointCloud2::ConstPtr &msg) +{ + pl_surf.clear(); + pl_corn.clear(); + pl_full.clear(); + pcl::PointCloud pl_orig; + pcl::fromROSMsg(*msg, pl_orig); + int plsize = pl_orig.size(); + pl_corn.reserve(plsize); + pl_surf.reserve(plsize); + if (feature_enabled) + { + for (int i = 0; i < N_SCANS; i++) + { + pl_buff[i].clear(); + pl_buff[i].reserve(plsize); + } + + for (uint i = 0; i < plsize; i++) + { + double range = pl_orig.points[i].x * pl_orig.points[i].x + pl_orig.points[i].y * pl_orig.points[i].y + pl_orig.points[i].z * pl_orig.points[i].z; + if (range < (blind * blind)) continue; + Eigen::Vector3d pt_vec; + PointType added_pt; + added_pt.x = pl_orig.points[i].x; + added_pt.y = pl_orig.points[i].y; + added_pt.z = pl_orig.points[i].z; + added_pt.intensity = pl_orig.points[i].intensity; + added_pt.normal_x = 0; + added_pt.normal_y = 0; + added_pt.normal_z = 0; + double yaw_angle = atan2(added_pt.y, added_pt.x) * 57.3; + if (yaw_angle >= 180.0) + yaw_angle -= 360.0; + if (yaw_angle <= -180.0) + yaw_angle += 360.0; + + added_pt.curvature = pl_orig.points[i].t * time_unit_scale; + if(pl_orig.points[i].ring < N_SCANS) + { + pl_buff[pl_orig.points[i].ring].push_back(added_pt); + } + } + + for (int j = 0; j < N_SCANS; j++) + { + PointCloudXYZI &pl = pl_buff[j]; + int linesize = pl.size(); + vector &types = typess[j]; + types.clear(); + types.resize(linesize); + linesize--; + for (uint i = 0; i < linesize; i++) + { + types[i].range = sqrt(pl[i].x * pl[i].x + pl[i].y * pl[i].y); + vx = pl[i].x - pl[i + 1].x; + vy = pl[i].y - pl[i + 1].y; + vz = pl[i].z - pl[i + 1].z; + types[i].dista = vx * vx + vy * vy + vz * vz; + } + types[linesize].range = sqrt(pl[linesize].x * pl[linesize].x + pl[linesize].y * pl[linesize].y); + give_feature(pl, types); + } + } + else + { + double time_stamp = msg->header.stamp.toSec(); + // cout << "===================================" << endl; + // printf("Pt size = %d, N_SCANS = %d\r\n", plsize, N_SCANS); + for (int i = 0; i < pl_orig.points.size(); i++) + { + if (i % point_filter_num != 0) continue; + + double range = pl_orig.points[i].x * pl_orig.points[i].x + pl_orig.points[i].y * pl_orig.points[i].y + pl_orig.points[i].z * pl_orig.points[i].z; + + if (range < (blind * blind)) continue; + + Eigen::Vector3d pt_vec; + PointType added_pt; + added_pt.x = pl_orig.points[i].x; + added_pt.y = pl_orig.points[i].y; + added_pt.z = pl_orig.points[i].z; + added_pt.intensity = pl_orig.points[i].intensity; + added_pt.normal_x = 0; + added_pt.normal_y = 0; + added_pt.normal_z = 0; + added_pt.curvature = pl_orig.points[i].t * time_unit_scale; // curvature unit: ms + + pl_surf.points.push_back(added_pt); + } + } + // pub_func(pl_surf, pub_full, msg->header.stamp); + // pub_func(pl_surf, pub_corn, msg->header.stamp); +} + +void Preprocess::velodyne_handler(const sensor_msgs::PointCloud2::ConstPtr &msg) +{ + pl_surf.clear(); + pl_corn.clear(); + pl_full.clear(); + + pcl::PointCloud pl_orig; + pcl::fromROSMsg(*msg, pl_orig); + int plsize = pl_orig.points.size(); + if (plsize == 0) return; + pl_surf.reserve(plsize); + + /*** These variables only works when no point timestamps given ***/ + double omega_l = 0.361 * SCAN_RATE; // scan angular velocity + std::vector is_first(N_SCANS,true); + std::vector yaw_fp(N_SCANS, 0.0); // yaw of first scan point + std::vector yaw_last(N_SCANS, 0.0); // yaw of last scan point + std::vector time_last(N_SCANS, 0.0); // last offset time + /*****************************************************************/ + + if (pl_orig.points[plsize - 1].time > 0) + { + given_offset_time = true; + } + else + { + given_offset_time = false; + double yaw_first = atan2(pl_orig.points[0].y, pl_orig.points[0].x) * 57.29578; + double yaw_end = yaw_first; + int layer_first = pl_orig.points[0].ring; + for (uint i = plsize - 1; i > 0; i--) + { + if (pl_orig.points[i].ring == layer_first) + { + yaw_end = atan2(pl_orig.points[i].y, pl_orig.points[i].x) * 57.29578; + break; + } + } + } + + if(feature_enabled) + { + for (int i = 0; i < N_SCANS; i++) + { + pl_buff[i].clear(); + pl_buff[i].reserve(plsize); + } + + for (int i = 0; i < plsize; i++) + { + PointType added_pt; + added_pt.normal_x = 0; + added_pt.normal_y = 0; + added_pt.normal_z = 0; + int layer = pl_orig.points[i].ring; + if (layer >= N_SCANS) continue; + added_pt.x = pl_orig.points[i].x; + added_pt.y = pl_orig.points[i].y; + added_pt.z = pl_orig.points[i].z; + added_pt.intensity = pl_orig.points[i].intensity; + added_pt.curvature = pl_orig.points[i].time * time_unit_scale; // units: ms + + if (!given_offset_time) + { + double yaw_angle = atan2(added_pt.y, added_pt.x) * 57.2957; + if (is_first[layer]) + { + // printf("layer: %d; is first: %d", layer, is_first[layer]); + yaw_fp[layer]=yaw_angle; + is_first[layer]=false; + added_pt.curvature = 0.0; + yaw_last[layer]=yaw_angle; + time_last[layer]=added_pt.curvature; + continue; + } + + if (yaw_angle <= yaw_fp[layer]) + { + added_pt.curvature = (yaw_fp[layer]-yaw_angle) / omega_l; + } + else + { + added_pt.curvature = (yaw_fp[layer]-yaw_angle+360.0) / omega_l; + } + + if (added_pt.curvature < time_last[layer]) added_pt.curvature+=360.0/omega_l; + + yaw_last[layer] = yaw_angle; + time_last[layer]=added_pt.curvature; + } + + pl_buff[layer].points.push_back(added_pt); + } + + for (int j = 0; j < N_SCANS; j++) + { + PointCloudXYZI &pl = pl_buff[j]; + int linesize = pl.size(); + if (linesize < 2) continue; + vector &types = typess[j]; + types.clear(); + types.resize(linesize); + linesize--; + for (uint i = 0; i < linesize; i++) + { + types[i].range = sqrt(pl[i].x * pl[i].x + pl[i].y * pl[i].y); + vx = pl[i].x - pl[i + 1].x; + vy = pl[i].y - pl[i + 1].y; + vz = pl[i].z - pl[i + 1].z; + types[i].dista = vx * vx + vy * vy + vz * vz; + } + types[linesize].range = sqrt(pl[linesize].x * pl[linesize].x + pl[linesize].y * pl[linesize].y); + give_feature(pl, types); + } + } + else + { + for (int i = 0; i < plsize; i++) + { + PointType added_pt; + // cout<<"!!!!!!"< (blind * blind)) + { + pl_surf.points.push_back(added_pt); + } + } + } + } +} + +void Preprocess::give_feature(pcl::PointCloud &pl, vector &types) +{ + int plsize = pl.size(); + int plsize2; + if(plsize == 0) + { + printf("something wrong\n"); + return; + } + uint head = 0; + + while(types[head].range < blind) + { + head++; + } + + // Surf + plsize2 = (plsize > group_size) ? (plsize - group_size) : 0; + + Eigen::Vector3d curr_direct(Eigen::Vector3d::Zero()); + Eigen::Vector3d last_direct(Eigen::Vector3d::Zero()); + + uint i_nex = 0, i2; + uint last_i = 0; uint last_i_nex = 0; + int last_state = 0; + int plane_type; + + for(uint i=head; i0.5) + if(last_state==1 && last_direct.norm()>0.1) + { + double mod = last_direct.transpose() * curr_direct; + if(mod>-0.707 && mod<0.707) + { + types[i].ftype = Edge_Plane; + } + else + { + types[i].ftype = Real_Plane; + } + } + + i = i_nex - 1; + last_state = 1; + } + else // if(plane_type == 2) + { + i = i_nex; + last_state = 0; + } + // else if(plane_type == 0) + // { + // if(last_state == 1) + // { + // uint i_nex_tem; + // uint j; + // for(j=last_i+1; j<=last_i_nex; j++) + // { + // uint i_nex_tem2 = i_nex_tem; + // Eigen::Vector3d curr_direct2; + + // uint ttem = plane_judge(pl, types, j, i_nex_tem, curr_direct2); + + // if(ttem != 1) + // { + // i_nex_tem = i_nex_tem2; + // break; + // } + // curr_direct = curr_direct2; + // } + + // if(j == last_i+1) + // { + // last_state = 0; + // } + // else + // { + // for(uint k=last_i_nex; k<=i_nex_tem; k++) + // { + // if(k != i_nex_tem) + // { + // types[k].ftype = Real_Plane; + // } + // else + // { + // types[k].ftype = Poss_Plane; + // } + // } + // i = i_nex_tem-1; + // i_nex = i_nex_tem; + // i2 = j-1; + // last_state = 1; + // } + + // } + // } + + last_i = i2; + last_i_nex = i_nex; + last_direct = curr_direct; + } + + plsize2 = plsize > 3 ? plsize - 3 : 0; + for(uint i=head+3; i=Real_Plane) + { + continue; + } + + if(types[i-1].dista<1e-16 || types[i].dista<1e-16) + { + continue; + } + + Eigen::Vector3d vec_a(pl[i].x, pl[i].y, pl[i].z); + Eigen::Vector3d vecs[2]; + + for(int j=0; j<2; j++) + { + int m = -1; + if(j == 1) + { + m = 1; + } + + if(types[i+m].range < blind) + { + if(types[i].range > inf_bound) + { + types[i].edj[j] = Nr_inf; + } + else + { + types[i].edj[j] = Nr_blind; + } + continue; + } + + vecs[j] = Eigen::Vector3d(pl[i+m].x, pl[i+m].y, pl[i+m].z); + vecs[j] = vecs[j] - vec_a; + + types[i].angle[j] = vec_a.dot(vecs[j]) / vec_a.norm() / vecs[j].norm(); + if(types[i].angle[j] < jump_up_limit) + { + types[i].edj[j] = Nr_180; + } + else if(types[i].angle[j] > jump_down_limit) + { + types[i].edj[j] = Nr_zero; + } + } + + types[i].intersect = vecs[Prev].dot(vecs[Next]) / vecs[Prev].norm() / vecs[Next].norm(); + if(types[i].edj[Prev]==Nr_nor && types[i].edj[Next]==Nr_zero && types[i].dista>0.0225 && types[i].dista>4*types[i-1].dista) + { + if(types[i].intersect > cos160) + { + if(edge_jump_judge(pl, types, i, Prev)) + { + types[i].ftype = Edge_Jump; + } + } + } + else if(types[i].edj[Prev]==Nr_zero && types[i].edj[Next]== Nr_nor && types[i-1].dista>0.0225 && types[i-1].dista>4*types[i].dista) + { + if(types[i].intersect > cos160) + { + if(edge_jump_judge(pl, types, i, Next)) + { + types[i].ftype = Edge_Jump; + } + } + } + else if(types[i].edj[Prev]==Nr_nor && types[i].edj[Next]==Nr_inf) + { + if(edge_jump_judge(pl, types, i, Prev)) + { + types[i].ftype = Edge_Jump; + } + } + else if(types[i].edj[Prev]==Nr_inf && types[i].edj[Next]==Nr_nor) + { + if(edge_jump_judge(pl, types, i, Next)) + { + types[i].ftype = Edge_Jump; + } + + } + else if(types[i].edj[Prev]>Nr_nor && types[i].edj[Next]>Nr_nor) + { + if(types[i].ftype == Nor) + { + types[i].ftype = Wire; + } + } + } + + plsize2 = plsize-1; + double ratio; + for(uint i=head+1; i types[i].dista) + { + ratio = types[i-1].dista / types[i].dista; + } + else + { + ratio = types[i].dista / types[i-1].dista; + } + + if(types[i].intersect &types, uint i_cur, uint &i_nex, Eigen::Vector3d &curr_direct) +{ + double group_dis = disA*types[i_cur].range + disB; + group_dis = group_dis * group_dis; + // i_nex = i_cur; + + double two_dis; + vector disarr; + disarr.reserve(20); + + for(i_nex=i_cur; i_nex= pl.size()) || (i_nex >= pl.size())) break; + + if(types[i_nex].range < blind) + { + curr_direct.setZero(); + return 2; + } + vx = pl[i_nex].x - pl[i_cur].x; + vy = pl[i_nex].y - pl[i_cur].y; + vz = pl[i_nex].z - pl[i_cur].z; + two_dis = vx*vx + vy*vy + vz*vz; + if(two_dis >= group_dis) + { + break; + } + disarr.push_back(types[i_nex].dista); + i_nex++; + } + + double leng_wid = 0; + double v1[3], v2[3]; + for(uint j=i_cur+1; j= pl.size()) || (i_cur >= pl.size())) break; + v1[0] = pl[j].x - pl[i_cur].x; + v1[1] = pl[j].y - pl[i_cur].y; + v1[2] = pl[j].z - pl[i_cur].z; + + v2[0] = v1[1]*vz - vy*v1[2]; + v2[1] = v1[2]*vx - v1[0]*vz; + v2[2] = v1[0]*vy - vx*v1[1]; + + double lw = v2[0]*v2[0] + v2[1]*v2[1] + v2[2]*v2[2]; + if(lw > leng_wid) + { + leng_wid = lw; + } + } + + + if((two_dis*two_dis/leng_wid) < p2l_ratio) + { + curr_direct.setZero(); + return 0; + } + + uint disarrsize = disarr.size(); + for(uint j=0; j=limit_maxmid || dismid_min>=limit_midmin) + { + curr_direct.setZero(); + return 0; + } + } + else + { + double dismax_min = disarr[0] / disarr[disarrsize-2]; + if(dismax_min >= limit_maxmin) + { + curr_direct.setZero(); + return 0; + } + } + + curr_direct << vx, vy, vz; + curr_direct.normalize(); + return 1; +} + +bool Preprocess::edge_jump_judge(const PointCloudXYZI &pl, vector &types, uint i, Surround nor_dir) +{ + if(nor_dir == 0) + { + if(types[i-1].rangeedgea*d2 || (d1-d2)>edgeb) + { + return false; + } + + return true; +} diff --git a/src/FAST_LIO/src/preprocess.h b/src/FAST_LIO/src/preprocess.h new file mode 100644 index 0000000..2012822 --- /dev/null +++ b/src/FAST_LIO/src/preprocess.h @@ -0,0 +1,124 @@ +#include +#include +#include +#include + +using namespace std; + +#define IS_VALID(a) ((abs(a)>1e8) ? true : false) + +typedef pcl::PointXYZINormal PointType; +typedef pcl::PointCloud PointCloudXYZI; + +enum LID_TYPE{AVIA = 1, VELO16, OUST64}; //{1, 2, 3} +enum TIME_UNIT{SEC = 0, MS = 1, US = 2, NS = 3}; +enum Feature{Nor, Poss_Plane, Real_Plane, Edge_Jump, Edge_Plane, Wire, ZeroPoint}; +enum Surround{Prev, Next}; +enum E_jump{Nr_nor, Nr_zero, Nr_180, Nr_inf, Nr_blind}; + +struct orgtype +{ + double range; + double dista; + double angle[2]; + double intersect; + E_jump edj[2]; + Feature ftype; + orgtype() + { + range = 0; + edj[Prev] = Nr_nor; + edj[Next] = Nr_nor; + ftype = Nor; + intersect = 2; + } +}; + +namespace velodyne_ros { + struct EIGEN_ALIGN16 Point { + PCL_ADD_POINT4D; + float intensity; + float time; + uint16_t ring; + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + }; +} // namespace velodyne_ros +POINT_CLOUD_REGISTER_POINT_STRUCT(velodyne_ros::Point, + (float, x, x) + (float, y, y) + (float, z, z) + (float, intensity, intensity) + (float, time, time) + (uint16_t, ring, ring) +) + +namespace ouster_ros { + struct EIGEN_ALIGN16 Point { + PCL_ADD_POINT4D; + float intensity; + uint32_t t; + uint16_t reflectivity; + uint8_t ring; + uint16_t ambient; + uint32_t range; + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + }; +} // namespace ouster_ros + +// clang-format off +POINT_CLOUD_REGISTER_POINT_STRUCT(ouster_ros::Point, + (float, x, x) + (float, y, y) + (float, z, z) + (float, intensity, intensity) + // use std::uint32_t to avoid conflicting with pcl::uint32_t + (std::uint32_t, t, t) + (std::uint16_t, reflectivity, reflectivity) + (std::uint8_t, ring, ring) + (std::uint16_t, ambient, ambient) + (std::uint32_t, range, range) +) + +class Preprocess +{ + public: +// EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + Preprocess(); + ~Preprocess(); + + void process(const livox_ros_driver2::CustomMsg::ConstPtr &msg, PointCloudXYZI::Ptr &pcl_out); + void process(const sensor_msgs::PointCloud2::ConstPtr &msg, PointCloudXYZI::Ptr &pcl_out); + void set(bool feat_en, int lid_type, double bld, int pfilt_num); + + // sensor_msgs::PointCloud2::ConstPtr pointcloud; + PointCloudXYZI pl_full, pl_corn, pl_surf; + PointCloudXYZI pl_buff[128]; //maximum 128 line lidar + vector typess[128]; //maximum 128 line lidar + float time_unit_scale; + int lidar_type, point_filter_num, N_SCANS, SCAN_RATE, time_unit; + double blind; + bool feature_enabled, given_offset_time; + ros::Publisher pub_full, pub_surf, pub_corn; + + + private: + void avia_handler(const livox_ros_driver2::CustomMsg::ConstPtr &msg); + void oust64_handler(const sensor_msgs::PointCloud2::ConstPtr &msg); + void velodyne_handler(const sensor_msgs::PointCloud2::ConstPtr &msg); + void give_feature(PointCloudXYZI &pl, vector &types); + void pub_func(PointCloudXYZI &pl, const ros::Time &ct); + int plane_judge(const PointCloudXYZI &pl, vector &types, uint i, uint &i_nex, Eigen::Vector3d &curr_direct); + bool small_plane(const PointCloudXYZI &pl, vector &types, uint i_cur, uint &i_nex, Eigen::Vector3d &curr_direct); + bool edge_jump_judge(const PointCloudXYZI &pl, vector &types, uint i, Surround nor_dir); + + int group_size; + double disA, disB, inf_bound; + double limit_maxmid, limit_midmin, limit_maxmin; + double p2l_ratio; + double jump_up_limit, jump_down_limit; + double cos160; + double edgea, edgeb; + double smallp_intersect, smallp_ratio; + double vx, vy, vz; +}; diff --git a/src/Point-LIO/.gitmodules b/src/Point-LIO/.gitmodules new file mode 100644 index 0000000..dd6379e --- /dev/null +++ b/src/Point-LIO/.gitmodules @@ -0,0 +1,8 @@ +[submodule "ikd-Tree"] + path = ikd-Tree + url = https://github.com/hku-mars/ikd-Tree.git + branch = fast_lio +[submodule "include/IKFoM"] + path = include/IKFoM + url = https://github.com/hku-mars/IKFoM.git + branch = toolkit diff --git a/src/Point-LIO/CMakeLists.txt b/src/Point-LIO/CMakeLists.txt new file mode 100755 index 0000000..1b5e314 --- /dev/null +++ b/src/Point-LIO/CMakeLists.txt @@ -0,0 +1,83 @@ +cmake_minimum_required(VERSION 2.8.3) +project(point_lio) + +SET(CMAKE_BUILD_TYPE "Debug") + +ADD_COMPILE_OPTIONS(-std=c++14 ) +ADD_COMPILE_OPTIONS(-std=c++14 ) +set( CMAKE_CXX_FLAGS "-std=c++14 -O3" ) + +add_definitions(-DROOT_DIR=\"${CMAKE_CURRENT_SOURCE_DIR}/\") + +set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -fexceptions" ) +set(CMAKE_CXX_STANDARD 14) +set(CMAKE_CXX_STANDARD_REQUIRED ON) +set(CMAKE_CXX_EXTENSIONS OFF) +set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++14 -pthread -std=c++0x -std=c++14 -fexceptions") + +message("Current CPU archtecture: ${CMAKE_SYSTEM_PROCESSOR}") +if(CMAKE_SYSTEM_PROCESSOR MATCHES "(x86)|(X86)|(amd64)|(AMD64)" ) + include(ProcessorCount) + ProcessorCount(N) + message("Processer number: ${N}") + if(N GREATER 5) + add_definitions(-DMP_EN) + add_definitions(-DMP_PROC_NUM=4) + message("core for MP: 3") + elseif(N GREATER 3) + math(EXPR PROC_NUM "${N} - 2") + add_definitions(-DMP_EN) + add_definitions(-DMP_PROC_NUM="${PROC_NUM}") + message("core for MP: ${PROC_NUM}") + else() + add_definitions(-DMP_PROC_NUM=1) + endif() +else() + add_definitions(-DMP_PROC_NUM=1) +endif() + +find_package(OpenMP QUIET) +set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS}") +set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${OpenMP_C_FLAGS}") + +find_package(PythonLibs REQUIRED) +find_path(MATPLOTLIB_CPP_INCLUDE_DIRS "matplotlibcpp.h") + +find_package(catkin REQUIRED COMPONENTS + geometry_msgs + nav_msgs + sensor_msgs + roscpp + rospy + std_msgs + pcl_ros + tf + livox_ros_driver2 + message_generation + eigen_conversions +) + +find_package(Eigen3 REQUIRED) +find_package(PCL 1.8 REQUIRED) + +message(Eigen: ${EIGEN3_INCLUDE_DIR}) + +include_directories( + ${catkin_INCLUDE_DIRS} + ${EIGEN3_INCLUDE_DIR} + ${PCL_INCLUDE_DIRS} + ${PYTHON_INCLUDE_DIRS} + include) + +catkin_package( + CATKIN_DEPENDS geometry_msgs nav_msgs roscpp rospy std_msgs message_runtime + DEPENDS EIGEN3 PCL + INCLUDE_DIRS +) + +add_executable(pointlio_mapping src/laserMapping.cpp include/ikd-Tree/ikd_Tree.cpp src/parameters.cpp src/preprocess.cpp src/Estimator.cpp) +target_link_libraries(pointlio_mapping ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${PYTHON_LIBRARIES}) +target_include_directories(pointlio_mapping PRIVATE ${PYTHON_INCLUDE_DIRS}) + + + diff --git a/src/Point-LIO/LICENSE b/src/Point-LIO/LICENSE new file mode 100644 index 0000000..d159169 --- /dev/null +++ b/src/Point-LIO/LICENSE @@ -0,0 +1,339 @@ + GNU GENERAL PUBLIC LICENSE + Version 2, June 1991 + + Copyright (C) 1989, 1991 Free Software Foundation, Inc., + 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA + Everyone is permitted to copy and distribute verbatim copies + of this license document, but changing it is not allowed. + + Preamble + + The licenses for most software are designed to take away your +freedom to share and change it. By contrast, the GNU General Public +License is intended to guarantee your freedom to share and change free +software--to make sure the software is free for all its users. This +General Public License applies to most of the Free Software +Foundation's software and to any other program whose authors commit to +using it. (Some other Free Software Foundation software is covered by +the GNU Lesser General Public License instead.) You can apply it to +your programs, too. + + When we speak of free software, we are referring to freedom, not +price. Our General Public Licenses are designed to make sure that you +have the freedom to distribute copies of free software (and charge for +this service if you wish), that you receive source code or can get it +if you want it, that you can change the software or use pieces of it +in new free programs; and that you know you can do these things. + + To protect your rights, we need to make restrictions that forbid +anyone to deny you these rights or to ask you to surrender the rights. +These restrictions translate to certain responsibilities for you if you +distribute copies of the software, or if you modify it. + + For example, if you distribute copies of such a program, whether +gratis or for a fee, you must give the recipients all the rights that +you have. You must make sure that they, too, receive or can get the +source code. And you must show them these terms so they know their +rights. + + We protect your rights with two steps: (1) copyright the software, and +(2) offer you this license which gives you legal permission to copy, +distribute and/or modify the software. + + Also, for each author's protection and ours, we want to make certain +that everyone understands that there is no warranty for this free +software. If the software is modified by someone else and passed on, we +want its recipients to know that what they have is not the original, so +that any problems introduced by others will not reflect on the original +authors' reputations. + + Finally, any free program is threatened constantly by software +patents. We wish to avoid the danger that redistributors of a free +program will individually obtain patent licenses, in effect making the +program proprietary. To prevent this, we have made it clear that any +patent must be licensed for everyone's free use or not licensed at all. + + The precise terms and conditions for copying, distribution and +modification follow. + + GNU GENERAL PUBLIC LICENSE + TERMS AND CONDITIONS FOR COPYING, DISTRIBUTION AND MODIFICATION + + 0. This License applies to any program or other work which contains +a notice placed by the copyright holder saying it may be distributed +under the terms of this General Public License. The "Program", below, +refers to any such program or work, and a "work based on the Program" +means either the Program or any derivative work under copyright law: +that is to say, a work containing the Program or a portion of it, +either verbatim or with modifications and/or translated into another +language. (Hereinafter, translation is included without limitation in +the term "modification".) Each licensee is addressed as "you". + +Activities other than copying, distribution and modification are not +covered by this License; they are outside its scope. The act of +running the Program is not restricted, and the output from the Program +is covered only if its contents constitute a work based on the +Program (independent of having been made by running the Program). +Whether that is true depends on what the Program does. + + 1. You may copy and distribute verbatim copies of the Program's +source code as you receive it, in any medium, provided that you +conspicuously and appropriately publish on each copy an appropriate +copyright notice and disclaimer of warranty; keep intact all the +notices that refer to this License and to the absence of any warranty; +and give any other recipients of the Program a copy of this License +along with the Program. + +You may charge a fee for the physical act of transferring a copy, and +you may at your option offer warranty protection in exchange for a fee. + + 2. You may modify your copy or copies of the Program or any portion +of it, thus forming a work based on the Program, and copy and +distribute such modifications or work under the terms of Section 1 +above, provided that you also meet all of these conditions: + + a) You must cause the modified files to carry prominent notices + stating that you changed the files and the date of any change. + + b) You must cause any work that you distribute or publish, that in + whole or in part contains or is derived from the Program or any + part thereof, to be licensed as a whole at no charge to all third + parties under the terms of this License. + + c) If the modified program normally reads commands interactively + when run, you must cause it, when started running for such + interactive use in the most ordinary way, to print or display an + announcement including an appropriate copyright notice and a + notice that there is no warranty (or else, saying that you provide + a warranty) and that users may redistribute the program under + these conditions, and telling the user how to view a copy of this + License. (Exception: if the Program itself is interactive but + does not normally print such an announcement, your work based on + the Program is not required to print an announcement.) + +These requirements apply to the modified work as a whole. If +identifiable sections of that work are not derived from the Program, +and can be reasonably considered independent and separate works in +themselves, then this License, and its terms, do not apply to those +sections when you distribute them as separate works. But when you +distribute the same sections as part of a whole which is a work based +on the Program, the distribution of the whole must be on the terms of +this License, whose permissions for other licensees extend to the +entire whole, and thus to each and every part regardless of who wrote it. + +Thus, it is not the intent of this section to claim rights or contest +your rights to work written entirely by you; rather, the intent is to +exercise the right to control the distribution of derivative or +collective works based on the Program. + +In addition, mere aggregation of another work not based on the Program +with the Program (or with a work based on the Program) on a volume of +a storage or distribution medium does not bring the other work under +the scope of this License. + + 3. You may copy and distribute the Program (or a work based on it, +under Section 2) in object code or executable form under the terms of +Sections 1 and 2 above provided that you also do one of the following: + + a) Accompany it with the complete corresponding machine-readable + source code, which must be distributed under the terms of Sections + 1 and 2 above on a medium customarily used for software interchange; or, + + b) Accompany it with a written offer, valid for at least three + years, to give any third party, for a charge no more than your + cost of physically performing source distribution, a complete + machine-readable copy of the corresponding source code, to be + distributed under the terms of Sections 1 and 2 above on a medium + customarily used for software interchange; or, + + c) Accompany it with the information you received as to the offer + to distribute corresponding source code. (This alternative is + allowed only for noncommercial distribution and only if you + received the program in object code or executable form with such + an offer, in accord with Subsection b above.) + +The source code for a work means the preferred form of the work for +making modifications to it. For an executable work, complete source +code means all the source code for all modules it contains, plus any +associated interface definition files, plus the scripts used to +control compilation and installation of the executable. However, as a +special exception, the source code distributed need not include +anything that is normally distributed (in either source or binary +form) with the major components (compiler, kernel, and so on) of the +operating system on which the executable runs, unless that component +itself accompanies the executable. + +If distribution of executable or object code is made by offering +access to copy from a designated place, then offering equivalent +access to copy the source code from the same place counts as +distribution of the source code, even though third parties are not +compelled to copy the source along with the object code. + + 4. You may not copy, modify, sublicense, or distribute the Program +except as expressly provided under this License. Any attempt +otherwise to copy, modify, sublicense or distribute the Program is +void, and will automatically terminate your rights under this License. +However, parties who have received copies, or rights, from you under +this License will not have their licenses terminated so long as such +parties remain in full compliance. + + 5. You are not required to accept this License, since you have not +signed it. However, nothing else grants you permission to modify or +distribute the Program or its derivative works. These actions are +prohibited by law if you do not accept this License. Therefore, by +modifying or distributing the Program (or any work based on the +Program), you indicate your acceptance of this License to do so, and +all its terms and conditions for copying, distributing or modifying +the Program or works based on it. + + 6. Each time you redistribute the Program (or any work based on the +Program), the recipient automatically receives a license from the +original licensor to copy, distribute or modify the Program subject to +these terms and conditions. You may not impose any further +restrictions on the recipients' exercise of the rights granted herein. +You are not responsible for enforcing compliance by third parties to +this License. + + 7. If, as a consequence of a court judgment or allegation of patent +infringement or for any other reason (not limited to patent issues), +conditions are imposed on you (whether by court order, agreement or +otherwise) that contradict the conditions of this License, they do not +excuse you from the conditions of this License. If you cannot +distribute so as to satisfy simultaneously your obligations under this +License and any other pertinent obligations, then as a consequence you +may not distribute the Program at all. For example, if a patent +license would not permit royalty-free redistribution of the Program by +all those who receive copies directly or indirectly through you, then +the only way you could satisfy both it and this License would be to +refrain entirely from distribution of the Program. + +If any portion of this section is held invalid or unenforceable under +any particular circumstance, the balance of the section is intended to +apply and the section as a whole is intended to apply in other +circumstances. + +It is not the purpose of this section to induce you to infringe any +patents or other property right claims or to contest validity of any +such claims; this section has the sole purpose of protecting the +integrity of the free software distribution system, which is +implemented by public license practices. Many people have made +generous contributions to the wide range of software distributed +through that system in reliance on consistent application of that +system; it is up to the author/donor to decide if he or she is willing +to distribute software through any other system and a licensee cannot +impose that choice. + +This section is intended to make thoroughly clear what is believed to +be a consequence of the rest of this License. + + 8. If the distribution and/or use of the Program is restricted in +certain countries either by patents or by copyrighted interfaces, the +original copyright holder who places the Program under this License +may add an explicit geographical distribution limitation excluding +those countries, so that distribution is permitted only in or among +countries not thus excluded. In such case, this License incorporates +the limitation as if written in the body of this License. + + 9. The Free Software Foundation may publish revised and/or new versions +of the General Public License from time to time. Such new versions will +be similar in spirit to the present version, but may differ in detail to +address new problems or concerns. + +Each version is given a distinguishing version number. If the Program +specifies a version number of this License which applies to it and "any +later version", you have the option of following the terms and conditions +either of that version or of any later version published by the Free +Software Foundation. If the Program does not specify a version number of +this License, you may choose any version ever published by the Free Software +Foundation. + + 10. If you wish to incorporate parts of the Program into other free +programs whose distribution conditions are different, write to the author +to ask for permission. For software which is copyrighted by the Free +Software Foundation, write to the Free Software Foundation; we sometimes +make exceptions for this. Our decision will be guided by the two goals +of preserving the free status of all derivatives of our free software and +of promoting the sharing and reuse of software generally. + + NO WARRANTY + + 11. BECAUSE THE PROGRAM IS LICENSED FREE OF CHARGE, THERE IS NO WARRANTY +FOR THE PROGRAM, TO THE EXTENT PERMITTED BY APPLICABLE LAW. EXCEPT WHEN +OTHERWISE STATED IN WRITING THE COPYRIGHT HOLDERS AND/OR OTHER PARTIES +PROVIDE THE PROGRAM "AS IS" WITHOUT WARRANTY OF ANY KIND, EITHER EXPRESSED +OR IMPLIED, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF +MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE. THE ENTIRE RISK AS +TO THE QUALITY AND PERFORMANCE OF THE PROGRAM IS WITH YOU. SHOULD THE +PROGRAM PROVE DEFECTIVE, YOU ASSUME THE COST OF ALL NECESSARY SERVICING, +REPAIR OR CORRECTION. + + 12. IN NO EVENT UNLESS REQUIRED BY APPLICABLE LAW OR AGREED TO IN WRITING +WILL ANY COPYRIGHT HOLDER, OR ANY OTHER PARTY WHO MAY MODIFY AND/OR +REDISTRIBUTE THE PROGRAM AS PERMITTED ABOVE, BE LIABLE TO YOU FOR DAMAGES, +INCLUDING ANY GENERAL, SPECIAL, INCIDENTAL OR CONSEQUENTIAL DAMAGES ARISING +OUT OF THE USE OR INABILITY TO USE THE PROGRAM (INCLUDING BUT NOT LIMITED +TO LOSS OF DATA OR DATA BEING RENDERED INACCURATE OR LOSSES SUSTAINED BY +YOU OR THIRD PARTIES OR A FAILURE OF THE PROGRAM TO OPERATE WITH ANY OTHER +PROGRAMS), EVEN IF SUCH HOLDER OR OTHER PARTY HAS BEEN ADVISED OF THE +POSSIBILITY OF SUCH DAMAGES. + + END OF TERMS AND CONDITIONS + + How to Apply These Terms to Your New Programs + + If you develop a new program, and you want it to be of the greatest +possible use to the public, the best way to achieve this is to make it +free software which everyone can redistribute and change under these terms. + + To do so, attach the following notices to the program. It is safest +to attach them to the start of each source file to most effectively +convey the exclusion of warranty; and each file should have at least +the "copyright" line and a pointer to where the full notice is found. + + + Copyright (C) + + This program is free software; you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation; either version 2 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License along + with this program; if not, write to the Free Software Foundation, Inc., + 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA. + +Also add information on how to contact you by electronic and paper mail. + +If the program is interactive, make it output a short notice like this +when it starts in an interactive mode: + + Gnomovision version 69, Copyright (C) year name of author + Gnomovision comes with ABSOLUTELY NO WARRANTY; for details type `show w'. + This is free software, and you are welcome to redistribute it + under certain conditions; type `show c' for details. + +The hypothetical commands `show w' and `show c' should show the appropriate +parts of the General Public License. Of course, the commands you use may +be called something other than `show w' and `show c'; they could even be +mouse-clicks or menu items--whatever suits your program. + +You should also get your employer (if you work as a programmer) or your +school, if any, to sign a "copyright disclaimer" for the program, if +necessary. Here is a sample; alter the names: + + Yoyodyne, Inc., hereby disclaims all copyright interest in the program + `Gnomovision' (which makes passes at compilers) written by James Hacker. + + , 1 April 1989 + Ty Coon, President of Vice + +This General Public License does not permit incorporating your program into +proprietary programs. If your program is a subroutine library, you may +consider it more useful to permit linking proprietary applications with the +library. If this is what you want to do, use the GNU Lesser General +Public License instead of this License. diff --git a/src/Point-LIO/Log/guide.md b/src/Point-LIO/Log/guide.md new file mode 100755 index 0000000..883b3d4 --- /dev/null +++ b/src/Point-LIO/Log/guide.md @@ -0,0 +1 @@ +Here saved the debug records which can be drew by the ../log/plot.py. The record function can be found frm the MACRO: DEBUG_FILE_DIR(name) in common_lib.h. diff --git a/src/Point-LIO/Log/imu.txt b/src/Point-LIO/Log/imu.txt new file mode 100755 index 0000000..e69de29 diff --git a/src/Point-LIO/Log/imu_pbp.txt b/src/Point-LIO/Log/imu_pbp.txt new file mode 100644 index 0000000..e69de29 diff --git a/src/Point-LIO/Log/mat_out.txt b/src/Point-LIO/Log/mat_out.txt new file mode 100644 index 0000000..e69de29 diff --git a/src/Point-LIO/Log/plot.py b/src/Point-LIO/Log/plot.py new file mode 100755 index 0000000..b5d1c1a --- /dev/null +++ b/src/Point-LIO/Log/plot.py @@ -0,0 +1,46 @@ +# import matplotlib +# matplotlib.use('Agg') +import numpy as np +import matplotlib.pyplot as plt + +a_out=np.loadtxt('mat_out.txt') +#######for normal####### +fig, axs = plt.subplots(3,2) +lab_out = ['', 'out-x', 'out-y', 'out-z'] +plot_ind = range(7,10) +time=a_out[:,0] +axs[0,0].set_title('Attitude') +axs[1,0].set_title('Translation') +axs[2,0].set_title('Velocity') +axs[0,1].set_title('bg') +axs[1,1].set_title('ba') +axs[2,1].set_title('Gravity') +for i in range(1,4): + for j in range(6): + axs[j%3, j//3].plot(time, a_out[:,i+j*3],'.-', label=lab_out[i]) + for j in range(6): + axs[j%3, j//3].grid() + axs[j%3, j//3].legend() +plt.grid() + #######for normal####### + + +#### Draw IMU data +#fig, axs = plt.subplots(2) +#imu=np.loadtxt('imu_pbp.txt') +#time=imu[:,0] +#axs[0].set_title('Gyroscope') +#axs[1].set_title('Accelerameter') +#lab_1 = ['gyr-x', 'gyr-y', 'gyr-z'] +#lab_2 = ['acc-x', 'acc-y', 'acc-z'] +#for i in range(3): + #if i==1: +# axs[0].plot(time, imu[:,i+1],'.-', label=lab_1[i]) +# axs[1].plot(time, imu[:,i+4],'.-', label=lab_2[i]) +#for i in range(2): + #axs[i].set_xlim(386,389) +# axs[i].grid() +# axs[i].legend() +#plt.grid() + +plt.show() diff --git a/src/Point-LIO/Log/plot_imu.py b/src/Point-LIO/Log/plot_imu.py new file mode 100755 index 0000000..0177273 --- /dev/null +++ b/src/Point-LIO/Log/plot_imu.py @@ -0,0 +1,125 @@ +# import matplotlib +# matplotlib.use('Agg') +import numpy as np +import matplotlib.pyplot as plt + +#### Draw IMU data +fig, axs = plt.subplots(2) +imu=np.loadtxt('imu_pbp.txt') +time=imu[:,0] +axs[0].set_title('Gyroscope') +axs[1].set_title('Accelerameter') +lab_1 = ['gyr-x', 'gyr-y', 'gyr-z'] +lab_2 = ['acc-x', 'acc-y', 'acc-z'] +for i in range(3): + #if i==1: + axs[0].plot(time, imu[:,i+1],'.-', label=lab_1[i]) + axs[1].plot(time, imu[:,i+4],'.-', label=lab_2[i]) +for i in range(2): + #axs[i].set_xlim(386,389) + axs[i].grid() + axs[i].legend() +plt.grid() + +#fig, axs = plt.subplots(5) +#axs[0].set_title('miss') +#axs[1].set_title('miss') +#axs[2].set_title('miss') +#axs[3].set_title('miss') +#axs[4].set_title('miss') +#len_time1 = np.arange(0,1977) +#len_time2 = np.arange(1977, 3954) +#len_time3 = np.arange(3954,5931) +#len_time4 = np.arange(5931,7908) +#len_time5 = np.arange(7908,9885) + #if i==1: +#axs[0].plot(len_time1, time[0:1977],'.-', label='check') +#axs[1].plot(len_time2, time[1977:3954],'.-', label='check') +#axs[2].plot(len_time3, time[3954:5931],'.-', label='check') +#axs[3].plot(len_time4, time[5931:7908],'.-', label='check') +#axs[4].plot(len_time5, time[7908:9885],'.-', label='check') + #axs[i].set_xlim(386,389) +#axs[0].grid() +#axs[0].legend() +#axs[1].grid() +#axs[1].legend() +#axs[2].grid() +#axs[2].legend() +#axs[3].grid() +#axs[3].legend() +#axs[4].grid() +#axs[4].legend() +#plt.grid() + +#fig, axs = plt.subplots(5) +#axs[0].set_title('miss') +#axs[1].set_title('miss') +#axs[2].set_title('miss') +#axs[3].set_title('miss') +#axs[4].set_title('miss') +#len_time1 = np.arange(9885,9885+1977) +#len_time2 = np.arange(9885+1977,9885+3954) +#len_time3 = np.arange(9885+3954,9885+5931) +#len_time4 = np.arange(9885+5931,9885+7908) +#len_time5 = np.arange(9885+7908,9885+9885) + #if i==1: +#axs[0].plot(len_time1, time[9885+0:9885+1977],'.-', label='check') +#axs[1].plot(len_time2, time[9885+1977:9885+3954],'.-', label='check') +#axs[2].plot(len_time3, time[9885+3954:9885+5931],'.-', label='check') +#axs[3].plot(len_time4, time[9885+5931:9885+7908],'.-', label='check') +#axs[4].plot(len_time5, time[9885+7908:9885+9885],'.-', label='check') + #axs[i].set_xlim(386,389) +#axs[0].grid() +#axs[0].legend() +#axs[1].grid() +#axs[1].legend() +#axs[2].grid() +#axs[2].legend() +#axs[3].grid() +#axs[3].legend() +#axs[4].grid() +#axs[4].legend() +#plt.grid() + +# #### Draw time calculation +# plt.figure(3) +# fig = plt.figure() +# font1 = {'family' : 'Times New Roman', +# 'weight' : 'normal', +# 'size' : 12, +# } +# c="red" +# a_out1=np.loadtxt('Log/mat_out_time_indoor1.txt') +# a_out2=np.loadtxt('Log/mat_out_time_indoor2.txt') +# a_out3=np.loadtxt('Log/mat_out_time_outdoor.txt') +# # n = a_out[:,1].size +# # time_mean = a_out[:,1].mean() +# # time_se = a_out[:,1].std() / np.sqrt(n) +# # time_err = a_out[:,1] - time_mean +# # feat_mean = a_out[:,2].mean() +# # feat_err = a_out[:,2] - feat_mean +# # feat_se = a_out[:,2].std() / np.sqrt(n) +# ax1 = fig.add_subplot(111) +# ax1.set_ylabel('Effective Feature Numbers',font1) +# ax1.boxplot(a_out1[:,2], showfliers=False, positions=[0.9]) +# ax1.boxplot(a_out2[:,2], showfliers=False, positions=[1.9]) +# ax1.boxplot(a_out3[:,2], showfliers=False, positions=[2.9]) +# ax1.set_ylim([0, 3000]) + +# ax2 = ax1.twinx() +# ax2.spines['right'].set_color('red') +# ax2.set_ylabel('Compute Time (ms)',font1) +# ax2.yaxis.label.set_color('red') +# ax2.tick_params(axis='y', colors='red') +# ax2.boxplot(a_out1[:,1]*1000, showfliers=False, positions=[1.1],boxprops=dict(color=c),capprops=dict(color=c),whiskerprops=dict(color=c)) +# ax2.boxplot(a_out2[:,1]*1000, showfliers=False, positions=[2.1],boxprops=dict(color=c),capprops=dict(color=c),whiskerprops=dict(color=c)) +# ax2.boxplot(a_out3[:,1]*1000, showfliers=False, positions=[3.1],boxprops=dict(color=c),capprops=dict(color=c),whiskerprops=dict(color=c)) +# ax2.set_xlim([0.5, 3.5]) +# ax2.set_ylim([0, 100]) + +# plt.xticks([1,2,3], ('Outdoor Scene', 'Indoor Scene 1', 'Indoor Scene 2')) +# # # print(time_se) +# # # print(a_out3[:,2]) +# plt.grid() +# plt.savefig("time.pdf", dpi=1200) +plt.show() diff --git a/src/Point-LIO/Log/plot_out.py b/src/Point-LIO/Log/plot_out.py new file mode 100755 index 0000000..8dd80cb --- /dev/null +++ b/src/Point-LIO/Log/plot_out.py @@ -0,0 +1,178 @@ +# import matplotlib +# matplotlib.use('Agg') +import numpy as np +import matplotlib.pyplot as plt + +# a_pre=np.loadtxt('mat_pre.txt') +a_out=np.loadtxt('mat_out.txt') +if((a_out.shape[1] != 19) & (a_out.shape[1] != 20)): + ######for ikfom + fig, axs = plt.subplots(4,2) + #lab_pre = ['', 'pre-x', 'pre-y', 'pre-z'] + lab_out = ['', 'out-x', 'out-y', 'out-z'] + plot_ind = range(7,10) + time=a_out[:,0] + axs[0,0].set_title('Attitude') + axs[1,0].set_title('Translation') + axs[2,0].set_title('Velocity') + axs[3,0].set_title('Angular velocity') + axs[0,1].set_title('Acceleration') + axs[1,1].set_title('Gravity') + axs[2,1].set_title('bg') + axs[3,1].set_title('ba') + for i in range(1,4): + for j in range(8): + #axs[j%4, j//4].plot(time, a_pre[:,i+j*3],'.-', label=lab_pre[i]) + axs[j%4, j//4].plot(time, a_out[:,i+j*3],'.-', label=lab_out[i]) + for j in range(8): + # axs[j].set_xlim(386,389) + axs[j%4, j//4].grid() + axs[j%4, j//4].legend() + plt.grid() + ######for ikfom####### +else: + #######for normal####### + fig, axs = plt.subplots(3,2) + lab_pre = ['', 'pre-x', 'pre-y', 'pre-z'] + lab_out = ['', 'out-x', 'out-y', 'out-z'] + plot_ind = range(7,10) + time=a_out[:,0] + time1 = a_pre[:,0] + axs[0,0].set_title('Attitude') + axs[1,0].set_title('Translation') + axs[2,0].set_title('Velocity') + axs[0,1].set_title('bg') + axs[1,1].set_title('ba') + axs[2,1].set_title('Gravity') + for i in range(1,4): + for j in range(6): + axs[j%3, j/3].plot(time1, a_pre[:,i+j*3],'.-', label=lab_pre[i]) + axs[j%3, j/3].plot(time, a_out[:,i+j*3],'.-', label=lab_out[i]) + for j in range(6): + # axs[j].set_xlim(386,389) + axs[j%3, j//3].grid() + axs[j%3, j//3].legend() + plt.grid() + #######for normal####### + + +#### Draw IMU data +fig, axs = plt.subplots(2) +imu=np.loadtxt('imu_pbp.txt') +time=imu[:,0] +axs[0].set_title('Gyroscope') +axs[1].set_title('Accelerameter') +lab_1 = ['gyr-x', 'gyr-y', 'gyr-z'] +lab_2 = ['acc-x', 'acc-y', 'acc-z'] +for i in range(3): + #if i==1: + axs[0].plot(time, imu[:,i+1],'.-', label=lab_1[i]) + axs[1].plot(time, imu[:,i+4],'.-', label=lab_2[i]) +for i in range(2): + #axs[i].set_xlim(386,389) + axs[i].grid() + axs[i].legend() +plt.grid() + +#fig, axs = plt.subplots(5) +#axs[0].set_title('miss') +#axs[1].set_title('miss') +#axs[2].set_title('miss') +#axs[3].set_title('miss') +#axs[4].set_title('miss') +#len_time1 = np.arange(0,1977) +#len_time2 = np.arange(1977, 3954) +#len_time3 = np.arange(3954,5931) +#len_time4 = np.arange(5931,7908) +#len_time5 = np.arange(7908,9885) + #if i==1: +#axs[0].plot(len_time1, time[0:1977],'.-', label='check') +#axs[1].plot(len_time2, time[1977:3954],'.-', label='check') +#axs[2].plot(len_time3, time[3954:5931],'.-', label='check') +#axs[3].plot(len_time4, time[5931:7908],'.-', label='check') +#axs[4].plot(len_time5, time[7908:9885],'.-', label='check') + #axs[i].set_xlim(386,389) +#axs[0].grid() +#axs[0].legend() +#axs[1].grid() +#axs[1].legend() +#axs[2].grid() +#axs[2].legend() +#axs[3].grid() +#axs[3].legend() +#axs[4].grid() +#axs[4].legend() +#plt.grid() + +#fig, axs = plt.subplots(5) +#axs[0].set_title('miss') +#axs[1].set_title('miss') +#axs[2].set_title('miss') +#axs[3].set_title('miss') +#axs[4].set_title('miss') +#len_time1 = np.arange(9885,9885+1977) +#len_time2 = np.arange(9885+1977,9885+3954) +#len_time3 = np.arange(9885+3954,9885+5931) +#len_time4 = np.arange(9885+5931,9885+7908) +#len_time5 = np.arange(9885+7908,9885+9885) + #if i==1: +#axs[0].plot(len_time1, time[9885+0:9885+1977],'.-', label='check') +#axs[1].plot(len_time2, time[9885+1977:9885+3954],'.-', label='check') +#axs[2].plot(len_time3, time[9885+3954:9885+5931],'.-', label='check') +#axs[3].plot(len_time4, time[9885+5931:9885+7908],'.-', label='check') +#axs[4].plot(len_time5, time[9885+7908:9885+9885],'.-', label='check') + #axs[i].set_xlim(386,389) +#axs[0].grid() +#axs[0].legend() +#axs[1].grid() +#axs[1].legend() +#axs[2].grid() +#axs[2].legend() +#axs[3].grid() +#axs[3].legend() +#axs[4].grid() +#axs[4].legend() +#plt.grid() + +# #### Draw time calculation +# plt.figure(3) +# fig = plt.figure() +# font1 = {'family' : 'Times New Roman', +# 'weight' : 'normal', +# 'size' : 12, +# } +# c="red" +# a_out1=np.loadtxt('Log/mat_out_time_indoor1.txt') +# a_out2=np.loadtxt('Log/mat_out_time_indoor2.txt') +# a_out3=np.loadtxt('Log/mat_out_time_outdoor.txt') +# # n = a_out[:,1].size +# # time_mean = a_out[:,1].mean() +# # time_se = a_out[:,1].std() / np.sqrt(n) +# # time_err = a_out[:,1] - time_mean +# # feat_mean = a_out[:,2].mean() +# # feat_err = a_out[:,2] - feat_mean +# # feat_se = a_out[:,2].std() / np.sqrt(n) +# ax1 = fig.add_subplot(111) +# ax1.set_ylabel('Effective Feature Numbers',font1) +# ax1.boxplot(a_out1[:,2], showfliers=False, positions=[0.9]) +# ax1.boxplot(a_out2[:,2], showfliers=False, positions=[1.9]) +# ax1.boxplot(a_out3[:,2], showfliers=False, positions=[2.9]) +# ax1.set_ylim([0, 3000]) + +# ax2 = ax1.twinx() +# ax2.spines['right'].set_color('red') +# ax2.set_ylabel('Compute Time (ms)',font1) +# ax2.yaxis.label.set_color('red') +# ax2.tick_params(axis='y', colors='red') +# ax2.boxplot(a_out1[:,1]*1000, showfliers=False, positions=[1.1],boxprops=dict(color=c),capprops=dict(color=c),whiskerprops=dict(color=c)) +# ax2.boxplot(a_out2[:,1]*1000, showfliers=False, positions=[2.1],boxprops=dict(color=c),capprops=dict(color=c),whiskerprops=dict(color=c)) +# ax2.boxplot(a_out3[:,1]*1000, showfliers=False, positions=[3.1],boxprops=dict(color=c),capprops=dict(color=c),whiskerprops=dict(color=c)) +# ax2.set_xlim([0.5, 3.5]) +# ax2.set_ylim([0, 100]) + +# plt.xticks([1,2,3], ('Outdoor Scene', 'Indoor Scene 1', 'Indoor Scene 2')) +# # # print(time_se) +# # # print(a_out3[:,2]) +# plt.grid() +# plt.savefig("time.pdf", dpi=1200) +plt.show() diff --git a/src/Point-LIO/Log/pos_log.txt b/src/Point-LIO/Log/pos_log.txt new file mode 100644 index 0000000..e69de29 diff --git a/src/Point-LIO/PCD/temp.txt b/src/Point-LIO/PCD/temp.txt new file mode 100644 index 0000000..8b13789 --- /dev/null +++ b/src/Point-LIO/PCD/temp.txt @@ -0,0 +1 @@ + diff --git a/src/Point-LIO/README.md b/src/Point-LIO/README.md new file mode 100644 index 0000000..179a295 --- /dev/null +++ b/src/Point-LIO/README.md @@ -0,0 +1,183 @@ +# Point-LIO +## 1. Introduction + +
+
+ +
+ The framework and key points of the Point-LIO. +
+ +**New features:** +1. would not fly under degeneration. +2. high odometry output frequency, 4k-8kHz. +3. robust to IMU saturation and severe vibration, and other aggressive motions (75 rad/s in our test). +4. no motion distortion. +5. computationally efficient, robust, versatile on public datasets with general motions. +6. As an odometry, Point-LIO could be used in various autonomous tasks, such as trajectory planning, control, and perception, especially in cases involving very fast ego-motions (e.g., in the presence of severe vibration and high angular or linear velocity) or requiring high-rate odometry output and mapping (e.g., for high-rate feedback control and perception). + +**Important notes:** + +A. Please make sure the IMU and LiDAR are **Synchronized**, that's important. + +B. Please obtain the saturation values of your used IMU (i.e., accelerator and gyroscope), and the units of the accelerator of your used IMU, then modify the .yaml file according to those settings, including values of 'satu_acc', 'satu_gyro', 'acc_norm'. That's improtant. + +C. The warning message "Failed to find match for field 'time'." means the timestamps of each LiDAR points are missed in the rosbag file. That is important because Point-LIO processes at the sampling time of each LiDAR point. + +D. We recommend to set the **extrinsic_est_en** to false if the extrinsic is given. As for the extrinsic initiallization, please refer to our recent work: [**Robust and Online LiDAR-inertial Initialization**](https://github.com/hku-mars/LiDAR_IMU_Init). + +E. If a high odometry output frequency without downsample is required, set ``` publish_odometry_without_downsample ``` as true. Then the warning message of tf "TF_REPEATED_DATA" will pop up in the terminal window, because the time interval between two publish odometery is too small. The following command could be used to suppress this warning to a smaller frequency: + +in your catkin_ws/src, + +git clone --branch throttle-tf-repeated-data-error git@github.com:BadgerTechnologies/geometry2.git + +Then rebuild, source setup.bash, run and then it should be reduced down to once every 10 seconds. If 10 seconds is still too much log output then change the ros::Duration(10.0) to 10000 seconds or whatever you like. + +F. If you want to use Point-LIO without imu, set the "imu_en" as false, and provide a predefined value of gavity in "gravity_init" as true as possible in the yaml file, and keep the "use_imu_as_input" as 0. + +## **1.1. Developers:** +The codes of this repo are contributed by: +[Dongjiao He (贺东娇)](https://github.com/Joanna-HE) and [Wei Xu (徐威)](https://github.com/XW-HKU) + + +## **1.2. Related paper** +Our paper is published on Advanced Intelligent Systems(AIS). [Point-LIO](https://onlinelibrary.wiley.com/doi/epdf/10.1002/aisy.202200459), DOI: 10.1002/aisy.202200459 + + +## **1.3. Related video** +Our accompany video is available on **YouTube**. +
+ +
+ +## 2. What can Point-LIO do? +### 2.1 Simultaneous LiDAR localization and mapping (SLAM) without motion distortion + +### 2.2 Produce high odometry output frequence and high bandwidth + +### 2.3 SLAM with aggressive motions even the IMU is saturated + +# **3. Prerequisites** + +## **3.1 Ubuntu and [ROS](https://www.ros.org/)** +We tested our code on Ubuntu20.04 with noetic. Ubuntu18.04 and lower versions have problems of environments to support the Point-LIO, try to avoid using Point-LIO in those systems. Additional ROS package is required: +``` +sudo apt-get install ros-xxx-pcl-conversions +``` + +## **3.2 Eigen** +Following the official [Eigen installation](eigen.tuxfamily.org/index.php?title=Main_Page), or directly install Eigen by: +``` +sudo apt-get install libeigen3-dev +``` + +## **3.3 livox_ros_driver** +Follow [livox_ros_driver Installation](https://github.com/Livox-SDK/livox_ros_driver). + +*Remarks:* +- Since the Point-LIO supports Livox serials LiDAR, so the **livox_ros_driver** must be installed and **sourced** before run any Point-LIO luanch file. +- How to source? The easiest way is add the line ``` source $Licox_ros_driver_dir$/devel/setup.bash ``` to the end of file ``` ~/.bashrc ```, where ``` $Licox_ros_driver_dir$ ``` is the directory of the livox ros driver workspace (should be the ``` ws_livox ``` directory if you completely followed the livox official document). + +## 4. Build +Clone the repository and catkin_make: + +``` + cd ~/$A_ROS_DIR$/src + git clone https://github.com/hku-mars/Point-LIO.git + cd Point-LIO + git submodule update --init + cd ../.. + catkin_make + source devel/setup.bash +``` +- Remember to source the livox_ros_driver before build (follow 3.3 **livox_ros_driver**) +- If you want to use a custom build of PCL, add the following line to ~/.bashrc +```export PCL_ROOT={CUSTOM_PCL_PATH}``` + +## 5. Directly run + +### 5.1 For Avia +Connect to your PC to Livox Avia LiDAR by following [Livox-ros-driver installation](https://github.com/Livox-SDK/livox_ros_driver), then +``` + cd ~/$Point_LIO_ROS_DIR$ + source devel/setup.bash + roslaunch point_lio mapping_avia.launch + roslaunch livox_ros_driver livox_lidar_msg.launch +``` +- For livox serials, Point-LIO only support the data collected by the ``` livox_lidar_msg.launch ``` since only its ``` livox_ros_driver/CustomMsg ``` data structure produces the timestamp of each LiDAR point which is very important for Point-LIO. ``` livox_lidar.launch ``` can not produce it right now. +- If you want to change the frame rate, please modify the **publish_freq** parameter in the [livox_lidar_msg.launch](https://github.com/Livox-SDK/livox_ros_driver/blob/master/livox_ros_driver/launch/livox_lidar_msg.launch) of [Livox-ros-driver](https://github.com/Livox-SDK/livox_ros_driver) before make the livox_ros_driver pakage. + +### 5.2 For Livox serials with external IMU + +mapping_avia.launch theratically supports mid-70, mid-40 or other livox serial LiDAR, but need to setup some parameters befor run: + +Edit ``` config/avia.yaml ``` to set the below parameters: + +1. LiDAR point cloud topic name: ``` lid_topic ``` +2. IMU topic name: ``` imu_topic ``` +3. Translational extrinsic: ``` extrinsic_T ``` +4. Rotational extrinsic: ``` extrinsic_R ``` (only support rotation matrix) +- The extrinsic parameters in Point-LIO is defined as the LiDAR's pose (position and rotation matrix) in IMU body frame (i.e. the IMU is the base frame). They can be found in the official manual. +5. Saturation value of IMU's accelerator and gyroscope: ```satu_acc```, ```satu_gyro``` +6. The norm of IMU's acceleration according to unit of acceleration messages: ``` acc_norm ``` + +### 5.3 For Velodyne or Ouster (Velodyne as an example) + +Step A: Setup before run + +Edit ``` config/velodyne.yaml ``` to set the below parameters: + +1. LiDAR point cloud topic name: ``` lid_topic ``` +2. IMU topic name: ``` imu_topic ``` (both internal and external, 6-aixes or 9-axies are fine) +3. Set the parameter ```timestamp_unit``` based on the unit of **time** (Velodyne) or **t** (Ouster) field in PoindCloud2 rostopic +4. Line number (we tested 16, 32 and 64 line, but not tested 128 or above): ``` scan_line ``` +5. Translational extrinsic: ``` extrinsic_T ``` +6. Rotational extrinsic: ``` extrinsic_R ``` (only support rotation matrix) +- The extrinsic parameters in Point-LIO is defined as the LiDAR's pose (position and rotation matrix) in IMU body frame (i.e. the IMU is the base frame). +7. Saturation value of IMU's accelerator and gyroscope: ```satu_acc```, ```satu_gyro``` +8. The norm of IMU's acceleration according to unit of acceleration messages: ``` acc_norm ``` + +Step B: Run below +``` + cd ~/$Point_LIO_ROS_DIR$ + source devel/setup.bash + roslaunch point_lio mapping_velody16.launch +``` + +Step C: Run LiDAR's ros driver or play rosbag. + +### 5.4 PCD file save + +Set ``` pcd_save_enable ``` in launchfile to ``` 1 ```. All the scans (in global frame) will be accumulated and saved to the file ``` Point-LIO/PCD/scans.pcd ``` after the Point-LIO is terminated. ```pcl_viewer scans.pcd``` can visualize the point clouds. + +*Tips for pcl_viewer:* +- change what to visualize/color by pressing keyboard 1,2,3,4,5 when pcl_viewer is running. +``` + 1 is all random + 2 is X values + 3 is Y values + 4 is Z values + 5 is intensity +``` + +# **6. Examples** + +The example datasets could be downloaded through [onedrive](https://connecthkuhk-my.sharepoint.com/:f:/g/personal/hdj65822_connect_hku_hk/EmRJYy4ZfAlMiIJ786ogCPoBcGQ2BAchuXjE5oJQjrQu0Q?e=igu44W). Pay attention that if you want to test on racing_drone.bag, [0.0, 9.810, 0.0] should be input in 'mapping/gravity_init' in avia.yaml, and set the 'start_in_aggressive_motion' as true in the yaml. Because this bag start from a high speed motion. And for PULSAR.bag, we change the measuring range of the gyroscope of the built-in IMU to 17.5 rad/s. Therefore, when you test on this bag, please change 'satu_gyro' to 17.5 in avia.yaml. + +## **6.1. Example-1: SLAM on datasets with aggressive motions where IMU is saturated** +
+ + +
+ +## **6.2. Example-2: Application on FPV and PULSAR** +
+ + +
+ +PULSAR is a self-rotating UAV actuated by only one motor, [PULSAR](https://github.com/hku-mars/PULSAR) + +## 7. Contact us +If you have any questions about this work, please feel free to contact me and Dr. Fu Zhang via email. diff --git a/src/Point-LIO/config/avia.yaml b/src/Point-LIO/config/avia.yaml new file mode 100755 index 0000000..4321930 --- /dev/null +++ b/src/Point-LIO/config/avia.yaml @@ -0,0 +1,57 @@ +common: + lid_topic: "/livox/lidar" + imu_topic: "/livox/imu" + con_frame: false # true: if you need to combine several LiDAR frames into one + con_frame_num: 1 # the number of frames combined + cut_frame: false # true: if you need to cut one LiDAR frame into several subframes + cut_frame_time_interval: 0.1 # should be integral fraction of 1 / LiDAR frequency + time_lag_imu_to_lidar: 0.0 # Time offset between LiDAR and IMU calibrated by other algorithms, e.g., LI-Init (find in Readme) + # the timesample of IMU is transferred from the current timeline to LiDAR's timeline by subtracting this value + +preprocess: + lidar_type: 1 + scan_line: 6 + timestamp_unit: 1 # the unit of time/t field in the PointCloud2 rostopic: 0-second, 1-milisecond, 2-microsecond, 3-nanosecond. + blind: 1.0 + +mapping: + imu_en: true + start_in_aggressive_motion: false # if true, a preknown gravity should be provided in following gravity_init + extrinsic_est_en: false # for aggressive motion, set this variable false + imu_time_inte: 0.005 # = 1 / frequency of IMU + satu_acc: 3.0 # the saturation value of IMU's acceleration. not related to the units + satu_gyro: 35 # the saturation value of IMU's angular velocity. not related to the units + acc_norm: 1.0 # 1.0 for g as unit, 9.81 for m/s^2 as unit of the IMU's acceleration + lidar_meas_cov: 0.001 # 0.001; 0.01 + acc_cov_output: 500 + gyr_cov_output: 1000 + b_acc_cov: 0.0001 + b_gyr_cov: 0.0001 + imu_meas_acc_cov: 0.1 #0.1 # 0.1 + imu_meas_omg_cov: 0.1 #0.01 # 0.1 + gyr_cov_input: 0.01 # for IMU as input model + acc_cov_input: 0.1 # for IMU as input model + plane_thr: 0.1 # 0.05, the threshold for plane criteria, the smaller, the flatter a plane + match_s: 81 + fov_degree: 360 + det_range: 450.0 + gravity_align: true # true to align the z axis of world frame with the direction of gravity, and the gravity direction should be specified below + gravity: [0.0, 0.0, -9.810] # [0.0, 9.810, 0.0] # gravity to be aligned + gravity_init: [0.0, 0.0, -9.810] # [0.0, 9.810, 0.0] # # preknown gravity in the first IMU body frame, use when imu_en is false or start from a non-stationary state + extrinsic_T: [ 0.04165, 0.02326, -0.0284 ] + extrinsic_R: [ 1, 0, 0, + 0, 1, 0, + 0, 0, 1 ] + +odometry: + publish_odometry_without_downsample: false + +publish: + path_en: true # false: close the path output + scan_publish_en: true # false: close all the point cloud output + scan_bodyframe_pub_en: false # true: output the point cloud scans in IMU-body-frame + +pcd_save: + pcd_save_en: false + interval: -1 # how many LiDAR frames saved in each pcd file; + # -1 : all frames will be saved in ONE pcd file, may lead to memory crash when having too much frames. \ No newline at end of file diff --git a/src/Point-LIO/config/horizon.yaml b/src/Point-LIO/config/horizon.yaml new file mode 100755 index 0000000..070bf90 --- /dev/null +++ b/src/Point-LIO/config/horizon.yaml @@ -0,0 +1,57 @@ +common: + lid_topic: "/livox/lidar" + imu_topic: "/livox/imu" + con_frame: false # true: if you need to combine several LiDAR frames into one + con_frame_num: 1 # the number of frames combined + cut_frame: false # true: if you need to cut one LiDAR frame into several subframes + cut_frame_time_interval: 0.1 # should be integral fraction of 1 / LiDAR frequency + time_lag_imu_to_lidar: 0.0 # Time offset between LiDAR and IMU calibrated by other algorithms, e.g., LI-Init (find in Readme), + # the timesample of IMU is transferred from the current timeline to LiDAR's timeline by subtracting this value + +preprocess: + lidar_type: 1 + scan_line: 6 + timestamp_unit: 1 # the unit of time/t field in the PointCloud2 rostopic: 0-second, 1-milisecond, 2-microsecond, 3-nanosecond. + blind: 4.0 + +mapping: + imu_en: true + start_in_aggressive_motion: false # if true, a preknown gravity should be provided in following gravity_init + extrinsic_est_en: false # for aggressive motion, set this variable false + imu_time_inte: 0.005 # = 1 / frequency of IMU + satu_acc: 3.0 # the saturation value of IMU's acceleration. not related to the units + satu_gyro: 35 # the saturation value of IMU's angular velocity. not related to the units + acc_norm: 1.0 # 1.0 for g as unit, 9.81 for m/s^2 as unit of the IMU's acceleration + lidar_meas_cov: 0.01 # 0.001 + acc_cov_output: 500 + gyr_cov_output: 1000 + b_acc_cov: 0.0001 + b_gyr_cov: 0.0001 + imu_meas_acc_cov: 0.01 #0.1 # 2 + imu_meas_omg_cov: 0.01 #0.1 # 2 + gyr_cov_input: 0.01 # for IMU as input model + acc_cov_input: 0.1 # for IMU as input model + plane_thr: 0.1 # 0.05, the threshold for plane criteria, the smaller, the flatter a plane + match_s: 81 + fov_degree: 100 + det_range: 260.0 + gravity_align: true # true to align the z axis of world frame with the direction of gravity, and the gravity direction should be specified below + gravity: [0.0, 0.0, -9.810] # [0.0, 9.810, 0.0] # gravity to be aligned + gravity_init: [0.0, 0.0, -9.810] # [0.0, 9.810, 0.0] # # preknown gravity in the first IMU body frame, use when imu_en is false or start from a non-stationary state + extrinsic_T: [ 0.05512, 0.02226, -0.0297 ] + extrinsic_R: [ 1, 0, 0, + 0, 1, 0, + 0, 0, 1 ] + +odometry: + publish_odometry_without_downsample: false + +publish: + path_en: true # false: close the path output + scan_publish_en: true # false: close all the point cloud output + scan_bodyframe_pub_en: false # true: output the point cloud scans in IMU-body-frame + +pcd_save: + pcd_save_en: false + interval: -1 # how many LiDAR frames saved in each pcd file; + # -1 : all frames will be saved in ONE pcd file, may lead to memory crash when having too much frames. \ No newline at end of file diff --git a/src/Point-LIO/config/mid360.yaml b/src/Point-LIO/config/mid360.yaml new file mode 100644 index 0000000..1375f31 --- /dev/null +++ b/src/Point-LIO/config/mid360.yaml @@ -0,0 +1,57 @@ +common: + lid_topic: "/livox/lidar" + imu_topic: "/livox/imu" + con_frame: false # true: if you need to combine several LiDAR frames into one + con_frame_num: 1 # the number of frames combined + cut_frame: false # true: if you need to cut one LiDAR frame into several subframes + cut_frame_time_interval: 0.1 # should be integral fraction of 1 / LiDAR frequency + time_lag_imu_to_lidar: 0.0 # Time offset between LiDAR and IMU calibrated by other algorithms, e.g., LI-Init (find in Readme) + # the timesample of IMU is transferred from the current timeline to LiDAR's timeline by subtracting this value + +preprocess: + lidar_type: 1 + scan_line: 4 + timestamp_unit: 1 # the unit of time/t field in the PointCloud2 rostopic: 0-second, 1-milisecond, 2-microsecond, 3-nanosecond. + blind: 0.5 + +mapping: + imu_en: true + start_in_aggressive_motion: false # if true, a preknown gravity should be provided in following gravity_init + extrinsic_est_en: false # for aggressive motion, set this variable false + imu_time_inte: 0.005 # = 1 / frequency of IMU + satu_acc: 3.0 # the saturation value of IMU's acceleration. not related to the units + satu_gyro: 35 # the saturation value of IMU's angular velocity. not related to the units + acc_norm: 1.0 # 1.0 for g as unit, 9.81 for m/s^2 as unit of the IMU's acceleration + lidar_meas_cov: 0.001 # 0.001; 0.01 + acc_cov_output: 500 + gyr_cov_output: 1000 + b_acc_cov: 0.0001 + b_gyr_cov: 0.0001 + imu_meas_acc_cov: 0.1 #0.1 # 0.1 + imu_meas_omg_cov: 0.1 #0.01 # 0.1 + gyr_cov_input: 0.01 # for IMU as input model + acc_cov_input: 0.1 # for IMU as input model + plane_thr: 0.1 # 0.05, the threshold for plane criteria, the smaller, the flatter a plane + match_s: 81 + fov_degree: 360 + det_range: 100 + gravity_align: true # true to align the z axis of world frame with the direction of gravity, and the gravity direction should be specified below + gravity: [0.0, 0.0, -9.810] # [0.0, 9.810, 0.0] # gravity to be aligned + gravity_init: [0.0, 0.0, -9.810] # [0.0, 9.810, 0.0] # # preknown gravity in the first IMU body frame, use when imu_en is false or start from a non-stationary state + extrinsic_T: [ -0.011, -0.02329, 0.04412 ] + extrinsic_R: [ 1, 0, 0, + 0, 1, 0, + 0, 0, 1 ] + +odometry: + publish_odometry_without_downsample: false + +publish: + path_en: true # false: close the path output + scan_publish_en: true # false: close all the point cloud output + scan_bodyframe_pub_en: false # true: output the point cloud scans in IMU-body-frame + +pcd_save: + pcd_save_en: true + interval: -1 # how many LiDAR frames saved in each pcd file; + # -1 : all frames will be saved in ONE pcd file, may lead to memory crash when having too much frames. \ No newline at end of file diff --git a/src/Point-LIO/config/ouster64.yaml b/src/Point-LIO/config/ouster64.yaml new file mode 100755 index 0000000..2ba04b4 --- /dev/null +++ b/src/Point-LIO/config/ouster64.yaml @@ -0,0 +1,57 @@ +common: + lid_topic: "/os_cloud_node/points" + imu_topic: "/os_cloud_node/imu" + con_frame: false # true: if you need to combine several LiDAR frames into one + con_frame_num: 1 # the number of frames combined + cut_frame: false # true: if you need to cut one LiDAR frame into several subframes + cut_frame_time_interval: 0.1 # should be integral fraction of 1 / LiDAR frequency + time_lag_imu_to_lidar: 0.0 # Time offset between LiDAR and IMU calibrated by other algorithms, e.g., LI-Init (find in Readme) + # the timesample of IMU is transferred from the current timeline to LiDAR's timeline by subtracting this value + +preprocess: + lidar_type: 3 # 2 #velodyne # 1 Livox Avia LiDAR + scan_line: 32 # 32 #velodyne 6 avia + timestamp_unit: 3 # the unit of time/t field in the PointCloud2 rostopic: 0-second, 1-milisecond, 2-microsecond, 3-nanosecond. + blind: 0.20 + +mapping: + imu_en: true + start_in_aggressive_motion: false # if true, a preknown gravity should be provided in following gravity_init + extrinsic_est_en: false # for aggressive motion, set this variable false + imu_time_inte: 0.01 # = 1 / frequency of IMU + satu_acc: 30.0 # the saturation value of IMU's acceleration. not related to the units + satu_gyro: 35 # the saturation value of IMU's angular velocity. not related to the units + acc_norm: 9.81 # 1.0 for g as unit, 9.81 for m/s^2 as unit of the IMU's acceleration + lidar_meas_cov: 0.1 # 0.01 + acc_cov_output: 500 + gyr_cov_output: 1000 + b_acc_cov: 0.0001 + b_gyr_cov: 0.0001 + imu_meas_acc_cov: 0.1 #0.1 # 2 + imu_meas_omg_cov: 0.1 #0.1 # 2 + gyr_cov_input: 0.01 # for IMU as input model + acc_cov_input: 0.1 # for IMU as input model + plane_thr: 0.1 # 0.05, the threshold for plane criteria, the smaller, the flatter a plane + match_s: 81 + fov_degree: 180 + det_range: 150.0 + gravity_align: true # true to align the z axis of world frame with the direction of gravity, and the gravity direction should be specified below + gravity: [0.0, 0.0, -9.810] # [0.0, 9.810, 0.0] # gravity to be aligned + gravity_init: [0.0, 0.0, -9.810] # [0.0, 9.810, 0.0] # # preknown gravity in the first IMU body frame, use when imu_en is false or start from a non-stationary state + extrinsic_T: [0.0, 0.0, 0.0] + extrinsic_R: [ 1, 0, 0, + 0, 1, 0, + 0, 0, 1 ] + +odometry: + publish_odometry_without_downsample: false + +publish: + path_en: true # false: close the path output + scan_publish_en: true # false: close all the point cloud output + scan_bodyframe_pub_en: false # true: output the point cloud scans in IMU-body-frame + +pcd_save: + pcd_save_en: false + interval: -1 # how many LiDAR frames saved in each pcd file; + # -1 : all frames will be saved in ONE pcd file, may lead to memory crash when having too much frames. \ No newline at end of file diff --git a/src/Point-LIO/config/velody16.yaml b/src/Point-LIO/config/velody16.yaml new file mode 100755 index 0000000..ed59f00 --- /dev/null +++ b/src/Point-LIO/config/velody16.yaml @@ -0,0 +1,63 @@ +common: + lid_topic: "/velodyne_points" + imu_topic: "/imu/data" + con_frame: false # true: if you need to combine several LiDAR frames into one + con_frame_num: 1 # the number of frames combined + cut_frame: false # true: if you need to cut one LiDAR frame into several subframes + cut_frame_time_interval: 0.1 # should be integral fraction of 1 / LiDAR frequency + time_lag_imu_to_lidar: 0.0 # Time offset between LiDAR and IMU calibrated by other algorithms, e.g., LI-Init (find in Readme) + # the timesample of IMU is transferred from the current timeline to LiDAR's timeline by subtracting this value + +preprocess: + lidar_type: 2 + scan_line: 32 + timestamp_unit: 2 # the unit of time/t field in the PointCloud2 rostopic: 0-second, 1-milisecond, 2-microsecond, 3-nanosecond. + blind: 2.0 + +mapping: + imu_en: true + start_in_aggressive_motion: false # if true, a preknown gravity should be provided in following gravity_init + extrinsic_est_en: false # for aggressive motion, set this variable false + imu_time_inte: 0.01 # = 1 / frequency of IMU + satu_acc: 30.0 # the saturation value of IMU's acceleration. not related to the units + satu_gyro: 35 # the saturation value of IMU's angular velocity. not related to the units + acc_norm: 9.81 # 1.0 for g as unit, 9.81 for m/s^2 as unit of the IMU's acceleration + lidar_meas_cov: 0.01 # 0.001 + acc_cov_output: 500 + gyr_cov_output: 1000 + b_acc_cov: 0.0001 + b_gyr_cov: 0.0001 + imu_meas_acc_cov: 0.1 #0.1 # 2 + imu_meas_omg_cov: 0.1 #0.1 # 2 + gyr_cov_input: 0.01 # for IMU as input model + acc_cov_input: 0.1 # for IMU as input model + plane_thr: 0.1 # 0.05, the threshold for plane criteria, the smaller, the flatter a plane + match_s: 81 + fov_degree: 180 + det_range: 100.0 + gravity_align: true # true to align the z axis of world frame with the direction of gravity, and the gravity direction should be specified below + gravity: [0.0, 0.0, -9.810] # [0.0, 9.810, 0.0] # gravity to be aligned + gravity_init: [0.0, 0.0, -9.810] # [0.0, 9.810, 0.0] # # preknown gravity in the first IMU body frame, use when imu_en is false or start from a non-stationary state + extrinsic_T: [ 0, 0, 0.28] # ulhk # [-0.5, 1.4, 1.5] # utbm + # extrinsic_R: [ 0, 1, 0, + # -1, 0, 0, + # 0, 0, 1 ] # ulhk 5 6 + # extrinsic_R: [ 0, -1, 0, + # 1, 0, 0, + # 0, 0, 1 ] # utbm 1, 2 + extrinsic_R: [ 1, 0, 0, + 0, 1, 0, + 0, 0, 1 ] # ulhk 4 utbm 3 + +odometry: + publish_odometry_without_downsample: false + +publish: + path_en: true # false: close the path output + scan_publish_en: true # false: close all the point cloud output + scan_bodyframe_pub_en: false # true: output the point cloud scans in IMU-body-frame + +pcd_save: + pcd_save_en: false + interval: -1 # how many LiDAR frames saved in each pcd file; + # -1 : all frames will be saved in ONE pcd file, may lead to memory crash when having too much frames. \ No newline at end of file diff --git a/src/Point-LIO/include/.vscode/c_cpp_properties.json b/src/Point-LIO/include/.vscode/c_cpp_properties.json new file mode 100755 index 0000000..d7fe77d --- /dev/null +++ b/src/Point-LIO/include/.vscode/c_cpp_properties.json @@ -0,0 +1,18 @@ +{ + "configurations": [ + { + "browse": { + "databaseFilename": "", + "limitSymbolsToIncludedHeaders": true + }, + "includePath": [ + "/home/ecstasy/catkin_ws/devel/include/**", + "/opt/ros/melodic/include/**", + "/home/ecstasy/catkin_ws/src/FAST_LIO/include/**", + "/usr/include/**" + ], + "name": "ROS" + } + ], + "version": 4 +} \ No newline at end of file diff --git a/src/Point-LIO/include/.vscode/settings.json b/src/Point-LIO/include/.vscode/settings.json new file mode 100755 index 0000000..4a9ea37 --- /dev/null +++ b/src/Point-LIO/include/.vscode/settings.json @@ -0,0 +1,6 @@ +{ + "python.autoComplete.extraPaths": [ + "/home/ecstasy/catkin_ws/devel/lib/python2.7/dist-packages", + "/opt/ros/melodic/lib/python2.7/dist-packages" + ] +} \ No newline at end of file diff --git a/src/Point-LIO/include/FOV_Checker/FOV_Checker.cpp b/src/Point-LIO/include/FOV_Checker/FOV_Checker.cpp new file mode 100755 index 0000000..69032bf --- /dev/null +++ b/src/Point-LIO/include/FOV_Checker/FOV_Checker.cpp @@ -0,0 +1,472 @@ +#include "FOV_Checker.h" + +FOV_Checker::FOV_Checker(){ + // fp = fopen("/home/ecstasy/catkin_ws/fov_data.csv","w"); + // fprintf(fp,"cur_pose_x,cur_pose_y,cur_pose_z,axis_x,axis_y,axis_z,theta,depth\n"); + // fclose(fp); +} + +FOV_Checker::~FOV_Checker(){ + +} + +void FOV_Checker::Set_Env(BoxPointType env_param){ + env = env_param; +} + +void FOV_Checker::Set_BoxLength(double box_len_param){ + box_length = box_len_param; +} + +void round_v3d(Eigen::Vector3d &vec, int decimal){ + double tmp; + int t; + for (int i = 0; i < 3; i++){ + t = pow(10,decimal); + tmp = round(vec(i)*t); + vec(i) = tmp/t; + } + return; +} + +void FOV_Checker::check_fov(Eigen::Vector3d cur_pose, Eigen::Vector3d axis, double theta, double depth, vector &boxes){ + round_v3d(cur_pose,4); + round_v3d(axis,3); + axis = axis/axis.norm(); + // fp = fopen("/home/ecstasy/catkin_ws/fov_data.csv","a"); + // fprintf(fp,"%f,%f,%f,%f,%f,%f,%0.4f,%0.1f,",cur_pose(0),cur_pose(1),cur_pose(2),axis(0),axis(1),axis(2),theta,depth); + // fclose(fp); + // cout << "cur_pose: " << cur_pose.transpose() << endl; + // cout<< "axis: " << axis.transpose() << endl; + // cout<< "theta: " << theta << " depth: " << depth << endl; + // cout<< "env: " << env.vertex_min[0] << " " << env.vertex_max[0] << endl; + double axis_angle[6], min_angle, gap, plane_u_min, plane_u_max; + Eigen::Vector3d plane_w, plane_u, plane_v, center_point, start_point, box_p; + Eigen::Vector3d box_p_min, box_p_max; + int i, j, k, index, maxn, start_i, max_uN, max_vN, max_ulogN, u_min, u_max; + bool flag = false, box_found = false; + boxes.clear(); + BoxPointType box; + axis_angle[0] = acos(axis(0)); + axis_angle[1] = acos(axis(1)); + axis_angle[2] = acos(axis(2)); + axis_angle[3] = acos(-axis(0)); + axis_angle[4] = acos(-axis(1)); + axis_angle[5] = acos(-axis(2)); + index = 1; + min_angle = axis_angle[0]; + for (i=1;i<6;i++){ + if (axis_angle[i]=0){ + box_p = box_p_min + plane_u * box_length * (u_min + pow(2,k)) + plane_v * box_length + plane_w * box_length; + box.vertex_min[0] = box_p_min(0); + box.vertex_min[1] = box_p_min(1); + box.vertex_min[2] = box_p_min(2); + box.vertex_max[0] = box_p(0); + box.vertex_max[1] = box_p(1); + box.vertex_max[2] = box_p(2); + if (!check_box(cur_pose, axis, theta, depth, box)) u_min = u_min + pow(2,k); + k = k-1; + } + k = max_ulogN; + u_max = 0; + while (k>=0){ + box_p = box_p_max - plane_u * box_length * (u_max + pow(2,k)) - plane_v * box_length - plane_w * box_length; + box.vertex_min[0] = box_p(0); + box.vertex_min[1] = box_p(1); + box.vertex_min[2] = box_p(2); + box.vertex_max[0] = box_p_max(0); + box.vertex_max[1] = box_p_max(1); + box.vertex_max[2] = box_p_max(2); + if (!check_box(cur_pose, axis, theta, depth, box)) u_max = u_max + pow(2,k); + + k = k-1; + } + u_max = max(0, max_uN - u_max - 1); + box_found = false; + //printf("---- u_min -> u_max: %d->%d\n",u_min,u_max); + for (k = u_min; k <= u_max; k++){ + box_p = box_p_min + plane_u * box_length * k; + box.vertex_min[0] = box_p(0); + box.vertex_min[1] = box_p(1); + box.vertex_min[2] = box_p(2); + box.vertex_max[0] = box_p(0) + box_length; + box.vertex_max[1] = box_p(1) + box_length; + box.vertex_max[2] = box_p(2) + box_length; + if (check_box_in_env(box)){ + //printf("---- FOUND: (%0.3f,%0.3f,%0.3f),(%0.3f,%0.3f,%0.3f)\n",box.vertex_min[0],box.vertex_min[1],box.vertex_min[2],box.vertex_max[0],box.vertex_max[1],box.vertex_max[2]); + box_found = true; + boxes.push_back(box); + } + } + if (box_found) { + flag = true; + } else { + if (j>1) break; + } + } + for (j = 1; j <= max_vN; j++){ + k = max_ulogN; + u_min = 0; + box_p_min = start_point.cwiseProduct(plane_w + plane_v) + plane_u * plane_u_min - plane_v * box_length * j; + box_p_max = plane_u * plane_u_max + start_point.cwiseProduct(plane_w + plane_v) - plane_v * box_length * (j-1) + plane_w * box_length; + //printf("---- DOWNSIDE (%0.3f,%0.3f,%0.3f),(%0.3f,%0.3f,%0.3f)\n",box_p_min[0],box_p_min[1],box_p_min[2],box_p_max[0],box_p_max[1],box_p_max[2]); + + while (k>=0){ + box_p = box_p_min + plane_u * box_length * (u_min + pow(2,k)) + plane_v * box_length + plane_w * box_length; + box.vertex_min[0] = box_p_min(0); + box.vertex_min[1] = box_p_min(1); + box.vertex_min[2] = box_p_min(2); + box.vertex_max[0] = box_p(0); + box.vertex_max[1] = box_p(1); + box.vertex_max[2] = box_p(2); + if (!check_box(cur_pose, axis, theta, depth, box)) u_min = u_min + pow(2,k); + k = k-1; + } + k = max_ulogN; + u_max = 0; + while (k>=0){ + box_p = box_p_max - plane_u * box_length * (u_max + pow(2,k)) - plane_v * box_length - plane_w * box_length; + box.vertex_min[0] = box_p(0); + box.vertex_min[1] = box_p(1); + box.vertex_min[2] = box_p(2); + box.vertex_max[0] = box_p_max(0); + box.vertex_max[1] = box_p_max(1); + box.vertex_max[2] = box_p_max(2); + if (!check_box(cur_pose, axis, theta, depth, box)) { + u_max = u_max + pow(2,k); + // printf("-------- Not Included: (%0.3f,%0.3f,%0.3f),(%0.3f,%0.3f,%0.3f)\n",box.vertex_min[0],box.vertex_min[1],box.vertex_min[2],box.vertex_max[0],box.vertex_max[1],box.vertex_max[2]); + } + + k = k-1; + } + u_max = max(0, max_uN - u_max - 1); + //printf("---- u_min -> u_max: %d->%d\n",u_min,u_max); + box_found = 0; + for (k = u_min; k <= u_max; k++){ + box_p = box_p_min + plane_u * box_length * k; + box.vertex_min[0] = box_p(0); + box.vertex_min[1] = box_p(1); + box.vertex_min[2] = box_p(2); + box.vertex_max[0] = box_p(0) + box_length; + box.vertex_max[1] = box_p(1) + box_length; + box.vertex_max[2] = box_p(2) + box_length; + if (check_box_in_env(box)){ + //printf("---- FOUND: (%0.3f,%0.3f,%0.3f),(%0.3f,%0.3f,%0.3f)\n",box.vertex_min[0],box.vertex_min[1],box.vertex_min[2],box.vertex_max[0],box.vertex_max[1],box.vertex_max[2]); + box_found = 1; + boxes.push_back(box); + } + } + if (box_found) { + flag = true; + } else { + if (j>1) break; + } + } + if (!flag && i>0) break; + } +} + +bool FOV_Checker::check_box(Eigen::Vector3d cur_pose, Eigen::Vector3d axis, double theta, double depth, const BoxPointType box){ + Eigen::Vector3d vertex[8]; + bool s; + vertex[0] = Eigen::Vector3d(box.vertex_min[0], box.vertex_min[1], box.vertex_min[2]); + vertex[1] = Eigen::Vector3d(box.vertex_min[0], box.vertex_min[1], box.vertex_max[2]); + vertex[2] = Eigen::Vector3d(box.vertex_min[0], box.vertex_max[1], box.vertex_min[2]); + vertex[3] = Eigen::Vector3d(box.vertex_min[0], box.vertex_max[1], box.vertex_max[2]); + vertex[4] = Eigen::Vector3d(box.vertex_max[0], box.vertex_min[1], box.vertex_min[2]); + vertex[5] = Eigen::Vector3d(box.vertex_max[0], box.vertex_min[1], box.vertex_max[2]); + vertex[6] = Eigen::Vector3d(box.vertex_max[0], box.vertex_max[1], box.vertex_min[2]); + vertex[7] = Eigen::Vector3d(box.vertex_max[0], box.vertex_max[1], box.vertex_max[2]); + for (int i = 0; i < 8; i++){ + if (check_point(cur_pose, axis, theta, depth, vertex[i])){ + return true; + } + } + Eigen::Vector3d center_point = (vertex[7]+vertex[0])/2.0; + if (check_point(cur_pose, axis, theta, depth, center_point)){ + return true; + } + PlaneType plane[6]; + plane[0].p[0] = vertex[0]; + plane[0].p[1] = vertex[2]; + plane[0].p[2] = vertex[1]; + plane[0].p[3] = vertex[3]; + + plane[1].p[0] = vertex[0]; + plane[1].p[1] = vertex[4]; + plane[1].p[2] = vertex[2]; + plane[1].p[3] = vertex[6]; + + plane[2].p[0] = vertex[0]; + plane[2].p[1] = vertex[4]; + plane[2].p[2] = vertex[1]; + plane[2].p[3] = vertex[5]; + + plane[3].p[0] = vertex[4]; + plane[3].p[1] = vertex[6]; + plane[3].p[2] = vertex[5]; + plane[3].p[3] = vertex[7]; + + plane[4].p[0] = vertex[2]; + plane[4].p[1] = vertex[6]; + plane[4].p[2] = vertex[3]; + plane[4].p[3] = vertex[7]; + + plane[5].p[0] = vertex[1]; + plane[5].p[1] = vertex[5]; + plane[5].p[2] = vertex[3]; + plane[5].p[3] = vertex[7]; + if (check_surface(cur_pose, axis, theta, depth, plane[0]) || check_surface(cur_pose, axis, theta, depth, plane[1]) || check_surface(cur_pose, axis, theta, depth, plane[2]) || check_surface(cur_pose, axis, theta, depth, plane[3]) || check_surface(cur_pose, axis, theta, depth, plane[4]) || check_surface(cur_pose, axis, theta, depth, plane[5])) + s = 1; + else + s = 0; + return s; +} + +bool FOV_Checker::check_surface(Eigen::Vector3d cur_pose, Eigen::Vector3d axis, double theta, double depth, PlaneType plane){ + Eigen::Vector3d plane_p, plane_u, plane_v, plane_w, pc, p, vec; + bool s; + double t, vec_dot_u, vec_dot_v; + plane_p = plane.p[0]; + plane_u = plane.p[1] - plane_p; + plane_v = plane.p[2] - plane_p; + if (check_line(cur_pose, axis, theta, depth, plane_p, plane_u) || check_line(cur_pose, axis, theta, depth, plane_p, plane_v) || check_line(cur_pose, axis, theta, depth, plane_p + plane_u, plane_v) || check_line(cur_pose, axis, theta, depth, plane_p + plane_v, plane_u)){ + s = 1; + return s; + } + pc = plane_p + (plane.p[3]-plane.p[0])/2; + if (check_point(cur_pose, axis, theta, depth, pc)){ + s = 1; + return s; + } + plane_w = plane_u.cross(plane_v); + p = plane_p - cur_pose; + t = (p.dot(plane_w))/(axis.dot(plane_w)); + vec = cur_pose + t * axis - plane_p; + vec_dot_u = vec.dot(plane_u)/plane_u.norm(); + vec_dot_v = vec.dot(plane_v)/plane_v.norm(); + if (t>=-eps_value && t<=depth && vec_dot_u>=-eps_value && vec_dot_u<=plane_u.norm() && vec_dot_v>=-eps_value && vec_dot_v <= plane_v.norm()) + s = 1; + else + s = 0; + return s; +} + +bool FOV_Checker::check_line(Eigen::Vector3d cur_pose, Eigen::Vector3d axis, double theta, double depth, Eigen::Vector3d line_p, Eigen::Vector3d line_vec){ + Eigen::Vector3d p, vec_1, vec_2; + double xl, yl, zl, xn, yn, zn, dot_1, dot_2, ln, pn, pl, l2, p2; + double A, B, C, delta, t1, t2; + bool s; + p = line_p - cur_pose; + xl = line_vec(0); yl = line_vec(1); zl = line_vec(2); + xn = axis(0); yn = axis(1); zn = axis(2); + vec_1 = line_p - cur_pose; + vec_2 = line_p + line_vec - cur_pose; + dot_1 = vec_1.dot(axis); + dot_2 = vec_2.dot(axis); + //printf("xl yl zl: %0.4f, %0.4f, %0.4f\n", xl, yl, zl); + //printf("xn yn zn: %0.4f, %0.4f, %0.4f\n", xn, yn, zn); + //printf("dot_1, dot_2, %0.4f, %0.4f\n",dot_1, dot_2); + if ((dot_1<0 && dot_2<0) || (dot_1>depth && dot_2>depth)){ + s = false; + return s; + } + ln = xl*xn+yl*yn+zl*zn; + pn = p(0)*xn+p(1)*yn+p(2)*zn; + pl = p(0)*xl+p(1)*yl+p(2)*zl; + l2 = xl*xl+yl*yl+zl*zl; + p2 = p.norm()*p.norm(); + //printf("ln: %0.4f\n",ln); + //printf("pn:%0.4f\n",pn); + //printf("pl:%0.4f\n",pl); + //printf("l2:%0.4f\n",l2); + //printf("p2:%0.4f\n",p2); + //printf("theta, cos(theta):%0.4f %0.4f\n",theta,cos(theta)); + A = ln * ln - l2 * cos(theta) * cos(theta); + B = 2 * pn * ln - 2 * cos(theta) * cos(theta)*pl; + C = pn * pn - p2 * cos(theta) * cos(theta); + //printf("A:%0.4f, B:%0.4f, C:%0.4f\n", A,B,C); + if (!(fabs(A)<=eps_value)){ + delta = B*B - 4*A*C; + //printf("delta: %0.4f\n",delta); + if (delta <= eps_value){ + if (A < -eps_value){ + s = false; + return s; + } + else{ + s = true; + return s; + } + } else { + double sqrt_delta = sqrt(delta); + t1 = (-B - sqrt_delta)/(2*A); + t2 = (-B + sqrt_delta)/(2*A); + if (t1>t2) swap(t1,t2); + //printf("t1,t2: %0.4f,%0.4f\n",t1,t2); + // printf("%d\n",check_point(cur_pose, axis, theta, depth, line_p + line_vec * t1)); + if ((t1>=-eps_value && t1<=1+eps_value) && check_point(cur_pose, axis, theta, depth, line_p + line_vec * t1)){ + s = true; + return s; + } + // printf("%d\n",check_point(cur_pose, axis, theta, depth, line_p + line_vec * t2)); + if ((t2>=-eps_value && t2<=1+eps_value) && check_point(cur_pose, axis, theta, depth, line_p + line_vec * t2)){ + s = true; + return s; + } + if (A>-eps_value && (t21-eps_value)){ + s = true; + return s; + } + if (A1-eps_value){ + s = true; + return s; + } + s = false; + } + } else{ + if (!(fabs(B)<=eps_value)){ + s = (B>-eps_value && -C/B<=1+eps_value) || (B=-eps_value); + return s; + } + else { + s = C>=-eps_value; + return s; + } + } + return false; +} + +bool FOV_Checker::check_point(Eigen::Vector3d cur_pose, Eigen::Vector3d axis, double theta, double depth, Eigen::Vector3d point){ + Eigen::Vector3d vec; + double proj_len; + bool s; + vec = point-cur_pose; + if (vec.transpose()*vec < 0.4 * box_length * box_length){ + return true; + } + proj_len = vec.dot(axis); + if (proj_len > depth){ + s = false; + return s; + } + //printf("acos: %0.4f\n",acos(proj_len/vec.norm())); + if (fabs(vec.norm()) <= 1e-4 || acos(proj_len/vec.norm()) <= theta + 0.0175) + s = true; + else + s = false; + return s; +} + +bool FOV_Checker::check_box_in_env(BoxPointType box){ + if (box.vertex_min[0] >= env.vertex_min[0]-eps_value && box.vertex_min[1] >= env.vertex_min[1]-eps_value && box.vertex_min[2] >= env.vertex_min[2]-eps_value && box.vertex_max[0]<= env.vertex_max[0]+eps_value && box.vertex_max[1]<= env.vertex_max[1]+eps_value && box.vertex_max[2]<= env.vertex_max[2]+eps_value){ + return true; + } else { + return false; + } +} + diff --git a/src/Point-LIO/include/FOV_Checker/FOV_Checker.h b/src/Point-LIO/include/FOV_Checker/FOV_Checker.h new file mode 100755 index 0000000..e7db390 --- /dev/null +++ b/src/Point-LIO/include/FOV_Checker/FOV_Checker.h @@ -0,0 +1,33 @@ +// Include Files +#pragma once +#include +#include +#include "ikd-Tree/ikd_Tree.h" +#include +#include + +#define eps_value 1e-6 + +struct PlaneType{ + Eigen::Vector3d p[4]; +}; + +class FOV_Checker{ +public: + FOV_Checker(); + ~FOV_Checker(); + void Set_Env(BoxPointType env_param); + void Set_BoxLength(double box_len_param); + void check_fov(Eigen::Vector3d cur_pose, Eigen::Vector3d axis, double theta, double depth, vector &boxes); + bool check_box(Eigen::Vector3d cur_pose, Eigen::Vector3d axis, double theta, double depth, const BoxPointType box); + bool check_line(Eigen::Vector3d cur_pose, Eigen::Vector3d axis, double theta, double depth, Eigen::Vector3d line_p, Eigen::Vector3d line_vec); + bool check_surface(Eigen::Vector3d cur_pose, Eigen::Vector3d axis, double theta, double depth, PlaneType plane); + bool check_point(Eigen::Vector3d cur_pose, Eigen::Vector3d axis, double theta, double depth, Eigen::Vector3d point); + bool check_box_in_env(BoxPointType box); +private: + BoxPointType env; + double box_length; + FILE *fp; + +}; + diff --git a/src/Point-LIO/include/IKFoM/.gitignore b/src/Point-LIO/include/IKFoM/.gitignore new file mode 100644 index 0000000..259148f --- /dev/null +++ b/src/Point-LIO/include/IKFoM/.gitignore @@ -0,0 +1,32 @@ +# 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 diff --git a/src/Point-LIO/include/IKFoM/.vscode/settings.json b/src/Point-LIO/include/IKFoM/.vscode/settings.json new file mode 100644 index 0000000..d756850 --- /dev/null +++ b/src/Point-LIO/include/IKFoM/.vscode/settings.json @@ -0,0 +1,5 @@ +{ + "files.associations": { + "complex": "cpp" + } +} \ No newline at end of file diff --git a/src/Point-LIO/include/IKFoM/IKFoM_toolkit/.vscode/settings.json b/src/Point-LIO/include/IKFoM/IKFoM_toolkit/.vscode/settings.json new file mode 100755 index 0000000..b619d8a --- /dev/null +++ b/src/Point-LIO/include/IKFoM/IKFoM_toolkit/.vscode/settings.json @@ -0,0 +1,62 @@ +{ + "files.associations": { + "iosfwd": "cpp", + "core": "cpp", + "cctype": "cpp", + "clocale": "cpp", + "cmath": "cpp", + "cstdarg": "cpp", + "cstddef": "cpp", + "cstdio": "cpp", + "cstdlib": "cpp", + "cstring": "cpp", + "ctime": "cpp", + "cwchar": "cpp", + "cwctype": "cpp", + "array": "cpp", + "atomic": "cpp", + "strstream": "cpp", + "*.tcc": "cpp", + "bitset": "cpp", + "chrono": "cpp", + "complex": "cpp", + "cstdint": "cpp", + "deque": "cpp", + "list": "cpp", + "unordered_map": "cpp", + "vector": "cpp", + "exception": "cpp", + "fstream": "cpp", + "functional": "cpp", + "initializer_list": "cpp", + "iomanip": "cpp", + "iostream": "cpp", + "istream": "cpp", + "limits": "cpp", + "new": "cpp", + "ostream": "cpp", + "numeric": "cpp", + "ratio": "cpp", + "sstream": "cpp", + "stdexcept": "cpp", + "streambuf": "cpp", + "system_error": "cpp", + "thread": "cpp", + "cfenv": "cpp", + "cinttypes": "cpp", + "tuple": "cpp", + "type_traits": "cpp", + "utility": "cpp", + "typeinfo": "cpp", + "algorithm": "cpp", + "iterator": "cpp", + "map": "cpp", + "memory": "cpp", + "memory_resource": "cpp", + "optional": "cpp", + "random": "cpp", + "set": "cpp", + "string": "cpp", + "string_view": "cpp" + } +} \ No newline at end of file diff --git a/src/Point-LIO/include/IKFoM/IKFoM_toolkit/esekfom/.vscode/settings.json b/src/Point-LIO/include/IKFoM/IKFoM_toolkit/esekfom/.vscode/settings.json new file mode 100755 index 0000000..c807bd5 --- /dev/null +++ b/src/Point-LIO/include/IKFoM/IKFoM_toolkit/esekfom/.vscode/settings.json @@ -0,0 +1,5 @@ +{ + "files.associations": { + "core": "cpp" + } +} \ No newline at end of file diff --git a/src/Point-LIO/include/IKFoM/IKFoM_toolkit/esekfom/esekfom.hpp b/src/Point-LIO/include/IKFoM/IKFoM_toolkit/esekfom/esekfom.hpp new file mode 100755 index 0000000..235a8a4 --- /dev/null +++ b/src/Point-LIO/include/IKFoM/IKFoM_toolkit/esekfom/esekfom.hpp @@ -0,0 +1,390 @@ +/* + * Copyright (c) 2019--2023, The University of Hong Kong + * All rights reserved. + * + * Author: Dongjiao HE + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the Universitaet Bremen nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#ifndef ESEKFOM_EKF_HPP +#define ESEKFOM_EKF_HPP + + +#include +#include + +#include +#include +#include +#include +#include +#include + +#include "../mtk/types/vect.hpp" +#include "../mtk/types/SOn.hpp" +#include "../mtk/types/S2.hpp" +#include "../mtk/types/SEn.hpp" +#include "../mtk/startIdx.hpp" +#include "../mtk/build_manifold.hpp" +#include "util.hpp" + +namespace esekfom { + +using namespace Eigen; + +template +struct dyn_share_modified +{ + bool valid; + bool converge; + T M_Noise; + Eigen::Matrix z; + Eigen::Matrix h_x; + Eigen::Matrix z_IMU; + Eigen::Matrix R_IMU; + bool satu_check[6]; +}; + +template +class esekf{ + + typedef esekf self; + enum{ + n = state::DOF, m = state::DIM, l = measurement::DOF + }; + +public: + + typedef typename state::scalar scalar_type; + typedef Matrix cov; + typedef Matrix cov_; + typedef SparseMatrix spMt; + typedef Matrix vectorized_state; + typedef Matrix flatted_state; + typedef flatted_state processModel(state &, const input &); + typedef Eigen::Matrix processMatrix1(state &, const input &); + typedef Eigen::Matrix processMatrix2(state &, const input &); + typedef Eigen::Matrix processnoisecovariance; + + typedef void measurementModel_dyn_share_modified(state &, dyn_share_modified &); + typedef Eigen::Matrix measurementMatrix1(state &); + typedef Eigen::Matrix measurementMatrix1_dyn(state &); + typedef Eigen::Matrix measurementMatrix2(state &); + typedef Eigen::Matrix measurementMatrix2_dyn(state &); + typedef Eigen::Matrix measurementnoisecovariance; + typedef Eigen::Matrix measurementnoisecovariance_dyn; + + esekf(const state &x = state(), + const cov &P = cov::Identity()): x_(x), P_(P){}; + + void init_dyn_share_modified(processModel f_in, processMatrix1 f_x_in, measurementModel_dyn_share_modified h_dyn_share_in) + { + f = f_in; + f_x = f_x_in; + // f_w = f_w_in; + h_dyn_share_modified_1 = h_dyn_share_in; + maximum_iter = 1; + x_.build_S2_state(); + x_.build_SO3_state(); + x_.build_vect_state(); + x_.build_SEN_state(); + } + + void init_dyn_share_modified_2h(processModel f_in, processMatrix1 f_x_in, measurementModel_dyn_share_modified h_dyn_share_in1, measurementModel_dyn_share_modified h_dyn_share_in2) + { + f = f_in; + f_x = f_x_in; + // f_w = f_w_in; + h_dyn_share_modified_1 = h_dyn_share_in1; + h_dyn_share_modified_2 = h_dyn_share_in2; + maximum_iter = 1; + x_.build_S2_state(); + x_.build_SO3_state(); + x_.build_vect_state(); + x_.build_SEN_state(); + } + + // iterated error state EKF propogation + void predict(double &dt, processnoisecovariance &Q, const input &i_in, bool predict_state, bool prop_cov){ + if (predict_state) + { + flatted_state f_ = f(x_, i_in); + x_.oplus(f_, dt); + } + + if (prop_cov) + { + flatted_state f_ = f(x_, i_in); + // state x_before = x_; + + cov_ f_x_ = f_x(x_, i_in); + cov f_x_final; + F_x1 = cov::Identity(); + for (std::vector, int> >::iterator it = x_.vect_state.begin(); it != x_.vect_state.end(); it++) { + int idx = (*it).first.first; + int dim = (*it).first.second; + int dof = (*it).second; + for(int i = 0; i < n; i++){ + for(int j=0; j res_temp_SO3; + MTK::vect<3, scalar_type> seg_SO3; + for (std::vector >::iterator it = x_.SO3_state.begin(); it != x_.SO3_state.end(); it++) { + int idx = (*it).first; + int dim = (*it).second; + for(int i = 0; i < 3; i++){ + seg_SO3(i) = -1 * f_(dim + i) * dt; + } + // MTK::SO3 res; + // res.w() = MTK::exp(res.vec(), seg_SO3, scalar_type(1/2)); + F_x1.template block<3, 3>(idx, idx) = MTK::SO3::exp(seg_SO3); // res.normalized().toRotationMatrix(); + res_temp_SO3 = MTK::A_matrix(seg_SO3); + for(int i = 0; i < n; i++){ + f_x_final. template block<3, 1>(idx, i) = res_temp_SO3 * (f_x_. template block<3, 1>(dim, i)); + } + } + + F_x1 += f_x_final * dt; + P_ = F_x1 * P_ * (F_x1).transpose() + Q * (dt * dt); + } + } + + bool update_iterated_dyn_share_modified() { + dyn_share_modified dyn_share; + state x_propagated = x_; + int dof_Measurement; + double m_noise; + for(int i=0; i z = dyn_share.z; + // Matrix R = dyn_share.R; + Matrix h_x = dyn_share.h_x; + // Matrix h_v = dyn_share.h_v; + dof_Measurement = h_x.rows(); + m_noise = dyn_share.M_Noise; + // dof_Measurement_noise = dyn_share.R.rows(); + // vectorized_state dx, dx_new; + // x_.boxminus(dx, x_propagated); + // dx_new = dx; + // P_ = P_propagated; + + Matrix PHT; + Matrix HPHT; + Matrix K_; + // if(n > dof_Measurement) + { + PHT = P_. template block(0, 0) * h_x.transpose(); + HPHT = h_x * PHT.topRows(12); + for (int m = 0; m < dof_Measurement; m++) + { + HPHT(m, m) += m_noise; + } + K_= PHT*HPHT.inverse(); + } + Matrix dx_ = K_ * z; // - h) + (K_x - Matrix::Identity()) * dx_new; + // state x_before = x_; + + x_.boxplus(dx_); + dyn_share.converge = true; + + // L_ = P_; + // Matrix res_temp_SO3; + // MTK::vect<3, scalar_type> seg_SO3; + // for(typename std::vector >::iterator it = x_.SO3_state.begin(); it != x_.SO3_state.end(); it++) { + // int idx = (*it).first; + // for(int i = 0; i < 3; i++){ + // seg_SO3(i) = dx_(i + idx); + // } + // res_temp_SO3 = A_matrix(seg_SO3).transpose(); + // for(int i = 0; i < n; i++){ + // L_. template block<3, 1>(idx, i) = res_temp_SO3 * (P_. template block<3, 1>(idx, i)); + // } + // { + // for(int i = 0; i < dof_Measurement; i++){ + // K_. template block<3, 1>(idx, i) = res_temp_SO3 * (K_. template block<3, 1>(idx, i)); + // } + // } + // for(int i = 0; i < n; i++){ + // L_. template block<1, 3>(i, idx) = (L_. template block<1, 3>(i, idx)) * res_temp_SO3.transpose(); + // // P_. template block<1, 3>(i, idx) = (P_. template block<1, 3>(i, idx)) * res_temp_SO3.transpose(); + // } + // for(int i = 0; i < n; i++){ + // P_. template block<1, 3>(i, idx) = (P_. template block<1, 3>(i, idx)) * res_temp_SO3.transpose(); + // } + // } + // Matrix res_temp_S2; + // MTK::vect<2, scalar_type> seg_S2; + // for(typename std::vector >::iterator it = x_.S2_state.begin(); it != x_.S2_state.end(); it++) { + // int idx = (*it).first; + + // for(int i = 0; i < 2; i++){ + // seg_S2(i) = dx_(i + idx); + // } + + // Eigen::Matrix Nx; + // Eigen::Matrix Mx; + // x_.S2_Nx_yy(Nx, idx); + // x_propagated.S2_Mx(Mx, seg_S2, idx); + // res_temp_S2 = Nx * Mx; + + // for(int i = 0; i < n; i++){ + // L_. template block<2, 1>(idx, i) = res_temp_S2 * (P_. template block<2, 1>(idx, i)); + // } + + // { + // for(int i = 0; i < dof_Measurement; i++){ + // K_. template block<2, 1>(idx, i) = res_temp_S2 * (K_. template block<2, 1>(idx, i)); + // } + // } + // for(int i = 0; i < n; i++){ + // L_. template block<1, 2>(i, idx) = (L_. template block<1, 2>(i, idx)) * res_temp_S2.transpose(); + // } + // for(int i = 0; i < n; i++){ + // P_. template block<1, 2>(i, idx) = (P_. template block<1, 2>(i, idx)) * res_temp_S2.transpose(); + // } + // } + // if(n > dof_Measurement) + { + P_ = P_ - K_*h_x*P_. template block<12, n>(0, 0); + } + } + return true; + } + + void update_iterated_dyn_share_IMU() { + + dyn_share_modified dyn_share; + for(int i=0; i z = dyn_share.z_IMU; + + Matrix PHT; + Matrix HP; + Matrix HPHT; + PHT.setZero(); + HP.setZero(); + HPHT.setZero(); + for (int l_ = 0; l_ < 6; l_++) + { + if (!dyn_share.satu_check[l_]) + { + PHT.col(l_) = P_.col(15+l_) + P_.col(24+l_); + HP.row(l_) = P_.row(15+l_) + P_.row(24+l_); + } + } + for (int l_ = 0; l_ < 6; l_++) + { + if (!dyn_share.satu_check[l_]) + { + HPHT.col(l_) = HP.col(15+l_) + HP.col(24+l_); + } + HPHT(l_, l_) += dyn_share.R_IMU(l_); //, l); + } + Eigen::Matrix K = PHT * HPHT.inverse(); + + Matrix dx_ = K * z; + + P_ -= K * HP; + x_.boxplus(dx_); + } + return; + } + + void change_x(state &input_state) + { + x_ = input_state; + + if((!x_.vect_state.size())&&(!x_.SO3_state.size())&&(!x_.S2_state.size())&&(!x_.SEN_state.size())) + { + x_.build_S2_state(); + x_.build_SO3_state(); + x_.build_vect_state(); + x_.build_SEN_state(); + } + } + + void change_P(cov &input_cov) + { + P_ = input_cov; + } + + const state& get_x() const { + return x_; + } + const cov& get_P() const { + return P_; + } + state x_; +private: + measurement m_; + cov P_; + spMt l_; + spMt f_x_1; + spMt f_x_2; + cov F_x1 = cov::Identity(); + cov F_x2 = cov::Identity(); + cov L_ = cov::Identity(); + + processModel *f; + processMatrix1 *f_x; + processMatrix2 *f_w; + + measurementMatrix1 *h_x; + measurementMatrix2 *h_v; + + measurementMatrix1_dyn *h_x_dyn; + measurementMatrix2_dyn *h_v_dyn; + + measurementModel_dyn_share_modified *h_dyn_share_modified_1; + + measurementModel_dyn_share_modified *h_dyn_share_modified_2; + + int maximum_iter = 0; + scalar_type limit[n]; + +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW +}; + +} // namespace esekfom + +#endif // ESEKFOM_EKF_HPP diff --git a/src/Point-LIO/include/IKFoM/IKFoM_toolkit/esekfom/util.hpp b/src/Point-LIO/include/IKFoM/IKFoM_toolkit/esekfom/util.hpp new file mode 100755 index 0000000..ab39fc4 --- /dev/null +++ b/src/Point-LIO/include/IKFoM/IKFoM_toolkit/esekfom/util.hpp @@ -0,0 +1,82 @@ +/* + * Copyright (c) 2019--2023, The University of Hong Kong + * All rights reserved. + * + * Author: Dongjiao HE + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the Universitaet Bremen nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#ifndef __MEKFOM_UTIL_HPP__ +#define __MEKFOM_UTIL_HPP__ + +#include +#include "../mtk/src/mtkmath.hpp" +namespace esekfom { + +template +class is_same { +public: + operator bool() { + return false; + } +}; +template +class is_same { +public: + operator bool() { + return true; + } +}; + +template +class is_double { +public: + operator bool() { + return false; + } +}; + +template<> +class is_double { +public: + operator bool() { + return true; + } +}; + +template +static T +id(const T &x) +{ + return x; +} + +} // namespace esekfom + +#endif // __MEKFOM_UTIL_HPP__ diff --git a/src/Point-LIO/include/IKFoM/IKFoM_toolkit/mtk/build_manifold.hpp b/src/Point-LIO/include/IKFoM/IKFoM_toolkit/mtk/build_manifold.hpp new file mode 100755 index 0000000..115ec7e --- /dev/null +++ b/src/Point-LIO/include/IKFoM/IKFoM_toolkit/mtk/build_manifold.hpp @@ -0,0 +1,248 @@ +// This is an advanced implementation of the algorithm described in the +// following paper: +// C. Hertzberg, R. Wagner, U. Frese, and L. Schroder. Integratinggeneric sensor fusion algorithms with sound state representationsthrough encapsulation of manifolds. +// CoRR, vol. abs/1107.1119, 2011.[Online]. Available: http://arxiv.org/abs/1107.1119 + +/* + * Copyright (c) 2019--2023, The University of Hong Kong + * All rights reserved. + * + * Modifier: Dongjiao HE + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the Universitaet Bremen nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/* + * Copyright (c) 2008--2011, Universitaet Bremen + * All rights reserved. + * + * Author: Christoph Hertzberg + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the Universitaet Bremen nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ +/** + * @file mtk/build_manifold.hpp + * @brief Macro to automatically construct compound manifolds. + * + */ +#ifndef MTK_AUTOCONSTRUCT_HPP_ +#define MTK_AUTOCONSTRUCT_HPP_ + +#include + +#include +#include +#include + +#include "src/SubManifold.hpp" +#include "startIdx.hpp" + +#ifndef PARSED_BY_DOXYGEN +//////// internals ////// + +#define MTK_APPLY_MACRO_ON_TUPLE(r, macro, tuple) macro tuple + +#define MTK_TRANSFORM_COMMA(macro, entries) BOOST_PP_SEQ_ENUM(BOOST_PP_SEQ_TRANSFORM_S(1, MTK_APPLY_MACRO_ON_TUPLE, macro, entries)) + +#define MTK_TRANSFORM(macro, entries) BOOST_PP_SEQ_FOR_EACH_R(1, MTK_APPLY_MACRO_ON_TUPLE, macro, entries) + +#define MTK_CONSTRUCTOR_ARG( type, id) const type& id = type() +#define MTK_CONSTRUCTOR_COPY( type, id) id(id) +#define MTK_BOXPLUS( type, id) id.boxplus(MTK::subvector(__vec, &self::id), __scale); +#define MTK_OPLUS( type, id) id.oplus(MTK::subvector_(__vec, &self::id), __scale); +#define MTK_BOXMINUS( type, id) id.boxminus(MTK::subvector(__res, &self::id), __oth.id); +#define MTK_HAT( type, id) if(id.IDX == idx){id.hat(vec, res);} +#define MTK_JACOB_RIGHT_INV( type, id) if(id.IDX == idx){id.Jacob_right_inv(vec, res);} +#define MTK_JACOB_RIGHT( type, id) if(id.IDX == idx){id.Jacob_right(vec, res);} +#define MTK_S2_hat( type, id) if(id.IDX == idx){id.S2_hat(res);} +#define MTK_S2_Nx_yy( type, id) if(id.IDX == idx){id.S2_Nx_yy(res);} +#define MTK_S2_Mx( type, id) if(id.IDX == idx){id.S2_Mx(res, dx);} +#define MTK_OSTREAM( type, id) << __var.id << " " +#define MTK_ISTREAM( type, id) >> __var.id +#define MTK_S2_state( type, id) if(id.TYP == 1){S2_state.push_back(std::make_pair(id.IDX, id.DIM));} +#define MTK_SO3_state( type, id) if(id.TYP == 2){(SO3_state).push_back(std::make_pair(id.IDX, id.DIM));} +#define MTK_vect_state( type, id) if(id.TYP == 0){(vect_state).push_back(std::make_pair(std::make_pair(id.IDX, id.DIM), type::DOF));} +#define MTK_SEN_state( type, id) if(id.TYP == 4){(SEN_state).push_back(std::make_pair(std::make_pair(id.IDX, id.DIM), type::DOF));} + +#define MTK_SUBVARLIST(seq, S2state, SO3state, SENstate) \ +BOOST_PP_FOR_1( \ + ( \ + BOOST_PP_SEQ_SIZE(seq), \ + BOOST_PP_SEQ_HEAD(seq), \ + BOOST_PP_SEQ_TAIL(seq) (~), \ + 0,\ + 0,\ + S2state,\ + SO3state,\ + SENstate ),\ + MTK_ENTRIES_TEST, MTK_ENTRIES_NEXT, MTK_ENTRIES_OUTPUT) + +#define MTK_PUT_TYPE(type, id, dof, dim, S2state, SO3state, SENstate) \ + MTK::SubManifold id; +#define MTK_PUT_TYPE_AND_ENUM(type, id, dof, dim, S2state, SO3state, SENstate) \ + MTK_PUT_TYPE(type, id, dof, dim, S2state, SO3state, SENstate) \ + enum {DOF = type::DOF + dof}; \ + enum {DIM = type::DIM+dim}; \ + typedef type::scalar scalar; + +#define MTK_ENTRIES_OUTPUT(r, state) MTK_ENTRIES_OUTPUT_I state +#define MTK_ENTRIES_OUTPUT_I(s, head, seq, dof, dim, S2state, SO3state, SENstate) \ + MTK_APPLY_MACRO_ON_TUPLE(~, \ + BOOST_PP_IF(BOOST_PP_DEC(s), MTK_PUT_TYPE, MTK_PUT_TYPE_AND_ENUM), \ + ( BOOST_PP_TUPLE_REM_2 head, dof, dim, S2state, SO3state, SENstate)) + +#define MTK_ENTRIES_TEST(r, state) MTK_TUPLE_ELEM_4_0 state + +//! this used to be BOOST_PP_TUPLE_ELEM_4_0: +#define MTK_TUPLE_ELEM_4_0(a,b,c,d,e,f, g, h) a + +#define MTK_ENTRIES_NEXT(r, state) MTK_ENTRIES_NEXT_I state +#define MTK_ENTRIES_NEXT_I(len, head, seq, dof, dim, S2state, SO3state, SENstate) ( \ + BOOST_PP_DEC(len), \ + BOOST_PP_SEQ_HEAD(seq), \ + BOOST_PP_SEQ_TAIL(seq), \ + dof + BOOST_PP_TUPLE_ELEM_2_0 head::DOF,\ + dim + BOOST_PP_TUPLE_ELEM_2_0 head::DIM,\ + S2state,\ + SO3state,\ + SENstate ) + +#endif /* not PARSED_BY_DOXYGEN */ + + +/** + * Construct a manifold. + * @param name is the class-name of the manifold, + * @param entries is the list of sub manifolds + * + * Entries must be given in a list like this: + * @code + * typedef MTK::trafo > Pose; + * typedef MTK::vect Vec3; + * MTK_BUILD_MANIFOLD(imu_state, + * ((Pose, pose)) + * ((Vec3, vel)) + * ((Vec3, acc_bias)) + * ) + * @endcode + * Whitespace is optional, but the double parentheses are necessary. + * Construction is done entirely in preprocessor. + * After construction @a name is also a manifold. Its members can be + * accessed by names given in @a entries. + * + * @note Variable types are not allowed to have commas, thus types like + * @c vect need to be typedef'ed ahead. + */ +#define MTK_BUILD_MANIFOLD(name, entries) \ +struct name { \ + typedef name self; \ + std::vector > S2_state;\ + std::vector > SO3_state;\ + std::vector, int> > vect_state;\ + std::vector, int> > SEN_state;\ + MTK_SUBVARLIST(entries, S2_state, SO3_state, SEN_state) \ + name ( \ + MTK_TRANSFORM_COMMA(MTK_CONSTRUCTOR_ARG, entries) \ + ) : \ + MTK_TRANSFORM_COMMA(MTK_CONSTRUCTOR_COPY, entries) {}\ + int getDOF() const { return DOF; } \ + void boxplus(const MTK::vectview & __vec, scalar __scale = 1 ) { \ + MTK_TRANSFORM(MTK_BOXPLUS, entries)\ + } \ + void oplus(const MTK::vectview & __vec, scalar __scale = 1 ) { \ + MTK_TRANSFORM(MTK_OPLUS, entries)\ + } \ + void boxminus(MTK::vectview __res, const name& __oth) const { \ + MTK_TRANSFORM(MTK_BOXMINUS, entries)\ + } \ + friend std::ostream& operator<<(std::ostream& __os, const name& __var){ \ + return __os MTK_TRANSFORM(MTK_OSTREAM, entries); \ + } \ + void build_S2_state(){\ + MTK_TRANSFORM(MTK_S2_state, entries)\ + }\ + void build_vect_state(){\ + MTK_TRANSFORM(MTK_vect_state, entries)\ + }\ + void build_SO3_state(){\ + MTK_TRANSFORM(MTK_SO3_state, entries)\ + }\ + void build_SEN_state(){\ + MTK_TRANSFORM(MTK_SEN_state, entries)\ + }\ + void Lie_hat(Eigen::VectorXd &vec, Eigen::MatrixXd &res, int idx) {\ + MTK_TRANSFORM(MTK_HAT, entries)\ + }\ + void Lie_Jacob_Right_Inv(Eigen::VectorXd &vec, Eigen::MatrixXd &res, int idx) {\ + MTK_TRANSFORM(MTK_JACOB_RIGHT_INV, entries)\ + }\ + void Lie_Jacob_Right(Eigen::VectorXd &vec, Eigen::MatrixXd &res, int idx) {\ + MTK_TRANSFORM(MTK_JACOB_RIGHT, entries)\ + }\ + void S2_hat(Eigen::Matrix &res, int idx) {\ + MTK_TRANSFORM(MTK_S2_hat, entries)\ + }\ + void S2_Nx_yy(Eigen::Matrix &res, int idx) {\ + MTK_TRANSFORM(MTK_S2_Nx_yy, entries)\ + }\ + void S2_Mx(Eigen::Matrix &res, Eigen::Matrix dx, int idx) {\ + MTK_TRANSFORM(MTK_S2_Mx, entries)\ + }\ + friend std::istream& operator>>(std::istream& __is, name& __var){ \ + return __is MTK_TRANSFORM(MTK_ISTREAM, entries); \ + } \ +}; + + + +#endif /*MTK_AUTOCONSTRUCT_HPP_*/ diff --git a/src/Point-LIO/include/IKFoM/IKFoM_toolkit/mtk/src/SubManifold.hpp b/src/Point-LIO/include/IKFoM/IKFoM_toolkit/mtk/src/SubManifold.hpp new file mode 100755 index 0000000..a1b13de --- /dev/null +++ b/src/Point-LIO/include/IKFoM/IKFoM_toolkit/mtk/src/SubManifold.hpp @@ -0,0 +1,123 @@ +// This is an advanced implementation of the algorithm described in the +// following paper: +// C. Hertzberg, R. Wagner, U. Frese, and L. Schroder. Integratinggeneric sensor fusion algorithms with sound state representationsthrough encapsulation of manifolds. +// CoRR, vol. abs/1107.1119, 2011.[Online]. Available: http://arxiv.org/abs/1107.1119 + +/* + * Copyright (c) 2019--2023, The University of Hong Kong + * All rights reserved. + * + * Modifier: Dongjiao HE + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the Universitaet Bremen nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/* + * Copyright (c) 2008--2011, Universitaet Bremen + * All rights reserved. + * + * Author: Christoph Hertzberg + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the Universitaet Bremen nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ +/** + * @file mtk/src/SubManifold.hpp + * @brief Defines the SubManifold class + */ + + +#ifndef SUBMANIFOLD_HPP_ +#define SUBMANIFOLD_HPP_ + + +#include "vectview.hpp" + + +namespace MTK { + +/** + * @ingroup SubManifolds + * Helper class for compound manifolds. + * This class wraps a manifold T and provides an enum IDX refering to the + * index of the SubManifold within the compound manifold. + * + * Memberpointers to a submanifold can be used for @ref SubManifolds "functions accessing submanifolds". + * + * @tparam T The manifold type of the sub-type + * @tparam idx The index of the sub-type within the compound manifold + */ +template +struct SubManifold : public T +{ + enum {IDX = idx, DIM = dim /*!< index of the sub-type within the compound manifold */ }; + //! manifold type + typedef T type; + + //! Construct from derived type + template + explicit + SubManifold(const X& t) : T(t) {}; + + //! Construct from internal type + //explicit + SubManifold(const T& t) : T(t) {}; + + //! inherit assignment operator + using T::operator=; + +}; + +} // namespace MTK + + +#endif /* SUBMANIFOLD_HPP_ */ diff --git a/src/Point-LIO/include/IKFoM/IKFoM_toolkit/mtk/src/mtkmath.hpp b/src/Point-LIO/include/IKFoM/IKFoM_toolkit/mtk/src/mtkmath.hpp new file mode 100755 index 0000000..a8770e4 --- /dev/null +++ b/src/Point-LIO/include/IKFoM/IKFoM_toolkit/mtk/src/mtkmath.hpp @@ -0,0 +1,294 @@ +// This is an advanced implementation of the algorithm described in the +// following paper: +// C. Hertzberg, R. Wagner, U. Frese, and L. Schroder. Integratinggeneric sensor fusion algorithms with sound state representationsthrough encapsulation of manifolds. +// CoRR, vol. abs/1107.1119, 2011.[Online]. Available: http://arxiv.org/abs/1107.1119 + +/* + * Copyright (c) 2019--2023, The University of Hong Kong + * All rights reserved. + * + * Modifier: Dongjiao HE + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the Universitaet Bremen nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/* + * Copyright (c) 2008--2011, Universitaet Bremen + * All rights reserved. + * + * Author: Christoph Hertzberg + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the Universitaet Bremen nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ +/** + * @file mtk/src/mtkmath.hpp + * @brief several math utility functions. + */ + +#ifndef MTKMATH_H_ +#define MTKMATH_H_ + +#include + +#include + +#include "../types/vect.hpp" + +#ifndef M_PI +#define M_PI 3.1415926535897932384626433832795 +#endif + + +namespace MTK { + +namespace internal { + +template +struct traits { + typedef typename Manifold::scalar scalar; + enum {DOF = Manifold::DOF}; + typedef vect vectorized_type; + typedef Eigen::Matrix matrix_type; +}; + +template<> +struct traits : traits > {}; +template<> +struct traits : traits > {}; + +} // namespace internal + +/** + * \defgroup MTKMath Mathematical helper functions + */ +//@{ + +//! constant @f$ \pi @f$ +const double pi = M_PI; + +template inline scalar tolerance(); + +template<> inline float tolerance() { return 1e-5f; } +template<> inline double tolerance() { return 1e-11; } + + +/** + * normalize @a x to @f$[-bound, bound] @f$. + * + * result for @f$ x = bound + 2\cdot n\cdot bound @f$ is arbitrary @f$\pm bound @f$. + */ +template +inline scalar normalize(scalar x, scalar bound){ //not used + if(std::fabs(x) <= bound) return x; + int r = (int)(x *(scalar(1.0)/ bound)); + return x - ((r + (r>>31) + 1) & ~1)*bound; +} + +/** + * Calculate cosine and sinc of sqrt(x2). + * @param x2 the squared angle must be non-negative + * @return a pair containing cos and sinc of sqrt(x2) + */ +template +std::pair cos_sinc_sqrt(const scalar &x2){ + using std::sqrt; + using std::cos; + using std::sin; + static scalar const taylor_0_bound = boost::math::tools::epsilon(); + static scalar const taylor_2_bound = sqrt(taylor_0_bound); + static scalar const taylor_n_bound = sqrt(taylor_2_bound); + + assert(x2>=0 && "argument must be non-negative and must not be nan/-nan"); + + // FIXME check if bigger bounds are possible + if(x2>=taylor_n_bound) { + // slow fall-back solution + scalar x = sqrt(x2); + return std::make_pair(cos(x), sin(x)/x); // x is greater than 0. + } + + // FIXME Replace by Horner-Scheme (4 instead of 5 FLOP/term, numerically more stable, theoretically cos and sinc can be calculated in parallel using SSE2 mulpd/addpd) + // TODO Find optimal coefficients using Remez algorithm + static scalar const inv[] = {1/3., 1/4., 1/5., 1/6., 1/7., 1/8., 1/9.}; + scalar cosi = 1., sinc=1; + scalar term = -1/2. * x2; + for(int i=0; i<3; ++i) { + cosi += term; + term *= inv[2*i]; + sinc += term; + term *= -inv[2*i+1] * x2; + } + + return std::make_pair(cosi, sinc); + +} + +template +Eigen::Matrix hat(const Base& v) { + Eigen::Matrix res; + res << 0, -v[2], v[1], + v[2], 0, -v[0], + -v[1], v[0], 0; + return res; +} + +template +Eigen::Matrix A_inv_trans(const Base& v){ + Eigen::Matrix res; + if(v.norm() > MTK::tolerance()) + { + res = Eigen::Matrix::Identity() + 0.5 * hat(v) + (1 - v.norm() * std::cos(v.norm() / 2) / 2 / std::sin(v.norm() / 2)) * hat(v) * hat(v) / v.squaredNorm(); + + } + else + { + res = Eigen::Matrix::Identity(); + } + + return res; +} + +template +Eigen::Matrix A_inv(const Base& v){ + Eigen::Matrix res; + if(v.norm() > MTK::tolerance()) + { + res = Eigen::Matrix::Identity() - 0.5 * hat(v) + (1 - v.norm() * std::cos(v.norm() / 2) / 2 / std::sin(v.norm() / 2)) * hat(v) * hat(v) / v.squaredNorm(); + + } + else + { + res = Eigen::Matrix::Identity(); + } + + return res; +} + +template +Eigen::Matrix S2_w_expw_( Eigen::Matrix v, scalar length) + { + Eigen::Matrix res; + scalar norm = std::sqrt(v[0]*v[0] + v[1]*v[1]); + if(norm < MTK::tolerance()){ + res = Eigen::Matrix::Zero(); + res(0, 1) = 1; + res(1, 2) = 1; + res /= length; + } + else{ + res << -v[0]*(1/norm-1/std::tan(norm))/std::sin(norm), norm/std::sin(norm), 0, + -v[1]*(1/norm-1/std::tan(norm))/std::sin(norm), 0, norm/std::sin(norm); + res /= length; + } + } + +template +Eigen::Matrix A_matrix(const Base & v){ + Eigen::Matrix res; + double squaredNorm = v[0] * v[0] + v[1] * v[1] + v[2] * v[2]; + double norm = std::sqrt(squaredNorm); + if(norm < MTK::tolerance()){ + res = Eigen::Matrix::Identity(); + } + else{ + res = Eigen::Matrix::Identity() + (1 - std::cos(norm)) / squaredNorm * hat(v) + (1 - std::sin(norm) / norm) / squaredNorm * hat(v) * hat(v); + } + return res; +} + +template +scalar exp(vectview result, vectview vec, const scalar& scale = 1) { + scalar norm2 = vec.squaredNorm(); + std::pair cos_sinc = cos_sinc_sqrt(scale*scale * norm2); + scalar mult = cos_sinc.second * scale; + result = mult * vec; + return cos_sinc.first; +} + + +/** + * Inverse function to @c exp. + * + * @param result @c vectview to the result + * @param w scalar part of input + * @param vec vector part of input + * @param scale scale result by this value + * @param plus_minus_periodicity if true values @f$[w, vec]@f$ and @f$[-w, -vec]@f$ give the same result + */ +template +void log(vectview result, + const scalar &w, const vectview vec, + const scalar &scale, bool plus_minus_periodicity) +{ + // FIXME implement optimized case for vec.squaredNorm() <= tolerance() * (w*w) via Rational Remez approximation ~> only one division + scalar nv = vec.norm(); + if(nv < tolerance()) { + if(!plus_minus_periodicity && w < 0) { + // find the maximal entry: + int i; + nv = vec.cwiseAbs().maxCoeff(&i); + result = scale * std::atan2(nv, w) * vect::Unit(i); + return; + } + nv = tolerance(); + } + scalar s = scale / nv * (plus_minus_periodicity ? std::atan(nv / w) : std::atan2(nv, w) ); + + result = s * vec; +} + + +} // namespace MTK + + +#endif /* MTKMATH_H_ */ diff --git a/src/Point-LIO/include/IKFoM/IKFoM_toolkit/mtk/src/vectview.hpp b/src/Point-LIO/include/IKFoM/IKFoM_toolkit/mtk/src/vectview.hpp new file mode 100755 index 0000000..5025071 --- /dev/null +++ b/src/Point-LIO/include/IKFoM/IKFoM_toolkit/mtk/src/vectview.hpp @@ -0,0 +1,168 @@ + +/* + * Copyright (c) 2008--2011, Universitaet Bremen + * All rights reserved. + * + * Author: Christoph Hertzberg + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the Universitaet Bremen nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ +/** + * @file mtk/src/vectview.hpp + * @brief Wrapper class around a pointer used as interface for plain vectors. + */ + +#ifndef VECTVIEW_HPP_ +#define VECTVIEW_HPP_ + +#include + +namespace MTK { + +/** + * A view to a vector. + * Essentially, @c vectview is only a pointer to @c scalar but can be used directly in @c Eigen expressions. + * The dimension of the vector is given as template parameter and type-checked when used in expressions. + * Data has to be modifiable. + * + * @tparam scalar Scalar type of the vector. + * @tparam dim Dimension of the vector. + * + * @todo @c vectview can be replaced by simple inheritance of @c Eigen::Map, as soon as they get const-correct + */ +namespace internal { + template + struct CovBlock { + typedef typename Eigen::Block, T1::DOF, T2::DOF> Type; + typedef typename Eigen::Block, T1::DOF, T2::DOF> ConstType; + }; + + template + struct CovBlock_ { + typedef typename Eigen::Block, T1::DIM, T2::DIM> Type; + typedef typename Eigen::Block, T1::DIM, T2::DIM> ConstType; + }; + + template + struct CrossCovBlock { + typedef typename Eigen::Block, T1::DOF, T2::DOF> Type; + typedef typename Eigen::Block, T1::DOF, T2::DOF> ConstType; + }; + + template + struct CrossCovBlock_ { + typedef typename Eigen::Block, T1::DIM, T2::DIM> Type; + typedef typename Eigen::Block, T1::DIM, T2::DIM> ConstType; + }; + + template + struct VectviewBase { + typedef Eigen::Matrix matrix_type; + typedef typename matrix_type::MapType Type; + typedef typename matrix_type::ConstMapType ConstType; + }; + + template + struct UnalignedType { + typedef T type; + }; +} + +template +class vectview : public internal::VectviewBase::Type { + typedef internal::VectviewBase VectviewBase; +public: + //! plain matrix type + typedef typename VectviewBase::matrix_type matrix_type; + //! base type + typedef typename VectviewBase::Type base; + //! construct from pointer + explicit + vectview(scalar* data, int dim_=dim) : base(data, dim_) {} + //! construct from plain matrix + vectview(matrix_type& m) : base(m.data(), m.size()) {} + //! construct from another @c vectview + vectview(const vectview &v) : base(v) {} + //! construct from Eigen::Block: + template + vectview(Eigen::VectorBlock block) : base(&block.coeffRef(0), block.size()) {} + template + vectview(Eigen::Block block) : base(&block.coeffRef(0), block.size()) {} + + //! inherit assignment operator + using base::operator=; + //! data pointer + scalar* data() {return const_cast(base::data());} +}; + +/** + * @c const version of @c vectview. + * Compared to @c Eigen::Map this implementation is const correct, i.e., + * data will not be modifiable using this view. + * + * @tparam scalar Scalar type of the vector. + * @tparam dim Dimension of the vector. + * + * @sa vectview + */ +template +class vectview : public internal::VectviewBase::ConstType { + typedef internal::VectviewBase VectviewBase; +public: + //! plain matrix type + typedef typename VectviewBase::matrix_type matrix_type; + //! base type + typedef typename VectviewBase::ConstType base; + //! construct from const pointer + explicit + vectview(const scalar* data, int dim_ = dim) : base(data, dim_) {} + //! construct from column vector + template + vectview(const Eigen::Matrix& m) : base(m.data()) {} + //! construct from row vector + template + vectview(const Eigen::Matrix& m) : base(m.data()) {} + //! construct from another @c vectview + vectview(vectview x) : base(x.data()) {} + //! construct from base + vectview(const base &x) : base(x) {} + /** + * Construct from Block + * @todo adapt this, when Block gets const-correct + */ + template + vectview(Eigen::VectorBlock block) : base(&block.coeffRef(0)) {} + template + vectview(Eigen::Block block) : base(&block.coeffRef(0)) {} + +}; + + +} // namespace MTK + +#endif /* VECTVIEW_HPP_ */ diff --git a/src/Point-LIO/include/IKFoM/IKFoM_toolkit/mtk/startIdx.hpp b/src/Point-LIO/include/IKFoM/IKFoM_toolkit/mtk/startIdx.hpp new file mode 100755 index 0000000..4dc2958 --- /dev/null +++ b/src/Point-LIO/include/IKFoM/IKFoM_toolkit/mtk/startIdx.hpp @@ -0,0 +1,328 @@ +// This is an advanced implementation of the algorithm described in the +// following paper: +// C. Hertzberg, R. Wagner, U. Frese, and L. Schroder. Integratinggeneric sensor fusion algorithms with sound state representationsthrough encapsulation of manifolds. +// CoRR, vol. abs/1107.1119, 2011.[Online]. Available: http://arxiv.org/abs/1107.1119 + +/* + * Copyright (c) 2019--2023, The University of Hong Kong + * All rights reserved. + * + * Modifier: Dongjiao HE + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the Universitaet Bremen nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/* + * Copyright (c) 2008--2011, Universitaet Bremen + * All rights reserved. + * + * Author: Christoph Hertzberg + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the Universitaet Bremen nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ +/** + * @file mtk/startIdx.hpp + * @brief Tools to access sub-elements of compound manifolds. + */ +#ifndef GET_START_INDEX_H_ +#define GET_START_INDEX_H_ + +#include + +#include "src/SubManifold.hpp" +#include "src/vectview.hpp" + +namespace MTK { + + +/** + * \defgroup SubManifolds Accessing Submanifolds + * For compound manifolds constructed using MTK_BUILD_MANIFOLD, member pointers + * can be used to get sub-vectors or matrix-blocks of a corresponding big matrix. + * E.g. for a type @a pose consisting of @a orient and @a trans the member pointers + * @c &pose::orient and @c &pose::trans give all required information and are still + * valid if the base type gets extended or the actual types of @a orient and @a trans + * change (e.g. from 2D to 3D). + * + * @todo Maybe require manifolds to typedef MatrixType and VectorType, etc. + */ +//@{ + +/** + * Determine the index of a sub-variable within a compound variable. + */ +template +int getStartIdx( MTK::SubManifold Base::*) +{ + return idx; +} + +template +int getStartIdx_( MTK::SubManifold Base::*) +{ + return dim; +} + +/** + * Determine the degrees of freedom of a sub-variable within a compound variable. + */ +template +int getDof( MTK::SubManifold Base::*) +{ + return T::DOF; +} +template +int getDim( MTK::SubManifold Base::*) +{ + return T::DIM; +} + +/** + * set the diagonal elements of a covariance matrix corresponding to a sub-variable + */ +template +void setDiagonal(Eigen::Matrix &cov, + MTK::SubManifold Base::*, const typename Base::scalar &val) +{ + cov.diagonal().template segment(idx).setConstant(val); +} + +template +void setDiagonal_(Eigen::Matrix &cov, + MTK::SubManifold Base::*, const typename Base::scalar &val) +{ + cov.diagonal().template segment(dim).setConstant(val); +} + +/** + * Get the subblock of corresponding to two members, i.e. + * \code + * Eigen::Matrix m; + * MTK::subblock(m, &Pose::orient, &Pose::trans) = some_expression; + * MTK::subblock(m, &Pose::trans, &Pose::orient) = some_expression.trans(); + * \endcode + * lets you modify mixed covariance entries in a bigger covariance matrix. + */ +template +typename MTK::internal::CovBlock::Type +subblock(Eigen::Matrix &cov, + MTK::SubManifold Base::*, MTK::SubManifold Base::*) +{ + return cov.template block(idx1, idx2); +} + +template +typename MTK::internal::CovBlock_::Type +subblock_(Eigen::Matrix &cov, + MTK::SubManifold Base::*, MTK::SubManifold Base::*) +{ + return cov.template block(dim1, dim2); +} + +template +typename MTK::internal::CrossCovBlock::Type +subblock(Eigen::Matrix &cov, MTK::SubManifold Base1::*, MTK::SubManifold Base2::*) +{ + return cov.template block(idx1, idx2); +} + +template +typename MTK::internal::CrossCovBlock_::Type +subblock_(Eigen::Matrix &cov, MTK::SubManifold Base1::*, MTK::SubManifold Base2::*) +{ + return cov.template block(dim1, dim2); +} +/** + * Get the subblock of corresponding to a member, i.e. + * \code + * Eigen::Matrix m; + * MTK::subblock(m, &Pose::orient) = some_expression; + * \endcode + * lets you modify covariance entries in a bigger covariance matrix. + */ +template +typename MTK::internal::CovBlock_::Type +subblock_(Eigen::Matrix &cov, + MTK::SubManifold Base::*) +{ + return cov.template block(dim, dim); +} + +template +typename MTK::internal::CovBlock::Type +subblock(Eigen::Matrix &cov, + MTK::SubManifold Base::*) +{ + return cov.template block(idx, idx); +} + +template +class get_cov { +public: + typedef Eigen::Matrix type; + typedef const Eigen::Matrix const_type; +}; + +template +class get_cov_ { +public: + typedef Eigen::Matrix type; + typedef const Eigen::Matrix const_type; +}; + +template +class get_cross_cov { +public: + typedef Eigen::Matrix type; + typedef const type const_type; +}; + +template +class get_cross_cov_ { +public: + typedef Eigen::Matrix type; + typedef const type const_type; +}; + + +template +vectview +subvector_impl_(vectview vec, SubManifold Base::*) +{ + return vec.template segment(dim); +} + +template +vectview +subvector_impl(vectview vec, SubManifold Base::*) +{ + return vec.template segment(idx); +} + +/** + * Get the subvector corresponding to a sub-manifold from a bigger vector. + */ + template +vectview +subvector_(vectview vec, SubManifold Base::* ptr) +{ + return subvector_impl_(vec, ptr); +} + +template +vectview +subvector(vectview vec, SubManifold Base::* ptr) +{ + return subvector_impl(vec, ptr); +} + +/** + * @todo This should be covered already by subvector(vectview vec,SubManifold Base::*) + */ +template +vectview +subvector(Eigen::Matrix& vec, SubManifold Base::* ptr) +{ + return subvector_impl(vectview(vec), ptr); +} + +template +vectview +subvector_(Eigen::Matrix& vec, SubManifold Base::* ptr) +{ + return subvector_impl_(vectview(vec), ptr); +} + +template +vectview +subvector_(const Eigen::Matrix& vec, SubManifold Base::* ptr) +{ + return subvector_impl_(vectview(vec), ptr); +} + +template +vectview +subvector(const Eigen::Matrix& vec, SubManifold Base::* ptr) +{ + return subvector_impl(vectview(vec), ptr); +} + + +/** + * const version of subvector(vectview vec,SubManifold Base::*) + */ +template +vectview +subvector_impl(const vectview cvec, SubManifold Base::*) +{ + return cvec.template segment(idx); +} + +template +vectview +subvector_impl_(const vectview cvec, SubManifold Base::*) +{ + return cvec.template segment(dim); +} + +template +vectview +subvector(const vectview cvec, SubManifold Base::* ptr) +{ + return subvector_impl(cvec, ptr); +} + + +} // namespace MTK + +#endif // GET_START_INDEX_H_ diff --git a/src/Point-LIO/include/IKFoM/IKFoM_toolkit/mtk/types/S2.hpp b/src/Point-LIO/include/IKFoM/IKFoM_toolkit/mtk/types/S2.hpp new file mode 100755 index 0000000..1ff0250 --- /dev/null +++ b/src/Point-LIO/include/IKFoM/IKFoM_toolkit/mtk/types/S2.hpp @@ -0,0 +1,326 @@ +// This is a NEW implementation of the algorithm described in the +// following paper: +// C. Hertzberg, R. Wagner, U. Frese, and L. Schroder. Integratinggeneric sensor fusion algorithms with sound state representationsthrough encapsulation of manifolds. +// CoRR, vol. abs/1107.1119, 2011.[Online]. Available: http://arxiv.org/abs/1107.1119 + +/* + * Copyright (c) 2019--2023, The University of Hong Kong + * All rights reserved. + * + * Modifier: Dongjiao HE + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the Universitaet Bremen nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/* + * Copyright (c) 2008--2011, Universitaet Bremen + * All rights reserved. + * + * Author: Christoph Hertzberg + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the Universitaet Bremen nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ +/** + * @file mtk/types/S2.hpp + * @brief Unit vectors on the sphere, or directions in 3D. + */ +#ifndef S2_H_ +#define S2_H_ + + +#include "vect.hpp" + +#include "SOn.hpp" +#include "../src/mtkmath.hpp" + + + + +namespace MTK { + +/** + * Manifold representation of @f$ S^2 @f$. + * Used for unit vectors on the sphere or directions in 3D. + * + * @todo add conversions from/to polar angles? + */ +template +struct S2 { + + typedef _scalar scalar; + typedef vect<3, scalar> vect_type; + typedef SO3 SO3_type; + typedef typename vect_type::base vec3; + scalar length = scalar(den)/scalar(num); + enum {DOF=2, TYP = 1, DIM = 3}; + +//private: + /** + * Unit vector on the sphere, or vector pointing in a direction + */ + vect_type vec; + +public: + S2() { + if(S2_typ == 3) vec=length * vec3(0, 0, std::sqrt(1)); + if(S2_typ == 2) vec=length * vec3(0, std::sqrt(1), 0); + if(S2_typ == 1) vec=length * vec3(std::sqrt(1), 0, 0); + } + S2(const scalar &x, const scalar &y, const scalar &z) : vec(vec3(x, y, z)) { + vec.normalize(); + vec = vec * length; + } + + S2(const vect_type &_vec) : vec(_vec) { + vec.normalize(); + vec = vec * length; + } + + void oplus(MTK::vectview delta, scalar scale = 1) + { + SO3_type res; + res.w() = MTK::exp(res.vec(), delta, scalar(scale/2)); + vec = res.normalized().toRotationMatrix() * vec; + } + + void boxplus(MTK::vectview delta, scalar scale=1) { + Eigen::Matrix Bx; + S2_Bx(Bx); + vect_type Bu = Bx*delta;SO3_type res; + res.w() = MTK::exp(res.vec(), Bu, scalar(scale/2)); + vec = res.normalized().toRotationMatrix() * vec; + } + + void boxminus(MTK::vectview res, const S2& other) const { + scalar v_sin = (MTK::hat(vec)*other.vec).norm(); + scalar v_cos = vec.transpose() * other.vec; + scalar theta = std::atan2(v_sin, v_cos); + if(v_sin < MTK::tolerance()) + { + if(std::fabs(theta) > MTK::tolerance() ) + { + res[0] = 3.1415926; + res[1] = 0; + } + else{ + res[0] = 0; + res[1] = 0; + } + } + else + { + S2 other_copy = other; + Eigen::MatrixBx; + other_copy.S2_Bx(Bx); + res = theta/v_sin * Bx.transpose() * MTK::hat(other.vec)*vec; + } + } + + void hat(Eigen::VectorXd& v, Eigen::MatrixXd &res) { + std::cout << "wrong idx" << std::endl; + } + void Jacob_right_inv(Eigen::VectorXd& v, Eigen::MatrixXd &res) { + std::cout << "wrong idx" << std::endl; + } + void Jacob_right(Eigen::VectorXd& v, Eigen::MatrixXd &res) { + std::cout << "wrong idx" << std::endl; + } + + void S2_hat(Eigen::Matrix &res) + { + Eigen::Matrix skew_vec; + skew_vec << scalar(0), -vec[2], vec[1], + vec[2], scalar(0), -vec[0], + -vec[1], vec[0], scalar(0); + res = skew_vec; + } + + + void S2_Bx(Eigen::Matrix &res) + { + if(S2_typ == 3) + { + if(vec[2] + length > tolerance()) + { + + res << length - vec[0]*vec[0]/(length+vec[2]), -vec[0]*vec[1]/(length+vec[2]), + -vec[0]*vec[1]/(length+vec[2]), length-vec[1]*vec[1]/(length+vec[2]), + -vec[0], -vec[1]; + res /= length; + } + else + { + res = Eigen::Matrix::Zero(); + res(1, 1) = -1; + res(2, 0) = 1; + } + } + else if(S2_typ == 2) + { + if(vec[1] + length > tolerance()) + { + + res << length - vec[0]*vec[0]/(length+vec[1]), -vec[0]*vec[2]/(length+vec[1]), + -vec[0], -vec[2], + -vec[0]*vec[2]/(length+vec[1]), length-vec[2]*vec[2]/(length+vec[1]); + res /= length; + } + else + { + res = Eigen::Matrix::Zero(); + res(1, 1) = -1; + res(2, 0) = 1; + } + } + else + { + if(vec[0] + length > tolerance()) + { + + res << -vec[1], -vec[2], + length - vec[1]*vec[1]/(length+vec[0]), -vec[2]*vec[1]/(length+vec[0]), + -vec[2]*vec[1]/(length+vec[0]), length-vec[2]*vec[2]/(length+vec[0]); + res /= length; + } + else + { + res = Eigen::Matrix::Zero(); + res(1, 1) = -1; + res(2, 0) = 1; + } + } + } + + void S2_Nx(Eigen::Matrix &res, S2& subtrahend) + { + if((vec+subtrahend.vec).norm() > tolerance()) + { + Eigen::Matrix Bx; + S2_Bx(Bx); + if((vec-subtrahend.vec).norm() > tolerance()) + { + scalar v_sin = (MTK::hat(vec)*subtrahend.vec).norm(); + scalar v_cos = vec.transpose() * subtrahend.vec; + + res = Bx.transpose() * (std::atan2(v_sin, v_cos)/v_sin*MTK::hat(vec)+MTK::hat(vec)*subtrahend.vec*((-v_cos/v_sin/v_sin/length/length/length/length+std::atan2(v_sin, v_cos)/v_sin/v_sin/v_sin)*subtrahend.vec.transpose()*MTK::hat(vec)*MTK::hat(vec)-vec.transpose()/length/length/length/length)); + } + else + { + res = 1/length/length*Bx.transpose()*MTK::hat(vec); + } + } + else + { + std::cerr << "No N(x, y) for x=-y" << std::endl; + std::exit(100); + } + } + + void S2_Nx_yy(Eigen::Matrix &res) + { + Eigen::Matrix Bx; + S2_Bx(Bx); + res = 1/length/length*Bx.transpose()*MTK::hat(vec); + } + + void S2_Mx(Eigen::Matrix &res, MTK::vectview delta) + { + Eigen::Matrix Bx; + S2_Bx(Bx); + if(delta.norm() < tolerance()) + { + res = -MTK::hat(vec)*Bx; + } + else{ + vect_type Bu = Bx*delta; + SO3_type exp_delta; + exp_delta.w() = MTK::exp(exp_delta.vec(), Bu, scalar(1/2)); + res = -exp_delta.normalized().toRotationMatrix()*MTK::hat(vec)*MTK::A_matrix(Bu).transpose()*Bx; + } + } + + operator const vect_type&() const{ + return vec; + } + + const vect_type& get_vect() const { + return vec; + } + + friend S2 operator*(const SO3& rot, const S2& dir) + { + S2 ret; + ret.vec = rot.normalized() * dir.vec; + return ret; + } + + scalar operator[](int idx) const {return vec[idx]; } + + friend std::ostream& operator<<(std::ostream &os, const S2& vec){ + return os << vec.vec.transpose() << " "; + } + friend std::istream& operator>>(std::istream &is, S2& vec){ + for(int i=0; i<3; ++i) + is >> vec.vec[i]; + vec.vec.normalize(); + vec.vec = vec.vec * vec.length; + return is; + + } +}; + + +} // namespace MTK + + +#endif /*S2_H_*/ diff --git a/src/Point-LIO/include/IKFoM/IKFoM_toolkit/mtk/types/SEn.hpp b/src/Point-LIO/include/IKFoM/IKFoM_toolkit/mtk/types/SEn.hpp new file mode 100755 index 0000000..2270136 --- /dev/null +++ b/src/Point-LIO/include/IKFoM/IKFoM_toolkit/mtk/types/SEn.hpp @@ -0,0 +1,334 @@ +// This is an advanced implementation of the algorithm described in the +// following paper: +// C. Hertzberg, R. Wagner, U. Frese, and L. Schroder. Integratinggeneric sensor fusion algorithms with sound state representationsthrough encapsulation of manifolds. +// CoRR, vol. abs/1107.1119, 2011.[Online]. Available: http://arxiv.org/abs/1107.1119 + +/* + * Copyright (c) 2019--2023, The University of Hong Kong + * All rights reserved. + * + * Modifier: Dongjiao HE + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the Universitaet Bremen nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/* + * Copyright (c) 2008--2011, Universitaet Bremen + * All rights reserved. + * + * Author: Christoph Hertzberg + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the Universitaet Bremen nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ +/** + * @file mtk/types/SEn.hpp + * @brief Standard Orthogonal Groups i.e.\ rotatation groups. + */ +#ifndef SEN_H_ +#define SEN_H_ + +#include + +#include "SOn.hpp" +#include "vect.hpp" +#include "../src/mtkmath.hpp" + + +namespace MTK { + + +/** + * Three-dimensional orientations represented as Quaternion. + * It is assumed that the internal Quaternion always stays normalized, + * should this not be the case, call inherited member function @c normalize(). + */ +template +struct SEN { + enum {DOF = num_of_vec_plus1, DIM = num_of_vec_plus1, TYP = 4}; + typedef _scalar scalar; + typedef Eigen::Matrix base; + typedef SO3 SO3_type; + // typedef Eigen::Quaternion base; + // typedef Eigen::Quaternion Quaternion; + typedef vect vect_type; + SO3_type SO3_data; + base mat; + + /** + * Construct from real part and three imaginary parts. + * Quaternion is normalized after construction. + */ + // SEN(const base& src) : mat(src) { + // // base::normalize(); + // } + + /** + * Construct from Eigen::Quaternion. + * @note Non-normalized input may result result in spurious behavior. + */ + SEN(const base& src = base::Identity()) : mat(src) {} + + /** + * Construct from rotation matrix. + * @note Invalid rotation matrices may lead to spurious behavior. + */ + // template + // SO3(const Eigen::MatrixBase& matrix) : base(matrix) {} + + /** + * Construct from arbitrary rotation type. + * @note Invalid rotation matrices may lead to spurious behavior. + */ + // template + // SO3(const Eigen::RotationBase& rotation) : base(rotation.derived()) {} + + //! @name Manifold requirements + + void boxplus(MTK::vectview vec, scalar scale=1) { + SEN delta = exp(vec, scale); // ? + mat = mat * delta.mat; + } + void boxminus(MTK::vectview res, const SEN& other) const { + base error_mat = other.mat.inverse() * mat; + res = log(error_mat); + } + //} + + void oplus(MTK::vectview vec, scalar scale=1) { + SEN delta = exp(vec, scale); + mat = mat * delta.mat; + } + + // void hat(MTK::vectview& v, Eigen::Matrix &res) { + void hat(Eigen::VectorXd& v, Eigen::MatrixXd &res) { + res = Eigen::Matrix::Zero(); + Eigen::Matrix psi; + psi << 0, -v[2], v[1], + v[2], 0, -v[0], + -v[1], v[0], 0; + res.block<3, 3>(0, 0) = psi; + for(int i = 3; i < v.size() / 3 + 2; i++) + { + res.block<3, 1>(0, i) = v.segment<3>(i + (i-3)*3); + } + // return res; + } + + // void Jacob_right_inv(MTK::vectview vec, Eigen::Matrix & res){ + void Jacob_right_inv(Eigen::VectorXd& vec, Eigen::MatrixXd &res){ + res = Eigen::Matrix::Zero(); + Eigen::Matrix M_v; + Eigen::VectorXd vec_psi, vec_ro; + Eigen::MatrixXd jac_v; + Eigen::MatrixXd hat_v, hat_ro; + vec_psi = vec.segment<3>(0); + // Eigen::Matrix ; + SO3_data.hat(vec_psi, hat_v); + SO3_data.Jacob_right_inv(vec_psi, jac_v); + double norm = vec_psi.norm(); + for(int i = 0; i < vec.size() / 3; i++) + { + res.block<3, 3>(i*3, i*3) = jac_v; + } + for(int i = 1; i < vec.size() / 3; i++) + { + vec_ro = vec.segment<3>(i * 3); + SO3_data.hat(vec_ro, hat_ro); + if(norm > MTK::tolerance()) + { + res.block<3,3>(i*3, 0) = 0.5 * hat_ro + (1 - norm * std::cos(norm / 2) / 2 / std::sin(norm / 2))/norm/norm * (hat_ro * hat_v + hat_v * hat_ro) + ((2 - norm * std::cos(norm / 2) / 2 / std::sin(norm / 2)) / 2 / norm / norm / norm / norm - 1 / 8 / norm / norm / std::sin(norm / 2) / std::sin(norm / 2)) * hat_v * (hat_ro * hat_v + hat_v * hat_ro) * hat_v; + } + else + { + res.block<3,3>(i*3, 0) = 0.5 * hat_ro; + } + + } + // return res; + } + + // void Jacob_right(MTK::vectview & vec, Eigen::Matrix &res){ + void Jacob_right(Eigen::VectorXd& vec, Eigen::MatrixXd &res){ + res = Eigen::Matrix::Zero(); + Eigen::MatrixXd hat_v, hat_ro; + Eigen::VectorXd vec_psi, vec_ro; + Eigen::MatrixXd jac_v; + vec_psi = vec.segment<3>(0); + // Eigen::Matrix ; + SO3_data.hat(vec_psi, hat_v); + SO3_data.Jacob_right(vec_psi, jac_v); + // double squaredNorm = v[0] * v[0] + v[1] * v[1] + v[2] * v[2]; + // double norm = std::sqrt(squaredNorm); + double norm = vec_psi.norm(); + for(int i = 0; i < vec.size() / 3; i++) + { + res.block<3, 3>(i*3, i*3) = jac_v; + } + for(int i = 1; i < vec.size() / 3; i++) + { + vec_ro = vec.segment<3>(i * 3); + SO3_data.hat(vec_ro, hat_ro); + if(norm > MTK::tolerance()) + { + res.block<3,3>(i*3, 0) = -1 * jac_v * (0.5 * hat_ro + (1 - norm * std::cos(norm / 2) / 2 / std::sin(norm / 2))/norm/norm * (hat_ro * hat_v + hat_v * hat_ro) + ((2 - norm * std::cos(norm / 2) / 2 / std::sin(norm / 2)) / 2 / norm / norm / norm / norm - 1 / 8 / norm / norm / std::sin(norm / 2) / std::sin(norm / 2)) * hat_v * (hat_ro * hat_v + hat_v * hat_ro) * hat_v) * jac_v; + } + else + { + res.block<3,3>(i*3, 0) = -0.5 * jac_v * hat_ro * jac_v; + } + + } + // return res; + } + + void S2_hat(Eigen::Matrix &res) + { + std::cerr << "wrong idx for S2" << std::endl; + res = Eigen::Matrix::Zero(); + } + void S2_Nx_yy(Eigen::Matrix &res) + { + std::cerr << "wrong idx for S2" << std::endl; + std::exit(100); + res = Eigen::Matrix::Zero(); + } + + void S2_Mx(Eigen::Matrix &res, MTK::vectview delta) + { + std::cerr << "wrong idx for S2" << std::endl; + std::exit(100); + res = Eigen::Matrix::Zero(); + } + + friend std::ostream& operator<<(std::ostream &os, const SEN& q){ + for(int i=0; i>(std::istream &is, SEN& q){ + // vect coeffs; + for(int i=0; i> q.mat(i, j); + } + } + // is >> q.mat; + // coeffs; + // q.coeffs() = coeffs.normalized(); + return is; + } + + //! @name Helper functions + //{ + /** + * Calculate the exponential map. In matrix terms this would correspond + * to the Rodrigues formula. + */ + // FIXME vectview<> can't be constructed from every MatrixBase<>, use const Vector3x& as workaround +// static SO3 exp(MTK::vectview dvec, scalar scale = 1){ + static SEN exp(const Eigen::Matrix& dvec, scalar scale = 1){ + SEN res; + res.mat = Eigen::Matrix::Identity(); + Eigen::Matrix exp_; //, jac; + Eigen::MatrixXd jac; + Eigen::Matrix psi; + Eigen::VectorXd minus_psi; + psi = dvec.template block<3,1>(0, 0); + minus_psi = -psi; + SO3_type SO3_temp; + exp_ = SO3_type::exp(psi); + SO3_temp.Jacob_right(minus_psi, jac); + res.mat.template block<3,3>(0, 0) = exp_; + for(int i = 3; i < DOF / 3 + 2; i++) + { + res.mat.template block<3, 1>(0, i) = jac * dvec.template block<3,1>(i + (i-3)*3,0); + } + return res; + } + /** + * Calculate the inverse of @c exp. + * Only guarantees that exp(log(x)) == x + */ + static Eigen::Matrix log(base &orient){ + Eigen::Matrix res; + Eigen::Matrix psi; + Eigen::VectorXd minus_psi; + Eigen::Matrix mat_psi; + Eigen::MatrixXd jac; + mat_psi = orient.template block<3, 3>(0, 0); + SO3_type SO3_temp; + SO3_type exp_psi(mat_psi); + psi = SO3_type::log(exp_psi); + minus_psi = -psi; + SO3_temp.Jacob_right_inv(minus_psi, jac); + for(int i = 3; i < dim_of_mat; i++) + { + res.template block<3,1>(i + (i-3)*3,0) = jac * orient.template block<3, 1>(0, i); + } + return res; + } +}; + + +} // namespace MTK + +#endif /*SON_H_*/ + diff --git a/src/Point-LIO/include/IKFoM/IKFoM_toolkit/mtk/types/SOn.hpp b/src/Point-LIO/include/IKFoM/IKFoM_toolkit/mtk/types/SOn.hpp new file mode 100755 index 0000000..d6ac674 --- /dev/null +++ b/src/Point-LIO/include/IKFoM/IKFoM_toolkit/mtk/types/SOn.hpp @@ -0,0 +1,360 @@ +// This is an advanced implementation of the algorithm described in the +// following paper: +// C. Hertzberg, R. Wagner, U. Frese, and L. Schroder. Integratinggeneric sensor fusion algorithms with sound state representationsthrough encapsulation of manifolds. +// CoRR, vol. abs/1107.1119, 2011.[Online]. Available: http://arxiv.org/abs/1107.1119 + +/* + * Copyright (c) 2019--2023, The University of Hong Kong + * All rights reserved. + * + * Modifier: Dongjiao HE + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the Universitaet Bremen nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/* + * Copyright (c) 2008--2011, Universitaet Bremen + * All rights reserved. + * + * Author: Christoph Hertzberg + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the Universitaet Bremen nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ +/** + * @file mtk/types/SOn.hpp + * @brief Standard Orthogonal Groups i.e.\ rotatation groups. + */ +#ifndef SON_H_ +#define SON_H_ + +#include + +#include "vect.hpp" +#include "../src/mtkmath.hpp" + + +namespace MTK { + + +/** + * Two-dimensional orientations represented as scalar. + * There is no guarantee that the representing scalar is within any interval, + * but the result of boxminus will always have magnitude @f$\le\pi @f$. + */ +template +struct SO2 : public Eigen::Rotation2D<_scalar> { + enum {DOF = 1, DIM = 2, TYP = 3}; + + typedef _scalar scalar; + typedef Eigen::Rotation2D base; + typedef vect vect_type; + + //! Construct from angle + SO2(const scalar& angle = 0) : base(angle) { } + + //! Construct from Eigen::Rotation2D + SO2(const base& src) : base(src) {} + + /** + * Construct from 2D vector. + * Resulting orientation will rotate the first unit vector to point to vec. + */ + SO2(const vect_type &vec) : base(atan2(vec[1], vec[0])) {}; + + + //! Calculate @c this->inverse() * @c r + SO2 operator%(const base &r) const { + return base::inverse() * r; + } + + //! Calculate @c this->inverse() * @c r + template + vect_type operator%(const Eigen::MatrixBase &vec) const { + return base::inverse() * vec; + } + + //! Calculate @c *this * @c r.inverse() + SO2 operator/(const SO2 &r) const { + return *this * r.inverse(); + } + + //! Gets the angle as scalar. + operator scalar() const { + return base::angle(); + } + void S2_hat(Eigen::Matrix &res) + { + res = Eigen::Matrix::Zero(); + } + //! @name Manifold requirements + void S2_Nx_yy(Eigen::Matrix &res) + { + std::cerr << "wrong idx for S2" << std::endl; + std::exit(100); + res = Eigen::Matrix::Zero(); + } + + void S2_Mx(Eigen::Matrix &res, MTK::vectview delta) + { + std::cerr << "wrong idx for S2" << std::endl; + std::exit(100); + res = Eigen::Matrix::Zero(); + } + + void oplus(MTK::vectview vec, scalar scale = 1) { + base::angle() += scale * vec[0]; + } + + void boxplus(MTK::vectview vec, scalar scale = 1) { + base::angle() += scale * vec[0]; + } + void boxminus(MTK::vectview res, const SO2& other) const { + res[0] = MTK::normalize(base::angle() - other.angle(), scalar(MTK::pi)); + } + + friend std::istream& operator>>(std::istream &is, SO2& ang){ + return is >> ang.angle(); + } + void hat(Eigen::VectorXd& v, Eigen::MatrixXd &res) { + std::cout << "wrong idx" << std::endl; + } + void Jacob_right_inv(Eigen::VectorXd& v, Eigen::MatrixXd &res) { + std::cout << "wrong idx" << std::endl; + } + void Jacob_right(Eigen::VectorXd& v, Eigen::MatrixXd &res) { + std::cout << "wrong idx" << std::endl; + } +}; + + +/** + * Three-dimensional orientations represented as Quaternion. + * It is assumed that the internal Quaternion always stays normalized, + * should this not be the case, call inherited member function @c normalize(). + */ +template //, int Options = Eigen::AutoAlign> +struct SO3 : public Eigen::Matrix<_scalar, 3, 3> { + enum {DOF = 3, DIM = 3, TYP = 2}; + typedef _scalar scalar; + typedef Eigen::Matrix base; + typedef Eigen::Matrix Matrix; + typedef vect vect_type; + + /** + * Construct from Eigen::Quaternion. + * @note Non-normalized input may result result in spurious behavior. + */ + SO3(const base& src = base::Identity()) : base(src) {} + + /** + * Construct from rotation matrix. + * @note Invalid rotation matrices may lead to spurious behavior. + */ + template + SO3(const Eigen::MatrixBase& matrix) : base(matrix) {} + + /** + * Construct from arbitrary rotation type. + * @note Invalid rotation matrices may lead to spurious behavior. + */ + // template + // SO3(const Eigen::RotationBase& rotation) : base(rotation.derived()) {} + + //! @name Manifold requirements + + // SO3 operator=(const base &r) const { + // return r; + // } + + // //! Calculate @c *this * @c r.inverse() + // SO3 operator*(const SO3 &r) const { + // return *this * r; + // } + + void boxplus(MTK::vectview vec, scalar scale=1) { + SO3 delta = exp(vec, scale); + *this = *this * delta; + } + void boxminus(MTK::vectview res, const SO3& other) const { + res = SO3::log(other.transpose() * *this); + } + //} + + void oplus(MTK::vectview vec, scalar scale=1) { + SO3 delta = exp(vec, scale); + *this = *this * delta; + } + + // void hat(MTK::vectview& v, Eigen::Matrix &res) { + void hat(Eigen::VectorXd& v, Eigen::MatrixXd &res) { + // Eigen::Matrix res; + res << 0, -v[2], v[1], + v[2], 0, -v[0], + -v[1], v[0], 0; + // return res; + } + + // void Jacob_right_inv(MTK::vectview vec, Eigen::Matrix & res){ + void Jacob_right_inv(Eigen::VectorXd& vec, Eigen::MatrixXd &res){ + Eigen::MatrixXd hat_v; + hat(vec, hat_v); + if(vec.norm() > MTK::tolerance()) + { + res = Eigen::Matrix::Identity() + 0.5 * hat_v + (1 - vec.norm() * std::cos(vec.norm() / 2) / 2 / std::sin(vec.norm() / 2)) * hat_v * hat_v / vec.squaredNorm(); + } + else + { + res = Eigen::Matrix::Identity(); + } + // return res; + } + + // void Jacob_right(MTK::vectview & v, Eigen::Matrix &res){ + void Jacob_right(Eigen::VectorXd& v, Eigen::MatrixXd &res){ + Eigen::MatrixXd hat_v; + hat(v, hat_v); + double squaredNorm = v[0] * v[0] + v[1] * v[1] + v[2] * v[2]; + double norm = std::sqrt(squaredNorm); + if(norm < MTK::tolerance()){ + res = Eigen::Matrix::Identity(); + } + else{ + res = Eigen::Matrix::Identity() - (1 - std::cos(norm)) / squaredNorm * hat_v + (1 - std::sin(norm) / norm) / squaredNorm * hat_v * hat_v; + } + // return res; + } + + void S2_hat(Eigen::Matrix &res) + { + res = Eigen::Matrix::Zero(); + } + void S2_Nx_yy(Eigen::Matrix &res) + { + std::cerr << "wrong idx for S2" << std::endl; + std::exit(100); + res = Eigen::Matrix::Zero(); + } + + void S2_Mx(Eigen::Matrix &res, MTK::vectview delta) + { + std::cerr << "wrong idx for S2" << std::endl; + std::exit(100); + res = Eigen::Matrix::Zero(); + } + + friend std::ostream& operator<<(std::ostream &os, const SO3& q){ // wrong! + return os << q.data() << " "; + } + + friend std::istream& operator>>(std::istream &is, SO3& q){ // wrong! + SO3 coeffs; + is >> coeffs; + q = coeffs; + return is; + } + + //! @name Helper functions + //{ + /** + * Calculate the exponential map. In matrix terms this would correspond + * to the Rodrigues formula. + */ + // FIXME vectview<> can't be constructed from every MatrixBase<>, use const Vector3x& as workaround +// static SO3 exp(MTK::vectview dvec, scalar scale = 1){ + static SO3 exp(const Eigen::Matrix& dvec, scalar scale = 1){ + Eigen::Matrix ang = dvec * scale; + double ang_norm = ang.norm(); + Eigen::Matrix Eye3 = Eigen::Matrix::Identity(); + if (ang_norm > 0.0000001) + { + Eigen::Matrix r_axis = ang / ang_norm; + Eigen::Matrix K; + K << 0.0, -r_axis(2), r_axis(1), + r_axis(2), 0.0, -r_axis(0), + -r_axis(1), r_axis(0), 0.0; // SKEW_SYM_MATRX(r_axis); + /// Roderigous Tranformation + return Eye3 + std::sin(ang_norm) * K + (1.0 - std::cos(ang_norm)) * K * K; + } + else + { + return Eye3; + } + } + /** + * Calculate the inverse of @c exp. + * Only guarantees that exp(log(x)) == x + */ + static Eigen::Vector3d log(const SO3 &orient){ + double theta = (orient.trace() > 3.0 - 1e-6) ? 0.0 : std::acos(0.5 * (orient.trace() - 1)); + Eigen::Matrix K(orient(2,1) - orient(1,2), orient(0,2) - orient(2,0), orient(1,0) - orient(0,1)); + return (std::abs(theta) < 0.001) ? (0.5 * K) : (0.5 * theta / std::sin(theta) * K); + } +}; + +namespace internal { +template +struct UnalignedType >{ + typedef SO2 type; +}; + +template +struct UnalignedType >{ + typedef SO3 type; +}; + +} // namespace internal + + +} // namespace MTK + +#endif /*SON_H_*/ \ No newline at end of file diff --git a/src/Point-LIO/include/IKFoM/IKFoM_toolkit/mtk/types/vect.hpp b/src/Point-LIO/include/IKFoM/IKFoM_toolkit/mtk/types/vect.hpp new file mode 100755 index 0000000..d75e6d7 --- /dev/null +++ b/src/Point-LIO/include/IKFoM/IKFoM_toolkit/mtk/types/vect.hpp @@ -0,0 +1,511 @@ +// This is an advanced implementation of the algorithm described in the +// following paper: +// C. Hertzberg, R. Wagner, U. Frese, and L. Schroder. Integratinggeneric sensor fusion algorithms with sound state representationsthrough encapsulation of manifolds. +// CoRR, vol. abs/1107.1119, 2011.[Online]. Available: http://arxiv.org/abs/1107.1119 + +/* + * Copyright (c) 2019--2023, The University of Hong Kong + * All rights reserved. + * + * Modifier: Dongjiao HE + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the Universitaet Bremen nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/* + * Copyright (c) 2008--2011, Universitaet Bremen + * All rights reserved. + * + * Author: Christoph Hertzberg + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the Universitaet Bremen nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ +/** + * @file mtk/types/vect.hpp + * @brief Basic vectors interpreted as manifolds. + * + * This file also implements a simple wrapper for matrices, for arbitrary scalars + * and for positive scalars. + */ +#ifndef VECT_H_ +#define VECT_H_ + +#include +#include +#include + +#include "../src/vectview.hpp" + +namespace MTK { + +static const Eigen::IOFormat IO_no_spaces(Eigen::StreamPrecision, Eigen::DontAlignCols, ",", ",", "", "", "[", "]"); + + +/** + * A simple vector class. + * Implementation is basically a wrapper around Eigen::Matrix with manifold + * requirements added. + */ +template +struct vect : public Eigen::Matrix<_scalar, D, 1, _Options> { + typedef Eigen::Matrix<_scalar, D, 1, _Options> base; + enum {DOF = D, DIM = D, TYP = 0}; + typedef _scalar scalar; + + //using base::operator=; + + /** Standard constructor. Sets all values to zero. */ + vect(const base &src = base::Zero()) : base(src) {} + + /** Constructor copying the value of the expression \a other */ + template + EIGEN_STRONG_INLINE vect(const Eigen::DenseBase& other) : base(other) {} + + /** Construct from memory. */ + vect(const scalar* src, int size = DOF) : base(base::Map(src, size)) { } + + void boxplus(MTK::vectview vec, scalar scale=1) { + *this += scale * vec; + } + void boxminus(MTK::vectview res, const vect& other) const { + res = *this - other; + } + + void oplus(MTK::vectview vec, scalar scale=1) { + *this += scale * vec; + } + + void hat(Eigen::VectorXd& v, Eigen::MatrixXd &res) { + std::cout << "wrong idx" << std::endl; + } + void Jacob_right_inv(Eigen::VectorXd& v, Eigen::MatrixXd &res) { + std::cout << "wrong idx" << std::endl; + } + void Jacob_right(Eigen::VectorXd& v, Eigen::MatrixXd &res) { + std::cout << "wrong idx" << std::endl; + } + + void S2_hat(Eigen::Matrix &res) + { + res = Eigen::Matrix::Zero(); + } + + void S2_Nx_yy(Eigen::Matrix &res) + { + std::cerr << "wrong idx for S2" << std::endl; + std::exit(100); + res = Eigen::Matrix::Zero(); + } + + void S2_Mx(Eigen::Matrix &res, MTK::vectview delta) + { + std::cerr << "wrong idx for S2" << std::endl; + std::exit(100); + res = Eigen::Matrix::Zero(); + } + + friend std::ostream& operator<<(std::ostream &os, const vect& v){ + // Eigen sometimes messes with the streams flags, so output manually: + for(int i=0; i>(std::istream &is, vect& v){ + char term=0; + is >> std::ws; // skip whitespace + switch(is.peek()) { + case '(': term=')'; is.ignore(1); break; + case '[': term=']'; is.ignore(1); break; + case '{': term='}'; is.ignore(1); break; + default: break; + } + if(D==Eigen::Dynamic) { + assert(term !=0 && "Dynamic vectors must be embraced"); + std::vector temp; + while(is.good() && is.peek() != term) { + scalar x; + is >> x; + temp.push_back(x); + if(is.peek()==',') is.ignore(1); + } + v = vect::Map(temp.data(), temp.size()); + } else + for(int i=0; i> v[i]; + if(is.peek()==',') { // ignore commas between values + is.ignore(1); + } + } + if(term!=0) { + char x; + is >> x; + if(x!=term) { + is.setstate(is.badbit); +// assert(x==term && "start and end bracket do not match!"); + } + } + return is; + } + + template + vectview tail(){ + BOOST_STATIC_ASSERT(0< dim && dim <= DOF); + return base::template tail(); + } + template + vectview tail() const{ + BOOST_STATIC_ASSERT(0< dim && dim <= DOF); + return base::template tail(); + } + template + vectview head(){ + BOOST_STATIC_ASSERT(0< dim && dim <= DOF); + return base::template head(); + } + template + vectview head() const{ + BOOST_STATIC_ASSERT(0< dim && dim <= DOF); + return base::template head(); + } +}; + + +/** + * A simple matrix class. + * Implementation is basically a wrapper around Eigen::Matrix with manifold + * requirements added, i.e., matrix is viewed as a plain vector for that. + */ +template::Options> +struct matrix : public Eigen::Matrix<_scalar, M, N, _Options> { + typedef Eigen::Matrix<_scalar, M, N, _Options> base; + enum {DOF = M * N, TYP = 4, DIM=0}; + typedef _scalar scalar; + + using base::operator=; + + /** Standard constructor. Sets all values to zero. */ + matrix() { + base::setZero(); + } + + /** Constructor copying the value of the expression \a other */ + template + EIGEN_STRONG_INLINE matrix(const Eigen::MatrixBase& other) : base(other) {} + + /** Construct from memory. */ + matrix(const scalar* src) : base(src) { } + + void boxplus(MTK::vectview vec, scalar scale = 1) { + *this += scale * base::Map(vec.data()); + } + void boxminus(MTK::vectview res, const matrix& other) const { + base::Map(res.data()) = *this - other; + } + + void hat(Eigen::VectorXd& v, Eigen::MatrixXd &res) { + std::cout << "wrong idx" << std::endl; + } + void Jacob_right_inv(Eigen::VectorXd& v, Eigen::MatrixXd &res) { + std::cout << "wrong idx" << std::endl; + } + void Jacob_right(Eigen::VectorXd& v, Eigen::MatrixXd &res) { + std::cout << "wrong idx" << std::endl; + } + + void S2_hat(Eigen::Matrix &res) + { + res = Eigen::Matrix::Zero(); + } + + void oplus(MTK::vectview vec, scalar scale = 1) { + *this += scale * base::Map(vec.data()); + } + + void S2_Nx_yy(Eigen::Matrix &res) + { + std::cerr << "wrong idx for S2" << std::endl; + std::exit(100); + res = Eigen::Matrix::Zero(); + } + + void S2_Mx(Eigen::Matrix &res, MTK::vectview delta) + { + std::cerr << "wrong idx for S2" << std::endl; + std::exit(100); + res = Eigen::Matrix::Zero(); + } + + friend std::ostream& operator<<(std::ostream &os, const matrix& mat){ + for(int i=0; i>(std::istream &is, matrix& mat){ + for(int i=0; i> mat.data()[i]; + } + return is; + } +};// @todo What if M / N = Eigen::Dynamic? + + + +/** + * A simple scalar type. + */ +template +struct Scalar { + enum {DOF = 1, TYP = 5, DIM=0}; + typedef _scalar scalar; + + scalar value; + + Scalar(const scalar& value = scalar(0)) : value(value) {} + operator const scalar&() const { return value; } + operator scalar&() { return value; } + Scalar& operator=(const scalar& val) { value = val; return *this; } + + void S2_hat(Eigen::Matrix &res) + { + res = Eigen::Matrix::Zero(); + } + + void S2_Nx_yy(Eigen::Matrix &res) + { + std::cerr << "wrong idx for S2" << std::endl; + std::exit(100); + res = Eigen::Matrix::Zero(); + } + + void S2_Mx(Eigen::Matrix &res, MTK::vectview delta) + { + std::cerr << "wrong idx for S2" << std::endl; + std::exit(100); + res = Eigen::Matrix::Zero(); + } + + void oplus(MTK::vectview vec, scalar scale=1) { + value += scale * vec[0]; + } + + void boxplus(MTK::vectview vec, scalar scale=1) { + value += scale * vec[0]; + } + void boxminus(MTK::vectview res, const Scalar& other) const { + res[0] = *this - other; + } + + void hat(Eigen::VectorXd& v, Eigen::MatrixXd &res) { + std::cout << "wrong idx" << std::endl; + } + void Jacob_right_inv(Eigen::VectorXd& v, Eigen::MatrixXd &res) { + std::cout << "wrong idx" << std::endl; + } + void Jacob_right(Eigen::VectorXd& v, Eigen::MatrixXd &res) { + std::cout << "wrong idx" << std::endl; + } +}; + +/** + * Positive scalars. + * Boxplus is implemented using multiplication by @f$x\boxplus\delta = x\cdot\exp(\delta) @f$. + */ +template +struct PositiveScalar { + enum {DOF = 1, TYP = 6, DIM=0}; + typedef _scalar scalar; + + scalar value; + + PositiveScalar(const scalar& value = scalar(1)) : value(value) { + assert(value > scalar(0)); + } + operator const scalar&() const { return value; } + PositiveScalar& operator=(const scalar& val) { assert(val>0); value = val; return *this; } + + void boxplus(MTK::vectview vec, scalar scale = 1) { + value *= std::exp(scale * vec[0]); + } + void boxminus(MTK::vectview res, const PositiveScalar& other) const { + res[0] = std::log(*this / other); + } + + void oplus(MTK::vectview vec, scalar scale = 1) { + value *= std::exp(scale * vec[0]); + } + + void hat(Eigen::VectorXd& v, Eigen::MatrixXd &res) { + std::cout << "wrong idx" << std::endl; + } + void Jacob_right_inv(Eigen::VectorXd& v, Eigen::MatrixXd &res) { + std::cout << "wrong idx" << std::endl; + } + void Jacob_right(Eigen::VectorXd& v, Eigen::MatrixXd &res) { + std::cout << "wrong idx" << std::endl; + } + + void S2_hat(Eigen::Matrix &res) + { + res = Eigen::Matrix::Zero(); + } + + void S2_Nx_yy(Eigen::Matrix &res) + { + std::cerr << "wrong idx for S2" << std::endl; + std::exit(100); + res = Eigen::Matrix::Zero(); + } + + void S2_Mx(Eigen::Matrix &res, MTK::vectview delta) + { + std::cerr << "wrong idx for S2" << std::endl; + std::exit(100); + res = Eigen::Matrix::Zero(); + } + + + friend std::istream& operator>>(std::istream &is, PositiveScalar& s){ + is >> s.value; + assert(s.value > 0); + return is; + } +}; + +template +struct Complex : public std::complex<_scalar>{ + enum {DOF = 2, TYP = 7, DIM=0}; + typedef _scalar scalar; + + typedef std::complex Base; + + Complex(const Base& value) : Base(value) {} + Complex(const scalar& re = 0.0, const scalar& im = 0.0) : Base(re, im) {} + Complex(const MTK::vectview &in) : Base(in[0], in[1]) {} + template + Complex(const Eigen::DenseBase &in) : Base(in[0], in[1]) {} + + void boxplus(MTK::vectview vec, scalar scale = 1) { + Base::real() += scale * vec[0]; + Base::imag() += scale * vec[1]; + }; + void boxminus(MTK::vectview res, const Complex& other) const { + Complex diff = *this - other; + res << diff.real(), diff.imag(); + } + + void S2_hat(Eigen::Matrix &res) + { + res = Eigen::Matrix::Zero(); + } + + void hat(Eigen::VectorXd& v, Eigen::MatrixXd &res) { + std::cout << "wrong idx" << std::endl; + } + void Jacob_right_inv(Eigen::VectorXd& v, Eigen::MatrixXd &res) { + std::cout << "wrong idx" << std::endl; + } + void Jacob_right(Eigen::VectorXd& v, Eigen::MatrixXd &res) { + std::cout << "wrong idx" << std::endl; + } + + void oplus(MTK::vectview vec, scalar scale = 1) { + Base::real() += scale * vec[0]; + Base::imag() += scale * vec[1]; + }; + + void S2_Nx_yy(Eigen::Matrix &res) + { + std::cerr << "wrong idx for S2" << std::endl; + std::exit(100); + res = Eigen::Matrix::Zero(); + } + + void S2_Mx(Eigen::Matrix &res, MTK::vectview delta) + { + std::cerr << "wrong idx for S2" << std::endl; + std::exit(100); + res = Eigen::Matrix::Zero(); + } + + scalar squaredNorm() const { + return std::pow(Base::real(),2) + std::pow(Base::imag(),2); + } + + const scalar& operator()(int i) const { + assert(0<=i && i<2 && "Index out of range"); + return i==0 ? Base::real() : Base::imag(); + } + scalar& operator()(int i){ + assert(0<=i && i<2 && "Index out of range"); + return i==0 ? Base::real() : Base::imag(); + } +}; + + +namespace internal { + +template +struct UnalignedType >{ + typedef vect type; +}; + +} // namespace internal + + +} // namespace MTK + + + + +#endif /*VECT_H_*/ diff --git a/src/Point-LIO/include/IKFoM/IKFoM_toolkit/mtk/types/wrapped_cv_mat.hpp b/src/Point-LIO/include/IKFoM/IKFoM_toolkit/mtk/types/wrapped_cv_mat.hpp new file mode 100755 index 0000000..b6643f1 --- /dev/null +++ b/src/Point-LIO/include/IKFoM/IKFoM_toolkit/mtk/types/wrapped_cv_mat.hpp @@ -0,0 +1,113 @@ +/* + * Copyright (c) 2010--2011, Universitaet Bremen and DFKI GmbH + * All rights reserved. + * + * Author: Rene Wagner + * Christoph Hertzberg + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the Universitaet Bremen nor the DFKI GmbH + * nor the names of its contributors may be used to endorse or + * promote products derived from this software without specific + * prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#ifndef WRAPPED_CV_MAT_HPP_ +#define WRAPPED_CV_MAT_HPP_ + +#include +#include + +namespace MTK { + +template +struct cv_f_type; + +template<> +struct cv_f_type +{ + enum {value = CV_64F}; +}; + +template<> +struct cv_f_type +{ + enum {value = CV_32F}; +}; + +/** + * cv_mat wraps a CvMat around an Eigen Matrix + */ +template +class cv_mat : public matrix +{ + typedef matrix base_type; + enum {type_ = cv_f_type::value}; + CvMat cv_mat_; + +public: + cv_mat() + { + cv_mat_ = cvMat(rows, cols, type_, base_type::data()); + } + + cv_mat(const cv_mat& oth) : base_type(oth) + { + cv_mat_ = cvMat(rows, cols, type_, base_type::data()); + } + + template + cv_mat(const Eigen::MatrixBase &value) : base_type(value) + { + cv_mat_ = cvMat(rows, cols, type_, base_type::data()); + } + + template + cv_mat& operator=(const Eigen::MatrixBase &value) + { + base_type::operator=(value); + return *this; + } + + cv_mat& operator=(const cv_mat& value) + { + base_type::operator=(value); + return *this; + } + + // FIXME: Maybe overloading operator& is not a good idea ... + CvMat* operator&() + { + return &cv_mat_; + } + const CvMat* operator&() const + { + return &cv_mat_; + } +}; + +} // namespace MTK + +#endif /* WRAPPED_CV_MAT_HPP_ */ diff --git a/src/Point-LIO/include/IKFoM/LICENSE b/src/Point-LIO/include/IKFoM/LICENSE new file mode 100644 index 0000000..d159169 --- /dev/null +++ b/src/Point-LIO/include/IKFoM/LICENSE @@ -0,0 +1,339 @@ + GNU GENERAL PUBLIC LICENSE + Version 2, June 1991 + + Copyright (C) 1989, 1991 Free Software Foundation, Inc., + 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA + Everyone is permitted to copy and distribute verbatim copies + of this license document, but changing it is not allowed. + + Preamble + + The licenses for most software are designed to take away your +freedom to share and change it. By contrast, the GNU General Public +License is intended to guarantee your freedom to share and change free +software--to make sure the software is free for all its users. This +General Public License applies to most of the Free Software +Foundation's software and to any other program whose authors commit to +using it. (Some other Free Software Foundation software is covered by +the GNU Lesser General Public License instead.) You can apply it to +your programs, too. + + When we speak of free software, we are referring to freedom, not +price. Our General Public Licenses are designed to make sure that you +have the freedom to distribute copies of free software (and charge for +this service if you wish), that you receive source code or can get it +if you want it, that you can change the software or use pieces of it +in new free programs; and that you know you can do these things. + + To protect your rights, we need to make restrictions that forbid +anyone to deny you these rights or to ask you to surrender the rights. +These restrictions translate to certain responsibilities for you if you +distribute copies of the software, or if you modify it. + + For example, if you distribute copies of such a program, whether +gratis or for a fee, you must give the recipients all the rights that +you have. You must make sure that they, too, receive or can get the +source code. And you must show them these terms so they know their +rights. + + We protect your rights with two steps: (1) copyright the software, and +(2) offer you this license which gives you legal permission to copy, +distribute and/or modify the software. + + Also, for each author's protection and ours, we want to make certain +that everyone understands that there is no warranty for this free +software. If the software is modified by someone else and passed on, we +want its recipients to know that what they have is not the original, so +that any problems introduced by others will not reflect on the original +authors' reputations. + + Finally, any free program is threatened constantly by software +patents. We wish to avoid the danger that redistributors of a free +program will individually obtain patent licenses, in effect making the +program proprietary. To prevent this, we have made it clear that any +patent must be licensed for everyone's free use or not licensed at all. + + The precise terms and conditions for copying, distribution and +modification follow. + + GNU GENERAL PUBLIC LICENSE + TERMS AND CONDITIONS FOR COPYING, DISTRIBUTION AND MODIFICATION + + 0. This License applies to any program or other work which contains +a notice placed by the copyright holder saying it may be distributed +under the terms of this General Public License. The "Program", below, +refers to any such program or work, and a "work based on the Program" +means either the Program or any derivative work under copyright law: +that is to say, a work containing the Program or a portion of it, +either verbatim or with modifications and/or translated into another +language. (Hereinafter, translation is included without limitation in +the term "modification".) Each licensee is addressed as "you". + +Activities other than copying, distribution and modification are not +covered by this License; they are outside its scope. The act of +running the Program is not restricted, and the output from the Program +is covered only if its contents constitute a work based on the +Program (independent of having been made by running the Program). +Whether that is true depends on what the Program does. + + 1. You may copy and distribute verbatim copies of the Program's +source code as you receive it, in any medium, provided that you +conspicuously and appropriately publish on each copy an appropriate +copyright notice and disclaimer of warranty; keep intact all the +notices that refer to this License and to the absence of any warranty; +and give any other recipients of the Program a copy of this License +along with the Program. + +You may charge a fee for the physical act of transferring a copy, and +you may at your option offer warranty protection in exchange for a fee. + + 2. You may modify your copy or copies of the Program or any portion +of it, thus forming a work based on the Program, and copy and +distribute such modifications or work under the terms of Section 1 +above, provided that you also meet all of these conditions: + + a) You must cause the modified files to carry prominent notices + stating that you changed the files and the date of any change. + + b) You must cause any work that you distribute or publish, that in + whole or in part contains or is derived from the Program or any + part thereof, to be licensed as a whole at no charge to all third + parties under the terms of this License. + + c) If the modified program normally reads commands interactively + when run, you must cause it, when started running for such + interactive use in the most ordinary way, to print or display an + announcement including an appropriate copyright notice and a + notice that there is no warranty (or else, saying that you provide + a warranty) and that users may redistribute the program under + these conditions, and telling the user how to view a copy of this + License. (Exception: if the Program itself is interactive but + does not normally print such an announcement, your work based on + the Program is not required to print an announcement.) + +These requirements apply to the modified work as a whole. If +identifiable sections of that work are not derived from the Program, +and can be reasonably considered independent and separate works in +themselves, then this License, and its terms, do not apply to those +sections when you distribute them as separate works. But when you +distribute the same sections as part of a whole which is a work based +on the Program, the distribution of the whole must be on the terms of +this License, whose permissions for other licensees extend to the +entire whole, and thus to each and every part regardless of who wrote it. + +Thus, it is not the intent of this section to claim rights or contest +your rights to work written entirely by you; rather, the intent is to +exercise the right to control the distribution of derivative or +collective works based on the Program. + +In addition, mere aggregation of another work not based on the Program +with the Program (or with a work based on the Program) on a volume of +a storage or distribution medium does not bring the other work under +the scope of this License. + + 3. You may copy and distribute the Program (or a work based on it, +under Section 2) in object code or executable form under the terms of +Sections 1 and 2 above provided that you also do one of the following: + + a) Accompany it with the complete corresponding machine-readable + source code, which must be distributed under the terms of Sections + 1 and 2 above on a medium customarily used for software interchange; or, + + b) Accompany it with a written offer, valid for at least three + years, to give any third party, for a charge no more than your + cost of physically performing source distribution, a complete + machine-readable copy of the corresponding source code, to be + distributed under the terms of Sections 1 and 2 above on a medium + customarily used for software interchange; or, + + c) Accompany it with the information you received as to the offer + to distribute corresponding source code. (This alternative is + allowed only for noncommercial distribution and only if you + received the program in object code or executable form with such + an offer, in accord with Subsection b above.) + +The source code for a work means the preferred form of the work for +making modifications to it. For an executable work, complete source +code means all the source code for all modules it contains, plus any +associated interface definition files, plus the scripts used to +control compilation and installation of the executable. However, as a +special exception, the source code distributed need not include +anything that is normally distributed (in either source or binary +form) with the major components (compiler, kernel, and so on) of the +operating system on which the executable runs, unless that component +itself accompanies the executable. + +If distribution of executable or object code is made by offering +access to copy from a designated place, then offering equivalent +access to copy the source code from the same place counts as +distribution of the source code, even though third parties are not +compelled to copy the source along with the object code. + + 4. You may not copy, modify, sublicense, or distribute the Program +except as expressly provided under this License. Any attempt +otherwise to copy, modify, sublicense or distribute the Program is +void, and will automatically terminate your rights under this License. +However, parties who have received copies, or rights, from you under +this License will not have their licenses terminated so long as such +parties remain in full compliance. + + 5. You are not required to accept this License, since you have not +signed it. However, nothing else grants you permission to modify or +distribute the Program or its derivative works. These actions are +prohibited by law if you do not accept this License. Therefore, by +modifying or distributing the Program (or any work based on the +Program), you indicate your acceptance of this License to do so, and +all its terms and conditions for copying, distributing or modifying +the Program or works based on it. + + 6. Each time you redistribute the Program (or any work based on the +Program), the recipient automatically receives a license from the +original licensor to copy, distribute or modify the Program subject to +these terms and conditions. You may not impose any further +restrictions on the recipients' exercise of the rights granted herein. +You are not responsible for enforcing compliance by third parties to +this License. + + 7. If, as a consequence of a court judgment or allegation of patent +infringement or for any other reason (not limited to patent issues), +conditions are imposed on you (whether by court order, agreement or +otherwise) that contradict the conditions of this License, they do not +excuse you from the conditions of this License. If you cannot +distribute so as to satisfy simultaneously your obligations under this +License and any other pertinent obligations, then as a consequence you +may not distribute the Program at all. For example, if a patent +license would not permit royalty-free redistribution of the Program by +all those who receive copies directly or indirectly through you, then +the only way you could satisfy both it and this License would be to +refrain entirely from distribution of the Program. + +If any portion of this section is held invalid or unenforceable under +any particular circumstance, the balance of the section is intended to +apply and the section as a whole is intended to apply in other +circumstances. + +It is not the purpose of this section to induce you to infringe any +patents or other property right claims or to contest validity of any +such claims; this section has the sole purpose of protecting the +integrity of the free software distribution system, which is +implemented by public license practices. Many people have made +generous contributions to the wide range of software distributed +through that system in reliance on consistent application of that +system; it is up to the author/donor to decide if he or she is willing +to distribute software through any other system and a licensee cannot +impose that choice. + +This section is intended to make thoroughly clear what is believed to +be a consequence of the rest of this License. + + 8. If the distribution and/or use of the Program is restricted in +certain countries either by patents or by copyrighted interfaces, the +original copyright holder who places the Program under this License +may add an explicit geographical distribution limitation excluding +those countries, so that distribution is permitted only in or among +countries not thus excluded. In such case, this License incorporates +the limitation as if written in the body of this License. + + 9. The Free Software Foundation may publish revised and/or new versions +of the General Public License from time to time. Such new versions will +be similar in spirit to the present version, but may differ in detail to +address new problems or concerns. + +Each version is given a distinguishing version number. If the Program +specifies a version number of this License which applies to it and "any +later version", you have the option of following the terms and conditions +either of that version or of any later version published by the Free +Software Foundation. If the Program does not specify a version number of +this License, you may choose any version ever published by the Free Software +Foundation. + + 10. If you wish to incorporate parts of the Program into other free +programs whose distribution conditions are different, write to the author +to ask for permission. For software which is copyrighted by the Free +Software Foundation, write to the Free Software Foundation; we sometimes +make exceptions for this. Our decision will be guided by the two goals +of preserving the free status of all derivatives of our free software and +of promoting the sharing and reuse of software generally. + + NO WARRANTY + + 11. BECAUSE THE PROGRAM IS LICENSED FREE OF CHARGE, THERE IS NO WARRANTY +FOR THE PROGRAM, TO THE EXTENT PERMITTED BY APPLICABLE LAW. EXCEPT WHEN +OTHERWISE STATED IN WRITING THE COPYRIGHT HOLDERS AND/OR OTHER PARTIES +PROVIDE THE PROGRAM "AS IS" WITHOUT WARRANTY OF ANY KIND, EITHER EXPRESSED +OR IMPLIED, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF +MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE. THE ENTIRE RISK AS +TO THE QUALITY AND PERFORMANCE OF THE PROGRAM IS WITH YOU. SHOULD THE +PROGRAM PROVE DEFECTIVE, YOU ASSUME THE COST OF ALL NECESSARY SERVICING, +REPAIR OR CORRECTION. + + 12. IN NO EVENT UNLESS REQUIRED BY APPLICABLE LAW OR AGREED TO IN WRITING +WILL ANY COPYRIGHT HOLDER, OR ANY OTHER PARTY WHO MAY MODIFY AND/OR +REDISTRIBUTE THE PROGRAM AS PERMITTED ABOVE, BE LIABLE TO YOU FOR DAMAGES, +INCLUDING ANY GENERAL, SPECIAL, INCIDENTAL OR CONSEQUENTIAL DAMAGES ARISING +OUT OF THE USE OR INABILITY TO USE THE PROGRAM (INCLUDING BUT NOT LIMITED +TO LOSS OF DATA OR DATA BEING RENDERED INACCURATE OR LOSSES SUSTAINED BY +YOU OR THIRD PARTIES OR A FAILURE OF THE PROGRAM TO OPERATE WITH ANY OTHER +PROGRAMS), EVEN IF SUCH HOLDER OR OTHER PARTY HAS BEEN ADVISED OF THE +POSSIBILITY OF SUCH DAMAGES. + + END OF TERMS AND CONDITIONS + + How to Apply These Terms to Your New Programs + + If you develop a new program, and you want it to be of the greatest +possible use to the public, the best way to achieve this is to make it +free software which everyone can redistribute and change under these terms. + + To do so, attach the following notices to the program. It is safest +to attach them to the start of each source file to most effectively +convey the exclusion of warranty; and each file should have at least +the "copyright" line and a pointer to where the full notice is found. + + + Copyright (C) + + This program is free software; you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation; either version 2 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License along + with this program; if not, write to the Free Software Foundation, Inc., + 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA. + +Also add information on how to contact you by electronic and paper mail. + +If the program is interactive, make it output a short notice like this +when it starts in an interactive mode: + + Gnomovision version 69, Copyright (C) year name of author + Gnomovision comes with ABSOLUTELY NO WARRANTY; for details type `show w'. + This is free software, and you are welcome to redistribute it + under certain conditions; type `show c' for details. + +The hypothetical commands `show w' and `show c' should show the appropriate +parts of the General Public License. Of course, the commands you use may +be called something other than `show w' and `show c'; they could even be +mouse-clicks or menu items--whatever suits your program. + +You should also get your employer (if you work as a programmer) or your +school, if any, to sign a "copyright disclaimer" for the program, if +necessary. Here is a sample; alter the names: + + Yoyodyne, Inc., hereby disclaims all copyright interest in the program + `Gnomovision' (which makes passes at compilers) written by James Hacker. + + , 1 April 1989 + Ty Coon, President of Vice + +This General Public License does not permit incorporating your program into +proprietary programs. If your program is a subroutine library, you may +consider it more useful to permit linking proprietary applications with the +library. If this is what you want to do, use the GNU Lesser General +Public License instead of this License. diff --git a/src/Point-LIO/include/IKFoM/README.md b/src/Point-LIO/include/IKFoM/README.md new file mode 100644 index 0000000..53a7dc5 --- /dev/null +++ b/src/Point-LIO/include/IKFoM/README.md @@ -0,0 +1,489 @@ +## IKFoM +**IKFoM** (Iterated Kalman Filters on Manifolds) is a computationally efficient and convenient toolkit for deploying iterated Kalman filters on various robotic systems, especially systems operating on high-dimension manifold. It implements a manifold-embedding Kalman filter which separates the manifold structures from system descriptions and is able to be used by only defining the system in a canonical form and calling the respective steps accordingly. The current implementation supports the full iterated Kalman filtering for systems on manifold and any of its sub-manifolds, and it is extendable to other types of manifold when necessary. + + +**Developers** + +[Dongjiao He](https://github.com/Joanna-HE) + +**Our related video**: https://youtu.be/sz_ZlDkl6fA + +## 1. Prerequisites + +### 1.1. **Eigen && Boost** +Eigen >= 3.3.4, Follow [Eigen Installation](http://eigen.tuxfamily.org/index.php?title=Main_Page). + +Boost >= 1.65. + +## 2. Usage when the measurement is of constant dimension and type. +Clone the repository: + +``` + git clone https://github.com/hku-mars/IKFoM.git +``` + +1. include the necessary head file: +``` +#include +``` +2. Select and instantiate the primitive manifolds: +``` + typedef MTK::SO3 SO3; // scalar type of variable: double + typedef MTK::vect<3, double> vect3; // dimension of the defined Euclidean variable: 3 + typedef MTK::S2 S2; // length of the S2 variable: 98/10; choose e1 as the original point of rotation: 1 +``` +3. Build system state, input and measurement as compound manifolds which are composed of the primitive manifolds: +``` +MTK_BUILD_MANIFOLD(state, // name of compound manifold: state +((vect3, pos)) // ((primitive manifold type, name of variable)) +((vect3, vel)) +((SO3, rot)) +((vect3, bg)) +((vect3, ba)) +((S2, grav)) +((SO3, offset_R_L_I)) +((vect3, offset_T_L_I)) +); +``` +4. Implement the vector field that is defined as , and its differentiation , , where w=0 could be left out: +``` +Eigen::Matrix f(state &s, const input &i) { + Eigen::Matrix res = Eigen::Matrix::Zero(); + res(0) = s.vel[0]; + res(1) = s.vel[1]; + res(2) = s.vel[2]; + return res; +} +Eigen::Matrix df_dx(state &s, const input &i) //notice S2 has length of 3 and dimension of 2 { + Eigen::Matrix cov = Eigen::Matrix::Zero(); + cov.template block<3, 3>(0, 12) = Eigen::Matrix3d::Identity(); + return cov; +} +Eigen::Matrix df_dw(state &s, const input &i) { + Eigen::Matrix cov = Eigen::Matrix::Zero(); + cov.template block<3, 3>(12, 3) = -s.rot.toRotationMatrix(); + return cov; +} +``` +Those functions would be called during the ekf state predict + +5. Implement the output equation and its differentiation , : +``` +measurement h(state &s, bool &valid) // the iteration stops before convergence whenever the user set valid as false +{ + if (condition){ valid = false; + } // other conditions could be used to stop the ekf update iteration before convergence, otherwise the iteration will not stop until the condition of convergence is satisfied. + measurement h_; + h_.position = s.pos; + return h_; +} +Eigen::Matrix dh_dx(state &s) {} +Eigen::Matrix dh_dv(state &s) {} +``` +Those functions would be called during the ekf state update + +6. Instantiate an **esekf** object **kf** and initialize it with initial or default state and covariance. + +(1) initial state and covariance: +``` +state init_state; +esekfom::esekf::cov init_P; +esekfom::esekf kf(init_state,init_P); +``` +(2) default state and covariance: +``` +esekfom::esekf kf; +``` +where **process_noise_dof** is the dimension of process noise, with the type of std int, and so for **measurement_noise_dof**. + +7. Deliver the defined models, std int maximum iteration numbers **Maximum_iter**, and the std array for testing convergence **epsi** into the **esekf** object: +``` +double epsi[state_dof] = {0.001}; +fill(epsi, epsi+state_dof, 0.001); // if the absolute of innovation of ekf update is smaller than epso, the update iteration is converged +kf.init(f, df_dx, df_dw, h, dh_dx, dh_dv, Maximum_iter, epsi); +``` +8. In the running time, once an input **in** is received with time interval **dt**, a propagation is executed: +``` +kf.predict(dt, Q, in); // process noise covariance: Q, an Eigen matrix +``` +9. Once a measurement **z** is received, an iterated update is executed: +``` +kf.update_iterated(z, R); // measurement noise covariance: R, an Eigen matrix +``` +*Remarks(1):* +- We also combine the output equation and its differentiation into an union function, whose usage is the same as the above steps 1-4, and steps 5-9 are shown as follows. + +5. Implement the output equation and its differentiation , : +``` +measurement h_share(state &s, esekfom::share_datastruct &share_data) +{ + if(share_data.converge) {} // this value is true means iteration is converged + if(condition) share_data.valid = false; // the iteration stops before convergence when this value is false if other conditions are satified + share_data.h_x = H_x; // H_x is the result matrix of the first differentiation + share_data.h_v = H_v; // H_v is the result matrix of the second differentiation + share_data.R = R; // R is the measurement noise covariance + share_data.z = z; // z is the obtained measurement + + measurement h_; + h_.position = s.pos; + return h_; +} +``` +This function would be called during ekf state update, and the output function and its derivatives, the measurement and the measurement noise would be obtained from this one union function + +6. Instantiate an **esekf** object **kf** and initialize it with initial or default state and covariance. + +(1) initial state and covariance: +``` +state init_state; +esekfom::esekf::cov init_P; +esekfom::esekf kf(init_state,init_P); +``` +(2) default state and covariance: +``` +esekfom::esekf kf; +``` +7. Deliver the defined models, std int maximum iteration numbers **Maximum_iter**, and the std array for testing convergence **epsi** into the **esekf** object: +``` +double epsi[state_dof] = {0.001}; +fill(epsi, epsi+state_dof, 0.001); // if the absolute of innovation of ekf update is smaller than epso, the update iteration is converged +kf.init_share(f, df_dx, df_dw, h_share, Maximum_iter, epsi); +``` +8. In the running time, once an input **in** is received with time interval **dt**, a propagation is executed: +``` +kf.predict(dt, Q, in); // process noise covariance: Q +``` +9. Once a measurement **z** is received, an iterated update is executed: +``` +kf.update_iterated_share(); +``` + +*Remarks(2):* +- The value of the state **x** and the covariance **P** are able to be changed by functions **change_x()** and **change_P()**: +``` +state set_x; +kf.change_x(set_x); +esekfom::esekf::cov set_P; +kf.change_P(set_P); +``` + +## 3. Usage when the measurement is an Eigen vector of changing dimension. + +Clone the repository: + +``` + git clone https://github.com/hku-mars/IKFoM.git +``` + +1. include the necessary head file: +``` +#include +``` +2. Select and instantiate the primitive manifolds: +``` + typedef MTK::SO3 SO3; // scalar type of variable: double + typedef MTK::vect<3, double> vect3; // dimension of the defined Euclidean variable: 3 + typedef MTK::S2 S2; // length of the S2 variable: 98/10; choose e1 as the original point of rotation: 1 +``` +3. Build system state and input as compound manifolds which are composed of the primitive manifolds: +``` +MTK_BUILD_MANIFOLD(state, // name of compound manifold: state +((vect3, pos)) // ((primitive manifold type, name of variable)) +((vect3, vel)) +((SO3, rot)) +((vect3, bg)) +((vect3, ba)) +((S2, grav)) +((SO3, offset_R_L_I)) +((vect3, offset_T_L_I)) +); +``` +4. Implement the vector field that is defined as , and its differentiation , , where w=0 could be left out: +``` +Eigen::Matrix f(state &s, const input &i) { + Eigen::Matrix res = Eigen::Matrix::Zero(); + res(0) = s.vel[0]; + res(1) = s.vel[1]; + res(2) = s.vel[2]; + return res; +} +Eigen::Matrix df_dx(state &s, const input &i) //notice S2 has length of 3 and dimension of 2 { + Eigen::Matrix cov = Eigen::Matrix::Zero(); + cov.template block<3, 3>(0, 12) = Eigen::Matrix3d::Identity(); + return cov; +} +Eigen::Matrix df_dw(state &s, const input &i) { + Eigen::Matrix cov = Eigen::Matrix::Zero(); + cov.template block<3, 3>(12, 3) = -s.rot.toRotationMatrix(); + return cov; +} +``` +Those functions would be called during ekf state predict + +5. Implement the output equation and its differentiation , : +``` +Eigen::Matrix h(state &s, bool &valid) //the iteration stops before convergence when valid is false { + if (condition){ valid = false; + } // other conditions could be used to stop the ekf update iteration before convergence, otherwise the iteration will not stop until the condition of convergence is satisfied. + Eigen::Matrix h_; + h_(0) = s.pos[0]; + return h_; +} +Eigen::Matrix dh_dx(state &s) {} +Eigen::Matrix dh_dv(state &s) {} +``` +Those functions would be called during ekf state update + +6. Instantiate an **esekf** object **kf** and initialize it with initial or default state and covariance. + +(1) initial state and covariance: +``` +state init_state; +esekfom::esekf::cov init_P; +esekfom::esekf kf(init_state,init_P); +``` +(2) default state and covariance: +``` +esekfom::esekf kf; +``` +where **process_noise_dof** is the dimension of process noise, with the type of std int, and so for **measurement_noise_dof** + +7. Deliver the defined models, std int maximum iteration numbers **Maximum_iter**, and the std array for testing convergence **epsi** into the **esekf** object: +``` +double epsi[state_dof] = {0.001}; +fill(epsi, epsi+state_dof, 0.001); // if the absolute of innovation of ekf update is smaller than epso, the update iteration is converged +kf.init_dyn(f, df_dx, df_dw, h, dh_dx, dh_dv, Maximum_iter, epsi); +``` +8. In the running time, once an input **in** is received with time interval **dt**, a propagation is executed: +``` +kf.predict(dt, Q, in); // process noise covariance: Q, an Eigen matrix +``` +9. Once a measurement **z** is received, an iterated update is executed: +``` +kf.update_iterated_dyn(z, R); // measurement noise covariance: R, an Eigen matrix +``` +*Remarks(1):* +- We also combine the output equation and its differentiation into an union function, whose usage is the same as the above steps 1-4, and steps 5-9 are shown as follows. +5. Implement the output equation and its differentiation , : +``` +Eigen::Matrix h_dyn_share(state &s, esekfom::dyn_share_datastruct &dyn_share_data) +{ + if(dyn_share_data.converge) {} // this value is true means iteration is converged + if(condition) share_data.valid = false; // the iteration stops before convergence when this value is false if other conditions are satified + dyn_share_data.h_x = H_x; // H_x is the result matrix of the first differentiation + dyn_share_data.h_v = H_v; // H_v is the result matrix of the second differentiation + dyn_share_data.R = R; // R is the measurement noise covariance + dyn_share_data.z = z; // z is the obtained measurement + + Eigen::Matrix h_; + h_(0) = s.pos[0]; + return h_; +} +This function would be called during ekf state update, and the output function and its derivatives, the measurement and the measurement noise would be obtained from this one union function +``` +6. Instantiate an **esekf** object **kf** and initialize it with initial or default state and covariance. +(1) initial state and covariance: +``` +state init_state; +esekfom::esekf::cov init_P; +esekfom::esekf kf(init_state,init_P); +``` +(2) default state and covariance: +``` +esekfom::esekf kf; +``` +7. Deliver the defined models, std int maximum iteration numbers **Maximum_iter**, and the std array for testing convergence **epsi** into the **esekf** object: +``` +double epsi[state_dof] = {0.001}; +fill(epsi, epsi+state_dof, 0.001); // if the absolute of innovation of ekf update is smaller than epso, the update iteration is converged +kf.init_dyn_share(f, df_dx, df_dw, h_dyn_share, Maximum_iter, epsi); +``` +8. In the running time, once an input **in** is received with time interval **dt**, a propagation is executed: +``` +kf.predict(dt, Q, in); // process noise covariance: Q, an Eigen matrix +``` +9. Once a measurement **z** is received, an iterated update is executed: +``` +kf.update_iterated_dyn_share(); +``` + +*Remarks(2):* +- The value of the state **x** and the covariance **P** are able to be changed by functions **change_x()** and **change_P()**: +``` +state set_x; +kf.change_x(set_x); +esekfom::esekf::cov set_P; +kf.change_P(set_P); +``` + +## 4. Usage when the measurement is a changing manifold during the run time. + +Clone the repository: + +``` + git clone https://github.com/hku-mars/IKFoM.git +``` + +1. include the necessary head file: +``` +#include +``` +2. Select and instantiate the primitive manifolds: +``` + typedef MTK::SO3 SO3; // scalar type of variable: double + typedef MTK::vect<3, double> vect3; // dimension of the defined Euclidean variable: 3 + typedef MTK::S2 S2; // length of the S2 variable: 98/10; choose e1 as the original point of rotation: 1 +``` +3. Build system state and input as compound manifolds which are composed of the primitive manifolds: +``` +MTK_BUILD_MANIFOLD(state, // name of compound manifold: state +((vect3, pos)) // ((primitive manifold type, name of variable)) +((vect3, vel)) +((SO3, rot)) +((vect3, bg)) +((vect3, ba)) +((S2, grav)) +((SO3, offset_R_L_I)) +((vect3, offset_T_L_I)) +); +``` +4. Implement the vector field that is defined as , and its differentiation , , where w=0 could be left out: +``` +Eigen::Matrix f(state &s, const input &i) { + Eigen::Matrix res = Eigen::Matrix::Zero(); + res(0) = s.vel[0]; + res(1) = s.vel[1]; + res(2) = s.vel[2]; + return res; +} +Eigen::Matrix df_dx(state &s, const input &i) //notice S2 has length of 3 and dimension of 2 { + Eigen::Matrix cov = Eigen::Matrix::Zero(); + cov.template block<3, 3>(0, 12) = Eigen::Matrix3d::Identity(); + return cov; +} +Eigen::Matrix df_dw(state &s, const input &i) { + Eigen::Matrix cov = Eigen::Matrix::Zero(); + cov.template block<3, 3>(12, 3) = -s.rot.toRotationMatrix(); + return cov; +} +``` +Those functions would be called during ekf state predict + +5. Implement the differentiation of the output equation , : +``` +Eigen::Matrix dh_dx(state &s, bool &valid) {} //the iteration stops before convergence when valid is false +Eigen::Matrix dh_dv(state &s, bool &valid) {} +``` +Those functions would be called during ekf state update + +6. Instantiate an **esekf** object **kf** and initialize it with initial or default state and covariance. + +(1) initial state and covariance: +``` +state init_state; +esekfom::esekf::cov init_P; +esekfom::esekf kf(init_state,init_P); +``` +(2) +``` +esekfom::esekf kf; +``` +Where **process_noise_dof** is the dimension of process noise, of type of std int + +7. Deliver the defined models, std int maximum iteration numbers **Maximum_iter**, and the std array for testing convergence **epsi** into the **esekf** object: +``` +double epsi[state_dof] = {0.001}; +fill(epsi, epsi+state_dof, 0.001); // if the absolute of innovation of ekf update is smaller than epso, the update iteration is converged +kf.init_dyn_runtime(f, df_dx, df_dw, dh_dx, dh_dv, Maximum_iter, epsi); +``` +8. In the running time, once an input **in** is received with time interval **dt**, a propagation is executed: +``` +kf.predict(dt, Q, in); // process noise covariance: Q +``` +9. Once a measurement **z** is received, build system measurement as compound manifolds following step 3 and implement the output equation : +``` +measurement h(state &s, bool &valid) //the iteration stops before convergence when valid is false +{ + if (condition) valid = false; // the update iteration could be stopped when the condition other than convergence is satisfied + measurement h_; + h_.pos = s.pos; + return h_; +} +``` +then an iterated update is executed: +``` +kf.update_iterated_dyn_runtime(z, R, h); // measurement noise covariance: R, an Eigen matrix +``` +*Remarks(1):* +- We also combine the output equation and its differentiation into an union function, whose usage is the same as the above steps 1-4, and steps 5-9 are shown as follows. +5. Instantiate an **esekf** object **kf** and initialize it with initial or default state and covariance. + +(1) initial state and covariance: +``` +state init_state; +esekfom::esekf::cov init_P; +esekfom::esekf kf(init_state,init_P); +``` +(2) default state and covariance: +``` +esekfom::esekf kf; +``` +6. Deliver the defined models, std int maximum iteration numbers **Maximum_iter**, and the std array for testing convergence **epsi** into the **esekf** object: +``` +double epsi[state_dof] = {0.001}; +fill(epsi, epsi+state_dof, 0.001); // if the absolute of innovation of ekf update is smaller than epso, the update iteration is converged +kf.init_dyn_runtime_share(f, df_dx, df_dw, Maximum_iter, epsi); +``` +7. In the running time, once an input **in** is received with time interval **dt**, a propagation is executed: +``` +kf.predict(dt, Q, in); // process noise covariance: Q. an Eigen matrix +``` +8. Once a measurement **z** is received, build system measurement as compound manifolds following step 3 and implement the output equation and its differentiation , : +``` +measurement h_dyn_runtime_share(state &s, esekfom::dyn_runtime_share_datastruct &dyn_runtime_share_data) +{ + if(dyn_runtime_share_data.converge) {} // this value is true means iteration is converged + if(condition) dyn_runtime_share_data.valid = false; // the iteration stops before convergence when this value is false, if conditions other than convergence is satisfied + dyn_runtime_share_data.h_x = H_x; // H_x is the result matrix of the first differentiation + dyn_runtime_share_data.h_v = H_v; // H_v is the result matrix of the second differentiation + dyn_runtime_share_data.R = R; // R is the measurement noise covariance + + measurement h_; + h_.pos = s.pos; + return h_; +} +``` +This function would be called during ekf state update, and the output function and its derivatives, the measurement and the measurement noise would be obtained from this one union function + +then an iterated update is executed: +``` +kf.update_iterated_dyn_runtime_share(z, h_dyn_runtime_share); +``` + +*Remarks(2):* +- The value of the state **x** and the covariance **P** are able to be changed by functions **change_x()** and **change_P()**: +``` +state set_x; +kf.change_x(set_x); +esekfom::esekf::cov set_P; +kf.change_P(set_P); +``` + +## 5. Run the sample +Clone the repository: + +``` + git clone https://github.com/hku-mars/IKFoM.git +``` +In the **Samples** file folder, there is the scource code that applys the **IKFoM** on the original source code from [FAST LIO](https://github.com/hku-mars/FAST_LIO). Please follow the README.md shown in that repository excepting the step **2. Build**, which is modified as: +``` +cd ~/catkin_ws/src +cp -r ~/IKFoM/Samples/FAST_LIO-stable FAST_LIO-stable +cd .. +catkin_make +source devel/setup.bash +``` + +## 6.Acknowledgments +Thanks for C. Hertzberg, R. Wagner, U. Frese, and L. Schroder. Integratinggeneric sensor fusion algorithms with sound state representationsthrough encapsulation of manifolds. + diff --git a/src/Point-LIO/include/common_lib.h b/src/Point-LIO/include/common_lib.h new file mode 100755 index 0000000..5de7546 --- /dev/null +++ b/src/Point-LIO/include/common_lib.h @@ -0,0 +1,176 @@ +#ifndef COMMON_LIB_H +#define COMMON_LIB_H + +#include +#include +#include +#include +#include +#include +#include +#include +using namespace std; +using namespace Eigen; + +#define PI_M (3.14159265358) +#define G_m_s2 (9.81) // Gravaty const in GuangDong/China +#define DIM_STATE (18) // Dimension of states (Let Dim(SO(3)) = 3) +#define DIM_PROC_N (12) // Dimension of process noise (Let Dim(SO(3)) = 3) +#define CUBE_LEN (6.0) +#define LIDAR_SP_LEN (2) +#define INIT_COV (0.0001) +#define NUM_MATCH_POINTS (5) +#define MAX_MEAS_DIM (10000) + +#define VEC_FROM_ARRAY(v) v[0],v[1],v[2] +#define VEC_FROM_ARRAY_SIX(v) v[0],v[1],v[2],v[3],v[4],v[5] +#define MAT_FROM_ARRAY(v) v[0],v[1],v[2],v[3],v[4],v[5],v[6],v[7],v[8] +#define CONSTRAIN(v,min,max) ((v>min)?((v (mat.data(), mat.data() + mat.rows() * mat.cols()) +#define DEBUG_FILE_DIR(name) (string(string(ROOT_DIR) + "Log/"+ name)) + +typedef pcl::PointXYZINormal PointType; +typedef pcl::PointXYZRGB PointTypeRGB; +typedef pcl::PointCloud PointCloudXYZI; +typedef pcl::PointCloud PointCloudXYZRGB; +typedef vector> PointVector; +typedef Vector3d V3D; +typedef Matrix3d M3D; +typedef Vector3f V3F; +typedef Matrix3f M3F; + +#define MD(a,b) Matrix +#define VD(a) Matrix +#define MF(a,b) Matrix +#define VF(a) Matrix + +const M3D Eye3d(M3D::Identity()); +const M3F Eye3f(M3F::Identity()); +const V3D Zero3d(0, 0, 0); +const V3F Zero3f(0, 0, 0); + +struct MeasureGroup // Lidar data and imu dates for the curent process +{ + MeasureGroup() + { + lidar_beg_time = 0.0; + lidar_last_time = 0.0; + this->lidar.reset(new PointCloudXYZI()); + }; + double lidar_beg_time; + double lidar_last_time; + PointCloudXYZI::Ptr lidar; + deque imu; +}; + +template +T calc_dist(PointType p1, PointType p2){ + T d = (p1.x - p2.x) * (p1.x - p2.x) + (p1.y - p2.y) * (p1.y - p2.y) + (p1.z - p2.z) * (p1.z - p2.z); + return d; +} + +template +T calc_dist(Eigen::Vector3d p1, PointType p2){ + T d = (p1(0) - p2.x) * (p1(0) - p2.x) + (p1(1) - p2.y) * (p1(1) - p2.y) + (p1(2) - p2.z) * (p1(2) - p2.z); + return d; +} + +template +std::vector time_compressing(const PointCloudXYZI::Ptr &point_cloud) +{ + int points_size = point_cloud->points.size(); + int j = 0; + std::vector time_seq; + // time_seq.clear(); + time_seq.reserve(points_size); + for(int i = 0; i < points_size - 1; i++) + { + j++; + if (point_cloud->points[i+1].curvature > point_cloud->points[i].curvature) + { + time_seq.emplace_back(j); + j = 0; + } + } + if (j == 0) + { + time_seq.emplace_back(1); + } + else + { + time_seq.emplace_back(j+1); + } + return time_seq; +} + +/* comment +plane equation: Ax + By + Cz + D = 0 +convert to: A/D*x + B/D*y + C/D*z = -1 +solve: A0*x0 = b0 +where A0_i = [x_i, y_i, z_i], x0 = [A/D, B/D, C/D]^T, b0 = [-1, ..., -1]^T +normvec: normalized x0 +*/ +template +bool esti_normvector(Matrix &normvec, const PointVector &point, const T &threshold, const int &point_num) +{ + MatrixXf A(point_num, 3); + MatrixXf b(point_num, 1); + b.setOnes(); + b *= -1.0f; + + for (int j = 0; j < point_num; j++) + { + A(j,0) = point[j].x; + A(j,1) = point[j].y; + A(j,2) = point[j].z; + } + normvec = A.colPivHouseholderQr().solve(b); + + for (int j = 0; j < point_num; j++) + { + if (fabs(normvec(0) * point[j].x + normvec(1) * point[j].y + normvec(2) * point[j].z + 1.0f) > threshold) + { + return false; + } + } + + normvec.normalize(); + return true; +} + +template +bool esti_plane(Matrix &pca_result, const PointVector &point, const T &threshold) +{ + Matrix A; + Matrix b; + A.setZero(); + b.setOnes(); + b *= -1.0f; + + for (int j = 0; j < NUM_MATCH_POINTS; j++) + { + A(j,0) = point[j].x; + A(j,1) = point[j].y; + A(j,2) = point[j].z; + } + + Matrix normvec = A.colPivHouseholderQr().solve(b); + + T n = normvec.norm(); + pca_result(0) = normvec(0) / n; + pca_result(1) = normvec(1) / n; + pca_result(2) = normvec(2) / n; + pca_result(3) = 1.0 / n; + + for (int j = 0; j < NUM_MATCH_POINTS; j++) + { + if (fabs(pca_result(0) * point[j].x + pca_result(1) * point[j].y + pca_result(2) * point[j].z + pca_result(3)) > threshold) + { + return false; + } + } + return true; +} + +#endif \ No newline at end of file diff --git a/src/Point-LIO/include/ikd-Tree/README.md b/src/Point-LIO/include/ikd-Tree/README.md new file mode 100644 index 0000000..e113a91 --- /dev/null +++ b/src/Point-LIO/include/ikd-Tree/README.md @@ -0,0 +1,2 @@ +# ikd-Tree +ikd-Tree is an incremental k-d tree for robotic applications. diff --git a/src/Point-LIO/include/ikd-Tree/ikd_Tree.cpp b/src/Point-LIO/include/ikd-Tree/ikd_Tree.cpp new file mode 100644 index 0000000..e8c4e86 --- /dev/null +++ b/src/Point-LIO/include/ikd-Tree/ikd_Tree.cpp @@ -0,0 +1,1728 @@ +#include "ikd_Tree.h" + +/* +Description: ikd-Tree: an incremental k-d tree for robotic applications +Author: Yixi Cai +email: yixicai@connect.hku.hk +*/ + +template +KD_TREE::KD_TREE(float delete_param, float balance_param, float box_length) +{ + delete_criterion_param = delete_param; + balance_criterion_param = balance_param; + downsample_size = box_length; + Rebuild_Logger.clear(); + termination_flag = false; + start_thread(); +} + +template +KD_TREE::~KD_TREE() +{ + stop_thread(); + Delete_Storage_Disabled = true; + delete_tree_nodes(&Root_Node); + PointVector().swap(PCL_Storage); + Rebuild_Logger.clear(); +} + + + +template +void KD_TREE::InitializeKDTree(float delete_param, float balance_param, float box_length) +{ + Set_delete_criterion_param(delete_param); + Set_balance_criterion_param(balance_param); + set_downsample_param(box_length); +} + +template +void KD_TREE::InitTreeNode(KD_TREE_NODE *root) +{ + root->point.x = 0.0f; + root->point.y = 0.0f; + root->point.z = 0.0f; + root->node_range_x[0] = 0.0f; + root->node_range_x[1] = 0.0f; + root->node_range_y[0] = 0.0f; + root->node_range_y[1] = 0.0f; + root->node_range_z[0] = 0.0f; + root->node_range_z[1] = 0.0f; + root->radius_sq = 0.0f; + root->division_axis = 0; + root->father_ptr = nullptr; + root->left_son_ptr = nullptr; + root->right_son_ptr = nullptr; + root->TreeSize = 0; + root->invalid_point_num = 0; + root->down_del_num = 0; + root->point_deleted = false; + root->tree_deleted = false; + root->need_push_down_to_left = false; + root->need_push_down_to_right = false; + root->point_downsample_deleted = false; + root->working_flag = false; + pthread_mutex_init(&(root->push_down_mutex_lock), NULL); +} + +template +int KD_TREE::size() +{ + int s = 0; + if (Rebuild_Ptr == nullptr || *Rebuild_Ptr != Root_Node) + { + if (Root_Node != nullptr) + { + return Root_Node->TreeSize; + } + else + { + return 0; + } + } + else + { + if (!pthread_mutex_trylock(&working_flag_mutex)) + { + s = Root_Node->TreeSize; + pthread_mutex_unlock(&working_flag_mutex); + return s; + } + else + { + return Treesize_tmp; + } + } +} + +template +BoxPointType KD_TREE::tree_range() +{ + BoxPointType range; + if (Rebuild_Ptr == nullptr || *Rebuild_Ptr != Root_Node) + { + if (Root_Node != nullptr) + { + range.vertex_min[0] = Root_Node->node_range_x[0]; + range.vertex_min[1] = Root_Node->node_range_y[0]; + range.vertex_min[2] = Root_Node->node_range_z[0]; + range.vertex_max[0] = Root_Node->node_range_x[1]; + range.vertex_max[1] = Root_Node->node_range_y[1]; + range.vertex_max[2] = Root_Node->node_range_z[1]; + } + else + { + memset(&range, 0, sizeof(range)); + } + } + else + { + if (!pthread_mutex_trylock(&working_flag_mutex)) + { + range.vertex_min[0] = Root_Node->node_range_x[0]; + range.vertex_min[1] = Root_Node->node_range_y[0]; + range.vertex_min[2] = Root_Node->node_range_z[0]; + range.vertex_max[0] = Root_Node->node_range_x[1]; + range.vertex_max[1] = Root_Node->node_range_y[1]; + range.vertex_max[2] = Root_Node->node_range_z[1]; + pthread_mutex_unlock(&working_flag_mutex); + } + else + { + memset(&range, 0, sizeof(range)); + } + } + return range; +} + +template +int KD_TREE::validnum() +{ + int s = 0; + if (Rebuild_Ptr == nullptr || *Rebuild_Ptr != Root_Node) + { + if (Root_Node != nullptr) + return (Root_Node->TreeSize - Root_Node->invalid_point_num); + else + return 0; + } + else + { + if (!pthread_mutex_trylock(&working_flag_mutex)) + { + s = Root_Node->TreeSize - Root_Node->invalid_point_num; + pthread_mutex_unlock(&working_flag_mutex); + return s; + } + else + { + return -1; + } + } +} + +template +void KD_TREE::root_alpha(float &alpha_bal, float &alpha_del) +{ + if (Rebuild_Ptr == nullptr || *Rebuild_Ptr != Root_Node) + { + alpha_bal = Root_Node->alpha_bal; + alpha_del = Root_Node->alpha_del; + return; + } + else + { + if (!pthread_mutex_trylock(&working_flag_mutex)) + { + alpha_bal = Root_Node->alpha_bal; + alpha_del = Root_Node->alpha_del; + pthread_mutex_unlock(&working_flag_mutex); + return; + } + else + { + alpha_bal = alpha_bal_tmp; + alpha_del = alpha_del_tmp; + return; + } + } +} + +template +void KD_TREE::start_thread() +{ + pthread_mutex_init(&termination_flag_mutex_lock, NULL); + pthread_mutex_init(&rebuild_ptr_mutex_lock, NULL); + pthread_mutex_init(&rebuild_logger_mutex_lock, NULL); + pthread_mutex_init(&points_deleted_rebuild_mutex_lock, NULL); + pthread_mutex_init(&working_flag_mutex, NULL); + pthread_mutex_init(&search_flag_mutex, NULL); + pthread_create(&rebuild_thread, NULL, multi_thread_ptr, (void *)this); + printf("Multi thread started \n"); +} + +template +void KD_TREE::stop_thread() +{ + pthread_mutex_lock(&termination_flag_mutex_lock); + termination_flag = true; + pthread_mutex_unlock(&termination_flag_mutex_lock); + if (rebuild_thread) + pthread_join(rebuild_thread, NULL); + pthread_mutex_destroy(&termination_flag_mutex_lock); + pthread_mutex_destroy(&rebuild_logger_mutex_lock); + pthread_mutex_destroy(&rebuild_ptr_mutex_lock); + pthread_mutex_destroy(&points_deleted_rebuild_mutex_lock); + pthread_mutex_destroy(&working_flag_mutex); + pthread_mutex_destroy(&search_flag_mutex); +} + +template +void *KD_TREE::multi_thread_ptr(void *arg) +{ + KD_TREE *handle = (KD_TREE *)arg; + handle->multi_thread_rebuild(); + return nullptr; +} + +template +void KD_TREE::multi_thread_rebuild() +{ + bool terminated = false; + KD_TREE_NODE *father_ptr, **new_node_ptr; + pthread_mutex_lock(&termination_flag_mutex_lock); + terminated = termination_flag; + pthread_mutex_unlock(&termination_flag_mutex_lock); + while (!terminated) + { + pthread_mutex_lock(&rebuild_ptr_mutex_lock); + pthread_mutex_lock(&working_flag_mutex); + if (Rebuild_Ptr != nullptr) + { + /* Traverse and copy */ + if (!Rebuild_Logger.empty()) + { + printf("\n\n\n\n\n\n\n\n\n\n\n ERROR!!! \n\n\n\n\n\n\n\n\n"); + } + rebuild_flag = true; + if (*Rebuild_Ptr == Root_Node) + { + Treesize_tmp = Root_Node->TreeSize; + Validnum_tmp = Root_Node->TreeSize - Root_Node->invalid_point_num; + alpha_bal_tmp = Root_Node->alpha_bal; + alpha_del_tmp = Root_Node->alpha_del; + } + KD_TREE_NODE *old_root_node = (*Rebuild_Ptr); + father_ptr = (*Rebuild_Ptr)->father_ptr; + PointVector().swap(Rebuild_PCL_Storage); + // Lock Search + pthread_mutex_lock(&search_flag_mutex); + while (search_mutex_counter != 0) + { + pthread_mutex_unlock(&search_flag_mutex); + usleep(1); + pthread_mutex_lock(&search_flag_mutex); + } + search_mutex_counter = -1; + pthread_mutex_unlock(&search_flag_mutex); + // Lock deleted points cache + pthread_mutex_lock(&points_deleted_rebuild_mutex_lock); + flatten(*Rebuild_Ptr, Rebuild_PCL_Storage, MULTI_THREAD_REC); + // Unlock deleted points cache + pthread_mutex_unlock(&points_deleted_rebuild_mutex_lock); + // Unlock Search + pthread_mutex_lock(&search_flag_mutex); + search_mutex_counter = 0; + pthread_mutex_unlock(&search_flag_mutex); + pthread_mutex_unlock(&working_flag_mutex); + /* Rebuild and update missed operations*/ + Operation_Logger_Type Operation; + KD_TREE_NODE *new_root_node = nullptr; + if (int(Rebuild_PCL_Storage.size()) > 0) + { + BuildTree(&new_root_node, 0, Rebuild_PCL_Storage.size() - 1, Rebuild_PCL_Storage); + // Rebuild has been done. Updates the blocked operations into the new tree + pthread_mutex_lock(&working_flag_mutex); + pthread_mutex_lock(&rebuild_logger_mutex_lock); + int tmp_counter = 0; + while (!Rebuild_Logger.empty()) + { + Operation = Rebuild_Logger.front(); + max_queue_size = max(max_queue_size, Rebuild_Logger.size()); + Rebuild_Logger.pop(); + pthread_mutex_unlock(&rebuild_logger_mutex_lock); + pthread_mutex_unlock(&working_flag_mutex); + run_operation(&new_root_node, Operation); + tmp_counter++; + if (tmp_counter % 10 == 0) + usleep(1); + pthread_mutex_lock(&working_flag_mutex); + pthread_mutex_lock(&rebuild_logger_mutex_lock); + } + pthread_mutex_unlock(&rebuild_logger_mutex_lock); + } + /* Replace to original tree*/ + // pthread_mutex_lock(&working_flag_mutex); + pthread_mutex_lock(&search_flag_mutex); + while (search_mutex_counter != 0) + { + pthread_mutex_unlock(&search_flag_mutex); + usleep(1); + pthread_mutex_lock(&search_flag_mutex); + } + search_mutex_counter = -1; + pthread_mutex_unlock(&search_flag_mutex); + if (father_ptr->left_son_ptr == *Rebuild_Ptr) + { + father_ptr->left_son_ptr = new_root_node; + } + else if (father_ptr->right_son_ptr == *Rebuild_Ptr) + { + father_ptr->right_son_ptr = new_root_node; + } + else + { + throw "Error: Father ptr incompatible with current node\n"; + } + if (new_root_node != nullptr) + new_root_node->father_ptr = father_ptr; + (*Rebuild_Ptr) = new_root_node; + int valid_old = old_root_node->TreeSize - old_root_node->invalid_point_num; + int valid_new = new_root_node->TreeSize - new_root_node->invalid_point_num; + if (father_ptr == STATIC_ROOT_NODE) + Root_Node = STATIC_ROOT_NODE->left_son_ptr; + KD_TREE_NODE *update_root = *Rebuild_Ptr; + while (update_root != nullptr && update_root != Root_Node) + { + update_root = update_root->father_ptr; + if (update_root->working_flag) + break; + if (update_root == update_root->father_ptr->left_son_ptr && update_root->father_ptr->need_push_down_to_left) + break; + if (update_root == update_root->father_ptr->right_son_ptr && update_root->father_ptr->need_push_down_to_right) + break; + Update(update_root); + } + pthread_mutex_lock(&search_flag_mutex); + search_mutex_counter = 0; + pthread_mutex_unlock(&search_flag_mutex); + Rebuild_Ptr = nullptr; + pthread_mutex_unlock(&working_flag_mutex); + rebuild_flag = false; + /* Delete discarded tree nodes */ + delete_tree_nodes(&old_root_node); + } + else + { + pthread_mutex_unlock(&working_flag_mutex); + } + pthread_mutex_unlock(&rebuild_ptr_mutex_lock); + pthread_mutex_lock(&termination_flag_mutex_lock); + terminated = termination_flag; + pthread_mutex_unlock(&termination_flag_mutex_lock); + usleep(100); + } + printf("Rebuild thread terminated normally\n"); +} + +template +void KD_TREE::run_operation(KD_TREE_NODE **root, Operation_Logger_Type operation) +{ + switch (operation.op) + { + case ADD_POINT: + Add_by_point(root, operation.point, false, (*root)->division_axis); + break; + case ADD_BOX: + Add_by_range(root, operation.boxpoint, false); + break; + case DELETE_POINT: + Delete_by_point(root, operation.point, false); + break; + case DELETE_BOX: + Delete_by_range(root, operation.boxpoint, false, false); + break; + case DOWNSAMPLE_DELETE: + Delete_by_range(root, operation.boxpoint, false, true); + break; + case PUSH_DOWN: + (*root)->tree_downsample_deleted |= operation.tree_downsample_deleted; + (*root)->point_downsample_deleted |= operation.tree_downsample_deleted; + (*root)->tree_deleted = operation.tree_deleted || (*root)->tree_downsample_deleted; + (*root)->point_deleted = (*root)->tree_deleted || (*root)->point_downsample_deleted; + if (operation.tree_downsample_deleted) + (*root)->down_del_num = (*root)->TreeSize; + if (operation.tree_deleted) + (*root)->invalid_point_num = (*root)->TreeSize; + else + (*root)->invalid_point_num = (*root)->down_del_num; + (*root)->need_push_down_to_left = true; + (*root)->need_push_down_to_right = true; + break; + default: + break; + } +} + +template +void KD_TREE::Build(PointVector point_cloud) +{ + if (Root_Node != nullptr) + { + delete_tree_nodes(&Root_Node); + } + if (point_cloud.size() == 0) + return; + STATIC_ROOT_NODE = new KD_TREE_NODE; + InitTreeNode(STATIC_ROOT_NODE); + BuildTree(&STATIC_ROOT_NODE->left_son_ptr, 0, point_cloud.size() - 1, point_cloud); + Update(STATIC_ROOT_NODE); + STATIC_ROOT_NODE->TreeSize = 0; + Root_Node = STATIC_ROOT_NODE->left_son_ptr; +} + +template +void KD_TREE::Nearest_Search(PointType point, int k_nearest, PointVector &Nearest_Points, vector &Point_Distance, float max_dist) +{ + MANUAL_HEAP q(2 * k_nearest); + q.clear(); + vector().swap(Point_Distance); + if (Rebuild_Ptr == nullptr || *Rebuild_Ptr != Root_Node) + { + Search(Root_Node, k_nearest, point, q, max_dist); + } + else + { + pthread_mutex_lock(&search_flag_mutex); + while (search_mutex_counter == -1) + { + pthread_mutex_unlock(&search_flag_mutex); + usleep(1); + pthread_mutex_lock(&search_flag_mutex); + } + search_mutex_counter += 1; + pthread_mutex_unlock(&search_flag_mutex); + Search(Root_Node, k_nearest, point, q, max_dist); + pthread_mutex_lock(&search_flag_mutex); + search_mutex_counter -= 1; + pthread_mutex_unlock(&search_flag_mutex); + } + int k_found = min(k_nearest, int(q.size())); + PointVector().swap(Nearest_Points); + vector().swap(Point_Distance); + for (int i = 0; i < k_found; i++) + { + Nearest_Points.insert(Nearest_Points.begin(), q.top().point); + Point_Distance.insert(Point_Distance.begin(), q.top().dist); + q.pop(); + } + return; +} + +template +void KD_TREE::Box_Search(const BoxPointType &Box_of_Point, PointVector &Storage) +{ + Storage.clear(); + Search_by_range(Root_Node, Box_of_Point, Storage); +} + +template +void KD_TREE::Radius_Search(PointType point, const float radius, PointVector &Storage) +{ + Storage.clear(); + Search_by_radius(Root_Node, point, radius, Storage); +} + +template +int KD_TREE::Add_Points(PointVector &PointToAdd, bool downsample_on) +{ + int NewPointSize = PointToAdd.size(); + int tree_size = size(); + BoxPointType Box_of_Point; + PointType downsample_result, mid_point; + bool downsample_switch = downsample_on && DOWNSAMPLE_SWITCH; + float min_dist, tmp_dist; + int tmp_counter = 0; + for (int i = 0; i < PointToAdd.size(); i++) + { + if (downsample_switch) + { + Box_of_Point.vertex_min[0] = floor(PointToAdd[i].x / downsample_size) * downsample_size; + Box_of_Point.vertex_max[0] = Box_of_Point.vertex_min[0] + downsample_size; + Box_of_Point.vertex_min[1] = floor(PointToAdd[i].y / downsample_size) * downsample_size; + Box_of_Point.vertex_max[1] = Box_of_Point.vertex_min[1] + downsample_size; + Box_of_Point.vertex_min[2] = floor(PointToAdd[i].z / downsample_size) * downsample_size; + Box_of_Point.vertex_max[2] = Box_of_Point.vertex_min[2] + downsample_size; + mid_point.x = Box_of_Point.vertex_min[0] + (Box_of_Point.vertex_max[0] - Box_of_Point.vertex_min[0]) / 2.0; + mid_point.y = Box_of_Point.vertex_min[1] + (Box_of_Point.vertex_max[1] - Box_of_Point.vertex_min[1]) / 2.0; + mid_point.z = Box_of_Point.vertex_min[2] + (Box_of_Point.vertex_max[2] - Box_of_Point.vertex_min[2]) / 2.0; + PointVector().swap(Downsample_Storage); + Search_by_range(Root_Node, Box_of_Point, Downsample_Storage); + min_dist = calc_dist(PointToAdd[i], mid_point); + downsample_result = PointToAdd[i]; + for (int index = 0; index < Downsample_Storage.size(); index++) + { + tmp_dist = calc_dist(Downsample_Storage[index], mid_point); + if (tmp_dist < min_dist) + { + min_dist = tmp_dist; + downsample_result = Downsample_Storage[index]; + } + } + if (Rebuild_Ptr == nullptr || *Rebuild_Ptr != Root_Node) + { + if (Downsample_Storage.size() > 1 || same_point(PointToAdd[i], downsample_result)) + { + if (Downsample_Storage.size() > 0) + Delete_by_range(&Root_Node, Box_of_Point, true, true); + Add_by_point(&Root_Node, downsample_result, true, Root_Node->division_axis); + tmp_counter++; + } + } + else + { + if (Downsample_Storage.size() > 1 || same_point(PointToAdd[i], downsample_result)) + { + Operation_Logger_Type operation_delete, operation; + operation_delete.boxpoint = Box_of_Point; + operation_delete.op = DOWNSAMPLE_DELETE; + operation.point = downsample_result; + operation.op = ADD_POINT; + pthread_mutex_lock(&working_flag_mutex); + if (Downsample_Storage.size() > 0) + Delete_by_range(&Root_Node, Box_of_Point, false, true); + Add_by_point(&Root_Node, downsample_result, false, Root_Node->division_axis); + tmp_counter++; + if (rebuild_flag) + { + pthread_mutex_lock(&rebuild_logger_mutex_lock); + if (Downsample_Storage.size() > 0) + Rebuild_Logger.push(operation_delete); + Rebuild_Logger.push(operation); + pthread_mutex_unlock(&rebuild_logger_mutex_lock); + } + pthread_mutex_unlock(&working_flag_mutex); + }; + } + } + else + { + if (Rebuild_Ptr == nullptr || *Rebuild_Ptr != Root_Node) + { + Add_by_point(&Root_Node, PointToAdd[i], true, Root_Node->division_axis); + } + else + { + Operation_Logger_Type operation; + operation.point = PointToAdd[i]; + operation.op = ADD_POINT; + pthread_mutex_lock(&working_flag_mutex); + Add_by_point(&Root_Node, PointToAdd[i], false, Root_Node->division_axis); + if (rebuild_flag) + { + pthread_mutex_lock(&rebuild_logger_mutex_lock); + Rebuild_Logger.push(operation); + pthread_mutex_unlock(&rebuild_logger_mutex_lock); + } + pthread_mutex_unlock(&working_flag_mutex); + } + } + } + return tmp_counter; +} + +template +void KD_TREE::Add_Point_Boxes(vector &BoxPoints) +{ + for (int i = 0; i < BoxPoints.size(); i++) + { + if (Rebuild_Ptr == nullptr || *Rebuild_Ptr != Root_Node) + { + Add_by_range(&Root_Node, BoxPoints[i], true); + } + else + { + Operation_Logger_Type operation; + operation.boxpoint = BoxPoints[i]; + operation.op = ADD_BOX; + pthread_mutex_lock(&working_flag_mutex); + Add_by_range(&Root_Node, BoxPoints[i], false); + if (rebuild_flag) + { + pthread_mutex_lock(&rebuild_logger_mutex_lock); + Rebuild_Logger.push(operation); + pthread_mutex_unlock(&rebuild_logger_mutex_lock); + } + pthread_mutex_unlock(&working_flag_mutex); + } + } + return; +} + +template +void KD_TREE::Delete_Points(PointVector &PointToDel) +{ + for (int i = 0; i < PointToDel.size(); i++) + { + if (Rebuild_Ptr == nullptr || *Rebuild_Ptr != Root_Node) + { + Delete_by_point(&Root_Node, PointToDel[i], true); + } + else + { + Operation_Logger_Type operation; + operation.point = PointToDel[i]; + operation.op = DELETE_POINT; + pthread_mutex_lock(&working_flag_mutex); + Delete_by_point(&Root_Node, PointToDel[i], false); + if (rebuild_flag) + { + pthread_mutex_lock(&rebuild_logger_mutex_lock); + Rebuild_Logger.push(operation); + pthread_mutex_unlock(&rebuild_logger_mutex_lock); + } + pthread_mutex_unlock(&working_flag_mutex); + } + } + return; +} + +template +int KD_TREE::Delete_Point_Boxes(vector &BoxPoints) +{ + int tmp_counter = 0; + for (int i = 0; i < BoxPoints.size(); i++) + { + if (Rebuild_Ptr == nullptr || *Rebuild_Ptr != Root_Node) + { + tmp_counter += Delete_by_range(&Root_Node, BoxPoints[i], true, false); + } + else + { + Operation_Logger_Type operation; + operation.boxpoint = BoxPoints[i]; + operation.op = DELETE_BOX; + pthread_mutex_lock(&working_flag_mutex); + tmp_counter += Delete_by_range(&Root_Node, BoxPoints[i], false, false); + if (rebuild_flag) + { + pthread_mutex_lock(&rebuild_logger_mutex_lock); + Rebuild_Logger.push(operation); + pthread_mutex_unlock(&rebuild_logger_mutex_lock); + } + pthread_mutex_unlock(&working_flag_mutex); + } + } + return tmp_counter; +} + +template +void KD_TREE::acquire_removed_points(PointVector &removed_points) +{ + pthread_mutex_lock(&points_deleted_rebuild_mutex_lock); + for (int i = 0; i < Points_deleted.size(); i++) + { + removed_points.push_back(Points_deleted[i]); + } + for (int i = 0; i < Multithread_Points_deleted.size(); i++) + { + removed_points.push_back(Multithread_Points_deleted[i]); + } + Points_deleted.clear(); + Multithread_Points_deleted.clear(); + pthread_mutex_unlock(&points_deleted_rebuild_mutex_lock); + return; +} + +template +void KD_TREE::BuildTree(KD_TREE_NODE **root, int l, int r, PointVector &Storage) +{ + if (l > r) + return; + *root = new KD_TREE_NODE; + InitTreeNode(*root); + int mid = (l + r) >> 1; + int div_axis = 0; + int i; + // Find the best division Axis + float min_value[3] = {INFINITY, INFINITY, INFINITY}; + float max_value[3] = {-INFINITY, -INFINITY, -INFINITY}; + float dim_range[3] = {0, 0, 0}; + for (i = l; i <= r; i++) + { + min_value[0] = min(min_value[0], Storage[i].x); + min_value[1] = min(min_value[1], Storage[i].y); + min_value[2] = min(min_value[2], Storage[i].z); + max_value[0] = max(max_value[0], Storage[i].x); + max_value[1] = max(max_value[1], Storage[i].y); + max_value[2] = max(max_value[2], Storage[i].z); + } + // Select the longest dimension as division axis + for (i = 0; i < 3; i++) + dim_range[i] = max_value[i] - min_value[i]; + for (i = 1; i < 3; i++) + if (dim_range[i] > dim_range[div_axis]) + div_axis = i; + // Divide by the division axis and recursively build. + + (*root)->division_axis = div_axis; + switch (div_axis) + { + case 0: + nth_element(begin(Storage) + l, begin(Storage) + mid, begin(Storage) + r + 1, point_cmp_x); + break; + case 1: + nth_element(begin(Storage) + l, begin(Storage) + mid, begin(Storage) + r + 1, point_cmp_y); + break; + case 2: + nth_element(begin(Storage) + l, begin(Storage) + mid, begin(Storage) + r + 1, point_cmp_z); + break; + default: + nth_element(begin(Storage) + l, begin(Storage) + mid, begin(Storage) + r + 1, point_cmp_x); + break; + } + (*root)->point = Storage[mid]; + KD_TREE_NODE *left_son = nullptr, *right_son = nullptr; + BuildTree(&left_son, l, mid - 1, Storage); + BuildTree(&right_son, mid + 1, r, Storage); + (*root)->left_son_ptr = left_son; + (*root)->right_son_ptr = right_son; + Update((*root)); + return; +} + +template +void KD_TREE::Rebuild(KD_TREE_NODE **root) +{ + KD_TREE_NODE *father_ptr; + if ((*root)->TreeSize >= Multi_Thread_Rebuild_Point_Num) + { + if (!pthread_mutex_trylock(&rebuild_ptr_mutex_lock)) + { + if (Rebuild_Ptr == nullptr || ((*root)->TreeSize > (*Rebuild_Ptr)->TreeSize)) + { + Rebuild_Ptr = root; + } + pthread_mutex_unlock(&rebuild_ptr_mutex_lock); + } + } + else + { + father_ptr = (*root)->father_ptr; + int size_rec = (*root)->TreeSize; + PCL_Storage.clear(); + flatten(*root, PCL_Storage, DELETE_POINTS_REC); + delete_tree_nodes(root); + BuildTree(root, 0, PCL_Storage.size() - 1, PCL_Storage); + if (*root != nullptr) + (*root)->father_ptr = father_ptr; + if (*root == Root_Node) + STATIC_ROOT_NODE->left_son_ptr = *root; + } + return; +} + +template +int KD_TREE::Delete_by_range(KD_TREE_NODE **root, BoxPointType boxpoint, bool allow_rebuild, bool is_downsample) +{ + if ((*root) == nullptr || (*root)->tree_deleted) + return 0; + (*root)->working_flag = true; + Push_Down(*root); + int tmp_counter = 0; + if (boxpoint.vertex_max[0] <= (*root)->node_range_x[0] || boxpoint.vertex_min[0] > (*root)->node_range_x[1]) + return 0; + if (boxpoint.vertex_max[1] <= (*root)->node_range_y[0] || boxpoint.vertex_min[1] > (*root)->node_range_y[1]) + return 0; + if (boxpoint.vertex_max[2] <= (*root)->node_range_z[0] || boxpoint.vertex_min[2] > (*root)->node_range_z[1]) + return 0; + if (boxpoint.vertex_min[0] <= (*root)->node_range_x[0] && boxpoint.vertex_max[0] > (*root)->node_range_x[1] && boxpoint.vertex_min[1] <= (*root)->node_range_y[0] && boxpoint.vertex_max[1] > (*root)->node_range_y[1] && boxpoint.vertex_min[2] <= (*root)->node_range_z[0] && boxpoint.vertex_max[2] > (*root)->node_range_z[1]) + { + (*root)->tree_deleted = true; + (*root)->point_deleted = true; + (*root)->need_push_down_to_left = true; + (*root)->need_push_down_to_right = true; + tmp_counter = (*root)->TreeSize - (*root)->invalid_point_num; + (*root)->invalid_point_num = (*root)->TreeSize; + if (is_downsample) + { + (*root)->tree_downsample_deleted = true; + (*root)->point_downsample_deleted = true; + (*root)->down_del_num = (*root)->TreeSize; + } + return tmp_counter; + } + if (!(*root)->point_deleted && boxpoint.vertex_min[0] <= (*root)->point.x && boxpoint.vertex_max[0] > (*root)->point.x && boxpoint.vertex_min[1] <= (*root)->point.y && boxpoint.vertex_max[1] > (*root)->point.y && boxpoint.vertex_min[2] <= (*root)->point.z && boxpoint.vertex_max[2] > (*root)->point.z) + { + (*root)->point_deleted = true; + tmp_counter += 1; + if (is_downsample) + (*root)->point_downsample_deleted = true; + } + Operation_Logger_Type delete_box_log; + struct timespec Timeout; + if (is_downsample) + delete_box_log.op = DOWNSAMPLE_DELETE; + else + delete_box_log.op = DELETE_BOX; + delete_box_log.boxpoint = boxpoint; + if ((Rebuild_Ptr == nullptr) || (*root)->left_son_ptr != *Rebuild_Ptr) + { + tmp_counter += Delete_by_range(&((*root)->left_son_ptr), boxpoint, allow_rebuild, is_downsample); + } + else + { + pthread_mutex_lock(&working_flag_mutex); + tmp_counter += Delete_by_range(&((*root)->left_son_ptr), boxpoint, false, is_downsample); + if (rebuild_flag) + { + pthread_mutex_lock(&rebuild_logger_mutex_lock); + Rebuild_Logger.push(delete_box_log); + pthread_mutex_unlock(&rebuild_logger_mutex_lock); + } + pthread_mutex_unlock(&working_flag_mutex); + } + if ((Rebuild_Ptr == nullptr) || (*root)->right_son_ptr != *Rebuild_Ptr) + { + tmp_counter += Delete_by_range(&((*root)->right_son_ptr), boxpoint, allow_rebuild, is_downsample); + } + else + { + pthread_mutex_lock(&working_flag_mutex); + tmp_counter += Delete_by_range(&((*root)->right_son_ptr), boxpoint, false, is_downsample); + if (rebuild_flag) + { + pthread_mutex_lock(&rebuild_logger_mutex_lock); + Rebuild_Logger.push(delete_box_log); + pthread_mutex_unlock(&rebuild_logger_mutex_lock); + } + pthread_mutex_unlock(&working_flag_mutex); + } + Update(*root); + if (Rebuild_Ptr != nullptr && *Rebuild_Ptr == *root && (*root)->TreeSize < Multi_Thread_Rebuild_Point_Num) + Rebuild_Ptr = nullptr; + bool need_rebuild = allow_rebuild & Criterion_Check((*root)); + if (need_rebuild) + Rebuild(root); + if ((*root) != nullptr) + (*root)->working_flag = false; + return tmp_counter; +} + +template +void KD_TREE::Delete_by_point(KD_TREE_NODE **root, PointType point, bool allow_rebuild) +{ + if ((*root) == nullptr || (*root)->tree_deleted) + return; + (*root)->working_flag = true; + Push_Down(*root); + if (same_point((*root)->point, point) && !(*root)->point_deleted) + { + (*root)->point_deleted = true; + (*root)->invalid_point_num += 1; + if ((*root)->invalid_point_num == (*root)->TreeSize) + (*root)->tree_deleted = true; + return; + } + Operation_Logger_Type delete_log; + struct timespec Timeout; + delete_log.op = DELETE_POINT; + delete_log.point = point; + if (((*root)->division_axis == 0 && point.x < (*root)->point.x) || ((*root)->division_axis == 1 && point.y < (*root)->point.y) || ((*root)->division_axis == 2 && point.z < (*root)->point.z)) + { + if ((Rebuild_Ptr == nullptr) || (*root)->left_son_ptr != *Rebuild_Ptr) + { + Delete_by_point(&(*root)->left_son_ptr, point, allow_rebuild); + } + else + { + pthread_mutex_lock(&working_flag_mutex); + Delete_by_point(&(*root)->left_son_ptr, point, false); + if (rebuild_flag) + { + pthread_mutex_lock(&rebuild_logger_mutex_lock); + Rebuild_Logger.push(delete_log); + pthread_mutex_unlock(&rebuild_logger_mutex_lock); + } + pthread_mutex_unlock(&working_flag_mutex); + } + } + else + { + if ((Rebuild_Ptr == nullptr) || (*root)->right_son_ptr != *Rebuild_Ptr) + { + Delete_by_point(&(*root)->right_son_ptr, point, allow_rebuild); + } + else + { + pthread_mutex_lock(&working_flag_mutex); + Delete_by_point(&(*root)->right_son_ptr, point, false); + if (rebuild_flag) + { + pthread_mutex_lock(&rebuild_logger_mutex_lock); + Rebuild_Logger.push(delete_log); + pthread_mutex_unlock(&rebuild_logger_mutex_lock); + } + pthread_mutex_unlock(&working_flag_mutex); + } + } + Update(*root); + if (Rebuild_Ptr != nullptr && *Rebuild_Ptr == *root && (*root)->TreeSize < Multi_Thread_Rebuild_Point_Num) + Rebuild_Ptr = nullptr; + bool need_rebuild = allow_rebuild & Criterion_Check((*root)); + if (need_rebuild) + Rebuild(root); + if ((*root) != nullptr) + (*root)->working_flag = false; + return; +} + +template +void KD_TREE::Add_by_range(KD_TREE_NODE **root, BoxPointType boxpoint, bool allow_rebuild) +{ + if ((*root) == nullptr) + return; + (*root)->working_flag = true; + Push_Down(*root); + if (boxpoint.vertex_max[0] <= (*root)->node_range_x[0] || boxpoint.vertex_min[0] > (*root)->node_range_x[1]) + return; + if (boxpoint.vertex_max[1] <= (*root)->node_range_y[0] || boxpoint.vertex_min[1] > (*root)->node_range_y[1]) + return; + if (boxpoint.vertex_max[2] <= (*root)->node_range_z[0] || boxpoint.vertex_min[2] > (*root)->node_range_z[1]) + return; + if (boxpoint.vertex_min[0] <= (*root)->node_range_x[0] && boxpoint.vertex_max[0] > (*root)->node_range_x[1] && boxpoint.vertex_min[1] <= (*root)->node_range_y[0] && boxpoint.vertex_max[1] > (*root)->node_range_y[1] && boxpoint.vertex_min[2] <= (*root)->node_range_z[0] && boxpoint.vertex_max[2] > (*root)->node_range_z[1]) + { + (*root)->tree_deleted = false || (*root)->tree_downsample_deleted; + (*root)->point_deleted = false || (*root)->point_downsample_deleted; + (*root)->need_push_down_to_left = true; + (*root)->need_push_down_to_right = true; + (*root)->invalid_point_num = (*root)->down_del_num; + return; + } + if (boxpoint.vertex_min[0] <= (*root)->point.x && boxpoint.vertex_max[0] > (*root)->point.x && boxpoint.vertex_min[1] <= (*root)->point.y && boxpoint.vertex_max[1] > (*root)->point.y && boxpoint.vertex_min[2] <= (*root)->point.z && boxpoint.vertex_max[2] > (*root)->point.z) + { + (*root)->point_deleted = (*root)->point_downsample_deleted; + } + Operation_Logger_Type add_box_log; + struct timespec Timeout; + add_box_log.op = ADD_BOX; + add_box_log.boxpoint = boxpoint; + if ((Rebuild_Ptr == nullptr) || (*root)->left_son_ptr != *Rebuild_Ptr) + { + Add_by_range(&((*root)->left_son_ptr), boxpoint, allow_rebuild); + } + else + { + pthread_mutex_lock(&working_flag_mutex); + Add_by_range(&((*root)->left_son_ptr), boxpoint, false); + if (rebuild_flag) + { + pthread_mutex_lock(&rebuild_logger_mutex_lock); + Rebuild_Logger.push(add_box_log); + pthread_mutex_unlock(&rebuild_logger_mutex_lock); + } + pthread_mutex_unlock(&working_flag_mutex); + } + if ((Rebuild_Ptr == nullptr) || (*root)->right_son_ptr != *Rebuild_Ptr) + { + Add_by_range(&((*root)->right_son_ptr), boxpoint, allow_rebuild); + } + else + { + pthread_mutex_lock(&working_flag_mutex); + Add_by_range(&((*root)->right_son_ptr), boxpoint, false); + if (rebuild_flag) + { + pthread_mutex_lock(&rebuild_logger_mutex_lock); + Rebuild_Logger.push(add_box_log); + pthread_mutex_unlock(&rebuild_logger_mutex_lock); + } + pthread_mutex_unlock(&working_flag_mutex); + } + Update(*root); + if (Rebuild_Ptr != nullptr && *Rebuild_Ptr == *root && (*root)->TreeSize < Multi_Thread_Rebuild_Point_Num) + Rebuild_Ptr = nullptr; + bool need_rebuild = allow_rebuild & Criterion_Check((*root)); + if (need_rebuild) + Rebuild(root); + if ((*root) != nullptr) + (*root)->working_flag = false; + return; +} + +template +void KD_TREE::Add_by_point(KD_TREE_NODE **root, PointType point, bool allow_rebuild, int father_axis) +{ + if (*root == nullptr) + { + *root = new KD_TREE_NODE; + InitTreeNode(*root); + (*root)->point = point; + (*root)->division_axis = (father_axis + 1) % 3; + Update(*root); + return; + } + (*root)->working_flag = true; + Operation_Logger_Type add_log; + struct timespec Timeout; + add_log.op = ADD_POINT; + add_log.point = point; + Push_Down(*root); + if (((*root)->division_axis == 0 && point.x < (*root)->point.x) || ((*root)->division_axis == 1 && point.y < (*root)->point.y) || ((*root)->division_axis == 2 && point.z < (*root)->point.z)) + { + if ((Rebuild_Ptr == nullptr) || (*root)->left_son_ptr != *Rebuild_Ptr) + { + Add_by_point(&(*root)->left_son_ptr, point, allow_rebuild, (*root)->division_axis); + } + else + { + pthread_mutex_lock(&working_flag_mutex); + Add_by_point(&(*root)->left_son_ptr, point, false, (*root)->division_axis); + if (rebuild_flag) + { + pthread_mutex_lock(&rebuild_logger_mutex_lock); + Rebuild_Logger.push(add_log); + pthread_mutex_unlock(&rebuild_logger_mutex_lock); + } + pthread_mutex_unlock(&working_flag_mutex); + } + } + else + { + if ((Rebuild_Ptr == nullptr) || (*root)->right_son_ptr != *Rebuild_Ptr) + { + Add_by_point(&(*root)->right_son_ptr, point, allow_rebuild, (*root)->division_axis); + } + else + { + pthread_mutex_lock(&working_flag_mutex); + Add_by_point(&(*root)->right_son_ptr, point, false, (*root)->division_axis); + if (rebuild_flag) + { + pthread_mutex_lock(&rebuild_logger_mutex_lock); + Rebuild_Logger.push(add_log); + pthread_mutex_unlock(&rebuild_logger_mutex_lock); + } + pthread_mutex_unlock(&working_flag_mutex); + } + } + Update(*root); + if (Rebuild_Ptr != nullptr && *Rebuild_Ptr == *root && (*root)->TreeSize < Multi_Thread_Rebuild_Point_Num) + Rebuild_Ptr = nullptr; + bool need_rebuild = allow_rebuild & Criterion_Check((*root)); + if (need_rebuild) + Rebuild(root); + if ((*root) != nullptr) + (*root)->working_flag = false; + return; +} + +template +void KD_TREE::Search(KD_TREE_NODE *root, int k_nearest, PointType point, MANUAL_HEAP &q, float max_dist) +{ + if (root == nullptr || root->tree_deleted) + return; + float cur_dist = calc_box_dist(root, point); + float max_dist_sqr = max_dist * max_dist; + if (cur_dist > max_dist_sqr) + return; + int retval; + if (root->need_push_down_to_left || root->need_push_down_to_right) + { + retval = pthread_mutex_trylock(&(root->push_down_mutex_lock)); + if (retval == 0) + { + Push_Down(root); + pthread_mutex_unlock(&(root->push_down_mutex_lock)); + } + else + { + pthread_mutex_lock(&(root->push_down_mutex_lock)); + pthread_mutex_unlock(&(root->push_down_mutex_lock)); + } + } + if (!root->point_deleted) + { + float dist = calc_dist(point, root->point); + if (dist <= max_dist_sqr && (q.size() < k_nearest || dist < q.top().dist)) + { + if (q.size() >= k_nearest) + q.pop(); + PointType_CMP current_point{root->point, dist}; + q.push(current_point); + } + } + int cur_search_counter; + float dist_left_node = calc_box_dist(root->left_son_ptr, point); + float dist_right_node = calc_box_dist(root->right_son_ptr, point); + if (q.size() < k_nearest || dist_left_node < q.top().dist && dist_right_node < q.top().dist) + { + if (dist_left_node <= dist_right_node) + { + if (Rebuild_Ptr == nullptr || *Rebuild_Ptr != root->left_son_ptr) + { + Search(root->left_son_ptr, k_nearest, point, q, max_dist); + } + else + { + pthread_mutex_lock(&search_flag_mutex); + while (search_mutex_counter == -1) + { + pthread_mutex_unlock(&search_flag_mutex); + usleep(1); + pthread_mutex_lock(&search_flag_mutex); + } + search_mutex_counter += 1; + pthread_mutex_unlock(&search_flag_mutex); + Search(root->left_son_ptr, k_nearest, point, q, max_dist); + pthread_mutex_lock(&search_flag_mutex); + search_mutex_counter -= 1; + pthread_mutex_unlock(&search_flag_mutex); + } + if (q.size() < k_nearest || dist_right_node < q.top().dist) + { + if (Rebuild_Ptr == nullptr || *Rebuild_Ptr != root->right_son_ptr) + { + Search(root->right_son_ptr, k_nearest, point, q, max_dist); + } + else + { + pthread_mutex_lock(&search_flag_mutex); + while (search_mutex_counter == -1) + { + pthread_mutex_unlock(&search_flag_mutex); + usleep(1); + pthread_mutex_lock(&search_flag_mutex); + } + search_mutex_counter += 1; + pthread_mutex_unlock(&search_flag_mutex); + Search(root->right_son_ptr, k_nearest, point, q, max_dist); + pthread_mutex_lock(&search_flag_mutex); + search_mutex_counter -= 1; + pthread_mutex_unlock(&search_flag_mutex); + } + } + } + else + { + if (Rebuild_Ptr == nullptr || *Rebuild_Ptr != root->right_son_ptr) + { + Search(root->right_son_ptr, k_nearest, point, q, max_dist); + } + else + { + pthread_mutex_lock(&search_flag_mutex); + while (search_mutex_counter == -1) + { + pthread_mutex_unlock(&search_flag_mutex); + usleep(1); + pthread_mutex_lock(&search_flag_mutex); + } + search_mutex_counter += 1; + pthread_mutex_unlock(&search_flag_mutex); + Search(root->right_son_ptr, k_nearest, point, q, max_dist); + pthread_mutex_lock(&search_flag_mutex); + search_mutex_counter -= 1; + pthread_mutex_unlock(&search_flag_mutex); + } + if (q.size() < k_nearest || dist_left_node < q.top().dist) + { + if (Rebuild_Ptr == nullptr || *Rebuild_Ptr != root->left_son_ptr) + { + Search(root->left_son_ptr, k_nearest, point, q, max_dist); + } + else + { + pthread_mutex_lock(&search_flag_mutex); + while (search_mutex_counter == -1) + { + pthread_mutex_unlock(&search_flag_mutex); + usleep(1); + pthread_mutex_lock(&search_flag_mutex); + } + search_mutex_counter += 1; + pthread_mutex_unlock(&search_flag_mutex); + Search(root->left_son_ptr, k_nearest, point, q, max_dist); + pthread_mutex_lock(&search_flag_mutex); + search_mutex_counter -= 1; + pthread_mutex_unlock(&search_flag_mutex); + } + } + } + } + else + { + if (dist_left_node < q.top().dist) + { + if (Rebuild_Ptr == nullptr || *Rebuild_Ptr != root->left_son_ptr) + { + Search(root->left_son_ptr, k_nearest, point, q, max_dist); + } + else + { + pthread_mutex_lock(&search_flag_mutex); + while (search_mutex_counter == -1) + { + pthread_mutex_unlock(&search_flag_mutex); + usleep(1); + pthread_mutex_lock(&search_flag_mutex); + } + search_mutex_counter += 1; + pthread_mutex_unlock(&search_flag_mutex); + Search(root->left_son_ptr, k_nearest, point, q, max_dist); + pthread_mutex_lock(&search_flag_mutex); + search_mutex_counter -= 1; + pthread_mutex_unlock(&search_flag_mutex); + } + } + if (dist_right_node < q.top().dist) + { + if (Rebuild_Ptr == nullptr || *Rebuild_Ptr != root->right_son_ptr) + { + Search(root->right_son_ptr, k_nearest, point, q, max_dist); + } + else + { + pthread_mutex_lock(&search_flag_mutex); + while (search_mutex_counter == -1) + { + pthread_mutex_unlock(&search_flag_mutex); + usleep(1); + pthread_mutex_lock(&search_flag_mutex); + } + search_mutex_counter += 1; + pthread_mutex_unlock(&search_flag_mutex); + Search(root->right_son_ptr, k_nearest, point, q, max_dist); + pthread_mutex_lock(&search_flag_mutex); + search_mutex_counter -= 1; + pthread_mutex_unlock(&search_flag_mutex); + } + } + } + return; +} + +template +void KD_TREE::Search_by_range(KD_TREE_NODE *root, BoxPointType boxpoint, PointVector &Storage) +{ + if (root == nullptr) + return; + Push_Down(root); + if (boxpoint.vertex_max[0] <= root->node_range_x[0] || boxpoint.vertex_min[0] > root->node_range_x[1]) + return; + if (boxpoint.vertex_max[1] <= root->node_range_y[0] || boxpoint.vertex_min[1] > root->node_range_y[1]) + return; + if (boxpoint.vertex_max[2] <= root->node_range_z[0] || boxpoint.vertex_min[2] > root->node_range_z[1]) + return; + if (boxpoint.vertex_min[0] <= root->node_range_x[0] && boxpoint.vertex_max[0] > root->node_range_x[1] && boxpoint.vertex_min[1] <= root->node_range_y[0] && boxpoint.vertex_max[1] > root->node_range_y[1] && boxpoint.vertex_min[2] <= root->node_range_z[0] && boxpoint.vertex_max[2] > root->node_range_z[1]) + { + flatten(root, Storage, NOT_RECORD); + return; + } + if (boxpoint.vertex_min[0] <= root->point.x && boxpoint.vertex_max[0] > root->point.x && boxpoint.vertex_min[1] <= root->point.y && boxpoint.vertex_max[1] > root->point.y && boxpoint.vertex_min[2] <= root->point.z && boxpoint.vertex_max[2] > root->point.z) + { + if (!root->point_deleted) + Storage.push_back(root->point); + } + if ((Rebuild_Ptr == nullptr) || root->left_son_ptr != *Rebuild_Ptr) + { + Search_by_range(root->left_son_ptr, boxpoint, Storage); + } + else + { + pthread_mutex_lock(&search_flag_mutex); + Search_by_range(root->left_son_ptr, boxpoint, Storage); + pthread_mutex_unlock(&search_flag_mutex); + } + if ((Rebuild_Ptr == nullptr) || root->right_son_ptr != *Rebuild_Ptr) + { + Search_by_range(root->right_son_ptr, boxpoint, Storage); + } + else + { + pthread_mutex_lock(&search_flag_mutex); + Search_by_range(root->right_son_ptr, boxpoint, Storage); + pthread_mutex_unlock(&search_flag_mutex); + } + return; +} + +template +void KD_TREE::Search_by_radius(KD_TREE_NODE *root, PointType point, float radius, PointVector &Storage) +{ + if (root == nullptr) + return; + Push_Down(root); + PointType range_center; + range_center.x = (root->node_range_x[0] + root->node_range_x[1]) * 0.5; + range_center.y = (root->node_range_y[0] + root->node_range_y[1]) * 0.5; + range_center.z = (root->node_range_z[0] + root->node_range_z[1]) * 0.5; + float dist = sqrt(calc_dist(range_center, point)); + if (dist > radius + sqrt(root->radius_sq)) return; + if (dist <= radius - sqrt(root->radius_sq)) + { + flatten(root, Storage, NOT_RECORD); + return; + } + if (!root->point_deleted && calc_dist(root->point, point) <= radius * radius){ + Storage.push_back(root->point); + } + if ((Rebuild_Ptr == nullptr) || root->left_son_ptr != *Rebuild_Ptr) + { + Search_by_radius(root->left_son_ptr, point, radius, Storage); + } + else + { + pthread_mutex_lock(&search_flag_mutex); + Search_by_radius(root->left_son_ptr, point, radius, Storage); + pthread_mutex_unlock(&search_flag_mutex); + } + if ((Rebuild_Ptr == nullptr) || root->right_son_ptr != *Rebuild_Ptr) + { + Search_by_radius(root->right_son_ptr, point, radius, Storage); + } + else + { + pthread_mutex_lock(&search_flag_mutex); + Search_by_radius(root->right_son_ptr, point, radius, Storage); + pthread_mutex_unlock(&search_flag_mutex); + } + return; +} + +template +bool KD_TREE::Criterion_Check(KD_TREE_NODE *root) +{ + if (root->TreeSize <= Minimal_Unbalanced_Tree_Size) + { + return false; + } + float balance_evaluation = 0.0f; + float delete_evaluation = 0.0f; + KD_TREE_NODE *son_ptr = root->left_son_ptr; + if (son_ptr == nullptr) + son_ptr = root->right_son_ptr; + delete_evaluation = float(root->invalid_point_num) / root->TreeSize; + balance_evaluation = float(son_ptr->TreeSize) / (root->TreeSize - 1); + if (delete_evaluation > delete_criterion_param) + { + return true; + } + if (balance_evaluation > balance_criterion_param || balance_evaluation < 1 - balance_criterion_param) + { + return true; + } + return false; +} + +template +void KD_TREE::Push_Down(KD_TREE_NODE *root) +{ + if (root == nullptr) + return; + Operation_Logger_Type operation; + operation.op = PUSH_DOWN; + operation.tree_deleted = root->tree_deleted; + operation.tree_downsample_deleted = root->tree_downsample_deleted; + if (root->need_push_down_to_left && root->left_son_ptr != nullptr) + { + if (Rebuild_Ptr == nullptr || *Rebuild_Ptr != root->left_son_ptr) + { + root->left_son_ptr->tree_downsample_deleted |= root->tree_downsample_deleted; + root->left_son_ptr->point_downsample_deleted |= root->tree_downsample_deleted; + root->left_son_ptr->tree_deleted = root->tree_deleted || root->left_son_ptr->tree_downsample_deleted; + root->left_son_ptr->point_deleted = root->left_son_ptr->tree_deleted || root->left_son_ptr->point_downsample_deleted; + if (root->tree_downsample_deleted) + root->left_son_ptr->down_del_num = root->left_son_ptr->TreeSize; + if (root->tree_deleted) + root->left_son_ptr->invalid_point_num = root->left_son_ptr->TreeSize; + else + root->left_son_ptr->invalid_point_num = root->left_son_ptr->down_del_num; + root->left_son_ptr->need_push_down_to_left = true; + root->left_son_ptr->need_push_down_to_right = true; + root->need_push_down_to_left = false; + } + else + { + pthread_mutex_lock(&working_flag_mutex); + root->left_son_ptr->tree_downsample_deleted |= root->tree_downsample_deleted; + root->left_son_ptr->point_downsample_deleted |= root->tree_downsample_deleted; + root->left_son_ptr->tree_deleted = root->tree_deleted || root->left_son_ptr->tree_downsample_deleted; + root->left_son_ptr->point_deleted = root->left_son_ptr->tree_deleted || root->left_son_ptr->point_downsample_deleted; + if (root->tree_downsample_deleted) + root->left_son_ptr->down_del_num = root->left_son_ptr->TreeSize; + if (root->tree_deleted) + root->left_son_ptr->invalid_point_num = root->left_son_ptr->TreeSize; + else + root->left_son_ptr->invalid_point_num = root->left_son_ptr->down_del_num; + root->left_son_ptr->need_push_down_to_left = true; + root->left_son_ptr->need_push_down_to_right = true; + if (rebuild_flag) + { + pthread_mutex_lock(&rebuild_logger_mutex_lock); + Rebuild_Logger.push(operation); + pthread_mutex_unlock(&rebuild_logger_mutex_lock); + } + root->need_push_down_to_left = false; + pthread_mutex_unlock(&working_flag_mutex); + } + } + if (root->need_push_down_to_right && root->right_son_ptr != nullptr) + { + if (Rebuild_Ptr == nullptr || *Rebuild_Ptr != root->right_son_ptr) + { + root->right_son_ptr->tree_downsample_deleted |= root->tree_downsample_deleted; + root->right_son_ptr->point_downsample_deleted |= root->tree_downsample_deleted; + root->right_son_ptr->tree_deleted = root->tree_deleted || root->right_son_ptr->tree_downsample_deleted; + root->right_son_ptr->point_deleted = root->right_son_ptr->tree_deleted || root->right_son_ptr->point_downsample_deleted; + if (root->tree_downsample_deleted) + root->right_son_ptr->down_del_num = root->right_son_ptr->TreeSize; + if (root->tree_deleted) + root->right_son_ptr->invalid_point_num = root->right_son_ptr->TreeSize; + else + root->right_son_ptr->invalid_point_num = root->right_son_ptr->down_del_num; + root->right_son_ptr->need_push_down_to_left = true; + root->right_son_ptr->need_push_down_to_right = true; + root->need_push_down_to_right = false; + } + else + { + pthread_mutex_lock(&working_flag_mutex); + root->right_son_ptr->tree_downsample_deleted |= root->tree_downsample_deleted; + root->right_son_ptr->point_downsample_deleted |= root->tree_downsample_deleted; + root->right_son_ptr->tree_deleted = root->tree_deleted || root->right_son_ptr->tree_downsample_deleted; + root->right_son_ptr->point_deleted = root->right_son_ptr->tree_deleted || root->right_son_ptr->point_downsample_deleted; + if (root->tree_downsample_deleted) + root->right_son_ptr->down_del_num = root->right_son_ptr->TreeSize; + if (root->tree_deleted) + root->right_son_ptr->invalid_point_num = root->right_son_ptr->TreeSize; + else + root->right_son_ptr->invalid_point_num = root->right_son_ptr->down_del_num; + root->right_son_ptr->need_push_down_to_left = true; + root->right_son_ptr->need_push_down_to_right = true; + if (rebuild_flag) + { + pthread_mutex_lock(&rebuild_logger_mutex_lock); + Rebuild_Logger.push(operation); + pthread_mutex_unlock(&rebuild_logger_mutex_lock); + } + root->need_push_down_to_right = false; + pthread_mutex_unlock(&working_flag_mutex); + } + } + return; +} + +template +void KD_TREE::Update(KD_TREE_NODE *root) +{ + KD_TREE_NODE *left_son_ptr = root->left_son_ptr; + KD_TREE_NODE *right_son_ptr = root->right_son_ptr; + float tmp_range_x[2] = {INFINITY, -INFINITY}; + float tmp_range_y[2] = {INFINITY, -INFINITY}; + float tmp_range_z[2] = {INFINITY, -INFINITY}; + // Update Tree Size + if (left_son_ptr != nullptr && right_son_ptr != nullptr) + { + root->TreeSize = left_son_ptr->TreeSize + right_son_ptr->TreeSize + 1; + root->invalid_point_num = left_son_ptr->invalid_point_num + right_son_ptr->invalid_point_num + (root->point_deleted ? 1 : 0); + root->down_del_num = left_son_ptr->down_del_num + right_son_ptr->down_del_num + (root->point_downsample_deleted ? 1 : 0); + root->tree_downsample_deleted = left_son_ptr->tree_downsample_deleted & right_son_ptr->tree_downsample_deleted & root->point_downsample_deleted; + root->tree_deleted = left_son_ptr->tree_deleted && right_son_ptr->tree_deleted && root->point_deleted; + if (root->tree_deleted || (!left_son_ptr->tree_deleted && !right_son_ptr->tree_deleted && !root->point_deleted)) + { + tmp_range_x[0] = min(min(left_son_ptr->node_range_x[0], right_son_ptr->node_range_x[0]), root->point.x); + tmp_range_x[1] = max(max(left_son_ptr->node_range_x[1], right_son_ptr->node_range_x[1]), root->point.x); + tmp_range_y[0] = min(min(left_son_ptr->node_range_y[0], right_son_ptr->node_range_y[0]), root->point.y); + tmp_range_y[1] = max(max(left_son_ptr->node_range_y[1], right_son_ptr->node_range_y[1]), root->point.y); + tmp_range_z[0] = min(min(left_son_ptr->node_range_z[0], right_son_ptr->node_range_z[0]), root->point.z); + tmp_range_z[1] = max(max(left_son_ptr->node_range_z[1], right_son_ptr->node_range_z[1]), root->point.z); + } + else + { + if (!left_son_ptr->tree_deleted) + { + tmp_range_x[0] = min(tmp_range_x[0], left_son_ptr->node_range_x[0]); + tmp_range_x[1] = max(tmp_range_x[1], left_son_ptr->node_range_x[1]); + tmp_range_y[0] = min(tmp_range_y[0], left_son_ptr->node_range_y[0]); + tmp_range_y[1] = max(tmp_range_y[1], left_son_ptr->node_range_y[1]); + tmp_range_z[0] = min(tmp_range_z[0], left_son_ptr->node_range_z[0]); + tmp_range_z[1] = max(tmp_range_z[1], left_son_ptr->node_range_z[1]); + } + if (!right_son_ptr->tree_deleted) + { + tmp_range_x[0] = min(tmp_range_x[0], right_son_ptr->node_range_x[0]); + tmp_range_x[1] = max(tmp_range_x[1], right_son_ptr->node_range_x[1]); + tmp_range_y[0] = min(tmp_range_y[0], right_son_ptr->node_range_y[0]); + tmp_range_y[1] = max(tmp_range_y[1], right_son_ptr->node_range_y[1]); + tmp_range_z[0] = min(tmp_range_z[0], right_son_ptr->node_range_z[0]); + tmp_range_z[1] = max(tmp_range_z[1], right_son_ptr->node_range_z[1]); + } + if (!root->point_deleted) + { + tmp_range_x[0] = min(tmp_range_x[0], root->point.x); + tmp_range_x[1] = max(tmp_range_x[1], root->point.x); + tmp_range_y[0] = min(tmp_range_y[0], root->point.y); + tmp_range_y[1] = max(tmp_range_y[1], root->point.y); + tmp_range_z[0] = min(tmp_range_z[0], root->point.z); + tmp_range_z[1] = max(tmp_range_z[1], root->point.z); + } + } + } + else if (left_son_ptr != nullptr) + { + root->TreeSize = left_son_ptr->TreeSize + 1; + root->invalid_point_num = left_son_ptr->invalid_point_num + (root->point_deleted ? 1 : 0); + root->down_del_num = left_son_ptr->down_del_num + (root->point_downsample_deleted ? 1 : 0); + root->tree_downsample_deleted = left_son_ptr->tree_downsample_deleted & root->point_downsample_deleted; + root->tree_deleted = left_son_ptr->tree_deleted && root->point_deleted; + if (root->tree_deleted || (!left_son_ptr->tree_deleted && !root->point_deleted)) + { + tmp_range_x[0] = min(left_son_ptr->node_range_x[0], root->point.x); + tmp_range_x[1] = max(left_son_ptr->node_range_x[1], root->point.x); + tmp_range_y[0] = min(left_son_ptr->node_range_y[0], root->point.y); + tmp_range_y[1] = max(left_son_ptr->node_range_y[1], root->point.y); + tmp_range_z[0] = min(left_son_ptr->node_range_z[0], root->point.z); + tmp_range_z[1] = max(left_son_ptr->node_range_z[1], root->point.z); + } + else + { + if (!left_son_ptr->tree_deleted) + { + tmp_range_x[0] = min(tmp_range_x[0], left_son_ptr->node_range_x[0]); + tmp_range_x[1] = max(tmp_range_x[1], left_son_ptr->node_range_x[1]); + tmp_range_y[0] = min(tmp_range_y[0], left_son_ptr->node_range_y[0]); + tmp_range_y[1] = max(tmp_range_y[1], left_son_ptr->node_range_y[1]); + tmp_range_z[0] = min(tmp_range_z[0], left_son_ptr->node_range_z[0]); + tmp_range_z[1] = max(tmp_range_z[1], left_son_ptr->node_range_z[1]); + } + if (!root->point_deleted) + { + tmp_range_x[0] = min(tmp_range_x[0], root->point.x); + tmp_range_x[1] = max(tmp_range_x[1], root->point.x); + tmp_range_y[0] = min(tmp_range_y[0], root->point.y); + tmp_range_y[1] = max(tmp_range_y[1], root->point.y); + tmp_range_z[0] = min(tmp_range_z[0], root->point.z); + tmp_range_z[1] = max(tmp_range_z[1], root->point.z); + } + } + } + else if (right_son_ptr != nullptr) + { + root->TreeSize = right_son_ptr->TreeSize + 1; + root->invalid_point_num = right_son_ptr->invalid_point_num + (root->point_deleted ? 1 : 0); + root->down_del_num = right_son_ptr->down_del_num + (root->point_downsample_deleted ? 1 : 0); + root->tree_downsample_deleted = right_son_ptr->tree_downsample_deleted & root->point_downsample_deleted; + root->tree_deleted = right_son_ptr->tree_deleted && root->point_deleted; + if (root->tree_deleted || (!right_son_ptr->tree_deleted && !root->point_deleted)) + { + tmp_range_x[0] = min(right_son_ptr->node_range_x[0], root->point.x); + tmp_range_x[1] = max(right_son_ptr->node_range_x[1], root->point.x); + tmp_range_y[0] = min(right_son_ptr->node_range_y[0], root->point.y); + tmp_range_y[1] = max(right_son_ptr->node_range_y[1], root->point.y); + tmp_range_z[0] = min(right_son_ptr->node_range_z[0], root->point.z); + tmp_range_z[1] = max(right_son_ptr->node_range_z[1], root->point.z); + } + else + { + if (!right_son_ptr->tree_deleted) + { + tmp_range_x[0] = min(tmp_range_x[0], right_son_ptr->node_range_x[0]); + tmp_range_x[1] = max(tmp_range_x[1], right_son_ptr->node_range_x[1]); + tmp_range_y[0] = min(tmp_range_y[0], right_son_ptr->node_range_y[0]); + tmp_range_y[1] = max(tmp_range_y[1], right_son_ptr->node_range_y[1]); + tmp_range_z[0] = min(tmp_range_z[0], right_son_ptr->node_range_z[0]); + tmp_range_z[1] = max(tmp_range_z[1], right_son_ptr->node_range_z[1]); + } + if (!root->point_deleted) + { + tmp_range_x[0] = min(tmp_range_x[0], root->point.x); + tmp_range_x[1] = max(tmp_range_x[1], root->point.x); + tmp_range_y[0] = min(tmp_range_y[0], root->point.y); + tmp_range_y[1] = max(tmp_range_y[1], root->point.y); + tmp_range_z[0] = min(tmp_range_z[0], root->point.z); + tmp_range_z[1] = max(tmp_range_z[1], root->point.z); + } + } + } + else + { + root->TreeSize = 1; + root->invalid_point_num = (root->point_deleted ? 1 : 0); + root->down_del_num = (root->point_downsample_deleted ? 1 : 0); + root->tree_downsample_deleted = root->point_downsample_deleted; + root->tree_deleted = root->point_deleted; + tmp_range_x[0] = root->point.x; + tmp_range_x[1] = root->point.x; + tmp_range_y[0] = root->point.y; + tmp_range_y[1] = root->point.y; + tmp_range_z[0] = root->point.z; + tmp_range_z[1] = root->point.z; + } + memcpy(root->node_range_x, tmp_range_x, sizeof(tmp_range_x)); + memcpy(root->node_range_y, tmp_range_y, sizeof(tmp_range_y)); + memcpy(root->node_range_z, tmp_range_z, sizeof(tmp_range_z)); + float x_L = (root->node_range_x[1] - root->node_range_x[0]) * 0.5; + float y_L = (root->node_range_y[1] - root->node_range_y[0]) * 0.5; + float z_L = (root->node_range_z[1] - root->node_range_z[0]) * 0.5; + root->radius_sq = x_L*x_L + y_L * y_L + z_L * z_L; + if (left_son_ptr != nullptr) + left_son_ptr->father_ptr = root; + if (right_son_ptr != nullptr) + right_son_ptr->father_ptr = root; + if (root == Root_Node && root->TreeSize > 3) + { + KD_TREE_NODE *son_ptr = root->left_son_ptr; + if (son_ptr == nullptr) + son_ptr = root->right_son_ptr; + float tmp_bal = float(son_ptr->TreeSize) / (root->TreeSize - 1); + root->alpha_del = float(root->invalid_point_num) / root->TreeSize; + root->alpha_bal = (tmp_bal >= 0.5 - EPSS) ? tmp_bal : 1 - tmp_bal; + } + return; +} + +template +void KD_TREE::flatten(KD_TREE_NODE *root, PointVector &Storage, delete_point_storage_set storage_type) +{ + if (root == nullptr) + return; + Push_Down(root); + if (!root->point_deleted) + { + Storage.push_back(root->point); + } + flatten(root->left_son_ptr, Storage, storage_type); + flatten(root->right_son_ptr, Storage, storage_type); + switch (storage_type) + { + case NOT_RECORD: + break; + case DELETE_POINTS_REC: + if (root->point_deleted && !root->point_downsample_deleted) + { + Points_deleted.push_back(root->point); + } + break; + case MULTI_THREAD_REC: + if (root->point_deleted && !root->point_downsample_deleted) + { + Multithread_Points_deleted.push_back(root->point); + } + break; + default: + break; + } + return; +} + +template +void KD_TREE::delete_tree_nodes(KD_TREE_NODE **root) +{ + if (*root == nullptr) + return; + Push_Down(*root); + delete_tree_nodes(&(*root)->left_son_ptr); + delete_tree_nodes(&(*root)->right_son_ptr); + + pthread_mutex_destroy(&(*root)->push_down_mutex_lock); + delete *root; + *root = nullptr; + + return; +} + +template +bool KD_TREE::same_point(PointType a, PointType b) +{ + return (fabs(a.x - b.x) < EPSS && fabs(a.y - b.y) < EPSS && fabs(a.z - b.z) < EPSS); +} + +template +float KD_TREE::calc_dist(PointType a, PointType b) +{ + float dist = 0.0f; + dist = (a.x - b.x) * (a.x - b.x) + (a.y - b.y) * (a.y - b.y) + (a.z - b.z) * (a.z - b.z); + return dist; +} + +template +float KD_TREE::calc_box_dist(KD_TREE_NODE *node, PointType point) +{ + if (node == nullptr) + return INFINITY; + float min_dist = 0.0; + if (point.x < node->node_range_x[0]) + min_dist += (point.x - node->node_range_x[0]) * (point.x - node->node_range_x[0]); + if (point.x > node->node_range_x[1]) + min_dist += (point.x - node->node_range_x[1]) * (point.x - node->node_range_x[1]); + if (point.y < node->node_range_y[0]) + min_dist += (point.y - node->node_range_y[0]) * (point.y - node->node_range_y[0]); + if (point.y > node->node_range_y[1]) + min_dist += (point.y - node->node_range_y[1]) * (point.y - node->node_range_y[1]); + if (point.z < node->node_range_z[0]) + min_dist += (point.z - node->node_range_z[0]) * (point.z - node->node_range_z[0]); + if (point.z > node->node_range_z[1]) + min_dist += (point.z - node->node_range_z[1]) * (point.z - node->node_range_z[1]); + return min_dist; +} +template +bool KD_TREE::point_cmp_x(PointType a, PointType b) { return a.x < b.x; } +template +bool KD_TREE::point_cmp_y(PointType a, PointType b) { return a.y < b.y; } +template +bool KD_TREE::point_cmp_z(PointType a, PointType b) { return a.z < b.z; } + +// Manual heap + + + +// manual queue + + +// Manual Instatiations +template class KD_TREE; +template class KD_TREE; +template class KD_TREE; + diff --git a/src/Point-LIO/include/ikd-Tree/ikd_Tree.h b/src/Point-LIO/include/ikd-Tree/ikd_Tree.h new file mode 100644 index 0000000..d4b302e --- /dev/null +++ b/src/Point-LIO/include/ikd-Tree/ikd_Tree.h @@ -0,0 +1,344 @@ +#pragma once +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#define EPSS 1e-6 +#define Minimal_Unbalanced_Tree_Size 10 +#define Multi_Thread_Rebuild_Point_Num 1500 +#define DOWNSAMPLE_SWITCH true +#define ForceRebuildPercentage 0.2 +#define Q_LEN 1000000 + +using namespace std; + +// typedef pcl::PointXYZINormal PointType; +// typedef vector> PointVector; + +struct BoxPointType +{ + float vertex_min[3]; + float vertex_max[3]; +}; + +enum operation_set +{ + ADD_POINT, + DELETE_POINT, + DELETE_BOX, + ADD_BOX, + DOWNSAMPLE_DELETE, + PUSH_DOWN +}; + +enum delete_point_storage_set +{ + NOT_RECORD, + DELETE_POINTS_REC, + MULTI_THREAD_REC +}; + +template +class KD_TREE +{ + // using MANUAL_Q_ = MANUAL_Q; + // using PointVector = std::vector; + + // using MANUAL_Q_ = MANUAL_Q; +public: + using PointVector = std::vector>; + using Ptr = std::shared_ptr>; + + struct KD_TREE_NODE + { + PointType point; + int division_axis; + int TreeSize = 1; + int invalid_point_num = 0; + int down_del_num = 0; + bool point_deleted = false; + bool tree_deleted = false; + bool point_downsample_deleted = false; + bool tree_downsample_deleted = false; + bool need_push_down_to_left = false; + bool need_push_down_to_right = false; + bool working_flag = false; + pthread_mutex_t push_down_mutex_lock; + float node_range_x[2], node_range_y[2], node_range_z[2]; + float radius_sq; + KD_TREE_NODE *left_son_ptr = nullptr; + KD_TREE_NODE *right_son_ptr = nullptr; + KD_TREE_NODE *father_ptr = nullptr; + // For paper data record + float alpha_del; + float alpha_bal; + }; + + struct Operation_Logger_Type + { + PointType point; + BoxPointType boxpoint; + bool tree_deleted, tree_downsample_deleted; + operation_set op; + }; + // static const PointType zeroP; + + struct PointType_CMP + { + PointType point; + float dist = 0.0; + PointType_CMP(PointType p = PointType(), float d = INFINITY) + { + this->point = p; + this->dist = d; + }; + bool operator<(const PointType_CMP &a) const + { + if (fabs(dist - a.dist) < 1e-10) + return point.x < a.point.x; + else + return dist < a.dist; + } + }; + + class MANUAL_HEAP + { + + public: + MANUAL_HEAP(int max_capacity = 100) + + { + cap = max_capacity; + heap = new PointType_CMP[max_capacity]; + heap_size = 0; + } + + ~MANUAL_HEAP() + { + delete[] heap; + } + void pop() + { + if (heap_size == 0) + return; + heap[0] = heap[heap_size - 1]; + heap_size--; + MoveDown(0); + return; + } + PointType_CMP top() + { + return heap[0]; + } + void push(PointType_CMP point) + { + if (heap_size >= cap) + return; + heap[heap_size] = point; + FloatUp(heap_size); + heap_size++; + return; + } + int size() + { + return heap_size; + } + void clear() + { + heap_size = 0; + return; + } + + private: + PointType_CMP *heap; + void MoveDown(int heap_index) + { + int l = heap_index * 2 + 1; + PointType_CMP tmp = heap[heap_index]; + while (l < heap_size) + { + if (l + 1 < heap_size && heap[l] < heap[l + 1]) + l++; + if (tmp < heap[l]) + { + heap[heap_index] = heap[l]; + heap_index = l; + l = heap_index * 2 + 1; + } + else + break; + } + heap[heap_index] = tmp; + return; + } + void FloatUp(int heap_index) + { + int ancestor = (heap_index - 1) / 2; + PointType_CMP tmp = heap[heap_index]; + while (heap_index > 0) + { + if (heap[ancestor] < tmp) + { + heap[heap_index] = heap[ancestor]; + heap_index = ancestor; + ancestor = (heap_index - 1) / 2; + } + else + break; + } + heap[heap_index] = tmp; + return; + } + int heap_size = 0; + int cap = 0; + }; + + class MANUAL_Q + { + private: + int head = 0, tail = 0, counter = 0; + Operation_Logger_Type q[Q_LEN]; + bool is_empty; + + public: + void pop() + { + if (counter == 0) + return; + head++; + head %= Q_LEN; + counter--; + if (counter == 0) + is_empty = true; + return; + } + Operation_Logger_Type front() + { + return q[head]; + } + Operation_Logger_Type back() + { + return q[tail]; + } + void clear() + { + head = 0; + tail = 0; + counter = 0; + is_empty = true; + return; + } + void push(Operation_Logger_Type op) + { + q[tail] = op; + counter++; + if (is_empty) + is_empty = false; + tail++; + tail %= Q_LEN; + } + bool empty() + { + return is_empty; + } + int size() + { + return counter; + } + }; + +private: + // Multi-thread Tree Rebuild + bool termination_flag = false; + bool rebuild_flag = false; + pthread_t rebuild_thread; + pthread_mutex_t termination_flag_mutex_lock, rebuild_ptr_mutex_lock, working_flag_mutex, search_flag_mutex; + pthread_mutex_t rebuild_logger_mutex_lock, points_deleted_rebuild_mutex_lock; + // queue Rebuild_Logger; + MANUAL_Q Rebuild_Logger; + PointVector Rebuild_PCL_Storage; + KD_TREE_NODE **Rebuild_Ptr = nullptr; + int search_mutex_counter = 0; + static void *multi_thread_ptr(void *arg); + void multi_thread_rebuild(); + void start_thread(); + void stop_thread(); + void run_operation(KD_TREE_NODE **root, Operation_Logger_Type operation); + // KD Tree Functions and augmented variables + int Treesize_tmp = 0, Validnum_tmp = 0; + float alpha_bal_tmp = 0.5, alpha_del_tmp = 0.0; + float delete_criterion_param = 0.5f; + float balance_criterion_param = 0.7f; + float downsample_size = 0.2f; + bool Delete_Storage_Disabled = false; + KD_TREE_NODE *STATIC_ROOT_NODE = nullptr; + PointVector Points_deleted; + PointVector Downsample_Storage; + PointVector Multithread_Points_deleted; + void InitTreeNode(KD_TREE_NODE *root); + void Test_Lock_States(KD_TREE_NODE *root); + void BuildTree(KD_TREE_NODE **root, int l, int r, PointVector &Storage); + void Rebuild(KD_TREE_NODE **root); + int Delete_by_range(KD_TREE_NODE **root, BoxPointType boxpoint, bool allow_rebuild, bool is_downsample); + void Delete_by_point(KD_TREE_NODE **root, PointType point, bool allow_rebuild); + void Add_by_point(KD_TREE_NODE **root, PointType point, bool allow_rebuild, int father_axis); + void Add_by_range(KD_TREE_NODE **root, BoxPointType boxpoint, bool allow_rebuild); + void Search(KD_TREE_NODE *root, int k_nearest, PointType point, MANUAL_HEAP &q, float max_dist); //priority_queue + void Search_by_range(KD_TREE_NODE *root, BoxPointType boxpoint, PointVector &Storage); + void Search_by_radius(KD_TREE_NODE *root, PointType point, float radius, PointVector &Storage); + bool Criterion_Check(KD_TREE_NODE *root); + void Push_Down(KD_TREE_NODE *root); + void Update(KD_TREE_NODE *root); + void delete_tree_nodes(KD_TREE_NODE **root); + void downsample(KD_TREE_NODE **root); + bool same_point(PointType a, PointType b); + float calc_dist(PointType a, PointType b); + float calc_box_dist(KD_TREE_NODE *node, PointType point); + static bool point_cmp_x(PointType a, PointType b); + static bool point_cmp_y(PointType a, PointType b); + static bool point_cmp_z(PointType a, PointType b); + +public: + KD_TREE(float delete_param = 0.5, float balance_param = 0.6, float box_length = 0.2); + ~KD_TREE(); + void Set_delete_criterion_param(float delete_param) + { + delete_criterion_param = delete_param; + } + void Set_balance_criterion_param(float balance_param) + { + balance_criterion_param = balance_param; + } + void set_downsample_param(float downsample_param) + { + downsample_size = downsample_param; + } + void InitializeKDTree(float delete_param = 0.5, float balance_param = 0.7, float box_length = 0.2); + int size(); + int validnum(); + void root_alpha(float &alpha_bal, float &alpha_del); + void Build(PointVector point_cloud); + void Nearest_Search(PointType point, int k_nearest, PointVector &Nearest_Points, vector &Point_Distance, float max_dist = INFINITY); + void Box_Search(const BoxPointType &Box_of_Point, PointVector &Storage); + void Radius_Search(PointType point, const float radius, PointVector &Storage); + int Add_Points(PointVector &PointToAdd, bool downsample_on); + void Add_Point_Boxes(vector &BoxPoints); + void Delete_Points(PointVector &PointToDel); + int Delete_Point_Boxes(vector &BoxPoints); + void flatten(KD_TREE_NODE *root, PointVector &Storage, delete_point_storage_set storage_type); + void acquire_removed_points(PointVector &removed_points); + BoxPointType tree_range(); + PointVector PCL_Storage; + KD_TREE_NODE *Root_Node = nullptr; + int max_queue_size = 0; +}; + +// template +// PointType KD_TREE::zeroP = PointType(0,0,0); diff --git a/src/Point-LIO/include/so3_math.h b/src/Point-LIO/include/so3_math.h new file mode 100755 index 0000000..d289aed --- /dev/null +++ b/src/Point-LIO/include/so3_math.h @@ -0,0 +1,113 @@ +#ifndef SO3_MATH_H +#define SO3_MATH_H + +#include +#include + +// #include + +#define SKEW_SYM_MATRX(v) 0.0,-v[2],v[1],v[2],0.0,-v[0],-v[1],v[0],0.0 + +template +Eigen::Matrix skew_sym_mat(const Eigen::Matrix &v) +{ + Eigen::Matrix skew_sym_mat; + skew_sym_mat<<0.0,-v[2],v[1],v[2],0.0,-v[0],-v[1],v[0],0.0; + return skew_sym_mat; +} + +template +Eigen::Matrix Exp(const Eigen::Matrix &&ang) +{ + T ang_norm = ang.norm(); + Eigen::Matrix Eye3 = Eigen::Matrix::Identity(); + if (ang_norm > 0.0000001) + { + Eigen::Matrix r_axis = ang / ang_norm; + Eigen::Matrix K; + K << SKEW_SYM_MATRX(r_axis); + /// Roderigous Tranformation + return Eye3 + std::sin(ang_norm) * K + (1.0 - std::cos(ang_norm)) * K * K; + } + else + { + return Eye3; + } +} + +template +Eigen::Matrix Exp(const Eigen::Matrix &ang_vel, const Ts &dt) +{ + T ang_vel_norm = ang_vel.norm(); + Eigen::Matrix Eye3 = Eigen::Matrix::Identity(); + + if (ang_vel_norm > 0.0000001) + { + Eigen::Matrix r_axis = ang_vel / ang_vel_norm; + Eigen::Matrix K; + + K << SKEW_SYM_MATRX(r_axis); + + T r_ang = ang_vel_norm * dt; + + /// Roderigous Tranformation + return Eye3 + std::sin(r_ang) * K + (1.0 - std::cos(r_ang)) * K * K; + } + else + { + return Eye3; + } +} + +template +Eigen::Matrix Exp(const T &v1, const T &v2, const T &v3) +{ + T &&norm = sqrt(v1 * v1 + v2 * v2 + v3 * v3); + Eigen::Matrix Eye3 = Eigen::Matrix::Identity(); + if (norm > 0.00001) + { + T r_ang[3] = {v1 / norm, v2 / norm, v3 / norm}; + Eigen::Matrix K; + K << SKEW_SYM_MATRX(r_ang); + + /// Roderigous Tranformation + return Eye3 + std::sin(norm) * K + (1.0 - std::cos(norm)) * K * K; + } + else + { + return Eye3; + } +} + +/* Logrithm of a Rotation Matrix */ +template +Eigen::Matrix Log(const Eigen::Matrix &R) +{ + T theta = (R.trace() > 3.0 - 1e-6) ? 0.0 : std::acos(0.5 * (R.trace() - 1)); + Eigen::Matrix K(R(2,1) - R(1,2), R(0,2) - R(2,0), R(1,0) - R(0,1)); + return (std::abs(theta) < 0.001) ? (0.5 * K) : (0.5 * theta / std::sin(theta) * K); +} + +template +Eigen::Matrix RotMtoEuler(const Eigen::Matrix &rot) +{ + T sy = sqrt(rot(0,0)*rot(0,0) + rot(1,0)*rot(1,0)); + bool singular = sy < 1e-6; + T x, y, z; + if(!singular) + { + x = atan2(rot(2, 1), rot(2, 2)); + y = atan2(-rot(2, 0), sy); + z = atan2(rot(1, 0), rot(0, 0)); + } + else + { + x = atan2(-rot(1, 2), rot(1, 1)); + y = atan2(-rot(2, 0), sy); + z = 0; + } + Eigen::Matrix ang(x, y, z); + return ang; +} + +#endif diff --git a/src/Point-LIO/launch/gdb_debug_example.launch b/src/Point-LIO/launch/gdb_debug_example.launch new file mode 100755 index 0000000..4495e76 --- /dev/null +++ b/src/Point-LIO/launch/gdb_debug_example.launch @@ -0,0 +1,25 @@ + + + + + + + + + + + + + + + + + + + + + + + launch-prefix="gdb -ex run --args" + + \ No newline at end of file diff --git a/src/Point-LIO/launch/mapping_avia.launch b/src/Point-LIO/launch/mapping_avia.launch new file mode 100755 index 0000000..2b1fc0e --- /dev/null +++ b/src/Point-LIO/launch/mapping_avia.launch @@ -0,0 +1,25 @@ + + + + + + + + + + + + + + + + + + + + + + + launch-prefix="gdb -ex run --args" + + \ No newline at end of file diff --git a/src/Point-LIO/launch/mapping_horizon.launch b/src/Point-LIO/launch/mapping_horizon.launch new file mode 100755 index 0000000..43c6968 --- /dev/null +++ b/src/Point-LIO/launch/mapping_horizon.launch @@ -0,0 +1,25 @@ + + + + + + + + + + + + + + + + + + + + + + + launch-prefix="gdb -ex run --args" + + \ No newline at end of file diff --git a/src/Point-LIO/launch/mapping_mid360.launch b/src/Point-LIO/launch/mapping_mid360.launch new file mode 100644 index 0000000..f338fd6 --- /dev/null +++ b/src/Point-LIO/launch/mapping_mid360.launch @@ -0,0 +1,25 @@ + + + + + + + + + + + + + + + + + + + + + + + launch-prefix="gdb -ex run --args" + + \ No newline at end of file diff --git a/src/Point-LIO/launch/mapping_ouster64.launch b/src/Point-LIO/launch/mapping_ouster64.launch new file mode 100755 index 0000000..5aa5fc7 --- /dev/null +++ b/src/Point-LIO/launch/mapping_ouster64.launch @@ -0,0 +1,25 @@ + + + + + + + + + + + + + + + + + + + + + + + launch-prefix="gdb -ex run --args" + + diff --git a/src/Point-LIO/launch/mapping_velody16.launch b/src/Point-LIO/launch/mapping_velody16.launch new file mode 100755 index 0000000..42e443c --- /dev/null +++ b/src/Point-LIO/launch/mapping_velody16.launch @@ -0,0 +1,25 @@ + + + + + + + + + + + + + + + + + + + + + + + launch-prefix="gdb -ex run --args" + + \ No newline at end of file diff --git a/src/Point-LIO/package.xml b/src/Point-LIO/package.xml new file mode 100755 index 0000000..2a0798d --- /dev/null +++ b/src/Point-LIO/package.xml @@ -0,0 +1,47 @@ + + + point_lio + 0.0.0 + + + This is a modified version of LOAM which is original algorithm + is described in the following paper: + J. Zhang and S. Singh. LOAM: Lidar Odometry and Mapping in Real-time. + Robotics: Science and Systems Conference (RSS). Berkeley, CA, July 2014. + + + claydergc + + BSD + + Dongjiao He + + catkin + geometry_msgs + nav_msgs + roscpp + rospy + std_msgs + sensor_msgs + tf + pcl_ros + livox_ros_driver2 + message_generation + + geometry_msgs + nav_msgs + sensor_msgs + roscpp + rospy + std_msgs + tf + pcl_ros + livox_ros_driver2 + message_runtime + + rostest + rosbag + + + + diff --git a/src/Point-LIO/rviz_cfg/.gitignore b/src/Point-LIO/rviz_cfg/.gitignore new file mode 100755 index 0000000..e69de29 diff --git a/src/Point-LIO/rviz_cfg/loam_livox.rviz b/src/Point-LIO/rviz_cfg/loam_livox.rviz new file mode 100755 index 0000000..cf2a595 --- /dev/null +++ b/src/Point-LIO/rviz_cfg/loam_livox.rviz @@ -0,0 +1,364 @@ +Panels: + - Class: rviz/Displays + Help Height: 0 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /Axes1 + - /mapping1 + - /mapping1/surround1 + - /mapping1/currPoints1 + - /mapping1/currPoints1/Autocompute Value Bounds1 + - /Odometry1/Odometry1 + - /Odometry1/Odometry1/Shape1 + - /Odometry1/Odometry1/Covariance1 + - /Odometry1/Odometry1/Covariance1/Position1 + - /Odometry1/Odometry1/Covariance1/Orientation1 + - /MarkerArray1/Namespaces1 + Splitter Ratio: 0.6432291865348816 + Tree Height: 777 + - 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 + Name: Time + SyncMode: 0 + SyncSource: surround +Preferences: + PromptSaveOnExit: true +Toolbars: + toolButtonStyle: 2 +Visualization Manager: + Class: "" + Displays: + - Alpha: 1 + Cell Size: 1000 + 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: 40 + Reference Frame: + Value: false + - Alpha: 1 + Class: rviz/Axes + Enabled: true + Length: 0.699999988079071 + Name: Axes + Radius: 0.05999999865889549 + Reference Frame: aft_mapped + Show Trail: false + Value: true + - 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: 238; 238; 236 + Color Transformer: Intensity + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Min Color: 238; 238; 236 + Name: surround + Position Transformer: XYZ + Queue Size: 1 + Selectable: false + Size (Pixels): 1 + Size (m): 0.05000000074505806 + Style: Points + Topic: /cloud_registered + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: true + - Alpha: 0.10000000149011612 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 15 + Min Value: -5 + Value: false + Axis: Z + Channel Name: intensity + Class: rviz/PointCloud2 + Color: 255; 255; 255 + Color Transformer: Intensity + Decay Time: 1000 + Enabled: true + Invert Rainbow: true + Max Color: 255; 255; 255 + Min Color: 0; 0; 0 + Name: currPoints + Position Transformer: XYZ + Queue Size: 100000 + Selectable: true + Size (Pixels): 1 + Size (m): 0.009999999776482582 + Style: Points + Topic: /cloud_registered + 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; 0 + Color Transformer: FlatColor + Decay Time: 0 + Enabled: false + Invert Rainbow: false + Max Color: 255; 255; 255 + Min Color: 0; 0; 0 + Name: PointCloud2 + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.10000000149011612 + Style: Flat Squares + Topic: /Laser_map + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: false + Enabled: true + Name: mapping + - Class: rviz/Group + Displays: + - Angle Tolerance: 0.009999999776482582 + Class: rviz/Odometry + Covariance: + Orientation: + Alpha: 0.5 + Color: 255; 255; 127 + Color Style: Unique + Frame: Local + Offset: 1 + Scale: 1 + Value: true + Position: + Alpha: 0.30000001192092896 + Color: 204; 51; 204 + Scale: 1 + Value: true + Value: true + Enabled: true + Keep: 1 + Name: Odometry + Position Tolerance: 0.0010000000474974513 + Queue Size: 10 + Shape: + Alpha: 1 + Axes Length: 1 + Axes Radius: 0.20000000298023224 + Color: 255; 85; 0 + Head Length: 0 + Head Radius: 0 + Shaft Length: 0.05000000074505806 + Shaft Radius: 0.05000000074505806 + Value: Axes + Topic: /Odometry + Unreliable: false + Value: true + Enabled: true + Name: Odometry + - Alpha: 1 + Class: rviz/Axes + Enabled: true + Length: 0.699999988079071 + Name: Axes + Radius: 0.10000000149011612 + Reference Frame: + Show Trail: false + Value: true + - Alpha: 0 + Buffer Length: 2 + Class: rviz/Path + Color: 25; 255; 255 + Enabled: true + Head Diameter: 0 + Head Length: 0 + Length: 0.30000001192092896 + Line Style: Billboards + Line Width: 0.20000000298023224 + Name: Path + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 25; 255; 255 + Pose Style: None + Queue Size: 10 + Radius: 0.029999999329447746 + Shaft Diameter: 0.4000000059604645 + Shaft Length: 0.4000000059604645 + Topic: /path + Unreliable: false + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: false + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/PointCloud2 + Color: 255; 255; 255 + Color Transformer: Intensity + Decay Time: 0 + Enabled: false + Invert Rainbow: false + Max Color: 239; 41; 41 + Max Intensity: 0 + Min Color: 239; 41; 41 + Min Intensity: 0 + Name: PointCloud2 + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 4 + Size (m): 0.30000001192092896 + Style: Spheres + Topic: /cloud_effected + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: false + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 13.139549255371094 + Min Value: -32.08251953125 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/PointCloud2 + Color: 138; 226; 52 + Color Transformer: FlatColor + Decay Time: 0 + Enabled: false + Invert Rainbow: false + Max Color: 138; 226; 52 + Min Color: 138; 226; 52 + Name: PointCloud2 + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.10000000149011612 + Style: Flat Squares + Topic: /Laser_map + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: false + - Class: rviz/MarkerArray + Enabled: false + Marker Topic: /MarkerArray + Name: MarkerArray + Namespaces: + {} + Queue Size: 100 + Value: false + Enabled: true + Global Options: + Background Color: 0; 0; 0 + Default Light: true + Fixed Frame: camera_init + Frame Rate: 10 + 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/Orbit + Distance: 38.84220504760742 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Field of View: 0.7853981852531433 + Focal Point: + X: 2.868299722671509 + Y: 11.85651683807373 + Z: 0.16846178472042084 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 0.28979647159576416 + Target Frame: global + Yaw: 4.53037691116333 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 1016 + Hide Left Dock: false + Hide Right Dock: true + QMainWindow State: 000000ff00000000fd0000000400000000000001c800000346fc020000000dfb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d00000346000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d0061006700650000000297000001dc0000000000000000fb0000000a0049006d0061006700650000000394000001600000000000000000fb0000000a0049006d00610067006501000002c5000000c70000000000000000fb0000000a0049006d00610067006501000002c5000000c70000000000000000fb0000000a0049006d00610067006501000002c5000000c700000000000000000000000100000152000004b7fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d000004b7000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000073800000052fc0100000002fb0000000800540069006d0065010000000000000738000003bc00fffffffb0000000800540069006d006501000000000000045000000000000000000000056a0000034600000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: true + Width: 1848 + X: 72 + Y: 27 diff --git a/src/Point-LIO/src/Estimator.cpp b/src/Point-LIO/src/Estimator.cpp new file mode 100755 index 0000000..f93ba96 --- /dev/null +++ b/src/Point-LIO/src/Estimator.cpp @@ -0,0 +1,452 @@ +// #include <../include/IKFoM/IKFoM_toolkit/esekfom/esekfom.hpp> +#include "Estimator.h" + +PointCloudXYZI::Ptr normvec(new PointCloudXYZI(100000, 1)); +std::vector time_seq; +PointCloudXYZI::Ptr feats_down_body(new PointCloudXYZI()); +PointCloudXYZI::Ptr feats_down_world(new PointCloudXYZI()); +std::vector pbody_list; +std::vector Nearest_Points; +KD_TREE ikdtree; +std::vector pointSearchSqDis(NUM_MATCH_POINTS); +bool point_selected_surf[100000] = {0}; +std::vector crossmat_list; +int effct_feat_num = 0; +int k; +int idx; +esekfom::esekf kf_input; +esekfom::esekf kf_output; +state_input state_in; +state_output state_out; +input_ikfom input_in; +V3D angvel_avr, acc_avr; + +V3D Lidar_T_wrt_IMU(Zero3d); +M3D Lidar_R_wrt_IMU(Eye3d); + +typedef MTK::vect<3, double> vect3; +typedef MTK::SO3 SO3; +typedef MTK::S2 S2; +typedef MTK::vect<1, double> vect1; +typedef MTK::vect<2, double> vect2; + +Eigen::Matrix process_noise_cov_input() +{ + Eigen::Matrix cov; + cov.setZero(); + cov.block<3, 3>(3, 3).diagonal() << gyr_cov_input, gyr_cov_input, gyr_cov_input; + cov.block<3, 3>(12, 12).diagonal() << acc_cov_input, acc_cov_input, acc_cov_input; + cov.block<3, 3>(15, 15).diagonal() << b_gyr_cov, b_gyr_cov, b_gyr_cov; + cov.block<3, 3>(18, 18).diagonal() << b_acc_cov, b_acc_cov, b_acc_cov; + // MTK::get_cov::type cov = MTK::get_cov::type::Zero(); + // MTK::setDiagonal(cov, &process_noise_input::ng, gyr_cov_input);// 0.03 + // MTK::setDiagonal(cov, &process_noise_input::na, acc_cov_input); // *dt 0.01 0.01 * dt * dt 0.05 + // MTK::setDiagonal(cov, &process_noise_input::nbg, b_gyr_cov); // *dt 0.00001 0.00001 * dt *dt 0.3 //0.001 0.0001 0.01 + // MTK::setDiagonal(cov, &process_noise_input::nba, b_acc_cov); //0.001 0.05 0.0001/out 0.01 + return cov; +} + +Eigen::Matrix process_noise_cov_output() +{ + Eigen::Matrix cov; + cov.setZero(); + cov.block<3, 3>(12, 12).diagonal() << vel_cov, vel_cov, vel_cov; + cov.block<3, 3>(15, 15).diagonal() << gyr_cov_output, gyr_cov_output, gyr_cov_output; + cov.block<3, 3>(18, 18).diagonal() << acc_cov_output, acc_cov_output, acc_cov_output; + cov.block<3, 3>(24, 24).diagonal() << b_gyr_cov, b_gyr_cov, b_gyr_cov; + cov.block<3, 3>(27, 27).diagonal() << b_acc_cov, b_acc_cov, b_acc_cov; + // MTK::get_cov::type cov = MTK::get_cov::type::Zero(); + // MTK::setDiagonal(cov, &process_noise_output::vel, vel_cov);// 0.03 + // MTK::setDiagonal(cov, &process_noise_output::ng, gyr_cov_output); // *dt 0.01 0.01 * dt * dt 0.05 + // MTK::setDiagonal(cov, &process_noise_output::na, acc_cov_output); // *dt 0.00001 0.00001 * dt *dt 0.3 //0.001 0.0001 0.01 + // MTK::setDiagonal(cov, &process_noise_output::nbg, b_gyr_cov); //0.001 0.05 0.0001/out 0.01 + // MTK::setDiagonal(cov, &process_noise_output::nba, b_acc_cov); //0.001 0.05 0.0001/out 0.01 + return cov; +} + +Eigen::Matrix get_f_input(state_input &s, const input_ikfom &in) +{ + Eigen::Matrix res = Eigen::Matrix::Zero(); + vect3 omega; + in.gyro.boxminus(omega, s.bg); + vect3 a_inertial = s.rot * (in.acc-s.ba); + for(int i = 0; i < 3; i++ ){ + res(i) = s.vel[i]; + res(i + 3) = omega[i]; + res(i + 12) = a_inertial[i] + s.gravity[i]; + } + return res; +} + +Eigen::Matrix get_f_output(state_output &s, const input_ikfom &in) +{ + Eigen::Matrix res = Eigen::Matrix::Zero(); + vect3 a_inertial = s.rot * s.acc; + for(int i = 0; i < 3; i++ ){ + res(i) = s.vel[i]; + res(i + 3) = s.omg[i]; + res(i + 12) = a_inertial[i] + s.gravity[i]; + } + return res; +} + +Eigen::Matrix df_dx_input(state_input &s, const input_ikfom &in) +{ + Eigen::Matrix cov = Eigen::Matrix::Zero(); + cov.template block<3, 3>(0, 12) = Eigen::Matrix3d::Identity(); + vect3 acc_; + in.acc.boxminus(acc_, s.ba); + vect3 omega; + in.gyro.boxminus(omega, s.bg); + cov.template block<3, 3>(12, 3) = -s.rot*MTK::hat(acc_); + cov.template block<3, 3>(12, 18) = -s.rot; + // Eigen::Matrix vec = Eigen::Matrix::Zero(); + // Eigen::Matrix grav_matrix; + // s.S2_Mx(grav_matrix, vec, 21); + cov.template block<3, 3>(12, 21) = Eigen::Matrix3d::Identity(); // grav_matrix; + cov.template block<3, 3>(3, 15) = -Eigen::Matrix3d::Identity(); + return cov; +} + +// Eigen::Matrix df_dw_input(state_input &s, const input_ikfom &in) +// { +// Eigen::Matrix cov = Eigen::Matrix::Zero(); +// cov.template block<3, 3>(12, 3) = -s.rot.normalized().toRotationMatrix(); +// cov.template block<3, 3>(3, 0) = -Eigen::Matrix3d::Identity(); +// cov.template block<3, 3>(15, 6) = Eigen::Matrix3d::Identity(); +// cov.template block<3, 3>(18, 9) = Eigen::Matrix3d::Identity(); +// return cov; +// } + +Eigen::Matrix df_dx_output(state_output &s, const input_ikfom &in) +{ + Eigen::Matrix cov = Eigen::Matrix::Zero(); + cov.template block<3, 3>(0, 12) = Eigen::Matrix3d::Identity(); + cov.template block<3, 3>(12, 3) = -s.rot*MTK::hat(s.acc); + cov.template block<3, 3>(12, 18) = s.rot; + // Eigen::Matrix vec = Eigen::Matrix::Zero(); + // Eigen::Matrix grav_matrix; + // s.S2_Mx(grav_matrix, vec, 21); + cov.template block<3, 3>(12, 21) = Eigen::Matrix3d::Identity(); // grav_matrix; + cov.template block<3, 3>(3, 15) = Eigen::Matrix3d::Identity(); + return cov; +} + +// Eigen::Matrix df_dw_output(state_output &s) +// { +// Eigen::Matrix cov = Eigen::Matrix::Zero(); +// cov.template block<3, 3>(12, 0) = Eigen::Matrix3d::Identity(); +// cov.template block<3, 3>(15, 3) = Eigen::Matrix3d::Identity(); +// cov.template block<3, 3>(18, 6) = Eigen::Matrix3d::Identity(); +// cov.template block<3, 3>(24, 9) = Eigen::Matrix3d::Identity(); +// cov.template block<3, 3>(27, 12) = Eigen::Matrix3d::Identity(); +// return cov; +// } + +vect3 SO3ToEuler(const SO3 &rot) +{ + // Eigen::Matrix _ang; + // Eigen::Vector4d q_data = orient.coeffs().transpose(); + // //scalar w=orient.coeffs[3], x=orient.coeffs[0], y=orient.coeffs[1], z=orient.coeffs[2]; + // double sqw = q_data[3]*q_data[3]; + // double sqx = q_data[0]*q_data[0]; + // double sqy = q_data[1]*q_data[1]; + // double sqz = q_data[2]*q_data[2]; + // double unit = sqx + sqy + sqz + sqw; // if normalized is one, otherwise is correction factor + // double test = q_data[3]*q_data[1] - q_data[2]*q_data[0]; + + // if (test > 0.49999*unit) { // singularity at north pole + + // _ang << 2 * std::atan2(q_data[0], q_data[3]), M_PI/2, 0; + // double temp[3] = {_ang[0] * 57.3, _ang[1] * 57.3, _ang[2] * 57.3}; + // vect3 euler_ang(temp, 3); + // return euler_ang; + // } + // if (test < -0.49999*unit) { // singularity at south pole + // _ang << -2 * std::atan2(q_data[0], q_data[3]), -M_PI/2, 0; + // double temp[3] = {_ang[0] * 57.3, _ang[1] * 57.3, _ang[2] * 57.3}; + // vect3 euler_ang(temp, 3); + // return euler_ang; + // } + + // _ang << + // std::atan2(2*q_data[0]*q_data[3]+2*q_data[1]*q_data[2] , -sqx - sqy + sqz + sqw), + // std::asin (2*test/unit), + // std::atan2(2*q_data[2]*q_data[3]+2*q_data[1]*q_data[0] , sqx - sqy - sqz + sqw); + // double temp[3] = {_ang[0] * 57.3, _ang[1] * 57.3, _ang[2] * 57.3}; + // vect3 euler_ang(temp, 3); + // return euler_ang; + double sy = sqrt(rot(0,0)*rot(0,0) + rot(1,0)*rot(1,0)); + bool singular = sy < 1e-6; + double x, y, z; + if(!singular) + { + x = atan2(rot(2, 1), rot(2, 2)); + y = atan2(-rot(2, 0), sy); + z = atan2(rot(1, 0), rot(0, 0)); + } + else + { + x = atan2(-rot(1, 2), rot(1, 1)); + y = atan2(-rot(2, 0), sy); + z = 0; + } + Eigen::Matrix ang(x, y, z); + return ang; +} + +void h_model_input(state_input &s, esekfom::dyn_share_modified &ekfom_data) +{ + bool match_in_map = false; + VF(4) pabcd; + pabcd.setZero(); + normvec->resize(time_seq[k]); + int effect_num_k = 0; + for (int j = 0; j < time_seq[k]; j++) + { + PointType &point_body_j = feats_down_body->points[idx+j+1]; + PointType &point_world_j = feats_down_world->points[idx+j+1]; + pointBodyToWorld(&point_body_j, &point_world_j); + V3D p_body = pbody_list[idx+j+1]; + V3D p_world; + p_world << point_world_j.x, point_world_j.y, point_world_j.z; + + { + auto &points_near = Nearest_Points[idx+j+1]; + + ikdtree.Nearest_Search(point_world_j, NUM_MATCH_POINTS, points_near, pointSearchSqDis, 2.236); //1.0); //, 3.0); // 2.236; + + if ((points_near.size() < NUM_MATCH_POINTS) || pointSearchSqDis[NUM_MATCH_POINTS - 1] > 5) // 5) + { + point_selected_surf[idx+j+1] = false; + } + else + { + point_selected_surf[idx+j+1] = false; + if (esti_plane(pabcd, points_near, plane_thr)) //(planeValid) + { + float pd2 = pabcd(0) * point_world_j.x + pabcd(1) * point_world_j.y + pabcd(2) * point_world_j.z + pabcd(3); + + if (p_body.norm() > match_s * pd2 * pd2) + { + point_selected_surf[idx+j+1] = true; + normvec->points[j].x = pabcd(0); + normvec->points[j].y = pabcd(1); + normvec->points[j].z = pabcd(2); + normvec->points[j].intensity = pabcd(3); + effect_num_k ++; + } + } + } + } + } + if (effect_num_k == 0) + { + ekfom_data.valid = false; + return; + } + ekfom_data.M_Noise = laser_point_cov; + ekfom_data.h_x = Eigen::MatrixXd::Zero(effect_num_k, 12); + ekfom_data.z.resize(effect_num_k); + int m = 0; + for (int j = 0; j < time_seq[k]; j++) + { + if(point_selected_surf[idx+j+1]) + { + V3D norm_vec(normvec->points[j].x, normvec->points[j].y, normvec->points[j].z); + + if (extrinsic_est_en) + { + V3D p_body = pbody_list[idx+j+1]; + M3D p_crossmat, p_imu_crossmat; + p_crossmat << SKEW_SYM_MATRX(p_body); + V3D point_imu = s.offset_R_L_I * p_body + s.offset_T_L_I; + p_imu_crossmat << SKEW_SYM_MATRX(point_imu); + V3D C(s.rot.transpose() * norm_vec); + V3D A(p_imu_crossmat * C); + V3D B(p_crossmat * s.offset_R_L_I.transpose() * C); + ekfom_data.h_x.block<1, 12>(m, 0) << norm_vec(0), norm_vec(1), norm_vec(2), VEC_FROM_ARRAY(A), VEC_FROM_ARRAY(B), VEC_FROM_ARRAY(C); + } + else + { + M3D point_crossmat = crossmat_list[idx+j+1]; + V3D C(s.rot.transpose() * norm_vec); + V3D A(point_crossmat * C); + ekfom_data.h_x.block<1, 12>(m, 0) << norm_vec(0), norm_vec(1), norm_vec(2), VEC_FROM_ARRAY(A), 0.0, 0.0, 0.0, 0.0, 0.0, 0.0; + } + ekfom_data.z(m) = -norm_vec(0) * feats_down_world->points[idx+j+1].x -norm_vec(1) * feats_down_world->points[idx+j+1].y -norm_vec(2) * feats_down_world->points[idx+j+1].z-normvec->points[j].intensity; + m++; + } + } + effct_feat_num += effect_num_k; +} + +void h_model_output(state_output &s, esekfom::dyn_share_modified &ekfom_data) +{ + bool match_in_map = false; + VF(4) pabcd; + pabcd.setZero(); + + normvec->resize(time_seq[k]); + int effect_num_k = 0; + for (int j = 0; j < time_seq[k]; j++) + { + PointType &point_body_j = feats_down_body->points[idx+j+1]; + PointType &point_world_j = feats_down_world->points[idx+j+1]; + pointBodyToWorld(&point_body_j, &point_world_j); + V3D p_body = pbody_list[idx+j+1]; + V3D p_world; + p_world << point_world_j.x, point_world_j.y, point_world_j.z; + { + auto &points_near = Nearest_Points[idx+j+1]; + + ikdtree.Nearest_Search(point_world_j, NUM_MATCH_POINTS, points_near, pointSearchSqDis, 2.236); + + if ((points_near.size() < NUM_MATCH_POINTS) || pointSearchSqDis[NUM_MATCH_POINTS - 1] > 5) + { + point_selected_surf[idx+j+1] = false; + } + else + { + point_selected_surf[idx+j+1] = false; + if (esti_plane(pabcd, points_near, plane_thr)) //(planeValid) + { + float pd2 = pabcd(0) * point_world_j.x + pabcd(1) * point_world_j.y + pabcd(2) * point_world_j.z + pabcd(3); + + if (p_body.norm() > match_s * pd2 * pd2) + { + // point_selected_surf[i] = true; + point_selected_surf[idx+j+1] = true; + normvec->points[j].x = pabcd(0); + normvec->points[j].y = pabcd(1); + normvec->points[j].z = pabcd(2); + normvec->points[j].intensity = pabcd(3); + effect_num_k ++; + } + } + } + } + } + if (effect_num_k == 0) + { + ekfom_data.valid = false; + return; + } + ekfom_data.M_Noise = laser_point_cov; + ekfom_data.h_x = Eigen::MatrixXd::Zero(effect_num_k, 12); + ekfom_data.z.resize(effect_num_k); + int m = 0; + for (int j = 0; j < time_seq[k]; j++) + { + if(point_selected_surf[idx+j+1]) + { + V3D norm_vec(normvec->points[j].x, normvec->points[j].y, normvec->points[j].z); + + if (extrinsic_est_en) + { + V3D p_body = pbody_list[idx+j+1]; + M3D p_crossmat, p_imu_crossmat; + p_crossmat << SKEW_SYM_MATRX(p_body); + V3D point_imu = s.offset_R_L_I * p_body + s.offset_T_L_I; + p_imu_crossmat << SKEW_SYM_MATRX(point_imu); + V3D C(s.rot.transpose() * norm_vec); + V3D A(p_imu_crossmat * C); + V3D B(p_crossmat * s.offset_R_L_I.transpose() * C); + ekfom_data.h_x.block<1, 12>(m, 0) << norm_vec(0), norm_vec(1), norm_vec(2), VEC_FROM_ARRAY(A), VEC_FROM_ARRAY(B), VEC_FROM_ARRAY(C); + } + else + { + M3D point_crossmat = crossmat_list[idx+j+1]; + V3D C(s.rot.transpose() * norm_vec); + V3D A(point_crossmat * C); + // V3D A(point_crossmat * state.rot_end.transpose() * norm_vec); + ekfom_data.h_x.block<1, 12>(m, 0) << norm_vec(0), norm_vec(1), norm_vec(2), VEC_FROM_ARRAY(A), 0.0, 0.0, 0.0, 0.0, 0.0, 0.0; + } + ekfom_data.z(m) = -norm_vec(0) * feats_down_world->points[idx+j+1].x -norm_vec(1) * feats_down_world->points[idx+j+1].y -norm_vec(2) * feats_down_world->points[idx+j+1].z-normvec->points[j].intensity; + m++; + } + } + effct_feat_num += effect_num_k; +} + +void h_model_IMU_output(state_output &s, esekfom::dyn_share_modified &ekfom_data) +{ + std::memset(ekfom_data.satu_check, false, 6); + ekfom_data.z_IMU.block<3,1>(0, 0) = angvel_avr - s.omg - s.bg; + ekfom_data.z_IMU.block<3,1>(3, 0) = acc_avr * G_m_s2 / acc_norm - s.acc - s.ba; + ekfom_data.R_IMU << imu_meas_omg_cov, imu_meas_omg_cov, imu_meas_omg_cov, imu_meas_acc_cov, imu_meas_acc_cov, imu_meas_acc_cov; + if(check_satu) + { + if(fabs(angvel_avr(0)) >= 0.99 * satu_gyro) + { + ekfom_data.satu_check[0] = true; + ekfom_data.z_IMU(0) = 0.0; + } + + if(fabs(angvel_avr(1)) >= 0.99 * satu_gyro) + { + ekfom_data.satu_check[1] = true; + ekfom_data.z_IMU(1) = 0.0; + } + + if(fabs(angvel_avr(2)) >= 0.99 * satu_gyro) + { + ekfom_data.satu_check[2] = true; + ekfom_data.z_IMU(2) = 0.0; + } + + if(fabs(acc_avr(0)) >= 0.99 * satu_acc) + { + ekfom_data.satu_check[3] = true; + ekfom_data.z_IMU(3) = 0.0; + } + + if(fabs(acc_avr(1)) >= 0.99 * satu_acc) + { + ekfom_data.satu_check[4] = true; + ekfom_data.z_IMU(4) = 0.0; + } + + if(fabs(acc_avr(2)) >= 0.99 * satu_acc) + { + ekfom_data.satu_check[5] = true; + ekfom_data.z_IMU(5) = 0.0; + } + } +} + +void pointBodyToWorld(PointType const * const pi, PointType * const po) +{ + V3D p_body(pi->x, pi->y, pi->z); + + V3D p_global; + if (extrinsic_est_en) + { + if (!use_imu_as_input) + { + p_global = kf_output.x_.rot * (kf_output.x_.offset_R_L_I * p_body + kf_output.x_.offset_T_L_I) + kf_output.x_.pos; + } + else + { + p_global = kf_input.x_.rot * (kf_input.x_.offset_R_L_I * p_body + kf_input.x_.offset_T_L_I) + kf_input.x_.pos; + } + } + else + { + if (!use_imu_as_input) + { + p_global = kf_output.x_.rot * (Lidar_R_wrt_IMU * p_body + Lidar_T_wrt_IMU) + kf_output.x_.pos; + } + else + { + p_global = kf_input.x_.rot * (Lidar_R_wrt_IMU * p_body + Lidar_T_wrt_IMU) + kf_input.x_.pos; + } + } + + po->x = p_global(0); + po->y = p_global(1); + po->z = p_global(2); + po->intensity = pi->intensity; +} + +const bool time_list(PointType &x, PointType &y) {return (x.curvature < y.curvature);}; \ No newline at end of file diff --git a/src/Point-LIO/src/Estimator.h b/src/Point-LIO/src/Estimator.h new file mode 100755 index 0000000..5f368df --- /dev/null +++ b/src/Point-LIO/src/Estimator.h @@ -0,0 +1,118 @@ +#ifndef Estimator_H +#define Estimator_H + +#include <../include/IKFoM/IKFoM_toolkit/esekfom/esekfom.hpp> +#include "common_lib.h" +#include "parameters.h" +#include +#include +#include +#include +#include +#include + +extern PointCloudXYZI::Ptr normvec; //(new PointCloudXYZI(100000, 1)); +extern std::vector time_seq; +extern PointCloudXYZI::Ptr feats_down_body; //(new PointCloudXYZI()); +extern PointCloudXYZI::Ptr feats_down_world; //(new PointCloudXYZI()); +extern std::vector pbody_list; +extern std::vector Nearest_Points; +extern KD_TREE ikdtree; +extern std::vector pointSearchSqDis; +extern bool point_selected_surf[100000]; // = {0}; +extern std::vector crossmat_list; +extern int effct_feat_num; +extern int k; +extern int idx; +extern V3D angvel_avr, acc_avr; + +extern V3D Lidar_T_wrt_IMU; //(Zero3d); +extern M3D Lidar_R_wrt_IMU; //(Eye3d); + +typedef MTK::vect<3, double> vect3; +typedef MTK::SO3 SO3; +typedef MTK::S2 S2; +typedef MTK::vect<1, double> vect1; +typedef MTK::vect<2, double> vect2; + +MTK_BUILD_MANIFOLD(state_input, +((vect3, pos)) +((SO3, rot)) +((SO3, offset_R_L_I)) +((vect3, offset_T_L_I)) +((vect3, vel)) +((vect3, bg)) +((vect3, ba)) +((vect3, gravity)) +); + +MTK_BUILD_MANIFOLD(state_output, +((vect3, pos)) +((SO3, rot)) +((SO3, offset_R_L_I)) +((vect3, offset_T_L_I)) +((vect3, vel)) +((vect3, omg)) +((vect3, acc)) +((vect3, gravity)) +((vect3, bg)) +((vect3, ba)) +); + +MTK_BUILD_MANIFOLD(input_ikfom, +((vect3, acc)) +((vect3, gyro)) +); + +MTK_BUILD_MANIFOLD(process_noise_input, +((vect3, ng)) +((vect3, na)) +((vect3, nbg)) +((vect3, nba)) +); + +MTK_BUILD_MANIFOLD(process_noise_output, +((vect3, vel)) +((vect3, ng)) +((vect3, na)) +((vect3, nbg)) +((vect3, nba)) +); + +extern esekfom::esekf kf_input; +extern esekfom::esekf kf_output; +extern state_input state_in; +extern state_output state_out; +extern input_ikfom input_in; + +Eigen::Matrix process_noise_cov_input(); + +Eigen::Matrix process_noise_cov_output(); + +//double L_offset_to_I[3] = {0.04165, 0.02326, -0.0284}; // Avia +//vect3 Lidar_offset_to_IMU(L_offset_to_I, 3); +Eigen::Matrix get_f_input(state_input &s, const input_ikfom &in); + +Eigen::Matrix get_f_output(state_output &s, const input_ikfom &in); + +Eigen::Matrix df_dx_input(state_input &s, const input_ikfom &in); + +// Eigen::Matrix df_dw_input(state_input &s, const input_ikfom &in); + +Eigen::Matrix df_dx_output(state_output &s, const input_ikfom &in); + +// Eigen::Matrix df_dw_output(state_output &s); + +vect3 SO3ToEuler(const SO3 &orient); + +void h_model_input(state_input &s, esekfom::dyn_share_modified &ekfom_data); + +void h_model_output(state_output &s, esekfom::dyn_share_modified &ekfom_data); + +void h_model_IMU_output(state_output &s, esekfom::dyn_share_modified &ekfom_data); + +void pointBodyToWorld(PointType const * const pi, PointType * const po); + +const bool time_list(PointType &x, PointType &y); // {return (x.curvature < y.curvature);}; + +#endif \ No newline at end of file diff --git a/src/Point-LIO/src/IMU_Processing.hpp b/src/Point-LIO/src/IMU_Processing.hpp new file mode 100755 index 0000000..0ccdf69 --- /dev/null +++ b/src/Point-LIO/src/IMU_Processing.hpp @@ -0,0 +1,181 @@ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +/// *************Preconfiguration + +#define MAX_INI_COUNT (100) + +/// *************IMU Process and undistortion +class ImuProcess +{ + public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + ImuProcess(); + ~ImuProcess(); + + void Reset(); + void Reset(double start_timestamp, const sensor_msgs::ImuConstPtr &lastimu); + void Process(const MeasureGroup &meas, PointCloudXYZI::Ptr pcl_un_); + void Set_init(Eigen::Vector3d &tmp_gravity, Eigen::Matrix3d &rot); + + ofstream fout_imu; + // double first_lidar_time; + int lidar_type; + bool imu_en; + V3D mean_acc, gravity_; + bool imu_need_init_ = true; + bool b_first_frame_ = true; + bool gravity_align_ = false; + + private: + void IMU_init(const MeasureGroup &meas, int &N); + V3D mean_gyr; + int init_iter_num = 1; +}; + +ImuProcess::ImuProcess() + : b_first_frame_(true), imu_need_init_(true), gravity_align_(false) +{ + imu_en = true; + init_iter_num = 1; + mean_acc = V3D(0, 0, 0.0); + mean_gyr = V3D(0, 0, 0); +} + +ImuProcess::~ImuProcess() {} + +void ImuProcess::Reset() +{ + ROS_WARN("Reset ImuProcess"); + mean_acc = V3D(0, 0, 0.0); + mean_gyr = V3D(0, 0, 0); + imu_need_init_ = true; + init_iter_num = 1; +} + +void ImuProcess::IMU_init(const MeasureGroup &meas, int &N) +{ + /** 1. initializing the gravity, gyro bias, acc and gyro covariance + ** 2. normalize the acceleration measurenments to unit gravity **/ + ROS_INFO("IMU Initializing: %.1f %%", double(N) / MAX_INI_COUNT * 100); + V3D cur_acc, cur_gyr; + + if (b_first_frame_) + { + Reset(); + N = 1; + b_first_frame_ = false; + const auto &imu_acc = meas.imu.front()->linear_acceleration; + const auto &gyr_acc = meas.imu.front()->angular_velocity; + mean_acc << imu_acc.x, imu_acc.y, imu_acc.z; + mean_gyr << gyr_acc.x, gyr_acc.y, gyr_acc.z; + } + + for (const auto &imu : meas.imu) + { + const auto &imu_acc = imu->linear_acceleration; + const auto &gyr_acc = imu->angular_velocity; + cur_acc << imu_acc.x, imu_acc.y, imu_acc.z; + cur_gyr << gyr_acc.x, gyr_acc.y, gyr_acc.z; + + mean_acc += (cur_acc - mean_acc) / N; + mean_gyr += (cur_gyr - mean_gyr) / N; + + N ++; + } +} + +void ImuProcess::Process(const MeasureGroup &meas, PointCloudXYZI::Ptr cur_pcl_un_) +{ + if (imu_en) + { + if(meas.imu.empty()) return; + ROS_ASSERT(meas.lidar != nullptr); + + if (imu_need_init_) + { + /// The very first lidar frame + IMU_init(meas, init_iter_num); + + imu_need_init_ = true; + + if (init_iter_num > MAX_INI_COUNT) + { + ROS_INFO("IMU Initializing: %.1f %%", 100.0); + imu_need_init_ = false; + *cur_pcl_un_ = *(meas.lidar); + } + return; + } + // if (!gravity_align_) gravity_align_ = true; + *cur_pcl_un_ = *(meas.lidar); + return; + } + else + { + // if (!b_first_frame_) + // {if (!gravity_align_) gravity_align_ = true;} + // else + // {b_first_frame_ = false; + // } + if (imu_need_init_) + { + imu_need_init_ = false; + } + *cur_pcl_un_ = *(meas.lidar); + return; + } +} + +void ImuProcess::Set_init(Eigen::Vector3d &tmp_gravity, Eigen::Matrix3d &rot) +{ + /** 1. initializing the gravity, gyro bias, acc and gyro covariance + ** 2. normalize the acceleration measurenments to unit gravity **/ + // V3D tmp_gravity = - mean_acc / mean_acc.norm() * G_m_s2; // state_gravity; + M3D hat_grav; + hat_grav << 0.0, gravity_(2), -gravity_(1), + -gravity_(2), 0.0, gravity_(0), + gravity_(1), -gravity_(0), 0.0; + double align_norm = (hat_grav * tmp_gravity).norm() / tmp_gravity.norm() / gravity_.norm(); + double align_cos = gravity_.transpose() * tmp_gravity; + align_cos = align_cos / gravity_.norm() / tmp_gravity.norm(); + if (align_norm < 1e-6) + { + if (align_cos > 1e-6) + { + rot = Eye3d; + } + else + { + rot = -Eye3d; + } + } + else + { + V3D align_angle = hat_grav * tmp_gravity / (hat_grav * tmp_gravity).norm() * acos(align_cos); + rot = Exp(align_angle(0), align_angle(1), align_angle(2)); + } +} \ No newline at end of file diff --git a/src/Point-LIO/src/laserMapping.cpp b/src/Point-LIO/src/laserMapping.cpp new file mode 100755 index 0000000..5f2ab2b --- /dev/null +++ b/src/Point-LIO/src/laserMapping.cpp @@ -0,0 +1,1378 @@ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include "IMU_Processing.hpp" +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include "parameters.h" +#include "Estimator.h" +// #include + + +#define MAXN (720000) +#define PUBFRAME_PERIOD (20) + +const float MOV_THRESHOLD = 1.5f; + +mutex mtx_buffer; +condition_variable sig_buffer; + +string root_dir = ROOT_DIR; + +int feats_down_size = 0, time_log_counter = 0, scan_count = 0, publish_count = 0; + +int frame_ct = 0; +double time_update_last = 0.0, time_current = 0.0, time_predict_last_const = 0.0, t_last = 0.0; + +shared_ptr p_imu(new ImuProcess()); +bool init_map = false, flg_first_scan = true; +PointCloudXYZI::Ptr ptr_con(new PointCloudXYZI()); + +// Time Log Variables +double T1[MAXN], s_plot[MAXN], s_plot2[MAXN], s_plot3[MAXN], s_plot11[MAXN]; +double match_time = 0, solve_time = 0, propag_time = 0, update_time = 0; + +bool lidar_pushed = false, flg_reset = false, flg_exit = false; + +vector cub_needrm; + +deque lidar_buffer; +deque time_buffer; +deque imu_deque; + +//surf feature in map +PointCloudXYZI::Ptr feats_undistort(new PointCloudXYZI()); +PointCloudXYZI::Ptr feats_down_body_space(new PointCloudXYZI()); +PointCloudXYZI::Ptr init_feats_world(new PointCloudXYZI()); + +pcl::VoxelGrid downSizeFilterSurf; +pcl::VoxelGrid downSizeFilterMap; + +V3D euler_cur; + +MeasureGroup Measures; + +sensor_msgs::Imu imu_last, imu_next; +sensor_msgs::Imu::ConstPtr imu_last_ptr; +nav_msgs::Path path; +nav_msgs::Odometry odomAftMapped; +geometry_msgs::PoseStamped msg_body_pose; + +void SigHandle(int sig) +{ + flg_exit = true; + ROS_WARN("catch sig %d", sig); + sig_buffer.notify_all(); +} + +inline void dump_lio_state_to_log(FILE *fp) +{ + V3D rot_ang; + if (!use_imu_as_input) + { + rot_ang = SO3ToEuler(kf_output.x_.rot); + } + else + { + rot_ang = SO3ToEuler(kf_input.x_.rot); + } + + fprintf(fp, "%lf ", Measures.lidar_beg_time - first_lidar_time); + fprintf(fp, "%lf %lf %lf ", rot_ang(0), rot_ang(1), rot_ang(2)); // Angle + if (use_imu_as_input) + { + fprintf(fp, "%lf %lf %lf ", kf_input.x_.pos(0), kf_input.x_.pos(1), kf_input.x_.pos(2)); // Pos + fprintf(fp, "%lf %lf %lf ", 0.0, 0.0, 0.0); // omega + fprintf(fp, "%lf %lf %lf ", kf_input.x_.vel(0), kf_input.x_.vel(1), kf_input.x_.vel(2)); // Vel + fprintf(fp, "%lf %lf %lf ", 0.0, 0.0, 0.0); // Acc + fprintf(fp, "%lf %lf %lf ", kf_input.x_.bg(0), kf_input.x_.bg(1), kf_input.x_.bg(2)); // Bias_g + fprintf(fp, "%lf %lf %lf ", kf_input.x_.ba(0), kf_input.x_.ba(1), kf_input.x_.ba(2)); // Bias_a + fprintf(fp, "%lf %lf %lf ", kf_input.x_.gravity(0), kf_input.x_.gravity(1), kf_input.x_.gravity(2)); // Bias_a + } + else + { + fprintf(fp, "%lf %lf %lf ", kf_output.x_.pos(0), kf_output.x_.pos(1), kf_output.x_.pos(2)); // Pos + fprintf(fp, "%lf %lf %lf ", 0.0, 0.0, 0.0); // omega + fprintf(fp, "%lf %lf %lf ", kf_output.x_.vel(0), kf_output.x_.vel(1), kf_output.x_.vel(2)); // Vel + fprintf(fp, "%lf %lf %lf ", 0.0, 0.0, 0.0); // Acc + fprintf(fp, "%lf %lf %lf ", kf_output.x_.bg(0), kf_output.x_.bg(1), kf_output.x_.bg(2)); // Bias_g + fprintf(fp, "%lf %lf %lf ", kf_output.x_.ba(0), kf_output.x_.ba(1), kf_output.x_.ba(2)); // Bias_a + fprintf(fp, "%lf %lf %lf ", kf_output.x_.gravity(0), kf_output.x_.gravity(1), kf_output.x_.gravity(2)); // Bias_a + } + fprintf(fp, "\r\n"); + fflush(fp); +} + +void pointBodyLidarToIMU(PointType const * const pi, PointType * const po) +{ + V3D p_body_lidar(pi->x, pi->y, pi->z); + V3D p_body_imu; + if (extrinsic_est_en) + { + if (!use_imu_as_input) + { + p_body_imu = kf_output.x_.offset_R_L_I * p_body_lidar + kf_output.x_.offset_T_L_I; + } + else + { + p_body_imu = kf_input.x_.offset_R_L_I * p_body_lidar + kf_input.x_.offset_T_L_I; + } + } + else + { + p_body_imu = Lidar_R_wrt_IMU * p_body_lidar + Lidar_T_wrt_IMU; + } + po->x = p_body_imu(0); + po->y = p_body_imu(1); + po->z = p_body_imu(2); + po->intensity = pi->intensity; +} + +int points_cache_size = 0; + +void points_cache_collect() // seems for debug +{ + PointVector points_history; + ikdtree.acquire_removed_points(points_history); + points_cache_size = points_history.size(); +} + +BoxPointType LocalMap_Points; +bool Localmap_Initialized = false; +void lasermap_fov_segment() +{ + cub_needrm.shrink_to_fit(); + + V3D pos_LiD; + if (use_imu_as_input) + { + pos_LiD = kf_input.x_.pos + kf_input.x_.rot * Lidar_T_wrt_IMU; + } + else + { + pos_LiD = kf_output.x_.pos + kf_output.x_.rot * Lidar_T_wrt_IMU; + } + if (!Localmap_Initialized){ + for (int i = 0; i < 3; i++){ + LocalMap_Points.vertex_min[i] = pos_LiD(i) - cube_len / 2.0; + LocalMap_Points.vertex_max[i] = pos_LiD(i) + cube_len / 2.0; + } + Localmap_Initialized = true; + return; + } + float dist_to_map_edge[3][2]; + bool need_move = false; + for (int i = 0; i < 3; i++){ + dist_to_map_edge[i][0] = fabs(pos_LiD(i) - LocalMap_Points.vertex_min[i]); + dist_to_map_edge[i][1] = fabs(pos_LiD(i) - LocalMap_Points.vertex_max[i]); + if (dist_to_map_edge[i][0] <= MOV_THRESHOLD * DET_RANGE || dist_to_map_edge[i][1] <= MOV_THRESHOLD * DET_RANGE) need_move = true; + } + if (!need_move) return; + BoxPointType New_LocalMap_Points, tmp_boxpoints; + New_LocalMap_Points = LocalMap_Points; + float mov_dist = max((cube_len - 2.0 * MOV_THRESHOLD * DET_RANGE) * 0.5 * 0.9, double(DET_RANGE * (MOV_THRESHOLD -1))); + for (int i = 0; i < 3; i++){ + tmp_boxpoints = LocalMap_Points; + if (dist_to_map_edge[i][0] <= MOV_THRESHOLD * DET_RANGE){ + New_LocalMap_Points.vertex_max[i] -= mov_dist; + New_LocalMap_Points.vertex_min[i] -= mov_dist; + tmp_boxpoints.vertex_min[i] = LocalMap_Points.vertex_max[i] - mov_dist; + cub_needrm.emplace_back(tmp_boxpoints); + } else if (dist_to_map_edge[i][1] <= MOV_THRESHOLD * DET_RANGE){ + New_LocalMap_Points.vertex_max[i] += mov_dist; + New_LocalMap_Points.vertex_min[i] += mov_dist; + tmp_boxpoints.vertex_max[i] = LocalMap_Points.vertex_min[i] + mov_dist; + cub_needrm.emplace_back(tmp_boxpoints); + } + } + LocalMap_Points = New_LocalMap_Points; + + points_cache_collect(); + if(cub_needrm.size() > 0) int kdtree_delete_counter = ikdtree.Delete_Point_Boxes(cub_needrm); +} + +void standard_pcl_cbk(const sensor_msgs::PointCloud2::ConstPtr &msg) +{ + mtx_buffer.lock(); + scan_count ++; + double preprocess_start_time = omp_get_wtime(); + if (msg->header.stamp.toSec() < last_timestamp_lidar) + { + ROS_ERROR("lidar loop back, clear buffer"); + // lidar_buffer.shrink_to_fit(); + + mtx_buffer.unlock(); + sig_buffer.notify_all(); + return; + } + + last_timestamp_lidar = msg->header.stamp.toSec(); + + PointCloudXYZI::Ptr ptr(new PointCloudXYZI()); + PointCloudXYZI::Ptr ptr_div(new PointCloudXYZI()); + double time_div = msg->header.stamp.toSec(); + p_pre->process(msg, ptr); + if (cut_frame) + { + sort(ptr->points.begin(), ptr->points.end(), time_list); + + for (int i = 0; i < ptr->size(); i++) + { + ptr_div->push_back(ptr->points[i]); + // cout << "check time:" << ptr->points[i].curvature << endl; + if (ptr->points[i].curvature / double(1000) + msg->header.stamp.toSec() - time_div > cut_frame_time_interval) + { + if(ptr_div->size() < 1) continue; + PointCloudXYZI::Ptr ptr_div_i(new PointCloudXYZI()); + *ptr_div_i = *ptr_div; + lidar_buffer.push_back(ptr_div_i); + time_buffer.push_back(time_div); + time_div += ptr->points[i].curvature / double(1000); + ptr_div->clear(); + } + } + if (!ptr_div->empty()) + { + lidar_buffer.push_back(ptr_div); + // ptr_div->clear(); + time_buffer.push_back(time_div); + } + } + else if (con_frame) + { + if (frame_ct == 0) + { + time_con = last_timestamp_lidar; //msg->header.stamp.toSec(); + } + if (frame_ct < con_frame_num) + { + for (int i = 0; i < ptr->size(); i++) + { + ptr->points[i].curvature += (last_timestamp_lidar - time_con) * 1000; + ptr_con->push_back(ptr->points[i]); + } + frame_ct ++; + } + else + { + PointCloudXYZI::Ptr ptr_con_i(new PointCloudXYZI()); + *ptr_con_i = *ptr_con; + lidar_buffer.push_back(ptr_con_i); + double time_con_i = time_con; + time_buffer.push_back(time_con_i); + ptr_con->clear(); + frame_ct = 0; + } + } + else + { + lidar_buffer.emplace_back(ptr); + time_buffer.emplace_back(msg->header.stamp.toSec()); + } + s_plot11[scan_count] = omp_get_wtime() - preprocess_start_time; + mtx_buffer.unlock(); + sig_buffer.notify_all(); +} + +void livox_pcl_cbk(const livox_ros_driver2::CustomMsg::ConstPtr &msg) +{ + mtx_buffer.lock(); + double preprocess_start_time = omp_get_wtime(); + scan_count ++; + if (msg->header.stamp.toSec() < last_timestamp_lidar) + { + ROS_ERROR("lidar loop back, clear buffer"); + + mtx_buffer.unlock(); + sig_buffer.notify_all(); + return; + } + + last_timestamp_lidar = msg->header.stamp.toSec(); + + PointCloudXYZI::Ptr ptr(new PointCloudXYZI()); + PointCloudXYZI::Ptr ptr_div(new PointCloudXYZI()); + p_pre->process(msg, ptr); + double time_div = msg->header.stamp.toSec(); + if (cut_frame) + { + sort(ptr->points.begin(), ptr->points.end(), time_list); + + for (int i = 0; i < ptr->size(); i++) + { + ptr_div->push_back(ptr->points[i]); + if (ptr->points[i].curvature / double(1000) + msg->header.stamp.toSec() - time_div > cut_frame_time_interval) + { + if(ptr_div->size() < 1) continue; + PointCloudXYZI::Ptr ptr_div_i(new PointCloudXYZI()); + // cout << "ptr div num:" << ptr_div->size() << endl; + *ptr_div_i = *ptr_div; + // cout << "ptr div i num:" << ptr_div_i->size() << endl; + lidar_buffer.push_back(ptr_div_i); + time_buffer.push_back(time_div); + time_div += ptr->points[i].curvature / double(1000); + ptr_div->clear(); + } + } + if (!ptr_div->empty()) + { + lidar_buffer.push_back(ptr_div); + // ptr_div->clear(); + time_buffer.push_back(time_div); + } + } + else if (con_frame) + { + if (frame_ct == 0) + { + time_con = last_timestamp_lidar; //msg->header.stamp.toSec(); + } + if (frame_ct < con_frame_num) + { + for (int i = 0; i < ptr->size(); i++) + { + ptr->points[i].curvature += (last_timestamp_lidar - time_con) * 1000; + ptr_con->push_back(ptr->points[i]); + } + frame_ct ++; + } + else + { + PointCloudXYZI::Ptr ptr_con_i(new PointCloudXYZI()); + *ptr_con_i = *ptr_con; + double time_con_i = time_con; + lidar_buffer.push_back(ptr_con_i); + time_buffer.push_back(time_con_i); + ptr_con->clear(); + frame_ct = 0; + } + } + else + { + lidar_buffer.emplace_back(ptr); + time_buffer.emplace_back(msg->header.stamp.toSec()); + } + s_plot11[scan_count] = omp_get_wtime() - preprocess_start_time; + mtx_buffer.unlock(); + sig_buffer.notify_all(); +} + +void imu_cbk(const sensor_msgs::Imu::ConstPtr &msg_in) +{ + publish_count ++; + sensor_msgs::Imu::Ptr msg(new sensor_msgs::Imu(*msg_in)); + + msg->header.stamp = ros::Time().fromSec(msg_in->header.stamp.toSec() - time_lag_imu_to_lidar); + double timestamp = msg->header.stamp.toSec(); + + mtx_buffer.lock(); + + if (timestamp < last_timestamp_imu) + { + ROS_ERROR("imu loop back, clear deque"); + // imu_deque.shrink_to_fit(); + mtx_buffer.unlock(); + sig_buffer.notify_all(); + return; + } + + imu_deque.emplace_back(msg); + last_timestamp_imu = timestamp; + mtx_buffer.unlock(); + sig_buffer.notify_all(); +} + +bool sync_packages(MeasureGroup &meas) +{ + if (!imu_en) + { + if (!lidar_buffer.empty()) + { + meas.lidar = lidar_buffer.front(); + meas.lidar_beg_time = time_buffer.front(); + time_buffer.pop_front(); + lidar_buffer.pop_front(); + if(meas.lidar->points.size() < 1) + { + cout << "lose lidar" << std::endl; + return false; + } + double end_time = meas.lidar->points.back().curvature; + for (auto pt: meas.lidar->points) + { + if (pt.curvature > end_time) + { + end_time = pt.curvature; + } + } + lidar_end_time = meas.lidar_beg_time + end_time / double(1000); + meas.lidar_last_time = lidar_end_time; + return true; + } + return false; + } + + if (lidar_buffer.empty() || imu_deque.empty()) + { + return false; + } + + /*** push a lidar scan ***/ + if(!lidar_pushed) + { + meas.lidar = lidar_buffer.front(); + if(meas.lidar->points.size() < 1) + { + cout << "lose lidar" << endl; + lidar_buffer.pop_front(); + time_buffer.pop_front(); + return false; + } + meas.lidar_beg_time = time_buffer.front(); + double end_time = meas.lidar->points.back().curvature; + for (auto pt: meas.lidar->points) + { + if (pt.curvature > end_time) + { + end_time = pt.curvature; + } + } + lidar_end_time = meas.lidar_beg_time + end_time / double(1000); + + meas.lidar_last_time = lidar_end_time; + lidar_pushed = true; + } + + if (last_timestamp_imu < lidar_end_time) + { + return false; + } + /*** push imu data, and pop from imu buffer ***/ + if (p_imu->imu_need_init_) + { + double imu_time = imu_deque.front()->header.stamp.toSec(); + meas.imu.shrink_to_fit(); + while ((!imu_deque.empty()) && (imu_time < lidar_end_time)) + { + imu_time = imu_deque.front()->header.stamp.toSec(); + if(imu_time > lidar_end_time) break; + meas.imu.emplace_back(imu_deque.front()); + imu_last = imu_next; + imu_last_ptr = imu_deque.front(); + imu_next = *(imu_deque.front()); + imu_deque.pop_front(); + } + } + else if(!init_map) + { + double imu_time = imu_deque.front()->header.stamp.toSec(); + meas.imu.shrink_to_fit(); + meas.imu.emplace_back(imu_last_ptr); + + while ((!imu_deque.empty()) && (imu_time < lidar_end_time)) + { + imu_time = imu_deque.front()->header.stamp.toSec(); + if(imu_time > lidar_end_time) break; + meas.imu.emplace_back(imu_deque.front()); + imu_last = imu_next; + imu_last_ptr = imu_deque.front(); + imu_next = *(imu_deque.front()); + imu_deque.pop_front(); + } + } + + lidar_buffer.pop_front(); + time_buffer.pop_front(); + lidar_pushed = false; + return true; +} + +int process_increments = 0; +void map_incremental() +{ + PointVector PointToAdd; + PointVector PointNoNeedDownsample; + PointToAdd.reserve(feats_down_size); + PointNoNeedDownsample.reserve(feats_down_size); + + for(int i = 0; i < feats_down_size; i++) + { + if (!Nearest_Points[i].empty()) + { + const PointVector &points_near = Nearest_Points[i]; + bool need_add = true; + PointType downsample_result, mid_point; + mid_point.x = floor(feats_down_world->points[i].x/filter_size_map_min)*filter_size_map_min + 0.5 * filter_size_map_min; + mid_point.y = floor(feats_down_world->points[i].y/filter_size_map_min)*filter_size_map_min + 0.5 * filter_size_map_min; + mid_point.z = floor(feats_down_world->points[i].z/filter_size_map_min)*filter_size_map_min + 0.5 * filter_size_map_min; + /* If the nearest points is definitely outside the downsample box */ + if (fabs(points_near[0].x - mid_point.x) > 0.866 * filter_size_map_min || fabs(points_near[0].y - mid_point.y) > 0.866 * filter_size_map_min || fabs(points_near[0].z - mid_point.z) > 0.866 * filter_size_map_min){ + PointNoNeedDownsample.emplace_back(feats_down_world->points[i]); + continue; + } + /* Check if there is a point already in the downsample box */ + float dist = calc_dist(feats_down_world->points[i],mid_point); + for (int readd_i = 0; readd_i < points_near.size(); readd_i ++) + { + /* Those points which are outside the downsample box should not be considered. */ + if (fabs(points_near[readd_i].x - mid_point.x) < 0.5 * filter_size_map_min && fabs(points_near[readd_i].y - mid_point.y) < 0.5 * filter_size_map_min && fabs(points_near[readd_i].z - mid_point.z) < 0.5 * filter_size_map_min) { + need_add = false; + break; + } + } + if (need_add) PointToAdd.emplace_back(feats_down_world->points[i]); + } + else + { + // PointToAdd.emplace_back(feats_down_world->points[i]); + PointNoNeedDownsample.emplace_back(feats_down_world->points[i]); + } + } + int add_point_size = ikdtree.Add_Points(PointToAdd, true); + ikdtree.Add_Points(PointNoNeedDownsample, false); +} + +void publish_init_kdtree(const ros::Publisher & pubLaserCloudFullRes) +{ + int size_init_ikdtree = ikdtree.size(); + PointCloudXYZI::Ptr laserCloudInit(new PointCloudXYZI(size_init_ikdtree, 1)); + + sensor_msgs::PointCloud2 laserCloudmsg; + PointVector ().swap(ikdtree.PCL_Storage); + ikdtree.flatten(ikdtree.Root_Node, ikdtree.PCL_Storage, NOT_RECORD); + + laserCloudInit->points = ikdtree.PCL_Storage; + pcl::toROSMsg(*laserCloudInit, laserCloudmsg); + + laserCloudmsg.header.stamp = ros::Time().fromSec(lidar_end_time); + laserCloudmsg.header.frame_id = "camera_init"; + pubLaserCloudFullRes.publish(laserCloudmsg); + +} + +PointCloudXYZI::Ptr pcl_wait_pub(new PointCloudXYZI(500000, 1)); +PointCloudXYZI::Ptr pcl_wait_save(new PointCloudXYZI()); +void publish_frame_world(const ros::Publisher & pubLaserCloudFullRes) +{ + if (scan_pub_en) + { + PointCloudXYZI::Ptr laserCloudFullRes(feats_down_body); + int size = laserCloudFullRes->points.size(); + + PointCloudXYZI::Ptr laserCloudWorld(new PointCloudXYZI(size, 1)); + + for (int i = 0; i < size; i++) + { + // if (i % 3 == 0) + // { + laserCloudWorld->points[i].x = feats_down_world->points[i].x; + laserCloudWorld->points[i].y = feats_down_world->points[i].y; + laserCloudWorld->points[i].z = feats_down_world->points[i].z; + laserCloudWorld->points[i].intensity = feats_down_world->points[i].intensity; // feats_down_world->points[i].y; // + // } + } + sensor_msgs::PointCloud2 laserCloudmsg; + pcl::toROSMsg(*laserCloudWorld, laserCloudmsg); + + laserCloudmsg.header.stamp = ros::Time().fromSec(lidar_end_time); + laserCloudmsg.header.frame_id = "camera_init"; + pubLaserCloudFullRes.publish(laserCloudmsg); + publish_count -= PUBFRAME_PERIOD; + } + + /**************** save map ****************/ + /* 1. make sure you have enough memories + /* 2. noted that pcd save will influence the real-time performences **/ + if (pcd_save_en) + { + int size = feats_down_world->points.size(); + PointCloudXYZI::Ptr laserCloudWorld(new PointCloudXYZI(size, 1)); + + for (int i = 0; i < size; i++) + { + laserCloudWorld->points[i].x = feats_down_world->points[i].x; + laserCloudWorld->points[i].y = feats_down_world->points[i].y; + laserCloudWorld->points[i].z = feats_down_world->points[i].z; + laserCloudWorld->points[i].intensity = feats_down_world->points[i].intensity; + } + + *pcl_wait_save += *laserCloudWorld; + + static int scan_wait_num = 0; + scan_wait_num ++; + if (pcl_wait_save->size() > 0 && pcd_save_interval > 0 && scan_wait_num >= pcd_save_interval) + { + pcd_index ++; + string all_points_dir(string(string(ROOT_DIR) + "PCD/scans_") + to_string(pcd_index) + string(".pcd")); + pcl::PCDWriter pcd_writer; + cout << "current scan saved to /PCD/" << all_points_dir << endl; + pcd_writer.writeBinary(all_points_dir, *pcl_wait_save); + pcl_wait_save->clear(); + scan_wait_num = 0; + } + } +} + +void publish_frame_body(const ros::Publisher & pubLaserCloudFull_body) +{ + int size = feats_undistort->points.size(); + PointCloudXYZI::Ptr laserCloudIMUBody(new PointCloudXYZI(size, 1)); + + for (int i = 0; i < size; i++) + { + pointBodyLidarToIMU(&feats_undistort->points[i], \ + &laserCloudIMUBody->points[i]); + } + + sensor_msgs::PointCloud2 laserCloudmsg; + pcl::toROSMsg(*laserCloudIMUBody, laserCloudmsg); + laserCloudmsg.header.stamp = ros::Time().fromSec(lidar_end_time); + laserCloudmsg.header.frame_id = "body"; + pubLaserCloudFull_body.publish(laserCloudmsg); + publish_count -= PUBFRAME_PERIOD; +} + +template +void set_posestamp(T & out) +{ + if (!use_imu_as_input) + { + out.position.x = kf_output.x_.pos(0); + out.position.y = kf_output.x_.pos(1); + out.position.z = kf_output.x_.pos(2); + Eigen::Quaterniond q(kf_output.x_.rot); + out.orientation.x = q.coeffs()[0]; + out.orientation.y = q.coeffs()[1]; + out.orientation.z = q.coeffs()[2]; + out.orientation.w = q.coeffs()[3]; + } + else + { + out.position.x = kf_input.x_.pos(0); + out.position.y = kf_input.x_.pos(1); + out.position.z = kf_input.x_.pos(2); + Eigen::Quaterniond q(kf_input.x_.rot); + out.orientation.x = q.coeffs()[0]; + out.orientation.y = q.coeffs()[1]; + out.orientation.z = q.coeffs()[2]; + out.orientation.w = q.coeffs()[3]; + } +} + +void publish_odometry(const ros::Publisher & pubOdomAftMapped) +{ + odomAftMapped.header.frame_id = "camera_init"; + odomAftMapped.child_frame_id = "aft_mapped"; + if (publish_odometry_without_downsample) + { + odomAftMapped.header.stamp = ros::Time().fromSec(time_current); + } + else + { + odomAftMapped.header.stamp = ros::Time().fromSec(lidar_end_time); + } + set_posestamp(odomAftMapped.pose.pose); + + pubOdomAftMapped.publish(odomAftMapped); + + static tf::TransformBroadcaster br; + tf::Transform transform; + tf::Quaternion q; + transform.setOrigin(tf::Vector3(odomAftMapped.pose.pose.position.x, \ + odomAftMapped.pose.pose.position.y, \ + odomAftMapped.pose.pose.position.z)); + q.setW(odomAftMapped.pose.pose.orientation.w); + q.setX(odomAftMapped.pose.pose.orientation.x); + q.setY(odomAftMapped.pose.pose.orientation.y); + q.setZ(odomAftMapped.pose.pose.orientation.z); + transform.setRotation( q ); + br.sendTransform( tf::StampedTransform( transform, odomAftMapped.header.stamp, "camera_init", "aft_mapped" ) ); +} + +void publish_path(const ros::Publisher pubPath) +{ + set_posestamp(msg_body_pose.pose); + // msg_body_pose.header.stamp = ros::Time::now(); + msg_body_pose.header.stamp = ros::Time().fromSec(lidar_end_time); + msg_body_pose.header.frame_id = "camera_init"; + static int jjj = 0; + jjj++; + // if (jjj % 2 == 0) // if path is too large, the rvis will crash + { + path.poses.emplace_back(msg_body_pose); + pubPath.publish(path); + } +} + +int main(int argc, char** argv) +{ + ros::init(argc, argv, "laserMapping"); + ros::NodeHandle nh("~"); + readParameters(nh); + cout<<"lidar_type: "< 179.9 ? 179.9 : (fov_deg + 10.0); + double HALF_FOV_COS = cos((FOV_DEG) * 0.5 * PI_M / 180.0); + + memset(point_selected_surf, true, sizeof(point_selected_surf)); + downSizeFilterSurf.setLeafSize(filter_size_surf_min, filter_size_surf_min, filter_size_surf_min); + downSizeFilterMap.setLeafSize(filter_size_map_min, filter_size_map_min, filter_size_map_min); + Lidar_T_wrt_IMU<lidar_type = p_pre->lidar_type = lidar_type; + p_imu->imu_en = imu_en; + + kf_input.init_dyn_share_modified(get_f_input, df_dx_input, h_model_input); + kf_output.init_dyn_share_modified_2h(get_f_output, df_dx_output, h_model_output, h_model_IMU_output); + Eigen::Matrix P_init = MD(24,24)::Identity() * 0.01; + P_init.block<3, 3>(21, 21) = MD(3,3)::Identity() * 0.0001; + P_init.block<6, 6>(15, 15) = MD(6,6)::Identity() * 0.001; + P_init.block<6, 6>(6, 6) = MD(6,6)::Identity() * 0.0001; + kf_input.change_P(P_init); + Eigen::Matrix P_init_output = MD(30,30)::Identity() * 0.01; + P_init_output.block<3, 3>(21, 21) = MD(3,3)::Identity() * 0.0001; + P_init_output.block<6, 6>(6, 6) = MD(6,6)::Identity() * 0.0001; + P_init_output.block<6, 6>(24, 24) = MD(6,6)::Identity() * 0.001; + kf_input.change_P(P_init); + kf_output.change_P(P_init_output); + Eigen::Matrix Q_input = process_noise_cov_input(); + Eigen::Matrix Q_output = process_noise_cov_output(); + /*** debug record ***/ + FILE *fp; + string pos_log_dir = root_dir + "/Log/pos_log.txt"; + fp = fopen(pos_log_dir.c_str(),"w"); + + ofstream fout_out, fout_imu_pbp; + fout_out.open(DEBUG_FILE_DIR("mat_out.txt"),ios::out); + fout_imu_pbp.open(DEBUG_FILE_DIR("imu_pbp.txt"),ios::out); + if (fout_out && fout_imu_pbp) + cout << "~~~~"<lidar_type == AVIA ? \ + nh.subscribe(lid_topic, 200000, livox_pcl_cbk) : \ + nh.subscribe(lid_topic, 200000, standard_pcl_cbk); + ros::Subscriber sub_imu = nh.subscribe(imu_topic, 200000, imu_cbk); + ros::Publisher pubLaserCloudFullRes = nh.advertise + ("/cloud_registered", 100000); + ros::Publisher pubLaserCloudFullRes_body = nh.advertise + ("/cloud_registered_body", 100000); + ros::Publisher pubLaserCloudEffect = nh.advertise + ("/cloud_effected", 100000); + ros::Publisher pubLaserCloudMap = nh.advertise + ("/Laser_map", 100000); + ros::Publisher pubOdomAftMapped = nh.advertise + ("/aft_mapped_to_init", 100000); + ros::Publisher pubPath = nh.advertise + ("/path", 100000); + ros::Publisher plane_pub = nh.advertise + ("/planner_normal", 1000); +//------------------------------------------------------------------------------------------------------ + signal(SIGINT, SigHandle); + ros::Rate rate(5000); + bool status = ros::ok(); + while (status) + { + if (flg_exit) break; + ros::spinOnce(); + if(sync_packages(Measures)) + { + if (flg_first_scan) + { + first_lidar_time = Measures.lidar_beg_time; + flg_first_scan = false; + cout << "first lidar time" << first_lidar_time << endl; + } + + if (flg_reset) + { + ROS_WARN("reset when rosbag play back"); + p_imu->Reset(); + flg_reset = false; + continue; + } + double t0,t1,t2,t3,t4,t5,match_start, solve_start; + match_time = 0; + solve_time = 0; + propag_time = 0; + update_time = 0; + t0 = omp_get_wtime(); + + p_imu->Process(Measures, feats_undistort); + + // if (feats_undistort->empty() || feats_undistort == NULL) + if (p_imu->imu_need_init_) + { + continue; + } + if(imu_en) + { + if (!p_imu->gravity_align_) + { + while (Measures.lidar_beg_time > imu_next.header.stamp.toSec()) + { + imu_last = imu_next; + imu_next = *(imu_deque.front()); + imu_deque.pop_front(); + // imu_deque.pop(); + } + if (non_station_start) + { + state_in.gravity << VEC_FROM_ARRAY(gravity_init); + state_out.gravity << VEC_FROM_ARRAY(gravity_init); + state_out.acc << VEC_FROM_ARRAY(gravity_init); + state_out.acc *= -1; + } + else + { + state_in.gravity = -1 * p_imu->mean_acc * G_m_s2 / acc_norm; + state_out.gravity = -1 * p_imu->mean_acc * G_m_s2 / acc_norm; + state_out.acc = p_imu->mean_acc * G_m_s2 / acc_norm; + } + if (gravity_align) + { + Eigen::Matrix3d rot_init; + p_imu->gravity_ << VEC_FROM_ARRAY(gravity); + p_imu->Set_init(state_in.gravity, rot_init); + state_in.gravity = p_imu->gravity_; + state_out.gravity = p_imu->gravity_; + state_in.rot = rot_init; + state_out.rot = rot_init; + // state_in.rot.normalize(); + // state_out.rot.normalize(); + state_out.acc = -rot_init.transpose() * state_out.gravity; + } + kf_input.change_x(state_in); + kf_output.change_x(state_out); + p_imu->gravity_align_ = true; + } + } + else + { + if (!p_imu->gravity_align_) + { + state_in.gravity << VEC_FROM_ARRAY(gravity_init); + if (gravity_align) + { + Eigen::Matrix3d rot_init; + p_imu->gravity_ << VEC_FROM_ARRAY(gravity); + p_imu->Set_init(state_in.gravity, rot_init); + state_out.gravity = p_imu->gravity_; + state_out.rot = rot_init; + // state_in.rot.normalize(); + // state_out.rot.normalize(); + state_out.acc = -rot_init.transpose() * state_out.gravity; + } + else + { + state_out.gravity << VEC_FROM_ARRAY(gravity_init); + state_out.acc << VEC_FROM_ARRAY(gravity_init); + state_out.acc *= -1; + } + // kf_input.change_x(state_in); + kf_output.change_x(state_out); + p_imu->gravity_align_ = true; + } + } + /*** Segment the map in lidar FOV ***/ + lasermap_fov_segment(); + /*** downsample the feature points in a scan ***/ + t1 = omp_get_wtime(); + if(space_down_sample) + { + downSizeFilterSurf.setInputCloud(feats_undistort); + downSizeFilterSurf.filter(*feats_down_body); + sort(feats_down_body->points.begin(), feats_down_body->points.end(), time_list); + } + else + { + feats_down_body = Measures.lidar; + sort(feats_down_body->points.begin(), feats_down_body->points.end(), time_list); + } + time_seq = time_compressing(feats_down_body); + feats_down_size = feats_down_body->points.size(); + + /*** initialize the map kdtree ***/ + if(!init_map) + { + if(ikdtree.Root_Node == nullptr) // + // if(feats_down_size > 5) + { + ikdtree.set_downsample_param(filter_size_map_min); + } + + feats_down_world->resize(feats_down_size); + for(int i = 0; i < feats_down_size; i++) + { + pointBodyToWorld(&(feats_down_body->points[i]), &(feats_down_world->points[i])); + } + for (size_t i = 0; i < feats_down_world->size(); i++) { + init_feats_world->points.emplace_back(feats_down_world->points[i]);} + if(init_feats_world->size() < init_map_size) continue; + ikdtree.Build(init_feats_world->points); + init_map = true; + publish_init_kdtree(pubLaserCloudMap); //(pubLaserCloudFullRes); + continue; + } + /*** ICP and Kalman filter update ***/ + normvec->resize(feats_down_size); + feats_down_world->resize(feats_down_size); + + Nearest_Points.resize(feats_down_size); + + t2 = omp_get_wtime(); + + /*** iterated state estimation ***/ + crossmat_list.reserve(feats_down_size); + pbody_list.reserve(feats_down_size); + // pbody_ext_list.reserve(feats_down_size); + + for (size_t i = 0; i < feats_down_body->size(); i++) + { + V3D point_this(feats_down_body->points[i].x, + feats_down_body->points[i].y, + feats_down_body->points[i].z); + pbody_list[i]=point_this; + if (extrinsic_est_en) + { + if (!use_imu_as_input) + { + point_this = kf_output.x_.offset_R_L_I * point_this + kf_output.x_.offset_T_L_I; + } + else + { + point_this = kf_input.x_.offset_R_L_I * point_this + kf_input.x_.offset_T_L_I; + } + } + else + { + point_this = Lidar_R_wrt_IMU * point_this + Lidar_T_wrt_IMU; + } + M3D point_crossmat; + point_crossmat << SKEW_SYM_MATRX(point_this); + crossmat_list[i]=point_crossmat; + } + + if (!use_imu_as_input) + { + // bool imu_upda_cov = false; + effct_feat_num = 0; + /**** point by point update ****/ + + double pcl_beg_time = Measures.lidar_beg_time; + idx = -1; + for (k = 0; k < time_seq.size(); k++) + { + PointType &point_body = feats_down_body->points[idx+time_seq[k]]; + + time_current = point_body.curvature / 1000.0 + pcl_beg_time; + + if (is_first_frame) + { + if(imu_en) + { + while (time_current > imu_next.header.stamp.toSec()) + { + imu_last = imu_next; + imu_next = *(imu_deque.front()); + imu_deque.pop_front(); + // imu_deque.pop(); + } + + angvel_avr< imu_next.header.stamp.toSec(); + while (imu_comes) + { + // imu_upda_cov = true; + angvel_avr< imu_next.header.stamp.toSec(); + // if (!imu_comes) + { + double dt_cov = imu_last.header.stamp.toSec() - time_update_last; + + if (dt_cov > 0.0) + { + time_update_last = imu_last.header.stamp.toSec(); + double propag_imu_start = omp_get_wtime(); + + kf_output.predict(dt_cov, Q_output, input_in, false, true); + + propag_time += omp_get_wtime() - propag_imu_start; + double solve_imu_start = omp_get_wtime(); + kf_output.update_iterated_dyn_share_IMU(); + solve_time += omp_get_wtime() - solve_imu_start; + } + } + } + } + + double dt = time_current - time_predict_last_const; + double propag_state_start = omp_get_wtime(); + if(!prop_at_freq_of_imu) + { + double dt_cov = time_current - time_update_last; + if (dt_cov > 0.0) + { + kf_output.predict(dt_cov, Q_output, input_in, false, true); + time_update_last = time_current; + } + } + kf_output.predict(dt, Q_output, input_in, true, false); + propag_time += omp_get_wtime() - propag_state_start; + time_predict_last_const = time_current; + // if(k == 0) + // { + // fout_imu_pbp << Measures.lidar_last_time - first_lidar_time << " " << imu_last.angular_velocity.x << " " << imu_last.angular_velocity.y << " " << imu_last.angular_velocity.z \ + // << " " << imu_last.linear_acceleration.x << " " << imu_last.linear_acceleration.y << " " << imu_last.linear_acceleration.z << endl; + // } + + double t_update_start = omp_get_wtime(); + + if (feats_down_size < 1) + { + ROS_WARN("No point, skip this scan!\n"); + idx += time_seq[k]; + continue; + } + if (!kf_output.update_iterated_dyn_share_modified()) + { + idx = idx+time_seq[k]; + continue; + } + + // if(prop_at_freq_of_imu) + // { + // double dt_cov = time_current - time_update_last; + // if ((dt_cov >= imu_time_inte)) // (point_cov_not_prop && imu_prop_cov) + // { + // double propag_cov_start = omp_get_wtime(); + // kf_output.predict(dt_cov, Q_output, input_in, false, true); + // // imu_upda_cov = false; + // time_update_last = time_current; + // propag_time += omp_get_wtime() - propag_cov_start; + // } + // } + + solve_start = omp_get_wtime(); + + if (publish_odometry_without_downsample) + { + /******* Publish odometry *******/ + + publish_odometry(pubOdomAftMapped); + if (runtime_pos_log) + { + state_out = kf_output.x_; + euler_cur = SO3ToEuler(state_out.rot); + fout_out << setw(20) << Measures.lidar_beg_time - first_lidar_time << " " << euler_cur.transpose() << " " << state_out.pos.transpose() << " " << state_out.vel.transpose() \ + <<" "<points.size()<points[idx+j+1]; + PointType &point_world_j = feats_down_world->points[idx+j+1]; + pointBodyToWorld(&point_body_j, &point_world_j); + } + + solve_time += omp_get_wtime() - solve_start; + + update_time += omp_get_wtime() - t_update_start; + idx += time_seq[k]; + // cout << "pbp output effect feat num:" << effct_feat_num << endl; + } + } + else + { + bool imu_prop_cov = false; + effct_feat_num = 0; + + double pcl_beg_time = Measures.lidar_beg_time; + idx = -1; + for (k = 0; k < time_seq.size(); k++) + { + PointType &point_body = feats_down_body->points[idx+time_seq[k]]; + time_current = point_body.curvature / 1000.0 + pcl_beg_time; + if (is_first_frame) + { + while (time_current > imu_next.header.stamp.toSec()) + { + imu_last = imu_next; + imu_next = *(imu_deque.front()); + imu_deque.pop_front(); + // imu_deque.pop(); + } + imu_prop_cov = true; + // imu_upda_cov = true; + + is_first_frame = false; + t_last = time_current; + time_update_last = time_current; + // if(prop_at_freq_of_imu) + { + input_in.gyro< imu_next.header.stamp.toSec()) // && !imu_deque.empty()) + { + imu_last = imu_next; + imu_next = *(imu_deque.front()); + imu_deque.pop_front(); + input_in.gyro< 0.0) + { + kf_input.predict(dt_cov, Q_input, input_in, false, true); + time_update_last = imu_last.header.stamp.toSec(); //time_current; + } + kf_input.predict(dt, Q_input, input_in, true, false); + t_last = imu_last.header.stamp.toSec(); + imu_prop_cov = true; + // imu_upda_cov = true; + } + + double dt = time_current - t_last; + t_last = time_current; + double propag_start = omp_get_wtime(); + + if(!prop_at_freq_of_imu) + { + double dt_cov = time_current - time_update_last; + if (dt_cov > 0.0) + { + kf_input.predict(dt_cov, Q_input, input_in, false, true); + time_update_last = time_current; + } + } + kf_input.predict(dt, Q_input, input_in, true, false); + + propag_time += omp_get_wtime() - propag_start; + + // if(k == 0) + // { + // fout_imu_pbp << Measures.lidar_last_time - first_lidar_time << " " << imu_last.angular_velocity.x << " " << imu_last.angular_velocity.y << " " << imu_last.angular_velocity.z \ + // << " " << imu_last.linear_acceleration.x << " " << imu_last.linear_acceleration.y << " " << imu_last.linear_acceleration.z << endl; + // } + + double t_update_start = omp_get_wtime(); + + if (feats_down_size < 1) + { + ROS_WARN("No point, skip this scan!\n"); + + idx += time_seq[k]; + continue; + } + if (!kf_input.update_iterated_dyn_share_modified()) + { + idx = idx+time_seq[k]; + continue; + } + + solve_start = omp_get_wtime(); + + // if(prop_at_freq_of_imu) + // { + // double dt_cov = time_current - time_update_last; + // if ((imu_prop_cov && dt_cov > 0.0) || (dt_cov >= imu_time_inte * 1.2)) + // { + // double propag_cov_start = omp_get_wtime(); + // kf_input.predict(dt_cov, Q_input, input_in, false, true); + // propag_time += omp_get_wtime() - propag_cov_start; + // time_update_last = time_current; + // imu_prop_cov = false; + // } + // } + if (publish_odometry_without_downsample) + { + /******* Publish odometry *******/ + + publish_odometry(pubOdomAftMapped); + if (runtime_pos_log) + { + state_in = kf_input.x_; + euler_cur = SO3ToEuler(state_in.rot); + fout_out << setw(20) << Measures.lidar_beg_time - first_lidar_time << " " << euler_cur.transpose() << " " << state_in.pos.transpose() << " " << state_in.vel.transpose() \ + <<" "<points.size()<points[idx+j+1]; + PointType &point_world_j = feats_down_world->points[idx+j+1]; + pointBodyToWorld(&point_body_j, &point_world_j); + } + solve_time += omp_get_wtime() - solve_start; + + update_time += omp_get_wtime() - t_update_start; + idx = idx + time_seq[k]; + } + } + + /******* Publish odometry downsample *******/ + if (!publish_odometry_without_downsample) + { + publish_odometry(pubOdomAftMapped); + } + + /*** add the feature points to map kdtree ***/ + t3 = omp_get_wtime(); + + if(feats_down_size > 4) + { + map_incremental(); + } + + t5 = omp_get_wtime(); + /******* Publish points *******/ + if (path_en) publish_path(pubPath); + if (scan_pub_en || pcd_save_en) publish_frame_world(pubLaserCloudFullRes); + if (scan_pub_en && scan_body_pub_en) publish_frame_body(pubLaserCloudFullRes_body); + + /*** Debug variables Logging ***/ + if (runtime_pos_log) + { + frame_num ++; + aver_time_consu = aver_time_consu * (frame_num - 1) / frame_num + (t5 - t0) / frame_num; + {aver_time_icp = aver_time_icp * (frame_num - 1)/frame_num + update_time/frame_num;} + aver_time_match = aver_time_match * (frame_num - 1)/frame_num + (match_time)/frame_num; + aver_time_solve = aver_time_solve * (frame_num - 1)/frame_num + solve_time/frame_num; + aver_time_propag = aver_time_propag * (frame_num - 1)/frame_num + propag_time / frame_num; + T1[time_log_counter] = Measures.lidar_beg_time; + s_plot[time_log_counter] = t5 - t0; + s_plot2[time_log_counter] = feats_undistort->points.size(); + s_plot3[time_log_counter] = aver_time_consu; + time_log_counter ++; + printf("[ mapping ]: time: IMU + Map + Input Downsample: %0.6f ave match: %0.6f ave solve: %0.6f ave ICP: %0.6f map incre: %0.6f ave total: %0.6f icp: %0.6f propogate: %0.6f \n",t1-t0,aver_time_match,aver_time_solve,t3-t1,t5-t3,aver_time_consu, aver_time_icp, aver_time_propag); + if (!publish_odometry_without_downsample) + { + if (!use_imu_as_input) + { + state_out = kf_output.x_; + euler_cur = SO3ToEuler(state_out.rot); + fout_out << setw(20) << Measures.lidar_beg_time - first_lidar_time << " " << euler_cur.transpose() << " " << state_out.pos.transpose() << " " << state_out.vel.transpose() \ + <<" "<points.size()<points.size()<size() > 0 && pcd_save_en) + { + string file_name = string("scans.pcd"); + string all_points_dir(string(string(ROOT_DIR) + "PCD/") + file_name); + pcl::PCDWriter pcd_writer; + pcd_writer.writeBinary(all_points_dir, *pcl_wait_save); + } + fout_out.close(); + fout_imu_pbp.close(); + + return 0; +} diff --git a/src/Point-LIO/src/parameters.cpp b/src/Point-LIO/src/parameters.cpp new file mode 100755 index 0000000..dd140a3 --- /dev/null +++ b/src/Point-LIO/src/parameters.cpp @@ -0,0 +1,90 @@ +#include "parameters.h" + +bool is_first_frame = true; +double lidar_end_time = 0.0, first_lidar_time = 0.0, time_con = 0.0; +double last_timestamp_lidar = -1.0, last_timestamp_imu = -1.0; +int pcd_index = 0; + +std::string lid_topic, imu_topic; +bool prop_at_freq_of_imu, check_satu, con_frame, cut_frame; +bool use_imu_as_input, space_down_sample, publish_odometry_without_downsample; +int init_map_size, con_frame_num; +double match_s, satu_acc, satu_gyro, cut_frame_time_interval; +float plane_thr; +double filter_size_surf_min, filter_size_map_min, fov_deg; +double cube_len; +float DET_RANGE; +bool imu_en, gravity_align, non_station_start; +double imu_time_inte; +double laser_point_cov, acc_norm; +double vel_cov, acc_cov_input, gyr_cov_input; +double gyr_cov_output, acc_cov_output, b_gyr_cov, b_acc_cov; +double imu_meas_acc_cov, imu_meas_omg_cov; +int lidar_type, pcd_save_interval; +std::vector gravity_init, gravity; +std::vector extrinT; +std::vector extrinR; +bool runtime_pos_log, pcd_save_en, path_en, extrinsic_est_en = true; +bool scan_pub_en, scan_body_pub_en; +shared_ptr p_pre; +double time_lag_imu_to_lidar = 0.0; + +void readParameters(ros::NodeHandle &nh) +{ + p_pre.reset(new Preprocess()); + nh.param("prop_at_freq_of_imu", prop_at_freq_of_imu, 1); + nh.param("use_imu_as_input", use_imu_as_input, 1); + nh.param("check_satu", check_satu, 1); + nh.param("init_map_size", init_map_size, 100); + nh.param("space_down_sample", space_down_sample, 1); + nh.param("mapping/satu_acc",satu_acc,3.0); + nh.param("mapping/satu_gyro",satu_gyro,35.0); + nh.param("mapping/acc_norm",acc_norm,1.0); + nh.param("mapping/plane_thr", plane_thr, 0.05f); + nh.param("point_filter_num", p_pre->point_filter_num, 2); + nh.param("common/lid_topic",lid_topic,"/livox/lidar"); + nh.param("common/imu_topic", imu_topic,"/livox/imu"); + nh.param("common/con_frame",con_frame,false); + nh.param("common/con_frame_num",con_frame_num,1); + nh.param("common/cut_frame",cut_frame,false); + nh.param("common/cut_frame_time_interval",cut_frame_time_interval,0.1); + nh.param("common/time_lag_imu_to_lidar",time_lag_imu_to_lidar,0.0); + nh.param("filter_size_surf",filter_size_surf_min,0.5); + nh.param("filter_size_map",filter_size_map_min,0.5); + nh.param("cube_side_length",cube_len,200); + nh.param("mapping/det_range",DET_RANGE,300.f); + nh.param("mapping/fov_degree",fov_deg,180); + nh.param("mapping/imu_en",imu_en,true); + nh.param("mapping/start_in_aggressive_motion",non_station_start,false); + nh.param("mapping/extrinsic_est_en",extrinsic_est_en,true); + nh.param("mapping/imu_time_inte",imu_time_inte,0.005); + nh.param("mapping/lidar_meas_cov",laser_point_cov,0.1); + nh.param("mapping/acc_cov_input",acc_cov_input,0.1); + nh.param("mapping/vel_cov",vel_cov,20); + nh.param("mapping/gyr_cov_input",gyr_cov_input,0.1); + nh.param("mapping/gyr_cov_output",gyr_cov_output,0.1); + nh.param("mapping/acc_cov_output",acc_cov_output,0.1); + nh.param("mapping/b_gyr_cov",b_gyr_cov,0.0001); + nh.param("mapping/b_acc_cov",b_acc_cov,0.0001); + nh.param("mapping/imu_meas_acc_cov",imu_meas_acc_cov,0.1); + nh.param("mapping/imu_meas_omg_cov",imu_meas_omg_cov,0.1); + nh.param("preprocess/blind", p_pre->blind, 1.0); + nh.param("preprocess/lidar_type", lidar_type, 1); + nh.param("preprocess/scan_line", p_pre->N_SCANS, 16); + nh.param("preprocess/scan_rate", p_pre->SCAN_RATE, 10); + nh.param("preprocess/timestamp_unit", p_pre->time_unit, 1); + nh.param("mapping/match_s", match_s, 81); + nh.param("mapping/gravity_align", gravity_align, true); + nh.param>("mapping/gravity", gravity, std::vector()); + nh.param>("mapping/gravity_init", gravity_init, std::vector()); + nh.param>("mapping/extrinsic_T", extrinT, std::vector()); + nh.param>("mapping/extrinsic_R", extrinR, std::vector()); + nh.param("odometry/publish_odometry_without_downsample", publish_odometry_without_downsample, false); + nh.param("publish/path_en",path_en, true); + nh.param("publish/scan_publish_en",scan_pub_en,1); + nh.param("publish/scan_bodyframe_pub_en",scan_body_pub_en,1); + nh.param("runtime_pos_log_enable", runtime_pos_log, 0); + nh.param("pcd_save/pcd_save_en", pcd_save_en, false); + nh.param("pcd_save/interval", pcd_save_interval, -1); +} + diff --git a/src/Point-LIO/src/parameters.h b/src/Point-LIO/src/parameters.h new file mode 100755 index 0000000..88eee85 --- /dev/null +++ b/src/Point-LIO/src/parameters.h @@ -0,0 +1,40 @@ +// #ifndef PARAM_H +// #define PARAM_H +#pragma once +#include +#include +#include +#include +#include "preprocess.h" + +extern bool is_first_frame; +extern double lidar_end_time, first_lidar_time, time_con; +extern double last_timestamp_lidar, last_timestamp_imu; +extern int pcd_index; + +extern std::string lid_topic, imu_topic; +extern bool prop_at_freq_of_imu, check_satu, con_frame, cut_frame; +extern bool use_imu_as_input, space_down_sample; +extern bool extrinsic_est_en, publish_odometry_without_downsample; +extern int init_map_size, con_frame_num; +extern double match_s, satu_acc, satu_gyro, cut_frame_time_interval; +extern float plane_thr; +extern double filter_size_surf_min, filter_size_map_min, fov_deg; +extern double cube_len; +extern float DET_RANGE; +extern bool imu_en, gravity_align, non_station_start; +extern double imu_time_inte; +extern double laser_point_cov, acc_norm; +extern double acc_cov_input, gyr_cov_input, vel_cov; +extern double gyr_cov_output, acc_cov_output, b_gyr_cov, b_acc_cov; +extern double imu_meas_acc_cov, imu_meas_omg_cov; +extern int lidar_type, pcd_save_interval; +extern std::vector gravity_init, gravity; +extern std::vector extrinT; +extern std::vector extrinR; +extern bool runtime_pos_log, pcd_save_en, path_en; +extern bool scan_pub_en, scan_body_pub_en; +extern shared_ptr p_pre; +extern double time_lag_imu_to_lidar; + +void readParameters(ros::NodeHandle &n); diff --git a/src/Point-LIO/src/preprocess.cpp b/src/Point-LIO/src/preprocess.cpp new file mode 100755 index 0000000..1cc56f6 --- /dev/null +++ b/src/Point-LIO/src/preprocess.cpp @@ -0,0 +1,795 @@ +#include "preprocess.h" + +#define RETURN0 0x00 +#define RETURN0AND1 0x10 + +Preprocess::Preprocess() + :lidar_type(AVIA), blind(0.01), point_filter_num(1) +{ + inf_bound = 10; + N_SCANS = 6; + SCAN_RATE = 10; + group_size = 8; + disA = 0.01; + disA = 0.1; // B? + p2l_ratio = 225; + limit_maxmid =6.25; + limit_midmin =6.25; + limit_maxmin = 3.24; + jump_up_limit = 170.0; + jump_down_limit = 8.0; + cos160 = 160.0; + edgea = 2; + edgeb = 0.1; + smallp_intersect = 172.5; + smallp_ratio = 1.2; + given_offset_time = false; + + jump_up_limit = cos(jump_up_limit/180*M_PI); + jump_down_limit = cos(jump_down_limit/180*M_PI); + cos160 = cos(cos160/180*M_PI); + smallp_intersect = cos(smallp_intersect/180*M_PI); +} + +Preprocess::~Preprocess() {} + +void Preprocess::set(bool feat_en, int lid_type, double bld, int pfilt_num) +{ + lidar_type = lid_type; + blind = bld; + point_filter_num = pfilt_num; +} + +void Preprocess::process(const livox_ros_driver2::CustomMsg::ConstPtr &msg, PointCloudXYZI::Ptr &pcl_out) +{ + avia_handler(msg); + *pcl_out = pl_surf; +} + +void Preprocess::process(const sensor_msgs::PointCloud2::ConstPtr &msg, PointCloudXYZI::Ptr &pcl_out) +{ + switch (time_unit) + { + case SEC: + time_unit_scale = 1.e3f; + break; + case MS: + time_unit_scale = 1.f; + break; + case US: + time_unit_scale = 1.e-3f; + break; + case NS: + time_unit_scale = 1.e-6f; + break; + default: + time_unit_scale = 1.f; + break; + } + + switch (lidar_type) + { + case OUST64: + oust64_handler(msg); + break; + + case VELO16: + velodyne_handler(msg); + break; + + case HESAIxt32: + hesai_handler(msg); + break; + + default: + printf("Error LiDAR Type"); + break; + } + *pcl_out = pl_surf; +} + +void Preprocess::avia_handler(const livox_ros_driver2::CustomMsg::ConstPtr &msg) +{ + pl_surf.clear(); + // pl_corn.clear(); + pl_full.clear(); + double t1 = omp_get_wtime(); + int plsize = msg->point_num; + + // pl_corn.reserve(plsize); + pl_surf.reserve(plsize); + pl_full.resize(plsize); + + uint valid_num = 0; + + for(uint i=1; ipoints[i].line < N_SCANS) && ((msg->points[i].tag & 0x30) == 0x10 || (msg->points[i].tag & 0x30) == 0x00)) + { + valid_num ++; + if (valid_num % point_filter_num == 0) + { + pl_full[i].x = msg->points[i].x; + pl_full[i].y = msg->points[i].y; + pl_full[i].z = msg->points[i].z; + pl_full[i].intensity = msg->points[i].reflectivity; + pl_full[i].curvature = msg->points[i].offset_time / float(1000000); // use curvature as time of each laser points, curvature unit: ms + + if (i==0) pl_full[i].curvature = fabs(pl_full[i].curvature) < 1.0 ? pl_full[i].curvature : 0.0; + else pl_full[i].curvature = fabs(pl_full[i].curvature - pl_full[i-1].curvature) < 1.0 ? pl_full[i].curvature : pl_full[i-1].curvature + 0.004166667f; + + if(((abs(pl_full[i].x - pl_full[i-1].x) > 1e-7) + || (abs(pl_full[i].y - pl_full[i-1].y) > 1e-7) + || (abs(pl_full[i].z - pl_full[i-1].z) > 1e-7)) + && (pl_full[i].x * pl_full[i].x + pl_full[i].y * pl_full[i].y + pl_full[i].z * pl_full[i].z > (blind * blind))) + { + pl_surf.push_back(pl_full[i]); + } + } + } + } + +} + +void Preprocess::oust64_handler(const sensor_msgs::PointCloud2::ConstPtr &msg) +{ + pl_surf.clear(); + // pl_corn.clear(); + // pl_full.clear(); + pcl::PointCloud pl_orig; + pcl::fromROSMsg(*msg, pl_orig); + int plsize = pl_orig.size(); + // pl_corn.reserve(plsize); + pl_surf.reserve(plsize); + + + double time_stamp = msg->header.stamp.toSec(); + // cout << "===================================" << endl; + // printf("Pt size = %d, N_SCANS = %d\r\n", plsize, N_SCANS); + for (int i = 0; i < pl_orig.points.size(); i++) + { + if (i % point_filter_num != 0) continue; + + double range = pl_orig.points[i].x * pl_orig.points[i].x + pl_orig.points[i].y * pl_orig.points[i].y + pl_orig.points[i].z * pl_orig.points[i].z; + + if (range < (blind * blind)) continue; + + Eigen::Vector3d pt_vec; + PointType added_pt; + added_pt.x = pl_orig.points[i].x; + added_pt.y = pl_orig.points[i].y; + added_pt.z = pl_orig.points[i].z; + added_pt.intensity = pl_orig.points[i].intensity; + added_pt.normal_x = 0; + added_pt.normal_y = 0; + added_pt.normal_z = 0; + added_pt.curvature = pl_orig.points[i].t * time_unit_scale; // curvature unit: ms + + pl_surf.points.push_back(added_pt); + } + +} + +void Preprocess::velodyne_handler(const sensor_msgs::PointCloud2::ConstPtr &msg) +{ + pl_surf.clear(); + // pl_corn.clear(); + // pl_full.clear(); + + pcl::PointCloud pl_orig; + pcl::fromROSMsg(*msg, pl_orig); + int plsize = pl_orig.points.size(); + if (plsize == 0) return; + pl_surf.reserve(plsize); + + /*** These variables only works when no point timestamps given ***/ + double omega_l = 0.361 * SCAN_RATE; // scan angular velocity + std::vector is_first(N_SCANS,true); + std::vector yaw_fp(N_SCANS, 0.0); // yaw of first scan point + std::vector yaw_last(N_SCANS, 0.0); // yaw of last scan point + std::vector time_last(N_SCANS, 0.0); // last offset time + /*****************************************************************/ + + if (pl_orig.points[plsize - 1].time > 0) + { + given_offset_time = true; + } + else + { + given_offset_time = false; + double yaw_first = atan2(pl_orig.points[0].y, pl_orig.points[0].x) * 57.29578; + double yaw_end = yaw_first; + int layer_first = pl_orig.points[0].ring; + for (uint i = plsize - 1; i > 0; i--) + { + if (pl_orig.points[i].ring == layer_first) + { + yaw_end = atan2(pl_orig.points[i].y, pl_orig.points[i].x) * 57.29578; + break; + } + } + } + + for (int i = 0; i < plsize; i++) + { + PointType added_pt; + // cout<<"!!!!!!"< (blind * blind)) + { + pl_surf.points.push_back(added_pt); + } + } + } + +} + +void Preprocess::hesai_handler(const sensor_msgs::PointCloud2::ConstPtr &msg) +{ + pl_surf.clear(); + // pl_corn.clear(); + // pl_full.clear(); + + pcl::PointCloud pl_orig; + pcl::fromROSMsg(*msg, pl_orig); + int plsize = pl_orig.points.size(); + if (plsize == 0) return; + pl_surf.reserve(plsize); + + /*** These variables only works when no point timestamps given ***/ + double omega_l = 0.361 * SCAN_RATE; // scan angular velocity + std::vector is_first(N_SCANS,true); + std::vector yaw_fp(N_SCANS, 0.0); // yaw of first scan point + std::vector yaw_last(N_SCANS, 0.0); // yaw of last scan point + std::vector time_last(N_SCANS, 0.0); // last offset time + /*****************************************************************/ + + if (pl_orig.points[plsize - 1].timestamp > 0) + { + given_offset_time = true; + } + else + { + given_offset_time = false; + double yaw_first = atan2(pl_orig.points[0].y, pl_orig.points[0].x) * 57.29578; + double yaw_end = yaw_first; + int layer_first = pl_orig.points[0].ring; + for (uint i = plsize - 1; i > 0; i--) + { + if (pl_orig.points[i].ring == layer_first) + { + yaw_end = atan2(pl_orig.points[i].y, pl_orig.points[i].x) * 57.29578; + break; + } + } + } + + double time_head = pl_orig.points[0].timestamp; + + for (int i = 0; i < plsize; i++) + { + PointType added_pt; + // cout<<"!!!!!!"< (blind * blind)) + { + pl_surf.points.push_back(added_pt); + } + } + } + +} + +void Preprocess::give_feature(pcl::PointCloud &pl, vector &types) +{ + int plsize = pl.size(); + int plsize2; + if(plsize == 0) + { + printf("something wrong\n"); + return; + } + uint head = 0; + + while(types[head].range < blind) + { + head++; + } + + // Surf + plsize2 = (plsize > group_size) ? (plsize - group_size) : 0; + + Eigen::Vector3d curr_direct(Eigen::Vector3d::Zero()); + Eigen::Vector3d last_direct(Eigen::Vector3d::Zero()); + + uint i_nex = 0, i2; + uint last_i = 0; uint last_i_nex = 0; + int last_state = 0; + int plane_type; + + for(uint i=head; i0.5) + if(last_state==1 && last_direct.norm()>0.1) + { + double mod = last_direct.transpose() * curr_direct; + if(mod>-0.707 && mod<0.707) + { + types[i].ftype = Edge_Plane; + } + else + { + types[i].ftype = Real_Plane; + } + } + + i = i_nex - 1; + last_state = 1; + } + else // if(plane_type == 2) + { + i = i_nex; + last_state = 0; + } + + last_i = i2; + last_i_nex = i_nex; + last_direct = curr_direct; + } + + plsize2 = plsize > 3 ? plsize - 3 : 0; + for(uint i=head+3; i=Real_Plane) + { + continue; + } + + if(types[i-1].dista<1e-16 || types[i].dista<1e-16) + { + continue; + } + + Eigen::Vector3d vec_a(pl[i].x, pl[i].y, pl[i].z); + Eigen::Vector3d vecs[2]; + + for(int j=0; j<2; j++) + { + int m = -1; + if(j == 1) + { + m = 1; + } + + if(types[i+m].range < blind) + { + if(types[i].range > inf_bound) + { + types[i].edj[j] = Nr_inf; + } + else + { + types[i].edj[j] = Nr_blind; + } + continue; + } + + vecs[j] = Eigen::Vector3d(pl[i+m].x, pl[i+m].y, pl[i+m].z); + vecs[j] = vecs[j] - vec_a; + + types[i].angle[j] = vec_a.dot(vecs[j]) / vec_a.norm() / vecs[j].norm(); + if(types[i].angle[j] < jump_up_limit) + { + types[i].edj[j] = Nr_180; + } + else if(types[i].angle[j] > jump_down_limit) + { + types[i].edj[j] = Nr_zero; + } + } + + types[i].intersect = vecs[Prev].dot(vecs[Next]) / vecs[Prev].norm() / vecs[Next].norm(); + if(types[i].edj[Prev]==Nr_nor && types[i].edj[Next]==Nr_zero && types[i].dista>0.0225 && types[i].dista>4*types[i-1].dista) + { + if(types[i].intersect > cos160) + { + if(edge_jump_judge(pl, types, i, Prev)) + { + types[i].ftype = Edge_Jump; + } + } + } + else if(types[i].edj[Prev]==Nr_zero && types[i].edj[Next]== Nr_nor && types[i-1].dista>0.0225 && types[i-1].dista>4*types[i].dista) + { + if(types[i].intersect > cos160) + { + if(edge_jump_judge(pl, types, i, Next)) + { + types[i].ftype = Edge_Jump; + } + } + } + else if(types[i].edj[Prev]==Nr_nor && types[i].edj[Next]==Nr_inf) + { + if(edge_jump_judge(pl, types, i, Prev)) + { + types[i].ftype = Edge_Jump; + } + } + else if(types[i].edj[Prev]==Nr_inf && types[i].edj[Next]==Nr_nor) + { + if(edge_jump_judge(pl, types, i, Next)) + { + types[i].ftype = Edge_Jump; + } + + } + else if(types[i].edj[Prev]>Nr_nor && types[i].edj[Next]>Nr_nor) + { + if(types[i].ftype == Nor) + { + types[i].ftype = Wire; + } + } + } + + plsize2 = plsize-1; + double ratio; + for(uint i=head+1; i types[i].dista) + { + ratio = types[i-1].dista / types[i].dista; + } + else + { + ratio = types[i].dista / types[i-1].dista; + } + + if(types[i].intersect &types, uint i_cur, uint &i_nex, Eigen::Vector3d &curr_direct) +{ + double group_dis = disA*types[i_cur].range + disB; + group_dis = group_dis * group_dis; + // i_nex = i_cur; + + double two_dis; + vector disarr; + disarr.reserve(20); + + for(i_nex=i_cur; i_nex= pl.size()) || (i_nex >= pl.size())) break; + + if(types[i_nex].range < blind) + { + curr_direct.setZero(); + return 2; + } + vx = pl[i_nex].x - pl[i_cur].x; + vy = pl[i_nex].y - pl[i_cur].y; + vz = pl[i_nex].z - pl[i_cur].z; + two_dis = vx*vx + vy*vy + vz*vz; + if(two_dis >= group_dis) + { + break; + } + disarr.push_back(types[i_nex].dista); + i_nex++; + } + + double leng_wid = 0; + double v1[3], v2[3]; + for(uint j=i_cur+1; j= pl.size()) || (i_cur >= pl.size())) break; + v1[0] = pl[j].x - pl[i_cur].x; + v1[1] = pl[j].y - pl[i_cur].y; + v1[2] = pl[j].z - pl[i_cur].z; + + v2[0] = v1[1]*vz - vy*v1[2]; + v2[1] = v1[2]*vx - v1[0]*vz; + v2[2] = v1[0]*vy - vx*v1[1]; + + double lw = v2[0]*v2[0] + v2[1]*v2[1] + v2[2]*v2[2]; + if(lw > leng_wid) + { + leng_wid = lw; + } + } + + + if((two_dis*two_dis/leng_wid) < p2l_ratio) + { + curr_direct.setZero(); + return 0; + } + + uint disarrsize = disarr.size(); + for(uint j=0; j=limit_maxmid || dismid_min>=limit_midmin) + { + curr_direct.setZero(); + return 0; + } + } + else + { + double dismax_min = disarr[0] / disarr[disarrsize-2]; + if(dismax_min >= limit_maxmin) + { + curr_direct.setZero(); + return 0; + } + } + + curr_direct << vx, vy, vz; + curr_direct.normalize(); + return 1; +} + +bool Preprocess::edge_jump_judge(const PointCloudXYZI &pl, vector &types, uint i, Surround nor_dir) +{ + if(nor_dir == 0) + { + if(types[i-1].rangeedgea*d2 || (d1-d2)>edgeb) + { + return false; + } + + return true; +} diff --git a/src/Point-LIO/src/preprocess.h b/src/Point-LIO/src/preprocess.h new file mode 100755 index 0000000..542d600 --- /dev/null +++ b/src/Point-LIO/src/preprocess.h @@ -0,0 +1,145 @@ +#pragma once + +#include +#include +#include +#include + +using namespace std; + +#define IS_VALID(a) ((abs(a)>1e8) ? true : false) + +typedef pcl::PointXYZINormal PointType; +typedef pcl::PointCloud PointCloudXYZI; + +enum LID_TYPE{AVIA = 1, VELO16, OUST64, HESAIxt32}; //{1, 2, 3, 4} +enum TIME_UNIT{SEC = 0, MS = 1, US = 2, NS = 3}; +enum Feature{Nor, Poss_Plane, Real_Plane, Edge_Jump, Edge_Plane, Wire, ZeroPoint}; +enum Surround{Prev, Next}; +enum E_jump{Nr_nor, Nr_zero, Nr_180, Nr_inf, Nr_blind}; + +struct orgtype +{ + double range; + double dista; + double angle[2]; + double intersect; + E_jump edj[2]; + Feature ftype; + orgtype() + { + range = 0; + edj[Prev] = Nr_nor; + edj[Next] = Nr_nor; + ftype = Nor; + intersect = 2; + } +}; + +namespace velodyne_ros { + struct EIGEN_ALIGN16 Point { + PCL_ADD_POINT4D; + float intensity; + float time; + uint16_t ring; + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + }; +} // namespace velodyne_ros +POINT_CLOUD_REGISTER_POINT_STRUCT(velodyne_ros::Point, + (float, x, x) + (float, y, y) + (float, z, z) + (float, intensity, intensity) + (float, time, time) + (std::uint16_t, ring, ring) +) + +namespace hesai_ros { + struct EIGEN_ALIGN16 Point { + PCL_ADD_POINT4D; + float intensity; + double timestamp; + uint16_t ring; + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + }; +} // namespace velodyne_ros +POINT_CLOUD_REGISTER_POINT_STRUCT(hesai_ros::Point, + (float, x, x) + (float, y, y) + (float, z, z) + (float, intensity, intensity) + (double, timestamp, timestamp) + (std::uint16_t, ring, ring) +) + +namespace ouster_ros { + struct EIGEN_ALIGN16 Point { + PCL_ADD_POINT4D; + float intensity; + uint32_t t; + uint16_t reflectivity; + uint8_t ring; + uint16_t ambient; + uint32_t range; + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + }; +} // namespace ouster_ros + +// clang-format off +POINT_CLOUD_REGISTER_POINT_STRUCT(ouster_ros::Point, + (float, x, x) + (float, y, y) + (float, z, z) + (float, intensity, intensity) + // use std::uint32_t to avoid conflicting with pcl::uint32_t + (std::uint32_t, t, t) + (std::uint16_t, reflectivity, reflectivity) + (std::uint8_t, ring, ring) + (std::uint16_t, ambient, ambient) + (std::uint32_t, range, range) +) + +class Preprocess +{ + public: +// EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + Preprocess(); + ~Preprocess(); + + void process(const livox_ros_driver2::CustomMsg::ConstPtr &msg, PointCloudXYZI::Ptr &pcl_out); + void process(const sensor_msgs::PointCloud2::ConstPtr &msg, PointCloudXYZI::Ptr &pcl_out); + void set(bool feat_en, int lid_type, double bld, int pfilt_num); + + // sensor_msgs::PointCloud2::ConstPtr pointcloud; + PointCloudXYZI pl_full, pl_corn, pl_surf; + PointCloudXYZI pl_buff[128]; //maximum 128 line lidar + vector typess[128]; //maximum 128 line lidar + float time_unit_scale; + int lidar_type, point_filter_num, N_SCANS, SCAN_RATE, time_unit; + double blind; + bool given_offset_time; + ros::Publisher pub_full, pub_surf, pub_corn; + + + private: + void avia_handler(const livox_ros_driver2::CustomMsg::ConstPtr &msg); + void oust64_handler(const sensor_msgs::PointCloud2::ConstPtr &msg); + void velodyne_handler(const sensor_msgs::PointCloud2::ConstPtr &msg); + void hesai_handler(const sensor_msgs::PointCloud2::ConstPtr &msg); + void give_feature(PointCloudXYZI &pl, vector &types); + void pub_func(PointCloudXYZI &pl, const ros::Time &ct); + int plane_judge(const PointCloudXYZI &pl, vector &types, uint i, uint &i_nex, Eigen::Vector3d &curr_direct); + bool small_plane(const PointCloudXYZI &pl, vector &types, uint i_cur, uint &i_nex, Eigen::Vector3d &curr_direct); + bool edge_jump_judge(const PointCloudXYZI &pl, vector &types, uint i, Surround nor_dir); + + int group_size; + double disA, disB, inf_bound; + double limit_maxmid, limit_midmin, limit_maxmin; + double p2l_ratio; + double jump_up_limit, jump_down_limit; + double cos160; + double edgea, edgeb; + double smallp_intersect, smallp_ratio; + double vx, vy, vz; +}; diff --git a/src/livox_ros_driver2 b/src/livox_ros_driver2 new file mode 160000 index 0000000..6b9356c --- /dev/null +++ b/src/livox_ros_driver2 @@ -0,0 +1 @@ +Subproject commit 6b9356cadf77084619ba406e6a0eb41163b08039