├── CMakeLists.txt ├── LICENSE ├── README.md ├── config ├── m2dgr.yaml ├── m2dgr │ ├── ring32_M_0.xml │ ├── ring32_M_1.xml │ ├── ring32_M_2.xml │ ├── ring32_M_3.xml │ ├── ring32_M_4.xml │ ├── ring32_M_5.xml │ ├── ring32_M_6.xml │ ├── ring32_M_7.xml │ ├── ring32_M_8.xml │ └── ring32_v.xml ├── newer_college.yaml ├── newer_college │ ├── ring64_M_0.xml │ ├── ring64_M_1.xml │ ├── ring64_M_2.xml │ ├── ring64_M_3.xml │ ├── ring64_M_4.xml │ ├── ring64_M_5.xml │ ├── ring64_M_6.xml │ ├── ring64_M_7.xml │ ├── ring64_M_8.xml │ └── ring64_v.xml ├── rviz_cfg │ └── voxel_mapping.rviz ├── viral.yaml └── viral │ ├── ring16_M_0.xml │ ├── ring16_M_1.xml │ ├── ring16_M_2.xml │ ├── ring16_M_3.xml │ ├── ring16_M_4.xml │ ├── ring16_M_5.xml │ ├── ring16_M_6.xml │ ├── ring16_M_7.xml │ ├── ring16_M_8.xml │ └── ring16_v.xml ├── include ├── Exp_mat.h ├── IKFoM_toolkit │ ├── esekfom │ │ ├── esekfom.hpp │ │ └── util.hpp │ └── mtk │ │ ├── build_manifold.hpp │ │ ├── src │ │ ├── SubManifold.hpp │ │ ├── mtkmath.hpp │ │ └── vectview.hpp │ │ ├── startIdx.hpp │ │ └── types │ │ ├── S2.hpp │ │ ├── SOn.hpp │ │ ├── vect.hpp │ │ └── wrapped_cv_mat.hpp ├── common_lib.h ├── matplotlibcpp.h ├── point_type.h ├── so3_math.h ├── use-ikfom.hpp └── voxel_map_util.hpp ├── launch ├── mapping_m2dgr.launch ├── mapping_newer_college.launch ├── mapping_viral.launch └── rviz_voxel_map.launch ├── package.xml └── src ├── IMU_Processing.hpp ├── converter ├── pandar_to_velodyne.cpp └── rs_to_velodyne.cpp ├── preprocess.cpp ├── preprocess.h ├── ring_fals ├── Image_normals.hpp ├── precomp.h ├── range_image.cpp └── range_image.h ├── tic_toc.h ├── visualizer_node.cpp └── voxelMapping.cpp /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(log_lio) 3 | 4 | #SET(CMAKE_BUILD_TYPE "Debug") 5 | #set(CMAKE_CXX_FLAGS "-std=c++11") 6 | set(CMAKE_BUILD_TYPE "Release") 7 | set(CMAKE_CXX_FLAGS_RELEASE "-O3 -g -pthread") 8 | #set(CMAKE_CXX_FLAGS_RELEASE "-O3 -Wall -g -pthread") 9 | 10 | ADD_COMPILE_OPTIONS(-std=c++14 ) 11 | ADD_COMPILE_OPTIONS(-std=c++14 ) 12 | set( CMAKE_CXX_FLAGS "-std=c++14 -O3" ) 13 | 14 | add_definitions(-DROOT_DIR=\"${CMAKE_CURRENT_SOURCE_DIR}/\") 15 | 16 | set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -fexceptions" ) 17 | set(CMAKE_CXX_STANDARD 14) 18 | set(CMAKE_CXX_STANDARD_REQUIRED ON) 19 | set(CMAKE_CXX_EXTENSIONS OFF) 20 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++14 -pthread -std=c++0x -std=c++14 -fexceptions") 21 | 22 | message("Current CPU archtecture: ${CMAKE_SYSTEM_PROCESSOR}") 23 | if(CMAKE_SYSTEM_PROCESSOR MATCHES "(x86)|(X86)|(amd64)|(AMD64)" ) 24 | include(ProcessorCount) 25 | ProcessorCount(N) 26 | message("Processer number: ${N}") 27 | if(N GREATER 14) 28 | add_definitions(-DMP_EN) 29 | add_definitions(-DMP_PROC_NUM=12) 30 | message("core for MP: 14") 31 | elseif(N GREATER 10) 32 | add_definitions(-DMP_EN) 33 | add_definitions(-DMP_PROC_NUM=8) 34 | message("core for MP: 8") 35 | elseif(N GREATER 7) 36 | add_definitions(-DMP_EN) 37 | add_definitions(-DMP_PROC_NUM=6) 38 | message("core for MP: 6") 39 | elseif(N GREATER 3) 40 | add_definitions(-DMP_EN) 41 | add_definitions(-DMP_PROC_NUM=3) 42 | message("core for MP: 3") 43 | else() 44 | add_definitions(-DMP_PROC_NUM=1) 45 | endif() 46 | else() 47 | add_definitions(-DMP_PROC_NUM=1) 48 | endif() 49 | 50 | find_package(Boost REQUIRED COMPONENTS timer) 51 | 52 | find_package(OpenMP QUIET) 53 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS}") 54 | set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${OpenMP_C_FLAGS}") 55 | 56 | find_package(PythonLibs REQUIRED) 57 | find_path(MATPLOTLIB_CPP_INCLUDE_DIRS "matplotlibcpp.h") 58 | 59 | find_package(catkin REQUIRED COMPONENTS 60 | geometry_msgs 61 | nav_msgs 62 | sensor_msgs 63 | roscpp 64 | rospy 65 | std_msgs 66 | pcl_ros 67 | tf 68 | # livox_ros_driver 69 | message_generation 70 | eigen_conversions 71 | ) 72 | 73 | find_package(Eigen3 REQUIRED) 74 | find_package(PCL 1.7 REQUIRED) 75 | #find_package(GTSAM REQUIRED QUIET) 76 | 77 | ## set OpenCV DIR 78 | if ($ENV{ROS_DISTRO} STREQUAL "kinetic") 79 | Set(OpenCV_DIR "/home/hk/opencv-3.3.1/build") # important find opencv version 80 | else() 81 | Set(OpenCV_DIR "/home/autolab/opencv-3.2.0/build") # important find opencv version, Neotic 82 | endif () 83 | find_package(OpenCV 3.2 QUIET) 84 | 85 | 86 | message(Eigen: ${EIGEN3_INCLUDE_DIR}) 87 | 88 | include_directories( 89 | ${catkin_INCLUDE_DIRS} 90 | ${EIGEN3_INCLUDE_DIR} 91 | ${PCL_INCLUDE_DIRS} 92 | ${PYTHON_INCLUDE_DIRS} 93 | ${OpenCV_INCLUDE_DIRS} 94 | include 95 | ) 96 | 97 | #add_message_files( 98 | # FILES 99 | # Pose6D.msg 100 | #) 101 | 102 | generate_messages( 103 | DEPENDENCIES 104 | geometry_msgs 105 | ) 106 | 107 | catkin_package( 108 | CATKIN_DEPENDS geometry_msgs nav_msgs roscpp rospy std_msgs message_runtime 109 | DEPENDS EIGEN3 PCL 110 | INCLUDE_DIRS 111 | ) 112 | 113 | add_executable(log_lio_node src/voxelMapping.cpp src/preprocess.cpp src/ring_fals/range_image.cpp) 114 | target_link_libraries(log_lio_node ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${PYTHON_LIBRARIES} ${OpenCV_LIBRARIES}) 115 | target_include_directories(log_lio_node PRIVATE ${PYTHON_INCLUDE_DIRS}) 116 | 117 | add_executable(pandar_to_velodyne src/converter/pandar_to_velodyne.cpp) 118 | target_link_libraries(pandar_to_velodyne ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${PYTHON_LIBRARIES}) 119 | target_include_directories(pandar_to_velodyne PRIVATE ${PYTHON_INCLUDE_DIRS}) 120 | 121 | add_executable(rs_to_velodyne src/converter/rs_to_velodyne.cpp) 122 | target_link_libraries(rs_to_velodyne ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${PYTHON_LIBRARIES}) 123 | target_include_directories(rs_to_velodyne PRIVATE ${PYTHON_INCLUDE_DIRS}) 124 | 125 | add_executable(visualize_node src/visualizer_node.cpp) 126 | target_link_libraries(visualize_node ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${PYTHON_LIBRARIES}) 127 | target_include_directories(visualize_node PRIVATE ${PYTHON_INCLUDE_DIRS}) 128 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # LOG-LIO2 2 | 3 | Our paper is now available at arxiv https://arxiv.org/abs/2405.01316. 4 | 5 | We first open source the early version of the code． 6 | 7 | For the readme, please temporarily refer to [LOG-LIO](https://github.com/tiev-tongji/LOG-LIO). 8 | 9 | A detailed readme will be released later. 10 | -------------------------------------------------------------------------------- /config/m2dgr.yaml: -------------------------------------------------------------------------------- 1 | common: 2 | lid_topic: "/velodyne_points" 3 | imu_topic: "/handsfree/imu" 4 | time_sync_en: false # ONLY turn on when external time synchronization is really not possible 5 | lidar_time_offset: 0.0 # begin time = lidar timestamp + time offset 6 | 7 | preprocess: 8 | lidar_type: 2 # 1 for Livox serials LiDAR, 2 for Velodyne LiDAR, 3 for ouster LiDAR, 9 | scan_line: 32 10 | scan_rate: 10 # only need to be set for velodyne, unit: Hz, 11 | timestamp_unit: 2 # the unit of time/t field in the PointCloud2 rostopic: 0-second, 1-milisecond, 2-microsecond, 3-nanosecond. 12 | Horizon_SCAN: 1800 # lidar horizontal resolution (Velodyne:1800, Ouster:512,1024,2048) 13 | blind: 2 14 | point_filter_num: 4 15 | 16 | mapping: 17 | down_sample_size: 0.4 18 | max_iteration: 3 19 | voxel_size: 1.6 20 | max_layer: 3 # 4 layer, 0, 1, 2, 3 21 | layer_point_size: [ 5, 5, 5, 5, 5 ] 22 | plannar_threshold: 0.01 23 | max_points_size: 1000 # 24 | max_cov_points_size: 1000 # 25 | 26 | fov_degree: 360 27 | det_range: 100.0 28 | extrinsic_est_en: false # true: enable the online estimation of IMU-LiDAR extrinsic, 29 | extrinsic_T: [ 0.27255, -0.00053, 0.17954 ] 30 | extrinsic_R: [ 1, 0, 0, 31 | 0, 1, 0, 32 | 0, 0, 1 ] 33 | 34 | noise_model: 35 | ranging_cov: 0.04 36 | angle_cov: 0.1 37 | acc_cov: 3.7686306102624571e-02 38 | gyr_cov: 2.3417543020438883e-03 39 | b_acc_cov: 1.1416642385952368e-03 40 | b_gyr_cov: 1.4428407712885209e-05 41 | 42 | 43 | publish: 44 | pub_voxel_map: true 45 | publish_max_voxel_layer: 1 # only publish 0,1,2 layer's plane 46 | path_en: true 47 | scan_publish_en: true # false: close all the point cloud output 48 | dense_publish_en: true # false: low down the points number in a global-frame point clouds scan. 49 | scan_bodyframe_pub_en: true # true: output the point cloud scans in IMU-body-frame 50 | 51 | pcd_save: 52 | pcd_save_en: false 53 | interval: -1 # how many LiDAR frames saved in each pcd file; 54 | # -1 : all frames will be saved in ONE pcd file, may lead to memory crash when having too much frames. 55 | 56 | log: 57 | enable: true 58 | 59 | normal: 60 | project_name: log_lio 61 | compute_table: false 62 | ring_table_dir: "/config/m2dgr" 63 | compute_normal: true 64 | check_normal: true 65 | 66 | cov_scale: 67 | roughness_cov_scale: 0.005 # 0.002 68 | incident_cov_max: 0.005 69 | incident_cov_scale: 0.5 # 1.0 70 | visual_ray_scale: 0.05 # along ray direction 71 | visual_tan_scale: 1.0 # non-ray direction 72 | visual_a_scale: 0.5 # transparent 73 | 74 | cov_incremental: 75 | lambda_cov_threshold: 0.001 76 | normal_cov_threshold: 0.001 # normal cov incremental term 2 magnitude 77 | normal_cov_incre_min: 200 78 | normal_cov_update_interval: 100 79 | num_update_thread: 4 80 | 81 | # prism or RTK in IMU frame 82 | ground_truth: 83 | extrinsic_T: [ 0.16, 0.0, 0.84 ] ## GNSS in IMU frame 84 | # extrinsic_T: [ 0.27255, -0.00053, 0.17954] ## LiDAR in IMU frame 85 | extrinsic_R: [ 1, 0, 0, 86 | 0, 1, 0, 87 | 0, 0, 1 ] 88 | -------------------------------------------------------------------------------- /config/newer_college.yaml: -------------------------------------------------------------------------------- 1 | common: 2 | lid_topic: "/os1_cloud_node/points" 3 | imu_topic: "/os1_cloud_node/imu" 4 | time_sync_en: false # ONLY turn on when external time synchronization is really not possible 5 | 6 | preprocess: 7 | lidar_type: 3 # 1 for Livox serials LiDAR, 2 for Velodyne LiDAR, 3 for ouster LiDAR, 8 | scan_line: 64 9 | scan_rate: 10 # only need to be set for velodyne, unit: Hz, 10 | timestamp_unit: 3 # 0-second, 1-milisecond, 2-microsecond, 3-nanosecond. 11 | Horizon_SCAN: 1024 # lidar horizontal resolution (Velodyne:1800, Ouster:512,1024,2048) 12 | blind: 2 13 | point_filter_num: 4 14 | 15 | mapping: 16 | down_sample_size: 0.4 17 | max_iteration: 3 18 | voxel_size: 3.2 19 | max_layer: 3 # 4 layer, 0, 1, 2, 3 20 | layer_point_size: [ 5, 5, 5, 5, 5 ] 21 | plannar_threshold: 0.01 22 | max_points_size: 2000 # #todo 23 | max_cov_points_size: 2000 # #todo 24 | 25 | fov_degree: 360 26 | det_range: 120.0 27 | extrinsic_est_en: false # true: enable the online estimation of IMU-LiDAR extrinsic, 28 | extrinsic_T: [-0.006253, 0.011775, -0.028535] # imu <-- lidar 29 | # extrinsic_q_en: true 30 | # extrinsic_q: [0.0, 0.0, -1.0, 0.0] #(qx, qy, qz, qw) 31 | extrinsic_R: [ -1, 0, 0, 32 | 0, -1, 0, 33 | 0, 0, 1 ] 34 | 35 | noise_model: 36 | ranging_cov: 0.04 37 | angle_cov: 0.1 38 | acc_cov: 0.1 39 | gyr_cov: 0.1 40 | b_acc_cov: 0.000106 41 | b_gyr_cov: 4e-06 42 | 43 | log: 44 | enable: true 45 | 46 | normal: 47 | project_name: log_lio 48 | compute_table: false 49 | ring_table_dir: "/config/newer_college" 50 | compute_normal: true 51 | # check_normal: false 52 | 53 | cov_scale: 54 | roughness_cov_scale: 0.01 # 0.002 55 | incident_cov_max: 0.01 56 | incident_cov_scale: 0.8 # 1.0 57 | visual_ray_scale: 0.05 # along ray direction 58 | visual_tan_scale: 1.0 # non-ray direction 59 | visual_a_scale: 0.5 # transparent 60 | 61 | cov_incremental: 62 | lambda_cov_threshold: 0.0001 63 | normal_cov_threshold: 0.00001 # normal cov incremental term 2 magnitude 64 | normal_cov_incre_min: 400000 65 | normal_cov_update_interval: 100 66 | num_update_thread: 4 67 | 68 | publish: 69 | pub_voxel_map: false 70 | publish_max_voxel_layer: 1 # only publish 0,1,2 layer's plane 71 | path_en: true 72 | scan_publish_en: true # false: close all the point cloud output 73 | dense_publish_en: true # false: low down the points number in a global-frame point clouds scan. 74 | scan_bodyframe_pub_en: true # true: output the point cloud scans in IMU-body-frame 75 | 76 | pcd_save: 77 | pcd_save_en: false 78 | interval: -1 # how many LiDAR frames saved in each pcd file; 79 | # -1 : all frames will be saved in ONE pcd file, may lead to memory crash when having too much frames. 80 | 81 | 82 | # Cam0 in IMU frame 83 | ground_truth: 84 | extrinsic_T: [ 0.04249269, 0.06466842, -0.01845775 ] 85 | extrinsic_R: [ 0.70992163, 0.02460003, 0.70385092, 86 | -0.70414167, 0.00493623, 0.71004236, 87 | 0.01399269, -0.99968519, 0.02082624 ] 88 | -------------------------------------------------------------------------------- /config/viral.yaml: -------------------------------------------------------------------------------- 1 | common: 2 | lid_topic: "/os1_cloud_node1/points" 3 | imu_topic: "/imu/imu" 4 | time_sync_en: false # ONLY turn on when external time synchronization is really not possible 5 | lidar_time_offset: -0.10 # begin time = lidar timestamp + time offset 6 | 7 | preprocess: 8 | lidar_type: 3 # 1 for Livox serials LiDAR, 2 for Velodyne LiDAR, 3 for ouster LiDAR, 9 | scan_line: 16 10 | scan_rate: 10 # only need to be set for velodyne, unit: Hz, 11 | timestamp_unit: 3 # 0-second, 1-milisecond, 2-microsecond, 3-nanosecond. 12 | Horizon_SCAN: 1024 # lidar horizontal resolution (Velodyne:1800, Ouster:512,1024,2048) 13 | blind: 2 14 | point_filter_num: 4 15 | 16 | mapping: 17 | down_sample_size: 0.5 18 | max_iteration: 3 19 | voxel_size: 2.0 20 | max_layer: 3 # 4 layer, 0, 1, 2, 3 21 | layer_point_size: [ 5, 5, 5, 5, 5 ] 22 | plannar_threshold: 0.01 23 | max_points_size: 1000 # #todo 24 | max_cov_points_size: 1000 # #todo 25 | 26 | fov_degree: 360 27 | det_range: 100.0 28 | extrinsic_est_en: false # true: enable the online estimation of IMU-LiDAR extrinsic, 29 | extrinsic_T: [ -0.05, 0.0, 0.055 ] # imu <-- lidar 30 | extrinsic_R: [ 1, 0, 0, 31 | 0, 1, 0, 32 | 0, 0, 1 ] 33 | 34 | noise_model: 35 | ranging_cov: 0.04 36 | angle_cov: 0.1 37 | acc_cov: 6.0e-2 38 | gyr_cov: 5.0e-3 39 | b_acc_cov: 8.0e-5 40 | b_gyr_cov: 3.0e-6 41 | 42 | publish: 43 | pub_voxel_map: false 44 | publish_max_voxel_layer: 1 # only publish 0,1,2 layer's plane 45 | path_en: true 46 | scan_publish_en: true # false: close all the point cloud output 47 | dense_publish_en: true # false: low down the points number in a global-frame point clouds scan. 48 | scan_bodyframe_pub_en: true # true: output the point cloud scans in IMU-body-frame 49 | 50 | pcd_save: 51 | pcd_save_en: false 52 | interval: -1 # how many LiDAR frames saved in each pcd file; 53 | # -1 : all frames will be saved in ONE pcd file, may lead to memory crash when having too much frames. 54 | 55 | log: 56 | enable: true 57 | 58 | normal: 59 | project_name: log_lio 60 | compute_table: false 61 | ring_table_dir: "/config/viral" 62 | compute_normal: true 63 | check_normal: false 64 | 65 | cov_scale: 66 | roughness_cov_scale: 0.005 # 0.002 67 | incident_cov_max: 0.005 68 | incident_cov_scale: 0.5 # 1.0 69 | visual_ray_scale: 300.0 # along ray direction 70 | visual_tan_scale: 300.0 # non-ray direction 71 | visual_a_scale: 0.3 # transparent 72 | 73 | cov_incremental: 74 | lambda_cov_threshold: 0.001 75 | normal_cov_threshold: 0.001 # normal cov incremental term 2 magnitude 76 | normal_cov_incre_min: 200 77 | normal_cov_update_interval: 100 78 | num_update_thread: 4 79 | 80 | # prism or RTK in IMU frame 81 | ground_truth: 82 | extrinsic_T: [ -0.293656, -0.012288, -0.273095 ] 83 | extrinsic_R: [ 1, 0, 0, 84 | 0, 1, 0, 85 | 0, 0, 1 ] 86 | -------------------------------------------------------------------------------- /include/Exp_mat.h: -------------------------------------------------------------------------------- 1 | #ifndef EXP_MAT_H 2 | #define EXP_MAT_H 3 | 4 | #include 5 | #include 6 | #include 7 | // #include 8 | 9 | #define SKEW_SYM_MATRX(v) 0.0,-v[2],v[1],v[2],0.0,-v[0],-v[1],v[0],0.0 10 | 11 | template 12 | Eigen::Matrix Exp(const Eigen::Matrix &&ang) 13 | { 14 | T ang_norm = ang.norm(); 15 | Eigen::Matrix Eye3 = Eigen::Matrix::Identity(); 16 | if (ang_norm > 0.0000001) 17 | { 18 | Eigen::Matrix r_axis = ang / ang_norm; 19 | Eigen::Matrix K; 20 | K << SKEW_SYM_MATRX(r_axis); 21 | /// Roderigous Tranformation 22 | return Eye3 + std::sin(ang_norm) * K + (1.0 - std::cos(ang_norm)) * K * K; 23 | } 24 | else 25 | { 26 | return Eye3; 27 | } 28 | } 29 | 30 | template 31 | Eigen::Matrix Exp(const Eigen::Matrix &ang_vel, const Ts &dt) 32 | { 33 | T ang_vel_norm = ang_vel.norm(); 34 | Eigen::Matrix Eye3 = Eigen::Matrix::Identity(); 35 | 36 | if (ang_vel_norm > 0.0000001) 37 | { 38 | Eigen::Matrix r_axis = ang_vel / ang_vel_norm; 39 | Eigen::Matrix K; 40 | 41 | K << SKEW_SYM_MATRX(r_axis); 42 | 43 | T r_ang = ang_vel_norm * dt; 44 | 45 | /// Roderigous Tranformation 46 | return Eye3 + std::sin(r_ang) * K + (1.0 - std::cos(r_ang)) * K * K; 47 | } 48 | else 49 | { 50 | return Eye3; 51 | } 52 | } 53 | 54 | template 55 | Eigen::Matrix Exp(const T &v1, const T &v2, const T &v3) 56 | { 57 | T &&norm = sqrt(v1 * v1 + v2 * v2 + v3 * v3); 58 | Eigen::Matrix Eye3 = Eigen::Matrix::Identity(); 59 | if (norm > 0.00001) 60 | { 61 | T r_ang[3] = {v1 / norm, v2 / norm, v3 / norm}; 62 | Eigen::Matrix K; 63 | K << SKEW_SYM_MATRX(r_ang); 64 | 65 | /// Roderigous Tranformation 66 | return Eye3 + std::sin(norm) * K + (1.0 - std::cos(norm)) * K * K; 67 | } 68 | else 69 | { 70 | return Eye3; 71 | } 72 | } 73 | 74 | /* Logrithm of a Rotation Matrix */ 75 | template 76 | Eigen::Matrix Log(const Eigen::Matrix &R) 77 | { 78 | T &&theta = std::acos(0.5 * (R.trace() - 1)); 79 | Eigen::Matrix K(R(2,1) - R(1,2), R(0,2) - R(2,0), R(1,0) - R(0,1)); 80 | return (std::abs(theta) < 0.001) ? (0.5 * K) : (0.5 * theta / std::sin(theta) * K); 81 | } 82 | 83 | // template 84 | // cv::Mat Exp(const T &v1, const T &v2, const T &v3) 85 | // { 86 | 87 | // T norm = sqrt(v1 * v1 + v2 * v2 + v3 * v3); 88 | // cv::Mat Eye3 = cv::Mat::eye(3, 3, CV_32F); 89 | // if (norm > 0.0000001) 90 | // { 91 | // T r_ang[3] = {v1 / norm, v2 / norm, v3 / norm}; 92 | // cv::Mat K = (cv::Mat_(3,3) << SKEW_SYM_MATRX(r_ang)); 93 | 94 | // /// Roderigous Tranformation 95 | // return Eye3 + std::sin(norm) * K + (1.0 - std::cos(norm)) * K * K; 96 | // } 97 | // else 98 | // { 99 | // return Eye3; 100 | // } 101 | // } 102 | 103 | #endif 104 | -------------------------------------------------------------------------------- /include/IKFoM_toolkit/esekfom/util.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2019--2023, The University of Hong Kong 3 | * All rights reserved. 4 | * 5 | * Author: Dongjiao HE 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of the Universitaet Bremen nor the names of its 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | */ 34 | 35 | #ifndef __MEKFOM_UTIL_HPP__ 36 | #define __MEKFOM_UTIL_HPP__ 37 | 38 | #include 39 | #include "../mtk/src/mtkmath.hpp" 40 | namespace esekfom { 41 | 42 | template 43 | class is_same { 44 | public: 45 | operator bool() { 46 | return false; 47 | } 48 | }; 49 | template 50 | class is_same { 51 | public: 52 | operator bool() { 53 | return true; 54 | } 55 | }; 56 | 57 | template 58 | class is_double { 59 | public: 60 | operator bool() { 61 | return false; 62 | } 63 | }; 64 | 65 | template<> 66 | class is_double { 67 | public: 68 | operator bool() { 69 | return true; 70 | } 71 | }; 72 | 73 | template 74 | static T 75 | id(const T &x) 76 | { 77 | return x; 78 | } 79 | 80 | } // namespace esekfom 81 | 82 | #endif // __MEKFOM_UTIL_HPP__ 83 | -------------------------------------------------------------------------------- /include/IKFoM_toolkit/mtk/build_manifold.hpp: -------------------------------------------------------------------------------- 1 | // This is an advanced implementation of the algorithm described in the 2 | // following paper: 3 | // C. Hertzberg, R. Wagner, U. Frese, and L. Schroder. Integratinggeneric sensor fusion algorithms with sound state representationsthrough encapsulation of manifolds. 4 | // CoRR, vol. abs/1107.1119, 2011.[Online]. Available: http://arxiv.org/abs/1107.1119 5 | 6 | /* 7 | * Copyright (c) 2019--2023, The University of Hong Kong 8 | * All rights reserved. 9 | * 10 | * Modifier: Dongjiao HE 11 | * 12 | * Redistribution and use in source and binary forms, with or without 13 | * modification, are permitted provided that the following conditions 14 | * are met: 15 | * 16 | * * Redistributions of source code must retain the above copyright 17 | * notice, this list of conditions and the following disclaimer. 18 | * * Redistributions in binary form must reproduce the above 19 | * copyright notice, this list of conditions and the following 20 | * disclaimer in the documentation and/or other materials provided 21 | * with the distribution. 22 | * * Neither the name of the Universitaet Bremen nor the names of its 23 | * contributors may be used to endorse or promote products derived 24 | * from this software without specific prior written permission. 25 | * 26 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 27 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 28 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 29 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 30 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 31 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 32 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 33 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 34 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 35 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 36 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 37 | * POSSIBILITY OF SUCH DAMAGE. 38 | */ 39 | 40 | /* 41 | * Copyright (c) 2008--2011, Universitaet Bremen 42 | * All rights reserved. 43 | * 44 | * Author: Christoph Hertzberg 45 | * 46 | * Redistribution and use in source and binary forms, with or without 47 | * modification, are permitted provided that the following conditions 48 | * are met: 49 | * 50 | * * Redistributions of source code must retain the above copyright 51 | * notice, this list of conditions and the following disclaimer. 52 | * * Redistributions in binary form must reproduce the above 53 | * copyright notice, this list of conditions and the following 54 | * disclaimer in the documentation and/or other materials provided 55 | * with the distribution. 56 | * * Neither the name of the Universitaet Bremen nor the names of its 57 | * contributors may be used to endorse or promote products derived 58 | * from this software without specific prior written permission. 59 | * 60 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 61 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 62 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 63 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 64 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 65 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 66 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 67 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 68 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 69 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 70 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 71 | * POSSIBILITY OF SUCH DAMAGE. 72 | */ 73 | /** 74 | * @file mtk/build_manifold.hpp 75 | * @brief Macro to automatically construct compound manifolds. 76 | * 77 | */ 78 | #ifndef MTK_AUTOCONSTRUCT_HPP_ 79 | #define MTK_AUTOCONSTRUCT_HPP_ 80 | 81 | #include 82 | 83 | #include 84 | #include 85 | #include 86 | 87 | #include "src/SubManifold.hpp" 88 | #include "startIdx.hpp" 89 | 90 | #ifndef PARSED_BY_DOXYGEN 91 | //////// internals ////// 92 | 93 | #define MTK_APPLY_MACRO_ON_TUPLE(r, macro, tuple) macro tuple 94 | 95 | #define MTK_TRANSFORM_COMMA(macro, entries) BOOST_PP_SEQ_ENUM(BOOST_PP_SEQ_TRANSFORM_S(1, MTK_APPLY_MACRO_ON_TUPLE, macro, entries)) 96 | 97 | #define MTK_TRANSFORM(macro, entries) BOOST_PP_SEQ_FOR_EACH_R(1, MTK_APPLY_MACRO_ON_TUPLE, macro, entries) 98 | 99 | #define MTK_CONSTRUCTOR_ARG( type, id) const type& id = type() 100 | #define MTK_CONSTRUCTOR_COPY( type, id) id(id) 101 | #define MTK_BOXPLUS( type, id) id.boxplus(MTK::subvector(__vec, &self::id), __scale); 102 | #define MTK_OPLUS( type, id) id.oplus(MTK::subvector_(__vec, &self::id), __scale); 103 | #define MTK_BOXMINUS( type, id) id.boxminus(MTK::subvector(__res, &self::id), __oth.id); 104 | #define MTK_S2_hat( type, id) if(id.IDX == idx){id.S2_hat(res);} 105 | #define MTK_S2_Nx_yy( type, id) if(id.IDX == idx){id.S2_Nx_yy(res);} 106 | #define MTK_S2_Mx( type, id) if(id.IDX == idx){id.S2_Mx(res, dx);} 107 | #define MTK_OSTREAM( type, id) << __var.id << " " 108 | #define MTK_ISTREAM( type, id) >> __var.id 109 | #define MTK_S2_state( type, id) if(id.TYP == 1){S2_state.push_back(std::make_pair(id.IDX, id.DIM));} 110 | #define MTK_SO3_state( type, id) if(id.TYP == 2){(SO3_state).push_back(std::make_pair(id.IDX, id.DIM));} 111 | #define MTK_vect_state( type, id) if(id.TYP == 0){(vect_state).push_back(std::make_pair(std::make_pair(id.IDX, id.DIM), type::DOF));} 112 | 113 | #define MTK_SUBVARLIST(seq, S2state, SO3state) \ 114 | BOOST_PP_FOR_1( \ 115 | ( \ 116 | BOOST_PP_SEQ_SIZE(seq), \ 117 | BOOST_PP_SEQ_HEAD(seq), \ 118 | BOOST_PP_SEQ_TAIL(seq) (~), \ 119 | 0,\ 120 | 0,\ 121 | S2state,\ 122 | SO3state ),\ 123 | MTK_ENTRIES_TEST, MTK_ENTRIES_NEXT, MTK_ENTRIES_OUTPUT) 124 | 125 | #define MTK_PUT_TYPE(type, id, dof, dim, S2state, SO3state) \ 126 | MTK::SubManifold id; 127 | #define MTK_PUT_TYPE_AND_ENUM(type, id, dof, dim, S2state, SO3state) \ 128 | MTK_PUT_TYPE(type, id, dof, dim, S2state, SO3state) \ 129 | enum {DOF = type::DOF + dof}; \ 130 | enum {DIM = type::DIM+dim}; \ 131 | typedef type::scalar scalar; 132 | 133 | #define MTK_ENTRIES_OUTPUT(r, state) MTK_ENTRIES_OUTPUT_I state 134 | #define MTK_ENTRIES_OUTPUT_I(s, head, seq, dof, dim, S2state, SO3state) \ 135 | MTK_APPLY_MACRO_ON_TUPLE(~, \ 136 | BOOST_PP_IF(BOOST_PP_DEC(s), MTK_PUT_TYPE, MTK_PUT_TYPE_AND_ENUM), \ 137 | ( BOOST_PP_TUPLE_REM_2 head, dof, dim, S2state, SO3state)) 138 | 139 | #define MTK_ENTRIES_TEST(r, state) MTK_TUPLE_ELEM_4_0 state 140 | 141 | //! this used to be BOOST_PP_TUPLE_ELEM_4_0: 142 | #define MTK_TUPLE_ELEM_4_0(a,b,c,d,e,f, g) a 143 | 144 | #define MTK_ENTRIES_NEXT(r, state) MTK_ENTRIES_NEXT_I state 145 | #define MTK_ENTRIES_NEXT_I(len, head, seq, dof, dim, S2state, SO3state) ( \ 146 | BOOST_PP_DEC(len), \ 147 | BOOST_PP_SEQ_HEAD(seq), \ 148 | BOOST_PP_SEQ_TAIL(seq), \ 149 | dof + BOOST_PP_TUPLE_ELEM_2_0 head::DOF,\ 150 | dim + BOOST_PP_TUPLE_ELEM_2_0 head::DIM,\ 151 | S2state,\ 152 | SO3state) 153 | 154 | #endif /* not PARSED_BY_DOXYGEN */ 155 | 156 | 157 | /** 158 | * Construct a manifold. 159 | * @param name is the class-name of the manifold, 160 | * @param entries is the list of sub manifolds 161 | * 162 | * Entries must be given in a list like this: 163 | * @code 164 | * typedef MTK::trafo > Pose; 165 | * typedef MTK::vect Vec3; 166 | * MTK_BUILD_MANIFOLD(imu_state, 167 | * ((Pose, pose)) 168 | * ((Vec3, vel)) 169 | * ((Vec3, acc_bias)) 170 | * ) 171 | * @endcode 172 | * Whitespace is optional, but the double parentheses are necessary. 173 | * Construction is done entirely in preprocessor. 174 | * After construction @a name is also a manifold. Its members can be 175 | * accessed by names given in @a entries. 176 | * 177 | * @note Variable types are not allowed to have commas, thus types like 178 | * @c vect need to be typedef'ed ahead. 179 | */ 180 | #define MTK_BUILD_MANIFOLD(name, entries) \ 181 | struct name { \ 182 | typedef name self; \ 183 | std::vector > S2_state;\ 184 | std::vector > SO3_state;\ 185 | std::vector, int> > vect_state;\ 186 | MTK_SUBVARLIST(entries, S2_state, SO3_state) \ 187 | name ( \ 188 | MTK_TRANSFORM_COMMA(MTK_CONSTRUCTOR_ARG, entries) \ 189 | ) : \ 190 | MTK_TRANSFORM_COMMA(MTK_CONSTRUCTOR_COPY, entries) {}\ 191 | int getDOF() const { return DOF; } \ 192 | void boxplus(const MTK::vectview & __vec, scalar __scale = 1 ) { \ 193 | MTK_TRANSFORM(MTK_BOXPLUS, entries)\ 194 | } \ 195 | void oplus(const MTK::vectview & __vec, scalar __scale = 1 ) { \ 196 | MTK_TRANSFORM(MTK_OPLUS, entries)\ 197 | } \ 198 | void boxminus(MTK::vectview __res, const name& __oth) const { \ 199 | MTK_TRANSFORM(MTK_BOXMINUS, entries)\ 200 | } \ 201 | friend std::ostream& operator<<(std::ostream& __os, const name& __var){ \ 202 | return __os MTK_TRANSFORM(MTK_OSTREAM, entries); \ 203 | } \ 204 | void build_S2_state(){\ 205 | MTK_TRANSFORM(MTK_S2_state, entries)\ 206 | }\ 207 | void build_vect_state(){\ 208 | MTK_TRANSFORM(MTK_vect_state, entries)\ 209 | }\ 210 | void build_SO3_state(){\ 211 | MTK_TRANSFORM(MTK_SO3_state, entries)\ 212 | }\ 213 | void S2_hat(Eigen::Matrix &res, int idx) {\ 214 | MTK_TRANSFORM(MTK_S2_hat, entries)\ 215 | }\ 216 | void S2_Nx_yy(Eigen::Matrix &res, int idx) {\ 217 | MTK_TRANSFORM(MTK_S2_Nx_yy, entries)\ 218 | }\ 219 | void S2_Mx(Eigen::Matrix &res, Eigen::Matrix dx, int idx) {\ 220 | MTK_TRANSFORM(MTK_S2_Mx, entries)\ 221 | }\ 222 | friend std::istream& operator>>(std::istream& __is, name& __var){ \ 223 | return __is MTK_TRANSFORM(MTK_ISTREAM, entries); \ 224 | } \ 225 | }; 226 | 227 | 228 | 229 | #endif /*MTK_AUTOCONSTRUCT_HPP_*/ 230 | -------------------------------------------------------------------------------- /include/IKFoM_toolkit/mtk/src/SubManifold.hpp: -------------------------------------------------------------------------------- 1 | // This is an advanced implementation of the algorithm described in the 2 | // following paper: 3 | // C. Hertzberg, R. Wagner, U. Frese, and L. Schroder. Integratinggeneric sensor fusion algorithms with sound state representationsthrough encapsulation of manifolds. 4 | // CoRR, vol. abs/1107.1119, 2011.[Online]. Available: http://arxiv.org/abs/1107.1119 5 | 6 | /* 7 | * Copyright (c) 2019--2023, The University of Hong Kong 8 | * All rights reserved. 9 | * 10 | * Modifier: Dongjiao HE 11 | * 12 | * Redistribution and use in source and binary forms, with or without 13 | * modification, are permitted provided that the following conditions 14 | * are met: 15 | * 16 | * * Redistributions of source code must retain the above copyright 17 | * notice, this list of conditions and the following disclaimer. 18 | * * Redistributions in binary form must reproduce the above 19 | * copyright notice, this list of conditions and the following 20 | * disclaimer in the documentation and/or other materials provided 21 | * with the distribution. 22 | * * Neither the name of the Universitaet Bremen nor the names of its 23 | * contributors may be used to endorse or promote products derived 24 | * from this software without specific prior written permission. 25 | * 26 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 27 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 28 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 29 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 30 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 31 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 32 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 33 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 34 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 35 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 36 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 37 | * POSSIBILITY OF SUCH DAMAGE. 38 | */ 39 | 40 | /* 41 | * Copyright (c) 2008--2011, Universitaet Bremen 42 | * All rights reserved. 43 | * 44 | * Author: Christoph Hertzberg 45 | * 46 | * Redistribution and use in source and binary forms, with or without 47 | * modification, are permitted provided that the following conditions 48 | * are met: 49 | * 50 | * * Redistributions of source code must retain the above copyright 51 | * notice, this list of conditions and the following disclaimer. 52 | * * Redistributions in binary form must reproduce the above 53 | * copyright notice, this list of conditions and the following 54 | * disclaimer in the documentation and/or other materials provided 55 | * with the distribution. 56 | * * Neither the name of the Universitaet Bremen nor the names of its 57 | * contributors may be used to endorse or promote products derived 58 | * from this software without specific prior written permission. 59 | * 60 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 61 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 62 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 63 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 64 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 65 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 66 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 67 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 68 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 69 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 70 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 71 | * POSSIBILITY OF SUCH DAMAGE. 72 | */ 73 | /** 74 | * @file mtk/src/SubManifold.hpp 75 | * @brief Defines the SubManifold class 76 | */ 77 | 78 | 79 | #ifndef SUBMANIFOLD_HPP_ 80 | #define SUBMANIFOLD_HPP_ 81 | 82 | 83 | #include "vectview.hpp" 84 | 85 | 86 | namespace MTK { 87 | 88 | /** 89 | * @ingroup SubManifolds 90 | * Helper class for compound manifolds. 91 | * This class wraps a manifold T and provides an enum IDX refering to the 92 | * index of the SubManifold within the compound manifold. 93 | * 94 | * Memberpointers to a submanifold can be used for @ref SubManifolds "functions accessing submanifolds". 95 | * 96 | * @tparam T The manifold type of the sub-type 97 | * @tparam idx The index of the sub-type within the compound manifold 98 | */ 99 | template 100 | struct SubManifold : public T 101 | { 102 | enum {IDX = idx, DIM = dim /*!< index of the sub-type within the compound manifold */ }; 103 | //! manifold type 104 | typedef T type; 105 | 106 | //! Construct from derived type 107 | template 108 | explicit 109 | SubManifold(const X& t) : T(t) {}; 110 | 111 | //! Construct from internal type 112 | //explicit 113 | SubManifold(const T& t) : T(t) {}; 114 | 115 | //! inherit assignment operator 116 | using T::operator=; 117 | 118 | }; 119 | 120 | } // namespace MTK 121 | 122 | 123 | #endif /* SUBMANIFOLD_HPP_ */ 124 | -------------------------------------------------------------------------------- /include/IKFoM_toolkit/mtk/src/mtkmath.hpp: -------------------------------------------------------------------------------- 1 | // This is an advanced implementation of the algorithm described in the 2 | // following paper: 3 | // C. Hertzberg, R. Wagner, U. Frese, and L. Schroder. Integratinggeneric sensor fusion algorithms with sound state representationsthrough encapsulation of manifolds. 4 | // CoRR, vol. abs/1107.1119, 2011.[Online]. Available: http://arxiv.org/abs/1107.1119 5 | 6 | /* 7 | * Copyright (c) 2019--2023, The University of Hong Kong 8 | * All rights reserved. 9 | * 10 | * Modifier: Dongjiao HE 11 | * 12 | * Redistribution and use in source and binary forms, with or without 13 | * modification, are permitted provided that the following conditions 14 | * are met: 15 | * 16 | * * Redistributions of source code must retain the above copyright 17 | * notice, this list of conditions and the following disclaimer. 18 | * * Redistributions in binary form must reproduce the above 19 | * copyright notice, this list of conditions and the following 20 | * disclaimer in the documentation and/or other materials provided 21 | * with the distribution. 22 | * * Neither the name of the Universitaet Bremen nor the names of its 23 | * contributors may be used to endorse or promote products derived 24 | * from this software without specific prior written permission. 25 | * 26 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 27 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 28 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 29 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 30 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 31 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 32 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 33 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 34 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 35 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 36 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 37 | * POSSIBILITY OF SUCH DAMAGE. 38 | */ 39 | 40 | /* 41 | * Copyright (c) 2008--2011, Universitaet Bremen 42 | * All rights reserved. 43 | * 44 | * Author: Christoph Hertzberg 45 | * 46 | * Redistribution and use in source and binary forms, with or without 47 | * modification, are permitted provided that the following conditions 48 | * are met: 49 | * 50 | * * Redistributions of source code must retain the above copyright 51 | * notice, this list of conditions and the following disclaimer. 52 | * * Redistributions in binary form must reproduce the above 53 | * copyright notice, this list of conditions and the following 54 | * disclaimer in the documentation and/or other materials provided 55 | * with the distribution. 56 | * * Neither the name of the Universitaet Bremen nor the names of its 57 | * contributors may be used to endorse or promote products derived 58 | * from this software without specific prior written permission. 59 | * 60 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 61 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 62 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 63 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 64 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 65 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 66 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 67 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 68 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 69 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 70 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 71 | * POSSIBILITY OF SUCH DAMAGE. 72 | */ 73 | /** 74 | * @file mtk/src/mtkmath.hpp 75 | * @brief several math utility functions. 76 | */ 77 | 78 | #ifndef MTKMATH_H_ 79 | #define MTKMATH_H_ 80 | 81 | #include 82 | 83 | #include 84 | 85 | #include "../types/vect.hpp" 86 | 87 | #ifndef M_PI 88 | #define M_PI 3.1415926535897932384626433832795 89 | #endif 90 | 91 | 92 | namespace MTK { 93 | 94 | namespace internal { 95 | 96 | template 97 | struct traits { 98 | typedef typename Manifold::scalar scalar; 99 | enum {DOF = Manifold::DOF}; 100 | typedef vect vectorized_type; 101 | typedef Eigen::Matrix matrix_type; 102 | }; 103 | 104 | template<> 105 | struct traits : traits > {}; 106 | template<> 107 | struct traits : traits > {}; 108 | 109 | } // namespace internal 110 | 111 | /** 112 | * \defgroup MTKMath Mathematical helper functions 113 | */ 114 | //@{ 115 | 116 | //! constant @f$ \pi @f$ 117 | const double pi = M_PI; 118 | 119 | template inline scalar tolerance(); 120 | 121 | template<> inline float tolerance() { return 1e-5f; } 122 | template<> inline double tolerance() { return 1e-11; } 123 | 124 | 125 | /** 126 | * normalize @a x to @f$[-bound, bound] @f$. 127 | * 128 | * result for @f$ x = bound + 2\cdot n\cdot bound @f$ is arbitrary @f$\pm bound @f$. 129 | */ 130 | template 131 | inline scalar normalize(scalar x, scalar bound){ //not used 132 | if(std::fabs(x) <= bound) return x; 133 | int r = (int)(x *(scalar(1.0)/ bound)); 134 | return x - ((r + (r>>31) + 1) & ~1)*bound; 135 | } 136 | 137 | /** 138 | * Calculate cosine and sinc of sqrt(x2). 139 | * @param x2 the squared angle must be non-negative 140 | * @return a pair containing cos and sinc of sqrt(x2) 141 | */ 142 | template 143 | std::pair cos_sinc_sqrt(const scalar &x2){ 144 | using std::sqrt; 145 | using std::cos; 146 | using std::sin; 147 | static scalar const taylor_0_bound = boost::math::tools::epsilon(); 148 | static scalar const taylor_2_bound = sqrt(taylor_0_bound); 149 | static scalar const taylor_n_bound = sqrt(taylor_2_bound); 150 | 151 | assert(x2>=0 && "argument must be non-negative"); 152 | 153 | // FIXME check if bigger bounds are possible 154 | if(x2>=taylor_n_bound) { 155 | // slow fall-back solution 156 | scalar x = sqrt(x2); 157 | return std::make_pair(cos(x), sin(x)/x); // x is greater than 0. 158 | } 159 | 160 | // FIXME Replace by Horner-Scheme (4 instead of 5 FLOP/term, numerically more stable, theoretically cos and sinc can be calculated in parallel using SSE2 mulpd/addpd) 161 | // TODO Find optimal coefficients using Remez algorithm 162 | static scalar const inv[] = {1/3., 1/4., 1/5., 1/6., 1/7., 1/8., 1/9.}; 163 | scalar cosi = 1., sinc=1; 164 | scalar term = -1/2. * x2; 165 | for(int i=0; i<3; ++i) { 166 | cosi += term; 167 | term *= inv[2*i]; 168 | sinc += term; 169 | term *= -inv[2*i+1] * x2; 170 | } 171 | 172 | return std::make_pair(cosi, sinc); 173 | 174 | } 175 | 176 | template 177 | Eigen::Matrix hat(const Base& v) { 178 | Eigen::Matrix res; 179 | res << 0, -v[2], v[1], 180 | v[2], 0, -v[0], 181 | -v[1], v[0], 0; 182 | return res; 183 | } 184 | 185 | template 186 | Eigen::Matrix A_inv_trans(const Base& v){ 187 | Eigen::Matrix res; 188 | if(v.norm() > MTK::tolerance()) 189 | { 190 | res = Eigen::Matrix::Identity() + 0.5 * hat(v) + (1 - v.norm() * std::cos(v.norm() / 2) / 2 / std::sin(v.norm() / 2)) * hat(v) * hat(v) / v.squaredNorm(); 191 | 192 | } 193 | else 194 | { 195 | res = Eigen::Matrix::Identity(); 196 | } 197 | 198 | return res; 199 | } 200 | 201 | template 202 | Eigen::Matrix A_inv(const Base& v){ 203 | Eigen::Matrix res; 204 | if(v.norm() > MTK::tolerance()) 205 | { 206 | res = Eigen::Matrix::Identity() - 0.5 * hat(v) + (1 - v.norm() * std::cos(v.norm() / 2) / 2 / std::sin(v.norm() / 2)) * hat(v) * hat(v) / v.squaredNorm(); 207 | 208 | } 209 | else 210 | { 211 | res = Eigen::Matrix::Identity(); 212 | } 213 | 214 | return res; 215 | } 216 | 217 | template 218 | Eigen::Matrix S2_w_expw_( Eigen::Matrix v, scalar length) 219 | { 220 | Eigen::Matrix res; 221 | scalar norm = std::sqrt(v[0]*v[0] + v[1]*v[1]); 222 | if(norm < MTK::tolerance()){ 223 | res = Eigen::Matrix::Zero(); 224 | res(0, 1) = 1; 225 | res(1, 2) = 1; 226 | res /= length; 227 | } 228 | else{ 229 | res << -v[0]*(1/norm-1/std::tan(norm))/std::sin(norm), norm/std::sin(norm), 0, 230 | -v[1]*(1/norm-1/std::tan(norm))/std::sin(norm), 0, norm/std::sin(norm); 231 | res /= length; 232 | } 233 | } 234 | 235 | template 236 | Eigen::Matrix A_matrix(const Base & v){ 237 | Eigen::Matrix res; 238 | double squaredNorm = v[0] * v[0] + v[1] * v[1] + v[2] * v[2]; 239 | double norm = std::sqrt(squaredNorm); 240 | if(norm < MTK::tolerance()){ 241 | res = Eigen::Matrix::Identity(); 242 | } 243 | else{ 244 | res = Eigen::Matrix::Identity() + (1 - std::cos(norm)) / squaredNorm * hat(v) + (1 - std::sin(norm) / norm) / squaredNorm * hat(v) * hat(v); 245 | } 246 | return res; 247 | } 248 | 249 | template 250 | scalar exp(vectview result, vectview vec, const scalar& scale = 1) { 251 | scalar norm2 = vec.squaredNorm(); 252 | std::pair cos_sinc = cos_sinc_sqrt(scale*scale * norm2); 253 | scalar mult = cos_sinc.second * scale; 254 | result = mult * vec; 255 | return cos_sinc.first; 256 | } 257 | 258 | 259 | /** 260 | * Inverse function to @c exp. 261 | * 262 | * @param result @c vectview to the result 263 | * @param w scalar part of input 264 | * @param vec vector part of input 265 | * @param scale scale result by this value 266 | * @param plus_minus_periodicity if true values @f$[w, vec]@f$ and @f$[-w, -vec]@f$ give the same result 267 | */ 268 | template 269 | void log(vectview result, 270 | const scalar &w, const vectview vec, 271 | const scalar &scale, bool plus_minus_periodicity) 272 | { 273 | // FIXME implement optimized case for vec.squaredNorm() <= tolerance() * (w*w) via Rational Remez approximation ~> only one division 274 | scalar nv = vec.norm(); 275 | if(nv < tolerance()) { 276 | if(!plus_minus_periodicity && w < 0) { 277 | // find the maximal entry: 278 | int i; 279 | nv = vec.cwiseAbs().maxCoeff(&i); 280 | result = scale * std::atan2(nv, w) * vect::Unit(i); 281 | return; 282 | } 283 | nv = tolerance(); 284 | } 285 | scalar s = scale / nv * (plus_minus_periodicity ? std::atan(nv / w) : std::atan2(nv, w) ); 286 | 287 | result = s * vec; 288 | } 289 | 290 | 291 | } // namespace MTK 292 | 293 | 294 | #endif /* MTKMATH_H_ */ 295 | -------------------------------------------------------------------------------- /include/IKFoM_toolkit/mtk/src/vectview.hpp: -------------------------------------------------------------------------------- 1 | 2 | /* 3 | * Copyright (c) 2008--2011, Universitaet Bremen 4 | * All rights reserved. 5 | * 6 | * Author: Christoph Hertzberg 7 | * 8 | * Redistribution and use in source and binary forms, with or without 9 | * modification, are permitted provided that the following conditions 10 | * are met: 11 | * 12 | * * Redistributions of source code must retain the above copyright 13 | * notice, this list of conditions and the following disclaimer. 14 | * * Redistributions in binary form must reproduce the above 15 | * copyright notice, this list of conditions and the following 16 | * disclaimer in the documentation and/or other materials provided 17 | * with the distribution. 18 | * * Neither the name of the Universitaet Bremen nor the names of its 19 | * contributors may be used to endorse or promote products derived 20 | * from this software without specific prior written permission. 21 | * 22 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 23 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 24 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 25 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 26 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 27 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 28 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 29 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 30 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 31 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 32 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 33 | * POSSIBILITY OF SUCH DAMAGE. 34 | */ 35 | /** 36 | * @file mtk/src/vectview.hpp 37 | * @brief Wrapper class around a pointer used as interface for plain vectors. 38 | */ 39 | 40 | #ifndef VECTVIEW_HPP_ 41 | #define VECTVIEW_HPP_ 42 | 43 | #include 44 | 45 | namespace MTK { 46 | 47 | /** 48 | * A view to a vector. 49 | * Essentially, @c vectview is only a pointer to @c scalar but can be used directly in @c Eigen expressions. 50 | * The dimension of the vector is given as template parameter and type-checked when used in expressions. 51 | * Data has to be modifiable. 52 | * 53 | * @tparam scalar Scalar type of the vector. 54 | * @tparam dim Dimension of the vector. 55 | * 56 | * @todo @c vectview can be replaced by simple inheritance of @c Eigen::Map, as soon as they get const-correct 57 | */ 58 | namespace internal { 59 | template 60 | struct CovBlock { 61 | typedef typename Eigen::Block, T1::DOF, T2::DOF> Type; 62 | typedef typename Eigen::Block, T1::DOF, T2::DOF> ConstType; 63 | }; 64 | 65 | template 66 | struct CovBlock_ { 67 | typedef typename Eigen::Block, T1::DIM, T2::DIM> Type; 68 | typedef typename Eigen::Block, T1::DIM, T2::DIM> ConstType; 69 | }; 70 | 71 | template 72 | struct CrossCovBlock { 73 | typedef typename Eigen::Block, T1::DOF, T2::DOF> Type; 74 | typedef typename Eigen::Block, T1::DOF, T2::DOF> ConstType; 75 | }; 76 | 77 | template 78 | struct CrossCovBlock_ { 79 | typedef typename Eigen::Block, T1::DIM, T2::DIM> Type; 80 | typedef typename Eigen::Block, T1::DIM, T2::DIM> ConstType; 81 | }; 82 | 83 | template 84 | struct VectviewBase { 85 | typedef Eigen::Matrix matrix_type; 86 | typedef typename matrix_type::MapType Type; 87 | typedef typename matrix_type::ConstMapType ConstType; 88 | }; 89 | 90 | template 91 | struct UnalignedType { 92 | typedef T type; 93 | }; 94 | } 95 | 96 | template 97 | class vectview : public internal::VectviewBase::Type { 98 | typedef internal::VectviewBase VectviewBase; 99 | public: 100 | //! plain matrix type 101 | typedef typename VectviewBase::matrix_type matrix_type; 102 | //! base type 103 | typedef typename VectviewBase::Type base; 104 | //! construct from pointer 105 | explicit 106 | vectview(scalar* data, int dim_=dim) : base(data, dim_) {} 107 | //! construct from plain matrix 108 | vectview(matrix_type& m) : base(m.data(), m.size()) {} 109 | //! construct from another @c vectview 110 | vectview(const vectview &v) : base(v) {} 111 | //! construct from Eigen::Block: 112 | template 113 | vectview(Eigen::VectorBlock block) : base(&block.coeffRef(0), block.size()) {} 114 | template 115 | vectview(Eigen::Block block) : base(&block.coeffRef(0), block.size()) {} 116 | 117 | //! inherit assignment operator 118 | using base::operator=; 119 | //! data pointer 120 | scalar* data() {return const_cast(base::data());} 121 | }; 122 | 123 | /** 124 | * @c const version of @c vectview. 125 | * Compared to @c Eigen::Map this implementation is const correct, i.e., 126 | * data will not be modifiable using this view. 127 | * 128 | * @tparam scalar Scalar type of the vector. 129 | * @tparam dim Dimension of the vector. 130 | * 131 | * @sa vectview 132 | */ 133 | template 134 | class vectview : public internal::VectviewBase::ConstType { 135 | typedef internal::VectviewBase VectviewBase; 136 | public: 137 | //! plain matrix type 138 | typedef typename VectviewBase::matrix_type matrix_type; 139 | //! base type 140 | typedef typename VectviewBase::ConstType base; 141 | //! construct from const pointer 142 | explicit 143 | vectview(const scalar* data, int dim_ = dim) : base(data, dim_) {} 144 | //! construct from column vector 145 | template 146 | vectview(const Eigen::Matrix& m) : base(m.data()) {} 147 | //! construct from row vector 148 | template 149 | vectview(const Eigen::Matrix& m) : base(m.data()) {} 150 | //! construct from another @c vectview 151 | vectview(vectview x) : base(x.data()) {} 152 | //! construct from base 153 | vectview(const base &x) : base(x) {} 154 | /** 155 | * Construct from Block 156 | * @todo adapt this, when Block gets const-correct 157 | */ 158 | template 159 | vectview(Eigen::VectorBlock block) : base(&block.coeffRef(0)) {} 160 | template 161 | vectview(Eigen::Block block) : base(&block.coeffRef(0)) {} 162 | 163 | }; 164 | 165 | 166 | } // namespace MTK 167 | 168 | #endif /* VECTVIEW_HPP_ */ 169 | -------------------------------------------------------------------------------- /include/IKFoM_toolkit/mtk/startIdx.hpp: -------------------------------------------------------------------------------- 1 | // This is an advanced implementation of the algorithm described in the 2 | // following paper: 3 | // C. Hertzberg, R. Wagner, U. Frese, and L. Schroder. Integratinggeneric sensor fusion algorithms with sound state representationsthrough encapsulation of manifolds. 4 | // CoRR, vol. abs/1107.1119, 2011.[Online]. Available: http://arxiv.org/abs/1107.1119 5 | 6 | /* 7 | * Copyright (c) 2019--2023, The University of Hong Kong 8 | * All rights reserved. 9 | * 10 | * Modifier: Dongjiao HE 11 | * 12 | * Redistribution and use in source and binary forms, with or without 13 | * modification, are permitted provided that the following conditions 14 | * are met: 15 | * 16 | * * Redistributions of source code must retain the above copyright 17 | * notice, this list of conditions and the following disclaimer. 18 | * * Redistributions in binary form must reproduce the above 19 | * copyright notice, this list of conditions and the following 20 | * disclaimer in the documentation and/or other materials provided 21 | * with the distribution. 22 | * * Neither the name of the Universitaet Bremen nor the names of its 23 | * contributors may be used to endorse or promote products derived 24 | * from this software without specific prior written permission. 25 | * 26 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 27 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 28 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 29 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 30 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 31 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 32 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 33 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 34 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 35 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 36 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 37 | * POSSIBILITY OF SUCH DAMAGE. 38 | */ 39 | 40 | /* 41 | * Copyright (c) 2008--2011, Universitaet Bremen 42 | * All rights reserved. 43 | * 44 | * Author: Christoph Hertzberg 45 | * 46 | * Redistribution and use in source and binary forms, with or without 47 | * modification, are permitted provided that the following conditions 48 | * are met: 49 | * 50 | * * Redistributions of source code must retain the above copyright 51 | * notice, this list of conditions and the following disclaimer. 52 | * * Redistributions in binary form must reproduce the above 53 | * copyright notice, this list of conditions and the following 54 | * disclaimer in the documentation and/or other materials provided 55 | * with the distribution. 56 | * * Neither the name of the Universitaet Bremen nor the names of its 57 | * contributors may be used to endorse or promote products derived 58 | * from this software without specific prior written permission. 59 | * 60 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 61 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 62 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 63 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 64 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 65 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 66 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 67 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 68 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 69 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 70 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 71 | * POSSIBILITY OF SUCH DAMAGE. 72 | */ 73 | /** 74 | * @file mtk/startIdx.hpp 75 | * @brief Tools to access sub-elements of compound manifolds. 76 | */ 77 | #ifndef GET_START_INDEX_H_ 78 | #define GET_START_INDEX_H_ 79 | 80 | #include 81 | 82 | #include "src/SubManifold.hpp" 83 | #include "src/vectview.hpp" 84 | 85 | namespace MTK { 86 | 87 | 88 | /** 89 | * \defgroup SubManifolds Accessing Submanifolds 90 | * For compound manifolds constructed using MTK_BUILD_MANIFOLD, member pointers 91 | * can be used to get sub-vectors or matrix-blocks of a corresponding big matrix. 92 | * E.g. for a type @a pose consisting of @a orient and @a trans the member pointers 93 | * @c &pose::orient and @c &pose::trans give all required information and are still 94 | * valid if the base type gets extended or the actual types of @a orient and @a trans 95 | * change (e.g. from 2D to 3D). 96 | * 97 | * @todo Maybe require manifolds to typedef MatrixType and VectorType, etc. 98 | */ 99 | //@{ 100 | 101 | /** 102 | * Determine the index of a sub-variable within a compound variable. 103 | */ 104 | template 105 | int getStartIdx( MTK::SubManifold Base::*) 106 | { 107 | return idx; 108 | } 109 | 110 | template 111 | int getStartIdx_( MTK::SubManifold Base::*) 112 | { 113 | return dim; 114 | } 115 | 116 | /** 117 | * Determine the degrees of freedom of a sub-variable within a compound variable. 118 | */ 119 | template 120 | int getDof( MTK::SubManifold Base::*) 121 | { 122 | return T::DOF; 123 | } 124 | template 125 | int getDim( MTK::SubManifold Base::*) 126 | { 127 | return T::DIM; 128 | } 129 | 130 | /** 131 | * set the diagonal elements of a covariance matrix corresponding to a sub-variable 132 | */ 133 | template 134 | void setDiagonal(Eigen::Matrix &cov, 135 | MTK::SubManifold Base::*, const typename Base::scalar &val) 136 | { 137 | cov.diagonal().template segment(idx).setConstant(val); 138 | } 139 | 140 | template 141 | void setDiagonal_(Eigen::Matrix &cov, 142 | MTK::SubManifold Base::*, const typename Base::scalar &val) 143 | { 144 | cov.diagonal().template segment(dim).setConstant(val); 145 | } 146 | 147 | /** 148 | * Get the subblock of corresponding to two members, i.e. 149 | * \code 150 | * Eigen::Matrix m; 151 | * MTK::subblock(m, &Pose::orient, &Pose::trans) = some_expression; 152 | * MTK::subblock(m, &Pose::trans, &Pose::orient) = some_expression.trans(); 153 | * \endcode 154 | * lets you modify mixed covariance entries in a bigger covariance matrix. 155 | */ 156 | template 157 | typename MTK::internal::CovBlock::Type 158 | subblock(Eigen::Matrix &cov, 159 | MTK::SubManifold Base::*, MTK::SubManifold Base::*) 160 | { 161 | return cov.template block(idx1, idx2); 162 | } 163 | 164 | template 165 | typename MTK::internal::CovBlock_::Type 166 | subblock_(Eigen::Matrix &cov, 167 | MTK::SubManifold Base::*, MTK::SubManifold Base::*) 168 | { 169 | return cov.template block(dim1, dim2); 170 | } 171 | 172 | template 173 | typename MTK::internal::CrossCovBlock::Type 174 | subblock(Eigen::Matrix &cov, MTK::SubManifold Base1::*, MTK::SubManifold Base2::*) 175 | { 176 | return cov.template block(idx1, idx2); 177 | } 178 | 179 | template 180 | typename MTK::internal::CrossCovBlock_::Type 181 | subblock_(Eigen::Matrix &cov, MTK::SubManifold Base1::*, MTK::SubManifold Base2::*) 182 | { 183 | return cov.template block(dim1, dim2); 184 | } 185 | /** 186 | * Get the subblock of corresponding to a member, i.e. 187 | * \code 188 | * Eigen::Matrix m; 189 | * MTK::subblock(m, &Pose::orient) = some_expression; 190 | * \endcode 191 | * lets you modify covariance entries in a bigger covariance matrix. 192 | */ 193 | template 194 | typename MTK::internal::CovBlock_::Type 195 | subblock_(Eigen::Matrix &cov, 196 | MTK::SubManifold Base::*) 197 | { 198 | return cov.template block(dim, dim); 199 | } 200 | 201 | template 202 | typename MTK::internal::CovBlock::Type 203 | subblock(Eigen::Matrix &cov, 204 | MTK::SubManifold Base::*) 205 | { 206 | return cov.template block(idx, idx); 207 | } 208 | 209 | template 210 | class get_cov { 211 | public: 212 | typedef Eigen::Matrix type; 213 | typedef const Eigen::Matrix const_type; 214 | }; 215 | 216 | template 217 | class get_cov_ { 218 | public: 219 | typedef Eigen::Matrix type; 220 | typedef const Eigen::Matrix const_type; 221 | }; 222 | 223 | template 224 | class get_cross_cov { 225 | public: 226 | typedef Eigen::Matrix type; 227 | typedef const type const_type; 228 | }; 229 | 230 | template 231 | class get_cross_cov_ { 232 | public: 233 | typedef Eigen::Matrix type; 234 | typedef const type const_type; 235 | }; 236 | 237 | 238 | template 239 | vectview 240 | subvector_impl_(vectview vec, SubManifold Base::*) 241 | { 242 | return vec.template segment(dim); 243 | } 244 | 245 | template 246 | vectview 247 | subvector_impl(vectview vec, SubManifold Base::*) 248 | { 249 | return vec.template segment(idx); 250 | } 251 | 252 | /** 253 | * Get the subvector corresponding to a sub-manifold from a bigger vector. 254 | */ 255 | template 256 | vectview 257 | subvector_(vectview vec, SubManifold Base::* ptr) 258 | { 259 | return subvector_impl_(vec, ptr); 260 | } 261 | 262 | template 263 | vectview 264 | subvector(vectview vec, SubManifold Base::* ptr) 265 | { 266 | return subvector_impl(vec, ptr); 267 | } 268 | 269 | /** 270 | * @todo This should be covered already by subvector(vectview vec,SubManifold Base::*) 271 | */ 272 | template 273 | vectview 274 | subvector(Eigen::Matrix& vec, SubManifold Base::* ptr) 275 | { 276 | return subvector_impl(vectview(vec), ptr); 277 | } 278 | 279 | template 280 | vectview 281 | subvector_(Eigen::Matrix& vec, SubManifold Base::* ptr) 282 | { 283 | return subvector_impl_(vectview(vec), ptr); 284 | } 285 | 286 | template 287 | vectview 288 | subvector_(const Eigen::Matrix& vec, SubManifold Base::* ptr) 289 | { 290 | return subvector_impl_(vectview(vec), ptr); 291 | } 292 | 293 | template 294 | vectview 295 | subvector(const Eigen::Matrix& vec, SubManifold Base::* ptr) 296 | { 297 | return subvector_impl(vectview(vec), ptr); 298 | } 299 | 300 | 301 | /** 302 | * const version of subvector(vectview vec,SubManifold Base::*) 303 | */ 304 | template 305 | vectview 306 | subvector_impl(const vectview cvec, SubManifold Base::*) 307 | { 308 | return cvec.template segment(idx); 309 | } 310 | 311 | template 312 | vectview 313 | subvector_impl_(const vectview cvec, SubManifold Base::*) 314 | { 315 | return cvec.template segment(dim); 316 | } 317 | 318 | template 319 | vectview 320 | subvector(const vectview cvec, SubManifold Base::* ptr) 321 | { 322 | return subvector_impl(cvec, ptr); 323 | } 324 | 325 | 326 | } // namespace MTK 327 | 328 | #endif // GET_START_INDEX_H_ 329 | -------------------------------------------------------------------------------- /include/IKFoM_toolkit/mtk/types/S2.hpp: -------------------------------------------------------------------------------- 1 | // This is a NEW implementation of the algorithm described in the 2 | // following paper: 3 | // C. Hertzberg, R. Wagner, U. Frese, and L. Schroder. Integratinggeneric sensor fusion algorithms with sound state representationsthrough encapsulation of manifolds. 4 | // CoRR, vol. abs/1107.1119, 2011.[Online]. Available: http://arxiv.org/abs/1107.1119 5 | 6 | /* 7 | * Copyright (c) 2019--2023, The University of Hong Kong 8 | * All rights reserved. 9 | * 10 | * Modifier: Dongjiao HE 11 | * 12 | * Redistribution and use in source and binary forms, with or without 13 | * modification, are permitted provided that the following conditions 14 | * are met: 15 | * 16 | * * Redistributions of source code must retain the above copyright 17 | * notice, this list of conditions and the following disclaimer. 18 | * * Redistributions in binary form must reproduce the above 19 | * copyright notice, this list of conditions and the following 20 | * disclaimer in the documentation and/or other materials provided 21 | * with the distribution. 22 | * * Neither the name of the Universitaet Bremen nor the names of its 23 | * contributors may be used to endorse or promote products derived 24 | * from this software without specific prior written permission. 25 | * 26 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 27 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 28 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 29 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 30 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 31 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 32 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 33 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 34 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 35 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 36 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 37 | * POSSIBILITY OF SUCH DAMAGE. 38 | */ 39 | 40 | /* 41 | * Copyright (c) 2008--2011, Universitaet Bremen 42 | * All rights reserved. 43 | * 44 | * Author: Christoph Hertzberg 45 | * 46 | * Redistribution and use in source and binary forms, with or without 47 | * modification, are permitted provided that the following conditions 48 | * are met: 49 | * 50 | * * Redistributions of source code must retain the above copyright 51 | * notice, this list of conditions and the following disclaimer. 52 | * * Redistributions in binary form must reproduce the above 53 | * copyright notice, this list of conditions and the following 54 | * disclaimer in the documentation and/or other materials provided 55 | * with the distribution. 56 | * * Neither the name of the Universitaet Bremen nor the names of its 57 | * contributors may be used to endorse or promote products derived 58 | * from this software without specific prior written permission. 59 | * 60 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 61 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 62 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 63 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 64 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 65 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 66 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 67 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 68 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 69 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 70 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 71 | * POSSIBILITY OF SUCH DAMAGE. 72 | */ 73 | /** 74 | * @file mtk/types/S2.hpp 75 | * @brief Unit vectors on the sphere, or directions in 3D. 76 | */ 77 | #ifndef S2_H_ 78 | #define S2_H_ 79 | 80 | 81 | #include "vect.hpp" 82 | 83 | #include "SOn.hpp" 84 | #include "../src/mtkmath.hpp" 85 | 86 | 87 | 88 | 89 | namespace MTK { 90 | 91 | /** 92 | * Manifold representation of @f$ S^2 @f$. 93 | * Used for unit vectors on the sphere or directions in 3D. 94 | * 95 | * @todo add conversions from/to polar angles? 96 | */ 97 | template 98 | struct S2 { 99 | 100 | typedef _scalar scalar; 101 | typedef vect<3, scalar> vect_type; 102 | typedef SO3 SO3_type; 103 | typedef typename vect_type::base vec3; 104 | scalar length = scalar(den)/scalar(num); 105 | enum {DOF=2, TYP = 1, DIM = 3}; 106 | 107 | //private: 108 | /** 109 | * Unit vector on the sphere, or vector pointing in a direction 110 | */ 111 | vect_type vec; 112 | 113 | public: 114 | S2() { 115 | if(S2_typ == 3) vec=length * vec3(0, 0, std::sqrt(1)); 116 | if(S2_typ == 2) vec=length * vec3(0, std::sqrt(1), 0); 117 | if(S2_typ == 1) vec=length * vec3(std::sqrt(1), 0, 0); 118 | } 119 | S2(const scalar &x, const scalar &y, const scalar &z) : vec(vec3(x, y, z)) { 120 | vec.normalize(); 121 | vec = vec * length; 122 | } 123 | 124 | S2(const vect_type &_vec) : vec(_vec) { 125 | vec.normalize(); 126 | vec = vec * length; 127 | } 128 | 129 | void oplus(MTK::vectview delta, scalar scale = 1) 130 | { 131 | SO3_type res; 132 | res.w() = MTK::exp(res.vec(), delta, scalar(scale/2)); 133 | vec = res.toRotationMatrix() * vec; 134 | } 135 | 136 | void boxplus(MTK::vectview delta, scalar scale=1) { 137 | Eigen::Matrix Bx; 138 | S2_Bx(Bx); 139 | vect_type Bu = Bx*delta;SO3_type res; 140 | res.w() = MTK::exp(res.vec(), Bu, scalar(scale/2)); 141 | vec = res.toRotationMatrix() * vec; 142 | } 143 | 144 | void boxminus(MTK::vectview res, const S2& other) const { 145 | scalar v_sin = (MTK::hat(vec)*other.vec).norm(); 146 | scalar v_cos = vec.transpose() * other.vec; 147 | scalar theta = std::atan2(v_sin, v_cos); 148 | if(v_sin < MTK::tolerance()) 149 | { 150 | if(std::fabs(theta) > MTK::tolerance() ) 151 | { 152 | res[0] = 3.1415926; 153 | res[1] = 0; 154 | } 155 | else{ 156 | res[0] = 0; 157 | res[1] = 0; 158 | } 159 | } 160 | else 161 | { 162 | S2 other_copy = other; 163 | Eigen::MatrixBx; 164 | other_copy.S2_Bx(Bx); 165 | res = theta/v_sin * Bx.transpose() * MTK::hat(other.vec)*vec; 166 | } 167 | } 168 | 169 | void S2_hat(Eigen::Matrix &res) 170 | { 171 | Eigen::Matrix skew_vec; 172 | skew_vec << scalar(0), -vec[2], vec[1], 173 | vec[2], scalar(0), -vec[0], 174 | -vec[1], vec[0], scalar(0); 175 | res = skew_vec; 176 | } 177 | 178 | 179 | void S2_Bx(Eigen::Matrix &res) 180 | { 181 | if(S2_typ == 3) 182 | { 183 | if(vec[2] + length > tolerance()) 184 | { 185 | 186 | res << length - vec[0]*vec[0]/(length+vec[2]), -vec[0]*vec[1]/(length+vec[2]), 187 | -vec[0]*vec[1]/(length+vec[2]), length-vec[1]*vec[1]/(length+vec[2]), 188 | -vec[0], -vec[1]; 189 | res /= length; 190 | } 191 | else 192 | { 193 | res = Eigen::Matrix::Zero(); 194 | res(1, 1) = -1; 195 | res(2, 0) = 1; 196 | } 197 | } 198 | else if(S2_typ == 2) 199 | { 200 | if(vec[1] + length > tolerance()) 201 | { 202 | 203 | res << length - vec[0]*vec[0]/(length+vec[1]), -vec[0]*vec[2]/(length+vec[1]), 204 | -vec[0], -vec[2], 205 | -vec[0]*vec[2]/(length+vec[1]), length-vec[2]*vec[2]/(length+vec[1]); 206 | res /= length; 207 | } 208 | else 209 | { 210 | res = Eigen::Matrix::Zero(); 211 | res(1, 1) = -1; 212 | res(2, 0) = 1; 213 | } 214 | } 215 | else 216 | { 217 | if(vec[0] + length > tolerance()) 218 | { 219 | 220 | res << -vec[1], -vec[2], 221 | length - vec[1]*vec[1]/(length+vec[0]), -vec[2]*vec[1]/(length+vec[0]), 222 | -vec[2]*vec[1]/(length+vec[0]), length-vec[2]*vec[2]/(length+vec[0]); 223 | res /= length; 224 | } 225 | else 226 | { 227 | res = Eigen::Matrix::Zero(); 228 | res(1, 1) = -1; 229 | res(2, 0) = 1; 230 | } 231 | } 232 | } 233 | 234 | void S2_Nx(Eigen::Matrix &res, S2& subtrahend) 235 | { 236 | if((vec+subtrahend.vec).norm() > tolerance()) 237 | { 238 | Eigen::Matrix Bx; 239 | S2_Bx(Bx); 240 | if((vec-subtrahend.vec).norm() > tolerance()) 241 | { 242 | scalar v_sin = (MTK::hat(vec)*subtrahend.vec).norm(); 243 | scalar v_cos = vec.transpose() * subtrahend.vec; 244 | 245 | res = Bx.transpose() * (std::atan2(v_sin, v_cos)/v_sin*MTK::hat(vec)+MTK::hat(vec)*subtrahend.vec*((-v_cos/v_sin/v_sin/length/length/length/length+std::atan2(v_sin, v_cos)/v_sin/v_sin/v_sin)*subtrahend.vec.transpose()*MTK::hat(vec)*MTK::hat(vec)-vec.transpose()/length/length/length/length)); 246 | } 247 | else 248 | { 249 | res = 1/length/length*Bx.transpose()*MTK::hat(vec); 250 | } 251 | } 252 | else 253 | { 254 | std::cerr << "No N(x, y) for x=-y" << std::endl; 255 | std::exit(100); 256 | } 257 | } 258 | 259 | void S2_Nx_yy(Eigen::Matrix &res) 260 | { 261 | Eigen::Matrix Bx; 262 | S2_Bx(Bx); 263 | res = 1/length/length*Bx.transpose()*MTK::hat(vec); 264 | } 265 | 266 | void S2_Mx(Eigen::Matrix &res, MTK::vectview delta) 267 | { 268 | Eigen::Matrix Bx; 269 | S2_Bx(Bx); 270 | if(delta.norm() < tolerance()) 271 | { 272 | res = -MTK::hat(vec)*Bx; 273 | } 274 | else{ 275 | vect_type Bu = Bx*delta; 276 | SO3_type exp_delta; 277 | exp_delta.w() = MTK::exp(exp_delta.vec(), Bu, scalar(1/2)); 278 | res = -exp_delta.toRotationMatrix()*MTK::hat(vec)*MTK::A_matrix(Bu).transpose()*Bx; 279 | } 280 | } 281 | 282 | operator const vect_type&() const{ 283 | return vec; 284 | } 285 | 286 | const vect_type& get_vect() const { 287 | return vec; 288 | } 289 | 290 | friend S2 operator*(const SO3& rot, const S2& dir) 291 | { 292 | S2 ret; 293 | ret.vec = rot * dir.vec; 294 | return ret; 295 | } 296 | 297 | scalar operator[](int idx) const {return vec[idx]; } 298 | 299 | friend std::ostream& operator<<(std::ostream &os, const S2& vec){ 300 | return os << vec.vec.transpose() << " "; 301 | } 302 | friend std::istream& operator>>(std::istream &is, S2& vec){ 303 | for(int i=0; i<3; ++i) 304 | is >> vec.vec[i]; 305 | vec.vec.normalize(); 306 | vec.vec = vec.vec * vec.length; 307 | return is; 308 | 309 | } 310 | }; 311 | 312 | 313 | } // namespace MTK 314 | 315 | 316 | #endif /*S2_H_*/ 317 | -------------------------------------------------------------------------------- /include/IKFoM_toolkit/mtk/types/SOn.hpp: -------------------------------------------------------------------------------- 1 | // This is an advanced implementation of the algorithm described in the 2 | // following paper: 3 | // C. Hertzberg, R. Wagner, U. Frese, and L. Schroder. Integratinggeneric sensor fusion algorithms with sound state representationsthrough encapsulation of manifolds. 4 | // CoRR, vol. abs/1107.1119, 2011.[Online]. Available: http://arxiv.org/abs/1107.1119 5 | 6 | /* 7 | * Copyright (c) 2019--2023, The University of Hong Kong 8 | * All rights reserved. 9 | * 10 | * Modifier: Dongjiao HE 11 | * 12 | * Redistribution and use in source and binary forms, with or without 13 | * modification, are permitted provided that the following conditions 14 | * are met: 15 | * 16 | * * Redistributions of source code must retain the above copyright 17 | * notice, this list of conditions and the following disclaimer. 18 | * * Redistributions in binary form must reproduce the above 19 | * copyright notice, this list of conditions and the following 20 | * disclaimer in the documentation and/or other materials provided 21 | * with the distribution. 22 | * * Neither the name of the Universitaet Bremen nor the names of its 23 | * contributors may be used to endorse or promote products derived 24 | * from this software without specific prior written permission. 25 | * 26 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 27 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 28 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 29 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 30 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 31 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 32 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 33 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 34 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 35 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 36 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 37 | * POSSIBILITY OF SUCH DAMAGE. 38 | */ 39 | 40 | /* 41 | * Copyright (c) 2008--2011, Universitaet Bremen 42 | * All rights reserved. 43 | * 44 | * Author: Christoph Hertzberg 45 | * 46 | * Redistribution and use in source and binary forms, with or without 47 | * modification, are permitted provided that the following conditions 48 | * are met: 49 | * 50 | * * Redistributions of source code must retain the above copyright 51 | * notice, this list of conditions and the following disclaimer. 52 | * * Redistributions in binary form must reproduce the above 53 | * copyright notice, this list of conditions and the following 54 | * disclaimer in the documentation and/or other materials provided 55 | * with the distribution. 56 | * * Neither the name of the Universitaet Bremen nor the names of its 57 | * contributors may be used to endorse or promote products derived 58 | * from this software without specific prior written permission. 59 | * 60 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 61 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 62 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 63 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 64 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 65 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 66 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 67 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 68 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 69 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 70 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 71 | * POSSIBILITY OF SUCH DAMAGE. 72 | */ 73 | /** 74 | * @file mtk/types/SOn.hpp 75 | * @brief Standard Orthogonal Groups i.e.\ rotatation groups. 76 | */ 77 | #ifndef SON_H_ 78 | #define SON_H_ 79 | 80 | #include 81 | 82 | #include "vect.hpp" 83 | #include "../src/mtkmath.hpp" 84 | 85 | 86 | namespace MTK { 87 | 88 | 89 | /** 90 | * Two-dimensional orientations represented as scalar. 91 | * There is no guarantee that the representing scalar is within any interval, 92 | * but the result of boxminus will always have magnitude @f$\le\pi @f$. 93 | */ 94 | template 95 | struct SO2 : public Eigen::Rotation2D<_scalar> { 96 | enum {DOF = 1, DIM = 2, TYP = 3}; 97 | 98 | typedef _scalar scalar; 99 | typedef Eigen::Rotation2D base; 100 | typedef vect vect_type; 101 | 102 | //! Construct from angle 103 | SO2(const scalar& angle = 0) : base(angle) { } 104 | 105 | //! Construct from Eigen::Rotation2D 106 | SO2(const base& src) : base(src) {} 107 | 108 | /** 109 | * Construct from 2D vector. 110 | * Resulting orientation will rotate the first unit vector to point to vec. 111 | */ 112 | SO2(const vect_type &vec) : base(atan2(vec[1], vec[0])) {}; 113 | 114 | 115 | //! Calculate @c this->inverse() * @c r 116 | SO2 operator%(const base &r) const { 117 | return base::inverse() * r; 118 | } 119 | 120 | //! Calculate @c this->inverse() * @c r 121 | template 122 | vect_type operator%(const Eigen::MatrixBase &vec) const { 123 | return base::inverse() * vec; 124 | } 125 | 126 | //! Calculate @c *this * @c r.inverse() 127 | SO2 operator/(const SO2 &r) const { 128 | return *this * r.inverse(); 129 | } 130 | 131 | //! Gets the angle as scalar. 132 | operator scalar() const { 133 | return base::angle(); 134 | } 135 | void S2_hat(Eigen::Matrix &res) 136 | { 137 | res = Eigen::Matrix::Zero(); 138 | } 139 | //! @name Manifold requirements 140 | void S2_Nx_yy(Eigen::Matrix &res) 141 | { 142 | std::cerr << "wrong idx for S2" << std::endl; 143 | std::exit(100); 144 | res = Eigen::Matrix::Zero(); 145 | } 146 | 147 | void S2_Mx(Eigen::Matrix &res, MTK::vectview delta) 148 | { 149 | std::cerr << "wrong idx for S2" << std::endl; 150 | std::exit(100); 151 | res = Eigen::Matrix::Zero(); 152 | } 153 | 154 | void oplus(MTK::vectview vec, scalar scale = 1) { 155 | base::angle() += scale * vec[0]; 156 | } 157 | 158 | void boxplus(MTK::vectview vec, scalar scale = 1) { 159 | base::angle() += scale * vec[0]; 160 | } 161 | void boxminus(MTK::vectview res, const SO2& other) const { 162 | res[0] = MTK::normalize(base::angle() - other.angle(), scalar(MTK::pi)); 163 | } 164 | 165 | friend std::istream& operator>>(std::istream &is, SO2& ang){ 166 | return is >> ang.angle(); 167 | } 168 | 169 | }; 170 | 171 | 172 | /** 173 | * Three-dimensional orientations represented as Quaternion. 174 | * It is assumed that the internal Quaternion always stays normalized, 175 | * should this not be the case, call inherited member function @c normalize(). 176 | */ 177 | template 178 | struct SO3 : public Eigen::Quaternion<_scalar, Options> { 179 | enum {DOF = 3, DIM = 3, TYP = 2}; 180 | typedef _scalar scalar; 181 | typedef Eigen::Quaternion base; 182 | typedef Eigen::Quaternion Quaternion; 183 | typedef vect vect_type; 184 | 185 | //! Calculate @c this->inverse() * @c r 186 | template EIGEN_STRONG_INLINE 187 | Quaternion operator%(const Eigen::QuaternionBase &r) const { 188 | return base::conjugate() * r; 189 | } 190 | 191 | //! Calculate @c this->inverse() * @c r 192 | template 193 | vect_type operator%(const Eigen::MatrixBase &vec) const { 194 | return base::conjugate() * vec; 195 | } 196 | 197 | //! Calculate @c this * @c r.conjugate() 198 | template EIGEN_STRONG_INLINE 199 | Quaternion operator/(const Eigen::QuaternionBase &r) const { 200 | return *this * r.conjugate(); 201 | } 202 | 203 | /** 204 | * Construct from real part and three imaginary parts. 205 | * Quaternion is normalized after construction. 206 | */ 207 | SO3(const scalar& w, const scalar& x, const scalar& y, const scalar& z) : base(w, x, y, z) { 208 | base::normalize(); 209 | } 210 | 211 | /** 212 | * Construct from Eigen::Quaternion. 213 | * @note Non-normalized input may result result in spurious behavior. 214 | */ 215 | SO3(const base& src = base::Identity()) : base(src) {} 216 | 217 | /** 218 | * Construct from rotation matrix. 219 | * @note Invalid rotation matrices may lead to spurious behavior. 220 | */ 221 | template 222 | SO3(const Eigen::MatrixBase& matrix) : base(matrix) {} 223 | 224 | /** 225 | * Construct from arbitrary rotation type. 226 | * @note Invalid rotation matrices may lead to spurious behavior. 227 | */ 228 | template 229 | SO3(const Eigen::RotationBase& rotation) : base(rotation.derived()) {} 230 | 231 | //! @name Manifold requirements 232 | 233 | void boxplus(MTK::vectview vec, scalar scale=1) { 234 | SO3 delta = exp(vec, scale); 235 | *this = *this * delta; 236 | } 237 | void boxminus(MTK::vectview res, const SO3& other) const { 238 | res = SO3::log(other.conjugate() * *this); 239 | } 240 | //} 241 | 242 | void oplus(MTK::vectview vec, scalar scale=1) { 243 | SO3 delta = exp(vec, scale); 244 | *this = *this * delta; 245 | } 246 | 247 | void S2_hat(Eigen::Matrix &res) 248 | { 249 | res = Eigen::Matrix::Zero(); 250 | } 251 | void S2_Nx_yy(Eigen::Matrix &res) 252 | { 253 | std::cerr << "wrong idx for S2" << std::endl; 254 | std::exit(100); 255 | res = Eigen::Matrix::Zero(); 256 | } 257 | 258 | void S2_Mx(Eigen::Matrix &res, MTK::vectview delta) 259 | { 260 | std::cerr << "wrong idx for S2" << std::endl; 261 | std::exit(100); 262 | res = Eigen::Matrix::Zero(); 263 | } 264 | 265 | friend std::ostream& operator<<(std::ostream &os, const SO3& q){ 266 | return os << q.coeffs().transpose() << " "; 267 | } 268 | 269 | friend std::istream& operator>>(std::istream &is, SO3& q){ 270 | vect<4,scalar> coeffs; 271 | is >> coeffs; 272 | q.coeffs() = coeffs.normalized(); 273 | return is; 274 | } 275 | 276 | //! @name Helper functions 277 | //{ 278 | /** 279 | * Calculate the exponential map. In matrix terms this would correspond 280 | * to the Rodrigues formula. 281 | */ 282 | // FIXME vectview<> can't be constructed from every MatrixBase<>, use const Vector3x& as workaround 283 | // static SO3 exp(MTK::vectview dvec, scalar scale = 1){ 284 | static SO3 exp(const Eigen::Matrix& dvec, scalar scale = 1){ 285 | SO3 res; 286 | res.w() = MTK::exp(res.vec(), dvec, scalar(scale/2)); 287 | return res; 288 | } 289 | /** 290 | * Calculate the inverse of @c exp. 291 | * Only guarantees that exp(log(x)) == x 292 | */ 293 | static typename base::Vector3 log(const SO3 &orient){ 294 | typename base::Vector3 res; 295 | MTK::log(res, orient.w(), orient.vec(), scalar(2), true); 296 | return res; 297 | } 298 | }; 299 | 300 | namespace internal { 301 | template 302 | struct UnalignedType >{ 303 | typedef SO2 type; 304 | }; 305 | 306 | template 307 | struct UnalignedType >{ 308 | typedef SO3 type; 309 | }; 310 | 311 | } // namespace internal 312 | 313 | 314 | } // namespace MTK 315 | 316 | #endif /*SON_H_*/ 317 | 318 | -------------------------------------------------------------------------------- /include/IKFoM_toolkit/mtk/types/vect.hpp: -------------------------------------------------------------------------------- 1 | // This is an advanced implementation of the algorithm described in the 2 | // following paper: 3 | // C. Hertzberg, R. Wagner, U. Frese, and L. Schroder. Integratinggeneric sensor fusion algorithms with sound state representationsthrough encapsulation of manifolds. 4 | // CoRR, vol. abs/1107.1119, 2011.[Online]. Available: http://arxiv.org/abs/1107.1119 5 | 6 | /* 7 | * Copyright (c) 2019--2023, The University of Hong Kong 8 | * All rights reserved. 9 | * 10 | * Modifier: Dongjiao HE 11 | * 12 | * Redistribution and use in source and binary forms, with or without 13 | * modification, are permitted provided that the following conditions 14 | * are met: 15 | * 16 | * * Redistributions of source code must retain the above copyright 17 | * notice, this list of conditions and the following disclaimer. 18 | * * Redistributions in binary form must reproduce the above 19 | * copyright notice, this list of conditions and the following 20 | * disclaimer in the documentation and/or other materials provided 21 | * with the distribution. 22 | * * Neither the name of the Universitaet Bremen nor the names of its 23 | * contributors may be used to endorse or promote products derived 24 | * from this software without specific prior written permission. 25 | * 26 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 27 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 28 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 29 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 30 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 31 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 32 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 33 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 34 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 35 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 36 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 37 | * POSSIBILITY OF SUCH DAMAGE. 38 | */ 39 | 40 | /* 41 | * Copyright (c) 2008--2011, Universitaet Bremen 42 | * All rights reserved. 43 | * 44 | * Author: Christoph Hertzberg 45 | * 46 | * Redistribution and use in source and binary forms, with or without 47 | * modification, are permitted provided that the following conditions 48 | * are met: 49 | * 50 | * * Redistributions of source code must retain the above copyright 51 | * notice, this list of conditions and the following disclaimer. 52 | * * Redistributions in binary form must reproduce the above 53 | * copyright notice, this list of conditions and the following 54 | * disclaimer in the documentation and/or other materials provided 55 | * with the distribution. 56 | * * Neither the name of the Universitaet Bremen nor the names of its 57 | * contributors may be used to endorse or promote products derived 58 | * from this software without specific prior written permission. 59 | * 60 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 61 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 62 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 63 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 64 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 65 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 66 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 67 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 68 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 69 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 70 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 71 | * POSSIBILITY OF SUCH DAMAGE. 72 | */ 73 | /** 74 | * @file mtk/types/vect.hpp 75 | * @brief Basic vectors interpreted as manifolds. 76 | * 77 | * This file also implements a simple wrapper for matrices, for arbitrary scalars 78 | * and for positive scalars. 79 | */ 80 | #ifndef VECT_H_ 81 | #define VECT_H_ 82 | 83 | #include 84 | #include 85 | #include 86 | 87 | #include "../src/vectview.hpp" 88 | 89 | namespace MTK { 90 | 91 | static const Eigen::IOFormat IO_no_spaces(Eigen::StreamPrecision, Eigen::DontAlignCols, ",", ",", "", "", "[", "]"); 92 | 93 | 94 | /** 95 | * A simple vector class. 96 | * Implementation is basically a wrapper around Eigen::Matrix with manifold 97 | * requirements added. 98 | */ 99 | template 100 | struct vect : public Eigen::Matrix<_scalar, D, 1, _Options> { 101 | typedef Eigen::Matrix<_scalar, D, 1, _Options> base; 102 | enum {DOF = D, DIM = D, TYP = 0}; 103 | typedef _scalar scalar; 104 | 105 | //using base::operator=; 106 | 107 | /** Standard constructor. Sets all values to zero. */ 108 | vect(const base &src = base::Zero()) : base(src) {} 109 | 110 | /** Constructor copying the value of the expression \a other */ 111 | template 112 | EIGEN_STRONG_INLINE vect(const Eigen::DenseBase& other) : base(other) {} 113 | 114 | /** Construct from memory. */ 115 | vect(const scalar* src, int size = DOF) : base(base::Map(src, size)) { } 116 | 117 | void boxplus(MTK::vectview vec, scalar scale=1) { 118 | *this += scale * vec; 119 | } 120 | void boxminus(MTK::vectview res, const vect& other) const { 121 | res = *this - other; 122 | } 123 | 124 | void oplus(MTK::vectview vec, scalar scale=1) { 125 | *this += scale * vec; 126 | } 127 | 128 | void S2_hat(Eigen::Matrix &res) 129 | { 130 | res = Eigen::Matrix::Zero(); 131 | } 132 | 133 | void S2_Nx_yy(Eigen::Matrix &res) 134 | { 135 | std::cerr << "wrong idx for S2" << std::endl; 136 | std::exit(100); 137 | res = Eigen::Matrix::Zero(); 138 | } 139 | 140 | void S2_Mx(Eigen::Matrix &res, MTK::vectview delta) 141 | { 142 | std::cerr << "wrong idx for S2" << std::endl; 143 | std::exit(100); 144 | res = Eigen::Matrix::Zero(); 145 | } 146 | 147 | friend std::ostream& operator<<(std::ostream &os, const vect& v){ 148 | // Eigen sometimes messes with the streams flags, so output manually: 149 | for(int i=0; i>(std::istream &is, vect& v){ 154 | char term=0; 155 | is >> std::ws; // skip whitespace 156 | switch(is.peek()) { 157 | case '(': term=')'; is.ignore(1); break; 158 | case '[': term=']'; is.ignore(1); break; 159 | case '{': term='}'; is.ignore(1); break; 160 | default: break; 161 | } 162 | if(D==Eigen::Dynamic) { 163 | assert(term !=0 && "Dynamic vectors must be embraced"); 164 | std::vector temp; 165 | while(is.good() && is.peek() != term) { 166 | scalar x; 167 | is >> x; 168 | temp.push_back(x); 169 | if(is.peek()==',') is.ignore(1); 170 | } 171 | v = vect::Map(temp.data(), temp.size()); 172 | } else 173 | for(int i=0; i> v[i]; 175 | if(is.peek()==',') { // ignore commas between values 176 | is.ignore(1); 177 | } 178 | } 179 | if(term!=0) { 180 | char x; 181 | is >> x; 182 | if(x!=term) { 183 | is.setstate(is.badbit); 184 | // assert(x==term && "start and end bracket do not match!"); 185 | } 186 | } 187 | return is; 188 | } 189 | 190 | template 191 | vectview tail(){ 192 | BOOST_STATIC_ASSERT(0< dim && dim <= DOF); 193 | return base::template tail(); 194 | } 195 | template 196 | vectview tail() const{ 197 | BOOST_STATIC_ASSERT(0< dim && dim <= DOF); 198 | return base::template tail(); 199 | } 200 | template 201 | vectview head(){ 202 | BOOST_STATIC_ASSERT(0< dim && dim <= DOF); 203 | return base::template head(); 204 | } 205 | template 206 | vectview head() const{ 207 | BOOST_STATIC_ASSERT(0< dim && dim <= DOF); 208 | return base::template head(); 209 | } 210 | }; 211 | 212 | 213 | /** 214 | * A simple matrix class. 215 | * Implementation is basically a wrapper around Eigen::Matrix with manifold 216 | * requirements added, i.e., matrix is viewed as a plain vector for that. 217 | */ 218 | template::Options> 219 | struct matrix : public Eigen::Matrix<_scalar, M, N, _Options> { 220 | typedef Eigen::Matrix<_scalar, M, N, _Options> base; 221 | enum {DOF = M * N, TYP = 4, DIM=0}; 222 | typedef _scalar scalar; 223 | 224 | using base::operator=; 225 | 226 | /** Standard constructor. Sets all values to zero. */ 227 | matrix() { 228 | base::setZero(); 229 | } 230 | 231 | /** Constructor copying the value of the expression \a other */ 232 | template 233 | EIGEN_STRONG_INLINE matrix(const Eigen::MatrixBase& other) : base(other) {} 234 | 235 | /** Construct from memory. */ 236 | matrix(const scalar* src) : base(src) { } 237 | 238 | void boxplus(MTK::vectview vec, scalar scale = 1) { 239 | *this += scale * base::Map(vec.data()); 240 | } 241 | void boxminus(MTK::vectview res, const matrix& other) const { 242 | base::Map(res.data()) = *this - other; 243 | } 244 | 245 | void S2_hat(Eigen::Matrix &res) 246 | { 247 | res = Eigen::Matrix::Zero(); 248 | } 249 | 250 | void oplus(MTK::vectview vec, scalar scale = 1) { 251 | *this += scale * base::Map(vec.data()); 252 | } 253 | 254 | void S2_Nx_yy(Eigen::Matrix &res) 255 | { 256 | std::cerr << "wrong idx for S2" << std::endl; 257 | std::exit(100); 258 | res = Eigen::Matrix::Zero(); 259 | } 260 | 261 | void S2_Mx(Eigen::Matrix &res, MTK::vectview delta) 262 | { 263 | std::cerr << "wrong idx for S2" << std::endl; 264 | std::exit(100); 265 | res = Eigen::Matrix::Zero(); 266 | } 267 | 268 | friend std::ostream& operator<<(std::ostream &os, const matrix& mat){ 269 | for(int i=0; i>(std::istream &is, matrix& mat){ 275 | for(int i=0; i> mat.data()[i]; 277 | } 278 | return is; 279 | } 280 | };// @todo What if M / N = Eigen::Dynamic? 281 | 282 | 283 | 284 | /** 285 | * A simple scalar type. 286 | */ 287 | template 288 | struct Scalar { 289 | enum {DOF = 1, TYP = 5, DIM=0}; 290 | typedef _scalar scalar; 291 | 292 | scalar value; 293 | 294 | Scalar(const scalar& value = scalar(0)) : value(value) {} 295 | operator const scalar&() const { return value; } 296 | operator scalar&() { return value; } 297 | Scalar& operator=(const scalar& val) { value = val; return *this; } 298 | 299 | void S2_hat(Eigen::Matrix &res) 300 | { 301 | res = Eigen::Matrix::Zero(); 302 | } 303 | 304 | void S2_Nx_yy(Eigen::Matrix &res) 305 | { 306 | std::cerr << "wrong idx for S2" << std::endl; 307 | std::exit(100); 308 | res = Eigen::Matrix::Zero(); 309 | } 310 | 311 | void S2_Mx(Eigen::Matrix &res, MTK::vectview delta) 312 | { 313 | std::cerr << "wrong idx for S2" << std::endl; 314 | std::exit(100); 315 | res = Eigen::Matrix::Zero(); 316 | } 317 | 318 | void oplus(MTK::vectview vec, scalar scale=1) { 319 | value += scale * vec[0]; 320 | } 321 | 322 | void boxplus(MTK::vectview vec, scalar scale=1) { 323 | value += scale * vec[0]; 324 | } 325 | void boxminus(MTK::vectview res, const Scalar& other) const { 326 | res[0] = *this - other; 327 | } 328 | }; 329 | 330 | /** 331 | * Positive scalars. 332 | * Boxplus is implemented using multiplication by @f$x\boxplus\delta = x\cdot\exp(\delta) @f$. 333 | */ 334 | template 335 | struct PositiveScalar { 336 | enum {DOF = 1, TYP = 6, DIM=0}; 337 | typedef _scalar scalar; 338 | 339 | scalar value; 340 | 341 | PositiveScalar(const scalar& value = scalar(1)) : value(value) { 342 | assert(value > scalar(0)); 343 | } 344 | operator const scalar&() const { return value; } 345 | PositiveScalar& operator=(const scalar& val) { assert(val>0); value = val; return *this; } 346 | 347 | void boxplus(MTK::vectview vec, scalar scale = 1) { 348 | value *= std::exp(scale * vec[0]); 349 | } 350 | void boxminus(MTK::vectview res, const PositiveScalar& other) const { 351 | res[0] = std::log(*this / other); 352 | } 353 | 354 | void oplus(MTK::vectview vec, scalar scale = 1) { 355 | value *= std::exp(scale * vec[0]); 356 | } 357 | 358 | void S2_hat(Eigen::Matrix &res) 359 | { 360 | res = Eigen::Matrix::Zero(); 361 | } 362 | 363 | void S2_Nx_yy(Eigen::Matrix &res) 364 | { 365 | std::cerr << "wrong idx for S2" << std::endl; 366 | std::exit(100); 367 | res = Eigen::Matrix::Zero(); 368 | } 369 | 370 | void S2_Mx(Eigen::Matrix &res, MTK::vectview delta) 371 | { 372 | std::cerr << "wrong idx for S2" << std::endl; 373 | std::exit(100); 374 | res = Eigen::Matrix::Zero(); 375 | } 376 | 377 | 378 | friend std::istream& operator>>(std::istream &is, PositiveScalar& s){ 379 | is >> s.value; 380 | assert(s.value > 0); 381 | return is; 382 | } 383 | }; 384 | 385 | template 386 | struct Complex : public std::complex<_scalar>{ 387 | enum {DOF = 2, TYP = 7, DIM=0}; 388 | typedef _scalar scalar; 389 | 390 | typedef std::complex Base; 391 | 392 | Complex(const Base& value) : Base(value) {} 393 | Complex(const scalar& re = 0.0, const scalar& im = 0.0) : Base(re, im) {} 394 | Complex(const MTK::vectview &in) : Base(in[0], in[1]) {} 395 | template 396 | Complex(const Eigen::DenseBase &in) : Base(in[0], in[1]) {} 397 | 398 | void boxplus(MTK::vectview vec, scalar scale = 1) { 399 | Base::real() += scale * vec[0]; 400 | Base::imag() += scale * vec[1]; 401 | }; 402 | void boxminus(MTK::vectview res, const Complex& other) const { 403 | Complex diff = *this - other; 404 | res << diff.real(), diff.imag(); 405 | } 406 | 407 | void S2_hat(Eigen::Matrix &res) 408 | { 409 | res = Eigen::Matrix::Zero(); 410 | } 411 | 412 | void oplus(MTK::vectview vec, scalar scale = 1) { 413 | Base::real() += scale * vec[0]; 414 | Base::imag() += scale * vec[1]; 415 | }; 416 | 417 | void S2_Nx_yy(Eigen::Matrix &res) 418 | { 419 | std::cerr << "wrong idx for S2" << std::endl; 420 | std::exit(100); 421 | res = Eigen::Matrix::Zero(); 422 | } 423 | 424 | void S2_Mx(Eigen::Matrix &res, MTK::vectview delta) 425 | { 426 | std::cerr << "wrong idx for S2" << std::endl; 427 | std::exit(100); 428 | res = Eigen::Matrix::Zero(); 429 | } 430 | 431 | scalar squaredNorm() const { 432 | return std::pow(Base::real(),2) + std::pow(Base::imag(),2); 433 | } 434 | 435 | const scalar& operator()(int i) const { 436 | assert(0<=i && i<2 && "Index out of range"); 437 | return i==0 ? Base::real() : Base::imag(); 438 | } 439 | scalar& operator()(int i){ 440 | assert(0<=i && i<2 && "Index out of range"); 441 | return i==0 ? Base::real() : Base::imag(); 442 | } 443 | }; 444 | 445 | 446 | namespace internal { 447 | 448 | template 449 | struct UnalignedType >{ 450 | typedef vect type; 451 | }; 452 | 453 | } // namespace internal 454 | 455 | 456 | } // namespace MTK 457 | 458 | 459 | 460 | 461 | #endif /*VECT_H_*/ 462 | -------------------------------------------------------------------------------- /include/IKFoM_toolkit/mtk/types/wrapped_cv_mat.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2010--2011, Universitaet Bremen and DFKI GmbH 3 | * All rights reserved. 4 | * 5 | * Author: Rene Wagner 6 | * Christoph Hertzberg 7 | * 8 | * Redistribution and use in source and binary forms, with or without 9 | * modification, are permitted provided that the following conditions 10 | * are met: 11 | * 12 | * * Redistributions of source code must retain the above copyright 13 | * notice, this list of conditions and the following disclaimer. 14 | * * Redistributions in binary form must reproduce the above 15 | * copyright notice, this list of conditions and the following 16 | * disclaimer in the documentation and/or other materials provided 17 | * with the distribution. 18 | * * Neither the name of the Universitaet Bremen nor the DFKI GmbH 19 | * nor the names of its contributors may be used to endorse or 20 | * promote products derived from this software without specific 21 | * prior written permission. 22 | * 23 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 24 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 25 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 26 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 27 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 28 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 29 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 30 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 31 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 32 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 33 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 34 | * POSSIBILITY OF SUCH DAMAGE. 35 | */ 36 | 37 | #ifndef WRAPPED_CV_MAT_HPP_ 38 | #define WRAPPED_CV_MAT_HPP_ 39 | 40 | #include 41 | #include 42 | 43 | namespace MTK { 44 | 45 | template 46 | struct cv_f_type; 47 | 48 | template<> 49 | struct cv_f_type 50 | { 51 | enum {value = CV_64F}; 52 | }; 53 | 54 | template<> 55 | struct cv_f_type 56 | { 57 | enum {value = CV_32F}; 58 | }; 59 | 60 | /** 61 | * cv_mat wraps a CvMat around an Eigen Matrix 62 | */ 63 | template 64 | class cv_mat : public matrix 65 | { 66 | typedef matrix base_type; 67 | enum {type_ = cv_f_type::value}; 68 | CvMat cv_mat_; 69 | 70 | public: 71 | cv_mat() 72 | { 73 | cv_mat_ = cvMat(rows, cols, type_, base_type::data()); 74 | } 75 | 76 | cv_mat(const cv_mat& oth) : base_type(oth) 77 | { 78 | cv_mat_ = cvMat(rows, cols, type_, base_type::data()); 79 | } 80 | 81 | template 82 | cv_mat(const Eigen::MatrixBase &value) : base_type(value) 83 | { 84 | cv_mat_ = cvMat(rows, cols, type_, base_type::data()); 85 | } 86 | 87 | template 88 | cv_mat& operator=(const Eigen::MatrixBase &value) 89 | { 90 | base_type::operator=(value); 91 | return *this; 92 | } 93 | 94 | cv_mat& operator=(const cv_mat& value) 95 | { 96 | base_type::operator=(value); 97 | return *this; 98 | } 99 | 100 | // FIXME: Maybe overloading operator& is not a good idea ... 101 | CvMat* operator&() 102 | { 103 | return &cv_mat_; 104 | } 105 | const CvMat* operator&() const 106 | { 107 | return &cv_mat_; 108 | } 109 | }; 110 | 111 | } // namespace MTK 112 | 113 | #endif /* WRAPPED_CV_MAT_HPP_ */ 114 | -------------------------------------------------------------------------------- /include/common_lib.h: -------------------------------------------------------------------------------- 1 | #ifndef COMMON_LIB_H 2 | #define COMMON_LIB_H 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | //#include 9 | #include 10 | #include 11 | #include 12 | #include 13 | 14 | #include "point_type.h" 15 | 16 | using namespace std; 17 | using namespace Eigen; 18 | 19 | #define USE_IKFOM 20 | 21 | #define PI_M (3.14159265358) 22 | #define G_m_s2 (9.81) // Gravaty const in GuangDong/China 23 | #define DIM_STATE (18) // Dimension of states (Let Dim(SO(3)) = 3) 24 | #define DIM_PROC_N (12) // Dimension of process noise (Let Dim(SO(3)) = 3) 25 | #define CUBE_LEN (6.0) 26 | #define LIDAR_SP_LEN (2) 27 | #define INIT_COV (1) 28 | #define NUM_MATCH_POINTS (5) 29 | #define MAX_MEAS_DIM (10000) 30 | 31 | #define VEC_FROM_ARRAY(v) v[0],v[1],v[2] 32 | #define MAT_FROM_ARRAY(v) v[0],v[1],v[2],v[3],v[4],v[5],v[6],v[7],v[8] 33 | #define CONSTRAIN(v,min,max) ((v>min)?((v (mat.data(), mat.data() + mat.rows() * mat.cols()) 36 | #define DEBUG_FILE_DIR(name) (string(string(ROOT_DIR) + "Log/"+ name)) 37 | 38 | typedef pcl::PointXYZINormal PointType; 39 | //typedef custom_PointType PointType; 40 | typedef pcl::PointCloud PointCloudXYZI; 41 | typedef vector> PointVector; 42 | typedef Vector3d V3D; 43 | typedef Matrix3d M3D; 44 | typedef Vector3f V3F; 45 | typedef Matrix3f M3F; 46 | typedef Eigen::Matrix M6D; 47 | 48 | #define MD(a,b) Matrix 49 | #define VD(a) Matrix 50 | #define MF(a,b) Matrix 51 | #define VF(a) Matrix 52 | 53 | M3D Eye3d(M3D::Identity()); 54 | M3F Eye3f(M3F::Identity()); 55 | V3D Zero3d(0, 0, 0); 56 | V3F Zero3f(0, 0, 0); 57 | 58 | struct MeasureGroup // Lidar data and imu dates for the curent process 59 | { 60 | MeasureGroup() 61 | { 62 | lidar_beg_time = 0.0; 63 | this->lidar.reset(new PointCloudXYZI()); 64 | }; 65 | double lidar_beg_time; 66 | double lidar_end_time; 67 | PointCloudXYZI::Ptr lidar; 68 | deque imu; 69 | }; 70 | 71 | struct Pose6D 72 | { 73 | double offset_time; 74 | double acc[3], gyr[3], vel[3], pos[3], rot[9]; 75 | }; 76 | 77 | struct StatesGroup 78 | { 79 | StatesGroup() { 80 | this->rot_end = M3D::Identity(); 81 | this->pos_end = Zero3d; 82 | this->vel_end = Zero3d; 83 | this->bias_g = Zero3d; 84 | this->bias_a = Zero3d; 85 | this->gravity = Zero3d; 86 | this->cov = MD(DIM_STATE,DIM_STATE)::Identity() * INIT_COV; 87 | this->cov.block<9,9>(9,9) = MD(9,9)::Identity() * 0.00001; 88 | }; 89 | 90 | StatesGroup(const StatesGroup& b) { 91 | this->rot_end = b.rot_end; 92 | this->pos_end = b.pos_end; 93 | this->vel_end = b.vel_end; 94 | this->bias_g = b.bias_g; 95 | this->bias_a = b.bias_a; 96 | this->gravity = b.gravity; 97 | this->cov = b.cov; 98 | }; 99 | 100 | StatesGroup& operator=(const StatesGroup& b) 101 | { 102 | this->rot_end = b.rot_end; 103 | this->pos_end = b.pos_end; 104 | this->vel_end = b.vel_end; 105 | this->bias_g = b.bias_g; 106 | this->bias_a = b.bias_a; 107 | this->gravity = b.gravity; 108 | this->cov = b.cov; 109 | return *this; 110 | }; 111 | 112 | StatesGroup operator+(const Matrix &state_add) 113 | { 114 | StatesGroup a; 115 | a.rot_end = this->rot_end * Exp(state_add(0,0), state_add(1,0), state_add(2,0)); 116 | a.pos_end = this->pos_end + state_add.block<3,1>(3,0); 117 | a.vel_end = this->vel_end + state_add.block<3,1>(6,0); 118 | a.bias_g = this->bias_g + state_add.block<3,1>(9,0); 119 | a.bias_a = this->bias_a + state_add.block<3,1>(12,0); 120 | a.gravity = this->gravity + state_add.block<3,1>(15,0); 121 | a.cov = this->cov; 122 | return a; 123 | }; 124 | 125 | StatesGroup& operator+=(const Matrix &state_add) 126 | { 127 | this->rot_end = this->rot_end * Exp(state_add(0,0), state_add(1,0), state_add(2,0)); 128 | this->pos_end += state_add.block<3,1>(3,0); 129 | this->vel_end += state_add.block<3,1>(6,0); 130 | this->bias_g += state_add.block<3,1>(9,0); 131 | this->bias_a += state_add.block<3,1>(12,0); 132 | this->gravity += state_add.block<3,1>(15,0); 133 | return *this; 134 | }; 135 | 136 | Matrix operator-(const StatesGroup& b) 137 | { 138 | Matrix a; 139 | M3D rotd(b.rot_end.transpose() * this->rot_end); 140 | a.block<3,1>(0,0) = Log(rotd); 141 | a.block<3,1>(3,0) = this->pos_end - b.pos_end; 142 | a.block<3,1>(6,0) = this->vel_end - b.vel_end; 143 | a.block<3,1>(9,0) = this->bias_g - b.bias_g; 144 | a.block<3,1>(12,0) = this->bias_a - b.bias_a; 145 | a.block<3,1>(15,0) = this->gravity - b.gravity; 146 | return a; 147 | }; 148 | 149 | void resetpose() 150 | { 151 | this->rot_end = M3D::Identity(); 152 | this->pos_end = Zero3d; 153 | this->vel_end = Zero3d; 154 | } 155 | 156 | M3D rot_end; // the estimated attitude (rotation matrix) at the end lidar point 157 | V3D pos_end; // the estimated position at the end lidar point (world frame) 158 | V3D vel_end; // the estimated velocity at the end lidar point (world frame) 159 | V3D bias_g; // gyroscope bias 160 | V3D bias_a; // accelerator bias 161 | V3D gravity; // the estimated gravity acceleration 162 | Matrix cov; // states covariance 163 | }; 164 | 165 | template 166 | T rad2deg(T radians) 167 | { 168 | return radians * 180.0 / PI_M; 169 | } 170 | 171 | template 172 | T deg2rad(T degrees) 173 | { 174 | return degrees * PI_M / 180.0; 175 | } 176 | 177 | template 178 | auto set_pose6d(const double t, const Matrix &a, const Matrix &g, \ 179 | const Matrix &v, const Matrix &p, const Matrix &R) 180 | { 181 | Pose6D rot_kp; 182 | rot_kp.offset_time = t; 183 | for (int i = 0; i < 3; i++) 184 | { 185 | rot_kp.acc[i] = a(i); 186 | rot_kp.gyr[i] = g(i); 187 | rot_kp.vel[i] = v(i); 188 | rot_kp.pos[i] = p(i); 189 | for (int j = 0; j < 3; j++) rot_kp.rot[i*3+j] = R(i,j); 190 | } 191 | return move(rot_kp); 192 | } 193 | 194 | /* comment 195 | plane equation: Ax + By + Cz + D = 0 196 | convert to: A/D*x + B/D*y + C/D*z = -1 197 | solve: A0*x0 = b0 198 | where A0_i = [x_i, y_i, z_i], x0 = [A/D, B/D, C/D]^T, b0 = [-1, ..., -1]^T 199 | normvec: normalized x0 200 | */ 201 | template 202 | bool esti_normvector(Matrix &normvec, const PointVector &point, const T &threshold, const int &point_num) 203 | { 204 | MatrixXf A(point_num, 3); 205 | MatrixXf b(point_num, 1); 206 | b.setOnes(); 207 | b *= -1.0f; 208 | 209 | for (int j = 0; j < point_num; j++) 210 | { 211 | A(j,0) = point[j].x; 212 | A(j,1) = point[j].y; 213 | A(j,2) = point[j].z; 214 | } 215 | normvec = A.colPivHouseholderQr().solve(b); 216 | 217 | for (int j = 0; j < point_num; j++) 218 | { 219 | if (fabs(normvec(0) * point[j].x + normvec(1) * point[j].y + normvec(2) * point[j].z + 1.0f) > threshold) 220 | { 221 | return false; 222 | } 223 | } 224 | 225 | normvec.normalize(); 226 | return true; 227 | } 228 | 229 | float calc_dist(PointType p1, PointType p2){ 230 | float d = (p1.x - p2.x) * (p1.x - p2.x) + (p1.y - p2.y) * (p1.y - p2.y) + (p1.z - p2.z) * (p1.z - p2.z); 231 | return d; 232 | } 233 | 234 | template 235 | bool esti_plane(Matrix &pca_result, const PointVector &point, const T &threshold) 236 | { 237 | Matrix A; 238 | Matrix b; 239 | A.setZero(); 240 | b.setOnes(); 241 | b *= -1.0f; 242 | 243 | for (int j = 0; j < NUM_MATCH_POINTS; j++) 244 | { 245 | A(j,0) = point[j].x; 246 | A(j,1) = point[j].y; 247 | A(j,2) = point[j].z; 248 | } 249 | 250 | Matrix normvec = A.colPivHouseholderQr().solve(b); 251 | 252 | T n = normvec.norm(); 253 | pca_result(0) = normvec(0) / n; 254 | pca_result(1) = normvec(1) / n; 255 | pca_result(2) = normvec(2) / n; 256 | pca_result(3) = 1.0 / n; 257 | 258 | for (int j = 0; j < NUM_MATCH_POINTS; j++) 259 | { 260 | if (fabs(pca_result(0) * point[j].x + pca_result(1) * point[j].y + pca_result(2) * point[j].z + pca_result(3)) > threshold) 261 | { 262 | return false; 263 | } 264 | } 265 | return true; 266 | } 267 | 268 | static Eigen::Vector3d R2ypr(const Eigen::Matrix3d &R) 269 | { 270 | Eigen::Vector3d n = R.col(0); 271 | Eigen::Vector3d o = R.col(1); 272 | Eigen::Vector3d a = R.col(2); 273 | 274 | Eigen::Vector3d ypr(3); 275 | double y = atan2(n(1), n(0)); 276 | double p = atan2(-n(2), n(0) * cos(y) + n(1) * sin(y)); 277 | double r = atan2(a(0) * sin(y) - a(1) * cos(y), -o(0) * sin(y) + o(1) * cos(y)); 278 | ypr(0) = y; 279 | ypr(1) = p; 280 | ypr(2) = r; 281 | 282 | return ypr / M_PI * 180.0; 283 | } 284 | 285 | template 286 | static Eigen::Matrix ypr2R(const Eigen::MatrixBase &ypr) 287 | { 288 | typedef typename Derived::Scalar Scalar_t; 289 | 290 | Scalar_t y = ypr(0) / 180.0 * M_PI; 291 | Scalar_t p = ypr(1) / 180.0 * M_PI; 292 | Scalar_t r = ypr(2) / 180.0 * M_PI; 293 | 294 | Eigen::Matrix Rz; 295 | Rz << cos(y), -sin(y), 0, 296 | sin(y), cos(y), 0, 297 | 0, 0, 1; 298 | 299 | Eigen::Matrix Ry; 300 | Ry << cos(p), 0., sin(p), 301 | 0., 1., 0., 302 | -sin(p), 0., cos(p); 303 | 304 | Eigen::Matrix Rx; 305 | Rx << 1., 0., 0., 306 | 0., cos(r), -sin(r), 307 | 0., sin(r), cos(r); 308 | 309 | return Rz * Ry * Rx; 310 | } 311 | 312 | Eigen::Matrix3d g2R(const Eigen::Vector3d &g) 313 | { 314 | Eigen::Matrix3d R0; 315 | Eigen::Vector3d ng1 = g.normalized(); 316 | Eigen::Vector3d ng2{0, 0, 1.0}; 317 | R0 = Eigen::Quaterniond::FromTwoVectors(ng1, ng2).toRotationMatrix(); 318 | double yaw = R2ypr(R0).x(); 319 | R0 = ypr2R(Eigen::Vector3d{-yaw, 0, 0}) * R0; 320 | // R0 = Utility::ypr2R(Eigen::Vector3d{-90, 0, 0}) * R0; 321 | return R0; 322 | } 323 | 324 | #endif -------------------------------------------------------------------------------- /include/point_type.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by hk on 12/8/22. 3 | // 4 | 5 | #pragma once 6 | #define PCL_NO_PRECOMPILE 7 | 8 | #include 9 | #include 10 | #include 11 | 12 | #ifndef LIO_POINT_TYPE_H 13 | #define LIO_POINT_TYPE_H 14 | 15 | struct voxelMap_PointType 16 | { 17 | PCL_ADD_POINT4D 18 | union { 19 | struct { 20 | float eigenvector_max_x; 21 | float eigenvector_max_y; 22 | float eigenvector_max_z; 23 | float eigenvalue_max; 24 | }; 25 | float eigen_max[4]; 26 | }; 27 | 28 | union { 29 | struct { 30 | float eigenvector_mid_x; 31 | float eigenvector_mid_y; 32 | float eigenvector_mid_z; 33 | float eigenvalue_mid; 34 | }; 35 | float eigen_mid[4]; 36 | }; 37 | 38 | union { 39 | struct { 40 | float eigenvector_min_x; 41 | float eigenvector_min_y; 42 | float eigenvector_min_z; 43 | float eigenvalue_min; 44 | }; 45 | float eigen_min[4]; 46 | }; 47 | 48 | } EIGEN_ALIGN16; 49 | POINT_CLOUD_REGISTER_POINT_STRUCT (voxelMap_PointType, 50 | (float, x, x) (float, y, y) (float, z, z) 51 | (float, eigenvector_max_x, eigenvector_max_x) (float, eigenvector_max_y, eigenvector_max_y) 52 | (float, eigenvector_max_z, eigenvector_max_z) (float, eigenvalue_max, eigenvalue_max) 53 | (float, eigenvector_mid_x, eigenvector_mid_x) (float, eigenvector_mid_y, eigenvector_mid_y) 54 | (float, eigenvector_mid_z, eigenvector_mid_z) (float, eigenvalue_mid, eigenvalue_mid) 55 | (float, eigenvector_min_x, eigenvector_min_x) (float, eigenvector_min_y, eigenvector_min_y) 56 | (float, eigenvector_min_z, eigenvector_min_z) (float, eigenvalue_min, eigenvalue_min) 57 | ) 58 | 59 | struct custom_PointType 60 | { 61 | PCL_ADD_POINT4D 62 | PCL_ADD_NORMAL4D; 63 | union { 64 | struct { 65 | float intensity; 66 | float curvature; 67 | float n_xx; // normal cov element 68 | float n_xy; 69 | }; 70 | float data_c[4]; 71 | }; 72 | 73 | union { 74 | struct { 75 | float n_xz; 76 | float n_yy; 77 | float n_yz; 78 | float n_zz; 79 | }; 80 | float data_s[4]; 81 | }; 82 | void getNormalCov(Eigen::Matrix3f & m) const 83 | { 84 | m(0, 0) = n_xx; m(0, 1) = n_xy; m(0, 2) = n_xz; 85 | m(1, 0) = n_xy; m(1, 1) = n_yy; m(1, 2) = n_yz; 86 | m(2, 0) = n_xz; m(2, 1) = n_yz; m(2, 2) = n_zz; 87 | } 88 | void recordNormalCov(const Eigen::Matrix3f & m) 89 | { 90 | n_xx = m(0, 0); n_xy = m(0, 1); n_xz = m(0, 2); 91 | n_yy = m(1, 1); n_yz = m(1, 2); n_zz = m(2, 2); 92 | } 93 | } EIGEN_ALIGN16; 94 | 95 | POINT_CLOUD_REGISTER_POINT_STRUCT (custom_PointType, 96 | (float, x, x) (float, y, y) (float, z, z) 97 | (float, normal_x, normal_x) (float, normal_y, normal_y) (float, normal_z, normal_z) 98 | (float, intensity, intensity) (float, curvature, curvature) (float, n_xx, n_xx) (float, n_xy, n_xy) 99 | (float, n_xz, n_xz) (float, n_yy, n_yy) (float, n_yz, n_yz) (float, n_zz, n_zz) 100 | ) 101 | 102 | // Velodyne 103 | struct PointXYZIRT 104 | { 105 | PCL_ADD_POINT4D 106 | PCL_ADD_INTENSITY; 107 | uint16_t ring; 108 | float time; 109 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 110 | } EIGEN_ALIGN16; 111 | 112 | POINT_CLOUD_REGISTER_POINT_STRUCT (PointXYZIRT, 113 | (float, x, x) (float, y, y) (float, z, z) 114 | (float, intensity, intensity) 115 | (uint16_t, ring, ring) 116 | (float, time, time) 117 | ) 118 | 119 | // Kitti odometry sequence without time 120 | struct PointXYZIR 121 | { 122 | PCL_ADD_POINT4D 123 | PCL_ADD_INTENSITY; 124 | uint16_t ring; 125 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 126 | } EIGEN_ALIGN16; 127 | 128 | POINT_CLOUD_REGISTER_POINT_STRUCT (PointXYZIR, 129 | (float, x, x) (float, y, y) (float, z, z) 130 | (float, intensity, intensity) 131 | (uint16_t, ring, ring) 132 | ) 133 | 134 | // Ouster 135 | struct ousterPointXYZIRT { 136 | PCL_ADD_POINT4D; 137 | PCL_ADD_INTENSITY; 138 | uint32_t t; 139 | uint16_t reflectivity; 140 | uint8_t ring; 141 | uint16_t noise; 142 | uint32_t range; 143 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 144 | }EIGEN_ALIGN16; 145 | 146 | POINT_CLOUD_REGISTER_POINT_STRUCT(ousterPointXYZIRT, 147 | (float, x, x) (float, y, y) (float, z, z) (float, intensity, intensity) 148 | (uint32_t, t, t) (uint16_t, reflectivity, reflectivity) 149 | (uint8_t, ring, ring) (uint16_t, noise, noise) (uint32_t, range, range) 150 | ) 151 | 152 | 153 | #endif //LIO_POINT_TYPE_H 154 | -------------------------------------------------------------------------------- /include/so3_math.h: -------------------------------------------------------------------------------- 1 | #ifndef SO3_MATH_H 2 | #define SO3_MATH_H 3 | 4 | #include 5 | #include 6 | 7 | #define SKEW_SYM_MATRX(v) 0.0,-v[2],v[1],v[2],0.0,-v[0],-v[1],v[0],0.0 8 | 9 | template 10 | Eigen::Matrix skew_sym_mat(const Eigen::Matrix &v) 11 | { 12 | Eigen::Matrix skew_sym_mat; 13 | skew_sym_mat<<0.0,-v[2],v[1],v[2],0.0,-v[0],-v[1],v[0],0.0; 14 | return skew_sym_mat; 15 | } 16 | 17 | template 18 | Eigen::Matrix Exp(const Eigen::Matrix &&ang) 19 | { 20 | T ang_norm = ang.norm(); 21 | Eigen::Matrix Eye3 = Eigen::Matrix::Identity(); 22 | if (ang_norm > 0.0000001) 23 | { 24 | Eigen::Matrix r_axis = ang / ang_norm; 25 | Eigen::Matrix K; 26 | K << SKEW_SYM_MATRX(r_axis); 27 | /// Roderigous Tranformation 28 | return Eye3 + std::sin(ang_norm) * K + (1.0 - std::cos(ang_norm)) * K * K; 29 | } 30 | else 31 | { 32 | return Eye3; 33 | } 34 | } 35 | 36 | template 37 | Eigen::Matrix Exp(const Eigen::Matrix &ang_vel, const Ts &dt) 38 | { 39 | T ang_vel_norm = ang_vel.norm(); 40 | Eigen::Matrix Eye3 = Eigen::Matrix::Identity(); 41 | 42 | if (ang_vel_norm > 0.0000001) 43 | { 44 | Eigen::Matrix r_axis = ang_vel / ang_vel_norm; 45 | Eigen::Matrix K; 46 | 47 | K << SKEW_SYM_MATRX(r_axis); 48 | 49 | T r_ang = ang_vel_norm * dt; 50 | 51 | /// Roderigous Tranformation 52 | return Eye3 + std::sin(r_ang) * K + (1.0 - std::cos(r_ang)) * K * K; 53 | } 54 | else 55 | { 56 | return Eye3; 57 | } 58 | } 59 | 60 | template 61 | Eigen::Matrix Exp(const T &v1, const T &v2, const T &v3) 62 | { 63 | T &&norm = sqrt(v1 * v1 + v2 * v2 + v3 * v3); 64 | Eigen::Matrix Eye3 = Eigen::Matrix::Identity(); 65 | if (norm > 0.00001) 66 | { 67 | T r_ang[3] = {v1 / norm, v2 / norm, v3 / norm}; 68 | Eigen::Matrix K; 69 | K << SKEW_SYM_MATRX(r_ang); 70 | 71 | /// Roderigous Tranformation 72 | return Eye3 + std::sin(norm) * K + (1.0 - std::cos(norm)) * K * K; 73 | } 74 | else 75 | { 76 | return Eye3; 77 | } 78 | } 79 | 80 | /* Logrithm of a Rotation Matrix */ 81 | template 82 | Eigen::Matrix Log(const Eigen::Matrix &R) 83 | { 84 | T theta = (R.trace() > 3.0 - 1e-6) ? 0.0 : std::acos(0.5 * (R.trace() - 1)); 85 | Eigen::Matrix K(R(2,1) - R(1,2), R(0,2) - R(2,0), R(1,0) - R(0,1)); 86 | return (std::abs(theta) < 0.001) ? (0.5 * K) : (0.5 * theta / std::sin(theta) * K); 87 | } 88 | 89 | template 90 | Eigen::Matrix RotMtoEuler(const Eigen::Matrix &rot) 91 | { 92 | T sy = sqrt(rot(0,0)*rot(0,0) + rot(1,0)*rot(1,0)); 93 | bool singular = sy < 1e-6; 94 | T x, y, z; 95 | if(!singular) 96 | { 97 | x = atan2(rot(2, 1), rot(2, 2)); 98 | y = atan2(-rot(2, 0), sy); 99 | z = atan2(rot(1, 0), rot(0, 0)); 100 | } 101 | else 102 | { 103 | x = atan2(-rot(1, 2), rot(1, 1)); 104 | y = atan2(-rot(2, 0), sy); 105 | z = 0; 106 | } 107 | Eigen::Matrix ang(x, y, z); 108 | return ang; 109 | } 110 | 111 | #endif 112 | -------------------------------------------------------------------------------- /include/use-ikfom.hpp: -------------------------------------------------------------------------------- 1 | #ifndef USE_IKFOM_H 2 | #define USE_IKFOM_H 3 | 4 | #include 5 | 6 | typedef MTK::vect<3, double> vect3; 7 | typedef MTK::SO3 SO3; 8 | typedef MTK::S2 S2; 9 | typedef MTK::vect<1, double> vect1; 10 | typedef MTK::vect<2, double> vect2; 11 | 12 | MTK_BUILD_MANIFOLD(state_ikfom, 13 | ((vect3, pos)) 14 | ((SO3, rot)) 15 | ((SO3, offset_R_L_I)) 16 | ((vect3, offset_T_L_I)) 17 | ((vect3, vel)) 18 | ((vect3, bg)) 19 | ((vect3, ba)) 20 | ((S2, grav)) 21 | ); 22 | 23 | MTK_BUILD_MANIFOLD(input_ikfom, 24 | ((vect3, acc)) 25 | ((vect3, gyro)) 26 | ); 27 | 28 | MTK_BUILD_MANIFOLD(process_noise_ikfom, 29 | ((vect3, ng)) 30 | ((vect3, na)) 31 | ((vect3, nbg)) 32 | ((vect3, nba)) 33 | ); 34 | 35 | MTK::get_cov::type process_noise_cov() 36 | { 37 | MTK::get_cov::type cov = MTK::get_cov::type::Zero(); 38 | MTK::setDiagonal(cov, &process_noise_ikfom::ng, 0.0001);// 0.03 39 | MTK::setDiagonal(cov, &process_noise_ikfom::na, 0.0001); // *dt 0.01 0.01 * dt * dt 0.05 40 | MTK::setDiagonal(cov, &process_noise_ikfom::nbg, 0.00001); // *dt 0.00001 0.00001 * dt *dt 0.3 //0.001 0.0001 0.01 41 | MTK::setDiagonal(cov, &process_noise_ikfom::nba, 0.00001); //0.001 0.05 0.0001/out 0.01 42 | return cov; 43 | } 44 | 45 | //double L_offset_to_I[3] = {0.04165, 0.02326, -0.0284}; // Avia 46 | //vect3 Lidar_offset_to_IMU(L_offset_to_I, 3); 47 | Eigen::Matrix get_f(state_ikfom &s, const input_ikfom &in) 48 | { 49 | Eigen::Matrix res = Eigen::Matrix::Zero(); 50 | vect3 omega; 51 | in.gyro.boxminus(omega, s.bg); 52 | vect3 a_inertial = s.rot * (in.acc-s.ba); 53 | for(int i = 0; i < 3; i++ ){ 54 | res(i) = s.vel[i]; 55 | res(i + 3) = omega[i]; 56 | res(i + 12) = a_inertial[i] + s.grav[i]; 57 | } 58 | return res; 59 | } 60 | 61 | Eigen::Matrix df_dx(state_ikfom &s, const input_ikfom &in) 62 | { 63 | Eigen::Matrix cov = Eigen::Matrix::Zero(); 64 | cov.template block<3, 3>(0, 12) = Eigen::Matrix3d::Identity(); 65 | vect3 acc_; 66 | in.acc.boxminus(acc_, s.ba); 67 | vect3 omega; 68 | in.gyro.boxminus(omega, s.bg); 69 | cov.template block<3, 3>(12, 3) = -s.rot.toRotationMatrix()*MTK::hat(acc_); 70 | cov.template block<3, 3>(12, 18) = -s.rot.toRotationMatrix(); 71 | Eigen::Matrix vec = Eigen::Matrix::Zero(); 72 | Eigen::Matrix grav_matrix; 73 | s.S2_Mx(grav_matrix, vec, 21); 74 | cov.template block<3, 2>(12, 21) = grav_matrix; 75 | cov.template block<3, 3>(3, 15) = -Eigen::Matrix3d::Identity(); 76 | return cov; 77 | } 78 | 79 | 80 | Eigen::Matrix df_dw(state_ikfom &s, const input_ikfom &in) 81 | { 82 | Eigen::Matrix cov = Eigen::Matrix::Zero(); 83 | cov.template block<3, 3>(12, 3) = -s.rot.toRotationMatrix(); 84 | cov.template block<3, 3>(3, 0) = -Eigen::Matrix3d::Identity(); 85 | cov.template block<3, 3>(15, 6) = Eigen::Matrix3d::Identity(); 86 | cov.template block<3, 3>(18, 9) = Eigen::Matrix3d::Identity(); 87 | return cov; 88 | } 89 | 90 | vect3 SO3ToEuler(const SO3 &orient) 91 | { 92 | Eigen::Matrix _ang; 93 | Eigen::Vector4d q_data = orient.coeffs().transpose(); 94 | //scalar w=orient.coeffs[3], x=orient.coeffs[0], y=orient.coeffs[1], z=orient.coeffs[2]; 95 | double sqw = q_data[3]*q_data[3]; 96 | double sqx = q_data[0]*q_data[0]; 97 | double sqy = q_data[1]*q_data[1]; 98 | double sqz = q_data[2]*q_data[2]; 99 | double unit = sqx + sqy + sqz + sqw; // if normalized is one, otherwise is correction factor 100 | double test = q_data[3]*q_data[1] - q_data[2]*q_data[0]; 101 | 102 | if (test > 0.49999*unit) { // singularity at north pole 103 | 104 | _ang << 2 * std::atan2(q_data[0], q_data[3]), M_PI/2, 0; 105 | double temp[3] = {_ang[0] * 57.3, _ang[1] * 57.3, _ang[2] * 57.3}; 106 | vect3 euler_ang(temp, 3); 107 | return euler_ang; 108 | } 109 | if (test < -0.49999*unit) { // singularity at south pole 110 | _ang << -2 * std::atan2(q_data[0], q_data[3]), -M_PI/2, 0; 111 | double temp[3] = {_ang[0] * 57.3, _ang[1] * 57.3, _ang[2] * 57.3}; 112 | vect3 euler_ang(temp, 3); 113 | return euler_ang; 114 | } 115 | 116 | _ang << 117 | std::atan2(2*q_data[0]*q_data[3]+2*q_data[1]*q_data[2] , -sqx - sqy + sqz + sqw), 118 | std::asin (2*test/unit), 119 | std::atan2(2*q_data[2]*q_data[3]+2*q_data[1]*q_data[0] , sqx - sqy - sqz + sqw); 120 | double temp[3] = {_ang[0] * 57.3, _ang[1] * 57.3, _ang[2] * 57.3}; 121 | vect3 euler_ang(temp, 3); 122 | // euler_ang[0] = roll, euler_ang[1] = pitch, euler_ang[2] = yaw 123 | return euler_ang; 124 | } 125 | 126 | #endif -------------------------------------------------------------------------------- /launch/mapping_m2dgr.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | -------------------------------------------------------------------------------- /launch/mapping_newer_college.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 16 | 17 | 18 | -------------------------------------------------------------------------------- /launch/mapping_viral.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 16 | 17 | 18 | -------------------------------------------------------------------------------- /launch/rviz_voxel_map.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 17 | 18 | 19 | -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | log_lio 4 | 0.0.0 5 | 6 | 7 | This is a modified version of LOAM which is original algorithm 8 | is described in the following paper: 9 | J. Zhang and S. Singh. LOAM: Lidar Odometry and Mapping in Real-time. 10 | Robotics: Science and Systems Conference (RSS). Berkeley, CA, July 2014. 11 | 12 | 13 | claydergc 14 | 15 | BSD 16 | 17 | Ji Zhang 18 | 19 | catkin 20 | geometry_msgs 21 | nav_msgs 22 | roscpp 23 | rospy 24 | std_msgs 25 | sensor_msgs 26 | tf 27 | pcl_ros 28 | livox_ros_driver 29 | message_generation 30 | 31 | geometry_msgs 32 | nav_msgs 33 | sensor_msgs 34 | roscpp 35 | rospy 36 | std_msgs 37 | tf 38 | pcl_ros 39 | livox_ros_driver 40 | message_runtime 41 | 42 | rostest 43 | rosbag 44 | 45 | 46 | 47 | 48 | -------------------------------------------------------------------------------- /src/IMU_Processing.hpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | #include 24 | #include 25 | #include "use-ikfom.hpp" 26 | 27 | /// *************Preconfiguration 28 | 29 | #define MAX_INI_COUNT (20) 30 | 31 | const bool time_list(PointType &x, PointType &y) {return (x.curvature < y.curvature);}; 32 | 33 | /// *************IMU Process and undistortion 34 | class ImuProcess 35 | { 36 | public: 37 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 38 | 39 | ImuProcess(); 40 | ~ImuProcess(); 41 | 42 | void Reset(); 43 | void Reset(double start_timestamp, const sensor_msgs::ImuConstPtr &lastimu); 44 | void set_extrinsic(const V3D &transl, const M3D &rot); 45 | void set_extrinsic(const V3D &transl); 46 | void set_extrinsic(const MD(4,4) &T); 47 | void set_gyr_cov(const V3D &scaler); 48 | void set_acc_cov(const V3D &scaler); 49 | void set_gyr_bias_cov(const V3D &b_g); 50 | void set_acc_bias_cov(const V3D &b_a); 51 | Eigen::Matrix Q; 52 | void Process(const MeasureGroup &meas, esekfom::esekf &kf_state, PointCloudXYZI::Ptr pcl_un_); 53 | 54 | ofstream fout_imu; 55 | V3D cov_acc; 56 | V3D cov_gyr; 57 | V3D cov_acc_scale; 58 | V3D cov_gyr_scale; 59 | V3D cov_bias_gyr; 60 | V3D cov_bias_acc; 61 | double first_lidar_time; 62 | 63 | SO3 Initial_R_wrt_G; 64 | 65 | int lidar_type = 0; 66 | private: 67 | void IMU_init(const MeasureGroup &meas, esekfom::esekf &kf_state, int &N); 68 | void UndistortPcl(const MeasureGroup &meas, esekfom::esekf &kf_state, PointCloudXYZI &pcl_in_out); 69 | 70 | PointCloudXYZI::Ptr cur_pcl_un_; 71 | sensor_msgs::ImuConstPtr last_imu_; 72 | deque v_imu_; 73 | vector IMUpose; 74 | vector v_rot_pcl_; 75 | M3D Lidar_R_wrt_IMU; 76 | V3D Lidar_T_wrt_IMU; 77 | V3D mean_acc; 78 | V3D mean_gyr; 79 | V3D angvel_last; 80 | V3D acc_s_last; 81 | double start_timestamp_; 82 | double last_lidar_end_time_; 83 | int init_iter_num = 1; 84 | bool b_first_frame_ = true; 85 | bool imu_need_init_ = true; 86 | }; 87 | 88 | ImuProcess::ImuProcess() 89 | : b_first_frame_(true), imu_need_init_(true), start_timestamp_(-1) 90 | { 91 | init_iter_num = 1; 92 | Q = process_noise_cov(); 93 | cov_acc = V3D(0.1, 0.1, 0.1); 94 | cov_gyr = V3D(0.1, 0.1, 0.1); 95 | cov_bias_gyr = V3D(0.0001, 0.0001, 0.0001); 96 | cov_bias_acc = V3D(0.0001, 0.0001, 0.0001); 97 | mean_acc = V3D(0, 0, -1.0); 98 | mean_gyr = V3D(0, 0, 0); 99 | angvel_last = Zero3d; 100 | Lidar_T_wrt_IMU = Zero3d; 101 | Lidar_R_wrt_IMU = Eye3d; 102 | last_imu_.reset(new sensor_msgs::Imu()); 103 | } 104 | 105 | ImuProcess::~ImuProcess() {} 106 | 107 | void ImuProcess::Reset() 108 | { 109 | // ROS_WARN("Reset ImuProcess"); 110 | mean_acc = V3D(0, 0, -1.0); 111 | mean_gyr = V3D(0, 0, 0); 112 | angvel_last = Zero3d; 113 | imu_need_init_ = true; 114 | start_timestamp_ = -1; 115 | init_iter_num = 1; 116 | v_imu_.clear(); 117 | IMUpose.clear(); 118 | last_imu_.reset(new sensor_msgs::Imu()); 119 | cur_pcl_un_.reset(new PointCloudXYZI()); 120 | } 121 | 122 | void ImuProcess::set_extrinsic(const MD(4,4) &T) 123 | { 124 | Lidar_T_wrt_IMU = T.block<3,1>(0,3); 125 | Lidar_R_wrt_IMU = T.block<3,3>(0,0); 126 | } 127 | 128 | void ImuProcess::set_extrinsic(const V3D &transl) 129 | { 130 | Lidar_T_wrt_IMU = transl; 131 | Lidar_R_wrt_IMU.setIdentity(); 132 | } 133 | 134 | void ImuProcess::set_extrinsic(const V3D &transl, const M3D &rot) 135 | { 136 | Lidar_T_wrt_IMU = transl; 137 | Lidar_R_wrt_IMU = rot; 138 | } 139 | 140 | void ImuProcess::set_gyr_cov(const V3D &scaler) 141 | { 142 | cov_gyr_scale = scaler; 143 | } 144 | 145 | void ImuProcess::set_acc_cov(const V3D &scaler) 146 | { 147 | cov_acc_scale = scaler; 148 | } 149 | 150 | void ImuProcess::set_gyr_bias_cov(const V3D &b_g) 151 | { 152 | cov_bias_gyr = b_g; 153 | } 154 | 155 | void ImuProcess::set_acc_bias_cov(const V3D &b_a) 156 | { 157 | cov_bias_acc = b_a; 158 | } 159 | 160 | void ImuProcess::IMU_init(const MeasureGroup &meas, esekfom::esekf &kf_state, int &N) 161 | { 162 | /** 1. initializing the gravity, gyro bias, acc and gyro covariance 163 | ** 2. normalize the acceleration measurenments to unit gravity **/ 164 | 165 | V3D cur_acc, cur_gyr; 166 | 167 | if (b_first_frame_) 168 | { 169 | Reset(); 170 | N = 1; 171 | b_first_frame_ = false; 172 | const auto &imu_acc = meas.imu.front()->linear_acceleration; 173 | const auto &gyr_acc = meas.imu.front()->angular_velocity; 174 | mean_acc << imu_acc.x, imu_acc.y, imu_acc.z; 175 | mean_gyr << gyr_acc.x, gyr_acc.y, gyr_acc.z; 176 | first_lidar_time = meas.lidar_beg_time; 177 | } 178 | 179 | for (const auto &imu : meas.imu) 180 | { 181 | const auto &imu_acc = imu->linear_acceleration; 182 | const auto &gyr_acc = imu->angular_velocity; 183 | cur_acc << imu_acc.x, imu_acc.y, imu_acc.z; 184 | cur_gyr << gyr_acc.x, gyr_acc.y, gyr_acc.z; 185 | 186 | mean_acc += (cur_acc - mean_acc) / N; 187 | mean_gyr += (cur_gyr - mean_gyr) / N; 188 | 189 | cov_acc = cov_acc * (N - 1.0) / N + (cur_acc - mean_acc).cwiseProduct(cur_acc - mean_acc) * (N - 1.0) / (N * N); 190 | cov_gyr = cov_gyr * (N - 1.0) / N + (cur_gyr - mean_gyr).cwiseProduct(cur_gyr - mean_gyr) * (N - 1.0) / (N * N); 191 | 192 | // cout<<"acc norm: "<::cov init_P = kf_state.get_P(); 209 | init_P.setIdentity(); 210 | // init_P.setZero(); 211 | init_P(6,6) = init_P(7,7) = init_P(8,8) = 0.00001; 212 | init_P(9,9) = init_P(10,10) = init_P(11,11) = 0.00001; 213 | init_P(15,15) = init_P(16,16) = init_P(17,17) = 0.0001; 214 | init_P(18,18) = init_P(19,19) = init_P(20,20) = 0.001; 215 | init_P(21,21) = init_P(22,22) = 0.00001; 216 | kf_state.change_P(init_P); 217 | last_imu_ = meas.imu.back(); 218 | 219 | } 220 | 221 | void ImuProcess::UndistortPcl(const MeasureGroup &meas, esekfom::esekf &kf_state, PointCloudXYZI &pcl_out) 222 | { 223 | /*** add the imu of the last frame-tail to the of current frame-head ***/ 224 | auto v_imu = meas.imu; 225 | v_imu.push_front(last_imu_); 226 | const double &imu_beg_time = v_imu.front()->header.stamp.toSec(); 227 | const double &imu_end_time = v_imu.back()->header.stamp.toSec(); 228 | const double &pcl_beg_time = meas.lidar_beg_time; 229 | const double &pcl_end_time = meas.lidar_end_time; 230 | 231 | /*** sort point clouds by offset time ***/ 232 | pcl_out = *(meas.lidar); 233 | sort(pcl_out.points.begin(), pcl_out.points.end(), time_list); 234 | // cout<<"[ IMU Process ]: Process lidar from "<header.stamp.toSec() < last_lidar_end_time_) continue; 255 | 256 | angvel_avr<<0.5 * (head->angular_velocity.x + tail->angular_velocity.x), 257 | 0.5 * (head->angular_velocity.y + tail->angular_velocity.y), 258 | 0.5 * (head->angular_velocity.z + tail->angular_velocity.z); 259 | acc_avr <<0.5 * (head->linear_acceleration.x + tail->linear_acceleration.x), 260 | 0.5 * (head->linear_acceleration.y + tail->linear_acceleration.y), 261 | 0.5 * (head->linear_acceleration.z + tail->linear_acceleration.z); 262 | 263 | // fout_imu << setw(10) << head->header.stamp.toSec() - first_lidar_time << " " << angvel_avr.transpose() << " " << acc_avr.transpose() << endl; 264 | 265 | acc_avr = acc_avr * G_m_s2 / mean_acc.norm(); // - state_inout.ba; 266 | 267 | if(head->header.stamp.toSec() < last_lidar_end_time_) 268 | { 269 | dt = tail->header.stamp.toSec() - last_lidar_end_time_; 270 | // dt = tail->header.stamp.toSec() - pcl_beg_time; 271 | } 272 | else 273 | { 274 | dt = tail->header.stamp.toSec() - head->header.stamp.toSec(); 275 | } 276 | 277 | in.acc = acc_avr; 278 | in.gyro = angvel_avr; 279 | Q.block<3, 3>(0, 0).diagonal() = cov_gyr; 280 | Q.block<3, 3>(3, 3).diagonal() = cov_acc; 281 | Q.block<3, 3>(6, 6).diagonal() = cov_bias_gyr; 282 | Q.block<3, 3>(9, 9).diagonal() = cov_bias_acc; 283 | 284 | kf_state.predict(dt, Q, in); 285 | 286 | /* save the poses at each IMU measurements */ 287 | imu_state = kf_state.get_x(); 288 | angvel_last = angvel_avr - imu_state.bg; 289 | acc_s_last = imu_state.rot * (acc_avr - imu_state.ba); 290 | for(int i=0; i<3; i++) 291 | { 292 | acc_s_last[i] += imu_state.grav[i]; 293 | } 294 | double &&offs_t = tail->header.stamp.toSec() - pcl_beg_time; 295 | IMUpose.push_back(set_pose6d(offs_t, acc_s_last, angvel_last, imu_state.vel, imu_state.pos, imu_state.rot.toRotationMatrix())); 296 | } 297 | 298 | /*** calculated the pos and attitude prediction at the frame-end ***/ 299 | double note = pcl_end_time > imu_end_time ? 1.0 : -1.0; 300 | dt = note * (pcl_end_time - imu_end_time); 301 | kf_state.predict(dt, Q, in); 302 | 303 | imu_state = kf_state.get_x(); 304 | last_imu_ = meas.imu.back(); 305 | last_lidar_end_time_ = pcl_end_time; 306 | 307 | if (lidar_type == 4) //specify for kitti date which without 'time' 308 | return; 309 | /*** undistort each lidar point (backward propagation) ***/ 310 | auto it_pcl = pcl_out.points.end() - 1; 311 | for (auto it_kp = IMUpose.end() - 1; it_kp != IMUpose.begin(); it_kp--) 312 | { 313 | auto head = it_kp - 1; 314 | auto tail = it_kp; 315 | R_imu<rot); 316 | // cout<<"head imu acc: "<vel); 318 | pos_imu<pos); 319 | acc_imu<acc); 320 | angvel_avr<gyr); 321 | 322 | for(; it_pcl->curvature / double(1000) > head->offset_time; it_pcl --) 323 | { 324 | dt = it_pcl->curvature / double(1000) - head->offset_time; 325 | 326 | /* Transform to the 'end' frame, using only the rotation 327 | * Note: Compensation direction is INVERSE of Frame's moving direction 328 | * So if we want to compensate a point at timestamp-i to the frame-e 329 | * P_compensate = R_imu_e ^ T * (R_i * P_i + T_ei) where T_ei is represented in global frame */ 330 | M3D R_i(R_imu * Exp(angvel_avr, dt)); 331 | 332 | V3D P_i(it_pcl->x, it_pcl->y, it_pcl->z); 333 | V3D T_ei(pos_imu + vel_imu * dt + 0.5 * acc_imu * dt * dt - imu_state.pos); 334 | V3D P_compensate = imu_state.offset_R_L_I.conjugate() * (imu_state.rot.conjugate() * (R_i * (imu_state.offset_R_L_I * P_i + imu_state.offset_T_L_I) + T_ei) - imu_state.offset_T_L_I);// not accurate! 335 | 336 | // save Undistorted points and their rotation 337 | it_pcl->x = P_compensate(0); 338 | it_pcl->y = P_compensate(1); 339 | it_pcl->z = P_compensate(2); 340 | 341 | if (it_pcl == pcl_out.points.begin()) break; 342 | } 343 | } 344 | } 345 | 346 | void ImuProcess::Process(const MeasureGroup &meas, esekfom::esekf &kf_state, PointCloudXYZI::Ptr cur_pcl_un_) 347 | { 348 | double t1,t2,t3; 349 | t1 = omp_get_wtime(); 350 | 351 | if(meas.imu.empty()) {return;}; 352 | ROS_ASSERT(meas.lidar != nullptr); 353 | 354 | if (imu_need_init_) 355 | { 356 | /// The very first lidar frame 357 | IMU_init(meas, kf_state, init_iter_num); 358 | 359 | imu_need_init_ = true; 360 | 361 | last_imu_ = meas.imu.back(); 362 | 363 | state_ikfom imu_state = kf_state.get_x(); 364 | if (init_iter_num > MAX_INI_COUNT) 365 | { 366 | cov_acc *= pow(G_m_s2 / mean_acc.norm(), 2); 367 | imu_need_init_ = false; 368 | 369 | cov_acc = cov_acc_scale; 370 | cov_gyr = cov_gyr_scale; 371 | ROS_INFO("IMU Initial Done"); 372 | // ROS_INFO("IMU Initial Done: Gravity: %.4f %.4f %.4f %.4f; state.bias_g: %.4f %.4f %.4f; acc covarience: %.8f %.8f %.8f; gry covarience: %.8f %.8f %.8f",\ 373 | // imu_state.grav[0], imu_state.grav[1], imu_state.grav[2], mean_acc.norm(), cov_bias_gyr[0], cov_bias_gyr[1], cov_bias_gyr[2], cov_acc[0], cov_acc[1], cov_acc[2], cov_gyr[0], cov_gyr[1], cov_gyr[2]); 374 | fout_imu.open(DEBUG_FILE_DIR("imu.txt"),ios::out); 375 | } 376 | 377 | return; 378 | } 379 | 380 | UndistortPcl(meas, kf_state, *cur_pcl_un_); 381 | 382 | t2 = omp_get_wtime(); 383 | t3 = omp_get_wtime(); 384 | 385 | // cout<<"[ IMU Process ]: Time: "< 3 | 4 | #include 5 | 6 | #include 7 | #include 8 | #include 9 | 10 | std::string output_type; 11 | 12 | static int RING_ID_MAP_RUBY[] = { 13 | 3, 66, 33, 96, 11, 74, 41, 104, 19, 82, 49, 112, 27, 90, 57, 120, 14 | 35, 98, 1, 64, 43, 106, 9, 72, 51, 114, 17, 80, 59, 122, 25, 88, 15 | 67, 34, 97, 0, 75, 42, 105, 8, 83, 50, 113, 16, 91, 58, 121, 24, 16 | 99, 2, 65, 32, 107, 10, 73, 40, 115, 18, 81, 48, 123, 26, 89, 56, 17 | 7, 70, 37, 100, 15, 78, 45, 108, 23, 86, 53, 116, 31, 94, 61, 124, 18 | 39, 102, 5, 68, 47, 110, 13, 76, 55, 118, 21, 84, 63, 126, 29, 92, 19 | 71, 38, 101, 4, 79, 46, 109, 12, 87, 54, 117, 20, 95, 62, 125, 28, 20 | 103, 6, 69, 36, 111, 14, 77, 44, 119, 22, 85, 52, 127, 30, 93, 60 21 | }; 22 | static int RING_ID_MAP_16[] = { 23 | 0, 1, 2, 3, 4, 5, 6, 7, 15, 14, 13, 12, 11, 10, 9, 8 24 | }; 25 | 26 | // rslidar和velodyne的格式有微小的区别 27 | // rslidar的点云格式 28 | struct PandarPointXYZIRT { 29 | PCL_ADD_POINT4D //添加pcl里xyz 30 | float intensity; 31 | double timestamp; 32 | uint16_t ring; ///< laser ring number 33 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW // make sure our new allocators are aligned,确保定义新类型点云内存与SSE对齐 34 | } EIGEN_ALIGN16; // 强制SSE填充以正确对齐内存 35 | 36 | POINT_CLOUD_REGISTER_POINT_STRUCT( 37 | PandarPointXYZIRT, 38 | (float, x, x)(float, y, y)(float, z, z)(float, intensity, intensity)( 39 | double, timestamp, timestamp)(uint16_t, ring, ring)) 40 | 41 | // velodyne的点云格式 42 | struct VelodynePointXYZIRT { 43 | PCL_ADD_POINT4D 44 | 45 | PCL_ADD_INTENSITY; 46 | uint16_t ring; 47 | float time; 48 | 49 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 50 | } EIGEN_ALIGN16; 51 | 52 | POINT_CLOUD_REGISTER_POINT_STRUCT (VelodynePointXYZIRT, 53 | (float, x, x)(float, y, y)(float, z, z)(float, intensity, intensity) 54 | (uint16_t, ring, ring)(float, time, time) 55 | ) 56 | 57 | struct VelodynePointXYZIR { 58 | PCL_ADD_POINT4D 59 | 60 | PCL_ADD_INTENSITY; 61 | uint16_t ring; 62 | 63 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 64 | } EIGEN_ALIGN16; 65 | 66 | POINT_CLOUD_REGISTER_POINT_STRUCT (VelodynePointXYZIR, 67 | (float, x, x)(float, y, y) 68 | (float, z, z)(float, intensity, intensity) 69 | (uint16_t, ring, ring) 70 | ) 71 | 72 | ros::Subscriber subRobosensePC; 73 | ros::Publisher pubRobosensePC; 74 | 75 | template 76 | bool has_nan(T point) { 77 | 78 | // remove nan point, or the feature assocaion will crash, the surf point will containing nan points 79 | // pcl remove nan not work normally 80 | // ROS_ERROR("Containing nan point!"); 81 | if (pcl_isnan(point.x) || pcl_isnan(point.y) || pcl_isnan(point.z)) { 82 | return true; 83 | } else { 84 | return false; 85 | } 86 | } 87 | 88 | template 89 | void publish_points(T &new_pc, const sensor_msgs::PointCloud2 &old_msg, double update_timestamp = 0) { 90 | // pc properties 91 | new_pc->is_dense = true; 92 | 93 | // publish 94 | sensor_msgs::PointCloud2 pc_new_msg; 95 | pcl::toROSMsg(*new_pc, pc_new_msg); 96 | pc_new_msg.header = old_msg.header; 97 | pc_new_msg.header.frame_id = "velodyne"; 98 | 99 | if (update_timestamp != 0) { 100 | pc_new_msg.header.stamp = ros::Time().fromSec(update_timestamp); 101 | } 102 | 103 | pubRobosensePC.publish(pc_new_msg); 104 | } 105 | 106 | void rsHandler_XYZI(sensor_msgs::PointCloud2 pc_msg) { 107 | pcl::PointCloud::Ptr pc(new pcl::PointCloud()); 108 | pcl::PointCloud::Ptr pc_new(new pcl::PointCloud()); 109 | pcl::fromROSMsg(pc_msg, *pc); 110 | 111 | // to new pointcloud 112 | for (int point_id = 0; point_id < pc->points.size(); ++point_id) { 113 | if (has_nan(pc->points[point_id])) 114 | continue; 115 | 116 | VelodynePointXYZIR new_point; 117 | new_point.x = pc->points[point_id].x; 118 | new_point.y = pc->points[point_id].y; 119 | new_point.z = pc->points[point_id].z; 120 | new_point.intensity = pc->points[point_id].intensity; 121 | // remap ring id 122 | if (pc->height == 16) { 123 | new_point.ring = RING_ID_MAP_16[point_id / pc->width]; 124 | } else if (pc->height == 128) { 125 | new_point.ring = RING_ID_MAP_RUBY[point_id % pc->height]; 126 | } 127 | pc_new->points.push_back(new_point); 128 | } 129 | 130 | publish_points(pc_new, pc_msg); 131 | } 132 | 133 | 134 | template 135 | void handle_pc_msg(const typename pcl::PointCloud::Ptr &pc_in, 136 | const typename pcl::PointCloud::Ptr &pc_out) { 137 | 138 | // to new pointcloud 139 | for (int point_id = 0; point_id < pc_in->points.size(); ++point_id) { 140 | if (has_nan(pc_in->points[point_id])) 141 | continue; 142 | T_out_p new_point; 143 | // std::copy(pc->points[point_id].data, pc->points[point_id].data + 4, new_point.data); 144 | new_point.x = pc_in->points[point_id].x; 145 | new_point.y = pc_in->points[point_id].y; 146 | new_point.z = pc_in->points[point_id].z; 147 | new_point.intensity = pc_in->points[point_id].intensity; 148 | // new_point.ring = pc->points[point_id].ring; 149 | // // 计算相对于第一个点的相对时间 150 | // new_point.time = float(pc->points[point_id].timestamp - pc->points[0].timestamp); 151 | pc_out->points.push_back(new_point); 152 | } 153 | } 154 | 155 | template 156 | void add_ring(const typename pcl::PointCloud::Ptr &pc_in, 157 | const typename pcl::PointCloud::Ptr &pc_out) { 158 | // to new pointcloud 159 | int valid_point_id = 0; 160 | for (int point_id = 0; point_id < pc_in->points.size(); ++point_id) { 161 | if (has_nan(pc_in->points[point_id])) 162 | continue; 163 | // 跳过nan点 164 | pc_out->points[valid_point_id++].ring = pc_in->points[point_id].ring; 165 | } 166 | } 167 | 168 | template 169 | void add_time(const typename pcl::PointCloud::Ptr &pc_in, 170 | const typename pcl::PointCloud::Ptr &pc_out, double& msg_frame_time) { 171 | 172 | double start_fire_time = 999999999999999; 173 | double stop_fire_time = 0; 174 | for (int point_id = 0; point_id < pc_in->points.size(); ++point_id) { 175 | if (pc_in->points[point_id].timestamp != 0 && pc_in->points[point_id].timestamp < start_fire_time) { 176 | start_fire_time = pc_in->points[point_id].timestamp; 177 | } 178 | 179 | if (pc_in->points[point_id].timestamp != 0 && pc_in->points[point_id].timestamp > stop_fire_time) { 180 | stop_fire_time = pc_in->points[point_id].timestamp; 181 | } 182 | } 183 | 184 | // to new pointcloud 185 | int valid_point_id = 0; 186 | for (int point_id = 0; point_id < pc_in->points.size(); ++point_id) { 187 | if (has_nan(pc_in->points[point_id])) 188 | continue; 189 | // 跳过nan点 190 | pc_out->points[valid_point_id++].time = float(pc_in->points[point_id].timestamp - pc_in->points[0].timestamp); 191 | } 192 | 193 | // ROS_WARN("FRAME: %f MIN T: %f 0T: %f", msg_frame_time, start_fire_time, pc_in->points[0].timestamp); 194 | 195 | // ROS_WARN("FRAME: %f MIN T: %f 0T: %f MAX T: %f last: %f", msg_frame_time, start_fire_time, pc_in->points[0].timestamp, stop_fire_time, pc_out->points[pc_out->size() - 1].time); 196 | // 更新帧时间为起始点时间 197 | // TODO 验证此处的准确性 198 | msg_frame_time = start_fire_time; 199 | } 200 | 201 | void rsHandler_XYZIRT(const sensor_msgs::PointCloud2 &pc_msg) { 202 | pcl::PointCloud::Ptr pc_in(new pcl::PointCloud()); 203 | pcl::fromROSMsg(pc_msg, *pc_in); 204 | 205 | if (output_type == "XYZIRT") { 206 | pcl::PointCloud::Ptr pc_out(new pcl::PointCloud()); 207 | handle_pc_msg(pc_in, pc_out); 208 | add_ring(pc_in, pc_out); 209 | 210 | double frame_start_T = pc_msg.header.stamp.toSec(); 211 | add_time(pc_in, pc_out, frame_start_T); 212 | publish_points(pc_out, pc_msg, frame_start_T); 213 | } else if (output_type == "XYZIR") { 214 | pcl::PointCloud::Ptr pc_out(new pcl::PointCloud()); 215 | handle_pc_msg(pc_in, pc_out); 216 | add_ring(pc_in, pc_out); 217 | publish_points(pc_out, pc_msg); 218 | } else if (output_type == "XYZI") { 219 | pcl::PointCloud::Ptr pc_out(new pcl::PointCloud()); 220 | handle_pc_msg(pc_in, pc_out); 221 | publish_points(pc_out, pc_msg); 222 | } 223 | } 224 | 225 | int main(int argc, char **argv) { 226 | ros::init(argc, argv, "rs_converter"); 227 | ros::NodeHandle nh; 228 | if (argc < 3) { 229 | ROS_ERROR( 230 | "Please specify input pointcloud type( XYZI or XYZIRT) and output pointcloud type(XYZI, XYZIR, XYZIRT)!!!"); 231 | exit(1); 232 | } else { 233 | // 输出点云类型 234 | output_type = argv[2]; 235 | 236 | if (std::strcmp("XYZI", argv[1]) == 0) { 237 | subRobosensePC = nh.subscribe("/hesai/pandar", 1, rsHandler_XYZI); 238 | } else if (std::strcmp("XYZIRT", argv[1]) == 0) { 239 | subRobosensePC = nh.subscribe("/hesai/pandar", 1, rsHandler_XYZIRT); 240 | } else { 241 | ROS_ERROR(argv[1]); 242 | ROS_ERROR("Unsupported input pointcloud type. Currently only support XYZI and XYZIRT."); 243 | exit(1); 244 | } 245 | } 246 | pubRobosensePC = nh.advertise("/velodyne_points", 1); 247 | 248 | ROS_INFO("Listening to /hesai/pandar ......"); 249 | ros::spin(); 250 | return 0; 251 | } 252 | -------------------------------------------------------------------------------- /src/converter/rs_to_velodyne.cpp: -------------------------------------------------------------------------------- 1 | //#include "utility.h" 2 | #include 3 | 4 | #include 5 | 6 | #include 7 | #include 8 | #include 9 | 10 | std::string output_type; 11 | 12 | static int RING_ID_MAP_RUBY[] = { 13 | 3, 66, 33, 96, 11, 74, 41, 104, 19, 82, 49, 112, 27, 90, 57, 120, 14 | 35, 98, 1, 64, 43, 106, 9, 72, 51, 114, 17, 80, 59, 122, 25, 88, 15 | 67, 34, 97, 0, 75, 42, 105, 8, 83, 50, 113, 16, 91, 58, 121, 24, 16 | 99, 2, 65, 32, 107, 10, 73, 40, 115, 18, 81, 48, 123, 26, 89, 56, 17 | 7, 70, 37, 100, 15, 78, 45, 108, 23, 86, 53, 116, 31, 94, 61, 124, 18 | 39, 102, 5, 68, 47, 110, 13, 76, 55, 118, 21, 84, 63, 126, 29, 92, 19 | 71, 38, 101, 4, 79, 46, 109, 12, 87, 54, 117, 20, 95, 62, 125, 28, 20 | 103, 6, 69, 36, 111, 14, 77, 44, 119, 22, 85, 52, 127, 30, 93, 60 21 | }; 22 | static int RING_ID_MAP_16[] = { 23 | 0, 1, 2, 3, 4, 5, 6, 7, 15, 14, 13, 12, 11, 10, 9, 8 24 | }; 25 | 26 | // rslidar和velodyne的格式有微小的区别 27 | // rslidar的点云格式 28 | struct RsPointXYZIRT { 29 | PCL_ADD_POINT4D; 30 | uint8_t intensity; 31 | uint16_t ring = 0; 32 | double timestamp = 0; 33 | 34 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 35 | } EIGEN_ALIGN16; 36 | POINT_CLOUD_REGISTER_POINT_STRUCT(RsPointXYZIRT, 37 | (float, x, x)(float, y, y)(float, z, z)(uint8_t, intensity, intensity) 38 | (uint16_t, ring, ring)(double, timestamp, timestamp)) 39 | 40 | // velodyne的点云格式 41 | struct VelodynePointXYZIRT { 42 | PCL_ADD_POINT4D 43 | 44 | PCL_ADD_INTENSITY; 45 | uint16_t ring; 46 | float time; 47 | 48 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 49 | } EIGEN_ALIGN16; 50 | 51 | POINT_CLOUD_REGISTER_POINT_STRUCT (VelodynePointXYZIRT, 52 | (float, x, x)(float, y, y)(float, z, z)(float, intensity, intensity) 53 | (uint16_t, ring, ring)(float, time, time) 54 | ) 55 | 56 | struct VelodynePointXYZIR { 57 | PCL_ADD_POINT4D 58 | 59 | PCL_ADD_INTENSITY; 60 | uint16_t ring; 61 | 62 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 63 | } EIGEN_ALIGN16; 64 | 65 | POINT_CLOUD_REGISTER_POINT_STRUCT (VelodynePointXYZIR, 66 | (float, x, x)(float, y, y) 67 | (float, z, z)(float, intensity, intensity) 68 | (uint16_t, ring, ring) 69 | ) 70 | 71 | ros::Subscriber subRobosensePC; 72 | ros::Publisher pubRobosensePC; 73 | 74 | bool use_frame_time; 75 | 76 | template 77 | bool has_nan(T point) { 78 | 79 | // remove nan point, or the feature assocaion will crash, the surf point will containing nan points 80 | // pcl remove nan not work normally 81 | // ROS_ERROR("Containing nan point!"); 82 | if (pcl_isnan(point.x) || pcl_isnan(point.y) || pcl_isnan(point.z)) { 83 | return true; 84 | } else { 85 | return false; 86 | } 87 | } 88 | 89 | template 90 | void publish_points(T &new_pc, const sensor_msgs::PointCloud2 &old_msg, double update_timestamp = 0) { 91 | // pc properties 92 | new_pc->is_dense = true; 93 | 94 | // publish 95 | sensor_msgs::PointCloud2 pc_new_msg; 96 | pcl::toROSMsg(*new_pc, pc_new_msg); 97 | pc_new_msg.header = old_msg.header; 98 | pc_new_msg.header.frame_id = "velodyne"; 99 | 100 | if (update_timestamp != 0) { 101 | pc_new_msg.header.stamp = ros::Time().fromSec(update_timestamp); 102 | } 103 | 104 | pubRobosensePC.publish(pc_new_msg); 105 | } 106 | 107 | void rsHandler_XYZI(sensor_msgs::PointCloud2 pc_msg) { 108 | pcl::PointCloud::Ptr pc(new pcl::PointCloud()); 109 | pcl::PointCloud::Ptr pc_new(new pcl::PointCloud()); 110 | pcl::fromROSMsg(pc_msg, *pc); 111 | 112 | // to new pointcloud 113 | for (int point_id = 0; point_id < pc->points.size(); ++point_id) { 114 | if (has_nan(pc->points[point_id])) 115 | continue; 116 | 117 | VelodynePointXYZIR new_point; 118 | new_point.x = pc->points[point_id].x; 119 | new_point.y = pc->points[point_id].y; 120 | new_point.z = pc->points[point_id].z; 121 | new_point.intensity = pc->points[point_id].intensity; 122 | // remap ring id 123 | if (pc->height == 16) { 124 | new_point.ring = RING_ID_MAP_16[point_id / pc->width]; 125 | } else if (pc->height == 128) { 126 | new_point.ring = RING_ID_MAP_RUBY[point_id % pc->height]; 127 | } 128 | pc_new->points.push_back(new_point); 129 | } 130 | 131 | publish_points(pc_new, pc_msg); 132 | } 133 | 134 | 135 | template 136 | void handle_pc_msg(const typename pcl::PointCloud::Ptr &pc_in, 137 | const typename pcl::PointCloud::Ptr &pc_out) { 138 | 139 | // to new pointcloud 140 | for (int point_id = 0; point_id < pc_in->points.size(); ++point_id) { 141 | if (has_nan(pc_in->points[point_id])) 142 | continue; 143 | T_out_p new_point; 144 | // std::copy(pc->points[point_id].data, pc->points[point_id].data + 4, new_point.data); 145 | new_point.x = pc_in->points[point_id].x; 146 | new_point.y = pc_in->points[point_id].y; 147 | new_point.z = pc_in->points[point_id].z; 148 | new_point.intensity = pc_in->points[point_id].intensity; 149 | // new_point.ring = pc->points[point_id].ring; 150 | // // 计算相对于第一个点的相对时间 151 | // new_point.time = float(pc->points[point_id].timestamp - pc->points[0].timestamp); 152 | pc_out->points.push_back(new_point); 153 | } 154 | } 155 | 156 | template 157 | void add_ring(const typename pcl::PointCloud::Ptr &pc_in, 158 | const typename pcl::PointCloud::Ptr &pc_out) { 159 | // to new pointcloud 160 | int valid_point_id = 0; 161 | for (int point_id = 0; point_id < pc_in->points.size(); ++point_id) { 162 | if (has_nan(pc_in->points[point_id])) 163 | continue; 164 | // 跳过nan点 165 | pc_out->points[valid_point_id++].ring = pc_in->points[point_id].ring; 166 | } 167 | } 168 | 169 | template 170 | void add_time(const typename pcl::PointCloud::Ptr &pc_in, 171 | const typename pcl::PointCloud::Ptr &pc_out, double& msg_frame_time) { 172 | 173 | double start_fire_time = 999999999999999; 174 | for (int point_id = 0; point_id < pc_in->points.size(); ++point_id) { 175 | if (pc_in->points[point_id].timestamp != 0 && pc_in->points[point_id].timestamp < start_fire_time) { 176 | start_fire_time = pc_in->points[point_id].timestamp; 177 | } 178 | } 179 | 180 | // to new pointcloud 181 | int valid_point_id = 0; 182 | for (int point_id = 0; point_id < pc_in->points.size(); ++point_id) { 183 | if (has_nan(pc_in->points[point_id])) 184 | continue; 185 | // 跳过nan点 186 | pc_out->points[valid_point_id++].time = float(pc_in->points[point_id].timestamp - pc_in->points[0].timestamp); 187 | } 188 | 189 | // ROS_INFO_THROTTLE(0.001, "FRAME: %f MIN T: %f 0T: %f", msg_frame_time, start_fire_time, pc_in->points[0].timestamp); 190 | // 更新帧时间为起始点时间 191 | // TODO 验证此处的准确性 192 | msg_frame_time = start_fire_time; 193 | } 194 | 195 | void rsHandler_XYZIRT(const sensor_msgs::PointCloud2 &pc_msg) { 196 | pcl::PointCloud::Ptr pc_in(new pcl::PointCloud()); 197 | pcl::fromROSMsg(pc_msg, *pc_in); 198 | 199 | if (output_type == "XYZIRT") { 200 | pcl::PointCloud::Ptr pc_out(new pcl::PointCloud()); 201 | handle_pc_msg(pc_in, pc_out); 202 | add_ring(pc_in, pc_out); 203 | 204 | double frame_start_T = pc_msg.header.stamp.toSec(); 205 | add_time(pc_in, pc_out, frame_start_T); 206 | if (use_frame_time){ 207 | // 发布的时间戳是原始帧的时间戳 208 | publish_points(pc_out, pc_msg); 209 | } 210 | else{ 211 | // 发布的帧时间戳是所有点中最小的时间戳 这个是比较通用的情况 212 | publish_points(pc_out, pc_msg, frame_start_T); 213 | } 214 | } else if (output_type == "XYZIR") { 215 | pcl::PointCloud::Ptr pc_out(new pcl::PointCloud()); 216 | handle_pc_msg(pc_in, pc_out); 217 | add_ring(pc_in, pc_out); 218 | publish_points(pc_out, pc_msg); 219 | } else if (output_type == "XYZI") { 220 | pcl::PointCloud::Ptr pc_out(new pcl::PointCloud()); 221 | handle_pc_msg(pc_in, pc_out); 222 | publish_points(pc_out, pc_msg); 223 | } 224 | } 225 | 226 | int main(int argc, char **argv) { 227 | ros::init(argc, argv, "rs_converter"); 228 | ros::NodeHandle nh; 229 | if (argc < 3) { 230 | ROS_ERROR( 231 | "Please specify input pointcloud type( XYZI or XYZIRT) and output pointcloud type(XYZI, XYZIR, XYZIRT)!!!"); 232 | exit(1); 233 | } else { 234 | // 输出点云类型 235 | output_type = argv[2]; 236 | 237 | if(argc == 4 and std::strcmp("true", argv[3]) == 0){ 238 | use_frame_time = true; 239 | ROS_INFO("Use Frame Time is ON."); 240 | } 241 | else{ 242 | use_frame_time = false; 243 | ROS_INFO("Use Frame Time is OFF."); 244 | } 245 | 246 | if (std::strcmp("XYZI", argv[1]) == 0) { 247 | subRobosensePC = nh.subscribe("/rslidar_points", 1, rsHandler_XYZI); 248 | } else if (std::strcmp("XYZIRT", argv[1]) == 0) { 249 | subRobosensePC = nh.subscribe("/rslidar_points", 1, rsHandler_XYZIRT); 250 | } else { 251 | ROS_ERROR(argv[1]); 252 | ROS_ERROR("Unsupported input pointcloud type. Currently only support XYZI and XYZIRT."); 253 | exit(1); 254 | } 255 | } 256 | pubRobosensePC = nh.advertise("/velodyne_points", 1); 257 | 258 | ROS_INFO("Listening to /rslidar_points ......"); 259 | ros::spin(); 260 | return 0; 261 | } 262 | -------------------------------------------------------------------------------- /src/preprocess.h: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | //#include 6 | 7 | #include "ring_fals/range_image.h" 8 | #include 9 | 10 | using namespace std; 11 | 12 | #define IS_VALID(a) ((abs(a)>1e8) ? true : false) 13 | 14 | typedef pcl::PointXYZINormal PointType; 15 | //typedef custom_PointType PointType; 16 | typedef pcl::PointCloud PointCloudXYZI; 17 | 18 | enum LID_TYPE{AVIA = 1, VELO16, OUST64, KITTI}; //{1, 2, 3} 19 | enum TIME_UNIT{SEC = 0, MS = 1, US = 2, NS = 3}; 20 | enum Feature{Nor, Poss_Plane, Real_Plane, Edge_Jump, Edge_Plane, Wire, ZeroPoint}; 21 | enum Surround{Prev, Next}; 22 | enum E_jump{Nr_nor, Nr_zero, Nr_180, Nr_inf, Nr_blind}; 23 | 24 | struct orgtype 25 | { 26 | double range; 27 | double dista; 28 | double angle[2]; 29 | double intersect; 30 | E_jump edj[2]; 31 | Feature ftype; 32 | orgtype() 33 | { 34 | range = 0; 35 | edj[Prev] = Nr_nor; 36 | edj[Next] = Nr_nor; 37 | ftype = Nor; 38 | intersect = 2; 39 | } 40 | }; 41 | 42 | namespace velodyne_ros { 43 | struct EIGEN_ALIGN16 Point { 44 | PCL_ADD_POINT4D; 45 | float intensity; 46 | float time; 47 | uint16_t ring; 48 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 49 | }; 50 | } // namespace velodyne_ros 51 | POINT_CLOUD_REGISTER_POINT_STRUCT(velodyne_ros::Point, 52 | (float, x, x) 53 | (float, y, y) 54 | (float, z, z) 55 | (float, intensity, intensity) 56 | (float, time, time) 57 | (uint16_t, ring, ring) 58 | ) 59 | 60 | namespace ouster_ros { 61 | struct EIGEN_ALIGN16 Point { 62 | PCL_ADD_POINT4D; 63 | float intensity; 64 | uint32_t t; 65 | uint16_t reflectivity; 66 | uint8_t ring; 67 | uint16_t ambient; 68 | uint32_t range; 69 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 70 | }; 71 | } // namespace ouster_ros 72 | 73 | // clang-format off 74 | POINT_CLOUD_REGISTER_POINT_STRUCT(ouster_ros::Point, 75 | (float, x, x) 76 | (float, y, y) 77 | (float, z, z) 78 | (float, intensity, intensity) 79 | // use std::uint32_t to avoid conflicting with pcl::uint32_t 80 | (std::uint32_t, t, t) 81 | (std::uint16_t, reflectivity, reflectivity) 82 | (std::uint8_t, ring, ring) 83 | (std::uint16_t, ambient, ambient) 84 | (std::uint32_t, range, range) 85 | ) 86 | 87 | class Preprocess 88 | { 89 | public: 90 | // EIGEN_MAKE_ALIGNED_OPERATOR_NEW 91 | 92 | Preprocess(); 93 | ~Preprocess(); 94 | 95 | // void process(const livox_ros_driver::CustomMsg::ConstPtr &msg, PointCloudXYZI::Ptr &pcl_out); 96 | void process(const sensor_msgs::PointCloud2::ConstPtr &msg, PointCloudXYZI::Ptr &pcl_out); 97 | void set(bool feat_en, int lid_type, double bld, int pfilt_num); 98 | void initNormalEstimator(); 99 | 100 | // sensor_msgs::PointCloud2::ConstPtr pointcloud; 101 | PointCloudXYZI pl_full, pl_corn, pl_surf, pl_surf_filtered; 102 | PointCloudXYZI::Ptr cloud_with_normal; 103 | PointCloudXYZI pl_buff[128]; //maximum 128 line lidar 104 | vector typess[128]; //maximum 128 line lidar 105 | float time_unit_scale; 106 | int lidar_type, point_filter_num, N_SCANS, SCAN_RATE, time_unit; 107 | double blind; 108 | bool feature_enabled, given_offset_time; 109 | ros::Publisher pub_full, pub_surf, pub_corn; 110 | 111 | // parameters for normal estimation 112 | RangeImage range_image; 113 | bool compute_table = false; 114 | bool compute_normal_cov = false; 115 | int Horizon_SCAN, image_cols; 116 | string ring_table_dir; 117 | 118 | bool runtime_log = false; 119 | bool compute_normal = false; 120 | float roughness_max = 2.0; 121 | 122 | private: 123 | // void avia_handler(const livox_ros_driver::CustomMsg::ConstPtr &msg); 124 | void oust64_handler(const sensor_msgs::PointCloud2::ConstPtr &msg); 125 | void velodyne_handler(const sensor_msgs::PointCloud2::ConstPtr &msg); 126 | void give_feature(PointCloudXYZI &pl, vector &types); 127 | void pub_func(PointCloudXYZI &pl, const ros::Time &ct); 128 | int plane_judge(const PointCloudXYZI &pl, vector &types, uint i, uint &i_nex, Eigen::Vector3d &curr_direct); 129 | bool small_plane(const PointCloudXYZI &pl, vector &types, uint i_cur, uint &i_nex, Eigen::Vector3d &curr_direct); 130 | bool edge_jump_judge(const PointCloudXYZI &pl, vector &types, uint i, Surround nor_dir); 131 | 132 | void projectPointCloud(const pcl::PointCloud::Ptr& cloud); 133 | void computeRingNormals(const pcl::PointCloud::Ptr& cloud); 134 | void extractCloudAndNormals(const pcl::PointCloud::Ptr& cloud); 135 | void flipNormalsTowardCenterAndNormalized(const cv::Vec3f& center, const pcl::PointCloud::Ptr& cloud, cv::Mat& image_normals); 136 | void NormalizeNormals(cv::Mat& image_normals); 137 | void saveNormalPCD(const std::string& file_name, const pcl::PointCloud::Ptr& cloud, const cv::Mat& normals_mat); 138 | void estimateNormals(const pcl::PointCloud::Ptr& cloud); 139 | void estimateNormals(const pcl::PointCloud::Ptr& cloud); 140 | bool pointInImage(const PointXYZIRT& point, int& row, int& col); 141 | 142 | void ouster2velodyne(const pcl::PointCloud& cloud_in, const pcl::PointCloud::Ptr& cloud_out); 143 | 144 | int group_size; 145 | double disA, disB, inf_bound; 146 | double limit_maxmid, limit_midmin, limit_maxmin; 147 | double p2l_ratio; 148 | double jump_up_limit, jump_down_limit; 149 | double cos160; 150 | double edgea, edgeb; 151 | double smallp_intersect, smallp_ratio; 152 | double vx, vy, vz; 153 | 154 | // parameters for normal estimation 155 | cv::Mat rangeMat; 156 | cv::Mat normals; //record ring normals 157 | cv::Mat_> cov_mat; 158 | cv::Mat plane_residual; //record ring normals residual in normal estimation 159 | cv::Mat grad_x; 160 | std::vector image2cloud; 161 | std::vector cloud2image; 162 | double aver_normal_time = 0.0, aver_proj_time = 0.0, aver_compu_time = 0.0, aver_smooth_time = 0.0; 163 | double proj_time = 0.0, compu_time = 0.0, smooth_time = 0.0; 164 | int num_scans = 0; 165 | float ang_res_x; // 360 / 1800 = 0.2 166 | cv::Mat Prewitt_x; 167 | }; 168 | -------------------------------------------------------------------------------- /src/ring_fals/Image_normals.hpp: -------------------------------------------------------------------------------- 1 | #include "precomp.h" 2 | #include 3 | 4 | /** Just compute the norm of a vector 5 | * @param vec a vector of size 3 and any type T 6 | * @return 7 | */ 8 | template 9 | T 10 | inline 11 | norm_vec(const cv::Vec &vec) 12 | { 13 | return std::sqrt(vec[0] * vec[0] + vec[1] * vec[1] + vec[2] * vec[2]); 14 | } 15 | 16 | /** Modify normals to make sure they point towards the camera 17 | * @param normals 18 | */ 19 | template 20 | inline 21 | void 22 | signNormal(const cv::Vec & normal_in, cv::Vec & normal_out) 23 | { 24 | cv::Vec res; 25 | if (normal_in[2] > 0) 26 | res = -normal_in / norm_vec(normal_in); 27 | else 28 | res = normal_in / norm_vec(normal_in); 29 | 30 | normal_out[0] = res[0]; 31 | normal_out[1] = res[1]; 32 | normal_out[2] = res[2]; 33 | } 34 | 35 | /** Normalized normals 36 | * @param normals 37 | */ 38 | template 39 | inline 40 | void 41 | normalizedNormal(const cv::Vec & normal_in, cv::Vec & normal_out) 42 | { 43 | normal_out = normal_in / norm_vec(normal_in); 44 | } 45 | 46 | /** Given 3d points, compute their distance to the origin 47 | * @param points 48 | * @return 49 | */ 50 | template 51 | cv::Mat_ 52 | computeRadius(const cv::Mat &points) 53 | { 54 | typedef cv::Vec PointT; 55 | 56 | // Compute the 57 | cv::Size size(points.cols, points.rows); 58 | cv::Mat_ r(size); 59 | if (points.isContinuous()) // 60 | size = cv::Size(points.cols * points.rows, 1); 61 | for (int y = 0; y < size.height; ++y) 62 | { 63 | const PointT* point = points.ptr < PointT > (y), *point_end = points.ptr < PointT > (y) + size.width; 64 | T * row = r[y]; 65 | for (; point != point_end; ++point, ++row) 66 | *row = norm_vec(*point); 67 | } 68 | 69 | return r; 70 | } 71 | 72 | // Compute theta and phi according to equation 3 of 73 | // ``Fast and Accurate Computation of Surface Normals from Range Images`` 74 | // by H. Badino, D. Huber, Y. Park and T. Kanade 75 | template 76 | void 77 | computeThetaPhi(int rows, int cols, const cv::Matx& K, cv::Mat &cos_theta, cv::Mat &sin_theta, 78 | cv::Mat &cos_phi, cv::Mat &sin_phi) 79 | { 80 | // Create some bogus coordinates 81 | cv::Mat depth_image = K(0, 0) * cv::Mat_ < T > ::ones(rows, cols); //fx 82 | cv::Mat points3d; 83 | cv::rgbd::depthTo3d(depth_image, cv::Mat(K), points3d); 84 | 85 | typedef cv::Vec Vec3T; 86 | 87 | cos_theta = cv::Mat_ < T > (rows, cols); 88 | sin_theta = cv::Mat_ < T > (rows, cols); 89 | cos_phi = cv::Mat_ < T > (rows, cols); 90 | sin_phi = cv::Mat_ < T > (rows, cols); 91 | cv::Mat r = computeRadius(points3d); 92 | 93 | for (int y = 0; y < rows; ++y) 94 | { 95 | T *row_cos_theta = cos_theta.ptr < T > (y), *row_sin_theta = sin_theta.ptr < T > (y); 96 | T *row_cos_phi = cos_phi.ptr < T > (y), *row_sin_phi = sin_phi.ptr < T > (y); 97 | const Vec3T * row_points = points3d.ptr < Vec3T > (y), *row_points_end = points3d.ptr < Vec3T 98 | > (y) + points3d.cols; 99 | const T * row_r = r.ptr < T > (y); 100 | for (; row_points < row_points_end; 101 | ++row_cos_theta, ++row_sin_theta, ++row_cos_phi, ++row_sin_phi, ++row_points, ++row_r) 102 | { 103 | // In the paper, z goes away from the camera, y goes down, x goes right 104 | // OpenCV has the same conventions 105 | // Theta goes from z to x (and actually goes from -pi/2 to pi/2, phi goes from z to y 106 | float theta = (float)std::atan2(row_points->val[0], row_points->val[2]); //azimuth x/z 107 | *row_cos_theta = std::cos(theta); 108 | *row_sin_theta = std::sin(theta); 109 | float phi = (float)std::asin(row_points->val[1] / (*row_r)); //phi， y/r 110 | *row_cos_phi = std::cos(phi); 111 | *row_sin_phi = std::sin(phi); 112 | } 113 | } 114 | } 115 | 116 | //template 117 | class LIDAR_FALS 118 | { 119 | public: 120 | typedef cv::Matx Mat33T; 121 | typedef cv::Vec Vec9T; 122 | typedef cv::Vec Vec3T; 123 | 124 | LIDAR_FALS(int rows, int cols, int window_size, int depth, const cv::Mat &K) 125 | {} 126 | 127 | LIDAR_FALS(int rows, int cols, int window_size) 128 | : rows_(rows) 129 | , cols_(cols) 130 | , window_size_(window_size) 131 | {} 132 | 133 | LIDAR_FALS() {} 134 | 135 | ~LIDAR_FALS() {} 136 | 137 | /** Compute cached data 138 | */ 139 | virtual void 140 | cache() 141 | { 142 | std::cout << "build look up table.\n"; 143 | 144 | cv::Mat cos_theta, sin_theta, cos_phi, sin_phi; // 145 | computeThetaPhi(rows_, cols_, K_, cos_theta, sin_theta, cos_phi, sin_phi); 146 | 147 | computeMInverse(cos_theta, sin_theta, cos_phi, sin_phi); 148 | } 149 | 150 | void computeMInverse(cv::Mat &cos_theta, cv::Mat &sin_theta, cv::Mat &cos_phi, cv::Mat &sin_phi) 151 | { 152 | std::vector channels(3); 153 | channels[0] = cos_phi.mul(cos_theta); 154 | channels[1] = -cos_phi.mul(sin_theta); 155 | channels[2] = sin_phi; 156 | merge(channels, V_); 157 | 158 | // Compute M 159 | cv::Mat_ M(rows_, cols_); 160 | Mat33T VVt; //todo save, M matrix in the next line 161 | const Vec3T * vec = V_[0]; 162 | Vec9T * M_ptr = M[0], *M_ptr_end = M_ptr + rows_ * cols_; 163 | for (; M_ptr != M_ptr_end; ++vec, ++M_ptr) 164 | { 165 | VVt = (*vec) * vec->t(); //v * v_t 166 | *M_ptr = Vec9T(VVt.val); 167 | } 168 | 169 | ///todo BorderTypes::BORDER_TRANSPARENT, error 170 | int border_type = cv::BorderTypes::BORDER_TRANSPARENT; 171 | boxFilter(M, M, M.depth(), cv::Size(window_size_, window_size_), cv::Point(-1, -1), false); 172 | 173 | // Compute M's inverse 174 | Mat33T M_inv; 175 | M_inv_.create(rows_, cols_); 176 | Vec9T * M_inv_ptr = M_inv_[0]; 177 | for (M_ptr = &M(0); M_ptr != M_ptr_end; ++M_inv_ptr, ++M_ptr) 178 | { 179 | // We have a semi-definite matrix 180 | invert(Mat33T(M_ptr->val), M_inv, cv::DECOMP_CHOLESKY); 181 | *M_inv_ptr = Vec9T(M_inv.val); 182 | } 183 | } 184 | 185 | void saveMInverse(std::string dir, std::string filename) 186 | { 187 | std::vector mats(9); 188 | for (int i = 0; i < 9; ++i) { 189 | mats[i].create(M_inv_.rows, M_inv_.cols, CV_32F); 190 | } 191 | cv::Mat mat_vv(M_inv_.rows, M_inv_.cols, CV_32FC3); 192 | for (int i = 0; i < M_inv_.rows; ++i) { 193 | for (int j = 0; j < M_inv_.cols; ++j) { 194 | const Vec9T& tmp(M_inv_.at(i, j)); 195 | for (int k = 0; k < 9; ++k) { 196 | mats[k].at(i, j) = tmp(k); 197 | } 198 | mat_vv.at(i, j) = V_.at(i, j); 199 | } 200 | } 201 | 202 | //save M inverse 203 | for (int i = 0; i < 9; ++i) { 204 | std::string file_id(std::to_string(i)); 205 | cv::FileStorage fs(dir + "/" + filename + "_M_" + file_id + ".xml", cv::FileStorage::WRITE); 206 | fs << filename + "_" + file_id << mats[i]; 207 | fs.release(); 208 | } 209 | //save v v_t 210 | cv::FileStorage fs(dir + "/" + filename + "_v" + + ".xml", cv::FileStorage::WRITE); 211 | fs << filename + "_vv" << mat_vv; 212 | fs.release(); 213 | } 214 | 215 | bool loadMInverse(std::string dir, std::string filename) 216 | { 217 | std::vector mats(9); 218 | for (int i = 0; i < 9; ++i) { 219 | mats[i].create(rows_, cols_, CV_32F); 220 | } 221 | 222 | for (int i = 0; i < 9; ++i) { 223 | std::string file_id(std::to_string(i)); 224 | cv::FileStorage fs(dir + "/" + filename + "_M_" + file_id + ".xml", cv::FileStorage::READ); 225 | if(!fs.isOpened()) 226 | { 227 | // std::cerr << "ERROR: Wrong path to ring normal M file" << std::endl; 228 | return false; 229 | } 230 | fs[filename + "_" + file_id] >> mats[i]; 231 | fs.release(); 232 | } 233 | 234 | M_inv_.create(rows_, cols_); 235 | 236 | for (int i = 0; i < rows_; ++i) { 237 | for (int j = 0; j < cols_; ++j) { 238 | Vec9T& tmp(M_inv_.at(i, j)); 239 | for (int k = 0; k < 9; ++k) { 240 | tmp(k) = mats[k].at(i, j); 241 | } 242 | } 243 | } 244 | 245 | V_.create(rows_, cols_); 246 | cv::FileStorage fs(dir + "/" + filename + "_v" + + ".xml", cv::FileStorage::READ); 247 | fs[filename + "_vv"] >> V_; 248 | fs.release(); 249 | return true; 250 | } 251 | 252 | /** Compute the normals 253 | * @param r 254 | * @return 255 | */ 256 | virtual void 257 | compute(const cv::Mat &r, cv::Mat & normals) const 258 | { 259 | // Compute B 260 | cv::Mat_ B(rows_, cols_); 261 | 262 | const float* row_r = r.ptr < float > (0), *row_r_end = row_r + rows_ * cols_; //distance 263 | const Vec3T *row_V = V_[0]; 264 | Vec3T *row_B = B[0]; 265 | for (; row_r != row_r_end; ++row_r, ++row_B, ++row_V) 266 | { 267 | if (*row_r==FLT_MAX) 268 | *row_B = Vec3T(); 269 | else 270 | *row_B = (*row_V) / (*row_r); //v_i / r_i 271 | } 272 | 273 | ///todo BorderTypes::BORDER_TRANSPARENT, error 274 | // Apply a box filter to B 275 | boxFilter(B, B, B.depth(), cv::Size(window_size_, window_size_), cv::Point(-1, -1), false); 276 | 277 | // compute the Minv*B products 278 | row_r = r.ptr < float > (0); 279 | const Vec3T * B_vec = B[0]; 280 | const Mat33T * M_inv = reinterpret_cast(M_inv_.ptr(0)); 281 | Vec3T *normal = normals.ptr(0); 282 | for (; row_r != row_r_end; ++row_r, ++B_vec, ++normal, ++M_inv) 283 | if (*row_r==FLT_MAX) 284 | { 285 | (*normal)[0] = *row_r; 286 | (*normal)[1] = *row_r; 287 | (*normal)[2] = *row_r; 288 | } 289 | else 290 | { 291 | Mat33T Mr = *M_inv; 292 | Vec3T Br = *B_vec; 293 | Vec3T MBr(Mr(0, 0) * Br[0] + Mr(0, 1)*Br[1] + Mr(0, 2)*Br[2], 294 | Mr(1, 0) * Br[0] + Mr(1, 1)*Br[1] + Mr(1, 2)*Br[2], 295 | Mr(2, 0) * Br[0] + Mr(2, 1)*Br[1] + Mr(2, 2)*Br[2]); 296 | //actually, can not flip normal here, because the point position is unknown 297 | signNormal(MBr, *normal); 298 | } 299 | } 300 | 301 | /** Compute the normals 302 | * @param r 303 | * @return 304 | */ 305 | virtual void 306 | compute(const cv::Mat &r, cv::Mat & normals, cv::Mat & residual) const 307 | { 308 | // Compute B 309 | cv::Mat_ B(rows_, cols_); 310 | 311 | const float* row_r = r.ptr < float > (0), *row_r_end = row_r + rows_ * cols_; 312 | const Vec3T *row_V = V_[0]; 313 | Vec3T *row_B = B[0]; 314 | for (; row_r != row_r_end; ++row_r, ++row_B, ++row_V) 315 | { 316 | if (*row_r==FLT_MAX) 317 | *row_B = Vec3T(); 318 | else 319 | *row_B = (*row_V) / (*row_r); //v_i / r_i 320 | } 321 | 322 | ///todo BorderTypes::BORDER_TRANSPARENT, error 323 | // Apply a box filter to B 324 | boxFilter(B, B, B.depth(), cv::Size(window_size_, window_size_), cv::Point(-1, -1), false); 325 | 326 | // compute the Minv*B products 327 | row_r = r.ptr < float > (0); 328 | const Vec3T * B_vec = B[0]; 329 | const Mat33T * M_inv = reinterpret_cast(M_inv_.ptr(0)); 330 | Vec3T *normal = normals.ptr(0); 331 | for (; row_r != row_r_end; ++row_r, ++B_vec, ++normal, ++M_inv) { 332 | if (*row_r==FLT_MAX) { 333 | (*normal)[0] = *row_r; 334 | (*normal)[1] = *row_r; 335 | (*normal)[2] = *row_r; 336 | } else { 337 | Mat33T Mr = *M_inv; 338 | Vec3T Br = *B_vec; 339 | Vec3T MBr(Mr(0, 0) * Br[0] + Mr(0, 1) * Br[1] + Mr(0, 2) * Br[2], 340 | Mr(1, 0) * Br[0] + Mr(1, 1) * Br[1] + Mr(1, 2) * Br[2], 341 | Mr(2, 0) * Br[0] + Mr(2, 1) * Br[1] + Mr(2, 2) * Br[2]); 342 | // signNormal(MBr, *normal); 343 | normalizedNormal(MBr, *normal); 344 | } 345 | } 346 | } 347 | 348 | /** Compute the covariance of normals 349 | * @param r 350 | * @return 351 | */ 352 | virtual void 353 | computeCovariance(const cv::Mat &r, cv::Mat_ & cov_mat) const 354 | { 355 | // Compute B 356 | // cv::Mat_ B(rows_, cols_); 357 | // 358 | const float* row_r = r.ptr < float > (0), *row_r_end = row_r + rows_ * cols_; 359 | // const Vec3T *row_V = V_[0]; 360 | // Mat33T VVt; //todo save, M matrix in the next line 361 | // Vec3T *row_B = B[0]; 362 | // for (; row_r != row_r_end; ++row_r, ++row_B, ++row_V) 363 | // { 364 | // if (*row_r==FLT_MAX) 365 | // *row_B = Vec3T(); 366 | // else 367 | // *row_B = (*row_V) / (*row_r); //v_i / r_i 368 | // } 369 | 370 | 371 | cv::Mat_ D(rows_, cols_); 372 | Mat33T VVt; //todo save, M matrix in the next line 373 | const Vec3T * vec = V_[0]; 374 | Mat33T * M_ptr = D[0], *M_ptr_end = M_ptr + rows_ * cols_; 375 | for (; M_ptr != M_ptr_end; ++vec, ++M_ptr, ++row_r) 376 | { 377 | M_ptr->all(0.0); 378 | 379 | // if (*row_r > 300 || *row_r == 0) 380 | // M_ptr->all(0.0); 381 | // else 382 | if (*row_r > 0 && *row_r < 300) 383 | { 384 | // VVt = (*vec) * vec->t() * (1.0 / pow(*row_r, 4)); //(v * v_t) / r^4 385 | // *M_ptr = VVt; 386 | *M_ptr = (*vec) * vec->t() * (1.0 / pow(*row_r, 4)); //(v * v_t) / r^4 387 | // if (isnan(1.0 / pow(*row_r, 4)) || isinf(1.0 / pow(*row_r, 4))) { 388 | // std::cout << "*vec" << std::endl; 389 | // std::cout << *vec << std::endl; 390 | // std::cout << *row_r << std::endl; 391 | // } 392 | } 393 | } 394 | 395 | ///todo BorderTypes::BORDER_TRANSPARENT, error 396 | // Apply a box filter to D 397 | boxFilter(D, D, D.depth(), cv::Size(window_size_, window_size_), cv::Point(-1, -1), false); 398 | 399 | // compute the Minv*B products 400 | row_r = r.ptr < float > (0); 401 | // const Vec9T * B_vec = D[0]; 402 | const Mat33T * d_mat = D[0]; 403 | const Mat33T * M_inv = reinterpret_cast(M_inv_.ptr(0)); 404 | Mat33T * cov = cov_mat.ptr(0); 405 | for (; row_r != row_r_end; ++row_r, ++d_mat, ++cov, ++M_inv) { 406 | if (*row_r==FLT_MAX) { 407 | cov->all(0.0); //todo 408 | } else { 409 | Mat33T Mr = *M_inv; 410 | *cov = Mr * (*d_mat) * Mr.t(); 411 | // Vec3T Br = *B_vec; 412 | // Vec3T MBr(Mr(0, 0) * Br[0] + Mr(0, 1) * Br[1] + Mr(0, 2) * Br[2], 413 | // Mr(1, 0) * Br[0] + Mr(1, 1) * Br[1] + Mr(1, 2) * Br[2], 414 | // Mr(2, 0) * Br[0] + Mr(2, 1) * Br[1] + Mr(2, 2) * Br[2]); 415 | // signNormal(MBr, *normal); 416 | // normalizedNormal(MBr, *normal); 417 | } 418 | if ( isnan((*cov)(0,0)) ) { 419 | std::cout << "*row_r " << *row_r << std::endl; 420 | std::cout << *M_inv < V_; //sin(theta) * cos(phi), sin(phi), cos(theta) * cos(phi) 438 | cv::Mat_ M_inv_; //M^-1 439 | }; -------------------------------------------------------------------------------- /src/ring_fals/precomp.h: -------------------------------------------------------------------------------- 1 | /*M/////////////////////////////////////////////////////////////////////////////////////// 2 | // 3 | // IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING. 4 | // 5 | // By downloading, copying, installing or using the software you agree to this license. 6 | // If you do not agree to this license, do not download, install, 7 | // copy or use the software. 8 | // 9 | // 10 | // License Agreement 11 | // For Open Source Computer Vision Library 12 | // 13 | // Copyright (C) 2000-2008, Intel Corporation, all rights reserved. 14 | // Copyright (C) 2009, Willow Garage Inc., all rights reserved. 15 | // Third party copyrights are property of their respective owners. 16 | // 17 | // Redistribution and use in source and binary forms, with or without modification, 18 | // are permitted provided that the following conditions are met: 19 | // 20 | // * Redistribution's of source code must retain the above copyright notice, 21 | // this list of conditions and the following disclaimer. 22 | // 23 | // * Redistribution's in binary form must reproduce the above copyright notice, 24 | // this list of conditions and the following disclaimer in the documentation 25 | // and/or other materials provided with the distribution. 26 | // 27 | // * The name of the copyright holders may not be used to endorse or promote products 28 | // derived from this software without specific prior written permission. 29 | // 30 | // This software is provided by the copyright holders and contributors "as is" and 31 | // any express or implied warranties, including, but not limited to, the implied 32 | // warranties of merchantability and fitness for a particular purpose are disclaimed. 33 | // In no event shall the Intel Corporation or contributors be liable for any direct, 34 | // indirect, incidental, special, exemplary, or consequential damages 35 | // (including, but not limited to, procurement of substitute goods or services; 36 | // loss of use, data, or profits; or business interruption) however caused 37 | // and on any theory of liability, whether in contract, strict liability, 38 | // or tort (including negligence or otherwise) arising in any way out of 39 | // the use of this software, even if advised of the possibility of such damage. 40 | // 41 | //M*/ 42 | 43 | #ifndef __OPENCV_PRECOMP_H__ 44 | #define __OPENCV_PRECOMP_H__ 45 | 46 | #include "opencv2/rgbd.hpp" 47 | #include "opencv2/calib3d.hpp" 48 | #include "opencv2/imgproc.hpp" 49 | #include "opencv2/core/utility.hpp" 50 | //#include "opencv2/core/private.hpp" 51 | #include 52 | #include 53 | #include 54 | #include 55 | 56 | #endif 57 | -------------------------------------------------------------------------------- /src/ring_fals/range_image.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Created by hk on 12/8/22. 3 | // 4 | 5 | #include "range_image.h" 6 | 7 | void RangeImage::createFromRings(const pcl::PointCloud::Ptr& cloud) 8 | { 9 | if (rows_ == 0 || cols_ == 0) { 10 | std::cout << "rows_ or cols_ == 0, can't create range image.\n"; 11 | return; 12 | } 13 | 14 | range_image = cv::Mat(rows_, cols_, CV_32F, range_init); 15 | cloud_id_image.resize(rows_ * cols_, -1); 16 | int cloudSize = (int)cloud->points.size(); 17 | // range image projection 18 | for (int i = 0; i < cloudSize; ++i) 19 | { 20 | const PointXYZIRT & thisPoint = cloud->points[i]; 21 | 22 | int rowIdn = cloud->points[i].ring; // ring 23 | if (rowIdn < 0 || rowIdn >= rows_) 24 | continue; 25 | 26 | if (downsampleRate > 0 && rowIdn % downsampleRate != 0) // 27 | continue; 28 | 29 | // float horizonAngle = atan2(thisPoint.x, thisPoint.y) * 180 / M_PI; 30 | // 31 | static float ang_res_x = 360.0/float(cols_); // 360 / 1800 = 0.2 32 | 33 | float horizonAngle = atan2(thisPoint.y, -thisPoint.x); 34 | if (horizonAngle < 0) 35 | horizonAngle += 2 * M_PI; 36 | horizonAngle = horizonAngle * 180 / M_PI; 37 | int columnIdn = round(horizonAngle/ ang_res_x); 38 | 39 | if (columnIdn < 0 || columnIdn >= cols_) 40 | continue; 41 | 42 | float range = sqrt(thisPoint.x * thisPoint.x + thisPoint.y * thisPoint.y +thisPoint.z * thisPoint.z);// sqrt(x^2 + y^2 + z^2) 43 | 44 | if (range < range_min || range > range_max) 45 | continue; 46 | 47 | if (range_image.at(rowIdn, columnIdn) != FLT_MAX) 48 | continue; 49 | 50 | // for the amsterdam dataset 51 | // if (range < 6.0 && rowIdn <= 7 && (columnIdn >= 1600 || columnIdn <= 200)) 52 | // continue; 53 | // if (thisPoint.z < -2.0) 54 | // continue; 55 | 56 | range_image.at(rowIdn, columnIdn) = range; 57 | if (input_value_max < range) 58 | input_value_max = range; 59 | 60 | int index = columnIdn + rowIdn * cols_; 61 | cloud_id_image[index] = i; 62 | 63 | } 64 | } 65 | 66 | void RangeImage::saveRangeImage(const std::string &file) { 67 | cv::Mat output = cv::Mat(range_image.rows, range_image.cols, CV_8U , 255); 68 | for (int i = 0; i < range_image.rows; ++i) { 69 | for (int j = 0; j < range_image.cols; ++j) { 70 | if (range_image.at(i, j) < 100.0) 71 | output.at(i, j) = range_image.at(i, j) / 100 * 255.0; 72 | else 73 | output.at(i, j) = 255; 74 | } 75 | } 76 | cv::imwrite(file, output); 77 | } 78 | 79 | void RangeImage::computeNormals(cv::OutputArray normals_out) { 80 | 81 | normals_out.create(rows_, cols_, CV_MAKETYPE(depth_, 3)); 82 | // if (points3d_in.empty()) 83 | // return; 84 | 85 | cv::Mat normals = normals_out.getMat(); 86 | lidar_fals.compute(range_image, normals); 87 | 88 | }; 89 | 90 | //in lidar frame 91 | void RangeImage::createFromCloud(const int& num_bins_v, const int& num_bins_h, 92 | const pcl::PointCloud::Ptr& cloud) 93 | { 94 | float bin_res_v = 180.0 / (float)num_bins_v; // vertical 95 | float bin_res_h = 360.0 / (float)num_bins_h; // horizon 96 | 97 | range_image = cv::Mat(num_bins_v, num_bins_h, CV_32F, cv::Scalar::all(FLT_MAX)); 98 | cloud_id_image.resize(rows_ * cols_, -1); 99 | 100 | //lidar frame 101 | for (int i = 0; i < (int)cloud->size(); ++i) 102 | { 103 | const PointType &p = cloud->points[i]; 104 | 105 | // find row id in range image 106 | float row_angle = atan2(p.z, sqrt(p.x * p.x + p.y * p.y)); // degrees, bottom -> up, -90 -> 0 -> 90 107 | row_angle = -row_angle * 180.0 / M_PI + 90.0; //bottom -> up, 180 -> 90 -> 0 108 | int row_id = round(row_angle / bin_res_v); 109 | 110 | // find column id in range image 111 | float col_angle = atan2(p.y, -p.x) * 180.0 / M_PI; // degrees, back -> left -> front -> right, 0 -> 360 112 | if (col_angle < 0) 113 | col_angle += 360; 114 | int col_id = round(col_angle / bin_res_h); 115 | 116 | // id may be out of boundary 117 | if (row_id < 0 || row_id >= num_bins_v || col_id < 0 || col_id >= num_bins_h) 118 | continue; 119 | // only keep points that's closer 120 | float dist = sqrt(p.x * p.x + p.y * p.y + p.z * p.z); 121 | if (dist < range_image.at(row_id, col_id)) 122 | { 123 | range_image.at(row_id, col_id) = dist; 124 | 125 | int index = col_id + row_id * cols_; 126 | cloud_id_image[index] = i; 127 | } 128 | } 129 | } 130 | 131 | void RangeImage::computeLookupTable() 132 | { 133 | if (range_image.empty()) 134 | return; 135 | } 136 | 137 | void RangeImage::createAndBuildTableFromRings(const int &num_rings, const int &num_horizon_scan, 138 | const pcl::PointCloud::Ptr &cloud) 139 | { 140 | if (rows_ != num_rings || cols_ != num_horizon_scan) { 141 | std::cout << "rows_ != num_rings || cols_ != num_horizon_scan. reset rows_ and cols\n"; 142 | rows_ = num_rings; 143 | cols_ = num_horizon_scan; 144 | lidar_fals.setRowsCols(num_rings, num_horizon_scan); 145 | } 146 | createAndBuildTableFromRings(cloud); 147 | } 148 | 149 | void RangeImage::createAndBuildTableFromRings(const pcl::PointCloud::Ptr &cloud) 150 | { 151 | if (rows_ == 0 || cols_ == 0) { 152 | std::cout << "rows_ == 0 or cols_ == 0, can't create range image.\n"; 153 | return; 154 | } 155 | 156 | int num_valid_bins = 0; 157 | range_image = cv::Mat(rows_, cols_, CV_32F, range_init); 158 | cloud_id_image.resize(rows_ * cols_, -1); 159 | 160 | //lidar frame，v = [cos(phi)cos(theta), -cos(phi)sin(theta), sin(phi)]^T 161 | cv::Mat cos_theta = cv::Mat(rows_, cols_, CV_32F); 162 | cv::Mat sin_theta = cv::Mat(rows_, cols_, CV_32F); 163 | cv::Mat cos_phi = cv::Mat(rows_, cols_, CV_32F); 164 | cv::Mat sin_phi = cv::Mat(rows_, cols_, CV_32F); 165 | int cloudSize = (int)cloud->points.size(); 166 | // range image projection 167 | for (int i = 0; i < cloudSize; ++i) 168 | { 169 | const PointXYZIRT & thisPoint = cloud->points[i]; 170 | 171 | int rowIdn = cloud->points[i].ring; // ring 172 | if (rowIdn < 0 || rowIdn >= rows_) 173 | continue; 174 | 175 | if (downsampleRate > 0 && rowIdn % downsampleRate != 0) // 176 | continue; 177 | 178 | float horizonAngle = atan2(thisPoint.x, thisPoint.y) * 180 / M_PI; //theta 179 | 180 | static float ang_res_x = 360.0/float(cols_); // 360 / 1800 = 0.2 181 | int columnIdn = -round((horizonAngle-90.0)/ang_res_x) + cols_/2; 182 | if (columnIdn >= cols_) 183 | columnIdn -= cols_; 184 | 185 | if (columnIdn < 0 || columnIdn >= cols_) 186 | continue; 187 | 188 | float range = sqrt(thisPoint.x * thisPoint.x + thisPoint.y * thisPoint.y +thisPoint.z * thisPoint.z);// sqrt(x^2 + y^2 + z^2) 189 | 190 | if (range < range_min || range > range_max) 191 | continue; 192 | 193 | if (range_image.at(rowIdn, columnIdn) != FLT_MAX) 194 | continue; 195 | 196 | // for the amsterdam dataset 197 | // if (range < 6.0 && rowIdn <= 7 && (columnIdn >= 1600 || columnIdn <= 200)) 198 | // continue; 199 | // if (thisPoint.z < -2.0) 200 | // continue; 201 | 202 | range_image.at(rowIdn, columnIdn) = range; 203 | // if (input_value_max < range) 204 | // input_value_max = range; 205 | 206 | //theta, phi used for building lookup table 207 | if (!table_valid) 208 | { 209 | float theta = (float) std::atan2(-thisPoint.y, thisPoint.x); //lidar frame azimuth -y / x 210 | float phi = (float) std::asin(thisPoint.z / range); //lidar frame phi， z/r 211 | cos_theta.at(rowIdn, columnIdn) = std::cos(theta); 212 | sin_theta.at(rowIdn, columnIdn) = std::sin(theta); 213 | cos_phi.at(rowIdn, columnIdn) = std::cos(phi); 214 | sin_phi.at(rowIdn, columnIdn) = std::sin(phi); 215 | } 216 | 217 | int index = columnIdn + rowIdn * cols_; //index 218 | cloud_id_image[index] = i; 219 | // fullCloud->points[index] = thisPoint; 220 | } 221 | 222 | //save mat for debug 223 | { 224 | // cv::Mat output = cv::Mat(rows_, cols_, CV_8U, 255); 225 | // for (int i = 0; i < rows_; ++i) { 226 | // for (int j = 0; j < cols_; ++j) { 227 | // if (range_image.at(i, j) < 100.0) 228 | // output.at(i, j) = range_image.at(i, j) / input_value_max * 255.0; 229 | // } 230 | // } 231 | // cv::imwrite("/tmp/cos_theta.jpg", cos_theta); 232 | } 233 | 234 | if (!table_valid) { 235 | lidar_fals.computeMInverse(cos_theta, sin_theta, cos_phi, sin_phi); 236 | table_valid = true; 237 | } 238 | } 239 | 240 | void RangeImage::buildTableFromRings(const pcl::PointCloud::Ptr &cloud) { 241 | 242 | if (rows_ == 0 || cols_ == 0) { 243 | std::cout << "rows_ == 0 or cols_ == 0, can't create range image.\n"; 244 | return; 245 | } 246 | 247 | // int num_valid_bins = 0; 248 | // range_image = cv::Mat(rows_, cols_, CV_32F, range_init); 249 | cloud_id_image.resize(rows_ * cols_, -1); 250 | 251 | 252 | //lidar frame，v = [cos(phi)cos(theta), -cos(phi)sin(theta), sin(phi)]^T 253 | // cv::Mat cos_theta = cv::Mat(rows_, cols_, CV_32F); 254 | // cv::Mat sin_theta = cv::Mat(rows_, cols_, CV_32F); 255 | // cv::Mat cos_phi = cv::Mat(rows_, cols_, CV_32F); 256 | // cv::Mat sin_phi = cv::Mat(rows_, cols_, CV_32F); 257 | int cloudSize = (int)cloud->points.size(); 258 | static float ang_res_x = 360.0 / float(cols_); //angle resolution 360 / 1800 = 0.2 259 | // range image projection 260 | for (int i = 0; i < cloudSize; ++i) 261 | { 262 | const PointXYZIRT & thisPoint = cloud->points[i]; 263 | 264 | int rowIdn = cloud->points[i].ring; // ring， 265 | if (rowIdn < 0 || rowIdn >= rows_) 266 | continue; 267 | 268 | float horizonAngle = atan2(thisPoint.y, -thisPoint.x); //theta 269 | if (horizonAngle < 0) 270 | horizonAngle += 2 * M_PI; 271 | horizonAngle = horizonAngle * 180 / M_PI; 272 | int columnIdn = round(horizonAngle/ ang_res_x); 273 | 274 | if (columnIdn < 0 || columnIdn >= cols_) 275 | continue; 276 | 277 | float range = sqrt(thisPoint.x * thisPoint.x + thisPoint.y * thisPoint.y +thisPoint.z * thisPoint.z);// sqrt(x^2 + y^2 + z^2) 278 | 279 | if (range < range_min || range > range_max) 280 | continue; 281 | 282 | // if (range_image.at(rowIdn, columnIdn) != FLT_MAX) 283 | // continue; 284 | // 285 | // range_image.at(rowIdn, columnIdn) = range; 286 | // if (input_value_max < range) 287 | // input_value_max = range; 288 | 289 | //theta, phi used for building lookup table 290 | float theta = (float) std::atan2(-thisPoint.y, thisPoint.x); //lidar frame azimuth，-y / x 291 | float phi = (float) std::asin(thisPoint.z / range); //lidar frame: phi， z/r 292 | int index = columnIdn + rowIdn * cols_; //index 293 | float num_p = static_cast(num_per_pixel[index]); 294 | float num_p_new = num_p + 1; 295 | cos_theta.at(rowIdn, columnIdn) = (num_p * cos_theta.at(rowIdn, columnIdn) + std::cos(theta)) / num_p_new; 296 | sin_theta.at(rowIdn, columnIdn) = (num_p * sin_theta.at(rowIdn, columnIdn) + std::sin(theta)) / num_p_new; 297 | cos_phi.at(rowIdn, columnIdn) = (num_p * cos_phi.at(rowIdn, columnIdn) + std::cos(phi)) / num_p_new; 298 | sin_phi.at(rowIdn, columnIdn) = (num_p * sin_phi.at(rowIdn, columnIdn) + std::sin(phi)) / num_p_new; 299 | 300 | ++num_per_pixel[index]; 301 | // cloud_id_image[index] = i; 302 | } 303 | ++num_cloud; 304 | 305 | if (num_cloud < min_table_cloud) 306 | return; 307 | 308 | // lidar_fals.computeMInverse(cos_theta, sin_theta, cos_phi, sin_phi); 309 | } 310 | 311 | void RangeImage::buildTableFromRings(const int &num_rings, const int &num_horizon_scan, 312 | const pcl::PointCloud::Ptr &cloud) { 313 | if (rows_ != num_rings || cols_ != num_horizon_scan) { 314 | std::cout << "rows_ != num_rings || cols_ != num_horizon_scan. reset rows_ and cols\n"; 315 | rows_ = num_rings; 316 | cols_ = num_horizon_scan; 317 | lidar_fals.setRowsCols(num_rings, num_horizon_scan); 318 | } 319 | buildTableFromRings(cloud); 320 | } 321 | 322 | void RangeImage::buildTableFromCloud() { 323 | float bin_res_v = M_PI / (float)rows_; // vertical(radian) 324 | float bin_res_h = 2.0 * M_PI / (float)cols_; // horizon(radian) 325 | 326 | //lidar系下点云 327 | for (int i = 0; i < rows_; ++i) { 328 | float phi = 0.5 * M_PI - ((float)i + 0.5) * bin_res_v; 329 | float c_phi = std::cos(phi); 330 | float s_phi = std::sin(phi); 331 | for (int j = 0; j < cols_; ++j) { 332 | float theta = ((float)j + 0.5) * bin_res_h - M_PI; 333 | cos_theta.at(i, j) = std::cos(theta); 334 | sin_theta.at(i, j) = std::sin(theta); 335 | cos_phi.at(i, j) = c_phi; 336 | sin_phi.at(i, j) = s_phi; 337 | } 338 | } 339 | lidar_fals.computeMInverse(cos_theta, sin_theta, cos_phi, sin_phi); 340 | } 341 | 342 | void RangeImage::createFromCloud(const pcl::PointCloud::Ptr &cloud) { 343 | createFromCloud(rows_, cols_, cloud); 344 | } 345 | 346 | void RangeImage::computeNormals(const cv::Mat &r, const cv::_OutputArray &normals_out) { 347 | // Get the normals 348 | normals_out.create(rows_, cols_, CV_MAKETYPE(depth_, 3)); 349 | 350 | cv::Mat normals = normals_out.getMat(); 351 | lidar_fals.compute(r, normals); 352 | } 353 | 354 | void RangeImage::computeNormals(const cv::Mat &r, const cv::_OutputArray &normals_out, 355 | cv::Mat &residual) { 356 | // Get the normals 357 | normals_out.create(rows_, cols_, CV_MAKETYPE(depth_, 3)); 358 | residual = cv::Mat(rows_, cols_, CV_32F, cv::Scalar::all(FLT_MAX)); 359 | 360 | 361 | cv::Mat normals = normals_out.getMat(); 362 | // cv::Mat res = residual.getMat(); 363 | lidar_fals.compute(r, normals, residual); 364 | } 365 | 366 | void RangeImage::computeCov(const cv::Mat &r, cv::Mat_ & cov_mat) { 367 | cov_mat = cv::Mat_ (rows_, cols_); 368 | 369 | lidar_fals.computeCovariance(r, cov_mat); 370 | } 371 | -------------------------------------------------------------------------------- /src/ring_fals/range_image.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by hk on 12/8/22. 3 | // 4 | 5 | 6 | #include 7 | #include 8 | #include 9 | 10 | #include 11 | #include 12 | #include 13 | 14 | #include "point_type.h" 15 | 16 | #include "Image_normals.hpp" 17 | 18 | #ifndef LVI_SAM_RANGE_IMAGE_H 19 | #define LVI_SAM_RANGE_IMAGE_H 20 | 21 | //in lidar frame [front, left, up] 22 | class RangeImage 23 | { 24 | typedef pcl::PointXYZI PointType; 25 | typedef cv::Matx Mat33T; 26 | typedef cv::Vec Vec9T; 27 | typedef cv::Vec Vec3T; 28 | public: 29 | explicit RangeImage() 30 | : num_bins_x(360) 31 | , num_bins_y(360) 32 | , lidar_fals() 33 | { 34 | // range_image = cv::Mat(num_bins_y, num_bins_x, CV_32F, cv::Scalar::all(FLT_MAX)); 35 | } 36 | 37 | explicit RangeImage(const int& rows, const int& cols ) 38 | : rows_(rows) 39 | , cols_(cols) 40 | , lidar_fals(rows, cols, 3) //(row，col，window_size) 41 | { 42 | // range_image = cv::Mat(num_bins_y, num_bins_x, CV_32F, cv::Scalar::all(FLT_MAX)); 43 | 44 | //structural information 45 | cos_theta = cv::Mat(rows_, cols_, CV_32F); 46 | sin_theta = cv::Mat(rows_, cols_, CV_32F); 47 | cos_phi = cv::Mat(rows_, cols_, CV_32F); 48 | sin_phi = cv::Mat(rows_, cols_, CV_32F); 49 | num_per_pixel.resize(rows_ * cols_, 0); 50 | } 51 | 52 | void createFromRings(const pcl::PointCloud::Ptr& cloud); 53 | 54 | //create range imageg and build lookup table for normal computation 55 | void createAndBuildTableFromRings(const pcl::PointCloud::Ptr& cloud); 56 | void createAndBuildTableFromRings(const int& num_rings, const int& num_horizon_scan 57 | , const pcl::PointCloud::Ptr& cloud); 58 | 59 | void buildTableFromRings(const pcl::PointCloud::Ptr& cloud); 60 | void buildTableFromRings(const int& num_rings, const int& num_horizon_scan 61 | , const pcl::PointCloud::Ptr& cloud); 62 | 63 | void createFromCloud(const int& num_bins_v, const int& num_bins_h, 64 | const pcl::PointCloud::Ptr& cloud); 65 | void createFromCloud(const pcl::PointCloud::Ptr& cloud); 66 | 67 | void buildTableFromCloud(); 68 | 69 | void saveRangeImage(const std::string& file); 70 | 71 | void computeNormals(); 72 | void computeNormals(cv::OutputArray normals_out); 73 | void computeNormals(const cv::Mat &range_image, cv::OutputArray normals_out); 74 | void computeNormals(const cv::Mat &range_image, cv::OutputArray normals_out, cv::Mat &residual); 75 | 76 | void computeCov(const cv::Mat &range_image, cv::Mat_ & cov_mat); 77 | 78 | 79 | void computeLookupTable(); 80 | 81 | bool loadLookupTable(std::string dir, std::string filename) 82 | {return lidar_fals.loadMInverse(dir, filename);} 83 | 84 | void saveLookupTable(std::string dir, std::string filename) 85 | {lidar_fals.saveMInverse(dir, filename);} 86 | 87 | void computeMInverse() {lidar_fals.computeMInverse(cos_theta, sin_theta, cos_phi, sin_phi);} 88 | 89 | enum CoordinateFrame 90 | { 91 | CAMERA_FRAME = 0, 92 | LASER_FRAME = 1 93 | }; 94 | 95 | // The actual range iamge. 96 | cv::Mat range_image; 97 | 98 | //only for visualization 99 | std::vector cloud_id_image; 100 | private: 101 | 102 | int num_bins_x, num_bins_y; 103 | 104 | float resolution_x, resolution_y; 105 | 106 | float range_min = 1.0; 107 | float range_max = 200.0; 108 | 109 | float input_value_max = 0; 110 | int downsampleRate = 1; 111 | 112 | cv::Scalar range_init = cv::Scalar::all(FLT_MAX); 113 | 114 | int num_cloud = 0; 115 | int min_table_cloud = 10; 116 | 117 | int rows_, cols_; 118 | int depth_ = CV_32F; 119 | cv::Mat K_; 120 | int window_size_; 121 | int method_; 122 | // mutable void* rgbd_normals_impl_; 123 | LIDAR_FALS lidar_fals; 124 | bool table_valid = false; 125 | // cv::Mat_ V_; //sin(theta) * cos(phi), sin(phi), cos(theta) * cos(phi) 126 | // cv::Mat_ M_inv_; //M^-1 127 | 128 | cv::Mat cos_theta; 129 | cv::Mat sin_theta; 130 | cv::Mat cos_phi; 131 | cv::Mat sin_phi; 132 | std::vector num_per_pixel; //points in each pixel 133 | }; 134 | 135 | 136 | #endif //LVI_SAM_RANGE_IMAGE_H 137 | -------------------------------------------------------------------------------- /src/tic_toc.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | 7 | class TicToc 8 | { 9 | public: 10 | TicToc() 11 | { 12 | tic(); 13 | } 14 | 15 | void tic() 16 | { 17 | start = std::chrono::system_clock::now(); 18 | } 19 | 20 | double toc() 21 | { 22 | end = std::chrono::system_clock::now(); 23 | std::chrono::duration elapsed_seconds = end - start; 24 | return elapsed_seconds.count() * 1000; 25 | } 26 | 27 | private: 28 | std::chrono::time_point start, end; 29 | }; 30 | -------------------------------------------------------------------------------- /src/visualizer_node.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Created by hk on 4/10/24. 3 | // 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include "IMU_Processing.hpp" 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | #include 24 | #include 25 | //#include 26 | #include 27 | 28 | #include 29 | #include 30 | 31 | #include "point_type.h" 32 | 33 | string root_dir = ROOT_DIR; 34 | //bool publish_voxel_map = false; 35 | int publish_max_voxel_layer = 0; 36 | 37 | typedef pcl::PointCloud voxel_map; 38 | typedef pcl::PointCloud::Ptr voxel_map_ptr; 39 | 40 | float max_color_scale = 0.0; 41 | float use_alpha = 0.8; 42 | float max_color_coef; 43 | 44 | void mapJet(double v, double vmin, double vmax, uint8_t &r, uint8_t &g, 45 | uint8_t &b) { 46 | r = 255; 47 | g = 255; 48 | b = 255; 49 | 50 | if (v < vmin) { 51 | v = vmin; 52 | } 53 | 54 | if (v > vmax) { 55 | v = vmax; 56 | } 57 | 58 | double dr, dg, db; 59 | 60 | if (v < 0.1242) { 61 | db = 0.504 + ((1. - 0.504) / 0.1242) * v; 62 | dg = dr = 0.; 63 | } else if (v < 0.3747) { 64 | db = 1.; 65 | dr = 0.; 66 | dg = (v - 0.1242) * (1. / (0.3747 - 0.1242)); 67 | } else if (v < 0.6253) { 68 | db = (0.6253 - v) * (1. / (0.6253 - 0.3747)); 69 | dg = 1.; 70 | dr = (v - 0.3747) * (1. / (0.6253 - 0.3747)); 71 | } else if (v < 0.8758) { 72 | db = 0.; 73 | dr = 1.; 74 | dg = (0.8758 - v) * (1. / (0.8758 - 0.6253)); 75 | } else { 76 | db = 0.; 77 | dg = 0.; 78 | dr = 1. - (v - 0.8758) * ((1. - 0.504) / (1. - 0.8758)); 79 | } 80 | 81 | r = (uint8_t)(255 * dr); 82 | g = (uint8_t)(255 * dg); 83 | b = (uint8_t)(255 * db); 84 | } 85 | 86 | void CalcVectQuation(const Eigen::Vector3d &x_vec, const Eigen::Vector3d &y_vec, 87 | const Eigen::Vector3d &z_vec, 88 | geometry_msgs::Quaternion &q) { 89 | 90 | Eigen::Matrix3d rot; 91 | rot << x_vec(0), x_vec(1), x_vec(2), y_vec(0), y_vec(1), y_vec(2), z_vec(0), 92 | z_vec(1), z_vec(2); 93 | Eigen::Matrix3d rotation = rot.transpose(); 94 | Eigen::Quaterniond eq(rotation); 95 | q.w = eq.w(); 96 | q.x = eq.x(); 97 | q.y = eq.y(); 98 | q.z = eq.z(); 99 | } 100 | 101 | int plane_id = 0; 102 | void pubSinglePlane(visualization_msgs::MarkerArray &plane_pub, 103 | const std::string plane_ns, const voxelMap_PointType & plane_data, 104 | const float alpha, const Eigen::Vector3d rgb) { 105 | visualization_msgs::Marker plane; 106 | plane.header.frame_id = "world"; 107 | plane.header.stamp = ros::Time(); 108 | plane.ns = plane_ns; 109 | plane.id = plane_id++; 110 | plane.type = visualization_msgs::Marker::CYLINDER; 111 | plane.action = visualization_msgs::Marker::ADD; 112 | plane.pose.position.x = plane_data.x; 113 | plane.pose.position.y = plane_data.y; 114 | plane.pose.position.z = plane_data.z; 115 | V3D eigenvector_max(plane_data.eigenvector_max_x, plane_data.eigenvector_max_y, plane_data.eigenvector_max_z); 116 | V3D eigenvector_mid(plane_data.eigenvector_mid_x, plane_data.eigenvector_mid_y, plane_data.eigenvector_mid_z); 117 | V3D eigenvector_min(plane_data.eigenvector_min_x, plane_data.eigenvector_min_y, plane_data.eigenvector_min_z); 118 | geometry_msgs::Quaternion q; 119 | CalcVectQuation(eigenvector_max, eigenvector_mid, eigenvector_min, q); 120 | plane.pose.orientation = q; 121 | plane.scale.x = 3 * sqrt(plane_data.eigenvalue_max); 122 | plane.scale.y = 3 * sqrt(plane_data.eigenvalue_mid); 123 | plane.scale.z = 2 * sqrt(plane_data.eigenvalue_min); 124 | plane.color.a = alpha; 125 | plane.color.r = rgb(0); 126 | plane.color.g = rgb(1); 127 | plane.color.b = rgb(2); 128 | plane.lifetime = ros::Duration(); 129 | plane_pub.markers.push_back(plane); 130 | } 131 | 132 | void pubVoxelMap(const voxel_map_ptr & vm_ptr, 133 | // const int pub_max_voxel_layer, 134 | const ros::Publisher &plane_map_pub) { 135 | 136 | 137 | visualization_msgs::MarkerArray voxel_plane; 138 | voxel_plane.markers.reserve(1000000); 139 | for (size_t i = 0; i < vm_ptr->size(); i++) { 140 | const voxelMap_PointType & voxel_data = vm_ptr->points[i]; 141 | // double trace = voxel_data.eigenvalue_max + voxel_data.eigenvalue_mid + voxel_data.eigenvalue_min; 142 | float color_scale = voxel_data.eigenvalue_min; 143 | 144 | if (color_scale >= max_color_scale) { 145 | color_scale = max_color_scale; 146 | } 147 | color_scale = color_scale * (1.0 / max_color_scale); 148 | // trace = pow(trace, pow_num); 149 | 150 | uint8_t r, g, b; 151 | mapJet(color_scale, 0, 1, r, g, b); 152 | Eigen::Vector3d plane_rgb(r / 256.0, g / 256.0, b / 256.0); 153 | double alpha; 154 | alpha = use_alpha; 155 | // if (pub_plane_list[i].is_plane) { 156 | // alpha = use_alpha; 157 | // } else { 158 | // alpha = 0; 159 | // } 160 | pubSinglePlane(voxel_plane, "plane", voxel_data, alpha, plane_rgb); 161 | } 162 | plane_map_pub.publish(voxel_plane); 163 | // loop.sleep(); 164 | } 165 | 166 | 167 | 168 | int main(int argc, char** argv) 169 | { 170 | ros::init(argc, argv, "laserMapping"); 171 | ros::NodeHandle nh; 172 | 173 | // // visualization params 174 | nh.param("max_color_scale", max_color_scale, 0.5); 175 | // nh.param("publish/publish_max_voxel_layer", publish_max_voxel_layer, 0); 176 | // 177 | // string pose_target_file = root_dir + "/Log/target_path.txt"; 178 | // string pos_target_end_time = root_dir + "/Log/target_end_path.txt"; 179 | 180 | ros::Publisher voxel_map_pub = nh.advertise("/planes", 10000); 181 | string voxel_map_file = root_dir + "/Log/voxel_map.pcd"; 182 | voxel_map_ptr planes(new voxel_map); 183 | pcl::io::loadPCDFile(voxel_map_file, *planes); 184 | cout << *planes << endl; 185 | 186 | 187 | double pow_num = 0.2; 188 | ros::Rate loop(500); 189 | 190 | // for (size_t i = 0; i < planes->size(); i++) { 191 | // const voxelMap_PointType &voxel_data = planes->points[i]; 192 | // float color_scale = voxel_data.eigenvalue_min; 193 | // max_color_scale = max(color_scale, max_color_scale); 194 | // } 195 | // max_color_scale *= max_color_coef; 196 | // cout << "max_color_scale" << max_color_scale << endl; 197 | ROS_INFO("max_color_scale: %f", max_color_scale); 198 | 199 | // signal(SIGINT, SigHandle); 200 | bool is_published = false; 201 | ros::Rate rate(1000); 202 | // bool status = ros::ok(); 203 | while ( ros::ok()) 204 | { 205 | /******* Publish voxel map *******/ 206 | if (!is_published && voxel_map_pub.getNumSubscribers() > 0) { 207 | pubVoxelMap(planes, voxel_map_pub); 208 | is_published = true; 209 | } 210 | // status = ros::ok(); 211 | rate.sleep(); 212 | } 213 | return 0; 214 | } 215 | --------------------------------------------------------------------------------