├── .github └── workflows │ ├── build.yaml │ └── github-pages.yml ├── .gitignore ├── .gitmodules ├── README.md ├── docs ├── Makefile └── source │ ├── _static │ └── github_button.css │ ├── _templates │ ├── github_button.html │ └── layout.html │ ├── conf.py │ ├── covariance_estimation.rst │ ├── gauss_newton │ ├── square_vs_huber.png │ └── square_vs_huber.py │ ├── index.rst │ ├── loam.rst │ ├── robust_stddev_estimator.rst │ ├── robust_stddev_estimator │ ├── outlier00.png │ ├── outlier10.png │ ├── outlier20.png │ └── stddev_comparison.png │ ├── stddev_comparison.py │ └── weighted_gauss_newton.rst ├── ekf_localizer ├── CMakeLists.txt ├── LICENSE ├── README.md ├── include │ └── ekf_localizer │ │ ├── aged_message_queue.hpp │ │ ├── check.hpp │ │ ├── covariance.hpp │ │ ├── delay.hpp │ │ ├── ekf_localizer.hpp │ │ ├── mahalanobis.hpp │ │ ├── matrix_types.hpp │ │ ├── normalize_yaw.hpp │ │ ├── pose_measurement.hpp │ │ ├── state_index.hpp │ │ ├── state_transition.hpp │ │ ├── string.hpp │ │ ├── tf.hpp │ │ ├── twist_measurement.hpp │ │ ├── update_interval.hpp │ │ └── warning.hpp ├── media │ ├── delay_model_eq.png │ ├── ekf_autoware_res.png │ ├── ekf_delay_comp.png │ ├── ekf_dynamics.png │ ├── ekf_flowchart.png │ └── ekf_smooth_update.png ├── package.xml ├── src │ ├── check.cpp │ ├── covariance.cpp │ ├── ekf_localizer.cpp │ ├── ekf_localizer_node.cpp │ ├── mahalanobis.cpp │ ├── pose_measurement.cpp │ ├── state_transition.cpp │ ├── string.cpp │ ├── tf.cpp │ ├── twist_measurement.cpp │ └── update_interval.cpp └── test │ ├── test_check.cpp │ ├── test_covariance.cpp │ ├── test_ekf_localizer.cpp │ ├── test_mahalanobis.cpp │ ├── test_measurement.cpp │ ├── test_pose_measurement.cpp │ ├── test_state_transition.cpp │ ├── test_string.cpp │ ├── test_tf.cpp │ └── test_update_interval.cpp ├── extraction ├── CMakeLists.txt ├── LICENSE ├── app │ └── feature_extraction.cpp ├── include │ └── lidar_feature_extraction │ │ ├── algorithm.hpp │ │ ├── cloud_iterator.hpp │ │ ├── color_points.hpp │ │ ├── convolution.hpp │ │ ├── curvature.hpp │ │ ├── fill.hpp │ │ ├── hyper_parameter.hpp │ │ ├── index_range.hpp │ │ ├── iterator.hpp │ │ ├── label.hpp │ │ ├── mapped_points.hpp │ │ ├── math.hpp │ │ ├── neighbor.hpp │ │ ├── occlusion.hpp │ │ ├── out_of_range.hpp │ │ ├── parallel_beam.hpp │ │ ├── point_label.hpp │ │ ├── range.hpp │ │ ├── range_message.hpp │ │ ├── ring.hpp │ │ └── subscription.hpp ├── package.xml ├── src │ ├── color_points.cpp │ ├── convolution.cpp │ ├── curvature.cpp │ ├── index_range.cpp │ ├── iterator.cpp │ ├── math.cpp │ └── ring.cpp └── test │ ├── test_algorithm.cpp │ ├── test_color_points.cpp │ ├── test_convolution.cpp │ ├── test_curvature.cpp │ ├── test_fill.cpp │ ├── test_index_range.cpp │ ├── test_iterator.cpp │ ├── test_label.cpp │ ├── test_mapped_points.cpp │ ├── test_math.cpp │ ├── test_neighbor.cpp │ ├── test_occlusion.cpp │ ├── test_out_of_range.cpp │ ├── test_parallel_beam.cpp │ ├── test_range.cpp │ ├── test_range_message.cpp │ └── test_ring.cpp ├── imu_integration ├── CMakeLists.txt ├── include │ └── imu_integration │ │ └── integration.hpp ├── package.xml ├── src │ └── integration.cpp └── test │ └── test_rkmk.cpp ├── kalman_filter ├── CMakeLists.txt ├── LICENSE ├── README.md ├── include │ └── kalman_filter │ │ ├── kalman_filter.hpp │ │ ├── matrix_size.hpp │ │ └── time_delay_kalman_filter.hpp ├── package.xml ├── src │ ├── kalman_filter.cpp │ └── time_delay_kalman_filter.cpp └── test │ ├── test_matrix_size.cpp │ └── test_time_delay_kalman_filter.cpp ├── lib ├── CMakeLists.txt ├── include │ └── lidar_feature_library │ │ ├── algorithm.hpp │ │ ├── convert_point_cloud_type.hpp │ │ ├── degree_to_radian.hpp │ │ ├── downsample.hpp │ │ ├── eigen.hpp │ │ ├── matrix_types.hpp │ │ ├── numeric.hpp │ │ ├── pcl_utils.hpp │ │ ├── point_type.hpp │ │ ├── qos.hpp │ │ ├── random.hpp │ │ ├── ros_msg.hpp │ │ ├── span.hpp │ │ └── stats.hpp ├── package.xml ├── src │ ├── algorithm.cpp │ ├── eigen.cpp │ ├── lib.cpp │ ├── pcl_utils.cpp │ ├── random.cpp │ ├── ros_msg.cpp │ └── stats.cpp └── test │ ├── test_algorithm.cpp │ ├── test_degree_to_radian.cpp │ ├── test_eigen.cpp │ ├── test_numeric.cpp │ ├── test_pcl_utils.cpp │ ├── test_random.cpp │ ├── test_ros_msg.cpp │ ├── test_span.cpp │ └── test_stats.cpp ├── lidar_feature_launch ├── config │ └── lidar_feature_extraction.param.yaml ├── launch │ ├── localization.launch.py │ ├── mapping.launch.py │ └── odometry.launch.py ├── lidar_feature_launch │ └── __init__.py ├── package.xml ├── resource │ └── lidar_feature_launch ├── setup.cfg ├── setup.py └── test │ ├── test_copyright.py │ ├── test_flake8.py │ └── test_pep257.py ├── localization ├── CMakeLists.txt ├── app │ ├── localization.cpp │ └── odometry.cpp ├── include │ └── lidar_feature_localization │ │ ├── alignment.hpp │ │ ├── degenerate.hpp │ │ ├── edge.hpp │ │ ├── edge_surface_map.hpp │ │ ├── edge_surface_scan.hpp │ │ ├── filter.hpp │ │ ├── jacobian.hpp │ │ ├── kdtree.hpp │ │ ├── loam_optimization_problem.hpp │ │ ├── localizer.hpp │ │ ├── map_io.hpp │ │ ├── math.hpp │ │ ├── matrix_type.hpp │ │ ├── odometry.hpp │ │ ├── optimization_result.hpp │ │ ├── optimizer.hpp │ │ ├── point_cloud_map.hpp │ │ ├── point_to_vector.hpp │ │ ├── pointcloud_to_matrix.hpp │ │ ├── pose_updater.hpp │ │ ├── posevec.hpp │ │ ├── rad2deg.hpp │ │ ├── recent_scans.hpp │ │ ├── robust.hpp │ │ ├── stamp_sorted_objects.hpp │ │ ├── subscriber.hpp │ │ └── surface.hpp ├── package.xml ├── src │ ├── alignment.cpp │ ├── degenerate.cpp │ ├── edge.cpp │ ├── jacobian.cpp │ ├── kdtree.cpp │ ├── optimizer.cpp │ ├── posevec.cpp │ └── robust.cpp └── test │ ├── test_alignment.cpp │ ├── test_degenerate.cpp │ ├── test_edge.cpp │ ├── test_filter.cpp │ ├── test_jacobian.cpp │ ├── test_kdtree.cpp │ ├── test_math.cpp │ ├── test_odometry.cpp │ ├── test_optimizer.cpp │ ├── test_pointcloud_to_matrix.cpp │ ├── test_posevec.cpp │ ├── test_recent_scans.cpp │ ├── test_robust.cpp │ └── test_stamp_sorted_objects.cpp ├── map_loader ├── CMakeLists.txt ├── package.xml └── src │ └── map_loader.cpp ├── map_tf_generator ├── CMakeLists.txt ├── Readme.md ├── launch │ └── map_tf_generator.launch.xml ├── package.xml └── src │ └── map_tf_generator_node.cpp ├── mapping ├── CMakeLists.txt ├── include │ └── lidar_feature_mapping │ │ └── map.hpp ├── package.xml ├── src │ └── mapping.cpp └── test │ └── test_map.cpp ├── maps └── .gitkeep ├── path_generator ├── CMakeLists.txt ├── include │ └── path_generator │ │ └── path_generator.hpp ├── package.xml ├── src │ └── node.cpp └── test │ └── test_path_generator.cpp ├── point_type_converter ├── package.xml ├── point_type_converter │ ├── __init__.py │ └── convert.py ├── resource │ └── point_type_converter ├── setup.cfg ├── setup.py └── test │ ├── test_convert.py │ ├── test_copyright.py │ ├── test_flake8.py │ └── test_pep257.py ├── rotationlib ├── CMakeLists.txt ├── include │ └── rotationlib │ │ ├── hat.hpp │ │ ├── jacobian │ │ └── quaternion.hpp │ │ └── quaternion.hpp ├── package.xml ├── src │ ├── hat.cpp │ ├── jacobian │ │ └── quaternion.cpp │ └── quaternion.cpp ├── test │ ├── test_hat.cpp │ ├── test_jacobian_quaternion.cpp │ └── test_quaternion.cpp └── testlib │ └── near_with_precision.hpp └── rviz ├── lidar_feature_convergence.rviz ├── lidar_feature_debug.rviz └── lidar_feature_localization.rviz /.github/workflows/github-pages.yml: -------------------------------------------------------------------------------- 1 | name: GitHub Pages 2 | 3 | on: 4 | push: 5 | branches: 6 | - develop 7 | 8 | jobs: 9 | deploy: 10 | runs-on: ubuntu-20.04 11 | 12 | # Grant GITHUB_TOKEN the permissions required to make a Pages deployment 13 | permissions: 14 | pages: write # to deploy to Pages 15 | id-token: write # to verify the deployment originates from an appropriate source 16 | 17 | # Deploy to the github-pages environment 18 | environment: 19 | name: github-pages 20 | url: ${{ steps.deployment.outputs.page_url }} 21 | 22 | concurrency: 23 | group: ${{ github.workflow }}-${{ github.ref }} 24 | 25 | steps: 26 | - uses: actions/checkout@v3 27 | 28 | - name: Setup Python 29 | uses: actions/setup-python@v3 30 | with: 31 | python-version: '3.8' 32 | 33 | - name: Upgrade pip 34 | run: | 35 | # install pip=>20.1 to use "pip cache dir" 36 | python3 -m pip install --upgrade pip 37 | 38 | - name: Install dependencies 39 | run: python3 -m pip install sphinx alabaster 40 | 41 | - name: Make the sphinx docs 42 | run: | 43 | make -C docs clean 44 | make -C docs html 45 | 46 | - name: Upload artifact 47 | uses: actions/upload-pages-artifact@v1 48 | with: 49 | # Upload entire repository 50 | path: './docs/build/html/' 51 | 52 | - name: Deploy to GitHub Pages 53 | id: deployment 54 | uses: actions/deploy-pages@v1 55 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | __pycache__/ 2 | /build 3 | /install 4 | /log 5 | /maps 6 | -------------------------------------------------------------------------------- /.gitmodules: -------------------------------------------------------------------------------- 1 | [submodule "localization/thirdparty/nanoflann"] 2 | path = localization/thirdparty/nanoflann 3 | url = https://github.com/jlblancoc/nanoflann.git 4 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | LiDAR feature-based localization 2 | ================================ 3 | 4 | ## Functionalities 5 | 6 | This package has two functionalities: 7 | 8 | * Map building module: constructs a map from undistorted LiDAR scans and their poses 9 | * Localization module: localizes LiDAR on a map 10 | 11 | ## Benefits 12 | 13 | * LOAM-like feature based algorithm enables localization in challenging environments such as tunnels, rice fields, etc. 14 | * Localization on a pre-built map realizes stable and robust localization in dynamic environments 15 | 16 | 17 | ## Citation 18 | 19 | This code makes use of [LIO-SAM](https://github.com/TixiaoShan/LIO-SAM) and [LeGO-LOAM](https://github.com/RobustFieldAutonomyLab/LeGO-LOAM). 20 | 21 | ``` 22 | @inproceedings{liosam2020shan, 23 | title={LIO-SAM: Tightly-coupled Lidar Inertial Odometry via Smoothing and Mapping}, 24 | author={Shan, Tixiao and Englot, Brendan and Meyers, Drew and Wang, Wei and Ratti, Carlo and Rus Daniela}, 25 | booktitle={IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS)}, 26 | pages={5135-5142}, 27 | year={2020}, 28 | organization={IEEE} 29 | } 30 | ``` 31 | 32 | ``` 33 | @inproceedings{legoloam2018shan, 34 | title={LeGO-LOAM: Lightweight and Ground-Optimized Lidar Odometry and Mapping on Variable Terrain}, 35 | author={Shan, Tixiao and Englot, Brendan}, 36 | booktitle={IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS)}, 37 | pages={4758-4765}, 38 | year={2018}, 39 | organization={IEEE} 40 | } 41 | ``` 42 | -------------------------------------------------------------------------------- /docs/Makefile: -------------------------------------------------------------------------------- 1 | # Minimal makefile for Sphinx documentation 2 | # 3 | 4 | # You can set these variables from the command line, and also 5 | # from the environment for the first two. 6 | SPHINXOPTS ?= 7 | SPHINXBUILD ?= sphinx-build 8 | SOURCEDIR = source 9 | BUILDDIR = build 10 | 11 | # Put it first so that "make" without argument is like "make help". 12 | help: 13 | @$(SPHINXBUILD) -M help "$(SOURCEDIR)" "$(BUILDDIR)" $(SPHINXOPTS) $(O) 14 | 15 | .PHONY: help Makefile 16 | 17 | # Catch-all target: route all unknown targets to Sphinx using the new 18 | # "make mode" option. $(O) is meant as a shortcut for $(SPHINXOPTS). 19 | %: Makefile 20 | @$(SPHINXBUILD) -M $@ "$(SOURCEDIR)" "$(BUILDDIR)" $(SPHINXOPTS) $(O) 21 | -------------------------------------------------------------------------------- /docs/source/_static/github_button.css: -------------------------------------------------------------------------------- 1 | #github-button-wrapper { 2 | padding-top: 10px; 3 | padding-bottom: 10px; 4 | } 5 | -------------------------------------------------------------------------------- /docs/source/_templates/github_button.html: -------------------------------------------------------------------------------- 1 | 2 |
3 | 4 | Star 5 |
6 | -------------------------------------------------------------------------------- /docs/source/_templates/layout.html: -------------------------------------------------------------------------------- 1 | {% extends "!layout.html" %} 2 | 3 | {% set sidebars = sidebars + ['github_button.html'] %} 4 | {% set css_files = css_files + ['_static/github_button.css'] %} 5 | 6 | {% block extrahead %} 7 | 8 | {{ super() }} 9 | {% endblock %} 10 | 11 | {% block content %} 12 | {{ super() }} 13 | {% endblock %} 14 | -------------------------------------------------------------------------------- /docs/source/conf.py: -------------------------------------------------------------------------------- 1 | # Configuration file for the Sphinx documentation builder. 2 | # 3 | # For the full list of built-in configuration values, see the documentation: 4 | # https://www.sphinx-doc.org/en/master/usage/configuration.html 5 | 6 | # -- Project information ----------------------------------------------------- 7 | # https://www.sphinx-doc.org/en/master/usage/configuration.html#project-information 8 | 9 | project = 'Localization' 10 | copyright = '2022, Takeshi Ishita' 11 | author = 'Takeshi Ishita' 12 | 13 | # -- General configuration --------------------------------------------------- 14 | # https://www.sphinx-doc.org/en/master/usage/configuration.html#general-configuration 15 | 16 | extensions = [] 17 | 18 | templates_path = ['_templates'] 19 | exclude_patterns = [] 20 | 21 | 22 | 23 | # -- Options for HTML output ------------------------------------------------- 24 | # https://www.sphinx-doc.org/en/master/usage/configuration.html#options-for-html-output 25 | 26 | html_theme = 'alabaster' 27 | html_static_path = ['_static'] 28 | -------------------------------------------------------------------------------- /docs/source/gauss_newton/square_vs_huber.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/tier4/lidar_feature_extraction/67c5eee072417ef29017e66993d64a978e7a0f96/docs/source/gauss_newton/square_vs_huber.png -------------------------------------------------------------------------------- /docs/source/gauss_newton/square_vs_huber.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | from matplotlib import pyplot as plt 3 | 4 | plt.rcParams['text.usetex'] = True 5 | 6 | 7 | x = np.linspace(-10., 10., 2001) 8 | 9 | 10 | def huber(e, k=2.): 11 | result = np.empty(e.shape[0]) 12 | 13 | test = e < k * k 14 | return test * e + (1-test) * (2 * k * np.sqrt(e) - k*k) 15 | 16 | 17 | plt.plot(x, x*x, alpha=0.6, label=r"$y=x^2$") 18 | plt.plot(x, huber(x*x), alpha=0.6, label=r"$y=\mathrm{huber}(x^2)$") 19 | 20 | plt.xlabel("x") 21 | plt.ylabel("y") 22 | 23 | plt.legend() 24 | 25 | plt.show() 26 | -------------------------------------------------------------------------------- /docs/source/index.rst: -------------------------------------------------------------------------------- 1 | .. Localization documentation master file, created by 2 | sphinx-quickstart on Fri Aug 19 20:16:49 2022. 3 | You can adapt this file completely to your liking, but it should at least 4 | contain the root `toctree` directive. 5 | 6 | Localization documentation 7 | ========================== 8 | 9 | .. toctree:: 10 | :maxdepth: 2 11 | :caption: Contents: 12 | 13 | weighted_gauss_newton 14 | robust_stddev_estimator 15 | covariance_estimation 16 | loam 17 | 18 | Indices and tables 19 | ================== 20 | 21 | * :ref:`genindex` 22 | * :ref:`modindex` 23 | * :ref:`search` 24 | -------------------------------------------------------------------------------- /docs/source/robust_stddev_estimator/outlier00.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/tier4/lidar_feature_extraction/67c5eee072417ef29017e66993d64a978e7a0f96/docs/source/robust_stddev_estimator/outlier00.png -------------------------------------------------------------------------------- /docs/source/robust_stddev_estimator/outlier10.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/tier4/lidar_feature_extraction/67c5eee072417ef29017e66993d64a978e7a0f96/docs/source/robust_stddev_estimator/outlier10.png -------------------------------------------------------------------------------- /docs/source/robust_stddev_estimator/outlier20.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/tier4/lidar_feature_extraction/67c5eee072417ef29017e66993d64a978e7a0f96/docs/source/robust_stddev_estimator/outlier20.png -------------------------------------------------------------------------------- /docs/source/robust_stddev_estimator/stddev_comparison.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/tier4/lidar_feature_extraction/67c5eee072417ef29017e66993d64a978e7a0f96/docs/source/robust_stddev_estimator/stddev_comparison.png -------------------------------------------------------------------------------- /docs/source/stddev_comparison.py: -------------------------------------------------------------------------------- 1 | from matplotlib import pyplot as plt 2 | import numpy as np 3 | from scipy.stats import norm 4 | 5 | 6 | n_samples = 10000 7 | n_estimation_times = 20 8 | 9 | 10 | def mad(x): 11 | return np.median(np.abs(x - np.median(x))) 12 | 13 | 14 | def generate_data(stddev, outlier_ratio): 15 | n_inliers = int((1. - outlier_ratio) * n_samples) 16 | n_outliers = int(outlier_ratio * n_samples) 17 | 18 | # inliers follow the normal distribution 19 | inliers = np.random.normal(0., stddev, n_inliers) 20 | 21 | # generate outliers from the uniform distribution with a much wider range 22 | v = 20. * stddev 23 | outliers = np.random.uniform(-v, v, n_outliers) 24 | 25 | return np.concatenate([inliers, outliers]) 26 | 27 | 28 | def estimate_stddev_by_mad(data): 29 | # estimate the standard deviation using median absolute deviation 30 | return mad(data) / norm.ppf(3./4) 31 | 32 | 33 | def estimate_stddev_by_std(data): 34 | # estimate the standard deviation in the general way 35 | return np.std(data, ddof=1) 36 | 37 | 38 | def run_estimation(stddev_true, outlier_ratio): 39 | stddev_est_mad = np.empty(n_estimation_times) 40 | stddev_est_std = np.empty(n_estimation_times) 41 | for i in range(n_estimation_times): 42 | data = generate_data(stddev_true, outlier_ratio) 43 | stddev_est_mad[i] = estimate_stddev_by_mad(data) 44 | stddev_est_std[i] = estimate_stddev_by_std(data) 45 | return stddev_est_mad, stddev_est_std 46 | 47 | 48 | fig = plt.figure() 49 | ax = fig.add_subplot(111) 50 | 51 | stddev_true = 2 52 | 53 | outlier_ratios = np.linspace(0., 0.2, 21) 54 | 55 | for outlier_ratio in outlier_ratios: 56 | stddev_est_mad, stddev_est_std = run_estimation(stddev_true, outlier_ratio) 57 | 58 | outlier_percentage = 100. * outlier_ratio 59 | 60 | ys = outlier_percentage * np.ones(n_estimation_times) 61 | 62 | ax.axhline(outlier_percentage, alpha=0.2) 63 | ax.scatter(stddev_est_mad, ys, c="blue", label="robust estimator") 64 | ax.scatter(stddev_est_std, ys, c="green", label="standard method") 65 | ax.scatter(stddev_true, outlier_percentage, c="red", label="ground truth") 66 | 67 | ax.set_xticks(np.linspace(2., 12., 11)) 68 | ax.set_yticks(100. * outlier_ratios) 69 | 70 | ax.set_ylabel("Outlier percentage [%]") 71 | ax.set_xlabel("Estimated standard deviation") 72 | 73 | lines, labels = ax.get_legend_handles_labels() 74 | 75 | plt.legend(lines[0:3], labels[0:3], loc="lower right") 76 | plt.show() 77 | -------------------------------------------------------------------------------- /ekf_localizer/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.14) 2 | project(ekf_localizer) 3 | 4 | set(CMAKE_BUILD_TYPE "Debug") 5 | set(CMAKE_CXX_STANDARD 17) 6 | set(CMAKE_CXX_FLAGS "-g -O3 -Wall -Wextra -Wpedantic -Werror -fprofile-arcs -ftest-coverage") 7 | 8 | find_package(ament_cmake_auto REQUIRED) 9 | find_package(Eigen3 REQUIRED) 10 | 11 | ament_auto_find_build_dependencies() 12 | 13 | ament_auto_add_library(ekf_localizer_lib SHARED 14 | src/check.cpp 15 | src/covariance.cpp 16 | src/ekf_localizer.cpp 17 | src/mahalanobis.cpp 18 | src/pose_measurement.cpp 19 | src/state_transition.cpp 20 | src/string.cpp 21 | src/tf.cpp 22 | src/twist_measurement.cpp 23 | src/update_interval.cpp 24 | ) 25 | 26 | target_link_libraries(ekf_localizer_lib Eigen3::Eigen) 27 | 28 | ament_auto_add_executable(ekf_localizer src/ekf_localizer_node.cpp) 29 | target_link_libraries(ekf_localizer ekf_localizer_lib) 30 | target_include_directories(ekf_localizer PUBLIC include) 31 | 32 | function(add_testcase filepath) 33 | get_filename_component(filename ${filepath} NAME) 34 | string(REGEX REPLACE ".cpp" "" test_name ${filename}) 35 | 36 | ament_add_gtest(${test_name} ${filepath}) 37 | target_link_libraries("${test_name}" ekf_localizer_lib) 38 | ament_target_dependencies(${test_name} ${${PROJECT_NAME}_FOUND_BUILD_DEPENDS}) 39 | endfunction() 40 | 41 | if(BUILD_TESTING) 42 | find_package(ament_cmake_gtest REQUIRED) 43 | 44 | file(GLOB 45 | FILES_TO_CHECK 46 | ${CMAKE_CURRENT_SOURCE_DIR}/src/* 47 | ${CMAKE_CURRENT_SOURCE_DIR}/include/* 48 | ${CMAKE_CURRENT_SOURCE_DIR}/test/*) 49 | 50 | find_package(ament_cmake_cpplint) 51 | ament_cpplint(${FILES_TO_CHECK}) 52 | 53 | find_package(ament_cmake_uncrustify) 54 | ament_uncrustify(${FILES_TO_CHECK}) 55 | 56 | set(TEST_FILES 57 | test/test_check.cpp 58 | # test/test_ekf_localizer.cpp 59 | test/test_mahalanobis.cpp 60 | test/test_measurement.cpp 61 | test/test_pose_measurement.cpp 62 | test/test_state_transition.cpp 63 | test/test_string.cpp 64 | test/test_tf.cpp 65 | test/test_update_interval.cpp) 66 | 67 | foreach(filepath ${TEST_FILES}) 68 | add_testcase(${filepath}) 69 | endforeach() 70 | endif() 71 | 72 | ament_auto_package() 73 | -------------------------------------------------------------------------------- /ekf_localizer/LICENSE: -------------------------------------------------------------------------------- 1 | Copyright 2018-2019 Autoware Foundation 2 | 3 | Licensed under the Apache License, Version 2.0 (the "License"); 4 | you may not use this file except in compliance with the License. 5 | You may obtain a copy of the License at 6 | 7 | http://www.apache.org/licenses/LICENSE-2.0 8 | 9 | Unless required by applicable law or agreed to in writing, software 10 | distributed under the License is distributed on an "AS IS" BASIS, 11 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | See the License for the specific language governing permissions and 13 | limitations under the License. 14 | -------------------------------------------------------------------------------- /ekf_localizer/include/ekf_localizer/aged_message_queue.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2018-2019 Autoware Foundation 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #ifndef EKF_LOCALIZER__AGED_MESSAGE_QUEUE_HPP_ 16 | #define EKF_LOCALIZER__AGED_MESSAGE_QUEUE_HPP_ 17 | 18 | #include 19 | 20 | template 21 | class AgedMessageQueue 22 | { 23 | public: 24 | explicit AgedMessageQueue(const int max_age) 25 | : max_age_(max_age) 26 | { 27 | } 28 | 29 | size_t size() const 30 | { 31 | return msgs_.size(); 32 | } 33 | 34 | void push(const Message & msg) 35 | { 36 | msgs_.push(msg); 37 | ages_.push(0); 38 | } 39 | 40 | Message pop() 41 | { 42 | const auto msg = msgs_.front(); 43 | const int age = ages_.front() + 1; 44 | msgs_.pop(); 45 | ages_.pop(); 46 | 47 | if (age < max_age_) { 48 | msgs_.push(msg); 49 | ages_.push(age); 50 | } 51 | 52 | return msg; 53 | } 54 | 55 | void clear() 56 | { 57 | msgs_ = std::queue(); 58 | ages_ = std::queue(); 59 | } 60 | 61 | private: 62 | const int max_age_; 63 | std::queue msgs_; 64 | std::queue ages_; 65 | }; 66 | 67 | #endif // EKF_LOCALIZER__AGED_MESSAGE_QUEUE_HPP_ 68 | -------------------------------------------------------------------------------- /ekf_localizer/include/ekf_localizer/check.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2018-2019 Autoware Foundation 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #ifndef EKF_LOCALIZER__CHECK_HPP_ 16 | #define EKF_LOCALIZER__CHECK_HPP_ 17 | 18 | #include 19 | 20 | #include 21 | 22 | #include "ekf_localizer/mahalanobis.hpp" 23 | #include "ekf_localizer/warning.hpp" 24 | 25 | #include "lidar_feature_library/numeric.hpp" 26 | 27 | 28 | void ShowDelayTimeWarning(const Warning & warning, const double delay_time); 29 | 30 | void ShowDelayStepWarning(const Warning & warning, const int delay_step, const int state_step); 31 | 32 | void ShowFrameIdWarning( 33 | const Warning & warning, 34 | const std::string & header_frame_id, 35 | const std::string & expected_frame_id); 36 | 37 | void ShowMahalanobisGateWarning(const Warning & warning); 38 | 39 | void ShowMeasurementMatrixNanInfWarning(const Warning & warning); 40 | 41 | bool CheckFrameId( 42 | const Warning & warning, 43 | const std::string & frame_id, 44 | const std::string & expected_frame_id); 45 | 46 | bool CheckDelayTime(const Warning & warning, const double delay_time); 47 | 48 | bool CheckDelayStep(const Warning & warning, const int delay_step, const int max_delay_step); 49 | 50 | bool CheckMeasurementMatrixNanInf(const Warning & warning, const Eigen::MatrixXd & M); 51 | 52 | bool CheckMahalanobisGate( 53 | const Warning & warning, 54 | const double & dist_max, 55 | const Eigen::MatrixXd & x1, 56 | const Eigen::MatrixXd & x2, 57 | const Eigen::MatrixXd & cov); 58 | 59 | #endif // EKF_LOCALIZER__CHECK_HPP_ 60 | -------------------------------------------------------------------------------- /ekf_localizer/include/ekf_localizer/covariance.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2018-2019 Autoware Foundation 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #ifndef EKF_LOCALIZER__COVARIANCE_HPP_ 16 | #define EKF_LOCALIZER__COVARIANCE_HPP_ 17 | 18 | #include 19 | 20 | #include 21 | 22 | #include "ekf_localizer/matrix_types.hpp" 23 | 24 | std::array EKFCovarianceToPoseMessageCovariance(const Matrix6d & P); 25 | std::array EKFCovarianceToTwistMessageCovariance(const Matrix6d & P); 26 | 27 | #endif // EKF_LOCALIZER__COVARIANCE_HPP_ 28 | -------------------------------------------------------------------------------- /ekf_localizer/include/ekf_localizer/delay.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2018-2019 Autoware Foundation 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #ifndef EKF_LOCALIZER__DELAY_HPP_ 16 | #define EKF_LOCALIZER__DELAY_HPP_ 17 | 18 | #include 19 | 20 | 21 | inline double ComputeDelayTime( 22 | const rclcpp::Time & current_time, 23 | const rclcpp::Time & message_stamp) 24 | { 25 | return (current_time - message_stamp).seconds(); 26 | } 27 | 28 | inline int ComputeDelayStep(const double delay_time, const double dt) 29 | { 30 | return std::roundf(std::max(delay_time, 0.) / dt); 31 | } 32 | 33 | #endif // EKF_LOCALIZER__DELAY_HPP_ 34 | -------------------------------------------------------------------------------- /ekf_localizer/include/ekf_localizer/mahalanobis.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2018-2019 Autoware Foundation 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | 16 | #ifndef EKF_LOCALIZER__MAHALANOBIS_HPP_ 17 | #define EKF_LOCALIZER__MAHALANOBIS_HPP_ 18 | 19 | #include 20 | #include 21 | 22 | 23 | double SquaredMahalanobis( 24 | const Eigen::VectorXd & x, 25 | const Eigen::VectorXd & y, 26 | const Eigen::MatrixXd & C); 27 | 28 | bool MahalanobisGate( 29 | const double & dist_max, const Eigen::MatrixXd & x, const Eigen::MatrixXd & obj_x, 30 | const Eigen::MatrixXd & cov); 31 | 32 | #endif // EKF_LOCALIZER__MAHALANOBIS_HPP_ 33 | -------------------------------------------------------------------------------- /ekf_localizer/include/ekf_localizer/matrix_types.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2022 Autoware Foundation 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #ifndef EKF_LOCALIZER__MATRIX_TYPES_HPP_ 16 | #define EKF_LOCALIZER__MATRIX_TYPES_HPP_ 17 | 18 | #include 19 | 20 | using Vector6d = Eigen::Matrix; 21 | using Matrix6d = Eigen::Matrix; 22 | 23 | #endif // EKF_LOCALIZER__MATRIX_TYPES_HPP_ 24 | -------------------------------------------------------------------------------- /ekf_localizer/include/ekf_localizer/normalize_yaw.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2018-2019 Autoware Foundation 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #ifndef EKF_LOCALIZER__NORMALIZE_YAW_HPP_ 16 | #define EKF_LOCALIZER__NORMALIZE_YAW_HPP_ 17 | 18 | 19 | // Noramlizes the yaw angle so that it fits in the range (-pi, pi) 20 | /** 21 | * @brief normalize yaw angle 22 | * @param yaw yaw angle 23 | * @return normalized yaw 24 | */ 25 | inline double normalizeYaw(const double & yaw) 26 | { 27 | return std::atan2(std::sin(yaw), std::cos(yaw)); 28 | } 29 | 30 | 31 | #endif // EKF_LOCALIZER__NORMALIZE_YAW_HPP_ 32 | -------------------------------------------------------------------------------- /ekf_localizer/include/ekf_localizer/state_index.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2022 Autoware Foundation 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #ifndef EKF_LOCALIZER__STATE_INDEX_HPP_ 16 | #define EKF_LOCALIZER__STATE_INDEX_HPP_ 17 | 18 | enum IDX 19 | { 20 | X = 0, 21 | Y = 1, 22 | YAW = 2, 23 | YAWB = 3, 24 | VX = 4, 25 | WZ = 5, 26 | }; 27 | 28 | #endif // EKF_LOCALIZER__STATE_INDEX_HPP_ 29 | -------------------------------------------------------------------------------- /ekf_localizer/include/ekf_localizer/state_transition.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2022 Autoware Foundation 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #ifndef EKF_LOCALIZER__STATE_TRANSITION_HPP_ 16 | #define EKF_LOCALIZER__STATE_TRANSITION_HPP_ 17 | 18 | #include "ekf_localizer/matrix_types.hpp" 19 | 20 | double normalizeYaw(const double & yaw); 21 | Vector6d predictNextState(const Vector6d & X_curr, const double dt); 22 | Matrix6d createStateTransitionMatrix(const Vector6d & X_curr, const double dt); 23 | Matrix6d processNoiseCovariance(const Eigen::Vector4d & variances); 24 | 25 | #endif // EKF_LOCALIZER__STATE_TRANSITION_HPP_ 26 | -------------------------------------------------------------------------------- /ekf_localizer/include/ekf_localizer/string.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2018-2019 Autoware Foundation 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #ifndef EKF_LOCALIZER__STRING_HPP_ 16 | #define EKF_LOCALIZER__STRING_HPP_ 17 | 18 | #include 19 | 20 | std::string EraseBeginSlash(const std::string & s); 21 | 22 | #endif // EKF_LOCALIZER__STRING_HPP_ 23 | -------------------------------------------------------------------------------- /ekf_localizer/include/ekf_localizer/tf.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2018-2019 Autoware Foundation 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #ifndef EKF_LOCALIZER__TF_HPP_ 16 | #define EKF_LOCALIZER__TF_HPP_ 17 | 18 | #include 19 | #include 20 | 21 | #include 22 | #include 23 | 24 | #include "ekf_localizer/warning.hpp" 25 | 26 | 27 | class TransformListener 28 | { 29 | public: 30 | explicit TransformListener(rclcpp::Node * node) 31 | : tf_buffer_(std::make_shared()), 32 | listener_(*tf_buffer_, node, false), 33 | warning_(std::make_shared(node)) 34 | { 35 | } 36 | 37 | std::optional LookupTransform( 38 | const std::string & parent_frame, 39 | const std::string & child_frame) const; 40 | 41 | private: 42 | const std::shared_ptr tf_buffer_; 43 | const tf2_ros::TransformListener listener_; 44 | const std::shared_ptr warning_; 45 | }; 46 | 47 | #endif // EKF_LOCALIZER__TF_HPP_ 48 | -------------------------------------------------------------------------------- /ekf_localizer/include/ekf_localizer/update_interval.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2018-2019 Autoware Foundation 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #ifndef EKF_LOCALIZER__UPDATE_INTERVAL_HPP_ 16 | #define EKF_LOCALIZER__UPDATE_INTERVAL_HPP_ 17 | 18 | #include 19 | #include 20 | 21 | 22 | inline double ComputeInterval(double frequency) 23 | { 24 | return 1.0 / std::max(frequency, 0.1); 25 | } 26 | 27 | // TODO(IshitaTakeshi) Make the constructor take the interval instead of 28 | // frequency becasue it may cause confusion that the constructor takes 29 | // the frequency [1/s] while the compute method takes the time [s] 30 | class UpdateInterval 31 | { 32 | public: 33 | explicit UpdateInterval(const double default_frequency) 34 | : default_frequency_(default_frequency), last_time_(std::nullopt) 35 | { 36 | } 37 | 38 | double Compute(const double current_time_second); 39 | 40 | private: 41 | const double default_frequency_; 42 | std::optional last_time_; 43 | }; 44 | 45 | #endif // EKF_LOCALIZER__UPDATE_INTERVAL_HPP_ 46 | -------------------------------------------------------------------------------- /ekf_localizer/include/ekf_localizer/warning.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2018-2019 Autoware Foundation 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #ifndef EKF_LOCALIZER__WARNING_HPP_ 16 | #define EKF_LOCALIZER__WARNING_HPP_ 17 | 18 | #include 19 | 20 | #include 21 | 22 | 23 | class Warning 24 | { 25 | public: 26 | explicit Warning(rclcpp::Node * node) 27 | : node_(node) 28 | { 29 | } 30 | 31 | void Info(const std::string & message) const 32 | { 33 | RCLCPP_INFO(node_->get_logger(), message.c_str()); 34 | } 35 | 36 | void Warn(const std::string & message) const 37 | { 38 | RCLCPP_WARN(node_->get_logger(), message.c_str()); 39 | } 40 | 41 | void Error(const std::string & message) const 42 | { 43 | RCLCPP_ERROR(node_->get_logger(), message.c_str()); 44 | } 45 | 46 | void WarnThrottle( 47 | const int duration_milliseconds, 48 | const std::string & message) const 49 | { 50 | RCLCPP_WARN_THROTTLE( 51 | node_->get_logger(), *(node_->get_clock()), 52 | std::chrono::milliseconds(duration_milliseconds).count(), 53 | message.c_str()); 54 | } 55 | 56 | private: 57 | rclcpp::Node * node_; 58 | }; 59 | 60 | #endif // EKF_LOCALIZER__WARNING_HPP_ 61 | -------------------------------------------------------------------------------- /ekf_localizer/media/delay_model_eq.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/tier4/lidar_feature_extraction/67c5eee072417ef29017e66993d64a978e7a0f96/ekf_localizer/media/delay_model_eq.png -------------------------------------------------------------------------------- /ekf_localizer/media/ekf_autoware_res.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/tier4/lidar_feature_extraction/67c5eee072417ef29017e66993d64a978e7a0f96/ekf_localizer/media/ekf_autoware_res.png -------------------------------------------------------------------------------- /ekf_localizer/media/ekf_delay_comp.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/tier4/lidar_feature_extraction/67c5eee072417ef29017e66993d64a978e7a0f96/ekf_localizer/media/ekf_delay_comp.png -------------------------------------------------------------------------------- /ekf_localizer/media/ekf_dynamics.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/tier4/lidar_feature_extraction/67c5eee072417ef29017e66993d64a978e7a0f96/ekf_localizer/media/ekf_dynamics.png -------------------------------------------------------------------------------- /ekf_localizer/media/ekf_flowchart.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/tier4/lidar_feature_extraction/67c5eee072417ef29017e66993d64a978e7a0f96/ekf_localizer/media/ekf_flowchart.png -------------------------------------------------------------------------------- /ekf_localizer/media/ekf_smooth_update.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/tier4/lidar_feature_extraction/67c5eee072417ef29017e66993d64a978e7a0f96/ekf_localizer/media/ekf_smooth_update.png -------------------------------------------------------------------------------- /ekf_localizer/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | ekf_localizer 5 | 0.1.0 6 | The ekf_localizer package 7 | horibe 8 | Apache License 2.0 9 | 10 | ament_cmake_auto 11 | eigen3_cmake_module 12 | 13 | eigen 14 | fmt 15 | geometry_msgs 16 | kalman_filter 17 | lidar_feature_library 18 | nav_msgs 19 | rclcpp 20 | rotationlib 21 | sensor_msgs 22 | std_msgs 23 | tf2 24 | tf2_ros 25 | 26 | ament_cmake_ros 27 | ament_lint_auto 28 | 29 | 30 | ament_cmake 31 | 32 | 33 | -------------------------------------------------------------------------------- /ekf_localizer/src/covariance.cpp: -------------------------------------------------------------------------------- 1 | // Copyright 2018-2019 Autoware Foundation 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #include 16 | 17 | #include 18 | 19 | #include "ekf_localizer/covariance.hpp" 20 | 21 | 22 | std::array EKFCovarianceToPoseMessageCovariance(const Matrix6d & P) 23 | { 24 | const double p00 = P(0, 0); 25 | const double p01 = P(0, 1); 26 | const double p02 = P(0, 2); 27 | const double p10 = P(1, 0); 28 | const double p11 = P(1, 1); 29 | const double p12 = P(1, 2); 30 | const double p20 = P(2, 0); 31 | const double p21 = P(2, 1); 32 | const double p22 = P(2, 2); 33 | 34 | return std::array{ 35 | p00, p01, 0.0, 0.0, 0.0, p02, 36 | p10, p11, 0.0, 0.0, 0.0, p12, 37 | 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 38 | 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 39 | 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 40 | p20, p21, 0.0, 0.0, 0.0, p22 41 | }; 42 | } 43 | 44 | std::array EKFCovarianceToTwistMessageCovariance(const Matrix6d & P) 45 | { 46 | const double p44 = P(4, 4); 47 | const double p45 = P(4, 5); 48 | const double p54 = P(5, 4); 49 | const double p55 = P(5, 5); 50 | 51 | return std::array{ 52 | p44, 0.0, 0.0, 0.0, 0.0, p45, 53 | 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 54 | 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 55 | 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 56 | 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 57 | p54, 0.0, 0.0, 0.0, 0.0, p55 58 | }; 59 | } 60 | -------------------------------------------------------------------------------- /ekf_localizer/src/ekf_localizer_node.cpp: -------------------------------------------------------------------------------- 1 | // Copyright 2018-2019 Autoware Foundation 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #include "ekf_localizer/ekf_localizer.hpp" 16 | 17 | #include 18 | 19 | int main(int argc, char ** argv) 20 | { 21 | rclcpp::init(argc, argv); 22 | rclcpp::NodeOptions node_options; 23 | auto node = std::make_shared("ekf_localizer", node_options); 24 | 25 | rclcpp::spin(node); 26 | 27 | return 0; 28 | } 29 | -------------------------------------------------------------------------------- /ekf_localizer/src/mahalanobis.cpp: -------------------------------------------------------------------------------- 1 | // Copyright 2018-2019 Autoware Foundation 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | 16 | #include "ekf_localizer/mahalanobis.hpp" 17 | 18 | 19 | double SquaredMahalanobis( 20 | const Eigen::VectorXd & x, 21 | const Eigen::VectorXd & y, 22 | const Eigen::MatrixXd & C) 23 | { 24 | const Eigen::VectorXd d = x - y; 25 | return d.dot(C.inverse() * d); 26 | } 27 | 28 | bool MahalanobisGate( 29 | const double & dist_max, const Eigen::MatrixXd & x, const Eigen::MatrixXd & obj_x, 30 | const Eigen::MatrixXd & cov) 31 | { 32 | return SquaredMahalanobis(x, obj_x, cov) <= dist_max * dist_max; 33 | } 34 | -------------------------------------------------------------------------------- /ekf_localizer/src/string.cpp: -------------------------------------------------------------------------------- 1 | // Copyright 2018-2019 Autoware Foundation 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | 16 | #include "ekf_localizer/string.hpp" 17 | 18 | std::string EraseBeginSlash(const std::string & s) 19 | { 20 | std::string a = s; 21 | if (a.front() == '/') { 22 | a.erase(0, 1); 23 | } 24 | return a; 25 | } 26 | -------------------------------------------------------------------------------- /ekf_localizer/src/tf.cpp: -------------------------------------------------------------------------------- 1 | // Copyright 2018-2019 Autoware Foundation 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | 16 | #include 17 | #include 18 | #include 19 | 20 | #include "ekf_localizer/tf.hpp" 21 | 22 | std::optional TransformListener::LookupTransform( 23 | const std::string & parent_frame, 24 | const std::string & child_frame) const 25 | { 26 | rclcpp::sleep_for(std::chrono::milliseconds(100)); 27 | 28 | for (int i = 0; i < 50; ++i) { 29 | try { 30 | auto transform = tf_buffer_->lookupTransform(parent_frame, child_frame, tf2::TimePointZero); 31 | return std::make_optional(transform); 32 | } catch (tf2::TransformException & ex) { 33 | warning_->Warn(ex.what()); 34 | rclcpp::sleep_for(std::chrono::milliseconds(100)); 35 | } 36 | } 37 | return std::nullopt; 38 | } 39 | -------------------------------------------------------------------------------- /ekf_localizer/src/update_interval.cpp: -------------------------------------------------------------------------------- 1 | // Copyright 2018-2019 Autoware Foundation 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #include 16 | 17 | #include 18 | 19 | #include "ekf_localizer/update_interval.hpp" 20 | 21 | 22 | double UpdateInterval::Compute(const double current_time_second) 23 | { 24 | if (!last_time_.has_value()) { 25 | last_time_ = std::make_optional(current_time_second); 26 | return ComputeInterval(default_frequency_); 27 | } 28 | 29 | if (current_time_second < last_time_.value()) { 30 | throw std::invalid_argument( 31 | fmt::format( 32 | "Detected jump back in time. " 33 | "current time = {:10.9f}, last time = {:10.9f}", 34 | current_time_second, last_time_.value())); 35 | } 36 | 37 | const double frequency = 1.0 / (current_time_second - last_time_.value()); 38 | last_time_ = std::make_optional(current_time_second); 39 | return ComputeInterval(frequency); 40 | } 41 | -------------------------------------------------------------------------------- /ekf_localizer/test/test_covariance.cpp: -------------------------------------------------------------------------------- 1 | // Copyright 2022 Autoware Foundation 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #include 16 | 17 | #include "ekf_localizer/covariance.hpp" 18 | 19 | 20 | TEST(EKFCovarianceToPoseMessageCovariance, SmokeTest) 21 | { 22 | { 23 | Matrix6d P = Matrix6d::Zero(); 24 | P(0, 0) = 1.; 25 | P(0, 1) = 2.; 26 | P(0, 2) = 3.; 27 | P(1, 0) = 4.; 28 | P(1, 1) = 5.; 29 | P(1, 2) = 6.; 30 | P(2, 0) = 7.; 31 | P(2, 1) = 8.; 32 | P(2, 2) = 9.; 33 | 34 | std::array covariance = EKFCovarianceToPoseMessageCovariance(P); 35 | EXPECT_EQ(covariance(0), 1.); 36 | EXPECT_EQ(covariance(1), 2.); 37 | EXPECT_EQ(covariance(2), 3.); 38 | EXPECT_EQ(covariance(6), 4.); 39 | EXPECT_EQ(covariance(7), 5.); 40 | EXPECT_EQ(covariance(11), 6.); 41 | EXPECT_EQ(covariance(30), 7.); 42 | EXPECT_EQ(covariance(31), 8.); 43 | EXPECT_EQ(covariance(35), 9.); 44 | } 45 | 46 | // ensure other elements are zero 47 | { 48 | Matrix6d P = Matrix6d::Zero(); 49 | std::array covariance = EKFCovarianceToPoseMessageCovariance(P); 50 | for (double e : covariance) { 51 | EXPECT_EQ(e, 0.); 52 | } 53 | } 54 | } 55 | 56 | TEST(EKFCovarianceToTwistMessageCovariance, SmokeTest) 57 | { 58 | { 59 | Matrix6d P = Matrix6d::Zero(); 60 | P(4, 4) = 1.; 61 | P(4, 5) = 2.; 62 | P(5, 4) = 3.; 63 | P(5, 5) = 4.; 64 | 65 | std::array covariance = EKFCovarianceToTwistMessageCovariance(P); 66 | EXPECT_EQ(covariance(0), 1.); 67 | EXPECT_EQ(covariance(5), 2.); 68 | EXPECT_EQ(covariance(30), 3.); 69 | EXPECT_EQ(covariance(35), 4.); 70 | } 71 | 72 | // ensure other elements are zero 73 | { 74 | Matrix6d P = Matrix6d::Zero(); 75 | std::array covariance = EKFCovarianceToTwistMessageCovariance(P); 76 | for (double e : covariance) { 77 | EXPECT_EQ(e, 0.); 78 | } 79 | } 80 | } 81 | -------------------------------------------------------------------------------- /ekf_localizer/test/test_mahalanobis.cpp: -------------------------------------------------------------------------------- 1 | // Copyright 2018-2019 Autoware Foundation 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #include 16 | 17 | #include "ekf_localizer/mahalanobis.hpp" 18 | 19 | 20 | constexpr double tolerance = 1e-8; 21 | 22 | 23 | TEST(MahalanobisGate, SquaredMahalanobis) 24 | { 25 | { 26 | Eigen::Vector2d x(0, 1); 27 | Eigen::Vector2d y(3, 2); 28 | Eigen::Matrix2d C; 29 | C << 30 | 10, 0, 31 | 0, 10; 32 | 33 | EXPECT_NEAR(SquaredMahalanobis(x, y, C), 1.0, tolerance); 34 | } 35 | 36 | { 37 | Eigen::Vector2d x(4, 1); 38 | Eigen::Vector2d y(1, 5); 39 | Eigen::Matrix2d C; 40 | C << 41 | 5, 0, 42 | 0, 5; 43 | 44 | EXPECT_NEAR(SquaredMahalanobis(x, y, C), 5.0, tolerance); 45 | } 46 | } 47 | 48 | TEST(MahalanobisGate, MahalanobisGate) 49 | { 50 | Eigen::Vector2d x(0, 1); 51 | Eigen::Vector2d y(3, 2); 52 | Eigen::Matrix2d C; 53 | C << 54 | 10, 0, 55 | 0, 10; 56 | 57 | EXPECT_FALSE(MahalanobisGate(0.99, x, y, C)); 58 | EXPECT_FALSE(MahalanobisGate(1.00, x, y, C)); 59 | EXPECT_TRUE(MahalanobisGate(1.01, x, y, C)); 60 | } 61 | -------------------------------------------------------------------------------- /ekf_localizer/test/test_measurement.cpp: -------------------------------------------------------------------------------- 1 | // Copyright 2022 Autoware Foundation 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #include 16 | 17 | #include "ekf_localizer/twist_measurement.hpp" 18 | 19 | 20 | TEST(Measurement, TwistMeasurementMatrix) 21 | { 22 | const Eigen::Matrix M = TwistMeasurementMatrix(); 23 | Eigen::Matrix expected; 24 | expected << 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1; 25 | EXPECT_EQ((M - expected).norm(), 0); 26 | } 27 | 28 | TEST(Measurement, TwistMeasurementCovariance) 29 | { 30 | { 31 | const std::array covariance = { 32 | 1, 0, 0, 0, 0, 2, 0, 0, 0, 0, 0, 6, 33 | 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 34 | 0, 0, 0, 0, 0, 0, 3, 0, 0, 0, 0, 4}; 35 | 36 | const Eigen::Matrix2d M = TwistMeasurementCovariance(covariance, 2); 37 | 38 | Eigen::Matrix2d expected; 39 | expected << 2, 4, 6, 8; 40 | 41 | EXPECT_EQ((M - expected).norm(), 0.); 42 | } 43 | 44 | { 45 | // Make sure that other elements are not changed 46 | std::array covariance; 47 | covariance.fill(0); 48 | const Eigen::Matrix2d M = TwistMeasurementCovariance(covariance, 2.); 49 | EXPECT_EQ(M.norm(), 0); 50 | } 51 | } 52 | -------------------------------------------------------------------------------- /ekf_localizer/test/test_pose_measurement.cpp: -------------------------------------------------------------------------------- 1 | // Copyright 2022 Autoware Foundation 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #include 16 | 17 | #include "ekf_localizer/pose_measurement.hpp" 18 | 19 | 20 | TEST(Measurement, PoseMeasurementMatrix) 21 | { 22 | const Eigen::Matrix M = PoseMeasurementMatrix(); 23 | Eigen::Matrix expected; 24 | expected << 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0; 25 | EXPECT_EQ((M - expected).norm(), 0); 26 | } 27 | 28 | TEST(Measurement, PoseMeasurementCovariance) 29 | { 30 | { 31 | const std::array covariance = { 32 | 1, 2, 0, 0, 0, 3, 4, 5, 0, 0, 0, 6, 33 | 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 34 | 0, 0, 0, 0, 0, 0, 7, 8, 0, 0, 0, 9}; 35 | 36 | const Eigen::Matrix3d M = PoseMeasurementCovariance(covariance, 2); 37 | 38 | Eigen::Matrix3d expected; 39 | expected << 2, 4, 6, 8, 10, 12, 14, 16, 18; 40 | 41 | EXPECT_EQ((M - expected).norm(), 0.); 42 | } 43 | 44 | { 45 | // Make sure that other elements are not changed 46 | std::array covariance; 47 | covariance.fill(0); 48 | const Eigen::Matrix3d M = PoseMeasurementCovariance(covariance, 2.); 49 | EXPECT_EQ(M.norm(), 0); 50 | } 51 | } 52 | -------------------------------------------------------------------------------- /ekf_localizer/test/test_string.cpp: -------------------------------------------------------------------------------- 1 | // Copyright 2018-2019 Autoware Foundation 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #include 16 | 17 | #include "ekf_localizer/string.hpp" 18 | 19 | 20 | TEST(String, EraseBeginSlash) 21 | { 22 | EXPECT_EQ(EraseBeginSlash("/topic"), "topic"); 23 | EXPECT_EQ(EraseBeginSlash("topic"), "topic"); // do nothing 24 | 25 | EXPECT_EQ(EraseBeginSlash(""), ""); 26 | EXPECT_EQ(EraseBeginSlash("/"), ""); 27 | } 28 | -------------------------------------------------------------------------------- /ekf_localizer/test/test_update_interval.cpp: -------------------------------------------------------------------------------- 1 | // Copyright 2018-2019 Autoware Foundation 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | 16 | #include 17 | 18 | #include "ekf_localizer/update_interval.hpp" 19 | 20 | 21 | constexpr double threshold = 1e-8; 22 | 23 | 24 | TEST(UpdateInterval, SmokeTest) 25 | { 26 | UpdateInterval interval(10.); 27 | 28 | const double dt1 = interval.Compute(10000.10); 29 | EXPECT_NEAR(0.1, dt1, threshold); 30 | 31 | const double dt2 = interval.Compute(10000.20); 32 | EXPECT_NEAR(0.1, dt2, threshold); 33 | 34 | const double dt3 = interval.Compute(10000.50); 35 | EXPECT_NEAR(0.3, dt3, threshold); 36 | 37 | const double dt4 = interval.Compute(10000.70); 38 | EXPECT_NEAR(0.2, dt4, threshold); 39 | } 40 | 41 | TEST(UpdateInterval, DetectJumpBackInTime) 42 | { 43 | UpdateInterval interval(10.); 44 | 45 | interval.Compute(1659000000.200000048); 46 | 47 | EXPECT_THROW( 48 | try { 49 | interval.Compute(1659000000.099999905); 50 | } catch (std::invalid_argument & e) { 51 | EXPECT_STREQ( 52 | "Detected jump back in time. " 53 | "current time = 1659000000.099999905, last time = 1659000000.200000048", 54 | e.what()); 55 | throw e; 56 | } 57 | , 58 | std::invalid_argument 59 | ); 60 | } 61 | 62 | TEST(UpdateInterval, MaxInterval) { 63 | UpdateInterval interval(0.09); 64 | 65 | const double dt1 = interval.Compute(10000.0); 66 | EXPECT_NEAR(10.0, dt1, threshold); 67 | 68 | const double dt2 = interval.Compute(10011.00); 69 | EXPECT_NEAR(10.0, dt2, threshold); 70 | } 71 | -------------------------------------------------------------------------------- /extraction/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.16) 2 | project(lidar_feature_extraction) 3 | 4 | set(CMAKE_BUILD_TYPE "Debug") 5 | set(CMAKE_CXX_STANDARD 17) 6 | set(CMAKE_CXX_FLAGS 7 | "-pg -O3 -Wall -Wextra -Wpedantic -Werror -fprofile-arcs -ftest-coverage") 8 | set(CMAKE_CXX_OUTPUT_EXTENSION_REPLACE ON) 9 | set(BUILD_TESTING ON) 10 | 11 | find_package(fmt REQUIRED) 12 | find_package(ament_cmake_auto REQUIRED) 13 | 14 | ament_auto_find_build_dependencies() 15 | 16 | set(SOURCE_FILES 17 | src/convolution.cpp 18 | src/ring.cpp 19 | src/iterator.cpp 20 | src/index_range.cpp 21 | src/color_points.cpp 22 | src/math.cpp 23 | src/curvature.cpp) 24 | 25 | ament_auto_add_library(lidar_feature_extraction_lib SHARED ${SOURCE_FILES}) 26 | target_link_libraries(lidar_feature_extraction_lib fmt) 27 | 28 | ament_auto_add_executable(lidar_feature_extraction app/feature_extraction.cpp) 29 | target_link_libraries(lidar_feature_extraction lidar_feature_extraction_lib) 30 | 31 | function(add_testcase filepath) 32 | get_filename_component(filename ${filepath} NAME) 33 | string(REGEX REPLACE ".cpp" "" test_name ${filename}) 34 | 35 | message("test_name = ${test_name}") 36 | 37 | ament_add_gmock(${test_name} ${filepath}) 38 | target_include_directories(${test_name} PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}/include) 39 | target_link_libraries(${test_name} lidar_feature_extraction_lib) 40 | ament_target_dependencies(${test_name} ${lidar_feature_extraction_FOUND_BUILD_DEPENDS}) 41 | endfunction() 42 | 43 | if (BUILD_TESTING) 44 | find_package(ament_lint_auto REQUIRED) 45 | 46 | list(APPEND AMENT_LINT_AUTO_EXCLUDE 47 | ament_cmake_cpplint 48 | ament_cmake_uncrustify 49 | ) 50 | 51 | ament_lint_auto_find_test_dependencies() 52 | 53 | file(GLOB SOURCE_FILES src/* include/*) 54 | 55 | find_package(ament_cmake_cpplint) 56 | ament_cpplint(${SOURCE_FILES}) 57 | 58 | find_package(ament_cmake_uncrustify) 59 | ament_uncrustify(${SOURCE_FILES}) 60 | 61 | file(GLOB TEST_FILES test/*) 62 | foreach (filepath ${TEST_FILES}) 63 | add_testcase(${filepath}) 64 | endforeach() 65 | endif() 66 | 67 | ament_auto_package() 68 | -------------------------------------------------------------------------------- /extraction/LICENSE: -------------------------------------------------------------------------------- 1 | BSD 3-Clause License 2 | 3 | Copyright (c) 2020, Tixiao Shan, Takeshi Ishita 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 | * Redistributions of source code must retain the above copyright notice, this 10 | list of conditions and the following disclaimer. 11 | 12 | * 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 | * 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 | -------------------------------------------------------------------------------- /extraction/include/lidar_feature_extraction/cloud_iterator.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2022 Tixiao Shan, Takeshi Ishita 2 | // 3 | // Redistribution and use in source and binary forms, with or without 4 | // modification, are permitted provided that the following conditions are met: 5 | // 6 | // * Redistributions of source code must retain the above copyright 7 | // notice, this list of conditions and the following disclaimer. 8 | // 9 | // * Redistributions in binary form must reproduce the above copyright 10 | // notice, this list of conditions and the following disclaimer in the 11 | // documentation and/or other materials provided with the distribution. 12 | // 13 | // * Neither the name of the Tixiao Shan, Takeshi Ishita nor the names of its 14 | // contributors may be used to endorse or promote products derived from 15 | // this software without specific prior written permission. 16 | // 17 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 21 | // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | // POSSIBILITY OF SUCH DAMAGE. 28 | 29 | 30 | #ifndef LIDAR_FEATURE_EXTRACTION__CLOUD_ITERATOR_HPP_ 31 | #define LIDAR_FEATURE_EXTRACTION__CLOUD_ITERATOR_HPP_ 32 | 33 | #include 34 | #include 35 | 36 | template 37 | using CloudIterator = typename pcl::PointCloud::iterator; 38 | 39 | template 40 | using CloudConstIterator = typename pcl::PointCloud::const_iterator; 41 | 42 | #endif // LIDAR_FEATURE_EXTRACTION__CLOUD_ITERATOR_HPP_ 43 | -------------------------------------------------------------------------------- /extraction/include/lidar_feature_extraction/convolution.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2022 Tixiao Shan, Takeshi Ishita 2 | // 3 | // Redistribution and use in source and binary forms, with or without 4 | // modification, are permitted provided that the following conditions are met: 5 | // 6 | // * Redistributions of source code must retain the above copyright 7 | // notice, this list of conditions and the following disclaimer. 8 | // 9 | // * Redistributions in binary form must reproduce the above copyright 10 | // notice, this list of conditions and the following disclaimer in the 11 | // documentation and/or other materials provided with the distribution. 12 | // 13 | // * Neither the name of the Tixiao Shan, Takeshi Ishita nor the names of its 14 | // contributors may be used to endorse or promote products derived from 15 | // this software without specific prior written permission. 16 | // 17 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 21 | // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | // POSSIBILITY OF SUCH DAMAGE. 28 | 29 | 30 | #ifndef LIDAR_FEATURE_EXTRACTION__CONVOLUTION_HPP_ 31 | #define LIDAR_FEATURE_EXTRACTION__CONVOLUTION_HPP_ 32 | 33 | #include 34 | 35 | #include 36 | 37 | #include "math.hpp" 38 | 39 | std::vector Convolution1D( 40 | const std::vector input, 41 | const std::vector weight); 42 | 43 | #endif // LIDAR_FEATURE_EXTRACTION__CONVOLUTION_HPP_ 44 | -------------------------------------------------------------------------------- /extraction/include/lidar_feature_extraction/curvature.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2022 Tixiao Shan, Takeshi Ishita 2 | // 3 | // Redistribution and use in source and binary forms, with or without 4 | // modification, are permitted provided that the following conditions are met: 5 | // 6 | // * Redistributions of source code must retain the above copyright 7 | // notice, this list of conditions and the following disclaimer. 8 | // 9 | // * Redistributions in binary form must reproduce the above copyright 10 | // notice, this list of conditions and the following disclaimer in the 11 | // documentation and/or other materials provided with the distribution. 12 | // 13 | // * Neither the name of the Tixiao Shan, Takeshi Ishita nor the names of its 14 | // contributors may be used to endorse or promote products derived from 15 | // this software without specific prior written permission. 16 | // 17 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 21 | // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | // POSSIBILITY OF SUCH DAMAGE. 28 | 29 | 30 | #ifndef LIDAR_FEATURE_EXTRACTION__CURVATURE_HPP_ 31 | #define LIDAR_FEATURE_EXTRACTION__CURVATURE_HPP_ 32 | 33 | #include 34 | #include 35 | 36 | #include 37 | 38 | #include "math.hpp" 39 | #include "convolution.hpp" 40 | 41 | std::vector MakeWeight(const int padding); 42 | 43 | std::vector CalcCurvature(const std::vector & range, const int padding); 44 | 45 | #endif // LIDAR_FEATURE_EXTRACTION__CURVATURE_HPP_ 46 | -------------------------------------------------------------------------------- /extraction/include/lidar_feature_extraction/iterator.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2022 Tixiao Shan, Takeshi Ishita 2 | // 3 | // Redistribution and use in source and binary forms, with or without 4 | // modification, are permitted provided that the following conditions are met: 5 | // 6 | // * Redistributions of source code must retain the above copyright 7 | // notice, this list of conditions and the following disclaimer. 8 | // 9 | // * Redistributions in binary form must reproduce the above copyright 10 | // notice, this list of conditions and the following disclaimer in the 11 | // documentation and/or other materials provided with the distribution. 12 | // 13 | // * Neither the name of the Tixiao Shan, Takeshi Ishita nor the names of its 14 | // contributors may be used to endorse or promote products derived from 15 | // this software without specific prior written permission. 16 | // 17 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 21 | // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | // POSSIBILITY OF SUCH DAMAGE. 28 | 29 | #ifndef LIDAR_FEATURE_EXTRACTION__ITERATOR_HPP_ 30 | #define LIDAR_FEATURE_EXTRACTION__ITERATOR_HPP_ 31 | 32 | #include 33 | 34 | #include 35 | 36 | template 37 | using ElementType = typename std::iterator_traits::value_type; 38 | 39 | std::vector irange(const int size); 40 | 41 | #endif // LIDAR_FEATURE_EXTRACTION__ITERATOR_HPP_ 42 | -------------------------------------------------------------------------------- /extraction/include/lidar_feature_extraction/math.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2022 Tixiao Shan, Takeshi Ishita 2 | // 3 | // Redistribution and use in source and binary forms, with or without 4 | // modification, are permitted provided that the following conditions are met: 5 | // 6 | // * Redistributions of source code must retain the above copyright 7 | // notice, this list of conditions and the following disclaimer. 8 | // 9 | // * Redistributions in binary form must reproduce the above copyright 10 | // notice, this list of conditions and the following disclaimer in the 11 | // documentation and/or other materials provided with the distribution. 12 | // 13 | // * Neither the name of the Tixiao Shan, Takeshi Ishita nor the names of its 14 | // contributors may be used to endorse or promote products derived from 15 | // this software without specific prior written permission. 16 | // 17 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 21 | // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | // POSSIBILITY OF SUCH DAMAGE. 28 | 29 | 30 | #ifndef LIDAR_FEATURE_EXTRACTION__MATH_HPP_ 31 | #define LIDAR_FEATURE_EXTRACTION__MATH_HPP_ 32 | 33 | #include 34 | 35 | 36 | inline double XYNorm(const double x, const double y) 37 | { 38 | return std::sqrt(x * x + y * y); 39 | } 40 | 41 | double CalcRadian(const double x1, const double y1, const double x2, const double y2); 42 | 43 | template 44 | double InnerProduct(T1 first1, T1 last1, T2 first2) 45 | { 46 | double sum = 0.; 47 | while (first1 != last1) { 48 | sum += (*first1) * (*first2); 49 | first1++; 50 | first2++; 51 | } 52 | return sum; 53 | } 54 | 55 | #endif // LIDAR_FEATURE_EXTRACTION__MATH_HPP_ 56 | -------------------------------------------------------------------------------- /extraction/include/lidar_feature_extraction/out_of_range.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2022 Tixiao Shan, Takeshi Ishita 2 | // 3 | // Redistribution and use in source and binary forms, with or without 4 | // modification, are permitted provided that the following conditions are met: 5 | // 6 | // * Redistributions of source code must retain the above copyright 7 | // notice, this list of conditions and the following disclaimer. 8 | // 9 | // * Redistributions in binary form must reproduce the above copyright 10 | // notice, this list of conditions and the following disclaimer in the 11 | // documentation and/or other materials provided with the distribution. 12 | // 13 | // * Neither the name of the Tixiao Shan, Takeshi Ishita nor the names of its 14 | // contributors may be used to endorse or promote products derived from 15 | // this software without specific prior written permission. 16 | // 17 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 21 | // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | // POSSIBILITY OF SUCH DAMAGE. 28 | 29 | #ifndef LIDAR_FEATURE_EXTRACTION__OUT_OF_RANGE_HPP_ 30 | #define LIDAR_FEATURE_EXTRACTION__OUT_OF_RANGE_HPP_ 31 | 32 | #include 33 | 34 | #include "lidar_feature_extraction/label.hpp" 35 | 36 | template 37 | void LabelOutOfRange( 38 | std::vector & labels, 39 | const Range & range, 40 | const double min_range, 41 | const double max_range) 42 | { 43 | for (int i = 0; i < range.size(); i++) { 44 | if (!IsInInclusiveRange(range(i), min_range, max_range)) { 45 | labels.at(i) = PointLabel::OutOfRange; 46 | } 47 | } 48 | } 49 | 50 | #endif // LIDAR_FEATURE_EXTRACTION__OUT_OF_RANGE_HPP_ 51 | -------------------------------------------------------------------------------- /extraction/include/lidar_feature_extraction/point_label.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2022 Tixiao Shan, Takeshi Ishita 2 | // 3 | // Redistribution and use in source and binary forms, with or without 4 | // modification, are permitted provided that the following conditions are met: 5 | // 6 | // * Redistributions of source code must retain the above copyright 7 | // notice, this list of conditions and the following disclaimer. 8 | // 9 | // * Redistributions in binary form must reproduce the above copyright 10 | // notice, this list of conditions and the following disclaimer in the 11 | // documentation and/or other materials provided with the distribution. 12 | // 13 | // * Neither the name of the Tixiao Shan, Takeshi Ishita nor the names of its 14 | // contributors may be used to endorse or promote products derived from 15 | // this software without specific prior written permission. 16 | // 17 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 21 | // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | // POSSIBILITY OF SUCH DAMAGE. 28 | 29 | #ifndef LIDAR_FEATURE_EXTRACTION__POINT_LABEL_HPP_ 30 | #define LIDAR_FEATURE_EXTRACTION__POINT_LABEL_HPP_ 31 | 32 | enum class PointLabel : uint8_t 33 | { 34 | Default, 35 | Edge, 36 | EdgeNeighbor, 37 | Surface, 38 | SurfaceNeighbor, 39 | OutOfRange, 40 | Occluded, 41 | ParallelBeam 42 | }; 43 | 44 | #endif // LIDAR_FEATURE_EXTRACTION__POINT_LABEL_HPP_ 45 | -------------------------------------------------------------------------------- /extraction/include/lidar_feature_extraction/subscription.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2022 Tixiao Shan, Takeshi Ishita 2 | // 3 | // Redistribution and use in source and binary forms, with or without 4 | // modification, are permitted provided that the following conditions are met: 5 | // 6 | // * Redistributions of source code must retain the above copyright 7 | // notice, this list of conditions and the following disclaimer. 8 | // 9 | // * Redistributions in binary form must reproduce the above copyright 10 | // notice, this list of conditions and the following disclaimer in the 11 | // documentation and/or other materials provided with the distribution. 12 | // 13 | // * Neither the name of the Tixiao Shan, Takeshi Ishita nor the names of its 14 | // contributors may be used to endorse or promote products derived from 15 | // this software without specific prior written permission. 16 | // 17 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 21 | // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | // POSSIBILITY OF SUCH DAMAGE. 28 | 29 | #ifndef LIDAR_FEATURE_EXTRACTION__SUBSCRIPTION_HPP_ 30 | #define LIDAR_FEATURE_EXTRACTION__SUBSCRIPTION_HPP_ 31 | 32 | inline rclcpp::SubscriptionOptions MutuallyExclusiveOption(rclcpp::Node & node) 33 | { 34 | const rclcpp::CallbackGroup::SharedPtr callback_group = 35 | node.create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); 36 | auto main_sub_opt = rclcpp::SubscriptionOptions(); 37 | main_sub_opt.callback_group = callback_group; 38 | return main_sub_opt; 39 | } 40 | 41 | #endif // LIDAR_FEATURE_EXTRACTION__SUBSCRIPTION_HPP_ 42 | -------------------------------------------------------------------------------- /extraction/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | lidar_feature_extraction 4 | 0.1.0 5 | Lidar feature extraction 6 | 7 | BSD 3-Clause 8 | Takeshi Ishita 9 | Tixiao Shan 10 | 11 | ament_cmake_auto 12 | eigen3_cmake_module 13 | 14 | eigen 15 | fmt 16 | libpcl-all-dev 17 | lidar_feature_library 18 | pcl_conversions 19 | pcl_ros 20 | range-v3 21 | rclcpp 22 | sensor_msgs 23 | std_msgs 24 | tf2 25 | tf2_geometry_msgs 26 | tf2_ros 27 | 28 | ament_cmake_gtest 29 | ament_cmake_gmock 30 | 31 | 32 | ament_cmake 33 | 34 | 35 | 36 | -------------------------------------------------------------------------------- /extraction/src/curvature.cpp: -------------------------------------------------------------------------------- 1 | // Copyright 2022 Tixiao Shan, Takeshi Ishita 2 | // 3 | // Redistribution and use in source and binary forms, with or without 4 | // modification, are permitted provided that the following conditions are met: 5 | // 6 | // * Redistributions of source code must retain the above copyright 7 | // notice, this list of conditions and the following disclaimer. 8 | // 9 | // * Redistributions in binary form must reproduce the above copyright 10 | // notice, this list of conditions and the following disclaimer in the 11 | // documentation and/or other materials provided with the distribution. 12 | // 13 | // * Neither the name of the Tixiao Shan, Takeshi Ishita nor the names of its 14 | // contributors may be used to endorse or promote products derived from 15 | // this software without specific prior written permission. 16 | // 17 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 21 | // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | // POSSIBILITY OF SUCH DAMAGE. 28 | 29 | 30 | #include 31 | #include 32 | 33 | #include "lidar_feature_extraction/curvature.hpp" 34 | 35 | 36 | std::vector MakeWeight(const int padding) 37 | { 38 | assert(padding > 0); 39 | std::vector weight(padding * 2 + 1, 1.); 40 | weight.at(padding) = -2. * padding; 41 | return weight; 42 | } 43 | 44 | std::vector CalcCurvature(const std::vector & range, const int padding) 45 | { 46 | const std::vector weight = MakeWeight(padding); 47 | auto f = [](const double v) {return v * v;}; 48 | const auto weighted = Convolution1D(range, weight); 49 | return weighted | ranges::views::transform(f) | ranges::to_vector; 50 | } 51 | -------------------------------------------------------------------------------- /extraction/src/iterator.cpp: -------------------------------------------------------------------------------- 1 | // Copyright 2022 Tixiao Shan, Takeshi Ishita 2 | // 3 | // Redistribution and use in source and binary forms, with or without 4 | // modification, are permitted provided that the following conditions are met: 5 | // 6 | // * Redistributions of source code must retain the above copyright 7 | // notice, this list of conditions and the following disclaimer. 8 | // 9 | // * Redistributions in binary form must reproduce the above copyright 10 | // notice, this list of conditions and the following disclaimer in the 11 | // documentation and/or other materials provided with the distribution. 12 | // 13 | // * Neither the name of the Tixiao Shan, Takeshi Ishita nor the names of its 14 | // contributors may be used to endorse or promote products derived from 15 | // this software without specific prior written permission. 16 | // 17 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 21 | // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | // POSSIBILITY OF SUCH DAMAGE. 28 | 29 | #include 30 | 31 | #include 32 | 33 | std::vector irange(const int size) 34 | { 35 | return ranges::views::ints(0, size) | ranges::to_vector; 36 | } 37 | -------------------------------------------------------------------------------- /extraction/src/math.cpp: -------------------------------------------------------------------------------- 1 | // Copyright 2022 Tixiao Shan, Takeshi Ishita 2 | // 3 | // Redistribution and use in source and binary forms, with or without 4 | // modification, are permitted provided that the following conditions are met: 5 | // 6 | // * Redistributions of source code must retain the above copyright 7 | // notice, this list of conditions and the following disclaimer. 8 | // 9 | // * Redistributions in binary form must reproduce the above copyright 10 | // notice, this list of conditions and the following disclaimer in the 11 | // documentation and/or other materials provided with the distribution. 12 | // 13 | // * Neither the name of the Tixiao Shan, Takeshi Ishita nor the names of its 14 | // contributors may be used to endorse or promote products derived from 15 | // this software without specific prior written permission. 16 | // 17 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 21 | // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | // POSSIBILITY OF SUCH DAMAGE. 28 | 29 | #include 30 | 31 | #include "lidar_feature_extraction/math.hpp" 32 | 33 | 34 | double CalcRadian(const double x1, const double y1, const double x2, const double y2) 35 | { 36 | const double dot = x1 * x2 + y1 * y2; 37 | const double norm1 = XYNorm(x1, y1); 38 | const double norm2 = XYNorm(x2, y2); 39 | 40 | if (norm1 == 0 && norm2 == 0) { 41 | throw std::invalid_argument("All input values are zero. Angle cannot be calculated"); 42 | } 43 | 44 | const double cos_angle = dot / (norm1 * norm2); 45 | return std::acos(cos_angle); 46 | } 47 | -------------------------------------------------------------------------------- /extraction/src/ring.cpp: -------------------------------------------------------------------------------- 1 | // Copyright 2022 Tixiao Shan, Takeshi Ishita 2 | // 3 | // Redistribution and use in source and binary forms, with or without 4 | // modification, are permitted provided that the following conditions are met: 5 | // 6 | // * Redistributions of source code must retain the above copyright 7 | // notice, this list of conditions and the following disclaimer. 8 | // 9 | // * Redistributions in binary form must reproduce the above copyright 10 | // notice, this list of conditions and the following disclaimer in the 11 | // documentation and/or other materials provided with the distribution. 12 | // 13 | // * Neither the name of the Tixiao Shan, Takeshi Ishita nor the names of its 14 | // contributors may be used to endorse or promote products derived from 15 | // this software without specific prior written permission. 16 | // 17 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 21 | // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | // POSSIBILITY OF SUCH DAMAGE. 28 | 29 | 30 | #include 31 | #include 32 | 33 | #include "lidar_feature_extraction/ring.hpp" 34 | 35 | 36 | bool RingIsAvailable(const std::vector & fields) 37 | { 38 | for (const auto & field : fields) { 39 | if (field.name == "ring") { 40 | return true; 41 | } 42 | } 43 | return false; 44 | } 45 | 46 | void RemoveSparseRings( 47 | std::unordered_map> & rings, 48 | const int n_min_points) 49 | { 50 | for (auto it = rings.begin(); it != rings.end(); ) { 51 | const auto & indices = it->second; 52 | const int size = static_cast(indices.size()); 53 | if (size < n_min_points) { 54 | it = rings.erase(it); 55 | continue; 56 | } 57 | it++; 58 | } 59 | } 60 | -------------------------------------------------------------------------------- /extraction/test/test_algorithm.cpp: -------------------------------------------------------------------------------- 1 | // Copyright 2022 Tixiao Shan, Takeshi Ishita 2 | // 3 | // Redistribution and use in source and binary forms, with or without 4 | // modification, are permitted provided that the following conditions are met: 5 | // 6 | // * Redistributions of source code must retain the above copyright 7 | // notice, this list of conditions and the following disclaimer. 8 | // 9 | // * Redistributions in binary form must reproduce the above copyright 10 | // notice, this list of conditions and the following disclaimer in the 11 | // documentation and/or other materials provided with the distribution. 12 | // 13 | // * Neither the name of the Tixiao Shan, Takeshi Ishita nor the names of its 14 | // contributors may be used to endorse or promote products derived from 15 | // this software without specific prior written permission. 16 | // 17 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 21 | // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | // POSSIBILITY OF SUCH DAMAGE. 28 | 29 | #include 30 | 31 | #include 32 | 33 | #include "lidar_feature_extraction/algorithm.hpp" 34 | 35 | 36 | TEST(Algorithm, Argsort) 37 | { 38 | { 39 | const std::vector values{0.3, 0.2, 1.0, 0.2, 0.0, 0.1}; 40 | const std::vector indices = Argsort(values.begin(), values.end()); 41 | EXPECT_THAT(indices, testing::ElementsAre(4, 5, 1, 3, 0, 2)); 42 | } 43 | 44 | { 45 | const std::vector values{0.0, 0.0, 0.0, 0.0, 0.0}; 46 | const std::vector indices = Argsort(values.begin(), values.end()); 47 | EXPECT_THAT(indices, testing::ElementsAre(0, 1, 2, 3, 4)); 48 | } 49 | } 50 | -------------------------------------------------------------------------------- /extraction/test/test_iterator.cpp: -------------------------------------------------------------------------------- 1 | // Copyright 2022 Tixiao Shan, Takeshi Ishita 2 | // 3 | // Redistribution and use in source and binary forms, with or without 4 | // modification, are permitted provided that the following conditions are met: 5 | // 6 | // * Redistributions of source code must retain the above copyright 7 | // notice, this list of conditions and the following disclaimer. 8 | // 9 | // * Redistributions in binary form must reproduce the above copyright 10 | // notice, this list of conditions and the following disclaimer in the 11 | // documentation and/or other materials provided with the distribution. 12 | // 13 | // * Neither the name of the Tixiao Shan, Takeshi Ishita nor the names of its 14 | // contributors may be used to endorse or promote products derived from 15 | // this software without specific prior written permission. 16 | // 17 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 21 | // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | // POSSIBILITY OF SUCH DAMAGE. 28 | 29 | 30 | #include 31 | 32 | #include "lidar_feature_extraction/iterator.hpp" 33 | 34 | 35 | TEST(Iterator, Irange) 36 | { 37 | EXPECT_THAT(irange(4), testing::ElementsAre(0, 1, 2, 3)); 38 | } 39 | -------------------------------------------------------------------------------- /imu_integration/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.16) 2 | project(imu_integration) 3 | 4 | set(CMAKE_BUILD_TYPE "Debug") 5 | set(CMAKE_CXX_STANDARD 17) 6 | set(CMAKE_CXX_FLAGS "-g -Wall -Wextra -Wpedantic -Werror -fprofile-arcs -ftest-coverage") 7 | set(CMAKE_CXX_OUTPUT_EXTENSION_REPLACE ON) 8 | set(BUILD_TESTING ON) 9 | 10 | find_package(ament_cmake REQUIRED) 11 | find_package(ament_cmake_auto REQUIRED) 12 | 13 | ament_auto_find_build_dependencies() 14 | 15 | ament_auto_add_executable(${PROJECT_NAME} src/integration.cpp) 16 | 17 | function(add_testcase filepath) 18 | get_filename_component(filename ${filepath} NAME) 19 | string(REGEX REPLACE ".cpp" "" test_name ${filename}) 20 | 21 | message("test_name = ${test_name}") 22 | 23 | ament_add_gmock(${test_name} ${filepath}) 24 | target_include_directories(${test_name} PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}/include) 25 | target_link_libraries(${test_name} fmt) 26 | ament_target_dependencies(${test_name} ${${PROJECT_NAME}_FOUND_BUILD_DEPENDS}) 27 | endfunction() 28 | 29 | if (BUILD_TESTING) 30 | find_package(ament_lint_auto REQUIRED) 31 | 32 | list(APPEND AMENT_LINT_AUTO_EXCLUDE 33 | ament_cmake_cpplint 34 | ament_cmake_uncrustify 35 | ) 36 | 37 | ament_lint_auto_find_test_dependencies() 38 | 39 | file(GLOB SOURCE_FILES src/* include/*) 40 | 41 | find_package(ament_cmake_cpplint) 42 | ament_cpplint(${SOURCE_FILES}) 43 | 44 | find_package(ament_cmake_uncrustify) 45 | ament_uncrustify(${SOURCE_FILES}) 46 | 47 | file(GLOB TEST_FILES test/*) 48 | foreach(filepath ${TEST_FILES}) 49 | add_testcase(${filepath}) 50 | endforeach() 51 | endif() 52 | 53 | ament_auto_package() 54 | -------------------------------------------------------------------------------- /imu_integration/include/imu_integration/integration.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2022 Ishita Takeshi 2 | // 3 | // Redistribution and use in source and binary forms, with or without 4 | // modification, are permitted provided that the following conditions are met: 5 | // 6 | // * Redistributions of source code must retain the above copyright 7 | // notice, this list of conditions and the following disclaimer. 8 | // 9 | // * Redistributions in binary form must reproduce the above copyright 10 | // notice, this list of conditions and the following disclaimer in the 11 | // documentation and/or other materials provided with the distribution. 12 | // 13 | // * Neither the name of the Ishita Takeshi nor the names of its 14 | // contributors may be used to endorse or promote products derived from 15 | // this software without specific prior written permission. 16 | // 17 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 21 | // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | // POSSIBILITY OF SUCH DAMAGE. 28 | 29 | 30 | #ifndef IMU_INTEGRATION__INTEGRATION_HPP_ 31 | #define IMU_INTEGRATION__INTEGRATION_HPP_ 32 | 33 | #endif // IMU_INTEGRATION__INTEGRATION_HPP_ 34 | -------------------------------------------------------------------------------- /imu_integration/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | imu_integration 4 | 0.0.0 5 | IMU integration 6 | 7 | BSD 3-Clause 8 | Takeshi Ishita 9 | 10 | ament_cmake_auto 11 | ament_cmake 12 | 13 | eigen 14 | rotationlib 15 | rclcpp 16 | 17 | ament_cmake_gtest 18 | ament_cmake_gmock 19 | 20 | 21 | ament_cmake 22 | 23 | 24 | 25 | -------------------------------------------------------------------------------- /imu_integration/src/integration.cpp: -------------------------------------------------------------------------------- 1 | // Copyright 2022 Ishita Takeshi 2 | // 3 | // Redistribution and use in source and binary forms, with or without 4 | // modification, are permitted provided that the following conditions are met: 5 | // 6 | // * Redistributions of source code must retain the above copyright 7 | // notice, this list of conditions and the following disclaimer. 8 | // 9 | // * Redistributions in binary form must reproduce the above copyright 10 | // notice, this list of conditions and the following disclaimer in the 11 | // documentation and/or other materials provided with the distribution. 12 | // 13 | // * Neither the name of the Ishita Takeshi nor the names of its 14 | // contributors may be used to endorse or promote products derived from 15 | // this software without specific prior written permission. 16 | // 17 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 21 | // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | // POSSIBILITY OF SUCH DAMAGE. 28 | 29 | #include 30 | #include 31 | 32 | #include 33 | 34 | #include "imu_integration/integration.hpp" 35 | 36 | 37 | class Integration : public rclcpp::Node 38 | { 39 | public: 40 | Integration() 41 | : Node("imu_integration") 42 | { 43 | } 44 | }; 45 | 46 | int main(int argc, char * argv[]) 47 | { 48 | rclcpp::init(argc, argv); 49 | 50 | rclcpp::spin(std::make_shared()); 51 | 52 | rclcpp::shutdown(); 53 | } 54 | -------------------------------------------------------------------------------- /imu_integration/test/test_rkmk.cpp: -------------------------------------------------------------------------------- 1 | // Copyright 2022 Ishita Takeshi 2 | // 3 | // Redistribution and use in source and binary forms, with or without 4 | // modification, are permitted provided that the following conditions are met: 5 | // 6 | // * Redistributions of source code must retain the above copyright 7 | // notice, this list of conditions and the following disclaimer. 8 | // 9 | // * Redistributions in binary form must reproduce the above copyright 10 | // notice, this list of conditions and the following disclaimer in the 11 | // documentation and/or other materials provided with the distribution. 12 | // 13 | // * Neither the name of the Ishita Takeshi nor the names of its 14 | // contributors may be used to endorse or promote products derived from 15 | // this software without specific prior written permission. 16 | // 17 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 21 | // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | // POSSIBILITY OF SUCH DAMAGE. 28 | 29 | #include 30 | -------------------------------------------------------------------------------- /kalman_filter/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.14) 2 | project(kalman_filter) 3 | 4 | set(CMAKE_CXX_FLAGS "-g -O3 -Wall -Wextra -Wpedantic -Werror -fprofile-arcs -ftest-coverage") 5 | 6 | find_package(ament_cmake_auto REQUIRED) 7 | ament_auto_find_build_dependencies() 8 | 9 | find_package(eigen3_cmake_module REQUIRED) 10 | find_package(Eigen3 REQUIRED) 11 | 12 | include_directories( 13 | SYSTEM 14 | ${EIGEN3_INCLUDE_DIR} 15 | ) 16 | 17 | ament_auto_add_library(kalman_filter SHARED 18 | src/kalman_filter.cpp 19 | src/time_delay_kalman_filter.cpp 20 | include/kalman_filter/kalman_filter.hpp 21 | include/kalman_filter/time_delay_kalman_filter.hpp 22 | ) 23 | 24 | function(add_testcase filepath) 25 | get_filename_component(filename ${filepath} NAME) 26 | string(REGEX REPLACE ".cpp" "" test_name ${filename}) 27 | 28 | ament_add_gtest(${test_name} ${filepath}) 29 | target_link_libraries("${test_name}" kalman_filter) 30 | ament_target_dependencies(${test_name} ${${PROJECT_NAME}_FOUND_BUILD_DEPENDS}) 31 | endfunction() 32 | 33 | if(BUILD_TESTING) 34 | find_package(ament_cmake_gtest REQUIRED) 35 | 36 | file(GLOB 37 | FILES_TO_CHECK 38 | ${CMAKE_CURRENT_SOURCE_DIR}/src/* 39 | ${CMAKE_CURRENT_SOURCE_DIR}/include/* 40 | ${CMAKE_CURRENT_SOURCE_DIR}/test/*) 41 | 42 | find_package(ament_cmake_cpplint) 43 | ament_cpplint(${FILES_TO_CHECK}) 44 | 45 | find_package(ament_cmake_uncrustify) 46 | ament_uncrustify(${FILES_TO_CHECK}) 47 | 48 | set(TEST_FILES 49 | test/test_matrix_size.cpp 50 | test/test_time_delay_kalman_filter.cpp) 51 | 52 | foreach(filepath ${TEST_FILES}) 53 | add_testcase(${filepath}) 54 | endforeach() 55 | endif() 56 | 57 | ament_auto_package() 58 | -------------------------------------------------------------------------------- /kalman_filter/LICENSE: -------------------------------------------------------------------------------- 1 | Copyright 2018-2019 Autoware Foundation 2 | 3 | Licensed under the Apache License, Version 2.0 (the "License"); 4 | you may not use this file except in compliance with the License. 5 | You may obtain a copy of the License at 6 | 7 | http://www.apache.org/licenses/LICENSE-2.0 8 | 9 | Unless required by applicable law or agreed to in writing, software 10 | distributed under the License is distributed on an "AS IS" BASIS, 11 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | See the License for the specific language governing permissions and 13 | limitations under the License. 14 | -------------------------------------------------------------------------------- /kalman_filter/README.md: -------------------------------------------------------------------------------- 1 | # kalman_filter 2 | 3 | ## Purpose 4 | 5 | This common package contains the kalman filter with time delay and the calculation of the kalman filter. 6 | 7 | ## Assumptions / Known limits 8 | 9 | TBD. 10 | -------------------------------------------------------------------------------- /kalman_filter/include/kalman_filter/matrix_size.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2022 Autoware Foundation 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #ifndef KALMAN_FILTER__MATRIX_SIZE_HPP_ 16 | #define KALMAN_FILTER__MATRIX_SIZE_HPP_ 17 | 18 | #include "kalman_filter/kalman_filter.hpp" 19 | 20 | inline bool hasZeroElements(const Eigen::MatrixXd & M) 21 | { 22 | return M.size() == 0; 23 | } 24 | 25 | #endif // KALMAN_FILTER__MATRIX_SIZE_HPP_ 26 | -------------------------------------------------------------------------------- /kalman_filter/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | kalman_filter 5 | 0.1.0 6 | The kalman filter package 7 | Yukihiro Saito Horibe 8 | Apache License 2.0 9 | 10 | Takamasa Horibe 11 | 12 | ament_cmake_auto 13 | 14 | lidar_feature_library 15 | 16 | ament_cmake_cppcheck 17 | ament_cmake_gtest 18 | ament_lint_auto 19 | 20 | 21 | ament_cmake 22 | 23 | 24 | -------------------------------------------------------------------------------- /kalman_filter/src/kalman_filter.cpp: -------------------------------------------------------------------------------- 1 | // Copyright 2018-2019 Autoware Foundation 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #include "kalman_filter/kalman_filter.hpp" 16 | 17 | #include 18 | 19 | #include "kalman_filter/matrix_size.hpp" 20 | 21 | #include "lidar_feature_library/numeric.hpp" 22 | 23 | 24 | KalmanFilter::KalmanFilter( 25 | const Eigen::VectorXd & x, const Eigen::MatrixXd & A, const Eigen::MatrixXd & B, 26 | const Eigen::MatrixXd & C, const Eigen::MatrixXd & Q, const Eigen::MatrixXd & R, 27 | const Eigen::MatrixXd & P) 28 | : x_(x), P_(P), A_(A), B_(B), C_(C), Q_(Q), R_(R) 29 | { 30 | assert(!hasZeroElements(x)); 31 | assert(!hasZeroElements(P)); 32 | assert(!hasZeroElements(A)); 33 | assert(!hasZeroElements(B)); 34 | assert(!hasZeroElements(C)); 35 | assert(!hasZeroElements(Q)); 36 | assert(!hasZeroElements(R)); 37 | } 38 | 39 | void KalmanFilter::init(const Eigen::VectorXd & x, const Eigen::MatrixXd & P0) 40 | { 41 | assert(!hasZeroElements(x)); 42 | assert(!hasZeroElements(P0)); 43 | x_ = x; 44 | P_ = P0; 45 | } 46 | 47 | double KalmanFilter::getXelement(unsigned int i) const {return x_(i);} 48 | 49 | void KalmanFilter::predict( 50 | const Eigen::VectorXd & u, const Eigen::MatrixXd & A, const Eigen::MatrixXd & B, 51 | const Eigen::MatrixXd & Q) 52 | { 53 | assert(A.cols() == x_.rows()); 54 | assert(B.cols() == u.rows()); 55 | assert(A.cols() == P_.rows()); 56 | assert(Q.cols() == Q.rows()); 57 | assert(A.rows() == Q.cols()); 58 | 59 | x_ = predictNextState(x_, u, A, B); 60 | P_ = predictNextCovariance(P_, A, Q); 61 | } 62 | 63 | void KalmanFilter::predict(const Eigen::VectorXd & u) {return predict(u, A_, B_, Q_);} 64 | 65 | void KalmanFilter::update(const Eigen::VectorXd & y) 66 | { 67 | const Eigen::MatrixXd K = calcKalmanGain(P_, C_, R_); 68 | 69 | if (HasNan(K) || HasInf(K)) { 70 | throw std::invalid_argument("K has invalid value"); 71 | } 72 | 73 | x_ = updateState(x_, y, C_, K); 74 | P_ = updateCovariance(P_, C_, K); 75 | } 76 | -------------------------------------------------------------------------------- /kalman_filter/test/test_matrix_size.cpp: -------------------------------------------------------------------------------- 1 | // Copyright 2022 Autoware Foundation 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #include 16 | 17 | #include "kalman_filter/matrix_size.hpp" 18 | 19 | 20 | TEST(HasZeroElements, SmokeTest) 21 | { 22 | EXPECT_TRUE(hasZeroElements(Eigen::MatrixXd(0, 0))); 23 | EXPECT_TRUE(hasZeroElements(Eigen::MatrixXd(0, 1))); 24 | EXPECT_TRUE(hasZeroElements(Eigen::MatrixXd(1, 0))); 25 | 26 | EXPECT_FALSE(hasZeroElements(Eigen::MatrixXd(1, 1))); 27 | EXPECT_FALSE(hasZeroElements(Eigen::MatrixXd(1, 2))); 28 | } 29 | -------------------------------------------------------------------------------- /lib/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.16) 2 | project(lidar_feature_library) 3 | 4 | set(CMAKE_BUILD_TYPE "Debug") 5 | set(CMAKE_CXX_STANDARD 17) 6 | set(CMAKE_CXX_FLAGS 7 | "-pg -O3 -Wall -Wextra -Wpedantic -Werror -fprofile-arcs -ftest-coverage") 8 | set(CMAKE_CXX_OUTPUT_EXTENSION_REPLACE ON) 9 | 10 | find_package(ament_cmake REQUIRED) 11 | find_package(ament_cmake_auto REQUIRED) 12 | 13 | find_package(fmt REQUIRED) 14 | find_package(PCL REQUIRED) 15 | 16 | ament_auto_find_build_dependencies() 17 | 18 | set(SOURCE_FILES 19 | src/algorithm.cpp 20 | src/lib.cpp 21 | src/eigen.cpp 22 | src/pcl_utils.cpp 23 | src/ros_msg.cpp 24 | src/random.cpp 25 | src/stats.cpp) 26 | 27 | ament_auto_add_library(${PROJECT_NAME} SHARED ${SOURCE_FILES}) 28 | target_link_libraries(${PROJECT_NAME} ${PCL_LIBRARIES}) 29 | 30 | function(add_testcase filepath) 31 | find_package(ament_cmake_gmock REQUIRED) 32 | 33 | get_filename_component(filename ${filepath} NAME) 34 | string(REGEX REPLACE ".cpp" "" test_name ${filename}) 35 | 36 | message("test_name = ${test_name}") 37 | 38 | ament_add_gmock(${test_name} ${filepath}) 39 | target_link_libraries(${test_name} fmt ${PROJECT_NAME}) 40 | ament_target_dependencies(${test_name} ${${PROJECT_NAME}_FOUND_BUILD_DEPENDS}) 41 | endfunction() 42 | 43 | 44 | if(BUILD_TESTING) 45 | find_package(ament_lint_auto REQUIRED) 46 | 47 | list(APPEND AMENT_LINT_AUTO_EXCLUDE 48 | ament_cmake_cpplint 49 | ament_cmake_uncrustify 50 | ) 51 | 52 | ament_lint_auto_find_test_dependencies() 53 | 54 | file(GLOB FILES_TO_CHECK include/lidar_feature_library/* src/*) 55 | 56 | find_package(ament_cmake_cpplint) 57 | ament_cpplint(${FILES_TO_CHECK}) 58 | 59 | find_package(ament_cmake_uncrustify) 60 | ament_uncrustify(${FILES_TO_CHECK}) 61 | 62 | file(GLOB TEST_FILES test/*) 63 | foreach(filepath ${TEST_FILES}) 64 | add_testcase(${filepath}) 65 | endforeach() 66 | endif() 67 | 68 | ament_auto_package() 69 | -------------------------------------------------------------------------------- /lib/include/lidar_feature_library/degree_to_radian.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2022 Tixiao Shan, Takeshi Ishita 2 | // 3 | // Redistribution and use in source and binary forms, with or without 4 | // modification, are permitted provided that the following conditions are met: 5 | // 6 | // * Redistributions of source code must retain the above copyright 7 | // notice, this list of conditions and the following disclaimer. 8 | // 9 | // * Redistributions in binary form must reproduce the above copyright 10 | // notice, this list of conditions and the following disclaimer in the 11 | // documentation and/or other materials provided with the distribution. 12 | // 13 | // * Neither the name of the Tixiao Shan, Takeshi Ishita nor the names of its 14 | // contributors may be used to endorse or promote products derived from 15 | // this software without specific prior written permission. 16 | // 17 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 21 | // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | // POSSIBILITY OF SUCH DAMAGE. 28 | 29 | #ifndef LIDAR_FEATURE_LIBRARY__DEGREE_TO_RADIAN_HPP_ 30 | #define LIDAR_FEATURE_LIBRARY__DEGREE_TO_RADIAN_HPP_ 31 | 32 | #include 33 | 34 | inline double DegreeToRadian(const double degree) 35 | { 36 | return degree * M_PI / 180.0; 37 | } 38 | 39 | #endif // LIDAR_FEATURE_LIBRARY__DEGREE_TO_RADIAN_HPP_ 40 | -------------------------------------------------------------------------------- /lib/include/lidar_feature_library/downsample.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2022 Tixiao Shan, Takeshi Ishita 2 | // 3 | // Redistribution and use in source and binary forms, with or without 4 | // modification, are permitted provided that the following conditions are met: 5 | // 6 | // * Redistributions of source code must retain the above copyright 7 | // notice, this list of conditions and the following disclaimer. 8 | // 9 | // * Redistributions in binary form must reproduce the above copyright 10 | // notice, this list of conditions and the following disclaimer in the 11 | // documentation and/or other materials provided with the distribution. 12 | // 13 | // * Neither the name of the Tixiao Shan, Takeshi Ishita nor the names of its 14 | // contributors may be used to endorse or promote products derived from 15 | // this software without specific prior written permission. 16 | // 17 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 21 | // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | // POSSIBILITY OF SUCH DAMAGE. 28 | 29 | 30 | #ifndef LIDAR_FEATURE_LIBRARY__DOWNSAMPLE_HPP_ 31 | #define LIDAR_FEATURE_LIBRARY__DOWNSAMPLE_HPP_ 32 | 33 | #include 34 | #include 35 | #include 36 | 37 | template 38 | typename pcl::PointCloud::Ptr Downsample( 39 | const typename pcl::PointCloud::Ptr & input_cloud, const float leaf_size) 40 | { 41 | pcl::VoxelGrid filter; 42 | typename pcl::PointCloud::Ptr downsampled(new pcl::PointCloud()); 43 | 44 | filter.setLeafSize(leaf_size, leaf_size, leaf_size); 45 | filter.setInputCloud(input_cloud); 46 | filter.filter(*downsampled); 47 | 48 | return downsampled; 49 | } 50 | 51 | #endif // LIDAR_FEATURE_LIBRARY__DOWNSAMPLE_HPP_ 52 | -------------------------------------------------------------------------------- /lib/include/lidar_feature_library/matrix_types.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2022 Tixiao Shan, Takeshi Ishita 2 | // 3 | // Redistribution and use in source and binary forms, with or without 4 | // modification, are permitted provided that the following conditions are met: 5 | // 6 | // * Redistributions of source code must retain the above copyright 7 | // notice, this list of conditions and the following disclaimer. 8 | // 9 | // * Redistributions in binary form must reproduce the above copyright 10 | // notice, this list of conditions and the following disclaimer in the 11 | // documentation and/or other materials provided with the distribution. 12 | // 13 | // * Neither the name of the Tixiao Shan, Takeshi Ishita nor the names of its 14 | // contributors may be used to endorse or promote products derived from 15 | // this software without specific prior written permission. 16 | // 17 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 21 | // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | // POSSIBILITY OF SUCH DAMAGE. 28 | 29 | 30 | #ifndef LIDAR_FEATURE_LIBRARY__MATRIX_TYPES_HPP_ 31 | #define LIDAR_FEATURE_LIBRARY__MATRIX_TYPES_HPP_ 32 | 33 | using Matrix6d = Eigen::Matrix; 34 | 35 | #endif // LIDAR_FEATURE_LIBRARY__MATRIX_TYPES_HPP_ 36 | -------------------------------------------------------------------------------- /lib/include/lidar_feature_library/numeric.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2018-2019 Autoware Foundation 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #ifndef LIDAR_FEATURE_LIBRARY__NUMERIC_HPP_ 16 | #define LIDAR_FEATURE_LIBRARY__NUMERIC_HPP_ 17 | 18 | #include 19 | 20 | #include 21 | 22 | inline bool HasInf(const Eigen::MatrixXd & v) 23 | { 24 | return v.array().isInf().any(); 25 | } 26 | 27 | inline bool HasNan(const Eigen::MatrixXd & v) 28 | { 29 | return v.array().isNaN().any(); 30 | } 31 | 32 | #endif // LIDAR_FEATURE_LIBRARY__NUMERIC_HPP_ 33 | -------------------------------------------------------------------------------- /lib/include/lidar_feature_library/qos.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2022 Tixiao Shan, Takeshi Ishita 2 | // 3 | // Redistribution and use in source and binary forms, with or without 4 | // modification, are permitted provided that the following conditions are met: 5 | // 6 | // * Redistributions of source code must retain the above copyright 7 | // notice, this list of conditions and the following disclaimer. 8 | // 9 | // * Redistributions in binary form must reproduce the above copyright 10 | // notice, this list of conditions and the following disclaimer in the 11 | // documentation and/or other materials provided with the distribution. 12 | // 13 | // * Neither the name of the Tixiao Shan, Takeshi Ishita nor the names of its 14 | // contributors may be used to endorse or promote products derived from 15 | // this software without specific prior written permission. 16 | // 17 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 21 | // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | // POSSIBILITY OF SUCH DAMAGE. 28 | 29 | #ifndef LIDAR_FEATURE_LIBRARY__QOS_HPP_ 30 | #define LIDAR_FEATURE_LIBRARY__QOS_HPP_ 31 | 32 | #include 33 | 34 | const rclcpp::QoS QOS_RELIABLE_VOLATILE = 35 | rclcpp::SensorDataQoS().reliable().durability_volatile(); 36 | const rclcpp::QoS QOS_RELIABLE_TRANSIENT_LOCAL = 37 | rclcpp::SensorDataQoS().reliable().transient_local(); 38 | 39 | #endif // LIDAR_FEATURE_LIBRARY__QOS_HPP_ 40 | -------------------------------------------------------------------------------- /lib/include/lidar_feature_library/random.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2022 Tixiao Shan, Takeshi Ishita 2 | // 3 | // Redistribution and use in source and binary forms, with or without 4 | // modification, are permitted provided that the following conditions are met: 5 | // 6 | // * Redistributions of source code must retain the above copyright 7 | // notice, this list of conditions and the following disclaimer. 8 | // 9 | // * Redistributions in binary form must reproduce the above copyright 10 | // notice, this list of conditions and the following disclaimer in the 11 | // documentation and/or other materials provided with the distribution. 12 | // 13 | // * Neither the name of the Tixiao Shan, Takeshi Ishita nor the names of its 14 | // contributors may be used to endorse or promote products derived from 15 | // this software without specific prior written permission. 16 | // 17 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 21 | // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | // POSSIBILITY OF SUCH DAMAGE. 28 | 29 | #ifndef LIDAR_FEATURE_LIBRARY__RANDOM_HPP_ 30 | #define LIDAR_FEATURE_LIBRARY__RANDOM_HPP_ 31 | 32 | #include 33 | 34 | #include 35 | #include 36 | 37 | std::vector RandomizedUniqueIndices(const size_t size); 38 | double SampleStandardDeviation(const Eigen::VectorXd & v); 39 | 40 | template 41 | class NormalDistribution 42 | { 43 | public: 44 | NormalDistribution(const double mean, const double stddev) 45 | : distribution_(mean, stddev) 46 | { 47 | } 48 | 49 | RealType operator()() 50 | { 51 | return distribution_(generator_); 52 | } 53 | 54 | private: 55 | std::default_random_engine generator_; 56 | std::normal_distribution distribution_; 57 | }; 58 | 59 | #endif // LIDAR_FEATURE_LIBRARY__RANDOM_HPP_ 60 | -------------------------------------------------------------------------------- /lib/include/lidar_feature_library/stats.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2022 Tixiao Shan, Takeshi Ishita 2 | // 3 | // Redistribution and use in source and binary forms, with or without 4 | // modification, are permitted provided that the following conditions are met: 5 | // 6 | // * Redistributions of source code must retain the above copyright 7 | // notice, this list of conditions and the following disclaimer. 8 | // 9 | // * Redistributions in binary form must reproduce the above copyright 10 | // notice, this list of conditions and the following disclaimer in the 11 | // documentation and/or other materials provided with the distribution. 12 | // 13 | // * Neither the name of the Tixiao Shan, Takeshi Ishita nor the names of its 14 | // contributors may be used to endorse or promote products derived from 15 | // this software without specific prior written permission. 16 | // 17 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 21 | // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | // POSSIBILITY OF SUCH DAMAGE. 28 | 29 | #ifndef LIDAR_FEATURE_LIBRARY__STATS_HPP_ 30 | #define LIDAR_FEATURE_LIBRARY__STATS_HPP_ 31 | 32 | #include 33 | 34 | #include 35 | #include 36 | 37 | double Median(const Eigen::VectorXd & m); 38 | double Median(const Eigen::ArrayXd & m); 39 | 40 | #endif // LIDAR_FEATURE_LIBRARY__STATS_HPP_ 41 | -------------------------------------------------------------------------------- /lib/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | lidar_feature_library 5 | 0.0.0 6 | Common data types and functions for feature-based localization 7 | Takeshi Ishita 8 | BSD 3-Clause 9 | 10 | ament_cmake 11 | 12 | fmt 13 | pcl_conversions 14 | pcl_ros 15 | rclcpp_components 16 | sensor_msgs 17 | tf2_eigen 18 | visualization_msgs 19 | 20 | ament_cmake_gmock 21 | ament_lint_auto 22 | ament_lint_common 23 | 24 | 25 | ament_cmake 26 | 27 | 28 | -------------------------------------------------------------------------------- /lib/src/algorithm.cpp: -------------------------------------------------------------------------------- 1 | // Copyright 2022 Tixiao Shan, Takeshi Ishita 2 | // 3 | // Redistribution and use in source and binary forms, with or without 4 | // modification, are permitted provided that the following conditions are met: 5 | // 6 | // * Redistributions of source code must retain the above copyright 7 | // notice, this list of conditions and the following disclaimer. 8 | // 9 | // * Redistributions in binary form must reproduce the above copyright 10 | // notice, this list of conditions and the following disclaimer in the 11 | // documentation and/or other materials provided with the distribution. 12 | // 13 | // * Neither the name of the Tixiao Shan, Takeshi Ishita nor the names of its 14 | // contributors may be used to endorse or promote products derived from 15 | // this software without specific prior written permission. 16 | // 17 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 21 | // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | // POSSIBILITY OF SUCH DAMAGE. 28 | 29 | #include 30 | 31 | #include "lidar_feature_library/algorithm.hpp" 32 | 33 | Eigen::Vector3d SortThreeValues(const Eigen::Vector3d & v) 34 | { 35 | double a = v(0); 36 | double b = v(1); 37 | double c = v(2); 38 | 39 | if (a > b) { 40 | std::swap(a, b); 41 | } 42 | if (b > c) { 43 | std::swap(b, c); 44 | } 45 | if (a > b) { 46 | std::swap(a, b); 47 | } 48 | return Eigen::Vector3d(a, b, c); 49 | } 50 | -------------------------------------------------------------------------------- /lib/src/lib.cpp: -------------------------------------------------------------------------------- 1 | // Copyright 2022 Tixiao Shan, Takeshi Ishita 2 | // 3 | // Redistribution and use in source and binary forms, with or without 4 | // modification, are permitted provided that the following conditions are met: 5 | // 6 | // * Redistributions of source code must retain the above copyright 7 | // notice, this list of conditions and the following disclaimer. 8 | // 9 | // * Redistributions in binary form must reproduce the above copyright 10 | // notice, this list of conditions and the following disclaimer in the 11 | // documentation and/or other materials provided with the distribution. 12 | // 13 | // * Neither the name of the Tixiao Shan, Takeshi Ishita nor the names of its 14 | // contributors may be used to endorse or promote products derived from 15 | // this software without specific prior written permission. 16 | // 17 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 21 | // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | // POSSIBILITY OF SUCH DAMAGE. 28 | 29 | #include "lidar_feature_library/convert_point_cloud_type.hpp" 30 | #include "lidar_feature_library/downsample.hpp" 31 | #include "lidar_feature_library/ros_msg.hpp" 32 | -------------------------------------------------------------------------------- /lib/src/pcl_utils.cpp: -------------------------------------------------------------------------------- 1 | // Copyright 2022 Tixiao Shan, Takeshi Ishita 2 | // 3 | // Redistribution and use in source and binary forms, with or without 4 | // modification, are permitted provided that the following conditions are met: 5 | // 6 | // * Redistributions of source code must retain the above copyright 7 | // notice, this list of conditions and the following disclaimer. 8 | // 9 | // * Redistributions in binary form must reproduce the above copyright 10 | // notice, this list of conditions and the following disclaimer in the 11 | // documentation and/or other materials provided with the distribution. 12 | // 13 | // * Neither the name of the Tixiao Shan, Takeshi Ishita nor the names of its 14 | // contributors may be used to endorse or promote products derived from 15 | // this software without specific prior written permission. 16 | // 17 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 21 | // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | // POSSIBILITY OF SUCH DAMAGE. 28 | 29 | #include "lidar_feature_library/pcl_utils.hpp" 30 | 31 | 32 | pcl::PointXYZ MakePointXYZ(const Eigen::Vector3d & v) 33 | { 34 | return pcl::PointXYZ(v(0), v(1), v(2)); 35 | } 36 | 37 | Eigen::MatrixXd Get( 38 | const pcl::PointCloud::Ptr & pointcloud, 39 | const std::vector & indices) 40 | { 41 | Eigen::MatrixXd A(indices.size(), 3); 42 | for (const auto & [j, index] : ranges::views::enumerate(indices)) { 43 | A.row(j) = GetXYZ(pointcloud->at(index)).transpose(); 44 | } 45 | return A; 46 | } 47 | -------------------------------------------------------------------------------- /lib/src/random.cpp: -------------------------------------------------------------------------------- 1 | // Copyright 2022 Tixiao Shan, Takeshi Ishita 2 | // 3 | // Redistribution and use in source and binary forms, with or without 4 | // modification, are permitted provided that the following conditions are met: 5 | // 6 | // * Redistributions of source code must retain the above copyright 7 | // notice, this list of conditions and the following disclaimer. 8 | // 9 | // * Redistributions in binary form must reproduce the above copyright 10 | // notice, this list of conditions and the following disclaimer in the 11 | // documentation and/or other materials provided with the distribution. 12 | // 13 | // * Neither the name of the Tixiao Shan, Takeshi Ishita nor the names of its 14 | // contributors may be used to endorse or promote products derived from 15 | // this software without specific prior written permission. 16 | // 17 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 21 | // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | // POSSIBILITY OF SUCH DAMAGE. 28 | 29 | #include 30 | #include 31 | #include 32 | 33 | #include "lidar_feature_library/random.hpp" 34 | 35 | 36 | std::vector RandomizedUniqueIndices(const size_t size) 37 | { 38 | std::vector v(size); 39 | std::iota(v.begin(), v.end(), 0); 40 | std::shuffle(v.begin(), v.end(), std::mt19937{std::random_device{}()}); 41 | return v; 42 | } 43 | 44 | double SampleStandardDeviation(const Eigen::VectorXd & v) 45 | { 46 | const double n = static_cast(v.size()); 47 | return std::sqrt((v.array() - v.mean()).square().sum() / (n - 1.)); 48 | } 49 | -------------------------------------------------------------------------------- /lib/test/test_degree_to_radian.cpp: -------------------------------------------------------------------------------- 1 | // Copyright 2022 Tixiao Shan, Takeshi Ishita 2 | // 3 | // Redistribution and use in source and binary forms, with or without 4 | // modification, are permitted provided that the following conditions are met: 5 | // 6 | // * Redistributions of source code must retain the above copyright 7 | // notice, this list of conditions and the following disclaimer. 8 | // 9 | // * Redistributions in binary form must reproduce the above copyright 10 | // notice, this list of conditions and the following disclaimer in the 11 | // documentation and/or other materials provided with the distribution. 12 | // 13 | // * Neither the name of the Tixiao Shan, Takeshi Ishita nor the names of its 14 | // contributors may be used to endorse or promote products derived from 15 | // this software without specific prior written permission. 16 | // 17 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 21 | // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | // POSSIBILITY OF SUCH DAMAGE. 28 | 29 | #include 30 | 31 | #include "lidar_feature_library/degree_to_radian.hpp" 32 | 33 | 34 | TEST(DegreeToRadian, DegreeToRadian) 35 | { 36 | EXPECT_EQ(DegreeToRadian(-90.), -M_PI / 2.); 37 | EXPECT_EQ(DegreeToRadian(0.), 0.); 38 | EXPECT_EQ(DegreeToRadian(180.), M_PI); 39 | EXPECT_EQ(DegreeToRadian(360.), 2. * M_PI); 40 | } 41 | -------------------------------------------------------------------------------- /lib/test/test_numeric.cpp: -------------------------------------------------------------------------------- 1 | // Copyright 2022 Tixiao Shan, Takeshi Ishita 2 | // 3 | // Redistribution and use in source and binary forms, with or without 4 | // modification, are permitted provided that the following conditions are met: 5 | // 6 | // * Redistributions of source code must retain the above copyright 7 | // notice, this list of conditions and the following disclaimer. 8 | // 9 | // * Redistributions in binary form must reproduce the above copyright 10 | // notice, this list of conditions and the following disclaimer in the 11 | // documentation and/or other materials provided with the distribution. 12 | // 13 | // * Neither the name of the Tixiao Shan, Takeshi Ishita nor the names of its 14 | // contributors may be used to endorse or promote products derived from 15 | // this software without specific prior written permission. 16 | // 17 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 21 | // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | // POSSIBILITY OF SUCH DAMAGE. 28 | 29 | #include 30 | 31 | #include "lidar_feature_library/numeric.hpp" 32 | 33 | TEST(Numeric, HasNan) 34 | { 35 | const double nan = std::nan(""); 36 | EXPECT_TRUE(HasNan(Eigen::Vector3d(0, nan, nan))); 37 | EXPECT_TRUE(HasNan(Eigen::Vector3d(nan, 0, 0))); 38 | EXPECT_FALSE(HasNan(Eigen::Vector3d(0, 0, 0))); 39 | } 40 | 41 | TEST(Numeric, HasInf) 42 | { 43 | const double inf = std::numeric_limits::infinity(); 44 | EXPECT_TRUE(HasInf(Eigen::Vector3d(0, inf, inf))); 45 | EXPECT_TRUE(HasInf(Eigen::Vector3d(inf, 0, 0))); 46 | EXPECT_FALSE(HasInf(Eigen::Vector3d(0, 0, 0))); 47 | } 48 | -------------------------------------------------------------------------------- /lidar_feature_launch/config/lidar_feature_extraction.param.yaml: -------------------------------------------------------------------------------- 1 | /**: 2 | ros__parameters: 3 | convolution_padding: 2 4 | neighbor_degree_threshold: 3.0 5 | distance_diff_threshold: 0.3 6 | parallel_beam_min_range_ratio: 0.02 7 | edge_threshold: 50.0 8 | min_range: 0.1 9 | max_range: 1000.0 10 | n_blocks: 6 11 | -------------------------------------------------------------------------------- /lidar_feature_launch/lidar_feature_launch/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/tier4/lidar_feature_extraction/67c5eee072417ef29017e66993d64a978e7a0f96/lidar_feature_launch/lidar_feature_launch/__init__.py -------------------------------------------------------------------------------- /lidar_feature_launch/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | lidar_feature_launch 5 | 0.0.0 6 | Lidar-feature localization and mapping launcher 7 | Takeshi Ishita 8 | BSD 3-Clause 9 | 10 | ros2launch 11 | 12 | ament_copyright 13 | ament_flake8 14 | ament_pep257 15 | python3-pytest 16 | 17 | 18 | ament_python 19 | 20 | 21 | -------------------------------------------------------------------------------- /lidar_feature_launch/resource/lidar_feature_launch: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/tier4/lidar_feature_extraction/67c5eee072417ef29017e66993d64a978e7a0f96/lidar_feature_launch/resource/lidar_feature_launch -------------------------------------------------------------------------------- /lidar_feature_launch/setup.cfg: -------------------------------------------------------------------------------- 1 | [develop] 2 | script_dir=$base/lib/lidar_feature_launch 3 | [install] 4 | install_scripts=$base/lib/lidar_feature_launch 5 | -------------------------------------------------------------------------------- /lidar_feature_launch/setup.py: -------------------------------------------------------------------------------- 1 | from pathlib import Path 2 | 3 | from setuptools import setup 4 | 5 | package_name = 'lidar_feature_launch' 6 | 7 | 8 | def get_launch_paths(): 9 | dirpath = Path('share', package_name, 'launch') 10 | names = Path('launch').glob('*.launch.py') 11 | return (str(dirpath), list(map(str, names))) 12 | 13 | 14 | def get_config_paths(): 15 | dirpath = Path('share', package_name, 'config') 16 | names = Path('config').glob('*.param.yaml') 17 | return (str(dirpath), list(map(str, names))) 18 | 19 | 20 | setup( 21 | name=package_name, 22 | version='0.0.0', 23 | packages=[package_name], 24 | data_files=[ 25 | ('share/ament_index/resource_index/packages', 26 | ['resource/' + package_name]), 27 | ('share/' + package_name, ['package.xml']), 28 | get_launch_paths(), 29 | get_config_paths(), 30 | ], 31 | install_requires=['setuptools'], 32 | zip_safe=True, 33 | maintainer='Takeshi Ishita', 34 | maintainer_email='ishitah.takeshi@gmail.com', 35 | description='Launcher', 36 | license='BSD 3-Clause', 37 | tests_require=['pytest'], 38 | entry_points={ 39 | 'console_scripts': [ 40 | ], 41 | }, 42 | ) 43 | -------------------------------------------------------------------------------- /lidar_feature_launch/test/test_copyright.py: -------------------------------------------------------------------------------- 1 | # Copyright 2015 Open Source Robotics Foundation, Inc. 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | from ament_copyright.main import main 16 | import pytest 17 | 18 | 19 | @pytest.mark.copyright 20 | @pytest.mark.linter 21 | def test_copyright(): 22 | rc = main(argv=['.', 'test']) 23 | assert rc == 0, 'Found errors' 24 | -------------------------------------------------------------------------------- /lidar_feature_launch/test/test_flake8.py: -------------------------------------------------------------------------------- 1 | # Copyright 2017 Open Source Robotics Foundation, Inc. 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | from ament_flake8.main import main_with_errors 16 | import pytest 17 | 18 | 19 | @pytest.mark.flake8 20 | @pytest.mark.linter 21 | def test_flake8(): 22 | rc, errors = main_with_errors(argv=[]) 23 | assert rc == 0, \ 24 | 'Found %d code style errors / warnings:\n' % len(errors) + \ 25 | '\n'.join(errors) 26 | -------------------------------------------------------------------------------- /lidar_feature_launch/test/test_pep257.py: -------------------------------------------------------------------------------- 1 | # Copyright 2015 Open Source Robotics Foundation, Inc. 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | from ament_pep257.main import main 16 | import pytest 17 | 18 | 19 | @pytest.mark.linter 20 | @pytest.mark.pep257 21 | def test_pep257(): 22 | rc = main(argv=['.', 'test']) 23 | assert rc == 0, 'Found code style errors / warnings' 24 | -------------------------------------------------------------------------------- /localization/include/lidar_feature_localization/degenerate.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2022 Tixiao Shan, Takeshi Ishita 2 | // 3 | // Redistribution and use in source and binary forms, with or without 4 | // modification, are permitted provided that the following conditions are met: 5 | // 6 | // * Redistributions of source code must retain the above copyright 7 | // notice, this list of conditions and the following disclaimer. 8 | // 9 | // * Redistributions in binary form must reproduce the above copyright 10 | // notice, this list of conditions and the following disclaimer in the 11 | // documentation and/or other materials provided with the distribution. 12 | // 13 | // * Neither the name of the Tixiao Shan, Takeshi Ishita nor the names of its 14 | // contributors may be used to endorse or promote products derived from 15 | // this software without specific prior written permission. 16 | // 17 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 21 | // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | // POSSIBILITY OF SUCH DAMAGE. 28 | 29 | #ifndef LIDAR_FEATURE_LOCALIZATION__DEGENERATE_HPP_ 30 | #define LIDAR_FEATURE_LOCALIZATION__DEGENERATE_HPP_ 31 | 32 | #include 33 | 34 | bool IsDegenerate(const Eigen::MatrixXd & C, const double threshold = 0.1); 35 | 36 | #endif // LIDAR_FEATURE_LOCALIZATION__DEGENERATE_HPP_ 37 | -------------------------------------------------------------------------------- /localization/include/lidar_feature_localization/edge_surface_scan.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2022 Tixiao Shan, Takeshi Ishita 2 | // 3 | // Redistribution and use in source and binary forms, with or without 4 | // modification, are permitted provided that the following conditions are met: 5 | // 6 | // * Redistributions of source code must retain the above copyright 7 | // notice, this list of conditions and the following disclaimer. 8 | // 9 | // * Redistributions in binary form must reproduce the above copyright 10 | // notice, this list of conditions and the following disclaimer in the 11 | // documentation and/or other materials provided with the distribution. 12 | // 13 | // * Neither the name of the Tixiao Shan, Takeshi Ishita nor the names of its 14 | // contributors may be used to endorse or promote products derived from 15 | // this software without specific prior written permission. 16 | // 17 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 21 | // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | // POSSIBILITY OF SUCH DAMAGE. 28 | 29 | #ifndef LIDAR_FEATURE_LOCALIZATION__EDGE_SURFACE_SCAN_HPP_ 30 | #define LIDAR_FEATURE_LOCALIZATION__EDGE_SURFACE_SCAN_HPP_ 31 | 32 | #include 33 | #include 34 | 35 | #include 36 | 37 | using EdgeSurfaceScan = std::tuple< 38 | pcl::PointCloud::Ptr, 39 | pcl::PointCloud::Ptr>; 40 | 41 | #endif // LIDAR_FEATURE_LOCALIZATION__EDGE_SURFACE_SCAN_HPP_ 42 | -------------------------------------------------------------------------------- /localization/include/lidar_feature_localization/filter.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2022 Tixiao Shan, Takeshi Ishita 2 | // 3 | // Redistribution and use in source and binary forms, with or without 4 | // modification, are permitted provided that the following conditions are met: 5 | // 6 | // * Redistributions of source code must retain the above copyright 7 | // notice, this list of conditions and the following disclaimer. 8 | // 9 | // * Redistributions in binary form must reproduce the above copyright 10 | // notice, this list of conditions and the following disclaimer in the 11 | // documentation and/or other materials provided with the distribution. 12 | // 13 | // * Neither the name of the Tixiao Shan, Takeshi Ishita nor the names of its 14 | // contributors may be used to endorse or promote products derived from 15 | // this software without specific prior written permission. 16 | // 17 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 21 | // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | // POSSIBILITY OF SUCH DAMAGE. 28 | 29 | #ifndef LIDAR_FEATURE_LOCALIZATION__FILTER_HPP_ 30 | #define LIDAR_FEATURE_LOCALIZATION__FILTER_HPP_ 31 | 32 | #include 33 | 34 | template 35 | auto Filter( 36 | const std::vector & flags, 37 | const Iter & values) 38 | { 39 | typedef typename Iter::value_type T; 40 | 41 | assert(flags.size() == values.size()); 42 | std::vector filtered; 43 | for (unsigned int i = 0; i < flags.size(); i++) { 44 | if (flags[i]) { 45 | filtered.push_back(values[i]); 46 | } 47 | } 48 | return filtered; 49 | } 50 | 51 | #endif // LIDAR_FEATURE_LOCALIZATION__FILTER_HPP_ 52 | -------------------------------------------------------------------------------- /localization/include/lidar_feature_localization/jacobian.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2022 Tixiao Shan, Takeshi Ishita 2 | // 3 | // Redistribution and use in source and binary forms, with or without 4 | // modification, are permitted provided that the following conditions are met: 5 | // 6 | // * Redistributions of source code must retain the above copyright 7 | // notice, this list of conditions and the following disclaimer. 8 | // 9 | // * Redistributions in binary form must reproduce the above copyright 10 | // notice, this list of conditions and the following disclaimer in the 11 | // documentation and/or other materials provided with the distribution. 12 | // 13 | // * Neither the name of the Tixiao Shan, Takeshi Ishita nor the names of its 14 | // contributors may be used to endorse or promote products derived from 15 | // this software without specific prior written permission. 16 | // 17 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 21 | // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | // POSSIBILITY OF SUCH DAMAGE. 28 | 29 | #ifndef LIDAR_FEATURE_LOCALIZATION__JACOBIAN_HPP_ 30 | #define LIDAR_FEATURE_LOCALIZATION__JACOBIAN_HPP_ 31 | 32 | #include 33 | 34 | #include 35 | 36 | #include "rotationlib/jacobian/quaternion.hpp" 37 | 38 | 39 | void FillJacobianRow( 40 | Eigen::MatrixXd & J, 41 | const int i, 42 | const Eigen::Matrix & drpdq, 43 | const Eigen::Vector3d & coeff); 44 | 45 | Eigen::MatrixXd MakeJacobian( 46 | const std::vector & points, 47 | const std::vector & coeffs, 48 | const Eigen::Quaterniond & q); 49 | 50 | #endif // LIDAR_FEATURE_LOCALIZATION__JACOBIAN_HPP_ 51 | -------------------------------------------------------------------------------- /localization/include/lidar_feature_localization/math.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2022 Tixiao Shan, Takeshi Ishita 2 | // 3 | // Redistribution and use in source and binary forms, with or without 4 | // modification, are permitted provided that the following conditions are met: 5 | // 6 | // * Redistributions of source code must retain the above copyright 7 | // notice, this list of conditions and the following disclaimer. 8 | // 9 | // * Redistributions in binary form must reproduce the above copyright 10 | // notice, this list of conditions and the following disclaimer in the 11 | // documentation and/or other materials provided with the distribution. 12 | // 13 | // * Neither the name of the Tixiao Shan, Takeshi Ishita nor the names of its 14 | // contributors may be used to endorse or promote products derived from 15 | // this software without specific prior written permission. 16 | // 17 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 21 | // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | // POSSIBILITY OF SUCH DAMAGE. 28 | 29 | #ifndef LIDAR_FEATURE_LOCALIZATION__MATH_HPP_ 30 | #define LIDAR_FEATURE_LOCALIZATION__MATH_HPP_ 31 | 32 | #include 33 | #include 34 | 35 | 36 | inline Eigen::VectorXd SolveLinear(const Eigen::MatrixXd & A, const Eigen::VectorXd & b) 37 | { 38 | assert(A.rows() == b.size()); 39 | return A.householderQr().solve(b); 40 | } 41 | 42 | #endif // LIDAR_FEATURE_LOCALIZATION__MATH_HPP_ 43 | -------------------------------------------------------------------------------- /localization/include/lidar_feature_localization/matrix_type.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2022 Tixiao Shan, Takeshi Ishita 2 | // 3 | // Redistribution and use in source and binary forms, with or without 4 | // modification, are permitted provided that the following conditions are met: 5 | // 6 | // * Redistributions of source code must retain the above copyright 7 | // notice, this list of conditions and the following disclaimer. 8 | // 9 | // * Redistributions in binary form must reproduce the above copyright 10 | // notice, this list of conditions and the following disclaimer in the 11 | // documentation and/or other materials provided with the distribution. 12 | // 13 | // * Neither the name of the Tixiao Shan, Takeshi Ishita nor the names of its 14 | // contributors may be used to endorse or promote products derived from 15 | // this software without specific prior written permission. 16 | // 17 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 21 | // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | // POSSIBILITY OF SUCH DAMAGE. 28 | 29 | #ifndef LIDAR_FEATURE_LOCALIZATION__MATRIX_TYPE_HPP_ 30 | #define LIDAR_FEATURE_LOCALIZATION__MATRIX_TYPE_HPP_ 31 | 32 | using Vector7d = Eigen::Matrix; 33 | using Vector6d = Eigen::Matrix; 34 | 35 | #endif // LIDAR_FEATURE_LOCALIZATION__MATRIX_TYPE_HPP_ 36 | -------------------------------------------------------------------------------- /localization/include/lidar_feature_localization/pointcloud_to_matrix.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2022 Tixiao Shan, Takeshi Ishita 2 | // 3 | // Redistribution and use in source and binary forms, with or without 4 | // modification, are permitted provided that the following conditions are met: 5 | // 6 | // * Redistributions of source code must retain the above copyright 7 | // notice, this list of conditions and the following disclaimer. 8 | // 9 | // * Redistributions in binary form must reproduce the above copyright 10 | // notice, this list of conditions and the following disclaimer in the 11 | // documentation and/or other materials provided with the distribution. 12 | // 13 | // * Neither the name of the Tixiao Shan, Takeshi Ishita nor the names of its 14 | // contributors may be used to endorse or promote products derived from 15 | // this software without specific prior written permission. 16 | // 17 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 21 | // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | // POSSIBILITY OF SUCH DAMAGE. 28 | 29 | #ifndef LIDAR_FEATURE_LOCALIZATION__POINTCLOUD_TO_MATRIX_HPP_ 30 | #define LIDAR_FEATURE_LOCALIZATION__POINTCLOUD_TO_MATRIX_HPP_ 31 | 32 | #include 33 | #include 34 | #include 35 | 36 | #include "lidar_feature_localization/point_to_vector.hpp" 37 | 38 | template 39 | Eigen::MatrixXd PointCloudToMatrix( 40 | const typename pcl::PointCloud::Ptr & map) 41 | { 42 | Eigen::MatrixXd points(map->size(), PointToVector::NumDimension()); 43 | for (size_t i = 0; i < map->size(); i++) { 44 | points.row(i) = PointToVector::Convert(map->at(i)); 45 | } 46 | return points; 47 | } 48 | 49 | #endif // LIDAR_FEATURE_LOCALIZATION__POINTCLOUD_TO_MATRIX_HPP_ 50 | -------------------------------------------------------------------------------- /localization/include/lidar_feature_localization/posevec.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2022 Tixiao Shan, Takeshi Ishita 2 | // 3 | // Redistribution and use in source and binary forms, with or without 4 | // modification, are permitted provided that the following conditions are met: 5 | // 6 | // * Redistributions of source code must retain the above copyright 7 | // notice, this list of conditions and the following disclaimer. 8 | // 9 | // * Redistributions in binary form must reproduce the above copyright 10 | // notice, this list of conditions and the following disclaimer in the 11 | // documentation and/or other materials provided with the distribution. 12 | // 13 | // * Neither the name of the Tixiao Shan, Takeshi Ishita nor the names of its 14 | // contributors may be used to endorse or promote products derived from 15 | // this software without specific prior written permission. 16 | // 17 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 21 | // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | // POSSIBILITY OF SUCH DAMAGE. 28 | 29 | #ifndef LIDAR_FEATURE_LOCALIZATION__POSEVEC_HPP_ 30 | #define LIDAR_FEATURE_LOCALIZATION__POSEVEC_HPP_ 31 | 32 | #include 33 | #include 34 | 35 | #include "lidar_feature_localization/matrix_type.hpp" 36 | 37 | Eigen::Quaterniond AngleAxisToQuaternion(const Eigen::Vector3d & theta); 38 | 39 | Eigen::Isometry3d MakePose( 40 | const Eigen::Quaterniond & q, 41 | const Eigen::Vector3d & t); 42 | 43 | #endif // LIDAR_FEATURE_LOCALIZATION__POSEVEC_HPP_ 44 | -------------------------------------------------------------------------------- /localization/include/lidar_feature_localization/rad2deg.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2022 Tixiao Shan, Takeshi Ishita 2 | // 3 | // Redistribution and use in source and binary forms, with or without 4 | // modification, are permitted provided that the following conditions are met: 5 | // 6 | // * Redistributions of source code must retain the above copyright 7 | // notice, this list of conditions and the following disclaimer. 8 | // 9 | // * Redistributions in binary form must reproduce the above copyright 10 | // notice, this list of conditions and the following disclaimer in the 11 | // documentation and/or other materials provided with the distribution. 12 | // 13 | // * Neither the name of the Tixiao Shan, Takeshi Ishita nor the names of its 14 | // contributors may be used to endorse or promote products derived from 15 | // this software without specific prior written permission. 16 | // 17 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 21 | // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | // POSSIBILITY OF SUCH DAMAGE. 28 | 29 | #include "lidar_feature_localization/math.hpp" 30 | 31 | #ifndef LIDAR_FEATURE_LOCALIZATION__RAD2DEG_HPP_ 32 | #define LIDAR_FEATURE_LOCALIZATION__RAD2DEG_HPP_ 33 | 34 | inline Eigen::MatrixXd Rad2Deg(const Eigen::MatrixXd & x) 35 | { 36 | return x * (180.0 / M_PI); 37 | } 38 | 39 | #endif // LIDAR_FEATURE_LOCALIZATION__RAD2DEG_HPP_ 40 | -------------------------------------------------------------------------------- /localization/include/lidar_feature_localization/robust.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2022 Tixiao Shan, Takeshi Ishita 2 | // 3 | // Redistribution and use in source and binary forms, with or without 4 | // modification, are permitted provided that the following conditions are met: 5 | // 6 | // * Redistributions of source code must retain the above copyright 7 | // notice, this list of conditions and the following disclaimer. 8 | // 9 | // * Redistributions in binary form must reproduce the above copyright 10 | // notice, this list of conditions and the following disclaimer in the 11 | // documentation and/or other materials provided with the distribution. 12 | // 13 | // * Neither the name of the Tixiao Shan, Takeshi Ishita nor the names of its 14 | // contributors may be used to endorse or promote products derived from 15 | // this software without specific prior written permission. 16 | // 17 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 21 | // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | // POSSIBILITY OF SUCH DAMAGE. 28 | 29 | #ifndef LIDAR_FEATURE_LOCALIZATION__ROBUST_HPP_ 30 | #define LIDAR_FEATURE_LOCALIZATION__ROBUST_HPP_ 31 | 32 | #include 33 | 34 | double MedianAbsoluteDeviation(const Eigen::VectorXd & v); 35 | double Scale(const Eigen::VectorXd & v); 36 | double Huber(const double e, const double k = 1.345); 37 | double HuberDerivative(const double e, const double k = 1.345); 38 | 39 | #endif // LIDAR_FEATURE_LOCALIZATION__ROBUST_HPP_ 40 | -------------------------------------------------------------------------------- /localization/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | lidar_feature_localization 5 | 0.1.0 6 | Lidar feature based localization and odometry 7 | Takeshi Ishita 8 | BSD 3-Clause 9 | 10 | ament_cmake 11 | 12 | eigen 13 | lidar_feature_library 14 | nav_msgs 15 | pcl_ros 16 | rcpputils 17 | rotationlib 18 | visualization_msgs 19 | 20 | ament_cmake_gmock 21 | ament_lint_auto 22 | ament_lint_common 23 | 24 | 25 | ament_cmake 26 | 27 | 28 | -------------------------------------------------------------------------------- /localization/src/degenerate.cpp: -------------------------------------------------------------------------------- 1 | // Copyright 2022 Tixiao Shan, Takeshi Ishita 2 | // 3 | // Redistribution and use in source and binary forms, with or without 4 | // modification, are permitted provided that the following conditions are met: 5 | // 6 | // * Redistributions of source code must retain the above copyright 7 | // notice, this list of conditions and the following disclaimer. 8 | // 9 | // * Redistributions in binary form must reproduce the above copyright 10 | // notice, this list of conditions and the following disclaimer in the 11 | // documentation and/or other materials provided with the distribution. 12 | // 13 | // * Neither the name of the Tixiao Shan, Takeshi Ishita nor the names of its 14 | // contributors may be used to endorse or promote products derived from 15 | // this software without specific prior written permission. 16 | // 17 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 21 | // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | // POSSIBILITY OF SUCH DAMAGE. 28 | 29 | #include "lidar_feature_localization/degenerate.hpp" 30 | 31 | 32 | bool IsDegenerate(const Eigen::MatrixXd & C, const double threshold) 33 | { 34 | const Eigen::SelfAdjointEigenSolver es(C); 35 | const Eigen::VectorXd eigenvalues = es.eigenvalues(); 36 | return (eigenvalues.array().abs() < threshold).any(); 37 | } 38 | -------------------------------------------------------------------------------- /localization/src/jacobian.cpp: -------------------------------------------------------------------------------- 1 | // Copyright 2022 Tixiao Shan, Takeshi Ishita 2 | // 3 | // Redistribution and use in source and binary forms, with or without 4 | // modification, are permitted provided that the following conditions are met: 5 | // 6 | // * Redistributions of source code must retain the above copyright 7 | // notice, this list of conditions and the following disclaimer. 8 | // 9 | // * Redistributions in binary form must reproduce the above copyright 10 | // notice, this list of conditions and the following disclaimer in the 11 | // documentation and/or other materials provided with the distribution. 12 | // 13 | // * Neither the name of the Tixiao Shan, Takeshi Ishita nor the names of its 14 | // contributors may be used to endorse or promote products derived from 15 | // this software without specific prior written permission. 16 | // 17 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 21 | // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | // POSSIBILITY OF SUCH DAMAGE. 28 | 29 | 30 | #include 31 | 32 | #include "lidar_feature_localization/jacobian.hpp" 33 | 34 | 35 | void FillJacobianRow( 36 | Eigen::MatrixXd & J, 37 | const int i, 38 | const Eigen::Matrix & drpdq, 39 | const Eigen::Vector3d & coeff) 40 | { 41 | J.block<1, 4>(i, 0) = coeff.transpose() * drpdq; 42 | J.block<1, 3>(i, 4) = coeff; 43 | } 44 | 45 | Eigen::MatrixXd MakeJacobian( 46 | const std::vector & points, 47 | const std::vector & coeffs, 48 | const Eigen::Quaterniond & q) 49 | { 50 | assert(points.size() == coeffs.size()); 51 | Eigen::MatrixXd J(points.size(), 7); 52 | for (unsigned int i = 0; i < points.size(); i++) { 53 | const Eigen::Matrix drpdq = rotationlib::DRpDq(q, points.at(i)); 54 | FillJacobianRow(J, i, drpdq, coeffs.at(i)); 55 | } 56 | return J; 57 | } 58 | -------------------------------------------------------------------------------- /localization/src/posevec.cpp: -------------------------------------------------------------------------------- 1 | // Copyright 2022 Tixiao Shan, Takeshi Ishita 2 | // 3 | // Redistribution and use in source and binary forms, with or without 4 | // modification, are permitted provided that the following conditions are met: 5 | // 6 | // * Redistributions of source code must retain the above copyright 7 | // notice, this list of conditions and the following disclaimer. 8 | // 9 | // * Redistributions in binary form must reproduce the above copyright 10 | // notice, this list of conditions and the following disclaimer in the 11 | // documentation and/or other materials provided with the distribution. 12 | // 13 | // * Neither the name of the Tixiao Shan, Takeshi Ishita nor the names of its 14 | // contributors may be used to endorse or promote products derived from 15 | // this software without specific prior written permission. 16 | // 17 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 21 | // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | // POSSIBILITY OF SUCH DAMAGE. 28 | 29 | #include "lidar_feature_localization/posevec.hpp" 30 | 31 | 32 | Eigen::Quaterniond AngleAxisToQuaternion(const Eigen::Vector3d & theta) 33 | { 34 | const double k = theta.norm(); 35 | if (k < 1e-8) { 36 | return Eigen::Quaterniond::Identity(); 37 | } 38 | 39 | const Eigen::Vector3d u = theta / k; 40 | 41 | const double w = std::cos(k / 2.); 42 | const Eigen::Vector3d xyz = u * std::sin(k / 2.); 43 | return Eigen::Quaterniond(w, xyz(0), xyz(1), xyz(2)); 44 | } 45 | 46 | Eigen::Isometry3d MakePose( 47 | const Eigen::Quaterniond & q, 48 | const Eigen::Vector3d & t) 49 | { 50 | Eigen::Isometry3d pose; 51 | pose.linear() = q.toRotationMatrix(); 52 | pose.translation() = t; 53 | return pose; 54 | } 55 | -------------------------------------------------------------------------------- /localization/test/test_degenerate.cpp: -------------------------------------------------------------------------------- 1 | // Copyright 2022 Tixiao Shan, Takeshi Ishita 2 | // 3 | // Redistribution and use in source and binary forms, with or without 4 | // modification, are permitted provided that the following conditions are met: 5 | // 6 | // * Redistributions of source code must retain the above copyright 7 | // notice, this list of conditions and the following disclaimer. 8 | // 9 | // * Redistributions in binary form must reproduce the above copyright 10 | // notice, this list of conditions and the following disclaimer in the 11 | // documentation and/or other materials provided with the distribution. 12 | // 13 | // * Neither the name of the Tixiao Shan, Takeshi Ishita nor the names of its 14 | // contributors may be used to endorse or promote products derived from 15 | // this software without specific prior written permission. 16 | // 17 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 21 | // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | // POSSIBILITY OF SUCH DAMAGE. 28 | 29 | #include 30 | #include "lidar_feature_localization/degenerate.hpp" 31 | 32 | TEST(OptimizationProblem, IsDegenerate) { 33 | Eigen::Matrix3d C; 34 | C << 35 | 0.41424324, 0.00859019, 0.48987231, 36 | 0.00859019, 0.70073226, 0.53176928, 37 | 0.48987231, 0.53176928, 0.09261140; 38 | 39 | // Eigenvalues of C should be -0.43457688, 0.52137662, 1.12078716 40 | EXPECT_TRUE(IsDegenerate(C, 1.)); 41 | EXPECT_FALSE(IsDegenerate(C, 0.4)); 42 | } 43 | -------------------------------------------------------------------------------- /localization/test/test_filter.cpp: -------------------------------------------------------------------------------- 1 | // Copyright 2022 Tixiao Shan, Takeshi Ishita 2 | // 3 | // Redistribution and use in source and binary forms, with or without 4 | // modification, are permitted provided that the following conditions are met: 5 | // 6 | // * Redistributions of source code must retain the above copyright 7 | // notice, this list of conditions and the following disclaimer. 8 | // 9 | // * Redistributions in binary form must reproduce the above copyright 10 | // notice, this list of conditions and the following disclaimer in the 11 | // documentation and/or other materials provided with the distribution. 12 | // 13 | // * Neither the name of the Tixiao Shan, Takeshi Ishita nor the names of its 14 | // contributors may be used to endorse or promote products derived from 15 | // this software without specific prior written permission. 16 | // 17 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 21 | // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | // POSSIBILITY OF SUCH DAMAGE. 28 | 29 | #include 30 | 31 | #include 32 | 33 | #include "lidar_feature_localization/filter.hpp" 34 | 35 | 36 | TEST(Filter, Filter) 37 | { 38 | std::vector a{4, 5, 1, 2, 0}; 39 | std::vector flags{true, false, true, true, false}; 40 | EXPECT_THAT(Filter(flags, a), testing::ElementsAre(4, 1, 2)); 41 | } 42 | -------------------------------------------------------------------------------- /localization/test/test_pointcloud_to_matrix.cpp: -------------------------------------------------------------------------------- 1 | // Copyright 2022 Tixiao Shan, Takeshi Ishita 2 | // 3 | // Redistribution and use in source and binary forms, with or without 4 | // modification, are permitted provided that the following conditions are met: 5 | // 6 | // * Redistributions of source code must retain the above copyright 7 | // notice, this list of conditions and the following disclaimer. 8 | // 9 | // * Redistributions in binary form must reproduce the above copyright 10 | // notice, this list of conditions and the following disclaimer in the 11 | // documentation and/or other materials provided with the distribution. 12 | // 13 | // * Neither the name of the Tixiao Shan, Takeshi Ishita nor the names of its 14 | // contributors may be used to endorse or promote products derived from 15 | // this software without specific prior written permission. 16 | // 17 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 21 | // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | // POSSIBILITY OF SUCH DAMAGE. 28 | 29 | #include 30 | 31 | #include "lidar_feature_library/point_type.hpp" 32 | 33 | #include "lidar_feature_localization/point_to_vector.hpp" 34 | #include "lidar_feature_localization/pointcloud_to_matrix.hpp" 35 | 36 | 37 | TEST(PointCloudToMatrix, PointXYZCRToVector) 38 | { 39 | const pcl::PointCloud::Ptr map(new pcl::PointCloud()); 40 | 41 | map->push_back(PointXYZCR(0., 1., 0., 2., 0)); 42 | map->push_back(PointXYZCR(1., 0., 1., 4., 1)); 43 | map->push_back(PointXYZCR(2., 0., 3., 4., 2)); 44 | 45 | const Eigen::MatrixXd matrix = PointCloudToMatrix(map); 46 | 47 | Eigen::MatrixXd expected(3, 4); 48 | expected << 49 | 0., 1., 0., 2., 50 | 1., 0., 1., 4., 51 | 2., 0., 3., 4.; 52 | 53 | EXPECT_EQ((matrix - expected).norm(), 0.); 54 | } 55 | -------------------------------------------------------------------------------- /localization/test/test_posevec.cpp: -------------------------------------------------------------------------------- 1 | // Copyright 2022 Tixiao Shan, Takeshi Ishita 2 | // 3 | // Redistribution and use in source and binary forms, with or without 4 | // modification, are permitted provided that the following conditions are met: 5 | // 6 | // * Redistributions of source code must retain the above copyright 7 | // notice, this list of conditions and the following disclaimer. 8 | // 9 | // * Redistributions in binary form must reproduce the above copyright 10 | // notice, this list of conditions and the following disclaimer in the 11 | // documentation and/or other materials provided with the distribution. 12 | // 13 | // * Neither the name of the Tixiao Shan, Takeshi Ishita nor the names of its 14 | // contributors may be used to endorse or promote products derived from 15 | // this software without specific prior written permission. 16 | // 17 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 21 | // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | // POSSIBILITY OF SUCH DAMAGE. 28 | 29 | #include 30 | 31 | #include "lidar_feature_localization/posevec.hpp" 32 | 33 | TEST(Posevec, AngleAxisToQuaternion) 34 | { 35 | { 36 | const Eigen::Quaterniond q = AngleAxisToQuaternion(Eigen::Vector3d(0, 0, 0)); 37 | EXPECT_EQ(q.norm(), 1.0); 38 | EXPECT_EQ(q.w(), 1.0); 39 | } 40 | 41 | { 42 | const Eigen::Vector3d theta = Eigen::Vector3d(0.2, 0.3, 0.4); 43 | const Eigen::Quaterniond q = AngleAxisToQuaternion(theta); 44 | 45 | const double k = theta.norm(); 46 | const Eigen::Matrix3d E = Eigen::AngleAxisd(k, theta / k).toRotationMatrix(); 47 | 48 | EXPECT_THAT(std::abs(q.norm() - 1.), testing::Le(1e-8)); 49 | EXPECT_THAT((q.toRotationMatrix() - E).norm(), testing::Le(1e-8)); 50 | } 51 | } 52 | -------------------------------------------------------------------------------- /map_loader/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.8) 2 | project(lidar_feature_map_loader) 3 | 4 | set(CMAKE_CXX_FLAGS 5 | "-g -O3 -Wall -Wextra -Wpedantic -Werror -fprofile-arcs -ftest-coverage") 6 | 7 | find_package(ament_cmake REQUIRED) 8 | find_package(ament_cmake_auto REQUIRED) 9 | 10 | ament_auto_find_build_dependencies() 11 | 12 | ament_auto_add_library(${PROJECT_NAME}_lib SHARED src/map_loader.cpp) 13 | rclcpp_components_register_node( 14 | ${PROJECT_NAME}_lib 15 | PLUGIN "MapLoaderNode" 16 | EXECUTABLE ${PROJECT_NAME}) 17 | 18 | if(BUILD_TESTING) 19 | find_package(ament_lint_auto REQUIRED) 20 | ament_lint_auto_find_test_dependencies() 21 | endif() 22 | 23 | ament_auto_package() 24 | -------------------------------------------------------------------------------- /map_loader/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | lidar_feature_map_loader 5 | 0.0.0 6 | Map loader 7 | Takeshi Ishita 8 | BSD 3-Clause 9 | 10 | ament_cmake 11 | ament_cmake_auto 12 | 13 | lidar_feature_library 14 | pcl_ros 15 | rclcpp 16 | sensor_msgs 17 | 18 | ament_lint_auto 19 | ament_lint_common 20 | 21 | 22 | ament_cmake 23 | 24 | 25 | -------------------------------------------------------------------------------- /map_tf_generator/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | project(map_tf_generator) 3 | 4 | ### Compile options 5 | if(NOT CMAKE_CXX_STANDARD) 6 | set(CMAKE_CXX_STANDARD 14) 7 | set(CMAKE_CXX_STANDARD_REQUIRED ON) 8 | set(CMAKE_CXX_EXTENSIONS OFF) 9 | endif() 10 | if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") 11 | add_compile_options(-Wall -O3 -Wextra -Wpedantic -Werror) 12 | endif() 13 | 14 | ### ROS Dependencies 15 | find_package(ament_cmake_auto REQUIRED) 16 | ament_auto_find_build_dependencies() 17 | 18 | ### non-ROS Dependencies 19 | find_package(PCL REQUIRED) 20 | 21 | # Generate exe file 22 | ament_auto_add_library(map_tf_generator_node SHARED 23 | src/map_tf_generator_node.cpp 24 | ) 25 | target_link_libraries(map_tf_generator_node ${PCL_LIBRARIES}) 26 | 27 | rclcpp_components_register_node(map_tf_generator_node 28 | PLUGIN "MapTFGeneratorNode" 29 | EXECUTABLE map_tf_generator 30 | ) 31 | 32 | if(BUILD_TESTING) 33 | find_package(ament_lint_auto REQUIRED) 34 | ament_lint_auto_find_test_dependencies() 35 | endif() 36 | 37 | ## Install 38 | ament_auto_package(INSTALL_TO_SHARE 39 | launch 40 | ) 41 | -------------------------------------------------------------------------------- /map_tf_generator/Readme.md: -------------------------------------------------------------------------------- 1 | # map_tf_generator 2 | 3 | ## Purpose 4 | 5 | This node broadcasts `viewer` frames for visualization of pointcloud map in Rviz. 6 | The position of `viewer` frames is the geometric center of input pointclouds. 7 | 8 | Note that there is no module to need `viewer` frames and this is used only for visualization. 9 | 10 | ## Inner-workings / Algorithms 11 | 12 | ## Inputs / Outputs 13 | 14 | ### Input 15 | 16 | | Name | Type | Description | 17 | | --------------------- | ------------------------------- | ----------------------------------------------------------------- | 18 | | `/map/pointcloud_map` | `sensor_msgs::msg::PointCloud2` | Subscribe pointcloud map to calculate position of `viewer` frames | 19 | 20 | ### Output 21 | 22 | | Name | Type | Description | 23 | | ------------ | ------------------------ | ------------------------- | 24 | | `/tf_static` | `tf2_msgs/msg/TFMessage` | Broadcast `viewer` frames | 25 | 26 | ## Parameters 27 | 28 | ### Node Parameters 29 | 30 | None 31 | 32 | ### Core Parameters 33 | 34 | | Name | Type | Default Value | Explanation | 35 | | -------------- | ------ | ------------- | ------------------------------------- | 36 | | `viewer_frame` | string | viewer | Name of `viewer` frame | 37 | | `map_frame` | string | map | The parent frame name of viewer frame | 38 | 39 | ## Assumptions / Known limits 40 | 41 | TBD. 42 | -------------------------------------------------------------------------------- /map_tf_generator/launch/map_tf_generator.launch.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | -------------------------------------------------------------------------------- /map_tf_generator/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | map_tf_generator 5 | 0.1.0 6 | map_tf_generator package as a ROS 2 node 7 | azumi-suzuki 8 | Apache License 2.0 9 | 10 | ament_cmake_auto 11 | 12 | libpcl-all-dev 13 | lidar_feature_library 14 | pcl_conversions 15 | rclcpp 16 | rclcpp_components 17 | std_msgs 18 | tf2 19 | tf2_ros 20 | 21 | ament_lint_auto 22 | 23 | 24 | ament_cmake 25 | 26 | 27 | -------------------------------------------------------------------------------- /mapping/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.8) 2 | project(lidar_feature_mapping) 3 | 4 | set(CMAKE_BUILD_TYPE "Debug") 5 | set(CMAKE_CXX_STANDARD 17) 6 | set(CMAKE_CXX_FLAGS 7 | "-g -O3 -Wall -Wextra -Wpedantic -Werror -fprofile-arcs -ftest-coverage") 8 | set(CMAKE_CXX_OUTPUT_EXTENSION_REPLACE ON) 9 | 10 | find_package(ament_cmake REQUIRED) 11 | find_package(ament_cmake_auto REQUIRED) 12 | 13 | ament_auto_find_build_dependencies() 14 | 15 | ament_auto_add_executable(${PROJECT_NAME} DIRECTORY src) 16 | 17 | function(add_testcase filepath) 18 | get_filename_component(filename ${filepath} NAME) 19 | string(REGEX REPLACE ".cpp" "" test_name ${filename}) 20 | 21 | message("test_name = ${test_name}") 22 | 23 | ament_add_gmock(${test_name} ${filepath}) 24 | target_include_directories(${test_name} PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}/include) 25 | ament_target_dependencies(${test_name} ${${PROJECT_NAME}_FOUND_BUILD_DEPENDS}) 26 | endfunction() 27 | 28 | if(BUILD_TESTING) 29 | find_package(ament_lint_auto REQUIRED) 30 | 31 | list(APPEND AMENT_LINT_AUTO_EXCLUDE 32 | ament_cmake_cpplint 33 | ament_cmake_uncrustify 34 | ) 35 | 36 | ament_lint_auto_find_test_dependencies() 37 | 38 | file(GLOB SOURCE_FILES src/* include/*) 39 | 40 | find_package(ament_cmake_cpplint) 41 | ament_cpplint(${SOURCE_FILES}) 42 | 43 | find_package(ament_cmake_uncrustify) 44 | ament_uncrustify(${SOURCE_FILES}) 45 | 46 | file(GLOB TEST_FILES test/*) 47 | foreach(filepath ${TEST_FILES}) 48 | add_testcase(${filepath}) 49 | endforeach() 50 | endif() 51 | 52 | ament_auto_package() 53 | -------------------------------------------------------------------------------- /mapping/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | lidar_feature_mapping 5 | 0.1.0 6 | Tool to build a lidar-feature map 7 | Takeshi Ishita 8 | BSD 3-Clause 9 | 10 | ament_cmake 11 | 12 | eigen 13 | geometry_msgs 14 | lidar_feature_library 15 | message_filters 16 | pcl_ros 17 | rclcpp 18 | sensor_msgs 19 | tf2_eigen 20 | 21 | ament_lint_auto 22 | ament_lint_common 23 | 24 | ament_cmake_gtest 25 | ament_cmake_gmock 26 | 27 | 28 | ament_cmake 29 | 30 | 31 | -------------------------------------------------------------------------------- /maps/.gitkeep: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/tier4/lidar_feature_extraction/67c5eee072417ef29017e66993d64a978e7a0f96/maps/.gitkeep -------------------------------------------------------------------------------- /path_generator/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.16) 2 | project(path_generator) 3 | 4 | set(CMAKE_BUILD_TYPE "Debug") 5 | set(CMAKE_CXX_STANDARD 17) 6 | set(CMAKE_CXX_FLAGS 7 | "-pg -O3 -g -Wall -Wextra -Wpedantic -Werror -fprofile-arcs -ftest-coverage") 8 | set(CMAKE_CXX_OUTPUT_EXTENSION_REPLACE ON) 9 | 10 | find_package(ament_cmake REQUIRED) 11 | find_package(ament_cmake_auto REQUIRED) 12 | 13 | ament_auto_find_build_dependencies() 14 | 15 | ament_auto_add_executable("path_generator" src/node.cpp) 16 | target_include_directories(${PROJECT_NAME} PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}/include) 17 | 18 | file(GLOB HEADER_FILES include/path_generator/*) 19 | target_precompile_headers(${PROJECT_NAME} PUBLIC ${HEADER_FILES}) 20 | 21 | function(add_testcase filepath) 22 | get_filename_component(filename ${filepath} NAME) 23 | string(REGEX REPLACE ".cpp" "" test_name ${filename}) 24 | 25 | message("test_name = ${test_name}") 26 | 27 | ament_add_gmock(${test_name} ${filepath}) 28 | target_include_directories(${test_name} PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}/include) 29 | ament_target_dependencies(${test_name} ${${PROJECT_NAME}_FOUND_BUILD_DEPENDS}) 30 | endfunction() 31 | 32 | if(BUILD_TESTING) 33 | find_package(ament_lint_auto REQUIRED) 34 | 35 | list(APPEND AMENT_LINT_AUTO_EXCLUDE 36 | ament_cmake_cpplint 37 | ament_cmake_uncrustify 38 | ) 39 | 40 | ament_lint_auto_find_test_dependencies() 41 | 42 | file(GLOB FILES_TO_CHECK src/* include/* test/*) 43 | 44 | find_package(ament_cmake_cpplint) 45 | ament_cpplint(${FILES_TO_CHECK}) 46 | 47 | find_package(ament_cmake_uncrustify) 48 | ament_uncrustify(${FILES_TO_CHECK}) 49 | 50 | file(GLOB TEST_FILES test/*) 51 | foreach(filepath ${TEST_FILES}) 52 | add_testcase(${filepath}) 53 | endforeach() 54 | endif() 55 | 56 | ament_auto_package() 57 | -------------------------------------------------------------------------------- /path_generator/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | path_generator 5 | 0.0.1 6 | Generates a path from subscribed poses 7 | Takeshi Ishita 8 | BSD 3-Clause 9 | 10 | ament_cmake 11 | 12 | geometry_msgs 13 | lidar_feature_library 14 | nav_msgs 15 | rclcpp 16 | 17 | ament_cmake_gmock 18 | ament_lint_auto 19 | ament_lint_common 20 | 21 | 22 | ament_cmake 23 | 24 | 25 | -------------------------------------------------------------------------------- /path_generator/src/node.cpp: -------------------------------------------------------------------------------- 1 | // Copyright 2022 Takeshi Ishita 2 | // 3 | // Redistribution and use in source and binary forms, with or without 4 | // modification, are permitted provided that the following conditions are met: 5 | // 6 | // * Redistributions of source code must retain the above copyright 7 | // notice, this list of conditions and the following disclaimer. 8 | // 9 | // * Redistributions in binary form must reproduce the above copyright 10 | // notice, this list of conditions and the following disclaimer in the 11 | // documentation and/or other materials provided with the distribution. 12 | // 13 | // * Neither the name of the Takeshi Ishita nor the names of its 14 | // contributors may be used to endorse or promote products derived from 15 | // this software without specific prior written permission. 16 | // 17 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 21 | // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | // POSSIBILITY OF SUCH DAMAGE. 28 | 29 | #include 30 | 31 | #include 32 | 33 | #include "path_generator/path_generator.hpp" 34 | 35 | 36 | int main(int argc, char * argv[]) 37 | { 38 | rclcpp::init(argc, argv); 39 | rclcpp::spin(std::make_shared("path", "pose")); 40 | rclcpp::shutdown(); 41 | return 0; 42 | } 43 | -------------------------------------------------------------------------------- /point_type_converter/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | point_type_converter 5 | 0.0.0 6 | PointCloud data type converter 7 | sonicair 8 | BSD 3-Clause 9 | 10 | ament_copyright 11 | ament_flake8 12 | ament_pep257 13 | python3-pytest 14 | 15 | 16 | ament_python 17 | 18 | 19 | -------------------------------------------------------------------------------- /point_type_converter/point_type_converter/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/tier4/lidar_feature_extraction/67c5eee072417ef29017e66993d64a978e7a0f96/point_type_converter/point_type_converter/__init__.py -------------------------------------------------------------------------------- /point_type_converter/resource/point_type_converter: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/tier4/lidar_feature_extraction/67c5eee072417ef29017e66993d64a978e7a0f96/point_type_converter/resource/point_type_converter -------------------------------------------------------------------------------- /point_type_converter/setup.cfg: -------------------------------------------------------------------------------- 1 | [develop] 2 | script_dir=$base/lib/point_type_converter 3 | [install] 4 | install_scripts=$base/lib/point_type_converter 5 | -------------------------------------------------------------------------------- /point_type_converter/setup.py: -------------------------------------------------------------------------------- 1 | from setuptools import setup 2 | 3 | package_name = 'point_type_converter' 4 | 5 | setup( 6 | name=package_name, 7 | version='0.0.0', 8 | packages=[package_name], 9 | data_files=[ 10 | ('share/ament_index/resource_index/packages', 11 | ['resource/' + package_name]), 12 | ('share/' + package_name, ['package.xml']), 13 | ], 14 | install_requires=['setuptools'], 15 | zip_safe=True, 16 | maintainer='Takeshi Ishita', 17 | maintainer_email='ishitah.takeshi@gmail.com', 18 | description='Point cloud conversion for the extraction module', 19 | license='BSD3-Clause', 20 | tests_require=['pytest'], 21 | entry_points={ 22 | 'console_scripts': [ 23 | 'listener = point_type_converter.convert:main', 24 | ], 25 | } 26 | ) 27 | -------------------------------------------------------------------------------- /point_type_converter/test/test_copyright.py: -------------------------------------------------------------------------------- 1 | # Copyright 2015 Open Source Robotics Foundation, Inc. 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | from ament_copyright.main import main 16 | import pytest 17 | 18 | 19 | @pytest.mark.copyright 20 | @pytest.mark.linter 21 | def test_copyright(): 22 | rc = main(argv=['.', 'test']) 23 | assert rc == 0, 'Found errors' 24 | -------------------------------------------------------------------------------- /point_type_converter/test/test_flake8.py: -------------------------------------------------------------------------------- 1 | # Copyright 2017 Open Source Robotics Foundation, Inc. 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | from ament_flake8.main import main_with_errors 16 | import pytest 17 | 18 | 19 | @pytest.mark.flake8 20 | @pytest.mark.linter 21 | def test_flake8(): 22 | rc, errors = main_with_errors(argv=[]) 23 | assert rc == 0, \ 24 | 'Found %d code style errors / warnings:\n' % len(errors) + \ 25 | '\n'.join(errors) 26 | -------------------------------------------------------------------------------- /point_type_converter/test/test_pep257.py: -------------------------------------------------------------------------------- 1 | # Copyright 2015 Open Source Robotics Foundation, Inc. 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | from ament_pep257.main import main 16 | import pytest 17 | 18 | 19 | @pytest.mark.linter 20 | @pytest.mark.pep257 21 | def test_pep257(): 22 | rc = main(argv=['.', 'test']) 23 | assert rc == 0, 'Found code style errors / warnings' 24 | -------------------------------------------------------------------------------- /rotationlib/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.16) 2 | project(rotationlib) 3 | 4 | set(CMAKE_BUILD_TYPE "Debug") 5 | set(CMAKE_CXX_STANDARD 17) 6 | set(CMAKE_CXX_FLAGS 7 | "-pg -O3 -Wall -Wextra -Wpedantic -Werror -fprofile-arcs -ftest-coverage") 8 | set(CMAKE_CXX_OUTPUT_EXTENSION_REPLACE ON) 9 | 10 | if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") 11 | add_compile_options(-Wall -Wextra -Wpedantic) 12 | endif() 13 | 14 | set(BUILD_TESTING ON) 15 | 16 | find_package(ament_cmake REQUIRED) 17 | find_package(ament_cmake_auto REQUIRED) 18 | 19 | ament_auto_find_build_dependencies() 20 | 21 | set(SOURCE_FILES 22 | src/hat.cpp 23 | src/quaternion.cpp 24 | src/jacobian/quaternion.cpp) 25 | 26 | ament_auto_add_library(${PROJECT_NAME}_lib SHARED ${SOURCE_FILES}) 27 | 28 | function(add_testcase filepath) 29 | get_filename_component(filename ${filepath} NAME) 30 | string(REGEX REPLACE ".cpp" "" test_name ${filename}) 31 | 32 | ament_add_gmock(${test_name} ${filepath}) 33 | target_link_libraries(${test_name} ${PROJECT_NAME}_lib) 34 | target_include_directories(${test_name} PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}/testlib) 35 | ament_target_dependencies(${test_name} ${${PROJECT_NAME}_FOUND_BUILD_DEPENDS}) 36 | endfunction() 37 | 38 | 39 | if(BUILD_TESTING) 40 | set(TEST_FILES 41 | test/test_hat.cpp 42 | test/test_quaternion.cpp 43 | test/test_jacobian_quaternion.cpp) 44 | 45 | find_package(ament_lint_auto REQUIRED) 46 | 47 | list(APPEND AMENT_LINT_AUTO_EXCLUDE 48 | ament_cmake_cpplint 49 | ament_cmake_uncrustify 50 | ) 51 | 52 | ament_lint_auto_find_test_dependencies() 53 | 54 | file(GLOB_RECURSE SOURCE_AND_HEADER src/* include/*) 55 | 56 | find_package(ament_cmake_cpplint) 57 | ament_cpplint(${SOURCE_AND_HEADER}) 58 | 59 | find_package(ament_cmake_uncrustify) 60 | ament_uncrustify(${SOURCE_AND_HEADER}) 61 | 62 | foreach(filepath ${TEST_FILES}) 63 | add_testcase(${filepath}) 64 | endforeach() 65 | endif() 66 | 67 | ament_auto_package() 68 | -------------------------------------------------------------------------------- /rotationlib/include/rotationlib/hat.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2022 Takeshi Ishita 2 | // 3 | // Redistribution and use in source and binary forms, with or without 4 | // modification, are permitted provided that the following conditions are met: 5 | // 6 | // * Redistributions of source code must retain the above copyright 7 | // notice, this list of conditions and the following disclaimer. 8 | // 9 | // * Redistributions in binary form must reproduce the above copyright 10 | // notice, this list of conditions and the following disclaimer in the 11 | // documentation and/or other materials provided with the distribution. 12 | // 13 | // * Neither the name of the Takeshi Ishita nor the names of its 14 | // contributors may be used to endorse or promote products derived from 15 | // this software without specific prior written permission. 16 | // 17 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 21 | // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | // POSSIBILITY OF SUCH DAMAGE. 28 | 29 | #ifndef ROTATIONLIB__HAT_HPP_ 30 | #define ROTATIONLIB__HAT_HPP_ 31 | 32 | #include 33 | 34 | namespace rotationlib 35 | { 36 | 37 | Eigen::Matrix3d Hat(const Eigen::Vector3d & v); 38 | 39 | } // namespace rotationlib 40 | 41 | #endif // ROTATIONLIB__HAT_HPP_ 42 | -------------------------------------------------------------------------------- /rotationlib/include/rotationlib/jacobian/quaternion.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2022 Takeshi Ishita 2 | // 3 | // Redistribution and use in source and binary forms, with or without 4 | // modification, are permitted provided that the following conditions are met: 5 | // 6 | // * Redistributions of source code must retain the above copyright 7 | // notice, this list of conditions and the following disclaimer. 8 | // 9 | // * Redistributions in binary form must reproduce the above copyright 10 | // notice, this list of conditions and the following disclaimer in the 11 | // documentation and/or other materials provided with the distribution. 12 | // 13 | // * Neither the name of the Takeshi Ishita nor the names of its 14 | // contributors may be used to endorse or promote products derived from 15 | // this software without specific prior written permission. 16 | // 17 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 21 | // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | // POSSIBILITY OF SUCH DAMAGE. 28 | 29 | #ifndef ROTATIONLIB__JACOBIAN__QUATERNION_HPP_ 30 | #define ROTATIONLIB__JACOBIAN__QUATERNION_HPP_ 31 | 32 | #include 33 | #include 34 | 35 | #include "rotationlib/hat.hpp" 36 | 37 | namespace rotationlib 38 | { 39 | 40 | Eigen::Matrix DRpDq(const Eigen::Quaterniond & q, const Eigen::Vector3d & p); 41 | 42 | } // namespace rotationlib 43 | 44 | #endif // ROTATIONLIB__JACOBIAN__QUATERNION_HPP_ 45 | -------------------------------------------------------------------------------- /rotationlib/include/rotationlib/quaternion.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2022 Takeshi Ishita 2 | // 3 | // Redistribution and use in source and binary forms, with or without 4 | // modification, are permitted provided that the following conditions are met: 5 | // 6 | // * Redistributions of source code must retain the above copyright 7 | // notice, this list of conditions and the following disclaimer. 8 | // 9 | // * Redistributions in binary form must reproduce the above copyright 10 | // notice, this list of conditions and the following disclaimer in the 11 | // documentation and/or other materials provided with the distribution. 12 | // 13 | // * Neither the name of the Takeshi Ishita nor the names of its 14 | // contributors may be used to endorse or promote products derived from 15 | // this software without specific prior written permission. 16 | // 17 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 21 | // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | // POSSIBILITY OF SUCH DAMAGE. 28 | 29 | 30 | #ifndef ROTATIONLIB__QUATERNION_HPP_ 31 | #define ROTATIONLIB__QUATERNION_HPP_ 32 | 33 | #include 34 | #include 35 | 36 | namespace rotationlib 37 | { 38 | 39 | Eigen::Quaterniond FromWXYZ(const Eigen::Vector4d & wxyz); 40 | 41 | Eigen::Vector4d ToWXYZ(const Eigen::Quaterniond & q); 42 | 43 | Eigen::Matrix4d LeftMultiplicationMatrix(const Eigen::Quaterniond & q); 44 | 45 | Eigen::Matrix4d RightMultiplicationMatrix(const Eigen::Quaterniond & q); 46 | 47 | Eigen::Quaterniond RPYToQuaternionXYZ(const double roll, const double pitch, const double yaw); 48 | 49 | } // namespace rotationlib 50 | 51 | #endif // ROTATIONLIB__QUATERNION_HPP_ 52 | -------------------------------------------------------------------------------- /rotationlib/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | rotationlib 5 | 0.0.0 6 | Rotation computation library 7 | Takeshi Ishita 8 | BSD 3-Clause 9 | 10 | eigen 11 | 12 | ament_cmake 13 | 14 | ament_cmake_gmock 15 | ament_lint_auto 16 | ament_lint_common 17 | 18 | 19 | ament_cmake 20 | 21 | 22 | -------------------------------------------------------------------------------- /rotationlib/src/hat.cpp: -------------------------------------------------------------------------------- 1 | // Copyright 2022 Takeshi Ishita 2 | // 3 | // Redistribution and use in source and binary forms, with or without 4 | // modification, are permitted provided that the following conditions are met: 5 | // 6 | // * Redistributions of source code must retain the above copyright 7 | // notice, this list of conditions and the following disclaimer. 8 | // 9 | // * Redistributions in binary form must reproduce the above copyright 10 | // notice, this list of conditions and the following disclaimer in the 11 | // documentation and/or other materials provided with the distribution. 12 | // 13 | // * Neither the name of the Takeshi Ishita nor the names of its 14 | // contributors may be used to endorse or promote products derived from 15 | // this software without specific prior written permission. 16 | // 17 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 21 | // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | // POSSIBILITY OF SUCH DAMAGE. 28 | 29 | #include "rotationlib/hat.hpp" 30 | 31 | 32 | namespace rotationlib 33 | { 34 | 35 | Eigen::Matrix3d Hat(const Eigen::Vector3d & v) 36 | { 37 | Eigen::Matrix3d M; 38 | M << 39 | 0., -v(2), v(1), 40 | v(2), 0., -v(0), 41 | -v(1), v(0), 0.; 42 | return M; 43 | } 44 | 45 | } // namespace rotationlib 46 | -------------------------------------------------------------------------------- /rotationlib/src/jacobian/quaternion.cpp: -------------------------------------------------------------------------------- 1 | // Copyright 2022 Takeshi Ishita 2 | // 3 | // Redistribution and use in source and binary forms, with or without 4 | // modification, are permitted provided that the following conditions are met: 5 | // 6 | // * Redistributions of source code must retain the above copyright 7 | // notice, this list of conditions and the following disclaimer. 8 | // 9 | // * Redistributions in binary form must reproduce the above copyright 10 | // notice, this list of conditions and the following disclaimer in the 11 | // documentation and/or other materials provided with the distribution. 12 | // 13 | // * Neither the name of the Takeshi Ishita nor the names of its 14 | // contributors may be used to endorse or promote products derived from 15 | // this software without specific prior written permission. 16 | // 17 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 21 | // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | // POSSIBILITY OF SUCH DAMAGE. 28 | 29 | #include "rotationlib/jacobian/quaternion.hpp" 30 | 31 | 32 | namespace rotationlib 33 | { 34 | 35 | Eigen::Matrix DRpDq(const Eigen::Quaterniond & q, const Eigen::Vector3d & p) 36 | { 37 | // Sola, Joan. 38 | // "Quaternion kinematics for the error-state Kalman filter." 39 | // arXiv preprint arXiv:1711.02508 (2017). 40 | // Equation 174 41 | const Eigen::Matrix3d I = Eigen::Matrix3d::Identity(); 42 | const Eigen::Matrix3d K = rotationlib::Hat(p); 43 | 44 | const double w = q.w(); 45 | const Eigen::Vector3d v = q.vec(); 46 | 47 | Eigen::Matrix half_j; 48 | half_j.col(0) = w * p + v.cross(p); 49 | half_j.rightCols(3) = v.dot(p) * I + v * p.transpose() - p * v.transpose() - w * K; 50 | 51 | return 2. * half_j; 52 | } 53 | 54 | } // namespace rotationlib 55 | -------------------------------------------------------------------------------- /rotationlib/test/test_hat.cpp: -------------------------------------------------------------------------------- 1 | // Copyright 2022 Takeshi Ishita 2 | // 3 | // Redistribution and use in source and binary forms, with or without 4 | // modification, are permitted provided that the following conditions are met: 5 | // 6 | // * Redistributions of source code must retain the above copyright 7 | // notice, this list of conditions and the following disclaimer. 8 | // 9 | // * Redistributions in binary form must reproduce the above copyright 10 | // notice, this list of conditions and the following disclaimer in the 11 | // documentation and/or other materials provided with the distribution. 12 | // 13 | // * Neither the name of the Takeshi Ishita nor the names of its 14 | // contributors may be used to endorse or promote products derived from 15 | // this software without specific prior written permission. 16 | // 17 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 21 | // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | // POSSIBILITY OF SUCH DAMAGE. 28 | 29 | #include 30 | 31 | #include "rotationlib/hat.hpp" 32 | 33 | TEST(Hat, Hat) 34 | { 35 | const Eigen::Matrix3d K = rotationlib::Hat(Eigen::Vector3d(1, 2, 3)); 36 | Eigen::Matrix3d expected; 37 | expected << 38 | 0., -3., 2., 39 | 3., 0., -1., 40 | -2., 1., 0.; 41 | EXPECT_EQ((K - expected).norm(), 0.); 42 | } 43 | -------------------------------------------------------------------------------- /rotationlib/testlib/near_with_precision.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2022 Takeshi Ishita 2 | // 3 | // Redistribution and use in source and binary forms, with or without 4 | // modification, are permitted provided that the following conditions are met: 5 | // 6 | // * Redistributions of source code must retain the above copyright 7 | // notice, this list of conditions and the following disclaimer. 8 | // 9 | // * Redistributions in binary form must reproduce the above copyright 10 | // notice, this list of conditions and the following disclaimer in the 11 | // documentation and/or other materials provided with the distribution. 12 | // 13 | // * Neither the name of the Takeshi Ishita nor the names of its 14 | // contributors may be used to endorse or promote products derived from 15 | // this software without specific prior written permission. 16 | // 17 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 21 | // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | // POSSIBILITY OF SUCH DAMAGE. 28 | 29 | #ifndef ROTATIONLIB__TESTLIB__NEAR_WITH_PRECISION_HPP_ 30 | #define ROTATIONLIB__TESTLIB__NEAR_WITH_PRECISION_HPP_ 31 | 32 | #include 33 | 34 | #include 35 | #include 36 | 37 | MATCHER_P(NearWithPrecision, precision, "") { 38 | return std::abs(std::get<0>(arg) - std::get<1>(arg)) < precision; 39 | } 40 | 41 | #endif // ROTATIONLIB__TESTLIB__NEAR_WITH_PRECISION_HPP_ 42 | --------------------------------------------------------------------------------