├── .gitignore ├── CMakeLists.txt ├── LICENSE ├── README.md ├── example ├── full_match │ ├── config_template.json │ ├── left │ │ ├── ir1.png │ │ ├── ir2.png │ │ ├── ir3.png │ │ ├── ir4.png │ │ ├── ir5.png │ │ ├── ir6.png │ │ ├── ir7.png │ │ └── ir8.png │ └── right │ │ ├── rgb1.png │ │ ├── rgb2.png │ │ ├── rgb3.png │ │ ├── rgb4.png │ │ ├── rgb5.png │ │ ├── rgb6.png │ │ ├── rgb7.png │ │ └── rgb8.png └── pattern_match │ ├── config_template.json │ ├── left │ ├── ir1.png │ ├── ir2.png │ ├── ir3.png │ ├── ir4.png │ ├── ir5.png │ ├── ir6.png │ ├── ir7.png │ └── ir8.png │ └── right │ ├── rgb1.png │ ├── rgb2.png │ ├── rgb3.png │ ├── rgb4.png │ ├── rgb5.png │ ├── rgb6.png │ ├── rgb7.png │ └── rgb8.png ├── include └── libcalib │ ├── calib_def.h │ ├── calibration.h │ ├── ceres_type.h │ ├── config.h │ ├── extract.h │ ├── fileio.h │ └── interface.h ├── sample ├── CMakeLists.txt ├── calibrate_single_camera.cc └── calibrate_stereo_camera.cc └── src ├── CMakeLists.txt ├── calibration.cc ├── ceres_type.cc ├── config.cc ├── extract.cc ├── fileio.cc └── interface.cc /.gitignore: -------------------------------------------------------------------------------- 1 | # ---> C 2 | # Object files 3 | *.o 4 | *.ko 5 | *.obj 6 | *.elf 7 | 8 | # Precompiled Headers 9 | *.gch 10 | *.pch 11 | 12 | # Libraries 13 | *.lib 14 | *.a 15 | *.la 16 | *.lo 17 | 18 | # Shared objects (inc. Windows DLLs) 19 | *.dll 20 | *.so 21 | *.so.* 22 | *.dylib 23 | 24 | # Executables 25 | *.exe 26 | *.out 27 | *.app 28 | *.i*86 29 | *.x86_64 30 | *.hex 31 | 32 | # Debug files 33 | *.dSYM/ 34 | 35 | # ---> C++ 36 | # Compiled Object files 37 | *.slo 38 | *.lo 39 | *.o 40 | *.obj 41 | 42 | # Precompiled Headers 43 | *.gch 44 | *.pch 45 | 46 | # Compiled Dynamic libraries 47 | *.so 48 | *.dylib 49 | *.dll 50 | 51 | # Fortran module files 52 | *.mod 53 | 54 | # Compiled Static libraries 55 | *.lai 56 | *.la 57 | *.a 58 | *.lib 59 | 60 | # Executables 61 | *.exe 62 | *.out 63 | *.app 64 | 65 | # ---> CMake 66 | CMakeCache.txt 67 | CMakeFiles 68 | CMakeScripts 69 | Makefile 70 | cmake_install.cmake 71 | install_manifest.txt 72 | /bin/ 73 | /backup/ 74 | 75 | # ---> JetBrains 76 | # Covers JetBrains IDEs: IntelliJ, RubyMine, PhpStorm, AppCode, PyCharm, CLion, Android Studio 77 | 78 | *.iml 79 | 80 | ## Directory-based project format: 81 | .idea/ 82 | # if you remove the above rule, at least ignore the following: 83 | 84 | # User-specific stuff: 85 | # .idea/workspace.xml 86 | # .idea/tasks.xml 87 | # .idea/dictionaries 88 | 89 | # Sensitive or high-churn files: 90 | # .idea/dataSources.ids 91 | # .idea/dataSources.xml 92 | # .idea/sqlDataSources.xml 93 | # .idea/dynamic.xml 94 | # .idea/uiDesigner.xml 95 | 96 | # Gradle: 97 | # .idea/gradle.xml 98 | # .idea/libraries 99 | 100 | # Mongo Explorer plugin: 101 | # .idea/mongoSettings.xml 102 | 103 | ## File-based project format: 104 | *.ipr 105 | *.iws 106 | 107 | ## Plugin-specific files: 108 | 109 | # IntelliJ 110 | /out/ 111 | 112 | # mpeltonen/sbt-idea plugin 113 | .idea_modules/ 114 | 115 | # JIRA plugin 116 | atlassian-ide-plugin.xml 117 | 118 | # Crashlytics plugin (for Android Studio and IntelliJ) 119 | com_crashlytics_export_strings.xml 120 | crashlytics.properties 121 | crashlytics-build.properties 122 | 123 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | project(libcalib) 3 | 4 | set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin) 5 | set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin) 6 | set(CMAKE_C_COMPILER gcc) 7 | set(CMAKE_CXX_COMPILER g++) 8 | set(CMAKE_CXX_FLAGS "-std=c++14 -w -msse4.1 -fopenmp") 9 | set(CMAKE_EXE_LINKER_FLAGS "-fPIE -pie") 10 | set(CMAKE_CXX_FLAGS_DEBUG "-g") 11 | set(CMAKE_CXX_FLAGS_RELEASE "-O3 -DNDEBUG -s") 12 | set(CMAKE_SKIP_BUILD_RPATH OFF) 13 | set(CMAKE_DEBUG_POSTFIX _d) 14 | set(CMAKE_POSITION_INDEPENDENT_CODE ON) 15 | 16 | option(SHARED_LIB "Request build of shared libraries." ON) 17 | if (SHARED_LIB) 18 | set(LIB_TYPE SHARED) 19 | else () 20 | set(LIB_TYPE STATIC) 21 | endif () 22 | 23 | find_package(OpenCV REQUIRED) 24 | find_package(Eigen3 REQUIRED) 25 | find_package(Ceres REQUIRED) 26 | 27 | add_subdirectory(${PROJECT_SOURCE_DIR}/sample) 28 | add_subdirectory(${PROJECT_SOURCE_DIR}/src) 29 | 30 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2019 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | Libcalib 2 | --- 3 | This repository contains some sources to calibrate stereo cameras. 4 | 5 | #### Dependencies 6 | - OpenCV 7 | - Eigen3 8 | - [Ceres](https://github.com/ceres-solver/ceres-solver), version 1.14.0 9 | - [Rapidjson](https://github.com/Tencent/rapidjson) 10 | - [Libcbdetect](https://github.com/ftdlyc/libcbdetect) 11 | 12 | #### Compilation 13 | 1. build libcbdetect 14 | 2. copy libcbdetect header files into libcalib/include/libcbdetect 15 | 3. copy rapidjson header files into libcalib/include/rapidjson 16 | 3. copy libcbdetect.so into libcalib/lib 17 | 4. build libcalib 18 | -------------------------------------------------------------------------------- /example/full_match/config_template.json: -------------------------------------------------------------------------------- 1 | { 2 | "pattern_type": 0, 3 | "pattern_size": 24, 4 | "num_boards": 1, 5 | "camera1": { 6 | "detect_method": 2, 7 | "norm": true, 8 | "norm_half_kernel_size": 64, 9 | "radius": [5, 7, 9], 10 | "init_loc_thr": 0.01, 11 | "polynomial_fit_half_kernel_size": 4, 12 | "score_thr": 0.01, 13 | "show_cornres": false 14 | }, 15 | "camera2": { 16 | "detect_method": 2, 17 | "norm": false, 18 | "norm_half_kernel_size": 64, 19 | "radius": [7, 9, 11], 20 | "init_loc_thr": 0.01, 21 | "polynomial_fit_half_kernel_size": 5, 22 | "score_thr": 0.01, 23 | "show_cornres": false 24 | } 25 | } 26 | -------------------------------------------------------------------------------- /example/full_match/left/ir1.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ftdlyc/libcalib/116a5561d446016e9b463d704c191dc3e5c703c2/example/full_match/left/ir1.png -------------------------------------------------------------------------------- /example/full_match/left/ir2.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ftdlyc/libcalib/116a5561d446016e9b463d704c191dc3e5c703c2/example/full_match/left/ir2.png -------------------------------------------------------------------------------- /example/full_match/left/ir3.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ftdlyc/libcalib/116a5561d446016e9b463d704c191dc3e5c703c2/example/full_match/left/ir3.png -------------------------------------------------------------------------------- /example/full_match/left/ir4.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ftdlyc/libcalib/116a5561d446016e9b463d704c191dc3e5c703c2/example/full_match/left/ir4.png -------------------------------------------------------------------------------- /example/full_match/left/ir5.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ftdlyc/libcalib/116a5561d446016e9b463d704c191dc3e5c703c2/example/full_match/left/ir5.png -------------------------------------------------------------------------------- /example/full_match/left/ir6.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ftdlyc/libcalib/116a5561d446016e9b463d704c191dc3e5c703c2/example/full_match/left/ir6.png -------------------------------------------------------------------------------- /example/full_match/left/ir7.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ftdlyc/libcalib/116a5561d446016e9b463d704c191dc3e5c703c2/example/full_match/left/ir7.png -------------------------------------------------------------------------------- /example/full_match/left/ir8.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ftdlyc/libcalib/116a5561d446016e9b463d704c191dc3e5c703c2/example/full_match/left/ir8.png -------------------------------------------------------------------------------- /example/full_match/right/rgb1.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ftdlyc/libcalib/116a5561d446016e9b463d704c191dc3e5c703c2/example/full_match/right/rgb1.png -------------------------------------------------------------------------------- /example/full_match/right/rgb2.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ftdlyc/libcalib/116a5561d446016e9b463d704c191dc3e5c703c2/example/full_match/right/rgb2.png -------------------------------------------------------------------------------- /example/full_match/right/rgb3.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ftdlyc/libcalib/116a5561d446016e9b463d704c191dc3e5c703c2/example/full_match/right/rgb3.png -------------------------------------------------------------------------------- /example/full_match/right/rgb4.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ftdlyc/libcalib/116a5561d446016e9b463d704c191dc3e5c703c2/example/full_match/right/rgb4.png -------------------------------------------------------------------------------- /example/full_match/right/rgb5.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ftdlyc/libcalib/116a5561d446016e9b463d704c191dc3e5c703c2/example/full_match/right/rgb5.png -------------------------------------------------------------------------------- /example/full_match/right/rgb6.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ftdlyc/libcalib/116a5561d446016e9b463d704c191dc3e5c703c2/example/full_match/right/rgb6.png -------------------------------------------------------------------------------- /example/full_match/right/rgb7.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ftdlyc/libcalib/116a5561d446016e9b463d704c191dc3e5c703c2/example/full_match/right/rgb7.png -------------------------------------------------------------------------------- /example/full_match/right/rgb8.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ftdlyc/libcalib/116a5561d446016e9b463d704c191dc3e5c703c2/example/full_match/right/rgb8.png -------------------------------------------------------------------------------- /example/pattern_match/config_template.json: -------------------------------------------------------------------------------- 1 | { 2 | "pattern_type": 1, 3 | "pattern_size": 25, 4 | "num_boards": 1, 5 | "camera1": { 6 | "detect_method": 2, 7 | "norm": true, 8 | "norm_half_kernel_size": 64, 9 | "radius": [5, 7, 9], 10 | "init_loc_thr": 0.01, 11 | "polynomial_fit_half_kernel_size": 4, 12 | "score_thr": 0.01, 13 | "show_cornres": false 14 | }, 15 | "camera2": { 16 | "detect_method": 2, 17 | "norm": false, 18 | "norm_half_kernel_size": 64, 19 | "radius": [7, 9, 11], 20 | "init_loc_thr": 0.01, 21 | "polynomial_fit_half_kernel_size": 5, 22 | "score_thr": 0.01, 23 | "show_cornres": false 24 | } 25 | } 26 | -------------------------------------------------------------------------------- /example/pattern_match/left/ir1.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ftdlyc/libcalib/116a5561d446016e9b463d704c191dc3e5c703c2/example/pattern_match/left/ir1.png -------------------------------------------------------------------------------- /example/pattern_match/left/ir2.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ftdlyc/libcalib/116a5561d446016e9b463d704c191dc3e5c703c2/example/pattern_match/left/ir2.png -------------------------------------------------------------------------------- /example/pattern_match/left/ir3.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ftdlyc/libcalib/116a5561d446016e9b463d704c191dc3e5c703c2/example/pattern_match/left/ir3.png -------------------------------------------------------------------------------- /example/pattern_match/left/ir4.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ftdlyc/libcalib/116a5561d446016e9b463d704c191dc3e5c703c2/example/pattern_match/left/ir4.png -------------------------------------------------------------------------------- /example/pattern_match/left/ir5.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ftdlyc/libcalib/116a5561d446016e9b463d704c191dc3e5c703c2/example/pattern_match/left/ir5.png -------------------------------------------------------------------------------- /example/pattern_match/left/ir6.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ftdlyc/libcalib/116a5561d446016e9b463d704c191dc3e5c703c2/example/pattern_match/left/ir6.png -------------------------------------------------------------------------------- /example/pattern_match/left/ir7.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ftdlyc/libcalib/116a5561d446016e9b463d704c191dc3e5c703c2/example/pattern_match/left/ir7.png -------------------------------------------------------------------------------- /example/pattern_match/left/ir8.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ftdlyc/libcalib/116a5561d446016e9b463d704c191dc3e5c703c2/example/pattern_match/left/ir8.png -------------------------------------------------------------------------------- /example/pattern_match/right/rgb1.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ftdlyc/libcalib/116a5561d446016e9b463d704c191dc3e5c703c2/example/pattern_match/right/rgb1.png -------------------------------------------------------------------------------- /example/pattern_match/right/rgb2.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ftdlyc/libcalib/116a5561d446016e9b463d704c191dc3e5c703c2/example/pattern_match/right/rgb2.png -------------------------------------------------------------------------------- /example/pattern_match/right/rgb3.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ftdlyc/libcalib/116a5561d446016e9b463d704c191dc3e5c703c2/example/pattern_match/right/rgb3.png -------------------------------------------------------------------------------- /example/pattern_match/right/rgb4.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ftdlyc/libcalib/116a5561d446016e9b463d704c191dc3e5c703c2/example/pattern_match/right/rgb4.png -------------------------------------------------------------------------------- /example/pattern_match/right/rgb5.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ftdlyc/libcalib/116a5561d446016e9b463d704c191dc3e5c703c2/example/pattern_match/right/rgb5.png -------------------------------------------------------------------------------- /example/pattern_match/right/rgb6.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ftdlyc/libcalib/116a5561d446016e9b463d704c191dc3e5c703c2/example/pattern_match/right/rgb6.png -------------------------------------------------------------------------------- /example/pattern_match/right/rgb7.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ftdlyc/libcalib/116a5561d446016e9b463d704c191dc3e5c703c2/example/pattern_match/right/rgb7.png -------------------------------------------------------------------------------- /example/pattern_match/right/rgb8.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ftdlyc/libcalib/116a5561d446016e9b463d704c191dc3e5c703c2/example/pattern_match/right/rgb8.png -------------------------------------------------------------------------------- /include/libcalib/calib_def.h: -------------------------------------------------------------------------------- 1 | /** 2 | * Copyright 2019, ftdlyc 3 | * Licensed under the MIT license. 4 | */ 5 | 6 | #pragma once 7 | #ifndef CALIB_CALIB_DEF_H 8 | #define CALIB_CALIB_DEF_H 9 | 10 | #if defined(_MSC_VER) 11 | 12 | #define CALIB_INLINE __forceinline 13 | #define CALIB_NO_INLINE 14 | 15 | #if defined(CALIB_DLL_EXPORT) 16 | #define CALIB_DLL_DECL __declspec(dllexport) 17 | #else 18 | #define CALIB_DLL_DECL __declspec(dllimport) 19 | #endif 20 | #define CALIB_DLL_LOCAL 21 | 22 | #elif defined(__GNUC__) 23 | 24 | #define CALIB_INLINE inline __attribute__((always_inline)) 25 | #define CALIB_NO_INLINE __attribute__((noinline)) 26 | 27 | #if defined(CALIB_DLL_EXPORT) 28 | #define CALIB_DLL_DECL __attribute__((visibility("default"))) 29 | #define CALIB_DLL_LOCAL __attribute__((visibility("hidden"))) 30 | #else 31 | #define CALIB_DLL_DECL 32 | #define CALIB_DLL_LOCAL __attribute__((visibility("hidden"))) 33 | #endif 34 | 35 | #else 36 | #error Unsupported compiler. 37 | #endif 38 | 39 | #endif //CALIB_CALIB_DEF_H 40 | -------------------------------------------------------------------------------- /include/libcalib/calibration.h: -------------------------------------------------------------------------------- 1 | /** 2 | * Copyright 2019, ftdlyc 3 | * Licensed under the MIT license. 4 | */ 5 | 6 | #pragma once 7 | #ifndef CALIB_CALIBRATION_H 8 | #define CALIB_CALIBRATION_H 9 | 10 | #include 11 | #include 12 | 13 | #include "libcalib/calib_def.h" 14 | 15 | namespace calib { 16 | 17 | CALIB_DLL_LOCAL double calibrate_camera(const std::vector>& world_points, 18 | const std::vector>& image_points, 19 | std::vector& K, 20 | std::vector& D, 21 | std::vector>& Rwc, 22 | std::vector>& Twc); 23 | 24 | CALIB_DLL_LOCAL double calibrate_stereo_camera(const std::vector>& world_points, 25 | const std::vector>& image_points_1, 26 | const std::vector>& image_points_2, 27 | std::vector& K1, 28 | std::vector& K2, 29 | std::vector& D1, 30 | std::vector& D2, 31 | std::vector>& Rwc1, 32 | std::vector>& Rwc2, 33 | std::vector>& Twc1, 34 | std::vector>& Twc2, 35 | std::vector& R, 36 | std::vector& T); 37 | 38 | } // namespace calib 39 | 40 | #endif //CALIB_CALIBRATION_H 41 | -------------------------------------------------------------------------------- /include/libcalib/ceres_type.h: -------------------------------------------------------------------------------- 1 | /** 2 | * Copyright 2019, ftdlyc 3 | * Licensed under the MIT license. 4 | */ 5 | 6 | #pragma once 7 | #ifndef CALIB_CERES_TYPE_H 8 | #define CALIB_CERES_TYPE_H 9 | 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | 16 | #include "libcalib/calib_def.h" 17 | 18 | namespace calib { 19 | 20 | CALIB_DLL_LOCAL class SE3Parameterization : public ceres::LocalParameterization { 21 | public: 22 | bool Plus(const double* x, 23 | const double* delta, 24 | double* x_plus_delta) const; 25 | 26 | bool ComputeJacobian(const double* x, double* jacobian) const; 27 | 28 | bool MultiplyByJacobian(const double* x, 29 | const int num_rows, 30 | const double* global_matrix, 31 | double* local_matrix) const; 32 | 33 | int GlobalSize() const; 34 | 35 | int LocalSize() const; 36 | }; 37 | 38 | CALIB_DLL_LOCAL class ProjectCostFunction : public ceres::CostFunction { 39 | public: 40 | ProjectCostFunction() = delete; 41 | 42 | ProjectCostFunction(const cv::Point2d& world_point, 43 | const cv::Point2d& image_point, 44 | bool with_k3 = false); 45 | 46 | bool Evaluate(double const* const* parameters, 47 | double* residuals, 48 | double** jacobians) const; 49 | 50 | private: 51 | double world_point_[3]; 52 | double image_point_[2]; 53 | bool with_k3_; 54 | }; 55 | 56 | CALIB_DLL_LOCAL class StereoCostFunction : public ceres::CostFunction { 57 | public: 58 | StereoCostFunction() = delete; 59 | 60 | StereoCostFunction(const cv::Point2d& world_point, 61 | const cv::Point2d& image_point_1, 62 | const cv::Point2d& image_point_2, 63 | bool with_k3 = false); 64 | 65 | bool Evaluate(double const* const* parameters, 66 | double* residuals, 67 | double** jacobians) const; 68 | 69 | private: 70 | double world_point_[3]; 71 | double image_point_1_[2]; 72 | double image_point_2_[2]; 73 | bool with_k3_; 74 | }; 75 | 76 | } // namespace calib 77 | 78 | #endif //CALIB_CERES_TYPE_H 79 | -------------------------------------------------------------------------------- /include/libcalib/config.h: -------------------------------------------------------------------------------- 1 | /** 2 | * Copyright 2019, ftdlyc 3 | * Licensed under the MIT license. 4 | */ 5 | 6 | #pragma once 7 | #ifndef CALIB_CONFIG_H 8 | #define CALIB_CONFIG_H 9 | 10 | #include 11 | 12 | #include "libcalib/calib_def.h" 13 | #include "libcbdetect/config.h" 14 | 15 | namespace calib { 16 | 17 | typedef struct DetectParams { 18 | cbdetect::Params detect_params; 19 | bool show_cornres = false; 20 | } DetectParams; 21 | 22 | typedef struct Params { 23 | double pattern_size; 24 | int num_boards; 25 | int pattern_type; 26 | DetectParams camera1; 27 | DetectParams camera2; 28 | 29 | Params() 30 | : pattern_size(24) 31 | , num_boards(1) 32 | , pattern_type(0){}; 33 | } Params; 34 | 35 | CALIB_DLL_LOCAL int get_config(const std::string& filename, Params& params); 36 | 37 | } // namespace calib 38 | 39 | #endif //CALIB_CONFIG_H 40 | -------------------------------------------------------------------------------- /include/libcalib/extract.h: -------------------------------------------------------------------------------- 1 | /** 2 | * Copyright 2019, ftdlyc 3 | * Licensed under the MIT license. 4 | */ 5 | 6 | #pragma once 7 | #ifndef CALIB_EXTRACT_H 8 | #define CALIB_EXTRACT_H 9 | 10 | #include 11 | 12 | #include "libcalib/config.h" 13 | 14 | namespace calib { 15 | 16 | CALIB_DLL_LOCAL std::vector extract_corners(const std::vector& images, 17 | std::vector>& image_points, 18 | std::vector>& world_points, 19 | Params& params); 20 | 21 | CALIB_DLL_LOCAL int extract_corners(const cv::Mat& images, 22 | std::vector& image_points, 23 | std::vector& world_points, 24 | Params& params); 25 | 26 | CALIB_DLL_LOCAL std::vector extract_corners_stereo(const std::vector& images_1, 27 | const std::vector& images_2, 28 | std::vector>& image_points_1, 29 | std::vector>& image_points_2, 30 | std::vector>& world_points, 31 | Params& params); 32 | 33 | CALIB_DLL_LOCAL int extract_corners_stereo(const cv::Mat& images_1, 34 | const cv::Mat& images_2, 35 | std::vector& image_points_1, 36 | std::vector& image_points_2, 37 | std::vector& world_points, 38 | Params& params); 39 | 40 | } // namespace calib 41 | 42 | #endif //CALIB_EXTRACT_H 43 | -------------------------------------------------------------------------------- /include/libcalib/fileio.h: -------------------------------------------------------------------------------- 1 | /** 2 | * Copyright 2019, ftdlyc 3 | * Licensed under the MIT license. 4 | */ 5 | 6 | #pragma once 7 | #ifndef CALIB_FILEIO_H 8 | #define CALIB_FILEIO_H 9 | 10 | #include 11 | #include 12 | 13 | #include "libcalib/calib_def.h" 14 | 15 | CALIB_DLL_DECL bool get_all_file(const char* path, const char* type, std::vector& files); 16 | 17 | #endif //CALIB_FILEIO_H 18 | -------------------------------------------------------------------------------- /include/libcalib/interface.h: -------------------------------------------------------------------------------- 1 | /** 2 | * Copyright 2019, ftdlyc 3 | * Licensed under the MIT license. 4 | */ 5 | 6 | #ifndef CALIB_INTERFACE_H 7 | #define CALIB_INTERFACE_H 8 | 9 | #include 10 | #include 11 | #include 12 | 13 | #include "libcalib/calib_def.h" 14 | 15 | namespace calib { 16 | 17 | CALIB_DLL_DECL int calibrate(const std::vector& images, 18 | const std::string& config_file, 19 | std::vector& K, 20 | std::vector& D, 21 | std::vector>& Rwc, 22 | std::vector>& Twc, 23 | std::vector& extract_res, 24 | double& error); 25 | 26 | CALIB_DLL_DECL int calibrate(const std::vector& images, 27 | const std::string& config_file, 28 | std::vector& K, 29 | std::vector& D, 30 | std::vector>& Rwc, 31 | std::vector>& Twc, 32 | std::vector& extract_res); 33 | 34 | CALIB_DLL_DECL int calibrate_stereo(const std::vector& images_1, 35 | const std::vector& images_2, 36 | const std::string& config_file, 37 | std::vector& K1, 38 | std::vector& K2, 39 | std::vector& D1, 40 | std::vector& D2, 41 | std::vector>& Rwc1, 42 | std::vector>& Rwc2, 43 | std::vector>& Twc1, 44 | std::vector>& Twc2, 45 | std::vector& R, 46 | std::vector& T, 47 | std::vector& extract_res, 48 | std::vector& error); 49 | 50 | CALIB_DLL_DECL int calibrate_stereo(const std::vector& images_1, 51 | const std::vector& images_2, 52 | const std::string& config_file, 53 | std::vector& K1, 54 | std::vector& K2, 55 | std::vector& D1, 56 | std::vector& D2, 57 | std::vector>& Rwc1, 58 | std::vector>& Rwc2, 59 | std::vector>& Twc1, 60 | std::vector>& Twc2, 61 | std::vector& R, 62 | std::vector& T, 63 | std::vector& extract_res); 64 | 65 | } // namespace calib 66 | 67 | #endif //CALIB_INTERFACE_H 68 | -------------------------------------------------------------------------------- /sample/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | include_directories(${PROJECT_SOURCE_DIR}/include ${OpenCV_INCLUDE_DIRS}) 2 | link_directories(${PROJECT_SOURCE_DIR}/lib) 3 | 4 | add_executable(calibrate_single_camera calibrate_single_camera.cc) 5 | add_executable(calibrate_stereo_camera calibrate_stereo_camera.cc) 6 | 7 | target_link_libraries(calibrate_single_camera calib ${OpenCV_LIBS}) 8 | target_link_libraries(calibrate_stereo_camera calib ${OpenCV_LIBS}) 9 | -------------------------------------------------------------------------------- /sample/calibrate_single_camera.cc: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include 4 | #include 5 | using namespace std::chrono; 6 | 7 | #include "libcalib/fileio.h" 8 | #include "libcalib/interface.h" 9 | 10 | int main() { 11 | std::vector K; 12 | std::vector D; 13 | std::vector> Rwc; 14 | std::vector> Twc; 15 | std::vector extract_res; 16 | 17 | std::vector images; 18 | std::vector files; 19 | get_all_file("../example/full_match/left", ".*png", files); 20 | for(int i = 0; i < files.size(); ++i) { 21 | cv::Mat image = cv::imread(files[i].c_str(), cv::IMREAD_GRAYSCALE); 22 | images.emplace_back(image); 23 | } 24 | 25 | auto t1 = high_resolution_clock::now(); 26 | double err; 27 | calib::calibrate(images, "../example/full_match/config_template.json", K, D, Rwc, Twc, extract_res, err); 28 | auto t2 = high_resolution_clock::now(); 29 | printf("Took: %.3f ms\n", duration_cast(t2 - t1).count() / 1000.0); 30 | 31 | printf("err = %lf\n", err); 32 | printf("%f %f %f %f %f\n", K[0], K[1], K[2], K[3], K[4]); 33 | printf("%f %f %f %f %f\n", D[0], D[1], D[2], D[3], D[4]); 34 | 35 | return 0; 36 | } 37 | -------------------------------------------------------------------------------- /sample/calibrate_stereo_camera.cc: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include 4 | #include 5 | using namespace std::chrono; 6 | 7 | #include "libcalib/fileio.h" 8 | #include "libcalib/interface.h" 9 | 10 | int main() { 11 | std::vector K1, K2, D1, D2; 12 | std::vector> Rwc1, Rwc2, Twc1, Twc2; 13 | std::vector R, T; 14 | std::vector extract_res; 15 | 16 | std::vector images_1, images_2; 17 | std::vector files_1, files_2; 18 | get_all_file("../example/full_match/left", ".*png", files_1); 19 | get_all_file("../example/full_match/right", ".*png", files_2); 20 | for(int i = 0; i < files_1.size(); ++i) { 21 | cv::Mat image = cv::imread(files_1[i].c_str(), cv::IMREAD_GRAYSCALE); 22 | images_1.emplace_back(image); 23 | } 24 | for(int i = 0; i < files_2.size(); ++i) { 25 | cv::Mat image = cv::imread(files_2[i].c_str(), cv::IMREAD_GRAYSCALE); 26 | images_2.emplace_back(image); 27 | } 28 | 29 | auto t1 = high_resolution_clock::now(); 30 | std::vector err; 31 | calib::calibrate_stereo(images_1, images_2, "../example/full_match/config_template.json", K1, K2, D1, D2, Rwc1, Rwc2, Twc1, Twc2, R, T, extract_res, err); 32 | auto t2 = high_resolution_clock::now(); 33 | printf("Took: %.3f ms\n", duration_cast(t2 - t1).count() / 1000.0); 34 | 35 | printf("err = %lf, err = %lf, err = %lf\n", err[0], err[1], err[2]); 36 | printf("K1: %f %f %f %f %f\n", K1[0], K1[1], K1[2], K1[3], K1[4]); 37 | printf("D1: %f %f %f %f %f\n", D1[0], D1[1], D1[2], D1[3], D1[4]); 38 | printf("K2: %f %f %f %f %f\n", K2[0], K2[1], K2[2], K2[3], K2[4]); 39 | printf("D2: %f %f %f %f %f\n", D2[0], D2[1], D2[2], D2[3], D2[4]); 40 | printf("R: %f %f %f\n", R[0], R[1], R[2]); 41 | printf("T: %f %f %f\n", T[0], T[1], T[2]); 42 | 43 | cv::Mat rotation; 44 | cv::Mat angle_axis = (cv::Mat_(3, 1) << R[0], R[1], R[2]); 45 | cv::Rodrigues(angle_axis, rotation); 46 | std::cout << rotation << "\n"; 47 | 48 | return 0; 49 | } 50 | -------------------------------------------------------------------------------- /src/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | set(THIRD_PARTY_INCLUDE_DIRS 2 | ${CERES_INCLUDE_DIRS} 3 | ${EIGEN3_INCLUDE_DIR} 4 | ${OpenCV_INCLUDE_DIRS}) 5 | set(THIRD_PARTY_LIBS 6 | ${CERES_LIBRARIES} 7 | ${OpenCV_LIBS}) 8 | 9 | add_definitions("-DCALIB_DLL_EXPORT") 10 | include_directories(${PROJECT_SOURCE_DIR}/include 11 | ${THIRD_PARTY_INCLUDE_DIRS}) 12 | link_directories(${PROJECT_SOURCE_DIR}/lib) 13 | 14 | aux_source_directory(. LIBCALIB_SOURCE_LIST) 15 | add_library(calib ${LIB_TYPE} ${LIBCALIB_SOURCE_LIST}) 16 | 17 | target_link_libraries(calib cbdetect ${THIRD_PARTY_LIBS}) 18 | 19 | -------------------------------------------------------------------------------- /src/calibration.cc: -------------------------------------------------------------------------------- 1 | /** 2 | * Copyright 2019, ftdlyc 3 | * Licensed under the MIT license. 4 | */ 5 | 6 | #include 7 | #include 8 | #include 9 | 10 | #include 11 | #include 12 | 13 | #include 14 | #include 15 | #include 16 | 17 | #include 18 | #include 19 | 20 | #include "libcalib/calibration.h" 21 | #include "libcalib/ceres_type.h" 22 | 23 | namespace calib { 24 | 25 | void init_camera_params(const std::vector>& world_points, 26 | const std::vector>& image_points, 27 | std::vector& K, 28 | std::vector& D, 29 | std::vector>& Rwc, 30 | std::vector>& Twc) { 31 | int num_images = world_points.size(); 32 | int num_points = 0; 33 | for(int i = 0; i < num_images; ++i) { 34 | num_points += world_points[i].size(); 35 | } 36 | 37 | std::vector homo_vec; 38 | Eigen::MatrixXd mat_v = Eigen::MatrixXd::Zero(2 * num_images, 6); 39 | for(int i = 0; i < num_images; ++i) { 40 | cv::Mat homo = cv::findHomography(world_points[i], image_points[i], cv::RANSAC); 41 | auto homo_at = [&homo](int x, int y) { return homo.at(y - 1, x - 1); }; 42 | mat_v.block(2 * i, 0, 1, 6) << homo_at(1, 1) * homo_at(2, 1), 43 | homo_at(1, 1) * homo_at(2, 2) + homo_at(1, 2) * homo_at(2, 1), 44 | homo_at(1, 2) * homo_at(2, 2), 45 | homo_at(1, 3) * homo_at(2, 1) + homo_at(1, 1) * homo_at(2, 3), 46 | homo_at(1, 3) * homo_at(2, 2) + homo_at(1, 2) * homo_at(2, 3), 47 | homo_at(1, 3) * homo_at(2, 3); 48 | mat_v.block(2 * i + 1, 0, 1, 6) << homo_at(1, 1) * homo_at(1, 1) - homo_at(2, 1) * homo_at(2, 1), 49 | homo_at(1, 1) * homo_at(1, 2) + homo_at(1, 2) * homo_at(1, 1) - 50 | homo_at(2, 1) * homo_at(2, 2) - homo_at(2, 2) * homo_at(2, 1), 51 | homo_at(1, 2) * homo_at(1, 2) - homo_at(2, 2) * homo_at(2, 2), 52 | homo_at(1, 3) * homo_at(1, 1) + homo_at(1, 1) * homo_at(1, 3) - 53 | homo_at(2, 3) * homo_at(2, 1) - homo_at(2, 1) * homo_at(2, 3), 54 | homo_at(1, 3) * homo_at(1, 2) + homo_at(1, 2) * homo_at(1, 3) - 55 | homo_at(2, 3) * homo_at(2, 2) - homo_at(2, 2) * homo_at(2, 3), 56 | homo_at(1, 3) * homo_at(1, 3) - homo_at(2, 3) * homo_at(2, 3); 57 | homo_vec.emplace_back(homo); 58 | } 59 | 60 | Eigen::JacobiSVD svd(mat_v.transpose() * mat_v, Eigen::ComputeFullU | Eigen::ComputeFullV); 61 | Eigen::Matrix mat_b = svd.matrixV().cast(); 62 | double b11 = mat_b(0, 5); 63 | double b12 = mat_b(1, 5); 64 | double b22 = mat_b(2, 5); 65 | double b13 = mat_b(3, 5); 66 | double b23 = mat_b(4, 5); 67 | double b33 = mat_b(5, 5); 68 | double cy = (b12 * b13 - b11 * b23) / (b11 * b22 - b12 * b12); 69 | double lambda = b33 - (b13 * b13 + cy * (b12 * b13 - b11 * b23)) / b11; 70 | double fx = sqrt(lambda / b11); 71 | double fy = sqrt(lambda * b11 / (b11 * b22 - b12 * b12)); 72 | double radio = -b12 * fx * fx * fy / lambda; 73 | double cx = radio * cy / fx - b13 * fx * fx / lambda; 74 | K[0] = fx; 75 | K[1] = fy; 76 | K[2] = 0.; 77 | K[3] = cx; 78 | K[4] = cy; 79 | Eigen::Matrix3d K_eigen; 80 | K_eigen << K[0], K[2], K[3], 81 | 0, K[1], K[4], 82 | 0, 0, 1; 83 | 84 | for(int i = 0; i < num_images; ++i) { 85 | Eigen::Matrix3d rotation_eigen; 86 | Eigen::Matrix translation_eigen; 87 | Eigen::Matrix3d homo_eigen; 88 | cv::cv2eigen(homo_vec[i], homo_eigen); 89 | double s = (K_eigen.inverse() * homo_eigen.col(0)).norm(); 90 | rotation_eigen.col(0) = K_eigen.inverse() * homo_eigen.col(0) / s; 91 | rotation_eigen.col(1) = K_eigen.inverse() * homo_eigen.col(1) / s; 92 | rotation_eigen.col(2) = rotation_eigen.col(0).cross(rotation_eigen.col(1)); 93 | translation_eigen = K_eigen.inverse() * homo_eigen.col(2) / s; 94 | 95 | cv::Mat rotation; 96 | cv::Mat angle_axis; 97 | cv::eigen2cv(rotation_eigen, rotation); 98 | cv::Rodrigues(rotation, angle_axis); 99 | 100 | Rwc[i][0] = angle_axis.at(0); 101 | Rwc[i][1] = angle_axis.at(1); 102 | Rwc[i][2] = angle_axis.at(2); 103 | Twc[i][0] = translation_eigen(0, 0); 104 | Twc[i][1] = translation_eigen(1, 0); 105 | Twc[i][2] = translation_eigen(2, 0); 106 | } 107 | 108 | D[0] = 0.; 109 | D[1] = 0.; 110 | D[2] = 0.; 111 | D[3] = 0.; 112 | D[4] = 0.; 113 | } 114 | 115 | double optimize_params(const std::vector>& world_points, 116 | const std::vector>& image_points, 117 | std::vector& K, 118 | std::vector& D, 119 | std::vector>& Rwc, 120 | std::vector>& Twc) { 121 | std::vector intrinsics = {K[0], K[1], K[3], K[4], D[0], D[1], D[2], D[3], D[4]}; 122 | std::vector> se3(world_points.size(), std::vector(6)); 123 | for(int i = 0; i < world_points.size(); ++i) { 124 | se3[i][0] = Twc[i][0]; 125 | se3[i][1] = Twc[i][1]; 126 | se3[i][2] = Twc[i][2]; 127 | se3[i][3] = Rwc[i][0]; 128 | se3[i][4] = Rwc[i][1]; 129 | se3[i][5] = Rwc[i][2]; 130 | } 131 | 132 | ceres::Problem problem; 133 | ceres::HuberLoss* loss_fuction = new ceres::HuberLoss(4.5); 134 | for(int i = 0; i < world_points.size(); ++i) { 135 | for(int j = 0; j < world_points[i].size(); ++j) { 136 | problem.AddResidualBlock(new ProjectCostFunction(world_points[i][j], image_points[i][j]), nullptr, intrinsics.data(), se3[i].data()); 137 | } 138 | problem.SetParameterization(se3[i].data(), new SE3Parameterization()); 139 | } 140 | 141 | ceres::Solver::Options options; 142 | options.linear_solver_type = ceres::DENSE_QR; 143 | options.minimizer_progress_to_stdout = false; 144 | ceres::Solver::Summary summary; 145 | ceres::Solve(options, &problem, &summary); 146 | 147 | K = {intrinsics[0], intrinsics[1], 0., intrinsics[2], intrinsics[3]}; 148 | D = {intrinsics[4], intrinsics[5], intrinsics[6], intrinsics[7], intrinsics[8]}; 149 | for(int i = 0; i < world_points.size(); ++i) { 150 | Twc[i][0] = se3[i][0]; 151 | Twc[i][1] = se3[i][1]; 152 | Twc[i][2] = se3[i][2]; 153 | Rwc[i][0] = se3[i][3]; 154 | Rwc[i][1] = se3[i][4]; 155 | Rwc[i][2] = se3[i][5]; 156 | } 157 | 158 | double err = sqrt(2 * summary.final_cost / summary.num_residual_blocks); 159 | return err; 160 | } 161 | 162 | double calibrate_camera(const std::vector>& world_points, 163 | const std::vector>& image_points, 164 | std::vector& K, 165 | std::vector& D, 166 | std::vector>& Rwc, 167 | std::vector>& Twc) { 168 | init_camera_params(world_points, image_points, K, D, Rwc, Twc); 169 | double err = optimize_params(world_points, image_points, K, D, Rwc, Twc); 170 | return err; 171 | } 172 | 173 | void init_stereo_params(std::vector>& Rwc1, 174 | std::vector>& Rwc2, 175 | std::vector>& Twc1, 176 | std::vector>& Twc2, 177 | std::vector& R, 178 | std::vector& T) { 179 | auto num_images = static_cast(Rwc1.size()); 180 | Eigen::MatrixXd mat_a = Eigen::MatrixXd::Zero(9 * num_images, 9); 181 | Eigen::MatrixXd mat_b = Eigen::MatrixXd::Zero(9 * num_images, 1); 182 | for(int i = 0; i < num_images; ++i) { 183 | Eigen::Matrix3d r1_t, r2; 184 | ceres::AngleAxisToRotationMatrix(Rwc1[i].data(), r1_t.data()); 185 | ceres::AngleAxisToRotationMatrix(Rwc2[i].data(), r2.data()); 186 | r1_t.transposeInPlace(); 187 | mat_a.block(9 * i, 0, 3, 3) = r1_t; 188 | mat_a.block(9 * i + 3, 3, 3, 3) = r1_t; 189 | mat_a.block(9 * i + 6, 6, 3, 3) = r1_t; 190 | mat_b.block(9 * i, 0, 9, 1) << r2(0, 0), r2(0, 1), r2(0, 2), 191 | r2(1, 0), r2(1, 1), r2(1, 2), 192 | r2(2, 0), r2(2, 1), r2(2, 2); 193 | } 194 | Eigen::MatrixXd r = (mat_a.transpose() * mat_a).inverse() * mat_a.transpose() * mat_b; 195 | r.resize(3, 3); 196 | r.transposeInPlace(); 197 | 198 | T[0] = T[1] = T[2] = 0.; 199 | for(int i = 0; i < num_images; ++i) { 200 | Eigen::Matrix t, t1(Twc1[i].data()), t2(Twc2[i].data()); 201 | t = t2 - r * t1; 202 | T[0] += t(0, 0); 203 | T[1] += t(1, 0); 204 | T[2] += t(2, 0); 205 | } 206 | T[0] /= num_images; 207 | T[1] /= num_images; 208 | T[2] /= num_images; 209 | ceres::RotationMatrixToAngleAxis(r.data(), R.data()); 210 | } 211 | 212 | double optimize_stereo_params(const std::vector>& world_points, 213 | const std::vector>& image_points_1, 214 | const std::vector>& image_points_2, 215 | std::vector& K1, 216 | std::vector& K2, 217 | std::vector& D1, 218 | std::vector& D2, 219 | std::vector>& Rwc1, 220 | std::vector>& Rwc2, 221 | std::vector>& Twc1, 222 | std::vector>& Twc2, 223 | std::vector& R, 224 | std::vector& T) { 225 | std::vector intrinsics_1 = {K1[0], K1[1], K1[3], K1[4], D1[0], D1[1], D1[2], D1[3], D1[4]}; 226 | std::vector intrinsics_2 = {K2[0], K2[1], K2[3], K2[4], D2[0], D2[1], D2[2], D2[3], D2[4]}; 227 | std::vector> se3_1(world_points.size(), std::vector(6)); 228 | std::vector se3 = {T[0], T[1], T[2], R[0], R[1], R[2]}; 229 | for(int i = 0; i < world_points.size(); ++i) { 230 | se3_1[i][0] = Twc1[i][0]; 231 | se3_1[i][1] = Twc1[i][1]; 232 | se3_1[i][2] = Twc1[i][2]; 233 | se3_1[i][3] = Rwc1[i][0]; 234 | se3_1[i][4] = Rwc1[i][1]; 235 | se3_1[i][5] = Rwc1[i][2]; 236 | } 237 | 238 | ceres::Problem problem; 239 | ceres::HuberLoss* loss_fuction = new ceres::HuberLoss(4.5); 240 | for(int i = 0; i < world_points.size(); ++i) { 241 | for(int j = 0; j < world_points[i].size(); ++j) { 242 | problem.AddResidualBlock(new StereoCostFunction(world_points[i][j], image_points_1[i][j], image_points_2[i][j]), nullptr, 243 | intrinsics_1.data(), intrinsics_2.data(), se3_1[i].data(), se3.data()); 244 | } 245 | problem.SetParameterization(se3_1[i].data(), new SE3Parameterization()); 246 | } 247 | problem.SetParameterization(se3.data(), new SE3Parameterization()); 248 | 249 | ceres::Solver::Options options; 250 | options.linear_solver_type = ceres::DENSE_QR; 251 | options.trust_region_strategy_type = ceres::DOGLEG; 252 | options.minimizer_progress_to_stdout = false; 253 | ceres::Solver::Summary summary; 254 | ceres::Solve(options, &problem, &summary); 255 | 256 | K1 = {intrinsics_1[0], intrinsics_1[1], 0., intrinsics_1[2], intrinsics_1[3]}; 257 | D1 = {intrinsics_1[4], intrinsics_1[5], intrinsics_1[6], intrinsics_1[7], intrinsics_1[8]}; 258 | K2 = {intrinsics_2[0], intrinsics_2[1], 0., intrinsics_2[2], intrinsics_2[3]}; 259 | D2 = {intrinsics_2[4], intrinsics_2[5], intrinsics_2[6], intrinsics_2[7], intrinsics_2[8]}; 260 | R = {se3[3], se3[4], se3[5]}; 261 | T = {se3[0], se3[1], se3[2]}; 262 | 263 | double q[4], q1[4], q2[4]; 264 | ceres::AngleAxisToQuaternion(R.data(), q); 265 | for(int i = 0; i < world_points.size(); ++i) { 266 | Twc1[i][0] = se3_1[i][0]; 267 | Twc1[i][1] = se3_1[i][1]; 268 | Twc1[i][2] = se3_1[i][2]; 269 | Rwc1[i][0] = se3_1[i][3]; 270 | Rwc1[i][1] = se3_1[i][4]; 271 | Rwc1[i][2] = se3_1[i][5]; 272 | 273 | ceres::AngleAxisToQuaternion(Rwc1[i].data(), q1); 274 | ceres::QuaternionProduct(q, q1, q2); 275 | ceres::QuaternionToAngleAxis(q2, Rwc2[i].data()); 276 | ceres::AngleAxisRotatePoint(R.data(), Twc1[i].data(), Twc2[i].data()); 277 | Twc2[i][0] += T[0]; 278 | Twc2[i][1] += T[1]; 279 | Twc2[i][2] += T[2]; 280 | } 281 | 282 | double err = sqrt(2 * summary.final_cost / summary.num_residual_blocks / 2); 283 | return err; 284 | } 285 | 286 | double calibrate_stereo_camera(const std::vector>& world_points, 287 | const std::vector>& image_points_1, 288 | const std::vector>& image_points_2, 289 | std::vector& K1, 290 | std::vector& K2, 291 | std::vector& D1, 292 | std::vector& D2, 293 | std::vector>& Rwc1, 294 | std::vector>& Rwc2, 295 | std::vector>& Twc1, 296 | std::vector>& Twc2, 297 | std::vector& R, 298 | std::vector& T) { 299 | init_stereo_params(Rwc1, Rwc2, Twc1, Twc2, R, T); 300 | double err = optimize_stereo_params(world_points, image_points_1, image_points_2, K1, K2, D1, D2, Rwc1, Rwc2, Twc1, Twc2, R, T); 301 | return err; 302 | } 303 | 304 | } // namespace calib 305 | -------------------------------------------------------------------------------- /src/ceres_type.cc: -------------------------------------------------------------------------------- 1 | /** 2 | * Copyright 2019, ftdlyc 3 | * Licensed under the MIT license. 4 | */ 5 | 6 | #include 7 | 8 | #include 9 | #include 10 | 11 | #include 12 | 13 | #include "libcalib/ceres_type.h" 14 | 15 | namespace calib { 16 | 17 | bool SE3Parameterization::Plus(const double* x, 18 | const double* delta, 19 | double* x_plus_delta) const { 20 | double q[4]; 21 | double q_delta[4]; 22 | double q_plus_delta[4]; 23 | ceres::AngleAxisToQuaternion(x + 3, q); 24 | ceres::AngleAxisToQuaternion(delta + 3, q_delta); 25 | ceres::QuaternionProduct(q_delta, q, q_plus_delta); 26 | ceres::QuaternionToAngleAxis(q_plus_delta, x_plus_delta + 3); 27 | 28 | double t[3]; 29 | ceres::AngleAxisRotatePoint(delta + 3, x, t); 30 | 31 | x_plus_delta[0] = t[0] + delta[0]; 32 | x_plus_delta[1] = t[1] + delta[1]; 33 | x_plus_delta[2] = t[2] + delta[2]; 34 | 35 | return true; 36 | } 37 | 38 | bool SE3Parameterization::ComputeJacobian(const double* x, double* jacobian) const { 39 | Eigen::Map>(jacobian, 6, 6) = Eigen::Matrix::Identity(); 40 | return true; 41 | } 42 | 43 | bool SE3Parameterization::MultiplyByJacobian(const double* x, 44 | const int num_rows, 45 | const double* global_matrix, 46 | double* local_matrix) const { 47 | std::copy(global_matrix, global_matrix + num_rows * GlobalSize(), local_matrix); 48 | return true; 49 | } 50 | 51 | int SE3Parameterization::GlobalSize() const { 52 | return 6; 53 | } 54 | 55 | int SE3Parameterization::LocalSize() const { 56 | return 6; 57 | } 58 | 59 | ProjectCostFunction::ProjectCostFunction(const cv::Point2d& world_point, 60 | const cv::Point2d& image_point, 61 | bool with_k3) 62 | : world_point_{world_point.x, world_point.y, 0.} 63 | , image_point_{image_point.x, image_point.y} 64 | , with_k3_(with_k3) { 65 | set_num_residuals(2); 66 | auto params = mutable_parameter_block_sizes(); 67 | params->emplace_back(9); // fx, fy, cx, cy, k1, k2, p1, p2, k3 68 | params->emplace_back(6); // se3(t, phi) 69 | } 70 | 71 | bool ProjectCostFunction::Evaluate(double const* const* parameters, 72 | double* residuals, 73 | double** jacobians) const { 74 | const double& fx = parameters[0][0]; 75 | const double& fy = parameters[0][1]; 76 | const double& cx = parameters[0][2]; 77 | const double& cy = parameters[0][3]; 78 | const double& k1 = parameters[0][4]; 79 | const double& k2 = parameters[0][5]; 80 | const double& p1 = parameters[0][6]; 81 | const double& p2 = parameters[0][7]; 82 | const double& k3 = parameters[0][8]; 83 | const double* phi = parameters[1] + 3; 84 | const double* t = parameters[1]; 85 | 86 | // project 87 | double pc[3]; 88 | ceres::AngleAxisRotatePoint(phi, world_point_, pc); 89 | double x = pc[0] + t[0]; 90 | double y = pc[1] + t[1]; 91 | double z = pc[2] + t[2]; 92 | 93 | if(abs(z) < 1e-3) return false; 94 | 95 | double xp = x / z; 96 | double yp = y / z; 97 | 98 | double xp2 = xp * xp; 99 | double yp2 = yp * yp; 100 | double xyp = xp * yp; 101 | double r2 = xp2 + yp2; 102 | 103 | double txp = (1 + r2 * (k1 + r2 * (k2 + r2 * k3))) * xp + 2. * p1 * xyp + p2 * (r2 + 2. * xp2); 104 | double typ = (1 + r2 * (k1 + r2 * (k2 + r2 * k3))) * yp + p1 * (r2 + 2. * yp2) + 2. * p2 * xyp; 105 | double i = fx * txp + cx; 106 | double j = fy * typ + cy; 107 | 108 | residuals[0] = i - image_point_[0]; 109 | residuals[1] = j - image_point_[1]; 110 | 111 | if(jacobians == NULL) return true; 112 | 113 | // jacobians 114 | double di_dtxp = fx; // di / dtxp 115 | double dj_dtyp = fy; // dj / dtyp 116 | 117 | if(jacobians[0] != NULL) { 118 | jacobians[0][0] = txp; // di / dfx 119 | jacobians[0][1] = 0.; // di / dfy 120 | jacobians[0][2] = 1.; // di / dcx 121 | jacobians[0][3] = 0.; // di / dcy 122 | jacobians[0][4] = di_dtxp * xp * r2; // di / dk1 123 | jacobians[0][5] = di_dtxp * xp * r2 * r2; // di / dk2 124 | jacobians[0][6] = di_dtxp * 2. * xyp; // di / dp1 125 | jacobians[0][7] = di_dtxp * (r2 + 2. * xp2); // di / dp2 126 | jacobians[0][8] = di_dtxp * xp * r2 * r2 * r2; // di / dk3 127 | jacobians[0][9] = 0.; // dj / dfx 128 | jacobians[0][10] = typ; // dj / dfy 129 | jacobians[0][11] = 0.; // dj / dcx 130 | jacobians[0][12] = 1.; // dj / dcy 131 | jacobians[0][13] = dj_dtyp * yp * r2; // dj / dk1 132 | jacobians[0][14] = dj_dtyp * yp * r2 * r2; // dj / dk2 133 | jacobians[0][15] = dj_dtyp * (r2 + 2. * yp2); // dj / dp1 134 | jacobians[0][16] = dj_dtyp * 2. * xyp; // dj / dp2 135 | jacobians[0][17] = dj_dtyp * yp * r2 * r2 * r2; // dj / dk3 136 | 137 | if(!with_k3_) { 138 | jacobians[0][8] = 0.; // di / dk3 139 | jacobians[0][17] = 0.; // dj / dk3 140 | } 141 | } 142 | 143 | double dtxp_dxp = 1 + k1 * (r2 + 2. * xp2) + k2 * r2 * (r2 + 4. * xp2) + 2. * p1 * yp + 6. * p2 * xp + k3 * r2 * r2 * (r2 + 6. * xp2); 144 | double dtyp_dyp = 1 + k1 * (r2 + 2. * yp2) + k2 * r2 * (r2 + 4. * yp2) + 6. * p1 * yp + 2. * p2 * xp + k3 * r2 * r2 * (r2 + 6. * yp2); 145 | double di_dxp = di_dtxp * dtxp_dxp; 146 | double dj_dyp = dj_dtyp * dtyp_dyp; 147 | 148 | double z2 = z * z; 149 | double di_dx = di_dxp / z; 150 | double di_dy = 0.; 151 | double di_dz = -di_dxp * x / z2; 152 | double dj_dx = 0.; 153 | double dj_dy = dj_dyp / z; 154 | double dj_dz = -dj_dyp * y / z2; 155 | 156 | if(jacobians[1] != NULL) { 157 | jacobians[1][0] = di_dx; // di / drho0 158 | jacobians[1][1] = di_dy; // di / drho1 159 | jacobians[1][2] = di_dz; // di / drho2 160 | jacobians[1][3] = y * di_dz; // di / dphi0 161 | jacobians[1][4] = di_dxp - x * di_dz; // di / dphi1 162 | jacobians[1][5] = -y * di_dx; // di / dphi2 163 | jacobians[1][6] = dj_dx; // dj / drho0 164 | jacobians[1][7] = dj_dy; // dj / drho1 165 | jacobians[1][8] = dj_dz; // dj / drho2 166 | jacobians[1][9] = -dj_dyp + y * dj_dz; // dj / dphi0 167 | jacobians[1][10] = -x * dj_dz; // dj / dphi1 168 | jacobians[1][11] = x * dj_dy; // dj / dphi2 169 | } 170 | 171 | return true; 172 | } 173 | 174 | StereoCostFunction::StereoCostFunction(const cv::Point2d& world_point, 175 | const cv::Point2d& image_point_1, 176 | const cv::Point2d& image_point_2, 177 | bool with_k3) 178 | : world_point_{world_point.x, world_point.y, 0.} 179 | , image_point_1_{image_point_1.x, image_point_1.y} 180 | , image_point_2_{image_point_2.x, image_point_2.y} 181 | , with_k3_(with_k3) { 182 | set_num_residuals(4); 183 | auto params = mutable_parameter_block_sizes(); 184 | params->emplace_back(9); // camera left fx, fy, cx, cy, k1, k2, p1, p2, k3 185 | params->emplace_back(9); // camera right fx, fy, cx, cy, k1, k2, p1, p2, k3 186 | params->emplace_back(6); // camera left se3(t, phi) 187 | params->emplace_back(6); // left to right se3(t, phi) 188 | } 189 | 190 | bool StereoCostFunction::Evaluate(double const* const* parameters, 191 | double* residuals, 192 | double** jacobians) const { 193 | const double& fx_1 = parameters[0][0]; 194 | const double& fy_1 = parameters[0][1]; 195 | const double& cx_1 = parameters[0][2]; 196 | const double& cy_1 = parameters[0][3]; 197 | const double& k1_1 = parameters[0][4]; 198 | const double& k2_1 = parameters[0][5]; 199 | const double& p1_1 = parameters[0][6]; 200 | const double& p2_1 = parameters[0][7]; 201 | const double& k3_1 = parameters[0][8]; 202 | const double& fx_2 = parameters[1][0]; 203 | const double& fy_2 = parameters[1][1]; 204 | const double& cx_2 = parameters[1][2]; 205 | const double& cy_2 = parameters[1][3]; 206 | const double& k1_2 = parameters[1][4]; 207 | const double& k2_2 = parameters[1][5]; 208 | const double& p1_2 = parameters[1][6]; 209 | const double& p2_2 = parameters[1][7]; 210 | const double& k3_2 = parameters[1][8]; 211 | const double* phi_1 = parameters[2] + 3; 212 | const double* t_1 = parameters[2]; 213 | const double* phi = parameters[3] + 3; 214 | const double* t = parameters[3]; 215 | 216 | // project 217 | double pc_1[3]; 218 | ceres::AngleAxisRotatePoint(phi_1, world_point_, pc_1); 219 | double x_1 = pc_1[0] + t_1[0]; 220 | double y_1 = pc_1[1] + t_1[1]; 221 | double z_1 = pc_1[2] + t_1[2]; 222 | 223 | double pc_1_tmp[3] = {x_1, y_1, z_1}; 224 | double pc_2[3]; 225 | ceres::AngleAxisRotatePoint(phi, pc_1_tmp, pc_2); 226 | double x_2 = pc_2[0] + t[0]; 227 | double y_2 = pc_2[1] + t[1]; 228 | double z_2 = pc_2[2] + t[2]; 229 | 230 | if(abs(z_1) < 1e-3 || abs(z_2) < 1e-3) return false; 231 | 232 | double xp_1 = x_1 / z_1; 233 | double yp_1 = y_1 / z_1; 234 | 235 | double xp2_1 = xp_1 * xp_1; 236 | double yp2_1 = yp_1 * yp_1; 237 | double xyp_1 = xp_1 * yp_1; 238 | double r2_1 = xp2_1 + yp2_1; 239 | 240 | double txp_1 = (1 + r2_1 * (k1_1 + r2_1 * (k2_1 + r2_1 * k3_1))) * xp_1 + 2. * p1_1 * xyp_1 + p2_1 * (r2_1 + 2. * xp2_1); 241 | double typ_1 = (1 + r2_1 * (k1_1 + r2_1 * (k2_1 + r2_1 * k3_1))) * yp_1 + p1_1 * (r2_1 + 2. * yp2_1) + 2. * p2_1 * xyp_1; 242 | double i_1 = fx_1 * txp_1 + cx_1; 243 | double j_1 = fy_1 * typ_1 + cy_1; 244 | 245 | double xp_2 = x_2 / z_2; 246 | double yp_2 = y_2 / z_2; 247 | 248 | double xp2_2 = xp_2 * xp_2; 249 | double yp2_2 = yp_2 * yp_2; 250 | double xyp_2 = xp_2 * yp_2; 251 | double r2_2 = xp2_2 + yp2_2; 252 | 253 | double txp_2 = (1 + r2_2 * (k1_2 + r2_2 * (k2_2 + r2_2 * k3_2))) * xp_2 + 2. * p1_2 * xyp_2 + p2_2 * (r2_2 + 2. * xp2_2); 254 | double typ_2 = (1 + r2_2 * (k1_2 + r2_2 * (k2_2 + r2_2 * k3_2))) * yp_2 + p1_2 * (r2_2 + 2. * yp2_2) + 2. * p2_2 * xyp_2; 255 | double i_2 = fx_2 * txp_2 + cx_2; 256 | double j_2 = fy_2 * typ_2 + cy_2; 257 | 258 | residuals[0] = i_1 - image_point_1_[0]; 259 | residuals[1] = j_1 - image_point_1_[1]; 260 | residuals[2] = i_2 - image_point_2_[0]; 261 | residuals[3] = j_2 - image_point_2_[1]; 262 | 263 | if(jacobians == NULL) return true; 264 | 265 | // jacobians 266 | double di_dtxp_1 = fx_1; // di_1 / dtxp_1 267 | double dj_dtyp_1 = fy_1; // dj_1 / dtyp_1 268 | double di_dtxp_2 = fx_2; // di_2 / dtxp_2 269 | double dj_dtyp_2 = fy_2; // dj_2 / dtyp_2 270 | 271 | if(jacobians[0] != NULL) { 272 | jacobians[0][0] = txp_1; // di_1 / dfx_1 273 | jacobians[0][1] = 0.; // di_1 / dfy_1 274 | jacobians[0][2] = 1.; // di_1 / dcx_1 275 | jacobians[0][3] = 0.; // di_1 / dcy_1 276 | jacobians[0][4] = di_dtxp_1 * xp_1 * r2_1; // di_1 / dk1_1 277 | jacobians[0][5] = di_dtxp_1 * xp_1 * r2_1 * r2_1; // di_1 / dk2_1 278 | jacobians[0][6] = di_dtxp_1 * 2. * xyp_1; // di_1 / dp1_1 279 | jacobians[0][7] = di_dtxp_1 * (r2_1 + 2. * xp2_1); // di_1 / dp2_1 280 | jacobians[0][8] = di_dtxp_1 * xp_1 * r2_1 * r2_1 * r2_1; // di_1 / dk3_1 281 | jacobians[0][9] = 0.; // dj_1 / dfx_1 282 | jacobians[0][10] = typ_1; // dj_1 / dfy_1 283 | jacobians[0][11] = 0.; // dj_1 / dcx_1 284 | jacobians[0][12] = 1.; // dj_1 / dcy_1 285 | jacobians[0][13] = dj_dtyp_1 * yp_1 * r2_1; // dj_1 / dk1_1 286 | jacobians[0][14] = dj_dtyp_1 * yp_1 * r2_1 * r2_1; // dj_1 / dk2_1 287 | jacobians[0][15] = dj_dtyp_1 * (r2_1 + 2. * yp2_1); // dj_1 / dp1_1 288 | jacobians[0][16] = dj_dtyp_1 * 2. * xyp_1; // dj_1 / dp2_1 289 | jacobians[0][17] = dj_dtyp_1 * yp_1 * r2_1 * r2_1 * r2_1; // dj_1 / dk3_1 290 | jacobians[0][18] = 0.; // di_2 / dfx_1 291 | jacobians[0][19] = 0.; // di_2 / dfy_1 292 | jacobians[0][20] = 0.; // di_2 / dcx_1 293 | jacobians[0][21] = 0.; // di_2 / dcy_1 294 | jacobians[0][22] = 0.; // di_2 / dk1_1 295 | jacobians[0][23] = 0.; // di_2 / dk2_1 296 | jacobians[0][24] = 0.; // di_2 / dp1_1 297 | jacobians[0][25] = 0.; // di_2 / dp2_1 298 | jacobians[0][26] = 0.; // di_2 / dk3_1 299 | jacobians[0][27] = 0.; // dj_2 / dfx_1 300 | jacobians[0][28] = 0.; // dj_2 / dfy_1 301 | jacobians[0][29] = 0.; // dj_2 / dcx_1 302 | jacobians[0][30] = 0.; // dj_2 / dcy_1 303 | jacobians[0][31] = 0.; // dj_2 / dk1_1 304 | jacobians[0][32] = 0.; // dj_2 / dk2_1 305 | jacobians[0][33] = 0.; // dj_2 / dp1_1 306 | jacobians[0][34] = 0.; // dj_2 / dp2_1 307 | jacobians[0][35] = 0.; // dj_2 / dk3_1 308 | 309 | if(!with_k3_) { 310 | jacobians[0][8] = 0.; // di_1 / dk3_1 311 | jacobians[0][17] = 0.; // dj_1 / dk3_1 312 | } 313 | } 314 | 315 | if(jacobians[1] != NULL) { 316 | jacobians[1][0] = 0.; // di_1 / dfx_2 317 | jacobians[1][1] = 0.; // di_1 / dfy_2 318 | jacobians[1][2] = 0.; // di_1 / dcx_2 319 | jacobians[1][3] = 0.; // di_1 / dcy_2 320 | jacobians[1][4] = 0.; // di_1 / dk1_2 321 | jacobians[1][5] = 0.; // di_1 / dk2_2 322 | jacobians[1][6] = 0.; // di_1 / dp1_2 323 | jacobians[1][7] = 0.; // di_1 / dp2_2 324 | jacobians[1][8] = 0.; // di_1 / dk3_2 325 | jacobians[1][9] = 0.; // dj_1 / dfx_2 326 | jacobians[1][10] = 0.; // dj_1 / dfy_2 327 | jacobians[1][11] = 0.; // dj_1 / dcx_2 328 | jacobians[1][12] = 0.; // dj_1 / dcy_2 329 | jacobians[1][13] = 0.; // dj_1 / dk1_2 330 | jacobians[1][14] = 0.; // dj_1 / dk2_2 331 | jacobians[1][15] = 0.; // dj_1 / dp1_2 332 | jacobians[1][16] = 0.; // dj_1 / dp2_2 333 | jacobians[1][17] = 0.; // dj_1 / dk3_2 334 | jacobians[1][18] = txp_2; // di_2 / dfx_2 335 | jacobians[1][19] = 0.; // di_2 / dfy_2 336 | jacobians[1][20] = 1.; // di_2 / dcx_2 337 | jacobians[1][21] = 0.; // di_2 / dcy_2 338 | jacobians[1][22] = di_dtxp_2 * xp_2 * r2_2; // di_2 / dk1_2 339 | jacobians[1][23] = di_dtxp_2 * xp_2 * r2_2 * r2_2; // di_2 / dk2_2 340 | jacobians[1][24] = di_dtxp_2 * 2. * xyp_2; // di_2 / dp1_2 341 | jacobians[1][25] = di_dtxp_2 * (r2_2 + 2. * xp2_2); // di_2 / dp2_2 342 | jacobians[1][26] = di_dtxp_2 * xp_2 * r2_2 * r2_2 * r2_2; // di_2 / dk3_2 343 | jacobians[1][27] = 0.; // dj_2 / dfx_2 344 | jacobians[1][28] = typ_2; // dj_2 / dfy_2 345 | jacobians[1][29] = 0.; // dj_2 / dcx_2 346 | jacobians[1][30] = 1.; // dj_2 / dcy_2 347 | jacobians[1][31] = dj_dtyp_2 * yp_2 * r2_2; // dj_2 / dk1_2 348 | jacobians[1][32] = dj_dtyp_2 * yp_2 * r2_2 * r2_2; // dj_2 / dk2_2 349 | jacobians[1][33] = dj_dtyp_2 * (r2_2 + 2. * yp2_2); // dj_2 / dp1_2 350 | jacobians[1][34] = dj_dtyp_2 * 2. * xyp_2; // dj_2 / dp2_2 351 | jacobians[1][35] = dj_dtyp_2 * yp_2 * r2_2 * r2_2 * r2_2; // dj_2 / dk3_2 352 | 353 | if(!with_k3_) { 354 | jacobians[1][26] = 0.; // di_2 / dk3_2 355 | jacobians[1][35] = 0.; // dj_2 / dk3_2 356 | } 357 | } 358 | 359 | double dtxp_dxp_1 = 1 + k1_1 * (r2_1 + 2. * xp2_1) + k2_1 * r2_1 * (r2_1 + 4. * xp2_1) + 2. * p1_1 * yp_1 + 6. * p2_1 * xp_1 + k3_1 * r2_1 * r2_1 * (r2_1 + 6. * xp2_1); 360 | double dtyp_dyp_1 = 1 + k1_1 * (r2_1 + 2. * yp2_1) + k2_1 * r2_1 * (r2_1 + 4. * yp2_1) + 6. * p1_1 * yp_1 + 2. * p2_1 * xp_1 + k3_1 * r2_1 * r2_1 * (r2_1 + 6. * yp2_1); 361 | double di_dxp_1 = di_dtxp_1 * dtxp_dxp_1; 362 | double dj_dyp_1 = dj_dtyp_1 * dtyp_dyp_1; 363 | 364 | double z2_1 = z_1 * z_1; 365 | double di_dx_1 = di_dxp_1 / z_1; 366 | double di_dy_1 = 0.; 367 | double di_dz_1 = -di_dxp_1 * x_1 / z2_1; 368 | double dj_dx_1 = 0.; 369 | double dj_dy_1 = dj_dyp_1 / z_1; 370 | double dj_dz_1 = -dj_dyp_1 * y_1 / z2_1; 371 | 372 | double dtxp_dxp_2 = 1 + k1_2 * (r2_2 + 2. * xp2_2) + k2_2 * r2_2 * (r2_2 + 4. * xp2_2) + 2. * p1_2 * yp_2 + 6. * p2_2 * xp_2 + k3_2 * r2_2 * r2_2 * (r2_2 + 6. * xp2_2); 373 | double dtyp_dyp_2 = 1 + k1_2 * (r2_2 + 2. * yp2_2) + k2_2 * r2_2 * (r2_2 + 4. * yp2_2) + 6. * p1_2 * yp_2 + 2. * p2_2 * xp_2 + k3_2 * r2_2 * r2_2 * (r2_2 + 6. * yp2_2); 374 | double di_dxp_2 = di_dtxp_2 * dtxp_dxp_2; 375 | double dj_dyp_2 = dj_dtyp_2 * dtyp_dyp_2; 376 | 377 | double z2_2 = z_2 * z_2; 378 | double di_dx_2 = di_dxp_2 / z_2; 379 | double di_dy_2 = 0.; 380 | double di_dz_2 = -di_dxp_2 * x_2 / z2_2; 381 | double dj_dx_2 = 0.; 382 | double dj_dy_2 = dj_dyp_2 / z_2; 383 | double dj_dz_2 = -dj_dyp_2 * y_2 / z2_2; 384 | 385 | double r[9]; // col major 386 | ceres::AngleAxisToRotationMatrix(phi, r); 387 | double di_2_dx_1 = di_dx_2 * r[0] + di_dy_2 * r[1] + di_dz_2 * r[2]; 388 | double di_2_dy_1 = di_dx_2 * r[3] + di_dy_2 * r[4] + di_dz_2 * r[5]; 389 | double di_2_dz_1 = di_dx_2 * r[6] + di_dy_2 * r[7] + di_dz_2 * r[8]; 390 | double dj_2_dx_1 = dj_dx_2 * r[0] + dj_dy_2 * r[1] + dj_dz_2 * r[2]; 391 | double dj_2_dy_1 = dj_dx_2 * r[3] + dj_dy_2 * r[4] + dj_dz_2 * r[4]; 392 | double dj_2_dz_1 = dj_dx_2 * r[6] + dj_dy_2 * r[7] + dj_dz_2 * r[8]; 393 | 394 | if(jacobians[2] != NULL) { 395 | jacobians[2][0] = di_dx_1; // di_1 / drho0_1 396 | jacobians[2][1] = di_dy_1; // di_1 / drho1_1 397 | jacobians[2][2] = di_dz_1; // di_1 / drho2_1 398 | jacobians[2][3] = y_1 * di_dz_1; // di_1 / dphi0_1 399 | jacobians[2][4] = di_dxp_1 - x_1 * di_dz_1; // di_1 / dphi1_1 400 | jacobians[2][5] = -y_1 * di_dx_1; // di_1 / dphi2_1 401 | jacobians[2][6] = dj_dx_1; // dj_1 / drho0_1 402 | jacobians[2][7] = dj_dy_1; // dj_1 / drho1_1 403 | jacobians[2][8] = dj_dz_1; // dj_1 / drho2_1 404 | jacobians[2][9] = -dj_dyp_1 + y_1 * dj_dz_1; // dj_1 / dphi0_1 405 | jacobians[2][10] = -x_1 * dj_dz_1; // dj_1 / dphi1_1 406 | jacobians[2][11] = x_1 * dj_dy_1; // dj_1 / dphi2_1 407 | jacobians[2][12] = di_2_dx_1; // di_2 / drho0_1 408 | jacobians[2][13] = di_2_dy_1; // di_2 / drho1_1 409 | jacobians[2][14] = di_2_dz_1; // di_2 / drho2_1 410 | jacobians[2][15] = -z_1 * di_2_dy_1 + y_1 * di_2_dz_1; // di_2 / dphi0_1 411 | jacobians[2][16] = z_1 * di_2_dx_1 - x_1 * di_2_dz_1; // di_2 / dphi1_1 412 | jacobians[2][17] = -y_1 * di_2_dx_1 + x_1 * di_2_dy_1; // di_2 / dphi2_1 413 | jacobians[2][18] = dj_2_dx_1; // dj_2 / drho0_1 414 | jacobians[2][19] = dj_2_dy_1; // dj_2 / drho1_1 415 | jacobians[2][20] = dj_2_dz_1; // dj_2 / drho2_1 416 | jacobians[2][21] = -z_1 * dj_2_dy_1 + y_1 * dj_2_dz_1; // dj_2 / dphi0_1 417 | jacobians[2][22] = z_1 * dj_2_dx_1 - x_1 * dj_2_dz_1; // dj_2 / dphi1_1 418 | jacobians[2][23] = -y_1 * dj_2_dx_1 + x_1 * dj_2_dy_1; // dj_2 / dphi2_1 419 | } 420 | 421 | if(jacobians[3] != NULL) { 422 | jacobians[3][0] = 0.; // di_1 / drho0 423 | jacobians[3][1] = 0.; // di_1 / drho1 424 | jacobians[3][2] = 0.; // di_1 / drho2 425 | jacobians[3][3] = 0.; // di_1 / dphi0 426 | jacobians[3][4] = 0.; // di_1 / dphi1 427 | jacobians[3][5] = 0.; // di_1 / dphi2 428 | jacobians[3][6] = 0.; // dj_1 / drho0 429 | jacobians[3][7] = 0.; // dj_1 / drho1 430 | jacobians[3][8] = 0.; // dj_1 / drho2 431 | jacobians[3][9] = 0.; // dj_1 / dphi0 432 | jacobians[3][10] = 0.; // dj_1 / dphi1 433 | jacobians[3][11] = 0.; // dj_1 / dphi2 434 | jacobians[3][12] = di_dx_2; // di_2 / drho0 435 | jacobians[3][13] = di_dy_2; // di_2 / drho1 436 | jacobians[3][14] = di_dz_2; // di_2 / drho2 437 | jacobians[3][15] = y_2 * di_dz_2; // di_2 / dphi0 438 | jacobians[3][16] = di_dxp_2 - x_2 * di_dz_2; // di_2 / dphi1 439 | jacobians[3][17] = -y_2 * di_dx_2; // di_2 / dphi2 440 | jacobians[3][18] = dj_dx_2; // dj_2 / drho0 441 | jacobians[3][19] = dj_dy_2; // dj_2 / drho1 442 | jacobians[3][20] = dj_dz_2; // dj_2 / drho2 443 | jacobians[3][21] = -dj_dyp_2 + y_2 * dj_dz_2; // dj_2 / dphi0 444 | jacobians[3][22] = -x_2 * dj_dz_2; // dj_2 / dphi1 445 | jacobians[3][23] = x_2 * dj_dy_2; // dj_2 / dphi2 446 | } 447 | 448 | return true; 449 | } 450 | 451 | } // namespace calib -------------------------------------------------------------------------------- /src/config.cc: -------------------------------------------------------------------------------- 1 | /** 2 | * Copyright 2019, ftdlyc 3 | * Licensed under the MIT license. 4 | */ 5 | 6 | #include 7 | #include 8 | 9 | #include "rapidjson/document.h" 10 | #include "rapidjson/istreamwrapper.h" 11 | 12 | #include "libcalib/config.h" 13 | #include "libcbdetect/config.h" 14 | 15 | namespace calib { 16 | 17 | int get_config(const std::string& filename, Params& params) { 18 | std::ifstream ifs(filename); 19 | rapidjson::IStreamWrapper isw(ifs); 20 | rapidjson::Document dom; 21 | dom.ParseStream(isw); 22 | ifs.close(); 23 | if(dom.HasParseError()) { 24 | return -1; 25 | } 26 | 27 | params.pattern_type = dom["pattern_type"].GetInt(); 28 | params.pattern_size = dom["pattern_size"].GetDouble(); 29 | params.num_boards = dom["num_boards"].GetInt(); 30 | 31 | if(dom.HasMember("camera1")) { 32 | auto dom1 = dom["camera1"].GetObject(); 33 | cbdetect::Params& detect_params = params.camera1.detect_params; 34 | detect_params.detect_method = static_cast(dom1["detect_method"].GetInt()); 35 | detect_params.norm = dom1["norm"].GetBool(); 36 | detect_params.norm_half_kernel_size = dom1["norm_half_kernel_size"].GetInt(); 37 | detect_params.radius.clear(); 38 | for(const auto& r : dom1["radius"].GetArray()) { 39 | detect_params.radius.emplace_back(r.GetInt()); 40 | } 41 | detect_params.init_loc_thr = dom1["init_loc_thr"].GetDouble(); 42 | detect_params.polynomial_fit_half_kernel_size = dom1["polynomial_fit_half_kernel_size"].GetInt(); 43 | detect_params.score_thr = dom1["score_thr"].GetDouble(); 44 | detect_params.show_processing = false; 45 | if(params.pattern_type == 0) { 46 | detect_params.corner_type = cbdetect::SaddlePoint; 47 | } 48 | params.camera1.show_cornres = dom1["show_cornres"].GetBool(); 49 | } 50 | 51 | if(dom.HasMember("camera2")) { 52 | auto dom2 = dom["camera2"].GetObject(); 53 | cbdetect::Params& detect_params = params.camera2.detect_params; 54 | detect_params.detect_method = static_cast(dom2["detect_method"].GetInt()); 55 | detect_params.norm = dom2["norm"].GetBool(); 56 | detect_params.norm_half_kernel_size = dom2["norm_half_kernel_size"].GetInt(); 57 | detect_params.radius.clear(); 58 | for(const auto& r : dom2["radius"].GetArray()) { 59 | detect_params.radius.emplace_back(r.GetInt()); 60 | } 61 | detect_params.init_loc_thr = dom2["init_loc_thr"].GetDouble(); 62 | detect_params.polynomial_fit_half_kernel_size = dom2["polynomial_fit_half_kernel_size"].GetInt(); 63 | detect_params.score_thr = dom2["score_thr"].GetDouble(); 64 | detect_params.show_processing = false; 65 | if(params.pattern_type == 0) { 66 | detect_params.corner_type = cbdetect::SaddlePoint; 67 | } 68 | params.camera2.show_cornres = dom2["show_cornres"].GetBool(); 69 | } 70 | 71 | return 0; 72 | } 73 | 74 | } // namespace calib 75 | -------------------------------------------------------------------------------- /src/extract.cc: -------------------------------------------------------------------------------- 1 | /** 2 | * Copyright 2019, ftdlyc 3 | * Licensed under the MIT license. 4 | */ 5 | 6 | #include 7 | #include 8 | #include 9 | 10 | #include 11 | 12 | #include "libcbdetect/boards_from_corners.h" 13 | #include "libcbdetect/find_corners.h" 14 | #include "libcbdetect/plot_boards.h" 15 | 16 | #include "libcalib/config.h" 17 | #include "libcalib/extract.h" 18 | 19 | namespace calib { 20 | 21 | // pattern 22 | // angle0 angle1 angle2 angle3 23 | // -2 -2 -2 -2 -2 -2 -2 -2 24 | // -2 -2 -2 -2 -2 -2 -2 -2 -2 25 | // -2 -2 -2 26 | int find_pattern(const std::vector>& chessboard, 27 | int& x, int& y, int& angle) { 28 | std::vector> pos; 29 | for(int j = 2; j < chessboard.size() - 2; ++j) { 30 | for(int i = 2; i < chessboard[0].size() - 2; ++i) { 31 | if(chessboard[j][i] != -2 || 32 | chessboard[j + 1][i] != -2 || 33 | chessboard[j][i + 1] != -2 || 34 | chessboard[j + 1][i + 1] != -2 || 35 | chessboard[j - 1][i + 1] < 0 || 36 | chessboard[j + 1][i + 2] < 0 || 37 | chessboard[j + 2][i] < 0 || 38 | chessboard[j][i - 1] < 0) continue; 39 | 40 | int n = (chessboard[j - 1][i] == -2) + (chessboard[j][i + 2] == -2) + (chessboard[j + 2][i + 1] == -2) + (chessboard[j + 1][i - 1] == -2); 41 | if(n != 1) continue; 42 | 43 | if(chessboard[j - 1][i] == -2) { // angle0 44 | if(chessboard[j - 2][i] < 0 || chessboard[j - 1][i - 1] < 0) continue; 45 | pos.emplace_back(std::make_tuple(i, j - 1, 0)); 46 | } else if(chessboard[j + 1][i - 1] == -2) { // angle1 47 | if(chessboard[j + 2][i - 1] < 0 || chessboard[j + 1][i - 2] < 0) continue; 48 | pos.emplace_back(std::make_tuple(i - 1, j + 1, 1)); 49 | } else if(chessboard[j + 2][i + 1] == -2) { // angle2 50 | if(chessboard[j + 2][i + 2] < 0 || chessboard[j + 3][i + 1] < 0) continue; 51 | pos.emplace_back(std::make_tuple(i + 1, j + 2, 2)); 52 | } else if(chessboard[j][i + 2] == -2) { // angle3 53 | if(chessboard[j - 1][i + 2] < 0 || chessboard[j][i + 3] < 0) continue; 54 | pos.emplace_back(std::make_tuple(i + 2, j, 3)); 55 | } 56 | } 57 | } 58 | 59 | if(pos.size() != 1) return -1; 60 | x = std::get<0>(pos[0]); 61 | y = std::get<1>(pos[0]); 62 | angle = std::get<2>(pos[0]); 63 | return 0; 64 | } 65 | 66 | int rotate_chessboard(const std::vector>& src, 67 | std::vector>& dst, 68 | int& x, int& y, int& angle) { 69 | switch(angle) { 70 | case 0: { 71 | dst = src; 72 | break; 73 | } 74 | case 1: { 75 | dst = std::move(std::vector>(src[0].size(), std::vector(src.size()))); 76 | for(int j = 0; j < src.size(); ++j) { 77 | for(int i = 0; i < src[0].size(); ++i) { 78 | dst[i][src.size() - 1 - j] = src[j][i]; 79 | } 80 | } 81 | int tmp = x; 82 | x = src.size() - 1 - y; 83 | y = tmp; 84 | break; 85 | } 86 | case 2: { 87 | dst = std::move(std::vector>(src.size(), std::vector(src[0].size()))); 88 | for(int j = 0; j < src.size(); ++j) { 89 | for(int i = 0; i < src[0].size(); ++i) { 90 | dst[src.size() - 1 - j][src[0].size() - 1 - i] = src[j][i]; 91 | } 92 | } 93 | x = src[0].size() - 1 - x; 94 | y = src.size() - 1 - y; 95 | break; 96 | } 97 | case 3: { 98 | dst = std::move(std::vector>(src[0].size(), std::vector(src.size()))); 99 | for(int j = 0; j < src.size(); ++j) { 100 | for(int i = 0; i < src[0].size(); ++i) { 101 | dst[src[0].size() - 1 - i][j] = src[j][i]; 102 | } 103 | } 104 | int tmp = x; 105 | x = y; 106 | y = src[0].size() - 1 - tmp; 107 | break; 108 | } 109 | } 110 | 111 | angle = 0; 112 | return 0; 113 | } 114 | 115 | int match_two_chessboards(const std::vector>& chessboard_1, 116 | const std::vector>& chessboard_2, 117 | const std::vector& points_1, 118 | const std::vector& points_2, 119 | std::vector& image_points_1, 120 | std::vector& image_points_2, 121 | std::vector& world_points, 122 | double pattern_size) { 123 | int x1, y1, angle1, x2, y2, angle2; 124 | if(find_pattern(chessboard_1, x1, y1, angle1) < 0) return -1; 125 | if(find_pattern(chessboard_2, x2, y2, angle2) < 0) return -1; 126 | 127 | std::vector> norm_board_1, norm_board_2; 128 | rotate_chessboard(chessboard_1, norm_board_1, x1, y1, angle1); 129 | rotate_chessboard(chessboard_2, norm_board_2, x2, y2, angle2); 130 | 131 | for(int j = 0; j < norm_board_1.size(); ++j) { 132 | for(int i = 0; i < norm_board_1[0].size(); ++i) { 133 | int ii = i - x1 + x2; 134 | int jj = j - y1 + y2; 135 | if(ii < 0 || ii >= norm_board_1[0].size() || 136 | jj < 0 || jj >= norm_board_1.size() || 137 | norm_board_1[j][i] < 0 || norm_board_2[jj][ii] < 0) continue; 138 | 139 | image_points_1.emplace_back(points_1[norm_board_1[j][i]]); 140 | image_points_2.emplace_back(points_2[norm_board_2[jj][ii]]); 141 | world_points.emplace_back(cv::Point2d(pattern_size * (i - 1), pattern_size * (j - 1))); 142 | } 143 | } 144 | 145 | return 0; 146 | } 147 | 148 | std::vector extract_corners(const std::vector& images, 149 | std::vector>& image_points, 150 | std::vector>& world_points, 151 | Params& params) { 152 | std::vector res(images.size(), 0); 153 | if(params.camera1.detect_params.corner_type == cbdetect::SaddlePoint) { 154 | params.camera1.detect_params.overlay = true; 155 | for(int i = 0; i < images.size(); ++i) { 156 | cbdetect::Corner chessboad_corners; 157 | std::vector chessboards; 158 | 159 | cbdetect::find_corners(images[i], chessboad_corners, params.camera1.detect_params); 160 | cbdetect::boards_from_corners(images[i], chessboad_corners, chessboards, params.camera1.detect_params); 161 | if(chessboards.empty()) { 162 | continue; 163 | } 164 | if(params.camera1.show_cornres) { 165 | cbdetect::plot_boards(images[i], chessboad_corners, chessboards, params.camera1.detect_params); 166 | } 167 | 168 | std::sort(chessboards.begin(), chessboards.end(), 169 | [](const cbdetect::Board& b1, const cbdetect::Board& b2) { 170 | return b1.num > b2.num; 171 | }); 172 | 173 | res[i] = std::min(params.num_boards, (int)chessboards.size()); 174 | for(int j = 0; j < res[i]; ++j) { 175 | std::vector>& chessboard = chessboards[j].idx; 176 | std::vector pi_buf, pw_buf; 177 | 178 | for(int jj = 0; jj < chessboard.size(); ++jj) { 179 | for(int ii = 0; ii < chessboard[0].size(); ++ii) { 180 | if(chessboard[jj][ii] < 0) continue; 181 | pi_buf.emplace_back(chessboad_corners.p[chessboard[jj][ii]]); 182 | pw_buf.emplace_back(cv::Point2d(params.pattern_size * (ii - 1), params.pattern_size * (jj - 1))); 183 | } 184 | } 185 | image_points.emplace_back(pi_buf); 186 | world_points.emplace_back(pw_buf); 187 | } 188 | } 189 | } 190 | if(params.camera1.detect_params.corner_type == cbdetect::MonkeySaddlePoint) { 191 | for(int i = 0; i < images.size(); ++i) { 192 | cbdetect::Corner deltille_corners; 193 | std::vector deltilles; 194 | 195 | cbdetect::find_corners(images[i], deltille_corners, params.camera1.detect_params); 196 | cbdetect::boards_from_corners(images[i], deltille_corners, deltilles, params.camera1.detect_params); 197 | if(deltilles.empty()) { 198 | continue; 199 | } 200 | if(params.camera1.show_cornres) { 201 | cbdetect::plot_boards(images[i], deltille_corners, deltilles, params.camera1.detect_params); 202 | } 203 | 204 | std::sort(deltilles.begin(), deltilles.end(), 205 | [](const cbdetect::Board& d1, const cbdetect::Board& d2) { 206 | return d1.num > d2.num; 207 | }); 208 | 209 | res[i] = std::min(params.num_boards, (int)deltilles.size()); 210 | double dx = params.pattern_size; 211 | double dy = params.pattern_size / 2.0 * std::sqrt(3); 212 | for(int j = 0; j < res[i]; ++j) { 213 | cbdetect::Board& deltille = deltilles[j]; 214 | std::vector pi_buf, pw_buf; 215 | 216 | for(int jj = 0; jj < deltille.idx.size(); ++jj) { 217 | double shift = -jj * dx / 2.0; 218 | for(int ii = 0; ii < deltille.idx[0].size(); ++ii) { 219 | if(deltille.idx[jj][ii] >= 0) { 220 | pi_buf.emplace_back(deltille_corners.p[deltille.idx[jj][ii]]); 221 | pw_buf.emplace_back(cv::Point2d(shift + dx * ii, dy * jj)); 222 | } 223 | } 224 | } 225 | image_points.emplace_back(pi_buf); 226 | world_points.emplace_back(pw_buf); 227 | } 228 | } 229 | } 230 | 231 | return res; 232 | } 233 | 234 | int extract_corners(const cv::Mat& images, 235 | std::vector& image_points, 236 | std::vector& world_points, 237 | Params& params) { 238 | params.camera1.detect_params.overlay = true; 239 | 240 | cbdetect::Corner corners; 241 | std::vector boards; 242 | cbdetect::find_corners(images, corners, params.camera1.detect_params); 243 | cbdetect::boards_from_corners(images, corners, boards, params.camera1.detect_params); 244 | 245 | if(params.camera1.show_cornres) { 246 | cbdetect::plot_boards(images, corners, boards, params.camera1.detect_params); 247 | } 248 | 249 | return 0; 250 | } 251 | 252 | std::vector extract_corners_stereo(const std::vector& images_1, 253 | const std::vector& images_2, 254 | std::vector>& image_points_1, 255 | std::vector>& image_points_2, 256 | std::vector>& world_points, 257 | Params& params) { 258 | int num_images = images_1.size(); 259 | std::vector res(num_images, 0); 260 | if(images_1.size() != images_2.size() || 261 | params.camera1.detect_params.corner_type != cbdetect::SaddlePoint || 262 | params.camera1.detect_params.corner_type != cbdetect::SaddlePoint) return res; 263 | 264 | params.camera1.detect_params.overlay = true; 265 | params.camera2.detect_params.overlay = true; 266 | 267 | for(int i = 0; i < num_images; ++i) { 268 | cbdetect::Corner chessboad_corners_1, chessboad_corners_2; 269 | std::vector chessboards_1, chessboards_2; 270 | cbdetect::find_corners(images_1[i], chessboad_corners_1, params.camera1.detect_params); 271 | cbdetect::boards_from_corners(images_1[i], chessboad_corners_1, chessboards_1, params.camera1.detect_params); 272 | cbdetect::find_corners(images_2[i], chessboad_corners_2, params.camera2.detect_params); 273 | cbdetect::boards_from_corners(images_2[i], chessboad_corners_2, chessboards_2, params.camera2.detect_params); 274 | 275 | if(chessboards_1.empty() || chessboards_2.empty()) { 276 | continue; 277 | } 278 | 279 | if(params.num_boards == 1) { 280 | std::sort(chessboards_1.begin(), chessboards_1.end(), 281 | [](const cbdetect::Board& b1, const cbdetect::Board& b2) { 282 | return b1.num > b2.num; 283 | }); 284 | std::sort(chessboards_2.begin(), chessboards_2.end(), 285 | [](const cbdetect::Board& b1, const cbdetect::Board& b2) { 286 | return b1.num > b2.num; 287 | }); 288 | } else { 289 | std::sort(chessboards_1.begin(), chessboards_1.end(), [&](const cbdetect::Board& b1, const cbdetect::Board& b2) -> bool { 290 | if(b1.num == 0) { 291 | return false; 292 | } else if(b2.num == 0) { 293 | return true; 294 | } else { 295 | const auto& chessboard_1 = b1.idx; 296 | const auto& chessboard_2 = b2.idx; 297 | cv::Point2d p1(0., 0.), p2(0., 0.); 298 | for(int j = 0; j < chessboard_1.size(); ++j) { 299 | for(int i = 0; i < chessboard_1[0].size(); ++i) { 300 | if(chessboard_1[j][i] < 0) continue; 301 | p1 += chessboad_corners_1.p[chessboard_1[j][i]]; 302 | } 303 | } 304 | for(int j = 0; j < chessboard_2.size(); ++j) { 305 | for(int i = 0; i < chessboard_2[0].size(); ++i) { 306 | if(chessboard_2[j][i] < 0) continue; 307 | p2 += chessboad_corners_1.p[chessboard_2[j][i]]; 308 | } 309 | } 310 | p1 /= b1.num; 311 | p2 /= b2.num; 312 | return (2 * p1.y + p1.x) < (2 * p2.y + p2.x); 313 | } 314 | }); 315 | std::sort(chessboards_2.begin(), chessboards_2.end(), [&](const cbdetect::Board& b1, const cbdetect::Board& b2) -> bool { 316 | if(b1.num == 0) { 317 | return false; 318 | } else if(b2.num == 0) { 319 | return true; 320 | } else { 321 | const auto& chessboard_1 = b1.idx; 322 | const auto& chessboard_2 = b2.idx; 323 | cv::Point2d p1(0., 0.), p2(0., 0.); 324 | for(int j = 0; j < chessboard_1.size(); ++j) { 325 | for(int i = 0; i < chessboard_1[0].size(); ++i) { 326 | if(chessboard_1[j][i] < 0) continue; 327 | p1 += chessboad_corners_2.p[chessboard_1[j][i]]; 328 | } 329 | } 330 | for(int j = 0; j < chessboard_2.size(); ++j) { 331 | for(int i = 0; i < chessboard_2[0].size(); ++i) { 332 | if(chessboard_2[j][i] < 0) continue; 333 | p2 += chessboad_corners_2.p[chessboard_2[j][i]]; 334 | } 335 | } 336 | p1 /= b1.num; 337 | p2 /= b2.num; 338 | return (2 * p1.y + p1.x) < (2 * p2.y + p2.x); 339 | } 340 | }); 341 | } 342 | 343 | if(params.camera1.show_cornres) { 344 | cbdetect::plot_boards(images_1[i], chessboad_corners_1, chessboards_1, params.camera1.detect_params); 345 | } 346 | if(params.camera2.show_cornres) { 347 | cbdetect::plot_boards(images_2[i], chessboad_corners_2, chessboards_2, params.camera1.detect_params); 348 | } 349 | 350 | int n = std::min(chessboards_1.size(), chessboards_2.size()); 351 | n = std::min(n, params.num_boards); 352 | res[i] = 0; 353 | 354 | if(params.pattern_type == 0) { // simple full match 355 | for(int j = 0; j < n; ++j) { 356 | if(chessboards_1[j].num != chessboards_2[j].num || chessboards_1[j].idx.size() != chessboards_2[j].idx.size()) continue; 357 | 358 | std::vector>& chessboard_1 = chessboards_1[j].idx; 359 | std::vector>& chessboard_2 = chessboards_2[j].idx; 360 | std::vector pi_buf_1, pi_buf_2, pw_buf; 361 | 362 | for(int jj = 0; jj < chessboard_1.size(); ++jj) { 363 | for(int ii = 0; ii < chessboard_1[0].size(); ++ii) { 364 | if(chessboard_1[jj][ii] < 0 || chessboard_2[jj][ii] < 0) continue; 365 | 366 | pi_buf_1.emplace_back(chessboad_corners_1.p[chessboard_1[jj][ii]]); 367 | pi_buf_2.emplace_back(chessboad_corners_2.p[chessboard_2[jj][ii]]); 368 | pw_buf.emplace_back(cv::Point2d(params.pattern_size * (ii - 1), params.pattern_size * (jj - 1))); 369 | } 370 | } 371 | image_points_1.emplace_back(pi_buf_1); 372 | image_points_2.emplace_back(pi_buf_2); 373 | world_points.emplace_back(pw_buf); 374 | 375 | ++res[i]; 376 | } 377 | } else { // pattern match 378 | for(int j = 0; j < n; ++j) { 379 | std::vector pi_buf_1, pi_buf_2, pw_buf; 380 | 381 | int ret = match_two_chessboards(chessboards_1[j].idx, chessboards_2[j].idx, 382 | chessboad_corners_1.p, chessboad_corners_2.p, 383 | pi_buf_1, pi_buf_2, pw_buf, 384 | params.pattern_size); 385 | if(ret < 0) continue; 386 | 387 | image_points_1.emplace_back(pi_buf_1); 388 | image_points_2.emplace_back(pi_buf_2); 389 | world_points.emplace_back(pw_buf); 390 | ++res[i]; 391 | } 392 | } 393 | } 394 | 395 | return res; 396 | } 397 | 398 | int extract_corners_stereo(const cv::Mat& images_1, 399 | const cv::Mat& images_2, 400 | std::vector& image_points_1, 401 | std::vector& image_points_2, 402 | std::vector& world_points, 403 | Params& params) { 404 | if(params.camera1.detect_params.corner_type != cbdetect::SaddlePoint || 405 | params.camera1.detect_params.corner_type != cbdetect::SaddlePoint) return -1; 406 | 407 | params.camera1.detect_params.overlay = true; 408 | params.camera2.detect_params.overlay = true; 409 | 410 | cbdetect::Corner chessboad_corners_1, chessboad_corners_2; 411 | std::vector chessboards_1, chessboards_2; 412 | cbdetect::find_corners(images_1, chessboad_corners_1, params.camera1.detect_params); 413 | cbdetect::boards_from_corners(images_1, chessboad_corners_1, chessboards_1, params.camera1.detect_params); 414 | cbdetect::find_corners(images_2, chessboad_corners_2, params.camera2.detect_params); 415 | cbdetect::boards_from_corners(images_2, chessboad_corners_2, chessboards_2, params.camera2.detect_params); 416 | 417 | if(params.camera1.show_cornres) { 418 | cbdetect::plot_boards(images_1, chessboad_corners_1, chessboards_1, params.camera1.detect_params); 419 | } 420 | if(params.camera2.show_cornres) { 421 | cbdetect::plot_boards(images_2, chessboad_corners_2, chessboards_2, params.camera1.detect_params); 422 | } 423 | 424 | if(chessboards_1.size() != 1 || chessboards_2.size() != 1) return -1; 425 | 426 | if(params.pattern_type == 0) { // simple full match 427 | if(chessboards_1[0].num != chessboards_2[0].num || chessboards_1[0].idx.size() != chessboards_2[0].idx.size()) return -1; 428 | 429 | std::vector>& chessboard_1 = chessboards_1[0].idx; 430 | std::vector>& chessboard_2 = chessboards_2[0].idx; 431 | 432 | for(int jj = 0; jj < chessboard_1.size(); ++jj) { 433 | for(int ii = 0; ii < chessboard_1[0].size(); ++ii) { 434 | if(chessboard_1[jj][ii] < 0 || chessboard_2[jj][ii] < 0) 435 | continue; 436 | 437 | image_points_1.emplace_back(chessboad_corners_1.p[chessboard_1[jj][ii]]); 438 | image_points_2.emplace_back(chessboad_corners_2.p[chessboard_2[jj][ii]]); 439 | world_points.emplace_back(cv::Point2d(params.pattern_size * (ii - 1), params.pattern_size * (jj - 1))); 440 | } 441 | } 442 | } else { // pattern match 443 | int ret = match_two_chessboards(chessboards_1[0].idx, chessboards_2[0].idx, 444 | chessboad_corners_1.p, chessboad_corners_2.p, 445 | image_points_1, image_points_2, world_points, 446 | params.pattern_size); 447 | if(ret < 0) return -1; 448 | } 449 | 450 | return 0; 451 | } 452 | 453 | } // namespace calib 454 | -------------------------------------------------------------------------------- /src/fileio.cc: -------------------------------------------------------------------------------- 1 | /** 2 | * Copyright 2019, ftdlyc 3 | * Licensed under the MIT license. 4 | */ 5 | 6 | #include "libcalib/fileio.h" 7 | 8 | #ifdef __linux__ 9 | 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | 17 | #endif 18 | #ifdef _WIN32 19 | 20 | #include 21 | #include 22 | #include 23 | #include 24 | 25 | #endif 26 | 27 | #include 28 | #include 29 | #include 30 | 31 | #ifdef __linux__ 32 | 33 | bool get_all_file(const char* path, const char* pattern, std::vector& files) { 34 | DIR* dir; 35 | if(!(dir = opendir(path))) { 36 | return false; 37 | }; 38 | files.clear(); 39 | struct dirent* p_dirent; 40 | std::regex reg(pattern); 41 | std::string path_str(path); 42 | if(path_str[path_str.size()] != '/') { 43 | path_str += "/"; 44 | } 45 | while((p_dirent = readdir(dir))) { 46 | if(p_dirent->d_type != 4) { 47 | if(std::regex_match(p_dirent->d_name, reg)) { 48 | files.emplace_back(path_str + std::string(p_dirent->d_name)); 49 | } 50 | } 51 | } 52 | std::sort(files.begin(), files.end()); 53 | closedir(dir); 54 | return true; 55 | } 56 | 57 | #endif 58 | 59 | #ifdef _WIN32 60 | 61 | bool get_all_file(const char* path, const char* type, std::vector& files) { 62 | intptr_t hFile = 0; 63 | struct _finddata_t fileinfo; 64 | files.clear(); 65 | if((hFile = _findfirst(std::string(path).append("/*.").append(type).c_str(), &fileinfo)) != -1) { 66 | do { 67 | if(!(fileinfo.attrib & _A_SUBDIR)) { 68 | files.push_back(std::string(path).append("/").append(fileinfo.name)); 69 | } 70 | } while(_findnext(hFile, &fileinfo) == 0); 71 | _findclose(hFile); 72 | std::sort(files.begin(), files.end()); 73 | return true; 74 | } else { 75 | return false; 76 | } 77 | } 78 | 79 | #endif 80 | -------------------------------------------------------------------------------- /src/interface.cc: -------------------------------------------------------------------------------- 1 | /** 2 | * Copyright 2019, ftdlyc 3 | * Licensed under the MIT license. 4 | */ 5 | 6 | #include 7 | 8 | #include 9 | 10 | #include 11 | 12 | #include "libcalib/calibration.h" 13 | #include "libcalib/config.h" 14 | #include "libcalib/extract.h" 15 | #include "libcalib/interface.h" 16 | 17 | namespace calib { 18 | 19 | int calibrate(const std::vector& images, 20 | const std::string& config_file, 21 | std::vector& K, 22 | std::vector& D, 23 | std::vector>& Rwc, 24 | std::vector>& Twc, 25 | std::vector& extract_res, 26 | double& error) { 27 | // load config 28 | Params params; 29 | if(get_config(config_file, params) < 0) { 30 | printf("Failed to load config from %s\n", config_file.c_str()); 31 | return -1; 32 | } 33 | 34 | // extract corners 35 | std::vector> image_points, world_points; 36 | extract_res = std::move(extract_corners(images, image_points, world_points, params)); 37 | if(image_points.size() < 4) { 38 | return -1; 39 | } 40 | 41 | // calibrate camera 42 | K.resize(5); 43 | D.resize(5); 44 | Rwc = std::move(std::vector>(image_points.size(), std::vector(3, 0))); 45 | Twc = std::move(std::vector>(image_points.size(), std::vector(3, 0))); 46 | error = calibrate_camera(world_points, image_points, K, D, Rwc, Twc); 47 | 48 | return 0; 49 | } 50 | 51 | int calibrate(const std::vector& images, 52 | const std::string& config_file, 53 | std::vector& K, 54 | std::vector& D, 55 | std::vector>& Rwc, 56 | std::vector>& Twc, 57 | std::vector& extract_res) { 58 | double error; 59 | return calibrate(images, config_file, K, D, Rwc, Twc, extract_res, error); 60 | } 61 | 62 | int calibrate_stereo(const std::vector& images_1, 63 | const std::vector& images_2, 64 | const std::string& config_file, 65 | std::vector& K1, 66 | std::vector& K2, 67 | std::vector& D1, 68 | std::vector& D2, 69 | std::vector>& Rwc1, 70 | std::vector>& Rwc2, 71 | std::vector>& Twc1, 72 | std::vector>& Twc2, 73 | std::vector& R, 74 | std::vector& T, 75 | std::vector& extract_res, 76 | std::vector& error) { 77 | // load config 78 | Params params; 79 | if(get_config(config_file, params) < 0) { 80 | printf("Failed to load config from %s\n", config_file.c_str()); 81 | return -1; 82 | } 83 | 84 | // extract corners 85 | std::vector> image_points_1, image_points_2, world_points; 86 | extract_res = std::move(extract_corners_stereo(images_1, images_2, image_points_1, image_points_2, world_points, params)); 87 | if(world_points.size() < 4) { 88 | return -1; 89 | } 90 | 91 | // calibrate camera 92 | K1.resize(5); 93 | K2.resize(5); 94 | D1.resize(5); 95 | D2.resize(5); 96 | Rwc1 = std::move(std::vector>(world_points.size(), std::vector(3, 0))); 97 | Rwc2 = std::move(std::vector>(world_points.size(), std::vector(3, 0))); 98 | Twc1 = std::move(std::vector>(world_points.size(), std::vector(3, 0))); 99 | Twc2 = std::move(std::vector>(world_points.size(), std::vector(3, 0))); 100 | R.resize(3); 101 | T.resize(3); 102 | 103 | error.resize(3); 104 | error[0] = calibrate_camera(world_points, image_points_1, K1, D1, Rwc1, Twc1); 105 | error[1] = calibrate_camera(world_points, image_points_2, K2, D2, Rwc2, Twc2); 106 | error[2] = calibrate_stereo_camera(world_points, image_points_1, image_points_2, K1, K2, D1, D2, Rwc1, Rwc2, Twc1, Twc2, R, T); 107 | 108 | return 0; 109 | } 110 | 111 | int calibrate_stereo(const std::vector& images_1, 112 | const std::vector& images_2, 113 | const std::string& config_file, 114 | std::vector& K1, 115 | std::vector& K2, 116 | std::vector& D1, 117 | std::vector& D2, 118 | std::vector>& Rwc1, 119 | std::vector>& Rwc2, 120 | std::vector>& Twc1, 121 | std::vector>& Twc2, 122 | std::vector& R, 123 | std::vector& T, 124 | std::vector& extract_res) { 125 | std::vector error; 126 | return calibrate_stereo(images_1, images_2, config_file, K1, K2, D1, D2, Rwc1, Rwc2, Twc1, Twc2, R, T, extract_res, error); 127 | } 128 | 129 | } // namespace calib 130 | --------------------------------------------------------------------------------