├── .gitignore ├── LICENSE ├── README.md ├── config ├── calibration.ini ├── doc │ ├── pose_init.png │ └── system_architecture.png └── localization.ini ├── launch └── ELiMaLoc.launch └── src ├── app └── localization │ ├── ekf_localization │ ├── CMakeLists.txt │ ├── include │ │ ├── ekf_algorithm.hpp │ │ ├── ekf_localization.hpp │ │ └── ekf_localization_config.hpp │ ├── launch │ │ └── ekf_localization.launch │ ├── package.xml │ ├── rviz │ │ └── ekf_localization_rviz.rviz │ └── src │ │ ├── ekf_algorithm.cpp │ │ └── ekf_localization.cpp │ ├── localization_interface │ ├── localization_functions.hpp │ └── localization_struct.hpp │ └── pcm_matching │ ├── CMakeLists.txt │ ├── README.md │ ├── include │ ├── pcm_matching.hpp │ ├── pcm_matching_config.hpp │ ├── registration.hpp │ └── voxel_hash_map.hpp │ ├── launch │ └── pcm_matching.launch │ ├── package.xml │ └── src │ ├── pcm_matching.cpp │ ├── registration.cpp │ └── voxel_hash_map.cpp └── bsw └── system └── ini_parser ├── convert_UTF.h ├── ini_parser.cpp ├── ini_parser.h └── simple_ini.h /.gitignore: -------------------------------------------------------------------------------- 1 | # files generated by CMake 2 | CMakeCache.txt 3 | CMakeFiles/ 4 | CTestTestfile.cmake 5 | Makefile 6 | cmake_install.cmake 7 | /src/CMakeLists.txt 8 | 9 | 10 | # catkin files 11 | catkin/ 12 | catkin_generated/ 13 | /build/ 14 | /devel/ 15 | /install/ 16 | /gtest/ 17 | /build-* 18 | /devel-* 19 | /.catkin_tools 20 | /logs/ 21 | 22 | # Dependency 23 | /depends/.catkin_workspace 24 | /depends/build/ 25 | /depends/devel/ 26 | /depends/src/CMakeLists.txt 27 | 28 | # GNU global tags 29 | GTAGS 30 | GRTAGS 31 | GSYMS 32 | GPATH 33 | 34 | # Python byte-compile file 35 | __pycache__/ 36 | *.py[co] 37 | 38 | # editor backup files 39 | *~ 40 | \#*\# 41 | 42 | # backup files 43 | *.bak 44 | 45 | # Eclipse 46 | .cproject 47 | .project 48 | .pydevproject 49 | .settings/ 50 | 51 | # QtCreator 52 | *.workspace 53 | *.workspace.user 54 | *.catkin_workspace 55 | 56 | # Visual Studio Code 57 | .vscode/ 58 | 59 | # CLion 60 | .idea/ 61 | cmake-build-debug/ 62 | 63 | # clang-format 64 | .clang-format 65 | 66 | # ROSBag files 67 | *.bag 68 | 69 | # Environments 70 | .env 71 | .venv 72 | env/ 73 | venv/ 74 | ENV/ 75 | 76 | *.pcd 77 | *.bag 78 | *.bin 79 | /.metadata/ 80 | *.tar.gz 81 | *.zip 82 | .config 83 | *.cache 84 | *.onnx 85 | *.pth 86 | *.trt 87 | *.cache 88 | 89 | /src/app/mapping/lio_sam/data/ 90 | /resources/map/pcm/ 91 | .cursorrules -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2024 Jaeyoung Jo 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # ELiMaLoc 2 | **EKF-based LiDAR-Inertial Map matching Localization** 3 | > **Note** 4 | > This code is still undergoing stabilization, but it is currently usable. 5 | 6 | ## Demo Video 7 |

8 | 9 | Demo Video 10 | 11 |

12 | 13 | ## Real Vehicle Autonomous Driving Demo 14 | 15 |

16 | 17 | Real Vehicle Autonomous Driving Demo 18 | 19 |

20 | 21 | ## TODO 22 | 1. CUDA based ICP 23 | 2. Localization evaluation on public dataset 24 | 25 | ## Menu 26 | - [System Architecture](#system-architecture) 27 | - [Dependency](#dependency) 28 | - [How to use](#usage) 29 | - [EKF Localization](#ekf_localization) 30 | - [PCM Matching](#pcm_matching) 31 | - [Parameter Settings](#parameter-settings) 32 | - [Test record file](#test-record-file) 33 | 34 | # System Architecture 35 | 36 |

37 | drawing 38 |

39 | 40 | ### Packages 41 | - `ekf_localization`: 24-DOF EKF-based localization 42 | - `pcm_matching`: Point cloud Map Matching 43 | 44 | ### Folder Structure 45 | ``` 46 | localization/ 47 | ├─ ekf_localization/ 48 | ├─ localization_interface/ 49 | ├─ pcm_matching/ 50 | └─ README.md 51 | ``` 52 | 53 | # Dependency 54 | ### 1. Install ROS messages and other libraries 55 | * for Ubuntu 20.04 (noetic) 56 | ```bash 57 | sudo apt install ros-noetic-jsk-rviz-plugins ros-noetic-ros-numpy ros-noetic-nmea-msgs ros-noetic-gps-common ros-noetic-can-msgs ros-noetic-derived-object-msgs 58 | ``` 59 | * other libraries 60 | ```bash 61 | sudo apt install net-tools libpugixml-dev libgeographic-dev rospack-tools libeigen3-dev liblapack-dev pkg-config swig cmake 62 | sudo apt-get install ros-noetic-mrt-cmake-modules 63 | ``` 64 | * Georgia Tech Smoothing and Mapping library 65 | ```bash 66 | sudo add-apt-repository ppa:borglab/gtsam-release-4.0 67 | sudo apt install libgtsam-dev libgtsam-unstable-dev 68 | ``` 69 | 70 | # How to use 71 | ### 1. Install 72 | Clone code as workspace 73 | ```bash 74 | git clone https://github.com/jaeyoungjo99/ELiMaLoc.git 75 | cd ELiMaLoc 76 | catkin_make 77 | ``` 78 | ### 2. Configure localization.ini, calibration.ini 79 | config/localization.ini 80 | config/calibration.ini 81 | 82 | ### 3. Place the point cloud map file 83 | resources/map/pcm 84 | ### 4. Modify ekf_localization.launch, pcm_matching.launch 85 | 1. ekf_localization 86 | - Set location parameters. Modify ref lat, lon, hgt if necessary 87 | - Set /use_sim_time to true when using bag data 88 | 2. pcm_matching 89 | - Set location parameters. Modify map_path if necessary 90 | - Set /use_sim_time to true when using bag data 91 | ### 5. Run launch 92 | `location`: location name in launch file 93 | `bag`: true or false for using bag data 94 | 1. Launch at once 95 | ```bash 96 | roslaunch launch/ELiMaLoc.launch location:=hanyang bag:=true 97 | ``` 98 | 2. Launch each 99 | ```bash 100 | rviz -d $(rospack find ekf_localization)/rviz/ekf_localization_rviz.rviz 101 | roslaunch ekf_localization ekf_localization.launch 102 | roslaunch pcm_matching pcm_matching.launch 103 | ``` 104 | ### 6. Initialize Pose via Rviz Click (when not using GPS) 105 |

106 | drawing 107 |

