first commit

.gitignore

@@ -0,0 +1,25 @@
+.vscode/
+build/
+devel/
+.idea/
+Logs
+*.active
+*.bag
+*.tar.gz
+*.log
+*.py[cod]
+*.egg
+*.egg-info
+/**/build
+/**/dist
+venv/
+.history/
+*.pcd
+*.bag
+*.pt
+*.onnx
+*.jpg
+*.mp4
+*.png
+*.out
+log/

install_dep_ddautopilot.sh

@@ -0,0 +1,61 @@
+#!/bin/bash
+
+# ROS Humble 依赖安装脚本
+
+set -e  # 遇到错误立即退出
+
+echo "========================================"
+echo "  开始安装 ROS Humble 依赖包"
+echo "========================================"
+
+# ==================== 系统基础工具 ====================
+echo "[1/3] 安装系统基础工具和库..."
+sudo apt-get update
+
+sudo apt-get install -y \
+    v4l-utils \
+    python3-pip \
+    libpcap-dev \
+    nlohmann-json3-dev \
+    libceres-dev \
+    libompl-dev \
+    libnanoflann-dev \
+    libbenchmark-dev \
+    libgraphicsmagick++-dev \
+    libserialport-dev \
+    libserial-dev
+
+# ==================== ROS Humble 核心包 ====================
+echo "[2/3] 安装 ROS Humble 核心功能包..."
+sudo apt-get install -y \
+    ros-humble-pcl-ros \
+    ros-humble-pcl-conversions \
+    ros-humble-apriltag-ros \
+    ros-humble-can-msgs \
+    ros-humble-geographic-msgs \
+    ros-humble-autoware-msgs \
+    ros-humble-bond \
+    ros-humble-bondcpp \
+    ros-humble-test-msgs \
+    ros-humble-behaviortree-cpp \
+    ros-humble-diagnostic-updater \
+    ros-humble-robot-localization \
+    ros-humble-nmea-msgs \
+    ros-humble-rviz-2d-overlay-msgs
+
+# ==================== Python 依赖 (PyPI) ====================
+echo "[3/3] 安装 Python PyPI 依赖..."
+# 使用清华镜像加速
+pip install -i https://pypi.tuna.tsinghua.edu.cn/simple \
+    'pydantic<2.0' \
+    loguru \
+    paho-mqtt \
+    jsonschema
+
+echo ""
+echo "========================================"
+echo "  所有依赖安装完成！"
+echo "========================================"
+echo ""
+echo "建议执行以下命令验证安装:"
+echo "  ros2 pkg list | grep humble"

install_dep_ddpatrol_x86.sh

@@ -0,0 +1,63 @@
+#!/bin/bash
+
+# 适用于 Jetson 等 ARM64 平台
+
+set -e  # 遇到错误立即退出
+
+echo "========================================"
+echo "  开始安装 CUDA TensorRT"
+echo "========================================"
+
+# ==================== 系统基础工具 ====================
+echo "[1/7] 安装系统基础工具和库..."
+sudo apt-get update
+
+sudo apt-get install -y \
+    v4l-utils \
+    python3-pip \
+    libsamplerate0-dev \
+    libmpg123-dev \
+    portaudio19-dev \
+
+# ==================== ROS Humble 核心包 ====================
+echo "[2/7] 安装 ROS Humble 核心功能包..."
+sudo apt-get install -y \
+    ros-humble-camera-info-manager \
+    ros-humble-image-transport-plugins \
+    ros-humble-ament-cmake-clang-format \
+    ros-humble-rosbridge-suite \
+
+# ==================== Python 依赖 (PyPI) ====================
+echo "[3/7] 安装 Python PyPI 依赖..."
+# 使用清华镜像加速
+pip install -i https://pypi.tuna.tsinghua.edu.cn/simple \
+    'pydantic<2.0' \
+    loguru \
+    paho-mqtt \
+    sounddevice \
+    jsonschema
+
+
+# ==================== PyTorch ====================
+echo "[5/7] 安装 PyTorch (x86 版本)..."
+# 注意：使用 Jetson AI Lab 的 JP6 CUDA 12.6 版本
+pip3 install torch torchvision \
+    --index-url https://download.pytorch.org/whl/cu126
+
+# ==================== 深度学习库 ====================
+echo "[6/7] 安装 Ultralytics (YOLO)..."
+pip install -i https://pypi.tuna.tsinghua.edu.cn/simple ultralytics
+
+# ==================== NumPy 版本固定 ====================
+echo "[7/7] 固定 NumPy 版本..."
+pip install numpy==1.26 -i https://pypi.tuna.tsinghua.edu.cn/simple
+
+echo ""
+echo "========================================"
+echo "  还剩CUDA（12.6） CUDNN TensorRT（10.4.0） 安装！"
+echo "========================================"
+echo ""
+echo "建议执行以下命令验证安装:"
+echo "  ros2 pkg list | grep humble"
+echo "  python3 -c 'import torch; print(torch.__version__)'"
+echo "  python3 -c 'import numpy; print(numpy.__version__)'"

src/autoStart/CMakeLists.txt

@@ -0,0 +1,29 @@
+cmake_minimum_required(VERSION 3.8)
+project(autoStart)
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+# find dependencies
+find_package(ament_cmake REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(std_msgs REQUIRED)
+
+install(DIRECTORY launch
+  DESTINATION share/${PROJECT_NAME}
+)
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  # the following line skips the linter which checks for copyrights
+  # comment the line when a copyright and license is added to all source files
+  set(ament_cmake_copyright_FOUND TRUE)
+  # the following line skips cpplint (only works in a git repo)
+  # comment the line when this package is in a git repo and when
+  # a copyright and license is added to all source files
+  set(ament_cmake_cpplint_FOUND TRUE)
+  ament_lint_auto_find_test_dependencies()
+endif()
+
+ament_package()

src/autoStart/launch/autostart_launch.py

@@ -0,0 +1,265 @@
+import os
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch.actions import IncludeLaunchDescription, DeclareLaunchArgument
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch_ros.actions import Node
+from launch.substitutions import LaunchConfiguration
+
+
+def generate_launch_description():
+    # imu_launch_dir = os.path.join(get_package_share_directory('yesense_std_ros2'), 'launch')
+    # lidar_launch_dir = os.path.join(get_package_share_directory('lslidar_driver'), 'launch')
+    lidar_launch_dir = os.path.join(
+        get_package_share_directory("livox_ros_driver2"), "launch_ROS2"
+    )
+    # rtk_launch_dir = os.path.join(
+    #     get_package_share_directory('beitian_bg620'), 'launch'
+    # )
+    rtk_launch_dir = os.path.join(
+        get_package_share_directory('aqinuo_d300'), 'launch'
+    )
+    lidar_filter_launch_dir = os.path.join(
+        get_package_share_directory("mid_360_filter"), "launch"
+    )
+    pointcloud_2_laserscan_launch_dir = os.path.join(
+        get_package_share_directory("pointcloud_2_laserscan_ros2"), "launch"
+    )
+    hdl_launch_dir = os.path.join(
+        get_package_share_directory('hdl_localization'), 'launch'
+    )
+    nav2_launch_dir = os.path.join(
+        get_package_share_directory("nav2_bringup"), "launch"
+    )
+    dreamdeck_attach_launch_dir = os.path.join(
+        get_package_share_directory("dreamdeck_attach"), "launch"
+    )
+    roi_clustering_launch_dir = os.path.join(
+        get_package_share_directory("roi_clustering"), "launch"
+    )
+    usb_cam_launch_dir = os.path.join(get_package_share_directory("usb_cam"), "launch")
+    apriltag_dock_pose_launch_dir = os.path.join(
+        get_package_share_directory("apriltag_dock_pose"), "launch"
+    )
+    audio_common_launch_dir = os.path.join(
+        get_package_share_directory("audio_common"), "launch"
+    )
+    media_streaming_launch_dir = os.path.join(
+        get_package_share_directory("media_streaming"), "launch"
+    )
+    speech_dialogue_launch_dir = os.path.join(
+        get_package_share_directory("speech_dialogue"), "launch"
+    )
+    screen_alert_launch_dir = os.path.join(
+        get_package_share_directory("screen_alert"), "launch"
+    )
+    dynamic_obstacle_predictor_launch_dir = os.path.join(
+        get_package_share_directory("dynamic_obstacle_predictor"), "launch"
+    )
+
+
+    # imu_cmd = IncludeLaunchDescription(
+    #     PythonLaunchDescriptionSource(
+    #         os.path.join(imu_launch_dir, 'yesense_node.launch.py')))
+
+    # lidar_cmd = IncludeLaunchDescription(
+    #     PythonLaunchDescriptionSource(
+    #         os.path.join(lidar_launch_dir, 'lslidar_c16_launch.py')))
+
+    lidar_cmd = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            os.path.join(lidar_launch_dir, "rviz_MID360_launch.py")
+        )
+    )
+
+    # rtk_cmd = IncludeLaunchDescription(
+    #     PythonLaunchDescriptionSource(
+    #         os.path.join(rtk_launch_dir, "bg620_driver.launch.py")
+    #     )
+    # )
+    rtk_cmd = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            os.path.join(rtk_launch_dir, "d300_driver.launch.py")
+        )
+    )
+
+    lidar_filter_cmd = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            os.path.join(lidar_filter_launch_dir, "mid_360_filter.launch.py")
+        )
+    )
+
+    pointcloud_2_laserscan_cmd = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            os.path.join(
+                pointcloud_2_laserscan_launch_dir, "pointcloud_2_laserscan.launch.py"
+            )
+        )
+    )
+
+    lidar_tf_launch_dir = os.path.join(
+        get_package_share_directory('robot_tf'), 'launch'
+    )
+    lidar_tf_cmd = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            os.path.join(lidar_tf_launch_dir, 'lidar_tf.launch.py')
+        )
+    )
+
+    hdl_localization_cmd = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            os.path.join(hdl_launch_dir, 'dd_localization.launch.py')
+        )
+    )
+
+    bringup_cmd = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            os.path.join(nav2_launch_dir, "navigation2.launch.py")
+        )
+    )
+
+    # rviz_cmd = IncludeLaunchDescription(
+    #     PythonLaunchDescriptionSource(
+    #         os.path.join(nav2_launch_dir, "rviz_launch.py")
+    #     )
+    # )
+    rviz_cmd = Node(
+          package='rviz2',
+          executable='rviz2',
+          arguments=['-d', os.path.join(
+              get_package_share_directory('nav2_bringup'),
+              'rviz', 'nav2_default_view.rviz')],
+          output='screen',
+          parameters=[{'use_sim_time': False}],
+      )
+
+    dreamdeck_attach_cmd = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            os.path.join(dreamdeck_attach_launch_dir, "dreamdeck_attach.launch.py")
+        )
+    )
+
+    roi_clustering_cmd = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            os.path.join(roi_clustering_launch_dir, "roi_clustering_4in1.launch.py")
+        )
+    )
+
+    usb_cam_cmd = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            os.path.join(usb_cam_launch_dir, "camera.launch.py")
+        )
+    )
+
+    apriltag_dock_pose_cmd = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            os.path.join(
+                apriltag_dock_pose_launch_dir, "apriltag_dock_pose_publisher.launch.py"
+            )
+        )
+    )
+
+    audio_common_cmd = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            os.path.join(audio_common_launch_dir, "audio_common.launch.py")
+        )
+    )
+
+    # audio_common_cmd = IncludeLaunchDescription(
+    #     PythonLaunchDescriptionSource(
+    #         os.path.join(audio_common_launch_dir, 'audio_play_sim.launch.py')))
+
+    detection_object_cmd = Node(
+        package="detection_object",
+        executable="vision",
+        name="vision",
+        output="screen",
+        respawn=True,
+        respawn_delay=2.0,
+        # 可以在这里添加额外的参数
+        # parameters=[{'some_parameter': 'some_value'}]
+    )
+
+    # DD追踪动作服务器节点
+    dreamdeck_track_cmd = Node(
+        package="dd_tracking",
+        executable="tracking_action_server",
+        name="dd_tracking_server",
+        output="screen",
+        respawn=True,
+        respawn_delay=2.0,
+        # 可以在这里添加额外的参数
+        # parameters=[{'some_parameter': 'some_value'}]
+    )
+
+    # DD恢复动作服务器节点
+    dreamdeck_recovery_cmd = Node(
+        package="dd_recovery",
+        executable="recovery_action_server",
+        name="dd_recovery_server",
+        output="screen",
+        respawn=True,
+        respawn_delay=2.0,
+        # 可以在这里添加额外的参数
+        # parameters=[{'some_parameter': 'some_value'}]
+    )
+
+    # DD视频流节点
+    media_streaming_cmd = Node(
+        package="media_streaming",
+        executable="media_streaming_node",
+        name="media_streaming_node",
+        output="screen",
+        respawn=True,
+        respawn_delay=2.0,
+        # 可以在这里添加额外的参数
+        # parameters=[{'some_parameter': 'some_value'}]
+    )
+    # DD音频流节点
+    speech_dialogue_cmd = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            os.path.join(speech_dialogue_launch_dir, "speech_dialogue.launch.py")
+        )
+    )
+
+    # DD屏幕告警节点
+    screen_alert_cmd = Node(
+        package="screen_alert",
+        executable="screen_alert_node",
+        name="screen_alert_node",
+        output="screen",
+        respawn=True,
+        respawn_delay=2.0,
+        # 可以在这里添加额外的参数
+        # parameters=[{'some_parameter': 'some_value'}]
+    )
+    dynamic_obstacle_predictor_cmd = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            os.path.join(dynamic_obstacle_predictor_launch_dir, "dynamic_obstacle_predictor.launch.py")
+        )
+    )
+    # Add the actions to launch all of the navigation nodes
+    ld = LaunchDescription()
+    # ld.add_action(imu_cmd)
+    ld.add_action(lidar_cmd)
+    ld.add_action(rtk_cmd)
+    ld.add_action(lidar_filter_cmd)
+    ld.add_action(pointcloud_2_laserscan_cmd)
+    ld.add_action(lidar_tf_cmd)
+    ld.add_action(hdl_localization_cmd)
+    ld.add_action(bringup_cmd)
+    ld.add_action(rviz_cmd)
+    ld.add_action(dreamdeck_attach_cmd)
+    ld.add_action(dreamdeck_track_cmd)
+    ld.add_action(dreamdeck_recovery_cmd)
+    ld.add_action(roi_clustering_cmd)
+    ld.add_action(usb_cam_cmd)
+    ld.add_action(apriltag_dock_pose_cmd)
+    ld.add_action(audio_common_cmd)
+    ld.add_action(detection_object_cmd)
+    ld.add_action(media_streaming_cmd)
+    ld.add_action(speech_dialogue_cmd)
+    ld.add_action(screen_alert_cmd)
+    ld.add_action(dynamic_obstacle_predictor_cmd)
+    
+    return ld

src/autoStart/launch/autostart_launch_calibration.py

@@ -0,0 +1,121 @@
+import os
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch.actions import IncludeLaunchDescription
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch_ros.actions import Node
+
+def generate_launch_description():
+    # imu_launch_dir = os.path.join(get_package_share_directory('yesense_std_ros2'), 'launch')
+    # lidar_launch_dir = os.path.join(get_package_share_directory('lslidar_driver'), 'launch')
+    lidar_launch_dir = os.path.join(get_package_share_directory('livox_ros_driver2'), 'launch_ROS2')
+    lidar_filter_launch_dir = os.path.join(get_package_share_directory('mid_360_filter'), 'launch')
+    pointcloud_2_laserscan_launch_dir = os.path.join(get_package_share_directory('pointcloud_2_laserscan_ros2'), 'launch')
+    hdl_launch_dir = os.path.join(get_package_share_directory('hdl_localization'), 'launch')
+    nav2_launch_dir = os.path.join(get_package_share_directory('nav2_bringup'), 'launch')
+    dreamdeck_attach_launch_dir = os.path.join(get_package_share_directory('dreamdeck_attach'), 'launch')
+    roi_clustering_launch_dir = os.path.join(get_package_share_directory('roi_clustering'), 'launch')
+    usb_cam_launch_dir = os.path.join(get_package_share_directory('usb_cam'), 'launch')
+    apriltag_dock_pose_launch_dir = os.path.join(get_package_share_directory('apriltag_dock_pose'), 'launch')
+    audio_common_launch_dir = os.path.join(get_package_share_directory('audio_common'), 'launch')
+     
+    # imu_cmd = IncludeLaunchDescription(
+    #     PythonLaunchDescriptionSource(
+    #         os.path.join(imu_launch_dir, 'yesense_node.launch.py')))
+
+    # lidar_cmd = IncludeLaunchDescription(
+    #     PythonLaunchDescriptionSource(
+    #         os.path.join(lidar_launch_dir, 'lslidar_c16_launch.py')))
+          
+    lidar_cmd = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            os.path.join(lidar_launch_dir, 'rviz_MID360_launch.py')))
+
+    lidar_filter_cmd = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            os.path.join(lidar_filter_launch_dir, 'mid_360_filter.launch.py')))
+        
+    pointcloud_2_laserscan_cmd = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            os.path.join(pointcloud_2_laserscan_launch_dir, 'pointcloud_2_laserscan.launch.py')))
+
+    hdl_localization_cmd = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            os.path.join(hdl_launch_dir, 'hdl_localization_2.launch.py')))
+
+    bringup_cmd = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            os.path.join(nav2_launch_dir, 'navigation2.launch.py')))
+        
+    dreamdeck_attach_cmd = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            os.path.join(dreamdeck_attach_launch_dir, 'dreamdeck_attach.launch.py')))
+    
+    roi_clustering_cmd = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            os.path.join(roi_clustering_launch_dir, 'roi_clustering_4in1.launch.py')))
+
+    usb_cam_cmd = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            os.path.join(usb_cam_launch_dir, 'camera.launch.py')))
+    
+    apriltag_dock_pose_cmd = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            os.path.join(apriltag_dock_pose_launch_dir, 'apriltag_dock_pose_publisher.launch.py')))
+    
+    audio_common_cmd = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            os.path.join(audio_common_launch_dir, 'audio_common.launch.py')))
+
+    detection_object_cmd = Node(
+        package='detection_object',
+        executable='vision',
+        name='vision',
+        output='screen',
+        respawn=True,
+        respawn_delay=2.0,
+        # 可以在这里添加额外的参数
+        # parameters=[{'some_parameter': 'some_value'}]
+    )
+
+    # DD追踪动作服务器节点
+    dreamdeck_track_cmd = Node(
+        package='dd_tracking',
+        executable='tracking_action_server',
+        name='dd_tracking_server',
+        output='screen',
+        respawn=True,
+        respawn_delay=2.0,
+        # 可以在这里添加额外的参数
+        # parameters=[{'some_parameter': 'some_value'}]
+    )
+
+    # DD恢复动作服务器节点
+    dreamdeck_recovery_cmd = Node(
+        package='dd_recovery',
+        executable='recovery_action_server',
+        name='dd_recovery_server',
+        output='screen',
+        respawn=True,
+        respawn_delay=2.0,
+        # 可以在这里添加额外的参数
+        # parameters=[{'some_parameter': 'some_value'}]
+    )
+
+    # Add the actions to launch all of the navigation nodes
+    ld = LaunchDescription()
+    # ld.add_action(imu_cmd)
+    ld.add_action(lidar_cmd)
+    ld.add_action(lidar_filter_cmd)
+    ld.add_action(pointcloud_2_laserscan_cmd)
+    # ld.add_action(hdl_localization_cmd)
+    # ld.add_action(bringup_cmd)
+    # ld.add_action(dreamdeck_attach_cmd)
+    # ld.add_action(dreamdeck_track_cmd)
+    # ld.add_action(dreamdeck_recovery_cmd)
+    ld.add_action(roi_clustering_cmd)
+    ld.add_action(usb_cam_cmd)
+    # ld.add_action(apriltag_dock_pose_cmd)
+    # ld.add_action(audio_common_cmd)
+    ld.add_action(detection_object_cmd)
+    return ld

src/autoStart/launch/autostart_launch_fusion.py

@@ -0,0 +1,121 @@
+import os
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch.actions import IncludeLaunchDescription
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch_ros.actions import Node
+
+def generate_launch_description():
+    # imu_launch_dir = os.path.join(get_package_share_directory('yesense_std_ros2'), 'launch')
+    # lidar_launch_dir = os.path.join(get_package_share_directory('lslidar_driver'), 'launch')
+    lidar_launch_dir = os.path.join(get_package_share_directory('livox_ros_driver2'), 'launch_ROS2')
+    lidar_filter_launch_dir = os.path.join(get_package_share_directory('mid_360_filter'), 'launch')
+    pointcloud_2_laserscan_launch_dir = os.path.join(get_package_share_directory('pointcloud_2_laserscan_ros2'), 'launch')
+    hdl_launch_dir = os.path.join(get_package_share_directory('hdl_localization'), 'launch')
+    nav2_launch_dir = os.path.join(get_package_share_directory('nav2_bringup'), 'launch')
+    dreamdeck_attach_launch_dir = os.path.join(get_package_share_directory('dreamdeck_attach'), 'launch')
+    roi_clustering_launch_dir = os.path.join(get_package_share_directory('roi_clustering'), 'launch')
+    usb_cam_launch_dir = os.path.join(get_package_share_directory('usb_cam'), 'launch')
+    apriltag_dock_pose_launch_dir = os.path.join(get_package_share_directory('apriltag_dock_pose'), 'launch')
+    audio_common_launch_dir = os.path.join(get_package_share_directory('audio_common'), 'launch')
+     
+    # imu_cmd = IncludeLaunchDescription(
+    #     PythonLaunchDescriptionSource(
+    #         os.path.join(imu_launch_dir, 'yesense_node.launch.py')))
+
+    # lidar_cmd = IncludeLaunchDescription(
+    #     PythonLaunchDescriptionSource(
+    #         os.path.join(lidar_launch_dir, 'lslidar_c16_launch.py')))
+          
+    lidar_cmd = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            os.path.join(lidar_launch_dir, 'rviz_MID360_launch.py')))
+
+    lidar_filter_cmd = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            os.path.join(lidar_filter_launch_dir, 'mid_360_filter.launch.py')))
+        
+    pointcloud_2_laserscan_cmd = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            os.path.join(pointcloud_2_laserscan_launch_dir, 'pointcloud_2_laserscan.launch.py')))
+
+    hdl_localization_cmd = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            os.path.join(hdl_launch_dir, 'hdl_localization_2.launch.py')))
+
+    bringup_cmd = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            os.path.join(nav2_launch_dir, 'navigation2.launch.py')))
+        
+    dreamdeck_attach_cmd = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            os.path.join(dreamdeck_attach_launch_dir, 'dreamdeck_attach.launch.py')))
+    
+    roi_clustering_cmd = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            os.path.join(roi_clustering_launch_dir, 'roi_clustering.launch.py')))
+
+    usb_cam_cmd = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            os.path.join(usb_cam_launch_dir, 'camera.launch.py')))
+    
+    apriltag_dock_pose_cmd = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            os.path.join(apriltag_dock_pose_launch_dir, 'apriltag_dock_pose_publisher.launch.py')))
+    
+    audio_common_cmd = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            os.path.join(audio_common_launch_dir, 'audio_common.launch.py')))
+
+    detection_object_cmd = Node(
+        package='detection_object',
+        executable='vision',
+        name='vision',
+        output='screen',
+        respawn=True,
+        respawn_delay=2.0,
+        # 可以在这里添加额外的参数
+        # parameters=[{'some_parameter': 'some_value'}]
+    )
+
+    # DD追踪动作服务器节点
+    dreamdeck_track_cmd = Node(
+        package='dd_tracking',
+        executable='tracking_action_server',
+        name='dd_tracking_server',
+        output='screen',
+        respawn=True,
+        respawn_delay=2.0,
+        # 可以在这里添加额外的参数
+        # parameters=[{'some_parameter': 'some_value'}]
+    )
+
+    # DD恢复动作服务器节点
+    dreamdeck_recovery_cmd = Node(
+        package='dd_recovery',
+        executable='recovery_action_server',
+        name='dd_recovery_server',
+        output='screen',
+        respawn=True,
+        respawn_delay=2.0,
+        # 可以在这里添加额外的参数
+        # parameters=[{'some_parameter': 'some_value'}]
+    )
+
+    # Add the actions to launch all of the navigation nodes
+    ld = LaunchDescription()
+    # ld.add_action(imu_cmd)
+    ld.add_action(lidar_cmd)
+    ld.add_action(lidar_filter_cmd)
+    ld.add_action(pointcloud_2_laserscan_cmd)
+    # ld.add_action(hdl_localization_cmd)
+    # ld.add_action(bringup_cmd)
+    # ld.add_action(dreamdeck_attach_cmd)
+    # ld.add_action(dreamdeck_track_cmd)
+    # ld.add_action(dreamdeck_recovery_cmd)
+    ld.add_action(roi_clustering_cmd)
+    ld.add_action(usb_cam_cmd)
+    # ld.add_action(apriltag_dock_pose_cmd)
+    # ld.add_action(audio_common_cmd)
+    ld.add_action(detection_object_cmd)
+    return ld

src/autoStart/launch/autostart_launch_old.py

