├── data ├── 251370668.pcd ├── 251371071.pcd ├── proctime.png └── relative.txt ├── .gitignore ├── scripts └── runtest_cuda.sh ├── include ├── fast_gicp │ ├── ndt │ │ ├── ndt_settings.hpp │ │ ├── ndt_cuda.hpp │ │ └── impl │ │ │ └── ndt_cuda_impl.hpp │ ├── gicp │ │ ├── gicp_settings.hpp │ │ ├── fast_gicp_st.hpp │ │ ├── fast_vgicp.hpp │ │ ├── fast_vgicp_cuda.hpp │ │ ├── experimental │ │ │ └── fast_gicp_mp.hpp │ │ ├── lsq_registration.hpp │ │ ├── fast_gicp.hpp │ │ ├── impl │ │ │ ├── fast_gicp_st_impl.hpp │ │ │ ├── fast_vgicp_cuda_impl.hpp │ │ │ ├── fast_vgicp_impl.hpp │ │ │ └── lsq_registration_impl.hpp │ │ └── fast_vgicp_voxel.hpp │ ├── cuda │ │ ├── brute_force_knn.cuh │ │ ├── covariance_regularization.cuh │ │ ├── covariance_estimation.cuh │ │ ├── compute_mahalanobis.cuh │ │ ├── find_voxel_correspondences.cuh │ │ ├── compute_derivatives.cuh │ │ ├── vector3_hash.cuh │ │ ├── ndt_compute_derivatives.cuh │ │ ├── gaussian_voxelmap.cuh │ │ ├── ndt_cuda.cuh │ │ └── fast_vgicp_cuda.cuh │ ├── time_utils.hpp │ └── so3 │ │ └── so3.hpp └── litamin2 │ ├── ceres_cost │ ├── litamin2_cost.hpp │ ├── PoseSE3Parameterization.hpp │ └── gicp_cost.hpp │ ├── litamin2.hpp │ └── impl │ └── litamin2_impl.hpp ├── src ├── litamin2 │ └── litamin2.cpp ├── fast_gicp │ ├── ndt │ │ └── ndt_cuda.cpp │ ├── gicp │ │ ├── fast_gicp.cpp │ │ ├── fast_vgicp.cpp │ │ ├── fast_vgicp_cuda.cpp │ │ ├── fast_gicp_st.cpp │ │ ├── lsq_registration.cpp │ │ └── experimental │ │ │ └── fast_gicp_mp.cpp │ └── cuda │ │ ├── covariance_estimation.cu │ │ ├── compute_mahalanobis.cu │ │ ├── find_voxel_correspondences.cu │ │ ├── brute_force_knn.cu │ │ ├── covariance_regularization.cu │ │ ├── covariance_estimation_rbf.cu │ │ ├── ndt_cuda.cu │ │ └── compute_derivatives.cu ├── kitti.py ├── kitti.cpp └── test │ └── gicp_test.cpp ├── launch ├── kitti.launch └── align.launch ├── package.xml ├── .gitmodules ├── config └── params.yaml ├── docker ├── melodic │ └── Dockerfile ├── noetic │ └── Dockerfile ├── melodic_llvm │ └── Dockerfile ├── noetic_llvm │ └── Dockerfile ├── kinetic │ └── Dockerfile ├── focal_cuda │ └── Dockerfile └── focal │ └── Dockerfile ├── .github └── workflows │ └── build.yml ├── LICENSE ├── cmake_modules ├── FindGLOG.cmake ├── FindPROJ4.cmake ├── FindGFLAGS.cmake ├── FindLIBLAS.cmake ├── FindSuiteSparse.cmake ├── FindG2O.cmake └── FindEIGEN3.cmake ├── setup.py ├── .clang-format ├── CMakeLists.txt └── README.md /data/251370668.pcd: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/FishInWave/fast-gicp/HEAD/data/251370668.pcd -------------------------------------------------------------------------------- /data/251371071.pcd: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/FishInWave/fast-gicp/HEAD/data/251371071.pcd -------------------------------------------------------------------------------- /data/proctime.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/FishInWave/fast-gicp/HEAD/data/proctime.png -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | .vscode/* 2 | build/* 3 | dist/* 4 | 5 | *.so 6 | *.py[cod] 7 | *.egg-info 8 | README.html 9 | -------------------------------------------------------------------------------- /scripts/runtest_cuda.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | docker build --tag focal_cuda -f docker/focal_cuda/Dockerfile . 3 | docker run --gpus all -it --rm focal_cuda /root/fast_gicp/build/gicp_test /root/fast_gicp/data 4 | -------------------------------------------------------------------------------- /data/relative.txt: -------------------------------------------------------------------------------- 1 | 0.999941 0.0108432 -0.000635437 0.485657 2 | -0.0108468 0.999924 -0.00587782 0.10642 3 | 0.000571654 0.00588436 0.999983 -0.0131581 4 | 0 0 0 1 5 | -------------------------------------------------------------------------------- /include/fast_gicp/ndt/ndt_settings.hpp: -------------------------------------------------------------------------------- 1 | #ifndef FAST_GICP_NDT_SETTINGS_HPP 2 | #define FAST_GICP_NDT_SETTINGS_HPP 3 | 4 | namespace fast_gicp { 5 | 6 | enum class NDTDistanceMode { P2D, D2D }; 7 | 8 | } // namespace fast_gicp 9 | 10 | #endif -------------------------------------------------------------------------------- /src/litamin2/litamin2.cpp: -------------------------------------------------------------------------------- 1 | #include "litamin2/litamin2.hpp" 2 | #include 3 | 4 | template class fast_gicp::LiTAMIN2; 5 | template class fast_gicp::LiTAMIN2; 6 | -------------------------------------------------------------------------------- /src/fast_gicp/ndt/ndt_cuda.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | template class fast_gicp::NDTCuda; 5 | template class fast_gicp::NDTCuda; 6 | -------------------------------------------------------------------------------- /src/fast_gicp/gicp/fast_gicp.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | template class fast_gicp::FastGICP; 5 | template class fast_gicp::FastGICP; 6 | -------------------------------------------------------------------------------- /src/fast_gicp/gicp/fast_vgicp.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | template class fast_gicp::FastVGICP; 5 | template class fast_gicp::FastVGICP; -------------------------------------------------------------------------------- /src/fast_gicp/gicp/fast_vgicp_cuda.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | template class fast_gicp::FastVGICPCuda; 5 | template class fast_gicp::FastVGICPCuda; 6 | -------------------------------------------------------------------------------- /src/fast_gicp/gicp/fast_gicp_st.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | template class fast_gicp::FastGICPSingleThread; 5 | template class fast_gicp::FastGICPSingleThread; 6 | -------------------------------------------------------------------------------- /src/fast_gicp/gicp/lsq_registration.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | template class fast_gicp::LsqRegistration; 5 | template class fast_gicp::LsqRegistration; 6 | -------------------------------------------------------------------------------- /launch/kitti.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | -------------------------------------------------------------------------------- /src/fast_gicp/gicp/experimental/fast_gicp_mp.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | // template class fast_gicp::FastGICPMultiPoints; 5 | template class fast_gicp::FastGICPMultiPoints; 6 | -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | fast_gicp 4 | 0.0.0 5 | A collection of fast point cloud registration implementations 6 | 7 | k.koide 8 | 9 | BSD 10 | 11 | catkin 12 | 13 | 14 | 15 | -------------------------------------------------------------------------------- /.gitmodules: -------------------------------------------------------------------------------- 1 | [submodule "thidparty/Sophus"] 2 | path = thirdparty/Sophus 3 | url = https://github.com/strasdat/Sophus.git 4 | [submodule "thirdparty/Eigen"] 5 | path = thirdparty/Eigen 6 | url = https://gitlab.com/libeigen/eigen.git 7 | [submodule "thirdparty/nvbio"] 8 | path = thirdparty/nvbio 9 | url = https://github.com/NVlabs/nvbio.git 10 | [submodule "thirdparty/pybind11"] 11 | path = thirdparty/pybind11 12 | url = https://github.com/pybind/pybind11.git 13 | -------------------------------------------------------------------------------- /include/fast_gicp/gicp/gicp_settings.hpp: -------------------------------------------------------------------------------- 1 | #ifndef FAST_GICP_GICP_SETTINGS_HPP 2 | #define FAST_GICP_GICP_SETTINGS_HPP 3 | 4 | namespace fast_gicp { 5 | 6 | enum class RegularizationMethod { NONE, MIN_EIG, NORMALIZED_MIN_EIG, PLANE, FROBENIUS }; 7 | 8 | enum class NeighborSearchMethod { DIRECT27, DIRECT7, DIRECT1, /* supported on only VGICP_CUDA */ DIRECT_RADIUS }; 9 | 10 | enum class VoxelAccumulationMode { ADDITIVE, ADDITIVE_WEIGHTED, MULTIPLICATIVE }; 11 | } 12 | 13 | #endif -------------------------------------------------------------------------------- /config/params.yaml: -------------------------------------------------------------------------------- 1 | # 存在移动硬盘里 2 | KITTIFile: "/media/xyw/Backup/AGV/dataset/KITTI/odometry/data_odometry_velodyne/dataset/sequences/00/velodyne" 3 | 4 | # liTAMIN2参数 5 | local_size: 1 6 | source_leaf_size: 6.0 # 10.0会爆掉 7 | target_leaf_size: 6.0 8 | 9 | min_points_in_voxel: 20 10 | 11 | use_target_voxel_filter: true 12 | voxel_target_leaf_size: 0.1 13 | use_source_voxel_filter: true 14 | voxel_source_leaf_size: 0.1 15 | sigmaICP: 0.5 16 | 17 | use_guess: true # true 比false在旋转和平移上都有增长 -------------------------------------------------------------------------------- /include/fast_gicp/cuda/brute_force_knn.cuh: -------------------------------------------------------------------------------- 1 | #ifndef FAST_GICP_CUDA_BRUTE_FORCE_KNN_CUH 2 | #define FAST_GICP_CUDA_BRUTE_FORCE_KNN_CUH 3 | 4 | #include 5 | 6 | #include 7 | 8 | namespace fast_gicp { 9 | namespace cuda { 10 | 11 | void brute_force_knn_search(const thrust::device_vector& source, const thrust::device_vector& target, int k, thrust::device_vector>& k_neighbors, bool do_sort=false); 12 | 13 | } 14 | } // namespace fast_gicp 15 | 16 | 17 | #endif -------------------------------------------------------------------------------- /include/fast_gicp/cuda/covariance_regularization.cuh: -------------------------------------------------------------------------------- 1 | #ifndef FAST_GICP_CUDA_COVARIANCE_REGULARIZATION_CUH 2 | #define FAST_GICP_CUDA_COVARIANCE_REGULARIZATION_CUH 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | namespace fast_gicp { 9 | namespace cuda { 10 | 11 | void covariance_regularization(thrust::device_vector& means, thrust::device_vector& covs, RegularizationMethod method); 12 | 13 | } // namespace cuda 14 | } // namespace fast_gicp 15 | 16 | #endif -------------------------------------------------------------------------------- /launch/align.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | -------------------------------------------------------------------------------- /include/fast_gicp/cuda/covariance_estimation.cuh: -------------------------------------------------------------------------------- 1 | #ifndef FAST_GICP_CUDA_COVARIANCE_ESTIMATION_CUH 2 | #define FAST_GICP_CUDA_COVARIANCE_ESTIMATION_CUH 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | namespace fast_gicp { 9 | namespace cuda { 10 | 11 | void covariance_estimation(const thrust::device_vector& points, int k, const thrust::device_vector& k_neighbors, thrust::device_vector& covariances); 12 | 13 | void covariance_estimation_rbf(const thrust::device_vector& points, double kernel_width, double max_dist, thrust::device_vector& covariances); 14 | } 15 | } // namespace fast_gicp 16 | 17 | #endif -------------------------------------------------------------------------------- /include/fast_gicp/cuda/compute_mahalanobis.cuh: -------------------------------------------------------------------------------- 1 | #ifndef FAST_GICP_CUDA_COMPUTE_MAHALANOBIS_CUH 2 | #define FAST_GICP_CUDA_COMPUTE_MAHALANOBIS_CUH 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | #include 9 | 10 | namespace fast_gicp { 11 | namespace cuda { 12 | 13 | void compute_mahalanobis( 14 | const thrust::device_vector& src_points, 15 | const thrust::device_vector& src_covs, 16 | const GaussianVoxelMap& voxelmap, 17 | const thrust::device_vector& voxel_correspondences, 18 | const Eigen::Isometry3f& linearized_x, 19 | thrust::device_vector& mahalanobis 20 | ); 21 | 22 | } 23 | } // namespace fast_gicp 24 | 25 | #endif -------------------------------------------------------------------------------- /include/fast_gicp/cuda/find_voxel_correspondences.cuh: -------------------------------------------------------------------------------- 1 | #ifndef FAST_GICP_CUDA_FIND_VOXEL_CORRESPONDENCES_CUH 2 | #define FAST_GICP_CUDA_FIND_VOXEL_CORRESPONDENCES_CUH 3 | 4 | #include 5 | #include 6 | 7 | #include 8 | #include 9 | #include 10 | 11 | namespace fast_gicp { 12 | namespace cuda { 13 | 14 | void find_voxel_correspondences( 15 | const thrust::device_vector& src_points, 16 | const GaussianVoxelMap& voxelmap, 17 | const thrust::device_ptr& x_ptr, 18 | const thrust::device_vector& offsets, 19 | thrust::device_vector>& correspondences) ; 20 | 21 | } // namespace cuda 22 | } // namespace fast_gicp 23 | 24 | #endif -------------------------------------------------------------------------------- /docker/melodic/Dockerfile: -------------------------------------------------------------------------------- 1 | FROM ros:melodic 2 | 3 | RUN apt-get update && apt-get install --no-install-recommends -y \ 4 | && apt-get install --no-install-recommends -y wget nano build-essential \ 5 | libgtest-dev libomp-dev ros-melodic-pcl-ros \ 6 | && apt-get clean \ 7 | && rm -rf /var/lib/apt/lists/* 8 | 9 | 10 | RUN mkdir -p /root/catkin_ws/src 11 | WORKDIR /root/catkin_ws/src 12 | RUN /bin/bash -c '. /opt/ros/melodic/setup.bash; catkin_init_workspace' 13 | 14 | COPY . /root/catkin_ws/src/fast_gicp/ 15 | WORKDIR /root/catkin_ws/src/fast_gicp 16 | RUN git submodule init && git submodule update 17 | 18 | WORKDIR /root/catkin_ws 19 | RUN /bin/bash -c '. /opt/ros/melodic/setup.bash; catkin_make' 20 | RUN sed -i "6i source \"/root/catkin_ws/devel/setup.bash\"" /ros_entrypoint.sh 21 | 22 | WORKDIR / 23 | 24 | ENTRYPOINT ["/ros_entrypoint.sh"] 25 | CMD ["bash"] 26 | -------------------------------------------------------------------------------- /include/fast_gicp/cuda/compute_derivatives.cuh: -------------------------------------------------------------------------------- 1 | #ifndef FAST_GICP_CUDA_COMPUTE_DERIVATIVES_CUH 2 | #define FAST_GICP_CUDA_COMPUTE_DERIVATIVES_CUH 3 | 4 | #include 5 | #include 6 | 7 | #include 8 | 9 | namespace fast_gicp { 10 | namespace cuda { 11 | 12 | double compute_derivatives( 13 | const thrust::device_vector& src_points, 14 | const thrust::device_vector& src_covs, 15 | const GaussianVoxelMap& voxelmap, 16 | const thrust::device_vector>& voxel_correspondences, 17 | const thrust::device_ptr& linearized_x_ptr, 18 | const thrust::device_ptr& x_ptr, 19 | Eigen::Matrix* H, 20 | Eigen::Matrix* b); 21 | } 22 | } // namespace fast_gicp 23 | 24 | #endif -------------------------------------------------------------------------------- /.github/workflows/build.yml: -------------------------------------------------------------------------------- 1 | name: Build 2 | 3 | on: 4 | push: 5 | branches: [ master ] 6 | pull_request: 7 | branches: [ master ] 8 | 9 | # Allows you to run this workflow manually from the Actions tab 10 | workflow_dispatch: 11 | 12 | jobs: 13 | build: 14 | runs-on: ubuntu-latest 15 | strategy: 16 | matrix: 17 | ROS_DISTRO: [focal, focal_cuda, melodic, melodic_llvm, noetic, noetic_llvm] 18 | 19 | steps: 20 | - uses: actions/checkout@v2 21 | 22 | - name: Docker login 23 | continue-on-error: true 24 | uses: docker/login-action@v1 25 | with: 26 | username: ${{ secrets.DOCKER_USERNAME }} 27 | password: ${{ secrets.DOCKER_TOKEN }} 28 | 29 | - name: Docker build 30 | uses: docker/build-push-action@v2 31 | with: 32 | file: ${{github.workspace}}/docker/${{ matrix.ROS_DISTRO }}/Dockerfile 33 | context: . 34 | push: false 35 | -------------------------------------------------------------------------------- /docker/noetic/Dockerfile: -------------------------------------------------------------------------------- 1 | FROM ros:noetic 2 | 3 | RUN apt-get update && apt-get install --no-install-recommends -y \ 4 | && apt-get install --no-install-recommends -y wget nano build-essential \ 5 | git libgtest-dev ros-noetic-pcl-ros \ 6 | && apt-get clean \ 7 | && rm -rf /var/lib/apt/lists/* 8 | 9 | RUN mkdir -p /root/catkin_ws/src 10 | WORKDIR /root/catkin_ws/src 11 | RUN /bin/bash -c '. /opt/ros/noetic/setup.bash; catkin_init_workspace' 12 | 13 | COPY . /root/catkin_ws/src/fast_gicp/ 14 | WORKDIR /root/catkin_ws/src/fast_gicp 15 | RUN git submodule init && git submodule update 16 | 17 | WORKDIR /root/catkin_ws 18 | RUN /bin/bash -c '. /opt/ros/noetic/setup.bash; catkin_make -DBUILD_test=ON' 19 | RUN sed -i "6i source \"/root/catkin_ws/devel/setup.bash\"" /ros_entrypoint.sh 20 | 21 | RUN /root/catkin_ws/devel/lib/fast_gicp/gicp_test /root/catkin_ws/src/fast_gicp/data 22 | 23 | WORKDIR / 24 | 25 | ENTRYPOINT ["/ros_entrypoint.sh"] 26 | CMD ["bash"] 27 | -------------------------------------------------------------------------------- /docker/melodic_llvm/Dockerfile: -------------------------------------------------------------------------------- 1 | FROM ros:melodic 2 | 3 | RUN apt-get update && apt-get install --no-install-recommends -y \ 4 | && apt-get install --no-install-recommends -y wget nano build-essential \ 5 | libgtest-dev libomp-dev clang lld ros-melodic-pcl-ros \ 6 | && apt-get clean \ 7 | && rm -rf /var/lib/apt/lists/* 8 | 9 | RUN update-alternatives --install /usr/bin/ld ld /usr/bin/ld.lld 50 10 | 11 | RUN mkdir -p /root/catkin_ws/src 12 | WORKDIR /root/catkin_ws/src 13 | RUN /bin/bash -c '. /opt/ros/melodic/setup.bash; catkin_init_workspace' 14 | 15 | COPY . /root/catkin_ws/src/fast_gicp/ 16 | WORKDIR /root/catkin_ws/src/fast_gicp 17 | RUN git submodule init && git submodule update 18 | 19 | WORKDIR /root/catkin_ws 20 | RUN /bin/bash -c '. /opt/ros/melodic/setup.bash; CC=clang CXX=clang++ catkin_make' 21 | RUN sed -i "6i source \"/root/catkin_ws/devel/setup.bash\"" /ros_entrypoint.sh 22 | 23 | WORKDIR / 24 | 25 | ENTRYPOINT ["/ros_entrypoint.sh"] 26 | CMD ["bash"] 27 | -------------------------------------------------------------------------------- /docker/noetic_llvm/Dockerfile: -------------------------------------------------------------------------------- 1 | FROM ros:noetic 2 | 3 | RUN apt-get update && apt-get install --no-install-recommends -y \ 4 | && apt-get install --no-install-recommends -y wget nano build-essential \ 5 | git clang lld libomp-dev libgtest-dev ros-noetic-pcl-ros \ 6 | && apt-get clean \ 7 | && rm -rf /var/lib/apt/lists/* 8 | 9 | RUN update-alternatives --install /usr/bin/ld ld /usr/bin/ld.lld 50 10 | 11 | RUN mkdir -p /root/catkin_ws/src 12 | WORKDIR /root/catkin_ws/src 13 | RUN /bin/bash -c '. /opt/ros/noetic/setup.bash; catkin_init_workspace' 14 | 15 | COPY . /root/catkin_ws/src/fast_gicp/ 16 | WORKDIR /root/catkin_ws/src/fast_gicp 17 | RUN git submodule init && git submodule update 18 | 19 | WORKDIR /root/catkin_ws 20 | RUN /bin/bash -c '. /opt/ros/noetic/setup.bash; CC=clang CXX=clang++ catkin_make -DBUILD_test=ON' 21 | RUN sed -i "6i source \"/root/catkin_ws/devel/setup.bash\"" /ros_entrypoint.sh 22 | 23 | RUN /root/catkin_ws/devel/lib/fast_gicp/gicp_test /root/catkin_ws/src/fast_gicp/data 24 | 25 | WORKDIR / 26 | 27 | ENTRYPOINT ["/ros_entrypoint.sh"] 28 | CMD ["bash"] 29 | -------------------------------------------------------------------------------- /docker/kinetic/Dockerfile: -------------------------------------------------------------------------------- 1 | FROM ros:kinetic 2 | 3 | RUN apt-get update && apt-get install --no-install-recommends -y \ 4 | && apt-get install --no-install-recommends -y wget nano build-essential \ 5 | ros-kinetic-pcl-ros \ 6 | && apt-get clean \ 7 | && rm -rf /var/lib/apt/lists/* 8 | 9 | # install cmake 3.19 10 | WORKDIR /root 11 | RUN wget https://github.com/Kitware/CMake/releases/download/v3.19.0-rc1/cmake-3.19.0-rc1.tar.gz 12 | RUN tar xzvf cmake-3.19.0-rc1.tar.gz 13 | 14 | WORKDIR /root/cmake-3.19.0-rc1 15 | RUN ./bootstrap 16 | RUN make -j$(nproc) && make install 17 | 18 | RUN mkdir -p /root/catkin_ws/src 19 | WORKDIR /root/catkin_ws/src 20 | RUN /bin/bash -c '. /opt/ros/kinetic/setup.bash; catkin_init_workspace' 21 | 22 | COPY . /root/catkin_ws/src/fast_gicp/ 23 | WORKDIR /root/catkin_ws/src/fast_gicp 24 | RUN git submodule init && git submodule update 25 | 26 | WORKDIR /root/catkin_ws 27 | RUN /bin/bash -c '. /opt/ros/kinetic/setup.bash; catkin_make' 28 | RUN sed -i "6i source \"/root/catkin_ws/devel/setup.bash\"" /ros_entrypoint.sh 29 | 30 | WORKDIR / 31 | 32 | ENTRYPOINT ["/ros_entrypoint.sh"] 33 | CMD ["bash"] 34 | -------------------------------------------------------------------------------- /include/fast_gicp/cuda/vector3_hash.cuh: -------------------------------------------------------------------------------- 1 | #ifndef FAST_GICP_CUDA_VECTOR3_HASH_CUH 2 | #define FAST_GICP_CUDA_VECTOR3_HASH_CUH 3 | 4 | namespace fast_gicp { 5 | namespace cuda { 6 | 7 | // taken from boost/hash.hpp 8 | __host__ __device__ inline void hash_combine(uint64_t& h, uint64_t k) { 9 | const uint64_t m = UINT64_C(0xc6a4a7935bd1e995); 10 | const int r = 47; 11 | 12 | k *= m; 13 | k ^= k >> r; 14 | k *= m; 15 | 16 | h ^= k; 17 | h *= m; 18 | 19 | h += 0xe6546b64; 20 | } 21 | 22 | inline __host__ __device__ bool equal(const Eigen::Vector3i& lhs, const Eigen::Vector3i& rhs) { 23 | return lhs[0] == rhs[0] && lhs[1] == rhs[1] && lhs[2] == rhs[2]; 24 | } 25 | 26 | // compute vector3i hash 27 | __host__ __device__ inline uint64_t vector3i_hash(const Eigen::Vector3i& x) { 28 | uint64_t seed = 0; 29 | hash_combine(seed, x[0]); 30 | hash_combine(seed, x[1]); 31 | hash_combine(seed, x[2]); 32 | return seed; 33 | } 34 | 35 | __host__ __device__ inline Eigen::Vector3i calc_voxel_coord(const Eigen::Vector3f& x, float resolution) { 36 | Eigen::Vector3i coord = (x.array() / resolution - 0.5).floor().cast(); 37 | return coord; 38 | } 39 | 40 | } 41 | } // namespace fast_gicp 42 | 43 | #endif -------------------------------------------------------------------------------- /docker/focal_cuda/Dockerfile: -------------------------------------------------------------------------------- 1 | FROM nvidia/cuda:11.1-devel-ubuntu20.04 2 | 3 | ENV DEBIAN_FRONTEND=noninteractive 4 | 5 | RUN apt-get update && apt-get install --no-install-recommends -y \ 6 | && apt-get install --no-install-recommends -y wget nano cmake build-essential git \ 7 | ca-certificates libeigen3-dev libboost-all-dev libflann-dev libvtk7-dev libomp-dev libgtest-dev \ 8 | && apt-get clean \ 9 | && rm -rf /var/lib/apt/lists/* 10 | 11 | WORKDIR /root 12 | RUN git clone https://github.com/PointCloudLibrary/pcl 13 | 14 | WORKDIR /root/pcl/build 15 | RUN cmake .. -DCMAKE_BUILD_TYPE=Release \ 16 | -DBUILD_geometry=OFF -DBUILD_keypoints=OFF -DBUILD_ml=OFF -DBUILD_outofcore=OFF -DBUILD_people=OFF \ 17 | -DBUILD_recognition=OFF -DBUILD_segmentation=OFF -DBUILD_stereo=OFF -DBUILD_surface=OFF -DBUILD_tools=OFF -DBUILD_tracking=OFF 18 | RUN make -j$(nproc) && make install 19 | 20 | COPY . /root/fast_gicp 21 | WORKDIR /root/fast_gicp 22 | RUN git submodule init && git submodule update 23 | 24 | RUN rm -rf /root/fast_gicp/build 25 | WORKDIR /root/fast_gicp/build 26 | RUN cmake .. -DCMAKE_BUILD_TYPE=Release -DBUILD_VGICP_CUDA=ON -DBUILD_PYTHON_BINDINGS=ON -DBUILD_apps=OFF -DBUILD_test=ON 27 | RUN make -j$(nproc) 28 | 29 | WORKDIR /root 30 | 31 | CMD ["bash"] 32 | -------------------------------------------------------------------------------- /docker/focal/Dockerfile: -------------------------------------------------------------------------------- 1 | FROM ubuntu:focal 2 | 3 | ENV DEBIAN_FRONTEND=noninteractive 4 | 5 | RUN apt-get update && apt-get install --no-install-recommends -y \ 6 | && apt-get install --no-install-recommends -y wget nano cmake build-essential git \ 7 | ca-certificates libeigen3-dev libboost-all-dev libflann-dev libvtk7-dev libomp-dev libgtest-dev \ 8 | && apt-get clean \ 9 | && rm -rf /var/lib/apt/lists/* 10 | 11 | WORKDIR /root 12 | RUN git clone https://github.com/PointCloudLibrary/pcl 13 | 14 | WORKDIR /root/pcl/build 15 | RUN cmake .. -DCMAKE_BUILD_TYPE=Release \ 16 | -DBUILD_geometry=OFF -DBUILD_keypoints=OFF -DBUILD_ml=OFF -DBUILD_outofcore=OFF -DBUILD_people=OFF \ 17 | -DBUILD_recognition=OFF -DBUILD_segmentation=OFF -DBUILD_stereo=OFF -DBUILD_surface=OFF -DBUILD_tools=OFF -DBUILD_tracking=OFF 18 | RUN make -j$(nproc) && make install 19 | 20 | COPY . /root/fast_gicp 21 | WORKDIR /root/fast_gicp 22 | RUN git submodule init && git submodule update 23 | 24 | RUN rm -rf /root/fast_gicp/build 25 | WORKDIR /root/fast_gicp/build 26 | RUN cmake .. -DCMAKE_BUILD_TYPE=Release -DBUILD_PYTHON_BINDINGS=ON -DBUILD_apps=OFF -DBUILD_test=ON 27 | RUN make -j$(nproc) 28 | 29 | RUN /root/fast_gicp/build/gicp_test /root/fast_gicp/data 30 | 31 | WORKDIR /root 32 | 33 | CMD ["bash"] 34 | -------------------------------------------------------------------------------- /include/fast_gicp/cuda/ndt_compute_derivatives.cuh: -------------------------------------------------------------------------------- 1 | #ifndef FAST_GICP_CUDA_NDT_COMPUTE_DERIVATIVES_CUH 2 | #define FAST_GICP_CUDA_NDT_COMPUTE_DERIVATIVES_CUH 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | #include 9 | 10 | namespace fast_gicp { 11 | namespace cuda { 12 | 13 | double p2d_ndt_compute_derivatives( 14 | const GaussianVoxelMap& target_voxelmap, 15 | const thrust::device_vector& source_points, 16 | const thrust::device_vector>& correspondences, 17 | const thrust::device_ptr& linearized_x_ptr, 18 | const thrust::device_ptr& x_ptr, 19 | Eigen::Matrix* H, 20 | Eigen::Matrix* b); 21 | 22 | double d2d_ndt_compute_derivatives( 23 | const GaussianVoxelMap& target_voxelmap, 24 | const GaussianVoxelMap& source_voxelmap, 25 | const thrust::device_vector>& correspondences, 26 | const thrust::device_ptr& linearized_x_ptr, 27 | const thrust::device_ptr& x_ptr, 28 | Eigen::Matrix* H, 29 | Eigen::Matrix* b); 30 | 31 | } // namespace cuda 32 | } // namespace fast_gicp 33 | 34 | #endif -------------------------------------------------------------------------------- /include/fast_gicp/cuda/gaussian_voxelmap.cuh: -------------------------------------------------------------------------------- 1 | #ifndef FAST_GICP_CUDA_GAUSSIAN_VOXELMAP_CUH 2 | #define FAST_GICP_CUDA_GAUSSIAN_VOXELMAP_CUH 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | namespace fast_gicp { 9 | namespace cuda { 10 | 11 | struct VoxelMapInfo { 12 | int num_voxels; 13 | int num_buckets; 14 | int max_bucket_scan_count; 15 | float voxel_resolution; 16 | }; 17 | 18 | class GaussianVoxelMap { 19 | public: 20 | GaussianVoxelMap(float resolution, int init_num_buckets = 8192, int max_bucket_scan_count = 10); 21 | 22 | void create_voxelmap(const thrust::device_vector& points); 23 | void create_voxelmap(const thrust::device_vector& points, const thrust::device_vector& covariances); 24 | 25 | private: 26 | void create_bucket_table(cudaStream_t stream, const thrust::device_vector& points); 27 | 28 | public: 29 | const int init_num_buckets; 30 | VoxelMapInfo voxelmap_info; 31 | thrust::device_vector voxelmap_info_ptr; 32 | 33 | thrust::device_vector> buckets; 34 | 35 | // voxel data 36 | thrust::device_vector num_points; 37 | thrust::device_vector voxel_means; 38 | thrust::device_vector voxel_covs; 39 | }; 40 | 41 | } // namespace cuda 42 | } // namespace fast_gicp 43 | 44 | #endif -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | BSD 3-Clause License 2 | 3 | Copyright (c) 2020, SMRT-AIST 4 | All rights reserved. 5 | 6 | Redistribution and use in source and binary forms, with or without 7 | modification, are permitted provided that the following conditions are met: 8 | 9 | 1. Redistributions of source code must retain the above copyright notice, this 10 | list of conditions and the following disclaimer. 11 | 12 | 2. Redistributions in binary form must reproduce the above copyright notice, 13 | this list of conditions and the following disclaimer in the documentation 14 | and/or other materials provided with the distribution. 15 | 16 | 3. Neither the name of the copyright holder nor the names of its 17 | contributors may be used to endorse or promote products derived from 18 | this software without specific prior written permission. 19 | 20 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 21 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 22 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 23 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 24 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 25 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 26 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 27 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 28 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 29 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 30 | -------------------------------------------------------------------------------- /cmake_modules/FindGLOG.cmake: -------------------------------------------------------------------------------- 1 | # - Try to find Glog 2 | # 3 | # The following variables are optionally searched for defaults 4 | # GLOG_ROOT_DIR: Base directory where all GLOG components are found 5 | # 6 | # The following are set after configuration is done: 7 | # GLOG_FOUND 8 | # GLOG_INCLUDE_DIRS 9 | # GLOG_LIBRARIES 10 | # GLOG_LIBRARYRARY_DIRS 11 | 12 | include(FindPackageHandleStandardArgs) 13 | 14 | set(GLOG_ROOT_DIR "" CACHE PATH "Folder contains Google glog") 15 | 16 | if(WIN32) 17 | find_path(GLOG_INCLUDE_DIR glog/logging.h 18 | PATHS ${GLOG_ROOT_DIR}/src/windows) 19 | else() 20 | find_path(GLOG_INCLUDE_DIR glog/logging.h 21 | PATHS ${GLOG_ROOT_DIR}) 22 | endif() 23 | 24 | if(MSVC) 25 | find_library(GLOG_LIBRARY_RELEASE libglog_static 26 | PATHS ${GLOG_ROOT_DIR} 27 | PATH_SUFFIXES Release) 28 | 29 | find_library(GLOG_LIBRARY_DEBUG libglog_static 30 | PATHS ${GLOG_ROOT_DIR} 31 | PATH_SUFFIXES Debug) 32 | 33 | set(GLOG_LIBRARY optimized ${GLOG_LIBRARY_RELEASE} debug ${GLOG_LIBRARY_DEBUG}) 34 | else() 35 | find_library(GLOG_LIBRARY glog 36 | PATHS ${GLOG_ROOT_DIR} 37 | PATH_SUFFIXES lib lib64) 38 | endif() 39 | 40 | find_package_handle_standard_args(Glog DEFAULT_MSG GLOG_INCLUDE_DIR GLOG_LIBRARY) 41 | 42 | if(GLOG_FOUND) 43 | set(GLOG_INCLUDE_DIRS ${GLOG_INCLUDE_DIR}) 44 | set(GLOG_LIBRARIES ${GLOG_LIBRARY}) 45 | message(STATUS "Found glog (include: ${GLOG_INCLUDE_DIR}, library: ${GLOG_LIBRARY})") 46 | mark_as_advanced(GLOG_ROOT_DIR GLOG_LIBRARY_RELEASE GLOG_LIBRARY_DEBUG 47 | GLOG_LIBRARY GLOG_INCLUDE_DIR) 48 | endif() -------------------------------------------------------------------------------- /cmake_modules/FindPROJ4.cmake: -------------------------------------------------------------------------------- 1 | ############################################################################### 2 | # CMake module to search for PROJ.4 library 3 | # 4 | # On success, the macro sets the following variables: 5 | # PROJ4_FOUND = if the library found 6 | # PROJ4_LIBRARY = full path to the library 7 | # PROJ4_INCLUDE_DIR = where to find the library headers 8 | # also defined, but not for general use are 9 | # PROJ4_LIBRARY, where to find the PROJ.4 library. 10 | # 11 | # Copyright (c) 2009 Mateusz Loskot 12 | # 13 | # Redistribution and use is allowed according to the terms of the BSD license. 14 | # For details see the accompanying COPYING-CMAKE-SCRIPTS file. 15 | # 16 | ############################################################################### 17 | 18 | # Try to use OSGeo4W installation 19 | IF(WIN32) 20 | SET(PROJ4_OSGEO4W_HOME "C:/OSGeo4W") 21 | 22 | IF($ENV{OSGEO4W_HOME}) 23 | SET(PROJ4_OSGEO4W_HOME "$ENV{OSGEO4W_HOME}") 24 | ENDIF() 25 | ENDIF(WIN32) 26 | 27 | FIND_PATH(PROJ4_INCLUDE_DIR proj_api.h 28 | PATHS ${PROJ4_OSGEO4W_HOME}/include 29 | DOC "Path to PROJ.4 library include directory") 30 | 31 | SET(PROJ4_NAMES ${PROJ4_NAMES} proj proj_i) 32 | FIND_LIBRARY(PROJ4_LIBRARY 33 | NAMES ${PROJ4_NAMES} 34 | PATHS ${PROJ4_OSGEO4W_HOME}/lib 35 | DOC "Path to PROJ.4 library file") 36 | 37 | # Handle the QUIETLY and REQUIRED arguments and set SPATIALINDEX_FOUND to TRUE 38 | # if all listed variables are TRUE 39 | INCLUDE(FindPackageHandleStandardArgs) 40 | FIND_PACKAGE_HANDLE_STANDARD_ARGS(PROJ4 DEFAULT_MSG PROJ4_LIBRARY PROJ4_INCLUDE_DIR) 41 | 42 | IF(PROJ4_FOUND) 43 | SET(PROJ4_LIBRARIES ${PROJ4_LIBRARY}) 44 | ENDIF() 45 | -------------------------------------------------------------------------------- /cmake_modules/FindGFLAGS.cmake: -------------------------------------------------------------------------------- 1 | # - Try to find GFLAGS 2 | # 3 | # The following variables are optionally searched for defaults 4 | # GFLAGS_ROOT_DIR: Base directory where all GFLAGS components are found 5 | # 6 | # The following are set after configuration is done: 7 | # GFLAGS_FOUND 8 | # GFLAGS_INCLUDE_DIRS 9 | # GFLAGS_LIBRARIES 10 | # GFLAGS_LIBRARYRARY_DIRS 11 | 12 | include(FindPackageHandleStandardArgs) 13 | 14 | set(GFLAGS_ROOT_DIR "" CACHE PATH "Folder contains Gflags") 15 | 16 | # We are testing only a couple of files in the include directories 17 | if(WIN32) 18 | find_path(GFLAGS_INCLUDE_DIR gflags/gflags.h 19 | PATHS ${GFLAGS_ROOT_DIR}/src/windows) 20 | else() 21 | find_path(GFLAGS_INCLUDE_DIR gflags/gflags.h 22 | PATHS ${GFLAGS_ROOT_DIR}) 23 | endif() 24 | 25 | if(MSVC) 26 | find_library(GFLAGS_LIBRARY_RELEASE 27 | NAMES libgflags 28 | PATHS ${GFLAGS_ROOT_DIR} 29 | PATH_SUFFIXES Release) 30 | 31 | find_library(GFLAGS_LIBRARY_DEBUG 32 | NAMES libgflags-debug 33 | PATHS ${GFLAGS_ROOT_DIR} 34 | PATH_SUFFIXES Debug) 35 | 36 | set(GFLAGS_LIBRARY optimized ${GFLAGS_LIBRARY_RELEASE} debug ${GFLAGS_LIBRARY_DEBUG}) 37 | else() 38 | find_library(GFLAGS_LIBRARY gflags) 39 | endif() 40 | 41 | find_package_handle_standard_args(GFlags DEFAULT_MSG GFLAGS_INCLUDE_DIR GFLAGS_LIBRARY) 42 | 43 | 44 | if(GFLAGS_FOUND) 45 | set(GFLAGS_INCLUDE_DIRS ${GFLAGS_INCLUDE_DIR}) 46 | set(GFLAGS_LIBRARIES ${GFLAGS_LIBRARY}) 47 | message(STATUS "Found gflags (include: ${GFLAGS_INCLUDE_DIR}, library: ${GFLAGS_LIBRARY})") 48 | mark_as_advanced(GFLAGS_LIBRARY_DEBUG GFLAGS_LIBRARY_RELEASE 49 | GFLAGS_LIBRARY GFLAGS_INCLUDE_DIR GFLAGS_ROOT_DIR) 50 | endif() -------------------------------------------------------------------------------- /src/kitti.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/python3 2 | import os 3 | import sys 4 | import time 5 | import numpy 6 | import pygicp 7 | from matplotlib import pyplot 8 | 9 | def main(): 10 | if len(sys.argv) < 2: 11 | print('usage: kitti.py /path/to/kitti/sequences/00/velodyne') 12 | return 13 | 14 | # List input files 15 | seq_path = sys.argv[1] 16 | filenames = sorted([seq_path + '/' + x for x in os.listdir(seq_path) if x.endswith('.bin')]) 17 | 18 | # You can choose any of FastGICP, FastVGICP, FastVGICPCuda, or NDTCuda 19 | reg = pygicp.FastGICP() 20 | 21 | # pygicp classes have the same interface as the C++ version 22 | # reg.set_num_threads(8) 23 | # reg.set_max_correspondence_distance(2.0) 24 | 25 | stamps = [time.time()] # for FPS calculation 26 | poses = [numpy.identity(4)] # sensor trajectory 27 | 28 | for i, filename in enumerate(filenames): 29 | # Read and downsample input cloud 30 | points = numpy.fromfile(filename, dtype=numpy.float32).reshape(-1, 4)[:, :3] 31 | points = pygicp.downsample(points, 0.25) 32 | 33 | if i == 0: 34 | reg.set_input_target(points) 35 | delta = numpy.identity(4) 36 | else: 37 | reg.set_input_source(points) 38 | delta = reg.align() 39 | reg.swap_source_and_target() 40 | 41 | # Accumulate the delta to compute the full sensor trajectory 42 | poses.append(poses[-1].dot(delta)) 43 | 44 | # FPS calculation for the last ten frames 45 | stamps = stamps[-9:] + [time.time()] 46 | print('fps:%.3f' % (len(stamps) / (stamps[-1] - stamps[0]))) 47 | 48 | # Plot the estimated trajectory 49 | traj = numpy.array([x[:3, 3] for x in poses]) 50 | 51 | if i % 30 == 0: 52 | pyplot.clf() 53 | pyplot.plot(traj[:, 0], traj[:, 1]) 54 | pyplot.axis('equal') 55 | pyplot.pause(0.01) 56 | 57 | 58 | if __name__ == '__main__': 59 | main() 60 | -------------------------------------------------------------------------------- /src/fast_gicp/cuda/covariance_estimation.cu: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | 5 | #include 6 | #include 7 | #include 8 | 9 | #include 10 | #include 11 | 12 | namespace fast_gicp { 13 | namespace cuda { 14 | 15 | namespace { 16 | struct covariance_estimation_kernel { 17 | covariance_estimation_kernel(const thrust::device_vector& points, int k, const thrust::device_vector& k_neighbors, thrust::device_vector& covariances) 18 | : k(k), points_ptr(points.data()), k_neighbors_ptr(k_neighbors.data()), covariances_ptr(covariances.data()) {} 19 | 20 | __host__ __device__ void operator()(int idx) const { 21 | // target points buffer & nn output buffer 22 | const Eigen::Vector3f* points = thrust::raw_pointer_cast(points_ptr); 23 | const int* k_neighbors = thrust::raw_pointer_cast(k_neighbors_ptr) + idx * k; 24 | Eigen::Matrix3f* cov = thrust::raw_pointer_cast(covariances_ptr) + idx; 25 | 26 | Eigen::Vector3f mean(0.0f, 0.0f, 0.0f); 27 | cov->setZero(); 28 | for(int i = 0; i < k; i++) { 29 | const auto& pt = points[k_neighbors[i]]; 30 | mean += pt; 31 | (*cov) += pt * pt.transpose(); 32 | } 33 | mean /= k; 34 | (*cov) = (*cov) / k - mean * mean.transpose(); 35 | } 36 | 37 | const int k; 38 | thrust::device_ptr points_ptr; 39 | thrust::device_ptr k_neighbors_ptr; 40 | 41 | thrust::device_ptr covariances_ptr; 42 | }; 43 | } // namespace 44 | 45 | void covariance_estimation(const thrust::device_vector& points, int k, const thrust::device_vector& k_neighbors, thrust::device_vector& covariances) { 46 | thrust::device_vector d_indices(points.size()); 47 | thrust::sequence(d_indices.begin(), d_indices.end()); 48 | 49 | covariances.resize(points.size()); 50 | thrust::for_each(d_indices.begin(), d_indices.end(), covariance_estimation_kernel(points, k, k_neighbors, covariances)); 51 | } 52 | } 53 | } 54 | -------------------------------------------------------------------------------- /include/litamin2/ceres_cost/litamin2_cost.hpp: -------------------------------------------------------------------------------- 1 | #ifndef REGISTRATION_COST_HPP 2 | #define REGISTRATION_COST_HPP 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | 10 | struct LiTAMIN2CostFunction 11 | { 12 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 13 | LiTAMIN2CostFunction(Eigen::Vector3d p_mean, Eigen::Vector3d q_mean, Eigen::Matrix3d p_cov, Eigen::Matrix3d q_cov, 14 | Eigen::Matrix3d lambdaI,double sigma_ICP) : 15 | p_mean_(p_mean), q_mean_(q_mean), p_cov_(p_cov), q_cov_(q_cov),lambdaI_(lambdaI),sigma_ICP_(sigma_ICP) {} 16 | 17 | template 18 | bool operator()(const T *const q, const T *const t, T *residuals) const 19 | { 20 | Eigen::Map> residuals_map(residuals); 21 | Eigen::Matrix p_m(p_mean_.cast()); 22 | Eigen::Matrix q_m(q_mean_.cast()); 23 | Eigen::Matrix p_c = p_cov_.cast(); 24 | Eigen::Matrix q_c = q_cov_.cast(); 25 | Eigen::Matrix lambI = lambdaI_.cast(); 26 | Eigen::Quaternion quat(q); 27 | Eigen::Matrix translation(t); 28 | 29 | Eigen::Matrix mahalanobis = (q_c + quat * p_c * (quat.inverse()) + lambdaI_).inverse(); 30 | mahalanobis.normalize(); 31 | Eigen::Matrix LT = mahalanobis.llt().matrixL().transpose(); 32 | residuals_map = LT * (q_m - (quat * p_m + translation)); 33 | T EICP = T(residuals_map.squaredNorm()); 34 | T sigma_square = T(sigma_ICP_*sigma_ICP_); 35 | residuals_map = (T(1.)-(EICP/(EICP+sigma_square)))*residuals_map; 36 | 37 | return true; 38 | } 39 | 40 | static ceres::CostFunction *Create(Eigen::Vector3d p_mean_, Eigen::Vector3d q_mean_, Eigen::Matrix3d p_cov_, 41 | Eigen::Matrix3d q_cov_,Eigen::Matrix3d lambdaI_, double sigma_ICP_){ 42 | // 残差是三维的,变量分别是思维和三维 43 | return (new ceres::AutoDiffCostFunction 44 | (new LiTAMIN2CostFunction(p_mean_,q_mean_,p_cov_,q_cov_,lambdaI_,sigma_ICP_))); 45 | } 46 | 47 | Eigen::Vector3d p_mean_, q_mean_; 48 | Eigen::Matrix3d p_cov_, q_cov_; 49 | Eigen::Matrix3d lambdaI_; 50 | double sigma_ICP_; 51 | }; 52 | #endif -------------------------------------------------------------------------------- /include/fast_gicp/cuda/ndt_cuda.cuh: -------------------------------------------------------------------------------- 1 | #ifndef FAST_GICP_NDT_CUDA_CUH 2 | #define FAST_GICP_NDT_CUDA_CUH 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | #include 10 | #include 11 | 12 | namespace thrust { 13 | template 14 | class pair; 15 | 16 | template 17 | class device_allocator; 18 | 19 | template 20 | class device_vector; 21 | } // namespace thrust 22 | 23 | namespace fast_gicp { 24 | namespace cuda { 25 | 26 | class GaussianVoxelMap; 27 | 28 | class NDTCudaCore { 29 | public: 30 | using Points = thrust::device_vector>; 31 | using Indices = thrust::device_vector>; 32 | using Matrices = thrust::device_vector>; 33 | using Correspondences = thrust::device_vector, thrust::device_allocator>>; 34 | using VoxelCoordinates = thrust::device_vector>; 35 | 36 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 37 | NDTCudaCore(); 38 | ~NDTCudaCore(); 39 | 40 | void set_distance_mode(fast_gicp::NDTDistanceMode mode); 41 | void set_resolution(double resolution); 42 | void set_neighbor_search_method(fast_gicp::NeighborSearchMethod method, double radius); 43 | 44 | void swap_source_and_target(); 45 | void set_source_cloud(const std::vector>& cloud); 46 | void set_target_cloud(const std::vector>& cloud); 47 | 48 | void create_voxelmaps(); 49 | void create_target_voxelmap(); 50 | void create_source_voxelmap(); 51 | 52 | void update_correspondences(const Eigen::Isometry3d& trans); 53 | double compute_error(const Eigen::Isometry3d& trans, Eigen::Matrix* H, Eigen::Matrix* b) const; 54 | 55 | public: 56 | fast_gicp::NDTDistanceMode distance_mode; 57 | double resolution; 58 | std::unique_ptr offsets; 59 | 60 | std::unique_ptr source_points; 61 | std::unique_ptr target_points; 62 | 63 | std::unique_ptr source_voxelmap; 64 | std::unique_ptr target_voxelmap; 65 | 66 | Eigen::Isometry3f linearized_x; 67 | std::unique_ptr correspondences; 68 | }; 69 | 70 | } // namespace cuda 71 | } // namespace fast_gicp 72 | 73 | #endif -------------------------------------------------------------------------------- /src/fast_gicp/cuda/compute_mahalanobis.cu: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include 4 | 5 | namespace fast_gicp { 6 | namespace cuda { 7 | 8 | namespace { 9 | 10 | struct compute_mahalanobis_kernel { 11 | compute_mahalanobis_kernel(const GaussianVoxelMap& voxelmap, const Eigen::Isometry3f& x) 12 | : R(x.linear()), 13 | t(x.translation()), 14 | voxel_num_points_ptr(voxelmap.num_points.data()), 15 | voxel_means_ptr(voxelmap.voxel_means.data()), 16 | voxel_covs_ptr(voxelmap.voxel_covs.data()) 17 | {} 18 | 19 | __host__ __device__ 20 | Eigen::Matrix3f operator() (const thrust::tuple& input) const { 21 | const Eigen::Vector3f& mean_A = thrust::get<0>(input); 22 | const Eigen::Matrix3f& cov_A = thrust::get<1>(input); 23 | int voxel_index = thrust::get<2>(input); 24 | 25 | if(voxel_index < 0) { 26 | return Eigen::Matrix3f::Identity(); 27 | } 28 | 29 | const Eigen::Matrix3f& cov_B = thrust::raw_pointer_cast(voxel_covs_ptr)[voxel_index]; 30 | Eigen::Matrix3f RCR = cov_B + R * cov_A* R.transpose(); 31 | return RCR.inverse(); 32 | } 33 | 34 | const Eigen::Matrix3f R; 35 | const Eigen::Vector3f t; 36 | 37 | thrust::device_ptr voxel_num_points_ptr; 38 | thrust::device_ptr voxel_means_ptr; 39 | thrust::device_ptr voxel_covs_ptr; 40 | }; 41 | } 42 | 43 | 44 | void compute_mahalanobis( 45 | const thrust::device_vector& src_points, 46 | const thrust::device_vector& src_covs, 47 | const GaussianVoxelMap& voxelmap, 48 | const thrust::device_vector& voxel_correspondences, 49 | const Eigen::Isometry3f& linearized_x, 50 | thrust::device_vector& mahalanobis 51 | ) { 52 | mahalanobis.resize(src_points.size()); 53 | thrust::transform( 54 | thrust::make_zip_iterator(thrust::make_tuple(src_points.begin(), src_covs.begin(), voxel_correspondences.begin())), 55 | thrust::make_zip_iterator(thrust::make_tuple(src_points.end(), src_covs.end(), voxel_correspondences.end())), 56 | mahalanobis.begin(), 57 | compute_mahalanobis_kernel(voxelmap, linearized_x) 58 | ); 59 | 60 | thrust::host_vector corrs = voxel_correspondences; 61 | thrust::host_vector> h_maha = mahalanobis; 62 | std::ofstream ofs("/tmp/vgicp_cuda_mahalanobis.txt"); 63 | for(int i=0; i 5 | #include 6 | 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | 14 | namespace fast_gicp { 15 | 16 | /** 17 | * @brief Fast GICP algorithm optimized for single threading 18 | */ 19 | template 20 | class FastGICPSingleThread : public FastGICP { 21 | public: 22 | using Scalar = float; 23 | using Matrix4 = typename pcl::Registration::Matrix4; 24 | 25 | using PointCloudSource = typename pcl::Registration::PointCloudSource; 26 | using PointCloudSourcePtr = typename PointCloudSource::Ptr; 27 | using PointCloudSourceConstPtr = typename PointCloudSource::ConstPtr; 28 | 29 | #if PCL_VERSION >= PCL_VERSION_CALC(1, 10, 0) 30 | using Ptr = pcl::shared_ptr>; 31 | using ConstPtr = pcl::shared_ptr>; 32 | #else 33 | using Ptr = boost::shared_ptr>; 34 | using ConstPtr = boost::shared_ptr>; 35 | #endif 36 | 37 | protected: 38 | using pcl::Registration::input_; 39 | using pcl::Registration::target_; 40 | 41 | using FastGICP::target_kdtree_; 42 | using FastGICP::correspondences_; 43 | using FastGICP::sq_distances_; 44 | using FastGICP::source_covs_; 45 | using FastGICP::target_covs_; 46 | using FastGICP::mahalanobis_; 47 | 48 | public: 49 | FastGICPSingleThread(); 50 | virtual ~FastGICPSingleThread() override; 51 | 52 | protected: 53 | virtual void computeTransformation(PointCloudSource& output, const Matrix4& guess) override; 54 | 55 | private: 56 | virtual void update_correspondences(const Eigen::Isometry3d& trans) override; 57 | 58 | virtual double linearize(const Eigen::Isometry3d& trans, Eigen::Matrix* H = nullptr, Eigen::Matrix* b = nullptr) override; 59 | 60 | virtual double compute_error(const Eigen::Isometry3d& trans) override; 61 | 62 | private: 63 | std::vector second_sq_distances_; 64 | std::vector> anchors_; 65 | }; 66 | } // namespace fast_gicp 67 | 68 | #endif 69 | -------------------------------------------------------------------------------- /include/fast_gicp/time_utils.hpp: -------------------------------------------------------------------------------- 1 | #ifndef TIME_UTILS_XYW_HPP 2 | #define TIME_UTILS_XYW_HPP 3 | #include 4 | #include 5 | #include 6 | 7 | using namespace std; 8 | /** 9 | * @brief Get the Local Time object 10 | * @ref https://www.runoob.com/w3cnote/cpp-time_t.html 11 | * @param y_m_d 12 | * @param h_m_s 13 | */ 14 | void getLocalTime(string y_m_d,string h_m_s){ 15 | time_t timep; 16 | time(&timep); 17 | char ymd[16],hms[16]; 18 | strftime(ymd,sizeof(ymd),"%Y-%m-%d",localtime(&timep)); 19 | strftime(hms,sizeof(hms),"%H:%M:%S",localtime(&timep)); 20 | y_m_d = ymd; 21 | h_m_s = hms; 22 | } 23 | /** 24 | * @brief 把UNIX时间戳拆成double，防止精度丢失 25 | * 26 | * @param time 27 | * @return double 28 | */ 29 | inline double string2time(string time) 30 | { 31 | string integer = time.substr(0, 10); // 整数 32 | string decimal = time.substr(10); //小数 33 | double out = stoi(integer) + stod(decimal) / 1000000.0; 34 | return out; 35 | } 36 | 37 | 38 | /** 39 | * @brief 从字符串中拆出时间戳 40 | * 41 | */ 42 | inline double getCameraTimestampfromFileName(string name) 43 | { 44 | return string2time(name.substr(11, 16)); 45 | } 46 | //toc的单位为ms 47 | namespace tic 48 | { 49 | // 计时 50 | class TicToc 51 | { 52 | private: 53 | std::chrono::time_point start, end; 54 | 55 | public: 56 | TicToc() { tic(); } 57 | 58 | void tic() { start = std::chrono::system_clock::now(); } 59 | 60 | double toc() 61 | { 62 | end = std::chrono::system_clock::now(); 63 | 64 | std::chrono::duration elapsed_seconds = end - start; 65 | return elapsed_seconds.count() * 1000; 66 | } 67 | }; 68 | // 多阶段计时 69 | class TicTocPart 70 | { 71 | public: 72 | TicTocPart() { tic(); } 73 | 74 | void tic() 75 | { 76 | start = std::chrono::system_clock::now(); 77 | tmp = start; 78 | } 79 | 80 | double toc() 81 | { 82 | std::chrono::time_point now = std::chrono::system_clock::now(); 83 | std::chrono::duration elapsed_seconds = now - tmp; 84 | tmp = now; 85 | 86 | return elapsed_seconds.count() * 1000; 87 | } 88 | 89 | double tocEnd() 90 | { 91 | end = std::chrono::system_clock::now(); 92 | 93 | std::chrono::duration elapsed_seconds = end - start; 94 | return elapsed_seconds.count() * 1000; 95 | } 96 | 97 | private: 98 | std::chrono::time_point start, end, tmp; 99 | }; 100 | } 101 | #endif -------------------------------------------------------------------------------- /include/fast_gicp/ndt/ndt_cuda.hpp: -------------------------------------------------------------------------------- 1 | #ifndef FAST_GICP_NDT_CUDA_HPP 2 | #define FAST_GICP_NDT_CUDA_HPP 3 | 4 | #include 5 | #include 6 | 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | 15 | namespace fast_gicp { 16 | 17 | namespace cuda { 18 | class NDTCudaCore; 19 | } 20 | 21 | template 22 | class NDTCuda : public LsqRegistration { 23 | public: 24 | using Scalar = float; 25 | using Matrix4 = typename pcl::Registration::Matrix4; 26 | 27 | using PointCloudSource = typename pcl::Registration::PointCloudSource; 28 | using PointCloudSourcePtr = typename PointCloudSource::Ptr; 29 | using PointCloudSourceConstPtr = typename PointCloudSource::ConstPtr; 30 | 31 | using PointCloudTarget = typename pcl::Registration::PointCloudTarget; 32 | using PointCloudTargetPtr = typename PointCloudTarget::Ptr; 33 | using PointCloudTargetConstPtr = typename PointCloudTarget::ConstPtr; 34 | 35 | #if PCL_VERSION >= PCL_VERSION_CALC(1, 10, 0) 36 | using Ptr = pcl::shared_ptr>; 37 | using ConstPtr = pcl::shared_ptr>; 38 | #else 39 | using Ptr = boost::shared_ptr>; 40 | using ConstPtr = boost::shared_ptr>; 41 | #endif 42 | 43 | protected: 44 | using pcl::Registration::reg_name_; 45 | using pcl::Registration::input_; 46 | using pcl::Registration::target_; 47 | using pcl::Registration::corr_dist_threshold_; 48 | 49 | public: 50 | NDTCuda(); 51 | virtual ~NDTCuda() override; 52 | 53 | void setDistanceMode(NDTDistanceMode mode); 54 | void setResolution(double resolution); 55 | void setNeighborSearchMethod(NeighborSearchMethod method, double radius = -1.0); 56 | 57 | virtual void swapSourceAndTarget() override; 58 | virtual void clearSource() override; 59 | virtual void clearTarget() override; 60 | 61 | virtual void setInputSource(const PointCloudSourceConstPtr& cloud) override; 62 | virtual void setInputTarget(const PointCloudTargetConstPtr& cloud) override; 63 | 64 | protected: 65 | virtual void computeTransformation(PointCloudSource& output, const Matrix4& guess) override; 66 | virtual double linearize(const Eigen::Isometry3d& trans, Eigen::Matrix* H, Eigen::Matrix* b) override; 67 | virtual double compute_error(const Eigen::Isometry3d& trans) override; 68 | 69 | protected: 70 | std::unique_ptr ndt_cuda_; 71 | }; 72 | } // namespace fast_gicp 73 | 74 | #endif -------------------------------------------------------------------------------- /include/fast_gicp/so3/so3.hpp: -------------------------------------------------------------------------------- 1 | #ifndef FAST_GICP_SO3_HPP 2 | #define FAST_GICP_SO3_HPP 3 | 4 | #include 5 | #include 6 | 7 | namespace fast_gicp { 8 | 9 | inline Eigen::Matrix3f skew(const Eigen::Vector3f& x) { 10 | Eigen::Matrix3f skew = Eigen::Matrix3f::Zero(); 11 | skew(0, 1) = -x[2]; 12 | skew(0, 2) = x[1]; 13 | skew(1, 0) = x[2]; 14 | skew(1, 2) = -x[0]; 15 | skew(2, 0) = -x[1]; 16 | skew(2, 1) = x[0]; 17 | 18 | return skew; 19 | } 20 | 21 | inline Eigen::Matrix3d skewd(const Eigen::Vector3d& x) { 22 | Eigen::Matrix3d skew = Eigen::Matrix3d::Zero(); 23 | skew(0, 1) = -x[2]; 24 | skew(0, 2) = x[1]; 25 | skew(1, 0) = x[2]; 26 | skew(1, 2) = -x[0]; 27 | skew(2, 0) = -x[1]; 28 | skew(2, 1) = x[0]; 29 | 30 | return skew; 31 | } 32 | 33 | /* 34 | * SO3 expmap code taken from Sophus 35 | * https://github.com/strasdat/Sophus/blob/593db47500ea1a2de5f0e6579c86147991509c59/sophus/so3.hpp#L585 36 | * 37 | * Copyright 2011-2017 Hauke Strasdat 38 | * 2012-2017 Steven Lovegrove 39 | * 40 | * Permission is hereby granted, free of charge, to any person obtaining a copy 41 | * of this software and associated documentation files (the "Software"), to 42 | * deal in the Software without restriction, including without limitation the 43 | * rights to use, copy, modify, merge, publish, distribute, sublicense, and/or 44 | * sell copies of the Software, and to permit persons to whom the Software is 45 | * furnished to do so, subject to the following conditions: 46 | * 47 | * The above copyright notice and this permission notice shall be included in 48 | * all copies or substantial portions of the Software. 49 | * 50 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 51 | * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 52 | * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 53 | * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 54 | * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING 55 | * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS 56 | * IN THE SOFTWARE. 57 | */ 58 | // 等价于 return Eigen::Quaterniond q(Eigen::AngleAxisd(v.norm(),v.normalized())); 59 | inline Eigen::Quaterniond so3_exp(const Eigen::Vector3d& omega) { 60 | double theta_sq = omega.dot(omega); 61 | 62 | double theta; 63 | double imag_factor; 64 | double real_factor; 65 | if(theta_sq < 1e-10) { 66 | theta = 0; 67 | double theta_quad = theta_sq * theta_sq; 68 | imag_factor = 0.5 - 1.0 / 48.0 * theta_sq + 1.0 / 3840.0 * theta_quad; 69 | real_factor = 1.0 - 1.0 / 8.0 * theta_sq + 1.0 / 384.0 * theta_quad; 70 | } else { 71 | theta = std::sqrt(theta_sq); 72 | double half_theta = 0.5 * theta; 73 | imag_factor = std::sin(half_theta) / theta; 74 | real_factor = std::cos(half_theta); 75 | } 76 | 77 | return Eigen::Quaterniond(real_factor, imag_factor * omega.x(), imag_factor * omega.y(), imag_factor * omega.z()); 78 | } 79 | 80 | inline Eigen::Vector3d so3_log(const Eigen::Isometry3d& transform) { 81 | Eigen::AngleAxisd aa(transform.rotation()); 82 | return Eigen::Vector3d(aa.angle()*aa.axis()); 83 | } 84 | 85 | } // namespace fast_gicp 86 | 87 | #endif -------------------------------------------------------------------------------- /include/litamin2/ceres_cost/PoseSE3Parameterization.hpp: -------------------------------------------------------------------------------- 1 | // Author of FLOAM: Wang Han 2 | // Email wh200720041@gmail.com 3 | // Homepage https://wanghan.pro 4 | #ifndef _LIDAR_OPTIMIZATION_ANALYTIC_H_ 5 | #define _LIDAR_OPTIMIZATION_ANALYTIC_H_ 6 | 7 | #include 8 | #include 9 | #include 10 | #include 11 | 12 | Eigen::Matrix3d skew(Eigen::Vector3d& mat_in) { 13 | Eigen::Matrix skew_mat; 14 | skew_mat.setZero(); 15 | skew_mat(0, 1) = -mat_in(2); 16 | skew_mat(0, 2) = mat_in(1); 17 | skew_mat(1, 2) = -mat_in(0); 18 | skew_mat(1, 0) = mat_in(2); 19 | skew_mat(2, 0) = -mat_in(1); 20 | skew_mat(2, 1) = mat_in(0); 21 | return skew_mat; 22 | } 23 | 24 | void getTransformFromSe3(const Eigen::Matrix& se3, Eigen::Quaterniond& q, Eigen::Vector3d& t) { 25 | Eigen::Vector3d omega(se3.data()); 26 | Eigen::Vector3d upsilon(se3.data() + 3); 27 | Eigen::Matrix3d Omega = skew(omega); 28 | 29 | double theta = omega.norm(); 30 | double half_theta = 0.5 * theta; 31 | 32 | double imag_factor; 33 | double real_factor = cos(half_theta); 34 | if (theta < 1e-10) { 35 | double theta_sq = theta * theta; 36 | double theta_po4 = theta_sq * theta_sq; 37 | imag_factor = 0.5 - 0.0208333 * theta_sq + 0.000260417 * theta_po4; 38 | } else { 39 | double sin_half_theta = sin(half_theta); 40 | imag_factor = sin_half_theta / theta; 41 | } 42 | 43 | q = Eigen::Quaterniond(real_factor, imag_factor * omega.x(), imag_factor * omega.y(), imag_factor * omega.z()); 44 | 45 | Eigen::Matrix3d J; 46 | if (theta < 1e-10) { 47 | J = q.matrix(); 48 | } else { 49 | Eigen::Matrix3d Omega2 = Omega * Omega; 50 | J = (Eigen::Matrix3d::Identity() + (1 - cos(theta)) / (theta * theta) * Omega + (theta - sin(theta)) / (pow(theta, 3)) * Omega2); 51 | } 52 | 53 | t = J * upsilon; 54 | } 55 | 56 | 57 | 58 | class PoseSE3Parameterization : public ceres::LocalParameterization { 59 | public: 60 | PoseSE3Parameterization() {} 61 | virtual ~PoseSE3Parameterization() {} 62 | // 从这里看，参数块应该是[q,t]，且q是[x,y,z,w] 63 | virtual bool Plus(const double* x, const double* delta, double* x_plus_delta) const { 64 | Eigen::Map trans(x + 4); 65 | 66 | Eigen::Quaterniond delta_q; 67 | Eigen::Vector3d delta_t; 68 | getTransformFromSe3(Eigen::Map>(delta), delta_q, delta_t); 69 | Eigen::Map quater(x); 70 | Eigen::Map quater_plus(x_plus_delta); 71 | Eigen::Map trans_plus(x_plus_delta + 4); 72 | // TODO 不需要归一化吗？ 73 | quater_plus = delta_q * quater; 74 | trans_plus = delta_q * trans + delta_t; 75 | 76 | return true; 77 | } 78 | // ref: https://github.com/wh200720041/floam/issues/50 79 | // 这是全局参数到局部参数的雅可比矩阵。全局是trans,quat，因此是7维，局部则是se3,因此是6维。 80 | // 答案中提到了J1*J2,其中J1是参数块对[t,q]求导来自于evaluate或者AD，J2则是这里的到李代数的参数化。 81 | virtual bool ComputeJacobian(const double* x, double* jacobian) const { 82 | Eigen::Map> j(jacobian); 83 | (j.topRows(6)).setIdentity(); 84 | (j.bottomRows(1)).setZero(); 85 | 86 | return true; 87 | } 88 | virtual int GlobalSize() const { return 7; } 89 | virtual int LocalSize() const { return 6; } 90 | }; 91 | 92 | #endif // _LIDAR_OPTIMIZATION_ANALYTIC_H_ 93 | -------------------------------------------------------------------------------- /cmake_modules/FindLIBLAS.cmake: -------------------------------------------------------------------------------- 1 | #--- 2 | # File: FindLIBLAS.cmake 3 | # 4 | # Find the native LIBLAS includes and library 5 | # 6 | # LIBLAS_INCLUDE_DIRS - where to find liblas's includes. 7 | # LIBLAS_LIBRARIES - List of libraries when using liblas. 8 | # LIBLAS_FOUND - True if liblas found. 9 | #--- 10 | 11 | 12 | # Set the include dir: 13 | find_path(LIBLAS_INCLUDE_DIR liblas/liblas.hpp) 14 | 15 | # Macro for setting libraries: 16 | macro(FIND_LIBLAS_LIBRARY MYLIBRARY MYLIBRARYNAME) 17 | 18 | find_library( 19 | "${MYLIBRARY}_DEBUG" 20 | NAMES "${MYLIBRARYNAME}${CMAKE_DEBUG_POSTFIX}" "lib${MYLIBRARYNAME}${CMAKE_DEBUG_POSTFIX}" 21 | PATHS 22 | ${LIBLAS_DIR}/lib/Debug 23 | ${LIBLAS_DIR}/lib64/Debug 24 | ${LIBLAS_DIR}/lib 25 | ${LIBLAS_DIR}/lib64 26 | $ENV{LIBLAS_DIR}/lib/debug 27 | $ENV{LIBLAS_DIR}/lib64/debug 28 | NO_DEFAULT_PATH 29 | ) 30 | 31 | find_library( 32 | "${MYLIBRARY}_DEBUG" 33 | NAMES "${MYLIBRARYNAME}${CMAKE_DEBUG_POSTFIX}" "lib${MYLIBRARYNAME}${CMAKE_DEBUG_POSTFIX}" 34 | PATHS 35 | ~/Library/Frameworks 36 | /Library/Frameworks 37 | /usr/local/lib 38 | /usr/local/lib64 39 | /usr/lib 40 | /usr/lib64 41 | /sw/lib 42 | /opt/local/lib 43 | /opt/csw/lib 44 | /opt/lib 45 | [HKEY_LOCAL_MACHINE\\SYSTEM\\CurrentControlSet\\Control\\Session\ Manager\\Environment;LIBLAS_ROOT]/lib 46 | /usr/freeware/lib64 47 | ) 48 | 49 | find_library( 50 | ${MYLIBRARY} 51 | NAMES "${MYLIBRARYNAME}${CMAKE_RELEASE_POSTFIX}" "lib${MYLIBRARYNAME}${CMAKE_RELEASE_POSTFIX}" 52 | PATHS 53 | ${LIBLAS_DIR}/lib/Release 54 | ${LIBLAS_DIR}/lib64/Release 55 | ${LIBLAS_DIR}/lib 56 | ${LIBLAS_DIR}/lib64 57 | $ENV{LIBLAS_DIR}/lib/Release 58 | $ENV{LIBLAS_DIR}/lib64/Release 59 | $ENV{LIBLAS_DIR}/lib 60 | $ENV{LIBLAS_DIR}/lib64 61 | $ENV{LIBLAS_DIR} 62 | $ENV{LIBLASDIR}/lib 63 | $ENV{LIBLASDIR}/lib64 64 | $ENV{LIBLASDIR} 65 | $ENV{LIBLAS_ROOT}/lib 66 | $ENV{LIBLAS_ROOT}/lib64 67 | NO_DEFAULT_PATH 68 | ) 69 | 70 | find_library( 71 | ${MYLIBRARY} 72 | NAMES "${MYLIBRARYNAME}${CMAKE_RELEASE_POSTFIX}" "lib${MYLIBRARYNAME}${CMAKE_RELEASE_POSTFIX}" 73 | PATHS 74 | ~/Library/Frameworks 75 | /Library/Frameworks 76 | /usr/local/lib 77 | /usr/local/lib64 78 | /usr/lib 79 | /usr/lib64 80 | /sw/lib 81 | /opt/local/lib 82 | /opt/csw/lib 83 | /opt/lib 84 | [HKEY_LOCAL_MACHINE\\SYSTEM\\CurrentControlSet\\Control\\Session\ Manager\\Environment;LIBLAS_ROOT]/lib 85 | /usr/freeware/lib64 86 | ) 87 | 88 | if( NOT ${MYLIBRARY}_DEBUG ) 89 | if( MYLIBRARY ) 90 | set( ${MYLIBRARY}_DEBUG ${MYLIBRARY} ) 91 | endif(MYLIBRARY) 92 | endif( NOT ${MYLIBRARY}_DEBUG ) 93 | 94 | endmacro(FIND_LIBLAS_LIBRARY LIBRARY LIBRARYNAME) 95 | 96 | FIND_LIBLAS_LIBRARY(LIBLAS_LIBRARY las) 97 | 98 | set(LIBLAS_FOUND "NO") 99 | if(LIBLAS_LIBRARY AND LIBLAS_INCLUDE_DIR) 100 | FIND_PACKAGE(Boost) # used by LIBLAS 101 | if(Boost_FOUND) 102 | set(LIBLAS_LIBRARIES ${LIBLAS_LIBRARY} ) 103 | set(LIBLAS_FOUND "YES") 104 | if(WIN32) 105 | link_directories(${Boost_LIBRARY_DIRS}) 106 | endif() 107 | endif() 108 | endif() -------------------------------------------------------------------------------- /cmake_modules/FindSuiteSparse.cmake: -------------------------------------------------------------------------------- 1 | FIND_PATH(CHOLMOD_INCLUDE_DIR NAMES cholmod.h amd.h camd.h 2 | PATHS 3 | ${SUITE_SPARSE_ROOT}/include 4 | /usr/include/suitesparse 5 | /usr/include/ufsparse 6 | /opt/local/include/ufsparse 7 | /usr/local/include/ufsparse 8 | /sw/include/ufsparse 9 | ) 10 | 11 | FIND_LIBRARY(CHOLMOD_LIBRARY NAMES cholmod 12 | PATHS 13 | ${SUITE_SPARSE_ROOT}/lib 14 | /usr/lib 15 | /usr/local/lib 16 | /opt/local/lib 17 | /sw/lib 18 | ) 19 | 20 | FIND_LIBRARY(AMD_LIBRARY NAMES SHARED NAMES amd 21 | PATHS 22 | ${SUITE_SPARSE_ROOT}/lib 23 | /usr/lib 24 | /usr/local/lib 25 | /opt/local/lib 26 | /sw/lib 27 | ) 28 | 29 | FIND_LIBRARY(CAMD_LIBRARY NAMES camd 30 | PATHS 31 | ${SUITE_SPARSE_ROOT}/lib 32 | /usr/lib 33 | /usr/local/lib 34 | /opt/local/lib 35 | /sw/lib 36 | ) 37 | 38 | FIND_LIBRARY(SUITESPARSECONFIG_LIBRARY NAMES suitesparseconfig 39 | PATHS 40 | ${SUITE_SPARSE_ROOT}/lib 41 | /usr/lib 42 | /usr/local/lib 43 | /opt/local/lib 44 | /sw/lib 45 | ) 46 | 47 | 48 | # Different platforms seemingly require linking against different sets of libraries 49 | IF(CYGWIN) 50 | FIND_PACKAGE(PkgConfig) 51 | FIND_LIBRARY(COLAMD_LIBRARY NAMES colamd 52 | PATHS 53 | /usr/lib 54 | /usr/local/lib 55 | /opt/local/lib 56 | /sw/lib 57 | ) 58 | PKG_CHECK_MODULES(LAPACK lapack) 59 | 60 | SET(CHOLMOD_LIBRARIES ${CHOLMOD_LIBRARY} ${AMD_LIBRARY} ${CAMD_LIBRARY} ${COLAMD_LIBRARY} ${CCOLAMD_LIBRARY} ${LAPACK_LIBRARIES}) 61 | 62 | # MacPorts build of the SparseSuite requires linking against extra libraries 63 | 64 | ELSEIF(APPLE) 65 | 66 | FIND_LIBRARY(COLAMD_LIBRARY NAMES colamd 67 | PATHS 68 | /usr/lib 69 | /usr/local/lib 70 | /opt/local/lib 71 | /sw/lib 72 | ) 73 | 74 | FIND_LIBRARY(CCOLAMD_LIBRARY NAMES ccolamd 75 | PATHS 76 | /usr/lib 77 | /usr/local/lib 78 | /opt/local/lib 79 | /sw/lib 80 | ) 81 | 82 | FIND_LIBRARY(METIS_LIBRARY NAMES metis 83 | PATHS 84 | /usr/lib 85 | /usr/local/lib 86 | /opt/local/lib 87 | /sw/lib 88 | ) 89 | 90 | SET(CHOLMOD_LIBRARIES ${CHOLMOD_LIBRARY} ${AMD_LIBRARY} ${CAMD_LIBRARY} ${COLAMD_LIBRARY} ${CCOLAMD_LIBRARY} ${METIS_LIBRARY} "-framework Accelerate") 91 | ELSE(APPLE) 92 | SET(CHOLMOD_LIBRARIES ${CHOLMOD_LIBRARY} ${AMD_LIBRARY}) 93 | ENDIF(CYGWIN) 94 | 95 | IF(CHOLMOD_INCLUDE_DIR AND CHOLMOD_LIBRARIES) 96 | SET(CHOLMOD_FOUND TRUE) 97 | ELSE(CHOLMOD_INCLUDE_DIR AND CHOLMOD_LIBRARIES) 98 | SET(CHOLMOD_FOUND FALSE) 99 | ENDIF(CHOLMOD_INCLUDE_DIR AND CHOLMOD_LIBRARIES) 100 | 101 | # Look for csparse; note the difference in the directory specifications! 102 | FIND_PATH(CSPARSE_INCLUDE_DIR NAMES cs.h 103 | PATHS 104 | /usr/include/suitesparse 105 | /usr/include 106 | /opt/local/include 107 | /usr/local/include 108 | /sw/include 109 | /usr/include/ufsparse 110 | /opt/local/include/ufsparse 111 | /usr/local/include/ufsparse 112 | /sw/include/ufsparse 113 | ) 114 | 115 | FIND_LIBRARY(CSPARSE_LIBRARY NAMES cxsparse 116 | PATHS 117 | /usr/lib 118 | /usr/local/lib 119 | /opt/local/lib 120 | /sw/lib 121 | ) 122 | 123 | IF(CSPARSE_INCLUDE_DIR AND CSPARSE_LIBRARY) 124 | SET(CSPARSE_FOUND TRUE) 125 | ELSE(CSPARSE_INCLUDE_DIR AND CSPARSE_LIBRARY) 126 | SET(CSPARSE_FOUND FALSE) 127 | ENDIF(CSPARSE_INCLUDE_DIR AND CSPARSE_LIBRARY) 128 | -------------------------------------------------------------------------------- /include/litamin2/litamin2.hpp: -------------------------------------------------------------------------------- 1 | #ifndef FAST_GICP_FAST_VGICP_HPP 2 | #define FAST_GICP_FAST_VGICP_HPP 3 | 4 | #include 5 | 6 | #include 7 | #include 8 | 9 | #include 10 | #include 11 | #include 12 | #include 13 | 14 | #include 15 | #include 16 | #include 17 | 18 | namespace fast_gicp { 19 | 20 | /** 21 | * @brief Fast Voxelized GICP algorithm boosted with OpenMP 22 | */ 23 | template 24 | class LiTAMIN2 : public FastGICP { 25 | public: 26 | using Scalar = float; 27 | using Matrix4 = typename pcl::Registration::Matrix4; 28 | 29 | using PointCloudSource = typename pcl::Registration::PointCloudSource; 30 | using PointCloudSourcePtr = typename PointCloudSource::Ptr; 31 | using PointCloudSourceConstPtr = typename PointCloudSource::ConstPtr; 32 | 33 | using PointCloudTarget = typename pcl::Registration::PointCloudTarget; 34 | using PointCloudTargetPtr = typename PointCloudTarget::Ptr; 35 | using PointCloudTargetConstPtr = typename PointCloudTarget::ConstPtr; 36 | 37 | #if PCL_VERSION >= PCL_VERSION_CALC(1, 10, 0) 38 | using Ptr = pcl::shared_ptr>; 39 | using ConstPtr = pcl::shared_ptr>; 40 | #else 41 | using Ptr = boost::shared_ptr>; 42 | using ConstPtr = boost::shared_ptr>; 43 | #endif 44 | 45 | protected: 46 | using pcl::Registration::input_; 47 | using pcl::Registration::target_; 48 | 49 | using FastGICP::num_threads_; 50 | using FastGICP::source_kdtree_; 51 | using FastGICP::target_kdtree_; 52 | using FastGICP::source_covs_; 53 | using FastGICP::target_covs_; 54 | 55 | public: 56 | LiTAMIN2(); 57 | virtual ~LiTAMIN2() override; 58 | 59 | void setResolution(double resolution); 60 | void setVoxelAccumulationMode(VoxelAccumulationMode mode); 61 | void setNeighborSearchMethod(NeighborSearchMethod method); 62 | 63 | virtual void swapSourceAndTarget() override; 64 | virtual void setInputTarget(const PointCloudTargetConstPtr& cloud) override; 65 | 66 | protected: 67 | virtual void computeTransformation(PointCloudSource& output, const Matrix4& guess) override; 68 | virtual void update_correspondences(const Eigen::Isometry3d& trans) override; 69 | virtual double linearize(const Eigen::Isometry3d& trans, Eigen::Matrix* H = nullptr, Eigen::Matrix* b = nullptr) override; 70 | virtual double compute_error(const Eigen::Isometry3d& trans) override; 71 | 72 | protected: 73 | double voxel_resolution_; 74 | NeighborSearchMethod search_method_; 75 | VoxelAccumulationMode voxel_mode_; 76 | 77 | std::unique_ptr> voxelmap_; 78 | 79 | std::vector> voxel_correspondences_; 80 | std::vector> voxel_mahalanobis_; 81 | }; 82 | } // namespace fast_gicp 83 | 84 | #endif 85 | -------------------------------------------------------------------------------- /include/fast_gicp/gicp/fast_vgicp.hpp: -------------------------------------------------------------------------------- 1 | #ifndef FAST_GICP_FAST_VGICP_HPP 2 | #define FAST_GICP_FAST_VGICP_HPP 3 | 4 | #include 5 | 6 | #include 7 | #include 8 | 9 | #include 10 | #include 11 | #include 12 | #include 13 | 14 | #include 15 | #include 16 | #include 17 | 18 | namespace fast_gicp { 19 | 20 | /** 21 | * @brief Fast Voxelized GICP algorithm boosted with OpenMP 22 | */ 23 | template 24 | class FastVGICP : public FastGICP { 25 | public: 26 | using Scalar = float; 27 | using Matrix4 = typename pcl::Registration::Matrix4; 28 | 29 | using PointCloudSource = typename pcl::Registration::PointCloudSource; 30 | using PointCloudSourcePtr = typename PointCloudSource::Ptr; 31 | using PointCloudSourceConstPtr = typename PointCloudSource::ConstPtr; 32 | 33 | using PointCloudTarget = typename pcl::Registration::PointCloudTarget; 34 | using PointCloudTargetPtr = typename PointCloudTarget::Ptr; 35 | using PointCloudTargetConstPtr = typename PointCloudTarget::ConstPtr; 36 | 37 | #if PCL_VERSION >= PCL_VERSION_CALC(1, 10, 0) 38 | using Ptr = pcl::shared_ptr>; 39 | using ConstPtr = pcl::shared_ptr>; 40 | #else 41 | using Ptr = boost::shared_ptr>; 42 | using ConstPtr = boost::shared_ptr>; 43 | #endif 44 | 45 | protected: 46 | using pcl::Registration::input_; 47 | using pcl::Registration::target_; 48 | 49 | using FastGICP::num_threads_; 50 | using FastGICP::source_kdtree_; 51 | using FastGICP::target_kdtree_; 52 | using FastGICP::source_covs_; 53 | using FastGICP::target_covs_; 54 | 55 | public: 56 | FastVGICP(); 57 | virtual ~FastVGICP() override; 58 | 59 | void setResolution(double resolution); 60 | void setVoxelAccumulationMode(VoxelAccumulationMode mode); 61 | void setNeighborSearchMethod(NeighborSearchMethod method); 62 | 63 | virtual void swapSourceAndTarget() override; 64 | virtual void setInputTarget(const PointCloudTargetConstPtr& cloud) override; 65 | 66 | protected: 67 | virtual void computeTransformation(PointCloudSource& output, const Matrix4& guess) override; 68 | virtual void update_correspondences(const Eigen::Isometry3d& trans) override; 69 | virtual double linearize(const Eigen::Isometry3d& trans, Eigen::Matrix* H = nullptr, Eigen::Matrix* b = nullptr) override; 70 | virtual double compute_error(const Eigen::Isometry3d& trans) override; 71 | 72 | protected: 73 | double voxel_resolution_; 74 | NeighborSearchMethod search_method_; 75 | VoxelAccumulationMode voxel_mode_; 76 | 77 | std::unique_ptr> voxelmap_; 78 | 79 | std::vector> voxel_correspondences_; 80 | std::vector> voxel_mahalanobis_; 81 | }; 82 | } // namespace fast_gicp 83 | 84 | #endif 85 | -------------------------------------------------------------------------------- /include/fast_gicp/gicp/fast_vgicp_cuda.hpp: -------------------------------------------------------------------------------- 1 | #ifndef FAST_GICP_FAST_VGICP_CUDA_HPP 2 | #define FAST_GICP_FAST_VGICP_CUDA_HPP 3 | 4 | #include 5 | #include 6 | 7 | #include 8 | #include 9 | #include 10 | #include 11 | 12 | #include 13 | #include 14 | 15 | namespace fast_gicp { 16 | 17 | namespace cuda { 18 | class FastVGICPCudaCore; 19 | } 20 | 21 | enum class NearestNeighborMethod { CPU_PARALLEL_KDTREE, GPU_BRUTEFORCE, GPU_RBF_KERNEL }; 22 | 23 | /** 24 | * @brief Fast Voxelized GICP algorithm boosted with CUDA 25 | */ 26 | template 27 | class FastVGICPCuda : public LsqRegistration { 28 | public: 29 | using Scalar = float; 30 | using Matrix4 = typename pcl::Registration::Matrix4; 31 | 32 | using PointCloudSource = typename pcl::Registration::PointCloudSource; 33 | using PointCloudSourcePtr = typename PointCloudSource::Ptr; 34 | using PointCloudSourceConstPtr = typename PointCloudSource::ConstPtr; 35 | 36 | using PointCloudTarget = typename pcl::Registration::PointCloudTarget; 37 | using PointCloudTargetPtr = typename PointCloudTarget::Ptr; 38 | using PointCloudTargetConstPtr = typename PointCloudTarget::ConstPtr; 39 | 40 | #if PCL_VERSION >= PCL_VERSION_CALC(1, 10, 0) 41 | using Ptr = pcl::shared_ptr>; 42 | using ConstPtr = pcl::shared_ptr>; 43 | #else 44 | using Ptr = boost::shared_ptr>; 45 | using ConstPtr = boost::shared_ptr>; 46 | #endif 47 | 48 | protected: 49 | using pcl::Registration::input_; 50 | using pcl::Registration::target_; 51 | 52 | public: 53 | FastVGICPCuda(); 54 | virtual ~FastVGICPCuda() override; 55 | 56 | void setCorrespondenceRandomness(int k); 57 | void setResolution(double resolution); 58 | void setKernelWidth(double kernel_width, double max_dist = -1.0); 59 | void setRegularizationMethod(RegularizationMethod method); 60 | void setNeighborSearchMethod(NeighborSearchMethod method, double radius = -1.0); 61 | void setNearestNeighborSearchMethod(NearestNeighborMethod method); 62 | 63 | virtual void swapSourceAndTarget() override; 64 | virtual void clearSource() override; 65 | virtual void clearTarget() override; 66 | 67 | virtual void setInputSource(const PointCloudSourceConstPtr& cloud) override; 68 | virtual void setInputTarget(const PointCloudTargetConstPtr& cloud) override; 69 | 70 | protected: 71 | virtual void computeTransformation(PointCloudSource& output, const Matrix4& guess) override; 72 | virtual double linearize(const Eigen::Isometry3d& trans, Eigen::Matrix* H = nullptr, Eigen::Matrix* b = nullptr) override; 73 | virtual double compute_error(const Eigen::Isometry3d& trans) override; 74 | 75 | template 76 | std::vector find_neighbors_parallel_kdtree(int k, typename pcl::PointCloud::ConstPtr cloud) const; 77 | 78 | private: 79 | int k_correspondences_; 80 | double voxel_resolution_; 81 | RegularizationMethod regularization_method_; 82 | NearestNeighborMethod neighbor_search_method_; 83 | 84 | std::unique_ptr vgicp_cuda_; 85 | }; 86 | 87 | } // namespace fast_gicp 88 | 89 | #endif 90 | -------------------------------------------------------------------------------- /include/fast_gicp/gicp/experimental/fast_gicp_mp.hpp: -------------------------------------------------------------------------------- 1 | #ifndef FAST_GICP_FAST_GICP_MP_HPP 2 | #define FAST_GICP_FAST_GICP_MP_HPP 3 | 4 | #include 5 | #include 6 | 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | 13 | namespace fast_gicp { 14 | 15 | template 16 | class FastGICPMultiPoints : public pcl::Registration { 17 | public: 18 | using Scalar = float; 19 | using Matrix4 = typename pcl::Registration::Matrix4; 20 | 21 | using PointCloudSource = typename pcl::Registration::PointCloudSource; 22 | using PointCloudSourcePtr = typename PointCloudSource::Ptr; 23 | using PointCloudSourceConstPtr = typename PointCloudSource::ConstPtr; 24 | 25 | using PointCloudTarget = typename pcl::Registration::PointCloudTarget; 26 | using PointCloudTargetPtr = typename PointCloudTarget::Ptr; 27 | using PointCloudTargetConstPtr = typename PointCloudTarget::ConstPtr; 28 | 29 | using pcl::Registration::reg_name_; 30 | using pcl::Registration::input_; 31 | using pcl::Registration::target_; 32 | 33 | using pcl::Registration::nr_iterations_; 34 | using pcl::Registration::max_iterations_; 35 | using pcl::Registration::final_transformation_; 36 | using pcl::Registration::transformation_epsilon_; 37 | using pcl::Registration::converged_; 38 | using pcl::Registration::corr_dist_threshold_; 39 | 40 | FastGICPMultiPoints(); 41 | virtual ~FastGICPMultiPoints() override; 42 | 43 | void setNumThreads(int n); 44 | 45 | void setRotationEpsilon(double eps); 46 | 47 | void setCorrespondenceRandomness(int k); 48 | 49 | void setRegularizationMethod(RegularizationMethod method); 50 | 51 | virtual void setInputSource(const PointCloudSourceConstPtr& cloud) override; 52 | 53 | virtual void setInputTarget(const PointCloudTargetConstPtr& cloud) override; 54 | 55 | protected: 56 | virtual void computeTransformation(PointCloudSource& output, const Matrix4& guess) override; 57 | 58 | private: 59 | bool is_converged(const Eigen::Matrix& delta) const; 60 | 61 | void update_correspondences(const Eigen::Matrix& x); 62 | 63 | Eigen::VectorXf loss_ls(const Eigen::Matrix& x, Eigen::MatrixXf* J) const; 64 | 65 | template 66 | bool calculate_covariances(const pcl::shared_ptr>& cloud, pcl::search::KdTree& kdtree, std::vector>& covariances); 67 | 68 | private: 69 | int num_threads_; 70 | int k_correspondences_; 71 | double rotation_epsilon_; 72 | double neighbor_search_radius_; 73 | 74 | RegularizationMethod regularization_method_; 75 | 76 | pcl::search::KdTree source_kdtree; 77 | pcl::search::KdTree target_kdtree; 78 | 79 | std::vector> source_covs; 80 | std::vector> target_covs; 81 | 82 | std::vector> correspondences; 83 | std::vector> sq_distances; 84 | }; 85 | } // namespace fast_gicp 86 | 87 | #endif 88 | -------------------------------------------------------------------------------- /include/fast_gicp/ndt/impl/ndt_cuda_impl.hpp: -------------------------------------------------------------------------------- 1 | #ifndef FAST_GICP_NDT_CUDA_IMPL_HPP 2 | #define FAST_GICP_NDT_CUDA_IMPL_HPP 3 | 4 | #include 5 | 6 | #include 7 | 8 | namespace fast_gicp { 9 | 10 | template 11 | NDTCuda::NDTCuda() : LsqRegistration() { 12 | this->reg_name_ = "NDTCuda"; 13 | ndt_cuda_.reset(new cuda::NDTCudaCore()); 14 | } 15 | 16 | template 17 | NDTCuda::~NDTCuda() {} 18 | 19 | template 20 | void NDTCuda::setDistanceMode(NDTDistanceMode mode) { 21 | ndt_cuda_->set_distance_mode(mode); 22 | } 23 | 24 | template 25 | void NDTCuda::setResolution(double resolution) { 26 | ndt_cuda_->set_resolution(resolution); 27 | } 28 | 29 | template 30 | void NDTCuda::setNeighborSearchMethod(NeighborSearchMethod method, double radius) { 31 | ndt_cuda_->set_neighbor_search_method(method, radius); 32 | } 33 | 34 | template 35 | void NDTCuda::swapSourceAndTarget() { 36 | ndt_cuda_->swap_source_and_target(); 37 | input_.swap(target_); 38 | } 39 | 40 | template 41 | void NDTCuda::clearSource() { 42 | input_.reset(); 43 | } 44 | 45 | template 46 | void NDTCuda::clearTarget() { 47 | target_.reset(); 48 | } 49 | 50 | template 51 | void NDTCuda::setInputSource(const PointCloudSourceConstPtr& cloud) { 52 | if (cloud == input_) { 53 | return; 54 | } 55 | pcl::Registration::setInputSource(cloud); 56 | 57 | std::vector> points(cloud->size()); 58 | std::transform(cloud->begin(), cloud->end(), points.begin(), [=](const PointSource& pt) { return pt.getVector3fMap(); }); 59 | ndt_cuda_->set_source_cloud(points); 60 | } 61 | 62 | template 63 | void NDTCuda::setInputTarget(const PointCloudTargetConstPtr& cloud) { 64 | if (cloud == target_) { 65 | return; 66 | } 67 | 68 | pcl::Registration::setInputTarget(cloud); 69 | 70 | std::vector> points(cloud->size()); 71 | std::transform(cloud->begin(), cloud->end(), points.begin(), [=](const PointTarget& pt) { return pt.getVector3fMap(); }); 72 | ndt_cuda_->set_target_cloud(points); 73 | } 74 | 75 | template 76 | void NDTCuda::computeTransformation(PointCloudSource& output, const Matrix4& guess) { 77 | ndt_cuda_->create_voxelmaps(); 78 | LsqRegistration::computeTransformation(output, guess); 79 | } 80 | 81 | template 82 | double NDTCuda::linearize(const Eigen::Isometry3d& trans, Eigen::Matrix* H, Eigen::Matrix* b) { 83 | ndt_cuda_->update_correspondences(trans); 84 | return ndt_cuda_->compute_error(trans, H, b); 85 | } 86 | 87 | template 88 | double NDTCuda::compute_error(const Eigen::Isometry3d& trans) { 89 | return ndt_cuda_->compute_error(trans, nullptr, nullptr); 90 | } 91 | 92 | } // namespace fast_gicp 93 | 94 | #endif -------------------------------------------------------------------------------- /cmake_modules/FindG2O.cmake: -------------------------------------------------------------------------------- 1 | FIND_PATH(G2O_INCLUDE_DIR g2o/core/base_vertex.h 2 | ${G2O_ROOT}/include 3 | $ENV{G2O_ROOT}/include 4 | $ENV{G2O_ROOT} 5 | /usr/local/include 6 | /usr/include 7 | /opt/local/include 8 | /sw/local/include 9 | /sw/include 10 | NO_DEFAULT_PATH 11 | ) 12 | 13 | # Macro to unify finding both the debug and release versions of the 14 | # libraries; this is adapted from the OpenSceneGraph FIND_LIBRARY 15 | # macro. 16 | 17 | MACRO(FIND_G2O_LIBRARY MYLIBRARY MYLIBRARYNAME) 18 | 19 | FIND_LIBRARY("${MYLIBRARY}_DEBUG" 20 | NAMES "g2o_${MYLIBRARYNAME}_d" 21 | PATHS 22 | ${G2O_ROOT}/lib/Debug 23 | ${G2O_ROOT}/lib 24 | $ENV{G2O_ROOT}/lib/Debug 25 | $ENV{G2O_ROOT}/lib 26 | NO_DEFAULT_PATH 27 | ) 28 | 29 | FIND_LIBRARY("${MYLIBRARY}_DEBUG" 30 | NAMES "g2o_${MYLIBRARYNAME}_d" 31 | PATHS 32 | ~/Library/Frameworks 33 | /Library/Frameworks 34 | /usr/local/lib 35 | /usr/local/lib64 36 | /usr/lib 37 | /usr/lib64 38 | /opt/local/lib 39 | /sw/local/lib 40 | /sw/lib 41 | NO_DEFAULT_PATH 42 | ) 43 | 44 | FIND_LIBRARY(${MYLIBRARY} 45 | NAMES "g2o_${MYLIBRARYNAME}" 46 | PATHS 47 | ${G2O_ROOT}/lib/Release 48 | ${G2O_ROOT}/lib 49 | $ENV{G2O_ROOT}/lib/Release 50 | $ENV{G2O_ROOT}/lib 51 | NO_DEFAULT_PATH 52 | ) 53 | 54 | FIND_LIBRARY(${MYLIBRARY} 55 | NAMES "g2o_${MYLIBRARYNAME}" 56 | PATHS 57 | ~/Library/Frameworks 58 | /Library/Frameworks 59 | /usr/local/lib 60 | /usr/local/lib64 61 | /usr/lib 62 | /usr/lib64 63 | /opt/local/lib 64 | /sw/local/lib 65 | /sw/lib 66 | NO_DEFAULT_PATH 67 | ) 68 | 69 | IF(NOT ${MYLIBRARY}_DEBUG) 70 | IF(MYLIBRARY) 71 | SET(${MYLIBRARY}_DEBUG ${MYLIBRARY}) 72 | ENDIF(MYLIBRARY) 73 | ENDIF( NOT ${MYLIBRARY}_DEBUG) 74 | 75 | ENDMACRO(FIND_G2O_LIBRARY LIBRARY LIBRARYNAME) 76 | 77 | # Find the core elements 78 | FIND_G2O_LIBRARY(G2O_STUFF_LIBRARY stuff) 79 | FIND_G2O_LIBRARY(G2O_CORE_LIBRARY core) 80 | 81 | # Find the CLI library 82 | FIND_G2O_LIBRARY(G2O_CLI_LIBRARY cli) 83 | 84 | # Find the pluggable solvers 85 | FIND_G2O_LIBRARY(G2O_SOLVER_CHOLMOD solver_cholmod) 86 | FIND_G2O_LIBRARY(G2O_SOLVER_CSPARSE solver_csparse) 87 | FIND_G2O_LIBRARY(G2O_SOLVER_CSPARSE_EXTENSION csparse_extension) 88 | FIND_G2O_LIBRARY(G2O_SOLVER_DENSE solver_dense) 89 | FIND_G2O_LIBRARY(G2O_SOLVER_PCG solver_pcg) 90 | FIND_G2O_LIBRARY(G2O_SOLVER_SLAM2D_LINEAR solver_slam2d_linear) 91 | FIND_G2O_LIBRARY(G2O_SOLVER_STRUCTURE_ONLY solver_structure_only) 92 | FIND_G2O_LIBRARY(G2O_SOLVER_EIGEN solver_eigen) 93 | 94 | # Find the predefined types 95 | FIND_G2O_LIBRARY(G2O_TYPES_DATA types_data) 96 | FIND_G2O_LIBRARY(G2O_TYPES_ICP types_icp) 97 | FIND_G2O_LIBRARY(G2O_TYPES_SBA types_sba) 98 | FIND_G2O_LIBRARY(G2O_TYPES_SCLAM2D types_sclam2d) 99 | FIND_G2O_LIBRARY(G2O_TYPES_SIM3 types_sim3) 100 | FIND_G2O_LIBRARY(G2O_TYPES_SLAM2D types_slam2d) 101 | FIND_G2O_LIBRARY(G2O_TYPES_SLAM3D types_slam3d) 102 | 103 | # G2O solvers declared found if we found at least one solver 104 | SET(G2O_SOLVERS_FOUND "NO") 105 | IF(G2O_SOLVER_CHOLMOD OR G2O_SOLVER_CSPARSE OR G2O_SOLVER_DENSE OR G2O_SOLVER_PCG OR G2O_SOLVER_SLAM2D_LINEAR OR G2O_SOLVER_STRUCTURE_ONLY OR G2O_SOLVER_EIGEN) 106 | SET(G2O_SOLVERS_FOUND "YES") 107 | ENDIF(G2O_SOLVER_CHOLMOD OR G2O_SOLVER_CSPARSE OR G2O_SOLVER_DENSE OR G2O_SOLVER_PCG OR G2O_SOLVER_SLAM2D_LINEAR OR G2O_SOLVER_STRUCTURE_ONLY OR G2O_SOLVER_EIGEN) 108 | 109 | # G2O itself declared found if we found the core libraries and at least one solver 110 | SET(G2O_FOUND "NO") 111 | IF(G2O_STUFF_LIBRARY AND G2O_CORE_LIBRARY AND G2O_INCLUDE_DIR AND G2O_SOLVERS_FOUND) 112 | SET(G2O_FOUND "YES") 113 | ENDIF(G2O_STUFF_LIBRARY AND G2O_CORE_LIBRARY AND G2O_INCLUDE_DIR AND G2O_SOLVERS_FOUND) 114 | -------------------------------------------------------------------------------- /include/fast_gicp/cuda/fast_vgicp_cuda.cuh: -------------------------------------------------------------------------------- 1 | #ifndef FAST_GICP_FAST_VGICP_CUDA_CORE_CUH 2 | #define FAST_GICP_FAST_VGICP_CUDA_CORE_CUH 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | #include 10 | 11 | namespace thrust { 12 | 13 | template 14 | class pair; 15 | 16 | template 17 | class device_allocator; 18 | 19 | template 20 | class device_vector; 21 | } // namespace thrust 22 | 23 | namespace fast_gicp { 24 | namespace cuda { 25 | 26 | class GaussianVoxelMap; 27 | 28 | class FastVGICPCudaCore { 29 | public: 30 | using Points = thrust::device_vector>; 31 | using Indices = thrust::device_vector>; 32 | using Matrices = thrust::device_vector>; 33 | using Correspondences = thrust::device_vector, thrust::device_allocator>>; 34 | using VoxelCoordinates = thrust::device_vector>; 35 | 36 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 37 | FastVGICPCudaCore(); 38 | ~FastVGICPCudaCore(); 39 | 40 | void set_resolution(double resolution); 41 | void set_kernel_params(double kernel_width, double kernel_max_dist); 42 | void set_neighbor_search_method(fast_gicp::NeighborSearchMethod method, double radius); 43 | 44 | void swap_source_and_target(); 45 | void set_source_cloud(const std::vector>& cloud); 46 | void set_target_cloud(const std::vector>& cloud); 47 | 48 | void set_source_neighbors(int k, const std::vector& neighbors); 49 | void set_target_neighbors(int k, const std::vector& neighbors); 50 | void find_source_neighbors(int k); 51 | void find_target_neighbors(int k); 52 | 53 | void calculate_source_covariances(RegularizationMethod method); 54 | void calculate_target_covariances(RegularizationMethod method); 55 | 56 | void calculate_source_covariances_rbf(RegularizationMethod method); 57 | void calculate_target_covariances_rbf(RegularizationMethod method); 58 | 59 | void get_source_covariances(std::vector>& covs) const; 60 | void get_target_covariances(std::vector>& covs) const; 61 | 62 | void get_voxel_num_points(std::vector& num_points) const; 63 | void get_voxel_means(std::vector>& means) const; 64 | void get_voxel_covs(std::vector>& covs) const; 65 | void get_voxel_correspondences(std::vector>& correspondences) const; 66 | 67 | void create_target_voxelmap(); 68 | 69 | void update_correspondences(const Eigen::Isometry3d& trans); 70 | 71 | double compute_error(const Eigen::Isometry3d& trans, Eigen::Matrix* H, Eigen::Matrix* b) const; 72 | 73 | public: 74 | double resolution; 75 | double kernel_width; 76 | double kernel_max_dist; 77 | std::unique_ptr offsets; 78 | 79 | std::unique_ptr source_points; 80 | std::unique_ptr target_points; 81 | 82 | std::unique_ptr source_neighbors; 83 | std::unique_ptr target_neighbors; 84 | 85 | std::unique_ptr source_covariances; 86 | std::unique_ptr target_covariances; 87 | 88 | std::unique_ptr voxelmap; 89 | 90 | Eigen::Isometry3f linearized_x; 91 | std::unique_ptr voxel_correspondences; 92 | }; 93 | 94 | } // namespace cuda 95 | } // namespace fast_gicp 96 | 97 | #endif -------------------------------------------------------------------------------- /include/fast_gicp/gicp/lsq_registration.hpp: -------------------------------------------------------------------------------- 1 | #ifndef FAST_GICP_LSQ_REGISTRATION_HPP 2 | #define FAST_GICP_LSQ_REGISTRATION_HPP 3 | 4 | #include 5 | #include 6 | 7 | #include 8 | #include 9 | #include 10 | 11 | namespace fast_gicp { 12 | 13 | enum class LSQ_OPTIMIZER_TYPE { GaussNewton, LevenbergMarquardt, LevenbergMarquardtNew, CeresDogleg }; 14 | 15 | template 16 | class LsqRegistration : public pcl::Registration { 17 | public: 18 | using Scalar = float; 19 | using Matrix4 = typename pcl::Registration::Matrix4; 20 | 21 | using PointCloudSource = typename pcl::Registration::PointCloudSource; 22 | using PointCloudSourcePtr = typename PointCloudSource::Ptr; 23 | using PointCloudSourceConstPtr = typename PointCloudSource::ConstPtr; 24 | 25 | using PointCloudTarget = typename pcl::Registration::PointCloudTarget; 26 | using PointCloudTargetPtr = typename PointCloudTarget::Ptr; 27 | using PointCloudTargetConstPtr = typename PointCloudTarget::ConstPtr; 28 | 29 | #if PCL_VERSION >= PCL_VERSION_CALC(1, 10, 0) 30 | using Ptr = pcl::shared_ptr>; 31 | using ConstPtr = pcl::shared_ptr>; 32 | #else 33 | using Ptr = boost::shared_ptr>; 34 | using ConstPtr = boost::shared_ptr>; 35 | #endif 36 | 37 | protected: 38 | using pcl::Registration::input_; 39 | using pcl::Registration::nr_iterations_; 40 | using pcl::Registration::max_iterations_; 41 | using pcl::Registration::final_transformation_; 42 | using pcl::Registration::transformation_epsilon_; 43 | using pcl::Registration::converged_; 44 | 45 | public: 46 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 47 | 48 | LsqRegistration(); 49 | virtual ~LsqRegistration(); 50 | 51 | void setRotationEpsilon(double eps); 52 | void setInitialLambdaFactor(double init_lambda_factor); 53 | void setDebugPrint(bool lm_debug_print); 54 | void setLSQType(LSQ_OPTIMIZER_TYPE lsq_type); 55 | 56 | const Eigen::Matrix& getFinalHessian() const; 57 | 58 | double evaluateCost(const Eigen::Matrix4f& relative_pose, Eigen::Matrix* H = nullptr, Eigen::Matrix* b = nullptr); 59 | 60 | virtual void swapSourceAndTarget() {} 61 | virtual void clearSource() {} 62 | virtual void clearTarget() {} 63 | 64 | protected: 65 | virtual void computeTransformation(PointCloudSource& output, const Matrix4& guess) override; 66 | 67 | bool is_converged(const Eigen::Isometry3d& delta) const; 68 | // linearize里会更新对应关系，并返回目标函数值 69 | virtual double linearize(const Eigen::Isometry3d& trans, Eigen::Matrix* H = nullptr, Eigen::Matrix* b = nullptr) = 0; 70 | virtual double compute_error(const Eigen::Isometry3d& trans) = 0; 71 | // 会被step_ceres调用 72 | virtual bool solve_ceres(Eigen::Isometry3d& trans,Eigen::Isometry3d& delta); 73 | 74 | bool step_optimize(Eigen::Isometry3d& x0, Eigen::Isometry3d& delta); 75 | bool step_gn(Eigen::Isometry3d& x0, Eigen::Isometry3d& delta); 76 | bool step_lm(Eigen::Isometry3d& x0, Eigen::Isometry3d& delta); 77 | bool step_lm_new(Eigen::Isometry3d& x0, Eigen::Isometry3d& delta); 78 | bool step_ceres(Eigen::Isometry3d& x0,Eigen::Isometry3d& delta); 79 | 80 | protected: 81 | double rotation_epsilon_; 82 | 83 | LSQ_OPTIMIZER_TYPE lsq_optimizer_type_; 84 | int lm_max_iterations_; 85 | double lm_init_lambda_factor_;// tao in paper 86 | double lm_lambda_; 87 | bool lm_debug_print_; 88 | double lm_sigma1_,lm_sigma2_; 89 | 90 | Eigen::Matrix final_hessian_; 91 | }; 92 | } // namespace fast_gicp 93 | 94 | #endif -------------------------------------------------------------------------------- /cmake_modules/FindEIGEN3.cmake: -------------------------------------------------------------------------------- 1 | # - Try to find Eigen3 lib 2 | # 3 | # This module supports requiring a minimum version, e.g. you can do 4 | # find_package(Eigen3 3.1.2) 5 | # to require version 3.1.2 or newer of Eigen3. 6 | # 7 | # Once done this will define 8 | # 9 | # EIGEN3_FOUND - system has eigen lib with correct version 10 | # EIGEN3_INCLUDE_DIR - the eigen include directory 11 | # EIGEN3_VERSION - eigen version 12 | # 13 | # This module reads hints about search locations from 14 | # the following enviroment variables: 15 | # 16 | # EIGEN3_ROOT 17 | # EIGEN3_ROOT_DIR 18 | 19 | # Copyright (c) 2006, 2007 Montel Laurent, 20 | # Copyright (c) 2008, 2009 Gael Guennebaud, 21 | # Copyright (c) 2009 Benoit Jacob 22 | # Redistribution and use is allowed according to the terms of the 2-clause BSD license. 23 | 24 | if(NOT Eigen3_FIND_VERSION) 25 | if(NOT Eigen3_FIND_VERSION_MAJOR) 26 | set(Eigen3_FIND_VERSION_MAJOR 2) 27 | endif(NOT Eigen3_FIND_VERSION_MAJOR) 28 | if(NOT Eigen3_FIND_VERSION_MINOR) 29 | set(Eigen3_FIND_VERSION_MINOR 91) 30 | endif(NOT Eigen3_FIND_VERSION_MINOR) 31 | if(NOT Eigen3_FIND_VERSION_PATCH) 32 | set(Eigen3_FIND_VERSION_PATCH 0) 33 | endif(NOT Eigen3_FIND_VERSION_PATCH) 34 | 35 | set(Eigen3_FIND_VERSION "${Eigen3_FIND_VERSION_MAJOR}.${Eigen3_FIND_VERSION_MINOR}.${Eigen3_FIND_VERSION_PATCH}") 36 | endif(NOT Eigen3_FIND_VERSION) 37 | 38 | macro(_eigen3_check_version) 39 | file(READ "${EIGEN3_INCLUDE_DIR}/Eigen/src/Core/util/Macros.h" _eigen3_version_header) 40 | 41 | string(REGEX MATCH "define[ \t]+EIGEN_WORLD_VERSION[ \t]+([0-9]+)" _eigen3_world_version_match "${_eigen3_version_header}") 42 | set(EIGEN3_WORLD_VERSION "${CMAKE_MATCH_1}") 43 | string(REGEX MATCH "define[ \t]+EIGEN_MAJOR_VERSION[ \t]+([0-9]+)" _eigen3_major_version_match "${_eigen3_version_header}") 44 | set(EIGEN3_MAJOR_VERSION "${CMAKE_MATCH_1}") 45 | string(REGEX MATCH "define[ \t]+EIGEN_MINOR_VERSION[ \t]+([0-9]+)" _eigen3_minor_version_match "${_eigen3_version_header}") 46 | set(EIGEN3_MINOR_VERSION "${CMAKE_MATCH_1}") 47 | 48 | set(EIGEN3_VERSION ${EIGEN3_WORLD_VERSION}.${EIGEN3_MAJOR_VERSION}.${EIGEN3_MINOR_VERSION}) 49 | if(${EIGEN3_VERSION} VERSION_LESS ${Eigen3_FIND_VERSION}) 50 | set(EIGEN3_VERSION_OK FALSE) 51 | else(${EIGEN3_VERSION} VERSION_LESS ${Eigen3_FIND_VERSION}) 52 | set(EIGEN3_VERSION_OK TRUE) 53 | endif(${EIGEN3_VERSION} VERSION_LESS ${Eigen3_FIND_VERSION}) 54 | 55 | if(NOT EIGEN3_VERSION_OK) 56 | 57 | message(STATUS "Eigen3 version ${EIGEN3_VERSION} found in ${EIGEN3_INCLUDE_DIR}, " 58 | "but at least version ${Eigen3_FIND_VERSION} is required") 59 | endif(NOT EIGEN3_VERSION_OK) 60 | endmacro(_eigen3_check_version) 61 | 62 | if (EIGEN3_INCLUDE_DIR) 63 | 64 | # in cache already 65 | _eigen3_check_version() 66 | set(EIGEN3_FOUND ${EIGEN3_VERSION_OK}) 67 | 68 | endif (EIGEN3_INCLUDE_DIR) 69 | 70 | if(NOT EIGEN3_INCLUDE_DIR) 71 | # search first if an Eigen3Config.cmake is available in the system, 72 | # if successful this would set EIGEN3_INCLUDE_DIR and the rest of 73 | # the script will work as usual 74 | find_package(Eigen3 ${Eigen3_FIND_VERSION} NO_MODULE QUIET 75 | PATHS 76 | /usr/lib/${CMAKE_LIBRARY_ARCHITECTURE}/cmake 77 | ) 78 | endif(NOT EIGEN3_INCLUDE_DIR) 79 | 80 | if(NOT EIGEN3_INCLUDE_DIR) 81 | find_path(EIGEN3_INCLUDE_DIR NAMES signature_of_eigen3_matrix_library 82 | HINTS 83 | ENV EIGEN3_ROOT_DIR 84 | PATHS 85 | ${CMAKE_INSTALL_PREFIX}/include 86 | ${KDE4_INCLUDE_DIR} 87 | PATH_SUFFIXES eigen3 eigen 88 | ) 89 | endif(NOT EIGEN3_INCLUDE_DIR) 90 | 91 | if(EIGEN3_INCLUDE_DIR) 92 | _eigen3_check_version() 93 | endif(EIGEN3_INCLUDE_DIR) 94 | 95 | if (NOT TARGET Eigen3::Eigen AND EIGEN3_VERSION_OK AND EIGEN3_INCLUDE_DIR) 96 | add_library(Eigen3::Eigen INTERFACE IMPORTED) 97 | set_target_properties(Eigen3::Eigen PROPERTIES 98 | INTERFACE_INCLUDE_DIRECTORIES "${EIGEN3_INCLUDE_DIR}") 99 | endif () 100 | 101 | 102 | include(FindPackageHandleStandardArgs) 103 | find_package_handle_standard_args(Eigen3 DEFAULT_MSG EIGEN3_INCLUDE_DIR EIGEN3_VERSION_OK) 104 | 105 | mark_as_advanced(EIGEN3_INCLUDE_DIR) -------------------------------------------------------------------------------- /src/fast_gicp/cuda/find_voxel_correspondences.cu: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | 11 | namespace fast_gicp { 12 | namespace cuda { 13 | 14 | namespace { 15 | 16 | struct find_voxel_correspondences_kernel { 17 | find_voxel_correspondences_kernel( 18 | const GaussianVoxelMap& voxelmap, 19 | const thrust::device_vector& src_points, 20 | const thrust::device_ptr& trans_ptr, 21 | thrust::device_ptr offset_ptr) 22 | : trans_ptr(trans_ptr), 23 | offset_ptr(offset_ptr), 24 | src_points_ptr(src_points.data()), 25 | voxelmap_info_ptr(voxelmap.voxelmap_info_ptr.data()), 26 | buckets_ptr(voxelmap.buckets.data()), 27 | voxel_num_points_ptr(voxelmap.num_points.data()), 28 | voxel_means_ptr(voxelmap.voxel_means.data()), 29 | voxel_covs_ptr(voxelmap.voxel_covs.data()) {} 30 | 31 | // lookup voxel 32 | __host__ __device__ int lookup_voxel(const Eigen::Vector3f& x) const { 33 | const VoxelMapInfo& voxelmap_info = *thrust::raw_pointer_cast(voxelmap_info_ptr); 34 | const auto& offset = *thrust::raw_pointer_cast(offset_ptr); 35 | 36 | Eigen::Vector3i coord = calc_voxel_coord(x, voxelmap_info.voxel_resolution) + offset; 37 | uint64_t hash = vector3i_hash(coord); 38 | 39 | for(int i = 0; i < voxelmap_info.max_bucket_scan_count; i++) { 40 | uint64_t bucket_index = (hash + i) % voxelmap_info.num_buckets; 41 | const thrust::pair& bucket = thrust::raw_pointer_cast(buckets_ptr)[bucket_index]; 42 | 43 | if(bucket.second < 0) { 44 | return -1; 45 | } 46 | 47 | if(bucket.first == coord) { 48 | return bucket.second; 49 | } 50 | } 51 | 52 | return -1; 53 | } 54 | 55 | __host__ __device__ thrust::pair operator()(int src_index) const { 56 | const auto& trans = *thrust::raw_pointer_cast(trans_ptr); 57 | 58 | const auto& pt = thrust::raw_pointer_cast(src_points_ptr)[src_index]; 59 | return thrust::make_pair(src_index, lookup_voxel(trans.linear() * pt + trans.translation())); 60 | } 61 | 62 | const thrust::device_ptr trans_ptr; 63 | const thrust::device_ptr offset_ptr; 64 | 65 | const thrust::device_ptr src_points_ptr; 66 | 67 | thrust::device_ptr voxelmap_info_ptr; 68 | thrust::device_ptr> buckets_ptr; 69 | 70 | thrust::device_ptr voxel_num_points_ptr; 71 | thrust::device_ptr voxel_means_ptr; 72 | thrust::device_ptr voxel_covs_ptr; 73 | }; 74 | 75 | struct invalid_correspondence_kernel { 76 | __host__ __device__ bool operator() (const thrust::pair& corr) const { 77 | return corr.first < 0 || corr.second < 0; 78 | } 79 | }; 80 | 81 | } // namespace 82 | 83 | void find_voxel_correspondences( 84 | const thrust::device_vector& src_points, 85 | const GaussianVoxelMap& voxelmap, 86 | const thrust::device_ptr& x_ptr, 87 | const thrust::device_vector& offsets, 88 | thrust::device_vector>& correspondences) { 89 | std::vector events(offsets.size()); 90 | 91 | // find correspondences 92 | correspondences.resize(src_points.size() * offsets.size()); 93 | for(int i=0; i(0), 96 | thrust::counting_iterator(src_points.size()), 97 | correspondences.begin() + src_points.size() * i, 98 | find_voxel_correspondences_kernel(voxelmap, src_points, x_ptr, offsets.data() + i)); 99 | 100 | events[i] = std::move(event); 101 | } 102 | 103 | // synchronize 104 | for(auto& event: events) { 105 | event.wait(); 106 | } 107 | 108 | // remove invlid correspondences 109 | auto remove_loc = thrust::remove_if(correspondences.begin(), correspondences.end(), invalid_correspondence_kernel()); 110 | correspondences.erase(remove_loc, correspondences.end()); 111 | } 112 | 113 | } // namespace cuda 114 | } // namespace fast_gicp 115 | -------------------------------------------------------------------------------- /src/fast_gicp/cuda/brute_force_knn.cu: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | #include 10 | #include 11 | 12 | namespace fast_gicp { 13 | namespace cuda { 14 | 15 | namespace { 16 | struct neighborsearch_kernel { 17 | neighborsearch_kernel(int k, const thrust::device_vector& target, thrust::device_vector>& k_neighbors) 18 | : k(k), num_target_points(target.size()), target_points_ptr(target.data()), k_neighbors_ptr(k_neighbors.data()) {} 19 | 20 | template 21 | __host__ __device__ void operator()(const Tuple& idx_x) const { 22 | // threadIdx doesn't work because thrust split for_each in two loops 23 | int idx = thrust::get<0>(idx_x); 24 | const Eigen::Vector3f& x = thrust::get<1>(idx_x); 25 | 26 | // target points buffer & nn output buffer 27 | const Eigen::Vector3f* pts = thrust::raw_pointer_cast(target_points_ptr); 28 | thrust::pair* k_neighbors = thrust::raw_pointer_cast(k_neighbors_ptr) + idx * k; 29 | 30 | // priority queue 31 | struct compare_type { 32 | bool operator()(const thrust::pair& lhs, const thrust::pair& rhs) { 33 | return lhs.first < rhs.first; 34 | } 35 | }; 36 | 37 | typedef nvbio::vector_view*> vector_type; 38 | typedef nvbio::priority_queue, vector_type, compare_type> queue_type; 39 | queue_type queue(vector_type(0, k_neighbors - 1)); 40 | 41 | for(int i = 0; i < k; i++) { 42 | float sq_dist = (pts[i] - x).squaredNorm(); 43 | queue.push(thrust::make_pair(sq_dist, i)); 44 | } 45 | 46 | for(int i = k; i < num_target_points; i++) { 47 | float sq_dist = (pts[i] - x).squaredNorm(); 48 | if(sq_dist < queue.top().first) { 49 | queue.pop(); 50 | queue.push(thrust::make_pair(sq_dist, i)); 51 | } 52 | } 53 | } 54 | 55 | const int k; 56 | const int num_target_points; 57 | thrust::device_ptr target_points_ptr; 58 | 59 | thrust::device_ptr> k_neighbors_ptr; 60 | }; 61 | 62 | struct sorting_kernel { 63 | sorting_kernel(int k, thrust::device_vector>& k_neighbors) : k(k), k_neighbors_ptr(k_neighbors.data()) {} 64 | 65 | __host__ __device__ void operator()(int idx) const { 66 | // target points buffer & nn output buffer 67 | thrust::pair* k_neighbors = thrust::raw_pointer_cast(k_neighbors_ptr) + idx * k; 68 | 69 | // priority queue 70 | struct compare_type { 71 | bool operator()(const thrust::pair& lhs, const thrust::pair& rhs) { 72 | return lhs.first < rhs.first; 73 | } 74 | }; 75 | 76 | typedef nvbio::vector_view*> vector_type; 77 | typedef nvbio::priority_queue, vector_type, compare_type> queue_type; 78 | queue_type queue(vector_type(k, k_neighbors - 1)); 79 | queue.m_size = k; 80 | 81 | for(int i = 0; i < k; i++) { 82 | thrust::pair poped = queue.top(); 83 | queue.pop(); 84 | 85 | k_neighbors[k - i - 1] = poped; 86 | } 87 | } 88 | 89 | const int k; 90 | thrust::device_ptr> k_neighbors_ptr; 91 | }; 92 | } 93 | 94 | void brute_force_knn_search(const thrust::device_vector& source, const thrust::device_vector& target, int k, thrust::device_vector>& k_neighbors, bool do_sort=false) { 95 | thrust::device_vector d_indices(source.size()); 96 | thrust::sequence(d_indices.begin(), d_indices.end()); 97 | 98 | auto first = thrust::make_zip_iterator(thrust::make_tuple(d_indices.begin(), source.begin())); 99 | auto last = thrust::make_zip_iterator(thrust::make_tuple(d_indices.end(), source.end())); 100 | 101 | // nvbio::priority_queue requires (k + 1) working space 102 | k_neighbors.resize(source.size() * k, thrust::make_pair(-1.0f, -1)); 103 | thrust::for_each(first, last, neighborsearch_kernel(k, target, k_neighbors)); 104 | 105 | if(do_sort) { 106 | thrust::for_each(d_indices.begin(), d_indices.end(), sorting_kernel(k, k_neighbors)); 107 | } 108 | } 109 | 110 | } 111 | } // namespace fast_gicp 112 | -------------------------------------------------------------------------------- /include/fast_gicp/gicp/fast_gicp.hpp: -------------------------------------------------------------------------------- 1 | #ifndef FAST_GICP_FAST_GICP_HPP 2 | #define FAST_GICP_FAST_GICP_HPP 3 | 4 | #include 5 | #include 6 | 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | 14 | namespace fast_gicp { 15 | 16 | /** 17 | * @brief Fast GICP algorithm optimized for multi threading with OpenMP 18 | */ 19 | template 20 | class FastGICP : public LsqRegistration { 21 | public: 22 | using Scalar = float; 23 | using Matrix4 = typename pcl::Registration::Matrix4; 24 | 25 | using PointCloudSource = typename pcl::Registration::PointCloudSource; 26 | using PointCloudSourcePtr = typename PointCloudSource::Ptr; 27 | using PointCloudSourceConstPtr = typename PointCloudSource::ConstPtr; 28 | 29 | using PointCloudTarget = typename pcl::Registration::PointCloudTarget; 30 | using PointCloudTargetPtr = typename PointCloudTarget::Ptr; 31 | using PointCloudTargetConstPtr = typename PointCloudTarget::ConstPtr; 32 | 33 | #if PCL_VERSION >= PCL_VERSION_CALC(1, 10, 0) 34 | using Ptr = pcl::shared_ptr>; 35 | using ConstPtr = pcl::shared_ptr>; 36 | #else 37 | using Ptr = boost::shared_ptr>; 38 | using ConstPtr = boost::shared_ptr>; 39 | #endif 40 | 41 | protected: 42 | using pcl::Registration::reg_name_; 43 | using pcl::Registration::input_; 44 | using pcl::Registration::target_; 45 | using pcl::Registration::corr_dist_threshold_; 46 | 47 | public: 48 | FastGICP(); 49 | virtual ~FastGICP() override; 50 | 51 | void setNumThreads(int n); 52 | void setCorrespondenceRandomness(int k); 53 | void setRegularizationMethod(RegularizationMethod method); 54 | void setLocalParameterization(bool isSE3){isSE3_ = isSE3;}; 55 | 56 | virtual void swapSourceAndTarget() override; 57 | virtual void clearSource() override; 58 | virtual void clearTarget() override; 59 | 60 | virtual void setInputSource(const PointCloudSourceConstPtr& cloud) override; 61 | virtual void setSourceCovariances(const std::vector>& covs); 62 | virtual void setInputTarget(const PointCloudTargetConstPtr& cloud) override; 63 | virtual void setTargetCovariances(const std::vector>& covs); 64 | 65 | const std::vector>& getSourceCovariances() const { 66 | return source_covs_; 67 | } 68 | 69 | const std::vector>& getTargetCovariances() const { 70 | return target_covs_; 71 | } 72 | 73 | protected: 74 | virtual void computeTransformation(PointCloudSource& output, const Matrix4& guess) override; 75 | 76 | virtual void update_correspondences(const Eigen::Isometry3d& trans); 77 | 78 | virtual double linearize(const Eigen::Isometry3d& trans, Eigen::Matrix* H, Eigen::Matrix* b) override; 79 | 80 | virtual double compute_error(const Eigen::Isometry3d& trans) override; 81 | 82 | virtual bool solve_ceres(Eigen::Isometry3d& trans, Eigen::Isometry3d& delta) override; 83 | template 84 | bool calculate_covariances(const typename pcl::PointCloud::ConstPtr& cloud, pcl::search::KdTree& kdtree, std::vector>& covariances); 85 | 86 | protected: 87 | int num_threads_; 88 | int k_correspondences_; 89 | bool isSE3_; 90 | 91 | RegularizationMethod regularization_method_; 92 | 93 | std::shared_ptr> source_kdtree_; 94 | std::shared_ptr> target_kdtree_; 95 | 96 | std::vector> source_covs_; 97 | std::vector> target_covs_; 98 | 99 | std::vector> mahalanobis_; 100 | 101 | std::vector correspondences_; 102 | std::vector sq_distances_; 103 | }; 104 | } // namespace fast_gicp 105 | 106 | #endif -------------------------------------------------------------------------------- /include/fast_gicp/gicp/impl/fast_gicp_st_impl.hpp: -------------------------------------------------------------------------------- 1 | #ifndef FAST_GICP_FAST_GICP_ST_IMPL_HPP 2 | #define FAST_GICP_FAST_GICP_ST_IMPL_HPP 3 | 4 | #include 5 | #include 6 | 7 | namespace fast_gicp { 8 | 9 | template 10 | FastGICPSingleThread::FastGICPSingleThread() : FastGICP() { 11 | this->reg_name_ = "FastGICPSingleThread"; 12 | this->num_threads_ = 1; 13 | } 14 | 15 | template 16 | FastGICPSingleThread::~FastGICPSingleThread() {} 17 | 18 | template 19 | void FastGICPSingleThread::computeTransformation(PointCloudSource& output, const Matrix4& guess) { 20 | anchors_.clear(); 21 | FastGICP::computeTransformation(output, guess); 22 | } 23 | 24 | template 25 | void FastGICPSingleThread::update_correspondences(const Eigen::Isometry3d& x) { 26 | assert(source_covs_.size() == input_->size()); 27 | assert(target_covs_.size() == target_->size()); 28 | 29 | Eigen::Isometry3f trans = x.template cast(); 30 | 31 | bool is_first = anchors_.empty(); 32 | 33 | correspondences_.resize(input_->size()); 34 | sq_distances_.resize(input_->size()); 35 | second_sq_distances_.resize(input_->size()); 36 | anchors_.resize(input_->size()); 37 | mahalanobis_.resize(input_->size()); 38 | 39 | std::vector k_indices; 40 | std::vector k_sq_dists; 41 | 42 | for (int i = 0; i < input_->size(); i++) { 43 | PointTarget pt; 44 | pt.getVector4fMap() = trans * input_->at(i).getVector4fMap(); 45 | 46 | if (!is_first) { 47 | double d = (pt.getVector4fMap() - anchors_[i]).norm(); 48 | double max_first = std::sqrt(sq_distances_[i]) + d; 49 | double min_second = std::sqrt(second_sq_distances_[i]) - d; 50 | 51 | if (max_first < min_second) { 52 | continue; 53 | } 54 | } 55 | 56 | target_kdtree_->nearestKSearch(pt, 2, k_indices, k_sq_dists); 57 | 58 | correspondences_[i] = k_sq_dists[0] < this->corr_dist_threshold_ * this->corr_dist_threshold_ ? k_indices[0] : -1; 59 | sq_distances_[i] = k_sq_dists[0]; 60 | second_sq_distances_[i] = k_sq_dists[1]; 61 | anchors_[i] = pt.getVector4fMap(); 62 | 63 | if (correspondences_[i] < 0) { 64 | continue; 65 | } 66 | 67 | const int target_index = correspondences_[i]; 68 | const auto& cov_A = source_covs_[i]; 69 | const auto& cov_B = target_covs_[target_index]; 70 | 71 | Eigen::Matrix4d RCR = cov_B + x.matrix() * cov_A * x.matrix().transpose(); 72 | RCR(3, 3) = 1.0; 73 | 74 | mahalanobis_[i] = RCR.inverse(); 75 | mahalanobis_[i](3, 3) = 0.0; 76 | } 77 | } 78 | 79 | template 80 | double FastGICPSingleThread::linearize(const Eigen::Isometry3d& trans, Eigen::Matrix* H, Eigen::Matrix* b) { 81 | update_correspondences(trans); 82 | if (H && b) { 83 | H->setZero(); 84 | b->setZero(); 85 | } 86 | 87 | double sum_errors = 0.0; 88 | for (int i = 0; i < input_->size(); i++) { 89 | int target_index = correspondences_[i]; 90 | if (target_index < 0) { 91 | continue; 92 | } 93 | 94 | const Eigen::Vector4d mean_A = input_->at(i).getVector4fMap().template cast(); 95 | const auto& cov_A = source_covs_[i]; 96 | 97 | const Eigen::Vector4d mean_B = target_->at(target_index).getVector4fMap().template cast(); 98 | const auto& cov_B = target_covs_[target_index]; 99 | 100 | const Eigen::Vector4d transed_mean_A = trans * mean_A; 101 | const Eigen::Vector4d error = mean_B - transed_mean_A; 102 | 103 | sum_errors += error.transpose() * mahalanobis_[i] * error; 104 | 105 | if (H == nullptr || b == nullptr) { 106 | continue; 107 | } 108 | 109 | Eigen::Matrix dtdx0 = Eigen::Matrix::Zero(); 110 | dtdx0.block<3, 3>(0, 0) = skewd(transed_mean_A.head<3>()); 111 | dtdx0.block<3, 3>(0, 3) = -Eigen::Matrix3d::Identity(); 112 | 113 | Eigen::Matrix jlossexp = dtdx0; 114 | 115 | (*H) += jlossexp.transpose() * mahalanobis_[i] * jlossexp; 116 | (*b) += jlossexp.transpose() * mahalanobis_[i] * error; 117 | } 118 | 119 | return sum_errors; 120 | } 121 | 122 | template 123 | double FastGICPSingleThread::compute_error(const Eigen::Isometry3d& trans) { 124 | return linearize(trans, nullptr, nullptr); 125 | } 126 | 127 | } // namespace fast_gicp 128 | 129 | #endif 130 | -------------------------------------------------------------------------------- /setup.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | import os 3 | import sys 4 | import glob 5 | import subprocess 6 | 7 | from setuptools import setup, Extension 8 | from setuptools.command.build_ext import build_ext 9 | 10 | # Convert distutils Windows platform specifiers to CMake -A arguments 11 | PLAT_TO_CMAKE = { 12 | "win32": "Win32", 13 | "win-amd64": "x64", 14 | "win-arm32": "ARM", 15 | "win-arm64": "ARM64", 16 | } 17 | 18 | 19 | # A CMakeExtension needs a sourcedir instead of a file list. 20 | # The name must be the _single_ output extension from the CMake build. 21 | # If you need multiple extensions, see scikit-build. 22 | class CMakeExtension(Extension): 23 | def __init__(self, name, sourcedir=""): 24 | Extension.__init__(self, name, sources=[]) 25 | self.sourcedir = os.path.abspath(sourcedir) 26 | 27 | 28 | class CMakeBuild(build_ext): 29 | def build_extension(self, ext): 30 | extdir = os.path.abspath(os.path.dirname(self.get_ext_fullpath(ext.name))) 31 | 32 | # required for auto-detection of auxiliary "native" libs 33 | if not extdir.endswith(os.path.sep): 34 | extdir += os.path.sep 35 | 36 | # cfg = "Debug" if self.debug else "Release" 37 | cfg = "Release" 38 | 39 | # CMake lets you override the generator - we need to check this. 40 | # Can be set with Conda-Build, for example. 41 | cmake_generator = os.environ.get("CMAKE_GENERATOR", "") 42 | 43 | # Set Python_EXECUTABLE instead if you use PYBIND11_FINDPYTHON 44 | # EXAMPLE_VERSION_INFO shows you how to pass a value into the C++ code 45 | # from Python. 46 | cmake_args = [ 47 | "-DCMAKE_LIBRARY_OUTPUT_DIRECTORY={}".format(extdir), 48 | "-DPYTHON_EXECUTABLE={}".format(sys.executable), 49 | "-DEXAMPLE_VERSION_INFO={}".format(self.distribution.get_version()), 50 | "-DCMAKE_BUILD_TYPE={}".format(cfg), # not used on MSVC, but no harm, 51 | # "-DBUILD_VGICP_CUDA=ON", 52 | "-DBUILD_PYTHON_BINDINGS=ON", 53 | ] 54 | build_args = [] 55 | 56 | if self.compiler.compiler_type != "msvc": 57 | # Using Ninja-build since it a) is available as a wheel and b) 58 | # multithreads automatically. MSVC would require all variables be 59 | # exported for Ninja to pick it up, which is a little tricky to do. 60 | # Users can override the generator with CMAKE_GENERATOR in CMake 61 | # 3.15+. 62 | if not cmake_generator: 63 | pass 64 | # cmake_args += ["-GNinja"] 65 | 66 | else: 67 | # Single config generators are handled "normally" 68 | single_config = any(x in cmake_generator for x in {"NMake", "Ninja"}) 69 | 70 | # CMake allows an arch-in-generator style for backward compatibility 71 | contains_arch = any(x in cmake_generator for x in {"ARM", "Win64"}) 72 | 73 | # Specify the arch if using MSVC generator, but only if it doesn't 74 | # contain a backward-compatibility arch spec already in the 75 | # generator name. 76 | if not single_config and not contains_arch: 77 | cmake_args += ["-A", PLAT_TO_CMAKE[self.plat_name]] 78 | 79 | # Multi-config generators have a different way to specify configs 80 | if not single_config: 81 | cmake_args += [ 82 | "-DCMAKE_LIBRARY_OUTPUT_DIRECTORY_{}={}".format(cfg.upper(), extdir) 83 | ] 84 | build_args += ["--config", cfg] 85 | 86 | # Set CMAKE_BUILD_PARALLEL_LEVEL to control the parallel build level 87 | # across all generators. 88 | if "CMAKE_BUILD_PARALLEL_LEVEL" not in os.environ: 89 | # self.parallel is a Python 3 only way to set parallel jobs by hand 90 | # using -j in the build_ext call, not supported by pip or PyPA-build. 91 | if hasattr(self, "parallel") and self.parallel: 92 | # CMake 3.12+ only. 93 | build_args += ["-j{}".format(self.parallel)] 94 | 95 | if not os.path.exists(self.build_temp): 96 | os.makedirs(self.build_temp) 97 | 98 | subprocess.check_call( 99 | ["cmake", ext.sourcedir] + cmake_args, cwd=self.build_temp 100 | ) 101 | subprocess.check_call( 102 | ["cmake", "--build", "."] + build_args, cwd=self.build_temp 103 | ) 104 | 105 | # The information here can also be placed in setup.cfg - better separation of 106 | # logic and declaration, and simpler if you include description/version in a file. 107 | setup( 108 | name="pygicp", 109 | version="0.0.1", 110 | author="k.koide", 111 | author_email="k.koide@aist.go.jp", 112 | description="A collection of GICP-based point cloud registration algorithms", 113 | long_description="", 114 | ext_modules=[CMakeExtension("pygicp")], 115 | cmdclass={"build_ext": CMakeBuild}, 116 | zip_safe=False, 117 | ) 118 | -------------------------------------------------------------------------------- /src/fast_gicp/cuda/covariance_regularization.cu: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include 4 | #include 5 | 6 | #include 7 | #include 8 | 9 | namespace fast_gicp { 10 | namespace cuda { 11 | 12 | namespace { 13 | 14 | struct svd_kernel { 15 | __host__ __device__ thrust::tuple operator()(const thrust::tuple& input) const { 16 | const auto& mean = thrust::get<0>(input); 17 | const auto& cov = thrust::get<1>(input); 18 | 19 | Eigen::SelfAdjointEigenSolver eig; 20 | eig.computeDirect(cov); 21 | 22 | return thrust::make_tuple(mean, eig.eigenvalues(), eig.eigenvectors()); 23 | } 24 | }; 25 | 26 | struct eigenvalue_filter_kernel { 27 | __host__ __device__ bool operator()(const thrust::tuple& input) const { 28 | const auto& values = thrust::get<1>(input); 29 | return values[1] > values[2] * 0.1; 30 | } 31 | }; 32 | 33 | struct svd_reconstruction_kernel { 34 | __host__ __device__ thrust::tuple operator()(const thrust::tuple& input) const { 35 | const auto& mean = thrust::get<0>(input); 36 | const auto& values = thrust::get<1>(input); 37 | const auto& vecs = thrust::get<2>(input); 38 | 39 | Eigen::Matrix3f values_diag = Eigen::Vector3f(1e-3f, 1.0f, 1.0f).asDiagonal(); 40 | Eigen::Matrix3f vecs_inv = vecs.inverse(); 41 | return thrust::make_tuple(mean, (vecs * values_diag * vecs_inv).eval()); 42 | } 43 | }; 44 | 45 | struct covariance_regularization_svd { 46 | __host__ __device__ void operator()(Eigen::Matrix3f& cov) const { 47 | Eigen::SelfAdjointEigenSolver eig; 48 | eig.computeDirect(cov); 49 | 50 | // why this doen't work...??? 51 | // cov = eig.eigenvectors() * values.asDiagonal() * eig.eigenvectors().inverse(); 52 | Eigen::Matrix3f values = Eigen::Vector3f(1e-3, 1, 1).asDiagonal(); 53 | Eigen::Matrix3f v_inv = eig.eigenvectors().inverse(); 54 | cov = eig.eigenvectors() * values * v_inv; 55 | 56 | // JacobiSVD is not supported on CUDA 57 | // Eigen::JacobiSVD(cov, Eigen::ComputeFullU | Eigen::ComputeFullV); 58 | // Eigen::Vector3f values(1, 1, 1e-3); 59 | // cov = svd.matrixU() * values.asDiagonal() * svd.matrixV().transpose(); 60 | } 61 | }; 62 | 63 | struct covariance_regularization_frobenius { 64 | __host__ __device__ void operator()(Eigen::Matrix3f& cov) const { 65 | float lambda = 1e-3; 66 | Eigen::Matrix3f C = cov + lambda * Eigen::Matrix3f::Identity(); 67 | Eigen::Matrix3f C_inv = C.inverse(); 68 | Eigen::Matrix3f C_norm = (C_inv / C_inv.norm()).inverse(); 69 | cov = C_norm; 70 | } 71 | }; 72 | 73 | struct covariance_regularization_mineig { 74 | __host__ __device__ void operator()(Eigen::Matrix3f& cov) const { 75 | Eigen::SelfAdjointEigenSolver eig; 76 | eig.computeDirect(cov); 77 | 78 | Eigen::Vector3f values = eig.eigenvalues(); 79 | for (int i = 0; i < 3; i++) { 80 | values[i] = fmaxf(1e-3f, values[i]); 81 | } 82 | 83 | Eigen::Matrix3f v_diag = values.asDiagonal(); 84 | Eigen::Matrix3f v_inv = eig.eigenvectors().inverse(); 85 | cov = eig.eigenvectors() * v_diag * v_inv; 86 | } 87 | }; 88 | 89 | } // namespace 90 | 91 | void covariance_regularization(thrust::device_vector& means, thrust::device_vector& covs, RegularizationMethod method) { 92 | if (method == RegularizationMethod::PLANE) { 93 | // thrust::for_each(covs.begin(), covs.end(), covariance_regularization_svd()); 94 | // return; 95 | 96 | thrust::device_vector means_(means.size()); 97 | thrust::device_vector covs_(covs.size()); 98 | 99 | auto first = thrust::make_transform_iterator(thrust::make_zip_iterator(thrust::make_tuple(means.begin(), covs.begin())), svd_kernel()); 100 | auto last = thrust::make_transform_iterator(thrust::make_zip_iterator(thrust::make_tuple(means.end(), covs.end())), svd_kernel()); 101 | auto output_first = thrust::make_transform_output_iterator(thrust::make_zip_iterator(thrust::make_tuple(means_.begin(), covs_.begin())), svd_reconstruction_kernel()); 102 | auto output_last = thrust::copy_if(first, last, output_first, eigenvalue_filter_kernel()); 103 | 104 | int num_valid_points = output_last - output_first; 105 | means_.erase(means_.begin() + num_valid_points, means_.end()); 106 | covs_.erase(covs_.begin() + num_valid_points, covs_.end()); 107 | 108 | means = std::move(means_); 109 | covs = std::move(covs_); 110 | } else if (method == RegularizationMethod::FROBENIUS) { 111 | thrust::for_each(covs.begin(), covs.end(), covariance_regularization_frobenius()); 112 | } else if (method == RegularizationMethod::MIN_EIG) { 113 | thrust::for_each(covs.begin(), covs.end(), covariance_regularization_mineig()); 114 | } else { 115 | std::cerr << "unimplemented covariance regularization method was selected!!" << std::endl; 116 | } 117 | } 118 | 119 | } // namespace cuda 120 | } // namespace fast_gicp -------------------------------------------------------------------------------- /.clang-format: -------------------------------------------------------------------------------- 1 | --- 2 | Language: Cpp 3 | # BasedOnStyle: Google 4 | AccessModifierOffset: -2 5 | AlignAfterOpenBracket: AlwaysBreak 6 | AlignConsecutiveMacros: false 7 | AlignConsecutiveAssignments: false 8 | AlignConsecutiveDeclarations: false 9 | AlignEscapedNewlines: Left 10 | AlignOperands: true 11 | AlignTrailingComments: true 12 | AllowAllArgumentsOnNextLine: false 13 | AllowAllConstructorInitializersOnNextLine: false 14 | AllowAllParametersOfDeclarationOnNextLine: false 15 | AllowShortBlocksOnASingleLine: Never 16 | AllowShortCaseLabelsOnASingleLine: false 17 | AllowShortFunctionsOnASingleLine: Inline 18 | AllowShortLambdasOnASingleLine: All 19 | AllowShortIfStatementsOnASingleLine: WithoutElse 20 | AllowShortLoopsOnASingleLine: true 21 | AlwaysBreakAfterDefinitionReturnType: None 22 | AlwaysBreakAfterReturnType: None 23 | AlwaysBreakBeforeMultilineStrings: true 24 | AlwaysBreakTemplateDeclarations: Yes 25 | BinPackArguments: false 26 | BinPackParameters: false 27 | BraceWrapping: 28 | AfterCaseLabel: false 29 | AfterClass: false 30 | AfterControlStatement: false 31 | AfterEnum: false 32 | AfterFunction: false 33 | AfterNamespace: false 34 | AfterObjCDeclaration: false 35 | AfterStruct: false 36 | AfterUnion: false 37 | AfterExternBlock: false 38 | BeforeCatch: false 39 | BeforeElse: false 40 | IndentBraces: false 41 | SplitEmptyFunction: true 42 | SplitEmptyRecord: true 43 | SplitEmptyNamespace: true 44 | BreakBeforeBinaryOperators: None 45 | BreakBeforeBraces: Attach 46 | BreakBeforeInheritanceComma: false 47 | BreakInheritanceList: BeforeColon 48 | BreakBeforeTernaryOperators: true 49 | BreakConstructorInitializersBeforeComma: false 50 | BreakConstructorInitializers: BeforeColon 51 | BreakAfterJavaFieldAnnotations: false 52 | BreakStringLiterals: true 53 | ColumnLimit: 180 54 | CommentPragmas: '^ IWYU pragma:' 55 | CompactNamespaces: false 56 | ConstructorInitializerAllOnOneLineOrOnePerLine: true 57 | ConstructorInitializerIndentWidth: 0 58 | ContinuationIndentWidth: 2 59 | Cpp11BracedListStyle: true 60 | DeriveLineEnding: true 61 | DerivePointerAlignment: false 62 | DisableFormat: false 63 | ExperimentalAutoDetectBinPacking: false 64 | FixNamespaceComments: true 65 | ForEachMacros: 66 | - foreach 67 | - Q_FOREACH 68 | - BOOST_FOREACH 69 | IncludeBlocks: Regroup 70 | IncludeCategories: 71 | - Regex: '^' 72 | Priority: 2 73 | SortPriority: 0 74 | - Regex: '^<.*\.h>' 75 | Priority: 1 76 | SortPriority: 0 77 | - Regex: '^<.*' 78 | Priority: 2 79 | SortPriority: 0 80 | - Regex: '.*' 81 | Priority: 3 82 | SortPriority: 0 83 | IncludeIsMainRegex: '([-_](test|unittest))?$' 84 | IncludeIsMainSourceRegex: '' 85 | IndentCaseLabels: true 86 | IndentGotoLabels: true 87 | IndentPPDirectives: None 88 | IndentWidth: 2 89 | IndentWrappedFunctionNames: false 90 | JavaScriptQuotes: Leave 91 | JavaScriptWrapImports: true 92 | KeepEmptyLinesAtTheStartOfBlocks: false 93 | MacroBlockBegin: '' 94 | MacroBlockEnd: '' 95 | MaxEmptyLinesToKeep: 1 96 | NamespaceIndentation: None 97 | ObjCBinPackProtocolList: Never 98 | ObjCBlockIndentWidth: 2 99 | ObjCSpaceAfterProperty: false 100 | ObjCSpaceBeforeProtocolList: true 101 | PenaltyBreakAssignment: 2 102 | PenaltyBreakBeforeFirstCallParameter: 1 103 | PenaltyBreakComment: 300 104 | PenaltyBreakFirstLessLess: 120 105 | PenaltyBreakString: 1000 106 | PenaltyBreakTemplateDeclaration: 10 107 | PenaltyExcessCharacter: 1000000 108 | PenaltyReturnTypeOnItsOwnLine: 200 109 | PointerAlignment: Left 110 | RawStringFormats: 111 | - Language: Cpp 112 | Delimiters: 113 | - cc 114 | - CC 115 | - cpp 116 | - Cpp 117 | - CPP 118 | - 'c++' 119 | - 'C++' 120 | CanonicalDelimiter: '' 121 | BasedOnStyle: google 122 | - Language: TextProto 123 | Delimiters: 124 | - pb 125 | - PB 126 | - proto 127 | - PROTO 128 | EnclosingFunctions: 129 | - EqualsProto 130 | - EquivToProto 131 | - PARSE_PARTIAL_TEXT_PROTO 132 | - PARSE_TEST_PROTO 133 | - PARSE_TEXT_PROTO 134 | - ParseTextOrDie 135 | - ParseTextProtoOrDie 136 | CanonicalDelimiter: '' 137 | BasedOnStyle: google 138 | ReflowComments: true 139 | SortIncludes: false 140 | SortUsingDeclarations: true 141 | SpaceAfterCStyleCast: false 142 | SpaceAfterLogicalNot: false 143 | SpaceAfterTemplateKeyword: true 144 | SpaceBeforeAssignmentOperators: true 145 | SpaceBeforeCpp11BracedList: false 146 | SpaceBeforeCtorInitializerColon: true 147 | SpaceBeforeInheritanceColon: true 148 | SpaceBeforeParens: ControlStatements 149 | SpaceBeforeRangeBasedForLoopColon: true 150 | SpaceInEmptyBlock: false 151 | SpaceInEmptyParentheses: false 152 | SpacesBeforeTrailingComments: 2 153 | SpacesInAngles: false 154 | SpacesInConditionalStatement: false 155 | SpacesInContainerLiterals: true 156 | SpacesInCStyleCastParentheses: false 157 | SpacesInParentheses: false 158 | SpacesInSquareBrackets: false 159 | SpaceBeforeSquareBrackets: false 160 | Standard: Auto 161 | StatementMacros: 162 | - Q_UNUSED 163 | - QT_REQUIRE_VERSION 164 | TabWidth: 8 165 | UseCRLF: false 166 | UseTab: Never 167 | ... 168 | 169 | -------------------------------------------------------------------------------- /src/fast_gicp/cuda/covariance_estimation_rbf.cu: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include 4 | 5 | #include 6 | #include 7 | 8 | namespace fast_gicp { 9 | namespace cuda { 10 | 11 | struct NormalDistribution { 12 | public: 13 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 14 | 15 | __host__ __device__ NormalDistribution() {} 16 | 17 | static __host__ __device__ NormalDistribution zero() { 18 | NormalDistribution dist; 19 | dist.sum_weights = 0.0f; 20 | dist.mean.setZero(); 21 | dist.cov.setZero(); 22 | return dist; 23 | } 24 | 25 | __host__ __device__ NormalDistribution operator+(const NormalDistribution& rhs) const { 26 | NormalDistribution sum; 27 | sum.sum_weights = sum_weights + rhs.sum_weights; 28 | sum.mean = mean + rhs.mean; 29 | sum.cov = cov + rhs.cov; 30 | return sum; 31 | } 32 | 33 | __host__ __device__ NormalDistribution& operator+=(const NormalDistribution& rhs) { 34 | sum_weights += rhs.sum_weights; 35 | mean += rhs.mean; 36 | cov += rhs.cov; 37 | return *this; 38 | } 39 | 40 | __host__ __device__ void accumulate(const float w, const Eigen::Vector3f& x) { 41 | sum_weights += w; 42 | mean += w * x; 43 | cov += w * x * x.transpose(); 44 | } 45 | 46 | __host__ __device__ NormalDistribution& finalize() { 47 | Eigen::Vector3f sum_pt = mean; 48 | mean /= sum_weights; 49 | cov = (cov - mean * sum_pt.transpose()) / sum_weights; 50 | 51 | return *this; 52 | } 53 | 54 | float sum_weights; 55 | Eigen::Vector3f mean; 56 | Eigen::Matrix3f cov; 57 | }; 58 | 59 | struct covariance_estimation_kernel { 60 | static const int BLOCK_SIZE = 512; 61 | 62 | covariance_estimation_kernel(thrust::device_ptr exp_factor_ptr, thrust::device_ptr max_dist_ptr, thrust::device_ptr points_ptr) 63 | : exp_factor_ptr(exp_factor_ptr), 64 | max_dist_ptr(max_dist_ptr), 65 | points_ptr(points_ptr) {} 66 | 67 | __host__ __device__ NormalDistribution operator()(const Eigen::Vector3f& x) const { 68 | const float exp_factor = *thrust::raw_pointer_cast(exp_factor_ptr); 69 | const float max_dist = *thrust::raw_pointer_cast(max_dist_ptr); 70 | const float max_dist_sq = max_dist * max_dist; 71 | const Eigen::Vector3f* points = thrust::raw_pointer_cast(points_ptr); 72 | 73 | NormalDistribution dist = NormalDistribution::zero(); 74 | for (int i = 0; i < BLOCK_SIZE; i++) { 75 | float sq_d = (x - points[i]).squaredNorm(); 76 | if (sq_d > max_dist_sq) { 77 | continue; 78 | } 79 | 80 | float w = expf(-exp_factor * sq_d); 81 | dist.accumulate(w, points[i]); 82 | } 83 | 84 | return dist; 85 | } 86 | 87 | thrust::device_ptr exp_factor_ptr; 88 | thrust::device_ptr max_dist_ptr; 89 | thrust::device_ptr points_ptr; 90 | }; 91 | 92 | struct finalization_kernel { 93 | finalization_kernel(const int stride, const thrust::device_vector& accumulated_dists) 94 | : stride(stride), 95 | accumulated_dists_first(accumulated_dists.data()), 96 | accumulated_dists_last(accumulated_dists.data() + accumulated_dists.size()) {} 97 | 98 | __host__ __device__ Eigen::Matrix3f operator()(int index) const { 99 | const NormalDistribution* dists = thrust::raw_pointer_cast(accumulated_dists_first); 100 | const NormalDistribution* dists_last = thrust::raw_pointer_cast(accumulated_dists_last); 101 | const int num_dists = dists_last - dists; 102 | 103 | NormalDistribution sum = dists[index]; 104 | for (int dist_index = index + stride; dist_index < num_dists; dist_index += stride) { 105 | sum += dists[dist_index]; 106 | } 107 | 108 | return sum.finalize().cov; 109 | } 110 | 111 | const int stride; 112 | thrust::device_ptr accumulated_dists_first; 113 | thrust::device_ptr accumulated_dists_last; 114 | }; 115 | 116 | void covariance_estimation_rbf(const thrust::device_vector& points, double kernel_width, double max_dist, thrust::device_vector& covariances) { 117 | covariances.resize(points.size()); 118 | 119 | thrust::device_vector constants(2); 120 | constants[0] = kernel_width; 121 | constants[1] = max_dist; 122 | thrust::device_ptr exp_factor_ptr = constants.data(); 123 | thrust::device_ptr max_dist_ptr = constants.data() + 1; 124 | 125 | int num_blocks = (points.size() + (covariance_estimation_kernel::BLOCK_SIZE - 1)) / covariance_estimation_kernel::BLOCK_SIZE; 126 | // padding 127 | thrust::device_vector ext_points(num_blocks * covariance_estimation_kernel::BLOCK_SIZE); 128 | thrust::copy(points.begin(), points.end(), ext_points.begin()); 129 | thrust::fill(ext_points.begin() + points.size(), ext_points.end(), Eigen::Vector3f(0.0f, 0.0f, 0.0f)); 130 | 131 | thrust::device_vector accumulated_dists(points.size() * num_blocks); 132 | 133 | thrust::system::cuda::detail::unique_stream stream; 134 | std::vector events(num_blocks); 135 | 136 | // accumulate kerneled point distributions 137 | for (int i = 0; i < num_blocks; i++) { 138 | covariance_estimation_kernel kernel(exp_factor_ptr, max_dist_ptr, ext_points.data() + covariance_estimation_kernel::BLOCK_SIZE * i); 139 | auto event = thrust::async::transform(points.begin(), points.end(), accumulated_dists.begin() + points.size() * i, kernel); 140 | events[i] = std::move(event); 141 | thrust::system::cuda::detail::create_dependency(stream, events[i]); 142 | } 143 | 144 | // finalize distributions 145 | thrust::transform( 146 | thrust::cuda::par.on(stream.native_handle()), 147 | thrust::counting_iterator(0), 148 | thrust::counting_iterator(points.size()), 149 | covariances.begin(), 150 | finalization_kernel(points.size(), accumulated_dists)); 151 | } 152 | 153 | } // namespace cuda 154 | } // namespace fast_gicp -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.10.0) 2 | message("Current Cmake version is : " ${CMAKE_VERSION}) 3 | project(fast_gicp) 4 | add_compile_options(-std=c++14) 5 | option(BUILD_VGICP_CUDA "Build GPU-powered VGICP" ON) 6 | option(BUILD_apps "Build application programs" ON) 7 | option(BUILD_test "Build test programs" OFF) 8 | option(BUILD_PYTHON_BINDINGS "Build python bindings" OFF) 9 | 10 | if(${CMAKE_SYSTEM_PROCESSOR} STREQUAL "aarch64") 11 | else() 12 | add_definitions(-msse -msse2 -msse3 -msse4 -msse4.1 -msse4.2) 13 | set(CMAKE_C_FLAGS "-msse -msse2 -msse3 -msse4 -msse4.1 -msse4.2") 14 | set(CMAKE_CXX_FLAGS "-msse -msse2 -msse3 -msse4 -msse4.1 -msse4.2") 15 | endif() 16 | 17 | OPTION(BUILD_WITH_CERES "Build with CERES for Non-linear Optimization" ON) 18 | ############ 19 | # SET(CMAKE_BUILD_TYPE "Debug") 20 | 21 | SET(CMAKE_BUILD_TYPE "Release") 22 | set(CMAKE_CXX_FLAGS_RELEASE "-O3 -pthread") 23 | 24 | 25 | LIST(APPEND CMAKE_MODULE_PATH "${PROJECT_SOURCE_DIR}/cmake_modules") 26 | 27 | # PCL 28 | FIND_PACKAGE(PCL REQUIRED QUIET) 29 | IF(PCL_FOUND) 30 | INCLUDE_DIRECTORIES(${PCL_INCLUDE_DIRS}) 31 | ADD_DEFINITIONS(${PCL_DEFINITIONS}) 32 | LIST(REMOVE_ITEM PCL_LIBRARIES "vtkproj4") 33 | MESSAGE("PCL [OK]:" ${PCL_INCLUDE_DIRS}) 34 | ENDIF(PCL_FOUND) 35 | # Ceres 36 | IF(BUILD_WITH_CERES) 37 | FIND_PACKAGE(Ceres REQUIRED QUIET) 38 | IF(Ceres_FOUND) 39 | INCLUDE_DIRECTORIES(${CERES_INCLUDE_DIRS}) 40 | MESSAGE("CERES [OK]:" ${CERES_INCLUDE_DIRS}) 41 | ADD_DEFINITIONS(-DCERES_ON) 42 | ENDIF(Ceres_FOUND) 43 | ENDIF(BUILD_WITH_CERES) 44 | 45 | find_package(Eigen3 REQUIRED) 46 | 47 | if(NOT BUILD_PYTHON_BINDINGS) 48 | find_package(catkin) 49 | endif() 50 | 51 | # OpenMP 52 | FIND_PACKAGE( OpenMP REQUIRED) 53 | if(OPENMP_FOUND) 54 | message("OPENMP FOUND") 55 | set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${OpenMP_C_FLAGS}") 56 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS}") 57 | set(CMAKE_EXE_LINKER_FLAGS "${CMAKE_EXE_LINKER_FLAGS} ${OpenMP_EXE_LINKER_FLAGS}") 58 | endif() 59 | 60 | if(BUILD_VGICP_CUDA) 61 | find_package(CUDA REQUIRED) 62 | include_directories(${CUDA_INCLUDE_DIRS}) 63 | link_directories(${CUDA_LIBRARY_DIRS}) 64 | endif() 65 | 66 | ################################### 67 | ## catkin specific configuration ## 68 | ################################### 69 | if(catkin_FOUND) 70 | catkin_package( 71 | INCLUDE_DIRS include 72 | LIBRARIES fast_gicp 73 | ) 74 | endif() 75 | 76 | ########### 77 | ## Build ## 78 | ########### 79 | SET(DEP_LIBS ${DEP_LIBS} ${PCL_LIBRARIES}) 80 | SET(DEP_LIBS ${DEP_LIBS} ${catkin_LIBRARIES}) 81 | IF(BUILD_WITH_CERES) 82 | #link ceres lib (optional) 83 | SET(DEP_LIBS ${DEP_LIBS} ${CERES_LIBRARIES}) 84 | ENDIF(BUILD_WITH_CERES) 85 | 86 | add_library(fast_gicp SHARED 87 | src/fast_gicp/gicp/lsq_registration.cpp 88 | src/fast_gicp/gicp/fast_gicp.cpp 89 | src/fast_gicp/gicp/fast_gicp_st.cpp 90 | src/fast_gicp/gicp/fast_vgicp.cpp 91 | ) 92 | target_link_libraries(fast_gicp 93 | ${DEP_LIBS} 94 | ) 95 | target_include_directories(fast_gicp PUBLIC 96 | include 97 | ${PCL_INCLUDE_DIRS} 98 | ${EIGEN3_INCLUDE_DIR} 99 | ) 100 | 101 | ### APPS ### 102 | if(BUILD_apps) 103 | add_executable(gicp_align src/align.cpp) 104 | add_dependencies(gicp_align fast_gicp) 105 | target_link_libraries(gicp_align 106 | ${DEP_LIBS} 107 | fast_gicp 108 | ) 109 | 110 | add_executable(gicp_kitti src/kitti.cpp) 111 | add_dependencies(gicp_kitti fast_gicp) 112 | target_link_libraries(gicp_kitti 113 | ${DEP_LIBS} 114 | fast_gicp 115 | ) 116 | endif() 117 | 118 | ### Python bindings ### 119 | if(BUILD_PYTHON_BINDINGS) 120 | add_subdirectory(thirdparty/pybind11) 121 | pybind11_add_module(pygicp 122 | src/python/main.cpp 123 | ) 124 | target_include_directories(pygicp PUBLIC 125 | include 126 | ${PCL_INCLUDE_DIRS} 127 | ${EIGEN3_INCLUDE_DIR} 128 | ) 129 | target_link_libraries(pygicp PRIVATE 130 | fast_gicp 131 | ) 132 | endif() 133 | 134 | ### CUDA ### 135 | if(BUILD_VGICP_CUDA) 136 | set(CUDA_NVCC_FLAGS "--expt-relaxed-constexpr") 137 | add_definitions(-DUSE_VGICP_CUDA) 138 | 139 | cuda_add_library(fast_vgicp_cuda SHARED 140 | src/fast_gicp/cuda/fast_vgicp_cuda.cu 141 | src/fast_gicp/cuda/brute_force_knn.cu 142 | src/fast_gicp/cuda/covariance_estimation.cu 143 | src/fast_gicp/cuda/covariance_estimation_rbf.cu 144 | src/fast_gicp/cuda/covariance_regularization.cu 145 | src/fast_gicp/cuda/gaussian_voxelmap.cu 146 | src/fast_gicp/cuda/find_voxel_correspondences.cu 147 | src/fast_gicp/cuda/compute_derivatives.cu 148 | src/fast_gicp/cuda/compute_mahalanobis.cu 149 | src/fast_gicp/cuda/ndt_cuda.cu 150 | src/fast_gicp/cuda/ndt_compute_derivatives.cu 151 | ) 152 | target_include_directories(fast_vgicp_cuda PRIVATE 153 | include 154 | thirdparty/Eigen 155 | thirdparty/nvbio 156 | ${catkin_INCLUDE_DIRS} 157 | ) 158 | target_link_libraries(fast_vgicp_cuda 159 | ${catkin_LIBRARIES} 160 | ) 161 | cuda_add_cublas_to_target(fast_vgicp_cuda) 162 | 163 | # add vgicp_cuda to libfast_gicp 164 | target_sources(fast_gicp PRIVATE 165 | src/fast_gicp/gicp/fast_vgicp_cuda.cpp 166 | src/fast_gicp/ndt/ndt_cuda.cpp 167 | ) 168 | target_link_libraries(fast_gicp 169 | fast_vgicp_cuda 170 | ) 171 | add_dependencies(fast_gicp fast_vgicp_cuda) 172 | if(catkin_FOUND) 173 | install(TARGETS fast_vgicp_cuda 174 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}) 175 | endif() 176 | endif() 177 | 178 | ### TEST ### 179 | if(BUILD_test) 180 | find_package(GTest REQUIRED) 181 | 182 | add_executable(gicp_test src/test/gicp_test.cpp) 183 | add_dependencies(gicp_test fast_gicp) 184 | target_link_libraries(gicp_test ${GTEST_LIBRARIES} ${PCL_LIBRARIES} fast_gicp) 185 | gtest_add_tests(TARGET gicp_test WORKING_DIRECTORY ${CMAKE_SOURCE_DIR} EXTRA_ARGS "${CMAKE_SOURCE_DIR}/data") 186 | endif() 187 | 188 | if(catkin_FOUND) 189 | install(TARGETS ${PROJECT_NAME} 190 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}) 191 | 192 | install(DIRECTORY include/ 193 | DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION} 194 | FILES_MATCHING PATTERN "*.hpp") 195 | endif() 196 | -------------------------------------------------------------------------------- /include/fast_gicp/gicp/fast_vgicp_voxel.hpp: -------------------------------------------------------------------------------- 1 | #ifndef FAST_GICP_FAST_VGICP_VOXEL_HPP 2 | #define FAST_GICP_FAST_VGICP_VOXEL_HPP 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | namespace fast_gicp { 9 | /** 10 | * @brief 选择搜索方式，方式的数字指的是周围的多少格 11 | * 12 | * @param search_method 13 | * @return std::vector> 14 | */ 15 | static std::vector> neighbor_offsets(NeighborSearchMethod search_method) { 16 | switch(search_method) { 17 | // clang-format off 18 | default: 19 | std::cerr << "unsupported neighbor search method" << std::endl; 20 | abort(); 21 | case NeighborSearchMethod::DIRECT1: 22 | return std::vector>{ 23 | Eigen::Vector3i(0, 0, 0) 24 | }; 25 | case NeighborSearchMethod::DIRECT7: 26 | return std::vector>{ 27 | Eigen::Vector3i(0, 0, 0), 28 | Eigen::Vector3i(1, 0, 0), 29 | Eigen::Vector3i(-1, 0, 0), 30 | Eigen::Vector3i(0, 1, 0), 31 | Eigen::Vector3i(0, -1, 0), 32 | Eigen::Vector3i(0, 0, 1), 33 | Eigen::Vector3i(0, 0, -1) 34 | }; 35 | case NeighborSearchMethod::DIRECT27: 36 | break; 37 | // clang-format on 38 | } 39 | 40 | std::vector> offsets27; 41 | for(int i = 0; i < 3; i++) { 42 | for(int j = 0; j < 3; j++) { 43 | for(int k = 0; k < 3; k++) { 44 | offsets27.push_back(Eigen::Vector3i(i - 1, j - 1, k - 1)); 45 | } 46 | } 47 | } 48 | return offsets27; 49 | } 50 | 51 | class Vector3iHash { 52 | public: 53 | size_t operator()(const Eigen::Vector3i& x) const { 54 | size_t seed = 0; 55 | boost::hash_combine(seed, x[0]); 56 | boost::hash_combine(seed, x[1]); 57 | boost::hash_combine(seed, x[2]); 58 | return seed; 59 | } 60 | }; 61 | 62 | struct GaussianVoxel { 63 | public: 64 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 65 | using Ptr = std::shared_ptr; 66 | 67 | GaussianVoxel() { 68 | num_points = 0; 69 | mean.setZero(); 70 | cov.setZero(); 71 | } 72 | virtual ~GaussianVoxel() {} 73 | 74 | virtual void append(const Eigen::Vector4d& mean_, const Eigen::Matrix4d& cov_) = 0; 75 | 76 | virtual void finalize() = 0; 77 | 78 | public: 79 | int num_points; 80 | Eigen::Vector4d mean; 81 | Eigen::Matrix4d cov; 82 | }; 83 | 84 | struct MultiplicativeGaussianVoxel : GaussianVoxel { 85 | public: 86 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 87 | 88 | MultiplicativeGaussianVoxel() : GaussianVoxel() {} 89 | virtual ~MultiplicativeGaussianVoxel() {} 90 | 91 | virtual void append(const Eigen::Vector4d& mean_, const Eigen::Matrix4d& cov_) override { 92 | num_points++; 93 | Eigen::Matrix4d cov_inv = cov_; 94 | cov_inv(3, 3) = 1; 95 | cov_inv = cov_inv.inverse().eval(); 96 | 97 | cov += cov_inv; 98 | mean += cov_inv * mean_; 99 | } 100 | 101 | virtual void finalize() override { 102 | cov(3, 3) = 1; 103 | mean[3] = 1; 104 | 105 | cov = cov.inverse().eval(); 106 | mean = (cov * mean).eval(); 107 | } 108 | }; 109 | 110 | struct AdditiveGaussianVoxel : GaussianVoxel { 111 | public: 112 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 113 | 114 | AdditiveGaussianVoxel() : GaussianVoxel() {} 115 | virtual ~AdditiveGaussianVoxel() {} 116 | 117 | virtual void append(const Eigen::Vector4d& mean_, const Eigen::Matrix4d& cov_) override { 118 | num_points++; 119 | mean += mean_; 120 | cov += cov_; 121 | } 122 | 123 | virtual void finalize() override { 124 | mean /= num_points; 125 | cov /= num_points; 126 | } 127 | }; 128 | 129 | template 130 | class GaussianVoxelMap { 131 | public: 132 | GaussianVoxelMap(double resolution, VoxelAccumulationMode mode) : voxel_resolution_(resolution), voxel_mode_(mode) {} 133 | 134 | void create_voxelmap(const pcl::PointCloud& cloud, const std::vector>& covs) { 135 | voxels_.clear(); 136 | for(int i = 0; i < cloud.size(); i++) { 137 | Eigen::Vector3i coord = voxel_coord(cloud.at(i).getVector4fMap().template cast()); 138 | 139 | auto found = voxels_.find(coord); 140 | if(found == voxels_.end()) { 141 | GaussianVoxel::Ptr voxel; 142 | switch(voxel_mode_) { 143 | case VoxelAccumulationMode::ADDITIVE: 144 | case VoxelAccumulationMode::ADDITIVE_WEIGHTED: 145 | voxel = std::shared_ptr(new AdditiveGaussianVoxel); 146 | break; 147 | case VoxelAccumulationMode::MULTIPLICATIVE: 148 | voxel = std::shared_ptr(new MultiplicativeGaussianVoxel); 149 | break; 150 | } 151 | found = voxels_.insert(found, std::make_pair(coord, voxel)); 152 | } 153 | 154 | auto& voxel = found->second; 155 | voxel->append(cloud.at(i).getVector4fMap().template cast(), covs[i]); 156 | } 157 | 158 | for(auto& voxel : voxels_) { 159 | voxel.second->finalize(); 160 | } 161 | } 162 | 163 | Eigen::Vector3i voxel_coord(const Eigen::Vector4d& x) const { 164 | return (x.array() / voxel_resolution_ - 0.5).floor().template cast().template head<3>(); 165 | } 166 | 167 | Eigen::Vector4d voxel_origin(const Eigen::Vector3i& coord) const { 168 | Eigen::Vector3d origin = (coord.template cast().array() + 0.5) * voxel_resolution_; 169 | return Eigen::Vector4d(origin[0], origin[1], origin[2], 1.0f); 170 | } 171 | 172 | GaussianVoxel::Ptr lookup_voxel(const Eigen::Vector3i& coord) const { 173 | auto found = voxels_.find(coord); 174 | if(found == voxels_.end()) { 175 | return nullptr; 176 | } 177 | 178 | return found->second; 179 | } 180 | 181 | private: 182 | double voxel_resolution_; 183 | VoxelAccumulationMode voxel_mode_; 184 | 185 | using VoxelMap = std::unordered_map, Eigen::aligned_allocator>>; 186 | VoxelMap voxels_; 187 | }; 188 | 189 | } // namespace fast_gicp 190 | 191 | #endif -------------------------------------------------------------------------------- /src/kitti.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | 7 | #include 8 | #include 9 | #include 10 | #include 11 | 12 | #include 13 | #include 14 | 15 | #include 16 | #include 17 | #include "fast_gicp/time_utils.hpp" 18 | #ifdef USE_VGICP_CUDA 19 | #include 20 | #include 21 | #endif 22 | 23 | class KittiLoader { 24 | public: 25 | KittiLoader(const std::string& dataset_path) : dataset_path(dataset_path) { 26 | for (num_frames = 0;; num_frames++) { 27 | std::string filename = (boost::format("%s/%010d.bin") % dataset_path % num_frames).str(); 28 | if (!boost::filesystem::exists(filename)) { 29 | break; 30 | } 31 | } 32 | 33 | if (num_frames == 0) { 34 | std::cerr << "error: no files in " << dataset_path << std::endl; 35 | } 36 | } 37 | ~KittiLoader() {} 38 | 39 | size_t size() const { return num_frames; } 40 | 41 | pcl::PointCloud::ConstPtr cloud(size_t i) const { 42 | std::string filename = (boost::format("%s/%010d.bin") % dataset_path % i).str(); 43 | FILE* file = fopen(filename.c_str(), "rb"); 44 | if (!file) { 45 | std::cerr << "error: failed to load " << filename << std::endl; 46 | return nullptr; 47 | } 48 | 49 | std::vector buffer(1000000); 50 | size_t num_points = fread(reinterpret_cast(buffer.data()), sizeof(float), buffer.size(), file) / 4; 51 | fclose(file); 52 | 53 | pcl::PointCloud::Ptr cloud(new pcl::PointCloud()); 54 | cloud->resize(num_points); 55 | 56 | for (int i = 0; i < num_points; i++) { 57 | auto& pt = cloud->at(i); 58 | pt.x = buffer[i * 4]; 59 | pt.y = buffer[i * 4 + 1]; 60 | pt.z = buffer[i * 4 + 2]; 61 | pt.intensity = buffer[i * 4 + 3]; 62 | } 63 | 64 | return cloud; 65 | } 66 | 67 | private: 68 | int num_frames; 69 | std::string dataset_path; 70 | }; 71 | 72 | int main(int argc, char** argv) { 73 | if (argc < 2) { 74 | std::cout << "usage: gicp_kitti /your/kitti/path/sequences/00/velodyne" << std::endl; 75 | return 0; 76 | } 77 | 78 | KittiLoader kitti(argv[1]); 79 | // 导出pcd 80 | // for(int i = 0;i < kitti.size();i++){ 81 | // pcl::PointCloud cloud = *kitti.cloud(i); 82 | // std::string filename = (boost::format("%s/pcd/%010d.pcd") % argv[1] % i).str(); 83 | // pcl::io::savePCDFileBinary(filename,cloud); 84 | // } 85 | // return 0; 86 | // use downsample_resolution=1.0 for fast registration 87 | double downsample_resolution = 0.25; 88 | pcl::ApproximateVoxelGrid voxelgrid; 89 | voxelgrid.setLeafSize(downsample_resolution, downsample_resolution, downsample_resolution); 90 | 91 | // registration method 92 | // you should fine-tune hyper-parameters (e.g., voxel resolution, max correspondence distance) for the best result 93 | fast_gicp::FastGICP gicp; 94 | // fast_gicp::FastVGICP gicp; 95 | // fast_gicp::FastVGICPCuda gicp; 96 | // fast_gicp::NDTCuda gicp; 97 | gicp.setNumThreads(4); 98 | gicp.setLSQType(fast_gicp::LSQ_OPTIMIZER_TYPE::CeresDogleg); 99 | // gicp.setResolution(1.0); 100 | gicp.setTransformationEpsilon(0.01); 101 | gicp.setMaximumIterations(64); 102 | gicp.setCorrespondenceRandomness(20); 103 | // gicp.setNearestNeighborSearchMethod(fast_gicp::NearestNeighborMethod::GPU_RBF_KERNEL); 104 | gicp.setMaxCorrespondenceDistance(1.0); 105 | 106 | // set initial frame as target 107 | voxelgrid.setInputCloud(kitti.cloud(0)); 108 | pcl::PointCloud::Ptr target(new pcl::PointCloud); 109 | voxelgrid.filter(*target); 110 | gicp.setInputTarget(target); 111 | 112 | // sensor pose sequence 113 | std::vector> poses(kitti.size()); 114 | poses[0].setIdentity(); 115 | 116 | // trajectory for visualization 117 | pcl::PointCloud::Ptr trajectory(new pcl::PointCloud); 118 | trajectory->push_back(pcl::PointXYZ(0.0f, 0.0f, 0.0f)); 119 | 120 | pcl::visualization::PCLVisualizer vis; 121 | vis.addPointCloud(trajectory, "trajectory"); 122 | 123 | // for calculating FPS 124 | boost::circular_buffer stamps(30); 125 | stamps.push_back(std::chrono::high_resolution_clock::now()); 126 | tic::TicToc t; 127 | t.tic(); 128 | for (int i = 1; i < 20; i++) { 129 | // set the current frame as source 130 | voxelgrid.setInputCloud(kitti.cloud(i)); 131 | pcl::PointCloud::Ptr source(new pcl::PointCloud); 132 | voxelgrid.filter(*source); 133 | gicp.setInputSource(source); 134 | 135 | // align and swap source and target cloud for next registration 136 | pcl::PointCloud::Ptr aligned(new pcl::PointCloud); 137 | gicp.align(*aligned); 138 | // gicp.swapSourceAndTarget(); 139 | gicp.setInputTarget(source); 140 | 141 | // accumulate pose 142 | poses[i] = poses[i - 1] * gicp.getFinalTransformation().cast(); 143 | 144 | // // FPS display 145 | // stamps.push_back(std::chrono::high_resolution_clock::now()); 146 | // std::cout << stamps.size() / (std::chrono::duration_cast(stamps.back() - stamps.front()).count() / 1e9) << "fps" << std::endl; 147 | 148 | // // visualization 149 | // trajectory->push_back(pcl::PointXYZ(poses[i](0, 3), poses[i](1, 3), poses[i](2, 3))); 150 | // vis.updatePointCloud(trajectory, pcl::visualization::PointCloudColorHandlerCustom(trajectory, 255.0, 0.0, 0.0), "trajectory"); 151 | // vis.spinOnce(); 152 | } 153 | cout << "time of vgicp : " << t.toc() << " ms." << endl; 154 | 155 | // save the estimated poses 156 | std::ofstream ofs("/home/xyw/00_pred.txt"); 157 | for (const auto& pose : poses) { 158 | for (int i = 0; i < 3; i++) { 159 | for (int j = 0; j < 4; j++) { 160 | if (i || j) { 161 | ofs << " "; 162 | } 163 | 164 | ofs << pose(i, j); 165 | } 166 | } 167 | ofs << std::endl; 168 | } 169 | 170 | return 0; 171 | } -------------------------------------------------------------------------------- /src/fast_gicp/cuda/ndt_cuda.cu: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include 4 | 5 | #include 6 | #include 7 | #include 8 | #include 9 | 10 | namespace fast_gicp { 11 | namespace cuda { 12 | 13 | NDTCudaCore::NDTCudaCore() { 14 | cudaDeviceSynchronize(); 15 | resolution = 1.0; 16 | linearized_x.setIdentity(); 17 | 18 | offsets.reset(new thrust::device_vector(1)); 19 | (*offsets)[0] = Eigen::Vector3i::Zero().eval(); 20 | 21 | distance_mode = fast_gicp::NDTDistanceMode::D2D; 22 | set_neighbor_search_method(fast_gicp::NeighborSearchMethod::DIRECT7, 0.0); 23 | } 24 | 25 | NDTCudaCore::~NDTCudaCore() {} 26 | 27 | void NDTCudaCore::set_distance_mode(fast_gicp::NDTDistanceMode mode) { 28 | this->distance_mode = mode; 29 | } 30 | 31 | void NDTCudaCore::set_resolution(double resolution) { 32 | this->resolution = resolution; 33 | } 34 | 35 | void NDTCudaCore::set_neighbor_search_method(fast_gicp::NeighborSearchMethod method, double radius) { 36 | thrust::host_vector> h_offsets; 37 | 38 | switch (method) { 39 | default: 40 | std::cerr << "here must not be reached" << std::endl; 41 | abort(); 42 | 43 | case fast_gicp::NeighborSearchMethod::DIRECT1: 44 | h_offsets.resize(1); 45 | h_offsets[0] = Eigen::Vector3i::Zero(); 46 | break; 47 | 48 | case fast_gicp::NeighborSearchMethod::DIRECT7: 49 | h_offsets.resize(7); 50 | h_offsets[0] = Eigen::Vector3i(0, 0, 0); 51 | h_offsets[1] = Eigen::Vector3i(1, 0, 0); 52 | h_offsets[2] = Eigen::Vector3i(-1, 0, 0); 53 | h_offsets[3] = Eigen::Vector3i(0, 1, 0); 54 | h_offsets[4] = Eigen::Vector3i(0, -1, 0); 55 | h_offsets[5] = Eigen::Vector3i(0, 0, 1); 56 | h_offsets[6] = Eigen::Vector3i(0, 0, -1); 57 | break; 58 | 59 | case fast_gicp::NeighborSearchMethod::DIRECT27: 60 | h_offsets.reserve(27); 61 | for (int i = 0; i < 3; i++) { 62 | for (int j = 0; j < 3; j++) { 63 | for (int k = 0; k < 3; k++) { 64 | h_offsets.push_back(Eigen::Vector3i(i - 1, j - 1, k - 1)); 65 | } 66 | } 67 | } 68 | break; 69 | 70 | case fast_gicp::NeighborSearchMethod::DIRECT_RADIUS: 71 | h_offsets.reserve(50); 72 | int range = std::ceil(radius); 73 | for (int i = -range; i <= range; i++) { 74 | for (int j = -range; j <= range; j++) { 75 | for (int k = -range; k <= range; k++) { 76 | Eigen::Vector3i offset(i, j, k); 77 | if (offset.cast().norm() <= radius + 1e-3) { 78 | h_offsets.push_back(offset); 79 | } 80 | } 81 | } 82 | } 83 | 84 | break; 85 | } 86 | 87 | *offsets = h_offsets; 88 | } 89 | 90 | void NDTCudaCore::swap_source_and_target() { 91 | source_points.swap(target_points); 92 | source_voxelmap.swap(target_voxelmap); 93 | } 94 | 95 | void NDTCudaCore::set_source_cloud(const std::vector>& cloud) { 96 | thrust::host_vector> points(cloud.begin(), cloud.end()); 97 | if (!source_points) { 98 | source_points.reset(new Points()); 99 | } 100 | 101 | *source_points = points; 102 | source_voxelmap.reset(); 103 | } 104 | 105 | void NDTCudaCore::set_target_cloud(const std::vector>& cloud) { 106 | thrust::host_vector> points(cloud.begin(), cloud.end()); 107 | if (!target_points) { 108 | target_points.reset(new Points()); 109 | } 110 | 111 | *target_points = points; 112 | target_voxelmap.reset(); 113 | } 114 | 115 | void NDTCudaCore::create_voxelmaps() { 116 | create_source_voxelmap(); 117 | create_target_voxelmap(); 118 | } 119 | 120 | void NDTCudaCore::create_source_voxelmap() { 121 | assert(source_points); 122 | if (source_voxelmap || distance_mode == fast_gicp::NDTDistanceMode::P2D) { 123 | return; 124 | } 125 | 126 | source_voxelmap.reset(new GaussianVoxelMap(resolution)); 127 | source_voxelmap->create_voxelmap(*source_points); 128 | covariance_regularization(source_voxelmap->voxel_means, source_voxelmap->voxel_covs, fast_gicp::RegularizationMethod::MIN_EIG); 129 | } 130 | 131 | void NDTCudaCore::create_target_voxelmap() { 132 | assert(target_points); 133 | if (target_voxelmap) { 134 | return; 135 | } 136 | 137 | target_voxelmap.reset(new GaussianVoxelMap(resolution)); 138 | target_voxelmap->create_voxelmap(*target_points); 139 | covariance_regularization(target_voxelmap->voxel_means, target_voxelmap->voxel_covs, fast_gicp::RegularizationMethod::MIN_EIG); 140 | } 141 | 142 | void NDTCudaCore::update_correspondences(const Eigen::Isometry3d& trans) { 143 | thrust::device_vector trans_ptr(1); 144 | trans_ptr[0] = trans.cast(); 145 | 146 | if (correspondences == nullptr) { 147 | correspondences.reset(new Correspondences()); 148 | } 149 | linearized_x = trans.cast(); 150 | 151 | switch (distance_mode) { 152 | case fast_gicp::NDTDistanceMode::P2D: 153 | find_voxel_correspondences(*source_points, *target_voxelmap, trans_ptr.data(), *offsets, *correspondences); 154 | break; 155 | 156 | case fast_gicp::NDTDistanceMode::D2D: 157 | find_voxel_correspondences(source_voxelmap->voxel_means, *target_voxelmap, trans_ptr.data(), *offsets, *correspondences); 158 | break; 159 | } 160 | } 161 | 162 | double NDTCudaCore::compute_error(const Eigen::Isometry3d& trans, Eigen::Matrix* H, Eigen::Matrix* b) const { 163 | thrust::host_vector> trans_(2); 164 | trans_[0] = linearized_x; 165 | trans_[1] = trans.cast(); 166 | 167 | thrust::device_vector trans_ptr = trans_; 168 | 169 | switch (distance_mode) { 170 | default: 171 | case fast_gicp::NDTDistanceMode::P2D: 172 | return p2d_ndt_compute_derivatives(*target_voxelmap, *source_points, *correspondences, trans_ptr.data(), trans_ptr.data() + 1, H, b); 173 | 174 | case fast_gicp::NDTDistanceMode::D2D: 175 | return d2d_ndt_compute_derivatives(*target_voxelmap, *source_voxelmap, *correspondences, trans_ptr.data(), trans_ptr.data() + 1, H, b); 176 | } 177 | } 178 | 179 | } // namespace cuda 180 | 181 | } // namespace fast_gicp -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # fast_gicp 2 | 3 | This package is a collection of GICP-based fast point cloud registration algorithms. It constains a multi-threaded GICP as well as multi-thread and GPU implementations of our voxelized GICP (VGICP) algorithm. All the implemented algorithms have the PCL registration interface so that they can be used as an inplace replacement for GICP in PCL. 4 | 5 | - FastGICP: multi-threaded GICP algorithm (**\~40FPS**) 6 | - FastGICPSingleThread: GICP algorithm optimized for single-threading (**\~15FPS**) 7 | - FastVGICP: multi-threaded and voxelized GICP algorithm (**\~70FPS**) 8 | - FastVGICPCuda: CUDA-accelerated voxelized GICP algorithm (**\~120FPS**) 9 | - NDTCuda: CUDA-accelerated D2D NDT algorithm (**\~500FPS**) 10 | ![proctime](data/proctime.png) 11 | 12 | [![Build](https://github.com/SMRT-AIST/fast_gicp/actions/workflows/build.yml/badge.svg)](https://github.com/SMRT-AIST/fast_gicp/actions/workflows/build.yml) on melodic & noetic 13 | 14 | ## Installation 15 | 16 | ### Dependencies 17 | - PCL 18 | - Eigen 19 | - OpenMP 20 | - CUDA (optional) 21 | - [Sophus](https://github.com/strasdat/Sophus) 22 | - [nvbio](https://github.com/NVlabs/nvbio) 23 | 24 | We have tested this package on Ubuntu 18.04/20.04 and CUDA 11.1. 25 | 26 | ### CUDA 27 | 28 | To enable the CUDA-powered implementations, set ```BUILD_VGICP_CUDA``` cmake option to ```ON```. 29 | 30 | 需要注意的是，如果需要使用CUDA，则cmake应该保持较高的版本，我自己的电脑上cmake 3.10.2无法使用CUDA11.2。 31 | 放弃CUDA 32 | 33 | 升级cmake的方法: https://cmake.org/download/ 34 | 35 | 在上述网址下载合适版本的源码压缩包后，解压，运行 `./bootstrap && make && sudo make install` 即可，默认会被安装到/usr/local里。 36 | 37 | 输入`cmake --version` 可以查看当前cmake版本 38 | 39 | ### ROS 40 | ```bash 41 | cd ~/catkin_ws/src 42 | git clone https://github.com/SMRT-AIST/fast_gicp --recursive 43 | cd .. && catkin_make -DCMAKE_BUILD_TYPE=Release 44 | # enable cuda-based implementations 45 | # cd .. && catkin_make -DCMAKE_BUILD_TYPE=Release -DBUILD_VGICP_CUDA=ON 46 | ``` 47 | 48 | ### Non-ROS 49 | ```bash 50 | git clone https://github.com/SMRT-AIST/fast_gicp --recursive 51 | mkdir fast_gicp/build && cd fast_gicp/build 52 | cmake .. -DCMAKE_BUILD_TYPE=Release 53 | # enable cuda-based implementations 54 | # cmake .. -DCMAKE_BUILD_TYPE=Release -DBUILD_VGICP_CUDA=ON 55 | make -j8 56 | ``` 57 | 58 | ### Python bindings 59 | ```bash 60 | cd fast_gicp 61 | python3 setup.py install --user 62 | ``` 63 | Note: If you are on a catkin-enabled environment and the installation doesn't work well, comment out ```find_package(catkin)``` in CMakeLists.txt and run the above installation command again. 64 | 65 | 66 | ```python 67 | import pygicp 68 | 69 | target = # Nx3 numpy array 70 | source = # Mx3 numpy array 71 | 72 | # 1. function interface 73 | matrix = pygicp.align_points(target, source) 74 | 75 | # optional arguments 76 | # initial_guess : Initial guess of the relative pose (4x4 matrix) 77 | # method : GICP, VGICP, VGICP_CUDA, or NDT_CUDA 78 | # downsample_resolution : Downsampling resolution (used only if positive) 79 | # k_correspondences : Number of points used for covariance estimation 80 | # max_correspondence_distance : Maximum distance for corresponding point search 81 | # voxel_resolution : Resolution of voxel-based algorithms 82 | # neighbor_search_method : DIRECT1, DIRECT7, DIRECT27, or DIRECT_RADIUS 83 | # neighbor_search_radius : Neighbor voxel search radius (for GPU-based methods) 84 | # num_threads : Number of threads 85 | 86 | 87 | # 2. class interface 88 | # you may want to downsample the input clouds before registration 89 | target = pygicp.downsample(target, 0.25) 90 | source = pygicp.downsample(source, 0.25) 91 | 92 | # pygicp.FastGICP has more or less the same interfaces as the C++ version 93 | gicp = pygicp.FastGICP() 94 | gicp.set_input_target(target) 95 | gicp.set_input_source(source) 96 | matrix = gicp.align() 97 | 98 | # optional 99 | gicp.set_num_threads(4) 100 | gicp.set_max_correspondence_distance(1.0) 101 | gicp.get_final_transformation() 102 | gicp.get_final_hessian() 103 | ``` 104 | 105 | ## Benchmark 106 | CPU:Core i9-9900K GPU:GeForce RTX2080Ti 107 | 108 | ```bash 109 | roscd fast_gicp/data 110 | rosrun fast_gicp gicp_align 251370668.pcd 251371071.pcd 111 | ``` 112 | 113 | ``` 114 | target:17249[pts] source:17518[pts] 115 | --- pcl_gicp --- 116 | single:127.508[msec] 100times:12549.4[msec] fitness_score:0.204892 117 | --- pcl_ndt --- 118 | single:53.5904[msec] 100times:5467.16[msec] fitness_score:0.229616 119 | --- fgicp_st --- 120 | single:111.324[msec] 100times:10662.7[msec] 100times_reuse:6794.59[msec] fitness_score:0.204379 121 | --- fgicp_mt --- 122 | single:20.1602[msec] 100times:1585[msec] 100times_reuse:1017.74[msec] fitness_score:0.204412 123 | --- vgicp_st --- 124 | single:112.001[msec] 100times:7959.9[msec] 100times_reuse:4408.22[msec] fitness_score:0.204067 125 | --- vgicp_mt --- 126 | single:18.1106[msec] 100times:1381[msec] 100times_reuse:806.53[msec] fitness_score:0.204067 127 | --- vgicp_cuda (parallel_kdtree) --- 128 | single:15.9587[msec] 100times:1451.85[msec] 100times_reuse:695.48[msec] fitness_score:0.204061 129 | --- vgicp_cuda (gpu_bruteforce) --- 130 | single:53.9113[msec] 100times:3463.5[msec] 100times_reuse:1703.41[msec] fitness_score:0.204049 131 | --- vgicp_cuda (gpu_rbf_kernel) --- 132 | single:5.91508[msec] 100times:590.725[msec] 100times_reuse:226.787[msec] fitness_score:0.20557 133 | ``` 134 | 135 | See [src/align.cpp](https://github.com/SMRT-AIST/fast_gicp/blob/master/src/align.cpp) for the detailed usage. 136 | 137 | ## Test on KITTI 138 | 139 | ### C++ 140 | 141 | ```bash 142 | # Perform frame-by-frame registration 143 | rosrun fast_gicp gicp_kitti /media/xyw/Backup/AGV/dataset/KITTI/rawdata/2011_10_03/2011_10_03_drive_0027_sync/velodyne_points/data 144 | ``` 145 | 146 | ![kitti00](https://user-images.githubusercontent.com/31344317/86207074-b98ac280-bba8-11ea-9687-e65f03aaf25b.png) 147 | 148 | ### Python 149 | 150 | ```bash 151 | cd fast_gicp/src 152 | python3 kitti.py /your/kitti/path/sequences/00/velodyne 153 | ``` 154 | 155 | ## Related packages 156 | - [ndt_omp](https://github.com/koide3/ndt_omp) 157 | - [fast_gicp](https://github.com/SMRT-AIST/fast_gicp) 158 | 159 | 160 | ## Papers 161 | - Kenji Koide, Masashi Yokozuka, Shuji Oishi, and Atsuhiko Banno, Voxelized GICP for fast and accurate 3D point cloud registration, ICRA2021 [[link]](https://easychair.org/publications/preprint/ftvV) 162 | 163 | ## Contact 164 | Kenji Koide, k.koide@aist.go.jp 165 | 166 | Human-Centered Mobility Research Center, National Institute of Advanced Industrial Science and Technology, Japan [\[URL\]](https://unit.aist.go.jp/rirc/en/team/smart_mobility.html) 167 | 168 | ## 使用注意 169 | fast-gicp和fast-vgicp现在不会重用协方差了，每一个点云传入，都要重新计算，以适应scan-to-map 170 | 程序已经被修改可以使用sync的数据，若要使用odom数据，则需要在kitti-helper处改成06d。 -------------------------------------------------------------------------------- /src/test/gicp_test.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | 12 | #include 13 | #include 14 | #include 15 | #ifdef USE_VGICP_CUDA 16 | #include 17 | #include 18 | #endif 19 | 20 | struct GICPTestBase : public testing::Test { 21 | public: 22 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 23 | using PointCloudConstPtr = pcl::PointCloud::ConstPtr; 24 | 25 | GICPTestBase() {} 26 | 27 | virtual void SetUp() { 28 | if (!load(data_directory)) { 29 | exit(1); 30 | } 31 | } 32 | 33 | bool load(const std::string& data_directory) { 34 | relative_pose.setIdentity(); 35 | 36 | std::ifstream ifs(data_directory + "/relative.txt"); 37 | if (!ifs) { 38 | return false; 39 | } 40 | 41 | for (int i = 0; i < 4; i++) { 42 | for (int j = 0; j < 4; j++) { 43 | ifs >> relative_pose(i, j); 44 | } 45 | } 46 | 47 | auto target = pcl::make_shared>(); 48 | auto source = pcl::make_shared>(); 49 | pcl::io::loadPCDFile(data_directory + "/251370668.pcd", *target); 50 | pcl::io::loadPCDFile(data_directory + "/251371071.pcd", *source); 51 | if (target->empty() || source->empty()) { 52 | return true; 53 | } 54 | 55 | pcl::VoxelGrid voxelgrid; 56 | voxelgrid.setLeafSize(0.2, 0.2, 0.2); 57 | 58 | auto filtered = pcl::make_shared>(); 59 | voxelgrid.setInputCloud(target); 60 | voxelgrid.filter(*filtered); 61 | filtered.swap(target); 62 | 63 | voxelgrid.setInputCloud(source); 64 | voxelgrid.filter(*filtered); 65 | filtered.swap(source); 66 | 67 | this->target = target; 68 | this->source = source; 69 | 70 | return true; 71 | } 72 | 73 | Eigen::Vector2f pose_error(const Eigen::Matrix4f estimated) const { 74 | Eigen::Matrix4f delta = relative_pose.inverse() * estimated; 75 | double t_error = delta.block<3, 1>(0, 3).norm(); 76 | double r_error = Eigen::AngleAxisf(delta.block<3, 3>(0, 0)).angle(); 77 | return Eigen::Vector2f(t_error, r_error); 78 | } 79 | 80 | static std::string data_directory; 81 | 82 | PointCloudConstPtr target; 83 | PointCloudConstPtr source; 84 | Eigen::Matrix4f relative_pose; 85 | }; 86 | 87 | std::string GICPTestBase::data_directory; 88 | 89 | TEST_F(GICPTestBase, LoadCheck) { 90 | EXPECT_NE(target, nullptr); 91 | EXPECT_NE(source, nullptr); 92 | EXPECT_FALSE(target->empty()); 93 | EXPECT_FALSE(source->empty()); 94 | } 95 | 96 | using Parameters = std::tuple; 97 | class AlignmentTest : public GICPTestBase, public testing::WithParamInterface { 98 | public: 99 | pcl::Registration::Ptr create_reg() { 100 | std::string method = std::get<0>(GetParam()); 101 | int num_threads = std::get<1>(GetParam()) ? 4 : 1; 102 | 103 | if (method == "GICP") { 104 | auto gicp = pcl::make_shared>(); 105 | gicp->setNumThreads(num_threads); 106 | gicp->swapSourceAndTarget(); 107 | return gicp; 108 | } else if (method == "VGICP") { 109 | auto vgicp = pcl::make_shared>(); 110 | vgicp->setNumThreads(num_threads); 111 | return vgicp; 112 | } else if (method == "VGICP_CUDA") { 113 | #ifdef USE_VGICP_CUDA 114 | auto vgicp = pcl::make_shared>(); 115 | return vgicp; 116 | #endif 117 | return nullptr; 118 | } else if (method == "NDT_CUDA") { 119 | #ifdef USE_VGICP_CUDA 120 | auto ndt = pcl::make_shared>(); 121 | return ndt; 122 | #endif 123 | return nullptr; 124 | } 125 | 126 | std::cerr << "unknown registration method:" << method << std::endl; 127 | return nullptr; 128 | } 129 | 130 | void swap_source_and_target(pcl::Registration::Ptr reg) { 131 | fast_gicp::LsqRegistration* lsq_reg = dynamic_cast*>(reg.get()); 132 | if (lsq_reg != nullptr) { 133 | lsq_reg->swapSourceAndTarget(); 134 | return; 135 | } 136 | 137 | std::cerr << "failed to swap source and target" << std::endl; 138 | } 139 | }; 140 | 141 | INSTANTIATE_TEST_SUITE_P(AlignmentTest2, AlignmentTest, testing::Combine(testing::Values("GICP", "VGICP", "VGICP_CUDA", "NDT_CUDA"), testing::Bool()), [](const auto& info) { 142 | std::stringstream sst; 143 | sst << std::get<0>(info.param) << (std::get<1>(info.param) ? "_MT" : "_ST"); 144 | return sst.str(); 145 | }); 146 | 147 | TEST_P(AlignmentTest, test) { 148 | const double t_tol = 0.05; 149 | const double r_tol = 1.0 * M_PI / 180.0; 150 | 151 | pcl::Registration::Ptr reg = create_reg(); 152 | if (reg == nullptr) { 153 | std::cout << "[ ] SKIP TEST" << std::endl; 154 | return; 155 | } 156 | 157 | // forward test 158 | auto aligned = pcl::make_shared>(); 159 | reg->setInputTarget(target); 160 | reg->setInputSource(source); 161 | reg->align(*aligned); 162 | 163 | Eigen::Vector2f errors = pose_error(reg->getFinalTransformation()); 164 | EXPECT_LT(errors[0], t_tol) << "FORWARD TEST"; 165 | EXPECT_LT(errors[1], r_tol) << "FORWARD TEST"; 166 | EXPECT_TRUE(reg->hasConverged()) << "FORWARD TEST"; 167 | 168 | // backward test 169 | reg->setInputTarget(source); 170 | reg->setInputSource(target); 171 | reg->align(*aligned); 172 | 173 | errors = pose_error(reg->getFinalTransformation().inverse()); 174 | EXPECT_LT(errors[0], t_tol) << "BACKWARD TEST"; 175 | EXPECT_LT(errors[1], r_tol) << "BACKWARD TEST"; 176 | EXPECT_TRUE(reg->hasConverged()) << "BACKWARD TEST"; 177 | 178 | // swap and set source 179 | reg = create_reg(); 180 | reg->setInputSource(target); 181 | swap_source_and_target(reg); 182 | reg->setInputSource(source); 183 | reg->align(*aligned); 184 | 185 | errors = pose_error(reg->getFinalTransformation()); 186 | EXPECT_LT(errors[0], t_tol) << "SWAP AND SET SOURCE TEST"; 187 | EXPECT_LT(errors[1], r_tol) << "SWAP AND SET SOURCE TEST"; 188 | EXPECT_TRUE(reg->hasConverged()) << "SWAP AND SET SOURCE TEST"; 189 | 190 | // swap and set target 191 | reg = create_reg(); 192 | reg->setInputTarget(source); 193 | swap_source_and_target(reg); // source:target, target:source 194 | reg->setInputTarget(target); 195 | reg->align(*aligned); 196 | 197 | errors = pose_error(reg->getFinalTransformation()); 198 | EXPECT_LT(errors[0], t_tol) << "SWAP AND SET TARGET TEST"; 199 | EXPECT_LT(errors[1], r_tol) << "SWAP AND SET TARGET TEST"; 200 | EXPECT_TRUE(reg->hasConverged()) << "SWAP AND SET TARGET TEST"; 201 | } 202 | 203 | int main(int argc, char** argv) { 204 | GICPTestBase::data_directory = argv[1]; 205 | testing::InitGoogleTest(&argc, argv); 206 | return RUN_ALL_TESTS(); 207 | } 208 | -------------------------------------------------------------------------------- /include/litamin2/ceres_cost/gicp_cost.hpp: -------------------------------------------------------------------------------- 1 | #ifndef GICP_COST_HPP 2 | #define GICP_COST_HPP 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include "litamin2/ceres_cost/PoseSE3Parameterization.hpp" 10 | // L^T*error 11 | struct GICP_FACTOR { 12 | GICP_FACTOR(Eigen::Vector3d p_mean, Eigen::Vector3d q_mean, Eigen::Matrix3d p_cov, Eigen::Matrix3d q_cov) : p_mean_(p_mean), q_mean_(q_mean), p_cov_(p_cov), q_cov_(q_cov) {} 13 | 14 | template 15 | bool operator()(const T* const q, const T* const t, T* residuals) const { 16 | Eigen::Map> residuals_map(residuals); 17 | Eigen::Matrix p_m(p_mean_.cast()); 18 | Eigen::Matrix q_m(q_mean_.cast()); 19 | Eigen::Matrix p_c = p_cov_.cast(); 20 | Eigen::Matrix q_c = q_cov_.cast(); 21 | 22 | Eigen::Quaternion quat(q); 23 | Eigen::Matrix translation(t); 24 | 25 | Eigen::Matrix mahalanobis = (q_c + quat * p_c * quat.inverse()).inverse(); 26 | Eigen::Matrix LT = mahalanobis.llt().matrixL().transpose(); 27 | residuals_map = LT * (q_m - (quat * p_m + translation)); 28 | 29 | return true; 30 | } 31 | 32 | static ceres::CostFunction* Create(Eigen::Vector3d p_mean_, Eigen::Vector3d q_mean_, Eigen::Matrix3d p_cov_, Eigen::Matrix3d q_cov_) { 33 | // 分别是残差，q，t的维度 34 | return (new ceres::AutoDiffCostFunction(new GICP_FACTOR(p_mean_, q_mean_, p_cov_, q_cov_))); 35 | } 36 | 37 | Eigen::Vector3d p_mean_, q_mean_; 38 | Eigen::Matrix3d p_cov_, q_cov_; 39 | }; 40 | 41 | class GICPAnalyticCostFunction : public ceres::SizedCostFunction<3, 7> { 42 | public: 43 | GICPAnalyticCostFunction(Eigen::Vector3d p_mean, Eigen::Vector3d q_mean, Eigen::Matrix3d p_cov, Eigen::Matrix3d q_cov) 44 | : p_mean_(p_mean), 45 | q_mean_(q_mean), 46 | p_cov_(p_cov), 47 | q_cov_(q_cov) {} 48 | virtual ~GICPAnalyticCostFunction() {} 49 | // parameters是[0,0,0,1,x,y,z] 50 | virtual bool Evaluate(double const* const* parameters, double* residuals, double** jacobians) const { 51 | // 默认已经归一化 52 | Eigen::Quaterniond q_last_curr(parameters[0]); 53 | Eigen::Vector3d t_last_curr(parameters[0] + 4); 54 | Eigen::Matrix3d LT = (q_cov_ + q_last_curr * q_cov_ * q_last_curr.inverse()).inverse().llt().matrixL().transpose(); 55 | Eigen::Map residuals_map(residuals); 56 | Eigen::Vector3d p_mean_trans = q_last_curr * p_mean_ + t_last_curr; 57 | residuals_map = LT * (q_mean_ - p_mean_trans); 58 | 59 | if (jacobians != NULL) { 60 | if (jacobians[0] != NULL) { 61 | Eigen::Map> J_se3(jacobians[0]); 62 | J_se3.setZero(); 63 | Eigen::Matrix dp_by_se3; 64 | dp_by_se3.block<3, 3>(0, 0) = skew(p_mean_trans); 65 | dp_by_se3.block<3, 3>(0, 3) = -Eigen::Matrix3d::Identity(); 66 | J_se3.block<3, 6>(0, 0) = LT * dp_by_se3; 67 | } 68 | } 69 | return true; 70 | } 71 | 72 | Eigen::Vector3d p_mean_, q_mean_; 73 | Eigen::Matrix3d p_cov_, q_cov_; 74 | }; 75 | 76 | // 弃用 77 | // 精度不高，而且耗时 78 | // Z是R*Sigma_i*RT对李代数的雅可比矩阵，来自于d2d-ndt，具体如何计算，我不清楚 79 | class GICPDoubleAnalyticCostFunction : public ceres::SizedCostFunction<3, 7> { 80 | public: 81 | GICPDoubleAnalyticCostFunction(Eigen::Vector3d p_mean, Eigen::Vector3d q_mean, Eigen::Matrix3d p_cov, Eigen::Matrix3d q_cov) 82 | : p_mean_(p_mean), 83 | q_mean_(q_mean), 84 | p_cov_(p_cov), 85 | q_cov_(q_cov) {} 86 | virtual ~GICPDoubleAnalyticCostFunction() {} 87 | // parameters是[0,0,0,1,x,y,z] 88 | virtual bool Evaluate(double const* const* parameters, double* residuals, double** jacobians) const { 89 | // 默认已经归一化 90 | Eigen::Quaterniond q_last_curr(parameters[0]); 91 | Eigen::Vector3d t_last_curr(parameters[0] + 4); 92 | Eigen::Matrix3d mahalanobis = (q_cov_ + q_last_curr * q_cov_ * q_last_curr.inverse()).inverse(); 93 | // Eigen::Matrix3d LT = mahalanobis.llt().matrixL().transpose(); 94 | Eigen::Map residuals_map(residuals); 95 | Eigen::Vector3d p_mean_trans = q_last_curr * p_mean_ + t_last_curr; 96 | residuals_map = mahalanobis * (q_mean_ - p_mean_trans); 97 | 98 | if (jacobians != NULL) { 99 | if (jacobians[0] != NULL) { 100 | Eigen::Map> J_se3(jacobians[0]); 101 | J_se3.setZero(); 102 | Eigen::Matrix dp_by_se3; 103 | Eigen::Matrix BZBU(Eigen::Matrix::Zero()); 104 | dp_by_se3.block<3, 3>(0, 0) = skew(p_mean_trans); 105 | dp_by_se3.block<3, 3>(0, 3) = -Eigen::Matrix3d::Identity(); 106 | Eigen::Matrix3d Z1, Z2, Z3; 107 | Z1 << 0, -p_cov_(0, 2), p_cov_(0, 1), -p_cov_(0, 2), -2 * p_cov_(1, 2), -p_cov_(2, 2) + p_cov_(1, 1), p_cov_(0, 1), -p_cov_(2, 2) + p_cov_(1, 1), 2 * p_cov_(1, 2); 108 | Z2 << 2 * p_cov_(0, 2), p_cov_(1, 2), -p_cov_(0, 0) + p_cov_(2, 2), p_cov_(1, 2), 0, -p_cov_(0, 1), -p_cov_(0, 0) + p_cov_(2, 2), -p_cov_(0, 1), -2 * p_cov_(0, 2); 109 | Z3 << -2 * p_cov_(0, 1), -p_cov_(1, 1) + p_cov_(0, 0), -p_cov_(1, 2), -p_cov_(1, 1) + p_cov_(0, 0), 2 * p_cov_(0, 1), p_cov_(0, 2), -p_cov_(1, 2), p_cov_(0, 2), 0; 110 | BZBU.block<3,1>(0,0) = mahalanobis * Z1 * mahalanobis * p_mean_trans; 111 | BZBU.block<3,1>(0,1) = mahalanobis * Z2 * mahalanobis * p_mean_trans; 112 | BZBU.block<3,1>(0,2) = mahalanobis * Z3 * mahalanobis * p_mean_trans; 113 | J_se3.block<3, 6>(0, 0) = -BZBU + mahalanobis * dp_by_se3; 114 | } 115 | } 116 | return true; 117 | } 118 | 119 | Eigen::Vector3d p_mean_, q_mean_; 120 | Eigen::Matrix3d p_cov_, q_cov_; 121 | }; 122 | 123 | // 正常工作，但精度并不是特别的高 124 | // TODO 删除无效代码 125 | class ICPAnalyticCostFunction : public ceres::SizedCostFunction<3, 7> { 126 | public: 127 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 128 | ICPAnalyticCostFunction(Eigen::Vector3d p_mean, Eigen::Vector3d q_mean, Eigen::Matrix3d p_cov, Eigen::Matrix3d q_cov) 129 | : p_mean_(p_mean), 130 | q_mean_(q_mean), 131 | p_cov_(p_cov), 132 | q_cov_(q_cov) {} 133 | virtual ~ICPAnalyticCostFunction() {} 134 | // parameters是[0,0,0,1,x,y,z] 135 | virtual bool Evaluate(double const* const* parameters, double* residuals, double** jacobians) const { 136 | // 默认已经归一化 137 | Eigen::Quaterniond q_last_curr(parameters[0]); 138 | Eigen::Vector3d t_last_curr(parameters[0] + 4); 139 | // Eigen::Matrix3d mahalanobis = q_cov_ + q_last_curr * q_cov_ * q_last_curr.inverse(); 140 | // Eigen::Matrix3d LT = mahalanobis.llt().matrixL().transpose(); 141 | Eigen::Map residuals_map(residuals); 142 | Eigen::Vector3d p_mean_trans = q_last_curr * p_mean_ + t_last_curr; 143 | residuals_map = q_mean_ - p_mean_trans; 144 | 145 | if (jacobians != NULL) { 146 | if (jacobians[0] != NULL) { 147 | Eigen::Map> J_se3(jacobians[0]); 148 | J_se3.setZero(); 149 | Eigen::Matrix dp_by_se3; 150 | dp_by_se3.block<3, 3>(0, 0) = skew(p_mean_trans); 151 | dp_by_se3.block<3, 3>(0, 3) = -Eigen::Matrix3d::Identity(); 152 | J_se3.block<3, 6>(0, 0) = dp_by_se3; 153 | } 154 | } 155 | return true; 156 | } 157 | 158 | Eigen::Vector3d p_mean_, q_mean_; 159 | Eigen::Matrix3d p_cov_, q_cov_; 160 | }; 161 | #endif -------------------------------------------------------------------------------- /src/fast_gicp/cuda/compute_derivatives.cu: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include 4 | #include 5 | 6 | #include 7 | #include 8 | #include 9 | #include 10 | 11 | #include 12 | 13 | namespace fast_gicp { 14 | namespace cuda { 15 | 16 | namespace { 17 | 18 | template 19 | struct compute_derivatives_kernel { 20 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 21 | 22 | compute_derivatives_kernel( 23 | const GaussianVoxelMap& voxelmap, 24 | const thrust::device_vector& src_means, 25 | const thrust::device_vector& src_covs, 26 | const thrust::device_ptr& x_eval_ptr, 27 | const thrust::device_ptr& x_ptr) 28 | : trans_eval_ptr(x_eval_ptr), 29 | trans_ptr(x_ptr), 30 | src_means_ptr(src_means.data()), 31 | src_covs_ptr(src_covs.data()), 32 | voxel_num_points_ptr(voxelmap.num_points.data()), 33 | voxel_means_ptr(voxelmap.voxel_means.data()), 34 | voxel_covs_ptr(voxelmap.voxel_covs.data()) {} 35 | 36 | // skew symmetric matrix 37 | __host__ __device__ Eigen::Matrix3f skew_symmetric(const Eigen::Vector3f& x) const { 38 | Eigen::Matrix3f skew = Eigen::Matrix3f::Zero(); 39 | skew(0, 1) = -x[2]; 40 | skew(0, 2) = x[1]; 41 | skew(1, 0) = x[2]; 42 | skew(1, 2) = -x[0]; 43 | skew(2, 0) = -x[1]; 44 | skew(2, 1) = x[0]; 45 | 46 | return skew; 47 | } 48 | 49 | // calculate derivatives 50 | __host__ __device__ T operator()(const thrust::pair& correspondence) const { 51 | const Eigen::Vector3f& mean_A = thrust::raw_pointer_cast(src_means_ptr)[correspondence.first]; 52 | const Eigen::Matrix3f& cov_A = thrust::raw_pointer_cast(src_covs_ptr)[correspondence.first]; 53 | 54 | if (correspondence.second < 0) { 55 | return thrust::make_tuple(0.0f, Eigen::Matrix::Zero().eval(), Eigen::Matrix::Zero().eval()); 56 | } 57 | 58 | int num_points = thrust::raw_pointer_cast(voxel_num_points_ptr)[correspondence.second]; 59 | const Eigen::Vector3f& mean_B = thrust::raw_pointer_cast(voxel_means_ptr)[correspondence.second]; 60 | const Eigen::Matrix3f& cov_B = thrust::raw_pointer_cast(voxel_covs_ptr)[correspondence.second]; 61 | 62 | if (num_points <= 0) { 63 | return thrust::make_tuple(0.0f, Eigen::Matrix::Zero().eval(), Eigen::Matrix::Zero().eval()); 64 | } 65 | 66 | const auto& trans_eval = *thrust::raw_pointer_cast(trans_eval_ptr); 67 | const auto& trans = *thrust::raw_pointer_cast(trans_ptr); 68 | 69 | Eigen::Matrix3f R_eval = trans_eval.linear(); 70 | Eigen::Matrix3f R = trans.linear(); 71 | Eigen::Vector3f t = trans.translation(); 72 | 73 | const Eigen::Vector3f transed_mean_A = R * mean_A + t; 74 | 75 | Eigen::Matrix3f RCR = R_eval * cov_A * R_eval.transpose(); 76 | Eigen::Matrix3f RCR_inv = (cov_B + RCR).inverse(); 77 | 78 | float w = sqrtf(num_points); 79 | Eigen::Vector3f error = mean_B - transed_mean_A; 80 | 81 | Eigen::Matrix dtdx0; 82 | dtdx0.block<3, 3>(0, 0) = skew_symmetric(transed_mean_A); 83 | dtdx0.block<3, 3>(0, 3) = -Eigen::Matrix3f::Identity(); 84 | 85 | Eigen::Matrix J = dtdx0; 86 | 87 | Eigen::Matrix H = w * J.transpose() * RCR_inv * J; 88 | Eigen::Matrix b = w * J.transpose() * RCR_inv * error; 89 | 90 | float err = w * error.transpose() * RCR_inv * error; 91 | return thrust::make_tuple(err, H, b); 92 | } 93 | 94 | thrust::device_ptr trans_eval_ptr; 95 | thrust::device_ptr trans_ptr; 96 | 97 | thrust::device_ptr src_means_ptr; 98 | thrust::device_ptr src_covs_ptr; 99 | 100 | thrust::device_ptr voxel_num_points_ptr; 101 | thrust::device_ptr voxel_means_ptr; 102 | thrust::device_ptr voxel_covs_ptr; 103 | }; 104 | 105 | template <> 106 | __host__ __device__ float compute_derivatives_kernel::operator()(const thrust::pair& correspondence) const { 107 | const Eigen::Vector3f& mean_A = thrust::raw_pointer_cast(src_means_ptr)[correspondence.first]; 108 | const Eigen::Matrix3f& cov_A = thrust::raw_pointer_cast(src_covs_ptr)[correspondence.first]; 109 | const int voxel_index = correspondence.second; 110 | 111 | if (voxel_index < 0) { 112 | return 0.0f; 113 | } 114 | 115 | int num_points = thrust::raw_pointer_cast(voxel_num_points_ptr)[voxel_index]; 116 | const Eigen::Vector3f& mean_B = thrust::raw_pointer_cast(voxel_means_ptr)[voxel_index]; 117 | const Eigen::Matrix3f& cov_B = thrust::raw_pointer_cast(voxel_covs_ptr)[voxel_index]; 118 | 119 | const auto& trans_eval = *thrust::raw_pointer_cast(trans_eval_ptr); 120 | const auto& trans = *thrust::raw_pointer_cast(trans_ptr); 121 | 122 | Eigen::Matrix3f R_eval = trans_eval.linear(); 123 | Eigen::Matrix3f R = trans.linear(); 124 | Eigen::Vector3f t = trans.translation(); 125 | 126 | const Eigen::Vector3f transed_mean_A = R * mean_A + t; 127 | 128 | Eigen::Matrix3f RCR = R_eval * cov_A * R_eval.transpose(); 129 | Eigen::Matrix3f RCR_inv = (cov_B + RCR).inverse(); 130 | 131 | float w = sqrtf(num_points); 132 | Eigen::Vector3f error = mean_B - transed_mean_A; 133 | 134 | return w * error.transpose() * RCR_inv * error; 135 | } 136 | 137 | struct sum_errors_kernel { 138 | template 139 | __host__ __device__ Tuple operator()(const Tuple& lhs, const Tuple& rhs) { 140 | return thrust::make_tuple(thrust::get<0>(lhs) + thrust::get<0>(rhs), thrust::get<1>(lhs) + thrust::get<1>(rhs), thrust::get<2>(lhs) + thrust::get<2>(rhs)); 141 | } 142 | }; 143 | 144 | template <> 145 | __host__ __device__ float sum_errors_kernel::operator()(const float& lhs, const float& rhs) { 146 | return lhs + rhs; 147 | } 148 | 149 | } // namespace 150 | 151 | double compute_derivatives( 152 | const thrust::device_vector& src_points, 153 | const thrust::device_vector& src_covs, 154 | const GaussianVoxelMap& voxelmap, 155 | const thrust::device_vector>& voxel_correspondences, 156 | const thrust::device_ptr& linearized_x_ptr, 157 | const thrust::device_ptr& x_ptr, 158 | Eigen::Matrix* H, 159 | Eigen::Matrix* b) { 160 | if (H == nullptr || b == nullptr) { 161 | float sum_errors = thrust::transform_reduce( 162 | voxel_correspondences.begin(), 163 | voxel_correspondences.end(), 164 | compute_derivatives_kernel(voxelmap, src_points, src_covs, linearized_x_ptr, x_ptr), 165 | 0.0f, 166 | sum_errors_kernel()); 167 | 168 | return sum_errors; 169 | } 170 | 171 | auto sum_errors = thrust::transform_reduce( 172 | voxel_correspondences.begin(), 173 | voxel_correspondences.end(), 174 | compute_derivatives_kernel, Eigen::Matrix>>(voxelmap, src_points, src_covs, linearized_x_ptr, x_ptr), 175 | thrust::make_tuple(0.0f, Eigen::Matrix::Zero().eval(), Eigen::Matrix::Zero().eval()), 176 | sum_errors_kernel()); 177 | 178 | if (H && b) { 179 | *H = thrust::get<1>(sum_errors).cast(); 180 | *b = thrust::get<2>(sum_errors).cast(); 181 | } 182 | 183 | return thrust::get<0>(sum_errors); 184 | } 185 | } // namespace cuda 186 | } // namespace fast_gicp 187 | -------------------------------------------------------------------------------- /include/fast_gicp/gicp/impl/fast_vgicp_cuda_impl.hpp: -------------------------------------------------------------------------------- 1 | #ifndef FAST_GICP_FAST_VGICP_CUDA_IMPL_HPP 2 | #define FAST_GICP_FAST_VGICP_CUDA_IMPL_HPP 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | #include 9 | #include 10 | #include 11 | #include 12 | 13 | #include 14 | #include 15 | 16 | #include 17 | #include 18 | 19 | namespace fast_gicp { 20 | 21 | template 22 | FastVGICPCuda::FastVGICPCuda() : LsqRegistration() { 23 | this->reg_name_ = "FastVGICPCuda"; 24 | k_correspondences_ = 20; 25 | voxel_resolution_ = 1.0; 26 | regularization_method_ = RegularizationMethod::PLANE; 27 | neighbor_search_method_ = NearestNeighborMethod::CPU_PARALLEL_KDTREE; 28 | 29 | vgicp_cuda_.reset(new cuda::FastVGICPCudaCore()); 30 | vgicp_cuda_->set_resolution(voxel_resolution_); 31 | vgicp_cuda_->set_kernel_params(0.5, 3.0); 32 | } 33 | 34 | template 35 | FastVGICPCuda::~FastVGICPCuda() {} 36 | 37 | template 38 | void FastVGICPCuda::setCorrespondenceRandomness(int k) {} 39 | 40 | template 41 | void FastVGICPCuda::setResolution(double resolution) { 42 | vgicp_cuda_->set_resolution(resolution); 43 | } 44 | 45 | template 46 | void FastVGICPCuda::setKernelWidth(double kernel_width, double max_dist) { 47 | if (max_dist <= 0.0) { 48 | max_dist = kernel_width * 5.0; 49 | } 50 | vgicp_cuda_->set_kernel_params(kernel_width, max_dist); 51 | } 52 | 53 | template 54 | void FastVGICPCuda::setRegularizationMethod(RegularizationMethod method) { 55 | regularization_method_ = method; 56 | } 57 | 58 | template 59 | void FastVGICPCuda::setNeighborSearchMethod(NeighborSearchMethod method, double radius) { 60 | vgicp_cuda_->set_neighbor_search_method(method, radius); 61 | } 62 | 63 | template 64 | void FastVGICPCuda::setNearestNeighborSearchMethod(NearestNeighborMethod method) { 65 | neighbor_search_method_ = method; 66 | } 67 | 68 | template 69 | void FastVGICPCuda::swapSourceAndTarget() { 70 | vgicp_cuda_->swap_source_and_target(); 71 | input_.swap(target_); 72 | } 73 | 74 | template 75 | void FastVGICPCuda::clearSource() { 76 | input_.reset(); 77 | } 78 | 79 | template 80 | void FastVGICPCuda::clearTarget() { 81 | target_.reset(); 82 | } 83 | 84 | template 85 | void FastVGICPCuda::setInputSource(const PointCloudSourceConstPtr& cloud) { 86 | // the input cloud is the same as the previous one 87 | if(cloud == input_) { 88 | return; 89 | } 90 | 91 | pcl::Registration::setInputSource(cloud); 92 | 93 | std::vector> points(cloud->size()); 94 | std::transform(cloud->begin(), cloud->end(), points.begin(), [=](const PointSource& pt) { return pt.getVector3fMap(); }); 95 | 96 | vgicp_cuda_->set_source_cloud(points); 97 | switch(neighbor_search_method_) { 98 | case NearestNeighborMethod::CPU_PARALLEL_KDTREE: { 99 | std::vector neighbors = find_neighbors_parallel_kdtree(k_correspondences_, cloud); 100 | vgicp_cuda_->set_source_neighbors(k_correspondences_, neighbors); 101 | vgicp_cuda_->calculate_source_covariances(regularization_method_); 102 | } break; 103 | case NearestNeighborMethod::GPU_BRUTEFORCE: 104 | vgicp_cuda_->find_source_neighbors(k_correspondences_); 105 | vgicp_cuda_->calculate_source_covariances(regularization_method_); 106 | break; 107 | case NearestNeighborMethod::GPU_RBF_KERNEL: 108 | vgicp_cuda_->calculate_source_covariances_rbf(regularization_method_); 109 | break; 110 | } 111 | } 112 | 113 | template 114 | void FastVGICPCuda::setInputTarget(const PointCloudTargetConstPtr& cloud) { 115 | // the input cloud is the same as the previous one 116 | if(cloud == target_) { 117 | return; 118 | } 119 | 120 | pcl::Registration::setInputTarget(cloud); 121 | 122 | std::vector> points(cloud->size()); 123 | std::transform(cloud->begin(), cloud->end(), points.begin(), [=](const PointTarget& pt) { return pt.getVector3fMap(); }); 124 | 125 | vgicp_cuda_->set_target_cloud(points); 126 | switch(neighbor_search_method_) { 127 | case NearestNeighborMethod::CPU_PARALLEL_KDTREE: { 128 | std::vector neighbors = find_neighbors_parallel_kdtree(k_correspondences_, cloud); 129 | vgicp_cuda_->set_target_neighbors(k_correspondences_, neighbors); 130 | vgicp_cuda_->calculate_target_covariances(regularization_method_); 131 | } break; 132 | case NearestNeighborMethod::GPU_BRUTEFORCE: 133 | vgicp_cuda_->find_target_neighbors(k_correspondences_); 134 | vgicp_cuda_->calculate_target_covariances(regularization_method_); 135 | break; 136 | case NearestNeighborMethod::GPU_RBF_KERNEL: 137 | vgicp_cuda_->calculate_target_covariances_rbf(regularization_method_); 138 | break; 139 | } 140 | vgicp_cuda_->create_target_voxelmap(); 141 | } 142 | 143 | template 144 | void FastVGICPCuda::computeTransformation(PointCloudSource& output, const Matrix4& guess) { 145 | vgicp_cuda_->set_resolution(this->voxel_resolution_); 146 | 147 | LsqRegistration::computeTransformation(output, guess); 148 | } 149 | 150 | template 151 | template 152 | std::vector FastVGICPCuda::find_neighbors_parallel_kdtree(int k, typename pcl::PointCloud::ConstPtr cloud) const { 153 | pcl::search::KdTree kdtree; 154 | kdtree.setInputCloud(cloud); 155 | std::vector neighbors(cloud->size() * k); 156 | 157 | #pragma omp parallel for schedule(guided, 8) 158 | for(int i = 0; i < cloud->size(); i++) { 159 | std::vector k_indices; 160 | std::vector k_sq_distances; 161 | kdtree.nearestKSearch(cloud->at(i), k, k_indices, k_sq_distances); 162 | 163 | std::copy(k_indices.begin(), k_indices.end(), neighbors.begin() + i * k); 164 | } 165 | 166 | return neighbors; 167 | } 168 | 169 | template 170 | double FastVGICPCuda::linearize(const Eigen::Isometry3d& trans, Eigen::Matrix* H, Eigen::Matrix* b) { 171 | vgicp_cuda_->update_correspondences(trans); 172 | return vgicp_cuda_->compute_error(trans, H, b); 173 | } 174 | 175 | template 176 | double FastVGICPCuda::compute_error(const Eigen::Isometry3d& trans) { 177 | return vgicp_cuda_->compute_error(trans, nullptr, nullptr); 178 | } 179 | 180 | } // namespace fast_gicp 181 | 182 | #endif 183 | -------------------------------------------------------------------------------- /include/litamin2/impl/litamin2_impl.hpp: -------------------------------------------------------------------------------- 1 | #ifndef FAST_GICP_LITAMIN2_IMPL_HPP 2 | #define FAST_GICP_LITAMIN2_IMPL_HPP 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | #include 9 | #include 10 | #include 11 | #include 12 | 13 | #include 14 | #include 15 | 16 | namespace fast_gicp { 17 | 18 | template 19 | LiTAMIN2::LiTAMIN2() : LiTAMIN2() { 20 | this->reg_name_ = "LiTAMIN2"; 21 | 22 | voxel_resolution_ = 3.0; 23 | search_method_ = NeighborSearchMethod::DIRECT1; 24 | voxel_mode_ = VoxelAccumulationMode::ADDITIVE; 25 | } 26 | 27 | template 28 | LiTAMIN2::~LiTAMIN2() {} 29 | 30 | template 31 | void LiTAMIN2::setResolution(double resolution) { 32 | voxel_resolution_ = resolution; 33 | } 34 | 35 | template 36 | void LiTAMIN2::setNeighborSearchMethod(NeighborSearchMethod method) { 37 | search_method_ = method; 38 | } 39 | 40 | template 41 | void LiTAMIN2::setVoxelAccumulationMode(VoxelAccumulationMode mode) { 42 | voxel_mode_ = mode; 43 | } 44 | 45 | template 46 | void LiTAMIN2::swapSourceAndTarget() { 47 | input_.swap(target_); 48 | source_kdtree_.swap(target_kdtree_); 49 | source_covs_.swap(target_covs_); 50 | voxelmap_.reset(); 51 | voxel_correspondences_.clear(); 52 | voxel_mahalanobis_.clear(); 53 | } 54 | 55 | template 56 | void LiTAMIN2::setInputTarget(const PointCloudTargetConstPtr& cloud) { 57 | // if (target_ == cloud) { 58 | // return; 59 | // } 60 | 61 | FastGICP::setInputTarget(cloud); 62 | voxelmap_.reset(); 63 | } 64 | 65 | template 66 | void LiTAMIN2::computeTransformation(PointCloudSource& output, const Matrix4& guess) { 67 | voxelmap_.reset(); 68 | 69 | FastGICP::computeTransformation(output, guess); 70 | } 71 | 72 | template 73 | void LiTAMIN2::update_correspondences(const Eigen::Isometry3d& trans) { 74 | voxel_correspondences_.clear(); 75 | auto offsets = neighbor_offsets(search_method_); 76 | 77 | std::vector>> corrs(num_threads_); 78 | for (auto& c : corrs) { 79 | c.reserve((input_->size() * offsets.size()) / num_threads_); 80 | } 81 | 82 | #pragma omp parallel for num_threads(num_threads_) schedule(guided, 8) 83 | for (int i = 0; i < input_->size(); i++) { 84 | const Eigen::Vector4d mean_A = input_->at(i).getVector4fMap().template cast(); 85 | Eigen::Vector4d transed_mean_A = trans * mean_A; 86 | Eigen::Vector3i coord = voxelmap_->voxel_coord(transed_mean_A); 87 | 88 | for (const auto& offset : offsets) { 89 | auto voxel = voxelmap_->lookup_voxel(coord + offset); 90 | if (voxel != nullptr) { 91 | corrs[omp_get_thread_num()].push_back(std::make_pair(i, voxel)); 92 | } 93 | } 94 | } 95 | 96 | voxel_correspondences_.reserve(input_->size() * offsets.size()); 97 | for (const auto& c : corrs) { 98 | voxel_correspondences_.insert(voxel_correspondences_.end(), c.begin(), c.end()); 99 | } 100 | 101 | // precompute combined covariances 102 | voxel_mahalanobis_.resize(voxel_correspondences_.size()); 103 | 104 | #pragma omp parallel for num_threads(num_threads_) schedule(guided, 8) 105 | for (int i = 0; i < voxel_correspondences_.size(); i++) { 106 | const auto& corr = voxel_correspondences_[i]; 107 | const auto& cov_A = source_covs_[corr.first]; 108 | const auto& cov_B = corr.second->cov; 109 | 110 | Eigen::Matrix4d RCR = cov_B + trans.matrix() * cov_A * trans.matrix().transpose(); 111 | RCR(3, 3) = 1.0; 112 | 113 | voxel_mahalanobis_[i] = RCR.inverse(); 114 | voxel_mahalanobis_[i](3, 3) = 0.0; 115 | } 116 | } 117 | 118 | template 119 | double LiTAMIN2::linearize(const Eigen::Isometry3d& trans, Eigen::Matrix* H, Eigen::Matrix* b) { 120 | if (voxelmap_ == nullptr) { 121 | voxelmap_.reset(new GaussianVoxelMap(voxel_resolution_, voxel_mode_)); 122 | voxelmap_->create_voxelmap(*target_, target_covs_); 123 | } 124 | 125 | update_correspondences(trans); 126 | 127 | double sum_errors = 0.0; 128 | std::vector, Eigen::aligned_allocator>> Hs(num_threads_); 129 | std::vector, Eigen::aligned_allocator>> bs(num_threads_); 130 | for (int i = 0; i < num_threads_; i++) { 131 | Hs[i].setZero(); 132 | bs[i].setZero(); 133 | } 134 | 135 | #pragma omp parallel for num_threads(num_threads_) reduction(+ : sum_errors) schedule(guided, 8) 136 | for (int i = 0; i < voxel_correspondences_.size(); i++) { 137 | const auto& corr = voxel_correspondences_[i]; 138 | auto target_voxel = corr.second; 139 | 140 | const Eigen::Vector4d mean_A = input_->at(corr.first).getVector4fMap().template cast(); 141 | const auto& cov_A = source_covs_[corr.first]; 142 | 143 | const Eigen::Vector4d mean_B = corr.second->mean; 144 | const auto& cov_B = corr.second->cov; 145 | 146 | const Eigen::Vector4d transed_mean_A = trans * mean_A; 147 | const Eigen::Vector4d error = mean_B - transed_mean_A; 148 | 149 | double w = std::sqrt(target_voxel->num_points); 150 | sum_errors += w * error.transpose() * voxel_mahalanobis_[i] * error; 151 | 152 | if (H == nullptr || b == nullptr) { 153 | continue; 154 | } 155 | 156 | Eigen::Matrix dtdx0 = Eigen::Matrix::Zero(); 157 | dtdx0.block<3, 3>(0, 0) = skewd(transed_mean_A.head<3>()); 158 | dtdx0.block<3, 3>(0, 3) = -Eigen::Matrix3d::Identity(); 159 | 160 | Eigen::Matrix jlossexp = dtdx0; 161 | 162 | Eigen::Matrix Hi = w * jlossexp.transpose() * voxel_mahalanobis_[i] * jlossexp; 163 | Eigen::Matrix bi = w * jlossexp.transpose() * voxel_mahalanobis_[i] * error; 164 | 165 | int thread_num = omp_get_thread_num(); 166 | Hs[thread_num] += Hi; 167 | bs[thread_num] += bi; 168 | } 169 | 170 | if (H && b) { 171 | H->setZero(); 172 | b->setZero(); 173 | for (int i = 0; i < num_threads_; i++) { 174 | (*H) += Hs[i]; 175 | (*b) += bs[i]; 176 | } 177 | } 178 | 179 | return sum_errors; 180 | } 181 | 182 | template 183 | double LiTAMIN2::compute_error(const Eigen::Isometry3d& trans) { 184 | double sum_errors = 0.0; 185 | #pragma omp parallel for num_threads(num_threads_) reduction(+ : sum_errors) 186 | for (int i = 0; i < voxel_correspondences_.size(); i++) { 187 | const auto& corr = voxel_correspondences_[i]; 188 | auto target_voxel = corr.second; 189 | 190 | const Eigen::Vector4d mean_A = input_->at(corr.first).getVector4fMap().template cast(); 191 | const auto& cov_A = source_covs_[corr.first]; 192 | 193 | const Eigen::Vector4d mean_B = corr.second->mean; 194 | const auto& cov_B = corr.second->cov; 195 | 196 | const Eigen::Vector4d transed_mean_A = trans * mean_A; 197 | const Eigen::Vector4d error = mean_B - transed_mean_A; 198 | 199 | double w = std::sqrt(target_voxel->num_points); 200 | sum_errors += w * error.transpose() * voxel_mahalanobis_[i] * error; 201 | } 202 | 203 | return sum_errors; 204 | } 205 | 206 | } // namespace fast_gicp 207 | 208 | #endif 209 | -------------------------------------------------------------------------------- /include/fast_gicp/gicp/impl/fast_vgicp_impl.hpp: -------------------------------------------------------------------------------- 1 | #ifndef FAST_GICP_FAST_VGICP_IMPL_HPP 2 | #define FAST_GICP_FAST_VGICP_IMPL_HPP 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | #include 9 | #include 10 | #include 11 | #include 12 | 13 | #include 14 | #include 15 | 16 | namespace fast_gicp { 17 | 18 | template 19 | FastVGICP::FastVGICP() : FastGICP() { 20 | this->reg_name_ = "FastVGICP"; 21 | 22 | voxel_resolution_ = 1.0; 23 | search_method_ = NeighborSearchMethod::DIRECT1; 24 | voxel_mode_ = VoxelAccumulationMode::ADDITIVE; 25 | } 26 | 27 | template 28 | FastVGICP::~FastVGICP() {} 29 | 30 | template 31 | void FastVGICP::setResolution(double resolution) { 32 | voxel_resolution_ = resolution; 33 | } 34 | 35 | template 36 | void FastVGICP::setNeighborSearchMethod(NeighborSearchMethod method) { 37 | search_method_ = method; 38 | } 39 | 40 | template 41 | void FastVGICP::setVoxelAccumulationMode(VoxelAccumulationMode mode) { 42 | voxel_mode_ = mode; 43 | } 44 | 45 | template 46 | void FastVGICP::swapSourceAndTarget() { 47 | input_.swap(target_); 48 | source_kdtree_.swap(target_kdtree_); 49 | source_covs_.swap(target_covs_); 50 | voxelmap_.reset(); 51 | voxel_correspondences_.clear(); 52 | voxel_mahalanobis_.clear(); 53 | } 54 | 55 | template 56 | void FastVGICP::setInputTarget(const PointCloudTargetConstPtr& cloud) { 57 | // if (target_ == cloud) { 58 | // return; 59 | // } 60 | 61 | FastGICP::setInputTarget(cloud); 62 | voxelmap_.reset(); 63 | } 64 | 65 | template 66 | void FastVGICP::computeTransformation(PointCloudSource& output, const Matrix4& guess) { 67 | voxelmap_.reset(); 68 | 69 | FastGICP::computeTransformation(output, guess); 70 | } 71 | 72 | template 73 | void FastVGICP::update_correspondences(const Eigen::Isometry3d& trans) { 74 | voxel_correspondences_.clear(); 75 | auto offsets = neighbor_offsets(search_method_); 76 | 77 | std::vector>> corrs(num_threads_); 78 | for (auto& c : corrs) { 79 | c.reserve((input_->size() * offsets.size()) / num_threads_); 80 | } 81 | 82 | #pragma omp parallel for num_threads(num_threads_) schedule(guided, 8) 83 | for (int i = 0; i < input_->size(); i++) { 84 | const Eigen::Vector4d mean_A = input_->at(i).getVector4fMap().template cast(); 85 | Eigen::Vector4d transed_mean_A = trans * mean_A; 86 | Eigen::Vector3i coord = voxelmap_->voxel_coord(transed_mean_A); 87 | 88 | for (const auto& offset : offsets) { 89 | auto voxel = voxelmap_->lookup_voxel(coord + offset); 90 | if (voxel != nullptr) { 91 | corrs[omp_get_thread_num()].push_back(std::make_pair(i, voxel)); 92 | } 93 | } 94 | } 95 | 96 | voxel_correspondences_.reserve(input_->size() * offsets.size()); 97 | for (const auto& c : corrs) { 98 | voxel_correspondences_.insert(voxel_correspondences_.end(), c.begin(), c.end()); 99 | } 100 | 101 | // precompute combined covariances 102 | voxel_mahalanobis_.resize(voxel_correspondences_.size()); 103 | 104 | #pragma omp parallel for num_threads(num_threads_) schedule(guided, 8) 105 | for (int i = 0; i < voxel_correspondences_.size(); i++) { 106 | const auto& corr = voxel_correspondences_[i]; 107 | const auto& cov_A = source_covs_[corr.first]; 108 | const auto& cov_B = corr.second->cov; 109 | 110 | Eigen::Matrix4d RCR = cov_B + trans.matrix() * cov_A * trans.matrix().transpose(); 111 | RCR(3, 3) = 1.0; 112 | 113 | voxel_mahalanobis_[i] = RCR.inverse(); 114 | voxel_mahalanobis_[i](3, 3) = 0.0; 115 | } 116 | } 117 | 118 | template 119 | double FastVGICP::linearize(const Eigen::Isometry3d& trans, Eigen::Matrix* H, Eigen::Matrix* b) { 120 | if (voxelmap_ == nullptr) { 121 | voxelmap_.reset(new GaussianVoxelMap(voxel_resolution_, voxel_mode_)); 122 | voxelmap_->create_voxelmap(*target_, target_covs_); 123 | } 124 | 125 | update_correspondences(trans); 126 | 127 | double sum_errors = 0.0; 128 | std::vector, Eigen::aligned_allocator>> Hs(num_threads_); 129 | std::vector, Eigen::aligned_allocator>> bs(num_threads_); 130 | for (int i = 0; i < num_threads_; i++) { 131 | Hs[i].setZero(); 132 | bs[i].setZero(); 133 | } 134 | 135 | #pragma omp parallel for num_threads(num_threads_) reduction(+ : sum_errors) schedule(guided, 8) 136 | for (int i = 0; i < voxel_correspondences_.size(); i++) { 137 | const auto& corr = voxel_correspondences_[i]; 138 | auto target_voxel = corr.second; 139 | 140 | const Eigen::Vector4d mean_A = input_->at(corr.first).getVector4fMap().template cast(); 141 | const auto& cov_A = source_covs_[corr.first]; 142 | 143 | const Eigen::Vector4d mean_B = corr.second->mean; 144 | const auto& cov_B = corr.second->cov; 145 | 146 | const Eigen::Vector4d transed_mean_A = trans * mean_A; 147 | const Eigen::Vector4d error = mean_B - transed_mean_A; 148 | 149 | double w = std::sqrt(target_voxel->num_points); 150 | sum_errors += w * error.transpose() * voxel_mahalanobis_[i] * error; 151 | 152 | if (H == nullptr || b == nullptr) { 153 | continue; 154 | } 155 | 156 | Eigen::Matrix dtdx0 = Eigen::Matrix::Zero(); 157 | dtdx0.block<3, 3>(0, 0) = skewd(transed_mean_A.head<3>()); 158 | dtdx0.block<3, 3>(0, 3) = -Eigen::Matrix3d::Identity(); 159 | 160 | Eigen::Matrix jlossexp = dtdx0; 161 | 162 | Eigen::Matrix Hi = w * jlossexp.transpose() * voxel_mahalanobis_[i] * jlossexp; 163 | Eigen::Matrix bi = w * jlossexp.transpose() * voxel_mahalanobis_[i] * error; 164 | 165 | int thread_num = omp_get_thread_num(); 166 | Hs[thread_num] += Hi; 167 | bs[thread_num] += bi; 168 | } 169 | 170 | if (H && b) { 171 | H->setZero(); 172 | b->setZero(); 173 | for (int i = 0; i < num_threads_; i++) { 174 | (*H) += Hs[i]; 175 | (*b) += bs[i]; 176 | } 177 | } 178 | 179 | return sum_errors; 180 | } 181 | 182 | template 183 | double FastVGICP::compute_error(const Eigen::Isometry3d& trans) { 184 | double sum_errors = 0.0; 185 | #pragma omp parallel for num_threads(num_threads_) reduction(+ : sum_errors) 186 | for (int i = 0; i < voxel_correspondences_.size(); i++) { 187 | const auto& corr = voxel_correspondences_[i]; 188 | auto target_voxel = corr.second; 189 | 190 | const Eigen::Vector4d mean_A = input_->at(corr.first).getVector4fMap().template cast(); 191 | const auto& cov_A = source_covs_[corr.first]; 192 | 193 | const Eigen::Vector4d mean_B = corr.second->mean; 194 | const auto& cov_B = corr.second->cov; 195 | 196 | const Eigen::Vector4d transed_mean_A = trans * mean_A; 197 | const Eigen::Vector4d error = mean_B - transed_mean_A; 198 | 199 | double w = std::sqrt(target_voxel->num_points); 200 | sum_errors += w * error.transpose() * voxel_mahalanobis_[i] * error; 201 | } 202 | 203 | return sum_errors; 204 | } 205 | 206 | } // namespace fast_gicp 207 | 208 | #endif 209 | -------------------------------------------------------------------------------- /include/fast_gicp/gicp/impl/lsq_registration_impl.hpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include 4 | #include 5 | 6 | namespace fast_gicp { 7 | 8 | template 9 | LsqRegistration::LsqRegistration() { 10 | this->reg_name_ = "LsqRegistration"; 11 | max_iterations_ = 64; 12 | rotation_epsilon_ = 2e-3; 13 | transformation_epsilon_ = 5e-4; 14 | 15 | lsq_optimizer_type_ = LSQ_OPTIMIZER_TYPE::LevenbergMarquardt; 16 | lm_debug_print_ = false; 17 | lm_max_iterations_ = 10; 18 | lm_init_lambda_factor_ = 1e-9; 19 | lm_lambda_ = -1.0; 20 | lm_sigma1_ = 1e-6; 21 | lm_sigma2_ = 1e-6; 22 | 23 | final_hessian_.setIdentity(); 24 | } 25 | 26 | template 27 | LsqRegistration::~LsqRegistration() {} 28 | 29 | template 30 | void LsqRegistration::setRotationEpsilon(double eps) { 31 | rotation_epsilon_ = eps; 32 | } 33 | 34 | template 35 | void LsqRegistration::setInitialLambdaFactor(double init_lambda_factor) { 36 | lm_init_lambda_factor_ = init_lambda_factor; 37 | } 38 | template 39 | void LsqRegistration::setLSQType(LSQ_OPTIMIZER_TYPE type) { 40 | lsq_optimizer_type_ = type; 41 | } 42 | template 43 | void LsqRegistration::setDebugPrint(bool lm_debug_print) { 44 | lm_debug_print_ = lm_debug_print; 45 | } 46 | 47 | template 48 | const Eigen::Matrix& LsqRegistration::getFinalHessian() const { 49 | return final_hessian_; 50 | } 51 | 52 | template 53 | double LsqRegistration::evaluateCost(const Eigen::Matrix4f& relative_pose, Eigen::Matrix* H, Eigen::Matrix* b) { 54 | return this->linearize(Eigen::Isometry3f(relative_pose).cast(), H, b); 55 | } 56 | 57 | template 58 | void LsqRegistration::computeTransformation(PointCloudSource& output, const Matrix4& guess) { 59 | Eigen::Isometry3d x0 = Eigen::Isometry3d(guess.template cast()); 60 | 61 | lm_lambda_ = -1.0; 62 | converged_ = false; 63 | 64 | if (lm_debug_print_) { 65 | std::cout << "********************************************" << std::endl; 66 | std::cout << "***************** optimize *****************" << std::endl; 67 | std::cout << "********************************************" << std::endl; 68 | } 69 | 70 | for (int i = 0; i < max_iterations_ && !converged_; i++) { 71 | nr_iterations_ = i; 72 | 73 | Eigen::Isometry3d delta; 74 | if (!step_optimize(x0, delta)) { 75 | std::cerr << "lm not converged!!" << std::endl; 76 | break; 77 | } 78 | 79 | converged_ = is_converged(delta); 80 | } 81 | 82 | final_transformation_ = x0.cast().matrix(); 83 | pcl::transformPointCloud(*input_, output, final_transformation_); 84 | } 85 | 86 | template 87 | bool LsqRegistration::is_converged(const Eigen::Isometry3d& delta) const { 88 | double accum = 0.0; 89 | Eigen::Matrix3d R = delta.linear() - Eigen::Matrix3d::Identity(); 90 | Eigen::Vector3d t = delta.translation(); 91 | 92 | Eigen::Matrix3d r_delta = 1.0 / rotation_epsilon_ * R.array().abs(); 93 | Eigen::Vector3d t_delta = 1.0 / transformation_epsilon_ * t.array().abs(); 94 | 95 | return std::max(r_delta.maxCoeff(), t_delta.maxCoeff()) < 1; 96 | } 97 | 98 | template 99 | bool LsqRegistration::step_optimize(Eigen::Isometry3d& x0, Eigen::Isometry3d& delta) { 100 | switch (lsq_optimizer_type_) { 101 | case LSQ_OPTIMIZER_TYPE::LevenbergMarquardt: 102 | return step_lm(x0, delta); 103 | case LSQ_OPTIMIZER_TYPE::GaussNewton: 104 | return step_gn(x0, delta); 105 | case LSQ_OPTIMIZER_TYPE::LevenbergMarquardtNew: 106 | return step_lm_new(x0, delta); 107 | case LSQ_OPTIMIZER_TYPE::CeresDogleg: 108 | return step_ceres(x0,delta); 109 | } 110 | 111 | return step_lm(x0, delta); 112 | } 113 | template 114 | bool LsqRegistration::solve_ceres(Eigen::Isometry3d& trans,Eigen::Isometry3d& delta) { 115 | std::cout << "This is lsq_restration method, your code is wrong! You should call child class's method" << std::endl; 116 | return true; 117 | } 118 | template 119 | bool LsqRegistration::step_gn(Eigen::Isometry3d& x0, Eigen::Isometry3d& delta) { 120 | Eigen::Matrix H; 121 | Eigen::Matrix b; 122 | double y0 = linearize(x0, &H, &b); 123 | 124 | Eigen::LDLT> solver(H); 125 | Eigen::Matrix d = solver.solve(-b); 126 | 127 | delta.setIdentity(); 128 | delta.linear() = so3_exp(d.head<3>()).toRotationMatrix(); 129 | delta.translation() = d.tail<3>(); 130 | 131 | x0 = delta * x0; 132 | final_hessian_ = H; 133 | 134 | return true; 135 | } 136 | 137 | template 138 | bool LsqRegistration::step_lm(Eigen::Isometry3d& x0, Eigen::Isometry3d& delta) { 139 | Eigen::Matrix H; 140 | Eigen::Matrix b; 141 | double y0 = linearize(x0, &H, &b); 142 | 143 | if (lm_lambda_ < 0.0) { 144 | lm_lambda_ = lm_init_lambda_factor_ * H.diagonal().array().abs().maxCoeff(); 145 | } 146 | 147 | double nu = 2.0; 148 | for (int i = 0; i < lm_max_iterations_; i++) { 149 | Eigen::LDLT> solver(H + lm_lambda_ * Eigen::Matrix::Identity()); 150 | Eigen::Matrix d = solver.solve(-b); 151 | 152 | delta.setIdentity(); 153 | delta.linear() = so3_exp(d.head<3>()).toRotationMatrix(); 154 | delta.translation() = d.tail<3>(); 155 | 156 | Eigen::Isometry3d xi = delta * x0; 157 | double yi = compute_error(xi); 158 | double rho = (y0 - yi) / (d.dot(lm_lambda_ * d - b)); 159 | 160 | if (lm_debug_print_) { 161 | if (i == 0) { 162 | std::cout << boost::format("--- LM optimization ---\n%5s %15s %15s %15s %15s %15s %5s\n") % "i" % "y0" % "yi" % "rho" % "lambda" % "|delta|" % "dec"; 163 | } 164 | char dec = rho > 0.0 ? 'x' : ' '; 165 | std::cout << boost::format("%5d %15g %15g %15g %15g %15g %5c") % i % y0 % yi % rho % lm_lambda_ % d.norm() % dec << std::endl; 166 | } 167 | 168 | if (rho < 0) { 169 | if (is_converged(delta)) { 170 | return true; 171 | } 172 | 173 | lm_lambda_ = nu * lm_lambda_; 174 | nu = 2 * nu; 175 | continue; 176 | } 177 | 178 | x0 = xi; 179 | lm_lambda_ = lm_lambda_ * std::max(1.0 / 3.0, 1 - std::pow(2 * rho - 1, 3)); 180 | final_hessian_ = H; 181 | return true; 182 | } 183 | 184 | return false; 185 | } 186 | 187 | template 188 | bool LsqRegistration::step_lm_new(Eigen::Isometry3d& x0, Eigen::Isometry3d& delta) { 189 | Eigen::Matrix H; 190 | Eigen::Matrix b; 191 | double y0 = linearize(x0, &H, &b); 192 | // 若J为0，则说明此处导数为0，且局部最优解=全局最优解 193 | if (b.array().abs().maxCoeff() <= lm_sigma1_) { 194 | return true; 195 | } 196 | // tau越大，越接近最速；tau越小越接近GN，速度越慢； 197 | if (lm_lambda_ < 0.0) { 198 | lm_lambda_ = lm_init_lambda_factor_ * H.diagonal().array().abs().maxCoeff(); 199 | } 200 | 201 | double nu = 2.0; 202 | for (int i = 0; i < lm_max_iterations_; i++) { 203 | Eigen::LDLT> solver(H + lm_lambda_ * Eigen::Matrix::Identity()); 204 | // d= delta x 205 | Eigen::Matrix d = solver.solve(-b); 206 | 207 | delta.setIdentity(); 208 | delta.linear() = so3_exp(d.head<3>()).toRotationMatrix(); 209 | delta.translation() = d.tail<3>(); 210 | // delta x基本不移动了，说明收敛了,与ICP收敛判据冲突，因此进行替换 211 | // Eigen::Vector3d x_v = so3_log(x0); 212 | // if(d.norm() <= lm_sigma2_*(x_v.norm()+lm_sigma2_)){ 213 | // return true; 214 | // } 215 | if (is_converged(delta)) { 216 | return true; 217 | } 218 | // xi = x + delta x 219 | Eigen::Isometry3d xi = delta * x0; 220 | // yi = F(x+delta x) 221 | double yi = compute_error(xi); 222 | double rho = (y0 - yi) / (0.5 * d.dot(lm_lambda_ * d - b)); 223 | 224 | if (rho < 0) { 225 | lm_lambda_ = nu * lm_lambda_; 226 | nu = 2 * nu; 227 | continue; 228 | } 229 | 230 | x0 = xi; 231 | lm_lambda_ = lm_lambda_ * std::max(1.0 / 3.0, 1 - std::pow(2 * rho - 1, 3)); 232 | final_hessian_ = H; 233 | return true; 234 | } 235 | 236 | return false; 237 | } 238 | template 239 | bool LsqRegistration::step_ceres(Eigen::Isometry3d& x0, Eigen::Isometry3d& delta) { 240 | return solve_ceres(x0,delta); 241 | } 242 | } // namespace fast_gicp --------------------------------------------------------------------------------