chore(deps): bump tj-actions/changed-files from 32 to 35

.github/workflows/build-and-test-differential.yaml

@@ -77,7 +77,7 @@ jobs:
 
       - name: Get modified files
         id: get-modified-files
-        uses: tj-actions/changed-files@v34
+        uses: tj-actions/changed-files@v35
         with:
           files: |
             **/*.cpp

localization/lidar_feature_localization/CMakeLists.txt

@@ -0,0 +1,179 @@
+cmake_minimum_required(VERSION 3.16)
+project(lidar_feature_localization)
+
+set(CMAKE_CXX_STANDARD 17)
+add_compile_options(-pg -g -O3 -Wall -Wextra -Wpedantic -Werror)
+
+if(${CMAKE_SYSTEM_PROCESSOR} MATCHES "x86_64")
+  add_compile_options(-msse -msse2 -msse3 -msse4 -msse4.1 -msse4.2)
+endif()
+
+set(CMAKE_BUILD_TYPE "Release")
+set(CMAKE_CXX_OUTPUT_EXTENSION_REPLACE ON)
+
+find_package(ament_cmake REQUIRED)
+find_package(ament_cmake_auto REQUIRED)
+find_package(fmt REQUIRED)
+
+ament_auto_find_build_dependencies()
+
+set(INCLUDE_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/include)
+
+set(LIBRARY_SOURCE_FILES
+  src/lib/algorithm.cpp
+  src/lib/csv.cpp
+  src/lib/eigen.cpp
+  src/lib/lib.cpp
+  src/lib/pcl_utils.cpp
+  src/lib/ros_msg.cpp
+  src/lib/random.cpp
+  src/lib/stats.cpp)
+
+set(LOCALIZATION_SOURCE_FILES
+  src/localization/alignment.cpp
+  src/localization/debug.cpp
+  src/localization/degenerate.cpp
+  src/localization/edge.cpp
+  src/localization/eigen.cpp
+  src/localization/jacobian.cpp
+  src/localization/kdtree.cpp
+  src/localization/odometry_integration.cpp
+  src/localization/optimizer.cpp
+  src/localization/posevec.cpp
+  src/localization/problem.cpp
+  src/localization/robust.cpp
+  src/localization/surface.cpp)
+
+set(EXTRACTION_SOURCE_FILES
+  src/extraction/convolution.cpp
+  src/extraction/curvature.cpp
+  src/extraction/index_range.cpp
+  src/extraction/iterator.cpp
+  src/extraction/label.cpp
+  src/extraction/math.cpp
+  src/extraction/ring.cpp)
+
+ament_auto_add_library(library SHARED ${LIBRARY_SOURCE_FILES})
+ament_auto_add_library(localization SHARED ${LOCALIZATION_SOURCE_FILES})
+ament_auto_add_library(extraction SHARED ${EXTRACTION_SOURCE_FILES})
+
+target_include_directories(library PUBLIC ${INCLUDE_DIRS})
+target_include_directories(localization PUBLIC ${INCLUDE_DIRS})
+target_include_directories(extraction PUBLIC ${INCLUDE_DIRS})
+
+target_link_libraries(library ${PCL_LIBRARIES} fmt)
+target_link_libraries(localization fmt)
+target_link_libraries(extraction fmt)
+
+ament_auto_add_executable("lidar_feature_localization" app/localization.cpp)
+target_link_libraries("lidar_feature_localization" library localization extraction)
+
+function(add_localization_testcase filepath)
+  get_filename_component(filename ${filepath} NAME)
+  string(REGEX REPLACE ".cpp" "" test_name_suffix ${filename})
+  set(test_name "localization_${test_name_suffix}")
+
+  ament_add_gmock(${test_name} ${filepath})
+  target_link_libraries(${test_name} library localization)
+  ament_target_dependencies(${test_name} ${${PROJECT_NAME}_FOUND_BUILD_DEPENDS})
+endfunction()
+
+function(add_extraction_testcase filepath)
+  get_filename_component(filename ${filepath} NAME)
+  string(REGEX REPLACE ".cpp" "" test_name_suffix ${filename})
+  set(test_name "extraction_${test_name_suffix}")
+
+  ament_add_gmock(${test_name} ${filepath})
+  target_link_libraries("${test_name}" library extraction)
+  ament_target_dependencies(${test_name} ${${PROJECT_NAME}_FOUND_BUILD_DEPENDS})
+endfunction()
+
+function(add_lib_testcase filepath)
+  get_filename_component(filename ${filepath} NAME)
+  string(REGEX REPLACE ".cpp" "" test_name_suffix ${filename})
+  set(test_name "lib_${test_name_suffix}")
+
+  ament_add_gmock(${test_name} ${filepath})
+  target_link_libraries("${test_name}" library)
+  ament_target_dependencies(${test_name} ${${PROJECT_NAME}_FOUND_BUILD_DEPENDS})
+endfunction()
+
+if(BUILD_TESTING)
+  file(GLOB
+    FILES_TO_CHECK
+    ${CMAKE_CURRENT_SOURCE_DIR}/app/*
+    ${CMAKE_CURRENT_SOURCE_DIR}/src/*
+    ${CMAKE_CURRENT_SOURCE_DIR}/include/*
+    ${CMAKE_CURRENT_SOURCE_DIR}/test/*)
+
+  find_package(ament_cmake_cpplint)
+  ament_cpplint(${FILES_TO_CHECK})
+
+  find_package(ament_cmake_uncrustify)
+  ament_uncrustify(${FILES_TO_CHECK})
+
+  find_package(ament_cmake_gmock REQUIRED)
+
+  set(LOCALIZATION_TEST_FILES
+    test/localization/test_alignment.cpp
+    test/localization/test_angular_integration.cpp
+    test/localization/test_degenerate.cpp
+    test/localization/test_edge.cpp
+    test/localization/test_filter.cpp
+    test/localization/test_jacobian.cpp
+    test/localization/test_kdtree.cpp
+    test/localization/test_math.cpp
+    test/localization/test_odometry_integration.cpp
+    test/localization/test_optimizer.cpp
+    test/localization/test_pointcloud_to_matrix.cpp
+    test/localization/test_recent_scans.cpp
+    test/localization/test_robust.cpp
+    test/localization/test_sliding_window.cpp
+    test/localization/test_stamp_sorted_objects.cpp
+    test/localization/test_surface.cpp
+    test/localization/test_velocity_integration.cpp)
+
+  foreach(filepath ${LOCALIZATION_TEST_FILES})
+    add_localization_testcase(${filepath})
+  endforeach()
+
+  set(EXTRACTION_TEST_FILES
+    test/extraction/test_iterator.cpp
+    test/extraction/test_algorithm.cpp
+    test/extraction/test_convolution.cpp
+    test/extraction/test_mapped_points.cpp
+    test/extraction/test_curvature.cpp
+    test/extraction/test_occlusion.cpp
+    test/extraction/test_out_of_range.cpp
+    test/extraction/test_range_message.cpp
+    test/extraction/test_range.cpp
+    test/extraction/test_neighbor.cpp
+    test/extraction/test_label.cpp
+    test/extraction/test_fill.cpp
+    test/extraction/test_parallel_beam.cpp
+    test/extraction/test_math.cpp
+    test/extraction/test_ring.cpp
+    test/extraction/test_index_range.cpp)
+
+  foreach(filepath ${EXTRACTION_TEST_FILES})
+    add_extraction_testcase(${filepath})
+  endforeach()
+
+  set(LIB_TEST_FILES
+    test/lib/test_algorithm.cpp
+    test/lib/test_csv.cpp
+    test/lib/test_degree_to_radian.cpp
+    test/lib/test_eigen.cpp
+    test/lib/test_numeric.cpp
+    test/lib/test_pcl_utils.cpp
+    test/lib/test_random.cpp
+    test/lib/test_ros_msg.cpp
+    test/lib/test_span.cpp
+    test/lib/test_stats.cpp)
+
+  foreach(filepath ${LIB_TEST_FILES})
+    add_lib_testcase(${filepath})
+  endforeach()
+endif()
+
+ament_auto_package(INSTALL_TO_SHARE launch config)

localization/lidar_feature_localization/README.md

@@ -0,0 +1,119 @@
+# lidar_feature_localization
+
+## Input / Output
+
+### Input
+
+| Name                             | Type                                            | Description                                                   |
+|:---------------------------------|:------------------------------------------------|:--------------------------------------------------------------|
+| `points_raw`                     | `sensor_msgs::msg::PointCloud2`                 | Rectified LiDAR scan                                          |
+| `optimization_start_pose`        | `geometry_msgs::msg::PoseWithCovarianceStamped` | Prior pose obtained from other estimators such as GNSS or EKF |
+
+### Output
+
+| Name                             | Type                                            | Description                                               |
+|:---------------------------------|:------------------------------------------------|:----------------------------------------------------------|
+| `edge_features`                  | `sensor_msgs::msg::PointCloud2`                 | Edge features extracted from LiDAR scan                   |
+| `surface_features`               | `sensor_msgs::msg::PointCloud2`                 | Surface features extracted from LiDAR scan                |
+| `estimated_pose`                 | `geometry_msgs::msg::PoseStamped`               | Estimated LiDAR pose on the map coordinate                |
+| `estimated_pose_with_covariance` | `geometry_msgs::msg::PoseWithCovarianceStamped` | Estimated LiDAR pose and covariance on the map coordinate |
+
+## Citation
+
+This code makes use of LIO-SAM and LeGO-LOAM.
+
+### [LIO-SAM](https://github.com/TixiaoShan/LIO-SAM)
+
+```
+@inproceedings{liosam2020shan,
+  title={LIO-SAM: Tightly-coupled Lidar Inertial Odometry via Smoothing and Mapping},
+  author={Shan, Tixiao and Englot, Brendan and Meyers, Drew and Wang, Wei and Ratti, Carlo and Rus Daniela},
+  booktitle={IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS)},
+  pages={5135-5142},
+  year={2020},
+  organization={IEEE}
+}
+```
+
+```
+BSD 3-Clause License
+
+Copyright (c) 2020, Tixiao Shan
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+
+* Redistributions of source code must retain the above copyright notice, this
+  list of conditions and the following disclaimer.
+
+* Redistributions in binary form must reproduce the above copyright notice,
+  this list of conditions and the following disclaimer in the documentation
+  and/or other materials provided with the distribution.
+
+* Neither the name of the copyright holder nor the names of its
+  contributors may be used to endorse or promote products derived from
+  this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
+FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
+OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+```
+
+### [LeGO-LOAM](https://github.com/RobustFieldAutonomyLab/LeGO-LOAM)
+
+```
+@inproceedings{legoloam2018shan,
+  title={LeGO-LOAM: Lightweight and Ground-Optimized Lidar Odometry and Mapping on Variable Terrain},
+  author={Shan, Tixiao and Englot, Brendan},
+  booktitle={IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS)},
+  pages={4758-4765},
+  year={2018},
+  organization={IEEE}
+}
+```
+
+```
+
+BSD 3-Clause License
+
+Copyright (c) 2018, Robust Field Autonomy Lab
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+
+* Redistributions of source code must retain the above copyright notice, this
+  list of conditions and the following disclaimer.
+
+* Redistributions in binary form must reproduce the above copyright notice,
+  this list of conditions and the following disclaimer in the documentation
+  and/or other materials provided with the distribution.
+
+* Neither the name of the copyright holder nor the names of its
+  contributors may be used to endorse or promote products derived from
+  this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
+FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
+OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+```
+
+## Special Thanks
+
+* [Tixiao Shan](https://github.com/TixiaoShan)
+* [RobustFieldAutonomyLab](https://github.com/RobustFieldAutonomyLab)