@@ -0,0 +1,211 @@
+import os
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch.actions import IncludeLaunchDescription
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch_ros.actions import Node
+
+
+def generate_launch_description():
+    # imu_launch_dir = os.path.join(get_package_share_directory('yesense_std_ros2'), 'launch')
+    # lidar_launch_dir = os.path.join(get_package_share_directory('lslidar_driver'), 'launch')
+    lidar_launch_dir = os.path.join(
+        get_package_share_directory("livox_ros_driver2"), "launch_ROS2"
+    )
+    lidar_filter_launch_dir = os.path.join(
+        get_package_share_directory("mid_360_filter"), "launch"
+    )
+    pointcloud_2_laserscan_launch_dir = os.path.join(
+        get_package_share_directory("pointcloud_2_laserscan_ros2"), "launch"
+    )
+    hdl_launch_dir = os.path.join(
+        get_package_share_directory("hdl_localization"), "launch"
+    )
+    nav2_launch_dir = os.path.join(
+        get_package_share_directory("nav2_bringup"), "launch"
+    )
+    dreamdeck_attach_launch_dir = os.path.join(
+        get_package_share_directory("dreamdeck_attach"), "launch"
+    )
+    roi_clustering_launch_dir = os.path.join(
+        get_package_share_directory("roi_clustering"), "launch"
+    )
+    usb_cam_launch_dir = os.path.join(get_package_share_directory("usb_cam"), "launch")
+    apriltag_dock_pose_launch_dir = os.path.join(
+        get_package_share_directory("apriltag_dock_pose"), "launch"
+    )
+    audio_common_launch_dir = os.path.join(
+        get_package_share_directory("audio_common"), "launch"
+    )
+    media_streaming_launch_dir = os.path.join(
+        get_package_share_directory("media_streaming"), "launch"
+    )
+    speech_dialogue_launch_dir = os.path.join(
+        get_package_share_directory("speech_dialogue"), "launch"
+    )
+    screen_alert_launch_dir = os.path.join(
+        get_package_share_directory("screen_alert"), "launch"
+    )
+
+    # imu_cmd = IncludeLaunchDescription(
+    #     PythonLaunchDescriptionSource(
+    #         os.path.join(imu_launch_dir, 'yesense_node.launch.py')))
+
+    # lidar_cmd = IncludeLaunchDescription(
+    #     PythonLaunchDescriptionSource(
+    #         os.path.join(lidar_launch_dir, 'lslidar_c16_launch.py')))
+
+    lidar_cmd = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            os.path.join(lidar_launch_dir, "rviz_MID360_launch.py")
+        )
+    )
+
+    lidar_filter_cmd = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            os.path.join(lidar_filter_launch_dir, "mid_360_filter.launch.py")
+        )
+    )
+
+    pointcloud_2_laserscan_cmd = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            os.path.join(
+                pointcloud_2_laserscan_launch_dir, "pointcloud_2_laserscan.launch.py"
+            )
+        )
+    )
+
+    hdl_localization_cmd = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            os.path.join(hdl_launch_dir, "hdl_localization_2.launch.py")
+        )
+    )
+
+    bringup_cmd = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            os.path.join(nav2_launch_dir, "navigation2.launch.py")
+        )
+    )
+
+    dreamdeck_attach_cmd = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            os.path.join(dreamdeck_attach_launch_dir, "dreamdeck_attach.launch.py")
+        )
+    )
+
+    roi_clustering_cmd = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            os.path.join(roi_clustering_launch_dir, "roi_clustering_4in1.launch.py")
+        )
+    )
+
+    usb_cam_cmd = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            os.path.join(usb_cam_launch_dir, "camera.launch.py")
+        )
+    )
+
+    apriltag_dock_pose_cmd = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            os.path.join(
+                apriltag_dock_pose_launch_dir, "apriltag_dock_pose_publisher.launch.py"
+            )
+        )
+    )
+
+    audio_common_cmd = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            os.path.join(audio_common_launch_dir, "audio_common.launch.py")
+        )
+    )
+
+    # audio_common_cmd = IncludeLaunchDescription(
+    #     PythonLaunchDescriptionSource(
+    #         os.path.join(audio_common_launch_dir, 'audio_play_sim.launch.py')))
+
+    detection_object_cmd = Node(
+        package="detection_object",
+        executable="vision",
+        name="vision",
+        output="screen",
+        respawn=True,
+        respawn_delay=2.0,
+        # 可以在这里添加额外的参数
+        # parameters=[{'some_parameter': 'some_value'}]
+    )
+
+    # DD追踪动作服务器节点
+    dreamdeck_track_cmd = Node(
+        package="dd_tracking",
+        executable="tracking_action_server",
+        name="dd_tracking_server",
+        output="screen",
+        respawn=True,
+        respawn_delay=2.0,
+        # 可以在这里添加额外的参数
+        # parameters=[{'some_parameter': 'some_value'}]
+    )
+
+    # DD恢复动作服务器节点
+    dreamdeck_recovery_cmd = Node(
+        package="dd_recovery",
+        executable="recovery_action_server",
+        name="dd_recovery_server",
+        output="screen",
+        respawn=True,
+        respawn_delay=2.0,
+        # 可以在这里添加额外的参数
+        # parameters=[{'some_parameter': 'some_value'}]
+    )
+
+    # DD视频流节点
+    media_streaming_cmd = Node(
+        package="media_streaming",
+        executable="media_streaming_node",
+        name="media_streaming_node",
+        output="screen",
+        respawn=True,
+        respawn_delay=2.0,
+        # 可以在这里添加额外的参数
+        # parameters=[{'some_parameter': 'some_value'}]
+    )
+    # DD音频流节点
+    speech_dialogue_cmd = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            os.path.join(speech_dialogue_launch_dir, "speech_dialogue.launch.py")
+        )
+    )
+
+    # DD屏幕告警节点
+    screen_alert_cmd = Node(
+        package="screen_alert",
+        executable="screen_alert_node",
+        name="screen_alert_node",
+        output="screen",
+        respawn=True,
+        respawn_delay=2.0,
+        # 可以在这里添加额外的参数
+        # parameters=[{'some_parameter': 'some_value'}]
+    )
+
+    # Add the actions to launch all of the navigation nodes
+    ld = LaunchDescription()
+    # ld.add_action(imu_cmd)
+    ld.add_action(lidar_cmd)
+    ld.add_action(lidar_filter_cmd)
+    ld.add_action(pointcloud_2_laserscan_cmd)
+    ld.add_action(hdl_localization_cmd)
+    ld.add_action(bringup_cmd)
+    ld.add_action(dreamdeck_attach_cmd)
+    ld.add_action(dreamdeck_track_cmd)
+    ld.add_action(dreamdeck_recovery_cmd)
+    ld.add_action(roi_clustering_cmd)
+    ld.add_action(usb_cam_cmd)
+    ld.add_action(apriltag_dock_pose_cmd)
+    ld.add_action(audio_common_cmd)
+    ld.add_action(detection_object_cmd)
+    ld.add_action(media_streaming_cmd)
+    ld.add_action(speech_dialogue_cmd)
+    ld.add_action(screen_alert_cmd)
+
+    return ld

src/autoStart/launch/sim_autostart_launch.py

@@ -0,0 +1,128 @@
+import os
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch.actions import IncludeLaunchDescription
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch_ros.actions import Node
+
+def generate_launch_description():
+    lidar_launch_dir = os.path.join(get_package_share_directory('lidar_sim'), 'launch')
+    velpose_sim_launch_dir = os.path.join(get_package_share_directory('velpose_sim'), 'launch')
+    lidar_filter_launch_dir = os.path.join(get_package_share_directory('mid_360_filter'), 'launch')
+    pointcloud_2_laserscan_launch_dir = os.path.join(get_package_share_directory('pointcloud_2_laserscan_ros2'), 'launch')
+    nav2_launch_dir = os.path.join(get_package_share_directory('nav2_bringup'), 'launch')
+    nav2_bringup_dir = get_package_share_directory('nav2_bringup')
+    dreamdeck_attach_launch_dir = os.path.join(get_package_share_directory('dreamdeck_attach'), 'launch')
+    roi_clustering_launch_dir = os.path.join(get_package_share_directory('roi_clustering'), 'launch')
+    usb_cam_launch_dir = os.path.join(get_package_share_directory('usb_cam'), 'launch')
+    apriltag_dock_pose_launch_dir = os.path.join(get_package_share_directory('apriltag_dock_pose'), 'launch')
+    # audio_common_launch_dir = os.path.join(get_package_share_directory('audio_common'), 'launch')
+    audio_common_launch_dir = os.path.join(get_package_share_directory('audio_play_sim'), 'launch')
+
+    lidar_tf = Node(
+        name='lidar_tf',
+        package='tf2_ros',
+        executable='static_transform_publisher',
+        arguments=['-0.15', '0.0', '0.5', '0', '0.1045',
+                   '0', '0.9945', 'base_footprint', 'livox_frame']
+    )
+
+    lidar_sim_cmd = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            os.path.join(lidar_launch_dir, 'lidar_sim.launch.py')))
+
+    velpose_sim_cmd = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            os.path.join(velpose_sim_launch_dir, 'velpose_sim.launch.py')))
+        
+    lidar_filter_cmd = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            os.path.join(lidar_filter_launch_dir, 'mid_360_filter.launch.py')))
+    
+    pointcloud_2_laserscan_cmd = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            os.path.join(pointcloud_2_laserscan_launch_dir, 'pointcloud_2_laserscan.launch.py')))
+
+    # 设置默认地图文件路径
+    default_map_file = os.path.join(
+        nav2_bringup_dir, 'maps', 'yuanqu_20250519_refined.yaml')
+
+    bringup_cmd = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            os.path.join(nav2_launch_dir, 'navigation2.launch.py')),
+        launch_arguments={
+            'map': default_map_file,
+            'use_sim_time': 'False',
+        }.items())
+
+    dreamdeck_attach_cmd = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            os.path.join(dreamdeck_attach_launch_dir, 'dreamdeck_attach.launch.py')))
+    
+    roi_clustering_cmd = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            os.path.join(roi_clustering_launch_dir, 'roi_clustering_4in1.launch.py')))
+
+    usb_cam_cmd = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            os.path.join(usb_cam_launch_dir, 'camera.launch.py')))
+    
+    apriltag_dock_pose_cmd = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            os.path.join(apriltag_dock_pose_launch_dir, 'apriltag_dock_pose_publisher.launch.py')))
+
+    # audio_common_cmd = IncludeLaunchDescription(
+    #     PythonLaunchDescriptionSource(
+    #         os.path.join(audio_common_launch_dir, 'audio_common.launch.py')))
+
+    audio_common_cmd = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            os.path.join(audio_common_launch_dir, 'audio_play_sim.launch.py')))
+
+
+    detection_object_cmd = Node(
+        package='detection_object',
+        executable='vision',
+        name='vision',
+        output='screen',
+        # 可以在这里添加额外的参数
+        # parameters=[{'some_parameter': 'some_value'}]
+    )
+
+    # DD追踪动作服务器节点
+    dreamdeck_track_cmd = Node(
+        package='dd_tracking',
+        executable='tracking_action_server',
+        name='dd_tracking_server',
+        output='screen',
+        # 可以在这里添加额外的参数
+        # parameters=[{'some_parameter': 'some_value'}]
+    )
+
+    # DD恢复动作服务器节点
+    dreamdeck_recovery_cmd = Node(
+        package='dd_recovery',
+        executable='recovery_action_server',
+        name='dd_recovery_server',
+        output='screen',
+        # 可以在这里添加额外的参数
+        # parameters=[{'some_parameter': 'some_value'}]
+    )
+
+    # Add the actions to launch all of the navigation nodes
+    ld = LaunchDescription()
+    ld.add_action(lidar_sim_cmd)
+    ld.add_action(lidar_tf)
+    ld.add_action(lidar_filter_cmd)
+    ld.add_action(velpose_sim_cmd)
+    ld.add_action(pointcloud_2_laserscan_cmd)
+    ld.add_action(bringup_cmd)
+    ld.add_action(dreamdeck_attach_cmd)
+    ld.add_action(dreamdeck_track_cmd)
+    ld.add_action(dreamdeck_recovery_cmd)
+    # ld.add_action(roi_clustering_cmd)
+    # ld.add_action(usb_cam_cmd)
+    # ld.add_action(apriltag_dock_pose_cmd)
+    ld.add_action(audio_common_cmd)
+    ld.add_action(detection_object_cmd)
+    return ld

src/autoStart/package.xml

@@ -0,0 +1,21 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>autoStart</name>
+  <version>0.0.0</version>
+  <description>TODO: Package description</description>
+  <maintainer email="monkey17336462890@163.com">dreamdeck</maintainer>
+  <license>TODO: License declaration</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <depend>rclcpp</depend>
+  <depend>std_msgs</depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

src/common/common_launch/CMakeLists.txt

@@ -0,0 +1,19 @@
+cmake_minimum_required(VERSION 3.8)
+project(common_launch)
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+find_package(ament_cmake REQUIRED)
+
+install(DIRECTORY launch
+  DESTINATION share/${PROJECT_NAME}
+)
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  ament_lint_auto_find_test_dependencies()
+endif()
+
+ament_package()

src/common/common_launch/launch/bag_record.launch.py

@@ -0,0 +1,69 @@
+import datetime
+import os
+
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument, ExecuteProcess, OpaqueFunction
+from launch.substitutions import LaunchConfiguration
+
+
+def create_recorder_action(context):
+    if LaunchConfiguration('record').perform(context) != 'true':
+        return []
+
+    topics_str = LaunchConfiguration('topics').perform(context)
+    topics = [topic.strip() for topic in topics_str.split(',') if topic.strip()]
+
+    bag_dir = LaunchConfiguration('bag_dir').perform(context)
+    if not bag_dir:
+        bag_dir = os.path.join(os.path.expanduser('~'), 'Downloads/dd_bags')
+    os.makedirs(bag_dir, exist_ok=True)
+
+    bag_prefix = LaunchConfiguration('bag_prefix').perform(context)
+    if not bag_prefix:
+        bag_prefix = 'data'
+
+    current_timestamp = datetime.datetime.now().strftime('%Y%m%d_%H%M%S')
+    bag_name = os.path.join(bag_dir, f'{bag_prefix}_{current_timestamp}')
+
+    cmd = [
+        'ros2',
+        'bag',
+        'record',
+        '-o',
+        bag_name,
+        '--storage',
+        'sqlite3',
+        '--max-cache-size',
+        str(100 * 1024 * 1024),
+        '--max-bag-size',
+        str(1024 * 1024 * 1024),
+        '--max-bag-duration',
+        '3600',
+    ]
+    cmd.extend(topics)
+
+    return [ExecuteProcess(cmd=cmd, name='rosbag2_recorder', output='screen')]
+
+
+def generate_launch_description():
+    return LaunchDescription(
+        [
+            DeclareLaunchArgument('record', default_value='true'),
+            DeclareLaunchArgument(
+                'topics',
+                default_value='/c16/lslidar_point_cloud,/imu/data_raw,/rtk/gpsOdom,/localization_pose',
+                description='split by comma',
+            ),
+            DeclareLaunchArgument(
+                'bag_dir',
+                default_value='',
+                description='Directory to save rosbag files',
+            ),
+            DeclareLaunchArgument(
+                'bag_prefix',
+                default_value='',
+                description='Prefix of the bag file',
+            ),
+            OpaqueFunction(function=create_recorder_action),
+        ]
+    )

src/common/common_launch/launch/bag_record_for_auto_drive.launch.py

@@ -0,0 +1,35 @@
+import os
+
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch.actions import IncludeLaunchDescription, DeclareLaunchArgument
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch.substitutions import LaunchConfiguration
+
+
+def generate_launch_description():
+    record_arg = DeclareLaunchArgument('record', default_value='true')
+    common_launch_dir = get_package_share_directory('common_launch')
+    topic_list = [
+        '/c16/lslidar_point_cloud',
+        '/imu/data_raw',
+        '/livox/lidar',
+        '/livox/imu',
+        '/rtk/gpsOdom',
+        '/rtk/gpsFix',
+        '/localization_pose',
+    ]
+    topics = ','.join(topic_list)
+    override_param = {
+        'record': LaunchConfiguration('record'),
+        'topics': topics,
+        'bag_dir': os.path.join(os.path.expanduser('~'), 'Downloads/dd_bags'),
+        'bag_prefix': 'ad',
+    }
+    record_cmd = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            os.path.join(common_launch_dir, 'launch/bag_record.launch.py')
+        ),
+        launch_arguments=override_param.items(),
+    )
+    return LaunchDescription([record_arg, record_cmd])

src/common/common_launch/launch/bag_record_for_localization_pose.launch.py

@@ -0,0 +1,34 @@
+import os
+
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch.actions import IncludeLaunchDescription, DeclareLaunchArgument
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch.substitutions import LaunchConfiguration
+
+
+def generate_launch_description():
+    record_arg = DeclareLaunchArgument('record', default_value='true')
+    common_launch_dir = get_package_share_directory('common_launch')
+    topic_list = [
+        '/localization_pose',
+        '/predicted_pose',
+        '/smoothed_predicted_pose',
+        '/rtk/gpsOdom',
+        '/tf',
+        '/tf_static',
+    ]
+    topics = ','.join(topic_list)
+    override_param = {
+        'record': LaunchConfiguration('record'),
+        'topics': topics,
+        'bag_dir': os.path.join(os.path.expanduser('~'), 'Downloads/dd_bags'),
+        'bag_prefix': 'localization',
+    }
+    record_cmd = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            os.path.join(common_launch_dir, 'launch/bag_record.launch.py')
+        ),
+        launch_arguments=override_param.items(),
+    )
+    return LaunchDescription([record_arg, record_cmd])

src/common/common_launch/launch/bag_record_for_map.launch.py

@@ -0,0 +1,34 @@
+import os
+
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch.actions import IncludeLaunchDescription, DeclareLaunchArgument
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch.substitutions import LaunchConfiguration
+
+
+def generate_launch_description():
+    record_arg = DeclareLaunchArgument('record', default_value='true')
+    common_launch_dir = get_package_share_directory('common_launch')
+    topic_list = [
+        '/c16/lslidar_point_cloud',
+        '/imu/data_raw',
+        '/livox/lidar',
+        '/livox/imu',
+        '/rtk/gpsOdom',
+        '/rtk/gpsFix',
+    ]
+    topics = ','.join(topic_list)
+    override_param = {
+        'record': LaunchConfiguration('record'),
+        'topics': topics,
+        'bag_dir': os.path.join(os.path.expanduser('~'), 'Downloads/dd_bags'),
+        'bag_prefix': 'map',
+    }
+    record_cmd = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            os.path.join(common_launch_dir, 'launch/bag_record.launch.py')
+        ),
+        launch_arguments=override_param.items(),
+    )
+    return LaunchDescription([record_arg, record_cmd])

src/common/common_launch/package.xml

@@ -0,0 +1,15 @@
+<?xml version="1.0"?>
+<package format="3">
+  <name>common_launch</name>
+  <version>0.0.0</version>
+  <description>The common_launch package</description>
+  <maintainer email="dreamdeck@todo.todo">admin</maintainer>
+  <license>TODO</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+  <build_depend>ament_cmake</build_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

src/common/file_util/CMakeLists.txt

@@ -0,0 +1,42 @@
+cmake_minimum_required(VERSION 3.8)
+project(file_util)
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+  set(CMAKE_CXX_STANDARD 17)
+  set(CMAKE_CXX_STANDARD_REQUIRED ON)
+endif()
+
+find_package(ament_cmake REQUIRED)
+
+include_directories(
+  include
+)
+
+add_library(${PROJECT_NAME} SHARED
+  src/file.cpp
+)
+
+target_include_directories(${PROJECT_NAME} PUBLIC
+  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+  $<INSTALL_INTERFACE:include>
+)
+
+install(TARGETS ${PROJECT_NAME}
+  EXPORT export_${PROJECT_NAME}
+  ARCHIVE DESTINATION lib
+  LIBRARY DESTINATION lib
+  RUNTIME DESTINATION bin
+)
+
+install(DIRECTORY include/ DESTINATION include)
+
+ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET)
+ament_export_include_directories(include)
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  ament_lint_auto_find_test_dependencies()
+endif()
+
+ament_package()

src/common/file_util/include/file_util/file.h

@@ -0,0 +1,32 @@
+#pragma once
+
+#include <filesystem>
+#include <string>
+#include <vector>
+
+namespace file_util {
+
+bool FileExists(const std::string& file_path);
+bool DirExists(const std::string& dir_path);
+bool PathExists(const std::string& path);
+bool CreateDir(const std::string& dir_path);
+bool CreateDirRecursive(const std::string& dir_path);
+std::string NormalizePath(const std::string& path);
+std::string JoinPath(const std::string& base_path, const std::string& component);
+bool IsAbsolutePath(const std::string& path);
+bool IsRelativePath(const std::string& path);
+std::vector<std::string> ListFiles(const std::string& dir_path);
+std::vector<std::string> ListDirs(const std::string& dir_path);
+std::vector<std::string> ListEntries(const std::string& dir_path);
+
+std::string GetParentDir(const std::string& path);
+std::string GetFilename(const std::string& path);
+std::string GetFilenameWithoutExtension(const std::string& path);
+std::string GetExtension(const std::string& path);
+
+bool RemoveFile(const std::string& file_path);
+bool RemoveDir(const std::string& dir_path);
+bool RemoveDirRecursive(const std::string& dir_path);
+bool CopyFile(const std::string& src_path, const std::string& dst_path);
+
+}  // namespace file_util

src/common/file_util/package.xml

@@ -0,0 +1,15 @@
+<?xml version="1.0"?>
+<package format="3">
+  <name>file_util</name>
+  <version>0.0.0</version>
+  <description>The file_util package</description>
+  <maintainer email="dreamdeck@todo.todo">admin</maintainer>
+  <license>TODO</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+  <build_depend>ament_cmake</build_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

src/common/file_util/src/file.cpp

@@ -0,0 +1,113 @@
+#include <file_util/file.h>
+
+namespace file_util {
+
+bool FileExists(const std::string& file_path) {
+  return std::filesystem::exists(file_path) && std::filesystem::is_regular_file(file_path);
+}
+
+bool DirExists(const std::string& dir_path) {
+  return std::filesystem::exists(dir_path) && std::filesystem::is_directory(dir_path);
+}
+
+bool PathExists(const std::string& path) { return std::filesystem::exists(path); }
+
+bool CreateDir(const std::string& dir_path) {
+  if (DirExists(dir_path)) {
+    return true;
+  }
+  return std::filesystem::create_directory(dir_path);
+}
+
+bool CreateDirRecursive(const std::string& dir_path) {
+  if (DirExists(dir_path)) {
+    return true;
+  }
+  return std::filesystem::create_directories(dir_path);
+}
+
+std::string NormalizePath(const std::string& path) {
+  return std::filesystem::path(path).lexically_normal().string();
+}
+
+std::string JoinPath(const std::string& base_path, const std::string& component) {
+  return (std::filesystem::path(base_path) / component).string();
+}
+
+bool IsAbsolutePath(const std::string& path) { return std::filesystem::path(path).is_absolute(); }
+
+bool IsRelativePath(const std::string& path) { return std::filesystem::path(path).is_relative(); }
+
+std::vector<std::string> ListFiles(const std::string& dir_path) {
+  std::vector<std::string> files;
+  for (const auto& entry : std::filesystem::directory_iterator(dir_path)) {
+    if (entry.is_regular_file()) {
+      files.push_back(entry.path().string());
+    }
+  }
+  return files;
+}
+
+std::vector<std::string> ListDirs(const std::string& dir_path) {
+  std::vector<std::string> dirs;
+  for (const auto& entry : std::filesystem::directory_iterator(dir_path)) {
+    if (entry.is_directory()) {
+      dirs.push_back(entry.path().string());
+    }
+  }
+  return dirs;
+}
+
+std::vector<std::string> ListEntries(const std::string& dir_path) {
+  std::vector<std::string> entries;
+  for (const auto& entry : std::filesystem::directory_iterator(dir_path)) {
+    entries.push_back(entry.path().string());
+  }
+  return entries;
+}
+
+std::string GetParentDir(const std::string& path) {
+  return std::filesystem::path(path).parent_path().string();
+}
+
+std::string GetFilename(const std::string& path) {
+  return std::filesystem::path(path).filename().string();
+}
+
+std::string GetFilenameWithoutExtension(const std::string& path) {
+  return std::filesystem::path(path).stem().string();
+}
+
+std::string GetExtension(const std::string& path) {
+  return std::filesystem::path(path).extension().string();
+}
+
+bool RemoveFile(const std::string& file_path) {
+  if (!FileExists(file_path)) {
+    return true;
+  }
+  return std::filesystem::remove(file_path);
+}
+
+bool RemoveDir(const std::string& dir_path) {
+  if (!DirExists(dir_path)) {
+    return true;
+  }
+  return std::filesystem::remove(dir_path);
+}
+
+bool RemoveDirRecursive(const std::string& dir_path) {
+  if (!DirExists(dir_path)) {
+    return true;
+  }
+  return std::filesystem::remove_all(dir_path) > 0;
+}
+
+bool CopyFile(const std::string& src_path, const std::string& dst_path) {
+  if (!FileExists(src_path)) {
+    return false;
+  }
+  return std::filesystem::copy_file(src_path, dst_path);
+}
+
+}  // namespace file_util

src/common/gps_util/CMakeLists.txt

@@ -0,0 +1,21 @@
+cmake_minimum_required(VERSION 3.8)
+project(gps_util)
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+find_package(ament_cmake REQUIRED)
+include_directories(
+  include
+)
+
+install(DIRECTORY include/ DESTINATION include)
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  ament_lint_auto_find_test_dependencies()
+endif()
+
+ament_export_include_directories(include)
+ament_package()

src/common/gps_util/include/gps_util/gps_tools.hpp

@@ -0,0 +1,133 @@
+#pragma once
+
+#include <iostream>
+
+#include <Eigen/Core>
+#include <boost/foreach.hpp>
+#include <sensor_msgs/msg/nav_sat_fix.hpp>
+
+#define DEG_TO_RAD 0.01745329252
+#define EARTH_MAJOR 6378137.0         ///< WGS84 MAJOR AXIS
+#define EARTH_MINOR 6356752.31424518  ///< WGS84 MINOR AXIS
+
+namespace gps_util {
+
+class GpsTools {
+ public:
+  GpsTools() { lla_origin_.setIdentity(); };
+
+  // Eigen::Vector3d LLA2ECEF(const Eigen::Vector3d &lla);
+  //  Eigen::Vector3d ECEF2LLA(const Eigen::Vector3d &ecef);
+  //  Eigen::Vector3d ECEF2ENU(const Eigen::Vector3d &ecef);
+  //  Eigen::Vector3d ENU2ECEF(const Eigen::Vector3d &enu);
+  // static Eigen::Vector3d GpsMsg2Eigen(const sensor_msgs::NavSatFix &gps_msgs);
+  // void updateGPSpose(const sensor_msgs::NavSatFix &gps_msgs);
+
+  /**
+   * ros msg to eigen
+   * @param gps_msgs
+   * @return
+   */
+  static Eigen::Vector3d GpsMsg2Eigen(const sensor_msgs::msg::NavSatFix& gps_msgs) {
+    Eigen::Vector3d lla(gps_msgs.latitude, gps_msgs.longitude, gps_msgs.altitude);
+    return lla;
+  }
+
+  /**
+   *  //2. LLA经度(longitude),纬度(latitude)和高度(altitude)经纬高坐标系 转(Earth-Centered,
+   * Earth-Fixed)
+   *  Z轴指向指向北，但不完全精确地与地球转动轴重合。转动轴有微小“摆动”，称之为“极运动(polar
+   * motion)” X轴在球面上与格林威治线和赤道的交点
+   * @param lla
+   * @return
+   */
+  Eigen::Vector3d LLA2ECEF(const Eigen::Vector3d& lla) {
+    Eigen::Vector3d ecef;
+    double lat = deg2rad(lla.x());
+    double lon = deg2rad(lla.y());
+    double alt = lla.z();
+    double earth_r =
+        pow(EARTH_MAJOR, 2) / sqrt(pow(EARTH_MAJOR * cos(lat), 2) + pow(EARTH_MINOR * sin(lat), 2));
+    ecef.x() = (earth_r + alt) * cos(lat) * cos(lon);
+    ecef.y() = (earth_r + alt) * cos(lat) * sin(lon);
+    ecef.z() = (pow(EARTH_MINOR / EARTH_MAJOR, 2) * earth_r + alt) * sin(lat);
+
+    return ecef;
+  }
+
+  Eigen::Vector3d ECEF2LLA(const Eigen::Vector3d& ecef) {
+    double e = sqrt((pow(EARTH_MAJOR, 2) - pow(EARTH_MINOR, 2)) / pow(EARTH_MAJOR, 2));
+    double e_ = sqrt((pow(EARTH_MAJOR, 2) - pow(EARTH_MINOR, 2)) / pow(EARTH_MINOR, 2));
+    double p = sqrt(pow(ecef.x(), 2) + pow(ecef.y(), 2));
+    double theta = atan2(ecef.z() * EARTH_MAJOR, p * EARTH_MINOR);
+
+    double lon = atan2(ecef.y(), ecef.x());
+    double lat = atan2((ecef.z() + pow(e_, 2) * EARTH_MINOR * pow(sin(theta), 3)),
+                       p - pow(e, 2) * EARTH_MAJOR * pow(cos(theta), 3));
+    double earth_r =
+        pow(EARTH_MAJOR, 2) / sqrt(pow(EARTH_MAJOR * cos(lat), 2) + pow(EARTH_MINOR * sin(lat), 2));
+    double alt = p / cos(lat) - earth_r;
+    Eigen::Vector3d lla(rad2deg(lat), rad2deg(lon), alt);
+    return lla;
+  }
+
+  Eigen::Vector3d ECEF2ENU(const Eigen::Vector3d& ecef) {
+    double lat = deg2rad(lla_origin_.x());
+    double lon = deg2rad(lla_origin_.y());
+
+    Eigen::Vector3d t = -LLA2ECEF(lla_origin_);
+    Eigen::Matrix3d r;
+    r << -sin(lon), cos(lon), 0, -cos(lon) * sin(lat), -sin(lat) * sin(lon), cos(lat),
+        cos(lon) * cos(lat), sin(lon) * cos(lat), sin(lat);
+
+    Eigen::Vector3d enu;
+    enu = ecef + t;
+    enu = r * enu;
+    return enu;
+  }
+
+  Eigen::Vector3d ENU2ECEF(const Eigen::Vector3d& enu) {
+    double lat = deg2rad(lla_origin_.x());
+    double lon = deg2rad(lla_origin_.y());
+
+    Eigen::Vector3d t = LLA2ECEF(lla_origin_);
+    Eigen::Matrix3d r;
+    r << -sin(lon), -cos(lon) * sin(lat), cos(lon) * cos(lat), cos(lon), -sin(lon) * sin(lat),
+        sin(lon) * cos(lat), 0, cos(lat), sin(lat);
+    Eigen::Vector3d ecef;
+    ecef = r * enu + t;
+    return ecef;
+  }
+
+  void updateGPSpose(const sensor_msgs::msg::NavSatFix& gps_msgs) {
+    // 检查状态4
+    if (gps_msgs.status.status == 4 || gps_msgs.status.status == 5 || gps_msgs.status.status == 1 ||
+        gps_msgs.status.status == 2) {
+      // 第一个的时候设置为起点
+      if (lla_origin_ == Eigen::Vector3d::Identity()) {
+        Eigen::Vector3d lla = GpsMsg2Eigen(gps_msgs);
+        lla_origin_ = lla;
+        std::cout << "GPS origin: " << lla_origin_ << "\n status: " << gps_msgs.status.status
+                  << std::endl;
+      } else {
+        Eigen::Vector3d lla = GpsMsg2Eigen(gps_msgs);
+        Eigen::Vector3d ecef = LLA2ECEF(lla);
+        Eigen::Vector3d enu = ECEF2ENU(ecef);
+        gps_pos_ = enu;
+        std::cout << "GPS lla_origin_: " << lla_origin_ << "\n curr" << gps_pos_ << std::endl;
+      }
+    }
+  }
+
+  // 变量部分
+  // 1.lla的起点
+  Eigen::Vector3d lla_origin_;
+  // 2.enu下的坐标
+  Eigen::Vector3d gps_pos_;
+
+ private:
+  static inline double deg2rad(const double& deg) { return deg * DEG_TO_RAD; };
+  static inline double rad2deg(const double& rad) { return rad / DEG_TO_RAD; }
+};
+
+}  // namespace gps_util

