houyongdi/DDAutopilot
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

first commit

Browse Source
This commit is contained in:
houyongdi
2026-07-27 14:39:59 +08:00
commit 1e7acac9ee
2571 changed files with 1252971 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files
.gitignore
Executable
+25
View File
@@ -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
+61
View File
@@ -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
+63
View File
@@ -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
+29
View File
@@ -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
+265
View File
@@ -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
+121
View File
@@ -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
+121
View File
@@ -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
+211
View File
@@ -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
+128
View File
@@ -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
+21
View File
@@ -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
+19
View File
@@ -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
+69
View File
@@ -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
+35
View File
@@ -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
+34
View File
@@ -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
+34
View File
@@ -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
+15
View File
@@ -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
Executable
+42
View File
@@ -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
Executable
+32
View File
@@ -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
Executable
+15
View File
@@ -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
Executable
+113
View File
@@ -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
Executable
+21
View File
@@ -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
Executable
+133
View File
@@ -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
Executable
+15
View File
@@ -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
+34
View File
@@ -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
+54
View File
@@ -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
+16
View File
@@ -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
+41
View File
@@ -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
+26
View File
@@ -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
+113
View File
@@ -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
+15
View File
@@ -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
Executable
+21
View File
@@ -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
Executable
+48
View File
@@ -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
Executable
+15
View File
@@ -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
+35
View File
@@ -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
+18
View File
@@ -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
+15
View File
@@ -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
+44
View File
@@ -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
+29
View File
@@ -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/audio/DebrisFloating.mp3
BIN
View File
Binary file not shown.
src/dd_resources/audio/Drowning.mp3
BIN
View File
Binary file not shown.
src/dd_resources/audio/Firework.mp3
BIN
View File
Binary file not shown.
src/dd_resources/audio/Fishing.mp3
BIN
View File
Binary file not shown.
src/dd_resources/audio/Grazing.mp3
BIN
View File
Binary file not shown.
src/dd_resources/audio/Litter.mp3
BIN
View File
Binary file not shown.
src/dd_resources/audio/RoadFlooding.mp3
BIN
View File
Binary file not shown.
src/dd_resources/audio/TreeFallen.mp3
BIN
View File
Binary file not shown.
src/dd_resources/audio/Vendor.mp3
BIN
View File
Binary file not shown.
src/dd_resources/audio/cycling.mp3
BIN
View File
Binary file not shown.
src/dd_resources/audio/falling.mp3
BIN
View File
Binary file not shown.
src/dd_resources/audio/fighting.mp3
BIN
View File
Binary file not shown.
src/dd_resources/audio/pets.mp3
BIN
View File
Binary file not shown.
src/dd_resources/audio/smoking.mp3
BIN
View File
Binary file not shown.
src/dd_resources/audio/test.wav
BIN
View File
Binary file not shown.
src/dd_resources/camera/calib_extrinsic/CameraExtrinsicMat_20260624_livox_wyh1back.yml
+48
View File
@@ -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
+43
View File
@@ -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
+47
View File
@@ -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
+49
View File
@@ -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
+20
View File
@@ -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
+20
View File
@@ -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
+20
View File
@@ -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
+20
View File
@@ -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.pgm
+2518
View File
File diff suppressed because one or more lines are too long
src/dd_resources/maps/yuanqu_20260104.yaml
+12
View File
@@ -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/model/yolo11m_9c_Robot_0708_py38.engine.bak
BIN
View File
Binary file not shown.
src/dd_resources/model/yolo26m_10c_rbt_0611.engine
BIN
View File
Binary file not shown.
src/dd_resources/package.xml
+21
View File
@@ -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
Executable
+168
View File
@@ -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
Executable
+168
View File
@@ -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
Executable
+29
View File
@@ -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
Executable
+157
View File
@@ -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
Executable
+4
View File
@@ -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
Executable
+17
View File
@@ -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
Executable
+24
View File
@@ -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
Executable
+25
View File
@@ -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
Executable
+17
View File
@@ -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
Executable
+16
View File
@@ -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
Executable
+97
View File
@@ -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
Executable
+24
View File
@@ -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
Executable
+44
View File
@@ -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
Executable
+34
View File
@@ -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
Executable
+73
View File
@@ -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
Executable
+43
View File
@@ -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
Executable
+87
View File
@@ -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
Executable
+280
View File
@@ -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
Executable
+103
View File
@@ -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
Executable
+68
View File
@@ -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
Executable
+84
View File
@@ -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
Executable
+89
View File
@@ -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
Executable
+186
View File
@@ -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
Executable
+13
View File
@@ -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
Executable
+302
View File
@@ -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
Executable
+129
View File
@@ -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
Executable
+182
View File
@@ -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
Executable
+208
View File
@@ -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
Executable
+174
View File
@@ -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
Executable
+88
View File
@@ -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
Executable
+94
View File
@@ -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
Executable
+74
View File
@@ -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
Executable
+10
View File
@@ -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
Executable
+81
View File
@@ -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

Some files were not shown because too many files have changed in this diff Show More