diff --git a/.cppcheck_suppressions.txt b/.cppcheck_suppressions.txt index 8450ded3..5b6b41b6 100644 --- a/.cppcheck_suppressions.txt +++ b/.cppcheck_suppressions.txt @@ -1,4 +1,4 @@ syntaxError preprocessorErrorDirective -uninitvar:include/gtsam_ext/optimizers/dogleg_optimizer_ext_impl.hpp \ No newline at end of file +uninitvar:include/gtsam_points/optimizers/dogleg_optimizer_ext_impl.hpp \ No newline at end of file diff --git a/.github/workflows/build.yml b/.github/workflows/build.yml index 0e836244..fb1cec57 100644 --- a/.github/workflows/build.yml +++ b/.github/workflows/build.yml @@ -16,7 +16,14 @@ jobs: runs-on: ubuntu-latest strategy: matrix: - DISTRO: [focal, focal_cuda11.2, focal_llvm, focal_cuda11.2_llvm, jammy, jammy_cuda11.8, jammy_llvm, jammy_cuda11.8_llvm] + DISTRO: [ + { "ubuntu": "jammy", "suffix": "gcc" }, + { "ubuntu": "jammy", "suffix": "llvm" }, + { "ubuntu": "jammy_cuda12.2", "suffix": "gcc.cuda" }, + { "ubuntu": "jammy_cuda12.2", "suffix": "llvm.cuda" }, + { "ubuntu": "noble", "suffix": "gcc" }, + { "ubuntu": "noble", "suffix": "llvm" } + ] steps: - uses: actions/checkout@v2 @@ -33,6 +40,8 @@ jobs: - name: Docker build uses: docker/build-push-action@v2 with: - file: ${{github.workspace}}/docker/${{ matrix.DISTRO }}/Dockerfile + file: ${{github.workspace}}/docker/ubuntu/Dockerfile.${{ matrix.DISTRO.suffix }} + build-args: | + BASE_IMAGE=koide3/gtsam_docker:${{ matrix.DISTRO.ubuntu }} context: . push: false diff --git a/.github/workflows/license.yml b/.github/workflows/license.yml new file mode 100644 index 00000000..a84e0268 --- /dev/null +++ b/.github/workflows/license.yml @@ -0,0 +1,21 @@ +name: license + +on: + push: + branches: [ master ] + paths-ignore: '**.md' + pull_request: + branches: [ master ] + paths-ignore: '**.md' + +jobs: + license_check: + name: License check + runs-on: ubuntu-latest + + steps: + - uses: actions/checkout@v4 + + - name: Check license + run: | + find include/gtsam_points src/gtsam_points -type f | xargs -I filename bash -c 'if ! grep -q Copyright filename; then echo filename : lisence not found; exit 1; fi' \ No newline at end of file diff --git a/.gitmodules b/.gitmodules index 5e02e3e6..e69de29b 100644 --- a/.gitmodules +++ b/.gitmodules @@ -1,6 +0,0 @@ -[submodule "thirdparty/Eigen"] - path = thirdparty/Eigen - url = https://gitlab.com/libeigen/eigen.git -[submodule "thirdparty/nanoflann"] - path = thirdparty/nanoflann - url = https://github.com/jlblancoc/nanoflann.git diff --git a/CMakeLists.txt b/CMakeLists.txt index 480507c7..c86f6dda 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,5 +1,5 @@ -cmake_minimum_required(VERSION 3.0.2) -project(gtsam_ext) +cmake_minimum_required(VERSION 3.22) +project(gtsam_points VERSION 0.1.0 LANGUAGES CXX) set(CMAKE_CXX_STANDARD 17) set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "${CMAKE_CURRENT_LIST_DIR}/cmake") @@ -13,9 +13,10 @@ option(BUILD_TESTS "Build test" OFF) option(BUILD_DEMO "Build demo programs" OFF) option(BUILD_EXAMPLE "Build example programs" OFF) option(BUILD_WITH_CUDA "Build with GPU support" OFF) -option(BUILD_WITH_CUDA_MULTIARCH "Build with GPU cross-platform support" OFF) +option(BUILD_WITH_CUDA_MULTIARCH "Build with CUDA multi-architecture support" OFF) option(BUILD_WITH_MARCH_NATIVE "Build with -march=native" OFF) option(ENABLE_CPPCHECK "Enable cppcheck" OFF) +option(ENABLE_COVERAGE "Enable coverage check" OFF) if(BUILD_WITH_MARCH_NATIVE) add_compile_options(-march=native) @@ -23,74 +24,60 @@ if(BUILD_WITH_MARCH_NATIVE) set(CMAKE_CXX_FLAGS "-march=native ${CMAKE_CXX_FLAGS}") endif() -find_package(Boost REQUIRED COMPONENTS timer filesystem) +find_package(Boost REQUIRED COMPONENTS filesystem) find_package(GTSAM REQUIRED) - +find_package(OpenMP REQUIRED) find_package(Eigen3 REQUIRED) -if(${BUILD_WITH_CUDA} AND ${EIGEN3_VERSION_STRING} VERSION_LESS "3.3.90") - set(EIGEN3_INCLUDE_DIR "thirdparty/Eigen") - message(STATUS "Use bundled Eigen because the system Eigen ${EIGEN3_VERSION_STRING} may not compatible with CUDA") - message(STATUS "Override EIGEN3_INCLUDE_DIR: ${EIGEN3_INCLUDE_DIR}") -endif() -find_package(OpenMP) -if (OPENMP_FOUND) - set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${OpenMP_C_FLAGS}") - set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS}") +if(${BUILD_WITH_CUDA} AND ${EIGEN3_VERSION_STRING} VERSION_LESS "3.3.90") + message(WARNING "Detected Eigen ${EIGEN3_VERSION_STRING} is not compatible with CUDA") + message(WARNING "Use Eigen 3.3.90 or later (3.4.0 is recommended)") endif() # GPU-related if(BUILD_WITH_CUDA) - find_package(CUDA REQUIRED) - add_definitions(-DBUILD_GTSAM_EXT_GPU) + add_definitions(-DBUILD_GTSAM_POINTS_GPU) + find_package(CUDAToolkit REQUIRED) + enable_language(CUDA) set(CUDA_STANDARD 17) - set(CMAKE_CUDA_FLAGS "${CMAKE_CUDA_FLAGS} -Wno-c99-extensions") - set(CUDA_NVCC_FLAGS "${CUDA_NVCC_FLAGS} --std=c++14 --expt-relaxed-constexpr") - - if(BUILD_WITH_CUDA_MULTIARCH) - set(CUDA_NVCC_FLAGS "${CUDA_NVCC_FLAGS} -arch=sm_52") - - set(CUDA_NVCC_FLAGS "${CUDA_NVCC_FLAGS} \ - -gencode=arch=compute_52,code=sm_52 \ - -gencode=arch=compute_60,code=sm_60 \ - -gencode=arch=compute_61,code=sm_61 \ - -gencode=arch=compute_70,code=sm_70 \ - -gencode=arch=compute_75,code=sm_75 \ - ") - - if(${CUDA_VERSION} VERSION_GREATER_EQUAL 11.2) - set(CUDA_NVCC_FLAGS "${CUDA_NVCC_FLAGS} \ - -gencode=arch=compute_80,code=sm_80 \ - -gencode=arch=compute_80,code=compute_80 \ - ") + + if(${CMAKE_VERSION} VERSION_GREATER_EQUAL "3.24.0") + if(BUILD_WITH_CUDA_MULTIARCH) + set(CMAKE_CUDA_ARCHITECTURES "all-major" CACHE STRING "CUDA architectures" FORCE) else() - set(CUDA_NVCC_FLAGS "${CUDA_NVCC_FLAGS} \ - -gencode=arch=compute_75,code=compute_75 \ - ") + set(CMAKE_CUDA_ARCHITECTURES "native" CACHE STRING "CUDA architectures" FORCE) endif() - message(STATUS "CUDA_MULTIARCH") else() - CUDA_SELECT_NVCC_ARCH_FLAGS(ARCH_FLAGS Auto) - list(APPEND CUDA_NVCC_FLAGS ${ARCH_FLAGS}) - message(STATUS "CUDA_SELECT_NVCC_ARCH_FLAGS:${ARCH_FLAGS}") + if(BUILD_WITH_CUDA_MULTIARCH) + set(CMAKE_CUDA_ARCHITECTURES 75 80 89 90 CACHE STRING "CUDA architectures" FORCE) + else() + if(NOT DEFINED CMAKE_CUDA_ARCHITECTURES OR CMAKE_CUDA_ARCHITECTURES STREQUAL "52") + message(WARNING "CMAKE_CUDA_ARCHITECTURES is not defined. Set to 89 by default") + set(CMAKE_CUDA_ARCHITECTURES 89 CACHE STRING "CUDA architectures" FORCE) + endif() + endif() endif() + message(STATUS "CMAKE_CUDA_ARCHITECTURES: ${CMAKE_CUDA_ARCHITECTURES}") + + set(CMAKE_CUDA_FLAGS "${CMAKE_CUDA_FLAGS} -Wno-c99-extensions") + set(CMAKE_CUDA_FLAGS "${CMAKE_CUDA_FLAGS} --expt-relaxed-constexpr") if(${CMAKE_SYSTEM_PROCESSOR} STREQUAL "aarch64") else() # Suppress Eigen-related CUDA warnings - set(CUDA_NVCC_FLAGS "${CUDA_NVCC_FLAGS} -Xcudafe --diag_suppress=177") - set(CUDA_NVCC_FLAGS "${CUDA_NVCC_FLAGS} -Xcudafe --diag_suppress=2739") - - if(${CUDA_VERSION} VERSION_GREATER_EQUAL 11.2) - set(CUDA_NVCC_FLAGS "${CUDA_NVCC_FLAGS} -Xcudafe --diag_suppress=20011") - set(CUDA_NVCC_FLAGS "${CUDA_NVCC_FLAGS} -Xcudafe --diag_suppress=20014") - set(CUDA_NVCC_FLAGS "${CUDA_NVCC_FLAGS} -Xcudafe --diag_suppress=set_but_not_used") - set(CUDA_NVCC_FLAGS "${CUDA_NVCC_FLAGS} -Xcudafe --diag_suppress=esa_on_defaulted_function_ignored") + set(CMAKE_CUDA_FLAGS "${CMAKE_CUDA_FLAGS} -Xcudafe --diag_suppress=177") + set(CMAKE_CUDA_FLAGS "${CMAKE_CUDA_FLAGS} -Xcudafe --diag_suppress=2739") + + if(${CUDAToolkit_VERSION} VERSION_GREATER_EQUAL 11.2) + set(CMAKE_CUDA_FLAGS "${CMAKE_CUDA_FLAGS} -Xcudafe --diag_suppress=20011") + set(CMAKE_CUDA_FLAGS "${CMAKE_CUDA_FLAGS} -Xcudafe --diag_suppress=20014") + set(CMAKE_CUDA_FLAGS "${CMAKE_CUDA_FLAGS} -Xcudafe --diag_suppress=set_but_not_used") + set(CMAKE_CUDA_FLAGS "${CMAKE_CUDA_FLAGS} -Xcudafe --diag_suppress=esa_on_defaulted_function_ignored") endif() - if(${CUDA_VERSION} VERSION_GREATER_EQUAL 11.6) - set(CUDA_NVCC_FLAGS "${CUDA_NVCC_FLAGS} -Xcudafe --diag_suppress=20236") + if(${CUDAToolkit_VERSION} VERSION_GREATER_EQUAL 11.6) + set(CMAKE_CUDA_FLAGS "${CMAKE_CUDA_FLAGS} -Xcudafe --diag_suppress=20236") endif() endif() endif() @@ -106,111 +93,128 @@ if(ENABLE_CPPCHECK) ) endif() +if(ENABLE_COVERAGE) + set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -coverage") + + find_program(LCOV lcov REQUIRED) + find_program(GENHTML genhtml REQUIRED) + + add_custom_target(coverage + COMMAND ${LCOV} --directory . --capture --output-file coverage.info + COMMAND ${GENHTML} --demangle-cpp -o coverage coverage.info + WORKING_DIRECTORY ${CMAKE_BINARY_DIR}) +endif() + ########### ## Build ## ########### -add_library(gtsam_ext SHARED +add_library(gtsam_points SHARED # util - src/gtsam_ext/util/covariance_estimation.cpp - src/gtsam_ext/util/normal_estimation.cpp - src/gtsam_ext/util/edge_plane_extraction.cpp - src/gtsam_ext/util/bspline.cpp - src/gtsam_ext/util/continuous_trajectory.cpp + src/gtsam_points/util/covariance_estimation.cpp + src/gtsam_points/util/normal_estimation.cpp + src/gtsam_points/util/bspline.cpp + src/gtsam_points/util/continuous_trajectory.cpp # ann - src/gtsam_ext/ann/kdtree.cpp - src/gtsam_ext/ann/intensity_kdtree.cpp - src/gtsam_ext/ann/ivox.cpp - src/gtsam_ext/ann/ivox_covariance_estimation.cpp + src/gtsam_points/ann/kdtree.cpp + src/gtsam_points/ann/intensity_kdtree.cpp + src/gtsam_points/ann/ivox.cpp + src/gtsam_points/ann/incremental_covariance_container.cpp + src/gtsam_points/ann/incremental_covariance_voxelmap.cpp # types - src/gtsam_ext/types/frame.cpp - src/gtsam_ext/types/frame_cpu.cpp - src/gtsam_ext/types/voxelized_frame_cpu.cpp - src/gtsam_ext/types/gaussian_voxelmap_cpu.cpp + src/gtsam_points/types/point_cloud.cpp + src/gtsam_points/types/point_cloud_cpu.cpp + src/gtsam_points/types/gaussian_voxelmap_cpu.cpp + src/gtsam_points/types/gaussian_voxelmap_cpu_funcs.cpp # factors - src/gtsam_ext/factors/integrated_matching_cost_factor.cpp - src/gtsam_ext/factors/integrated_icp_factor.cpp - src/gtsam_ext/factors/integrated_gicp_factor.cpp - src/gtsam_ext/factors/integrated_vgicp_factor.cpp - src/gtsam_ext/factors/integrated_loam_factor.cpp - src/gtsam_ext/factors/intensity_gradients.cpp - src/gtsam_ext/factors/intensity_gradients_ivox.cpp - src/gtsam_ext/factors/integrated_colored_gicp_factor.cpp - src/gtsam_ext/factors/integrated_color_consistency_factor.cpp - src/gtsam_ext/factors/integrated_ct_icp_factor.cpp - src/gtsam_ext/factors/integrated_ct_gicp_factor.cpp - src/gtsam_ext/factors/bundle_adjustment_factor_evm.cpp - src/gtsam_ext/factors/bundle_adjustment_factor_lsq.cpp + src/gtsam_points/factors/integrated_matching_cost_factor.cpp + src/gtsam_points/factors/integrated_icp_factor.cpp + src/gtsam_points/factors/integrated_gicp_factor.cpp + src/gtsam_points/factors/integrated_vgicp_factor.cpp + src/gtsam_points/factors/integrated_loam_factor.cpp + src/gtsam_points/factors/intensity_gradients.cpp + src/gtsam_points/factors/integrated_colored_gicp_factor.cpp + src/gtsam_points/factors/integrated_color_consistency_factor.cpp + src/gtsam_points/factors/integrated_ct_icp_factor.cpp + src/gtsam_points/factors/integrated_ct_gicp_factor.cpp + src/gtsam_points/factors/bundle_adjustment_factor_evm.cpp + src/gtsam_points/factors/bundle_adjustment_factor_lsq.cpp # experimental - src/gtsam_ext/factors/experimental/continuous_time_icp_factor.cpp + src/gtsam_points/factors/experimental/continuous_time_icp_factor.cpp # optimizers - src/gtsam_ext/optimizers/linearization_hook.cpp - src/gtsam_ext/optimizers/linear_system_builder.cpp - src/gtsam_ext/optimizers/gaussian_factor_graph_solver.cpp - src/gtsam_ext/optimizers/levenberg_marquardt_ext.cpp - src/gtsam_ext/optimizers/levenberg_marquardt_optimization_status.cpp - src/gtsam_ext/optimizers/isam2_ext.cpp - src/gtsam_ext/optimizers/isam2_ext_impl.cpp - src/gtsam_ext/optimizers/incremental_fixed_lag_smoother_ext.cpp - src/gtsam_ext/optimizers/incremental_fixed_lag_smoother_ext_with_fallback.cpp - # src/gtsam_ext/optimizers/dogleg_optimizer_ext.cpp - src/gtsam_ext/optimizers/dogleg_optimizer_ext_impl.cpp + src/gtsam_points/optimizers/fast_scatter.cpp + src/gtsam_points/optimizers/linearization_hook.cpp + src/gtsam_points/optimizers/linear_system_builder.cpp + src/gtsam_points/optimizers/gaussian_factor_graph_solver.cpp + src/gtsam_points/optimizers/levenberg_marquardt_ext.cpp + src/gtsam_points/optimizers/levenberg_marquardt_optimization_status.cpp + src/gtsam_points/optimizers/isam2_ext.cpp + src/gtsam_points/optimizers/isam2_ext_impl.cpp + src/gtsam_points/optimizers/incremental_fixed_lag_smoother_ext.cpp + src/gtsam_points/optimizers/incremental_fixed_lag_smoother_ext_with_fallback.cpp + # src/gtsam_points/optimizers/dogleg_optimizer_ext.cpp + src/gtsam_points/optimizers/dogleg_optimizer_ext_impl.cpp ) -target_include_directories(gtsam_ext PUBLIC - include - ${EIGEN3_INCLUDE_DIR} - thirdparty/nanoflann/include - ${Boost_INCLUDE_DIRS} - ${GTSAM_INCLUDE_DIRS} +target_include_directories(gtsam_points PUBLIC + $ + $ + $ ) -target_link_libraries(gtsam_ext - pthread - ${Boost_LIBRARIES} - ${GTSAM_LIBRARIES} +target_link_libraries(gtsam_points + Boost::boost + Boost::filesystem + Eigen3::Eigen + GTSAM::GTSAM + OpenMP::OpenMP_CXX ) # GPU-related if(BUILD_WITH_CUDA) - cuda_add_library(gtsam_ext_cuda SHARED + add_library(gtsam_points_cuda SHARED # cuda-related - src/gtsam_ext/cuda/check_error.cu - src/gtsam_ext/cuda/check_error_cusolver.cu - src/gtsam_ext/cuda/check_error_curand.cu - src/gtsam_ext/cuda/cuda_memory.cu - src/gtsam_ext/cuda/cuda_stream.cu - src/gtsam_ext/cuda/cuda_buffer.cu - src/gtsam_ext/cuda/cuda_device_sync.cu - src/gtsam_ext/cuda/cuda_device_prop.cu - src/gtsam_ext/cuda/cuda_graph.cu - src/gtsam_ext/cuda/cuda_graph_exec.cu - src/gtsam_ext/cuda/gl_buffer_map.cpp - src/gtsam_ext/cuda/nonlinear_factor_set_gpu.cu - src/gtsam_ext/cuda/nonlinear_factor_set_gpu_create.cu - src/gtsam_ext/cuda/stream_roundrobin.cu - src/gtsam_ext/cuda/stream_temp_buffer_roundrobin.cu + src/gtsam_points/cuda/check_error.cu + src/gtsam_points/cuda/check_error_cusolver.cu + src/gtsam_points/cuda/check_error_curand.cu + src/gtsam_points/cuda/cuda_memory.cu + src/gtsam_points/cuda/cuda_stream.cu + src/gtsam_points/cuda/cuda_buffer.cu + src/gtsam_points/cuda/cuda_device_sync.cu + src/gtsam_points/cuda/cuda_device_prop.cu + src/gtsam_points/cuda/cuda_graph.cu + src/gtsam_points/cuda/cuda_graph_exec.cu + src/gtsam_points/cuda/gl_buffer_map.cu + src/gtsam_points/cuda/nonlinear_factor_set_gpu.cu + src/gtsam_points/cuda/nonlinear_factor_set_gpu_create.cu + src/gtsam_points/cuda/stream_roundrobin.cu + src/gtsam_points/cuda/stream_temp_buffer_roundrobin.cu # types - src/gtsam_ext/types/frame.cu - src/gtsam_ext/types/frame_gpu.cu - src/gtsam_ext/types/voxelized_frame_gpu.cu - src/gtsam_ext/types/gaussian_voxelmap_gpu.cu + src/gtsam_points/types/point_cloud.cu + src/gtsam_points/types/point_cloud_gpu.cu + src/gtsam_points/types/gaussian_voxelmap_gpu.cu + src/gtsam_points/types/gaussian_voxelmap_gpu_funcs.cu # factors - src/gtsam_ext/factors/integrated_vgicp_derivatives.cu - src/gtsam_ext/factors/integrated_vgicp_derivatives_inliers.cu - src/gtsam_ext/factors/integrated_vgicp_derivatives_compute.cu - src/gtsam_ext/factors/integrated_vgicp_derivatives_linearize.cu - src/gtsam_ext/factors/integrated_vgicp_factor_gpu.cu + src/gtsam_points/factors/integrated_vgicp_derivatives.cu + src/gtsam_points/factors/integrated_vgicp_derivatives_inliers.cu + src/gtsam_points/factors/integrated_vgicp_derivatives_compute.cu + src/gtsam_points/factors/integrated_vgicp_derivatives_linearize.cu + src/gtsam_points/factors/integrated_vgicp_factor_gpu.cu # util - src/gtsam_ext/util/easy_profiler_cuda.cu + src/gtsam_points/util/easy_profiler_cuda.cu ) - target_include_directories(gtsam_ext_cuda PUBLIC - include - ${EIGEN3_INCLUDE_DIR} - ${Boost_INCLUDE_DIRS} - ${GTSAM_INCLUDE_DIRS} - ${CUDA_INCLUDE_DIRS} + target_include_directories(gtsam_points_cuda PUBLIC + $ + $ ) - target_link_libraries(gtsam_ext - gtsam_ext_cuda + target_link_libraries(gtsam_points_cuda + Boost::boost + Eigen3::Eigen + GTSAM::GTSAM + OpenMP::OpenMP_CXX + CUDA::cudart + ) + + target_link_libraries(gtsam_points + gtsam_points_cuda ) endif() @@ -223,8 +227,7 @@ if(BUILD_DEMO) get_filename_component(demo_name ${demo_src} NAME_WE) add_executable(${demo_name} ${demo_src}) - target_include_directories(${demo_name} PUBLIC ${Iridescence_INCLUDE_DIRS}) - target_link_libraries(${demo_name} gtsam_ext ${Iridescence_LIBRARIES}) + target_link_libraries(${demo_name} gtsam_points Iridescence::Iridescence) endforeach() endif() @@ -237,8 +240,7 @@ if(BUILD_EXAMPLE) get_filename_component(example_name ${example_src} NAME_WE) add_executable(${example_name} ${example_src}) - target_include_directories(${example_name} PUBLIC ${Iridescence_INCLUDE_DIRS}) - target_link_libraries(${example_name} gtsam_ext ${Iridescence_LIBRARIES}) + target_link_libraries(${example_name} gtsam_points Iridescence::Iridescence) endforeach() endif() @@ -260,8 +262,53 @@ if(BUILD_TESTS) get_filename_component(test_name ${test_src} NAME_WE) add_executable(${test_name} ${test_src}) - target_link_libraries(${test_name} gtsam_ext gtest_main) - target_include_directories(${test_name} PRIVATE ${Boost_INCLUDE_DIRS}) + target_link_libraries(${test_name} gtsam_points gtest_main) + target_include_directories(${test_name} PRIVATE ${Boost_INCLUDE_DIRS} src/test/include) gtest_discover_tests(${test_name} WORKING_DIRECTORY "${CMAKE_SOURCE_DIR}") endforeach() endif() + + +############# +## Install ## +############# + +include(GNUInstallDirs) +install(DIRECTORY include/ thirdparty/nanoflann/include/ DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}) + +list(APPEND GTSAM_POINTS_LIBRARIES gtsam_points) +if(BUILD_WITH_CUDA) + list(APPEND GTSAM_POINTS_LIBRARIES gtsam_points_cuda) +endif() + +install(TARGETS ${GTSAM_POINTS_LIBRARIES} + EXPORT gtsam_points-targets + LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR} + ARCHIVE DESTINATION ${CMAKE_INSTALL_LIBDIR} + RUNTIME DESTINATION ${CMAKE_INSTALL_BINDIR} +) +set(CMAKE_CONFIG_INSTALL_DIR + "${CMAKE_INSTALL_LIBDIR}/cmake/gtsam_points" + CACHE PATH "Install directory for CMake config files" +) +include(CMakePackageConfigHelpers) +install(EXPORT gtsam_points-targets + FILE gtsam_points-targets.cmake + NAMESPACE gtsam_points:: + DESTINATION ${CMAKE_CONFIG_INSTALL_DIR} +) +configure_package_config_file( + "${CMAKE_CURRENT_SOURCE_DIR}/cmake/gtsam_points-config.cmake.in" + "${CMAKE_CURRENT_BINARY_DIR}/gtsam_points-config.cmake" + INSTALL_DESTINATION ${CMAKE_CONFIG_INSTALL_DIR} +) +write_basic_package_version_file( + "${CMAKE_CURRENT_BINARY_DIR}/gtsam_points-config-version.cmake" + VERSION ${VERSION} + COMPATIBILITY SameMajorVersion +) +install(FILES + "${CMAKE_CURRENT_BINARY_DIR}/gtsam_points-config.cmake" + "${CMAKE_CURRENT_BINARY_DIR}/gtsam_points-config-version.cmake" + DESTINATION ${CMAKE_CONFIG_INSTALL_DIR} +) diff --git a/README.md b/README.md index 2ad0b39d..a6e3ed8b 100644 --- a/README.md +++ b/README.md @@ -1,55 +1,55 @@ -# gtsam_ext +# gtsam_points -This is a collection of GTSAM factors and optimizers for range-based SLAM. +This is a collection of [GTSAM](https://gtsam.org/) factors and optimizers for range-based SLAM. -Tested on Ubuntu 20.04 and CUDA 11.6 / Ubuntu 22.04 and CUDA 11.8 / NVIDIA Jetson Xavier and Orin (JetPack 5.0.1). +Tested on Ubuntu 22.04 and CUDA 12.2 / NVIDIA Jetson Xavier and Orin (JetPack 5.0.1) with **GTSAM 4.2a9**. -[![Build](https://github.com/koide3/gtsam_ext/actions/workflows/build.yml/badge.svg)](https://github.com/koide3/gtsam_ext/actions/workflows/build.yml) +[![Build](https://github.com/koide3/gtsam_points/actions/workflows/build.yml/badge.svg)](https://github.com/koide3/gtsam_points/actions/workflows/build.yml) ## Factors ### Scan Matching Factors - **IntegratedICPFactor & IntegratedPointToPlaneICPFactor** - The conventional point-to-point and point-to-plane ICP [[1]](#ICP) + The conventional point-to-point and point-to-plane ICP [[1]](#ICP). - **IntegratedGICPFactor** - Generalized ICP based on the distribution-to-distribution distance [[2]](#GICP) + Generalized ICP based on the distribution-to-distribution distance [[2]](#GICP). - **IntegratedVGICPFactor** - GICP with voxel-based data association and multi-distribution-correspondence [[3]](#VGICP1)[[4]](#VGICP2) + GICP with voxel-based data association and multi-distribution-correspondence [[3]](#VGICP1)[[4]](#VGICP2). - **IntegratedVGICPFactorGPU** - GPU implementation of VGICP [[3]](#VGICP1)[[4]](#VGICP2) - To enable this factor, set ```-DBUILD_WITH_CUDA=ON``` + GPU implementation of VGICP [[3]](#VGICP1)[[4]](#VGICP2). + To enable this factor, set ```-DBUILD_WITH_CUDA=ON```. - **IntegratedLOAMFactor** - Matching cost factor based on the combination of point-to-plane and point-to-edge distances [[5]](#LOAM)[[6]](#LEGO) + Matching cost factor based on the combination of point-to-plane and point-to-edge distances [[5]](#LOAM)[[6]](#LEGO). ### Colored Scan Matching Factors - **IntegratedColorConsistencyFactor** - Photometric ICP error [[7]](#COLORED) + Photometric ICP error [[7]](#COLORED). - **IntegratedColoredGICPFactor** - Photometric ICP error + GICP geometric error [[2]](#GICP)[[7]](#COLORED) + Photometric ICP error + GICP geometric error [[2]](#GICP)[[7]](#COLORED). ### Continuous-time ICP Factors - **IntegratedCT_ICPFactor** - Continuous Time ICP Factor [[8]](#CTICP) + Continuous Time ICP Factor [[8]](#CTICP). - **IntegratedCT_GICPFactor** - Continuous Time ICP with GICP's D2D matching cost [[2]](#GICP)[[8]](#CTICP) + Continuous Time ICP with GICP's D2D matching cost [[2]](#GICP)[[8]](#CTICP). ### Bundle Adjustment Factors - **PlaneEVMFactor and EdgeEVMFactor** - Bundle adjustment factor based on Eigenvalue minimization [[9]](#BA_EVM) + Bundle adjustment factor based on Eigenvalue minimization [[9]](#BA_EVM). - **LsqBundleAdjustmentFactor** - Bundle adjustment factor based on EVM and EF optimal condition satisfaction [[10]](#BA_LSQ) + Bundle adjustment factor based on EVM and EF optimal condition satisfaction [[10]](#BA_LSQ). ## Optimizers for GPU Factors -All the following optimizers were derived from the implementations in GTSAM +All the following optimizers were derived from the implementations in GTSAM. - **LevenbergMarquardtOptimizerExt** - **ISAM2Ext** @@ -58,16 +58,18 @@ All the following optimizers were derived from the implementations in GTSAM ## Nearest Neighbor Search - **KdTree** - Standard KdTree-based nearest neighbor search using [nanoflann](https://github.com/jlblancoc/nanoflann) -- **iVox** - Incremental voxel-based nearest neighbor search [[11]](#IVOX) + KdTree with parallel tree construction. Derived from [nanoflann](https://github.com/jlblancoc/nanoflann). +- **IncrementalVoxelMap** + Incremental voxel-based nearest neighbor search (iVox) [[11]](#IVOX). +- **IncrementalCovarianceVoxelMap** + Incremental voxelmap with online normal and covariance estimation. ## Continuous-Time Trajectory (Under development) - **B-Spline** - Cubic B-Spline-based interpolation and linear acceleration and angular velocity expressions [[12]](#BSPLINE_D) + Cubic B-Spline-based interpolation and linear acceleration and angular velocity expressions [[12]](#BSPLINE_D). - **ContinuousTrajectory** - Cubic B-Spline-based continuous trajectory representation for offline batch optimization + Cubic B-Spline-based continuous trajectory representation for offline batch optimization. ## Installation @@ -75,7 +77,10 @@ All the following optimizers were derived from the implementations in GTSAM ```bash # Install gtsam git clone https://github.com/borglab/gtsam -mkdir gtsam/build && cd gtsam/build +cd gtsam +git checkout 4.2a9 + +mkdir build && cd build cmake .. \ -DGTSAM_BUILD_EXAMPLES_ALWAYS=OFF \ -DGTSAM_BUILD_TESTS=OFF \ @@ -94,9 +99,9 @@ cmake .. -DCMAKE_BUILD_TYPE=Release make -j$(nproc) sudo make install -## Build gtsam_ext -git clone https://github.com/koide3/gtsam_ext --recursive -mkdir gtsam_ext/build && cd gtsam/build +## Build gtsam_points +git clone https://github.com/koide3/gtsam_points +mkdir gtsam_points/build && cd gtsam_points/build cmake .. -DCMAKE_BUILD_TYPE=Release # Optional cmake arguments @@ -104,22 +109,32 @@ cmake .. -DCMAKE_BUILD_TYPE=Release # -DBUILD_DEMO=OFF \ # -DBUILD_TESTS=OFF \ # -DBUILD_WITH_CUDA=OFF \ -# -DBUILD_WITH_MARCH_NATIVE=OFF \ -# -DBUILD_WITH_SYSTEM_EIGEN=ON +# -DBUILD_WITH_MARCH_NATIVE=OFF make -j$(nproc) +sudo make install ``` ## Demo ```bash -cd gtsam_ext +cd gtsam_points ./build/demo_matching_cost_factors ./build/demo_bundle_adjustment ./build/demo_continuous_time ./build/demo_continuous_trajectory +./build/demo_colored_registration ``` +## Videos + +- [Multi-scan registration of 5 frames (= A graph with 10 registration factors)](https://youtu.be/HCXCWlx_VOM) +- [Bundle adjustment factor](https://youtu.be/tuDV0GCOZXg) +- [Continuous-time ICP factor](https://youtu.be/Xv2-qDlzQYM) +- [Colored ICP factor](https://youtu.be/xEQmiFV79LU) +- [Incremental voxel mapping and normal estimation](https://youtu.be/gDiKqQDc7yo) +- [SE3 BSpline interpolation](https://youtu.be/etAI8go3b8U) + ## License This library is released under the MIT license. diff --git a/cmake/FindGTSAM.cmake b/cmake/FindGTSAM.cmake index 0779a922..12b39e6f 100644 --- a/cmake/FindGTSAM.cmake +++ b/cmake/FindGTSAM.cmake @@ -22,5 +22,10 @@ if(GTSAM_LIB AND GTSAM_UNSTABLE_LIB AND TBB_LIB) set(GTSAM_LIBRARIES ${GTSAM_LIB} ${GTSAM_UNSTABLE_LIB} ${TBB_LIB} ${TBB_MALLOC_LIB}) endif() +add_library(GTSAM::GTSAM INTERFACE IMPORTED GLOBAL) +set_target_properties(GTSAM::GTSAM PROPERTIES + INTERFACE_INCLUDE_DIRECTORIES "${GTSAM_INCLUDE_DIRS}" + INTERFACE_LINK_LIBRARIES "${GTSAM_LIBRARIES}") + include(FindPackageHandleStandardArgs) find_package_handle_standard_args(GTSAM DEFAULT_MSG GTSAM_INCLUDE_DIRS GTSAM_LIBRARIES) diff --git a/cmake/FindIridescence.cmake b/cmake/FindIridescence.cmake deleted file mode 100644 index ab9ff69b..00000000 --- a/cmake/FindIridescence.cmake +++ /dev/null @@ -1,16 +0,0 @@ -find_path(Iridescence_INCLUDE_DIRS glk/drawable.hpp - HINTS /usr/local/include/iridescence /usr/include/iridescence - DOC "Iridescence include directories") - -find_library(Iridescence_LIBRARY NAMES iridescence - HINTS /usr/local/lib /usr/lib - DOC "Iridescence libraries") - - find_library(gl_imgui_LIBRARY NAMES gl_imgui - HINTS /usr/local/lib /usr/lib - DOC "Iridescence libraries") - -set(Iridescence_LIBRARIES ${Iridescence_LIBRARY} ${gl_imgui_LIBRARY}) - -include(FindPackageHandleStandardArgs) -find_package_handle_standard_args(Iridescence DEFAULT_MSG Iridescence_INCLUDE_DIRS Iridescence_LIBRARIES) diff --git a/cmake/gtsam_points-config.cmake.in b/cmake/gtsam_points-config.cmake.in new file mode 100644 index 00000000..dcadead6 --- /dev/null +++ b/cmake/gtsam_points-config.cmake.in @@ -0,0 +1,24 @@ +# Config file for the gtsam_points package +# +# Usage from an external project: +# +# find_package(gtsam_points REQUIRED) +# target_link_libraries(MY_TARGET_NAME gtsam_points::gtsam_points) +# +@PACKAGE_INIT@ + +include_guard() + +set(BUILD_WITH_CUDA @BUILD_WITH_CUDA@) + +include(CMakeFindDependencyMacro) +find_dependency(Eigen3 REQUIRED) +find_dependency(GTSAM REQUIRED) +find_dependency(OpenMP REQUIRED) +find_dependency(Boost REQUIRED COMPONENTS filesystem) + +if(BUILD_WITH_CUDA) + find_dependency(CUDAToolkit REQUIRED) +endif() + +include("${CMAKE_CURRENT_LIST_DIR}/gtsam_points-targets.cmake") \ No newline at end of file diff --git a/docker/focal_cuda10.2/Dockerfile b/docker/focal_cuda10.2/Dockerfile deleted file mode 100644 index c3d88580..00000000 --- a/docker/focal_cuda10.2/Dockerfile +++ /dev/null @@ -1,50 +0,0 @@ -FROM nvidia/cuda:10.2-devel-ubuntu18.04 - -ENV DEBIAN_FRONTEND=noninteractive - -RUN apt-get update \ - && apt-get upgrade -y \ - # add gtsam respository - && apt-get install --no-install-recommends -y \ - software-properties-common \ - && add-apt-repository ppa:borglab/gtsam-develop \ - # install dependencies - && apt-get install --no-install-recommends -y \ - wget nano build-essential \ - git cmake libeigen3-dev ca-certificates \ - libtbb-dev libboost-all-dev libgtest-dev libmetis-dev \ - libgtsam-dev libgtsam-unstable-dev \ - libglm-dev libglfw3-dev libpng-dev libjpeg-dev \ - # install the latest cmake - && apt-get purge -y cmake \ - && wget -O - https://apt.kitware.com/keys/kitware-archive-latest.asc 2>/dev/null | gpg --dearmor - | tee /usr/share/keyrings/kitware-archive-keyring.gpg >/dev/null \ - && echo 'deb [signed-by=/usr/share/keyrings/kitware-archive-keyring.gpg] https://apt.kitware.com/ubuntu/ bionic main' | tee /etc/apt/sources.list.d/kitware.list >/dev/null \ - && apt-get update \ - && apt-get install --no-install-recommends -y cmake \ - # clean - && apt-get clean \ - && rm -rf /var/lib/apt/lists/* - -WORKDIR /root -RUN git clone https://github.com/koide3/iridescence.git --recursive -WORKDIR /root/iridescence/build -RUN cmake .. -RUN make -j$(nproc) && make install - -WORKDIR /root -RUN wget https://github.com/NVIDIA/cub/archive/refs/tags/1.8.0.tar.gz -RUN tar xzvf 1.8.0.tar.gz -RUN mv cub-1.8.0/cub /usr/local/cuda/include/ - -COPY . /root/gtsam_ext -WORKDIR /root/gtsam_ext/build -RUN rm -rf * -RUN cmake .. \ - -DBUILD_DEMO=ON \ - -DBUILD_TESTS=ON \ - -DBUILD_WITH_CUDA=ON \ - -DBUILD_EXAMPLE=ON \ - -DCMAKE_BUILD_TYPE=Release -RUN make -j$(nproc) - -CMD ["bash"] diff --git a/docker/focal_cuda11.2/Dockerfile b/docker/focal_cuda11.2/Dockerfile deleted file mode 100644 index 17429b89..00000000 --- a/docker/focal_cuda11.2/Dockerfile +++ /dev/null @@ -1,14 +0,0 @@ -FROM koide3/gtsam_docker:focal_cuda11.2 - -COPY . /root/gtsam_ext -WORKDIR /root/gtsam_ext/build -RUN rm -rf * -RUN cmake .. \ - -DBUILD_DEMO=ON \ - -DBUILD_TESTS=ON \ - -DBUILD_WITH_CUDA=ON \ - -DBUILD_EXAMPLE=ON \ - -DCMAKE_BUILD_TYPE=Release -RUN make -j$(nproc) - -CMD ["bash"] diff --git a/docker/jammy/Dockerfile b/docker/jammy/Dockerfile deleted file mode 100644 index 67ac4f4a..00000000 --- a/docker/jammy/Dockerfile +++ /dev/null @@ -1,15 +0,0 @@ -FROM koide3/gtsam_docker:jammy - -COPY . /root/gtsam_ext -WORKDIR /root/gtsam_ext/build -RUN rm -rf * -RUN cmake .. \ - -DBUILD_DEMO=ON \ - -DBUILD_TESTS=ON \ - -DBUILD_EXAMPLE=ON \ - -DBUILD_WITH_CUDA=OFF \ - -DCMAKE_BUILD_TYPE=Release -RUN make -j$(nproc) -RUN make test - -CMD ["bash"] diff --git a/docker/jammy_cuda11.8_llvm/Dockerfile b/docker/jammy_cuda11.8_llvm/Dockerfile deleted file mode 100644 index 4b70a7c9..00000000 --- a/docker/jammy_cuda11.8_llvm/Dockerfile +++ /dev/null @@ -1,20 +0,0 @@ -FROM koide3/gtsam_docker:jammy_cuda11.8 - -RUN update-alternatives --install /usr/bin/ld ld /usr/bin/ld.lld 50 - -# resolve omp_is_initial_device-related errors -RUN sed -i '496{s/^/\/\//}' /usr/lib/llvm-14/lib/clang/14.0.0/include/omp.h - -COPY . /root/gtsam_ext -WORKDIR /root/gtsam_ext/build -RUN rm -rf * -RUN CC=clang CXX=clang++ \ - cmake .. \ - -DBUILD_DEMO=ON \ - -DBUILD_TESTS=ON \ - -DBUILD_WITH_CUDA=ON \ - -DBUILD_EXAMPLE=ON \ - -DCMAKE_BUILD_TYPE=Release -RUN make -j$(nproc) - -CMD ["bash"] diff --git a/docker/jammy_llvm/Dockerfile b/docker/jammy_llvm/Dockerfile deleted file mode 100644 index 9da87240..00000000 --- a/docker/jammy_llvm/Dockerfile +++ /dev/null @@ -1,18 +0,0 @@ -FROM koide3/gtsam_docker:jammy - -RUN update-alternatives --install /usr/bin/ld ld /usr/bin/ld.lld 50 - -COPY . /root/gtsam_ext -WORKDIR /root/gtsam_ext/build -RUN rm -rf * -RUN CC=clang CXX=clang++ \ - cmake .. \ - -DBUILD_DEMO=ON \ - -DBUILD_TESTS=ON \ - -DBUILD_EXAMPLE=ON \ - -DBUILD_WITH_CUDA=OFF \ - -DCMAKE_BUILD_TYPE=Release -RUN make -j$(nproc) -RUN make test - -CMD ["bash"] diff --git a/docker/focal/Dockerfile b/docker/ubuntu/Dockerfile.gcc similarity index 62% rename from docker/focal/Dockerfile rename to docker/ubuntu/Dockerfile.gcc index 7a8e4b7b..2d283055 100644 --- a/docker/focal/Dockerfile +++ b/docker/ubuntu/Dockerfile.gcc @@ -1,7 +1,9 @@ -FROM koide3/gtsam_docker:focal +ARG BASE_IMAGE=koide3/gtsam_docker:jammy -COPY . /root/gtsam_ext -WORKDIR /root/gtsam_ext/build +FROM ${BASE_IMAGE} + +COPY . /root/gtsam_points +WORKDIR /root/gtsam_points/build RUN rm -rf * RUN cmake .. \ -DBUILD_DEMO=ON \ diff --git a/docker/jammy_cuda11.8/Dockerfile b/docker/ubuntu/Dockerfile.gcc.cuda similarity index 53% rename from docker/jammy_cuda11.8/Dockerfile rename to docker/ubuntu/Dockerfile.gcc.cuda index a773c8a7..912a7aad 100644 --- a/docker/jammy_cuda11.8/Dockerfile +++ b/docker/ubuntu/Dockerfile.gcc.cuda @@ -1,13 +1,16 @@ -FROM koide3/gtsam_docker:jammy_cuda11.8 +ARG BASE_IMAGE=koide3/gtsam_docker:jammy -COPY . /root/gtsam_ext -WORKDIR /root/gtsam_ext/build +FROM ${BASE_IMAGE} + +COPY . /root/gtsam_points +WORKDIR /root/gtsam_points/build RUN rm -rf * RUN cmake .. \ -DBUILD_DEMO=ON \ -DBUILD_TESTS=ON \ - -DBUILD_WITH_CUDA=ON \ -DBUILD_EXAMPLE=ON \ + -DBUILD_WITH_CUDA=ON \ + -DBUILD_WITH_CUDA_MULTIARCH=ON \ -DCMAKE_BUILD_TYPE=Release RUN make -j$(nproc) diff --git a/docker/focal_llvm/Dockerfile b/docker/ubuntu/Dockerfile.llvm similarity index 70% rename from docker/focal_llvm/Dockerfile rename to docker/ubuntu/Dockerfile.llvm index c7bfbcae..21722db1 100644 --- a/docker/focal_llvm/Dockerfile +++ b/docker/ubuntu/Dockerfile.llvm @@ -1,9 +1,11 @@ -FROM koide3/gtsam_docker:focal +ARG BASE_IMAGE=koide3/gtsam_docker:jammy + +FROM ${BASE_IMAGE} RUN update-alternatives --install /usr/bin/ld ld /usr/bin/ld.lld 50 -COPY . /root/gtsam_ext -WORKDIR /root/gtsam_ext/build +COPY . /root/gtsam_points +WORKDIR /root/gtsam_points/build RUN rm -rf * RUN CC=clang CXX=clang++ \ cmake .. \ diff --git a/docker/focal_cuda11.2_llvm/Dockerfile b/docker/ubuntu/Dockerfile.llvm.cuda similarity index 63% rename from docker/focal_cuda11.2_llvm/Dockerfile rename to docker/ubuntu/Dockerfile.llvm.cuda index 5b93ae4c..18ee622d 100644 --- a/docker/focal_cuda11.2_llvm/Dockerfile +++ b/docker/ubuntu/Dockerfile.llvm.cuda @@ -1,9 +1,11 @@ -FROM koide3/gtsam_docker:focal_cuda11.2 +ARG BASE_IMAGE=koide3/gtsam_docker:jammy + +FROM ${BASE_IMAGE} RUN update-alternatives --install /usr/bin/ld ld /usr/bin/ld.lld 50 -COPY . /root/gtsam_ext -WORKDIR /root/gtsam_ext/build +COPY . /root/gtsam_points +WORKDIR /root/gtsam_points/build RUN rm -rf * RUN CC=clang CXX=clang++ \ cmake .. \ @@ -11,6 +13,7 @@ RUN CC=clang CXX=clang++ \ -DBUILD_TESTS=ON \ -DBUILD_EXAMPLE=ON \ -DBUILD_WITH_CUDA=ON \ + -DBUILD_WITH_CUDA_MULTIARCH=ON \ -DCMAKE_BUILD_TYPE=Release RUN make -j$(nproc) diff --git a/docs/Doxyfile b/docs/Doxyfile index ed87550a..1e219094 100644 --- a/docs/Doxyfile +++ b/docs/Doxyfile @@ -32,7 +32,7 @@ DOXYFILE_ENCODING = UTF-8 # title of most generated pages and in a few other places. # The default value is: My Project. -PROJECT_NAME = "gtsam_ext" +PROJECT_NAME = "gtsam_points" # The PROJECT_NUMBER tag can be used to enter a project or revision number. This # could be handy for archiving the generated documentation or if some version @@ -917,7 +917,7 @@ RECURSIVE = YES # Note that relative paths are relative to the directory from which doxygen is # run. -EXCLUDE = ../include/gtsam_ext/factors/experimental ../include/gtsam_ext/optimizers +EXCLUDE = ../include/gtsam_points/factors/experimental ../include/gtsam_points/optimizers # The EXCLUDE_SYMLINKS tag can be used to select whether or not files or # directories that are symbolic links (a Unix file system feature) are excluded diff --git a/include/gtsam_ext/ann/ivox.hpp b/include/gtsam_ext/ann/ivox.hpp deleted file mode 100644 index 7ee06310..00000000 --- a/include/gtsam_ext/ann/ivox.hpp +++ /dev/null @@ -1,181 +0,0 @@ -// SPDX-License-Identifier: MIT -// Copyright (c) 2021 Kenji Koide (k.koide@aist.go.jp) - -#pragma once - -#include -#include -#include -#include - -#include - -#include -#include -#include -#include - -namespace gtsam_ext { - -/** - * @brief Container to hold points in a voxel - */ -struct LinearContainer { -public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW - - using Ptr = std::shared_ptr; - using ConstPtr = std::shared_ptr; - - LinearContainer(const int lru_count); - ~LinearContainer(); - - int size() const { return points.size(); } - void insert(const Eigen::Vector4d& point, const double insertion_dist_sq_thresh); - void insert(const Frame& frame, const int i, const double insertion_dist_sq_thresh); - -public: - mutable std::atomic_int last_lru_count; - int serial_id; - - std::vector> points; - std::vector> normals; - std::vector> covs; - std::vector intensities; -}; - -/** - * @brief Voxel-based incremental nearest neighbor search - * Bai et al., "Faster-LIO: Lightweight Tightly Coupled Lidar-Inertial Odometry Using Parallel Sparse Incremental Voxels", IEEE RA-L, 2022 - * @note Only the linear iVox is implemented - * @note To the compatibility with other nearest neighbor search methods, this implementation returns indices that encode the voxel and point IDs. - * The first ```point_id_bits``` (e.g., 32) bits of a point index represent the point ID, and the rest ```voxel_id_bits``` (e.g., 32) bits - * represent the voxel ID that contains the point. - */ -struct iVox : public NearestNeighborSearch { -public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW - - using Ptr = std::shared_ptr; - using ConstPtr = std::shared_ptr; - - /** - * @brief Construct a new iVox - * - * @param voxel_resolution Voxel resolution - * @param insertion_dist_thresh Minimum distance between points - * @param lru_thresh LRC caching threshold - */ - iVox(const double voxel_resolution = 0.5, const double insertion_dist_thresh = 0.05, const int lru_thresh = 10); - virtual ~iVox() override; - - /** - * @brief Insert points and all available attributes into the iVox - * @param frame Input frame - */ - virtual void insert(const Frame& frame); - - /** - * @brief Find the closest point - * @param pt Query point - * @param k_indices Index output - * @param k_sq_dists Distance output - * @return Number of found neighbors (0 or 1) - */ - size_t nearest_neighbor_search(const double* pt, size_t* k_indices, double* k_sq_dists) const; - - /** - * @brief Find k-nearest neighbors - * @note Indices encode voxel and point IDs of neighbor points - * First "voxel_id_bits" of an index indicates a sequential voxel ID - * Last "point_id_bits" of an index indicates a point ID in the voxel - * @param pt Query point - * @param k Number of neighbors - * @param k_indices Indices output - * @param k_sq_dists Distances output - * @return Number of found neighbors - */ - virtual size_t knn_search(const double* pt, size_t k, size_t* k_indices, double* k_sq_dists) const override; - - // Parameter setters - void set_voxel_resolution(const double resolution) { voxel_resolution = resolution; } ///< Voxel resolution - void set_insertion_dist_thresh(const double dist) { insertion_dist_sq_thresh = dist * dist; } ///< Minimum distance between points in a cell - void set_lru_cycle(const int lru_cycle) { this->lru_cycle = lru_cycle; } ///< LRU clearing cycle - void set_lru_thresh(const int lru_thresh) { this->lru_thresh = lru_thresh; } ///< LRU cache threshold (larger keeps more voxels) - void set_neighbor_voxel_mode(const int mode) { offsets = neighbor_offsets(mode); } ///< Neighboring voxel search mode (1/7/19/27) - - // Attribute check - bool has_points() const { return points_available; } ///< Check if the iVox has points - bool has_normals() const { return normals_available; } ///< Check if the iVox has normals - bool has_covs() const { return covs_available; } ///< Check if the iVox has covariances - bool has_intensities() const { return intensities_available; } ///< Check if the iVox has intensities - - // Point attribute accessors - const Eigen::Vector4d& point(const size_t i) const; ///< Get a point - const Eigen::Vector4d& normal(const size_t i) const; ///< Get the normal of a point - const Eigen::Matrix4d& cov(const size_t i) const; ///< Get the covariance of a point - double intensity(const size_t i) const; ///< Get the intensity of a point - - /// Number of voxels - int num_voxels() const { return voxelmap.size(); } - - /// Extract all points in iVox - virtual std::vector> voxel_points() const; - virtual std::vector> voxel_normals() const; - virtual std::vector> voxel_covs() const; - -protected: - inline size_t calc_index(const size_t voxel_id, const size_t point_id) const { return (voxel_id << point_id_bits) | point_id; } - inline size_t voxel_id(const size_t i) const { return i >> point_id_bits; } ///< Extract the point ID from an index - inline size_t point_id(const size_t i) const { return i & ((1ul << point_id_bits) - 1); } ///< Extract the voxel ID from an index - - const Eigen::Vector3i voxel_coord(const Eigen::Vector4d& point) const; - std::vector> neighbor_offsets(const int neighbor_voxel_mode) const; - -protected: - static constexpr int point_id_bits = 32; ///< Use the first 32 bits of point index to represent point ID - static constexpr int voxel_id_bits = 64 - point_id_bits; ///< Use the rest bits to represent voxel ID - static_assert(sizeof(size_t) == 8, "size_t is not 8 bytes!! point_id_bits and voxel_id_bits should be tuned accordingly!!"); - - bool points_available; - bool normals_available; - bool covs_available; - bool intensities_available; - - double voxel_resolution; ///< Voxel resolution - double insertion_dist_sq_thresh; ///< Minimum distance between points in a voxel - int lru_cycle; ///< LRU clearing check cycle - int lru_thresh; ///< LRU caching threshold - std::vector> offsets; ///< Neighbor voxel offsets - - int lru_count; ///< Counter to manage LRU voxel deletion - - using VoxelMap = std::unordered_map< - Eigen::Vector3i, - LinearContainer::Ptr, - XORVector3iHash, - std::equal_to, - Eigen::aligned_allocator>>; - - VoxelMap voxelmap; ///< Voxelmap - std::vector voxels; ///< Flattened voxelmap for linear indexing -}; - -namespace frame { - -template <> -struct traits { - static bool has_points(const iVox& ivox) { return ivox.has_points(); } - static bool has_normals(const iVox& ivox) { return ivox.has_normals(); } - static bool has_covs(const iVox& ivox) { return ivox.has_covs(); } - static bool has_intensities(const iVox& ivox) { return ivox.has_intensities(); } - - static const Eigen::Vector4d& point(const iVox& ivox, size_t i) { return ivox.point(i); } - static const Eigen::Vector4d& normal(const iVox& ivox, size_t i) { return ivox.normal(i); } - static const Eigen::Matrix4d& cov(const iVox& ivox, size_t i) { return ivox.cov(i); } - static double intensity(const iVox& ivox, size_t i) { return ivox.intensity(i); } -}; - -} // namespace frame - -} // namespace gtsam_ext \ No newline at end of file diff --git a/include/gtsam_ext/ann/ivox_covariance_estimation.hpp b/include/gtsam_ext/ann/ivox_covariance_estimation.hpp deleted file mode 100644 index 28f0ab50..00000000 --- a/include/gtsam_ext/ann/ivox_covariance_estimation.hpp +++ /dev/null @@ -1,55 +0,0 @@ -// SPDX-License-Identifier: MIT -// Copyright (c) 2021 Kenji Koide (k.koide@aist.go.jp) - -#pragma once - -#include - -namespace gtsam_ext { - -class iVoxCovarianceEstimation : public iVox { -public: - iVoxCovarianceEstimation( - const double voxel_resolution, - const double min_points_dist, - const int lru_thresh, - const int k_neighbors, - const int num_threads = 1); - ~iVoxCovarianceEstimation() override; - - virtual void insert(const Frame& frame) override; - - const Eigen::Vector4d& normal(const size_t i) const; - const Eigen::Matrix4d& cov(const size_t i) const; - - virtual std::vector> voxel_normals() const override; - virtual std::vector> voxel_covs() const override; - -private: - std::pair - estimate_cov_and_normal(const Eigen::Vector4d& point, const int num_found, const size_t* k_indices, const double* k_sq_dists) const; - -private: - int k_neighbors; - int num_threads; - - VoxelMap covmap; - std::vector covs; -}; - -namespace frame { - -template <> -struct traits { - static bool has_points(const iVoxCovarianceEstimation& ivox) { return ivox.has_points(); } - static bool has_normals(const iVoxCovarianceEstimation& ivox) { return true; } - static bool has_covs(const iVoxCovarianceEstimation& ivox) { return true; } - - static const Eigen::Vector4d& point(const iVoxCovarianceEstimation& ivox, size_t i) { return ivox.point(i); } - static const Eigen::Vector4d& normal(const iVoxCovarianceEstimation& ivox, size_t i) { return ivox.normal(i); } - static const Eigen::Matrix4d& cov(const iVoxCovarianceEstimation& ivox, size_t i) { return ivox.cov(i); } -}; - -} // namespace frame - -} // namespace gtsam_ext \ No newline at end of file diff --git a/include/gtsam_ext/ann/kdtree.hpp b/include/gtsam_ext/ann/kdtree.hpp deleted file mode 100644 index 1a03537d..00000000 --- a/include/gtsam_ext/ann/kdtree.hpp +++ /dev/null @@ -1,55 +0,0 @@ -// SPDX-License-Identifier: MIT -// Copyright (c) 2021 Kenji Koide (k.koide@aist.go.jp) - -#pragma once - -#include -#include -#include - -#include - -// forward declaration -namespace nanoflann { - -template -class L2_Simple_Adaptor; - -template -class KDTreeSingleIndexAdaptor; - -} // namespace nanoflann - -namespace gtsam_ext { - -/** - * @brief KdTree-based nearest neighbor search - * @note This is just a thin wrapper for nanoflann - */ -struct KdTree : public NearestNeighborSearch { -public: - using Index = nanoflann::KDTreeSingleIndexAdaptor, KdTree, 3, size_t>; - - KdTree(const Eigen::Vector4d* points, int num_points); - virtual ~KdTree() override; - - inline size_t kdtree_get_point_count() const { return num_points; } - inline double kdtree_get_pt(const size_t idx, const size_t dim) const { return points[idx][dim]; } - - template - bool kdtree_get_bbox(BBox&) const { - return false; - } - - virtual size_t knn_search(const double* pt, size_t k, size_t* k_indices, double* k_sq_dists) const override; - -public: - const int num_points; - const Eigen::Vector4d* points; - - double search_eps; - - std::unique_ptr index; -}; - -} // namespace gtsam_ext diff --git a/include/gtsam_ext/ann/kdtree2.hpp b/include/gtsam_ext/ann/kdtree2.hpp deleted file mode 100644 index c0319792..00000000 --- a/include/gtsam_ext/ann/kdtree2.hpp +++ /dev/null @@ -1,67 +0,0 @@ -// SPDX-License-Identifier: MIT -// Copyright (c) 2021 Kenji Koide (k.koide@aist.go.jp) - -#pragma once - -#include -#include -#include - -#include -#include -#include - -namespace gtsam_ext { - -/** - * @brief KdTree-based nearest neighbor search - * @note This is nanoflann wrapper with traits-based point cloud interface. - */ -template -struct KdTree2 : public NearestNeighborSearch { -public: - using Index = nanoflann::KDTreeSingleIndexAdaptor, double>, KdTree2, 3, size_t>; - - KdTree2(const std::shared_ptr& frame) - : frame(frame), - search_eps(-1.0), - index(new Index(3, *this, nanoflann::KDTreeSingleIndexAdaptorParams(10))) { - if (frame::size(*frame) == 0) { - std::cerr << "error: empty frame is given for KdTree2" << std::endl; - std::cerr << " : frame::size() may not be implemented" << std::endl; - } - - index->buildIndex(); - } - virtual ~KdTree2() override {} - - inline size_t kdtree_get_point_count() const { return frame::size(*frame); } - inline double kdtree_get_pt(const size_t idx, const size_t dim) const { return frame::point(*frame, idx)[dim]; } - - template - bool kdtree_get_bbox(BBox&) const { - return false; - } - - virtual size_t knn_search(const double* pt, size_t k, size_t* k_indices, double* k_sq_dists) const override { - if (search_eps > 0.0) { - nanoflann::KNNResultSet result_set(k); - result_set.init(k_indices, k_sq_dists); - nanoflann::SearchParams search_params; - search_params.eps = search_eps; - index->findNeighbors(result_set, pt, search_params); - return result_set.size(); - } - - return index->knnSearch(pt, k, k_indices, k_sq_dists); - } - -public: - const std::shared_ptr frame; - - double search_eps; - - std::unique_ptr index; -}; - -} // namespace gtsam_ext diff --git a/include/gtsam_ext/ann/projective_search.hpp b/include/gtsam_ext/ann/projective_search.hpp deleted file mode 100644 index 2121759f..00000000 --- a/include/gtsam_ext/ann/projective_search.hpp +++ /dev/null @@ -1,125 +0,0 @@ -// SPDX-License-Identifier: MIT -// Copyright (c) 2021 Kenji Koide (k.koide@aist.go.jp) - -#pragma once - -#include -#include -#include -#include - -namespace gtsam_ext { - -struct OmniProjectiveSearchParams { - int rows; - int cols; - double v_angle_min; - double v_angle_max; -}; - -// WIP -struct OmniProjectiveSearch : public NearestNeighborSearch { -public: - OmniProjectiveSearch(const Eigen::Vector4d* points, int num_points) : OmniProjectiveSearch(points, num_points, auto_param(points, num_points)) {} - - OmniProjectiveSearch(const Eigen::Vector4d* points, int num_points, const OmniProjectiveSearchParams& params) - : points(points), - num_points(num_points), - params(params), - grid(params.rows * params.cols) { - // - for (int i = 0; i < num_points; i++) { - Eigen::Vector2i index = project(points[i]); - if (index.y() < 0 || index.y() > params.rows - 1) { - continue; - } - - auto& cell = lookup(index); - cell.push_back(i); - } - } - - virtual ~OmniProjectiveSearch() override {} - - bool compare(const std::pair& lhs, const std::pair& rhs) { return lhs.second > rhs.second; } - - virtual size_t knn_search(const double* pt_, size_t k, size_t* k_indices, double* k_sq_dists) const override { - Eigen::Map pt(pt_); - Eigen::Vector2i center = project(pt); - - const int half_window = 1; - const int y_min = std::max(0, center.y() - half_window); - const int y_max = std::max(params.rows - 1, center.y() + half_window); - - const auto comp = [](const std::pair& lhs, const std::pair& rhs) { return lhs.second > rhs.second; }; - - std::priority_queue, decltype(comp)> queue(comp); - for (int x = center.x() - half_window; x <= center.x() + half_window; x++) { - for (int y = y_min; y <= y_max; y++) { - const auto& cell = lookup(Eigen::Vector2i(x, y)); - for (const auto index : cell) { - queue.push(std::make_pair(index, (points[index] - pt).squaredNorm())); - if (queue.size() > k) { - queue.pop(); - } - } - } - } - - /* - int num_found = std::min(candidates.size(), k); - - for (int i = 0; i < num_found; i++) { - k_indices[i] = candidates[i].first; - k_sq_dists[i] = candidates[i].second; - } - - return num_found; - */ - return 0; - }; - - Eigen::Vector2i project(const Eigen::Vector4d& pt) const { - double v_angle = std::atan2(pt.z(), pt.head<2>().norm()); - double h_angle = std::atan2(pt.y(), pt.x()); - - double v_p = (v_angle - params.v_angle_min) / (params.v_angle_max - params.v_angle_min); - double h_p = (h_angle + M_PI) / (2.0 * M_PI); - - return Eigen::Array2d(h_p * params.cols, v_p * params.rows).floor().cast(); - } - - std::vector& lookup(const Eigen::Vector2i& index) { - const int x = index.x() >= 0 ? index.x() % params.cols : index.x() + params.cols; - const int y = std::max(0, std::min(params.rows - 1, index.y())); - return grid[y * params.cols + x]; - } - - const std::vector& lookup(const Eigen::Vector2i& index) const { - const int x = index.x() >= 0 ? index.x() % params.cols : index.x() + params.cols; - const int y = std::max(0, std::min(params.rows - 1, index.y())); - return grid[y * params.cols + x]; - } - -private: - OmniProjectiveSearchParams auto_param(const Eigen::Vector4d* points, int num_points) const { - std::vector v_angles(num_points); - for (int i = 0; i < num_points; i++) { - double v_angle = std::atan2(points[i].z(), points[i].head<2>().norm()); - v_angles[i] = v_angle; - } - std::sort(v_angles.begin(), v_angles.end()); - const double v_angle_min = v_angles[v_angles.size() * 0.025]; - const double v_angle_max = v_angles[v_angles.size() * 0.975]; - return OmniProjectiveSearchParams{64, 1024, v_angle_min, v_angle_max}; - } - -private: - const Eigen::Vector4d* points; - const int num_points; - - const OmniProjectiveSearchParams params; - - std::vector> grid; -}; -} // namespace gtsam_ext \ No newline at end of file diff --git a/include/gtsam_ext/factors/loose_prior_factor.hpp b/include/gtsam_ext/factors/loose_prior_factor.hpp deleted file mode 100644 index fceebfe4..00000000 --- a/include/gtsam_ext/factors/loose_prior_factor.hpp +++ /dev/null @@ -1,56 +0,0 @@ -// SPDX-License-Identifier: MIT -// Copyright (c) 2021 Kenji Koide (k.koide@aist.go.jp) - -#pragma once - -#include -#include - -namespace gtsam_ext { - -// This factor "loosely" fixes a variable at the current estimate point -// This can be used to fix the gauge freedom (there should be a better way though...) -// Note: Consider using LinearDampingFactor instead of this -template -class LoosePriorFactor : public gtsam::NonlinearFactor { -public: - using T = VALUE; - using shared_ptr = boost::shared_ptr>; - - LoosePriorFactor() {} - ~LoosePriorFactor() override {} - - LoosePriorFactor(gtsam::Key key, const T& prior, const gtsam::SharedNoiseModel& model = nullptr) - : gtsam::NonlinearFactor(gtsam::KeyVector{key}), - noise_model_(model), - prior_(new gtsam::PriorFactor(key, prior, model)) {} - - virtual size_t dim() const override { return prior_->dim(); } - - virtual double error(const gtsam::Values& values) const override { return prior_->error(values); } - - virtual boost::shared_ptr linearize(const gtsam::Values& values) const override { - T current_value = values.at(keys()[0]); - if (!prior_ || prior_->error(values) > 1e-3) { - prior_.reset(new gtsam::PriorFactor(keys()[0], current_value, noise_model_)); - } - - return prior_->linearize(values); - } - -private: - /** Serialization function */ - friend class boost::serialization::access; - template - void serialize(ARCHIVE& ar, const unsigned int /*version*/) { - namespace bs = ::boost::serialization; - ar& boost::serialization::make_nvp("NonlinearFactor", boost::serialization::base_object(*this)); - ar& BOOST_SERIALIZATION_NVP(noise_model_); - } - -private: - gtsam::SharedNoiseModel noise_model_; - mutable boost::shared_ptr> prior_; -}; - -} // namespace gtsam \ No newline at end of file diff --git a/include/gtsam_ext/types/frame.hpp b/include/gtsam_ext/types/frame.hpp deleted file mode 100644 index df3e9d63..00000000 --- a/include/gtsam_ext/types/frame.hpp +++ /dev/null @@ -1,152 +0,0 @@ -// SPDX-License-Identifier: MIT -// Copyright (c) 2021 Kenji Koide (k.koide@aist.go.jp) - -#pragma once - -#include -#include -#include -#include -#include -#include - -namespace gtsam_ext { - -class GaussianVoxelMapCPU; -class GaussianVoxelMapGPU; - -/** - * @brief Standard Frame class that holds only pointers to point attributes - * @note If you don't want to manage the lifetime of point data by yourself, use gtsam_ext::FrameCPU. - */ -struct Frame { -public: - using Ptr = std::shared_ptr; - using ConstPtr = std::shared_ptr; - - Frame() - : num_points(0), - times(nullptr), - points(nullptr), - normals(nullptr), - covs(nullptr), - intensities(nullptr), - times_gpu(nullptr), - points_gpu(nullptr), - normals_gpu(nullptr), - covs_gpu(nullptr), - intensities_gpu(nullptr) {} - virtual ~Frame() {} - - /// Number of points - size_t size() const { return num_points; } - - bool has_times() const; ///< Check if the frame has per-point timestamps - bool has_points() const; ///< Check if the frame has points - bool has_normals() const; ///< Check if the frame has point normals - bool has_covs() const; ///< Check if the frame has point covariances - bool has_intensities() const; ///< Check if the frame has point intensities - - bool check_times() const; ///< Warn if the frame doesn't have times - bool check_points() const; ///< Warn if the frame doesn't have points - bool check_normals() const; ///< Warn if the frame doesn't have normals - bool check_covs() const; ///< Warn if the frame doesn't have covs - bool check_intensities() const; ///< Warn if the frame doesn't have intensities - - bool has_times_gpu() const; ///< Check if the frame has per-point timestamps on GPU - bool has_points_gpu() const; ///< Check if the frame has points on GPU - bool has_normals_gpu() const; ///< Check if the frame has point normals on GPU - bool has_covs_gpu() const; ///< Check if the frame has point covariances on GPU - bool has_intensities_gpu() const; ///< Check if the frame has point intensities on GPU - - bool check_times_gpu() const; ///< Warn if the frame doesn't have times on GPU - bool check_points_gpu() const; ///< Warn if the frame doesn't have points on GPU - bool check_normals_gpu() const; ///< Warn if the frame doesn't have normals on GPU - bool check_covs_gpu() const; ///< Warn if the frame doesn't have covs on GPU - bool check_intensities_gpu() const; ///< Warn if the frame doesn't have intensities on GPU - - /** - * @brief Get the pointer to an aux attribute - * @param attrib Attribute name - * @return If the attribute exists, returns the pointer to it. Otherwise, returns nullptr. - */ - template - const T* aux_attribute(const std::string& attrib) const { - const auto found = aux_attributes.find(attrib); - if (found == aux_attributes.end()) { - std::cerr << "warning: attribute " << attrib << " not found!!" << std::endl; - return nullptr; - } - - if (sizeof(T) != found->second.first) { - std::cerr << "warning: attribute element size mismatch!! attrib:" << attrib << " size:" << found->second.first << " requested:" << sizeof(T) - << std::endl; - } - - return static_cast(found->second.second); - } - - /** - * @brief Save the frame data - * @param path Destination path - */ - void save(const std::string& path) const; - - /** - * @brief Save the frame data with a compact representation without unnecessary fields (e.g., the last element of homogeneous coordinates). - * @param path - */ - void save_compact(const std::string& path) const; - -public: - size_t num_points; ///< Number of points - - double* times; ///< Per-point timestamp w.r.t. the first point (should be sorted) - Eigen::Vector4d* points; ///< Point coordinates (x, y, z, 1) - Eigen::Vector4d* normals; ///< Point normals (nx, ny, nz, 0) - Eigen::Matrix4d* covs; ///< Point covariances cov(3, 3) = 0 - double* intensities; ///< Point intensities - - /// Aux attributes > - std::unordered_map> aux_attributes; - - float* times_gpu; ///< Per-point timestamp on GPU - Eigen::Vector3f* points_gpu; ///< Point coordinates on GPU - Eigen::Vector3f* normals_gpu; ///< Point normals on GPU - Eigen::Matrix3f* covs_gpu; ///< Point covariances on GPU - float* intensities_gpu; ///< Point intensities on GPU - - // Voxelmaps - std::shared_ptr voxels; ///< Voxelmap on CPU - std::shared_ptr voxels_gpu; ///< Voxelmap on GPU -}; -} // namespace gtsam_ext - -#ifndef DONT_DEFINE_FRAME_TRAITS -#include - -namespace gtsam_ext { -namespace frame { - -template <> -struct traits { - static int size(const Frame& frame) { return frame.size(); } - - static bool has_times(const Frame& frame) { return frame.has_times(); } - static bool has_points(const Frame& frame) { return frame.has_points(); } - static bool has_normals(const Frame& frame) { return frame.has_normals(); } - static bool has_covs(const Frame& frame) { return frame.has_covs(); } - static bool has_intensities(const Frame& frame) { return frame.has_intensities(); } - - static double time(const Frame& frame, size_t i) { return frame.times[i]; } - static const Eigen::Vector4d& point(const Frame& frame, size_t i) { return frame.points[i]; } - static const Eigen::Vector4d& normal(const Frame& frame, size_t i) { return frame.normals[i]; } - static const Eigen::Matrix4d& cov(const Frame& frame, size_t i) { return frame.covs[i]; } - static double intensity(const Frame& frame, size_t i) { return frame.intensities[i]; } - - static const Eigen::Vector4d* points_ptr(const Frame& frame) { return frame.points; } -}; - -} // namespace frame -} // namespace gtsam_ext -#endif diff --git a/include/gtsam_ext/types/frame_cpu.hpp b/include/gtsam_ext/types/frame_cpu.hpp deleted file mode 100644 index 7d9ce734..00000000 --- a/include/gtsam_ext/types/frame_cpu.hpp +++ /dev/null @@ -1,266 +0,0 @@ -// SPDX-License-Identifier: MIT -// Copyright (c) 2021 Kenji Koide (k.koide@aist.go.jp) - -#pragma once - -#include -#include -#include - -#include - -struct CUstream_st; - -namespace gtsam_ext { - -/** - * @brief Point cloud frame on CPU memory - */ -struct FrameCPU : public Frame { -public: - using Ptr = std::shared_ptr; - using ConstPtr = std::shared_ptr; - - /** - * @brief Constructor - * @param points Pointer to point data - * @param num_points Number of points - */ - template - FrameCPU(const Eigen::Matrix* points, int num_points); - - /** - * @brief Constructor - * @param points Points - */ - template - FrameCPU(const std::vector, Alloc>& points) : FrameCPU(points.data(), points.size()) {} - - /// deep copy constructor - FrameCPU(const Frame& frame); - - FrameCPU(); - ~FrameCPU(); - - template - void add_times(const T* times, int num_points); - template - void add_times(const std::vector& times) { - add_times(times.data(), times.size()); - } - - template - void add_points(const Eigen::Matrix* points, int num_points); - template - void add_points(const std::vector, Alloc>& points) { - add_points(points.data(), points.size()); - } - - template - void add_normals(const Eigen::Matrix* normals, int num_points); - template - void add_normals(const std::vector, Alloc>& normals) { - add_normals(normals.data(), normals.size()); - } - - template - void add_covs(const Eigen::Matrix* covs, int num_points); - template - void add_covs(const std::vector, Alloc>& covs) { - add_covs(covs.data(), covs.size()); - } - - template - void add_intensities(const T* intensities, int num_points); - template - void add_intensities(const std::vector& intensities) { - add_intensities(intensities.data(), intensities.size()); - } - - template - void add_aux_attribute(const std::string& attrib_name, const T* values, int num_points) { - auto attributes = std::make_shared>(values, values + num_points); - aux_attributes_storage[attrib_name] = attributes; - aux_attributes[attrib_name] = std::make_pair(sizeof(T), attributes->data()); - } - template - void add_aux_attribute(const std::string& attrib_name, const std::vector& values) { - add_aux_attribute(attrib_name, values.data(), values.size()); - } - - static FrameCPU::Ptr load(const std::string& path); - -public: - std::vector times_storage; - std::vector> points_storage; - std::vector> normals_storage; - std::vector> covs_storage; - std::vector intensities_storage; - - std::unordered_map> aux_attributes_storage; -}; - -/** - * @brief Sample points - * @param frame Input points - * @param indices Point indices - */ -FrameCPU::Ptr sample(const Frame::ConstPtr& frame, const std::vector& indices); - -/** - * @brief Naive random sampling - * @param frame Input points - * @param sampling_rate Random sampling rate in [0, 1] - * @param mt RNG - * @return Downsampled points - */ -FrameCPU::Ptr random_sampling(const Frame::ConstPtr& frame, const double sampling_rate, std::mt19937& mt); - -/** - * @brief Voxel grid downsampling - * @note This algorithm takes the average of point attributes (whatever it is) of each voxel - * - * @param frame Input points - * @param voxel_resolution Voxel resolution - */ -FrameCPU::Ptr voxelgrid_sampling(const Frame::ConstPtr& frame, const double voxel_resolution); - -/** - * @brief Voxel grid random sampling - * @note This algorithm randomly samples points such that the number of sampled points of each voxel becomes (more or less) the same. - * This algorithm avoids mixing point attributes (unlike the standard voxelgrid downsampling), and thus can provide spatially - * well-distributed point samples with several attributes (e.g., normals and covs). - * - * @param frame Input points - * @param voxel_resolution Voxel resolution - * @param sampling_rate Random sampling rate in [0, 1] - * @param mt RNG - * @return Downsampled points - */ -FrameCPU::Ptr randomgrid_sampling(const Frame::ConstPtr& frame, const double voxel_resolution, const double sampling_rate, std::mt19937& mt); - -/** - * @brief Extract points for which pred returns true - * @param frame Input points - * @param pred Predicate function that takes Eigen::Vector4d and returns bool - */ -template -FrameCPU::Ptr filter(const Frame::ConstPtr& frame, const Func& pred) { - std::vector indices; - indices.reserve(frame->size()); - for (int i = 0; i < frame->size(); i++) { - if (pred(frame->points[i])) { - indices.push_back(i); - } - } - return sample(frame, indices); -} - -/** - * @brief Extract points for which pred returns true - * @param frame Input points - * @param pred Predicate function that takes a point index and returns bool - */ -template -FrameCPU::Ptr filter_by_index(const Frame::ConstPtr& frame, const Func& pred) { - std::vector indices; - indices.reserve(frame->size()); - for (int i = 0; i < frame->size(); i++) { - if (pred(i)) { - indices.push_back(i); - } - } - return sample(frame, indices); -} - -/** - * @brief Sort points - * @param frame Input points - * @param pred Comparison function that takes two point indices (lhs and rhs) and returns true if lhs < rhs - */ -template -FrameCPU::Ptr sort(const Frame::ConstPtr& frame, const Compare& comp) { - std::vector indices(frame->size()); - std::iota(indices.begin(), indices.end(), 0); - std::sort(indices.begin(), indices.end(), comp); - return gtsam_ext::sample(frame, indices); -} - -/** - * @brief Sort points by time - * @param frame Input points - */ -FrameCPU::Ptr sort_by_time(const Frame::ConstPtr& frame); - -/** - * @brief Transform points, normals, and covariances - * - * @param frame Input points - * @param transformation Transformation - * @return Transformed points - */ -template -FrameCPU::Ptr transform(const Frame::ConstPtr& frame, const Eigen::Transform& transformation); - -/** - * @brief Transform points, normals, and covariances inplace - * @param frame [in/out] Points to be transformed - * @param transformation Transformation - */ -template -void transform_inplace(Frame::Ptr& frame, const Eigen::Transform& transformation); - -/** - * @brief Statistical outlier removal - * @param frame Input points - * @param neighbors Neighbor indices - * @param k Number of neighbors (neighbors.size() must be >= frame->size() * k) - * @param std_thresh Standard deviation multiplication threshold - * @return Inlier point indices - */ -std::vector find_inlier_points(const Frame::ConstPtr& frame, const std::vector& neighbors, const int k, const double std_thresh = 1.0); - -/** - * @brief Statistical outlier removal - * @param frame Input points - * @param neighbors Neighbor indices - * @param k Number of neighbors (neighbors.size() must be >= frame->size() * k) - * @param std_thresh Standard deviation multiplication threshold - * @return Filtered point cloud - */ -FrameCPU::Ptr remove_outliers(const Frame::ConstPtr& frame, const std::vector& neighbors, const int k, const double std_thresh = 1.0); - -/** - * @brief Statistical outlier removal - * @param frame Input points - * @param k Number of neighbors (neighbors.size() must be >= frame->size() * k) - * @param std_thresh Standard deviation multiplication threshold - * @return Filtered point cloud - */ -FrameCPU::Ptr remove_outliers(const Frame::ConstPtr& frame, const int k = 10, const double std_thresh = 1.0, const int num_threads = 1); - -/** - * @brief Merge a set of voxelized frames into one frame - * @note This function only merges points and covs and discard other point attributes. - * @param poses Poses of input frames - * @param frames Input frames - * @param downsample_resolution Downsampling resolution - * @return Merged frame - */ -Frame::Ptr merge_frames( - const std::vector>& poses, - const std::vector& frames, - double downsample_resolution); - -Frame::Ptr merge_frames_gpu( - const std::vector>& poses, - const std::vector& frames, - double downsample_resolution, - CUstream_st* stream = 0); - -Frame::Ptr merge_frames_auto( - const std::vector>& poses, - const std::vector& frames, - double downsample_resolution); - -} // namespace gtsam_ext diff --git a/include/gtsam_ext/types/gaussian_voxelmap.hpp b/include/gtsam_ext/types/gaussian_voxelmap.hpp deleted file mode 100644 index e1e831a0..00000000 --- a/include/gtsam_ext/types/gaussian_voxelmap.hpp +++ /dev/null @@ -1,28 +0,0 @@ -// SPDX-License-Identifier: MIT -// Copyright (c) 2021 Kenji Koide (k.koide@aist.go.jp) - -#pragma once - -#include - -namespace gtsam_ext { - -/** - * @brief Gaussian distribution voxelmap - */ -class GaussianVoxelMap { -public: - using Ptr = std::shared_ptr; - using ConstPtr = std::shared_ptr; - - GaussianVoxelMap() {} - virtual ~GaussianVoxelMap() {} - - /// Voxel resolution - virtual double voxel_resolution() const = 0; - - /// Insert a point cloud frame into the voxelmap - virtual void insert(const Frame& frame) = 0; -}; - -} // namespace gtsam_ext \ No newline at end of file diff --git a/include/gtsam_ext/types/gaussian_voxelmap_cpu.hpp b/include/gtsam_ext/types/gaussian_voxelmap_cpu.hpp deleted file mode 100644 index 98aea53a..00000000 --- a/include/gtsam_ext/types/gaussian_voxelmap_cpu.hpp +++ /dev/null @@ -1,119 +0,0 @@ -// SPDX-License-Identifier: MIT -// Copyright (c) 2021 Kenji Koide (k.koide@aist.go.jp) - -#pragma once - -#include -#include -#include - -#include -#include -#include - -namespace gtsam_ext { - -/** - * @brief Gaussian distribution voxel - */ -struct GaussianVoxel { -public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW - using Ptr = std::shared_ptr; - using ConstPtr = std::shared_ptr; - - GaussianVoxel(); - ~GaussianVoxel(); - - /** - * @brief Add a point to the voxel distribution. - * \note finalize() must be called before using calculated mean and cov. - */ - void append(const Eigen::Vector4d& mean_, const Eigen::Matrix4d& cov_); - - /** - * @brief Finalize the distribution parameters (mean and cov). - * \note You can add more points and re-finalize the distribution parameters over and over. - */ - void finalize(); - -public: - bool finalized; ///< If the mean and cov are finalized - mutable std::atomic_int last_lru_count; ///< LRU cache count - - int num_points; ///< Number of inserted points - Eigen::Vector4d mean; ///< Mean. If it is not finalized, it stores num_points * mean instead; - Eigen::Matrix4d cov; ///< Covariance. If it is not finalized, it stores num_points * cov instead; -}; - -/** - * @brief Gaussian distribution voxelmap - */ -class GaussianVoxelMapCPU : public GaussianVoxelMap { -public: - using Ptr = std::shared_ptr; - using ConstPtr = std::shared_ptr; - - /** - * @brief Constructor - * @param resolution Voxel resolution - */ - GaussianVoxelMapCPU(double resolution); - virtual ~GaussianVoxelMapCPU(); - - /// LRU clearing check cycle - void set_lru_cycle(const int cycle) { lru_cycle = cycle; } - - /// LRU cache threshold - void set_lru_thresh(const int thresh) { lru_thresh = thresh; } - - /// Voxel resolution - virtual double voxel_resolution() const override { return resolution; } - - /** - * @brief Insert a point cloud frame into the voxelmap - * @param frame Point cloud frame to be inserted - */ - virtual void insert(const Frame& frame) override; - - /** - * @brief Calculate the coordinates of a voxel that contains $x$. - * @return Calculated voxel coordinates - */ - Eigen::Vector3i voxel_coord(const Eigen::Vector4d& x) const; - - /** - * @brief Look up a voxel - * @param coord Voxel coordinates - */ - GaussianVoxel::Ptr lookup_voxel(const Eigen::Vector3i& coord) const; - - /** - * @brief Save the voxelmap - * @param path Destination path to save the voxelmap - */ - void save_compact(const std::string& path) const; - - /** - * @brief Save a voxelmap from a file - * @param path Path to a voxelmap file to be loaded - */ - static GaussianVoxelMapCPU::Ptr load(const std::string& path); - -public: - using VoxelMap = std::unordered_map< - Eigen::Vector3i, - GaussianVoxel::Ptr, - XORVector3iHash, - std::equal_to, - Eigen::aligned_allocator>>; - - int lru_count; ///< LRU count - int lru_cycle; ///< LRU check cycle - int lru_thresh; ///< LRU threshold. Voxels that are not observed longer than this are removed from the voxelmap. - - double resolution; ///< Voxel resolution - VoxelMap voxels; ///< Voxelmap -}; - -} // namespace gtsam_ext diff --git a/include/gtsam_ext/types/voxelized_frame_cpu.hpp b/include/gtsam_ext/types/voxelized_frame_cpu.hpp deleted file mode 100644 index 40adc3d4..00000000 --- a/include/gtsam_ext/types/voxelized_frame_cpu.hpp +++ /dev/null @@ -1,127 +0,0 @@ -// SPDX-License-Identifier: MIT -// Copyright (c) 2021 Kenji Koide (k.koide@aist.go.jp) - -#pragma once - -#include -#include - -struct CUstream_st; - -namespace gtsam_ext { - -struct VoxelizedFrameCPU : public FrameCPU { -public: - using Ptr = std::shared_ptr; - using ConstPtr = std::shared_ptr; - - template - VoxelizedFrameCPU(double voxel_resolution, const Eigen::Matrix* points, const Eigen::Matrix* covs, int num_points); - - template class Alloc> - VoxelizedFrameCPU( - double voxel_resolution, - const std::vector, Alloc>>& points, - const std::vector, Alloc>>& covs) - : VoxelizedFrameCPU(voxel_resolution, points.data(), covs.data(), points.size()) {} - - VoxelizedFrameCPU(double voxel_resolution, const Frame& frame); - - VoxelizedFrameCPU(); - ~VoxelizedFrameCPU(); - - void create_voxelmap(double voxel_resolution); -}; - -/** - * @brief Calculate the fraction of points fell in target's voxels - * @param target Target voxelized frame - * @param source Source frame - * @param delta T_target_source - * @return Overlap rate - */ -double overlap(const GaussianVoxelMap::ConstPtr& target, const Frame::ConstPtr& source, const Eigen::Isometry3d& delta); - -double overlap(const Frame::ConstPtr& target, const Frame::ConstPtr& source, const Eigen::Isometry3d& delta); - -/** - * @brief Calculate the fraction of points fell in targets' voxels - * @param target Set of target voxelized frames - * @param source Source frame - * @param delta Set of T_target_source - * @return Overlap rate - */ -double overlap( - const std::vector& targets, - const Frame::ConstPtr& source, - const std::vector>& deltas); - -double overlap( - const std::vector& targets, - const Frame::ConstPtr& source, - const std::vector>& deltas); - -/** - * @brief Calculate the fraction of points fell in target voxels on GPU - * @note Source points and target voxelmap must be pre-allocated on GPU. - * @param target Target voxelmap - * @param source Source frame - * @param delta_gpu T_target_source (on GPU memory) - * @return Overlap rate - */ -double -overlap_gpu(const GaussianVoxelMap::ConstPtr& target, const Frame::ConstPtr& source, const Eigen::Isometry3f* delta_gpu, CUstream_st* stream = 0); - -/// @brief Calculate the fraction of points fell in target voxels on GPU -double overlap_gpu(const Frame::ConstPtr& target, const Frame::ConstPtr& source, const Eigen::Isometry3f* delta_gpu, CUstream_st* stream = 0); - -/** - * @brief Calculate the fraction of points fell in target voxels on GPU - * @note Source points and target voxelmap must be pre-allocated on GPU. - * @param target Target voxelized frame - * @param source Source frame - * @param delta T_target_source - * @return Overlap rate - */ -double overlap_gpu(const GaussianVoxelMap::ConstPtr& target, const Frame::ConstPtr& source, const Eigen::Isometry3d& delta, CUstream_st* stream = 0); - -/// @brief Calculate the fraction of points fell in target voxels on GPU -double overlap_gpu(const Frame::ConstPtr& target, const Frame::ConstPtr& source, const Eigen::Isometry3d& delta, CUstream_st* stream = 0); - -/** - * @brief Calculate the fraction of points fell in targets' voxels on GPU - * @note Source points and targets' voxelmap must be pre-allocated on GPU. - * @param targets Set of target voxelized frames - * @param source Source frame - * @param deltas Set of T_target_source - * @return Overlap rate - */ -double overlap_gpu( - const std::vector& targets, - const Frame::ConstPtr& source, - const std::vector>& deltas, - CUstream_st* stream = 0); - -/// @brief Calculate the fraction of points fell in targets' voxels on GPU -double overlap_gpu( - const std::vector& targets, - const Frame::ConstPtr& source, - const std::vector>& deltas, - CUstream_st* stream = 0); - -// Automatically select CPU or GPU method -double overlap_auto(const GaussianVoxelMap::ConstPtr& target, const Frame::ConstPtr& source, const Eigen::Isometry3d& delta); - -double overlap_auto(const Frame::ConstPtr& target, const Frame::ConstPtr& source, const Eigen::Isometry3d& delta); - -double overlap_auto( - const std::vector& targets, - const Frame::ConstPtr& source, - const std::vector>& deltas); - -double overlap_auto( - const std::vector& targets, - const Frame::ConstPtr& source, - const std::vector>& deltas); - -} // namespace gtsam_ext diff --git a/include/gtsam_ext/types/voxelized_frame_gpu.hpp b/include/gtsam_ext/types/voxelized_frame_gpu.hpp deleted file mode 100644 index e7891df4..00000000 --- a/include/gtsam_ext/types/voxelized_frame_gpu.hpp +++ /dev/null @@ -1,40 +0,0 @@ -// SPDX-License-Identifier: MIT -// Copyright (c) 2021 Kenji Koide (k.koide@aist.go.jp) - -#pragma once - -#include -#include - -namespace gtsam_ext { - -struct VoxelizedFrameGPU : public FrameGPU { -public: - using Ptr = std::shared_ptr; - using ConstPtr = std::shared_ptr; - - template - VoxelizedFrameGPU(double voxel_resolution, const Eigen::Matrix* points, const Eigen::Matrix* covs, int num_points); - - template typename Alloc> - VoxelizedFrameGPU( - double voxel_resolution, - const std::vector, Alloc>>& points, - const std::vector, Alloc>>& covs) - : VoxelizedFrameGPU(voxel_resolution, points.data(), covs.data(), points.size()) {} - - VoxelizedFrameGPU(double voxel_resolution, const Frame& frame); - VoxelizedFrameGPU(); - ~VoxelizedFrameGPU(); - - void create_voxelmap(double voxel_resolution, CUstream_st* stream = 0); - void create_voxelmap_gpu(double voxel_resolution, CUstream_st* stream = 0); - - // copy data from GPU to CPU - std::vector> get_voxel_buckets_gpu() const; - std::vector get_voxel_num_points_gpu() const; - std::vector> get_voxel_means_gpu() const; - std::vector> get_voxel_covs_gpu() const; -}; - -} // namespace gtsam_ext diff --git a/include/gtsam_ext/util/covariance_estimation.hpp b/include/gtsam_ext/util/covariance_estimation.hpp deleted file mode 100644 index 8a7a6a0a..00000000 --- a/include/gtsam_ext/util/covariance_estimation.hpp +++ /dev/null @@ -1,28 +0,0 @@ -// SPDX-License-Identifier: MIT -// Copyright (c) 2021 Kenji Koide (k.koide@aist.go.jp) - -#pragma once - -#include -#include - -namespace gtsam_ext { - -/** - * @brief Estimate point covariances from neighboring points - * @param points Input points - * @param num_points Number of input points - * @param k_neighbors Number of neighboring points for covariance estimation - * @param num_threads Number of threads - * @return Estimated covariances - */ -std::vector> -estimate_covariances(const Eigen::Vector4d* points, int num_points, int k_neighbors = 10, int num_threads = 1); - -template -std::vector> -estimate_covariances(const std::vector& points, int k_neighbors = 10, int num_threads = 1) { - return estimate_covariances(points.data(), points.size(), k_neighbors, num_threads); -} - -} // namespace gtsam_ext \ No newline at end of file diff --git a/include/gtsam_ext/util/edge_plane_extraction.hpp b/include/gtsam_ext/util/edge_plane_extraction.hpp deleted file mode 100644 index ac13b07a..00000000 --- a/include/gtsam_ext/util/edge_plane_extraction.hpp +++ /dev/null @@ -1,46 +0,0 @@ -// SPDX-License-Identifier: MIT -// Copyright (c) 2021 Kenji Koide (k.koide@aist.go.jp) - -#pragma once - -#include -#include -#include - -namespace gtsam_ext { - -struct ScanLineInformation { - int size() const { return tilt_angles.size(); } - - int num_points(int i) const { return point_counts[i]; } - double angle(int i) const { return tilt_angles[i]; } - - std::vector point_counts; - std::vector tilt_angles; -}; - -/** - * @brief Estimate beam projection angles of velodyne-like point cloud - * - * @param points Points - * @param num_points Number of points - * @param num_scan_lines Number of scan lines (-1 to automatically estimate) - * @param angle_eps Minimum angle between scan lines - * @return Estimated scan line information - */ -ScanLineInformation -estimate_scan_lines(const Eigen::Vector4d* points, int num_points, int num_scan_lines = -1, double angle_eps = 0.05 * M_PI / 180.0); - -/** - * @brief Extract edge and plane points - * Zhang and Singh, "LOAM: Lidar Odometry and Mapping in Real-time", RSS2014 - * - * @param scan_lines Scan line information - * @param points Points - * @param num_points Number of points - * @return Extracted edge and plane points - */ -std::pair -extract_edge_plane_points(const ScanLineInformation& scan_lines, const Eigen::Vector4d* points, int num_points); - -} // namespace gtsam_ext diff --git a/include/gtsam_points/ann/flat_container.hpp b/include/gtsam_points/ann/flat_container.hpp new file mode 100644 index 00000000..422179ee --- /dev/null +++ b/include/gtsam_points/ann/flat_container.hpp @@ -0,0 +1,100 @@ +// SPDX-FileCopyrightText: Copyright 2024 Kenji Koide +// SPDX-License-Identifier: MIT +#pragma once + +#include +#include +#include +#include +#include +#include + +namespace gtsam_points { + +/// @brief Point container with a flat vector. +struct FlatContainer { +public: + /// @brief FlatContainer setting. + struct Setting { + void set_min_dist_in_cell(double dist) { this->min_sq_dist_in_cell = dist * dist; } + void set_max_num_points_in_cell(size_t num_points) { this->max_num_points_in_cell = num_points; } + + double min_sq_dist_in_cell = 0.1 * 0.1; ///< Minimum squared distance between points in a cell. + size_t max_num_points_in_cell = 20; ///< Maximum number of points in a cell. + }; + + /// @brief Constructor. + FlatContainer() { points.reserve(10); } + + /// @brief Number of points. + size_t size() const { return points.size(); } + + /// @brief Add a point to the container. + void add(const Setting& setting, const PointCloud& points, size_t i) { + if ( + this->points.size() >= setting.max_num_points_in_cell || // + std::any_of( + this->points.begin(), + this->points.end(), + [&](const auto& pt) { return (pt - points.points[i]).squaredNorm() < setting.min_sq_dist_in_cell; }) // + ) { + return; + } + + this->points.emplace_back(points.points[i]); + if (points.normals) { + this->normals.emplace_back(points.normals[i]); + } + if (points.covs) { + this->covs.emplace_back(points.covs[i]); + } + if (points.intensities) { + this->intensities.emplace_back(points.intensities[i]); + } + } + + /// @brief Finalize the container (Nothing to do for FlatContainer). + void finalize() {} + + /// @brief Find k nearest neighbors. + /// @param pt Query point + /// @param result Result + template + void knn_search(const Eigen::Vector4d& pt, Result& result) const { + if (points.empty()) { + return; + } + + for (size_t i = 0; i < points.size(); i++) { + const double sq_dist = (points[i] - pt).squaredNorm(); + result.push(i, sq_dist); + } + } + +public: + std::vector points; ///< Points + std::vector normals; ///< Normals + std::vector covs; ///< Covariances + std::vector intensities; ///< Intensities +}; + +namespace frame { + +template <> +struct traits { + static int size(const FlatContainer& frame) { return frame.size(); } + + static bool has_points(const FlatContainer& frame) { return !frame.points.empty(); } + static bool has_normals(const FlatContainer& frame) { return !frame.normals.empty(); } + static bool has_covs(const FlatContainer& frame) { return !frame.covs.empty(); } + static bool has_intensities(const FlatContainer& frame) { return !frame.intensities.empty(); } + + static const Eigen::Vector4d& point(const FlatContainer& frame, size_t i) { return frame.points[i]; } + static const Eigen::Vector4d& normal(const FlatContainer& frame, size_t i) { return frame.normals[i]; } + static const Eigen::Matrix4d& cov(const FlatContainer& frame, size_t i) { return frame.covs[i]; } + static double intensity(const FlatContainer& frame, size_t i) { return frame.intensities[i]; } +}; + +} // namespace frame + +} // namespace gtsam_points diff --git a/include/gtsam_points/ann/impl/incremental_voxelmap_impl.hpp b/include/gtsam_points/ann/impl/incremental_voxelmap_impl.hpp new file mode 100644 index 00000000..45a5380a --- /dev/null +++ b/include/gtsam_points/ann/impl/incremental_voxelmap_impl.hpp @@ -0,0 +1,230 @@ +// SPDX-FileCopyrightText: Copyright 2024 Kenji Koide +// SPDX-License-Identifier: MIT +#pragma once + +#include + +#include +#include + +namespace gtsam_points { + +template +IncrementalVoxelMap::IncrementalVoxelMap(double leaf_size) +: inv_leaf_size(1.0 / leaf_size), + lru_horizon(10), + lru_clear_cycle(10), + lru_counter(0), + offsets(neighbor_offsets(7)) {} + +template +IncrementalVoxelMap::~IncrementalVoxelMap() {} + +template +void IncrementalVoxelMap::clear() { + lru_counter = 0; + flat_voxels.clear(); + voxels.clear(); +} + +template +void IncrementalVoxelMap::insert(const PointCloud& points) { + // Insert points to the voxelmap + for (size_t i = 0; i < points.size(); i++) { + const Eigen::Vector3i coord = fast_floor(points.points[i] * inv_leaf_size).template head<3>(); + + auto found = voxels.find(coord); + if (found == voxels.end()) { + auto voxel = std::make_shared>(VoxelInfo(coord, lru_counter), VoxelContents()); + + found = voxels.emplace_hint(found, coord, flat_voxels.size()); + flat_voxels.emplace_back(voxel); + } + + auto& [info, voxel] = *flat_voxels[found->second]; + info.lru = lru_counter; + voxel.add(voxel_setting, points, i); + } + + if ((++lru_counter) % lru_clear_cycle == 0) { + // Remove least recently used voxels + auto remove_counter = + std::remove_if(flat_voxels.begin(), flat_voxels.end(), [&](const std::shared_ptr>& voxel) { + return voxel->first.lru + lru_horizon < lru_counter; + }); + flat_voxels.erase(remove_counter, flat_voxels.end()); + + // Rehash + voxels.clear(); + for (size_t i = 0; i < flat_voxels.size(); i++) { + voxels[flat_voxels[i]->first.coord] = i; + } + } + + // Finalize voxel means and covs + for (auto& voxel : flat_voxels) { + voxel->second.finalize(); + } +} + +template +size_t IncrementalVoxelMap::knn_search(const double* pt, size_t k, size_t* k_indices, double* k_sq_dists) const { + const Eigen::Vector4d query = (Eigen::Vector4d() << pt[0], pt[1], pt[2], 1.0).finished(); + const Eigen::Vector3i center = fast_floor(query * inv_leaf_size).template head<3>(); + + size_t voxel_index = 0; + const auto index_transform = [&](const size_t point_index) { return calc_index(voxel_index, point_index); }; + + KnnResult<-1, decltype(index_transform)> result(k_indices, k_sq_dists, k, index_transform); + for (const auto& offset : offsets) { + const Eigen::Vector3i coord = center + offset; + const auto found = voxels.find(coord); + if (found == voxels.end()) { + continue; + } + + voxel_index = found->second; + const auto& voxel = flat_voxels[voxel_index]->second; + voxel.knn_search(query, result); + } + + return result.num_found(); +} + +template +std::vector IncrementalVoxelMap::neighbor_offsets(const int neighbor_voxel_mode) const { + switch (neighbor_voxel_mode) { + case 1: + return std::vector{Eigen::Vector3i(0, 0, 0)}; + case 7: + return std::vector{ + Eigen::Vector3i(0, 0, 0), + Eigen::Vector3i(1, 0, 0), + Eigen::Vector3i(-1, 0, 0), + Eigen::Vector3i(0, 1, 0), + Eigen::Vector3i(0, -1, 0), + Eigen::Vector3i(0, 0, 1), + Eigen::Vector3i(0, 0, -1)}; + case 19: { + std::vector offsets; + for (int i = -1; i <= 1; i++) { + for (int j = -1; j <= 1; j++) { + for (int k = -1; k <= 1; k++) { + if (std::abs(i) == 1 && std::abs(j) == 1 && std::abs(k) == 1) { + continue; + } + + offsets.push_back(Eigen::Vector3i(i, j, k)); + } + } + } + return offsets; + } + case 27: { + std::vector offsets; + for (int i = -1; i <= 1; i++) { + for (int j = -1; j <= 1; j++) { + for (int k = -1; k <= 1; k++) { + offsets.push_back(Eigen::Vector3i(i, j, k)); + } + } + } + return offsets; + } + + default: + std::cerr << "error: invalid neighbor voxel mode " << neighbor_voxel_mode << std::endl; + std::cerr << " : neighbor voxel mode must be 1, 7, 19, or 27" << std::endl; + return std::vector(); + } +} + +template +bool IncrementalVoxelMap::has_points() const { + return flat_voxels.empty() ? false : frame::has_points(flat_voxels.front()->second); +} + +template +bool IncrementalVoxelMap::has_normals() const { + return flat_voxels.empty() ? false : frame::has_normals(flat_voxels.front()->second); +} + +template +bool IncrementalVoxelMap::has_covs() const { + return flat_voxels.empty() ? false : frame::has_covs(flat_voxels.front()->second); +} + +template +bool IncrementalVoxelMap::has_intensities() const { + return flat_voxels.empty() ? false : frame::has_intensities(flat_voxels.front()->second); +} + +template +std::vector IncrementalVoxelMap::voxel_points() const { + std::vector points; + points.reserve(flat_voxels.size() * 10); + visit_points([&](const auto& voxel, const int i) { points.emplace_back(frame::point(voxel, i)); }); + return points; +} + +template +std::vector IncrementalVoxelMap::voxel_normals() const { + std::vector normals; + normals.reserve(flat_voxels.size() * 10); + visit_points([&](const auto& voxel, const int i) { normals.emplace_back(frame::normal(voxel, i)); }); + return normals; +} + +template +std::vector IncrementalVoxelMap::voxel_covs() const { + std::vector covs; + covs.reserve(flat_voxels.size() * 10); + visit_points([&](const auto& voxel, const int i) { covs.emplace_back(frame::cov(voxel, i)); }); + return covs; +} + +template +std::vector IncrementalVoxelMap::voxel_intensities() const { + std::vector intensities; + intensities.reserve(flat_voxels.size() * 10); + visit_points([&](const auto& voxel, const int i) { intensities.emplace_back(frame::intensity(voxel, i)); }); + return intensities; +} + +template +PointCloudCPU::Ptr IncrementalVoxelMap::voxel_data() const { + auto frame = std::make_shared(); + frame->points_storage.reserve(flat_voxels.size() * 10); + if (has_normals()) { + frame->normals_storage.reserve(flat_voxels.size() * 10); + } + if (has_covs()) { + frame->covs_storage.reserve(flat_voxels.size() * 10); + } + if (has_intensities()) { + frame->intensities_storage.reserve(flat_voxels.size() * 10); + } + + visit_points([&](const auto& voxel, const int i) { + frame->points_storage.emplace_back(frame::point(voxel, i)); + if (frame::has_normals(voxel)) { + frame->normals_storage.emplace_back(frame::normal(voxel, i)); + } + if (frame::has_covs(voxel)) { + frame->covs_storage.emplace_back(frame::cov(voxel, i)); + } + if (frame::has_intensities(voxel)) { + frame->intensities_storage.emplace_back(frame::intensity(voxel, i)); + } + }); + + frame->num_points = frame->points_storage.size(); + frame->points = frame->points_storage.data(); + frame->normals = frame->normals_storage.empty() ? nullptr : frame->normals_storage.data(); + frame->covs = frame->covs_storage.empty() ? nullptr : frame->covs_storage.data(); + frame->intensities = frame->intensities_storage.empty() ? nullptr : frame->intensities_storage.data(); + + return frame; +} + +} // namespace gtsam_points diff --git a/include/gtsam_points/ann/incremental_covariance_container.hpp b/include/gtsam_points/ann/incremental_covariance_container.hpp new file mode 100644 index 00000000..d068872a --- /dev/null +++ b/include/gtsam_points/ann/incremental_covariance_container.hpp @@ -0,0 +1,111 @@ +// SPDX-FileCopyrightText: Copyright 2024 Kenji Koide +// SPDX-License-Identifier: MIT +#pragma once + +#include +#include + +namespace gtsam_points { + +/// @brief Linear container for incremental covariance and normal estimation. +struct IncrementalCovarianceContainer { +public: + /// @brief FlatContainer setting. + struct Setting { + void set_min_dist_in_cell(double dist) { this->min_sq_dist_in_cell = dist * dist; } + void set_max_num_points_in_cell(size_t num_points) { this->max_num_points_in_cell = num_points; } + + double min_sq_dist_in_cell = 0.1 * 0.1; ///< Minimum squared distance between points in a cell. + size_t max_num_points_in_cell = 20; ///< Maximum number of points in a cell. + }; + + /// @brief Constructor. + IncrementalCovarianceContainer(); + + /// @brief Number of points in the container. + size_t size() const { return points.size(); } + + /// @brief Add a point to the container. + void add(const Setting& setting, const PointCloud& points, size_t i); + + /// @brief Finalize the container (Nothing to do for FlatContainer). + void finalize() {} + + /// @brief Find k nearest neighbors. + /// @param pt Query point + /// @param result Result + template + void knn_search(const Eigen::Vector4d& pt, Result& result) const { + if (points.empty()) { + return; + } + + for (size_t i = 0; i < points.size(); i++) { + if (!valid(flags[i])) { + continue; + } + + result.push(i, (points[i] - pt).squaredNorm()); + } + } + + /// @brief Find k nearest neighbors. + /// @param pt Query point + /// @param result Result + template + void knn_search_force(const Eigen::Vector4d& pt, Result& result) const { + if (points.empty()) { + return; + } + + for (size_t i = 0; i < points.size(); i++) { + result.push(i, (points[i] - pt).squaredNorm()); + } + } + + /// @brief Set the i-th point as valid. + void set_valid(int i) { flags[i] |= VALID_BIT; } + + /// @brief Check if the i-th point is valid. + bool valid(int i) const { return flags[i] & VALID_BIT; } + + /// @brief Get the time when the i-th point was inserted. + size_t birthday(int i) const { return flags[i] & BIRTHDAY_MASK; } + + /// @brief Get the time since the i-th point was inserted. + size_t age(int i, size_t lru) const { return lru - birthday(i); } + + /// @brief Remove old invalid points. + size_t remove_old_invalid(int age_thresh, size_t lru); + +public: + static constexpr size_t VALID_BIT = 1ull << 63; + static constexpr size_t BIRTHDAY_MASK = (VALID_BIT >> 1) - 1; + + std::vector points; ///< Points + + std::vector flags; ///< State flags + std::vector normals; ///< Normals + std::vector covs; ///< Covariances +}; + +namespace frame { + +template <> +struct traits { + static int size(const IncrementalCovarianceContainer& frame) { return frame.size(); } + + static bool has_points(const IncrementalCovarianceContainer& frame) { return !frame.points.empty(); } + static bool has_normals(const IncrementalCovarianceContainer& frame) { return true; } + static bool has_covs(const IncrementalCovarianceContainer& frame) { return true; } + static bool has_intensities(const IncrementalCovarianceContainer& frame) { return false; } + + static const Eigen::Vector4d& point(const IncrementalCovarianceContainer& frame, size_t i) { return frame.points[i]; } + static const Eigen::Vector4d& normal(const IncrementalCovarianceContainer& frame, size_t i) { return frame.normals[i]; } + static const Eigen::Matrix4d& cov(const IncrementalCovarianceContainer& frame, size_t i) { return frame.covs[i]; } + static double intensity(const IncrementalCovarianceContainer& frame, size_t i) { return 0.0; } +}; + +} // namespace frame + +} // namespace gtsam_points \ No newline at end of file diff --git a/include/gtsam_points/ann/incremental_covariance_voxelmap.hpp b/include/gtsam_points/ann/incremental_covariance_voxelmap.hpp new file mode 100644 index 00000000..4ace33a7 --- /dev/null +++ b/include/gtsam_points/ann/incremental_covariance_voxelmap.hpp @@ -0,0 +1,92 @@ +// SPDX-FileCopyrightText: Copyright 2024 Kenji Koide +// SPDX-License-Identifier: MIT +#pragma once + +#include +#include +#include +#include + +namespace gtsam_points { + +/// @brief Incremental voxelmap with online covariance and normal estimation +struct IncrementalCovarianceVoxelMap : public IncrementalVoxelMap { +public: + /// @brief Constructor. + /// @param voxel_resolution Voxel resolution + IncrementalCovarianceVoxelMap(double voxel_resolution); + virtual ~IncrementalCovarianceVoxelMap(); + + /// @brief Set the number of neighbors for covariance estimation. + void set_num_neighbors(int num_neighbors); + /// @brief Set the minimum number of neighbors for covariance estimation. + void set_min_num_neighbors(int min_num_neighbors); + /// @brief Set the number of warmup cycles. Covariances of new points in this period are not re-evaluated every frame. + void set_warmup_cycles(int warmup_cycles); + /// @brief Set the number of lowrate update cycles. Covariances of invalid points are re-evaluated every this period. + void set_lowrate_cycles(int lowrate_cycles); + /// @brief Set the age threshold for removing invalid points. Invalid points older than this are removed. + void set_remove_invalid_age_thresh(int remove_invalid_age_thresh); + /// @brief Set the threshold scale for normal validation. + void set_eig_stddev_thresh_scale(double eig_stddev_thresh_scale); + /// @brief Set the number of threads for normal estimation. + void set_num_threads(int num_threads); + + /// @brief Clear the voxelmap. + virtual void clear() override; + + /// @brief Insert point into the voxelmap. + virtual void insert(const PointCloud& points) override; + + /// @brief Find k-nearest neighbors. This only finds neighbors with valid covariances. + virtual size_t knn_search(const double* pt, size_t k, size_t* k_indices, double* k_sq_dists) const override; + + /// @brief Find k-nearest neighbors. This finds neighbors regardless of the validity of covariances. + size_t knn_search_force(const double* pt, size_t k, size_t* k_indices, double* k_sq_dists) const; + + /// @brief Get valid point indices. If num_threads is -1, the member variable num_threads is used. + std::vector valid_indices(int num_threads = -1) const; + /// @brief Get points from indices. + std::vector voxel_points(const std::vector& indices) const; + /// @brief Get normals from indices. + std::vector voxel_normals(const std::vector& indices) const; + /// @brief Get covariances from indices. + std::vector voxel_covs(const std::vector& indices) const; + + /// @brief Get voxel points + virtual std::vector voxel_points() const override; + /// @brief Get voxel normals + virtual std::vector voxel_normals() const override; + /// @brief Get voxel covariances + virtual std::vector voxel_covs() const override; + +protected: + int num_neighbors; ///< Number of neighbors for covariance estimation. + int min_num_neighbors; ///< Minimum number of neighbors for covariance estimation. + int warmup_cycles; ///< Number of cycles for covariance warmup. + int lowrate_cycles; ///< Number of cycles for lowrate covariance estimation. + int remove_invalid_age_thresh; ///< Age threshold for removing invalid points. + double eig_stddev_thresh_scale; ///< Threshold scale for normal validation. + int num_threads; ///< Number of threads for normal estimation. + + std::deque> eig_stats; ///< Running statistics for eigenvalues. +}; + +namespace frame { + +template <> +struct traits { + static bool has_points(const IncrementalCovarianceVoxelMap& ivox) { return ivox.has_points(); } + static bool has_normals(const IncrementalCovarianceVoxelMap& ivox) { return ivox.has_normals(); } + static bool has_covs(const IncrementalCovarianceVoxelMap& ivox) { return ivox.has_covs(); } + static bool has_intensities(const IncrementalCovarianceVoxelMap& ivox) { return ivox.has_intensities(); } + + static const Eigen::Vector4d& point(const IncrementalCovarianceVoxelMap& ivox, size_t i) { return ivox.point(i); } + static const Eigen::Vector4d& normal(const IncrementalCovarianceVoxelMap& ivox, size_t i) { return ivox.normal(i); } + static const Eigen::Matrix4d& cov(const IncrementalCovarianceVoxelMap& ivox, size_t i) { return ivox.cov(i); } + static double intensity(const IncrementalCovarianceVoxelMap& ivox, size_t i) { return ivox.intensity(i); } +}; + +} // namespace frame + +} // namespace gtsam_points diff --git a/include/gtsam_points/ann/incremental_voxelmap.hpp b/include/gtsam_points/ann/incremental_voxelmap.hpp new file mode 100644 index 00000000..d8a5c13e --- /dev/null +++ b/include/gtsam_points/ann/incremental_voxelmap.hpp @@ -0,0 +1,145 @@ +// SPDX-FileCopyrightText: Copyright 2024 Kenji Koide +// SPDX-License-Identifier: MIT +#pragma once + +#include +#include +#include +#include + +namespace gtsam_points { + +/// @brief Voxel meta information. +struct VoxelInfo { +public: + /// @brief Default constructor. + VoxelInfo() : lru(0), coord(-1, -1, -1) {} + + /// @brief Constructor. + /// @param coord Integer voxel coordinates + /// @param lru LRU counter for caching + VoxelInfo(const Eigen::Vector3i& coord, size_t lru) : lru(lru), coord(coord) {} + +public: + size_t lru; ///< Last used time + Eigen::Vector3i coord; ///< Voxel coordinate +}; + +/// @brief Incremental voxelmap. +/// This class supports incremental point cloud insertion and LRU-based voxel deletion. +/// @note This class can be used as a point cloud as well as a neighbor search structure. +/// @note For the compatibility with other nearest neighbor search methods, this implementation returns indices that encode the voxel and point IDs. +/// The first ```point_id_bits``` (e.g., 32) bits of a point index represent the point ID, and the rest ```voxel_id_bits``` (e.g., 32) bits +/// represent the voxel ID that contains the point. The specified point can be looked up by `voxelmap.point(index)`; +template +struct IncrementalVoxelMap : public NearestNeighborSearch { +public: + using Ptr = std::shared_ptr; + using ConstPtr = std::shared_ptr; + + /// @brief Constructor. + /// @param leaf_size Voxel size + explicit IncrementalVoxelMap(double leaf_size); + virtual ~IncrementalVoxelMap(); + + /// @brief Voxel resolution. + void set_voxel_resolution(const double leaf_size) { inv_leaf_size = 1.0 / leaf_size; } + /// @brief LRU cache clearing cycle. + void set_lru_clear_cycle(const int lru_clear_cycle) { this->lru_clear_cycle = lru_clear_cycle; } + /// @brief LRU cache horizon. + void set_lru_horizon(const int lru_horizon) { this->lru_horizon = lru_horizon; } + /// @brief Neighboring voxel search mode (1, 7, 19, or 27). + void set_neighbor_voxel_mode(const int mode) { offsets = neighbor_offsets(mode); } + /// @brief Voxel setting. + typename VoxelContents::Setting& voxel_insertion_setting() { return voxel_setting; } + + /// @brief Voxel size. + double leaf_size() const { return 1.0 / inv_leaf_size; } + + /// @brief Number of voxels in the voxelmap. + size_t num_voxels() const { return flat_voxels.size(); } + + /// @brief Clear the voxelmap. + virtual void clear(); + + /// @brief Insert points to the voxelmap. + /// @param points Point cloud + /// @param T Transformation matrix + virtual void insert(const PointCloud& points); + + /// @brief Find k nearest neighbors. + /// @param pt Query point + /// @param k Number of neighbors to search + /// @param k_indices Indices of the k nearest neighbors + /// @param k_sq_dists Squared distances of the k nearest neighbors + /// @return Number of found neighbors + virtual size_t knn_search(const double* pt, size_t k, size_t* k_indices, double* k_sq_dists) const override; + + /// @brief Calculate the global point index from the voxel index and the point index. + inline size_t calc_index(const size_t voxel_id, const size_t point_id) const { return (voxel_id << point_id_bits) | point_id; } + inline size_t voxel_id(const size_t i) const { return i >> point_id_bits; } ///< Extract the point ID from a global index. + inline size_t point_id(const size_t i) const { return i & ((1ull << point_id_bits) - 1); } ///< Extract the voxel ID from a global index. + + bool has_points() const; + bool has_normals() const; + bool has_covs() const; + bool has_intensities() const; + + decltype(auto) point(const size_t i) const { return frame::point(flat_voxels[voxel_id(i)]->second, point_id(i)); } + decltype(auto) normal(const size_t i) const { return frame::normal(flat_voxels[voxel_id(i)]->second, point_id(i)); } + decltype(auto) cov(const size_t i) const { return frame::cov(flat_voxels[voxel_id(i)]->second, point_id(i)); } + decltype(auto) intensity(const size_t i) const { return frame::intensity(flat_voxels[voxel_id(i)]->second, point_id(i)); } + + virtual std::vector voxel_points() const; + virtual std::vector voxel_normals() const; + virtual std::vector voxel_covs() const; + virtual std::vector voxel_intensities() const; + + virtual PointCloudCPU::Ptr voxel_data() const; + +protected: + std::vector neighbor_offsets(const int neighbor_voxel_mode) const; + + template + void visit_points(const Func& f) const { + for (const auto& voxel : flat_voxels) { + for (int i = 0; i < frame::size(voxel->second); i++) { + f(voxel->second, i); + } + } + } + +protected: + static_assert(sizeof(size_t) == 8, "size_t must be 64-bit"); + static constexpr int point_id_bits = 32; ///< Use the first 32 bits for point id + static constexpr int voxel_id_bits = 64 - point_id_bits; ///< Use the remaining bits for voxel id + double inv_leaf_size; ///< Inverse of the voxel size + std::vector offsets; ///< Neighbor voxel offsets + + size_t lru_horizon; ///< LRU horizon size. Voxels that have not been accessed for lru_horizon steps are deleted. + size_t lru_clear_cycle; ///< LRU clear cycle. Voxel deletion is performed every lru_clear_cycle steps. + size_t lru_counter; ///< LRU counter. Incremented every step. + + typename VoxelContents::Setting voxel_setting; ///< Voxel setting. + std::vector>> flat_voxels; ///< Voxel contents. + std::unordered_map voxels; ///< Voxel index map. +}; + +namespace frame { + +template +struct traits> { + static bool has_points(const IncrementalVoxelMap& ivox) { return ivox.has_points(); } + static bool has_normals(const IncrementalVoxelMap& ivox) { return ivox.has_normals(); } + static bool has_covs(const IncrementalVoxelMap& ivox) { return ivox.has_covs(); } + static bool has_intensities(const IncrementalVoxelMap& ivox) { return ivox.has_intensities(); } + + static const Eigen::Vector4d& point(const IncrementalVoxelMap& ivox, size_t i) { return ivox.point(i); } + static const Eigen::Vector4d& normal(const IncrementalVoxelMap& ivox, size_t i) { return ivox.normal(i); } + static const Eigen::Matrix4d& cov(const IncrementalVoxelMap& ivox, size_t i) { return ivox.cov(i); } + static double intensity(const IncrementalVoxelMap& ivox, size_t i) { return ivox.intensity(i); } +}; + +} // namespace frame + +} // namespace gtsam_points diff --git a/include/gtsam_ext/ann/intensity_kdtree.hpp b/include/gtsam_points/ann/intensity_kdtree.hpp similarity index 79% rename from include/gtsam_ext/ann/intensity_kdtree.hpp rename to include/gtsam_points/ann/intensity_kdtree.hpp index 0c8173dd..0a6ee569 100644 --- a/include/gtsam_ext/ann/intensity_kdtree.hpp +++ b/include/gtsam_points/ann/intensity_kdtree.hpp @@ -4,9 +4,21 @@ #pragma once #include -#include +#include +#include -namespace gtsam_ext { +// forward declaration +namespace nanoflann { + +template +class L2_Simple_Adaptor; + +template +class KDTreeSingleIndexAdaptor; + +} // namespace nanoflann + +namespace gtsam_points { /** * @brief KdTree on intensity augmented coordinates. @@ -53,4 +65,4 @@ struct IntensityKdTree : public NearestNeighborSearch { std::unique_ptr index; }; -} // namespace gtsam_ext \ No newline at end of file +} // namespace gtsam_points \ No newline at end of file diff --git a/include/gtsam_points/ann/ivox.hpp b/include/gtsam_points/ann/ivox.hpp new file mode 100644 index 00000000..915f1ed0 --- /dev/null +++ b/include/gtsam_points/ann/ivox.hpp @@ -0,0 +1,23 @@ +// SPDX-License-Identifier: MIT +// Copyright (c) 2021 Kenji Koide (k.koide@aist.go.jp) + +#pragma once + +#include +#include + +#include + +#include +#include + +namespace gtsam_points { + +/** + * @brief Voxel-based incremental nearest neighbor search + * Bai et al., "Faster-LIO: Lightweight Tightly Coupled Lidar-Inertial Odometry Using Parallel Sparse Incremental Voxels", IEEE RA-L, 2022 + * @note Only the linear iVox is implemented + */ +using iVox = IncrementalVoxelMap; + +} // namespace gtsam_points \ No newline at end of file diff --git a/include/gtsam_points/ann/ivox_covariance_estimation.hpp b/include/gtsam_points/ann/ivox_covariance_estimation.hpp new file mode 100644 index 00000000..89d16701 --- /dev/null +++ b/include/gtsam_points/ann/ivox_covariance_estimation.hpp @@ -0,0 +1,13 @@ +// SPDX-License-Identifier: MIT +// Copyright (c) 2021 Kenji Koide (k.koide@aist.go.jp) + +#pragma once + +#include +#include + +namespace gtsam_points { + +using iVoxCovarianceEstimation = IncrementalCovarianceVoxelMap; + +} // namespace gtsam_points \ No newline at end of file diff --git a/include/gtsam_points/ann/kdtree.hpp b/include/gtsam_points/ann/kdtree.hpp new file mode 100644 index 00000000..104beee5 --- /dev/null +++ b/include/gtsam_points/ann/kdtree.hpp @@ -0,0 +1,45 @@ +// SPDX-License-Identifier: MIT +// Copyright (c) 2021 Kenji Koide (k.koide@aist.go.jp) + +#pragma once + +#include +#include +#include + +#include + +namespace gtsam_points { + +template +struct UnsafeKdTree; +struct AxisAlignedProjection; + +/** + * @brief KdTree-based nearest neighbor search + */ +struct KdTree : public NearestNeighborSearch { +public: + using Index = UnsafeKdTree; + + KdTree(const Eigen::Vector4d* points, int num_points, int build_num_threads = 1); + virtual ~KdTree() override; + + /// @brief Find k nearest neighbors + /// @param pt Query point (must be 4D vector [x, y, z, 1]) + /// @param k Number of neighbors to search + /// @param k_indices Indices of k nearest neighbors + /// @param k_sq_dists Squared distances of k nearest neighbors + /// @return Number of neighbors found + virtual size_t knn_search(const double* pt, size_t k, size_t* k_indices, double* k_sq_dists) const override; + +public: + const int num_points; + const Eigen::Vector4d* points; + + double search_eps; + + std::unique_ptr index; +}; + +} // namespace gtsam_points diff --git a/include/gtsam_points/ann/kdtree2.hpp b/include/gtsam_points/ann/kdtree2.hpp new file mode 100644 index 00000000..eeb2bb79 --- /dev/null +++ b/include/gtsam_points/ann/kdtree2.hpp @@ -0,0 +1,53 @@ +// SPDX-License-Identifier: MIT +// Copyright (c) 2021 Kenji Koide (k.koide@aist.go.jp) + +#pragma once + +#include +#include +#include + +#include +#include +#include + +namespace gtsam_points { + +/** + * @brief KdTree-based nearest neighbor search + */ +template +struct KdTree2 : public NearestNeighborSearch { +public: + using Index = UnsafeKdTree; + + KdTree2(const std::shared_ptr& frame, int build_num_threads = 1) + : frame(frame), + search_eps(-1.0), + index(new Index(*this->frame, KdTreeBuilderOMP(build_num_threads))) { + if (frame::size(*frame) == 0) { + std::cerr << "error: empty frame is given for KdTree2" << std::endl; + std::cerr << " : frame::size() may not be implemented" << std::endl; + } + } + virtual ~KdTree2() override {} + + /// @brief Find k nearest neighbors + /// @param pt Query point (must be 4D vector [x, y, z, 1]) + /// @param k Number of neighbors to search + /// @param k_indices Indices of k nearest neighbors + /// @param k_sq_dists Squared distances of k nearest neighbors + /// @return Number of neighbors found + virtual size_t knn_search(const double* pt, size_t k, size_t* k_indices, double* k_sq_dists) const override { + return index->knn_search(Eigen::Map(pt), k, k_indices, k_sq_dists); + } + +public: + const std::shared_ptr frame; + + double search_eps; + + std::unique_ptr index; +}; + +} // namespace gtsam_points diff --git a/include/gtsam_points/ann/knn_result.hpp b/include/gtsam_points/ann/knn_result.hpp new file mode 100644 index 00000000..0fdaee6e --- /dev/null +++ b/include/gtsam_points/ann/knn_result.hpp @@ -0,0 +1,111 @@ +// SPDX-FileCopyrightText: Copyright 2024 Kenji Koide +// SPDX-License-Identifier: MIT +#pragma once + +#include + +namespace gtsam_points { + +/// @brief K-nearest neighbor search setting. +struct KnnSetting { +public: + /// @brief Check if the result satisfies the early termination condition. + template + bool fulfilled(const Result& result) const { + return result.worst_distance() < epsilon; + } + +public: + double epsilon = 0.0; ///< Early termination threshold +}; + +/// @brief Identity transformation function. (use std::identity instead in C++20) +struct identity_transform { + template + T operator()(const T& x) const { + return x; + } +}; + +/// @brief K-nearest neighbor search result container. +/// @tparam N Number of neighbors to search. If N == -1, the number of neighbors is dynamicaly determined. +template +struct KnnResult { +public: + static constexpr size_t INVALID = std::numeric_limits::max(); + + /// @brief Constructor + /// @param indices Buffer to store indices (must be larger than k=max(N, num_neighbors)) + /// @param distances Buffer to store distances (must be larger than k=max(N, num_neighbors)) + /// @param num_neighbors Number of neighbors to search (must be -1 for static case N > 0) + /// @param index_transform Index transformation function (e.g., local point index -> global voxel + point index) + explicit KnnResult(size_t* indices, double* distances, int num_neighbors = -1, const IndexTransform& index_transform = IndexTransform()) + : index_transform(index_transform), + capacity(num_neighbors), + num_found_neighbors(0), + indices(indices), + distances(distances) { + if constexpr (N > 0) { + if (num_neighbors >= 0) { + std::cerr << "warning: Specifying dynamic num_neighbors=" << num_neighbors << " for a static KNN result container (N=" << N << ")" + << std::endl; + abort(); + } + } else { + if (num_neighbors <= 0) { + std::cerr << "error: Specifying invalid num_neighbors=" << num_neighbors << " for a dynamic KNN result container" << std::endl; + abort(); + } + } + + std::fill(this->indices, this->indices + buffer_size(), INVALID); + std::fill(this->distances, this->distances + buffer_size(), std::numeric_limits::max()); + } + + /// @brief Buffer size (i.e., Maximum number of neighbors) + size_t buffer_size() const { + if constexpr (N > 0) { + return N; + } else { + return capacity; + } + } + + /// @brief Number of found neighbors. + size_t num_found() const { return num_found_neighbors; } + + /// @brief Worst distance in the result. + double worst_distance() const { return distances[buffer_size() - 1]; } + + /// @brief Push a pair of point index and distance to the result. + void push(size_t index, double distance) { + if (distance >= worst_distance()) { + return; + } + + if constexpr (N == 1) { + indices[0] = index_transform(index); + distances[0] = distance; + } else { + int insert_loc = std::min(num_found_neighbors, buffer_size() - 1); + for (; insert_loc > 0 && distance < distances[insert_loc - 1]; insert_loc--) { + indices[insert_loc] = indices[insert_loc - 1]; + distances[insert_loc] = distances[insert_loc - 1]; + } + + indices[insert_loc] = index_transform(index); + distances[insert_loc] = distance; + } + + num_found_neighbors = std::min(num_found_neighbors + 1, buffer_size()); + } + +public: + const IndexTransform& index_transform; + const int capacity; ///< Maximum number of neighbors to search + int num_found_neighbors; ///< Number of found neighbors + size_t* indices; ///< Indices of neighbors + double* distances; ///< Distances to neighbors +}; + +} // namespace gtsam_points diff --git a/include/gtsam_ext/ann/nearest_neighbor_search.hpp b/include/gtsam_points/ann/nearest_neighbor_search.hpp similarity index 93% rename from include/gtsam_ext/ann/nearest_neighbor_search.hpp rename to include/gtsam_points/ann/nearest_neighbor_search.hpp index d4ad88e6..c1029d59 100644 --- a/include/gtsam_ext/ann/nearest_neighbor_search.hpp +++ b/include/gtsam_points/ann/nearest_neighbor_search.hpp @@ -5,7 +5,7 @@ #include -namespace gtsam_ext { +namespace gtsam_points { /** * @brief Nearest neighbor search interface @@ -27,4 +27,4 @@ struct NearestNeighborSearch { */ virtual size_t knn_search(const double* pt, size_t k, size_t* k_indices, double* k_sq_dists) const { return 0; }; }; -} // namespace gtsam_ext \ No newline at end of file +} // namespace gtsam_points \ No newline at end of file diff --git a/include/gtsam_points/ann/small_kdtree.hpp b/include/gtsam_points/ann/small_kdtree.hpp new file mode 100644 index 00000000..8fa409da --- /dev/null +++ b/include/gtsam_points/ann/small_kdtree.hpp @@ -0,0 +1,382 @@ +// SPDX-FileCopyrightText: Copyright 2024 Kenji Koide +// SPDX-License-Identifier: MIT + +// KdTree code derived from small_gicp. +// While the following KdTree code is written from scratch, it is heavily inspired by the nanoflann library. +// Thus, the following original license of nanoflann is included to be sure. + +// https://github.com/jlblancoc/nanoflann/blob/master/include/nanoflann.hpp +/*********************************************************************** + * Software License Agreement (BSD License) + * + * Copyright 2008-2009 Marius Muja (mariusm@cs.ubc.ca). All rights reserved. + * Copyright 2008-2009 David G. Lowe (lowe@cs.ubc.ca). All rights reserved. + * Copyright 2011-2024 Jose Luis Blanco (joseluisblancoc@gmail.com). + * All rights reserved. + * + * THE BSD LICENSE + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. 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. + * + * THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``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 AUTHOR 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. + *************************************************************************/ +#pragma once + +#include +#include +#include + +#include +#include + +namespace gtsam_points { + +/// @brief Parameters to control the projection axis search. +struct ProjectionSetting { + int max_scan_count = 128; ///< Maximum number of points to use for the axis search. +}; + +/// @brief Conventional axis-aligned projection (i.e., selecting any of XYZ axes with the largest variance). +struct AxisAlignedProjection { +public: + /// @brief Project the point to the selected axis. + /// @param pt Point to project + /// @return Projected value + double operator()(const Eigen::Vector4d& pt) const { return pt[axis]; } + + /// @brief Find the axis with the largest variance. + /// @param points Point cloud + /// @param first First point index iterator + /// @param last Last point index iterator + /// @param setting Search setting + /// @return Projection with the largest variance axis + template + static AxisAlignedProjection + find_axis(const PointCloud& points, IndexConstIterator first, IndexConstIterator last, const ProjectionSetting& setting) { + const size_t N = std::distance(first, last); + Eigen::Vector4d sum_pt = Eigen::Vector4d::Zero(); + Eigen::Vector4d sum_sq = Eigen::Vector4d::Zero(); + + const size_t step = N < setting.max_scan_count ? 1 : N / setting.max_scan_count; + const size_t num_steps = N / step; + for (int i = 0; i < num_steps; i++) { + const auto itr = first + step * i; + const Eigen::Vector4d pt = frame::point(points, *itr); + sum_pt += pt; + sum_sq += pt.cwiseProduct(pt); + } + + const Eigen::Vector4d mean = sum_pt / sum_pt.w(); + const Eigen::Vector4d var = (sum_sq - mean.cwiseProduct(sum_pt)); + + return AxisAlignedProjection{var[0] > var[1] ? (var[0] > var[2] ? 0 : 2) : (var[1] > var[2] ? 1 : 2)}; + } + +public: + int axis; ///< Axis index (0: X, 1: Y, 2: Z) +}; + +using NodeIndexType = std::uint32_t; +static constexpr NodeIndexType INVALID_NODE = std::numeric_limits::max(); + +/// @brief KdTree node. +template +struct KdTreeNode { + union { + struct Leaf { + NodeIndexType first; ///< First point index in the leaf node. + NodeIndexType last; ///< Last point index in the leaf node. + } lr; ///< Leaf node. + struct NonLeaf { + Projection proj; ///< Projection axis. + double thresh; ///< Threshold value. + } sub; ///< Non-leaf node. + } node_type; + + NodeIndexType left = INVALID_NODE; ///< Left child node index. + NodeIndexType right = INVALID_NODE; ///< Right child node index. +}; + +/// @brief Single thread Kd-tree builder. +struct KdTreeBuilder { +public: + /// @brief Build KdTree + /// @param kdtree Kd-tree to build + /// @param points Point cloud + template + void build_tree(KdTree& kdtree, const PointCloud& points) const { + kdtree.indices.resize(frame::size(points)); + std::iota(kdtree.indices.begin(), kdtree.indices.end(), 0); + + size_t node_count = 0; + kdtree.nodes.resize(frame::size(points)); + kdtree.root = create_node(kdtree, node_count, points, kdtree.indices.begin(), kdtree.indices.begin(), kdtree.indices.end()); + kdtree.nodes.resize(node_count); + } + + /// @brief Create a Kd-tree node from the given point indices. + /// @param global_first Global first point index iterator (i.e., this->indices.begin()). + /// @param first First point index iterator to be scanned. + /// @param last Last point index iterator to be scanned. + /// @return Index of the created node. + template + NodeIndexType create_node( + KdTree& kdtree, + size_t& node_count, + const PointCloud& points, + IndexConstIterator global_first, + IndexConstIterator first, + IndexConstIterator last) const { + const size_t N = std::distance(first, last); + const NodeIndexType node_index = node_count++; + auto& node = kdtree.nodes[node_index]; + + // Create a leaf node. + if (N <= max_leaf_size) { + // std::sort(first, last); + node.node_type.lr.first = std::distance(global_first, first); + node.node_type.lr.last = std::distance(global_first, last); + + return node_index; + } + + // Find the best axis to split the input points. + using Projection = typename KdTree::Projection; + const auto proj = Projection::find_axis(points, first, last, projection_setting); + const auto median_itr = first + N / 2; + std::nth_element(first, median_itr, last, [&](size_t i, size_t j) { return proj(frame::point(points, i)) < proj(frame::point(points, j)); }); + + // Create a non-leaf node. + node.node_type.sub.proj = proj; + node.node_type.sub.thresh = proj(frame::point(points, *median_itr)); + + // Create left and right child nodes. + node.left = create_node(kdtree, node_count, points, global_first, first, median_itr); + node.right = create_node(kdtree, node_count, points, global_first, median_itr, last); + + return node_index; + } + +public: + int max_leaf_size = 20; ///< Maximum number of points in a leaf node. + ProjectionSetting projection_setting; ///< Projection setting. +}; + +/// @brief Kd-tree builder with OpenMP. +struct KdTreeBuilderOMP { +public: + /// @brief Constructor + /// @param num_threads Number of threads + KdTreeBuilderOMP(int num_threads = 4) : num_threads(num_threads), max_leaf_size(20) {} + + /// @brief Build KdTree + template + void build_tree(KdTree& kdtree, const PointCloud& points) const { + kdtree.indices.resize(frame::size(points)); + std::iota(kdtree.indices.begin(), kdtree.indices.end(), 0); + + std::atomic_uint64_t node_count = 0; + kdtree.nodes.resize(frame::size(points)); + +#ifndef _MSC_VER +#pragma omp parallel num_threads(num_threads) + { +#pragma omp single nowait + { kdtree.root = create_node(kdtree, node_count, points, kdtree.indices.begin(), kdtree.indices.begin(), kdtree.indices.end()); } + } +#else + kdtree.root = create_node(kdtree, node_count, points, kdtree.indices.begin(), kdtree.indices.begin(), kdtree.indices.end()); +#endif + + kdtree.nodes.resize(node_count); + } + + /// @brief Create a Kd-tree node from the given point indices. + /// @param global_first Global first point index iterator (i.e., this->indices.begin()). + /// @param first First point index iterator to be scanned. + /// @param last Last point index iterator to be scanned. + /// @return Index of the created node. + template + NodeIndexType create_node( + KdTree& kdtree, + std::atomic_uint64_t& node_count, + const PointCloud& points, + IndexConstIterator global_first, + IndexConstIterator first, + IndexConstIterator last) const { + const size_t N = std::distance(first, last); + const NodeIndexType node_index = node_count++; + auto& node = kdtree.nodes[node_index]; + + // Create a leaf node. + if (N <= max_leaf_size) { + // std::sort(first, last); + node.node_type.lr.first = std::distance(global_first, first); + node.node_type.lr.last = std::distance(global_first, last); + + return node_index; + } + + // Find the best axis to split the input points. + using Projection = typename KdTree::Projection; + const auto proj = Projection::find_axis(points, first, last, projection_setting); + const auto median_itr = first + N / 2; + std::nth_element(first, median_itr, last, [&](size_t i, size_t j) { return proj(frame::point(points, i)) < proj(frame::point(points, j)); }); + + // Create a non-leaf node. + node.node_type.sub.proj = proj; + node.node_type.sub.thresh = proj(frame::point(points, *median_itr)); + + // Create left and right child nodes. +#ifndef _MSC_VER +#pragma omp task default(shared) if (N > 512) + node.left = create_node(kdtree, node_count, points, global_first, first, median_itr); +#pragma omp task default(shared) if (N > 512) + node.right = create_node(kdtree, node_count, points, global_first, median_itr, last); +#pragma omp taskwait +#else + node.left = create_node(kdtree, node_count, points, global_first, first, median_itr); + node.right = create_node(kdtree, node_count, points, global_first, median_itr, last); +#endif + + return node_index; + } + +public: + int num_threads; ///< Number of threads + int max_leaf_size; ///< Maximum number of points in a leaf node. + ProjectionSetting projection_setting; ///< Projection setting. +}; + +/// @brief "Unsafe" KdTree. +/// @note This class does not hold the ownership of the input points. +/// You must keep the input points along with this class. +template +struct UnsafeKdTree { +public: + using Projection = Projection_; + using Node = KdTreeNode; + + /// @brief Constructor + /// @param points Point cloud + /// @param builder Kd-tree builder + template + explicit UnsafeKdTree(const PointCloud& points, const Builder& builder = KdTreeBuilder()) : points(points) { + if (frame::size(points) == 0) { + std::cerr << "warning: Empty point cloud" << std::endl; + return; + } + + builder.build_tree(*this, points); + } + + /// @brief Find the nearest neighbor. + /// @param query Query point + /// @param k_indices Index of the nearest neighbor + /// @param k_sq_dists Squared distance to the nearest neighbor + /// @param setting KNN search setting + /// @return Number of found neighbors (0 or 1) + size_t nearest_neighbor_search(const Eigen::Vector3d& query_, size_t* k_indices, double* k_sq_dists, const KnnSetting& setting = KnnSetting()) + const { + const Eigen::Vector4d query = (Eigen::Vector4d() << query_, 1.0).finished(); + return knn_search<1>(query, k_indices, k_sq_dists, setting); + } + + /// @brief Find k-nearest neighbors. This method uses dynamic memory allocation. + /// @param query Query point + /// @param k Number of neighbors + /// @param k_indices Indices of neighbors + /// @param k_sq_dists Squared distances to neighbors + /// @param setting KNN search setting + /// @return Number of found neighbors + size_t knn_search(const Eigen::Vector3d& query_, int k, size_t* k_indices, double* k_sq_dists, const KnnSetting& setting = KnnSetting()) const { + const Eigen::Vector4d query = (Eigen::Vector4d() << query_, 1.0).finished(); + KnnResult<-1> result(k_indices, k_sq_dists, k); + knn_search(query, root, result, setting); + return result.num_found(); + } + + /// @brief Find k-nearest neighbors. This method uses fixed and static memory allocation. Might be faster for small k. + /// @param query Query point + /// @param k_indices Indices of neighbors + /// @param k_sq_dists Squared distances to neighbors + /// @param setting KNN search setting + /// @return Number of found neighbors + template + size_t knn_search(const Eigen::Vector3d& query_, size_t* k_indices, double* k_sq_dists, const KnnSetting& setting = KnnSetting()) const { + const Eigen::Vector4d query = (Eigen::Vector4d() << query_, 1.0).finished(); + KnnResult result(k_indices, k_sq_dists); + knn_search(query, root, result, setting); + return result.num_found(); + } + +private: + /// @brief Find k-nearest neighbors. + template + bool knn_search(const Eigen::Vector4d& query, NodeIndexType node_index, Result& result, const KnnSetting& setting) const { + const auto& node = nodes[node_index]; + + // Check if it's a leaf node. + if (node.left == INVALID_NODE) { + // Compare the query point with all points in the leaf node. + for (size_t i = node.node_type.lr.first; i < node.node_type.lr.last; i++) { + const double sq_dist = (frame::point(points, indices[i]) - query).squaredNorm(); + result.push(indices[i], sq_dist); + } + return !setting.fulfilled(result); + } + + const double val = node.node_type.sub.proj(query); + const double diff = val - node.node_type.sub.thresh; + const double cut_sq_dist = diff * diff; + + NodeIndexType best_child; + NodeIndexType other_child; + + if (diff < 0.0) { + best_child = node.left; + other_child = node.right; + } else { + best_child = node.right; + other_child = node.left; + } + + // Check the best child node first. + if (!knn_search(query, best_child, result, setting)) { + return false; + } + + // Check if the other child node needs to be tested. + if (result.worst_distance() > cut_sq_dist) { + return knn_search(query, other_child, result, setting); + } + + return true; + } + +public: + const PointCloud& points; ///< Input points + std::vector indices; ///< Point indices refered by nodes + + NodeIndexType root; ///< Root node index (should be zero) + std::vector nodes; ///< Kd-tree nodes +}; + +} // namespace gtsam_points diff --git a/include/gtsam_ext/cuda/check_error.cuh b/include/gtsam_points/cuda/check_error.cuh similarity index 84% rename from include/gtsam_ext/cuda/check_error.cuh rename to include/gtsam_points/cuda/check_error.cuh index b3b2bd17..1ba75fbc 100644 --- a/include/gtsam_ext/cuda/check_error.cuh +++ b/include/gtsam_points/cuda/check_error.cuh @@ -8,7 +8,7 @@ #include -namespace gtsam_ext { +namespace gtsam_points { class CUDACheckError { public: @@ -17,4 +17,4 @@ public: extern CUDACheckError check_error; -} // namespace gtsam_ext \ No newline at end of file +} // namespace gtsam_points \ No newline at end of file diff --git a/include/gtsam_ext/cuda/check_error_curand.cuh b/include/gtsam_points/cuda/check_error_curand.cuh similarity index 87% rename from include/gtsam_ext/cuda/check_error_curand.cuh rename to include/gtsam_points/cuda/check_error_curand.cuh index e845b750..2fc3185c 100644 --- a/include/gtsam_ext/cuda/check_error_curand.cuh +++ b/include/gtsam_points/cuda/check_error_curand.cuh @@ -6,7 +6,7 @@ #include #include -namespace gtsam_ext { +namespace gtsam_points { class CurandCheckError { public: @@ -20,4 +20,4 @@ public: extern CurandCheckError check_curand; -} // namespace gtsam_ext \ No newline at end of file +} // namespace gtsam_points \ No newline at end of file diff --git a/include/gtsam_ext/cuda/check_error_cusolver.cuh b/include/gtsam_points/cuda/check_error_cusolver.cuh similarity index 87% rename from include/gtsam_ext/cuda/check_error_cusolver.cuh rename to include/gtsam_points/cuda/check_error_cusolver.cuh index 81fed3fb..8b008531 100644 --- a/include/gtsam_ext/cuda/check_error_cusolver.cuh +++ b/include/gtsam_points/cuda/check_error_cusolver.cuh @@ -6,7 +6,7 @@ #include #include -namespace gtsam_ext { +namespace gtsam_points { class CusolverCheckError { public: @@ -20,4 +20,4 @@ public: extern CusolverCheckError check_cusolver; -} // namespace gtsam_ext \ No newline at end of file +} // namespace gtsam_points \ No newline at end of file diff --git a/include/gtsam_ext/cuda/cuda_buffer.hpp b/include/gtsam_points/cuda/cuda_buffer.hpp similarity index 82% rename from include/gtsam_ext/cuda/cuda_buffer.hpp rename to include/gtsam_points/cuda/cuda_buffer.hpp index b9a07dc2..3288af0c 100644 --- a/include/gtsam_ext/cuda/cuda_buffer.hpp +++ b/include/gtsam_points/cuda/cuda_buffer.hpp @@ -7,12 +7,12 @@ struct CUstream_st; -namespace gtsam_ext { +namespace gtsam_points { /** * @brief Device buffer for asynchronous data transfer. * @note To enable asynchronous upload/download, use_pinned_buffer needs to be true. -*/ + */ class CUDABuffer { public: CUDABuffer(bool use_pinned_buffer = true); @@ -23,22 +23,35 @@ class CUDABuffer { * doesn't shrink them when buffer_size < size. * @param size Buffer size * @param stream CUDA stream - */ + */ void resize(size_t size, CUstream_st* stream); + /** + * @brief Upload data from the host pinned buffer to the device buffer. + * @param stream CUDA stream + */ + void upload(CUstream_st* stream); + + /** + * @brief Upload data from the host pinned buffer to the device buffer. + * @param size Data size (must be smaller than buffer_size) + * @param stream CUDA stream + */ + void upload(size_t size, CUstream_st* stream); + /** * @brief Upload data to the device buffer. If size > buffer_size, * the buffers will be resized before uploading. * @param buffer Input data * @param size Buffer size * @param stream CUDA stream - */ + */ void upload(const void* buffer, size_t size, CUstream_st* stream); /** * @brief Download data from the device buffer to the pinned host buffer. * @param stream CUDA stream - */ + */ void download(CUstream_st* stream); /** @@ -51,14 +64,14 @@ class CUDABuffer { /** * @brief Buffer size. - */ + */ size_t size() const; /** * @brief Pinned host buffer. * @note If use_pinned_buffer is false, the pinned host buffer * will not be allocated, and this method returns nullptr. - */ + */ void* host_buffer(); /** @@ -91,4 +104,4 @@ class CUDABuffer { void* d_buffer; }; -} // namespace gtsam_ext +} // namespace gtsam_points diff --git a/include/gtsam_ext/cuda/cuda_device_prop.hpp b/include/gtsam_points/cuda/cuda_device_prop.hpp similarity index 88% rename from include/gtsam_ext/cuda/cuda_device_prop.hpp rename to include/gtsam_points/cuda/cuda_device_prop.hpp index 829df43e..286c2ad7 100644 --- a/include/gtsam_ext/cuda/cuda_device_prop.hpp +++ b/include/gtsam_points/cuda/cuda_device_prop.hpp @@ -6,7 +6,7 @@ #include #include -namespace gtsam_ext { +namespace gtsam_points { std::vector cuda_device_names(); diff --git a/include/gtsam_ext/cuda/cuda_device_sync.hpp b/include/gtsam_points/cuda/cuda_device_sync.hpp similarity index 88% rename from include/gtsam_ext/cuda/cuda_device_sync.hpp rename to include/gtsam_points/cuda/cuda_device_sync.hpp index ec9d30a4..0db0c4e2 100644 --- a/include/gtsam_ext/cuda/cuda_device_sync.hpp +++ b/include/gtsam_points/cuda/cuda_device_sync.hpp @@ -3,7 +3,7 @@ #pragma once -namespace gtsam_ext { +namespace gtsam_points { /** * @brief Synchronize all CUDA devices diff --git a/include/gtsam_ext/cuda/cuda_graph.cuh b/include/gtsam_points/cuda/cuda_graph.cuh similarity index 78% rename from include/gtsam_ext/cuda/cuda_graph.cuh rename to include/gtsam_points/cuda/cuda_graph.cuh index 467248d5..409a1d3b 100644 --- a/include/gtsam_ext/cuda/cuda_graph.cuh +++ b/include/gtsam_points/cuda/cuda_graph.cuh @@ -4,14 +4,14 @@ #pragma once #include -#include -#include -#include +#include +#include +#include struct CUgraph_st; struct CUgraphNode_st; -namespace gtsam_ext { +namespace gtsam_points { class CUDAGraph { public: CUDAGraph(); @@ -22,7 +22,7 @@ public: template CUgraphNode_st* add_kernel(const Func&& func) { - gtsam_ext::CUDAStream stream; + gtsam_points::CUDAStream stream; CUgraph_st* sub_graph; check_error << cudaStreamBeginCapture(stream, cudaStreamCaptureModeGlobal); @@ -44,4 +44,4 @@ private: CUgraph_st* graph; }; -} // namespace gtsam_ext +} // namespace gtsam_points diff --git a/include/gtsam_ext/cuda/cuda_graph_exec.hpp b/include/gtsam_points/cuda/cuda_graph_exec.hpp similarity index 89% rename from include/gtsam_ext/cuda/cuda_graph_exec.hpp rename to include/gtsam_points/cuda/cuda_graph_exec.hpp index d1efdb99..99222ea6 100644 --- a/include/gtsam_ext/cuda/cuda_graph_exec.hpp +++ b/include/gtsam_points/cuda/cuda_graph_exec.hpp @@ -7,7 +7,7 @@ struct CUgraph_st; struct CUgraphExec_st; struct CUstream_st; -namespace gtsam_ext { +namespace gtsam_points { class CUDAGraphExec { public: @@ -23,4 +23,4 @@ class CUDAGraphExec { CUgraphExec_st* instance; }; -} // namespace gtsam_ext +} // namespace gtsam_points diff --git a/include/gtsam_ext/cuda/cuda_malloc_async.hpp b/include/gtsam_points/cuda/cuda_malloc_async.hpp similarity index 74% rename from include/gtsam_ext/cuda/cuda_malloc_async.hpp rename to include/gtsam_points/cuda/cuda_malloc_async.hpp index db9585b9..f3805c69 100644 --- a/include/gtsam_ext/cuda/cuda_malloc_async.hpp +++ b/include/gtsam_points/cuda/cuda_malloc_async.hpp @@ -1,3 +1,6 @@ +// SPDX-License-Identifier: MIT +// Copyright (c) 2021 Kenji Koide (k.koide@aist.go.jp) + #pragma once #if (CUDA_VERSION < 11000) diff --git a/include/gtsam_ext/cuda/cuda_memory.hpp b/include/gtsam_points/cuda/cuda_memory.hpp similarity index 98% rename from include/gtsam_ext/cuda/cuda_memory.hpp rename to include/gtsam_points/cuda/cuda_memory.hpp index 01876509..867969a4 100644 --- a/include/gtsam_ext/cuda/cuda_memory.hpp +++ b/include/gtsam_points/cuda/cuda_memory.hpp @@ -7,7 +7,7 @@ struct CUstream_st; -namespace gtsam_ext { +namespace gtsam_points { // Simple wrapper functions to manipulate objects on GPU memory from non-CUDA code @@ -85,4 +85,4 @@ void cuda_upload(T* dst, const T* src, CUstream_st* stream = nullptr) { cuda_host_to_device(dst, src, sizeof(T), stream); } -} // namespace gtsam_ext +} // namespace gtsam_points diff --git a/include/gtsam_ext/cuda/cuda_stream.hpp b/include/gtsam_points/cuda/cuda_stream.hpp similarity index 94% rename from include/gtsam_ext/cuda/cuda_stream.hpp rename to include/gtsam_points/cuda/cuda_stream.hpp index 73bc883c..1370d7e7 100644 --- a/include/gtsam_ext/cuda/cuda_stream.hpp +++ b/include/gtsam_points/cuda/cuda_stream.hpp @@ -7,7 +7,7 @@ struct CUstream_st; -namespace gtsam_ext { +namespace gtsam_points { struct CUDAStream { public: @@ -44,4 +44,4 @@ struct RegisteredMemory { void* ptr; }; -} // namespace gtsam_ext \ No newline at end of file +} // namespace gtsam_points \ No newline at end of file diff --git a/include/gtsam_ext/cuda/gl_buffer_map.hpp b/include/gtsam_points/cuda/gl_buffer_map.hpp similarity index 73% rename from include/gtsam_ext/cuda/gl_buffer_map.hpp rename to include/gtsam_points/cuda/gl_buffer_map.hpp index 71e89484..6ad96081 100644 --- a/include/gtsam_ext/cuda/gl_buffer_map.hpp +++ b/include/gtsam_points/cuda/gl_buffer_map.hpp @@ -1,10 +1,13 @@ +// SPDX-License-Identifier: MIT +// Copyright (c) 2021 Kenji Koide (k.koide@aist.go.jp) + #pragma once #include struct cudaGraphicsResource; -namespace gtsam_ext { +namespace gtsam_points { class GLBufferMap { public: @@ -23,4 +26,4 @@ class GLBufferMap { void* d_buffer; }; -} // namespace gtsam_ext +} // namespace gtsam_points diff --git a/include/gtsam_ext/cuda/kernels/inlier_access_kernel.cuh b/include/gtsam_points/cuda/kernels/inlier_access_kernel.cuh similarity index 80% rename from include/gtsam_ext/cuda/kernels/inlier_access_kernel.cuh rename to include/gtsam_points/cuda/kernels/inlier_access_kernel.cuh index 1cd0df4d..0b28d643 100644 --- a/include/gtsam_ext/cuda/kernels/inlier_access_kernel.cuh +++ b/include/gtsam_points/cuda/kernels/inlier_access_kernel.cuh @@ -1,8 +1,11 @@ +// SPDX-License-Identifier: MIT +// Copyright (c) 2021 Kenji Koide (k.koide@aist.go.jp) + #pragma once #include -namespace gtsam_ext { +namespace gtsam_points { template struct inlier_access_kernel { @@ -22,4 +25,4 @@ struct inlier_access_kernel { const int* source_inliers; }; -} // namespace gtsam_ext \ No newline at end of file +} // namespace gtsam_points \ No newline at end of file diff --git a/include/gtsam_ext/cuda/kernels/linearized_system.cuh b/include/gtsam_points/cuda/kernels/linearized_system.cuh similarity index 97% rename from include/gtsam_ext/cuda/kernels/linearized_system.cuh rename to include/gtsam_points/cuda/kernels/linearized_system.cuh index 70f5915e..7c97f16c 100644 --- a/include/gtsam_ext/cuda/kernels/linearized_system.cuh +++ b/include/gtsam_points/cuda/kernels/linearized_system.cuh @@ -6,7 +6,7 @@ #include #include -namespace gtsam_ext { +namespace gtsam_points { struct LinearizedSystem6 { public: @@ -66,4 +66,4 @@ public: Eigen::Matrix b_source; }; -} // namespace gtsam_ext \ No newline at end of file +} // namespace gtsam_points \ No newline at end of file diff --git a/include/gtsam_ext/cuda/kernels/lookup_voxels.cuh b/include/gtsam_points/cuda/kernels/lookup_voxels.cuh similarity index 95% rename from include/gtsam_ext/cuda/kernels/lookup_voxels.cuh rename to include/gtsam_points/cuda/kernels/lookup_voxels.cuh index e0b41297..6d9b0681 100644 --- a/include/gtsam_ext/cuda/kernels/lookup_voxels.cuh +++ b/include/gtsam_points/cuda/kernels/lookup_voxels.cuh @@ -10,10 +10,10 @@ #include #include -#include -#include +#include +#include -namespace gtsam_ext { +namespace gtsam_points { template struct lookup_voxels_kernel { @@ -101,4 +101,4 @@ struct invalid_correspondence_kernel { } }; -} // namespace gtsam_ext +} // namespace gtsam_points diff --git a/include/gtsam_ext/cuda/kernels/pose.cuh b/include/gtsam_points/cuda/kernels/pose.cuh similarity index 93% rename from include/gtsam_ext/cuda/kernels/pose.cuh rename to include/gtsam_points/cuda/kernels/pose.cuh index 61f8325a..3af3edce 100644 --- a/include/gtsam_ext/cuda/kernels/pose.cuh +++ b/include/gtsam_points/cuda/kernels/pose.cuh @@ -7,7 +7,7 @@ #include #include -namespace gtsam_ext { +namespace gtsam_points { inline __host__ __device__ Eigen::Matrix3f skew_symmetric(const Eigen::Vector3f& x) { Eigen::Matrix3f skew = Eigen::Matrix3f::Zero(); @@ -26,4 +26,4 @@ inline __host__ bool large_displacement(const Eigen::Isometry3f& x_p, const Eige return std::abs(Eigen::AngleAxisf(delta.linear()).angle()) > angle_eps || delta.translation().norm() > trans_eps; } -} // namespace gtsam_ext +} // namespace gtsam_points diff --git a/include/gtsam_ext/cuda/kernels/untie.cuh b/include/gtsam_points/cuda/kernels/untie.cuh similarity index 92% rename from include/gtsam_ext/cuda/kernels/untie.cuh rename to include/gtsam_points/cuda/kernels/untie.cuh index 8c28947d..d2f03a5f 100644 --- a/include/gtsam_ext/cuda/kernels/untie.cuh +++ b/include/gtsam_points/cuda/kernels/untie.cuh @@ -5,7 +5,7 @@ #include -namespace gtsam_ext { +namespace gtsam_points { template struct untie_pair_first { @@ -22,4 +22,4 @@ struct untie_tuple { __device__ auto operator()(const Tuple& tuple) const -> decltype(thrust::get(tuple)) { return thrust::get(tuple); } }; -} // namespace gtsam_ext \ No newline at end of file +} // namespace gtsam_points \ No newline at end of file diff --git a/include/gtsam_ext/cuda/kernels/vector3_hash.cuh b/include/gtsam_points/cuda/kernels/vector3_hash.cuh similarity index 83% rename from include/gtsam_ext/cuda/kernels/vector3_hash.cuh rename to include/gtsam_points/cuda/kernels/vector3_hash.cuh index 55458ca5..5cb980d8 100644 --- a/include/gtsam_ext/cuda/kernels/vector3_hash.cuh +++ b/include/gtsam_points/cuda/kernels/vector3_hash.cuh @@ -6,7 +6,7 @@ #include #include -namespace gtsam_ext { +namespace gtsam_points { // taken from boost/hash.hpp inline __host__ __device__ void hash_combine(uint64_t& h, uint64_t k) { @@ -36,9 +36,14 @@ inline __host__ __device__ uint64_t vector3i_hash(const Eigen::Vector3i& x) { return seed; } +inline __host__ __device__ Eigen::Array3i fast_floor(const Eigen::Array3f& pt) { + const Eigen::Array3i ncoord = pt.cast(); + return ncoord - (pt < ncoord.cast()).cast(); +}; + // real vector -> voxel index vector inline __host__ __device__ Eigen::Vector3i calc_voxel_coord(const Eigen::Vector3f& x, float resolution) { - Eigen::Vector3i coord = (x.array() / resolution - 0.5).floor().cast(); + Eigen::Vector3i coord = fast_floor(x.array() / resolution).cast(); return coord; } @@ -67,4 +72,4 @@ inline __host__ __device__ int lookup_voxel( return -1; } -} // namespace gtsam_ext +} // namespace gtsam_points diff --git a/include/gtsam_ext/cuda/kernels/vgicp_derivatives.cuh b/include/gtsam_points/cuda/kernels/vgicp_derivatives.cuh similarity index 96% rename from include/gtsam_ext/cuda/kernels/vgicp_derivatives.cuh rename to include/gtsam_points/cuda/kernels/vgicp_derivatives.cuh index 3e3cea27..4cd7a6b8 100644 --- a/include/gtsam_ext/cuda/kernels/vgicp_derivatives.cuh +++ b/include/gtsam_points/cuda/kernels/vgicp_derivatives.cuh @@ -6,11 +6,11 @@ #include #include -#include -#include -#include +#include +#include +#include -namespace gtsam_ext { +namespace gtsam_points { struct vgicp_derivatives_kernel { vgicp_derivatives_kernel( @@ -137,4 +137,4 @@ struct vgicp_error_kernel { thrust::device_ptr source_covs_ptr; }; -} // namespace gtsam_ext +} // namespace gtsam_points diff --git a/include/gtsam_ext/cuda/nonlinear_factor_set_gpu.hpp b/include/gtsam_points/cuda/nonlinear_factor_set_gpu.hpp similarity index 95% rename from include/gtsam_ext/cuda/nonlinear_factor_set_gpu.hpp rename to include/gtsam_points/cuda/nonlinear_factor_set_gpu.hpp index 43d419ca..adfada42 100644 --- a/include/gtsam_ext/cuda/nonlinear_factor_set_gpu.hpp +++ b/include/gtsam_points/cuda/nonlinear_factor_set_gpu.hpp @@ -13,12 +13,12 @@ #include #include -#include -#include +#include +#include struct CUstream_st; -namespace gtsam_ext { +namespace gtsam_points { /** * @brief This class holds a set of GPU-based NonlinearFactors and manages their linearization and cost evaluation tasks @@ -107,4 +107,4 @@ class NonlinearFactorSetGPU : public NonlinearFactorSet { thrust::device_vector evaluation_output_buffer_gpu; }; -} // namespace gtsam_ext +} // namespace gtsam_points diff --git a/include/gtsam_ext/cuda/nonlinear_factor_set_gpu_create.hpp b/include/gtsam_points/cuda/nonlinear_factor_set_gpu_create.hpp similarity index 61% rename from include/gtsam_ext/cuda/nonlinear_factor_set_gpu_create.hpp rename to include/gtsam_points/cuda/nonlinear_factor_set_gpu_create.hpp index ffcb7783..bbd1b575 100644 --- a/include/gtsam_ext/cuda/nonlinear_factor_set_gpu_create.hpp +++ b/include/gtsam_points/cuda/nonlinear_factor_set_gpu_create.hpp @@ -3,10 +3,10 @@ #pragma once -#include +#include -namespace gtsam_ext { +namespace gtsam_points { std::shared_ptr create_nonlinear_factor_set_gpu(); -} // namespace gtsam_ext +} // namespace gtsam_points diff --git a/include/gtsam_ext/cuda/stream_roundrobin.hpp b/include/gtsam_points/cuda/stream_roundrobin.hpp similarity index 89% rename from include/gtsam_ext/cuda/stream_roundrobin.hpp rename to include/gtsam_points/cuda/stream_roundrobin.hpp index 4fb7fc44..7b9f9dc6 100644 --- a/include/gtsam_ext/cuda/stream_roundrobin.hpp +++ b/include/gtsam_points/cuda/stream_roundrobin.hpp @@ -8,7 +8,7 @@ struct CUstream_st; -namespace gtsam_ext { +namespace gtsam_points { /** * @brief Roundrobin for CUDA streams @@ -29,4 +29,4 @@ class StreamRoundRobin { std::vector streams; }; -} // namespace gtsam_ext \ No newline at end of file +} // namespace gtsam_points \ No newline at end of file diff --git a/include/gtsam_ext/cuda/stream_temp_buffer_roundrobin.hpp b/include/gtsam_points/cuda/stream_temp_buffer_roundrobin.hpp similarity index 93% rename from include/gtsam_ext/cuda/stream_temp_buffer_roundrobin.hpp rename to include/gtsam_points/cuda/stream_temp_buffer_roundrobin.hpp index 2efdf527..4bd09b7d 100644 --- a/include/gtsam_ext/cuda/stream_temp_buffer_roundrobin.hpp +++ b/include/gtsam_points/cuda/stream_temp_buffer_roundrobin.hpp @@ -7,7 +7,7 @@ #include #include -#include +#include // forward declaration namespace thrust { @@ -20,7 +20,7 @@ class device_vector; } // namespace thrust -namespace gtsam_ext { +namespace gtsam_points { /** * @brief Temporary buffer manager @@ -64,4 +64,4 @@ class StreamTempBufferRoundRobin { std::unordered_map buffer_map; }; -} // namespace gtsam_ext \ No newline at end of file +} // namespace gtsam_points \ No newline at end of file diff --git a/include/gtsam_ext/factors/balm_feature.hpp b/include/gtsam_points/factors/balm_feature.hpp similarity index 95% rename from include/gtsam_ext/factors/balm_feature.hpp rename to include/gtsam_points/factors/balm_feature.hpp index 85fb7b22..74c87d87 100644 --- a/include/gtsam_ext/factors/balm_feature.hpp +++ b/include/gtsam_points/factors/balm_feature.hpp @@ -5,7 +5,7 @@ #include -namespace gtsam_ext { +namespace gtsam_points { /** * @brief Utility class to calculate eigenvalues and derivatives of those @@ -19,7 +19,7 @@ struct BALMFeature { * @brief Constructor * @param points Points in each sensor coordinate (untransformed points) */ - BALMFeature(const std::vector>& points) { + BALMFeature(const std::vector& points) { Eigen::Vector3d sum_pts = Eigen::Vector3d::Zero(); Eigen::Matrix3d sum_cross = Eigen::Matrix3d::Zero(); for (const auto& pt : points) { @@ -106,4 +106,4 @@ struct BALMFeature { Eigen::Matrix3d eigenvectors; // }; -} // namespace gtsam_ext \ No newline at end of file +} // namespace gtsam_points \ No newline at end of file diff --git a/include/gtsam_ext/factors/bundle_adjustment_factor.hpp b/include/gtsam_points/factors/bundle_adjustment_factor.hpp similarity index 97% rename from include/gtsam_ext/factors/bundle_adjustment_factor.hpp rename to include/gtsam_points/factors/bundle_adjustment_factor.hpp index 0e0769a6..ed07c079 100644 --- a/include/gtsam_ext/factors/bundle_adjustment_factor.hpp +++ b/include/gtsam_points/factors/bundle_adjustment_factor.hpp @@ -6,7 +6,7 @@ #include #include -namespace gtsam_ext { +namespace gtsam_points { /** * @brief Base class of range-based bundle adjustment factors diff --git a/include/gtsam_ext/factors/bundle_adjustment_factor_evm.hpp b/include/gtsam_points/factors/bundle_adjustment_factor_evm.hpp similarity index 85% rename from include/gtsam_ext/factors/bundle_adjustment_factor_evm.hpp rename to include/gtsam_points/factors/bundle_adjustment_factor_evm.hpp index 711956e7..87cbb8f1 100644 --- a/include/gtsam_ext/factors/bundle_adjustment_factor_evm.hpp +++ b/include/gtsam_points/factors/bundle_adjustment_factor_evm.hpp @@ -7,9 +7,9 @@ #include #include #include -#include +#include -namespace gtsam_ext { +namespace gtsam_points { struct BALMFeature; @@ -51,17 +51,16 @@ class EVMBundleAdjustmentFactorBase : public BundleAdjustmentFactorBase { protected: template - double calc_eigenvalue(const std::vector>& transed_points, Eigen::MatrixXd* H = nullptr, Eigen::MatrixXd* J = nullptr) - const; + double calc_eigenvalue(const std::vector& transed_points, Eigen::MatrixXd* H = nullptr, Eigen::MatrixXd* J = nullptr) const; - Eigen::MatrixXd calc_pose_derivatives(const std::vector>& transed_points) const; + Eigen::MatrixXd calc_pose_derivatives(const std::vector& transed_points) const; gtsam::GaussianFactor::shared_ptr compose_factor(const Eigen::MatrixXd& H, const Eigen::MatrixXd& J, double error) const; protected: double error_scale; std::vector keys; - std::vector> points; + std::vector points; std::unordered_map key_index; }; @@ -94,4 +93,4 @@ class EdgeEVMFactor : public EVMBundleAdjustmentFactorBase { virtual double error(const gtsam::Values& c) const override; virtual boost::shared_ptr linearize(const gtsam::Values& c) const override; }; -} // namespace gtsam_ext \ No newline at end of file +} // namespace gtsam_points \ No newline at end of file diff --git a/include/gtsam_ext/factors/bundle_adjustment_factor_lsq.hpp b/include/gtsam_points/factors/bundle_adjustment_factor_lsq.hpp similarity index 93% rename from include/gtsam_ext/factors/bundle_adjustment_factor_lsq.hpp rename to include/gtsam_points/factors/bundle_adjustment_factor_lsq.hpp index ca6cfcbb..61198987 100644 --- a/include/gtsam_ext/factors/bundle_adjustment_factor_lsq.hpp +++ b/include/gtsam_points/factors/bundle_adjustment_factor_lsq.hpp @@ -3,9 +3,9 @@ #pragma once -#include +#include -namespace gtsam_ext { +namespace gtsam_points { /** * @brief Bundle adjustment factor based on EVM and EF optimal condition satisfaction @@ -46,4 +46,4 @@ class LsqBundleAdjustmentFactor : public BundleAdjustmentFactorBase { mutable Eigen::Vector3d global_normal; }; -} // namespace gtsam_ext +} // namespace gtsam_points diff --git a/include/gtsam_ext/factors/experimental/between_sim3_se3_factor.hpp b/include/gtsam_points/factors/experimental/between_sim3_se3_factor.hpp similarity index 97% rename from include/gtsam_ext/factors/experimental/between_sim3_se3_factor.hpp rename to include/gtsam_points/factors/experimental/between_sim3_se3_factor.hpp index 333fd440..5a4acbc3 100644 --- a/include/gtsam_ext/factors/experimental/between_sim3_se3_factor.hpp +++ b/include/gtsam_points/factors/experimental/between_sim3_se3_factor.hpp @@ -7,7 +7,7 @@ #include #include -namespace gtsam_ext { +namespace gtsam_points { gtsam::Pose3 scaled_transform(const gtsam::Similarity3& sim3, gtsam::OptionalJacobian<6, 7> H = boost::none) { if (H) { @@ -53,4 +53,4 @@ class BetweenSim3SE3Factor : public gtsam::NoiseModelFactor2 #include -#include +#include -namespace gtsam_ext { +namespace gtsam_points { class KdTree; @@ -31,7 +31,7 @@ class CTICPFactorExpr : public gtsam::NoiseModelFactor { CTICPFactorExpr( gtsam::Key source_t0_key, // source pose at the scan beginning gtsam::Key source_t1_key, // source pose at the scan ending - const std::shared_ptr& target, + const std::shared_ptr& target, const std::shared_ptr& target_tree, const double source_t0, // time of the very first point in source const double source_t1, // time of the very last point in source @@ -49,7 +49,7 @@ class CTICPFactorExpr : public gtsam::NoiseModelFactor { gtsam::Double_ calc_error() const; private: - const std::shared_ptr target; + const std::shared_ptr target; const std::shared_ptr target_tree; const double source_time; // source point time \in [0.0, 1.0] @@ -75,7 +75,7 @@ class IntegratedCTICPFactorExpr : public gtsam::NonlinearFactor { virtual double error(const gtsam::Values& values) const override; virtual boost::shared_ptr linearize(const gtsam::Values& values) const override; - std::vector> deskewed_source_points(const gtsam::Values& values) const; + std::vector deskewed_source_points(const gtsam::Values& values) const; private: gtsam::NonlinearFactorGraph::shared_ptr graph; @@ -84,8 +84,12 @@ class IntegratedCTICPFactorExpr : public gtsam::NonlinearFactor { /** * @brief Create a set of CT-ICP factors */ -gtsam::NonlinearFactorGraph::shared_ptr -create_cticp_factors(gtsam::Key source_t0_key, gtsam::Key source_t1_key, const Frame::ConstPtr& target, const Frame::ConstPtr& source, const gtsam::SharedNoiseModel& noise_model); +gtsam::NonlinearFactorGraph::shared_ptr create_cticp_factors( + gtsam::Key source_t0_key, + gtsam::Key source_t1_key, + const PointCloud::ConstPtr& target, + const PointCloud::ConstPtr& source, + const gtsam::SharedNoiseModel& noise_model); /** * @brief Create a nonlinear factor that wraps a set of CT-ICP factors @@ -93,8 +97,8 @@ create_cticp_factors(gtsam::Key source_t0_key, gtsam::Key source_t1_key, const F IntegratedCTICPFactorExpr::shared_ptr create_integrated_cticp_factor( gtsam::Key source_t0_key, gtsam::Key source_t1_key, - const Frame::ConstPtr& target, - const Frame::ConstPtr& source, + const PointCloud::ConstPtr& target, + const PointCloud::ConstPtr& source, const gtsam::SharedNoiseModel& noise_model); -} // namespace gtsam_ext \ No newline at end of file +} // namespace gtsam_points \ No newline at end of file diff --git a/include/gtsam_ext/factors/experimental/expression_icp_factor.hpp b/include/gtsam_points/factors/experimental/expression_icp_factor.hpp similarity index 79% rename from include/gtsam_ext/factors/experimental/expression_icp_factor.hpp rename to include/gtsam_points/factors/experimental/expression_icp_factor.hpp index 8b8aa000..497adb34 100644 --- a/include/gtsam_ext/factors/experimental/expression_icp_factor.hpp +++ b/include/gtsam_points/factors/experimental/expression_icp_factor.hpp @@ -5,9 +5,9 @@ #include #include -#include +#include -namespace gtsam_ext { +namespace gtsam_points { class KdTree; @@ -22,7 +22,7 @@ class ICPFactorExpr : public gtsam::NoiseModelFactor { ICPFactorExpr( gtsam::Key target_key, gtsam::Key source_key, - const std::shared_ptr& target, + const std::shared_ptr& target, const std::shared_ptr& target_tree, const gtsam::Point3& source, const gtsam::SharedNoiseModel& noise_model); @@ -36,7 +36,7 @@ class ICPFactorExpr : public gtsam::NoiseModelFactor { gtsam::Point3_ calc_error() const; private: - const std::shared_ptr target; + const std::shared_ptr target; const std::shared_ptr target_tree; const gtsam::Pose3_ delta; @@ -50,7 +50,7 @@ class ICPFactorExpr : public gtsam::NoiseModelFactor { * @brief Create a set of ICPFactorExpr */ gtsam::NonlinearFactorGraph::shared_ptr -create_icp_factors(gtsam::Key target_key, gtsam::Key source_key, const Frame::ConstPtr& target, const Frame::ConstPtr& source, const gtsam::SharedNoiseModel& noise_model); +create_icp_factors(gtsam::Key target_key, gtsam::Key source_key, const PointCloud::ConstPtr& target, const PointCloud::ConstPtr& source, const gtsam::SharedNoiseModel& noise_model); /** * @brief Create a nonlinear factor that wraps a set of ICP factors @@ -58,8 +58,8 @@ create_icp_factors(gtsam::Key target_key, gtsam::Key source_key, const Frame::Co gtsam::NonlinearFactor::shared_ptr create_integrated_icp_factor( gtsam::Key target_key, gtsam::Key source_key, - const Frame::ConstPtr& target, - const Frame::ConstPtr& source, + const PointCloud::ConstPtr& target, + const PointCloud::ConstPtr& source, const gtsam::SharedNoiseModel& noise_model); -} // namespace gtsam_ext \ No newline at end of file +} // namespace gtsam_points \ No newline at end of file diff --git a/include/gtsam_ext/factors/intensity_gradients_ivox.hpp b/include/gtsam_points/factors/experimental/intensity_gradients_ivox_.hpp similarity index 89% rename from include/gtsam_ext/factors/intensity_gradients_ivox.hpp rename to include/gtsam_points/factors/experimental/intensity_gradients_ivox_.hpp index 14056675..59d244a1 100644 --- a/include/gtsam_ext/factors/intensity_gradients_ivox.hpp +++ b/include/gtsam_points/factors/experimental/intensity_gradients_ivox_.hpp @@ -3,10 +3,10 @@ #pragma once -#include -#include +#include +#include -namespace gtsam_ext { +namespace gtsam_points { class IntensityGradientsiVox : public iVox { public: @@ -17,12 +17,12 @@ class IntensityGradientsiVox : public iVox { * @brief Insert points and all available attributes into the iVox * @param frame Input frame */ - virtual void insert(const Frame& frame) override; + virtual void insert(const PointCloud& frame) override; const Eigen::Vector4d& normal(const size_t i) const; const Eigen::Vector4d& intensity_gradient(const size_t i) const; - std::vector> voxel_normals() const override; + std::vector voxel_normals() const override; std::vector voxel_intensities() const; std::vector voxel_intensity_gradients() const; @@ -63,4 +63,4 @@ struct traits { } // namespace frame -} // namespace gtsam_ext \ No newline at end of file +} // namespace gtsam_points \ No newline at end of file diff --git a/include/gtsam_ext/factors/impl/integrated_color_consistency_factor_impl.hpp b/include/gtsam_points/factors/impl/integrated_color_consistency_factor_impl.hpp similarity index 92% rename from include/gtsam_ext/factors/impl/integrated_color_consistency_factor_impl.hpp rename to include/gtsam_points/factors/impl/integrated_color_consistency_factor_impl.hpp index 16687846..1d7b1de5 100644 --- a/include/gtsam_ext/factors/impl/integrated_color_consistency_factor_impl.hpp +++ b/include/gtsam_points/factors/impl/integrated_color_consistency_factor_impl.hpp @@ -1,12 +1,12 @@ // SPDX-License-Identifier: MIT // Copyright (c) 2021 Kenji Koide (k.koide@aist.go.jp) -#include +#include #include -#include +#include -namespace gtsam_ext { +namespace gtsam_points { template IntegratedColorConsistencyFactor_::IntegratedColorConsistencyFactor_( @@ -117,11 +117,11 @@ double IntegratedColorConsistencyFactor_, Eigen::aligned_allocator>> Hs_target; - std::vector, Eigen::aligned_allocator>> Hs_source; - std::vector, Eigen::aligned_allocator>> Hs_target_source; - std::vector, Eigen::aligned_allocator>> bs_target; - std::vector, Eigen::aligned_allocator>> bs_source; + std::vector> Hs_target; + std::vector> Hs_source; + std::vector> Hs_target_source; + std::vector> bs_target; + std::vector> bs_source; if (H_target && H_source && H_target_source && b_target && b_source) { Hs_target.resize(num_threads, Eigen::Matrix::Zero()); @@ -211,4 +211,4 @@ double IntegratedColorConsistencyFactor_ +#include #include -#include +#include -namespace gtsam_ext { +namespace gtsam_points { template IntegratedColoredGICPFactor_::IntegratedColoredGICPFactor_( @@ -131,11 +131,11 @@ double IntegratedColoredGICPFactor_, Eigen::aligned_allocator>> Hs_target; - std::vector, Eigen::aligned_allocator>> Hs_source; - std::vector, Eigen::aligned_allocator>> Hs_target_source; - std::vector, Eigen::aligned_allocator>> bs_target; - std::vector, Eigen::aligned_allocator>> bs_source; + std::vector> Hs_target; + std::vector> Hs_source; + std::vector> Hs_target_source; + std::vector> bs_target; + std::vector> bs_source; if (H_target && H_source && H_target_source && b_target && b_source) { Hs_target.resize(num_threads, Eigen::Matrix::Zero()); @@ -241,4 +241,4 @@ double IntegratedColoredGICPFactor_ +#include #include -#include +#include -namespace gtsam_ext { +namespace gtsam_points { template IntegratedCT_GICPFactor_::IntegratedCT_GICPFactor_( @@ -75,11 +75,11 @@ boost::shared_ptr IntegratedCT_GICPFactor_update_correspondences(); double sum_errors = 0.0; - std::vector> Hs_00(this->num_threads, gtsam::Matrix6::Zero()); - std::vector> Hs_01(this->num_threads, gtsam::Matrix6::Zero()); - std::vector> Hs_11(this->num_threads, gtsam::Matrix6::Zero()); - std::vector> bs_0(this->num_threads, gtsam::Vector6::Zero()); - std::vector> bs_1(this->num_threads, gtsam::Vector6::Zero()); + std::vector Hs_00(this->num_threads, gtsam::Matrix6::Zero()); + std::vector Hs_01(this->num_threads, gtsam::Matrix6::Zero()); + std::vector Hs_11(this->num_threads, gtsam::Matrix6::Zero()); + std::vector bs_0(this->num_threads, gtsam::Vector6::Zero()); + std::vector bs_1(this->num_threads, gtsam::Vector6::Zero()); gtsam::Matrix6 H_00 = gtsam::Matrix6::Zero(); gtsam::Matrix6 H_01 = gtsam::Matrix6::Zero(); @@ -170,12 +170,11 @@ void IntegratedCT_GICPFactor_::update_correspondences( const long target_index = this->correspondences[i]; const auto& cov_A = frame::cov(*this->source, i); const auto& cov_B = frame::cov(*this->target, target_index); - Eigen::Matrix4d RCR = (cov_B + pose * cov_A * pose.transpose()); - RCR(3, 3) = 1.0; + const Eigen::Matrix4d RCR = (cov_B + pose * cov_A * pose.transpose()); - mahalanobis[i] = RCR.inverse(); - mahalanobis[i](3, 3) = 0.0; + mahalanobis[i].setZero(); + mahalanobis[i].block<3, 3>(0, 0) = RCR.block<3, 3>(0, 0).inverse(); } } } -} // namespace gtsam_ext \ No newline at end of file +} // namespace gtsam_points \ No newline at end of file diff --git a/include/gtsam_ext/factors/impl/integrated_ct_icp_factor_impl.hpp b/include/gtsam_points/factors/impl/integrated_ct_icp_factor_impl.hpp similarity index 94% rename from include/gtsam_ext/factors/impl/integrated_ct_icp_factor_impl.hpp rename to include/gtsam_points/factors/impl/integrated_ct_icp_factor_impl.hpp index f8a06ca7..dd08d490 100644 --- a/include/gtsam_ext/factors/impl/integrated_ct_icp_factor_impl.hpp +++ b/include/gtsam_points/factors/impl/integrated_ct_icp_factor_impl.hpp @@ -1,14 +1,12 @@ // SPDX-License-Identifier: MIT // Copyright (c) 2021 Kenji Koide (k.koide@aist.go.jp) -#include +#include #include -#include +#include -#include - -namespace gtsam_ext { +namespace gtsam_points { template IntegratedCT_ICPFactor_::IntegratedCT_ICPFactor_( @@ -209,9 +207,7 @@ void IntegratedCT_ICPFactor_::update_correspondences() } template -std::vector> IntegratedCT_ICPFactor_::deskewed_source_points( - const gtsam::Values& values, - bool local) { +std::vector IntegratedCT_ICPFactor_::deskewed_source_points(const gtsam::Values& values, bool local) { update_poses(values); if (local) { @@ -220,7 +216,7 @@ std::vector> Integrat } } - std::vector> deskewed(frame::size(*source)); + std::vector deskewed(frame::size(*source)); for (int i = 0; i < frame::size(*source); i++) { const int time_index = time_indices[i]; const auto& pose = source_poses[time_index]; @@ -230,4 +226,4 @@ std::vector> Integrat return deskewed; } -} // namespace gtsam_ext \ No newline at end of file +} // namespace gtsam_points \ No newline at end of file diff --git a/include/gtsam_ext/factors/impl/integrated_gicp_factor_impl.hpp b/include/gtsam_points/factors/impl/integrated_gicp_factor_impl.hpp similarity index 90% rename from include/gtsam_ext/factors/impl/integrated_gicp_factor_impl.hpp rename to include/gtsam_points/factors/impl/integrated_gicp_factor_impl.hpp index e3734ef9..7f4b11e7 100644 --- a/include/gtsam_ext/factors/impl/integrated_gicp_factor_impl.hpp +++ b/include/gtsam_points/factors/impl/integrated_gicp_factor_impl.hpp @@ -1,14 +1,14 @@ // SPDX-License-Identifier: MIT // Copyright (c) 2021 Kenji Koide (k.koide@aist.go.jp) -#include +#include #include #include -#include -#include +#include +#include -namespace gtsam_ext { +namespace gtsam_points { template IntegratedGICPFactor_::IntegratedGICPFactor_( @@ -17,7 +17,7 @@ IntegratedGICPFactor_::IntegratedGICPFactor_( const std::shared_ptr& target, const std::shared_ptr& source, const std::shared_ptr& target_tree) -: gtsam_ext::IntegratedMatchingCostFactor(target_key, source_key), +: gtsam_points::IntegratedMatchingCostFactor(target_key, source_key), num_threads(1), max_correspondence_distance_sq(1.0), correspondence_update_tolerance_rot(0.0), @@ -57,7 +57,7 @@ IntegratedGICPFactor_::IntegratedGICPFactor_( const std::shared_ptr& target, const std::shared_ptr& source, const std::shared_ptr& target_tree) -: gtsam_ext::IntegratedMatchingCostFactor(fixed_target_pose, source_key), +: gtsam_points::IntegratedMatchingCostFactor(fixed_target_pose, source_key), num_threads(1), max_correspondence_distance_sq(1.0), correspondence_update_tolerance_rot(0.0), @@ -150,11 +150,11 @@ double IntegratedGICPFactor_::evaluate( // double sum_errors = 0.0; - std::vector, Eigen::aligned_allocator>> Hs_target; - std::vector, Eigen::aligned_allocator>> Hs_source; - std::vector, Eigen::aligned_allocator>> Hs_target_source; - std::vector, Eigen::aligned_allocator>> bs_target; - std::vector, Eigen::aligned_allocator>> bs_source; + std::vector> Hs_target; + std::vector> Hs_source; + std::vector> Hs_target_source; + std::vector> bs_target; + std::vector> bs_source; if (H_target && H_source && H_target_source && b_target && b_source) { Hs_target.resize(num_threads, Eigen::Matrix::Zero()); @@ -228,4 +228,4 @@ double IntegratedGICPFactor_::evaluate( return sum_errors; } -} // namespace gtsam_ext +} // namespace gtsam_points diff --git a/include/gtsam_ext/factors/impl/integrated_icp_factor_impl.hpp b/include/gtsam_points/factors/impl/integrated_icp_factor_impl.hpp similarity index 86% rename from include/gtsam_ext/factors/impl/integrated_icp_factor_impl.hpp rename to include/gtsam_points/factors/impl/integrated_icp_factor_impl.hpp index c912151e..bc84c541 100644 --- a/include/gtsam_ext/factors/impl/integrated_icp_factor_impl.hpp +++ b/include/gtsam_points/factors/impl/integrated_icp_factor_impl.hpp @@ -1,16 +1,15 @@ // SPDX-License-Identifier: MIT // Copyright (c) 2021 Kenji Koide (k.koide@aist.go.jp) -#include +#include -#include #include #include -#include +#include -#include +#include -namespace gtsam_ext { +namespace gtsam_points { template IntegratedICPFactor_::IntegratedICPFactor_( @@ -20,7 +19,7 @@ IntegratedICPFactor_::IntegratedICPFactor_( const std::shared_ptr& source, const std::shared_ptr& target_tree, bool use_point_to_plane) -: gtsam_ext::IntegratedMatchingCostFactor(target_key, source_key), +: gtsam_points::IntegratedMatchingCostFactor(target_key, source_key), num_threads(1), max_correspondence_distance_sq(1.0), use_point_to_plane(use_point_to_plane), @@ -53,7 +52,7 @@ IntegratedICPFactor_::IntegratedICPFactor_( const std::shared_ptr& target, const std::shared_ptr& source, bool use_point_to_plane) -: gtsam_ext::IntegratedICPFactor_(target_key, source_key, target, source, nullptr, use_point_to_plane) {} +: gtsam_points::IntegratedICPFactor_(target_key, source_key, target, source, nullptr, use_point_to_plane) {} template IntegratedICPFactor_::IntegratedICPFactor_( @@ -63,7 +62,7 @@ IntegratedICPFactor_::IntegratedICPFactor_( const std::shared_ptr& source, const std::shared_ptr& target_tree, bool use_point_to_plane) -: gtsam_ext::IntegratedMatchingCostFactor(fixed_target_pose, source_key), +: gtsam_points::IntegratedMatchingCostFactor(fixed_target_pose, source_key), num_threads(1), max_correspondence_distance_sq(1.0), use_point_to_plane(use_point_to_plane), @@ -96,7 +95,7 @@ IntegratedICPFactor_::IntegratedICPFactor_( const std::shared_ptr& target, const std::shared_ptr& source, bool use_point_to_plane) -: gtsam_ext::IntegratedICPFactor_(fixed_target_pose, source_key, target, source, nullptr, use_point_to_plane) {} +: gtsam_points::IntegratedICPFactor_(fixed_target_pose, source_key, target, source, nullptr, use_point_to_plane) {} template IntegratedICPFactor_::~IntegratedICPFactor_() {} @@ -148,11 +147,11 @@ double IntegratedICPFactor_::evaluate( // double sum_errors = 0.0; - std::vector, Eigen::aligned_allocator>> Hs_target; - std::vector, Eigen::aligned_allocator>> Hs_source; - std::vector, Eigen::aligned_allocator>> Hs_target_source; - std::vector, Eigen::aligned_allocator>> bs_target; - std::vector, Eigen::aligned_allocator>> bs_source; + std::vector> Hs_target; + std::vector> Hs_source; + std::vector> Hs_target_source; + std::vector> bs_target; + std::vector> bs_source; if (H_target && H_source && H_target_source && b_target && b_source) { Hs_target.resize(num_threads, Eigen::Matrix::Zero()); @@ -231,4 +230,4 @@ double IntegratedICPFactor_::evaluate( return sum_errors; } -} // namespace gtsam_ext +} // namespace gtsam_points diff --git a/include/gtsam_ext/factors/impl/integrated_loam_factor_impl.hpp b/include/gtsam_points/factors/impl/integrated_loam_factor_impl.hpp similarity index 90% rename from include/gtsam_ext/factors/impl/integrated_loam_factor_impl.hpp rename to include/gtsam_points/factors/impl/integrated_loam_factor_impl.hpp index 3c0caf48..68a0c276 100644 --- a/include/gtsam_ext/factors/impl/integrated_loam_factor_impl.hpp +++ b/include/gtsam_points/factors/impl/integrated_loam_factor_impl.hpp @@ -1,12 +1,12 @@ // SPDX-License-Identifier: MIT // Copyright (c) 2021 Kenji Koide (k.koide@aist.go.jp) -#include +#include #include -#include +#include -namespace gtsam_ext { +namespace gtsam_points { template IntegratedPointToPlaneFactor_::IntegratedPointToPlaneFactor_( @@ -15,7 +15,7 @@ IntegratedPointToPlaneFactor_::IntegratedPointToPlaneF const std::shared_ptr& target, const std::shared_ptr& source, const std::shared_ptr& target_tree) -: gtsam_ext::IntegratedMatchingCostFactor(target_key, source_key), +: gtsam_points::IntegratedMatchingCostFactor(target_key, source_key), num_threads(1), max_correspondence_distance_sq(1.0), correspondence_update_tolerance_rot(0.0), @@ -98,11 +98,11 @@ double IntegratedPointToPlaneFactor_::evaluate( // double sum_errors = 0.0; - std::vector, Eigen::aligned_allocator>> Hs_target; - std::vector, Eigen::aligned_allocator>> Hs_source; - std::vector, Eigen::aligned_allocator>> Hs_target_source; - std::vector, Eigen::aligned_allocator>> bs_target; - std::vector, Eigen::aligned_allocator>> bs_source; + std::vector> Hs_target; + std::vector> Hs_source; + std::vector> Hs_target_source; + std::vector> bs_target; + std::vector> bs_source; if (H_target && H_source && H_target_source && b_target && b_source) { Hs_target.resize(num_threads, Eigen::Matrix::Zero()); @@ -186,7 +186,7 @@ IntegratedPointToEdgeFactor_::IntegratedPointToEdgeFac const std::shared_ptr& target, const std::shared_ptr& source, const std::shared_ptr& target_tree) -: gtsam_ext::IntegratedMatchingCostFactor(target_key, source_key), +: gtsam_points::IntegratedMatchingCostFactor(target_key, source_key), num_threads(1), max_correspondence_distance_sq(1.0), correspondence_update_tolerance_rot(0.0), @@ -212,7 +212,7 @@ IntegratedPointToEdgeFactor_::IntegratedPointToEdgeFac gtsam::Key source_key, const std::shared_ptr& target, const std::shared_ptr& source) -: gtsam_ext::IntegratedPointToEdgeFactor_(target_key, source_key, target, source, nullptr) {} +: gtsam_points::IntegratedPointToEdgeFactor_(target_key, source_key, target, source, nullptr) {} template IntegratedPointToEdgeFactor_::~IntegratedPointToEdgeFactor_() {} @@ -264,11 +264,11 @@ double IntegratedPointToEdgeFactor_::evaluate( // double sum_errors = 0.0; - std::vector, Eigen::aligned_allocator>> Hs_target; - std::vector, Eigen::aligned_allocator>> Hs_source; - std::vector, Eigen::aligned_allocator>> Hs_target_source; - std::vector, Eigen::aligned_allocator>> bs_target; - std::vector, Eigen::aligned_allocator>> bs_source; + std::vector> Hs_target; + std::vector> Hs_source; + std::vector> Hs_target_source; + std::vector> bs_target; + std::vector> bs_source; if (H_target && H_source && H_target_source && b_target && b_source) { Hs_target.resize(num_threads, Eigen::Matrix::Zero()); @@ -360,7 +360,7 @@ IntegratedLOAMFactor_::IntegratedLOAMFactor_( const std::shared_ptr& source_planes, const std::shared_ptr& target_edges_tree, const std::shared_ptr& target_planes_tree) -: gtsam_ext::IntegratedMatchingCostFactor(target_key, source_key), +: gtsam_points::IntegratedMatchingCostFactor(target_key, source_key), enable_correspondence_validation(false) { // edge_factor.reset( @@ -377,7 +377,7 @@ IntegratedLOAMFactor_::IntegratedLOAMFactor_( const std::shared_ptr& target_planes, const std::shared_ptr& source_edges, const std::shared_ptr& source_planes) -: gtsam_ext::IntegratedMatchingCostFactor(target_key, source_key), +: gtsam_points::IntegratedMatchingCostFactor(target_key, source_key), enable_correspondence_validation(false) { // edge_factor.reset(new IntegratedPointToEdgeFactor_(target_key, source_key, target_edges, source_edges)); @@ -394,9 +394,9 @@ void IntegratedLOAMFactor_::set_num_threads(int n) { } template -void IntegratedLOAMFactor_::set_max_corresponding_distance(double dist_edge, double dist_plane) { - edge_factor->set_max_corresponding_distance(dist_edge); - plane_factor->set_max_corresponding_distance(dist_plane); +void IntegratedLOAMFactor_::set_max_correspondence_distance(double dist_edge, double dist_plane) { + edge_factor->set_max_correspondence_distance(dist_edge); + plane_factor->set_max_correspondence_distance(dist_plane); } template @@ -492,4 +492,4 @@ void IntegratedLOAMFactor_::validate_correspondences() } } -} // namespace gtsam_ext \ No newline at end of file +} // namespace gtsam_points \ No newline at end of file diff --git a/include/gtsam_ext/factors/impl/integrated_vgicp_factor_impl.hpp b/include/gtsam_points/factors/impl/integrated_vgicp_factor_impl.hpp similarity index 77% rename from include/gtsam_ext/factors/impl/integrated_vgicp_factor_impl.hpp rename to include/gtsam_points/factors/impl/integrated_vgicp_factor_impl.hpp index 9312aa23..9c9f4556 100644 --- a/include/gtsam_ext/factors/impl/integrated_vgicp_factor_impl.hpp +++ b/include/gtsam_points/factors/impl/integrated_vgicp_factor_impl.hpp @@ -1,13 +1,13 @@ // SPDX-License-Identifier: MIT // Copyright (c) 2021 Kenji Koide (k.koide@aist.go.jp) -#include +#include #include #include -#include +#include -namespace gtsam_ext { +namespace gtsam_points { template IntegratedVGICPFactor_::IntegratedVGICPFactor_( @@ -15,7 +15,7 @@ IntegratedVGICPFactor_::IntegratedVGICPFactor_( gtsam::Key source_key, const GaussianVoxelMap::ConstPtr& target_voxels, const std::shared_ptr& source) -: gtsam_ext::IntegratedMatchingCostFactor(target_key, source_key), +: gtsam_points::IntegratedMatchingCostFactor(target_key, source_key), num_threads(1), target_voxels(std::dynamic_pointer_cast(target_voxels)), source(source) { @@ -36,21 +36,13 @@ IntegratedVGICPFactor_::IntegratedVGICPFactor_( } } -template -IntegratedVGICPFactor_::IntegratedVGICPFactor_( - gtsam::Key target_key, - gtsam::Key source_key, - const Frame::ConstPtr& target, - const std::shared_ptr& source) -: IntegratedVGICPFactor_(target_key, source_key, target->voxels, source) {} - template IntegratedVGICPFactor_::IntegratedVGICPFactor_( const gtsam::Pose3& fixed_target_pose, gtsam::Key source_key, const GaussianVoxelMap::ConstPtr& target_voxels, const std::shared_ptr& source) -: gtsam_ext::IntegratedMatchingCostFactor(fixed_target_pose, source_key), +: gtsam_points::IntegratedMatchingCostFactor(fixed_target_pose, source_key), num_threads(1), target_voxels(std::dynamic_pointer_cast(target_voxels)), source(source) { @@ -71,14 +63,6 @@ IntegratedVGICPFactor_::IntegratedVGICPFactor_( } } -template -IntegratedVGICPFactor_::IntegratedVGICPFactor_( - const gtsam::Pose3& fixed_target_pose, - gtsam::Key source_key, - const Frame::ConstPtr& target, - const std::shared_ptr& source) -: IntegratedVGICPFactor_(fixed_target_pose, source_key, target->voxels, source) {} - template IntegratedVGICPFactor_::~IntegratedVGICPFactor_() {} @@ -91,13 +75,15 @@ void IntegratedVGICPFactor_::update_correspondences(const Eigen::Is for (int i = 0; i < frame::size(*source); i++) { Eigen::Vector4d pt = delta * frame::point(*source, i); Eigen::Vector3i coord = target_voxels->voxel_coord(pt); - auto voxel = target_voxels->lookup_voxel(coord); + const auto voxel_id = target_voxels->lookup_voxel_index(coord); - if (voxel == nullptr) { + if (voxel_id < 0) { correspondences[i] = nullptr; mahalanobis[i].setIdentity(); } else { - correspondences[i] = voxel.get(); + const auto voxel = &target_voxels->lookup_voxel(voxel_id); + + correspondences[i] = voxel; Eigen::Matrix4d RCR = (voxel->cov + delta.matrix() * frame::cov(*source, i) * delta.matrix().transpose()); RCR(3, 3) = 1.0; @@ -118,11 +104,11 @@ double IntegratedVGICPFactor_::evaluate( // double sum_errors = 0.0; - std::vector, Eigen::aligned_allocator>> Hs_target; - std::vector, Eigen::aligned_allocator>> Hs_source; - std::vector, Eigen::aligned_allocator>> Hs_target_source; - std::vector, Eigen::aligned_allocator>> bs_target; - std::vector, Eigen::aligned_allocator>> bs_source; + std::vector> Hs_target; + std::vector> Hs_source; + std::vector> Hs_target_source; + std::vector> bs_target; + std::vector> bs_source; if (H_target && H_source && H_target_source && b_target && b_source) { Hs_target.resize(num_threads, Eigen::Matrix::Zero()); @@ -196,4 +182,4 @@ double IntegratedVGICPFactor_::evaluate( return sum_errors; } -} // namespace gtsam_ext +} // namespace gtsam_points diff --git a/include/gtsam_ext/factors/integrated_color_consistency_factor.hpp b/include/gtsam_points/factors/integrated_color_consistency_factor.hpp similarity index 88% rename from include/gtsam_ext/factors/integrated_color_consistency_factor.hpp rename to include/gtsam_points/factors/integrated_color_consistency_factor.hpp index 9957c98c..f8ba6944 100644 --- a/include/gtsam_ext/factors/integrated_color_consistency_factor.hpp +++ b/include/gtsam_points/factors/integrated_color_consistency_factor.hpp @@ -6,11 +6,11 @@ #include #include -#include -#include -#include +#include +#include +#include -namespace gtsam_ext { +namespace gtsam_points { struct NearestNeighborSearch; @@ -18,8 +18,8 @@ struct NearestNeighborSearch; * @brief Photometric consistency factor between point clouds * * @note This factor uses (x, y, z, intensity) to query nearest neighbor search - * The 4th element (intensity) will be simply ignored if a standard gtsam_ext::KdTree is given - * while it can provide additional distance information between points if gtsam_ext::IntensityKdTree is used + * The 4th element (intensity) will be simply ignored if a standard gtsam_points::KdTree is given + * while it can provide additional distance information between points if gtsam_points::IntensityKdTree is used * * @note While the use of IntensityKdTree significantly improves the convergence speed, * it can affect optimization stability in some cases @@ -27,10 +27,10 @@ struct NearestNeighborSearch; * Park et al., "Colored Point Cloud Registration Revisited", ICCV2017 */ template < - typename TargetFrame = gtsam_ext::Frame, - typename SourceFrame = gtsam_ext::Frame, - typename IntensityGradients = gtsam_ext::IntensityGradients> -class IntegratedColorConsistencyFactor_ : public gtsam_ext::IntegratedMatchingCostFactor { + typename TargetFrame = gtsam_points::PointCloud, + typename SourceFrame = gtsam_points::PointCloud, + typename IntensityGradients = gtsam_points::IntensityGradients> +class IntegratedColorConsistencyFactor_ : public gtsam_points::IntegratedMatchingCostFactor { public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW using shared_ptr = boost::shared_ptr>; @@ -119,4 +119,4 @@ class IntegratedColorConsistencyFactor_ : public gtsam_ext::IntegratedMatchingCo using IntegratedColorConsistencyFactor = IntegratedColorConsistencyFactor_<>; -} // namespace gtsam_ext \ No newline at end of file +} // namespace gtsam_points \ No newline at end of file diff --git a/include/gtsam_ext/factors/integrated_colored_gicp_factor.hpp b/include/gtsam_points/factors/integrated_colored_gicp_factor.hpp similarity index 83% rename from include/gtsam_ext/factors/integrated_colored_gicp_factor.hpp rename to include/gtsam_points/factors/integrated_colored_gicp_factor.hpp index 22d4ff1a..7df5078b 100644 --- a/include/gtsam_ext/factors/integrated_colored_gicp_factor.hpp +++ b/include/gtsam_points/factors/integrated_colored_gicp_factor.hpp @@ -6,11 +6,11 @@ #include #include -#include -#include -#include +#include +#include +#include -namespace gtsam_ext { +namespace gtsam_points { struct NearestNeighborSearch; @@ -18,8 +18,8 @@ struct NearestNeighborSearch; * @brief Colored GICP matching cost factor * * @note This factor uses (x, y, z, intensity) to query nearest neighbor search - * The 4th element (intensity) will be simply ignored if a standard gtsam_ext::KdTree is given - * while it can provide additional distance information between points if gtsam_ext::IntensityKdTree is used + * The 4th element (intensity) will be simply ignored if a standard gtsam_points::KdTree is given + * while it can provide additional distance information between points if gtsam_points::IntensityKdTree is used * * @note While the use of IntensityKdTree significantly improves the convergence speed, * it can affect optimization stability in some cases @@ -28,10 +28,10 @@ struct NearestNeighborSearch; * Park et al., "Colored Point Cloud Registration Revisited", ICCV2017 */ template < - typename TargetFrame = gtsam_ext::Frame, - typename SourceFrame = gtsam_ext::Frame, - typename IntensityGradients = gtsam_ext::IntensityGradients> -class IntegratedColoredGICPFactor_ : public gtsam_ext::IntegratedMatchingCostFactor { + typename TargetFrame = gtsam_points::PointCloud, + typename SourceFrame = gtsam_points::PointCloud, + typename IntensityGradients = gtsam_points::IntensityGradients> +class IntegratedColoredGICPFactor_ : public gtsam_points::IntegratedMatchingCostFactor { public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW using shared_ptr = boost::shared_ptr>; @@ -88,9 +88,9 @@ class IntegratedColoredGICPFactor_ : public gtsam_ext::IntegratedMatchingCostFac double correspondence_update_tolerance_trans; mutable Eigen::Isometry3d last_correspondence_point; mutable std::vector correspondences; - mutable std::vector> mahalanobis; + mutable std::vector mahalanobis; }; using IntegratedColoredGICPFactor = IntegratedColoredGICPFactor_<>; -} // namespace gtsam_ext \ No newline at end of file +} // namespace gtsam_points \ No newline at end of file diff --git a/include/gtsam_ext/factors/integrated_ct_gicp_factor.hpp b/include/gtsam_points/factors/integrated_ct_gicp_factor.hpp similarity index 87% rename from include/gtsam_ext/factors/integrated_ct_gicp_factor.hpp rename to include/gtsam_points/factors/integrated_ct_gicp_factor.hpp index dc211264..556e3a1b 100644 --- a/include/gtsam_ext/factors/integrated_ct_gicp_factor.hpp +++ b/include/gtsam_points/factors/integrated_ct_gicp_factor.hpp @@ -3,16 +3,16 @@ #pragma once -#include +#include -namespace gtsam_ext { +namespace gtsam_points { /** * @brief Continuous Time ICP with GICP's D2D cost * Bellenbach et al., "CT-ICP: Real-time Elastic LiDAR Odometry with Loop Closure", 2021 * Segal et al., "Generalized-ICP", RSS2005 */ -template +template class IntegratedCT_GICPFactor_ : public IntegratedCT_ICPFactor_ { public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW @@ -54,9 +54,9 @@ class IntegratedCT_GICPFactor_ : public IntegratedCT_ICPFactor_> mahalanobis; + mutable std::vector mahalanobis; }; using IntegratedCT_GICPFactor = IntegratedCT_GICPFactor_<>; -} // namespace gtsam_ext +} // namespace gtsam_points diff --git a/include/gtsam_ext/factors/integrated_ct_icp_factor.hpp b/include/gtsam_points/factors/integrated_ct_icp_factor.hpp similarity index 75% rename from include/gtsam_ext/factors/integrated_ct_icp_factor.hpp rename to include/gtsam_points/factors/integrated_ct_icp_factor.hpp index 3d30196b..bb657128 100644 --- a/include/gtsam_ext/factors/integrated_ct_icp_factor.hpp +++ b/include/gtsam_points/factors/integrated_ct_icp_factor.hpp @@ -6,9 +6,9 @@ #include #include -#include +#include -namespace gtsam_ext { +namespace gtsam_points { struct NearestNeighborSearch; @@ -16,7 +16,7 @@ struct NearestNeighborSearch; * @brief Continuous Time ICP Factor * Bellenbach et al., "CT-ICP: Real-time Elastic LiDAR Odometry with Loop Closure", 2021 */ -template +template class IntegratedCT_ICPFactor_ : public gtsam::NonlinearFactor { public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW @@ -57,13 +57,13 @@ class IntegratedCT_ICPFactor_ : public gtsam::NonlinearFactor { virtual boost::shared_ptr linearize(const gtsam::Values& values) const override; void set_num_threads(int n) { num_threads = n; } - void set_max_corresponding_distance(double dist) { max_correspondence_distance_sq = dist * dist; } + void set_max_correspondence_distance(double dist) { max_correspondence_distance_sq = dist * dist; } const std::vector& get_time_table() const { return time_table; } const std::vector& get_time_indices() const { return time_indices; } - const std::vector>& get_source_poses() const { return source_poses; } + const std::vector& get_source_poses() const { return source_poses; } - std::vector> deskewed_source_points(const gtsam::Values& values, bool local = false); + std::vector deskewed_source_points(const gtsam::Values& values, bool local = false); public: virtual void update_poses(const gtsam::Values& values) const; @@ -78,9 +78,9 @@ class IntegratedCT_ICPFactor_ : public gtsam::NonlinearFactor { std::shared_ptr target_tree; std::vector time_table; - mutable std::vector> source_poses; - mutable std::vector> pose_derivatives_t0; - mutable std::vector> pose_derivatives_t1; + mutable std::vector source_poses; + mutable std::vector pose_derivatives_t0; + mutable std::vector pose_derivatives_t1; std::vector time_indices; mutable std::vector correspondences; @@ -91,4 +91,4 @@ class IntegratedCT_ICPFactor_ : public gtsam::NonlinearFactor { using IntegratedCT_ICPFactor = IntegratedCT_ICPFactor_<>; -} // namespace gtsam_ext +} // namespace gtsam_points diff --git a/include/gtsam_ext/factors/integrated_gicp_factor.hpp b/include/gtsam_points/factors/integrated_gicp_factor.hpp similarity index 89% rename from include/gtsam_ext/factors/integrated_gicp_factor.hpp rename to include/gtsam_points/factors/integrated_gicp_factor.hpp index fc312e5a..45136089 100644 --- a/include/gtsam_ext/factors/integrated_gicp_factor.hpp +++ b/include/gtsam_points/factors/integrated_gicp_factor.hpp @@ -6,10 +6,10 @@ #include #include -#include -#include +#include +#include -namespace gtsam_ext { +namespace gtsam_points { struct NearestNeighborSearch; @@ -17,8 +17,8 @@ struct NearestNeighborSearch; * @brief Generalized ICP matching cost factor * Segal et al., "Generalized-ICP", RSS2005 */ -template -class IntegratedGICPFactor_ : public gtsam_ext::IntegratedMatchingCostFactor { +template +class IntegratedGICPFactor_ : public gtsam_points::IntegratedMatchingCostFactor { public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW using shared_ptr = boost::shared_ptr; @@ -76,7 +76,7 @@ class IntegratedGICPFactor_ : public gtsam_ext::IntegratedMatchingCostFactor { /// @brief Set the maximum distance between corresponding points. /// Correspondences with distances larger than this will be rejected (i.e., correspondence trimming). - void set_max_corresponding_distance(double dist) { max_correspondence_distance_sq = dist * dist; } + void set_max_correspondence_distance(double dist) { max_correspondence_distance_sq = dist * dist; } /// @brief Correspondences are updated only when the displacement from the last update point is larger than these threshold values. /// @note Default values are angle=trans=0 and correspondences are updated every linearization call. @@ -114,7 +114,7 @@ class IntegratedGICPFactor_ : public gtsam_ext::IntegratedMatchingCostFactor { double correspondence_update_tolerance_trans; mutable Eigen::Isometry3d last_correspondence_point; mutable std::vector correspondences; - mutable std::vector> mahalanobis; + mutable std::vector mahalanobis; std::shared_ptr target; std::shared_ptr source; @@ -122,4 +122,4 @@ class IntegratedGICPFactor_ : public gtsam_ext::IntegratedMatchingCostFactor { using IntegratedGICPFactor = IntegratedGICPFactor_<>; -} // namespace gtsam_ext \ No newline at end of file +} // namespace gtsam_points \ No newline at end of file diff --git a/include/gtsam_ext/factors/integrated_icp_factor.hpp b/include/gtsam_points/factors/integrated_icp_factor.hpp similarity index 81% rename from include/gtsam_ext/factors/integrated_icp_factor.hpp rename to include/gtsam_points/factors/integrated_icp_factor.hpp index 5c4393ff..81dc96b2 100644 --- a/include/gtsam_ext/factors/integrated_icp_factor.hpp +++ b/include/gtsam_points/factors/integrated_icp_factor.hpp @@ -6,10 +6,10 @@ #include #include -#include -#include +#include +#include -namespace gtsam_ext { +namespace gtsam_points { struct NearestNeighborSearch; @@ -17,11 +17,11 @@ struct NearestNeighborSearch; * @brief Naive point-to-point ICP matching cost factor * Zhang, "Iterative Point Matching for Registration of Free-Form Curve", IJCV1994 */ -template -class IntegratedICPFactor_ : public gtsam_ext::IntegratedMatchingCostFactor { +template +class IntegratedICPFactor_ : public gtsam_points::IntegratedMatchingCostFactor { public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW - using shared_ptr = boost::shared_ptr>; + using shared_ptr = boost::shared_ptr>; /** * @brief Create a binary ICP factor between two poses. @@ -82,7 +82,7 @@ class IntegratedICPFactor_ : public gtsam_ext::IntegratedMatchingCostFactor { /// @brief Set the maximum distance between corresponding points. /// Correspondences with distances larger than this will be rejected (i.e., correspondence trimming). - void set_max_corresponding_distance(double dist) { max_correspondence_distance_sq = dist * dist; } + void set_max_correspondence_distance(double dist) { max_correspondence_distance_sq = dist * dist; } /// @brief Enable or disable point-to-plane distance computation. void set_point_to_plane_distance(bool use) { use_point_to_plane = use; } @@ -125,11 +125,19 @@ class IntegratedICPFactor_ : public gtsam_ext::IntegratedMatchingCostFactor { /** * @brief Point-to-plane ICP factor */ -template -class IntegratedPointToPlaneICPFactor_ : public gtsam_ext::IntegratedICPFactor_ { +template +class IntegratedPointToPlaneICPFactor_ : public gtsam_points::IntegratedICPFactor_ { public: using shared_ptr = boost::shared_ptr>; + IntegratedPointToPlaneICPFactor_( + gtsam::Key target_key, + gtsam::Key source_key, + const std::shared_ptr& target, + const std::shared_ptr& source, + const std::shared_ptr& target_tree) + : IntegratedICPFactor_(target_key, source_key, target, source, target_tree, true) {} + IntegratedPointToPlaneICPFactor_( gtsam::Key target_key, gtsam::Key source_key, @@ -141,4 +149,4 @@ class IntegratedPointToPlaneICPFactor_ : public gtsam_ext::IntegratedICPFactor_< using IntegratedICPFactor = IntegratedICPFactor_<>; using IntegratedPointToPlaneICPFactor = IntegratedPointToPlaneICPFactor_<>; -} // namespace gtsam_ext \ No newline at end of file +} // namespace gtsam_points \ No newline at end of file diff --git a/include/gtsam_ext/factors/integrated_loam_factor.hpp b/include/gtsam_points/factors/integrated_loam_factor.hpp similarity index 84% rename from include/gtsam_ext/factors/integrated_loam_factor.hpp rename to include/gtsam_points/factors/integrated_loam_factor.hpp index 3659e5dc..5475ba43 100644 --- a/include/gtsam_ext/factors/integrated_loam_factor.hpp +++ b/include/gtsam_points/factors/integrated_loam_factor.hpp @@ -6,10 +6,10 @@ #include #include -#include -#include +#include +#include -namespace gtsam_ext { +namespace gtsam_points { struct NearestNeighborSearch; @@ -25,8 +25,8 @@ class IntegratedPointToEdgeFactor_; * Zhang and Singh, "LOAM: LiDAR Odometry and Mapping in Real-time", RSS2014 * Tixiao and Brendan, "LeGO-LOAM: Lightweight and Ground-Optimized Lidar Odometry and Mapping on Variable Terrain", IROS2018 */ -template -class IntegratedLOAMFactor_ : public gtsam_ext::IntegratedMatchingCostFactor { +template +class IntegratedLOAMFactor_ : public gtsam_points::IntegratedMatchingCostFactor { public: GTSAM_MAKE_ALIGNED_OPERATOR_NEW using shared_ptr = boost::shared_ptr>; @@ -54,7 +54,7 @@ class IntegratedLOAMFactor_ : public gtsam_ext::IntegratedMatchingCostFactor { // note: If your GTSAM is built with TBB, linearization is already multi-threaded // : and setting n>1 can rather affect the processing speed void set_num_threads(int n); - void set_max_corresponding_distance(double dist_edge, double dist_plane); + void set_max_correspondence_distance(double dist_edge, double dist_plane); void set_correspondence_update_tolerance(double angle, double trans); void set_enable_correspondence_validation(bool enable); @@ -82,8 +82,8 @@ class IntegratedLOAMFactor_ : public gtsam_ext::IntegratedMatchingCostFactor { }; // Point-to-plane distance -template -class IntegratedPointToPlaneFactor_ : public gtsam_ext::IntegratedMatchingCostFactor { +template +class IntegratedPointToPlaneFactor_ : public gtsam_points::IntegratedMatchingCostFactor { public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW using shared_ptr = boost::shared_ptr>; @@ -106,7 +106,7 @@ class IntegratedPointToPlaneFactor_ : public gtsam_ext::IntegratedMatchingCostFa ~IntegratedPointToPlaneFactor_(); void set_num_threads(int n) { num_threads = n; } - void set_max_corresponding_distance(double dist) { max_correspondence_distance_sq = dist * dist; } + void set_max_correspondence_distance(double dist) { max_correspondence_distance_sq = dist * dist; } void set_correspondence_update_tolerance(double angle, double trans) { correspondence_update_tolerance_rot = angle; correspondence_update_tolerance_trans = trans; @@ -140,8 +140,8 @@ class IntegratedPointToPlaneFactor_ : public gtsam_ext::IntegratedMatchingCostFa }; // Point-to-edge distance -template -class IntegratedPointToEdgeFactor_ : public gtsam_ext::IntegratedMatchingCostFactor { +template +class IntegratedPointToEdgeFactor_ : public gtsam_points::IntegratedMatchingCostFactor { public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW using shared_ptr = boost::shared_ptr>; @@ -164,7 +164,7 @@ class IntegratedPointToEdgeFactor_ : public gtsam_ext::IntegratedMatchingCostFac ~IntegratedPointToEdgeFactor_(); void set_num_threads(int n) { num_threads = n; } - void set_max_corresponding_distance(double dist) { max_correspondence_distance_sq = dist * dist; } + void set_max_correspondence_distance(double dist) { max_correspondence_distance_sq = dist * dist; } void set_correspondence_update_tolerance(double angle, double trans) { correspondence_update_tolerance_rot = angle; correspondence_update_tolerance_trans = trans; @@ -197,8 +197,8 @@ class IntegratedPointToEdgeFactor_ : public gtsam_ext::IntegratedMatchingCostFac std::shared_ptr source; }; -using IntegratedLOAMFactor = gtsam_ext::IntegratedLOAMFactor_<>; -using IntegratedPointToPlaneFactor = gtsam_ext::IntegratedPointToPlaneFactor_<>; -using IntegratedPointToEdgeFactor = gtsam_ext::IntegratedPointToEdgeFactor_<>; +using IntegratedLOAMFactor = gtsam_points::IntegratedLOAMFactor_<>; +using IntegratedPointToPlaneFactor = gtsam_points::IntegratedPointToPlaneFactor_<>; +using IntegratedPointToEdgeFactor = gtsam_points::IntegratedPointToEdgeFactor_<>; -} // namespace gtsam_ext +} // namespace gtsam_points diff --git a/include/gtsam_ext/factors/integrated_matching_cost_factor.hpp b/include/gtsam_points/factors/integrated_matching_cost_factor.hpp similarity index 97% rename from include/gtsam_ext/factors/integrated_matching_cost_factor.hpp rename to include/gtsam_points/factors/integrated_matching_cost_factor.hpp index 449e5470..056ed354 100644 --- a/include/gtsam_ext/factors/integrated_matching_cost_factor.hpp +++ b/include/gtsam_points/factors/integrated_matching_cost_factor.hpp @@ -8,7 +8,7 @@ #include #include -namespace gtsam_ext { +namespace gtsam_points { /** * @brief Abstraction of LSQ-based scan matching constraints between point clouds @@ -71,4 +71,4 @@ class IntegratedMatchingCostFactor : public gtsam::NonlinearFactor { bool is_binary; Eigen::Isometry3d fixed_target_pose; }; -} // namespace gtsam_ext \ No newline at end of file +} // namespace gtsam_points \ No newline at end of file diff --git a/include/gtsam_ext/factors/integrated_vgicp_derivatives.cuh b/include/gtsam_points/factors/integrated_vgicp_derivatives.cuh similarity index 92% rename from include/gtsam_ext/factors/integrated_vgicp_derivatives.cuh rename to include/gtsam_points/factors/integrated_vgicp_derivatives.cuh index 6abb83ab..e9ec5c4c 100644 --- a/include/gtsam_ext/factors/integrated_vgicp_derivatives.cuh +++ b/include/gtsam_points/factors/integrated_vgicp_derivatives.cuh @@ -11,11 +11,11 @@ #include #include -#include +#include struct CUstream_st; -namespace gtsam_ext { +namespace gtsam_points { class LinearizedSystem6; class TempBufferManager; @@ -26,7 +26,7 @@ public: IntegratedVGICPDerivatives( const GaussianVoxelMapGPU::ConstPtr& target, - const Frame::ConstPtr& source, + const PointCloud::ConstPtr& source, CUstream_st* ext_stream, std::shared_ptr temp_buffer); ~IntegratedVGICPDerivatives(); @@ -70,7 +70,7 @@ private: std::shared_ptr temp_buffer; GaussianVoxelMapGPU::ConstPtr target; - Frame::ConstPtr source; + PointCloud::ConstPtr source; Eigen::Isometry3f inlier_evaluation_point; @@ -78,4 +78,4 @@ private: int* num_inliers_gpu; int* source_inliers; }; -} // namespace gtsam_ext \ No newline at end of file +} // namespace gtsam_points \ No newline at end of file diff --git a/include/gtsam_ext/factors/integrated_vgicp_factor.hpp b/include/gtsam_points/factors/integrated_vgicp_factor.hpp similarity index 73% rename from include/gtsam_ext/factors/integrated_vgicp_factor.hpp rename to include/gtsam_points/factors/integrated_vgicp_factor.hpp index 633cb818..85bf3343 100644 --- a/include/gtsam_ext/factors/integrated_vgicp_factor.hpp +++ b/include/gtsam_points/factors/integrated_vgicp_factor.hpp @@ -6,11 +6,11 @@ #include #include -#include -#include -#include +#include +#include +#include -namespace gtsam_ext { +namespace gtsam_points { struct GaussianVoxel; @@ -19,8 +19,8 @@ struct GaussianVoxel; * Koide et al., "Voxelized GICP for Fast and Accurate 3D Point Cloud Registration", ICRA2021 * Koide et al., "Globally Consistent 3D LiDAR Mapping with GPU-accelerated GICP Matching Cost Factors", RA-L2021 */ -template -class IntegratedVGICPFactor_ : public gtsam_ext::IntegratedMatchingCostFactor { +template +class IntegratedVGICPFactor_ : public gtsam_points::IntegratedMatchingCostFactor { public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW using shared_ptr = boost::shared_ptr; @@ -38,13 +38,6 @@ class IntegratedVGICPFactor_ : public gtsam_ext::IntegratedMatchingCostFactor { const GaussianVoxelMap::ConstPtr& target_voxels, const std::shared_ptr& source); - ///< Create a binary VGICP factor between target and source poses. - IntegratedVGICPFactor_( - gtsam::Key target_key, - gtsam::Key source_key, - const Frame::ConstPtr& target, - const std::shared_ptr& source); - /** * @brief Create a unary VGICP factor between a fixed target pose and an active source pose. * @param fixed_target_pose Fixed target pose @@ -58,13 +51,6 @@ class IntegratedVGICPFactor_ : public gtsam_ext::IntegratedMatchingCostFactor { const GaussianVoxelMap::ConstPtr& target_voxels, const std::shared_ptr& source); - ///< Create a unary VGICP factor between a fixed target pose and an active source pose. - IntegratedVGICPFactor_( - const gtsam::Pose3& fixed_target_pose, - gtsam::Key source_key, - const Frame::ConstPtr& target, - const std::shared_ptr& source); - virtual ~IntegratedVGICPFactor_() override; /// @brief Set the number of thread used for linearization of this factor. @@ -72,6 +58,13 @@ class IntegratedVGICPFactor_ : public gtsam_ext::IntegratedMatchingCostFactor { /// and setting n>1 can rather affect the processing speed. void set_num_threads(int n) { num_threads = n; } + /// @brief Compute the fraction of inlier points that have correspondences with a distance smaller than the trimming threshold. + double inlier_fraction() const { + const int outliers = std::count(correspondences.begin(), correspondences.end(), nullptr); + const int inliers = correspondences.size() - outliers; + return static_cast(inliers) / correspondences.size(); + } + private: virtual void update_correspondences(const Eigen::Isometry3d& delta) const override; @@ -88,7 +81,7 @@ class IntegratedVGICPFactor_ : public gtsam_ext::IntegratedMatchingCostFactor { // I'm unhappy to have mutable members... mutable std::vector correspondences; - mutable std::vector> mahalanobis; + mutable std::vector mahalanobis; std::shared_ptr target_voxels; std::shared_ptr source; @@ -96,4 +89,4 @@ class IntegratedVGICPFactor_ : public gtsam_ext::IntegratedMatchingCostFactor { using IntegratedVGICPFactor = IntegratedVGICPFactor_<>; -} // namespace gtsam_ext \ No newline at end of file +} // namespace gtsam_points \ No newline at end of file diff --git a/include/gtsam_ext/factors/integrated_vgicp_factor_gpu.hpp b/include/gtsam_points/factors/integrated_vgicp_factor_gpu.hpp similarity index 73% rename from include/gtsam_ext/factors/integrated_vgicp_factor_gpu.hpp rename to include/gtsam_points/factors/integrated_vgicp_factor_gpu.hpp index 0cac2ce8..c1a21906 100644 --- a/include/gtsam_ext/factors/integrated_vgicp_factor_gpu.hpp +++ b/include/gtsam_points/factors/integrated_vgicp_factor_gpu.hpp @@ -3,9 +3,9 @@ #pragma once -#include -#include -#include +#include +#include +#include struct CUstream_st; @@ -13,7 +13,7 @@ namespace gtsam { class Pose3; } -namespace gtsam_ext { +namespace gtsam_points { class LinearizedSystem6; class IntegratedVGICPDerivatives; @@ -24,7 +24,7 @@ class TempBufferManager; * Koide et al., "Voxelized GICP for Fast and Accurate 3D Point Cloud Registration", ICRA2021 * Koide et al., "Globally Consistent 3D LiDAR Mapping with GPU-accelerated GICP Matching Cost Factors", RA-L2021 */ -class IntegratedVGICPFactorGPU : public gtsam_ext::NonlinearFactorGPU { +class IntegratedVGICPFactorGPU : public gtsam_points::NonlinearFactorGPU { public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW using shared_ptr = boost::shared_ptr; @@ -42,21 +42,9 @@ class IntegratedVGICPFactorGPU : public gtsam_ext::NonlinearFactorGPU { gtsam::Key target_key, gtsam::Key source_key, const GaussianVoxelMap::ConstPtr& target, - const Frame::ConstPtr& source, - CUstream_st* stream, - std::shared_ptr temp_buffer); - - /// Create a binary VGICP_GPU factor between target and source poses. - IntegratedVGICPFactorGPU(gtsam::Key target_key, gtsam::Key source_key, const Frame::ConstPtr& target, const std::shared_ptr& source); - - /// Create a binary VGICP_GPU factor between target and source poses. - IntegratedVGICPFactorGPU( - gtsam::Key target_key, - gtsam::Key source_key, - const Frame::ConstPtr& target, - const Frame::ConstPtr& source, - CUstream_st* stream, - std::shared_ptr temp_buffer); + const PointCloud::ConstPtr& source, + CUstream_st* stream = nullptr, + std::shared_ptr temp_buffer = nullptr); /** * @brief Create a unary VGICP_GPU factor between a fixed target pose and an active source pose. @@ -71,31 +59,16 @@ class IntegratedVGICPFactorGPU : public gtsam_ext::NonlinearFactorGPU { const gtsam::Pose3& fixed_target_pose, gtsam::Key source_key, const GaussianVoxelMap::ConstPtr& target, - const Frame::ConstPtr& source, - CUstream_st* stream, - std::shared_ptr temp_buffer); + const PointCloud::ConstPtr& source, + CUstream_st* stream = nullptr, + std::shared_ptr temp_buffer = nullptr); /// Create a unary VGICP_GPU factor between a fixed target pose and an active source pose. - IntegratedVGICPFactorGPU( - const gtsam::Pose3& fixed_target_pose, - gtsam::Key source_key, - const Frame::ConstPtr& target, - const Frame::ConstPtr& source); - IntegratedVGICPFactorGPU( const gtsam::Pose3& fixed_target_pose, gtsam::Key source_key, const GaussianVoxelMap::ConstPtr& target, - const Frame::ConstPtr& source); - - /// Create a unary VGICP_GPU factor between a fixed target pose and an active source pose. - IntegratedVGICPFactorGPU( - const gtsam::Pose3& fixed_target_pose, - gtsam::Key source_key, - const Frame::ConstPtr& target, - const Frame::ConstPtr& source, - CUstream_st* stream, - std::shared_ptr temp_buffer); + const PointCloud::ConstPtr& source); virtual ~IntegratedVGICPFactorGPU() override; @@ -150,7 +123,7 @@ class IntegratedVGICPFactorGPU : public gtsam_ext::NonlinearFactorGPU { Eigen::Isometry3f fixed_target_pose; GaussianVoxelMapGPU::ConstPtr target; - Frame::ConstPtr source; + PointCloud::ConstPtr source; std::unique_ptr derivatives; @@ -161,4 +134,4 @@ class IntegratedVGICPFactorGPU : public gtsam_ext::NonlinearFactorGPU { mutable std::unique_ptr linearization_result; }; -} // namespace gtsam_ext \ No newline at end of file +} // namespace gtsam_points \ No newline at end of file diff --git a/include/gtsam_ext/factors/intensity_gradients.hpp b/include/gtsam_points/factors/intensity_gradients.hpp similarity index 69% rename from include/gtsam_ext/factors/intensity_gradients.hpp rename to include/gtsam_points/factors/intensity_gradients.hpp index 3547b33f..eaa8c70c 100644 --- a/include/gtsam_ext/factors/intensity_gradients.hpp +++ b/include/gtsam_points/factors/intensity_gradients.hpp @@ -7,11 +7,11 @@ #include #include -#include -#include -#include +#include +#include +#include -namespace gtsam_ext { +namespace gtsam_points { /** * @brief Intensity gradients for colored ICP @@ -29,7 +29,7 @@ class IntensityGradients { * @param k_neighbors Number of neighbors used for intensity gradient estimation * @param num_threads Number of threads */ - static IntensityGradients::Ptr estimate(const Frame::ConstPtr& frame, int k_neighbors = 10, int num_threads = 1); + static IntensityGradients::Ptr estimate(const PointCloud::ConstPtr& frame, int k_neighbors = 10, int num_threads = 1); /** * @brief Estimate normals, covs, and intensity gradients @@ -40,12 +40,12 @@ class IntensityGradients { * @param num_threads Number of threads */ static IntensityGradients::Ptr - estimate(const gtsam_ext::FrameCPU::Ptr& frame, int k_geom_neighbors = 10, int k_photo_neighbors = 20, int num_threads = 1); + estimate(const gtsam_points::PointCloudCPU::Ptr& frame, int k_geom_neighbors = 10, int k_photo_neighbors = 20, int num_threads = 1); - static IntensityGradients::Ptr estimate(const Frame::ConstPtr& frame, const std::vector& neighbors, int k_photo_neighbors); + static IntensityGradients::Ptr estimate(const PointCloud::ConstPtr& frame, const std::vector& neighbors, int k_photo_neighbors); public: - std::vector> intensity_gradients; + std::vector intensity_gradients; }; namespace frame { @@ -57,4 +57,4 @@ struct traits { } // namespace frame -} // namespace gtsam_ext \ No newline at end of file +} // namespace gtsam_points \ No newline at end of file diff --git a/include/gtsam_ext/factors/linear_damping_factor.hpp b/include/gtsam_points/factors/linear_damping_factor.hpp similarity index 98% rename from include/gtsam_ext/factors/linear_damping_factor.hpp rename to include/gtsam_points/factors/linear_damping_factor.hpp index 7cf6ff0f..7e4032fe 100644 --- a/include/gtsam_ext/factors/linear_damping_factor.hpp +++ b/include/gtsam_points/factors/linear_damping_factor.hpp @@ -6,7 +6,7 @@ #include #include -namespace gtsam_ext { +namespace gtsam_points { /** * @brief This factor adds a constant diagonal matrix to the Hessian matrix. diff --git a/include/gtsam_ext/factors/nonlinear_factor_gpu.hpp b/include/gtsam_points/factors/nonlinear_factor_gpu.hpp similarity index 94% rename from include/gtsam_ext/factors/nonlinear_factor_gpu.hpp rename to include/gtsam_points/factors/nonlinear_factor_gpu.hpp index 67471014..1684104e 100644 --- a/include/gtsam_ext/factors/nonlinear_factor_gpu.hpp +++ b/include/gtsam_points/factors/nonlinear_factor_gpu.hpp @@ -13,7 +13,7 @@ template class device_ptr; } -namespace gtsam_ext { +namespace gtsam_points { /** * @brief Base class for GPU-based nonlinear factors @@ -21,7 +21,7 @@ namespace gtsam_ext { * we issue all linearization tasks of GPU-based factors and copy all * required data for linearization (e.g., current estimate) at once. * - * To allow gtsam_ext::NonlinearFactorSetGPU to manage linearization, you need to implement the following methods: + * To allow gtsam_points::NonlinearFactorSetGPU to manage linearization, you need to implement the following methods: * - linearization_(input|output)_size() : Define the size of input and result data for linearization * - set_linearization_point() : Write the data to be uploaded to the GPU before linearization * - issue_linearization() : Issue the linearization task @@ -36,7 +36,7 @@ namespace gtsam_ext { * - sync() : Wait for the ICP computation task * - store_linearized() : Read the linearized factor from lin_output_cpu * - * Optimizers in gtsam_ext calls NonlinearFactorSetGPU's linearization routine before calling linearize() of each factor. + * Optimizers in gtsam_points calls NonlinearFactorSetGPU's linearization routine before calling linearize() of each factor. * You should thus store the linearized factor to a temporary member and just return it when linearize() is called. */ class NonlinearFactorGPU : public gtsam::NonlinearFactor { @@ -125,4 +125,4 @@ class NonlinearFactorGPU : public gtsam::NonlinearFactor { virtual void sync() = 0; }; -} // namespace gtsam_ext \ No newline at end of file +} // namespace gtsam_points \ No newline at end of file diff --git a/include/gtsam_ext/factors/pose3_calib_factor.hpp b/include/gtsam_points/factors/pose3_calib_factor.hpp similarity index 97% rename from include/gtsam_ext/factors/pose3_calib_factor.hpp rename to include/gtsam_points/factors/pose3_calib_factor.hpp index 01e26e05..e820b4c5 100644 --- a/include/gtsam_ext/factors/pose3_calib_factor.hpp +++ b/include/gtsam_points/factors/pose3_calib_factor.hpp @@ -6,7 +6,7 @@ #include #include -namespace gtsam_ext { +namespace gtsam_points { /** * @brief Factor(xi, xj, Tij) that evaluates (xj_inv * xi) * Tij @@ -52,4 +52,4 @@ class Pose3CalibFactor : public gtsam::NoiseModelFactor3 #include -namespace gtsam_ext { +namespace gtsam_points { /** * @brief Factor(xi, xj, xk) s.t. xk = Slerp(xi, xj, t) @@ -83,4 +83,4 @@ class Pose3InterpolationFactor : public gtsam::NoiseModelFactor3 #include -namespace gtsam_ext { +namespace gtsam_points { class RotateVector3Factor : public gtsam::NoiseModelFactor2 { public: diff --git a/include/gtsam_ext/optimizers/dogleg_optimizer_ext.hpp b/include/gtsam_points/optimizers/dogleg_optimizer_ext.hpp similarity index 92% rename from include/gtsam_ext/optimizers/dogleg_optimizer_ext.hpp rename to include/gtsam_points/optimizers/dogleg_optimizer_ext.hpp index 320c3f33..1286d2d0 100644 --- a/include/gtsam_ext/optimizers/dogleg_optimizer_ext.hpp +++ b/include/gtsam_points/optimizers/dogleg_optimizer_ext.hpp @@ -21,7 +21,7 @@ #include #include -namespace gtsam_ext { +namespace gtsam_points { /** * This class performs Dogleg nonlinear optimization @@ -44,7 +44,10 @@ class GTSAM_EXPORT DoglegOptimizerExt : public gtsam::NonlinearOptimizer { * @param initialValues The initial variable assignments * @param params The optimization parameters */ - DoglegOptimizerExt(const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& initialValues, const gtsam::DoglegParams& params = gtsam::DoglegParams()); + DoglegOptimizerExt( + const gtsam::NonlinearFactorGraph& graph, + const gtsam::Values& initialValues, + const gtsam::DoglegParams& params = gtsam::DoglegParams()); /** Standard constructor, requires a nonlinear factor graph, initial * variable assignments, and optimization parameters. For convenience this @@ -85,4 +88,4 @@ class GTSAM_EXPORT DoglegOptimizerExt : public gtsam::NonlinearOptimizer { gtsam::DoglegParams ensureHasOrdering(gtsam::DoglegParams params, const gtsam::NonlinearFactorGraph& graph) const; }; -} // namespace gtsam_ext +} // namespace gtsam_points diff --git a/include/gtsam_ext/optimizers/dogleg_optimizer_ext_impl.hpp b/include/gtsam_points/optimizers/dogleg_optimizer_ext_impl.hpp similarity index 94% rename from include/gtsam_ext/optimizers/dogleg_optimizer_ext_impl.hpp rename to include/gtsam_points/optimizers/dogleg_optimizer_ext_impl.hpp index d3111f23..5ee0fd72 100644 --- a/include/gtsam_ext/optimizers/dogleg_optimizer_ext_impl.hpp +++ b/include/gtsam_points/optimizers/dogleg_optimizer_ext_impl.hpp @@ -20,9 +20,9 @@ #include #include -#include +#include -namespace gtsam_ext { +namespace gtsam_points { /** This class contains the implementation of the Dogleg algorithm. It is used * by DoglegOptimizer and can be used to easily put together custom versions of @@ -122,7 +122,8 @@ struct GTSAM_EXPORT DoglegOptimizerImplExt { * @param dx_n The Gauss-Newton point * @return The dogleg point \f$ \delta x_d \f$ */ - static gtsam::VectorValues ComputeDoglegPoint(double delta, const gtsam::VectorValues& dx_u, const gtsam::VectorValues& dx_n, const bool verbose = false); + static gtsam::VectorValues + ComputeDoglegPoint(double delta, const gtsam::VectorValues& dx_u, const gtsam::VectorValues& dx_n, const bool verbose = false); /** Compute the point on the line between the steepest descent point and the * Newton's method point intersecting the trust region boundary. @@ -158,7 +159,11 @@ typename DoglegOptimizerImplExt::IterationResult DoglegOptimizerImplExt::Iterate IterationResult result; bool stay = true; - enum { NONE, INCREASED_DELTA, DECREASED_DELTA } lastAction = NONE; // Used to prevent alternating between increasing and decreasing in one iteration + enum { + NONE, + INCREASED_DELTA, + DECREASED_DELTA + } lastAction = NONE; // Used to prevent alternating between increasing and decreasing in one iteration while (stay) { gttic(Dog_leg_point); // Compute dog leg point @@ -189,7 +194,9 @@ typename DoglegOptimizerImplExt::IterationResult DoglegOptimizerImplExt::Iterate gttic(adjust_delta); // Compute gain ratio. Here we take advantage of the invariant that the // Bayes' net error at zero is equal to the nonlinear error - const double rho = std::abs(f_error - result.f_error) < 1e-15 || std::abs(M_error - new_M_error) < 1e-15 ? 0.5 : (f_error - result.f_error) / (M_error - new_M_error); + const double rho = std::abs(f_error - result.f_error) < 1e-15 || std::abs(M_error - new_M_error) < 1e-15 + ? 0.5 + : (f_error - result.f_error) / (M_error - new_M_error); if (verbose) std::cout << std::setprecision(15) << "rho = " << rho << std::endl; @@ -263,4 +270,4 @@ typename DoglegOptimizerImplExt::IterationResult DoglegOptimizerImplExt::Iterate return result; } -} // namespace gtsam_ext +} // namespace gtsam_points diff --git a/include/gtsam_points/optimizers/fast_scatter.hpp b/include/gtsam_points/optimizers/fast_scatter.hpp new file mode 100644 index 00000000..9b606ed5 --- /dev/null +++ b/include/gtsam_points/optimizers/fast_scatter.hpp @@ -0,0 +1,31 @@ +/* ---------------------------------------------------------------------------- \ + \ \ + * GTSAM Copyright 2010, Georgia Tech Research Corporation, \ + * Atlanta, Georgia 30332-0415 \ + * All Rights Reserved \ + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) \ + \ \ + * See LICENSE for the license information \ + \ \ + * -------------------------------------------------------------------------- */ + +/** + * @file Scatter.h + * @brief Maps global variable indices to slot indices + * @author Richard Roberts + * @author Frank Dellaert + * @date June 2015 + */ + +#pragma once + +#include + +namespace gtsam_points { + +class FastScatter : public gtsam::Scatter { +public: + FastScatter(const gtsam::GaussianFactorGraph& gfg, const gtsam::Ordering& ordering); +}; + +} // namespace gtsam_points diff --git a/include/gtsam_ext/optimizers/gaussian_factor_graph_solver.hpp b/include/gtsam_points/optimizers/gaussian_factor_graph_solver.hpp similarity index 96% rename from include/gtsam_ext/optimizers/gaussian_factor_graph_solver.hpp rename to include/gtsam_points/optimizers/gaussian_factor_graph_solver.hpp index e6422661..fe3f978e 100644 --- a/include/gtsam_ext/optimizers/gaussian_factor_graph_solver.hpp +++ b/include/gtsam_points/optimizers/gaussian_factor_graph_solver.hpp @@ -6,7 +6,7 @@ #include #include -namespace gtsam_ext { +namespace gtsam_points { class DenseLinearSolver; class SparseLinearSolver; @@ -49,4 +49,4 @@ class GTSAMGaussianFactorGraphSolver : public GaussianFactorGraphSolver { virtual gtsam::VectorValues solve(const gtsam::GaussianFactorGraph& gfg, const gtsam::Ordering& ordering) override; }; -} // namespace gtsam_ext +} // namespace gtsam_points diff --git a/include/gtsam_ext/optimizers/gradient_descent.hpp b/include/gtsam_points/optimizers/gradient_descent.hpp similarity index 98% rename from include/gtsam_ext/optimizers/gradient_descent.hpp rename to include/gtsam_points/optimizers/gradient_descent.hpp index a84c3a8d..cb8fecbc 100644 --- a/include/gtsam_ext/optimizers/gradient_descent.hpp +++ b/include/gtsam_points/optimizers/gradient_descent.hpp @@ -11,7 +11,7 @@ #include #include -namespace gtsam_ext { +namespace gtsam_points { struct NumericalGradientDescentStatus { std::string to_string() const { return (boost::format("%d: error:%.6f -> %.6f lambda:%10g") % iterations % error_old % error_new % lambda).str(); } @@ -135,4 +135,4 @@ class NumericalGradientDescent : public gtsam::NonlinearOptimizer { std::chrono::high_resolution_clock::time_point optimization_start_time; }; -} // namespace gtsam_ext \ No newline at end of file +} // namespace gtsam_points \ No newline at end of file diff --git a/include/gtsam_ext/optimizers/incremental_fixed_lag_smoother_ext.hpp b/include/gtsam_points/optimizers/incremental_fixed_lag_smoother_ext.hpp similarity index 98% rename from include/gtsam_ext/optimizers/incremental_fixed_lag_smoother_ext.hpp rename to include/gtsam_points/optimizers/incremental_fixed_lag_smoother_ext.hpp index cab99cf6..90d759e5 100644 --- a/include/gtsam_ext/optimizers/incremental_fixed_lag_smoother_ext.hpp +++ b/include/gtsam_points/optimizers/incremental_fixed_lag_smoother_ext.hpp @@ -21,9 +21,9 @@ #pragma once #include -#include +#include -namespace gtsam_ext { +namespace gtsam_points { using namespace gtsam; @@ -135,4 +135,4 @@ class IncrementalFixedLagSmootherExt : public FixedLagSmoother { }; // IncrementalFixedLagSmootherExt -} // namespace gtsam_ext +} // namespace gtsam_points diff --git a/include/gtsam_ext/optimizers/incremental_fixed_lag_smoother_with_fallback.hpp b/include/gtsam_points/optimizers/incremental_fixed_lag_smoother_with_fallback.hpp similarity index 90% rename from include/gtsam_ext/optimizers/incremental_fixed_lag_smoother_with_fallback.hpp rename to include/gtsam_points/optimizers/incremental_fixed_lag_smoother_with_fallback.hpp index a8dabc1c..b560324f 100644 --- a/include/gtsam_ext/optimizers/incremental_fixed_lag_smoother_with_fallback.hpp +++ b/include/gtsam_points/optimizers/incremental_fixed_lag_smoother_with_fallback.hpp @@ -2,9 +2,10 @@ // Copyright (c) 2021 Kenji Koide (k.koide@aist.go.jp) #pragma once -#include +#include +#include -namespace gtsam_ext { +namespace gtsam_points { class IncrementalFixedLagSmootherExtWithFallback : public IncrementalFixedLagSmootherExt { public: @@ -17,6 +18,7 @@ class IncrementalFixedLagSmootherExtWithFallback : public IncrementalFixedLagSmo const KeyTimestampMap& timestamps = KeyTimestampMap(), const gtsam::FactorIndices& factorsToRemove = gtsam::FactorIndices()) override; + bool fallbackHappened() const { return fallback_happend; } gtsam::Values calculateEstimate() const override; const gtsam::Value& calculateEstimate(gtsam::Key key) const; @@ -61,10 +63,11 @@ class IncrementalFixedLagSmootherExtWithFallback : public IncrementalFixedLagSmo private: double current_stamp; mutable gtsam::Values values; + mutable std::atomic_bool fallback_happend; gtsam::NonlinearFactorGraph factors; std::unordered_map> factor_map; mutable gtsam::FixedLagSmootherKeyTimestampMap stamps; mutable std::unique_ptr smoother; }; -} // namespace gtsam_ext \ No newline at end of file +} // namespace gtsam_points \ No newline at end of file diff --git a/include/gtsam_ext/optimizers/isam2_ext.hpp b/include/gtsam_points/optimizers/isam2_ext.hpp similarity index 99% rename from include/gtsam_ext/optimizers/isam2_ext.hpp rename to include/gtsam_points/optimizers/isam2_ext.hpp index 5e43d4c5..1bc858a1 100644 --- a/include/gtsam_ext/optimizers/isam2_ext.hpp +++ b/include/gtsam_points/optimizers/isam2_ext.hpp @@ -28,11 +28,11 @@ #include #include -#include +#include #include -namespace gtsam_ext { +namespace gtsam_points { using namespace gtsam; @@ -323,4 +323,4 @@ class ISAM2Ext : public BayesTree { std::unique_ptr linearization_hook; }; // ISAM2 -} // namespace gtsam_ext +} // namespace gtsam_points diff --git a/include/gtsam_ext/optimizers/isam2_ext_dummy.hpp b/include/gtsam_points/optimizers/isam2_ext_dummy.hpp similarity index 75% rename from include/gtsam_ext/optimizers/isam2_ext_dummy.hpp rename to include/gtsam_points/optimizers/isam2_ext_dummy.hpp index fd4f2de1..911d8e70 100644 --- a/include/gtsam_ext/optimizers/isam2_ext_dummy.hpp +++ b/include/gtsam_points/optimizers/isam2_ext_dummy.hpp @@ -1,10 +1,13 @@ +// SPDX-License-Identifier: MIT +// Copyright (c) 2021 Kenji Koide (k.koide@aist.go.jp) + #pragma once -#include +#include -namespace gtsam_ext { +namespace gtsam_points { -class ISAM2ExtDummy : public gtsam_ext::ISAM2Ext { +class ISAM2ExtDummy : public gtsam_points::ISAM2Ext { public: explicit ISAM2ExtDummy(const ISAM2Params& params) {} virtual ~ISAM2ExtDummy() {} @@ -27,4 +30,4 @@ class ISAM2ExtDummy : public gtsam_ext::ISAM2Ext { return result; } }; -} // namespace gtsam_ext \ No newline at end of file +} // namespace gtsam_points \ No newline at end of file diff --git a/include/gtsam_ext/optimizers/isam2_ext_impl.hpp b/include/gtsam_points/optimizers/isam2_ext_impl.hpp similarity index 99% rename from include/gtsam_ext/optimizers/isam2_ext_impl.hpp rename to include/gtsam_points/optimizers/isam2_ext_impl.hpp index ae99b4bc..4e0bfe53 100644 --- a/include/gtsam_ext/optimizers/isam2_ext_impl.hpp +++ b/include/gtsam_points/optimizers/isam2_ext_impl.hpp @@ -18,7 +18,7 @@ #pragma once -#include +#include #include #include @@ -40,7 +40,7 @@ using namespace boost::adaptors; #include #include -namespace gtsam_ext { +namespace gtsam_points { using namespace gtsam; @@ -519,4 +519,4 @@ struct GTSAM_EXPORT UpdateImpl { } }; -} // namespace gtsam_ext +} // namespace gtsam_points diff --git a/include/gtsam_ext/optimizers/isam2_result_ext.hpp b/include/gtsam_points/optimizers/isam2_result_ext.hpp similarity index 96% rename from include/gtsam_ext/optimizers/isam2_result_ext.hpp rename to include/gtsam_points/optimizers/isam2_result_ext.hpp index bf1c0b79..3b196b20 100644 --- a/include/gtsam_ext/optimizers/isam2_result_ext.hpp +++ b/include/gtsam_points/optimizers/isam2_result_ext.hpp @@ -11,7 +11,7 @@ #include -namespace gtsam_ext { +namespace gtsam_points { struct ISAM2ResultExt : public gtsam::ISAM2Result { public: ISAM2ResultExt(bool enableDetailedResults = false) @@ -60,4 +60,4 @@ struct ISAM2ResultExt : public gtsam::ISAM2Result { double elapsed_time; }; -} // namespace gtsam_ext +} // namespace gtsam_points diff --git a/include/gtsam_ext/optimizers/levenberg_marquardt_ext.hpp b/include/gtsam_points/optimizers/levenberg_marquardt_ext.hpp similarity index 91% rename from include/gtsam_ext/optimizers/levenberg_marquardt_ext.hpp rename to include/gtsam_points/optimizers/levenberg_marquardt_ext.hpp index 654d4179..e0f25f01 100644 --- a/include/gtsam_ext/optimizers/levenberg_marquardt_ext.hpp +++ b/include/gtsam_points/optimizers/levenberg_marquardt_ext.hpp @@ -27,10 +27,10 @@ #include #include -#include -#include +#include +#include -namespace gtsam_ext { +namespace gtsam_points { class LinearizationHook; @@ -64,4 +64,4 @@ class LevenbergMarquardtOptimizerExt : public gtsam::NonlinearOptimizer { gtsam::GaussianFactorGraph::shared_ptr linearized; }; -} // namespace gtsam_ext +} // namespace gtsam_points diff --git a/include/gtsam_ext/optimizers/levenberg_marquardt_ext_params.hpp b/include/gtsam_points/optimizers/levenberg_marquardt_ext_params.hpp similarity index 82% rename from include/gtsam_ext/optimizers/levenberg_marquardt_ext_params.hpp rename to include/gtsam_points/optimizers/levenberg_marquardt_ext_params.hpp index 30dd7a24..96262802 100644 --- a/include/gtsam_ext/optimizers/levenberg_marquardt_ext_params.hpp +++ b/include/gtsam_points/optimizers/levenberg_marquardt_ext_params.hpp @@ -6,10 +6,10 @@ #include #include -#include -#include +#include +#include -namespace gtsam_ext { +namespace gtsam_points { class LevenbergMarquardtExtParams : public gtsam::LevenbergMarquardtParams { public: @@ -28,4 +28,4 @@ class LevenbergMarquardtExtParams : public gtsam::LevenbergMarquardtParams { std::function status_msg_callback; }; -} // namespace gtsam_ext +} // namespace gtsam_points diff --git a/include/gtsam_ext/optimizers/levenberg_marquardt_optimization_status.hpp b/include/gtsam_points/optimizers/levenberg_marquardt_optimization_status.hpp similarity index 94% rename from include/gtsam_ext/optimizers/levenberg_marquardt_optimization_status.hpp rename to include/gtsam_points/optimizers/levenberg_marquardt_optimization_status.hpp index 9645ccaa..67f3e503 100644 --- a/include/gtsam_ext/optimizers/levenberg_marquardt_optimization_status.hpp +++ b/include/gtsam_points/optimizers/levenberg_marquardt_optimization_status.hpp @@ -5,7 +5,7 @@ #include -namespace gtsam_ext { +namespace gtsam_points { struct LevenbergMarquardtOptimizationStatus { public: @@ -27,4 +27,4 @@ struct LevenbergMarquardtOptimizationStatus { double linear_solver_time; // time spent for solving the linear system }; -} // namespace gtsam_ext +} // namespace gtsam_points diff --git a/include/gtsam_ext/optimizers/linear_solver.hpp b/include/gtsam_points/optimizers/linear_solver.hpp similarity index 90% rename from include/gtsam_ext/optimizers/linear_solver.hpp rename to include/gtsam_points/optimizers/linear_solver.hpp index 48e4343f..7017cc9d 100644 --- a/include/gtsam_ext/optimizers/linear_solver.hpp +++ b/include/gtsam_points/optimizers/linear_solver.hpp @@ -6,7 +6,7 @@ #include #include -namespace gtsam_ext { +namespace gtsam_points { class DenseLinearSolver { public: @@ -22,4 +22,4 @@ class SparseLinearSolver { virtual Eigen::VectorXd solve(const Eigen::SparseMatrix& A, const Eigen::VectorXd& b) = 0; }; -} // namespace gtsam_ext +} // namespace gtsam_points diff --git a/include/gtsam_ext/optimizers/linear_system_builder.hpp b/include/gtsam_points/optimizers/linear_system_builder.hpp similarity index 90% rename from include/gtsam_ext/optimizers/linear_system_builder.hpp rename to include/gtsam_points/optimizers/linear_system_builder.hpp index c4fff009..7187831e 100644 --- a/include/gtsam_ext/optimizers/linear_system_builder.hpp +++ b/include/gtsam_points/optimizers/linear_system_builder.hpp @@ -5,8 +5,9 @@ #include #include +#include -namespace gtsam_ext { +namespace gtsam_points { /** * @brief A helper class to build a dense linear system from a Gaussian factor graph. @@ -27,7 +28,7 @@ class DenseLinearSystemBuilder { gtsam::VectorValues delta(const Eigen::VectorXd& x); public: - const gtsam::Scatter scatter; + const gtsam_points::FastScatter scatter; Eigen::MatrixXd A; Eigen::VectorXd b; @@ -53,7 +54,7 @@ class SparseLinearSystemBuilderBase { gtsam::VectorValues delta(const Eigen::VectorXd& x); public: - const gtsam::Scatter scatter; + const gtsam_points::FastScatter scatter; Eigen::SparseMatrix A; // Lower triangular Eigen::VectorXd b; @@ -71,4 +72,4 @@ class SparseLinearSystemBuilder : public SparseLinearSystemBuilderBase { virtual ~SparseLinearSystemBuilder() override {} }; -} // namespace gtsam_ext +} // namespace gtsam_points diff --git a/include/gtsam_ext/optimizers/linearization_hook.hpp b/include/gtsam_points/optimizers/linearization_hook.hpp similarity index 96% rename from include/gtsam_ext/optimizers/linearization_hook.hpp rename to include/gtsam_points/optimizers/linearization_hook.hpp index 6179ce82..e115eb6b 100644 --- a/include/gtsam_ext/optimizers/linearization_hook.hpp +++ b/include/gtsam_points/optimizers/linearization_hook.hpp @@ -6,7 +6,7 @@ #include #include -namespace gtsam_ext { +namespace gtsam_points { class NonlinearFactorSet { public: @@ -56,4 +56,4 @@ class LinearizationHook { std::vector> hooks; }; -} // namespace gtsam_ext \ No newline at end of file +} // namespace gtsam_points \ No newline at end of file diff --git a/include/gtsam_ext/types/dummy_frame.hpp b/include/gtsam_points/types/dummy_frame.hpp similarity index 88% rename from include/gtsam_ext/types/dummy_frame.hpp rename to include/gtsam_points/types/dummy_frame.hpp index 23dc5812..317e8486 100644 --- a/include/gtsam_ext/types/dummy_frame.hpp +++ b/include/gtsam_points/types/dummy_frame.hpp @@ -1,6 +1,9 @@ +// SPDX-License-Identifier: MIT +// Copyright (c) 2021 Kenji Koide (k.koide@aist.go.jp) + #pragma once -namespace gtsam_ext { +namespace gtsam_points { /// Dummy frame class for build test struct DummyFrame {}; @@ -26,4 +29,4 @@ struct traits { static const Eigen::Vector4d* points_ptr(const DummyFrame& frame) { return nullptr; } }; } // namespace frame -} // namespace gtsam_ext \ No newline at end of file +} // namespace gtsam_points \ No newline at end of file diff --git a/include/gtsam_ext/types/frame_traits.hpp b/include/gtsam_points/types/frame_traits.hpp similarity index 94% rename from include/gtsam_ext/types/frame_traits.hpp rename to include/gtsam_points/types/frame_traits.hpp index 17620a42..7bb58eea 100644 --- a/include/gtsam_ext/types/frame_traits.hpp +++ b/include/gtsam_points/types/frame_traits.hpp @@ -9,7 +9,7 @@ #include -namespace gtsam_ext { +namespace gtsam_points { namespace frame { @@ -40,7 +40,8 @@ template struct has_times_defined : std::false_type {}; template -struct has_times_defined::has_times), const T&>::value>> : std::true_type {}; +struct has_times_defined::has_times), const T&>::value>> +: std::true_type {}; template std::enable_if_t::value, bool> has_times(const T& t) { @@ -132,7 +133,7 @@ double time(const T& t, size_t i) { } template -auto point(const T& t, size_t i) { +decltype(auto) point(const T& t, size_t i) { return traits::point(t, i); } @@ -145,7 +146,7 @@ struct normal_defined::value>* = nullptr> -auto normal(const T& t, size_t i) { +decltype(auto) normal(const T& t, size_t i) { return traits::normal(t, i); } @@ -157,17 +158,17 @@ Eigen::Vector4d normal(const T& t, size_t i) { } template -auto cov(const T& t, size_t i) { +decltype(auto) cov(const T& t, size_t i) { return traits::cov(t, i); } template -auto intensity(const T& t, size_t i) { +decltype(auto) intensity(const T& t, size_t i) { return traits::intensity(t, i); } template -auto intensity_gradient(const T& t, size_t i) { +decltype(auto) intensity_gradient(const T& t, size_t i) { return traits::intensity_gradient(t, i); } @@ -198,4 +199,4 @@ auto intensity_gpu(const T& t, size_t i) { } // namespace frame -} // namespace gtsam_ext \ No newline at end of file +} // namespace gtsam_points \ No newline at end of file diff --git a/include/gtsam_points/types/gaussian_voxelmap.hpp b/include/gtsam_points/types/gaussian_voxelmap.hpp new file mode 100644 index 00000000..6717b2a4 --- /dev/null +++ b/include/gtsam_points/types/gaussian_voxelmap.hpp @@ -0,0 +1,128 @@ +// SPDX-License-Identifier: MIT +// Copyright (c) 2021 Kenji Koide (k.koide@aist.go.jp) + +#pragma once + +#include + +// forward declaration +struct CUstream_st; + +namespace gtsam_points { + +/** + * @brief Gaussian distribution voxelmap + */ +class GaussianVoxelMap { +public: + using Ptr = std::shared_ptr; + using ConstPtr = std::shared_ptr; + + GaussianVoxelMap() {} + virtual ~GaussianVoxelMap() {} + + /// Voxel resolution + virtual double voxel_resolution() const = 0; + + /// Insert a point cloud frame into the voxelmap + virtual void insert(const PointCloud& frame) = 0; +}; + +/** + * @brief Calculate the fraction of points fell in target's voxels + * @param target Target voxelized frame + * @param source Source frame + * @param T_target_source T_target_source + * @return Overlap rate + */ +double overlap(const GaussianVoxelMap::ConstPtr& target, const PointCloud::ConstPtr& source, const Eigen::Isometry3d& T_target_source); + +/** + * @brief Calculate the fraction of points fell in targets' voxels + * @param target Set of target voxelized frames + * @param source Source frame + * @param delta Set of T_target_source + * @return Overlap rate + */ +double overlap( + const std::vector& targets, + const PointCloud::ConstPtr& source, + const std::vector& Ts_target_source); + +/// @brief Calculate the fraction of points fell in targets' voxels +template +std::enable_if_t, double> +overlap(const std::vector& targets, const PointCloud::ConstPtr& source, const std::vector& Ts_target_source) { + const std::vector targets_(targets.begin(), targets.end()); + return overlap(targets_, source, Ts_target_source); +} + +/** + * @brief Calculate the fraction of points fell in target voxels on GPU + * @note Source points and target voxelmap must be pre-allocated on GPU. + * @param target Target voxelmap + * @param source Source frame + * @param delta_gpu T_target_source (on GPU memory) + * @return Overlap rate + */ +double overlap_gpu( + const GaussianVoxelMap::ConstPtr& target, + const PointCloud::ConstPtr& source, + const Eigen::Isometry3f* delta_gpu, + CUstream_st* stream = 0); + +/** + * @brief Calculate the fraction of points fell in target voxels on GPU + * @note Source points and target voxelmap must be pre-allocated on GPU. + * @param target Target voxelmap + * @param source Source frame + * @param delta T_target_source + * @return Overlap rate + */ +double +overlap_gpu(const GaussianVoxelMap::ConstPtr& target, const PointCloud::ConstPtr& source, const Eigen::Isometry3d& delta, CUstream_st* stream = 0); + +/// @brief Calculate the fraction of points fell in target voxels on GPU +double overlap_gpu(const PointCloud::ConstPtr& target, const PointCloud::ConstPtr& source, const Eigen::Isometry3d& delta, CUstream_st* stream = 0); + +/** + * @brief Calculate the fraction of points fell in targets' voxels on GPU + * @note Source points and targets' voxelmap must be pre-allocated on GPU. + * @param targets Set of target voxelized frames + * @param source Source frame + * @param deltas Set of T_target_source + * @return Overlap rate + */ +double overlap_gpu( + const std::vector& targets, + const PointCloud::ConstPtr& source, + const std::vector& deltas, + CUstream_st* stream = 0); + +/// @brief Calculate the fraction of points fell in targets' voxels on GPU +template +std::enable_if_t, double> overlap_gpu( + const std::vector& targets, + const PointCloud::ConstPtr& source, + const std::vector& deltas, + CUstream_st* stream = 0) { + const std::vector targets_(targets.begin(), targets.end()); + return overlap_gpu(targets_, source, deltas, stream); +} + +// Automatically select CPU or GPU method +double overlap_auto(const GaussianVoxelMap::ConstPtr& target, const PointCloud::ConstPtr& source, const Eigen::Isometry3d& delta); + +double overlap_auto( + const std::vector& targets, + const PointCloud::ConstPtr& source, + const std::vector& deltas); + +template +std::enable_if_t, double> +overlap_auto(const std::vector& targets, const PointCloud::ConstPtr& source, const std::vector& deltas) { + const std::vector targets_(targets.begin(), targets.end()); + return overlap_auto(targets_, source, deltas); +} + +} // namespace gtsam_points \ No newline at end of file diff --git a/include/gtsam_points/types/gaussian_voxelmap_cpu.hpp b/include/gtsam_points/types/gaussian_voxelmap_cpu.hpp new file mode 100644 index 00000000..e3a8ca61 --- /dev/null +++ b/include/gtsam_points/types/gaussian_voxelmap_cpu.hpp @@ -0,0 +1,127 @@ +// SPDX-FileCopyrightText: Copyright 2024 Kenji Koide +// SPDX-License-Identifier: MIT +#pragma once + +#include +#include +#include +#include + +namespace gtsam_points { + +/// @brief Gaussian voxel that computes and stores voxel mean and covariance. +struct GaussianVoxel { +public: + using Ptr = std::shared_ptr; + using ConstPtr = std::shared_ptr; + + struct Setting {}; + + /// @brief Constructor. + GaussianVoxel() : finalized(false), num_points(0), mean(Eigen::Vector4d::Zero()), cov(Eigen::Matrix4d::Zero()) {} + + /// @brief Number of points in the voxel (Always 1 for GaussianVoxel). + size_t size() const { return 1; } + + /// @brief Add a point to the voxel. + /// @param setting Setting + /// @param transformed_pt Transformed point mean + /// @param points Point cloud + /// @param i Index of the point + /// @param T Transformation matrix + void add(const Setting& setting, const PointCloud& points, size_t i); + + /// @brief Finalize the voxel mean and covariance. + void finalize(); + + /// @brief Find k nearest neighbors. + /// @param pt Query point + /// @param result Result + template + void knn_search(const Eigen::Vector4d& pt, Result& result) const { + const double sq_dist = (pt - mean).squaredNorm(); + result.push(0, sq_dist); + } + +public: + bool finalized; ///< If true, mean and cov are finalized, otherwise they represent the sum of input points + size_t num_points; ///< Number of input points + Eigen::Vector4d mean; ///< Mean + Eigen::Matrix4d cov; ///< Covariance +}; + +namespace frame { + +template <> +struct traits { + static int size(const GaussianVoxel& frame) { return frame.size(); } + + static bool has_points(const GaussianVoxel& frame) { return true; } + static bool has_normals(const GaussianVoxel& frame) { return false; } + static bool has_covs(const GaussianVoxel& frame) { return true; } + static bool has_intensities(const GaussianVoxel& frame) { return false; } + + static const Eigen::Vector4d& point(const GaussianVoxel& frame, size_t i) { return frame.mean; } + static const Eigen::Vector4d normal(const GaussianVoxel& frame, size_t i) { return Eigen::Vector4d::Zero(); } + static const Eigen::Matrix4d& cov(const GaussianVoxel& frame, size_t i) { return frame.cov; } + static double intensity(const GaussianVoxel& frame, size_t i) { return 0.0; } +}; + +} // namespace frame + +class GaussianVoxelMapCPU : public GaussianVoxelMap, public IncrementalVoxelMap { +public: + using Ptr = std::shared_ptr; + using ConstPtr = std::shared_ptr; + + /// @brief Constructor. + /// @param resolution Voxel resolution + GaussianVoxelMapCPU(double resolution); + virtual ~GaussianVoxelMapCPU(); + + /// Voxel resolution + virtual double voxel_resolution() const override; + + /// @brief Compute the voxel index corresponding to a point. + Eigen::Vector3i voxel_coord(const Eigen::Vector4d& x) const; + + /// @brief Look up a voxel index. If the voxel does not exist, return -1. + int lookup_voxel_index(const Eigen::Vector3i& coord) const; + + /// @brief Look up a voxel. + const GaussianVoxel& lookup_voxel(int voxel_id) const; + + /// @brief Insert a point cloud frame into the voxelmap. + virtual void insert(const PointCloud& frame) override; + + /** + * @brief Save the voxelmap + * @param path Destination path to save the voxelmap + */ + void save_compact(const std::string& path) const; + + /** + * @brief Save a voxelmap from a file + * @param path Path to a voxelmap file to be loaded + */ + static GaussianVoxelMapCPU::Ptr load(const std::string& path); +}; + +namespace frame { + +template <> +struct traits { + static bool has_points(const GaussianVoxelMapCPU& ivox) { return ivox.has_points(); } + static bool has_normals(const GaussianVoxelMapCPU& ivox) { return ivox.has_normals(); } + static bool has_covs(const GaussianVoxelMapCPU& ivox) { return ivox.has_covs(); } + static bool has_intensities(const GaussianVoxelMapCPU& ivox) { return ivox.has_intensities(); } + + static decltype(auto) point(const GaussianVoxelMapCPU& ivox, size_t i) { return ivox.point(i); } + static decltype(auto) normal(const GaussianVoxelMapCPU& ivox, size_t i) { return ivox.normal(i); } + static decltype(auto) cov(const GaussianVoxelMapCPU& ivox, size_t i) { return ivox.cov(i); } + static decltype(auto) intensity(const GaussianVoxelMapCPU& ivox, size_t i) { return ivox.intensity(i); } +}; + +} // namespace frame + +} // namespace gtsam_points diff --git a/include/gtsam_ext/types/gaussian_voxelmap_gpu.hpp b/include/gtsam_points/types/gaussian_voxelmap_gpu.hpp similarity index 81% rename from include/gtsam_ext/types/gaussian_voxelmap_gpu.hpp rename to include/gtsam_points/types/gaussian_voxelmap_gpu.hpp index af91ca16..b2a0c089 100644 --- a/include/gtsam_ext/types/gaussian_voxelmap_gpu.hpp +++ b/include/gtsam_points/types/gaussian_voxelmap_gpu.hpp @@ -6,7 +6,7 @@ #include #include -#include +#include // forward declaration struct CUstream_st; @@ -17,7 +17,7 @@ class pair; } // namespace thrust -namespace gtsam_ext { +namespace gtsam_points { /** * @brief Voxel hashmap information @@ -58,18 +58,18 @@ class GaussianVoxelMapGPU : public GaussianVoxelMap { /// Insert a point cloud frame into the voxelmap /// @note Incremental insertion is not supported for GPU - virtual void insert(const Frame& frame) override; + virtual void insert(const PointCloud& frame) override; private: - void create_bucket_table(CUstream_st* stream, const Frame& frame); + void create_bucket_table(CUstream_st* stream, const PointCloud& frame); public: CUstream_st* stream; - const int init_num_buckets; ///< Initial number of buckets - const double target_points_drop_rate; ///< Allowable points drop rate - VoxelMapInfo voxelmap_info; ///< Voxelmap information - VoxelMapInfo* voxelmap_info_ptr; ///< Voxelmap information on GPU memory + const int init_num_buckets; ///< Initial number of buckets + const double target_points_drop_rate; ///< Allowable points drop rate + VoxelMapInfo voxelmap_info; ///< Voxelmap information + VoxelMapInfo* voxelmap_info_ptr; ///< Voxelmap information on GPU memory thrust::pair* buckets; ///< Voxel buckets for hashing @@ -82,4 +82,4 @@ class GaussianVoxelMapGPU : public GaussianVoxelMap { std::vector download_voxel_means(const GaussianVoxelMapGPU& voxelmap, CUstream_st* stream = nullptr); std::vector download_voxel_covs(const GaussianVoxelMapGPU& voxelmap, CUstream_st* stream = nullptr); -} // namespace gtsam_ext \ No newline at end of file +} // namespace gtsam_points \ No newline at end of file diff --git a/include/gtsam_points/types/point_cloud.hpp b/include/gtsam_points/types/point_cloud.hpp new file mode 100644 index 00000000..ea982b8c --- /dev/null +++ b/include/gtsam_points/types/point_cloud.hpp @@ -0,0 +1,149 @@ +// SPDX-License-Identifier: MIT +// Copyright (c) 2021 Kenji Koide (k.koide@aist.go.jp) + +#pragma once + +#include +#include +#include +#include +#include +#include + +namespace gtsam_points { + +/** + * @brief Standard point cloud class that holds only pointers to point attributes + * @note If you don't want to manage the lifetime of point data by yourself, use gtsam_points::PointCloudCPU. + */ +struct PointCloud { +public: + using Ptr = std::shared_ptr; + using ConstPtr = std::shared_ptr; + + PointCloud() + : num_points(0), + times(nullptr), + points(nullptr), + normals(nullptr), + covs(nullptr), + intensities(nullptr), + times_gpu(nullptr), + points_gpu(nullptr), + normals_gpu(nullptr), + covs_gpu(nullptr), + intensities_gpu(nullptr) {} + virtual ~PointCloud() {} + + // Forbid copy + PointCloud(const PointCloud&) = delete; + PointCloud& operator=(PointCloud const&) = delete; + + /// Number of points + size_t size() const { return num_points; } + + bool has_times() const; ///< Check if the point cloud has per-point timestamps + bool has_points() const; ///< Check if the point cloud has points + bool has_normals() const; ///< Check if the point cloud has point normals + bool has_covs() const; ///< Check if the point cloud has point covariances + bool has_intensities() const; ///< Check if the point cloud has point intensities + + bool check_times() const; ///< Warn if the point cloud doesn't have times + bool check_points() const; ///< Warn if the point cloud doesn't have points + bool check_normals() const; ///< Warn if the point cloud doesn't have normals + bool check_covs() const; ///< Warn if the point cloud doesn't have covs + bool check_intensities() const; ///< Warn if the point cloud doesn't have intensities + + bool has_times_gpu() const; ///< Check if the point cloud has per-point timestamps on GPU + bool has_points_gpu() const; ///< Check if the point cloud has points on GPU + bool has_normals_gpu() const; ///< Check if the point cloud has point normals on GPU + bool has_covs_gpu() const; ///< Check if the point cloud has point covariances on GPU + bool has_intensities_gpu() const; ///< Check if the point cloud has point intensities on GPU + + bool check_times_gpu() const; ///< Warn if the point cloud doesn't have times on GPU + bool check_points_gpu() const; ///< Warn if the point cloud doesn't have points on GPU + bool check_normals_gpu() const; ///< Warn if the point cloud doesn't have normals on GPU + bool check_covs_gpu() const; ///< Warn if the point cloud doesn't have covs on GPU + bool check_intensities_gpu() const; ///< Warn if the point cloud doesn't have intensities on GPU + + /** + * @brief Get the pointer to an aux attribute + * @param attrib Attribute name + * @return Returns the pointer to it if the specified attribute exists. Otherwise, returns nullptr. + */ + template + const T* aux_attribute(const std::string& attrib) const { + const auto found = aux_attributes.find(attrib); + if (found == aux_attributes.end()) { + std::cerr << "warning: attribute " << attrib << " not found!!" << std::endl; + return nullptr; + } + + if (sizeof(T) != found->second.first) { + std::cerr << "warning: attribute element size mismatch!! attrib:" << attrib << " size:" << found->second.first << " requested:" << sizeof(T) + << std::endl; + } + + return static_cast(found->second.second); + } + + /** + * @brief Save the point cloud data + * @param path Destination path + */ + void save(const std::string& path) const; + + /** + * @brief Save the point cloud data with a compact representation without unnecessary fields (e.g., the last element of homogeneous coordinates). + * @param path + */ + void save_compact(const std::string& path) const; + +public: + size_t num_points; ///< Number of points + + double* times; ///< Per-point timestamp w.r.t. the first point (should be sorted) + Eigen::Vector4d* points; ///< Point coordinates (x, y, z, 1) + Eigen::Vector4d* normals; ///< Point normals (nx, ny, nz, 0) + Eigen::Matrix4d* covs; ///< Point covariances cov(3, 3) = 0 + double* intensities; ///< Point intensities + + /// Aux attributes > + std::unordered_map> aux_attributes; + + float* times_gpu; ///< Per-point timestamp on GPU + Eigen::Vector3f* points_gpu; ///< Point coordinates on GPU + Eigen::Vector3f* normals_gpu; ///< Point normals on GPU + Eigen::Matrix3f* covs_gpu; ///< Point covariances on GPU + float* intensities_gpu; ///< Point intensities on GPU +}; +} // namespace gtsam_points + +#ifndef DONT_DEFINE_FRAME_TRAITS +#include + +namespace gtsam_points { +namespace frame { + +template <> +struct traits { + static int size(const PointCloud& frame) { return frame.size(); } + + static bool has_times(const PointCloud& frame) { return frame.has_times(); } + static bool has_points(const PointCloud& frame) { return frame.has_points(); } + static bool has_normals(const PointCloud& frame) { return frame.has_normals(); } + static bool has_covs(const PointCloud& frame) { return frame.has_covs(); } + static bool has_intensities(const PointCloud& frame) { return frame.has_intensities(); } + + static double time(const PointCloud& frame, size_t i) { return frame.times[i]; } + static const Eigen::Vector4d& point(const PointCloud& frame, size_t i) { return frame.points[i]; } + static const Eigen::Vector4d& normal(const PointCloud& frame, size_t i) { return frame.normals[i]; } + static const Eigen::Matrix4d& cov(const PointCloud& frame, size_t i) { return frame.covs[i]; } + static double intensity(const PointCloud& frame, size_t i) { return frame.intensities[i]; } + + static const Eigen::Vector4d* points_ptr(const PointCloud& frame) { return frame.points; } +}; + +} // namespace frame +} // namespace gtsam_points +#endif diff --git a/include/gtsam_points/types/point_cloud_cpu.hpp b/include/gtsam_points/types/point_cloud_cpu.hpp new file mode 100644 index 00000000..5eadcf10 --- /dev/null +++ b/include/gtsam_points/types/point_cloud_cpu.hpp @@ -0,0 +1,328 @@ +// SPDX-License-Identifier: MIT +// Copyright (c) 2021 Kenji Koide (k.koide@aist.go.jp) + +#pragma once + +#include +#include +#include + +#include + +struct CUstream_st; + +namespace gtsam_points { + +/** + * @brief Point cloud frame on CPU memory + */ +struct PointCloudCPU : public PointCloud { +public: + using Ptr = std::shared_ptr; + using ConstPtr = std::shared_ptr; + + PointCloudCPU(); + ~PointCloudCPU(); + + /** + * @brief Constructor + * @param points Pointer to point data + * @param num_points Number of points + */ + template + PointCloudCPU(const Eigen::Matrix* points, int num_points); + + /** + * @brief Constructor + * @param points Points + */ + template + PointCloudCPU(const std::vector, Alloc>& points) : PointCloudCPU(points.data(), points.size()) {} + + // Forbid shallow copy + PointCloudCPU(const PointCloudCPU& points) = delete; + PointCloudCPU& operator=(PointCloudCPU const&) = delete; + + /// Deep copy + static PointCloudCPU::Ptr clone(const PointCloud& points); + + template + void add_times(const T* times, int num_points); + template + void add_times(const std::vector& times) { + add_times(times.data(), times.size()); + } + + template + void add_points(const Eigen::Matrix* points, int num_points); + template + void add_points(const std::vector, Alloc>& points) { + add_points(points.data(), points.size()); + } + + template + void add_normals(const Eigen::Matrix* normals, int num_points); + template + void add_normals(const std::vector, Alloc>& normals) { + add_normals(normals.data(), normals.size()); + } + + template + void add_covs(const Eigen::Matrix* covs, int num_points); + template + void add_covs(const std::vector, Alloc>& covs) { + add_covs(covs.data(), covs.size()); + } + + template + void add_intensities(const T* intensities, int num_points); + template + void add_intensities(const std::vector& intensities) { + add_intensities(intensities.data(), intensities.size()); + } + + template + void add_aux_attribute(const std::string& attrib_name, const T* values, int num_points) { + auto attributes = std::make_shared>(values, values + num_points); + aux_attributes_storage[attrib_name] = attributes; + aux_attributes[attrib_name] = std::make_pair(sizeof(T), attributes->data()); + } + template + void add_aux_attribute(const std::string& attrib_name, const std::vector& values) { + add_aux_attribute(attrib_name, values.data(), values.size()); + } + + static PointCloudCPU::Ptr load(const std::string& path); + +public: + std::vector times_storage; + std::vector points_storage; + std::vector normals_storage; + std::vector covs_storage; + std::vector intensities_storage; + + std::unordered_map> aux_attributes_storage; +}; + +/** + * @brief Sample points by indices. + * @param points Input points + * @param indices Point indices + * @return Sampled points + */ +PointCloudCPU::Ptr sample(const PointCloud::ConstPtr& points, const std::vector& indices); + +/** + * @brief Naive random sampling. + * @param points Input points + * @param sampling_rate Random sampling rate in [0, 1] + * @param mt RNG + * @return Downsampled points + */ +PointCloudCPU::Ptr random_sampling(const PointCloud::ConstPtr& points, const double sampling_rate, std::mt19937& mt); + +/** + * @brief Voxel grid downsampling. + * @note This algorithm takes the average of point attributes (whatever it is) of each voxel. + * + * @param points Input points + * @param voxel_resolution Voxel resolution + * @param num_threads Number of threads + * @return Downsampled points + */ +PointCloudCPU::Ptr voxelgrid_sampling(const PointCloud::ConstPtr& points, const double voxel_resolution, int num_threads = 1); + +/** + * @brief Voxel grid random sampling. + * @note This algorithm randomly samples points such that the number of sampled points of each voxel becomes (more or less) the same. + * This algorithm avoids mixing point attributes (unlike the standard voxelgrid downsampling), and thus can provide spatially + * well-distributed point samples with several attributes (e.g., normals and covs). + * + * @param points Input points + * @param voxel_resolution Voxel resolution + * @param sampling_rate Random sampling rate in [0, 1] + * @param mt RNG + * @param num_threads Number of threads + * @return Downsampled points + */ +PointCloudCPU::Ptr randomgrid_sampling( + const PointCloud::ConstPtr& points, + const double voxel_resolution, + const double sampling_rate, + std::mt19937& mt, + int num_threads = 1); + +/** + * @brief Extract points for which pred returns true. + * @param points Input points + * @param pred Predicate function that takes Eigen::Vector4d and returns bool + */ +template +PointCloudCPU::Ptr filter(const PointCloud::ConstPtr& points, const Func& pred) { + std::vector indices; + indices.reserve(points->size()); + for (int i = 0; i < points->size(); i++) { + if (pred(points->points[i])) { + indices.push_back(i); + } + } + return sample(points, indices); +} + +/** + * @brief Extract points for which pred returns true. + * @param points Input points + * @param pred Predicate function that takes a point index and returns bool + */ +template +PointCloudCPU::Ptr filter_by_index(const PointCloud::ConstPtr& points, const Func& pred) { + std::vector indices; + indices.reserve(points->size()); + for (int i = 0; i < points->size(); i++) { + if (pred(i)) { + indices.push_back(i); + } + } + return sample(points, indices); +} + +/** + * @brief Sort points + * @param points Input points + * @param pred Comparison function that takes two point indices (lhs and rhs) and returns true if lhs < rhs + */ +template +PointCloudCPU::Ptr sort(const PointCloud::ConstPtr& points, const Compare& comp) { + std::vector indices(points->size()); + std::iota(indices.begin(), indices.end(), 0); + std::sort(indices.begin(), indices.end(), comp); + return gtsam_points::sample(points, indices); +} + +/** + * @brief Sort points by time + * @param points Input points + */ +PointCloudCPU::Ptr sort_by_time(const PointCloud::ConstPtr& points); + +/** + * @brief Transform points, normals, and covariances + * + * @param points Input points + * @param transformation Transformation + * @return Transformed points + */ +template +PointCloudCPU::Ptr transform(const PointCloud::ConstPtr& points, const Eigen::Transform& transformation); + +/** + * @brief Transform points, normals, and covariances inplace + * @param points [in/out] Points to be transformed + * @param transformation Transformation + */ +template +void transform_inplace(PointCloud& points, const Eigen::Transform& transformation); + +/** + * @brief Transform points, normals, and covariances inplace + * @param points [in/out] Points to be transformed + * @param transformation Transformation + */ +template +void transform_inplace(PointCloud::Ptr points, const Eigen::Transform& transformation) { + transform_inplace(*points, transformation); +} + +/** + * @brief Statistical outlier removal + * @param points Input points + * @param neighbors Neighbor indices + * @param k Number of neighbors (neighbors.size() must be >= points->size() * k) + * @param std_thresh Standard deviation multiplication threshold + * @return Inlier point indices + */ +std::vector +find_inlier_points(const PointCloud::ConstPtr& points, const std::vector& neighbors, const int k, const double std_thresh = 1.0); + +/** + * @brief Statistical outlier removal + * @param points Input points + * @param neighbors Neighbor indices + * @param k Number of neighbors (neighbors.size() must be >= points->size() * k) + * @param std_thresh Standard deviation multiplication threshold + * @return Filtered point cloud + */ +PointCloudCPU::Ptr remove_outliers(const PointCloud::ConstPtr& points, const std::vector& neighbors, const int k, const double std_thresh = 1.0); + +/** + * @brief Statistical outlier removal + * @param points Input points + * @param k Number of neighbors (neighbors.size() must be >= points->size() * k) + * @param std_thresh Standard deviation multiplication threshold + * @return Filtered point cloud + */ +PointCloudCPU::Ptr remove_outliers(const PointCloud::ConstPtr& points, const int k = 10, const double std_thresh = 1.0, const int num_threads = 1); + +/** + * @brief Merge a set of frames into one frame + * @note This function only merges points and covs and discard other point attributes. + * @param poses Poses of input frames + * @param frames Input frames + * @param downsample_resolution Downsampling resolution + * @return Merged frame + */ +PointCloud::Ptr +merge_frames(const std::vector& poses, const std::vector& frames, double downsample_resolution); + +/// @brief Merge a set of frames into one frame +template +std::enable_if_t, PointCloud::Ptr> +merge_frames(const std::vector& poses, const std::vector& frames, double downsample_resolution) { + std::vector frames_(frames.begin(), frames.end()); + return merge_frames(poses, frames_, downsample_resolution); +} + +/** + * @brief Merge a set of frames into one frame on the GPU + * @note This function only merges points and covs and discard other point attributes. + * @param poses Poses of input frames + * @param frames Input frames (must be PointCloudGPU) + * @param downsample_resolution Downsampling resolution + * @param stream CUDA stream + * @return Merged frame (PointCloudGPU) + */ +PointCloud::Ptr merge_frames_gpu( + const std::vector& poses, + const std::vector& frames, + double downsample_resolution, + CUstream_st* stream = 0); + +template +std::enable_if_t, PointCloud::Ptr> merge_frames_gpu( + const std::vector& poses, + const std::vector& frames, + double downsample_resolution, + CUstream_st* stream = 0) { + std::vector frames_(frames.begin(), frames.end()); + return merge_frames_gpu(poses, frames_, downsample_resolution, stream); +} + +/** + * @brief Merge a set of frames into one frame. The device (CPU or GPU) to run the algorithm is automatically selected. + * @note This function only merges points and covs and discard other point attributes. + * @param poses Poses of input frames + * @param frames Input frames + * @param downsample_resolution Downsampling resolution + * @return Merged frame + */ +PointCloud::Ptr +merge_frames_auto(const std::vector& poses, const std::vector& frames, double downsample_resolution); + +template +std::enable_if_t, PointCloud::Ptr> +merge_frames_auto(const std::vector& poses, const std::vector& frames, double downsample_resolution) { + std::vector frames_(frames.begin(), frames.end()); + return merge_frames_auto(poses, frames_, downsample_resolution); +} + +} // namespace gtsam_points diff --git a/include/gtsam_ext/types/frame_gpu.hpp b/include/gtsam_points/types/point_cloud_gpu.hpp similarity index 70% rename from include/gtsam_ext/types/frame_gpu.hpp rename to include/gtsam_points/types/point_cloud_gpu.hpp index d799cf01..967c6798 100644 --- a/include/gtsam_ext/types/frame_gpu.hpp +++ b/include/gtsam_points/types/point_cloud_gpu.hpp @@ -6,37 +6,42 @@ #include #include -#include -#include +#include +#include // forward declaration struct CUstream_st; -namespace gtsam_ext { +namespace gtsam_points { /** * @brief Point cloud frame on GPU memory */ -struct FrameGPU : public FrameCPU { +struct PointCloudGPU : public PointCloudCPU { public: - using Ptr = std::shared_ptr; - using ConstPtr = std::shared_ptr; + using Ptr = std::shared_ptr; + using ConstPtr = std::shared_ptr; + + PointCloudGPU(); + ~PointCloudGPU(); template - FrameGPU(const Eigen::Matrix* points, int num_points); + PointCloudGPU(const Eigen::Matrix* points, int num_points); template - FrameGPU(const std::vector, Alloc>& points) : FrameGPU(points.data(), points.size()) {} + PointCloudGPU(const std::vector, Alloc>& points) : PointCloudGPU(points.data(), points.size()) {} - FrameGPU(const Frame& frame, CUstream_st* stream = 0); + // Forbid shallow copy + PointCloudGPU(const PointCloudGPU& points) = delete; + PointCloudGPU& operator=(PointCloudGPU const&) = delete; - FrameGPU(); - ~FrameGPU(); + // Deep copy + static PointCloudGPU::Ptr clone(const PointCloud& frame, CUstream_st* stream = 0); // add_*_gpu() only adds attributes to GPU storage template void add_times(const T* times, int num_points, CUstream_st* stream = 0) { - FrameCPU::add_times(times, num_points); + PointCloudCPU::add_times(times, num_points); add_times_gpu(times, num_points, stream); } template @@ -53,7 +58,7 @@ struct FrameGPU : public FrameCPU { template void add_points(const Eigen::Matrix* points, int num_points, CUstream_st* stream = 0) { - FrameCPU::add_points(points, num_points); + PointCloudCPU::add_points(points, num_points); add_points_gpu(points, num_points, stream); } template @@ -70,7 +75,7 @@ struct FrameGPU : public FrameCPU { template void add_normals(const Eigen::Matrix* normals, int num_points, CUstream_st* stream = 0) { - FrameCPU::add_normals(normals, num_points); + PointCloudCPU::add_normals(normals, num_points); add_normals_gpu(normals, num_points, stream); } template @@ -87,7 +92,7 @@ struct FrameGPU : public FrameCPU { template void add_covs(const Eigen::Matrix* covs, int num_points, CUstream_st* stream = 0) { - FrameCPU::add_covs(covs, num_points); + PointCloudCPU::add_covs(covs, num_points); add_covs_gpu(covs, num_points, stream); } template @@ -104,7 +109,7 @@ struct FrameGPU : public FrameCPU { template void add_intensities(const T* intensities, int num_points, CUstream_st* stream = 0) { - FrameCPU::add_intensities(intensities, num_points); + PointCloudCPU::add_intensities(intensities, num_points); add_intensities_gpu(intensities, num_points, stream); } template @@ -123,8 +128,10 @@ struct FrameGPU : public FrameCPU { }; // Device to host data transfer -std::vector download_points_gpu(const gtsam_ext::Frame& frame, CUstream_st* stream = nullptr); -std::vector download_covs_gpu(const gtsam_ext::Frame& frame, CUstream_st* stream = nullptr); -std::vector download_normals_gpu(const gtsam_ext::Frame& frame, CUstream_st* stream = nullptr); +std::vector download_points_gpu(const gtsam_points::PointCloud& frame, CUstream_st* stream = nullptr); +std::vector download_covs_gpu(const gtsam_points::PointCloud& frame, CUstream_st* stream = nullptr); +std::vector download_normals_gpu(const gtsam_points::PointCloud& frame, CUstream_st* stream = nullptr); +std::vector download_intensities_gpu(const gtsam_points::PointCloud& frame, CUstream_st* stream = nullptr); +std::vector download_times_gpu(const gtsam_points::PointCloud& frame, CUstream_st* stream = nullptr); -} // namespace gtsam_ext +} // namespace gtsam_points diff --git a/include/gtsam_ext/util/bspline.hpp b/include/gtsam_points/util/bspline.hpp similarity index 99% rename from include/gtsam_ext/util/bspline.hpp rename to include/gtsam_points/util/bspline.hpp index 0708aee2..8d58ebd2 100644 --- a/include/gtsam_ext/util/bspline.hpp +++ b/include/gtsam_points/util/bspline.hpp @@ -5,7 +5,7 @@ #include -namespace gtsam_ext { +namespace gtsam_points { /** * @brief B-Spline pose interpolation @@ -148,4 +148,4 @@ inline gtsam::Vector6_ bspline_imu(const gtsam::Key key1, const gtsam::Double_& return bspline_imu(gtsam::Pose3_(key1 - 1), gtsam::Pose3_(key1), gtsam::Pose3_(key1 + 1), gtsam::Pose3_(key1 + 2), t, knot_interval, g); } -} // namespace gtsam_ext +} // namespace gtsam_points diff --git a/include/gtsam_ext/util/continuous_trajectory.hpp b/include/gtsam_points/util/continuous_trajectory.hpp similarity index 76% rename from include/gtsam_ext/util/continuous_trajectory.hpp rename to include/gtsam_points/util/continuous_trajectory.hpp index 6ebf1b71..451a16be 100644 --- a/include/gtsam_ext/util/continuous_trajectory.hpp +++ b/include/gtsam_points/util/continuous_trajectory.hpp @@ -12,7 +12,7 @@ namespace gtsam { class LevenbergMarquardtParams; } -namespace gtsam_ext { +namespace gtsam_points { /** * @brief Continuous trajectory class for offline batch optimization @@ -27,50 +27,50 @@ class ContinuousTrajectory { * @param end_time End time of the trajectory * @param knot_interval Time interval of spline control knots */ - ContinuousTrajectory(const char symbol, const double start_time, const double end_time, const double knot_interval); + ContinuousTrajectory(char symbol, double start_time, double end_time, double knot_interval); /** * @brief Time of a spline knot */ - const double knot_stamp(const int i) const; + double knot_stamp(int i) const; /** * @brief Key knot ID for a given time */ - const int knot_id(const double t) const; + int knot_id(double t) const; /** * @brief Number of spline knots */ - const int knot_max_id() const; + int knot_max_id() const; /** * @brief Get an expression of the interpolated time at t * @param t Time * @param t_ Expression of t */ - const gtsam::Pose3_ pose(const double t, const gtsam::Double_& t_); + gtsam::Pose3_ pose(double t, const gtsam::Double_& t_); /** * @brief Calculate the interpolated time at t * @param values Values including knot poses * @param t Time */ - const gtsam::Pose3 pose(const gtsam::Values& values, const double t); + gtsam::Pose3 pose(const gtsam::Values& values, double t); /** * @brief Get an expression of the linear acceleration and angular velocity at t * @param t Time * @param t_ Expression of t */ - const gtsam::Vector6_ imu(const double t, const gtsam::Double_& t_, const Eigen::Vector3d& g = Eigen::Vector3d(0.0, 0.0, 9.80665)); + gtsam::Vector6_ imu(double t, const gtsam::Double_& t_, const Eigen::Vector3d& g = Eigen::Vector3d(0.0, 0.0, 9.80665)); /** * @brief Calculate the linear acceleration and angular velocity at t * @param values Values including knot poses * @param t Time */ - const gtsam::Vector6 imu(const gtsam::Values& values, const double t, const Eigen::Vector3d& g = Eigen::Vector3d(0.0, 0.0, 9.80665)); + gtsam::Vector6 imu(const gtsam::Values& values, double t, const Eigen::Vector3d& g = Eigen::Vector3d(0.0, 0.0, 9.80665)); /** * @brief Optimize spline knots to fit the interpolated trajectory to a set of poses @@ -95,4 +95,4 @@ class ContinuousTrajectory { const double knot_interval; }; -} // namespace gtsam_ext +} // namespace gtsam_points diff --git a/include/gtsam_points/util/covariance_estimation.hpp b/include/gtsam_points/util/covariance_estimation.hpp new file mode 100644 index 00000000..00b33795 --- /dev/null +++ b/include/gtsam_points/util/covariance_estimation.hpp @@ -0,0 +1,55 @@ +// SPDX-License-Identifier: MIT +// Copyright (c) 2021 Kenji Koide (k.koide@aist.go.jp) + +#pragma once + +#include +#include + +namespace gtsam_points { + +/** + * @brief Covariance estimation parameters + */ +struct CovarianceEstimationParams { +public: + enum RegularizationMethod { NONE, EIG }; + + CovarianceEstimationParams() : num_threads(1), k_neighbors(10), regularization_method(EIG), eigen_values(1e-3, 1.0, 1.0) {} + +public: + int num_threads; ///< Number of threads + int k_neighbors; ///< Number of neighboring points used for covariance estimation + RegularizationMethod regularization_method; ///< Regularization method + Eigen::Vector3d eigen_values; ///< Eigenvalues used for EIG regularization +}; + +/** + * @brief Estimate point covariances from neighboring points + * @param points Input points + * @param num_points Number of input points + * @param params Estimation params + * @return Estimated covariances + */ +std::vector estimate_covariances(const Eigen::Vector4d* points, int num_points, const CovarianceEstimationParams& params); + +/** + * @brief Estimate point covariances from neighboring points + * @param points Input points + * @param num_points Number of input points + * @param k_neighbors Number of neighboring points for covariance estimation + * @param eigen_values Eigenvalues used for regularization (default=[1e-3, 1, 1]) + * @param num_threads Number of threads + * @return Estimated covariances + */ +std::vector +estimate_covariances(const Eigen::Vector4d* points, int num_points, int k_neighbors, const Eigen::Vector3d& eigen_values, int num_threads); + +std::vector estimate_covariances(const Eigen::Vector4d* points, int num_points, int k_neighbors = 10, int num_threads = 1); + +template +std::vector estimate_covariances(const std::vector& points, int k_neighbors = 10, int num_threads = 1) { + return estimate_covariances(points.data(), points.size(), k_neighbors, num_threads); +} + +} // namespace gtsam_points \ No newline at end of file diff --git a/include/gtsam_ext/util/easy_profiler.hpp b/include/gtsam_points/util/easy_profiler.hpp similarity index 91% rename from include/gtsam_ext/util/easy_profiler.hpp rename to include/gtsam_points/util/easy_profiler.hpp index a326d5ff..68fc0ca3 100644 --- a/include/gtsam_ext/util/easy_profiler.hpp +++ b/include/gtsam_points/util/easy_profiler.hpp @@ -1,3 +1,6 @@ +// SPDX-License-Identifier: MIT +// Copyright (c) 2021 Kenji Koide (k.koide@aist.go.jp) + #pragma once #include @@ -5,7 +8,7 @@ #include #include -namespace gtsam_ext { +namespace gtsam_points { class EasyProfiler { public: @@ -63,7 +66,8 @@ class EasyProfiler { times.push_back(std::chrono::high_resolution_clock::now()); if (debug) { - ost << ">> " << label << " (" << std::chrono::duration_cast(times.back() - times.front()).count() / 1e6 << "[msec])" << std::endl; + ost << ">> " << label << " (" << std::chrono::duration_cast(times.back() - times.front()).count() / 1e6 << "[msec])" + << std::endl; } } @@ -78,4 +82,4 @@ class EasyProfiler { std::ostream& ost; std::ofstream ofs; }; -} // namespace glim \ No newline at end of file +} // namespace gtsam_points \ No newline at end of file diff --git a/include/gtsam_ext/util/easy_profiler_cuda.hpp b/include/gtsam_points/util/easy_profiler_cuda.hpp similarity index 85% rename from include/gtsam_ext/util/easy_profiler_cuda.hpp rename to include/gtsam_points/util/easy_profiler_cuda.hpp index 0664311d..68015b12 100644 --- a/include/gtsam_ext/util/easy_profiler_cuda.hpp +++ b/include/gtsam_points/util/easy_profiler_cuda.hpp @@ -1,3 +1,6 @@ +// SPDX-License-Identifier: MIT +// Copyright (c) 2021 Kenji Koide (k.koide@aist.go.jp) + #pragma once #include @@ -8,7 +11,7 @@ struct CUevent_st; struct CUstream_st; -namespace gtsam_ext { +namespace gtsam_points { class EasyProfilerCuda { public: EasyProfilerCuda( @@ -42,4 +45,4 @@ class EasyProfilerCuda { std::ofstream ofs; }; -} // namespace gtsam_ext +} // namespace gtsam_points diff --git a/include/gtsam_ext/util/expressions.hpp b/include/gtsam_points/util/expressions.hpp similarity index 98% rename from include/gtsam_ext/util/expressions.hpp rename to include/gtsam_points/util/expressions.hpp index 653c5431..a2c3a9de 100644 --- a/include/gtsam_ext/util/expressions.hpp +++ b/include/gtsam_points/util/expressions.hpp @@ -7,7 +7,7 @@ #include #include -namespace gtsam_ext { +namespace gtsam_points { namespace internal { @@ -127,4 +127,4 @@ inline gtsam::Pose3_ inverse(const gtsam::Pose3_& x) { return gtsam::Pose3_(f, x); } -} // namespace gtsam_ext \ No newline at end of file +} // namespace gtsam_points \ No newline at end of file diff --git a/include/gtsam_points/util/fast_floor.hpp b/include/gtsam_points/util/fast_floor.hpp new file mode 100644 index 00000000..cca10d34 --- /dev/null +++ b/include/gtsam_points/util/fast_floor.hpp @@ -0,0 +1,17 @@ +// SPDX-FileCopyrightText: Copyright 2024 Kenji Koide +// SPDX-License-Identifier: MIT +#pragma once + +#include + +namespace gtsam_points { + +/// @brief Fast floor (https://stackoverflow.com/questions/824118/why-is-floor-so-slow). +/// @param pt Double vector +/// @return Floored int vector +inline Eigen::Array4i fast_floor(const Eigen::Array4d& pt) { + const Eigen::Array4i ncoord = pt.cast(); + return ncoord - (pt < ncoord.cast()).cast(); +}; + +} // namespace gtsam_points diff --git a/include/gtsam_points/util/jacobian_test.hpp b/include/gtsam_points/util/jacobian_test.hpp new file mode 100644 index 00000000..bcb1bcd6 --- /dev/null +++ b/include/gtsam_points/util/jacobian_test.hpp @@ -0,0 +1,92 @@ +// SPDX-FileCopyrightText: Copyright 2024 Kenji Koide +// SPDX-License-Identifier: MIT +#pragma once + +#include +#include +#include +#include + +namespace gtsam_points { + +/** + * @brief Utility function for testing Jacobian of a factor. + * @note This function cannot properly evaluate Jacobian of factors that measures errors in a manifold (e.g., BetweenFactor). + * + * @example + +auto generate_factor(std::mt19937& mt) { + std::uniform_real_distribution<> udist(-1.0, 1.0); + + gtsam::Vector3 rel_pose = Eigen::Vector3d(udist(mt), udist(mt), udist(mt)); + return gtsam::make_shared>(0, 1, rel_pose, gtsam::noiseModel::Isotropic::Sigma(6, 1.0)); +} + +gtsam::Values generate_values(std::mt19937& mt) { + std::uniform_real_distribution<> udist(-1.0, 1.0); + + gtsam::Values values; + values.insert(0, Eigen::Vector3d(udist(mt), udist(mt), udist(mt))); + values.insert(1, Eigen::Vector3d(udist(mt), udist(mt), udist(mt))); + return values; +} + +int main(int argc, char** argv) { + std::mt19937 mt; + for (int i = 0; i < 5; i++) { + gtsam_points::test_factor_jacobian(mt, generate_factor, generate_values); + } + return 0; +} + + */ +template +bool test_factor_jacobian(std::mt19937& mt, const GenerateFactor& generate_factor, const GenerateValues& generate_values) { + auto factor = generate_factor(mt); + gtsam::Values values = generate_values(mt); + + std::vector Hs_a(factor->keys().size()); + std::vector Hs_d(factor->keys().size()); + gtsam::Vector x0 = factor->unwhitenedError(values, Hs_a); + + const double eps = 1e-6; + + for (int i = 0; i < factor->keys().size(); i++) { + const auto key = factor->keys()[i]; + gtsam::VectorValues delta = values.zeroVectors(); + const int D = delta[key].size(); + + gtsam::Matrix Hd(x0.size(), D); + for (int j = 0; j < D; j++) { + delta[key].setZero(); + delta[key][j] = eps; + + gtsam::Vector xi = factor->unwhitenedError(values.retract(delta)); + Hd.col(j) = (xi - x0) / eps; + } + + Hs_d[i] = Hd; + } + + bool ok = true; + for (int i = 0; i < factor->keys().size(); i++) { + ok &= ((Hs_a[i] - Hs_d[i]).array().abs() < 1e-3).all(); + } + + if (ok) { + std::cout << "OK (x0=" << x0.transpose() << ")" << std::endl; + } else { + std::cout << "Failed (x0=" << x0.transpose() << ")" << std::endl; + + for (int i = 0; i < factor->keys().size(); i++) { + std::cout << "*** key " << i << " ***" << std::endl; + std::cerr << "--- Ha ---" << std::endl << Hs_a[i] << std::endl; + std::cerr << "--- Hd ---" << std::endl << Hs_d[i] << std::endl; + std::cerr << "--- err ---" << std::endl << Hs_a[i] - Hs_d[i] << std::endl; + } + } + + return ok; +} + +} // namespace gtsam_points diff --git a/include/gtsam_ext/util/normal_estimation.hpp b/include/gtsam_points/util/normal_estimation.hpp similarity index 63% rename from include/gtsam_ext/util/normal_estimation.hpp rename to include/gtsam_points/util/normal_estimation.hpp index 470c9320..0ffa359c 100644 --- a/include/gtsam_ext/util/normal_estimation.hpp +++ b/include/gtsam_points/util/normal_estimation.hpp @@ -6,7 +6,7 @@ #include #include -namespace gtsam_ext { +namespace gtsam_points { /** * @brief Estimate point normals from covariances @@ -16,8 +16,7 @@ namespace gtsam_ext { * @param num_threads Number of threads * @return Estimated normals */ -std::vector> -estimate_normals(const Eigen::Vector4d* points, const Eigen::Matrix4d* covs, int num_points, int num_threads = 1); +std::vector estimate_normals(const Eigen::Vector4d* points, const Eigen::Matrix4d* covs, int num_points, int num_threads = 1); /** * @brief Estimate point normals from neighboring points @@ -27,11 +26,10 @@ estimate_normals(const Eigen::Vector4d* points, const Eigen::Matrix4d* covs, int * @param num_threads Number of threads * @return Estimated normals */ -std::vector> -estimate_normals(const Eigen::Vector4d* points, int num_points, int k_neighbors = 10, int num_threads = 1); +std::vector estimate_normals(const Eigen::Vector4d* points, int num_points, int k_neighbors = 10, int num_threads = 1); template