src/common/gps_util/package.xml

@@ -0,0 +1,15 @@
+<?xml version="1.0"?>
+<package format="3">
+  <name>gps_util</name>
+  <version>0.0.0</version>
+  <description>The gps_util package</description>
+  <maintainer email="dreamdeck@todo.todo">admin</maintainer>
+  <license>TODO</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+  <build_depend>ament_cmake</build_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

src/common/log_util/CMakeLists.txt

@@ -0,0 +1,34 @@
+cmake_minimum_required(VERSION 3.8)
+project(log_util)
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+find_package(ament_cmake REQUIRED)
+find_package(rclcpp REQUIRED)
+include_directories(
+  include
+)
+
+install(DIRECTORY include/ DESTINATION include)
+
+add_executable(log_throttle_main
+  src/log_throttle_main.cpp
+)
+ament_target_dependencies(log_throttle_main
+  rclcpp
+)
+
+install(TARGETS
+  log_throttle_main
+  DESTINATION lib/${PROJECT_NAME}
+)
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  ament_lint_auto_find_test_dependencies()
+endif()
+
+ament_export_include_directories(include)
+ament_package()

src/common/log_util/include/log_util/log_throttle.hpp

@@ -0,0 +1,54 @@
+#pragma once
+
+#include <chrono>
+#include <mutex>
+#include <unordered_map>
+
+#include <rclcpp/rclcpp.hpp>
+
+struct LogThrottle {
+  using Clock = std::chrono::steady_clock;
+  using TimePoint = Clock::time_point;
+
+  static inline std::unordered_map<std::string, TimePoint> global_log_map;
+  static inline std::mutex global_mutex;
+
+  template <typename... Args>
+  static void Info(const char* id, rclcpp::Logger logger, double interval_sec, const char* format,
+                   Args&&... args) {
+    std::lock_guard<std::mutex> lock(global_mutex);
+    auto now = Clock::now();
+    auto& last = global_log_map[id];
+
+    if (now - last >= std::chrono::seconds((int)interval_sec)) {
+      RCLCPP_INFO(logger, format, std::forward<Args>(args)...);
+      last = now;
+    }
+  }
+
+  template <typename... Args>
+  static void Warn(const char* id, rclcpp::Logger logger, double interval_sec, const char* format,
+                   Args&&... args) {
+    std::lock_guard<std::mutex> lock(global_mutex);
+    auto now = Clock::now();
+    auto& last = global_log_map[id];
+
+    if (now - last >= std::chrono::seconds((int)interval_sec)) {
+      RCLCPP_WARN(logger, format, std::forward<Args>(args)...);
+      last = now;
+    }
+  }
+
+  template <typename... Args>
+  static void Error(const char* id, rclcpp::Logger logger, double interval_sec, const char* format,
+                    Args&&... args) {
+    std::lock_guard<std::mutex> lock(global_mutex);
+    auto now = Clock::now();
+    auto& last = global_log_map[id];
+
+    if (now - last >= std::chrono::seconds((int)interval_sec)) {
+      RCLCPP_ERROR(logger, format, std::forward<Args>(args)...);
+      last = now;
+    }
+  }
+};

src/common/log_util/package.xml

@@ -0,0 +1,16 @@
+<?xml version="1.0"?>
+<package format="3">
+  <name>log_util</name>
+  <version>0.0.0</version>
+  <description>The log_util package</description>
+  <maintainer email="dreamdeck@todo.todo">admin</maintainer>
+  <license>TODO</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+  <build_depend>ament_cmake</build_depend>
+  <depend>rclcpp</depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

src/common/log_util/src/log_throttle_main.cpp

@@ -0,0 +1,41 @@
+#include <rclcpp/rclcpp.hpp>
+
+#include "log_util/log_throttle.hpp"
+
+class LogThrottleNode : public rclcpp::Node {
+ public:
+  LogThrottleNode() : Node("log_throttle_node") {
+    timer1_ = this->create_wall_timer(std::chrono::milliseconds(100),
+                                      std::bind(&LogThrottleNode::timer1_callback, this));
+    timer2_ = this->create_wall_timer(std::chrono::milliseconds(100),
+                                      std::bind(&LogThrottleNode::timer2_callback, this));
+    timer3_ = this->create_wall_timer(std::chrono::milliseconds(100),
+                                      std::bind(&LogThrottleNode::timer3_callback, this));
+    RCLCPP_INFO(get_logger(), "✅ 三个定时器启动，测试多线程节流");
+  }
+
+ private:
+  void timer1_callback() {
+    LogThrottle::Info("timer1", get_logger(), 1.0, "[TIMER 1] [INFO] 节流测试");
+  }
+
+  void timer2_callback() {
+    LogThrottle::Warn("timer2", get_logger(), 2.0, "[TIMER 2] [WARN] 节流测试");
+  }
+
+  void timer3_callback() {
+    LogThrottle::Error("timer3", get_logger(), 3.0, "[TIMER 3] [ERROR] 节流测试");
+  }
+
+  rclcpp::TimerBase::SharedPtr timer1_;
+  rclcpp::TimerBase::SharedPtr timer2_;
+  rclcpp::TimerBase::SharedPtr timer3_;
+};
+
+int main(int argc, char** argv) {
+  rclcpp::init(argc, argv);
+  auto node = std::make_shared<LogThrottleNode>();
+  rclcpp::spin(node);
+  rclcpp::shutdown();
+  return 0;
+}

src/common/pcl_util/CMakeLists.txt

@@ -0,0 +1,26 @@
+cmake_minimum_required(VERSION 3.8)
+project(pcl_util)
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+find_package(ament_cmake REQUIRED)
+find_package(PCL REQUIRED QUIET)
+find_package(Eigen3 REQUIRED)
+include_directories(
+  include
+  ${EIGEN3_INCLUDE_DIRS}
+  ${PCL_INCLUDE_DIRS}
+)
+
+install(DIRECTORY include/ DESTINATION include)
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  ament_lint_auto_find_test_dependencies()
+endif()
+
+ament_export_include_directories(include)
+# ament_export_libraries(${PROJECT_NAME})
+ament_package()

src/common/pcl_util/include/pcl_util/custom_point_type.h

@@ -0,0 +1,113 @@
+#pragma once
+
+#include <Eigen/Core>
+#include <pcl/point_cloud.h>
+#include <pcl/point_traits.h>
+#include <pcl/point_types.h>
+
+namespace pcl_util {
+
+struct VelodynePointXYZIRT {
+  PCL_ADD_POINT4D
+  PCL_ADD_INTENSITY;
+  uint16_t ring;
+  double time;
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+} EIGEN_ALIGN16;
+
+struct LeishenPointXYZIRT {
+  PCL_ADD_POINT4D
+  PCL_ADD_INTENSITY;
+  uint16_t ring;
+  float time;
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+} EIGEN_ALIGN16;
+
+struct PandarPointXYZIRT {
+  PCL_ADD_POINT4D
+  float intensity;
+  double timestamp;
+  uint16_t ring;                   ///< laser ring number
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW  // make sure our new allocators are aligned
+} EIGEN_ALIGN16;
+
+struct OusterPointXYZIRT {
+  PCL_ADD_POINT4D;
+  float intensity;
+  //  uint32_t time;
+  uint16_t reflectivity;
+  uint8_t ring;
+  std::uint16_t ambient;  // additional property of p.ouster
+  float time;
+  uint16_t noise;
+  uint32_t range;
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+} EIGEN_ALIGN16;
+
+struct RobosensePointXYZIRT {
+  PCL_ADD_POINT4D
+  float intensity;
+  uint16_t ring;  ///< laser ring number
+  double timestamp;
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW  // make sure our new allocators are aligned
+} EIGEN_ALIGN16;
+
+struct PointXYZIRPYT {
+  PCL_ADD_POINT4D
+  PCL_ADD_INTENSITY;
+  float roll;
+  float pitch;
+  float yaw;
+  double time;
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+} EIGEN_ALIGN16;
+
+struct LiovxPointCustomMsg {
+  PCL_ADD_POINT4D
+  PCL_ADD_INTENSITY;
+  std::uint8_t tag;
+  std::uint8_t line;
+  double timestamp;
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+} EIGEN_ALIGN16;
+
+}  // namespace pcl_util
+
+POINT_CLOUD_REGISTER_POINT_STRUCT(
+    pcl_util::VelodynePointXYZIRT,
+    (float, x, x)(float, y, y)(float, z, z)(float, intensity, intensity)(uint16_t, ring,
+                                                                         ring)(double, time, time))
+
+POINT_CLOUD_REGISTER_POINT_STRUCT(pcl_util::LeishenPointXYZIRT,
+                                  (float, x, x)(float, y, y)(float, z,
+                                                             z)(float, intensity,
+                                                                intensity)(uint16_t, ring,
+                                                                           ring)(float, time, time))
+
+POINT_CLOUD_REGISTER_POINT_STRUCT(pcl_util::PandarPointXYZIRT,
+                                  (float, x, x)(float, y, y)(float, z, z)(
+                                      float, intensity, intensity)(double, timestamp,
+                                                                   timestamp)(uint16_t, ring, ring))
+
+POINT_CLOUD_REGISTER_POINT_STRUCT(
+    pcl_util::OusterPointXYZIRT,
+    (float, x, x)(float, y, y)(float, z, z)(float, intensity, intensity)(
+        uint16_t, reflectivity, reflectivity)(uint8_t, ring, ring)(std::uint16_t, ambient, ambient)(
+        float, time, time)(uint16_t, noise, noise)(uint32_t, range, range))
+
+POINT_CLOUD_REGISTER_POINT_STRUCT(pcl_util::RobosensePointXYZIRT,
+                                  (float, x, x)(float, y, y)(float, z, z)(float, intensity,
+                                                                          intensity)(
+                                      uint16_t, ring, ring)(double, timestamp, timestamp))
+
+POINT_CLOUD_REGISTER_POINT_STRUCT(pcl_util::PointXYZIRPYT,
+                                  (float, x, x)(float, y, y)(float, z, z)(float, intensity,
+                                                                          intensity)(float, roll,
+                                                                                     roll)(
+                                      float, pitch, pitch)(float, yaw, yaw)(double, time, time))
+
+POINT_CLOUD_REGISTER_POINT_STRUCT(pcl_util::LiovxPointCustomMsg,
+                                  (float, x, x)(float, y, y)(float, z, z)(float, intensity,
+                                                                          intensity)(
+                                      std::uint8_t, tag, tag)(std::uint8_t, line,
+                                                              line)(double, timestamp, timestamp))

src/common/pcl_util/package.xml

@@ -0,0 +1,15 @@
+<?xml version="1.0"?>
+<package format="3">
+  <name>pcl_util</name>
+  <version>0.0.0</version>
+  <description>The pcl_util package</description>
+  <maintainer email="dreamdeck@todo.todo">admin</maintainer>
+  <license>TODO</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+  <build_depend>ament_cmake</build_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

src/common/time_util/CMakeLists.txt

@@ -0,0 +1,21 @@
+cmake_minimum_required(VERSION 3.8)
+project(time_util)
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+find_package(ament_cmake REQUIRED)
+include_directories(
+  include
+)
+
+install(DIRECTORY include/ DESTINATION include)
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  ament_lint_auto_find_test_dependencies()
+endif()
+
+ament_export_include_directories(include)
+ament_package()

src/common/time_util/include/time_util/tictoc.h

@@ -0,0 +1,48 @@
+
+#pragma once
+
+#include <chrono>
+#include <ctime>
+#include <sstream>
+#include <string>
+
+namespace time_util {
+
+class TicToc {
+ public:
+  TicToc() { tic(); }
+
+  TicToc(bool _disp) {
+    disp_ = _disp;
+    tic();
+  }
+
+  void tic() { start_time_ = std::chrono::system_clock::now(); }
+
+  std::string toc(const std::string& about_task) {
+    end_time_ = std::chrono::system_clock::now();
+    const std::chrono::duration<double> elapsed_seconds = end_time_ - start_time_;
+    const double elapsed_s = elapsed_seconds.count();
+
+    std::stringstream ss;
+    ss.precision(3);
+    if (disp_) {
+      if (!about_task.empty()) {
+        ss << about_task << " : " << std::to_string(elapsed_s) << " sec.";
+      } else {
+        ss << std::to_string(elapsed_s) << " sec.";
+      }
+    }
+    tic();
+    return ss.str();
+  }
+
+  std::string toc() { return toc(""); }
+
+ private:
+  std::chrono::time_point<std::chrono::system_clock> start_time_;
+  std::chrono::time_point<std::chrono::system_clock> end_time_;
+  bool disp_ = false;
+};
+
+}  // namespace time_util

src/common/time_util/package.xml

@@ -0,0 +1,15 @@
+<?xml version="1.0"?>
+<package format="3">
+  <name>time_util</name>
+  <version>0.0.0</version>
+  <description>The time_util package</description>
+  <maintainer email="dreamdeck@todo.todo">admin</maintainer>
+  <license>TODO</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+  <build_depend>ament_cmake</build_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

src/common/transform_util/CMakeLists.txt

@@ -0,0 +1,35 @@
+cmake_minimum_required(VERSION 3.8)
+project(transform_util)
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+set(CMAKE_CXX_STANDARD 17)
+set(CMAKE_BUILD_TYPE Release)
+
+find_package(ament_cmake REQUIRED)
+find_package(Eigen3 REQUIRED)
+include_directories(
+  include
+  ${EIGEN3_INCLUDE_DIRS}
+)
+
+add_library(${PROJECT_NAME} src/transform.cpp)
+
+install(TARGETS ${PROJECT_NAME}
+  EXPORT export_${PROJECT_NAME}
+  ARCHIVE DESTINATION lib
+  LIBRARY DESTINATION lib
+  RUNTIME DESTINATION bin
+)
+
+install(DIRECTORY include/ DESTINATION include)
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  ament_lint_auto_find_test_dependencies()
+endif()
+
+ament_export_include_directories(include)
+ament_export_libraries(${PROJECT_NAME})
+ament_package()

src/common/transform_util/include/transform_util/transform.h

@@ -0,0 +1,18 @@
+
+#pragma once
+
+#include <vector>
+
+#include <Eigen/Geometry>
+
+namespace transform_util {
+
+struct PoseStamp {
+  double stamp_sec = 0.0;
+  Eigen::Vector3f pos;
+  Eigen::Quaternionf rot;
+};
+
+PoseStamp InterpolatePose(const std::vector<PoseStamp>& poses, double timestamp);
+
+}  // namespace transform_util

src/common/transform_util/package.xml

@@ -0,0 +1,15 @@
+<?xml version="1.0"?>
+<package format="3">
+  <name>transform_util</name>
+  <version>0.0.0</version>
+  <description>The transform_util package</description>
+  <maintainer email="dreamdeck@todo.todo">admin</maintainer>
+  <license>TODO</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+  <build_depend>ament_cmake</build_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

src/common/transform_util/src/transform.cpp

@@ -0,0 +1,44 @@
+
+#include "transform_util/transform.h"
+
+namespace transform_util {
+
+PoseStamp InterpolatePose(
+    const std::vector<PoseStamp>& poses, double timestamp) {
+  PoseStamp zero_pose{};
+  zero_pose.pos.setZero();
+  zero_pose.rot.setIdentity();
+  zero_pose.stamp_sec = timestamp;
+  if (poses.size() < 2u) {
+    if (!poses.empty()) {
+      return poses.front();
+    }
+    return zero_pose;
+  }
+  if (timestamp <= poses.front().stamp_sec) {
+    return poses.front();
+  }
+  if (timestamp >= poses.back().stamp_sec) {
+    return poses.back();
+  }
+  size_t left = 0, right = poses.size() - 1;
+  while (right - left > 1) {
+    size_t mid = (left + right) / 2;
+    if (poses[mid].stamp_sec <= timestamp) {
+      left = mid;
+    } else {
+      right = mid;
+    }
+  }
+  const PoseStamp& p0 = poses[left];
+  const PoseStamp& p1 = poses[right];
+  const double dt_total = p1.stamp_sec - p0.stamp_sec;
+  const float alpha = static_cast<float>((timestamp - p0.stamp_sec) / dt_total);
+  PoseStamp res;
+  res.stamp_sec = timestamp;
+  res.pos = p0.pos * (1.0f - alpha) + p1.pos * alpha;
+  res.rot = p0.rot.slerp(alpha, p1.rot);
+  return res;
+}
+
+}  // namespace transform_util

src/dd_resources/CMakeLists.txt

