├── CMakeLists.txt ├── Log └── .gitignore ├── README.md ├── config ├── avia.yaml ├── nclt.yaml ├── ntu.yaml ├── offline_pointcloud.yaml └── velodyne.yaml ├── font └── Roboto-Medium.ttf ├── include ├── common_lib.h ├── feature.h ├── ikd-Tree │ ├── README.md │ ├── ikd_Tree.cpp │ └── ikd_Tree.h ├── so3_math.h └── types.h ├── launch ├── mapping_avia.launch ├── mapping_nclt.launch ├── mapping_ntu_viral.launch ├── mapping_pointcloud.launch └── mapping_velody64.launch ├── msg ├── Pose6D.msg └── States.msg ├── package.xml ├── paper └── ImMesh_tro.pdf ├── rviz_cfg └── ImMesh.rviz ├── src ├── IMU_Processing.cpp ├── IMU_Processing.h ├── ImMesh_mesh_reconstruction.cpp ├── ImMesh_node.cpp ├── meshing │ ├── delaunay │ │ └── openCV_subdiv2d_index.hpp │ ├── imgui.ini │ ├── mesh_rec_display.cpp │ ├── mesh_rec_display.hpp │ ├── mesh_rec_geometry.cpp │ ├── mesh_rec_geometry.hpp │ ├── optical_flow │ │ ├── lkpyramid.cpp │ │ └── lkpyramid.hpp │ ├── package.xml │ ├── r3live │ │ ├── image_frame.cpp │ │ ├── image_frame.hpp │ │ ├── pointcloud_rgbd.cpp │ │ ├── pointcloud_rgbd.hpp │ │ ├── triangle.cpp │ │ └── triangle.hpp │ └── tinycolormap.hpp ├── preprocess.cpp ├── preprocess.h ├── shader │ ├── image_shader.fs │ ├── image_shader.vs │ ├── learnopengl │ │ ├── animation.h │ │ ├── animator.h │ │ ├── animdata.h │ │ ├── assimp_glm_helpers.h │ │ ├── bone.h │ │ ├── camera.h │ │ ├── entity.h │ │ ├── filesystem.h │ │ ├── mesh.h │ │ ├── model.h │ │ ├── model_animation.h │ │ ├── shader.h │ │ ├── shader_c.h │ │ ├── shader_m.h │ │ ├── shader_s.h │ │ └── shader_t.h │ ├── points_shader.fs │ ├── points_shader.vs │ ├── points_shader_rgb.fs │ ├── points_shader_rgb.vs │ ├── shader_m.h │ ├── shader_s.h │ ├── stb_image.h │ ├── stb_image_aug.c │ ├── stb_image_aug.h │ ├── tools_my_camera_pose_shader.h │ ├── tools_my_dbg_utility.h │ ├── tools_my_point_shader.h │ ├── tools_my_shader_common.h │ ├── tools_my_texture_triangle_shader.h │ ├── triangle_facets.fs │ └── triangle_facets.vs ├── tools │ ├── common_tools.h │ ├── imgui │ │ ├── imconfig.h │ │ ├── imgui.cpp │ │ ├── imgui.h │ │ ├── imgui_demo.cpp │ │ ├── imgui_draw.cpp │ │ ├── imgui_impl_allegro5.cpp │ │ ├── imgui_impl_allegro5.h │ │ ├── imgui_impl_android.cpp │ │ ├── imgui_impl_android.h │ │ ├── imgui_impl_dx10.cpp │ │ ├── imgui_impl_dx10.h │ │ ├── imgui_impl_dx11.cpp │ │ ├── imgui_impl_dx11.h │ │ ├── imgui_impl_dx12.cpp │ │ ├── imgui_impl_dx12.h │ │ ├── imgui_impl_dx9.cpp │ │ ├── imgui_impl_dx9.h │ │ ├── imgui_impl_glfw.cpp │ │ ├── imgui_impl_glfw.h │ │ ├── imgui_impl_glut.cpp │ │ ├── imgui_impl_glut.h │ │ ├── imgui_impl_metal.h │ │ ├── imgui_impl_metal.mm │ │ ├── imgui_impl_opengl2.cpp │ │ ├── imgui_impl_opengl2.h │ │ ├── imgui_impl_opengl3.cpp │ │ ├── imgui_impl_opengl3.h │ │ ├── imgui_impl_opengl3_loader.h │ │ ├── imgui_impl_osx.h │ │ ├── imgui_impl_osx.mm │ │ ├── imgui_impl_sdl.cpp │ │ ├── imgui_impl_sdl.h │ │ ├── imgui_impl_sdlrenderer.cpp │ │ ├── imgui_impl_sdlrenderer.h │ │ ├── imgui_impl_vulkan.cpp │ │ ├── imgui_impl_vulkan.h │ │ ├── imgui_impl_wgpu.cpp │ │ ├── imgui_impl_wgpu.h │ │ ├── imgui_impl_win32.cpp │ │ ├── imgui_impl_win32.h │ │ ├── imgui_internal.h │ │ ├── imgui_tables.cpp │ │ ├── imgui_widgets.cpp │ │ ├── imstb_rectpack.h │ │ ├── imstb_textedit.h │ │ └── imstb_truetype.h │ ├── lib_sophus │ │ ├── average.hpp │ │ ├── common.hpp │ │ ├── example_ensure_handler.cpp │ │ ├── formatstring.hpp │ │ ├── geometry.hpp │ │ ├── interpolate.hpp │ │ ├── interpolate_details.hpp │ │ ├── local_parameterization_se3.hpp │ │ ├── local_parameterization_split_se3.hpp │ │ ├── num_diff.hpp │ │ ├── rotation_matrix.hpp │ │ ├── rxso2.hpp │ │ ├── rxso3.hpp │ │ ├── se2.hpp │ │ ├── se3.hpp │ │ ├── sim2.hpp │ │ ├── sim3.hpp │ │ ├── sim_details.hpp │ │ ├── so2.hpp │ │ ├── so3.hpp │ │ ├── test_macros.hpp │ │ ├── types.hpp │ │ └── velocities.hpp │ ├── openGL_libs │ │ ├── gl_draw_founction.hpp │ │ ├── glad.c │ │ ├── glad.h │ │ ├── glad │ │ │ └── glad.h │ │ ├── glformattraits.h │ │ ├── openGL_camera.cpp │ │ ├── openGL_camera.hpp │ │ ├── openGL_camera_view.cpp │ │ └── tinycolormap.hpp │ ├── os_compatible.hpp │ ├── tinycolormap.hpp │ ├── tools_angleaxis_ad.h │ ├── tools_ceres.hpp │ ├── tools_color_printf.hpp │ ├── tools_data_io.hpp │ ├── tools_eigen.hpp │ ├── tools_graphics.hpp │ ├── tools_kd_hash.hpp │ ├── tools_logger.hpp │ ├── tools_mem_used.h │ ├── tools_openCV_3_to_4.hpp │ ├── tools_random.hpp │ ├── tools_ros.hpp │ ├── tools_serialization.hpp │ ├── tools_thread_pool.hpp │ └── tools_timer.hpp ├── voxel_loc.cpp ├── voxel_loc.hpp ├── voxel_mapping.cpp ├── voxel_mapping.hpp └── voxel_mapping_common.cpp └── supply └── Supplementary_material.pdf /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(ImMesh) 3 | 4 | SET(CMAKE_BUILD_TYPE "Release") 5 | 6 | ADD_COMPILE_OPTIONS(-std=c++14 ) 7 | 8 | add_definitions(-DROOT_DIR=\"${CMAKE_CURRENT_SOURCE_DIR}/\") 9 | 10 | set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -fexceptions" ) 11 | set(CMAKE_CXX_STANDARD 14) 12 | set(CMAKE_CXX_STANDARD_REQUIRED ON) 13 | set(CMAKE_CXX_EXTENSIONS OFF) 14 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -g -std=c++17 -g -O3 -ltbb -lboost_system -msse2 -msse3 -pthread -w") # -Wall 15 | 16 | message("Current CPU archtecture: ${CMAKE_SYSTEM_PROCESSOR}") 17 | if(CMAKE_SYSTEM_PROCESSOR MATCHES "(x86)|(X86)|(amd64)|(AMD64)" ) 18 | include(ProcessorCount) 19 | ProcessorCount(N) 20 | message("Processer number: ${N}") 21 | if(N GREATER 5) 22 | add_definitions(-DMP_EN) 23 | add_definitions(-DMP_PROC_NUM=4) 24 | message("core for MP: 4") 25 | elseif(N GREATER 3) 26 | math(EXPR PROC_NUM "${N} - 2") 27 | add_definitions(-DMP_EN) 28 | add_definitions(-DMP_PROC_NUM="${PROC_NUM}") 29 | message("core for MP: ${PROC_NUM}") 30 | else() 31 | add_definitions(-DMP_PROC_NUM=1) 32 | endif() 33 | else() 34 | add_definitions(-DMP_PROC_NUM=1) 35 | endif() 36 | 37 | find_package(OpenMP QUIET) 38 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS}") 39 | set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${OpenMP_C_FLAGS}") 40 | 41 | find_package(catkin REQUIRED COMPONENTS 42 | geometry_msgs 43 | nav_msgs 44 | sensor_msgs 45 | roscpp 46 | rospy 47 | std_msgs 48 | pcl_ros 49 | tf 50 | livox_ros_driver 51 | message_generation 52 | eigen_conversions 53 | cv_bridge 54 | ) 55 | 56 | find_package(Eigen3 REQUIRED) 57 | find_package(PCL 1.6 REQUIRED) 58 | find_package(OpenCV REQUIRED) 59 | find_package(CGAL REQUIRED) 60 | FIND_PACKAGE(Boost REQUIRED COMPONENTS filesystem iostreams program_options system serialization) 61 | set(Sophus_LIBRARIES libSophus.so) 62 | 63 | message(Eigen: ${EIGEN3_INCLUDE_DIR}) 64 | 65 | include_directories( 66 | ${catkin_INCLUDE_DIRS} 67 | ${EIGEN3_INCLUDE_DIR} 68 | ${PCL_INCLUDE_DIRS} 69 | ${PYTHON_INCLUDE_DIRS} 70 | # ${OpenCV_INCLUDE_DIRS} 71 | ${Sophus_INCLUDE_DIRS} 72 | ./include 73 | 74 | # ===== Meshing ===== 75 | ./include/ikd-Tree/ 76 | ./src/meshing/ 77 | ./src/meshing//r3live/ 78 | ./src/meshing//kd_tree_dev/ 79 | ./src/meshing//rgb_map/ 80 | ./src/tools/ 81 | ./src/tools/imgui 82 | ./src/tools/shader 83 | ./src/tools/openGL_libs 84 | ./src 85 | ) 86 | 87 | add_message_files( 88 | FILES 89 | Pose6D.msg 90 | # States.msg 91 | ) 92 | 93 | generate_messages( 94 | DEPENDENCIES 95 | geometry_msgs 96 | ) 97 | 98 | catkin_package( 99 | CATKIN_DEPENDS geometry_msgs nav_msgs roscpp rospy std_msgs message_runtime 100 | DEPENDS EIGEN3 PCL OpenCV Sophus 101 | INCLUDE_DIRS 102 | ) 103 | 104 | set(FONTPATH ${CMAKE_CURRENT_SOURCE_DIR}/font/) 105 | add_definitions(-DFONT_DIR=\"${FONTPATH}/\") 106 | 107 | set(SHADERPATH ${CMAKE_CURRENT_SOURCE_DIR}/src/shader/) 108 | add_definitions(-DSHADER_DIR=\"${SHADERPATH}/\") 109 | 110 | add_executable(ImMesh_mapping 111 | src/ImMesh_node.cpp 112 | src/ImMesh_mesh_reconstruction.cpp 113 | include/ikd-Tree/ikd_Tree.cpp 114 | src/voxel_mapping.cpp 115 | src/voxel_mapping_common.cpp 116 | src/voxel_loc.cpp 117 | src/preprocess.cpp 118 | src/IMU_Processing.cpp 119 | 120 | ./src/meshing/mesh_rec_display.cpp 121 | ./src/meshing/mesh_rec_geometry.cpp 122 | ./src/meshing/r3live/triangle.cpp 123 | ./src/meshing/r3live/image_frame.cpp 124 | ./src/meshing/r3live/pointcloud_rgbd.cpp 125 | ./src/tools/openGL_libs/openGL_camera.cpp 126 | ./src/tools/openGL_libs/openGL_camera_view.cpp 127 | ./src/tools/openGL_libs/glad.c 128 | 129 | ./src/tools/imgui/imgui.cpp 130 | ./src/tools/imgui/imgui_draw.cpp 131 | ./src/tools/imgui/imgui_impl_glfw.cpp 132 | ./src/tools/imgui/imgui_impl_opengl3.cpp 133 | ./src/tools/imgui/imgui_tables.cpp 134 | ./src/tools/imgui/imgui_widgets.cpp 135 | ) 136 | target_link_libraries(ImMesh_mapping 137 | ${catkin_LIBRARIES} 138 | ${PCL_LIBRARIES} 139 | ${PYTHON_LIBRARIES} 140 | CGAL::CGAL 141 | tbb 142 | ${OpenCV_LIBRARIES} 143 | ${Boost_LIBRARIES} 144 | ${Boost_SYSTEM_LIBRARIES} 145 | ${Boost_FILESYSTEM_LIBRARY} 146 | ${Boost_SERIALIZATION_LIBRARY} # serialization 147 | ${CERES_LIBRARIES} 148 | pcl_common 149 | pcl_io 150 | pcl_filters 151 | pcl_kdtree 152 | GL GLU glfw X11 Xrandr pthread Xi dl Xinerama Xcursor) -------------------------------------------------------------------------------- /Log/.gitignore: -------------------------------------------------------------------------------- 1 | /imu.txt 2 | /mat_pre.txt 3 | /kitti_log_voxelmap.txt 4 | -------------------------------------------------------------------------------- /config/avia.yaml: -------------------------------------------------------------------------------- 1 | feature_extract_enable : 0 2 | point_filter_num : 1 3 | max_iteration : 4 4 | dense_map_enable : 1 5 | filter_size_surf : 0.4 6 | filter_size_map : 0.4 7 | cube_side_length : 1000 8 | debug : 0 9 | min_img_count : 150000 10 | grid_size : 40 11 | patch_size : 8 12 | img_enable : 0 13 | lidar_enable : 1 14 | outlier_threshold : 78 # 78 100 156 15 | img_point_cov : 1000 #100 16 | laser_point_cov : 0.001 #0.001 17 | 18 | common: 19 | lid_topic: "/livox/lidar" 20 | imu_topic: "/livox/imu" 21 | 22 | preprocess: 23 | lidar_type: 1 # Livox Avia LiDAR 24 | scan_line: 6 25 | timestamp_unit: 2 26 | blind: 1 # blind x m disable 27 | 28 | mapping: 29 | imu_en: true 30 | imu_int_frame: 30 31 | acc_cov: 0.5 32 | gyr_cov: 0.3 33 | b_acc_cov: 0.01 34 | b_gyr_cov: 0.01 35 | fov_degree: 180 36 | # extrinsic_T: [ 0.0, 0.0, -0.0 ] 37 | # extrinsic_R: [ 1, 0, 0, 38 | # 0, 1, 0, 39 | # 0, 0, 1] 40 | extrinsic_T: [ 0.04165, 0.02326, -0.0284 ] 41 | extrinsic_R: [ 1, 0, 0, 42 | 0, 1, 0, 43 | 0, 0, 1] 44 | 45 | voxel: 46 | voxel_map_en: true 47 | pub_plane_en: false 48 | dept_err: 0.02 49 | beam_err: 0.05 50 | min_eigen_value: 0.01 51 | match_s: 0.9 52 | sigma_num: 3 53 | voxel_size: 0.5 54 | max_layer: 2 55 | max_points_size: 100 56 | layer_init_size: [5,5,5,5,5] 57 | 58 | publish: 59 | path_en: false 60 | scan_publish_en: true # false: close all the point cloud output 61 | dense_publish_en: false # false: low down the points number in a global-frame point clouds scan. 62 | scan_bodyframe_pub_en: false # true: output the point cloud scans in IMU-body-frame 63 | effect_point_pub: true 64 | 65 | pcd_save: 66 | pcd_save_en: false 67 | type: 0 # 0 - World Frame, 1 - Body Frame; 68 | interval: -1 # how many LiDAR frames saved in each pcd file; 69 | # -1 : all frames will be saved in ONE pcd file, may lead to memory crash when having too much frames. 70 | 71 | image_save: 72 | img_save_en: false 73 | rot_dist: 0.0 # 0.05 74 | pos_dist: 0.0 # 0.15 75 | interval: 1 76 | 77 | meshing: 78 | points_minimum_scale: 0.1 # The minimum distance between any of two points 79 | voxel_resolution: 0.4 # The resolution of voxel 80 | region_size: 10.0 # Size of region 81 | number_of_pts_append_to_map: 10000 # Number of pts appending to map (per-frame) 82 | if_draw_mesh: 1 83 | 84 | 85 | 86 | -------------------------------------------------------------------------------- /config/nclt.yaml: -------------------------------------------------------------------------------- 1 | feature_extract_enable : 0 2 | point_filter_num : 10 3 | max_iteration : 2 4 | dense_map_enable : 0 5 | filter_size_surf : 0.75 6 | filter_size_map : 0.75 7 | cube_side_length : 1000 8 | debug : 0 9 | min_img_count : 150000 10 | grid_size : 40 11 | patch_size : 8 12 | img_enable : 0 13 | lidar_enable : 1 14 | outlier_threshold : 78 # 78 100 156 15 | img_point_cov : 1000 #100 16 | laser_point_cov : 0.001 #0.001 17 | 18 | common: 19 | lid_topic: "/points_raw" 20 | imu_topic: "/imu_raw" 21 | 22 | preprocess: 23 | lidar_type: 6 # Velodyne32 Lidar 24 | scan_line: 32 25 | timestamp_unit: 2 26 | blind: 1 # blind x m disable 27 | calib_laser: false # true for KITTI Odometry dataset 28 | 29 | mapping: 30 | imu_en: true 31 | imu_int_frame: 30 32 | acc_cov: 0.5 33 | gyr_cov: 0.3 34 | fov_degree: 180 35 | extrinsic_T: [ 0.0, 0.0, 0.28] 36 | extrinsic_R: [ 1, 0, 0, 37 | 0, 1, 0, 38 | 0, 0, 1] 39 | 40 | voxel: 41 | voxel_map_en: true 42 | pub_plane_en: false 43 | dept_err: 0.03 44 | beam_err: 0.1 45 | min_eigen_value: 0.01 46 | match_s: 0.9 47 | sigma_num: 3 48 | voxel_size: 2 49 | max_layer: 2 50 | max_points_size: 100 51 | layer_init_size: [5,5,5,5,5] 52 | 53 | 54 | camera: 55 | img_topic: /left_camera/image 56 | Rcl: [ 0.000910902, -0.999994, 0.00343779, # new extrinsic 57 | -0.0101394, -0.00344672, -0.999943, 58 | 0.999948, 0.000875869, -0.0101423] 59 | Pcl: [0.051708, -0.004601, -0.023075] 60 | 61 | publish: 62 | path_en: false 63 | scan_publish_en: true # false: close all the point cloud output 64 | dense_publish_en: false # false: low down the points number in a global-frame point clouds scan. 65 | scan_bodyframe_pub_en: false # true: output the point cloud scans in IMU-body-frame 66 | effect_point_pub: true 67 | pub_point_skip: 100 68 | 69 | pcd_save: 70 | pcd_save_en: false 71 | type: 0 # 0 - World Frame, 1 - Body Frame; 72 | interval: -1 # how many LiDAR frames saved in each pcd file; 73 | # -1 : all frames will be saved in ONE pcd file, may lead to memory crash when having too much frames. 74 | 75 | image_save: 76 | img_save_en: false 77 | rot_dist: 0.0 # 0.05 78 | pos_dist: 0.0 # 0.15 79 | interval: 1 80 | 81 | meshing: 82 | points_minimum_scale: 0.1 # The minimum distance between any of two points 83 | voxel_resolution: 0.4 # The resolution of voxel 84 | region_size: 10.0 # Size of region 85 | number_of_pts_append_to_map: 10000 # Number of pts appending to map (per-frame) 86 | if_draw_mesh: 1 -------------------------------------------------------------------------------- /config/ntu.yaml: -------------------------------------------------------------------------------- 1 | feature_extract_enable : 0 2 | point_filter_num : 1 3 | max_iteration : 4 4 | dense_map_enable : 0 5 | filter_size_surf : 0.25 6 | filter_size_map : 0.25 7 | cube_side_length : 1000 8 | debug : 0 9 | min_img_count : 150000 10 | grid_size : 40 11 | patch_size : 8 12 | img_enable : 0 13 | lidar_enable : 1 14 | outlier_threshold : 78 # 78 100 156 15 | img_point_cov : 1000 #100 16 | laser_point_cov : 0.001 #0.001 17 | 18 | common: 19 | lid_topic: "/os1_cloud_node1/points" 20 | imu_topic: "/os1_cloud_node1/imu" 21 | 22 | preprocess: 23 | lidar_type: 3 # Livox Avia LiDAR 24 | scan_line: 16 25 | timestamp_unit: 3 26 | blind: 1 # blind x m disable 27 | 28 | mapping: 29 | imu_en: true 30 | imu_int_frame: 30 31 | acc_cov: 0.5 32 | gyr_cov: 0.3 33 | b_acc_cov: 0.0001 34 | b_gyr_cov: 0.0001 35 | fov_degree: 180 36 | extrinsic_T: [ -0.050, 0.000, 0.055 ] 37 | extrinsic_R: [ 1, 0, 0, 38 | 0, 1, 0, 39 | 0, 0, 1] 40 | 41 | voxel: 42 | voxel_map_en: true 43 | pub_plane_en: false 44 | dept_err: 0.02 45 | beam_err: 0.05 46 | min_eigen_value: 0.01 47 | match_s: 0.9 48 | sigma_num: 3 49 | voxel_size: 1 50 | max_layer: 2 51 | max_points_size: 100 52 | layer_init_size: [5,5,5,5,5] 53 | 54 | publish: 55 | path_en: false 56 | scan_publish_en: true # false: close all the point cloud output 57 | dense_publish_en: false # false: low down the points number in a global-frame point clouds scan. 58 | scan_bodyframe_pub_en: false # true: output the point cloud scans in IMU-body-frame 59 | effect_point_pub: true 60 | pub_point_skip: 1 61 | pcd_save: 62 | pcd_save_en: false 63 | type: 0 # 0 - World Frame, 1 - Body Frame; 64 | interval: -1 # how many LiDAR frames saved in each pcd file; 65 | # -1 : all frames will be saved in ONE pcd file, may lead to memory crash when having too much frames. 66 | 67 | image_save: 68 | img_save_en: false 69 | rot_dist: 0.0 # 0.05 70 | pos_dist: 0.0 # 0.15 71 | interval: 1 72 | 73 | meshing: 74 | points_minimum_scale: 0.1 # The minimum distance between any of two points 75 | voxel_resolution: 0.4 # The resolution of voxel 76 | region_size: 10.0 # Size of region 77 | number_of_pts_append_to_map: 10000 # Number of pts appending to map (per-frame) 78 | if_draw_mesh: 1 79 | 80 | 81 | 82 | -------------------------------------------------------------------------------- /config/offline_pointcloud.yaml: -------------------------------------------------------------------------------- 1 | feature_extract_enable : 0 2 | point_filter_num : 1 3 | max_iteration : 4 4 | dense_map_enable : 1 5 | filter_size_surf : 0.4 6 | filter_size_map : 0.4 7 | cube_side_length : 1000 8 | debug : 0 9 | min_img_count : 150000 10 | grid_size : 40 11 | patch_size : 8 12 | img_enable : 0 13 | lidar_enable : 1 14 | outlier_threshold : 78 # 78 100 156 15 | img_point_cov : 1000 #100 16 | laser_point_cov : 0.001 #0.001 17 | 18 | common: 19 | lid_topic: "/livox/lidar" 20 | imu_topic: "/livox/imu" 21 | 22 | preprocess: 23 | lidar_type: 1 # Livox Avia LiDAR 24 | scan_line: 6 25 | blind: 1 # blind x m disable 26 | 27 | mapping: 28 | imu_en: true 29 | imu_int_frame: 30 30 | acc_cov: 0.5 31 | gyr_cov: 0.3 32 | b_acc_cov: 0.01 33 | b_gyr_cov: 0.01 34 | fov_degree: 180 35 | extrinsic_T: [ 0.04165, 0.02326, -0.0284 ] 36 | extrinsic_R: [ 1, 0, 0, 37 | 0, 1, 0, 38 | 0, 0, 1] 39 | 40 | voxel: 41 | voxel_map_en: true 42 | pub_plane_en: false 43 | dept_err: 0.02 44 | beam_err: 0.05 45 | min_eigen_value: 0.01 46 | match_s: 0.9 47 | sigma_num: 3 48 | voxel_size: 0.5 49 | max_layer: 2 50 | max_points_size: 100 51 | layer_init_size: [5,5,5,5,5] 52 | 53 | publish: 54 | path_en: false 55 | scan_publish_en: true # false: close all the point cloud output 56 | dense_publish_en: false # false: low down the points number in a global-frame point clouds scan. 57 | scan_bodyframe_pub_en: false # true: output the point cloud scans in IMU-body-frame 58 | effect_point_pub: true 59 | 60 | image_save: 61 | img_save_en: false 62 | rot_dist: 0.0 # 0.05 63 | pos_dist: 0.0 # 0.15 64 | interval: 1 65 | 66 | meshing: 67 | distance_scale: 1.0 # The global distance scale 68 | points_minimum_scale: 0.1 # The minimum distance between any of two points 69 | voxel_resolution: 0.4 # The resolution of voxel 70 | region_size: 10.0 # Size of region 71 | number_of_pts_append_to_map: 50000000 # Number of pts appending to map (per-frame) 72 | if_draw_mesh: 1 73 | enable_mesh_rec: 1 74 | -------------------------------------------------------------------------------- /config/velodyne.yaml: -------------------------------------------------------------------------------- 1 | feature_extract_enable : 0 2 | point_filter_num : 1 3 | max_iteration : 3 4 | dense_map_enable : 0 5 | filter_size_surf : 0.5 6 | filter_size_map : 0.5 7 | cube_side_length : 1000 8 | debug : 0 9 | min_img_count : 150000 10 | grid_size : 40 11 | patch_size : 8 12 | img_enable : 0 13 | lidar_enable : 1 14 | outlier_threshold : 78 # 78 100 156 15 | img_point_cov : 1000 #100 16 | laser_point_cov : 0.001 #0.001 17 | 18 | common: 19 | lid_topic: "/velodyne_points" 20 | imu_topic: "" 21 | 22 | preprocess: 23 | lidar_type: 2 # Velodyne Lidar 24 | scan_line: 64 25 | blind: 1 # blind x m disable 26 | calib_laser: true # true for KITTI Odometry dataset 27 | 28 | mapping: 29 | imu_en: false 30 | imu_int_frame: 30 31 | acc_cov: 1.0 32 | gyr_cov: 0.5 33 | fov_degree: 180 34 | extrinsic_T: [ 0.0, 0.0, 0.0] 35 | extrinsic_R: [ 1, 0, 0, 36 | 0, 1, 0, 37 | 0, 0, 1] 38 | 39 | voxel: 40 | voxel_map_en: true 41 | pub_plane_en: false 42 | dept_err: 0.04 43 | beam_err: 0.1 44 | min_eigen_value: 0.01 45 | match_s: 0.9 46 | sigma_num: 3 47 | voxel_size: 3 48 | max_layer: 4 49 | max_points_size: 1000 50 | layer_init_size: [5,5,5,5,5] 51 | 52 | camera: 53 | img_topic: /left_camera/image 54 | Rcl: [ 0.000910902, -0.999994, 0.00343779, # new extrinsic 55 | -0.0101394, -0.00344672, -0.999943, 56 | 0.999948, 0.000875869, -0.0101423] 57 | Pcl: [0.051708, -0.004601, -0.023075] 58 | 59 | publish: 60 | path_en: false 61 | scan_publish_en: true # false: close all the point cloud output 62 | dense_publish_en: false # false: low down the points number in a global-frame point clouds scan. 63 | scan_bodyframe_pub_en: false # true: output the point cloud scans in IMU-body-frame 64 | effect_point_pub: true 65 | pub_point_skip: 5 66 | 67 | pcd_save: 68 | pcd_save_en: false 69 | type: 0 # 0 - World Frame, 1 - Body Frame; 70 | interval: -1 # how many LiDAR frames saved in each pcd file; 71 | # -1 : all frames will be saved in ONE pcd file, may lead to memory crash when having too much frames. 72 | 73 | image_save: 74 | img_save_en: false 75 | rot_dist: 0.0 # 0.05 76 | pos_dist: 0.0 # 0.15 77 | interval: 1 78 | 79 | meshing: 80 | points_minimum_scale: 0.1 # The minimum distance between any of two points 81 | voxel_resolution: 0.4 # The resolution of voxel 82 | region_size: 10.0 # Size of region 83 | number_of_pts_append_to_map: 10000 # Number of pts appending to map (per-frame) 84 | if_draw_mesh: 1 -------------------------------------------------------------------------------- /font/Roboto-Medium.ttf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/hku-mars/ImMesh/3bbc89e4cd2acfa4797834e79b06e8e86b3fa891/font/Roboto-Medium.ttf -------------------------------------------------------------------------------- /include/feature.h: -------------------------------------------------------------------------------- 1 | // This file is part of SVO - Semi-direct Visual Odometry. 2 | // 3 | // Copyright (C) 2014 Christian Forster 4 | // (Robotics and Perception Group, University of Zurich, Switzerland). 5 | // 6 | // SVO is free software: you can redistribute it and/or modify it under the 7 | // terms of the GNU General Public License as published by the Free Software 8 | // Foundation, either version 3 of the License, or any later version. 9 | // 10 | // SVO is distributed in the hope that it will be useful, but WITHOUT ANY 11 | // WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS 12 | // FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details. 13 | // 14 | // You should have received a copy of the GNU General Public License 15 | // along with this program. If not, see . 16 | 17 | #ifndef SVO_FEATURE_H_ 18 | #define SVO_FEATURE_H_ 19 | 20 | #include 21 | #include 22 | #include 23 | 24 | namespace lidar_selection { 25 | 26 | // A salient image region that is tracked across frames. 27 | struct Feature 28 | { 29 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 30 | 31 | enum FeatureType { 32 | CORNER, 33 | EDGELET 34 | }; 35 | int id_; 36 | FeatureType type; //!< Type can be corner or edgelet. 37 | Frame* frame; //!< Pointer to frame in which the feature was detected. 38 | cv::Mat img; 39 | vector ImgPyr; 40 | Vector2d px; //!< Coordinates in pixels on pyramid level 0. 41 | Vector3d f; //!< Unit-bearing vector of the feature. 42 | int level; //!< Image pyramid level where feature was extracted. 43 | Point* point; //!< Pointer to 3D point which corresponds to the feature. 44 | Vector2d grad; //!< Dominant gradient direction for edglets, normalized. 45 | float score; 46 | float error; 47 | // Vector2d grad_cur_; //!< edgelete grad direction in cur frame 48 | SE3 T_f_w_; 49 | float* patch; 50 | Feature(Point* _point, float* _patch, const Vector2d& _px, const Vector3d& _f, const SE3& _T_f_w, const float &_score, int _level) : 51 | type(CORNER), 52 | px(_px), 53 | f(_f), 54 | T_f_w_(_T_f_w), 55 | level(_level), 56 | patch(_patch), 57 | point(_point), 58 | score(_score) 59 | {} 60 | inline Vector3d pos() const { return T_f_w_.inverse().translation(); } 61 | }; 62 | 63 | } // namespace lidar_selection 64 | 65 | #endif // SVO_FEATURE_H_ 66 | -------------------------------------------------------------------------------- /include/ikd-Tree/README.md: -------------------------------------------------------------------------------- 1 | # ikd-Tree 2 | ikd-Tree is an incremental k-d tree for robotic applications. 3 | -------------------------------------------------------------------------------- /include/so3_math.h: -------------------------------------------------------------------------------- 1 | #ifndef SO3_MATH_H 2 | #define SO3_MATH_H 3 | 4 | #include 5 | #include 6 | 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 = (R.trace() > 3.0 - 1e-6) ? 0.0 : 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 | Eigen::Matrix RotMtoEuler(const Eigen::Matrix &rot) 85 | { 86 | T sy = sqrt(rot(0,0)*rot(0,0) + rot(1,0)*rot(1,0)); 87 | bool singular = sy < 1e-6; 88 | T x, y, z; 89 | if(!singular) 90 | { 91 | x = atan2(rot(2, 1), rot(2, 2)); 92 | y = atan2(-rot(2, 0), sy); 93 | z = atan2(rot(1, 0), rot(0, 0)); 94 | } 95 | else 96 | { 97 | x = atan2(-rot(1, 2), rot(1, 1)); 98 | y = atan2(-rot(2, 0), sy); 99 | z = 0; 100 | } 101 | Eigen::Matrix ang(x, y, z); 102 | return ang; 103 | } 104 | 105 | #endif 106 | -------------------------------------------------------------------------------- /include/types.h: -------------------------------------------------------------------------------- 1 | #ifndef TYPES_H 2 | #define TYPES_H 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | typedef pcl::PointXYZINormal PointType; 9 | typedef pcl::PointXYZRGB PointTypeRGB; 10 | typedef pcl::PointCloud PointCloudXYZI; 11 | typedef std::vector> PointVector; 12 | typedef pcl::PointCloud PointCloudXYZRGB; 13 | 14 | typedef Eigen::Vector2f V2F; 15 | typedef Eigen::Vector2d V2D; 16 | typedef Eigen::Vector3d V3D; 17 | typedef Eigen::Matrix3d M3D; 18 | typedef Eigen::Vector3f V3F; 19 | typedef Eigen::Matrix3f M3F; 20 | 21 | #define MD(a,b) Eigen::Matrix 22 | #define VD(a) Eigen::Matrix 23 | #define MF(a,b) Eigen::Matrix 24 | #define VF(a) Eigen::Matrix 25 | 26 | struct Pose6D 27 | { 28 | /*** the preintegrated Lidar states at the time of IMU measurements in a frame ***/ 29 | double offset_time; // the offset time of IMU measurement w.r.t the first lidar point 30 | double acc[3]; // the preintegrated total acceleration (global frame) at the Lidar origin 31 | double gyr[3]; // the unbiased angular velocity (body frame) at the Lidar origin 32 | double vel[3]; // the preintegrated velocity (global frame) at the Lidar origin 33 | double pos[3]; // the preintegrated position (global frame) at the Lidar origin 34 | double rot[9]; // the preintegrated rotation (global frame) at the Lidar origin 35 | }; 36 | 37 | #endif -------------------------------------------------------------------------------- /launch/mapping_avia.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | -------------------------------------------------------------------------------- /launch/mapping_nclt.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | -------------------------------------------------------------------------------- /launch/mapping_ntu_viral.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | -------------------------------------------------------------------------------- /launch/mapping_pointcloud.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | -------------------------------------------------------------------------------- /launch/mapping_velody64.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | -------------------------------------------------------------------------------- /msg/Pose6D.msg: -------------------------------------------------------------------------------- 1 | # the preintegrated Lidar states at the time of IMU measurements in a frame 2 | float64 offset_time # the offset time of IMU measurement w.r.t the first lidar point 3 | float64[3] acc # the preintegrated total acceleration (global frame) at the Lidar origin 4 | float64[3] gyr # the unbiased angular velocity (body frame) at the Lidar origin 5 | float64[3] vel # the preintegrated velocity (global frame) at the Lidar origin 6 | float64[3] pos # the preintegrated position (global frame) at the Lidar origin 7 | float64[9] rot # the preintegrated rotation (global frame) at the Lidar origin -------------------------------------------------------------------------------- /msg/States.msg: -------------------------------------------------------------------------------- 1 | Header header # timestamp of the first lidar in a frame 2 | float64[] rot_end # the estimated attitude (rotation matrix) at the end lidar point 3 | float64[] pos_end # the estimated position at the end lidar point (world frame) 4 | float64[] vel_end # the estimated velocity at the end lidar point (world frame) 5 | float64[] bias_gyr # gyroscope bias 6 | float64[] bias_acc # accelerator bias 7 | float64[] gravity # the estimated gravity acceleration 8 | float64[] cov # states covariance 9 | # Pose6D[] IMUpose # 6D pose at each imu measurements -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | ImMesh 4 | 0.0.0 5 | 6 | 7 | ] ImMesh: An Immediate LiDAR Localization and Meshing Framework 8 | 9 | 10 | claydergc 11 | 12 | BSD 13 | 14 | Ji Zhang 15 | 16 | catkin 17 | geometry_msgs 18 | nav_msgs 19 | roscpp 20 | rospy 21 | std_msgs 22 | sensor_msgs 23 | tf 24 | pcl_ros 25 | livox_ros_driver 26 | message_generation 27 | cv_bridge 28 | 29 | geometry_msgs 30 | nav_msgs 31 | sensor_msgs 32 | roscpp 33 | rospy 34 | std_msgs 35 | tf 36 | pcl_ros 37 | livox_ros_driver 38 | message_runtime 39 | cv_bridge 40 | 41 | rostest 42 | rosbag 43 | 44 | 45 | 46 | 47 | -------------------------------------------------------------------------------- /paper/ImMesh_tro.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/hku-mars/ImMesh/3bbc89e4cd2acfa4797834e79b06e8e86b3fa891/paper/ImMesh_tro.pdf -------------------------------------------------------------------------------- /src/IMU_Processing.h: -------------------------------------------------------------------------------- 1 | /* 2 | This code is the implementation of our paper "ImMesh: An Immediate LiDAR Localization and Meshing Framework". 3 | 4 | The source code of this package is released under GPLv2 license. We only allow it free for personal and academic usage. 5 | 6 | If you use any code of this repo in your academic research, please cite at least one of our papers: 7 | [1] Lin, Jiarong, et al. "Immesh: An immediate lidar localization and meshing framework." IEEE Transactions on Robotics 8 | (T-RO 2023) 9 | [2] Yuan, Chongjian, et al. "Efficient and probabilistic adaptive voxel mapping for accurate online lidar odometry." 10 | IEEE Robotics and Automation Letters (RA-L 2022) 11 | [3] Lin, Jiarong, and Fu Zhang. "R3LIVE: A Robust, Real-time, RGB-colored, LiDAR-Inertial-Visual tightly-coupled 12 | state Estimation and mapping package." IEEE International Conference on Robotics and Automation (ICRA 2022) 13 | 14 | For commercial use, please contact me and Dr. Fu Zhang to negotiate a 15 | different license. 16 | 17 | Redistribution and use in source and binary forms, with or without 18 | modification, are permitted provided that the following conditions are met: 19 | 20 | 1. Redistributions of source code must retain the above copyright notice, 21 | this list of conditions and the following disclaimer. 22 | 2. Redistributions in binary form must reproduce the above copyright notice, 23 | this list of conditions and the following disclaimer in the documentation 24 | and/or other materials provided with the distribution. 25 | 3. Neither the name of the copyright holder nor the names of its 26 | contributors may be used to endorse or promote products derived from this 27 | software without specific prior written permission. 28 | 29 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 30 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 31 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 32 | ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 33 | LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 34 | CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 35 | SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 36 | INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 37 | CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 38 | ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 39 | POSSIBILITY OF SUCH DAMAGE. 40 | */ 41 | #ifndef IMU_PROCESSING_H 42 | #define IMU_PROCESSING_H 43 | #include 44 | #include 45 | #include 46 | #include 47 | #include 48 | #include 49 | #include 50 | #include 51 | #include 52 | #include 53 | #include 54 | #include 55 | #include 56 | #include 57 | #include 58 | #include 59 | #include 60 | #include 61 | #include 62 | #include 63 | #include 64 | #include 65 | #include 66 | // #include 67 | #include 68 | 69 | #ifdef USE_IKFOM 70 | #include "use-ikfom.hpp" 71 | #endif 72 | 73 | /// *************Preconfiguration 74 | 75 | // #define MAX_INI_COUNT (10) 76 | 77 | const bool time_list(PointType &x, PointType &y); //{return (x.curvature < y.curvature);}; 78 | 79 | /// *************IMU Process and undistortion 80 | class ImuProcess 81 | { 82 | public: 83 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 84 | 85 | ImuProcess(); 86 | ~ImuProcess(); 87 | 88 | void Reset(); 89 | void Reset(double start_timestamp, const sensor_msgs::ImuConstPtr &lastimu); 90 | void push_update_state(double offs_t, StatesGroup state); 91 | void set_extrinsic(const V3D &transl, const M3D &rot); 92 | void set_extrinsic(const V3D &transl); 93 | void set_extrinsic(const MD(4,4) &T); 94 | void set_gyr_cov_scale(const V3D &scaler); 95 | void set_acc_cov_scale(const V3D &scaler); 96 | void set_gyr_bias_cov(const V3D &b_g); 97 | void set_acc_bias_cov(const V3D &b_a); 98 | void set_imu_init_frame_num(const int &num); 99 | void disable_imu(); 100 | #ifdef USE_IKFOM 101 | Eigen::Matrix Q; 102 | void Process(const MeasureGroup &meas, esekfom::esekf &kf_state, PointCloudXYZI::Ptr pcl_un_); 103 | #else 104 | void Process(LidarMeasureGroup &lidar_meas, StatesGroup &stat, PointCloudXYZI::Ptr cur_pcl_un_); 105 | void Process2(LidarMeasureGroup &lidar_meas, StatesGroup &stat, PointCloudXYZI::Ptr cur_pcl_un_); 106 | void UndistortPcl(LidarMeasureGroup &lidar_meas, StatesGroup &state_inout, PointCloudXYZI &pcl_out); 107 | #endif 108 | 109 | ros::NodeHandle nh; 110 | ofstream fout_imu; 111 | V3D cov_acc; 112 | V3D cov_gyr; 113 | V3D cov_acc_scale; 114 | V3D cov_gyr_scale; 115 | V3D cov_bias_gyr; 116 | V3D cov_bias_acc; 117 | double first_lidar_time; 118 | 119 | private: 120 | #ifdef USE_IKFOM 121 | void IMU_init(const MeasureGroup &meas, esekfom::esekf &kf_state, int &N); 122 | void UndistortPcl(const MeasureGroup &meas, esekfom::esekf &kf_state, PointCloudXYZI &pcl_in_out); 123 | #else 124 | void IMU_init(const MeasureGroup &meas, StatesGroup &state, int &N); 125 | // void UndistortPcl(const MeasureGroup &meas, StatesGroup &state_inout, PointCloudXYZI &pcl_in_out); 126 | void Forward(const MeasureGroup &meas, StatesGroup &state_inout, double pcl_beg_time, double end_time); 127 | void Backward(const LidarMeasureGroup &lidar_meas, StatesGroup &state_inout, PointCloudXYZI &pcl_out); 128 | void Forward_without_imu(LidarMeasureGroup &meas, StatesGroup &state_inout, PointCloudXYZI &pcl_out); 129 | #endif 130 | 131 | PointCloudXYZI::Ptr cur_pcl_un_; 132 | sensor_msgs::ImuConstPtr last_imu_; 133 | // StatesGroup last_state; 134 | deque v_imu_; 135 | vector IMUpose; 136 | vector v_rot_pcl_; 137 | M3D Lid_rot_to_IMU; 138 | V3D Lid_offset_to_IMU; 139 | V3D mean_acc; 140 | V3D mean_gyr; 141 | V3D angvel_last; 142 | V3D acc_s_last; 143 | V3D last_acc; 144 | V3D last_ang; 145 | double start_timestamp_; 146 | double last_lidar_end_time_; 147 | double time_last_scan; 148 | int init_iter_num = 1, MAX_INI_COUNT = 3; 149 | bool b_first_frame_ = true; 150 | bool imu_need_init_ = true; 151 | bool imu_en = true; 152 | }; 153 | #endif -------------------------------------------------------------------------------- /src/meshing/delaunay/openCV_subdiv2d_index.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include 3 | 4 | class Subdiv2DIndex : public cv::Subdiv2D 5 | { 6 | public : 7 | Subdiv2DIndex(cv::Rect rectangle); 8 | 9 | //Source code of Subdiv2D: https://github.com/opencv/opencv/blob/master/modules/imgproc/src/subdivision2d.cpp#L762 10 | //The implementation tweaks getTrianglesList() so that only the indice of the triangle inside the image are returned 11 | void getTrianglesIndices(std::vector &ind) const; 12 | }; 13 | 14 | Subdiv2DIndex::Subdiv2DIndex(cv::Rect rectangle) : cv::Subdiv2D{rectangle} 15 | { 16 | } 17 | 18 | void Subdiv2DIndex::getTrianglesIndices(std::vector &triangleList) const 19 | { 20 | triangleList.clear(); 21 | int i, total = (int)(qedges.size() * 4); 22 | std::vector edgemask(total, false); 23 | const bool filterPoints = true; 24 | cv::Rect2f rect(topLeft.x, topLeft.y, bottomRight.x - topLeft.x, bottomRight.y - topLeft.y); 25 | 26 | for (i = 4; i < total; i += 2) 27 | { 28 | if (edgemask[i]) 29 | continue; 30 | cv::Point2f a, b, c; 31 | int edge_a = i; 32 | int indexA = edgeOrg(edge_a, &a) -4; 33 | if (filterPoints && !rect.contains(a)) 34 | continue; 35 | int edge_b = getEdge(edge_a, NEXT_AROUND_LEFT); 36 | int indexB = edgeOrg(edge_b, &b) - 4; 37 | if (filterPoints && !rect.contains(b)) 38 | continue; 39 | int edge_c = getEdge(edge_b, NEXT_AROUND_LEFT); 40 | int indexC = edgeOrg(edge_c, &c) - 4; 41 | if (filterPoints && !rect.contains(c)) 42 | continue; 43 | edgemask[edge_a] = true; 44 | edgemask[edge_b] = true; 45 | edgemask[edge_c] = true; 46 | 47 | triangleList.push_back(indexA); 48 | triangleList.push_back(indexB); 49 | triangleList.push_back(indexC); 50 | } 51 | } 52 | -------------------------------------------------------------------------------- /src/meshing/imgui.ini: -------------------------------------------------------------------------------- 1 | [Window][Debug##Default] 2 | Pos=60,60 3 | Size=400,400 4 | Collapsed=0 5 | 6 | [Window][My window] 7 | Pos=60,60 8 | Size=757,945 9 | Collapsed=0 10 | 11 | [Window][Camera Pose] 12 | Pos=872,115 13 | Size=442,714 14 | Collapsed=0 15 | 16 | -------------------------------------------------------------------------------- /src/meshing/mesh_rec_display.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include "tools/openGL_libs/glad.h" 3 | #include "tools/openGL_libs/openGL_camera.hpp" 4 | #include "shader/tools_my_texture_triangle_shader.h" 5 | #include "shader/tools_my_camera_pose_shader.h" 6 | #include "shader/tools_my_dbg_utility.h" 7 | #include "shader/tools_my_point_shader.h" 8 | #include "mesh_rec_geometry.hpp" 9 | 10 | void init_openGL_shader(); 11 | void draw_triangle( const Cam_view & gl_cam); 12 | void draw_camera_trajectory(int current_frame_idx, float pt_disp_size = 5); 13 | void draw_camera_pose( int current_frame_idx, float pt_disp_size = 5, float display_cam_size = 1.0); 14 | void display_current_LiDAR_pts( int current_frame_idx, double pts_size, vec_4f color = vec_4f( 1.0, 1.0, 1.0, 0.5 ) ); 15 | void display_reinforced_LiDAR_pts( std::vector< vec_3f > & pt_vec, double pts_size, vec_3f color = vec_3f (1.0, 0.0, 1.0) ); 16 | void service_refresh_and_synchronize_triangle(double sleep_time = 1); -------------------------------------------------------------------------------- /src/meshing/mesh_rec_geometry.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | 8 | #include "opencv2/opencv.hpp" 9 | 10 | #include "pointcloud_rgbd.hpp" 11 | #include "triangle.hpp" 12 | #include 13 | 14 | #include 15 | 16 | #include "tools/tools_color_printf.hpp" 17 | #include "tools/tools_data_io.hpp" 18 | #include "tools/tools_logger.hpp" 19 | #include "tools/tools_color_printf.hpp" 20 | #include "tools/tools_eigen.hpp" 21 | #include "tools/tools_random.hpp" 22 | #include "tools/tools_serialization.hpp" 23 | #include "tools/tools_graphics.hpp" 24 | 25 | #include "ikd_Tree.h" 26 | 27 | 28 | #include 29 | #include 30 | #include 31 | 32 | #define NUMBER_OF_POSE_SIZE -1 33 | typedef std::vector< std::pair< std::vector< vec_4 >, Eigen::Matrix< double, NUMBER_OF_POSE_SIZE, 1 > > > LiDAR_frame_pts_and_pose_vec; 34 | 35 | typedef CGAL::Exact_predicates_inexact_constructions_kernel K; 36 | 37 | typedef pcl::PointXYZINormal PointType ; 38 | using PointVector = std::vector< PointType, Eigen::aligned_allocator< PointType > >; 39 | 40 | void save_to_ply_file(std::string ply_file = std::string("/home/ziv/temp/ply/rec_mesh_smoothed.ply"), double smooth_factor = 0.1, double knn = 20); 41 | void smooth_all_pts(double smooth_factor = 0.1, double knn = 20); 42 | void triangle_compare( const Triangle_set & remove_triangles, const std::vector< long > & add_triangles, 43 | Triangle_set & res_remove_triangles, Triangle_set & res_add_triangles, Triangle_set * exist_triangles = nullptr ); 44 | 45 | void correct_triangle_index(Triangle_ptr & ptr , const vec_3 & camera_center, const vec_3 & _short_axis); 46 | std::vector remove_outlier_pts(const std::vector & rgb_pts_vec, const RGB_voxel_ptr & voxel_ptr ); 47 | std::vector retrieve_neighbor_pts_kdtree(const std::vector & rgb_pts_vec ); 48 | 49 | // Compute angle of vector(pb-pa) and (pc-pa) 50 | double compute_angle( vec_2 &pa, vec_2 &pb, vec_2 &pc ); 51 | bool is_face_is_ok( Common_tools::Delaunay2::Face &face, double maximum_angle = 180 ); 52 | std::vector< long > delaunay_triangulation( std::vector< RGB_pt_ptr > &rgb_pt_vec, vec_3 &long_axis, vec_3 &mid_axis, vec_3 &short_axis, 53 | std::set< long > &convex_hull_index, std::set< long > & inner_hull_index ); 54 | 55 | std::vector< vec_3f > init_camera_frustum_vertex(Eigen::Matrix3f cam_K, int w, int h, float scale ); 56 | void draw_all_camera_pose( float draw_camera_size = 1.0 ); 57 | -------------------------------------------------------------------------------- /src/meshing/optical_flow/lkpyramid.hpp: -------------------------------------------------------------------------------- 1 | // This file is modified from lkpyramid.hpp of openCV 2 | #pragma once 3 | #include "opencv2/core.hpp" 4 | #include "opencv2/highgui.hpp" 5 | #include "opencv2/video/tracking.hpp" 6 | #include "opencv2/imgproc.hpp" 7 | #define CV_CPU_HAS_SUPPORT_SSE2 1 8 | #define USING_OPENCV_TBB 1 9 | #include "opencv2/core/hal/intrin.hpp" 10 | #include "tools_logger.hpp" 11 | #include "tools_timer.hpp" 12 | #include 13 | #include 14 | #include "tools_thread_pool.hpp" 15 | 16 | enum 17 | { 18 | cv_OPTFLOW_USE_INITIAL_FLOW = 4, 19 | cv_OPTFLOW_LK_GET_MIN_EIGENVALS = 8, 20 | cv_OPTFLOW_FARNEBACK_GAUSSIAN = 256 21 | }; 22 | 23 | typedef short deriv_type; 24 | 25 | inline int opencv_buildOpticalFlowPyramid(cv::InputArray img, cv::OutputArrayOfArrays pyramid, 26 | cv::Size winSize, int maxLevel, bool withDerivatives = true, 27 | int pyrBorder = cv::BORDER_REFLECT_101, 28 | int derivBorder = cv::BORDER_CONSTANT, 29 | bool tryReuseInputImage = true); 30 | 31 | inline void calc_sharr_deriv(const cv::Mat &src, cv::Mat &dst); 32 | 33 | void calculate_optical_flow(cv::InputArray prevImg, cv::InputArray nextImg, 34 | cv::InputArray prevPts, cv::InputOutputArray nextPts, 35 | cv::OutputArray status, cv::OutputArray err, 36 | cv::Size winSize = cv::Size(21, 21), int maxLevel = 3, 37 | cv::TermCriteria criteria = cv::TermCriteria(cv::TermCriteria::COUNT + cv::TermCriteria::EPS, 30, 0.01), 38 | int flags = 0, double minEigThreshold = 1e-4); 39 | 40 | inline int calculate_LK_optical_flow(const cv::Range &range, const cv::Mat &_prevImg, const cv::Mat &_prevDeriv, const cv::Mat &_nextImg, 41 | const cv::Point2f *_prevPts, cv::Point2f *_nextPts, 42 | uchar *_status, float *_err, 43 | cv::Size _winSize, cv::TermCriteria _criteria, 44 | int _level, int _maxLevel, int _flags, float _minEigThreshold); 45 | 46 | struct opencv_LKTrackerInvoker : cv::ParallelLoopBody 47 | { 48 | opencv_LKTrackerInvoker(const cv::Mat *_prevImg, const cv::Mat *_prevDeriv, const cv::Mat *_nextImg, 49 | const cv::Point2f *_prevPts, cv::Point2f *_nextPts, 50 | uchar *_status, float *_err, 51 | cv::Size _winSize, cv::TermCriteria _criteria, 52 | int _level, int _maxLevel, int _flags, float _minEigThreshold); 53 | void operator()(const cv::Range &range) const; 54 | bool calculate( cv::Range range) const; 55 | const cv::Mat *prevImg; 56 | const cv::Mat *nextImg; 57 | const cv::Mat *prevDeriv; 58 | const cv::Point2f *prevPts; 59 | cv::Point2f *nextPts; 60 | uchar *status; 61 | float *err; 62 | cv::Size winSize; 63 | cv::TermCriteria criteria; 64 | int level; 65 | int maxLevel; 66 | int flags; 67 | float minEigThreshold; 68 | }; 69 | 70 | class LK_optical_flow_kernel 71 | { 72 | public: 73 | cv::Size get_win_size() const { return m_lk_win_size; } 74 | void set_win_size(cv::Size winSize_) { m_lk_win_size = winSize_; } 75 | 76 | int get_max_level() const { return m_maxLevel; } 77 | void set_max_level(int maxLevel_) { m_maxLevel = maxLevel_; } 78 | 79 | cv::TermCriteria get_term_criteria() const { return m_terminate_criteria; } 80 | void set_term_criteria(cv::TermCriteria &crit_) { m_terminate_criteria = crit_; } 81 | 82 | int get_flags() const { return flags; } 83 | void set_flags(int flags_) { flags = flags_; } 84 | 85 | double get_min_eig_threshold() const { return minEigThreshold; } 86 | void set_min_eig_threshold(double minEigThreshold_) { minEigThreshold = minEigThreshold_; } 87 | 88 | LK_optical_flow_kernel(cv::Size winSize_ = cv::Size(21, 21), 89 | int maxLevel_ = 3, 90 | cv::TermCriteria criteria_ = cv::TermCriteria(cv::TermCriteria::COUNT + cv::TermCriteria::EPS, 30, 0.01), 91 | int flags_ = 0, 92 | double minEigThreshold_ = 1e-4 93 | ) : m_lk_win_size(winSize_), m_maxLevel(maxLevel_), m_terminate_criteria(criteria_), flags(flags_), minEigThreshold(minEigThreshold_) 94 | { 95 | set_termination_criteria(m_terminate_criteria); 96 | } 97 | 98 | /** 99 | @brief Calculates a sparse optical flow. 100 | @param prevImg First input image. 101 | @param nextImg Second input image of the same cv::Size and the same type as prevImg. 102 | @param prevPts Vector of 2D points for which the flow needs to be found. 103 | @param nextPts Output vector of 2D points containing the calculated new positions of input features in the second image. 104 | @param status Output status vector. Each element of the vector is set to 1 if the 105 | flow for the corresponding features has been found. Otherwise, it is set to 0. 106 | @param err Optional output vector that contains error response for each point (inverse confidence). 107 | **/ 108 | void calc(cv::InputArray prevImg, cv::InputArray nextImg, 109 | cv::InputArray prevPts, cv::InputOutputArray nextPts, 110 | cv::OutputArray status, 111 | cv::OutputArray err = cv::noArray()); 112 | 113 | void allocate_img_deriv_memory( std::vector &img_pyr, 114 | std::vector &img_pyr_deriv_I, 115 | std::vector &img_pyr_deriv_I_buff); 116 | void calc_image_deriv_Sharr(std::vector &img_pyr, 117 | std::vector &img_pyr_deriv_I, 118 | std::vector &img_pyr_deriv_I_buff); 119 | void set_termination_criteria(cv::TermCriteria &crit); 120 | 121 | public: 122 | std::vector m_prev_img_pyr, m_curr_img_pyr; 123 | std::vector m_prev_img_deriv_I, m_prev_img_deriv_I_buff; 124 | std::vector m_curr_img_deriv_I, m_curr_img_deriv_I_buff; 125 | cv::Size m_lk_win_size; 126 | int m_maxLevel; 127 | cv::TermCriteria m_terminate_criteria; 128 | int flags; 129 | double minEigThreshold; 130 | 131 | void swap_image_buffer(); 132 | 133 | int track_image(const cv::Mat & curr_img, const std::vector & last_tracked_pts, std::vector & curr_tracked_pts, 134 | std::vector & status, int opm_method = 3 ); // opm_method: [0] openCV parallel_body [1] openCV parallel for [2] Thread pool 135 | }; -------------------------------------------------------------------------------- /src/meshing/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | r4live_dev 4 | 1.0.0 5 | R3LIVE: A Robust, Real-time, RGB-colored, LiDAR-Inertial-Visual tightly-coupled state Estimation and mapping package 6 | Jiarong.Lin 7 | GPLv2 8 | 9 | catkin 10 | roscpp 11 | livox_ros_driver 12 | 13 | catkin 14 | roscpp 15 | livox_ros_driver 16 | 17 | 18 | -------------------------------------------------------------------------------- /src/meshing/r3live/triangle.cpp: -------------------------------------------------------------------------------- 1 | #include "triangle.hpp" 2 | 3 | vec_3 Triangle_manager::get_triangle_center(const Triangle_ptr& tri_ptr) 4 | { 5 | vec_3 triangle_pos = ( m_pointcloud_map->m_rgb_pts_vec[ tri_ptr->m_tri_pts_id[ 0 ] ]->get_pos() + 6 | m_pointcloud_map->m_rgb_pts_vec[ tri_ptr->m_tri_pts_id[ 1 ] ]->get_pos() + 7 | m_pointcloud_map->m_rgb_pts_vec[ tri_ptr->m_tri_pts_id[ 2 ] ]->get_pos() ); 8 | triangle_pos = triangle_pos / 3.0; 9 | return triangle_pos; 10 | } 11 | 12 | int Triangle_manager::get_all_triangle_list(std::vector< Triangle_set > & triangle_list, std::mutex * mutex, int sleep_us_each_query) 13 | { 14 | int all_triangle_num = 0; 15 | triangle_list.clear(); 16 | for ( auto &sync_triangle_set : m_triangle_set_in_region.m_map_3d_hash_map ) 17 | { 18 | if ( mutex != nullptr ) 19 | mutex->lock(); 20 | Triangle_set tri_set; 21 | sync_triangle_set.second.get_triangle_set(tri_set) ; 22 | triangle_list.push_back( tri_set ); 23 | all_triangle_num += triangle_list.back().size(); 24 | if ( mutex != nullptr ) 25 | mutex->unlock(); 26 | if ( sleep_us_each_query != 0 ) 27 | { 28 | // std::this_thread::yield(); 29 | std::this_thread::sleep_for( std::chrono::microseconds( sleep_us_each_query ) ); 30 | } 31 | } 32 | return all_triangle_num; 33 | } 34 | 35 | void Triangle_manager::insert_triangle_to_list( const Triangle_ptr& tri_ptr , const int& frame_idx) 36 | { 37 | vec_3 triangle_pos = get_triangle_center( tri_ptr ); 38 | int hash_3d_x = std::round( triangle_pos( 0 ) / m_region_size ); 39 | int hash_3d_y = std::round( triangle_pos( 1 ) / m_region_size ); 40 | int hash_3d_z = std::round( triangle_pos( 2 ) / m_region_size ); 41 | Sync_triangle_set* sync_triangle_set_ptr = m_triangle_set_in_region.get_data( hash_3d_x, hash_3d_y, hash_3d_z ); 42 | if ( sync_triangle_set_ptr == nullptr ) 43 | { 44 | sync_triangle_set_ptr = new Sync_triangle_set(); 45 | sync_triangle_set_ptr->insert( tri_ptr ); 46 | m_triangle_set_in_region.insert( hash_3d_x, hash_3d_y, hash_3d_z, *sync_triangle_set_ptr ); 47 | m_triangle_set_vector.push_back( m_triangle_set_in_region.get_data( hash_3d_x, hash_3d_y, hash_3d_z ) ); 48 | } 49 | else 50 | { 51 | sync_triangle_set_ptr->insert( tri_ptr ); 52 | } 53 | } 54 | 55 | void Triangle_manager::erase_triangle_from_list( const Triangle_ptr& tri_ptr , const int & frame_idx) 56 | { 57 | vec_3 triangle_pos = get_triangle_center( tri_ptr ); 58 | int hash_3d_x = std::round( triangle_pos( 0 ) / m_region_size ); 59 | int hash_3d_y = std::round( triangle_pos( 1 ) / m_region_size ); 60 | int hash_3d_z = std::round( triangle_pos( 2 ) / m_region_size ); 61 | Sync_triangle_set* triangle_set_ptr = m_triangle_set_in_region.get_data( hash_3d_x, hash_3d_y, hash_3d_z ); 62 | if ( triangle_set_ptr == nullptr ) 63 | { 64 | return; 65 | } 66 | else 67 | { 68 | triangle_set_ptr->erase( tri_ptr ); 69 | } 70 | } 71 | 72 | int Triangle_manager::get_triangle_list_size() 73 | { 74 | int tri_list_size = 0; 75 | for ( auto& triangle_set : m_triangle_set_in_region.m_map_3d_hash_map ) 76 | { 77 | tri_list_size += triangle_set.second.get_triangle_set_size(); 78 | } 79 | return tri_list_size; 80 | } -------------------------------------------------------------------------------- /src/shader/image_shader.fs: -------------------------------------------------------------------------------- 1 | /* 2 | This code is the implementation of our paper "ImMesh: An Immediate LiDAR Localization and Meshing Framework". 3 | 4 | The source code of this package is released under GPLv2 license. We only allow it free for personal and academic usage. 5 | 6 | If you use any code of this repo in your academic research, please cite at least one of our papers: 7 | [1] Lin, Jiarong, et al. "Immesh: An immediate lidar localization and meshing framework." IEEE Transactions on Robotics 8 | (T-RO 2023) 9 | [2] Yuan, Chongjian, et al. "Efficient and probabilistic adaptive voxel mapping for accurate online lidar odometry." 10 | IEEE Robotics and Automation Letters (RA-L 2022) 11 | [3] Lin, Jiarong, and Fu Zhang. "R3LIVE: A Robust, Real-time, RGB-colored, LiDAR-Inertial-Visual tightly-coupled 12 | state Estimation and mapping package." IEEE International Conference on Robotics and Automation (ICRA 2022) 13 | 14 | For commercial use, please contact me and Dr. Fu Zhang to negotiate a 15 | different license. 16 | 17 | Redistribution and use in source and binary forms, with or without 18 | modification, are permitted provided that the following conditions are met: 19 | 20 | 1. Redistributions of source code must retain the above copyright notice, 21 | this list of conditions and the following disclaimer. 22 | 2. Redistributions in binary form must reproduce the above copyright notice, 23 | this list of conditions and the following disclaimer in the documentation 24 | and/or other materials provided with the distribution. 25 | 3. Neither the name of the copyright holder nor the names of its 26 | contributors may be used to endorse or promote products derived from this 27 | software without specific prior written permission. 28 | 29 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 30 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 31 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 32 | ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 33 | LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 34 | CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 35 | SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 36 | INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 37 | CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 38 | ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 39 | POSSIBILITY OF SUCH DAMAGE. 40 | */ 41 | #version 330 core 42 | out vec4 FragColor; 43 | 44 | in vec2 TexCoords; 45 | 46 | uniform sampler2D TexID; 47 | 48 | void main() 49 | { 50 | FragColor = texture(TexID, TexCoords); 51 | // FragColor = vec4( 1, 1, 1, 1 ); 52 | // FragColor = texture(texture_1, TexCoords); 53 | } -------------------------------------------------------------------------------- /src/shader/image_shader.vs: -------------------------------------------------------------------------------- 1 | /* 2 | This code is the implementation of our paper "ImMesh: An Immediate LiDAR Localization and Meshing Framework". 3 | 4 | The source code of this package is released under GPLv2 license. We only allow it free for personal and academic usage. 5 | 6 | If you use any code of this repo in your academic research, please cite at least one of our papers: 7 | [1] Lin, Jiarong, et al. "Immesh: An immediate lidar localization and meshing framework." IEEE Transactions on Robotics 8 | (T-RO 2023) 9 | [2] Yuan, Chongjian, et al. "Efficient and probabilistic adaptive voxel mapping for accurate online lidar odometry." 10 | IEEE Robotics and Automation Letters (RA-L 2022) 11 | [3] Lin, Jiarong, and Fu Zhang. "R3LIVE: A Robust, Real-time, RGB-colored, LiDAR-Inertial-Visual tightly-coupled 12 | state Estimation and mapping package." IEEE International Conference on Robotics and Automation (ICRA 2022) 13 | 14 | For commercial use, please contact me and Dr. Fu Zhang to negotiate a 15 | different license. 16 | 17 | Redistribution and use in source and binary forms, with or without 18 | modification, are permitted provided that the following conditions are met: 19 | 20 | 1. Redistributions of source code must retain the above copyright notice, 21 | this list of conditions and the following disclaimer. 22 | 2. Redistributions in binary form must reproduce the above copyright notice, 23 | this list of conditions and the following disclaimer in the documentation 24 | and/or other materials provided with the distribution. 25 | 3. Neither the name of the copyright holder nor the names of its 26 | contributors may be used to endorse or promote products derived from this 27 | software without specific prior written permission. 28 | 29 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 30 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 31 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 32 | ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 33 | LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 34 | CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 35 | SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 36 | INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 37 | CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 38 | ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 39 | POSSIBILITY OF SUCH DAMAGE. 40 | */ 41 | #version 330 core 42 | layout (location = 0) in vec3 aPos; 43 | layout (location = 1) in vec2 aCoor; 44 | 45 | out vec2 TexCoords; 46 | 47 | uniform mat4 projection_mul_view; 48 | uniform float pointSize; 49 | 50 | void main() 51 | { 52 | gl_Position = projection_mul_view * vec4(aPos, 1.0); 53 | gl_PointSize = pointSize; 54 | TexCoords = aCoor; 55 | } -------------------------------------------------------------------------------- /src/shader/learnopengl/animation.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | 12 | struct AssimpNodeData 13 | { 14 | glm::mat4 transformation; 15 | std::string name; 16 | int childrenCount; 17 | std::vector children; 18 | }; 19 | 20 | class Animation 21 | { 22 | public: 23 | Animation() = default; 24 | 25 | Animation(const std::string& animationPath, Model* model) 26 | { 27 | Assimp::Importer importer; 28 | const aiScene* scene = importer.ReadFile(animationPath, aiProcess_Triangulate); 29 | assert(scene && scene->mRootNode); 30 | auto animation = scene->mAnimations[0]; 31 | m_Duration = animation->mDuration; 32 | m_TicksPerSecond = animation->mTicksPerSecond; 33 | aiMatrix4x4 globalTransformation = scene->mRootNode->mTransformation; 34 | globalTransformation = globalTransformation.Inverse(); 35 | ReadHierarchyData(m_RootNode, scene->mRootNode); 36 | ReadMissingBones(animation, *model); 37 | } 38 | 39 | ~Animation() 40 | { 41 | } 42 | 43 | Bone* FindBone(const std::string& name) 44 | { 45 | auto iter = std::find_if(m_Bones.begin(), m_Bones.end(), 46 | [&](const Bone& Bone) 47 | { 48 | return Bone.GetBoneName() == name; 49 | } 50 | ); 51 | if (iter == m_Bones.end()) return nullptr; 52 | else return &(*iter); 53 | } 54 | 55 | 56 | inline float GetTicksPerSecond() { return m_TicksPerSecond; } 57 | inline float GetDuration() { return m_Duration;} 58 | inline const AssimpNodeData& GetRootNode() { return m_RootNode; } 59 | inline const std::map& GetBoneIDMap() 60 | { 61 | return m_BoneInfoMap; 62 | } 63 | 64 | private: 65 | void ReadMissingBones(const aiAnimation* animation, Model& model) 66 | { 67 | int size = animation->mNumChannels; 68 | 69 | auto& boneInfoMap = model.GetBoneInfoMap();//getting m_BoneInfoMap from Model class 70 | int& boneCount = model.GetBoneCount(); //getting the m_BoneCounter from Model class 71 | 72 | //reading channels(bones engaged in an animation and their keyframes) 73 | for (int i = 0; i < size; i++) 74 | { 75 | auto channel = animation->mChannels[i]; 76 | std::string boneName = channel->mNodeName.data; 77 | 78 | if (boneInfoMap.find(boneName) == boneInfoMap.end()) 79 | { 80 | boneInfoMap[boneName].id = boneCount; 81 | boneCount++; 82 | } 83 | m_Bones.push_back(Bone(channel->mNodeName.data, 84 | boneInfoMap[channel->mNodeName.data].id, channel)); 85 | } 86 | 87 | m_BoneInfoMap = boneInfoMap; 88 | } 89 | 90 | void ReadHierarchyData(AssimpNodeData& dest, const aiNode* src) 91 | { 92 | assert(src); 93 | 94 | dest.name = src->mName.data; 95 | dest.transformation = AssimpGLMHelpers::ConvertMatrixToGLMFormat(src->mTransformation); 96 | dest.childrenCount = src->mNumChildren; 97 | 98 | for (int i = 0; i < src->mNumChildren; i++) 99 | { 100 | AssimpNodeData newData; 101 | ReadHierarchyData(newData, src->mChildren[i]); 102 | dest.children.push_back(newData); 103 | } 104 | } 105 | float m_Duration; 106 | int m_TicksPerSecond; 107 | std::vector m_Bones; 108 | AssimpNodeData m_RootNode; 109 | std::map m_BoneInfoMap; 110 | }; 111 | 112 | -------------------------------------------------------------------------------- /src/shader/learnopengl/animator.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | 11 | class Animator 12 | { 13 | public: 14 | Animator(Animation* animation) 15 | { 16 | m_CurrentTime = 0.0; 17 | m_CurrentAnimation = animation; 18 | 19 | m_FinalBoneMatrices.reserve(100); 20 | 21 | for (int i = 0; i < 100; i++) 22 | m_FinalBoneMatrices.push_back(glm::mat4(1.0f)); 23 | } 24 | 25 | void UpdateAnimation(float dt) 26 | { 27 | m_DeltaTime = dt; 28 | if (m_CurrentAnimation) 29 | { 30 | m_CurrentTime += m_CurrentAnimation->GetTicksPerSecond() * dt; 31 | m_CurrentTime = fmod(m_CurrentTime, m_CurrentAnimation->GetDuration()); 32 | CalculateBoneTransform(&m_CurrentAnimation->GetRootNode(), glm::mat4(1.0f)); 33 | } 34 | } 35 | 36 | void PlayAnimation(Animation* pAnimation) 37 | { 38 | m_CurrentAnimation = pAnimation; 39 | m_CurrentTime = 0.0f; 40 | } 41 | 42 | void CalculateBoneTransform(const AssimpNodeData* node, glm::mat4 parentTransform) 43 | { 44 | std::string nodeName = node->name; 45 | glm::mat4 nodeTransform = node->transformation; 46 | 47 | Bone* Bone = m_CurrentAnimation->FindBone(nodeName); 48 | 49 | if (Bone) 50 | { 51 | Bone->Update(m_CurrentTime); 52 | nodeTransform = Bone->GetLocalTransform(); 53 | } 54 | 55 | glm::mat4 globalTransformation = parentTransform * nodeTransform; 56 | 57 | auto boneInfoMap = m_CurrentAnimation->GetBoneIDMap(); 58 | if (boneInfoMap.find(nodeName) != boneInfoMap.end()) 59 | { 60 | int index = boneInfoMap[nodeName].id; 61 | glm::mat4 offset = boneInfoMap[nodeName].offset; 62 | m_FinalBoneMatrices[index] = globalTransformation * offset; 63 | } 64 | 65 | for (int i = 0; i < node->childrenCount; i++) 66 | CalculateBoneTransform(&node->children[i], globalTransformation); 67 | } 68 | 69 | std::vector GetFinalBoneMatrices() 70 | { 71 | return m_FinalBoneMatrices; 72 | } 73 | 74 | private: 75 | std::vector m_FinalBoneMatrices; 76 | Animation* m_CurrentAnimation; 77 | float m_CurrentTime; 78 | float m_DeltaTime; 79 | 80 | }; 81 | -------------------------------------------------------------------------------- /src/shader/learnopengl/animdata.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | struct BoneInfo 6 | { 7 | /*id is index in finalBoneMatrices*/ 8 | int id; 9 | 10 | /*offset matrix transforms vertex from model space to bone space*/ 11 | glm::mat4 offset; 12 | 13 | }; 14 | #pragma once 15 | -------------------------------------------------------------------------------- /src/shader/learnopengl/assimp_glm_helpers.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | 10 | class AssimpGLMHelpers 11 | { 12 | public: 13 | 14 | static inline glm::mat4 ConvertMatrixToGLMFormat(const aiMatrix4x4& from) 15 | { 16 | glm::mat4 to; 17 | //the a,b,c,d in assimp is the row ; the 1,2,3,4 is the column 18 | to[0][0] = from.a1; to[1][0] = from.a2; to[2][0] = from.a3; to[3][0] = from.a4; 19 | to[0][1] = from.b1; to[1][1] = from.b2; to[2][1] = from.b3; to[3][1] = from.b4; 20 | to[0][2] = from.c1; to[1][2] = from.c2; to[2][2] = from.c3; to[3][2] = from.c4; 21 | to[0][3] = from.d1; to[1][3] = from.d2; to[2][3] = from.d3; to[3][3] = from.d4; 22 | return to; 23 | } 24 | 25 | static inline glm::vec3 GetGLMVec(const aiVector3D& vec) 26 | { 27 | return glm::vec3(vec.x, vec.y, vec.z); 28 | } 29 | 30 | static inline glm::quat GetGLMQuat(const aiQuaternion& pOrientation) 31 | { 32 | return glm::quat(pOrientation.w, pOrientation.x, pOrientation.y, pOrientation.z); 33 | } 34 | }; -------------------------------------------------------------------------------- /src/shader/learnopengl/bone.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | /* Container for bone data */ 4 | 5 | #include 6 | #include 7 | #include 8 | #include 9 | #define GLM_ENABLE_EXPERIMENTAL 10 | #include 11 | #include 12 | 13 | struct KeyPosition 14 | { 15 | glm::vec3 position; 16 | float timeStamp; 17 | }; 18 | 19 | struct KeyRotation 20 | { 21 | glm::quat orientation; 22 | float timeStamp; 23 | }; 24 | 25 | struct KeyScale 26 | { 27 | glm::vec3 scale; 28 | float timeStamp; 29 | }; 30 | 31 | class Bone 32 | { 33 | public: 34 | Bone(const std::string& name, int ID, const aiNodeAnim* channel) 35 | : 36 | m_Name(name), 37 | m_ID(ID), 38 | m_LocalTransform(1.0f) 39 | { 40 | m_NumPositions = channel->mNumPositionKeys; 41 | 42 | for (int positionIndex = 0; positionIndex < m_NumPositions; ++positionIndex) 43 | { 44 | aiVector3D aiPosition = channel->mPositionKeys[positionIndex].mValue; 45 | float timeStamp = channel->mPositionKeys[positionIndex].mTime; 46 | KeyPosition data; 47 | data.position = AssimpGLMHelpers::GetGLMVec(aiPosition); 48 | data.timeStamp = timeStamp; 49 | m_Positions.push_back(data); 50 | } 51 | 52 | m_NumRotations = channel->mNumRotationKeys; 53 | for (int rotationIndex = 0; rotationIndex < m_NumRotations; ++rotationIndex) 54 | { 55 | aiQuaternion aiOrientation = channel->mRotationKeys[rotationIndex].mValue; 56 | float timeStamp = channel->mRotationKeys[rotationIndex].mTime; 57 | KeyRotation data; 58 | data.orientation = AssimpGLMHelpers::GetGLMQuat(aiOrientation); 59 | data.timeStamp = timeStamp; 60 | m_Rotations.push_back(data); 61 | } 62 | 63 | m_NumScalings = channel->mNumScalingKeys; 64 | for (int keyIndex = 0; keyIndex < m_NumScalings; ++keyIndex) 65 | { 66 | aiVector3D scale = channel->mScalingKeys[keyIndex].mValue; 67 | float timeStamp = channel->mScalingKeys[keyIndex].mTime; 68 | KeyScale data; 69 | data.scale = AssimpGLMHelpers::GetGLMVec(scale); 70 | data.timeStamp = timeStamp; 71 | m_Scales.push_back(data); 72 | } 73 | } 74 | 75 | void Update(float animationTime) 76 | { 77 | glm::mat4 translation = InterpolatePosition(animationTime); 78 | glm::mat4 rotation = InterpolateRotation(animationTime); 79 | glm::mat4 scale = InterpolateScaling(animationTime); 80 | m_LocalTransform = translation * rotation * scale; 81 | } 82 | glm::mat4 GetLocalTransform() { return m_LocalTransform; } 83 | std::string GetBoneName() const { return m_Name; } 84 | int GetBoneID() { return m_ID; } 85 | 86 | 87 | 88 | int GetPositionIndex(float animationTime) 89 | { 90 | for (int index = 0; index < m_NumPositions - 1; ++index) 91 | { 92 | if (animationTime < m_Positions[index + 1].timeStamp) 93 | return index; 94 | } 95 | assert(0); 96 | } 97 | 98 | int GetRotationIndex(float animationTime) 99 | { 100 | for (int index = 0; index < m_NumRotations - 1; ++index) 101 | { 102 | if (animationTime < m_Rotations[index + 1].timeStamp) 103 | return index; 104 | } 105 | assert(0); 106 | } 107 | 108 | int GetScaleIndex(float animationTime) 109 | { 110 | for (int index = 0; index < m_NumScalings - 1; ++index) 111 | { 112 | if (animationTime < m_Scales[index + 1].timeStamp) 113 | return index; 114 | } 115 | assert(0); 116 | } 117 | 118 | 119 | private: 120 | 121 | float GetScaleFactor(float lastTimeStamp, float nextTimeStamp, float animationTime) 122 | { 123 | float scaleFactor = 0.0f; 124 | float midWayLength = animationTime - lastTimeStamp; 125 | float framesDiff = nextTimeStamp - lastTimeStamp; 126 | scaleFactor = midWayLength / framesDiff; 127 | return scaleFactor; 128 | } 129 | 130 | glm::mat4 InterpolatePosition(float animationTime) 131 | { 132 | if (1 == m_NumPositions) 133 | return glm::translate(glm::mat4(1.0f), m_Positions[0].position); 134 | 135 | int p0Index = GetPositionIndex(animationTime); 136 | int p1Index = p0Index + 1; 137 | float scaleFactor = GetScaleFactor(m_Positions[p0Index].timeStamp, 138 | m_Positions[p1Index].timeStamp, animationTime); 139 | glm::vec3 finalPosition = glm::mix(m_Positions[p0Index].position, m_Positions[p1Index].position 140 | , scaleFactor); 141 | return glm::translate(glm::mat4(1.0f), finalPosition); 142 | } 143 | 144 | glm::mat4 InterpolateRotation(float animationTime) 145 | { 146 | if (1 == m_NumRotations) 147 | { 148 | auto rotation = glm::normalize(m_Rotations[0].orientation); 149 | return glm::toMat4(rotation); 150 | } 151 | 152 | int p0Index = GetRotationIndex(animationTime); 153 | int p1Index = p0Index + 1; 154 | float scaleFactor = GetScaleFactor(m_Rotations[p0Index].timeStamp, 155 | m_Rotations[p1Index].timeStamp, animationTime); 156 | glm::quat finalRotation = glm::slerp(m_Rotations[p0Index].orientation, m_Rotations[p1Index].orientation 157 | , scaleFactor); 158 | finalRotation = glm::normalize(finalRotation); 159 | return glm::toMat4(finalRotation); 160 | 161 | } 162 | 163 | glm::mat4 InterpolateScaling(float animationTime) 164 | { 165 | if (1 == m_NumScalings) 166 | return glm::scale(glm::mat4(1.0f), m_Scales[0].scale); 167 | 168 | int p0Index = GetScaleIndex(animationTime); 169 | int p1Index = p0Index + 1; 170 | float scaleFactor = GetScaleFactor(m_Scales[p0Index].timeStamp, 171 | m_Scales[p1Index].timeStamp, animationTime); 172 | glm::vec3 finalScale = glm::mix(m_Scales[p0Index].scale, m_Scales[p1Index].scale 173 | , scaleFactor); 174 | return glm::scale(glm::mat4(1.0f), finalScale); 175 | } 176 | 177 | std::vector m_Positions; 178 | std::vector m_Rotations; 179 | std::vector m_Scales; 180 | int m_NumPositions; 181 | int m_NumRotations; 182 | int m_NumScalings; 183 | 184 | glm::mat4 m_LocalTransform; 185 | std::string m_Name; 186 | int m_ID; 187 | }; 188 | 189 | -------------------------------------------------------------------------------- /src/shader/learnopengl/camera.h: -------------------------------------------------------------------------------- 1 | #ifndef CAMERA_H 2 | #define CAMERA_H 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | #include 9 | 10 | // Defines several possible options for camera movement. Used as abstraction to stay away from window-system specific input methods 11 | enum Camera_Movement { 12 | FORWARD, 13 | BACKWARD, 14 | LEFT, 15 | RIGHT 16 | }; 17 | 18 | // Default camera values 19 | const float YAW = -90.0f; 20 | const float PITCH = 0.0f; 21 | const float SPEED = 2.5f; 22 | const float SENSITIVITY = 0.1f; 23 | const float ZOOM = 45.0f; 24 | 25 | 26 | // An abstract camera class that processes input and calculates the corresponding Euler Angles, Vectors and Matrices for use in OpenGL 27 | class Camera 28 | { 29 | public: 30 | // camera Attributes 31 | glm::vec3 Position; 32 | glm::vec3 Front; 33 | glm::vec3 Up; 34 | glm::vec3 Right; 35 | glm::vec3 WorldUp; 36 | // euler Angles 37 | float Yaw; 38 | float Pitch; 39 | // camera options 40 | float MovementSpeed; 41 | float MouseSensitivity; 42 | float Zoom; 43 | 44 | // constructor with vectors 45 | Camera(glm::vec3 position = glm::vec3(0.0f, 0.0f, 0.0f), glm::vec3 up = glm::vec3(0.0f, 1.0f, 0.0f), float yaw = YAW, float pitch = PITCH) : Front(glm::vec3(0.0f, 0.0f, -1.0f)), MovementSpeed(SPEED), MouseSensitivity(SENSITIVITY), Zoom(ZOOM) 46 | { 47 | Position = position; 48 | WorldUp = up; 49 | Yaw = yaw; 50 | Pitch = pitch; 51 | updateCameraVectors(); 52 | } 53 | // constructor with scalar values 54 | Camera(float posX, float posY, float posZ, float upX, float upY, float upZ, float yaw, float pitch) : Front(glm::vec3(0.0f, 0.0f, -1.0f)), MovementSpeed(SPEED), MouseSensitivity(SENSITIVITY), Zoom(ZOOM) 55 | { 56 | Position = glm::vec3(posX, posY, posZ); 57 | WorldUp = glm::vec3(upX, upY, upZ); 58 | Yaw = yaw; 59 | Pitch = pitch; 60 | updateCameraVectors(); 61 | } 62 | 63 | // returns the view matrix calculated using Euler Angles and the LookAt Matrix 64 | glm::mat4 GetViewMatrix() 65 | { 66 | return glm::lookAt(Position, Position + Front, Up); 67 | } 68 | 69 | // processes input received from any keyboard-like input system. Accepts input parameter in the form of camera defined ENUM (to abstract it from windowing systems) 70 | void ProcessKeyboard(Camera_Movement direction, float deltaTime) 71 | { 72 | float velocity = MovementSpeed * deltaTime; 73 | if (direction == FORWARD) 74 | Position += Front * velocity; 75 | if (direction == BACKWARD) 76 | Position -= Front * velocity; 77 | if (direction == LEFT) 78 | Position -= Right * velocity; 79 | if (direction == RIGHT) 80 | Position += Right * velocity; 81 | } 82 | 83 | // processes input received from a mouse input system. Expects the offset value in both the x and y direction. 84 | void ProcessMouseMovement(float xoffset, float yoffset, GLboolean constrainPitch = true) 85 | { 86 | xoffset *= MouseSensitivity; 87 | yoffset *= MouseSensitivity; 88 | 89 | Yaw += xoffset; 90 | Pitch += yoffset; 91 | 92 | // make sure that when pitch is out of bounds, screen doesn't get flipped 93 | if (constrainPitch) 94 | { 95 | if (Pitch > 89.0f) 96 | Pitch = 89.0f; 97 | if (Pitch < -89.0f) 98 | Pitch = -89.0f; 99 | } 100 | 101 | // update Front, Right and Up Vectors using the updated Euler angles 102 | updateCameraVectors(); 103 | } 104 | 105 | // processes input received from a mouse scroll-wheel event. Only requires input on the vertical wheel-axis 106 | void ProcessMouseScroll(float yoffset) 107 | { 108 | Zoom -= (float)yoffset; 109 | if (Zoom < 1.0f) 110 | Zoom = 1.0f; 111 | if (Zoom > 45.0f) 112 | Zoom = 45.0f; 113 | } 114 | 115 | private: 116 | // calculates the front vector from the Camera's (updated) Euler Angles 117 | void updateCameraVectors() 118 | { 119 | // calculate the new Front vector 120 | glm::vec3 front; 121 | front.x = cos(glm::radians(Yaw)) * cos(glm::radians(Pitch)); 122 | front.y = sin(glm::radians(Pitch)); 123 | front.z = sin(glm::radians(Yaw)) * cos(glm::radians(Pitch)); 124 | Front = glm::normalize(front); 125 | // also re-calculate the Right and Up vector 126 | Right = glm::normalize(glm::cross(Front, WorldUp)); // normalize the vectors, because their length gets closer to 0 the more you look up or down which results in slower movement. 127 | Up = glm::normalize(glm::cross(Right, Front)); 128 | } 129 | }; 130 | #endif 131 | -------------------------------------------------------------------------------- /src/shader/learnopengl/filesystem.h: -------------------------------------------------------------------------------- 1 | #ifndef FILESYSTEM_H 2 | #define FILESYSTEM_H 3 | 4 | #include 5 | #include 6 | static const char * logl_root = "/home/ziv/opt/LearnOpenGL"; 7 | // #include "root_directory.h" // This is a configuration file generated by CMake. 8 | 9 | class FileSystem 10 | { 11 | private: 12 | typedef std::string (*Builder) (const std::string& path); 13 | 14 | public: 15 | static std::string getPath(const std::string& path) 16 | { 17 | static std::string(*pathBuilder)(std::string const &) = getPathBuilder(); 18 | return (*pathBuilder)(path); 19 | } 20 | 21 | private: 22 | static std::string const & getRoot() 23 | { 24 | static char const * envRoot = getenv("LOGL_ROOT_PATH"); 25 | static char const * givenRoot = (envRoot != nullptr ? envRoot : logl_root); 26 | static std::string root = (givenRoot != nullptr ? givenRoot : ""); 27 | return root; 28 | } 29 | 30 | //static std::string(*foo (std::string const &)) getPathBuilder() 31 | static Builder getPathBuilder() 32 | { 33 | if (getRoot() != "") 34 | return &FileSystem::getPathRelativeRoot; 35 | else 36 | return &FileSystem::getPathRelativeBinary; 37 | } 38 | 39 | static std::string getPathRelativeRoot(const std::string& path) 40 | { 41 | return getRoot() + std::string("/") + path; 42 | } 43 | 44 | static std::string getPathRelativeBinary(const std::string& path) 45 | { 46 | return "../../../" + path; 47 | } 48 | 49 | 50 | }; 51 | 52 | // FILESYSTEM_H 53 | #endif 54 | -------------------------------------------------------------------------------- /src/shader/learnopengl/mesh.h: -------------------------------------------------------------------------------- 1 | #ifndef MESH_H 2 | #define MESH_H 3 | 4 | #include // holds all OpenGL type declarations 5 | 6 | #include 7 | #include 8 | 9 | #include "./shader.h" 10 | 11 | #include 12 | #include 13 | using namespace std; 14 | 15 | #define MAX_BONE_INFLUENCE 4 16 | 17 | struct Vertex { 18 | // position 19 | glm::vec3 Position; 20 | // normal 21 | glm::vec3 Normal; 22 | // texCoords 23 | glm::vec2 TexCoords; 24 | // tangent 25 | glm::vec3 Tangent; 26 | // bitangent 27 | glm::vec3 Bitangent; 28 | //bone indexes which will influence this vertex 29 | int m_BoneIDs[MAX_BONE_INFLUENCE]; 30 | //weights from each bone 31 | float m_Weights[MAX_BONE_INFLUENCE]; 32 | }; 33 | 34 | struct Texture { 35 | unsigned int id; 36 | string type; 37 | string path; 38 | }; 39 | 40 | class Mesh { 41 | public: 42 | // mesh Data 43 | vector vertices; 44 | vector indices; 45 | vector textures; 46 | unsigned int VAO; 47 | 48 | // constructor 49 | Mesh(vector vertices, vector indices, vector textures) 50 | { 51 | this->vertices = vertices; 52 | this->indices = indices; 53 | this->textures = textures; 54 | 55 | // now that we have all the required data, set the vertex buffers and its attribute pointers. 56 | setupMesh(); 57 | } 58 | 59 | // render the mesh 60 | void Draw(Shader &shader) 61 | { 62 | // bind appropriate textures 63 | unsigned int diffuseNr = 1; 64 | unsigned int specularNr = 1; 65 | unsigned int normalNr = 1; 66 | unsigned int heightNr = 1; 67 | for(unsigned int i = 0; i < textures.size(); i++) 68 | { 69 | glActiveTexture(GL_TEXTURE0 + i); // active proper texture unit before binding 70 | // retrieve texture number (the N in diffuse_textureN) 71 | string number; 72 | string name = textures[i].type; 73 | if(name == "texture_diffuse") 74 | number = std::to_string(diffuseNr++); 75 | else if(name == "texture_specular") 76 | number = std::to_string(specularNr++); // transfer unsigned int to string 77 | else if(name == "texture_normal") 78 | number = std::to_string(normalNr++); // transfer unsigned int to string 79 | else if(name == "texture_height") 80 | number = std::to_string(heightNr++); // transfer unsigned int to string 81 | 82 | // now set the sampler to the correct texture unit 83 | glUniform1i(glGetUniformLocation(shader.ID, (name + number).c_str()), i); 84 | // and finally bind the texture 85 | glBindTexture(GL_TEXTURE_2D, textures[i].id); 86 | } 87 | 88 | // draw mesh 89 | glBindVertexArray(VAO); 90 | glDrawElements(GL_TRIANGLES, static_cast(indices.size()), GL_UNSIGNED_INT, 0); 91 | glBindVertexArray(0); 92 | 93 | // always good practice to set everything back to defaults once configured. 94 | glActiveTexture(GL_TEXTURE0); 95 | } 96 | 97 | private: 98 | // render data 99 | unsigned int VBO, EBO; 100 | 101 | // initializes all the buffer objects/arrays 102 | void setupMesh() 103 | { 104 | // create buffers/arrays 105 | glGenVertexArrays(1, &VAO); 106 | glGenBuffers(1, &VBO); 107 | glGenBuffers(1, &EBO); 108 | 109 | glBindVertexArray(VAO); 110 | // load data into vertex buffers 111 | glBindBuffer(GL_ARRAY_BUFFER, VBO); 112 | // A great thing about structs is that their memory layout is sequential for all its items. 113 | // The effect is that we can simply pass a pointer to the struct and it translates perfectly to a glm::vec3/2 array which 114 | // again translates to 3/2 floats which translates to a byte array. 115 | glBufferData(GL_ARRAY_BUFFER, vertices.size() * sizeof(Vertex), &vertices[0], GL_STATIC_DRAW); 116 | 117 | glBindBuffer(GL_ELEMENT_ARRAY_BUFFER, EBO); 118 | glBufferData(GL_ELEMENT_ARRAY_BUFFER, indices.size() * sizeof(unsigned int), &indices[0], GL_STATIC_DRAW); 119 | 120 | // set the vertex attribute pointers 121 | // vertex Positions 122 | glEnableVertexAttribArray(0); 123 | glVertexAttribPointer(0, 3, GL_FLOAT, GL_FALSE, sizeof(Vertex), (void*)0); 124 | // vertex normals 125 | glEnableVertexAttribArray(1); 126 | glVertexAttribPointer(1, 3, GL_FLOAT, GL_FALSE, sizeof(Vertex), (void*)offsetof(Vertex, Normal)); 127 | // vertex texture coords 128 | glEnableVertexAttribArray(2); 129 | glVertexAttribPointer(2, 2, GL_FLOAT, GL_FALSE, sizeof(Vertex), (void*)offsetof(Vertex, TexCoords)); 130 | // vertex tangent 131 | glEnableVertexAttribArray(3); 132 | glVertexAttribPointer(3, 3, GL_FLOAT, GL_FALSE, sizeof(Vertex), (void*)offsetof(Vertex, Tangent)); 133 | // vertex bitangent 134 | glEnableVertexAttribArray(4); 135 | glVertexAttribPointer(4, 3, GL_FLOAT, GL_FALSE, sizeof(Vertex), (void*)offsetof(Vertex, Bitangent)); 136 | // ids 137 | glEnableVertexAttribArray(5); 138 | glVertexAttribIPointer(5, 4, GL_INT, sizeof(Vertex), (void*)offsetof(Vertex, m_BoneIDs)); 139 | 140 | // weights 141 | glEnableVertexAttribArray(6); 142 | glVertexAttribPointer(6, 4, GL_FLOAT, GL_FALSE, sizeof(Vertex), (void*)offsetof(Vertex, m_Weights)); 143 | glBindVertexArray(0); 144 | } 145 | }; 146 | #endif 147 | -------------------------------------------------------------------------------- /src/shader/learnopengl/shader_c.h: -------------------------------------------------------------------------------- 1 | #ifndef COMPUTE_SHADER_H 2 | #define COMPUTE_SHADER_H 3 | 4 | #include 5 | #include 6 | 7 | #include 8 | #include 9 | #include 10 | #include 11 | 12 | class ComputeShader 13 | { 14 | public: 15 | unsigned int ID; 16 | // constructor generates the shader on the fly 17 | // ------------------------------------------------------------------------ 18 | ComputeShader(const char* computePath) 19 | { 20 | // 1. retrieve the vertex/fragment source code from filePath 21 | std::string computeCode; 22 | std::ifstream cShaderFile; 23 | // ensure ifstream objects can throw exceptions: 24 | cShaderFile.exceptions(std::ifstream::failbit | std::ifstream::badbit); 25 | try 26 | { 27 | // open files 28 | cShaderFile.open(computePath); 29 | 30 | std::stringstream cShaderStream; 31 | // read file's buffer contents into streams 32 | cShaderStream << cShaderFile.rdbuf(); 33 | // close file handlers 34 | cShaderFile.close(); 35 | // convert stream into string 36 | computeCode = cShaderStream.str(); 37 | } 38 | catch (std::ifstream::failure& e) 39 | { 40 | std::cout << "ERROR::SHADER::FILE_NOT_SUCCESSFULLY_READ: " << e.what() << std::endl; 41 | } 42 | const char* cShaderCode = computeCode.c_str(); 43 | // 2. compile shaders 44 | unsigned int compute; 45 | // compute shader 46 | compute = glCreateShader(GL_COMPUTE_SHADER); 47 | glShaderSource(compute, 1, &cShaderCode, NULL); 48 | glCompileShader(compute); 49 | checkCompileErrors(compute, "COMPUTE"); 50 | 51 | // shader Program 52 | ID = glCreateProgram(); 53 | glAttachShader(ID, compute); 54 | glLinkProgram(ID); 55 | checkCompileErrors(ID, "PROGRAM"); 56 | // delete the shaders as they're linked into our program now and no longer necessary 57 | glDeleteShader(compute); 58 | } 59 | // activate the shader 60 | // ------------------------------------------------------------------------ 61 | void use() 62 | { 63 | glUseProgram(ID); 64 | } 65 | // utility uniform functions 66 | // ------------------------------------------------------------------------ 67 | void setBool(const std::string &name, bool value) const 68 | { 69 | glUniform1i(glGetUniformLocation(ID, name.c_str()), (int)value); 70 | } 71 | // ------------------------------------------------------------------------ 72 | void setInt(const std::string &name, int value) const 73 | { 74 | glUniform1i(glGetUniformLocation(ID, name.c_str()), value); 75 | } 76 | // ------------------------------------------------------------------------ 77 | void setFloat(const std::string &name, float value) const 78 | { 79 | glUniform1f(glGetUniformLocation(ID, name.c_str()), value); 80 | } 81 | // ------------------------------------------------------------------------ 82 | void setVec2(const std::string &name, const glm::vec2 &value) const 83 | { 84 | glUniform2fv(glGetUniformLocation(ID, name.c_str()), 1, &value[0]); 85 | } 86 | void setVec2(const std::string &name, float x, float y) const 87 | { 88 | glUniform2f(glGetUniformLocation(ID, name.c_str()), x, y); 89 | } 90 | // ------------------------------------------------------------------------ 91 | void setVec3(const std::string &name, const glm::vec3 &value) const 92 | { 93 | glUniform3fv(glGetUniformLocation(ID, name.c_str()), 1, &value[0]); 94 | } 95 | void setVec3(const std::string &name, float x, float y, float z) const 96 | { 97 | glUniform3f(glGetUniformLocation(ID, name.c_str()), x, y, z); 98 | } 99 | // ------------------------------------------------------------------------ 100 | void setVec4(const std::string &name, const glm::vec4 &value) const 101 | { 102 | glUniform4fv(glGetUniformLocation(ID, name.c_str()), 1, &value[0]); 103 | } 104 | void setVec4(const std::string &name, float x, float y, float z, float w) 105 | { 106 | glUniform4f(glGetUniformLocation(ID, name.c_str()), x, y, z, w); 107 | } 108 | // ------------------------------------------------------------------------ 109 | void setMat2(const std::string &name, const glm::mat2 &mat) const 110 | { 111 | glUniformMatrix2fv(glGetUniformLocation(ID, name.c_str()), 1, GL_FALSE, &mat[0][0]); 112 | } 113 | // ------------------------------------------------------------------------ 114 | void setMat3(const std::string &name, const glm::mat3 &mat) const 115 | { 116 | glUniformMatrix3fv(glGetUniformLocation(ID, name.c_str()), 1, GL_FALSE, &mat[0][0]); 117 | } 118 | // ------------------------------------------------------------------------ 119 | void setMat4(const std::string &name, const glm::mat4 &mat) const 120 | { 121 | glUniformMatrix4fv(glGetUniformLocation(ID, name.c_str()), 1, GL_FALSE, &mat[0][0]); 122 | } 123 | 124 | private: 125 | // utility function for checking shader compilation/linking errors. 126 | // ------------------------------------------------------------------------ 127 | void checkCompileErrors(GLuint shader, std::string type) 128 | { 129 | GLint success; 130 | GLchar infoLog[1024]; 131 | if(type != "PROGRAM") 132 | { 133 | glGetShaderiv(shader, GL_COMPILE_STATUS, &success); 134 | if(!success) 135 | { 136 | glGetShaderInfoLog(shader, 1024, NULL, infoLog); 137 | std::cout << "ERROR::SHADER_COMPILATION_ERROR of type: " << type << "\n" << infoLog << "\n -- --------------------------------------------------- -- " << std::endl; 138 | } 139 | } 140 | else 141 | { 142 | glGetProgramiv(shader, GL_LINK_STATUS, &success); 143 | if(!success) 144 | { 145 | glGetProgramInfoLog(shader, 1024, NULL, infoLog); 146 | std::cout << "ERROR::PROGRAM_LINKING_ERROR of type: " << type << "\n" << infoLog << "\n -- --------------------------------------------------- -- " << std::endl; 147 | } 148 | } 149 | } 150 | }; 151 | #endif -------------------------------------------------------------------------------- /src/shader/learnopengl/shader_s.h: -------------------------------------------------------------------------------- 1 | #ifndef SHADER_H 2 | #define SHADER_H 3 | 4 | #include 5 | 6 | #include 7 | #include 8 | #include 9 | #include 10 | 11 | class Shader 12 | { 13 | public: 14 | unsigned int ID; 15 | // constructor generates the shader on the fly 16 | // ------------------------------------------------------------------------ 17 | Shader(const char* vertexPath, const char* fragmentPath) 18 | { 19 | // 1. retrieve the vertex/fragment source code from filePath 20 | std::string vertexCode; 21 | std::string fragmentCode; 22 | std::ifstream vShaderFile; 23 | std::ifstream fShaderFile; 24 | // ensure ifstream objects can throw exceptions: 25 | vShaderFile.exceptions (std::ifstream::failbit | std::ifstream::badbit); 26 | fShaderFile.exceptions (std::ifstream::failbit | std::ifstream::badbit); 27 | try 28 | { 29 | // open files 30 | vShaderFile.open(vertexPath); 31 | fShaderFile.open(fragmentPath); 32 | std::stringstream vShaderStream, fShaderStream; 33 | // read file's buffer contents into streams 34 | vShaderStream << vShaderFile.rdbuf(); 35 | fShaderStream << fShaderFile.rdbuf(); 36 | // close file handlers 37 | vShaderFile.close(); 38 | fShaderFile.close(); 39 | // convert stream into string 40 | vertexCode = vShaderStream.str(); 41 | fragmentCode = fShaderStream.str(); 42 | } 43 | catch (std::ifstream::failure& e) 44 | { 45 | std::cout << "ERROR::SHADER::FILE_NOT_SUCCESSFULLY_READ: " << e.what() << std::endl; 46 | } 47 | const char* vShaderCode = vertexCode.c_str(); 48 | const char * fShaderCode = fragmentCode.c_str(); 49 | // 2. compile shaders 50 | unsigned int vertex, fragment; 51 | // vertex shader 52 | vertex = glCreateShader(GL_VERTEX_SHADER); 53 | glShaderSource(vertex, 1, &vShaderCode, NULL); 54 | glCompileShader(vertex); 55 | checkCompileErrors(vertex, "VERTEX"); 56 | // fragment Shader 57 | fragment = glCreateShader(GL_FRAGMENT_SHADER); 58 | glShaderSource(fragment, 1, &fShaderCode, NULL); 59 | glCompileShader(fragment); 60 | checkCompileErrors(fragment, "FRAGMENT"); 61 | // shader Program 62 | ID = glCreateProgram(); 63 | glAttachShader(ID, vertex); 64 | glAttachShader(ID, fragment); 65 | glLinkProgram(ID); 66 | checkCompileErrors(ID, "PROGRAM"); 67 | // delete the shaders as they're linked into our program now and no longer necessary 68 | glDeleteShader(vertex); 69 | glDeleteShader(fragment); 70 | } 71 | // activate the shader 72 | // ------------------------------------------------------------------------ 73 | void use() 74 | { 75 | glUseProgram(ID); 76 | } 77 | // utility uniform functions 78 | // ------------------------------------------------------------------------ 79 | void setBool(const std::string &name, bool value) const 80 | { 81 | glUniform1i(glGetUniformLocation(ID, name.c_str()), (int)value); 82 | } 83 | // ------------------------------------------------------------------------ 84 | void setInt(const std::string &name, int value) const 85 | { 86 | glUniform1i(glGetUniformLocation(ID, name.c_str()), value); 87 | } 88 | // ------------------------------------------------------------------------ 89 | void setFloat(const std::string &name, float value) const 90 | { 91 | glUniform1f(glGetUniformLocation(ID, name.c_str()), value); 92 | } 93 | 94 | private: 95 | // utility function for checking shader compilation/linking errors. 96 | // ------------------------------------------------------------------------ 97 | void checkCompileErrors(unsigned int shader, std::string type) 98 | { 99 | int success; 100 | char infoLog[1024]; 101 | if (type != "PROGRAM") 102 | { 103 | glGetShaderiv(shader, GL_COMPILE_STATUS, &success); 104 | if (!success) 105 | { 106 | glGetShaderInfoLog(shader, 1024, NULL, infoLog); 107 | std::cout << "ERROR::SHADER_COMPILATION_ERROR of type: " << type << "\n" << infoLog << "\n -- --------------------------------------------------- -- " << std::endl; 108 | } 109 | } 110 | else 111 | { 112 | glGetProgramiv(shader, GL_LINK_STATUS, &success); 113 | if (!success) 114 | { 115 | glGetProgramInfoLog(shader, 1024, NULL, infoLog); 116 | std::cout << "ERROR::PROGRAM_LINKING_ERROR of type: " << type << "\n" << infoLog << "\n -- --------------------------------------------------- -- " << std::endl; 117 | } 118 | } 119 | } 120 | }; 121 | #endif 122 | -------------------------------------------------------------------------------- /src/shader/points_shader.fs: -------------------------------------------------------------------------------- 1 | /* 2 | This code is the implementation of our paper "ImMesh: An Immediate LiDAR Localization and Meshing Framework". 3 | 4 | The source code of this package is released under GPLv2 license. We only allow it free for personal and academic usage. 5 | 6 | If you use any code of this repo in your academic research, please cite at least one of our papers: 7 | [1] Lin, Jiarong, et al. "Immesh: An immediate lidar localization and meshing framework." IEEE Transactions on Robotics 8 | (T-RO 2023) 9 | [2] Yuan, Chongjian, et al. "Efficient and probabilistic adaptive voxel mapping for accurate online lidar odometry." 10 | IEEE Robotics and Automation Letters (RA-L 2022) 11 | [3] Lin, Jiarong, and Fu Zhang. "R3LIVE: A Robust, Real-time, RGB-colored, LiDAR-Inertial-Visual tightly-coupled 12 | state Estimation and mapping package." IEEE International Conference on Robotics and Automation (ICRA 2022) 13 | 14 | For commercial use, please contact me and Dr. Fu Zhang to negotiate a 15 | different license. 16 | 17 | Redistribution and use in source and binary forms, with or without 18 | modification, are permitted provided that the following conditions are met: 19 | 20 | 1. Redistributions of source code must retain the above copyright notice, 21 | this list of conditions and the following disclaimer. 22 | 2. Redistributions in binary form must reproduce the above copyright notice, 23 | this list of conditions and the following disclaimer in the documentation 24 | and/or other materials provided with the distribution. 25 | 3. Neither the name of the copyright holder nor the names of its 26 | contributors may be used to endorse or promote products derived from this 27 | software without specific prior written permission. 28 | 29 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 30 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 31 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 32 | ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 33 | LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 34 | CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 35 | SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 36 | INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 37 | CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 38 | ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 39 | POSSIBILITY OF SUCH DAMAGE. 40 | */ 41 | #version 330 core 42 | 43 | // layout (location = 1) in vec2 point_color; 44 | 45 | uniform vec3 pointColor; 46 | uniform float pointAlpha; 47 | 48 | out vec4 color; 49 | 50 | void main() 51 | { 52 | color = vec4(pointColor, pointAlpha); // set the color and alpha of the points 53 | // color = vec4(1, 1, 1 , 1); // set the color and alpha of the points 54 | } -------------------------------------------------------------------------------- /src/shader/points_shader.vs: -------------------------------------------------------------------------------- 1 | /* 2 | This code is the implementation of our paper "ImMesh: An Immediate LiDAR Localization and Meshing Framework". 3 | 4 | The source code of this package is released under GPLv2 license. We only allow it free for personal and academic usage. 5 | 6 | If you use any code of this repo in your academic research, please cite at least one of our papers: 7 | [1] Lin, Jiarong, et al. "Immesh: An immediate lidar localization and meshing framework." IEEE Transactions on Robotics 8 | (T-RO 2023) 9 | [2] Yuan, Chongjian, et al. "Efficient and probabilistic adaptive voxel mapping for accurate online lidar odometry." 10 | IEEE Robotics and Automation Letters (RA-L 2022) 11 | [3] Lin, Jiarong, and Fu Zhang. "R3LIVE: A Robust, Real-time, RGB-colored, LiDAR-Inertial-Visual tightly-coupled 12 | state Estimation and mapping package." IEEE International Conference on Robotics and Automation (ICRA 2022) 13 | 14 | For commercial use, please contact me and Dr. Fu Zhang to negotiate a 15 | different license. 16 | 17 | Redistribution and use in source and binary forms, with or without 18 | modification, are permitted provided that the following conditions are met: 19 | 20 | 1. Redistributions of source code must retain the above copyright notice, 21 | this list of conditions and the following disclaimer. 22 | 2. Redistributions in binary form must reproduce the above copyright notice, 23 | this list of conditions and the following disclaimer in the documentation 24 | and/or other materials provided with the distribution. 25 | 3. Neither the name of the copyright holder nor the names of its 26 | contributors may be used to endorse or promote products derived from this 27 | software without specific prior written permission. 28 | 29 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 30 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 31 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 32 | ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 33 | LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 34 | CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 35 | SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 36 | INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 37 | CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 38 | ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 39 | POSSIBILITY OF SUCH DAMAGE. 40 | */ 41 | #version 330 core 42 | layout (location = 0) in vec3 aPos; 43 | 44 | 45 | uniform mat4 projection_mul_view; 46 | uniform float pointSize; 47 | 48 | void main() 49 | { 50 | gl_Position = projection_mul_view * vec4(aPos, 1.0); 51 | gl_PointSize = pointSize; 52 | } -------------------------------------------------------------------------------- /src/shader/points_shader_rgb.fs: -------------------------------------------------------------------------------- 1 | /* 2 | This code is the implementation of our paper "ImMesh: An Immediate LiDAR Localization and Meshing Framework". 3 | 4 | The source code of this package is released under GPLv2 license. We only allow it free for personal and academic usage. 5 | 6 | If you use any code of this repo in your academic research, please cite at least one of our papers: 7 | [1] Lin, Jiarong, et al. "Immesh: An immediate lidar localization and meshing framework." IEEE Transactions on Robotics 8 | (T-RO 2023) 9 | [2] Yuan, Chongjian, et al. "Efficient and probabilistic adaptive voxel mapping for accurate online lidar odometry." 10 | IEEE Robotics and Automation Letters (RA-L 2022) 11 | [3] Lin, Jiarong, and Fu Zhang. "R3LIVE: A Robust, Real-time, RGB-colored, LiDAR-Inertial-Visual tightly-coupled 12 | state Estimation and mapping package." IEEE International Conference on Robotics and Automation (ICRA 2022) 13 | 14 | For commercial use, please contact me and Dr. Fu Zhang to negotiate a 15 | different license. 16 | 17 | Redistribution and use in source and binary forms, with or without 18 | modification, are permitted provided that the following conditions are met: 19 | 20 | 1. Redistributions of source code must retain the above copyright notice, 21 | this list of conditions and the following disclaimer. 22 | 2. Redistributions in binary form must reproduce the above copyright notice, 23 | this list of conditions and the following disclaimer in the documentation 24 | and/or other materials provided with the distribution. 25 | 3. Neither the name of the copyright holder nor the names of its 26 | contributors may be used to endorse or promote products derived from this 27 | software without specific prior written permission. 28 | 29 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 30 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 31 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 32 | ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 33 | LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 34 | CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 35 | SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 36 | INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 37 | CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 38 | ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 39 | POSSIBILITY OF SUCH DAMAGE. 40 | */ 41 | // Fragment Shader 42 | #version 330 core 43 | in vec3 fragColor; 44 | out vec4 FragColor; 45 | uniform float pointAlpha; 46 | 47 | void main() 48 | { 49 | FragColor = vec4(fragColor, pointAlpha); 50 | } -------------------------------------------------------------------------------- /src/shader/points_shader_rgb.vs: -------------------------------------------------------------------------------- 1 | /* 2 | This code is the implementation of our paper "ImMesh: An Immediate LiDAR Localization and Meshing Framework". 3 | 4 | The source code of this package is released under GPLv2 license. We only allow it free for personal and academic usage. 5 | 6 | If you use any code of this repo in your academic research, please cite at least one of our papers: 7 | [1] Lin, Jiarong, et al. "Immesh: An immediate lidar localization and meshing framework." IEEE Transactions on Robotics 8 | (T-RO 2023) 9 | [2] Yuan, Chongjian, et al. "Efficient and probabilistic adaptive voxel mapping for accurate online lidar odometry." 10 | IEEE Robotics and Automation Letters (RA-L 2022) 11 | [3] Lin, Jiarong, and Fu Zhang. "R3LIVE: A Robust, Real-time, RGB-colored, LiDAR-Inertial-Visual tightly-coupled 12 | state Estimation and mapping package." IEEE International Conference on Robotics and Automation (ICRA 2022) 13 | 14 | For commercial use, please contact me and Dr. Fu Zhang to negotiate a 15 | different license. 16 | 17 | Redistribution and use in source and binary forms, with or without 18 | modification, are permitted provided that the following conditions are met: 19 | 20 | 1. Redistributions of source code must retain the above copyright notice, 21 | this list of conditions and the following disclaimer. 22 | 2. Redistributions in binary form must reproduce the above copyright notice, 23 | this list of conditions and the following disclaimer in the documentation 24 | and/or other materials provided with the distribution. 25 | 3. Neither the name of the copyright holder nor the names of its 26 | contributors may be used to endorse or promote products derived from this 27 | software without specific prior written permission. 28 | 29 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 30 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 31 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 32 | ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 33 | LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 34 | CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 35 | SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 36 | INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 37 | CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 38 | ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 39 | POSSIBILITY OF SUCH DAMAGE. 40 | */ 41 | #version 330 core 42 | layout (location = 0) in vec3 aPos; 43 | layout (location = 1) in float aColor; 44 | 45 | out vec3 fragColor; 46 | 47 | uniform mat4 projection_mul_view; 48 | uniform float pointSize; 49 | 50 | void main() 51 | { 52 | gl_Position = projection_mul_view * vec4(aPos, 1.0); 53 | int color_int = int(aColor); // I Don't know why here need a cast 54 | fragColor = vec3( ( ( color_int >> 16 ) & 0xff ), 55 | ( ( color_int >> 8 ) & 0xff ), 56 | ( ( color_int >> 0 ) & 0xff ) ) / 255.0; 57 | gl_PointSize = pointSize; 58 | } -------------------------------------------------------------------------------- /src/shader/shader_s.h: -------------------------------------------------------------------------------- 1 | #ifndef SHADER_H 2 | #define SHADER_H 3 | 4 | // #include 5 | #include "../r4live_dev/openGL/glad.h" 6 | 7 | #include 8 | #include 9 | #include 10 | #include 11 | 12 | class Shader 13 | { 14 | public: 15 | unsigned int ID; 16 | // constructor generates the shader on the fly 17 | // ------------------------------------------------------------------------ 18 | Shader(const char* vertexPath, const char* fragmentPath) 19 | { 20 | // 1. retrieve the vertex/fragment source code from filePath 21 | std::string vertexCode; 22 | std::string fragmentCode; 23 | std::ifstream vShaderFile; 24 | std::ifstream fShaderFile; 25 | // ensure ifstream objects can throw exceptions: 26 | vShaderFile.exceptions (std::ifstream::failbit | std::ifstream::badbit); 27 | fShaderFile.exceptions (std::ifstream::failbit | std::ifstream::badbit); 28 | try 29 | { 30 | // open files 31 | vShaderFile.open(vertexPath); 32 | fShaderFile.open(fragmentPath); 33 | std::stringstream vShaderStream, fShaderStream; 34 | // read file's buffer contents into streams 35 | vShaderStream << vShaderFile.rdbuf(); 36 | fShaderStream << fShaderFile.rdbuf(); 37 | // close file handlers 38 | vShaderFile.close(); 39 | fShaderFile.close(); 40 | // convert stream into string 41 | vertexCode = vShaderStream.str(); 42 | fragmentCode = fShaderStream.str(); 43 | } 44 | catch (std::ifstream::failure& e) 45 | { 46 | std::cout << "ERROR::SHADER::FILE_NOT_SUCCESFULLY_READ: " << e.what() << std::endl; 47 | } 48 | const char* vShaderCode = vertexCode.c_str(); 49 | const char * fShaderCode = fragmentCode.c_str(); 50 | // 2. compile shaders 51 | unsigned int vertex, fragment; 52 | // vertex shader 53 | vertex = glCreateShader(GL_VERTEX_SHADER); 54 | glShaderSource(vertex, 1, &vShaderCode, NULL); 55 | glCompileShader(vertex); 56 | checkCompileErrors(vertex, "VERTEX"); 57 | // fragment Shader 58 | fragment = glCreateShader(GL_FRAGMENT_SHADER); 59 | glShaderSource(fragment, 1, &fShaderCode, NULL); 60 | glCompileShader(fragment); 61 | checkCompileErrors(fragment, "FRAGMENT"); 62 | // shader Program 63 | ID = glCreateProgram(); 64 | glAttachShader(ID, vertex); 65 | glAttachShader(ID, fragment); 66 | glLinkProgram(ID); 67 | checkCompileErrors(ID, "PROGRAM"); 68 | // delete the shaders as they're linked into our program now and no longer necessary 69 | glDeleteShader(vertex); 70 | glDeleteShader(fragment); 71 | } 72 | // activate the shader 73 | // ------------------------------------------------------------------------ 74 | void use() 75 | { 76 | glUseProgram(ID); 77 | } 78 | // utility uniform functions 79 | // ------------------------------------------------------------------------ 80 | void setBool(const std::string &name, bool value) const 81 | { 82 | glUniform1i(glGetUniformLocation(ID, name.c_str()), (int)value); 83 | } 84 | // ------------------------------------------------------------------------ 85 | void setInt(const std::string &name, int value) const 86 | { 87 | glUniform1i(glGetUniformLocation(ID, name.c_str()), value); 88 | } 89 | // ------------------------------------------------------------------------ 90 | void setFloat(const std::string &name, float value) const 91 | { 92 | glUniform1f(glGetUniformLocation(ID, name.c_str()), value); 93 | } 94 | 95 | private: 96 | // utility function for checking shader compilation/linking errors. 97 | // ------------------------------------------------------------------------ 98 | void checkCompileErrors(unsigned int shader, std::string type) 99 | { 100 | int success; 101 | char infoLog[1024]; 102 | if (type != "PROGRAM") 103 | { 104 | glGetShaderiv(shader, GL_COMPILE_STATUS, &success); 105 | if (!success) 106 | { 107 | glGetShaderInfoLog(shader, 1024, NULL, infoLog); 108 | std::cout << "ERROR::SHADER_COMPILATION_ERROR of type: " << type << "\n" << infoLog << "\n -- --------------------------------------------------- -- " << std::endl; 109 | } 110 | } 111 | else 112 | { 113 | glGetProgramiv(shader, GL_LINK_STATUS, &success); 114 | if (!success) 115 | { 116 | glGetProgramInfoLog(shader, 1024, NULL, infoLog); 117 | std::cout << "ERROR::PROGRAM_LINKING_ERROR of type: " << type << "\n" << infoLog << "\n -- --------------------------------------------------- -- " << std::endl; 118 | } 119 | } 120 | } 121 | }; 122 | #endif 123 | -------------------------------------------------------------------------------- /src/shader/triangle_facets.fs: -------------------------------------------------------------------------------- 1 | #version 330 core 2 | out vec4 FragColor; 3 | 4 | in vec3 Normal; 5 | in vec3 FragPos; 6 | in vec3 objectColor; 7 | 8 | uniform bool if_light; 9 | uniform vec3 lightPos; 10 | uniform vec3 lightColor; 11 | 12 | void main() 13 | { 14 | // ambient 15 | float ambientStrength = 0.2; 16 | vec3 ambient = ambientStrength * lightColor; 17 | 18 | // diffuse 19 | if ( if_light ) 20 | { 21 | vec3 norm = normalize( Normal ); 22 | vec3 lightDir = normalize( lightPos - FragPos ); 23 | // float diff = max((dot(norm, lightDir)), 0.0); 24 | float diff = max( abs( dot( norm, lightDir ) ) * 0.5, 0.0 ); // Set abs to enable two-sided lighting 25 | vec3 diffuse = diff * lightColor; 26 | 27 | vec3 result = ( ambient + diffuse ) * objectColor; 28 | FragColor = vec4( result, 1.0 ); 29 | } 30 | else 31 | { 32 | FragColor = vec4( objectColor, 1.0 ); 33 | } 34 | // FragColor = vec4(vec3(1,1,1), 1.0); 35 | } -------------------------------------------------------------------------------- /src/shader/triangle_facets.vs: -------------------------------------------------------------------------------- 1 | #version 330 core 2 | layout (location = 0) in vec3 aPos; 3 | layout (location = 1) in vec3 aNormal; 4 | layout (location = 2) in float aColor; 5 | 6 | out vec3 FragPos; 7 | out vec3 Normal; 8 | out vec3 objectColor; 9 | 10 | uniform mat4 view; 11 | uniform mat4 projection; 12 | 13 | void main() 14 | { 15 | // FragPos = vec3(model * vec4(aPos, 1.0)); 16 | FragPos = aPos; 17 | Normal = aNormal; 18 | int color_int = int(aColor); // I Don't know why here need a cast 19 | objectColor = vec3( ( ( color_int >> 16 ) & 0xff ), 20 | ( ( color_int >> 8 ) & 0xff ), 21 | ( ( color_int >> 0 ) & 0xff ) ) / 255.0; 22 | gl_Position = projection * view * vec4(FragPos, 1.0); 23 | } -------------------------------------------------------------------------------- /src/tools/common_tools.h: -------------------------------------------------------------------------------- 1 | #ifndef __COMMON_TOOLS_H__ 2 | #define __COMMON_TOOLS_H__ 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include "tools_logger.hpp" 12 | #include "tools_timer.hpp" 13 | #include "tools_random.hpp" 14 | #include "tools_json.hpp" 15 | #include "os_compatible.hpp" 16 | 17 | namespace Common_tools 18 | { 19 | template 20 | static std::vector vector_2d_to_1d(std::vector> & pt_vec_vec) 21 | { 22 | std::vector pt_vec; 23 | for (unsigned int i = 0; i < pt_vec_vec.size(); i++) 24 | { 25 | pt_vec.insert(pt_vec.end(), pt_vec_vec[i].begin(), pt_vec_vec[i].end()); 26 | } 27 | return pt_vec; 28 | }; 29 | 30 | template 31 | std::vector std_set_to_vector (const std::set & input_set) 32 | { 33 | std::vector output_vector; 34 | for(auto it = input_set.begin() ; it!= input_set.end(); it++) 35 | { 36 | output_vector.push_back(*(it)); 37 | } 38 | return output_vector; 39 | }; 40 | 41 | template 42 | std::set std_vector_to_set (const std::vector & input_vector) 43 | { 44 | std::set output_set; 45 | for(auto it = input_vector.begin() ; it!= input_vector.end(); it++) 46 | { 47 | output_set.insert(*(it)); 48 | } 49 | return output_set; 50 | }; 51 | 52 | template 53 | void maintain_maximum_thread_pool(std::list & thread_pool, size_t maximum_parallel_thread) 54 | { 55 | if(thread_pool.size() >= (size_t) maximum_parallel_thread) 56 | { 57 | //printf("******** Maximum thread number reach, current size = %d, wait finish... *****\r\n", thread_pool.size()); 58 | while ( 1 ) 59 | { 60 | //printf_line; 61 | for ( auto it = thread_pool.begin(); it != thread_pool.end(); it++ ) 62 | { 63 | 64 | auto status = (*it)->wait_for(std::chrono::nanoseconds(1)); 65 | if ( status == std::future_status::ready ) 66 | { 67 | //cout << "status == std::future_status::ready" << endl; 68 | delete *it; 69 | thread_pool.erase( it ); 70 | break; 71 | } 72 | } 73 | if(thread_pool.size() < (size_t) maximum_parallel_thread) 74 | { 75 | break; 76 | } 77 | std::this_thread::sleep_for( std::chrono::nanoseconds( 1 ) ); 78 | } 79 | 80 | } 81 | 82 | }; 83 | 84 | inline bool if_file_exist( const std::string &name ) 85 | { 86 | //Copy from: https://stackoverflow.com/questions/12774207/fastest-way-to-check-if-a-file-exist-using-standard-c-c11-c 87 | struct stat buffer; 88 | return ( stat( name.c_str(), &buffer ) == 0 ); 89 | }; 90 | } 91 | #endif 92 | -------------------------------------------------------------------------------- /src/tools/imgui/imgui_impl_allegro5.h: -------------------------------------------------------------------------------- 1 | // dear imgui: Renderer + Platform Backend for Allegro 5 2 | // (Info: Allegro 5 is a cross-platform general purpose library for handling windows, inputs, graphics, etc.) 3 | 4 | // Implemented features: 5 | // [X] Renderer: User texture binding. Use 'ALLEGRO_BITMAP*' as ImTextureID. Read the FAQ about ImTextureID! 6 | // [X] Platform: Keyboard support. Since 1.87 we are using the io.AddKeyEvent() function. Pass ImGuiKey values to all key functions e.g. ImGui::IsKeyPressed(ImGuiKey_Space). [Legacy ALLEGRO_KEY_* values will also be supported unless IMGUI_DISABLE_OBSOLETE_KEYIO is set] 7 | // [X] Platform: Clipboard support (from Allegro 5.1.12) 8 | // [X] Platform: Mouse cursor shape and visibility. Disable with 'io.ConfigFlags |= ImGuiConfigFlags_NoMouseCursorChange'. 9 | // Issues: 10 | // [ ] Renderer: The renderer is suboptimal as we need to unindex our buffers and convert vertices manually. 11 | // [ ] Platform: Missing gamepad support. 12 | 13 | // You can use unmodified imgui_impl_* files in your project. See examples/ folder for examples of using this. 14 | // Prefer including the entire imgui/ repository into your project (either as a copy or as a submodule), and only build the backends you need. 15 | // If you are new to Dear ImGui, read documentation from the docs/ folder + read the top of imgui.cpp. 16 | // Read online: https://github.com/ocornut/imgui/tree/master/docs 17 | 18 | #pragma once 19 | #include "imgui.h" // IMGUI_IMPL_API 20 | 21 | struct ALLEGRO_DISPLAY; 22 | union ALLEGRO_EVENT; 23 | 24 | IMGUI_IMPL_API bool ImGui_ImplAllegro5_Init(ALLEGRO_DISPLAY* display); 25 | IMGUI_IMPL_API void ImGui_ImplAllegro5_Shutdown(); 26 | IMGUI_IMPL_API void ImGui_ImplAllegro5_NewFrame(); 27 | IMGUI_IMPL_API void ImGui_ImplAllegro5_RenderDrawData(ImDrawData* draw_data); 28 | IMGUI_IMPL_API bool ImGui_ImplAllegro5_ProcessEvent(ALLEGRO_EVENT* event); 29 | 30 | // Use if you want to reset your rendering device without losing Dear ImGui state. 31 | IMGUI_IMPL_API bool ImGui_ImplAllegro5_CreateDeviceObjects(); 32 | IMGUI_IMPL_API void ImGui_ImplAllegro5_InvalidateDeviceObjects(); 33 | -------------------------------------------------------------------------------- /src/tools/imgui/imgui_impl_android.h: -------------------------------------------------------------------------------- 1 | // dear imgui: Platform Binding for Android native app 2 | // This needs to be used along with the OpenGL 3 Renderer (imgui_impl_opengl3) 3 | 4 | // Implemented features: 5 | // [X] Platform: Keyboard support. Since 1.87 we are using the io.AddKeyEvent() function. Pass ImGuiKey values to all key functions e.g. ImGui::IsKeyPressed(ImGuiKey_Space). [Legacy AKEYCODE_* values will also be supported unless IMGUI_DISABLE_OBSOLETE_KEYIO is set] 6 | // Missing features: 7 | // [ ] Platform: Clipboard support. 8 | // [ ] Platform: Gamepad support. Enable with 'io.ConfigFlags |= ImGuiConfigFlags_NavEnableGamepad'. 9 | // [ ] Platform: Mouse cursor shape and visibility. Disable with 'io.ConfigFlags |= ImGuiConfigFlags_NoMouseCursorChange'. FIXME: Check if this is even possible with Android. 10 | // Important: 11 | // - Consider using SDL or GLFW backend on Android, which will be more full-featured than this. 12 | // - FIXME: On-screen keyboard currently needs to be enabled by the application (see examples/ and issue #3446) 13 | // - FIXME: Unicode character inputs needs to be passed by Dear ImGui by the application (see examples/ and issue #3446) 14 | 15 | // You can use unmodified imgui_impl_* files in your project. See examples/ folder for examples of using this. 16 | // Prefer including the entire imgui/ repository into your project (either as a copy or as a submodule), and only build the backends you need. 17 | // If you are new to Dear ImGui, read documentation from the docs/ folder + read the top of imgui.cpp. 18 | // Read online: https://github.com/ocornut/imgui/tree/master/docs 19 | 20 | #pragma once 21 | 22 | struct ANativeWindow; 23 | struct AInputEvent; 24 | 25 | IMGUI_IMPL_API bool ImGui_ImplAndroid_Init(ANativeWindow* window); 26 | IMGUI_IMPL_API int32_t ImGui_ImplAndroid_HandleInputEvent(AInputEvent* input_event); 27 | IMGUI_IMPL_API void ImGui_ImplAndroid_Shutdown(); 28 | IMGUI_IMPL_API void ImGui_ImplAndroid_NewFrame(); 29 | -------------------------------------------------------------------------------- /src/tools/imgui/imgui_impl_dx10.h: -------------------------------------------------------------------------------- 1 | // dear imgui: Renderer Backend for DirectX10 2 | // This needs to be used along with a Platform Backend (e.g. Win32) 3 | 4 | // Implemented features: 5 | // [X] Renderer: User texture binding. Use 'ID3D10ShaderResourceView*' as ImTextureID. Read the FAQ about ImTextureID! 6 | // [X] Renderer: Multi-viewport support (multiple windows). Enable with 'io.ConfigFlags |= ImGuiConfigFlags_ViewportsEnable'. 7 | // [X] Renderer: Large meshes support (64k+ vertices) with 16-bit indices. 8 | 9 | // You can use unmodified imgui_impl_* files in your project. See examples/ folder for examples of using this. 10 | // Prefer including the entire imgui/ repository into your project (either as a copy or as a submodule), and only build the backends you need. 11 | // If you are new to Dear ImGui, read documentation from the docs/ folder + read the top of imgui.cpp. 12 | // Read online: https://github.com/ocornut/imgui/tree/master/docs 13 | 14 | #pragma once 15 | #include "imgui.h" // IMGUI_IMPL_API 16 | 17 | struct ID3D10Device; 18 | 19 | IMGUI_IMPL_API bool ImGui_ImplDX10_Init(ID3D10Device* device); 20 | IMGUI_IMPL_API void ImGui_ImplDX10_Shutdown(); 21 | IMGUI_IMPL_API void ImGui_ImplDX10_NewFrame(); 22 | IMGUI_IMPL_API void ImGui_ImplDX10_RenderDrawData(ImDrawData* draw_data); 23 | 24 | // Use if you want to reset your rendering device without losing Dear ImGui state. 25 | IMGUI_IMPL_API void ImGui_ImplDX10_InvalidateDeviceObjects(); 26 | IMGUI_IMPL_API bool ImGui_ImplDX10_CreateDeviceObjects(); 27 | -------------------------------------------------------------------------------- /src/tools/imgui/imgui_impl_dx11.h: -------------------------------------------------------------------------------- 1 | // dear imgui: Renderer Backend for DirectX11 2 | // This needs to be used along with a Platform Backend (e.g. Win32) 3 | 4 | // Implemented features: 5 | // [X] Renderer: User texture binding. Use 'ID3D11ShaderResourceView*' as ImTextureID. Read the FAQ about ImTextureID! 6 | // [X] Renderer: Multi-viewport support (multiple windows). Enable with 'io.ConfigFlags |= ImGuiConfigFlags_ViewportsEnable'. 7 | // [X] Renderer: Large meshes support (64k+ vertices) with 16-bit indices. 8 | 9 | // You can use unmodified imgui_impl_* files in your project. See examples/ folder for examples of using this. 10 | // Prefer including the entire imgui/ repository into your project (either as a copy or as a submodule), and only build the backends you need. 11 | // If you are new to Dear ImGui, read documentation from the docs/ folder + read the top of imgui.cpp. 12 | // Read online: https://github.com/ocornut/imgui/tree/master/docs 13 | 14 | #pragma once 15 | #include "imgui.h" // IMGUI_IMPL_API 16 | 17 | struct ID3D11Device; 18 | struct ID3D11DeviceContext; 19 | 20 | IMGUI_IMPL_API bool ImGui_ImplDX11_Init(ID3D11Device* device, ID3D11DeviceContext* device_context); 21 | IMGUI_IMPL_API void ImGui_ImplDX11_Shutdown(); 22 | IMGUI_IMPL_API void ImGui_ImplDX11_NewFrame(); 23 | IMGUI_IMPL_API void ImGui_ImplDX11_RenderDrawData(ImDrawData* draw_data); 24 | 25 | // Use if you want to reset your rendering device without losing Dear ImGui state. 26 | IMGUI_IMPL_API void ImGui_ImplDX11_InvalidateDeviceObjects(); 27 | IMGUI_IMPL_API bool ImGui_ImplDX11_CreateDeviceObjects(); 28 | -------------------------------------------------------------------------------- /src/tools/imgui/imgui_impl_dx12.h: -------------------------------------------------------------------------------- 1 | // dear imgui: Renderer Backend for DirectX12 2 | // This needs to be used along with a Platform Backend (e.g. Win32) 3 | 4 | // Implemented features: 5 | // [X] Renderer: User texture binding. Use 'D3D12_GPU_DESCRIPTOR_HANDLE' as ImTextureID. Read the FAQ about ImTextureID! 6 | // [X] Renderer: Multi-viewport support (multiple windows). Enable with 'io.ConfigFlags |= ImGuiConfigFlags_ViewportsEnable'. 7 | // [X] Renderer: Large meshes support (64k+ vertices) with 16-bit indices. 8 | 9 | // Important: to compile on 32-bit systems, this backend requires code to be compiled with '#define ImTextureID ImU64'. 10 | // See imgui_impl_dx12.cpp file for details. 11 | 12 | // You can use unmodified imgui_impl_* files in your project. See examples/ folder for examples of using this. 13 | // Prefer including the entire imgui/ repository into your project (either as a copy or as a submodule), and only build the backends you need. 14 | // If you are new to Dear ImGui, read documentation from the docs/ folder + read the top of imgui.cpp. 15 | // Read online: https://github.com/ocornut/imgui/tree/master/docs 16 | 17 | #pragma once 18 | #include "imgui.h" // IMGUI_IMPL_API 19 | #include // DXGI_FORMAT 20 | 21 | struct ID3D12Device; 22 | struct ID3D12DescriptorHeap; 23 | struct ID3D12GraphicsCommandList; 24 | struct D3D12_CPU_DESCRIPTOR_HANDLE; 25 | struct D3D12_GPU_DESCRIPTOR_HANDLE; 26 | 27 | // cmd_list is the command list that the implementation will use to render imgui draw lists. 28 | // Before calling the render function, caller must prepare cmd_list by resetting it and setting the appropriate 29 | // render target and descriptor heap that contains font_srv_cpu_desc_handle/font_srv_gpu_desc_handle. 30 | // font_srv_cpu_desc_handle and font_srv_gpu_desc_handle are handles to a single SRV descriptor to use for the internal font texture. 31 | IMGUI_IMPL_API bool ImGui_ImplDX12_Init(ID3D12Device* device, int num_frames_in_flight, DXGI_FORMAT rtv_format, ID3D12DescriptorHeap* cbv_srv_heap, 32 | D3D12_CPU_DESCRIPTOR_HANDLE font_srv_cpu_desc_handle, D3D12_GPU_DESCRIPTOR_HANDLE font_srv_gpu_desc_handle); 33 | IMGUI_IMPL_API void ImGui_ImplDX12_Shutdown(); 34 | IMGUI_IMPL_API void ImGui_ImplDX12_NewFrame(); 35 | IMGUI_IMPL_API void ImGui_ImplDX12_RenderDrawData(ImDrawData* draw_data, ID3D12GraphicsCommandList* graphics_command_list); 36 | 37 | // Use if you want to reset your rendering device without losing Dear ImGui state. 38 | IMGUI_IMPL_API void ImGui_ImplDX12_InvalidateDeviceObjects(); 39 | IMGUI_IMPL_API bool ImGui_ImplDX12_CreateDeviceObjects(); 40 | -------------------------------------------------------------------------------- /src/tools/imgui/imgui_impl_dx9.h: -------------------------------------------------------------------------------- 1 | // dear imgui: Renderer Backend for DirectX9 2 | // This needs to be used along with a Platform Backend (e.g. Win32) 3 | 4 | // Implemented features: 5 | // [X] Renderer: User texture binding. Use 'LPDIRECT3DTEXTURE9' as ImTextureID. Read the FAQ about ImTextureID! 6 | // [X] Renderer: Multi-viewport support (multiple windows). Enable with 'io.ConfigFlags |= ImGuiConfigFlags_ViewportsEnable'. 7 | // [X] Renderer: Large meshes support (64k+ vertices) with 16-bit indices. 8 | 9 | // You can use unmodified imgui_impl_* files in your project. See examples/ folder for examples of using this. 10 | // Prefer including the entire imgui/ repository into your project (either as a copy or as a submodule), and only build the backends you need. 11 | // If you are new to Dear ImGui, read documentation from the docs/ folder + read the top of imgui.cpp. 12 | // Read online: https://github.com/ocornut/imgui/tree/master/docs 13 | 14 | #pragma once 15 | #include "imgui.h" // IMGUI_IMPL_API 16 | 17 | struct IDirect3DDevice9; 18 | 19 | IMGUI_IMPL_API bool ImGui_ImplDX9_Init(IDirect3DDevice9* device); 20 | IMGUI_IMPL_API void ImGui_ImplDX9_Shutdown(); 21 | IMGUI_IMPL_API void ImGui_ImplDX9_NewFrame(); 22 | IMGUI_IMPL_API void ImGui_ImplDX9_RenderDrawData(ImDrawData* draw_data); 23 | 24 | // Use if you want to reset your rendering device without losing Dear ImGui state. 25 | IMGUI_IMPL_API bool ImGui_ImplDX9_CreateDeviceObjects(); 26 | IMGUI_IMPL_API void ImGui_ImplDX9_InvalidateDeviceObjects(); 27 | -------------------------------------------------------------------------------- /src/tools/imgui/imgui_impl_glfw.h: -------------------------------------------------------------------------------- 1 | // dear imgui: Platform Backend for GLFW 2 | // This needs to be used along with a Renderer (e.g. OpenGL3, Vulkan, WebGPU..) 3 | // (Info: GLFW is a cross-platform general purpose library for handling windows, inputs, OpenGL/Vulkan graphics context creation, etc.) 4 | // (Requires: GLFW 3.1+. Prefer GLFW 3.3+ for full feature support.) 5 | 6 | // Implemented features: 7 | // [X] Platform: Clipboard support. 8 | // [X] Platform: Keyboard support. Since 1.87 we are using the io.AddKeyEvent() function. Pass ImGuiKey values to all key functions e.g. ImGui::IsKeyPressed(ImGuiKey_Space). [Legacy GLFW_KEY_* values will also be supported unless IMGUI_DISABLE_OBSOLETE_KEYIO is set] 9 | // [X] Platform: Gamepad support. Enable with 'io.ConfigFlags |= ImGuiConfigFlags_NavEnableGamepad'. 10 | // [x] Platform: Mouse cursor shape and visibility. Disable with 'io.ConfigFlags |= ImGuiConfigFlags_NoMouseCursorChange' (note: the resizing cursors requires GLFW 3.4+). 11 | // [X] Platform: Multi-viewport support (multiple windows). Enable with 'io.ConfigFlags |= ImGuiConfigFlags_ViewportsEnable'. 12 | 13 | // Issues: 14 | // [ ] Platform: Multi-viewport support: ParentViewportID not honored, and so io.ConfigViewportsNoDefaultParent has no effect (minor). 15 | 16 | // You can use unmodified imgui_impl_* files in your project. See examples/ folder for examples of using this. 17 | // Prefer including the entire imgui/ repository into your project (either as a copy or as a submodule), and only build the backends you need. 18 | // If you are new to Dear ImGui, read documentation from the docs/ folder + read the top of imgui.cpp. 19 | // Read online: https://github.com/ocornut/imgui/tree/master/docs 20 | 21 | // About GLSL version: 22 | // The 'glsl_version' initialization parameter defaults to "#version 150" if NULL. 23 | // Only override if your GL version doesn't handle this GLSL version. Keep NULL if unsure! 24 | 25 | #pragma once 26 | #include "imgui.h" // IMGUI_IMPL_API 27 | 28 | struct GLFWwindow; 29 | struct GLFWmonitor; 30 | 31 | IMGUI_IMPL_API bool ImGui_ImplGlfw_InitForOpenGL(GLFWwindow* window, bool install_callbacks); 32 | IMGUI_IMPL_API bool ImGui_ImplGlfw_InitForVulkan(GLFWwindow* window, bool install_callbacks); 33 | IMGUI_IMPL_API bool ImGui_ImplGlfw_InitForOther(GLFWwindow* window, bool install_callbacks); 34 | IMGUI_IMPL_API void ImGui_ImplGlfw_Shutdown(); 35 | IMGUI_IMPL_API void ImGui_ImplGlfw_NewFrame(); 36 | 37 | // GLFW callbacks (installer) 38 | // - When calling Init with 'install_callbacks=true': ImGui_ImplGlfw_InstallCallbacks() is called. GLFW callbacks will be installed for you. They will chain-call user's previously installed callbacks, if any. 39 | // - When calling Init with 'install_callbacks=false': GLFW callbacks won't be installed. You will need to call individual function yourself from your own GLFW callbacks. 40 | IMGUI_IMPL_API void ImGui_ImplGlfw_InstallCallbacks(GLFWwindow* window); 41 | IMGUI_IMPL_API void ImGui_ImplGlfw_RestoreCallbacks(GLFWwindow* window); 42 | 43 | // GLFW callbacks (individual callbacks to call if you didn't install callbacks) 44 | IMGUI_IMPL_API void ImGui_ImplGlfw_WindowFocusCallback(GLFWwindow* window, int focused); // Since 1.84 45 | IMGUI_IMPL_API void ImGui_ImplGlfw_CursorEnterCallback(GLFWwindow* window, int entered); // Since 1.84 46 | IMGUI_IMPL_API void ImGui_ImplGlfw_CursorPosCallback(GLFWwindow* window, double x, double y); // Since 1.87 47 | IMGUI_IMPL_API void ImGui_ImplGlfw_MouseButtonCallback(GLFWwindow* window, int button, int action, int mods); 48 | IMGUI_IMPL_API void ImGui_ImplGlfw_ScrollCallback(GLFWwindow* window, double xoffset, double yoffset); 49 | IMGUI_IMPL_API void ImGui_ImplGlfw_KeyCallback(GLFWwindow* window, int key, int scancode, int action, int mods); 50 | IMGUI_IMPL_API void ImGui_ImplGlfw_CharCallback(GLFWwindow* window, unsigned int c); 51 | IMGUI_IMPL_API void ImGui_ImplGlfw_MonitorCallback(GLFWmonitor* monitor, int event); 52 | -------------------------------------------------------------------------------- /src/tools/imgui/imgui_impl_glut.h: -------------------------------------------------------------------------------- 1 | // dear imgui: Platform Backend for GLUT/FreeGLUT 2 | // This needs to be used along with a Renderer (e.g. OpenGL2) 3 | 4 | // !!! GLUT/FreeGLUT IS OBSOLETE PREHISTORIC SOFTWARE. Using GLUT is not recommended unless you really miss the 90's. !!! 5 | // !!! If someone or something is teaching you GLUT today, you are being abused. Please show some resistance. !!! 6 | // !!! Nowadays, prefer using GLFW or SDL instead! 7 | 8 | // Implemented features: 9 | // [X] Platform: Partial keyboard support. Since 1.87 we are using the io.AddKeyEvent() function. Pass ImGuiKey values to all key functions e.g. ImGui::IsKeyPressed(ImGuiKey_Space). [Legacy GLUT values will also be supported unless IMGUI_DISABLE_OBSOLETE_KEYIO is set] 10 | // Issues: 11 | // [ ] Platform: GLUT is unable to distinguish e.g. Backspace from CTRL+H or TAB from CTRL+I 12 | // [ ] Platform: Missing mouse cursor shape/visibility support. 13 | // [ ] Platform: Missing clipboard support (not supported by Glut). 14 | // [ ] Platform: Missing gamepad support. 15 | 16 | // You can use unmodified imgui_impl_* files in your project. See examples/ folder for examples of using this. 17 | // Prefer including the entire imgui/ repository into your project (either as a copy or as a submodule), and only build the backends you need. 18 | // If you are new to Dear ImGui, read documentation from the docs/ folder + read the top of imgui.cpp. 19 | // Read online: https://github.com/ocornut/imgui/tree/master/docs 20 | 21 | #pragma once 22 | #include "imgui.h" // IMGUI_IMPL_API 23 | 24 | IMGUI_IMPL_API bool ImGui_ImplGLUT_Init(); 25 | IMGUI_IMPL_API void ImGui_ImplGLUT_InstallFuncs(); 26 | IMGUI_IMPL_API void ImGui_ImplGLUT_Shutdown(); 27 | IMGUI_IMPL_API void ImGui_ImplGLUT_NewFrame(); 28 | 29 | // You can call ImGui_ImplGLUT_InstallFuncs() to get all those functions installed automatically, 30 | // or call them yourself from your own GLUT handlers. We are using the same weird names as GLUT for consistency.. 31 | //---------------------------------------- GLUT name --------------------------------------------- Decent Name --------- 32 | IMGUI_IMPL_API void ImGui_ImplGLUT_ReshapeFunc(int w, int h); // ~ ResizeFunc 33 | IMGUI_IMPL_API void ImGui_ImplGLUT_MotionFunc(int x, int y); // ~ MouseMoveFunc 34 | IMGUI_IMPL_API void ImGui_ImplGLUT_MouseFunc(int button, int state, int x, int y); // ~ MouseButtonFunc 35 | IMGUI_IMPL_API void ImGui_ImplGLUT_MouseWheelFunc(int button, int dir, int x, int y); // ~ MouseWheelFunc 36 | IMGUI_IMPL_API void ImGui_ImplGLUT_KeyboardFunc(unsigned char c, int x, int y); // ~ CharPressedFunc 37 | IMGUI_IMPL_API void ImGui_ImplGLUT_KeyboardUpFunc(unsigned char c, int x, int y); // ~ CharReleasedFunc 38 | IMGUI_IMPL_API void ImGui_ImplGLUT_SpecialFunc(int key, int x, int y); // ~ KeyPressedFunc 39 | IMGUI_IMPL_API void ImGui_ImplGLUT_SpecialUpFunc(int key, int x, int y); // ~ KeyReleasedFunc 40 | -------------------------------------------------------------------------------- /src/tools/imgui/imgui_impl_metal.h: -------------------------------------------------------------------------------- 1 | // dear imgui: Renderer Backend for Metal 2 | // This needs to be used along with a Platform Backend (e.g. OSX) 3 | 4 | // Implemented features: 5 | // [X] Renderer: User texture binding. Use 'MTLTexture' as ImTextureID. Read the FAQ about ImTextureID! 6 | // [X] Renderer: Large meshes support (64k+ vertices) with 16-bit indices. 7 | // [X] Renderer: Multi-viewport support (multiple windows). Enable with 'io.ConfigFlags |= ImGuiConfigFlags_ViewportsEnable'. 8 | 9 | // You can use unmodified imgui_impl_* files in your project. See examples/ folder for examples of using this. 10 | // Prefer including the entire imgui/ repository into your project (either as a copy or as a submodule), and only build the backends you need. 11 | // If you are new to Dear ImGui, read documentation from the docs/ folder + read the top of imgui.cpp. 12 | // Read online: https://github.com/ocornut/imgui/tree/master/docs 13 | 14 | #include "imgui.h" // IMGUI_IMPL_API 15 | 16 | //----------------------------------------------------------------------------- 17 | // ObjC API 18 | //----------------------------------------------------------------------------- 19 | 20 | #ifdef __OBJC__ 21 | 22 | @class MTLRenderPassDescriptor; 23 | @protocol MTLDevice, MTLCommandBuffer, MTLRenderCommandEncoder; 24 | 25 | IMGUI_IMPL_API bool ImGui_ImplMetal_Init(id device); 26 | IMGUI_IMPL_API void ImGui_ImplMetal_Shutdown(); 27 | IMGUI_IMPL_API void ImGui_ImplMetal_NewFrame(MTLRenderPassDescriptor* renderPassDescriptor); 28 | IMGUI_IMPL_API void ImGui_ImplMetal_RenderDrawData(ImDrawData* drawData, 29 | id commandBuffer, 30 | id commandEncoder); 31 | 32 | // Called by Init/NewFrame/Shutdown 33 | IMGUI_IMPL_API bool ImGui_ImplMetal_CreateFontsTexture(id device); 34 | IMGUI_IMPL_API void ImGui_ImplMetal_DestroyFontsTexture(); 35 | IMGUI_IMPL_API bool ImGui_ImplMetal_CreateDeviceObjects(id device); 36 | IMGUI_IMPL_API void ImGui_ImplMetal_DestroyDeviceObjects(); 37 | 38 | #endif 39 | 40 | //----------------------------------------------------------------------------- 41 | // C++ API 42 | //----------------------------------------------------------------------------- 43 | 44 | // Enable Metal C++ binding support with '#define IMGUI_IMPL_METAL_CPP' in your imconfig.h file 45 | // More info about using Metal from C++: https://developer.apple.com/metal/cpp/ 46 | 47 | #ifdef IMGUI_IMPL_METAL_CPP 48 | #include 49 | #ifndef __OBJC__ 50 | 51 | IMGUI_IMPL_API bool ImGui_ImplMetal_Init(MTL::Device* device); 52 | IMGUI_IMPL_API void ImGui_ImplMetal_Shutdown(); 53 | IMGUI_IMPL_API void ImGui_ImplMetal_NewFrame(MTL::RenderPassDescriptor* renderPassDescriptor); 54 | IMGUI_IMPL_API void ImGui_ImplMetal_RenderDrawData(ImDrawData* draw_data, 55 | MTL::CommandBuffer* commandBuffer, 56 | MTL::RenderCommandEncoder* commandEncoder); 57 | 58 | // Called by Init/NewFrame/Shutdown 59 | IMGUI_IMPL_API bool ImGui_ImplMetal_CreateFontsTexture(MTL::Device* device); 60 | IMGUI_IMPL_API void ImGui_ImplMetal_DestroyFontsTexture(); 61 | IMGUI_IMPL_API bool ImGui_ImplMetal_CreateDeviceObjects(MTL::Device* device); 62 | IMGUI_IMPL_API void ImGui_ImplMetal_DestroyDeviceObjects(); 63 | 64 | #endif 65 | #endif 66 | -------------------------------------------------------------------------------- /src/tools/imgui/imgui_impl_opengl2.h: -------------------------------------------------------------------------------- 1 | // dear imgui: Renderer Backend for OpenGL2 (legacy OpenGL, fixed pipeline) 2 | // This needs to be used along with a Platform Backend (e.g. GLFW, SDL, Win32, custom..) 3 | 4 | // Implemented features: 5 | // [X] Renderer: User texture binding. Use 'GLuint' OpenGL texture identifier as void*/ImTextureID. Read the FAQ about ImTextureID! 6 | // [X] Renderer: Multi-viewport support (multiple windows). Enable with 'io.ConfigFlags |= ImGuiConfigFlags_ViewportsEnable'. 7 | 8 | // You can use unmodified imgui_impl_* files in your project. See examples/ folder for examples of using this. 9 | // Prefer including the entire imgui/ repository into your project (either as a copy or as a submodule), and only build the backends you need. 10 | // If you are new to Dear ImGui, read documentation from the docs/ folder + read the top of imgui.cpp. 11 | // Read online: https://github.com/ocornut/imgui/tree/master/docs 12 | 13 | // **DO NOT USE THIS CODE IF YOUR CODE/ENGINE IS USING MODERN OPENGL (SHADERS, VBO, VAO, etc.)** 14 | // **Prefer using the code in imgui_impl_opengl3.cpp** 15 | // This code is mostly provided as a reference to learn how ImGui integration works, because it is shorter to read. 16 | // If your code is using GL3+ context or any semi modern OpenGL calls, using this is likely to make everything more 17 | // complicated, will require your code to reset every single OpenGL attributes to their initial state, and might 18 | // confuse your GPU driver. 19 | // The GL2 code is unable to reset attributes or even call e.g. "glUseProgram(0)" because they don't exist in that API. 20 | 21 | #pragma once 22 | #include "imgui.h" // IMGUI_IMPL_API 23 | 24 | IMGUI_IMPL_API bool ImGui_ImplOpenGL2_Init(); 25 | IMGUI_IMPL_API void ImGui_ImplOpenGL2_Shutdown(); 26 | IMGUI_IMPL_API void ImGui_ImplOpenGL2_NewFrame(); 27 | IMGUI_IMPL_API void ImGui_ImplOpenGL2_RenderDrawData(ImDrawData* draw_data); 28 | 29 | // Called by Init/NewFrame/Shutdown 30 | IMGUI_IMPL_API bool ImGui_ImplOpenGL2_CreateFontsTexture(); 31 | IMGUI_IMPL_API void ImGui_ImplOpenGL2_DestroyFontsTexture(); 32 | IMGUI_IMPL_API bool ImGui_ImplOpenGL2_CreateDeviceObjects(); 33 | IMGUI_IMPL_API void ImGui_ImplOpenGL2_DestroyDeviceObjects(); 34 | -------------------------------------------------------------------------------- /src/tools/imgui/imgui_impl_opengl3.h: -------------------------------------------------------------------------------- 1 | // dear imgui: Renderer Backend for modern OpenGL with shaders / programmatic pipeline 2 | // - Desktop GL: 2.x 3.x 4.x 3 | // - Embedded GL: ES 2.0 (WebGL 1.0), ES 3.0 (WebGL 2.0) 4 | // This needs to be used along with a Platform Backend (e.g. GLFW, SDL, Win32, custom..) 5 | 6 | // Implemented features: 7 | // [X] Renderer: User texture binding. Use 'GLuint' OpenGL texture identifier as void*/ImTextureID. Read the FAQ about ImTextureID! 8 | // [X] Renderer: Multi-viewport support (multiple windows). Enable with 'io.ConfigFlags |= ImGuiConfigFlags_ViewportsEnable'. 9 | // [x] Renderer: Large meshes support (64k+ vertices) with 16-bit indices (Desktop OpenGL only). 10 | 11 | // You can use unmodified imgui_impl_* files in your project. See examples/ folder for examples of using this. 12 | // Prefer including the entire imgui/ repository into your project (either as a copy or as a submodule), and only build the backends you need. 13 | // If you are new to Dear ImGui, read documentation from the docs/ folder + read the top of imgui.cpp. 14 | // Read online: https://github.com/ocornut/imgui/tree/master/docs 15 | 16 | // About GLSL version: 17 | // The 'glsl_version' initialization parameter should be NULL (default) or a "#version XXX" string. 18 | // On computer platform the GLSL version default to "#version 130". On OpenGL ES 3 platform it defaults to "#version 300 es" 19 | // Only override if your GL version doesn't handle this GLSL version. See GLSL version table at the top of imgui_impl_opengl3.cpp. 20 | 21 | #pragma once 22 | #include "imgui.h" // IMGUI_IMPL_API 23 | 24 | // Backend API 25 | IMGUI_IMPL_API bool ImGui_ImplOpenGL3_Init(const char* glsl_version = NULL); 26 | IMGUI_IMPL_API void ImGui_ImplOpenGL3_Shutdown(); 27 | IMGUI_IMPL_API void ImGui_ImplOpenGL3_NewFrame(); 28 | IMGUI_IMPL_API void ImGui_ImplOpenGL3_RenderDrawData(ImDrawData* draw_data); 29 | 30 | // (Optional) Called by Init/NewFrame/Shutdown 31 | IMGUI_IMPL_API bool ImGui_ImplOpenGL3_CreateFontsTexture(); 32 | IMGUI_IMPL_API void ImGui_ImplOpenGL3_DestroyFontsTexture(); 33 | IMGUI_IMPL_API bool ImGui_ImplOpenGL3_CreateDeviceObjects(); 34 | IMGUI_IMPL_API void ImGui_ImplOpenGL3_DestroyDeviceObjects(); 35 | 36 | // Specific OpenGL ES versions 37 | //#define IMGUI_IMPL_OPENGL_ES2 // Auto-detected on Emscripten 38 | //#define IMGUI_IMPL_OPENGL_ES3 // Auto-detected on iOS/Android 39 | 40 | // You can explicitly select GLES2 or GLES3 API by using one of the '#define IMGUI_IMPL_OPENGL_LOADER_XXX' in imconfig.h or compiler command-line. 41 | #if !defined(IMGUI_IMPL_OPENGL_ES2) \ 42 | && !defined(IMGUI_IMPL_OPENGL_ES3) 43 | 44 | // Try to detect GLES on matching platforms 45 | #if defined(__APPLE__) 46 | #include 47 | #endif 48 | #if (defined(__APPLE__) && (TARGET_OS_IOS || TARGET_OS_TV)) || (defined(__ANDROID__)) 49 | #define IMGUI_IMPL_OPENGL_ES3 // iOS, Android -> GL ES 3, "#version 300 es" 50 | #elif defined(__EMSCRIPTEN__) || defined(__amigaos4__) 51 | #define IMGUI_IMPL_OPENGL_ES2 // Emscripten -> GL ES 2, "#version 100" 52 | #else 53 | // Otherwise imgui_impl_opengl3_loader.h will be used. 54 | #endif 55 | 56 | #endif 57 | -------------------------------------------------------------------------------- /src/tools/imgui/imgui_impl_osx.h: -------------------------------------------------------------------------------- 1 | // dear imgui: Platform Backend for OSX / Cocoa 2 | // This needs to be used along with a Renderer (e.g. OpenGL2, OpenGL3, Vulkan, Metal..) 3 | // [ALPHA] Early backend, not well tested. If you want a portable application, prefer using the GLFW or SDL platform Backends on Mac. 4 | 5 | // Implemented features: 6 | // [X] Platform: Mouse cursor shape and visibility. Disable with 'io.ConfigFlags |= ImGuiConfigFlags_NoMouseCursorChange'. 7 | // [X] Platform: Keyboard support. Since 1.87 we are using the io.AddKeyEvent() function. Pass ImGuiKey values to all key functions e.g. ImGui::IsKeyPressed(ImGuiKey_Space). [Legacy kVK_* values will also be supported unless IMGUI_DISABLE_OBSOLETE_KEYIO is set] 8 | // [X] Platform: OSX clipboard is supported within core Dear ImGui (no specific code in this backend). 9 | // [X] Platform: Gamepad support. Enabled with 'io.ConfigFlags |= ImGuiConfigFlags_NavEnableGamepad'. 10 | // [X] Platform: IME support. 11 | // [X] Platform: Multi-viewport / platform windows. 12 | 13 | // You can use unmodified imgui_impl_* files in your project. See examples/ folder for examples of using this. 14 | // Prefer including the entire imgui/ repository into your project (either as a copy or as a submodule), and only build the backends you need. 15 | // If you are new to Dear ImGui, read documentation from the docs/ folder + read the top of imgui.cpp. 16 | // Read online: https://github.com/ocornut/imgui/tree/master/docs 17 | 18 | #include "imgui.h" // IMGUI_IMPL_API 19 | 20 | #ifdef __OBJC__ 21 | 22 | @class NSEvent; 23 | @class NSView; 24 | 25 | IMGUI_IMPL_API bool ImGui_ImplOSX_Init(NSView* _Nonnull view); 26 | IMGUI_IMPL_API void ImGui_ImplOSX_Shutdown(); 27 | IMGUI_IMPL_API void ImGui_ImplOSX_NewFrame(NSView* _Nullable view); 28 | 29 | #endif 30 | 31 | //----------------------------------------------------------------------------- 32 | // C++ API 33 | //----------------------------------------------------------------------------- 34 | 35 | #ifdef IMGUI_IMPL_METAL_CPP_EXTENSIONS 36 | // #include 37 | #ifndef __OBJC__ 38 | 39 | IMGUI_IMPL_API bool ImGui_ImplOSX_Init(void* _Nonnull view); 40 | IMGUI_IMPL_API void ImGui_ImplOSX_Shutdown(); 41 | IMGUI_IMPL_API void ImGui_ImplOSX_NewFrame(void* _Nullable view); 42 | 43 | #endif 44 | #endif 45 | -------------------------------------------------------------------------------- /src/tools/imgui/imgui_impl_sdl.h: -------------------------------------------------------------------------------- 1 | // dear imgui: Platform Backend for SDL2 2 | // This needs to be used along with a Renderer (e.g. DirectX11, OpenGL3, Vulkan..) 3 | // (Info: SDL2 is a cross-platform general purpose library for handling windows, inputs, graphics context creation, etc.) 4 | 5 | // Implemented features: 6 | // [X] Platform: Clipboard support. 7 | // [X] Platform: Keyboard support. Since 1.87 we are using the io.AddKeyEvent() function. Pass ImGuiKey values to all key functions e.g. ImGui::IsKeyPressed(ImGuiKey_Space). [Legacy SDL_SCANCODE_* values will also be supported unless IMGUI_DISABLE_OBSOLETE_KEYIO is set] 8 | // [X] Platform: Gamepad support. Enabled with 'io.ConfigFlags |= ImGuiConfigFlags_NavEnableGamepad'. 9 | // [X] Platform: Mouse cursor shape and visibility. Disable with 'io.ConfigFlags |= ImGuiConfigFlags_NoMouseCursorChange'. 10 | // [X] Platform: Multi-viewport support (multiple windows). Enable with 'io.ConfigFlags |= ImGuiConfigFlags_ViewportsEnable'. 11 | // Missing features: 12 | // [ ] Platform: SDL2 handling of IME under Windows appears to be broken and it explicitly disable the regular Windows IME. You can restore Windows IME by compiling SDL with SDL_DISABLE_WINDOWS_IME. 13 | // [ ] Platform: Multi-viewport + Minimized windows seems to break mouse wheel events (at least under Windows). 14 | 15 | // You can use unmodified imgui_impl_* files in your project. See examples/ folder for examples of using this. 16 | // Prefer including the entire imgui/ repository into your project (either as a copy or as a submodule), and only build the backends you need. 17 | // If you are new to Dear ImGui, read documentation from the docs/ folder + read the top of imgui.cpp. 18 | // Read online: https://github.com/ocornut/imgui/tree/master/docs 19 | 20 | #pragma once 21 | #include "imgui.h" // IMGUI_IMPL_API 22 | 23 | struct SDL_Window; 24 | struct SDL_Renderer; 25 | typedef union SDL_Event SDL_Event; 26 | 27 | IMGUI_IMPL_API bool ImGui_ImplSDL2_InitForOpenGL(SDL_Window* window, void* sdl_gl_context); 28 | IMGUI_IMPL_API bool ImGui_ImplSDL2_InitForVulkan(SDL_Window* window); 29 | IMGUI_IMPL_API bool ImGui_ImplSDL2_InitForD3D(SDL_Window* window); 30 | IMGUI_IMPL_API bool ImGui_ImplSDL2_InitForMetal(SDL_Window* window); 31 | IMGUI_IMPL_API bool ImGui_ImplSDL2_InitForSDLRenderer(SDL_Window* window, SDL_Renderer* renderer); 32 | IMGUI_IMPL_API void ImGui_ImplSDL2_Shutdown(); 33 | IMGUI_IMPL_API void ImGui_ImplSDL2_NewFrame(); 34 | IMGUI_IMPL_API bool ImGui_ImplSDL2_ProcessEvent(const SDL_Event* event); 35 | 36 | #ifndef IMGUI_DISABLE_OBSOLETE_FUNCTIONS 37 | static inline void ImGui_ImplSDL2_NewFrame(SDL_Window*) { ImGui_ImplSDL2_NewFrame(); } // 1.84: removed unnecessary parameter 38 | #endif 39 | -------------------------------------------------------------------------------- /src/tools/imgui/imgui_impl_sdlrenderer.h: -------------------------------------------------------------------------------- 1 | // dear imgui: Renderer Backend for SDL_Renderer 2 | // (Requires: SDL 2.0.17+) 3 | 4 | // Important to understand: SDL_Renderer is an _optional_ component of SDL. 5 | // For a multi-platform app consider using e.g. SDL+DirectX on Windows and SDL+OpenGL on Linux/OSX. 6 | // If your application will want to render any non trivial amount of graphics other than UI, 7 | // please be aware that SDL_Renderer offers a limited graphic API to the end-user and it might 8 | // be difficult to step out of those boundaries. 9 | // However, we understand it is a convenient choice to get an app started easily. 10 | 11 | // Implemented features: 12 | // [X] Renderer: User texture binding. Use 'SDL_Texture*' as ImTextureID. Read the FAQ about ImTextureID! 13 | // [X] Renderer: Large meshes support (64k+ vertices) with 16-bit indices. 14 | // Missing features: 15 | // [ ] Renderer: Multi-viewport support (multiple windows). 16 | 17 | #pragma once 18 | #include "imgui.h" // IMGUI_IMPL_API 19 | 20 | struct SDL_Renderer; 21 | 22 | IMGUI_IMPL_API bool ImGui_ImplSDLRenderer_Init(SDL_Renderer* renderer); 23 | IMGUI_IMPL_API void ImGui_ImplSDLRenderer_Shutdown(); 24 | IMGUI_IMPL_API void ImGui_ImplSDLRenderer_NewFrame(); 25 | IMGUI_IMPL_API void ImGui_ImplSDLRenderer_RenderDrawData(ImDrawData* draw_data); 26 | 27 | // Called by Init/NewFrame/Shutdown 28 | IMGUI_IMPL_API bool ImGui_ImplSDLRenderer_CreateFontsTexture(); 29 | IMGUI_IMPL_API void ImGui_ImplSDLRenderer_DestroyFontsTexture(); 30 | IMGUI_IMPL_API bool ImGui_ImplSDLRenderer_CreateDeviceObjects(); 31 | IMGUI_IMPL_API void ImGui_ImplSDLRenderer_DestroyDeviceObjects(); 32 | -------------------------------------------------------------------------------- /src/tools/imgui/imgui_impl_wgpu.h: -------------------------------------------------------------------------------- 1 | // dear imgui: Renderer for WebGPU 2 | // This needs to be used along with a Platform Binding (e.g. GLFW) 3 | // (Please note that WebGPU is currently experimental, will not run on non-beta browsers, and may break.) 4 | 5 | // Implemented features: 6 | // [X] Renderer: User texture binding. Use 'WGPUTextureView' as ImTextureID. Read the FAQ about ImTextureID! 7 | // [X] Renderer: Large meshes support (64k+ vertices) with 16-bit indices. 8 | 9 | // You can use unmodified imgui_impl_* files in your project. See examples/ folder for examples of using this. 10 | // Prefer including the entire imgui/ repository into your project (either as a copy or as a submodule), and only build the backends you need. 11 | // If you are new to Dear ImGui, read documentation from the docs/ folder + read the top of imgui.cpp. 12 | // Read online: https://github.com/ocornut/imgui/tree/master/docs 13 | 14 | #pragma once 15 | #include "imgui.h" // IMGUI_IMPL_API 16 | #include 17 | 18 | IMGUI_IMPL_API bool ImGui_ImplWGPU_Init(WGPUDevice device, int num_frames_in_flight, WGPUTextureFormat rt_format); 19 | IMGUI_IMPL_API void ImGui_ImplWGPU_Shutdown(); 20 | IMGUI_IMPL_API void ImGui_ImplWGPU_NewFrame(); 21 | IMGUI_IMPL_API void ImGui_ImplWGPU_RenderDrawData(ImDrawData* draw_data, WGPURenderPassEncoder pass_encoder); 22 | 23 | // Use if you want to reset your rendering device without losing Dear ImGui state. 24 | IMGUI_IMPL_API void ImGui_ImplWGPU_InvalidateDeviceObjects(); 25 | IMGUI_IMPL_API bool ImGui_ImplWGPU_CreateDeviceObjects(); 26 | -------------------------------------------------------------------------------- /src/tools/imgui/imgui_impl_win32.h: -------------------------------------------------------------------------------- 1 | // dear imgui: Platform Backend for Windows (standard windows API for 32 and 64 bits applications) 2 | // This needs to be used along with a Renderer (e.g. DirectX11, OpenGL3, Vulkan..) 3 | 4 | // Implemented features: 5 | // [X] Platform: Clipboard support (for Win32 this is actually part of core dear imgui) 6 | // [X] Platform: Keyboard support. Since 1.87 we are using the io.AddKeyEvent() function. Pass ImGuiKey values to all key functions e.g. ImGui::IsKeyPressed(ImGuiKey_Space). [Legacy VK_* values will also be supported unless IMGUI_DISABLE_OBSOLETE_KEYIO is set] 7 | // [X] Platform: Gamepad support. Enabled with 'io.ConfigFlags |= ImGuiConfigFlags_NavEnableGamepad'. 8 | // [X] Platform: Mouse cursor shape and visibility. Disable with 'io.ConfigFlags |= ImGuiConfigFlags_NoMouseCursorChange'. 9 | // [X] Platform: Multi-viewport support (multiple windows). Enable with 'io.ConfigFlags |= ImGuiConfigFlags_ViewportsEnable'. 10 | 11 | // You can use unmodified imgui_impl_* files in your project. See examples/ folder for examples of using this. 12 | // Prefer including the entire imgui/ repository into your project (either as a copy or as a submodule), and only build the backends you need. 13 | // If you are new to Dear ImGui, read documentation from the docs/ folder + read the top of imgui.cpp. 14 | // Read online: https://github.com/ocornut/imgui/tree/master/docs 15 | 16 | #pragma once 17 | #include "imgui.h" // IMGUI_IMPL_API 18 | 19 | IMGUI_IMPL_API bool ImGui_ImplWin32_Init(void* hwnd); 20 | IMGUI_IMPL_API void ImGui_ImplWin32_Shutdown(); 21 | IMGUI_IMPL_API void ImGui_ImplWin32_NewFrame(); 22 | 23 | // Win32 message handler your application need to call. 24 | // - Intentionally commented out in a '#if 0' block to avoid dragging dependencies on from this helper. 25 | // - You should COPY the line below into your .cpp code to forward declare the function and then you can call it. 26 | // - Call from your application's message handler. Keep calling your message handler unless this function returns TRUE. 27 | 28 | #if 0 29 | extern IMGUI_IMPL_API LRESULT ImGui_ImplWin32_WndProcHandler(HWND hWnd, UINT msg, WPARAM wParam, LPARAM lParam); 30 | #endif 31 | 32 | // DPI-related helpers (optional) 33 | // - Use to enable DPI awareness without having to create an application manifest. 34 | // - Your own app may already do this via a manifest or explicit calls. This is mostly useful for our examples/ apps. 35 | // - In theory we could call simple functions from Windows SDK such as SetProcessDPIAware(), SetProcessDpiAwareness(), etc. 36 | // but most of the functions provided by Microsoft require Windows 8.1/10+ SDK at compile time and Windows 8/10+ at runtime, 37 | // neither we want to require the user to have. So we dynamically select and load those functions to avoid dependencies. 38 | IMGUI_IMPL_API void ImGui_ImplWin32_EnableDpiAwareness(); 39 | IMGUI_IMPL_API float ImGui_ImplWin32_GetDpiScaleForHwnd(void* hwnd); // HWND hwnd 40 | IMGUI_IMPL_API float ImGui_ImplWin32_GetDpiScaleForMonitor(void* monitor); // HMONITOR monitor 41 | 42 | // Transparency related helpers (optional) [experimental] 43 | // - Use to enable alpha compositing transparency with the desktop. 44 | // - Use together with e.g. clearing your framebuffer with zero-alpha. 45 | IMGUI_IMPL_API void ImGui_ImplWin32_EnableAlphaCompositing(void* hwnd); // HWND hwnd 46 | -------------------------------------------------------------------------------- /src/tools/lib_sophus/common.hpp: -------------------------------------------------------------------------------- 1 | /// @file 2 | /// Common functionality. 3 | 4 | #ifndef SOPHUS_COMMON_HPP 5 | #define SOPHUS_COMMON_HPP 6 | 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | 13 | #include 14 | 15 | #if !defined(SOPHUS_DISABLE_ENSURES) 16 | #include "formatstring.hpp" 17 | #endif //!defined(SOPHUS_DISABLE_ENSURES) 18 | 19 | // following boost's assert.hpp 20 | #undef SOPHUS_ENSURE 21 | 22 | // ENSURES are similar to ASSERTS, but they are always checked for (including in 23 | // release builds). At the moment there are no ASSERTS in Sophus which should 24 | // only be used for checks which are performance critical. 25 | 26 | #ifdef __GNUC__ 27 | #define SOPHUS_FUNCTION __PRETTY_FUNCTION__ 28 | #elif (_MSC_VER >= 1310) 29 | #define SOPHUS_FUNCTION __FUNCTION__ 30 | #else 31 | #define SOPHUS_FUNCTION "unknown" 32 | #endif 33 | 34 | // Make sure this compiles with older versions of Eigen which do not have 35 | // EIGEN_DEVICE_FUNC defined. 36 | #ifndef EIGEN_DEVICE_FUNC 37 | #define EIGEN_DEVICE_FUNC 38 | #endif 39 | 40 | // NVCC on windows has issues with defaulting the Map specialization constructors, so 41 | // special case that specific platform case. 42 | #if defined(_MSC_VER) && defined(__CUDACC__) 43 | #define SOPHUS_WINDOW_NVCC_FALLBACK 44 | #endif 45 | 46 | // Make sure this compiles with older versions of Eigen which do not have 47 | // EIGEN_DEFAULT_COPY_CONSTRUCTOR defined 48 | #ifndef EIGEN_DEFAULT_COPY_CONSTRUCTOR 49 | #if EIGEN_HAS_CXX11 && !defined(SOPHUS_WINDOW_NVCC_FALLBACK) 50 | #define EIGEN_DEFAULT_COPY_CONSTRUCTOR(CLASS) EIGEN_DEVICE_FUNC CLASS(const CLASS&) = default; 51 | #else 52 | #define EIGEN_DEFAULT_COPY_CONSTRUCTOR(CLASS) 53 | #endif 54 | #ifndef EIGEN_INHERIT_ASSIGNMENT_OPERATORS 55 | #error "eigen must have EIGEN_INHERIT_ASSIGNMENT_OPERATORS" 56 | #endif 57 | #define SOPHUS_INHERIT_ASSIGNMENT_OPERATORS(Derived) \ 58 | EIGEN_INHERIT_ASSIGNMENT_OPERATORS(Derived) \ 59 | EIGEN_DEFAULT_COPY_CONSTRUCTOR(Derived) 60 | #endif 61 | 62 | #ifndef SOPHUS_INHERIT_ASSIGNMENT_OPERATORS 63 | #ifndef EIGEN_INHERIT_ASSIGNMENT_OPERATORS 64 | #error "eigen must have EIGEN_INHERIT_ASSIGNMENT_OPERATORS" 65 | #endif 66 | #define SOPHUS_INHERIT_ASSIGNMENT_OPERATORS(Derived) EIGEN_INHERIT_ASSIGNMENT_OPERATORS(Derived) 67 | #endif 68 | 69 | #define SOPHUS_FUNC EIGEN_DEVICE_FUNC 70 | 71 | #if defined(SOPHUS_DISABLE_ENSURES) 72 | 73 | #define SOPHUS_ENSURE(expr, ...) ((void)0) 74 | 75 | #elif defined(SOPHUS_ENABLE_ENSURE_HANDLER) 76 | 77 | namespace Sophus { 78 | void ensureFailed(char const* function, char const* file, int line, 79 | char const* description); 80 | } 81 | 82 | #define SOPHUS_ENSURE(expr, ...) \ 83 | ((expr) ? ((void)0) \ 84 | : ::Sophus::ensureFailed( \ 85 | SOPHUS_FUNCTION, __FILE__, __LINE__, \ 86 | Sophus::details::FormatString(__VA_ARGS__).c_str())) 87 | #else 88 | // LCOV_EXCL_START 89 | 90 | namespace Sophus { 91 | template 92 | SOPHUS_FUNC void defaultEnsure(char const* function, char const* file, int line, 93 | char const* description, Args&&... args) { 94 | std::printf("Sophus ensure failed in function '%s', file '%s', line %d.\n", 95 | function, file, line); 96 | #ifdef __CUDACC__ 97 | std::printf("%s", description); 98 | #else 99 | std::cout << details::FormatString(description, std::forward(args)...) 100 | << std::endl; 101 | std::abort(); 102 | #endif 103 | } 104 | } // namespace Sophus 105 | 106 | // LCOV_EXCL_STOP 107 | #define SOPHUS_ENSURE(expr, ...) \ 108 | ((expr) ? ((void)0) \ 109 | : Sophus::defaultEnsure(SOPHUS_FUNCTION, __FILE__, __LINE__, \ 110 | ##__VA_ARGS__)) 111 | #endif 112 | 113 | namespace Sophus { 114 | 115 | template 116 | struct Constants { 117 | SOPHUS_FUNC static Scalar epsilon() { return Scalar(1e-10); } 118 | 119 | SOPHUS_FUNC static Scalar epsilonSqrt() { 120 | using std::sqrt; 121 | return sqrt(epsilon()); 122 | } 123 | 124 | SOPHUS_FUNC static Scalar pi() { 125 | return Scalar(3.141592653589793238462643383279502884); 126 | } 127 | }; 128 | 129 | template <> 130 | struct Constants { 131 | SOPHUS_FUNC static float constexpr epsilon() { 132 | return static_cast(1e-5); 133 | } 134 | 135 | SOPHUS_FUNC static float epsilonSqrt() { return std::sqrt(epsilon()); } 136 | 137 | SOPHUS_FUNC static float constexpr pi() { 138 | return 3.141592653589793238462643383279502884f; 139 | } 140 | }; 141 | 142 | /// Nullopt type of lightweight optional class. 143 | struct nullopt_t { 144 | explicit constexpr nullopt_t() {} 145 | }; 146 | 147 | constexpr nullopt_t nullopt{}; 148 | 149 | /// Lightweight optional implementation which requires ``T`` to have a 150 | /// default constructor. 151 | /// 152 | /// TODO: Replace with std::optional once Sophus moves to c++17. 153 | /// 154 | template 155 | class optional { 156 | public: 157 | optional() : is_valid_(false) {} 158 | 159 | optional(nullopt_t) : is_valid_(false) {} 160 | 161 | optional(T const& type) : type_(type), is_valid_(true) {} 162 | 163 | explicit operator bool() const { return is_valid_; } 164 | 165 | T const* operator->() const { 166 | SOPHUS_ENSURE(is_valid_, "must be valid"); 167 | return &type_; 168 | } 169 | 170 | T* operator->() { 171 | SOPHUS_ENSURE(is_valid_, "must be valid"); 172 | return &type_; 173 | } 174 | 175 | T const& operator*() const { 176 | SOPHUS_ENSURE(is_valid_, "must be valid"); 177 | return type_; 178 | } 179 | 180 | T& operator*() { 181 | SOPHUS_ENSURE(is_valid_, "must be valid"); 182 | return type_; 183 | } 184 | 185 | private: 186 | T type_; 187 | bool is_valid_; 188 | }; 189 | 190 | template 191 | using enable_if_t = typename std::enable_if::type; 192 | 193 | template 194 | struct IsUniformRandomBitGenerator { 195 | static const bool value = std::is_unsigned::value && 196 | std::is_unsigned::value && 197 | std::is_unsigned::value; 198 | }; 199 | } // namespace Sophus 200 | 201 | #endif // SOPHUS_COMMON_HPP 202 | -------------------------------------------------------------------------------- /src/tools/lib_sophus/example_ensure_handler.cpp: -------------------------------------------------------------------------------- 1 | #include "common.hpp" 2 | 3 | #include 4 | #include 5 | 6 | namespace Sophus { 7 | void ensureFailed(char const* function, char const* file, int line, 8 | char const* description) { 9 | std::printf("Sophus ensure failed in function '%s', file '%s', line %d.\n", 10 | file, function, line); 11 | std::printf("Description: %s\n", description); 12 | std::abort(); 13 | } 14 | } // namespace Sophus 15 | -------------------------------------------------------------------------------- /src/tools/lib_sophus/formatstring.hpp: -------------------------------------------------------------------------------- 1 | /// @file 2 | /// FormatString functionality 3 | 4 | #ifndef SOPHUS_FORMATSTRING_HPP 5 | #define SOPHUS_FORMATSTRING_HPP 6 | 7 | #include 8 | 9 | namespace Sophus { 10 | namespace details { 11 | 12 | // Following: http://stackoverflow.com/a/22759544 13 | template 14 | class IsStreamable { 15 | private: 16 | template 17 | static auto test(int) 18 | -> decltype(std::declval() << std::declval(), 19 | std::true_type()); 20 | 21 | template 22 | static auto test(...) -> std::false_type; 23 | 24 | public: 25 | static bool const value = decltype(test(0))::value; 26 | }; 27 | 28 | template 29 | class ArgToStream { 30 | public: 31 | static void impl(std::stringstream& stream, T&& arg) { 32 | stream << std::forward(arg); 33 | } 34 | }; 35 | 36 | inline void FormatStream(std::stringstream& stream, char const* text) { 37 | stream << text; 38 | return; 39 | } 40 | 41 | // Following: http://en.cppreference.com/w/cpp/language/parameter_pack 42 | template 43 | void FormatStream(std::stringstream& stream, char const* text, T&& arg, 44 | Args&&... args) { 45 | static_assert(IsStreamable::value, 46 | "One of the args has no ostream overload!"); 47 | for (; *text != '\0'; ++text) { 48 | if (*text == '%') { 49 | ArgToStream::impl(stream, std::forward(arg)); 50 | FormatStream(stream, text + 1, std::forward(args)...); 51 | return; 52 | } 53 | stream << *text; 54 | } 55 | stream << "\nFormat-Warning: There are " << sizeof...(Args) + 1 56 | << " args unused."; 57 | return; 58 | } 59 | 60 | template 61 | std::string FormatString(char const* text, Args&&... args) { 62 | std::stringstream stream; 63 | FormatStream(stream, text, std::forward(args)...); 64 | return stream.str(); 65 | } 66 | 67 | inline std::string FormatString() { return std::string(); } 68 | 69 | } // namespace details 70 | } // namespace Sophus 71 | 72 | #endif //SOPHUS_FORMATSTRING_HPP 73 | -------------------------------------------------------------------------------- /src/tools/lib_sophus/geometry.hpp: -------------------------------------------------------------------------------- 1 | /// @file 2 | /// Transformations between poses and hyperplanes. 3 | 4 | #ifndef GEOMETRY_HPP 5 | #define GEOMETRY_HPP 6 | 7 | #include "se2.hpp" 8 | #include "se3.hpp" 9 | #include "so2.hpp" 10 | #include "so3.hpp" 11 | #include "types.hpp" 12 | 13 | namespace Sophus { 14 | 15 | /// Takes in a rotation ``R_foo_plane`` and returns the corresponding line 16 | /// normal along the y-axis (in reference frame ``foo``). 17 | /// 18 | template 19 | Vector2 normalFromSO2(SO2 const& R_foo_line) { 20 | return R_foo_line.matrix().col(1); 21 | } 22 | 23 | /// Takes in line normal in reference frame foo and constructs a corresponding 24 | /// rotation matrix ``R_foo_line``. 25 | /// 26 | /// Precondition: ``normal_foo`` must not be close to zero. 27 | /// 28 | template 29 | SO2 SO2FromNormal(Vector2 normal_foo) { 30 | SOPHUS_ENSURE(normal_foo.squaredNorm() > Constants::epsilon(), "%", 31 | normal_foo.transpose()); 32 | normal_foo.normalize(); 33 | return SO2(normal_foo.y(), -normal_foo.x()); 34 | } 35 | 36 | /// Takes in a rotation ``R_foo_plane`` and returns the corresponding plane 37 | /// normal along the z-axis 38 | /// (in reference frame ``foo``). 39 | /// 40 | template 41 | Vector3 normalFromSO3(SO3 const& R_foo_plane) { 42 | return R_foo_plane.matrix().col(2); 43 | } 44 | 45 | /// Takes in plane normal in reference frame foo and constructs a corresponding 46 | /// rotation matrix ``R_foo_plane``. 47 | /// 48 | /// Note: The ``plane`` frame is defined as such that the normal points along 49 | /// the positive z-axis. One can specify hints for the x-axis and y-axis 50 | /// of the ``plane`` frame. 51 | /// 52 | /// Preconditions: 53 | /// - ``normal_foo``, ``xDirHint_foo``, ``yDirHint_foo`` must not be close to 54 | /// zero. 55 | /// - ``xDirHint_foo`` and ``yDirHint_foo`` must be approx. perpendicular. 56 | /// 57 | template 58 | Matrix3 rotationFromNormal(Vector3 const& normal_foo, 59 | Vector3 xDirHint_foo = Vector3(T(1), T(0), 60 | T(0)), 61 | Vector3 yDirHint_foo = Vector3(T(0), T(1), 62 | T(0))) { 63 | SOPHUS_ENSURE(xDirHint_foo.dot(yDirHint_foo) < Constants::epsilon(), 64 | "xDirHint (%) and yDirHint (%) must be perpendicular.", 65 | xDirHint_foo.transpose(), yDirHint_foo.transpose()); 66 | using std::abs; 67 | using std::sqrt; 68 | T const xDirHint_foo_sqr_length = xDirHint_foo.squaredNorm(); 69 | T const yDirHint_foo_sqr_length = yDirHint_foo.squaredNorm(); 70 | T const normal_foo_sqr_length = normal_foo.squaredNorm(); 71 | SOPHUS_ENSURE(xDirHint_foo_sqr_length > Constants::epsilon(), "%", 72 | xDirHint_foo.transpose()); 73 | SOPHUS_ENSURE(yDirHint_foo_sqr_length > Constants::epsilon(), "%", 74 | yDirHint_foo.transpose()); 75 | SOPHUS_ENSURE(normal_foo_sqr_length > Constants::epsilon(), "%", 76 | normal_foo.transpose()); 77 | 78 | Matrix3 basis_foo; 79 | basis_foo.col(2) = normal_foo; 80 | 81 | if (abs(xDirHint_foo_sqr_length - T(1)) > Constants::epsilon()) { 82 | xDirHint_foo.normalize(); 83 | } 84 | if (abs(yDirHint_foo_sqr_length - T(1)) > Constants::epsilon()) { 85 | yDirHint_foo.normalize(); 86 | } 87 | if (abs(normal_foo_sqr_length - T(1)) > Constants::epsilon()) { 88 | basis_foo.col(2).normalize(); 89 | } 90 | 91 | T abs_x_dot_z = abs(basis_foo.col(2).dot(xDirHint_foo)); 92 | T abs_y_dot_z = abs(basis_foo.col(2).dot(yDirHint_foo)); 93 | if (abs_x_dot_z < abs_y_dot_z) { 94 | // basis_foo.z and xDirHint are far from parallel. 95 | basis_foo.col(1) = basis_foo.col(2).cross(xDirHint_foo).normalized(); 96 | basis_foo.col(0) = basis_foo.col(1).cross(basis_foo.col(2)); 97 | } else { 98 | // basis_foo.z and yDirHint are far from parallel. 99 | basis_foo.col(0) = yDirHint_foo.cross(basis_foo.col(2)).normalized(); 100 | basis_foo.col(1) = basis_foo.col(2).cross(basis_foo.col(0)); 101 | } 102 | T det = basis_foo.determinant(); 103 | // sanity check 104 | SOPHUS_ENSURE(abs(det - T(1)) < Constants::epsilon(), 105 | "Determinant of basis is not 1, but %. Basis is \n%\n", det, 106 | basis_foo); 107 | return basis_foo; 108 | } 109 | 110 | /// Takes in plane normal in reference frame foo and constructs a corresponding 111 | /// rotation matrix ``R_foo_plane``. 112 | /// 113 | /// See ``rotationFromNormal`` for details. 114 | /// 115 | template 116 | SO3 SO3FromNormal(Vector3 const& normal_foo) { 117 | return SO3(rotationFromNormal(normal_foo)); 118 | } 119 | 120 | /// Returns a line (wrt. to frame ``foo``), given a pose of the ``line`` in 121 | /// reference frame ``foo``. 122 | /// 123 | /// Note: The plane is defined by X-axis of the ``line`` frame. 124 | /// 125 | template 126 | Line2 lineFromSE2(SE2 const& T_foo_line) { 127 | return Line2(normalFromSO2(T_foo_line.so2()), T_foo_line.translation()); 128 | } 129 | 130 | /// Returns the pose ``T_foo_line``, given a line in reference frame ``foo``. 131 | /// 132 | /// Note: The line is defined by X-axis of the frame ``line``. 133 | /// 134 | template 135 | SE2 SE2FromLine(Line2 const& line_foo) { 136 | T const d = line_foo.offset(); 137 | Vector2 const n = line_foo.normal(); 138 | SO2 const R_foo_plane = SO2FromNormal(n); 139 | return SE2(R_foo_plane, -d * n); 140 | } 141 | 142 | /// Returns a plane (wrt. to frame ``foo``), given a pose of the ``plane`` in 143 | /// reference frame ``foo``. 144 | /// 145 | /// Note: The plane is defined by XY-plane of the frame ``plane``. 146 | /// 147 | template 148 | Plane3 planeFromSE3(SE3 const& T_foo_plane) { 149 | return Plane3(normalFromSO3(T_foo_plane.so3()), T_foo_plane.translation()); 150 | } 151 | 152 | /// Returns the pose ``T_foo_plane``, given a plane in reference frame ``foo``. 153 | /// 154 | /// Note: The plane is defined by XY-plane of the frame ``plane``. 155 | /// 156 | template 157 | SE3 SE3FromPlane(Plane3 const& plane_foo) { 158 | T const d = plane_foo.offset(); 159 | Vector3 const n = plane_foo.normal(); 160 | SO3 const R_foo_plane = SO3FromNormal(n); 161 | return SE3(R_foo_plane, -d * n); 162 | } 163 | 164 | /// Takes in a hyperplane and returns unique representation by ensuring that the 165 | /// ``offset`` is not negative. 166 | /// 167 | template 168 | Eigen::Hyperplane makeHyperplaneUnique( 169 | Eigen::Hyperplane const& plane) { 170 | if (plane.offset() >= 0) { 171 | return plane; 172 | } 173 | 174 | return Eigen::Hyperplane(-plane.normal(), -plane.offset()); 175 | } 176 | 177 | } // namespace Sophus 178 | 179 | #endif // GEOMETRY_HPP 180 | -------------------------------------------------------------------------------- /src/tools/lib_sophus/interpolate.hpp: -------------------------------------------------------------------------------- 1 | /// @file 2 | /// Interpolation for Lie groups. 3 | 4 | #ifndef SOPHUS_INTERPOLATE_HPP 5 | #define SOPHUS_INTERPOLATE_HPP 6 | 7 | #include 8 | 9 | #include "interpolate_details.hpp" 10 | 11 | namespace Sophus { 12 | 13 | /// This function interpolates between two Lie group elements ``foo_T_bar`` 14 | /// and ``foo_T_baz`` with an interpolation factor of ``alpha`` in [0, 1]. 15 | /// 16 | /// It returns a pose ``foo_T_quiz`` with ``quiz`` being a frame between ``bar`` 17 | /// and ``baz``. If ``alpha=0`` it returns ``foo_T_bar``. If it is 1, it returns 18 | /// ``foo_T_bar``. 19 | /// 20 | /// (Since interpolation on Lie groups is inverse-invariant, we can equivalently 21 | /// think of the input arguments as being ``bar_T_foo``, ``baz_T_foo`` and the 22 | /// return value being ``quiz_T_foo``.) 23 | /// 24 | /// Precondition: ``p`` must be in [0, 1]. 25 | /// 26 | template 27 | enable_if_t::supported, G> interpolate( 28 | G const& foo_T_bar, G const& foo_T_baz, Scalar2 p = Scalar2(0.5f)) { 29 | using Scalar = typename G::Scalar; 30 | Scalar inter_p(p); 31 | SOPHUS_ENSURE(inter_p >= Scalar(0) && inter_p <= Scalar(1), 32 | "p (%) must in [0, 1]."); 33 | return foo_T_bar * G::exp(inter_p * (foo_T_bar.inverse() * foo_T_baz).log()); 34 | } 35 | 36 | } // namespace Sophus 37 | 38 | #endif // SOPHUS_INTERPOLATE_HPP 39 | -------------------------------------------------------------------------------- /src/tools/lib_sophus/interpolate_details.hpp: -------------------------------------------------------------------------------- 1 | #ifndef SOPHUS_INTERPOLATE_DETAILS_HPP 2 | #define SOPHUS_INTERPOLATE_DETAILS_HPP 3 | 4 | #include "rxso2.hpp" 5 | #include "rxso3.hpp" 6 | #include "se2.hpp" 7 | #include "se3.hpp" 8 | #include "sim2.hpp" 9 | #include "sim3.hpp" 10 | #include "so2.hpp" 11 | #include "so3.hpp" 12 | 13 | namespace Sophus { 14 | namespace interp_details { 15 | 16 | template 17 | struct Traits; 18 | 19 | template 20 | struct Traits> { 21 | static bool constexpr supported = true; 22 | 23 | static bool hasShortestPathAmbiguity(SO2 const& foo_T_bar) { 24 | using std::abs; 25 | Scalar angle = foo_T_bar.log(); 26 | return abs(abs(angle) - Constants::pi()) < 27 | Constants::epsilon(); 28 | } 29 | }; 30 | 31 | template 32 | struct Traits> { 33 | static bool constexpr supported = true; 34 | 35 | static bool hasShortestPathAmbiguity(RxSO2 const& foo_T_bar) { 36 | return Traits>::hasShortestPathAmbiguity(foo_T_bar.so2()); 37 | } 38 | }; 39 | 40 | template 41 | struct Traits> { 42 | static bool constexpr supported = true; 43 | 44 | static bool hasShortestPathAmbiguity(SO3 const& foo_T_bar) { 45 | using std::abs; 46 | Scalar angle = foo_T_bar.logAndTheta().theta; 47 | return abs(abs(angle) - Constants::pi()) < 48 | Constants::epsilon(); 49 | } 50 | }; 51 | 52 | template 53 | struct Traits> { 54 | static bool constexpr supported = true; 55 | 56 | static bool hasShortestPathAmbiguity(RxSO3 const& foo_T_bar) { 57 | return Traits>::hasShortestPathAmbiguity(foo_T_bar.so3()); 58 | } 59 | }; 60 | 61 | template 62 | struct Traits> { 63 | static bool constexpr supported = true; 64 | 65 | static bool hasShortestPathAmbiguity(SE2 const& foo_T_bar) { 66 | return Traits>::hasShortestPathAmbiguity(foo_T_bar.so2()); 67 | } 68 | }; 69 | 70 | template 71 | struct Traits> { 72 | static bool constexpr supported = true; 73 | 74 | static bool hasShortestPathAmbiguity(SE3 const& foo_T_bar) { 75 | return Traits>::hasShortestPathAmbiguity(foo_T_bar.so3()); 76 | } 77 | }; 78 | 79 | template 80 | struct Traits> { 81 | static bool constexpr supported = true; 82 | 83 | static bool hasShortestPathAmbiguity(Sim2 const& foo_T_bar) { 84 | return Traits>::hasShortestPathAmbiguity( 85 | foo_T_bar.rxso2().so2()); 86 | ; 87 | } 88 | }; 89 | 90 | template 91 | struct Traits> { 92 | static bool constexpr supported = true; 93 | 94 | static bool hasShortestPathAmbiguity(Sim3 const& foo_T_bar) { 95 | return Traits>::hasShortestPathAmbiguity( 96 | foo_T_bar.rxso3().so3()); 97 | ; 98 | } 99 | }; 100 | 101 | } // namespace interp_details 102 | } // namespace Sophus 103 | 104 | #endif // SOPHUS_INTERPOLATE_DETAILS_HPP 105 | -------------------------------------------------------------------------------- /src/tools/lib_sophus/local_parameterization_se3.hpp: -------------------------------------------------------------------------------- 1 | #ifndef SOPHUS_TEST_LOCAL_PARAMETERIZATION_SE3_HPP 2 | #define SOPHUS_TEST_LOCAL_PARAMETERIZATION_SE3_HPP 3 | 4 | #include 5 | #include "se3.hpp" 6 | 7 | namespace Sophus { 8 | 9 | class LocalParameterizationSE3 : public ceres::LocalParameterization { 10 | public: 11 | virtual ~LocalParameterizationSE3() {} 12 | 13 | // SE3 plus operation for Ceres 14 | // 15 | // T * exp(x) 16 | // 17 | virtual bool Plus(double const* T_raw, double const* delta_raw, 18 | double* T_plus_delta_raw) const { 19 | Eigen::Map const T(T_raw); 20 | Eigen::Map const delta(delta_raw); 21 | Eigen::Map T_plus_delta(T_plus_delta_raw); 22 | T_plus_delta = T * SE3d::exp(delta); 23 | return true; 24 | } 25 | 26 | // Jacobian of SE3 plus operation for Ceres 27 | // 28 | // Dx T * exp(x) with x=0 29 | // 30 | virtual bool ComputeJacobian(double const* T_raw, 31 | double* jacobian_raw) const { 32 | Eigen::Map T(T_raw); 33 | Eigen::Map> jacobian( 34 | jacobian_raw); 35 | jacobian = T.Dx_this_mul_exp_x_at_0(); 36 | return true; 37 | } 38 | 39 | virtual int GlobalSize() const { return SE3d::num_parameters; } 40 | 41 | virtual int LocalSize() const { return SE3d::DoF; } 42 | }; 43 | } // namespace Sophus 44 | 45 | #endif 46 | -------------------------------------------------------------------------------- /src/tools/lib_sophus/num_diff.hpp: -------------------------------------------------------------------------------- 1 | /// @file 2 | /// Numerical differentiation using finite differences 3 | 4 | #ifndef SOPHUS_NUM_DIFF_HPP 5 | #define SOPHUS_NUM_DIFF_HPP 6 | 7 | #include 8 | #include 9 | #include 10 | 11 | #include "types.hpp" 12 | 13 | namespace Sophus { 14 | 15 | namespace details { 16 | template 17 | class Curve { 18 | public: 19 | template 20 | static auto num_diff(Fn curve, Scalar t, Scalar h) -> decltype(curve(t)) { 21 | using ReturnType = decltype(curve(t)); 22 | static_assert(std::is_floating_point::value, 23 | "Scalar must be a floating point type."); 24 | static_assert(IsFloatingPoint::value, 25 | "ReturnType must be either a floating point scalar, " 26 | "vector or matrix."); 27 | 28 | return (curve(t + h) - curve(t - h)) / (Scalar(2) * h); 29 | } 30 | }; 31 | 32 | template 33 | class VectorField { 34 | public: 35 | static Eigen::Matrix num_diff( 36 | std::function(Sophus::Vector)> 37 | vector_field, 38 | Sophus::Vector const& a, Scalar eps) { 39 | static_assert(std::is_floating_point::value, 40 | "Scalar must be a floating point type."); 41 | Eigen::Matrix J; 42 | Sophus::Vector h; 43 | h.setZero(); 44 | for (int i = 0; i < M; ++i) { 45 | h[i] = eps; 46 | J.col(i) = 47 | (vector_field(a + h) - vector_field(a - h)) / (Scalar(2) * eps); 48 | h[i] = Scalar(0); 49 | } 50 | 51 | return J; 52 | } 53 | }; 54 | 55 | template 56 | class VectorField { 57 | public: 58 | static Eigen::Matrix num_diff( 59 | std::function(Scalar)> vector_field, 60 | Scalar const& a, Scalar eps) { 61 | return details::Curve::num_diff(std::move(vector_field), a, eps); 62 | } 63 | }; 64 | } // namespace details 65 | 66 | /// Calculates the derivative of a curve at a point ``t``. 67 | /// 68 | /// Here, a curve is a function from a Scalar to a Euclidean space. Thus, it 69 | /// returns either a Scalar, a vector or a matrix. 70 | /// 71 | template 72 | auto curveNumDiff(Fn curve, Scalar t, 73 | Scalar h = Constants::epsilonSqrt()) 74 | -> decltype(details::Curve::num_diff(std::move(curve), t, h)) { 75 | return details::Curve::num_diff(std::move(curve), t, h); 76 | } 77 | 78 | /// Calculates the derivative of a vector field at a point ``a``. 79 | /// 80 | /// Here, a vector field is a function from a vector space to another vector 81 | /// space. 82 | /// 83 | template 84 | Eigen::Matrix vectorFieldNumDiff( 85 | Fn vector_field, ScalarOrVector const& a, 86 | Scalar eps = Constants::epsilonSqrt()) { 87 | return details::VectorField::num_diff(std::move(vector_field), 88 | a, eps); 89 | } 90 | 91 | } // namespace Sophus 92 | 93 | #endif // SOPHUS_NUM_DIFF_HPP 94 | -------------------------------------------------------------------------------- /src/tools/lib_sophus/rotation_matrix.hpp: -------------------------------------------------------------------------------- 1 | /// @file 2 | /// Rotation matrix helper functions. 3 | 4 | #ifndef SOPHUS_ROTATION_MATRIX_HPP 5 | #define SOPHUS_ROTATION_MATRIX_HPP 6 | 7 | #include 8 | #include 9 | 10 | #include "types.hpp" 11 | 12 | namespace Sophus { 13 | 14 | /// Takes in arbitrary square matrix and returns true if it is 15 | /// orthogonal. 16 | template 17 | SOPHUS_FUNC bool isOrthogonal(Eigen::MatrixBase const& R) { 18 | using Scalar = typename D::Scalar; 19 | static int const N = D::RowsAtCompileTime; 20 | static int const M = D::ColsAtCompileTime; 21 | 22 | static_assert(N == M, "must be a square matrix"); 23 | static_assert(N >= 2, "must have compile time dimension >= 2"); 24 | 25 | return (R * R.transpose() - Matrix::Identity()).norm() < 26 | Constants::epsilon(); 27 | } 28 | 29 | /// Takes in arbitrary square matrix and returns true if it is 30 | /// "scaled-orthogonal" with positive determinant. 31 | /// 32 | template 33 | SOPHUS_FUNC bool isScaledOrthogonalAndPositive(Eigen::MatrixBase const& sR) { 34 | using Scalar = typename D::Scalar; 35 | static int const N = D::RowsAtCompileTime; 36 | static int const M = D::ColsAtCompileTime; 37 | using std::pow; 38 | using std::sqrt; 39 | 40 | Scalar det = sR.determinant(); 41 | 42 | if (det <= Scalar(0)) { 43 | return false; 44 | } 45 | 46 | Scalar scale_sqr = pow(det, Scalar(2. / N)); 47 | 48 | static_assert(N == M, "must be a square matrix"); 49 | static_assert(N >= 2, "must have compile time dimension >= 2"); 50 | 51 | return (sR * sR.transpose() - scale_sqr * Matrix::Identity()) 52 | .template lpNorm() < 53 | sqrt(Constants::epsilon()); 54 | } 55 | 56 | /// Takes in arbitrary square matrix (2x2 or larger) and returns closest 57 | /// orthogonal matrix with positive determinant. 58 | template 59 | SOPHUS_FUNC enable_if_t< 60 | std::is_floating_point::value, 61 | Matrix> 62 | makeRotationMatrix(Eigen::MatrixBase const& R) { 63 | using Scalar = typename D::Scalar; 64 | static int const N = D::RowsAtCompileTime; 65 | static int const M = D::ColsAtCompileTime; 66 | 67 | static_assert(N == M, "must be a square matrix"); 68 | static_assert(N >= 2, "must have compile time dimension >= 2"); 69 | 70 | Eigen::JacobiSVD> svd( 71 | R, Eigen::ComputeFullU | Eigen::ComputeFullV); 72 | 73 | // Determine determinant of orthogonal matrix U*V'. 74 | Scalar d = (svd.matrixU() * svd.matrixV().transpose()).determinant(); 75 | // Starting from the identity matrix D, set the last entry to d (+1 or 76 | // -1), so that det(U*D*V') = 1. 77 | Matrix Diag = Matrix::Identity(); 78 | Diag(N - 1, N - 1) = d; 79 | return svd.matrixU() * Diag * svd.matrixV().transpose(); 80 | } 81 | 82 | } // namespace Sophus 83 | 84 | #endif // SOPHUS_ROTATION_MATRIX_HPP 85 | -------------------------------------------------------------------------------- /src/tools/lib_sophus/sim_details.hpp: -------------------------------------------------------------------------------- 1 | #ifndef SOPHUS_SIM_DETAILS_HPP 2 | #define SOPHUS_SIM_DETAILS_HPP 3 | 4 | #include "types.hpp" 5 | 6 | namespace Sophus { 7 | namespace details { 8 | 9 | template 10 | Matrix calcW(Matrix const& Omega, 11 | Scalar const theta, Scalar const sigma) { 12 | using std::abs; 13 | using std::cos; 14 | using std::exp; 15 | using std::sin; 16 | static Matrix const I = Matrix::Identity(); 17 | static Scalar const one(1); 18 | static Scalar const half(0.5); 19 | Matrix const Omega2 = Omega * Omega; 20 | Scalar const scale = exp(sigma); 21 | Scalar A, B, C; 22 | if (abs(sigma) < Constants::epsilon()) { 23 | C = one; 24 | if (abs(theta) < Constants::epsilon()) { 25 | A = half; 26 | B = Scalar(1. / 6.); 27 | } else { 28 | Scalar theta_sq = theta * theta; 29 | A = (one - cos(theta)) / theta_sq; 30 | B = (theta - sin(theta)) / (theta_sq * theta); 31 | } 32 | } else { 33 | C = (scale - one) / sigma; 34 | if (abs(theta) < Constants::epsilon()) { 35 | Scalar sigma_sq = sigma * sigma; 36 | A = ((sigma - one) * scale + one) / sigma_sq; 37 | B = (scale * half * sigma_sq + scale - one - sigma * scale) / 38 | (sigma_sq * sigma); 39 | } else { 40 | Scalar theta_sq = theta * theta; 41 | Scalar a = scale * sin(theta); 42 | Scalar b = scale * cos(theta); 43 | Scalar c = theta_sq + sigma * sigma; 44 | A = (a * sigma + (one - b) * theta) / (theta * c); 45 | B = (C - ((b - one) * sigma + a * theta) / (c)) * one / (theta_sq); 46 | } 47 | } 48 | return A * Omega + B * Omega2 + C * I; 49 | } 50 | 51 | template 52 | Matrix calcWInv(Matrix const& Omega, 53 | Scalar const theta, Scalar const sigma, 54 | Scalar const scale) { 55 | using std::abs; 56 | using std::cos; 57 | using std::sin; 58 | static Matrix const I = Matrix::Identity(); 59 | static Scalar const half(0.5); 60 | static Scalar const one(1); 61 | static Scalar const two(2); 62 | Matrix const Omega2 = Omega * Omega; 63 | Scalar const scale_sq = scale * scale; 64 | Scalar const theta_sq = theta * theta; 65 | Scalar const sin_theta = sin(theta); 66 | Scalar const cos_theta = cos(theta); 67 | 68 | Scalar a, b, c; 69 | if (abs(sigma * sigma) < Constants::epsilon()) { 70 | c = one - half * sigma; 71 | a = -half; 72 | if (abs(theta_sq) < Constants::epsilon()) { 73 | b = Scalar(1. / 12.); 74 | } else { 75 | b = (theta * sin_theta + two * cos_theta - two) / 76 | (two * theta_sq * (cos_theta - one)); 77 | } 78 | } else { 79 | Scalar const scale_cu = scale_sq * scale; 80 | c = sigma / (scale - one); 81 | if (abs(theta_sq) < Constants::epsilon()) { 82 | a = (-sigma * scale + scale - one) / ((scale - one) * (scale - one)); 83 | b = (scale_sq * sigma - two * scale_sq + scale * sigma + two * scale) / 84 | (two * scale_cu - Scalar(6) * scale_sq + Scalar(6) * scale - two); 85 | } else { 86 | Scalar const s_sin_theta = scale * sin_theta; 87 | Scalar const s_cos_theta = scale * cos_theta; 88 | a = (theta * s_cos_theta - theta - sigma * s_sin_theta) / 89 | (theta * (scale_sq - two * s_cos_theta + one)); 90 | b = -scale * 91 | (theta * s_sin_theta - theta * sin_theta + sigma * s_cos_theta - 92 | scale * sigma + sigma * cos_theta - sigma) / 93 | (theta_sq * (scale_cu - two * scale * s_cos_theta - scale_sq + 94 | two * s_cos_theta + scale - one)); 95 | } 96 | } 97 | return a * Omega + b * Omega2 + c * I; 98 | } 99 | 100 | } // namespace details 101 | } // namespace Sophus 102 | 103 | #endif 104 | -------------------------------------------------------------------------------- /src/tools/lib_sophus/velocities.hpp: -------------------------------------------------------------------------------- 1 | #ifndef SOPHUS_VELOCITIES_HPP 2 | #define SOPHUS_VELOCITIES_HPP 3 | 4 | #include 5 | 6 | #include "num_diff.hpp" 7 | #include "se3.hpp" 8 | 9 | namespace Sophus { 10 | namespace experimental { 11 | // Experimental since the API will certainly change drastically in the future. 12 | 13 | // Transforms velocity vector by rotation ``foo_R_bar``. 14 | // 15 | // Note: vel_bar can be either a linear or a rotational velocity vector. 16 | // 17 | template 18 | Vector3 transformVelocity(SO3 const& foo_R_bar, 19 | Vector3 const& vel_bar) { 20 | // For rotational velocities note that: 21 | // 22 | // vel_bar = vee(foo_R_bar * hat(vel_bar) * foo_R_bar^T) 23 | // = vee(hat(Adj(foo_R_bar) * vel_bar)) 24 | // = Adj(foo_R_bar) * vel_bar 25 | // = foo_R_bar * vel_bar. 26 | // 27 | return foo_R_bar * vel_bar; 28 | } 29 | 30 | // Transforms velocity vector by pose ``foo_T_bar``. 31 | // 32 | // Note: vel_bar can be either a linear or a rotational velocity vector. 33 | // 34 | template 35 | Vector3 transformVelocity(SE3 const& foo_T_bar, 36 | Vector3 const& vel_bar) { 37 | return transformVelocity(foo_T_bar.so3(), vel_bar); 38 | } 39 | 40 | // finite difference approximation of instantanious velocity in frame foo 41 | // 42 | template 43 | Vector3 finiteDifferenceRotationalVelocity( 44 | std::function(Scalar)> const& foo_R_bar, Scalar t, 45 | Scalar h = Constants::epsilon()) { 46 | // https://en.wikipedia.org/w/index.php?title=Angular_velocity&oldid=791867792#Angular_velocity_tensor 47 | // 48 | // W = dR(t)/dt * R^{-1}(t) 49 | Matrix3 dR_dt_in_frame_foo = curveNumDiff( 50 | [&foo_R_bar](Scalar t0) -> Matrix3 { 51 | return foo_R_bar(t0).matrix(); 52 | }, 53 | t, h); 54 | // velocity tensor 55 | Matrix3 W_in_frame_foo = 56 | dR_dt_in_frame_foo * (foo_R_bar(t)).inverse().matrix(); 57 | return SO3::vee(W_in_frame_foo); 58 | } 59 | 60 | // finite difference approximation of instantanious velocity in frame foo 61 | // 62 | template 63 | Vector3 finiteDifferenceRotationalVelocity( 64 | std::function(Scalar)> const& foo_T_bar, Scalar t, 65 | Scalar h = Constants::epsilon()) { 66 | return finiteDifferenceRotationalVelocity( 67 | [&foo_T_bar](Scalar t) -> SO3 { return foo_T_bar(t).so3(); }, t, 68 | h); 69 | } 70 | 71 | } // namespace experimental 72 | } // namespace Sophus 73 | 74 | #endif // SOPHUS_VELOCITIES_HPP 75 | -------------------------------------------------------------------------------- /src/tools/openGL_libs/glformattraits.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | // this part of codes is copied from Pangolin: https://github.com/stevenlovegrove/Pangolin 3 | #include // Will drag system OpenGL headers 4 | #include 5 | #include 6 | 7 | 8 | #ifndef GL_LUMINANCE32UI_EXT // I don't know why this is not defined if include GLAD 9 | #include "GL/glext.h" 10 | #endif 11 | 12 | 13 | template 14 | struct GlFormatTraits; 15 | //{ 16 | // static const GLint glinternalformat = 0; 17 | // static const GLenum glformat = 0; 18 | // static const GLenum gltype = 0; 19 | // static const T glmin = 0; 20 | // static const T glmax = 0; 21 | //}; 22 | 23 | template<> 24 | struct GlFormatTraits 25 | { 26 | static const GLint glinternalformat = GL_LUMINANCE8; 27 | static const GLenum glformat = GL_LUMINANCE; 28 | static const GLenum gltype = GL_UNSIGNED_BYTE; 29 | static const size_t components = 1; 30 | }; 31 | 32 | template<> 33 | struct GlFormatTraits 34 | { 35 | static const GLint glinternalformat = GL_LUMINANCE16; 36 | static const GLenum glformat = GL_LUMINANCE; 37 | static const GLenum gltype = GL_UNSIGNED_SHORT; 38 | static const size_t components = 1; 39 | }; 40 | 41 | template<> 42 | struct GlFormatTraits 43 | { 44 | static const GLint glinternalformat = GL_LUMINANCE32UI_EXT; 45 | static const GLenum glformat = GL_LUMINANCE_INTEGER_EXT; 46 | static const GLenum gltype = GL_UNSIGNED_INT; 47 | static const size_t components = 1; 48 | }; 49 | 50 | template<> 51 | struct GlFormatTraits 52 | { 53 | static const GLint glinternalformat = GL_LUMINANCE32I_EXT; 54 | static const GLenum glformat = GL_LUMINANCE_INTEGER_EXT; 55 | static const GLenum gltype = GL_INT; 56 | static const size_t components = 1; 57 | }; 58 | 59 | template<> 60 | struct GlFormatTraits 61 | { 62 | static const GLint glinternalformat = GL_LUMINANCE32F_ARB; 63 | static const GLenum glformat = GL_LUMINANCE; 64 | static const GLenum gltype = GL_FLOAT; 65 | static const size_t components = 1; 66 | }; 67 | 68 | template<> 69 | struct GlFormatTraits 70 | { 71 | static const GLint glinternalformat = GL_LUMINANCE32F_ARB; 72 | static const GLenum glformat = GL_LUMINANCE; 73 | static const GLenum gltype = GL_DOUBLE; 74 | static const size_t components = 1; 75 | }; 76 | 77 | 78 | ////////////////////////////////////////////////////////////////// 79 | 80 | template <> 81 | struct GlFormatTraits> 82 | { 83 | static const GLint glinternalformat = GL_RG8; 84 | static const GLenum glformat = GL_RG; 85 | static const GLenum gltype = GL_UNSIGNED_BYTE; 86 | static const size_t components = 2; 87 | }; 88 | 89 | template <> 90 | struct GlFormatTraits> 91 | { 92 | static const GLint glinternalformat = GL_RG16; 93 | static const GLenum glformat = GL_RG; 94 | static const GLenum gltype = GL_UNSIGNED_SHORT; 95 | static const size_t components = 2; 96 | }; 97 | 98 | template <> 99 | struct GlFormatTraits 100 | { 101 | static const GLint glinternalformat = GL_RG32I; 102 | static const GLenum glformat = GL_RG; 103 | static const GLenum gltype = GL_INT; 104 | static const size_t components = 2; 105 | }; 106 | 107 | template <> 108 | struct GlFormatTraits 109 | { 110 | static const GLint glinternalformat = GL_RG32F; 111 | static const GLenum glformat = GL_RG; 112 | static const GLenum gltype = GL_FLOAT; 113 | static const size_t components = 2; 114 | }; 115 | 116 | template <> 117 | struct GlFormatTraits 118 | { 119 | static const GLint glinternalformat = GL_RG32F; 120 | static const GLenum glformat = GL_RG; 121 | static const GLenum gltype = GL_DOUBLE; 122 | static const size_t components = 2; 123 | }; 124 | 125 | ////////////////////////////////////////////////////////////////// 126 | 127 | template <> 128 | struct GlFormatTraits> 129 | { 130 | static const GLint glinternalformat = GL_RGB8; 131 | static const GLenum glformat = GL_RGB; 132 | static const GLenum gltype = GL_UNSIGNED_BYTE; 133 | static const size_t components = 3; 134 | }; 135 | 136 | template <> 137 | struct GlFormatTraits> 138 | { 139 | static const GLint glinternalformat = GL_RGBA16; 140 | static const GLenum glformat = GL_RGB; 141 | static const GLenum gltype = GL_UNSIGNED_SHORT; 142 | static const size_t components = 3; 143 | }; 144 | 145 | template <> 146 | struct GlFormatTraits 147 | { 148 | static const GLint glinternalformat = GL_RGB32F; 149 | static const GLenum glformat = GL_RGB; 150 | static const GLenum gltype = GL_FLOAT; 151 | static const size_t components = 3; 152 | }; 153 | 154 | template <> 155 | struct GlFormatTraits 156 | { 157 | static const GLint glinternalformat = GL_RGB32F; 158 | static const GLenum glformat = GL_RGB; 159 | static const GLenum gltype = GL_DOUBLE; 160 | static const size_t components = 3; 161 | }; 162 | 163 | ////////////////////////////////////////////////////////////////// 164 | 165 | template <> 166 | struct GlFormatTraits> 167 | { 168 | static const GLint glinternalformat = GL_RGBA8; 169 | static const GLenum glformat = GL_RGBA; 170 | static const GLenum gltype = GL_UNSIGNED_BYTE; 171 | static const size_t components = 4; 172 | }; 173 | 174 | template <> 175 | struct GlFormatTraits> 176 | { 177 | static const GLint glinternalformat = GL_RGBA16; 178 | static const GLenum glformat = GL_RGBA; 179 | static const GLenum gltype = GL_UNSIGNED_SHORT; 180 | static const size_t components = 4; 181 | }; 182 | 183 | template <> 184 | struct GlFormatTraits 185 | { 186 | static const GLint glinternalformat = GL_RGBA32F; 187 | static const GLenum glformat = GL_RGBA; 188 | static const GLenum gltype = GL_FLOAT; 189 | static const size_t components = 4; 190 | }; 191 | 192 | template <> 193 | struct GlFormatTraits 194 | { 195 | static const GLint glinternalformat = GL_RGBA32F; 196 | static const GLenum glformat = GL_RGBA; 197 | static const GLenum gltype = GL_DOUBLE; 198 | static const size_t components = 4; 199 | }; 200 | 201 | -------------------------------------------------------------------------------- /src/tools/os_compatible.hpp: -------------------------------------------------------------------------------- 1 | #ifndef __OS_COMPATIBLE_HPP__ 2 | #define __OS_COMPATIBLE_HPP__ 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include //need for such like printf(...) 9 | #include 10 | #include 11 | #if defined _MSC_VER 12 | #include 13 | #elif defined __GNUC__ 14 | #include 15 | #include 16 | #endif 17 | namespace Common_tools 18 | { 19 | inline void create_dir(std::string dir) 20 | { 21 | #if defined _MSC_VER 22 | _mkdir(dir.data()); 23 | #elif defined __GNUC__ 24 | mkdir(dir.data(), 0777); 25 | #endif 26 | } 27 | 28 | // Using asprintf() on windows 29 | // https://stackoverflow.com/questions/40159892/using-asprintf-on-windows 30 | #ifndef _vscprintf 31 | /* For some reason, MSVC fails to honour this #ifndef. */ 32 | /* Hence function renamed to _vscprintf_so(). */ 33 | inline int _vscprintf_so(const char * format, va_list pargs) { 34 | int retval; 35 | va_list argcopy; 36 | va_copy(argcopy, pargs); 37 | retval = vsnprintf(NULL, 0, format, argcopy); 38 | va_end(argcopy); 39 | return retval; 40 | } 41 | #endif // 42 | 43 | #ifndef vasprintf 44 | inline int vasprintf(char **strp, const char *fmt, va_list ap) { 45 | int len = _vscprintf_so(fmt, ap); 46 | if (len == -1) return -1; 47 | char *str = (char*)malloc((size_t)len + 1); 48 | if (!str) return -1; 49 | int r = vsnprintf(str, len + 1, fmt, ap); /* "secure" version of vsprintf */ 50 | if (r == -1) return free(str), -1; 51 | *strp = str; 52 | return r; 53 | } 54 | #endif // vasprintf 55 | } 56 | #endif -------------------------------------------------------------------------------- /src/tools/tools_color_printf.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | This code is the implementation of our paper "R3LIVE: A Robust, Real-time, RGB-colored, 3 | LiDAR-Inertial-Visual tightly-coupled state Estimation and mapping package". 4 | 5 | Author: Jiarong Lin < ziv.lin.ljr@gmail.com > 6 | 7 | If you use any code of this repo in your academic research, please cite at least 8 | one of our papers: 9 | [1] Lin, Jiarong, and Fu Zhang. "R3LIVE: A Robust, Real-time, RGB-colored, 10 | LiDAR-Inertial-Visual tightly-coupled state Estimation and mapping package." 11 | [2] Xu, Wei, et al. "Fast-lio2: Fast direct lidar-inertial odometry." 12 | [3] Lin, Jiarong, et al. "R2LIVE: A Robust, Real-time, LiDAR-Inertial-Visual 13 | tightly-coupled state Estimator and mapping." 14 | [4] Xu, Wei, and Fu Zhang. "Fast-lio: A fast, robust lidar-inertial odometry 15 | package by tightly-coupled iterated kalman filter." 16 | [5] Cai, Yixi, Wei Xu, and Fu Zhang. "ikd-Tree: An Incremental KD Tree for 17 | Robotic Applications." 18 | [6] Lin, Jiarong, and Fu Zhang. "Loam-livox: A fast, robust, high-precision 19 | LiDAR odometry and mapping package for LiDARs of small FoV." 20 | 21 | For commercial use, please contact me < ziv.lin.ljr@gmail.com > and 22 | Dr. Fu Zhang < fuzhang@hku.hk >. 23 | 24 | Redistribution and use in source and binary forms, with or without 25 | modification, are permitted provided that the following conditions are met: 26 | 27 | 1. Redistributions of source code must retain the above copyright notice, 28 | this list of conditions and the following disclaimer. 29 | 2. Redistributions in binary form must reproduce the above copyright notice, 30 | this list of conditions and the following disclaimer in the documentation 31 | and/or other materials provided with the distribution. 32 | 3. Neither the name of the copyright holder nor the names of its 33 | contributors may be used to endorse or promote products derived from this 34 | software without specific prior written permission. 35 | 36 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 37 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 38 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 39 | ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 40 | LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 41 | CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 42 | SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 43 | INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 44 | CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 45 | ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 46 | POSSIBILITY OF SUCH DAMAGE. 47 | */ 48 | #ifndef __TOOLS_COLOR_PRINT_HPP__ 49 | #define __TOOLS_COLOR_PRINT_HPP__ 50 | #include 51 | #include 52 | #include 53 | /* 54 | * Make screen print colorful :) 55 | * Author: Jiarong Lin 56 | * Related link: 57 | * [1] https://en.wikipedia.org/wiki/ANSI_escape_code 58 | * [2] https://github.com/caffedrine/CPP_Utils/blob/597fe5200be87fa1db2d2aa5d8a07c3bc32a66cd/Utils/include/AnsiColors.h 59 | * 60 | */ 61 | 62 | // const std::string _tools_color_printf_version = "V1.0"; 63 | // const std::string _tools_color_printf_info = "[Init]: Add macros, add scope_color()"; 64 | const std::string _tools_color_printf_version = "V1.2"; 65 | const std::string _tools_color_printf_info = "[Enh]: Add delete lines, ANSI_SCREEN_FLUSH"; 66 | using std::cout; 67 | using std::endl; 68 | // clang-format off 69 | #ifdef EMPTY_ANSI_COLORS 70 | #define ANSI_COLOR_RED "" 71 | #define ANSI_COLOR_RED_BOLD "" 72 | #define ANSI_COLOR_GREEN "" 73 | #define ANSI_COLOR_GREEN_BOLD "" 74 | #define ANSI_COLOR_YELLOW "" 75 | #define ANSI_COLOR_YELLOW_BOLD "" 76 | #define ANSI_COLOR_BLUE "" 77 | #define ANSI_COLOR_BLUE_BOLD "" 78 | #define ANSI_COLOR_MAGENTA "" 79 | #define ANSI_COLOR_MAGENTA_BOLD "" 80 | #else 81 | #define ANSI_COLOR_RED "\x1b[0;31m" 82 | #define ANSI_COLOR_RED_BOLD "\x1b[1;31m" 83 | #define ANSI_COLOR_RED_BG "\x1b[0;41m" 84 | 85 | #define ANSI_COLOR_GREEN "\x1b[0;32m" 86 | #define ANSI_COLOR_GREEN_BOLD "\x1b[1;32m" 87 | #define ANSI_COLOR_GREEN_BG "\x1b[0;42m" 88 | 89 | #define ANSI_COLOR_YELLOW "\x1b[0;33m" 90 | #define ANSI_COLOR_YELLOW_BOLD "\x1b[1;33m" 91 | #define ANSI_COLOR_YELLOW_BG "\x1b[0;43m" 92 | 93 | #define ANSI_COLOR_BLUE "\x1b[0;34m" 94 | #define ANSI_COLOR_BLUE_BOLD "\x1b[1;34m" 95 | #define ANSI_COLOR_BLUE_BG "\x1b[0;44m" 96 | 97 | #define ANSI_COLOR_MAGENTA "\x1b[0;35m" 98 | #define ANSI_COLOR_MAGENTA_BOLD "\x1b[1;35m" 99 | #define ANSI_COLOR_MAGENTA_BG "\x1b[0;45m" 100 | 101 | #define ANSI_COLOR_CYAN "\x1b[0;36m" 102 | #define ANSI_COLOR_CYAN_BOLD "\x1b[1;36m" 103 | #define ANSI_COLOR_CYAN_BG "\x1b[0;46m" 104 | 105 | #define ANSI_COLOR_WHITE "\x1b[0;37m" 106 | #define ANSI_COLOR_WHITE_BOLD "\x1b[1;37m" 107 | #define ANSI_COLOR_WHITE_BG "\x1b[0;47m" 108 | 109 | #define ANSI_COLOR_BLACK "\x1b[0;30m" 110 | #define ANSI_COLOR_BLACK_BOLD "\x1b[1;30m" 111 | #define ANSI_COLOR_BLACK_BG "\x1b[0;40m" 112 | 113 | #define ANSI_COLOR_RESET "\x1b[0m" 114 | 115 | #define ANSI_DELETE_LAST_LINE "\033[A\33[2K\r" 116 | #define ANSI_DELETE_CURRENT_LINE "\33[2K\r" 117 | #define ANSI_SCREEN_FLUSH std::fflush(stdout); 118 | 119 | #define SET_PRINT_COLOR( a ) cout << a ; 120 | 121 | #endif 122 | // clang-format on 123 | 124 | struct _Scope_color 125 | { 126 | _Scope_color( const char * color ) 127 | { 128 | cout << color; 129 | } 130 | 131 | ~_Scope_color() 132 | { 133 | cout << ANSI_COLOR_RESET; 134 | } 135 | }; 136 | 137 | #define scope_color(a) _Scope_color _scope(a); 138 | 139 | // inline int demo_test_color_printf() 140 | // { 141 | // int i, j, n; 142 | 143 | // for ( i = 0; i < 11; i++ ) 144 | // { 145 | // for ( j = 0; j < 10; j++ ) 146 | // { 147 | // n = 10 * i + j; 148 | // if ( n > 108 ) 149 | // break; 150 | // printf( "\033[%dm %3d\033[m", n, n ); 151 | // } 152 | // printf( "\n" ); 153 | // } 154 | // return ( 0 ); 155 | // }; 156 | #endif -------------------------------------------------------------------------------- /src/tools/tools_graphics.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include 3 | #include 4 | #include 5 | 6 | // #include 7 | // https://doc.cgal.org/latest/Kernel_23/index.html#Kernel_23Kernel 8 | #include 9 | #include 10 | #include 11 | // #include 12 | #include 13 | #include 14 | 15 | #include 16 | #include 17 | 18 | namespace Common_tools 19 | { 20 | typedef CGAL::Simple_cartesian CGAL_Kernel; 21 | // typedef CGAL::Exact_predicates_inexact_constructions_kernel CGAL_Kernel; 22 | // typedef CGAL::Exact_predicates_exact_constructions_kernel CGAL_Kernel; 23 | typedef CGAL_Kernel::Point_2 Point_2; 24 | typedef CGAL_Kernel::Segment_2 Segment_2; 25 | typedef CGAL_Kernel::Line_2 Line_2; 26 | typedef CGAL_Kernel::Intersect_2 Intersect_2; 27 | typedef CGAL_Kernel::Triangle_2 Triangle_2; 28 | typedef CGAL::Polygon_2< CGAL_Kernel > Polygon_2; 29 | 30 | typedef CGAL_Kernel::Point_3 Point_3; 31 | typedef CGAL_Kernel::Triangle_3 Triangle_3; 32 | typedef CGAL_Kernel::Segment_3 Segment_3; 33 | 34 | typedef CGAL::Convex_hull_traits_adapter_2< CGAL_Kernel, CGAL::Pointer_property_map< Point_2 >::type > Convex_hull_traits_2; 35 | 36 | typedef CGAL::Triangulation_vertex_base_with_info_2< unsigned, CGAL_Kernel > TVb2; 37 | typedef CGAL::Triangulation_data_structure_2< TVb2 > Tds2; 38 | typedef CGAL::Delaunay_triangulation_2< CGAL_Kernel, Tds2 > Delaunay2; 39 | typedef Delaunay2::Point D2_Point; 40 | typedef Delaunay2::Vertex_handle D2_Vertex_handle; 41 | typedef CGAL::Convex_hull_traits_adapter_2< CGAL_Kernel, CGAL::Pointer_property_map< D2_Point >::type > Convex_hull_traits_2; 42 | 43 | using std::cout; 44 | using std::endl; 45 | 46 | void inline printf_triangle_pair( Triangle_2 &triangle_a, Triangle_2 &triangle_b ) 47 | { 48 | // clang-format off 49 | printf("Tri {(%.2f, %.2f), (%.2f, %.2f), (%.2f, %.2f)} V.S. {(%.2f, %.2f), (%.2f, %.2f), (%.2f, %.2f)}", 50 | triangle_a.vertex(0).x(), triangle_a.vertex(0).y(), 51 | triangle_a.vertex(1).x(), triangle_a.vertex(1).y(), 52 | triangle_a.vertex(2).x(), triangle_a.vertex(2).y(), 53 | triangle_b.vertex(0).x(), triangle_b.vertex(0).y(), 54 | triangle_a.vertex(1).x(), triangle_b.vertex(1).y(), 55 | triangle_b.vertex(2).x(), triangle_b.vertex(2).y() ); 56 | // clang-format on 57 | } 58 | 59 | inline int triangle_intersect_triangle( Triangle_2 &triangle_a, Triangle_2 &triangle_b, double area_threshold = 0.001 ) 60 | { 61 | CGAL::Intersection_traits< CGAL_Kernel, Triangle_2, Triangle_2 >::result_type result = CGAL::intersection( triangle_a, triangle_b ); 62 | 63 | // cout << "=== Test triangle intersection result ===" << endl; 64 | // printf_triangle_pair(triangle_a, triangle_b); 65 | if ( result ) 66 | { 67 | // cout << "Intersect!" << endl; 68 | if ( const Point_2 *p = boost::get< Point_2 >( &*result ) ) 69 | { 70 | // const Point_2 *p = boost::get< Point_2 >( &*result ); 71 | // std::cout << "Intersection as points: "<< *p << std::endl; 72 | return 1; 73 | } 74 | else if ( const Segment_2 *s = boost::get< Segment_2 >( &*result ) ) 75 | { 76 | // std::cout << "Intersection as Segment: " << *s << std::endl; 77 | return 2; 78 | } 79 | else if ( const Triangle_2 *tri = boost::get< Triangle_2 >( &*result ) ) 80 | { 81 | // const Point_2 *p = boost::get< Point_2 >( &*result ); 82 | // printf("Intersection as triangle: {(%.2f, %.2f), (%.2f, %.2f), (%.2f, %.2f)}\r\n", 83 | // (*tri).vertex(0).x(), (*tri).vertex(0).y(), 84 | // (*tri).vertex(1).x(), (*tri).vertex(1).y(), 85 | // (*tri).vertex(2).x(), (*tri).vertex(2).y() ); 86 | // std::cout << "Intersection as triangle: "<< *tri << ", area = " << (*tri).area() << std::endl; 87 | if((*tri).area() < area_threshold) 88 | { 89 | return 0; 90 | } 91 | std::cout << "Intersection as triangle: "<< *tri << ", area = " << (*tri).area() << std::endl; 92 | return 3; 93 | } 94 | else if ( const std::vector *vec_pts = boost::get< std::vector >( &*result ) ) 95 | { 96 | Polygon_2 poly(vec_pts->begin(), vec_pts->end()); 97 | // cout << "Intersection as polygon: "<< vec_pts->size() << ", area = " << poly.area() << endl; 98 | // for(int i =0 ;i < (*vec_pts).size(); i++) 99 | // { 100 | // cout << "("<< (*vec_pts)[i] << "), "; 101 | // } 102 | // cout << endl; 103 | if( poly.area() < area_threshold) 104 | { 105 | return 0; 106 | } 107 | cout << "Intersection as polygon: "<< vec_pts->size() << ", area = " << poly.area() << endl; 108 | return (4 + vec_pts->size()); 109 | } 110 | } 111 | else 112 | { 113 | // cout << "No intersection" << endl; 114 | return 0; 115 | } 116 | cout << "=== Test triangle intersection end ===" << endl; 117 | 118 | } 119 | } // namespace Common_tools -------------------------------------------------------------------------------- /src/tools/tools_openCV_3_to_4.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | This code is the implementation of our paper "R3LIVE: A Robust, Real-time, RGB-colored, 3 | LiDAR-Inertial-Visual tightly-coupled state Estimation and mapping package". 4 | 5 | Author: Jiarong Lin < ziv.lin.ljr@gmail.com > 6 | 7 | If you use any code of this repo in your academic research, please cite at least 8 | one of our papers: 9 | [1] Lin, Jiarong, and Fu Zhang. "R3LIVE: A Robust, Real-time, RGB-colored, 10 | LiDAR-Inertial-Visual tightly-coupled state Estimation and mapping package." 11 | [2] Xu, Wei, et al. "Fast-lio2: Fast direct lidar-inertial odometry." 12 | [3] Lin, Jiarong, et al. "R2LIVE: A Robust, Real-time, LiDAR-Inertial-Visual 13 | tightly-coupled state Estimator and mapping." 14 | [4] Xu, Wei, and Fu Zhang. "Fast-lio: A fast, robust lidar-inertial odometry 15 | package by tightly-coupled iterated kalman filter." 16 | [5] Cai, Yixi, Wei Xu, and Fu Zhang. "ikd-Tree: An Incremental KD Tree for 17 | Robotic Applications." 18 | [6] Lin, Jiarong, and Fu Zhang. "Loam-livox: A fast, robust, high-precision 19 | LiDAR odometry and mapping package for LiDARs of small FoV." 20 | 21 | For commercial use, please contact me < ziv.lin.ljr@gmail.com > and 22 | Dr. Fu Zhang < fuzhang@hku.hk >. 23 | 24 | Redistribution and use in source and binary forms, with or without 25 | modification, are permitted provided that the following conditions are met: 26 | 27 | 1. Redistributions of source code must retain the above copyright notice, 28 | this list of conditions and the following disclaimer. 29 | 2. Redistributions in binary form must reproduce the above copyright notice, 30 | this list of conditions and the following disclaimer in the documentation 31 | and/or other materials provided with the distribution. 32 | 3. Neither the name of the copyright holder nor the names of its 33 | contributors may be used to endorse or promote products derived from this 34 | software without specific prior written permission. 35 | 36 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 37 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 38 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 39 | ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 40 | LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 41 | CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 42 | SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 43 | INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 44 | CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 45 | ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 46 | POSSIBILITY OF SUCH DAMAGE. 47 | */ 48 | #pragma once 49 | #include "tools_color_printf.hpp" 50 | #include 51 | #include 52 | 53 | // This file aim at making captable between openCV 3.0 and openCV 4.0 54 | // TODO 55 | 56 | #if (CV_MAJOR_VERSION==4) 57 | #define CV_FM_8POINT cv::FM_8POINT 58 | #define CV_AA cv::LINE_AA 59 | // "cv::COLOR_RGB2GRAY" @ openCV 4.0 60 | // #define CV_RGB2GRAY 7 61 | #define CV_CAP_PROP_FRAME_HEIGHT CAP_PROP_FRAME_HEIGHT 62 | #define CV_CAP_PROP_FRAME_WIDTH CAP_PROP_FRAME_WIDTH 63 | #define CAP_PROP_FRAME_COUNT CAP_PROP_FPS 64 | //#define CV_RGB2GRAY COLOR_RGB2GRAY 65 | #endif 66 | 67 | -------------------------------------------------------------------------------- /src/tools/tools_ros.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | This code is the implementation of our paper "R3LIVE: A Robust, Real-time, RGB-colored, 3 | LiDAR-Inertial-Visual tightly-coupled state Estimation and mapping package". 4 | 5 | Author: Jiarong Lin < ziv.lin.ljr@gmail.com > 6 | 7 | If you use any code of this repo in your academic research, please cite at least 8 | one of our papers: 9 | [1] Lin, Jiarong, and Fu Zhang. "R3LIVE: A Robust, Real-time, RGB-colored, 10 | LiDAR-Inertial-Visual tightly-coupled state Estimation and mapping package." 11 | [2] Xu, Wei, et al. "Fast-lio2: Fast direct lidar-inertial odometry." 12 | [3] Lin, Jiarong, et al. "R2LIVE: A Robust, Real-time, LiDAR-Inertial-Visual 13 | tightly-coupled state Estimator and mapping." 14 | [4] Xu, Wei, and Fu Zhang. "Fast-lio: A fast, robust lidar-inertial odometry 15 | package by tightly-coupled iterated kalman filter." 16 | [5] Cai, Yixi, Wei Xu, and Fu Zhang. "ikd-Tree: An Incremental KD Tree for 17 | Robotic Applications." 18 | [6] Lin, Jiarong, and Fu Zhang. "Loam-livox: A fast, robust, high-precision 19 | LiDAR odometry and mapping package for LiDARs of small FoV." 20 | 21 | For commercial use, please contact me < ziv.lin.ljr@gmail.com > and 22 | Dr. Fu Zhang < fuzhang@hku.hk >. 23 | 24 | Redistribution and use in source and binary forms, with or without 25 | modification, are permitted provided that the following conditions are met: 26 | 27 | 1. Redistributions of source code must retain the above copyright notice, 28 | this list of conditions and the following disclaimer. 29 | 2. Redistributions in binary form must reproduce the above copyright notice, 30 | this list of conditions and the following disclaimer in the documentation 31 | and/or other materials provided with the distribution. 32 | 3. Neither the name of the copyright holder nor the names of its 33 | contributors may be used to endorse or promote products derived from this 34 | software without specific prior written permission. 35 | 36 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 37 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 38 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 39 | ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 40 | LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 41 | CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 42 | SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 43 | INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 44 | CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 45 | ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 46 | POSSIBILITY OF SUCH DAMAGE. 47 | */ 48 | #ifndef __TOOLS_ROS_HPP__ 49 | #define __TOOLS_ROS_HPP__ 50 | #include 51 | #include 52 | #include 53 | 54 | using std::cout; 55 | using std::endl; 56 | 57 | namespace Common_tools 58 | { 59 | template < typename T > 60 | inline T get_ros_parameter( ros::NodeHandle &nh, const std::string parameter_name, T ¶meter, T default_val ) 61 | { 62 | nh.param< T >( parameter_name.c_str(), parameter, default_val ); 63 | // ENABLE_SCREEN_PRINTF; 64 | cout << "[Ros_parameter]: " << parameter_name << " ==> " << parameter << std::endl; 65 | return parameter; 66 | } 67 | 68 | template < typename T > 69 | inline std::vector< T > get_ros_parameter_array( ros::NodeHandle &nh, const std::string parameter_name ) 70 | { 71 | std::vector< T > config_vector; 72 | nh.getParam( parameter_name, config_vector ); 73 | cout << "[Ros_configurations]: " << parameter_name << ":{ "; 74 | for(int i =0 ; i < config_vector.size(); i++) 75 | { 76 | cout << config_vector[i] << ", "; 77 | } 78 | cout << "\b\b" << " }" << endl; 79 | return config_vector; 80 | } 81 | } 82 | 83 | using Common_tools::get_ros_parameter; 84 | 85 | #endif -------------------------------------------------------------------------------- /supply/Supplementary_material.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/hku-mars/ImMesh/3bbc89e4cd2acfa4797834e79b06e8e86b3fa891/supply/Supplementary_material.pdf --------------------------------------------------------------------------------