├── .clang-format ├── .clang-tidy ├── .gitignore ├── .gitlab-ci.yml ├── .gitmodules ├── .pre-commit-config.yaml ├── .style.yapf ├── CMakeLists.txt ├── LICENSE ├── README.md ├── cmake_modules ├── FindEigen3.cmake ├── FindOpenGV.cmake └── FindTBB.cmake ├── data ├── aprilgrid_5x4_uzh.json ├── aprilgrid_6x6.json ├── euroc_config.json ├── euroc_config_no_factors.json ├── euroc_config_no_weights.json ├── euroc_config_vo.json ├── euroc_ds_calib.json ├── euroc_eucm_calib.json ├── iccv21 │ ├── basalt_batch_config.toml │ └── experiments-iccv.toml ├── kitti_config.json ├── tumvi_512_config.json ├── tumvi_512_ds_calib.json └── tumvi_512_eucm_calib.json ├── doc ├── BatchEvaluation.md ├── Calibration.md ├── DevSetup.md ├── Realsense.md ├── Simulation.md ├── VioMapping.md ├── Vo.md └── img │ ├── MH_05_MAPPING.png │ ├── MH_05_OPT_FLOW.png │ ├── MH_05_VIO.png │ ├── SIM_MAPPER.png │ ├── SIM_VIO.png │ ├── euroc_cam_calib.png │ ├── euroc_imu_calib.png │ ├── kalibr_cam_calib.png │ ├── kalibr_imu_calib.png │ ├── kitti.png │ ├── kitti_video.png │ ├── magistrale1_mapping.png │ ├── magistrale1_vio.png │ ├── qt_creator_beautifier_clang_format.png │ ├── qt_creator_beautifier_general.png │ ├── qt_creator_configure_project.png │ ├── qt_creator_plugins.png │ ├── realsense_setup.jpg │ ├── t265_cam_calib.png │ ├── t265_imu_calib.png │ ├── t265_inv_resp_irradiance.png │ ├── t265_record.png │ ├── t265_time_align_error.png │ ├── t265_time_align_gyro.png │ ├── t265_vio.png │ ├── teaser.png │ ├── tumvi_cam_calib.png │ ├── tumvi_imu_calib.png │ ├── uzh_cam_calib.png │ └── uzh_imu_calib.png ├── docker ├── b_image_bionic │ └── Dockerfile ├── b_image_focal │ └── Dockerfile └── b_image_jammy │ └── Dockerfile ├── include └── basalt │ ├── calibration │ ├── aprilgrid.h │ ├── calibration_helper.h │ ├── cam_calib.h │ ├── cam_imu_calib.h │ └── vignette.h │ ├── device │ └── rs_t265.h │ ├── hash_bow │ └── hash_bow.h │ ├── io │ ├── dataset_io.h │ ├── dataset_io_euroc.h │ ├── dataset_io_kitti.h │ ├── dataset_io_rosbag.h │ ├── dataset_io_uzh.h │ └── marg_data_io.h │ ├── linearization │ ├── block_diagonal.hpp │ ├── imu_block.hpp │ ├── landmark_block.hpp │ ├── landmark_block_abs_dynamic.hpp │ ├── linearization_abs_qr.hpp │ ├── linearization_abs_sc.hpp │ ├── linearization_base.hpp │ └── linearization_rel_sc.hpp │ ├── optical_flow │ ├── frame_to_frame_optical_flow.h │ ├── multiscale_frame_to_frame_optical_flow.h │ ├── optical_flow.h │ ├── patch.h │ ├── patch_optical_flow.h │ └── patterns.h │ ├── optimization │ ├── accumulator.h │ ├── linearize.h │ ├── poses_linearize.h │ ├── poses_optimize.h │ ├── spline_linearize.h │ └── spline_optimize.h │ ├── utils │ ├── ba_utils.h │ ├── cast_utils.hpp │ ├── common_types.h │ ├── filesystem.h │ ├── format.hpp │ ├── imu_types.h │ ├── keypoints.h │ ├── nfr.h │ ├── sim_utils.h │ ├── system_utils.h │ ├── test_utils.h │ ├── time_utils.hpp │ ├── tracks.h │ ├── union_find.h │ ├── vio_config.h │ └── vis_utils.h │ └── vi_estimator │ ├── ba_base.h │ ├── landmark_database.h │ ├── marg_helper.h │ ├── nfr_mapper.h │ ├── sc_ba_base.h │ ├── sqrt_ba_base.h │ ├── sqrt_keypoint_vio.h │ ├── sqrt_keypoint_vo.h │ └── vio_estimator.h ├── python └── basalt │ ├── __init__.py │ ├── experiments.py │ ├── generate_tables.py │ ├── latex │ ├── containers.py │ ├── plot.py │ ├── results_table.py │ ├── summarize_sequences_table.py │ ├── templates.py │ └── util.py │ ├── log.py │ ├── metric.py │ ├── nullspace.py │ ├── run.py │ └── util.py ├── scripts ├── basalt_capture_mocap.py ├── basalt_convert_kitti_calib.py ├── basalt_response_calib.py ├── basalt_verify_dataset.py ├── batch │ ├── generate-batch-configs.py │ ├── generate-tables.py │ ├── list-jobs.sh │ ├── plot.py │ ├── query-config.py │ ├── rerun-failed-in.sh │ ├── rerun-one-in.sh │ ├── run-all-in.sh │ └── run-one.sh ├── clang-format-all.sh ├── compare_calib.py ├── eval_full │ ├── gen_results.py │ ├── gen_results_kitti.py │ ├── gen_results_tumvi.py │ ├── run_evaluations.sh │ ├── run_evaluations_kitti.sh │ └── run_evaluations_tumvi.sh ├── install_deps.sh ├── install_mac_os_deps.sh ├── install_ubuntu_deps.sh ├── templates │ └── license-py-sh.tmpl ├── update-license-headers.sh ├── update_submodules.sh └── yapf-all.sh ├── src ├── calibrate.cpp ├── calibrate_imu.cpp ├── calibration │ ├── aprilgrid.cpp │ ├── calibraiton_helper.cpp │ ├── cam_calib.cpp │ ├── cam_imu_calib.cpp │ └── vignette.cpp ├── device │ └── rs_t265.cpp ├── io │ ├── dataset_io.cpp │ └── marg_data_io.cpp ├── kitti_eval.cpp ├── linearization │ ├── landmark_block.cpp │ ├── linearization_abs_qr.cpp │ ├── linearization_abs_sc.cpp │ ├── linearization_base.cpp │ └── linearization_rel_sc.cpp ├── mapper.cpp ├── mapper_sim.cpp ├── mapper_sim_naive.cpp ├── opt_flow.cpp ├── optical_flow │ └── optical_flow.cpp ├── rs_t265_record.cpp ├── rs_t265_vio.cpp ├── time_alignment.cpp ├── utils │ ├── keypoints.cpp │ ├── system_utils.cpp │ ├── time_utils.cpp │ └── vio_config.cpp ├── vi_estimator │ ├── ba_base.cpp │ ├── landmark_database.cpp │ ├── marg_helper.cpp │ ├── nfr_mapper.cpp │ ├── sc_ba_base.cpp │ ├── sqrt_ba_base.cpp │ ├── sqrt_keypoint_vio.cpp │ ├── sqrt_keypoint_vo.cpp │ └── vio_estimator.cpp ├── vio.cpp └── vio_sim.cpp ├── test ├── CMakeLists.txt └── src │ ├── test_linearization.cpp │ ├── test_nfr.cpp │ ├── test_patch.cpp │ ├── test_qr.cpp │ ├── test_spline_opt.cpp │ └── test_vio.cpp └── thirdparty ├── CMakeLists.txt ├── apriltag ├── CMakeLists.txt ├── ethz_apriltag2 │ ├── CMakeLists.txt │ ├── LICENSE │ ├── include │ │ └── apriltags │ │ │ ├── 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 │ ├── package.xml │ └── src │ │ ├── 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 │ │ └── example │ │ ├── CMakeLists.txt │ │ ├── Serial.cpp │ │ ├── Serial.h │ │ ├── apriltags_demo.cpp │ │ ├── arduino_tags │ │ └── arduino_tags.ino │ │ └── imu.cpp ├── include │ └── basalt │ │ └── utils │ │ └── apriltag.h └── src │ └── apriltag.cpp ├── json └── nlohmann │ ├── json.hpp │ └── json_fwd.hpp └── ros ├── CMakeLists.txt └── include ├── console_bridge_export.h ├── geometry_msgs ├── Accel.h ├── AccelStamped.h ├── AccelWithCovariance.h ├── AccelWithCovarianceStamped.h ├── Inertia.h ├── InertiaStamped.h ├── Point.h ├── Point32.h ├── PointStamped.h ├── Polygon.h ├── PolygonStamped.h ├── Pose.h ├── Pose2D.h ├── PoseArray.h ├── PoseStamped.h ├── PoseWithCovariance.h ├── PoseWithCovarianceStamped.h ├── Quaternion.h ├── QuaternionStamped.h ├── Transform.h ├── TransformStamped.h ├── Twist.h ├── TwistStamped.h ├── TwistWithCovariance.h ├── TwistWithCovarianceStamped.h ├── Vector3.h ├── Vector3Stamped.h ├── Wrench.h └── WrenchStamped.h ├── sensor_msgs ├── BatteryState.h ├── CameraInfo.h ├── ChannelFloat32.h ├── CompressedImage.h ├── FluidPressure.h ├── Illuminance.h ├── Image.h ├── Imu.h ├── JointState.h ├── Joy.h ├── JoyFeedback.h ├── JoyFeedbackArray.h ├── LaserEcho.h ├── LaserScan.h ├── MagneticField.h ├── MultiDOFJointState.h ├── MultiEchoLaserScan.h ├── NavSatFix.h ├── NavSatStatus.h ├── PointCloud.h ├── PointCloud2.h ├── PointField.h ├── Range.h ├── RegionOfInterest.h ├── RelativeHumidity.h ├── SetCameraInfo.h ├── SetCameraInfoRequest.h ├── SetCameraInfoResponse.h ├── Temperature.h ├── TimeReference.h ├── distortion_models.h ├── fill_image.h ├── image_encodings.h ├── impl │ └── point_cloud2_iterator.h ├── point_cloud2_iterator.h ├── point_cloud_conversion.h └── point_field_conversion.h └── std_msgs ├── Bool.h ├── Byte.h ├── ByteMultiArray.h ├── Char.h ├── ColorRGBA.h ├── Duration.h ├── Empty.h ├── Float32.h ├── Float32MultiArray.h ├── Float64.h ├── Float64MultiArray.h ├── Header.h ├── Int16.h ├── Int16MultiArray.h ├── Int32.h ├── Int32MultiArray.h ├── Int64.h ├── Int64MultiArray.h ├── Int8.h ├── Int8MultiArray.h ├── MultiArrayDimension.h ├── MultiArrayLayout.h ├── String.h ├── Time.h ├── UInt16.h ├── UInt16MultiArray.h ├── UInt32.h ├── UInt32MultiArray.h ├── UInt64.h ├── UInt64MultiArray.h ├── UInt8.h ├── UInt8MultiArray.h ├── builtin_bool.h ├── builtin_double.h ├── builtin_float.h ├── builtin_int16.h ├── builtin_int32.h ├── builtin_int64.h ├── builtin_int8.h ├── builtin_string.h ├── builtin_uint16.h ├── builtin_uint32.h ├── builtin_uint64.h ├── builtin_uint8.h ├── header_deprecated_def.h └── trait_macros.h /.clang-format: -------------------------------------------------------------------------------- 1 | --- 2 | Language: Cpp 3 | BasedOnStyle: Google 4 | IndentWidth: 2 5 | IncludeBlocks: Preserve 6 | ... 7 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | cmake-build* 2 | .idea 3 | CMakeLists.txt.user 4 | build* 5 | scripts/eval/eval_* 6 | scripts/eval_full/eval_* 7 | stats_*.*json 8 | __pycache__ 9 | -------------------------------------------------------------------------------- /.gitmodules: -------------------------------------------------------------------------------- 1 | [submodule "thirdparty/CLI11"] 2 | path = thirdparty/CLI11 3 | url = https://github.com/CLIUtils/CLI11.git 4 | [submodule "thirdparty/opengv"] 5 | path = thirdparty/opengv 6 | url = https://github.com/laurentkneip/opengv.git 7 | [submodule "thirdparty/Pangolin"] 8 | path = thirdparty/Pangolin 9 | url = https://github.com/stevenlovegrove/Pangolin.git 10 | [submodule "thirdparty/ros_comm"] 11 | path = thirdparty/ros/ros_comm 12 | url = https://github.com/ros/ros_comm.git 13 | [submodule "thirdparty/roscpp_core"] 14 | path = thirdparty/ros/roscpp_core 15 | # url = https://github.com/ros/roscpp_core.git 16 | url = https://github.com/NikolausDemmel/roscpp_core.git 17 | [submodule "thirdparty/console_bridge"] 18 | path = thirdparty/ros/console_bridge 19 | url = https://github.com/ros/console_bridge.git 20 | [submodule "thirdparty/basalt-headers"] 21 | path = thirdparty/basalt-headers 22 | url = https://gitlab.com/VladyslavUsenko/basalt-headers.git 23 | [submodule "thirdparty/magic_enum"] 24 | path = thirdparty/magic_enum 25 | url = https://github.com/Neargye/magic_enum.git 26 | -------------------------------------------------------------------------------- /.pre-commit-config.yaml: -------------------------------------------------------------------------------- 1 | # To use: 2 | # 3 | # pre-commit run -a 4 | # 5 | # Or: 6 | # 7 | # pre-commit install # (runs every time you commit in git) 8 | # 9 | # To update this file: 10 | # 11 | # pre-commit autoupdate 12 | # 13 | # See https://github.com/pre-commit/pre-commit 14 | 15 | exclude: thirdparty/ 16 | repos: 17 | # Standard hooks 18 | - repo: https://github.com/pre-commit/pre-commit-hooks 19 | rev: "v4.2.0" 20 | hooks: 21 | - id: check-added-large-files 22 | - id: check-case-conflict 23 | - id: check-merge-conflict 24 | - id: check-symlinks 25 | - id: check-toml 26 | - id: check-json 27 | - id: check-yaml 28 | exclude: ".gitlab-ci.yml" 29 | - id: debug-statements 30 | - id: end-of-file-fixer 31 | - id: mixed-line-ending 32 | - id: requirements-txt-fixer 33 | - id: trailing-whitespace 34 | - id: check-ast 35 | 36 | # Changes tabs to spaces 37 | - repo: https://github.com/Lucas-C/pre-commit-hooks 38 | rev: "v1.1.13" 39 | hooks: 40 | - id: remove-tabs 41 | 42 | # Clang format the codebase automatically 43 | - repo: https://github.com/pre-commit/mirrors-clang-format 44 | rev: "v14.0.3" 45 | hooks: 46 | - id: clang-format 47 | exclude: "^.*thirdparty/.*$" 48 | args: ["-style=file"] 49 | types_or: [c++, c, cuda] 50 | -------------------------------------------------------------------------------- /.style.yapf: -------------------------------------------------------------------------------- 1 | [style] 2 | based_on_style = google 3 | column_limit = 120 4 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | BSD 3-Clause License 2 | 3 | Copyright (c) 2019, Vladyslav Usenko and Nikolaus Demmel. 4 | All rights reserved. 5 | 6 | Redistribution and use in source and binary forms, with or without 7 | modification, are permitted provided that the following conditions are met: 8 | 9 | * Redistributions of source code must retain the above copyright notice, this 10 | list of conditions and the following disclaimer. 11 | 12 | * Redistributions in binary form must reproduce the above copyright notice, 13 | this list of conditions and the following disclaimer in the documentation 14 | and/or other materials provided with the distribution. 15 | 16 | * Neither the name of the copyright holder nor the names of its 17 | contributors may be used to endorse or promote products derived from 18 | this software without specific prior written permission. 19 | 20 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 21 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 22 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 23 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 24 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 25 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 26 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 27 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 28 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 29 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 30 | -------------------------------------------------------------------------------- /cmake_modules/FindEigen3.cmake: -------------------------------------------------------------------------------- 1 | # - Try to find Eigen3 lib 2 | # 3 | # This module supports requiring a minimum version, e.g. you can do 4 | # find_package(Eigen3 3.1.2) 5 | # to require version 3.1.2 or newer of Eigen3. 6 | # 7 | # Once done this will define 8 | # 9 | # EIGEN3_FOUND - system has eigen lib with correct version 10 | # EIGEN3_INCLUDE_DIR - the eigen include directory 11 | # EIGEN3_VERSION - eigen version 12 | 13 | # Copyright (c) 2006, 2007 Montel Laurent, 14 | # Copyright (c) 2008, 2009 Gael Guennebaud, 15 | # Copyright (c) 2009 Benoit Jacob 16 | # Redistribution and use is allowed according to the terms of the 2-clause BSD license. 17 | 18 | # Adaptations (c) Nikolaus Demmel 2019 19 | # - pass NO_DEFAULT_PATH --> only works when passing exact HINTS 20 | 21 | 22 | if(NOT Eigen3_FIND_VERSION) 23 | if(NOT Eigen3_FIND_VERSION_MAJOR) 24 | set(Eigen3_FIND_VERSION_MAJOR 2) 25 | endif(NOT Eigen3_FIND_VERSION_MAJOR) 26 | if(NOT Eigen3_FIND_VERSION_MINOR) 27 | set(Eigen3_FIND_VERSION_MINOR 91) 28 | endif(NOT Eigen3_FIND_VERSION_MINOR) 29 | if(NOT Eigen3_FIND_VERSION_PATCH) 30 | set(Eigen3_FIND_VERSION_PATCH 0) 31 | endif(NOT Eigen3_FIND_VERSION_PATCH) 32 | 33 | set(Eigen3_FIND_VERSION "${Eigen3_FIND_VERSION_MAJOR}.${Eigen3_FIND_VERSION_MINOR}.${Eigen3_FIND_VERSION_PATCH}") 34 | endif(NOT Eigen3_FIND_VERSION) 35 | 36 | macro(_eigen3_check_version) 37 | file(READ "${EIGEN3_INCLUDE_DIR}/Eigen/src/Core/util/Macros.h" _eigen3_version_header) 38 | 39 | string(REGEX MATCH "define[ \t]+EIGEN_WORLD_VERSION[ \t]+([0-9]+)" _eigen3_world_version_match "${_eigen3_version_header}") 40 | set(EIGEN3_WORLD_VERSION "${CMAKE_MATCH_1}") 41 | string(REGEX MATCH "define[ \t]+EIGEN_MAJOR_VERSION[ \t]+([0-9]+)" _eigen3_major_version_match "${_eigen3_version_header}") 42 | set(EIGEN3_MAJOR_VERSION "${CMAKE_MATCH_1}") 43 | string(REGEX MATCH "define[ \t]+EIGEN_MINOR_VERSION[ \t]+([0-9]+)" _eigen3_minor_version_match "${_eigen3_version_header}") 44 | set(EIGEN3_MINOR_VERSION "${CMAKE_MATCH_1}") 45 | 46 | set(EIGEN3_VERSION ${EIGEN3_WORLD_VERSION}.${EIGEN3_MAJOR_VERSION}.${EIGEN3_MINOR_VERSION}) 47 | if(${EIGEN3_VERSION} VERSION_LESS ${Eigen3_FIND_VERSION}) 48 | set(EIGEN3_VERSION_OK FALSE) 49 | else(${EIGEN3_VERSION} VERSION_LESS ${Eigen3_FIND_VERSION}) 50 | set(EIGEN3_VERSION_OK TRUE) 51 | endif(${EIGEN3_VERSION} VERSION_LESS ${Eigen3_FIND_VERSION}) 52 | 53 | if(NOT EIGEN3_VERSION_OK) 54 | 55 | message(STATUS "Eigen3 version ${EIGEN3_VERSION} found in ${EIGEN3_INCLUDE_DIR}, " 56 | "but at least version ${Eigen3_FIND_VERSION} is required") 57 | endif(NOT EIGEN3_VERSION_OK) 58 | endmacro(_eigen3_check_version) 59 | 60 | if (EIGEN3_INCLUDE_DIR) 61 | 62 | # in cache already 63 | _eigen3_check_version() 64 | set(EIGEN3_FOUND ${EIGEN3_VERSION_OK}) 65 | 66 | else (EIGEN3_INCLUDE_DIR) 67 | 68 | find_path(EIGEN3_INCLUDE_DIR NAMES signature_of_eigen3_matrix_library 69 | HINTS ${EIGEN_INCLUDE_DIR_HINTS} 70 | NO_DEFAULT_PATH 71 | ) 72 | 73 | if(EIGEN3_INCLUDE_DIR) 74 | _eigen3_check_version() 75 | endif(EIGEN3_INCLUDE_DIR) 76 | 77 | include(FindPackageHandleStandardArgs) 78 | find_package_handle_standard_args(Eigen3 DEFAULT_MSG EIGEN3_INCLUDE_DIR EIGEN3_VERSION_OK) 79 | 80 | mark_as_advanced(EIGEN3_INCLUDE_DIR) 81 | 82 | endif(EIGEN3_INCLUDE_DIR) 83 | -------------------------------------------------------------------------------- /data/aprilgrid_5x4_uzh.json: -------------------------------------------------------------------------------- 1 | { 2 | "tagCols": 5, 3 | "tagRows": 4, 4 | "tagSize": 0.075, 5 | "tagSpacing": 0.2 6 | } 7 | -------------------------------------------------------------------------------- /data/aprilgrid_6x6.json: -------------------------------------------------------------------------------- 1 | { 2 | "tagCols": 6, 3 | "tagRows": 6, 4 | "tagSize": 0.088, 5 | "tagSpacing": 0.3 6 | } 7 | -------------------------------------------------------------------------------- /data/euroc_config.json: -------------------------------------------------------------------------------- 1 | { 2 | "value0": { 3 | "config.optical_flow_type": "frame_to_frame", 4 | "config.optical_flow_detection_grid_size": 50, 5 | "config.optical_flow_max_recovered_dist2": 0.04, 6 | "config.optical_flow_pattern": 51, 7 | "config.optical_flow_max_iterations": 5, 8 | "config.optical_flow_epipolar_error": 0.005, 9 | "config.optical_flow_levels": 3, 10 | "config.optical_flow_skip_frames": 1, 11 | "config.vio_linearization_type": "ABS_QR", 12 | "config.vio_sqrt_marg": true, 13 | "config.vio_max_states": 3, 14 | "config.vio_max_kfs": 7, 15 | "config.vio_min_frames_after_kf": 5, 16 | "config.vio_new_kf_keypoints_thresh": 0.7, 17 | "config.vio_debug": false, 18 | "config.vio_extended_logging": false, 19 | "config.vio_obs_std_dev": 0.5, 20 | "config.vio_obs_huber_thresh": 1.0, 21 | "config.vio_min_triangulation_dist": 0.05, 22 | "config.vio_outlier_threshold": 3.0, 23 | "config.vio_filter_iteration": 4, 24 | "config.vio_max_iterations": 7, 25 | "config.vio_enforce_realtime": false, 26 | "config.vio_use_lm": true, 27 | "config.vio_lm_lambda_initial": 1e-4, 28 | "config.vio_lm_lambda_min": 1e-6, 29 | "config.vio_lm_lambda_max": 1e2, 30 | "config.vio_lm_landmark_damping_variant": 1, 31 | "config.vio_lm_pose_damping_variant": 1, 32 | "config.vio_scale_jacobian": false, 33 | "config.vio_init_pose_weight": 1e8, 34 | "config.vio_init_ba_weight": 1e1, 35 | "config.vio_init_bg_weight": 1e2, 36 | "config.vio_marg_lost_landmarks": true, 37 | "config.vio_kf_marg_feature_ratio": 0.1, 38 | "config.mapper_obs_std_dev": 0.25, 39 | "config.mapper_obs_huber_thresh": 1.5, 40 | "config.mapper_detection_num_points": 800, 41 | "config.mapper_num_frames_to_match": 30, 42 | "config.mapper_frames_to_match_threshold": 0.04, 43 | "config.mapper_min_matches": 20, 44 | "config.mapper_ransac_threshold": 5e-5, 45 | "config.mapper_min_track_length": 5, 46 | "config.mapper_max_hamming_distance": 70, 47 | "config.mapper_second_best_test_ratio": 1.2, 48 | "config.mapper_bow_num_bits": 16, 49 | "config.mapper_min_triangulation_dist": 0.07, 50 | "config.mapper_no_factor_weights": false, 51 | "config.mapper_use_factors": true, 52 | "config.mapper_use_lm": true, 53 | "config.mapper_lm_lambda_min": 1e-32, 54 | "config.mapper_lm_lambda_max": 1e3 55 | } 56 | } 57 | -------------------------------------------------------------------------------- /data/euroc_config_no_factors.json: -------------------------------------------------------------------------------- 1 | { 2 | "value0": { 3 | "config.optical_flow_type": "frame_to_frame", 4 | "config.optical_flow_detection_grid_size": 50, 5 | "config.optical_flow_max_recovered_dist2": 0.04, 6 | "config.optical_flow_pattern": 51, 7 | "config.optical_flow_max_iterations": 5, 8 | "config.optical_flow_epipolar_error": 0.005, 9 | "config.optical_flow_levels": 3, 10 | "config.optical_flow_skip_frames": 1, 11 | "config.vio_linearization_type": "ABS_QR", 12 | "config.vio_sqrt_marg": true, 13 | "config.vio_max_states": 3, 14 | "config.vio_max_kfs": 7, 15 | "config.vio_min_frames_after_kf": 5, 16 | "config.vio_new_kf_keypoints_thresh": 0.7, 17 | "config.vio_debug": false, 18 | "config.vio_extended_logging": false, 19 | "config.vio_obs_std_dev": 0.5, 20 | "config.vio_obs_huber_thresh": 1.0, 21 | "config.vio_min_triangulation_dist": 0.05, 22 | "config.vio_outlier_threshold": 3.0, 23 | "config.vio_filter_iteration": 4, 24 | "config.vio_max_iterations": 7, 25 | "config.vio_enforce_realtime": false, 26 | "config.vio_use_lm": false, 27 | "config.vio_lm_lambda_initial": 1e-8, 28 | "config.vio_lm_lambda_min": 1e-32, 29 | "config.vio_lm_lambda_max": 1e2, 30 | "config.vio_lm_landmark_damping_variant": 1, 31 | "config.vio_lm_pose_damping_variant": 1, 32 | "config.vio_scale_jacobian": false, 33 | "config.vio_init_pose_weight": 1e8, 34 | "config.vio_init_ba_weight": 1e1, 35 | "config.vio_init_bg_weight": 1e2, 36 | "config.vio_marg_lost_landmarks": true, 37 | "config.vio_kf_marg_feature_ratio": 0.1, 38 | "config.mapper_obs_std_dev": 0.25, 39 | "config.mapper_obs_huber_thresh": 1.5, 40 | "config.mapper_detection_num_points": 800, 41 | "config.mapper_num_frames_to_match": 30, 42 | "config.mapper_frames_to_match_threshold": 0.04, 43 | "config.mapper_min_matches": 20, 44 | "config.mapper_ransac_threshold": 5e-5, 45 | "config.mapper_min_track_length": 5, 46 | "config.mapper_max_hamming_distance": 70, 47 | "config.mapper_second_best_test_ratio": 1.2, 48 | "config.mapper_bow_num_bits": 16, 49 | "config.mapper_min_triangulation_dist": 0.07, 50 | "config.mapper_no_factor_weights": false, 51 | "config.mapper_use_factors": false, 52 | "config.mapper_use_lm": true, 53 | "config.mapper_lm_lambda_min": 1e-32, 54 | "config.mapper_lm_lambda_max": 1e3 55 | } 56 | } 57 | -------------------------------------------------------------------------------- /data/euroc_config_no_weights.json: -------------------------------------------------------------------------------- 1 | { 2 | "value0": { 3 | "config.optical_flow_type": "frame_to_frame", 4 | "config.optical_flow_detection_grid_size": 50, 5 | "config.optical_flow_max_recovered_dist2": 0.04, 6 | "config.optical_flow_pattern": 51, 7 | "config.optical_flow_max_iterations": 5, 8 | "config.optical_flow_epipolar_error": 0.005, 9 | "config.optical_flow_levels": 3, 10 | "config.optical_flow_skip_frames": 1, 11 | "config.vio_linearization_type": "ABS_QR", 12 | "config.vio_sqrt_marg": true, 13 | "config.vio_max_states": 3, 14 | "config.vio_max_kfs": 7, 15 | "config.vio_min_frames_after_kf": 5, 16 | "config.vio_new_kf_keypoints_thresh": 0.7, 17 | "config.vio_debug": false, 18 | "config.vio_extended_logging": false, 19 | "config.vio_obs_std_dev": 0.5, 20 | "config.vio_obs_huber_thresh": 1.0, 21 | "config.vio_min_triangulation_dist": 0.05, 22 | "config.vio_outlier_threshold": 3.0, 23 | "config.vio_filter_iteration": 4, 24 | "config.vio_max_iterations": 7, 25 | "config.vio_enforce_realtime": false, 26 | "config.vio_use_lm": false, 27 | "config.vio_lm_lambda_initial": 1e-8, 28 | "config.vio_lm_lambda_min": 1e-32, 29 | "config.vio_lm_lambda_max": 1e2, 30 | "config.vio_lm_landmark_damping_variant": 1, 31 | "config.vio_lm_pose_damping_variant": 1, 32 | "config.vio_scale_jacobian": false, 33 | "config.vio_init_pose_weight": 1e8, 34 | "config.vio_init_ba_weight": 1e1, 35 | "config.vio_init_bg_weight": 1e2, 36 | "config.vio_marg_lost_landmarks": true, 37 | "config.vio_kf_marg_feature_ratio": 0.1, 38 | "config.mapper_obs_std_dev": 0.25, 39 | "config.mapper_obs_huber_thresh": 1.5, 40 | "config.mapper_detection_num_points": 800, 41 | "config.mapper_num_frames_to_match": 30, 42 | "config.mapper_frames_to_match_threshold": 0.04, 43 | "config.mapper_min_matches": 20, 44 | "config.mapper_ransac_threshold": 5e-5, 45 | "config.mapper_min_track_length": 5, 46 | "config.mapper_max_hamming_distance": 70, 47 | "config.mapper_second_best_test_ratio": 1.2, 48 | "config.mapper_bow_num_bits": 16, 49 | "config.mapper_min_triangulation_dist": 0.07, 50 | "config.mapper_no_factor_weights": true, 51 | "config.mapper_use_factors": true, 52 | "config.mapper_use_lm": true, 53 | "config.mapper_lm_lambda_min": 1e-32, 54 | "config.mapper_lm_lambda_max": 1e3 55 | } 56 | } 57 | -------------------------------------------------------------------------------- /data/euroc_config_vo.json: -------------------------------------------------------------------------------- 1 | { 2 | "value0": { 3 | "config.optical_flow_type": "frame_to_frame", 4 | "config.optical_flow_detection_grid_size": 40, 5 | "config.optical_flow_max_recovered_dist2": 0.04, 6 | "config.optical_flow_pattern": 51, 7 | "config.optical_flow_max_iterations": 5, 8 | "config.optical_flow_epipolar_error": 0.005, 9 | "config.optical_flow_levels": 3, 10 | "config.optical_flow_skip_frames": 1, 11 | "config.vio_linearization_type": "ABS_QR", 12 | "config.vio_sqrt_marg": true, 13 | "config.vio_max_states": 3, 14 | "config.vio_max_kfs": 7, 15 | "config.vio_min_frames_after_kf": 1, 16 | "config.vio_new_kf_keypoints_thresh": 0.8, 17 | "config.vio_debug": false, 18 | "config.vio_extended_logging": false, 19 | "config.vio_obs_std_dev": 0.5, 20 | "config.vio_obs_huber_thresh": 1.0, 21 | "config.vio_min_triangulation_dist": 0.05, 22 | "config.vio_outlier_threshold": 3.0, 23 | "config.vio_filter_iteration": 4, 24 | "config.vio_max_iterations": 7, 25 | "config.vio_enforce_realtime": false, 26 | "config.vio_use_lm": true, 27 | "config.vio_lm_lambda_initial": 1e-4, 28 | "config.vio_lm_lambda_min": 1e-5, 29 | "config.vio_lm_lambda_max": 1e2, 30 | "config.vio_lm_landmark_damping_variant": 1, 31 | "config.vio_lm_pose_damping_variant": 1, 32 | "config.vio_scale_jacobian": false, 33 | "config.vio_init_pose_weight": 1e8, 34 | "config.vio_init_ba_weight": 1e1, 35 | "config.vio_init_bg_weight": 1e2, 36 | "config.vio_marg_lost_landmarks": true, 37 | "config.vio_kf_marg_feature_ratio": 0.2, 38 | "config.mapper_obs_std_dev": 0.25, 39 | "config.mapper_obs_huber_thresh": 1.5, 40 | "config.mapper_detection_num_points": 800, 41 | "config.mapper_num_frames_to_match": 30, 42 | "config.mapper_frames_to_match_threshold": 0.04, 43 | "config.mapper_min_matches": 20, 44 | "config.mapper_ransac_threshold": 5e-5, 45 | "config.mapper_min_track_length": 5, 46 | "config.mapper_max_hamming_distance": 70, 47 | "config.mapper_second_best_test_ratio": 1.2, 48 | "config.mapper_bow_num_bits": 16, 49 | "config.mapper_min_triangulation_dist": 0.07, 50 | "config.mapper_no_factor_weights": false, 51 | "config.mapper_use_factors": true, 52 | "config.mapper_use_lm": true, 53 | "config.mapper_lm_lambda_min": 1e-32, 54 | "config.mapper_lm_lambda_max": 1e3 55 | } 56 | } 57 | -------------------------------------------------------------------------------- /data/kitti_config.json: -------------------------------------------------------------------------------- 1 | { 2 | "value0": { 3 | "config.optical_flow_type": "frame_to_frame", 4 | "config.optical_flow_detection_grid_size": 30, 5 | "config.optical_flow_max_recovered_dist2": 0.04, 6 | "config.optical_flow_pattern": 51, 7 | "config.optical_flow_max_iterations": 5, 8 | "config.optical_flow_epipolar_error": 0.001, 9 | "config.optical_flow_levels": 4, 10 | "config.optical_flow_skip_frames": 1, 11 | "config.vio_linearization_type": "ABS_QR", 12 | "config.vio_sqrt_marg": true, 13 | "config.vio_max_states": 3, 14 | "config.vio_max_kfs": 7, 15 | "config.vio_min_frames_after_kf": 1, 16 | "config.vio_new_kf_keypoints_thresh": 0.7, 17 | "config.vio_debug": false, 18 | "config.vio_extended_logging": false, 19 | "config.vio_obs_std_dev": 0.5, 20 | "config.vio_obs_huber_thresh": 1.0, 21 | "config.vio_min_triangulation_dist": 0.05, 22 | "config.vio_outlier_threshold": 3.0, 23 | "config.vio_filter_iteration": 4, 24 | "config.vio_max_iterations": 7, 25 | "config.vio_enforce_realtime": false, 26 | "config.vio_use_lm": false, 27 | "config.vio_lm_lambda_initial": 1e-4, 28 | "config.vio_lm_lambda_min": 1e-6, 29 | "config.vio_lm_lambda_max": 1e2, 30 | "config.vio_lm_landmark_damping_variant": 1, 31 | "config.vio_lm_pose_damping_variant": 1, 32 | "config.vio_scale_jacobian": false, 33 | "config.vio_init_pose_weight": 1e8, 34 | "config.vio_init_ba_weight": 1e1, 35 | "config.vio_init_bg_weight": 1e2, 36 | "config.vio_marg_lost_landmarks": true, 37 | "config.vio_kf_marg_feature_ratio": 0.1, 38 | "config.mapper_obs_std_dev": 0.25, 39 | "config.mapper_obs_huber_thresh": 1.5, 40 | "config.mapper_detection_num_points": 800, 41 | "config.mapper_num_frames_to_match": 30, 42 | "config.mapper_frames_to_match_threshold": 0.04, 43 | "config.mapper_min_matches": 20, 44 | "config.mapper_ransac_threshold": 5e-5, 45 | "config.mapper_min_track_length": 5, 46 | "config.mapper_max_hamming_distance": 70, 47 | "config.mapper_second_best_test_ratio": 1.2, 48 | "config.mapper_bow_num_bits": 16, 49 | "config.mapper_min_triangulation_dist": 0.07, 50 | "config.mapper_no_factor_weights": false, 51 | "config.mapper_use_factors": true, 52 | "config.mapper_use_lm": true, 53 | "config.mapper_lm_lambda_min": 1e-32, 54 | "config.mapper_lm_lambda_max": 1e3 55 | } 56 | } 57 | -------------------------------------------------------------------------------- /data/tumvi_512_config.json: -------------------------------------------------------------------------------- 1 | { 2 | "value0": { 3 | "config.optical_flow_type": "frame_to_frame", 4 | "config.optical_flow_detection_grid_size": 40, 5 | "config.optical_flow_max_recovered_dist2": 0.04, 6 | "config.optical_flow_pattern": 51, 7 | "config.optical_flow_max_iterations": 5, 8 | "config.optical_flow_epipolar_error": 0.005, 9 | "config.optical_flow_levels": 3, 10 | "config.optical_flow_skip_frames": 1, 11 | "config.vio_linearization_type": "ABS_QR", 12 | "config.vio_sqrt_marg": true, 13 | "config.vio_max_states": 3, 14 | "config.vio_max_kfs": 7, 15 | "config.vio_min_frames_after_kf": 5, 16 | "config.vio_new_kf_keypoints_thresh": 0.7, 17 | "config.vio_debug": false, 18 | "config.vio_extended_logging": false, 19 | "config.vio_obs_std_dev": 0.5, 20 | "config.vio_obs_huber_thresh": 1.0, 21 | "config.vio_min_triangulation_dist": 0.05, 22 | "config.vio_outlier_threshold": 3.0, 23 | "config.vio_filter_iteration": 4, 24 | "config.vio_max_iterations": 7, 25 | "config.vio_enforce_realtime": false, 26 | "config.vio_use_lm": false, 27 | "config.vio_lm_lambda_initial": 1e-7, 28 | "config.vio_lm_lambda_min": 1e-6, 29 | "config.vio_lm_lambda_max": 1e2, 30 | "config.vio_lm_landmark_damping_variant": 1, 31 | "config.vio_lm_pose_damping_variant": 1, 32 | "config.vio_scale_jacobian": false, 33 | "config.vio_init_pose_weight": 1e8, 34 | "config.vio_init_ba_weight": 1e1, 35 | "config.vio_init_bg_weight": 1e2, 36 | "config.vio_marg_lost_landmarks": true, 37 | "config.vio_kf_marg_feature_ratio": 0.1, 38 | "config.mapper_obs_std_dev": 0.25, 39 | "config.mapper_obs_huber_thresh": 1.5, 40 | "config.mapper_detection_num_points": 800, 41 | "config.mapper_num_frames_to_match": 30, 42 | "config.mapper_frames_to_match_threshold": 0.04, 43 | "config.mapper_min_matches": 20, 44 | "config.mapper_ransac_threshold": 5e-5, 45 | "config.mapper_min_track_length": 5, 46 | "config.mapper_max_hamming_distance": 70, 47 | "config.mapper_second_best_test_ratio": 1.2, 48 | "config.mapper_bow_num_bits": 16, 49 | "config.mapper_min_triangulation_dist": 0.07, 50 | "config.mapper_no_factor_weights": false, 51 | "config.mapper_use_factors": true, 52 | "config.mapper_use_lm": false, 53 | "config.mapper_lm_lambda_min": 1e-32, 54 | "config.mapper_lm_lambda_max": 1e3 55 | } 56 | } 57 | -------------------------------------------------------------------------------- /doc/DevSetup.md: -------------------------------------------------------------------------------- 1 | 2 | 3 | ### Clang-format 4 | We use clang-format to maintain a consistent formating of the code. Since there are small differences between different version of clang-format we use version 11 on all platforms. 5 | 6 | On **Ubuntu 20.04 or 18.04** run the following commands to install clang-format-11 7 | ``` 8 | wget -O - https://apt.llvm.org/llvm-snapshot.gpg.key|sudo apt-key add - 9 | sudo sh -c 'echo "deb http://apt.llvm.org/$(lsb_release -sc)/ llvm-toolchain-$(lsb_release -sc)-11 main" > /etc/apt/sources.list.d/llvm11.list' 10 | sudo apt-get update 11 | sudo apt-get install clang-format-11 12 | ``` 13 | 14 | On **MacOS** [Homebrew](https://brew.sh/) should install the right version of clang-format: 15 | ``` 16 | brew install clang-format 17 | ``` 18 | 19 | ### Realsense Drivers (Optional) 20 | If you want to use the code with Realsense T265 cameras you should install the realsense library. 21 | 22 | On **Ubuntu 20.04 or 18.04** run the following commands: 23 | ``` 24 | sudo apt-key adv --keyserver hkp://keyserver.ubuntu.com:80 --recv-key C8B3A55A6F3EFCDE 25 | sudo sh -c 'echo "deb http://realsense-hw-public.s3.amazonaws.com/Debian/apt-repo bionic main" > /etc/apt/sources.list.d/realsense.list' 26 | sudo apt-get update 27 | sudo apt-get install librealsense2-dev librealsense2-gl-dev librealsense2-dkms librealsense2-utils librealsense2-dkms 28 | ``` 29 | 30 | On **MacOS** run: 31 | ``` 32 | brew install librealsense 33 | ``` 34 | 35 | ### Install and configure QtCreator 36 | Download and install QtCreator. On **Ubuntu 20.04 or 18.04** run: 37 | ``` 38 | wget https://download.qt.io/official_releases/qtcreator/4.10/4.10.0/qt-creator-opensource-linux-x86_64-4.10.0.run 39 | chmod +x qt-creator-opensource-linux-x86_64-4.10.0.run 40 | ./qt-creator-opensource-linux-x86_64-4.10.0.run 41 | ``` 42 | 43 | On **MacOS** run: 44 | ``` 45 | brew cask install qt-creator 46 | ``` 47 | 48 | After installation, go to `Help` -> `About plugins...` in the menu and enable Beautifier plugin (formats the code automatically on save): 49 | 50 | ![qt_creator_plugins](/doc/img/qt_creator_plugins.png) 51 | 52 | Go to `Tools` -> `Options` and select the Beautifier tab. There select ClangFormat as the tool in `General` tab. 53 | 54 | ![qt_creator_beautifier_general](/doc/img/qt_creator_beautifier_general.png) 55 | 56 | Select file as predefined style in `Clang Format` tab. Also select `None` as the fallback style. For **Ubuntu 20.04 or 18.04** change the executable name to `/usr/bin/clang-format-11`. 57 | 58 | ![qt_creator_beautifier_clang_format](/doc/img/qt_creator_beautifier_clang_format.png) 59 | 60 | ### Build project 61 | First, clone the project repository. 62 | ``` 63 | git clone --recursive https://gitlab.com/VladyslavUsenko/basalt.git 64 | ``` 65 | 66 | After that, in QtCreator open to the `CMakeLists.txt` in the `basalt` folder and configure the project with `Release with Debug Info` configuration. The build directory should point to `//basalt/build`. 67 | 68 | ![qt_creator_configure_project](/doc/img/qt_creator_configure_project.png) 69 | 70 | Finally, you should be able to build and run the project. 71 | -------------------------------------------------------------------------------- /doc/Vo.md: -------------------------------------------------------------------------------- 1 | ## KITTI dataset 2 | 3 | [![teaser](/doc/img/kitti_video.png)](https://www.youtube.com/watch?v=M_ZcNgExUNc) 4 | 5 | We demonstrate the usage of the system with the [KITTI dataset](http://www.cvlibs.net/datasets/kitti/eval_odometry.php) as an example. 6 | 7 | **Note:** The path to calibration and configuration files used here works for the APT installation. If you compile from source specify the appropriate path to the files in [data folder](/data/). 8 | 9 | Download the sequences (`data_odometry_gray.zip`) from the dataset and extract it. 10 | ``` 11 | # We assume you have extracted the sequences in ~/dataset_gray/sequences/ 12 | # Convert calibration to the basalt format 13 | basalt_convert_kitti_calib.py ~/dataset_gray/sequences/00/ 14 | 15 | # If you want to convert calibrations for all sequences use the following command 16 | for i in {00..21}; do basalt_convert_kitti_calib.py ~/dataset_gray/sequences/$i/; done 17 | ``` 18 | Optionally you can also copy the provided ground-truth poses to `poses.txt` in the corresponding sequence. 19 | 20 | ### Visual odometry 21 | To run the visual odometry execute the following command. 22 | ``` 23 | basalt_vio --dataset-path ~/dataset_gray/sequences/00/ --cam-calib /work/kitti/dataset_gray/sequences/00/basalt_calib.json --dataset-type kitti --config-path /usr/etc/basalt/kitti_config.json --show-gui 1 --use-imu 0 24 | ``` 25 | ![magistrale1_vio](/doc/img/kitti.png) 26 | -------------------------------------------------------------------------------- /doc/img/MH_05_MAPPING.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/VladyslavUsenko/basalt/9656344fa138df1f5b86135ded5433e6fb45151a/doc/img/MH_05_MAPPING.png -------------------------------------------------------------------------------- /doc/img/MH_05_OPT_FLOW.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/VladyslavUsenko/basalt/9656344fa138df1f5b86135ded5433e6fb45151a/doc/img/MH_05_OPT_FLOW.png -------------------------------------------------------------------------------- /doc/img/MH_05_VIO.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/VladyslavUsenko/basalt/9656344fa138df1f5b86135ded5433e6fb45151a/doc/img/MH_05_VIO.png -------------------------------------------------------------------------------- /doc/img/SIM_MAPPER.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/VladyslavUsenko/basalt/9656344fa138df1f5b86135ded5433e6fb45151a/doc/img/SIM_MAPPER.png -------------------------------------------------------------------------------- /doc/img/SIM_VIO.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/VladyslavUsenko/basalt/9656344fa138df1f5b86135ded5433e6fb45151a/doc/img/SIM_VIO.png -------------------------------------------------------------------------------- /doc/img/euroc_cam_calib.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/VladyslavUsenko/basalt/9656344fa138df1f5b86135ded5433e6fb45151a/doc/img/euroc_cam_calib.png -------------------------------------------------------------------------------- /doc/img/euroc_imu_calib.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/VladyslavUsenko/basalt/9656344fa138df1f5b86135ded5433e6fb45151a/doc/img/euroc_imu_calib.png -------------------------------------------------------------------------------- /doc/img/kalibr_cam_calib.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/VladyslavUsenko/basalt/9656344fa138df1f5b86135ded5433e6fb45151a/doc/img/kalibr_cam_calib.png -------------------------------------------------------------------------------- /doc/img/kalibr_imu_calib.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/VladyslavUsenko/basalt/9656344fa138df1f5b86135ded5433e6fb45151a/doc/img/kalibr_imu_calib.png -------------------------------------------------------------------------------- /doc/img/kitti.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/VladyslavUsenko/basalt/9656344fa138df1f5b86135ded5433e6fb45151a/doc/img/kitti.png -------------------------------------------------------------------------------- /doc/img/kitti_video.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/VladyslavUsenko/basalt/9656344fa138df1f5b86135ded5433e6fb45151a/doc/img/kitti_video.png -------------------------------------------------------------------------------- /doc/img/magistrale1_mapping.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/VladyslavUsenko/basalt/9656344fa138df1f5b86135ded5433e6fb45151a/doc/img/magistrale1_mapping.png -------------------------------------------------------------------------------- /doc/img/magistrale1_vio.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/VladyslavUsenko/basalt/9656344fa138df1f5b86135ded5433e6fb45151a/doc/img/magistrale1_vio.png -------------------------------------------------------------------------------- /doc/img/qt_creator_beautifier_clang_format.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/VladyslavUsenko/basalt/9656344fa138df1f5b86135ded5433e6fb45151a/doc/img/qt_creator_beautifier_clang_format.png -------------------------------------------------------------------------------- /doc/img/qt_creator_beautifier_general.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/VladyslavUsenko/basalt/9656344fa138df1f5b86135ded5433e6fb45151a/doc/img/qt_creator_beautifier_general.png -------------------------------------------------------------------------------- /doc/img/qt_creator_configure_project.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/VladyslavUsenko/basalt/9656344fa138df1f5b86135ded5433e6fb45151a/doc/img/qt_creator_configure_project.png -------------------------------------------------------------------------------- /doc/img/qt_creator_plugins.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/VladyslavUsenko/basalt/9656344fa138df1f5b86135ded5433e6fb45151a/doc/img/qt_creator_plugins.png -------------------------------------------------------------------------------- /doc/img/realsense_setup.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/VladyslavUsenko/basalt/9656344fa138df1f5b86135ded5433e6fb45151a/doc/img/realsense_setup.jpg -------------------------------------------------------------------------------- /doc/img/t265_cam_calib.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/VladyslavUsenko/basalt/9656344fa138df1f5b86135ded5433e6fb45151a/doc/img/t265_cam_calib.png -------------------------------------------------------------------------------- /doc/img/t265_imu_calib.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/VladyslavUsenko/basalt/9656344fa138df1f5b86135ded5433e6fb45151a/doc/img/t265_imu_calib.png -------------------------------------------------------------------------------- /doc/img/t265_inv_resp_irradiance.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/VladyslavUsenko/basalt/9656344fa138df1f5b86135ded5433e6fb45151a/doc/img/t265_inv_resp_irradiance.png -------------------------------------------------------------------------------- /doc/img/t265_record.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/VladyslavUsenko/basalt/9656344fa138df1f5b86135ded5433e6fb45151a/doc/img/t265_record.png -------------------------------------------------------------------------------- /doc/img/t265_time_align_error.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/VladyslavUsenko/basalt/9656344fa138df1f5b86135ded5433e6fb45151a/doc/img/t265_time_align_error.png -------------------------------------------------------------------------------- /doc/img/t265_time_align_gyro.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/VladyslavUsenko/basalt/9656344fa138df1f5b86135ded5433e6fb45151a/doc/img/t265_time_align_gyro.png -------------------------------------------------------------------------------- /doc/img/t265_vio.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/VladyslavUsenko/basalt/9656344fa138df1f5b86135ded5433e6fb45151a/doc/img/t265_vio.png -------------------------------------------------------------------------------- /doc/img/teaser.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/VladyslavUsenko/basalt/9656344fa138df1f5b86135ded5433e6fb45151a/doc/img/teaser.png -------------------------------------------------------------------------------- /doc/img/tumvi_cam_calib.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/VladyslavUsenko/basalt/9656344fa138df1f5b86135ded5433e6fb45151a/doc/img/tumvi_cam_calib.png -------------------------------------------------------------------------------- /doc/img/tumvi_imu_calib.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/VladyslavUsenko/basalt/9656344fa138df1f5b86135ded5433e6fb45151a/doc/img/tumvi_imu_calib.png -------------------------------------------------------------------------------- /doc/img/uzh_cam_calib.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/VladyslavUsenko/basalt/9656344fa138df1f5b86135ded5433e6fb45151a/doc/img/uzh_cam_calib.png -------------------------------------------------------------------------------- /doc/img/uzh_imu_calib.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/VladyslavUsenko/basalt/9656344fa138df1f5b86135ded5433e6fb45151a/doc/img/uzh_imu_calib.png -------------------------------------------------------------------------------- /docker/b_image_bionic/Dockerfile: -------------------------------------------------------------------------------- 1 | FROM ubuntu:18.04 2 | 3 | RUN apt-get update && apt-get install -y wget gnupg lsb-release software-properties-common && apt-get clean all 4 | 5 | RUN wget -O - https://apt.llvm.org/llvm-snapshot.gpg.key| apt-key add - 6 | RUN echo "deb http://apt.llvm.org/bionic/ llvm-toolchain-bionic-13 main" > /etc/apt/sources.list.d/llvm11.list 7 | #RUN echo "deb http://apt.llvm.org/bionic/ llvm-toolchain-bionic-15 main" > /etc/apt/sources.list.d/llvm12.list 8 | 9 | RUN apt-key adv --keyserver hkp://keyserver.ubuntu.com:80 --recv-key F6E65AC044F831AC80A06380C8B3A55A6F3EFCDE 10 | RUN add-apt-repository "deb https://librealsense.intel.com/Debian/apt-repo $(lsb_release -cs) main" -u 11 | 12 | RUN apt-get update && DEBIAN_FRONTEND=noninteractive apt-get install -y cmake git libtbb-dev libeigen3-dev libglew-dev ccache libjpeg-dev libpng-dev openssh-client liblz4-dev libbz2-dev libboost-regex-dev libboost-filesystem-dev libboost-date-time-dev libboost-program-options-dev libopencv-dev libpython2.7-dev libgtest-dev lsb-core gcovr ggcov lcov librealsense2-dev librealsense2-gl-dev librealsense2-dkms librealsense2-utils doxygen graphviz libsuitesparse-dev clang-13 clang-format-13 clang-tidy-13 libfmt-dev && apt-get clean all 13 | -------------------------------------------------------------------------------- /docker/b_image_focal/Dockerfile: -------------------------------------------------------------------------------- 1 | FROM ubuntu:20.04 2 | 3 | RUN apt-get update && apt-get install -y wget gnupg lsb-release software-properties-common && apt-get clean all 4 | 5 | RUN wget -O - https://apt.llvm.org/llvm-snapshot.gpg.key| apt-key add - 6 | RUN echo "deb http://apt.llvm.org/focal/ llvm-toolchain-focal-14 main" > /etc/apt/sources.list.d/llvm11.list 7 | RUN echo "deb http://apt.llvm.org/focal/ llvm-toolchain-focal-15 main" > /etc/apt/sources.list.d/llvm12.list 8 | 9 | RUN apt-key adv --keyserver hkp://keyserver.ubuntu.com:80 --recv-key F6E65AC044F831AC80A06380C8B3A55A6F3EFCDE 10 | RUN add-apt-repository "deb https://librealsense.intel.com/Debian/apt-repo $(lsb_release -cs) main" -u 11 | 12 | RUN apt-get update && DEBIAN_FRONTEND=noninteractive apt-get install -y cmake git libtbb-dev libeigen3-dev libglew-dev ccache libjpeg-dev libpng-dev openssh-client liblz4-dev libbz2-dev libboost-regex-dev libboost-filesystem-dev libboost-date-time-dev libboost-program-options-dev libopencv-dev libpython2.7-dev libgtest-dev lsb-core gcovr ggcov lcov librealsense2-dev librealsense2-gl-dev librealsense2-dkms librealsense2-utils doxygen graphviz libsuitesparse-dev clang-14 clang-format-14 clang-tidy-14 clang-15 clang-format-15 clang-tidy-15 libfmt-dev && apt-get clean all 13 | -------------------------------------------------------------------------------- /docker/b_image_jammy/Dockerfile: -------------------------------------------------------------------------------- 1 | FROM ubuntu:22.04 2 | 3 | RUN apt-get update && apt-get install -y wget gnupg lsb-release software-properties-common && apt-get clean all 4 | 5 | RUN wget -O - https://apt.llvm.org/llvm-snapshot.gpg.key| apt-key add - 6 | RUN echo "deb http://apt.llvm.org/jammy/ llvm-toolchain-jammy-14 main" > /etc/apt/sources.list.d/llvm11.list 7 | RUN echo "deb http://apt.llvm.org/jammy/ llvm-toolchain-jammy-15 main" > /etc/apt/sources.list.d/llvm12.list 8 | 9 | RUN apt-key adv --keyserver hkp://keyserver.ubuntu.com:80 --recv-key F6E65AC044F831AC80A06380C8B3A55A6F3EFCDE 10 | RUN add-apt-repository "deb https://librealsense.intel.com/Debian/apt-repo $(lsb_release -cs) main" -u 11 | 12 | RUN apt-get update && DEBIAN_FRONTEND=noninteractive apt-get install -y cmake git libtbb-dev libeigen3-dev libglew-dev ccache libjpeg-dev libpng-dev openssh-client liblz4-dev libbz2-dev libboost-regex-dev libboost-filesystem-dev libboost-date-time-dev libboost-program-options-dev libopencv-dev libpython2.7-dev libgtest-dev lsb-core gcovr lcov librealsense2-dev librealsense2-gl-dev librealsense2-dkms librealsense2-utils doxygen graphviz libsuitesparse-dev clang-14 clang-format-14 clang-tidy-14 clang-15 clang-format-15 clang-tidy-15 libfmt-dev libstdc++-12-dev && apt-get clean all 13 | -------------------------------------------------------------------------------- /include/basalt/calibration/aprilgrid.h: -------------------------------------------------------------------------------- 1 | /** 2 | BSD 3-Clause License 3 | 4 | This file is part of the Basalt project. 5 | https://gitlab.com/VladyslavUsenko/basalt.git 6 | 7 | Copyright (c) 2019, Vladyslav Usenko and Nikolaus Demmel. 8 | All rights reserved. 9 | 10 | Redistribution and use in source and binary forms, with or without 11 | modification, are permitted provided that the following conditions are met: 12 | 13 | * Redistributions of source code must retain the above copyright notice, this 14 | list of conditions and the following disclaimer. 15 | 16 | * Redistributions in binary form must reproduce the above copyright notice, 17 | this list of conditions and the following disclaimer in the documentation 18 | and/or other materials provided with the distribution. 19 | 20 | * Neither the name of the copyright holder nor the names of its 21 | contributors may be used to endorse or promote products derived from 22 | this software without specific prior written permission. 23 | 24 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 25 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 26 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 27 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 28 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 29 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 30 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 31 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 32 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 33 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 34 | */ 35 | #pragma once 36 | 37 | #include 38 | 39 | namespace basalt { 40 | 41 | struct AprilGrid { 42 | AprilGrid(const std::string &config_path); 43 | 44 | Eigen::aligned_vector aprilgrid_corner_pos_3d; 45 | Eigen::aligned_vector aprilgrid_vignette_pos_3d; 46 | 47 | inline int getTagCols() const { return tagCols; } 48 | inline int getTagRows() const { return tagRows; } 49 | 50 | private: 51 | int tagCols; // number of apriltags 52 | int tagRows; // number of apriltags 53 | double tagSize; // size of apriltag, edge to edge [m] 54 | double tagSpacing; // ratio of space between tags to tagSize 55 | }; 56 | 57 | } // namespace basalt 58 | -------------------------------------------------------------------------------- /include/basalt/io/marg_data_io.h: -------------------------------------------------------------------------------- 1 | /** 2 | BSD 3-Clause License 3 | 4 | This file is part of the Basalt project. 5 | https://gitlab.com/VladyslavUsenko/basalt.git 6 | 7 | Copyright (c) 2019, Vladyslav Usenko and Nikolaus Demmel. 8 | All rights reserved. 9 | 10 | Redistribution and use in source and binary forms, with or without 11 | modification, are permitted provided that the following conditions are met: 12 | 13 | * Redistributions of source code must retain the above copyright notice, this 14 | list of conditions and the following disclaimer. 15 | 16 | * Redistributions in binary form must reproduce the above copyright notice, 17 | this list of conditions and the following disclaimer in the documentation 18 | and/or other materials provided with the distribution. 19 | 20 | * Neither the name of the copyright holder nor the names of its 21 | contributors may be used to endorse or promote products derived from 22 | this software without specific prior written permission. 23 | 24 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 25 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 26 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 27 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 28 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 29 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 30 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 31 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 32 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 33 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 34 | */ 35 | #pragma once 36 | 37 | #include 38 | #include 39 | 40 | #include 41 | 42 | namespace basalt { 43 | 44 | class MargDataSaver { 45 | public: 46 | using Ptr = std::shared_ptr; 47 | 48 | MargDataSaver(const std::string& path); 49 | ~MargDataSaver() { 50 | saving_thread->join(); 51 | saving_img_thread->join(); 52 | } 53 | tbb::concurrent_bounded_queue in_marg_queue; 54 | 55 | private: 56 | std::shared_ptr saving_thread; 57 | std::shared_ptr saving_img_thread; 58 | 59 | tbb::concurrent_bounded_queue save_image_queue; 60 | }; 61 | 62 | class MargDataLoader { 63 | public: 64 | using Ptr = std::shared_ptr; 65 | 66 | MargDataLoader(); 67 | 68 | void start(const std::string& path); 69 | ~MargDataLoader() { 70 | if (processing_thread) processing_thread->join(); 71 | } 72 | 73 | tbb::concurrent_bounded_queue* out_marg_queue; 74 | 75 | private: 76 | std::shared_ptr processing_thread; 77 | }; 78 | } // namespace basalt 79 | -------------------------------------------------------------------------------- /include/basalt/linearization/block_diagonal.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | #include 6 | 7 | #include 8 | 9 | namespace basalt { 10 | 11 | // TODO: expand IndexedBlocks to small class / struct that also holds info on 12 | // block size and number of blocks, so we don't have to pass it around all the 13 | // time and we can directly implement things link adding diagonal and matrix 14 | // vector products in this sparse block diagonal matrix. 15 | 16 | // map of camera index to block; used to represent sparse block diagonal matrix 17 | template 18 | using IndexedBlocks = 19 | std::unordered_map>; 21 | 22 | // scale dimensions of JTJ as you would do for jacobian scaling of J beforehand, 23 | // with diagonal scaling matrix D: For jacobain we would use JD, so for JTJ we 24 | // use DJTJD. 25 | template 26 | void scale_jacobians( 27 | IndexedBlocks& block_diagonal, size_t num_blocks, size_t block_size, 28 | const Eigen::Matrix& scaling_vector) { 29 | BASALT_ASSERT(num_blocks * block_size == 30 | unsigned_cast(scaling_vector.size())); 31 | for (auto& [cam_idx, block] : block_diagonal) { 32 | auto D = 33 | scaling_vector.segment(block_size * cam_idx, block_size).asDiagonal(); 34 | block = D * block * D; 35 | } 36 | } 37 | 38 | // add diagonal to block-diagonal matrix; missing blocks are assumed to be 0 39 | template 40 | void add_diagonal(IndexedBlocks& block_diagonal, size_t num_blocks, 41 | size_t block_size, 42 | const Eigen::Matrix& diagonal) { 43 | BASALT_ASSERT(num_blocks * block_size == unsigned_cast(diagonal.size())); 44 | for (size_t idx = 0; idx < num_blocks; ++idx) { 45 | auto [it, is_new] = block_diagonal.try_emplace(idx); 46 | if (is_new) { 47 | it->second = diagonal.segment(block_size * idx, block_size).asDiagonal(); 48 | } else { 49 | it->second += diagonal.segment(block_size * idx, block_size).asDiagonal(); 50 | } 51 | } 52 | } 53 | 54 | // sum up diagonal blocks in hash map for parallel reduction 55 | template 56 | class BlockDiagonalAccumulator { 57 | public: 58 | using VecX = Eigen::Matrix; 59 | using MatX = Eigen::Matrix; 60 | 61 | inline void add(size_t idx, MatX&& block) { 62 | auto [it, is_new] = block_diagonal_.try_emplace(idx); 63 | if (is_new) { 64 | it->second = std::move(block); 65 | } else { 66 | it->second += block; 67 | } 68 | } 69 | 70 | inline void add_diag(size_t num_blocks, size_t block_size, 71 | const VecX& diagonal) { 72 | add_diagonal(block_diagonal_, num_blocks, block_size, diagonal); 73 | } 74 | 75 | inline void join(BlockDiagonalAccumulator& b) { 76 | for (auto& [k, v] : b.block_diagonal_) { 77 | auto [it, is_new] = block_diagonal_.try_emplace(k); 78 | if (is_new) { 79 | it->second = std::move(v); 80 | } else { 81 | it->second += v; 82 | } 83 | } 84 | } 85 | 86 | IndexedBlocks block_diagonal_; 87 | }; 88 | 89 | } // namespace basalt 90 | -------------------------------------------------------------------------------- /include/basalt/linearization/linearization_base.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | #include 6 | #include 7 | #include 8 | 9 | namespace basalt { 10 | 11 | template 12 | class LinearizationBase { 13 | public: 14 | using Scalar = Scalar_; 15 | static constexpr int POSE_SIZE = POSE_SIZE_; 16 | 17 | using VecX = Eigen::Matrix; 18 | using MatX = Eigen::Matrix; 19 | 20 | struct Options { 21 | typename LandmarkBlock::Options lb_options; 22 | LinearizationType linearization_type; 23 | }; 24 | 25 | virtual ~LinearizationBase() = default; 26 | 27 | virtual void log_problem_stats(ExecutionStats& stats) const = 0; 28 | 29 | virtual Scalar linearizeProblem(bool* numerically_valid = nullptr) = 0; 30 | 31 | virtual void performQR() = 0; 32 | 33 | // virtual void setPoseDamping(const Scalar lambda) = 0; 34 | 35 | // virtual bool hasPoseDamping() const = 0; 36 | 37 | virtual Scalar backSubstitute(const VecX& pose_inc) = 0; 38 | 39 | // virtual VecX getJp_diag2() const = 0; 40 | 41 | // virtual void scaleJl_cols() = 0; 42 | 43 | // virtual void scaleJp_cols(const VecX& jacobian_scaling) = 0; 44 | 45 | // virtual void setLandmarkDamping(Scalar lambda) = 0; 46 | 47 | virtual void get_dense_Q2Jp_Q2r(MatX& Q2Jp, VecX& Q2r) const = 0; 48 | 49 | virtual void get_dense_H_b(MatX& H, VecX& b) const = 0; 50 | 51 | static std::unique_ptr create( 52 | BundleAdjustmentBase* estimator, const AbsOrderMap& aom, 53 | const Options& options, 54 | const MargLinData* marg_lin_data = nullptr, 55 | const ImuLinData* imu_lin_data = nullptr, 56 | const std::set* used_frames = nullptr, 57 | const std::unordered_set* lost_landmarks = nullptr, 58 | int64_t last_state_to_marg = std::numeric_limits::max()); 59 | }; 60 | 61 | bool isLinearizationSqrt(const LinearizationType& type); 62 | 63 | } // namespace basalt 64 | -------------------------------------------------------------------------------- /include/basalt/optical_flow/optical_flow.h: -------------------------------------------------------------------------------- 1 | /** 2 | BSD 3-Clause License 3 | 4 | This file is part of the Basalt project. 5 | https://gitlab.com/VladyslavUsenko/basalt.git 6 | 7 | Copyright (c) 2019, Vladyslav Usenko and Nikolaus Demmel. 8 | All rights reserved. 9 | 10 | Redistribution and use in source and binary forms, with or without 11 | modification, are permitted provided that the following conditions are met: 12 | 13 | * Redistributions of source code must retain the above copyright notice, this 14 | list of conditions and the following disclaimer. 15 | 16 | * Redistributions in binary form must reproduce the above copyright notice, 17 | this list of conditions and the following disclaimer in the documentation 18 | and/or other materials provided with the distribution. 19 | 20 | * Neither the name of the copyright holder nor the names of its 21 | contributors may be used to endorse or promote products derived from 22 | this software without specific prior written permission. 23 | 24 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 25 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 26 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 27 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 28 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 29 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 30 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 31 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 32 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 33 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 34 | */ 35 | #pragma once 36 | 37 | #include 38 | 39 | #include 40 | 41 | #include 42 | 43 | #include 44 | #include 45 | #include 46 | #include 47 | 48 | #include 49 | 50 | namespace basalt { 51 | 52 | using KeypointId = size_t; 53 | 54 | struct OpticalFlowInput { 55 | using Ptr = std::shared_ptr; 56 | 57 | int64_t t_ns; 58 | std::vector img_data; 59 | }; 60 | 61 | struct OpticalFlowResult { 62 | using Ptr = std::shared_ptr; 63 | 64 | int64_t t_ns; 65 | std::vector> 66 | observations; 67 | 68 | std::vector> pyramid_levels; 69 | 70 | OpticalFlowInput::Ptr input_images; 71 | }; 72 | 73 | class OpticalFlowBase { 74 | public: 75 | using Ptr = std::shared_ptr; 76 | 77 | tbb::concurrent_bounded_queue input_queue; 78 | tbb::concurrent_bounded_queue* output_queue = nullptr; 79 | 80 | Eigen::MatrixXf patch_coord; 81 | }; 82 | 83 | class OpticalFlowFactory { 84 | public: 85 | static OpticalFlowBase::Ptr getOpticalFlow(const VioConfig& config, 86 | const Calibration& cam); 87 | }; 88 | } // namespace basalt 89 | -------------------------------------------------------------------------------- /include/basalt/utils/cast_utils.hpp: -------------------------------------------------------------------------------- 1 | /** 2 | BSD 3-Clause License 3 | 4 | This file is part of the Basalt project. 5 | https://gitlab.com/VladyslavUsenko/basalt.git 6 | 7 | Copyright (c) 2021, Vladyslav Usenko and Nikolaus Demmel. 8 | All rights reserved. 9 | 10 | Redistribution and use in source and binary forms, with or without 11 | modification, are permitted provided that the following conditions are met: 12 | 13 | * Redistributions of source code must retain the above copyright notice, this 14 | list of conditions and the following disclaimer. 15 | 16 | * Redistributions in binary form must reproduce the above copyright notice, 17 | this list of conditions and the following disclaimer in the documentation 18 | and/or other materials provided with the distribution. 19 | 20 | * Neither the name of the copyright holder nor the names of its 21 | contributors may be used to endorse or promote products derived from 22 | this software without specific prior written permission. 23 | 24 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 25 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 26 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 27 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 28 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 29 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 30 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 31 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 32 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 33 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 34 | */ 35 | 36 | #pragma once 37 | 38 | #include 39 | 40 | namespace basalt { 41 | 42 | template 43 | typename std::make_signed_t signed_cast(I v) { 44 | static_assert(std::is_unsigned_v, "no unsigned"); 45 | return static_cast>(v); 46 | } 47 | 48 | template 49 | typename std::make_unsigned_t unsigned_cast(I v) { 50 | static_assert(std::is_signed_v, "no signed"); 51 | return static_cast>(v); 52 | } 53 | 54 | // copy-assign map while casting Scalar type of values 55 | template 56 | void assign_cast_map_values(M1& a, const M2& b) { 57 | using Scalar = typename M1::mapped_type::Scalar; 58 | a.clear(); 59 | for (const auto& [k, v] : b) { 60 | a.try_emplace(k, v.template cast()); 61 | } 62 | } 63 | 64 | } // namespace basalt 65 | -------------------------------------------------------------------------------- /include/basalt/utils/filesystem.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | // Check for feature test macro for 4 | #if defined(__cpp_lib_filesystem) 5 | #define INCLUDE_STD_FILESYSTEM_EXPERIMENTAL 0 6 | 7 | // Check for feature test macro for 8 | #elif defined(__cpp_lib_experimental_filesystem) 9 | #define INCLUDE_STD_FILESYSTEM_EXPERIMENTAL 1 10 | 11 | // We can't check if headers exist... 12 | // Let's assume experimental to be safe 13 | #elif !defined(__has_include) 14 | #define INCLUDE_STD_FILESYSTEM_EXPERIMENTAL 1 15 | 16 | // Check if the header "" exists 17 | #elif __has_include() 18 | 19 | // If we're compiling on Visual Studio and are not compiling with C++17, we need 20 | // to use experimental 21 | #ifdef _MSC_VER 22 | 23 | // Check and include header that defines "_HAS_CXX17" 24 | #if __has_include() 25 | #include 26 | 27 | // Check for enabled C++17 support 28 | #if defined(_HAS_CXX17) && _HAS_CXX17 29 | // We're using C++17, so let's use the normal version 30 | #define INCLUDE_STD_FILESYSTEM_EXPERIMENTAL 0 31 | #endif 32 | #endif 33 | 34 | // If the marco isn't defined yet, that means any of the other VS specific 35 | // checks failed, so we need to use experimental 36 | #ifndef INCLUDE_STD_FILESYSTEM_EXPERIMENTAL 37 | #define INCLUDE_STD_FILESYSTEM_EXPERIMENTAL 1 38 | #endif 39 | 40 | // Not on Visual Studio. Let's use the normal version 41 | #else // #ifdef _MSC_VER 42 | #define INCLUDE_STD_FILESYSTEM_EXPERIMENTAL 0 43 | #endif 44 | 45 | // Check if the header "" exists 46 | #elif __has_include() 47 | #define INCLUDE_STD_FILESYSTEM_EXPERIMENTAL 1 48 | 49 | // Fail if neither header is available with a nice error message 50 | #else 51 | #error Could not find system header "" or "" 52 | #endif 53 | 54 | // We priously determined that we need the exprimental version 55 | #if INCLUDE_STD_FILESYSTEM_EXPERIMENTAL 56 | #include 57 | 58 | namespace basalt { 59 | namespace fs = std::experimental::filesystem; 60 | } 61 | 62 | // We have a decent compiler and can use the normal version 63 | #else 64 | #include 65 | 66 | namespace basalt { 67 | namespace fs = std::filesystem; 68 | } 69 | 70 | #endif 71 | -------------------------------------------------------------------------------- /include/basalt/utils/format.hpp: -------------------------------------------------------------------------------- 1 | /** 2 | BSD 3-Clause License 3 | 4 | This file is part of the Basalt project. 5 | https://gitlab.com/VladyslavUsenko/basalt.git 6 | 7 | Copyright (c) 2022, Vladyslav Usenko and Nikolaus Demmel. 8 | All rights reserved. 9 | 10 | Redistribution and use in source and binary forms, with or without 11 | modification, are permitted provided that the following conditions are met: 12 | 13 | * Redistributions of source code must retain the above copyright notice, this 14 | list of conditions and the following disclaimer. 15 | 16 | * Redistributions in binary form must reproduce the above copyright notice, 17 | this list of conditions and the following disclaimer in the documentation 18 | and/or other materials provided with the distribution. 19 | 20 | * Neither the name of the copyright holder nor the names of its 21 | contributors may be used to endorse or promote products derived from 22 | this software without specific prior written permission. 23 | 24 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 25 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 26 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 27 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 28 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 29 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 30 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 31 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 32 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 33 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 34 | */ 35 | #pragma once 36 | 37 | #include 38 | #include 39 | 40 | namespace basalt { 41 | 42 | // libfmt's string literal formatter implementation was deprecated in 2021-12, 43 | // so provide our own (see also: 44 | // https://github.com/fmtlib/fmt/issues/2640#issuecomment-1048894376) 45 | 46 | namespace literals { 47 | 48 | inline auto operator"" _format(const char* s, size_t n) { 49 | return [=](auto&&... args) { 50 | #if FMT_VERSION < 50000 51 | return fmt::format(s, args...); 52 | #elif FMT_VERSION < 80100 53 | return fmt::format(std::string_view(s, n), args...); 54 | #else 55 | return fmt::format(fmt::runtime(std::string_view(s, n)), args...); 56 | #endif 57 | }; 58 | } 59 | 60 | } // namespace literals 61 | 62 | // make the _format string literal available in the whole namespace 63 | using namespace literals; 64 | 65 | } // namespace basalt 66 | -------------------------------------------------------------------------------- /include/basalt/utils/sim_utils.h: -------------------------------------------------------------------------------- 1 | /** 2 | BSD 3-Clause License 3 | 4 | This file is part of the Basalt project. 5 | https://gitlab.com/VladyslavUsenko/basalt.git 6 | 7 | Copyright (c) 2019, Vladyslav Usenko and Nikolaus Demmel. 8 | All rights reserved. 9 | 10 | Redistribution and use in source and binary forms, with or without 11 | modification, are permitted provided that the following conditions are met: 12 | 13 | * Redistributions of source code must retain the above copyright notice, this 14 | list of conditions and the following disclaimer. 15 | 16 | * Redistributions in binary form must reproduce the above copyright notice, 17 | this list of conditions and the following disclaimer in the documentation 18 | and/or other materials provided with the distribution. 19 | 20 | * Neither the name of the copyright holder nor the names of its 21 | contributors may be used to endorse or promote products derived from 22 | this software without specific prior written permission. 23 | 24 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 25 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 26 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 27 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 28 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 29 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 30 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 31 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 32 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 33 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 34 | */ 35 | #pragma once 36 | 37 | #include 38 | 39 | namespace basalt { 40 | 41 | struct SimObservations { 42 | Eigen::aligned_vector pos; 43 | std::vector id; 44 | }; 45 | 46 | } // namespace basalt 47 | 48 | namespace cereal { 49 | template 50 | void serialize(Archive& ar, basalt::SimObservations& c) { 51 | ar(c.pos, c.id); 52 | } 53 | } // namespace cereal 54 | -------------------------------------------------------------------------------- /include/basalt/utils/system_utils.h: -------------------------------------------------------------------------------- 1 | /** 2 | BSD 3-Clause License 3 | 4 | This file is part of the Basalt project. 5 | https://gitlab.com/VladyslavUsenko/basalt.git 6 | 7 | Copyright (c) 2019, Vladyslav Usenko and Nikolaus Demmel. 8 | All rights reserved. 9 | 10 | Redistribution and use in source and binary forms, with or without 11 | modification, are permitted provided that the following conditions are met: 12 | 13 | * Redistributions of source code must retain the above copyright notice, this 14 | list of conditions and the following disclaimer. 15 | 16 | * Redistributions in binary form must reproduce the above copyright notice, 17 | this list of conditions and the following disclaimer in the documentation 18 | and/or other materials provided with the distribution. 19 | 20 | * Neither the name of the copyright holder nor the names of its 21 | contributors may be used to endorse or promote products derived from 22 | this software without specific prior written permission. 23 | 24 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 25 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 26 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 27 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 28 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 29 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 30 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 31 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 32 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 33 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 34 | */ 35 | #ifndef BASALT_SYSTEM_UTILS_H 36 | #define BASALT_SYSTEM_UTILS_H 37 | 38 | #include 39 | #include 40 | 41 | namespace basalt { 42 | 43 | inline std::string ensure_trailing_slash(const std::string& path) { 44 | if (!path.empty() && path[path.size() - 1] != '/') { 45 | return path + "/"; 46 | } else { 47 | return path; 48 | } 49 | } 50 | 51 | struct MemoryInfo { 52 | uint64_t resident_memory = 0; //!< in bytes 53 | uint64_t resident_memory_peak = 0; //!< in bytes 54 | }; 55 | 56 | bool get_memory_info(MemoryInfo& info); 57 | 58 | } // namespace basalt 59 | 60 | #endif // include guard 61 | -------------------------------------------------------------------------------- /include/basalt/utils/test_utils.h: -------------------------------------------------------------------------------- 1 | /** 2 | BSD 3-Clause License 3 | 4 | This file is part of the Basalt project. 5 | https://gitlab.com/VladyslavUsenko/basalt.git 6 | 7 | Copyright (c) 2019, Vladyslav Usenko and Nikolaus Demmel. 8 | All rights reserved. 9 | 10 | Redistribution and use in source and binary forms, with or without 11 | modification, are permitted provided that the following conditions are met: 12 | 13 | * Redistributions of source code must retain the above copyright notice, this 14 | list of conditions and the following disclaimer. 15 | 16 | * Redistributions in binary form must reproduce the above copyright notice, 17 | this list of conditions and the following disclaimer in the documentation 18 | and/or other materials provided with the distribution. 19 | 20 | * Neither the name of the copyright holder nor the names of its 21 | contributors may be used to endorse or promote products derived from 22 | this software without specific prior written permission. 23 | 24 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 25 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 26 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 27 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 28 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 29 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 30 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 31 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 32 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 33 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 34 | */ 35 | #ifndef BASALT_TEST_UTILS_H 36 | #define BASALT_TEST_UTILS_H 37 | 38 | #include 39 | 40 | template 41 | void test_jacobian_code(const std::string& name, 42 | const Eigen::MatrixBase& Ja, F func, 43 | const Eigen::MatrixBase& x0, 44 | double eps = 1e-8, double max_norm = 1e-4) { 45 | typedef typename Derived1::Scalar Scalar; 46 | 47 | Eigen::Matrix Jn = Ja; 48 | Jn.setZero(); 49 | 50 | Eigen::Matrix inc = x0; 51 | for (int i = 0; i < Jn.cols(); i++) { 52 | inc.setZero(); 53 | inc[i] += eps; 54 | 55 | Eigen::Matrix fpe = func(x0 + inc); 56 | Eigen::Matrix fme = func(x0 - inc); 57 | 58 | Jn.col(i) = (fpe - fme) / (2 * eps); 59 | } 60 | 61 | Scalar diff = (Ja - Jn).norm(); 62 | 63 | if (diff > max_norm || !Ja.allFinite()) { 64 | std::cerr << name << std::endl; 65 | std::cerr << "Numeric Jacobian is different from analytic. Norm difference " 66 | << diff << std::endl; 67 | std::cerr << "Ja\n" << Ja << std::endl; 68 | std::cerr << "Jn\n" << Jn << std::endl; 69 | } else { 70 | // std::cout << name << std::endl; 71 | // std::cout << "Success" << std::endl; 72 | // std::cout << "Ja\n" << Ja << std::endl; 73 | // std::cout << "Jn\n" << Jn << std::endl; 74 | } 75 | } 76 | 77 | #endif // BASALT_TEST_UTILS_H 78 | -------------------------------------------------------------------------------- /include/basalt/vi_estimator/marg_helper.h: -------------------------------------------------------------------------------- 1 | /** 2 | BSD 3-Clause License 3 | 4 | This file is part of the Basalt project. 5 | https://gitlab.com/VladyslavUsenko/basalt.git 6 | 7 | Copyright (c) 2019, Vladyslav Usenko and Nikolaus Demmel. 8 | All rights reserved. 9 | 10 | Redistribution and use in source and binary forms, with or without 11 | modification, are permitted provided that the following conditions are met: 12 | 13 | * Redistributions of source code must retain the above copyright notice, this 14 | list of conditions and the following disclaimer. 15 | 16 | * Redistributions in binary form must reproduce the above copyright notice, 17 | this list of conditions and the following disclaimer in the documentation 18 | and/or other materials provided with the distribution. 19 | 20 | * Neither the name of the copyright holder nor the names of its 21 | contributors may be used to endorse or promote products derived from 22 | this software without specific prior written permission. 23 | 24 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 25 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 26 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 27 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 28 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 29 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 30 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 31 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 32 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 33 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 34 | */ 35 | 36 | #pragma once 37 | 38 | #include 39 | #include 40 | 41 | namespace basalt { 42 | 43 | template 44 | class MargHelper { 45 | public: 46 | using Scalar = Scalar_; 47 | using MatX = Eigen::Matrix; 48 | using VecX = Eigen::Matrix; 49 | 50 | // Modifies abs_H and abs_b as a side effect. 51 | static void marginalizeHelperSqToSq(MatX& abs_H, VecX& abs_b, 52 | const std::set& idx_to_keep, 53 | const std::set& idx_to_marg, 54 | MatX& marg_H, VecX& marg_b); 55 | 56 | // Modifies abs_H and abs_b as a side effect. 57 | static void marginalizeHelperSqToSqrt(MatX& abs_H, VecX& abs_b, 58 | const std::set& idx_to_keep, 59 | const std::set& idx_to_marg, 60 | MatX& marg_sqrt_H, VecX& marg_sqrt_b); 61 | 62 | // Modifies Q2Jp and Q2r as a side effect. 63 | static void marginalizeHelperSqrtToSqrt(MatX& Q2Jp, VecX& Q2r, 64 | const std::set& idx_to_keep, 65 | const std::set& idx_to_marg, 66 | MatX& marg_sqrt_H, VecX& marg_sqrt_b); 67 | }; 68 | } // namespace basalt 69 | -------------------------------------------------------------------------------- /python/basalt/__init__.py: -------------------------------------------------------------------------------- 1 | # 2 | # BSD 3-Clause License 3 | # 4 | # This file is part of the Basalt project. 5 | # https://gitlab.com/VladyslavUsenko/basalt.git 6 | # 7 | # Copyright (c) 2019-2021, Vladyslav Usenko and Nikolaus Demmel. 8 | # All rights reserved. 9 | # 10 | -------------------------------------------------------------------------------- /python/basalt/latex/templates.py: -------------------------------------------------------------------------------- 1 | # 2 | # BSD 3-Clause License 3 | # 4 | # This file is part of the Basalt project. 5 | # https://gitlab.com/VladyslavUsenko/basalt.git 6 | # 7 | # Copyright (c) 2019-2021, Vladyslav Usenko and Nikolaus Demmel. 8 | # All rights reserved. 9 | # 10 | screenread_sty = r""" 11 | \ProvidesPackage{screenread} 12 | % Copyright (C) 2012 John Collins, collins@phys.psu.edu 13 | % License: LPPL 1.2 14 | 15 | % Note: To avoid compatibility issues between geometry and at least one 16 | % class file, it may be better to set all the dimensions by hand. 17 | 18 | % 20 Nov 2014 - use `pageBreakSection` instead of clobbering `section` 19 | % - increase longest page size to 575cm 20 | % - make top, right, and left margins something sensible and 21 | % a bit more aesthetically pleasing 22 | % 24 Jan 2012 Argument to \SetScreen is screen width 23 | % 23 Jan 2012 Remove package showlayout 24 | % 22 Jan 2012 Initial version, based on ideas in 25 | % B. Veytsman amd M. Ware, Tugboat 32 (2011) 261. 26 | 27 | \RequirePackage{everyshi} 28 | \RequirePackage{geometry} 29 | 30 | %======================= 31 | 32 | \pagestyle{empty} 33 | 34 | \EveryShipout{% 35 | \pdfpageheight=\pagetotal 36 | \advance\pdfpageheight by 2in 37 | \advance\pdfpageheight by \topmargin 38 | \advance\pdfpageheight by \textheight % This and next allow for footnotes 39 | \advance\pdfpageheight by -\pagegoal 40 | } 41 | 42 | \AtEndDocument{\pagebreak} 43 | 44 | \def\pageBreakSection{\pagebreak\section} 45 | 46 | \newlength\screenwidth 47 | \newlength{\savedscreenwidth} 48 | 49 | \newcommand\SetScreen[1]{% 50 | % Argument #1 is the screen width. 51 | % Set appropriate layout parameters, with only a little white space 52 | % around the text. 53 | \setlength\screenwidth{#1}% 54 | \setlength\savedscreenwidth{#1}% 55 | \setlength\textwidth{#1}% 56 | \addtolength\textwidth{-2cm}% 57 | \geometry{layoutwidth=\screenwidth, 58 | paperwidth=\screenwidth, 59 | textwidth=\textwidth, 60 | layoutheight=575cm, 61 | paperheight=575cm, 62 | textheight=575cm, 63 | top=1cm, 64 | left=1cm, 65 | right=1cm, 66 | hcentering=true 67 | }% 68 | } 69 | 70 | \newcommand\SetPageScreenWidth[1]{% 71 | \setlength\savedscreenwidth{\screenwidth}% 72 | \setlength\screenwidth{#1}% 73 | \pdfpagewidth\screenwidth% 74 | \setlength\textwidth{\screenwidth}% 75 | \addtolength\textwidth{-2cm}% 76 | } 77 | 78 | \newcommand\RestorePageScreenWidth{% 79 | \setlength\screenwidth{\savedscreenwidth}% 80 | \pdfpagewidth\screenwidth% 81 | \setlength\textwidth{\screenwidth}% 82 | \addtolength\textwidth{-2cm}% 83 | } 84 | 85 | 86 | % Compute a reasonable default screen width, and set it 87 | \setlength\screenwidth{\textwidth} 88 | \addtolength\screenwidth{1cm} 89 | \SetScreen{\screenwidth} 90 | 91 | \endinput 92 | 93 | """ 94 | -------------------------------------------------------------------------------- /python/basalt/latex/util.py: -------------------------------------------------------------------------------- 1 | # 2 | # BSD 3-Clause License 3 | # 4 | # This file is part of the Basalt project. 5 | # https://gitlab.com/VladyslavUsenko/basalt.git 6 | # 7 | # Copyright (c) 2019-2021, Vladyslav Usenko and Nikolaus Demmel. 8 | # All rights reserved. 9 | # 10 | import math 11 | import numpy as np 12 | 13 | 14 | def best_two_non_repeating(array, reverse=False): 15 | if reverse: 16 | best = -math.inf 17 | second = -math.inf 18 | for v in array: 19 | if v > best: 20 | second = best 21 | best = v 22 | elif v < best and v > second: 23 | second = v 24 | else: 25 | best = math.inf 26 | second = math.inf 27 | for v in array: 28 | if v < best: 29 | second = best 30 | best = v 31 | elif v > best and v < second: 32 | second = v 33 | 34 | return best, second 35 | 36 | 37 | def format_ratio(val, val_ref=None, decimals=0): 38 | if val_ref == 0: 39 | return "{}".format(math.inf) 40 | else: 41 | if val_ref is not None: 42 | val = float(val) / float(val_ref) 43 | return "{:.{prec}f}".format(val, prec=decimals) 44 | 45 | 46 | def format_ratio_percent(val, val_ref=None, decimals=0): 47 | if val_ref == 0: 48 | return "{}".format(val) 49 | else: 50 | if val_ref is not None: 51 | val = float(val) / float(val_ref) 52 | val = 100 * val 53 | return "{:.{prec}f}%".format(val, prec=decimals) 54 | 55 | 56 | def rotation2d(theta_deg): 57 | theta = np.radians(theta_deg) 58 | 59 | R = np.array(((np.cos(theta), -np.sin(theta)), (np.sin(theta), np.cos(theta)))) 60 | 61 | return R 62 | -------------------------------------------------------------------------------- /python/basalt/log.py: -------------------------------------------------------------------------------- 1 | # 2 | # BSD 3-Clause License 3 | # 4 | # This file is part of the Basalt project. 5 | # https://gitlab.com/VladyslavUsenko/basalt.git 6 | # 7 | # Copyright (c) 2019-2021, Vladyslav Usenko and Nikolaus Demmel. 8 | # All rights reserved. 9 | # 10 | import ubjson 11 | import json 12 | import os 13 | 14 | import numpy as np 15 | 16 | from collections import Mapping 17 | from munch import Munch 18 | from munch import munchify 19 | 20 | 21 | class ExecutionStats(Munch): 22 | 23 | def __init__(self, path): 24 | data = self._load(path) 25 | if data is None: 26 | Munch.__init__(self) 27 | else: 28 | Munch.__init__(self, data) 29 | 30 | def _load(self, path): 31 | 32 | if path.endswith("ubjson"): 33 | with open(path, 'rb') as f: 34 | data = ubjson.load(f) 35 | else: 36 | with open(path, 'r') as f: 37 | data = json.load(f) 38 | 39 | if isinstance(data, Mapping): 40 | data = self._convert(data) 41 | 42 | return munchify(data) 43 | 44 | def _convert(self, data): 45 | 46 | data_new = dict() 47 | 48 | for k, v in data.items(): 49 | if k.endswith("__values"): 50 | continue # skip; processed together with __index 51 | elif k.endswith("__index"): 52 | idx = v 53 | values = np.array(data[k.replace("__index", "__values")]) 54 | # convert to list of arrays according to start indices 55 | res = np.split(values, idx[1:]) 56 | if all(len(res[0]) == len(x) for x in res): 57 | res = np.array(res) 58 | data_new[k.replace("__index", "")] = res 59 | else: 60 | data_new[k] = np.array(v) 61 | 62 | return data_new 63 | 64 | def _is_imu(self): 65 | return len(self.marg_ev[0]) == 15 66 | 67 | 68 | def detect_log_path(dir, basename): 69 | 70 | for ext in ["ubjson", "json"]: 71 | path = os.path.join(dir, basename + "." + ext) 72 | if os.path.isfile(path): 73 | return path 74 | 75 | return None 76 | 77 | 78 | def load_execution_stats(dir, basename): 79 | 80 | path = detect_log_path(dir, basename) 81 | 82 | if path is not None: 83 | return ExecutionStats(path) 84 | else: 85 | return None 86 | 87 | 88 | class Log(Munch): 89 | 90 | @staticmethod 91 | def load(dir): 92 | 93 | log = Log(all=load_execution_stats(dir, "stats_all"), 94 | sums=load_execution_stats(dir, "stats_sums"), 95 | vio=load_execution_stats(dir, "stats_vio")) 96 | 97 | if all([v is None for v in log.values()]): 98 | return None 99 | else: 100 | return log 101 | 102 | def __init__(self, *args, **kwargs): 103 | Munch.__init__(self, *args, **kwargs) 104 | 105 | def duration(self): 106 | return (self.sums.frame_id[-1] - self.sums.frame_id[0]) * 1e-9 107 | -------------------------------------------------------------------------------- /python/basalt/util.py: -------------------------------------------------------------------------------- 1 | # 2 | # BSD 3-Clause License 3 | # 4 | # This file is part of the Basalt project. 5 | # https://gitlab.com/VladyslavUsenko/basalt.git 6 | # 7 | # Copyright (c) 2019-2021, Vladyslav Usenko and Nikolaus Demmel. 8 | # All rights reserved. 9 | # 10 | import os 11 | import json 12 | import platform 13 | import subprocess 14 | import re 15 | import numpy as np 16 | 17 | from munch import munchify 18 | 19 | 20 | def copy_subdict(d, keys): 21 | res = dict() 22 | for k in keys: 23 | if k in d: 24 | res[k] = d[k] 25 | return res 26 | 27 | 28 | def load_json_if_exists(filepath): 29 | if os.path.isfile(filepath): 30 | with open(filepath, 'r') as f: 31 | return munchify(json.load(f)) 32 | return None 33 | 34 | 35 | def load_text_if_exists(filepath): 36 | if os.path.isfile(filepath): 37 | with open(filepath, 'r') as f: 38 | return f.read() 39 | return None 40 | 41 | 42 | def load_trajectory_tum(filepath): 43 | # first row is header 44 | # format for each row is: ts x y z qz qy qz qw 45 | traj = np.loadtxt(filepath, delimiter=" ", skiprows=1) 46 | # return just translation for now 47 | traj = traj[:, 1:4] 48 | return traj 49 | 50 | 51 | def load_trajectory_tum_if_exists(filepath): 52 | if os.path.isfile(filepath): 53 | return load_trajectory_tum(filepath) 54 | return None 55 | 56 | 57 | def os_open_file(filepath): 58 | if platform.system() == 'Darwin': 59 | subprocess.call(('open', filepath)) 60 | elif platform.system() == 'Windows': 61 | os.startfile(filepath) 62 | else: 63 | subprocess.call(('xdg-open', filepath)) 64 | 65 | 66 | # key for 'human' sorting 67 | def alphanum(key): 68 | 69 | def convert(text): 70 | return float(text) if text.isdigit() else text 71 | 72 | return [convert(c) for c in re.split('([-+]?[0-9]*\.?[0-9]*)', key)] 73 | -------------------------------------------------------------------------------- /scripts/basalt_capture_mocap.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # 3 | # BSD 3-Clause License 4 | # 5 | # This file is part of the Basalt project. 6 | # https://gitlab.com/VladyslavUsenko/basalt.git 7 | # 8 | # Copyright (c) 2019-2021, Vladyslav Usenko and Nikolaus Demmel. 9 | # All rights reserved. 10 | # 11 | 12 | import sys 13 | import os 14 | import rospy 15 | import argparse 16 | from geometry_msgs.msg import TransformStamped 17 | 18 | 19 | def callback(data): 20 | global out_file, time_offset 21 | if not out_file: 22 | return 23 | 24 | if not time_offset: 25 | time_offset = rospy.Time().now() - data.header.stamp 26 | 27 | out_file.write('{},{},{},{},{},{},{},{}\n'.format( 28 | data.header.stamp + time_offset, 29 | data.transform.translation.x, 30 | data.transform.translation.y, 31 | data.transform.translation.z, 32 | data.transform.rotation.w, 33 | data.transform.rotation.x, 34 | data.transform.rotation.y, 35 | data.transform.rotation.z 36 | )) 37 | 38 | 39 | def listener(): 40 | rospy.init_node('listener', anonymous=True) 41 | rospy.Subscriber('/vrpn_client/raw_transform', TransformStamped, callback) 42 | 43 | rospy.spin() 44 | 45 | 46 | if __name__ == '__main__': 47 | 48 | parser = argparse.ArgumentParser(description='Record Motion Capture messages from ROS (/vrpn_client/raw_transform).') 49 | parser.add_argument('-d', '--dataset-path', required=True, help="Path to store the result") 50 | args = parser.parse_args() 51 | 52 | dataset_path = args.dataset_path 53 | 54 | out_file = None 55 | time_offset = None 56 | 57 | if not os.path.exists(dataset_path): 58 | os.makedirs(dataset_path) 59 | 60 | out_file = open(dataset_path + '/data.csv', 'w') 61 | out_file.write('#timestamp [ns], p_RS_R_x [m], p_RS_R_y [m], p_RS_R_z [m], q_RS_w [], q_RS_x [], q_RS_y [], q_RS_z []\n') 62 | listener() 63 | out_file.close() 64 | -------------------------------------------------------------------------------- /scripts/batch/generate-tables.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # 3 | # BSD 3-Clause License 4 | # 5 | # This file is part of the Basalt project. 6 | # https://gitlab.com/VladyslavUsenko/basalt.git 7 | # 8 | # Copyright (c) 2019-2021, Vladyslav Usenko and Nikolaus Demmel. 9 | # All rights reserved. 10 | # 11 | 12 | # Dependencies: 13 | # pip3 install -U --user py_ubjson matplotlib numpy munch scipy pylatex toml 14 | 15 | # also: latexmk and latex 16 | # 17 | # Ubuntu: 18 | # sudo apt install texlive-latex-extra latexmk 19 | import os 20 | import sys 21 | 22 | sys.path.insert(0, os.path.abspath(os.path.join(os.path.dirname(__file__), "..", "..", "python"))) 23 | 24 | import basalt.generate_tables 25 | 26 | basalt.generate_tables.main() 27 | -------------------------------------------------------------------------------- /scripts/batch/plot.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # 3 | # BSD 3-Clause License 4 | # 5 | # This file is part of the Basalt project. 6 | # https://gitlab.com/VladyslavUsenko/basalt.git 7 | # 8 | # Copyright (c) 2019-2021, Vladyslav Usenko and Nikolaus Demmel. 9 | # All rights reserved. 10 | # 11 | 12 | import os 13 | import sys 14 | 15 | sys.path.insert(0, os.path.abspath(os.path.join(os.path.dirname(__file__), "..", "..", "python"))) 16 | 17 | import basalt.nullspace 18 | 19 | basalt.nullspace.main() 20 | -------------------------------------------------------------------------------- /scripts/batch/query-config.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # 3 | # BSD 3-Clause License 4 | # 5 | # This file is part of the Basalt project. 6 | # https://gitlab.com/VladyslavUsenko/basalt.git 7 | # 8 | # Copyright (c) 2019-2021, Vladyslav Usenko and Nikolaus Demmel. 9 | # All rights reserved. 10 | # 11 | 12 | # 13 | # Example usage: 14 | # $ ./query-config.py path/to/basalt_config.json value0.\"config.vio_debug\" 15 | # 10G 16 | 17 | import json 18 | import toml 19 | import argparse 20 | import sys 21 | 22 | 23 | def parse_query(query): 24 | query_list = [] 25 | 26 | quote_open_char = None 27 | curr = "" 28 | for c in query: 29 | if quote_open_char: 30 | if c == quote_open_char: 31 | quote_open_char = None 32 | else: 33 | curr += c 34 | elif c in ['"', "'"]: 35 | quote_open_char = c 36 | elif c == '.': 37 | query_list.append(curr) 38 | curr = "" 39 | else: 40 | curr += c 41 | query_list.append(curr) 42 | 43 | return query_list 44 | 45 | 46 | def query_config(path, query, default_value=None, format_env=False, format_cli=False): 47 | query_list = parse_query(query) 48 | with open(path) as f: 49 | cfg = json.load(f) 50 | try: 51 | curr = cfg 52 | for q in query_list: 53 | curr = curr[q] 54 | result = curr 55 | except: 56 | if default_value is None: 57 | result = "" 58 | else: 59 | result = default_value 60 | if isinstance(result, dict): 61 | if format_env: 62 | lines = [] 63 | for k, v in result.items(): 64 | # NOTE: assumes no special escaping is necessary 65 | lines.append("{}='{}'".format(k, v)) 66 | return "\n".join(lines) 67 | elif format_cli: 68 | args = ["--{} {}".format(k, v) for k, v in result.items()] 69 | return " ".join(args) 70 | else: 71 | result = toml.dumps(result) 72 | else: 73 | result = "{}".format(result) 74 | return result 75 | 76 | 77 | def main(): 78 | parser = argparse.ArgumentParser("Parse toml file and print content of query key.") 79 | parser.add_argument("config_path", help="path to toml file") 80 | parser.add_argument("query", help="query string") 81 | parser.add_argument("default_value", help="value printed if query is not successful", nargs='?') 82 | parser.add_argument( 83 | "--format-env", 84 | action="store_true", 85 | help="Expect dictionary as query result and output like environment variables, i.e. VAR='VALUE' lines.") 86 | parser.add_argument("--format-cli", 87 | action="store_true", 88 | help="Expect dictionary as query result and output like cli arguments, i.e. --VAR 'VALUE'.") 89 | args = parser.parse_args() 90 | 91 | res = query_config(args.config_path, 92 | args.query, 93 | default_value=args.default_value, 94 | format_env=args.format_env, 95 | format_cli=args.format_cli) 96 | print(res) 97 | 98 | 99 | if __name__ == "__main__": 100 | main() 101 | -------------------------------------------------------------------------------- /scripts/batch/rerun-failed-in.sh: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env bash 2 | ## 3 | ## BSD 3-Clause License 4 | ## 5 | ## This file is part of the Basalt project. 6 | ## https://gitlab.com/VladyslavUsenko/basalt.git 7 | ## 8 | ## Copyright (c) 2019-2021, Vladyslav Usenko and Nikolaus Demmel. 9 | ## All rights reserved. 10 | ## 11 | 12 | # 13 | # Usage: 14 | # rerun-failed-in.sh FOLDER 15 | # 16 | # Reruns all failed experiments that are found in a given folder. 17 | 18 | set -e 19 | 20 | SCRIPT_DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" >/dev/null 2>&1 && pwd )" 21 | 22 | "$SCRIPT_DIR"/list-jobs.sh "$1" -s -o failed | while read f; do 23 | echo "$f" 24 | "$SCRIPT_DIR"/rerun-one-in.sh "$f" || true 25 | done 26 | -------------------------------------------------------------------------------- /scripts/batch/rerun-one-in.sh: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env bash 2 | ## 3 | ## BSD 3-Clause License 4 | ## 5 | ## This file is part of the Basalt project. 6 | ## https://gitlab.com/VladyslavUsenko/basalt.git 7 | ## 8 | ## Copyright (c) 2019-2021, Vladyslav Usenko and Nikolaus Demmel. 9 | ## All rights reserved. 10 | ## 11 | 12 | set -e 13 | set -x 14 | 15 | SCRIPT_DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" >/dev/null 2>&1 && pwd )" 16 | 17 | FOLDER="${1}" 18 | 19 | cd "$FOLDER" 20 | 21 | # backup previous files 22 | DATE=`date +'%Y%m%d-%H%M%S'` 23 | BACKUP_FOLDER=results-backup-$DATE 24 | for f in *.jobid *.log stats*.*json; do 25 | if [ -f $f ]; then 26 | mkdir -p $BACKUP_FOLDER 27 | mv $f $BACKUP_FOLDER/ 28 | fi 29 | done 30 | 31 | echo "Created" > status.log 32 | echo "Restarted" >> status.log 33 | 34 | echo "Starting run in $PWD" 35 | "$SCRIPT_DIR"/run-one.sh "$PWD" 36 | -------------------------------------------------------------------------------- /scripts/batch/run-all-in.sh: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env bash 2 | ## 3 | ## BSD 3-Clause License 4 | ## 5 | ## This file is part of the Basalt project. 6 | ## https://gitlab.com/VladyslavUsenko/basalt.git 7 | ## 8 | ## Copyright (c) 2019-2021, Vladyslav Usenko and Nikolaus Demmel. 9 | ## All rights reserved. 10 | ## 11 | 12 | 13 | # given folder with basalt_config_*.json, run optimization for each config in 14 | # corresponding subfolder 15 | 16 | set -e 17 | set -x 18 | 19 | # number of logical cores on linux and macos 20 | NUM_CORES=`(which nproc > /dev/null && nproc) || sysctl -n hw.logicalcpu || echo 1` 21 | 22 | echo "Running on '`hostname`', nproc: $NUM_CORES" 23 | 24 | SCRIPT_DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" >/dev/null 2>&1 && pwd )" 25 | 26 | # loop over all arguments, and in each folder find configs and run them 27 | for FOLDER in "$@" 28 | do 29 | 30 | pushd "$FOLDER" 31 | 32 | FILE_PATTERN='basalt_config_*.json' 33 | FILE_REGEX='basalt_config_(.*)\.json' 34 | 35 | DATE=`date +'%Y%m%d-%H%M%S'` 36 | mkdir -p $DATE 37 | 38 | declare -a RUN_DIRS=() 39 | 40 | for f in `find . -name "$FILE_PATTERN" -type f | sort`; do 41 | if [[ `basename $f` =~ $FILE_REGEX ]]; then 42 | RUN_DIR=${DATE}/`dirname $f`/${BASH_REMATCH[1]} 43 | echo "Creating run with config $f in $RUN_DIR" 44 | mkdir -p "$RUN_DIR" 45 | cp $f "$RUN_DIR"/basalt_config.json 46 | echo "Created" > "$RUN_DIR"/status.log 47 | RUN_DIRS+=($RUN_DIR) 48 | else 49 | echo "Skipping $f" 50 | fi 51 | done 52 | 53 | for RUN_DIR in "${RUN_DIRS[@]}"; do 54 | echo "Starting run in $RUN_DIR" 55 | "$SCRIPT_DIR"/run-one.sh "$RUN_DIR" || true 56 | done 57 | 58 | popd 59 | 60 | done 61 | -------------------------------------------------------------------------------- /scripts/batch/run-one.sh: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env bash 2 | ## 3 | ## BSD 3-Clause License 4 | ## 5 | ## This file is part of the Basalt project. 6 | ## https://gitlab.com/VladyslavUsenko/basalt.git 7 | ## 8 | ## Copyright (c) 2019-2021, Vladyslav Usenko and Nikolaus Demmel. 9 | ## All rights reserved. 10 | ## 11 | 12 | # 13 | # This script runs on the slurm nodes to run rootba for one config. 14 | 15 | set -e 16 | set -o pipefail 17 | set -x 18 | 19 | error() { 20 | local parent_lineno="$1" 21 | local message="$2" 22 | local code="${3:-1}" 23 | if [[ -n "$message" ]] ; then 24 | echo "Error on or near line ${parent_lineno}: ${message}; exiting with status ${code}" 25 | else 26 | echo "Error on or near line ${parent_lineno}; exiting with status ${code}" 27 | fi 28 | echo "Failed" >> status.log 29 | exit "${code}" 30 | } 31 | trap 'error ${LINENO}' ERR 32 | 33 | # number of logical cores on linux and macos 34 | NUM_CORES=`(which nproc > /dev/null && nproc) || sysctl -n hw.logicalcpu || echo 1` 35 | 36 | SCRIPT_DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" >/dev/null 2>&1 && pwd )" 37 | 38 | BASALT_BIN_DIR="${BASALT_BIN_DIR:-$SCRIPT_DIR/../../build}" 39 | 40 | FOLDER="${1}" 41 | 42 | cd "$FOLDER" 43 | 44 | if ! which time 2> /dev/null; then 45 | echo "Did not find 'time' executable. Not installed?" 46 | exit 1 47 | fi 48 | 49 | if [[ "$OSTYPE" == "darwin"* ]]; then 50 | TIMECMD="`which time` -lp" 51 | else 52 | TIMECMD="`which time` -v" 53 | fi 54 | 55 | echo "Started" >> status.log 56 | 57 | # set environment variables according to config 58 | while read l; do 59 | if [ -n "$l" ]; then 60 | eval "export $l" 61 | fi 62 | done <<< `"$SCRIPT_DIR"/query-config.py basalt_config.json batch_run.env --format-env` 63 | 64 | # lookup executable to run 65 | EXECUTABLE=`"$SCRIPT_DIR"/query-config.py basalt_config.json batch_run.executable basalt_vio` 66 | 67 | # lookup args 68 | ARGS=`"$SCRIPT_DIR"/query-config.py basalt_config.json batch_run.args --format-cli` 69 | 70 | CMD="$BASALT_BIN_DIR/$EXECUTABLE" 71 | 72 | echo "Running on '`hostname`', nproc: $NUM_CORES, bin: $CMD" 73 | 74 | # run as many times as specified (for timing tests to make sure filecache is hot); default is once 75 | rm -f output.log 76 | NUM_RUNS=`"$SCRIPT_DIR"/query-config.py basalt_config.json batch_run.num_runs 1` 77 | echo "Will run $NUM_RUNS times." 78 | for i in $(seq $NUM_RUNS); do 79 | echo ">>> Run $i" |& tee -a output.log 80 | { $TIMECMD "$CMD" $ARGS; } |& tee -a output.log 81 | done 82 | 83 | echo "Completed" >> status.log 84 | -------------------------------------------------------------------------------- /scripts/clang-format-all.sh: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env bash 2 | ## 3 | ## BSD 3-Clause License 4 | ## 5 | ## This file is part of the Basalt project. 6 | ## https://gitlab.com/VladyslavUsenko/basalt.git 7 | ## 8 | ## Copyright (c) 2019-2021, Vladyslav Usenko and Nikolaus Demmel. 9 | ## All rights reserved. 10 | ## 11 | 12 | # Format all source files in the project. 13 | # Optionally take folder as argument; default is full inlude and src dirs. 14 | 15 | set -e 16 | 17 | SCRIPT_DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" >/dev/null 2>&1 && pwd )" 18 | 19 | FOLDER="${1:-$SCRIPT_DIR/../include $SCRIPT_DIR/../src $SCRIPT_DIR/../test/src}" 20 | 21 | CLANG_FORMAT_COMMANDS="clang-format-15 clang-format-14 clang-format-13 clang-format-12 clang-format-11 clang-format-10 clang-format-9 clang-format" 22 | 23 | # find the first available command: 24 | for CMD in $CLANG_FORMAT_COMMANDS; do 25 | if hash $CMD 2>/dev/null; then 26 | CLANG_FORMAT_CMD=$CMD 27 | break 28 | fi 29 | done 30 | 31 | if [ -z $CLANG_FORMAT_CMD ]; then 32 | echo "clang-format not installed..." 33 | exit 1 34 | fi 35 | 36 | # clang format check version 37 | MAJOR_VERSION_NEEDED=8 38 | 39 | MAJOR_VERSION_DETECTED=`$CLANG_FORMAT_CMD -version | sed -n -E 's/.*version ([0-9]+).*/\1/p'` 40 | if [ -z $MAJOR_VERSION_DETECTED ]; then 41 | echo "Failed to parse major version (`$CLANG_FORMAT_CMD -version`)" 42 | exit 1 43 | fi 44 | 45 | echo "clang-format version $MAJOR_VERSION_DETECTED (`$CLANG_FORMAT_CMD -version`)" 46 | 47 | if [ $MAJOR_VERSION_DETECTED -lt $MAJOR_VERSION_NEEDED ]; then 48 | echo "Looks like your clang format is too old; need at least version $MAJOR_VERSION_NEEDED" 49 | exit 1 50 | fi 51 | 52 | find $FOLDER -iname "*.?pp" -or -iname "*.h" | xargs $CLANG_FORMAT_CMD -verbose -i 53 | -------------------------------------------------------------------------------- /scripts/eval_full/gen_results.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # 3 | # BSD 3-Clause License 4 | # 5 | # This file is part of the Basalt project. 6 | # https://gitlab.com/VladyslavUsenko/basalt.git 7 | # 8 | # Copyright (c) 2019-2021, Vladyslav Usenko and Nikolaus Demmel. 9 | # All rights reserved. 10 | # 11 | 12 | import os 13 | import sys 14 | import json 15 | 16 | 17 | datasets = ['Seq.', 'MH_01_easy', 'MH_02_easy', 'MH_03_medium', 'MH_04_difficult', 18 | 'MH_05_difficult', 'V1_01_easy', 'V1_02_medium', 19 | 'V1_03_difficult', 'V2_01_easy', 'V2_02_medium'] 20 | 21 | 22 | # Other results. 23 | 24 | 25 | vio = { 26 | 'ate' : ['VIO RMS ATE [m]'], 27 | 'time' : ['VIO Time [s]'], 28 | 'num_frames' : ['VIO Num. Frames'] 29 | } 30 | 31 | mapping = { 32 | 'ate' : ['MAP RMS ATE [m]'], 33 | 'time' : ['MAP Time [s]'], 34 | 'num_frames' : ['MAP Num. KFs'] 35 | } 36 | 37 | pose_graph = { 38 | 'ate' : ['PG RMS ATE [m]'], 39 | 'time' : ['PG Time [s]'], 40 | 'num_frames' : ['PG Num. KFs'] 41 | } 42 | 43 | pure_ba = { 44 | 'ate' : ['PG RMS ATE [m]'], 45 | 'time' : ['PG Time [s]'], 46 | 'num_frames' : ['PG Num. KFs'] 47 | } 48 | 49 | out_dir = sys.argv[1] 50 | 51 | def load_data(x, prefix, key): 52 | fname = out_dir + '/' + prefix + '_' + key 53 | if os.path.isfile(fname): 54 | with open(fname, 'r') as f: 55 | j = json.load(f) 56 | res = round(j['rms_ate'], 3) 57 | x['ate'].append(float(res)) 58 | x['time'].append(round(j['exec_time_ns']*1e-9, 3)) 59 | x['num_frames'].append(j['num_frames']) 60 | else: 61 | x['ate'].append(float('Inf')) 62 | x['time'].append(float('Inf')) 63 | x['num_frames'].append(float('Inf')) 64 | 65 | 66 | for key in datasets[1:]: 67 | load_data(vio, 'vio', key) 68 | load_data(mapping, 'mapper', key) 69 | load_data(pose_graph, 'mapper_no_weights', key) 70 | load_data(pure_ba, 'mapper_no_factors', key) 71 | 72 | 73 | row_format ="{:>17}" + "{:>13}" * (len(datasets)-1) 74 | 75 | datasets_short = [x[:5] for x in datasets] 76 | 77 | print('\nVisual-Inertial Odometry') 78 | print(row_format.format(*datasets_short)) 79 | 80 | print(row_format.format(*vio['ate'])) 81 | #print(row_format.format(*vio['time'])) 82 | print(row_format.format(*vio['num_frames'])) 83 | 84 | print('\nVisual-Inertial Mapping') 85 | print(row_format.format(*datasets_short)) 86 | 87 | print(row_format.format(*mapping['ate'])) 88 | #print(row_format.format(*mapping['time'])) 89 | print(row_format.format(*mapping['num_frames'])) 90 | 91 | 92 | print('\nPose-Graph optimization (Identity weights for all factors)') 93 | print(row_format.format(*datasets_short)) 94 | 95 | print(row_format.format(*pose_graph['ate'])) 96 | #print(row_format.format(*pose_graph['time'])) 97 | print(row_format.format(*pose_graph['num_frames'])) 98 | 99 | 100 | print('\nPure BA optimization (no factors from the recovery used)') 101 | print(row_format.format(*datasets_short)) 102 | 103 | print(row_format.format(*pure_ba['ate'])) 104 | #print(row_format.format(*pure_ba['time'])) 105 | print(row_format.format(*pure_ba['num_frames'])) 106 | -------------------------------------------------------------------------------- /scripts/eval_full/gen_results_kitti.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # 3 | # BSD 3-Clause License 4 | # 5 | # This file is part of the Basalt project. 6 | # https://gitlab.com/VladyslavUsenko/basalt.git 7 | # 8 | # Copyright (c) 2019-2021, Vladyslav Usenko and Nikolaus Demmel. 9 | # All rights reserved. 10 | # 11 | 12 | import os 13 | import sys 14 | import json 15 | 16 | 17 | datasets = ['Seq.', '00', '02', '03','04', '05', '06','07', '08', '09', '10'] 18 | lengths = [100, 200, 300, 400, 500, 600, 700, 800] 19 | 20 | # Other results. 21 | 22 | 23 | vo = { 24 | 'trans_error': {}, 25 | 'rot_error': {} 26 | } 27 | 28 | for l in lengths: 29 | vo['trans_error'][l] = ['Trans. error [%] ' + str(l) + 'm.'] 30 | vo['rot_error'][l] = ['Rot. error [deg/m] ' + str(l) + 'm.'] 31 | 32 | 33 | out_dir = sys.argv[1] 34 | 35 | mean_values = { 36 | 'mean_trans_error' : 0.0, 37 | 'mean_rot_error' : 0.0, 38 | 'total_num_meas' : 0.0 39 | } 40 | 41 | def load_data(x, prefix, key, mean_values): 42 | fname = out_dir + '/' + prefix + '_' + key + '.txt' 43 | if os.path.isfile(fname): 44 | with open(fname, 'r') as f: 45 | j = json.load(f) 46 | res = j['results'] 47 | for v in lengths: 48 | num_meas = res[str(v)]['num_meas'] 49 | trans_error = res[str(v)]['trans_error'] 50 | rot_error = res[str(v)]['rot_error'] 51 | x['trans_error'][int(v)].append(round(trans_error, 5)) 52 | x['rot_error'][int(v)].append(round(rot_error, 5)) 53 | if num_meas > 0: 54 | mean_values['mean_trans_error'] += trans_error*num_meas 55 | mean_values['mean_rot_error'] += rot_error*num_meas 56 | mean_values['total_num_meas'] += num_meas 57 | else: 58 | for v in lengths: 59 | x['trans_error'][int(v)].append(float('inf')) 60 | x['rot_error'][int(v)].append(float('inf')) 61 | 62 | for key in datasets[1:]: 63 | load_data(vo, 'rpe', key, mean_values) 64 | 65 | 66 | row_format ="{:>24}" + "{:>10}" * (len(datasets)-1) 67 | 68 | datasets_short = [x[:5] for x in datasets] 69 | 70 | print('\nVisual Odometry (Stereo)') 71 | print(row_format.format(*datasets_short)) 72 | 73 | for l in lengths: 74 | print(row_format.format(*(vo['trans_error'][l]))) 75 | 76 | print() 77 | 78 | for l in lengths: 79 | print(row_format.format(*(vo['rot_error'][l]))) 80 | 81 | 82 | print('Mean translation error [%] ', mean_values['mean_trans_error']/mean_values['total_num_meas']) 83 | print('Mean rotation error [deg/m] ', mean_values['mean_rot_error']/mean_values['total_num_meas']) 84 | -------------------------------------------------------------------------------- /scripts/eval_full/gen_results_tumvi.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # 3 | # BSD 3-Clause License 4 | # 5 | # This file is part of the Basalt project. 6 | # https://gitlab.com/VladyslavUsenko/basalt.git 7 | # 8 | # Copyright (c) 2019-2021, Vladyslav Usenko and Nikolaus Demmel. 9 | # All rights reserved. 10 | # 11 | 12 | import os 13 | import sys 14 | import json 15 | 16 | 17 | datasets = ['Seq.', 'dataset-corridor1_512_16', 'dataset-magistrale1_512_16', 'dataset-room1_512_16', 'dataset-slides1_512_16'] 18 | 19 | # Other results. 20 | 21 | 22 | vio = { 23 | 'ate' : ['VIO RMS ATE [m]'], 24 | 'time' : ['VIO Time [s]'], 25 | 'num_frames' : ['VIO Num. Frames'] 26 | } 27 | 28 | out_dir = sys.argv[1] 29 | 30 | def load_data(x, prefix, key): 31 | fname = out_dir + '/' + prefix + '_' + key 32 | if os.path.isfile(fname): 33 | with open(fname, 'r') as f: 34 | j = json.load(f) 35 | res = round(j['rms_ate'], 3) 36 | x['ate'].append(float(res)) 37 | x['time'].append(round(j['exec_time_ns']*1e-9, 3)) 38 | x['num_frames'].append(j['num_frames']) 39 | else: 40 | x['ate'].append(float('Inf')) 41 | x['time'].append(float('Inf')) 42 | x['num_frames'].append(float('Inf')) 43 | 44 | 45 | for key in datasets[1:]: 46 | load_data(vio, 'vio', key) 47 | 48 | 49 | row_format ="{:>17}" + "{:>13}" * (len(datasets)-1) 50 | 51 | datasets_short = [x[8:].split('_')[0] for x in datasets] 52 | 53 | print('\nVisual-Inertial Odometry') 54 | print(row_format.format(*datasets_short)) 55 | 56 | print(row_format.format(*vio['ate'])) 57 | #print(row_format.format(*vio['time'])) 58 | print(row_format.format(*vio['num_frames'])) 59 | -------------------------------------------------------------------------------- /scripts/eval_full/run_evaluations.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | ## 3 | ## BSD 3-Clause License 4 | ## 5 | ## This file is part of the Basalt project. 6 | ## https://gitlab.com/VladyslavUsenko/basalt.git 7 | ## 8 | ## Copyright (c) 2019-2021, Vladyslav Usenko and Nikolaus Demmel. 9 | ## All rights reserved. 10 | ## 11 | 12 | set -e 13 | set -x 14 | 15 | DATASET_PATH=/data/euroc 16 | 17 | DATASETS=(MH_01_easy MH_02_easy MH_03_medium MH_04_difficult MH_05_difficult V1_01_easy V1_02_medium V1_03_difficult V2_01_easy V2_02_medium) 18 | 19 | 20 | folder_name=eval_results 21 | mkdir $folder_name 22 | 23 | 24 | 25 | for d in ${DATASETS[$CI_NODE_INDEX-1]}; do 26 | basalt_vio --dataset-path $DATASET_PATH/$d --cam-calib /usr/etc/basalt/euroc_eucm_calib.json \ 27 | --dataset-type euroc --show-gui 0 --config-path /usr/etc/basalt/euroc_config.json \ 28 | --result-path $folder_name/vio_$d --marg-data eval_tmp_marg_data --save-trajectory tum 29 | 30 | mv trajectory.txt $folder_name/traj_vio_$d.txt 31 | 32 | basalt_mapper --show-gui 0 --cam-calib /usr/etc/basalt/euroc_eucm_calib.json --config-path /usr/etc/basalt/euroc_config.json --marg-data eval_tmp_marg_data \ 33 | --result-path $folder_name/mapper_$d 34 | 35 | basalt_mapper --show-gui 0 --cam-calib /usr/etc/basalt/euroc_eucm_calib.json --config-path /usr/etc/basalt/euroc_config_no_weights.json --marg-data eval_tmp_marg_data \ 36 | --result-path $folder_name/mapper_no_weights_$d 37 | 38 | basalt_mapper --show-gui 0 --cam-calib /usr/etc/basalt/euroc_eucm_calib.json --config-path /usr/etc/basalt/euroc_config_no_factors.json --marg-data eval_tmp_marg_data \ 39 | --result-path $folder_name/mapper_no_factors_$d 40 | 41 | rm -rf eval_tmp_marg_data 42 | done 43 | 44 | #./gen_results.py $folder_name > euroc_results.txt 45 | -------------------------------------------------------------------------------- /scripts/eval_full/run_evaluations_kitti.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | ## 3 | ## BSD 3-Clause License 4 | ## 5 | ## This file is part of the Basalt project. 6 | ## https://gitlab.com/VladyslavUsenko/basalt.git 7 | ## 8 | ## Copyright (c) 2019-2021, Vladyslav Usenko and Nikolaus Demmel. 9 | ## All rights reserved. 10 | ## 11 | 12 | set -e 13 | set -x 14 | 15 | DATASET_PATH=/data/kitti_odom_grey/sequences 16 | 17 | DATASETS=(00 02 03 04 05 06 07 08 09 10) 18 | 19 | 20 | folder_name=eval_results_kitti 21 | mkdir $folder_name 22 | 23 | for d in ${DATASETS[$CI_NODE_INDEX-1]}; do 24 | echo $d 25 | basalt_vio --dataset-path $DATASET_PATH/$d --cam-calib $DATASET_PATH/$d/basalt_calib.json \ 26 | --dataset-type kitti --show-gui 0 --config-path /usr/etc/basalt/kitti_config.json --result-path $folder_name/vo_$d --save-trajectory kitti --use-imu 0 27 | 28 | mv trajectory_kitti.txt $folder_name/kitti_$d.txt 29 | 30 | basalt_kitti_eval --traj-path $folder_name/kitti_$d.txt --gt-path $DATASET_PATH/$d/poses.txt --result-path $folder_name/rpe_$d.txt 31 | 32 | done 33 | -------------------------------------------------------------------------------- /scripts/eval_full/run_evaluations_tumvi.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | ## 3 | ## BSD 3-Clause License 4 | ## 5 | ## This file is part of the Basalt project. 6 | ## https://gitlab.com/VladyslavUsenko/basalt.git 7 | ## 8 | ## Copyright (c) 2019-2021, Vladyslav Usenko and Nikolaus Demmel. 9 | ## All rights reserved. 10 | ## 11 | 12 | set -e 13 | set -x 14 | 15 | DATASET_PATH=/data/tumvi/512_16/ 16 | 17 | DATASETS=( 18 | dataset-corridor1_512_16 19 | dataset-magistrale1_512_16 20 | dataset-room1_512_16 21 | dataset-slides1_512_16 22 | ) 23 | 24 | 25 | folder_name=eval_results_tumvi 26 | mkdir $folder_name 27 | 28 | 29 | 30 | for d in ${DATASETS[$CI_NODE_INDEX-1]}; do 31 | basalt_vio --dataset-path $DATASET_PATH/$d --cam-calib /usr/etc/basalt/tumvi_512_eucm_calib.json \ 32 | --dataset-type euroc --show-gui 0 --config-path /usr/etc/basalt/tumvi_512_config.json \ 33 | --result-path $folder_name/vio_$d --save-trajectory tum 34 | 35 | mv trajectory.txt $folder_name/${d}_basalt_poses.txt 36 | 37 | done 38 | 39 | #./gen_results_tumvi.py $folder_name > euroc_tumvi.txt 40 | -------------------------------------------------------------------------------- /scripts/install_deps.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | ## 3 | ## BSD 3-Clause License 4 | ## 5 | ## This file is part of the Basalt project. 6 | ## https://gitlab.com/VladyslavUsenko/basalt.git 7 | ## 8 | ## Copyright (c) 2019-2021, Vladyslav Usenko and Nikolaus Demmel. 9 | ## All rights reserved. 10 | ## 11 | 12 | DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" >/dev/null 2>&1 && pwd )" 13 | 14 | if [[ "$OSTYPE" == "darwin"* ]]; then 15 | ${DIR}/install_mac_os_deps.sh 16 | else 17 | ${DIR}/install_ubuntu_deps.sh 18 | fi 19 | -------------------------------------------------------------------------------- /scripts/install_mac_os_deps.sh: -------------------------------------------------------------------------------- 1 | #!/bin/sh 2 | ## 3 | ## BSD 3-Clause License 4 | ## 5 | ## This file is part of the Basalt project. 6 | ## https://gitlab.com/VladyslavUsenko/basalt.git 7 | ## 8 | ## Copyright (c) 2019-2021, Vladyslav Usenko and Nikolaus Demmel. 9 | ## All rights reserved. 10 | ## 11 | 12 | brew install \ 13 | boost \ 14 | opencv \ 15 | cmake \ 16 | pkgconfig \ 17 | lz4 \ 18 | clang-format \ 19 | tbb \ 20 | glew \ 21 | eigen \ 22 | ccache \ 23 | lz4 \ 24 | fmt 25 | 26 | brew install llvm 27 | -------------------------------------------------------------------------------- /scripts/install_ubuntu_deps.sh: -------------------------------------------------------------------------------- 1 | #!/bin/sh 2 | ## 3 | ## BSD 3-Clause License 4 | ## 5 | ## This file is part of the Basalt project. 6 | ## https://gitlab.com/VladyslavUsenko/basalt.git 7 | ## 8 | ## Copyright (c) 2019-2021, Vladyslav Usenko and Nikolaus Demmel. 9 | ## All rights reserved. 10 | ## 11 | 12 | sudo apt-get update 13 | sudo apt-get install -y gcc g++ cmake git libtbb-dev libeigen3-dev libglew-dev ccache libjpeg-dev libpng-dev liblz4-dev libbz2-dev libboost-regex-dev libboost-filesystem-dev libboost-date-time-dev libboost-program-options-dev libgtest-dev libopencv-dev libfmt-dev 14 | -------------------------------------------------------------------------------- /scripts/templates/license-py-sh.tmpl: -------------------------------------------------------------------------------- 1 | BSD 3-Clause License 2 | 3 | This file is part of the Basalt project. 4 | https://gitlab.com/VladyslavUsenko/basalt.git 5 | 6 | Copyright (c) ${years}, ${owner}. 7 | All rights reserved. 8 | -------------------------------------------------------------------------------- /scripts/update-license-headers.sh: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env bash 2 | ## 3 | ## BSD 3-Clause License 4 | ## 5 | ## This file is part of the Basalt project. 6 | ## https://gitlab.com/VladyslavUsenko/basalt.git 7 | ## 8 | ## Copyright (c) 2019-2021, Vladyslav Usenko and Nikolaus Demmel. 9 | ## All rights reserved. 10 | ## 11 | 12 | # Update license headers in source files. 13 | 14 | # Dependency: licenseheaders python package (install with pip) 15 | 16 | # TODO: Make it also update C++ files automatically. (Consider files with multiple headers, e.g. track.h and union_find.h) 17 | 18 | SCRIPT_DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" >/dev/null 2>&1 && pwd )" 19 | 20 | DIRS=( 21 | "$SCRIPT_DIR/../python/" 22 | "$SCRIPT_DIR/../scripts" 23 | ) 24 | 25 | YEAR="2019-2021" 26 | OWNER="Vladyslav Usenko and Nikolaus Demmel" 27 | TEMPLATE="$SCRIPT_DIR/templates/license-py-sh.tmpl" 28 | 29 | for d in "${DIRS[@]}" 30 | do 31 | licenseheaders -d "$d" -y $YEAR -o "$OWNER" -t "$TEMPLATE" -vv 32 | done 33 | -------------------------------------------------------------------------------- /scripts/update_submodules.sh: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env bash 2 | ## 3 | ## BSD 3-Clause License 4 | ## 5 | ## This file is part of the Basalt project. 6 | ## https://gitlab.com/VladyslavUsenko/basalt.git 7 | ## 8 | ## Copyright (c) 2019-2021, Vladyslav Usenko and Nikolaus Demmel. 9 | ## All rights reserved. 10 | ## 11 | 12 | set -x 13 | 14 | git submodule sync --recursive 15 | git submodule update --init --recursive 16 | -------------------------------------------------------------------------------- /scripts/yapf-all.sh: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env bash 2 | ## 3 | ## BSD 3-Clause License 4 | ## 5 | ## This file is part of the Basalt project. 6 | ## https://gitlab.com/VladyslavUsenko/basalt.git 7 | ## 8 | ## Copyright (c) 2019-2021, Vladyslav Usenko and Nikolaus Demmel. 9 | ## All rights reserved. 10 | ## 11 | 12 | 13 | # Format all python source files in the project. 14 | # Optionally take folder as argument; default are `python` and `script` dirs. 15 | 16 | set -e 17 | 18 | SCRIPT_DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" >/dev/null 2>&1 && pwd )" 19 | 20 | # default folders if not passed 21 | if [ $# -lt 1 ]; then 22 | set -- "$SCRIPT_DIR"/../python "$SCRIPT_DIR/batch" 23 | fi 24 | 25 | echo "Formatting: $@" 26 | 27 | yapf -i -r "$@" 28 | -------------------------------------------------------------------------------- /src/calibrate.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | BSD 3-Clause License 3 | 4 | This file is part of the Basalt project. 5 | https://gitlab.com/VladyslavUsenko/basalt.git 6 | 7 | Copyright (c) 2019, Vladyslav Usenko and Nikolaus Demmel. 8 | All rights reserved. 9 | 10 | Redistribution and use in source and binary forms, with or without 11 | modification, are permitted provided that the following conditions are met: 12 | 13 | * Redistributions of source code must retain the above copyright notice, this 14 | list of conditions and the following disclaimer. 15 | 16 | * Redistributions in binary form must reproduce the above copyright notice, 17 | this list of conditions and the following disclaimer in the documentation 18 | and/or other materials provided with the distribution. 19 | 20 | * Neither the name of the copyright holder nor the names of its 21 | contributors may be used to endorse or promote products derived from 22 | this software without specific prior written permission. 23 | 24 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 25 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 26 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 27 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 28 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 29 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 30 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 31 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 32 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 33 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 34 | */ 35 | 36 | #include 37 | 38 | #include 39 | 40 | int main(int argc, char **argv) { 41 | std::string dataset_path; 42 | std::string dataset_type; 43 | std::string aprilgrid_path; 44 | std::string result_path; 45 | std::vector cam_types; 46 | std::string cache_dataset_name = "calib-cam"; 47 | int skip_images = 1; 48 | 49 | CLI::App app{"Calibrate IMU"}; 50 | 51 | app.add_option("--dataset-path", dataset_path, "Path to dataset")->required(); 52 | app.add_option("--result-path", result_path, "Path to result folder") 53 | ->required(); 54 | app.add_option("--dataset-type", dataset_type, "Dataset type (euroc, bag)") 55 | ->required(); 56 | 57 | app.add_option("--aprilgrid", aprilgrid_path, 58 | "Path to Aprilgrid config file)") 59 | ->required(); 60 | 61 | app.add_option("--cache-name", cache_dataset_name, 62 | "Name to save cached files"); 63 | 64 | app.add_option("--skip-images", skip_images, "Number of images to skip"); 65 | app.add_option("--cam-types", cam_types, 66 | "Type of cameras (eucm, ds, kb4, pinhole)") 67 | ->required(); 68 | 69 | try { 70 | app.parse(argc, argv); 71 | } catch (const CLI::ParseError &e) { 72 | return app.exit(e); 73 | } 74 | 75 | basalt::CamCalib cv(dataset_path, dataset_type, aprilgrid_path, result_path, 76 | cache_dataset_name, skip_images, cam_types); 77 | 78 | cv.renderingLoop(); 79 | 80 | return 0; 81 | } 82 | -------------------------------------------------------------------------------- /src/io/dataset_io.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | BSD 3-Clause License 3 | 4 | This file is part of the Basalt project. 5 | https://gitlab.com/VladyslavUsenko/basalt.git 6 | 7 | Copyright (c) 2019, Vladyslav Usenko and Nikolaus Demmel. 8 | All rights reserved. 9 | 10 | Redistribution and use in source and binary forms, with or without 11 | modification, are permitted provided that the following conditions are met: 12 | 13 | * Redistributions of source code must retain the above copyright notice, this 14 | list of conditions and the following disclaimer. 15 | 16 | * Redistributions in binary form must reproduce the above copyright notice, 17 | this list of conditions and the following disclaimer in the documentation 18 | and/or other materials provided with the distribution. 19 | 20 | * Neither the name of the copyright holder nor the names of its 21 | contributors may be used to endorse or promote products derived from 22 | this software without specific prior written permission. 23 | 24 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 25 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 26 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 27 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 28 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 29 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 30 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 31 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 32 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 33 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 34 | */ 35 | 36 | #include 37 | #include 38 | #include 39 | #include 40 | #include 41 | 42 | namespace basalt { 43 | 44 | DatasetIoInterfacePtr DatasetIoFactory::getDatasetIo( 45 | const std::string &dataset_type, bool load_mocap_as_gt) { 46 | if (dataset_type == "euroc") { 47 | // return DatasetIoInterfacePtr(); 48 | return DatasetIoInterfacePtr(new EurocIO(load_mocap_as_gt)); 49 | } else if (dataset_type == "bag") { 50 | return DatasetIoInterfacePtr(new RosbagIO); 51 | } else if (dataset_type == "uzh") { 52 | return DatasetIoInterfacePtr(new UzhIO); 53 | } else if (dataset_type == "kitti") { 54 | return DatasetIoInterfacePtr(new KittiIO); 55 | } else { 56 | std::cerr << "Dataset type " << dataset_type << " is not supported" 57 | << std::endl; 58 | std::abort(); 59 | } 60 | } 61 | 62 | } // namespace basalt 63 | -------------------------------------------------------------------------------- /src/linearization/landmark_block.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | BSD 3-Clause License 3 | 4 | This file is part of the Basalt project. 5 | https://gitlab.com/VladyslavUsenko/basalt.git 6 | 7 | Copyright (c) 2021, Vladyslav Usenko and Nikolaus Demmel. 8 | All rights reserved. 9 | 10 | Redistribution and use in source and binary forms, with or without 11 | modification, are permitted provided that the following conditions are met: 12 | 13 | * Redistributions of source code must retain the above copyright notice, this 14 | list of conditions and the following disclaimer. 15 | 16 | * Redistributions in binary form must reproduce the above copyright notice, 17 | this list of conditions and the following disclaimer in the documentation 18 | and/or other materials provided with the distribution. 19 | 20 | * Neither the name of the copyright holder nor the names of its 21 | contributors may be used to endorse or promote products derived from 22 | this software without specific prior written permission. 23 | 24 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 25 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 26 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 27 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 28 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 29 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 30 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 31 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 32 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 33 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 34 | */ 35 | 36 | #include 37 | #include 38 | 39 | namespace basalt { 40 | 41 | template 42 | template 43 | std::unique_ptr> 44 | LandmarkBlock::createLandmarkBlock() { 45 | return std::make_unique>(); 46 | } 47 | 48 | // ////////////////////////////////////////////////////////////////// 49 | // instatiate factory templates 50 | 51 | #ifdef BASALT_INSTANTIATIONS_DOUBLE 52 | // Scalar=double, POSE_SIZE=6 53 | template std::unique_ptr> 54 | LandmarkBlock::createLandmarkBlock<6>(); 55 | #endif 56 | 57 | #ifdef BASALT_INSTANTIATIONS_FLOAT 58 | // Scalar=float, POSE_SIZE=6 59 | template std::unique_ptr> 60 | LandmarkBlock::createLandmarkBlock<6>(); 61 | #endif 62 | 63 | } // namespace basalt 64 | -------------------------------------------------------------------------------- /src/utils/system_utils.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | BSD 3-Clause License 3 | 4 | This file is part of the Basalt project. 5 | https://gitlab.com/VladyslavUsenko/basalt.git 6 | 7 | Copyright (c) 2021, Vladyslav Usenko and Nikolaus Demmel. 8 | All rights reserved. 9 | 10 | Redistribution and use in source and binary forms, with or without 11 | modification, are permitted provided that the following conditions are met: 12 | 13 | * Redistributions of source code must retain the above copyright notice, this 14 | list of conditions and the following disclaimer. 15 | 16 | * Redistributions in binary form must reproduce the above copyright notice, 17 | this list of conditions and the following disclaimer in the documentation 18 | and/or other materials provided with the distribution. 19 | 20 | * Neither the name of the copyright holder nor the names of its 21 | contributors may be used to endorse or promote products derived from 22 | this software without specific prior written permission. 23 | 24 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 25 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 26 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 27 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 28 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 29 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 30 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 31 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 32 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 33 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 34 | */ 35 | 36 | #include 37 | 38 | #include 39 | 40 | #if __APPLE__ 41 | #include 42 | #include 43 | #elif __linux__ 44 | #include 45 | 46 | #include 47 | #endif 48 | 49 | namespace basalt { 50 | 51 | bool get_memory_info(MemoryInfo& info) { 52 | #if __APPLE__ 53 | mach_task_basic_info_data_t t_info; 54 | mach_msg_type_number_t t_info_count = MACH_TASK_BASIC_INFO_COUNT; 55 | 56 | if (KERN_SUCCESS != task_info(mach_task_self(), MACH_TASK_BASIC_INFO, 57 | (task_info_t)&t_info, &t_info_count)) { 58 | return false; 59 | } 60 | info.resident_memory = t_info.resident_size; 61 | info.resident_memory_peak = t_info.resident_size_max; 62 | 63 | /* 64 | struct rusage resource_usage; 65 | getrusage(RUSAGE_SELF, &resource_usage); 66 | info.resident_memory_peak = resource_usage.ru_maxrss; 67 | */ 68 | 69 | return true; 70 | #elif __linux__ 71 | 72 | // get current memory first 73 | std::size_t program_size = 0; 74 | std::size_t resident_size = 0; 75 | 76 | std::ifstream fs("/proc/self/statm"); 77 | if (fs.fail()) { 78 | return false; 79 | } 80 | fs >> program_size; 81 | fs >> resident_size; 82 | 83 | info.resident_memory = resident_size * sysconf(_SC_PAGESIZE); 84 | 85 | // get peak memory after that 86 | struct rusage resource_usage; 87 | getrusage(RUSAGE_SELF, &resource_usage); 88 | info.resident_memory_peak = resource_usage.ru_maxrss * 1024; 89 | 90 | return true; 91 | #else 92 | return false; 93 | #endif 94 | } 95 | 96 | } // namespace basalt 97 | -------------------------------------------------------------------------------- /test/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.10...3.18) 2 | 3 | # Note: add_subdirectory(googletest ...) is called in basalt-headers 4 | 5 | include_directories(../thirdparty/basalt-headers/test/include) 6 | 7 | 8 | add_executable(test_spline_opt src/test_spline_opt.cpp) 9 | target_link_libraries(test_spline_opt gtest gtest_main basalt) 10 | 11 | add_executable(test_vio src/test_vio.cpp) 12 | target_link_libraries(test_vio gtest gtest_main basalt) 13 | 14 | add_executable(test_nfr src/test_nfr.cpp) 15 | target_link_libraries(test_nfr gtest gtest_main basalt) 16 | 17 | add_executable(test_qr src/test_qr.cpp) 18 | target_link_libraries(test_qr gtest gtest_main basalt) 19 | 20 | add_executable(test_linearization src/test_linearization.cpp) 21 | target_link_libraries(test_linearization gtest gtest_main basalt) 22 | 23 | add_executable(test_patch src/test_patch.cpp) 24 | target_link_libraries(test_patch gtest gtest_main basalt) 25 | 26 | enable_testing() 27 | 28 | include(GoogleTest) 29 | 30 | #gtest_discover_tests(test_spline_opt DISCOVERY_TIMEOUT 60) 31 | #gtest_discover_tests(test_vio DISCOVERY_TIMEOUT 60) 32 | #gtest_discover_tests(test_nfr DISCOVERY_TIMEOUT 60) 33 | 34 | gtest_add_tests(TARGET test_spline_opt AUTO) 35 | gtest_add_tests(TARGET test_vio AUTO) 36 | gtest_add_tests(TARGET test_nfr AUTO) 37 | gtest_add_tests(TARGET test_qr AUTO) 38 | gtest_add_tests(TARGET test_linearization AUTO) 39 | gtest_add_tests(TARGET test_patch AUTO) 40 | -------------------------------------------------------------------------------- /test/src/test_patch.cpp: -------------------------------------------------------------------------------- 1 | 2 | 3 | #include 4 | #include 5 | 6 | #include 7 | 8 | #include "gtest/gtest.h" 9 | #include "test_utils.h" 10 | 11 | struct SmoothFunction { 12 | template 13 | Scalar interp(const Eigen::Matrix& p) const { 14 | return sin(p[0] / 100.0 + p[1] / 20.0); 15 | } 16 | 17 | template 18 | Eigen::Matrix interpGrad( 19 | const Eigen::Matrix& p) const { 20 | return Eigen::Matrix(sin(p[0] / 100.0 + p[1] / 20.0), 21 | cos(p[0] / 100.0 + p[1] / 20.0) / 100.0, 22 | cos(p[0] / 100.0 + p[1] / 20.0) / 20.0); 23 | } 24 | 25 | template 26 | BASALT_HOST_DEVICE inline bool InBounds( 27 | const Eigen::MatrixBase& p, 28 | const typename Derived::Scalar border) const { 29 | UNUSED(p); 30 | UNUSED(border); 31 | return true; 32 | } 33 | }; 34 | 35 | TEST(Patch, ImageInterpolateGrad) { 36 | Eigen::Vector2i offset(231, 123); 37 | 38 | SmoothFunction img; 39 | 40 | Eigen::Vector2d pd = offset.cast() + Eigen::Vector2d(0.4, 0.34345); 41 | 42 | Eigen::Vector3d val_grad = img.interpGrad(pd); 43 | Eigen::Matrix J_x = val_grad.tail<2>(); 44 | 45 | test_jacobian( 46 | "d_res_d_x", J_x, 47 | [&](const Eigen::Vector2d& x) { 48 | return Eigen::Matrix(img.interp(pd + x)); 49 | }, 50 | Eigen::Vector2d::Zero(), 1); 51 | } 52 | 53 | TEST(Patch, PatchSe2Jac) { 54 | Eigen::Vector2i offset(231, 123); 55 | 56 | SmoothFunction img_view; 57 | 58 | Eigen::Vector2d pd = offset.cast() + Eigen::Vector2d(0.4, 0.34345); 59 | 60 | using PatternT = basalt::Pattern52; 61 | using PatchT = basalt::OpticalFlowPatch; 62 | 63 | double mean, mean2; 64 | 65 | PatchT::VectorP data, data2; 66 | PatchT::MatrixP3 J_se2; 67 | 68 | basalt::OpticalFlowPatch>::setDataJacSe2( 69 | img_view, pd, mean, data, J_se2); 70 | 71 | basalt::OpticalFlowPatch>::setData( 72 | img_view, pd, mean2, data2); 73 | 74 | EXPECT_NEAR(mean, mean2, 1e-8); 75 | EXPECT_TRUE(data.isApprox(data2)); 76 | 77 | test_jacobian( 78 | "d_res_d_se2", J_se2, 79 | [&](const Eigen::Vector3d& x) { 80 | Sophus::SE2d transform = Sophus::SE2d::exp(x); 81 | 82 | double mean3; 83 | PatchT::VectorP data3; 84 | 85 | basalt::OpticalFlowPatch>::setData( 86 | img_view, pd, mean3, data3, &transform); 87 | 88 | return data3; 89 | }, 90 | Eigen::Vector3d::Zero()); 91 | } 92 | -------------------------------------------------------------------------------- /thirdparty/apriltag/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.2...3.18) 2 | 3 | 4 | SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -fPIC") 5 | SET(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -fPIC") 6 | 7 | #file(GLOB APRILTAG_SRCS "${CMAKE_CURRENT_SOURCE_DIR}/apriltag-2016-12-01/*.c" "${CMAKE_CURRENT_SOURCE_DIR}/apriltag-2016-12-01/common/*.c") 8 | #include_directories(apriltag-2016-12-01) 9 | 10 | include_directories(${OpenCV_INCLUDE_DIR}) 11 | 12 | file(GLOB APRILTAG_SRCS "ethz_apriltag2/src/*.cc") 13 | include_directories(ethz_apriltag2/include) 14 | 15 | 16 | include_directories(../../include) 17 | include_directories(../basalt-headers/include) 18 | include_directories(../basalt-headers/thirdparty/Sophus) 19 | 20 | add_library(apriltag STATIC ${APRILTAG_SRCS} include/basalt/utils/apriltag.h src/apriltag.cpp) 21 | 22 | target_include_directories(apriltag PUBLIC include) 23 | target_link_libraries(apriltag PUBLIC basalt::opencv Sophus::Sophus) 24 | 25 | 26 | -------------------------------------------------------------------------------- /thirdparty/apriltag/ethz_apriltag2/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8) 2 | 3 | project(ethz_apriltag2) 4 | 5 | find_package(catkin REQUIRED COMPONENTS cmake_modules) 6 | include_directories(${catkin_INCLUDE_DIRS}) 7 | 8 | find_package(Eigen REQUIRED) 9 | 10 | catkin_package( 11 | DEPENDS eigen opencv 12 | INCLUDE_DIRS include ${Eigen_INCLUDE_DIRS} 13 | LIBRARIES ${PROJECT_NAME} 14 | ) 15 | 16 | set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} ${PROJECT_SOURCE_DIR}/cmake/) 17 | 18 | find_package(Eigen REQUIRED) 19 | find_package(OpenCV REQUIRED) 20 | 21 | add_definitions(-fPIC -O3) 22 | include_directories(include ${Eigen_INCLUDE_DIRS} ${catkin_INCLUDE_DIRS}) 23 | 24 | #library 25 | file(GLOB SOURCE_FILES "src/*.cc") 26 | add_library(${PROJECT_NAME} ${SOURCE_FILES}) 27 | target_link_libraries(${PROJECT_NAME} ${OpenCV_LIBS} ${catkin_LIBRARIES}) 28 | 29 | #demo 30 | if(NOT APPLE) 31 | add_executable(apriltags_demo src/example/apriltags_demo.cpp src/example/Serial.cpp) 32 | target_link_libraries(apriltags_demo ${PROJECT_NAME} v4l2) 33 | endif() 34 | 35 | 36 | -------------------------------------------------------------------------------- /thirdparty/apriltag/ethz_apriltag2/LICENSE: -------------------------------------------------------------------------------- 1 | 2 | Copyright (c) 2014, Paul Furgale, Jérôme Maye and Jörn Rehder, Autonomous Systems Lab, 3 | ETH Zurich, Switzerland 4 | Copyright (c) 2014, Thomas Schneider, Skybotix AG, Switzerland 5 | All rights reserved. 6 | 7 | Redistribution and use in source and binary forms, with or without modification, 8 | are permitted provided that the following conditions are met: 9 | 10 | Redistributions of source code must retain the above copyright notice, this 11 | list of conditions and the following disclaimer. 12 | 13 | Redistributions in binary form must reproduce the above copyright notice, 14 | this list of conditions and the following disclaimer in the documentation 15 | and/or other materials provided with the distribution. 16 | 17 | All advertising materials mentioning features or use of this software must 18 | display the following acknowledgement: This product includes software developed 19 | by the Autonomous Systems Lab and Skybotix AG. 20 | 21 | Neither the name of the Autonomous Systems Lab and Skybotix AG nor the names 22 | of its contributors may be used to endorse or promote products derived from 23 | this software without specific prior written permission. 24 | 25 | THIS SOFTWARE IS PROVIDED BY THE AUTONOMOUS SYSTEMS LAB AND SKYBOTIX AG ''AS IS'' 26 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 27 | WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. 28 | IN NO EVENT SHALL the AUTONOMOUS SYSTEMS LAB OR SKYBOTIX AG BE LIABLE FOR ANY DIRECT, 29 | INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT 30 | NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 31 | PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, 32 | WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 33 | ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY 34 | OF SUCH DAMAGE. 35 | -------------------------------------------------------------------------------- /thirdparty/apriltag/ethz_apriltag2/include/apriltags/Edge.h: -------------------------------------------------------------------------------- 1 | #ifndef EDGE_H 2 | #define EDGE_H 3 | 4 | #include 5 | 6 | #include "apriltags/FloatImage.h" 7 | 8 | namespace AprilTags { 9 | 10 | class FloatImage; 11 | class UnionFindSimple; 12 | 13 | using std::min; 14 | using std::max; 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, 49 | const FloatImage& theta, const FloatImage& mag, 50 | std::vector &edges, size_t &nEdges); 51 | 52 | //! Process edges in order of increasing cost, merging clusters if we can do so without exceeding the thetaThresh. 53 | static void mergeEdges(std::vector &edges, UnionFindSimple &uf, float tmin[], float tmax[], float mmin[], float mmax[]); 54 | 55 | }; 56 | 57 | } // namespace 58 | 59 | #endif 60 | -------------------------------------------------------------------------------- /thirdparty/apriltag/ethz_apriltag2/include/apriltags/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(const FloatImage& other) = default; 33 | 34 | FloatImage& operator=(const FloatImage& other); 35 | 36 | float get(int x, int y) const { return pixels[y * width + x]; } 37 | void set(int x, int y, float v) { pixels[y * width + x] = v; } 38 | 39 | int getWidth() const { return width; } 40 | int getHeight() const { return height; } 41 | int getNumFloatImagePixels() const { return width * height; } 42 | const std::vector& getFloatImagePixels() const { return pixels; } 43 | 44 | //! TODO: Fix decimateAvg function. DO NOT USE! 45 | void decimateAvg(); 46 | 47 | //! Rescale all values so that they are between [0,1] 48 | void normalize(); 49 | 50 | void filterFactoredCentered(const std::vector& fhoriz, 51 | const std::vector& fvert); 52 | 53 | template 54 | void copyToSketch(DualCoding::Sketch& sketch) { 55 | for (int i = 0; i < getNumFloatImagePixels(); i++) 56 | sketch[i] = (T)(255.0f * getFloatImagePixels()[i]); 57 | } 58 | 59 | void printMinMax() const; 60 | }; 61 | 62 | } // namespace AprilTags 63 | 64 | #endif 65 | -------------------------------------------------------------------------------- /thirdparty/apriltag/ethz_apriltag2/include/apriltags/GLine2D.h: -------------------------------------------------------------------------------- 1 | #ifndef GLINE2D_H 2 | #define GLINE2D_H 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | #include "apriltags/MathUtil.h" 9 | #include "apriltags/XYWeight.h" 10 | 11 | namespace AprilTags { 12 | 13 | //! A 2D line 14 | class GLine2D { 15 | public: 16 | 17 | //! Create a new line. 18 | GLine2D(); 19 | 20 | //! Create a new line. 21 | /* @param slope the slope 22 | * @param b the y intercept 23 | */ 24 | GLine2D(float slope, float b); 25 | 26 | //! Create a new line. 27 | /* @param dx A change in X corresponding to dy 28 | * @param dy A change in Y corresponding to dx 29 | * @param p A point that the line passes through 30 | */ 31 | GLine2D(float dX, float dY, const std::pair& pt); 32 | 33 | //! Create a new line through two points. 34 | /* @param p1 the first point 35 | * @param p2 the second point 36 | */ 37 | GLine2D(const std::pair& p1, const std::pair& p2); 38 | 39 | //! Get the coordinate of a point (on this line), with zero corresponding to the point 40 | //! on the that is perpendicular toa line passing through the origin and the line. 41 | /* This allows easy computation if one point is between two other points on the line: 42 | * compute the line coordinate of all three points and test if a<=b<=c. This is 43 | * implemented by computing the dot product of the vector 'p' with the 44 | * line's direct unit vector. 45 | */ 46 | float getLineCoordinate(const std::pair& p); 47 | 48 | //! The inverse of getLineCoordinate. 49 | std::pair getPointOfCoordinate(float coord); 50 | 51 | //!Compute the point where two lines intersect, or (-1,0) if the lines are parallel. 52 | std::pair intersectionWith(const GLine2D& line) const; 53 | 54 | static GLine2D lsqFitXYW(const std::vector& xyweights); 55 | 56 | inline float getDx() const { return dx; } 57 | inline float getDy() const { return dy; } 58 | inline float getFirst() const { return p.first; } 59 | inline float getSecond() const { return p.second; } 60 | 61 | protected: 62 | void normalizeSlope(); 63 | void normalizeP(); 64 | 65 | private: 66 | float dx, dy; 67 | std::pair p; //!< A point the line passes through; when normalized, it is the point closest to the origin (hence perpendicular to the line) 68 | bool didNormalizeSlope; 69 | bool didNormalizeP; 70 | }; 71 | 72 | } // namespace 73 | 74 | #endif 75 | -------------------------------------------------------------------------------- /thirdparty/apriltag/ethz_apriltag2/include/apriltags/GLineSegment2D.h: -------------------------------------------------------------------------------- 1 | #ifndef GLINESEGMENT2D_H 2 | #define GLINESEGMENT2D_H 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | #include "apriltags/GLine2D.h" 10 | #include "apriltags/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 30 | 31 | #endif 32 | -------------------------------------------------------------------------------- /thirdparty/apriltag/ethz_apriltag2/include/apriltags/Gaussian.h: -------------------------------------------------------------------------------- 1 | #ifndef GAUSSIAN_H 2 | #define GAUSSIAN_H 3 | 4 | #include 5 | #include 6 | 7 | namespace AprilTags { 8 | 9 | class Gaussian { 10 | 11 | public: 12 | static bool warned; 13 | 14 | //! Returns a Gaussian filter of size n. 15 | /*! @param sigma standard deviation of the Gaussian 16 | * @param n length of the Gaussian (must be odd) 17 | */ 18 | static std::vector makeGaussianFilter(float sigma, int n); 19 | 20 | //! Convolve the input 'a' (which begins at offset aoff and is alen elements in length) with the filter 'f'. 21 | /*! The result is deposited in 'r' at offset 'roff'. f.size() should be odd. 22 | * The output is shifted by -f.size()/2, so that there is no net time delay. 23 | * @param a input vector of pixels 24 | * @param aoff 25 | * @param alen 26 | * @param f 27 | * @param r the resultant array of pixels 28 | * @param roff 29 | */ 30 | static void convolveSymmetricCentered(const std::vector& a, unsigned int aoff, unsigned int alen, 31 | const std::vector& f, std::vector& r, unsigned int roff); 32 | 33 | }; 34 | 35 | } // namespace 36 | 37 | #endif 38 | -------------------------------------------------------------------------------- /thirdparty/apriltag/ethz_apriltag2/include/apriltags/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 45 | 46 | #endif 47 | -------------------------------------------------------------------------------- /thirdparty/apriltag/ethz_apriltag2/include/apriltags/Homography33.h: -------------------------------------------------------------------------------- 1 | //-*-c++-*- 2 | 3 | #ifndef HOMOGRAPHY33_H 4 | #define HOMOGRAPHY33_H 5 | 6 | #include 7 | #include 8 | 9 | #include 10 | 11 | // NOTE: In Basalt we use homography, since for fisheye-distorted 12 | // images, interpolation is not good enough, resulting in a lot less 13 | // valid detections. At the same time we don't case about speed too 14 | // much. 15 | 16 | // interpolate points instead of using homography 17 | //#define INTERPOLATE 18 | // use stable version of homography recover (opencv, includes refinement step) 19 | #define STABLE_H 20 | 21 | //! Compute 3x3 homography using Direct Linear Transform 22 | /* 23 | * 24 | * DEPRECATED - DEPRECATED - DEPRECATED - DEPRECATED 25 | * 26 | * use TagDetection::getRelativeTransform() instead 27 | * 28 | * 29 | * y = Hx (y = image coordinates in homogeneous coordinates, H = 3x3 30 | * homography matrix, x = homogeneous 2D world coordinates) 31 | * 32 | * For each point correspondence, constrain y x Hx = 0 (where x is 33 | * cross product). This means that they have the same direction, and 34 | * ignores scale factor. 35 | * 36 | * We rewrite this as Ah = 0, where h is the 9x1 column vector of the 37 | * elements of H. Each point correspondence gives us 3 equations of 38 | * rank 2. The solution h is the minimum right eigenvector of A, 39 | * which we can obtain via SVD, USV' = A. h is the right-most column 40 | * of V'. 41 | * 42 | * We will actually maintain A'A internally, which is 9x9 regardless 43 | * of how many correspondences we're given, and has the same 44 | * eigenvectors as A. 45 | */ 46 | class Homography33 { 47 | public: 48 | //! Constructor 49 | Homography33(const std::pair &opticalCenter); 50 | 51 | #ifdef STABLE_H 52 | void setCorrespondences(const std::vector< std::pair > &srcPts, 53 | const std::vector< std::pair > &dstPts); 54 | #else 55 | void addCorrespondence(float worldx, float worldy, float imagex, float imagey); 56 | #endif 57 | 58 | //! Note that the returned H matrix does not reflect cxy. 59 | Eigen::Matrix3d& getH(); 60 | 61 | const std::pair getCXY() const { return cxy; } 62 | 63 | void compute(); 64 | 65 | std::pair project(float worldx, float worldy); 66 | 67 | private: 68 | std::pair cxy; 69 | Eigen::Matrix fA; 70 | Eigen::Matrix3d H; 71 | bool valid; 72 | #ifdef STABLE_H 73 | std::vector< std::pair > srcPts, dstPts; 74 | #endif 75 | }; 76 | 77 | #endif 78 | -------------------------------------------------------------------------------- /thirdparty/apriltag/ethz_apriltag2/include/apriltags/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 | 20 | //! Returns the square of a value. 21 | static inline float square(float x) { return x*x; } 22 | 23 | static inline float distance2D(const std::pair &p0, const std::pair &p1) { 24 | float dx = p0.first - p1.first; 25 | float dy = p0.second - p1.second; 26 | return std::sqrt(dx*dx + dy*dy); 27 | } 28 | 29 | //! Returns a result in [-Pi, Pi] 30 | static inline float mod2pi(float vin) { 31 | const float twopi = 2 * (float)M_PI; 32 | const float twopi_inv = 1.f / (2.f * (float)M_PI); 33 | float absv = std::abs(vin); 34 | float q = absv*twopi_inv + 0.5f; 35 | int qi = (int) q; 36 | float r = absv - qi*twopi; 37 | return (vin<0) ? -r : r; 38 | } 39 | 40 | //! Returns a value of v wrapped such that ref and v differ by no more than +/- Pi 41 | static inline float mod2pi(float ref, float v) { return ref + mod2pi(v-ref); } 42 | 43 | // lousy approximation of arctan function, but good enough for our purposes (about 4 degrees) 44 | static inline double fast_atan2(double y, double x) { 45 | double coeff_1 = M_PI/4; 46 | double coeff_2 = 3*coeff_1; 47 | double abs_y = fabs(y)+1e-10; // kludge to prevent 0/0 condition 48 | 49 | double angle; 50 | 51 | if (x >= 0) { 52 | double r = (x - abs_y) / (x + abs_y); 53 | angle = coeff_1 - coeff_1 * r; 54 | } else { 55 | double r = (x + abs_y) / (abs_y - x); 56 | angle = coeff_2 - coeff_1 * r; 57 | } 58 | 59 | if (y < 0) 60 | return -angle; // negate if in quad III or IV 61 | else 62 | return angle; 63 | } 64 | 65 | }; 66 | 67 | } // namespace 68 | 69 | #endif 70 | -------------------------------------------------------------------------------- /thirdparty/apriltag/ethz_apriltag2/include/apriltags/Quad.h: -------------------------------------------------------------------------------- 1 | #ifndef QUAD_H 2 | #define QUAD_H 3 | 4 | #include 5 | #include 6 | 7 | #include 8 | 9 | #include "apriltags/Homography33.h" 10 | 11 | namespace AprilTags { 12 | 13 | class FloatImage; 14 | class Segment; 15 | 16 | using std::min; 17 | using std::max; 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< std::pair >& 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 the upper right cell is at (1,1). 31 | std::pair interpolate(float x, float y); 32 | 33 | //! Same as interpolate, except that the coordinates are interpreted between 0 and 1, instead of -1 and 1. 34 | std::pair interpolate01(float x, float y); 35 | 36 | //! Points for the quad (in pixel coordinates), in counter clockwise order. These points are the intersections of segments. 37 | std::vector< std::pair > quadPoints; 38 | 39 | //! Segments composing this quad 40 | std::vector segments; 41 | 42 | //! Total length (in pixels) of the actual perimeter observed for the quad. 43 | /*! This is in contrast to the geometric perimeter, some of which 44 | * may not have been directly observed but rather inferred by 45 | * intersecting segments. Quads with more observed perimeter are 46 | * preferred over others. */ 47 | float observedPerimeter; 48 | 49 | //! Given that the whole quad spans from (0,0) to (1,1) in "quad space", compute the pixel coordinates for a given point within that quad. 50 | /*! Note that for most of the Quad's existence, we will not know the correct orientation of the tag. */ 51 | Homography33 homography; 52 | 53 | //! Searches through a vector of Segments to form Quads. 54 | /* @param quads any discovered quads will be added to this list 55 | * @param path the segments currently part of the search 56 | * @param parent the first segment in the quad 57 | * @param depth how deep in the search are we? 58 | */ 59 | static void search(const FloatImage& fImage, std::vector& path, 60 | Segment& parent, int depth, std::vector& quads, 61 | const std::pair& opticalCenter); 62 | 63 | #ifdef INTERPOLATE 64 | private: 65 | Eigen::Vector2f p0, p3, p01, p32; 66 | #endif 67 | 68 | }; 69 | 70 | } // namespace 71 | 72 | #endif 73 | -------------------------------------------------------------------------------- /thirdparty/apriltag/ethz_apriltag2/include/apriltags/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 minimumLineLength; //!< In pixels. Calculated based on minimum plausible decoding size for Tag9 family. 16 | 17 | float getX0() const { return x0; } 18 | void setX0(float newValue) { x0 = newValue; } 19 | 20 | float getY0() const { return y0; } 21 | void setY0(float newValue) { y0 = newValue; } 22 | 23 | float getX1() const { return x1; } 24 | void setX1(float newValue) { x1 = newValue; } 25 | 26 | float getY1() const { return y1; } 27 | void setY1(float newValue) { y1 = newValue; } 28 | 29 | float getTheta() const { return theta; } 30 | void setTheta(float newValue) { theta = newValue; } 31 | 32 | float getLength() const { return length; } 33 | void setLength(float newValue) { length = newValue; } 34 | 35 | //! Returns the length of the Segment. 36 | float segmentLength(); 37 | 38 | //! Print endpoint coordinates of this segment. 39 | void printSegment(); 40 | 41 | //! ID of Segment. 42 | int getId() const { return segmentId; } 43 | 44 | std::vector children; 45 | 46 | private: 47 | float x0, y0, x1, y1; 48 | float theta; // gradient direction (points towards white) 49 | float length; // length of line segment in pixels 50 | int segmentId; 51 | static int idCounter; 52 | }; 53 | 54 | } // namsepace 55 | 56 | #endif 57 | -------------------------------------------------------------------------------- /thirdparty/apriltag/ethz_apriltag2/include/apriltags/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[] = 37 | { 0x231bLL, 0x2ea5LL, 0x346aLL, 0x45b9LL, 0x79a6LL, 0x7f6bLL, 0xb358LL, 0xe745LL, 0xfe59LL, 0x156dLL, 0x380bLL, 0xf0abLL, 0x0d84LL, 0x4736LL, 0x8c72LL, 0xaf10LL, 0x093cLL, 0x93b4LL, 0xa503LL, 0x468fLL, 0xe137LL, 0x5795LL, 0xdf42LL, 0x1c1dLL, 0xe9dcLL, 0x73adLL, 0xad5fLL, 0xd530LL, 0x07caLL, 0xaf2eLL }; 38 | 39 | static const TagCodes tagCodes16h5 = TagCodes(16, 5, t16h5, sizeof(t16h5)/sizeof(t16h5[0])); 40 | 41 | } 42 | -------------------------------------------------------------------------------- /thirdparty/apriltag/ethz_apriltag2/include/apriltags/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, 11 | 0xb91dLL, 0xe745LL, 0x156dLL, 0xd3d2LL, 0xdf5cLL, 0x4736LL, 0x8c72LL, 0x5a02LL, 12 | 0xd32bLL, 0x1867LL, 0x468fLL, 0xdc91LL, 0x4940LL, 0xa9edLL, 0x2bd5LL, 0x599aLL, 13 | 0x9009LL, 0x61f6LL, 0x3850LL, 0x8157LL, 0xbfcaLL, 0x987cLL}; 14 | 15 | static const TagCodes tagCodes16h5_other = TagCodes(16, 5, t16h5_other, sizeof(t16h5_other)/sizeof(t16h5_other[0])); 16 | 17 | } 18 | -------------------------------------------------------------------------------- /thirdparty/apriltag/ethz_apriltag2/include/apriltags/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, 0xdea688LL, 0x81d927LL, 0x51b241LL, 0xdbc8aeLL, 0x1e50e19LL, 0x15819d2LL, 0x16d8282LL, 0x163e035LL, 0x9d9b81LL, 0x173eec4LL, 0xae3a09LL, 0x5f7c51LL, 0x1a137fcLL, 0xdc9562LL, 0x1802e45LL, 0x1c3542cLL, 0x870fa4LL, 0x914709LL, 0x16684f0LL, 0xc8f2a5LL, 0x833ebbLL, 0x59717fLL, 0x13cd050LL, 0xfa0ad1LL, 0x1b763b0LL, 0xb991ceLL }; 49 | 50 | static const TagCodes tagCodes25h9 = TagCodes(25, 9, t25h9, sizeof(t25h9)/sizeof(t25h9[0])); 51 | 52 | } 53 | -------------------------------------------------------------------------------- /thirdparty/apriltag/ethz_apriltag2/include/apriltags/TagDetector.h: -------------------------------------------------------------------------------- 1 | #ifndef TAGDETECTOR_H 2 | #define TAGDETECTOR_H 3 | 4 | #include 5 | 6 | #include "opencv2/opencv.hpp" 7 | 8 | #include "apriltags//TagDetection.h" 9 | #include "apriltags//TagFamily.h" 10 | #include "apriltags//FloatImage.h" 11 | 12 | namespace AprilTags { 13 | 14 | class TagDetector { 15 | public: 16 | 17 | const TagFamily thisTagFamily; 18 | 19 | //! Constructor 20 | // note: TagFamily is instantiated here from TagCodes 21 | TagDetector(const TagCodes& tagCodes, const size_t blackBorder=2) : thisTagFamily(tagCodes, blackBorder) {} 22 | 23 | std::vector extractTags(const cv::Mat& image); 24 | 25 | }; 26 | 27 | } // namespace 28 | 29 | #endif 30 | -------------------------------------------------------------------------------- /thirdparty/apriltag/ethz_apriltag2/include/apriltags/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) { 18 | init(); 19 | }; 20 | 21 | int getSetSize(int thisId) { return data[getRepresentative(thisId)].size; } 22 | 23 | int getRepresentative(int thisId); 24 | 25 | //! Returns the id of the merged node. 26 | /* @param aId 27 | * @param bId 28 | */ 29 | int connectNodes(int aId, int bId); 30 | 31 | void printDataVector() const; 32 | 33 | private: 34 | void init(); 35 | 36 | std::vector data; 37 | }; 38 | 39 | } // namespace 40 | 41 | #endif 42 | -------------------------------------------------------------------------------- /thirdparty/apriltag/ethz_apriltag2/include/apriltags/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) : 13 | x(xval), y(yval), weight(weightval) {} 14 | 15 | }; 16 | 17 | } // namespace 18 | 19 | #endif 20 | -------------------------------------------------------------------------------- /thirdparty/apriltag/ethz_apriltag2/include/apriltags/pch.h: -------------------------------------------------------------------------------- 1 | #include "apriltags/Edge.h" 2 | #include "apriltags/FloatImage.h" 3 | #include "apriltags/GLine2D.h" 4 | //#include "apriltags/GLineSegment2D.h" 5 | #include "apriltags/Gaussian.h" 6 | //#include "apriltags/GrayModel.h" 7 | //#include "apriltags/Gridder.h" 8 | #include "apriltags/Homography33.h" 9 | #include "apriltags/MathUtil.h" 10 | //#include "apriltags/Quad.h" 11 | //#include "apriltags/Segment.h" 12 | //#include "apriltags/TagDetection.h" 13 | //#include "apriltags/TagDetector.h" 14 | //#include "apriltags/TagFamily.h" 15 | #include "apriltags/UnionFindSimple.h" 16 | #include "apriltags/XYWeight.h" 17 | 18 | -------------------------------------------------------------------------------- /thirdparty/apriltag/ethz_apriltag2/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | ethz_apriltag2 4 | ethz_apriltag2 5 | 0.0.1 6 | Paul Furgale 7 | Paul Furgale 8 | New BSD 9 | catkin 10 | cmake_modules 11 | opencv 12 | eigen2 13 | 14 | -------------------------------------------------------------------------------- /thirdparty/apriltag/ethz_apriltag2/src/FloatImage.cc: -------------------------------------------------------------------------------- 1 | #include "apriltags/FloatImage.h" 2 | #include "apriltags/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()) 19 | pixels.resize(other.pixels.size()); 20 | pixels = other.pixels; 21 | return *this; 22 | } 23 | 24 | void FloatImage::decimateAvg() { 25 | int nWidth = width/2; 26 | int nHeight = height/2; 27 | 28 | for (int y = 0; y < nHeight; y++) 29 | for (int x = 0; x < nWidth; x++) 30 | pixels[y*nWidth+x] = pixels[(2*y)*width + (2*x)]; 31 | 32 | width = nWidth; 33 | height = nHeight; 34 | pixels.resize(nWidth*nHeight); 35 | } 36 | 37 | void FloatImage::normalize() { 38 | const float maxVal = *max_element(pixels.begin(),pixels.end()); 39 | const float minVal = *min_element(pixels.begin(),pixels.end()); 40 | const float range = maxVal - minVal; 41 | const float rescale = 1 / range; 42 | for ( unsigned int i = 0; i < pixels.size(); i++ ) 43 | pixels[i] = (pixels[i]-minVal) * rescale; 44 | } 45 | 46 | void FloatImage::filterFactoredCentered(const std::vector& fhoriz, const std::vector& fvert) { 47 | // do horizontal 48 | std::vector r(pixels); 49 | 50 | for (int y = 0; y < height; y++) { 51 | Gaussian::convolveSymmetricCentered(pixels, y*width, width, fhoriz, r, y*width); 52 | } 53 | 54 | // do vertical 55 | std::vector tmp(height); // column before convolution 56 | std::vector tmp2(height); // column after convolution 57 | 58 | for (int x = 0; x < width; x++) { 59 | 60 | // copy the column out for locality 61 | for (int y = 0; y < height; y++) 62 | tmp[y] = r[y*width + x]; 63 | 64 | Gaussian::convolveSymmetricCentered(tmp, 0, height, fvert, tmp2, 0); 65 | 66 | for (int y = 0; y < height; y++) 67 | pixels[y*width + x] = tmp2[y]; 68 | } 69 | } 70 | 71 | void FloatImage::printMinMax() const { 72 | std::cout << "Min: " << *min_element(pixels.begin(),pixels.end()) << ", Max: " << *max_element(pixels.begin(),pixels.end()) << std::endl; 73 | //for (int i = 0; i < getNumFloatImagePixels(); i++) 74 | // std::cout << "Index[" << i << "]: " << this->normalize().getFloatImagePixels()[i] << endl; 75 | } 76 | 77 | } // namespace 78 | -------------------------------------------------------------------------------- /thirdparty/apriltag/ethz_apriltag2/src/GLineSegment2D.cc: -------------------------------------------------------------------------------- 1 | #include "apriltags/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 | for (unsigned int i = 0; i < xyweight.size(); i++) { 15 | std::pair p(xyweight[i].x, xyweight[i].y); 16 | float coord = gline.getLineCoordinate(p); 17 | maxcoord = std::max(maxcoord, coord); 18 | mincoord = std::min(mincoord, coord); 19 | } 20 | 21 | std::pair minValue = gline.getPointOfCoordinate(mincoord); 22 | std::pair maxValue = gline.getPointOfCoordinate(maxcoord); 23 | return GLineSegment2D(minValue,maxValue); 24 | } 25 | 26 | } // namespace 27 | -------------------------------------------------------------------------------- /thirdparty/apriltag/ethz_apriltag2/src/Gaussian.cc: -------------------------------------------------------------------------------- 1 | #include "apriltags/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++) 13 | f[i] = 0; 14 | f[n/2] = 1; 15 | return f; 16 | } 17 | 18 | double const inv_variance = 1./(2*sigma*sigma); 19 | float sum = 0; 20 | for (int i = 0; i < n; i++) { 21 | int j = i - n/2; 22 | f[i] = (float)std::exp(-j*j * inv_variance); 23 | sum += f[i]; 24 | } 25 | 26 | // normalize the gaussian 27 | for (int i = 0; i < n; i++) 28 | f[i] /= sum; 29 | 30 | return f; 31 | } 32 | 33 | void Gaussian::convolveSymmetricCentered(const std::vector& a, unsigned int aoff, unsigned int alen, 34 | const std::vector& f, std::vector& r, unsigned int roff) { 35 | if ((f.size()&1)== 0 && !warned) { 36 | std::cout<<"convolveSymmetricCentered Warning: filter is not odd length\n"; 37 | warned = true; 38 | } 39 | 40 | for (size_t i = f.size()/2; i < f.size(); i++) { 41 | double acc = 0; 42 | for (size_t j = 0; j < f.size(); j++) { 43 | if ((aoff + i) < j || (aoff + i) >= (alen + j)) 44 | acc += a[aoff] * f[j]; 45 | else 46 | acc += a[aoff + i - j] * f[j]; 47 | } 48 | r[roff + i - f.size()/2] = (float)acc; 49 | } 50 | 51 | for (size_t i = f.size(); i < alen; i++) { 52 | double acc = 0; 53 | for (unsigned int j = 0; j < f.size(); j++) { 54 | acc += a[aoff + i - j] * f[j]; 55 | } 56 | r[roff + i - f.size()/2] = (float)acc; 57 | } 58 | 59 | for (size_t i = alen; i < alen + f.size()/2; i++) { 60 | double acc = 0; 61 | for (size_t j = 0; j < f.size(); j++) { 62 | if ((aoff + i) >= (alen + j) || (aoff + i) < j) 63 | acc += a[aoff + alen - 1] * f[j]; 64 | else 65 | acc += a[aoff + i - j] * f[j]; 66 | } 67 | r[roff + i - f.size()/2] = (float)acc; 68 | } 69 | } 70 | 71 | } // namespace 72 | -------------------------------------------------------------------------------- /thirdparty/apriltag/ethz_apriltag2/src/GrayModel.cc: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include 4 | #include 5 | 6 | #include "apriltags/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++) 57 | A(j,i) = A(i,j); 58 | 59 | // try { 60 | // Ainv = A.inverse(); 61 | bool invertible; 62 | double det_unused; 63 | A.computeInverseAndDetWithCheck(Ainv, det_unused, invertible); 64 | if (invertible) { 65 | v = Ainv * b; 66 | return; 67 | } 68 | std::cerr << "AprilTags::GrayModel::compute() has underflow in matrix inverse\n"; 69 | // } 70 | // catch (std::underflow_error&) { 71 | // std::cerr << "AprilTags::GrayModel::compute() has underflow in matrix inverse\n"; 72 | // } 73 | } 74 | 75 | // If we get here, either nobs < 6 or the matrix inverse generated 76 | // an underflow, so use a constant model. 77 | v.setZero(); // need the cast to avoid operator= ambiguity wrt. const-ness 78 | v[3] = b[3] / nobs; 79 | } 80 | 81 | } // namespace 82 | -------------------------------------------------------------------------------- /thirdparty/apriltag/ethz_apriltag2/src/MathUtil.cc: -------------------------------------------------------------------------------- 1 | #include "apriltags/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 12 | -------------------------------------------------------------------------------- /thirdparty/apriltag/ethz_apriltag2/src/Segment.cc: -------------------------------------------------------------------------------- 1 | #include "apriltags/Segment.h" 2 | #include 3 | 4 | namespace AprilTags { 5 | 6 | const float Segment::minimumLineLength = 4; 7 | 8 | Segment::Segment() 9 | : children(), x0(0), y0(0), x1(0), y1(0), theta(0), length(0), segmentId(++idCounter) {} 10 | 11 | float Segment::segmentLength() { 12 | return std::sqrt((x1-x0)*(x1-x0) + (y1-y0)*(y1-y0)); 13 | } 14 | 15 | void Segment::printSegment() { 16 | std::cout <<"("<< x0 <<","<< y0 <<"), "<<"("<< x1 <<","<< y1 <<")" << std::endl; 17 | } 18 | 19 | int Segment::idCounter = 0; 20 | 21 | } // namespace 22 | -------------------------------------------------------------------------------- /thirdparty/apriltag/ethz_apriltag2/src/UnionFindSimple.cc: -------------------------------------------------------------------------------- 1 | #include "apriltags/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) 9 | return thisId; 10 | 11 | // otherwise, recurse... 12 | int root = getRepresentative(data[thisId].id); 13 | 14 | // short circuit the path 15 | data[thisId].id = root; 16 | 17 | return root; 18 | } 19 | 20 | void UnionFindSimple::printDataVector() const { 21 | for (unsigned int i = 0; i < data.size(); i++) 22 | std::cout << "data[" << i << "]: " << " 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) 30 | return aRoot; 31 | 32 | int asz = data[aRoot].size; 33 | int bsz = data[bRoot].size; 34 | 35 | if (asz > bsz) { 36 | data[bRoot].id = aRoot; 37 | data[aRoot].size += bsz; 38 | return aRoot; 39 | } else { 40 | data[aRoot].id = bRoot; 41 | data[bRoot].size += asz; 42 | return bRoot; 43 | } 44 | } 45 | 46 | void UnionFindSimple::init() { 47 | for (unsigned int i = 0; i < data.size(); i++) { 48 | // everyone is their own cluster of size 1 49 | data[i].id = i; 50 | data[i].size = 1; 51 | } 52 | } 53 | 54 | } // namespace 55 | -------------------------------------------------------------------------------- /thirdparty/apriltag/ethz_apriltag2/src/example/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required (VERSION 2.6) 2 | 3 | link_libraries(apriltags) 4 | 5 | add_executable(apriltags_demo apriltags_demo.cpp Serial.cpp) 6 | pods_install_executables(apriltags_demo) 7 | 8 | add_executable(imu imu.cpp Serial.cpp) 9 | pods_install_executables(imu) 10 | -------------------------------------------------------------------------------- /thirdparty/apriltag/ethz_apriltag2/src/example/Serial.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file Serial.cpp 3 | * @brief Simple serial interface, for example to talk to Arduino. 4 | * @author: Michael Kaess 5 | */ 6 | 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | 14 | #include "Serial.h" 15 | 16 | using namespace std; 17 | 18 | 19 | // open a serial port connection 20 | void Serial::open(const string& port, int rate) { 21 | m_serialPort = ::open(port.c_str(), O_RDWR | O_NOCTTY | O_NDELAY); 22 | if (m_serialPort==-1) { 23 | cout << "Unable to open serial port" << endl; 24 | exit(1); 25 | } 26 | fcntl(m_serialPort, F_SETFL,0); // O_NONBLOCK might be needed for write... 27 | 28 | struct termios port_settings; // structure to store the port settings in 29 | tcgetattr(m_serialPort, &port_settings); // get current settings 30 | 31 | speed_t b; 32 | switch(rate) { 33 | case(9600): 34 | b = B9600; 35 | break; 36 | case(19200): 37 | b = B19200; 38 | break; 39 | case(38400): 40 | b = B38400; 41 | break; 42 | case(115200): 43 | b = B115200; 44 | break; 45 | default: 46 | cout << "Error: Unknown baud rate requested in Serial.open()" << endl; 47 | exit(1); 48 | } 49 | 50 | cfsetispeed(&port_settings, b); // set baud rates 51 | cfsetospeed(&port_settings, b); 52 | 53 | port_settings.c_cflag &= ~PARENB; // set no parity, stop bits, 8 data bits 54 | port_settings.c_cflag &= ~CSTOPB; 55 | port_settings.c_cflag &= ~CSIZE; 56 | port_settings.c_cflag |= CS8; 57 | 58 | tcsetattr(m_serialPort, TCSANOW, &port_settings); // apply the settings to the port 59 | } 60 | 61 | // read a single character 62 | int Serial::read() const { 63 | unsigned char result; 64 | if (::read(m_serialPort, &result, 1) == 1) { 65 | return (int)result; 66 | } else { 67 | return -1; 68 | } 69 | } 70 | 71 | // read until special character up to a maximum number of bytes 72 | string Serial::readBytesUntil(unsigned char until, int max_length) { 73 | string result(max_length, ' '); 74 | int n = 0; 75 | int c; 76 | do { 77 | c = read(); 78 | if (c<0) { // wait for more characters 79 | usleep(100); 80 | } else { 81 | result[n] = (unsigned char)c; 82 | n++; 83 | } 84 | } while ((c != (int)until) && (n < max_length)); 85 | result.resize(n); 86 | return result; 87 | } 88 | 89 | // send a string 90 | void Serial::print(string str) const { 91 | int res = ::write(m_serialPort, str.c_str(), str.length()); 92 | } 93 | 94 | // send an integer 95 | void Serial::print(int num) const { 96 | stringstream stream; 97 | stream << num; 98 | string str = stream.str(); 99 | print(str); 100 | } 101 | 102 | // send a double 103 | void Serial::print(double num) const { 104 | stringstream stream; 105 | stream << num; 106 | string str = stream.str(); 107 | print(str); 108 | } 109 | 110 | // send a float 111 | void Serial::print(float num) const { 112 | print((double)num); 113 | } 114 | -------------------------------------------------------------------------------- /thirdparty/apriltag/ethz_apriltag2/src/example/Serial.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file Serial.h 3 | * @brief Simple serial interface, for example to talk to Arduino. 4 | * @author: Michael Kaess 5 | */ 6 | 7 | #pragma once 8 | 9 | #include 10 | 11 | 12 | class Serial { 13 | 14 | int m_serialPort; // file description for the serial port 15 | 16 | public: 17 | 18 | Serial() : m_serialPort(-1) {} 19 | 20 | // open a serial port connection 21 | void open(const std::string& port, int rate = 115200); 22 | 23 | // read a single character 24 | int read() const; 25 | 26 | // read until special character up to a maximum number of bytes 27 | std::string readBytesUntil(unsigned char until, int length = 300); 28 | 29 | // send a string 30 | void print(std::string str) const; 31 | 32 | // send an integer 33 | void print(int num) const; 34 | 35 | // send a double 36 | void print(double num) const; 37 | 38 | // send a float 39 | void print(float num) const; 40 | 41 | }; 42 | -------------------------------------------------------------------------------- /thirdparty/apriltag/ethz_apriltag2/src/example/arduino_tags/arduino_tags.ino: -------------------------------------------------------------------------------- 1 | /* 2 | ArduinoTag 3 | Turns on an LED whenever Raspberry Pi sees an Apriltag. 4 | 5 | Michael Kaess 04/13 6 | */ 7 | 8 | // pin number for LED 9 | int led = 9; 10 | 11 | // information about detected tag 12 | int tagId; 13 | float x, y, z; 14 | 15 | // runs once when the program starts 16 | void setup() { 17 | // open serial port 18 | Serial.begin(115200); 19 | // initialize pin as output 20 | pinMode(led, OUTPUT); 21 | } 22 | 23 | // runs over and over again 24 | void loop() { 25 | // check if new data is available 26 | if (Serial.available() > 0) { 27 | tagId = Serial.parseInt(); 28 | x = Serial.parseFloat(); 29 | y = Serial.parseFloat(); 30 | z = Serial.parseFloat(); 31 | Serial.read(); // ignore newline character 32 | if (tagId >= 0) { 33 | digitalWrite(led, HIGH); 34 | } else { 35 | digitalWrite(led, LOW); 36 | } 37 | } 38 | // wait for 20ms before checking again for new data 39 | delay(20); 40 | } 41 | -------------------------------------------------------------------------------- /thirdparty/apriltag/ethz_apriltag2/src/example/imu.cpp: -------------------------------------------------------------------------------- 1 | // Example of reading from ArduIMU (v3) via serial port 2 | // Michael Kaess, May 2013 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | #include "Serial.h" 10 | 11 | using namespace std; 12 | 13 | 14 | void parse(string s) { 15 | // note that ex (the rotation matrix) and mg? (the magnetormeter 16 | // readings) below are optional, depending on how the ArduIMU was 17 | // configured 18 | 19 | float version; // ArduIMU code version 20 | float gyroX, gyroY, gyroZ; // raw gyroscope values 21 | float accelX, accelY, accelZ; // raw accelerometer values 22 | // int ex[9]; // rotation matrix (scaled by 10^7) 23 | float roll, pitch, yaw; // Euler angles 24 | int imuh; // IMU health 25 | // float mgx, mgy, mgz, mgh; // magnetometer readings 26 | int tow; // GPS time 27 | 28 | // cout << s; 29 | 30 | // try to parse the line 31 | int num = sscanf(s.c_str(), 32 | //"!!!VER:%g,AN0:%g,AN1:%g,AN2:%g,AN3:%g,AN4:%g,AN5:%g,EX0:%i,EX1:%i,EX2:%i,EX3:%i,EX4:%i,EX5:%i,EX6:%i,EX7:%i,EX8:%i,RLL:%g,PCH:%g,YAW:%g,IMUH:%i,TOW:%i***", 33 | "!!!VER:%g,AN0:%g,AN1:%g,AN2:%g,AN3:%g,AN4:%g,AN5:%g,RLL:%g,PCH:%g,YAW:%g,IMUH:%i,TOW:%i", 34 | //"!!!VER:%g,AN0:%g,AN1:%g,AN2:%g,AN3:%g,AN4:%g,AN5:%g,RLL:%g,PCH:%g,YAW:%g,IMUH:%i,MGX:%g,MGY:%g,MGZ:%g,MGH:%g,TOW:%i", 35 | &version, &gyroX, &gyroY, &gyroZ, &accelX, &accelY, &accelZ, 36 | // &ex[0], &ex[1], &ex[2], &ex[3], &ex[4], &ex[5], &ex[6], &ex[7], &ex[8], 37 | &roll, &pitch, &yaw, 38 | &imuh, 39 | // &mgx, &mgy, &mgz, &mgh, 40 | &tow); 41 | 42 | // did we read the correct number of entries, or did the line contain other information? 43 | if (num==12 || num==16 || num==21) { 44 | cout << "Euler angles: " << yaw << "," << pitch << "," << roll << endl; 45 | } else { 46 | // cout << "Could not parse string " << num << endl; 47 | } 48 | 49 | } 50 | 51 | 52 | int main() { 53 | 54 | Serial serial; 55 | serial.open("/dev/ttyUSB0", 38400); // might need to change to your USB port 56 | 57 | // read and parse one line at a time 58 | while (true) { 59 | string s = serial.readBytesUntil('\n'); 60 | parse(s); 61 | } 62 | 63 | return 0; 64 | } 65 | -------------------------------------------------------------------------------- /thirdparty/apriltag/include/basalt/utils/apriltag.h: -------------------------------------------------------------------------------- 1 | 2 | #include 3 | #include 4 | #include 5 | 6 | namespace basalt { 7 | 8 | struct ApriltagDetectorData; 9 | 10 | class ApriltagDetector { 11 | public: 12 | ApriltagDetector(int numTags); 13 | 14 | ~ApriltagDetector(); 15 | 16 | void detectTags(basalt::ManagedImage& img_raw, 17 | Eigen::aligned_vector& corners, 18 | std::vector& ids, std::vector& radii, 19 | Eigen::aligned_vector& corners_rejected, 20 | std::vector& ids_rejected, 21 | std::vector& radii_rejected); 22 | 23 | private: 24 | ApriltagDetectorData* data; 25 | }; 26 | 27 | } // namespace basalt 28 | -------------------------------------------------------------------------------- /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 | -------------------------------------------------------------------------------- /thirdparty/ros/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.2) 2 | 3 | 4 | SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -fPIC -Wno-unused-parameter -Wno-deprecated-declarations") 5 | SET(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -fPIC -Wno-deprecated-declarations") 6 | 7 | if (CMAKE_CXX_COMPILER_ID STREQUAL "GNU" AND CMAKE_CXX_COMPILER_VERSION VERSION_GREATER_EQUAL 9) 8 | SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wno-catch-value") 9 | endif() 10 | 11 | #find_package(catkin REQUIRED COMPONENTS cpp_common roscpp_serialization roscpp_traits rostime roslz4) 12 | find_package(Boost REQUIRED COMPONENTS date_time filesystem program_options regex) 13 | find_package(BZip2 REQUIRED) 14 | find_library(lz4_LIBRARIES NAMES lz4) 15 | 16 | file(GLOB CONSOLE_BRIDGE_SRCS "${CMAKE_CURRENT_SOURCE_DIR}/console_bridge/src/*.cpp") 17 | file(GLOB CPP_COMMON_SRCS "${CMAKE_CURRENT_SOURCE_DIR}/roscpp_core/cpp_common/src/*.cpp") 18 | file(GLOB ROSCPP_SERIALIZATION_SRCS "${CMAKE_CURRENT_SOURCE_DIR}/roscpp_core/roscpp_serialization/src/*.cpp") 19 | file(GLOB ROSTIME_SRCS "${CMAKE_CURRENT_SOURCE_DIR}/roscpp_core/rostime/src/*.cpp") 20 | 21 | file(GLOB ROSBAG_SRCS "${CMAKE_CURRENT_SOURCE_DIR}/ros_comm/tools/rosbag_storage/src/*.cpp") 22 | file(GLOB ROSLZ4_SRCS "${CMAKE_CURRENT_SOURCE_DIR}/ros_comm/utilities/roslz4/src/[a-z]*.c") 23 | 24 | add_library(rosbag STATIC ${ROSBAG_SRCS} ${ROSTIME_SRCS} ${CPP_COMMON_SRCS} ${ROSCPP_SERIALIZATION_SRCS} ${ROSLZ4_SRCS} ${CONSOLE_BRIDGE_SRCS}) 25 | 26 | target_include_directories(rosbag PUBLIC 27 | include 28 | console_bridge/include 29 | roscpp_core/cpp_common/include 30 | roscpp_core/rostime/include 31 | roscpp_core/roscpp_serialization/include 32 | roscpp_core/roscpp_traits/include 33 | ros_comm/utilities/roslz4/include 34 | ros_comm/tools/rosbag_storage/include) 35 | 36 | target_link_libraries(rosbag PUBLIC ${Boost_LIBRARIES} ${BZIP2_LIBRARIES} ${lz4_LIBRARIES}) 37 | 38 | -------------------------------------------------------------------------------- /thirdparty/ros/include/console_bridge_export.h: -------------------------------------------------------------------------------- 1 | 2 | #ifndef CONSOLE_BRIDGE_DLLAPI_H 3 | #define CONSOLE_BRIDGE_DLLAPI_H 4 | 5 | #ifdef CONSOLE_BRIDGE_STATIC_DEFINE 6 | # define CONSOLE_BRIDGE_DLLAPI 7 | # define CONSOLE_BRIDGE_NO_EXPORT 8 | #else 9 | # ifndef CONSOLE_BRIDGE_DLLAPI 10 | # ifdef console_bridge_EXPORTS 11 | /* We are building this library */ 12 | # define CONSOLE_BRIDGE_DLLAPI __attribute__((visibility("default"))) 13 | # else 14 | /* We are using this library */ 15 | # define CONSOLE_BRIDGE_DLLAPI __attribute__((visibility("default"))) 16 | # endif 17 | # endif 18 | 19 | # ifndef CONSOLE_BRIDGE_NO_EXPORT 20 | # define CONSOLE_BRIDGE_NO_EXPORT __attribute__((visibility("hidden"))) 21 | # endif 22 | #endif 23 | 24 | #ifndef CONSOLE_BRIDGE_DEPRECATED 25 | # define CONSOLE_BRIDGE_DEPRECATED __attribute__ ((__deprecated__)) 26 | #endif 27 | 28 | #ifndef CONSOLE_BRIDGE_DEPRECATED_EXPORT 29 | # define CONSOLE_BRIDGE_DEPRECATED_EXPORT CONSOLE_BRIDGE_DLLAPI CONSOLE_BRIDGE_DEPRECATED 30 | #endif 31 | 32 | #ifndef CONSOLE_BRIDGE_DEPRECATED_NO_EXPORT 33 | # define CONSOLE_BRIDGE_DEPRECATED_NO_EXPORT CONSOLE_BRIDGE_NO_EXPORT CONSOLE_BRIDGE_DEPRECATED 34 | #endif 35 | 36 | #define DEFINE_NO_DEPRECATED 0 37 | #if DEFINE_NO_DEPRECATED 38 | # define CONSOLE_BRIDGE_NO_DEPRECATED 39 | #endif 40 | 41 | #endif 42 | -------------------------------------------------------------------------------- /thirdparty/ros/include/sensor_msgs/distortion_models.h: -------------------------------------------------------------------------------- 1 | 2 | /********************************************************************* 3 | * Software License Agreement (BSD License) 4 | * 5 | * Copyright (c) 2010, Willow Garage, Inc. 6 | * All rights reserved. 7 | * 8 | * Redistribution and use in source and binary forms, with or without 9 | * modification, are permitted provided that the following conditions 10 | * are met: 11 | * 12 | * * Redistributions of source code must retain the above copyright 13 | * notice, this list of conditions and the following disclaimer. 14 | * * Redistributions in binary form must reproduce the above 15 | * copyright notice, this list of conditions and the following 16 | * disclaimer in the documentation and/or other materials provided 17 | * with the distribution. 18 | * * Neither the name of the Willow Garage nor the names of its 19 | * contributors may be used to endorse or promote products derived 20 | * from this software without specific prior written permission. 21 | * 22 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 23 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 24 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 25 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 26 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 27 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 28 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 29 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 30 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 31 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 32 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 33 | * POSSIBILITY OF SUCH DAMAGE. 34 | *********************************************************************/ 35 | 36 | #ifndef SENSOR_MSGS_DISTORTION_MODELS_H 37 | #define SENSOR_MSGS_DISTORTION_MODELS_H 38 | 39 | #include 40 | 41 | namespace sensor_msgs 42 | { 43 | namespace distortion_models 44 | { 45 | const std::string PLUMB_BOB = "plumb_bob"; 46 | const std::string RATIONAL_POLYNOMIAL = "rational_polynomial"; 47 | const std::string EQUIDISTANT = "equidistant"; 48 | } 49 | } 50 | 51 | #endif 52 | -------------------------------------------------------------------------------- /thirdparty/ros/include/sensor_msgs/fill_image.h: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2008, Willow Garage, Inc. 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of the Willow Garage nor the names of its 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | *********************************************************************/ 34 | 35 | #ifndef FILLIMAGE_HH 36 | #define FILLIMAGE_HH 37 | 38 | #include "sensor_msgs/Image.h" 39 | #include "sensor_msgs/image_encodings.h" 40 | 41 | namespace sensor_msgs 42 | { 43 | 44 | static inline bool fillImage(Image& image, 45 | const std::string& encoding_arg, 46 | uint32_t rows_arg, 47 | uint32_t cols_arg, 48 | uint32_t step_arg, 49 | const void* data_arg) 50 | { 51 | image.encoding = encoding_arg; 52 | image.height = rows_arg; 53 | image.width = cols_arg; 54 | image.step = step_arg; 55 | size_t st0 = (step_arg * rows_arg); 56 | image.data.resize(st0); 57 | memcpy(&image.data[0], data_arg, st0); 58 | 59 | image.is_bigendian = 0; 60 | return true; 61 | } 62 | 63 | static inline void clearImage(Image& image) 64 | { 65 | image.data.resize(0); 66 | } 67 | } 68 | 69 | 70 | #endif 71 | -------------------------------------------------------------------------------- /thirdparty/ros/include/std_msgs/builtin_bool.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2009, Willow Garage, Inc. 3 | * 4 | * Redistribution and use in source and binary forms, with or without 5 | * modification, are permitted provided that the following conditions are met: 6 | * * Redistributions of source code must retain the above copyright notice, 7 | * this list of conditions and the following disclaimer. 8 | * * Redistributions in binary form must reproduce the above copyright 9 | * notice, this list of conditions and the following disclaimer in the 10 | * documentation and/or other materials provided with the distribution. 11 | * * Neither the names of Willow Garage, Inc. nor the names of its 12 | * contributors may be used to endorse or promote products derived from 13 | * this software without specific prior written permission. 14 | * 15 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 16 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 17 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 18 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 19 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 20 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 21 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 22 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 23 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 24 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 25 | * POSSIBILITY OF SUCH DAMAGE. 26 | */ 27 | 28 | #ifndef STD_MSGS_BUILTIN_BOOL_H 29 | #define STD_MSGS_BUILTIN_BOOL_H 30 | 31 | #include "trait_macros.h" 32 | #include 33 | 34 | STD_MSGS_DEFINE_BUILTIN_TRAITS(bool, Bool, 0x8b94c1b53db61fb6ULL, 0xaed406028ad6332aULL) 35 | 36 | #endif 37 | -------------------------------------------------------------------------------- /thirdparty/ros/include/std_msgs/builtin_double.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2009, Willow Garage, Inc. 3 | * 4 | * Redistribution and use in source and binary forms, with or without 5 | * modification, are permitted provided that the following conditions are met: 6 | * * Redistributions of source code must retain the above copyright notice, 7 | * this list of conditions and the following disclaimer. 8 | * * Redistributions in binary form must reproduce the above copyright 9 | * notice, this list of conditions and the following disclaimer in the 10 | * documentation and/or other materials provided with the distribution. 11 | * * Neither the names of Willow Garage, Inc. nor the names of its 12 | * contributors may be used to endorse or promote products derived from 13 | * this software without specific prior written permission. 14 | * 15 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 16 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 17 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 18 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 19 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 20 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 21 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 22 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 23 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 24 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 25 | * POSSIBILITY OF SUCH DAMAGE. 26 | */ 27 | 28 | #ifndef STD_MSGS_BUILTIN_DOUBLE_H 29 | #define STD_MSGS_BUILTIN_DOUBLE_H 30 | 31 | #include "trait_macros.h" 32 | #include 33 | 34 | STD_MSGS_DEFINE_BUILTIN_TRAITS(double, Float64, 0xfdb28210bfa9d7c9ULL, 0x1146260178d9a584ULL) 35 | 36 | #endif 37 | -------------------------------------------------------------------------------- /thirdparty/ros/include/std_msgs/builtin_float.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2009, Willow Garage, Inc. 3 | * 4 | * Redistribution and use in source and binary forms, with or without 5 | * modification, are permitted provided that the following conditions are met: 6 | * * Redistributions of source code must retain the above copyright notice, 7 | * this list of conditions and the following disclaimer. 8 | * * Redistributions in binary form must reproduce the above copyright 9 | * notice, this list of conditions and the following disclaimer in the 10 | * documentation and/or other materials provided with the distribution. 11 | * * Neither the names of Willow Garage, Inc. nor the names of its 12 | * contributors may be used to endorse or promote products derived from 13 | * this software without specific prior written permission. 14 | * 15 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 16 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 17 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 18 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 19 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 20 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 21 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 22 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 23 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 24 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 25 | * POSSIBILITY OF SUCH DAMAGE. 26 | */ 27 | 28 | #ifndef STD_MSGS_BUILTIN_FLOAT_H 29 | #define STD_MSGS_BUILTIN_FLOAT_H 30 | 31 | #include "trait_macros.h" 32 | #include 33 | 34 | STD_MSGS_DEFINE_BUILTIN_TRAITS(float, Float32, 0x73fcbf46b49191e6ULL, 0x72908e50842a83d4ULL) 35 | 36 | #endif 37 | -------------------------------------------------------------------------------- /thirdparty/ros/include/std_msgs/builtin_int16.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2009, Willow Garage, Inc. 3 | * 4 | * Redistribution and use in source and binary forms, with or without 5 | * modification, are permitted provided that the following conditions are met: 6 | * * Redistributions of source code must retain the above copyright notice, 7 | * this list of conditions and the following disclaimer. 8 | * * Redistributions in binary form must reproduce the above copyright 9 | * notice, this list of conditions and the following disclaimer in the 10 | * documentation and/or other materials provided with the distribution. 11 | * * Neither the names of Willow Garage, Inc. nor the names of its 12 | * contributors may be used to endorse or promote products derived from 13 | * this software without specific prior written permission. 14 | * 15 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 16 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 17 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 18 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 19 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 20 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 21 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 22 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 23 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 24 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 25 | * POSSIBILITY OF SUCH DAMAGE. 26 | */ 27 | 28 | #ifndef STD_MSGS_BUILTIN_INT16_H 29 | #define STD_MSGS_BUILTIN_INT16_H 30 | 31 | #include "trait_macros.h" 32 | #include 33 | 34 | STD_MSGS_DEFINE_BUILTIN_TRAITS(int16_t, Int16, 0x8524586e34fbd7cbULL, 0x1c08c5f5f1ca0e57ULL) 35 | 36 | #endif 37 | -------------------------------------------------------------------------------- /thirdparty/ros/include/std_msgs/builtin_int32.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2009, Willow Garage, Inc. 3 | * 4 | * Redistribution and use in source and binary forms, with or without 5 | * modification, are permitted provided that the following conditions are met: 6 | * * Redistributions of source code must retain the above copyright notice, 7 | * this list of conditions and the following disclaimer. 8 | * * Redistributions in binary form must reproduce the above copyright 9 | * notice, this list of conditions and the following disclaimer in the 10 | * documentation and/or other materials provided with the distribution. 11 | * * Neither the names of Willow Garage, Inc. nor the names of its 12 | * contributors may be used to endorse or promote products derived from 13 | * this software without specific prior written permission. 14 | * 15 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 16 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 17 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 18 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 19 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 20 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 21 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 22 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 23 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 24 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 25 | * POSSIBILITY OF SUCH DAMAGE. 26 | */ 27 | 28 | #ifndef STD_MSGS_BUILTIN_INT32_H 29 | #define STD_MSGS_BUILTIN_INT32_H 30 | 31 | #include "trait_macros.h" 32 | #include 33 | 34 | STD_MSGS_DEFINE_BUILTIN_TRAITS(int32_t, Int32, 0xda5909fbe378aeafULL, 0x85e547e830cc1bb7ULL) 35 | 36 | #endif 37 | -------------------------------------------------------------------------------- /thirdparty/ros/include/std_msgs/builtin_int64.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2009, Willow Garage, Inc. 3 | * 4 | * Redistribution and use in source and binary forms, with or without 5 | * modification, are permitted provided that the following conditions are met: 6 | * * Redistributions of source code must retain the above copyright notice, 7 | * this list of conditions and the following disclaimer. 8 | * * Redistributions in binary form must reproduce the above copyright 9 | * notice, this list of conditions and the following disclaimer in the 10 | * documentation and/or other materials provided with the distribution. 11 | * * Neither the names of Willow Garage, Inc. nor the names of its 12 | * contributors may be used to endorse or promote products derived from 13 | * this software without specific prior written permission. 14 | * 15 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 16 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 17 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 18 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 19 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 20 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 21 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 22 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 23 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 24 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 25 | * POSSIBILITY OF SUCH DAMAGE. 26 | */ 27 | 28 | #ifndef STD_MSGS_BUILTIN_INT64_H 29 | #define STD_MSGS_BUILTIN_INT64_H 30 | 31 | #include "trait_macros.h" 32 | #include 33 | 34 | STD_MSGS_DEFINE_BUILTIN_TRAITS(int64_t, Int64, 0x34add168574510e6ULL, 0xe17f5d23ecc077efULL) 35 | 36 | #endif 37 | -------------------------------------------------------------------------------- /thirdparty/ros/include/std_msgs/builtin_int8.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2009, Willow Garage, Inc. 3 | * 4 | * Redistribution and use in source and binary forms, with or without 5 | * modification, are permitted provided that the following conditions are met: 6 | * * Redistributions of source code must retain the above copyright notice, 7 | * this list of conditions and the following disclaimer. 8 | * * Redistributions in binary form must reproduce the above copyright 9 | * notice, this list of conditions and the following disclaimer in the 10 | * documentation and/or other materials provided with the distribution. 11 | * * Neither the names of Willow Garage, Inc. nor the names of its 12 | * contributors may be used to endorse or promote products derived from 13 | * this software without specific prior written permission. 14 | * 15 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 16 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 17 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 18 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 19 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 20 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 21 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 22 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 23 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 24 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 25 | * POSSIBILITY OF SUCH DAMAGE. 26 | */ 27 | 28 | #ifndef STD_MSGS_BUILTIN_INT8_H 29 | #define STD_MSGS_BUILTIN_INT8_H 30 | 31 | #include "trait_macros.h" 32 | #include 33 | 34 | STD_MSGS_DEFINE_BUILTIN_TRAITS(int8_t, Int8, 0x27ffa0c9c4b8fb84ULL, 0x92252bcad9e5c57bULL) 35 | 36 | #endif 37 | -------------------------------------------------------------------------------- /thirdparty/ros/include/std_msgs/builtin_string.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2009, Willow Garage, Inc. 3 | * 4 | * Redistribution and use in source and binary forms, with or without 5 | * modification, are permitted provided that the following conditions are met: 6 | * * Redistributions of source code must retain the above copyright notice, 7 | * this list of conditions and the following disclaimer. 8 | * * Redistributions in binary form must reproduce the above copyright 9 | * notice, this list of conditions and the following disclaimer in the 10 | * documentation and/or other materials provided with the distribution. 11 | * * Neither the names of Willow Garage, Inc. nor the names of its 12 | * contributors may be used to endorse or promote products derived from 13 | * this software without specific prior written permission. 14 | * 15 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 16 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 17 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 18 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 19 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 20 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 21 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 22 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 23 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 24 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 25 | * POSSIBILITY OF SUCH DAMAGE. 26 | */ 27 | 28 | #ifndef STD_MSGS_BUILTIN_STRING_H 29 | #define STD_MSGS_BUILTIN_STRING_H 30 | 31 | #include "trait_macros.h" 32 | #include 33 | 34 | namespace ros 35 | { 36 | namespace message_traits 37 | { 38 | 39 | template 40 | struct MD5Sum, ContainerAllocator> > 41 | { 42 | static const char* value() 43 | { 44 | ROS_STATIC_ASSERT(MD5Sum::static_value1 == 0x992ce8a1687cec8cULL); 45 | ROS_STATIC_ASSERT(MD5Sum::static_value2 == 0x8bd883ec73ca41d1ULL); 46 | return MD5Sum >::value(); 47 | } 48 | 49 | static const char* value(const std::basic_string, ContainerAllocator>&) 50 | { 51 | return value(); 52 | } 53 | }; 54 | 55 | template 56 | struct DataType, ContainerAllocator > > 57 | { 58 | static const char* value() 59 | { 60 | return DataType >::value(); 61 | } 62 | 63 | static const char* value(const std::basic_string, ContainerAllocator >&) 64 | { 65 | return value(); 66 | } 67 | }; 68 | 69 | template 70 | struct Definition, ContainerAllocator > > 71 | { 72 | static const char* value() 73 | { 74 | return Definition >::value(); 75 | } 76 | 77 | static const char* value(const std::basic_string, ContainerAllocator >&) 78 | { 79 | return value(); 80 | } 81 | }; 82 | 83 | } 84 | } 85 | 86 | #endif 87 | -------------------------------------------------------------------------------- /thirdparty/ros/include/std_msgs/builtin_uint16.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2009, Willow Garage, Inc. 3 | * 4 | * Redistribution and use in source and binary forms, with or without 5 | * modification, are permitted provided that the following conditions are met: 6 | * * Redistributions of source code must retain the above copyright notice, 7 | * this list of conditions and the following disclaimer. 8 | * * Redistributions in binary form must reproduce the above copyright 9 | * notice, this list of conditions and the following disclaimer in the 10 | * documentation and/or other materials provided with the distribution. 11 | * * Neither the names of Willow Garage, Inc. nor the names of its 12 | * contributors may be used to endorse or promote products derived from 13 | * this software without specific prior written permission. 14 | * 15 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 16 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 17 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 18 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 19 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 20 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 21 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 22 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 23 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 24 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 25 | * POSSIBILITY OF SUCH DAMAGE. 26 | */ 27 | 28 | #ifndef STD_MSGS_BUILTIN_UINT16_H 29 | #define STD_MSGS_BUILTIN_UINT16_H 30 | 31 | #include "trait_macros.h" 32 | #include 33 | 34 | STD_MSGS_DEFINE_BUILTIN_TRAITS(uint16_t, UInt16, 0x1df79edf208b629fULL, 0xe6b81923a544552dULL) 35 | 36 | #endif 37 | -------------------------------------------------------------------------------- /thirdparty/ros/include/std_msgs/builtin_uint32.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2009, Willow Garage, Inc. 3 | * 4 | * Redistribution and use in source and binary forms, with or without 5 | * modification, are permitted provided that the following conditions are met: 6 | * * Redistributions of source code must retain the above copyright notice, 7 | * this list of conditions and the following disclaimer. 8 | * * Redistributions in binary form must reproduce the above copyright 9 | * notice, this list of conditions and the following disclaimer in the 10 | * documentation and/or other materials provided with the distribution. 11 | * * Neither the names of Willow Garage, Inc. nor the names of its 12 | * contributors may be used to endorse or promote products derived from 13 | * this software without specific prior written permission. 14 | * 15 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 16 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 17 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 18 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 19 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 20 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 21 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 22 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 23 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 24 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 25 | * POSSIBILITY OF SUCH DAMAGE. 26 | */ 27 | 28 | #ifndef STD_MSGS_BUILTIN_UINT32_H 29 | #define STD_MSGS_BUILTIN_UINT32_H 30 | 31 | #include "trait_macros.h" 32 | #include 33 | 34 | STD_MSGS_DEFINE_BUILTIN_TRAITS(uint32_t, UInt32, 0x304a39449588c7f8ULL, 0xce2df6e8001c5fceULL) 35 | 36 | #endif 37 | -------------------------------------------------------------------------------- /thirdparty/ros/include/std_msgs/builtin_uint64.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2009, Willow Garage, Inc. 3 | * 4 | * Redistribution and use in source and binary forms, with or without 5 | * modification, are permitted provided that the following conditions are met: 6 | * * Redistributions of source code must retain the above copyright notice, 7 | * this list of conditions and the following disclaimer. 8 | * * Redistributions in binary form must reproduce the above copyright 9 | * notice, this list of conditions and the following disclaimer in the 10 | * documentation and/or other materials provided with the distribution. 11 | * * Neither the names of Willow Garage, Inc. nor the names of its 12 | * contributors may be used to endorse or promote products derived from 13 | * this software without specific prior written permission. 14 | * 15 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 16 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 17 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 18 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 19 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 20 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 21 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 22 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 23 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 24 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 25 | * POSSIBILITY OF SUCH DAMAGE. 26 | */ 27 | 28 | #ifndef STD_MSGS_BUILTIN_UINT64_H 29 | #define STD_MSGS_BUILTIN_UINT64_H 30 | 31 | #include "trait_macros.h" 32 | #include 33 | 34 | STD_MSGS_DEFINE_BUILTIN_TRAITS(uint64_t, UInt64, 0x1b2a79973e8bf53dULL, 0x7b53acb71299cb57ULL) 35 | 36 | #endif 37 | -------------------------------------------------------------------------------- /thirdparty/ros/include/std_msgs/builtin_uint8.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2009, Willow Garage, Inc. 3 | * 4 | * Redistribution and use in source and binary forms, with or without 5 | * modification, are permitted provided that the following conditions are met: 6 | * * Redistributions of source code must retain the above copyright notice, 7 | * this list of conditions and the following disclaimer. 8 | * * Redistributions in binary form must reproduce the above copyright 9 | * notice, this list of conditions and the following disclaimer in the 10 | * documentation and/or other materials provided with the distribution. 11 | * * Neither the names of Willow Garage, Inc. nor the names of its 12 | * contributors may be used to endorse or promote products derived from 13 | * this software without specific prior written permission. 14 | * 15 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 16 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 17 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 18 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 19 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 20 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 21 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 22 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 23 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 24 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 25 | * POSSIBILITY OF SUCH DAMAGE. 26 | */ 27 | 28 | #ifndef STD_MSGS_BUILTIN_UINT8_H 29 | #define STD_MSGS_BUILTIN_UINT8_H 30 | 31 | #include "trait_macros.h" 32 | #include 33 | 34 | STD_MSGS_DEFINE_BUILTIN_TRAITS(uint8_t, UInt8, 0x7c8164229e7d2c17ULL, 0xeb95e9231617fdeeULL) 35 | 36 | #endif 37 | -------------------------------------------------------------------------------- /thirdparty/ros/include/std_msgs/trait_macros.h: -------------------------------------------------------------------------------- 1 | 2 | /* 3 | * Copyright (C) 2009, Willow Garage, Inc. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * * Redistributions of source code must retain the above copyright notice, 8 | * this list of conditions and the following disclaimer. 9 | * * Redistributions in binary form must reproduce the above copyright 10 | * notice, this list of conditions and the following disclaimer in the 11 | * documentation and/or other materials provided with the distribution. 12 | * * Neither the names of Willow Garage, Inc. nor the names of its 13 | * contributors may be used to endorse or promote products derived from 14 | * this software without specific prior written permission. 15 | * 16 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 17 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 18 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 19 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 20 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 21 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 22 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 23 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 24 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 25 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 26 | * POSSIBILITY OF SUCH DAMAGE. 27 | */ 28 | 29 | #ifndef STD_MSGS_TRAIT_MACROS_H 30 | #define STD_MSGS_TRAIT_MACROS_H 31 | 32 | #define STD_MSGS_DEFINE_BUILTIN_TRAITS(builtin, msg, static_md5sum1, static_md5sum2) \ 33 | namespace ros \ 34 | { \ 35 | namespace message_traits \ 36 | { \ 37 | \ 38 | template<> struct MD5Sum \ 39 | { \ 40 | static const char* value() \ 41 | { \ 42 | return MD5Sum::value(); \ 43 | } \ 44 | \ 45 | static const char* value(const builtin&) \ 46 | { \ 47 | return value(); \ 48 | } \ 49 | }; \ 50 | \ 51 | template<> struct DataType \ 52 | { \ 53 | static const char* value() \ 54 | { \ 55 | return DataType::value(); \ 56 | } \ 57 | \ 58 | static const char* value(const builtin&) \ 59 | { \ 60 | return value(); \ 61 | } \ 62 | }; \ 63 | \ 64 | template<> struct Definition \ 65 | { \ 66 | static const char* value() \ 67 | { \ 68 | return Definition::value(); \ 69 | } \ 70 | \ 71 | static const char* value(const builtin&) \ 72 | { \ 73 | return value(); \ 74 | } \ 75 | }; \ 76 | \ 77 | } \ 78 | } 79 | 80 | #endif // STD_MSGS_TRAIT_MACROS_H 81 | --------------------------------------------------------------------------------