├── .gitattributes ├── FisheyeDataSet └── test_images │ ├── Fisheye1_1.jpg │ ├── Fisheye1_10.jpg │ ├── Fisheye1_11.jpg │ ├── Fisheye1_12.jpg │ ├── Fisheye1_13.jpg │ ├── Fisheye1_14.jpg │ ├── Fisheye1_15.jpg │ ├── Fisheye1_2.jpg │ ├── Fisheye1_3.jpg │ ├── Fisheye1_4.jpg │ ├── Fisheye1_5.jpg │ ├── Fisheye1_6.jpg │ ├── Fisheye1_7.jpg │ ├── Fisheye1_8.jpg │ ├── Fisheye1_9.jpg │ ├── Fisheye2_1.jpg │ ├── Fisheye2_10.jpg │ ├── Fisheye2_11.jpg │ ├── Fisheye2_12.jpg │ ├── Fisheye2_13.jpg │ ├── Fisheye2_14.jpg │ ├── Fisheye2_15.jpg │ ├── Fisheye2_16.jpg │ ├── Fisheye2_2.jpg │ ├── Fisheye2_3.jpg │ ├── Fisheye2_4.jpg │ ├── Fisheye2_5.jpg │ ├── Fisheye2_6.jpg │ ├── Fisheye2_7.jpg │ ├── Fisheye2_8.jpg │ ├── Fisheye2_9.jpg │ ├── GOPR001.JPG │ ├── GOPR1.jpg │ ├── GOPR10.jpg │ ├── GOPR11.jpg │ ├── GOPR2.jpg │ ├── GOPR3.jpg │ ├── GOPR4.jpg │ ├── GOPR5.jpg │ ├── GOPR6.jpg │ ├── GOPR7.jpg │ ├── GOPR8.jpg │ ├── GOPR9.jpg │ ├── checkerboard_sizes.txt │ ├── thumbs1.jpg │ ├── thumbs2.jpg │ └── thumbs3.jpg ├── README.md ├── cpp_model_impl ├── CmakeLists.txt └── src │ ├── CMakeLists.txt │ ├── cam_model_general.cpp │ ├── cam_model_general.h │ └── cam_model_general_test.cpp └── src ├── C_calib_data.m ├── Step1_perform_test_calibrations.m ├── Step2_compare_results.m ├── bundleAdjustmentUrban.m ├── bundleErrUrban.m ├── click_calibUrban.m ├── errCenterUrban.m ├── eulerFromR.m ├── findcenterUrban.m ├── findinvpolyUrban.m ├── get_checkerboard_cornersUrban.m ├── ocam_calibUrban.m ├── ocam_calib_guiUrban.m └── optimizefunction.m /.gitattributes: -------------------------------------------------------------------------------- 1 | # Auto detect text files and perform LF normalization 2 | * text=auto 3 | 4 | # Custom for Visual Studio 5 | *.cs diff=csharp 6 | *.sln merge=union 7 | *.csproj merge=union 8 | *.vbproj merge=union 9 | *.fsproj merge=union 10 | *.dbproj merge=union 11 | 12 | # Standard to msysgit 13 | *.doc diff=astextplain 14 | *.DOC diff=astextplain 15 | *.docx diff=astextplain 16 | *.DOCX diff=astextplain 17 | *.dot diff=astextplain 18 | *.DOT diff=astextplain 19 | *.pdf diff=astextplain 20 | *.PDF diff=astextplain 21 | *.rtf diff=astextplain 22 | *.RTF diff=astextplain 23 | -------------------------------------------------------------------------------- /FisheyeDataSet/test_images/Fisheye1_1.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/urbste/ImprovedOcamCalib/164dd8d96b1bee7e4aba9b0b100a85fcb2f0ba4e/FisheyeDataSet/test_images/Fisheye1_1.jpg -------------------------------------------------------------------------------- /FisheyeDataSet/test_images/Fisheye1_10.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/urbste/ImprovedOcamCalib/164dd8d96b1bee7e4aba9b0b100a85fcb2f0ba4e/FisheyeDataSet/test_images/Fisheye1_10.jpg -------------------------------------------------------------------------------- /FisheyeDataSet/test_images/Fisheye1_11.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/urbste/ImprovedOcamCalib/164dd8d96b1bee7e4aba9b0b100a85fcb2f0ba4e/FisheyeDataSet/test_images/Fisheye1_11.jpg -------------------------------------------------------------------------------- /FisheyeDataSet/test_images/Fisheye1_12.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/urbste/ImprovedOcamCalib/164dd8d96b1bee7e4aba9b0b100a85fcb2f0ba4e/FisheyeDataSet/test_images/Fisheye1_12.jpg -------------------------------------------------------------------------------- /FisheyeDataSet/test_images/Fisheye1_13.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/urbste/ImprovedOcamCalib/164dd8d96b1bee7e4aba9b0b100a85fcb2f0ba4e/FisheyeDataSet/test_images/Fisheye1_13.jpg -------------------------------------------------------------------------------- /FisheyeDataSet/test_images/Fisheye1_14.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/urbste/ImprovedOcamCalib/164dd8d96b1bee7e4aba9b0b100a85fcb2f0ba4e/FisheyeDataSet/test_images/Fisheye1_14.jpg -------------------------------------------------------------------------------- /FisheyeDataSet/test_images/Fisheye1_15.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/urbste/ImprovedOcamCalib/164dd8d96b1bee7e4aba9b0b100a85fcb2f0ba4e/FisheyeDataSet/test_images/Fisheye1_15.jpg -------------------------------------------------------------------------------- /FisheyeDataSet/test_images/Fisheye1_2.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/urbste/ImprovedOcamCalib/164dd8d96b1bee7e4aba9b0b100a85fcb2f0ba4e/FisheyeDataSet/test_images/Fisheye1_2.jpg -------------------------------------------------------------------------------- /FisheyeDataSet/test_images/Fisheye1_3.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/urbste/ImprovedOcamCalib/164dd8d96b1bee7e4aba9b0b100a85fcb2f0ba4e/FisheyeDataSet/test_images/Fisheye1_3.jpg -------------------------------------------------------------------------------- /FisheyeDataSet/test_images/Fisheye1_4.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/urbste/ImprovedOcamCalib/164dd8d96b1bee7e4aba9b0b100a85fcb2f0ba4e/FisheyeDataSet/test_images/Fisheye1_4.jpg -------------------------------------------------------------------------------- /FisheyeDataSet/test_images/Fisheye1_5.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/urbste/ImprovedOcamCalib/164dd8d96b1bee7e4aba9b0b100a85fcb2f0ba4e/FisheyeDataSet/test_images/Fisheye1_5.jpg -------------------------------------------------------------------------------- /FisheyeDataSet/test_images/Fisheye1_6.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/urbste/ImprovedOcamCalib/164dd8d96b1bee7e4aba9b0b100a85fcb2f0ba4e/FisheyeDataSet/test_images/Fisheye1_6.jpg -------------------------------------------------------------------------------- /FisheyeDataSet/test_images/Fisheye1_7.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/urbste/ImprovedOcamCalib/164dd8d96b1bee7e4aba9b0b100a85fcb2f0ba4e/FisheyeDataSet/test_images/Fisheye1_7.jpg -------------------------------------------------------------------------------- /FisheyeDataSet/test_images/Fisheye1_8.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/urbste/ImprovedOcamCalib/164dd8d96b1bee7e4aba9b0b100a85fcb2f0ba4e/FisheyeDataSet/test_images/Fisheye1_8.jpg -------------------------------------------------------------------------------- /FisheyeDataSet/test_images/Fisheye1_9.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/urbste/ImprovedOcamCalib/164dd8d96b1bee7e4aba9b0b100a85fcb2f0ba4e/FisheyeDataSet/test_images/Fisheye1_9.jpg -------------------------------------------------------------------------------- /FisheyeDataSet/test_images/Fisheye2_1.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/urbste/ImprovedOcamCalib/164dd8d96b1bee7e4aba9b0b100a85fcb2f0ba4e/FisheyeDataSet/test_images/Fisheye2_1.jpg -------------------------------------------------------------------------------- /FisheyeDataSet/test_images/Fisheye2_10.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/urbste/ImprovedOcamCalib/164dd8d96b1bee7e4aba9b0b100a85fcb2f0ba4e/FisheyeDataSet/test_images/Fisheye2_10.jpg -------------------------------------------------------------------------------- /FisheyeDataSet/test_images/Fisheye2_11.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/urbste/ImprovedOcamCalib/164dd8d96b1bee7e4aba9b0b100a85fcb2f0ba4e/FisheyeDataSet/test_images/Fisheye2_11.jpg -------------------------------------------------------------------------------- /FisheyeDataSet/test_images/Fisheye2_12.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/urbste/ImprovedOcamCalib/164dd8d96b1bee7e4aba9b0b100a85fcb2f0ba4e/FisheyeDataSet/test_images/Fisheye2_12.jpg -------------------------------------------------------------------------------- /FisheyeDataSet/test_images/Fisheye2_13.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/urbste/ImprovedOcamCalib/164dd8d96b1bee7e4aba9b0b100a85fcb2f0ba4e/FisheyeDataSet/test_images/Fisheye2_13.jpg -------------------------------------------------------------------------------- /FisheyeDataSet/test_images/Fisheye2_14.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/urbste/ImprovedOcamCalib/164dd8d96b1bee7e4aba9b0b100a85fcb2f0ba4e/FisheyeDataSet/test_images/Fisheye2_14.jpg -------------------------------------------------------------------------------- /FisheyeDataSet/test_images/Fisheye2_15.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/urbste/ImprovedOcamCalib/164dd8d96b1bee7e4aba9b0b100a85fcb2f0ba4e/FisheyeDataSet/test_images/Fisheye2_15.jpg -------------------------------------------------------------------------------- /FisheyeDataSet/test_images/Fisheye2_16.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/urbste/ImprovedOcamCalib/164dd8d96b1bee7e4aba9b0b100a85fcb2f0ba4e/FisheyeDataSet/test_images/Fisheye2_16.jpg -------------------------------------------------------------------------------- /FisheyeDataSet/test_images/Fisheye2_2.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/urbste/ImprovedOcamCalib/164dd8d96b1bee7e4aba9b0b100a85fcb2f0ba4e/FisheyeDataSet/test_images/Fisheye2_2.jpg -------------------------------------------------------------------------------- /FisheyeDataSet/test_images/Fisheye2_3.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/urbste/ImprovedOcamCalib/164dd8d96b1bee7e4aba9b0b100a85fcb2f0ba4e/FisheyeDataSet/test_images/Fisheye2_3.jpg -------------------------------------------------------------------------------- /FisheyeDataSet/test_images/Fisheye2_4.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/urbste/ImprovedOcamCalib/164dd8d96b1bee7e4aba9b0b100a85fcb2f0ba4e/FisheyeDataSet/test_images/Fisheye2_4.jpg -------------------------------------------------------------------------------- /FisheyeDataSet/test_images/Fisheye2_5.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/urbste/ImprovedOcamCalib/164dd8d96b1bee7e4aba9b0b100a85fcb2f0ba4e/FisheyeDataSet/test_images/Fisheye2_5.jpg -------------------------------------------------------------------------------- /FisheyeDataSet/test_images/Fisheye2_6.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/urbste/ImprovedOcamCalib/164dd8d96b1bee7e4aba9b0b100a85fcb2f0ba4e/FisheyeDataSet/test_images/Fisheye2_6.jpg -------------------------------------------------------------------------------- /FisheyeDataSet/test_images/Fisheye2_7.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/urbste/ImprovedOcamCalib/164dd8d96b1bee7e4aba9b0b100a85fcb2f0ba4e/FisheyeDataSet/test_images/Fisheye2_7.jpg -------------------------------------------------------------------------------- /FisheyeDataSet/test_images/Fisheye2_8.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/urbste/ImprovedOcamCalib/164dd8d96b1bee7e4aba9b0b100a85fcb2f0ba4e/FisheyeDataSet/test_images/Fisheye2_8.jpg -------------------------------------------------------------------------------- /FisheyeDataSet/test_images/Fisheye2_9.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/urbste/ImprovedOcamCalib/164dd8d96b1bee7e4aba9b0b100a85fcb2f0ba4e/FisheyeDataSet/test_images/Fisheye2_9.jpg -------------------------------------------------------------------------------- /FisheyeDataSet/test_images/GOPR001.JPG: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/urbste/ImprovedOcamCalib/164dd8d96b1bee7e4aba9b0b100a85fcb2f0ba4e/FisheyeDataSet/test_images/GOPR001.JPG -------------------------------------------------------------------------------- /FisheyeDataSet/test_images/GOPR1.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/urbste/ImprovedOcamCalib/164dd8d96b1bee7e4aba9b0b100a85fcb2f0ba4e/FisheyeDataSet/test_images/GOPR1.jpg -------------------------------------------------------------------------------- /FisheyeDataSet/test_images/GOPR10.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/urbste/ImprovedOcamCalib/164dd8d96b1bee7e4aba9b0b100a85fcb2f0ba4e/FisheyeDataSet/test_images/GOPR10.jpg -------------------------------------------------------------------------------- /FisheyeDataSet/test_images/GOPR11.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/urbste/ImprovedOcamCalib/164dd8d96b1bee7e4aba9b0b100a85fcb2f0ba4e/FisheyeDataSet/test_images/GOPR11.jpg -------------------------------------------------------------------------------- /FisheyeDataSet/test_images/GOPR2.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/urbste/ImprovedOcamCalib/164dd8d96b1bee7e4aba9b0b100a85fcb2f0ba4e/FisheyeDataSet/test_images/GOPR2.jpg -------------------------------------------------------------------------------- /FisheyeDataSet/test_images/GOPR3.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/urbste/ImprovedOcamCalib/164dd8d96b1bee7e4aba9b0b100a85fcb2f0ba4e/FisheyeDataSet/test_images/GOPR3.jpg -------------------------------------------------------------------------------- /FisheyeDataSet/test_images/GOPR4.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/urbste/ImprovedOcamCalib/164dd8d96b1bee7e4aba9b0b100a85fcb2f0ba4e/FisheyeDataSet/test_images/GOPR4.jpg -------------------------------------------------------------------------------- /FisheyeDataSet/test_images/GOPR5.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/urbste/ImprovedOcamCalib/164dd8d96b1bee7e4aba9b0b100a85fcb2f0ba4e/FisheyeDataSet/test_images/GOPR5.jpg -------------------------------------------------------------------------------- /FisheyeDataSet/test_images/GOPR6.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/urbste/ImprovedOcamCalib/164dd8d96b1bee7e4aba9b0b100a85fcb2f0ba4e/FisheyeDataSet/test_images/GOPR6.jpg -------------------------------------------------------------------------------- /FisheyeDataSet/test_images/GOPR7.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/urbste/ImprovedOcamCalib/164dd8d96b1bee7e4aba9b0b100a85fcb2f0ba4e/FisheyeDataSet/test_images/GOPR7.jpg -------------------------------------------------------------------------------- /FisheyeDataSet/test_images/GOPR8.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/urbste/ImprovedOcamCalib/164dd8d96b1bee7e4aba9b0b100a85fcb2f0ba4e/FisheyeDataSet/test_images/GOPR8.jpg -------------------------------------------------------------------------------- /FisheyeDataSet/test_images/GOPR9.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/urbste/ImprovedOcamCalib/164dd8d96b1bee7e4aba9b0b100a85fcb2f0ba4e/FisheyeDataSet/test_images/GOPR9.jpg -------------------------------------------------------------------------------- /FisheyeDataSet/test_images/checkerboard_sizes.txt: -------------------------------------------------------------------------------- 1 | Name #squares_x #squares_x size[mm] 2 | ------------------------------------------------------ 3 | Fisheye1_#.jpg 5 7 32.5 4 | Fisheye2_#.jpg 5 7 117.0 5 | GoPro#.jpg 5 7 117.0 -------------------------------------------------------------------------------- /FisheyeDataSet/test_images/thumbs1.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/urbste/ImprovedOcamCalib/164dd8d96b1bee7e4aba9b0b100a85fcb2f0ba4e/FisheyeDataSet/test_images/thumbs1.jpg -------------------------------------------------------------------------------- /FisheyeDataSet/test_images/thumbs2.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/urbste/ImprovedOcamCalib/164dd8d96b1bee7e4aba9b0b100a85fcb2f0ba4e/FisheyeDataSet/test_images/thumbs2.jpg -------------------------------------------------------------------------------- /FisheyeDataSet/test_images/thumbs3.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/urbste/ImprovedOcamCalib/164dd8d96b1bee7e4aba9b0b100a85fcb2f0ba4e/FisheyeDataSet/test_images/thumbs3.jpg -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Improved OcamCalib 2 | 3 | This repository contains addon files for Scaramuzzas OcamCalib Toolbox. 4 | More information about the addon can be found in the paper. 5 | If you use the addons, consider citing it. 6 | 7 | @article{urban2015improved, 8 | title={Improved wide-angle, fisheye and omnidirectional camera calibration}, 9 | author={Urban, Steffen and Leitloff, Jens and Hinz, Stefan}, 10 | journal={ISPRS Journal of Photogrammetry and Remote Sensing}, 11 | volume={108}, 12 | pages={72--79}, 13 | year={2015}, 14 | publisher={Elsevier} 15 | } 16 | 17 | ## Installation instruction ## 18 | 1. Download and extract the original toolbox Scaramuzza_OCamCalib_v3.0 from 19 | Davide Scaramuzzas Homepage [Link](https://sites.google.com/site/scarabotix/ocamcalib-toolbox/ocamcalib-toolbox-download-page) 20 | (link worked june 2015) 21 | 2. Download this repository and copy the content of the src folder to the main 22 | directory of ocam_calib. 23 | 3. You should be asked to replace 24 | C_calib_data.m and 25 | optimizefunction.m -> click yes. 26 | The first file contains additional variables for statistics. 27 | The second file contains additional code lines to save statistics but 28 | is actually not used by the improved toolbox. 29 | 30 | If you want to run the test data sets, 31 | copy the images from "FisheyeDataSets/" to the same folder 32 | (contains 3 image data sets and test scripts) 33 | 34 | ## Calibration and Tests ## 35 | 36 | ### How to calibrate a single camera: ### 37 | 1. Run ocam_calibUrban.m !!! (instead of ocam_calib) 38 | 2. press (in that order) 39 | * read names 40 | * extract grid corners 41 | * calibration 42 | * non-linear refinement (LM least squares) 43 | or robust non-linear refinement (LM least squares with Huber) 44 | 45 | ### How to calibrate all test images/cameras at once ### 46 | 1. If not done already, 47 | copy the images from FisheyeDataSets to the ocam_calib folder 48 | 2. Download SampleImages.zip from Davide Scaramuzzas Homepage [Link](https://sites.google.com/site/scarabotix/ocamcalib-toolbox/ocamcalib-toolbox-download-page) 49 | 3. Run Step1_perform_test_calibrations.m (this takes quite a while) 50 | (If you want to run it fully automatic with polynomial order 4, go to 51 | calibration.m and comment out line 31) 52 | 4. Compare results with Step2_compare_results.m 53 | 54 | all image files should be .jpg . Otherwise you have to change the test script 55 | Step1_perform_test_calibrations.m 56 | -------------------------------------------------------------------------------- /cpp_model_impl/CmakeLists.txt: -------------------------------------------------------------------------------- 1 | # Steffen Urban email: steffen.urban@kit.edu, 2 | # Copyright (C) 2014 Steffen Urban 3 | # 4 | # This program is free software; you can redistribute it and/or modify 5 | # it under the terms of the GNU General Public License as published by 6 | # the Free Software Foundation; either version 2 of the License, or 7 | # (at your option) any later version. 8 | # 9 | # This program is distributed in the hope that it will be useful, 10 | # but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | # GNU General Public License for more details. 13 | # 14 | # You should have received a copy of the GNU General Public License along 15 | # with this program; if not, write to the Free Software Foundation, Inc., 16 | # 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA. 17 | 18 | # external packages: OpenCV >= 3.0.0 19 | 20 | 21 | cmake_minimum_required (VERSION 3.0.0) 22 | project (cam_model_general) 23 | 24 | ##################### 25 | # opencv 26 | ##################### 27 | find_package(OpenCV 3.0.0 REQUIRED) 28 | INCLUDE_DIRECTORIES(${OpenCV_INCLUDE_DIRS}) 29 | 30 | add_subdirectory(src) 31 | 32 | 33 | 34 | 35 | 36 | -------------------------------------------------------------------------------- /cpp_model_impl/src/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | include_directories(../) 2 | 3 | add_executable(cam_model_general_test cam_model_general_test.cpp) 4 | add_library(cam_model_general cam_model_general.cpp 5 | cam_model_general.h) 6 | target_link_libraries(cam_model_general_test 7 | cam_model_general 8 | ${OpenCV_LIBRARIES}) -------------------------------------------------------------------------------- /cpp_model_impl/src/cam_model_general.cpp: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/urbste/ImprovedOcamCalib/164dd8d96b1bee7e4aba9b0b100a85fcb2f0ba4e/cpp_model_impl/src/cam_model_general.cpp -------------------------------------------------------------------------------- /cpp_model_impl/src/cam_model_general.h: -------------------------------------------------------------------------------- 1 | /* Steffen Urban (Karlsruhe Institute of Technology) 2 | * Email : urbste@googlemail.com 3 | * Copyright(C) 2015 Steffen Urban 4 | * 5 | * This program is free software; you can redistribute it and / or modify 6 | * it under the terms of the GNU General Public License as published by 7 | * the Free Software Foundation; either version 2 of the License, or 8 | * (at your option) any later version. 9 | * 10 | * This program is distributed in the hope that it will be useful, 11 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 12 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.See the 13 | * GNU General Public License for more details. 14 | * 15 | * You should have received a copy of the GNU General Public License along 16 | * with this program; if not, write to the Free Software Foundation, Inc., 17 | * 51 Franklin Street, Fifth Floor, Boston, MA 02110 - 1301 USA. 18 | */ 19 | 20 | #ifndef CAM_MODEL_GENERAL_H 21 | #define CAM_MODEL_GENERAL_H 22 | 23 | #include 24 | 25 | // horner scheme for evaluating polynomials at a value x 26 | template 27 | T horner(T* coeffs, int s, T x) 28 | { 29 | T res = 0.0; 30 | for (int i = s - 1; i >= 0; i--) 31 | res = res * x + coeffs[i]; 32 | 33 | return res; 34 | } 35 | 36 | // template class implementation of the general atan model 37 | template 38 | class cCamModelGeneral 39 | { 40 | public: 41 | // construtors 42 | cCamModelGeneral() : 43 | c(T(1)), 44 | d(T(0)), 45 | e(T(0)), 46 | u0(T(0)), 47 | v0(T(0)), 48 | p((cv::Mat_(1, 1) << T(1))), 49 | invP((cv::Mat_(1, 1) << T(1))), 50 | p_deg(1), 51 | invP_deg(1), 52 | Iwidth(T(0)), Iheight(T(0)) 53 | {} 54 | 55 | cCamModelGeneral(cv::Vec cdeu0v0, 56 | cv::Mat_ p_, 57 | cv::Mat_ invP_) : 58 | c(cdeu0v0[0]), 59 | d(cdeu0v0[1]), 60 | e(cdeu0v0[2]), 61 | u0(cdeu0v0[3]), 62 | v0(cdeu0v0[4]), 63 | p(p_), 64 | invP(invP_) 65 | { 66 | // initialize degree of polynomials 67 | p_deg = (p_.rows > 1) ? p_.rows : p_deg = p_.cols; 68 | invP_deg = (p_.rows > 1) ? invP_deg = invP_.rows : invP_deg = invP_.cols; 69 | 70 | cde1 = (cv::Mat_(2, 2) << c, d, e, T(1)); 71 | } 72 | 73 | cCamModelGeneral(cv::Vec cdeu0v0, 74 | cv::Mat_ p_, 75 | cv::Mat_ invP_, 76 | int Iw_, int Ih_) : 77 | c(cdeu0v0[0]), 78 | d(cdeu0v0[1]), 79 | e(cdeu0v0[2]), 80 | u0(cdeu0v0[3]), 81 | v0(cdeu0v0[4]), 82 | p(p_), 83 | invP(invP_), 84 | Iwidth(Iw_), 85 | Iheight(Ih_) 86 | { 87 | // initialize degree of polynomials 88 | p_deg = (p_.rows > 1) ? p_.rows : p_deg = p_.cols; 89 | invP_deg = (p_.rows > 1) ? invP_deg = invP_.rows : invP_deg = invP_.cols; 90 | 91 | cde1 = (cv::Mat_(2, 2) << c, d, e, T(1)); 92 | } 93 | 94 | ~cCamModelGeneral(){} 95 | 96 | 97 | template inline void 98 | WorldToImg(const T& x, const T& y, const T& z, // 3D scene point 99 | T& u, T& v) // 2D image point 100 | { 101 | T norm = sqrt(x*x + y*y); 102 | if (norm == T(0)) 103 | norm = 1e-14; 104 | 105 | T theta = atan(-z / norm); 106 | T rho = horner((T*)invP.data, invP_deg, theta); 107 | 108 | T uu = x / norm * rho; 109 | T vv = y / norm * rho; 110 | 111 | u = uu*c + vv*d + u0; 112 | v = uu*e + vv + v0; 113 | 114 | } 115 | 116 | template inline void 117 | WorldToImg(const cv::Point3_& X, // 3D scene point 118 | cv::Point_& m) // 2D image point 119 | { 120 | T norm = sqrt(X.x*X.x + X.y*X.y); 121 | 122 | if (norm == T(0)) 123 | norm = 1e-14; 124 | 125 | T theta = atan(-X.z / norm); 126 | 127 | T rho = horner((T*)invP.data, invP_deg, theta); 128 | 129 | T uu = X.x / norm * rho; 130 | T vv = X.y / norm * rho; 131 | 132 | m.x = uu*c + vv*d + u0; 133 | m.y = uu*e + vv + v0; 134 | } 135 | 136 | // fastest by about factor 2 137 | template inline void 138 | WorldToImg(const cv::Vec& X, // 3D scene point 139 | cv::Vec& m) // 2D image point 140 | { 141 | 142 | double norm = cv::sqrt(X(0)*X(0) + X(1)*X(1)); 143 | 144 | if (norm == 0.0) 145 | norm = 1e-14; 146 | 147 | double theta = atan(-X(2) / norm); 148 | 149 | double rho = horner((T*)invP.data, invP_deg, theta); 150 | 151 | double uu = X(0) / norm * rho; 152 | double vv = X(1) / norm * rho; 153 | 154 | m(0) = uu*c + vv*d + u0; 155 | m(1) = uu*e + vv + v0; 156 | } 157 | 158 | template inline void 159 | ImgToWorld(T& x, T& y, T& z, // 3D scene point 160 | const T& u, const T& v) // 2D image point 161 | { 162 | T invAff = c - d*e; 163 | T u_t = u - u0; 164 | T v_t = v - v0; 165 | // inverse affine matrix image to sensor plane conversion 166 | x = (1 * u_t - d * v_t) / invAff; 167 | y = (-e * u_t + c * v_t) / invAff; 168 | T X2 = x*x; 169 | T Y2 = y*y; 170 | z = -horner((T*)p.data, p_deg, sqrt(X2 + Y2)); 171 | 172 | // normalize vectors spherically 173 | T norm = sqrt(X2 + Y2 + z*z); 174 | x /= norm; 175 | y /= norm; 176 | z /= norm; 177 | } 178 | 179 | template inline void 180 | ImgToWorld(cv::Point3_& X, // 3D scene point 181 | const cv::Point_& m) // 2D image point 182 | { 183 | T invAff = c - d*e; 184 | T u_t = m.x - u0; 185 | T v_t = m.y - v0; 186 | // inverse affine matrix image to sensor plane conversion 187 | X.x = (1 * u_t - d * v_t) / invAff; 188 | X.y = (-e * u_t + c * v_t) / invAff; 189 | T X2 = X.x*X.x; 190 | T Y2 = X.y*X.y; 191 | X.z = -horner((T*)p.data, p_deg, sqrt(X2 + Y2)); 192 | 193 | // normalize vectors spherically 194 | T norm = sqrt(X2 + Y2 + X.z*X.z); 195 | X.x /= norm; 196 | X.y /= norm; 197 | X.z /= norm; 198 | } 199 | 200 | template inline void 201 | ImgToWorld(cv::Vec& X, // 3D scene point 202 | const cv::Vec& m) // 2D image point 203 | { 204 | T invAff = c - d*e; 205 | T u_t = m(0) - u0; 206 | T v_t = m(1) - v0; 207 | // inverse affine matrix image to sensor plane conversion 208 | X(0) = (1 * u_t - d * v_t) / invAff; 209 | X(1) = (-e * u_t + c * v_t) / invAff; 210 | T X2 = X(0)*X(0); 211 | T Y2 = X(1)*X(1); 212 | X(2) = -horner((T*)p.data, p_deg, sqrt(X2 + Y2)); 213 | 214 | // normalize vectors spherically 215 | T norm = sqrt(X2 + Y2 + X(2)*X(2)); 216 | X(0) /= norm; 217 | X(1) /= norm; 218 | X(2) /= norm; 219 | } 220 | 221 | // get functions 222 | T Get_c() { return c; } 223 | T Get_d() { return d; } 224 | T Get_e() { return e; } 225 | 226 | T Get_u0() { return u0; } 227 | T Get_v0() { return v0; } 228 | 229 | int GetInvDeg() { return invP_deg; } 230 | int GetPolDeg() { return p_deg; } 231 | 232 | cv::Mat_ Get_invP() { return invP; } 233 | cv::Mat_ Get_P() { return p; } 234 | 235 | T GetWidth() { return Iwidth; } 236 | T GetHeight() { return Iheight; } 237 | 238 | cv::Mat GetMirrorMask(int pyrL) { return mirrorMasks[pyrL]; } 239 | void SetMirrorMasks(std::vector mirrorMasks_) { mirrorMasks = mirrorMasks_; } 240 | 241 | bool isPointInMirrorMask(const T& u, const T& v, int pyr) 242 | { 243 | int ur = cvRound(u); 244 | int vr = cvRound(v); 245 | // check image bounds 246 | if (ur >= mirrorMasks[pyr].cols || ur <= 0 || 247 | vr >= mirrorMasks[pyr].rows || vr <= 0) 248 | return false; 249 | // check mirror 250 | if (mirrorMasks[pyr].at(vr, ur) > 0) 251 | return true; 252 | else return false; 253 | } 254 | 255 | private: 256 | // affin parameters 257 | T c; 258 | T d; 259 | T e; 260 | cv::Mat_ cde1; 261 | // principal point 262 | T u0; 263 | T v0; 264 | // polynomial 265 | cv::Mat_ p; 266 | int p_deg; 267 | // inverse polynomial 268 | cv::Mat_ invP; 269 | int invP_deg; 270 | // image width and height 271 | int Iwidth; 272 | int Iheight; 273 | // mirror mask on pyramid levels 274 | std::vector mirrorMasks; 275 | }; 276 | 277 | 278 | #endif -------------------------------------------------------------------------------- /cpp_model_impl/src/cam_model_general_test.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | #include "cam_model_general.h" 5 | 6 | using namespace std; 7 | using namespace cv; 8 | 9 | double time2double(std::chrono::steady_clock::time_point start, 10 | std::chrono::steady_clock::time_point end) 11 | { 12 | return static_cast( 13 | std::chrono::duration_cast(end - start).count() * (double)1e-9); 14 | } 15 | 16 | double dRand(double fMin, double fMax) 17 | { 18 | return fMin + (double)rand() / RAND_MAX * (fMax - fMin); 19 | } 20 | 21 | int main() 22 | { 23 | // number of iterations for speed test 24 | long iterations = 1e8; 25 | 26 | // take some real world cam model 27 | // this is the camera model of data set Fisheye1_ 28 | // ATTENTION!! I also switched the principal point coordinates 29 | Vec interior_orientation(0.998883018922937, -0.0115128845387445, 30 | 0.0107836324042904, 544.763473297893, 378.781825009886); 31 | Mat_ p = (Mat_(5, 1) << -338.405137634369, 32 | 0.0, 33 | 0.00120189826837736, 34 | - 1.27438189154991e-06, 35 | 2.85466623521256e-09); 36 | // attention: this is the reverse order of findinvpoly 37 | // as matlab evaluates the polynomials differently 38 | Mat_ pInv = (Mat_(11, 1) << 510.979186217526, 39 | 291.393724562448, 40 | -13.8758863124724, 41 | 42.4238251854176, 42 | 23.054291112414, 43 | -7.18539785128328, 44 | 14.1452111052043, 45 | 18.5034196957122, 46 | -2.39675686593404, 47 | -7.18896323060144, 48 | -1.85081569557094); 49 | 50 | // here comes the camera model 51 | cCamModelGeneral camModel(interior_orientation, p, pInv); 52 | 53 | // test the correctness of the implementation, at least internally 54 | double x0 = dRand(0, 5); 55 | double y0 = dRand(0, 5); 56 | double z0 = dRand(0, 5); 57 | 58 | Vec3d vec3d(x0, y0, z0); 59 | Vec3d vec3d_normalized = (1/norm(vec3d)) * vec3d; 60 | Vec2d projection; 61 | Vec3d unprojected; 62 | camModel.WorldToImg(vec3d, projection); 63 | cout << "projected point: " << projection << endl; 64 | 65 | camModel.ImgToWorld(unprojected, projection); 66 | cerr << "difference after unproject: " << norm(vec3d_normalized - unprojected)<< endl; 67 | 68 | std::chrono::steady_clock::time_point begin = std::chrono::steady_clock::now(); 69 | // timings 70 | for (int i = 0; i < iterations; ++i) 71 | { 72 | camModel.WorldToImg(vec3d, projection); 73 | camModel.ImgToWorld(unprojected, projection); 74 | } 75 | std::chrono::steady_clock::time_point end = std::chrono::steady_clock::now(); 76 | cout << "total time for " << iterations << " iterations of world2cam and cam2world: " << time2double(begin, end) << " s"< k; 74 | weight(find(a)) = v(a)./v(a); 75 | weight(find(b)) = k./abs(v(b)); 76 | weight = weight'; 77 | end 78 | 79 | 80 | -------------------------------------------------------------------------------- /src/click_calibUrban.m: -------------------------------------------------------------------------------- 1 | % Steffen Urban email: steffen.urban@kit.edu 2 | % Copyright (C) 2014 Steffen Urban 3 | % 4 | % This program is free software; you can redistribute it and/or modify 5 | % it under the terms of the GNU General Public License as published by 6 | % the Free Software Foundation; either version 2 of the License, or 7 | % (at your option) any later version. 8 | % 9 | % This program is distributed in the hope that it will be useful, 10 | % but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | % MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | % GNU General Public License for more details. 13 | % 14 | % You should have received a copy of the GNU General Public License along 15 | % with this program; if not, write to the Free Software Foundation, Inc., 16 | % 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA. 17 | 18 | % 04.03.2014 by Steffen Urban 19 | % this is a modified file from 20 | % Davide Scaramuzzas Toolbox OcamCalib 21 | % original filename: click_calib.m 22 | 23 | function click_calibUrban(calib_data) 24 | 25 | calib_data.calibrated = 0; %this flag is - when the camera has not yet been calibrated 26 | 27 | if isempty(calib_data.n_ima), 28 | data_calib(calib_data); 29 | end; 30 | 31 | check_active_images(calib_data); 32 | 33 | if isempty(calib_data.I{calib_data.ind_active(1)}), 34 | ima_read_calib(calib_data); 35 | if isempty(calib_data.ind_read), 36 | disp('Cannot extract corners without images'); 37 | return; 38 | end; 39 | end; 40 | 41 | fprintf(1,'\nExtraction of the grid corners on the images\n'); 42 | 43 | if isempty(calib_data.map), calib_data.map = gray(256); end; 44 | 45 | if ~isempty(calib_data.dX), 46 | dX_default = calib_data.dX; 47 | end; 48 | 49 | if ~isempty(calib_data.dY), 50 | dY_default = calib_data.dY; 51 | end; 52 | 53 | if ~isempty(calib_data.n_sq_x), 54 | n_sq_x_default = calib_data.n_sq_x; 55 | end; 56 | 57 | if ~isempty(calib_data.n_sq_y), 58 | n_sq_y_default = calib_data.n_sq_y; 59 | end; 60 | 61 | if ~isempty(calib_data.taylor_order), 62 | calib_data.taylor_order_default = calib_data.taylor_order; 63 | end; 64 | 65 | 66 | if ~exist('dX_default')|~exist('dY_default'); 67 | dX_default = 30; 68 | dY_default = 30; 69 | end; 70 | 71 | if ~exist('n_sq_x_default')|~exist('n_sq_y_default'), 72 | n_sq_x_default = 10; 73 | n_sq_y_default = 10; 74 | end; 75 | 76 | if ~exist('wintx_default')|~exist('winty_default'), 77 | wintx_default = max(round(calib_data.ocam_model.width/128),round(calib_data.ocam_model.height/96)); 78 | winty_default = wintx_default; 79 | clear wintx winty 80 | end; 81 | 82 | if ~exist('xc_default'), 83 | xc_default = round(calib_data.ocam_model.height/2); 84 | end; 85 | 86 | if ~exist('yc_default'), 87 | yc_default = round(calib_data.ocam_model.width/2); 88 | end; 89 | 90 | if ~exist('taylor_order_default') 91 | calib_data.taylor_order_default=4; 92 | end; 93 | 94 | if ~exist('wintx') | ~exist('winty'), 95 | for kk = 1:calib_data.n_ima, 96 | eval(['clear wintx_' num2str(kk)]); 97 | eval(['clear winty_' num2str(kk)]); 98 | end; 99 | 100 | end; 101 | 102 | 103 | 104 | if ~exist('dont_ask'), 105 | dont_ask = 0; 106 | end; 107 | 108 | 109 | if ~isempty(calib_data.ima_proc) 110 | fprintf(1,'\nCurrently, corners have been extracted for the following image(s): %s\n',num2str(calib_data.ima_proc)); 111 | suppress_image = input(' Do you want to suppress the current image(s) ([] = no, other = yes)? ','s'); 112 | if isempty(suppress_image), 113 | ima_numbers = input('Type a vector containing the Images to add (e.g. [1 2 3]) = '); 114 | for i=ima_numbers 115 | if ~isempty(find(calib_data.ima_proc==i)), 116 | fprintf(1,'\nyou have already extracted corners from image %d',i); 117 | replace_image = input(', are you sure you want to replace this image ([] = yes, other = no)?\n','s'); 118 | if isempty(replace_image) 119 | calib_data.ima_proc=calib_data.ima_proc(find(calib_data.ima_proc~=i)); 120 | else 121 | ima_numbers(find(ima_numbers==i))=0; 122 | end 123 | end 124 | end 125 | ima_numbers= ima_numbers(find(ima_numbers~=0)); 126 | else 127 | calib_data.ima_proc=[]; 128 | ima_numbers=[]; 129 | answer = input('\nType the images you want to process (e.g. [1 2 3], [] = all images) = '); 130 | if isempty(answer) 131 | ima_numbers = 1:calib_data.n_ima; 132 | else 133 | ima_numbers=answer; 134 | end 135 | end; 136 | else 137 | answer=input('\nType the images you want to process (e.g. [1 2 3], [] = all images) = '); 138 | if isempty(answer) 139 | ima_numbers = 1:calib_data.n_ima; 140 | else 141 | ima_numbers=answer; 142 | end 143 | end; 144 | 145 | manual_squares=1; 146 | 147 | if manual_squares, 148 | calib_data.n_sq_x = input(['Number of squares along the X direction ([]=' num2str(n_sq_x_default) ') = ']); %6 149 | if isempty(calib_data.n_sq_x), calib_data.n_sq_x = n_sq_x_default; end; 150 | calib_data.n_sq_y = input(['Number of squares along the Y direction ([]=' num2str(n_sq_y_default) ') = ']); %6 151 | if isempty(calib_data.n_sq_y), calib_data.n_sq_y = n_sq_y_default; end; 152 | end; 153 | 154 | num_points=(calib_data.n_sq_x+1)*(calib_data.n_sq_y+1); 155 | 156 | n_sq_x_default = calib_data.n_sq_x; 157 | n_sq_y_default = calib_data.n_sq_y; 158 | 159 | 160 | if (isempty(calib_data.dX))|(isempty(calib_data.dY)), % This question is now asked only once 161 | % Enter the size of each square 162 | 163 | calib_data.dX = input(['Size dX of each square along the X direction ([]=' num2str(dX_default) 'mm) = ']); 164 | calib_data.dY = input(['Size dY of each square along the Y direction ([]=' num2str(dY_default) 'mm) = ']); 165 | if isempty(calib_data.dX), calib_data.dX = dX_default; else dX_default = calib_data.dX; end; 166 | if isempty(calib_data.dY), calib_data.dY = dY_default; else dY_default = calib_data.dY; end; 167 | 168 | else 169 | 170 | fprintf(1,['Size of each square along the X direction: dX=' num2str(calib_data.dX) 'mm\n']); 171 | fprintf(1,['Size of each square along the Y direction: dY=' num2str(calib_data.dY) 'mm (Note: To reset the size of the squares, clear the variables dX and dY)\n']); 172 | %fprintf(1,'Note: To reset the size of the squares, clear the variables dX and dY\n'); 173 | 174 | end; 175 | 176 | square_vert_side=calib_data.dX; %mm 177 | square_horiz_side=calib_data.dY; %mm 178 | num_vert_square=calib_data.n_sq_x; 179 | num_horiz_square=calib_data.n_sq_y; 180 | 181 | calib_data.ocam_model.xc=input(['X coordinate (along height) of the omnidirectional image center = ([]=' num2str(xc_default) ') = ']); 182 | calib_data.ocam_model.yc=input(['Y coordinate (along width) of the omnidirectional image center = ([]=' num2str(yc_default) ') = ']); 183 | if isempty(calib_data.ocam_model.xc), calib_data.ocam_model.xc = xc_default; else xc_default = calib_data.ocam_model.xc; end; 184 | if isempty(calib_data.ocam_model.yc), calib_data.ocam_model.yc = yc_default; else yc_default = calib_data.ocam_model.yc; end; 185 | % xc=385.48; 186 | % yc=516.36; 187 | 188 | 189 | fprintf(1,'\nEXTRACTION OF THE GRID CORNERS\n'); 190 | fprintf(1,'Do you want to use the automatic image selection\n'); 191 | answer=input('or do you want to process the images individually ( [] = automatic, other = individual )? ','s'); 192 | 193 | if isempty(answer) 194 | use_video_mode = 1; 195 | %% ================ 196 | % added code steffen urban 197 | use_corner_find = 1; 198 | %% ================ 199 | else 200 | use_video_mode = 0; 201 | fprintf(1,'Do you want to use the automatic corner extraction\n'); 202 | answer=input('or do you want to extract all the points manually ( [] = automatic, other = manual )? ','s'); 203 | 204 | % If you opted for the AUTOMATIC extraction 205 | if isempty(answer) 206 | use_automatic = 1; 207 | %% ================ 208 | % added code steffen urban 209 | use_corner_find = 1; 210 | %% ================ 211 | else 212 | use_automatic = 0; 213 | end 214 | 215 | fprintf(1,'\n'); 216 | % If you opted for the MANUAL extraction 217 | if use_automatic == 0 %IF manual Extraction 218 | answer=input('Do you want your clicking to be assisted by a corner detector ( [] = yes, other = no )? ','s'); 219 | if isempty(answer) 220 | use_corner_find=1; 221 | disp('Window size for corner finder (wintx and winty): '); 222 | calib_data.wintx = input(['wintx ([] = ' num2str(wintx_default) ') = ']); 223 | if isempty(calib_data.wintx), calib_data.wintx = wintx_default; end; 224 | calib_data.wintx = round(calib_data.wintx); 225 | calib_data.winty = input(['winty ([] = ' num2str(winty_default) ') = ']); 226 | if isempty(calib_data.winty), calib_data.winty = winty_default; end; 227 | winty = round(calib_data.winty); 228 | fprintf(1,'Window size = %dx%d\n',2*calib_data.wintx+1,2*calib_data.winty+1); 229 | 230 | else 231 | %% ================ 232 | % added code steffen urban 233 | use_corner_find = 1; 234 | %% ================ 235 | end 236 | end 237 | end 238 | 239 | 240 | 241 | %Arranging the pixel of the world 242 | calib_data.Xt=[]; 243 | calib_data.Yt=[]; 244 | for i=0:calib_data.n_sq_x 245 | for j=0:calib_data.n_sq_y 246 | calib_data.Yt=[calib_data.Yt;j*calib_data.dY]; 247 | calib_data.Xt=[calib_data.Xt;i*calib_data.dX]; 248 | end 249 | end 250 | 251 | 252 | if use_video_mode == 0 253 | for kk = ima_numbers, 254 | if ~isempty(calib_data.I{kk}) 255 | if use_automatic == 0 %IF manual extraction 256 | click_ima_calib(kk,use_corner_find,calib_data); 257 | else %IF automatic extraction 258 | click_ima_calib_rufli(kk,use_corner_find,calib_data); 259 | %click_ima_calib_vladimir 260 | end 261 | calib_data.active_images(kk) = 1; 262 | calib_data.ima_proc= sort([calib_data.ima_proc, kk]); 263 | end; 264 | end; 265 | else 266 | count = 0; 267 | for kk = ima_numbers 268 | 269 | [callBack, x, y] = get_checkerboard_cornersUrban(kk,use_corner_find,calib_data); 270 | 271 | if callBack == 1 272 | count = count + 1; 273 | calib_data.Xp_abs(:,:,kk) = x; 274 | calib_data.Yp_abs(:,:,kk) = y; 275 | calib_data.active_images(kk) = 1; 276 | calib_data.ima_proc= sort([calib_data.ima_proc, kk]); 277 | end 278 | end 279 | end 280 | 281 | check_active_images(calib_data); 282 | 283 | fprintf(1,'\nCorner extraction finished.\n'); 284 | 285 | end 286 | 287 | 288 | -------------------------------------------------------------------------------- /src/errCenterUrban.m: -------------------------------------------------------------------------------- 1 | % Steffen Urban email: steffen.urban@kit.edu 2 | % Copyright (C) 2014 Steffen Urban 3 | % 4 | % This program is free software; you can redistribute it and/or modify 5 | % it under the terms of the GNU General Public License as published by 6 | % the Free Software Foundation; either version 2 of the License, or 7 | % (at your option) any later version. 8 | % 9 | % This program is distributed in the hope that it will be useful, 10 | % but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | % MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | % GNU General Public License for more details. 13 | % 14 | % You should have received a copy of the GNU General Public License along 15 | % with this program; if not, write to the Free Software Foundation, Inc., 16 | % 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA. 17 | 18 | % 04.03.2014 by Steffen Urban 19 | % error function for the center of distortion search 20 | 21 | function [error] = errCenterUrban(x, calib_data) 22 | 23 | error = 0; 24 | xc = x(1); 25 | yc = x(2); 26 | % call calibration function 27 | [RRfin, ss] = calibrate(calib_data.Xt, calib_data.Yt, calib_data.Xp_abs, ... 28 | calib_data.Yp_abs, xc,yc, ... 29 | calib_data.taylor_order, ... 30 | calib_data.ima_proc); 31 | lauf = 1; 32 | M = [calib_data.Xt,calib_data.Yt,ones(size(calib_data.Xt))]; 33 | 34 | for i = 1:size(RRfin,3) 35 | % if calibration was not possible add a high value 36 | % to penalize the minimization away from that point 37 | if calib_data.RRfin(:,:,i)==0 38 | error= error+sum(ones(length(calib_data.Xp_abs),1)*sqrt( (calib_data.ocam_model.width/2)^2 + (calib_data.ocam_model.height/2)^2)); 39 | else 40 | Mc = RRfin(:,:,i)*M'; 41 | Xpp=calib_data.Xp_abs(:,:,i); 42 | Ypp=calib_data.Yp_abs(:,:,i); 43 | [xp1,yp1] = omni3d2pixel(ss, Mc, calib_data.ocam_model.width, calib_data.ocam_model.height); 44 | if (isinf(xp1) | isinf(yp1)) 45 | error = error+sum(ones(length(calib_data.Xp_abs),1)*sqrt( (calib_data.ocam_model.width/2)^2 + (calib_data.ocam_model.height/2)^2)); 46 | else 47 | xp = xp1 + xc; 48 | yp = yp1 + yc; 49 | lauf = lauf+length(Xpp); 50 | error = error + sum((Xpp-xp').^2) + sum((Ypp-yp').^2); 51 | end 52 | 53 | end 54 | end 55 | error = sqrt(error / lauf); 56 | 57 | end 58 | 59 | -------------------------------------------------------------------------------- /src/eulerFromR.m: -------------------------------------------------------------------------------- 1 | % this function extracts euler angles from a rotation matrix R 2 | % Steffen Urban email: steffen.urban@kit.edu 3 | % Copyright (C) 2014 Steffen Urban 4 | % 5 | % This program is free software: you can redistribute it and/or modify 6 | % it under the terms of the GNU General Public License as published by 7 | % the Free Software Foundation, either version 3 of the License, or 8 | % (at your option) any later version. 9 | % 10 | % This program is distributed in the hope that it will be useful, 11 | % but WITHOUT ANY WARRANTY; without even the implied warranty of 12 | % MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 13 | % GNU General Public License for more details. 14 | % 15 | % You should have received a copy of the GNU General Public License 16 | % along with this program. If not, see . 17 | 18 | % this function extracts euler angles from a rotation matrix R 19 | function psithetaphi = eulerFromR(R,plusPI) 20 | if nargin < 2 21 | plusPI=0; 22 | end 23 | 24 | psi2=0; 25 | theta2=0; 26 | phi2 =0; 27 | if (R(3,1)~=1 && R(3,1)~=-1) 28 | theta1=-asin(R(3,1)); 29 | theta2=pi-theta1; 30 | 31 | psi1=atan2(R(3,2)/cos(theta1),R(3,3)/cos(theta1)); 32 | psi2=atan2(R(3,2)/cos(theta2),R(3,3)/cos(theta2)); 33 | phi1=atan2(R(2,1)/cos(theta1),R(1,1)/cos(theta1)); 34 | phi2=atan2(R(2,1)/cos(theta2),R(1,1)/cos(theta2)); 35 | 36 | psi = psi1; 37 | theta = theta1; 38 | phi = phi1; 39 | else 40 | phi=0; 41 | if R(3,1)== -1 42 | theta=pi/2; 43 | psi=phi+atan2(R(1,2),R(1,3)); 44 | else 45 | theta = -pi/2; 46 | psi = -phi+atan2(-R(1,2),-R(1,3)); 47 | end 48 | end 49 | if plusPI 50 | psithetaphi = [psi2, theta2, phi2]; 51 | else 52 | psithetaphi = [psi, theta, phi]; 53 | end 54 | 55 | end 56 | 57 | 58 | -------------------------------------------------------------------------------- /src/findcenterUrban.m: -------------------------------------------------------------------------------- 1 | % Steffen Urban email: steffen.urban@kit.edu 2 | % Copyright (C) 2014 Steffen Urban 3 | % 4 | % This program is free software; you can redistribute it and/or modify 5 | % it under the terms of the GNU General Public License as published by 6 | % the Free Software Foundation; either version 2 of the License, or 7 | % (at your option) any later version. 8 | % 9 | % This program is distributed in the hope that it will be useful, 10 | % but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | % MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | % GNU General Public License for more details. 13 | % 14 | % You should have received a copy of the GNU General Public License along 15 | % with this program; if not, write to the Free Software Foundation, Inc., 16 | % 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA. 17 | 18 | % 04.03.2014 by Steffen Urban 19 | % this is a modified file from 20 | % Davide Scaramuzzas Toolbox OcamCalib 21 | % original filename: findcenter.m 22 | 23 | function findcenterUrban(calib_data) 24 | 25 | if isempty(calib_data.ima_proc) | isempty(calib_data.Xp_abs) 26 | fprintf(1,'\nNo corner data available. Extract grid corners before calibrating.\n\n'); 27 | return; 28 | end 29 | 30 | 31 | fprintf(1,'\nComputing center coordinates.\n\n'); 32 | 33 | if isempty(calib_data.taylor_order), 34 | calib_data.taylor_order = calib_data.taylor_order_default; 35 | end 36 | 37 | %% ================ 38 | % added code 39 | options = optimset('Display','off','MaxIter',10000,'LargeScale','off'); 40 | x0 = [calib_data.ocam_model.xc, calib_data.ocam_model.yc]; 41 | 42 | [x0,~,~,~] = fminsearch(@errCenterUrban, x0, options, calib_data); 43 | 44 | calib_data.ocam_model.xc = x0(1); 45 | calib_data.ocam_model.yc = x0(2); 46 | calib_data.ocam_model.c=1; 47 | calib_data.ocam_model.d=0; 48 | calib_data.ocam_model.e=0; 49 | 50 | %% do calibration again 51 | [calib_data.RRfin,calib_data.ocam_model.ss] = calibrate(calib_data.Xt, calib_data.Yt, calib_data.Xp_abs, calib_data.Yp_abs, calib_data.ocam_model.xc, calib_data.ocam_model.yc, calib_data.taylor_order, calib_data.ima_proc); 52 | calib_data.calibrated = 1; %This flag i s1 when the camera has been calibrated 53 | 54 | % reproject 55 | M=[calib_data.Xt,calib_data.Yt,zeros(size(calib_data.Xt))]; 56 | [allerr,rms] = reprojectpoints_advUrban(calib_data.ocam_model, calib_data.RRfin, calib_data.ima_proc, calib_data.Xp_abs, calib_data.Yp_abs, M); 57 | calib_data.rmsAfterCenter = rms; 58 | 59 | ss = calib_data.ocam_model.ss; 60 | ss 61 | 62 | figure(3); 63 | set(3,'Name','Calibration results','NumberTitle','off'); 64 | subplot(2,1,1); 65 | plot(0:floor(calib_data.ocam_model.width/2),polyval([ss(end:-1:1)],[0:floor(calib_data.ocam_model.width/2)])); grid on; axis equal; 66 | xlabel('Distance ''rho'' from the image center in pixels'); 67 | ylabel('f(rho)'); 68 | title('Forward projection function'); 69 | % 70 | subplot(2,1,2); 71 | plot(0:floor(calib_data.ocam_model.width/2),180/pi*atan2(0:floor(calib_data.ocam_model.width/2),-polyval([ss(end:-1:1)],[0:floor(calib_data.ocam_model.width/2)]))-90); grid on; 72 | xlabel('Distance ''rho'' from the image center in pixels'); 73 | ylabel('Degrees'); 74 | title('Angle of optical ray as a function of distance from circle center (pixels)'); 75 | 76 | calib_data.calibrated = 1; %This flag is 1 when the camera has been calibrated 77 | 78 | -------------------------------------------------------------------------------- /src/findinvpolyUrban.m: -------------------------------------------------------------------------------- 1 | %FINDINVPOLY finds the inverse polynomial specified in the argument. 2 | % [POL, ERR, N] = FINDINVPOLY(SS, RADIUS, N) finds an approximation of the inverse polynomial specified in OCAM_MODEL.SS. 3 | % The returned polynomial POL is used in WORLD2CAM_FAST to compute the reprojected point very efficiently. 4 | % 5 | % SS is the polynomial which describe the mirrror/lens model. 6 | % RADIUS is the radius (pixels) of the omnidirectional picture. 7 | % ERR is the error (pixel) that you commit in using the returned 8 | % polynomial instead of the inverse SS. N is searched so that 9 | % that ERR is < 0.01 pixels. 10 | % 11 | % Copyright (C) 2008 DAVIDE SCARAMUZZA, ETH Zurich 12 | % Author: Davide Scaramuzza - email: davide.scaramuzza@ieee.org 13 | 14 | function [pol, err, N] = findinvpolyUrban(ss, I) 15 | 16 | disp('choose about 10 points along the border of the visible image region! Press enter if finished') 17 | figure 18 | imshow(uint8(I)); 19 | [x, y] =ginput; 20 | X=[x' 21 | y']; 22 | 23 | [~, A, B, ~] = fitellipse(X); 24 | radius = mean([A,B]); 25 | 26 | if nargin < 3 27 | maxerr = inf; 28 | N = 1; 29 | while maxerr > 0.01 %Repeat until the reprojection error is smaller than 0.01 pixels 30 | N = N + 1; 31 | [pol, err,N] = findinvpoly2(ss, radius, N); 32 | maxerr = max(err); 33 | end 34 | else 35 | [pol, err, N] = findinvpoly2(ss, radius, N); 36 | end 37 | 38 | function [pol, err, N, r] = findinvpoly2(ss, radius, N) 39 | 40 | theta = -pi/2:0.01:1.4; 41 | r = invFUN(ss, theta, radius); 42 | ind = find(r~=inf); 43 | theta = theta(ind); 44 | r = r(ind); 45 | 46 | pol = polyfit(theta,r,N); 47 | 48 | err = abs( r - polyval(pol, theta)); %approximation error in pixels 49 | 50 | 51 | function r=invFUN(ss, theta, radius) 52 | 53 | m=tan(theta); 54 | 55 | r=[]; 56 | poly_coef=ss(end:-1:1); 57 | poly_coef_tmp=poly_coef; 58 | for j=1:length(m) 59 | poly_coef_tmp(end-1)=poly_coef(end-1)-m(j); 60 | rhoTmp=roots(poly_coef_tmp); 61 | res=rhoTmp(find(imag(rhoTmp)==0 & rhoTmp>0 & rhoTmp1 63 | r(j)=inf; 64 | else 65 | r(j)=res; 66 | end 67 | end -------------------------------------------------------------------------------- /src/get_checkerboard_cornersUrban.m: -------------------------------------------------------------------------------- 1 | % Steffen Urban email: steffen.urban@kit.edu 2 | % Copyright (C) 2014 Steffen Urban 3 | % 4 | % This program is free software; you can redistribute it and/or modify 5 | % it under the terms of the GNU General Public License as published by 6 | % the Free Software Foundation; either version 2 of the License, or 7 | % (at your option) any later version. 8 | % 9 | % This program is distributed in the hope that it will be useful, 10 | % but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | % MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | % GNU General Public License for more details. 13 | % 14 | % You should have received a copy of the GNU General Public License along 15 | % with this program; if not, write to the Free Software Foundation, Inc., 16 | % 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA. 17 | 18 | % 04.03.2014 by Steffen Urban 19 | % this is a modified file from 20 | % Davide Scaramuzzas Toolbox OcamCalib 21 | % filename: get_checkerboard_corners.m 22 | % Code was changed to force subpixel corners 23 | 24 | function [callBack, Xp_abs, Yp_abs] = get_checkerboard_cornersUrban(kk, use_corner_find, calib_data) 25 | 26 | fclose('all'); 27 | if exist('autoCornerFinder/cToMatlab/cornerInfo.txt','file') 28 | delete('autoCornerFinder/cToMatlab/cornerInfo.txt'); 29 | end 30 | if exist('autoCornerFinder/cToMatlab/cornersX.txt','file') 31 | delete('autoCornerFinder/cToMatlab/cornersX.txt'); 32 | end 33 | if exist('autoCornerFinder/cToMatlab/cornersY.txt','file') 34 | delete('autoCornerFinder/cToMatlab/cornersY.txt'); 35 | end 36 | if exist('autoCornerFinder/cToMatlab/error.txt','file') 37 | delete('autoCornerFinder/cToMatlab/error.txt'); 38 | end 39 | 40 | %INITIALIZATIONS 41 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 42 | fprintf(1,'\nProcessing image %s...',calib_data.L{kk}); 43 | 44 | I = imread(calib_data.L{kk}); 45 | 46 | Xp_abs = 0; 47 | Yp_abs = 0; 48 | 49 | %Tell the automatic corner extractor, which image file to process 50 | fid = fopen('./autoCornerFinder/pictures.txt','w'); 51 | fprintf(fid,'../%s',calib_data.L{kk}); 52 | fclose(fid); 53 | 54 | %Call the automatic corner extraction algorithm 55 | %Width and height in the algorithm are defined differently than 56 | %in this toolbox: Its the number of inernal corners instead of 57 | %internal quadrangles. 58 | %-m specifies the number of corners the algorithm has to find, before it 59 | %terminates 60 | %If callback = -1 ->An error occured, automatic corner finding is aborted 61 | % = 0 ->Not enough corners have been found, add some manually 62 | % = 1 ->Enough corners have been found for calibration 63 | 64 | 65 | %Visualization turned OFF 66 | cd autoCornerFinder; 67 | 68 | callString = (['FindCorners.exe -w ' num2str(calib_data.n_sq_x+1) ' -h ' num2str(calib_data.n_sq_y+1) ' -m ' num2str((calib_data.n_sq_x+1) * (calib_data.n_sq_y+1)) ' pictures.txt']); 69 | 70 | if ~ispc %if not Windows 71 | callString = ['./' callString]; 72 | end 73 | %Visualization turned ON 74 | %callString = (['cd autoCornerFinder & FindCornersVisual.exe -w ' num2str(n_sq_x+1) ' -h ' num2str(n_sq_y+1) ' -m ' num2str((n_sq_x+1) * (n_sq_y+1)) ' pictures.txt']); 75 | 76 | %Visualization turned ON and Saving of the images turned ON 77 | %WARNING: Does somehow not work under Windows 2000... 78 | %callString = (['cd autoCornerFinder & FindCornersVisualSave.exe -w ' num2str(n_sq_x+1) ' -h ' num2str(n_sq_y+1) ' -m ' num2str((n_sq_x+1) * (n_sq_y+1)) ' pictures.txt']); 79 | 80 | callBack = system(callString); 81 | cd .. 82 | %Do error checking 83 | if callBack == 0 84 | %Display the error message 85 | fprintf(1,'Image omitted -- Not all corners found\n'); 86 | return; 87 | end 88 | 89 | if callBack ~= 1 90 | %Display the error message 91 | filename = 'autoCornerFinder/cToMatlab/error.txt'; 92 | fid = fopen(filename, 'r'); 93 | line = fgetl(fid); 94 | fclose(fid); 95 | fprintf(1,'Image omitted -- During corner finding an error occured: %s \n',line); 96 | return; 97 | end 98 | 99 | %Open the corner size information file 100 | filename = 'autoCornerFinder/cToMatlab/cornerInfo.txt'; 101 | fid = fopen(filename, 'r'); 102 | cornerInfo = fscanf(fid, '%g %g', [1 2]); 103 | fclose(fid); 104 | 105 | %Open the files with the found corners 106 | filename = 'autoCornerFinder/cToMatlab/cornersX.txt'; 107 | fid = fopen(filename, 'r'); 108 | cornersX = fscanf(fid, '%g %g', [cornerInfo(2) cornerInfo(1)]); 109 | cornersX = cornersX'; 110 | fclose(fid); 111 | 112 | filename = 'autoCornerFinder/cToMatlab/cornersY.txt'; 113 | fid = fopen(filename, 'r'); 114 | cornersY = fscanf(fid, '%g %g', [cornerInfo(2) cornerInfo(1)]); 115 | cornersY = cornersY'; 116 | fclose(fid); 117 | 118 | %VARIABLE DEFINITIONS 119 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 120 | numCorners = (calib_data.n_sq_x+1)*(calib_data.n_sq_y+1); 121 | numCornerThreshold = (calib_data.n_sq_x+1)*(calib_data.n_sq_y+1); 122 | numOfFoundCorners = 0; 123 | cornerNumber = 0; 124 | cornersNumbering = -1*ones(cornerInfo(1),cornerInfo(2)); 125 | startingCorner = []; 126 | nextFoundCorner = [-1,-1]; 127 | deltaCols = 0; 128 | startingCornerSet = false; 129 | %Index of the corner with the smallest index 130 | min_i = []; 131 | min_j = []; 132 | %Used for image zoom in 133 | [size1, size2] = size(I); 134 | min_x = max(size1, size2) + 1; 135 | max_x = 0; 136 | min_y = max(size1, size2) + 1; 137 | max_y = 0; 138 | 139 | 140 | %INPUT CORNER VALUE AND MATRIX SIZE ADAPTATIONS 141 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 142 | %If there are entire rows of zeros at the end of the file, this means that 143 | %the cornerfinder is not entirely shure about the location of the board. 144 | %Then eventually additional corners are needed in the front. 145 | [iSave, dummy] = find(cornersX >= 0); 146 | iSave = max(iSave); 147 | %Add at least one row! This is because the columns are maybe ambiguous, 148 | %and then we don't know whether they belong to the current row or to the 149 | %next one... 150 | if (cornerInfo(1) - iSave >= 0) 151 | cornersX = [-1* ones(cornerInfo(1) - iSave + 1, cornerInfo(2)); cornersX]; 152 | cornersY = [-1* ones(cornerInfo(1) - iSave + 1, cornerInfo(2)); cornersY]; 153 | end 154 | [rows, cols] = size(cornersX); 155 | 156 | %Add one pixel to every non "-1" value, since Matlab starts numbering 157 | %at one, whereas c++ starts at zero. 158 | flagStart = true; 159 | for i = 1:1:rows 160 | for j = 1:1:cols 161 | %Define the starting corner as the first found corner 162 | %which has a neighbor corner which was also found 163 | if j ~= cols && flagStart == true 164 | if cornersX(i,j) >= 0 && cornersX(i,j+1) >= 0 165 | startingCorner = [i,j]; 166 | flagStart = false; 167 | end 168 | end 169 | if cornersX(i,j) >= 0 170 | cornersX(i,j) = cornersX(i,j) + 1; 171 | %Count the number of found corners 172 | numOfFoundCorners = numOfFoundCorners + 1; 173 | %Determine the minimal and maximal cordinates of all 174 | %found corners. ->Needed further down 175 | if cornersX(i,j) > max_x 176 | max_x = cornersX(i,j); 177 | end 178 | if cornersX(i,j) < min_x 179 | min_x = cornersX(i,j); 180 | end 181 | end 182 | if cornersY(i,j) >= 0 183 | cornersY(i,j) = cornersY(i,j) + 1; 184 | if cornersY(i,j) > max_y 185 | max_y = cornersY(i,j); 186 | end 187 | if cornersY(i,j) < min_y 188 | min_y = cornersY(i,j); 189 | end 190 | end 191 | end 192 | end 193 | 194 | 195 | %PREPARATIONS FOR PROPER PLOT ZOOM-IN 196 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 197 | min_x = min_x - (max_x - min_x)*(1 - numOfFoundCorners/numCorners + 0.2); 198 | max_x = max_x + (max_x - min_x)*(1 - numOfFoundCorners/numCorners + 0.2); 199 | min_y = min_y - (max_y - min_y)*(1 - numOfFoundCorners/numCorners + 0.2); 200 | max_y = max_y + (max_y - min_y)*(1 - numOfFoundCorners/numCorners + 0.2); 201 | 202 | min_x = max(min_x,0); 203 | max_x = min(max_x,size2); 204 | min_y = max(min_y,0); 205 | max_y = min(max_y,size1); 206 | 207 | 208 | % %ORIENTATION AMBIGUITY? 209 | % %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 210 | % %Decide whether the position and orientation of the checkerboard 211 | % %can be unambiguously determined 212 | % if min((n_sq_y+1), (n_sq_x+1)) ~= min(cornerInfo(1),cornerInfo(2)) 213 | % if flagStart == true 214 | % %DISPLAY AN ERROR AND RETURN 215 | % end 216 | % %There is some ambiguity, get rid of it 217 | % figure(2); 218 | % imagesc(I); 219 | % colormap(gray); 220 | % set(2,'color',[1 1 1]); 221 | % title({['Image ' num2str(kk)]}); 222 | % h = get(gca, 'title'); 223 | % set(h, 'FontWeight', 'bold') 224 | % axis([min_x max_x min_y max_y]); 225 | % 226 | % 227 | % figure(2); hold on; 228 | % i = startingCorner(1) 229 | % j = startingCorner(2) 230 | % %Plot the starting corner and its neighbor 231 | % plot( cornersX(i,j),cornersY(i,j),'+','color','red','linewidth',2); 232 | % plot( cornersX(i,j+1),cornersY(i,j+1),'+','color','red','linewidth',2); 233 | % text( cornersX(i,j)+3,cornersY(i,j)+3,num2str(0) ) 234 | % text( cornersX(i,j+1)+3,cornersY(i,j+1)+3,num2str(1) ) 235 | % set(findobj('type', 'text'), 'color', 'red'); 236 | % hold off; 237 | % 238 | % %Get user input on behalf of the board orientation 239 | % numCornersDirZeroOne = input(['The automatic corner finder was not able to decide how the pattern is oriented. Please indicate the number of corners in direction of corners [0 -> 1]: ']); 240 | % %We look in row direction 241 | % deltaCols = cornerInfo(2) - numCornersDirZeroOne; 242 | % end 243 | 244 | 245 | %DRAW AND NUMBER FOUND CORNERS 246 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 247 | %Draw the found corners onto the image and number them 248 | %Define the first encountered found corner as number "zero" 249 | PlotXxiX = []; 250 | PlotXxiY = []; 251 | PlotCornersX = []; 252 | PlotCornersY = []; 253 | PlotCornerNumber = []; 254 | 255 | for i = 1:1:rows 256 | for j = 1:1:(cols-deltaCols) 257 | if cornersX(i,j) ~= -1 258 | %Save for plotting later 259 | PlotCornersX = [PlotCornersX,cornersX(i,j)]; 260 | PlotCornersY = [PlotCornersY,cornersY(i,j)]; 261 | PlotCornerNumber = [PlotCornerNumber, cornerNumber]; 262 | if use_corner_find 263 | %Apply the corner finder 264 | % [xxi] = cornerfinder([cornersX(i,j);cornersY(i,j)],I,winty,wintx); 265 | %% ================ 266 | % added(changed code steffen urban 267 | wintx = ceil(calib_data.ocam_model.height/100); 268 | winty = ceil(calib_data.ocam_model.height/100); 269 | [xxi] = cornerfinder([cornersX(i,j);cornersY(i,j)], single(I), winty, wintx); 270 | 271 | cornersX(i,j) = xxi(1); 272 | cornersY(i,j) = xxi(2); 273 | 274 | %% ================ 275 | %Save for plotting later 276 | PlotXxiX = [PlotXxiX,xxi(1)]; 277 | PlotXxiY = [PlotXxiY,xxi(2)]; 278 | end 279 | if cornerNumber == 0 280 | %Change starting corner to this one 281 | %Needed further down 282 | startingCorner = [i,j]; 283 | startingCornerSet = true; 284 | end 285 | end 286 | if startingCornerSet == true 287 | cornerNumber = cornerNumber + 1; 288 | end 289 | end 290 | end 291 | 292 | if 0 293 | figure(2); 294 | imagesc(I); 295 | colormap(gray); 296 | set(2,'color',[1 1 1]); 297 | title({['Image ' num2str(kk)]; [num2str(numOfFoundCorners) ' / ' num2str(numCorners) ' corner have been found.']; ['Press ENTER to continue.']}); 298 | h = get(gca, 'title'); 299 | set(h, 'FontWeight', 'bold') 300 | axis([min_x max_x min_y max_y]) 301 | 302 | 303 | %Plot the original corners 304 | figure(2); hold on; 305 | plot( PlotCornersX,PlotCornersY,'+','color','red','linewidth',2); 306 | if use_corner_find 307 | %Plot the "corner finder enhanced" corners 308 | plot( PlotXxiX,PlotXxiY,'+','color','blue','linewidth',2); 309 | end 310 | 311 | text( PlotCornersX'+3,PlotCornersY'+3,num2str(PlotCornerNumber') ) 312 | set(findobj('type', 'text'), 'color', 'red'); 313 | drawnow; 314 | hold off 315 | end 316 | %If no negative corners were designated, then "min_i" and "min_j" 317 | %are still empty 318 | if isempty(min_i) 319 | min_i = startingCorner(1); 320 | min_j = startingCorner(2); 321 | end 322 | 323 | 324 | %SAVE CORNER INFORMATION IN 2 VECTORS 325 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 326 | %Save all corners in two arrays for further processing 327 | %by other functions 328 | x = []; 329 | y = []; 330 | 331 | %Start with the smallest found corner and then append the larger ones 332 | iteration = 0; 333 | while true 334 | %Iterators 335 | i = min_i + floor(iteration/(cols-deltaCols)); 336 | j = mod( (min_j - 1 + iteration),cols-deltaCols ) + 1; 337 | iteration = iteration + 1; 338 | 339 | x = [x,cornersX(i,j)]; 340 | y = [y,cornersY(i,j)]; 341 | 342 | if(iteration >= numCorners | i > rows) 343 | break 344 | end 345 | end 346 | 347 | 348 | 349 | % %DOES NOT WORK RELIABLY! 350 | % %3. In case of a n x m board, where n is even and m is odd (or vice 351 | % %versa) there still exists a 180 degree ambiguity. This could be get 352 | % %rid off here. 353 | % %We define the starting corner as the corner where "a black checker is 354 | % %outernmost". Only proceed if one dimension is odd and the other even! 355 | % if (mod(n_cor_min,2) ~= 0 & mod(n_cor_max,2) == 0) | (mod(n_cor_min,2) == 0 & mod(n_cor_max,2) ~= 0) 356 | % %Determine the intensity at the given locations in the image 357 | % [dummy,lengthl] = size(x); 358 | % intens1 = I( floor((y(1,1)+y(1,n_cor_max+2))/2), floor((x(1,1)+x(1,n_cor_max+2))/2) ) 359 | % intens2 = I( floor((y(1,lengthl)+y(1,lengthl-n_cor_max-1))/2), floor((x(1,lengthl)+x(1,lengthl-n_cor_max-2))/2) ) 360 | % I(10,10) 361 | % if intens1 > intens2 362 | % %We need to reverse the numbering 363 | % xTemp = x; 364 | % yTemp = y; 365 | % for i = 1:1:lengthl 366 | % x(i) = xTemp(lengthl+1 - i); 367 | % y(i) = yTemp(lengthl+1 - i); 368 | % end 369 | % end 370 | % end 371 | 372 | 373 | 374 | %REPOSITION CORNERS (IF NEEDED) 375 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 376 | %Allow the user (if needed) to reposition any of the corners 377 | [dummy,sizex] = size(x); 378 | iNumber = 1:1:sizex; 379 | 380 | 381 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 382 | % Check, whether the numbering increases along the longer or the shorter pattern dimention: 383 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 384 | n_cor_min = min(calib_data.n_sq_x+1, calib_data.n_sq_y+1); 385 | n_cor_max = max(calib_data.n_sq_x+1, calib_data.n_sq_y+1); 386 | 387 | hmin = zeros(1,n_cor_max*n_cor_min-1); 388 | hmin(1:n_cor_min:end) = 1; 389 | hmax = zeros(1,n_cor_max*n_cor_min-1); 390 | hmax(1:n_cor_max:end) = 1; 391 | 392 | dxy = sqrt(diff(x).^2+diff(y).^2); 393 | 394 | pmin = conv(hmin,dxy); 395 | pmin = max(pmin(:)); 396 | pmax=conv(hmax,dxy); 397 | pmax = max(pmax(:)); 398 | if pmin(1)>pmax(1) 399 | inc_dir = n_cor_min; 400 | oth_dir = n_cor_max; 401 | else 402 | inc_dir = n_cor_max; 403 | oth_dir = n_cor_min; 404 | end 405 | 406 | % Rearrange numbering from starting point 407 | xTemp = x; 408 | yTemp = y; 409 | area = []; 410 | for i = 1:length(x)-1 411 | 412 | xborder = [xTemp(1,1:inc_dir-1),xTemp(1,inc_dir:inc_dir:end-inc_dir),xTemp(1,end:-1:end-inc_dir+2),xTemp(1,end-inc_dir+1:-inc_dir:1)]; 413 | yborder = [yTemp(1,1:inc_dir-1),yTemp(1,inc_dir:inc_dir:end-inc_dir),yTemp(1,end:-1:end-inc_dir+2),yTemp(1,end-inc_dir+1:-inc_dir:1)]; 414 | area(i) = abs(trapz(xborder,yborder)); 415 | xTemp = [xTemp(1,2:end),xTemp(1,1)]; %Shift points one step forward 416 | yTemp = [yTemp(1,2:end),yTemp(1,1)]; %Shift points one step forward 417 | end 418 | shift = find( area == max(area) ) - 1; 419 | if shift > 0 420 | x = [x(1,1+shift:end) , x(1,1:shift)]; 421 | y = [y(1,1+shift:end) , y(1,1:shift)]; 422 | end 423 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 424 | % Visualize 425 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 426 | if 0 427 | figure(2); 428 | imagesc(I); 429 | colormap(gray); 430 | hold on; 431 | %Plot the corners 432 | plot( x,y,'+','color','red','linewidth',2); 433 | %Plot the corresponding number 434 | text( x'+3,y'+3,num2str(iNumber') ) 435 | set(findobj('type', 'text'), 'color', 'red'); 436 | axis([min_x max_x min_y max_y]); 437 | end 438 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 439 | 440 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 441 | %ASSIGN STARTING CORNER 442 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 443 | %This algorithm was first designed to be used with square patterns only, 444 | %where the starting corner is meaningless, since every orientation +n*90 445 | %degrees is structurally equivalent. 446 | %With the extension to non-square checker boards, this is no longer the 447 | %case and, depending on the specific board, 2 or 4 orientations can be 448 | %distinguished. 449 | %1. Check, whether we are dealing with a non-square board: 450 | if calib_data.n_sq_x ~= calib_data.n_sq_y 451 | %2. Check, whether the numbering increases along the longer pattern 452 | %dimention: 453 | % n_cor_min = min(n_sq_x+1, n_sq_y+1); 454 | % n_cor_max = max(n_sq_x+1, n_sq_y+1); 455 | % dist1 = (x(1,n_cor_min)-x(1,n_cor_min+1))^2 + (y(1,n_cor_min)-y(1,n_cor_min+1))^2; 456 | % dist2 = (x(1,n_cor_max)-x(1,n_cor_max+1))^2 + (y(1,n_cor_max)-y(1,n_cor_max+1))^2; 457 | 458 | n_cor_x = calib_data.n_sq_x+1; 459 | n_cor_y = calib_data.n_sq_y+1; 460 | dist1 = (x(1,n_cor_x)-x(1,n_cor_x+1))^2 + (y(1,n_cor_x)-y(1,n_cor_x+1))^2; 461 | dist2 = (x(1,n_cor_y)-x(1,n_cor_y+1))^2 + (y(1,n_cor_y)-y(1,n_cor_y+1))^2; 462 | 463 | if dist1 > dist2 464 | %We have it wrongly numbered, renumber 465 | xTemp = x; 466 | yTemp = y; 467 | [dummy,lengthl] = size(x); 468 | iterMult = n_cor_x; 469 | iterOffset = 0; 470 | for i = 1:1:lengthl 471 | j = mod(i-1,n_cor_y)+1; 472 | x(i) = xTemp(j*iterMult-iterOffset); 473 | y(i) = yTemp(j*iterMult-iterOffset); 474 | if j*iterMult > n_cor_x*(n_cor_y-1) 475 | iterOffset = iterOffset + 1; 476 | end 477 | end 478 | end 479 | end 480 | 481 | 482 | Yp_abs = x'; 483 | Xp_abs = y'; 484 | 485 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 486 | % Visualize 487 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 488 | if 1 489 | figure(2); 490 | imagesc(I); 491 | colormap(gray); 492 | title(['Image ' num2str(kk)]); 493 | h = get(gca, 'title'); 494 | set(h, 'FontWeight', 'bold') 495 | hold on; 496 | %Plot the corners 497 | plot( x,y,'+','color','red','linewidth',2); 498 | %Plot the corresponding number 499 | text( x'+3,y'+3,num2str(iNumber') ) 500 | set(findobj('type', 'text'), 'color', 'red'); 501 | axis([min_x max_x min_y max_y]); 502 | draw_axes(Xp_abs, Yp_abs,calib_data.n_sq_y); 503 | drawnow; 504 | hold off; 505 | % close(2); 506 | end 507 | 508 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 509 | %DELETE FILES 510 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 511 | %We delete the interface files between Matlab and c++, in order to prevent 512 | %reloading old data in case of some errors. 513 | delete('autoCornerFinder/cToMatlab/cornerInfo.txt'); 514 | delete('autoCornerFinder/cToMatlab/cornersX.txt'); 515 | delete('autoCornerFinder/cToMatlab/cornersY.txt'); 516 | delete('autoCornerFinder/cToMatlab/error.txt'); 517 | 518 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 519 | %Display finished message 520 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 521 | fprintf(1,'Done\n'); 522 | 523 | 524 | -------------------------------------------------------------------------------- /src/ocam_calibUrban.m: -------------------------------------------------------------------------------- 1 | % Steffen Urban email: steffen.urban@kit.edu 2 | % Copyright (C) 2014 Steffen Urban 3 | % 4 | % This program is free software; you can redistribute it and/or modify 5 | % it under the terms of the GNU General Public License as published by 6 | % the Free Software Foundation; either version 2 of the License, or 7 | % (at your option) any later version. 8 | % 9 | % This program is distributed in the hope that it will be useful, 10 | % but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | % MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | % GNU General Public License for more details. 13 | % 14 | % You should have received a copy of the GNU General Public License along 15 | % with this program; if not, write to the Free Software Foundation, Inc., 16 | % 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA. 17 | 18 | % 04.03.2014 by Steffen Urban 19 | % this is a modified file from 20 | % Davide Scaramuzzas Toolbox OcamCalib 21 | % filename: ocam_calib_gui.m 22 | % call ocam_calib_guiUrban instead of ocan_calib_gui 23 | 24 | set_up_global; 25 | ocam_calib_guiUrban; 26 | -------------------------------------------------------------------------------- /src/ocam_calib_guiUrban.m: -------------------------------------------------------------------------------- 1 | % Steffen Urban email: steffen.urban@kit.edu 2 | % Copyright (C) 2014 Steffen Urban 3 | % 4 | % This program is free software; you can redistribute it and/or modify 5 | % it under the terms of the GNU General Public License as published by 6 | % the Free Software Foundation; either version 2 of the License, or 7 | % (at your option) any later version. 8 | % 9 | % This program is distributed in the hope that it will be useful, 10 | % but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | % MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | % GNU General Public License for more details. 13 | % 14 | % You should have received a copy of the GNU General Public License along 15 | % with this program; if not, write to the Free Software Foundation, Inc., 16 | % 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA. 17 | 18 | % 04.03.2014 by Steffen Urban 19 | % this is a modified file from 20 | % Davide Scaramuzzas Toolbox OcamCalib 21 | % filename: ocam_calib_gui.m 22 | % The structure of the menu is modified 23 | 24 | 25 | function ocam_calib_guiUrban 26 | 27 | cell_list = {}; 28 | 29 | fig_number = 1; 30 | title_figure = 'Improved Omnidirectional Camera Calibration Toolbox'; 31 | cell_list{1,1} = {'Read names','data_calib(calib_data);'}; 32 | cell_list{1,2} = {'Extract grid corners','click_calibUrban(calib_data);'}; 33 | cell_list{1,3} = {'Calibration','calibration(calib_data);'}; 34 | cell_list{1,4} = {'Non-linear Refinement','bundleAdjustmentUrban(calib_data,false);'}; 35 | 36 | cell_list{2,1} = {'Robust Non-linear Refinement','bundleAdjustmentUrban(calib_data,true);'}; 37 | cell_list{2,2} = {'Reproject on images','reproject_calib(calib_data);'}; 38 | cell_list{2,3} = {'Show Extrinsic','create_simulation_points(calib_data);'}; 39 | cell_list{2,4} = {'Analyse error','analyse_error(calib_data);'}; 40 | 41 | cell_list{3,1} = {'Recomp. corners','recomp_corner_calib(calib_data);'}; 42 | cell_list{3,2} = {'Show calib results','show_calib_results(calib_data);'}; 43 | cell_list{3,3} = {'Save','saving_calib(calib_data);'}; 44 | cell_list{3,4} = {'Load','loading_calib;'}; 45 | 46 | cell_list{4,1} = {'Export Data','exportData2TXT(calib_data);'}; 47 | cell_list{4,2} = {'Exit',['clear; disp(''Bye. To run again, type ocam_calib.''); close(' num2str(fig_number) ');']}; %{'Exit','calib_gui;'}; 48 | 49 | 50 | 51 | 52 | show_window(cell_list,fig_number,title_figure); 53 | 54 | 55 | %-------- End editable region -------------% 56 | 57 | 58 | %------- DO NOT EDIT ANYTHING BELOW THIS LINE -----------% 59 | 60 | function show_window(cell_list,fig_number,title_figure,x_size,y_size,gap_x,font_name,font_size) 61 | 62 | 63 | if ~exist('cell_list'), 64 | error('No description of the functions'); 65 | end; 66 | 67 | if ~exist('fig_number'), 68 | fig_number = 1; 69 | end; 70 | if ~exist('title_figure'), 71 | title_figure = ''; 72 | end; 73 | if ~exist('x_size'), 74 | x_size = 150; 75 | end; 76 | if ~exist('y_size'), 77 | y_size = 16; 78 | end; 79 | if ~exist('gap_x'), 80 | gap_x = 0; 81 | end; 82 | if ~exist('font_name'), 83 | font_name = 'clean'; 84 | end; 85 | if ~exist('font_size'), 86 | font_size = 10; 87 | end; 88 | 89 | figure(fig_number); clf; 90 | pos = get(fig_number,'Position'); 91 | 92 | [n_row,n_col] = size(cell_list); 93 | 94 | fig_size_x = x_size*n_col+(n_col+1)*gap_x; 95 | fig_size_y = y_size*n_row+(n_row+1)*gap_x; 96 | 97 | set(fig_number,'Units','points', ... 98 | 'BackingStore','off', ... 99 | 'Color',[0.8 0.8 0.8], ... 100 | 'MenuBar','none', ... 101 | 'Resize','off', ... 102 | 'Name',title_figure, ... 103 | 'Position',[pos(1)-20 pos(2)-20 fig_size_x fig_size_y], ... 104 | 'NumberTitle','off'); %,'WindowButtonMotionFcn',['figure(' num2str(fig_number) ');']); 105 | 106 | h_mat = zeros(n_row,n_col); 107 | 108 | posx = zeros(n_row,n_col); 109 | posy = zeros(n_row,n_col); 110 | 111 | for i=n_row:-1:1, 112 | for j = n_col:-1:1, 113 | posx(i,j) = gap_x+(j-1)*(x_size+gap_x); 114 | posy(i,j) = fig_size_y - i*(gap_x+y_size); 115 | end; 116 | end; 117 | 118 | for i=n_row:-1:1, 119 | for j = n_col:-1:1, 120 | if ~isempty(cell_list{i,j}), 121 | if ~isempty(cell_list{i,j}{1}) & ~isempty(cell_list{i,j}{2}), 122 | h_mat(i,j) = uicontrol('Parent',fig_number, ... 123 | 'Units','points', ... 124 | 'Callback',cell_list{i,j}{2}, ... 125 | 'ListboxTop',0, ... 126 | 'Position',[posx(i,j) posy(i,j) x_size y_size], ... 127 | 'String',cell_list{i,j}{1}, ... 128 | 'fontsize',font_size,... 129 | 'fontname',font_name,... 130 | 'Tag','Pushbutton1'); 131 | end; 132 | end; 133 | end; 134 | end; 135 | 136 | %------ END PROTECTED REGION ----------------% 137 | -------------------------------------------------------------------------------- /src/optimizefunction.m: -------------------------------------------------------------------------------- 1 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 2 | % Copyright (C) 2006 DAVIDE SCARAMUZZA 3 | % 4 | % Author: Davide Scaramuzza - email: davsca@tiscali.it 5 | % 6 | % This program is free software; you can redistribute it and/or modify 7 | % it under the terms of the GNU General Public License as published by 8 | % the Free Software Foundation; either version 2 of the License, or 9 | % (at your option) any later version. 10 | % 11 | % This program is distributed in the hope that it will be useful, 12 | % but WITHOUT ANY WARRANTY; without even the implied warranty of 13 | % MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 14 | % GNU General Public License for more details. 15 | % 16 | % You should have received a copy of the GNU General Public License 17 | % along with this program; if not, write to the Free Software 18 | % Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 19 | % USA 20 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 21 | 22 | % 04.03.2014 by Steffen Urban 23 | % this is a modified file from 24 | % Davide Scaramuzzas Toolbox OcamCalib 25 | % filename: optimizefunction.m 26 | % Code was added to calculate the standard deviations of the ext ori 27 | 28 | function optimizefunction(calib_data) 29 | tol_MSE = 1e-4; 30 | MSE_old = 0; 31 | MSE_new = Inf; 32 | iter = 0; 33 | 34 | fprintf(1,'This function alternately refines EXTRINSIC and INTRINSIC calibration parameters\n'); 35 | fprintf(1,'by using an interative non linear minimization method \n'); 36 | fprintf(1,'Because of the computations involved this refinement can take some seconds\n'); 37 | 38 | fprintf(1,'Loop interrupting: Press enter to stop refinement. (OCamCalib GUI must be selected!)\n'); 39 | 40 | % max_iter = input('\n Maximum number of iterations ([] = 100, 0 = abort, -1 = no limit) = '); 41 | 42 | % if ~isempty(max_iter) 43 | % if max_iter == 0 44 | % return; 45 | % elseif max_iter == -1; 46 | % max_iter = Inf; 47 | % end 48 | % else 49 | max_iter = 100; 50 | % end 51 | 52 | 53 | if isempty(calib_data.n_ima) | calib_data.calibrated==0, 54 | fprintf(1,'\nNo calibration data available. You must first calibrate your camera.\nClick on "Calibration" or "Find center"\n\n'); 55 | return; 56 | end; 57 | 58 | pause(0.01); 59 | figure(1); %obrand used for runtime interrupting 60 | pause(0.01); 61 | set(gcf,'CurrentChar', 'a'); 62 | 63 | 64 | while (iter < max_iter && abs(MSE_new - MSE_old) > tol_MSE) && get(gcf,'CurrentChar') ~= 13 65 | iter = iter + 1; 66 | fprintf(1, '\nIteration %i\n',iter); 67 | 68 | fprintf(1,'Starting refinement of EXTRINSIC parameters...\n'); 69 | 70 | options=optimset('Display','off',... 71 | 'LargeScale','off', ... 72 | 'TolX',1e-4,... 73 | 'TolFun',1e-4,... 74 | 'DerivativeCheck','off',... 75 | 'Diagnostics','off',... 76 | 'Jacobian','off',... 77 | 'JacobMult',[],... % JacobMult set to [] by default 78 | 'JacobPattern','sparse(ones(Jrows,Jcols))',... 79 | 'MaxFunEvals','100*numberOfVariables',... 80 | 'DiffMaxChange',1e-1,... 81 | 'DiffMinChange',1e-8,... 82 | 'PrecondBandWidth',0,... 83 | 'TypicalX','ones(numberOfVariables,1)',... 84 | 'MaxPCGIter','max(1,floor(numberOfVariables/2))', ... 85 | 'TolPCG',0.1,... 86 | 'MaxIter',10000); 87 | 88 | 89 | 90 | if (isempty(calib_data.ocam_model.c) & isempty(calib_data.ocam_model.d) & isempty(calib_data.ocam_model.e)) 91 | calib_data.ocam_model.c=1; 92 | calib_data.ocam_model.d=0; 93 | calib_data.ocam_model.e=0; 94 | end 95 | int_par=[calib_data.ocam_model.c,calib_data.ocam_model.d,calib_data.ocam_model.e,calib_data.ocam_model.xc,calib_data.ocam_model.yc]; 96 | M=[calib_data.Xt,calib_data.Yt,zeros(size(calib_data.Xt))]; 97 | fprintf(1,'Optimizing chessboard pose '); 98 | for i=calib_data.ima_proc 99 | fprintf(1,'%d, ',i); 100 | R=calib_data.RRfin(:,:,i); 101 | R(:,3)=cross(R(:,1),R(:,2)); 102 | r=rodrigues(R); 103 | t=calib_data.RRfin(:,3,i); 104 | x0=[r(1),r(2),r(3),t(1),t(2),t(3)]; 105 | 106 | [x0,~,vExtr,~,~, ~, jacExtr] =lsqnonlin(@prova,x0,-inf*ones(size(x0)),inf*ones(size(x0)),options,calib_data.ocam_model.ss,int_par,calib_data.Xp_abs(:,:,i), calib_data.Yp_abs(:,:,i),M,calib_data.ocam_model.width,calib_data.ocam_model.height); 107 | 108 | RRfinOpt(:,:,i)=rodrigues(x0(1:3)); 109 | RRfinOpt(:,3,i)=x0(4:6)'; 110 | %% ================ 111 | % added code, steffen urban 112 | sigma0q = 1^2; 113 | [rows,cols]=size(jacExtr); 114 | v = vExtr; 115 | J = jacExtr; 116 | Qll = sigma0q*eye(size(J,1),size(J,1)); 117 | P0 = inv(Qll); 118 | % a posteriori variance 119 | calib_data.statEO{i}.sg0 = (v'*P0*v) / (rows-cols); 120 | % empirical covariance matrix 121 | calib_data.statEO{i}.Exx = calib_data.statEO{i}.sg0 * pinv(J'*P0*J); 122 | calib_data.statEO{i}.varEO = diag(calib_data.statEO{i}.Exx); % variance of ext ori parameters 123 | calib_data.statEO{i}.stdEO = sqrt(calib_data.statEO{i}.varEO); % standard deviation of ext ori parameters 124 | calib_data.optimized = true; 125 | %% ================ 126 | end 127 | 128 | 129 | 130 | calib_data.RRfin=RRfinOpt; 131 | 132 | fprintf(1,'\nStarting refinement of INTRINSIC parameters...\n'); 133 | 134 | ss0=calib_data.ocam_model.ss; 135 | 136 | % options=optimset('Display','off',... 137 | % 'LargeScale','off', ... 138 | % 'TolX',1e-4,... 139 | % 'TolFun',1e-4,... 140 | % 'DerivativeCheck','off',... 141 | % 'Diagnostics','off',... 142 | % 'Jacobian','off',... 143 | % 'JacobMult',[],... % JacobMult set to [] by default 144 | % 'JacobPattern','sparse(ones(Jrows,Jcols))',... 145 | % 'MaxFunEvals','100*numberOfVariables',... 146 | % 'DiffMaxChange',1e-1,... 147 | % 'DiffMinChange',1e-8,... 148 | % 'PrecondBandWidth',0,... 149 | % 'TypicalX','ones(numberOfVariables,1)',... 150 | % 'MaxPCGIter','max(1,floor(numberOfVariables/2))', ... 151 | % 'TolPCG',0.1,... 152 | % 'MaxIter',10000); 153 | options=optimset('Display','off',... 154 | 'LargeScale','off', ... 155 | 'TolX',1e-4,... 156 | 'TolFun',1e-4,... 157 | 'Algorithm','levenberg-marquardt'); 158 | 159 | f0=[1,1,calib_data.ocam_model.c,calib_data.ocam_model.d,calib_data.ocam_model.e,ones(1,size(calib_data.ocam_model.ss,1))]; 160 | lb=[0,0,0,-1,-1,zeros(1,size(calib_data.ocam_model.ss,1))]; 161 | ub=[2,2,2,1,1,2*ones(1,size(calib_data.ocam_model.ss,1))]; 162 | 163 | % [ssout,~,vIO,~,~,~,jacsIO] =lsqnonlin(@prova3,f0,lb,ub,options,calib_data.ocam_model.xc,calib_data.ocam_model.yc,ss0,calib_data.RRfin,calib_data.ima_proc,calib_data.Xp_abs,calib_data.Yp_abs,M, calib_data.ocam_model.width, calib_data.ocam_model.height); 164 | [ssout,~,vIO,~,~,~,jacsIO] =lsqnonlin(@prova3,f0,-inf*ones(1,length(x0)),inf*ones(1,length(x0)),options,calib_data.ocam_model.xc,calib_data.ocam_model.yc,ss0,calib_data.RRfin,calib_data.ima_proc,calib_data.Xp_abs,calib_data.Yp_abs,M, calib_data.ocam_model.width, calib_data.ocam_model.height); 165 | 166 | %% ================ 167 | % added code , steffen urban 168 | sigma0q = 1^2; 169 | [rows,cols]=size(jacsIO); 170 | jacsIO(:,7) = []; 171 | v = vIO; 172 | J = jacsIO; 173 | Qll = sigma0q*eye(size(J,1),size(J,1)); 174 | P0 = inv(Qll); 175 | % a posteriori variance 176 | calib_data.statIO.sg0 = (v'*P0*v) / (rows-cols); 177 | % empirical covariance matrix 178 | calib_data.statIO.Exx = calib_data.statIO.sg0 * inv(J'*P0*J); 179 | calib_data.statIO.varIO = diag(calib_data.statIO.Exx); % variance of ext ori parameters 180 | calib_data.statIO.stdIO = sqrt(abs(calib_data.statIO.varIO)); % standard deviation of ext ori parameters 181 | calib_data.optimized = true; 182 | ssc = ssout(6:end); 183 | %% ================ 184 | 185 | calib_data.ocam_model.ss=ss0.*ssc'; 186 | calib_data.ocam_model.xc=calib_data.ocam_model.xc*ssout(1); 187 | calib_data.ocam_model.yc=calib_data.ocam_model.yc*ssout(2); 188 | calib_data.ocam_model.c=ssout(3); 189 | calib_data.ocam_model.d=ssout(4); 190 | calib_data.ocam_model.e=ssout(5); 191 | 192 | [err,stderr,MSE] = reprojectpoints_quiet(calib_data.ocam_model, calib_data.RRfin, calib_data.ima_proc, calib_data.Xp_abs, calib_data.Yp_abs, M); 193 | fprintf(1,'Sum of squared errors: %f\n',MSE); 194 | MSE_old = MSE_new; 195 | 196 | end 197 | 198 | if get(gcf,'CurrentChar') == 13 199 | fprintf(1,'\n\nCamera model refinement interrupted'); 200 | else 201 | fprintf(1,'\n\nCamera model optimized'); 202 | end 203 | [err,stderr,MSE] = reprojectpoints_adv(calib_data.ocam_model, calib_data.RRfin, calib_data.ima_proc, calib_data.Xp_abs, calib_data.Yp_abs, M); 204 | ss = calib_data.ocam_model.ss; 205 | ss 206 | 207 | %% ================ 208 | % added code 209 | calib_data.errMean = err; 210 | calib_data.errStd = stderr; 211 | calib_data.mse = MSE; 212 | calib_data.rms = sqrt(sum(v.^2)/length(v)); 213 | fprintf(1,'Root mean square[pixel]:: %f\n',calib_data.rms); 214 | %% ================ --------------------------------------------------------------------------------