@@ -0,0 +1,29 @@
+cmake_minimum_required(VERSION 3.8)
+project(dd_resources)
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+# find dependencies
+find_package(ament_cmake REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(std_msgs REQUIRED)
+
+install(DIRECTORY audio camera maps model
+  DESTINATION share/${PROJECT_NAME}
+)
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  # the following line skips the linter which checks for copyrights
+  # comment the line when a copyright and license is added to all source files
+  set(ament_cmake_copyright_FOUND TRUE)
+  # the following line skips cpplint (only works in a git repo)
+  # comment the line when this package is in a git repo and when
+  # a copyright and license is added to all source files
+  set(ament_cmake_cpplint_FOUND TRUE)
+  ament_lint_auto_find_test_dependencies()
+endif()
+
+ament_package()

src/dd_resources/camera/calib_extrinsic/CameraExtrinsicMat_20260624_livox_wyh1back.yml

@@ -0,0 +1,48 @@
+%YAML:1.0
+---
+CameraExtrinsicMat: !!opencv-matrix
+   rows: 4
+   cols: 4
+   dt: d      
+     
+   data: [-0.0025202727097469895, 0.9996465691694472, -0.026464787420683873, -0.081,
+         0.22561716209539392, -0.02521407851953442, -0.9738896993050247, 0.331,
+         -0.9742127818877605, -0.008425377864774732, -0.2254738756804134, -0.4346,
+         0.0, 0.0, 0.0, 1.0]
+
+   # data: [0.000074,0.9998979,-0.014275119,0.0,
+   #     -0.04365115,-0.0142582,-0.9989451,-0.16,
+   #     -0.9990467, 0.000697, 0.04364543, -0.12,
+   #    0., 0., 0., 1.] 
+livox_correction: !!opencv-matrix
+   rows: 4
+   cols: 4
+   dt: d
+   data: [ 1, 0, 0, 0,
+           0, 1, 0, 0,
+          0, 0, 1, 0,
+          0,  0, 0, 1]
+CameraMat: !!opencv-matrix
+   rows: 3
+   cols: 3
+   dt: d
+   data: [164.2446,   0.     , 162.0393,
+           0.     , 164.20118, 125.23548,
+           0.     ,   0.     ,   1.      ]
+   #  data: [328.489298 ,   0.     , 324.0786,
+   #         0.     , 328.402363, 250.47097 ,
+   #         0.     ,   0.     ,   1.     ]
+DistCoeff: !!opencv-matrix
+   rows: 5
+   cols: 1
+   dt: d
+   data: [0.023580,-0.026010,0.000183,0.001761,0.00000]
+ImageSize: [ 320, 240 ]
+ReprojectionError: 1.8573832381209129e-02
+Inipara:
+  roll: 2.14
+  pitch: 103.04
+  yaw: -89.36
+  tx: -0.081
+  ty: 0.331
+  tz: -0.4346

src/dd_resources/camera/calib_extrinsic/CameraExtrinsicMat_20260624_livox_wyh1front.yml

@@ -0,0 +1,43 @@
+%YAML:1.0
+---
+CameraExtrinsicMat: !!opencv-matrix
+   rows: 4
+   cols: 4
+   dt: d
+   data: [-0.0004499471246195107, -0.9998105778944554, -0.019457797355822478, 0.0127,
+         0.3682865488297685, 0.01792447755663097, -0.9295395263545174, -0.1104,
+         0.9297122218722542, -0.007584288673108252, 0.3682087221491085, 0.0097,
+         0.0, 0.0, 0.0, 1.0]
+livox_correction: !!opencv-matrix
+   rows: 4
+   cols: 4
+   dt: d
+   data: [ 1, 0, 0, 0,
+           0, 1, 0, 0,
+          0, 0, 1, 0,
+          0,  0, 0, 1]
+   # data: [ 0, -1, 0, 0, cam-lidar 0.16 m
+   #         0, 0, -1, 0, left- front cam 0.18m
+   #        1, 0, 0, 0,   back- front cam 0.1m
+   #        0,  0, 0, 1]  livox angle - 15
+CameraMat: !!opencv-matrix
+   rows: 3
+   cols: 3
+   dt: d
+   data: [328.489298 ,   0.     , 324.0786,
+           0.     , 328.402363, 250.47097 ,
+           0.     ,   0.     ,   1.     ]
+DistCoeff: !!opencv-matrix
+   rows: 5
+   cols: 1
+   dt: d
+   data: [0.023580,-0.026010,0.000183,0.001761,0.00000]
+ImageSize: [ 640, 480 ]
+ReprojectionError: 1.8573832381209129e-02
+Inipara:
+  roll: -1.18
+  pitch: -68.39
+  yaw: 90.07
+  tx: 0.0127
+  ty: -0.1104
+  tz: 0.0097

src/dd_resources/camera/calib_extrinsic/CameraExtrinsicMat_20260624_livox_wyh1left.yml

@@ -0,0 +1,47 @@
+%YAML:1.0
+---
+CameraExtrinsicMat: !!opencv-matrix
+   rows: 4
+   cols: 4
+   dt: d
+   data: [0.9801064273672534, 0.023373579215237795, 0.1970915189146095, 0.17629999999999998,
+         0.1970901309374208, 0.0023858594658885054, -0.9803824702439755, 0.2884,
+         -0.023385279995526833, 0.9997239536338409, -0.002268305590695068, -0.1084,
+         0.0, 0.0, 0.0, 1.0]
+
+   # data: [0.98715072,0.0024058,0.15977313,0.18,
+   #     0.155471857,0.216453,-0.9638344,0.05,
+   #     -0.0369021, 0.976289, 0.2132974, -0.05,
+   #    0., 0., 0., 1.]   
+livox_correction: !!opencv-matrix
+   rows: 4
+   cols: 4
+   dt: d
+   data: [ 1, 0, 0, 0,
+           0, 1, 0, 0,
+          0, 0, 1, 0,
+          0,  0, 0, 1]
+CameraMat: !!opencv-matrix
+   rows: 3
+   cols: 3
+   dt: d
+   data: [164.2446,   0.     , 162.0393,
+           0.     , 164.20118, 125.23548,
+           0.     ,   0.     ,   1.      ]
+   #  data: [328.489298 ,   0.     , 324.0786,
+   #         0.     , 328.402363, 250.47097 ,
+   #         0.     ,   0.     ,   1.     ]
+DistCoeff: !!opencv-matrix
+   rows: 5
+   cols: 1
+   dt: d
+   data: [0.023580,-0.026010,0.000183,0.001761,0.00000]
+ImageSize: [ 320, 240 ]
+ReprojectionError: 1.8573832381209129e-02
+Inipara:
+  roll: 90.13
+  pitch: 1.34
+  yaw: 11.37
+  tx: 0.17629999999999998
+  ty: 0.2884
+  tz: -0.1084

src/dd_resources/camera/calib_extrinsic/CameraExtrinsicMat_20260624_livox_wyh1right.yml

@@ -0,0 +1,49 @@
+%YAML:1.0
+---
+CameraExtrinsicMat: !!opencv-matrix
+   rows: 4
+   cols: 4
+   dt: d
+   # data: [-0.982263882243335,-0.002405936792164, -0.187487790990617,-0.18,
+   #     0.183555029669429,-0.216452999617953,-0.958882584697775,0.0,
+   #     -0.038275234882012, -0.976290000018678, 0.213055364941303, -0.05,
+   #    0., 0., 0., 1.]
+   
+   data: [-0.9908658973868822, -4.707185369725833e-05, -0.1348509220581346, -0.1945,
+         0.134850930273723, -0.0003458774400784691, -0.9908658370198802, 0.47850000000000004,
+         1.2246467991473532e-16, -0.9999999390765166, 0.00034906584331001375, -0.115,
+         0.0, 0.0, 0.0, 1.0]
+
+livox_correction: !!opencv-matrix
+   rows: 4
+   cols: 4
+   dt: d
+   data: [ 1, 0, 0, 0,
+           0, 1, 0, 0,
+          0, 0, 1, 0,
+          0,  0, 0, 1]
+
+CameraMat: !!opencv-matrix
+   rows: 3
+   cols: 3
+   dt: d
+   data: [164.2446,   0.     , 162.0393,
+           0.     , 164.20118, 125.23548,
+           0.     ,   0.     ,   1.      ]
+   #  data: [328.489298 ,   0.     , 324.0786,
+   #         0.     , 328.402363, 250.47097 ,
+   #         0.     ,   0.     ,   1.     ]
+DistCoeff: !!opencv-matrix
+   rows: 5
+   cols: 1
+   dt: d
+   data: [0.023580,-0.026010,0.000183,0.001761,0.00000]  
+ImageSize: [ 320, 240 ]
+ReprojectionError: 1.8573832381209129e-02
+Inipara:
+  roll: 90.02
+  pitch: -180
+  yaw: -7.75
+  tx: -0.1945
+  ty: 0.47850000000000004
+  tz: -0.115

src/dd_resources/camera/calib_intrinsic/camera_info_back.yaml

@@ -0,0 +1,20 @@
+image_width: 320
+image_height: 240
+camera_name: camera_back
+camera_matrix:
+  rows: 3
+  cols: 3
+  data: [438.783367, 0.000000, 305.593336, 0.000000, 437.302876, 243.738352, 0.000000, 0.000000, 1.000000]
+distortion_model: plumb_bob
+distortion_coefficients:
+  rows: 1
+  cols: 5
+  data: [-0.361976, 0.110510, 0.001014, 0.000505, 0.000000]
+rectification_matrix:
+  rows: 3
+  cols: 3
+  data: [0.999978, 0.002789, -0.006046, -0.002816, 0.999986, -0.004401, 0.006034, 0.004417, 0.999972]
+projection_matrix:
+  rows: 3
+  cols: 4
+  data: [393.653800, 0.000000, 322.797939, 0.000000, 0.000000, 393.653800, 241.090902, 0.000000, 0.000000, 0.000000, 1.000000, 0.000000]

src/dd_resources/camera/calib_intrinsic/camera_info_front.yaml

@@ -0,0 +1,20 @@
+image_width: 640
+image_height: 480
+camera_name: camera_front
+camera_matrix:
+  rows: 3
+  cols: 3
+  data: [328.489298, 0.000000, 324.0786, 0.000000, 328.402363, 250.47097, 0.000000, 0.000000, 1.000000]
+distortion_model: plumb_bob
+distortion_coefficients:
+  rows: 1
+  cols: 5
+  data: [0.023580, -0.026010, 0.000183, 0.001761, 0.00000]
+rectification_matrix:
+  rows: 3
+  cols: 3
+  data: [0.999978, 0.002789, -0.006046, -0.002816, 0.999986, -0.004401, 0.006034, 0.004417, 0.999972]
+projection_matrix:
+  rows: 3
+  cols: 4
+  data: [328.489298, 0.000000, 324.0786, 0.000000, 0.000000, 328.402363, 250.47097, 0.000000, 0.000000, 0.000000, 1.000000, 0.000000]

src/dd_resources/camera/calib_intrinsic/camera_info_left.yaml

@@ -0,0 +1,20 @@
+image_width: 320
+image_height: 240
+camera_name: camera_left
+camera_matrix:
+  rows: 3
+  cols: 3
+  data: [438.783367, 0.000000, 305.593336, 0.000000, 437.302876, 243.738352, 0.000000, 0.000000, 1.000000]
+distortion_model: plumb_bob
+distortion_coefficients:
+  rows: 1
+  cols: 5
+  data: [-0.361976, 0.110510, 0.001014, 0.000505, 0.000000]
+rectification_matrix:
+  rows: 3
+  cols: 3
+  data: [0.999978, 0.002789, -0.006046, -0.002816, 0.999986, -0.004401, 0.006034, 0.004417, 0.999972]
+projection_matrix:
+  rows: 3
+  cols: 4
+  data: [393.653800, 0.000000, 322.797939, 0.000000, 0.000000, 393.653800, 241.090902, 0.000000, 0.000000, 0.000000, 1.000000, 0.000000]

src/dd_resources/camera/calib_intrinsic/camera_info_right.yaml

@@ -0,0 +1,20 @@
+image_width: 320
+image_height: 240
+camera_name: camera_right
+camera_matrix:
+  rows: 3
+  cols: 3
+  data: [438.783367, 0.000000, 305.593336, 0.000000, 437.302876, 243.738352, 0.000000, 0.000000, 1.000000]
+distortion_model: plumb_bob
+distortion_coefficients:
+  rows: 1
+  cols: 5
+  data: [-0.361976, 0.110510, 0.001014, 0.000505, 0.000000]
+rectification_matrix:
+  rows: 3
+  cols: 3
+  data: [0.999978, 0.002789, -0.006046, -0.002816, 0.999986, -0.004401, 0.006034, 0.004417, 0.999972]
+projection_matrix:
+  rows: 3
+  cols: 4
+  data: [393.653800, 0.000000, 322.797939, 0.000000, 0.000000, 393.653800, 241.090902, 0.000000, 0.000000, 0.000000, 1.000000, 0.000000]

src/dd_resources/maps/yuanqu_20260104.yaml

@@ -0,0 +1,12 @@
+image: yuanqu_20260104.pgm
+resolution: 0.05
+origin: [-12.854159, 12.889038, 0.0]
+negate: 0
+occupied_thresh: 0.65
+free_thresh: 0.196
+
+# Map metadata
+map_name: PointCloudMap
+created: 2026-05-18T03:07:23.785Z
+width: 1591
+height: 2515

src/dd_resources/package.xml

@@ -0,0 +1,21 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>dd_resources</name>
+  <version>0.0.0</version>
+  <description>TODO: Package description</description>
+  <maintainer email="monkey17336462890@163.com">dreamdeck</maintainer>
+  <license>TODO: License declaration</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <depend>rclcpp</depend>
+  <depend>std_msgs</depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

src/lidar_localization/fast_gicp/.clang-format

@@ -0,0 +1,168 @@
+---
+Language:        Cpp
+# BasedOnStyle:  Google
+AccessModifierOffset: -2
+AlignAfterOpenBracket: AlwaysBreak
+AlignConsecutiveMacros: false
+AlignConsecutiveAssignments: false
+AlignConsecutiveDeclarations: false
+AlignEscapedNewlines: Left
+AlignOperands:   true
+AlignTrailingComments: true
+AllowAllArgumentsOnNextLine: false
+AllowAllConstructorInitializersOnNextLine: false
+AllowAllParametersOfDeclarationOnNextLine: false
+AllowShortBlocksOnASingleLine: Never
+AllowShortCaseLabelsOnASingleLine: false
+AllowShortFunctionsOnASingleLine: Inline
+AllowShortLambdasOnASingleLine: All
+AllowShortIfStatementsOnASingleLine: WithoutElse
+AllowShortLoopsOnASingleLine: true
+AlwaysBreakAfterDefinitionReturnType: None
+AlwaysBreakAfterReturnType: None
+AlwaysBreakBeforeMultilineStrings: true
+AlwaysBreakTemplateDeclarations: Yes
+BinPackArguments: false
+BinPackParameters: false
+BraceWrapping:
+  AfterCaseLabel:  false
+  AfterClass:      false
+  AfterControlStatement: false
+  AfterEnum:       false
+  AfterFunction:   false
+  AfterNamespace:  false
+  AfterObjCDeclaration: false
+  AfterStruct:     false
+  AfterUnion:      false
+  AfterExternBlock: false
+  BeforeCatch:     false
+  BeforeElse:      false
+  IndentBraces:    false
+  SplitEmptyFunction: true
+  SplitEmptyRecord: true
+  SplitEmptyNamespace: true
+BreakBeforeBinaryOperators: None
+BreakBeforeBraces: Attach
+BreakBeforeInheritanceComma: false
+BreakInheritanceList: BeforeColon
+BreakBeforeTernaryOperators: true
+BreakConstructorInitializersBeforeComma: false
+BreakConstructorInitializers: BeforeColon
+BreakAfterJavaFieldAnnotations: false
+BreakStringLiterals: true
+ColumnLimit:     180
+CommentPragmas:  '^ IWYU pragma:'
+CompactNamespaces: false
+ConstructorInitializerAllOnOneLineOrOnePerLine: true
+ConstructorInitializerIndentWidth: 0
+ContinuationIndentWidth: 2
+Cpp11BracedListStyle: true
+DeriveLineEnding: true
+DerivePointerAlignment: false
+DisableFormat:   false
+ExperimentalAutoDetectBinPacking: false
+FixNamespaceComments: true
+ForEachMacros:
+  - foreach
+  - Q_FOREACH
+  - BOOST_FOREACH
+IncludeBlocks:   Regroup
+IncludeCategories:
+  - Regex:           '^<ext/.*\.h>'
+    Priority:        2
+    SortPriority:    0
+  - Regex:           '^<.*\.h>'
+    Priority:        1
+    SortPriority:    0
+  - Regex:           '^<.*'
+    Priority:        2
+    SortPriority:    0
+  - Regex:           '.*'
+    Priority:        3
+    SortPriority:    0
+IncludeIsMainRegex: '([-_](test|unittest))?$'
+IncludeIsMainSourceRegex: ''
+IndentCaseLabels: true
+IndentGotoLabels: true
+IndentPPDirectives: None
+IndentWidth:     2
+IndentWrappedFunctionNames: false
+JavaScriptQuotes: Leave
+JavaScriptWrapImports: true
+KeepEmptyLinesAtTheStartOfBlocks: false
+MacroBlockBegin: ''
+MacroBlockEnd:   ''
+MaxEmptyLinesToKeep: 1
+NamespaceIndentation: None
+ObjCBinPackProtocolList: Never
+ObjCBlockIndentWidth: 2
+ObjCSpaceAfterProperty: false
+ObjCSpaceBeforeProtocolList: true
+PenaltyBreakAssignment: 2
+PenaltyBreakBeforeFirstCallParameter: 1
+PenaltyBreakComment: 300
+PenaltyBreakFirstLessLess: 120
+PenaltyBreakString: 1000
+PenaltyBreakTemplateDeclaration: 10
+PenaltyExcessCharacter: 1000000
+PenaltyReturnTypeOnItsOwnLine: 200
+PointerAlignment: Left
+RawStringFormats:
+  - Language:        Cpp
+    Delimiters:
+      - cc
+      - CC
+      - cpp
+      - Cpp
+      - CPP
+      - 'c++'
+      - 'C++'
+    CanonicalDelimiter: ''
+    BasedOnStyle:    google
+  - Language:        TextProto
+    Delimiters:
+      - pb
+      - PB
+      - proto
+      - PROTO
+    EnclosingFunctions:
+      - EqualsProto
+      - EquivToProto
+      - PARSE_PARTIAL_TEXT_PROTO
+      - PARSE_TEST_PROTO
+      - PARSE_TEXT_PROTO
+      - ParseTextOrDie
+      - ParseTextProtoOrDie
+    CanonicalDelimiter: ''
+    BasedOnStyle:    google
+ReflowComments:  true
+SortIncludes:    false
+SortUsingDeclarations: true
+SpaceAfterCStyleCast: false
+SpaceAfterLogicalNot: false
+SpaceAfterTemplateKeyword: true
+SpaceBeforeAssignmentOperators: true
+SpaceBeforeCpp11BracedList: false
+SpaceBeforeCtorInitializerColon: true
+SpaceBeforeInheritanceColon: true
+SpaceBeforeParens: ControlStatements
+SpaceBeforeRangeBasedForLoopColon: true
+SpaceInEmptyBlock: false
+SpaceInEmptyParentheses: false
+SpacesBeforeTrailingComments: 2
+SpacesInAngles:  false
+SpacesInConditionalStatement: false
+SpacesInContainerLiterals: true
+SpacesInCStyleCastParentheses: false
+SpacesInParentheses: false
+SpacesInSquareBrackets: false
+SpaceBeforeSquareBrackets: false
+Standard:        Auto
+StatementMacros:
+  - Q_UNUSED
+  - QT_REQUIRE_VERSION
+TabWidth:        8
+UseCRLF:         false
+UseTab:          Never
+...
+

src/lidar_localization/fast_gicp/CMakeLists.txt

@@ -0,0 +1,168 @@
+cmake_minimum_required(VERSION 3.5)
+project(fast_gicp)
+
+option(BUILD_VGICP_CUDA "Build GPU-powered VGICP" OFF)
+option(BUILD_apps "Build application programs" ON)
+option(BUILD_test "Build test programs" OFF)
+option(BUILD_PYTHON_BINDINGS "Build python bindings" OFF)
+
+if(${CMAKE_SYSTEM_PROCESSOR} STREQUAL "aarch64")
+else()
+  add_definitions(-msse -msse2 -msse3 -msse4 -msse4.1 -msse4.2)
+  set(CMAKE_C_FLAGS "-msse -msse2 -msse3 -msse4 -msse4.1 -msse4.2")
+  set(CMAKE_CXX_FLAGS "-msse -msse2 -msse3 -msse4 -msse4.1 -msse4.2")
+endif()
+
+set(CMAKE_BUILD_TYPE "Release")
+
+find_package(PCL REQUIRED)
+find_package(Eigen3 REQUIRED)
+
+if(NOT BUILD_PYTHON_BINDINGS)
+  find_package(ament_cmake_ros)
+endif()
+
+find_package(OpenMP)
+if (OPENMP_FOUND)
+  set (CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${OpenMP_C_FLAGS}")
+  set (CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS}")
+endif()
+
+if(BUILD_VGICP_CUDA)
+  find_package(CUDA REQUIRED)
+  include_directories(${CUDA_INCLUDE_DIRS})
+  link_directories(${CUDA_LIBRARY_DIRS})
+endif()
+
+###########
+## Build ##
+###########
+
+add_library(fast_gicp SHARED
+  src/fast_gicp/gicp/lsq_registration.cpp
+  src/fast_gicp/gicp/fast_gicp.cpp
+  src/fast_gicp/gicp/fast_gicp_st.cpp
+  src/fast_gicp/gicp/fast_vgicp.cpp
+)
+target_link_libraries(fast_gicp
+  ${PCL_LIBRARIES}
+)
+target_include_directories(fast_gicp PUBLIC
+  include
+  ${PCL_INCLUDE_DIRS}
+  ${EIGEN3_INCLUDE_DIR}
+)
+
+###################################
+## ament specific configuration ##
+###################################
+if(ament_cmake_ros_FOUND)
+  ament_export_include_directories(include)
+  ament_export_libraries(fast_gicp)
+  ament_export_dependencies(Eigen3)
+  ament_export_dependencies(PCL)
+
+  install(
+    TARGETS fast_gicp
+    EXPORT fast_gicp
+    ARCHIVE DESTINATION lib
+    LIBRARY DESTINATION lib
+    RUNTIME DESTINATION bin
+  )
+
+  install(
+    DIRECTORY include/
+    DESTINATION include
+  )
+
+  ament_package()
+endif()
+
+### APPS ###
+if(BUILD_apps)
+  add_executable(gicp_align src/align.cpp)
+  add_dependencies(gicp_align fast_gicp)
+  target_link_libraries(gicp_align
+    ${PCL_LIBRARIES}
+    fast_gicp
+  )
+
+  add_executable(gicp_kitti src/kitti.cpp)
+  add_dependencies(gicp_kitti fast_gicp)
+  target_link_libraries(gicp_kitti
+    ${PCL_LIBRARIES}
+    fast_gicp
+  )
+endif()
+
+### Python bindings ###
+if(BUILD_PYTHON_BINDINGS)
+  add_subdirectory(thirdparty/pybind11)
+  pybind11_add_module(pygicp
+    src/python/main.cpp
+  )
+  target_include_directories(pygicp PUBLIC
+    include
+    ${PCL_INCLUDE_DIRS}
+    ${EIGEN3_INCLUDE_DIR}
+  )
+  target_link_libraries(pygicp PRIVATE
+    fast_gicp
+  )
+endif()
+
+### CUDA ###
+if(BUILD_VGICP_CUDA)
+  set(CUDA_NVCC_FLAGS "--expt-relaxed-constexpr")
+  add_definitions(-DUSE_VGICP_CUDA)
+
+  cuda_add_library(fast_vgicp_cuda SHARED
+    src/fast_gicp/cuda/fast_vgicp_cuda.cu
+    src/fast_gicp/cuda/brute_force_knn.cu
+    src/fast_gicp/cuda/covariance_estimation.cu
+    src/fast_gicp/cuda/covariance_estimation_rbf.cu
+    src/fast_gicp/cuda/covariance_regularization.cu
+    src/fast_gicp/cuda/gaussian_voxelmap.cu
+    src/fast_gicp/cuda/find_voxel_correspondences.cu
+    src/fast_gicp/cuda/compute_derivatives.cu
+    src/fast_gicp/cuda/compute_mahalanobis.cu
+    src/fast_gicp/cuda/ndt_cuda.cu
+    src/fast_gicp/cuda/ndt_compute_derivatives.cu
+  )
+  target_include_directories(fast_vgicp_cuda PRIVATE
+    include
+    thirdparty/Eigen
+    thirdparty/nvbio
+    ${catkin_INCLUDE_DIRS}
+  )
+  target_link_libraries(fast_vgicp_cuda
+    ${catkin_LIBRARIES}
+  )
+  cuda_add_cublas_to_target(fast_vgicp_cuda)
+
+  # add vgicp_cuda to libfast_gicp
+  target_sources(fast_gicp PRIVATE
+    src/fast_gicp/gicp/fast_vgicp_cuda.cpp
+    src/fast_gicp/ndt/ndt_cuda.cpp
+  )
+  target_link_libraries(fast_gicp
+    fast_vgicp_cuda
+  )
+  add_dependencies(fast_gicp fast_vgicp_cuda)
+  if(ament_cmake_ros_FOUND)
+    install(
+      TARGETS fast_vgicp_cuda
+      LIBRARY DESTINATION lib
+    )
+  endif()
+endif()
+
+### TEST ###
+if(BUILD_test)
+  find_package(GTest REQUIRED)
+
+  add_executable(gicp_test src/test/gicp_test.cpp)
+  add_dependencies(gicp_test fast_gicp)
+  target_link_libraries(gicp_test ${GTEST_LIBRARIES} ${PCL_LIBRARIES} fast_gicp)
+  gtest_add_tests(TARGET gicp_test WORKING_DIRECTORY ${CMAKE_SOURCE_DIR} EXTRA_ARGS "${CMAKE_SOURCE_DIR}/data")
+endif()

src/lidar_localization/fast_gicp/LICENSE

@@ -0,0 +1,29 @@
+BSD 3-Clause License
+
+Copyright (c) 2020, SMRT-AIST
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+
+1. Redistributions of source code must retain the above copyright notice, this
+   list of conditions and the following disclaimer.
+
+2. Redistributions in binary form must reproduce the above copyright notice,
+   this list of conditions and the following disclaimer in the documentation
+   and/or other materials provided with the distribution.
+
+3. 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.

src/lidar_localization/fast_gicp/README.md

@@ -0,0 +1,157 @@
+# fast_gicp
+
+This package is a collection of GICP-based fast point cloud registration algorithms. It constains a multi-threaded GICP as well as multi-thread and GPU implementations of our voxelized GICP (VGICP) algorithm. All the implemented algorithms have the PCL registration interface so that they can be used as an inplace replacement for GICP in PCL.
+
+- FastGICP: multi-threaded GICP algorithm (**\~40FPS**)
+- FastGICPSingleThread: GICP algorithm optimized for single-threading (**\~15FPS**)
+- FastVGICP: multi-threaded and voxelized GICP algorithm (**\~70FPS**)
+- FastVGICPCuda: CUDA-accelerated voxelized GICP algorithm (**\~120FPS**)
+- NDTCuda: CUDA-accelerated D2D NDT algorithm (**\~500FPS**)
+![proctime](data/proctime.png)
+
+[![Build](https://github.com/SMRT-AIST/fast_gicp/actions/workflows/build.yml/badge.svg)](https://github.com/SMRT-AIST/fast_gicp/actions/workflows/build.yml) on melodic & noetic
+
+## Installation
+
+### Dependencies
+- PCL
+- Eigen
+- OpenMP
+- CUDA (optional)
+- [Sophus](https://github.com/strasdat/Sophus)
+- [nvbio](https://github.com/NVlabs/nvbio)
+
+We have tested this package on Ubuntu 18.04/20.04 and CUDA 11.1.
+
+### CUDA
+
+To enable the CUDA-powered implementations, set ```BUILD_VGICP_CUDA``` cmake option to ```ON```.
+
+### ROS
+```bash
+cd ~/catkin_ws/src
+git clone https://github.com/SMRT-AIST/fast_gicp --recursive
+cd .. && catkin_make -DCMAKE_BUILD_TYPE=Release
+# enable cuda-based implementations
+# cd .. && catkin_make -DCMAKE_BUILD_TYPE=Release -DBUILD_VGICP_CUDA=ON
+```
+
+### Non-ROS
+```bash
+git clone https://github.com/SMRT-AIST/fast_gicp --recursive
+mkdir fast_gicp/build && cd fast_gicp/build
+cmake .. -DCMAKE_BUILD_TYPE=Release
+# enable cuda-based implementations
+# cmake .. -DCMAKE_BUILD_TYPE=Release -DBUILD_VGICP_CUDA=ON
+make -j8
+```
+
+### Python bindings
+```bash
+cd fast_gicp
+python3 setup.py install --user
+```
+Note: If you are on a catkin-enabled environment and the installation doesn't work well, comment out ```find_package(catkin)``` in CMakeLists.txt and run the above installation command again.
+
+
+```python
+import pygicp
+
+target = # Nx3 numpy array
+source = # Mx3 numpy array
+
+# 1. function interface
+matrix = pygicp.align_points(target, source)
+
+# optional arguments
+# initial_guess               : Initial guess of the relative pose (4x4 matrix)
+# method                      : GICP, VGICP, VGICP_CUDA, or NDT_CUDA
+# downsample_resolution       : Downsampling resolution (used only if positive)
+# k_correspondences           : Number of points used for covariance estimation
+# max_correspondence_distance : Maximum distance for corresponding point search
+# voxel_resolution            : Resolution of voxel-based algorithms
+# neighbor_search_method      : DIRECT1, DIRECT7, DIRECT27, or DIRECT_RADIUS
+# neighbor_search_radius      : Neighbor voxel search radius (for GPU-based methods)
+# num_threads                 : Number of threads
+
+
+# 2. class interface
+# you may want to downsample the input clouds before registration
+target = pygicp.downsample(target, 0.25)
+source = pygicp.downsample(source, 0.25)
+
+# pygicp.FastGICP has more or less the same interfaces as the C++ version
+gicp = pygicp.FastGICP()
+gicp.set_input_target(target)
+gicp.set_input_source(source)
+matrix = gicp.align()
+
+# optional
+gicp.set_num_threads(4)
+gicp.set_max_correspondence_distance(1.0)
+gicp.get_final_transformation()
+gicp.get_final_hessian()
+```
+
+## Benchmark
+CPU:Core i9-9900K GPU:GeForce RTX2080Ti
+
+```bash
+roscd fast_gicp/data
+rosrun fast_gicp gicp_align 251370668.pcd 251371071.pcd
+```
+
+```
+target:17249[pts] source:17518[pts]
+--- pcl_gicp ---
+single:127.508[msec] 100times:12549.4[msec] fitness_score:0.204892
+--- pcl_ndt ---
+single:53.5904[msec] 100times:5467.16[msec] fitness_score:0.229616
+--- fgicp_st ---
+single:111.324[msec] 100times:10662.7[msec] 100times_reuse:6794.59[msec] fitness_score:0.204379
+--- fgicp_mt ---
+single:20.1602[msec] 100times:1585[msec] 100times_reuse:1017.74[msec] fitness_score:0.204412
+--- vgicp_st ---
+single:112.001[msec] 100times:7959.9[msec] 100times_reuse:4408.22[msec] fitness_score:0.204067
+--- vgicp_mt ---
+single:18.1106[msec] 100times:1381[msec] 100times_reuse:806.53[msec] fitness_score:0.204067
+--- vgicp_cuda (parallel_kdtree) ---
+single:15.9587[msec] 100times:1451.85[msec] 100times_reuse:695.48[msec] fitness_score:0.204061
+--- vgicp_cuda (gpu_bruteforce) ---
+single:53.9113[msec] 100times:3463.5[msec] 100times_reuse:1703.41[msec] fitness_score:0.204049
+--- vgicp_cuda (gpu_rbf_kernel) ---
+single:5.91508[msec] 100times:590.725[msec] 100times_reuse:226.787[msec] fitness_score:0.20557
+```
+
+See [src/align.cpp](https://github.com/SMRT-AIST/fast_gicp/blob/master/src/align.cpp) for the detailed usage.
+
+## Test on KITTI
+
+### C++
+
+```bash
+# Perform frame-by-frame registration
+rosrun fast_gicp gicp_kitti /your/kitti/path/sequences/00/velodyne
+```
+
+![kitti00](https://user-images.githubusercontent.com/31344317/86207074-b98ac280-bba8-11ea-9687-e65f03aaf25b.png)
+
+### Python
+
+```bash
+cd fast_gicp/src
+python3 kitti.py /your/kitti/path/sequences/00/velodyne
+```
+
+## Related packages
+- [ndt_omp](https://github.com/koide3/ndt_omp)
+- [fast_gicp](https://github.com/SMRT-AIST/fast_gicp)
+
+
+## Papers
+- Kenji Koide, Masashi Yokozuka, Shuji Oishi, and Atsuhiko Banno, Voxelized GICP for fast and accurate 3D point cloud registration, ICRA2021 [[link]](https://easychair.org/publications/preprint/ftvV)
+
+## Contact
+Kenji Koide, k.koide@aist.go.jp
+
+Human-Centered Mobility Research Center, National Institute of Advanced Industrial Science and Technology, Japan  [\[URL\]](https://unit.aist.go.jp/rirc/en/team/smart_mobility.html)

src/lidar_localization/fast_gicp/data/relative.txt

@@ -0,0 +1,4 @@
+    0.999941    0.0108432 -0.000635437     0.485657
+  -0.0108468     0.999924  -0.00587782      0.10642
+ 0.000571654   0.00588436     0.999983   -0.0131581
+           0            0            0            1

src/lidar_localization/fast_gicp/include/fast_gicp/cuda/brute_force_knn.cuh

@@ -0,0 +1,17 @@
+#ifndef FAST_GICP_CUDA_BRUTE_FORCE_KNN_CUH
+#define FAST_GICP_CUDA_BRUTE_FORCE_KNN_CUH
+
+#include <Eigen/Core>
+
+#include <thrust/device_vector.h>
+
+namespace fast_gicp {
+  namespace cuda {
+
+void brute_force_knn_search(const thrust::device_vector<Eigen::Vector3f>& source, const thrust::device_vector<Eigen::Vector3f>& target, int k, thrust::device_vector<thrust::pair<float, int>>& k_neighbors, bool do_sort=false);
+
+  }
+} // namespace fast_gicp
+
+
+#endif

src/lidar_localization/fast_gicp/include/fast_gicp/cuda/compute_derivatives.cuh

@@ -0,0 +1,24 @@
+#ifndef FAST_GICP_CUDA_COMPUTE_DERIVATIVES_CUH
+#define FAST_GICP_CUDA_COMPUTE_DERIVATIVES_CUH
+
+#include <Eigen/Core>
+#include <thrust/device_vector.h>
+
+#include <fast_gicp/cuda/gaussian_voxelmap.cuh>
+
+namespace fast_gicp {
+  namespace cuda {
+
+  double compute_derivatives(
+    const thrust::device_vector<Eigen::Vector3f>& src_points,
+    const thrust::device_vector<Eigen::Matrix3f>& src_covs,
+    const GaussianVoxelMap& voxelmap,
+    const thrust::device_vector<thrust::pair<int, int>>& voxel_correspondences,
+    const thrust::device_ptr<const Eigen::Isometry3f>& linearized_x_ptr,
+    const thrust::device_ptr<const Eigen::Isometry3f>& x_ptr,
+    Eigen::Matrix<double, 6, 6>* H,
+    Eigen::Matrix<double, 6, 1>* b);
+  }
+}  // namespace fast_gicp
+
+#endif

src/lidar_localization/fast_gicp/include/fast_gicp/cuda/compute_mahalanobis.cuh

@@ -0,0 +1,25 @@
+#ifndef FAST_GICP_CUDA_COMPUTE_MAHALANOBIS_CUH
+#define FAST_GICP_CUDA_COMPUTE_MAHALANOBIS_CUH
+
+#include <Eigen/Core>
+#include <Eigen/Geometry>
+#include <thrust/device_vector.h>
+
+#include <fast_gicp/cuda/gaussian_voxelmap.cuh>
+
+namespace fast_gicp {
+  namespace cuda {
+
+void compute_mahalanobis(
+  const thrust::device_vector<Eigen::Vector3f>& src_points,
+  const thrust::device_vector<Eigen::Matrix3f>& src_covs,
+  const GaussianVoxelMap& voxelmap,
+  const thrust::device_vector<int>& voxel_correspondences,
+  const Eigen::Isometry3f& linearized_x,
+  thrust::device_vector<Eigen::Matrix3f>& mahalanobis
+  );
+
+  }
+}  // namespace fast_gicp
+
+#endif

src/lidar_localization/fast_gicp/include/fast_gicp/cuda/covariance_estimation.cuh

@@ -0,0 +1,17 @@
+#ifndef FAST_GICP_CUDA_COVARIANCE_ESTIMATION_CUH
+#define FAST_GICP_CUDA_COVARIANCE_ESTIMATION_CUH
+
+#include <Eigen/Core>
+#include <thrust/device_vector.h>
+#include <fast_gicp/gicp/gicp_settings.hpp>
+
+namespace fast_gicp {
+namespace cuda {
+
+void covariance_estimation(const thrust::device_vector<Eigen::Vector3f>& points, int k, const thrust::device_vector<int>& k_neighbors, thrust::device_vector<Eigen::Matrix3f>& covariances);
+
+void covariance_estimation_rbf(const thrust::device_vector<Eigen::Vector3f>& points, double kernel_width, double max_dist, thrust::device_vector<Eigen::Matrix3f>& covariances);
+}
+  }  // namespace fast_gicp
+
+#endif

src/lidar_localization/fast_gicp/include/fast_gicp/cuda/covariance_regularization.cuh

@@ -0,0 +1,16 @@
+#ifndef FAST_GICP_CUDA_COVARIANCE_REGULARIZATION_CUH
+#define FAST_GICP_CUDA_COVARIANCE_REGULARIZATION_CUH
+
+#include <Eigen/Core>
+#include <thrust/device_vector.h>
+#include <fast_gicp/gicp/gicp_settings.hpp>
+
+namespace fast_gicp {
+namespace cuda {
+
+void covariance_regularization(thrust::device_vector<Eigen::Vector3f>& means, thrust::device_vector<Eigen::Matrix3f>& covs, RegularizationMethod method);
+
+}  // namespace cuda
+}  // namespace fast_gicp
+
+#endif

src/lidar_localization/fast_gicp/include/fast_gicp/cuda/fast_vgicp_cuda.cuh

@@ -0,0 +1,97 @@
+#ifndef FAST_GICP_FAST_VGICP_CUDA_CORE_CUH
+#define FAST_GICP_FAST_VGICP_CUDA_CORE_CUH
+
+#include <memory>
+#include <vector>
+#include <Eigen/Core>
+#include <Eigen/Geometry>
+
+#include <fast_gicp/gicp/gicp_settings.hpp>
+
+namespace thrust {
+
+template <typename T1, typename T2>
+class pair;
+
+template <typename T>
+class device_allocator;
+
+template <typename T, typename Alloc>
+class device_vector;
+}  // namespace thrust
+
+namespace fast_gicp {
+namespace cuda {
+
+class GaussianVoxelMap;
+
+class FastVGICPCudaCore {
+public:
+  using Points = thrust::device_vector<Eigen::Vector3f, thrust::device_allocator<Eigen::Vector3f>>;
+  using Indices = thrust::device_vector<int, thrust::device_allocator<int>>;
+  using Matrices = thrust::device_vector<Eigen::Matrix3f, thrust::device_allocator<Eigen::Matrix3f>>;
+  using Correspondences = thrust::device_vector<thrust::pair<int, int>, thrust::device_allocator<thrust::pair<int, int>>>;
+  using VoxelCoordinates = thrust::device_vector<Eigen::Vector3i, thrust::device_allocator<Eigen::Vector3i>>;
+
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+  FastVGICPCudaCore();
+  ~FastVGICPCudaCore();
+
+  void set_resolution(double resolution);
+  void set_kernel_params(double kernel_width, double kernel_max_dist);
+  void set_neighbor_search_method(fast_gicp::NeighborSearchMethod method, double radius);
+
+  void swap_source_and_target();
+  void set_source_cloud(const std::vector<Eigen::Vector3f, Eigen::aligned_allocator<Eigen::Vector3f>>& cloud);
+  void set_target_cloud(const std::vector<Eigen::Vector3f, Eigen::aligned_allocator<Eigen::Vector3f>>& cloud);
+
+  void set_source_neighbors(int k, const std::vector<int>& neighbors);
+  void set_target_neighbors(int k, const std::vector<int>& neighbors);
+  void find_source_neighbors(int k);
+  void find_target_neighbors(int k);
+
+  void calculate_source_covariances(RegularizationMethod method);
+  void calculate_target_covariances(RegularizationMethod method);
+
+  void calculate_source_covariances_rbf(RegularizationMethod method);
+  void calculate_target_covariances_rbf(RegularizationMethod method);
+
+  void get_source_covariances(std::vector<Eigen::Matrix3f, Eigen::aligned_allocator<Eigen::Matrix3f>>& covs) const;
+  void get_target_covariances(std::vector<Eigen::Matrix3f, Eigen::aligned_allocator<Eigen::Matrix3f>>& covs) const;
+
+  void get_voxel_num_points(std::vector<int>& num_points) const;
+  void get_voxel_means(std::vector<Eigen::Vector3f, Eigen::aligned_allocator<Eigen::Vector3f>>& means) const;
+  void get_voxel_covs(std::vector<Eigen::Matrix3f, Eigen::aligned_allocator<Eigen::Matrix3f>>& covs) const;
+  void get_voxel_correspondences(std::vector<std::pair<int, int>>& correspondences) const;
+
+  void create_target_voxelmap();
+
+  void update_correspondences(const Eigen::Isometry3d& trans);
+
+  double compute_error(const Eigen::Isometry3d& trans, Eigen::Matrix<double, 6, 6>* H, Eigen::Matrix<double, 6, 1>* b) const;
+
+public:
+  double resolution;
+  double kernel_width;
+  double kernel_max_dist;
+  std::unique_ptr<VoxelCoordinates> offsets;
+
+  std::unique_ptr<Points> source_points;
+  std::unique_ptr<Points> target_points;
+
+  std::unique_ptr<Indices> source_neighbors;
+  std::unique_ptr<Indices> target_neighbors;
+
+  std::unique_ptr<Matrices> source_covariances;
+  std::unique_ptr<Matrices> target_covariances;
+
+  std::unique_ptr<GaussianVoxelMap> voxelmap;
+
+  Eigen::Isometry3f linearized_x;
+  std::unique_ptr<Correspondences> voxel_correspondences;
+};
+
+}  // namespace cuda
+}  // namespace fast_gicp
+
+#endif

src/lidar_localization/fast_gicp/include/fast_gicp/cuda/find_voxel_correspondences.cuh

@@ -0,0 +1,24 @@
+#ifndef FAST_GICP_CUDA_FIND_VOXEL_CORRESPONDENCES_CUH
+#define FAST_GICP_CUDA_FIND_VOXEL_CORRESPONDENCES_CUH
+
+#include <Eigen/Core>
+#include <Eigen/Geometry>
+
+#include <thrust/pair.h>
+#include <thrust/device_vector.h>
+#include <fast_gicp/cuda/gaussian_voxelmap.cuh>
+
+namespace fast_gicp {
+namespace cuda {
+
+void find_voxel_correspondences(
+  const thrust::device_vector<Eigen::Vector3f>& src_points,
+  const GaussianVoxelMap& voxelmap,
+  const thrust::device_ptr<const Eigen::Isometry3f>& x_ptr,
+  const thrust::device_vector<Eigen::Vector3i>& offsets,
+  thrust::device_vector<thrust::pair<int, int>>& correspondences) ;
+
+}  // namespace cuda
+}  // namespace fast_gicp
+
+#endif

src/lidar_localization/fast_gicp/include/fast_gicp/cuda/gaussian_voxelmap.cuh

@@ -0,0 +1,44 @@
+#ifndef FAST_GICP_CUDA_GAUSSIAN_VOXELMAP_CUH
+#define FAST_GICP_CUDA_GAUSSIAN_VOXELMAP_CUH
+
+#include <Eigen/Core>
+#include <thrust/host_vector.h>
+#include <thrust/device_vector.h>
+
+namespace fast_gicp {
+namespace cuda {
+
+struct VoxelMapInfo {
+  int num_voxels;
+  int num_buckets;
+  int max_bucket_scan_count;
+  float voxel_resolution;
+};
+
+class GaussianVoxelMap {
+public:
+  GaussianVoxelMap(float resolution, int init_num_buckets = 8192, int max_bucket_scan_count = 10);
+
+  void create_voxelmap(const thrust::device_vector<Eigen::Vector3f>& points);
+  void create_voxelmap(const thrust::device_vector<Eigen::Vector3f>& points, const thrust::device_vector<Eigen::Matrix3f>& covariances);
+
+private:
+  void create_bucket_table(cudaStream_t stream, const thrust::device_vector<Eigen::Vector3f>& points);
+
+public:
+  const int init_num_buckets;
+  VoxelMapInfo voxelmap_info;
+  thrust::device_vector<VoxelMapInfo> voxelmap_info_ptr;
+
+  thrust::device_vector<thrust::pair<Eigen::Vector3i, int>> buckets;
+
+  // voxel data
+  thrust::device_vector<int> num_points;
+  thrust::device_vector<Eigen::Vector3f> voxel_means;
+  thrust::device_vector<Eigen::Matrix3f> voxel_covs;
+};
+
+}  // namespace cuda
+}  // namespace fast_gicp
+
+#endif

src/lidar_localization/fast_gicp/include/fast_gicp/cuda/ndt_compute_derivatives.cuh

@@ -0,0 +1,34 @@
+#ifndef FAST_GICP_CUDA_NDT_COMPUTE_DERIVATIVES_CUH
+#define FAST_GICP_CUDA_NDT_COMPUTE_DERIVATIVES_CUH
+
+#include <Eigen/Core>
+#include <Eigen/Geometry>
+#include <thrust/device_vector.h>
+
+#include <fast_gicp/cuda/gaussian_voxelmap.cuh>
+
+namespace fast_gicp {
+namespace cuda {
+
+double p2d_ndt_compute_derivatives(
+  const GaussianVoxelMap& target_voxelmap,
+  const thrust::device_vector<Eigen::Vector3f>& source_points,
+  const thrust::device_vector<thrust::pair<int, int>>& correspondences,
+  const thrust::device_ptr<const Eigen::Isometry3f>& linearized_x_ptr,
+  const thrust::device_ptr<const Eigen::Isometry3f>& x_ptr,
+  Eigen::Matrix<double, 6, 6>* H,
+  Eigen::Matrix<double, 6, 1>* b);
+
+double d2d_ndt_compute_derivatives(
+  const GaussianVoxelMap& target_voxelmap,
+  const GaussianVoxelMap& source_voxelmap,
+  const thrust::device_vector<thrust::pair<int, int>>& correspondences,
+  const thrust::device_ptr<const Eigen::Isometry3f>& linearized_x_ptr,
+  const thrust::device_ptr<const Eigen::Isometry3f>& x_ptr,
+  Eigen::Matrix<double, 6, 6>* H,
+  Eigen::Matrix<double, 6, 1>* b);
+
+}  // namespace cuda
+}  // namespace fast_gicp
+
+#endif

src/lidar_localization/fast_gicp/include/fast_gicp/cuda/ndt_cuda.cuh

@@ -0,0 +1,73 @@
+#ifndef FAST_GICP_NDT_CUDA_CUH
+#define FAST_GICP_NDT_CUDA_CUH
+
+#include <memory>
+#include <vector>
+#include <Eigen/Core>
+#include <Eigen/Geometry>
+
+#include <fast_gicp/ndt/ndt_settings.hpp>
+#include <fast_gicp/gicp/gicp_settings.hpp>
+
+namespace thrust {
+template <typename T1, typename T2>
+class pair;
+
+template <typename T>
+class device_allocator;
+
+template <typename T, typename Alloc>
+class device_vector;
+}  // namespace thrust
+
+namespace fast_gicp {
+namespace cuda {
+
+class GaussianVoxelMap;
+
+class NDTCudaCore {
+public:
+  using Points = thrust::device_vector<Eigen::Vector3f, thrust::device_allocator<Eigen::Vector3f>>;
+  using Indices = thrust::device_vector<int, thrust::device_allocator<int>>;
+  using Matrices = thrust::device_vector<Eigen::Matrix3f, thrust::device_allocator<Eigen::Matrix3f>>;
+  using Correspondences = thrust::device_vector<thrust::pair<int, int>, thrust::device_allocator<thrust::pair<int, int>>>;
+  using VoxelCoordinates = thrust::device_vector<Eigen::Vector3i, thrust::device_allocator<Eigen::Vector3i>>;
+
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+  NDTCudaCore();
+  ~NDTCudaCore();
+
+  void set_distance_mode(fast_gicp::NDTDistanceMode mode);
+  void set_resolution(double resolution);
+  void set_neighbor_search_method(fast_gicp::NeighborSearchMethod method, double radius);
+
+  void swap_source_and_target();
+  void set_source_cloud(const std::vector<Eigen::Vector3f, Eigen::aligned_allocator<Eigen::Vector3f>>& cloud);
+  void set_target_cloud(const std::vector<Eigen::Vector3f, Eigen::aligned_allocator<Eigen::Vector3f>>& cloud);
+
+  void create_voxelmaps();
+  void create_target_voxelmap();
+  void create_source_voxelmap();
+
+  void update_correspondences(const Eigen::Isometry3d& trans);
+  double compute_error(const Eigen::Isometry3d& trans, Eigen::Matrix<double, 6, 6>* H, Eigen::Matrix<double, 6, 1>* b) const;
+
+public:
+  fast_gicp::NDTDistanceMode distance_mode;
+  double resolution;
+  std::unique_ptr<VoxelCoordinates> offsets;
+
+  std::unique_ptr<Points> source_points;
+  std::unique_ptr<Points> target_points;
+
+  std::unique_ptr<GaussianVoxelMap> source_voxelmap;
+  std::unique_ptr<GaussianVoxelMap> target_voxelmap;
+
+  Eigen::Isometry3f linearized_x;
+  std::unique_ptr<Correspondences> correspondences;
+};
+
+}  // namespace cuda
+}  // namespace fast_gicp
+
+#endif

src/lidar_localization/fast_gicp/include/fast_gicp/cuda/vector3_hash.cuh

@@ -0,0 +1,43 @@
+#ifndef FAST_GICP_CUDA_VECTOR3_HASH_CUH
+#define FAST_GICP_CUDA_VECTOR3_HASH_CUH
+
+namespace fast_gicp {
+  namespace cuda {
+
+// taken from boost/hash.hpp
+__host__ __device__  inline void hash_combine(uint64_t& h, uint64_t k) {
+  const uint64_t m = UINT64_C(0xc6a4a7935bd1e995);
+  const int r = 47;
+
+  k *= m;
+  k ^= k >> r;
+  k *= m;
+
+  h ^= k;
+  h *= m;
+
+  h += 0xe6546b64;
+}
+
+inline __host__ __device__ bool equal(const Eigen::Vector3i& lhs, const Eigen::Vector3i& rhs) {
+  return lhs[0] == rhs[0] && lhs[1] == rhs[1] && lhs[2] == rhs[2];
+}
+
+// compute vector3i hash
+__host__ __device__ inline uint64_t vector3i_hash(const Eigen::Vector3i& x) {
+  uint64_t seed = 0;
+  hash_combine(seed, x[0]);
+  hash_combine(seed, x[1]);
+  hash_combine(seed, x[2]);
+  return seed;
+}
+
+__host__ __device__ inline Eigen::Vector3i calc_voxel_coord(const Eigen::Vector3f& x, float resolution) {
+  Eigen::Vector3i coord = (x.array() / resolution - 0.5).floor().cast<int>();
+  return coord;
+}
+
+  }
+}  // namespace fast_gicp
+
+#endif

src/lidar_localization/fast_gicp/include/fast_gicp/gicp/experimental/fast_gicp_mp.hpp

@@ -0,0 +1,87 @@
+#ifndef FAST_GICP_FAST_GICP_MP_HPP
+#define FAST_GICP_FAST_GICP_MP_HPP
+
+#include <Eigen/Core>
+#include <Eigen/Geometry>
+
+#include <pcl/point_types.h>
+#include <pcl/point_cloud.h>
+#include <pcl/search/kdtree.h>
+#include <pcl/registration/registration.h>
+#include <fast_gicp/gicp/gicp_settings.hpp>
+
+namespace fast_gicp {
+
+template<typename PointSource, typename PointTarget>
+class FastGICPMultiPoints : public pcl::Registration<PointSource, PointTarget, float> {
+public:
+  using Scalar = float;
+  using Matrix4 = typename pcl::Registration<PointSource, PointTarget, Scalar>::Matrix4;
+
+  using PointCloudSource = typename pcl::Registration<PointSource, PointTarget, Scalar>::PointCloudSource;
+  using PointCloudSourcePtr = typename PointCloudSource::Ptr;
+  using PointCloudSourceConstPtr = typename PointCloudSource::ConstPtr;
+
+  using PointCloudTarget = typename pcl::Registration<PointSource, PointTarget, Scalar>::PointCloudTarget;
+  using PointCloudTargetPtr = typename PointCloudTarget::Ptr;
+  using PointCloudTargetConstPtr = typename PointCloudTarget::ConstPtr;
+
+  using pcl::Registration<PointSource, PointTarget, Scalar>::reg_name_;
+  using pcl::Registration<PointSource, PointTarget, Scalar>::input_;
+  using pcl::Registration<PointSource, PointTarget, Scalar>::target_;
+
+  using pcl::Registration<PointSource, PointTarget, Scalar>::nr_iterations_;
+  using pcl::Registration<PointSource, PointTarget, Scalar>::max_iterations_;
+  using pcl::Registration<PointSource, PointTarget, Scalar>::final_transformation_;
+  using pcl::Registration<PointSource, PointTarget, Scalar>::transformation_epsilon_;
+  using pcl::Registration<PointSource, PointTarget, Scalar>::converged_;
+  using pcl::Registration<PointSource, PointTarget, Scalar>::corr_dist_threshold_;
+
+  FastGICPMultiPoints();
+  virtual ~FastGICPMultiPoints() override;
+
+  void setNumThreads(int n);
+
+  void setRotationEpsilon(double eps);
+
+  void setCorrespondenceRandomness(int k);
+
+  void setRegularizationMethod(RegularizationMethod method);
+
+  virtual void setInputSource(const PointCloudSourceConstPtr& cloud) override;
+
+  virtual void setInputTarget(const PointCloudTargetConstPtr& cloud) override;
+
+protected:
+  virtual void computeTransformation(PointCloudSource& output, const Matrix4& guess) override;
+
+private:
+  bool is_converged(const Eigen::Matrix<float, 6, 1>& delta) const;
+
+  void update_correspondences(const Eigen::Matrix<float, 6, 1>& x);
+
+  Eigen::VectorXf loss_ls(const Eigen::Matrix<float, 6, 1>& x, Eigen::MatrixXf* J) const;
+
+  template<typename PointT>
+  bool calculate_covariances(const pcl::shared_ptr<const pcl::PointCloud<PointT>>& cloud, pcl::search::KdTree<PointT>& kdtree, std::vector<Matrix4, Eigen::aligned_allocator<Matrix4>>& covariances);
+
+private:
+  int num_threads_;
+  int k_correspondences_;
+  double rotation_epsilon_;
+  double neighbor_search_radius_;
+
+  RegularizationMethod regularization_method_;
+
+  pcl::search::KdTree<PointSource> source_kdtree;
+  pcl::search::KdTree<PointTarget> target_kdtree;
+
+  std::vector<Matrix4, Eigen::aligned_allocator<Matrix4>> source_covs;
+  std::vector<Matrix4, Eigen::aligned_allocator<Matrix4>> target_covs;
+
+  std::vector<std::vector<int>> correspondences;
+  std::vector<std::vector<float>> sq_distances;
+};
+}  // namespace fast_gicp
+
+#endif

src/lidar_localization/fast_gicp/include/fast_gicp/gicp/experimental/fast_gicp_mp_impl.hpp

@@ -0,0 +1,280 @@
+#ifndef FAST_GICP_FAST_GICP_MP_IMPL_HPP
+#define FAST_GICP_FAST_GICP_MP_IMPL_HPP
+
+#include <Eigen/Core>
+#include <Eigen/Geometry>
+
+#include <pcl/point_types.h>
+#include <pcl/point_cloud.h>
+#include <pcl/search/kdtree.h>
+#include <pcl/registration/registration.h>
+
+#include <sophus/so3.hpp>
+#include <fast_gicp/so3/so3.hpp>
+#include <fast_gicp/opt/gauss_newton.hpp>
+#include <fast_gicp/gicp/experimental/fast_gicp_mp.hpp>
+
+namespace fast_gicp {
+
+template<typename PointSource, typename PointTarget>
+FastGICPMultiPoints<PointSource, PointTarget>::FastGICPMultiPoints() {
+#ifdef _OPENMP
+  num_threads_ = omp_get_max_threads();
+#else
+  num_threads_ = 1;
+#endif
+
+  k_correspondences_ = 20;
+  reg_name_ = "FastGICPMultiPoints";
+  max_iterations_ = 64;
+  rotation_epsilon_ = 1e-5;
+  transformation_epsilon_ = 1e-5;
+  // corr_dist_threshold_ = 1.0;
+  regularization_method_ = RegularizationMethod::PLANE;
+  corr_dist_threshold_ = std::numeric_limits<float>::max();
+  neighbor_search_radius_ = 0.5;
+}
+
+template<typename PointSource, typename PointTarget>
+FastGICPMultiPoints<PointSource, PointTarget>::~FastGICPMultiPoints() {}
+
+template<typename PointSource, typename PointTarget>
+void FastGICPMultiPoints<PointSource, PointTarget>::setRotationEpsilon(double eps) {
+  rotation_epsilon_ = eps;
+}
+
+template<typename PointSource, typename PointTarget>
+void FastGICPMultiPoints<PointSource, PointTarget>::setNumThreads(int n) {
+  num_threads_ = n;
+
+#ifdef _OPENMP
+  if(n == 0) {
+    num_threads_ = omp_get_max_threads();
+  }
+#endif
+}
+
+template<typename PointSource, typename PointTarget>
+void FastGICPMultiPoints<PointSource, PointTarget>::setCorrespondenceRandomness(int k) {
+  k_correspondences_ = k;
+}
+
+template<typename PointSource, typename PointTarget>
+void FastGICPMultiPoints<PointSource, PointTarget>::setRegularizationMethod(RegularizationMethod method) {
+  regularization_method_ = method;
+}
+
+template<typename PointSource, typename PointTarget>
+void FastGICPMultiPoints<PointSource, PointTarget>::setInputSource(const PointCloudSourceConstPtr& cloud) {
+  pcl::Registration<PointSource, PointTarget, Scalar>::setInputSource(cloud);
+  calculate_covariances(*cloud, source_kdtree, source_covs);
+}
+
+template<typename PointSource, typename PointTarget>
+void FastGICPMultiPoints<PointSource, PointTarget>::setInputTarget(const PointCloudTargetConstPtr& cloud) {
+  pcl::Registration<PointSource, PointTarget, Scalar>::setInputTarget(cloud);
+  calculate_covariances(cloud, target_kdtree, target_covs);
+}
+
+template<typename PointSource, typename PointTarget>
+void FastGICPMultiPoints<PointSource, PointTarget>::computeTransformation(PointCloudSource& output, const Matrix4& guess) {
+  Eigen::Matrix<float, 6, 1> x0;
+  x0.head<3>() = Sophus::SO3f(guess.template block<3, 3>(0, 0)).log();
+  x0.tail<3>() = guess.template block<3, 1>(0, 3);
+
+  // if(x0.head<3>().norm() < 1e-2) {
+  //   x0.head<3>() = (Eigen::Vector3f::Random()).normalized() * 1e-2;
+  // }
+
+  converged_ = false;
+  GaussNewton<double, 6> solver;
+
+  for(int i = 0; i < max_iterations_; i++) {
+    nr_iterations_ = i;
+
+    update_correspondences(x0);
+    Eigen::MatrixXf J;
+    Eigen::VectorXf loss = loss_ls(x0, &J);
+
+    Eigen::Matrix<float, 6, 1> delta = solver.delta(loss.cast<double>(), J.cast<double>()).cast<float>();
+
+    x0.head<3>() = (Sophus::SO3f::exp(-delta.head<3>()) * Sophus::SO3f::exp(x0.head<3>())).log();
+    x0.tail<3>() -= delta.tail<3>();
+
+    if(is_converged(delta)) {
+      converged_ = true;
+      break;
+    }
+  }
+
+  final_transformation_.setIdentity();
+  final_transformation_.template block<3, 3>(0, 0) = Sophus::SO3f::exp(x0.head<3>()).matrix();
+  final_transformation_.template block<3, 1>(0, 3) = x0.tail<3>();
+
+  pcl::transformPointCloud(*input_, output, final_transformation_);
+}
+
+template<typename PointSource, typename PointTarget>
+bool FastGICPMultiPoints<PointSource, PointTarget>::is_converged(const Eigen::Matrix<float, 6, 1>& delta) const {
+  double accum = 0.0;
+  Eigen::Matrix3f R = Sophus::SO3f::exp(delta.head<3>()).matrix() - Eigen::Matrix3f::Identity();
+  Eigen::Vector3f t = delta.tail<3>();
+
+  Eigen::Matrix3f r_delta = 1.0 / rotation_epsilon_ * R.array().abs();
+  Eigen::Vector3f t_delta = 1.0 / transformation_epsilon_ * t.array().abs();
+
+  return std::max(r_delta.maxCoeff(), t_delta.maxCoeff()) < 1;
+}
+
+template<typename PointSource, typename PointTarget>
+void FastGICPMultiPoints<PointSource, PointTarget>::update_correspondences(const Eigen::Matrix<float, 6, 1>& x) {
+  Eigen::Matrix4f trans = Eigen::Matrix4f::Identity();
+  trans.block<3, 3>(0, 0) = Sophus::SO3f::exp(x.head<3>()).matrix();
+  trans.block<3, 1>(0, 3) = x.tail<3>();
+
+  correspondences.resize(input_->size());
+  sq_distances.resize(input_->size());
+
+#pragma omp parallel for num_threads(num_threads_)
+  for(int i = 0; i < input_->size(); i++) {
+    PointTarget pt;
+    pt.getVector4fMap() = trans * input_->at(i).getVector4fMap();
+
+    std::vector<int> k_indices;
+    std::vector<float> k_sq_dists;
+    target_kdtree.radiusSearch(pt, neighbor_search_radius_, k_indices, k_sq_dists);
+
+    if(k_indices.empty()) {
+      //  target_kdtree.nearestKSearch(pt, 1, k_indices, k_sq_dists);
+    }
+
+    correspondences[i] = k_indices;
+    sq_distances[i] = k_sq_dists;
+  }
+}
+
+template<typename PointSource, typename PointTarget>
+Eigen::VectorXf FastGICPMultiPoints<PointSource, PointTarget>::loss_ls(const Eigen::Matrix<float, 6, 1>& x, Eigen::MatrixXf* J) const {
+  Eigen::Matrix4f trans = Eigen::Matrix4f::Identity();
+  trans.block<3, 3>(0, 0) = Sophus::SO3f::exp(x.head<3>()).matrix();
+  trans.block<3, 1>(0, 3) = x.tail<3>();
+
+  std::vector<Eigen::Vector3f, Eigen::aligned_allocator<Eigen::Vector3f>> losses(input_->size());
+  // use row-major arrangement for ease of repacking
+  std::vector<Eigen::Matrix<float, 3, 6, Eigen::RowMajor>, Eigen::aligned_allocator<Eigen::Matrix<float, 3, 6, Eigen::RowMajor>>> Js(input_->size());
+
+  std::atomic_int count(0);
+
+#pragma omp parallel for num_threads(num_threads_)
+  for(int i = 0; i < correspondences.size(); i++) {
+    int source_index = i;
+    const auto& mean_A = input_->at(source_index).getVector4fMap();
+    const auto& cov_A = source_covs[source_index];
+
+    const auto& k_indices = correspondences[i];
+    const auto& k_sq_dists = sq_distances[i];
+    if(k_indices.empty()) {
+      continue;
+    }
+
+    double sum_w = 0.0;
+    Eigen::Vector4d sum_mean_B = Eigen::Vector4d::Zero();
+    Eigen::Matrix4d sum_cov_B = Eigen::Matrix4d::Zero();
+    for(int j = 0; j < k_indices.size(); j++) {
+      double w = 1 - std::sqrt(k_sq_dists[j]) / neighbor_search_radius_;
+      w = std::max(1e-3, std::min(1.0, w));
+      sum_w += w;
+
+      int target_index = k_indices[j];
+
+      sum_mean_B += w * target_->at(target_index).getVector4fMap().template cast<double>();
+      sum_cov_B += w * target_covs[target_index].template cast<double>();
+    }
+
+    Eigen::Vector4f mean_B = (sum_mean_B / sum_w).template cast<float>();
+    Eigen::Matrix4f cov_B = (sum_cov_B / sum_w).template cast<float>();
+
+    Eigen::Vector4f transed_mean_A = trans * mean_A;
+    Eigen::Vector4f d = mean_B - transed_mean_A;
+    Eigen::Matrix4f RCR = cov_B + trans * cov_A * trans.transpose();
+    RCR(3, 3) = 1;
+
+    Eigen::Matrix4f RCR_inv = RCR.inverse();
+    Eigen::Vector4f RCRd = RCR_inv * d;
+
+    Eigen::Matrix<float, 4, 6> dtdx0 = Eigen::Matrix<float, 4, 6>::Zero();
+    dtdx0.block<3, 3>(0, 0) = skew(transed_mean_A.head<3>());
+    dtdx0.block<3, 3>(0, 3) = -Eigen::Matrix3f::Identity();
+
+    Eigen::Matrix<float, 4, 6> jlossexp = RCR_inv * dtdx0;
+
+    int n = count++;
+    losses[n] = RCRd.head<3>();
+    Js[n] = jlossexp.block<3, 6>(0, 0);
+  }
+
+  int final_size = count;
+  Eigen::VectorXf loss = Eigen::Map<Eigen::VectorXf>(losses.front().data(), final_size * 3);
+  *J = Eigen::Map<Eigen::MatrixXf>(Js.front().data(), 6, final_size * 3).transpose();
+
+  return loss;
+}
+
+template<typename PointSource, typename PointTarget>
+template<typename PointT>
+bool FastGICPMultiPoints<PointSource, PointTarget>::calculate_covariances(const pcl::shared_ptr<const pcl::PointCloud<PointT>>& cloud, pcl::search::KdTree<PointT>& kdtree, std::vector<Matrix4, Eigen::aligned_allocator<Matrix4>>& covariances) {
+  kdtree.setInputCloud(cloud);
+  covariances.resize(cloud->size());
+
+#pragma omp parallel for num_threads(num_threads_)
+  for(int i = 0; i < cloud->size(); i++) {
+    std::vector<int> k_indices;
+    std::vector<float> k_sq_distances;
+    kdtree.nearestKSearch(cloud->at(i), k_correspondences_, k_indices, k_sq_distances);
+
+    Eigen::Matrix<float, 4, -1> data(4, k_correspondences_);
+
+    for(int j = 0; j < k_indices.size(); j++) {
+      data.col(j) = cloud->at(k_indices[j]).getVector4fMap();
+    }
+
+    data.colwise() -= data.rowwise().mean().eval();
+    Eigen::Matrix4f cov = data * data.transpose();
+
+    if(regularization_method_ == RegularizationMethod::FROBENIUS) {
+      double lambda = 1e-3;
+      Eigen::Matrix3d C = cov.block<3, 3>(0, 0).cast<double>() + lambda * Eigen::Matrix3d::Identity();
+      Eigen::Matrix3d C_inv = C.inverse();
+      covariances[i].setZero();
+      covariances[i].template block<3, 3>(0, 0) = (C_inv / C_inv.norm()).inverse().cast<float>();
+    } else {
+      Eigen::JacobiSVD<Eigen::Matrix3f> svd(cov.block<3, 3>(0, 0), Eigen::ComputeFullU | Eigen::ComputeFullV);
+      Eigen::Vector3f values;
+
+      switch(regularization_method_) {
+        default:
+          std::cerr << "here must not be reached" << std::endl;
+          abort();
+        case RegularizationMethod::PLANE:
+          values = Eigen::Vector3f(1, 1, 1e-3);
+          break;
+        case RegularizationMethod::MIN_EIG:
+          values = svd.singularValues().array().max(1e-3);
+          break;
+        case RegularizationMethod::NORMALIZED_MIN_EIG:
+          values = svd.singularValues() / svd.singularValues().maxCoeff();
+          values = values.array().max(1e-3);
+          break;
+      }
+
+      covariances[i].setZero();
+      covariances[i].template block<3, 3>(0, 0) = svd.matrixU() * values.asDiagonal() * svd.matrixV().transpose();
+    }
+  }
+
+  return true;
+}
+
+}  // namespace fast_gicp
+
+#endif

src/lidar_localization/fast_gicp/include/fast_gicp/gicp/fast_gicp.hpp

@@ -0,0 +1,103 @@
+#ifndef FAST_GICP_FAST_GICP_HPP
+#define FAST_GICP_FAST_GICP_HPP
+
+#include <Eigen/Core>
+#include <Eigen/Geometry>
+
+#include <pcl/point_types.h>
+#include <pcl/point_cloud.h>
+#include <pcl/search/kdtree.h>
+#include <pcl/registration/registration.h>
+#include <fast_gicp/gicp/lsq_registration.hpp>
+#include <fast_gicp/gicp/gicp_settings.hpp>
+
+namespace fast_gicp {
+
+/**
+ * @brief Fast GICP algorithm optimized for multi threading with OpenMP
+ */
+template<typename PointSource, typename PointTarget>
+class FastGICP : public LsqRegistration<PointSource, PointTarget> {
+public:
+  using Scalar = float;
+  using Matrix4 = typename pcl::Registration<PointSource, PointTarget, Scalar>::Matrix4;
+
+  using PointCloudSource = typename pcl::Registration<PointSource, PointTarget, Scalar>::PointCloudSource;
+  using PointCloudSourcePtr = typename PointCloudSource::Ptr;
+  using PointCloudSourceConstPtr = typename PointCloudSource::ConstPtr;
+
+  using PointCloudTarget = typename pcl::Registration<PointSource, PointTarget, Scalar>::PointCloudTarget;
+  using PointCloudTargetPtr = typename PointCloudTarget::Ptr;
+  using PointCloudTargetConstPtr = typename PointCloudTarget::ConstPtr;
+
+#if PCL_VERSION >= PCL_VERSION_CALC(1, 10, 0)
+  using Ptr = pcl::shared_ptr<FastGICP<PointSource, PointTarget>>;
+  using ConstPtr = pcl::shared_ptr<const FastGICP<PointSource, PointTarget>>;
+#else
+  using Ptr = boost::shared_ptr<FastGICP<PointSource, PointTarget>>;
+  using ConstPtr = boost::shared_ptr<const FastGICP<PointSource, PointTarget>>;
+#endif
+
+protected:
+  using pcl::Registration<PointSource, PointTarget, Scalar>::reg_name_;
+  using pcl::Registration<PointSource, PointTarget, Scalar>::input_;
+  using pcl::Registration<PointSource, PointTarget, Scalar>::target_;
+  using pcl::Registration<PointSource, PointTarget, Scalar>::corr_dist_threshold_;
+
+public:
+  FastGICP();
+  virtual ~FastGICP() override;
+
+  void setNumThreads(int n);
+  void setCorrespondenceRandomness(int k);
+  void setRegularizationMethod(RegularizationMethod method);
+
+  virtual void swapSourceAndTarget() override;
+  virtual void clearSource() override;
+  virtual void clearTarget() override;
+
+  virtual void setInputSource(const PointCloudSourceConstPtr& cloud) override;
+  virtual void setSourceCovariances(const std::vector<Eigen::Matrix4d, Eigen::aligned_allocator<Eigen::Matrix4d>>& covs);
+  virtual void setInputTarget(const PointCloudTargetConstPtr& cloud) override;
+  virtual void setTargetCovariances(const std::vector<Eigen::Matrix4d, Eigen::aligned_allocator<Eigen::Matrix4d>>& covs);
+
+  const std::vector<Eigen::Matrix4d, Eigen::aligned_allocator<Eigen::Matrix4d>>& getSourceCovariances() const {
+    return source_covs_;
+  }
+
+  const std::vector<Eigen::Matrix4d, Eigen::aligned_allocator<Eigen::Matrix4d>>& getTargetCovariances() const {
+    return target_covs_;
+  }
+
+protected:
+  virtual void computeTransformation(PointCloudSource& output, const Matrix4& guess) override;
+
+  virtual void update_correspondences(const Eigen::Isometry3d& trans);
+
+  virtual double linearize(const Eigen::Isometry3d& trans, Eigen::Matrix<double, 6, 6>* H, Eigen::Matrix<double, 6, 1>* b) override;
+
+  virtual double compute_error(const Eigen::Isometry3d& trans) override;
+
+  template<typename PointT>
+  bool calculate_covariances(const typename pcl::PointCloud<PointT>::ConstPtr& cloud, pcl::search::KdTree<PointT>& kdtree, std::vector<Eigen::Matrix4d, Eigen::aligned_allocator<Eigen::Matrix4d>>& covariances);
+
+protected:
+  int num_threads_;
+  int k_correspondences_;
+
+  RegularizationMethod regularization_method_;
+
+  std::shared_ptr<pcl::search::KdTree<PointSource>> source_kdtree_;
+  std::shared_ptr<pcl::search::KdTree<PointTarget>> target_kdtree_;
+
+  std::vector<Eigen::Matrix4d, Eigen::aligned_allocator<Eigen::Matrix4d>> source_covs_;
+  std::vector<Eigen::Matrix4d, Eigen::aligned_allocator<Eigen::Matrix4d>> target_covs_;
+
+  std::vector<Eigen::Matrix4d, Eigen::aligned_allocator<Eigen::Matrix4d>> mahalanobis_;
+
+  std::vector<int> correspondences_;
+  std::vector<float> sq_distances_;
+};
+}  // namespace fast_gicp
+
+#endif

src/lidar_localization/fast_gicp/include/fast_gicp/gicp/fast_gicp_st.hpp

@@ -0,0 +1,68 @@
+#ifndef FAST_GICP_FAST_GICP_ST_HPP
+#define FAST_GICP_FAST_GICP_ST_HPP
+
+#include <Eigen/Core>
+#include <Eigen/Geometry>
+
+#include <pcl/point_types.h>
+#include <pcl/point_cloud.h>
+#include <pcl/search/kdtree.h>
+#include <pcl/registration/registration.h>
+#include <fast_gicp/gicp/fast_gicp.hpp>
+#include <fast_gicp/gicp/gicp_settings.hpp>
+
+namespace fast_gicp {
+
+/**
+ * @brief Fast GICP algorithm optimized for single threading
+ */
+template<typename PointSource, typename PointTarget>
+class FastGICPSingleThread : public FastGICP<PointSource, PointTarget> {
+public:
+  using Scalar = float;
+  using Matrix4 = typename pcl::Registration<PointSource, PointTarget, Scalar>::Matrix4;
+
+  using PointCloudSource = typename pcl::Registration<PointSource, PointTarget, Scalar>::PointCloudSource;
+  using PointCloudSourcePtr = typename PointCloudSource::Ptr;
+  using PointCloudSourceConstPtr = typename PointCloudSource::ConstPtr;
+
+#if PCL_VERSION >= PCL_VERSION_CALC(1, 10, 0)
+  using Ptr = pcl::shared_ptr<FastGICPSingleThread<PointSource, PointTarget>>;
+  using ConstPtr = pcl::shared_ptr<const FastGICPSingleThread<PointSource, PointTarget>>;
+#else
+  using Ptr = boost::shared_ptr<FastGICPSingleThread<PointSource, PointTarget>>;
+  using ConstPtr = boost::shared_ptr<const FastGICPSingleThread<PointSource, PointTarget>>;
+#endif
+
+protected:
+  using pcl::Registration<PointSource, PointTarget, Scalar>::input_;
+  using pcl::Registration<PointSource, PointTarget, Scalar>::target_;
+
+  using FastGICP<PointSource, PointTarget>::target_kdtree_;
+  using FastGICP<PointSource, PointTarget>::correspondences_;
+  using FastGICP<PointSource, PointTarget>::sq_distances_;
+  using FastGICP<PointSource, PointTarget>::source_covs_;
+  using FastGICP<PointSource, PointTarget>::target_covs_;
+  using FastGICP<PointSource, PointTarget>::mahalanobis_;
+
+public:
+  FastGICPSingleThread();
+  virtual ~FastGICPSingleThread() override;
+
+protected:
+  virtual void computeTransformation(PointCloudSource& output, const Matrix4& guess) override;
+
+private:
+  virtual void update_correspondences(const Eigen::Isometry3d& trans) override;
+
+  virtual double linearize(const Eigen::Isometry3d& trans, Eigen::Matrix<double, 6, 6>* H = nullptr, Eigen::Matrix<double, 6, 1>* b = nullptr) override;
+
+  virtual double compute_error(const Eigen::Isometry3d& trans) override;
+
+private:
+  std::vector<float> second_sq_distances_;
+  std::vector<Eigen::Vector4f, Eigen::aligned_allocator<Eigen::Vector4f>> anchors_;
+};
+}  // namespace fast_gicp
+
+#endif

src/lidar_localization/fast_gicp/include/fast_gicp/gicp/fast_vgicp.hpp

@@ -0,0 +1,84 @@
+#ifndef FAST_GICP_FAST_VGICP_HPP
+#define FAST_GICP_FAST_VGICP_HPP
+
+#include <unordered_map>
+
+#include <Eigen/Core>
+#include <Eigen/Geometry>
+
+#include <pcl/point_types.h>
+#include <pcl/point_cloud.h>
+#include <pcl/search/kdtree.h>
+#include <pcl/registration/registration.h>
+
+#include <fast_gicp/gicp/gicp_settings.hpp>
+#include <fast_gicp/gicp/fast_gicp.hpp>
+#include <fast_gicp/gicp/fast_vgicp_voxel.hpp>
+
+namespace fast_gicp {
+
+/**
+ * @brief Fast Voxelized GICP algorithm boosted with OpenMP
+ */
+template<typename PointSource, typename PointTarget>
+class FastVGICP : public FastGICP<PointSource, PointTarget> {
+public:
+  using Scalar = float;
+  using Matrix4 = typename pcl::Registration<PointSource, PointTarget, Scalar>::Matrix4;
+
+  using PointCloudSource = typename pcl::Registration<PointSource, PointTarget, Scalar>::PointCloudSource;
+  using PointCloudSourcePtr = typename PointCloudSource::Ptr;
+  using PointCloudSourceConstPtr = typename PointCloudSource::ConstPtr;
+
+  using PointCloudTarget = typename pcl::Registration<PointSource, PointTarget, Scalar>::PointCloudTarget;
+  using PointCloudTargetPtr = typename PointCloudTarget::Ptr;
+  using PointCloudTargetConstPtr = typename PointCloudTarget::ConstPtr;
+
+#if PCL_VERSION >= PCL_VERSION_CALC(1, 10, 0)
+  using Ptr = pcl::shared_ptr<FastVGICP<PointSource, PointTarget>>;
+  using ConstPtr = pcl::shared_ptr<const FastVGICP<PointSource, PointTarget>>;
+#else
+  using Ptr = boost::shared_ptr<FastVGICP<PointSource, PointTarget>>;
+  using ConstPtr = boost::shared_ptr<const FastVGICP<PointSource, PointTarget>>;
+#endif
+
+protected:
+  using pcl::Registration<PointSource, PointTarget, Scalar>::input_;
+  using pcl::Registration<PointSource, PointTarget, Scalar>::target_;
+
+  using FastGICP<PointSource, PointTarget>::num_threads_;
+  using FastGICP<PointSource, PointTarget>::source_kdtree_;
+  using FastGICP<PointSource, PointTarget>::target_kdtree_;
+  using FastGICP<PointSource, PointTarget>::source_covs_;
+  using FastGICP<PointSource, PointTarget>::target_covs_;
+
+public:
+  FastVGICP();
+  virtual ~FastVGICP() override;
+
+  void setResolution(double resolution);
+  void setVoxelAccumulationMode(VoxelAccumulationMode mode);
+  void setNeighborSearchMethod(NeighborSearchMethod method);
+
+  virtual void swapSourceAndTarget() override;
+  virtual void setInputTarget(const PointCloudTargetConstPtr& cloud) override;
+
+protected:
+  virtual void computeTransformation(PointCloudSource& output, const Matrix4& guess) override;
+  virtual void update_correspondences(const Eigen::Isometry3d& trans) override;
+  virtual double linearize(const Eigen::Isometry3d& trans, Eigen::Matrix<double, 6, 6>* H = nullptr, Eigen::Matrix<double, 6, 1>* b = nullptr) override;
+  virtual double compute_error(const Eigen::Isometry3d& trans) override;
+
+protected:
+  double voxel_resolution_;
+  NeighborSearchMethod search_method_;
+  VoxelAccumulationMode voxel_mode_;
+
+  std::unique_ptr<GaussianVoxelMap<PointTarget>> voxelmap_;
+
+  std::vector<std::pair<int, GaussianVoxel::Ptr>> voxel_correspondences_;
+  std::vector<Eigen::Matrix4d, Eigen::aligned_allocator<Eigen::Matrix4d>> voxel_mahalanobis_;
+};
+}  // namespace fast_gicp
+
+#endif

src/lidar_localization/fast_gicp/include/fast_gicp/gicp/fast_vgicp_cuda.hpp

@@ -0,0 +1,89 @@
+#ifndef FAST_GICP_FAST_VGICP_CUDA_HPP
+#define FAST_GICP_FAST_VGICP_CUDA_HPP
+
+#include <Eigen/Core>
+#include <Eigen/Geometry>
+
+#include <pcl/point_types.h>
+#include <pcl/point_cloud.h>
+#include <pcl/search/kdtree.h>
+#include <pcl/registration/registration.h>
+
+#include <fast_gicp/gicp/gicp_settings.hpp>
+#include <fast_gicp/gicp/lsq_registration.hpp>
+
+namespace fast_gicp {
+
+namespace cuda {
+class FastVGICPCudaCore;
+}
+
+enum class NearestNeighborMethod { CPU_PARALLEL_KDTREE, GPU_BRUTEFORCE, GPU_RBF_KERNEL };
+
+/**
+ * @brief Fast Voxelized GICP algorithm boosted with CUDA
+ */
+template<typename PointSource, typename PointTarget>
+class FastVGICPCuda : public LsqRegistration<PointSource, PointTarget> {
+public:
+  using Scalar = float;
+  using Matrix4 = typename pcl::Registration<PointSource, PointTarget, Scalar>::Matrix4;
+
+  using PointCloudSource = typename pcl::Registration<PointSource, PointTarget, Scalar>::PointCloudSource;
+  using PointCloudSourcePtr = typename PointCloudSource::Ptr;
+  using PointCloudSourceConstPtr = typename PointCloudSource::ConstPtr;
+
+  using PointCloudTarget = typename pcl::Registration<PointSource, PointTarget, Scalar>::PointCloudTarget;
+  using PointCloudTargetPtr = typename PointCloudTarget::Ptr;
+  using PointCloudTargetConstPtr = typename PointCloudTarget::ConstPtr;
+
+#if PCL_VERSION >= PCL_VERSION_CALC(1, 10, 0)
+  using Ptr = pcl::shared_ptr<FastVGICPCuda<PointSource, PointTarget>>;
+  using ConstPtr = pcl::shared_ptr<const FastVGICPCuda<PointSource, PointTarget>>;
+#else
+  using Ptr = boost::shared_ptr<FastVGICPCuda<PointSource, PointTarget>>;
+  using ConstPtr = boost::shared_ptr<const FastVGICPCuda<PointSource, PointTarget>>;
+#endif
+
+protected:
+  using pcl::Registration<PointSource, PointTarget, Scalar>::input_;
+  using pcl::Registration<PointSource, PointTarget, Scalar>::target_;
+
+public:
+  FastVGICPCuda();
+  virtual ~FastVGICPCuda() override;
+
+  void setCorrespondenceRandomness(int k);
+  void setResolution(double resolution);
+  void setKernelWidth(double kernel_width, double max_dist = -1.0);
+  void setRegularizationMethod(RegularizationMethod method);
+  void setNeighborSearchMethod(NeighborSearchMethod method, double radius = -1.0);
+  void setNearestNeighborSearchMethod(NearestNeighborMethod method);
+
+  virtual void swapSourceAndTarget() override;
+  virtual void clearSource() override;
+  virtual void clearTarget() override;
+
+  virtual void setInputSource(const PointCloudSourceConstPtr& cloud) override;
+  virtual void setInputTarget(const PointCloudTargetConstPtr& cloud) override;
+
+protected:
+  virtual void computeTransformation(PointCloudSource& output, const Matrix4& guess) override;
+  virtual double linearize(const Eigen::Isometry3d& trans, Eigen::Matrix<double, 6, 6>* H = nullptr, Eigen::Matrix<double, 6, 1>* b = nullptr) override;
+  virtual double compute_error(const Eigen::Isometry3d& trans) override;
+
+  template<typename PointT>
+  std::vector<int> find_neighbors_parallel_kdtree(int k, typename pcl::PointCloud<PointT>::ConstPtr cloud) const;
+
+private:
+  int k_correspondences_;
+  double voxel_resolution_;
+  RegularizationMethod regularization_method_;
+  NearestNeighborMethod neighbor_search_method_;
+
+  std::unique_ptr<cuda::FastVGICPCudaCore> vgicp_cuda_;
+};
+
+}  // namespace fast_gicp
+
+#endif

src/lidar_localization/fast_gicp/include/fast_gicp/gicp/fast_vgicp_voxel.hpp

@@ -0,0 +1,186 @@
+#ifndef FAST_GICP_FAST_VGICP_VOXEL_HPP
+#define FAST_GICP_FAST_VGICP_VOXEL_HPP
+
+#include <unordered_map>
+#include <boost/functional/hash.hpp>
+#include <fast_gicp/gicp/gicp_settings.hpp>
+
+namespace fast_gicp {
+
+static std::vector<Eigen::Vector3i, Eigen::aligned_allocator<Eigen::Vector3i>> neighbor_offsets(NeighborSearchMethod search_method) {
+  switch(search_method) {
+      // clang-format off
+    default:
+      std::cerr << "unsupported neighbor search method" << std::endl;
+      abort();
+    case NeighborSearchMethod::DIRECT1:
+      return std::vector<Eigen::Vector3i, Eigen::aligned_allocator<Eigen::Vector3i>>{
+        Eigen::Vector3i(0, 0, 0)
+      };
+    case NeighborSearchMethod::DIRECT7:
+      return std::vector<Eigen::Vector3i, Eigen::aligned_allocator<Eigen::Vector3i>>{
+        Eigen::Vector3i(0, 0, 0),
+        Eigen::Vector3i(1, 0, 0),
+        Eigen::Vector3i(-1, 0, 0),
+        Eigen::Vector3i(0, 1, 0),
+        Eigen::Vector3i(0, -1, 0),
+        Eigen::Vector3i(0, 0, 1),
+        Eigen::Vector3i(0, 0, -1)
+      };
+    case NeighborSearchMethod::DIRECT27:
+      break;
+      // clang-format on
+  }
+
+  std::vector<Eigen::Vector3i, Eigen::aligned_allocator<Eigen::Vector3i>> offsets27;
+  for(int i = 0; i < 3; i++) {
+    for(int j = 0; j < 3; j++) {
+      for(int k = 0; k < 3; k++) {
+        offsets27.push_back(Eigen::Vector3i(i - 1, j - 1, k - 1));
+      }
+    }
+  }
+  return offsets27;
+}
+
+class Vector3iHash {
+public:
+  size_t operator()(const Eigen::Vector3i& x) const {
+    size_t seed = 0;
+    boost::hash_combine(seed, x[0]);
+    boost::hash_combine(seed, x[1]);
+    boost::hash_combine(seed, x[2]);
+    return seed;
+  }
+};
+
+struct GaussianVoxel {
+public:
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+  using Ptr = std::shared_ptr<GaussianVoxel>;
+
+  GaussianVoxel() {
+    num_points = 0;
+    mean.setZero();
+    cov.setZero();
+  }
+  virtual ~GaussianVoxel() {}
+
+  virtual void append(const Eigen::Vector4d& mean_, const Eigen::Matrix4d& cov_) = 0;
+
+  virtual void finalize() = 0;
+
+public:
+  int num_points;
+  Eigen::Vector4d mean;
+  Eigen::Matrix4d cov;
+};
+
+struct MultiplicativeGaussianVoxel : GaussianVoxel {
+public:
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+  MultiplicativeGaussianVoxel() : GaussianVoxel() {}
+  virtual ~MultiplicativeGaussianVoxel() {}
+
+  virtual void append(const Eigen::Vector4d& mean_, const Eigen::Matrix4d& cov_) override {
+    num_points++;
+    Eigen::Matrix4d cov_inv = cov_;
+    cov_inv(3, 3) = 1;
+    cov_inv = cov_inv.inverse().eval();
+
+    cov += cov_inv;
+    mean += cov_inv * mean_;
+  }
+
+  virtual void finalize() override {
+    cov(3, 3) = 1;
+    mean[3] = 1;
+
+    cov = cov.inverse().eval();
+    mean = (cov * mean).eval();
+  }
+};
+
+struct AdditiveGaussianVoxel : GaussianVoxel {
+public:
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+  AdditiveGaussianVoxel() : GaussianVoxel() {}
+  virtual ~AdditiveGaussianVoxel() {}
+
+  virtual void append(const Eigen::Vector4d& mean_, const Eigen::Matrix4d& cov_) override {
+    num_points++;
+    mean += mean_;
+    cov += cov_;
+  }
+
+  virtual void finalize() override {
+    mean /= num_points;
+    cov /= num_points;
+  }
+};
+
+template<typename PointT>
+class GaussianVoxelMap {
+public:
+  GaussianVoxelMap(double resolution, VoxelAccumulationMode mode) : voxel_resolution_(resolution), voxel_mode_(mode) {}
+
+  void create_voxelmap(const pcl::PointCloud<PointT>& cloud, const std::vector<Eigen::Matrix4d, Eigen::aligned_allocator<Eigen::Matrix4d>>& covs) {
+    voxels_.clear();
+    for(int i = 0; i < cloud.size(); i++) {
+      Eigen::Vector3i coord = voxel_coord(cloud.at(i).getVector4fMap().template cast<double>());
+
+      auto found = voxels_.find(coord);
+      if(found == voxels_.end()) {
+        GaussianVoxel::Ptr voxel;
+        switch(voxel_mode_) {
+          case VoxelAccumulationMode::ADDITIVE:
+          case VoxelAccumulationMode::ADDITIVE_WEIGHTED:
+            voxel = std::shared_ptr<AdditiveGaussianVoxel>(new AdditiveGaussianVoxel);
+            break;
+          case VoxelAccumulationMode::MULTIPLICATIVE:
+            voxel = std::shared_ptr<MultiplicativeGaussianVoxel>(new MultiplicativeGaussianVoxel);
+            break;
+        }
+        found = voxels_.insert(found, std::make_pair(coord, voxel));
+      }
+
+      auto& voxel = found->second;
+      voxel->append(cloud.at(i).getVector4fMap().template cast<double>(), covs[i]);
+    }
+
+    for(auto& voxel : voxels_) {
+      voxel.second->finalize();
+    }
+  }
+
+  Eigen::Vector3i voxel_coord(const Eigen::Vector4d& x) const {
+    return (x.array() / voxel_resolution_ - 0.5).floor().template cast<int>().template head<3>();
+  }
+
+  Eigen::Vector4d voxel_origin(const Eigen::Vector3i& coord) const {
+    Eigen::Vector3d origin = (coord.template cast<double>().array() + 0.5) * voxel_resolution_;
+    return Eigen::Vector4d(origin[0], origin[1], origin[2], 1.0f);
+  }
+
+  GaussianVoxel::Ptr lookup_voxel(const Eigen::Vector3i& coord) const {
+    auto found = voxels_.find(coord);
+    if(found == voxels_.end()) {
+      return nullptr;
+    }
+
+    return found->second;
+  }
+
+private:
+  double voxel_resolution_;
+  VoxelAccumulationMode voxel_mode_;
+
+  using VoxelMap = std::unordered_map<Eigen::Vector3i, GaussianVoxel::Ptr, Vector3iHash, std::equal_to<Eigen::Vector3i>, Eigen::aligned_allocator<std::pair<const Eigen::Vector3i, GaussianVoxel::Ptr>>>;
+  VoxelMap voxels_;
+};
+
+}  // namespace fast_gicp
+
+#endif

src/lidar_localization/fast_gicp/include/fast_gicp/gicp/gicp_settings.hpp

@@ -0,0 +1,13 @@
+#ifndef FAST_GICP_GICP_SETTINGS_HPP
+#define FAST_GICP_GICP_SETTINGS_HPP
+
+namespace fast_gicp {
+
+enum class RegularizationMethod { NONE, MIN_EIG, NORMALIZED_MIN_EIG, PLANE, FROBENIUS };
+
+enum class NeighborSearchMethod { DIRECT27, DIRECT7, DIRECT1, /* supported on only VGICP_CUDA */ DIRECT_RADIUS };
+
+enum class VoxelAccumulationMode { ADDITIVE, ADDITIVE_WEIGHTED, MULTIPLICATIVE };
+}
+
+#endif

src/lidar_localization/fast_gicp/include/fast_gicp/gicp/impl/fast_gicp_impl.hpp

@@ -0,0 +1,302 @@
+#ifndef FAST_GICP_FAST_GICP_IMPL_HPP
+#define FAST_GICP_FAST_GICP_IMPL_HPP
+
+#include <fast_gicp/so3/so3.hpp>
+
+namespace fast_gicp {
+
+template <typename PointSource, typename PointTarget>
+FastGICP<PointSource, PointTarget>::FastGICP() {
+#ifdef _OPENMP
+  num_threads_ = omp_get_max_threads();
+#else
+  num_threads_ = 1;
+#endif
+
+  k_correspondences_ = 20;
+  reg_name_ = "FastGICP";
+  corr_dist_threshold_ = std::numeric_limits<float>::max();
+
+  regularization_method_ = RegularizationMethod::PLANE;
+  source_kdtree_.reset(new pcl::search::KdTree<PointSource>);
+  target_kdtree_.reset(new pcl::search::KdTree<PointTarget>);
+}
+
+template <typename PointSource, typename PointTarget>
+FastGICP<PointSource, PointTarget>::~FastGICP() {}
+
+template <typename PointSource, typename PointTarget>
+void FastGICP<PointSource, PointTarget>::setNumThreads(int n) {
+  num_threads_ = n;
+
+#ifdef _OPENMP
+  if (n == 0) {
+    num_threads_ = omp_get_max_threads();
+  }
+#endif
+}
+
+template <typename PointSource, typename PointTarget>
+void FastGICP<PointSource, PointTarget>::setCorrespondenceRandomness(int k) {
+  k_correspondences_ = k;
+}
+
+template <typename PointSource, typename PointTarget>
+void FastGICP<PointSource, PointTarget>::setRegularizationMethod(RegularizationMethod method) {
+  regularization_method_ = method;
+}
+
+template <typename PointSource, typename PointTarget>
+void FastGICP<PointSource, PointTarget>::swapSourceAndTarget() {
+  input_.swap(target_);
+  source_kdtree_.swap(target_kdtree_);
+  source_covs_.swap(target_covs_);
+
+  correspondences_.clear();
+  sq_distances_.clear();
+}
+
+template <typename PointSource, typename PointTarget>
+void FastGICP<PointSource, PointTarget>::clearSource() {
+  input_.reset();
+  source_covs_.clear();
+}
+
+template <typename PointSource, typename PointTarget>
+void FastGICP<PointSource, PointTarget>::clearTarget() {
+  target_.reset();
+  target_covs_.clear();
+}
+
+template <typename PointSource, typename PointTarget>
+void FastGICP<PointSource, PointTarget>::setInputSource(const PointCloudSourceConstPtr& cloud) {
+  if (input_ == cloud) {
+    return;
+  }
+
+  pcl::Registration<PointSource, PointTarget, Scalar>::setInputSource(cloud);
+  source_kdtree_->setInputCloud(cloud);
+  source_covs_.clear();
+}
+
+template <typename PointSource, typename PointTarget>
+void FastGICP<PointSource, PointTarget>::setInputTarget(const PointCloudTargetConstPtr& cloud) {
+  if (target_ == cloud) {
+    return;
+  }
+  pcl::Registration<PointSource, PointTarget, Scalar>::setInputTarget(cloud);
+  target_kdtree_->setInputCloud(cloud);
+  target_covs_.clear();
+}
+
+template <typename PointSource, typename PointTarget>
+void FastGICP<PointSource, PointTarget>::setSourceCovariances(const std::vector<Eigen::Matrix4d, Eigen::aligned_allocator<Eigen::Matrix4d>>& covs) {
+  source_covs_ = covs;
+}
+
+template <typename PointSource, typename PointTarget>
+void FastGICP<PointSource, PointTarget>::setTargetCovariances(const std::vector<Eigen::Matrix4d, Eigen::aligned_allocator<Eigen::Matrix4d>>& covs) {
+  target_covs_ = covs;
+}
+
+template <typename PointSource, typename PointTarget>
+void FastGICP<PointSource, PointTarget>::computeTransformation(PointCloudSource& output, const Matrix4& guess) {
+  if (source_covs_.size() != input_->size()) {
+    calculate_covariances(input_, *source_kdtree_, source_covs_);
+  }
+  if (target_covs_.size() != target_->size()) {
+    calculate_covariances(target_, *target_kdtree_, target_covs_);
+  }
+
+  LsqRegistration<PointSource, PointTarget>::computeTransformation(output, guess);
+}
+
+template <typename PointSource, typename PointTarget>
+void FastGICP<PointSource, PointTarget>::update_correspondences(const Eigen::Isometry3d& trans) {
+  assert(source_covs_.size() == input_->size());
+  assert(target_covs_.size() == target_->size());
+
+  Eigen::Isometry3f trans_f = trans.cast<float>();
+
+  correspondences_.resize(input_->size());
+  sq_distances_.resize(input_->size());
+  mahalanobis_.resize(input_->size());
+
+  std::vector<int> k_indices(1);
+  std::vector<float> k_sq_dists(1);
+
+#pragma omp parallel for num_threads(num_threads_) firstprivate(k_indices, k_sq_dists) schedule(guided, 8)
+  for (int i = 0; i < input_->size(); i++) {
+    PointTarget pt;
+    pt.getVector4fMap() = trans_f * input_->at(i).getVector4fMap();
+
+    target_kdtree_->nearestKSearch(pt, 1, k_indices, k_sq_dists);
+
+    sq_distances_[i] = k_sq_dists[0];
+    correspondences_[i] = k_sq_dists[0] < corr_dist_threshold_ * corr_dist_threshold_ ? k_indices[0] : -1;
+
+    if (correspondences_[i] < 0) {
+      continue;
+    }
+
+    const int target_index = correspondences_[i];
+    const auto& cov_A = source_covs_[i];
+    const auto& cov_B = target_covs_[target_index];
+
+    Eigen::Matrix4d RCR = cov_B + trans.matrix() * cov_A * trans.matrix().transpose();
+    RCR(3, 3) = 1.0;
+
+    mahalanobis_[i] = RCR.inverse();
+    mahalanobis_[i](3, 3) = 0.0f;
+  }
+}
+
+template <typename PointSource, typename PointTarget>
+double FastGICP<PointSource, PointTarget>::linearize(const Eigen::Isometry3d& trans, Eigen::Matrix<double, 6, 6>* H, Eigen::Matrix<double, 6, 1>* b) {
+  update_correspondences(trans);
+
+  double sum_errors = 0.0;
+  std::vector<Eigen::Matrix<double, 6, 6>, Eigen::aligned_allocator<Eigen::Matrix<double, 6, 6>>> Hs(num_threads_);
+  std::vector<Eigen::Matrix<double, 6, 1>, Eigen::aligned_allocator<Eigen::Matrix<double, 6, 1>>> bs(num_threads_);
+  for (int i = 0; i < num_threads_; i++) {
+    Hs[i].setZero();
+    bs[i].setZero();
+  }
+
+#pragma omp parallel for num_threads(num_threads_) reduction(+ : sum_errors) schedule(guided, 8)
+  for (int i = 0; i < input_->size(); i++) {
+    int target_index = correspondences_[i];
+    if (target_index < 0) {
+      continue;
+    }
+
+    const Eigen::Vector4d mean_A = input_->at(i).getVector4fMap().template cast<double>();
+    const auto& cov_A = source_covs_[i];
+
+    const Eigen::Vector4d mean_B = target_->at(target_index).getVector4fMap().template cast<double>();
+    const auto& cov_B = target_covs_[target_index];
+
+    const Eigen::Vector4d transed_mean_A = trans * mean_A;
+    const Eigen::Vector4d error = mean_B - transed_mean_A;
+
+    sum_errors += error.transpose() * mahalanobis_[i] * error;
+
+    if (H == nullptr || b == nullptr) {
+      continue;
+    }
+
+    Eigen::Matrix<double, 4, 6> dtdx0 = Eigen::Matrix<double, 4, 6>::Zero();
+    dtdx0.block<3, 3>(0, 0) = skewd(transed_mean_A.head<3>());
+    dtdx0.block<3, 3>(0, 3) = -Eigen::Matrix3d::Identity();
+
+    Eigen::Matrix<double, 4, 6> jlossexp = dtdx0;
+
+    Eigen::Matrix<double, 6, 6> Hi = jlossexp.transpose() * mahalanobis_[i] * jlossexp;
+    Eigen::Matrix<double, 6, 1> bi = jlossexp.transpose() * mahalanobis_[i] * error;
+
+    Hs[omp_get_thread_num()] += Hi;
+    bs[omp_get_thread_num()] += bi;
+  }
+
+  if (H && b) {
+    H->setZero();
+    b->setZero();
+    for (int i = 0; i < num_threads_; i++) {
+      (*H) += Hs[i];
+      (*b) += bs[i];
+    }
+  }
+
+  return sum_errors;
+}
+
+template <typename PointSource, typename PointTarget>
+double FastGICP<PointSource, PointTarget>::compute_error(const Eigen::Isometry3d& trans) {
+  double sum_errors = 0.0;
+
+#pragma omp parallel for num_threads(num_threads_) reduction(+ : sum_errors) schedule(guided, 8)
+  for (int i = 0; i < input_->size(); i++) {
+    int target_index = correspondences_[i];
+    if (target_index < 0) {
+      continue;
+    }
+
+    const Eigen::Vector4d mean_A = input_->at(i).getVector4fMap().template cast<double>();
+    const auto& cov_A = source_covs_[i];
+
+    const Eigen::Vector4d mean_B = target_->at(target_index).getVector4fMap().template cast<double>();
+    const auto& cov_B = target_covs_[target_index];
+
+    const Eigen::Vector4d transed_mean_A = trans * mean_A;
+    const Eigen::Vector4d error = mean_B - transed_mean_A;
+
+    sum_errors += error.transpose() * mahalanobis_[i] * error;
+  }
+
+  return sum_errors;
+}
+
+template <typename PointSource, typename PointTarget>
+template <typename PointT>
+bool FastGICP<PointSource, PointTarget>::calculate_covariances(
+  const typename pcl::PointCloud<PointT>::ConstPtr& cloud,
+  pcl::search::KdTree<PointT>& kdtree,
+  std::vector<Eigen::Matrix4d, Eigen::aligned_allocator<Eigen::Matrix4d>>& covariances) {
+  if (kdtree.getInputCloud() != cloud) {
+    kdtree.setInputCloud(cloud);
+  }
+  covariances.resize(cloud->size());
+
+#pragma omp parallel for num_threads(num_threads_) schedule(guided, 8)
+  for (int i = 0; i < cloud->size(); i++) {
+    std::vector<int> k_indices;
+    std::vector<float> k_sq_distances;
+    kdtree.nearestKSearch(cloud->at(i), k_correspondences_, k_indices, k_sq_distances);
+
+    Eigen::Matrix<double, 4, -1> neighbors(4, k_correspondences_);
+    for (int j = 0; j < k_indices.size(); j++) {
+      neighbors.col(j) = cloud->at(k_indices[j]).getVector4fMap().template cast<double>();
+    }
+
+    neighbors.colwise() -= neighbors.rowwise().mean().eval();
+    Eigen::Matrix4d cov = neighbors * neighbors.transpose() / k_correspondences_;
+
+    if (regularization_method_ == RegularizationMethod::NONE) {
+      covariances[i] = cov;
+    } else if (regularization_method_ == RegularizationMethod::FROBENIUS) {
+      double lambda = 1e-3;
+      Eigen::Matrix3d C = cov.block<3, 3>(0, 0).cast<double>() + lambda * Eigen::Matrix3d::Identity();
+      Eigen::Matrix3d C_inv = C.inverse();
+      covariances[i].setZero();
+      covariances[i].template block<3, 3>(0, 0) = (C_inv / C_inv.norm()).inverse();
+    } else {
+      Eigen::JacobiSVD<Eigen::Matrix3d> svd(cov.block<3, 3>(0, 0), Eigen::ComputeFullU | Eigen::ComputeFullV);
+      Eigen::Vector3d values;
+
+      switch (regularization_method_) {
+        default:
+          std::cerr << "here must not be reached" << std::endl;
+          abort();
+        case RegularizationMethod::PLANE:
+          values = Eigen::Vector3d(1, 1, 1e-3);
+          break;
+        case RegularizationMethod::MIN_EIG:
+          values = svd.singularValues().array().max(1e-3);
+          break;
+        case RegularizationMethod::NORMALIZED_MIN_EIG:
+          values = svd.singularValues() / svd.singularValues().maxCoeff();
+          values = values.array().max(1e-3);
+          break;
+      }
+
+      covariances[i].setZero();
+      covariances[i].template block<3, 3>(0, 0) = svd.matrixU() * values.asDiagonal() * svd.matrixV().transpose();
+    }
+  }
+
+  return true;
+}
+
+}  // namespace fast_gicp
+
+#endif

src/lidar_localization/fast_gicp/include/fast_gicp/gicp/impl/fast_gicp_st_impl.hpp

@@ -0,0 +1,129 @@
+#ifndef FAST_GICP_FAST_GICP_ST_IMPL_HPP
+#define FAST_GICP_FAST_GICP_ST_IMPL_HPP
+
+#include <fast_gicp/so3/so3.hpp>
+#include <fast_gicp/gicp/fast_gicp_st.hpp>
+
+namespace fast_gicp {
+
+template <typename PointSource, typename PointTarget>
+FastGICPSingleThread<PointSource, PointTarget>::FastGICPSingleThread() : FastGICP<PointSource, PointTarget>() {
+  this->reg_name_ = "FastGICPSingleThread";
+  this->num_threads_ = 1;
+}
+
+template <typename PointSource, typename PointTarget>
+FastGICPSingleThread<PointSource, PointTarget>::~FastGICPSingleThread() {}
+
+template <typename PointSource, typename PointTarget>
+void FastGICPSingleThread<PointSource, PointTarget>::computeTransformation(PointCloudSource& output, const Matrix4& guess) {
+  anchors_.clear();
+  FastGICP<PointSource, PointTarget>::computeTransformation(output, guess);
+}
+
+template <typename PointSource, typename PointTarget>
+void FastGICPSingleThread<PointSource, PointTarget>::update_correspondences(const Eigen::Isometry3d& x) {
+  assert(source_covs_.size() == input_->size());
+  assert(target_covs_.size() == target_->size());
+
+  Eigen::Isometry3f trans = x.template cast<float>();
+
+  bool is_first = anchors_.empty();
+
+  correspondences_.resize(input_->size());
+  sq_distances_.resize(input_->size());
+  second_sq_distances_.resize(input_->size());
+  anchors_.resize(input_->size());
+  mahalanobis_.resize(input_->size());
+
+  std::vector<int> k_indices;
+  std::vector<float> k_sq_dists;
+
+  for (int i = 0; i < input_->size(); i++) {
+    PointTarget pt;
+    pt.getVector4fMap() = trans * input_->at(i).getVector4fMap();
+
+    if (!is_first) {
+      double d = (pt.getVector4fMap() - anchors_[i]).norm();
+      double max_first = std::sqrt(sq_distances_[i]) + d;
+      double min_second = std::sqrt(second_sq_distances_[i]) - d;
+
+      if (max_first < min_second) {
+        continue;
+      }
+    }
+
+    target_kdtree_->nearestKSearch(pt, 2, k_indices, k_sq_dists);
+
+    correspondences_[i] = k_sq_dists[0] < this->corr_dist_threshold_ * this->corr_dist_threshold_ ? k_indices[0] : -1;
+    sq_distances_[i] = k_sq_dists[0];
+    second_sq_distances_[i] = k_sq_dists[1];
+    anchors_[i] = pt.getVector4fMap();
+
+    if (correspondences_[i] < 0) {
+      continue;
+    }
+
+    const int target_index = correspondences_[i];
+    const auto& cov_A = source_covs_[i];
+    const auto& cov_B = target_covs_[target_index];
+
+    Eigen::Matrix4d RCR = cov_B + x.matrix() * cov_A * x.matrix().transpose();
+    RCR(3, 3) = 1.0;
+
+    mahalanobis_[i] = RCR.inverse();
+    mahalanobis_[i](3, 3) = 0.0;
+  }
+}
+
+template <typename PointSource, typename PointTarget>
+double FastGICPSingleThread<PointSource, PointTarget>::linearize(const Eigen::Isometry3d& trans, Eigen::Matrix<double, 6, 6>* H, Eigen::Matrix<double, 6, 1>* b) {
+  if (H && b) {
+    update_correspondences(trans);
+    H->setZero();
+    b->setZero();
+  }
+
+  double sum_errors = 0.0;
+  for (int i = 0; i < input_->size(); i++) {
+    int target_index = correspondences_[i];
+    if (target_index < 0) {
+      continue;
+    }
+
+    const Eigen::Vector4d mean_A = input_->at(i).getVector4fMap().template cast<double>();
+    const auto& cov_A = source_covs_[i];
+
+    const Eigen::Vector4d mean_B = target_->at(target_index).getVector4fMap().template cast<double>();
+    const auto& cov_B = target_covs_[target_index];
+
+    const Eigen::Vector4d transed_mean_A = trans * mean_A;
+    const Eigen::Vector4d error = mean_B - transed_mean_A;
+
+    sum_errors += error.transpose() * mahalanobis_[i] * error;
+
+    if (H == nullptr || b == nullptr) {
+      continue;
+    }
+
+    Eigen::Matrix<double, 4, 6> dtdx0 = Eigen::Matrix<double, 4, 6>::Zero();
+    dtdx0.block<3, 3>(0, 0) = skewd(transed_mean_A.head<3>());
+    dtdx0.block<3, 3>(0, 3) = -Eigen::Matrix3d::Identity();
+
+    Eigen::Matrix<double, 4, 6> jlossexp = dtdx0;
+
+    (*H) += jlossexp.transpose() * mahalanobis_[i] * jlossexp;
+    (*b) += jlossexp.transpose() * mahalanobis_[i] * error;
+  }
+
+  return sum_errors;
+}
+
+template <typename PointSource, typename PointTarget>
+double FastGICPSingleThread<PointSource, PointTarget>::compute_error(const Eigen::Isometry3d& trans) {
+  return linearize(trans, nullptr, nullptr);
+}
+
+}  // namespace fast_gicp
+
+#endif

src/lidar_localization/fast_gicp/include/fast_gicp/gicp/impl/fast_vgicp_cuda_impl.hpp

@@ -0,0 +1,182 @@
+#ifndef FAST_GICP_FAST_VGICP_CUDA_IMPL_HPP
+#define FAST_GICP_FAST_VGICP_CUDA_IMPL_HPP
+
+#include <atomic>
+#include <Eigen/Core>
+#include <Eigen/Geometry>
+
+#include <pcl/point_types.h>
+#include <pcl/point_cloud.h>
+#include <pcl/search/kdtree.h>
+#include <pcl/registration/registration.h>
+
+#include <fast_gicp/gicp/fast_vgicp_cuda.hpp>
+#include <fast_gicp/cuda/fast_vgicp_cuda.cuh>
+
+#include <thrust/host_vector.h>
+#include <thrust/device_vector.h>
+
+namespace fast_gicp {
+
+template<typename PointSource, typename PointTarget>
+FastVGICPCuda<PointSource, PointTarget>::FastVGICPCuda() : LsqRegistration<PointSource, PointTarget>() {
+  this->reg_name_ = "FastVGICPCuda";
+  k_correspondences_ = 20;
+  voxel_resolution_ = 1.0;
+  regularization_method_ = RegularizationMethod::PLANE;
+  neighbor_search_method_ = NearestNeighborMethod::CPU_PARALLEL_KDTREE;
+
+  vgicp_cuda_.reset(new cuda::FastVGICPCudaCore());
+  vgicp_cuda_->set_resolution(voxel_resolution_);
+  vgicp_cuda_->set_kernel_params(0.5, 3.0);
+}
+
+template<typename PointSource, typename PointTarget>
+FastVGICPCuda<PointSource, PointTarget>::~FastVGICPCuda() {}
+
+template<typename PointSource, typename PointTarget>
+void FastVGICPCuda<PointSource, PointTarget>::setCorrespondenceRandomness(int k) {}
+
+template<typename PointSource, typename PointTarget>
+void FastVGICPCuda<PointSource, PointTarget>::setResolution(double resolution) {
+  vgicp_cuda_->set_resolution(resolution);
+}
+
+template <typename PointSource, typename PointTarget>
+void FastVGICPCuda<PointSource, PointTarget>::setKernelWidth(double kernel_width, double max_dist) {
+  if (max_dist <= 0.0) {
+    max_dist = kernel_width * 5.0;
+  }
+  vgicp_cuda_->set_kernel_params(kernel_width, max_dist);
+}
+
+template<typename PointSource, typename PointTarget>
+void FastVGICPCuda<PointSource, PointTarget>::setRegularizationMethod(RegularizationMethod method) {
+  regularization_method_ = method;
+}
+
+template <typename PointSource, typename PointTarget>
+void FastVGICPCuda<PointSource, PointTarget>::setNeighborSearchMethod(NeighborSearchMethod method, double radius) {
+  vgicp_cuda_->set_neighbor_search_method(method, radius);
+}
+
+template <typename PointSource, typename PointTarget>
+void FastVGICPCuda<PointSource, PointTarget>::setNearestNeighborSearchMethod(NearestNeighborMethod method) {
+  neighbor_search_method_ = method;
+}
+
+template<typename PointSource, typename PointTarget>
+void FastVGICPCuda<PointSource, PointTarget>::swapSourceAndTarget() {
+  vgicp_cuda_->swap_source_and_target();
+  input_.swap(target_);
+}
+
+template<typename PointSource, typename PointTarget>
+void FastVGICPCuda<PointSource, PointTarget>::clearSource() {
+  input_.reset();
+}
+
+template<typename PointSource, typename PointTarget>
+void FastVGICPCuda<PointSource, PointTarget>::clearTarget() {
+  target_.reset();
+}
+
+template<typename PointSource, typename PointTarget>
+void FastVGICPCuda<PointSource, PointTarget>::setInputSource(const PointCloudSourceConstPtr& cloud) {
+  // the input cloud is the same as the previous one
+  if(cloud == input_) {
+    return;
+  }
+
+  pcl::Registration<PointSource, PointTarget, Scalar>::setInputSource(cloud);
+
+  std::vector<Eigen::Vector3f, Eigen::aligned_allocator<Eigen::Vector3f>> points(cloud->size());
+  std::transform(cloud->begin(), cloud->end(), points.begin(), [=](const PointSource& pt) { return pt.getVector3fMap(); });
+
+  vgicp_cuda_->set_source_cloud(points);
+  switch(neighbor_search_method_) {
+    case NearestNeighborMethod::CPU_PARALLEL_KDTREE: {
+      std::vector<int> neighbors = find_neighbors_parallel_kdtree<PointSource>(k_correspondences_, cloud);
+      vgicp_cuda_->set_source_neighbors(k_correspondences_, neighbors);
+      vgicp_cuda_->calculate_source_covariances(regularization_method_);
+    } break;
+    case NearestNeighborMethod::GPU_BRUTEFORCE:
+      vgicp_cuda_->find_source_neighbors(k_correspondences_);
+      vgicp_cuda_->calculate_source_covariances(regularization_method_);
+      break;
+    case NearestNeighborMethod::GPU_RBF_KERNEL:
+      vgicp_cuda_->calculate_source_covariances_rbf(regularization_method_);
+      break;
+  }
+}
+
+template<typename PointSource, typename PointTarget>
+void FastVGICPCuda<PointSource, PointTarget>::setInputTarget(const PointCloudTargetConstPtr& cloud) {
+  // the input cloud is the same as the previous one
+  if(cloud == target_) {
+    return;
+  }
+
+  pcl::Registration<PointSource, PointTarget, Scalar>::setInputTarget(cloud);
+
+  std::vector<Eigen::Vector3f, Eigen::aligned_allocator<Eigen::Vector3f>> points(cloud->size());
+  std::transform(cloud->begin(), cloud->end(), points.begin(), [=](const PointTarget& pt) { return pt.getVector3fMap(); });
+
+  vgicp_cuda_->set_target_cloud(points);
+  switch(neighbor_search_method_) {
+    case NearestNeighborMethod::CPU_PARALLEL_KDTREE: {
+      std::vector<int> neighbors = find_neighbors_parallel_kdtree<PointTarget>(k_correspondences_, cloud);
+      vgicp_cuda_->set_target_neighbors(k_correspondences_, neighbors);
+      vgicp_cuda_->calculate_target_covariances(regularization_method_);
+    } break;
+    case NearestNeighborMethod::GPU_BRUTEFORCE:
+      vgicp_cuda_->find_target_neighbors(k_correspondences_);
+      vgicp_cuda_->calculate_target_covariances(regularization_method_);
+      break;
+    case NearestNeighborMethod::GPU_RBF_KERNEL:
+      vgicp_cuda_->calculate_target_covariances_rbf(regularization_method_);
+      break;
+  }
+  vgicp_cuda_->create_target_voxelmap();
+}
+
+template<typename PointSource, typename PointTarget>
+void FastVGICPCuda<PointSource, PointTarget>::computeTransformation(PointCloudSource& output, const Matrix4& guess) {
+  vgicp_cuda_->set_resolution(this->voxel_resolution_);
+
+  LsqRegistration<PointSource, PointTarget>::computeTransformation(output, guess);
+}
+
+template<typename PointSource, typename PointTarget>
+template<typename PointT>
+std::vector<int> FastVGICPCuda<PointSource, PointTarget>::find_neighbors_parallel_kdtree(int k, typename pcl::PointCloud<PointT>::ConstPtr cloud) const {
+  pcl::search::KdTree<PointT> kdtree;
+  kdtree.setInputCloud(cloud);
+  std::vector<int> neighbors(cloud->size() * k);
+
+#pragma omp parallel for schedule(guided, 8)
+  for(int i = 0; i < cloud->size(); i++) {
+    std::vector<int> k_indices;
+    std::vector<float> k_sq_distances;
+    kdtree.nearestKSearch(cloud->at(i), k, k_indices, k_sq_distances);
+
+    std::copy(k_indices.begin(), k_indices.end(), neighbors.begin() + i * k);
+  }
+
+  return neighbors;
+}
+
+template<typename PointSource, typename PointTarget>
+double FastVGICPCuda<PointSource, PointTarget>::linearize(const Eigen::Isometry3d& trans, Eigen::Matrix<double, 6, 6>* H, Eigen::Matrix<double, 6, 1>* b) {
+  vgicp_cuda_->update_correspondences(trans);
+  return vgicp_cuda_->compute_error(trans, H, b);
+}
+
+template<typename PointSource, typename PointTarget>
+double FastVGICPCuda<PointSource, PointTarget>::compute_error(const Eigen::Isometry3d& trans) {
+  return vgicp_cuda_->compute_error(trans, nullptr, nullptr);
+}
+
+}  // namespace fast_gicp
+
+#endif

src/lidar_localization/fast_gicp/include/fast_gicp/gicp/impl/fast_vgicp_impl.hpp

@@ -0,0 +1,208 @@
+#ifndef FAST_GICP_FAST_VGICP_IMPL_HPP
+#define FAST_GICP_FAST_VGICP_IMPL_HPP
+
+#include <atomic>
+#include <Eigen/Core>
+#include <Eigen/Geometry>
+
+#include <pcl/point_types.h>
+#include <pcl/point_cloud.h>
+#include <pcl/search/kdtree.h>
+#include <pcl/registration/registration.h>
+
+#include <fast_gicp/so3/so3.hpp>
+#include <fast_gicp/gicp/fast_vgicp.hpp>
+
+namespace fast_gicp {
+
+template <typename PointSource, typename PointTarget>
+FastVGICP<PointSource, PointTarget>::FastVGICP() : FastGICP<PointSource, PointTarget>() {
+  this->reg_name_ = "FastVGICP";
+
+  voxel_resolution_ = 1.0;
+  search_method_ = NeighborSearchMethod::DIRECT1;
+  voxel_mode_ = VoxelAccumulationMode::ADDITIVE;
+}
+
+template <typename PointSource, typename PointTarget>
+FastVGICP<PointSource, PointTarget>::~FastVGICP() {}
+
+template <typename PointSource, typename PointTarget>
+void FastVGICP<PointSource, PointTarget>::setResolution(double resolution) {
+  voxel_resolution_ = resolution;
+}
+
+template <typename PointSource, typename PointTarget>
+void FastVGICP<PointSource, PointTarget>::setNeighborSearchMethod(NeighborSearchMethod method) {
+  search_method_ = method;
+}
+
+template <typename PointSource, typename PointTarget>
+void FastVGICP<PointSource, PointTarget>::setVoxelAccumulationMode(VoxelAccumulationMode mode) {
+  voxel_mode_ = mode;
+}
+
+template <typename PointSource, typename PointTarget>
+void FastVGICP<PointSource, PointTarget>::swapSourceAndTarget() {
+  input_.swap(target_);
+  source_kdtree_.swap(target_kdtree_);
+  source_covs_.swap(target_covs_);
+  voxelmap_.reset();
+  voxel_correspondences_.clear();
+  voxel_mahalanobis_.clear();
+}
+
+template <typename PointSource, typename PointTarget>
+void FastVGICP<PointSource, PointTarget>::setInputTarget(const PointCloudTargetConstPtr& cloud) {
+  if (target_ == cloud) {
+    return;
+  }
+
+  FastGICP<PointSource, PointTarget>::setInputTarget(cloud);
+  voxelmap_.reset();
+}
+
+template <typename PointSource, typename PointTarget>
+void FastVGICP<PointSource, PointTarget>::computeTransformation(PointCloudSource& output, const Matrix4& guess) {
+  voxelmap_.reset();
+
+  FastGICP<PointSource, PointTarget>::computeTransformation(output, guess);
+}
+
+template <typename PointSource, typename PointTarget>
+void FastVGICP<PointSource, PointTarget>::update_correspondences(const Eigen::Isometry3d& trans) {
+  voxel_correspondences_.clear();
+  auto offsets = neighbor_offsets(search_method_);
+
+  std::vector<std::vector<std::pair<int, GaussianVoxel::Ptr>>> corrs(num_threads_);
+  for (auto& c : corrs) {
+    c.reserve((input_->size() * offsets.size()) / num_threads_);
+  }
+
+#pragma omp parallel for num_threads(num_threads_) schedule(guided, 8)
+  for (int i = 0; i < input_->size(); i++) {
+    const Eigen::Vector4d mean_A = input_->at(i).getVector4fMap().template cast<double>();
+    Eigen::Vector4d transed_mean_A = trans * mean_A;
+    Eigen::Vector3i coord = voxelmap_->voxel_coord(transed_mean_A);
+
+    for (const auto& offset : offsets) {
+      auto voxel = voxelmap_->lookup_voxel(coord + offset);
+      if (voxel != nullptr) {
+        corrs[omp_get_thread_num()].push_back(std::make_pair(i, voxel));
+      }
+    }
+  }
+
+  voxel_correspondences_.reserve(input_->size() * offsets.size());
+  for (const auto& c : corrs) {
+    voxel_correspondences_.insert(voxel_correspondences_.end(), c.begin(), c.end());
+  }
+
+  // precompute combined covariances
+  voxel_mahalanobis_.resize(voxel_correspondences_.size());
+
+#pragma omp parallel for num_threads(num_threads_) schedule(guided, 8)
+  for (int i = 0; i < voxel_correspondences_.size(); i++) {
+    const auto& corr = voxel_correspondences_[i];
+    const auto& cov_A = source_covs_[corr.first];
+    const auto& cov_B = corr.second->cov;
+
+    Eigen::Matrix4d RCR = cov_B + trans.matrix() * cov_A * trans.matrix().transpose();
+    RCR(3, 3) = 1.0;
+
+    voxel_mahalanobis_[i] = RCR.inverse();
+    voxel_mahalanobis_[i](3, 3) = 0.0;
+  }
+}
+
+template <typename PointSource, typename PointTarget>
+double FastVGICP<PointSource, PointTarget>::linearize(const Eigen::Isometry3d& trans, Eigen::Matrix<double, 6, 6>* H, Eigen::Matrix<double, 6, 1>* b) {
+  if (voxelmap_ == nullptr) {
+    voxelmap_.reset(new GaussianVoxelMap<PointTarget>(voxel_resolution_, voxel_mode_));
+    voxelmap_->create_voxelmap(*target_, target_covs_);
+  }
+
+  update_correspondences(trans);
+
+  double sum_errors = 0.0;
+  std::vector<Eigen::Matrix<double, 6, 6>, Eigen::aligned_allocator<Eigen::Matrix<double, 6, 6>>> Hs(num_threads_);
+  std::vector<Eigen::Matrix<double, 6, 1>, Eigen::aligned_allocator<Eigen::Matrix<double, 6, 1>>> bs(num_threads_);
+  for (int i = 0; i < num_threads_; i++) {
+    Hs[i].setZero();
+    bs[i].setZero();
+  }
+
+#pragma omp parallel for num_threads(num_threads_) reduction(+ : sum_errors) schedule(guided, 8)
+  for (int i = 0; i < voxel_correspondences_.size(); i++) {
+    const auto& corr = voxel_correspondences_[i];
+    auto target_voxel = corr.second;
+
+    const Eigen::Vector4d mean_A = input_->at(corr.first).getVector4fMap().template cast<double>();
+    const auto& cov_A = source_covs_[corr.first];
+
+    const Eigen::Vector4d mean_B = corr.second->mean;
+    const auto& cov_B = corr.second->cov;
+
+    const Eigen::Vector4d transed_mean_A = trans * mean_A;
+    const Eigen::Vector4d error = mean_B - transed_mean_A;
+
+    double w = std::sqrt(target_voxel->num_points);
+    sum_errors += w * error.transpose() * voxel_mahalanobis_[i] * error;
+
+    if (H == nullptr || b == nullptr) {
+      continue;
+    }
+
+    Eigen::Matrix<double, 4, 6> dtdx0 = Eigen::Matrix<double, 4, 6>::Zero();
+    dtdx0.block<3, 3>(0, 0) = skewd(transed_mean_A.head<3>());
+    dtdx0.block<3, 3>(0, 3) = -Eigen::Matrix3d::Identity();
+
+    Eigen::Matrix<double, 4, 6> jlossexp = dtdx0;
+
+    Eigen::Matrix<double, 6, 6> Hi = w * jlossexp.transpose() * voxel_mahalanobis_[i] * jlossexp;
+    Eigen::Matrix<double, 6, 1> bi = w * jlossexp.transpose() * voxel_mahalanobis_[i] * error;
+
+    int thread_num = omp_get_thread_num();
+    Hs[thread_num] += Hi;
+    bs[thread_num] += bi;
+  }
+
+  if (H && b) {
+    H->setZero();
+    b->setZero();
+    for (int i = 0; i < num_threads_; i++) {
+      (*H) += Hs[i];
+      (*b) += bs[i];
+    }
+  }
+
+  return sum_errors;
+}
+
+template <typename PointSource, typename PointTarget>
+double FastVGICP<PointSource, PointTarget>::compute_error(const Eigen::Isometry3d& trans) {
+  double sum_errors = 0.0;
+#pragma omp parallel for num_threads(num_threads_) reduction(+ : sum_errors)
+  for (int i = 0; i < voxel_correspondences_.size(); i++) {
+    const auto& corr = voxel_correspondences_[i];
+    auto target_voxel = corr.second;
+
+    const Eigen::Vector4d mean_A = input_->at(corr.first).getVector4fMap().template cast<double>();
+    const auto& cov_A = source_covs_[corr.first];
+
+    const Eigen::Vector4d mean_B = corr.second->mean;
+    const auto& cov_B = corr.second->cov;
+
+    const Eigen::Vector4d transed_mean_A = trans * mean_A;
+    const Eigen::Vector4d error = mean_B - transed_mean_A;
+
+    double w = std::sqrt(target_voxel->num_points);
+    sum_errors += w * error.transpose() * voxel_mahalanobis_[i] * error;
+  }
+
+  return sum_errors;
+}
+
+}  // namespace fast_gicp
+
+#endif

src/lidar_localization/fast_gicp/include/fast_gicp/gicp/impl/lsq_registration_impl.hpp

@@ -0,0 +1,174 @@
+#include <fast_gicp/gicp/lsq_registration.hpp>
+
+#include <boost/format.hpp>
+#include <fast_gicp/so3/so3.hpp>
+
+namespace fast_gicp {
+
+template <typename PointTarget, typename PointSource>
+LsqRegistration<PointTarget, PointSource>::LsqRegistration() {
+  this->reg_name_ = "LsqRegistration";
+  max_iterations_ = 64;
+  rotation_epsilon_ = 2e-3;
+  transformation_epsilon_ = 5e-4;
+
+  lsq_optimizer_type_ = LSQ_OPTIMIZER_TYPE::LevenbergMarquardt;
+  lm_debug_print_ = false;
+  lm_max_iterations_ = 10;
+  lm_init_lambda_factor_ = 1e-9;
+  lm_lambda_ = -1.0;
+
+  final_hessian_.setIdentity();
+}
+
+template <typename PointTarget, typename PointSource>
+LsqRegistration<PointTarget, PointSource>::~LsqRegistration() {}
+
+template <typename PointTarget, typename PointSource>
+void LsqRegistration<PointTarget, PointSource>::setRotationEpsilon(double eps) {
+  rotation_epsilon_ = eps;
+}
+
+template <typename PointTarget, typename PointSource>
+void LsqRegistration<PointTarget, PointSource>::setInitialLambdaFactor(double init_lambda_factor) {
+  lm_init_lambda_factor_ = init_lambda_factor;
+}
+
+template <typename PointTarget, typename PointSource>
+void LsqRegistration<PointTarget, PointSource>::setDebugPrint(bool lm_debug_print) {
+  lm_debug_print_ = lm_debug_print;
+}
+
+template <typename PointTarget, typename PointSource>
+const Eigen::Matrix<double, 6, 6>& LsqRegistration<PointTarget, PointSource>::getFinalHessian() const {
+  return final_hessian_;
+}
+
+template <typename PointTarget, typename PointSource>
+double LsqRegistration<PointTarget, PointSource>::evaluateCost(const Eigen::Matrix4f& relative_pose, Eigen::Matrix<double, 6, 6>* H, Eigen::Matrix<double, 6, 1>* b) {
+  return this->linearize(Eigen::Isometry3f(relative_pose).cast<double>(), H, b);
+}
+
+template <typename PointTarget, typename PointSource>
+void LsqRegistration<PointTarget, PointSource>::computeTransformation(PointCloudSource& output, const Matrix4& guess) {
+  Eigen::Isometry3d x0 = Eigen::Isometry3d(guess.template cast<double>());
+
+  lm_lambda_ = -1.0;
+  converged_ = false;
+
+  if (lm_debug_print_) {
+    std::cout << "********************************************" << std::endl;
+    std::cout << "***************** optimize *****************" << std::endl;
+    std::cout << "********************************************" << std::endl;
+  }
+
+  for (int i = 0; i < max_iterations_ && !converged_; i++) {
+    nr_iterations_ = i;
+
+    Eigen::Isometry3d delta;
+    if (!step_optimize(x0, delta)) {
+      std::cerr << "lm not converged!!" << std::endl;
+      break;
+    }
+
+    converged_ = is_converged(delta);
+  }
+
+  final_transformation_ = x0.cast<float>().matrix();
+  pcl::transformPointCloud(*input_, output, final_transformation_);
+}
+
+template <typename PointTarget, typename PointSource>
+bool LsqRegistration<PointTarget, PointSource>::is_converged(const Eigen::Isometry3d& delta) const {
+  double accum = 0.0;
+  Eigen::Matrix3d R = delta.linear() - Eigen::Matrix3d::Identity();
+  Eigen::Vector3d t = delta.translation();
+
+  Eigen::Matrix3d r_delta = 1.0 / rotation_epsilon_ * R.array().abs();
+  Eigen::Vector3d t_delta = 1.0 / transformation_epsilon_ * t.array().abs();
+
+  return std::max(r_delta.maxCoeff(), t_delta.maxCoeff()) < 1;
+}
+
+template <typename PointTarget, typename PointSource>
+bool LsqRegistration<PointTarget, PointSource>::step_optimize(Eigen::Isometry3d& x0, Eigen::Isometry3d& delta) {
+  switch (lsq_optimizer_type_) {
+    case LSQ_OPTIMIZER_TYPE::LevenbergMarquardt:
+      return step_lm(x0, delta);
+    case LSQ_OPTIMIZER_TYPE::GaussNewton:
+      return step_gn(x0, delta);
+  }
+
+  return step_lm(x0, delta);
+}
+
+template <typename PointTarget, typename PointSource>
+bool LsqRegistration<PointTarget, PointSource>::step_gn(Eigen::Isometry3d& x0, Eigen::Isometry3d& delta) {
+  Eigen::Matrix<double, 6, 6> H;
+  Eigen::Matrix<double, 6, 1> b;
+  double y0 = linearize(x0, &H, &b);
+
+  Eigen::LDLT<Eigen::Matrix<double, 6, 6>> solver(H);
+  Eigen::Matrix<double, 6, 1> d = solver.solve(-b);
+
+  delta.setIdentity();
+  delta.linear() = so3_exp(d.head<3>()).toRotationMatrix();
+  delta.translation() = d.tail<3>();
+
+  x0 = delta * x0;
+  final_hessian_ = H;
+
+  return true;
+}
+
+template <typename PointTarget, typename PointSource>
+bool LsqRegistration<PointTarget, PointSource>::step_lm(Eigen::Isometry3d& x0, Eigen::Isometry3d& delta) {
+  Eigen::Matrix<double, 6, 6> H;
+  Eigen::Matrix<double, 6, 1> b;
+  double y0 = linearize(x0, &H, &b);
+
+  if (lm_lambda_ < 0.0) {
+    lm_lambda_ = lm_init_lambda_factor_ * H.diagonal().array().abs().maxCoeff();
+  }
+
+  double nu = 2.0;
+  for (int i = 0; i < lm_max_iterations_; i++) {
+    Eigen::LDLT<Eigen::Matrix<double, 6, 6>> solver(H + lm_lambda_ * Eigen::Matrix<double, 6, 6>::Identity());
+    Eigen::Matrix<double, 6, 1> d = solver.solve(-b);
+
+    delta.setIdentity();
+    delta.linear() = so3_exp(d.head<3>()).toRotationMatrix();
+    delta.translation() = d.tail<3>();
+
+    Eigen::Isometry3d xi = delta * x0;
+    double yi = compute_error(xi);
+    double rho = (y0 - yi) / (d.dot(lm_lambda_ * d - b));
+
+    if (lm_debug_print_) {
+      if (i == 0) {
+        std::cout << boost::format("--- LM optimization ---\n%5s %15s %15s %15s %15s %15s %5s\n") % "i" % "y0" % "yi" % "rho" % "lambda" % "|delta|" % "dec";
+      }
+      char dec = rho > 0.0 ? 'x' : ' ';
+      std::cout << boost::format("%5d %15g %15g %15g %15g %15g %5c") % i % y0 % yi % rho % lm_lambda_ % d.norm() % dec << std::endl;
+    }
+
+    if (rho < 0) {
+      if (is_converged(delta)) {
+        return true;
+      }
+
+      lm_lambda_ = nu * lm_lambda_;
+      nu = 2 * nu;
+      continue;
+    }
+
+    x0 = xi;
+    lm_lambda_ = lm_lambda_ * std::max(1.0 / 3.0, 1 - std::pow(2 * rho - 1, 3));
+    final_hessian_ = H;
+    return true;
+  }
+
+  return false;
+}
+
+}  // namespace fast_gicp

src/lidar_localization/fast_gicp/include/fast_gicp/gicp/lsq_registration.hpp

@@ -0,0 +1,88 @@
+#ifndef FAST_GICP_LSQ_REGISTRATION_HPP
+#define FAST_GICP_LSQ_REGISTRATION_HPP
+
+#include <Eigen/Core>
+#include <Eigen/Geometry>
+
+#include <pcl/point_types.h>
+#include <pcl/point_cloud.h>
+#include <pcl/registration/registration.h>
+
+namespace fast_gicp {
+
+enum class LSQ_OPTIMIZER_TYPE { GaussNewton, LevenbergMarquardt };
+
+template<typename PointSource, typename PointTarget>
+class LsqRegistration : public pcl::Registration<PointSource, PointTarget, float> {
+public:
+  using Scalar = float;
+  using Matrix4 = typename pcl::Registration<PointSource, PointTarget, Scalar>::Matrix4;
+
+  using PointCloudSource = typename pcl::Registration<PointSource, PointTarget, Scalar>::PointCloudSource;
+  using PointCloudSourcePtr = typename PointCloudSource::Ptr;
+  using PointCloudSourceConstPtr = typename PointCloudSource::ConstPtr;
+
+  using PointCloudTarget = typename pcl::Registration<PointSource, PointTarget, Scalar>::PointCloudTarget;
+  using PointCloudTargetPtr = typename PointCloudTarget::Ptr;
+  using PointCloudTargetConstPtr = typename PointCloudTarget::ConstPtr;
+
+#if PCL_VERSION >= PCL_VERSION_CALC(1, 10, 0)
+  using Ptr = pcl::shared_ptr<LsqRegistration<PointSource, PointTarget>>;
+  using ConstPtr = pcl::shared_ptr<const LsqRegistration<PointSource, PointTarget>>;
+#else
+  using Ptr = boost::shared_ptr<LsqRegistration<PointSource, PointTarget>>;
+  using ConstPtr = boost::shared_ptr<const LsqRegistration<PointSource, PointTarget>>;
+#endif
+
+protected:
+  using pcl::Registration<PointSource, PointTarget, Scalar>::input_;
+  using pcl::Registration<PointSource, PointTarget, Scalar>::nr_iterations_;
+  using pcl::Registration<PointSource, PointTarget, Scalar>::max_iterations_;
+  using pcl::Registration<PointSource, PointTarget, Scalar>::final_transformation_;
+  using pcl::Registration<PointSource, PointTarget, Scalar>::transformation_epsilon_;
+  using pcl::Registration<PointSource, PointTarget, Scalar>::converged_;
+
+public:
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+  LsqRegistration();
+  virtual ~LsqRegistration();
+
+  void setRotationEpsilon(double eps);
+  void setInitialLambdaFactor(double init_lambda_factor);
+  void setDebugPrint(bool lm_debug_print);
+
+  const Eigen::Matrix<double, 6, 6>& getFinalHessian() const;
+
+  double evaluateCost(const Eigen::Matrix4f& relative_pose, Eigen::Matrix<double, 6, 6>* H = nullptr, Eigen::Matrix<double, 6, 1>* b = nullptr);
+
+  virtual void swapSourceAndTarget() {}
+  virtual void clearSource() {}
+  virtual void clearTarget() {}
+
+protected:
+  virtual void computeTransformation(PointCloudSource& output, const Matrix4& guess) override;
+
+  bool is_converged(const Eigen::Isometry3d& delta) const;
+
+  virtual double linearize(const Eigen::Isometry3d& trans, Eigen::Matrix<double, 6, 6>* H = nullptr, Eigen::Matrix<double, 6, 1>* b = nullptr) = 0;
+  virtual double compute_error(const Eigen::Isometry3d& trans) = 0;
+
+  bool step_optimize(Eigen::Isometry3d& x0, Eigen::Isometry3d& delta);
+  bool step_gn(Eigen::Isometry3d& x0, Eigen::Isometry3d& delta);
+  bool step_lm(Eigen::Isometry3d& x0, Eigen::Isometry3d& delta);
+
+protected:
+  double rotation_epsilon_;
+
+  LSQ_OPTIMIZER_TYPE lsq_optimizer_type_;
+  int lm_max_iterations_;
+  double lm_init_lambda_factor_;
+  double lm_lambda_;
+  bool lm_debug_print_;
+
+  Eigen::Matrix<double, 6, 6> final_hessian_;
+};
+}  // namespace fast_gicp
+
+#endif

src/lidar_localization/fast_gicp/include/fast_gicp/ndt/impl/ndt_cuda_impl.hpp

@@ -0,0 +1,94 @@
+#ifndef FAST_GICP_NDT_CUDA_IMPL_HPP
+#define FAST_GICP_NDT_CUDA_IMPL_HPP
+
+#include <fast_gicp/ndt/ndt_cuda.hpp>
+
+#include <fast_gicp/cuda/ndt_cuda.cuh>
+
+namespace fast_gicp {
+
+template <typename PointSource, typename PointTarget>
+NDTCuda<PointSource, PointTarget>::NDTCuda() : LsqRegistration<PointSource, PointTarget>() {
+  this->reg_name_ = "NDTCuda";
+  ndt_cuda_.reset(new cuda::NDTCudaCore());
+}
+
+template <typename PointSource, typename PointTarget>
+NDTCuda<PointSource, PointTarget>::~NDTCuda() {}
+
+template <typename PointSource, typename PointTarget>
+void NDTCuda<PointSource, PointTarget>::setDistanceMode(NDTDistanceMode mode) {
+  ndt_cuda_->set_distance_mode(mode);
+}
+
+template <typename PointSource, typename PointTarget>
+void NDTCuda<PointSource, PointTarget>::setResolution(double resolution) {
+  ndt_cuda_->set_resolution(resolution);
+}
+
+template <typename PointSource, typename PointTarget>
+void NDTCuda<PointSource, PointTarget>::setNeighborSearchMethod(NeighborSearchMethod method, double radius) {
+  ndt_cuda_->set_neighbor_search_method(method, radius);
+}
+
+template <typename PointSource, typename PointTarget>
+void NDTCuda<PointSource, PointTarget>::swapSourceAndTarget() {
+  ndt_cuda_->swap_source_and_target();
+  input_.swap(target_);
+}
+
+template <typename PointSource, typename PointTarget>
+void NDTCuda<PointSource, PointTarget>::clearSource() {
+  input_.reset();
+}
+
+template <typename PointSource, typename PointTarget>
+void NDTCuda<PointSource, PointTarget>::clearTarget() {
+  target_.reset();
+}
+
+template <typename PointSource, typename PointTarget>
+void NDTCuda<PointSource, PointTarget>::setInputSource(const PointCloudSourceConstPtr& cloud) {
+  if (cloud == input_) {
+    return;
+  }
+  pcl::Registration<PointSource, PointTarget, Scalar>::setInputSource(cloud);
+
+  std::vector<Eigen::Vector3f, Eigen::aligned_allocator<Eigen::Vector3f>> points(cloud->size());
+  std::transform(cloud->begin(), cloud->end(), points.begin(), [=](const PointSource& pt) { return pt.getVector3fMap(); });
+  ndt_cuda_->set_source_cloud(points);
+}
+
+template <typename PointSource, typename PointTarget>
+void NDTCuda<PointSource, PointTarget>::setInputTarget(const PointCloudTargetConstPtr& cloud) {
+  if (cloud == target_) {
+    return;
+  }
+
+  pcl::Registration<PointSource, PointTarget, Scalar>::setInputTarget(cloud);
+
+  std::vector<Eigen::Vector3f, Eigen::aligned_allocator<Eigen::Vector3f>> points(cloud->size());
+  std::transform(cloud->begin(), cloud->end(), points.begin(), [=](const PointTarget& pt) { return pt.getVector3fMap(); });
+  ndt_cuda_->set_target_cloud(points);
+}
+
+template <typename PointSource, typename PointTarget>
+void NDTCuda<PointSource, PointTarget>::computeTransformation(PointCloudSource& output, const Matrix4& guess) {
+  ndt_cuda_->create_voxelmaps();
+  LsqRegistration<PointSource, PointTarget>::computeTransformation(output, guess);
+}
+
+template <typename PointSource, typename PointTarget>
+double NDTCuda<PointSource, PointTarget>::linearize(const Eigen::Isometry3d& trans, Eigen::Matrix<double, 6, 6>* H, Eigen::Matrix<double, 6, 1>* b) {
+  ndt_cuda_->update_correspondences(trans);
+  return ndt_cuda_->compute_error(trans, H, b);
+}
+
+template <typename PointSource, typename PointTarget>
+double NDTCuda<PointSource, PointTarget>::compute_error(const Eigen::Isometry3d& trans) {
+  return ndt_cuda_->compute_error(trans, nullptr, nullptr);
+}
+
+}  // namespace fast_gicp
+
+#endif

src/lidar_localization/fast_gicp/include/fast_gicp/ndt/ndt_cuda.hpp

@@ -0,0 +1,74 @@
+#ifndef FAST_GICP_NDT_CUDA_HPP
+#define FAST_GICP_NDT_CUDA_HPP
+
+#include <Eigen/Core>
+#include <Eigen/Geometry>
+
+#include <pcl/point_types.h>
+#include <pcl/point_cloud.h>
+#include <pcl/search/kdtree.h>
+#include <pcl/registration/registration.h>
+#include <fast_gicp/gicp/lsq_registration.hpp>
+#include <fast_gicp/gicp/gicp_settings.hpp>
+#include <fast_gicp/ndt/ndt_settings.hpp>
+
+namespace fast_gicp {
+
+namespace cuda {
+class NDTCudaCore;
+}
+
+template <typename PointSource, typename PointTarget>
+class NDTCuda : public LsqRegistration<PointSource, PointTarget> {
+public:
+  using Scalar = float;
+  using Matrix4 = typename pcl::Registration<PointSource, PointTarget, Scalar>::Matrix4;
+
+  using PointCloudSource = typename pcl::Registration<PointSource, PointTarget, Scalar>::PointCloudSource;
+  using PointCloudSourcePtr = typename PointCloudSource::Ptr;
+  using PointCloudSourceConstPtr = typename PointCloudSource::ConstPtr;
+
+  using PointCloudTarget = typename pcl::Registration<PointSource, PointTarget, Scalar>::PointCloudTarget;
+  using PointCloudTargetPtr = typename PointCloudTarget::Ptr;
+  using PointCloudTargetConstPtr = typename PointCloudTarget::ConstPtr;
+
+#if PCL_VERSION >= PCL_VERSION_CALC(1, 10, 0)
+  using Ptr = pcl::shared_ptr<NDTCuda<PointSource, PointTarget>>;
+  using ConstPtr = pcl::shared_ptr<const NDTCuda<PointSource, PointTarget>>;
+#else
+  using Ptr = boost::shared_ptr<NDTCuda<PointSource, PointTarget>>;
+  using ConstPtr = boost::shared_ptr<const NDTCuda<PointSource, PointTarget>>;
+#endif
+
+protected:
+  using pcl::Registration<PointSource, PointTarget, Scalar>::reg_name_;
+  using pcl::Registration<PointSource, PointTarget, Scalar>::input_;
+  using pcl::Registration<PointSource, PointTarget, Scalar>::target_;
+  using pcl::Registration<PointSource, PointTarget, Scalar>::corr_dist_threshold_;
+
+public:
+  NDTCuda();
+  virtual ~NDTCuda() override;
+
+  void setDistanceMode(NDTDistanceMode mode);
+  void setResolution(double resolution);
+  void setNeighborSearchMethod(NeighborSearchMethod method, double radius = -1.0);
+
+  virtual void swapSourceAndTarget() override;
+  virtual void clearSource() override;
+  virtual void clearTarget() override;
+
+  virtual void setInputSource(const PointCloudSourceConstPtr& cloud) override;
+  virtual void setInputTarget(const PointCloudTargetConstPtr& cloud) override;
+
+protected:
+  virtual void computeTransformation(PointCloudSource& output, const Matrix4& guess) override;
+  virtual double linearize(const Eigen::Isometry3d& trans, Eigen::Matrix<double, 6, 6>* H, Eigen::Matrix<double, 6, 1>* b) override;
+  virtual double compute_error(const Eigen::Isometry3d& trans) override;
+
+protected:
+  std::unique_ptr<cuda::NDTCudaCore> ndt_cuda_;
+};
+}  // namespace fast_gicp
+
+#endif

src/lidar_localization/fast_gicp/include/fast_gicp/ndt/ndt_settings.hpp

@@ -0,0 +1,10 @@
+#ifndef FAST_GICP_NDT_SETTINGS_HPP
+#define FAST_GICP_NDT_SETTINGS_HPP
+
+namespace fast_gicp {
+
+enum class NDTDistanceMode { P2D, D2D };
+
+}  // namespace fast_gicp
+
+#endif

src/lidar_localization/fast_gicp/include/fast_gicp/so3/so3.hpp

@@ -0,0 +1,81 @@
+#ifndef FAST_GICP_SO3_HPP
+#define FAST_GICP_SO3_HPP
+
+#include <Eigen/Core>
+#include <Eigen/Geometry>
+
+namespace fast_gicp {
+
+inline Eigen::Matrix3f skew(const Eigen::Vector3f& x) {
+  Eigen::Matrix3f skew = Eigen::Matrix3f::Zero();
+  skew(0, 1) = -x[2];
+  skew(0, 2) = x[1];
+  skew(1, 0) = x[2];
+  skew(1, 2) = -x[0];
+  skew(2, 0) = -x[1];
+  skew(2, 1) = x[0];
+
+  return skew;
+}
+
+inline Eigen::Matrix3d skewd(const Eigen::Vector3d& x) {
+  Eigen::Matrix3d skew = Eigen::Matrix3d::Zero();
+  skew(0, 1) = -x[2];
+  skew(0, 2) = x[1];
+  skew(1, 0) = x[2];
+  skew(1, 2) = -x[0];
+  skew(2, 0) = -x[1];
+  skew(2, 1) = x[0];
+
+  return skew;
+}
+
+/*
+ * SO3 expmap code taken from Sophus
+ * https://github.com/strasdat/Sophus/blob/593db47500ea1a2de5f0e6579c86147991509c59/sophus/so3.hpp#L585
+ *
+ * Copyright 2011-2017 Hauke Strasdat
+ *           2012-2017 Steven Lovegrove
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to
+ * deal in the Software without restriction, including without limitation the
+ * rights  to use, copy, modify, merge, publish, distribute, sublicense, and/or
+ * sell copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
+ * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS
+ * IN THE SOFTWARE.
+ */
+inline Eigen::Quaterniond so3_exp(const Eigen::Vector3d& omega) {
+  double theta_sq = omega.dot(omega);
+
+  double theta;
+  double imag_factor;
+  double real_factor;
+  if(theta_sq < 1e-10) {
+    theta = 0;
+    double theta_quad = theta_sq * theta_sq;
+    imag_factor = 0.5 - 1.0 / 48.0 * theta_sq + 1.0 / 3840.0 * theta_quad;
+    real_factor = 1.0 - 1.0 / 8.0 * theta_sq + 1.0 / 384.0 * theta_quad;
+  } else {
+    theta = std::sqrt(theta_sq);
+    double half_theta = 0.5 * theta;
+    imag_factor = std::sin(half_theta) / theta;
+    real_factor = std::cos(half_theta);
+  }
+
+  return Eigen::Quaterniond(real_factor, imag_factor * omega.x(), imag_factor * omega.y(), imag_factor * omega.z());
+}
+
+}  // namespace fast_gicp
+
+#endif