├── .clang-format ├── .cmake-format ├── .github └── workflows │ ├── clang_format.yml │ ├── cmake_format.yml │ └── main.yml ├── .run-clang-format ├── .run-cmake-format ├── LICENSE ├── README.md ├── cal_primer.md ├── ci ├── dependencies.repos ├── docs ├── mod_circle_target_annotated.png └── pinhole_camera_model.png ├── rct_common ├── CHANGELOG.rst ├── CMakeLists.txt ├── include │ └── rct_common │ │ └── print_utils.h └── package.xml ├── rct_examples ├── CHANGELOG.rst ├── CMakeLists.txt ├── README.md ├── data │ ├── positioner_kinematic_measurements.yaml │ ├── test_set_10x10 │ │ ├── cal_data.yaml │ │ ├── camera_intr.yaml │ │ ├── camera_intr_calibrated.yaml │ │ ├── images │ │ │ ├── 0.png │ │ │ ├── 1.png │ │ │ ├── 10.png │ │ │ ├── 11.png │ │ │ ├── 12.png │ │ │ ├── 13.png │ │ │ ├── 14.png │ │ │ ├── 2.png │ │ │ ├── 3.png │ │ │ ├── 4.png │ │ │ ├── 5.png │ │ │ ├── 6.png │ │ │ ├── 7.png │ │ │ ├── 8.png │ │ │ └── 9.png │ │ ├── noise_qualification │ │ │ ├── camera_to_target_guess.yaml │ │ │ └── data.yaml │ │ ├── pose_guesses.yaml │ │ ├── poses │ │ │ ├── 0.yaml │ │ │ ├── 1.yaml │ │ │ ├── 10.yaml │ │ │ ├── 11.yaml │ │ │ ├── 12.yaml │ │ │ ├── 13.yaml │ │ │ ├── 14.yaml │ │ │ ├── 2.yaml │ │ │ ├── 3.yaml │ │ │ ├── 4.yaml │ │ │ ├── 5.yaml │ │ │ ├── 6.yaml │ │ │ ├── 7.yaml │ │ │ ├── 8.yaml │ │ │ └── 9.yaml │ │ └── target.yaml │ └── test_set_charuco │ │ ├── cal_data.yaml │ │ ├── camera_intr.yaml │ │ ├── camera_intr_calibrated.yaml │ │ ├── images │ │ ├── 0.png │ │ ├── 1.png │ │ ├── 10.png │ │ ├── 11.png │ │ ├── 12.png │ │ ├── 13.png │ │ ├── 14.png │ │ ├── 15.png │ │ ├── 2.png │ │ ├── 3.png │ │ ├── 4.png │ │ ├── 5.png │ │ ├── 6.png │ │ ├── 7.png │ │ ├── 8.png │ │ └── 9.png │ │ ├── pose_guesses.yaml │ │ ├── poses │ │ ├── 0.yaml │ │ ├── 1.yaml │ │ ├── 10.yaml │ │ ├── 11.yaml │ │ ├── 12.yaml │ │ ├── 13.yaml │ │ ├── 14.yaml │ │ ├── 15.yaml │ │ ├── 2.yaml │ │ ├── 3.yaml │ │ ├── 4.yaml │ │ ├── 5.yaml │ │ ├── 6.yaml │ │ ├── 7.yaml │ │ ├── 8.yaml │ │ └── 9.yaml │ │ └── target.yaml ├── docs │ ├── moving_camera_cell.png │ └── static_camera_cell.png ├── launch │ ├── camera_intrinsic_calibration_example.launch │ ├── camera_intrinsic_calibration_validation.launch │ ├── camera_on_wrist_example.launch │ ├── kinematic_calibration_example.launch │ ├── multi_static_camera_example.launch │ ├── multi_static_camera_multi_step_example.launch │ ├── noise_qualification_2d.launch │ ├── pnp_example.launch │ ├── solve_multi_static_camera_pnp_example.launch │ └── static_camera_example.launch ├── package.xml └── src │ ├── examples │ ├── camera_on_wrist.cpp │ └── kinematic_calibration.cpp │ └── tools │ ├── camera_intrinsic_calibration_validation.cpp │ ├── camera_on_wrist_extrinsic.cpp │ ├── hand_eye_calibration_analysis.h │ ├── intrinsic_calibration.cpp │ ├── multi_static_camera_extrinsic.cpp │ ├── multi_static_camera_multi_step_extrinsic.cpp │ ├── noise_qualification_2d.cpp │ ├── solve_multi_camera_pnp.cpp │ ├── solve_pnp.cpp │ └── static_camera_extrinsic.cpp ├── rct_image_tools ├── CHANGELOG.rst ├── CMakeLists.txt ├── README.md ├── docs │ ├── input.png │ └── output.png ├── include │ └── rct_image_tools │ │ ├── aruco_finder.h │ │ ├── aruco_grid_target.h │ │ ├── charuco_finder.h │ │ ├── charuco_grid_target.h │ │ ├── circle_detector.h │ │ ├── image_utils.h │ │ ├── modified_circle_grid_finder.h │ │ ├── modified_circle_grid_target.h │ │ ├── target.h │ │ ├── target_features.h │ │ └── target_finder.h ├── package.xml ├── script │ ├── calibration_target.py │ └── example_5x5_15mm_spaces.pdf ├── src │ ├── rct_image_tools │ │ ├── aruco_finder.cpp │ │ ├── aruco_grid_target.cpp │ │ ├── charuco_finder.cpp │ │ ├── charuco_grid_target.cpp │ │ ├── circle_detector.cpp │ │ ├── modified_circle_grid_finder.cpp │ │ └── modified_circle_grid_target.cpp │ └── target_finder_tests.cpp └── test │ ├── CMakeLists.txt │ ├── images │ ├── aruco.jpg │ ├── charuco_obscured.jpg │ ├── charuco_one_corner.jpg │ ├── charuco_unobscured.jpg │ └── modified_circle_grid_10x10_0.025.png │ └── target_finder_utest.cpp ├── rct_optimizations ├── CHANGELOG.rst ├── CMakeLists.txt ├── include │ └── rct_optimizations │ │ ├── ceres_math_utilities.h │ │ ├── circle_fit.h │ │ ├── covariance_analysis.h │ │ ├── covariance_types.h │ │ ├── dh_chain.h │ │ ├── dh_chain_kinematic_calibration.h │ │ ├── eigen_conversions.h │ │ ├── experimental │ │ ├── camera_intrinsic.h │ │ └── multi_camera_pnp.h │ │ ├── extrinsic_hand_eye.h │ │ ├── extrinsic_multi_static_camera.h │ │ ├── extrinsic_multi_static_camera_only.h │ │ ├── extrinsic_multi_static_camera_wrist_only.h │ │ ├── local_parameterization.h │ │ ├── maximum_likelihood.h │ │ ├── pnp.h │ │ ├── serialization │ │ ├── eigen.h │ │ ├── problems.h │ │ └── types.h │ │ ├── types.h │ │ └── validation │ │ ├── camera_intrinsic_calibration_validation.h │ │ ├── homography_validation.h │ │ ├── noise_qualification.h │ │ └── projection.h ├── package.xml ├── src │ └── rct_optimizations │ │ ├── camera_intrinsic.cpp │ │ ├── circle_fit.cpp │ │ ├── covariance_analysis.cpp │ │ ├── dh_chain.cpp │ │ ├── dh_chain_kinematic_calibration.cpp │ │ ├── eigen_conversions.cpp │ │ ├── extrinsic_hand_eye.cpp │ │ ├── extrinsic_multi_static_camera.cpp │ │ ├── extrinsic_multi_static_camera_only.cpp │ │ ├── extrinsic_multi_static_camera_wrist_only.cpp │ │ ├── multi_camera_pnp.cpp │ │ ├── pnp.cpp │ │ └── validation │ │ ├── camera_intrinsic_calibration_validation.cpp │ │ ├── homography_validation.cpp │ │ ├── noise_qualification.cpp │ │ └── projection.cpp └── test │ ├── CMakeLists.txt │ ├── camera_intrinsic_calibration_validation_utest.cpp │ ├── conversion_utest.cpp │ ├── covariance_utest.cpp │ ├── dh_chain_kinematic_calibration_utest.cpp │ ├── dh_chain_kinematic_measurement_utest.cpp │ ├── dh_parameter_utest.cpp │ ├── extrinsic_hand_eye_dh_chain_utest.cpp │ ├── extrinsic_hand_eye_utest.cpp │ ├── extrinsic_multi_static_camera_utest.cpp │ ├── homography_utest.cpp │ ├── include │ └── rct_optimizations_tests │ │ ├── dh_chain_observation_creator.h │ │ ├── observation_creator.h │ │ ├── pose_generator.h │ │ └── utilities.h │ ├── local_parameterization_utest.cpp │ ├── maximum_likelihood_utest.cpp │ ├── noise_qualification_utest.cpp │ ├── pnp_utest.cpp │ ├── serialization_utest.cpp │ └── src │ ├── dh_chain_observation_creator.cpp │ ├── observation_creator.cpp │ ├── pose_generator.cpp │ └── utilities.cpp ├── rct_ros_tools ├── CHANGELOG.rst ├── CMakeLists.txt ├── README.md ├── include │ └── rct_ros_tools │ │ ├── data_set.h │ │ ├── exceptions.h │ │ ├── loader_utils.h │ │ ├── parameter_loaders.h │ │ └── target_finder_plugin.h ├── launch │ └── command_line_data_collection.launch ├── package.xml ├── plugin_description.xml ├── src │ ├── command_line_cal.cpp │ ├── data_set.cpp │ ├── loader_utils.cpp │ ├── parameter_loaders.cpp │ └── target_loader_plugins.cpp └── test │ ├── charuco_grid_target.yaml │ ├── modified_circle_grid_target.yaml │ ├── target_finder_plugin.test │ └── target_finder_plugin_utest.cpp └── robot_cal_tools ├── CHANGELOG.rst ├── CMakeLists.txt └── package.xml /.clang-format: -------------------------------------------------------------------------------- 1 | --- 2 | BasedOnStyle: Google 3 | AccessModifierOffset: -2 4 | AlignEscapedNewlinesLeft: false 5 | AlignTrailingComments: true 6 | AlignAfterOpenBracket: Align 7 | AllowAllParametersOfDeclarationOnNextLine: false 8 | AllowShortFunctionsOnASingleLine: true 9 | AllowShortIfStatementsOnASingleLine: false 10 | AllowShortLoopsOnASingleLine: false 11 | AllowShortLoopsOnASingleLine: false 12 | AlwaysBreakBeforeMultilineStrings: false 13 | AlwaysBreakTemplateDeclarations: true 14 | BinPackArguments: false 15 | BinPackParameters: false 16 | BreakBeforeBinaryOperators: false 17 | BreakBeforeTernaryOperators: false 18 | BreakConstructorInitializersBeforeComma: true 19 | ColumnLimit: 120 20 | ConstructorInitializerAllOnOneLineOrOnePerLine: true 21 | ConstructorInitializerIndentWidth: 2 22 | ContinuationIndentWidth: 4 23 | Cpp11BracedListStyle: false 24 | DerivePointerBinding: false 25 | ExperimentalAutoDetectBinPacking: false 26 | IndentCaseLabels: true 27 | IndentFunctionDeclarationAfterType: false 28 | IndentWidth: 2 29 | MaxEmptyLinesToKeep: 1 30 | NamespaceIndentation: None 31 | ObjCSpaceBeforeProtocolList: true 32 | PenaltyBreakBeforeFirstCallParameter: 19 33 | PenaltyBreakComment: 60 34 | PenaltyBreakFirstLessLess: 1000 35 | PenaltyBreakString: 1 36 | PenaltyExcessCharacter: 1000 37 | PenaltyReturnTypeOnItsOwnLine: 90 38 | PointerBindsToType: true 39 | SortIncludes: false 40 | SpaceAfterControlStatementKeyword: true 41 | SpaceAfterCStyleCast: false 42 | SpaceBeforeAssignmentOperators: true 43 | SpaceInEmptyParentheses: false 44 | SpacesBeforeTrailingComments: 2 45 | SpacesInAngles: false 46 | SpacesInCStyleCastParentheses: false 47 | SpacesInParentheses: false 48 | Standard: Auto 49 | TabWidth: 2 50 | UseTab: Never 51 | 52 | # Configure each individual brace in BraceWrapping 53 | BreakBeforeBraces: Custom 54 | 55 | # Control of individual brace wrapping cases 56 | BraceWrapping: { 57 | AfterClass: 'true' 58 | AfterControlStatement: 'true' 59 | AfterEnum : 'true' 60 | AfterFunction : 'true' 61 | AfterNamespace : 'true' 62 | AfterStruct : 'true' 63 | AfterUnion : 'true' 64 | BeforeCatch : 'true' 65 | BeforeElse : 'true' 66 | IndentBraces : 'false' 67 | } 68 | ... 69 | -------------------------------------------------------------------------------- /.github/workflows/clang_format.yml: -------------------------------------------------------------------------------- 1 | name: Clang-Format 2 | 3 | on: 4 | push: 5 | branches: 6 | - master 7 | pull_request: 8 | 9 | jobs: 10 | clang_format: 11 | name: Clang Format 12 | runs-on: ubuntu-20.04 13 | steps: 14 | - uses: actions/checkout@v2 15 | 16 | - name: Run Clang Format Check 17 | run: | 18 | sudo apt install clang-format 19 | RED='\033[0;31m' 20 | NC='\033[0m' # No Color 21 | ./.run-clang-format 22 | output=$(git diff) 23 | if [ -n "$output" ]; then printf "${RED}Clang format error: run script './.run-clang-format'${NC}\n"; fi 24 | if [ -n "$output" ]; then printf "${RED}${output}${NC}\n"; fi 25 | if [ -n "$output" ]; then exit 1; else exit 0; fi 26 | -------------------------------------------------------------------------------- /.github/workflows/cmake_format.yml: -------------------------------------------------------------------------------- 1 | name: CMake-Format 2 | 3 | on: 4 | push: 5 | branches: 6 | - master 7 | pull_request: 8 | 9 | jobs: 10 | cmake_lang: 11 | name: CMake Format 12 | runs-on: ubuntu-20.04 13 | steps: 14 | - uses: actions/checkout@v2 15 | 16 | - name: Run CMake Lang Format Check 17 | run: | 18 | sudo pip3 install cmakelang 19 | RED='\033[0;31m' 20 | NC='\033[0m' # No Color 21 | ./.run-cmake-format 22 | output=$(git diff) 23 | if [ -n "$output" ]; then printf "${RED}CMake format error: run script './.run-cmake-format'${NC}\n"; fi 24 | if [ -n "$output" ]; then printf "${RED}${output}${NC}\n"; fi 25 | if [ -n "$output" ]; then exit 1; else exit 0; fi 26 | -------------------------------------------------------------------------------- /.github/workflows/main.yml: -------------------------------------------------------------------------------- 1 | name: CI 2 | 3 | on: 4 | push: 5 | branches: 6 | - master 7 | pull_request: 8 | schedule: 9 | - cron: '0 5 * * *' 10 | 11 | jobs: 12 | ci: 13 | strategy: 14 | fail-fast: false 15 | matrix: 16 | distro: [noetic] 17 | 18 | runs-on: ubuntu-latest 19 | name: ${{ matrix.distro }} 20 | steps: 21 | - name: Checkout 22 | uses: actions/checkout@v1 23 | - name: Build 24 | uses: 'ros-industrial/industrial_ci@master' 25 | env: 26 | BADGE: ${{ matrix.distro }} 27 | ROS_DISTRO: ${{ matrix.distro }} 28 | ROS_REPO: main 29 | UPSTREAM_WORKSPACE: dependencies.repos 30 | VERBOSE_TESTS: true 31 | TARGET_CMAKE_ARGS: "-DRCT_BUILD_TESTS=ON -DRCT_ENABLE_RUN_TESTS=OFF" 32 | CTEST_OUTPUT_ON_FAILURE: true 33 | AFTER_SCRIPT: "cd $target_ws/build/rct_optimizations && ctest -V && cd $target_ws/build/rct_image_tools && ctest -V" 34 | -------------------------------------------------------------------------------- /.run-clang-format: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | find . -type d \( -name gtest_ros \) -prune -o -type f -regex '.*\.\(cpp\|hpp\|cc\|cxx\|h\|hxx\)' -exec clang-format -style=file -i {} \; 3 | -------------------------------------------------------------------------------- /.run-cmake-format: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | find . \( -name CMakeLists.txt -o -name \*.cmake \) -exec cmake-format -i {} \; 3 | -------------------------------------------------------------------------------- /ci: -------------------------------------------------------------------------------- 1 | .github/workflows/ -------------------------------------------------------------------------------- /dependencies.repos: -------------------------------------------------------------------------------- 1 | repositories: 2 | ros_industrial_cmake_boilerplate: 3 | type: git 4 | url: https://github.com/ros-industrial/ros_industrial_cmake_boilerplate.git 5 | version: 0.6.2 6 | -------------------------------------------------------------------------------- /docs/mod_circle_target_annotated.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Jmeyer1292/robot_cal_tools/2089a06d11b13323cf6c358a89524231801e5b06/docs/mod_circle_target_annotated.png -------------------------------------------------------------------------------- /docs/pinhole_camera_model.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Jmeyer1292/robot_cal_tools/2089a06d11b13323cf6c358a89524231801e5b06/docs/pinhole_camera_model.png -------------------------------------------------------------------------------- /rct_common/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package rct_common 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 0.2.1 (2024-08-13) 6 | ------------------ 7 | * Add missing build depend ros_industrial_cmake_boilerplate 8 | * Contributors: Levi Armstrong 9 | 10 | 0.2.0 (2024-07-25) 11 | ------------------ 12 | * fix cxx target version 13 | * Update to use RICB (`#109 `_) 14 | * Updated to use RICB 15 | * Updated CI 16 | * Dropped Bionic build 17 | * Linters (`#108 `_) 18 | * Add clang format linter and CI build 19 | * Add cmake format linter and CI build 20 | * Add CI badges to the readme 21 | * Replace symlinks with symlink to CI directory 22 | * Updated workflows 23 | * Change clang format version 24 | * Ran clang format 25 | * Ran CMake format 26 | * Added git to ADDITIONAL_DEBS field 27 | --------- 28 | Co-authored-by: Levi Armstrong 29 | * Contributors: Levi Armstrong, Michael Ripperger 30 | 31 | 0.1.1 (2022-05-17) 32 | ------------------ 33 | * remove changelogs 34 | * Add changelogs 35 | * Move utilities from ros pkg to common pkg and add missing dependencies cmake config 36 | * Convert rct_optimizations to be a ROS-generic CMake package (`#42 `_) 37 | * make rct_optimizations a pure CMake package 38 | change other packages so they treat rct_optimizations as a pure CMake package 39 | Fix erroneously commented-out Eigen3 dependency 40 | remove pattern matching filter from include install 41 | Add rct_common package, move macros and GTest infrastructure to it 42 | rename RCT_ENABLE_RUN_TESTING to RCT_RUN_TESTS 43 | Add flags to build and run tests in CI 44 | install git in CI env 45 | add git to ADDITIONAL_DEBS for industrial_ci docker image 46 | Remove RCT_RUN_TESTS flag from CI config 47 | link rct_examples test against GTest 48 | * set RCT_RUN_TESTS=True 49 | * Contributors: Joseph Schornak, Levi Armstrong 50 | 51 | 0.1.0 (2020-03-27) 52 | ------------------ 53 | -------------------------------------------------------------------------------- /rct_common/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.16.0) 2 | 3 | # Extract package name and version from package.xml 4 | find_package(ros_industrial_cmake_boilerplate REQUIRED) 5 | extract_package_metadata(pkg) 6 | project(${pkg_extracted_name} VERSION ${pkg_extracted_version} LANGUAGES CXX) 7 | 8 | find_package(Eigen3 REQUIRED) 9 | if(NOT EIGEN3_INCLUDE_DIRS) 10 | set(EIGEN3_INCLUDE_DIRS ${EIGEN3_INCLUDE_DIR}) 11 | endif() 12 | if(NOT TARGET Eigen3::Eigen) 13 | find_package(Threads REQUIRED) 14 | add_library(Eigen3::Eigen IMPORTED INTERFACE) 15 | set_property(TARGET Eigen3::Eigen PROPERTY INTERFACE_COMPILE_DEFINITIONS ${EIGEN3_DEFINITIONS}) 16 | set_property(TARGET Eigen3::Eigen PROPERTY INTERFACE_INCLUDE_DIRECTORIES ${EIGEN3_INCLUDE_DIRS}) 17 | endif() 18 | 19 | find_gtest() 20 | 21 | add_library(${PROJECT_NAME} INTERFACE) 22 | target_compile_features(${PROJECT_NAME} INTERFACE cxx_std_11) 23 | target_include_directories(${PROJECT_NAME} INTERFACE "$" 24 | "$") 25 | target_link_libraries(${PROJECT_NAME} INTERFACE Eigen3::Eigen) 26 | 27 | configure_package(TARGETS ${PROJECT_NAME} NAMESPACE rct DEPENDENCIES Eigen3) 28 | 29 | install(DIRECTORY include/${PROJECT_NAME} DESTINATION include) 30 | -------------------------------------------------------------------------------- /rct_common/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | rct_common 4 | 0.2.1 5 | The rct_common package. Contains macros and external libraries for the robot_cal_tools project. 6 | Joseph Schornak 7 | Joseph Schornak 8 | Apache 2.0 9 | ros_industrial_cmake_boilerplate 10 | eigen 11 | 12 | cmake 13 | 14 | 15 | -------------------------------------------------------------------------------- /rct_examples/data/test_set_10x10/cal_data.yaml: -------------------------------------------------------------------------------- 1 | - pose: poses/0.yaml 2 | image: images/0.png 3 | - pose: poses/1.yaml 4 | image: images/1.png 5 | - pose: poses/10.yaml 6 | image: images/10.png 7 | - pose: poses/11.yaml 8 | image: images/11.png 9 | - pose: poses/12.yaml 10 | image: images/12.png 11 | - pose: poses/13.yaml 12 | image: images/13.png 13 | - pose: poses/14.yaml 14 | image: images/14.png 15 | - pose: poses/2.yaml 16 | image: images/2.png 17 | - pose: poses/3.yaml 18 | image: images/3.png 19 | - pose: poses/4.yaml 20 | image: images/4.png 21 | - pose: poses/5.yaml 22 | image: images/5.png 23 | - pose: poses/6.yaml 24 | image: images/6.png 25 | - pose: poses/7.yaml 26 | image: images/7.png 27 | - pose: poses/8.yaml 28 | image: images/8.png 29 | - pose: poses/9.yaml 30 | image: images/9.png 31 | -------------------------------------------------------------------------------- /rct_examples/data/test_set_10x10/camera_intr.yaml: -------------------------------------------------------------------------------- 1 | intrinsics: 2 | fx: 550.0 # pixels 3 | fy: 550.0 # pixels 4 | cx: 320.0 5 | cy: 240.0 6 | -------------------------------------------------------------------------------- /rct_examples/data/test_set_10x10/camera_intr_calibrated.yaml: -------------------------------------------------------------------------------- 1 | intrinsics: 2 | fx: 542.324 # pixels 3 | fy: 543.515 # pixels 4 | cx: 320.474 5 | cy: 226.039 6 | -------------------------------------------------------------------------------- /rct_examples/data/test_set_10x10/images/0.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Jmeyer1292/robot_cal_tools/2089a06d11b13323cf6c358a89524231801e5b06/rct_examples/data/test_set_10x10/images/0.png -------------------------------------------------------------------------------- /rct_examples/data/test_set_10x10/images/1.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Jmeyer1292/robot_cal_tools/2089a06d11b13323cf6c358a89524231801e5b06/rct_examples/data/test_set_10x10/images/1.png -------------------------------------------------------------------------------- /rct_examples/data/test_set_10x10/images/10.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Jmeyer1292/robot_cal_tools/2089a06d11b13323cf6c358a89524231801e5b06/rct_examples/data/test_set_10x10/images/10.png -------------------------------------------------------------------------------- /rct_examples/data/test_set_10x10/images/11.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Jmeyer1292/robot_cal_tools/2089a06d11b13323cf6c358a89524231801e5b06/rct_examples/data/test_set_10x10/images/11.png -------------------------------------------------------------------------------- /rct_examples/data/test_set_10x10/images/12.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Jmeyer1292/robot_cal_tools/2089a06d11b13323cf6c358a89524231801e5b06/rct_examples/data/test_set_10x10/images/12.png -------------------------------------------------------------------------------- /rct_examples/data/test_set_10x10/images/13.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Jmeyer1292/robot_cal_tools/2089a06d11b13323cf6c358a89524231801e5b06/rct_examples/data/test_set_10x10/images/13.png -------------------------------------------------------------------------------- /rct_examples/data/test_set_10x10/images/14.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Jmeyer1292/robot_cal_tools/2089a06d11b13323cf6c358a89524231801e5b06/rct_examples/data/test_set_10x10/images/14.png -------------------------------------------------------------------------------- /rct_examples/data/test_set_10x10/images/2.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Jmeyer1292/robot_cal_tools/2089a06d11b13323cf6c358a89524231801e5b06/rct_examples/data/test_set_10x10/images/2.png -------------------------------------------------------------------------------- /rct_examples/data/test_set_10x10/images/3.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Jmeyer1292/robot_cal_tools/2089a06d11b13323cf6c358a89524231801e5b06/rct_examples/data/test_set_10x10/images/3.png -------------------------------------------------------------------------------- /rct_examples/data/test_set_10x10/images/4.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Jmeyer1292/robot_cal_tools/2089a06d11b13323cf6c358a89524231801e5b06/rct_examples/data/test_set_10x10/images/4.png -------------------------------------------------------------------------------- /rct_examples/data/test_set_10x10/images/5.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Jmeyer1292/robot_cal_tools/2089a06d11b13323cf6c358a89524231801e5b06/rct_examples/data/test_set_10x10/images/5.png -------------------------------------------------------------------------------- /rct_examples/data/test_set_10x10/images/6.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Jmeyer1292/robot_cal_tools/2089a06d11b13323cf6c358a89524231801e5b06/rct_examples/data/test_set_10x10/images/6.png -------------------------------------------------------------------------------- /rct_examples/data/test_set_10x10/images/7.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Jmeyer1292/robot_cal_tools/2089a06d11b13323cf6c358a89524231801e5b06/rct_examples/data/test_set_10x10/images/7.png -------------------------------------------------------------------------------- /rct_examples/data/test_set_10x10/images/8.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Jmeyer1292/robot_cal_tools/2089a06d11b13323cf6c358a89524231801e5b06/rct_examples/data/test_set_10x10/images/8.png -------------------------------------------------------------------------------- /rct_examples/data/test_set_10x10/images/9.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Jmeyer1292/robot_cal_tools/2089a06d11b13323cf6c358a89524231801e5b06/rct_examples/data/test_set_10x10/images/9.png -------------------------------------------------------------------------------- /rct_examples/data/test_set_10x10/noise_qualification/camera_to_target_guess.yaml: -------------------------------------------------------------------------------- 1 | camera_to_target_guess: 2 | x: 0.00330134 3 | y: 0.0778156 4 | z: 0.77218 5 | qx: 0.999656 6 | qy: 0.00396669 7 | qz: 0.020924 8 | qw: 0.0152998 9 | -------------------------------------------------------------------------------- /rct_examples/data/test_set_10x10/noise_qualification/data.yaml: -------------------------------------------------------------------------------- 1 | # List of images relative to a data directory 2 | # In this example case all the images are the same to produce a zero-noise result 3 | - image: images/0.png 4 | - image: images/0.png 5 | - image: images/0.png 6 | - image: images/0.png 7 | - image: images/0.png 8 | - image: images/0.png 9 | - image: images/0.png 10 | - image: images/0.png 11 | - image: images/0.png 12 | - image: images/0.png 13 | - image: images/0.png 14 | - image: images/0.png 15 | - image: images/0.png 16 | - image: images/0.png 17 | -------------------------------------------------------------------------------- /rct_examples/data/test_set_10x10/pose_guesses.yaml: -------------------------------------------------------------------------------- 1 | wrist_to_camera_guess: 2 | x: 0.0 # Note: Until I fix this YOU NEED to make this a DOUBLE! Don't write just '0'. 3 | y: 0.0 4 | z: 0.0 5 | qx: 0.0 6 | qy: 0.0 7 | qz: -0.7071068 8 | qw: 0.7071068 9 | base_to_target_guess: 10 | x: 1.0 11 | y: 0.0 12 | z: 0.0 13 | qx: 0.0 14 | qy: 0.0 15 | qz: -0.7071068 16 | qw: 0.7071068 17 | -------------------------------------------------------------------------------- /rct_examples/data/test_set_10x10/poses/0.yaml: -------------------------------------------------------------------------------- 1 | x: 0.7169496300523684 2 | y: 0.00330346692205527 3 | z: 0.853313460465923 4 | qx: -0.01709391593413424 5 | qy: 0.99974825023776 6 | qz: -0.01437638468984423 7 | qw: -0.002133951223335981 8 | -------------------------------------------------------------------------------- /rct_examples/data/test_set_10x10/poses/1.yaml: -------------------------------------------------------------------------------- 1 | x: 0.5752203716195865 2 | y: -0.3993293991199381 3 | z: 0.8533152764463749 4 | qx: -0.01712514830113558 5 | qy: 0.9997478481584079 6 | qz: -0.01436601236563321 7 | qw: -0.002141748586426575 8 | -------------------------------------------------------------------------------- /rct_examples/data/test_set_10x10/poses/10.yaml: -------------------------------------------------------------------------------- 1 | x: 0.7899668099030647 2 | y: 0.494772358953413 3 | z: 0.8235759374169627 4 | qx: -0.008952666652400187 5 | qy: 0.976748329697353 6 | qz: -0.21418403321934 7 | qw: 0.002783901424029574 8 | -------------------------------------------------------------------------------- /rct_examples/data/test_set_10x10/poses/11.yaml: -------------------------------------------------------------------------------- 1 | x: 0.5369650312415266 2 | y: 0.5332172925000433 3 | z: 0.8235768753310129 4 | qx: -0.008918378584601094 5 | qy: 0.976749856976095 6 | qz: -0.2141788411884198 7 | qw: 0.002757427748006871 8 | -------------------------------------------------------------------------------- /rct_examples/data/test_set_10x10/poses/12.yaml: -------------------------------------------------------------------------------- 1 | x: 0.9746811324097581 2 | y: 0.53320309865587 3 | z: 0.8235751155677318 4 | qx: -0.008948721209816324 5 | qy: 0.976746237897856 6 | qz: -0.2141938884217242 7 | qw: 0.002772238062639001 8 | -------------------------------------------------------------------------------- /rct_examples/data/test_set_10x10/poses/13.yaml: -------------------------------------------------------------------------------- 1 | x: 0.8561403673919278 2 | y: 0.10122175943146 3 | z: 0.8235774451108588 4 | qx: -0.00887865852480246 5 | qy: 0.976751987147199 6 | qz: -0.2141709382465148 7 | qw: 0.002744856536426867 8 | -------------------------------------------------------------------------------- /rct_examples/data/test_set_10x10/poses/14.yaml: -------------------------------------------------------------------------------- 1 | x: 0.6138783687686044 2 | y: 0.1012315598668618 3 | z: 0.823583487031208 4 | qx: -0.0088876471292792 5 | qy: 0.9767525297996354 6 | qz: -0.2141683240967401 7 | qw: 0.002726575161537544 8 | -------------------------------------------------------------------------------- /rct_examples/data/test_set_10x10/poses/2.yaml: -------------------------------------------------------------------------------- 1 | x: 0.9311356631850386 2 | y: -0.3993114699429116 3 | z: 0.8533157130230188 4 | qx: -0.01710461305168881 5 | qy: 0.9997482212615435 6 | qz: -0.01436378175586931 7 | qw: -0.002146641656553289 8 | -------------------------------------------------------------------------------- /rct_examples/data/test_set_10x10/poses/3.yaml: -------------------------------------------------------------------------------- 1 | x: 0.9311397197768709 2 | y: 0.1559287773723496 3 | z: 0.8533166206036564 4 | qx: -0.0170850119839446 5 | qy: 0.9997487240208317 6 | qz: -0.01435415675230236 7 | qw: -0.002132924789479186 8 | -------------------------------------------------------------------------------- /rct_examples/data/test_set_10x10/poses/4.yaml: -------------------------------------------------------------------------------- 1 | x: 0.5980310182164968 2 | y: 0.1592168829054929 3 | z: 0.8533092234597746 4 | qx: -0.01710257626786444 5 | qy: 0.999748106124043 6 | qz: -0.01437389988730297 7 | qw: -0.002148764394012373 8 | -------------------------------------------------------------------------------- /rct_examples/data/test_set_10x10/poses/5.yaml: -------------------------------------------------------------------------------- 1 | x: 0.5757661715410868 2 | y: -0.03248677499005915 3 | z: 0.827753447853608 4 | qx: -0.02173781932748391 5 | qy: 0.9793775412379023 6 | qz: -0.02020068310240068 7 | qw: 0.1998475202044875 8 | -------------------------------------------------------------------------------- /rct_examples/data/test_set_10x10/poses/6.yaml: -------------------------------------------------------------------------------- 1 | x: 1.226950606599079 2 | y: -0.0324858313724839 3 | z: 0.8277426967723728 4 | qx: -0.006823639824368027 5 | qy: 0.9252304786347886 6 | qz: -0.02893314029691856 7 | qw: -0.3782391739856493 8 | -------------------------------------------------------------------------------- /rct_examples/data/test_set_10x10/poses/7.yaml: -------------------------------------------------------------------------------- 1 | x: 1.226957184681144 2 | y: -0.0324715118688171 3 | z: 0.8277324271981921 4 | qx: -0.02331601617295925 5 | qy: 0.9555615633309285 6 | qz: -0.05439302701246936 7 | qw: -0.2887903403626845 8 | -------------------------------------------------------------------------------- /rct_examples/data/test_set_10x10/poses/8.yaml: -------------------------------------------------------------------------------- 1 | x: 0.8855574804670588 2 | y: -0.4401272363254717 3 | z: 0.8277192554757641 4 | qx: 0.02250156935045375 5 | qy: 0.9753312030822024 6 | qz: 0.1447877876458878 7 | qw: -0.1651036650701602 8 | -------------------------------------------------------------------------------- /rct_examples/data/test_set_10x10/poses/9.yaml: -------------------------------------------------------------------------------- 1 | x: 0.9062908906356706 2 | y: -0.3940744376345787 3 | z: 0.828102980819179 4 | qx: 0.1992007500290545 5 | qy: 0.9657456252076497 6 | qz: 0.1641147018957322 7 | qw: -0.02684796457504554 8 | -------------------------------------------------------------------------------- /rct_examples/data/test_set_10x10/target.yaml: -------------------------------------------------------------------------------- 1 | target_finder: 2 | type: rct_ros_tools::ModifiedCircleGridTargetFinderPlugin 3 | rows: 10 4 | cols: 10 5 | spacing: 0.0254 # (meters between dot centers) 6 | circle_detector_params: 7 | minThreshold: 20 8 | maxThreshold: 220 9 | nThresholds: 20 10 | 11 | minRepeatability: 3 12 | circleInclusionRadius: 5 13 | maxRadiusDiff: 5 14 | 15 | maxAverageEllipseError: 0.02 16 | 17 | filterByColor: true 18 | circleColor: 0 19 | 20 | filterByArea: true 21 | minArea: 25.0 22 | maxArea: 5000.0 23 | 24 | filterByCircularity: false 25 | minCircularity: 0.8 26 | maxCircularity: 99999999.0 27 | 28 | filterByInertia: false 29 | minInertiaRatio: 0.1 30 | maxInertiaRatio: 99999999.0 31 | 32 | filterByConvexity: true 33 | minConvexity: 0.95 34 | maxConvexity: 99999999.0 35 | -------------------------------------------------------------------------------- /rct_examples/data/test_set_charuco/cal_data.yaml: -------------------------------------------------------------------------------- 1 | - image: images/0.png 2 | pose: poses/0.yaml 3 | - pose: poses/1.yaml 4 | image: images/1.png 5 | - pose: poses/2.yaml 6 | image: images/2.png 7 | - pose: poses/3.yaml 8 | image: images/3.png 9 | - image: images/4.png 10 | pose: poses/4.yaml 11 | - pose: poses/5.yaml 12 | image: images/5.png 13 | - image: images/6.png 14 | pose: poses/6.yaml 15 | - image: images/7.png 16 | pose: poses/7.yaml 17 | - image: images/8.png 18 | pose: poses/8.yaml 19 | - image: images/9.png 20 | pose: poses/9.yaml 21 | - image: images/10.png 22 | pose: poses/10.yaml 23 | - pose: poses/11.yaml 24 | image: images/11.png 25 | - image: images/12.png 26 | pose: poses/12.yaml 27 | - image: images/13.png 28 | pose: poses/13.yaml 29 | - image: images/14.png 30 | pose: poses/14.yaml 31 | - image: images/15.png 32 | pose: poses/15.yaml -------------------------------------------------------------------------------- /rct_examples/data/test_set_charuco/camera_intr.yaml: -------------------------------------------------------------------------------- 1 | intrinsics: 2 | fx: 1400.0 # pixels 3 | fy: 1400.0 # pixels 4 | cx: 800.0 5 | cy: 600.0 6 | -------------------------------------------------------------------------------- /rct_examples/data/test_set_charuco/camera_intr_calibrated.yaml: -------------------------------------------------------------------------------- 1 | intrinsics: 2 | fx: 1352.02747 3 | fy: 1356.14287 4 | cx: 789.67065 5 | cy: 627.2995 6 | -------------------------------------------------------------------------------- /rct_examples/data/test_set_charuco/images/0.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Jmeyer1292/robot_cal_tools/2089a06d11b13323cf6c358a89524231801e5b06/rct_examples/data/test_set_charuco/images/0.png -------------------------------------------------------------------------------- /rct_examples/data/test_set_charuco/images/1.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Jmeyer1292/robot_cal_tools/2089a06d11b13323cf6c358a89524231801e5b06/rct_examples/data/test_set_charuco/images/1.png -------------------------------------------------------------------------------- /rct_examples/data/test_set_charuco/images/10.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Jmeyer1292/robot_cal_tools/2089a06d11b13323cf6c358a89524231801e5b06/rct_examples/data/test_set_charuco/images/10.png -------------------------------------------------------------------------------- /rct_examples/data/test_set_charuco/images/11.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Jmeyer1292/robot_cal_tools/2089a06d11b13323cf6c358a89524231801e5b06/rct_examples/data/test_set_charuco/images/11.png -------------------------------------------------------------------------------- /rct_examples/data/test_set_charuco/images/12.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Jmeyer1292/robot_cal_tools/2089a06d11b13323cf6c358a89524231801e5b06/rct_examples/data/test_set_charuco/images/12.png -------------------------------------------------------------------------------- /rct_examples/data/test_set_charuco/images/13.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Jmeyer1292/robot_cal_tools/2089a06d11b13323cf6c358a89524231801e5b06/rct_examples/data/test_set_charuco/images/13.png -------------------------------------------------------------------------------- /rct_examples/data/test_set_charuco/images/14.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Jmeyer1292/robot_cal_tools/2089a06d11b13323cf6c358a89524231801e5b06/rct_examples/data/test_set_charuco/images/14.png -------------------------------------------------------------------------------- /rct_examples/data/test_set_charuco/images/15.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Jmeyer1292/robot_cal_tools/2089a06d11b13323cf6c358a89524231801e5b06/rct_examples/data/test_set_charuco/images/15.png -------------------------------------------------------------------------------- /rct_examples/data/test_set_charuco/images/2.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Jmeyer1292/robot_cal_tools/2089a06d11b13323cf6c358a89524231801e5b06/rct_examples/data/test_set_charuco/images/2.png -------------------------------------------------------------------------------- /rct_examples/data/test_set_charuco/images/3.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Jmeyer1292/robot_cal_tools/2089a06d11b13323cf6c358a89524231801e5b06/rct_examples/data/test_set_charuco/images/3.png -------------------------------------------------------------------------------- /rct_examples/data/test_set_charuco/images/4.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Jmeyer1292/robot_cal_tools/2089a06d11b13323cf6c358a89524231801e5b06/rct_examples/data/test_set_charuco/images/4.png -------------------------------------------------------------------------------- /rct_examples/data/test_set_charuco/images/5.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Jmeyer1292/robot_cal_tools/2089a06d11b13323cf6c358a89524231801e5b06/rct_examples/data/test_set_charuco/images/5.png -------------------------------------------------------------------------------- /rct_examples/data/test_set_charuco/images/6.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Jmeyer1292/robot_cal_tools/2089a06d11b13323cf6c358a89524231801e5b06/rct_examples/data/test_set_charuco/images/6.png -------------------------------------------------------------------------------- /rct_examples/data/test_set_charuco/images/7.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Jmeyer1292/robot_cal_tools/2089a06d11b13323cf6c358a89524231801e5b06/rct_examples/data/test_set_charuco/images/7.png -------------------------------------------------------------------------------- /rct_examples/data/test_set_charuco/images/8.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Jmeyer1292/robot_cal_tools/2089a06d11b13323cf6c358a89524231801e5b06/rct_examples/data/test_set_charuco/images/8.png -------------------------------------------------------------------------------- /rct_examples/data/test_set_charuco/images/9.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Jmeyer1292/robot_cal_tools/2089a06d11b13323cf6c358a89524231801e5b06/rct_examples/data/test_set_charuco/images/9.png -------------------------------------------------------------------------------- /rct_examples/data/test_set_charuco/pose_guesses.yaml: -------------------------------------------------------------------------------- 1 | base_to_camera_guess: 2 | x: -0.0196464 # Note: Until I fix this YOU NEED to make this a DOUBLE! Don't write just '0'. 3 | y: 1.27843 4 | z: 0.283965 5 | qx: -0.0480798 6 | qy: -0.558686 7 | qz: 0.827604 8 | qw: -0.0251035 9 | wrist_to_target_guess: 10 | x: 0.0633135 11 | y: 0.119958 12 | z: 0.211056 13 | qx: -0.633597 14 | qy: 0.262427 15 | qz: 0.664998 16 | qw: 0.295794 17 | -------------------------------------------------------------------------------- /rct_examples/data/test_set_charuco/poses/0.yaml: -------------------------------------------------------------------------------- 1 | x: 0.113074204220918 2 | y: 0.5892802789268905 3 | z: 0.652673790716681 4 | qz: 0.09319502827772819 5 | qx: -0.518597252130659 6 | qw: 0.2389701531981282 7 | qy: 0.815637690808429 -------------------------------------------------------------------------------- /rct_examples/data/test_set_charuco/poses/1.yaml: -------------------------------------------------------------------------------- 1 | z: 0.6493242659074356 2 | qy: 0.8156548751868672 3 | qw: 0.2389443970328892 4 | qz: 0.0932410034123143 5 | qx: -0.5185738279099222 6 | x: -0.05534076331168009 7 | y: 0.671557822839395 -------------------------------------------------------------------------------- /rct_examples/data/test_set_charuco/poses/10.yaml: -------------------------------------------------------------------------------- 1 | z: 0.5337298726624968 2 | x: 0.005153729303247256 3 | qw: -0.4031335469642314 4 | y: 0.8746571070597891 5 | qy: 0.8749842086206083 6 | qx: -0.2575699813289879 7 | qz: 0.07445591108024914 -------------------------------------------------------------------------------- /rct_examples/data/test_set_charuco/poses/11.yaml: -------------------------------------------------------------------------------- 1 | y: 0.5832048743461382 2 | qz: 0.1225477391404354 3 | x: 0.2126411705711696 4 | qw: -0.3976457797384892 5 | qx: -0.07748841346178788 6 | qy: 0.9060107235937731 7 | z: 0.5879841922658178 -------------------------------------------------------------------------------- /rct_examples/data/test_set_charuco/poses/12.yaml: -------------------------------------------------------------------------------- 1 | y: 0.6041493378056203 2 | x: -0.1504460344277249 3 | z: 0.5875944649135195 4 | qw: -0.4160991721128268 5 | qy: 0.8436692056515871 6 | qx: -0.3392389784539254 7 | qz: 0.0008160268241617773 -------------------------------------------------------------------------------- /rct_examples/data/test_set_charuco/poses/13.yaml: -------------------------------------------------------------------------------- 1 | qz: 0.1082716566605072 2 | qx: -0.3867936866185109 3 | x: -0.1739839082982338 4 | qy: 0.8300727507087611 5 | qw: -0.3868425013966822 6 | z: 0.6983525061364482 7 | y: 0.7671056883742549 -------------------------------------------------------------------------------- /rct_examples/data/test_set_charuco/poses/14.yaml: -------------------------------------------------------------------------------- 1 | qz: 0.1827140650399575 2 | qw: -0.3577521736897814 3 | x: 0.136967611442945 4 | qx: -0.2148017481822395 5 | z: 0.6987003230613983 6 | y: 0.7729268612066653 7 | qy: 0.8902186032849824 -------------------------------------------------------------------------------- /rct_examples/data/test_set_charuco/poses/15.yaml: -------------------------------------------------------------------------------- 1 | qx: -0.5139273879666715 2 | qw: -0.04306857961584857 3 | z: 1.051225779077351 4 | qz: -0.0261508947783002 5 | qy: 0.8563526540216487 6 | y: 0.09491322756054421 7 | x: 0.3904715375152571 -------------------------------------------------------------------------------- /rct_examples/data/test_set_charuco/poses/2.yaml: -------------------------------------------------------------------------------- 1 | x: -0.05534032392551747 2 | z: 0.6493230523330574 3 | y: 0.671552824814702 4 | qw: 0.2233733272846846 5 | qz: 0.1260665121843097 6 | qx: -0.3981059798485375 7 | qy: 0.8807515086403701 -------------------------------------------------------------------------------- /rct_examples/data/test_set_charuco/poses/3.yaml: -------------------------------------------------------------------------------- 1 | x: -0.00220624729956348 2 | qy: 0.7175709664444335 3 | qx: -0.4274595917458887 4 | z: 0.6895418722451742 5 | y: 0.6353307581715532 6 | qw: 0.4641823528670378 7 | qz: -0.294796453213503 -------------------------------------------------------------------------------- /rct_examples/data/test_set_charuco/poses/4.yaml: -------------------------------------------------------------------------------- 1 | qy: 0.8925315006107449 2 | qz: -0.355491927019211 3 | qx: -0.2291567109425286 4 | x: -0.09015344996788141 5 | z: 0.6637870323665627 6 | y: 0.5775223545798629 7 | qw: -0.1565254358617214 -------------------------------------------------------------------------------- /rct_examples/data/test_set_charuco/poses/5.yaml: -------------------------------------------------------------------------------- 1 | qx: -0.5566221996194233 2 | qz: 0.1296022235234896 3 | qw: 0.03676301970421652 4 | x: -0.08739294216730098 5 | qy: 0.819770376953711 6 | y: 0.659099196102267 7 | z: 0.6412235330974134 -------------------------------------------------------------------------------- /rct_examples/data/test_set_charuco/poses/6.yaml: -------------------------------------------------------------------------------- 1 | y: 0.8228186736370173 2 | x: -0.1180929332711085 3 | qx: -0.5566220802535535 4 | qy: 0.8197708773022536 5 | qz: 0.1296003216136348 6 | qw: 0.0367603745756597 7 | z: 0.5466667720320662 -------------------------------------------------------------------------------- /rct_examples/data/test_set_charuco/poses/7.yaml: -------------------------------------------------------------------------------- 1 | qw: -0.05781787128475047 2 | z: 0.5493465772376277 3 | qz: 0.1748273285268999 4 | y: 0.8276773655316582 5 | x: -0.1267605521273628 6 | qy: 0.959177310365944 7 | qx: -0.2146424614082689 -------------------------------------------------------------------------------- /rct_examples/data/test_set_charuco/poses/8.yaml: -------------------------------------------------------------------------------- 1 | qy: 0.9731390040970653 2 | y: 0.8098050194040343 3 | qz: 0.04314112953953605 4 | z: 0.552364723100556 5 | x: -0.1476031424904063 6 | qx: 0.03659478750459112 7 | qw: -0.2231594568341625 -------------------------------------------------------------------------------- /rct_examples/data/test_set_charuco/poses/9.yaml: -------------------------------------------------------------------------------- 1 | z: 0.6108896152869768 2 | qx: 0.07001738751809528 3 | x: -0.04430488747556816 4 | qw: -0.3993341688908818 5 | y: 0.6095219662014785 6 | qz: -0.1015225786234006 7 | qy: 0.9084728686268098 -------------------------------------------------------------------------------- /rct_examples/data/test_set_charuco/target.yaml: -------------------------------------------------------------------------------- 1 | target_finder: 2 | type: rct_ros_tools::CharucoGridTargetFinderPlugin 3 | rows: 7 4 | cols: 5 5 | chessboard_dim: 0.036195 6 | aruco_marker_dim: 0.018256 7 | dictionary: 10 # DICT_6X6_250 8 | -------------------------------------------------------------------------------- /rct_examples/docs/moving_camera_cell.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Jmeyer1292/robot_cal_tools/2089a06d11b13323cf6c358a89524231801e5b06/rct_examples/docs/moving_camera_cell.png -------------------------------------------------------------------------------- /rct_examples/docs/static_camera_cell.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Jmeyer1292/robot_cal_tools/2089a06d11b13323cf6c358a89524231801e5b06/rct_examples/docs/static_camera_cell.png -------------------------------------------------------------------------------- /rct_examples/launch/camera_intrinsic_calibration_example.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | -------------------------------------------------------------------------------- /rct_examples/launch/camera_intrinsic_calibration_validation.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | -------------------------------------------------------------------------------- /rct_examples/launch/camera_on_wrist_example.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | -------------------------------------------------------------------------------- /rct_examples/launch/kinematic_calibration_example.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | camera_mount_to_camera_guess: 8 | x: 2.2 9 | y: 0.7 10 | z: 1.075 11 | qw: 1.0 12 | qx: 0.0 13 | qy: 0.0 14 | qz: 0.0 15 | target_mount_to_target_guess: 16 | x: 0.17 17 | y: -0.65 18 | z: 0.5 19 | qw: 0.547419 20 | qx: 0.447585 21 | qy: -0.547419 22 | qz: -0.447585 23 | 24 | 25 | 26 | -------------------------------------------------------------------------------- /rct_examples/launch/multi_static_camera_example.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | -------------------------------------------------------------------------------- /rct_examples/launch/multi_static_camera_multi_step_example.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | -------------------------------------------------------------------------------- /rct_examples/launch/noise_qualification_2d.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | -------------------------------------------------------------------------------- /rct_examples/launch/pnp_example.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | -------------------------------------------------------------------------------- /rct_examples/launch/solve_multi_static_camera_pnp_example.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | -------------------------------------------------------------------------------- /rct_examples/launch/static_camera_example.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | -------------------------------------------------------------------------------- /rct_examples/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | rct_examples 4 | 0.2.1 5 | The rct_examples package 6 | 7 | Jonathan Meyer 8 | Jonathan Meyer 9 | 10 | Apache 2.0 11 | 12 | catkin 13 | ros_industrial_cmake_boilerplate 14 | rct_image_tools 15 | rct_optimizations 16 | rct_ros_tools 17 | roslib 18 | 19 | 20 | -------------------------------------------------------------------------------- /rct_examples/src/tools/noise_qualification_2d.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | 13 | using namespace rct_optimizations; 14 | using namespace rct_image_tools; 15 | using namespace rct_ros_tools; 16 | 17 | std::string WINDOW = "window"; 18 | 19 | template 20 | T get(const ros::NodeHandle& nh, const std::string& key) 21 | { 22 | T val; 23 | if (!nh.getParam(key, val)) 24 | throw std::runtime_error("Failed to get '" + key + "' parameter"); 25 | return val; 26 | } 27 | 28 | int main(int argc, char** argv) 29 | { 30 | ros::init(argc, argv, "noise_qualification_2d"); 31 | ros::NodeHandle pnh("~"); 32 | 33 | try 34 | { 35 | // Parse parameters 36 | std::string data_path = get(pnh, "data_path"); 37 | std::string data_file = get(pnh, "data_file"); 38 | 39 | // Load the target finder 40 | auto target_finder_config = get(pnh, "target_finder"); 41 | const std::string target_finder_type = static_cast(target_finder_config["type"]); 42 | pluginlib::ClassLoader loader("rct_ros_tools", "rct_ros_tools::TargetFinderPlugin"); 43 | boost::shared_ptr target_finder = loader.createInstance(target_finder_type); 44 | target_finder->init(toYAML(target_finder_config)); 45 | 46 | // Load camera intrinsics 47 | CameraIntrinsics camera = loadIntrinsics(pnh, "intrinsics"); 48 | 49 | // Load an initial guess for the camera to target transformation 50 | Eigen::Isometry3d camera_to_target_guess = loadPose(pnh, "camera_to_target_guess"); 51 | 52 | // Load the data file which specifies the location of the images on which to perform the noise qualification 53 | YAML::Node root = YAML::LoadFile(data_file); 54 | 55 | cv::namedWindow(WINDOW, cv::WINDOW_NORMAL); 56 | 57 | // Set up the noise qualification inputs 58 | std::vector problem_set; 59 | problem_set.reserve(root.size()); 60 | for (std::size_t i = 0; i < root.size(); ++i) 61 | { 62 | // Each entry should have an image path. This path is relative to the root_path directory! 63 | const auto img_path = root[i]["image"].as(); 64 | const std::string image_name = data_path + "/" + img_path; 65 | static cv::Mat image = readImageOpenCV(image_name); 66 | 67 | // Find the observations in the image 68 | rct_image_tools::TargetFeatures target_features; 69 | try 70 | { 71 | target_features = target_finder->findTargetFeatures(image); 72 | if (target_features.empty()) 73 | throw std::runtime_error("Failed to find any target features"); 74 | ROS_INFO_STREAM("Found " << target_features.size() << " target features"); 75 | 76 | // Show the points we detected 77 | cv::imshow(WINDOW, target_finder->drawTargetFeatures(image, target_features)); 78 | cv::waitKey(); 79 | } 80 | catch (const std::runtime_error& ex) 81 | { 82 | ROS_WARN_STREAM("Image " << i << ": '" << ex.what() << "'"); 83 | cv::imshow(WINDOW, image); 84 | cv::waitKey(); 85 | continue; 86 | } 87 | 88 | // Set up the PnP problem for this image 89 | PnPProblem problem; 90 | problem.intr = camera; 91 | problem.camera_to_target_guess = camera_to_target_guess; 92 | 93 | // Add the detected correspondences 94 | problem.correspondences = target_finder->target().createCorrespondences(target_features); 95 | 96 | problem_set.push_back(problem); 97 | } 98 | 99 | // Perform the noise qualification 100 | PnPNoiseStat result = qualifyNoise2D(problem_set); 101 | 102 | // Print the results 103 | Eigen::IOFormat fmt(4, 0, ",", "\n", "[", "]"); 104 | ROS_INFO_STREAM("Camera to Target Noise Results"); 105 | ROS_INFO_STREAM("Position mean (m)\n" << result.p_stat.mean.transpose().format(fmt)); 106 | ROS_INFO_STREAM("Position standard deviation (m)\n" << result.p_stat.stdev.transpose().format(fmt)); 107 | ROS_INFO_STREAM("Quaternion mean (qx, qy, qz, qw)\n" << result.q_stat.mean.coeffs().transpose().format(fmt)); 108 | ROS_INFO_STREAM("Quaternion standard deviation\n" << result.q_stat.stdev); 109 | } 110 | catch (const std::exception& ex) 111 | { 112 | ROS_ERROR_STREAM(ex.what()); 113 | return -1; 114 | } 115 | 116 | return 0; 117 | } 118 | -------------------------------------------------------------------------------- /rct_image_tools/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.16.0) 2 | 3 | # Extract package name and version from package.xml 4 | find_package(ros_industrial_cmake_boilerplate REQUIRED) 5 | extract_package_metadata(pkg) 6 | project(${pkg_extracted_name} VERSION ${pkg_extracted_version} LANGUAGES CXX) 7 | 8 | find_package(Boost REQUIRED COMPONENTS filesystem) 9 | find_package(OpenCV REQUIRED) 10 | find_package(yaml-cpp REQUIRED) 11 | find_package(rct_optimizations REQUIRED) 12 | 13 | find_package(Eigen3 REQUIRED) 14 | if(NOT EIGEN3_INCLUDE_DIRS) 15 | set(EIGEN3_INCLUDE_DIRS ${EIGEN3_INCLUDE_DIR}) 16 | endif() 17 | if(NOT TARGET Eigen3::Eigen) 18 | find_package(Threads REQUIRED) 19 | add_library(Eigen3::Eigen IMPORTED INTERFACE) 20 | set_property(TARGET Eigen3::Eigen PROPERTY INTERFACE_COMPILE_DEFINITIONS ${EIGEN3_DEFINITIONS}) 21 | set_property(TARGET Eigen3::Eigen PROPERTY INTERFACE_INCLUDE_DIRECTORIES ${EIGEN3_INCLUDE_DIRS}) 22 | endif() 23 | 24 | add_library( 25 | ${PROJECT_NAME} SHARED 26 | src/${PROJECT_NAME}/circle_detector.cpp 27 | src/${PROJECT_NAME}/modified_circle_grid_target.cpp 28 | src/${PROJECT_NAME}/modified_circle_grid_finder.cpp 29 | src/${PROJECT_NAME}/aruco_grid_target.cpp 30 | src/${PROJECT_NAME}/aruco_finder.cpp 31 | src/${PROJECT_NAME}/charuco_grid_target.cpp 32 | src/${PROJECT_NAME}/charuco_finder.cpp) 33 | target_compile_features(${PROJECT_NAME} PUBLIC cxx_std_11) 34 | target_compile_options(${PROJECT_NAME} PRIVATE -Wall -Wextra) 35 | target_include_directories(${PROJECT_NAME} PUBLIC "$" 36 | "$") 37 | target_link_libraries( 38 | ${PROJECT_NAME} 39 | PUBLIC Boost::boost 40 | Boost::filesystem 41 | opencv_core 42 | opencv_aruco 43 | opencv_features2d 44 | opencv_imgproc 45 | opencv_highgui 46 | yaml-cpp 47 | Eigen3::Eigen 48 | rct::rct_optimizations) 49 | 50 | # Modified circle grid finder executable TODO: turn this into a unit test 51 | add_executable(${PROJECT_NAME}_test src/target_finder_tests.cpp) 52 | target_compile_features(${PROJECT_NAME} PRIVATE cxx_std_11) 53 | target_compile_options(${PROJECT_NAME}_test PRIVATE -Wall -Wextra) 54 | target_include_directories(${PROJECT_NAME}_test PUBLIC "$" 55 | "$") 56 | target_link_libraries(${PROJECT_NAME}_test ${PROJECT_NAME} rct::rct_optimizations) 57 | 58 | if(RCT_BUILD_TESTS) 59 | enable_testing() 60 | add_run_tests_target(ENABLE ${RCT_ENABLE_RUN_TESTS}) 61 | add_subdirectory(test) 62 | endif() 63 | 64 | configure_package( 65 | NAMESPACE rct 66 | TARGETS ${PROJECT_NAME} 67 | ${PROJECT_NAME}_test 68 | DEPENDENCIES 69 | "Boost REQUIRED COMPONENTS filesystem" 70 | OpenCV 71 | rct_optimizations 72 | yaml-cpp) 73 | 74 | install(DIRECTORY include/${PROJECT_NAME} DESTINATION include) 75 | -------------------------------------------------------------------------------- /rct_image_tools/README.md: -------------------------------------------------------------------------------- 1 | # RCT Image Tools 2 | ## Description 3 | This package provides some support for extracting "target features" from images of targets. Nominally, we use the "modified circle grid" target. See the discussion of target 4 | conventions in [the calibration primer](../cal_primer.md). 5 | 6 | Jeremy Zoss wrote an awesome target creation script that can be found [here](script/calibration_target.py). See the file's docs for more information. 7 | 8 | ## Examples 9 | Given this input image: 10 | 11 | ![Input-Image](./docs/input.png) 12 | 13 | When running your calibration you should see this: 14 | 15 | ![Output-Image](./docs/output.png) 16 | 17 | Note that the large dot is labeled as the origin and is in the bottom left corner if you were to rotate the first point to the top left. 18 | ***It is possible for this to screw up*** when points are observed from very skew angles. Make sure it looks right or you will have bad 19 | convergence. 20 | 21 | -------------------------------------------------------------------------------- /rct_image_tools/docs/input.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Jmeyer1292/robot_cal_tools/2089a06d11b13323cf6c358a89524231801e5b06/rct_image_tools/docs/input.png -------------------------------------------------------------------------------- /rct_image_tools/docs/output.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Jmeyer1292/robot_cal_tools/2089a06d11b13323cf6c358a89524231801e5b06/rct_image_tools/docs/output.png -------------------------------------------------------------------------------- /rct_image_tools/include/rct_image_tools/aruco_finder.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | 6 | #include 7 | #include 8 | 9 | namespace rct_image_tools 10 | { 11 | /** 12 | * @brief This class finds 2D target features from images of a specified ArUco gridboard target. 13 | * The main advantage of this kind of target is that partial views still provide usable correspondences. 14 | * Target features are returned as a map where the marker ID is the key and the image coordinates of the 15 | * marker corners are the mapped value. 16 | */ 17 | class ArucoGridBoardTargetFinder : public TargetFinder 18 | { 19 | public: 20 | ArucoGridBoardTargetFinder(const ArucoGridTarget& target); 21 | 22 | /** 23 | * @brief Detect marker corner coordinates in the provided image. 24 | * @param image - Input image, ideally containing an ArUco gridboard. 25 | * @return Map matching marker ID numbers to a vector of marker corner coordinates. The vector will contain 26 | * four corners defined in the same order as in the output of the function cv::aruco::DetectMarkers() 27 | * (e.g. clockwise from the "origin" corner). 28 | */ 29 | virtual TargetFeatures findTargetFeatures(const cv::Mat& image) const override; 30 | 31 | /** 32 | * @brief A debugging utility that will draw a set of target features onto an image for 33 | * display purposes. Usually you want to call findTargetFeatures() above then this with the result. 34 | * @param image - The image of the target 35 | * @param target_features - The target features identified in the input image 36 | */ 37 | virtual cv::Mat drawTargetFeatures(const cv::Mat& image, const TargetFeatures& target_features) const override; 38 | 39 | virtual const Target& target() const override { return target_; } 40 | 41 | protected: 42 | const ArucoGridTarget target_; 43 | }; 44 | 45 | } // namespace rct_image_tools 46 | -------------------------------------------------------------------------------- /rct_image_tools/include/rct_image_tools/aruco_grid_target.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | #include 6 | 7 | namespace rct_image_tools 8 | { 9 | /** 10 | * @brief Structure containing relevant data for a ArUco grid target 11 | */ 12 | struct ArucoGridTarget : Target 13 | { 14 | /** 15 | * @brief Constructor 16 | * @param rows - number of rows in the target 17 | * @param cols - number of columns in the target 18 | * @param aruco_marker_dim - The length of the side of one ArUco marker (m) 19 | * @param marker_gap - The size of the gap between adjacent arUco markers (m) 20 | * @param dictionary_id - The dictionary of ArUco markers to use 21 | */ 22 | ArucoGridTarget(const int rows, 23 | const int cols, 24 | const float aruco_marker_dim, 25 | const float marker_gap, 26 | const int dictionary_id = cv::aruco::DICT_6X6_250); 27 | 28 | /** 29 | * @brief Constructor 30 | * @param board_in - OpenCV ArUco GridBoard object defining rows, columns, marker size, and marker spacing 31 | */ 32 | ArucoGridTarget(const cv::Ptr& board_in); 33 | 34 | bool operator==(const ArucoGridTarget& other) const; 35 | 36 | /** 37 | * @brief Creates a set of correspondences between the corners of each ArUco tag observed in an image (ordered 38 | * clockwise from the "origin" corner) and their counterparts in the target (matched by ID) 39 | * @param target_features - Map of ArUco tag corners observed in an image 40 | * @return Set of corresponding features in the image to features in the ArUco grid target 41 | */ 42 | virtual rct_optimizations::Correspondence2D3D::Set 43 | createCorrespondences(const TargetFeatures& target_features) const override; 44 | 45 | /** @brief Representation of the ArUco grid target */ 46 | cv::Ptr board; 47 | /** @brief Map of 3D ArUco tag corners with corresponding IDs */ 48 | std::map> points; 49 | }; 50 | } // namespace rct_image_tools 51 | -------------------------------------------------------------------------------- /rct_image_tools/include/rct_image_tools/charuco_finder.h: -------------------------------------------------------------------------------- 1 | /** 2 | * ChAruco gridboard detector, following the same pattern as ModifiedCircleGridTargetFinder. 3 | * Author: John Berkebile 4 | */ 5 | #pragma once 6 | 7 | #include 8 | #include 9 | 10 | namespace rct_image_tools 11 | { 12 | /** 13 | * @brief This class finds 2D features from images of a specified ChArUco gridboard target. 14 | * The main advantage of this kind of target is that partial views still provide usable correspondences. 15 | */ 16 | class CharucoGridBoardTargetFinder : public TargetFinder 17 | { 18 | public: 19 | CharucoGridBoardTargetFinder(const CharucoGridTarget& target); 20 | 21 | /** 22 | * @brief Detects chessboard intersection coordinates in the provided image. 23 | * @param image - Input image, ideally containing a ChArUco grid target. 24 | * @return Map matching marker ID numbers to the 2D position of the chessboard intersections 25 | */ 26 | virtual TargetFeatures findTargetFeatures(const cv::Mat& image) const override; 27 | 28 | /** 29 | * @brief A debugging utility that will draw target features set onto an input image for display purposes 30 | * @param image - Input image, ideally containing a ChArUco grid target 31 | * @param target_features - Chessboard intersections (obtained by calling @ref findTargetFeatures) 32 | * @return An image with the chessboard intersections and IDs overlaid on the input image 33 | */ 34 | virtual cv::Mat drawTargetFeatures(const cv::Mat& image, const TargetFeatures& target_features) const override; 35 | 36 | virtual const Target& target() const override { return target_; } 37 | 38 | protected: 39 | const CharucoGridTarget target_; 40 | }; 41 | 42 | } // namespace rct_image_tools 43 | -------------------------------------------------------------------------------- /rct_image_tools/include/rct_image_tools/charuco_grid_target.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | #include 6 | #include 7 | #include 8 | 9 | namespace rct_image_tools 10 | { 11 | /** 12 | * @brief Structure containing relevant data for a ChArUco grid target 13 | */ 14 | struct CharucoGridTarget : public Target 15 | { 16 | /** 17 | * @brief Constructor 18 | * @param rows - number of rows in the target 19 | * @param cols - number of columns in the target 20 | * @param chessboard_dim - The length of the side of one chessboard square (m) 21 | * @param aruco_marker_dim - The length of the side of one ArUco marker (m) 22 | * @param dictionary_id - The enumeration ID of the dictionary of ArUco markers to use. 23 | * ArUco 25 | * dictionary enumerations 26 | */ 27 | CharucoGridTarget(const int rows, 28 | const int cols, 29 | const float chessboard_dim, 30 | const float aruco_marker_dim, 31 | const int dictionary_id = cv::aruco::DICT_6X6_250); 32 | 33 | CharucoGridTarget(const cv::Ptr& board_in); 34 | 35 | bool operator==(const CharucoGridTarget& other) const; 36 | 37 | /** 38 | * @brief Creates a set of correspondences between chessboard intersections observed in an image and their 39 | * counterparts in the target (matched by ID) 40 | * @param target_features - Map of observed chessboard intersections and their IDs 41 | * @return Set of corresponding features in the image to features in the ChArUco target 42 | */ 43 | virtual rct_optimizations::Correspondence2D3D::Set 44 | createCorrespondences(const TargetFeatures& target_features) const override; 45 | 46 | /** @brief Representation of the ChArUco board target */ 47 | cv::Ptr board; 48 | /** @brief Map of 3D chessboard corners with corresponding IDs */ 49 | std::map points; 50 | }; 51 | 52 | } // namespace rct_image_tools 53 | -------------------------------------------------------------------------------- /rct_image_tools/include/rct_image_tools/circle_detector.h: -------------------------------------------------------------------------------- 1 | #ifndef RCT_IMAGE_TOOLS_CIRCLE_DETECTOR_H 2 | #define RCT_IMAGE_TOOLS_CIRCLE_DETECTOR_H 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | namespace rct_image_tools 9 | { 10 | struct CircleDetectorParams 11 | { 12 | /** @brief The minimum grayscale pixel intensity value at which to start the image thresholding (inclusive) */ 13 | float minThreshold = 50; 14 | /** @brief The maximum grayscale pixel intensity value at which to stop the image thresholding (inclusive) */ 15 | float maxThreshold = 220; 16 | /** @brief The number of thresholding steps to apply */ 17 | std::size_t nThresholds = 18; 18 | 19 | /** @brief The number of times a particular circle must be identified to be considered valid (must be <= the number of 20 | * threshold steps) */ 21 | size_t minRepeatability = 3; 22 | /** @brief The radius (pixels) around an identified circle within which new detected blobs will be considered to be 23 | * the same feature as the circle */ 24 | float circleInclusionRadius = 5; 25 | /** @brief The maximum difference in radius (pixels) between an identified circle and a detected blob, above which 26 | * the blob will not be considered to be the same feature as the previously identified circle */ 27 | float maxRadiusDiff = 5; 28 | 29 | /** @brief The maximum average deviation of the contour of a blob from its calculated ellipse fit (percentage) */ 30 | float maxAverageEllipseError = 0.02f; 31 | 32 | /** @brief Flag for color filtering */ 33 | bool filterByColor = true; 34 | /** @brief Color intensity of circle center in the binary image (value must be 0 or 255) */ 35 | uchar circleColor = 0; 36 | 37 | /** @brief Flag for filtering by area */ 38 | bool filterByArea = true; 39 | /** @brief Minimum blob area (px^2) */ 40 | float minArea = 25.0f; 41 | /** @brief Maximum blob area (px^2) */ 42 | float maxArea = 5000.0f; 43 | 44 | /** @brief Flag for circularity filtering */ 45 | bool filterByCircularity = false; 46 | /** @brief Minimum blob circularity ratio - for a perfect circle this value is 1.0 / PI (~0.333) */ 47 | float minCircularity = 0.8f; 48 | /**@ brief Maximum blob circularity ratio */ 49 | float maxCircularity = std::numeric_limits::max(); 50 | 51 | /** @brief Flag for inertia filtering */ 52 | bool filterByInertia = false; 53 | /** @brief Minimum blob inertia ratio */ 54 | float minInertiaRatio = 0.3f; 55 | /** @brief Maximum blob inertia ratio - for a perfect circle, this value is 1.0 */ 56 | float maxInertiaRatio = std::numeric_limits::max(); 57 | 58 | /** @brief Flag for convexity filtering */ 59 | bool filterByConvexity = true; 60 | /** @brief Minimum blob convexity */ 61 | float minConvexity = 0.95f; 62 | /** @brief Maximum blob convexity */ 63 | float maxConvexity = std::numeric_limits::max(); 64 | }; 65 | 66 | class CircleDetector : public cv::FeatureDetector 67 | { 68 | public: 69 | CircleDetector(const CircleDetectorParams& parameters); 70 | 71 | struct CV_EXPORTS Center 72 | { 73 | cv::Point2d location; 74 | double radius; 75 | double confidence; 76 | }; 77 | 78 | /** 79 | * @brief Detects circle keypoints in an image 80 | * @param image 81 | * @param keypoints 82 | * @param mask 83 | */ 84 | virtual void detect(cv::InputArray image, 85 | std::vector& keypoints, 86 | cv::InputArray mask = cv::noArray()) override; 87 | 88 | /** 89 | * @brief Draws the contours and keypoints of detected circles 90 | * @param image 91 | * @return 92 | */ 93 | cv::Mat drawDetectedCircles(const cv::Mat& image); 94 | 95 | /** 96 | * @brief Creates a circle detector pointer from a parameter structure 97 | * @param params 98 | * @return 99 | */ 100 | static cv::Ptr create(const CircleDetectorParams& params = CircleDetectorParams()); 101 | 102 | protected: 103 | const CircleDetectorParams params; 104 | }; 105 | } // namespace rct_image_tools 106 | #endif // RCT_IMAGE_TOOLS_CIRCLE_DETECTOR_H 107 | -------------------------------------------------------------------------------- /rct_image_tools/include/rct_image_tools/image_utils.h: -------------------------------------------------------------------------------- 1 | #ifndef RCT_IMAGE_UTILS_H 2 | #define RCT_IMAGE_UTILS_H 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | namespace rct_image_tools 10 | { 11 | /** 12 | * @brief Get the uv coordinates of a point reprojected into the image frame 13 | * @param camera_to_target The transformation from the camera frame to the target frame 14 | * @param intr The intrinsic values of the camera 15 | * @param target_points A vector of target points 16 | * @return A vector of uv values in the image frame 17 | */ 18 | inline std::vector getReprojections(const Eigen::Isometry3d& camera_to_target, 19 | const rct_optimizations::CameraIntrinsics& intr, 20 | const std::vector& target_points) 21 | { 22 | std::vector reprojections; 23 | for (const auto& point_in_target : target_points) 24 | { 25 | Eigen::Vector3d in_camera = camera_to_target * point_in_target; 26 | 27 | double uv[2]; 28 | rct_optimizations::projectPoint(intr, in_camera.data(), uv); 29 | 30 | reprojections.push_back(cv::Point2d(uv[0], uv[1])); 31 | } 32 | return reprojections; 33 | } 34 | 35 | /** 36 | * @brief Draw a set of reprojections on an image 37 | * @param reprojections A vector of reprojections to be drawn on the image 38 | * @param size The size of circle to drawn 39 | * @param color The color of the circle drawn 40 | * @param image The image to draw the reprojections on 41 | */ 42 | inline void drawReprojections(const std::vector& reprojections, int size, cv::Scalar color, cv::Mat& image) 43 | { 44 | for (const auto& pt : reprojections) 45 | { 46 | cv::circle(image, pt, size, color); 47 | } 48 | } 49 | 50 | } // namespace rct_image_tools 51 | #endif // RCT_IMAGE_UTILS_H 52 | -------------------------------------------------------------------------------- /rct_image_tools/include/rct_image_tools/modified_circle_grid_finder.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | 7 | namespace rct_image_tools 8 | { 9 | /** 10 | * @brief This class finds 2D features (circle centers) from images of a known ModifiedCircleGridTarget. 11 | * All points must be seen or it will fail. Features are returned in the same order as points are defined in the target. 12 | */ 13 | class ModifiedCircleGridTargetFinder : public TargetFinder 14 | { 15 | public: 16 | ModifiedCircleGridTargetFinder(const ModifiedCircleGridTarget& target); 17 | ModifiedCircleGridTargetFinder(const ModifiedCircleGridTarget& target, const CircleDetectorParams& params); 18 | 19 | /** 20 | * @brief Finds target features in an input image 21 | * @param image 22 | * @return 23 | */ 24 | virtual TargetFeatures findTargetFeatures(const cv::Mat& image) const override; 25 | 26 | /** 27 | * @brief A debugging utility that will draw target features onto an image for display purposes. 28 | * Usually you want to call findTargetFeatures() above then this with the result. 29 | */ 30 | virtual cv::Mat drawTargetFeatures(const cv::Mat& image, const TargetFeatures& target_features) const override; 31 | 32 | virtual const Target& target() const override { return target_; } 33 | 34 | inline const CircleDetectorParams& getCircleDetectorParams() const { return params_; } 35 | 36 | protected: 37 | const ModifiedCircleGridTarget target_; 38 | const CircleDetectorParams params_; 39 | }; 40 | 41 | } // namespace rct_image_tools 42 | -------------------------------------------------------------------------------- /rct_image_tools/include/rct_image_tools/modified_circle_grid_target.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | namespace rct_image_tools 6 | { 7 | /** 8 | * @brief Structure containing the necessary data to represent a modified circle grid target 9 | */ 10 | struct ModifiedCircleGridTarget : Target 11 | { 12 | /** 13 | * @brief Constructor 14 | * @param rows - Number of rows in the target 15 | * @param cols - Number of columns in the target 16 | * @param spacing - The spacing between adjacent circle centers (m) 17 | */ 18 | ModifiedCircleGridTarget(const unsigned rows, const unsigned cols, const double spacing); 19 | 20 | bool operator==(const ModifiedCircleGridTarget& other) const; 21 | 22 | virtual rct_optimizations::Correspondence2D3D::Set 23 | createCorrespondences(const TargetFeatures& target_features) const override; 24 | 25 | std::vector createPoints() const; 26 | 27 | unsigned rows; 28 | unsigned cols; 29 | double spacing; 30 | }; 31 | 32 | } // namespace rct_image_tools 33 | -------------------------------------------------------------------------------- /rct_image_tools/include/rct_image_tools/target.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | 6 | namespace rct_image_tools 7 | { 8 | /** 9 | * @brief Base class for calibration target definitions 10 | */ 11 | struct Target 12 | { 13 | Target() = default; 14 | virtual ~Target() = default; 15 | 16 | /** 17 | * @brief Creates a set of correspondences between an input set of target features (identified in a 2D image) and the 18 | * same features from the known geometry of the target 19 | * @param target_features - map of target features identified in a 2D image 20 | * @return 21 | */ 22 | virtual rct_optimizations::Correspondence2D3D::Set 23 | createCorrespondences(const TargetFeatures& target_features) const = 0; 24 | }; 25 | 26 | } // namespace rct_image_tools 27 | -------------------------------------------------------------------------------- /rct_image_tools/include/rct_image_tools/target_features.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | 7 | namespace rct_image_tools 8 | { 9 | /** @brief Typedef for an STL vector of Eigen vector objects. Use a specialized allocator in the case that types 10 | * divisible by 16 bytes are used, specifically Eigen::Vector2d */ 11 | template 12 | using VectorEigenVector = 13 | std::vector, Eigen::aligned_allocator>>; 14 | 15 | /** Typedef for an STL map with an unsigned integer key and STL vector of Eigen vector value */ 16 | template 17 | using MapVectorEigenVector = std::map>; 18 | 19 | /** @brief Typedef for a container of target features from a calibration target. 20 | * This definition allows for multiple features to be associated with a single unique identifier 21 | * (such as the corners of an ArUco tag) 22 | */ 23 | using TargetFeatures = MapVectorEigenVector<2>; 24 | 25 | } // namespace rct_image_tools 26 | -------------------------------------------------------------------------------- /rct_image_tools/include/rct_image_tools/target_finder.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | 7 | namespace rct_image_tools 8 | { 9 | /** 10 | * @brief Base class for target finders 11 | */ 12 | class TargetFinder 13 | { 14 | public: 15 | TargetFinder() = default; 16 | virtual ~TargetFinder() = default; 17 | 18 | /** 19 | * @brief Finds the features of the target in an image 20 | * @param image 21 | * @return 22 | */ 23 | virtual TargetFeatures findTargetFeatures(const cv::Mat& image) const = 0; 24 | 25 | /** 26 | * @brief Draws the target features on an input image 27 | * @param image 28 | * @param target_features 29 | * @return 30 | */ 31 | virtual cv::Mat drawTargetFeatures(const cv::Mat& image, const TargetFeatures& target_features) const = 0; 32 | 33 | /** 34 | * @brief Returns the definition of the target used by the finder 35 | * @return 36 | */ 37 | virtual const Target& target() const = 0; 38 | }; 39 | 40 | } // namespace rct_image_tools 41 | -------------------------------------------------------------------------------- /rct_image_tools/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | rct_image_tools 4 | 0.2.1 5 | The rct_image_tools package 6 | 7 | Jonathan Meyer 8 | 9 | Apache 2.0 10 | 11 | ros_industrial_cmake_boilerplate 12 | boost 13 | rct_common 14 | rct_optimizations 15 | yaml-cpp 16 | libopencv-dev 17 | eigen 18 | 19 | 20 | cmake 21 | 22 | 23 | 24 | -------------------------------------------------------------------------------- /rct_image_tools/script/example_5x5_15mm_spaces.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Jmeyer1292/robot_cal_tools/2089a06d11b13323cf6c358a89524231801e5b06/rct_image_tools/script/example_5x5_15mm_spaces.pdf -------------------------------------------------------------------------------- /rct_image_tools/src/rct_image_tools/aruco_finder.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * ArUco gridboard detector, following the same pattern as ModifiedCircleGridTargetFinder. 3 | * Author: Joseph Schornak 4 | */ 5 | 6 | #include "rct_image_tools/aruco_finder.h" 7 | 8 | namespace rct_image_tools 9 | { 10 | ArucoGridBoardTargetFinder::ArucoGridBoardTargetFinder(const ArucoGridTarget& target) : TargetFinder(), target_(target) 11 | { 12 | } 13 | 14 | TargetFeatures ArucoGridBoardTargetFinder::findTargetFeatures(const cv::Mat& image) const 15 | { 16 | TargetFeatures map_ids_to_obs_corners; 17 | 18 | std::vector> marker_corners, rejected_candidates; 19 | std::vector marker_ids; 20 | cv::Ptr parameters(new cv::aruco::DetectorParameters); 21 | 22 | cv::aruco::detectMarkers( 23 | image, target_.board->dictionary, marker_corners, marker_ids, parameters, rejected_candidates); 24 | cv::aruco::refineDetectedMarkers(image, target_.board, marker_corners, marker_ids, rejected_candidates); 25 | 26 | for (unsigned i = 0; i < marker_ids.size(); i++) 27 | { 28 | std::vector corner_pts = marker_corners[i]; 29 | VectorEigenVector<2> obs_pts(4); 30 | 31 | for (unsigned j = 0; j < corner_pts.size(); j++) 32 | { 33 | obs_pts[j] = Eigen::Vector2d(corner_pts[j].x, corner_pts[j].y).cast(); 34 | } 35 | map_ids_to_obs_corners.emplace(marker_ids[i], obs_pts); 36 | } 37 | return map_ids_to_obs_corners; 38 | } 39 | 40 | cv::Mat ArucoGridBoardTargetFinder::drawTargetFeatures(const cv::Mat& image, 41 | const TargetFeatures& target_features) const 42 | { 43 | std::vector marker_ids; 44 | std::vector> marker_corners; 45 | for (auto it = target_features.begin(); it != target_features.end(); ++it) 46 | { 47 | marker_ids.push_back(it->first); 48 | std::vector cv_obs(it->second.size()); 49 | std::transform(it->second.begin(), it->second.end(), cv_obs.begin(), [](const Eigen::Vector2d& o) { 50 | return cv::Point2d(o.x(), o.y()); 51 | }); 52 | marker_corners.push_back(cv_obs); 53 | } 54 | cv::aruco::drawDetectedMarkers(image, marker_corners, marker_ids); 55 | return image; 56 | } 57 | } // namespace rct_image_tools 58 | -------------------------------------------------------------------------------- /rct_image_tools/src/rct_image_tools/aruco_grid_target.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | namespace 4 | { 5 | /** 6 | * @brief For a given ArUco GridBoard, create a map of marker corner coordinates keyed to marker IDs. 7 | * @param board - ArUco GridBoard to use when generating the map 8 | * @return Resulting map. Keys are the IDs for the ArUco markers in the board. Values are vectors containing the four 9 | * board-relative coordinates for the corners of the marker. 10 | */ 11 | std::map> mapArucoIdsToObjPts(const cv::Ptr& board) 12 | { 13 | std::map> map_ids_to_corners; 14 | for (std::size_t i = 0; i < board->ids.size(); i++) 15 | { 16 | std::vector obj_pts(board->objPoints[i].size()); 17 | std::transform( 18 | board->objPoints[i].begin(), 19 | board->objPoints[i].end(), 20 | obj_pts.begin(), 21 | [](const cv::Point3f& o) -> Eigen::Vector3d { return Eigen::Vector3f(o.x, o.y, o.z).cast(); }); 22 | 23 | map_ids_to_corners.insert(std::make_pair(board->ids[i], obj_pts)); 24 | } 25 | return map_ids_to_corners; 26 | } 27 | } // namespace 28 | 29 | namespace rct_image_tools 30 | { 31 | ArucoGridTarget::ArucoGridTarget(const int rows, 32 | const int cols, 33 | const float aruco_marker_dim, 34 | const float marker_gap, 35 | const int dictionary_id) 36 | : ArucoGridTarget(cv::aruco::GridBoard::create(cols, 37 | rows, 38 | aruco_marker_dim, 39 | marker_gap, 40 | cv::aruco::getPredefinedDictionary(dictionary_id))) 41 | { 42 | } 43 | 44 | ArucoGridTarget::ArucoGridTarget(const cv::Ptr& board_in) 45 | : board(board_in), points(mapArucoIdsToObjPts(board)) 46 | { 47 | } 48 | 49 | bool ArucoGridTarget::operator==(const ArucoGridTarget& other) const 50 | { 51 | auto board_size = board->getGridSize(); 52 | auto other_board_size = other.board->getGridSize(); 53 | bool equal = true; 54 | equal &= other_board_size.width == board_size.width; 55 | equal &= other_board_size.height == board_size.height; 56 | equal &= 57 | (std::abs(other.board->getMarkerLength() - board->getMarkerLength()) < std::numeric_limits::epsilon()); 58 | equal &= (std::abs(other.board->getMarkerSeparation() - board->getMarkerSeparation()) < 59 | std::numeric_limits::epsilon()); 60 | equal &= other.points == points; 61 | return equal; 62 | } 63 | 64 | rct_optimizations::Correspondence2D3D::Set 65 | ArucoGridTarget::createCorrespondences(const TargetFeatures& target_features) const 66 | { 67 | rct_optimizations::Correspondence2D3D::Set correspondences; 68 | correspondences.reserve(target_features.size()); 69 | 70 | for (auto it = target_features.begin(); it != target_features.end(); ++it) 71 | { 72 | for (std::size_t i = 0; i < it->second.size(); ++i) 73 | { 74 | rct_optimizations::Correspondence2D3D corr; 75 | corr.in_target = points.at(it->first).at(i); 76 | corr.in_image = target_features.at(it->first).at(i); 77 | correspondences.push_back(corr); 78 | } 79 | } 80 | return correspondences; 81 | } 82 | 83 | } // namespace rct_image_tools 84 | -------------------------------------------------------------------------------- /rct_image_tools/src/rct_image_tools/charuco_finder.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | #include 5 | 6 | namespace rct_image_tools 7 | { 8 | CharucoGridBoardTargetFinder::CharucoGridBoardTargetFinder(const CharucoGridTarget& target) 9 | : TargetFinder(), target_(target) 10 | { 11 | } 12 | 13 | TargetFeatures CharucoGridBoardTargetFinder::findTargetFeatures(const cv::Mat& image) const 14 | { 15 | // Create a generic set of parameters 16 | // TODO: expose the setting of these parameters 17 | cv::Ptr parameters = cv::aruco::DetectorParameters::create(); 18 | 19 | // Detect the ArUco markers 20 | std::vector marker_ids; 21 | std::vector> marker_corners; 22 | cv::aruco::detectMarkers(image, target_.board->dictionary, marker_corners, marker_ids, parameters); 23 | 24 | if (marker_ids.empty()) 25 | { 26 | throw std::runtime_error("No ArUco markers were detected"); 27 | } 28 | 29 | // Detect the chessboard intersections given the observed ArUco markers 30 | std::vector charuco_corners; 31 | std::vector charuco_ids; 32 | int detected_corners = cv::aruco::interpolateCornersCharuco( 33 | marker_corners, marker_ids, image, target_.board, charuco_corners, charuco_ids); 34 | 35 | // Create the map of observed features 36 | TargetFeatures target_features; 37 | for (unsigned i = 0; i < static_cast(detected_corners); i++) 38 | { 39 | const cv::Point2f& corner = charuco_corners.at(i); 40 | VectorEigenVector<2> v_obs; 41 | v_obs.push_back(Eigen::Vector2d(corner.x, corner.y)); 42 | target_features.emplace(static_cast(charuco_ids[i]), v_obs); 43 | } 44 | 45 | return target_features; 46 | } 47 | 48 | cv::Mat CharucoGridBoardTargetFinder::drawTargetFeatures(const cv::Mat& image, 49 | const TargetFeatures& target_features) const 50 | { 51 | std::vector charuco_ids; 52 | charuco_ids.reserve(target_features.size()); 53 | 54 | std::vector charuco_corners; 55 | charuco_corners.reserve(target_features.size()); 56 | 57 | for (auto it = target_features.begin(); it != target_features.end(); ++it) 58 | { 59 | // Add the ID 60 | charuco_ids.push_back(static_cast(it->first)); 61 | 62 | // Add the image coordinates 63 | const Eigen::Vector2f& pt = it->second.at(0).cast(); 64 | charuco_corners.push_back(cv::Point2f(pt.x(), pt.y())); 65 | } 66 | 67 | // Draw the detected corners 68 | cv::aruco::drawDetectedCornersCharuco(image, charuco_corners, charuco_ids, cv::Scalar(255, 0, 0)); 69 | 70 | return image; 71 | } 72 | 73 | } // namespace rct_image_tools 74 | -------------------------------------------------------------------------------- /rct_image_tools/src/rct_image_tools/charuco_grid_target.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | namespace 4 | { 5 | /** 6 | * @brief For a given ChArUco board, create a map of chessboard intersection coordinates keyed to marker IDs. 7 | * @param board - ChArUco board to use when generating the map 8 | * @return Resulting map. Keys are the indices of chessboard intersections in the board. Values are board-relative 9 | * coordinates for each intersection. 10 | */ 11 | std::map mapCharucoIdsToObjPts(const cv::Ptr& board) 12 | { 13 | std::map points; 14 | for (std::size_t i = 0; i < board->chessboardCorners.size(); i++) 15 | { 16 | const auto& corner = board->chessboardCorners.at(i); 17 | Eigen::Vector3f pt(corner.x, corner.y, corner.z); 18 | points.emplace(i, pt.cast()); 19 | } 20 | return points; 21 | } 22 | } // namespace 23 | 24 | namespace rct_image_tools 25 | { 26 | CharucoGridTarget::CharucoGridTarget(const int rows, 27 | const int cols, 28 | const float chessboard_dim, 29 | const float aruco_marker_dim, 30 | const int dictionary_id) 31 | : CharucoGridTarget::CharucoGridTarget( 32 | cv::aruco::CharucoBoard::create(cols, 33 | rows, 34 | chessboard_dim, 35 | aruco_marker_dim, 36 | cv::aruco::getPredefinedDictionary(dictionary_id))) 37 | { 38 | } 39 | 40 | CharucoGridTarget::CharucoGridTarget(const cv::Ptr& board_in) 41 | : board(board_in), points(mapCharucoIdsToObjPts(board)) 42 | { 43 | } 44 | 45 | bool CharucoGridTarget::operator==(const CharucoGridTarget& other) const 46 | { 47 | auto board_size = board->getChessboardSize(); 48 | auto other_board_size = other.board->getChessboardSize(); 49 | bool equal = true; 50 | equal &= other.points == points; 51 | equal &= 52 | (std::abs(other.board->getMarkerLength() - board->getMarkerLength()) < std::numeric_limits::epsilon()); 53 | equal &= 54 | (std::abs(other.board->getSquareLength() - board->getSquareLength()) < std::numeric_limits::epsilon()); 55 | equal &= other_board_size.width == board_size.width; 56 | equal &= other_board_size.height == board_size.height; 57 | return equal; 58 | } 59 | 60 | std::vector 61 | CharucoGridTarget::createCorrespondences(const TargetFeatures& target_features) const 62 | { 63 | std::vector correspondences; 64 | correspondences.reserve(target_features.size()); 65 | 66 | for (auto it = target_features.begin(); it != target_features.end(); it++) 67 | { 68 | rct_optimizations::Correspondence2D3D corr; 69 | corr.in_target = points.at(it->first); 70 | // Get the first (and only) feature from the current iterator 71 | corr.in_image = target_features.at(it->first).at(0); 72 | correspondences.push_back(corr); 73 | } 74 | return correspondences; 75 | } 76 | 77 | } // namespace rct_image_tools 78 | -------------------------------------------------------------------------------- /rct_image_tools/src/rct_image_tools/modified_circle_grid_target.cpp: -------------------------------------------------------------------------------- 1 | #include "rct_image_tools/modified_circle_grid_target.h" 2 | 3 | namespace rct_image_tools 4 | { 5 | ModifiedCircleGridTarget::ModifiedCircleGridTarget(const unsigned rows_, const unsigned cols_, const double spacing_) 6 | : rows(rows_), cols(cols_), spacing(spacing_) 7 | { 8 | } 9 | 10 | bool ModifiedCircleGridTarget::operator==(const ModifiedCircleGridTarget& other) const 11 | { 12 | bool equal = true; 13 | equal &= (rows == other.rows); 14 | equal &= (cols == other.cols); 15 | equal &= (std::abs(spacing - other.spacing) < std::numeric_limits::epsilon()); 16 | 17 | return equal; 18 | } 19 | 20 | rct_optimizations::Correspondence2D3D::Set 21 | ModifiedCircleGridTarget::createCorrespondences(const TargetFeatures& target_features) const 22 | { 23 | std::vector target_points = createPoints(); 24 | assert(target_features.size() == target_points.size()); 25 | 26 | rct_optimizations::Correspondence2D3D::Set correspondences; 27 | correspondences.reserve(target_features.size()); 28 | 29 | for (std::size_t i = 0; i < target_points.size(); ++i) 30 | { 31 | rct_optimizations::Correspondence2D3D corr; 32 | corr.in_target = target_points.at(i); 33 | // Get the first (and only) element of the features at the current index; increment the index 34 | corr.in_image = target_features.at(i).at(0); 35 | correspondences.push_back(corr); 36 | } 37 | 38 | return correspondences; 39 | } 40 | 41 | std::vector ModifiedCircleGridTarget::createPoints() const 42 | { 43 | std::vector points; 44 | points.reserve(rows * cols); 45 | 46 | for (std::size_t i = 1; i < (rows + 1); i++) 47 | { 48 | double y = (rows - i) * spacing; 49 | for (std::size_t j = 0; j < cols; j++) 50 | { 51 | double x = j * spacing; 52 | Eigen::Vector3d point(x, y, 0.0); 53 | points.push_back(point); 54 | } 55 | } 56 | 57 | assert(points.size() == (rows * cols)); 58 | 59 | return points; 60 | } 61 | 62 | } // namespace rct_image_tools 63 | -------------------------------------------------------------------------------- /rct_image_tools/src/target_finder_tests.cpp: -------------------------------------------------------------------------------- 1 | #include "rct_image_tools/modified_circle_grid_finder.h" 2 | #include 3 | #include 4 | 5 | int main(int argc, char** argv) 6 | { 7 | if (argc != 4) 8 | { 9 | std::cout << "Usage: rosrun rct_image_tools rct_image_tools_test " 10 | " \n"; 11 | return 1; 12 | } 13 | 14 | // Parse parameters 15 | std::string path(argv[1]); 16 | int rows = std::stoi(argv[2]); 17 | int cols = std::stoi(argv[3]); 18 | 19 | // Load Image 20 | cv::Mat m = cv::imread(path); 21 | 22 | // Define target - I hard-code the spacing parameter because it doesn't matter for this example 23 | rct_image_tools::ModifiedCircleGridTarget target(rows, cols, 0.01); 24 | 25 | // Create a finder that works with this target 26 | rct_image_tools::ModifiedCircleGridTargetFinder target_finder(target); 27 | 28 | // Attempt to find the grid: The optional will be set if it succeeded. 29 | try 30 | { 31 | rct_image_tools::TargetFeatures obs = target_finder.findTargetFeatures(m); 32 | std::cout << "Grid observed: Found " << obs.size() << " features!\n"; 33 | 34 | // For debug purposes, let's draw the dots 35 | cv::Mat dots = target_finder.drawTargetFeatures(m, obs); 36 | // And display the resulting image 37 | cv::imshow("out", dots); 38 | cv::moveWindow("out", 400, 400); 39 | cv::waitKey(); 40 | } 41 | catch (const std::exception& ex) 42 | { 43 | std::cout << ex.what() << std::endl; 44 | } 45 | 46 | return 0; 47 | } 48 | -------------------------------------------------------------------------------- /rct_image_tools/test/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | find_package(GTest REQUIRED) 2 | 3 | add_executable(${PROJECT_NAME}_target_finder_test target_finder_utest.cpp) 4 | target_link_libraries(${PROJECT_NAME}_target_finder_test PRIVATE ${PROJECT_NAME} GTest::GTest GTest::Main) 5 | add_gtest_discover_tests(${PROJECT_NAME}_target_finder_test) 6 | add_dependencies(${PROJECT_NAME}_target_finder_test ${PROJECT_NAME}) 7 | add_dependencies(run_tests ${PROJECT_NAME}_target_finder_test) 8 | target_compile_definitions(${PROJECT_NAME}_target_finder_test 9 | PUBLIC TEST_SUPPORT_DIR="${CMAKE_CURRENT_SOURCE_DIR}/images/") 10 | 11 | # Install the test executables so they can be run independently later if needed 12 | install( 13 | TARGETS ${PROJECT_NAME}_target_finder_test 14 | RUNTIME DESTINATION bin/tests 15 | LIBRARY DESTINATION lib/tests 16 | ARCHIVE DESTINATION lib/tests) 17 | -------------------------------------------------------------------------------- /rct_image_tools/test/images/aruco.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Jmeyer1292/robot_cal_tools/2089a06d11b13323cf6c358a89524231801e5b06/rct_image_tools/test/images/aruco.jpg -------------------------------------------------------------------------------- /rct_image_tools/test/images/charuco_obscured.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Jmeyer1292/robot_cal_tools/2089a06d11b13323cf6c358a89524231801e5b06/rct_image_tools/test/images/charuco_obscured.jpg -------------------------------------------------------------------------------- /rct_image_tools/test/images/charuco_one_corner.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Jmeyer1292/robot_cal_tools/2089a06d11b13323cf6c358a89524231801e5b06/rct_image_tools/test/images/charuco_one_corner.jpg -------------------------------------------------------------------------------- /rct_image_tools/test/images/charuco_unobscured.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Jmeyer1292/robot_cal_tools/2089a06d11b13323cf6c358a89524231801e5b06/rct_image_tools/test/images/charuco_unobscured.jpg -------------------------------------------------------------------------------- /rct_image_tools/test/images/modified_circle_grid_10x10_0.025.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Jmeyer1292/robot_cal_tools/2089a06d11b13323cf6c358a89524231801e5b06/rct_image_tools/test/images/modified_circle_grid_10x10_0.025.png -------------------------------------------------------------------------------- /rct_optimizations/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.16.0) 2 | 3 | # Extract package name and version from package.xml 4 | find_package(ros_industrial_cmake_boilerplate REQUIRED) 5 | extract_package_metadata(pkg) 6 | project(${pkg_extracted_name} VERSION ${pkg_extracted_version} LANGUAGES CXX) 7 | 8 | find_package(rct_common REQUIRED) 9 | find_package(Boost REQUIRED) 10 | find_package(Ceres REQUIRED) 11 | find_package(yaml-cpp REQUIRED) 12 | 13 | add_library( 14 | ${PROJECT_NAME} SHARED 15 | # Utilities 16 | src/${PROJECT_NAME}/eigen_conversions.cpp 17 | src/${PROJECT_NAME}/covariance_analysis.cpp 18 | # Optimizations (Simple) 19 | src/${PROJECT_NAME}/circle_fit.cpp 20 | # Optimizations (multiple cameras) 21 | src/${PROJECT_NAME}/extrinsic_multi_static_camera.cpp 22 | src/${PROJECT_NAME}/extrinsic_multi_static_camera_only.cpp 23 | src/${PROJECT_NAME}/extrinsic_multi_static_camera_wrist_only.cpp 24 | # Optimizations (extrinsic hand-eye, 2D and 3D cameras) 25 | src/${PROJECT_NAME}/extrinsic_hand_eye.cpp 26 | # Optimizations (Experimental) - Intrinsic 27 | src/${PROJECT_NAME}/camera_intrinsic.cpp 28 | src/${PROJECT_NAME}/pnp.cpp 29 | src/${PROJECT_NAME}/multi_camera_pnp.cpp 30 | # DH Chain 31 | src/${PROJECT_NAME}/dh_chain.cpp 32 | # Validation Tools 33 | src/${PROJECT_NAME}/validation/camera_intrinsic_calibration_validation.cpp 34 | # Noise Qualification 35 | src/${PROJECT_NAME}/validation/noise_qualification.cpp 36 | # Target Homography 37 | src/${PROJECT_NAME}/validation/homography_validation.cpp 38 | # Projection 39 | src/${PROJECT_NAME}/validation/projection.cpp 40 | # DH Chain Kinematic Calibration 41 | src/${PROJECT_NAME}/dh_chain.cpp 42 | src/${PROJECT_NAME}/dh_chain_kinematic_calibration.cpp) 43 | target_compile_features(${PROJECT_NAME} PUBLIC cxx_std_11) 44 | target_compile_options(${PROJECT_NAME} PRIVATE -Wall -Wextra) 45 | target_include_directories(${PROJECT_NAME} PUBLIC "$" 46 | "$") 47 | target_include_directories(${PROJECT_NAME} SYSTEM PUBLIC ${CERES_INCLUDE_DIRS}) 48 | target_link_libraries( 49 | ${PROJECT_NAME} 50 | PUBLIC ${CERES_LIBRARIES} 51 | rct::rct_common 52 | Boost::boost 53 | yaml-cpp) 54 | 55 | if(RCT_BUILD_TESTS) 56 | enable_testing() 57 | add_run_tests_target(ENABLE ${RCT_ENABLE_RUN_TESTS}) 58 | add_subdirectory(test) 59 | endif() 60 | 61 | configure_package( 62 | NAMESPACE rct 63 | TARGETS ${PROJECT_NAME} 64 | DEPENDENCIES 65 | Boost 66 | rct_common 67 | yaml-cpp 68 | Ceres) 69 | 70 | install(DIRECTORY include/${PROJECT_NAME} DESTINATION include) 71 | -------------------------------------------------------------------------------- /rct_optimizations/include/rct_optimizations/ceres_math_utilities.h: -------------------------------------------------------------------------------- 1 | #ifndef RCT_CERES_MATH_UTILITIES_H 2 | #define RCT_CERES_MATH_UTILITIES_H 3 | 4 | #include 5 | #include 6 | 7 | namespace rct_optimizations 8 | { 9 | template 10 | inline void transformPoint(const T angle_axis[3], const T tx[3], const T point[3], T t_point[3]) 11 | { 12 | ceres::AngleAxisRotatePoint(angle_axis, point, t_point); 13 | 14 | t_point[0] = t_point[0] + tx[0]; 15 | t_point[1] = t_point[1] + tx[1]; 16 | t_point[2] = t_point[2] + tx[2]; 17 | } 18 | 19 | template 20 | inline void poseTransformPoint(const Pose6d& pose, const T point[3], T t_point[3]) 21 | { 22 | T angle_axis[3]; 23 | 24 | angle_axis[0] = T(pose.rx()); 25 | angle_axis[1] = T(pose.ry()); 26 | angle_axis[2] = T(pose.rz()); 27 | 28 | T translation[3]; 29 | translation[0] = T(pose.x()); 30 | translation[1] = T(pose.y()); 31 | translation[2] = T(pose.z()); 32 | 33 | transformPoint(angle_axis, translation, point, t_point); 34 | } 35 | 36 | template 37 | inline void projectPoint(const CameraIntrinsics& intr, const T point[3], T xy_image[2]) 38 | { 39 | T xp1 = point[0]; 40 | T yp1 = point[1]; 41 | T zp1 = point[2]; 42 | 43 | // Scale into the image plane by distance away from camera 44 | T xp, yp; 45 | 46 | if (zp1 == T(0)) // Avoid divide by zero 47 | { 48 | xp = xp1; 49 | yp = yp1; 50 | } 51 | else 52 | { 53 | xp = xp1 / zp1; 54 | yp = yp1 / zp1; 55 | } 56 | 57 | // Perform projection using focal length and camera optical center into image 58 | // plane 59 | xy_image[0] = intr.fx() * xp + intr.cx(); 60 | xy_image[1] = intr.fy() * yp + intr.cy(); 61 | } 62 | 63 | template 64 | inline Eigen::Matrix projectPoint(const rct_optimizations::CameraIntrinsics& intr, 65 | const Eigen::Matrix& point) 66 | { 67 | // Scale the input point by its distance from the camera (i.e. z-coordinate) 68 | Eigen::Matrix scaled_point(point); 69 | scaled_point(2) = T(1.0); 70 | 71 | // Avoid divide by zero 72 | if (std::abs(point.z() - T(0.0) > T(1.e-10))) 73 | { 74 | scaled_point.x() = point.x() / point.z(); 75 | scaled_point.y() = point.y() / point.z(); 76 | } 77 | 78 | /* Create the camera parameters matrix 79 | * | fx 0 cx | * | x/z | = | u | 80 | * | 0 fy cy | * | y/z | = | v | 81 | * | 0 0 1 | * | 1 | = | 1 | 82 | */ 83 | Eigen::Matrix3d camera_matrix; 84 | camera_matrix << intr.fx(), 0.0, intr.cx(), 0.0, intr.fy(), intr.cy(), 0.0, 0.0, 1.0; 85 | 86 | // Perform projection using focal length and camera optical center into image plane 87 | Eigen::Matrix image_pt = camera_matrix.cast() * scaled_point; 88 | 89 | // Return only the top two elements of the vector 90 | return image_pt.template head<2>(); 91 | } 92 | 93 | } // namespace rct_optimizations 94 | 95 | #endif // RCT_CERES_MATH_UTILITIES_H 96 | -------------------------------------------------------------------------------- /rct_optimizations/include/rct_optimizations/circle_fit.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | 8 | namespace rct_optimizations 9 | { 10 | /** 11 | * @brief Cost function for the error between a point and a circle. 12 | */ 13 | class CircleDistCost 14 | { 15 | public: 16 | CircleDistCost(const double& x, const double& y) : x_(x), y_(y) {} 17 | 18 | /** 19 | * @brief One formulation of a residual function for error relative to the center of a circle. 20 | * Presented to the solver as three size-1 double parameters. 21 | * @param sample A double pointer which contains {x, y, r_sqrt}. 22 | */ 23 | template 24 | bool operator()(const T* const sample, T* residual) const 25 | { 26 | T r = sample[2] * sample[2]; 27 | 28 | T x_pos = x_ - sample[0]; 29 | T y_pos = y_ - sample[1]; 30 | 31 | residual[0] = r * r - x_pos * x_pos - y_pos * y_pos; 32 | return true; 33 | } 34 | 35 | /** 36 | * @brief An alternative formulation of a residual function for error relative to the center of a circle. 37 | * Here, the inputs (x, y, r_sqrt) are provided as separate parameters. This is mathematically identical to the 38 | * previous version, but the number and size of the parameters are presented differently to the solver (one size-3 39 | * double parameter). 40 | * @param x_sample The x-coordinate of the center of the circle. 41 | * @param y_sample The y-coordinate of the center of the circle. 42 | * @param r_sqrt_sample The square root of the radius of the circle. 43 | */ 44 | template 45 | bool operator()(const T* const x_sample, const T* const y_sample, const T* const r_sqrt_sample, T* residual) const 46 | { 47 | T r = r_sqrt_sample[0] * r_sqrt_sample[0]; // Using sqrt(radius) prevents the radius from ending up negative and 48 | // improves numerical stability. 49 | 50 | T x_pos = x_ - x_sample[0]; 51 | T y_pos = y_ - y_sample[0]; 52 | 53 | residual[0] = r * r - x_pos * x_pos - y_pos * y_pos; 54 | return true; 55 | } 56 | 57 | private: 58 | double x_, y_; 59 | }; 60 | 61 | /** 62 | * @brief Problem setup info for the circle fit optimization. 63 | */ 64 | struct CircleFitProblem 65 | { 66 | /** 67 | * @brief Observations around the edge of the circle. 2D points in the form (x, y). 68 | */ 69 | std::vector observations; 70 | 71 | /** 72 | * @brief Estimate for the initial x-coordinate of the center of the circle. 73 | */ 74 | double x_center_initial; 75 | 76 | /** 77 | * @brief Estimate for the initial y-coordinate of the center of the circle. 78 | */ 79 | double y_center_initial; 80 | 81 | /** 82 | * @brief Estimate for the initial radius of the circle. 83 | */ 84 | double radius_initial; 85 | 86 | const std::vector labels = { "x", "y", "r" }; 87 | }; 88 | 89 | /** 90 | * @brief Results for the circle fit optimization 91 | */ 92 | struct CircleFitResult 93 | { 94 | /** 95 | * @brief True if converged, False if any other result. 96 | */ 97 | bool converged; 98 | 99 | /** 100 | * @brief Initial average error per observation. 101 | */ 102 | double initial_cost_per_obs; 103 | 104 | /** 105 | * @brief Final average error per observation. 106 | */ 107 | double final_cost_per_obs; 108 | 109 | /** 110 | * @brief The covariance matrix for the problem. A square 3x3 matrix giving covariance between each pair of elements. 111 | * Order of elements is [x, y, radius]. 112 | */ 113 | CovarianceResult covariance; 114 | 115 | /** 116 | * @brief Calculated circle center x-coord. 117 | */ 118 | double x_center; 119 | 120 | /** 121 | * @brief Calculated circle center y-coord. 122 | */ 123 | double y_center; 124 | 125 | /** 126 | * @brief Calculated circle radius. 127 | */ 128 | double radius; 129 | }; 130 | 131 | /** 132 | * @brief Function that solves the circle fit problem. 133 | * @param Input observations and guesses. 134 | * @return Output results. 135 | */ 136 | CircleFitResult optimize(const CircleFitProblem& params); 137 | 138 | } // namespace rct_optimizations 139 | -------------------------------------------------------------------------------- /rct_optimizations/include/rct_optimizations/covariance_types.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | 12 | namespace rct_optimizations 13 | { 14 | /** @brief A double value identified by one or two name strings. */ 15 | struct NamedParam 16 | { 17 | /** @brief Pair of names identifying this parameter. For types with just one name (e.g. standard deviation), only 18 | * names.first is used. */ 19 | std::pair names; 20 | /** @brief Value of this parameter. */ 21 | std::double_t value; 22 | 23 | /** 24 | * @brief Format the NamedParam as a string. 25 | * @return 26 | */ 27 | std::string toString() const 28 | { 29 | std::stringstream ss; 30 | ss << names.first.c_str() << " " << names.second.c_str() << " " << value; 31 | return ss.str(); 32 | } 33 | }; 34 | 35 | /** 36 | * @brief Covariance results for optimization parameters. 37 | * Contains standard deviations, covariances, and correlation coefficients, as well as the original covariance and 38 | * correlation matrices. 39 | */ 40 | struct CovarianceResult 41 | { 42 | /** @brief standard deviations */ 43 | std::vector standard_deviations; 44 | /** @brief correlation_coeffs */ 45 | std::vector correlation_coeffs; 46 | /** @brief covariances */ 47 | std::vector covariances; 48 | /** @brief Covariance matrix output from Ceres */ 49 | Eigen::MatrixXd covariance_matrix; 50 | /** @brief Correlation matrix */ 51 | Eigen::MatrixXd correlation_matrix; 52 | 53 | /** 54 | * @brief Returns named correlation coefficients that exceed @ref threshold. 55 | * @param Magnitude of a correlation coefficient that will result in it being returned. 56 | * @return Vector of NamedParams for correlation coefficients above @ref threshold. 57 | */ 58 | std::vector getCorrelationCoeffOutsideThreshold(const std::double_t& threshold) const 59 | { 60 | std::vector out; 61 | for (auto corr : correlation_coeffs) 62 | { 63 | if (std::abs(corr.value) > threshold) 64 | out.push_back(corr); 65 | } 66 | std::sort(out.begin(), out.end(), [](NamedParam a, NamedParam b) { return std::abs(a.value) > std::abs(b.value); }); 67 | return out; 68 | } 69 | 70 | /** 71 | * @brief Format NamedParam contents as a string. 72 | * @return 73 | */ 74 | std::string toString() const 75 | { 76 | std::string out; 77 | out.append("Std. Devs.\n"); 78 | for (auto std_dev : standard_deviations) 79 | { 80 | out.append(std_dev.toString() + "\n"); 81 | } 82 | 83 | out.append("\nCovariance\n"); 84 | for (auto cov : covariances) 85 | { 86 | out.append(cov.toString() + "\n"); 87 | } 88 | 89 | out.append("\nCorrelation Coeffs.\n"); 90 | for (auto corr : correlation_coeffs) 91 | { 92 | out.append(corr.toString() + "\n"); 93 | } 94 | return out; 95 | } 96 | 97 | /** 98 | * @brief Compose a string with a list of NamedParams for correlation coefficients above @ref threshold. 99 | * @param threshold 100 | * @return 101 | */ 102 | std::string printCorrelationCoeffAboveThreshold(const std::double_t& threshold) const 103 | { 104 | auto above_thresh = getCorrelationCoeffOutsideThreshold(threshold); 105 | 106 | if (above_thresh.size() == 0) 107 | { 108 | return std::string("No correlation coefficients with magnitude > " + std::to_string(threshold) + "\n"); 109 | } 110 | 111 | std::string out("\nCorrelation Coeff. > " + std::to_string(threshold) + ":\n"); 112 | for (auto corr : above_thresh) 113 | { 114 | out.append(corr.toString() + "\n"); 115 | } 116 | return out; 117 | } 118 | }; 119 | } // namespace rct_optimizations 120 | -------------------------------------------------------------------------------- /rct_optimizations/include/rct_optimizations/eigen_conversions.h: -------------------------------------------------------------------------------- 1 | #ifndef RCT_EIGEN_CONVERSIONS_H 2 | #define RCT_EIGEN_CONVERSIONS_H 3 | 4 | #include "rct_optimizations/types.h" 5 | #include 6 | 7 | namespace rct_optimizations 8 | { 9 | Pose6d poseEigenToCal(const Eigen::Isometry3d& pose); 10 | 11 | Eigen::Isometry3d poseCalToEigen(const Pose6d& pose); 12 | 13 | } // namespace rct_optimizations 14 | 15 | #endif // RCT_EIGEN_CONVERSIONS_H 16 | -------------------------------------------------------------------------------- /rct_optimizations/include/rct_optimizations/experimental/camera_intrinsic.h: -------------------------------------------------------------------------------- 1 | #ifndef RCT_CAMERA_INTRINSIC_H 2 | #define RCT_CAMERA_INTRINSIC_H 3 | 4 | #include 5 | #include "rct_optimizations/types.h" 6 | #include "boost/optional.hpp" 7 | 8 | namespace rct_optimizations 9 | { 10 | struct IntrinsicEstimationProblem 11 | { 12 | std::vector image_observations; 13 | CameraIntrinsics intrinsics_guess; 14 | bool use_extrinsic_guesses; 15 | std::vector extrinsic_guesses; 16 | 17 | std::string label_extr = "pose"; 18 | const std::array labels_intrinsic_params = { 19 | { "fx", "fy", "cx", "cy", "k1", "k2", "p1", "p2", "k3" } 20 | }; 21 | const std::array labels_isometry3d = { { "x", "y", "z", "rx", "ry", "rz" } }; 22 | }; 23 | 24 | struct IntrinsicEstimationResult 25 | { 26 | bool converged; 27 | double initial_cost_per_obs; 28 | double final_cost_per_obs; 29 | 30 | CameraIntrinsics intrinsics; 31 | std::array distortions; 32 | 33 | std::vector target_transforms; 34 | 35 | CovarianceResult covariance; 36 | }; 37 | 38 | IntrinsicEstimationResult optimize(const IntrinsicEstimationProblem& params); 39 | 40 | } // namespace rct_optimizations 41 | 42 | #endif // RCT_CAMERA_INTRINSIC_H 43 | -------------------------------------------------------------------------------- /rct_optimizations/include/rct_optimizations/experimental/multi_camera_pnp.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Computes the pose of a "target" seen in one or more intrinsically and extrinsically calibrated 3 | * cameras. The pose of these cameras must be specified in a common "base" frame, and the guess 4 | * at the target must be specified in this same base frame. The answer will also be in this common 5 | * base frame. 6 | * 7 | * Note that this function is logically equivalent to the solver available in pnp.h assuming that 8 | * you specified ONE camera with an identity transform. 9 | * 10 | * author: Levi Armstrong 11 | */ 12 | 13 | #ifndef RCT_MULTI_CAMERA_PNP_H 14 | #define RCT_MULTI_CAMERA_PNP_H 15 | 16 | #include "rct_optimizations/types.h" 17 | 18 | namespace rct_optimizations 19 | { 20 | struct MultiCameraPnPProblem 21 | { 22 | /** @brief The basic camera intrinsic propeties: fx, fy, cx, cy used to reproject points; 23 | one for each camera. Should have same length as @e base_to_camera. */ 24 | std::vector intr; 25 | 26 | /** @brief The "base frame" to "camera frame" transform; one for each camera. Should have same 27 | length as @e intr. */ 28 | std::vector base_to_camera; 29 | 30 | /** @brief A sequence of observation sets corresponding to the image locations. 31 | * Each observation set consists of a set of correspodences: a 3D position (e.g. a dot) in "target 32 | * frame" to the image location it was detected at (2D). The outer-most vector is for each camera, 33 | * the inner vector is the images valid for that camera. */ 34 | std::vector image_observations; 35 | 36 | /** @brief Your best guess for transforms, "base to target", for a given observation set taken.*/ 37 | Eigen::Isometry3d base_to_target_guess; 38 | }; 39 | 40 | struct MultiCameraPnPResult 41 | { 42 | /** 43 | * @brief Whether the underlying solver converged. If this is false, your calibration did not go well. 44 | * If this is true, your calibration MAY have gone well. 45 | */ 46 | bool converged; 47 | 48 | /** 49 | * @brief The initial reprojection error (in pixels) per residual based on your input guesses. 50 | */ 51 | double initial_cost_per_obs; 52 | 53 | /** 54 | * @brief The final reprojection error (in pixels) per residual after optimization. Note that each circle 55 | * has two residuals: a U and V error in the image. So a value of 1.2 means that each circle was described 56 | * to within 1.2 pixels in X and 1.2 pixels in Y. 57 | * 58 | * A low value here is encouraging if you had a diversity of images. If you took few images, you can get 59 | * a low score without getting a calibration that describes your workcell. 60 | */ 61 | double final_cost_per_obs; 62 | 63 | /** @brief The final location of the target. */ 64 | Eigen::Isometry3d base_to_target; 65 | }; 66 | 67 | MultiCameraPnPResult optimize(const MultiCameraPnPProblem& params); 68 | 69 | } // namespace rct_optimizations 70 | 71 | #endif // RCT_MULTI_STATIC_CAMERA_PNP_H 72 | -------------------------------------------------------------------------------- /rct_optimizations/include/rct_optimizations/extrinsic_hand_eye.h: -------------------------------------------------------------------------------- 1 | /* 2 | * This file defines a solver for calibrating the EXTRINSIC parameters of a 3D 3 | * camera attached to the wrist of a moving robot. It works by imaging a static target 4 | * from many robot wrist positions. 5 | * 6 | * The only difference between this the and 2D, pinhole variety is that the correspondences 7 | * in the observation set are 3D to 3D instead of 2D to 3D. This is meant for 3D sensors where 8 | * you don't know (or don't want to know) the intrinsics of the sensor or they aren't well 9 | * described by the pinhole model. 10 | * 11 | * For example, this calibration has been used to detect 3D features in a "3D image" produced 12 | * by the IFM O3D3xx cameras. Sometimes you may want to use this for Openni cameras where 13 | * because of terrible drivers your calibration does not affect the depth data. 14 | * 15 | * See extrinsic_camera_on_wrist.h for a description of the other parameters. 16 | */ 17 | #pragma once 18 | 19 | #include 20 | #include 21 | #include 22 | #include 23 | 24 | namespace rct_optimizations 25 | { 26 | struct ExtrinsicHandEyeProblem2D3D 27 | { 28 | typename Observation2D3D::Set observations; 29 | CameraIntrinsics intr; 30 | Eigen::Isometry3d target_mount_to_target_guess; 31 | Eigen::Isometry3d camera_mount_to_camera_guess; 32 | 33 | std::array labels_isometry3d = { { "x", "y", "z", "rx", "ry", "rz" } }; 34 | std::string label_target_mount_to_target = "target_mount_to_target"; 35 | std::string label_camera_mount_to_camera = "camera_mount_to_camera"; 36 | 37 | inline bool operator==(const ExtrinsicHandEyeProblem2D3D& rhs) const 38 | { 39 | return observations == rhs.observations && intr == rhs.intr && 40 | target_mount_to_target_guess.isApprox(rhs.target_mount_to_target_guess) && 41 | camera_mount_to_camera_guess.isApprox(rhs.camera_mount_to_camera_guess); 42 | } 43 | }; 44 | 45 | struct ExtrinsicHandEyeProblem3D3D 46 | { 47 | typename Observation3D3D::Set observations; 48 | Eigen::Isometry3d target_mount_to_target_guess; 49 | Eigen::Isometry3d camera_mount_to_camera_guess; 50 | 51 | std::array labels_isometry3d = { { "x", "y", "z", "rx", "ry", "rz" } }; 52 | std::string label_target_mount_to_target = "target_mount_to_target"; 53 | std::string label_camera_mount_to_camera = "camera_mount_to_camera"; 54 | 55 | inline bool operator==(const ExtrinsicHandEyeProblem3D3D& rhs) const 56 | { 57 | return observations == rhs.observations && 58 | target_mount_to_target_guess.isApprox(rhs.target_mount_to_target_guess) && 59 | camera_mount_to_camera_guess.isApprox(rhs.camera_mount_to_camera_guess); 60 | } 61 | }; 62 | 63 | struct ExtrinsicHandEyeResult 64 | { 65 | bool converged; 66 | double initial_cost_per_obs; 67 | double final_cost_per_obs; 68 | 69 | Eigen::Isometry3d target_mount_to_target; 70 | Eigen::Isometry3d camera_mount_to_camera; 71 | 72 | CovarianceResult covariance; 73 | }; 74 | 75 | ExtrinsicHandEyeResult optimize(const ExtrinsicHandEyeProblem2D3D& params); 76 | ExtrinsicHandEyeResult optimize(const ExtrinsicHandEyeProblem3D3D& params); 77 | 78 | } // namespace rct_optimizations 79 | -------------------------------------------------------------------------------- /rct_optimizations/include/rct_optimizations/extrinsic_multi_static_camera.h: -------------------------------------------------------------------------------- 1 | /* 2 | * This file mirrors the interface for ExtrinsicStaticCameraMovingTarget found in 3 | * rct_optimizations/extrinsic_static_camera.h but extends it to work for simultaneously 4 | * localizing multiple cameras. 5 | * 6 | * Thus you now pass: 7 | * 1. A vector of intrinsics, one for each camera 8 | * 2. A vector of vector of wrist poses, one for each camera 9 | * 2. A vector of vector of image observations, one for each camera 10 | * 3. A vector of base to camera guesses, one for each camera 11 | * 12 | * author: Levi Armstrong 13 | */ 14 | 15 | #ifndef RCT_EXTRINSIC_MULTI_STATIC_CAMERA_H 16 | #define RCT_EXTRINSIC_MULTI_STATIC_CAMERA_H 17 | 18 | #include 19 | #include 20 | #include 21 | #include 22 | 23 | namespace rct_optimizations 24 | { 25 | struct ExtrinsicMultiStaticCameraMovingTargetProblem 26 | { 27 | /** @brief The basic camera intrinsic propeties: fx, fy, cx, cy used to reproject points; 28 | one for each camera */ 29 | std::vector intr; 30 | 31 | /** @brief The transforms, "base to wrist", at which each observation set was taken. The outer 32 | * vector is for each camera, the inner vector is the poses valid for that camera. This inner 33 | * vector should match the inner vector of @e image_observations in size. 34 | */ 35 | std::vector> wrist_poses; 36 | 37 | /** @brief A sequence of observation sets corresponding to the image locations in @e wrist_poses. 38 | * Each observation set consists of a set of correspodences: a 3D position (e.g. a dot) in "target 39 | * frame" to the image location it was detected at (2D). The outer-most vector is for each camera, 40 | * the inner vector is the images valid for that camera. 41 | */ 42 | std::vector> image_observations; 43 | 44 | /** @brief Your best guess at the "wrist frame" to "target frame" transform */ 45 | Eigen::Isometry3d wrist_to_target_guess; 46 | 47 | /** @brief Your best guess at the "base frame" to "camera frame" transform; one for each camera */ 48 | std::vector base_to_camera_guess; 49 | 50 | const std::array labels_isometry3d = { { "x", "y", "z", "rx", "ry", "rz" } }; 51 | 52 | std::string label_wrist_to_target = "wrist_to_target"; 53 | 54 | std::string label_base_to_camera = "base_to_camera"; 55 | 56 | std::vector labels_image_observations; 57 | }; 58 | 59 | struct ExtrinsicMultiStaticCameraMovingTargetResult 60 | { 61 | /** 62 | * @brief Whether the underlying solver converged. If this is false, your calibration did not go well. 63 | * If this is true, your calibration MAY have gone well. 64 | */ 65 | bool converged; 66 | 67 | /** 68 | * @brief The initial reprojection error (in pixels) per residual based on your input guesses. 69 | */ 70 | double initial_cost_per_obs; 71 | 72 | /** 73 | * @brief The final reprojection error (in pixels) per residual after optimization. Note that each circle 74 | * has two residuals: a U and V error in the image. So a value of 1.2 means that each circle was described 75 | * to within 1.2 pixels in X and 1.2 pixels in Y. 76 | * 77 | * A low value here is encouraging if you had a diversity of images. If you took few images, you can get 78 | * a low score without getting a calibration that describes your workcell. 79 | */ 80 | double final_cost_per_obs; 81 | 82 | /** 83 | * @brief The final calibrated result of "wrist frame" to "target frame". 84 | */ 85 | Eigen::Isometry3d wrist_to_target; 86 | 87 | /** 88 | * @brief The final calibrated result of "base frame" to "camera optical frame". 89 | */ 90 | std::vector base_to_camera; 91 | 92 | CovarianceResult covariance; 93 | }; 94 | 95 | ExtrinsicMultiStaticCameraMovingTargetResult optimize(const ExtrinsicMultiStaticCameraMovingTargetProblem& params); 96 | 97 | } // namespace rct_optimizations 98 | 99 | #endif // RCT_EXTRINSIC_MULTI_STATIC_CAMERA_H 100 | -------------------------------------------------------------------------------- /rct_optimizations/include/rct_optimizations/extrinsic_multi_static_camera_only.h: -------------------------------------------------------------------------------- 1 | /* 2 | * This file mirrors the interface for ExtrinsicStaticCameraMovingTarget found in 3 | * rct_optimizations/extrinsic_static_camera.h but extends it to work for simultaneously 4 | * localizing multiple cameras. 5 | * 6 | * Thus you now pass: 7 | * 1. A vector of intrinsics, one for each camera 8 | * 2. A vector of vector of wrist poses, one for each camera 9 | * 2. A vector of vector of image observations, one for each camera 10 | * 3. A vector of base to camera guesses, one for each camera 11 | * 12 | * author: Levi Armstrong 13 | */ 14 | 15 | #ifndef RCT_EXTRINSIC_MULTI_STATIC_CAMERA_ONLY_H 16 | #define RCT_EXTRINSIC_MULTI_STATIC_CAMERA_ONLY_H 17 | 18 | #include 19 | #include 20 | #include 21 | #include 22 | 23 | namespace rct_optimizations 24 | { 25 | struct ExtrinsicMultiStaticCameraOnlyProblem 26 | { 27 | /** @brief This is usefull in camera to camera calibration like stereo calibration and 28 | * the transform between cameras is needed 29 | */ 30 | bool fix_first_camera; 31 | 32 | /** @brief The basic camera intrinsic propeties: fx, fy, cx, cy used to reproject points; 33 | one for each camera */ 34 | std::vector intr; 35 | 36 | /** @brief The transforms, "base to target", at which each observation set was taken. 37 | * The vector is the poses valid for each camera. This vector should match the inner 38 | * vector of @e image_observations in size. 39 | */ 40 | std::vector base_to_target_guess; 41 | 42 | /** @brief A sequence of observation sets corresponding to the image locations in @e base_to_target_guess. 43 | * Each observation set consists of a set of correspodences: a 3D position (e.g. a dot) in "target 44 | * frame" to the image location it was detected at (2D). The outer-most vector is for each camera, 45 | * the inner vector is the images valid for that camera. 46 | */ 47 | std::vector> image_observations; 48 | 49 | /** @brief Your best guess at the "base frame" to "camera frame" transform; one for each camera */ 50 | std::vector base_to_camera_guess; 51 | 52 | std::array labels_isometry3d = { { "x", "y", "z", "rx", "ry", "rz" } }; 53 | 54 | std::string label_base_to_target = "base_to_target"; 55 | 56 | std::string label_base_to_camera = "base_to_camera"; 57 | 58 | std::vector labels_image_observations; 59 | }; 60 | 61 | struct ExtrinsicMultiStaticCameraOnlyResult 62 | { 63 | /** 64 | * @brief Whether the underlying solver converged. If this is false, your calibration did not go well. 65 | * If this is true, your calibration MAY have gone well. 66 | */ 67 | bool converged; 68 | 69 | /** 70 | * @brief The initial reprojection error (in pixels) per residual based on your input guesses. 71 | */ 72 | double initial_cost_per_obs; 73 | 74 | /** 75 | * @brief The final reprojection error (in pixels) per residual after optimization. Note that each circle 76 | * has two residuals: a U and V error in the image. So a value of 1.2 means that each circle was described 77 | * to within 1.2 pixels in X and 1.2 pixels in Y. 78 | * 79 | * A low value here is encouraging if you had a diversity of images. If you took few images, you can get 80 | * a low score without getting a calibration that describes your workcell. 81 | */ 82 | double final_cost_per_obs; 83 | 84 | /** @brief The final calibrated result of "base frame" to "target frame". */ 85 | std::vector base_to_target; 86 | 87 | /** @brief The final calibrated result of "base frame" to "camera optical frame". */ 88 | std::vector base_to_camera; 89 | 90 | CovarianceResult covariance; 91 | }; 92 | 93 | ExtrinsicMultiStaticCameraOnlyResult optimize(const ExtrinsicMultiStaticCameraOnlyProblem& params); 94 | 95 | } // namespace rct_optimizations 96 | 97 | #endif // RCT_EXTRINSIC_MULTI_STATIC_CAMERA_ONLY_H 98 | -------------------------------------------------------------------------------- /rct_optimizations/include/rct_optimizations/extrinsic_multi_static_camera_wrist_only.h: -------------------------------------------------------------------------------- 1 | /* 2 | * This file mirrors the interface for ExtrinsicStaticCameraMovingTarget found in 3 | * rct_optimizations/extrinsic_static_camera.h but extends it to work for simultaneously 4 | * localizing multiple cameras. 5 | * 6 | * Thus you now pass: 7 | * 1. A vector of intrinsics, one for each camera 8 | * 2. A vector of vector of wrist poses, one for each camera 9 | * 2. A vector of vector of image observations, one for each camera 10 | * 3. A vector of base to camera guesses, one for each camera 11 | * 12 | * author: Levi Armstrong 13 | */ 14 | 15 | #ifndef RCT_EXTRINSIC_MULTI_STATIC_CAMERA_WRIST_ONLY_H 16 | #define RCT_EXTRINSIC_MULTI_STATIC_CAMERA_WRIST_ONLY_H 17 | 18 | #include 19 | #include 20 | #include 21 | #include 22 | 23 | namespace rct_optimizations 24 | { 25 | struct ExtrinsicMultiStaticCameraMovingTargetWristOnlyProblem 26 | { 27 | /** @brief The basic camera intrinsic propeties: fx, fy, cx, cy used to reproject points; 28 | one for each camera */ 29 | std::vector intr; 30 | 31 | /** @brief The transforms, "base to wrist", at which each observation set was taken. 32 | * The vector is the poses valid for each camera. This vector should match the inner 33 | * vector of @e image_observations in size. 34 | */ 35 | std::vector wrist_poses; 36 | 37 | /** @brief A sequence of observation sets corresponding to the image locations in @e wrist_poses. 38 | * Each observation set consists of a set of correspodences: a 3D position (e.g. a dot) in "target 39 | * frame" to the image location it was detected at (2D). The outer-most vector is for each camera, 40 | * the inner vector is the images valid for that camera. 41 | */ 42 | std::vector> image_observations; 43 | 44 | /** @brief Your best guess at the "wrist frame" to "target frame" transform */ 45 | Eigen::Isometry3d wrist_to_target_guess; 46 | 47 | /** @brief Your best guess at the "base frame" to "camera frame" transform; one for each camera. 48 | * Also it assumses the relationship between the cameras is correct and fixed, so it will 49 | * calibrating the set of cameras using a single transformation. 50 | */ 51 | std::vector base_to_camera_guess; 52 | 53 | const std::array labels_isometry3d = { { "x", "y", "z", "rx", "ry", "rz" } }; 54 | 55 | std::string label_wrist_to_target = "wrist_to_target"; 56 | 57 | std::string label_base_to_camera = "base_to_camera"; 58 | 59 | std::vector labels_image_observations; 60 | }; 61 | 62 | struct ExtrinsicMultiStaticCameraMovingTargetWristOnlyResult 63 | { 64 | /** 65 | * @brief Whether the underlying solver converged. If this is false, your calibration did not go well. 66 | * If this is true, your calibration MAY have gone well. 67 | */ 68 | bool converged; 69 | 70 | /** 71 | * @brief The initial reprojection error (in pixels) per residual based on your input guesses. 72 | */ 73 | double initial_cost_per_obs; 74 | 75 | /** 76 | * @brief The final reprojection error (in pixels) per residual after optimization. Note that each circle 77 | * has two residuals: a U and V error in the image. So a value of 1.2 means that each circle was described 78 | * to within 1.2 pixels in X and 1.2 pixels in Y. 79 | * 80 | * A low value here is encouraging if you had a diversity of images. If you took few images, you can get 81 | * a low score without getting a calibration that describes your workcell. 82 | */ 83 | double final_cost_per_obs; 84 | 85 | /** 86 | * @brief The final calibrated result of "wrist frame" to "target frame". 87 | */ 88 | Eigen::Isometry3d wrist_to_target; 89 | 90 | /** 91 | * @brief The final calibrated result of "base frame" to "camera optical frame". 92 | */ 93 | std::vector base_to_camera; 94 | 95 | CovarianceResult covariance; 96 | }; 97 | 98 | ExtrinsicMultiStaticCameraMovingTargetWristOnlyResult 99 | optimize(const ExtrinsicMultiStaticCameraMovingTargetWristOnlyProblem& params); 100 | 101 | } // namespace rct_optimizations 102 | 103 | #endif // RCT_EXTRINSIC_MULTI_STATIC_CAMERA_WRIST_ONLY_H 104 | -------------------------------------------------------------------------------- /rct_optimizations/include/rct_optimizations/maximum_likelihood.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | 6 | namespace rct_optimizations 7 | { 8 | /** 9 | * @brief Cost function for a parameter block based on the expected means and standard deviations for the values in the 10 | * block. The inputs are an array of mean values and an array of standard deviations corresponding to the optimization 11 | * variables in the parameter block 12 | * 13 | * Ceres provides an example of a maximum likelihood constraint here: 14 | * https://ceres-solver.googlesource.com/ceres-solver/+/master/examples/robot_pose_mle.cc#165 15 | * blob: 83c0903d11f2558a9eb48884addc44853ffb903b 16 | */ 17 | class MaximumLikelihood 18 | { 19 | public: 20 | MaximumLikelihood(const Eigen::ArrayXXd& mean_, const Eigen::ArrayXXd& stdev_) : mean(mean_), stdev(stdev_) 21 | { 22 | if (mean.size() != stdev.size()) 23 | throw OptimizationException("Mean and standard deviation matrix are not the same size"); 24 | } 25 | 26 | template 27 | bool operator()(T const* const* parameter, T* residual) const 28 | { 29 | using ArrayXX = Eigen::Array; 30 | Eigen::Map param_array(parameter[0], mean.rows(), mean.cols()); 31 | 32 | Eigen::Map residual_array(residual, mean.rows(), mean.cols()); 33 | residual_array = (param_array - mean.cast()) / stdev.cast(); 34 | 35 | return true; 36 | } 37 | 38 | Eigen::ArrayXXd mean; 39 | Eigen::ArrayXXd stdev; 40 | }; 41 | 42 | } // namespace rct_optimizations 43 | -------------------------------------------------------------------------------- /rct_optimizations/include/rct_optimizations/pnp.h: -------------------------------------------------------------------------------- 1 | #ifndef RCT_PNP_H 2 | #define RCT_PNP_H 3 | 4 | #include 5 | #include 6 | 7 | namespace rct_optimizations 8 | { 9 | struct PnPProblem 10 | { 11 | rct_optimizations::CameraIntrinsics intr; 12 | Correspondence2D3D::Set correspondences; 13 | 14 | Eigen::Isometry3d camera_to_target_guess; 15 | 16 | std::string label_camera_to_target_guess = "camera_to_target"; 17 | const std::array labels_translation = { { "x", "y", "z" } }; 18 | const std::array labels_rotation = { { "rx", "ry", "rz" } }; 19 | }; 20 | 21 | struct PnPProblem3D 22 | { 23 | Correspondence3D3D::Set correspondences; 24 | 25 | Eigen::Isometry3d camera_to_target_guess; 26 | 27 | std::string label_camera_to_target_guess = "camera_to_target"; 28 | const std::array labels_translation = { { "x", "y", "z" } }; 29 | const std::array labels_rotation = { { "rx", "ry", "rz" } }; 30 | }; 31 | 32 | struct PnPResult 33 | { 34 | bool converged; 35 | double initial_cost_per_obs; 36 | double final_cost_per_obs; 37 | 38 | Eigen::Isometry3d camera_to_target; 39 | 40 | CovarianceResult covariance; 41 | }; 42 | 43 | PnPResult optimize(const PnPProblem& params); 44 | PnPResult optimize(const PnPProblem3D& params); 45 | 46 | } // namespace rct_optimizations 47 | 48 | #endif // RCT_PNP_H 49 | -------------------------------------------------------------------------------- /rct_optimizations/include/rct_optimizations/serialization/eigen.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | 6 | namespace YAML 7 | { 8 | template 9 | struct convert> 10 | { 11 | using T = Eigen::Matrix; 12 | 13 | static Node encode(const T& val) 14 | { 15 | YAML::Node node; 16 | node["x"] = val.x(); 17 | node["y"] = val.y(); 18 | return node; 19 | } 20 | 21 | static bool decode(const YAML::Node& node, T& val) 22 | { 23 | val.x() = node["x"].as(); 24 | val.y() = node["y"].as(); 25 | 26 | return true; 27 | } 28 | }; 29 | 30 | template 31 | struct convert> 32 | { 33 | using T = Eigen::Matrix; 34 | 35 | static Node encode(const T& val) 36 | { 37 | YAML::Node node; 38 | node["x"] = val.x(); 39 | node["y"] = val.y(); 40 | node["z"] = val.z(); 41 | return node; 42 | } 43 | 44 | static bool decode(const YAML::Node& node, T& val) 45 | { 46 | val.x() = node["x"].as(); 47 | val.y() = node["y"].as(); 48 | val.z() = node["z"].as(); 49 | 50 | return true; 51 | } 52 | }; 53 | 54 | template 55 | struct convert> 56 | { 57 | using T = Eigen::Transform; 58 | 59 | static Node encode(const T& val) 60 | { 61 | YAML::Node node; 62 | node["x"] = val.translation().x(); 63 | node["y"] = val.translation().y(); 64 | node["z"] = val.translation().z(); 65 | 66 | Eigen::Quaternion quat(val.linear()); 67 | node["qw"] = quat.w(); 68 | node["qx"] = quat.x(); 69 | node["qy"] = quat.y(); 70 | node["qz"] = quat.z(); 71 | 72 | return node; 73 | } 74 | 75 | static bool decode(const YAML::Node& node, T& val) 76 | { 77 | Eigen::Matrix trans; 78 | trans.x() = node["x"].as(); 79 | trans.y() = node["y"].as(); 80 | trans.z() = node["z"].as(); 81 | 82 | Eigen::Quaternion quat; 83 | quat.w() = node["qw"].as(); 84 | quat.x() = node["qx"].as(); 85 | quat.y() = node["qy"].as(); 86 | quat.z() = node["qz"].as(); 87 | 88 | val = Eigen::Translation(trans) * quat; 89 | 90 | return true; 91 | } 92 | }; 93 | 94 | } // namespace YAML 95 | -------------------------------------------------------------------------------- /rct_optimizations/include/rct_optimizations/serialization/types.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | 7 | namespace YAML 8 | { 9 | template <> 10 | struct convert 11 | { 12 | using T = rct_optimizations::CameraIntrinsics; 13 | 14 | static Node encode(const T& rhs) 15 | { 16 | YAML::Node node; 17 | node["cx"] = rhs.cx(); 18 | node["cy"] = rhs.cy(); 19 | node["fx"] = rhs.fx(); 20 | node["fy"] = rhs.fy(); 21 | return node; 22 | } 23 | 24 | static bool decode(const YAML::Node& node, T& rhs) 25 | { 26 | if (node.size() != 4) 27 | return false; 28 | 29 | rhs.cx() = node["cx"].as(); 30 | rhs.cy() = node["cy"].as(); 31 | rhs.fx() = node["fx"].as(); 32 | rhs.fy() = node["fy"].as(); 33 | 34 | return true; 35 | } 36 | }; 37 | 38 | template 39 | struct convert> 40 | { 41 | using T = rct_optimizations::Correspondence; 42 | 43 | static Node encode(const T& corr) 44 | { 45 | YAML::Node node; 46 | node["in_image"] = corr.in_image; 47 | node["in_target"] = corr.in_target; 48 | return node; 49 | } 50 | 51 | static bool decode(const YAML::Node& node, T& rhs) 52 | { 53 | if (node.size() != 2) 54 | return false; 55 | 56 | rhs.in_image = node["in_image"].as(); 57 | rhs.in_target = node["in_target"].as(); 58 | 59 | return true; 60 | } 61 | }; 62 | 63 | template 64 | struct convert> 65 | { 66 | using T = rct_optimizations::Observation; 67 | 68 | static Node encode(const T& obs) 69 | { 70 | YAML::Node node; 71 | node["correspondences"] = obs.correspondence_set; 72 | node["to_target_mount"] = obs.to_target_mount; 73 | node["to_camera_mount"] = obs.to_camera_mount; 74 | return node; 75 | } 76 | 77 | static bool decode(const YAML::Node& node, T& obs) 78 | { 79 | if (node.size() != 3) 80 | return false; 81 | 82 | obs.correspondence_set = node["correspondences"].as(); 83 | obs.to_target_mount = node["to_target_mount"].as(); 84 | obs.to_camera_mount = node["to_camera_mount"].as(); 85 | 86 | return true; 87 | } 88 | }; 89 | 90 | template <> 91 | struct convert 92 | { 93 | using T = rct_optimizations::DHTransform; 94 | static bool decode(const Node& n, T& val) 95 | { 96 | // Joint type 97 | switch (static_cast(n["type"].as())) 98 | { 99 | case rct_optimizations::DHJointType::LINEAR: 100 | val.type = rct_optimizations::DHJointType::LINEAR; 101 | break; 102 | case rct_optimizations::DHJointType::REVOLUTE: 103 | val.type = rct_optimizations::DHJointType::REVOLUTE; 104 | break; 105 | default: 106 | throw std::runtime_error("Invalid joint type"); 107 | } 108 | 109 | // DH offsets 110 | { 111 | YAML::Node d = n["d"]; 112 | YAML::Node theta = n["theta"]; 113 | YAML::Node r = n["r"]; 114 | YAML::Node alpha = n["alpha"]; 115 | 116 | Eigen::Vector4d params; 117 | params << d.as(), theta.as(), r.as(), alpha.as(); 118 | 119 | val.params = params; 120 | } 121 | 122 | val.name = n["joint_name"].as(); 123 | 124 | return true; 125 | } 126 | }; 127 | 128 | template <> 129 | struct convert 130 | { 131 | using T = rct_optimizations::DHChain; 132 | inline static bool decode(const Node& n, T& val) 133 | { 134 | val.base_offset_ = n["base_transform"].as(); 135 | 136 | const YAML::Node& transforms = n["dh_transforms"]; 137 | val.transforms_.clear(); 138 | val.transforms_.reserve(transforms.size()); 139 | for (std::size_t i = 0; i < transforms.size(); i++) 140 | { 141 | val.transforms_.push_back(transforms[i].as()); 142 | } 143 | 144 | return true; 145 | } 146 | }; 147 | 148 | } // namespace YAML 149 | -------------------------------------------------------------------------------- /rct_optimizations/include/rct_optimizations/validation/homography_validation.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include 3 | #include 4 | 5 | namespace rct_optimizations 6 | { 7 | /** 8 | * @brief A simple structure for choosing which correspondence indices to use for generating the homography matrix 9 | */ 10 | struct CorrespondenceSampler 11 | { 12 | virtual ~CorrespondenceSampler() = default; 13 | virtual std::vector getSampleCorrespondenceIndices() const = 0; 14 | }; 15 | 16 | /** 17 | * @brief A correspondence sampler specifically for grid targets 18 | */ 19 | struct GridCorrespondenceSampler : CorrespondenceSampler 20 | { 21 | GridCorrespondenceSampler(const std::size_t rows_, const std::size_t cols_, const std::size_t stride_ = 1); 22 | 23 | virtual std::vector getSampleCorrespondenceIndices() const final override; 24 | 25 | /** @brief Number of rows in the target */ 26 | const std::size_t rows; 27 | /** @brief Number of columns in the target */ 28 | const std::size_t cols; 29 | /** 30 | * @brief Number elements associated with each grid point (i.e. depth) 31 | * ArUco grid targets, for example, have 4 observations associated with each element in the grid, so the stride would 32 | * be 4 33 | */ 34 | const std::size_t stride; 35 | }; 36 | 37 | /** 38 | * @brief A correspondence sampler that randomly chooses a specifiable number of correspondence indices with a uniform 39 | * probablility to use in generating a homography transform 40 | */ 41 | struct RandomCorrespondenceSampler : CorrespondenceSampler 42 | { 43 | RandomCorrespondenceSampler(const std::size_t n_correspondences_, 44 | const std::size_t n_samples_, 45 | const unsigned seed_ = std::random_device{}()); 46 | 47 | virtual std::vector getSampleCorrespondenceIndices() const final override; 48 | 49 | /** @brief Number of total correspondences */ 50 | const std::size_t n_correspondences; 51 | /** @brief Number of samples with which to calculate the homography transform. This number must be at least 4. 52 | * Typically a lower number of samples (i.e. 4) does not produce an accurate homography transform; one quarter to half 53 | * the total number of correspondences is a good rule of thumb */ 54 | const std::size_t n_samples; 55 | /** @brief Random seed*/ 56 | const unsigned seed; 57 | }; 58 | 59 | /** 60 | * @brief Computes the error between a set of corresponding planar data points (points on a planar target and their 61 | * corresponding locations in the image plane) This function creates a homography matrix which transforms data from one 62 | * plane onto another plane (i.e. from the target plane to the image plane). This matrix is created using only a subset 63 | * of the correspondences. The matrix is then used to estimate the location of all target points in the image plane. The 64 | * error between these estimates and the actual values is a good measurement of how accurately a set of 2D camera 65 | * measurements represent a calbration target with known geometry 66 | * 67 | * Assumptions: 68 | * - Both sets of points lie on a plane (i.e. points on a planar calibration target and points on the image plane) 69 | * 70 | * @param correspondences - A set of corresponding points 71 | * @param correspondence_sampler - a struct for choosing correspondence indices to generate the homography matrix 72 | * @return A vector of homography errors for each correspondence 73 | * @throws Exception when number of correspondences does not exceed 2x the number of samples 74 | */ 75 | Eigen::VectorXd calculateHomographyError(const Correspondence2D3D::Set& correspondences, 76 | const CorrespondenceSampler& correspondence_sampler); 77 | 78 | /** 79 | * @brief Calculates the homography error for correspondences of 3D planar points using @ref calculateHomographyError 80 | * @param correspondences - A set of corresponding points 81 | * @param correspondence_sampler - a struct for choosing correspondence indices to generate the homography matrix 82 | * @return A vector of homography errors for each correspondence 83 | */ 84 | Eigen::VectorXd calculateHomographyError(const Correspondence3D3D::Set& correspondences, 85 | const CorrespondenceSampler& correspondence_sampler); 86 | 87 | } // namespace rct_optimizations 88 | -------------------------------------------------------------------------------- /rct_optimizations/include/rct_optimizations/validation/noise_qualification.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include "rct_optimizations/types.h" 5 | #include "rct_optimizations/pnp.h" 6 | 7 | namespace rct_optimizations 8 | { 9 | /** 10 | * @brief Holds the mean and standard deviation of a position vector 11 | */ 12 | struct PositionStats 13 | { 14 | Eigen::Vector3d mean; 15 | Eigen::Vector3d stdev; 16 | }; 17 | 18 | /** 19 | * @brief Contains the mean and standard deviation of a quaternion orientation 20 | */ 21 | struct QuaternionStats 22 | { 23 | Eigen::Quaterniond mean; 24 | double stdev; 25 | }; 26 | 27 | /** 28 | * @brief Noise statistics for a position vector and quaternion orientation 29 | */ 30 | struct PnPNoiseStat 31 | { 32 | PositionStats p_stat; 33 | QuaternionStats q_stat; 34 | }; 35 | 36 | /** 37 | * @brief Computes the mean and standard deviation of a set of quaternions 38 | * @param quaternions 39 | * @return 40 | */ 41 | QuaternionStats computeQuaternionStats(const std::vector& quaternions); 42 | 43 | /** 44 | * @brief Computes the mean of a set of quaternions 45 | * @param orientations 46 | * @return 47 | */ 48 | Eigen::Quaterniond computeQuaternionMean(const std::vector& orientations); 49 | 50 | /** 51 | * @brief This function qualifies 2D sensor noise by 52 | * comparing PnP results from images taken at same pose. 53 | * Sensor noise can be understood by inspecting the returned standard 54 | * deviations. 55 | * @param Sets of PnP 2D problem parameters 56 | * @return Noise Statistics: a vector of means & std devs 57 | */ 58 | PnPNoiseStat qualifyNoise2D(const std::vector& params); 59 | 60 | /** 61 | * @brief This function qualifies 3D sensor noise by 62 | * comparing PnP results from scans taken at the same pose. 63 | * Sensor noise can be understood by inspecting the returned standard 64 | * deviations. 65 | * @param params 3D image parameters 66 | * @return Noise Statiscics: a vector of standard deviations and the mean pos 67 | */ 68 | PnPNoiseStat qualifyNoise3D(const std::vector& params); 69 | 70 | } // namespace rct_optimizations 71 | -------------------------------------------------------------------------------- /rct_optimizations/include/rct_optimizations/validation/projection.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | #include 6 | 7 | namespace rct_optimizations 8 | { 9 | /** 10 | * @brief Computes the vector from an input point on a line to the point where the line intersects with the input plane 11 | * See this link for more information 12 | * @param plane_normal Unit normal vector defining the plane 13 | * @param plane_pt Point on the plane 14 | * @param line Unit vector defining the line 15 | * @param line_pt Point on the line 16 | * @param epsilon 17 | */ 18 | Eigen::Vector3d computeLinePlaneIntersection(const Eigen::Vector3d& plane_normal, 19 | const Eigen::Vector3d& plane_pt, 20 | const Eigen::Vector3d& line, 21 | const Eigen::Vector3d& line_pt, 22 | const double epsilon = 1.0e-6); 23 | 24 | /** 25 | * @brief Projects a set of 2D source points (e.g., from an image observation) onto a 3D plane (e.g., a flat calibration 26 | * target) 27 | * @param source Set of 2D points 28 | * @param source_to_plane Matrix that transforms the source points into the frame of the plane (e.g., camera to target 29 | * transform) 30 | * @param k 3x3 camera projection matrix 31 | * @return 32 | */ 33 | Eigen::MatrixX3d project3D(const Eigen::MatrixX2d& source, 34 | const Eigen::Isometry3d& source_to_plane, 35 | const Eigen::Matrix3d& k); 36 | 37 | /** 38 | * @brief Projects the 2D target features from an observation onto the 3D plane of the calibrated target and computes 39 | * the difference between the projections and the known target features 40 | */ 41 | Eigen::ArrayXd compute3DProjectionError(const Observation2D3D& obs, 42 | const CameraIntrinsics& intr, 43 | const Eigen::Isometry3d& camera_mount_to_camera, 44 | const Eigen::Isometry3d& target_mount_to_target); 45 | 46 | /** 47 | * @brief Projects the 2D target features from a set of observations onto the 3D plane of the calibrated target and 48 | * computes the difference between the projections and the known target features 49 | */ 50 | Eigen::ArrayXd compute3DProjectionError(const Observation2D3D::Set& obs, 51 | const CameraIntrinsics& intr, 52 | const Eigen::Isometry3d& camera_mount_to_camera, 53 | const Eigen::Isometry3d& target_mount_to_target); 54 | 55 | } // namespace rct_optimizations 56 | -------------------------------------------------------------------------------- /rct_optimizations/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | rct_optimizations 4 | 0.2.1 5 | The rct_optimizations package 6 | 7 | Jonathan Meyer 8 | 9 | Jonathan Meyer 10 | 11 | 14 | Geoffrey Chiou 15 | Chris Lewis 16 | 17 | Apache 2.0 18 | 19 | ros_industrial_cmake_boilerplate 20 | boost 21 | libceres-dev 22 | rct_common 23 | yaml-cpp 24 | 25 | 26 | cmake 27 | 28 | 29 | -------------------------------------------------------------------------------- /rct_optimizations/src/rct_optimizations/circle_fit.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | 5 | rct_optimizations::CircleFitResult rct_optimizations::optimize(const rct_optimizations::CircleFitProblem& params) 6 | { 7 | double x = params.x_center_initial; 8 | double y = params.y_center_initial; 9 | double r = params.radius_initial; 10 | 11 | double r_sqrt = sqrt(r); 12 | 13 | std::vector circle_params(3, 0.0); 14 | circle_params[0] = x; 15 | circle_params[1] = y; 16 | circle_params[2] = r_sqrt; 17 | 18 | rct_optimizations::CircleFitResult result; 19 | 20 | ceres::Problem problem; 21 | 22 | ceres::LossFunction* loss_fn = nullptr; 23 | 24 | for (auto obs : params.observations) 25 | { 26 | auto* cost_fn = new CircleDistCost(obs.x(), obs.y()); 27 | 28 | auto* cost_block = new ceres::AutoDiffCostFunction(cost_fn); 29 | problem.AddResidualBlock(cost_block, loss_fn, circle_params.data()); 30 | } 31 | 32 | ceres::Solver::Options options; 33 | options.max_num_iterations = 500; 34 | options.linear_solver_type = ceres::DENSE_QR; 35 | ceres::Solver::Summary summary; 36 | ceres::Solve(options, &problem, &summary); 37 | 38 | std::cout << summary.BriefReport() << std::endl; 39 | 40 | std::map> param_block_labels; 41 | param_block_labels[circle_params.data()] = params.labels; 42 | 43 | result.converged = summary.termination_type == ceres::TerminationType::CONVERGENCE; 44 | result.x_center = circle_params[0]; 45 | result.y_center = circle_params[1]; 46 | result.radius = pow(circle_params[2], 2); 47 | result.initial_cost_per_obs = summary.initial_cost / summary.num_residuals; 48 | result.final_cost_per_obs = summary.final_cost / summary.num_residuals; 49 | result.covariance = rct_optimizations::computeCovariance(problem, param_block_labels); 50 | 51 | return result; 52 | } 53 | -------------------------------------------------------------------------------- /rct_optimizations/src/rct_optimizations/dh_chain.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | namespace rct_optimizations 5 | { 6 | // DH Transform 7 | 8 | DHTransform::DHTransform(const Eigen::Vector4d& params_, DHJointType type_) 9 | : DHTransform(params_, type_, "joint", std::random_device{}()) 10 | { 11 | } 12 | 13 | DHTransform::DHTransform(const Eigen::Vector4d& params_, DHJointType type_, const std::string& name_) 14 | : DHTransform(params_, type_, name_, std::random_device{}()) 15 | { 16 | } 17 | 18 | DHTransform::DHTransform(const Eigen::Vector4d& params_, 19 | DHJointType type_, 20 | const std::string& name_, 21 | std::size_t random_seed_) 22 | : params(params_), type(type_), name(name_), random_seed(random_seed_) 23 | { 24 | } 25 | 26 | double DHTransform::createRandomJointValue() const 27 | { 28 | // Create a static random number generator so that this object does not get continuously re-instantiated 29 | static std::mt19937 mt_rand = std::mt19937(random_seed); 30 | 31 | // Create a new uniform distribution object with the current min and max values 32 | std::uniform_real_distribution dist(min, max); 33 | 34 | return dist(mt_rand); 35 | } 36 | 37 | std::array DHTransform::getParamLabels() const 38 | { 39 | return std::array({ 40 | name + "_d", 41 | name + "_theta", 42 | name + "_r", 43 | name + "_alpha", 44 | }); 45 | } 46 | 47 | // DH Chain 48 | 49 | DHChain::DHChain(std::vector transforms, const Eigen::Isometry3d& base_offset) 50 | : transforms_(std::move(transforms)), base_offset_(base_offset) 51 | { 52 | } 53 | 54 | DHChain::DHChain(const DHChain& other, const Eigen::MatrixX4d& dh_offsets) 55 | { 56 | transforms_.reserve(other.transforms_.size()); 57 | for (std::size_t i = 0; i < other.transforms_.size(); ++i) 58 | { 59 | const DHTransform& transform = other.transforms_.at(i); 60 | transforms_.emplace_back(transform.params + dh_offsets.row(i).transpose(), transform.type); 61 | } 62 | 63 | base_offset_ = other.base_offset_; 64 | } 65 | 66 | Eigen::VectorXd DHChain::createUniformlyRandomPose() const 67 | { 68 | Eigen::VectorXd joints(transforms_.size()); 69 | for (std::size_t i = 0; i < transforms_.size(); ++i) 70 | { 71 | joints[i] = transforms_[i].createRandomJointValue(); 72 | } 73 | return joints; 74 | } 75 | 76 | std::size_t DHChain::dof() const { return transforms_.size(); } 77 | 78 | Eigen::MatrixX4d DHChain::getDHTable() const 79 | { 80 | Eigen::MatrixX4d out(dof(), 4); 81 | for (std::size_t i = 0; i < transforms_.size(); ++i) 82 | { 83 | out.row(i) = transforms_[i].params.transpose(); 84 | } 85 | return out; 86 | } 87 | 88 | std::vector DHChain::getJointTypes() const 89 | { 90 | std::vector out; 91 | out.reserve(transforms_.size()); 92 | for (const auto& t : transforms_) 93 | { 94 | out.push_back(t.type); 95 | } 96 | return out; 97 | } 98 | 99 | std::vector> DHChain::getParamLabels() const 100 | { 101 | std::vector> out; 102 | for (const auto& t : transforms_) 103 | { 104 | out.push_back(t.getParamLabels()); 105 | } 106 | return out; 107 | } 108 | 109 | Eigen::Isometry3d DHChain::getBaseOffset() const { return base_offset_; } 110 | 111 | Eigen::Isometry3d DHChain::getRelativeTransform(int joint_index, double value) const 112 | { 113 | if (joint_index < 0 || joint_index >= static_cast(transforms_.size())) 114 | throw std::runtime_error("getRelativeTransform, Invalid joint index"); 115 | 116 | return transforms_[joint_index].createRelativeTransform(value); 117 | } 118 | 119 | } // namespace rct_optimizations 120 | -------------------------------------------------------------------------------- /rct_optimizations/src/rct_optimizations/eigen_conversions.cpp: -------------------------------------------------------------------------------- 1 | #include "rct_optimizations/eigen_conversions.h" 2 | 3 | rct_optimizations::Pose6d rct_optimizations::poseEigenToCal(const Eigen::Isometry3d& pose) 4 | { 5 | Pose6d p; 6 | p.x() = pose.translation().x(); 7 | p.y() = pose.translation().y(); 8 | p.z() = pose.translation().z(); 9 | 10 | Eigen::AngleAxisd aa(pose.linear()); 11 | Eigen::Vector3d a = aa.axis() * aa.angle(); 12 | 13 | p.rx() = a.x(); 14 | p.ry() = a.y(); 15 | p.rz() = a.z(); 16 | return p; 17 | } 18 | 19 | Eigen::Isometry3d rct_optimizations::poseCalToEigen(const rct_optimizations::Pose6d& pose) 20 | { 21 | Eigen::Isometry3d p = Eigen::Isometry3d::Identity(); 22 | p.translation().x() = pose.x(); 23 | p.translation().y() = pose.y(); 24 | p.translation().z() = pose.z(); 25 | 26 | const Eigen::Vector3d rr(pose.rx(), pose.ry(), pose.rz()); 27 | const double dot_product = rr.dot(rr); 28 | 29 | // Check for 0-rotation 30 | if (dot_product > std::numeric_limits::epsilon()) 31 | { 32 | Eigen::AngleAxisd aa(rr.norm(), rr.normalized()); 33 | p.linear() = aa.toRotationMatrix(); 34 | } 35 | else 36 | { 37 | p.linear()(0, 0) = 1.0; // Logic taken from Ceres' own aaxis to rot matrix conversion 38 | p.linear()(1, 0) = rr[2]; 39 | p.linear()(2, 0) = -rr[1]; 40 | p.linear()(0, 1) = -rr[2]; 41 | p.linear()(1, 1) = 1.0; 42 | p.linear()(2, 1) = rr[0]; 43 | p.linear()(0, 2) = rr[1]; 44 | p.linear()(1, 2) = -rr[0]; 45 | p.linear()(2, 2) = 1.0; 46 | } 47 | 48 | return p; 49 | } 50 | -------------------------------------------------------------------------------- /rct_optimizations/src/rct_optimizations/multi_camera_pnp.cpp: -------------------------------------------------------------------------------- 1 | #include "rct_optimizations/experimental/multi_camera_pnp.h" 2 | #include "rct_optimizations/ceres_math_utilities.h" 3 | #include "rct_optimizations/eigen_conversions.h" 4 | #include "rct_optimizations/types.h" 5 | 6 | #include 7 | 8 | using namespace rct_optimizations; 9 | 10 | namespace 11 | { 12 | class ReprojectionCost 13 | { 14 | public: 15 | ReprojectionCost(const Eigen::Vector2d& obs, 16 | const CameraIntrinsics& intr, 17 | const Eigen::Isometry3d& camera_to_base, 18 | const Eigen::Vector3d& point_in_target) 19 | : obs_(obs), intr_(intr), camera_to_base_pose_(poseEigenToCal(camera_to_base)), target_pt_(point_in_target) 20 | { 21 | } 22 | 23 | template 24 | bool operator()(const T* const pose_base_to_target, T* residual) const 25 | { 26 | const T* target_angle_axis = pose_base_to_target + 0; 27 | const T* target_position = pose_base_to_target + 3; 28 | 29 | T world_point[3]; // Point in world coordinates (base of robot) 30 | T camera_point[3]; // Point in camera coordinates 31 | 32 | // Transform points into camera coordinates 33 | T target_pt[3]; 34 | target_pt[0] = T(target_pt_(0)); 35 | target_pt[1] = T(target_pt_(1)); 36 | target_pt[2] = T(target_pt_(2)); 37 | 38 | transformPoint(target_angle_axis, target_position, target_pt, world_point); 39 | poseTransformPoint(camera_to_base_pose_, world_point, camera_point); 40 | 41 | // Compute projected point into image plane and compute residual 42 | T xy_image[2]; 43 | projectPoint(intr_, camera_point, xy_image); 44 | 45 | residual[0] = xy_image[0] - obs_.x(); 46 | residual[1] = xy_image[1] - obs_.y(); 47 | 48 | return true; 49 | } 50 | 51 | private: 52 | Eigen::Vector2d obs_; 53 | CameraIntrinsics intr_; 54 | Pose6d camera_to_base_pose_; 55 | Eigen::Vector3d target_pt_; 56 | }; 57 | 58 | } // namespace 59 | 60 | rct_optimizations::MultiCameraPnPResult 61 | rct_optimizations::optimize(const rct_optimizations::MultiCameraPnPProblem& params) 62 | { 63 | Pose6d internal_base_to_target = poseEigenToCal(params.base_to_target_guess); 64 | 65 | ceres::Problem problem; 66 | 67 | for (std::size_t c = 0; c < params.base_to_camera.size(); ++c) // For each camera 68 | { 69 | for (std::size_t i = 0; i < params.image_observations[c].size(); ++i) // For each 3D point seen in the 2D image 70 | { 71 | // Define 72 | const auto& img_obs = params.image_observations[c][i].in_image; 73 | const auto& point_in_target = params.image_observations[c][i].in_target; 74 | const auto& base_to_camera = params.base_to_camera[c]; 75 | const auto& intr = params.intr[c]; 76 | 77 | // Allocate Ceres data structures - ownership is taken by the ceres 78 | // Problem data structure 79 | auto* cost_fn = new ReprojectionCost(img_obs, intr, base_to_camera.inverse(), point_in_target); 80 | 81 | auto* cost_block = new ceres::AutoDiffCostFunction(cost_fn); 82 | 83 | problem.AddResidualBlock(cost_block, NULL, internal_base_to_target.values.data()); 84 | } 85 | } // end for each camera 86 | 87 | ceres::Solver::Options options; 88 | ceres::Solver::Summary summary; 89 | 90 | ceres::Solve(options, &problem, &summary); 91 | 92 | MultiCameraPnPResult result; 93 | result.converged = summary.termination_type == ceres::CONVERGENCE; 94 | result.base_to_target = poseCalToEigen(internal_base_to_target); 95 | result.initial_cost_per_obs = summary.initial_cost / summary.num_residuals; 96 | result.final_cost_per_obs = summary.final_cost / summary.num_residuals; 97 | return result; 98 | } 99 | -------------------------------------------------------------------------------- /rct_optimizations/test/conversion_utest.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | // Test the conversion of a pose from Eigen to internal calibration format and back 5 | TEST(EigenConversions, there_and_back_identity) 6 | { 7 | const auto e_pose = Eigen::Isometry3d::Identity(); 8 | const auto cal_pose = rct_optimizations::poseEigenToCal(e_pose); 9 | const auto recovered_e_pose = rct_optimizations::poseCalToEigen(cal_pose); 10 | 11 | const Eigen::Matrix4d diff = e_pose.matrix() - recovered_e_pose.matrix(); 12 | EXPECT_TRUE(diff.isZero()); 13 | } 14 | 15 | // Test the conversion of a pose from Eigen to internal calibration format and back 16 | TEST(EigenConversions, there_and_back) 17 | { 18 | auto e_pose = Eigen::Isometry3d::Identity(); 19 | e_pose = e_pose * Eigen::Translation3d(0.5, 0.75, 1.0) * Eigen::AngleAxisd(M_PI_2, Eigen::Vector3d::UnitY()); 20 | const auto cal_pose = rct_optimizations::poseEigenToCal(e_pose); 21 | const auto recovered_e_pose = rct_optimizations::poseCalToEigen(cal_pose); 22 | 23 | const Eigen::Matrix4d diff = e_pose.matrix() - recovered_e_pose.matrix(); 24 | EXPECT_TRUE(diff.isZero()); 25 | } 26 | 27 | int main(int argc, char** argv) 28 | { 29 | testing::InitGoogleTest(&argc, argv); 30 | return RUN_ALL_TESTS(); 31 | } 32 | -------------------------------------------------------------------------------- /rct_optimizations/test/homography_utest.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | 7 | using namespace rct_optimizations; 8 | 9 | const unsigned TARGET_ROWS = 5; 10 | const unsigned TARGET_COLS = 7; 11 | const double SPACING = 0.025; 12 | 13 | class HomographyTest : public ::testing::Test 14 | { 15 | public: 16 | HomographyTest() 17 | : camera(test::makeKinectCamera()) 18 | , target(TARGET_ROWS, TARGET_COLS, SPACING) 19 | , target_to_camera(Eigen::Isometry3d::Identity()) 20 | , sampler(TARGET_ROWS, TARGET_COLS) 21 | { 22 | double x = static_cast(TARGET_ROWS - 1) * SPACING / 2.0; 23 | double y = static_cast(TARGET_COLS - 1) * SPACING / 2.0; 24 | target_to_camera.translate(Eigen::Vector3d(x, y, 0.5)); 25 | target_to_camera.rotate(Eigen::AngleAxisd(M_PI, Eigen::Vector3d::UnitX())); 26 | } 27 | 28 | test::Camera camera; 29 | test::Target target; 30 | Eigen::Isometry3d target_to_camera; 31 | GridCorrespondenceSampler sampler; 32 | }; 33 | 34 | TEST_F(HomographyTest, PerfectInitialConditions) 35 | { 36 | rct_optimizations::Correspondence2D3D::Set correspondence_set; 37 | EXPECT_NO_THROW(correspondence_set = 38 | test::getCorrespondences(target_to_camera, Eigen::Isometry3d::Identity(), camera, target, true)); 39 | 40 | Eigen::VectorXd error = calculateHomographyError(correspondence_set, sampler); 41 | // Expect all of the errors to be extremely small 42 | EXPECT_LT(error.maxCoeff(), 1.0e-12); 43 | } 44 | 45 | TEST_F(HomographyTest, SwapCorrespondencesConditions) 46 | { 47 | rct_optimizations::Correspondence2D3D::Set correspondence_set; 48 | EXPECT_NO_THROW(correspondence_set = 49 | test::getCorrespondences(target_to_camera, Eigen::Isometry3d::Identity(), camera, target, true)); 50 | 51 | // Swap the image measurements between 2 arbitrary correspondences 52 | Correspondence2D3D& c1 = correspondence_set.at(10); 53 | Eigen::Vector2d in_image_1 = c1.in_image; 54 | Correspondence2D3D& c2 = correspondence_set.at(21); 55 | Eigen::Vector2d in_image_2 = c2.in_image; 56 | c1.in_image = in_image_2; 57 | c2.in_image = in_image_1; 58 | 59 | Eigen::VectorXd error = calculateHomographyError(correspondence_set, sampler); 60 | 61 | // Expect the error of the points used to create the homography matrix to be very small 62 | std::vector sample_indices = sampler.getSampleCorrespondenceIndices(); 63 | for (std::size_t idx : sample_indices) 64 | { 65 | EXPECT_LT(error(idx), 1.0e-12); 66 | } 67 | 68 | // Expect the homography to be off by more than one pixel because of the swap 69 | EXPECT_GT(error.maxCoeff(), 1.0); 70 | } 71 | 72 | TEST_F(HomographyTest, NoisyCorrespondences) 73 | { 74 | rct_optimizations::Correspondence2D3D::Set correspondence_set; 75 | EXPECT_NO_THROW(correspondence_set = 76 | test::getCorrespondences(target_to_camera, Eigen::Isometry3d::Identity(), camera, target, true)); 77 | 78 | // Add Gaussian noise to the image features 79 | std::mt19937 mt_rand(RCT_RANDOM_SEED); 80 | const double stdev = 1.0; 81 | std::normal_distribution dist(0.0, stdev); 82 | for (Correspondence2D3D& corr : correspondence_set) 83 | { 84 | Eigen::Vector2d noise; 85 | noise << dist(mt_rand), dist(mt_rand); 86 | corr.in_image += noise; 87 | } 88 | 89 | Eigen::VectorXd error = calculateHomographyError(correspondence_set, sampler); 90 | 91 | // TODO: create an informed expectation for the max or average error. Initial testing indicates that the average error 92 | // can occasionally be > 4 * stddev 93 | // double stdev_mag = stdev * std::sqrt(2.0); 94 | EXPECT_GT(error.maxCoeff(), 0.0); 95 | EXPECT_GT(error.mean(), 0.0); 96 | } 97 | 98 | int main(int argc, char** argv) 99 | { 100 | testing::InitGoogleTest(&argc, argv); 101 | return RUN_ALL_TESTS(); 102 | } 103 | -------------------------------------------------------------------------------- /rct_optimizations/test/include/rct_optimizations_tests/observation_creator.h: -------------------------------------------------------------------------------- 1 | #ifndef RCT_OPTIMIZATIONS_TESTS_OBS_CREATOR_H 2 | #define RCT_OPTIMIZATIONS_TESTS_OBS_CREATOR_H 3 | 4 | #include 5 | #include 6 | 7 | namespace rct_optimizations 8 | { 9 | namespace test 10 | { 11 | /** 12 | * @brief Observes a set of simulated 2D-3D correspondences given a 2D camera and target definition 13 | * This method performs validity checks to ensure target observations appear in the camera field of view 14 | * @param camera_pose - the transform from a common frame to the camera 15 | * @param target_pose - the transform from a common frame to the target 16 | * @param camera - the camera intrinsic parameters 17 | * @param target - the observation target definition 18 | * @param require_all - flag to require all target observations be seen in the camera 19 | * @return A set of all "seen" correspondences 20 | * @throws Exception if @ref require_all is true and not all observations are seen 21 | */ 22 | Correspondence2D3D::Set getCorrespondences(const Eigen::Isometry3d& camera_pose, 23 | const Eigen::Isometry3d& target_pose, 24 | const Camera& camera, 25 | const Target& target, 26 | const bool require_all = false); 27 | 28 | /** 29 | * @brief Observes a set of simulated 3D-3D correspondences given a target definition. 30 | * Note this method does not consider whether or not target observations can be seen by the camera 31 | * @param camera_pose - the transform from a common frame to the camera 32 | * @param target_pose - the transform from a common frame to the target 33 | * @param target - the observation target definition 34 | * @return 35 | */ 36 | Correspondence3D3D::Set getCorrespondences(const Eigen::Isometry3d& camera_pose, 37 | const Eigen::Isometry3d& target_pose, 38 | const Target& target) noexcept; 39 | 40 | /** 41 | * @brief Creates a 2D-3D observation set 42 | * @param camera - camera intrinsics 43 | * @param target - target definition 44 | * @param pose_generators - vector of shared pointers to variants of base class for camera pose generation 45 | * @param true_target_mount_to_target - the true transform from the target mount to the target 46 | * @param true_camera_mount_to_camera - the true transform from the camera mount to the camera 47 | * @param camera_base_to_target_base - the transform from the camera base frame to the target base frame (typically 48 | * identity) 49 | * @return 50 | */ 51 | Observation2D3D::Set 52 | createObservations(const Camera& camera, 53 | const Target& target, 54 | const std::vector>& pose_generators, 55 | const Eigen::Isometry3d& true_target_mount_to_target, 56 | const Eigen::Isometry3d& true_camera_mount_to_camera, 57 | const Eigen::Isometry3d& camera_base_to_target_base = Eigen::Isometry3d::Identity()); 58 | 59 | /** 60 | * @brief Creates a 3D-3D observation set 61 | * @param target - target definition 62 | * @param pose_generators - vector of shared pointers to variants of base class for camera pose generation 63 | * @param true_target_mount_to_target - the true transform from the target mount to the target 64 | * @param true_camera_mount_to_camera - the true transform from the camera mount to the camera 65 | * @param camera_base_to_target_base - the transform from the camera base frame to the target base frame (typically 66 | * identity) 67 | * @return 68 | */ 69 | Observation3D3D::Set 70 | createObservations(const Target& target, 71 | const std::vector>& pose_generators, 72 | const Eigen::Isometry3d& true_target_mount_to_target, 73 | const Eigen::Isometry3d& true_camera_mount_to_camera, 74 | const Eigen::Isometry3d& camera_base_to_target_base = Eigen::Isometry3d::Identity()); 75 | 76 | } // namespace test 77 | } // namespace rct_optimizations 78 | 79 | #endif // RCT_OPTIMIZATIONS_TESTS_OBS_CREATOR_H 80 | -------------------------------------------------------------------------------- /rct_optimizations/test/include/rct_optimizations_tests/utilities.h: -------------------------------------------------------------------------------- 1 | #ifndef RCT_OPTIMIZATIONS_TESTS_UTILITIES_H 2 | #define RCT_OPTIMIZATIONS_TESTS_UTILITIES_H 3 | 4 | #include 5 | #include 6 | 7 | namespace rct_optimizations 8 | { 9 | namespace test 10 | { 11 | /** 12 | * @brief a test pin-hole camera that has intrinsics, but also image size data needed for generating simulated 13 | * information 14 | */ 15 | struct Camera 16 | { 17 | CameraIntrinsics intr; 18 | int width; 19 | int height; 20 | }; 21 | 22 | /** 23 | * @brief Create a test camera set with kinect-like parameters 24 | * @return 25 | */ 26 | Camera makeKinectCamera(); 27 | 28 | /** 29 | * @brief A sample grid target for test purposes 30 | * Looking down at the target 31 | * - The origin is in the lower left corner 32 | * - The x coordinate defines the feature column 33 | * - The y coordinate defines the feature row 34 | * - The points are ordered row-wise from the top left corner to the bottom right corner 35 | */ 36 | struct Target 37 | { 38 | Target() = default; 39 | 40 | Target(const unsigned rows, const unsigned cols, const double spacing) 41 | : origin_idx((rows - 1) * cols), center(double(rows - 1) * spacing / 2.0, double(cols - 1) * spacing / 2.0, 0.0) 42 | { 43 | points.reserve(rows * cols); 44 | 45 | for (unsigned i = 1; i < (rows + 1); i++) 46 | { 47 | double y = (rows - i) * spacing; 48 | for (unsigned j = 0; j < cols; j++) 49 | { 50 | double x = j * spacing; 51 | Eigen::Vector3d point(x, y, 0.0); 52 | points.push_back(point); 53 | } 54 | } 55 | 56 | origin_idx = (rows - 1) * cols; 57 | } 58 | 59 | std::vector points; 60 | std::size_t origin_idx; 61 | Eigen::Vector3d center; 62 | }; 63 | 64 | /** 65 | * @brief Creates a pose that is perturbed with the input noise levels relative to the input pose 66 | * @param pose - reference pose 67 | * @param spatial_noise - translational noise standard deviation (m) 68 | * @param angle_noise - angular noise standard deviation (rad) 69 | * @return 70 | */ 71 | Eigen::Isometry3d perturbPose(const Eigen::Isometry3d& pose, double spatial_noise, double angle_noise); 72 | 73 | /** 74 | * @brief Creates a DH parameter-based robot representation of an ABB IRB2400 75 | * @return 76 | */ 77 | DHChain createABBIRB2400(); 78 | 79 | /** 80 | * @brief Creates a DH parameter-based robot representation of a generic two-axis part positioner with arbitrary base 81 | * offset 82 | * @return 83 | */ 84 | DHChain createTwoAxisPositioner(); 85 | 86 | /** 87 | * @brief Creates a kinematic chain whose DH parameters are pertubed with Gaussian noise relative to the reference 88 | * kinematic chain 89 | * @param in - reference kinematic chain 90 | * @param stddev - standard deviation to apply to all DH parameters individually 91 | * @return 92 | */ 93 | DHChain perturbDHChain(const DHChain& in, const double stddev); 94 | 95 | } // namespace test 96 | } // namespace rct_optimizations 97 | 98 | #endif // RCT_OPTIMIZATIONS_TESTS_UTILITIES_H 99 | -------------------------------------------------------------------------------- /rct_optimizations/test/local_parameterization_utest.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | using namespace rct_optimizations; 10 | 11 | TEST(LocalParameterizationTests, SubsetParameterization) 12 | { 13 | // Create matrices of mean and standard deviation values for each parameter 14 | Eigen::ArrayXXd mean(Eigen::ArrayXXd::Random(6, 4)); 15 | const double stdev_val = 0.1; 16 | Eigen::ArrayXXd stdev(Eigen::ArrayXXd::Constant(6, 4, stdev_val)); 17 | 18 | // Create a matrix of random values scaled up from a range of [0, 1] 19 | const double scale = 10.0; 20 | Eigen::ArrayXXd params(Eigen::ArrayXXd::Random(6, 4) * scale); 21 | 22 | Eigen::IOFormat fmt(4, 0, "|", "\n", "|", "|"); 23 | std::cout << "Mean\n" << mean.format(fmt) << std::endl; 24 | std::cout << "Original Parameters:\n" << params.format(fmt) << std::endl; 25 | 26 | auto* ml = new MaximumLikelihood(mean, stdev); 27 | 28 | // Run an optimization to see if we can drive the random numbers to the mean 29 | ceres::Problem problem; 30 | auto* cost_block = new ceres::DynamicAutoDiffCostFunction(ml); 31 | cost_block->AddParameterBlock(params.size()); 32 | cost_block->SetNumResiduals(params.size()); 33 | problem.AddResidualBlock(cost_block, nullptr, params.data()); 34 | 35 | // All values 36 | { 37 | std::map> mask; 38 | mask[params.data()] = std::vector(params.size()); 39 | std::iota(mask[params.data()].begin(), mask[params.data()].end(), 0); 40 | EXPECT_NO_THROW(addSubsetParameterization(problem, mask)); 41 | 42 | // Expect there to be no parameterization, but the entire block should be constant 43 | EXPECT_EQ(problem.GetParameterization(params.data()), nullptr); 44 | EXPECT_TRUE(problem.IsParameterBlockConstant(params.data())); 45 | } 46 | 47 | // Index out of range 48 | { 49 | std::map> mask; 50 | mask[params.data()] = std::vector(params.size()); 51 | int bad_idx = params.size() * 2; 52 | mask[params.data()].insert(mask[params.data()].begin(), { bad_idx, 0, 1, 2 }); 53 | EXPECT_THROW(addSubsetParameterization(problem, mask), OptimizationException); 54 | EXPECT_EQ(problem.GetParameterization(params.data()), nullptr); 55 | } 56 | 57 | // Empty mask 58 | { 59 | std::map> mask; 60 | EXPECT_NO_THROW(addSubsetParameterization(problem, mask)); 61 | // An empty mask should not have added any local parameterization 62 | EXPECT_EQ(problem.GetParameterization(params.data()), nullptr); 63 | } 64 | 65 | // Hold the zero-th row constant 66 | Eigen::ArrayXXd original_params = params; 67 | { 68 | std::map> mask; 69 | mask[params.data()] = std::vector(); 70 | // Remember, Eigen stores values internally in column-wise order 71 | for (Eigen::Index i = 0; i < params.cols(); ++i) 72 | { 73 | mask[params.data()].push_back(i * params.rows()); 74 | } 75 | EXPECT_NO_THROW(addSubsetParameterization(problem, mask)); 76 | EXPECT_NE(problem.GetParameterization(params.data()), nullptr); 77 | } 78 | 79 | // Solve the optimization 80 | ceres::Solver::Options options; 81 | ceres::Solver::Summary summary; 82 | ceres::Solve(options, &problem, &summary); 83 | 84 | EXPECT_TRUE(summary.termination_type == ceres::CONVERGENCE); 85 | 86 | std::cout << "Optimized Parameters\n" << params.format(fmt) << std::endl; 87 | 88 | // Calculate the difference between the parameters 89 | Eigen::ArrayXXd diff = original_params - params; 90 | EXPECT_LE(diff.row(0).abs().sum(), std::numeric_limits::epsilon()); 91 | } 92 | 93 | int main(int argc, char** argv) 94 | { 95 | testing::InitGoogleTest(&argc, argv); 96 | return RUN_ALL_TESTS(); 97 | } 98 | -------------------------------------------------------------------------------- /rct_optimizations/test/maximum_likelihood_utest.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | 8 | using namespace rct_optimizations; 9 | 10 | TEST(MaximumLikelihoodTests, NaiveTest) 11 | { 12 | // Create matrices of mean and standard deviation values for each parameter 13 | Eigen::ArrayXXd mean(Eigen::ArrayXXd::Random(6, 4)); 14 | const double stdev_val = 0.1; 15 | Eigen::ArrayXXd stdev(Eigen::ArrayXXd::Constant(6, 4, stdev_val)); 16 | 17 | // Create a matrix of random values scaled up from a range of [0, 1] 18 | const double scale = 10.0; 19 | Eigen::ArrayXXd rand(Eigen::ArrayXXd::Random(6, 4) * scale); 20 | 21 | Eigen::IOFormat fmt(4, 0, "|", "\n", "|", "|"); 22 | std::cout << "Mean\n" << mean.format(fmt) << std::endl; 23 | std::cout << "Random:\n" << rand.format(fmt) << std::endl; 24 | 25 | auto* ml = new MaximumLikelihood(mean, stdev); 26 | 27 | // Check the cost function 28 | { 29 | Eigen::ArrayXXd residual(Eigen::ArrayXXd::Zero(6, 4)); 30 | std::vector params = { rand.data() }; 31 | 32 | bool success = ml->operator()(params.data(), residual.data()); 33 | EXPECT_TRUE(success); 34 | EXPECT_TRUE(residual.isApprox((rand - mean) / stdev)); 35 | } 36 | 37 | // Check that inputs of different sizes to the cost function results in an exception 38 | { 39 | EXPECT_THROW(MaximumLikelihood(mean, stdev.block<2, 2>(0, 0)), OptimizationException); 40 | } 41 | 42 | // Run an optimization to see if we can drive the random numbers to the mean 43 | ceres::Problem problem; 44 | auto* cost_block = new ceres::DynamicAutoDiffCostFunction(ml); 45 | cost_block->AddParameterBlock(rand.size()); 46 | cost_block->SetNumResiduals(rand.size()); 47 | problem.AddResidualBlock(cost_block, nullptr, rand.data()); 48 | 49 | // Solve the optimization 50 | ceres::Solver::Options options; 51 | ceres::Solver::Summary summary; 52 | ceres::Solve(options, &problem, &summary); 53 | 54 | EXPECT_TRUE(summary.termination_type == ceres::CONVERGENCE); 55 | 56 | // Calculate the difference between the parameters 57 | Eigen::ArrayXXd diff = (rand - mean).abs(); 58 | std::cout << "Difference:\n" << diff.format(fmt) << std::endl; 59 | 60 | // Check element-wise that absolute difference between the mean matrix and optimized random matrix is almost zero 61 | for (auto row = 0; row < diff.rows(); ++row) 62 | { 63 | for (auto col = 0; col < diff.cols(); ++col) 64 | { 65 | EXPECT_LT(diff(row, col), 1.0e-12); 66 | } 67 | } 68 | } 69 | 70 | int main(int argc, char** argv) 71 | { 72 | testing::InitGoogleTest(&argc, argv); 73 | return RUN_ALL_TESTS(); 74 | } 75 | -------------------------------------------------------------------------------- /rct_optimizations/test/serialization_utest.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | 7 | using namespace rct_optimizations; 8 | 9 | template 10 | void serialize(const std::string& file, const T& val) 11 | { 12 | std::ofstream ofh(file); 13 | if (!ofh) 14 | throw std::runtime_error("Failed to open file '" + file + "'"); 15 | 16 | YAML::Node n = YAML::Node(val); 17 | ofh << n; 18 | } 19 | 20 | template 21 | T deserialize(const std::string& file) 22 | { 23 | YAML::Node node; 24 | node = YAML::LoadFile(file); 25 | return node.as(); 26 | } 27 | 28 | template 29 | struct ProblemCreator 30 | { 31 | static ProblemT createProblem(const Eigen::Isometry3d& true_target, 32 | const Eigen::Isometry3d& true_camera, 33 | const std::shared_ptr& pose_generator, 34 | const test::Target& target); 35 | }; 36 | 37 | template <> 38 | ExtrinsicHandEyeProblem2D3D 39 | ProblemCreator::createProblem(const Eigen::Isometry3d& true_target, 40 | const Eigen::Isometry3d& true_camera, 41 | const std::shared_ptr& pose_generator, 42 | const test::Target& target) 43 | { 44 | test::Camera camera = test::makeKinectCamera(); 45 | 46 | ExtrinsicHandEyeProblem2D3D problem; 47 | problem.intr = camera.intr; 48 | problem.target_mount_to_target_guess = true_target; 49 | problem.camera_mount_to_camera_guess = true_camera; 50 | problem.observations = test::createObservations(camera, target, { pose_generator }, true_target, true_camera); 51 | 52 | return problem; 53 | } 54 | 55 | template <> 56 | ExtrinsicHandEyeProblem3D3D 57 | ProblemCreator::createProblem(const Eigen::Isometry3d& true_target, 58 | const Eigen::Isometry3d& true_camera, 59 | const std::shared_ptr& pose_generator, 60 | const test::Target& target) 61 | { 62 | ExtrinsicHandEyeProblem3D3D problem; 63 | problem.target_mount_to_target_guess = true_target; 64 | problem.camera_mount_to_camera_guess = true_camera; 65 | problem.observations = test::createObservations(target, { pose_generator }, true_target, true_camera); 66 | return problem; 67 | } 68 | 69 | template 70 | class SerializationTestFixture : public ::testing::Test 71 | { 72 | public: 73 | SerializationTestFixture() 74 | : target_mount_to_target(Eigen::Isometry3d::Identity()) 75 | , camera_mount_to_camera(Eigen::Isometry3d::Identity()) 76 | , target(5, 7, 0.025) 77 | , pg(std::make_shared()) 78 | { 79 | target_mount_to_target.translate(Eigen::Vector3d(1.0, 0, 0.0)); 80 | 81 | camera_mount_to_camera.translation() = Eigen::Vector3d(0.05, 0, 0.1); 82 | camera_mount_to_camera.linear() << 0, 0, 1, -1, 0, 0, 0, -1, 0; 83 | } 84 | 85 | Eigen::Isometry3d target_mount_to_target; 86 | Eigen::Isometry3d camera_mount_to_camera; 87 | test::Target target; 88 | std::shared_ptr pg; 89 | }; 90 | 91 | using Implementations = 92 | testing::Types; 93 | 94 | TYPED_TEST_CASE(SerializationTestFixture, Implementations); 95 | 96 | TYPED_TEST(SerializationTestFixture, Test) 97 | { 98 | TypeParam problem = ProblemCreator::createProblem( 99 | this->target_mount_to_target, this->camera_mount_to_camera, this->pg, this->target); 100 | 101 | const std::string filename = "/tmp/problem.yaml"; 102 | ASSERT_NO_THROW(serialize(filename, problem)); 103 | TypeParam deserialized_problem; 104 | ASSERT_NO_THROW(deserialized_problem = deserialize(filename)); 105 | ASSERT_EQ(problem, deserialized_problem); 106 | } 107 | 108 | int main(int argc, char** argv) 109 | { 110 | testing::InitGoogleTest(&argc, argv); 111 | return RUN_ALL_TESTS(); 112 | } 113 | -------------------------------------------------------------------------------- /rct_optimizations/test/src/utilities.cpp: -------------------------------------------------------------------------------- 1 | #include "rct_optimizations_tests/utilities.h" 2 | #include 3 | 4 | namespace rct_optimizations 5 | { 6 | namespace test 7 | { 8 | Camera makeKinectCamera() 9 | { 10 | CameraIntrinsics intr; 11 | intr.fx() = 550.0; 12 | intr.fy() = 550.0; 13 | intr.cx() = 640 / 2; 14 | intr.cy() = 480 / 2; 15 | 16 | Camera camera; 17 | camera.intr = intr; 18 | camera.width = 640; 19 | camera.height = 480; 20 | 21 | return camera; 22 | } 23 | 24 | Eigen::Isometry3d perturbPose(const Eigen::Isometry3d& pose, double spatial_noise, double angle_noise) 25 | { 26 | auto mt_rand = std::mt19937(RCT_RANDOM_SEED); 27 | 28 | auto spatial_dist = std::bind(std::uniform_real_distribution{ -spatial_noise, spatial_noise }, mt_rand); 29 | auto angle_dist = std::bind(std::uniform_real_distribution{ -angle_noise, angle_noise }, mt_rand); 30 | auto one_to_one = std::bind(std::uniform_real_distribution{ -1, 1 }, mt_rand); 31 | 32 | Eigen::Vector3d translation(spatial_dist(), spatial_dist(), spatial_dist()); 33 | Eigen::Vector3d rot_axis(one_to_one(), one_to_one(), one_to_one()); 34 | rot_axis.normalize(); 35 | 36 | double angle = angle_dist(); 37 | 38 | Eigen::Isometry3d new_pose = pose * Eigen::Translation3d(translation) * Eigen::AngleAxisd(angle, rot_axis); 39 | 40 | return new_pose; 41 | } 42 | 43 | DHChain createABBIRB2400() 44 | { 45 | std::vector transforms; 46 | transforms.reserve(6); 47 | 48 | Eigen::Vector4d p1, p2, p3, p4, p5, p6; 49 | 50 | p1 << 0.615, 0.0, 0.100, -M_PI / 2.0; 51 | p2 << 0.0, -M_PI / 2.0, 0.705, 0.0; 52 | p3 << 0.0, 0.0, 0.135, -M_PI / 2.0; 53 | p4 << 0.755, 0.0, 0.0, M_PI / 2.0; 54 | p5 << 0.0, 0.0, 0.0, -M_PI / 2.0; 55 | p6 << 0.085, M_PI, 0.0, 0.0; 56 | 57 | transforms.emplace_back(p1, DHJointType::REVOLUTE, "j1", RCT_RANDOM_SEED); 58 | transforms.emplace_back(p2, DHJointType::REVOLUTE, "j2", RCT_RANDOM_SEED); 59 | transforms.emplace_back(p3, DHJointType::REVOLUTE, "j3", RCT_RANDOM_SEED); 60 | transforms.emplace_back(p4, DHJointType::REVOLUTE, "j4", RCT_RANDOM_SEED); 61 | transforms.emplace_back(p5, DHJointType::REVOLUTE, "j5", RCT_RANDOM_SEED); 62 | transforms.emplace_back(p6, DHJointType::REVOLUTE, "j6", RCT_RANDOM_SEED); 63 | 64 | return DHChain(transforms); 65 | } 66 | 67 | DHChain createTwoAxisPositioner() 68 | { 69 | std::vector transforms; 70 | transforms.reserve(2); 71 | 72 | Eigen::Vector4d p1, p2; 73 | p1 << 0.0, 0.0, 0.0, -M_PI / 2.0; 74 | p2 << -0.475, -M_PI / 2.0, 0.0, 0.0; 75 | 76 | // Add the first DH transform 77 | { 78 | DHTransform t(p1, DHJointType::REVOLUTE, "j1", RCT_RANDOM_SEED); 79 | t.max = M_PI; 80 | t.min = -M_PI; 81 | transforms.push_back(t); 82 | } 83 | // Add the second DH transform 84 | { 85 | DHTransform dh_transform(p2, DHJointType::REVOLUTE, "j2", RCT_RANDOM_SEED); 86 | dh_transform.max = 2.0 * M_PI; 87 | dh_transform.min = -2.0 * M_PI; 88 | transforms.push_back(dh_transform); 89 | } 90 | 91 | // Set an arbitrary base offset 92 | Eigen::Isometry3d base_offset(Eigen::Isometry3d::Identity()); 93 | base_offset.translate(Eigen::Vector3d(2.2, 0.0, 1.6)); 94 | base_offset.rotate(Eigen::AngleAxisd(M_PI / 2.0, Eigen::Vector3d::UnitX())); 95 | 96 | return DHChain(transforms, base_offset); 97 | } 98 | 99 | DHChain perturbDHChain(const DHChain& in, const double stddev) 100 | { 101 | std::mt19937 mt_rand(RCT_RANDOM_SEED); 102 | std::normal_distribution norm(0.0, stddev); 103 | 104 | // Get the joint types and nominal DH table 105 | std::vector joint_types = in.getJointTypes(); 106 | Eigen::MatrixX4d dh = in.getDHTable(); 107 | 108 | // Perturb each value in the DH table randomly by the input standard deviation 109 | dh = dh.unaryExpr([&norm, &mt_rand](const double val) { return val + norm(mt_rand); }); 110 | 111 | std::vector transforms; 112 | transforms.reserve(joint_types.size()); 113 | for (std::size_t i = 0; i < joint_types.size(); ++i) 114 | { 115 | transforms.emplace_back(dh.row(i), joint_types[i], "j" + std::to_string(i + 1), RCT_RANDOM_SEED); 116 | } 117 | return DHChain(transforms, in.getBaseOffset()); 118 | } 119 | 120 | } // namespace test 121 | } // namespace rct_optimizations 122 | -------------------------------------------------------------------------------- /rct_ros_tools/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.16.0) 2 | project(rct_ros_tools) 3 | 4 | add_compile_options(-Wall -Wextra) 5 | 6 | find_package(rct_common REQUIRED) 7 | find_package(rct_optimizations REQUIRED) 8 | find_package(rct_image_tools REQUIRED) 9 | 10 | find_package(OpenCV REQUIRED) 11 | 12 | find_package(Eigen3 REQUIRED) 13 | if(NOT EIGEN3_INCLUDE_DIRS) 14 | set(EIGEN3_INCLUDE_DIRS ${EIGEN3_INCLUDE_DIR}) 15 | endif() 16 | if(NOT TARGET Eigen3::Eigen) 17 | find_package(Threads REQUIRED) 18 | add_library(Eigen3::Eigen IMPORTED INTERFACE) 19 | set_property(TARGET Eigen3::Eigen PROPERTY INTERFACE_COMPILE_DEFINITIONS ${EIGEN3_DEFINITIONS}) 20 | set_property(TARGET Eigen3::Eigen PROPERTY INTERFACE_INCLUDE_DIRECTORIES ${EIGEN3_INCLUDE_DIRS}) 21 | endif() 22 | 23 | find_package(yaml-cpp REQUIRED) 24 | 25 | find_package( 26 | catkin REQUIRED 27 | COMPONENTS roscpp 28 | # TODO: Export these to a different package dedicated to 'ros tools' 29 | tf2_ros 30 | tf2_eigen 31 | cv_bridge 32 | image_transport 33 | std_srvs 34 | pluginlib) 35 | 36 | # This package only provides examples. It does not export any tools. 37 | catkin_package( 38 | INCLUDE_DIRS 39 | include 40 | LIBRARIES 41 | ${PROJECT_NAME} 42 | CATKIN_DEPENDS 43 | roscpp 44 | tf2_ros 45 | tf2_eigen 46 | cv_bridge 47 | image_transport 48 | std_srvs 49 | pluginlib 50 | DEPENDS 51 | rct_common 52 | rct_optimizations 53 | rct_image_tools 54 | OpenCV 55 | Eigen3 56 | yaml-cpp) 57 | 58 | # ###################################################################################################################### 59 | # Build ## 60 | # ###################################################################################################################### 61 | include_directories(include ${catkin_INCLUDE_DIRS}) 62 | 63 | # Create a library for loading and saving "data sets": directory based archives of poses and images that can be reloaded 64 | # for testing and development purposes. 65 | add_library(${PROJECT_NAME} src/data_set.cpp src/loader_utils.cpp src/parameter_loaders.cpp) 66 | add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 67 | target_link_libraries( 68 | ${PROJECT_NAME} 69 | yaml-cpp 70 | ${OpenCV_LIBRARIES} 71 | rct::rct_optimizations 72 | rct::rct_image_tools 73 | rct::rct_common) 74 | target_compile_features(${PROJECT_NAME} PUBLIC cxx_std_11) 75 | 76 | add_library(${PROJECT_NAME}_target_loader_plugins src/target_loader_plugins.cpp) 77 | target_link_libraries( 78 | ${PROJECT_NAME}_target_loader_plugins 79 | ${catkin_LIBRARIES} 80 | yaml-cpp 81 | rct::rct_image_tools) 82 | target_compile_features(${PROJECT_NAME}_target_loader_plugins PUBLIC cxx_std_11) 83 | 84 | # Executable for collecting data sets via subscribers and triggered with services See readme (TODO: Write a readme) 85 | add_executable(${PROJECT_NAME}_cmd src/command_line_cal.cpp) 86 | set_target_properties(${PROJECT_NAME}_cmd PROPERTIES OUTPUT_NAME command_line_data_collection PREFIX "") 87 | add_dependencies(${PROJECT_NAME}_cmd ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 88 | target_link_libraries( 89 | ${PROJECT_NAME}_cmd 90 | ${catkin_LIBRARIES} 91 | ${PROJECT_NAME} 92 | rct::rct_image_tools) 93 | target_compile_features(${PROJECT_NAME}_cmd PRIVATE cxx_std_11) 94 | 95 | # ###################################################################################################################### 96 | # Testing ## 97 | # ###################################################################################################################### 98 | 99 | if(CATKIN_ENABLE_TESTING AND RCT_BUILD_TESTS) 100 | find_package(rostest REQUIRED) 101 | find_package(GTest REQUIRED) 102 | add_rostest_gtest(${PROJECT_NAME}_target_finder_plugin_utest test/target_finder_plugin.test 103 | test/target_finder_plugin_utest.cpp) 104 | target_link_libraries( 105 | ${PROJECT_NAME}_target_finder_plugin_utest 106 | ${PROJECT_NAME} 107 | GTest::GTest 108 | GTest::Main 109 | ${catkin_LIBRARIES}) 110 | target_compile_features(${PROJECT_NAME}_target_finder_plugin_utest PRIVATE cxx_std_11) 111 | endif() 112 | 113 | # ###################################################################################################################### 114 | # Install ## 115 | # ###################################################################################################################### 116 | 117 | install( 118 | TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_cmd 119 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 120 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 121 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) 122 | 123 | install(DIRECTORY include/${PROJECT_NAME}/ DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}) 124 | -------------------------------------------------------------------------------- /rct_ros_tools/README.md: -------------------------------------------------------------------------------- 1 | # RCT ROS Tools 2 | ** THIS IS UNDER CONSTRUCTION ** 3 | 4 | ## Description 5 | This package contains some basic tools for loading, saving, and collecting simple data sets for offline processing. 6 | This **IS NOT CURRENTLY MEANT TO ROBUST** and feature-full. I really just wanted to facilitate testing. 7 | 8 | ## Loading/Saving Calibration Data Sets 9 | I wrote some very basic tools for loading/saving robot pose & image pair data sets: 10 | ```c++ 11 | // ... 12 | 13 | // Load the data set (and make sure it worked) 14 | const std::string data_path = ros::package::getPath("rct_examples") + "/data/test_set_10x10/cal_data.yaml"; 15 | boost::optional maybe_data_set = rct_ros_tools::parseFromFile(data_path); 16 | // Attempt to load the data set via the data record yaml file: 17 | if (!maybe_data_set) 18 | { 19 | std::cerr << "Failed to parse data set from path = " << data_path << "\n"; 20 | return 1; 21 | } 22 | // Now that its loaded let's create some aliases to make this nicer 23 | const std::vector& image_set = maybe_data_set->images; 24 | const std::vector& wrist_poses = maybe_data_set->tool_poses; 25 | 26 | // ... 27 | ``` 28 | 29 | The file pointed to, `cal_data.yaml`, has the following structure: 30 | ```yaml 31 | - pose: poses/0.yaml 32 | image: images/0.png 33 | - pose: poses/1.yaml 34 | image: images/1.png 35 | - pose: poses/2.yaml 36 | image: images/2.png 37 | - pose: poses/3.yaml 38 | image: images/3.png 39 | - pose: poses/4.yaml 40 | image: images/4.png 41 | 42 | # ... 43 | ``` 44 | 45 | Each array entry is a pair of image and pose data. Note that all paths are relative to `cal_data.yaml` itself. 46 | Each pose file has a simple structure: 47 | ```yaml 48 | x: 0.7169496300523684 49 | y: 0.00330346692205527 50 | z: 0.853313460465923 51 | qx: -0.01709391593413424 52 | qy: 0.99974825023776 53 | qz: -0.01437638468984423 54 | qw: -0.002133951223335981 55 | ``` 56 | 57 | ## Data Collection Tools 58 | There's one active tool for command line data collection of a single transform and image: 59 | 60 | ``` 61 | roslaunch rct_ros_tools command_line_data_collection.launch target_file:= 62 | ``` 63 | 64 | See the `rct_examples/config` directory for target examples. Additional parameters include `base_frame` and `tool_frame` to specify robot transforms, which default to `base_link` and `tool0` respectively. 65 | The `image_topic` arg can be used to control topic subscribed to: the node will publish annotated images to `image_topic + _observer` so you can see if the target is detected. 66 | -------------------------------------------------------------------------------- /rct_ros_tools/include/rct_ros_tools/data_set.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | 6 | #include 7 | #include 8 | #include 9 | 10 | namespace rct_ros_tools 11 | { 12 | struct ExtrinsicDataSet 13 | { 14 | std::vector images; 15 | std::vector tool_poses; 16 | }; 17 | 18 | boost::optional parseFromFile(const std::string& path); 19 | 20 | cv::Mat readImageOpenCV(const std::string& path); 21 | 22 | bool saveToDirectory(const std::string& path, const ExtrinsicDataSet& data); 23 | 24 | /** 25 | * @brief This class is used to generate correspondence sets for one/multiple static cameras 26 | * and a single moveing target. 27 | */ 28 | class ExtrinsicCorrespondenceDataSet 29 | { 30 | public: 31 | ExtrinsicCorrespondenceDataSet(const std::vector& extrinsic_data_set, 32 | const rct_image_tools::TargetFinder& target_finder, 33 | bool debug = false); 34 | 35 | /** @brief Get the number of cameras */ 36 | std::size_t getCameraCount() const; 37 | 38 | /** @brief Get the number of images taken by each camera */ 39 | std::size_t getImageCount() const; 40 | 41 | /** @brief Get numbers of cameras that found a image */ 42 | std::size_t getImageCameraCount(std::size_t image_index) const; 43 | 44 | /** @brief Get number of images found by a camera */ 45 | std::size_t getCameraImageCount(std::size_t camera_index) const; 46 | 47 | /** @brief Get if a correspondence set was found camera image pair */ 48 | bool foundCorrespondence(std::size_t camera_index, std::size_t image_index) const; 49 | 50 | /** @brief Get the correspondence set for a given camera and image index */ 51 | const rct_optimizations::Correspondence2D3D::Set& getCorrespondenceSet(std::size_t camera_index, 52 | std::size_t image_index) const; 53 | 54 | private: 55 | /** @brief Correspondence pairs for a given image and camera */ 56 | Eigen::Matrix correspondences_; 57 | 58 | /** @brief Mask matrix indicating if the target was found */ 59 | Eigen::Matrix mask_; 60 | }; 61 | 62 | } // namespace rct_ros_tools 63 | -------------------------------------------------------------------------------- /rct_ros_tools/include/rct_ros_tools/exceptions.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | 6 | namespace rct_ros_tools 7 | { 8 | /** 9 | * \brief Thrown during errors in loading or parsing data files 10 | */ 11 | class BadFileException : public std::runtime_error 12 | { 13 | public: 14 | BadFileException(const std::string& what) : std::runtime_error(what) {} 15 | }; 16 | 17 | } // namespace rct_ros_tools 18 | -------------------------------------------------------------------------------- /rct_ros_tools/include/rct_ros_tools/loader_utils.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | 6 | namespace rct_ros_tools 7 | { 8 | /** 9 | * @brief Converts an XmlRpcValue object into a YAML node 10 | * @param value 11 | * @return 12 | */ 13 | YAML::Node toYAML(XmlRpc::XmlRpcValue value); 14 | 15 | } // namespace rct_ros_tools 16 | -------------------------------------------------------------------------------- /rct_ros_tools/include/rct_ros_tools/parameter_loaders.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | 7 | namespace rct_ros_tools 8 | { 9 | /** 10 | * \brief Load camera intrinsics from a ROS parameter 11 | * \throws ros::InvalidParameterException 12 | */ 13 | rct_optimizations::CameraIntrinsics loadIntrinsics(const ros::NodeHandle& nh, const std::string& key); 14 | 15 | /** 16 | * \brief Load a target from a ROS parameter. Returns false if an error occurs. 17 | */ 18 | bool loadIntrinsics(const ros::NodeHandle& nh, const std::string& key, rct_optimizations::CameraIntrinsics& intr); 19 | 20 | /** 21 | * \brief Load camera intrinsics from a YAML file 22 | * \throws rct_ros_tools::BadFileException 23 | */ 24 | rct_optimizations::CameraIntrinsics loadIntrinsics(const std::string& path); 25 | 26 | /** 27 | * \brief Load camera intrinsics from a YAML file. Returns false if an error occurs. 28 | */ 29 | bool loadIntrinsics(const std::string& path, rct_optimizations::CameraIntrinsics& intrinsics); 30 | 31 | /** 32 | * \brief Load a pose from a ROS parameter 33 | * \throws ros::InvalidParameterException 34 | */ 35 | Eigen::Isometry3d loadPose(const ros::NodeHandle& nh, const std::string& key); 36 | 37 | /** 38 | * \brief Load a pose from a ROS parameter. Returns false if an error occurs. 39 | */ 40 | bool loadPose(const ros::NodeHandle& nh, const std::string& key, Eigen::Isometry3d& pose); 41 | 42 | /** 43 | * \brief Load a pose from a YAML file 44 | * \throws rct_ros_tools::BadFileException 45 | */ 46 | Eigen::Isometry3d loadPose(const std::string& path); 47 | 48 | /** 49 | * \brief Load a pose from a YAML file. Returns false if an error occurs. 50 | */ 51 | bool loadPose(const std::string& path, Eigen::Isometry3d& pose); 52 | 53 | } // namespace rct_ros_tools 54 | -------------------------------------------------------------------------------- /rct_ros_tools/include/rct_ros_tools/target_finder_plugin.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | 6 | #include 7 | #include 8 | 9 | namespace rct_ros_tools 10 | { 11 | /** 12 | * @brief Target finder plugin that can be initialized by file or XmlRpcValue (i.e. from a ROS parameter) 13 | */ 14 | class TargetFinderPlugin : public rct_image_tools::TargetFinder 15 | { 16 | public: 17 | using TargetFinder::TargetFinder; 18 | 19 | rct_image_tools::TargetFeatures findTargetFeatures(const cv::Mat& image) const override 20 | { 21 | return finder_->findTargetFeatures(image); 22 | } 23 | 24 | cv::Mat drawTargetFeatures(const cv::Mat& image, 25 | const rct_image_tools::TargetFeatures& target_features) const override 26 | { 27 | return finder_->drawTargetFeatures(image, target_features); 28 | } 29 | 30 | const rct_image_tools::Target& target() const override { return finder_->target(); } 31 | 32 | virtual void init(const YAML::Node& config) = 0; 33 | 34 | protected: 35 | std::shared_ptr finder_; 36 | }; 37 | 38 | } // namespace rct_ros_tools 39 | -------------------------------------------------------------------------------- /rct_ros_tools/launch/command_line_data_collection.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | -------------------------------------------------------------------------------- /rct_ros_tools/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | rct_ros_tools 4 | 0.2.1 5 | The rct_ros_tools package 6 | 7 | Jon Meyer 8 | Jon Meyer 9 | 10 | Apache 2.0 11 | 12 | catkin 13 | 14 | cv_bridge 15 | image_transport 16 | pluginlib 17 | rct_common 18 | rct_image_tools 19 | rct_optimizations 20 | roscpp 21 | std_srvs 22 | tf2_ros 23 | tf2_eigen 24 | yaml-cpp 25 | 26 | 27 | 28 | 29 | 30 | -------------------------------------------------------------------------------- /rct_ros_tools/plugin_description.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | -------------------------------------------------------------------------------- /rct_ros_tools/src/loader_utils.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | 5 | namespace 6 | { 7 | XmlRpc::XmlRpcValue::Type getArrayType(const XmlRpc::XmlRpcValue& xml) 8 | { 9 | if (xml.getType() != XmlRpc::XmlRpcValue::TypeArray) 10 | throw std::runtime_error("Input XmlRpcValue is not an array"); 11 | 12 | // Get the type of the first element 13 | XmlRpc::XmlRpcValue::Type type = xml[0].getType(); 14 | 15 | // Check that all subsequent elements are of the same type 16 | for (long i = 1; i < xml.size(); ++i) 17 | { 18 | if (xml[i].getType() != type) 19 | { 20 | std::stringstream ss; 21 | ss << "Type mismatch in array (element 0 = '" << type << "', element " << i << " = '" << xml[i].getType() << "')"; 22 | throw std::runtime_error(ss.str()); 23 | } 24 | } 25 | 26 | // Return the type 27 | return type; 28 | } 29 | 30 | template 31 | std::vector fromArray(XmlRpc::XmlRpcValue xml) 32 | { 33 | std::vector vec; 34 | vec.reserve(xml.size()); 35 | for (int i = 0; i < xml.size(); i++) 36 | { 37 | vec.push_back(static_cast(xml[i])); 38 | } 39 | return vec; 40 | } 41 | 42 | } // namespace 43 | 44 | namespace rct_ros_tools 45 | { 46 | YAML::Node toYAML(XmlRpc::XmlRpcValue value) 47 | { 48 | YAML::Node node; 49 | 50 | if (value.getType() != XmlRpc::XmlRpcValue::TypeStruct) 51 | throw std::runtime_error("Input XmlRpc object is not a struct"); 52 | 53 | // Loop over all elements in the XmlRpcValue 54 | for (auto it = value.begin(); it != value.end(); ++it) 55 | { 56 | switch (it->second.getType()) 57 | { 58 | case XmlRpc::XmlRpcValue::TypeStruct: 59 | node[it->first] = toYAML(it->second); 60 | break; 61 | case XmlRpc::XmlRpcValue::TypeArray: 62 | switch (getArrayType(it->second)) 63 | { 64 | case XmlRpc::XmlRpcValue::TypeBoolean: 65 | node[it->first] = fromArray(it->second); 66 | break; 67 | case XmlRpc::XmlRpcValue::TypeInt: 68 | node[it->first] = fromArray(it->second); 69 | break; 70 | case XmlRpc::XmlRpcValue::TypeDouble: 71 | node[it->first] = fromArray(it->second); 72 | break; 73 | case XmlRpc::XmlRpcValue::TypeString: 74 | node[it->first] = fromArray(it->second); 75 | break; 76 | default: 77 | throw std::runtime_error("Unsupported array type (" + std::to_string(it->second.getType()) + ")"); 78 | } 79 | break; 80 | case XmlRpc::XmlRpcValue::TypeBoolean: 81 | node[it->first] = static_cast(it->second); 82 | break; 83 | case XmlRpc::XmlRpcValue::TypeInt: 84 | node[it->first] = static_cast(it->second); 85 | break; 86 | case XmlRpc::XmlRpcValue::TypeDouble: 87 | node[it->first] = static_cast(it->second); 88 | break; 89 | case XmlRpc::XmlRpcValue::TypeString: 90 | node[it->first] = static_cast(it->second); 91 | break; 92 | default: 93 | throw std::runtime_error("Unsupported type '" + std::to_string(it->second.getType()) + "'"); 94 | } 95 | } 96 | return node; 97 | } 98 | 99 | } // namespace rct_ros_tools 100 | -------------------------------------------------------------------------------- /rct_ros_tools/src/parameter_loaders.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | 6 | #include 7 | 8 | template 9 | static bool read(XmlRpc::XmlRpcValue& xml, const std::string& key, T& value) 10 | { 11 | if (!xml.hasMember(key)) 12 | return false; 13 | try 14 | { 15 | value = static_cast(xml[key]); 16 | } 17 | catch (const XmlRpc::XmlRpcException& ex) 18 | { 19 | ROS_ERROR_STREAM(ex.getMessage()); 20 | return false; 21 | } 22 | return true; 23 | } 24 | 25 | namespace rct_ros_tools 26 | { 27 | rct_optimizations::CameraIntrinsics loadIntrinsics(const ros::NodeHandle& nh, const std::string& key) 28 | { 29 | XmlRpc::XmlRpcValue xml; 30 | if (!nh.getParam(key, xml)) 31 | throw ros::InvalidParameterException(key); 32 | 33 | rct_optimizations::CameraIntrinsics intr; 34 | if (!read(xml, "fx", intr.fx())) 35 | throw ros::InvalidParameterException(key + "/fx"); 36 | if (!read(xml, "fy", intr.fy())) 37 | throw ros::InvalidParameterException(key + "/fy"); 38 | if (!read(xml, "cx", intr.cx())) 39 | throw ros::InvalidParameterException(key + "/cx"); 40 | if (!read(xml, "cy", intr.cy())) 41 | throw ros::InvalidParameterException(key + "/cy"); 42 | 43 | return intr; 44 | } 45 | 46 | bool loadIntrinsics(const ros::NodeHandle& nh, const std::string& key, rct_optimizations::CameraIntrinsics& intr) 47 | { 48 | try 49 | { 50 | intr = loadIntrinsics(nh, key); 51 | } 52 | catch (ros::InvalidParameterException& ex) 53 | { 54 | ROS_ERROR_STREAM("Failed to load intrisic parameter: " << ex.what()); 55 | return false; 56 | } 57 | return true; 58 | } 59 | 60 | rct_optimizations::CameraIntrinsics loadIntrinsics(const std::string& path) 61 | { 62 | try 63 | { 64 | YAML::Node n = YAML::LoadFile(path); 65 | return n.as(); 66 | } 67 | catch (YAML::Exception& ex) 68 | { 69 | throw BadFileException(std::string("YAML failure: ") + ex.what()); 70 | } 71 | } 72 | 73 | bool loadIntrinsics(const std::string& path, rct_optimizations::CameraIntrinsics& intrinsics) 74 | { 75 | try 76 | { 77 | intrinsics = loadIntrinsics(path); 78 | } 79 | catch (BadFileException& ex) 80 | { 81 | ROS_ERROR_STREAM("Failed to load intrinsics from file: " << ex.what()); 82 | return false; 83 | } 84 | return true; 85 | } 86 | 87 | Eigen::Isometry3d loadPose(const ros::NodeHandle& nh, const std::string& key) 88 | { 89 | XmlRpc::XmlRpcValue xml; 90 | if (!nh.getParam(key, xml)) 91 | throw ros::InvalidParameterException(key); 92 | 93 | double x, y, z, qx, qy, qz, qw; 94 | if (!read(xml, "x", x)) 95 | throw ros::InvalidParameterException(key + "/x"); 96 | if (!read(xml, "y", y)) 97 | throw ros::InvalidParameterException(key + "/y"); 98 | if (!read(xml, "z", z)) 99 | throw ros::InvalidParameterException(key + "/z"); 100 | if (!read(xml, "qx", qx)) 101 | throw ros::InvalidParameterException(key + "/qx"); 102 | if (!read(xml, "qy", qy)) 103 | throw ros::InvalidParameterException(key + "/qy"); 104 | if (!read(xml, "qz", qz)) 105 | throw ros::InvalidParameterException(key + "/qz"); 106 | if (!read(xml, "qw", qw)) 107 | throw ros::InvalidParameterException(key + "/qw"); 108 | 109 | Eigen::Isometry3d pose = 110 | Eigen::Translation3d(Eigen::Vector3d(x, y, z)) * Eigen::AngleAxisd(Eigen::Quaterniond(qw, qx, qy, qz)); 111 | return pose; 112 | } 113 | 114 | bool loadPose(const ros::NodeHandle& nh, const std::string& key, Eigen::Isometry3d& pose) 115 | { 116 | try 117 | { 118 | pose = loadPose(nh, key); 119 | } 120 | catch (ros::InvalidParameterException& ex) 121 | { 122 | ROS_ERROR_STREAM("Failed to load pose parameter: " << ex.what()); 123 | return false; 124 | } 125 | return true; 126 | } 127 | 128 | Eigen::Isometry3d loadPose(const std::string& path) 129 | { 130 | try 131 | { 132 | YAML::Node n = YAML::LoadFile(path); 133 | return n.as(); 134 | } 135 | catch (YAML::Exception& ex) 136 | { 137 | throw BadFileException(std::string("YAML failure: ") + ex.what()); 138 | } 139 | } 140 | 141 | bool loadPose(const std::string& path, Eigen::Isometry3d& pose) 142 | { 143 | try 144 | { 145 | pose = loadPose(path); 146 | } 147 | catch (BadFileException& ex) 148 | { 149 | ROS_ERROR_STREAM("Failed to load pose from file: " << ex.what()); 150 | return false; 151 | } 152 | return true; 153 | } 154 | 155 | } // namespace rct_ros_tools 156 | -------------------------------------------------------------------------------- /rct_ros_tools/test/charuco_grid_target.yaml: -------------------------------------------------------------------------------- 1 | target_finder: 2 | type: rct_ros_tools::CharucoGridTargetFinderPlugin 3 | rows: 7 4 | cols: 5 5 | chessboard_dim: 0.036195 6 | aruco_marker_dim: 0.018256 7 | dictionary: 10 # DICT_6X6_250 8 | -------------------------------------------------------------------------------- /rct_ros_tools/test/modified_circle_grid_target.yaml: -------------------------------------------------------------------------------- 1 | target_finder: 2 | type: rct_ros_tools::ModifiedCircleGridTargetFinderPlugin 3 | rows: 7 4 | cols: 5 5 | spacing: 0.025 6 | -------------------------------------------------------------------------------- /rct_ros_tools/test/target_finder_plugin.test: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | -------------------------------------------------------------------------------- /rct_ros_tools/test/target_finder_plugin_utest.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | const static std::string TARGET_FILENAME_PARAM = "target_file"; 10 | const static std::string TARGET_DEFINITION_PARAM = "target_finder"; 11 | 12 | TEST(Tools, TargetFinderPluginTest) 13 | { 14 | ros::NodeHandle pnh("~"); 15 | 16 | // Create the plugin loader 17 | const std::string package = "rct_ros_tools"; 18 | const std::string base_class = "rct_ros_tools::TargetFinderPlugin"; 19 | pluginlib::ClassLoader loader(package, base_class); 20 | 21 | // Get the declared plugins known to this base class 22 | std::vector plugins = loader.getDeclaredClasses(); 23 | 24 | // Check the number of defined plugins 25 | ASSERT_EQ(plugins.size(), 2); 26 | 27 | // Attempt to load and initialize each plugin using the two available methods 28 | for (const std::string plugin : plugins) 29 | { 30 | // Extract a namespace from the second part of the plugin name 31 | std::regex re(".*::(.*)"); 32 | std::smatch match; 33 | ASSERT_TRUE(std::regex_search(plugin, match, re)); 34 | const std::string ns = match[1]; 35 | 36 | // Create the plugin 37 | boost::shared_ptr finder; 38 | ASSERT_NO_THROW(finder = loader.createInstance(plugin)); 39 | 40 | // Initialize the plugin 41 | const std::string target_definition_param = ns + "/" + TARGET_DEFINITION_PARAM; 42 | XmlRpc::XmlRpcValue config; 43 | ASSERT_TRUE(pnh.getParam(target_definition_param, config)); 44 | ASSERT_NO_THROW(finder->init(rct_ros_tools::toYAML(config))); 45 | } 46 | } 47 | 48 | int main(int argc, char** argv) 49 | { 50 | ::testing::InitGoogleTest(&argc, argv); 51 | ros::init(argc, argv, "target_loader_utest"); 52 | return RUN_ALL_TESTS(); 53 | } 54 | -------------------------------------------------------------------------------- /robot_cal_tools/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package robot_cal_tools 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 0.2.1 (2024-08-13) 6 | ------------------ 7 | 8 | 0.2.0 (2024-07-25) 9 | ------------------ 10 | * fix cxx target version 11 | * Contributors: Levi Armstrong 12 | 13 | 0.1.1 (2022-05-17) 14 | ------------------ 15 | * remove changelogs 16 | * Fix robot_cal_tools version 17 | * Add changelogs 18 | * DH Object Serialization (`#100 `_) 19 | * Added packages to meta package package.xml 20 | * Updated DH chain to be YAML serializable 21 | * Updated DH chain related code for addition of private default constructor 22 | * Added serialization for DH types 23 | * Removed restriction on size of inputs for YAML decoding 24 | * Contributors: Levi Armstrong, Michael Ripperger 25 | 26 | 0.1.0 (2020-03-27) 27 | ------------------ 28 | * Initial commit 29 | * Contributors: Jonathan Meyer 30 | -------------------------------------------------------------------------------- /robot_cal_tools/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.16.0) 2 | project(robot_cal_tools) 3 | find_package(catkin REQUIRED) 4 | catkin_metapackage() 5 | -------------------------------------------------------------------------------- /robot_cal_tools/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | robot_cal_tools 4 | 0.2.1 5 | The robot_cal_tools package 6 | 7 | Jon Meyer 8 | 9 | TODO 10 | 11 | catkin 12 | rct_optimizations 13 | rct_image_tools 14 | rct_ros_tools 15 | rct_examples 16 | 17 | 18 | 19 | 20 | 21 | 22 | --------------------------------------------------------------------------------