108 | 109 | See [CallbackInitialPose](src/app/localization/pcm_matching/src/pcm_matching.cpp#L365) function for more details. 110 | 111 | 1. Change Views Type to `TopDownOrtho` 112 | 2. Click the green `2D Pose Estimate` on the top panel and drag in the desired direction 113 | 3. Check the terminal for the purple `[Init Pose] Publish Init ...` message 114 | 4. If `[Init Pose] ICP failed!` appears, click again to reinitialize 115 | 116 | 117 | # `ekf_localization` 118 | ## Purpose and Features 119 | ### IMU-based EKF State Estimation 120 | - 24-DOF EKF for IMU gravity estimation and bias removal (refer to FAST-LIO equations) 121 | - Supports various state updates (PCM, GPS, NavSatFix, CAN) 122 | - Supports ZUPT through vehicle stop detection 123 | 124 | ## I/O 125 | - Input 126 | - IMU data sensor_msgs/Imu 127 | - Update 128 | - PCM estimated position nav_msgs/Odometry 129 | - PCM estimated initial position nav_msgs/Odometry 130 | - NavSatFix nav_msgs/NavSatFix 131 | - CAN 132 | - Basic CAN geometry_msgs::TwistStamped 133 | - Output 134 | - Estimated position nav_msgs/Odometry 135 | 136 | # `pcm_matching` 137 | ## Purpose and Features 138 | ### Point cloud Map Matching 139 | - Point cloud Deskewing 140 | - Voxel Hashing-based map structuring and nearest point search 141 | - Supports various ICP methods (P2P, GICP, VGICP, AVGICP) 142 | - Supports initial position setting via Rviz click (/init_pose) 143 | - Detailed description: [PCM Matching README.md](src/app/localization/pcm_matching/README.md) 144 | ## I/O 145 | - Input 146 | - Estimated position nav_msgs/Odometry 147 | - Point cloud sensor_msgs/PointCloud2 148 | - Output 149 | - Estimated position nav_msgs/Odometry (Position, Covariance) 150 | - PCM visualization sensor_msgs/PointCloud2 151 | 152 | # `localization_interface` 153 | ## Purpose 154 | - Provides key functions and structures for localization algorithms 155 | - Functions related to Lie Algebra, structures related to EKF State 156 | 157 | # Parameter Settings 158 | /config/localization.ini 159 | 160 | ## [common_variable] 161 | - Sensor settings and basic coordinate system settings 162 | - `lidar_type`: Type of LiDAR to use (velodyne, ouster) 163 | - `lidar_scan_time_end`: 1 if LiDAR time is 0.0 for the last point, otherwise 0 164 | - `lidar_time_delay`: LiDAR delay (sec) 165 | - `lidar_topic_name`: LiDAR topic name (sensor_msgs/PointCloud2) 166 | - `can_topic_name`: CAN topic name (geometry_msgs/TwistStamped) 167 | - `imu_topic_name`: IMU topic name (sensor_msgs/Imu) 168 | - `navsatfix_topic_name`: NavSatFix topic name (sensor_msgs/NavSatFix) 169 | - `projection_mode`: Coordinate system projection method (Cartesian, UTM) 170 | 171 | ## [ekf_localization] 172 | - EKF algorithm settings 173 | - Debug output 174 | - `debug_print`: Whether to output debug input data 175 | - `debug_imu_print`: Whether to output IMU debug 176 | - IMU settings 177 | - `imu_gravity`: Initial value of gravitational acceleration 178 | - `imu_estimate_gravity`: Whether to estimate gravitational acceleration 179 | - `use_zupt`: Whether to use ZUPT 180 | - `use_complementary_filter`: Whether to use Complementary Filter (forced in BESTPOS mode) 181 | - GPS settings 182 | - `gps_type`: GPS type (0: INSPVAX, 1: BESTPOS and BESTVEL, 2: NavSatFix) 183 | - `gnss_uncertainy_max_m`: Maximum cov limit for GPS input 184 | - Sensor usage 185 | - `use_gps`: Use GPS update 186 | - `use_imu`: Use IMU prediction (if not used, CA model prediction using ROS Time) 187 | - `use_can`: Use CAN speed update 188 | - `use_pcm_matching`: Use PCM matching 189 | - CAN settings 190 | - `can_vel_scale_factor`: CAN speed scale factor 191 | - Initial state and uncertainty 192 | - `ekf_init_x/y/z_m`: Initial position 193 | - `ekf_init_roll/pitch/yaw_deg`: Initial attitude 194 | - `ekf_state_uncertainty_*`: State uncertainty settings 195 | - `ekf_imu_uncertainty_*`: IMU sensor uncertainty settings 196 | - `ekf_imu_bias_cov_*`: IMU bias uncertainty settings 197 | - `ekf_gnss_min_cov_*`: Minimum GPS uncertainty settings 198 | - `ekf_can_meas_uncertainty_*`: CAN measurement uncertainty settings 199 | 200 | ## [pcm_matching] 201 | - PCM matching settings 202 | - Debug output 203 | - `debug_print`: Whether to output debug 204 | - Map settings 205 | - `pcm_voxel_size`: Voxel size (m) (recommended 0.5 ~ 2.0) 206 | - `pcm_voxel_max_point`: Maximum points in a voxel (search speed slows if too large for P2P, GICP) (recommended ~50) 207 | - Input preprocessing 208 | - `input_max_dist`: Input point cloud distance filtering (recommended ~100) 209 | - `input_index_sampling`: Point cloud index sampling (recommended 5 or more) 210 | - `input_voxel_ds_m`: Input point voxel sampling size (recommended 1.0 ~ 2.0) 211 | - ICP settings 212 | - `icp_method`: ICP method selection (0: P2P, **1: GICP**, 2: VGICP, 3: AVGICP) 213 | - `max_iteration`: Maximum ICP iterations 214 | - `max_search_dist`: Maximum search distance for correspondences (m) (recommended ~5.0) 215 | - `lm_lambda`: Levenburg lambda value (recommended ~0.5) 216 | - `icp_termination_threshold_m`: ICP termination translation+angle threshold (recommended ~0.02) 217 | - `min_overlap_ratio`: Minimum overlap ratio with map (0 ~ 1.0) 218 | - `max_fitness_score`: Maximum fitness score considered as ICP success (recommended ~0.5) 219 | - `gicp_cov_search_dist`: Search range for cov calculation around target points in GICP (recommended 0.5 ~ 1.0) 220 | - Radar settings 221 | - `use_radar_cov`: Whether to use radar cov (0: do not use radar cov) 222 | - `doppler_trans_lambda`: 0.0 geometric, 1.0 doppler (applies only when use_radar_cov is 1) 223 | - `range_variance_m`: Radar distance uncertainty 224 | - `azimuth_variance_deg`: Radar horizontal resolution uncertainty 225 | - `elevation_variance_deg`: Radar vertical resolution uncertainty 226 | # Test record file 227 | ### Hanyang University - [Google Drive](https://drive.google.com/drive/folders/1TAqY0102CSjbJHfW3_qR7VP3j8kbSD4L?usp=sharing) 228 | - init_x: 3.11 229 | - init_y: 2.60 230 | - init_z: -0.4 231 | - init_yaw: 150.13 232 | ``` 233 | hanyang_gps_imu_velodyne 234 | ├─ 37.238551_126.772530_0.000000_kcity_1203_filtered_02.pcd 235 | ├─ hanyang_loop.7z (.bag file) 236 | └─ calibration.ini 237 | ``` 238 | 239 | ### K City - [Google Drive](https://drive.google.com/drive/folders/1OwCfnxhYPpe8f1wP9jFGlpPVwOeegtaO?usp=sharing) 240 | - init_x: 52.56 241 | - init_y: 70.09 242 | - init_z: 6.32 243 | - init_yaw: 62.17 244 | ``` 245 | kcity_gps_imu_velodyne 246 | ├─ 37.558200_127.044500_66.000000_hanyang_02m.pcd 247 | ├─ kcity_loop.7z (.bag file) 248 | └─ calibration.ini 249 | ``` 250 | 251 | # References 252 | - EKF State Estimation: [FAST-LIO2](https://github.com/hku-mars/FAST_LIO) Xu, Wei, et al. (HKU MARS Lab) 253 | - ICP Framework: [KISS-ICP](https://github.com/PRBonn/kiss-icp) Vizzo, Ignacio, et al. (PRBonn Lab) 254 | - ICP Methodology: [FAST GICP](https://github.com/koide3/fast_gicp) Koide, Kenji, et al. (Tokyo Institute of Technology) 255 | - Deskewing and Utility Functions: [LIO-SAM](https://github.com/TixiaoShan/LIO-SAM) Shan, Tixiao, et al. (Harbin Institute of Technology) 256 | - Lie Algebra Functions: [Sophus](https://github.com/strasdat/Sophus) Strasdat et al. 257 | 258 | # README Version History 259 | - 2024/11/19: Initial version (Jaeyoung Jo wodud3743@gmail.com) 260 | - 2024/12/05: ini modification (Jaeyoung Jo wodud3743@gmail.com) 261 | - 2024/12/11: Final modification (Jaeyoung Jo wodud3743@gmail.com) 262 | - 2024/12/18: Remove private dependency (Jaeyoung Jo wodud3743@gmail.com) 263 | - 2024/12/19: Add private test record file (Jaeyoung Jo wodud3743@gmail.com) 264 | -------------------------------------------------------------------------------- /config/calibration.ini: -------------------------------------------------------------------------------- 1 | [Rear To Imu] 2 | parent_frame_id = ego_frame 3 | child_frame_id = imu 4 | ; transform_xyz_m = 0.0 0.0 0.5 5 | ; rotation_rpy_deg = -0.7 -1.5 -89.0 6 | transform_xyz_m = 0.0 0.0 0.0 7 | rotation_rpy_deg = 0.0 0.0 0.0 8 | 9 | [Rear To Gps] 10 | parent_frame_id = ego_frame 11 | child_frame_id = gps 12 | transform_xyz_m = 0.0 0.0 0.5 13 | rotation_rpy_deg = 0.0 0.0 1.3 14 | 15 | [Rear To Main LiDAR] 16 | parent_frame_id = ego_frame 17 | child_frame_id = velodyne 18 | ; transform_xyz_m = 1.2 0.0 1.88 19 | ; rotation_rpy_deg = 0.0 1.0 0.5 20 | transform_xyz_m = 0.0961 -0.1338 0.3032 21 | rotation_rpy_deg = -1.26 -0.876 0.287 -------------------------------------------------------------------------------- /config/doc/pose_init.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jaeyoungjo99/ELiMaLoc/8254ee7e76def5bd0cf84530441997f0ab0229b3/config/doc/pose_init.png -------------------------------------------------------------------------------- /config/doc/system_architecture.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jaeyoungjo99/ELiMaLoc/8254ee7e76def5bd0cf84530441997f0ab0229b3/config/doc/system_architecture.png -------------------------------------------------------------------------------- /config/localization.ini: -------------------------------------------------------------------------------- 1 | ; - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - 2 | [common_variable] 3 | lidar_type = velodyne 4 | ; ouster or default pcl format 5 | lidar_scan_time_end = 1 ; 1: Lidar time is when the last point is time 0.0. Ioniq VLP32 is 1 6 | lidar_time_delay = 0.03 ; Lidar output delay (in seconds) 7 | lidar_topic_name = /velodyne_points 8 | ; /center/rslidar_points /velodyne_points /ouster/points 9 | can_topic_name = /kusv_CanInfo_geo_msg 10 | imu_topic_name = /imu/data 11 | ; /ouster/imu /imu/data_raw /gps/imu /imu/data 12 | navsatfix_topic_name = /gps/fix 13 | 14 | projection_mode = Cartesian ; Cartesian, UTM 15 | 16 | [ekf_localization] 17 | debug_print = 0 ; Print debug msg 18 | debug_imu_print = 0 ; Print Imu debug msg 19 | 20 | imu_gravity = 9.81 ; Earth Gravity for imu sensor 21 | imu_estimate_gravity = 1 ; 0: Do not estimate gravity, 1: Estimate gravity 22 | imu_estimate_calibration = 0 ; 1: estimate vehicle to imu RPY 23 | use_zupt = 0 ; Zero Velocity Potential Update 24 | use_complementary_filter = 1 ; Complementary Filter (Forced use in BESTPOS mode) 25 | 26 | gps_type = 2 ; 0: INSPVAX, 1: BESTPOS and BESTVEL, 2: NavSatFix 27 | gnss_uncertainy_max_m = 1.0 ; Maximum cov limit for gnss input 28 | 29 | use_gps = 0 ; 1: use gps for Update, 0: Only use GPS for pose initialization 30 | use_imu = 1 ; 1: use imu for prediction. 0: use system state for prediction 31 | use_can = 0 ; 1: use can for velocity update 32 | use_pcm_matching = 1 ; Use PCM matching 33 | 34 | can_vel_scale_factor = 1.0 ; tuning param 35 | 36 | ; EKF Initial pose 37 | ; Hanyang University 38 | ; ekf_init_x_m = 3.11 39 | ; ekf_init_y_m = 2.60 40 | ; ekf_init_z_m = -0.4 41 | ; ekf_init_roll_deg = 0.0 42 | ; ekf_init_pitch_deg = 0.0 43 | ; ekf_init_yaw_deg = 150.13 44 | 45 | ; GEODE Dataset stairs alpha 46 | ekf_init_x_m = -11.2379716113593 47 | ekf_init_y_m = 2.61441970033043 48 | ekf_init_z_m = -0.576108843465071 49 | ekf_init_roll_deg = -0.1147311 50 | ekf_init_pitch_deg = -0.4350557 51 | ekf_init_yaw_deg = -129.8725387 52 | 53 | ; EKF State uncertainty 54 | ekf_state_uncertainty_pos_m = 0.02 ; state position uncertainty 55 | ekf_state_uncertainty_rot_deg = 0.2 ; state rotation uncertainty 56 | ekf_state_uncertainty_vel_mps = 2.0 ; state velocity uncertainty 57 | ekf_state_uncertainty_gyro_dps = 5.0 ; only use non imu mode 58 | ekf_state_uncertainty_acc_mps = 100.0 ; only use non imu mode 59 | 60 | ; IMU Sensor uncertainty 61 | ekf_imu_uncertainty_gyro_dps = 0.01 ; imu gyro uncertainty 62 | ekf_imu_uncertainty_acc_mps = 0.001 ; imu acceleration uncertainty 63 | 64 | ekf_imu_bias_cov_gyro = 0.0001 ; imu gyro bias uncertainty (Don't touch) 65 | ekf_imu_bias_cov_acc = 0.0001 ; imu acceleration bias uncertainty (Don't touch) 66 | 67 | ; absolute minimum cov of gnss position. Only for GPS 68 | ekf_gnss_min_cov_x_m = 0.2 69 | ekf_gnss_min_cov_y_m = 0.2 70 | ekf_gnss_min_cov_z_m = 0.7 71 | ekf_gnss_min_cov_roll_deg = 0.0 72 | ekf_gnss_min_cov_pitch_deg = 0.0 73 | ekf_gnss_min_cov_yaw_deg = 0.0 74 | 75 | ekf_can_meas_uncertainty_vel_mps = 2.0 ; CAN velocity uncertainty 76 | ekf_can_meas_uncertainty_yaw_rate_deg = 10.0 ; CAN yaw rate uncertainty 77 | 78 | ekf_bestvel_meas_uncertainty_vel_mps = 1.0 ; BESTVEL velocity uncertainty 79 | 80 | [pcm_matching] 81 | debug_print = 0 ; Print debug msg 82 | 83 | pcm_voxel_size = 1.0 ; Voxel size 84 | pcm_voxel_max_point = 30 ; Maximum number of points in a voxel 85 | 86 | run_deskew = 1 ; 1: true 0: false 87 | input_max_dist = 100 ; Input point cloud distance filtering 88 | input_index_sampling = 5 ; Sampling by index of pointcloud 89 | input_voxel_ds_m = 1.5 ; Sampling leaving only one per voxel 90 | 91 | icp_method = 1 ; 0:P2P, 1:GICP, 2:VGICP, 3:AVGICP 92 | voxel_search_method = 2 ; 0: 1voxel, 1: 7voxel, 2: 27voxel ; TODO: 93 | gicp_cov_search_dist = 0.4 ; Search range for cov calculation around target point in GICP 94 | 95 | max_thread = 10 ; Maximum number of CPU threads to use. Set to 0 or less for maximum use 96 | max_iteration = 10 ; Maximum number of icp iterations 97 | max_search_dist = 5.0 ; Maximum search distance for corresponding 98 | lm_lambda = 0.5 ; Levenburg lambda value 99 | icp_termination_threshold_m = 0.02 ; icp termination translation+angle threshold 100 | 101 | min_overlap_ratio = 0.4 ; Minimum ratio of points related to the map 102 | max_fitness_score = 0.5 ; Maximum fitness score considered as icp success 103 | 104 | ; Params for radar 105 | use_radar_cov = 0 ; 1: use radar cov, 0: do not use radar cov 106 | doppler_trans_lambda = 0.5 ; 0.0 geometric, 1.0 doppler (applies only when use_doppler is 1) 107 | range_variance_m = 1.0 ; for radar 108 | azimuth_variance_deg = 0.4 ; for radar 109 | elevation_variance_deg = 0.4 ; for radar 110 | -------------------------------------------------------------------------------- /launch/ELiMaLoc.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | -------------------------------------------------------------------------------- /src/app/localization/ekf_localization/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(ekf_localization) 3 | set(CMAKE_EXPORT_COMPILE_COMMANDS ON) 4 | 5 | ## Compile as C++14, supported in ROS Kinetic and newer 6 | add_compile_options(-std=c++14) 7 | 8 | ## Find catkin macros and libraries 9 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 10 | ## is used, also find other catkin packages 11 | find_package(catkin REQUIRED COMPONENTS 12 | roscpp 13 | rosbag 14 | tf 15 | cv_bridge 16 | std_msgs 17 | visualization_msgs 18 | geometry_msgs 19 | ) 20 | 21 | find_package(Eigen3 REQUIRED) 22 | 23 | ## System dependencies are found with CMake's conventions 24 | find_package(Boost REQUIRED COMPONENTS system) 25 | 26 | ################################### 27 | ## catkin specific configuration ## 28 | ################################### 29 | ## The catkin_package macro generates cmake config files for your package 30 | ## Declare things to be passed to dependent projects 31 | ## INCLUDE_DIRS: uncomment this if your package contains header files 32 | ## LIBRARIES: libraries you create in this project that dependent projects also need 33 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 34 | ## DEPENDS: system dependencies of this project that dependent projects also need 35 | catkin_package( 36 | INCLUDE_DIRS include 37 | # INCLUDE_DIRS include 38 | # LIBRARIES trajectory 39 | # CATKIN_DEPENDS roscpp rospy std_msgs 40 | # DEPENDS system_lib 41 | ) 42 | 43 | ########### 44 | ## Build ## 45 | ########### 46 | 47 | ## Specify additional locations of header files 48 | ## Your package locations should be listed before other locations 49 | 50 | add_subdirectory(${CMAKE_SOURCE_DIR}/lib/third_party/GeographicLib ${CMAKE_BINARY_DIR}/GeographicLib_build) 51 | 52 | include_directories( 53 | include 54 | ${catkin_INCLUDE_DIRS} 55 | ${OpenCV_INCLUDE_DIRS} 56 | ${EIGEN3_INCLUDE_DIR} 57 | 58 | # BSW 59 | ${CMAKE_SOURCE_DIR}/bsw/system/ini_parser 60 | 61 | # Library 62 | ../localization_interface 63 | # /usr/include/GeographicLib 64 | ${CMAKE_SOURCE_DIR}/lib/third_party/GeographicLib/src 65 | ) 66 | 67 | ## Declare a C++ library 68 | # add_library(${PROJECT_NAME} 69 | # src/${PROJECT_NAME}/trajectory.cpp 70 | # ) 71 | 72 | ## Add cmake target dependencies of the library 73 | ## as an example, code may need to be generated before libraries 74 | ## either from message generation or dynamic reconfigure 75 | # add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 76 | 77 | ## Declare a C++ executable 78 | ## With catkin_make all packages are built within a single CMake context 79 | ## The recommended prefix ensures that target names across packages don't collide 80 | add_executable(${PROJECT_NAME} 81 | # BSW 82 | ${CMAKE_SOURCE_DIR}/bsw/system/ini_parser/ini_parser.cpp 83 | 84 | src/ekf_localization.cpp 85 | src/ekf_algorithm.cpp 86 | ) 87 | 88 | ## Rename C++ executable without prefix 89 | ## The above recommended prefix causes long target names, the following renames the 90 | ## target back to the shorter version for ease of user use 91 | ## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" 92 | # set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") 93 | 94 | ## Add cmake target dependencies of the executable 95 | ## same as for the library above 96 | add_dependencies(${PROJECT_NAME} 97 | ${${PROJECT_NAME}_EXPORTED_TARGETS} 98 | ${catkin_EXPORTED_TARGETS} 99 | ) 100 | 101 | ## Specify libraries to link a library or executable target against 102 | target_link_libraries(${PROJECT_NAME} 103 | ${catkin_LIBRARIES} 104 | ${OpenCV_LIBRARIES} 105 | GeographicLib 106 | ) -------------------------------------------------------------------------------- /src/app/localization/ekf_localization/include/ekf_algorithm.hpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file ekf_algorithm.hpp 3 | * @author Jaeyoung Jo 4 | * @brief Extended Kalman Filter for various state estimation 5 | * @version 0.2 6 | * @date 2024-12-18 7 | * 8 | * @copyright Copyright (c) 2024 9 | */ 10 | 11 | #ifndef __EKF_ALGORITHM__ 12 | #define __EKF_ALGORITHM__ 13 | #pragma once 14 | 15 | #include 16 | 17 | #include "localization_functions.hpp" 18 | #include "localization_struct.hpp" 19 | 20 | #include "ekf_localization_config.hpp" 21 | 22 | // STD header 23 | #include 24 | #include 25 | #include 26 | #include 27 | #include 28 | #include 29 | #include 30 | 31 | // ROS header 32 | #include 33 | #include 34 | #include 35 | #include 36 | #include 37 | #include 38 | 39 | #include 40 | 41 | #define S_X 0 42 | #define S_Y 1 43 | #define S_Z 2 44 | #define S_ROLL 3 45 | #define S_PITCH 4 46 | #define S_YAW 5 47 | #define S_VX 6 48 | #define S_VY 7 49 | #define S_VZ 8 50 | #define S_ROLL_RATE 9 51 | #define S_PITCH_RATE 10 52 | #define S_YAW_RATE 11 53 | #define S_AX 12 54 | #define S_AY 13 55 | #define S_AZ 14 56 | #define S_B_ROLL_RATE 15 57 | #define S_B_PITCH_RATE 16 58 | #define S_B_YAW_RATE 17 59 | #define S_B_AX 18 60 | #define S_B_AY 19 61 | #define S_B_AZ 20 62 | #define S_G_X 21 63 | #define S_G_Y 22 64 | #define S_G_Z 23 65 | #define S_IMU_ROLL 24 66 | #define S_IMU_PITCH 25 67 | #define S_IMU_YAW 26 68 | 69 | #define STATE_ORDER 27 70 | 71 | #define GNSS_MEAS_ORDER 6 // x y z roll pitch yaw 72 | #define CAN_MEAS_ORDER 2 // v_local_x, yaw_rate 73 | #define INIT_STATE_COV 100.0 74 | 75 | #define DEBUG_PRINT_DT 1.0 // Printing cout dt 76 | 77 | using namespace ros; 78 | using namespace tf; 79 | using namespace std; 80 | 81 | class EkfAlgorithm { 82 | public: 83 | EkfAlgorithm(EkfLocalizationConfig cfg); 84 | ~EkfAlgorithm(); 85 | 86 | void Init(); 87 | 88 | void UpdateDynamicConfig(EkfLocalizationConfig cfg); 89 | 90 | // EKF Functions 91 | bool RunPrediction(double cur_timestamp); 92 | 93 | bool RunPredictionImu(double cur_timestamp, ImuStruct imu_input); 94 | bool RunGnssUpdate(EkfGnssMeasurement gnss_input); 95 | bool RunCanUpdate(CanStruct can_input); 96 | 97 | bool IsStateInitialized() const { return b_state_initialized_; } 98 | bool IsYawInitialized() const { return b_yaw_initialized_; } 99 | bool IsRotationStabilized() const { return b_rotation_stabilized_; } 100 | bool IsStateStabilized() const { return b_state_stabilized_; } 101 | 102 | EgoState GetCurrentState(); 103 | 104 | Eigen::Vector3d GetImuCalibration(); 105 | 106 | private: 107 | // Utils 108 | 109 | // ZUPT : Zero Velocity Potential Update 110 | void ZuptImu(ImuStruct imu_input); 111 | void ZuptCan(CanStruct can_input); 112 | 113 | void ComplementaryKalmanFilter(ImuStruct imu_input); 114 | void CalibrateVehicleToImu(ImuStruct imu_input); 115 | 116 | template 117 | void UpdateEkfState(const Eigen::Matrix& K, // 칼만 게인 118 | const Eigen::Matrix& Y, // 잔차 119 | Eigen::Matrix& P, // 공분산 행렬 120 | const Eigen::Matrix& H, // 관측 행렬 121 | EkfState& X // EKF 상태 122 | ) { 123 | // State update 124 | Eigen::Matrix state_update = K * Y; 125 | X.pos += state_update.head<3>(); // Position update 126 | X.vel += state_update.block<3, 1>(S_VX, 0); 127 | X.gyro += state_update.block<3, 1>(S_ROLL_RATE, 0); 128 | X.acc += state_update.block<3, 1>(S_AX, 0); 129 | X.bg += state_update.block<3, 1>(S_B_ROLL_RATE, 0); 130 | X.ba += state_update.block<3, 1>(S_B_AX, 0); 131 | X.grav += state_update.block<3, 1>(S_G_X, 0); 132 | 133 | // Quaternion to rotation update 134 | Eigen::Vector3d rot_delta = state_update.segment<3>(3); 135 | Eigen::Quaterniond quat_delta(Eigen::AngleAxisd(rot_delta.norm(), rot_delta.normalized())); 136 | X.rot = (X.rot * quat_delta).normalized(); 137 | 138 | // Quaternion to imu rotation 139 | Eigen::Vector3d imu_rot_delta = state_update.segment<3>(24); 140 | Eigen::Quaterniond imu_quat_delta(Eigen::AngleAxisd(imu_rot_delta.norm(), imu_rot_delta.normalized())); 141 | X.imu_rot = (X.imu_rot * imu_quat_delta).normalized(); 142 | 143 | // Covariance update 144 | P = P - K * H * P; 145 | } 146 | 147 | private: // state check 148 | void CheckStateInitialized() { 149 | if (sqrt(P_(S_ROLL, S_ROLL)) < 5.0 * M_PI / 180.0 && sqrt(P_(S_PITCH, S_PITCH)) < 5.0 * M_PI / 180.0 && 150 | sqrt(P_(S_YAW, S_YAW)) < 5.0 * M_PI / 180.0 && sqrt(P_(S_X, S_X)) < 1.0 && sqrt(P_(S_Y, S_Y)) < 1.0) { 151 | if (b_state_initialized_ == false) { 152 | std::cout << GREEN << REVERSE << "STATE Initialized!" << RESET << std::endl; 153 | b_state_initialized_ = true; 154 | } 155 | } 156 | else { 157 | if (b_state_initialized_ == true) { 158 | std::cout << YELLOW << "STATE Not Initialized!" << RESET << std::endl; 159 | b_state_initialized_ = false; 160 | } 161 | } 162 | } 163 | 164 | void CheckYawInitialized() { 165 | if (sqrt(P_(S_YAW, S_YAW)) < 5.0 * M_PI / 180.0) { 166 | if (b_yaw_initialized_ == false) { 167 | std::cout << GREEN << REVERSE << "YAW Initialized!" << RESET << std::endl; 168 | b_yaw_initialized_ = true; 169 | } 170 | } 171 | else { 172 | if (b_yaw_initialized_ == true) { 173 | std::cout << YELLOW << "YAW Not Initialized!" << RESET << std::endl; 174 | b_yaw_initialized_ = false; 175 | } 176 | } 177 | } 178 | 179 | void CheckRotationStabilized() { 180 | if (sqrt(P_(S_ROLL, S_ROLL)) < 0.2 * M_PI / 180.0 && sqrt(P_(S_PITCH, S_PITCH)) < 0.2 * M_PI / 180.0 && 181 | sqrt(P_(S_YAW, S_YAW)) < 0.2 * M_PI / 180.0) { 182 | if (b_rotation_stabilized_ == false) { 183 | std::cout << GREEN << REVERSE << "ROTATION Stabilized!" << RESET << std::endl; 184 | b_rotation_stabilized_ = true; 185 | } 186 | } 187 | else { 188 | if (b_rotation_stabilized_ == true) { 189 | std::cout << YELLOW << "ROTATION Unstabilized!" << RESET << std::endl; 190 | b_rotation_stabilized_ = false; 191 | } 192 | } 193 | } 194 | 195 | void CheckStateStabilized() { 196 | if (sqrt(P_(S_ROLL, S_ROLL)) < 0.2 * M_PI / 180.0 && sqrt(P_(S_PITCH, S_PITCH)) < 0.2 * M_PI / 180.0 && 197 | sqrt(P_(S_YAW, S_YAW)) < 0.2 * M_PI / 180.0 && sqrt(P_(S_X, S_X)) < 0.5 && sqrt(P_(S_Y, S_Y)) < 0.5) { 198 | if (b_state_stabilized_ == false) { 199 | std::cout << GREEN << REVERSE << "STATE Stabilized!" << RESET << std::endl; 200 | b_state_stabilized_ = true; 201 | } 202 | } 203 | else { 204 | if (b_state_stabilized_ == true) { 205 | std::cout << YELLOW << "STATE Unstabilized!" << RESET << std::endl; 206 | b_state_stabilized_ = false; 207 | } 208 | } 209 | } 210 | 211 | void PrintState() { 212 | std::cout << RESET << "\n----------------------------------------" << std::endl; 213 | 214 | // GNSS Warning 215 | if (prev_timestamp_ - prev_gnss_.timestamp > 1.0) { 216 | std::cout << YELLOW << "GNSS Not Updated!" << RESET << std::endl; 217 | } 218 | 219 | // Print data setting ex: GPS: NavSatFix, CAN: O, PCM: X 220 | const char* gps_types[] = {"NavSatFix", "Odometry"}; 221 | std::cout << "GPS: " << (cfg_.b_use_gps ? gps_types[(int)cfg_.i_gps_type] : "X") << ", "; 222 | std::cout << "CAN: " << (cfg_.b_use_can ? "O" : "X") << ", "; 223 | std::cout << "PCM: " << (cfg_.b_use_pcm_matching ? "O" : "X") << std::endl; 224 | 225 | // State init print 226 | if (b_state_initialized_ == false) { 227 | std::cout << YELLOW << "State Not Initialized!, " << RESET; 228 | } 229 | else { 230 | std::cout << GREEN << "State Init, " << RESET; 231 | } 232 | 233 | if (b_state_stabilized_ == false) { 234 | std::cout << YELLOW << "State Unstabilized!" << RESET << std::endl; 235 | } 236 | else { 237 | std::cout << GREEN << "State Stabilized" << RESET << std::endl; 238 | } 239 | 240 | std::cout << std::fixed << std::setprecision(3); 241 | std::cout << "State Std\n" 242 | << "X: " << sqrt(P_(S_X, S_X)) << " Y: " << sqrt(P_(S_Y, S_Y)) << " Z: " << sqrt(P_(S_Z, S_Z)) 243 | << " m \n" 244 | << "Roll: " << sqrt(P_(S_ROLL, S_ROLL)) * 180.0 / M_PI 245 | << " Pitch: " << sqrt(P_(S_PITCH, S_PITCH)) * 180.0 / M_PI 246 | << " Yaw: " << sqrt(P_(S_YAW, S_YAW)) * 180.0 / M_PI << " deg \n" 247 | << std::endl; 248 | 249 | if (cfg_.b_imu_estimate_calibration) { 250 | std::cout << "IMU Calibration:\n" 251 | << "Rot: " << RotToVec(S_.imu_rot.toRotationMatrix()).transpose() * 180.0 / M_PI << " deg\n" 252 | << "Std: " << sqrt(P_(S_IMU_ROLL, S_IMU_ROLL)) * 180.0 / M_PI << " " 253 | << sqrt(P_(S_IMU_PITCH, S_IMU_PITCH)) * 180.0 / M_PI << " " 254 | << sqrt(P_(S_IMU_YAW, S_IMU_YAW)) * 180.0 / M_PI << " deg" << std::endl; 255 | } 256 | 257 | std::cout << std::fixed << std::setprecision(6); 258 | 259 | std::cout << "----------------------------------------" << std::endl; 260 | } 261 | 262 | private: // config 263 | EkfLocalizationConfig cfg_; 264 | 265 | private: // variables 266 | // mutex 267 | std::mutex mutex_state_; 268 | 269 | bool b_reset_for_init_prediction_ = true; 270 | 271 | bool b_state_initialized_ = false; 272 | bool b_yaw_initialized_ = false; 273 | bool b_rotation_stabilized_ = false; 274 | bool b_state_stabilized_ = false; 275 | 276 | bool b_pcm_init_on_going_ = false; 277 | bool b_vehicle_imu_calib_started_ = false; 278 | 279 | double d_can_yaw_rate_bias_rad_ = 0.0; 280 | 281 | EkfState S_; // state 282 | Eigen::Matrix P_; // covariance 283 | 284 | int i_pcm_update_count_ = 0; 285 | 286 | CanStruct prev_can_; 287 | EkfGnssMeasurement prev_gnss_; 288 | EgoState prev_ego_state_; 289 | double prev_timestamp_; 290 | }; 291 | 292 | #endif -------------------------------------------------------------------------------- /src/app/localization/ekf_localization/include/ekf_localization.hpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file ekf_localization.hpp 3 | * @author Jaeyoung Jo 4 | * @brief Extended Kalman Filter for various state estimation 5 | * @version 0.1 6 | * @date 2024-10-28 7 | * 8 | * @copyright Copyright (c) 2024 9 | */ 10 | 11 | #ifndef __EKF_LOCALIZATION__ 12 | #define __EKF_LOCALIZATION__ 13 | #pragma once 14 | 15 | // STD header 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | #include 24 | 25 | // System Header 26 | #include "ini_parser.h" 27 | 28 | // ROS header 29 | #include 30 | #include 31 | #include 32 | #include 33 | #include 34 | #include 35 | #include 36 | #include 37 | #include 38 | #include 39 | #include 40 | 41 | // Utility header 42 | #include 43 | 44 | 45 | // Algorithm header 46 | #include "ekf_algorithm.hpp" 47 | #include "ekf_localization_config.hpp" 48 | 49 | #include 50 | #include "GeographicLib/UTMUPS.hpp" 51 | #include "GeographicLib/Geocentric.hpp" 52 | 53 | using namespace ros; 54 | using namespace tf; 55 | using namespace std; 56 | 57 | class EkfLocalization { 58 | public: 59 | // Constructor 60 | explicit EkfLocalization(std::string node_name, double period); 61 | // Destructor 62 | virtual ~EkfLocalization(); 63 | 64 | void Init(); 65 | void Run(); 66 | void PublishInThread(); // 실제로 사용되는 publish 함수 67 | void ProcessINI(); 68 | 69 | bool GnssTimeCompensation(const EkfGnssMeasurement& i_gnss, EkfGnssMeasurement& o_gnss); 70 | 71 | void UpdateEgoMarker(EgoState ego_ekf_state); 72 | void UpdateGpsEgoMarker(EkfGnssMeasurement ego_gps_state); 73 | void UpdateTF(EgoState ego_ekf_state); 74 | void UpdateEkfOdom(EgoState ego_ekf_state); 75 | void UpdateGpsOdom(EkfGnssMeasurement gnss); 76 | void UpdateEkfText(const EgoState ego_ekf_state); 77 | 78 | void CallbackNavsatFix(const sensor_msgs::NavSatFix::ConstPtr& msg); 79 | void CallbackCAN(const geometry_msgs::TwistStampedConstPtr& msg); 80 | void CallbackImu(const sensor_msgs::Imu::ConstPtr& msg); 81 | void CallbackPcmOdom(const nav_msgs::Odometry::ConstPtr& msg); 82 | void CallbackPcmInitOdom(const nav_msgs::Odometry::ConstPtr& msg); 83 | 84 | Eigen::Vector3d ProjectGpsPoint(const double& lat, const double& lon, const double& height); 85 | 86 | void Exec(int num_thread=4); 87 | void MainLoop(); 88 | 89 | private: 90 | ros::Subscriber sub_navsatfix_; 91 | ros::Subscriber sub_can_; 92 | ros::Subscriber sub_imu_; 93 | ros::Subscriber sub_pcm_odom_; 94 | ros::Subscriber sub_pcm_init_odom_; 95 | 96 | ros::Publisher pub_vehicle_state_; 97 | ros::Publisher pub_reference_; 98 | ros::Publisher pub_ekf_pose_odom_; 99 | ros::Publisher pub_ekf_ego_marker_; 100 | ros::Publisher pub_gps_ego_marker_; 101 | ros::Publisher pub_rviz_ekf_text_; 102 | 103 | ros::Publisher pub_x_plot_; 104 | ros::Publisher pub_y_plot_; 105 | ros::Publisher pub_z_plot_; 106 | ros::Publisher pub_vx_plot_; 107 | ros::Publisher pub_vy_plot_; 108 | ros::Publisher pub_vz_plot_; 109 | ros::Publisher pub_ax_plot_; 110 | ros::Publisher pub_ay_plot_; 111 | ros::Publisher pub_az_plot_; 112 | ros::Publisher pub_roll_deg_plot_; 113 | ros::Publisher pub_pitch_deg_plot_; 114 | ros::Publisher pub_yaw_deg_plot_; 115 | 116 | ros::Publisher pub_satellite_nav_fix_; 117 | 118 | ros::Publisher pub_gps_pose_odom_; 119 | 120 | private: // algorithm 121 | std::shared_ptr ptr_ekf_algorithm_; 122 | 123 | private: // config 124 | IniParser util_ini_parser_; 125 | IniParser util_calib_ini_parser_; 126 | EkfLocalizationConfig cfg_; 127 | 128 | private: // mutex 129 | std::mutex mutex_vehicle_can_; 130 | std::mutex mutex_ekf_state_deque_; 131 | 132 | private: // Variables 133 | // Data 134 | geometry_msgs::TwistStamped i_can_; 135 | 136 | std::deque deq_ekf_state_; 137 | 138 | std::vector vec_ego_state_; 139 | std::vector> vec_ekf_time_; 140 | 141 | bool b_bestpos_used_ = false; 142 | 143 | // msgs 144 | nav_msgs::Odometry o_ekf_pose_odom_; 145 | visualization_msgs::Marker o_ekf_ego_cov_marker_msgs_; 146 | jsk_rviz_plugins::OverlayText o_rviz_ekf_text_; 147 | 148 | nav_msgs::Odometry o_gps_pose_odom_; 149 | visualization_msgs::Marker o_gps_cov_marker_msgs_; 150 | 151 | tf::TransformBroadcaster tf_broadcaster_; 152 | 153 | std::string task_name_; 154 | double task_period_; 155 | double task_rate_; 156 | ros::Time update_time_; 157 | ros::Duration execution_time_; 158 | }; 159 | 160 | #endif -------------------------------------------------------------------------------- /src/app/localization/ekf_localization/include/ekf_localization_config.hpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file ekf_localization_config.hpp 3 | * @brief configuration hpp file for ekf localization node 4 | * 5 | * @authors Jaeyoung Jo (wodud3743@gmail.com) 6 | * 7 | * @date 2024-12-18 created by Jaeyoung Jo 8 | * 9 | */ 10 | 11 | #ifndef __EKF_LOCALIZATION_CONFIG_HPP__ 12 | #define __EKF_LOCALIZATION_CONFIG_HPP__ 13 | #pragma once 14 | 15 | // STD Header 16 | #include 17 | 18 | enum class GpsType { NAVSATFIX = 0, BESTPOS, ODOMETRY }; 19 | 20 | typedef struct { 21 | // Node 22 | bool b_debug_print = false; 23 | int i_vehicle_origin = 0; 24 | std::string projection_mode = ""; 25 | double ref_latitude = 0.0; 26 | double ref_longitude = 0.0; 27 | double ref_altitude = 0.0; 28 | 29 | double gnss_uncertainy_max_m; 30 | 31 | std::string s_can_topic_name = ""; 32 | std::string s_imu_topic_name = ""; 33 | std::string s_navsatfix_topic_name = ""; 34 | 35 | GpsType i_gps_type; 36 | bool b_use_gps; 37 | bool b_use_can; 38 | bool b_use_imu; 39 | bool b_use_pcm_matching; 40 | 41 | std::vector vec_d_ego_to_imu_trans; 42 | std::vector vec_d_ego_to_imu_rot; 43 | Eigen::Vector3d vec_ego_to_imu_trans; 44 | Eigen::Matrix3d mat_ego_to_imu_rot; 45 | 46 | std::vector vec_d_ego_to_gps_trans; 47 | std::vector vec_d_ego_to_gps_rot; 48 | Eigen::Vector3d vec_ego_to_gps_trans; 49 | Eigen::Matrix3d mat_ego_to_gps_rot; 50 | 51 | // Algorithm 52 | bool b_debug_imu_print; 53 | 54 | double d_can_vel_scale_factor; 55 | 56 | double d_imu_gravity; 57 | bool b_imu_estimate_gravity; 58 | bool b_imu_estimate_calibration; 59 | 60 | bool b_use_zupt; 61 | bool b_use_complementary_filter; 62 | 63 | double d_ekf_init_x_m; 64 | double d_ekf_init_y_m; 65 | double d_ekf_init_z_m; 66 | double d_ekf_init_roll_deg; 67 | double d_ekf_init_pitch_deg; 68 | double d_ekf_init_yaw_deg; 69 | 70 | double d_state_std_pos_m; 71 | double d_state_std_rot_deg; 72 | double d_state_std_vel_mps; 73 | double d_state_std_gyro_dps; 74 | double d_state_std_acc_mps; 75 | 76 | double d_imu_std_gyro_dps; 77 | double d_imu_std_acc_mps; 78 | 79 | double d_ekf_imu_bias_cov_gyro; 80 | double d_ekf_imu_bias_cov_acc; 81 | 82 | double d_ekf_gnss_min_cov_x_m; 83 | double d_ekf_gnss_min_cov_y_m; 84 | double d_ekf_gnss_min_cov_z_m; 85 | double d_ekf_gnss_min_cov_roll_deg; 86 | double d_ekf_gnss_min_cov_pitch_deg; 87 | double d_ekf_gnss_min_cov_yaw_deg; 88 | 89 | double d_ekf_can_meas_uncertainty_vel_mps; 90 | double d_ekf_can_meas_uncertainty_yaw_rate_deg; 91 | 92 | double d_ekf_bestvel_meas_uncertainty_vel_mps; 93 | 94 | } EkfLocalizationConfig; 95 | 96 | #endif // __EKF_LOCALIZATION_CONFIG_HPP__ -------------------------------------------------------------------------------- /src/app/localization/ekf_localization/launch/ekf_localization.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | -------------------------------------------------------------------------------- /src/app/localization/ekf_localization/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | ekf_localization 4 | 0.0.0 5 | The ekf_localization package 6 | 7 | 8 | 9 | 10 | JaeyoungJo 11 | 12 | 13 | 14 | 15 | MIT 16 | 17 | 18 | 19 | catkin 20 | roscpp 21 | roscpp 22 | roscpp 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | -------------------------------------------------------------------------------- /src/app/localization/ekf_localization/rviz/ekf_localization_rviz.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 70 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Global Options1 8 | - /Status1 9 | - /EgoEkf Grid1/Status1 10 | - /TF1/Frames1 11 | - /EKF1/GPS Pose Odom1/Shape1 12 | - /EKF1/GPS Pose Odom1/Covariance1/Position1 13 | - /EKF1/EKF Pose Odom1/Covariance1/Position1 14 | - /Map Matching1 15 | - /Map Matching1/PCM Pose Odom1/Status1 16 | - /Map Matching1/PCM Pose Odom1/Covariance1/Position1 17 | - /Map Matching1/PCM Pose Odom1/Covariance1/Orientation1 18 | - /Map Matching1/Pcm Init Pose Odom1/Shape1 19 | - /Map Matching1/VoxelMap1/Autocompute Value Bounds1 20 | Splitter Ratio: 0.47900113463401794 21 | Tree Height: 741 22 | - Class: rviz/Selection 23 | Name: Selection 24 | - Class: rviz/Tool Properties 25 | Expanded: 26 | - /2D Pose Estimate1 27 | - /Publish Point1 28 | Name: Tool Properties 29 | Splitter Ratio: 0.5886790156364441 30 | - Class: rviz/Views 31 | Expanded: 32 | - /Current View1 33 | Name: Views 34 | Splitter Ratio: 0.5 35 | - Class: rviz/Time 36 | Name: Time 37 | SyncMode: 0 38 | SyncSource: VoxelMap 39 | Preferences: 40 | PromptSaveOnExit: true 41 | Toolbars: 42 | toolButtonStyle: 2 43 | Visualization Manager: 44 | Class: "" 45 | Displays: 46 | - Alpha: 1 47 | Class: rviz/Axes 48 | Enabled: true 49 | Length: 20 50 | Name: World Axis 51 | Radius: 1 52 | Reference Frame: world 53 | Show Trail: false 54 | Value: true 55 | - Alpha: 1 56 | Cell Size: 10 57 | Class: rviz/Grid 58 | Color: 160; 160; 164 59 | Enabled: false 60 | Line Style: 61 | Line Width: 0.029999999329447746 62 | Value: Lines 63 | Name: world Grid 64 | Normal Cell Count: 0 65 | Offset: 66 | X: 0 67 | Y: 0 68 | Z: 0 69 | Plane: XY 70 | Plane Cell Count: 200 71 | Reference Frame: world 72 | Value: false 73 | - Alpha: 0.5 74 | Cell Size: 1 75 | Class: rviz/Grid 76 | Color: 35; 221; 230 77 | Enabled: true 78 | Line Style: 79 | Line Width: 0.029999999329447746 80 | Value: Lines 81 | Name: EgoEkf Grid 82 | Normal Cell Count: 0 83 | Offset: 84 | X: 0 85 | Y: 0 86 | Z: 0 87 | Plane: XY 88 | Plane Cell Count: 20 89 | Reference Frame: ego_frame 90 | Value: true 91 | - Class: jsk_rviz_plugin/TFTrajectory 92 | Enabled: true 93 | Name: TFTrajectory 94 | Value: true 95 | color: 25; 255; 240 96 | duration: 20 97 | frame: ego_frame 98 | line_width: 0.029999999329447746 99 | - Class: rviz/TF 100 | Enabled: true 101 | Filter (blacklist): "" 102 | Filter (whitelist): "" 103 | Frame Timeout: 15 104 | Frames: 105 | All Enabled: false 106 | ego_frame: 107 | Value: true 108 | imu: 109 | Value: true 110 | velodyne: 111 | Value: true 112 | world: 113 | Value: true 114 | Marker Alpha: 1 115 | Marker Scale: 2 116 | Name: TF 117 | Show Arrows: true 118 | Show Axes: true 119 | Show Names: true 120 | Tree: 121 | ego_frame: 122 | imu: 123 | {} 124 | velodyne: 125 | {} 126 | world: 127 | {} 128 | Update Interval: 0 129 | Value: true 130 | - Class: rviz/Group 131 | Displays: 132 | - Angle Tolerance: 0.10000000149011612 133 | Class: rviz/Odometry 134 | Covariance: 135 | Orientation: 136 | Alpha: 0.5 137 | Color: 255; 255; 127 138 | Color Style: Unique 139 | Frame: Local 140 | Offset: 1 141 | Scale: 1 142 | Value: false 143 | Position: 144 | Alpha: 0.10000000149011612 145 | Color: 157; 0; 0 146 | Scale: 1 147 | Value: true 148 | Value: true 149 | Enabled: true 150 | Keep: 100 151 | Name: GPS Pose Point 152 | Position Tolerance: 0.10000000149011612 153 | Queue Size: 10 154 | Shape: 155 | Alpha: 0.5 156 | Axes Length: 0.0010000000474974513 157 | Axes Radius: 0.4000000059604645 158 | Color: 255; 25; 0 159 | Head Length: 0.5 160 | Head Radius: 0.5 161 | Shaft Length: 0 162 | Shaft Radius: 0 163 | Value: Axes 164 | Topic: /app/loc/gps_pose_odom 165 | Unreliable: false 166 | Value: true 167 | - Angle Tolerance: 0.10000000149011612 168 | Class: rviz/Odometry 169 | Covariance: 170 | Orientation: 171 | Alpha: 0.5 172 | Color: 255; 255; 127 173 | Color Style: Unique 174 | Frame: Local 175 | Offset: 1 176 | Scale: 1 177 | Value: false 178 | Position: 179 | Alpha: 0.10000000149011612 180 | Color: 157; 0; 0 181 | Scale: 1 182 | Value: true 183 | Value: true 184 | Enabled: false 185 | Keep: 100 186 | Name: GPS Pose Odom 187 | Position Tolerance: 0.10000000149011612 188 | Queue Size: 100 189 | Shape: 190 | Alpha: 0.5 191 | Axes Length: 1 192 | Axes Radius: 0.10000000149011612 193 | Color: 255; 25; 0 194 | Head Length: 0.30000001192092896 195 | Head Radius: 0.30000001192092896 196 | Shaft Length: 1 197 | Shaft Radius: 0.10000000149011612 198 | Value: Arrow 199 | Topic: /app/loc/gps_pose_odom 200 | Unreliable: false 201 | Value: false 202 | - Angle Tolerance: 0.10000000149011612 203 | Class: rviz/Odometry 204 | Covariance: 205 | Orientation: 206 | Alpha: 0.5 207 | Color: 255; 255; 127 208 | Color Style: Unique 209 | Frame: Local 210 | Offset: 1 211 | Scale: 1 212 | Value: false 213 | Position: 214 | Alpha: 0.10000000149011612 215 | Color: 63; 193; 204 216 | Scale: 1 217 | Value: true 218 | Value: false 219 | Enabled: true 220 | Keep: 100 221 | Name: EKF Pose Odom 222 | Position Tolerance: 0.10000000149011612 223 | Queue Size: 100 224 | Shape: 225 | Alpha: 0.800000011920929 226 | Axes Length: 0.5 227 | Axes Radius: 0.07000000029802322 228 | Color: 3; 255; 247 229 | Head Length: 0.30000001192092896 230 | Head Radius: 0.30000001192092896 231 | Shaft Length: 1 232 | Shaft Radius: 0.10000000149011612 233 | Value: Axes 234 | Topic: /app/loc/ekf_pose_odom 235 | Unreliable: false 236 | Value: true 237 | - Class: rviz/Marker 238 | Enabled: true 239 | Marker Topic: /app/loc/ekf_ego_marker 240 | Name: EgoEkfMarker 241 | Namespaces: 242 | {} 243 | Queue Size: 100 244 | Value: true 245 | - Class: rviz/Marker 246 | Enabled: false 247 | Marker Topic: /app/loc/gps_ego_marker 248 | Name: EgoGpsMarker 249 | Namespaces: 250 | {} 251 | Queue Size: 100 252 | Value: false 253 | - Align Bottom: false 254 | Background Alpha: 0.800000011920929 255 | Background Color: 0; 0; 0 256 | Class: jsk_rviz_plugin/OverlayText 257 | Enabled: false 258 | Foreground Alpha: 0.800000011920929 259 | Foreground Color: 25; 255; 240 260 | Invert Shadow: false 261 | Name: EKF Status 262 | Overtake BG Color Properties: false 263 | Overtake FG Color Properties: false 264 | Overtake Position Properties: true 265 | Topic: /app/loc/ekf_text 266 | Value: false 267 | font: DejaVu Sans Mono 268 | height: 128 269 | left: 0 270 | line width: 2 271 | text size: 12 272 | top: 0 273 | width: 128 274 | - Buffer length: 50 275 | Class: jsk_rviz_plugin/Plotter2D 276 | Enabled: true 277 | Name: EKF X 278 | Show Value: true 279 | Topic: /app/loc/ekf/x_plot 280 | Value: true 281 | auto color change: false 282 | auto scale: true 283 | background color: 0; 0; 0 284 | backround alpha: 0.5 285 | border: true 286 | foreground alpha: 0.699999988079071 287 | foreground color: 25; 255; 240 288 | height: 100 289 | left: 100 290 | linewidth: 2 291 | max color: 255; 0; 0 292 | max value: 1 293 | min value: -1 294 | show caption: true 295 | text size: 12 296 | top: 20 297 | update interval: 0.03999999910593033 298 | width: 100 299 | - Buffer length: 50 300 | Class: jsk_rviz_plugin/Plotter2D 301 | Enabled: true 302 | Name: EKF Y 303 | Show Value: true 304 | Topic: /app/loc/ekf/y_plot 305 | Value: true 306 | auto color change: false 307 | auto scale: true 308 | background color: 0; 0; 0 309 | backround alpha: 0.5 310 | border: true 311 | foreground alpha: 0.699999988079071 312 | foreground color: 25; 255; 240 313 | height: 100 314 | left: 200 315 | linewidth: 2 316 | max color: 255; 0; 0 317 | max value: 1 318 | min value: -1 319 | show caption: true 320 | text size: 12 321 | top: 20 322 | update interval: 0.03999999910593033 323 | width: 100 324 | - Buffer length: 50 325 | Class: jsk_rviz_plugin/Plotter2D 326 | Enabled: true 327 | Name: EKF Z 328 | Show Value: true 329 | Topic: /app/loc/ekf/z_plot 330 | Value: true 331 | auto color change: false 332 | auto scale: true 333 | background color: 0; 0; 0 334 | backround alpha: 0.5 335 | border: true 336 | foreground alpha: 0.699999988079071 337 | foreground color: 25; 255; 240 338 | height: 100 339 | left: 300 340 | linewidth: 2 341 | max color: 255; 0; 0 342 | max value: 1 343 | min value: -1 344 | show caption: true 345 | text size: 12 346 | top: 20 347 | update interval: 0.03999999910593033 348 | width: 100 349 | - Buffer length: 50 350 | Class: jsk_rviz_plugin/Plotter2D 351 | Enabled: true 352 | Name: EKF VX 353 | Show Value: true 354 | Topic: /app/loc/ekf/vx_plot 355 | Value: true 356 | auto color change: false 357 | auto scale: true 358 | background color: 0; 0; 0 359 | backround alpha: 0.5 360 | border: true 361 | foreground alpha: 0.699999988079071 362 | foreground color: 0; 255; 0 363 | height: 100 364 | left: 400 365 | linewidth: 2 366 | max color: 255; 0; 0 367 | max value: 1 368 | min value: -1 369 | show caption: true 370 | text size: 12 371 | top: 20 372 | update interval: 0.03999999910593033 373 | width: 100 374 | - Buffer length: 50 375 | Class: jsk_rviz_plugin/Plotter2D 376 | Enabled: true 377 | Name: EKF VY 378 | Show Value: true 379 | Topic: /app/loc/ekf/vy_plot 380 | Value: true 381 | auto color change: false 382 | auto scale: true 383 | background color: 0; 0; 0 384 | backround alpha: 0.5 385 | border: true 386 | foreground alpha: 0.699999988079071 387 | foreground color: 0; 255; 0 388 | height: 100 389 | left: 500 390 | linewidth: 2 391 | max color: 255; 0; 0 392 | max value: 1 393 | min value: -1 394 | show caption: true 395 | text size: 12 396 | top: 20 397 | update interval: 0.03999999910593033 398 | width: 100 399 | - Buffer length: 50 400 | Class: jsk_rviz_plugin/Plotter2D 401 | Enabled: true 402 | Name: EKF VZ 403 | Show Value: true 404 | Topic: /app/loc/ekf/vz_plot 405 | Value: true 406 | auto color change: false 407 | auto scale: true 408 | background color: 0; 0; 0 409 | backround alpha: 0.5 410 | border: true 411 | foreground alpha: 0.699999988079071 412 | foreground color: 0; 255; 0 413 | height: 100 414 | left: 600 415 | linewidth: 2 416 | max color: 255; 0; 0 417 | max value: 1 418 | min value: -1 419 | show caption: true 420 | text size: 12 421 | top: 20 422 | update interval: 0.03999999910593033 423 | width: 100 424 | - Buffer length: 50 425 | Class: jsk_rviz_plugin/Plotter2D 426 | Enabled: true 427 | Name: EKF AX 428 | Show Value: true 429 | Topic: /app/loc/ekf/ax_plot 430 | Value: true 431 | auto color change: false 432 | auto scale: true 433 | background color: 0; 0; 0 434 | backround alpha: 0.5 435 | border: true 436 | foreground alpha: 0.699999988079071 437 | foreground color: 255; 85; 0 438 | height: 100 439 | left: 700 440 | linewidth: 2 441 | max color: 255; 0; 0 442 | max value: 1 443 | min value: -1 444 | show caption: true 445 | text size: 12 446 | top: 20 447 | update interval: 0.03999999910593033 448 | width: 100 449 | - Buffer length: 50 450 | Class: jsk_rviz_plugin/Plotter2D 451 | Enabled: true 452 | Name: EKF AY 453 | Show Value: true 454 | Topic: /app/loc/ekf/ay_plot 455 | Value: true 456 | auto color change: false 457 | auto scale: true 458 | background color: 0; 0; 0 459 | backround alpha: 0.5 460 | border: true 461 | foreground alpha: 0.699999988079071 462 | foreground color: 255; 85; 0 463 | height: 100 464 | left: 800 465 | linewidth: 2 466 | max color: 255; 0; 0 467 | max value: 1 468 | min value: -1 469 | show caption: true 470 | text size: 12 471 | top: 20 472 | update interval: 0.03999999910593033 473 | width: 100 474 | - Buffer length: 50 475 | Class: jsk_rviz_plugin/Plotter2D 476 | Enabled: true 477 | Name: EKF AZ 478 | Show Value: true 479 | Topic: /app/loc/ekf/az_plot 480 | Value: true 481 | auto color change: false 482 | auto scale: true 483 | background color: 0; 0; 0 484 | backround alpha: 0.5 485 | border: true 486 | foreground alpha: 0.699999988079071 487 | foreground color: 255; 85; 0 488 | height: 100 489 | left: 900 490 | linewidth: 2 491 | max color: 255; 0; 0 492 | max value: 1 493 | min value: -1 494 | show caption: true 495 | text size: 12 496 | top: 20 497 | update interval: 0.03999999910593033 498 | width: 100 499 | - Class: jsk_rviz_plugin/PieChart 500 | Enabled: true 501 | Name: EKF Roll 502 | Topic: /app/loc/ekf/roll_deg_plot 503 | Value: true 504 | auto color change: false 505 | background color: 0; 0; 0 506 | backround alpha: 0 507 | clockwise rotate direction: false 508 | foreground alpha: 0.699999988079071 509 | foreground alpha 2: 0.4000000059604645 510 | foreground color: 25; 255; 240 511 | left: 100 512 | max color: 255; 0; 0 513 | max color change threthold: 0 514 | max value: -5 515 | med color: 255; 0; 0 516 | med color change threthold: 0 517 | min value: 5 518 | show caption: true 519 | size: 100 520 | text size: 12 521 | top: 150 522 | - Class: jsk_rviz_plugin/PieChart 523 | Enabled: true 524 | Name: EKF Pitch 525 | Topic: /app/loc/ekf/pitch_deg_plot 526 | Value: true 527 | auto color change: false 528 | background color: 0; 0; 0 529 | backround alpha: 0 530 | clockwise rotate direction: false 531 | foreground alpha: 0.699999988079071 532 | foreground alpha 2: 0.4000000059604645 533 | foreground color: 25; 255; 240 534 | left: 200 535 | max color: 255; 0; 0 536 | max color change threthold: 0 537 | max value: -5 538 | med color: 255; 0; 0 539 | med color change threthold: 0 540 | min value: 5 541 | show caption: true 542 | size: 100 543 | text size: 12 544 | top: 150 545 | - Class: jsk_rviz_plugin/PieChart 546 | Enabled: true 547 | Name: EKF Yaw 548 | Topic: /app/loc/ekf/yaw_deg_plot 549 | Value: true 550 | auto color change: false 551 | background color: 0; 0; 0 552 | backround alpha: 0 553 | clockwise rotate direction: false 554 | foreground alpha: 0.699999988079071 555 | foreground alpha 2: 0.4000000059604645 556 | foreground color: 25; 255; 240 557 | left: 300 558 | max color: 255; 0; 0 559 | max color change threthold: 0 560 | max value: -180 561 | med color: 255; 0; 0 562 | med color change threthold: 0 563 | min value: 180 564 | show caption: true 565 | size: 100 566 | text size: 12 567 | top: 150 568 | Enabled: true 569 | Name: EKF 570 | - Class: rviz/Group 571 | Displays: 572 | - Angle Tolerance: 0.10000000149011612 573 | Class: rviz/Odometry 574 | Covariance: 575 | Orientation: 576 | Alpha: 0.5 577 | Color: 255; 255; 127 578 | Color Style: Unique 579 | Frame: Local 580 | Offset: 1 581 | Scale: 1 582 | Value: true 583 | Position: 584 | Alpha: 0.4000000059604645 585 | Color: 113; 204; 28 586 | Scale: 1 587 | Value: true 588 | Value: true 589 | Enabled: true 590 | Keep: 50 591 | Name: PCM Pose Odom 592 | Position Tolerance: 0.10000000149011612 593 | Queue Size: 100 594 | Shape: 595 | Alpha: 0.800000011920929 596 | Axes Length: 1.5 597 | Axes Radius: 0.11999999731779099 598 | Color: 0; 172; 0 599 | Head Length: 0.30000001192092896 600 | Head Radius: 0.30000001192092896 601 | Shaft Length: 1 602 | Shaft Radius: 0.10000000149011612 603 | Value: Arrow 604 | Topic: /app/loc/pcm_odom 605 | Unreliable: true 606 | Value: true 607 | - Angle Tolerance: 0.10000000149011612 608 | Class: rviz/Odometry 609 | Covariance: 610 | Orientation: 611 | Alpha: 0.5 612 | Color: 255; 255; 127 613 | Color Style: Unique 614 | Frame: Local 615 | Offset: 1 616 | Scale: 1 617 | Value: true 618 | Position: 619 | Alpha: 0.30000001192092896 620 | Color: 204; 51; 204 621 | Scale: 1 622 | Value: true 623 | Value: true 624 | Enabled: true 625 | Keep: 100 626 | Name: Pcm Init Pose Odom 627 | Position Tolerance: 0.10000000149011612 628 | Queue Size: 10 629 | Shape: 630 | Alpha: 1 631 | Axes Length: 1 632 | Axes Radius: 0.10000000149011612 633 | Color: 227; 155; 255 634 | Head Length: 0.5 635 | Head Radius: 0.6000000238418579 636 | Shaft Length: 1.5 637 | Shaft Radius: 0.20000000298023224 638 | Value: Arrow 639 | Topic: /app/loc/pcm_init_odom 640 | Unreliable: false 641 | Value: true 642 | - Alpha: 0.6000000238418579 643 | Autocompute Intensity Bounds: true 644 | Autocompute Value Bounds: 645 | Max Value: 10 646 | Min Value: -10 647 | Value: true 648 | Axis: Z 649 | Channel Name: intensity 650 | Class: rviz/PointCloud2 651 | Color: 249; 172; 255 652 | Color Transformer: FlatColor 653 | Decay Time: 0 654 | Enabled: false 655 | Invert Rainbow: false 656 | Max Color: 255; 255; 255 657 | Min Color: 0; 0; 0 658 | Name: Init Pose PC 659 | Position Transformer: XYZ 660 | Queue Size: 10 661 | Selectable: true 662 | Size (Pixels): 3 663 | Size (m): 0.009999999776482582 664 | Style: Points 665 | Topic: /app/loc/pcm_init_pc 666 | Unreliable: false 667 | Use Fixed Frame: true 668 | Use rainbow: true 669 | Value: false 670 | - Class: rviz/MarkerArray 671 | Enabled: false 672 | Marker Topic: /app/loc/voxel_map_cov 673 | Name: VoxelMapCov 674 | Namespaces: 675 | {} 676 | Queue Size: 100 677 | Value: false 678 | - Alpha: 0.5 679 | Autocompute Intensity Bounds: true 680 | Autocompute Value Bounds: 681 | Max Value: 15 682 | Min Value: -5 683 | Value: false 684 | Axis: Z 685 | Channel Name: normal_z 686 | Class: rviz/PointCloud2 687 | Color: 255; 255; 255 688 | Color Transformer: Intensity 689 | Decay Time: 0 690 | Enabled: true 691 | Invert Rainbow: false 692 | Max Color: 255; 255; 255 693 | Min Color: 0; 0; 0 694 | Name: VoxelMap 695 | Position Transformer: XYZ 696 | Queue Size: 10 697 | Selectable: true 698 | Size (Pixels): 2 699 | Size (m): 0.20000000298023224 700 | Style: Points 701 | Topic: /app/loc/voxel_map_pc 702 | Unreliable: false 703 | Use Fixed Frame: true 704 | Use rainbow: true 705 | Value: true 706 | - Alpha: 0.30000001192092896 707 | Autocompute Intensity Bounds: true 708 | Autocompute Value Bounds: 709 | Max Value: 10 710 | Min Value: -10 711 | Value: true 712 | Axis: Z 713 | Channel Name: intensity 714 | Class: rviz/PointCloud2 715 | Color: 255; 255; 255 716 | Color Transformer: Intensity 717 | Decay Time: 0 718 | Enabled: true 719 | Invert Rainbow: false 720 | Max Color: 255; 255; 255 721 | Min Color: 0; 0; 0 722 | Name: OriginPoint 723 | Position Transformer: XYZ 724 | Queue Size: 10 725 | Selectable: true 726 | Size (Pixels): 2 727 | Size (m): 0.009999999776482582 728 | Style: Points 729 | Topic: /velodyne_points 730 | Unreliable: false 731 | Use Fixed Frame: true 732 | Use rainbow: true 733 | Value: true 734 | - Alpha: 0.6000000238418579 735 | Autocompute Intensity Bounds: true 736 | Autocompute Value Bounds: 737 | Max Value: 9.870832443237305 738 | Min Value: -3.2593212127685547 739 | Value: true 740 | Axis: Z 741 | Channel Name: intensity 742 | Class: rviz/PointCloud2 743 | Color: 255; 255; 255 744 | Color Transformer: Intensity 745 | Decay Time: 0 746 | Enabled: true 747 | Invert Rainbow: false 748 | Max Color: 255; 255; 255 749 | Min Color: 0; 0; 0 750 | Name: Undistort PC 751 | Position Transformer: XYZ 752 | Queue Size: 10 753 | Selectable: true 754 | Size (Pixels): 2 755 | Size (m): 0.009999999776482582 756 | Style: Points 757 | Topic: /app/loc/undistort_pc 758 | Unreliable: false 759 | Use Fixed Frame: true 760 | Use rainbow: true 761 | Value: true 762 | - Alpha: 0.4000000059604645 763 | Autocompute Intensity Bounds: false 764 | Autocompute Value Bounds: 765 | Max Value: 10 766 | Min Value: -10 767 | Value: true 768 | Axis: Z 769 | Channel Name: intensity 770 | Class: rviz/PointCloud2 771 | Color: 255; 255; 255 772 | Color Transformer: Intensity 773 | Decay Time: 0 774 | Enabled: false 775 | Invert Rainbow: false 776 | Max Color: 255; 255; 255 777 | Max Intensity: 100 778 | Min Color: 0; 0; 0 779 | Min Intensity: 0 780 | Name: ICP World 781 | Position Transformer: XYZ 782 | Queue Size: 10 783 | Selectable: true 784 | Size (Pixels): 3 785 | Size (m): 0.009999999776482582 786 | Style: Points 787 | Topic: /app/loc/icp_full_map_pc 788 | Unreliable: false 789 | Use Fixed Frame: true 790 | Use rainbow: false 791 | Value: false 792 | - Alpha: 0.699999988079071 793 | Autocompute Intensity Bounds: true 794 | Autocompute Value Bounds: 795 | Max Value: 10 796 | Min Value: -10 797 | Value: true 798 | Axis: Z 799 | Channel Name: intensity 800 | Class: rviz/PointCloud2 801 | Color: 255; 255; 255 802 | Color Transformer: FlatColor 803 | Decay Time: 0 804 | Enabled: true 805 | Invert Rainbow: false 806 | Max Color: 255; 255; 255 807 | Min Color: 0; 0; 0 808 | Name: ICP World DS 809 | Position Transformer: XYZ 810 | Queue Size: 10 811 | Selectable: true 812 | Size (Pixels): 3 813 | Size (m): 0.009999999776482582 814 | Style: Points 815 | Topic: /app/loc/icp_map_pc 816 | Unreliable: false 817 | Use Fixed Frame: true 818 | Use rainbow: true 819 | Value: true 820 | Enabled: true 821 | Name: Map Matching 822 | Enabled: true 823 | Global Options: 824 | Background Color: 0; 0; 0 825 | Default Light: true 826 | Fixed Frame: world 827 | Frame Rate: 30 828 | Name: root 829 | Tools: 830 | - Class: rviz/Interact 831 | Hide Inactive Objects: true 832 | - Class: rviz/MoveCamera 833 | - Class: rviz/Select 834 | - Class: rviz/FocusCamera 835 | - Class: rviz/Measure 836 | - Class: rviz/SetInitialPose 837 | Theta std deviation: 0.2617993950843811 838 | Topic: /initialpose 839 | X std deviation: 0.5 840 | Y std deviation: 0.5 841 | - Class: rviz/SetGoal 842 | Topic: /move_base_simple/goal 843 | - Class: rviz/PublishPoint 844 | Single click: true 845 | Topic: /clicked_point 846 | Value: true 847 | Views: 848 | Current: 849 | Angle: 0 850 | Class: rviz/TopDownOrtho 851 | Enable Stereo Rendering: 852 | Stereo Eye Separation: 0.05999999865889549 853 | Stereo Focal Distance: 1 854 | Swap Stereo Eyes: false 855 | Value: false 856 | Invert Z Axis: false 857 | Name: Current View 858 | Near Clip Distance: 0.009999999776482582 859 | Scale: 17.27579689025879 860 | Target Frame: world 861 | X: -2.390747547149658 862 | Y: 1.8564375638961792 863 | Saved: ~ 864 | Window Geometry: 865 | Displays: 866 | collapsed: false 867 | Height: 1017 868 | Hide Left Dock: false 869 | Hide Right Dock: false 870 | QMainWindow State: 000000ff00000000fd00000004000000000000015600000366fc0200000009fb0000001200530065006c0065006300740069006f006e000000003d000003cd0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b00000366000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d0061006700650000000000ffffffff0000000000000000000000010000010f00000366fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003b00000366000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000078000000037fc0100000002fb0000000800540069006d00650100000000000007800000030700fffffffb0000000800540069006d006501000000000000045000000000000000000000050f0000036600000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 871 | Selection: 872 | collapsed: false 873 | Time: 874 | collapsed: false 875 | Tool Properties: 876 | collapsed: false 877 | Views: 878 | collapsed: false 879 | Width: 1920 880 | X: 1920 881 | Y: 23 882 | -------------------------------------------------------------------------------- /src/app/localization/localization_interface/localization_functions.hpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file localization_struct.hpp 3 | * @author Jaeyoung Jo 4 | * @brief Various functions for EKF based localization 5 | * @version 0.1 6 | * @date 2024-10-28 7 | * 8 | * @copyright Copyright (c) 2024 9 | */ 10 | 11 | #pragma once 12 | #ifndef _EKF_COMMON_FUNCTIONS_HPP_ 13 | #define _EKF_COMMON_FUNCTIONS_HPP_ 14 | #define PCL_NO_PRECOMPILE 15 | 16 | #include 17 | 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | #include 24 | #include 25 | 26 | #include // for opencv4 27 | 28 | #include 29 | 30 | #include 31 | #include 32 | #include 33 | #include 34 | 35 | #include 36 | #include 37 | #include 38 | #include 39 | #include 40 | #include 41 | #include 42 | #include 43 | #include 44 | #include 45 | #include 46 | #include 47 | #include 48 | #include 49 | #include 50 | #include 51 | #include 52 | 53 | // Libraries 54 | #include 55 | 56 | #include 57 | #include 58 | #include 59 | #include 60 | 61 | #include 62 | #include 63 | #include 64 | 65 | #include 66 | 67 | #include "localization_struct.hpp" 68 | 69 | // COUT macros 70 | #define START_TIMER auto start_time_##__LINE__ = std::chrono::high_resolution_clock::now() 71 | #define STOP_TIMER(name) std::cout << name << " Time: " \ 72 | << GetDurationMs(start_time_##__LINE__) << " ms" << RESET << std::endl 73 | 74 | #define START_TIMER_NAMED(timer_name) auto start_time_##timer_name = std::chrono::high_resolution_clock::now() 75 | #define STOP_TIMER_NAMED(timer_name, name) std::cout << name << " Time: " \ 76 | << GetDurationMs(start_time_##timer_name) << " ms" << RESET << std::endl 77 | 78 | 79 | // cout color 80 | const std::string RESET = "\033[0m"; 81 | const std::string RED = "\033[31m"; // ERROR 82 | const std::string GREEN = "\033[32m"; // Update, Correction 83 | const std::string YELLOW = "\033[33m"; // WARN 84 | const std::string BLUE = "\033[34m"; // blue (PCM) 85 | const std::string MAGENTA = "\033[35m"; // Purple (IMU) 86 | const std::string CYAN = "\033[36m"; // mint (CAN) 87 | const std::string WHITE = "\033[37m"; // white 88 | 89 | const std::string BOLD = "\033[1m"; 90 | const std::string DIM = "\033[2m"; 91 | const std::string UNDERLINE = "\033[4m"; 92 | const std::string BLINK = "\033[5m"; 93 | const std::string REVERSE = "\033[7m"; 94 | 95 | 96 | /** 97 | * Time measurement function (convert nanoseconds to milliseconds) 98 | * @param start_time start time 99 | * @return elapsed time (milliseconds) 100 | */ 101 | inline double GetDurationMs(const std::chrono::high_resolution_clock::time_point& start_time) { 102 | auto duration = std::chrono::duration_cast( 103 | std::chrono::high_resolution_clock::now() - start_time).count(); 104 | return duration / 1e6; 105 | } 106 | 107 | /** 108 | * Convert IMU message angular velocity to ROS format 109 | * @param thisImuMsg IMU message 110 | * @param angular_x,y,z angular velocity of each axis (output) 111 | */ 112 | template 113 | inline void ImuAngular2RosAngular(sensor_msgs::Imu* thisImuMsg, T* angular_x, T* angular_y, T* angular_z) { 114 | *angular_x = thisImuMsg->angular_velocity.x; 115 | *angular_y = thisImuMsg->angular_velocity.y; 116 | *angular_z = thisImuMsg->angular_velocity.z; 117 | } 118 | 119 | /** 120 | * Convert IMU data to vehicle coordinate system (only rotation) 121 | * @param imu_in input IMU message 122 | * @param rotation_calibration ego to IMU rotation matrix 123 | * @return ego-based IMU structure 124 | */ 125 | inline ImuStruct ImuStructConverter(const sensor_msgs::Imu& imu_in, const Eigen::Matrix3d& rotation_calibration) { 126 | // rotation_calibration 는 ego --> imu 127 | 128 | ImuStruct imu_out; 129 | imu_out.timestamp = imu_in.header.stamp.toSec(); 130 | 131 | // Rotate linear acceleration 132 | Eigen::Vector3d acc(imu_in.linear_acceleration.x, imu_in.linear_acceleration.y, imu_in.linear_acceleration.z); 133 | imu_out.acc = rotation_calibration * acc; 134 | 135 | // Rotate angular velocity 136 | Eigen::Vector3d gyr(imu_in.angular_velocity.x, imu_in.angular_velocity.y, imu_in.angular_velocity.z); 137 | imu_out.gyro = rotation_calibration * gyr; 138 | 139 | return imu_out; 140 | } 141 | 142 | /** 143 | * Convert IMU data to vehicle coordinate system (considering both rotation and translation) 144 | * @param imu_in input IMU message 145 | * @param rotation_calibration rotation matrix from ego to IMU 146 | * @param translation_calibration translation vector from ego to IMU 147 | * @return ego-based IMU structure (including centrifugal effects) 148 | */ 149 | inline ImuStruct ImuStructConverter(const sensor_msgs::Imu& imu_in, 150 | const Eigen::Matrix3d& rotation_calibration, 151 | const Eigen::Vector3d& translation_calibration) { 152 | // rotation_calibration: ego --> imu 153 | // translation_calibration: ego --> imu (ego coordinate system) 154 | // output imu is ego-based 155 | 156 | ImuStruct imu_out; 157 | imu_out.timestamp = imu_in.header.stamp.toSec(); 158 | 159 | // imu --> ego rotation matrix 160 | Eigen::Matrix3d R_imu_to_ego = rotation_calibration; 161 | 162 | // gyro 163 | Eigen::Vector3d gyr(imu_in.angular_velocity.x, imu_in.angular_velocity.y, imu_in.angular_velocity.z); 164 | 165 | gyr = R_imu_to_ego * gyr; 166 | imu_out.gyro = gyr; 167 | 168 | // acceleration 169 | Eigen::Vector3d acc(imu_in.linear_acceleration.x, imu_in.linear_acceleration.y, imu_in.linear_acceleration.z); 170 | 171 | // 1. basic rotation 172 | acc = R_imu_to_ego * acc; 173 | 174 | // 2. Add centrifugal effect (ω × (ω × r)) 175 | Eigen::Vector3d centrifugal = gyr.cross(gyr.cross(-translation_calibration)); 176 | 177 | // Final acceleration = rotated acceleration + centrifugal force 178 | imu_out.acc = acc + centrifugal; 179 | 180 | return imu_out; 181 | } 182 | 183 | /** 184 | * Convert IMU data to ROS message 185 | * @param imu_in input IMU message 186 | * @param rotation_calibration ego to IMU rotation matrix 187 | * @return converted ROS IMU message 188 | */ 189 | inline sensor_msgs::Imu ImuConverterToSensorMsg(const sensor_msgs::Imu& imu_in, 190 | const Eigen::Matrix3d& rotation_calibration) { 191 | // rotation_calibration 는 ego --> imu 192 | 193 | sensor_msgs::Imu imu_out = imu_in; 194 | 195 | // Rotate linear acceleration 196 | Eigen::Vector3d acc(imu_in.linear_acceleration.x, imu_in.linear_acceleration.y, imu_in.linear_acceleration.z); 197 | acc = rotation_calibration * acc; 198 | imu_out.linear_acceleration.x = acc.x(); 199 | imu_out.linear_acceleration.y = acc.y(); 200 | imu_out.linear_acceleration.z = acc.z(); 201 | 202 | // Rotate angular velocity 203 | Eigen::Vector3d gyr(imu_in.angular_velocity.x, imu_in.angular_velocity.y, imu_in.angular_velocity.z); 204 | gyr = rotation_calibration * gyr; 205 | imu_out.angular_velocity.x = gyr.x(); 206 | imu_out.angular_velocity.y = gyr.y(); 207 | imu_out.angular_velocity.z = gyr.z(); 208 | 209 | return imu_out; 210 | } 211 | 212 | /** 213 | * Calculate interpolated transformation between two transformation matrices 214 | * @param affine_trans_between start and end transformation 215 | * @param dt_scan current time and start time difference 216 | * @param dt_trans end time and start time difference 217 | * @return interpolated transformation matrix 218 | */ 219 | inline Eigen::Affine3f InterpolateTfWithTime(const Eigen::Affine3f& affine_trans_between, double dt_scan, double dt_trans) { 220 | // Check if dt_trans is zero to avoid division by zero 221 | if (dt_trans == 0.0) { 222 | return Eigen::Affine3f::Identity(); 223 | } 224 | 225 | // Compute the interpolation ratio 226 | double ratio = dt_scan / dt_trans; 227 | 228 | // Interpolate translation part 229 | Eigen::Vector3f translation = affine_trans_between.translation() * ratio; 230 | 231 | // Interpolate rotation part using quaternion slerp (for smooth interpolation) 232 | Eigen::Quaternionf rotation(affine_trans_between.rotation()); 233 | Eigen::Quaternionf interpolatedRotation = Eigen::Quaternionf::Identity().slerp(ratio, rotation); 234 | 235 | // Combine translation and rotation to create the final transformation 236 | Eigen::Affine3f affine_interpolated_transform = Eigen::Affine3f::Identity(); 237 | affine_interpolated_transform.translate(translation); 238 | affine_interpolated_transform.rotate(interpolatedRotation); 239 | 240 | return affine_interpolated_transform; 241 | } 242 | 243 | /** 244 | * Normalize angle to 0~360 degrees 245 | * @param d_angle_deg input angle (degrees) 246 | * @return normalized angle (degrees) 247 | */ 248 | inline double NormAngleDeg(double d_angle_deg) { 249 | double d_angle_norm_deg = d_angle_deg; 250 | 251 | // Set the input angle into the 0~pi 252 | while (d_angle_norm_deg > 360.) d_angle_norm_deg -= 360.; 253 | while (d_angle_norm_deg < 0.) d_angle_norm_deg += 360.; 254 | 255 | return d_angle_norm_deg; 256 | } 257 | 258 | /** 259 | * Normalize angle to -π~π radians 260 | * @param d_angle_rad input angle (radians) 261 | * @return normalized angle (radians) 262 | */ 263 | inline double NormAngleRad(double d_angle_rad) { 264 | double d_angle_norm_rad = d_angle_rad; 265 | 266 | // Set the input angle into the 0~pi 267 | while (d_angle_norm_rad > M_PI) d_angle_norm_rad -= M_PI * 2.; 268 | while (d_angle_norm_rad < -M_PI) d_angle_norm_rad += M_PI * 2.; 269 | 270 | return d_angle_norm_rad; 271 | } 272 | 273 | /** 274 | * Calculate angle difference (-180~180 degrees) 275 | * @param d_ref_deg reference angle (degrees) 276 | * @param d_rel_deg relative angle (degrees) 277 | * @return angle difference (degrees) 278 | */ 279 | inline double AngleDiffDeg(double d_ref_deg, double d_rel_deg) { 280 | double d_angle_diff_deg = d_rel_deg - d_ref_deg; 281 | 282 | // calculate angle difference 283 | while (d_angle_diff_deg > 180.) d_angle_diff_deg = d_angle_diff_deg - 360.; 284 | while (d_angle_diff_deg < -180.) d_angle_diff_deg = d_angle_diff_deg + 360.; 285 | 286 | return d_angle_diff_deg; 287 | } 288 | 289 | /** 290 | * Calculate angle difference (-π~π radians) 291 | * @param d_ref_rad reference angle (radians) 292 | * @param d_rel_rad relative angle (radians) 293 | * @return angle difference (radians) 294 | */ 295 | inline double AngleDiffRad(double d_ref_rad, double d_rel_rad) { 296 | double d_angle_diff_rad = d_rel_rad - d_ref_rad; 297 | 298 | // calculate angle difference 299 | while (d_angle_diff_rad > M_PI) d_angle_diff_rad = d_angle_diff_rad - 2. * M_PI; 300 | while (d_angle_diff_rad < -M_PI) d_angle_diff_rad = d_angle_diff_rad + 2. * M_PI; 301 | 302 | return d_angle_diff_rad; 303 | } 304 | 305 | 306 | // Rotation Matrix to Euler angles (to avoid gimbal lock) 307 | /** 308 | * Convert rotation matrix to Euler angles (to avoid gimbal lock) 309 | * @param R 3x3 rotation matrix 310 | * @return 3D vector containing roll, pitch, yaw 311 | */ 312 | inline Eigen::Vector3d RotToVec(const Eigen::Matrix3d& R) { 313 | Eigen::Vector3d angles; 314 | 315 | // Special case handling (to detect gimbal lock) 316 | if (std::abs(R(2, 0)) > 0.998) { // gimbal lock occurs 317 | angles(2) = std::atan2(-R(1, 2), R(1, 1)); 318 | angles(1) = M_PI / 2 * (R(2, 0) >= 0 ? 1 : -1); 319 | angles(0) = 0; 320 | } 321 | else { 322 | angles(1) = std::asin(-R(2, 0)); 323 | angles(0) = std::atan2(R(2, 1) / std::cos(angles(1)), R(2, 2) / std::cos(angles(1))); 324 | angles(2) = std::atan2(R(1, 0) / std::cos(angles(1)), R(0, 0) / std::cos(angles(1))); 325 | } 326 | 327 | // Normalize angles to be within -π and π 328 | angles(0) = std::fmod(angles(0) + M_PI, 2 * M_PI) - M_PI; 329 | angles(1) = std::fmod(angles(1) + M_PI, 2 * M_PI) - M_PI; 330 | angles(2) = std::fmod(angles(2) + M_PI, 2 * M_PI) - M_PI; 331 | 332 | return angles; 333 | } 334 | 335 | /** 336 | * Convert Euler angles to rotation matrix 337 | * @param angles 3D vector containing roll, pitch, yaw 338 | * @return 3x3 rotation matrix 339 | */ 340 | inline Eigen::Matrix3d VecToRot(const Eigen::Vector3d& angles) { 341 | Eigen::Matrix3d R = (Eigen::AngleAxisd(angles.z(), Eigen::Vector3d::UnitZ()) * 342 | Eigen::AngleAxisd(angles.y(), Eigen::Vector3d::UnitY()) * 343 | Eigen::AngleAxisd(angles.x(), Eigen::Vector3d::UnitX())).toRotationMatrix(); 344 | return R; 345 | } 346 | 347 | 348 | // Calculate angle difference between two quaternions 349 | /** 350 | * Calculate angle difference between two quaternions 351 | * @param state_quat state quaternion 352 | * @param measurement_quat measurement quaternion 353 | * @return angle difference (radians) 354 | */ 355 | inline Eigen::Vector3d CalEulerResidualFromQuat(const Eigen::Quaterniond& state_quat, 356 | const Eigen::Quaterniond& measurement_quat) { 357 | // Normalize quaternions to rotation matrices 358 | Eigen::Vector3d state_angles = RotToVec(state_quat.normalized().toRotationMatrix()); 359 | Eigen::Vector3d meas_angles = RotToVec(measurement_quat.normalized().toRotationMatrix()); 360 | 361 | // Calculate Euler angle residual 362 | Eigen::Vector3d res_euler = meas_angles - state_angles; 363 | 364 | // Normalize angles to be within -π and π 365 | res_euler.x() = NormAngleRad(res_euler.x()); 366 | res_euler.y() = NormAngleRad(res_euler.y()); 367 | res_euler.z() = NormAngleRad(res_euler.z()); 368 | 369 | return res_euler; 370 | } 371 | 372 | /* 373 | Lie Algebra 374 | */ 375 | /** 376 | * Convert vector to skew-symmetric matrix 377 | * @param V 3D vector 378 | * @return 3x3 skew-symmetric matrix 379 | */ 380 | inline Eigen::Matrix3d SkewSymmetricMatrix(const Eigen::Vector3d& V) { 381 | Eigen::Matrix3d M; 382 | 383 | M << 0.0, -V(2), V(1), V(2), 0.0, -V(0), -V(1), V(0), 0.0; 384 | return M; 385 | } 386 | 387 | // SO(3) non-linear space --> so(3) linear space 388 | /** 389 | * SO(3) non-linear space --> so(3) linear space (log mapping) 390 | * @param R SO(3) rotation matrix 391 | * @return so(3) 3D vector 392 | */ 393 | inline Eigen::Vector3d Log(const Eigen::Matrix3d& R) { 394 | double cos_theta = (R.trace() - 1) / 2.0; 395 | cos_theta = std::min(1.0, std::max(-1.0, cos_theta)); // Clamping for numerical stability 396 | double theta = std::acos(cos_theta); 397 | 398 | if (std::abs(theta) < 1e-5) { 399 | return Eigen::Vector3d::Zero(); 400 | } 401 | Eigen::Matrix3d log_matrix = (R - R.transpose()) / (2.0 * std::sin(theta)); 402 | return theta * Eigen::Vector3d(log_matrix(2, 1), log_matrix(0, 2), log_matrix(1, 0)); 403 | } 404 | 405 | /** 406 | * so(3) linear space --> SO(3) non-linear space (exp mapping) 407 | * @param omega so(3) 3D vector 408 | * @return SO(3) rotation matrix 409 | */ 410 | inline Eigen::Matrix3d Exp(const Eigen::Vector3d& omega) { 411 | double theta = omega.norm(); 412 | Eigen::Matrix3d Eye3 = Eigen::Matrix3d::Identity(); 413 | if (theta < 1e-5) { 414 | return Eye3; 415 | } 416 | Eigen::Vector3d axis = omega / theta; 417 | Eigen::Matrix3d K = SkewSymmetricMatrix(axis); 418 | return Eye3 + std::sin(theta) * K + (1 - std::cos(theta)) * K * K; 419 | } 420 | 421 | /** 422 | * Calculate rotation matrix change using angular velocity and time 423 | * @param gyro angular velocity vector 424 | * @param d_dt_sec time step 425 | * @return rotation matrix change 426 | */ 427 | inline Eigen::Matrix3d ExpGyroToRotMatrix(const Eigen::Vector3d& gyro, double d_dt_sec) { 428 | Eigen::Vector3d omega = gyro * d_dt_sec; // Angular velocity scaled by time step 429 | return Exp(omega); // Use the ExpMap function to get the rotation matrix 430 | } 431 | 432 | 433 | /** 434 | * Calculate quaternion change using angular velocity and time 435 | * @param gyro angular velocity vector 436 | * @param d_dt_sec time step 437 | * @return quaternion change 438 | */ 439 | inline Eigen::Quaterniond ExpGyroToQuat(const Eigen::Vector3d& gyro, double d_dt_sec) { 440 | Eigen::Vector3d omega = gyro * d_dt_sec; // Angular velocity vector scaled by time step 441 | Eigen::Matrix3d rotation_matrix = Exp(omega); // Use the Exp function 442 | return Eigen::Quaterniond(rotation_matrix); // Convert rotation matrix to quaternion 443 | } 444 | 445 | /* 446 | Given: 447 | δ_rot = Exp(gyro * Δt) 448 | ∂δ_rot / ∂ω = ∂Exp(ω) / ∂ω = ∂/∂ω (I + sin(θ) * K + (1 - cos(θ)) * K^2) 449 | ω = gyro * Δt 450 | θ = ||ω|| 451 | 452 | The partial derivative of δ_rot with respect to gyro: 453 | 454 | ∂δ_rot / ∂gyro ≈ Δt * (I + (1 - cos(θ)) / θ^2 * K + (θ - sin(θ)) / θ^3 * K^2) 455 | 456 | where: 457 | - I is the identity matrix 458 | - K is the skew-symmetric matrix of ω/θ 459 | */ 460 | /** 461 | * Calculate partial derivative of rotation with respect to angular velocity 462 | * @param gyro angular velocity vector 463 | * @param d_dt_sec time step 464 | * @return 3x3 Jacobian matrix 465 | */ 466 | inline Eigen::Matrix3d PartialDerivativeRotWrtGyro(const Eigen::Vector3d& gyro, double d_dt_sec) { 467 | 468 | Eigen::Vector3d omega = gyro * d_dt_sec; // angular velocity vector scaled by time step 469 | double theta = omega.norm(); // total angular velocity 470 | 471 | if (theta < 1e-5) { 472 | return Eigen::Matrix3d::Zero(); // Near-zero rotation, derivative is approximately zero 473 | } 474 | 475 | Eigen::Vector3d axis = omega / theta; // rotation axis = angular velocity vector / total angular velocity 476 | Eigen::Matrix3d K = SkewSymmetricMatrix(axis); // skew-symmetric matrix: rotation vector representation in SO(3) 477 | Eigen::Matrix3d partial_derivative = d_dt_sec * 478 | (Eigen::Matrix3d::Identity() 479 | + (1 - std::cos(theta)) / (theta * theta) * K 480 | + (theta - std::sin(theta)) / (theta * theta * theta) * K * K); 481 | 482 | return partial_derivative; 483 | } 484 | 485 | /** 486 | * Convert global velocity to local velocity 487 | * @param global_vx,vy,vz global velocity 488 | * @param roll_rad,pitch_rad,yaw_rad current pose angles 489 | * @param local_vx,vy,vz local velocity (output) 490 | */ 491 | inline void ConvertGlobalToLocalVelocity(const double global_vx, const double global_vy, const double global_vz, 492 | const double roll_rad, const double pitch_rad, const double yaw_rad, 493 | double& local_vx, double& local_vy, double& local_vz) { 494 | // Calculate rotation matrix elements 495 | const double cos_yaw = cos(yaw_rad); 496 | const double sin_yaw = sin(yaw_rad); 497 | const double cos_pitch = cos(pitch_rad); 498 | const double sin_pitch = sin(pitch_rad); 499 | const double cos_roll = cos(roll_rad); 500 | const double sin_roll = sin(roll_rad); 501 | 502 | // ZYX rotation matrix (transposed for global to local conversion) 503 | // First row determines local_vx 504 | local_vx = global_vx * (cos_yaw * cos_pitch) + global_vy * (sin_yaw * cos_pitch) + global_vz * (-sin_pitch); 505 | 506 | // Second row determines local_vy 507 | local_vy = global_vx * (cos_yaw * sin_pitch * sin_roll - sin_yaw * cos_roll) + 508 | global_vy * (sin_yaw * sin_pitch * sin_roll + cos_yaw * cos_roll) + global_vz * (cos_pitch * sin_roll); 509 | 510 | // Third row determines local_vz 511 | local_vz = global_vx * (cos_yaw * sin_pitch * cos_roll + sin_yaw * sin_roll) + 512 | global_vy * (sin_yaw * sin_pitch * cos_roll - cos_yaw * sin_roll) + global_vz * (cos_pitch * cos_roll); 513 | } 514 | 515 | /** 516 | * Convert local angular velocity to global angular velocity 517 | * @param local_roll_rate,pitch_rate,yaw_rate local angular velocity 518 | * @param roll_rad,pitch_rad,yaw_rad current pose angles 519 | * @param global_roll_rate,pitch_rate,yaw_rate global angular velocity (output) 520 | */ 521 | inline void ConvertLocalToGlobalAngularRate(const double local_roll_rate, const double local_pitch_rate, 522 | const double local_yaw_rate, const double roll_rad, 523 | const double pitch_rad, const double yaw_rad, 524 | double& global_roll_rate, double& global_pitch_rate, 525 | double& global_yaw_rate) 526 | { 527 | // 삼각함수 값 계산 528 | const double cos_roll = cos(roll_rad); 529 | const double sin_roll = sin(roll_rad); 530 | const double cos_pitch = cos(pitch_rad); 531 | const double sin_pitch = sin(pitch_rad); 532 | 533 | // Relationship between Euler angular velocity and vehicle angular velocity 534 | // [global_roll_rate] [1 0 -sin_pitch ] [local_roll_rate ] 535 | // [global_pitch_rate] = [0 cos_roll sin_roll*cos_pitch] [local_pitch_rate] 536 | // [global_yaw_rate ] [0 -sin_roll cos_roll*cos_pitch] [local_yaw_rate ] 537 | 538 | global_roll_rate = local_roll_rate + local_pitch_rate * 0 + local_yaw_rate * (-sin_pitch); 539 | 540 | global_pitch_rate = local_roll_rate * 0 + local_pitch_rate * cos_roll + local_yaw_rate * (sin_roll * cos_pitch); 541 | 542 | global_yaw_rate = local_roll_rate * 0 + local_pitch_rate * (-sin_roll) + local_yaw_rate * (cos_roll * cos_pitch); 543 | } 544 | 545 | /** 546 | * Convert global angular velocity to local angular velocity 547 | * @param global_roll_rate,pitch_rate,yaw_rate global angular velocity 548 | * @param roll_rad,pitch_rad,yaw_rad current pose angles 549 | * @param local_roll_rate,pitch_rate,yaw_rate local angular velocity (output) 550 | */ 551 | inline void ConvertGlobalToLocalAngularRate(const double global_roll_rate, const double global_pitch_rate, 552 | const double global_yaw_rate, const double roll_rad, 553 | const double pitch_rad, const double yaw_rad, 554 | double& local_roll_rate, double& local_pitch_rate, 555 | double& local_yaw_rate) 556 | { 557 | // 삼각함수 값 계산 558 | const double cos_roll = cos(roll_rad); 559 | const double sin_roll = sin(roll_rad); 560 | const double cos_pitch = cos(pitch_rad); 561 | const double sin_pitch = sin(pitch_rad); 562 | 563 | // Inverse matrix calculation for converting global angular velocity to local angular velocity 564 | // [local_roll_rate ] [1 0 -sin_pitch ]^-1 [global_roll_rate ] 565 | // [local_pitch_rate] = [0 cos_roll sin_roll*cos_pitch]^-1 [global_pitch_rate] 566 | // [local_yaw_rate ] [0 -sin_roll cos_roll*cos_pitch]^-1 [global_yaw_rate ] 567 | 568 | const double det = cos_pitch * cos_roll; 569 | if (fabs(det) < 1e-6) { 570 | std::cerr << "Singularity encountered during angular rate conversion!" << std::endl; 571 | return; 572 | } 573 | 574 | // local angular velocity calculation 575 | local_roll_rate = 576 | global_roll_rate + global_pitch_rate * (sin_roll / cos_pitch) + global_yaw_rate * (-cos_roll / cos_pitch); 577 | 578 | local_pitch_rate = global_pitch_rate * cos_roll + global_yaw_rate * sin_roll; 579 | 580 | local_yaw_rate = global_pitch_rate * (-sin_roll / cos_pitch) + global_yaw_rate * (cos_roll / cos_pitch); 581 | } 582 | 583 | #endif 584 | -------------------------------------------------------------------------------- /src/app/localization/localization_interface/localization_struct.hpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file localization_struct.hpp 3 | * @author Jaeyoung Jo 4 | * @brief Various structs for EKF based localization 5 | * @version 0.1 6 | * @date 2024-10-28 7 | * 8 | * @copyright Copyright (c) 2024 9 | */ 10 | 11 | #ifndef __EKF_COMMON_STRUCT__ 12 | #define __EKF_COMMON_STRUCT__ 13 | #pragma once 14 | 15 | // STD header 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | #include 24 | 25 | // Libraries 26 | #include 27 | 28 | enum class GnssSource { NOVATEL, NAVSATFIX, BESTPOS, PCM, PCM_INIT }; 29 | 30 | typedef struct { 31 | double timestamp; 32 | 33 | double latitude; 34 | double longitude; 35 | double height; 36 | 37 | double x_m; 38 | double y_m; 39 | double z_m; // new 1014 40 | 41 | double vx; // local coord 42 | double vy; // local coord 43 | double vz; // local coord 44 | 45 | double roll_vel; 46 | double pitch_vel; 47 | double yaw_vel; 48 | 49 | double ax; // local coord 50 | double ay; // local coord 51 | double az; // local coord 52 | 53 | double roll_rad; 54 | double pitch_rad; 55 | double yaw_rad; 56 | 57 | double latitude_std; // world 58 | double longitude_std; // world 59 | double height_std; // new 1014 60 | 61 | double x_cov_m; // ego 62 | double y_cov_m; // ego 63 | double z_cov_m; // ego new 1014 64 | 65 | double vx_cov_ms; 66 | double vy_cov_ms; // new 1014 67 | double vz_cov_ms; // new 1014 68 | 69 | double roll_cov_rad; // new 1014 70 | double pitch_cov_rad; // new 1014 71 | double yaw_cov_rad; 72 | 73 | } EgoState; 74 | 75 | typedef struct { 76 | double timestamp; 77 | 78 | GnssSource gnss_source; 79 | 80 | double latitude; 81 | double longitude; 82 | double height; 83 | 84 | double x_m; 85 | double y_m; 86 | double z_m; // new 1014 87 | 88 | double vx; 89 | double vy; 90 | double vz; 91 | 92 | double roll_rad; 93 | double pitch_rad; 94 | double yaw_rad; 95 | 96 | double latitude_std; 97 | double longitude_std; 98 | double height_std; 99 | 100 | double x_cov_m; // local_x 101 | double y_cov_m; // local_y 102 | double z_cov_m; // local_y new 1014 103 | 104 | double vx_cov_ms; 105 | double vy_cov_ms; // new 1014 106 | double vz_cov_ms; // new 1014 107 | 108 | double roll_cov_rad; // new 1014 109 | double pitch_cov_rad; // new 1014 110 | double yaw_cov_rad; 111 | } GnssStruct; 112 | 113 | typedef struct { 114 | double timestamp = 0.0; 115 | double vel_dir = 0.0; // velocity direction from East, Counterclockwise is positive 116 | double hor_vel = 0.0; // horizontal velocity 117 | double ver_vel = 0.0; // vertical velocity 118 | } BestVelStruct; 119 | 120 | typedef struct { 121 | double timestamp = 0.0; 122 | Eigen::Vector3d vel = Eigen::Vector3d::Zero(); // local velocity, only x is valid 123 | Eigen::Vector3d gyro = Eigen::Vector3d::Zero(); // local gyro, only z is valid 124 | } CanStruct; 125 | 126 | typedef struct { 127 | double timestamp = 0.0; 128 | Eigen::Vector3d acc = Eigen::Vector3d::Zero(); 129 | Eigen::Vector3d gyro = Eigen::Vector3d::Zero(); 130 | } ImuStruct; 131 | 132 | // EKF state 133 | typedef struct { 134 | double timestamp = 0.0; 135 | Eigen::Vector3d pos = Eigen::Vector3d::Zero(); // global 136 | Eigen::Quaterniond rot = Eigen::Quaterniond::Identity(); // global 137 | Eigen::Vector3d vel = Eigen::Vector3d::Zero(); // global 138 | Eigen::Vector3d gyro = Eigen::Vector3d::Zero(); // local 139 | Eigen::Vector3d acc = Eigen::Vector3d::Zero(); // global 140 | Eigen::Vector3d bg = Eigen::Vector3d::Zero(); // bias gyro 141 | Eigen::Vector3d ba = Eigen::Vector3d::Zero(); // bias acc 142 | Eigen::Vector3d grav = Eigen::Vector3d::Zero(); // global gravity 143 | Eigen::Quaterniond imu_rot = Eigen::Quaterniond::Identity(); 144 | } EkfState; 145 | 146 | typedef struct { 147 | double timestamp = 0.0; 148 | GnssSource gnss_source = GnssSource::NOVATEL; // Define UNKNOWN as a default in GnssSource enum 149 | Eigen::Vector3d pos = Eigen::Vector3d::Zero(); // global 150 | Eigen::Quaterniond rot = Eigen::Quaterniond::Identity(); // global 151 | Eigen::Matrix3d pos_cov = Eigen::Matrix3d::Identity(); // position covariance (3x3 matrix) 152 | Eigen::Matrix3d rot_cov = Eigen::Matrix3d::Identity(); // rotation covariance (3x3 matrix) 153 | } EkfGnssMeasurement; 154 | 155 | #endif -------------------------------------------------------------------------------- /src/app/localization/pcm_matching/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(pcm_matching) 3 | set(CMAKE_EXPORT_COMPILE_COMMANDS ON) 4 | 5 | 6 | ## Compile as C++14, supported in ROS Kinetic and newer 7 | add_compile_options(-std=c++14) 8 | 9 | ## Find catkin macros and libraries 10 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 11 | ## is used, also find other catkin packages 12 | find_package(catkin REQUIRED COMPONENTS 13 | roscpp 14 | rosbag 15 | tf 16 | pcl_conversions 17 | pcl_ros 18 | cv_bridge 19 | std_msgs 20 | sensor_msgs 21 | geometry_msgs 22 | # autohyu_msgs 23 | ) 24 | 25 | ## System dependencies are found with CMake's conventions 26 | find_package(Boost REQUIRED COMPONENTS system) 27 | find_package(TBB REQUIRED) 28 | find_package(PCL REQUIRED) 29 | find_package(OpenCV REQUIRED) 30 | 31 | link_directories(${PCL_LIBRARY_DIRS}) 32 | # add_definitions(${PCL_DEFINITIONS}) 33 | ################################### 34 | ## catkin specific configuration ## 35 | ################################### 36 | ## The catkin_package macro generates cmake config files for your package 37 | ## Declare things to be passed to dependent projects 38 | ## INCLUDE_DIRS: uncomment this if your package contains header files 39 | ## LIBRARIES: libraries you create in this project that dependent projects also need 40 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 41 | ## DEPENDS: system dependencies of this project that dependent projects also need 42 | catkin_package( 43 | INCLUDE_DIRS include 44 | CATKIN_DEPENDS 45 | std_msgs 46 | nav_msgs 47 | geometry_msgs 48 | sensor_msgs 49 | ) 50 | 51 | ########### 52 | ## Build ## 53 | ########### 54 | 55 | ## Specify additional locations of header files 56 | ## Your package locations should be listed before other locations 57 | include_directories( 58 | include 59 | ${catkin_INCLUDE_DIRS} 60 | ${OpenCV_INCLUDE_DIRS} 61 | ${PCL_INCLUDE_DIRS} 62 | 63 | # BSW 64 | ${CMAKE_SOURCE_DIR}/bsw/system/ini_parser 65 | 66 | ../localization_interface 67 | ) 68 | 69 | ## Declare a C++ library 70 | # add_library(${PROJECT_NAME} 71 | # src/${PROJECT_NAME}/trajectory.cpp 72 | # ) 73 | 74 | add_executable(${PROJECT_NAME} 75 | # BSW 76 | ${CMAKE_SOURCE_DIR}/bsw/system/ini_parser/ini_parser.cpp 77 | 78 | src/voxel_hash_map.cpp 79 | src/registration.cpp 80 | src/pcm_matching.cpp 81 | ) 82 | add_dependencies(${PROJECT_NAME} 83 | ${${PROJECT_NAME}_EXPORTED_TARGETS} 84 | ${catkin_EXPORTED_TARGETS} 85 | ) 86 | target_link_libraries(${PROJECT_NAME} 87 | ${catkin_LIBRARIES} 88 | ${OpenCV_LIBRARIES} 89 | ${PCL_LIBRARIES} 90 | TBB::tbb 91 | ) -------------------------------------------------------------------------------- /src/app/localization/pcm_matching/README.md: -------------------------------------------------------------------------------- 1 | # PCM Matching README 2 | ### Package for Point Cloud Map Matching 3 | ### Made by Jaeyoung Jo 4 | 5 | ## Purpose and Features 6 | ### Perform Point Cloud Map Matching 7 | - Point Cloud Deskewing 8 | - Map structuring and nearest point search based on Voxel Hashing 9 | - Supports various ICP methods (P2P, GICP, VGICP, AVGICP) 10 | - Supports initial position setting via Rviz click (/init_pose) 11 | 12 | ## I/O 13 | - Input 14 | - Estimated position nav_msgs/Odometry 15 | - Point cloud sensor_msgs/PointCloud2 16 | - Output 17 | - Estimated position nav_msgs/Odometry (Position, Covariance) 18 | - PCM visualization sensor_msgs/PointCloud2 19 | 20 | ## Files 21 | - `pcm_matching.cpp`: ROS wrapper. Main function and process execution 22 | - `voxel_hash_map.cpp`: Voxel Hashing map structuring and nearest point search. Structure definition 23 | - `registration.cpp`: Perform ICP with various methods and calculate covariance 24 | 25 | # Matching Process 26 | 27 | ### 1. Initial Validation 28 | - Concurrency control through mutex lock 29 | - Validate input point cloud 30 | 31 | ### 2. Preprocessing Stage 32 | 1. LiDAR message time correction 33 | - Apply configured time delay 34 | - Convert point cloud based on Ouster/General LiDAR type 35 | 2. Distance-based filtering 36 | - Remove points farther than the configured maximum distance 37 | 38 | ### 3. Deskewing (Point Cloud Distortion Correction) 39 | - Motion correction using IMU data 40 | - Correct point positions during scan start/end time. Move to scan end time 41 | 42 | ### 4. Pose Synchronization 43 | - Calculate synchronized pose at scan end time 44 | - Interpolation using IMU and Odometry data 45 | - If no matching pose, use integration 46 | 47 | ### 5. Point Cloud Downsampling 48 | - Convert original points to PointStruct format 49 | - Perform voxel-based downsampling 50 | 51 | ### 6. Perform Registration 52 | - Input: 53 | - Source points (local) 54 | - Voxel map 55 | - Initial pose 56 | - Previous pose (for radar) 57 | - dt (for radar) 58 | - Output: 59 | - Registration result pose 60 | - Success status 61 | - Fitness score 62 | 63 | ### 7. Result Validation and Publishing 64 | 1. Check registration success 65 | 2. Convert ICP pose data to ego coordinate system 66 | 3. Convert result point cloud to world coordinate system 67 | 4. Publish the following topics: 68 | - PCM odometry 69 | - ICP result point cloud (downsampled) 70 | - ICP result full point cloud 71 | 72 | # ICP Methodology 73 | ### P2P (Point to Point) 74 | - Search and associate nearest points in voxel map, then perform ICP 75 | 76 | ### **GICP (Generalized ICP)** 77 | - In the Init function, search for points within gicp_cov_search_dist around each map point 78 | - Calculate covariance of searched points and assign to each point 79 | - During ICP, search and associate nearest points in voxel map, then perform GICP (Covariance not performed on source) 80 | 81 | ### VGICP (Voxel GICP) 82 | - In the Init function, pre-calculate covariance representing each voxel in the voxel map 83 | - During ICP, find nearest voxel covariance for each point and perform GICP 84 | 85 | ### AVGICP (All Voxel GICP) 86 | - In the Init function, pre-calculate covariance representing each voxel in the voxel map 87 | - During ICP, associate each point with all 27 surrounding voxel covariances and perform GICP 88 | 89 | # ICP Covariance Calculation 90 | 91 | 1. Use the inverse of J^t J of Jacobian J during ICP as state covariance 92 | 2. Calculate Translation and Rotation separately 93 | - Translation: Uncertainty in x, y, z directions 94 | - Rotation: Uncertainty in roll, pitch, yaw angles 95 | 3. Normalize remaining elements of the matrix based on the minimum value of the diagonal matrix of covariance 96 | 4. Final Covariance Calculation 97 | - Translation: Multiply normalized matrix by fitness score 98 | - Rotation: Multiply normalized matrix by pre-configured angle_std -------------------------------------------------------------------------------- /src/app/localization/pcm_matching/include/pcm_matching.hpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file pcm_matching.hpp 3 | * @author Jaeyoung Jo 4 | * @brief Pointcloud map matching algorithm with deskewing 5 | * @version 0.1 6 | * @date 2024-10-28 7 | * 8 | * @copyright Copyright (c) 2024 9 | */ 10 | 11 | #ifndef __PCM_MATCHING_HPP__ 12 | #define __PCM_MATCHING_HPP__ 13 | #pragma once 14 | 15 | // STD header 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | #include 24 | 25 | // System Header 26 | #include "ini_parser.h" 27 | 28 | // ROS header 29 | #include 30 | #include 31 | #include 32 | #include 33 | #include 34 | #include 35 | #include 36 | #include 37 | #include 38 | #include 39 | #include 40 | #include 41 | #include 42 | 43 | // Utility header 44 | #include 45 | #include 46 | // PCL 47 | #include 48 | #include 49 | #include 50 | #include 51 | #include 52 | #include 53 | 54 | #include 55 | #include 56 | #include 57 | #include 58 | #include 59 | #include 60 | #include 61 | #include 62 | #include 63 | #include 64 | #include 65 | #include 66 | #include 67 | #include 68 | #include 69 | 70 | #include 71 | 72 | #include "localization_functions.hpp" 73 | #include "localization_struct.hpp" 74 | #include "pcm_matching_config.hpp" 75 | 76 | // Algorithm Header 77 | #include "registration.hpp" 78 | #include "voxel_hash_map.hpp" 79 | 80 | /* Types */ 81 | struct PointXYZIT { 82 | PCL_ADD_POINT4D; 83 | float intensity; 84 | float time; // point time after scan start 85 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW // make sure our new allocators are aligned 86 | } EIGEN_ALIGN16; // enforce SSE padding for correct memory alignment 87 | 88 | POINT_CLOUD_REGISTER_POINT_STRUCT(PointXYZIT, (float, x, x)(float, y, y)(float, z, z)(float, intensity, 89 | intensity)(float, time, time)) 90 | 91 | struct OusterPointXYZIRT { 92 | PCL_ADD_POINT4D; 93 | float intensity; 94 | uint32_t t; 95 | uint16_t reflectivity; 96 | uint16_t ring; 97 | uint16_t ambient; 98 | uint32_t range; 99 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 100 | } EIGEN_ALIGN16; 101 | 102 | POINT_CLOUD_REGISTER_POINT_STRUCT(OusterPointXYZIRT, 103 | (float, x, x)(float, y, y)(float, z, z)(float, intensity, intensity)(uint32_t, t, t)( 104 | uint16_t, reflectivity, 105 | reflectivity)(uint16_t, ring, ring)(uint16_t, ambient, ambient)(uint32_t, 106 | range, range)) 107 | 108 | using namespace ros; 109 | using namespace tf; 110 | using namespace std; 111 | 112 | typedef pcl::PointXYZINormal PointType; 113 | const int i_queue_length_ = 2000; 114 | 115 | class PcmMatching { 116 | public: 117 | explicit PcmMatching(std::string node_name, double period); 118 | virtual ~PcmMatching(); 119 | 120 | void Init(); 121 | void Run(); 122 | void Publish(); 123 | void ProcessINI(); 124 | 125 | void CallbackPointCloud(const sensor_msgs::PointCloud2::ConstPtr &msg); 126 | void CallbackImu(const sensor_msgs::Imu::ConstPtr &msg); 127 | void CallbackEkfState(const nav_msgs::Odometry::ConstPtr &msg); 128 | void CallbackInitialPose(const geometry_msgs::PoseWithCovarianceStamped::ConstPtr &msg); 129 | 130 | void Exec(int num_thread=4); 131 | void MainLoop(); 132 | 133 | private: // Deskewing 134 | bool DeskewPointCloud(const pcl::PointCloud::Ptr &distort_pcptr, double timestamp); 135 | void ImuDeskewInfo(); 136 | void OdomDeskewInfo(); 137 | void FindRotation(double d_point_time, float *f_rot_x_cur, float *f_rot_y_cur, float *f_rot_z_cur); 138 | void FindPosition(double d_rel_time, float *f_pos_x_cur, float *f_pos_y_cur, float *f_pos_z_cur); 139 | PointType DeskewPoint(PointType *pcl_point, double d_rel_time); 140 | 141 | private: // Visualization 142 | void SortEigenvaluesAndEigenvectors(Eigen::Vector3d &vec_eigenvalues, Eigen::Matrix3d &mat_eigenvectors); 143 | void VisualizeCovMap(ros::Publisher &marker_pub, const std::vector &vec_cov_map, ros::Time thisStamp, 144 | std::string thisFrame); 145 | void PublishPcmOdom(Eigen::Matrix4d icp_ego_pose, ros::Time thisStamp, std::string thisFrame); 146 | void BroadcastStaticTf(ros::Time thisStamp, std::string lidarFrame, std::string imuFrame); 147 | 148 | private: // utils 149 | pcl::PointCloud OusterCloudmsg2cloud(sensor_msgs::PointCloud2 cloudmsg); 150 | pcl::PointCloud Cloudmsg2cloud(sensor_msgs::PointCloud2 cloudmsg); 151 | 152 | bool GetInterpolatedPose(double d_time_scan_cur_, Eigen::Affine3f &o_transform); 153 | void FilterPointsByDistance(pcl::PointCloud::Ptr &input_pcptr); 154 | 155 | template 156 | sensor_msgs::PointCloud2 PublishCloud(const ros::Publisher &thisPub, const T &thisCloud, ros::Time thisStamp, 157 | std::string thisFrame) { 158 | sensor_msgs::PointCloud2 tempCloud; 159 | pcl::toROSMsg(*thisCloud, tempCloud); 160 | tempCloud.header.stamp = thisStamp; 161 | tempCloud.header.frame_id = thisFrame; 162 | thisPub.publish(tempCloud); 163 | return tempCloud; 164 | } 165 | 166 | void PointStruct2Pcl(const std::vector &vec_points, pcl::PointCloud::Ptr &pcl_points, 167 | bool b_cal_norm = false) { 168 | pcl_points->clear(); 169 | pcl_points->points.resize(vec_points.size()); 170 | 171 | // 각 PointStruct를 pcl_points에 직접 설정 172 | for (size_t i = 0; i < vec_points.size(); ++i) { 173 | const auto &point_struct = vec_points[i]; 174 | PointType &pcl_point = pcl_points->points[i]; 175 | 176 | pcl_point.x = static_cast(point_struct.pose.x()); 177 | pcl_point.y = static_cast(point_struct.pose.y()); 178 | pcl_point.z = static_cast(point_struct.pose.z()); 179 | pcl_point.intensity = static_cast(point_struct.intensity); 180 | 181 | if (b_cal_norm == true) { 182 | const CovStruct &cov_data = vec_points[i].covariance; 183 | 184 | Eigen::SelfAdjointEigenSolver eig_solver(cov_data.cov); 185 | Eigen::Vector3d eigenvalues = eig_solver.eigenvalues(); 186 | Eigen::Matrix3d eigenvectors = eig_solver.eigenvectors(); 187 | 188 | // sort eigenvalues and eigenvectors by size 189 | SortEigenvaluesAndEigenvectors(eigenvalues, eigenvectors); 190 | 191 | // set color based on eigenvector direction 192 | Eigen::Vector3d main_axis = eigenvectors.col(2); // main axis (eigenvector corresponding to largest eigenvalue) 193 | pcl_point.normal_x = fabs(main_axis(0)); // reflect X-axis component in red 194 | pcl_point.normal_y = fabs(main_axis(1)); // reflect Y-axis component in green 195 | pcl_point.normal_z = fabs(main_axis(2)); // reflect Z-axis component in blue 196 | } 197 | } 198 | 199 | // update size and properties 200 | pcl_points->width = pcl_points->points.size(); 201 | pcl_points->height = 1; 202 | pcl_points->is_dense = true; 203 | } 204 | 205 | void Pcl2PointStruct(const pcl::PointCloud::Ptr &pcl_points, std::vector &vec_points) { 206 | // allocate memory for vector to minimize memory allocation 207 | vec_points.clear(); 208 | vec_points.reserve(pcl_points->points.size()); 209 | 210 | // convert point cloud data to PointStruct and insert into vector 211 | for (const auto &pcl_point : pcl_points->points) { 212 | PointStruct point_struct; 213 | point_struct.pose = Eigen::Vector3d(pcl_point.x, pcl_point.y, pcl_point.z); 214 | point_struct.local = point_struct.pose; // set same value 215 | point_struct.intensity = pcl_point.intensity; 216 | 217 | vec_points.emplace_back(std::move(point_struct)); 218 | } 219 | 220 | } 221 | 222 | Eigen::Vector3d NormalizeDiagonalCovariance(const Eigen::Matrix3d &rotation_covariance) { 223 | // Extract the diagonal elements as a vector 224 | Eigen::Vector3d diag = rotation_covariance.diagonal(); 225 | 226 | // Find the smallest diagonal value 227 | double min_diag = diag.minCoeff(); 228 | 229 | // Avoid division by zero or excessively small values 230 | const double min_threshold = 1e-9; 231 | if (min_diag < min_threshold) { 232 | diag *= 1e9; // Scale up all elements 233 | min_diag = diag.minCoeff(); 234 | if (min_diag < min_threshold) { 235 | min_diag = min_threshold; // Ensure the minimum threshold is respected 236 | } 237 | } 238 | 239 | // Normalize each diagonal value by the smallest value 240 | Eigen::Vector3d normalized = diag / min_diag; 241 | 242 | // Clamp values to a reasonable range 243 | const double max_cap = 5.0; 244 | const double min_cap = 1.0; 245 | normalized = normalized.cwiseMin(max_cap).cwiseMax(min_cap); 246 | 247 | return normalized; 248 | } 249 | 250 | Eigen::Matrix3d NormalizeCovariance(const Eigen::Matrix3d &i_covariance) { 251 | // Copy input covariance to a local matrix 252 | Eigen::Matrix3d i_cov = i_covariance; 253 | 254 | // Find the smallest diagonal value 255 | double min_diag = std::min({i_cov(0, 0), i_cov(1, 1), i_cov(2, 2)}); 256 | 257 | // Ensure the minimum value is not too small to avoid division by zero 258 | const double min_threshold = 1e-9; 259 | if (min_diag <= min_threshold) { 260 | i_cov *= 1e9; 261 | 262 | min_diag = std::min({i_cov(0, 0), i_cov(1, 1), i_cov(2, 2)}); 263 | if (min_diag < min_threshold) min_diag = min_threshold; 264 | } 265 | 266 | // Normalize all elements of the covariance matrix 267 | Eigen::Matrix3d norm_cov = i_cov / min_diag; 268 | // Cap the maximum value for each element in the matrix 269 | const double max_cap = 5.0; // Set your desired maximum cap 270 | norm_cov = norm_cov.unaryExpr([max_cap](double elem) { return std::min(elem, max_cap); }); 271 | 272 | return norm_cov; 273 | } 274 | 275 | void UpdateCovarianceField(boost::array &covariance, const Eigen::Matrix3d &translation_cov_norm_mat, 276 | const Eigen::Matrix3d &rotation_cov_norm_mat) { 277 | // Assign translation covariance to the top-left 3x3 block 278 | for (int row = 0; row < 3; ++row) { 279 | for (int col = 0; col < 3; ++col) { 280 | covariance[row * 6 + col] = translation_cov_norm_mat(row, col); 281 | } 282 | } 283 | 284 | // Assign rotation covariance to the bottom-right 3x3 block 285 | for (int row = 0; row < 3; ++row) { 286 | for (int col = 0; col < 3; ++col) { 287 | covariance[(row + 3) * 6 + (col + 3)] = rotation_cov_norm_mat(row, col); 288 | } 289 | } 290 | } 291 | 292 | // config 293 | private: 294 | IniParser util_ini_parser_; 295 | IniParser util_calib_ini_parser_; 296 | PcmMatchingConfig cfg_; 297 | RegistrationConfig registration_config_; 298 | 299 | // ROS I/O 300 | private: 301 | ros::Subscriber sub_point_cloud_; 302 | ros::Subscriber sub_ekf_geo_; 303 | ros::Subscriber sub_imu_; 304 | ros::Subscriber sub_initial_pose_; 305 | 306 | ros::Publisher pub_pcm_gnss_; 307 | ros::Publisher pub_pcm_odom_; 308 | ros::Publisher pub_pcm_init_odom_; 309 | ros::Publisher pub_pcm_init_pc_; 310 | ros::Publisher pub_undistort_pc_; 311 | ros::Publisher pub_voxel_map_pc_; 312 | ros::Publisher pub_voxel_map_cov_; 313 | ros::Publisher pub_icp_map_pc_; 314 | ros::Publisher pub_icp_full_map_pc_; 315 | 316 | nav_msgs::Odometry i_ekf_state_; 317 | sensor_msgs::PointCloud2 i_lidar_point_cloud_msg_; 318 | 319 | pcl::PointCloud::Ptr o_undistort_pcptr_; 320 | 321 | std::deque deq_ego_state_struct_; 322 | pcl::PointCloud::Ptr i_lidar_pcptr_; 323 | std::tuple::Ptr, std::string, ros::Time> i_lidar_tuple_; 324 | 325 | pcl::PointCloud::Ptr o_voxel_map_pcptr_; 326 | sensor_msgs::PointCloud2 o_voxel_map_msg_; 327 | 328 | pcl::PointCloud::Ptr o_icp_map_pcptr_; 329 | pcl::PointCloud::Ptr o_icp_full_map_pcptr_; 330 | sensor_msgs::PointCloud2 o_icp_map_pc_msg_; 331 | 332 | // Data avialable flag 333 | bool b_is_imu_available_ = false; 334 | bool b_is_odom_available_ = false; 335 | 336 | Eigen::Matrix6d icp_local_cov_; 337 | 338 | // mutex 339 | private: 340 | std::mutex mutex_imu_; 341 | std::mutex mutex_odom_; 342 | std::mutex mutex_pcl_; 343 | 344 | bool b_initializing_status_ = false; 345 | bool b_get_first_odom_ = false; 346 | // tbb control 347 | private: 348 | std::unique_ptr tbb_control_; 349 | 350 | private: // deskew 351 | std::deque deq_imu_; 352 | std::deque deq_odom_; 353 | 354 | bool b_first_point_ = true; 355 | int i_imu_pointer_cur_; 356 | float f_odom_incre_x_; 357 | float f_odom_incre_y_; 358 | float f_odom_incre_z_; 359 | Eigen::Affine3f mat_trans_start_inverse_; 360 | double d_time_scan_cur_; 361 | double d_time_scan_end_; 362 | 363 | double *vec_d_imu_time_ = new double[i_queue_length_]; 364 | double *vec_d_imu_rot_x_ = new double[i_queue_length_]; 365 | double *vec_d_imu_rot_y_ = new double[i_queue_length_]; 366 | double *vec_d_imu_rot_z_ = new double[i_queue_length_]; 367 | 368 | pcl::PointCloud::Ptr undistort_pcptr_; 369 | 370 | private: // map matching 371 | VoxelHashMap local_map_; 372 | Registration registration_; 373 | 374 | std::string task_name_; 375 | double task_period_; 376 | double task_rate_; 377 | ros::Time update_time_; 378 | ros::Duration execution_time_; 379 | }; 380 | 381 | #endif // __PCM_MATCHING_HPP__ -------------------------------------------------------------------------------- /src/app/localization/pcm_matching/include/pcm_matching_config.hpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file pcm_matching_config.hpp 3 | * @brief configuration hpp file for Point Cloud Map matching node 4 | * 5 | * @authors Jaeyoung Jo (wodud3743@gmail.com) 6 | * 7 | * @date 2024-11-08 created by Jaeyoung Jo 8 | * 9 | */ 10 | 11 | #ifndef __PCM_MATCHING_CONFIG_HPP__ 12 | #define __PCM_MATCHING_CONFIG_HPP__ 13 | #pragma once 14 | 15 | // STD Header 16 | #include 17 | #include 18 | #include 19 | 20 | typedef struct { 21 | 22 | std::string s_lidar_type; 23 | bool b_lidar_scan_time_end; 24 | std::string s_lidar_topic_name; 25 | std::string s_imu_topic_name; 26 | std::vector vec_d_ego_to_lidar_trans; 27 | std::vector vec_d_ego_to_lidar_rot; 28 | std::vector vec_d_ego_to_imu_rot; 29 | double d_lidar_time_delay; 30 | 31 | bool b_debug_print; 32 | 33 | std::string pcm_file_path; 34 | double d_pcm_voxel_size; 35 | int i_pcm_voxel_max_point; 36 | bool b_run_deskew; 37 | double d_input_max_dist; 38 | int i_input_index_sampling; 39 | double d_input_voxel_ds_m; 40 | double d_icp_pose_std_m; 41 | double d_icp_orientation_std_deg; 42 | 43 | Eigen::Matrix4d tf_ego_to_lidar; 44 | 45 | } PcmMatchingConfig; 46 | 47 | #endif // __PCM_MATCHING_CONFIG_HPP__ -------------------------------------------------------------------------------- /src/app/localization/pcm_matching/include/registration.hpp: -------------------------------------------------------------------------------- 1 | /****************************************************************************/ 2 | // Module: registration.hpp 3 | // Description: registration algorithm 4 | // 5 | // Authors: Jaeyoung Jo (wodud3743@gmail.com) 6 | // Version: 0.1 7 | // 8 | // Revision History 9 | // Oct 18, 2024: Jaeyoung Jo - Created. 10 | // XXXX XX, 2023: XXXXXXX XX - 11 | /****************************************************************************/ 12 | 13 | #ifndef __REGISTRATION_HPP__ 14 | #define __REGISTRATION_HPP__ 15 | #pragma once 16 | 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | 24 | // PCL 25 | #include 26 | #include 27 | #include 28 | #include 29 | #include 30 | #include 31 | #include 32 | #include 33 | #include 34 | #include 35 | #include 36 | #include 37 | 38 | #include "localization_functions.hpp" 39 | #include "localization_struct.hpp" 40 | 41 | #include "voxel_hash_map.hpp" 42 | 43 | namespace Eigen { 44 | using Matrix6d = Eigen::Matrix; 45 | using Matrix3_6d = Eigen::Matrix; 46 | using Vector6d = Eigen::Matrix; 47 | 48 | using Matrix3_3d = Eigen::Matrix; 49 | using Matrix2_3d = Eigen::Matrix; 50 | using Matrix1_3d = Eigen::Matrix; 51 | using Matrix1_4d = Eigen::Matrix; 52 | using Matrix1_6d = Eigen::Matrix; 53 | using Matrix4_6d = Eigen::Matrix; 54 | 55 | // using Matrix3d = Eigen::Matrix; 56 | } // namespace Eigen 57 | 58 | using Correspondences = std::vector>; 59 | 60 | typedef enum { P2P, GICP, VGICP, AVGICP } IcpMethod; 61 | 62 | struct RegistrationConfig { 63 | // Parameters 64 | int i_max_thread; 65 | IcpMethod icp_method; 66 | int voxel_search_method; 67 | double gicp_cov_search_dist; 68 | bool use_radar_cov; 69 | int max_iteration; 70 | double max_search_dist; 71 | double lm_lambda; 72 | double icp_termination_threshold_m; 73 | double min_overlap_ratio; 74 | double max_fitness_score; 75 | 76 | double doppler_trans_lambda; 77 | double range_variance_m; 78 | double azimuth_variance_deg; 79 | double elevation_variance_deg; 80 | Eigen::Vector3d ego_to_lidar_trans; 81 | Eigen::Matrix3d ego_to_lidar_rot; 82 | Eigen::Matrix3d ego_to_imu_rot; 83 | 84 | bool b_debug_print; 85 | }; 86 | 87 | struct Velocity { 88 | Eigen::Vector3d linear; // linear velocity 89 | Eigen::Vector3d angular; // angular velocity 90 | 91 | double time_diff_sec; 92 | 93 | // default constructor - initialize members to zero 94 | Velocity() : linear(Eigen::Vector3d::Zero()), angular(Eigen::Vector3d::Zero()), time_diff_sec(0.1) {} 95 | 96 | // constructor with parameters 97 | Velocity(const Eigen::Vector3d& lin, const Eigen::Vector3d& ang, const double& time_diff) 98 | : linear(lin), angular(ang), time_diff_sec(time_diff) {} 99 | }; 100 | 101 | struct Registration { 102 | Registration() {} 103 | Registration(RegistrationConfig config) { config_ = config; } 104 | void Init(RegistrationConfig config) { config_ = config; } 105 | 106 | Eigen::Matrix4d AlignCloudsLocal(std::vector& source_global, 107 | const std::vector& target_global, Eigen::Matrix4d& last_icp_pose, 108 | double trans_th, RegistrationConfig m_config); 109 | Eigen::Matrix4d AlignCloudsLocalPointCov(std::vector& source_global, 110 | const std::vector& target_global, Eigen::Matrix6d& local_cov, 111 | Eigen::Matrix4d& last_icp_pose, double trans_th, 112 | RegistrationConfig m_config); 113 | Eigen::Matrix4d AlignCloudsLocalVoxelCov(std::vector& source_global, 114 | const std::vector& target_cov_global, 115 | Eigen::Matrix4d& last_icp_pose, double trans_th, 116 | RegistrationConfig m_config); 117 | Eigen::Matrix4d AlignCloudsGlobal(std::vector& source_global, 118 | const std::vector& target_global, double trans_th, 119 | RegistrationConfig m_config); 120 | 121 | // Local coordinate frame, voxel map, initial position initial_guess 122 | Eigen::Matrix4d RunRegister(const std::vector& source_local, const VoxelHashMap& voxel_map, 123 | const Eigen::Matrix4d& initial_guess, RegistrationConfig m_config, bool& is_success, 124 | double& fitness_score, Eigen::Matrix6d& local_cov); 125 | 126 | inline void TransformPoints(const Eigen::Matrix4d& T, std::vector& points) { 127 | std::transform(points.cbegin(), points.cend(), points.begin(), [&](const auto& point) { 128 | Eigen::Vector4d p(point.pose.x(), point.pose.y(), point.pose.z(), 1.0); 129 | Eigen::Vector4d p_transformed = T * p; 130 | PointStruct transformed_point = point; // copy all properties 131 | transformed_point.pose = p_transformed.head<3>(); 132 | return transformed_point; 133 | }); 134 | } 135 | 136 | inline void TransformPoints(const Eigen::Matrix4d& T, const std::vector& points, 137 | std::vector& o_points) { 138 | // assert(points.size() == o_points.size()); // check if points and o_points have the same size 139 | o_points.resize(points.size()); 140 | 141 | std::transform(points.cbegin(), points.cend(), o_points.begin(), [&](const auto& point) { 142 | Eigen::Vector4d p(point.pose.x(), point.pose.y(), point.pose.z(), 1.0); 143 | Eigen::Vector4d p_transformed = T * p; 144 | PointStruct transformed_point = point; // copy all properties 145 | transformed_point.pose = p_transformed.head<3>(); 146 | return transformed_point; 147 | }); 148 | } 149 | 150 | inline void SeperatePointsZ(const std::vector& i_points, std::vector& up_points, 151 | std::vector& down_points, double z) { 152 | // clear up_points and down_points 153 | up_points.clear(); 154 | down_points.clear(); 155 | 156 | // iterate through each point in i_points and separate them based on z coordinate 157 | for (const auto& point : i_points) { 158 | if (point.pose.z() > z) { 159 | up_points.push_back(point); // add to up_points if higher than z 160 | } 161 | else { 162 | down_points.push_back(point); // add to down_points if lower than or equal to z 163 | } 164 | } 165 | } 166 | 167 | inline Velocity CalculateVelocity(const Eigen::Matrix4d& transform, double delta_t_sec) { 168 | // rotation matrix R 169 | Eigen::Matrix3d rotation = transform.block<3, 3>(0, 0); 170 | // translation vector t 171 | Eigen::Vector3d translation = transform.block<3, 1>(0, 3); 172 | 173 | // linear velocity v (divide translation change by time) 174 | Eigen::Vector3d linear_vel = translation / delta_t_sec; 175 | 176 | // angular velocity matrix omega calculation (use logarithm map) 177 | Eigen::AngleAxisd angle_axis(rotation); 178 | Eigen::Vector3d angular_vel = angle_axis.angle() * angle_axis.axis() / delta_t_sec; 179 | 180 | // create Velocity structure and return 181 | Velocity velocity(linear_vel, angular_vel, delta_t_sec); 182 | 183 | return velocity; 184 | } 185 | 186 | inline PointStruct CalPointCov(const PointStruct point, double range_var_m, double azim_var_deg, 187 | double ele_var_deg) { 188 | PointStruct cov_point = point; 189 | double dist = sqrt(point.pose.x() * point.pose.x() + point.pose.y() * point.pose.y()); 190 | double s_x = range_var_m; 191 | double s_y = std::max(0.1, dist * sin(azim_var_deg / 180 * M_PI)); // 0.00873 192 | double s_z = std::max(0.1, dist * sin(ele_var_deg / 180 * M_PI)); // 0.01745 193 | 194 | double ele_angle = atan2(point.pose.z(), dist); 195 | double azi_angle = atan2(point.pose.y(), point.pose.x()); 196 | Eigen::AngleAxisd pitchAngle(Eigen::AngleAxisd(ele_angle, Eigen::Vector3d::UnitY())); 197 | Eigen::AngleAxisd yawAngle(Eigen::AngleAxisd(azi_angle, Eigen::Vector3d::UnitZ())); 198 | Eigen::Matrix3d R; // Rotation matrix 199 | R = yawAngle * pitchAngle; 200 | 201 | Eigen::Matrix3d S; // Scaling matix 202 | S << s_x, 0.0, 0.0, 0.0, s_y, 0.0, 0.0, 0.0, s_z; 203 | 204 | Eigen::Matrix3d cov = R * S; 205 | cov_point.covariance.cov = cov; 206 | 207 | return cov_point; 208 | } 209 | 210 | inline void CalFramePointCov(std::vector& points, double range_var_m, double azim_var_deg, 211 | double ele_var_deg) { 212 | tbb::parallel_for(tbb::blocked_range(0, points.size()), [&](const tbb::blocked_range& range) { 213 | for (size_t i = range.begin(); i != range.end(); ++i) { 214 | points[i] = CalPointCov(points[i], range_var_m, azim_var_deg, ele_var_deg); 215 | } 216 | }); 217 | } 218 | 219 | inline double square(double x) { return x * x; } 220 | 221 | inline Eigen::Matrix3d vectorToSkewSymmetricMatrix(const Eigen::Vector3d& vec) { 222 | Eigen::Matrix3d skew_symmetric; 223 | skew_symmetric << 0, -vec.z(), vec.y(), vec.z(), 0, -vec.x(), -vec.y(), vec.x(), 0; 224 | return skew_symmetric; 225 | } 226 | 227 | RegistrationConfig config_; 228 | 229 | double d_fitness_score_ = 0.0; 230 | }; 231 | 232 | #endif // __REGISTRATION_HPP__ -------------------------------------------------------------------------------- /src/app/localization/pcm_matching/include/voxel_hash_map.hpp: -------------------------------------------------------------------------------- 1 | /****************************************************************************/ 2 | // Module: voxel_hash_map.hpp 3 | // Description: voxel_hash_map algorithm 4 | // 5 | // Authors: Jaeyoung Jo (wodud3743@gmail.com) 6 | // Version: 0.1 7 | // 8 | // Revision History 9 | // June 19, 2024: Jaeyoung Jo - Created. 10 | // XXXX XX, 2023: XXXXXXX XX - 11 | /****************************************************************************/ 12 | 13 | #ifndef __VOXEL_HASH_MAP_HPP__ 14 | #define __VOXEL_HASH_MAP_HPP__ 15 | #pragma once 16 | 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | 24 | // PCL 25 | #include 26 | #include 27 | #include 28 | #include 29 | #include 30 | #include 31 | #include 32 | #include 33 | #include 34 | #include 35 | #include 36 | #include 37 | 38 | #include "localization_functions.hpp" 39 | #include "localization_struct.hpp" 40 | 41 | struct CovStruct { 42 | Eigen::Matrix3d cov; // 3x3 covariance matrix 43 | Eigen::Vector3d mean; // 3D mean vector 44 | 45 | CovStruct() : cov(Eigen::Matrix3d::Identity()), mean(Eigen::Vector3d::Zero()) {} 46 | 47 | CovStruct(const Eigen::Matrix3d &c, const Eigen::Vector3d &m) : cov(c), mean(m) {} 48 | 49 | void reset() { 50 | cov = Eigen::Matrix3d::Identity(); 51 | mean = Eigen::Vector3d::Zero(); 52 | } 53 | }; 54 | 55 | struct PointStruct { 56 | Eigen::Vector3d pose; 57 | Eigen::Vector3d local; 58 | CovStruct covariance; 59 | float vel; // mps 60 | float azi_angle; // deg Right to Left 61 | float ele_angle; // deg. Down to Up 62 | 63 | double intensity; 64 | 65 | // constructor 66 | PointStruct() 67 | : pose(Eigen::Vector3d::Zero()), 68 | local(Eigen::Vector3d::Zero()), 69 | covariance(CovStruct()), 70 | vel(0.0), 71 | azi_angle(0.0), 72 | ele_angle(0.0), 73 | intensity(0.0) {} 74 | 75 | ~PointStruct(){}; 76 | 77 | // reset function 78 | void reset() { 79 | pose.setZero(); 80 | local.setZero(); 81 | covariance.reset(); 82 | vel = 0.0; 83 | azi_angle = 0.0; 84 | ele_angle = 0.0; 85 | intensity = 0.0; 86 | } 87 | }; 88 | 89 | struct VoxelHashMap { 90 | using RadarPointVector = std::vector; 91 | using RadarPointVectorTuple = std::tuple; 92 | using Voxel = Eigen::Vector3i; 93 | 94 | struct VoxelBlock { 95 | // buffer of points with a max limit of n_points 96 | std::vector points; 97 | CovStruct covariance; 98 | int num_points; 99 | double map_resolution; 100 | 101 | inline void AddPoint(const PointStruct &point, const int max_point) { 102 | if (points.size() < static_cast(num_points)) { 103 | points.push_back(point); 104 | } 105 | } 106 | inline void AddPointWithSpacing(const PointStruct &point, const int max_point) { 107 | if (points.size() < static_cast(num_points) && 108 | std::none_of(points.cbegin(), points.cend(), [&](const auto &voxel_point) { 109 | return (voxel_point.pose - point.pose).norm() < map_resolution; 110 | })) { 111 | points.push_back(point); 112 | } 113 | } 114 | inline void CalVoxelCov() { 115 | int n = points.size(); 116 | 117 | covariance.cov = Eigen::Matrix3d::Identity(); 118 | covariance.mean = Eigen::Vector3d::Zero(); 119 | 120 | if (n == 0) { 121 | return; 122 | } 123 | else if (n == 1) { 124 | covariance.mean = points[0].pose; 125 | return; 126 | } 127 | 128 | // row: 3(x,y,z), column: number of points 129 | Eigen::Matrix neighbors(3, n); 130 | 131 | for (int j = 0; j < n; j++) { 132 | neighbors.col(j) = points[j].pose; 133 | } 134 | 135 | Eigen::Vector3d mean = neighbors.rowwise().mean(); 136 | 137 | neighbors.colwise() -= mean; 138 | Eigen::Matrix3d cov = (neighbors * neighbors.transpose()) / (n - 1); 139 | 140 | // Plane regularization 141 | Eigen::JacobiSVD svd(cov, Eigen::ComputeFullU | Eigen::ComputeFullV); 142 | Eigen::Vector3d values = Eigen::Vector3d(1, 1, 1e-3); 143 | 144 | cov = svd.matrixU() * values.asDiagonal() * svd.matrixV().transpose(); 145 | 146 | covariance.cov = cov; 147 | covariance.mean = mean; 148 | } 149 | }; 150 | struct VoxelHash { 151 | size_t operator()(const Voxel &voxel) const { 152 | const uint32_t *vec = reinterpret_cast(voxel.data()); 153 | return ((1 << 20) - 1) & (vec[0] * 73856093 ^ vec[1] * 19349669 ^ vec[2] * 83492791); 154 | } 155 | }; 156 | 157 | VoxelHashMap() {} 158 | VoxelHashMap(double voxel_size, int max_points_per_voxel) { 159 | voxel_size_ = voxel_size; 160 | max_points_per_voxel_ = max_points_per_voxel; 161 | } 162 | 163 | void Init(double voxel_size, int max_points_per_voxel); 164 | 165 | std::tuple, std::vector> GetCorrespondencePoints( 166 | const RadarPointVector &vec_points, double d_max_correspondence_dist) const; 167 | 168 | std::tuple, std::vector> GetCorrespondencesCov( 169 | const RadarPointVector &vec_points, double d_max_correspondence_dist) const; 170 | 171 | std::tuple, std::vector> GetCorrespondencesAllCov( 172 | const RadarPointVector &vec_points, double d_max_correspondence_dist) const; 173 | 174 | std::vector GetAdjacentVoxels(const PointStruct &point, int range) const; 175 | 176 | inline Voxel PointToVoxel(const Eigen::Vector3d &point, const double voxel_size) const { 177 | return VoxelHashMap::Voxel(static_cast(std::floor(point.x() / voxel_size)), 178 | static_cast(std::floor(point.y() / voxel_size)), 179 | static_cast(std::floor(point.z() / voxel_size))); 180 | } 181 | 182 | // calculate covariance for each voxel 183 | inline void CalVoxelCovAll() { 184 | // for (auto &[voxel, voxel_block] : map_) { 185 | // (void)voxel; 186 | // voxel_block.CalVoxelCov(); 187 | // } 188 | 189 | tbb::parallel_for_each(map_.begin(), map_.end(), [](auto &voxel_pair) { 190 | (void)voxel_pair.first; 191 | voxel_pair.second.CalVoxelCov(); 192 | }); 193 | } 194 | 195 | inline void ProcessVoxelBlock(VoxelBlock &voxel_block, const double d_search_dist_squared) { 196 | // Iterate over each point in the voxel 197 | for (auto &point : voxel_block.points) { 198 | // Container for neighbors 199 | std::vector neighbors; 200 | 201 | // Include the current point's pose 202 | neighbors.push_back(point.pose); 203 | 204 | // Get adjacent voxels using the helper function 205 | std::vector adjacent_voxels = GetAdjacentVoxels(point, 2); 206 | 207 | // Iterate over adjacent voxels 208 | for (const auto &neighbor_voxel : adjacent_voxels) { 209 | // Check if the neighboring voxel exists 210 | auto neighbor_it = map_.find(neighbor_voxel); 211 | if (neighbor_it == map_.end()) continue; 212 | 213 | const VoxelBlock &neighbor_block = neighbor_it->second; 214 | 215 | // Iterate over points in the neighboring voxel 216 | for (const auto &neighbor_point : neighbor_block.points) { 217 | // Check distance 218 | if ((neighbor_point.pose - point.pose).squaredNorm() <= d_search_dist_squared) { 219 | neighbors.push_back(neighbor_point.pose); 220 | } 221 | } 222 | } 223 | 224 | // Calculate covariance for the current point 225 | if (neighbors.size() == 1) { 226 | point.covariance.cov = Eigen::Matrix3d::Identity(); 227 | point.covariance.mean = point.pose; 228 | } 229 | else { 230 | Eigen::Matrix neighbor_matrix(3, neighbors.size()); 231 | 232 | for (size_t i = 0; i < neighbors.size(); ++i) { 233 | neighbor_matrix.col(i) = neighbors[i]; 234 | } 235 | 236 | Eigen::Vector3d mean = neighbor_matrix.rowwise().mean(); 237 | neighbor_matrix.colwise() -= mean; 238 | Eigen::Matrix3d cov = (neighbor_matrix * neighbor_matrix.transpose()) / (neighbors.size() - 1); 239 | 240 | // Plane regularization 241 | Eigen::JacobiSVD svd(cov, Eigen::ComputeFullU | Eigen::ComputeFullV); 242 | Eigen::Vector3d values = Eigen::Vector3d(1, 1, 1e-3); 243 | 244 | cov = svd.matrixU() * values.asDiagonal() * svd.matrixV().transpose(); 245 | 246 | point.covariance.cov = cov; 247 | point.covariance.mean = mean; 248 | } 249 | } 250 | } 251 | 252 | inline void CalPointCovAll(double d_search_dist) { 253 | // Square of the search distance for efficient comparison 254 | double d_search_dist_squared = d_search_dist * d_search_dist; 255 | tbb::parallel_for_each(map_.begin(), map_.end(), 256 | [&](auto &voxel_pair) { ProcessVoxelBlock(voxel_pair.second, d_search_dist_squared); }); 257 | } 258 | 259 | // voxel grid sampling to keep only one point per voxel 260 | inline std::vector VoxelDownsample(const std::vector &points, const double voxel_size) { 261 | // voxel as key, first SRadarPoint as value 262 | std::unordered_map grid; 263 | grid.reserve(points.size()); 264 | 265 | // calculate voxel coordinates based on SRadarPoint's pose, then add if not exists 266 | std::for_each(points.cbegin(), points.cend(), [&](const PointStruct &point) { 267 | VoxelHashMap::Voxel voxel = VoxelHashMap::PointToVoxel(point.pose, voxel_size); 268 | // skip if point exists in voxel 269 | if (grid.find(voxel) == grid.end()) { 270 | grid.insert({voxel, point}); 271 | } 272 | }); 273 | 274 | // 다운샘플링된 SRadarPoint들을 저장할 벡터 275 | std::vector points_downsampled; 276 | points_downsampled.reserve(grid.size()); 277 | 278 | // store SRadarPoint in hashmap to downsampled result 279 | std::for_each(grid.cbegin(), grid.cend(), 280 | [&](const auto &voxel_and_point) { points_downsampled.emplace_back(voxel_and_point.second); }); 281 | 282 | return points_downsampled; 283 | } 284 | 285 | inline bool FindGroundHeight(const Eigen::Vector2d &position, double &ground_z) const { 286 | double d_search_range = 5.0; 287 | double d_squred_search_range = d_search_range * d_search_range; 288 | std::vector vec_map_points = Pointcloud(); 289 | 290 | std::vector points_within_range; 291 | for (const auto &point : vec_map_points) { 292 | Eigen::Vector2d point_xy(point.pose.x(), point.pose.y()); 293 | if ((point_xy - position).squaredNorm() <= d_squred_search_range) { 294 | points_within_range.push_back(point); 295 | } 296 | } 297 | 298 | if (points_within_range.size() <= 3) { 299 | return false; 300 | } 301 | 302 | constexpr size_t N = 5; // Number of points to use for calculating ground height 303 | std::partial_sort(points_within_range.begin(), 304 | points_within_range.begin() + std::min(N, points_within_range.size()), 305 | points_within_range.end(), 306 | [](const PointStruct &a, const PointStruct &b) { return a.pose.z() < b.pose.z(); }); 307 | 308 | std::vector ground_points; 309 | for (size_t i = 0; i < std::min(N, points_within_range.size()); ++i) { 310 | ground_points.push_back(points_within_range[i].pose); 311 | } 312 | 313 | Eigen::Matrix neighbor_matrix(3, ground_points.size()); 314 | for (size_t i = 0; i < ground_points.size(); ++i) { 315 | neighbor_matrix.col(i) = ground_points[i]; 316 | } 317 | 318 | Eigen::Vector3d mean = neighbor_matrix.rowwise().mean(); 319 | ground_z = mean.z(); // Set the calculated mean Z value as ground height 320 | 321 | return true; 322 | } 323 | 324 | inline void Clear() { map_.clear(); } 325 | inline bool Empty() const { return map_.empty(); } 326 | 327 | void Update(const RadarPointVector &points, const Eigen::Vector3d &origin); 328 | void AddPoints(const RadarPointVector &points); 329 | std::vector Pointcloud() const; 330 | std::vector Covariances() const; 331 | 332 | double voxel_size_; 333 | int max_points_per_voxel_; 334 | std::unordered_map map_; 335 | }; 336 | 337 | #endif // __VOXEL_HASH_MAP_HPP__ -------------------------------------------------------------------------------- /src/app/localization/pcm_matching/launch/pcm_matching.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | -------------------------------------------------------------------------------- /src/app/localization/pcm_matching/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | pcm_matching 4 | 0.0.0 5 | The pcm_matching package 6 | 7 | 8 | 9 | 10 | JaeyoungJo 11 | 12 | 13 | 14 | 15 | MIT 16 | 17 | 18 | 19 | catkin 20 | roscpp 21 | std_msgs 22 | nav_msgs 23 | geometry_msgs 24 | sensor_msgs 25 | pcl_ros 26 | pcl_conversions 27 | roscpp 28 | std_msgs 29 | nav_msgs 30 | geometry_msgs 31 | sensor_msgs 32 | pcl_ros 33 | pcl_conversions 34 | roscpp 35 | std_msgs 36 | nav_msgs 37 | geometry_msgs 38 | sensor_msgs 39 | pcl_ros 40 | pcl_conversions 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | -------------------------------------------------------------------------------- /src/app/localization/pcm_matching/src/registration.cpp: -------------------------------------------------------------------------------- 1 | /****************************************************************************/ 2 | // Module: registration.cpp 3 | // Description: registration algorithm 4 | // 5 | // Authors: Jaeyoung Jo (wodud3743@gmail.com) 6 | // Version: 0.1 7 | // 8 | // Revision History 9 | // Oct 18, 2024: Jaeyoung Jo - Created. 10 | // XXXX XX, 2023: XXXXXXX XX - 11 | /****************************************************************************/ 12 | 13 | #include "registration.hpp" 14 | 15 | Eigen::Matrix4d Registration::AlignCloudsLocal(std::vector& source_global, 16 | const std::vector& target_global, 17 | Eigen::Matrix4d& last_icp_pose, double trans_th, 18 | RegistrationConfig m_config) { 19 | // sensor_velocity : relative velocity based on previous sensor frame 20 | 21 | Eigen::Matrix6d JTJ = Eigen::Matrix6d::Zero(); // 6x6 J^T J 22 | Eigen::Vector6d JTr = Eigen::Vector6d::Zero(); // 6x1 J^T R 23 | 24 | Eigen::Matrix4d last_icp_pose_inv = last_icp_pose.inverse(); 25 | 26 | double d_residual_sum = 0.0; 27 | 28 | for (size_t i = 0; i < source_global.size(); ++i) { 29 | Eigen::Vector4d target_hom_global(target_global[i].pose.x(), target_global[i].pose.y(), 30 | target_global[i].pose.z(), 1.0); 31 | Eigen::Vector4d target_hom_local = last_icp_pose_inv * target_hom_global; 32 | 33 | const Eigen::Vector3d target_local = target_hom_local.head<3>(); 34 | const Eigen::Vector3d residual_local = target_local - source_global[i].local.head<3>(); 35 | 36 | Eigen::Matrix3_6d J_g; // Jacobian Geometry (3x6) 37 | 38 | // 1. Jacobian Geometry 39 | // [ I(3x3), -(T p_k)^ ] 40 | J_g.block<3, 3>(0, 0) = Eigen::Matrix3d::Identity(); 41 | J_g.block<3, 3>(0, 3) = -1.0 * vectorToSkewSymmetricMatrix(source_global[i].local); 42 | 43 | // double sensor_dist = source_global[i].local.norm(); 44 | double weight_g = square(trans_th) / square(trans_th + residual_local.squaredNorm()); 45 | 46 | // 6x6 += (3x6).t * (3x3) * (3x6) 47 | JTJ.noalias() += weight_g * J_g.transpose() * J_g; // 6x3 3x6 = 6x6 48 | JTr.noalias() += weight_g * J_g.transpose() * residual_local; // 6x3 3x1 = 6x1 49 | 50 | d_residual_sum += residual_local.norm(); 51 | } 52 | 53 | d_fitness_score_ = d_residual_sum / source_global.size(); 54 | 55 | Eigen::Matrix6d JTJ_diag = JTJ.diagonal().asDiagonal(); 56 | const Eigen::Vector6d x_tot = (JTJ + m_config.lm_lambda * JTJ_diag).ldlt().solve(JTr); 57 | 58 | Eigen::Vector3d rotation_vector = x_tot.tail<3>(); // rpy 59 | Eigen::Matrix4d transformation = Eigen::Matrix4d::Identity(); 60 | transformation.block<3, 3>(0, 0) = 61 | (Eigen::AngleAxisd(rotation_vector.norm(), rotation_vector.normalized())).toRotationMatrix(); // rotation 62 | transformation.block<3, 1>(0, 3) = x_tot.head<3>(); // transform xyz 63 | 64 | // small transformation based on sensor frame 65 | return transformation; 66 | } 67 | 68 | Eigen::Matrix4d Registration::AlignCloudsLocalPointCov(std::vector& source_global, 69 | const std::vector& target_global, 70 | Eigen::Matrix6d& local_cov, Eigen::Matrix4d& last_icp_pose, 71 | double trans_th, RegistrationConfig m_config) { 72 | // sensor_velocity : relative velocity based on previous sensor frame 73 | 74 | Eigen::Matrix6d JTJ = Eigen::Matrix6d::Zero(); // 6x6 J^T J 75 | Eigen::Vector6d JTr = Eigen::Vector6d::Zero(); // 6x1 J^T R 76 | 77 | Eigen::Matrix3d mahalanobis_local = Eigen::Matrix3d::Identity(); 78 | Eigen::Matrix3d sensor_rot = last_icp_pose.block<3, 3>(0, 0); 79 | Eigen::Matrix3d sensor_rot_inv = sensor_rot.inverse(); 80 | 81 | Eigen::Matrix4d last_icp_pose_inv = last_icp_pose.inverse(); 82 | 83 | double d_residual_sum = 0.0; 84 | 85 | for (size_t i = 0; i < source_global.size(); ++i) { 86 | const auto& target_cov = target_global[i].covariance; 87 | 88 | // calculate eigenvalues and eigenvectors from covariance matrix 89 | Eigen::SelfAdjointEigenSolver eigen_solver(target_cov.cov); 90 | // eigenvector corresponding to smallest eigenvalue is normal vector 91 | Eigen::Vector3d vec_normal_global = eigen_solver.eigenvectors().col(0); 92 | 93 | // transform normal vector from global to local frame 94 | Eigen::Vector3d vec_normal_local = sensor_rot_inv.matrix() * vec_normal_global; 95 | vec_normal_local.normalize(); // normalize 96 | 97 | Eigen::Vector4d target_hom_global(target_cov.mean(0), target_cov.mean(1), target_cov.mean(2), 1.0); 98 | Eigen::Vector4d target_hom_local = last_icp_pose_inv * target_hom_global; 99 | 100 | const Eigen::Vector3d target_local = target_hom_local.head<3>(); 101 | const Eigen::Vector3d residual_local = target_local - source_global[i].local.head<3>(); 102 | 103 | Eigen::Matrix3_6d J_g; // Jacobian Geometry (3x6) 104 | 105 | const auto& cov_A = Eigen::Matrix3d::Identity(); // source sensor local frame 106 | const auto& cov_B = target_cov.cov; // target cov is global map frame 107 | Eigen::Matrix3d RCR = sensor_rot_inv.matrix() * cov_B * sensor_rot_inv.matrix().transpose(); // local 108 | 109 | if (m_config.use_radar_cov == true) { 110 | RCR += source_global[i].covariance.cov; 111 | } 112 | 113 | mahalanobis_local = RCR.inverse(); // local 114 | 115 | // 1. Jacobian Geometry 116 | // [ I(3x3), -(T p_k)^ ] 117 | J_g.block<3, 3>(0, 0) = Eigen::Matrix3d::Identity(); 118 | J_g.block<3, 3>(0, 3) = -1.0 * vectorToSkewSymmetricMatrix(source_global[i].local); 119 | 120 | // double sensor_dist = source_global[i].local.norm(); 121 | double weight_g = square(trans_th) / square(trans_th + residual_local.squaredNorm()) * 0.8 + 0.2; 122 | 123 | // 6x6 += (3x6).t * (3x3) * (3x6) 124 | JTJ.noalias() += weight_g * J_g.transpose() * mahalanobis_local * J_g; // 6x3 3x6 = 6x6 125 | JTr.noalias() += weight_g * J_g.transpose() * mahalanobis_local * residual_local; // 6x3 3x1 = 6x1 126 | 127 | // Point-to-plane 거리 계산 및 누적 128 | double d_point_to_plane_dist = std::fabs(residual_local.dot(vec_normal_local)); 129 | 130 | // d_residual_sum += residual_local.norm(); 131 | d_residual_sum += d_point_to_plane_dist; 132 | } 133 | 134 | d_fitness_score_ = d_residual_sum / source_global.size(); 135 | 136 | Eigen::Matrix6d JTJ_diag = JTJ.diagonal().asDiagonal(); 137 | Eigen::Matrix6d regularized_JTJ = JTJ + m_config.lm_lambda * JTJ_diag; 138 | const Eigen::Vector6d x_tot = regularized_JTJ.ldlt().solve(JTr); 139 | 140 | // Inverse of JTJ to compute covariance 141 | Eigen::Matrix6d covariance = regularized_JTJ.inverse(); 142 | local_cov = covariance; 143 | 144 | Eigen::Vector3d rotation_vector = x_tot.tail<3>(); // rpy 145 | Eigen::Matrix4d transformation = Eigen::Matrix4d::Identity(); 146 | transformation.block<3, 3>(0, 0) = 147 | (Eigen::AngleAxisd(rotation_vector.norm(), rotation_vector.normalized())).toRotationMatrix(); // rotation 148 | transformation.block<3, 1>(0, 3) = x_tot.head<3>(); // transform xyz 149 | 150 | // small transformation based on sensor frame 151 | return transformation; 152 | } 153 | 154 | Eigen::Matrix4d Registration::AlignCloudsLocalVoxelCov(std::vector& source_global, 155 | const std::vector& target_cov_global, 156 | Eigen::Matrix4d& last_icp_pose, double trans_th, 157 | RegistrationConfig m_config) { 158 | // sensor_velocity : relative velocity based on previous sensor frame 159 | 160 | Eigen::Matrix6d JTJ = Eigen::Matrix6d::Zero(); // 6x6 J^T J 161 | Eigen::Vector6d JTr = Eigen::Vector6d::Zero(); // 6x1 J^T R 162 | 163 | Eigen::Matrix3d mahalanobis_local = Eigen::Matrix3d::Identity(); 164 | Eigen::Matrix3d sensor_rot = last_icp_pose.block<3, 3>(0, 0); 165 | Eigen::Matrix3d sensor_rot_inv = sensor_rot.inverse(); 166 | 167 | Eigen::Matrix4d last_icp_pose_inv = last_icp_pose.inverse(); 168 | 169 | double d_residual_sum = 0.0; 170 | // find nearest target cov for each source_global point 171 | for (size_t i = 0; i < source_global.size(); ++i) { 172 | // find center of target cov 173 | const auto& target_cov = target_cov_global[i]; 174 | 175 | // convert center of target cov to homogeneous coordinates and then to local frame 176 | Eigen::Vector4d target_hom_global(target_cov.mean(0), target_cov.mean(1), target_cov.mean(2), 1.0); 177 | Eigen::Vector4d target_hom_local = last_icp_pose_inv * target_hom_global; 178 | 179 | // calculate local coordinates and residual 180 | const Eigen::Vector3d target_local = target_hom_local.head<3>(); 181 | const Eigen::Vector3d residual_local = target_local - source_global[i].local.head<3>(); 182 | 183 | Eigen::Matrix3_6d J_g; // Jacobian Geometry (3x6) 184 | 185 | const auto& cov_A = Eigen::Matrix3d::Identity(); // source sensor local frame 186 | const auto& cov_B = target_cov.cov; // target cov is global map frame 187 | Eigen::Matrix3d RCR = sensor_rot_inv.matrix() * cov_B * sensor_rot_inv.matrix().transpose(); // local 188 | if (m_config.use_radar_cov == true) { 189 | RCR += source_global[i].covariance.cov; 190 | } 191 | mahalanobis_local = RCR.inverse(); // local 192 | 193 | // 1. Jacobian Geometry 194 | // [ I(3x3), -(T p_k)^ ] 195 | J_g.block<3, 3>(0, 0) = Eigen::Matrix3d::Identity(); 196 | J_g.block<3, 3>(0, 3) = -1.0 * vectorToSkewSymmetricMatrix(source_global[i].local); 197 | 198 | // 각 포인트에 대한 가중치 계산 (공분산 기반) 199 | double weight_g = square(trans_th) / square(trans_th + residual_local.squaredNorm()); 200 | 201 | if (weight_g < 0.01) continue; 202 | 203 | // 6x6 += (3x6).t * (3x3) * (3x6) 204 | JTJ.noalias() += weight_g * J_g.transpose() * mahalanobis_local * J_g; // 6x3 3x6 = 6x6 205 | JTr.noalias() += weight_g * J_g.transpose() * mahalanobis_local * residual_local; // 6x3 3x1 = 6x1 206 | 207 | d_residual_sum += residual_local.norm(); 208 | } 209 | 210 | d_fitness_score_ = d_residual_sum / source_global.size(); 211 | 212 | // apply Levenberg-Marquardt method 213 | Eigen::Matrix6d JTJ_diag = JTJ.diagonal().asDiagonal(); 214 | const Eigen::Vector6d x_tot = (JTJ + m_config.lm_lambda * JTJ_diag).ldlt().solve(JTr); 215 | 216 | // calculate rotation vector and transformation 217 | Eigen::Vector3d rotation_vector = x_tot.tail<3>(); // rpy 218 | Eigen::Matrix4d transformation = Eigen::Matrix4d::Identity(); 219 | transformation.block<3, 3>(0, 0) = 220 | (Eigen::AngleAxisd(rotation_vector.norm(), rotation_vector.normalized())).toRotationMatrix(); // rotation 221 | transformation.block<3, 1>(0, 3) = x_tot.head<3>(); // transform xyz 222 | 223 | // Source Point 의 센서 좌표 기준 미소 transformation 반환 224 | return transformation; 225 | } 226 | 227 | Eigen::Matrix4d Registration::AlignCloudsGlobal(std::vector& source_global, 228 | const std::vector& target_global, double trans_th, 229 | RegistrationConfig m_config) { 230 | // sensor_velocity : relative velocity based on previous sensor frame 231 | 232 | Eigen::Matrix6d JTJ = Eigen::Matrix6d::Zero(); // 6x6 J^T J 233 | Eigen::Vector6d JTr = Eigen::Vector6d::Zero(); // 6x1 J^T R 234 | 235 | for (size_t i = 0; i < source_global.size(); ++i) { 236 | const Eigen::Vector3d residual_global = target_global[i].pose - source_global[i].pose; 237 | 238 | Eigen::Matrix3_6d J_g; // Jacobian Geometry (3x6) 239 | Eigen::Matrix1_3d R_tot; // Residual Total (1x3) 240 | 241 | // 1. Jacobian Geometry 242 | // [ I(3x3), -(T p_k)^ ] 243 | J_g.block<3, 3>(0, 0) = Eigen::Matrix3d::Identity(); 244 | J_g.block<3, 3>(0, 3) = -1.0 * vectorToSkewSymmetricMatrix(source_global[i].pose); 245 | 246 | double weight_g = square(trans_th) / square(trans_th + residual_global.squaredNorm()); 247 | 248 | if (std::isnan(weight_g)) { 249 | continue; 250 | } 251 | 252 | // Geometric Residual with weight 253 | R_tot.block<1, 3>(0, 0) = residual_global; 254 | 255 | // 6x6 += (3x6).t * (3x3) * (3x6) 256 | JTJ.noalias() += J_g.transpose() * weight_g * J_g; // 6x3 3x6 = 6x6 257 | JTr.noalias() += J_g.transpose() * weight_g * R_tot.transpose(); // 6x3 3x1 = 6x1 258 | } 259 | 260 | Eigen::Matrix6d JTJ_diag = JTJ.diagonal().asDiagonal(); 261 | const Eigen::Vector6d x_tot = (JTJ + m_config.lm_lambda * JTJ_diag).ldlt().solve(JTr); 262 | 263 | Eigen::Vector3d rotation_vector = x_tot.tail<3>(); // rpy 264 | Eigen::Matrix4d transformation = Eigen::Matrix4d::Identity(); 265 | transformation.block<3, 3>(0, 0) = 266 | (Eigen::AngleAxisd(rotation_vector.norm(), rotation_vector.normalized())).toRotationMatrix(); // rotation 267 | transformation.block<3, 1>(0, 3) = x_tot.head<3>(); // transform xyz 268 | 269 | // global transform 270 | return transformation; 271 | } 272 | 273 | // Local frame, voxel map, initial position initial_guess 274 | Eigen::Matrix4d Registration::RunRegister(const std::vector& source_local, const VoxelHashMap& voxel_map, 275 | const Eigen::Matrix4d& initial_guess, RegistrationConfig m_config, 276 | bool& is_success, double& fitness_score, Eigen::Matrix6d& local_cov) { 277 | std::vector> vec_cor_origin_pair; 278 | std::vector source_c_global, target_c_global; 279 | std::vector target_cov_c_global; 280 | local_cov = Eigen::Matrix6d::Identity(); 281 | 282 | int i_source_total_num = source_local.size(); 283 | int i_source_corr_num = 0; 284 | double corres_ratio = 0.0; 285 | 286 | // 1. Create source in global frame and calculate point uncertainty 287 | std::vector source_global; 288 | source_global.resize(source_local.size()); 289 | TransformPoints(initial_guess, source_local, source_global); 290 | 291 | if (voxel_map.Empty()) { 292 | std::cout << YELLOW << "VOXEL MAP EMPTY!" << RESET << std::endl; 293 | is_success = false; 294 | return initial_guess; 295 | } 296 | 297 | // 2. ICP 수행 298 | Eigen::Matrix4d last_icp_pose = initial_guess; 299 | Eigen::Matrix4d estimation_local = Eigen::Matrix4d::Identity(); // small change in sensor frame 300 | Velocity iter_velocity; 301 | 302 | if (m_config.use_radar_cov == true) { 303 | CalFramePointCov(source_global, m_config.range_variance_m, m_config.azimuth_variance_deg, 304 | m_config.elevation_variance_deg); 305 | } 306 | 307 | auto start = std::chrono::steady_clock::now(); 308 | int i_iteration = 0; 309 | double total_correspondence_time_ms = 0; 310 | for (int j = 0; j < m_config.max_iteration; ++j) { 311 | i_iteration++; 312 | // Get Correspodence in global frame 313 | // source_c_global : valid points in source_global 314 | // target_c_global : points in map associated with source_c_global or cov 315 | 316 | auto correspondence_start = std::chrono::steady_clock::now(); 317 | 318 | switch (m_config.icp_method) { 319 | case IcpMethod::P2P: 320 | std::tie(source_c_global, target_c_global) = 321 | voxel_map.GetCorrespondencePoints(source_global, m_config.max_search_dist); 322 | break; 323 | case IcpMethod::GICP: 324 | std::tie(source_c_global, target_c_global) = 325 | voxel_map.GetCorrespondencePoints(source_global, m_config.max_search_dist); 326 | break; 327 | case IcpMethod::VGICP: 328 | std::tie(source_c_global, target_cov_c_global) = 329 | voxel_map.GetCorrespondencesCov(source_global, m_config.max_search_dist); 330 | break; 331 | case IcpMethod::AVGICP: 332 | std::tie(source_c_global, target_cov_c_global) = 333 | voxel_map.GetCorrespondencesAllCov(source_global, m_config.max_search_dist); 334 | break; 335 | } 336 | 337 | auto correspondence_end = std::chrono::steady_clock::now(); 338 | auto correspondence_elapsed_ms = 339 | std::chrono::duration_cast(correspondence_end - correspondence_start) 340 | .count(); 341 | total_correspondence_time_ms += correspondence_elapsed_ms; 342 | 343 | if (m_config.b_debug_print) { 344 | std::cout << "[Registration] Total Correspondence Time for: " << i_iteration << " in " 345 | << (correspondence_elapsed_ms / 1000.0) << " ms, and cores num: " << source_c_global.size() 346 | << RESET << std::endl; 347 | } 348 | 349 | i_source_corr_num = source_c_global.size(); 350 | 351 | corres_ratio = (float)i_source_corr_num / i_source_total_num; 352 | if (corres_ratio < m_config.min_overlap_ratio) { 353 | std::cout << YELLOW << "[RunRegister] Small corresponding ratio. " << corres_ratio << RESET << std::endl; 354 | is_success = false; 355 | return last_icp_pose; 356 | } 357 | 358 | switch (m_config.icp_method) { 359 | case IcpMethod::P2P: 360 | estimation_local = AlignCloudsLocal(source_c_global, target_c_global, last_icp_pose, 361 | m_config.max_search_dist, m_config); 362 | break; 363 | case IcpMethod::GICP: 364 | estimation_local = AlignCloudsLocalPointCov(source_c_global, target_c_global, local_cov, last_icp_pose, 365 | m_config.max_search_dist, m_config); 366 | break; 367 | case IcpMethod::VGICP: 368 | estimation_local = AlignCloudsLocalVoxelCov(source_c_global, target_cov_c_global, last_icp_pose, 369 | m_config.max_search_dist, m_config); 370 | break; 371 | case IcpMethod::AVGICP: 372 | estimation_local = AlignCloudsLocalVoxelCov(source_c_global, target_cov_c_global, last_icp_pose, 373 | m_config.max_search_dist, m_config); 374 | break; 375 | } 376 | 377 | // calculate estimated pose in global frame 378 | last_icp_pose = last_icp_pose * estimation_local; 379 | 380 | // check ICP termination condition 381 | Eigen::AngleAxisd angleAxis(estimation_local.block<3, 3>(0, 0)); 382 | double rot_norm = angleAxis.angle(); 383 | 384 | double transform_norm = rot_norm + estimation_local.block<3, 1>(0, 3).norm(); 385 | if (transform_norm < m_config.icp_termination_threshold_m) { 386 | break; 387 | } 388 | 389 | // small movement in global frame 390 | TransformPoints(last_icp_pose, source_local, source_global); 391 | } 392 | 393 | auto end = std::chrono::steady_clock::now(); 394 | auto elapsed_ns = std::chrono::duration_cast(end - start).count(); 395 | 396 | if (m_config.b_debug_print) { 397 | std::cout << "[Registration] Total Correspondence Time: " << (total_correspondence_time_ms / 1000.0) << " ms" 398 | << RESET << std::endl; 399 | std::cout << "[Registration] RunRegister: iteration " << i_iteration << " executed in " << (elapsed_ns / 1000.0) 400 | << " ms" << RESET << std::endl; 401 | 402 | std::cout << GREEN << "[RunRegister] Corresponding ratio " << corres_ratio << RESET << std::endl; 403 | } 404 | 405 | if (d_fitness_score_ > m_config.max_fitness_score) { 406 | std::cout << YELLOW << "[RunRegister] ICP Fitness Score Low " << d_fitness_score_ << RESET << std::endl; 407 | is_success = false; 408 | return last_icp_pose; 409 | } 410 | 411 | if (m_config.b_debug_print) { 412 | std::cout << GREEN << "[RunRegister] ICP Fitness Score " << d_fitness_score_ << RESET << std::endl; 413 | } 414 | 415 | fitness_score = d_fitness_score_; 416 | is_success = true; 417 | return last_icp_pose; 418 | } -------------------------------------------------------------------------------- /src/app/localization/pcm_matching/src/voxel_hash_map.cpp: -------------------------------------------------------------------------------- 1 | #include "voxel_hash_map.hpp" 2 | 3 | // Variables and functions only used in this cpp 4 | namespace { 5 | struct ResultTuple { 6 | ResultTuple() = default; 7 | ResultTuple(std::size_t n) { 8 | vec_source.reserve(n); 9 | vec_target.reserve(n); 10 | } 11 | std::vector vec_source; 12 | std::vector vec_target; 13 | }; 14 | 15 | struct ResultTupleCov { 16 | ResultTupleCov() = default; 17 | ResultTupleCov(std::size_t n) { 18 | vec_source.reserve(n); 19 | vec_target.reserve(n); 20 | } 21 | std::vector vec_source; 22 | std::vector vec_target; 23 | }; 24 | } // namespace 25 | 26 | void VoxelHashMap::Init(double voxel_size, int max_points_per_voxel) { 27 | voxel_size_ = voxel_size; 28 | max_points_per_voxel_ = max_points_per_voxel; 29 | } 30 | 31 | std::tuple, std::vector> VoxelHashMap::GetCorrespondencePoints( 32 | const RadarPointVector& vec_points, double d_max_correspondence_dist) const { 33 | const double d_max_dist_squared = d_max_correspondence_dist * d_max_correspondence_dist; 34 | 35 | auto GetClosestNeighbor = [&](const PointStruct& point) -> PointStruct { 36 | std::vector vec_voxels = GetAdjacentVoxels(point, 2); 37 | PointStruct closest_neighbor; 38 | double d_closest_dist_squared = std::numeric_limits::max(); 39 | 40 | for (const auto& voxel : vec_voxels) { 41 | auto search = map_.find(voxel); 42 | if (search != map_.end()) { 43 | for (const auto& neighbor : search->second.points) { 44 | double d_dist_squared = (neighbor.pose - point.pose).squaredNorm(); 45 | if (d_dist_squared < d_closest_dist_squared) { 46 | closest_neighbor = neighbor; 47 | d_closest_dist_squared = d_dist_squared; 48 | } 49 | } 50 | } 51 | } 52 | return closest_neighbor; 53 | }; 54 | 55 | using points_iterator = RadarPointVector::const_iterator; 56 | 57 | auto result = tbb::parallel_reduce( 58 | tbb::blocked_range(vec_points.cbegin(), vec_points.cend()), ResultTuple(0), 59 | [&](const tbb::blocked_range& r, ResultTuple res) -> ResultTuple { 60 | auto& [vec_source, vec_target] = res; 61 | 62 | for (auto it = r.begin(); it != r.end(); ++it) { 63 | const auto& point = *it; 64 | PointStruct closest_neighbor = GetClosestNeighbor(point); 65 | 66 | if ((closest_neighbor.pose - point.pose).squaredNorm() < d_max_dist_squared) { 67 | vec_source.emplace_back(point); 68 | vec_target.emplace_back(closest_neighbor); 69 | } 70 | } 71 | 72 | return res; 73 | }, 74 | [](ResultTuple a, const ResultTuple& b) -> ResultTuple { 75 | auto& [vec_source_a, vec_target_a] = a; 76 | const auto& [vec_source_b, vec_target_b] = b; 77 | 78 | vec_source_a.insert(vec_source_a.end(), std::make_move_iterator(vec_source_b.begin()), 79 | std::make_move_iterator(vec_source_b.end())); 80 | vec_target_a.insert(vec_target_a.end(), std::make_move_iterator(vec_target_b.begin()), 81 | std::make_move_iterator(vec_target_b.end())); 82 | 83 | return a; 84 | }); 85 | 86 | auto& [vec_source, vec_target] = result; 87 | return std::make_pair(std::move(vec_source), std::move(vec_target)); 88 | } 89 | 90 | std::tuple, std::vector> VoxelHashMap::GetCorrespondencesCov( 91 | const RadarPointVector& vec_points, double d_max_correspondence_dist) const { 92 | auto GetClosestCovariance = [&](const PointStruct& point) { 93 | std::vector vec_voxels = GetAdjacentVoxels(point, 2); 94 | std::vector vec_neighbors_cov; 95 | vec_neighbors_cov.reserve(vec_voxels.size()); 96 | 97 | for (const auto& voxel : vec_voxels) { 98 | auto search = map_.find(voxel); 99 | if (search != map_.end() && search->second.points.size() > 0) { 100 | vec_neighbors_cov.push_back(search->second.covariance); 101 | } 102 | } 103 | 104 | CovStruct closest_cov; 105 | double d_closest_dist_squared = std::numeric_limits::max(); 106 | for (const auto& cov : vec_neighbors_cov) { 107 | double d_dist_squared = (cov.mean - point.pose).squaredNorm(); 108 | if (d_dist_squared < d_closest_dist_squared) { 109 | closest_cov = cov; 110 | d_closest_dist_squared = d_dist_squared; 111 | } 112 | } 113 | 114 | return closest_cov; 115 | }; 116 | 117 | using points_iterator = RadarPointVector::const_iterator; 118 | const double d_max_dist_squared = d_max_correspondence_dist * d_max_correspondence_dist; 119 | 120 | auto result = tbb::parallel_reduce( 121 | tbb::blocked_range(vec_points.cbegin(), vec_points.cend()), ResultTupleCov(0), 122 | [&](const tbb::blocked_range& r, ResultTupleCov res) -> ResultTupleCov { 123 | auto& [vec_source, vec_target] = res; 124 | 125 | for (auto it = r.begin(); it != r.end(); ++it) { 126 | const auto& point = *it; 127 | CovStruct closest_cov = GetClosestCovariance(point); 128 | 129 | if ((closest_cov.mean - point.pose).squaredNorm() < d_max_dist_squared) { 130 | vec_source.emplace_back(point); 131 | vec_target.emplace_back(closest_cov); 132 | } 133 | } 134 | 135 | return res; 136 | }, 137 | [](ResultTupleCov a, const ResultTupleCov& b) -> ResultTupleCov { 138 | auto& [vec_source_a, vec_target_a] = a; 139 | const auto& [vec_source_b, vec_target_b] = b; 140 | 141 | vec_source_a.insert(vec_source_a.end(), std::make_move_iterator(vec_source_b.begin()), 142 | std::make_move_iterator(vec_source_b.end())); 143 | vec_target_a.insert(vec_target_a.end(), std::make_move_iterator(vec_target_b.begin()), 144 | std::make_move_iterator(vec_target_b.end())); 145 | 146 | return a; 147 | }); 148 | 149 | auto& [vec_source, vec_target] = result; 150 | return std::make_tuple(std::move(vec_source), std::move(vec_target)); 151 | } 152 | 153 | std::tuple, std::vector> VoxelHashMap::GetCorrespondencesAllCov( 154 | const RadarPointVector& vec_points, double d_max_correspondence_dist) const { 155 | auto GetAllCovariances = [&](const PointStruct& point) { 156 | std::vector vec_voxels = GetAdjacentVoxels(point, 1); 157 | std::vector vec_neighbors_cov; 158 | vec_neighbors_cov.reserve(vec_voxels.size()); 159 | 160 | for (const auto& voxel : vec_voxels) { 161 | auto search = map_.find(voxel); 162 | if (search != map_.end() && !search->second.points.empty()) { 163 | vec_neighbors_cov.emplace_back(search->second.covariance); 164 | } 165 | } 166 | 167 | return vec_neighbors_cov; 168 | }; 169 | 170 | using points_iterator = RadarPointVector::const_iterator; 171 | const double d_max_dist_squared = d_max_correspondence_dist * d_max_correspondence_dist; 172 | 173 | auto result = tbb::parallel_reduce( 174 | tbb::blocked_range(vec_points.cbegin(), vec_points.cend()), ResultTupleCov(0), 175 | [&](const tbb::blocked_range& r, ResultTupleCov res) -> ResultTupleCov { 176 | auto& [vec_source, vec_target] = res; 177 | 178 | for (auto it = r.begin(); it != r.end(); ++it) { 179 | const auto& point = *it; 180 | auto vec_neighbors_cov = GetAllCovariances(point); 181 | 182 | for (const auto& cov : vec_neighbors_cov) { 183 | if ((cov.mean - point.pose).squaredNorm() < d_max_dist_squared) { 184 | vec_source.emplace_back(point); 185 | vec_target.emplace_back(cov); 186 | } 187 | } 188 | } 189 | 190 | return res; 191 | }, 192 | [](ResultTupleCov a, const ResultTupleCov& b) -> ResultTupleCov { 193 | auto& [vec_source_a, vec_target_a] = a; 194 | const auto& [vec_source_b, vec_target_b] = b; 195 | 196 | vec_source_a.insert(vec_source_a.end(), std::make_move_iterator(vec_source_b.begin()), 197 | std::make_move_iterator(vec_source_b.end())); 198 | vec_target_a.insert(vec_target_a.end(), std::make_move_iterator(vec_target_b.begin()), 199 | std::make_move_iterator(vec_target_b.end())); 200 | 201 | return a; 202 | }); 203 | 204 | auto& [vec_source, vec_target] = result; 205 | return std::make_tuple(std::move(vec_source), std::move(vec_target)); 206 | } 207 | 208 | std::vector VoxelHashMap::GetAdjacentVoxels(const PointStruct& point, int range) const { 209 | std::vector voxels; 210 | 211 | VoxelHashMap::Voxel voxel = PointToVoxel(point.pose, voxel_size_); 212 | 213 | int voxel_x = voxel.x(); 214 | int voxel_y = voxel.y(); 215 | int voxel_z = voxel.z(); 216 | 217 | int voxel_neighbor = 1; 218 | int voxel_search_num = 27; // 3x3x3 = 27 219 | 220 | if (range == 0) { 221 | return std::vector{voxel}; 222 | } 223 | else if (range == 1) { 224 | return std::vector{VoxelHashMap::Voxel(voxel_x + 0, voxel_y + 0, voxel_z + 0), 225 | VoxelHashMap::Voxel(voxel_x + 1, voxel_y + 0, voxel_z + 0), 226 | VoxelHashMap::Voxel(voxel_x - 1, voxel_y + 0, voxel_z + 0), 227 | VoxelHashMap::Voxel(voxel_x + 0, voxel_y + 1, voxel_z + 0), 228 | VoxelHashMap::Voxel(voxel_x + 0, voxel_y - 1, voxel_z + 0), 229 | VoxelHashMap::Voxel(voxel_x + 0, voxel_y + 0, voxel_z + 1), 230 | VoxelHashMap::Voxel(voxel_x + 0, voxel_y + 0, voxel_z - 1)}; 231 | } 232 | else { 233 | voxels.reserve(voxel_search_num); 234 | for (int i = voxel.x() - voxel_neighbor; i < voxel.x() + voxel_neighbor + 1; ++i) { 235 | for (int j = voxel.y() - voxel_neighbor; j < voxel.y() + voxel_neighbor + 1; ++j) { 236 | for (int k = voxel.z() - voxel_neighbor; k < voxel.z() + voxel_neighbor + 1; ++k) { 237 | voxels.emplace_back(i, j, k); 238 | } 239 | } 240 | } 241 | return voxels; 242 | } 243 | } 244 | 245 | std::vector VoxelHashMap::Pointcloud() const { 246 | std::vector points; 247 | points.reserve(max_points_per_voxel_ * map_.size()); // max points per voxel * total voxels 248 | for (const auto& [voxel, voxel_block] : map_) { // iterate over each voxel 249 | (void)voxel; // unused 250 | for (const auto& point : voxel_block.points) { // iterate over points in voxel 251 | points.push_back(point); 252 | } 253 | } 254 | return points; 255 | } 256 | 257 | std::vector VoxelHashMap::Covariances() const { 258 | std::vector covariances; 259 | covariances.reserve(map_.size()); // max points per voxel * total voxels 260 | for (const auto& [voxel, voxel_block] : map_) { // iterate over each voxel 261 | (void)voxel; // unused 262 | if (voxel_block.points.size() > 2) covariances.push_back(voxel_block.covariance); 263 | } 264 | return covariances; 265 | } 266 | 267 | // update points in global frame 268 | void VoxelHashMap::Update(const RadarPointVector& points, const Eigen::Vector3d& origin) { AddPoints(points); } 269 | 270 | void VoxelHashMap::AddPoints(const RadarPointVector& points) { 271 | if (points.size() == 0) return; 272 | const double map_resolution = std::sqrt(voxel_size_ * voxel_size_ / max_points_per_voxel_); 273 | 274 | std::for_each(points.cbegin(), points.cend(), [&](const auto& point) { // point = PointStruct 275 | auto voxel = Voxel((point.pose / voxel_size_).template cast()); 276 | auto search = map_.find(voxel); 277 | if (search != map_.end()) { 278 | auto& voxel_block = search->second; 279 | voxel_block.AddPointWithSpacing(point, max_points_per_voxel_); 280 | } 281 | else { 282 | map_.insert({voxel, VoxelBlock{{point}, CovStruct(), max_points_per_voxel_, map_resolution}}); 283 | } 284 | }); 285 | } 286 | -------------------------------------------------------------------------------- /src/bsw/system/ini_parser/convert_UTF.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2001-2004 Unicode, Inc. 3 | * 4 | * Disclaimer 5 | * 6 | * This source code is provided as is by Unicode, Inc. No claims are 7 | * made as to fitness for any particular purpose. No warranties of any 8 | * kind are expressed or implied. The recipient agrees to determine 9 | * applicability of information provided. If this file has been 10 | * purchased on magnetic or optical media from Unicode, Inc., the 11 | * sole remedy for any claim will be exchange of defective media 12 | * within 90 days of receipt. 13 | * 14 | * Limitations on Rights to Redistribute This Code 15 | * 16 | * Unicode, Inc. hereby grants the right to freely use the information 17 | * supplied in this file in the creation of products supporting the 18 | * Unicode Standard, and to make copies of this file in any form 19 | * for internal or external distribution as long as this notice 20 | * remains attached. 21 | */ 22 | 23 | /* --------------------------------------------------------------------- 24 | 25 | Conversions between UTF32, UTF-16, and UTF-8. Header file. 26 | 27 | Several funtions are included here, forming a complete set of 28 | conversions between the three formats. UTF-7 is not included 29 | here, but is handled in a separate source file. 30 | 31 | Each of these routines takes pointers to input buffers and output 32 | buffers. The input buffers are const. 33 | 34 | Each routine converts the text between *sourceStart and sourceEnd, 35 | putting the result into the buffer between *targetStart and 36 | targetEnd. Note: the end pointers are *after* the last item: e.g. 37 | *(sourceEnd - 1) is the last item. 38 | 39 | The return result indicates whether the conversion was successful, 40 | and if not, whether the problem was in the source or target buffers. 41 | (Only the first encountered problem is indicated.) 42 | 43 | After the conversion, *sourceStart and *targetStart are both 44 | updated to point to the end of last text successfully converted in 45 | the respective buffers. 46 | 47 | Input parameters: 48 | sourceStart - pointer to a pointer to the source buffer. 49 | The contents of this are modified on return so that 50 | it points at the next thing to be converted. 51 | targetStart - similarly, pointer to pointer to the target buffer. 52 | sourceEnd, targetEnd - respectively pointers to the ends of the 53 | two buffers, for overflow checking only. 54 | 55 | These conversion functions take a ConversionFlags argument. When this 56 | flag is set to strict, both irregular sequences and isolated surrogates 57 | will cause an error. When the flag is set to lenient, both irregular 58 | sequences and isolated surrogates are converted. 59 | 60 | Whether the flag is strict or lenient, all illegal sequences will cause 61 | an error return. This includes sequences such as: , , 62 | or in UTF-8, and values above 0x10FFFF in UTF-32. Conformant code 63 | must check for illegal sequences. 64 | 65 | When the flag is set to lenient, characters over 0x10FFFF are converted 66 | to the replacement character; otherwise (when the flag is set to strict) 67 | they constitute an error. 68 | 69 | Output parameters: 70 | The value "sourceIllegal" is returned from some routines if the input 71 | sequence is malformed. When "sourceIllegal" is returned, the source 72 | value will point to the illegal value that caused the problem. E.g., 73 | in UTF-8 when a sequence is malformed, it points to the start of the 74 | malformed sequence. 75 | 76 | Author: Mark E. Davis, 1994. 77 | Rev History: Rick McGowan, fixes & updates May 2001. 78 | Fixes & updates, Sept 2001. 79 | 80 | ------------------------------------------------------------------------ */ 81 | 82 | /* --------------------------------------------------------------------- 83 | The following 4 definitions are compiler-specific. 84 | The C standard does not guarantee that wchar_t has at least 85 | 16 bits, so wchar_t is no less portable than unsigned short! 86 | All should be unsigned values to avoid sign extension during 87 | bit mask & shift operations. 88 | ------------------------------------------------------------------------ */ 89 | 90 | typedef unsigned int UTF32; /* at least 32 bits */ 91 | typedef unsigned short UTF16; /* at least 16 bits */ 92 | typedef unsigned char UTF8; /* typically 8 bits */ 93 | typedef unsigned char Boolean; /* 0 or 1 */ 94 | 95 | /* Some fundamental constants */ 96 | #define UNI_REPLACEMENT_CHAR (UTF32)0x0000FFFD 97 | #define UNI_MAX_BMP (UTF32)0x0000FFFF 98 | #define UNI_MAX_UTF16 (UTF32)0x0010FFFF 99 | #define UNI_MAX_UTF32 (UTF32)0x7FFFFFFF 100 | #define UNI_MAX_LEGAL_UTF32 (UTF32)0x0010FFFF 101 | 102 | typedef enum { 103 | conversionOK, /* conversion successful */ 104 | sourceExhausted, /* partial character in source, but hit end */ 105 | targetExhausted, /* insuff. room in target for conversion */ 106 | sourceIllegal /* source sequence is illegal/malformed */ 107 | } ConversionResult; 108 | 109 | typedef enum { 110 | strictConversion = 0, 111 | lenientConversion 112 | } ConversionFlags; 113 | 114 | /* This is for C++ and does no harm in C */ 115 | #ifdef __cplusplus 116 | extern "C" { 117 | #endif 118 | 119 | ConversionResult ConvertUTF8toUTF16 ( 120 | const UTF8** sourceStart, const UTF8* sourceEnd, 121 | UTF16** targetStart, UTF16* targetEnd, ConversionFlags flags); 122 | 123 | ConversionResult ConvertUTF16toUTF8 ( 124 | const UTF16** sourceStart, const UTF16* sourceEnd, 125 | UTF8** targetStart, UTF8* targetEnd, ConversionFlags flags); 126 | 127 | ConversionResult ConvertUTF8toUTF32 ( 128 | const UTF8** sourceStart, const UTF8* sourceEnd, 129 | UTF32** targetStart, UTF32* targetEnd, ConversionFlags flags); 130 | 131 | ConversionResult ConvertUTF32toUTF8 ( 132 | const UTF32** sourceStart, const UTF32* sourceEnd, 133 | UTF8** targetStart, UTF8* targetEnd, ConversionFlags flags); 134 | 135 | ConversionResult ConvertUTF16toUTF32 ( 136 | const UTF16** sourceStart, const UTF16* sourceEnd, 137 | UTF32** targetStart, UTF32* targetEnd, ConversionFlags flags); 138 | 139 | ConversionResult ConvertUTF32toUTF16 ( 140 | const UTF32** sourceStart, const UTF32* sourceEnd, 141 | UTF16** targetStart, UTF16* targetEnd, ConversionFlags flags); 142 | 143 | Boolean isLegalUTF8Sequence(const UTF8 *source, const UTF8 *sourceEnd); 144 | 145 | #ifdef __cplusplus 146 | } 147 | #endif 148 | 149 | /* --------------------------------------------------------------------- */ 150 | -------------------------------------------------------------------------------- /src/bsw/system/ini_parser/ini_parser.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #define SI_SUPPORT_IOSTREAMS 3 | #include 4 | 5 | #include // std::stringstream 6 | #include // std::setprecision 7 | 8 | #include 9 | #include 10 | #include 11 | #include 12 | 13 | IniParser::IniParser() 14 | { 15 | 16 | } 17 | 18 | IniParser::~IniParser() 19 | { 20 | 21 | } 22 | 23 | bool IniParser::Init(std::string strFile) 24 | { 25 | m_strCfgFile = strFile; 26 | struct stat attr; 27 | if( stat( strFile.c_str(), &attr) == -1 ) return false; 28 | m_fileInitTime = attr.st_mtime; 29 | 30 | return true; 31 | } 32 | 33 | bool IniParser::IsFileUpdated(){ 34 | struct stat attr; 35 | if( stat( m_strCfgFile.c_str(), &attr) == -1 ) return false; 36 | if( (attr.st_mtime == m_fileUpdateTime) && (m_fileUpdateTime != -1) ) return false; 37 | m_fileUpdateTime = attr.st_mtime; 38 | return true; 39 | } 40 | 41 | bool IniParser::ParseConfig(std::string strAppName, std::string strKeyName, std::string& Output) 42 | { 43 | // ini file load 44 | CSimpleIniA ini; 45 | ini.SetUnicode(); 46 | SI_Error rc = ini.LoadFile(m_strCfgFile.c_str()); 47 | if (rc < 0) return false; 48 | 49 | // Get value 50 | const char * cBuffer = ini.GetValue(strAppName.c_str(), strKeyName.c_str(), ""); 51 | if(cBuffer == "") 52 | return false; 53 | 54 | Output = std::string(cBuffer); 55 | 56 | return true; 57 | } 58 | 59 | bool IniParser::ParseConfig(std::string strAppName, std::string strKeyName, bool& Output) 60 | { 61 | // ini file load 62 | CSimpleIniA ini; 63 | ini.SetUnicode(); 64 | SI_Error rc = ini.LoadFile(m_strCfgFile.c_str()); 65 | if (rc < 0) return false; 66 | 67 | // Get value 68 | const char * cBuffer = ini.GetValue(strAppName.c_str(), strKeyName.c_str(), ""); 69 | if(cBuffer == "") 70 | return false; 71 | 72 | if (atoi(cBuffer) > 0) 73 | Output = true; 74 | else 75 | Output = false; 76 | 77 | return true; 78 | } 79 | 80 | bool IniParser::ParseConfig(std::string strAppName, std::string strKeyName, int& Output) 81 | { 82 | // ini file load 83 | CSimpleIniA ini; 84 | ini.SetUnicode(); 85 | SI_Error rc = ini.LoadFile(m_strCfgFile.c_str()); 86 | if (rc < 0) return false; 87 | 88 | // Get value 89 | const char * cBuffer = ini.GetValue(strAppName.c_str(), strKeyName.c_str(), ""); 90 | if(cBuffer == "") 91 | return false; 92 | 93 | Output = atoi(cBuffer); 94 | 95 | return true; 96 | } 97 | 98 | bool IniParser::ParseConfig(std::string strAppName, std::string strKeyName, unsigned int & Output) 99 | { 100 | // ini file load 101 | CSimpleIniA ini; 102 | ini.SetUnicode(); 103 | SI_Error rc = ini.LoadFile(m_strCfgFile.c_str()); 104 | if (rc < 0) return false; 105 | 106 | // Get value 107 | const char * cBuffer = ini.GetValue(strAppName.c_str(), strKeyName.c_str(), ""); 108 | if(cBuffer == "") 109 | return false; 110 | 111 | Output = (unsigned int)atoi(cBuffer); 112 | 113 | return true; 114 | } 115 | bool IniParser::ParseConfig(std::string strAppName, std::string strKeyName, float& Output) 116 | { 117 | // ini file load 118 | CSimpleIniA ini; 119 | ini.SetUnicode(); 120 | SI_Error rc = ini.LoadFile(m_strCfgFile.c_str()); 121 | if (rc < 0) return false; 122 | 123 | // Get value 124 | const char * cBuffer = ini.GetValue(strAppName.c_str(), strKeyName.c_str(), ""); 125 | if(cBuffer == "") 126 | return false; 127 | 128 | Output = (float)atof(cBuffer); 129 | 130 | return true; 131 | } 132 | bool IniParser::ParseConfig(std::string strAppName, std::string strKeyName, double& Output) 133 | { 134 | // ini file load 135 | CSimpleIniA ini; 136 | ini.SetUnicode(); 137 | SI_Error rc = ini.LoadFile(m_strCfgFile.c_str()); 138 | if (rc < 0) return false; 139 | 140 | // Get value 141 | const char * cBuffer = ini.GetValue(strAppName.c_str(), strKeyName.c_str(), ""); 142 | if(cBuffer == "") 143 | return false; 144 | 145 | Output = atof(cBuffer); 146 | 147 | return true; 148 | } 149 | bool IniParser::ParseConfig(std::string strAppName, std::string strKeyName, std::vector& Output) 150 | { 151 | // ini file load 152 | CSimpleIniA ini; 153 | ini.SetUnicode(); 154 | SI_Error rc = ini.LoadFile(m_strCfgFile.c_str()); 155 | if (rc < 0) return false; 156 | 157 | // Get value 158 | const char * cBuffer = ini.GetValue(strAppName.c_str(), strKeyName.c_str(), ""); 159 | if(cBuffer == "") 160 | return false; 161 | 162 | if (ExtractArrayValueFromString(cBuffer, Output)==false) 163 | return false; 164 | 165 | return true; 166 | } 167 | 168 | bool IniParser::ParseConfig(std::string strAppName, std::string strKeyName, std::vector>& Output) 169 | { 170 | // ini file load 171 | CSimpleIniA ini; 172 | ini.SetUnicode(); 173 | SI_Error rc = ini.LoadFile(m_strCfgFile.c_str()); 174 | if (rc < 0) return false; 175 | 176 | Output.clear(); 177 | 178 | int nID = 0; 179 | for (;;) 180 | { 181 | // Get value string 182 | const char * cBuffer = ini.GetValue(strAppName.c_str(), (strKeyName + "_" + std::to_string(nID)).c_str(), ""); 183 | if(cBuffer == "") 184 | break; 185 | 186 | // Converting 187 | std::vector vecArray; 188 | if (!ExtractArrayValueFromString(cBuffer, vecArray)) { 189 | break; 190 | } 191 | 192 | // Push to calibration vector 193 | Output.push_back(vecArray); 194 | 195 | // ID increasing 196 | nID++; 197 | } 198 | 199 | if (nID > 0) { 200 | return true; 201 | } 202 | else { 203 | return false; 204 | } 205 | } 206 | 207 | bool IniParser::ExtractArrayValueFromString(std::string strArray, std::vector& vecValues) 208 | { 209 | vecValues.clear(); 210 | std::stringstream ssArray(strArray); 211 | 212 | std::string strTmpElement; 213 | while (ssArray >> strTmpElement) 214 | { 215 | 216 | if (ssArray.peek() == ',') { 217 | ssArray.ignore(); 218 | } 219 | 220 | double dTmpElement = 0.0; 221 | 222 | if (strTmpElement == "inf") //you could also add it with negative infinity 223 | { 224 | dTmpElement = std::numeric_limits::infinity(); 225 | } 226 | else if (strTmpElement == "-inf") //you could also add it with negative infinity 227 | { 228 | dTmpElement = -std::numeric_limits::infinity(); 229 | } 230 | else 231 | { 232 | dTmpElement = std::stod(strTmpElement); 233 | } 234 | 235 | vecValues.push_back(dTmpElement); 236 | } 237 | return true; 238 | } 239 | 240 | 241 | 242 | IniExporter::IniExporter() 243 | { 244 | } 245 | 246 | IniExporter::~IniExporter() 247 | { 248 | } 249 | 250 | bool IniExporter::Init(std::string strFile) 251 | { 252 | m_strCfgFile = strFile; 253 | 254 | return true; 255 | } 256 | 257 | bool IniExporter::ExportConfig(std::string strAppName, std::string strKeyName, std::string Val) 258 | { 259 | // ini file load 260 | CSimpleIniA ini; 261 | ini.SetUnicode(); 262 | ini.LoadFile(m_strCfgFile.c_str()); 263 | 264 | // Set value 265 | ini.SetValue(strAppName.c_str(), strKeyName.c_str(), Val.c_str()); 266 | 267 | // Save file 268 | SI_Error rc = ini.SaveFile(m_strCfgFile.c_str()); 269 | if (rc < 0) return false; 270 | 271 | return true; 272 | } 273 | 274 | bool IniExporter::ExportConfig(std::string strAppName, std::string strKeyName, bool Val) 275 | { 276 | // ini file load 277 | CSimpleIniA ini; 278 | ini.SetUnicode(); 279 | ini.LoadFile(m_strCfgFile.c_str()); 280 | 281 | // Set value 282 | std::string strTmp = std::to_string(Val).c_str(); 283 | ini.SetValue(strAppName.c_str(), strKeyName.c_str(), strTmp.c_str()); 284 | 285 | // Save file 286 | SI_Error rc = ini.SaveFile(m_strCfgFile.c_str()); 287 | if (rc < 0) return false; 288 | return true; 289 | } 290 | 291 | bool IniExporter::ExportConfig(std::string strAppName, std::string strKeyName, int Val) 292 | { 293 | // ini file load 294 | CSimpleIniA ini; 295 | ini.SetUnicode(); 296 | ini.LoadFile(m_strCfgFile.c_str()); 297 | 298 | // Set value 299 | ini.SetValue(strAppName.c_str(), strKeyName.c_str(), std::to_string(Val).c_str()); 300 | 301 | // Save file 302 | SI_Error rc = ini.SaveFile(m_strCfgFile.c_str()); 303 | if (rc < 0) return false; 304 | return true; 305 | } 306 | 307 | bool IniExporter::ExportConfig(std::string strAppName, std::string strKeyName, unsigned int Val) 308 | { 309 | // ini file load 310 | CSimpleIniA ini; 311 | ini.SetUnicode(); 312 | ini.LoadFile(m_strCfgFile.c_str()); 313 | 314 | // Set value 315 | ini.SetValue(strAppName.c_str(), strKeyName.c_str(), std::to_string(Val).c_str()); 316 | 317 | // Save file 318 | SI_Error rc = ini.SaveFile(m_strCfgFile.c_str()); 319 | if (rc < 0) return false; 320 | 321 | return true; 322 | } 323 | 324 | bool IniExporter::ExportConfig(std::string strAppName, std::string strKeyName, float Val) 325 | { 326 | // ini file load 327 | CSimpleIniA ini; 328 | ini.SetUnicode(); 329 | ini.LoadFile(m_strCfgFile.c_str()); 330 | 331 | // Set value 332 | ini.SetValue(strAppName.c_str(), strKeyName.c_str(), std::to_string(Val).c_str()); 333 | 334 | // Save file 335 | SI_Error rc = ini.SaveFile(m_strCfgFile.c_str()); 336 | if (rc < 0) return false; 337 | 338 | return true; 339 | } 340 | 341 | bool IniExporter::ExportConfig(std::string strAppName, std::string strKeyName, double Val, unsigned int nPrecision) 342 | { 343 | // ini file load 344 | CSimpleIniA ini; 345 | ini.SetUnicode(); 346 | ini.LoadFile(m_strCfgFile.c_str()); 347 | 348 | // Set value 349 | ini.SetValue(strAppName.c_str(), strKeyName.c_str(), getDoubleToString(Val, nPrecision).c_str()); 350 | 351 | // Save file 352 | SI_Error rc = ini.SaveFile(m_strCfgFile.c_str()); 353 | if (rc < 0) return false; 354 | 355 | return true; 356 | } 357 | 358 | bool IniExporter::ExportConfig(std::string strAppName, std::string strKeyName, std::vector Val) 359 | { 360 | std::string strKeyValue = ""; 361 | for (auto itVal = Val.begin(); itVal != Val.end(); itVal++) 362 | { 363 | strKeyValue = strKeyValue + std::to_string(*itVal) + " "; 364 | } 365 | 366 | 367 | // ini file load 368 | CSimpleIniA ini; 369 | ini.SetUnicode(); 370 | ini.LoadFile(m_strCfgFile.c_str()); 371 | 372 | // Set value 373 | ini.SetValue(strAppName.c_str(), strKeyName.c_str(), strKeyValue.c_str()); 374 | 375 | // Save file 376 | SI_Error rc = ini.SaveFile(m_strCfgFile.c_str()); 377 | if (rc < 0) return false; 378 | 379 | return true; 380 | } 381 | 382 | bool IniExporter::ExportConfig(std::string strAppName, std::string strKeyName, std::vector> Val) 383 | { 384 | bool bIsCfgComplete = true; 385 | 386 | int nID = 0; 387 | for (auto itVal = Val.begin(); itVal != Val.end(); itVal++) 388 | { 389 | std::string strKeyNameID = strKeyName + "_" + std::to_string(nID); 390 | std::string strKeyValueID = ""; 391 | for (auto itVVal = itVal->begin(); itVVal != itVal->end(); itVVal++) 392 | { 393 | strKeyValueID = strKeyValueID + std::to_string(*itVVal) + " "; 394 | } 395 | 396 | // ini file load 397 | CSimpleIniA ini; 398 | ini.SetUnicode(); 399 | ini.LoadFile(m_strCfgFile.c_str()); 400 | 401 | // Set value 402 | ini.SetValue(strAppName.c_str(), strKeyNameID.c_str(), strKeyValueID.c_str()); 403 | 404 | // Save file 405 | SI_Error rc = ini.SaveFile(m_strCfgFile.c_str()); 406 | if (rc < 0) bIsCfgComplete = false; 407 | 408 | 409 | nID++; // ID increase 410 | } 411 | 412 | return bIsCfgComplete; 413 | 414 | } 415 | 416 | std::string IniExporter::getDoubleToString(double value, unsigned int nPrecision) 417 | { 418 | std::stringstream out; 419 | out << std::setprecision(nPrecision) << value; 420 | return out.str(); 421 | } 422 | -------------------------------------------------------------------------------- /src/bsw/system/ini_parser/ini_parser.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | 7 | class IniParser 8 | { 9 | public: 10 | IniParser( ); 11 | ~IniParser( ); 12 | 13 | bool Init( std::string strFile ); 14 | bool IsFileUpdated(); 15 | bool ParseConfig( std::string strAppName , std::string strKeyName , std::string& Output ); 16 | bool ParseConfig( std::string strAppName , std::string strKeyName , bool& Output ); 17 | bool ParseConfig( std::string strAppName , std::string strKeyName , int& Output ); 18 | bool ParseConfig(std::string strAppName, std::string strKeyName, unsigned int& Output); 19 | bool ParseConfig(std::string strAppName, std::string strKeyName, float& Output); 20 | bool ParseConfig( std::string strAppName , std::string strKeyName , double& Output ); 21 | bool ParseConfig(std::string strAppName, std::string strKeyName, std::vector& Output); 22 | bool ParseConfig(std::string strAppName, std::string strKeyName, std::vector>& Output); 23 | 24 | private: 25 | const static int FILE_PATH_BUFF_SIZE = 255; 26 | std::string m_strCfgFile; 27 | time_t m_fileUpdateTime=-1; 28 | time_t m_fileInitTime=-1; 29 | bool ExtractArrayValueFromString(std::string strArray, std::vector& vecValues); 30 | 31 | }; 32 | 33 | class IniExporter 34 | { 35 | public: 36 | IniExporter(); 37 | ~IniExporter(); 38 | 39 | bool Init(std::string strFile); 40 | bool ExportConfig(std::string strAppName, std::string strKeyName, std::string Val); 41 | bool ExportConfig(std::string strAppName, std::string strKeyName, bool Val); 42 | bool ExportConfig(std::string strAppName, std::string strKeyName, int Val); 43 | bool ExportConfig(std::string strAppName, std::string strKeyName, unsigned int Val); 44 | bool ExportConfig(std::string strAppName, std::string strKeyName, float Val); 45 | bool ExportConfig(std::string strAppName, std::string strKeyName, double Val, unsigned int nPrecision = 18); 46 | bool ExportConfig(std::string strAppName, std::string strKeyName, std::vector Val); 47 | bool ExportConfig(std::string strAppName, std::string strKeyName, std::vector> Val); 48 | 49 | private: 50 | const static int FILE_PATH_BUFF_SIZE = 255; 51 | std::string m_strCfgFile; 52 | 53 | private: 54 | std::string getDoubleToString(double value, unsigned int nPrecision); 55 | }; 56 | --------------------------------------------------------------------------------