├── .clang-format ├── .gitignore ├── .gitmodules ├── CMakeLists.txt ├── LICENSE ├── README.md ├── cmake ├── FindGLM.cmake ├── FindGlog.cmake └── getGitInfo.cmake ├── config ├── calib.json ├── camera_config │ ├── F4_FE_calib │ ├── F4_left.yaml │ ├── F4_left_calib.yaml │ ├── FE_right.yaml │ ├── cl_realsense_480p_1.yaml │ ├── computer.yaml │ ├── dabai_right.yaml │ ├── data2.yaml │ ├── equidistant.yaml │ ├── fov.yaml │ ├── mei.yaml │ ├── pinhole.yaml │ ├── red_circ_1.yaml │ ├── tc_realsense_480p_0.yaml │ ├── tc_realsense_480p_1.yaml │ └── test.yaml ├── data_test.json ├── ext1.json ├── ext2.json └── pts.json ├── data ├── UbuntuMono-R.ttf ├── cali_data │ ├── camera_calib │ │ └── data.json │ ├── laser_cam_right.json │ └── red_cicle_calc_1.json ├── imgs │ ├── april_board.jpg │ ├── april_board.png │ ├── blob_board.png │ └── 图片生成.pptx ├── models │ ├── Intel_RealSense_Depth_Camera_D435.ply │ └── SIMO-LS-20S.ply ├── shader │ ├── font.frag │ ├── font.vert │ ├── rainbow.frag │ ├── rainbow.vert │ ├── texture.frag │ └── texture.vert └── times.ttf ├── doc ├── class.mmd ├── classDiagram.drawio ├── imgs │ ├── 1.png │ └── 2.png └── module.km ├── include ├── algorithm │ ├── laser_cam_ceres.h │ ├── line_detector.h │ ├── pose_local_parameterization.h │ ├── random │ │ ├── RandomGenerators.h │ │ └── random_shuffle.h │ ├── ransac │ │ ├── ransac.h │ │ ├── ransac_applications.h │ │ └── ransac_impl.h │ ├── two_cameras_ceres.h │ ├── two_lasers_ceres.h │ └── util.h ├── calibration │ ├── calib_base.hpp │ ├── calibration_state.hpp │ ├── camera_calib.hpp │ ├── camera_laser_calib.hpp │ ├── task_back_ground.hpp │ ├── two_cameras_calib.hpp │ └── two_lasers_calib.hpp ├── dev │ ├── april_board.hpp │ ├── blob_board.hpp │ ├── board.hpp │ ├── camera.hpp │ ├── chess_board.hpp │ ├── image_show.hpp │ ├── laser.hpp │ ├── sensor.hpp │ ├── sensor_data.hpp │ ├── sensor_manager.hpp │ └── util.hpp ├── glk │ ├── colormap.hpp │ ├── drawble.hpp │ ├── frame_buffer.hpp │ ├── glsl_shader.hpp │ ├── lines.hpp │ ├── loaders │ │ ├── miniply.h │ │ └── ply_loader.hpp │ ├── mesh.hpp │ ├── mesh_utils.hpp │ ├── pointcloud_buffer.hpp │ ├── primitives │ │ ├── cone.hpp │ │ ├── coordinate_system.hpp │ │ ├── cube.hpp │ │ ├── grid.hpp │ │ ├── icosahedron.hpp │ │ └── primitives.hpp │ ├── simple_lines.hpp │ ├── text_renderer.hpp │ ├── texture.hpp │ └── texture_renderer.hpp ├── guik │ ├── camera_control.hpp │ ├── gl_canvas.hpp │ ├── imgui_application.hpp │ ├── progress_interface.hpp │ ├── progress_modal.hpp │ └── projection_control.hpp ├── robot_basic_tools.hpp ├── util │ ├── draw_frame_selector.hpp │ ├── extrinsics_publisher.hpp │ ├── image_loader.hpp │ ├── singleton.hpp │ └── tf_tree.hpp └── version_info.h.in ├── launch ├── run.launch └── sim.launch ├── package.xml ├── src ├── algorithm │ ├── laser_cam_ceres.cpp │ ├── line_detector.cpp │ ├── line_fitting.cpp │ ├── random │ │ └── RandomGenerator.cpp │ ├── ransac │ │ └── ransac_applications.cpp │ ├── two_cameras_ceres.cpp │ ├── two_lasers_ceres.cpp │ └── util.cpp ├── calibration │ ├── calib_base.cpp │ ├── calibration_state.cpp │ ├── camera_calib.cpp │ ├── camera_laser_calib.cpp │ ├── two_cameras_calib.cpp │ └── two_lasers_calib.cpp ├── dev │ ├── april_board.cpp │ ├── blob_board.cpp │ ├── camera.cpp │ ├── chess_board.cpp │ ├── image_show.cpp │ ├── laser.cpp │ ├── sensor.cpp │ ├── sensor_manager.cpp │ └── util.cpp ├── glk │ ├── colormap.cpp │ ├── frame_buffer.cpp │ ├── glsl_shader.cpp │ ├── lines.cpp │ ├── loaders │ │ ├── miniply.cpp │ │ └── ply_loader.cpp │ ├── mesh.cpp │ ├── pointcloud_buffer.cpp │ ├── primitives │ │ └── primitives.cpp │ └── simple_lines.cpp ├── guik │ ├── camera_control.cpp │ ├── gl_canvas.cpp │ ├── imgui_application.cpp │ └── projection_control.cpp ├── robot_basic_tools.cpp └── util │ ├── extrinsics_publisher.cpp │ ├── image_loader.cpp │ └── tf_tree.cpp ├── srv └── Extrinsic.srv └── thirdparty ├── camera_model ├── CMakeLists.txt ├── include │ └── camera_model │ │ ├── apriltag │ │ ├── Edge.h │ │ ├── FloatImage.h │ │ ├── GLine2D.h │ │ ├── GLineSegment2D.h │ │ ├── Gaussian.h │ │ ├── GrayModel.h │ │ ├── Gridder.h │ │ ├── Homography33.h │ │ ├── MathUtil.h │ │ ├── Quad.h │ │ ├── Segment.h │ │ ├── Tag16h5.h │ │ ├── Tag16h5_other.h │ │ ├── Tag25h7.h │ │ ├── Tag25h9.h │ │ ├── Tag36h11.h │ │ ├── Tag36h11_other.h │ │ ├── Tag36h9.h │ │ ├── TagDetection.h │ │ ├── TagDetector.h │ │ ├── TagFamily.h │ │ ├── UnionFindSimple.h │ │ ├── XYWeight.h │ │ └── pch.h │ │ ├── apriltag_frontend │ │ ├── GridCalibrationTargetAprilgrid.hpp │ │ └── GridCalibrationTargetBase.hpp │ │ ├── calib │ │ ├── CameraCalibration.h │ │ └── CameraCalibrationTest.h │ │ ├── camera_models │ │ ├── Camera.h │ │ ├── CameraFactory.h │ │ ├── CataCamera.h │ │ ├── CostFunctionFactory.h │ │ ├── EquidistantCamera.h │ │ ├── FovCamera.h │ │ ├── PinholeCamera.h │ │ ├── PinholeFullCamera.h │ │ ├── PolyFisheyeCamera.h │ │ ├── ScaramuzzaCamera.h │ │ └── SplineCamera.h │ │ ├── chessboard │ │ ├── Chessboard.h │ │ ├── ChessboardCorner.h │ │ ├── ChessboardQuad.h │ │ └── Spline.h │ │ ├── code_utils │ │ ├── cv_utils.h │ │ ├── eigen_utils.h │ │ ├── math_utils │ │ │ ├── Polynomial.h │ │ │ └── math_utils.h │ │ ├── ros_utils.h │ │ └── sys_utils.h │ │ ├── gpl │ │ ├── EigenQuaternionParameterization.h │ │ ├── EigenUtils.h │ │ └── gpl.h │ │ └── sparse_graph │ │ └── Transform.h ├── readme.md └── src │ ├── april_calib.cc │ ├── apriltag │ ├── Edge.cc │ ├── FloatImage.cc │ ├── GLine2D.cc │ ├── GLineSegment2D.cc │ ├── Gaussian.cc │ ├── GrayModel.cc │ ├── Homography33.cc │ ├── MathUtil.cc │ ├── Quad.cc │ ├── Segment.cc │ ├── TagDetection.cc │ ├── TagDetector.cc │ ├── TagFamily.cc │ └── UnionFindSimple.cc │ ├── apriltag_frontend │ ├── GridCalibrationTargetAprilgrid.cpp │ └── GridCalibrationTargetBase.cpp │ ├── backward.hpp │ ├── calib │ ├── CameraCalibration.cc │ └── CameraCalibrationTest.cc │ ├── camera_models │ ├── Camera.cc │ ├── CameraFactory.cc │ ├── CataCamera.cc │ ├── CostFunctionFactory.cc │ ├── EquidistantCamera.cc │ ├── FovCamera.cpp │ ├── PinholeCamera.cc │ ├── PinholeFullCamera.cc │ ├── PolyFisheyeCamera.cpp │ ├── ScaramuzzaCamera.cc │ └── SplineCamera.cpp │ ├── chessboard │ └── Chessboard.cc │ ├── code_utils │ ├── cv_utils.cc │ ├── math_utils │ │ └── Polynomial.cpp │ └── poly_test.cpp │ ├── gpl │ ├── EigenQuaternionParameterization.cc │ └── gpl.cc │ ├── intrinsic_calib.cc │ ├── sparse_graph │ └── Transform.cc │ └── test_calib.cc ├── cv_bridge_rbt ├── CMakeLists.txt ├── include │ └── cv_bridge_rbt │ │ ├── cv_bridge.h │ │ └── rgb_colors.h ├── readme.md └── src │ ├── cv_bridge.cpp │ └── rgb_colors.cpp ├── gl3w ├── GL │ ├── gl3w.h │ └── glcorearb.h ├── KHR │ └── khrplatform.h ├── gl3w.c └── readme ├── json └── nlohmann │ ├── adl_serializer.hpp │ ├── byte_container_with_subtype.hpp │ ├── detail │ ├── conversions │ │ ├── from_json.hpp │ │ ├── to_chars.hpp │ │ └── to_json.hpp │ ├── exceptions.hpp │ ├── hash.hpp │ ├── input │ │ ├── binary_reader.hpp │ │ ├── input_adapters.hpp │ │ ├── json_sax.hpp │ │ ├── lexer.hpp │ │ ├── parser.hpp │ │ └── position_t.hpp │ ├── iterators │ │ ├── internal_iterator.hpp │ │ ├── iter_impl.hpp │ │ ├── iteration_proxy.hpp │ │ ├── iterator_traits.hpp │ │ ├── json_reverse_iterator.hpp │ │ └── primitive_iterator.hpp │ ├── json_pointer.hpp │ ├── json_ref.hpp │ ├── macro_scope.hpp │ ├── macro_unscope.hpp │ ├── meta │ │ ├── cpp_future.hpp │ │ ├── detected.hpp │ │ ├── is_sax.hpp │ │ ├── type_traits.hpp │ │ └── void_t.hpp │ ├── output │ │ ├── binary_writer.hpp │ │ ├── output_adapters.hpp │ │ └── serializer.hpp │ └── value_t.hpp │ ├── json.hpp │ ├── json_fwd.hpp │ ├── ordered_map.hpp │ └── thirdparty │ └── hedley │ ├── hedley.hpp │ └── hedley_undef.hpp └── public ├── stb_image.h └── stb_image_write.h /.gitignore: -------------------------------------------------------------------------------- 1 | .idea/ 2 | build/ 3 | cmake-build-debug/ 4 | .vscode/ 5 | data/calib_data/ 6 | *.ini 7 | gitstats/ 8 | include/version_info.h 9 | doc/*.png 10 | .lib_path_config 11 | 12 | # Prerequisites 13 | *.d 14 | 15 | # Compiled Object files 16 | *.slo 17 | *.lo 18 | *.o 19 | *.obj 20 | 21 | # Precompiled Headers 22 | *.gch 23 | *.pch 24 | 25 | # Compiled Dynamic libraries 26 | *.so 27 | *.dylib 28 | *.dll 29 | 30 | # Fortran module files 31 | *.mod 32 | *.smod 33 | 34 | # Compiled Static libraries 35 | *.lai 36 | *.la 37 | *.a 38 | *.lib 39 | 40 | # Executables 41 | *.exe 42 | *.out 43 | *.app -------------------------------------------------------------------------------- /.gitmodules: -------------------------------------------------------------------------------- 1 | [submodule "thirdparty/imgui"] 2 | path = thirdparty/imgui 3 | url = https://github.com/ocornut/imgui 4 | [submodule "thirdparty/portable-file-dialogs"] 5 | path = thirdparty/portable-file-dialogs 6 | url = https://github.com/samhocevar/portable-file-dialogs 7 | -------------------------------------------------------------------------------- /cmake/FindGLM.cmake: -------------------------------------------------------------------------------- 1 | # FindGLM - attempts to locate the glm matrix/vector library. 2 | # 3 | # This module defines the following variables (on success): 4 | # GLM_INCLUDE_DIRS - where to find glm/glm.hpp 5 | # GLM_FOUND - if the library was successfully located 6 | # 7 | # It is trying a few standard installation locations, but can be customized 8 | # with the following variables: 9 | # GLM_ROOT_DIR - root directory of a glm installation 10 | # Headers are expected to be found in either: 11 | # /glm/glm.hpp OR 12 | # /include/glm/glm.hpp 13 | # This variable can either be a cmake or environment 14 | # variable. Note however that changing the value 15 | # of the environment varible will NOT result in 16 | # re-running the header search and therefore NOT 17 | # adjust the variables set by this module. 18 | #============================================================================= 19 | # Copyright 2012 Carsten Neumann 20 | # 21 | # Distributed under the OSI-approved BSD License (the "License"); 22 | # see accompanying file Copyright.txt for details. 23 | # 24 | # This software is distributed WITHOUT ANY WARRANTY; without even the 25 | # implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. 26 | # See the License for more information. 27 | #============================================================================= 28 | # (To distribute this file outside of CMake, substitute the full 29 | # License text for the above reference.) 30 | # default search dirs 31 | 32 | SET(_glm_HEADER_SEARCH_DIRS 33 | "/usr/include" 34 | "/usr/local/include" 35 | "${CMAKE_SOURCE_DIR}/includes" 36 | "C:/Program Files (x86)/glm") 37 | # check environment variable 38 | SET(_glm_ENV_ROOT_DIR "$ENV{GLM_ROOT_DIR}") 39 | IF (NOT GLM_ROOT_DIR AND _glm_ENV_ROOT_DIR) 40 | SET(GLM_ROOT_DIR "${_glm_ENV_ROOT_DIR}") 41 | ENDIF (NOT GLM_ROOT_DIR AND _glm_ENV_ROOT_DIR) 42 | # put user specified location at beginning of search 43 | IF (GLM_ROOT_DIR) 44 | SET(_glm_HEADER_SEARCH_DIRS "${GLM_ROOT_DIR}" 45 | "${GLM_ROOT_DIR}/include" 46 | ${_glm_HEADER_SEARCH_DIRS}) 47 | ENDIF (GLM_ROOT_DIR) 48 | # locate header 49 | FIND_PATH(GLM_INCLUDE_DIR "glm/glm.hpp" 50 | PATHS ${_glm_HEADER_SEARCH_DIRS}) 51 | INCLUDE(FindPackageHandleStandardArgs) 52 | FIND_PACKAGE_HANDLE_STANDARD_ARGS(GLM DEFAULT_MSG 53 | GLM_INCLUDE_DIR) 54 | IF (GLM_FOUND) 55 | SET(GLM_INCLUDE_DIRS "${GLM_INCLUDE_DIR}") 56 | MESSAGE(STATUS "GLM_INCLUDE_DIR = ${GLM_INCLUDE_DIR}") 57 | ENDIF (GLM_FOUND) 58 | -------------------------------------------------------------------------------- /cmake/getGitInfo.cmake: -------------------------------------------------------------------------------- 1 | # get git hash 2 | macro(get_git_hash _git_hash) # 宏的开始 3 | find_package(Git QUIET) # 查找Git，QUIET静默方式不报错 4 | if (GIT_FOUND) 5 | execute_process( # 执行一个子进程 6 | COMMAND ${GIT_EXECUTABLE} log -1 --pretty=format:%h # 命令 7 | OUTPUT_VARIABLE ${_git_hash} # 输出字符串存入变量 8 | OUTPUT_STRIP_TRAILING_WHITESPACE # 删除字符串尾的换行符 9 | ERROR_QUIET # 对执行错误静默 10 | WORKING_DIRECTORY # 执行路径 11 | ${CMAKE_CURRENT_SOURCE_DIR} 12 | ) 13 | endif () 14 | endmacro() # 宏的结束 15 | 16 | # get git branch 17 | macro(get_git_branch _git_branch) # 宏的开始 18 | find_package(Git QUIET) # 查找Git，QUIET静默方式不报错 19 | if (GIT_FOUND) 20 | execute_process( # 执行一个子进程 21 | COMMAND ${GIT_EXECUTABLE} symbolic-ref --short -q HEAD 22 | OUTPUT_VARIABLE ${_git_branch} # 输出字符串存入变量 23 | OUTPUT_STRIP_TRAILING_WHITESPACE # 删除字符串尾的换行符 24 | ERROR_QUIET # 对执行错误静默 25 | WORKING_DIRECTORY # 执行路径 26 | ${CMAKE_CURRENT_SOURCE_DIR} 27 | ) 28 | endif () 29 | endmacro() # 宏的结束 30 | -------------------------------------------------------------------------------- /config/calib.json: -------------------------------------------------------------------------------- 1 | { 2 | "data": { 3 | "pitch": -17.99040412902832, 4 | "roll": -6.946762561798096, 5 | "tx": 0.1124219223856926, 6 | "ty": -0.011084129102528095, 7 | "tz": -0.022970931604504585, 8 | "yaw": -5.115530490875244 9 | }, 10 | "frame": { 11 | "from_frame": "camera_left_rgb_optical_frame", 12 | "to_frame": "camera_right_rgb_optical_frame" 13 | }, 14 | "type": "two_cameras_extrinsics" 15 | } 16 | -------------------------------------------------------------------------------- /config/camera_config/F4_left.yaml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | --- 3 | model_type: PINHOLE 4 | camera_name: left 5 | image_width: 640 6 | image_height: 480 7 | distortion_parameters: 8 | k1: 0. 9 | k2: 0. 10 | p1: 0. 11 | p2: 0. 12 | projection_parameters: 13 | fx: 4.5221859741210938e+02 14 | fy: 4.5221859741210938e+02 15 | cx: 3.2293414306640625e+02 16 | cy: 2.3313650512695312e+02 17 | -------------------------------------------------------------------------------- /config/camera_config/F4_left_calib.yaml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | --- 3 | model_type: PINHOLE 4 | camera_name: left 5 | image_width: 640 6 | image_height: 480 7 | distortion_parameters: 8 | k1: 1.0281429359465862e-01 9 | k2: -1.5935772703911277e-01 10 | p1: -9.8570054826015337e-04 11 | p2: -3.0672152884772290e-03 12 | projection_parameters: 13 | fx: 4.6168366697530666e+02 14 | fy: 4.6319526545394530e+02 15 | cx: 3.2126483687022488e+02 16 | cy: 2.3062948491822735e+02 17 | -------------------------------------------------------------------------------- /config/camera_config/FE_right.yaml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | --- 3 | model_type: PINHOLE 4 | camera_name: right 5 | image_width: 640 6 | image_height: 480 7 | distortion_parameters: 8 | k1: 0. 9 | k2: 0. 10 | p1: 0. 11 | p2: 0. 12 | projection_parameters: 13 | fx: 4.5565319824218750e+02 14 | fy: 4.5565319824218750e+02 15 | cx: 3.3105447387695312e+02 16 | cy: 2.4101264953613281e+02 17 | -------------------------------------------------------------------------------- /config/camera_config/cl_realsense_480p_1.yaml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | --- 3 | model_type: PINHOLE 4 | camera_name: realsense_480p 5 | image_width: 640 6 | image_height: 480 7 | distortion_parameters: 8 | k1: 0. 9 | k2: 0. 10 | p1: 0. 11 | p2: 0. 12 | projection_parameters: 13 | fx: 615.536865234375 14 | fy: 614.5320434570312 15 | cx: 323.1097412109375 16 | cy: 233.85525512695312 17 | -------------------------------------------------------------------------------- /config/camera_config/computer.yaml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | --- 3 | model_type: PINHOLE 4 | camera_name: realsense_480p 5 | image_width: 640 6 | image_height: 480 7 | distortion_parameters: 8 | k1: 2.2371147018815964e-03 9 | k2: -5.4571102136116297e-07 10 | p1: 3.7218043925504908e-02 11 | p2: -3.4413064579289505e-02 12 | projection_parameters: 13 | fx: 1.1665188765553607e+02 14 | fy: 1.8375782870480887e+01 15 | cx: 5.6385085447791357e+02 16 | cy: -9.1861959658048568e+02 17 | -------------------------------------------------------------------------------- /config/camera_config/dabai_right.yaml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | --- 3 | model_type: PINHOLE 4 | camera_name: right 5 | image_width: 640 6 | image_height: 480 7 | distortion_parameters: 8 | k1: 9.4297019472541807e-02 9 | k2: -1.4222511336075405e-01 10 | p1: 2.3881095084687374e-03 11 | p2: 2.6923660239946585e-03 12 | projection_parameters: 13 | fx: 4.5993101912150360e+02 14 | fy: 4.5844762022237541e+02 15 | cx: 3.2987008754390706e+02 16 | cy: 2.4499237319247823e+02 17 | -------------------------------------------------------------------------------- /config/camera_config/data2.yaml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | --- 3 | model_type: PINHOLE 4 | camera_name: realsense_480p 5 | image_width: 640 6 | image_height: 480 7 | distortion_parameters: 8 | k1: 1.3328570471814288e-01 9 | k2: -3.6256886347690642e-01 10 | p1: 8.5441236117558589e-04 11 | p2: -5.2012167049520141e-03 12 | projection_parameters: 13 | fx: 7.4640931247416927e+02 14 | fy: 7.4567967232006185e+02 15 | cx: 2.9957395208670425e+02 16 | cy: 1.9409710418350852e+02 17 | -------------------------------------------------------------------------------- /config/camera_config/equidistant.yaml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | --- 3 | model_type: KANNALA_BRANDT 4 | camera_name: new camere 1 5 | image_width: 640 6 | image_height: 480 7 | projection_parameters: 8 | k2: 0. 9 | k3: 0. 10 | k4: 0. 11 | k5: 0. 12 | mu: 0. 13 | mv: 0. 14 | u0: 0. 15 | v0: 0. 16 | -------------------------------------------------------------------------------- /config/camera_config/fov.yaml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | model_type: FOV 3 | camera_name: tango_fov 4 | image_width: 640 5 | image_height: 480 6 | projection_parameters: 7 | fx: 255.944 8 | fy: 256.394 9 | u0: 317.557 10 | v0: 234.345 11 | omg: 0.925194 12 | -------------------------------------------------------------------------------- /config/camera_config/mei.yaml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | --- 3 | model_type: MEI 4 | camera_name: new camere 2 5 | image_width: 640 6 | image_height: 480 7 | mirror_parameters: 8 | xi: 0. 9 | distortion_parameters: 10 | k1: 0. 11 | k2: 0. 12 | p1: 0. 13 | p2: 0. 14 | projection_parameters: 15 | gamma1: 0. 16 | gamma2: 0. 17 | u0: 0. 18 | v0: 0. 19 | -------------------------------------------------------------------------------- /config/camera_config/pinhole.yaml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | --- 3 | model_type: PINHOLE 4 | camera_name: test 5 | image_width: 640 6 | image_height: 480 7 | distortion_parameters: 8 | k1: 0. 9 | k2: 0. 10 | p1: 0. 11 | p2: 0. 12 | projection_parameters: 13 | fx: 988. 14 | fy: 996. 15 | cx: 630. 16 | cy: 335. 17 | -------------------------------------------------------------------------------- /config/camera_config/red_circ_1.yaml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | --- 3 | model_type: PINHOLE 4 | camera_name: new camere 1 5 | image_width: 640 6 | image_height: 400 7 | distortion_parameters: 8 | k1: -4.6512119230995030e-01 9 | k2: 3.1290515299397736e-01 10 | p1: -3.3085104837033400e-04 11 | p2: -1.8987835492852499e-03 12 | projection_parameters: 13 | fx: 7.5663102568369436e+02 14 | fy: 7.5818172781637440e+02 15 | cx: 3.5070636125342628e+02 16 | cy: 2.4038832190630231e+02 17 | -------------------------------------------------------------------------------- /config/camera_config/tc_realsense_480p_0.yaml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | --- 3 | model_type: PINHOLE 4 | camera_name: tc_realsense_480p_0 5 | image_width: 640 6 | image_height: 480 7 | distortion_parameters: 8 | k1: 0. 9 | k2: 0. 10 | p1: 0. 11 | p2: 0. 12 | projection_parameters: 13 | fx: 602.4816284179688 14 | fy: 601.3778686523438 15 | cx: 329.1150207519531 16 | cy: 237.7969512939453 17 | -------------------------------------------------------------------------------- /config/camera_config/tc_realsense_480p_1.yaml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | --- 3 | model_type: PINHOLE 4 | camera_name: tc_realsense_480p_1 5 | image_width: 640 6 | image_height: 480 7 | distortion_parameters: 8 | k1: 0. 9 | k2: 0. 10 | p1: 0. 11 | p2: 0. 12 | projection_parameters: 13 | fx: 610.9580078125 14 | fy: 609.43505859375 15 | cx: 322.04974365234375 16 | cy: 254.81814575195312 17 | -------------------------------------------------------------------------------- /config/camera_config/test.yaml: -------------------------------------------------------------------------------- 1 | %YAML:1.0 2 | --- 3 | model_type: PINHOLE 4 | camera_name: left 5 | image_width: 640 6 | image_height: 480 7 | distortion_parameters: 8 | k1: 9.4297019472541807e-02 9 | k2: -1.4222511336075405e-01 10 | p1: 2.3881095084687374e-03 11 | p2: 2.6923660239946585e-03 12 | projection_parameters: 13 | fx: 4.5993101912150360e+02 14 | fy: 4.5844762022237541e+02 15 | cx: 3.2987008754390706e+02 16 | cy: 2.4499237319247823e+02 17 | -------------------------------------------------------------------------------- /config/ext1.json: -------------------------------------------------------------------------------- 1 | { 2 | "data": { 3 | "tx": 0.09682229906320572 4 | }, 5 | "type": "two_cameras_extrinsics" 6 | } 7 | -------------------------------------------------------------------------------- /config/ext2.json: -------------------------------------------------------------------------------- 1 | { 2 | "data": { 3 | "pitch": -47.61869812011719, 4 | "roll": -3.5748493671417236, 5 | "tx": 0.09682229906320572, 6 | "ty": -0.0005089850747026503, 7 | "tz": 0.03531910106539726, 8 | "yaw": -5.237128257751465 9 | }, 10 | "type": "two_cameras_extrinsic" 11 | } 12 | -------------------------------------------------------------------------------- /config/pts.json: -------------------------------------------------------------------------------- 1 | { 2 | "data": { 3 | "pitch": -7.12496280670166, 4 | "roll": -5.552037715911865, 5 | "tx": 0.09644882380962372, 6 | "ty": -0.006567069329321384, 7 | "tz": -0.01374613307416439, 8 | "yaw": -9.05516529083252 9 | }, 10 | "frame": { 11 | "from_frame": "camera_right_rgb_optical_frame", 12 | "to_frame": "camera_left_rgb_optical_frame" 13 | }, 14 | "type": "two_cameras_extrinsics" 15 | } 16 | -------------------------------------------------------------------------------- /data/UbuntuMono-R.ttf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/hardjet/robot_basic_tools/8260bcf46f68e7711319f8067c963748e68ce984/data/UbuntuMono-R.ttf -------------------------------------------------------------------------------- /data/imgs/april_board.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/hardjet/robot_basic_tools/8260bcf46f68e7711319f8067c963748e68ce984/data/imgs/april_board.jpg -------------------------------------------------------------------------------- /data/imgs/april_board.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/hardjet/robot_basic_tools/8260bcf46f68e7711319f8067c963748e68ce984/data/imgs/april_board.png -------------------------------------------------------------------------------- /data/imgs/blob_board.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/hardjet/robot_basic_tools/8260bcf46f68e7711319f8067c963748e68ce984/data/imgs/blob_board.png -------------------------------------------------------------------------------- /data/imgs/图片生成.pptx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/hardjet/robot_basic_tools/8260bcf46f68e7711319f8067c963748e68ce984/data/imgs/图片生成.pptx -------------------------------------------------------------------------------- /data/models/Intel_RealSense_Depth_Camera_D435.ply: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/hardjet/robot_basic_tools/8260bcf46f68e7711319f8067c963748e68ce984/data/models/Intel_RealSense_Depth_Camera_D435.ply -------------------------------------------------------------------------------- /data/models/SIMO-LS-20S.ply: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/hardjet/robot_basic_tools/8260bcf46f68e7711319f8067c963748e68ce984/data/models/SIMO-LS-20S.ply -------------------------------------------------------------------------------- /data/shader/font.frag: -------------------------------------------------------------------------------- 1 | #version 330 core 2 | in vec2 TexCoords; 3 | out vec4 color; 4 | 5 | uniform sampler2D text; 6 | uniform vec3 textColor; 7 | 8 | void main() 9 | { 10 | vec4 sampled = vec4(1.0, 1.0, 1.0, texture(text, TexCoords).r); 11 | color = vec4(textColor, 1.0) * sampled; 12 | } 13 | 14 | -------------------------------------------------------------------------------- /data/shader/font.vert: -------------------------------------------------------------------------------- 1 | #version 330 core 2 | layout (location = 0) in vec4 vertex; // 3 | out vec2 TexCoords; 4 | 5 | uniform mat4 projection; 6 | 7 | void main() 8 | { 9 | gl_Position = projection * vec4(vertex.xy, 0.0, 1.0); 10 | TexCoords = vertex.zw; 11 | } 12 | 13 | -------------------------------------------------------------------------------- /data/shader/rainbow.frag: -------------------------------------------------------------------------------- 1 | #version 330 2 | uniform vec2 z_range; 3 | uniform int z_clipping; 4 | uniform int color_mode; 5 | 6 | in vec4 frag_color; 7 | flat in ivec4 frag_info; 8 | in vec3 frag_world_position; 9 | 10 | layout (location=0) out vec4 color; 11 | layout (location=1) out ivec4 info; 12 | 13 | void main() { 14 | if(z_clipping != 0 && color_mode == 0 && (frag_world_position.z < z_range[0] || frag_world_position.z > z_range[1])) { 15 | discard; 16 | } 17 | 18 | color = frag_color; 19 | info = frag_info; 20 | } -------------------------------------------------------------------------------- /data/shader/rainbow.vert: -------------------------------------------------------------------------------- 1 | #version 330 2 | uniform float point_size; 3 | uniform float point_scale; 4 | uniform mat4 model_matrix; 5 | uniform mat4 view_matrix; 6 | uniform mat4 projection_matrix; 7 | 8 | uniform int color_mode; 9 | uniform vec4 material_color; 10 | 11 | uniform vec2 z_range; 12 | uniform ivec4 info_values; 13 | 14 | in vec3 vert_position; 15 | //in vec3 vert_direction; // line direction 16 | in vec4 vert_color; 17 | in ivec4 vert_info; 18 | 19 | out vec4 frag_color; 20 | flat out ivec4 frag_info; 21 | out vec3 frag_world_position; 22 | 23 | vec3 turbo(in float x) { 24 | const vec4 kRedVec4 = vec4(0.13572138, 4.61539260, -42.66032258, 132.13108234); 25 | const vec4 kGreenVec4 = vec4(0.09140261, 2.19418839, 4.84296658, -14.18503333); 26 | const vec4 kBlueVec4 = vec4(0.10667330, 12.64194608, -60.58204836, 110.36276771); 27 | const vec2 kRedVec2 = vec2(-152.94239396, 59.28637943); 28 | const vec2 kGreenVec2 = vec2(4.27729857, 2.82956604); 29 | const vec2 kBlueVec2 = vec2(-89.90310912, 27.34824973); 30 | 31 | x = clamp(x, 0.0, 1.0); 32 | vec4 v4 = vec4(1.0, x, x * x, x * x * x); 33 | vec2 v2 = v4.zw * v4.z; 34 | return vec3( 35 | dot(v4, kRedVec4) + dot(v2, kRedVec2), 36 | dot(v4, kGreenVec4) + dot(v2, kGreenVec2), 37 | dot(v4, kBlueVec4) + dot(v2, kBlueVec2) 38 | ); 39 | } 40 | 41 | vec4 rainbow(vec3 position) { 42 | float p = (position.z - z_range[0]) / (z_range[1] - z_range[0]); 43 | return vec4(turbo(p), 1.0); 44 | } 45 | 46 | void main() { 47 | vec4 world_position = model_matrix * vec4(vert_position, 1.0); 48 | frag_world_position = world_position.xyz; 49 | gl_Position = projection_matrix * view_matrix * world_position; 50 | 51 | frag_info = info_values; 52 | if(color_mode == 0) { 53 | frag_color = rainbow(frag_world_position); 54 | } else if(color_mode == 1) { 55 | frag_color = material_color; 56 | } else if(color_mode == 2) { 57 | frag_color = vert_color; 58 | frag_info = vert_info; 59 | } 60 | 61 | vec3 ndc = gl_Position.xyz / gl_Position.w; 62 | float z_dist = 1.0 - ndc.z; 63 | gl_PointSize = point_scale * point_size * z_dist; 64 | } -------------------------------------------------------------------------------- /data/shader/texture.frag: -------------------------------------------------------------------------------- 1 | #version 330 2 | uniform sampler2D texture_sampler; 3 | 4 | in vec2 texcoord; 5 | out vec4 color; 6 | 7 | void main() { 8 | // color = vec4(texcoord.xy, 0.0, 1.0); 9 | color = texture(texture_sampler, texcoord); 10 | } 11 | -------------------------------------------------------------------------------- /data/shader/texture.vert: -------------------------------------------------------------------------------- 1 | #version 330 2 | in vec3 vert_position; 3 | out vec2 texcoord; 4 | 5 | void main() { 6 | texcoord = 0.5 * vert_position.xy + vec2(0.5, 0.5); 7 | gl_Position = vec4(vert_position, 1.0); 8 | } 9 | -------------------------------------------------------------------------------- /data/times.ttf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/hardjet/robot_basic_tools/8260bcf46f68e7711319f8067c963748e68ce984/data/times.ttf -------------------------------------------------------------------------------- /doc/imgs/1.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/hardjet/robot_basic_tools/8260bcf46f68e7711319f8067c963748e68ce984/doc/imgs/1.png -------------------------------------------------------------------------------- /doc/imgs/2.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/hardjet/robot_basic_tools/8260bcf46f68e7711319f8067c963748e68ce984/doc/imgs/2.png -------------------------------------------------------------------------------- /include/algorithm/laser_cam_ceres.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | 6 | namespace algorithm { 7 | 8 | struct Observation { 9 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 10 | Observation() { 11 | tag_pose_q_ca = Eigen::Quaterniond(1, 0, 0, 0); 12 | tag_pose_t_ca = Eigen::Vector3d::Zero(); 13 | } 14 | // april board在相机坐标系下的位姿 15 | Eigen::Quaterniond tag_pose_q_ca; 16 | Eigen::Vector3d tag_pose_t_ca; 17 | // 该时刻激光点云的数据 18 | std::vector points; 19 | // 该时刻激光点云的数据 20 | std::vector points_on_line; 21 | }; 22 | 23 | void CamLaserCalClosedSolution(const std::vector& obs, Eigen::Matrix4d& Tlc); 24 | void CamLaserCalibration(const std::vector& obs, Eigen::Matrix4d& Trc, bool use_linefitting_data = true, 25 | bool use_boundary_constraint = false); 26 | 27 | } // namespace algorithm 28 | -------------------------------------------------------------------------------- /include/algorithm/line_detector.h: -------------------------------------------------------------------------------- 1 | 2 | #include 3 | #include 4 | #include 5 | 6 | namespace cv { 7 | class Mat; 8 | } 9 | 10 | namespace algorithm { 11 | class LineDetector { 12 | public: 13 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 14 | LineDetector(const sensor_msgs::LaserScan& scan, double angle_range, double max_range); 15 | LineDetector(const sensor_msgs::LaserScan& scan, double angle_range, double max_range, int point_limit, 16 | int inlier_limit, double thd_dist, double z, Eigen::Matrix r, Eigen::Vector3d t, int id); 17 | 18 | /** 19 | * 使用ransac找直线点 20 | * @param line_pts 21 | * @param line_params 22 | * @param img 23 | * @param dist_thd 点到直线的距离门限 24 | * @param min_num_of_pts 直线最少包含的点数 25 | * @return 26 | */ 27 | bool find_line_ransac(std::vector& line_pts, Eigen::Vector3d& line_params, cv::Mat& img, 28 | double dist_thd = 0.05, uint32_t min_num_of_pts = 50) const; 29 | 30 | /// 找直线点 31 | bool find_line(std::vector& line_pts, Eigen::Vector3d& line_params, cv::Mat& img) const; 32 | 33 | /// 找两条直线(ransac + fitting) 34 | bool find_two_lines(std::array& lines_params, std::array& lines_min_max, 35 | cv::Mat& img, cv::Mat& ortho) const; 36 | 37 | /// 返回激光pc 38 | pcl::PointCloud::Ptr get_pc(); 39 | 40 | private: 41 | /// 将LaserScan转换为点云 42 | void scan2points(const sensor_msgs::LaserScan& scan_in); 43 | 44 | private: 45 | // 直线搜索范围 单位m 46 | double max_range_; 47 | // 角度范围[-angle_range_, +angle_range_] 单位deg 48 | double angle_range_; 49 | // xs和ys的最小个数 50 | int thd_points_; 51 | // ransac inliers的最小个数 52 | int thd_inliers_; 53 | // inliers distance 54 | double thd_dist_; 55 | // 图像显示的宽度 56 | const int img_w_ = 320; 57 | // 图像显示焦距 58 | const double img_focal_ = 600; 59 | // 拍照高度 60 | const double img_z_ = 5.0; 61 | const double ortho_z_ = 2.0; 62 | // laserscan转换为点云，在有效区域 63 | std::vector points_; 64 | // laserscan转换为点云，在有效区域，外参转换后 65 | std::vector points_projected_; 66 | // laserscan转换为点云，有效区域外 67 | std::vector outlier_points_; 68 | // 最小角度 69 | double angle_min_{0.}; 70 | // 最大角度 71 | double angle_max_{0.}; 72 | // 角增量 73 | double angle_inc_{0.}; 74 | 75 | // 以图像的方式显示 76 | std::shared_ptr img_ptr_; 77 | // 以图像的方式显示两个设备的点 78 | std::shared_ptr ortho_ptr_; 79 | // 外参 - 旋转 80 | Eigen::Matrix3d rotation_; 81 | // 外参 - 位移 82 | Eigen::Vector3d translation_; 83 | // 本设备pc 84 | pcl::PointCloud::Ptr pc_; 85 | // Id 86 | int id_; 87 | }; 88 | } // namespace algorithm -------------------------------------------------------------------------------- /include/algorithm/random/random_shuffle.h: -------------------------------------------------------------------------------- 1 | /* +------------------------------------------------------------------------+ 2 | | Mobile Robot Programming Toolkit (MRPT) | 3 | | https://www.mrpt.org/ | 4 | | | 5 | | Copyright (c) 2005-2021, Individual contributors, see AUTHORS file | 6 | | See: https://www.mrpt.org/Authors - All rights reserved. | 7 | | Released under BSD License. See: https://www.mrpt.org/License | 8 | +------------------------------------------------------------------------+ */ 9 | 10 | #pragma once 11 | 12 | #include // iterator_traits 13 | #include // uniform_int_distribution 14 | #include // std::swap 15 | 16 | namespace algorithm { 17 | namespace random { 18 | /** Uniform shuffle a sequence. 19 | *\ingroup mrpt_random_grp 20 | */ 21 | template 22 | void shuffle(RandomIt first, RandomIt last, URBG&& g) { 23 | typedef typename std::iterator_traits::difference_type diff_t; 24 | typedef std::uniform_int_distribution distr_t; 25 | typedef typename distr_t::param_type param_t; 26 | distr_t D; 27 | diff_t n = last - first; 28 | for (diff_t i = n - 1; i > 0; --i) std::swap(first[i], first[D(g, param_t(0, i))]); 29 | } 30 | 31 | /** Uniform shuffle a sequence. 32 | *\ingroup mrpt_random_grp 33 | */ 34 | template 35 | void shuffle(RandomIt first, RandomIt last) { 36 | std::random_device rd; // used for random seed 37 | std::mt19937 g(rd()); 38 | shuffle(first, last, g); 39 | } 40 | } // namespace random 41 | } // namespace algorithm 42 | -------------------------------------------------------------------------------- /include/algorithm/ransac/ransac_applications.h: -------------------------------------------------------------------------------- 1 | /* +------------------------------------------------------------------------+ 2 | | Mobile Robot Programming Toolkit (MRPT) | 3 | | https://www.mrpt.org/ | 4 | | | 5 | | Copyright (c) 2005-2021, Individual contributors, see AUTHORS file | 6 | | See: https://www.mrpt.org/Authors - All rights reserved. | 7 | | Released under BSD License. See: https://www.mrpt.org/License | 8 | +------------------------------------------------------------------------+ */ 9 | #pragma once 10 | 11 | #include "algorithm/ransac/ransac.h" 12 | 13 | namespace algorithm { 14 | namespace ransac { 15 | 16 | /** Fit a number of 2-D lines to a given point cloud, automatically determining 17 | * the number of existing lines by means of the provided threshold and minimum 18 | * number of supporting inliers. 19 | * \param out_detected_lines The output list of pairs: number of supporting 20 | * inliers, detected line. 21 | * \param threshold The maximum distance between a point and a temptative line 22 | * such as the point is considered an inlier. 23 | * \param min_inliers_for_valid_line The minimum number of supporting inliers 24 | * to consider a line as valid. 25 | */ 26 | 27 | void ransac_detect_2D_lines(const Eigen::VectorXd& x, const Eigen::VectorXd& y, 28 | std::vector>& out_detected_lines, double threshold, 29 | size_t min_inliers_for_valid_line); 30 | 31 | } // namespace ransac 32 | } // namespace algorithm 33 | -------------------------------------------------------------------------------- /include/algorithm/two_cameras_ceres.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | #include "opencv2/core/types.hpp" 7 | 8 | namespace camera_model { 9 | class Camera; 10 | } 11 | 12 | namespace algorithm { 13 | namespace TwoCamerasCalib { 14 | 15 | struct Observation { 16 | // 图像上的点 17 | std::vector image_points; 18 | // 空间点 19 | std::vector object_points; 20 | }; 21 | 22 | /** 23 | * 使用ceres计算位姿 24 | * @param camera_ptr 25 | * @param obs 26 | * @param T_21 相机1到相机2的变换矩阵 27 | */ 28 | bool calibrate(const boost::shared_ptr &camera_ptr, const Observation &obs, 29 | Eigen::Matrix4d &T_21); 30 | 31 | } // namespace TwoCamerasCalib 32 | } // namespace algorithm -------------------------------------------------------------------------------- /include/algorithm/two_lasers_ceres.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include 3 | #include 4 | 5 | namespace algorithm { 6 | 7 | struct Observation { 8 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 9 | 10 | // 激光1在a面上线段的单位向量 11 | Eigen::Vector3d l_1_a; 12 | // 激光1在a面上线段中点 13 | Eigen::Vector3d c_1_a; 14 | // 激光1在b面上线段的单位向量 15 | Eigen::Vector3d l_1_b; 16 | // 激光1在b面上线段中点 17 | Eigen::Vector3d c_1_b; 18 | // 激光2在a面上线段的单位向量 19 | Eigen::Vector3d l_2_a; 20 | // 激光2在a面上线段中点 21 | Eigen::Vector3d c_2_a; 22 | // 激光2在b面上线段的单位向量 23 | Eigen::Vector3d l_2_b; 24 | // 激光2在b面上线段中点 25 | Eigen::Vector3d c_2_b; 26 | // 权重 27 | double scale{1.0}; 28 | }; 29 | 30 | /** 31 | * 使用ceres求解，自己构建梯度求解以及LocalParameterization 32 | * @param obs 观测量集合 33 | * @param T 激光2到1的变换矩阵 34 | */ 35 | void TwoLasersCalibration(const std::vector &obs, Eigen::Matrix4d &T12, bool is_tx_fixed); 36 | 37 | /** 38 | * 使用ceres求解，使用AutoDiff以及ceres::EigenQuaternionParameterization 39 | * @param obs 观测量集合 40 | * @param T 激光2到1的变换矩阵 41 | */ 42 | void TwoLasersCalibrationAutoDiff(const std::vector &obs, Eigen::Matrix4d &T12); 43 | 44 | /** 45 | * 原生解法 46 | * @param obs 观测量集合 47 | * @param T 激光2到1的变换矩阵 48 | */ 49 | void TwoLasersCalibrationNaive(const std::vector &obs, Eigen::Matrix4d &T12); 50 | 51 | } // namespace algorithm 52 | -------------------------------------------------------------------------------- /include/algorithm/util.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include 3 | #include 4 | #include 5 | #include 6 | 7 | namespace algorithm { 8 | 9 | #define DEG2RAD_RBT(x) (x * M_PI / 180.0) 10 | #define RAD2DEG_RBT(x) (x * 180.0 / M_PI) 11 | 12 | struct EulerAngles { 13 | double yaw{0.}; 14 | double pitch{0.}; 15 | double roll{0.}; 16 | friend std::ostream& operator<<(std::ostream&, const EulerAngles&); 17 | }; 18 | 19 | /// 切空间到se3 20 | Eigen::Matrix4d lie_to_se3(const Eigen::Matrix& lie); 21 | 22 | /// ZYX顺序转为四元数 23 | Eigen::Quaterniond ypr2quat(double yaw, double pitch, double roll); 24 | 25 | /// 四元数转欧拉角 26 | EulerAngles quat2euler(const Eigen::Quaterniond& q); 27 | 28 | /// 3个点确定一个平面 29 | Eigen::Vector4d plane_from_3pts(const Eigen::Vector3d& x1, const Eigen::Vector3d& x2, const Eigen::Vector3d& x3); 30 | 31 | /// 直线与平面的交点 32 | // bool plane_line_intersect_point(const Eigen::Vector4d& plane, const Eigen::Vector3d& line_p1, 33 | // const Eigen::Vector3d& line_p2, Eigen::Vector3d& p); 34 | bool plane_line_intersect_point(const Eigen::Vector3d& plane_normal, const Eigen::Vector3d& p_on_plane, 35 | const Eigen::Vector3d& line_vector, const Eigen::Vector3d& p_on_line, 36 | Eigen::Vector3d& p); 37 | 38 | /// 直线拟合 39 | bool line_fitting_ceres(const std::vector& points, Eigen::Vector2d& line_params); 40 | 41 | /// 计算斜对称矩阵 reference to gtsam 42 | /** 43 | * skew symmetric matrix returns this: 44 | * 0 -wz wy 45 | * wz 0 -wx 46 | * -wy wx 0 47 | * @param wx 3 dimensional vector 48 | * @param wy 49 | * @param wz 50 | * @return a 3*3 skew symmetric matrix 51 | */ 52 | inline Eigen::Matrix3d skew_symmetric(double wx, double wy, double wz) { 53 | return (Eigen::Matrix3d() << 0.0, -wz, +wy, +wz, 0.0, -wx, -wy, +wx, 0.0).finished(); 54 | } 55 | 56 | /// 计算斜对称矩阵 57 | template 58 | inline Eigen::Matrix3d skew_symmetric(const Eigen::MatrixBase& w) { 59 | return skew_symmetric(w(0), w(1), w(2)); 60 | } 61 | 62 | template 63 | static Eigen::Matrix skewSymmetric(const Eigen::MatrixBase& m) { 64 | Eigen::Matrix skew; 65 | skew << typename Derived::Scalar(0), -m(2), m(1), m(2), typename Derived::Scalar(0), -m(0), -m(1), m(0), 66 | typename Derived::Scalar(0); 67 | return skew; 68 | } 69 | 70 | void remove_cols(Eigen::MatrixXd& data, const std::vector& idx_to_remove); 71 | 72 | } // namespace algorithm 73 | -------------------------------------------------------------------------------- /include/calibration/calibration_state.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | 7 | namespace calibration { 8 | 9 | // typedef void(*func_ptr)(); 10 | class BaseCalib; 11 | 12 | class CalibrationState { 13 | public: 14 | CalibrationState() : calib_context_(nullptr) {} 15 | virtual ~CalibrationState() = default; 16 | 17 | virtual int id() = 0; 18 | virtual std::string name() = 0; 19 | virtual void calibration() = 0; 20 | void set_context(BaseCalib* context) { calib_context_ = context; } 21 | 22 | public: 23 | // std::vector tasks_; 24 | BaseCalib* calib_context_; 25 | }; 26 | 27 | class StateIdle : public CalibrationState { 28 | public: 29 | int id() override; 30 | std::string name() override; 31 | void calibration() override; 32 | }; 33 | 34 | class StateStart : public CalibrationState { 35 | public: 36 | int id() override; 37 | std::string name() override; 38 | void calibration() override; 39 | }; 40 | 41 | class StateGetPose : public CalibrationState { 42 | public: 43 | int id() override; 44 | std::string name() override; 45 | void calibration() override; 46 | }; 47 | 48 | class StateCheckSteady : public CalibrationState { 49 | public: 50 | int id() override; 51 | std::string name() override; 52 | void calibration() override; 53 | }; 54 | 55 | class StateStartCalib : public CalibrationState { 56 | public: 57 | int id() override; 58 | std::string name() override; 59 | void calibration() override; 60 | }; 61 | 62 | class StateInCalib : public CalibrationState { 63 | public: 64 | int id() override; 65 | std::string name() override; 66 | void calibration() override; 67 | }; 68 | 69 | } // namespace calibration -------------------------------------------------------------------------------- /include/dev/april_board.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | 6 | namespace aslam { 7 | namespace cameras { 8 | class GridCalibrationTargetAprilgrid; 9 | } 10 | } // namespace aslam 11 | 12 | namespace glk { 13 | class GLSLShader; 14 | class Drawable; 15 | } // namespace glk 16 | 17 | namespace dev { 18 | 19 | /** 20 | * @brief 标定板对象 21 | * 22 | */ 23 | class AprilBoard { 24 | public: 25 | explicit AprilBoard(std::string& data_path); 26 | 27 | /** 28 | * @brief 画ui 29 | */ 30 | void draw_ui(); 31 | 32 | /** 33 | * 画3d显示部分 34 | * @param shader 35 | */ 36 | void draw_gl(glk::GLSLShader& shader); 37 | 38 | /** 39 | * @brief 打开显示ui开关 40 | */ 41 | void show() { b_show_window_ = true; }; 42 | 43 | /** 44 | * @brief 打开显示3d开关 45 | */ 46 | void show_3d() { b_show_3d_ = true; }; 47 | 48 | /** 49 | * 获取sensor位姿 50 | * @param pose 51 | */ 52 | const Eigen::Matrix4f& get_pose() const { return T_; } 53 | 54 | /** 55 | * 设置sensor位姿 56 | * @param new_pose 57 | */ 58 | void set_pose(const Eigen::Matrix4f& new_pose) { T_ = new_pose; } 59 | 60 | public: 61 | // 标定板对象 62 | boost::shared_ptr board{nullptr}; 63 | 64 | private: 65 | /// 更新aprilboard 3d边框显示相关 66 | void update_aprilboard_edges(); 67 | 68 | private: 69 | // 图片texture id 70 | unsigned int texture_id_{0}; 71 | // 图像宽度 72 | int img_width_{0}; 73 | // 图像高度 74 | int img_height_{0}; 75 | // 显示ui窗口 76 | bool b_show_window_{false}; 77 | // 是否显示3d内容 78 | bool b_show_3d_{false}; 79 | // tagRows 80 | uint tag_rows_{6}; 81 | // tagCols 82 | uint tag_cols_{6}; 83 | // tagSize 84 | double tag_size_{0.03}; 85 | // tagSpacing 86 | double tag_spacing_{0.3}; 87 | // 板子长度 88 | double board_lenght_{}; 89 | // 板子高度 90 | double board_height_{}; 91 | // 显示角点 ---- 弃用，点没有深度，无法正常显示 92 | // std::shared_ptr points_3d_ptr_{nullptr}; 93 | // april board上的角点 94 | std::vector april_points_; 95 | // april board的边 96 | std::shared_ptr april_edges_ptr_{nullptr}; 97 | // 设备在世界坐标系下的位姿 98 | Eigen::Matrix4f T_{}; 99 | }; 100 | 101 | } // namespace dev 102 | -------------------------------------------------------------------------------- /include/dev/blob_board.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include 3 | #include 4 | #include 5 | #include 6 | namespace glk { 7 | class GLSLShader; 8 | class Drawable; 9 | } // namespace glk 10 | namespace dev { 11 | /** 12 | * @brief 棋盘格标定板对象 13 | * 14 | */ 15 | class blob_board { 16 | public: 17 | explicit blob_board(std::string& data_path); 18 | /** 19 | * @brief 画ui 20 | */ 21 | void draw_ui(); 22 | /** 23 | * 画3d显示部分 24 | * @param shader 25 | */ 26 | void draw_gl(glk::GLSLShader& shader); 27 | /** 28 | * @brief 打开显示ui开关 29 | */ 30 | void show() { b_show_window_ = true; }; 31 | /** 32 | * @brief 打开显示3d开关 33 | */ 34 | void show_3d() { b_show_3d_ = true; }; 35 | /** 36 | * 获取sensor位姿 37 | * @param pose 38 | */ 39 | const Eigen::Matrix4f& get_pose() const { return T_; } 40 | /** 41 | * 设置sensor位姿 42 | * @param new_pose 43 | */ 44 | void set_pose(const Eigen::Matrix4f& new_pose) { T_ = new_pose; } 45 | 46 | public: 47 | cv::Size get_board_size() const { return cv::Size{tag_rows_, tag_cols_}; } 48 | double get_tag_size_() const { return tag_size_; } 49 | 50 | private: 51 | /// 更新chessboard 3d边框显示相关 52 | void update_blob_board_edges(); 53 | 54 | private: 55 | // 图片texture id 56 | unsigned int texture_id_{0}; 57 | // 图像宽度 58 | int img_width_{0}; 59 | // 图像高度 60 | int img_height_{0}; 61 | // 显示ui窗口 62 | bool b_show_window_{false}; 63 | // 是否显示3d内容 64 | bool b_show_3d_{false}; 65 | // tagRows 66 | int tag_rows_{4}; 67 | // tagCols 68 | int tag_cols_{8}; 69 | // tagSize 70 | double tag_size_{0.052}; 71 | // 板子长度 72 | double board_lenght_{}; 73 | // 板子高度 74 | double board_height_{}; 75 | // 显示角点 ---- 弃用，点没有深度，无法正常显示 76 | // std::shared_ptr points_3d_ptr_{nullptr}; 77 | // chess board上的角点 78 | std::vector blob_board_points_; 79 | // april board的边 80 | std::shared_ptr blob_edges_ptr_{nullptr}; 81 | // 设备在世界坐标系下的位姿 82 | Eigen::Matrix4f T_{}; 83 | }; 84 | 85 | } // namespace dev 86 | -------------------------------------------------------------------------------- /include/dev/board.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include 3 | #include 4 | #include 5 | // 前置声明 6 | namespace glk { 7 | class GLSLShader; 8 | } 9 | 10 | namespace dev { 11 | class SensorManager; 12 | } // namespace dev 13 | namespace dev { 14 | 15 | class Board { 16 | public: 17 | explicit Board(std::string& data_path) {} 18 | virtual ~Board() = default; 19 | /** 20 | * @brief 画ui 21 | */ 22 | virtual void draw_ui() = 0; 23 | /** 24 | * 画3d显示部分 25 | * @param shader 26 | */ 27 | virtual void draw_gl(glk::GLSLShader& shader) = 0; 28 | /** 29 | * 计算图像的角点和对应的3D点 30 | * @param 31 | */ 32 | virtual void computeObservation(); 33 | /** 34 | * @brief 打开显示ui开关 35 | */ 36 | void show() { b_show_window_ = true; }; 37 | /** 38 | * @brief 打开显示3d开关 39 | */ 40 | void show_3d() { b_show_3d_ = true; }; 41 | /** 42 | * 获取sensor位姿 43 | * @param pose 44 | */ 45 | const Eigen::Matrix4f& get_pose() const { return T_; } 46 | /** 47 | * 设置sensor位姿 48 | * @param new_pose 49 | */ 50 | void set_pose(const Eigen::Matrix4f& new_pose) { T_ = new_pose; } 51 | 52 | private: 53 | /** 54 | * 更新棋盘格在3d场景中的显示 55 | */ 56 | virtual void update_board_edges(); 57 | 58 | private: 59 | // 图片texture id 60 | unsigned int texture_id_{0}; 61 | // 图像宽度 62 | int img_width_{0}; 63 | // 图像高度 64 | int img_height_{0}; 65 | // tagSize 66 | double tag_size_{0.03}; //单位mm 67 | // tagSpacing 68 | double tag_spacing_{0.3}; //标准棋盘格标定板无此成员 69 | // 板子长度 70 | double board_lenght_{}; 71 | // 板子高度 72 | double board_height_{}; 73 | // 是否显示ui窗口 74 | bool b_show_window_{false}; 75 | // 是否显示3d内容 76 | bool b_show_3d_{false}; 77 | // 设备在世界坐标系下的位姿 78 | Eigen::Matrix4f T_{}; 79 | }; 80 | 81 | } // namespace dev -------------------------------------------------------------------------------- /include/dev/camera.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | 6 | #include "dev/sensor.hpp" 7 | 8 | namespace camera_model { 9 | class Camera; 10 | } 11 | 12 | namespace cv_bridge { 13 | class CvImage; 14 | } 15 | 16 | namespace dev { 17 | 18 | template 19 | class SensorData; 20 | 21 | class ImageShow; 22 | 23 | /** 24 | * @brief Camera object 25 | * 26 | */ 27 | class Camera : public Sensor { 28 | public: 29 | using Ptr = std::shared_ptr; 30 | 31 | Camera(const std::string& name, ros::NodeHandle& ros_nh); 32 | 33 | ~Camera() override = default; 34 | 35 | /// 释放3d模型 36 | void free() override { free_model(); } 37 | 38 | /// opengl渲染 39 | void draw_gl(glk::GLSLShader& shader, const std::shared_ptr& canvas_ptr) override; 40 | /// imgui绘图 41 | void draw_ui() override; 42 | 43 | /// 获取当前图像数据 44 | boost::shared_ptr data(); 45 | /// 获取当前相机对象 46 | boost::shared_ptr camera_model() const { return inst_ptr_; }; 47 | /// 更新相机参数 48 | void update_params(); 49 | 50 | private: 51 | // 当前类型 52 | int current_camera_type_{2}; 53 | // 是否显示图像 54 | bool b_show_image_{false}; 55 | // ros topic 使能 56 | bool b_enable_topic_[2]{false, false}; 57 | // 相机对象 58 | boost::shared_ptr inst_ptr_{nullptr}; 59 | // 相机内参 60 | std::vector inst_params_; 61 | // 获取图像数据 62 | std::shared_ptr> image_data_ptr_; 63 | // 获取深度点云数据 64 | std::shared_ptr> points_data_ptr_; 65 | // 图像显示对象 66 | std::shared_ptr im_show_ptr_{nullptr}; 67 | // 保存转码后的图像数据 68 | boost::shared_ptr image_cv_ptr_{nullptr}; 69 | 70 | private: 71 | /// 创建指定类型的相机 72 | void creat_instance(int current_camera_type); 73 | /// 绘制参数ui 74 | void draw_ui_params(); 75 | /// 绘制话题ui 76 | void draw_ui_topic_name(); 77 | /// 检查当前设备在线状态 78 | void check_online_status(); 79 | /// 需要将图像消息转为cv::Mat 80 | bool cv_convert(boost::shared_ptr& msg); 81 | }; 82 | 83 | } // namespace dev 84 | -------------------------------------------------------------------------------- /include/dev/chess_board.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include 3 | #include 4 | #include 5 | namespace camera_model { 6 | class Chessboard; 7 | } 8 | namespace glk { 9 | class GLSLShader; 10 | class Drawable; 11 | } // namespace glk 12 | namespace dev { 13 | /** 14 | * @brief 棋盘格标定板对象 15 | * 16 | */ 17 | class chessboard { 18 | public: 19 | explicit chessboard(std::string& data_path); 20 | /** 21 | * @brief 画ui 22 | */ 23 | void draw_ui(); 24 | /** 25 | * 画3d显示部分 26 | * @param shader 27 | */ 28 | void draw_gl(glk::GLSLShader& shader); 29 | /** 30 | * @brief 打开显示ui开关 31 | */ 32 | void show() { b_show_window_ = true; }; 33 | /** 34 | * @brief 打开显示3d开关 35 | */ 36 | void show_3d() { b_show_3d_ = true; }; 37 | /** 38 | * 获取sensor位姿 39 | * @param pose 40 | */ 41 | const Eigen::Matrix4f& get_pose() const { return T_; } 42 | /** 43 | * 设置sensor位姿 44 | * @param new_pose 45 | */ 46 | void set_pose(const Eigen::Matrix4f& new_pose) { T_ = new_pose; } 47 | 48 | public: 49 | // 标定板对象 50 | boost::shared_ptr board{nullptr}; 51 | 52 | private: 53 | /// 更新chessboard 3d边框显示相关 54 | void update_chessboard_edges(); 55 | 56 | private: 57 | // 图片texture id 58 | unsigned int texture_id_{0}; 59 | // 图像宽度 60 | int img_width_{0}; 61 | // 图像高度 62 | int img_height_{0}; 63 | // 显示ui窗口 64 | bool b_show_window_{false}; 65 | // 是否显示3d内容 66 | bool b_show_3d_{false}; 67 | // tagRows 68 | int tag_rows_{6}; 69 | // tagCols 70 | int tag_cols_{9}; 71 | // tagSize 72 | double tag_size_{0.03}; 73 | // 板子长度 74 | double board_lenght_{}; 75 | // 板子高度 76 | double board_height_{}; 77 | // 显示角点 ---- 弃用，点没有深度，无法正常显示 78 | // std::shared_ptr points_3d_ptr_{nullptr}; 79 | // chess board上的角点 80 | std::vector chess_board_points_; 81 | // april board的边 82 | std::shared_ptr chess_edges_ptr_{nullptr}; 83 | // 设备在世界坐标系下的位姿 84 | Eigen::Matrix4f T_{}; 85 | }; 86 | 87 | } // namespace dev 88 | -------------------------------------------------------------------------------- /include/dev/image_show.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | 7 | namespace cv_bridge { 8 | class CvImage; 9 | } 10 | 11 | namespace dev { 12 | 13 | class ImageShow { 14 | public: 15 | ImageShow(); 16 | 17 | ~ImageShow(); 18 | 19 | /** 20 | * @brief 更新当前图像 21 | * @param image 图像信息保存在cv::Mat 22 | */ 23 | void update_image(boost::shared_ptr& image); 24 | 25 | /// 使能图像显示 26 | void enable(const std::string& window_name, bool b_use_opencv = false); 27 | 28 | /// 关闭图像显示 29 | void disable(); 30 | 31 | /// 显示图像 32 | void show_image(bool& b_show_image); 33 | 34 | private: 35 | /// 显示图像操作，放在单独的线程中 36 | void show_in_opencv(); 37 | 38 | /// 更新纹理数据，支持灰度和RGB 39 | void update_texture(); 40 | 41 | private: 42 | // 是否显示图片 43 | bool b_show_image_{false}; 44 | 45 | // 是否使用opencv显示 46 | bool b_use_opencv_{false}; 47 | 48 | // 数据就绪 49 | bool is_texture_ready_{false}; 50 | 51 | // 是否需要更新texture 52 | bool b_need_update_texture_{false}; 53 | 54 | // 资源锁 55 | std::mutex mtx_; 56 | 57 | // 显示图片opencv线程 58 | std::thread thread_; 59 | 60 | // 窗口名称 61 | std::string window_name_; 62 | 63 | // 保存转码后的图像数据 64 | boost::shared_ptr image_cv_ptr_{nullptr}; 65 | 66 | // 纹理id 67 | unsigned int image_texture_{0}; 68 | }; 69 | 70 | } // namespace dev -------------------------------------------------------------------------------- /include/dev/laser.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include "dev/sensor.hpp" 5 | 6 | namespace glk { 7 | class PointCloudBuffer; 8 | } 9 | 10 | namespace dev { 11 | 12 | template 13 | class SensorData; 14 | 15 | class ImageShow; 16 | 17 | /** 18 | * @brief Laser object 19 | * 20 | */ 21 | class Laser : public Sensor { 22 | public: 23 | using Ptr = std::shared_ptr; 24 | 25 | Laser(const std::string& name, ros::NodeHandle& ros_nh); 26 | 27 | ~Laser() override = default; 28 | 29 | void free() override; 30 | 31 | /// opengl渲染 32 | void draw_gl(glk::GLSLShader& shader, const std::shared_ptr& canvas_ptr) override; 33 | /// imgui绘图 34 | void draw_ui() override; 35 | /// 获取当前激光数据 36 | boost::shared_ptr data(); 37 | 38 | private: 39 | // 是否显示激光 40 | bool b_show_laser_{true}; 41 | // ros topic 使能 42 | bool b_enable_topic_{false}; 43 | // 点云数据 44 | std::shared_ptr> laser_data_ptr_{nullptr}; 45 | // 点云数据显示所用 46 | std::shared_ptr pointcloud_buffer_ptr_{nullptr}; 47 | // 当前pointcloud_buffer中数据的时间ns数 48 | uint32_t time_ns_{0}; 49 | 50 | private: 51 | /// 绘制话题ui 52 | void draw_ui_topic_name(); 53 | /// 检查当前设备在线状态 54 | void check_online_status(); 55 | }; 56 | 57 | } // namespace dev 58 | -------------------------------------------------------------------------------- /include/dev/sensor_data.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | 7 | #include 8 | 9 | namespace dev { 10 | 11 | /** 12 | * @brief 传感器数据类 13 | * 14 | */ 15 | template 16 | class SensorData { 17 | public: 18 | typedef boost::shared_ptr MConstPtr; 19 | 20 | explicit SensorData(ros::NodeHandle& ros_nh, int msgs_deque_size = 10) 21 | : nh_(ros_nh), msgs_deque_size_(msgs_deque_size) {} 22 | 23 | /** 24 | * @brief 订阅指定话题。如果首次订阅则新建订阅对象，重新订阅则使用之前的订阅对象 25 | * @param topic 订阅话题 26 | * @param queue_size 27 | * @param callback_queue 回调 28 | */ 29 | void subscribe(const std::string& topic, uint32_t queue_size) { 30 | // 之前已经订阅过，需要判断是否是同一个topic 31 | if (sub_ && topic == cur_topic_name_) { 32 | // 重连即可 33 | sub_->subscribe(); 34 | } else { 35 | sub_ = nullptr; 36 | // 新建订阅对象 37 | sub_ = boost::make_shared>(nh_, topic, queue_size, 38 | ros::TransportHints().tcpNoDelay()); 39 | // 注册回调函数 40 | sub_->registerCallback( 41 | typename message_filters::SimpleFilter::Callback(boost::bind(&SensorData::callback, this, _1))); 42 | // 更新当前话题名称 43 | cur_topic_name_ = topic; 44 | } 45 | } 46 | 47 | /** 48 | * @brief 停止订阅当前话题 49 | */ 50 | void unsubscribe() { 51 | if (sub_) { 52 | sub_->unsubscribe(); 53 | } 54 | } 55 | 56 | /** 57 | * @brief 设置消息缓存大小 58 | * @param 消息缓存大小 > 0 59 | */ 60 | void set_msgs_deque_size(unsigned int cache_size) { 61 | if (cache_size == 0) { 62 | return; 63 | } 64 | msgs_deque_size_ = cache_size; 65 | } 66 | 67 | /** 68 | * @brief 设置数据速率因子 69 | * @param data_rate 70 | */ 71 | void set_data_rate(int data_rate) { data_rate_factor_ = data_rate; } 72 | 73 | MConstPtr data() { 74 | boost::mutex::scoped_lock lock(msgs_lock_); 75 | if (msgs_.empty()) { 76 | return nullptr; 77 | } else { 78 | return msgs_.back(); 79 | } 80 | } 81 | 82 | private: 83 | /** 84 | * @brief 数据回调 85 | * @param msg 86 | */ 87 | void callback(const MConstPtr& msg) { 88 | static int data_cnt = 0; 89 | if (data_cnt == data_rate_factor_) { 90 | data_cnt = 0; 91 | boost::mutex::scoped_lock lock(msgs_lock_); 92 | // 判断大小 93 | while (msgs_.size() >= msgs_deque_size_) { 94 | msgs_.pop_front(); 95 | } 96 | // 放入队尾 97 | msgs_.push_back(msg); 98 | } else { 99 | data_cnt++; 100 | } 101 | } 102 | 103 | /// ros::NodeHandle 104 | ros::NodeHandle& nh_; 105 | /// 消息订阅对象指针 106 | boost::shared_ptr> sub_{nullptr}; 107 | /// 当前订阅的话题 108 | std::string cur_topic_name_; 109 | 110 | /// 数据锁 111 | mutable boost::mutex msgs_lock_; 112 | /// 消息队列长度 113 | unsigned int msgs_deque_size_; 114 | /// 接收到消息队列 115 | std::deque msgs_; 116 | /// 数据速率控制 117 | int data_rate_factor_{0}; 118 | }; 119 | 120 | } // namespace dev 121 | -------------------------------------------------------------------------------- /include/dev/sensor_manager.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | 8 | #include "dev/sensor.hpp" 9 | #include "glk/text_renderer.hpp" 10 | #include "guik/gl_canvas.hpp" 11 | 12 | namespace dev { 13 | 14 | /** 15 | * @brief SensorManager object (使用单实例模板初始化) 16 | * 17 | */ 18 | class SensorManager { 19 | public: 20 | using Ptr = std::shared_ptr; 21 | 22 | explicit SensorManager(ros::NodeHandle& ros_nh) : nh_(ros_nh){}; 23 | 24 | ~SensorManager(); 25 | 26 | /** 27 | * @brief 添加传感器 28 | * @param sensor 29 | */ 30 | void add_sensor(dev::Sensor::Ptr& sensor); 31 | 32 | /** 33 | * @brief opengl渲染 34 | */ 35 | void draw_gl(glk::GLSLShader& shader, const std::shared_ptr& canvas_ptr); 36 | 37 | /** 38 | * @brief 画ui 39 | */ 40 | void draw_ui(); 41 | 42 | /// 释放资源，主要是opengl 相关资源 43 | void free(); 44 | 45 | private: 46 | /// 检查并清楚需要删除的传感器 47 | void check_and_clear_sensors(); 48 | 49 | /// 调用所有传感器的draw_ui 50 | void call_sensors_draw_ui(); 51 | 52 | public: 53 | // 使用unordered_map，list便于删除操作 54 | std::unordered_map> sensors_map; 55 | 56 | private: 57 | // ros 句柄 58 | ros::NodeHandle& nh_; 59 | 60 | // 设备个数 61 | int sensor_num_{0}; 62 | }; 63 | 64 | } // namespace dev 65 | -------------------------------------------------------------------------------- /include/dev/util.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | namespace dev { 4 | 5 | void help_marker(const char* desc); 6 | 7 | void get_topic_name_from_list(const std::string& target_topic_type, std::vector& candidates); 8 | 9 | void show_pfd_info(const std::string& title, const std::string& msg); 10 | 11 | } // namespace dev 12 | -------------------------------------------------------------------------------- /include/glk/colormap.hpp: -------------------------------------------------------------------------------- 1 | #ifndef GLK_COLORMAP_HPP 2 | #define GLK_COLORMAP_HPP 3 | 4 | #include 5 | 6 | namespace glk { 7 | 8 | enum COLORMAP_TYPE { TURBO = 0, NUM_COLORMAPS }; 9 | 10 | Eigen::Vector4i colormap(COLORMAP_TYPE type, int x); 11 | Eigen::Vector4f colormapf(COLORMAP_TYPE type, float x); 12 | Eigen::Vector4i colormap_categorical(COLORMAP_TYPE type, int x, int num_categories); 13 | Eigen::Vector4f colormap_categoricalf(COLORMAP_TYPE type, int x, int num_categories); 14 | } // namespace glk 15 | 16 | #endif -------------------------------------------------------------------------------- /include/glk/drawble.hpp: -------------------------------------------------------------------------------- 1 | #ifndef GLK_DRAWABLE_HPP 2 | #define GLK_DRAWABLE_HPP 3 | 4 | namespace glk { 5 | 6 | class GLSLShader; 7 | 8 | /** 9 | * @brief Drawable object interface 10 | * 11 | */ 12 | class Drawable { 13 | public: 14 | virtual ~Drawable() = default; 15 | virtual void free() {} 16 | 17 | virtual void draw(glk::GLSLShader &shader) const = 0; 18 | }; 19 | 20 | } // namespace glk 21 | 22 | #endif -------------------------------------------------------------------------------- /include/glk/frame_buffer.hpp: -------------------------------------------------------------------------------- 1 | #ifndef GLK_FRAME_BUFFER_HPP 2 | #define GLK_FRAME_BUFFER_HPP 3 | 4 | #include 5 | #include 6 | #include "GL/gl3w.h" 7 | #include 8 | 9 | namespace glk { 10 | 11 | class Texture; 12 | 13 | /** 14 | * @brief OpenGL FrameBuffer wrapper class 15 | * 16 | */ 17 | class FrameBuffer { 18 | public: 19 | explicit FrameBuffer(const Eigen::Vector2i& size); 20 | 21 | ~FrameBuffer(); 22 | 23 | void bind(); 24 | void unbind() const; 25 | 26 | void add_color_buffer(GLuint internal_format = GL_RGBA, GLuint format = GL_RGBA, GLuint type = GL_UNSIGNED_BYTE); 27 | 28 | const Texture& color() { return *color_buffers[0]; } 29 | const Texture& color(int i) { return *color_buffers[i]; } 30 | const Texture& depth() { return *depth_buffer; } 31 | 32 | private: 33 | int width; 34 | int height; 35 | 36 | GLint viewport[4]{}; 37 | 38 | std::vector> color_buffers; // color_buffers是一个里面装了指向Texture的指针的vector 39 | std::shared_ptr depth_buffer; 40 | 41 | GLuint frame_buffer{}; // GLuint = unsigned int 42 | }; 43 | 44 | } // namespace glk 45 | 46 | #endif -------------------------------------------------------------------------------- /include/glk/glsl_shader.hpp: -------------------------------------------------------------------------------- 1 | #ifndef GLK_GLSL_SHADER_HPP 2 | #define GLK_GLSL_SHADER_HPP 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | #include "GL/gl3w.h" 9 | #include 10 | 11 | namespace glk { 12 | 13 | /** 14 | * @brief GLSL shader class 15 | * 16 | */ 17 | class GLSLShader { 18 | public: 19 | GLSLShader() = default; 20 | 21 | /** 22 | * @brief load GLSL shader from files 23 | * 24 | * @param shader_path "shader_path".vert and "shader_path".frag will be loaded 25 | * @return if the shader is successfully loaded 26 | */ 27 | bool init(const std::string& shader_path); 28 | 29 | /** 30 | * @brief bind the shader 31 | */ 32 | void use() const { 33 | glUseProgram(shader_program); 34 | } // void glUseProgram(GLuint program) - 35 | // 使用程序对象作为当前渲染状态的一部分，program提供程序句柄，该程序对象的可执行文件将用作当前渲染状态的一部分 36 | 37 | /** @brief find attribute variable location **/ 38 | GLint attrib(const std::string& name); 39 | /** @brief find uniform variable location **/ 40 | GLint uniform(const std::string& name); 41 | 42 | /*** getter and setter for uniforms ***/ 43 | Eigen::Vector4f get_uniform4f(const std::string& name) { 44 | Eigen::Vector4f vec; 45 | glGetUniformfv(shader_program, uniform(name), vec.data()); 46 | return vec; 47 | } 48 | 49 | Eigen::Matrix4f get_uniform_matrix4f(const std::string& name) { 50 | Eigen::Matrix4f mat; 51 | glGetUniformfv(shader_program, uniform(name), mat.data()); 52 | return mat; 53 | } 54 | 55 | void set_uniform(const std::string& name, int value) { glUniform1i(uniform(name), value); } 56 | void set_uniform(const std::string& name, float value) { glUniform1f(uniform(name), value); } 57 | void set_uniform(const std::string& name, const Eigen::Vector2f& vector) { 58 | glUniform2fv(uniform(name), 1, vector.data()); 59 | } 60 | void set_uniform(const std::string& name, const Eigen::Vector3f& vector) { 61 | glUniform3fv(uniform(name), 1, vector.data()); 62 | } 63 | void set_uniform(const std::string& name, const Eigen::Vector4f& vector) { 64 | glUniform4fv(uniform(name), 1, vector.data()); 65 | } 66 | void set_uniform(const std::string& name, const Eigen::Vector4i& vector) { 67 | glUniform4iv(uniform(name), 1, vector.data()); 68 | } 69 | void set_uniform(const std::string& name, const Eigen::Matrix4f& matrix) { 70 | glUniformMatrix4fv(uniform(name), 1, GL_FALSE, matrix.data()); 71 | } 72 | GLuint get_shader_program() const { return shader_program; } 73 | 74 | private: 75 | static GLuint read_shader_from_file(const std::string& filename, GLuint shader_type); 76 | 77 | private: 78 | GLuint shader_program{}; 79 | std::unordered_map attrib_cache; 80 | std::unordered_map uniform_cache; 81 | }; 82 | 83 | } // namespace glk 84 | 85 | #endif -------------------------------------------------------------------------------- /include/glk/lines.hpp: -------------------------------------------------------------------------------- 1 | #ifndef GLK_LINES_HPP 2 | #define GLK_LINES_HPP 3 | 4 | #include 5 | #include 6 | 7 | #include "glk/drawble.hpp" 8 | 9 | namespace glk { 10 | 11 | /** 12 | * @brief A class to draw a set of lines 13 | * 14 | */ 15 | class Lines : public Drawable { 16 | public: 17 | Lines(float line_width, const std::vector>& vertices, 18 | const std::vector>& colors = 19 | std::vector>(), 20 | const std::vector>& infos = 21 | std::vector>()); 22 | ~Lines() override; 23 | 24 | void draw(glk::GLSLShader& shader) const override; 25 | 26 | private: 27 | Lines(const Lines&); 28 | Lines& operator=(const Lines&); 29 | 30 | private: 31 | int num_vertices; 32 | int num_indices; 33 | 34 | GLuint vao; // vertex array object 35 | GLuint vbo; // vertices 36 | GLuint cbo; // colors 37 | GLuint ibo; // infos 38 | GLuint ebo{}; // elements 39 | }; 40 | } // namespace glk 41 | 42 | #endif -------------------------------------------------------------------------------- /include/glk/loaders/ply_loader.hpp: -------------------------------------------------------------------------------- 1 | #ifndef GLK_PLY_LOADER_HPP 2 | #define GLK_PLY_LOADER_HPP 3 | 4 | #include 5 | #include 6 | 7 | namespace glk { 8 | 9 | /** 10 | * @brief Loader class for PLY file format 11 | * @note if it fails to read a file, the members become empty 12 | */ 13 | class PLYLoader { 14 | public: 15 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 16 | 17 | explicit PLYLoader(const std::string& filename); 18 | 19 | public: 20 | std::vector> vertices; 21 | std::vector> normals; 22 | std::vector indices; 23 | }; 24 | 25 | } // namespace glk 26 | 27 | #endif -------------------------------------------------------------------------------- /include/glk/mesh.hpp: -------------------------------------------------------------------------------- 1 | #ifndef GLK_MESH_HPP 2 | #define GLK_MESH_HPP 3 | 4 | #include 5 | #include 6 | 7 | #include "GL/gl3w.h" 8 | #include "glk/drawble.hpp" 9 | 10 | namespace glk { 11 | 12 | /** 13 | * @brief Mesh class 14 | * 15 | */ 16 | class Mesh : public Drawable { 17 | public: 18 | Mesh(const std::vector> &vertices, 19 | const std::vector> &normals, 20 | const std::vector &indices); 21 | 22 | void free() override; 23 | 24 | void draw(glk::GLSLShader &shader) const override; 25 | 26 | private: 27 | Mesh(const Mesh &); 28 | 29 | Mesh &operator=(const Mesh &); 30 | 31 | private: 32 | int num_vertices; 33 | int num_indices; 34 | 35 | GLuint vao{}; 36 | GLuint vbo{}; 37 | GLuint nbo{}; 38 | GLuint ebo{}; 39 | }; 40 | 41 | } // namespace glk 42 | 43 | #endif -------------------------------------------------------------------------------- /include/glk/mesh_utils.hpp: -------------------------------------------------------------------------------- 1 | #ifndef GLK_PRIMITIVES_MESH_UTILS_HPP 2 | #define GLK_PRIMITIVES_MESH_UTILS_HPP 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | namespace glk { 9 | 10 | /** 11 | * @brief A class to make a mesh to be flat (so-called flat shading) 12 | * 13 | */ 14 | class Flatize { 15 | public: 16 | Flatize(const std::vector>& vertices_, 17 | const std::vector& indices_) { 18 | vertices.resize(indices_.size()); 19 | normals.resize(indices_.size()); 20 | indices.resize(indices_.size()); 21 | for (int i = 0; i < indices_.size(); i += 3) { 22 | Eigen::Vector3f v1 = vertices_[indices_[i]]; 23 | Eigen::Vector3f v2 = vertices_[indices_[i + 1]]; 24 | Eigen::Vector3f v3 = vertices_[indices_[i + 2]]; 25 | 26 | Eigen::Vector3f n = (v2 - v1).cross(v3 - v2); 27 | 28 | for (int j = 0; j < 3; j++) { 29 | vertices[i + j] = vertices_[indices_[i + j]]; 30 | normals[i + j] = n; 31 | indices[i + j] = i + j; 32 | } 33 | } 34 | } 35 | 36 | public: 37 | std::vector> vertices; 38 | std::vector> normals; 39 | std::vector indices; 40 | }; 41 | 42 | /** 43 | * @brief A class to estimate normals of vertices 44 | * 45 | */ 46 | class NormalEstimater { 47 | public: 48 | NormalEstimater(const std::vector>& vertices, 49 | const std::vector& indices) { 50 | normals.resize(vertices.size(), Eigen::Vector3f::Zero()); 51 | for (int i = 0; i < indices.size(); i += 3) { 52 | Eigen::Vector3f v1 = vertices[indices[i]]; 53 | Eigen::Vector3f v2 = vertices[indices[i + 1]]; 54 | Eigen::Vector3f v3 = vertices[indices[i + 2]]; 55 | 56 | Eigen::Vector3f n = (v2 - v1).cross(v3 - v2); 57 | 58 | normals[indices[i]] += n; 59 | normals[indices[i + 1]] += n; 60 | normals[indices[i + 2]] += n; 61 | } 62 | 63 | for (auto& normal : normals) { 64 | normal.normalize(); 65 | } 66 | } 67 | 68 | public: 69 | std::vector> normals; 70 | }; 71 | 72 | } // namespace glk 73 | 74 | #endif -------------------------------------------------------------------------------- /include/glk/pointcloud_buffer.hpp: -------------------------------------------------------------------------------- 1 | #ifndef GLK_POINTCLOUD_BUFFER_HPP 2 | #define GLK_POINTCLOUD_BUFFER_HPP 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | #include 9 | #include 10 | 11 | #include "glk/drawble.hpp" 12 | 13 | namespace glk { 14 | 15 | class GLSLShader; 16 | 17 | class PointCloudBuffer : public Drawable { 18 | public: 19 | using Ptr = std::shared_ptr; 20 | 21 | explicit PointCloudBuffer(const std::string& cloud_filename); 22 | explicit PointCloudBuffer(const pcl::PointCloud::ConstPtr& cloud); 23 | explicit PointCloudBuffer(const std::vector& points); 24 | 25 | void free() override; 26 | 27 | void draw(glk::GLSLShader& shader) const override; 28 | 29 | private: 30 | GLuint vao{}; 31 | GLuint vbo{}; 32 | int stride_{}; 33 | int num_points_{}; 34 | }; 35 | 36 | } // namespace glk 37 | 38 | #endif -------------------------------------------------------------------------------- /include/glk/primitives/cone.hpp: -------------------------------------------------------------------------------- 1 | #ifndef GLK_PRIMITIVES_CONE_HPP 2 | #define GLK_PRIMITIVES_CONE_HPP 3 | 4 | #include 5 | #include 6 | 7 | namespace glk { 8 | 9 | class Cone { 10 | public: 11 | explicit Cone(int div = 10) { 12 | vertices.emplace_back(Eigen::Vector3f::Zero()); 13 | vertices.emplace_back(Eigen::Vector3f::UnitZ()); 14 | 15 | double step = 2.0 * M_PI / div; 16 | for (int i = 0; i < div; i++) { 17 | double rad = step * i; 18 | vertices.emplace_back(Eigen::Vector3f(std::cos(rad), std::sin(rad), 0.0f)); 19 | 20 | int current_index = i + 2; 21 | int next_index = ((i + 1) % div) + 2; 22 | 23 | indices.push_back(0); 24 | indices.push_back(current_index); 25 | indices.push_back(next_index); 26 | 27 | indices.push_back(current_index); 28 | indices.push_back(1); 29 | indices.push_back(next_index); 30 | } 31 | } 32 | 33 | public: 34 | std::vector> vertices; 35 | std::vector indices; 36 | }; 37 | 38 | } // namespace glk 39 | 40 | #endif -------------------------------------------------------------------------------- /include/glk/primitives/coordinate_system.hpp: -------------------------------------------------------------------------------- 1 | #ifndef GLK_PRIMITIVES_COORDINATE_SYSTEM_HPP 2 | #define GLK_PRIMITIVES_COORDINATE_SYSTEM_HPP 3 | 4 | #include 5 | #include 6 | 7 | namespace glk { 8 | 9 | class CoordinateSystem { 10 | public: 11 | CoordinateSystem() { 12 | vertices.emplace_back(Eigen::Vector3f::Zero()); 13 | vertices.emplace_back(Eigen::Vector3f::UnitX()); 14 | vertices.emplace_back(Eigen::Vector3f::Zero()); 15 | vertices.emplace_back(Eigen::Vector3f::UnitY()); 16 | vertices.emplace_back(Eigen::Vector3f::Zero()); 17 | vertices.emplace_back(Eigen::Vector3f::UnitZ()); 18 | 19 | colors.emplace_back(Eigen::Vector4f(1.0f, 0.0f, 0.0f, 1.0f)); 20 | colors.emplace_back(Eigen::Vector4f(1.0f, 0.0f, 0.0f, 1.0f)); 21 | colors.emplace_back(Eigen::Vector4f(0.0f, 1.0f, 0.0f, 1.0f)); 22 | colors.emplace_back(Eigen::Vector4f(0.0f, 1.0f, 0.0f, 1.0f)); 23 | colors.emplace_back(Eigen::Vector4f(0.0f, 0.0f, 1.0f, 1.0f)); 24 | colors.emplace_back(Eigen::Vector4f(0.0f, 0.0f, 1.0f, 1.0f)); 25 | } 26 | 27 | public: 28 | std::vector> vertices; 29 | std::vector> colors; 30 | }; 31 | } // namespace glk 32 | 33 | #endif -------------------------------------------------------------------------------- /include/glk/primitives/cube.hpp: -------------------------------------------------------------------------------- 1 | #ifndef GLK_PRIMITIVES_CUBE_HPP 2 | #define GLK_PRIMITIVES_CUBE_HPP 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | namespace glk { 9 | 10 | class Cube { 11 | public: 12 | Cube() { 13 | // 6 ------ 7 X 14 | // /| /| | 15 | // 4 ------ 5 | 0 --- Z 16 | // | | | | / 17 | // | 2 -----| 3 Y 18 | // |/ |/ 19 | // 0 ------ 1 20 | std::vector> vertices_ = { 21 | Eigen::Vector3f(-0.5f, -0.5f, -0.5f), // 0 22 | Eigen::Vector3f(-0.5f, -0.5f, 0.5f), // 1 23 | Eigen::Vector3f(-0.5f, 0.5f, -0.5f), // 2 24 | Eigen::Vector3f(-0.5f, 0.5f, 0.5f), // 3 25 | Eigen::Vector3f(0.5f, -0.5f, -0.5f), // 4 26 | Eigen::Vector3f(0.5f, -0.5f, 0.5f), // 5 27 | Eigen::Vector3f(0.5f, 0.5f, -0.5f), // 6 28 | Eigen::Vector3f(0.5f, 0.5f, 0.5f), // 7 29 | }; 30 | 31 | std::vector> normals_(vertices_.size()); 32 | for (int i = 0; i < vertices_.size(); i++) { 33 | normals_[i] = vertices_[i].normalized(); 34 | } 35 | 36 | std::vector indices_ = {0, 1, 2, 1, 3, 2, 1, 5, 3, 3, 5, 7, 0, 4, 1, 1, 4, 5, 37 | 3, 6, 2, 3, 7, 6, 0, 2, 4, 2, 6, 4, 4, 6, 5, 5, 6, 7}; 38 | 39 | this->vertices.swap(vertices_); 40 | this->indices.swap(indices_); 41 | } 42 | 43 | public: 44 | std::vector> vertices; 45 | std::vector> normals; 46 | std::vector indices; 47 | }; 48 | 49 | } // namespace glk 50 | 51 | #endif -------------------------------------------------------------------------------- /include/glk/primitives/grid.hpp: -------------------------------------------------------------------------------- 1 | #ifndef GLK_PRIMITIVES_GRID_HPP 2 | #define GLK_PRIMITIVES_GRID_HPP 3 | 4 | #include 5 | #include 6 | 7 | namespace glk { 8 | 9 | class Grid { 10 | public: 11 | explicit Grid(double half_extent = 5.0, double step = 1.0) { 12 | for (double x = -half_extent; x <= half_extent + 1e-9;) { 13 | vertices.emplace_back(Eigen::Vector3f(x, -half_extent, 0.0f)); 14 | vertices.emplace_back(Eigen::Vector3f(x, half_extent, 0.0f)); 15 | vertices.emplace_back(Eigen::Vector3f(-half_extent, x, 0.0f)); 16 | vertices.emplace_back(Eigen::Vector3f(half_extent, x, 0.0f)); 17 | x += step; 18 | } 19 | } 20 | 21 | public: 22 | std::vector> vertices; 23 | }; 24 | } // namespace glk 25 | 26 | #endif -------------------------------------------------------------------------------- /include/glk/primitives/icosahedron.hpp: -------------------------------------------------------------------------------- 1 | #ifndef GLK_PRIMITIVES_ICOSAHEDRON_HPP 2 | #define GLK_PRIMITIVES_ICOSAHEDRON_HPP 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | namespace glk { 9 | 10 | class Icosahedron { 11 | public: 12 | Icosahedron() { 13 | double t = (1.0 + std::sqrt(5.0)) / 2.0; 14 | 15 | std::vector> vertices_local = { 16 | Eigen::Vector3f(-1, t, 0), Eigen::Vector3f(1, t, 0), Eigen::Vector3f(-1, -t, 0), Eigen::Vector3f(1, -t, 0), 17 | Eigen::Vector3f(0, -1, t), Eigen::Vector3f(0, 1, t), Eigen::Vector3f(0, -1, -t), Eigen::Vector3f(0, 1, -t), 18 | Eigen::Vector3f(t, 0, -1), Eigen::Vector3f(t, 0, 1), Eigen::Vector3f(-t, 0, -1), Eigen::Vector3f(-t, 0, 1)}; 19 | 20 | std::vector> normals_local(vertices_local.size()); 21 | for (int i = 0; i < vertices_local.size(); i++) { 22 | normals_local[i] = vertices_local[i].normalized(); 23 | } 24 | 25 | std::vector indices_local = {0, 11, 5, 0, 5, 1, 0, 1, 7, 0, 7, 10, 0, 10, 11, 1, 5, 9, 5, 11, 26 | 4, 11, 10, 2, 10, 7, 6, 7, 1, 8, 3, 9, 4, 3, 4, 2, 3, 2, 6, 3, 27 | 6, 8, 3, 8, 9, 4, 9, 5, 2, 4, 11, 6, 2, 10, 8, 6, 7, 9, 8, 1}; 28 | 29 | this->vertices.swap(vertices_local); 30 | this->normals.swap(normals_local); 31 | this->indices.swap(indices_local); 32 | } 33 | 34 | void subdivide() { 35 | std::vector new_indices; 36 | for (int i = 0; i < indices.size(); i += 3) { 37 | int a = insert_middle_point(indices[i], indices[i + 1]); 38 | int b = insert_middle_point(indices[i + 1], indices[i + 2]); 39 | int c = insert_middle_point(indices[i + 2], indices[i]); 40 | 41 | std::vector tessellated = {indices[i], a, c, indices[i + 1], b, a, indices[i + 2], c, b, a, b, c}; 42 | 43 | new_indices.insert(new_indices.end(), tessellated.begin(), tessellated.end()); 44 | } 45 | 46 | indices.swap(new_indices); 47 | 48 | normals.resize(vertices.size()); 49 | for (int i = 0; i < vertices.size(); i++) { 50 | normals[i] = vertices[i].normalized(); 51 | } 52 | } 53 | 54 | void spherize() { 55 | for (auto &vertex : vertices) { 56 | vertex.normalize(); 57 | } 58 | 59 | normals.resize(vertices.size()); 60 | for (int i = 0; i < vertices.size(); i++) { 61 | normals[i] = vertices[i].normalized(); 62 | } 63 | } 64 | 65 | private: 66 | int insert_middle_point(int v1, int v2) { 67 | int smaller = std::min(v1, v2); 68 | int greater = std::max(v1, v2); 69 | int key = (smaller << 16) + greater; 70 | 71 | auto found = middle_points_cache.find(key); 72 | if (found != middle_points_cache.end()) { 73 | return found->second; 74 | } 75 | 76 | Eigen::Vector3f new_v = (vertices[v1] + vertices[v2]) / 2.0f; 77 | vertices.push_back(new_v); 78 | 79 | middle_points_cache.insert(found, std::make_pair(key, vertices.size() - 1)); 80 | return int(vertices.size() - 1); 81 | } 82 | 83 | public: 84 | std::vector> vertices; 85 | std::vector> normals; 86 | std::vector indices; 87 | 88 | std::map middle_points_cache; 89 | }; 90 | 91 | } // namespace glk 92 | 93 | #endif -------------------------------------------------------------------------------- /include/glk/primitives/primitives.hpp: -------------------------------------------------------------------------------- 1 | #ifndef GLK_PRIMITIVES_HPP 2 | #define GLK_PRIMITIVES_HPP 3 | 4 | #include 5 | #include 6 | 7 | namespace glk { 8 | class Drawable; 9 | } 10 | 11 | namespace glk { 12 | 13 | class Primitives { 14 | private: 15 | Primitives() { meshes.resize(NUM_PRIMITIVES, nullptr); } 16 | ~Primitives() { delete instance_; } 17 | 18 | public: 19 | enum PrimitiveType { ICOSAHEDRON = 0, SPHERE, CUBE, CONE, GRID, COORDINATE_SYSTEM, NUM_PRIMITIVES }; 20 | 21 | static Primitives *instance() { 22 | if (instance_ == nullptr) { 23 | instance_ = new Primitives(); 24 | } 25 | return instance_; 26 | } 27 | 28 | const glk::Drawable &primitive(PrimitiveType type); 29 | 30 | private: 31 | static Primitives *instance_; 32 | 33 | std::vector> meshes; 34 | }; 35 | } // namespace glk 36 | 37 | #endif -------------------------------------------------------------------------------- /include/glk/simple_lines.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | 6 | #include "GL/gl3w.h" 7 | #include "glk/drawble.hpp" 8 | 9 | namespace glk { 10 | 11 | /** 12 | * @brief A class to draw a set of simple lines 13 | * 14 | */ 15 | class SimpleLines : public Drawable { 16 | public: 17 | explicit SimpleLines(const std::vector>& vertices, 18 | const std::vector>& colors = 19 | std::vector>(), 20 | const std::vector>& infos = 21 | std::vector>()); 22 | ~SimpleLines() override; 23 | 24 | void draw(glk::GLSLShader& shader) const override; 25 | 26 | private: 27 | SimpleLines(const SimpleLines&); 28 | SimpleLines& operator=(const SimpleLines&); 29 | 30 | private: 31 | int num_vertices; 32 | // int num_indices; 33 | 34 | GLuint vao; // vertex array object 35 | GLuint vbo; // vertices 36 | GLuint cbo; // colors 37 | GLuint ibo; // infos 38 | // GLuint ebo{}; // elements 39 | }; 40 | } // namespace glk 41 | -------------------------------------------------------------------------------- /include/glk/texture.hpp: -------------------------------------------------------------------------------- 1 | #ifndef GLK_TEXTURE_HPP 2 | #define GLK_TEXTURE_HPP 3 | 4 | #include 5 | #include 6 | #include "GL/gl3w.h" 7 | 8 | namespace glk { 9 | 10 | /** 11 | * @brief OpenGL texture wrapper 12 | * 13 | */ 14 | class Texture { 15 | public: 16 | Texture(const Eigen::Vector2i& size, GLuint internal_format, GLuint format, 17 | GLuint type) // internal_format告诉OpenGL内部用什么格式存储和使用这个纹理数据 ， type是GL_RGBA的类型 18 | : width(size[0]), height(size[1]) { 19 | glGenTextures(1, &texture); 20 | glBindTexture(GL_TEXTURE_2D, texture); 21 | glTexImage2D(GL_TEXTURE_2D, 0, internal_format, size[0], size[1], 0, format, type, nullptr); 22 | glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_NEAREST); 23 | glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_NEAREST); 24 | glBindTexture(GL_TEXTURE_2D, 0); 25 | } 26 | 27 | ~Texture() { glDeleteRenderbuffers(1, &texture); } 28 | 29 | GLuint id() const { return texture; } 30 | Eigen::Vector2i size() const { return Eigen::Vector2i{width, height}; } 31 | 32 | template 33 | std::vector read_pixels(GLuint format = GL_RGBA, GLuint type = GL_UNSIGNED_BYTE) const { 34 | std::vector pixels(width * height * 4); 35 | glBindTexture(GL_TEXTURE_2D, texture); 36 | glGetTexImage(GL_TEXTURE_2D, 0, format, type, pixels.data()); 37 | return pixels; 38 | } 39 | 40 | private: 41 | int width; 42 | int height; 43 | GLuint texture{}; 44 | }; 45 | } // namespace glk 46 | 47 | #endif -------------------------------------------------------------------------------- /include/glk/texture_renderer.hpp: -------------------------------------------------------------------------------- 1 | #ifndef GLK_TEXTURE_RENDERER_HPP 2 | #define GLK_TEXTURE_RENDERER_HPP 3 | 4 | #include "GL/gl3w.h" 5 | #include "glk/glsl_shader.hpp" 6 | 7 | namespace glk { 8 | 9 | /** 10 | * @brief TextureRenderer 11 | * 12 | */ 13 | class TextureRenderer { 14 | public: 15 | explicit TextureRenderer(const std::string& data_directory) { 16 | std::vector> vertices = { 17 | Eigen::Vector3f(-1.0f, -1.0f, 0.0f), Eigen::Vector3f(1.0f, -1.0f, 0.0f), Eigen::Vector3f(-1.0f, 1.0f, 0.0f), 18 | Eigen::Vector3f(1.0f, 1.0f, 0.0f)}; 19 | 20 | glGenVertexArrays(1, &vao); 21 | glBindVertexArray(vao); 22 | 23 | glGenBuffers(1, &vbo); 24 | glBindBuffer(GL_ARRAY_BUFFER, vbo); 25 | glBufferData(GL_ARRAY_BUFFER, sizeof(float) * 3 * vertices.size(), vertices.data(), GL_STATIC_DRAW); 26 | 27 | if (!shader.init(data_directory + "/shader/texture")) { 28 | return; 29 | } 30 | 31 | shader.use(); 32 | GLint position_loc = shader.attrib("vert_position"); 33 | 34 | glBindVertexArray(vao); 35 | glEnableVertexAttribArray(position_loc); 36 | 37 | glBindBuffer(GL_ARRAY_BUFFER, vbo); 38 | glVertexAttribPointer(0, 3, GL_FLOAT, GL_FALSE, 0, nullptr); 39 | 40 | glBindBuffer(GL_ARRAY_BUFFER, 0); 41 | // glDisableVertexAttribArray(0); 42 | } 43 | 44 | ~TextureRenderer() { 45 | glDeleteVertexArrays(1, &vao); 46 | glDeleteBuffers(1, &vbo); 47 | } 48 | 49 | void draw(GLuint texture) { 50 | shader.use(); 51 | 52 | glEnable(GL_TEXTURE_2D); 53 | glBindTexture(GL_TEXTURE_2D, texture); 54 | 55 | glBindVertexArray(vao); 56 | glDrawArrays(GL_TRIANGLE_STRIP, 0, 4); 57 | glBindBuffer(GL_ARRAY_BUFFER, 0); 58 | 59 | glDisable(GL_TEXTURE_2D); 60 | } 61 | 62 | private: 63 | GLuint vao{}; 64 | GLuint vbo{}; 65 | 66 | glk::GLSLShader shader; 67 | }; 68 | 69 | } // namespace glk 70 | 71 | #endif -------------------------------------------------------------------------------- /include/guik/camera_control.hpp: -------------------------------------------------------------------------------- 1 | #ifndef GLK_CAMERA_CONTROL_HPP 2 | #define GLK_CAMERA_CONTROL_HPP 3 | 4 | #include 5 | #include "GL/gl3w.h" 6 | #include 7 | #include 8 | 9 | namespace guik { 10 | 11 | /** 12 | * @brief A class to contol camera position with mouse 13 | * 14 | */ 15 | class CameraControl { 16 | public: 17 | virtual ~CameraControl() = default; 18 | 19 | /** @brief mouse button callback */ 20 | virtual void mouse(const Eigen::Vector2i& p, int button, bool down) = 0; 21 | 22 | /** @brief mouse dragging callback */ 23 | virtual void drag(const Eigen::Vector2i& p, int button) = 0; 24 | 25 | /** @brief mouse scroll callback */ 26 | virtual void scroll(const Eigen::Vector2f& rel) = 0; 27 | 28 | /** @brief camera view matrix */ 29 | virtual Eigen::Matrix4f view_matrix() const = 0; 30 | 31 | virtual double access_distance() const = 0; 32 | }; 33 | 34 | /** 35 | * @brief A simple arctic camera control implementation 36 | * 37 | */ 38 | class ArcCameraControl : public CameraControl { 39 | public: 40 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 41 | 42 | ArcCameraControl(); 43 | 44 | ~ArcCameraControl() override; 45 | 46 | void mouse(const Eigen::Vector2i& p, int button, bool down) override; 47 | void drag(const Eigen::Vector2i& p, int button) override; 48 | void scroll(const Eigen::Vector2f& rel) override; 49 | 50 | Eigen::Quaternionf rotation() const; 51 | Eigen::Matrix4f view_matrix() const override; 52 | double access_distance() const override; 53 | 54 | private: 55 | Eigen::Vector3f center; 56 | double distance; 57 | 58 | Eigen::Vector2i drag_last_pos; 59 | 60 | bool left_button_down; 61 | double theta; 62 | double phi; 63 | 64 | bool middle_button_down; 65 | }; 66 | 67 | } // namespace guik 68 | 69 | #endif -------------------------------------------------------------------------------- /include/guik/gl_canvas.hpp: -------------------------------------------------------------------------------- 1 | #ifndef GLK_GL_CANVAS_CANVAS_HPP 2 | #define GLK_GL_CANVAS_CANVAS_HPP 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | 10 | #include "glk/text_renderer.hpp" 11 | 12 | namespace glk { 13 | class GLSLShader; 14 | class FrameBuffer; 15 | class TextureRenderer; 16 | } // namespace glk 17 | 18 | namespace guik { 19 | class CameraControl; 20 | class ProjectionControl; 21 | } // namespace guik 22 | 23 | namespace guik { 24 | 25 | class Parameter { 26 | public: 27 | Parameter(std::string text, float x, float y, float scale, glm::vec3 color = glm::vec3(0.3, 0.3, 0.8)) 28 | : text_(std::move(text)), bl_x_(x), bl_y_(y), scale_(scale), color_(color) {} 29 | 30 | public: 31 | std::string text_; 32 | float bl_x_, bl_y_, scale_; 33 | glm::vec3 color_; 34 | }; 35 | 36 | /** 37 | * @brief OpenGL canvas for imgui 38 | * 39 | */ 40 | class GLCanvas { 41 | public: 42 | GLCanvas(const std::string& data_directory, const Eigen::Vector2i& size); 43 | 44 | bool ready() const; 45 | 46 | void reset_camera(); 47 | void set_size(const Eigen::Vector2i& fb_size); 48 | void mouse_control() const; 49 | 50 | void bind(bool clear_buffers = true) const; 51 | void unbind() const; 52 | 53 | void render_to_screen(int color_buffer_id = 0); 54 | 55 | Eigen::Vector4i pick_info(const Eigen::Vector2i& p, int window = 2) const; 56 | float pick_depth(const Eigen::Vector2i& p, int window = 2) const; 57 | Eigen::Vector3f unproject(const Eigen::Vector2i& p, float depth) const; 58 | 59 | void show_shader_setting(); 60 | 61 | void draw_ui(); 62 | void show_projection_setting() const; 63 | 64 | std::pair transformation_matrices() const; 65 | Eigen::Vector2i window_size() const; 66 | double viewer_diatance() const; 67 | 68 | public: 69 | Eigen::Vector2i size; 70 | std::unique_ptr shader; 71 | std::unique_ptr frame_buffer; 72 | std::unique_ptr texture_renderer; 73 | std::unique_ptr text_renderer; 74 | std::vector text_renderer_params; 75 | 76 | std::unique_ptr camera_control; 77 | std::unique_ptr projection_control; 78 | 79 | private: 80 | bool show_window{false}; 81 | float point_size; 82 | float min_z; 83 | float max_z; 84 | bool z_clipping; 85 | }; 86 | 87 | } // namespace guik 88 | 89 | #endif -------------------------------------------------------------------------------- /include/guik/imgui_application.hpp: -------------------------------------------------------------------------------- 1 | #ifndef GUIK_IMGUI_APPLICATION_HPP 2 | #define GUIK_IMGUI_APPLICATION_HPP 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | class GLFWwindow; 9 | 10 | namespace guik { 11 | class Application { 12 | public: 13 | Application(); 14 | virtual ~Application(); 15 | 16 | virtual bool init(const char* window_name, const char* imgui_config_path, const Eigen::Vector2i& size, 17 | const char* glsl_version); 18 | 19 | virtual void free() {} 20 | 21 | void run(); 22 | 23 | void close(); 24 | 25 | Eigen::Vector2i framebuffer_size(); 26 | virtual void framebuffer_size_callback(const Eigen::Vector2i& size); 27 | 28 | virtual void draw_ui(); 29 | 30 | virtual void draw_gl(); 31 | 32 | protected: 33 | GLFWwindow* window; 34 | }; 35 | 36 | } // namespace guik 37 | 38 | #endif -------------------------------------------------------------------------------- /include/guik/progress_interface.hpp: -------------------------------------------------------------------------------- 1 | #ifndef GUIK_PROGRESS_INTERFACE_HPP 2 | #define GUIK_PROGRESS_INTERFACE_HPP 3 | 4 | #include 5 | 6 | namespace guik { 7 | 8 | class ProgressInterface { 9 | public: 10 | ProgressInterface() = default; 11 | 12 | virtual ~ProgressInterface() = default; 13 | 14 | virtual void set_title(const std::string &title) {} 15 | 16 | virtual void set_text(const std::string &text) {} 17 | 18 | virtual void set_maximum(int max) {} 19 | 20 | virtual void set_current(int current) {} 21 | 22 | virtual void increment() {} 23 | }; 24 | 25 | } // namespace guik 26 | 27 | #endif -------------------------------------------------------------------------------- /include/guik/progress_modal.hpp: -------------------------------------------------------------------------------- 1 | #ifndef GUIK_PROGRESS_MODAL_HPP 2 | #define GUIK_PROGRESS_MODAL_HPP 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | 11 | #include "imgui.h" 12 | #include 13 | 14 | #include "guik/progress_interface.hpp" 15 | 16 | namespace guik { 17 | 18 | class ProgressModal : public ProgressInterface { 19 | public: 20 | explicit ProgressModal(std::string modal_name) 21 | : modal_name_(std::move(modal_name)), running_(false), max_(0), current_(0) {} 22 | 23 | ~ProgressModal() override { 24 | if (thread_.joinable()) { 25 | thread_.join(); 26 | } 27 | } 28 | 29 | void set_title(const std::string &title) override { 30 | std::lock_guard lock(mutex); 31 | this->title_ = title; 32 | } 33 | 34 | void set_text(const std::string &text) override { 35 | std::lock_guard lock(mutex); 36 | this->text_ = text; 37 | } 38 | 39 | void set_maximum(int max) override { this->max_ = max; } 40 | 41 | void set_current(int current) override { this->current_ = current; } 42 | 43 | void increment() override { current_++; } 44 | 45 | template 46 | void open(const std::string &task_name, const std::function &task) { 47 | this->task_name_ = task_name; 48 | this->title_.clear(); 49 | this->text_.clear(); 50 | 51 | ImGui::OpenPopup(modal_name_.c_str()); 52 | result_.clear(); 53 | running_ = true; 54 | current_ = 0; 55 | 56 | thread_ = std::thread([this, task]() { 57 | result_ = task(*this); 58 | running_ = false; 59 | }); 60 | } 61 | 62 | template 63 | T result() { 64 | T ret = boost::any_cast(result_); 65 | result_.clear(); 66 | return ret; 67 | } 68 | 69 | bool run(const std::string &task_name) { 70 | if (task_name != this->task_name_) { 71 | return false; 72 | } 73 | 74 | bool terminated = false; 75 | if (ImGui::BeginPopupModal(modal_name_.c_str(), nullptr, 76 | ImGuiWindowFlags_AlwaysAutoResize | ImGuiWindowFlags_NoTitleBar)) { 77 | { 78 | std::lock_guard lock(mutex); 79 | if (!title_.empty()) { 80 | ImGui::Text("%s", title_.c_str()); 81 | } 82 | 83 | ImGui::Text("%c %s", "|/-\\"[(int)(ImGui::GetTime() / 0.05f) & 3], text_.c_str()); 84 | } 85 | 86 | float fraction = current_ / static_cast(max_); 87 | ImGui::ProgressBar(fraction, ImVec2(256, 16)); 88 | 89 | if (!running_) { 90 | thread_.join(); 91 | ImGui::CloseCurrentPopup(); 92 | terminated = true; 93 | } 94 | ImGui::EndPopup(); 95 | } 96 | 97 | return terminated; 98 | } 99 | 100 | private: 101 | std::mutex mutex; 102 | std::string modal_name_; 103 | std::string title_; 104 | std::string text_; 105 | 106 | std::atomic_bool running_; 107 | std::atomic_int max_; 108 | std::atomic_int current_; 109 | 110 | std::string task_name_; 111 | 112 | std::thread thread_; 113 | boost::any result_; 114 | }; 115 | 116 | } // namespace guik 117 | 118 | #endif -------------------------------------------------------------------------------- /include/guik/projection_control.hpp: -------------------------------------------------------------------------------- 1 | #ifndef PROJECTION_CONTROL_HPP 2 | #define PROJECTION_CONTROL_HPP 3 | 4 | #include 5 | 6 | namespace guik { 7 | 8 | class ProjectionControl { 9 | public: 10 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 11 | 12 | explicit ProjectionControl(Eigen::Vector2i size); 13 | ~ProjectionControl(); 14 | 15 | void set_size(const Eigen::Vector2i& _size) { this->size = _size; } 16 | 17 | Eigen::Matrix4f projection_matrix() const; 18 | 19 | void draw_ui(); 20 | 21 | void show(); 22 | 23 | private: 24 | bool b_show_window; 25 | Eigen::Vector2i size; 26 | 27 | int projection_mode; 28 | 29 | float fovy; 30 | float width; 31 | float near; 32 | float far; 33 | }; 34 | 35 | } // namespace guik 36 | 37 | #endif -------------------------------------------------------------------------------- /include/robot_basic_tools.hpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include "util/tf_tree.hpp" 4 | #include "guik/imgui_application.hpp" 5 | 6 | namespace dev { 7 | class SensorManager; 8 | class AprilBoard; 9 | class chessboard; 10 | } // namespace dev 11 | 12 | namespace calibration { 13 | class CamLaserCalib; 14 | class TwoLasersCalib; 15 | class TwoCamerasCalib; 16 | class CameraCalib; 17 | } // namespace calibration 18 | 19 | class RobotBasicTools : public guik::Application { 20 | public: 21 | explicit RobotBasicTools(ros::NodeHandle &ros_nh) : Application(), nh_(ros_nh) {} 22 | 23 | ~RobotBasicTools() override = default; 24 | 25 | /** 26 | * @brief initialize the application 27 | * @param window_name 28 | * @param size 29 | * @param glsl_version 30 | * @return true 31 | * @return false 32 | */ 33 | bool init(const char *window_name, const char *imgui_config_path, const Eigen::Vector2i &size, 34 | const char *glsl_version) override; 35 | 36 | /** 37 | * @brief draw ImGui-based UI 38 | * 39 | */ 40 | void draw_ui() override; 41 | 42 | /** 43 | * @brief draw OpenGL related stuffs on the main canvas 44 | * 45 | */ 46 | void draw_gl() override; 47 | 48 | /** 49 | * @brief frame buffer size change callback 50 | * 51 | * @param size 52 | */ 53 | void framebuffer_size_callback(const Eigen::Vector2i &size) override { main_canvas_ptr_->set_size(size); } 54 | 55 | void free() override; 56 | 57 | private: 58 | /** 59 | * @brief draw main menu 60 | * 61 | */ 62 | void main_menu(); 63 | 64 | /** 65 | * @brief handling mouse input 66 | */ 67 | void mouse_control(); 68 | 69 | /** 70 | * @brief context menu 71 | */ 72 | void context_menu(); 73 | 74 | private: 75 | // ros nodehandle 76 | ros::NodeHandle &nh_; 77 | // 鼠标右击位置 78 | Eigen::Vector2i right_clicked_pos_; 79 | // 当前鼠标位置 80 | Eigen::Vector2i cur_mouse_pos_; 81 | 82 | // 主画布 83 | std::shared_ptr main_canvas_ptr_; 84 | 85 | // 进度条 86 | std::unique_ptr progress_ptr_; 87 | 88 | // 传感器管理器 89 | std::shared_ptr sensor_manager_ptr_; 90 | 91 | // APRILTAG标定板 92 | std::shared_ptr april_board_ptr_; 93 | 94 | // 标准棋盘格标定板 95 | std::shared_ptr chess_board_ptr_; 96 | 97 | // 标准圆形标定板 98 | std::shared_ptr blob_board_ptr_; 99 | 100 | // 相机与单线激光标定 101 | std::unique_ptr cl_calib_ptr_; 102 | 103 | // 两个单线激光标定 104 | std::unique_ptr tl_calib_ptr_; 105 | 106 | // 两个相机标定 107 | std::unique_ptr tc_calib_ptr_; 108 | 109 | // 单目相机标定 110 | std::unique_ptr cam_calib_ptr_; 111 | 112 | // tf树 113 | std::shared_ptr tf_tree_ptr_; 114 | 115 | // 热键标记(ALT) 116 | bool b_hotkey_alt_pressed_ = false; 117 | // 选中的物体集合 118 | // std::set selected_id; 119 | 120 | // ui显示标记 121 | bool b_show_imgui_demo_{false}; 122 | }; 123 | -------------------------------------------------------------------------------- /include/util/draw_frame_selector.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include 3 | namespace util { 4 | 5 | void draw_frame_selector(const std::string& name); 6 | 7 | } // namespace util -------------------------------------------------------------------------------- /include/util/extrinsics_publisher.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | namespace util { 3 | 4 | bool publish_extrinsics(ros::NodeHandle& nh_, std::array ext, const std::string& to_frame, 5 | const std::string& from_frame, const std::string& child_selected, 6 | const std::string& parent_selected); 7 | 8 | } // namespace util -------------------------------------------------------------------------------- /include/util/image_loader.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | namespace util { 3 | 4 | /// Simple helper function to load an image into a OpenGL texture with common settings 5 | bool LoadTextureFromFile(const std::string &filename, unsigned int &img_texture, int &img_width, int &img_height); 6 | 7 | } // namespace util -------------------------------------------------------------------------------- /include/util/singleton.hpp: -------------------------------------------------------------------------------- 1 | // reference to https://my.oschina.net/u/3102542/blog/1563222 2 | 3 | #pragma once 4 | 5 | namespace util { 6 | 7 | #include 8 | #include 9 | 10 | template 11 | class Singleton { 12 | public: 13 | //获取全局单例对象 14 | template 15 | static std::shared_ptr instance(Args&&... args) { 16 | static std::shared_ptr singleton_inst_; 17 | static std::mutex mtx_; 18 | if (!singleton_inst_) { 19 | std::lock_guard gLock(mtx_); 20 | if (nullptr == singleton_inst_) { 21 | singleton_inst_ = std::make_shared(std::forward(args)...); 22 | } 23 | } 24 | return singleton_inst_; 25 | } 26 | 27 | /// 禁用拷贝构造 28 | Singleton(const Singleton&) = delete; 29 | 30 | /// 禁用赋值操作 31 | Singleton& operator=(const Singleton&) = delete; 32 | 33 | private: 34 | explicit Singleton() = default; 35 | }; 36 | 37 | // template 38 | // std::shared_ptr Singleton::singleton_inst_{nullptr}; 39 | // 40 | // template 41 | // std::mutex Singleton::mtx_; 42 | 43 | } // namespace util 44 | -------------------------------------------------------------------------------- /include/util/tf_tree.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | 7 | #include 8 | #include 9 | #include 10 | 11 | namespace util { 12 | 13 | class TfTree { 14 | public: 15 | explicit TfTree(const ros::NodeHandle& nh); 16 | void show(); 17 | void draw_ui(); 18 | std::vector get_major_frames(); 19 | std::vector get_tf_major_frames(); 20 | std::vector get_tf_transforms(); 21 | void print_frames(); 22 | 23 | public: 24 | void tf_static_callback(const tf2_msgs::TFMessage& msg); 25 | void tf_callback(const tf2_msgs::TFMessage& msg); 26 | 27 | private: 28 | ros::NodeHandle ros_nh_; 29 | ros::Subscriber tf_static_sub_; 30 | ros::Subscriber tf_sub_; 31 | 32 | std::vector tfs_transforms_; 33 | std::set tfs_frames_set_; 34 | std::vector tfs_frames_vector_; 35 | std::vector tfs_major_frames_; 36 | 37 | std::vector tf_transforms_; 38 | std::set> tf_frame_pairs_; 39 | std::set tf_frames_set_; 40 | std::vector tf_frames_vector_; 41 | std::vector tf_major_frames_; 42 | 43 | ImGuiWindowFlags window_flags_ = 0; 44 | bool b_show_window_{false}; 45 | }; 46 | 47 | } // namespace util 48 | -------------------------------------------------------------------------------- /include/version_info.h.in: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #cmakedefine USE_VERSION_INFO 4 | #ifdef USE_VERSION_INFO 5 | #define VERSION_MAJOR @VERSION_MAJOR@ 6 | #define VERSION_MINOR @VERSION_MINOR@ 7 | #define VERSION_PATCH @VERSION_PATCH@ 8 | #endif 9 | 10 | #define BUILD_TIMESTAMP "@BUILD_TIMESTAMP@" 11 | 12 | #define GIT_BRANCH "@GIT_BRANCH@" 13 | #define GIT_HASH "@GIT_HASH@" 14 | -------------------------------------------------------------------------------- /launch/run.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | -------------------------------------------------------------------------------- /launch/sim.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | robot_basic_tools 4 | 1.0.0 5 | The robot_basic_tools package 6 | 7 | hardjet 8 | 9 | GPLv3 10 | 11 | roscpp 12 | roslib 13 | sensor_msgs 14 | std_msgs 15 | geometry_msgs 16 | message_generation 17 | 18 | catkin 19 | 20 | message_runtime 21 | roscpp 22 | tf2 23 | tf2_ros 24 | tf2_geometry_msgs 25 | std_msgs 26 | geometry_msgs 27 | sensor_msgs 28 | 29 | 30 | 31 | 32 | -------------------------------------------------------------------------------- /src/algorithm/line_fitting.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include "ceres/ceres.h" 4 | #include "algorithm/util.h" 5 | 6 | namespace algorithm { 7 | 8 | struct LineFittingResidfual { 9 | LineFittingResidfual(double x, double y) : x_(x), y_(y) {} 10 | 11 | template 12 | bool operator()(const T *const m, T *residual) const { 13 | residual[0] = m[0] * T(x_) + m[1] * T(y_) + T(1.); 14 | return true; 15 | } 16 | 17 | private: 18 | // Observations for a sample. 19 | const double x_; 20 | const double y_; 21 | }; 22 | 23 | /** 24 | * 25 | * @param points 26 | * @param line_params 直线模型 Ax + By + 1 = 0 27 | */ 28 | bool line_fitting_ceres(const std::vector &points, Eigen::Vector2d &line_params) { 29 | double line[2] = {line_params(0), line_params(1)}; 30 | 31 | ceres::Problem problem; 32 | for (auto obi : points) { 33 | // debug 34 | // std::cout << obi.transpose() << std::endl; 35 | 36 | ceres::CostFunction *costfunction = 37 | new ceres::AutoDiffCostFunction(new LineFittingResidfual(obi.x(), obi.y())); 38 | 39 | // ceres::LossFunctionWrapper* loss_function(new ceres::HuberLoss(1.0), ceres::TAKE_OWNERSHIP); 40 | ceres::LossFunction *loss_function = new ceres::CauchyLoss(0.05); 41 | problem.AddResidualBlock(costfunction, loss_function, line); 42 | } 43 | 44 | ceres::Solver::Options options; 45 | options.linear_solver_type = ceres::DENSE_QR; 46 | options.max_num_iterations = 20; 47 | 48 | ceres::Solver::Summary summary; 49 | ceres::Solve(options, &problem, &summary); 50 | 51 | // std::cout << summary.FullReport() << std::endl; 52 | 53 | line_params(0) = line[0]; 54 | line_params(1) = line[1]; 55 | 56 | return summary.IsSolutionUsable(); 57 | } 58 | 59 | } // namespace algorithm 60 | -------------------------------------------------------------------------------- /src/algorithm/random/RandomGenerator.cpp: -------------------------------------------------------------------------------- 1 | /* +------------------------------------------------------------------------+ 2 | | Mobile Robot Programming Toolkit (MRPT) | 3 | | https://www.mrpt.org/ | 4 | | | 5 | | Copyright (c) 2005-2021, Individual contributors, see AUTHORS file | 6 | | See: https://www.mrpt.org/Authors - All rights reserved. | 7 | | Released under BSD License. See: https://www.mrpt.org/License | 8 | +------------------------------------------------------------------------+ */ 9 | 10 | #include "algorithm/random/RandomGenerators.h" 11 | 12 | using namespace algorithm::random; 13 | 14 | // The global instance of CRandomGenerator for single-thread programs: 15 | static CRandomGenerator randomGenerator; 16 | 17 | // Code from the implementation by Rick Wagner 18 | // http://www-personal.umich.edu/~wagnerr/MersenneTwister.html 19 | inline uint32_t hiBit(const uint32_t u) { return u & 0x80000000UL; } 20 | inline uint32_t loBit(const uint32_t u) { return u & 0x00000001UL; } 21 | inline uint32_t loBits(const uint32_t u) { return u & 0x7fffffffUL; } 22 | inline uint32_t mixBits(const uint32_t u, const uint32_t v) { return hiBit(u) | loBits(v); } 23 | inline uint32_t twist(const uint32_t m, const uint32_t s0, const uint32_t s1) { 24 | return m ^ (mixBits(s0, s1) >> 1) ^ (-loBit(s1) & 0x9908b0dfUL); 25 | } 26 | 27 | Generator_MT19937::result_type Generator_MT19937::operator()() { 28 | if (!m_index) generateNumbers(); 29 | 30 | uint32_t y = m_MT[m_index]; 31 | y ^= y >> 11; 32 | y ^= (y << 7) & 2636928640U; // 0x9d2c5680 33 | y ^= (y << 15) & 4022730752U; // 0xefc60000 34 | y ^= (y >> 18); 35 | 36 | // Wrap index to [0,623]. 37 | m_index++; 38 | if (m_index >= 624) m_index = 0; 39 | 40 | return y; 41 | } 42 | 43 | void Generator_MT19937::generateNumbers() { 44 | if (!m_seed_initialized) this->seed(std::random_device{}()); 45 | 46 | // Code from the implementation by Rick Wagner 47 | // http://www-personal.umich.edu/~wagnerr/MersenneTwister.html 48 | // and: 49 | // http://www.math.sci.hiroshima-u.ac.jp/~m-mat/MT/MT2002/CODES/mt19937ar.c 50 | // 51 | const int N = 624; // length of state vector 52 | const int M = 397; // period parameter 53 | 54 | uint32_t* p = m_MT; 55 | for (int i = N - M; i--; ++p) *p = twist(p[M], p[0], p[1]); 56 | for (int i = M; --i; ++p) *p = twist(p[M - N], p[0], p[1]); 57 | *p = twist(p[M - N], p[0], m_MT[0]); 58 | } 59 | 60 | void Generator_MT19937::seed(const uint32_t seed) { 61 | m_seed_initialized = true; 62 | m_MT[0] = seed; 63 | // 0x6c078965 64 | for (uint32_t i = 1; i < 624; i++) 65 | m_MT[i] = static_cast(1812433253 * (m_MT[i - 1] ^ (m_MT[i - 1] >> 30)) + i); 66 | 67 | m_index = 0; 68 | } 69 | 70 | CRandomGenerator& algorithm::random::getRandomGenerator() { return randomGenerator; } 71 | uint64_t CRandomGenerator::drawUniform64bit() { return m_uint64(m_MT19937); } 72 | uint32_t CRandomGenerator::drawUniform32bit() { return m_uint32(m_MT19937); } 73 | void CRandomGenerator::randomize(const uint32_t seed) { m_MT19937.seed(seed); } 74 | 75 | void CRandomGenerator::randomize() { m_MT19937.seed(std::random_device{}()); } 76 | 77 | double CRandomGenerator::drawGaussian1D_normalized() { return m_normdistribution(m_MT19937); } 78 | -------------------------------------------------------------------------------- /src/algorithm/two_cameras_ceres.cpp: -------------------------------------------------------------------------------- 1 | #include "ceres/ceres.h" 2 | 3 | #include "camera_model/camera_models/Camera.h" 4 | #include "camera_model/camera_models/CostFunctionFactory.h" 5 | #include "camera_model/gpl/EigenQuaternionParameterization.h" 6 | #include "camera_model/gpl/EigenUtils.h" 7 | #include "camera_model/sparse_graph/Transform.h" 8 | 9 | #include "algorithm/util.h" 10 | #include "algorithm/two_cameras_ceres.h" 11 | 12 | namespace algorithm { 13 | namespace TwoCamerasCalib { 14 | 15 | bool calibrate(const boost::shared_ptr &camera_ptr, const Observation &obs, 16 | Eigen::Matrix4d &T_21) { 17 | // 1到2的旋转变换 18 | // Eigen::Quaterniond q_21(T_21.block<3, 3>(0, 0)); 19 | 20 | // 打印初始信息 21 | // EulerAngles euler = quat2euler(q_21); 22 | // std::cout << "euler_init: " << euler << std::endl; 23 | // std::cout << "T_21_init: \n" << T_21 << std::endl; 24 | 25 | camera_model::Transform transform_21(T_21); 26 | 27 | // 构建优化问题 28 | ceres::Problem problem; 29 | for (uint i = 0; i < obs.object_points.size(); i++) { 30 | ceres::CostFunction *costFunction = camera_model::CostFunctionFactory::instance()->generateCostFunction( 31 | camera_ptr, obs.object_points.at(i), obs.image_points.at(i), camera_model::CAMERA_POSE); 32 | 33 | ceres::LossFunction *lossFunction = new ceres::CauchyLoss(1.0); 34 | problem.AddResidualBlock(costFunction, lossFunction, transform_21.rotationData(), transform_21.translationData()); 35 | } 36 | 37 | // 添加参数块 38 | ceres::LocalParameterization *quaternionParameterization = new camera_model::EigenQuaternionParameterization; 39 | problem.SetParameterization(transform_21.rotationData(), quaternionParameterization); 40 | 41 | // 设置求解器 42 | ceres::Solver::Options options; 43 | // options.minimizer_progress_to_stdout = true; 44 | options.max_num_iterations = 1000; 45 | options.num_threads = 4; 46 | options.linear_solver_type = ceres::DENSE_QR; 47 | options.trust_region_strategy_type = ceres::DOGLEG; 48 | // options.logging_type = ceres::SILENT; 49 | 50 | ceres::Solver::Summary summary; 51 | ceres::Solve(options, &problem, &summary); 52 | 53 | // std::cout << summary.FullReport() << std::endl; 54 | 55 | T_21 = transform_21.toMatrix(); 56 | // euler = algorithm::quat2euler(transform_21.rotation()); 57 | // std::cout << "afrer opt: " << euler << std::endl; 58 | // std::cout << "T_21:\n" << T_21 << std::endl; 59 | 60 | return summary.IsSolutionUsable(); 61 | } 62 | } // namespace TwoCamerasCalib 63 | } // namespace algorithm 64 | -------------------------------------------------------------------------------- /src/calibration/calib_base.cpp: -------------------------------------------------------------------------------- 1 | //#include "imgui.h" 2 | // 3 | //#include "dev/camera.hpp" 4 | //#include "dev/sensor.hpp" 5 | //#include "dev/sensor_manager.hpp" 6 | //#include "calibration/calibration_state.hpp" 7 | //#include "calibration/calib_base.hpp" 8 | 9 | namespace calibration {} // namespace calibration 10 | -------------------------------------------------------------------------------- /src/calibration/calibration_state.cpp: -------------------------------------------------------------------------------- 1 | #include "calibration/calibration_state.hpp" 2 | #include "calibration/camera_calib.hpp" 3 | 4 | namespace calibration { 5 | 6 | std::string StateIdle::name() { return "STATE_IDLE"; } 7 | std::string StateStart::name() { return "STATE_START"; } 8 | std::string StateGetPose::name() { return "STATE_GET_POSE"; } 9 | std::string StateCheckSteady::name() { return "STATE_CHECK_STEADY"; } 10 | std::string StateStartCalib::name() { return "STATE_START_CALIB"; } 11 | std::string StateInCalib::name() { return "STATE_IN_CALIB"; } 12 | 13 | int StateIdle::id() { return 0; } 14 | int StateStart::id() { return 1; } 15 | int StateGetPose::id() { return 2; } 16 | int StateCheckSteady::id() { return 3; } 17 | int StateStartCalib::id() { return 4; } 18 | int StateInCalib::id() { return 5; } 19 | 20 | void StateIdle::calibration() { calib_context_->change_current_state(calib_context_->next_state()); } 21 | 22 | void StateStart::calibration() { 23 | if (calib_context_->instrument_available()) { 24 | calib_context_->change_current_state(std::make_shared()); 25 | } else { 26 | calib_context_->change_current_state(std::make_shared()); 27 | } 28 | } 29 | 30 | void StateGetPose::calibration() { 31 | if (calib_context_->pose_valid() == 1) { 32 | calib_context_->change_current_state(std::make_shared()); 33 | } else if (calib_context_->pose_valid() == 2) { 34 | calib_context_->change_current_state(std::make_shared()); 35 | } 36 | } 37 | 38 | void StateCheckSteady::calibration() { 39 | calib_context_->check_steady(); 40 | calib_context_->change_current_state(std::make_shared()); 41 | } 42 | 43 | void StateStartCalib::calibration() { calib_context_->change_current_state(std::make_shared()); } 44 | 45 | void StateInCalib::calibration() { 46 | if (calib_context_->do_calib() == 1) { 47 | calib_context_->change_current_state(std::make_shared()); 48 | } else if (calib_context_->do_calib() == 2) { 49 | std::cout << "calibration failed!!!" << std::endl; 50 | } 51 | } 52 | 53 | } // namespace calibration -------------------------------------------------------------------------------- /src/dev/util.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include "imgui.h" 4 | #include "portable-file-dialogs.h" 5 | #include "dev/util.hpp" 6 | 7 | namespace dev { 8 | 9 | void help_marker(const char* desc) { 10 | ImGui::TextDisabled("(?)"); 11 | if (ImGui::IsItemHovered()) { 12 | ImGui::BeginTooltip(); 13 | ImGui::PushTextWrapPos(ImGui::GetFontSize() * 35.0f); 14 | ImGui::TextUnformatted(desc); 15 | ImGui::PopTextWrapPos(); 16 | ImGui::EndTooltip(); 17 | } 18 | } 19 | 20 | void get_topic_name_from_list(const std::string& target_topic_type, std::vector& candidates) { 21 | ros::master::V_TopicInfo master_topics; 22 | ros::master::getTopics(master_topics); 23 | 24 | candidates.clear(); 25 | 26 | for (auto& info : master_topics) { 27 | if (info.datatype == target_topic_type) { 28 | candidates.push_back(info.name); 29 | } 30 | } 31 | } 32 | 33 | void show_pfd_info(const std::string& title, const std::string& msg) { 34 | pfd::message message(title, msg, pfd::choice::ok); 35 | while (!message.ready()) { 36 | usleep(1000); 37 | } 38 | } 39 | 40 | } // namespace dev -------------------------------------------------------------------------------- /src/glk/frame_buffer.cpp: -------------------------------------------------------------------------------- 1 | #include "glk/texture.hpp" 2 | #include "glk/frame_buffer.hpp" 3 | 4 | namespace glk { 5 | 6 | FrameBuffer::FrameBuffer(const Eigen::Vector2i& size) : width(size[0]), height(size[1]) { 7 | color_buffers.push_back(std::make_shared(size, GL_RGBA, GL_RGBA, GL_UNSIGNED_BYTE)); 8 | depth_buffer = std::make_shared(size, GL_DEPTH_COMPONENT32F, GL_DEPTH_COMPONENT, GL_FLOAT); 9 | 10 | glGenFramebuffers(1, &frame_buffer); 11 | glBindFramebuffer(GL_FRAMEBUFFER, frame_buffer); 12 | 13 | glFramebufferTexture2D(GL_FRAMEBUFFER, GL_COLOR_ATTACHMENT0, GL_TEXTURE_2D, color_buffers[0]->id(), 0); 14 | glFramebufferTexture2D(GL_FRAMEBUFFER, GL_DEPTH_ATTACHMENT, GL_TEXTURE_2D, depth_buffer->id(), 0); 15 | 16 | GLenum color_attachments[] = {GL_COLOR_ATTACHMENT0, GL_COLOR_ATTACHMENT1, GL_COLOR_ATTACHMENT2}; 17 | glDrawBuffers(static_cast(color_buffers.size()), color_attachments); 18 | 19 | glBindFramebuffer(GL_FRAMEBUFFER, 0); 20 | } 21 | 22 | FrameBuffer::~FrameBuffer() { glDeleteFramebuffers(1, &frame_buffer); } 23 | 24 | void FrameBuffer::bind() { 25 | glGetIntegerv(GL_VIEWPORT, viewport); 26 | 27 | glViewport(0, 0, width, height); 28 | glBindFramebuffer(GL_FRAMEBUFFER, frame_buffer); 29 | } 30 | 31 | void FrameBuffer::unbind() const { 32 | glBindFramebuffer(GL_FRAMEBUFFER, 0); 33 | glViewport(viewport[0], viewport[1], viewport[2], viewport[3]); 34 | } 35 | 36 | void FrameBuffer::add_color_buffer(GLuint internal_format, GLuint format, GLuint type) { 37 | glBindFramebuffer(GL_FRAMEBUFFER, frame_buffer); 38 | 39 | color_buffers.push_back(std::make_shared(color_buffers.front()->size(), internal_format, format, type)); 40 | glFramebufferTexture2D(GL_FRAMEBUFFER, GL_COLOR_ATTACHMENT0 + color_buffers.size() - 1, GL_TEXTURE_2D, 41 | color_buffers.back()->id(), 0); 42 | 43 | std::vector color_attachments(color_buffers.size()); 44 | for (int i = 0; i < color_buffers.size(); i++) { 45 | color_attachments[i] = GL_COLOR_ATTACHMENT0 + i; 46 | } 47 | glDrawBuffers(static_cast(color_buffers.size()), color_attachments.data()); 48 | 49 | glBindFramebuffer(GL_FRAMEBUFFER, 0); 50 | } 51 | 52 | } // namespace glk 53 | -------------------------------------------------------------------------------- /src/glk/mesh.cpp: -------------------------------------------------------------------------------- 1 | #include "glk/glsl_shader.hpp" 2 | #include "glk/mesh.hpp" 3 | 4 | namespace glk { 5 | 6 | Mesh::Mesh(const std::vector>& vertices, 7 | const std::vector>& normals, 8 | const std::vector& indices) 9 | : num_vertices(vertices.size()), num_indices(indices.size()) { 10 | glGenVertexArrays(1, &vao); 11 | glBindVertexArray(vao); 12 | 13 | glGenBuffers(1, &vbo); 14 | glBindBuffer(GL_ARRAY_BUFFER, vbo); 15 | glBufferData(GL_ARRAY_BUFFER, sizeof(float) * vertices.size() * 3, vertices.data(), GL_STATIC_DRAW); 16 | 17 | // glGenBuffers(1, &nbo); 18 | // glBindBuffer(GL_ARRAY_BUFFER, nbo); 19 | // glBufferData(GL_ARRAY_BUFFER, sizeof(float) * normals.size() * 3, normals.data(), GL_STATIC_DRAW); 20 | 21 | glGenBuffers(1, &ebo); 22 | glBindBuffer(GL_ELEMENT_ARRAY_BUFFER, ebo); 23 | glBufferData(GL_ELEMENT_ARRAY_BUFFER, sizeof(int) * indices.size(), indices.data(), GL_STATIC_DRAW); 24 | 25 | glBindVertexArray(0); 26 | glBindBuffer(GL_ARRAY_BUFFER, 0); 27 | glBindBuffer(GL_ELEMENT_ARRAY_BUFFER, 0); 28 | 29 | printf("Mesh: [%d], [%d]\n", num_vertices, num_indices); 30 | } 31 | 32 | void Mesh::free() { 33 | glDeleteVertexArrays(1, &vao); 34 | 35 | glDeleteBuffers(1, &vbo); 36 | glDeleteBuffers(1, &ebo); 37 | } 38 | 39 | void Mesh::draw(glk::GLSLShader& shader) const { 40 | GLint position_loc = shader.attrib("vert_position"); 41 | // GLint normal_loc = shader.attrib("vert_normal"); 42 | 43 | glBindVertexArray(vao); 44 | 45 | glEnableVertexAttribArray(position_loc); 46 | // glEnableVertexAttribArray(normal_loc); 47 | 48 | glBindBuffer(GL_ARRAY_BUFFER, vbo); 49 | glVertexAttribPointer(position_loc, 3, GL_FLOAT, GL_FALSE, 0, nullptr); 50 | 51 | // glBindBuffer(GL_ARRAY_BUFFER, nbo); 52 | // glVertexAttribPointer(normal_loc, 3, GL_FLOAT, GL_FALSE, 0, nullptr); 53 | 54 | glBindBuffer(GL_ELEMENT_ARRAY_BUFFER, ebo); 55 | glDrawElements(GL_TRIANGLES, num_indices, GL_UNSIGNED_INT, nullptr); 56 | glBindBuffer(GL_ELEMENT_ARRAY_BUFFER, 0); 57 | 58 | glDisableVertexAttribArray(position_loc); 59 | // glDisableVertexAttribArray(normal_loc); 60 | 61 | glBindBuffer(GL_ARRAY_BUFFER, 0); 62 | glBindVertexArray(0); 63 | } 64 | 65 | } // namespace glk -------------------------------------------------------------------------------- /src/glk/pointcloud_buffer.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | 5 | #include "glk/glsl_shader.hpp" 6 | #include "glk/pointcloud_buffer.hpp" 7 | 8 | namespace glk { 9 | 10 | PointCloudBuffer::PointCloudBuffer(const std::string &cloud_filename) { 11 | pcl::PointCloud::Ptr cloud(new pcl::PointCloud()); 12 | if (pcl::io::loadPCDFile(cloud_filename, *cloud)) { 13 | std::cerr << "error: failed to load " << cloud_filename << std::endl; 14 | num_points_ = 0; 15 | return; 16 | } 17 | 18 | stride_ = sizeof(pcl::PointXYZI); 19 | num_points_ = int(cloud->size()); 20 | 21 | glGenVertexArrays(1, &vao); 22 | glBindVertexArray(vao); 23 | 24 | glGenBuffers(1, &vbo); 25 | glBindBuffer(GL_ARRAY_BUFFER, vbo); 26 | glBufferData(GL_ARRAY_BUFFER, cloud->size() * sizeof(pcl::PointXYZI), cloud->points.data(), GL_STATIC_DRAW); 27 | } 28 | 29 | PointCloudBuffer::PointCloudBuffer(const pcl::PointCloud::ConstPtr &cloud) { 30 | stride_ = sizeof(pcl::PointXYZI); 31 | num_points_ = cloud->size(); 32 | 33 | glGenVertexArrays(1, &vao); 34 | glBindVertexArray(vao); 35 | 36 | glGenBuffers(1, &vbo); 37 | glBindBuffer(GL_ARRAY_BUFFER, vbo); 38 | glBufferData(GL_ARRAY_BUFFER, cloud->size() * sizeof(pcl::PointXYZI), cloud->points.data(), GL_STATIC_DRAW); 39 | } 40 | 41 | PointCloudBuffer::PointCloudBuffer(const std::vector &points) { 42 | num_points_ = int(points.size()); 43 | 44 | stride_ = sizeof(Eigen::Vector3f); 45 | // 将Vector3d转为Vector3f 46 | std::vector pts(points.size()); 47 | for (size_t i = 0; i < points.size(); i++) { 48 | pts.at(i) = points.at(i).cast(); 49 | } 50 | 51 | glGenVertexArrays(1, &vao); 52 | glBindVertexArray(vao); 53 | 54 | glGenBuffers(1, &vbo); 55 | glBindBuffer(GL_ARRAY_BUFFER, vbo); 56 | glBufferData(GL_ARRAY_BUFFER, pts.size() * stride_, pts.data(), GL_STATIC_DRAW); 57 | } 58 | 59 | void PointCloudBuffer::free() { 60 | glDeleteVertexArrays(1, &vao); 61 | glDeleteBuffers(1, &vbo); 62 | } 63 | 64 | void PointCloudBuffer::draw(glk::GLSLShader &shader) const { 65 | if (num_points_ == 0) { 66 | return; 67 | } 68 | 69 | GLint position_loc = shader.attrib("vert_position"); 70 | 71 | glBindVertexArray(vao); 72 | glEnableVertexAttribArray(position_loc); 73 | 74 | glBindBuffer(GL_ARRAY_BUFFER, vbo); 75 | glVertexAttribPointer(0, 3, GL_FLOAT, GL_FALSE, stride_, nullptr); 76 | 77 | glDrawArrays(GL_POINTS, 0, num_points_); 78 | 79 | glDisableVertexAttribArray(position_loc); 80 | glBindBuffer(GL_ARRAY_BUFFER, 0); 81 | } 82 | 83 | } // namespace glk 84 | -------------------------------------------------------------------------------- /src/glk/primitives/primitives.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include "glk/drawble.hpp" 3 | #include "glk/mesh.hpp" 4 | #include "glk/lines.hpp" 5 | #include "glk/mesh_utils.hpp" 6 | 7 | #include "glk/primitives/grid.hpp" 8 | #include "glk/primitives/cube.hpp" 9 | #include "glk/primitives/cone.hpp" 10 | #include "glk/primitives/icosahedron.hpp" 11 | #include "glk/primitives/coordinate_system.hpp" 12 | #include "glk/loaders/ply_loader.hpp" 13 | #include "glk/primitives/primitives.hpp" 14 | 15 | namespace glk { 16 | 17 | Primitives *Primitives::instance_ = nullptr; 18 | 19 | const glk::Drawable &Primitives::primitive(PrimitiveType type) { 20 | if (meshes[type] == nullptr) { 21 | switch (type) { 22 | default: 23 | std::cerr << "error : unknown primitive type " << type << std::endl; 24 | break; 25 | case ICOSAHEDRON: { 26 | // 二十面体 27 | glk::Icosahedron icosahedron; 28 | glk::Flatize flat(icosahedron.vertices, icosahedron.indices); 29 | meshes[type].reset(new glk::Mesh(flat.vertices, flat.normals, flat.indices)); 30 | } break; 31 | case SPHERE: { 32 | // 球面 33 | glk::Icosahedron icosahedron; 34 | icosahedron.subdivide(); 35 | icosahedron.subdivide(); 36 | icosahedron.spherize(); 37 | meshes[type].reset(new glk::Mesh(icosahedron.vertices, icosahedron.normals, icosahedron.indices)); 38 | } break; 39 | case CUBE: { 40 | // 立方体 41 | glk::Cube cube; 42 | // 扁平化 43 | glk::Flatize flat(cube.vertices, cube.indices); 44 | meshes[type].reset(new glk::Mesh(flat.vertices, flat.normals, flat.indices)); 45 | } break; 46 | case CONE: { 47 | // 椎体 48 | glk::Cone cone; 49 | glk::Flatize flat(cone.vertices, cone.indices); 50 | meshes[type].reset(new glk::Mesh(flat.vertices, flat.normals, flat.indices)); 51 | } break; 52 | case GRID: { // 栅格 53 | glk::Grid grid; 54 | meshes[type].reset(new glk::Lines(0.01f, grid.vertices)); 55 | } break; 56 | case COORDINATE_SYSTEM: { 57 | glk::CoordinateSystem coord; 58 | meshes[type].reset(new glk::Lines(0.04f, coord.vertices, coord.colors)); 59 | } break; 60 | } 61 | } 62 | 63 | return *meshes[type]; 64 | } 65 | } // namespace glk -------------------------------------------------------------------------------- /src/guik/camera_control.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | 8 | #include "guik/camera_control.hpp" 9 | 10 | namespace guik { 11 | 12 | ArcCameraControl::ArcCameraControl() 13 | : center(0.0f, 0.0f, 0.0f), distance(5.0f), left_button_down(false), theta(0.0f), phi(-60.0f * M_PI / 180.0f) { 14 | left_button_down = false; 15 | middle_button_down = false; 16 | } 17 | 18 | ArcCameraControl::~ArcCameraControl() {} 19 | 20 | void ArcCameraControl::mouse(const Eigen::Vector2i& p, int button, bool down) { 21 | if (button == 0) { 22 | left_button_down = down; 23 | } 24 | if (button == 2) { 25 | middle_button_down = down; 26 | } 27 | drag_last_pos = p; 28 | } 29 | 30 | void ArcCameraControl::drag(const Eigen::Vector2i& p, int button) { 31 | Eigen::Vector2i rel = p - drag_last_pos; 32 | 33 | if (left_button_down) { 34 | theta -= rel[0] * 0.01f; 35 | phi -= rel[1] * 0.01f; 36 | 37 | phi = std::min(M_PI_2 - 0.01, std::max(-M_PI_2 + 0.01, phi)); 38 | } 39 | 40 | if (middle_button_down) { 41 | center += Eigen::AngleAxisf(theta + M_PI_2, Eigen::Vector3f::UnitZ()) * Eigen::Vector3f(-rel[0], rel[1], 0.0f) * 42 | distance * 0.001f; 43 | } 44 | 45 | drag_last_pos = p; 46 | } 47 | 48 | double ArcCameraControl::access_distance() const { return distance; } 49 | 50 | void ArcCameraControl::scroll(const Eigen::Vector2f& rel) { 51 | if (rel[0] > 0) { 52 | distance = distance * 0.9f; 53 | } else if (rel[0] < 0) { 54 | distance = distance * 1.1f; 55 | } 56 | 57 | distance = std::max(0.1, distance); 58 | } 59 | 60 | Eigen::Quaternionf ArcCameraControl::rotation() const { 61 | return Eigen::AngleAxisf(theta, Eigen::Vector3f::UnitZ()) * Eigen::AngleAxisf(phi, Eigen::Vector3f::UnitY()); 62 | } 63 | 64 | Eigen::Matrix4f ArcCameraControl::view_matrix() const { 65 | Eigen::Vector3f offset = rotation() * Eigen::Vector3f(distance, 0.0f, 0.0f); 66 | Eigen::Vector3f eye = center + offset; 67 | 68 | glm::mat4 mat = glm::lookAt(glm::vec3(eye[0], eye[1], eye[2]), glm::vec3(center[0], center[1], center[2]), 69 | glm::vec3(0.0f, 0.0f, 1.0f)); 70 | return Eigen::Map(glm::value_ptr(mat)); 71 | } 72 | 73 | } // namespace guik 74 | -------------------------------------------------------------------------------- /src/guik/projection_control.cpp: -------------------------------------------------------------------------------- 1 | #include "guik/projection_control.hpp" 2 | 3 | #include "imgui.h" 4 | 5 | #include 6 | #include 7 | #include 8 | #include 9 | 10 | namespace guik { 11 | 12 | ProjectionControl::ProjectionControl(Eigen::Vector2i size) 13 | : b_show_window(false), 14 | size(std::move(size)), 15 | projection_mode(0), 16 | fovy(30.0f), 17 | width(10.0f), 18 | near(0.1f), 19 | far(100.0f) {} 20 | 21 | ProjectionControl::~ProjectionControl() = default; 22 | 23 | Eigen::Matrix4f ProjectionControl::projection_matrix() const { 24 | float aspect_ratio = size[0] / static_cast(size[1]); 25 | 26 | glm::mat4 proj; 27 | if (projection_mode == 0) { 28 | proj = glm::perspective(fovy * M_PI / 180.0, aspect_ratio, near, far); 29 | } else { 30 | proj = glm::ortho(-width / 2.0f, width / 2.0f, -width / 2.0f / aspect_ratio, width / 2.0 / aspect_ratio, 31 | near, far); 32 | } 33 | 34 | return Eigen::Map(glm::value_ptr(proj)); 35 | } 36 | 37 | void ProjectionControl::show() { b_show_window = true; } 38 | 39 | void ProjectionControl::draw_ui() { 40 | if (!b_show_window) { 41 | return; 42 | } 43 | 44 | ImGui::Begin("projection control", &b_show_window, ImGuiWindowFlags_AlwaysAutoResize); 45 | const char* modes[] = {"PERSPECTIVE", "ORTHOGONAL"}; 46 | ImGui::Combo("Mode", &projection_mode, modes, IM_ARRAYSIZE(modes)); 47 | if (projection_mode == 0) { 48 | ImGui::DragFloat("FOV", &fovy, 0.1f, 1.0f, 180.0f); 49 | } else { 50 | ImGui::DragFloat("Width", &width, 0.1f, 1.0f, 1000.0f); 51 | } 52 | 53 | ImGui::DragFloat("Near", &near, 1.0f, 0.1f, far); 54 | ImGui::DragFloat("Far", &far, 1.0f, near, 10000.0f); 55 | 56 | ImGui::End(); 57 | } 58 | } // namespace guik -------------------------------------------------------------------------------- /src/util/extrinsics_publisher.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | 6 | #include "util/extrinsics_publisher.hpp" 7 | #include "algorithm/util.h" 8 | #include "robot_basic_tools/Extrinsic.h" 9 | 10 | namespace util { 11 | bool publish_extrinsics(ros::NodeHandle& nh_, std::array ext, const std::string& child_true, 12 | const std::string& parent_true, const std::string& child_selected, 13 | const std::string& parent_selected) { 14 | ros::ServiceClient client = nh_.serviceClient("/calibrator_listener/sensor_extrinsic"); 15 | 16 | geometry_msgs::TransformStamped ext_msg; 17 | tf::Quaternion q = tf::createQuaternionFromRPY(DEG2RAD_RBT(ext[3]), DEG2RAD_RBT(ext[4]), DEG2RAD_RBT(ext[5])); 18 | tf::Vector3 t = tf::Vector3(ext[0], ext[1], ext[2]); 19 | tf::transformStampedTFToMsg( 20 | tf::StampedTransform(tf::Transform(q, t), ros::Time::now(), "/" + parent_true, "/" + child_true), ext_msg); 21 | printf("----- publish_extrinsics() ..... parent_true = /%s, child_true = /%s\n", parent_true.c_str(), 22 | child_true.c_str()); 23 | 24 | robot_basic_tools::Extrinsic output; 25 | output.request.tfs = ext_msg; 26 | output.request.parent_selected = parent_selected; 27 | output.request.child_selected = child_selected; 28 | printf("----- publish_extrinsics() ..... parent_selected = /%s, child_selected = /%s\n", parent_selected.c_str(), 29 | child_selected.c_str()); 30 | 31 | if (client.call(output)) { 32 | ROS_INFO("Response from server: %d", output.response.feedback); 33 | } else { 34 | ROS_ERROR("Failed to call service sensor_extrinsic"); 35 | return false; 36 | } 37 | return true; 38 | } 39 | } // namespace util -------------------------------------------------------------------------------- /src/util/image_loader.cpp: -------------------------------------------------------------------------------- 1 | 2 | #include 3 | // #include 4 | 5 | #include "GL/gl3w.h" 6 | #include "util/image_loader.hpp" 7 | 8 | namespace util { 9 | 10 | // Simple helper function to load an image into a OpenGL texture with common settings 11 | bool LoadTextureFromFile(const std::string &filename, GLuint &img_texture, int &img_width, int &img_height) { 12 | // Load from file 13 | cv::Mat image_mat = cv::imread(filename); 14 | if (image_mat.empty()) { 15 | return false; 16 | } 17 | 18 | img_width = image_mat.cols; 19 | img_height = image_mat.rows; 20 | 21 | // cv::Mat image_fix_size; 22 | // // 转为指定为大小 23 | // if (image_mat.size().width != img_width || image_mat.size().height != img_height) { 24 | // cv::resize(image_mat, image_fix_size, cv::Size{img_width, img_height}); 25 | // } else { 26 | // image_fix_size = image_mat; 27 | // } 28 | 29 | // Create a OpenGL texture identifier 30 | GLuint image_texture; 31 | glGenTextures(1, &image_texture); 32 | glBindTexture(GL_TEXTURE_2D, image_texture); 33 | 34 | // Setup filtering parameters for display 35 | glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_LINEAR); 36 | glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_LINEAR); 37 | glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_S, GL_REPEAT); 38 | glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_T, GL_REPEAT); 39 | 40 | // Upload pixels into texture 41 | glPixelStorei(GL_UNPACK_ALIGNMENT, 1); 42 | glTexImage2D(GL_TEXTURE_2D, 0, GL_RGB, img_width, img_height, 0, GL_BGR, GL_UNSIGNED_BYTE, image_mat.data); 43 | 44 | img_texture = image_texture; 45 | return true; 46 | } 47 | } // namespace util -------------------------------------------------------------------------------- /srv/Extrinsic.srv: -------------------------------------------------------------------------------- 1 | geometry_msgs/TransformStamped tfs 2 | string parent_selected 3 | string child_selected 4 | --- 5 | bool feedback -------------------------------------------------------------------------------- /thirdparty/camera_model/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | project(camera_model) 2 | 3 | set(CMAKE_BUILD_TYPE "Release") 4 | #set(CMAKE_CXX_FLAGS "-std=c++11 -march=native") 5 | set(CMAKE_CXX_FLAGS "-std=c++11") 6 | set(CMAKE_CXX_FLAGS_RELEASE "-O3 -fPIC -fopenmp -Wno-all") 7 | 8 | find_package(Boost REQUIRED COMPONENTS 9 | filesystem 10 | program_options 11 | system) 12 | message(STATUS "Boost_LIBRARIES: ${Boost_LIBRARIES}") 13 | 14 | # OpenCV 15 | 16 | find_package(OpenCV 4.0 QUIET 17 | COMPONENTS 18 | core 19 | imgproc 20 | calib3d 21 | highgui 22 | ) 23 | if (NOT OpenCV_FOUND) 24 | find_package(OpenCV 4.0 PATHS ${OpenCV_LIB_DIR} NO_DEFAULT_PATH REQUIRED 25 | COMPONENTS 26 | core 27 | imgproc 28 | calib3d 29 | highgui 30 | ) 31 | endif () 32 | message(STATUS "Use OpenCV ${OpenCV_VERSION}") 33 | message(STATUS "OpenCV_INCLUDE_DIRS: ${OpenCV_INCLUDE_DIRS}") 34 | message(STATUS "OpenCV_LIBRARIES: ${OpenCV_LIBRARIES}") 35 | 36 | # Ceres 37 | find_package(Ceres 2.0 QUIET) 38 | if (NOT Ceres_FOUND) 39 | find_package(Ceres PATHS ${Ceres_LIB_DIR} NO_DEFAULT_PATH REQUIRED) 40 | endif () 41 | 42 | aux_source_directory(./src/apriltag AprilTagSrc) 43 | aux_source_directory(./src/apriltag_frontend AprilTagSrc2) 44 | 45 | set(CAMERA_MODEL_FILES 46 | ./src/camera_models/Camera.cc 47 | ./src/camera_models/CostFunctionFactory.cc 48 | ./src/camera_models/CameraFactory.cc 49 | ./src/camera_models/PinholeCamera.cc 50 | ./src/camera_models/PinholeFullCamera.cc 51 | ./src/camera_models/CataCamera.cc 52 | ./src/camera_models/EquidistantCamera.cc 53 | ./src/camera_models/ScaramuzzaCamera.cc 54 | ./src/camera_models/PolyFisheyeCamera.cpp 55 | ./src/camera_models/SplineCamera.cpp 56 | ./src/camera_models/FovCamera.cpp 57 | ./src/sparse_graph/Transform.cc 58 | ./src/code_utils/math_utils/Polynomial.cpp 59 | ./src/code_utils/cv_utils.cc 60 | ./src/gpl/EigenQuaternionParameterization.cc 61 | ./src/gpl/gpl.cc 62 | ${AprilTagSrc} 63 | ${AprilTagSrc2} 64 | ) 65 | 66 | add_library(${PROJECT_NAME} SHARED 67 | src/chessboard/Chessboard.cc 68 | src/calib/CameraCalibration.cc 69 | ${CAMERA_MODEL_FILES} 70 | ) 71 | 72 | set_target_properties(${PROJECT_NAME} PROPERTIES 73 | LIBRARY_OUTPUT_DIRECTORY ${RDPARTY_SO_PATH} 74 | ) 75 | 76 | target_include_directories(${PROJECT_NAME} PUBLIC 77 | $ 78 | ) 79 | 80 | target_link_libraries(${PROJECT_NAME} 81 | PUBLIC 82 | ${Boost_LIBRARIES} 83 | ${OpenCV_LIBS} 84 | ${CERES_LIBRARIES} 85 | ) 86 | 87 | 88 | -------------------------------------------------------------------------------- /thirdparty/camera_model/include/camera_model/apriltag/Edge.h: -------------------------------------------------------------------------------- 1 | #ifndef EDGE_H 2 | #define EDGE_H 3 | 4 | #include 5 | 6 | #include "FloatImage.h" 7 | 8 | namespace AprilTags { 9 | 10 | class FloatImage; 11 | class UnionFindSimple; 12 | 13 | using std::max; 14 | using std::min; 15 | 16 | //! Represents an edge between adjacent pixels in the image. 17 | /*! The edge is encoded by the indices of the two pixels. Edge cost 18 | * is proportional to the difference in local orientations. 19 | */ 20 | class Edge { 21 | public: 22 | static float const minMag; //!< minimum intensity gradient for an edge to be recognized 23 | static float const maxEdgeCost; //!< 30 degrees = maximum acceptable difference in local orientations 24 | static int const WEIGHT_SCALE; // was 10000 25 | static float const thetaThresh; //!< theta threshold for merging edges 26 | static float const magThresh; //!< magnitude threshold for merging edges 27 | 28 | int pixelIdxA; 29 | int pixelIdxB; 30 | int cost; 31 | 32 | //! Constructor 33 | Edge() : pixelIdxA(), pixelIdxB(), cost() {} 34 | 35 | //! Compare edges based on cost 36 | inline bool operator<(const Edge &other) const { return (cost < other.cost); } 37 | 38 | //! Cost of an edge between two adjacent pixels; -1 if no edge here 39 | /*! An edge exists between adjacent pixels if the magnitude of the 40 | intensity gradient at both pixels is above threshold. The edge 41 | cost is proportional to the difference in the local orientation at 42 | the two pixels. Lower cost is better. A cost of -1 means there 43 | is no edge here (intensity gradien fell below threshold). 44 | */ 45 | static int edgeCost(float theta0, float theta1, float mag1); 46 | 47 | //! Calculates and inserts up to four edges into 'edges', a vector of Edges. 48 | static void calcEdges(float theta0, int x, int y, const FloatImage &theta, const FloatImage &mag, 49 | std::vector &edges, size_t &nEdges); 50 | 51 | //! Process edges in order of increasing cost, merging clusters if we can do so without exceeding the thetaThresh. 52 | static void mergeEdges(std::vector &edges, UnionFindSimple &uf, float tmin[], float tmax[], float mmin[], 53 | float mmax[]); 54 | }; 55 | 56 | } // namespace AprilTags 57 | 58 | #endif 59 | -------------------------------------------------------------------------------- /thirdparty/camera_model/include/camera_model/apriltag/FloatImage.h: -------------------------------------------------------------------------------- 1 | #ifndef FLOATIMAGE_H 2 | #define FLOATIMAGE_H 3 | 4 | #include 5 | #include 6 | 7 | namespace DualCoding { 8 | typedef unsigned char uchar; 9 | template 10 | class Sketch; 11 | } // namespace DualCoding 12 | 13 | namespace AprilTags { 14 | 15 | //! Represent an image as a vector of floats in [0,1] 16 | class FloatImage { 17 | private: 18 | int width; 19 | int height; 20 | std::vector pixels; 21 | 22 | public: 23 | //! Default constructor 24 | FloatImage(); 25 | 26 | //! Construct an empty image 27 | FloatImage(int widthArg, int heightArg); 28 | 29 | //! Constructor that copies pixels from an array 30 | FloatImage(int widthArg, int heightArg, const std::vector& pArg); 31 | 32 | FloatImage& operator=(const FloatImage& other); 33 | 34 | float get(int x, int y) const { return pixels[y * width + x]; } 35 | void set(int x, int y, float v) { pixels[y * width + x] = v; } 36 | 37 | int getWidth() const { return width; } 38 | int getHeight() const { return height; } 39 | int getNumFloatImagePixels() const { return width * height; } 40 | const std::vector& getFloatImagePixels() const { return pixels; } 41 | 42 | //! TODO: Fix decimateAvg function. DO NOT USE! 43 | void decimateAvg(); 44 | 45 | //! Rescale all values so that they are between [0,1] 46 | void normalize(); 47 | 48 | void filterFactoredCentered(const std::vector& fhoriz, const std::vector& fvert); 49 | 50 | template 51 | void copyToSketch(DualCoding::Sketch& sketch) { 52 | for (int i = 0; i < getNumFloatImagePixels(); i++) sketch[i] = (T)(255.0f * getFloatImagePixels()[i]); 53 | } 54 | 55 | void printMinMax() const; 56 | }; 57 | 58 | } // namespace AprilTags 59 | 60 | #endif 61 | -------------------------------------------------------------------------------- /thirdparty/camera_model/include/camera_model/apriltag/GLine2D.h: -------------------------------------------------------------------------------- 1 | #ifndef GLINE2D_H 2 | #define GLINE2D_H 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | #include "MathUtil.h" 9 | #include "XYWeight.h" 10 | 11 | namespace AprilTags { 12 | 13 | //! A 2D line 14 | class GLine2D { 15 | public: 16 | //! Create a new line. 17 | GLine2D(); 18 | 19 | //! Create a new line. 20 | /* @param slope the slope 21 | * @param b the y intercept 22 | */ 23 | GLine2D(float slope, float b); 24 | 25 | //! Create a new line. 26 | /* @param dx A change in X corresponding to dy 27 | * @param dy A change in Y corresponding to dx 28 | * @param p A point that the line passes through 29 | */ 30 | GLine2D(float dX, float dY, const std::pair& pt); 31 | 32 | //! Create a new line through two points. 33 | /* @param p1 the first point 34 | * @param p2 the second point 35 | */ 36 | GLine2D(const std::pair& p1, const std::pair& p2); 37 | 38 | //! Get the coordinate of a point (on this line), with zero corresponding to the point 39 | //! on the that is perpendicular toa line passing through the origin and the line. 40 | /* This allows easy computation if one point is between two other points on the line: 41 | * compute the line coordinate of all three points and test if a<=b<=c. This is 42 | * implemented by computing the dot product of the vector 'p' with the 43 | * line's direct unit vector. 44 | */ 45 | float getLineCoordinate(const std::pair& p); 46 | 47 | //! The inverse of getLineCoordinate. 48 | std::pair getPointOfCoordinate(float coord); 49 | 50 | //! Compute the point where two lines intersect, or (-1,0) if the lines are parallel. 51 | std::pair intersectionWith(const GLine2D& line) const; 52 | 53 | static GLine2D lsqFitXYW(const std::vector& xyweights); 54 | 55 | inline float getDx() const { return dx; } 56 | inline float getDy() const { return dy; } 57 | inline float getFirst() const { return p.first; } 58 | inline float getSecond() const { return p.second; } 59 | 60 | protected: 61 | void normalizeSlope(); 62 | void normalizeP(); 63 | 64 | private: 65 | float dx, dy; 66 | std::pair p; //!< A point the line passes through; when normalized, it is the point closest to the 67 | //!< origin (hence perpendicular to the line) 68 | bool didNormalizeSlope; 69 | bool didNormalizeP; 70 | }; 71 | 72 | } // namespace AprilTags 73 | 74 | #endif 75 | -------------------------------------------------------------------------------- /thirdparty/camera_model/include/camera_model/apriltag/GLineSegment2D.h: -------------------------------------------------------------------------------- 1 | #ifndef GLINESEGMENT2D_H 2 | #define GLINESEGMENT2D_H 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | #include "GLine2D.h" 10 | #include "XYWeight.h" 11 | 12 | namespace AprilTags { 13 | 14 | //! A 2D line with endpoints. 15 | class GLineSegment2D { 16 | public: 17 | GLineSegment2D(const std::pair &p0Arg, const std::pair &p1Arg); 18 | static GLineSegment2D lsqFitXYW(const std::vector &xyweight); 19 | std::pair getP0() const { return p0; } 20 | std::pair getP1() const { return p1; } 21 | 22 | private: 23 | GLine2D line; 24 | std::pair p0; 25 | std::pair p1; 26 | int weight; 27 | }; 28 | 29 | } // namespace AprilTags 30 | 31 | #endif 32 | -------------------------------------------------------------------------------- /thirdparty/camera_model/include/camera_model/apriltag/Gaussian.h: -------------------------------------------------------------------------------- 1 | #ifndef GAUSSIAN_H 2 | #define GAUSSIAN_H 3 | 4 | #include 5 | #include 6 | 7 | namespace AprilTags { 8 | 9 | class Gaussian { 10 | public: 11 | static bool warned; 12 | 13 | //! Returns a Gaussian filter of size n. 14 | /*! @param sigma standard deviation of the Gaussian 15 | * @param n length of the Gaussian (must be odd) 16 | */ 17 | static std::vector makeGaussianFilter(float sigma, int n); 18 | 19 | //! Convolve the input 'a' (which begins at offset aoff and is alen elements in length) with the filter 'f'. 20 | /*! The result is deposited in 'r' at offset 'roff'. f.size() should be odd. 21 | * The output is shifted by -f.size()/2, so that there is no net time delay. 22 | * @param a input vector of pixels 23 | * @param aoff 24 | * @param alen 25 | * @param f 26 | * @param r the resultant array of pixels 27 | * @param roff 28 | */ 29 | static void convolveSymmetricCentered(const std::vector& a, unsigned int aoff, unsigned int alen, 30 | const std::vector& f, std::vector& r, unsigned int roff); 31 | }; 32 | 33 | } // namespace AprilTags 34 | 35 | #endif 36 | -------------------------------------------------------------------------------- /thirdparty/camera_model/include/camera_model/apriltag/GrayModel.h: -------------------------------------------------------------------------------- 1 | //-*-c++-*- 2 | 3 | #ifndef GRAYMODEL_H 4 | #define GRAYMODEL_H 5 | 6 | #include 7 | #include 8 | 9 | namespace AprilTags { 10 | 11 | //! Fits a grayscale model over an area of pixels. 12 | /*! The model is of the form: c1*x + c2*y + c3*x*y + c4 = value 13 | * 14 | * We use this model to compute spatially-varying thresholds for 15 | * reading bits. 16 | */ 17 | class GrayModel { 18 | public: 19 | GrayModel(); 20 | 21 | void addObservation(float x, float y, float gray); 22 | 23 | inline int getNumObservations() { return nobs; } 24 | 25 | float interpolate(float x, float y); 26 | 27 | private: 28 | void compute(); 29 | 30 | // We're solving Av = b. 31 | // 32 | // For each observation, we add a row to A of the form [x y xy 1] 33 | // and to b of the form gray*[x y xy 1]. v is the vector [c1 c2 c3 c4]. 34 | // 35 | // The least-squares solution to the system is v = inv(A'A)A'b 36 | 37 | Eigen::Matrix4d A; 38 | Eigen::Vector4d v; 39 | Eigen::Vector4d b; 40 | int nobs; 41 | bool dirty; //!< True if we've added an observation and need to recompute v 42 | }; 43 | 44 | } // namespace AprilTags 45 | 46 | #endif 47 | -------------------------------------------------------------------------------- /thirdparty/camera_model/include/camera_model/apriltag/Homography33.h: -------------------------------------------------------------------------------- 1 | //-*-c++-*- 2 | 3 | #ifndef HOMOGRAPHY33_H 4 | #define HOMOGRAPHY33_H 5 | 6 | #include 7 | #include 8 | 9 | #include 10 | 11 | // interpolate points instead of using homography 12 | #define INTERPOLATE 13 | // use stable version of homography recover (opencv, includes refinement step) 14 | #define STABLE_H 15 | 16 | //! Compute 3x3 homography using Direct Linear Transform 17 | /* 18 | * 19 | * DEPRECATED - DEPRECATED - DEPRECATED - DEPRECATED 20 | * 21 | * use TagDetection::getRelativeTransform() instead 22 | * 23 | * 24 | * y = Hx (y = image coordinates in homogeneous coordinates, H = 3x3 25 | * homography matrix, x = homogeneous 2D world coordinates) 26 | * 27 | * For each point correspondence, constrain y x Hx = 0 (where x is 28 | * cross product). This means that they have the same direction, and 29 | * ignores scale factor. 30 | * 31 | * We rewrite this as Ah = 0, where h is the 9x1 column vector of the 32 | * elements of H. Each point correspondence gives us 3 equations of 33 | * rank 2. The solution h is the minimum right eigenvector of A, 34 | * which we can obtain via SVD, USV' = A. h is the right-most column 35 | * of V'. 36 | * 37 | * We will actually maintain A'A internally, which is 9x9 regardless 38 | * of how many correspondences we're given, and has the same 39 | * eigenvectors as A. 40 | */ 41 | class Homography33 { 42 | public: 43 | //! Constructor 44 | Homography33(const std::pair &opticalCenter); 45 | 46 | #ifdef STABLE_H 47 | void setCorrespondences(const std::vector > &srcPts, 48 | const std::vector > &dstPts); 49 | #else 50 | void addCorrespondence(float worldx, float worldy, float imagex, float imagey); 51 | #endif 52 | 53 | //! Note that the returned H matrix does not reflect cxy. 54 | Eigen::Matrix3d &getH(); 55 | 56 | const std::pair getCXY() const { return cxy; } 57 | 58 | void compute(); 59 | 60 | std::pair project(float worldx, float worldy); 61 | 62 | private: 63 | std::pair cxy; 64 | Eigen::Matrix fA; 65 | Eigen::Matrix3d H; 66 | bool valid; 67 | #ifdef STABLE_H 68 | std::vector > srcPts, dstPts; 69 | #endif 70 | }; 71 | 72 | #endif 73 | -------------------------------------------------------------------------------- /thirdparty/camera_model/include/camera_model/apriltag/MathUtil.h: -------------------------------------------------------------------------------- 1 | //-*-c++-*- 2 | 3 | #ifndef MATHUTIL_H 4 | #define MATHUTIL_H 5 | 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | 12 | namespace AprilTags { 13 | 14 | std::ostream &operator<<(std::ostream &os, const std::pair &pt); 15 | 16 | //! Miscellaneous math utilities and fast exp functions. 17 | class MathUtil { 18 | public: 19 | //! Returns the square of a value. 20 | static inline float square(float x) { return x * x; } 21 | 22 | static inline float distance2D(const std::pair &p0, const std::pair &p1) { 23 | float dx = p0.first - p1.first; 24 | float dy = p0.second - p1.second; 25 | return std::sqrt(dx * dx + dy * dy); 26 | } 27 | 28 | //! Returns a result in [-Pi, Pi] 29 | static inline float mod2pi(float vin) { 30 | const float twopi = 2 * (float)M_PI; 31 | const float twopi_inv = 1.f / (2.f * (float)M_PI); 32 | float absv = std::abs(vin); 33 | float q = absv * twopi_inv + 0.5f; 34 | int qi = (int)q; 35 | float r = absv - qi * twopi; 36 | return (vin < 0) ? -r : r; 37 | } 38 | 39 | //! Returns a value of v wrapped such that ref and v differ by no more than +/- Pi 40 | static inline float mod2pi(float ref, float v) { return ref + mod2pi(v - ref); } 41 | 42 | // lousy approximation of arctan function, but good enough for our purposes (about 4 degrees) 43 | static inline double fast_atan2(double y, double x) { 44 | double coeff_1 = M_PI / 4; 45 | double coeff_2 = 3 * coeff_1; 46 | double abs_y = fabs(y) + 1e-10; // kludge to prevent 0/0 condition 47 | 48 | double angle; 49 | 50 | if (x >= 0) { 51 | double r = (x - abs_y) / (x + abs_y); 52 | angle = coeff_1 - coeff_1 * r; 53 | } else { 54 | double r = (x + abs_y) / (abs_y - x); 55 | angle = coeff_2 - coeff_1 * r; 56 | } 57 | 58 | if (y < 0) 59 | return -angle; // negate if in quad III or IV 60 | else 61 | return angle; 62 | } 63 | }; 64 | 65 | } // namespace AprilTags 66 | 67 | #endif 68 | -------------------------------------------------------------------------------- /thirdparty/camera_model/include/camera_model/apriltag/Quad.h: -------------------------------------------------------------------------------- 1 | #ifndef QUAD_H 2 | #define QUAD_H 3 | 4 | #include 5 | #include 6 | 7 | #include 8 | 9 | #include "Homography33.h" 10 | 11 | namespace AprilTags { 12 | 13 | class FloatImage; 14 | class Segment; 15 | 16 | using std::max; 17 | using std::min; 18 | 19 | //! Represents four segments that form a loop, and might be a tag. 20 | class Quad { 21 | public: 22 | static const int minimumEdgeLength = 6; //!< Minimum size of a tag (in pixels) as measured along edges and diagonals 23 | static float const maxQuadAspectRatio; //!< Early pruning of quads with insane ratios. 24 | 25 | //! Constructor 26 | /*! (x,y) are the optical center of the camera, which is 27 | * needed to correctly compute the homography. */ 28 | Quad(const std::vector >& p, const std::pair& opticalCenter); 29 | 30 | //! Interpolate given that the lower left corner of the lower left cell is at (-1,-1) and the upper right corner of 31 | //! the upper right cell is at (1,1). 32 | std::pair interpolate(float x, float y); 33 | 34 | //! Same as interpolate, except that the coordinates are interpreted between 0 and 1, instead of -1 and 1. 35 | std::pair interpolate01(float x, float y); 36 | 37 | //! Points for the quad (in pixel coordinates), in counter clockwise order. These points are the intersections of 38 | //! segments. 39 | std::vector > quadPoints; 40 | 41 | //! Segments composing this quad 42 | std::vector segments; 43 | 44 | //! Total length (in pixels) of the actual perimeter observed for the quad. 45 | /*! This is in contrast to the geometric perimeter, some of which 46 | * may not have been directly observed but rather inferred by 47 | * intersecting segments. Quads with more observed perimeter are 48 | * preferred over others. */ 49 | float observedPerimeter; 50 | 51 | //! Given that the whole quad spans from (0,0) to (1,1) in "quad space", compute the pixel coordinates for a given 52 | //! point within that quad. 53 | /*! Note that for most of the Quad's existence, we will not know the correct orientation of the tag. */ 54 | Homography33 homography; 55 | 56 | //! Searches through a vector of Segments to form Quads. 57 | /* @param quads any discovered quads will be added to this list 58 | * @param path the segments currently part of the search 59 | * @param parent the first segment in the quad 60 | * @param depth how deep in the search are we? 61 | */ 62 | static void search(const FloatImage& fImage, std::vector& path, Segment& parent, int depth, 63 | std::vector& quads, const std::pair& opticalCenter); 64 | 65 | #ifdef INTERPOLATE 66 | private: 67 | Eigen::Vector2f p0, p3, p01, p32; 68 | #endif 69 | }; 70 | 71 | } // namespace AprilTags 72 | 73 | #endif 74 | -------------------------------------------------------------------------------- /thirdparty/camera_model/include/camera_model/apriltag/Segment.h: -------------------------------------------------------------------------------- 1 | #ifndef SEGMENT_H 2 | #define SEGMENT_H 3 | 4 | #include 5 | #include 6 | 7 | namespace AprilTags { 8 | 9 | //! Represents a line fit to a set of pixels whose gradients are similiar. 10 | class Segment { 11 | public: 12 | Segment(); 13 | 14 | static int const minimumSegmentSize = 4; //!< Minimum number of pixels in a segment before we'll fit a line to it. 15 | static float const 16 | minimumLineLength; //!< In pixels. Calculated based on minimum plausible decoding size for Tag9 family. 17 | 18 | float getX0() const { return x0; } 19 | void setX0(float newValue) { x0 = newValue; } 20 | 21 | float getY0() const { return y0; } 22 | void setY0(float newValue) { y0 = newValue; } 23 | 24 | float getX1() const { return x1; } 25 | void setX1(float newValue) { x1 = newValue; } 26 | 27 | float getY1() const { return y1; } 28 | void setY1(float newValue) { y1 = newValue; } 29 | 30 | float getTheta() const { return theta; } 31 | void setTheta(float newValue) { theta = newValue; } 32 | 33 | float getLength() const { return length; } 34 | void setLength(float newValue) { length = newValue; } 35 | 36 | //! Returns the length of the Segment. 37 | float segmentLength(); 38 | 39 | //! Print endpoint coordinates of this segment. 40 | void printSegment(); 41 | 42 | //! ID of Segment. 43 | int getId() const { return segmentId; } 44 | 45 | std::vector children; 46 | 47 | private: 48 | float x0, y0, x1, y1; 49 | float theta; // gradient direction (points towards white) 50 | float length; // length of line segment in pixels 51 | int segmentId; 52 | static int idCounter; 53 | }; 54 | 55 | } // namespace AprilTags 56 | 57 | #endif 58 | -------------------------------------------------------------------------------- /thirdparty/camera_model/include/camera_model/apriltag/Tag16h5.h: -------------------------------------------------------------------------------- 1 | /** Tag family with 30 distinct codes. 2 | bits: 16, minimum hamming: 5, minimum complexity: 5 3 | 4 | Max bits corrected False positive rate 5 | 0 0.045776 % 6 | 1 0.778198 % 7 | 2 6.271362 % 8 | 9 | Generation time: 0.309000 s 10 | 11 | Hamming distance between pairs of codes (accounting for rotation): 12 | 13 | 0 0 14 | 1 0 15 | 2 0 16 | 3 0 17 | 4 0 18 | 5 120 19 | 6 172 20 | 7 91 21 | 8 33 22 | 9 13 23 | 10 6 24 | 11 0 25 | 12 0 26 | 13 0 27 | 14 0 28 | 15 0 29 | 16 0 30 | **/ 31 | 32 | #pragma once 33 | 34 | namespace AprilTags { 35 | 36 | const unsigned long long t16h5[] = {0x231bLL, 0x2ea5LL, 0x346aLL, 0x45b9LL, 0x79a6LL, 0x7f6bLL, 0xb358LL, 0xe745LL, 37 | 0xfe59LL, 0x156dLL, 0x380bLL, 0xf0abLL, 0x0d84LL, 0x4736LL, 0x8c72LL, 0xaf10LL, 38 | 0x093cLL, 0x93b4LL, 0xa503LL, 0x468fLL, 0xe137LL, 0x5795LL, 0xdf42LL, 0x1c1dLL, 39 | 0xe9dcLL, 0x73adLL, 0xad5fLL, 0xd530LL, 0x07caLL, 0xaf2eLL}; 40 | 41 | static const TagCodes tagCodes16h5 = TagCodes(16, 5, t16h5, sizeof(t16h5) / sizeof(t16h5[0])); 42 | 43 | } // namespace AprilTags 44 | -------------------------------------------------------------------------------- /thirdparty/camera_model/include/camera_model/apriltag/Tag16h5_other.h: -------------------------------------------------------------------------------- 1 | // These codes were generated by TagFamilyGenerator.java from Ed Olson 2 | 3 | // tag16h5Codes : 16 bits, minimum Hamming distance 5, minimum complexity 5 4 | 5 | #pragma once 6 | 7 | namespace AprilTags { 8 | 9 | const unsigned long long t16h5_other[] = { 10 | 0x231bLL, 0x2ea5LL, 0x346aLL, 0x45b9LL, 0x6857LL, 0x7f6bLL, 0xad93LL, 0xb358LL, 0xb91dLL, 0xe745LL, 11 | 0x156dLL, 0xd3d2LL, 0xdf5cLL, 0x4736LL, 0x8c72LL, 0x5a02LL, 0xd32bLL, 0x1867LL, 0x468fLL, 0xdc91LL, 12 | 0x4940LL, 0xa9edLL, 0x2bd5LL, 0x599aLL, 0x9009LL, 0x61f6LL, 0x3850LL, 0x8157LL, 0xbfcaLL, 0x987cLL}; 13 | 14 | static const TagCodes tagCodes16h5_other = TagCodes(16, 5, t16h5_other, sizeof(t16h5_other) / sizeof(t16h5_other[0])); 15 | 16 | } // namespace AprilTags 17 | -------------------------------------------------------------------------------- /thirdparty/camera_model/include/camera_model/apriltag/Tag25h9.h: -------------------------------------------------------------------------------- 1 | /** Tag family with 35 distinct codes. 2 | bits: 25, minimum hamming: 9, minimum complexity: 8 3 | 4 | Max bits corrected False positive rate 5 | 0 0.000104 % 6 | 1 0.002712 % 7 | 2 0.034004 % 8 | 3 0.273913 % 9 | 4 1.593411 % 10 | 11 | Generation time: 31.283000 s 12 | 13 | Hamming distance between pairs of codes (accounting for rotation): 14 | 15 | 0 0 16 | 1 0 17 | 2 0 18 | 3 0 19 | 4 0 20 | 5 0 21 | 6 0 22 | 7 0 23 | 8 0 24 | 9 156 25 | 10 214 26 | 11 120 27 | 12 64 28 | 13 29 29 | 14 11 30 | 15 1 31 | 16 0 32 | 17 0 33 | 18 0 34 | 19 0 35 | 20 0 36 | 21 0 37 | 22 0 38 | 23 0 39 | 24 0 40 | 25 0 41 | **/ 42 | 43 | #pragma once 44 | 45 | namespace AprilTags { 46 | 47 | const unsigned long long t25h9[] = { 48 | 0x155cbf1LL, 0x1e4d1b6LL, 0x17b0b68LL, 0x1eac9cdLL, 0x12e14ceLL, 0x3548bbLL, 0x7757e6LL, 0x1065dabLL, 0x1baa2e7LL, 49 | 0xdea688LL, 0x81d927LL, 0x51b241LL, 0xdbc8aeLL, 0x1e50e19LL, 0x15819d2LL, 0x16d8282LL, 0x163e035LL, 0x9d9b81LL, 50 | 0x173eec4LL, 0xae3a09LL, 0x5f7c51LL, 0x1a137fcLL, 0xdc9562LL, 0x1802e45LL, 0x1c3542cLL, 0x870fa4LL, 0x914709LL, 51 | 0x16684f0LL, 0xc8f2a5LL, 0x833ebbLL, 0x59717fLL, 0x13cd050LL, 0xfa0ad1LL, 0x1b763b0LL, 0xb991ceLL}; 52 | 53 | static const TagCodes tagCodes25h9 = TagCodes(25, 9, t25h9, sizeof(t25h9) / sizeof(t25h9[0])); 54 | 55 | } // namespace AprilTags 56 | -------------------------------------------------------------------------------- /thirdparty/camera_model/include/camera_model/apriltag/TagDetector.h: -------------------------------------------------------------------------------- 1 | #ifndef TAGDETECTOR_H 2 | #define TAGDETECTOR_H 3 | 4 | #include 5 | 6 | #include "opencv2/opencv.hpp" 7 | 8 | #include "TagDetection.h" 9 | #include "TagFamily.h" 10 | #include "FloatImage.h" 11 | 12 | namespace AprilTags { 13 | 14 | class TagDetector { 15 | public: 16 | const TagFamily thisTagFamily; 17 | 18 | //! Constructor 19 | // note: TagFamily is instantiated here from TagCodes 20 | TagDetector(const TagCodes& tagCodes, const size_t blackBorder = 2) : thisTagFamily(tagCodes, blackBorder) {} 21 | 22 | std::vector extractTags(const cv::Mat& image); 23 | }; 24 | 25 | } // namespace AprilTags 26 | 27 | #endif 28 | -------------------------------------------------------------------------------- /thirdparty/camera_model/include/camera_model/apriltag/UnionFindSimple.h: -------------------------------------------------------------------------------- 1 | #ifndef UNIONFINDSIMPLE_H 2 | #define UNIONFINDSIMPLE_H 3 | 4 | #include 5 | 6 | namespace AprilTags { 7 | 8 | //! Implementation of disjoint set data structure using the union-find algorithm 9 | class UnionFindSimple { 10 | //! Identifies parent ids and sizes. 11 | struct Data { 12 | int id; 13 | int size; 14 | }; 15 | 16 | public: 17 | explicit UnionFindSimple(int maxId) : data(maxId) { init(); }; 18 | 19 | int getSetSize(int thisId) { return data[getRepresentative(thisId)].size; } 20 | 21 | int getRepresentative(int thisId); 22 | 23 | //! Returns the id of the merged node. 24 | /* @param aId 25 | * @param bId 26 | */ 27 | int connectNodes(int aId, int bId); 28 | 29 | void printDataVector() const; 30 | 31 | private: 32 | void init(); 33 | 34 | std::vector data; 35 | }; 36 | 37 | } // namespace AprilTags 38 | 39 | #endif 40 | -------------------------------------------------------------------------------- /thirdparty/camera_model/include/camera_model/apriltag/XYWeight.h: -------------------------------------------------------------------------------- 1 | #ifndef XYWeight_H_ 2 | #define XYWeight_H_ 3 | 4 | namespace AprilTags { 5 | 6 | //! Represents a triple holding an x value, y value, and weight value. 7 | struct XYWeight { 8 | float x; 9 | float y; 10 | float weight; 11 | 12 | XYWeight(float xval, float yval, float weightval) : x(xval), y(yval), weight(weightval) {} 13 | }; 14 | 15 | } // namespace AprilTags 16 | 17 | #endif 18 | -------------------------------------------------------------------------------- /thirdparty/camera_model/include/camera_model/apriltag/pch.h: -------------------------------------------------------------------------------- 1 | #include "Edge.h" 2 | #include "FloatImage.h" 3 | #include "GLine2D.h" 4 | //#include "GLineSegment2D.h" 5 | #include "Gaussian.h" 6 | //#include "GrayModel.h" 7 | //#include "Gridder.h" 8 | #include "Homography33.h" 9 | #include "MathUtil.h" 10 | //#include "Quad.h" 11 | //#include "Segment.h" 12 | //#include "TagDetection.h" 13 | //#include "TagDetector.h" 14 | //#include "TagFamily.h" 15 | #include "UnionFindSimple.h" 16 | #include "XYWeight.h" 17 | -------------------------------------------------------------------------------- /thirdparty/camera_model/include/camera_model/camera_models/CameraFactory.h: -------------------------------------------------------------------------------- 1 | #ifndef CAMERAFACTORY_H 2 | #define CAMERAFACTORY_H 3 | 4 | #include 5 | #include 6 | 7 | #include "camera_model/camera_models/Camera.h" 8 | 9 | namespace camera_model { 10 | 11 | class CameraFactory { 12 | public: 13 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 14 | CameraFactory(); 15 | 16 | static boost::shared_ptr instance(); 17 | 18 | static CameraPtr generateCamera(Camera::ModelType modelType, const std::string& cameraName, 19 | const cv::Size& imageSize); 20 | 21 | static CameraPtr generateCameraFromYamlFile(const std::string& filename); 22 | 23 | private: 24 | static boost::shared_ptr m_instance; 25 | }; 26 | 27 | } // namespace camera_model 28 | 29 | #endif 30 | -------------------------------------------------------------------------------- /thirdparty/camera_model/include/camera_model/camera_models/CostFunctionFactory.h: -------------------------------------------------------------------------------- 1 | #ifndef COSTFUNCTIONFACTORY_H 2 | #define COSTFUNCTIONFACTORY_H 3 | 4 | #include 5 | #include 6 | 7 | #include "camera_model/camera_models/Camera.h" 8 | 9 | namespace ceres { 10 | class CostFunction; 11 | } 12 | 13 | namespace camera_model { 14 | 15 | enum { 16 | CAMERA_INTRINSICS = 1 << 0, 17 | CAMERA_POSE = 1 << 1, 18 | POINT_3D = 1 << 2, 19 | ODOMETRY_INTRINSICS = 1 << 3, 20 | ODOMETRY_3D_POSE = 1 << 4, 21 | ODOMETRY_6D_POSE = 1 << 5, 22 | CAMERA_ODOMETRY_TRANSFORM = 1 << 6 23 | }; 24 | 25 | class CostFunctionFactory { 26 | public: 27 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 28 | CostFunctionFactory(); 29 | 30 | static boost::shared_ptr instance(void); 31 | 32 | ceres::CostFunction* generateCostFunction(const CameraConstPtr& camera, const Eigen::Vector3d& observed_P, 33 | const Eigen::Vector2d& observed_p, int flags) const; 34 | 35 | private: 36 | static boost::shared_ptr m_instance; 37 | }; 38 | } // namespace camera_model 39 | 40 | #endif 41 | -------------------------------------------------------------------------------- /thirdparty/camera_model/include/camera_model/chessboard/Chessboard.h: -------------------------------------------------------------------------------- 1 | #ifndef CHESSBOARD_H 2 | #define CHESSBOARD_H 3 | 4 | #include 5 | #include 6 | 7 | namespace camera_model { 8 | 9 | // forward declarations 10 | class ChessboardCorner; 11 | typedef boost::shared_ptr ChessboardCornerPtr; 12 | class ChessboardQuad; 13 | typedef boost::shared_ptr ChessboardQuadPtr; 14 | 15 | class Chessboard { 16 | public: 17 | Chessboard(cv::Size boardSize, cv::Mat& image); 18 | Chessboard(cv::Size boardSize, double &tag_size); 19 | void findCorners(bool useOpenCV = false); 20 | bool findCorners(cv::Mat& image, 21 | std::vector &objectPoints, 22 | std::vector &imagePoints, 23 | bool useOpenCV= false); 24 | const std::vector& getCorners(void) const; 25 | bool cornersFound(void) const; 26 | 27 | const cv::Mat& getImage(void) const; 28 | const cv::Mat& getSketch(void) const; 29 | 30 | const int& cols(void) const; 31 | const int& rows(void) const; 32 | 33 | const double& get_tagsize(void) const; 34 | 35 | private: 36 | bool findChessboardCorners(const cv::Mat& image, const cv::Size& patternSize, std::vector& corners, 37 | int flags, bool useOpenCV); 38 | 39 | bool findChessboardCornersImproved(const cv::Mat& image, const cv::Size& patternSize, 40 | std::vector& corners, int flags); 41 | 42 | void cleanFoundConnectedQuads(std::vector& quadGroup, cv::Size patternSize); 43 | 44 | void findConnectedQuads(std::vector& quads, std::vector& group, int group_idx, 45 | int dilation); 46 | 47 | // int checkQuadGroup(std::vector& quadGroup, 48 | // std::vector& outCorners, 49 | // cv::Size patternSize); 50 | 51 | void labelQuadGroup(std::vector& quad_group, cv::Size patternSize, bool firstRun); 52 | 53 | void findQuadNeighbors(std::vector& quads, int dilation); 54 | 55 | int augmentBestRun(std::vector& candidateQuads, int candidateDilation, 56 | std::vector& existingQuads, int existingDilation); 57 | 58 | void generateQuads(std::vector& quads, cv::Mat& image, int flags, int dilation, bool firstRun); 59 | 60 | bool checkQuadGroup(std::vector& quads, std::vector& corners, 61 | cv::Size patternSize); 62 | 63 | void getQuadrangleHypotheses(const std::vector >& contours, 64 | std::vector >& quads, int classId) const; 65 | 66 | bool checkChessboard(const cv::Mat& image, cv::Size patternSize) const; 67 | 68 | bool checkBoardMonotony(std::vector& corners, cv::Size patternSize); 69 | 70 | bool matchCorners(ChessboardQuadPtr& quad1, int corner1, ChessboardQuadPtr& quad2, int corner2) const; 71 | 72 | cv::Mat mImage; 73 | cv::Mat mSketch; 74 | std::vector mCorners; 75 | cv::Size mBoardSize; 76 | bool mCornersFound; 77 | double m_tag_size; //单位米 78 | }; 79 | } // namespace camera_model 80 | 81 | #endif 82 | -------------------------------------------------------------------------------- /thirdparty/camera_model/include/camera_model/chessboard/ChessboardCorner.h: -------------------------------------------------------------------------------- 1 | #ifndef CHESSBOARDCORNER_H 2 | #define CHESSBOARDCORNER_H 3 | 4 | #include 5 | #include 6 | 7 | namespace camera_model { 8 | 9 | class ChessboardCorner; 10 | typedef boost::shared_ptr ChessboardCornerPtr; 11 | 12 | class ChessboardCorner { 13 | public: 14 | ChessboardCorner() : row(0), column(0), needsNeighbor(true), count(0) {} 15 | 16 | float meanDist(int &n) const { 17 | float sum = 0; 18 | n = 0; 19 | for (int i = 0; i < 4; ++i) { 20 | if (neighbors[i].get()) { 21 | float dx = neighbors[i]->pt.x - pt.x; 22 | float dy = neighbors[i]->pt.y - pt.y; 23 | sum += sqrt(dx * dx + dy * dy); 24 | n++; 25 | } 26 | } 27 | return sum / std::max(n, 1); 28 | } 29 | 30 | cv::Point2f pt; // X and y coordinates 31 | int row; // Row and column of the corner 32 | int column; // in the found pattern 33 | bool needsNeighbor; // Does the corner require a neighbor? 34 | int count; // number of corner neighbors 35 | ChessboardCornerPtr neighbors[4]; // pointer to all corner neighbors 36 | }; 37 | 38 | } // namespace camera_model 39 | 40 | #endif 41 | -------------------------------------------------------------------------------- /thirdparty/camera_model/include/camera_model/chessboard/ChessboardQuad.h: -------------------------------------------------------------------------------- 1 | #ifndef CHESSBOARDQUAD_H 2 | #define CHESSBOARDQUAD_H 3 | 4 | #include 5 | 6 | #include "camera_model/chessboard/ChessboardCorner.h" 7 | 8 | namespace camera_model { 9 | 10 | class ChessboardQuad; 11 | typedef boost::shared_ptr ChessboardQuadPtr; 12 | 13 | class ChessboardQuad { 14 | public: 15 | ChessboardQuad() : count(0), group_idx(-1), edge_len(FLT_MAX), labeled(false) {} 16 | 17 | int count; // Number of quad neighbors 18 | int group_idx; // Quad group ID 19 | float edge_len; // Smallest side length^2 20 | ChessboardCornerPtr corners[4]; // Coordinates of quad corners 21 | ChessboardQuadPtr neighbors[4]; // Pointers of quad neighbors 22 | bool labeled; // Has this corner been labeled? 23 | }; 24 | 25 | } // namespace camera_model 26 | 27 | #endif 28 | -------------------------------------------------------------------------------- /thirdparty/camera_model/include/camera_model/code_utils/cv_utils.h: -------------------------------------------------------------------------------- 1 | #ifndef CV_UTLIS_H 2 | #define CV_UTLIS_H 3 | 4 | #include 5 | 6 | namespace cv_utils { 7 | namespace fisheye { 8 | class PreProcess { 9 | public: 10 | PreProcess(); 11 | PreProcess(const cv::Size _raw_image_size, const cv::Size _roi_size, const cv::Point _center, 12 | const float _resize_scale); 13 | void resetPreProcess(cv::Size _roi_size, cv::Point _center, float _resize_scale); 14 | 15 | cv::Mat do_preprocess(cv::Mat image_input); 16 | 17 | float resize_scale; 18 | int roi_row_start; 19 | int roi_col_start; 20 | int roi_row_end; 21 | int roi_col_end; 22 | 23 | bool is_preprocess; 24 | bool is_resize_only; 25 | }; 26 | } // namespace fisheye 27 | } // namespace cv_utils 28 | #endif // CV_UTLIS_H 29 | -------------------------------------------------------------------------------- /thirdparty/camera_model/include/camera_model/code_utils/eigen_utils.h: -------------------------------------------------------------------------------- 1 | #ifndef EIGEN_UTLIS_H 2 | #define EIGEN_UTLIS_H 3 | 4 | #include 5 | 6 | #include 7 | 8 | using namespace std; 9 | 10 | namespace eigen_utils { 11 | 12 | typedef Eigen::Matrix Vector; 13 | typedef Eigen::Matrix Matrix; 14 | 15 | template 16 | struct EigenTypes { 17 | typedef Eigen::Matrix Matrix; 18 | 19 | typedef Eigen::Matrix Vector; 20 | }; 21 | 22 | /** 23 | * 24 | */ 25 | template 26 | inline vector_t SwapSequence(vector_t vec_in) { 27 | // int size = vec_in.size(); 28 | // vector_t vec_out(size); 29 | 30 | // for (int i = 0; i < size; i++) 31 | // { 32 | // vec_out(size - 1 - i) = vec_in(i); 33 | // } 34 | 35 | // return vec_out; 36 | 37 | return vec_in.reverse(); 38 | } 39 | 40 | inline Vector pushback(Vector vec_in, const double value) { 41 | Vector vec_out(vec_in.size() + 1); 42 | 43 | vec_out.segment(0, vec_in.size()) = vec_in; 44 | vec_out(vec_in.size()) = value; 45 | 46 | return vec_out; 47 | } 48 | 49 | template 50 | inline void copyMat3ToArry(const Eigen::Matrix mat, T* data) { 51 | data[0] = T(mat(0, 0)); 52 | data[1] = T(mat(1, 0)); 53 | data[2] = T(mat(2, 0)); 54 | data[3] = T(mat(0, 1)); 55 | data[4] = T(mat(1, 1)); 56 | data[5] = T(mat(2, 1)); 57 | data[6] = T(mat(0, 2)); 58 | data[7] = T(mat(1, 2)); 59 | data[8] = T(mat(2, 2)); 60 | } 61 | template 62 | inline void copyArryToMat3(T* data, Eigen::Matrix& mat) { 63 | mat(0, 0) = data[0]; 64 | mat(1, 0) = data[1]; 65 | mat(2, 0) = data[2]; 66 | mat(0, 1) = data[3]; 67 | mat(1, 1) = data[4]; 68 | mat(2, 1) = data[5]; 69 | mat(0, 2) = data[6]; 70 | mat(1, 2) = data[7]; 71 | mat(2, 2) = data[8]; 72 | } 73 | template 74 | inline void copyVector3ToArry(const Eigen::Matrix vec, T* data) { 75 | data[0] = T(vec(0)); 76 | data[1] = T(vec(1)); 77 | data[2] = T(vec(2)); 78 | } 79 | template 80 | inline void copyArryToVector3(T* data, Eigen::Matrix& vec) { 81 | vec(0) = data[0]; 82 | vec(1) = data[1]; 83 | vec(2) = data[2]; 84 | } 85 | 86 | }; // namespace eigen_utils 87 | 88 | #endif // EIGEN_UTLIS_H 89 | -------------------------------------------------------------------------------- /thirdparty/camera_model/include/camera_model/code_utils/math_utils/Polynomial.h: -------------------------------------------------------------------------------- 1 | #ifndef POLYNOMIAL_H 2 | #define POLYNOMIAL_H 3 | 4 | #include 5 | #include 6 | 7 | #include 8 | 9 | #include 10 | #include 11 | 12 | namespace math_utils { 13 | 14 | class Polynomial { 15 | public: 16 | Polynomial(); 17 | Polynomial(const int _order); 18 | Polynomial(const eigen_utils::Vector _coeff); 19 | 20 | eigen_utils::Vector getRealRoot(double _y); 21 | eigen_utils::Vector getRealRoot(double _y, double x_min, double x_max); 22 | double getOneRealRoot(double _y, double x_min, double x_max); 23 | 24 | eigen_utils::Vector getValue(const eigen_utils::Vector& in); 25 | double getValue(const double in); 26 | 27 | void setPolyOrder(int _order); 28 | int getPolyOrder() const; 29 | 30 | void setPolyCoeff(const eigen_utils::Vector& value); 31 | void setPolyCoeff(int order_index, double value); 32 | 33 | eigen_utils::Vector getPolyCoeff() const; 34 | double getPolyCoeff(int order_index) const; 35 | 36 | Polynomial& operator=(const Polynomial& other); 37 | friend std::ostream& operator<<(std::ostream& out, const Polynomial& poly); 38 | std::string toString(void) const; 39 | 40 | private: 41 | double Evaluate(double _x); 42 | 43 | // _y = f(x) 44 | bool FindRoots(const double y, const eigen_utils::Vector& polynomial_in, eigen_utils::Vector& real, 45 | eigen_utils::Vector& imaginary); 46 | 47 | void FindLinearPolynomialRoots(const eigen_utils::Vector& Polynomial, eigen_utils::Vector& real, 48 | eigen_utils::Vector& imag); 49 | 50 | void FindQuadraticPolynomialRoots(const eigen_utils::Vector& Polynomial, eigen_utils::Vector& real, 51 | eigen_utils::Vector& imag); 52 | 53 | void BuildCompanionMatrix(const eigen_utils::Vector& Polynomial, eigen_utils::Matrix* companion_matrix_ptr); 54 | 55 | // Balancing function as described by B. N. Parlett and C. Reinsch, 56 | // "Balancing a Matrix for Calculation of Eigenvalues and Eigenvectors". 57 | // In: Numerische Mathematik, Volume 13, Number 4 (1969), 293-304, 58 | // Springer Berlin / Heidelberg. DOI: 10.1007/BF02165404 59 | void BalanceCompanionMatrix(eigen_utils::Matrix* companion_matrix_ptr); 60 | 61 | private: 62 | int m_order; 63 | // c0, c1, c2, ..., cn 64 | // c0 + c1X + c2X^2 + ... +cnX^n 65 | eigen_utils::Vector m_coeff; 66 | }; 67 | 68 | typedef struct { 69 | double x; 70 | double y; 71 | } Sample; 72 | 73 | class PolynomialFit : public Polynomial { 74 | public: 75 | PolynomialFit(int _order); 76 | PolynomialFit(int _order, eigen_utils::Vector _x, eigen_utils::Vector _y); 77 | 78 | void loadSamples(const eigen_utils::Vector& x, const eigen_utils::Vector& y); 79 | void loadSample(const Sample sample); 80 | void clearSamples(); 81 | 82 | eigen_utils::Vector getCoeff(); 83 | Polynomial& getPolynomial(); 84 | 85 | private: 86 | eigen_utils::Vector Fit(); 87 | 88 | private: 89 | int data_size; 90 | vector samples; 91 | Polynomial* poly; 92 | eigen_utils::Vector coeff; 93 | }; 94 | } // namespace math_utils 95 | #endif // POLYNOMIAL_H 96 | -------------------------------------------------------------------------------- /thirdparty/camera_model/include/camera_model/code_utils/sys_utils.h: -------------------------------------------------------------------------------- 1 | #ifndef SYS_UTILS_H 2 | #define SYS_UTILS_H 3 | 4 | #include 5 | #include 6 | 7 | namespace sys_utils { 8 | 9 | inline bool float_equal(const float a, const float b) { 10 | if (std::abs(a - b) < 1e-6) 11 | return true; 12 | else 13 | return false; 14 | } 15 | 16 | inline bool double_equal(const double a, const double b) { 17 | if (std::abs(a - b) < 1e-6) 18 | return true; 19 | else 20 | return false; 21 | } 22 | 23 | inline unsigned long long timeInMicroseconds(void) { 24 | struct timespec tp; 25 | 26 | clock_gettime(CLOCK_REALTIME, &tp); 27 | 28 | return (tp.tv_sec * 1000000 + tp.tv_nsec / 1000); 29 | } 30 | 31 | inline double timeInSeconds(void) { 32 | struct timespec tp; 33 | 34 | clock_gettime(CLOCK_REALTIME, &tp); 35 | 36 | return (static_cast(tp.tv_sec) + static_cast(tp.tv_nsec) / 1000000000.0); 37 | } 38 | 39 | inline void PrintWarning(std::string str) { std::cout << "\033[33;40;1m" << str << "\033[0m" << std::endl; } 40 | 41 | inline void PrintError(std::string str) { std::cout << "\033[31;47;1m" << str << "\033[0m" << std::endl; } 42 | 43 | inline void PrintInfo(std::string str) { std::cout << "\033[32;40;1m" << str << "\033[0m" << std::endl; } 44 | } // namespace sys_utils 45 | #endif // SYS_UTILS_H 46 | -------------------------------------------------------------------------------- /thirdparty/camera_model/include/camera_model/gpl/EigenQuaternionParameterization.h: -------------------------------------------------------------------------------- 1 | #ifndef EIGENQUATERNIONPARAMETERIZATION_H 2 | #define EIGENQUATERNIONPARAMETERIZATION_H 3 | 4 | #include "ceres/local_parameterization.h" 5 | 6 | namespace camera_model { 7 | 8 | class EigenQuaternionParameterization : public ceres::LocalParameterization { 9 | public: 10 | virtual ~EigenQuaternionParameterization() {} 11 | virtual bool Plus(const double* x, const double* delta, double* x_plus_delta) const; 12 | virtual bool ComputeJacobian(const double* x, double* jacobian) const; 13 | virtual int GlobalSize() const { return 4; } 14 | virtual int LocalSize() const { return 3; } 15 | 16 | private: 17 | template 18 | void EigenQuaternionProduct(const T z[4], const T w[4], T zw[4]) const; 19 | }; 20 | 21 | template 22 | void EigenQuaternionParameterization::EigenQuaternionProduct(const T z[4], const T w[4], T zw[4]) const { 23 | zw[0] = z[3] * w[0] + z[0] * w[3] + z[1] * w[2] - z[2] * w[1]; 24 | zw[1] = z[3] * w[1] - z[0] * w[2] + z[1] * w[3] + z[2] * w[0]; 25 | zw[2] = z[3] * w[2] + z[0] * w[1] - z[1] * w[0] + z[2] * w[3]; 26 | zw[3] = z[3] * w[3] - z[0] * w[0] - z[1] * w[1] - z[2] * w[2]; 27 | } 28 | 29 | } // namespace camera_model 30 | 31 | #endif 32 | -------------------------------------------------------------------------------- /thirdparty/camera_model/include/camera_model/gpl/gpl.h: -------------------------------------------------------------------------------- 1 | #ifndef GPL_H 2 | #define GPL_H 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | namespace camera_model { 9 | 10 | template 11 | const T clamp(const T& v, const T& a, const T& b) { 12 | return std::min(b, std::max(a, v)); 13 | } 14 | 15 | double hypot3(double x, double y, double z); 16 | float hypot3f(float x, float y, float z); 17 | 18 | template 19 | const T normalizeTheta(const T& theta) { 20 | T normTheta = theta; 21 | 22 | while (normTheta < -M_PI) { 23 | normTheta += 2.0 * M_PI; 24 | } 25 | while (normTheta > M_PI) { 26 | normTheta -= 2.0 * M_PI; 27 | } 28 | 29 | return normTheta; 30 | } 31 | 32 | double d2r(double deg); 33 | float d2r(float deg); 34 | double r2d(double rad); 35 | float r2d(float rad); 36 | 37 | double sinc(double theta); 38 | 39 | template 40 | const T square(const T& x) { 41 | return x * x; 42 | } 43 | 44 | template 45 | const T cube(const T& x) { 46 | return x * x * x; 47 | } 48 | 49 | template 50 | const T random(const T& a, const T& b) { 51 | return static_cast(rand()) / RAND_MAX * (b - a) + a; 52 | } 53 | 54 | template 55 | const T randomNormal(const T& sigma) { 56 | T x1, x2, w; 57 | 58 | do { 59 | x1 = 2.0 * random(0.0, 1.0) - 1.0; 60 | x2 = 2.0 * random(0.0, 1.0) - 1.0; 61 | w = x1 * x1 + x2 * x2; 62 | } while (w >= 1.0 || w == 0.0); 63 | 64 | w = sqrt((-2.0 * log(w)) / w); 65 | 66 | return x1 * w * sigma; 67 | } 68 | 69 | unsigned long long timeInMicroseconds(void); 70 | 71 | double timeInSeconds(void); 72 | 73 | void colorDepthImage(cv::Mat& imgDepth, cv::Mat& imgColoredDepth, float minRange, float maxRange); 74 | 75 | bool colormap(const std::string& name, unsigned char idx, float& r, float& g, float& b); 76 | 77 | std::vector bresLine(int x0, int y0, int x1, int y1); 78 | std::vector bresCircle(int x0, int y0, int r); 79 | 80 | void fitCircle(const std::vector& points, double& centerX, double& centerY, double& radius); 81 | 82 | std::vector intersectCircles(double x1, double y1, double r1, double x2, double y2, double r2); 83 | 84 | void LLtoUTM(double latitude, double longitude, double& utmNorthing, double& utmEasting, std::string& utmZone); 85 | void UTMtoLL(double utmNorthing, double utmEasting, const std::string& utmZone, double& latitude, double& longitude); 86 | 87 | long int timestampDiff(uint64_t t1, uint64_t t2); 88 | 89 | } // namespace camera_model 90 | 91 | #endif 92 | -------------------------------------------------------------------------------- /thirdparty/camera_model/include/camera_model/sparse_graph/Transform.h: -------------------------------------------------------------------------------- 1 | #ifndef TRANSFORM_H 2 | #define TRANSFORM_H 3 | 4 | #include 5 | #include 6 | // #include 7 | 8 | namespace camera_model { 9 | 10 | class Transform { 11 | public: 12 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 13 | 14 | Transform(); 15 | explicit Transform(const Eigen::Matrix4d& H); 16 | 17 | Eigen::Quaterniond& rotation(); 18 | const Eigen::Quaterniond& rotation() const; 19 | double* rotationData(); 20 | const double* rotationData() const; 21 | 22 | Eigen::Vector3d& translation(); 23 | const Eigen::Vector3d& translation() const; 24 | double* translationData(); 25 | const double* translationData() const; 26 | 27 | Eigen::Matrix4d toMatrix() const; 28 | 29 | private: 30 | Eigen::Quaterniond m_q; 31 | Eigen::Vector3d m_t; 32 | }; 33 | 34 | } // namespace camera_model 35 | 36 | #endif 37 | -------------------------------------------------------------------------------- /thirdparty/camera_model/readme.md: -------------------------------------------------------------------------------- 1 | modified from https://github.com/gaowenliang/camera_model 2 | -------------------------------------------------------------------------------- /thirdparty/camera_model/src/apriltag/FloatImage.cc: -------------------------------------------------------------------------------- 1 | #include "camera_model/apriltag/FloatImage.h" 2 | #include "camera_model/apriltag/Gaussian.h" 3 | #include 4 | 5 | namespace AprilTags { 6 | 7 | FloatImage::FloatImage() : width(0), height(0), pixels() {} 8 | 9 | FloatImage::FloatImage(int widthArg, int heightArg) 10 | : width(widthArg), height(heightArg), pixels(widthArg * heightArg) {} 11 | 12 | FloatImage::FloatImage(int widthArg, int heightArg, const std::vector& pArg) 13 | : width(widthArg), height(heightArg), pixels(pArg) {} 14 | 15 | FloatImage& FloatImage::operator=(const FloatImage& other) { 16 | width = other.width; 17 | height = other.height; 18 | if (pixels.size() != other.pixels.size()) pixels.resize(other.pixels.size()); 19 | pixels = other.pixels; 20 | return *this; 21 | } 22 | 23 | void FloatImage::decimateAvg() { 24 | int nWidth = width / 2; 25 | int nHeight = height / 2; 26 | 27 | for (int y = 0; y < nHeight; y++) 28 | for (int x = 0; x < nWidth; x++) pixels[y * nWidth + x] = pixels[(2 * y) * width + (2 * x)]; 29 | 30 | width = nWidth; 31 | height = nHeight; 32 | pixels.resize(nWidth * nHeight); 33 | } 34 | 35 | void FloatImage::normalize() { 36 | const float maxVal = *max_element(pixels.begin(), pixels.end()); 37 | const float minVal = *min_element(pixels.begin(), pixels.end()); 38 | const float range = maxVal - minVal; 39 | const float rescale = 1 / range; 40 | for (unsigned int i = 0; i < pixels.size(); i++) pixels[i] = (pixels[i] - minVal) * rescale; 41 | } 42 | 43 | void FloatImage::filterFactoredCentered(const std::vector& fhoriz, const std::vector& fvert) { 44 | // do horizontal 45 | std::vector r(pixels); 46 | 47 | for (int y = 0; y < height; y++) { 48 | Gaussian::convolveSymmetricCentered(pixels, y * width, width, fhoriz, r, y * width); 49 | } 50 | 51 | // do vertical 52 | std::vector tmp(height); // column before convolution 53 | std::vector tmp2(height); // column after convolution 54 | 55 | for (int x = 0; x < width; x++) { 56 | // copy the column out for locality 57 | for (int y = 0; y < height; y++) tmp[y] = r[y * width + x]; 58 | 59 | Gaussian::convolveSymmetricCentered(tmp, 0, height, fvert, tmp2, 0); 60 | 61 | for (int y = 0; y < height; y++) pixels[y * width + x] = tmp2[y]; 62 | } 63 | } 64 | 65 | void FloatImage::printMinMax() const { 66 | std::cout << "Min: " << *min_element(pixels.begin(), pixels.end()) 67 | << ", Max: " << *max_element(pixels.begin(), pixels.end()) << std::endl; 68 | // for (int i = 0; i < getNumFloatImagePixels(); i++) 69 | // std::cout << "Index[" << i << "]: " << this->normalize().getFloatImagePixels()[i] << endl; 70 | } 71 | 72 | } // namespace AprilTags 73 | -------------------------------------------------------------------------------- /thirdparty/camera_model/src/apriltag/GLineSegment2D.cc: -------------------------------------------------------------------------------- 1 | #include "camera_model/apriltag/GLineSegment2D.h" 2 | #include 3 | 4 | namespace AprilTags { 5 | 6 | GLineSegment2D::GLineSegment2D(const std::pair& p0Arg, const std::pair& p1Arg) 7 | : line(p0Arg, p1Arg), p0(p0Arg), p1(p1Arg), weight() {} 8 | 9 | GLineSegment2D GLineSegment2D::lsqFitXYW(const std::vector& xyweight) { 10 | GLine2D gline = GLine2D::lsqFitXYW(xyweight); 11 | float maxcoord = -std::numeric_limits::infinity(); 12 | float mincoord = std::numeric_limits::infinity(); 13 | ; 14 | 15 | for (unsigned int i = 0; i < xyweight.size(); i++) { 16 | std::pair p(xyweight[i].x, xyweight[i].y); 17 | float coord = gline.getLineCoordinate(p); 18 | maxcoord = std::max(maxcoord, coord); 19 | mincoord = std::min(mincoord, coord); 20 | } 21 | 22 | std::pair minValue = gline.getPointOfCoordinate(mincoord); 23 | std::pair maxValue = gline.getPointOfCoordinate(maxcoord); 24 | return GLineSegment2D(minValue, maxValue); 25 | } 26 | 27 | } // namespace AprilTags 28 | -------------------------------------------------------------------------------- /thirdparty/camera_model/src/apriltag/Gaussian.cc: -------------------------------------------------------------------------------- 1 | #include "camera_model/apriltag/Gaussian.h" 2 | #include 3 | 4 | namespace AprilTags { 5 | 6 | bool Gaussian::warned = false; 7 | 8 | std::vector Gaussian::makeGaussianFilter(float sigma, int n) { 9 | std::vector f(n, 0.0f); 10 | 11 | if (sigma == 0) { 12 | for (int i = 0; i < n; i++) f[i] = 0; 13 | f[n / 2] = 1; 14 | return f; 15 | } 16 | 17 | double const inv_variance = 1. / (2 * sigma * sigma); 18 | float sum = 0; 19 | for (int i = 0; i < n; i++) { 20 | int j = i - n / 2; 21 | f[i] = (float)std::exp(-j * j * inv_variance); 22 | sum += f[i]; 23 | } 24 | 25 | // normalize the gaussian 26 | for (int i = 0; i < n; i++) f[i] /= sum; 27 | 28 | return f; 29 | } 30 | 31 | void Gaussian::convolveSymmetricCentered(const std::vector& a, unsigned int aoff, unsigned int alen, 32 | const std::vector& f, std::vector& r, unsigned int roff) { 33 | if ((f.size() & 1) == 0 && !warned) { 34 | std::cout << "convolveSymmetricCentered Warning: filter is not odd length\n"; 35 | warned = true; 36 | } 37 | 38 | for (size_t i = f.size() / 2; i < f.size(); i++) { 39 | double acc = 0; 40 | for (size_t j = 0; j < f.size(); j++) { 41 | if ((aoff + i) < j || (aoff + i) >= (alen + j)) 42 | acc += a[aoff] * f[j]; 43 | else 44 | acc += a[aoff + i - j] * f[j]; 45 | } 46 | r[roff + i - f.size() / 2] = (float)acc; 47 | } 48 | 49 | for (size_t i = f.size(); i < alen; i++) { 50 | double acc = 0; 51 | for (unsigned int j = 0; j < f.size(); j++) { 52 | acc += a[aoff + i - j] * f[j]; 53 | } 54 | r[roff + i - f.size() / 2] = (float)acc; 55 | } 56 | 57 | for (size_t i = alen; i < alen + f.size() / 2; i++) { 58 | double acc = 0; 59 | for (size_t j = 0; j < f.size(); j++) { 60 | if ((aoff + i) >= (alen + j) || (aoff + i) < j) 61 | acc += a[aoff + alen - 1] * f[j]; 62 | else 63 | acc += a[aoff + i - j] * f[j]; 64 | } 65 | r[roff + i - f.size() / 2] = (float)acc; 66 | } 67 | } 68 | 69 | } // namespace AprilTags 70 | -------------------------------------------------------------------------------- /thirdparty/camera_model/src/apriltag/GrayModel.cc: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include 4 | #include 5 | 6 | #include "camera_model/apriltag/GrayModel.h" 7 | 8 | namespace AprilTags { 9 | 10 | GrayModel::GrayModel() : A(), v(), b(), nobs(0), dirty(false) { 11 | A.setZero(); 12 | v.setZero(); 13 | b.setZero(); 14 | } 15 | 16 | void GrayModel::addObservation(float x, float y, float gray) { 17 | float xy = x * y; 18 | 19 | // update only upper-right elements. A'A is symmetric, 20 | // we'll fill the other elements in later. 21 | A(0, 0) += x * x; 22 | A(0, 1) += x * y; 23 | A(0, 2) += x * xy; 24 | A(0, 3) += x; 25 | A(1, 1) += y * y; 26 | A(1, 2) += y * xy; 27 | A(1, 3) += y; 28 | A(2, 2) += xy * xy; 29 | A(2, 3) += xy; 30 | A(3, 3) += 1; 31 | 32 | b[0] += x * gray; 33 | b[1] += y * gray; 34 | b[2] += xy * gray; 35 | b[3] += gray; 36 | 37 | nobs++; 38 | dirty = true; 39 | } 40 | 41 | float GrayModel::interpolate(float x, float y) { 42 | if (dirty) compute(); 43 | return v[0] * x + v[1] * y + v[2] * x * y + v[3]; 44 | } 45 | 46 | void GrayModel::compute() { 47 | // we really only need 4 linearly independent observations to fit our answer, but we'll be very 48 | // sensitive to noise if we don't have an over-determined system. Thus, require at least 6 49 | // observations (or we'll use a constant model below). 50 | 51 | dirty = false; 52 | if (nobs >= 6) { 53 | // make symmetric 54 | Eigen::Matrix4d Ainv; 55 | for (int i = 0; i < 4; i++) 56 | for (int j = i + 1; j < 4; j++) A(j, i) = A(i, j); 57 | 58 | // try { 59 | // Ainv = A.inverse(); 60 | bool invertible; 61 | double det_unused; 62 | A.computeInverseAndDetWithCheck(Ainv, det_unused, invertible); 63 | if (invertible) { 64 | v = Ainv * b; 65 | return; 66 | } 67 | std::cerr << "AprilTags::GrayModel::compute() has underflow in matrix inverse\n"; 68 | // } 69 | // catch (std::underflow_error&) { 70 | // std::cerr << "AprilTags::GrayModel::compute() has underflow in matrix inverse\n"; 71 | // } 72 | } 73 | 74 | // If we get here, either nobs < 6 or the matrix inverse generated 75 | // an underflow, so use a constant model. 76 | v.setZero(); // need the cast to avoid operator= ambiguity wrt. const-ness 77 | v[3] = b[3] / nobs; 78 | } 79 | 80 | } // namespace AprilTags 81 | -------------------------------------------------------------------------------- /thirdparty/camera_model/src/apriltag/MathUtil.cc: -------------------------------------------------------------------------------- 1 | #include "camera_model/apriltag/MathUtil.h" 2 | 3 | namespace AprilTags { 4 | 5 | // Output operator for std::pair, useful for debugging 6 | std::ostream &operator<<(std::ostream &os, const std::pair &pt) { 7 | os << pt.first << "," << pt.second; 8 | return os; 9 | } 10 | 11 | } // namespace AprilTags 12 | -------------------------------------------------------------------------------- /thirdparty/camera_model/src/apriltag/Segment.cc: -------------------------------------------------------------------------------- 1 | #include "camera_model/apriltag/Segment.h" 2 | #include 3 | 4 | namespace AprilTags { 5 | 6 | const float Segment::minimumLineLength = 4; 7 | 8 | Segment::Segment() : children(), x0(0), y0(0), x1(0), y1(0), theta(0), length(0), segmentId(++idCounter) {} 9 | 10 | float Segment::segmentLength() { return std::sqrt((x1 - x0) * (x1 - x0) + (y1 - y0) * (y1 - y0)); } 11 | 12 | void Segment::printSegment() { 13 | std::cout << "(" << x0 << "," << y0 << "), " 14 | << "(" << x1 << "," << y1 << ")" << std::endl; 15 | } 16 | 17 | int Segment::idCounter = 0; 18 | 19 | } // namespace AprilTags 20 | -------------------------------------------------------------------------------- /thirdparty/camera_model/src/apriltag/UnionFindSimple.cc: -------------------------------------------------------------------------------- 1 | #include "camera_model/apriltag/UnionFindSimple.h" 2 | #include 3 | 4 | namespace AprilTags { 5 | 6 | int UnionFindSimple::getRepresentative(int thisId) { 7 | // terminal case: a node is its own parent 8 | if (data[thisId].id == thisId) return thisId; 9 | 10 | // otherwise, recurse... 11 | int root = getRepresentative(data[thisId].id); 12 | 13 | // short circuit the path 14 | data[thisId].id = root; 15 | 16 | return root; 17 | } 18 | 19 | void UnionFindSimple::printDataVector() const { 20 | for (unsigned int i = 0; i < data.size(); i++) 21 | std::cout << "data[" << i << "]: " 22 | << " id:" << data[i].id << " size:" << data[i].size << std::endl; 23 | } 24 | 25 | int UnionFindSimple::connectNodes(int aId, int bId) { 26 | int aRoot = getRepresentative(aId); 27 | int bRoot = getRepresentative(bId); 28 | 29 | if (aRoot == bRoot) return aRoot; 30 | 31 | int asz = data[aRoot].size; 32 | int bsz = data[bRoot].size; 33 | 34 | if (asz > bsz) { 35 | data[bRoot].id = aRoot; 36 | data[aRoot].size += bsz; 37 | return aRoot; 38 | } else { 39 | data[aRoot].id = bRoot; 40 | data[bRoot].size += asz; 41 | return bRoot; 42 | } 43 | } 44 | 45 | void UnionFindSimple::init() { 46 | for (unsigned int i = 0; i < data.size(); i++) { 47 | // everyone is their own cluster of size 1 48 | data[i].id = i; 49 | data[i].size = 1; 50 | } 51 | } 52 | 53 | } // namespace AprilTags 54 | -------------------------------------------------------------------------------- /thirdparty/camera_model/src/apriltag_frontend/GridCalibrationTargetBase.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | namespace aslam { 5 | namespace cameras { 6 | 7 | GridCalibrationTargetBase::GridCalibrationTargetBase(size_t rows, size_t cols) : _rows(rows), _cols(cols) {} 8 | 9 | /// \brief get all points from the target expressed in the target frame 10 | Eigen::MatrixXd GridCalibrationTargetBase::points() const { return _points; } 11 | 12 | std::vector GridCalibrationTargetBase::points3d() const { 13 | std::vector pts; 14 | for (int i = 0; i < _points.rows(); i++) { 15 | Eigen::Vector3d pt = _points.row(i); 16 | pts.emplace_back(pt.x(), pt.y(), pt.z()); 17 | } 18 | return pts; 19 | } 20 | 21 | void GridCalibrationTargetBase::points(std::vector& pts) const { 22 | pts.clear(); 23 | for (int i = 0; i < _points.rows(); i++) { 24 | Eigen::Vector3d pt = _points.row(i); 25 | pts.emplace_back(pt); 26 | } 27 | } 28 | 29 | /// \brief get a point from the target expressed in the target frame 30 | Eigen::Vector3d GridCalibrationTargetBase::point(size_t i) const { return _points.row(i); } 31 | 32 | /// \brief get the grid coordinates for a point 33 | std::pair GridCalibrationTargetBase::pointToGridCoordinates(size_t i) const { 34 | return std::pair{i % cols(), (int)i / cols()}; 35 | } 36 | 37 | /// \brief get the point index from the grid coordinates 38 | size_t GridCalibrationTargetBase::gridCoordinatesToPoint(size_t r, size_t c) const { return cols() * r + c; } 39 | 40 | /// \brief get a point from the target expressed in the target frame 41 | /// by row and column 42 | Eigen::Vector3d GridCalibrationTargetBase::gridPoint(size_t r, size_t c) const { 43 | return _points.row(gridCoordinatesToPoint(r, c)); 44 | } 45 | 46 | double* GridCalibrationTargetBase::getPointDataPointer(size_t i) { return &_points(i, 0); } 47 | 48 | } // namespace cameras 49 | } // namespace aslam 50 | -------------------------------------------------------------------------------- /thirdparty/camera_model/src/code_utils/cv_utils.cc: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | cv_utils::fisheye::PreProcess::PreProcess(const cv::Size _raw_image_size, const cv::Size _roi_size, 4 | const cv::Point _center, const float _resize_scale) 5 | : is_preprocess(false), is_resize_only(false) { 6 | /* clang-format off */ 7 | if ( _raw_image_size.width >= _roi_size.width 8 | && _raw_image_size.height >= _roi_size.height 9 | && _center.x <= _raw_image_size.width 10 | && _center.y <= _raw_image_size.height 11 | && _roi_size.width != 0 12 | && _roi_size.height != 0 ) 13 | is_preprocess = true; 14 | else 15 | is_preprocess = false; 16 | /* clang-format on */ 17 | 18 | if (is_preprocess // 19 | && _raw_image_size.width == _roi_size.width // 20 | && _raw_image_size.height == _roi_size.height) { 21 | is_resize_only = true; 22 | std::cout << "[#INFO] resize_only." << std::endl; 23 | } 24 | 25 | if (is_preprocess) 26 | resetPreProcess(_roi_size, _center, _resize_scale); 27 | else 28 | std::cout << "[#ERROR] Parameters error." << std::endl; 29 | } 30 | 31 | void cv_utils::fisheye::PreProcess::resetPreProcess(cv::Size _roi_size, cv::Point _center, float _resize_scale) { 32 | if (_resize_scale < 0) 33 | resize_scale = 1.0; 34 | else 35 | resize_scale = _resize_scale; 36 | 37 | roi_row_start = _center.y - _roi_size.height / 2; 38 | roi_row_end = roi_row_start + _roi_size.height; 39 | roi_col_start = _center.x - _roi_size.width / 2; 40 | roi_col_end = roi_col_start + _roi_size.width; 41 | 42 | std::cout << "[#INFO] ROI row: start " << roi_row_start << " ,end " << roi_row_end << std::endl; 43 | std::cout << "[#INFO] ROI col: start " << roi_col_start << " ,end " << roi_col_end << std::endl; 44 | } 45 | 46 | cv::Mat cv_utils::fisheye::PreProcess::do_preprocess(cv::Mat image_input) { 47 | if (is_resize_only) { 48 | cv::Mat image_input_resized; 49 | cv::resize(image_input, image_input_resized, 50 | cv::Size(image_input.cols * resize_scale, // 51 | image_input.rows * resize_scale)); 52 | return image_input_resized; 53 | } else if (is_preprocess) { 54 | // std::cout << " is_preprocess true " << std::endl; 55 | cv::Mat image_input_roi = image_input(cv::Range(roi_row_start, roi_row_end), cv::Range(roi_col_start, roi_col_end)); 56 | 57 | cv::Mat image_input_resized; 58 | cv::resize(image_input_roi, image_input_resized, 59 | cv::Size(image_input_roi.cols * resize_scale, // 60 | image_input_roi.rows * resize_scale)); 61 | return image_input_resized; 62 | } else { 63 | // std::cout << " is_preprocess false " << std::endl; 64 | return image_input; 65 | } 66 | } 67 | -------------------------------------------------------------------------------- /thirdparty/camera_model/src/gpl/EigenQuaternionParameterization.cc: -------------------------------------------------------------------------------- 1 | #include "camera_model/gpl/EigenQuaternionParameterization.h" 2 | 3 | #include 4 | 5 | namespace camera_model { 6 | 7 | bool EigenQuaternionParameterization::Plus(const double* x, const double* delta, double* x_plus_delta) const { 8 | const double norm_delta = sqrt(delta[0] * delta[0] + delta[1] * delta[1] + delta[2] * delta[2]); 9 | if (norm_delta > 0.0) { 10 | const double sin_delta_by_delta = (sin(norm_delta) / norm_delta); 11 | double q_delta[4]; 12 | q_delta[0] = sin_delta_by_delta * delta[0]; 13 | q_delta[1] = sin_delta_by_delta * delta[1]; 14 | q_delta[2] = sin_delta_by_delta * delta[2]; 15 | q_delta[3] = cos(norm_delta); 16 | EigenQuaternionProduct(q_delta, x, x_plus_delta); 17 | } else { 18 | for (int i = 0; i < 4; ++i) { 19 | x_plus_delta[i] = x[i]; 20 | } 21 | } 22 | return true; 23 | } 24 | 25 | bool EigenQuaternionParameterization::ComputeJacobian(const double* x, double* jacobian) const { 26 | jacobian[0] = x[3]; 27 | jacobian[1] = x[2]; 28 | jacobian[2] = -x[1]; // NOLINT 29 | jacobian[3] = -x[2]; 30 | jacobian[4] = x[3]; 31 | jacobian[5] = x[0]; // NOLINT 32 | jacobian[6] = x[1]; 33 | jacobian[7] = -x[0]; 34 | jacobian[8] = x[3]; // NOLINT 35 | jacobian[9] = -x[0]; 36 | jacobian[10] = -x[1]; 37 | jacobian[11] = -x[2]; // NOLINT 38 | return true; 39 | } 40 | 41 | } // namespace camera_model 42 | -------------------------------------------------------------------------------- /thirdparty/camera_model/src/sparse_graph/Transform.cc: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | namespace camera_model { 4 | 5 | Transform::Transform() { 6 | m_q.setIdentity(); 7 | m_t.setZero(); 8 | } 9 | 10 | Transform::Transform(const Eigen::Matrix4d& H) { 11 | m_q = Eigen::Quaterniond(H.block<3, 3>(0, 0)); 12 | m_t = H.block<3, 1>(0, 3); 13 | } 14 | 15 | Eigen::Quaterniond& Transform::rotation() { return m_q; } 16 | 17 | const Eigen::Quaterniond& Transform::rotation() const { return m_q; } 18 | 19 | double* Transform::rotationData() { return m_q.coeffs().data(); } 20 | 21 | const double* Transform::rotationData() const { return m_q.coeffs().data(); } 22 | 23 | Eigen::Vector3d& Transform::translation() { return m_t; } 24 | 25 | const Eigen::Vector3d& Transform::translation() const { return m_t; } 26 | 27 | double* Transform::translationData() { return m_t.data(); } 28 | 29 | const double* Transform::translationData() const { return m_t.data(); } 30 | 31 | Eigen::Matrix4d Transform::toMatrix() const { 32 | Eigen::Matrix4d H; 33 | H.setIdentity(); 34 | H.block<3, 3>(0, 0) = m_q.toRotationMatrix(); 35 | H.block<3, 1>(0, 3) = m_t; 36 | 37 | return H; 38 | } 39 | 40 | } // namespace camera_model 41 | -------------------------------------------------------------------------------- /thirdparty/cv_bridge_rbt/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | project(cv_bridge_rbt) 2 | 3 | set(CMAKE_BUILD_TYPE "Release") 4 | set(CMAKE_CXX_FLAGS "-std=c++11") 5 | set(CMAKE_CXX_FLAGS_RELEASE "-O3") 6 | 7 | find_package(Boost 1.58 REQUIRED 8 | COMPONENTS 9 | iostreams) 10 | message(STATUS "Boost_LIBRARIES: ${Boost_LIBRARIES}") 11 | 12 | # OpenCV 13 | find_package(OpenCV 4.0 QUIET 14 | COMPONENTS 15 | core 16 | imgproc 17 | calib3d 18 | highgui 19 | ) 20 | if (NOT OpenCV_FOUND) 21 | find_package(OpenCV 4.0 PATHS ${OpenCV_LIB_DIR} NO_DEFAULT_PATH REQUIRED 22 | COMPONENTS 23 | core 24 | imgproc 25 | calib3d 26 | highgui 27 | ) 28 | endif () 29 | message(STATUS "Use OpenCV ${OpenCV_VERSION}") 30 | message(STATUS "OpenCV_INCLUDE_DIRS: ${OpenCV_INCLUDE_DIRS}") 31 | message(STATUS "OpenCV_LIBRARIES: ${OpenCV_LIBRARIES}") 32 | 33 | find_package(catkin REQUIRED 34 | COMPONENTS 35 | rosconsole 36 | sensor_msgs) 37 | 38 | # add library 39 | add_library(${PROJECT_NAME} SHARED 40 | src/cv_bridge.cpp 41 | src/rgb_colors.cpp) 42 | 43 | set_target_properties(${PROJECT_NAME} PROPERTIES 44 | LIBRARY_OUTPUT_DIRECTORY ${RDPARTY_SO_PATH} 45 | ) 46 | 47 | target_include_directories(${PROJECT_NAME} PUBLIC 48 | $;${catkin_INCLUDE_DIRS} 49 | ) 50 | 51 | target_link_libraries(${PROJECT_NAME} 52 | PUBLIC 53 | ${Boost_LIBRARIES} 54 | ${catkin_LIBRARIES} 55 | ${OpenCV_LIBS} 56 | ${CERES_LIBRARIES} 57 | ) 58 | 59 | 60 | -------------------------------------------------------------------------------- /thirdparty/cv_bridge_rbt/readme.md: -------------------------------------------------------------------------------- 1 | modified from https://github.com/ros-perception/vision_opencv/tree/noetic/cv_bridge 2 | -------------------------------------------------------------------------------- /thirdparty/gl3w/readme: -------------------------------------------------------------------------------- 1 | generated at https://github.com/skaslev/gl3w/tree/869ff0b0b3420a21ab08205a738a6a563eb36c7c -------------------------------------------------------------------------------- /thirdparty/json/nlohmann/adl_serializer.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | #include 6 | #include 7 | 8 | namespace nlohmann 9 | { 10 | 11 | template 12 | struct adl_serializer 13 | { 14 | /*! 15 | @brief convert a JSON value to any value type 16 | 17 | This function is usually called by the `get()` function of the 18 | @ref basic_json class (either explicit or via conversion operators). 19 | 20 | @param[in] j JSON value to read from 21 | @param[in,out] val value to write to 22 | */ 23 | template 24 | static auto from_json(BasicJsonType&& j, ValueType& val) noexcept( 25 | noexcept(::nlohmann::from_json(std::forward(j), val))) 26 | -> decltype(::nlohmann::from_json(std::forward(j), val), void()) 27 | { 28 | ::nlohmann::from_json(std::forward(j), val); 29 | } 30 | 31 | /*! 32 | @brief convert any value type to a JSON value 33 | 34 | This function is usually called by the constructors of the @ref basic_json 35 | class. 36 | 37 | @param[in,out] j JSON value to write to 38 | @param[in] val value to read from 39 | */ 40 | template 41 | static auto to_json(BasicJsonType& j, ValueType&& val) noexcept( 42 | noexcept(::nlohmann::to_json(j, std::forward(val)))) 43 | -> decltype(::nlohmann::to_json(j, std::forward(val)), void()) 44 | { 45 | ::nlohmann::to_json(j, std::forward(val)); 46 | } 47 | }; 48 | 49 | } // namespace nlohmann 50 | -------------------------------------------------------------------------------- /thirdparty/json/nlohmann/detail/input/position_t.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include // size_t 4 | 5 | namespace nlohmann 6 | { 7 | namespace detail 8 | { 9 | /// struct to capture the start position of the current token 10 | struct position_t 11 | { 12 | /// the total number of characters read 13 | std::size_t chars_read_total = 0; 14 | /// the number of characters read in the current line 15 | std::size_t chars_read_current_line = 0; 16 | /// the number of lines read 17 | std::size_t lines_read = 0; 18 | 19 | /// conversion to size_t to preserve SAX interface 20 | constexpr operator size_t() const 21 | { 22 | return chars_read_total; 23 | } 24 | }; 25 | 26 | } // namespace detail 27 | } // namespace nlohmann 28 | -------------------------------------------------------------------------------- /thirdparty/json/nlohmann/detail/iterators/internal_iterator.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | namespace nlohmann 6 | { 7 | namespace detail 8 | { 9 | /*! 10 | @brief an iterator value 11 | 12 | @note This structure could easily be a union, but MSVC currently does not allow 13 | unions members with complex constructors, see https://github.com/nlohmann/json/pull/105. 14 | */ 15 | template struct internal_iterator 16 | { 17 | /// iterator for JSON objects 18 | typename BasicJsonType::object_t::iterator object_iterator {}; 19 | /// iterator for JSON arrays 20 | typename BasicJsonType::array_t::iterator array_iterator {}; 21 | /// generic iterator for all other types 22 | primitive_iterator_t primitive_iterator {}; 23 | }; 24 | } // namespace detail 25 | } // namespace nlohmann 26 | -------------------------------------------------------------------------------- /thirdparty/json/nlohmann/detail/iterators/iterator_traits.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include // random_access_iterator_tag 4 | 5 | #include 6 | #include 7 | 8 | namespace nlohmann 9 | { 10 | namespace detail 11 | { 12 | template 13 | struct iterator_types {}; 14 | 15 | template 16 | struct iterator_types < 17 | It, 18 | void_t> 20 | { 21 | using difference_type = typename It::difference_type; 22 | using value_type = typename It::value_type; 23 | using pointer = typename It::pointer; 24 | using reference = typename It::reference; 25 | using iterator_category = typename It::iterator_category; 26 | }; 27 | 28 | // This is required as some compilers implement std::iterator_traits in a way that 29 | // doesn't work with SFINAE. See https://github.com/nlohmann/json/issues/1341. 30 | template 31 | struct iterator_traits 32 | { 33 | }; 34 | 35 | template 36 | struct iterator_traits < T, enable_if_t < !std::is_pointer::value >> 37 | : iterator_types 38 | { 39 | }; 40 | 41 | template 42 | struct iterator_traits::value>> 43 | { 44 | using iterator_category = std::random_access_iterator_tag; 45 | using value_type = T; 46 | using difference_type = ptrdiff_t; 47 | using pointer = T*; 48 | using reference = T&; 49 | }; 50 | } // namespace detail 51 | } // namespace nlohmann 52 | -------------------------------------------------------------------------------- /thirdparty/json/nlohmann/detail/iterators/primitive_iterator.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include // ptrdiff_t 4 | #include // numeric_limits 5 | 6 | namespace nlohmann 7 | { 8 | namespace detail 9 | { 10 | /* 11 | @brief an iterator for primitive JSON types 12 | 13 | This class models an iterator for primitive JSON types (boolean, number, 14 | string). It's only purpose is to allow the iterator/const_iterator classes 15 | to "iterate" over primitive values. Internally, the iterator is modeled by 16 | a `difference_type` variable. Value begin_value (`0`) models the begin, 17 | end_value (`1`) models past the end. 18 | */ 19 | class primitive_iterator_t 20 | { 21 | private: 22 | using difference_type = std::ptrdiff_t; 23 | static constexpr difference_type begin_value = 0; 24 | static constexpr difference_type end_value = begin_value + 1; 25 | 26 | /// iterator as signed integer type 27 | difference_type m_it = (std::numeric_limits::min)(); 28 | 29 | public: 30 | constexpr difference_type get_value() const noexcept 31 | { 32 | return m_it; 33 | } 34 | 35 | /// set iterator to a defined beginning 36 | void set_begin() noexcept 37 | { 38 | m_it = begin_value; 39 | } 40 | 41 | /// set iterator to a defined past the end 42 | void set_end() noexcept 43 | { 44 | m_it = end_value; 45 | } 46 | 47 | /// return whether the iterator can be dereferenced 48 | constexpr bool is_begin() const noexcept 49 | { 50 | return m_it == begin_value; 51 | } 52 | 53 | /// return whether the iterator is at end 54 | constexpr bool is_end() const noexcept 55 | { 56 | return m_it == end_value; 57 | } 58 | 59 | friend constexpr bool operator==(primitive_iterator_t lhs, primitive_iterator_t rhs) noexcept 60 | { 61 | return lhs.m_it == rhs.m_it; 62 | } 63 | 64 | friend constexpr bool operator<(primitive_iterator_t lhs, primitive_iterator_t rhs) noexcept 65 | { 66 | return lhs.m_it < rhs.m_it; 67 | } 68 | 69 | primitive_iterator_t operator+(difference_type n) noexcept 70 | { 71 | auto result = *this; 72 | result += n; 73 | return result; 74 | } 75 | 76 | friend constexpr difference_type operator-(primitive_iterator_t lhs, primitive_iterator_t rhs) noexcept 77 | { 78 | return lhs.m_it - rhs.m_it; 79 | } 80 | 81 | primitive_iterator_t& operator++() noexcept 82 | { 83 | ++m_it; 84 | return *this; 85 | } 86 | 87 | primitive_iterator_t const operator++(int) noexcept 88 | { 89 | auto result = *this; 90 | ++m_it; 91 | return result; 92 | } 93 | 94 | primitive_iterator_t& operator--() noexcept 95 | { 96 | --m_it; 97 | return *this; 98 | } 99 | 100 | primitive_iterator_t const operator--(int) noexcept 101 | { 102 | auto result = *this; 103 | --m_it; 104 | return result; 105 | } 106 | 107 | primitive_iterator_t& operator+=(difference_type n) noexcept 108 | { 109 | m_it += n; 110 | return *this; 111 | } 112 | 113 | primitive_iterator_t& operator-=(difference_type n) noexcept 114 | { 115 | m_it -= n; 116 | return *this; 117 | } 118 | }; 119 | } // namespace detail 120 | } // namespace nlohmann 121 | -------------------------------------------------------------------------------- /thirdparty/json/nlohmann/detail/json_ref.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | 6 | #include 7 | 8 | namespace nlohmann 9 | { 10 | namespace detail 11 | { 12 | template 13 | class json_ref 14 | { 15 | public: 16 | using value_type = BasicJsonType; 17 | 18 | json_ref(value_type&& value) 19 | : owned_value(std::move(value)) 20 | , value_ref(&owned_value) 21 | , is_rvalue(true) 22 | {} 23 | 24 | json_ref(const value_type& value) 25 | : value_ref(const_cast(&value)) 26 | , is_rvalue(false) 27 | {} 28 | 29 | json_ref(std::initializer_list init) 30 | : owned_value(init) 31 | , value_ref(&owned_value) 32 | , is_rvalue(true) 33 | {} 34 | 35 | template < 36 | class... Args, 37 | enable_if_t::value, int> = 0 > 38 | json_ref(Args && ... args) 39 | : owned_value(std::forward(args)...) 40 | , value_ref(&owned_value) 41 | , is_rvalue(true) 42 | {} 43 | 44 | // class should be movable only 45 | json_ref(json_ref&&) = default; 46 | json_ref(const json_ref&) = delete; 47 | json_ref& operator=(const json_ref&) = delete; 48 | json_ref& operator=(json_ref&&) = delete; 49 | ~json_ref() = default; 50 | 51 | value_type moved_or_copied() const 52 | { 53 | if (is_rvalue) 54 | { 55 | return std::move(*value_ref); 56 | } 57 | return *value_ref; 58 | } 59 | 60 | value_type const& operator*() const 61 | { 62 | return *static_cast(value_ref); 63 | } 64 | 65 | value_type const* operator->() const 66 | { 67 | return static_cast(value_ref); 68 | } 69 | 70 | private: 71 | mutable value_type owned_value = nullptr; 72 | value_type* value_ref = nullptr; 73 | const bool is_rvalue = true; 74 | }; 75 | } // namespace detail 76 | } // namespace nlohmann 77 | -------------------------------------------------------------------------------- /thirdparty/json/nlohmann/detail/macro_unscope.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | // restore GCC/clang diagnostic settings 4 | #if defined(__clang__) || defined(__GNUC__) || defined(__GNUG__) 5 | #pragma GCC diagnostic pop 6 | #endif 7 | #if defined(__clang__) 8 | #pragma GCC diagnostic pop 9 | #endif 10 | 11 | // clean up 12 | #undef JSON_ASSERT 13 | #undef JSON_INTERNAL_CATCH 14 | #undef JSON_CATCH 15 | #undef JSON_THROW 16 | #undef JSON_TRY 17 | #undef JSON_HAS_CPP_14 18 | #undef JSON_HAS_CPP_17 19 | #undef NLOHMANN_BASIC_JSON_TPL_DECLARATION 20 | #undef NLOHMANN_BASIC_JSON_TPL 21 | #undef JSON_EXPLICIT 22 | 23 | #include 24 | -------------------------------------------------------------------------------- /thirdparty/json/nlohmann/detail/meta/cpp_future.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include // size_t 4 | #include // conditional, enable_if, false_type, integral_constant, is_constructible, is_integral, is_same, remove_cv, remove_reference, true_type 5 | 6 | namespace nlohmann 7 | { 8 | namespace detail 9 | { 10 | // alias templates to reduce boilerplate 11 | template 12 | using enable_if_t = typename std::enable_if::type; 13 | 14 | template 15 | using uncvref_t = typename std::remove_cv::type>::type; 16 | 17 | // implementation of C++14 index_sequence and affiliates 18 | // source: https://stackoverflow.com/a/32223343 19 | template 20 | struct index_sequence 21 | { 22 | using type = index_sequence; 23 | using value_type = std::size_t; 24 | static constexpr std::size_t size() noexcept 25 | { 26 | return sizeof...(Ints); 27 | } 28 | }; 29 | 30 | template 31 | struct merge_and_renumber; 32 | 33 | template 34 | struct merge_and_renumber, index_sequence> 35 | : index_sequence < I1..., (sizeof...(I1) + I2)... > {}; 36 | 37 | template 38 | struct make_index_sequence 39 | : merge_and_renumber < typename make_index_sequence < N / 2 >::type, 40 | typename make_index_sequence < N - N / 2 >::type > {}; 41 | 42 | template<> struct make_index_sequence<0> : index_sequence<> {}; 43 | template<> struct make_index_sequence<1> : index_sequence<0> {}; 44 | 45 | template 46 | using index_sequence_for = make_index_sequence; 47 | 48 | // dispatch utility (taken from ranges-v3) 49 | template struct priority_tag : priority_tag < N - 1 > {}; 50 | template<> struct priority_tag<0> {}; 51 | 52 | // taken from ranges-v3 53 | template 54 | struct static_const 55 | { 56 | static constexpr T value{}; 57 | }; 58 | 59 | template 60 | constexpr T static_const::value; 61 | } // namespace detail 62 | } // namespace nlohmann 63 | -------------------------------------------------------------------------------- /thirdparty/json/nlohmann/detail/meta/detected.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | #include 6 | 7 | // https://en.cppreference.com/w/cpp/experimental/is_detected 8 | namespace nlohmann 9 | { 10 | namespace detail 11 | { 12 | struct nonesuch 13 | { 14 | nonesuch() = delete; 15 | ~nonesuch() = delete; 16 | nonesuch(nonesuch const&) = delete; 17 | nonesuch(nonesuch const&&) = delete; 18 | void operator=(nonesuch const&) = delete; 19 | void operator=(nonesuch&&) = delete; 20 | }; 21 | 22 | template class Op, 25 | class... Args> 26 | struct detector 27 | { 28 | using value_t = std::false_type; 29 | using type = Default; 30 | }; 31 | 32 | template class Op, class... Args> 33 | struct detector>, Op, Args...> 34 | { 35 | using value_t = std::true_type; 36 | using type = Op; 37 | }; 38 | 39 | template class Op, class... Args> 40 | using is_detected = typename detector::value_t; 41 | 42 | template class Op, class... Args> 43 | using detected_t = typename detector::type; 44 | 45 | template class Op, class... Args> 46 | using detected_or = detector; 47 | 48 | template class Op, class... Args> 49 | using detected_or_t = typename detected_or::type; 50 | 51 | template class Op, class... Args> 52 | using is_detected_exact = std::is_same>; 53 | 54 | template class Op, class... Args> 55 | using is_detected_convertible = 56 | std::is_convertible, To>; 57 | } // namespace detail 58 | } // namespace nlohmann 59 | -------------------------------------------------------------------------------- /thirdparty/json/nlohmann/detail/meta/void_t.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | namespace nlohmann 4 | { 5 | namespace detail 6 | { 7 | template struct make_void 8 | { 9 | using type = void; 10 | }; 11 | template using void_t = typename make_void::type; 12 | } // namespace detail 13 | } // namespace nlohmann 14 | -------------------------------------------------------------------------------- /thirdparty/json/nlohmann/detail/value_t.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include // array 4 | #include // size_t 5 | #include // uint8_t 6 | #include // string 7 | 8 | namespace nlohmann 9 | { 10 | namespace detail 11 | { 12 | /////////////////////////// 13 | // JSON type enumeration // 14 | /////////////////////////// 15 | 16 | /*! 17 | @brief the JSON type enumeration 18 | 19 | This enumeration collects the different JSON types. It is internally used to 20 | distinguish the stored values, and the functions @ref basic_json::is_null(), 21 | @ref basic_json::is_object(), @ref basic_json::is_array(), 22 | @ref basic_json::is_string(), @ref basic_json::is_boolean(), 23 | @ref basic_json::is_number() (with @ref basic_json::is_number_integer(), 24 | @ref basic_json::is_number_unsigned(), and @ref basic_json::is_number_float()), 25 | @ref basic_json::is_discarded(), @ref basic_json::is_primitive(), and 26 | @ref basic_json::is_structured() rely on it. 27 | 28 | @note There are three enumeration entries (number_integer, number_unsigned, and 29 | number_float), because the library distinguishes these three types for numbers: 30 | @ref basic_json::number_unsigned_t is used for unsigned integers, 31 | @ref basic_json::number_integer_t is used for signed integers, and 32 | @ref basic_json::number_float_t is used for floating-point numbers or to 33 | approximate integers which do not fit in the limits of their respective type. 34 | 35 | @sa @ref basic_json::basic_json(const value_t value_type) -- create a JSON 36 | value with the default value for a given type 37 | 38 | @since version 1.0.0 39 | */ 40 | enum class value_t : std::uint8_t 41 | { 42 | null, ///< null value 43 | object, ///< object (unordered set of name/value pairs) 44 | array, ///< array (ordered collection of values) 45 | string, ///< string value 46 | boolean, ///< boolean value 47 | number_integer, ///< number value (signed integer) 48 | number_unsigned, ///< number value (unsigned integer) 49 | number_float, ///< number value (floating-point) 50 | binary, ///< binary array (ordered collection of bytes) 51 | discarded ///< discarded by the parser callback function 52 | }; 53 | 54 | /*! 55 | @brief comparison operator for JSON types 56 | 57 | Returns an ordering that is similar to Python: 58 | - order: null < boolean < number < object < array < string < binary 59 | - furthermore, each type is not smaller than itself 60 | - discarded values are not comparable 61 | - binary is represented as a b"" string in python and directly comparable to a 62 | string; however, making a binary array directly comparable with a string would 63 | be surprising behavior in a JSON file. 64 | 65 | @since version 1.0.0 66 | */ 67 | inline bool operator<(const value_t lhs, const value_t rhs) noexcept 68 | { 69 | static constexpr std::array order = {{ 70 | 0 /* null */, 3 /* object */, 4 /* array */, 5 /* string */, 71 | 1 /* boolean */, 2 /* integer */, 2 /* unsigned */, 2 /* float */, 72 | 6 /* binary */ 73 | } 74 | }; 75 | 76 | const auto l_index = static_cast(lhs); 77 | const auto r_index = static_cast(rhs); 78 | return l_index < order.size() && r_index < order.size() && order[l_index] < order[r_index]; 79 | } 80 | } // namespace detail 81 | } // namespace nlohmann 82 | -------------------------------------------------------------------------------- /thirdparty/json/nlohmann/json_fwd.hpp: -------------------------------------------------------------------------------- 1 | #ifndef INCLUDE_NLOHMANN_JSON_FWD_HPP_ 2 | #define INCLUDE_NLOHMANN_JSON_FWD_HPP_ 3 | 4 | #include // int64_t, uint64_t 5 | #include // map 6 | #include // allocator 7 | #include // string 8 | #include // vector 9 | 10 | /*! 11 | @brief namespace for Niels Lohmann 12 | @see https://github.com/nlohmann 13 | @since version 1.0.0 14 | */ 15 | namespace nlohmann 16 | { 17 | /*! 18 | @brief default JSONSerializer template argument 19 | 20 | This serializer ignores the template arguments and uses ADL 21 | ([argument-dependent lookup](https://en.cppreference.com/w/cpp/language/adl)) 22 | for serialization. 23 | */ 24 | template 25 | struct adl_serializer; 26 | 27 | template class ObjectType = 28 | std::map, 29 | template class ArrayType = std::vector, 30 | class StringType = std::string, class BooleanType = bool, 31 | class NumberIntegerType = std::int64_t, 32 | class NumberUnsignedType = std::uint64_t, 33 | class NumberFloatType = double, 34 | template class AllocatorType = std::allocator, 35 | template class JSONSerializer = 36 | adl_serializer, 37 | class BinaryType = std::vector> 38 | class basic_json; 39 | 40 | /*! 41 | @brief JSON Pointer 42 | 43 | A JSON pointer defines a string syntax for identifying a specific value 44 | within a JSON document. It can be used with functions `at` and 45 | `operator[]`. Furthermore, JSON pointers are the base for JSON patches. 46 | 47 | @sa [RFC 6901](https://tools.ietf.org/html/rfc6901) 48 | 49 | @since version 2.0.0 50 | */ 51 | template 52 | class json_pointer; 53 | 54 | /*! 55 | @brief default JSON class 56 | 57 | This type is the default specialization of the @ref basic_json class which 58 | uses the standard template types. 59 | 60 | @since version 1.0.0 61 | */ 62 | using json = basic_json<>; 63 | 64 | template 65 | struct ordered_map; 66 | 67 | /*! 68 | @brief ordered JSON class 69 | 70 | This type preserves the insertion order of object keys. 71 | 72 | @since version 3.9.0 73 | */ 74 | using ordered_json = basic_json; 75 | 76 | } // namespace nlohmann 77 | 78 | #endif // INCLUDE_NLOHMANN_JSON_FWD_HPP_ 79 | --------------------------------------------------------------------------------