├── HOWTO.txt ├── ICG_functions ├── ICG_boundingBox.m ├── ICG_checkerboardCorners.m ├── ICG_createCalibrationMarker.m ├── ICG_createRegistrationTemplate.m ├── ICG_extractCornersLoop.m ├── ICG_findMarker.m ├── ICG_fitAffinity2D.m ├── ICG_initialCornerGuess.m ├── ICG_interp2.m ├── ICG_normXCorr2.m ├── ICG_normalizePoints.m ├── ICG_pointsInsideRegion.m ├── ICG_projectiveRegistration.m ├── normxcorr2_src │ ├── CMakeLists.txt │ ├── cmake │ │ └── FindMatlab_ICG.cmake │ └── normxcorr2_mex.cpp └── private │ ├── ConnectedComponents.m │ ├── GetOrientationIdentity.m │ ├── adaptivethreshold.m │ ├── calcIntersection.m │ ├── findMarkerCorners.m │ ├── getSimilarity.m │ ├── hessian.m │ ├── homo_ic.m │ ├── init_h.m │ ├── jacobian_h.m │ ├── lineSegments.m │ ├── normalise2dpts.m │ ├── normxcorr2_mex.mexa64 │ ├── normxcorr2_mex.mexw64 │ ├── opencv_core2411.dll │ ├── opencv_imgproc2411.dll │ ├── quadtobox_h.m │ ├── sd_images.m │ ├── sd_update.m │ ├── selectBestMatch.m │ └── warp_h.m ├── LICENSE.txt ├── README.md ├── TOOLBOX_calib ├── Compute3D.m ├── ComputeStripes.m ├── Distor2Calib.m ├── Meshing.m ├── README.txt ├── Rectangle2Square.m ├── TestFunction.m ├── UnWarpPlane.m ├── add_suppress.m ├── affine.m ├── align_structures.m ├── analyse_error.m ├── anisdiff.m ├── apply_distortion.m ├── apply_distortion2.m ├── apply_fisheye_distortion.m ├── calib.m ├── calib_gui.m ├── calib_gui_fisheye.m ├── calib_gui_no_read.m ├── calib_gui_normal.m ├── calib_stereo.m ├── calibration_pattern │ ├── pattern.eps │ └── pattern.pdf ├── cam_proj_calib.m ├── cam_proj_calib_optim.m ├── cam_proj_extract_param.m ├── centercirclefinder.m ├── check_active_images.m ├── check_convergence.m ├── check_directory.m ├── check_extracted_images.m ├── clear_windows.m ├── clearwin.m ├── click_calib.m ├── click_calib_fisheye_no_read.m ├── click_calib_no_read.m ├── click_ima_calib.m ├── click_ima_calib3D.m ├── click_ima_calib_fisheye_no_read.m ├── click_ima_calib_fisheye_no_read.m~ ├── click_ima_calib_no_read.m ├── click_stereo.m ├── combine_calib.m ├── comp_distortion.m ├── comp_distortion2.m ├── comp_distortion_oulu.m ├── comp_error_calib.m ├── comp_error_calib_fisheye.m ├── comp_ext_calib.m ├── comp_ext_calib_fisheye.m ├── comp_fisheye_distortion.m ├── compose_motion.m ├── compute_collineation.m ├── compute_epipole.m ├── compute_extrinsic.m ├── compute_extrinsic_init.m ├── compute_extrinsic_init_fisheye.m ├── compute_extrinsic_refine.m ├── compute_extrinsic_refine2.m ├── compute_extrinsic_refine_fisheye.m ├── compute_homography.m ├── cornerfinder.m ├── cornerfinder2.m ├── cornerfinder_saddle_point.m ├── count_squares.m ├── count_squares_distorted.m ├── count_squares_fisheye_distorted.m ├── dAB.m ├── data_calib.m ├── data_calib_no_read.m ├── downsample.m ├── edgefinder.m ├── eliminate_boundary.m ├── error_analysis.m ├── error_cam_proj.m ├── error_cam_proj2.m ├── error_cam_proj3.m ├── error_depth.m ├── error_depth_list.m ├── export_calib_data.m ├── ext_calib.m ├── ext_calib2.m ├── ext_calib_stereo.m ├── extract_distortion_data.m ├── extract_grid.m ├── extract_grid_manual.m ├── extract_parameters.m ├── extract_parameters3D.m ├── extract_parameters_fisheye.m ├── extrinsic_computation.m ├── fixallvariables.m ├── fixvariable.m ├── fov.m ├── ginput2.m ├── ginput3.m ├── ginput4.m ├── go_calib_optim.m ├── go_calib_optim_fisheye_no_read.asv ├── go_calib_optim_fisheye_no_read.m ├── go_calib_optim_iter.m ├── go_calib_optim_iter_fisheye.m ├── go_calib_optim_iter_weak.m ├── go_calib_optim_no_read.m ├── go_calib_stereo.m ├── ima_read_calib.m ├── ima_read_calib_no_read.m ├── init_intrinsic_param.m ├── init_intrinsic_param_fisheye.m ├── inverse_motion.m ├── is3D.m ├── load_image.m ├── load_stereo_calib_files.m ├── loading_calib.m ├── loading_stereo_calib.m ├── loadinr.m ├── loadpgm.m ├── loadppm.m ├── manual_corner_extraction.m ├── manual_corner_extraction_no_read.m ├── mean_std_robust.m ├── merge_calibration_sets.m ├── merge_two_datasets.m ├── mosaic.m ├── mosaic_no_read.m ├── normalize.m ├── normalize2.m ├── normalize_pixel.m ├── normalize_pixel_fisheye.m ├── pattern.eps ├── pgmread.m ├── point_distribution.m ├── project2_oulu.m ├── project_points.m ├── project_points2.m ├── project_points3.m ├── project_points_fisheye.m ├── project_points_weak.m ├── projectedGrid.m ├── projector_calib.m ├── projector_ima_corners.m ├── projector_marker.m ├── readras.m ├── recomp_corner_calib.m ├── recomp_corner_calib_fisheye_no_read.m ├── recomp_corner_calib_no_read.m ├── recomp_corner_calib_saddle_points.m ├── rect.m ├── rect_index.m ├── rectify_stereo_pair.m ├── reproject_calib.m ├── reproject_calib_no_read.m ├── rigid_motion.m ├── rodrigues.m ├── rotation.m ├── run_error_analysis.m ├── saveinr.m ├── savepgm.m ├── saveppm.m ├── saving_calib.m ├── saving_calib_ascii.m ├── saving_calib_ascii_fisheye.m ├── saving_calib_fisheye.m ├── saving_calib_no_results.m ├── saving_stereo_calib.m ├── scanner_calibration_script.m ├── scanning_script.m ├── script_fit_distortion.m ├── script_fit_distortion_fisheye.m ├── show_calib_results.m ├── show_calib_results_fisheye.m ├── show_stereo_calib_results.m ├── show_window.m ├── skew3.m ├── small_test_script.m ├── smooth_images.m ├── startup.m ├── stereo_gui.m ├── stereo_triangulation.m ├── undistort_image.m ├── undistort_image_color.m ├── undistort_image_no_read.m ├── undistort_sequence.m ├── visualize_distortions.m ├── willson_convert.m ├── willson_read.m ├── write_image.m └── writeras.m ├── auto_calib.m ├── autocalibration.m ├── calib_gui_normal_auto.m ├── calib_stereo_auto.m ├── download_scipt.sh ├── make_target.m ├── readme └── style.css └── testdata ├── image_001.jpg ├── image_002.jpg ├── image_003.jpg ├── image_004.jpg ├── image_005.jpg ├── image_006.jpg ├── image_007.jpg ├── image_008.jpg ├── image_009.jpg ├── image_010.jpg ├── image_011.jpg ├── image_012.jpg ├── image_013.jpg ├── image_014.jpg ├── image_015.jpg ├── image_016.jpg ├── image_017.jpg ├── image_018.jpg ├── image_019.jpg └── image_020.jpg /ICG_functions/ICG_boundingBox.m: -------------------------------------------------------------------------------- 1 | function intervals_ndimsx2 = ICG_boundingBox (points); 2 | % intervals_ndimsx2 = ICG_boundingBox (points); 3 | 4 | ndims = size(points, 1); 5 | intervals_ndimsx2 = zeros(ndims, 2); 6 | for i=1:ndims, 7 | intervals_ndimsx2(i,1) = min(points(i,:)); 8 | intervals_ndimsx2(i,2) = max(points(i,:)); 9 | end; -------------------------------------------------------------------------------- /ICG_functions/ICG_findMarker.m: -------------------------------------------------------------------------------- 1 | function [marker_corners, marker_similarity] = ICG_findMarker (image, template, corner_threshold, min_area, intensity_threshold) 2 | % [marker_corners, marker_similarity] = ICG_findMarker (image, template, corner_threshold, min_area, intensity_threshold) 3 | 4 | if nargin < 5, 5 | intensity_threshold = []; 6 | end; 7 | 8 | if size(image,3) ~= 1 9 | error('Image should only have one plane') 10 | end 11 | 12 | if size(template) ~= [32 32] 13 | error('Template picture dimenstion must be 32x32!') 14 | end 15 | 16 | [corners_col, corners_row] = findMarkerCorners(image, corner_threshold, min_area, intensity_threshold); 17 | 18 | if isempty(corners_col), 19 | marker_corners = []; 20 | marker_similarity = 0; 21 | return; 22 | end; 23 | 24 | warning('off', 'Images:maketform:conditionNumberofAIsHigh'); 25 | matches = GetOrientationIdentity(image, template, corners_col, corners_row); 26 | warning('on', 'Images:maketform:conditionNumberofAIsHigh'); 27 | [marker_corners, marker_similarity] = selectBestMatch(matches); 28 | marker_corners = marker_corners'; 29 | if marker_similarity < .7 30 | % refine marker corners and calculate similarity 31 | % [marker_corners, marker_similarity] = ICG_refineMarkerCorners(image, marker_corners, template); 32 | end 33 | -------------------------------------------------------------------------------- /ICG_functions/ICG_fitAffinity2D.m: -------------------------------------------------------------------------------- 1 | function H = ICG_fitAffinity2D(varargin); 2 | 3 | % HOMOGRAPHY2D - computes 2D homography 4 | % 5 | % Usage: H = homography2d(x1, x2) 6 | % H = homography2d(x) 7 | % 8 | % Arguments: 9 | % x1 - 3xN set of homogeneous points 10 | % x2 - 3xN set of homogeneous points such that x1<->x2 11 | % 12 | % x - If a single argument is supplied it is assumed that it 13 | % is in the form x = [x1; x2] 14 | % Returns: 15 | % H - the 3x3 homography such that x2 = H*x1 16 | % 17 | % This code follows the normalised direct linear transformation 18 | % algorithm given by Hartley and Zisserman "Multiple View Geometry in 19 | % Computer Vision" p92. 20 | % 21 | 22 | % Peter Kovesi 23 | % School of Computer Science & Software Engineering 24 | % The University of Western Australia 25 | % pk at csse uwa edu au 26 | % http://www.csse.uwa.edu.au/~pk 27 | % 28 | % May 2003 - Original version. 29 | % Feb 2004 - Single argument allowed for to enable use with RANSAC. 30 | % Feb 2005 - SVD changed to 'Economy' decomposition (thanks to Paul O'Leary) 31 | 32 | [x1, x2] = checkargs(varargin(:)); 33 | 34 | % Attempt to normalise each set of points so that the origin 35 | % is at centroid and mean distance from origin is sqrt(2). 36 | [x1, T1] = normalise2dpts(x1); 37 | [x2, T2] = normalise2dpts(x2); 38 | 39 | % Note that it may have not been possible to normalise 40 | % the points if one was at infinity so the following does not 41 | % assume that scale parameter w = 1. 42 | 43 | Npts = size(x1,2); 44 | A = zeros(3*Npts,9); 45 | 46 | O = [0 0 0]; 47 | for n = 1:Npts 48 | 49 | X = x1(:,n)'; 50 | x = x2(1,n); y = x2(2,n); w = x2(3,n); 51 | A(3*n-2,:) = [ O -w*X y*X]; 52 | A(3*n-1,:) = [ w*X O -x*X]; 53 | A(3*n ,:) = [-y*X x*X O ]; 54 | end 55 | 56 | %Reduce Problem to Affinity 57 | A(:, 7:8) = []; 58 | 59 | [U,D,V] = svd(A,0); % 'Economy' decomposition for speed 60 | 61 | solution = [V(1:6,7); 0; 0; V(7,7)]; 62 | H = reshape(solution,3,3)'; 63 | 64 | % Denormalise 65 | H = T2\H*T1; 66 | 67 | 68 | %-------------------------------------------------------------------------- 69 | % Function to check argument values and set defaults 70 | 71 | function [x1, x2] = checkargs(arg); 72 | 73 | if length(arg) == 2 74 | x1 = arg{1}; 75 | x2 = arg{2}; 76 | if ~all(size(x1)==size(x2)) 77 | error('x1 and x2 must have the same size'); 78 | elseif size(x1,1) ~= 3 79 | error('x1 and x2 must be 3xN'); 80 | end 81 | 82 | elseif length(arg) == 1 83 | if size(arg{1},1) ~= 6 84 | error('Single argument x must be 6xN'); 85 | else 86 | x1 = arg{1}(1:3,:); 87 | x2 = arg{1}(4:6,:); 88 | end 89 | else 90 | error('Wrong number of arguments supplied'); 91 | end 92 | -------------------------------------------------------------------------------- /ICG_functions/ICG_normXCorr2.m: -------------------------------------------------------------------------------- 1 | function result = ICG_normXCorr2 (template, image, shape); 2 | 3 | % NORMXCORR2_MEX 4 | % RESULT = normxcorr2_mex(TEMPLATE, IMAGE, SHAPE) 5 | % 6 | % TEMPLATE - type double, size <= size of image 7 | % IMAGE - type double 8 | % SHAPE - one of: 'valid', 'same', 'full'. same as conv2 shape parameter 9 | % 10 | % RESULT - type double, values in [-1,1]. size depends on SHAPE 11 | % 12 | % the syntax of this function is identical to Matlab's 13 | % normxcorr2, except for the output size. the formula for the output size 14 | % is: 15 | % 16 | % size(template) = [tp_H, tp_W] 17 | % size(image) = [im_H, im_W] 18 | % 19 | % SHAPE='valid' size(result) = [im_H-tp_H+1, im_W-tp_W+1] 20 | % SHAPE='same' size(result) = [im_H im_W] 21 | % SHAPE='full' size(result) = [im_H+tp_H-1, im_W+tp_W-1] 22 | % 23 | % note: 24 | % all choices of the SHAPE parameter yield the same output. by this I mean 25 | % that the 'same' and 'full' cases just zero-pad the result so that they 26 | % are the correct size (for your convenience). this implementation cannot return partial matching 27 | % results near the boundary. 28 | % 29 | % the following are equivalent: 30 | % result = normxcorr2_mex(template, image, 'full'); 31 | % AND 32 | % result = normxcorr2(template, image); 33 | % except that normxcorr2_mex has 0's in the 'invalid' area along the boundary 34 | % 35 | % SEE ALSO CONV2 (for an explanation of SHAPE), or normxcorr2_demo.m (for a demo) 36 | % 37 | % Major NB I want made clear: 38 | % The core code uses the cvMatchTemplate from the OpenCV. I don't know who 39 | % was responsible for this routine, but it is extremely well coded and I want 40 | % to give them credit. All I've done is write the MEX interface. 41 | % For more on this, see the readme. 42 | % 43 | % Daniel Eaton, danieljameseaton@gmail.com, 2005 44 | 45 | result = normxcorr2_mex(template, image, shape); -------------------------------------------------------------------------------- /ICG_functions/ICG_normalizePoints.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RobVisLab/camera_calibration/f56c17c297653ba0a156d125c2b162b19432b48b/ICG_functions/ICG_normalizePoints.m -------------------------------------------------------------------------------- /ICG_functions/ICG_pointsInsideRegion.m: -------------------------------------------------------------------------------- 1 | function mask = ICG_pointsInsideRegion (points, intervals_ndimsx2); 2 | % mask = ICG_pointsInsideRegion (points, intervals_ndimsx2); 3 | 4 | mask = logical(ones(1, size(points, 2))); 5 | ndims = size(points, 1); 6 | for i=1:ndims, 7 | mask = mask & ... 8 | points(i,:) >= intervals_ndimsx2(i,1) & ... 9 | points(i,:) <= intervals_ndimsx2(i,2); 10 | end; 11 | 12 | -------------------------------------------------------------------------------- /ICG_functions/ICG_projectiveRegistration.m: -------------------------------------------------------------------------------- 1 | function fit = ICG_projectiveRegistration (img, tmplt, p_init, n_iters, verbose); 2 | % HOMO_IC - Homography image alignment using inverse-compositional algorithm 3 | % FIT = HOMO_IC(IMG, TMPLT, P_INIT, N_ITERS, VERBOSE) 4 | % Align the template image TMPLT to an example image IMG using a 5 | % projective warp initialised using P_INIT. Iterate for N_ITERS iterations. 6 | % To display the fit graphically set VERBOSE non-zero. 7 | % 8 | % p_init = [p1, p4, p7 paper_equivalent = [p1, p3, p5 9 | % p2, p5, p8 p2, p4, p6 10 | % p3, p6, 1]; p7, p8, 1]; 11 | % 12 | % This assumes greyscale images and rectangular templates. 13 | % 14 | % c.f. Baker-Matthews 15 | % 16 | % ATTENTION: this is NOT a classical homography!!!!!! 17 | % If H(:) = [h1, h2, ..., h8, 1], is a homography, then 18 | % p_init(i) = h(i) for i elem {2, 3, 4, 6, 7, 8} and h(i)-1 19 | % for i elem {1, 5}. 20 | 21 | % Iain Matthews, Simon Baker, Carnegie Mellon University, Pittsburgh 22 | % $Id: ICG_projectiveRegistration.m,v 1.2 2006-12-18 13:48:54 ruether Exp $ 23 | 24 | fit = homo_ic (img, tmplt, p_init, n_iters, verbose); -------------------------------------------------------------------------------- /ICG_functions/normxcorr2_src/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | project (cudaApplications CXX C) 2 | 3 | cmake_minimum_required(VERSION 2.6) 4 | 5 | FILE(TO_CMAKE_PATH "${CMAKE_CURRENT_SOURCE_DIR}/cmake/" OT_CMAKE_MODULE_PATH) 6 | SET(CMAKE_MODULE_PATH ${OT_CMAKE_MODULE_PATH}) 7 | SET(CMAKE_LIBRARY_OUTPUT_DIRECTORY "${CMAKE_CURRENT_SOURCE_DIR}/../private") 8 | 9 | # COMPILER FLAGS 10 | IF(WIN32) 11 | ADD_DEFINITIONS(-DWIN32) 12 | ELSE(WIN32) 13 | ADD_DEFINITIONS( -Wall) 14 | ADD_DEFINITIONS(-O3 -march=core2 ) 15 | ENDIF(WIN32) 16 | #SET(CMAKE_OSX_ARCHITECTURES i386) 17 | 18 | ##----------------------------------------------------------------------------- 19 | ## OpenCV 20 | FIND_PACKAGE(OpenCV REQUIRED) 21 | INCLUDE_DIRECTORIES( ${OpenCV_INCLUDE_DIRS} ) 22 | 23 | ##----------------------------------------------------------------------------- 24 | ## MATLAB 25 | FIND_PACKAGE(Matlab_ICG) 26 | INCLUDE_DIRECTORIES(${MATLAB_INCLUDE_DIR}) 27 | 28 | 29 | SET( SOURCES 30 | normxcorr2_mex.cpp 31 | ) 32 | 33 | INCLUDE_DIRECTORIES( 34 | ${CMAKE_CURRENT_SOURCE_DIR} 35 | ) 36 | 37 | ADD_LIBRARY(normxcorr2_mex SHARED ${HEADERS} ${SOURCES}) 38 | 39 | SET_TARGET_PROPERTIES(normxcorr2_mex PROPERTIES PREFIX "") 40 | SET_TARGET_PROPERTIES(normxcorr2_mex PROPERTIES SUFFIX ${MATLAB_SUFFIX}) 41 | if(WIN32) 42 | SET_TARGET_PROPERTIES(normxcorr2_mex PROPERTIES LINK_FLAGS "/export:mexFunction") 43 | endif(WIN32) 44 | if(WIN32) 45 | target_link_libraries(normxcorr2_mex ${OpenCV_LIBRARIES} ${MATLAB_LIBRARIES}) 46 | else(WIN32) 47 | target_link_libraries(normxcorr2_mex ${OpenCV_LIBRARIES} ${MATLAB_LIBRARIES} m) 48 | endif(WIN32) 49 | -------------------------------------------------------------------------------- /ICG_functions/private/ConnectedComponents.m: -------------------------------------------------------------------------------- 1 | function [labeled_image,area] = ConnectedComponents(thresholded_image,min_region_area) 2 | % [labeled_image,area] = ConnectedComponents(thresholded_image,min_region_area) 3 | [num_rows num_cols] = size(thresholded_image); 4 | 5 | 6 | % Add frame around image 7 | thresholded_image(1,:)=1; 8 | thresholded_image(num_rows,:)=1; 9 | thresholded_image(:,1)=1; 10 | thresholded_image(:,num_cols)=1; 11 | inverted_image = ~thresholded_image; 12 | % if(min(num_rows,num_cols)>200) 13 | inverted_image = imopen(inverted_image, strel('square', 3)); 14 | inverted_image = imclose(inverted_image, strel('square', 3)); 15 | % else 16 | % inverted_image = imopen(inverted_image, strel('disk', 1)); 17 | % inverted_image = imclose(inverted_image, strel('disk', 1)); 18 | % end 19 | [temp_labeled_image num_regions] = bwlabel(inverted_image,8); 20 | region_props = regionprops(temp_labeled_image, 'BoundingBox'); 21 | bb = cat(1, region_props(:).BoundingBox); 22 | convex_areas = bb(:, 3) .* bb(:, 4); 23 | good_label_mask = convex_areas >= min_region_area; 24 | bad_label_mask = ~good_label_mask; 25 | convex_areas(bad_label_mask) = []; 26 | area = convex_areas; 27 | labeled_image = zeros(size(temp_labeled_image)); 28 | good_labels = find(good_label_mask); 29 | for i=1:length(good_labels), 30 | current_label = good_labels(i); 31 | mask = temp_labeled_image==current_label; 32 | labeled_image(mask) = current_label; 33 | end; 34 | 35 | % region_histogram = hist(reshape(thresholded_image,num_rows*num_cols,1),-0.5:1:num_regions+0.5); 36 | % region_histogram = region_histogram(2:length(region_histogram)); 37 | % It's a tiny bit faster, when very large areas are dropped, too, but 38 | % it's not as robust that way, especially when an unrealisticly small 39 | % min_area is used. 40 | % regions_to_keep = find(region_histogram>min_region_area); % & region_histogram<20*min_region_area); 41 | % 42 | % labeled_image = zeros(num_rows,num_cols); 43 | % final_region_index = 1; 44 | % for current_region=regions_to_keep 45 | % [row col] = find(thresholded_image==current_region); 46 | % area(final_region_index) = length(row); 47 | % labeled_image((col-1)*num_rows+row) = final_region_index; 48 | % final_region_index = final_region_index+1; 49 | % end 50 | -------------------------------------------------------------------------------- /ICG_functions/private/GetOrientationIdentity.m: -------------------------------------------------------------------------------- 1 | function matches = GetOrientationIdentity (thresh_image, template, corners_col, corners_row) 2 | %cross correlates with stored version 3 | 4 | [num_squares dummy]=size(corners_col); 5 | matches = cell(1,num_squares); 6 | 7 | for square_index = 1:num_squares 8 | square_corners_col = corners_col(square_index,:); 9 | square_corners_row = corners_row(square_index,:); 10 | source_points = [square_corners_col', square_corners_row']; 11 | target_points = [1 32; 1 1; 32 1; 32 32]; 12 | try %%% the following might fail in case the source points are degenerate! 13 | projective_transformation = maketform('projective', source_points, target_points); 14 | rectified_pattern = imtransform(thresh_image, projective_transformation, 'bilinear', 'XData', [1, 32], 'YData', [1, 32]); 15 | 16 | corr = zeros(1,4); 17 | for temp_orientation = 1:4, 18 | 19 | %%% calculating the whole corralation is a waste! only one value is 20 | %%% needed 21 | %temp_correlation = normxcorr2 (template, rectified_pattern); 22 | %corr(temp_orientation) = temp_correlation(32,32); 23 | 24 | corr(temp_orientation) = getSimilarity(template,rectified_pattern); 25 | template = rot90(template); 26 | end; 27 | [max_corr, max_ind] = max(corr); 28 | corner_sequence = circshift(1:4, [0 max_ind-1]); 29 | matches{square_index} = struct('similarity', max_corr, 'pattern_corners',... 30 | [corners_col(square_index,corner_sequence); corners_row(square_index,corner_sequence)]'); 31 | catch 32 | matches{square_index} = []; 33 | end 34 | end 35 | -------------------------------------------------------------------------------- /ICG_functions/private/adaptivethreshold.m: -------------------------------------------------------------------------------- 1 | function bw_img = adaptivethreshold(img, min_area) 2 | 3 | img = im2double(img); % just in case 4 | img = mat2gray(img); 5 | scale_factor = 0.3; 6 | sub_img = imresize(img, scale_factor); 7 | 8 | % Estimate the necessary window size (should be 4 checkers wide). 9 | % The min_area is the area of a square target which is 3 checkers wide. 10 | window_size = round(sqrt(min_area) * 4/3 * scale_factor); 11 | 12 | m_sub_img = imfilter(sub_img, fspecial('average', window_size), 'replicate'); 13 | m_img = imresize(m_sub_img, size(img)); 14 | bw_img = (img - m_img)>0; 15 | -------------------------------------------------------------------------------- /ICG_functions/private/findMarkerCorners.m: -------------------------------------------------------------------------------- 1 | function [corners_col, corners_row] = findMarkerCorners(image, corner_threshold, min_area, intensity_threshold) 2 | 3 | image = im2double(image); 4 | image = image./max(image(:)); 5 | 6 | if nargin < 4 || isempty(intensity_threshold), 7 | pattern_bwimage = adaptivethreshold(image, min_area/10); 8 | else 9 | pattern_bwimage = im2bw(image, intensity_threshold); 10 | end 11 | 12 | [connectivity_image,area] = ConnectedComponents(pattern_bwimage, min_area); 13 | B = bwboundaries(connectivity_image,8,'noholes'); 14 | [B,corners] = lineSegments(B,area,corner_threshold); 15 | 16 | warning('off', 'MATLAB:singularMatrix'); 17 | [corners_col corners_row] = calcIntersection(B,corners); 18 | warning('on', 'MATLAB:singularMatrix'); 19 | [num_rows, num_cols] = size(image); 20 | 21 | bad_indices = []; 22 | for i=1:size(corners_col,1), 23 | current_region_col = corners_col(i,:); 24 | current_region_row = corners_row(i,:); 25 | 26 | % Are we outside of the image with one of the *corners*? 27 | if sum(current_region_col<0 | current_region_row<0 | current_region_col>num_cols | current_region_row>num_rows)>0, 28 | bad_indices = [bad_indices, i]; 29 | continue; 30 | end 31 | 32 | % Are we suspiciously close to one of the image borders with any *edge*? 33 | edge_too_close_threshold = 7; 34 | if sum(current_region_row - edge_too_close_threshold < 0) > 1 || ... 35 | sum(current_region_row + edge_too_close_threshold > num_rows) > 1 || ... 36 | sum(current_region_col - edge_too_close_threshold < 0) > 1 || ... 37 | sum(current_region_col + edge_too_close_threshold > num_cols) > 1 38 | bad_indices = [bad_indices, i]; 39 | %disp(sprintf('####### dropping marker %d, too close to the border', i)); 40 | end 41 | end 42 | corners_col(bad_indices,:) = []; 43 | corners_row(bad_indices,:) = []; 44 | -------------------------------------------------------------------------------- /ICG_functions/private/getSimilarity.m: -------------------------------------------------------------------------------- 1 | function similarity = getSimilarity(template,rectified_pattern) 2 | template = double(template(:)); 3 | template = template-mean(template); 4 | rectified_pattern = double(rectified_pattern(:)); 5 | rectified_pattern = rectified_pattern-mean(rectified_pattern); 6 | 7 | similarity = sum(template.*rectified_pattern)/(norm(template)*norm(rectified_pattern)); 8 | -------------------------------------------------------------------------------- /ICG_functions/private/hessian.m: -------------------------------------------------------------------------------- 1 | function H = hessian(VI_dW_dp, N_p, w) 2 | % HESSIAN - Compute Hessian 3 | % H = HESSIAN(VI_DW_DP, N_P, W) 4 | 5 | % Iain Matthews, Simon Baker, Carnegie Mellon University, Pittsburgh 6 | % $Id: hessian.m,v 1.2 2007-05-03 12:16:51 ruether Exp $ 7 | 8 | if nargin<3 error('Not enough input arguments'); end 9 | 10 | H = zeros(N_p, N_p); 11 | for i=1:N_p 12 | h1 = VI_dW_dp(:,((i-1)*w)+1:((i-1)*w)+w); 13 | for j=1:N_p 14 | h2 = VI_dW_dp(:,((j-1)*w)+1:((j-1)*w)+w); 15 | product = h1 .* h2; 16 | sum1 = sum(product); 17 | H(j, i) = sum(sum1); 18 | end 19 | end 20 | -------------------------------------------------------------------------------- /ICG_functions/private/init_h.m: -------------------------------------------------------------------------------- 1 | % init_h.m 2 | % Common initialisation things for all homography algorithms 3 | 4 | % Iain Matthews, Simon Baker, Carnegie Mellon University, Pittsburgh 5 | % $Id: init_h.m,v 1.2 2007-05-03 12:22:25 ruether Exp $ 6 | 7 | % Need to process image data, must be real 8 | if ~isa(img, 'double') 9 | img = double(img); 10 | end 11 | 12 | % Projective warp 13 | N_p = 8; 14 | if prod(size(p_init)) ~= 9 15 | error('Number of warp parameters incorrect'); 16 | end 17 | 18 | % Initial warp parameters 19 | warp_p = p_init; 20 | 21 | % Template size 22 | h = size(tmplt, 1); 23 | w = size(tmplt, 2); 24 | 25 | % Template verticies, rectangular [minX minY; minX maxY; maxX maxY; maxX minY] 26 | tmplt_pts = [1 1; 1 h; w h; w 1]'; 27 | 28 | % Verbose display of fitting? 29 | if verbose >= 2, 30 | verb_info = verb_init_h(img, tmplt, tmplt_pts, warp_p); 31 | end 32 | -------------------------------------------------------------------------------- /ICG_functions/private/jacobian_h.m: -------------------------------------------------------------------------------- 1 | function dW_dp = jacobian_h(nx, ny, warp_p); 2 | % JACOBIAN_H - Compute Jacobian for projective warp 3 | % DW_DP = JACOBIAN_H(WIDTH, HEIGHT, P) 4 | 5 | % Iain Matthews, Simon Baker, Carnegie Mellon University, Pittsburgh 6 | % $Id: jacobian_h.m,v 1.2 2007-06-08 14:07:18 ruether Exp $ 7 | 8 | % Easy bits 9 | jac_x = kron([0:nx - 1],ones(ny, 1)); 10 | jac_y = kron([0:ny - 1]',ones(1, nx)); 11 | jac_zero = zeros(ny, nx); 12 | jac_one = ones(ny, nx); 13 | 14 | % Complicated bits are just homography of all image coordinates 15 | xy = [repmat([1:ny]',nx,1) kron([1:nx]', ones(ny,1))]; 16 | xy = [xy ones(length(xy),1)]'; 17 | M = warp_p; 18 | M(1,1) = M(1,1) + 1; 19 | M(2,2) = M(2,2) + 1; 20 | uv = M * xy; 21 | % uvc = uv ./ repmat(uv(3,:),3,1); 22 | uvc = uv; 23 | uvc(1,:) = uvc(1,:) ./ uv(3,:); 24 | uvc(2,:) = uvc(2,:) ./ uv(3,:); 25 | uvc(3,:) = uvc(3,:) ./ uv(3,:); 26 | 27 | u_x = reshape(uvc(1,:),nx,ny)'; 28 | u_y = reshape(uvc(2,:),nx,ny)'; 29 | v = reshape(uv(3,:),nx,ny)'; 30 | 31 | % Divide each jacobian image by v 32 | iv = 1 ./ v; 33 | jac_x = iv .* jac_x; 34 | jac_y = iv .* jac_y; 35 | jac_one = iv .* jac_one; 36 | 37 | [nr, nc] = size(jac_x); 38 | dW_dp = zeros(2*nr, 8*nc); 39 | dW_dp(1:nr, 1:nc) = jac_x; 40 | dW_dp(1:nr, nc+1:2*nc) = jac_zero; 41 | dW_dp(1:nr, 2*nc+1:3*nc) = -jac_x .* u_x; 42 | dW_dp(1:nr, 3*nc+1:4*nc) = jac_y; 43 | dW_dp(1:nr, 4*nc+1:5*nc) = jac_zero; 44 | dW_dp(1:nr, 5*nc+1:6*nc) = -jac_y .* u_x; 45 | dW_dp(1:nr, 6*nc+1:7*nc) = jac_one; 46 | dW_dp(1:nr, 7*nc+1:8*nc) = jac_zero; 47 | 48 | dW_dp(nr+1:2*nr, 1:nc) = jac_zero; 49 | dW_dp(nr+1:2*nr, nc+1:2*nc) = jac_x; 50 | dW_dp(nr+1:2*nr, 2*nc+1:3*nc) = -jac_x .* u_y; 51 | dW_dp(nr+1:2*nr, 3*nc+1:4*nc) = jac_zero; 52 | dW_dp(nr+1:2*nr, 4*nc+1:5*nc) = jac_y; 53 | dW_dp(nr+1:2*nr, 5*nc+1:6*nc) = -jac_y .* u_y; 54 | dW_dp(nr+1:2*nr, 6*nc+1:7*nc) = jac_zero; 55 | dW_dp(nr+1:2*nr, 7*nc+1:8*nc) = jac_one; 56 | 57 | 58 | 59 | 60 | % dW_dp = [jac_x, jac_zero, -jac_x .* u_x, jac_y, jac_zero, -jac_y .* u_x, jac_one, jac_zero; 61 | % jac_zero, jac_x, -jac_x .* u_y, jac_zero, jac_y, -jac_y .* u_y, jac_zero, jac_one]; -------------------------------------------------------------------------------- /ICG_functions/private/normalise2dpts.m: -------------------------------------------------------------------------------- 1 | % NORMALISE2DPTS - normalises 2D homogeneous points 2 | % 3 | % Function translates and normalises a set of 2D homogeneous points 4 | % so that their centroid is at the origin and their mean distance from 5 | % the origin is sqrt(2). This process typically improves the 6 | % conditioning of any equations used to solve homographies, fundamental 7 | % matrices etc. 8 | % 9 | % Usage: [newpts, T] = normalise2dpts(pts) 10 | % 11 | % Argument: 12 | % pts - 3xN array of 2D homogeneous coordinates 13 | % 14 | % Returns: 15 | % newpts - 3xN array of transformed 2D homogeneous coordinates. The 16 | % scaling parameter is normalised to 1 unless the point is at 17 | % infinity. 18 | % T - The 3x3 transformation matrix, newpts = T*pts 19 | % 20 | % If there are some points at infinity the normalisation transform 21 | % is calculated using just the finite points. Being a scaling and 22 | % translating transform this will not affect the points at infinity. 23 | 24 | % Peter Kovesi 25 | % School of Computer Science & Software Engineering 26 | % The University of Western Australia 27 | % pk at csse uwa edu au 28 | % http://www.csse.uwa.edu.au/~pk 29 | % 30 | % May 2003 - Original version 31 | % February 2004 - Modified to deal with points at infinity. 32 | 33 | 34 | function [newpts, T] = normalise2dpts(pts) 35 | 36 | if size(pts,1) ~= 3 37 | error('pts must be 3xN'); 38 | end 39 | 40 | % Find the indices of the points that are not at infinity 41 | finiteind = find(abs(pts(3,:)) > eps); 42 | 43 | if length(finiteind) ~= size(pts,2) 44 | warning('Some points are at infinity'); 45 | end 46 | 47 | % If only one point available do nothing 48 | if(size(pts,2)<2) 49 | T = eye(3); 50 | newpts = pts; 51 | return; 52 | end 53 | 54 | % For the finite points ensure homogeneous coords have scale of 1 55 | pts(1,finiteind) = pts(1,finiteind)./pts(3,finiteind); 56 | pts(2,finiteind) = pts(2,finiteind)./pts(3,finiteind); 57 | pts(3,finiteind) = 1; 58 | 59 | c = mean(pts(1:2,finiteind),2); % Centroid of finite points 60 | newp(1,finiteind) = pts(1,finiteind)-c(1); % Shift origin to centroid. 61 | newp(2,finiteind) = pts(2,finiteind)-c(2); 62 | 63 | meandist = mean(sqrt(newp(1,finiteind).^2 + newp(2,finiteind).^2)); 64 | 65 | scale = sqrt(2)/meandist; 66 | 67 | T = [scale 0 -scale*c(1) 68 | 0 scale -scale*c(2) 69 | 0 0 1 ]; 70 | 71 | newpts = T*pts; 72 | 73 | 74 | -------------------------------------------------------------------------------- /ICG_functions/private/normxcorr2_mex.mexa64: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RobVisLab/camera_calibration/f56c17c297653ba0a156d125c2b162b19432b48b/ICG_functions/private/normxcorr2_mex.mexa64 -------------------------------------------------------------------------------- /ICG_functions/private/normxcorr2_mex.mexw64: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RobVisLab/camera_calibration/f56c17c297653ba0a156d125c2b162b19432b48b/ICG_functions/private/normxcorr2_mex.mexw64 -------------------------------------------------------------------------------- /ICG_functions/private/opencv_core2411.dll: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RobVisLab/camera_calibration/f56c17c297653ba0a156d125c2b162b19432b48b/ICG_functions/private/opencv_core2411.dll -------------------------------------------------------------------------------- /ICG_functions/private/opencv_imgproc2411.dll: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RobVisLab/camera_calibration/f56c17c297653ba0a156d125c2b162b19432b48b/ICG_functions/private/opencv_imgproc2411.dll -------------------------------------------------------------------------------- /ICG_functions/private/quadtobox_h.m: -------------------------------------------------------------------------------- 1 | function wimg = quadtobox_h(xgrid, ygrid, img, dst, M, tmplt_x_grid, tmplt_y_grid, f_type) 2 | % QUADTOBOX_HOMO - Warp contents of a quadrilateral to a rectangle 3 | % W = QUADTOBOX_HOMO(IMG, DST, M, F_TYPE) 4 | % 5 | % Destination corners, DST = [x1, x2, x3, x4; y1, y2, y3, y4]; 6 | % These are the (assumed rectangular) corner points of the template 7 | % image and must be integers. 8 | % 9 | % The projection matrix, M transforms DST into the image IMG 10 | % [u' v' w] (image) = M * [x y 1] (DST - template); 11 | % u = u' / w; v = v' / w; 12 | % 13 | % Matlab style indices, i.e. start from 1. 14 | 15 | % Iain Matthews, Simon Baker, Carnegie Mellon University, Pittsburgh 16 | % $Id: quadtobox_h.m,v 1.5 2008-09-19 12:45:39 ruether Exp $ 17 | 18 | % Check args 19 | if nargin<8 f_type = 'bilinear'; end 20 | if nargin<7 error('Invalid args'); end 21 | 22 | % Dimensions of destination image - integers, assume rectangle 23 | minv = min(dst'); 24 | maxv = max(dst'); 25 | 26 | % Get all points in destination to sample 27 | % [tmplt_x_grid tmplt_y_grid] = meshgrid(1:maxv(1), 1:maxv(2)); 28 | 29 | xy = [reshape(tmplt_x_grid, prod(size(tmplt_x_grid)), 1)'; reshape(tmplt_y_grid, prod(size(tmplt_y_grid)), 1)']; 30 | xy = [xy; ones(1,size(xy,2))]; 31 | 32 | % Transform into source 33 | uv = M * xy; 34 | 35 | % Divide for homography 36 | uv = uv ./ repmat(uv(3,:),3,1); 37 | 38 | % Remove homogeneous 39 | uv = uv(1:2,:)'; 40 | 41 | % Sample 42 | xi = reshape(uv(:,1),maxv(2),maxv(1)); 43 | yi = reshape(uv(:,2),maxv(2),maxv(1)); 44 | wimg = ICG_interp2(xgrid, ygrid, img, xi, yi, f_type); 45 | 46 | % Check for NaN background pixels - replace them with a background of 0 47 | idx = find(isnan(wimg)); 48 | if ~isempty(idx) 49 | wimg(idx) = 0; 50 | end 51 | -------------------------------------------------------------------------------- /ICG_functions/private/sd_images.m: -------------------------------------------------------------------------------- 1 | function VI_dW_dp = sd_images(dW_dp, nabla_Ix, nabla_Iy, N_p, h, w) 2 | % SD_IMAGES - Compute steepest descent images 3 | % VI_DW_DP = SD_IMAGES(DW_DP, NABLA_IX, NABLA_IY, N_P, H, W) 4 | 5 | % Iain Matthews, Simon Baker, Carnegie Mellon University, Pittsburgh 6 | % $Id: sd_images.m,v 1.1 2006-05-31 15:21:32 ruether Exp $ 7 | 8 | if nargin<6 error('Not enough input arguments'); end 9 | 10 | for p=1:N_p 11 | Tx = nabla_Ix .* dW_dp(1:h,((p-1)*w)+1:((p-1)*w)+w); 12 | Ty = nabla_Iy .* dW_dp(h+1:end,((p-1)*w)+1:((p-1)*w)+w); 13 | VI_dW_dp(:,((p-1)*w)+1:((p-1)*w)+w) = Tx + Ty; 14 | end 15 | -------------------------------------------------------------------------------- /ICG_functions/private/sd_update.m: -------------------------------------------------------------------------------- 1 | function sd_delta_p = sd_update(VI_dW_dp, error_img, N_p, w) 2 | % SD_UPDATE - Compute steepest descent parameter updates 3 | % SD_DELTA_P = SD_UPDATE(VI_DW_DP, ERROR_IMG, N_P, W) 4 | 5 | % Iain Matthews, Simon Baker, Carnegie Mellon University, Pittsburgh 6 | % $Id: sd_update.m,v 1.2 2007-05-03 12:22:37 ruether Exp $ 7 | 8 | if nargin<4 error('Not enough input arguments'); end 9 | 10 | sd_delta_p = zeros(N_p, 1); 11 | for p=1:N_p 12 | h1 = VI_dW_dp(:,((p-1)*w)+1:((p-1)*w)+w); 13 | product = h1 .* error_img; 14 | sum1 = sum(product); 15 | sd_delta_p(p) = sum(sum1); 16 | end 17 | -------------------------------------------------------------------------------- /ICG_functions/private/selectBestMatch.m: -------------------------------------------------------------------------------- 1 | function [marker_corners, marker_similarity, maxIdx, maxTemplateIdx] = selectBestMatch(matches, isTemplateArray) 2 | 3 | if nargin < 2 4 | isTemplateArray = 0; 5 | end 6 | 7 | if isTemplateArray ~= 0 8 | marker_similarity = -Inf; 9 | marker_corners = []; 10 | maxIdx = 0; 11 | for tIdx = 1:length(matches) 12 | [mc, msim, midx] = select(matches{tIdx}); 13 | if msim > marker_similarity 14 | marker_similarity = msim; 15 | marker_corners = mc; 16 | maxIdx = midx; 17 | maxTemplateIdx = tIdx; 18 | end 19 | end 20 | else 21 | maxTemplateIdx = 1; 22 | [marker_corners, marker_similarity, maxIdx] = select(matches); 23 | end 24 | 25 | 26 | function [marker_corners, marker_similarity, maxIdx] = select(matches) 27 | 28 | marker_similarity = -Inf; 29 | marker_corners = []; 30 | maxIdx = 0; 31 | for i = 1:length(matches), 32 | if ~isempty(matches{i}) && matches{i}.similarity > marker_similarity, 33 | marker_similarity = matches{i}.similarity; 34 | marker_corners = matches{i}.pattern_corners; 35 | maxIdx = i; 36 | end 37 | end 38 | -------------------------------------------------------------------------------- /ICG_functions/private/warp_h.m: -------------------------------------------------------------------------------- 1 | function wimg = warp_h(xgrid, ygrid, img, p, dst, tmplt_x_grid, tmplt_y_grid) 2 | % WARP_H - Projective warp the image 3 | % WIMG = WARP_H(xgrid, ygrid, IMG, P, DST) 4 | % Warp image IMG to WIMG. DST are the destination points, i.e. the corners 5 | % of the template image. P are the affine warp parameters that project 6 | % DST into IMG. 7 | % 8 | % P = [p1, p4, p7 (i.e. transpose of what is in LK20-1) 9 | % p2, p5, p8 10 | % p3, p6, 1]; 11 | 12 | % Iain Matthews, Simon Baker, Carnegie Mellon University, Pittsburgh 13 | % $Id: warp_h.m,v 1.3 2006-11-07 09:42:18 ruether Exp $ 14 | 15 | if nargin<7 error('Not enough input arguments'); end 16 | 17 | M = p; 18 | M(1,1) = M(1,1) + 1; 19 | M(2,2) = M(2,2) + 1; 20 | 21 | % Use bilinear filtering to warp back to template 22 | wimg = quadtobox_h(xgrid, ygrid, img, dst, M, tmplt_x_grid, tmplt_y_grid, 'bilinear'); 23 | -------------------------------------------------------------------------------- /LICENSE.txt: -------------------------------------------------------------------------------- 1 | The MIT License (MIT) 2 | 3 | Copyright (c) 2016 David Ferstl 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 | 23 | -------------------------------------------------------------------------------- /TOOLBOX_calib/Compute3D.m: -------------------------------------------------------------------------------- 1 | function [Xc,Xp] = Compute3D(xc,xp,R,T,fc,fp,cc,cp,kc,kp,alpha_c,alpha_p); 2 | 3 | % [Xc,Xp] = Compute3D(xc,xp,R,T,fc,fp,cc,cp,kc,kp,alpha_c,alpha_p); 4 | % 5 | % Reconstruction of the 3D structure of the striped object. 6 | % 7 | % Xc : The 3D coordinates of the points in the camera reference frame 8 | % Xp : The 3D coordinates of the points in the projector reference frame 9 | % 10 | % xc, xp: Camera coordinates and projector coordinates from ComputeStripes 11 | % R,T : rigid motion from camera to projector: Xp = R*Xc + T 12 | % fc,fp : Camera and Projector focal lengths 13 | % cc,cp : Camera and Projector center of projection 14 | % kc,kp : Camera and Projector distortion factors 15 | % alpha_c, alpha_p: skew coefficients for camera and projector 16 | % 17 | % The set R,T,fc,fp,cc,cp and kc comes from the calibration. 18 | 19 | % Intel Corporation - Dec. 2003 20 | % (c) Jean-Yves Bouguet 21 | 22 | 23 | if nargin < 12, 24 | alpha_p = 0; 25 | if nargin < 11, 26 | alpha_c = 0; 27 | end; 28 | end; 29 | 30 | 31 | Np = size(xc,2); 32 | 33 | 34 | xc = normalize_pixel(xc,fc,cc,kc,alpha_c); 35 | 36 | xp = (xp - cp(1))/fp(1); 37 | 38 | xp_save = xp; % save the real distorted x - coordinates + alpha'ed 39 | 40 | 41 | if (norm(kp) == 0)&(alpha_p == 0), 42 | N_rep = 1; 43 | else 44 | N_rep = 5; 45 | end; 46 | 47 | 48 | % xp is the first entry of the undistorted projector coordinates (iteratively refined) 49 | % xc is the complete undistorted camera coordinates 50 | for kk = 1:N_rep, 51 | 52 | R2 = R([1 3],:); 53 | if length(T) > 2, 54 | Tp = T([1 3]); % The old technique for calibration 55 | else 56 | Tp = T; % The new technique for calibration (using stripes only) 57 | end; 58 | 59 | % Triangulation: 60 | 61 | D1 = [-xc(1,:);xc(1,:).*xp(1,:);-xc(2,:);xc(2,:).*xp(1,:);-ones(1,Np);xp(1,:)]; 62 | D2 = R2(:)*ones(1,Np); 63 | 64 | D = sum(D1.*D2); 65 | N1 = [-ones(1,Np);xp(1,:)]; 66 | N2 = -sum(N1.*(Tp*ones(1,Np))); 67 | Z = N2./D; 68 | Xc = (ones(3,1)*Z).*[xc;ones(1,Np)]; 69 | 70 | % reproject on the projetor view, and apply distortion... 71 | 72 | Xp = R*Xc + T*ones(1,Np); 73 | 74 | xp_v = [Xp(1,:)./Xp(3,:); Xp(2,:)./Xp(3,:)]; 75 | 76 | xp_v(1,:) = xp_v(1,:) + alpha_p * xp_v(2,:); 77 | 78 | xp_dist = apply_distortion(xp_v,kp); 79 | 80 | %norm(xp_dist(1,:) - xp_save) 81 | 82 | xp_dist(1,:) = xp_save; 83 | 84 | xp_v = comp_distortion(xp_dist,kp); 85 | 86 | xp_v(1,:) = xp_v(1,:) - alpha_p * xp_v(2,:); 87 | 88 | xp = xp_v(1,:); 89 | 90 | end; 91 | -------------------------------------------------------------------------------- /TOOLBOX_calib/README.txt: -------------------------------------------------------------------------------- 1 | Camera Calibration Toolbox for Matlab 2 | ------------------------------------- 3 | 4 | The complete documentation of this toolbox is available online at 5 | http://www.vision.caltech.edu/bouguetj/calib_doc/ 6 | 7 | The lastest version of the toolbox can also be downloaded from this web site. 8 | 9 | If you have any question/suggestion/bug report please send me email at 10 | jean-yves.bouguet@intel.com 11 | 12 | 13 | Enjoy the toolbox! 14 | 15 | 16 | Jean-Yves Bouguet 17 | Intel Corporation 18 | 19 | 20 | October 15th, 2004 -------------------------------------------------------------------------------- /TOOLBOX_calib/Rectangle2Square.m: -------------------------------------------------------------------------------- 1 | function [square] = Rectangle2Square(rectangle,L,W); 2 | 3 | % Generate the square from a rectangle of known segment lengths 4 | % from pt1 to pt2 : L 5 | % from pt2 to pt3 : W 6 | 7 | [u_hori,u_vert] = UnWarpPlane(rectangle); 8 | 9 | coeff_x = sqrt(W/L); 10 | coeff_y = 1/coeff_x; 11 | 12 | x_coord = [ 0 coeff_x coeff_x 0]; 13 | y_coord = [ 0 0 coeff_y coeff_y]; 14 | 15 | 16 | square = rectangle(:,1) * ones(1,4) + u_hori*x_coord + u_vert*y_coord; 17 | square = square ./ (ones(3,1)*square(3,:)); 18 | 19 | 20 | -------------------------------------------------------------------------------- /TOOLBOX_calib/UnWarpPlane.m: -------------------------------------------------------------------------------- 1 | function [u_hori,u_vert] = UnWarpPlane(x1,x2,x3,x4); 2 | 3 | % Recovers the two 3D directions of the rectangular patch x1x2x3x4 4 | % x1 is the origin point, ie any point of planar coordinate (x,y) on the 5 | % rectangular patch will be projected on the image plane at: 6 | % x1 + x * u_hori + y * u_vert 7 | % 8 | % Note: u_hori and u_vert are also the two vanishing points. 9 | 10 | 11 | if nargin < 4, 12 | 13 | x4 = x1(:,4); 14 | x3 = x1(:,3); 15 | x2 = x1(:,2); 16 | x1 = x1(:,1); 17 | 18 | end; 19 | 20 | 21 | % Image Projection: 22 | L1 = cross(x1,x2); 23 | L2 = cross(x4,x3); 24 | L3 = cross(x2,x3); 25 | L4 = cross(x1,x4); 26 | 27 | % Vanishing point: 28 | V1 = cross(L1,L2); 29 | V2 = cross(L3,L4); 30 | 31 | % Horizon line: 32 | H = cross(V1,V2); 33 | 34 | if H(3) < 0, H = -H; end; 35 | 36 | 37 | H = H / norm(H); 38 | 39 | 40 | X1 = x1 / dot(H,x1); 41 | X2 = x2 / dot(H,x2); 42 | X3 = x3 / dot(H,x3); 43 | X4 = x4 / dot(H,x4); 44 | 45 | scale = X1(3); 46 | 47 | X1 = X1/scale; 48 | X2 = X2/scale; 49 | X3 = X3/scale; 50 | X4 = X4/scale; 51 | 52 | 53 | u_hori = X2 - X1; 54 | u_vert = X4 - X1; 55 | -------------------------------------------------------------------------------- /TOOLBOX_calib/affine.m: -------------------------------------------------------------------------------- 1 | function A = affine(X,n,om,T,fc,cc,alpha_c) 2 | 3 | if nargin < 7, 4 | alpha_c = 0; 5 | if nargin < 6, 6 | cc = [0;0]; 7 | if nargin < 5, 8 | fc = [1;1]; 9 | if nargin < 4, 10 | T = zeros(3,1); 11 | if nargin < 3, 12 | om = zeros(3,1); 13 | if nargin < 2, 14 | n = [0;0;-1]; 15 | if nargin < 1, 16 | error('Error: affine needs some arguments: A = affine(X,n,om,T,fc,cc,alpha_c);'); 17 | end; 18 | end; 19 | end; 20 | end; 21 | end; 22 | end; 23 | end; 24 | 25 | 26 | KK = [fc(1) alpha_c*fc(1) cc(1); 0 fc(2) cc(2);0 0 1]; 27 | R = rodrigues(om); 28 | omega = n / dot(n,X); 29 | x = X/X(3); 30 | 31 | H = KK * [R + T*omega'] * inv(KK); 32 | 33 | A = (H(3,3)*H(1:2,1:2) - H(1:2,3)*H(3,1:2) + (H(3,2)*H(1:2,1) - H(3,1)*H(1:2,2))*[x(2) -x(1)])/(H(3,:)*x)^2; 34 | -------------------------------------------------------------------------------- /TOOLBOX_calib/align_structures.m: -------------------------------------------------------------------------------- 1 | function [om,T,Y] = align_structures(X1,X2), 2 | 3 | % Find om (R) and T, such that Y = R*X1 + T is as close as 4 | % possible to X2. 5 | 6 | [m,n] = size(X1); 7 | 8 | % initialization: 9 | 10 | 11 | 12 | 13 | % initialize param to no motion: 14 | param = zeros(6,1); 15 | change = 1; 16 | 17 | while change > 1e-6, 18 | 19 | om = param(1:3); 20 | T = param(4:6); 21 | 22 | [Y,dYdom,dYdT] = rigid_motion(X1,om,T); 23 | 24 | J = [dYdom dYdT]; 25 | 26 | err = X2(:) - Y(:); 27 | 28 | param_up = inv(J'*J)*J'*err; 29 | 30 | param = param + param_up; 31 | 32 | change = norm(param_up)/norm(param); 33 | 34 | end; 35 | 36 | om = param(1:3); 37 | 38 | T = param(4:6); 39 | 40 | [Y,dYdom,dYdT] = rigid_motion(X1,om,T); 41 | -------------------------------------------------------------------------------- /TOOLBOX_calib/anisdiff.m: -------------------------------------------------------------------------------- 1 | function Is = anisdiff(I,sigI,NIter); 2 | 3 | if nargin < 3, 4 | NIter = 4; 5 | if nargin < 2, 6 | sigI = 10; 7 | end; 8 | end; 9 | 10 | % Function that diffuse an image anisotropially. 11 | 12 | Is = I; 13 | 14 | [ny,nx] = size(I); 15 | 16 | c_n = zeros(ny-2,nx-2); 17 | c_s = zeros(ny-2,nx-2); 18 | c_w = zeros(ny-2,nx-2); 19 | c_e = zeros(ny-2,nx-2); 20 | c_nw = zeros(ny-2,nx-2); 21 | c_ne = zeros(ny-2,nx-2); 22 | c_sw = zeros(ny-2,nx-2); 23 | c_se = zeros(ny-2,nx-2); 24 | c_c = ones(ny-2,nx-2); 25 | 26 | 27 | for k=1:NIter, 28 | 29 | dI_n = Is(2:end-1,2:end-1) - Is(1:end-2,2:end-1); 30 | dI_s = Is(2:end-1,2:end-1) - Is(3:end,2:end-1); 31 | dI_w = Is(2:end-1,2:end-1) - Is(2:end-1,1:end-2); 32 | dI_e = Is(2:end-1,2:end-1) - Is(2:end-1,3:end); 33 | dI_nw = Is(2:end-1,2:end-1) - Is(1:end-2,1:end-2); 34 | dI_ne = Is(2:end-1,2:end-1) - Is(1:end-2,3:end); 35 | dI_sw = Is(2:end-1,2:end-1) - Is(3:end,1:end-2); 36 | dI_se = Is(2:end-1,2:end-1) - Is(3:end,3:end); 37 | 38 | 39 | c_n = exp(-.5*(dI_n/sigI).^2)/8; 40 | c_s = exp(-.5*(dI_s/sigI).^2)/8; 41 | c_w = exp(-.5*(dI_w/sigI).^2)/8; 42 | c_e = exp(-.5*(dI_e/sigI).^2)/8; 43 | c_nw = exp(-.5*(dI_nw/sigI).^2)/16; 44 | c_ne = exp(-.5*(dI_ne/sigI).^2)/16; 45 | c_sw = exp(-.5*(dI_sw/sigI).^2)/16; 46 | c_se = exp(-.5*(dI_se/sigI).^2)/16; 47 | 48 | c_c = 1 - (c_n + c_s + c_w + c_e + c_nw + c_ne + c_sw + c_se); 49 | 50 | 51 | Is(2:end-1,2:end-1) = c_c .* Is(2:end-1,2:end-1) + c_n .* Is(1:end-2,2:end-1) + c_s .* Is(3:end,2:end-1) + ... 52 | c_w .* Is(2:end-1,1:end-2) + c_e .* Is(2:end-1,3:end) + c_nw .* Is(1:end-2,1:end-2) + c_ne .* Is(1:end-2,3:end) + ... 53 | c_sw .* Is(3:end,1:end-2) + c_se .* Is(3:end,3:end); 54 | 55 | end; 56 | -------------------------------------------------------------------------------- /TOOLBOX_calib/apply_distortion.m: -------------------------------------------------------------------------------- 1 | function [xd,dxddk] = apply_distortion(x,k) 2 | 3 | 4 | % Complete the distortion vector if you are using the simple distortion model: 5 | length_k = length(k); 6 | if length_k <5 , 7 | k = [k ; zeros(5-length_k,1)]; 8 | end; 9 | 10 | 11 | [m,n] = size(x); 12 | 13 | % Add distortion: 14 | 15 | r2 = x(1,:).^2 + x(2,:).^2; 16 | 17 | r4 = r2.^2; 18 | 19 | r6 = r2.^3; 20 | 21 | 22 | % Radial distortion: 23 | 24 | cdist = 1 + k(1) * r2 + k(2) * r4 + k(5) * r6; 25 | 26 | if nargout > 1, 27 | dcdistdk = [ r2' r4' zeros(n,2) r6']; 28 | end; 29 | 30 | 31 | xd1 = x .* (ones(2,1)*cdist); 32 | 33 | coeff = (reshape([cdist;cdist],2*n,1)*ones(1,3)); 34 | 35 | if nargout > 1, 36 | dxd1dk = zeros(2*n,5); 37 | dxd1dk(1:2:end,:) = (x(1,:)'*ones(1,5)) .* dcdistdk; 38 | dxd1dk(2:2:end,:) = (x(2,:)'*ones(1,5)) .* dcdistdk; 39 | end; 40 | 41 | 42 | % tangential distortion: 43 | 44 | a1 = 2.*x(1,:).*x(2,:); 45 | a2 = r2 + 2*x(1,:).^2; 46 | a3 = r2 + 2*x(2,:).^2; 47 | 48 | delta_x = [k(3)*a1 + k(4)*a2 ; 49 | k(3) * a3 + k(4)*a1]; 50 | 51 | aa = (2*k(3)*x(2,:)+6*k(4)*x(1,:))'*ones(1,3); 52 | bb = (2*k(3)*x(1,:)+2*k(4)*x(2,:))'*ones(1,3); 53 | cc = (6*k(3)*x(2,:)+2*k(4)*x(1,:))'*ones(1,3); 54 | 55 | if nargout > 1, 56 | ddelta_xdk = zeros(2*n,5); 57 | ddelta_xdk(1:2:end,3) = a1'; 58 | ddelta_xdk(1:2:end,4) = a2'; 59 | ddelta_xdk(2:2:end,3) = a3'; 60 | ddelta_xdk(2:2:end,4) = a1'; 61 | end; 62 | 63 | xd = xd1 + delta_x; 64 | 65 | if nargout > 1, 66 | dxddk = dxd1dk + ddelta_xdk ; 67 | if length_k < 5, 68 | dxddk = dxddk(:,1:length_k); 69 | end; 70 | end; 71 | 72 | 73 | return; 74 | 75 | % Test of the Jacobians: 76 | 77 | n = 10; 78 | 79 | lk = 1; 80 | 81 | x = 10*randn(2,n); 82 | k = 0.5*randn(lk,1); 83 | 84 | [xd,dxddk] = apply_distortion(x,k); 85 | 86 | 87 | % Test on k: OK!! 88 | 89 | dk = 0.001 * norm(k)*randn(lk,1); 90 | k2 = k + dk; 91 | 92 | [x2] = apply_distortion(x,k2); 93 | 94 | x_pred = xd + reshape(dxddk * dk,2,n); 95 | 96 | 97 | norm(x2-xd)/norm(x2 - x_pred) 98 | -------------------------------------------------------------------------------- /TOOLBOX_calib/apply_distortion2.m: -------------------------------------------------------------------------------- 1 | function [xd,dxddk,dxddx] = apply_distortion2(x,k) 2 | 3 | 4 | [m,n] = size(x); 5 | 6 | % Add distortion: 7 | 8 | r2 = x(1,:).^2 + x(2,:).^2; 9 | 10 | r4 = r2.^2; 11 | 12 | r6 = r2.^3; 13 | 14 | 15 | % Radial distortion: 16 | 17 | cdist = 1 + k(1) * r2 + k(2) * r4 + k(5) * r6; 18 | 19 | if nargout > 1, 20 | dcdistdk = [ r2' r4' zeros(n,2) r6']; 21 | end; 22 | 23 | 24 | xd1 = x .* (ones(2,1)*cdist); 25 | 26 | coeff = (reshape([cdist;cdist],2*n,1)*ones(1,3)); 27 | 28 | if nargout > 1, 29 | dxd1dk = zeros(2*n,5); 30 | dxd1dk(1:2:end,:) = (x(1,:)'*ones(1,5)) .* dcdistdk; 31 | dxd1dk(2:2:end,:) = (x(2,:)'*ones(1,5)) .* dcdistdk; 32 | end; 33 | 34 | 35 | % tangential distortion: 36 | 37 | a1 = 2.*x(1,:).*x(2,:); 38 | a2 = r2 + 2*x(1,:).^2; 39 | a3 = r2 + 2*x(2,:).^2; 40 | 41 | delta_x = [k(3)*a1 + k(4)*a2 ; 42 | k(3) * a3 + k(4)*a1]; 43 | 44 | aa = (2*k(3)*x(2,:)+6*k(4)*x(1,:))'*ones(1,3); 45 | bb = (2*k(3)*x(1,:)+2*k(4)*x(2,:))'*ones(1,3); 46 | cc = (6*k(3)*x(2,:)+2*k(4)*x(1,:))'*ones(1,3); 47 | 48 | if nargout > 1, 49 | ddelta_xdk = zeros(2*n,5); 50 | ddelta_xdk(1:2:end,3) = a1'; 51 | ddelta_xdk(1:2:end,4) = a2'; 52 | ddelta_xdk(2:2:end,3) = a3'; 53 | ddelta_xdk(2:2:end,4) = a1'; 54 | end; 55 | 56 | xd = xd1 + delta_x; 57 | 58 | if nargout > 1, 59 | dxddk = dxd1dk + ddelta_xdk ; 60 | end; 61 | 62 | if nargout > 2, 63 | d1 = 1 + k(1).*a2' + k(2).*r2'.*(a2+2*x(1,:).^2)' + k(5).*r4'.*(a2+4*x(1,:).^2)' + 2*k(3).*x(2,:)' + 6*k(4).*x(1,:)'; 64 | d2 = 1 + k(1).*a3' + k(2).*r2'.*(a3+2*x(2,:).^2)' + k(5).*r4'.*(a3+4*x(2,:).^2)' + 6*k(3).*x(2,:)' + 2*k(4).*x(1,:)'; 65 | d3 = (k(1) + 2*k(2).*r2' + 3*k(5).*r4').*a1' + 2*k(3).*x(1,:)' + 2*k(4).*x(2,:)'; 66 | 67 | i = [1:2:2*n 1:2:2*n 2:2:2*n 2:2:2*n]; 68 | j = [1:2:2*n 2:2:2*n 1:2:2*n 2:2:2*n]; 69 | s = [d1' d3' d3' d2']; 70 | 71 | dxddx = sparse(i, j, s, 2*n, 2*n); 72 | end 73 | 74 | return; 75 | 76 | % Test of the Jacobians: 77 | 78 | n = 10; 79 | 80 | x = 10*randn(2,n); 81 | k = 0.5*randn(5,1); 82 | 83 | [xd,dxddk,dxddx] = apply_distortion2(x,k); 84 | 85 | 86 | % Test on k: OK!! 87 | 88 | dk = 0.001 * norm(k)*randn(5,1); 89 | k2 = k + dk; 90 | 91 | [x2] = apply_distortion2(x,k2); 92 | 93 | x_pred = xd + reshape(dxddk * dk,2,n); 94 | 95 | 96 | norm(x2-xd)/norm(x2 - x_pred) 97 | 98 | % Test on x: 99 | dx = 0.000001 *randn(2,n); 100 | x2 = x + dx; 101 | 102 | xd2 = apply_distortion2(x2,k); 103 | 104 | x_pred = xd + reshape(dxddx*dx(:),2,n); 105 | 106 | norm(xd2-xd)/norm(xd2-x_pred) -------------------------------------------------------------------------------- /TOOLBOX_calib/apply_fisheye_distortion.m: -------------------------------------------------------------------------------- 1 | function [xd, dxd_dx] = apply_fisheye_distortion(x,k) 2 | 3 | %apply_fisheye_distortion.m 4 | % 5 | %[x] = apply_fisheye_distortion(xd,k) 6 | % 7 | %Apply the fisheye distortions 8 | % 9 | %INPUT: x: undistorted (normalized) point coordinates in the image plane (2xN matrix) 10 | % k: Fisheye distortion coefficients (5x1 vector) 11 | % 12 | %OUTPUT: xd: distorted (normalized) point coordinates in the image plane (2xN matrix) 13 | % dxd_dx : jacobian (2x2xN matrix) 14 | 15 | r = sqrt(x(1,:).^2 + x(2,:).^2); 16 | 17 | theta = atan(r); 18 | theta_d = theta .* (1 + k(1)*theta.^2 + k(2)*theta.^4 + k(3)*theta.^6 + k(4)*theta.^8); 19 | 20 | scaling = ones(1,length(r)); 21 | 22 | ind_good = find(r > 1e-8); 23 | 24 | scaling(ind_good) = theta_d(ind_good) ./ r(ind_good); 25 | 26 | xd = x .* (ones(2,1)*scaling); 27 | 28 | if (nargout > 1) 29 | n = size(x,2); 30 | dr_dx = x; % 2xn 31 | dr_dx(:,ind_good) = dr_dx(:,ind_good) ./ repmat(r(ind_good), [2,1]); 32 | 33 | theta_d_r2 = scaling; 34 | theta_d_r2(ind_good) = theta_d_r2(ind_good) ./ r(ind_good); 35 | 36 | dtheta_dr = 1 ./ (1 + r.^2); 37 | dtheta_d_dtheta = 1 + 3*k(1)*theta.^2 + 5*k(2)*theta.^4 + ... 38 | 7*k(3)*theta.^6 + 9*k(4)*theta.^8; 39 | dtheta_d_dr_r = dtheta_d_dtheta .* dtheta_dr; % 1xn 40 | dtheta_d_dr_r(ind_good) = dtheta_d_dr_r(ind_good) ./ r(ind_good); 41 | 42 | dxd_dx = zeros(2,2,n); 43 | for i = 1:n 44 | dxd_dx(:,:,i) = scaling(i)*eye(2,2) + x(:,i)*(dtheta_d_dr_r(i) - ... 45 | theta_d_r2(i))*dr_dx(:,i)'; 46 | end 47 | end 48 | -------------------------------------------------------------------------------- /TOOLBOX_calib/calib.m: -------------------------------------------------------------------------------- 1 | function calib(mode), 2 | 3 | % calib(mode) 4 | % 5 | % Runs the Camera Calibration Toolbox. 6 | % Set mode to 1 to run the memory efficient version. 7 | % Any other value for mode will run the normal version (see documentation) 8 | 9 | if nargin < 1, 10 | calib_gui; 11 | else 12 | calib_gui(mode); 13 | end; -------------------------------------------------------------------------------- /TOOLBOX_calib/calib_gui.m: -------------------------------------------------------------------------------- 1 | %function calib_gui(mode) 2 | 3 | % calib_gui(mode) 4 | % 5 | % Runs the Camera Calibration Toolbox. 6 | % Set mode to 1 to run the memory efficient version. 7 | % Any other value for mode will run the normal version (see documentation) 8 | % 9 | % INFORMATION ABOUT THE MEMORY EFFICIENT MODE FOR THE CAMERA CALIBRATION TOOLBOX: 10 | % 11 | % If your calibration images are large, or if you calibrate using a lot of images, you may have experienced memory problems 12 | % in Matlab when using the calibration toolbox (OUT OF MEMORY errors). If this is the case, you can now run the 13 | % new memory efficient version of the toolbox that loads every image one by one without storing them all in memory. 14 | % If you choose to run the standard version of the toolbox now, you can always switch to the other memory efficient mode 15 | % later in case the OUT OF MEMORY error message is encountered. The two modes of operation are totally compatible. 16 | 17 | cell_list = {}; 18 | 19 | fig_number = 1; 20 | 21 | title_figure = 'Camera Calibration Toolbox - Select mode of operation:'; 22 | 23 | cell_list{1,1} = {'Standard (all the images are stored in memory)','calib_gui_normal;'}; 24 | cell_list{2,1} = {'Memory efficient (the images are loaded one by one)','calib_gui_no_read;'}; 25 | cell_list{3,1} = {'Exit',['disp(''Bye. To run again, type calib_gui.''); close(' num2str(fig_number) ');']}; 26 | 27 | show_window(cell_list,fig_number,title_figure,290,18,0,'clean',12); -------------------------------------------------------------------------------- /TOOLBOX_calib/calib_gui_fisheye.m: -------------------------------------------------------------------------------- 1 | %function calib_gui_no_read, 2 | 3 | 4 | cell_list = {}; 5 | 6 | 7 | %-------- Begin editable region -------------% 8 | %-------- Begin editable region -------------% 9 | 10 | 11 | fig_number = 1; 12 | 13 | title_figure = 'Fisheye Camera Calibration Toolbox'; 14 | 15 | cell_list{1,1} = {'Image names','data_calib_no_read;'}; 16 | cell_list{1,2} = {'Read images','ima_read_calib_no_read;'}; 17 | cell_list{1,3} = {'Extract grid corners','click_calib_fisheye_no_read;'}; 18 | cell_list{1,4} = {'Calibration','go_calib_optim_fisheye_no_read;'}; 19 | cell_list{2,1} = {'Show Extrinsic','ext_calib;'}; 20 | cell_list{2,2} = {'Reproject on images','reproject_calib_no_read;'}; 21 | cell_list{2,3} = {'Analyse error','analyse_error;'}; 22 | cell_list{2,4} = {'Recomp. corners','recomp_corner_calib_fisheye_no_read;'}; %recomp_corner_calib_no_read;'}; 23 | cell_list{3,1} = {'Add/Suppress images','add_suppress;'}; 24 | cell_list{3,2} = {'Save','saving_calib_fisheye;'}; 25 | cell_list{3,3} = {'Load','loading_calib;'}; 26 | cell_list{3,4} = {'Exit',['disp(''Bye. To run again, type calib_gui.''); close(' num2str(fig_number) ');']}; %{'Exit','calib_gui;'}; 27 | cell_list{4,1} = {'Comp. Extrinsic','extrinsic_computation;'}; 28 | cell_list{4,2} = {'Undistort image','undistort_image_no_read;'}; 29 | cell_list{4,3} = {'Export calib data','export_calib_data;'}; 30 | cell_list{4,4} = {'Show calib results','show_calib_results_fisheye;'}; 31 | %cell_list{5,1} = {'Smooth images','smooth_images;'}; 32 | 33 | show_window(cell_list,fig_number,title_figure,130,18,0,'clean',12); 34 | 35 | 36 | %-------- End editable region -------------% 37 | %-------- End editable region -------------% 38 | 39 | 40 | -------------------------------------------------------------------------------- /TOOLBOX_calib/calib_gui_no_read.m: -------------------------------------------------------------------------------- 1 | %function calib_gui_no_read, 2 | 3 | 4 | cell_list = {}; 5 | 6 | 7 | %-------- Begin editable region -------------% 8 | %-------- Begin editable region -------------% 9 | 10 | 11 | fig_number = 1; 12 | 13 | title_figure = 'Camera Calibration Toolbox - Memory efficient version'; 14 | 15 | cell_list{1,1} = {'Image names','data_calib_no_read;'}; 16 | cell_list{1,2} = {'Read images','ima_read_calib_no_read;'}; 17 | cell_list{1,3} = {'Extract grid corners','click_calib_no_read;'}; 18 | cell_list{1,4} = {'Calibration','go_calib_optim_no_read;'}; 19 | cell_list{2,1} = {'Show Extrinsic','ext_calib;'}; 20 | cell_list{2,2} = {'Reproject on images','reproject_calib_no_read;'}; 21 | cell_list{2,3} = {'Analyse error','analyse_error;'}; 22 | cell_list{2,4} = {'Recomp. corners','recomp_corner_calib_no_read;'}; 23 | cell_list{3,1} = {'Add/Suppress images','add_suppress;'}; 24 | cell_list{3,2} = {'Save','saving_calib;'}; 25 | cell_list{3,3} = {'Load','loading_calib;'}; 26 | cell_list{3,4} = {'Exit',['disp(''Bye. To run again, type calib_gui.''); close(' num2str(fig_number) ');']}; %{'Exit','calib_gui;'}; 27 | cell_list{4,1} = {'Comp. Extrinsic','extrinsic_computation;'}; 28 | cell_list{4,2} = {'Undistort image','undistort_image_no_read;'}; 29 | cell_list{4,3} = {'Export calib data','export_calib_data;'}; 30 | cell_list{4,4} = {'Show calib results','show_calib_results;'}; 31 | %cell_list{5,1} = {'Smooth images','smooth_images;'}; 32 | 33 | show_window(cell_list,fig_number,title_figure,130,18,0,'clean',12); 34 | 35 | 36 | %-------- End editable region -------------% 37 | %-------- End editable region -------------% 38 | -------------------------------------------------------------------------------- /TOOLBOX_calib/calib_gui_normal.m: -------------------------------------------------------------------------------- 1 | %function calib_gui_normal, 2 | 3 | 4 | cell_list = {}; 5 | 6 | 7 | %-------- Begin editable region -------------% 8 | %-------- Begin editable region -------------% 9 | 10 | 11 | fig_number = 1; 12 | 13 | title_figure = 'Camera Calibration Toolbox - Standard Version'; 14 | 15 | clear fc cc kc KK 16 | kc = zeros(5,1); 17 | clearwin; 18 | 19 | cell_list{1,1} = {'Image names','data_calib;'}; 20 | cell_list{1,2} = {'Read images','ima_read_calib;'}; 21 | cell_list{1,3} = {'Extract grid corners','click_calib;'}; 22 | cell_list{1,4} = {'Calibration','go_calib_optim;'}; 23 | cell_list{2,1} = {'Show Extrinsic','ext_calib;'}; 24 | cell_list{2,2} = {'Reproject on images','reproject_calib;'}; 25 | cell_list{2,3} = {'Analyse error','analyse_error;'}; 26 | cell_list{2,4} = {'Recomp. corners','recomp_corner_calib;'}; 27 | cell_list{3,1} = {'Add/Suppress images','add_suppress;'}; 28 | cell_list{3,2} = {'Save','saving_calib;'}; 29 | cell_list{3,3} = {'Load','loading_calib;'}; 30 | cell_list{3,4} = {'Exit',['disp(''Bye. To run again, type calib_gui.''); close(' num2str(fig_number) ');']}; %{'Exit','calib_gui;'}; 31 | cell_list{4,1} = {'Comp. Extrinsic','extrinsic_computation;'}; 32 | cell_list{4,2} = {'Undistort image','undistort_image;'}; 33 | cell_list{4,3} = {'Export calib data','export_calib_data;'}; 34 | cell_list{4,4} = {'Show calib results','show_calib_results;'}; 35 | %cell_list{5,1} = {'Smooth images','smooth_images;'}; 36 | 37 | 38 | show_window(cell_list,fig_number,title_figure,130,18,0,'clean',12); 39 | 40 | 41 | %-------- End editable region -------------% 42 | %-------- End editable region -------------% 43 | 44 | -------------------------------------------------------------------------------- /TOOLBOX_calib/calib_stereo.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RobVisLab/camera_calibration/f56c17c297653ba0a156d125c2b162b19432b48b/TOOLBOX_calib/calib_stereo.m -------------------------------------------------------------------------------- /TOOLBOX_calib/calibration_pattern/pattern.eps: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RobVisLab/camera_calibration/f56c17c297653ba0a156d125c2b162b19432b48b/TOOLBOX_calib/calibration_pattern/pattern.eps -------------------------------------------------------------------------------- /TOOLBOX_calib/cam_proj_calib_optim.m: -------------------------------------------------------------------------------- 1 | load camera_results; 2 | 3 | string_global = 'global n_ima'; 4 | for kk = 1:n_ima, 5 | string_global = [string_global ' x_' num2str(kk) ' X_' num2str(kk) ' xproj_' num2str(kk) ' x_proj_' num2str(kk)]; 6 | end; 7 | eval(string_global); 8 | 9 | 10 | %----------------- global optimization: --------------------- 11 | 12 | load projector_data; % load the projector corners (previously saved) 13 | load projector_results; 14 | 15 | solution_projector = solution; 16 | 17 | 18 | load camera_results; 19 | 20 | solution_camera = solution; 21 | 22 | param_cam = solution_camera([1:10 16:end]); 23 | param_proj = solution_projector([1:10 16:end]); 24 | 25 | param = [param_cam;param_proj]; 26 | 27 | 28 | % Restart the minimization from here (if need be): 29 | load camera_results; 30 | load calib_cam_proj_optim2; 31 | 32 | 33 | options = [1 1e-4 1e-4 1e-6 0 0 0 0 0 0 0 0 0 12000 0 1e-8 0.1 0]; 34 | 35 | %if 0, % use the full distortion model: 36 | 37 | % fprintf(1,'Take the complete distortion model\n'); 38 | 39 | % test the global error function: 40 | % e_global = error_cam_proj(param); 41 | 42 | % param_init = param; 43 | 44 | % param = leastsq('error_cam_proj',param,options); 45 | 46 | 47 | %else 48 | 49 | % Use a limitd distortion model (no 6th order) 50 | fprintf(1,'Take the 6th order distortion coefficient out\n'); 51 | 52 | param = param([1:9 11:11+6*n_ima-1 11+6*n_ima:11+6*n_ima+9-1 11+6*n_ima+9+1:end]); 53 | 54 | % test the global error function: 55 | e_global2 = error_cam_proj2(param); 56 | 57 | param_init = param; 58 | 59 | param = leastsq('error_cam_proj2',param,options); 60 | 61 | param = [param(1:9);0;param(10:10+6*n_ima-1);param(10+6*n_ima:10+6*n_ima+9-1);0;param(10+6*n_ima+9:end)]; 62 | 63 | %end; 64 | 65 | 66 | 67 | 68 | % Extract the parameters: 69 | 70 | cam_proj_extract_param; 71 | 72 | 73 | % Relative prosition of camera wrt world: 74 | omc = omc_1; 75 | Rc = Rc_1; 76 | Tc = Tc_1; 77 | 78 | % relative position of projector wrt world: 79 | Rp = R*Rc; 80 | omp = rodrigues(Rp); 81 | Tp = T + R*Tc; 82 | 83 | eval(['save calib_cam_proj_optim3 R om T fc fp cc cp alpha_c alpha_p kc kp Rc Rp Tc Tp omc omp param param_init']); 84 | 85 | no_image = 0; 86 | % Image size: (may or may not be available) 87 | nx = 640; 88 | ny = 480; 89 | 90 | comp_error_calib; 91 | 92 | % Save the optimal camera parameters: 93 | saving_calib; 94 | copyfile('Calib_Results.mat','camera_results_optim3.mat'); 95 | delete('Calib_Results.mat'); 96 | 97 | % Save the optimal camera parameters: 98 | fc = fp; 99 | cc = cp; 100 | alpha_c = alpha_p; 101 | kc = kp; 102 | 103 | n_ima = 1; 104 | X_1 = X_proj; 105 | x_1 = x_proj; 106 | omc_1 = om; 107 | Tc_1 = T; 108 | Rc_1 = R; 109 | 110 | % Image size: (may or may not be available) 111 | nx = 1024; 112 | ny = 768; 113 | 114 | % No calibration image is available (only the corner coordinates) 115 | no_image = 1; 116 | 117 | comp_error_calib; 118 | 119 | saving_calib; 120 | copyfile('Calib_Results.mat','projector_results_optim3.mat'); 121 | delete('Calib_Results.mat'); 122 | 123 | -------------------------------------------------------------------------------- /TOOLBOX_calib/cam_proj_extract_param.m: -------------------------------------------------------------------------------- 1 | 2 | % Computation of the errors: 3 | 4 | fc = param(1:2); 5 | cc = param(3:4); 6 | alpha_c = param(5); 7 | kc = param(6:10); 8 | 9 | e_cam = []; 10 | 11 | for kk = 1:n_ima, 12 | omckk = param(11+(kk-1)*6:11+(kk-1)*6+2); 13 | Tckk = param(11+(kk-1)*6+3:11+(kk-1)*6+3+2); 14 | 15 | eval(['Xkk = X_' num2str(kk) ';']); 16 | eval(['xkk = x_' num2str(kk) ';']); 17 | 18 | ekk = xkk - project_points2(Xkk,omckk,Tckk,fc,cc,kc,alpha_c); 19 | 20 | Rckk = rodrigues(omckk); 21 | eval(['omc_' num2str(kk) '= omckk;']); 22 | eval(['Tc_' num2str(kk) '= Tckk;']); 23 | eval(['Rc_' num2str(kk) '= Rckk;']); 24 | 25 | e_cam = [e_cam ekk]; 26 | 27 | end; 28 | 29 | X_proj = []; 30 | x_proj = []; 31 | 32 | for kk = 1:n_ima, 33 | eval(['xproj = xproj_' num2str(kk) ';']); 34 | xprojn = normalize_pixel(xproj,fc,cc,kc,alpha_c); 35 | eval(['Rc = Rc_' num2str(kk) ';']); 36 | eval(['Tc = Tc_' num2str(kk) ';']); 37 | Np_proj = size(xproj,2); 38 | Zc = ((Rc(:,3)'*Tc) * (1./(Rc(:,3)' * [xprojn; ones(1,Np_proj)]))); 39 | Xcp = (ones(3,1)*Zc) .* [xprojn; ones(1,Np_proj)]; % % in the camera frame 40 | eval(['X_proj_' num2str(kk) ' = Xcp;']); % coordinates of the points in the 41 | eval(['X_proj = [X_proj X_proj_' num2str(kk) '];']); 42 | eval(['x_proj = [x_proj x_proj_' num2str(kk) '];']); 43 | end; 44 | 45 | 46 | fp = param((1:2)+n_ima * 6 + 10); 47 | cp = param((3:4)+n_ima * 6 + 10); 48 | alpha_p = param((5)+n_ima * 6 + 10); 49 | kp = param((6:10)+n_ima * 6 + 10); 50 | 51 | om = param(10+n_ima*6+10+1:10+n_ima*6+10+1+2); 52 | T = param(10+n_ima*6+10+1+2+1:10+n_ima*6+10+1+2+1+2); 53 | R = rodrigues(om); 54 | 55 | e_proj = x_proj - project_points2(X_proj,om,T,fp,cp,kp,alpha_p); 56 | 57 | e_global = [e_cam e_proj]; 58 | 59 | -------------------------------------------------------------------------------- /TOOLBOX_calib/check_active_images.m: -------------------------------------------------------------------------------- 1 | if n_ima ~= 0, 2 | 3 | if ~exist('active_images'), 4 | active_images = ones(1,n_ima); 5 | end; 6 | n_act = length(active_images); 7 | if n_act < n_ima, 8 | active_images = [active_images ones(1,n_ima-n_act)]; 9 | else 10 | if n_act > n_ima, 11 | active_images = active_images(1:n_ima); 12 | end; 13 | end; 14 | 15 | ind_active = find(active_images); 16 | 17 | if prod(double(active_images == 0)), 18 | disp('Error: There is no active image. Run Add/Suppress images to add images'); 19 | return; 20 | end; 21 | 22 | if exist('center_optim'), 23 | center_optim = double(center_optim); 24 | end; 25 | if exist('est_alpha'), 26 | est_alpha = double(est_alpha); 27 | end; 28 | if exist('est_dist'), 29 | est_dist = double(est_dist); 30 | end; 31 | if exist('est_fc'), 32 | est_fc = double(est_fc); 33 | end; 34 | if exist('est_aspect_ratio'), 35 | est_aspect_ratio = double(est_aspect_ratio); 36 | end; 37 | 38 | end; 39 | -------------------------------------------------------------------------------- /TOOLBOX_calib/check_convergence.m: -------------------------------------------------------------------------------- 1 | %%% Replay the set of solution vectors: 2 | 3 | 4 | if ~exist('param_list'), 5 | if ~exist('solution'); 6 | fprintf(1,'Error: Need to calibrate first\n'); 7 | return; 8 | else 9 | param_list = solution; 10 | end; 11 | end; 12 | 13 | N_iter = size(param_list,2); 14 | 15 | if N_iter == 1, 16 | fprintf(1,'Warning: There is a unique state in the list of parameters.\n'); 17 | end; 18 | 19 | 20 | 21 | %M = moviein(N_iter); 22 | 23 | for nn = 1:N_iter, 24 | 25 | solution = param_list(:,nn); 26 | 27 | extract_parameters; 28 | comp_error_calib; 29 | 30 | ext_calib; 31 | 32 | drawnow; 33 | 34 | % Mnn = getframe(gcf); 35 | 36 | % M(:,nn) = Mnn; 37 | 38 | end; 39 | 40 | %fig = gcf; 41 | 42 | 43 | %figure(fig+1); 44 | %close; 45 | %figure(fig+1); 46 | 47 | %clf; 48 | %movie(M,20); 49 | -------------------------------------------------------------------------------- /TOOLBOX_calib/check_extracted_images.m: -------------------------------------------------------------------------------- 1 | check_active_images; 2 | 3 | for kk = ind_active, 4 | 5 | if ~exist(['x_' num2str(kk)]), 6 | 7 | fprintf(1,'WARNING: Need to extract grid corners on image %d\n',kk); 8 | 9 | active_images(kk) = 0; 10 | 11 | eval(['dX_' num2str(kk) ' = NaN;']); 12 | eval(['dY_' num2str(kk) ' = NaN;']); 13 | 14 | eval(['wintx_' num2str(kk) ' = NaN;']); 15 | eval(['winty_' num2str(kk) ' = NaN;']); 16 | 17 | eval(['x_' num2str(kk) ' = NaN*ones(2,1);']); 18 | eval(['X_' num2str(kk) ' = NaN*ones(3,1);']); 19 | 20 | eval(['n_sq_x_' num2str(kk) ' = NaN;']); 21 | eval(['n_sq_y_' num2str(kk) ' = NaN;']); 22 | 23 | else 24 | 25 | eval(['xkk = x_' num2str(kk) ';']); 26 | 27 | if isnan(xkk(1)), 28 | 29 | fprintf(1,'WARNING: Need to extract grid corners on image %d - This image is now set inactive\n',kk); 30 | 31 | active_images(kk) = 0; 32 | 33 | end; 34 | 35 | end; 36 | 37 | end; 38 | -------------------------------------------------------------------------------- /TOOLBOX_calib/clear_windows.m: -------------------------------------------------------------------------------- 1 | for kk = 1:n_ima, 2 | eval(['clear wintx_' num2str(kk)]); 3 | eval(['clear winty_' num2str(kk)]); 4 | end; 5 | -------------------------------------------------------------------------------- /TOOLBOX_calib/clearwin.m: -------------------------------------------------------------------------------- 1 | % Function that clears all the wintx_i and winty_i 2 | % In normal operation of the toolbox, this function should not be 3 | % useful. 4 | % only in cases where you want to re-extract corners using the Extract grid corners another time... not common. You might as well use the Recomp. corners. 5 | 6 | if exist('n_ima','var')~=1 7 | return; 8 | end; 9 | 10 | for kk = 1:n_ima, 11 | 12 | eval(['clear wintx_' num2str(kk) ' winty_' num2str(kk)]); 13 | 14 | end; 15 | -------------------------------------------------------------------------------- /TOOLBOX_calib/click_stereo.m: -------------------------------------------------------------------------------- 1 | function [xL,xR] = click_stereo(NUMBER_OF_POINTS,IL,IR,R,T,fc_right,cc_right,kc_right,alpha_c_right,fc_left,cc_left,kc_left,alpha_c_left); 2 | 3 | 4 | figure(1); 5 | image(IL); 6 | 7 | figure(2); 8 | image(IR); 9 | 10 | [ny,nx] = size(IL); 11 | 12 | xL = []; 13 | xR = []; 14 | 15 | for kk = 1:NUMBER_OF_POINTS, 16 | 17 | figure(1); 18 | hold on; 19 | x = ginput(1); 20 | plot(x(1),x(2),'g.'); 21 | hold off; 22 | x = x'-1; 23 | 24 | xL = [xL x]; 25 | 26 | [epipole] = compute_epipole(x,R,T,fc_right,cc_right,kc_right,alpha_c_right,fc_left,cc_left,kc_left,alpha_c_left); 27 | 28 | figure(2); 29 | hold on; 30 | h = plot(epipole(1,:)+1,epipole(2,:)+1,'r.','markersize',1); 31 | hold off; 32 | 33 | x2 = ginput(1); 34 | x2 = x2' - 1; 35 | 36 | NN = size(epipole,2); 37 | d = sum((epipole - repmat(x2,1,NN)).^2); 38 | [junk,indmin] = min(d); 39 | 40 | x2 = epipole(:,indmin); 41 | 42 | xR = [xR x2]; 43 | 44 | delete(h); 45 | 46 | figure(2); 47 | hold on; 48 | plot(x2(1)+1,x2(2)+1,'g.'); 49 | drawnow; 50 | hold off; 51 | 52 | end; 53 | 54 | -------------------------------------------------------------------------------- /TOOLBOX_calib/combine_calib.m: -------------------------------------------------------------------------------- 1 | %%% Script that combines two calibration sets together. 2 | 3 | 4 | dir; 5 | 6 | name1 = input('Calibration file name #1: ','s'); 7 | name2 = input('Calibration file name #2: ','s'); 8 | 9 | 10 | load(name1); 11 | 12 | n_ima_1 = n_ima; 13 | 14 | 15 | load(name2); 16 | 17 | n_ima_2= n_ima; 18 | active_images_2 = active_images; 19 | 20 | for kk=n_ima:-1:1, 21 | 22 | eval(['X_' num2str(kk+n_ima_1) '=X_' num2str(kk) ';' ]); 23 | eval(['x_' num2str(kk+n_ima_1) '=x_' num2str(kk) ';' ]); 24 | eval(['dX_' num2str(kk+n_ima_1) '=dX_' num2str(kk) ';' ]); 25 | eval(['dY_' num2str(kk+n_ima_1) '=dY_' num2str(kk) ';' ]); 26 | eval(['n_sq_x_' num2str(kk+n_ima_1) '=n_sq_x_' num2str(kk) ';' ]); 27 | eval(['n_sq_y_' num2str(kk+n_ima_1) '=n_sq_y_' num2str(kk) ';' ]); 28 | eval(['wintx_' num2str(kk+n_ima_1) '=wintx_' num2str(kk) ';' ]); 29 | eval(['winty_' num2str(kk+n_ima_1) '=winty_' num2str(kk) ';' ]); 30 | 31 | end; 32 | 33 | load(name1); 34 | 35 | n_ima = n_ima + n_ima_2; 36 | active_images = [ active_images active_images_2]; 37 | 38 | %no_image = 1; 39 | 40 | clear calib_name format_image type_numbering image_numbers N_slots -------------------------------------------------------------------------------- /TOOLBOX_calib/comp_distortion.m: -------------------------------------------------------------------------------- 1 | function [x_comp] = comp_distortion(x_dist,k2); 2 | 3 | % [x_comp] = comp_distortion(x_dist,k2); 4 | % 5 | % compensates the radial distortion of the camera 6 | % on the image plane. 7 | % 8 | % x_dist : the image points got without considering the 9 | % radial distortion. 10 | % x : The image plane points after correction for the distortion 11 | % 12 | % x and x_dist are 2xN arrays 13 | % 14 | % NOTE : This compensation has to be done after the substraction 15 | % of the center of projection, and division by the focal 16 | % length. 17 | % 18 | % (do it up to a second order approximation) 19 | 20 | 21 | [two,N] = size(x_dist); 22 | 23 | 24 | if (two ~= 2 ), 25 | error('ERROR : The dimension of the points should be 2xN'); 26 | end; 27 | 28 | 29 | if length(k2) > 1, 30 | 31 | [x_comp] = comp_distortion_oulu(x_dist,k2); 32 | 33 | else 34 | 35 | radius_2= x_dist(1,:).^2 + x_dist(2,:).^2; 36 | radial_distortion = 1 + ones(2,1)*(k2 * radius_2); 37 | radius_2_comp = (x_dist(1,:).^2 + x_dist(2,:).^2) ./ radial_distortion(1,:); 38 | radial_distortion = 1 + ones(2,1)*(k2 * radius_2_comp); 39 | x_comp = x_dist ./ radial_distortion; 40 | 41 | end; 42 | 43 | %% Function completely checked : It works fine !!! -------------------------------------------------------------------------------- /TOOLBOX_calib/comp_distortion2.m: -------------------------------------------------------------------------------- 1 | function [x_comp] = comp_distortion(x_dist,k2); 2 | 3 | % [x_comp] = comp_distortion(x_dist,k2); 4 | % 5 | % compensates the radial distortion of the camera 6 | % on the image plane. 7 | % 8 | % x_dist : the image points got without considering the 9 | % radial distortion. 10 | % k2: Radial distortion factor 11 | % 12 | % x : The image plane points after correction for the distortion 13 | % 14 | % x and x_dist are 2xN arrays 15 | % 16 | % NOTE : This compensation has to be done after the substraction 17 | % of the center of projection, and division by the focal 18 | % length. 19 | % 20 | % Solve for cubic roots using method from Numerical Recipes in C 2nd Ed. 21 | % pages 184-185. 22 | 23 | 24 | % California Institute of Technology 25 | % (c) Jean-Yves Bouguet - April 27th, 1998 26 | 27 | % fully checked! JYB, april 27th, 1998 - 2am 28 | 29 | if k2 ~= 0, 30 | 31 | [two,N] = size(x_dist); 32 | 33 | if (two ~= 2 ), 34 | error('ERROR : The dimension of the points should be 2xN'); 35 | end; 36 | 37 | 38 | ph = atan2(x_dist(2,:),x_dist(1,:)); 39 | 40 | Q = -1/(3*k2); 41 | R = -x_dist(1,:)./(2*k2*cos(ph)); 42 | 43 | R2 = R.^2; 44 | Q3 = Q^3; 45 | 46 | 47 | if k2 < 0, 48 | 49 | % this works in all practical situations (it starts failing for very large 50 | % values of k2) 51 | 52 | th = acos(R./sqrt(Q3)); 53 | r = -2*sqrt(Q)*cos((th-2*pi)/3); 54 | 55 | else 56 | 57 | % note: this always works, even for ridiculous values of k2 58 | 59 | A = (sqrt(R2-Q3)-R).^(1/3); 60 | B = Q*(1./A); 61 | r = (A+B); 62 | 63 | end; 64 | 65 | x_comp = [r.*cos(ph); r.*sin(ph)]; 66 | 67 | else 68 | 69 | x_comp = x_dist; 70 | 71 | end; 72 | 73 | x_comp = real(x_comp); 74 | -------------------------------------------------------------------------------- /TOOLBOX_calib/comp_distortion_oulu.m: -------------------------------------------------------------------------------- 1 | function [x] = comp_distortion_oulu(xd,k); 2 | 3 | %comp_distortion_oulu.m 4 | % 5 | %[x] = comp_distortion_oulu(xd,k) 6 | % 7 | %Compensates for radial and tangential distortion. Model From Oulu university. 8 | %For more informatino about the distortion model, check the forward projection mapping function: 9 | %project_points.m 10 | % 11 | %INPUT: xd: distorted (normalized) point coordinates in the image plane (2xN matrix) 12 | % k: Distortion coefficients (radial and tangential) (4x1 vector) 13 | % 14 | %OUTPUT: x: undistorted (normalized) point coordinates in the image plane (2xN matrix) 15 | % 16 | %Method: Iterative method for compensation. 17 | % 18 | %NOTE: This compensation has to be done after the subtraction 19 | % of the principal point, and division by the focal length. 20 | 21 | 22 | if length(k) == 1, 23 | 24 | [x] = comp_distortion(xd,k); 25 | 26 | else 27 | 28 | k1 = k(1); 29 | k2 = k(2); 30 | k3 = k(5); 31 | p1 = k(3); 32 | p2 = k(4); 33 | 34 | x = xd; % initial guess 35 | 36 | for kk=1:20, 37 | 38 | r_2 = sum(x.^2); 39 | k_radial = 1 + k1 * r_2 + k2 * r_2.^2 + k3 * r_2.^3; 40 | delta_x = [2*p1*x(1,:).*x(2,:) + p2*(r_2 + 2*x(1,:).^2); 41 | p1 * (r_2 + 2*x(2,:).^2)+2*p2*x(1,:).*x(2,:)]; 42 | x = (xd - delta_x)./(ones(2,1)*k_radial); 43 | 44 | end; 45 | 46 | end; 47 | 48 | -------------------------------------------------------------------------------- /TOOLBOX_calib/comp_error_calib.m: -------------------------------------------------------------------------------- 1 | %%%%%%%%%%%%%%%%%%%% RECOMPUTES THE REPROJECTION ERROR %%%%%%%%%%%%%%%%%%%%%%%% 2 | 3 | check_active_images; 4 | 5 | % Reproject the patterns on the images, and compute the pixel errors: 6 | 7 | ex = []; % Global error vector 8 | x = []; % Detected corners on the image plane 9 | y = []; % Reprojected points 10 | 11 | if ~exist('alpha_c'), 12 | alpha_c = 0; 13 | end; 14 | 15 | for kk = 1:n_ima, 16 | 17 | eval(['omckk = omc_' num2str(kk) ';']); 18 | eval(['Tckk = Tc_' num2str(kk) ';']); 19 | 20 | if active_images(kk) & (~isnan(omckk(1,1))), 21 | 22 | %Rkk = rodrigues(omckk); 23 | 24 | eval(['y_' num2str(kk) ' = project_points2(X_' num2str(kk) ',omckk,Tckk,fc,cc,kc,alpha_c);']); 25 | 26 | eval(['ex_' num2str(kk) ' = x_' num2str(kk) ' - y_' num2str(kk) ';']); 27 | 28 | eval(['x_kk = x_' num2str(kk) ';']); 29 | 30 | eval(['ex = [ex ex_' num2str(kk) '];']); 31 | eval(['x = [x x_' num2str(kk) '];']); 32 | eval(['y = [y y_' num2str(kk) '];']); 33 | 34 | else 35 | 36 | % eval(['y_' num2str(kk) ' = NaN*ones(2,1);']); 37 | 38 | 39 | % If inactivated image, the error does not make sense: 40 | eval(['ex_' num2str(kk) ' = NaN*ones(2,1);']); 41 | 42 | end; 43 | 44 | end; 45 | 46 | err_std = std(ex')'; 47 | -------------------------------------------------------------------------------- /TOOLBOX_calib/comp_error_calib_fisheye.m: -------------------------------------------------------------------------------- 1 | %%%%%%%%%%%%%%%%%%%% RECOMPUTES THE REPROJECTION ERROR %%%%%%%%%%%%%%%%%%%%%%%% 2 | 3 | check_active_images; 4 | 5 | % Reproject the patterns on the images, and compute the pixel errors: 6 | 7 | ex = []; % Global error vector 8 | x = []; % Detected corners on the image plane 9 | y = []; % Reprojected points 10 | 11 | if ~exist('alpha_c'), 12 | alpha_c = 0; 13 | end; 14 | 15 | for kk = 1:n_ima, 16 | 17 | eval(['omckk = omc_' num2str(kk) ';']); 18 | eval(['Tckk = Tc_' num2str(kk) ';']); 19 | 20 | if active_images(kk) & (~isnan(omckk(1,1))), 21 | 22 | %Rkk = rodrigues(omckk); 23 | 24 | eval(['y_' num2str(kk) ' = project_points_fisheye(X_' num2str(kk) ',omckk,Tckk,fc,cc,kc,alpha_c);']); 25 | 26 | eval(['ex_' num2str(kk) ' = x_' num2str(kk) ' - y_' num2str(kk) ';']); 27 | 28 | eval(['x_kk = x_' num2str(kk) ';']); 29 | 30 | eval(['ex = [ex ex_' num2str(kk) '];']); 31 | eval(['x = [x x_' num2str(kk) '];']); 32 | eval(['y = [y y_' num2str(kk) '];']); 33 | 34 | else 35 | 36 | % eval(['y_' num2str(kk) ' = NaN*ones(2,1);']); 37 | 38 | 39 | % If inactivated image, the error does not make sense: 40 | eval(['ex_' num2str(kk) ' = NaN*ones(2,1);']); 41 | 42 | end; 43 | 44 | end; 45 | 46 | err_std = std(ex')'; 47 | -------------------------------------------------------------------------------- /TOOLBOX_calib/comp_ext_calib.m: -------------------------------------------------------------------------------- 1 | %%% Computes the extrinsic parameters for all the active calibration images 2 | 3 | check_active_images; 4 | 5 | N_points_views = zeros(1,n_ima); 6 | 7 | for kk = 1:n_ima, 8 | 9 | if exist(['x_' num2str(kk)]), 10 | 11 | eval(['x_kk = x_' num2str(kk) ';']); 12 | eval(['X_kk = X_' num2str(kk) ';']); 13 | 14 | if (isnan(x_kk(1,1))), 15 | if active_images(kk), 16 | fprintf(1,'Warning: Cannot calibrate with image %d. Need to extract grid corners first.\n',kk) 17 | fprintf(1,' Set active_images(%d)=1; and run Extract grid corners.\n',kk) 18 | end; 19 | end; 20 | if active_images(kk), 21 | N_points_views(kk) = size(x_kk,2); 22 | [omckk,Tckk] = compute_extrinsic_init(x_kk,X_kk,fc,cc,kc,alpha_c); 23 | [omckk,Tckk,Rckk,JJ_kk] = compute_extrinsic_refine(omckk,Tckk,x_kk,X_kk,fc,cc,kc,alpha_c,20,thresh_cond); 24 | if check_cond, 25 | if (cond(JJ_kk)> thresh_cond), 26 | active_images(kk) = 0; 27 | omckk = NaN*ones(3,1); 28 | Tckk = NaN*ones(3,1); 29 | fprintf(1,'\nWarning: View #%d ill-conditioned. This image is now set inactive.\n',kk) 30 | desactivated_images = [desactivated_images kk]; 31 | end; 32 | end; 33 | if isnan(omckk(1,1)), 34 | %fprintf(1,'\nWarning: Desactivating image %d. Re-activate it later by typing:\nactive_images(%d)=1;\nand re-run optimization\n',[kk kk]) 35 | active_images(kk) = 0; 36 | end; 37 | else 38 | omckk = NaN*ones(3,1); 39 | Tckk = NaN*ones(3,1); 40 | end; 41 | 42 | else 43 | 44 | omckk = NaN*ones(3,1); 45 | Tckk = NaN*ones(3,1); 46 | 47 | if active_images(kk), 48 | fprintf(1,'Warning: Cannot calibrate with image %d. Need to extract grid corners first.\n',kk) 49 | fprintf(1,' Set active_images(%d)=1; and run Extract grid corners.\n',kk) 50 | end; 51 | 52 | active_images(kk) = 0; 53 | 54 | end; 55 | 56 | eval(['omc_' num2str(kk) ' = omckk;']); 57 | eval(['Tc_' num2str(kk) ' = Tckk;']); 58 | 59 | end; 60 | 61 | 62 | check_active_images; 63 | -------------------------------------------------------------------------------- /TOOLBOX_calib/comp_ext_calib_fisheye.m: -------------------------------------------------------------------------------- 1 | %%% Computes the extrinsic parameters for all the active calibration images 2 | 3 | check_active_images; 4 | 5 | N_points_views = zeros(1,n_ima); 6 | 7 | for kk = 1:n_ima, 8 | 9 | if exist(['x_' num2str(kk)]), 10 | 11 | eval(['x_kk = x_' num2str(kk) ';']); 12 | eval(['X_kk = X_' num2str(kk) ';']); 13 | 14 | if (isnan(x_kk(1,1))), 15 | if active_images(kk), 16 | fprintf(1,'Warning: Cannot calibrate with image %d. Need to extract grid corners first.\n',kk) 17 | fprintf(1,' Set active_images(%d)=1; and run Extract grid corners.\n',kk) 18 | end; 19 | end; 20 | if active_images(kk), 21 | N_points_views(kk) = size(x_kk,2); 22 | [omckk,Tckk] = compute_extrinsic_init_fisheye(x_kk,X_kk,fc,cc,kc,alpha_c); 23 | [omckk,Tckk,Rckk,JJ_kk] = compute_extrinsic_refine_fisheye(omckk,Tckk,x_kk,X_kk,fc,cc,kc,alpha_c,20,thresh_cond); 24 | if check_cond, 25 | if (cond(JJ_kk)> thresh_cond), 26 | active_images(kk) = 0; 27 | omckk = NaN*ones(3,1); 28 | Tckk = NaN*ones(3,1); 29 | fprintf(1,'\nWarning: View #%d ill-conditioned. This image is now set inactive.\n',kk) 30 | desactivated_images = [desactivated_images kk]; 31 | end; 32 | end; 33 | if isnan(omckk(1,1)), 34 | %fprintf(1,'\nWarning: Desactivating image %d. Re-activate it later by typing:\nactive_images(%d)=1;\nand re-run optimization\n',[kk kk]) 35 | active_images(kk) = 0; 36 | end; 37 | else 38 | omckk = NaN*ones(3,1); 39 | Tckk = NaN*ones(3,1); 40 | end; 41 | 42 | else 43 | 44 | omckk = NaN*ones(3,1); 45 | Tckk = NaN*ones(3,1); 46 | 47 | if active_images(kk), 48 | fprintf(1,'Warning: Cannot calibrate with image %d. Need to extract grid corners first.\n',kk) 49 | fprintf(1,' Set active_images(%d)=1; and run Extract grid corners.\n',kk) 50 | end; 51 | 52 | active_images(kk) = 0; 53 | 54 | end; 55 | 56 | eval(['omc_' num2str(kk) ' = omckk;']); 57 | eval(['Tc_' num2str(kk) ' = Tckk;']); 58 | 59 | end; 60 | 61 | 62 | check_active_images; 63 | -------------------------------------------------------------------------------- /TOOLBOX_calib/comp_fisheye_distortion.m: -------------------------------------------------------------------------------- 1 | function [x] = comp_fisheye_distortion(xd,k) 2 | 3 | %comp_fisheye_distortion.m 4 | % 5 | %[x] = comp_fisheye_distortion(xd,k) 6 | % 7 | %Compensates for fisheye distortions 8 | % 9 | %INPUT: xd: distorted (normalized) point coordinates in the image plane (2xN matrix) 10 | % k: Fisheye distortion coefficients (5x1 vector) 11 | % 12 | %OUTPUT: x: undistorted (normalized) point coordinates in the image plane (2xN matrix) 13 | % 14 | %Method: Iterative method for compensation. 15 | % 16 | %NOTE: This compensation has to be done after the subtraction 17 | % of the principal point, and division by the focal length. 18 | 19 | theta_d = sqrt(xd(1,:).^2 + xd(2,:).^2); 20 | theta = theta_d; % initial guess 21 | for kk=1:20, 22 | theta = theta_d ./ (1 + k(1)*theta.^2 + k(2)*theta.^4 + k(3)*theta.^6 + k(4)*theta.^8); 23 | end; 24 | scaling = tan(theta) ./ theta_d; 25 | 26 | x = xd .* (ones(2,1)*scaling); 27 | 28 | return; 29 | 30 | % Test 31 | 32 | n = 4; 33 | xxx = rand(2,n); 34 | 35 | xxx = [[1.0840 0.3152 0.2666 0.9347 ];[ 0.7353 0.6101 -0.6415 -0.8006]]; 36 | 37 | k = 0.00 * randn(4,1); 38 | 39 | [xd] = comp_fisheye_distortion(xxx,k); 40 | x2 = apply_fisheye_distortion(xd,k); 41 | norm(x2-xd)/norm(x2-xxx) 42 | 43 | 44 | %[xd] = apply_fisheye_distortion(xxx,k); 45 | %x2 = comp_fisheye_distortion(xd,k); 46 | %norm(x2-xd)/norm(x2-xxx) 47 | 48 | 49 | 50 | -------------------------------------------------------------------------------- /TOOLBOX_calib/compose_motion.m: -------------------------------------------------------------------------------- 1 | function [om3,T3,dom3dom1,dom3dT1,dom3dom2,dom3dT2,dT3dom1,dT3dT1,dT3dom2,dT3dT2] = compose_motion(om1,T1,om2,T2); 2 | 3 | % Rotations: 4 | 5 | [R1,dR1dom1] = rodrigues(om1); 6 | [R2,dR2dom2] = rodrigues(om2); 7 | 8 | R3 = R2 * R1; 9 | 10 | [dR3dR2,dR3dR1] = dAB(R2,R1); 11 | 12 | [om3,dom3dR3] = rodrigues(R3); 13 | 14 | dom3dom1 = dom3dR3 * dR3dR1 * dR1dom1; 15 | dom3dom2 = dom3dR3 * dR3dR2 * dR2dom2; 16 | 17 | dom3dT1 = zeros(3,3); 18 | dom3dT2 = zeros(3,3); 19 | 20 | 21 | % Translations: 22 | 23 | T3t = R2 * T1; 24 | 25 | [dT3tdR2,dT3tdT1] = dAB(R2,T1); 26 | 27 | dT3tdom2 = dT3tdR2 * dR2dom2; 28 | 29 | 30 | T3 = T3t + T2; 31 | 32 | dT3dT1 = dT3tdT1; 33 | dT3dT2 = eye(3); 34 | 35 | dT3dom2 = dT3tdom2; 36 | dT3dom1 = zeros(3,3); 37 | 38 | 39 | return; 40 | 41 | % Test of the Jacobians: 42 | 43 | om1 = randn(3,1); 44 | om2 = randn(3,1); 45 | T1 = 10*randn(3,1); 46 | T2 = 10*randn(3,1); 47 | 48 | [om3,T3,dom3dom1,dom3dT1,dom3dom2,dom3dT2,dT3dom1,dT3dT1,dT3dom2,dT3dT2] = compose_motion(om1,T1,om2,T2); 49 | 50 | 51 | dom1 = randn(3,1) / 1000; 52 | dom2 = randn(3,1) / 1000; 53 | dT1 = randn(3,1) / 10000; 54 | dT2 = randn(3,1) / 10000; 55 | 56 | [om3r,T3r] = compose_motion(om1+dom1,T1+dT1,om2+dom2,T2+dT2); 57 | 58 | om3p = om3 + dom3dom1*dom1 + dom3dT1*dT1 + dom3dom2*dom2 + dom3dT2*dT2; 59 | T3p = T3 + dT3dom1*dom1 + dT3dT1*dT1 + dT3dom2*dom2 + dT3dT2*dT2; 60 | 61 | norm(om3r - om3) / norm(om3r - om3p) 62 | norm(T3r - T3) / norm(T3r - T3p) 63 | -------------------------------------------------------------------------------- /TOOLBOX_calib/compute_collineation.m: -------------------------------------------------------------------------------- 1 | function [H,Hnorm,inv_Hnorm] = compute_collineation (a00, a10, a11, a01); 2 | 3 | % new formalism using homographies 4 | 5 | a00 = a00 / a00(3); 6 | a10 = a10 / a10(3); 7 | a11 = a11 / a11(3); 8 | a01 = a01 / a01(3); 9 | 10 | 11 | % Prenormalization of point coordinates (very important): 12 | % (Affine normalization) 13 | 14 | ax = [a00(1);a10(1);a11(1);a01(1)]; 15 | ay = [a00(2);a10(2);a11(2);a01(2)]; 16 | 17 | mxx = mean(ax); 18 | myy = mean(ay); 19 | ax = ax - mxx; 20 | ay = ay - myy; 21 | 22 | scxx = mean(abs(ax)); 23 | scyy = mean(abs(ay)); 24 | 25 | 26 | Hnorm = [1/scxx 0 -mxx/scxx;0 1/scyy -myy/scyy;0 0 1]; 27 | inv_Hnorm = [scxx 0 mxx ; 0 scyy myy; 0 0 1]; 28 | 29 | 30 | a00n = Hnorm*a00; 31 | a10n = Hnorm*a10; 32 | a11n = Hnorm*a11; 33 | a01n = Hnorm*a01; 34 | 35 | 36 | % Computation of the vanishing points: 37 | 38 | V1n = cross(cross(a00n,a10n),cross(a01n,a11n)); 39 | V2n = cross(cross(a00n,a01n),cross(a10n,a11n)); 40 | 41 | V1 = inv_Hnorm*V1n; 42 | V2 = inv_Hnorm*V2n; 43 | 44 | 45 | % Normalizaion of the vanishing points: 46 | 47 | V1n = V1n/norm(V1n); 48 | V2n = V2n/norm(V2n); 49 | 50 | 51 | % Closed-form solution of the coefficients: 52 | 53 | alpha_x = (a10n(2)*a00n(1) - a10n(1)*a00n(2))/(V1n(2)*a10n(1)-V1n(1)*a10n(2)); 54 | 55 | alpha_y = (a01n(2)*a00n(1) - a01n(1)*a00n(2))/(V2n(2)*a01n(1)-V2n(1)*a01n(2)); 56 | 57 | 58 | % Remaining Homography 59 | 60 | Hrem = [alpha_x*V1n alpha_y*V2n a00n]; 61 | 62 | 63 | % Final homography: 64 | 65 | H = inv_Hnorm*Hrem; 66 | 67 | -------------------------------------------------------------------------------- /TOOLBOX_calib/compute_epipole.m: -------------------------------------------------------------------------------- 1 | function [epipole] = compute_epipole(xLp,R,T,fc_right,cc_right,kc_right,alpha_c_right,fc_left,cc_left,kc_left,alpha_c_left,D); 2 | 3 | if ~exist('D'), 4 | D = 400; 5 | end; 6 | 7 | uo = [ normalize_pixel(xLp,fc_left,cc_left,kc_left,alpha_c_left); 1 ]; 8 | 9 | S = [ 0 -T(3) T(2) 10 | T(3) 0 -T(1) 11 | -T(2) T(1) 0 ]; 12 | 13 | l_epipole = (S*R)*uo; 14 | 15 | KK_right = [fc_right(1) alpha_c_right * fc_right(1) cc_right(1) ; 0 fc_right(2) cc_right(2) ; 0 0 1]; 16 | 17 | N_line = 800; 18 | 19 | if norm(l_epipole(2)) > norm(l_epipole(1)), 20 | 21 | % Horizontal line: 22 | 23 | limit_x_pos = ((xLp(1) + D/2) - cc_right(1)) / fc_right(1); 24 | limit_x_neg = ((xLp(1) - D/2) - cc_right(1)) / fc_right(1); 25 | 26 | 27 | %limit_x_pos = ((nx-1) - cc_right(1)) / fc_right(1); 28 | %limit_x_neg = (0 - cc_right(1)) / fc_right(1); 29 | 30 | x_list = (limit_x_pos - limit_x_neg) * ((0:(N_line-1)) / (N_line-1)) + limit_x_neg; 31 | 32 | 33 | pt = cross(repmat(l_epipole,1,N_line),[ones(1,N_line);zeros(1,N_line);-x_list]); 34 | 35 | 36 | else 37 | 38 | limit_y_pos = ((xLp(2) + D/2) - cc_right(2)) / fc_right(2); 39 | limit_y_neg = ((xLp(2) - D/2) - cc_right(2)) / fc_right(2); 40 | 41 | %limit_y_pos = ((ny-1) - cc_right(2)) / fc_right(2); 42 | %limit_y_neg = (0 - cc_right(2)) / fc_right(2); 43 | 44 | y_list = (limit_y_pos - limit_y_neg) * ((0:(N_line-1)) / (N_line-1)) + limit_y_neg; 45 | 46 | 47 | pt = cross(repmat(l_epipole,1,N_line),[zeros(1,N_line);ones(1,N_line);-y_list]); 48 | 49 | 50 | end; 51 | 52 | 53 | pt = [pt(1,:) ./ pt(3,:) ; pt(2,:)./pt(3,:)]; 54 | ptd = apply_distortion(pt,kc_right); 55 | epipole = KK_right * [ ptd ; ones(1,N_line)]; 56 | 57 | epipole = epipole(1:2,:); -------------------------------------------------------------------------------- /TOOLBOX_calib/count_squares.m: -------------------------------------------------------------------------------- 1 | function ns = count_squares(I,x1,y1,x2,y2,win); 2 | 3 | [ny,nx] = size(I); 4 | 5 | if ((x1-win <= 0) || (x1+win >= nx) || (y1-win <= 0) || (y1+win >= ny) || ... 6 | (x2-win <= 0) || (x2+win >= nx) || (y2-win <= 0) || (y2+win >= ny)) 7 | ns = -1; 8 | return; 9 | end; 10 | 11 | if ((x1 - x2)^2+(y1-y2)^2) < win, 12 | ns = -1; 13 | return; 14 | end; 15 | 16 | lambda = [y1 - y2;x2 - x1;x1*y2 - x2*y1]; 17 | lambda = 1/sqrt(lambda(1)^2 + lambda(2)^2) * lambda; 18 | l1 = lambda + [0;0;win]; 19 | l2 = lambda - [0;0;win]; 20 | dx = x2-x1; 21 | dy = y2 - y1; 22 | 23 | if abs(dx) > abs(dy), 24 | if x2 > x1, 25 | xs = x1:x2; 26 | else 27 | xs = x1:-1:x2; 28 | end; 29 | ys = -(lambda(3) + lambda(1)*xs)/lambda(2); 30 | else 31 | if y2 > y1, 32 | ys = y1:y2; 33 | else 34 | ys = y1:-1:y2; 35 | end; 36 | xs = -(lambda(3) + lambda(2)*ys)/lambda(1); 37 | end; 38 | 39 | Np = length(xs); 40 | xs_mat = ones(2*win + 1,1)*xs; 41 | ys_mat = ones(2*win + 1,1)*ys; 42 | win_mat = (-win:win)'*ones(1,Np); 43 | xs_mat2 = round(xs_mat - win_mat * lambda(1)); 44 | ys_mat2 = round(ys_mat - win_mat * lambda(2)); 45 | ind_mat = (xs_mat2 - 1) * ny + ys_mat2; 46 | ima_patch = zeros(2*win + 1,Np); 47 | ima_patch(:) = I(ind_mat(:)); 48 | 49 | %ima2 = ima_patch(:,win+1:end-win); 50 | 51 | filtk = [ones(win,Np);zeros(1,Np);-ones(win,Np)]; 52 | out_f = sum(filtk.*ima_patch); 53 | out_f_f = conv2(out_f,[1/4 1/2 1/4],'same'); 54 | out_f_f = out_f_f(win+1:end-win); 55 | ns = length(find(((out_f_f(2:end)>=0)&(out_f_f(1:end-1)<0)) | ((out_f_f(2:end)<=0)&(out_f_f(1:end-1)>0))))+1; 56 | 57 | return; 58 | -------------------------------------------------------------------------------- /TOOLBOX_calib/count_squares_distorted.m: -------------------------------------------------------------------------------- 1 | function ns = count_squares_distorted(I,x1,y1,x2,y2,win, fg, cg, kg); 2 | 3 | [ny,nx] = size(I); 4 | 5 | if ((x1-win <= 0) || (x1+win >= nx) || (y1-win <= 0) || (y1+win >= ny) || ... 6 | (x2-win <= 0) || (x2+win >= nx) || (y2-win <= 0) || (y2+win >= ny)) 7 | ns = -1; 8 | return; 9 | end; 10 | 11 | 12 | if ((x1 - x2)^2+(y1-y2)^2) < win, 13 | ns = -1; 14 | return; 15 | end; 16 | 17 | nX = round(sqrt((x1-x2)^2 + (y1-y2)^2)); 18 | alpha_x = (0:nX)/nX; 19 | pt1n = normalize_pixel([x1;y1]-1,fg,cg,kg,0); 20 | pt2n = normalize_pixel([x2;y2]-1,fg,cg,kg,0); 21 | 22 | ptsn = repmat(pt1n,[1 nX+1]) + (pt2n - pt1n)*alpha_x; 23 | 24 | pts = apply_distortion2(ptsn,kg); 25 | pts(1,:) = fg(1)*pts(1,:) + cg(1); 26 | pts(2,:) = fg(2)*pts(2,:) + cg(2); 27 | 28 | % Check that the curve is within the image: 29 | good_line = (min(pts(1,:))-win > 0) && (max(pts(1,:))+win < (nx-1)) && ... 30 | (min(pts(2,:))-win > 0) && (max(pts(2,:))+win <(ny-1)); 31 | 32 | if ~good_line, 33 | ns = -1; 34 | return; 35 | end; 36 | 37 | % Deviate the trajectory orthogonally: 38 | lambda = [y1 - y2 ; x2 - x1]; 39 | lambda = lambda / sqrt(sum(lambda.^2)); 40 | 41 | Np = size(pts,2); 42 | xs_mat = ones(2*win + 1,1)*pts(1,:); 43 | ys_mat = ones(2*win + 1,1)*pts(2,:); 44 | win_mat = (-win:win)'*ones(1,Np); 45 | xs_mat2 = round(xs_mat - win_mat * lambda(1)); 46 | ys_mat2 = round(ys_mat - win_mat * lambda(2)); 47 | 48 | 49 | ind_mat = (xs_mat2) * ny + ys_mat2 + 1; 50 | ima_patch = zeros(2*win + 1,Np); 51 | ima_patch(:) = I(ind_mat(:)); 52 | 53 | filtk = [ones(win,Np);zeros(1,Np);-ones(win,Np)]; 54 | out_f = sum(filtk.*ima_patch); 55 | out_f_f = conv2(out_f,[1/4 1/2 1/4],'same'); 56 | out_f_f = out_f_f(win+1:end-win); 57 | ns = length(find(((out_f_f(2:end)>=0)&(out_f_f(1:end-1)<0)) | ((out_f_f(2:end)<=0)&(out_f_f(1:end-1)>0))))+1; 58 | 59 | 60 | -------------------------------------------------------------------------------- /TOOLBOX_calib/count_squares_fisheye_distorted.m: -------------------------------------------------------------------------------- 1 | function ns = count_squares_fisheye_distorted(I,x1,y1,x2,y2,win, fg, cg, kg); 2 | 3 | [ny,nx] = size(I); 4 | 5 | if ((x1-win <= 0) || (x1+win >= nx) || (y1-win <= 0) || (y1+win >= ny) || ... 6 | (x2-win <= 0) || (x2+win >= nx) || (y2-win <= 0) || (y2+win >= ny)) 7 | ns = -1; 8 | return; 9 | end; 10 | 11 | 12 | if ((x1 - x2)^2+(y1-y2)^2) < win, 13 | ns = -1; 14 | return; 15 | end; 16 | 17 | nX = round(sqrt((x1-x2)^2 + (y1-y2)^2)); 18 | alpha_x = (0:nX)/nX; 19 | 20 | pt1n = normalize_pixel_fisheye([x1;y1]-1,fg,cg,kg,0); 21 | pt2n = normalize_pixel_fisheye([x2;y2]-1,fg,cg,kg,0); 22 | 23 | ptsn = repmat(pt1n,[1 nX+1]) + (pt2n - pt1n)*alpha_x; 24 | 25 | pts = apply_fisheye_distortion(ptsn,kg); 26 | pts(1,:) = fg(1)*pts(1,:) + cg(1); 27 | pts(2,:) = fg(2)*pts(2,:) + cg(2); 28 | 29 | % Check that the curve is within the image: 30 | good_line = (min(pts(1,:))-win > 0) && (max(pts(1,:))+win < (nx-1)) && ... 31 | (min(pts(2,:))-win > 0) && (max(pts(2,:))+win <(ny-1)); 32 | 33 | if ~good_line, 34 | ns = -1; 35 | return; 36 | end; 37 | 38 | % Deviate the trajectory orthogonally: 39 | lambda = [y1 - y2 ; x2 - x1]; 40 | lambda = lambda / sqrt(sum(lambda.^2)); 41 | 42 | Np = size(pts,2); 43 | xs_mat = ones(2*win + 1,1)*pts(1,:); 44 | ys_mat = ones(2*win + 1,1)*pts(2,:); 45 | win_mat = (-win:win)'*ones(1,Np); 46 | xs_mat2 = round(xs_mat - win_mat * lambda(1)); 47 | ys_mat2 = round(ys_mat - win_mat * lambda(2)); 48 | ind_mat = (xs_mat2) * ny + ys_mat2 + 1; 49 | ima_patch = zeros(2*win + 1,Np); 50 | ima_patch(:) = I(ind_mat(:)); 51 | 52 | filtk = [ones(win,Np);zeros(1,Np);-ones(win,Np)]; 53 | out_f = sum(filtk.*ima_patch); 54 | out_f_f = conv2(out_f,[1/4 1/2 1/4],'same'); 55 | out_f_f = out_f_f(win+1:end-win); 56 | ns = length(find(((out_f_f(2:end)>=0)&(out_f_f(1:end-1)<0)) | ((out_f_f(2:end)<=0)&(out_f_f(1:end-1)>0))))+1; 57 | -------------------------------------------------------------------------------- /TOOLBOX_calib/dAB.m: -------------------------------------------------------------------------------- 1 | function [dABdA,dABdB] = dAB(A,B); 2 | 3 | % [dABdA,dABdB] = dAB(A,B); 4 | % 5 | % returns : dABdA and dABdB 6 | 7 | [p,n] = size(A); [n2,q] = size(B); 8 | 9 | if n2 ~= n, 10 | error(' A and B must have equal inner dimensions'); 11 | end; 12 | 13 | if issparse(A) | issparse(B) | p*q*p*n>625 , 14 | dABdA=spalloc(p*q,p*n,p*q*n); 15 | else 16 | dABdA=zeros(p*q,p*n); 17 | end; 18 | 19 | 20 | for i=1:q, 21 | for j=1:p, 22 | ij = j + (i-1)*p; 23 | for k=1:n, 24 | kj = j + (k-1)*p; 25 | dABdA(ij,kj) = B(k,i); 26 | end; 27 | end; 28 | end; 29 | 30 | 31 | if issparse(A) | issparse(B) | p*q*n*q>625 , 32 | dABdB=spalloc(p*q,n*q,p*q*n); 33 | else 34 | dABdB=zeros(p*q,q*n); 35 | end; 36 | 37 | for i=1:q 38 | dABdB([i*p-p+1:i*p]',[i*n-n+1:i*n]) = A; 39 | end; 40 | -------------------------------------------------------------------------------- /TOOLBOX_calib/data_calib.m: -------------------------------------------------------------------------------- 1 | %%% This script alets the user enter the name of the images (base name, numbering scheme,... 2 | 3 | 4 | % Checks that there are some images in the directory: 5 | 6 | l_ras = dir('*ras'); 7 | s_ras = size(l_ras,1); 8 | l_bmp = dir('*bmp'); 9 | s_bmp = size(l_bmp,1); 10 | l_tif = dir('*tif'); 11 | s_tif = size(l_tif,1); 12 | l_pgm = dir('*pgm'); 13 | s_pgm = size(l_pgm,1); 14 | l_ppm = dir('*ppm'); 15 | s_ppm = size(l_ppm,1); 16 | l_jpg = dir('*jpg'); 17 | s_jpg = size(l_jpg,1); 18 | l_jpeg = dir('*jpeg'); 19 | s_jpeg = size(l_jpeg,1); 20 | 21 | s_tot = s_ras + s_bmp + s_tif + s_pgm + s_jpg + s_ppm + s_jpeg; 22 | 23 | if s_tot < 1, 24 | fprintf(1,'No image in this directory in either ras, bmp, tif, pgm, ppm or jpg format. Change directory and try again.\n'); 25 | return; 26 | end; 27 | 28 | 29 | % IF yes, display the directory content: 30 | 31 | dir; 32 | 33 | Nima_valid = 0; 34 | 35 | while (Nima_valid==0), 36 | 37 | fprintf(1,'\n'); 38 | calib_name = input('Basename camera calibration images (without number nor suffix): ','s'); 39 | 40 | format_image = '0'; 41 | 42 | while format_image == '0', 43 | 44 | format_image = input('Image format: ([]=''r''=''ras'', ''b''=''bmp'', ''t''=''tif'', ''p''=''pgm'', ''j''=''jpg'', ''m''=''ppm'') ','s'); 45 | 46 | if isempty(format_image), 47 | format_image = 'ras'; 48 | end; 49 | 50 | if lower(format_image(1)) == 'm', 51 | format_image = 'ppm'; 52 | else 53 | if lower(format_image(1)) == 'b', 54 | format_image = 'bmp'; 55 | else 56 | if lower(format_image(1)) == 't', 57 | format_image = 'tif'; 58 | else 59 | if lower(format_image(1)) == 'p', 60 | format_image = 'pgm'; 61 | else 62 | if lower(format_image(1)) == 'j', 63 | format_image = 'jpg'; 64 | else 65 | if lower(format_image(1)) == 'r', 66 | format_image = 'ras'; 67 | else 68 | if lower(format_image(1)) == 'g', 69 | format_image = 'jpeg'; 70 | else 71 | disp('Invalid image format'); 72 | format_image = '0'; % Ask for format once again 73 | end; 74 | end; 75 | end; 76 | end; 77 | end; 78 | end; 79 | end; 80 | end; 81 | 82 | 83 | check_directory; 84 | 85 | end; 86 | 87 | 88 | 89 | %string_save = 'save calib_data n_ima type_numbering N_slots image_numbers format_image calib_name first_num'; 90 | 91 | %eval(string_save); 92 | 93 | 94 | 95 | if (Nima_valid~=0), 96 | % Reading images: 97 | 98 | ima_read_calib; % may be launched from the toolbox itself 99 | % Show all the calibration images: 100 | 101 | if ~isempty(ind_read), 102 | 103 | mosaic; 104 | 105 | end; 106 | 107 | end; 108 | 109 | -------------------------------------------------------------------------------- /TOOLBOX_calib/data_calib_no_read.m: -------------------------------------------------------------------------------- 1 | %%% This script alets the user enter the name of the images (base name, numbering scheme,... 2 | 3 | 4 | % Checks that there are some images in the directory: 5 | 6 | l_ras = dir('*ras'); 7 | s_ras = size(l_ras,1); 8 | l_bmp = dir('*bmp'); 9 | s_bmp = size(l_bmp,1); 10 | l_tif = dir('*tif'); 11 | s_tif = size(l_tif,1); 12 | l_pgm = dir('*pgm'); 13 | s_pgm = size(l_pgm,1); 14 | l_ppm = dir('*ppm'); 15 | s_ppm = size(l_ppm,1); 16 | l_jpg = dir('*jpg'); 17 | s_jpg = size(l_jpg,1); 18 | l_jpeg = dir('*jpeg'); 19 | s_jpeg = size(l_jpeg,1); 20 | 21 | s_tot = s_ras + s_bmp + s_tif + s_pgm + s_jpg + s_ppm + s_jpeg; 22 | 23 | if s_tot < 1, 24 | fprintf(1,'No image in this directory in either ras, bmp, tif, pgm, ppm or jpg format. Change directory and try again.\n'); 25 | break; 26 | end; 27 | 28 | 29 | % IF yes, display the directory content: 30 | 31 | dir; 32 | 33 | Nima_valid = 0; 34 | 35 | while (Nima_valid==0), 36 | 37 | fprintf(1,'\n'); 38 | calib_name = input('Basename camera calibration images (without number nor suffix): ','s'); 39 | 40 | format_image = '0'; 41 | 42 | while format_image == '0', 43 | 44 | format_image = input('Image format: ([]=''r''=''ras'', ''b''=''bmp'', ''t''=''tif'', ''p''=''pgm'', ''j''=''jpeg'',''g''=''jpeg'', ''m''=''ppm'') ','s'); 45 | 46 | if isempty(format_image), 47 | format_image = 'ras'; 48 | end; 49 | 50 | if lower(format_image(1)) == 'm', 51 | format_image = 'ppm'; 52 | else 53 | if lower(format_image(1)) == 'b', 54 | format_image = 'bmp'; 55 | else 56 | if lower(format_image(1)) == 't', 57 | format_image = 'tif'; 58 | else 59 | if lower(format_image(1)) == 'p', 60 | format_image = 'pgm'; 61 | else 62 | if lower(format_image(1)) == 'j', 63 | format_image = 'jpg'; 64 | else 65 | if lower(format_image(1)) == 'r', 66 | format_image = 'ras'; 67 | else 68 | if lower(format_image(1)) == 'g', 69 | format_image = 'jpeg'; 70 | else 71 | disp('Invalid image format'); 72 | format_image = '0'; % Ask for format once again 73 | end; 74 | end; 75 | end; 76 | end; 77 | end; 78 | end; 79 | end; 80 | end; 81 | 82 | 83 | check_directory; 84 | 85 | end; 86 | 87 | 88 | 89 | if (Nima_valid~=0), 90 | % Reading images: 91 | 92 | ima_read_calib_no_read; % may be launched from the toolbox itself 93 | 94 | 95 | fprintf(1,'\n'); 96 | 97 | fprintf(1,'To display the thumbnail images of all the calibration images, you may run mosaic_no_read (may be slow)\n'); 98 | 99 | end; 100 | -------------------------------------------------------------------------------- /TOOLBOX_calib/downsample.m: -------------------------------------------------------------------------------- 1 | function V = downsample(U) 2 | 3 | % Try the 5x5 filter for building the pyramid: 4 | % Now works with color images! 5 | 6 | U = double(U); 7 | 8 | [r,c,k] = size(U); 9 | 10 | p = floor((r+1)/2); % row 11 | q = floor((c+1)/2); % col 12 | 13 | 14 | U2 = zeros(r+2,c+2,k); 15 | 16 | for i=1:k 17 | 18 | U2(:,:,i) = [U(:,:,i) U(:,end,i)*ones(1,2); ones(2,1)*U(end,:,i) U(end,end,i)*ones(2,2)]; 19 | 20 | end; 21 | 22 | cp = 2*(0:(p-1))+1; % row 23 | cq = 2*(0:(q-1))+1; % col 24 | 25 | 26 | r2 = length(cp); 27 | c2 = length(cq); 28 | 29 | e = cq+1; 30 | ee = cq+2; 31 | w = cq-1; w(1) = 1; 32 | ww = cq-2; ww(1) = 1; ww(2) = 1; 33 | n = cp-1; n(1) = 1; 34 | nn = cp-2; nn(1) = 1; nn(2) = 1; 35 | s = cp+1; 36 | ss = cp+2; 37 | 38 | V = zeros(r2,c2,k); 39 | 40 | for i = 1:k, 41 | 42 | V(:,:,i) = (36*U2(cp,cq,i) + 24*(U2(n,cq,i) + U2(s,cq,i) + U2(cp,e,i) + U2(cp,w,i)) + ... 43 | 16 * (U2(n,e,i) + U2(s,e,i) + U2(n,w,i) + U2(s,w,i)) + ... 44 | 6 * (U2(nn,cq,i) + U2(ss,cq,i) + U2(cp,ee,i) + U2(cp,ww,i)) + ... 45 | 4 * (U2(n,ww,i) + U2(nn,w,i) + U2(n,ee,i) + U2(nn,e,i) + U2(s,ww,i) + U2(ss,w,i) + U2(s,ee,i) + U2(ss,e,i)) + ... 46 | (U2(nn,ee,i) + U2(ss,ee,i) + U2(nn,ww,i) + U2(ss,ww,i)))/256; 47 | 48 | end; 49 | 50 | return 51 | 52 | % DOWNSAMPLE2 9-point subsampling (see Burt,Adelson IEEE Tcomm 31, 532) 53 | % V = downsample2(U) 54 | 55 | [r,c] = size(U); 56 | 57 | p = floor((r+1)/2); 58 | q = floor((c+1)/2); 59 | 60 | 61 | U2 = [U U(:,end); U(end,:) U(end,end)]; 62 | 63 | 64 | cq = 2*(0:(q-1))+1; 65 | cp = 2*(0:(p-1))+1; 66 | 67 | %cp = 2*([1:p]'-1)+1; 68 | %cq = 2*([1:q]-1)+1; 69 | 70 | e = cq+1; %e(q) = e(q)-1; 71 | w = cq-1; w(1) = w(1)+1; 72 | n = cp-1; n(1) = n(1)+1; 73 | s = cp+1; %s(p) = s(p)-1; 74 | 75 | % Interior 76 | V = 0.25 * U2(cp,cq) + ... 77 | 0.125*(U2(n,cq) + U2(s,cq) + U2(cp,e) + U2(cp,w)) + ... 78 | 0.0625*(U2(n,e) + U2(s,e) + U2(n,w) + U2(s,w)); 79 | -------------------------------------------------------------------------------- /TOOLBOX_calib/eliminate_boundary.m: -------------------------------------------------------------------------------- 1 | function Im2 = eliminate_boundary(Im); 2 | 3 | 4 | [nny,nnx] = size(Im); 5 | 6 | Im2 = zeros(nny,nnx); 7 | 8 | Im2(2:end-1,2:end-1) = (Im(2:end-1,2:end-1) & Im(1:end-2,2:end-1) & Im(1:end-2,1:end-2) & Im(1:end-2,3:end) & ... 9 | Im(3:end,2:end-1) & Im(3:end,1:end-2) & Im(3:end,3:end) & Im(2:end-1,1:end-2) & Im(2:end-1,3:end)); 10 | -------------------------------------------------------------------------------- /TOOLBOX_calib/error_cam_proj.m: -------------------------------------------------------------------------------- 1 | function e_global = error_cam_proj(param); 2 | 3 | global n_ima x_1 X_1 xproj_1 x_proj_1 x_2 X_2 xproj_2 x_proj_2 x_3 X_3 xproj_3 x_proj_3 x_4 X_4 xproj_4 x_proj_4 x_5 X_5 xproj_5 x_proj_5 x_6 X_6 xproj_6 x_proj_6 x_7 X_7 xproj_7 x_proj_7 x_8 X_8 xproj_8 x_proj_8 x_9 X_9 xproj_9 x_proj_9 x_10 X_10 xproj_10 x_proj_10 x_11 X_11 xproj_11 x_proj_11 x_12 X_12 xproj_12 x_proj_12 x_13 X_13 xproj_13 x_proj_13 x_14 X_14 xproj_14 x_proj_14 x_15 X_15 xproj_15 x_proj_15 x_16 X_16 xproj_16 x_proj_16 x_17 X_17 xproj_17 x_proj_17 x_18 X_18 xproj_18 x_proj_18 x_19 X_19 xproj_19 x_proj_19 x_20 X_20 xproj_20 x_proj_20 x_21 X_21 xproj_21 x_proj_21 x_22 X_22 xproj_22 x_proj_22 x_23 X_23 xproj_23 x_proj_23 x_24 X_24 xproj_24 x_proj_24 x_25 X_25 xproj_25 x_proj_25 x_26 X_26 xproj_26 x_proj_26 x_27 X_27 xproj_27 x_proj_27 x_28 X_28 xproj_28 x_proj_28 x_29 X_29 xproj_29 x_proj_29 x_30 X_30 xproj_30 x_proj_30 4 | 5 | % Computation of the errors: 6 | 7 | fc = param(1:2); 8 | cc = param(3:4); 9 | alpha_c = param(5); 10 | kc = param(6:10); 11 | 12 | e_cam = []; 13 | 14 | for kk = 1:n_ima, 15 | omckk = param(11+(kk-1)*6:11+(kk-1)*6+2); 16 | Tckk = param(11+(kk-1)*6+3:11+(kk-1)*6+3+2); 17 | 18 | eval(['Xkk = X_' num2str(kk) ';']); 19 | eval(['xkk = x_' num2str(kk) ';']); 20 | 21 | ekk = xkk - project_points2(Xkk,omckk,Tckk,fc,cc,kc,alpha_c); 22 | 23 | Rckk = rodrigues(omckk); 24 | eval(['omc_' num2str(kk) '= omckk;']); 25 | eval(['Tc_' num2str(kk) '= Tckk;']); 26 | eval(['Rc_' num2str(kk) '= Rckk;']); 27 | 28 | e_cam = [e_cam ekk]; 29 | 30 | end; 31 | 32 | X_proj = []; 33 | x_proj = []; 34 | 35 | for kk = 1:n_ima, 36 | eval(['xproj = xproj_' num2str(kk) ';']); 37 | xprojn = normalize_pixel(xproj,fc,cc,kc,alpha_c); 38 | eval(['Rc = Rc_' num2str(kk) ';']); 39 | eval(['Tc = Tc_' num2str(kk) ';']); 40 | Np_proj = size(xproj,2); 41 | Zc = ((Rc(:,3)'*Tc) * (1./(Rc(:,3)' * [xprojn; ones(1,Np_proj)]))); 42 | Xcp = (ones(3,1)*Zc) .* [xprojn; ones(1,Np_proj)]; % % in the camera frame 43 | eval(['X_proj_' num2str(kk) ' = Xcp;']); % coordinates of the points in the 44 | eval(['X_proj = [X_proj X_proj_' num2str(kk) '];']); 45 | eval(['x_proj = [x_proj x_proj_' num2str(kk) '];']); 46 | end; 47 | 48 | fp = param((1:2)+n_ima * 6 + 10); 49 | cp = param((3:4)+n_ima * 6 + 10); 50 | alpha_p = param((5)+n_ima * 6 + 10); 51 | kp = param((6:10)+n_ima * 6 + 10); 52 | 53 | om = param(10+n_ima*6+10+1:10+n_ima*6+10+1+2); 54 | T = param(10+n_ima*6+10+1+2+1:10+n_ima*6+10+1+2+1+2); 55 | 56 | 57 | e_proj = x_proj - project_points2(X_proj,om,T,fp,cp,kp,alpha_p); 58 | 59 | 60 | e_global = [e_cam e_proj]; 61 | 62 | -------------------------------------------------------------------------------- /TOOLBOX_calib/error_cam_proj2.m: -------------------------------------------------------------------------------- 1 | function e_global = error_cam_proj(param); 2 | 3 | global n_ima x_1 X_1 xproj_1 x_proj_1 x_2 X_2 xproj_2 x_proj_2 x_3 X_3 xproj_3 x_proj_3 x_4 X_4 xproj_4 x_proj_4 x_5 X_5 xproj_5 x_proj_5 x_6 X_6 xproj_6 x_proj_6 x_7 X_7 xproj_7 x_proj_7 x_8 X_8 xproj_8 x_proj_8 x_9 X_9 xproj_9 x_proj_9 x_10 X_10 xproj_10 x_proj_10 x_11 X_11 xproj_11 x_proj_11 x_12 X_12 xproj_12 x_proj_12 x_13 X_13 xproj_13 x_proj_13 x_14 X_14 xproj_14 x_proj_14 x_15 X_15 xproj_15 x_proj_15 x_16 X_16 xproj_16 x_proj_16 x_17 X_17 xproj_17 x_proj_17 x_18 X_18 xproj_18 x_proj_18 x_19 X_19 xproj_19 x_proj_19 x_20 X_20 xproj_20 x_proj_20 x_21 X_21 xproj_21 x_proj_21 x_22 X_22 xproj_22 x_proj_22 x_23 X_23 xproj_23 x_proj_23 x_24 X_24 xproj_24 x_proj_24 x_25 X_25 xproj_25 x_proj_25 x_26 X_26 xproj_26 x_proj_26 x_27 X_27 xproj_27 x_proj_27 x_28 X_28 xproj_28 x_proj_28 x_29 X_29 xproj_29 x_proj_29 x_30 X_30 xproj_30 x_proj_30 4 | 5 | % This is the same model, but with a simpler distortion model (no 6th order) 6 | 7 | % Computation of the errors: 8 | 9 | fc = param(1:2); 10 | cc = param(3:4); 11 | alpha_c = param(5); 12 | kc = [param(6:9);0]; 13 | 14 | e_cam = []; 15 | 16 | for kk = 1:n_ima, 17 | omckk = param(11+(kk-1)*6-1:11+(kk-1)*6+2-1); 18 | Tckk = param(11+(kk-1)*6+3-1:11+(kk-1)*6+3+2-1); 19 | 20 | eval(['Xkk = X_' num2str(kk) ';']); 21 | eval(['xkk = x_' num2str(kk) ';']); 22 | 23 | ekk = xkk - project_points2(Xkk,omckk,Tckk,fc,cc,kc,alpha_c); 24 | 25 | Rckk = rodrigues(omckk); 26 | eval(['omc_' num2str(kk) '= omckk;']); 27 | eval(['Tc_' num2str(kk) '= Tckk;']); 28 | eval(['Rc_' num2str(kk) '= Rckk;']); 29 | 30 | e_cam = [e_cam ekk]; 31 | 32 | end; 33 | 34 | X_proj = []; 35 | x_proj = []; 36 | 37 | for kk = 1:n_ima, 38 | eval(['xproj = xproj_' num2str(kk) ';']); 39 | xprojn = normalize_pixel(xproj,fc,cc,kc,alpha_c); 40 | eval(['Rc = Rc_' num2str(kk) ';']); 41 | eval(['Tc = Tc_' num2str(kk) ';']); 42 | Np_proj = size(xproj,2); 43 | Zc = ((Rc(:,3)'*Tc) * (1./(Rc(:,3)' * [xprojn; ones(1,Np_proj)]))); 44 | Xcp = (ones(3,1)*Zc) .* [xprojn; ones(1,Np_proj)]; % % in the camera frame 45 | eval(['X_proj_' num2str(kk) ' = Xcp;']); % coordinates of the points in the 46 | eval(['X_proj = [X_proj X_proj_' num2str(kk) '];']); 47 | eval(['x_proj = [x_proj x_proj_' num2str(kk) '];']); 48 | end; 49 | 50 | fp = param((1:2)+n_ima * 6 + 10-1); 51 | cp = param((3:4)+n_ima * 6 + 10-1); 52 | alpha_p = param((5)+n_ima * 6 + 10-1); 53 | kp = [param((6-1:10-2)+n_ima * 6 + 10);0]; 54 | 55 | om = param(10+n_ima*6+10+1-2:10+n_ima*6+10+1+2-2); 56 | T = param(10+n_ima*6+10+1+2+1-2:10+n_ima*6+10+1+2+1+2-2); 57 | 58 | 59 | e_proj = x_proj - project_points2(X_proj,om,T,fp,cp,kp,alpha_p); 60 | 61 | 62 | e_global = [e_cam e_proj]; 63 | 64 | -------------------------------------------------------------------------------- /TOOLBOX_calib/error_cam_proj3.m: -------------------------------------------------------------------------------- 1 | function e_global = error_cam_proj(param); 2 | 3 | global n_ima x_1 X_1 xproj_1 x_proj_1 x_2 X_2 xproj_2 x_proj_2 x_3 X_3 xproj_3 x_proj_3 x_4 X_4 xproj_4 x_proj_4 x_5 X_5 xproj_5 x_proj_5 x_6 X_6 xproj_6 x_proj_6 x_7 X_7 xproj_7 x_proj_7 x_8 X_8 xproj_8 x_proj_8 x_9 X_9 xproj_9 x_proj_9 x_10 X_10 xproj_10 x_proj_10 x_11 X_11 xproj_11 x_proj_11 x_12 X_12 xproj_12 x_proj_12 x_13 X_13 xproj_13 x_proj_13 x_14 X_14 xproj_14 x_proj_14 x_15 X_15 xproj_15 x_proj_15 x_16 X_16 xproj_16 x_proj_16 x_17 X_17 xproj_17 x_proj_17 x_18 X_18 xproj_18 x_proj_18 x_19 X_19 xproj_19 x_proj_19 x_20 X_20 xproj_20 x_proj_20 x_21 X_21 xproj_21 x_proj_21 x_22 X_22 xproj_22 x_proj_22 x_23 X_23 xproj_23 x_proj_23 x_24 X_24 xproj_24 x_proj_24 x_25 X_25 xproj_25 x_proj_25 x_26 X_26 xproj_26 x_proj_26 x_27 X_27 xproj_27 x_proj_27 x_28 X_28 xproj_28 x_proj_28 x_29 X_29 xproj_29 x_proj_29 x_30 X_30 xproj_30 x_proj_30 4 | 5 | % This is the same model, but with a simpler distortion model (no 6th order) 6 | 7 | % Computation of the errors: 8 | 9 | fc = param(1:2); 10 | cc = param(3:4); 11 | alpha_c = 0; 12 | kc = [param(5);zeros(4,1)]; 13 | 14 | e_cam = []; 15 | 16 | for kk = 1:n_ima, 17 | omckk = param(kk*6:kk*6+2); 18 | Tckk = param(kk*6+3:kk*6+5); 19 | 20 | eval(['Xkk = X_' num2str(kk) ';']); 21 | eval(['xkk = x_' num2str(kk) ';']); 22 | 23 | ekk = xkk - project_points2(Xkk,omckk,Tckk,fc,cc,kc,alpha_c); 24 | 25 | Rckk = rodrigues(omckk); 26 | eval(['omc_' num2str(kk) '= omckk;']); 27 | eval(['Tc_' num2str(kk) '= Tckk;']); 28 | eval(['Rc_' num2str(kk) '= Rckk;']); 29 | 30 | e_cam = [e_cam ekk]; 31 | 32 | end; 33 | 34 | X_proj = []; 35 | x_proj = []; 36 | 37 | for kk = 1:n_ima, 38 | eval(['xproj = xproj_' num2str(kk) ';']); 39 | xprojn = normalize_pixel(xproj,fc,cc,kc,alpha_c); 40 | eval(['Rc = Rc_' num2str(kk) ';']); 41 | eval(['Tc = Tc_' num2str(kk) ';']); 42 | Np_proj = size(xproj,2); 43 | Zc = ((Rc(:,3)'*Tc) * (1./(Rc(:,3)' * [xprojn; ones(1,Np_proj)]))); 44 | Xcp = (ones(3,1)*Zc) .* [xprojn; ones(1,Np_proj)]; % % in the camera frame 45 | eval(['X_proj_' num2str(kk) ' = Xcp;']); % coordinates of the points in the 46 | eval(['X_proj = [X_proj X_proj_' num2str(kk) '];']); 47 | eval(['x_proj = [x_proj x_proj_' num2str(kk) '];']); 48 | end; 49 | 50 | fp = param((1:2)+n_ima * 6 + 5); 51 | cp = param((3:4)+n_ima * 6 + 5); 52 | alpha_p = 0; 53 | kp = [param((5)+n_ima * 6 + 5);zeros(4,1)]; 54 | 55 | om = param((6:8)+n_ima * 6 + 5); 56 | T = param((9:11)+n_ima * 6 + 5); 57 | 58 | 59 | e_proj = x_proj - project_points2(X_proj,om,T,fp,cp,kp,alpha_p); 60 | 61 | 62 | e_global = [e_cam e_proj]; 63 | 64 | -------------------------------------------------------------------------------- /TOOLBOX_calib/error_depth.m: -------------------------------------------------------------------------------- 1 | function err_shape = error_depth(param_dist,xcn,xpn,R,T,X_shape,ind); 2 | 3 | 4 | 5 | X_new = depth_compute(xcn,xpn,[param_dist],R,T); 6 | 7 | 8 | N_pt_calib = size(xcn,2); 9 | 10 | % UnNormalized shape extraction: 11 | 12 | X_shape2 = X_new; 13 | X_shape2 = X_shape2 - (X_shape2(:,1)*ones(1,N_pt_calib)); 14 | 15 | % map the second vector at [1;0;0]: 16 | 17 | omu = -cross([1;0;0],X_shape2(:,2)); 18 | omu = acos((dot([1;0;0],X_shape2(:,2)))/norm(X_shape2(:,2)))*(omu / norm(omu)); 19 | Ru = rodrigues(omu); 20 | 21 | X_shape2 = Ru* X_shape2; 22 | 23 | omu2 = -cross([0;1;0],[0;X_shape2(2:3,ind)]); 24 | omu2 = acos((dot([0;1;0],[0;X_shape2(2:3,ind)]))/norm([0;X_shape2(2:3,ind)]))*(omu2 / norm(omu2)); 25 | Ru2 = rodrigues(omu2); 26 | 27 | X_shape2 = Ru2* X_shape2; 28 | 29 | 30 | % Error: 31 | 32 | err_shape = X_shape2(:,2:end) - X_shape(:,2:end); 33 | 34 | err_shape = err_shape(:); 35 | 36 | 37 | 38 | %err_depth = Z_new - Z_ref; 39 | -------------------------------------------------------------------------------- /TOOLBOX_calib/error_depth_list.m: -------------------------------------------------------------------------------- 1 | function err_total = error_depth_list(param_dist,xcn_list,xpn_list,R,T,X_shape_list,ind_list); 2 | 3 | 4 | N_view = length(ind_list); 5 | 6 | err_total = []; 7 | 8 | N_pts = zeros(1,N_view); 9 | 10 | for kk = 1:N_view, 11 | 12 | xcn = xcn_list{kk}; 13 | xpn = xpn_list{kk}; 14 | ind = ind_list{kk}; 15 | 16 | xpn = xpn([1 3],:); 17 | 18 | X_shape = X_shape_list{kk}; 19 | 20 | 21 | X_new = depth_compute(xcn,xpn,[param_dist],R,T); 22 | 23 | 24 | N_pt_calib = size(xcn,2); 25 | 26 | % UnNormalized shape extraction: 27 | 28 | X_shape2 = X_new; 29 | X_shape2 = X_shape2 - (X_shape2(:,1)*ones(1,N_pt_calib)); 30 | 31 | % map the second vector at [1;0;0]: 32 | 33 | omu = -cross([1;0;0],X_shape2(:,2)); 34 | omu = acos((dot([1;0;0],X_shape2(:,2)))/norm(X_shape2(:,2)))*(omu / norm(omu)); 35 | Ru = rodrigues(omu); 36 | 37 | X_shape2 = Ru* X_shape2; 38 | 39 | omu2 = -cross([0;1;0],[0;X_shape2(2:3,ind)]); 40 | omu2 = acos((dot([0;1;0],[0;X_shape2(2:3,ind)]))/norm([0;X_shape2(2:3,ind)]))*(omu2 / norm(omu2)); 41 | Ru2 = rodrigues(omu2); 42 | 43 | X_shape2 = Ru2* X_shape2; 44 | 45 | 46 | % Error: 47 | 48 | err_shape = X_shape2(:,2:end) - X_shape(:,2:end); 49 | 50 | err_shape = err_shape(:); 51 | 52 | N_pts(kk) = N_pt_calib; 53 | 54 | err_total = [ err_total ; err_shape ]; 55 | 56 | end; 57 | 58 | 59 | %err_depth = Z_new - Z_ref; 60 | -------------------------------------------------------------------------------- /TOOLBOX_calib/extract_distortion_data.m: -------------------------------------------------------------------------------- 1 | %%% Small script file that etst 2 | 3 | %load Calib_Results; 4 | 5 | % Collect all the points distorted (xd) and undistorted (xn) from the 6 | % images: 7 | 8 | xn = []; 9 | xd = []; 10 | for kk = ind_active, 11 | eval(['x_kk = x_' num2str(kk) ';']); 12 | xd_kk = normalize_pixel(x_kk,fc,cc,zeros(5,1),alpha_c); 13 | eval(['X_kk = X_' num2str(kk) ';']); 14 | eval(['omckk = omc_' num2str(kk) ';']); 15 | eval(['Tckk = Tc_' num2str(kk) ';']); 16 | xn_kk = project_points2(X_kk,omckk,Tckk); 17 | xd = [xd xd_kk]; 18 | xn = [xn xn_kk]; 19 | end; 20 | 21 | 22 | % Data points: 23 | r = sqrt(sum(xn.^2)); % The undistorted radii 24 | rp = sqrt(sum(xd.^2)); % The distorted radii 25 | 26 | %--- Try different analytical models to fit r_prime = D(r) 27 | 28 | ri = 0.005:.005:max(r); 29 | 30 | % Calibration toolbox model: 31 | rt = ri .* (1 + kc(1)*ri.^2 + kc(2)*ri.^4 + kc(5)*ri.^6); 32 | 33 | 34 | 35 | return; 36 | 37 | 38 | figure(10); 39 | clf; 40 | h1 = plot(r,rp,'r.','markersize',.1); hold on; 41 | h2 = plot(ri,rt,'r-','linewidth',.1); 42 | title('Radial distortion function (with unit focal) - r prime = D(r)'); 43 | xlabel('r'); 44 | ylabel('r prime'); 45 | zoom on; 46 | 47 | -------------------------------------------------------------------------------- /TOOLBOX_calib/extract_parameters.m: -------------------------------------------------------------------------------- 1 | 2 | %%% Extraction of the final intrinsic and extrinsic paramaters: 3 | 4 | check_active_images; 5 | 6 | if ~exist('solution_error') 7 | solution_error = zeros(6*n_ima + 15,1); 8 | end; 9 | 10 | fc = solution(1:2);%*** 11 | cc = solution(3:4);%*** 12 | alpha_c = solution(5);%*** 13 | kc = solution(6:10);%*** 14 | 15 | fc_error = solution_error(1:2); 16 | cc_error = solution_error(3:4); 17 | alpha_c_error = solution_error(5); 18 | kc_error = solution_error(6:10); 19 | 20 | % Calibration matrix: 21 | 22 | KK = [fc(1) fc(1)*alpha_c cc(1);0 fc(2) cc(2); 0 0 1]; 23 | inv_KK = inv(KK); 24 | 25 | % Extract the extrinsic paramters, and recomputer the collineations 26 | 27 | for kk = 1:n_ima, 28 | 29 | if active_images(kk), 30 | 31 | omckk = solution(15+6*(kk-1) + 1:15+6*(kk-1) + 3);%*** 32 | Tckk = solution(15+6*(kk-1) + 4:15+6*(kk-1) + 6);%*** 33 | 34 | omckk_error = solution_error(15+6*(kk-1) + 1:15+6*(kk-1) + 3); 35 | Tckk_error = solution_error(15+6*(kk-1) + 4:15+6*(kk-1) + 6); 36 | 37 | Rckk = rodrigues(omckk); 38 | 39 | Hkk = KK * [Rckk(:,1) Rckk(:,2) Tckk]; 40 | 41 | Hkk = Hkk / Hkk(3,3); 42 | 43 | else 44 | 45 | omckk = NaN*ones(3,1); 46 | Tckk = NaN*ones(3,1); 47 | Rckk = NaN*ones(3,3); 48 | Hkk = NaN*ones(3,3); 49 | omckk_error = NaN*ones(3,1); 50 | Tckk_error = NaN*ones(3,1); 51 | 52 | end; 53 | 54 | eval(['omc_' num2str(kk) ' = omckk;']); 55 | eval(['Rc_' num2str(kk) ' = Rckk;']); 56 | eval(['Tc_' num2str(kk) ' = Tckk;']); 57 | eval(['H_' num2str(kk) '= Hkk;']); 58 | eval(['omc_error_' num2str(kk) ' = omckk_error;']); 59 | eval(['Tc_error_' num2str(kk) ' = Tckk_error;']); 60 | 61 | end; 62 | -------------------------------------------------------------------------------- /TOOLBOX_calib/extract_parameters3D.m: -------------------------------------------------------------------------------- 1 | 2 | %%% Extraction of the final intrinsic and extrinsic paramaters: 3 | 4 | 5 | fc = solution(1:2); 6 | kc = solution(3:6); 7 | cc = solution(6*n_ima + 4 +3:6*n_ima + 5 +3); 8 | 9 | % Calibration matrix: 10 | 11 | KK = [fc(1) 0 cc(1);0 fc(2) cc(2); 0 0 1]; 12 | inv_KK = inv(KK); 13 | 14 | % Extract the extrinsic paramters, and recomputer the collineations 15 | 16 | for kk = 1:n_ima, 17 | 18 | omckk = solution(4+6*(kk-1) + 3:6*kk + 3); 19 | 20 | Tckk = solution(6*kk+1 + 3:6*kk+3 + 3); 21 | 22 | Rckk = rodrigues(omckk); 23 | 24 | Hlkk = KK * [Rckk(:,1) Rckk(:,2) Tckk]; 25 | 26 | Hlkk = Hlkk / Hlkk(3,3); 27 | 28 | eval(['omc_' num2str(kk) ' = omckk;']); 29 | eval(['Rc_' num2str(kk) ' = Rckk;']); 30 | eval(['Tc_' num2str(kk) ' = Tckk;']); 31 | 32 | eval(['Hl_' num2str(kk) '=Hlkk;']); 33 | 34 | end; 35 | 36 | 37 | -------------------------------------------------------------------------------- /TOOLBOX_calib/extract_parameters_fisheye.m: -------------------------------------------------------------------------------- 1 | 2 | %%% Extraction of the final intrinsic and extrinsic paramaters: 3 | 4 | check_active_images; 5 | 6 | if ~exist('solution_error') 7 | solution_error = zeros(6*n_ima + 15,1); 8 | end; 9 | 10 | fc = solution(1:2);%*** 11 | cc = solution(3:4);%*** 12 | alpha_c = solution(5);%*** 13 | kc = solution(6:9);%*** 14 | 15 | fc_error = solution_error(1:2); 16 | cc_error = solution_error(3:4); 17 | alpha_c_error = solution_error(5); 18 | kc_error = solution_error(6:9); 19 | 20 | % Calibration matrix: 21 | 22 | KK = [fc(1) fc(1)*alpha_c cc(1);0 fc(2) cc(2); 0 0 1]; 23 | inv_KK = inv(KK); 24 | 25 | % Extract the extrinsic paramters, and recomputer the collineations 26 | 27 | for kk = 1:n_ima, 28 | 29 | if active_images(kk), 30 | 31 | omckk = solution(15+6*(kk-1) + 1:15+6*(kk-1) + 3);%*** 32 | Tckk = solution(15+6*(kk-1) + 4:15+6*(kk-1) + 6);%*** 33 | 34 | omckk_error = solution_error(15+6*(kk-1) + 1:15+6*(kk-1) + 3); 35 | Tckk_error = solution_error(15+6*(kk-1) + 4:15+6*(kk-1) + 6); 36 | 37 | Rckk = rodrigues(omckk); 38 | 39 | Hkk = KK * [Rckk(:,1) Rckk(:,2) Tckk]; 40 | 41 | Hkk = Hkk / Hkk(3,3); 42 | 43 | else 44 | 45 | omckk = NaN*ones(3,1); 46 | Tckk = NaN*ones(3,1); 47 | Rckk = NaN*ones(3,3); 48 | Hkk = NaN*ones(3,3); 49 | omckk_error = NaN*ones(3,1); 50 | Tckk_error = NaN*ones(3,1); 51 | 52 | end; 53 | 54 | eval(['omc_' num2str(kk) ' = omckk;']); 55 | eval(['Rc_' num2str(kk) ' = Rckk;']); 56 | eval(['Tc_' num2str(kk) ' = Tckk;']); 57 | eval(['H_' num2str(kk) '= Hkk;']); 58 | eval(['omc_error_' num2str(kk) ' = omckk_error;']); 59 | eval(['Tc_error_' num2str(kk) ' = Tckk_error;']); 60 | 61 | end; 62 | -------------------------------------------------------------------------------- /TOOLBOX_calib/fixallvariables.m: -------------------------------------------------------------------------------- 1 | % Code that clears all empty or NaN variables 2 | 3 | varlist = whos; 4 | 5 | if ~isempty(varlist), 6 | 7 | Nvar = size(varlist,1); 8 | 9 | for c_var = 1:Nvar, 10 | 11 | var2fix = varlist(c_var).name; 12 | 13 | fixvariable; 14 | 15 | end; 16 | 17 | end; 18 | 19 | clear varlist var2fix Nvar c_var -------------------------------------------------------------------------------- /TOOLBOX_calib/fixvariable.m: -------------------------------------------------------------------------------- 1 | % Code that clears an empty variable, or a NaN vsriable. 2 | % Does not clear structures, or cells. 3 | 4 | if exist('var2fix')==1, 5 | if eval(['exist(''' var2fix ''') == 1']), 6 | if eval(['isempty(' var2fix ')']), 7 | eval(['clear ' var2fix ]); 8 | else 9 | if eval(['~isstruct(' var2fix ')']), 10 | if eval(['~iscell(' var2fix ')']), 11 | if eval(['isnan(' var2fix '(1))']), 12 | eval(['clear ' var2fix ]); 13 | end; 14 | end; 15 | end; 16 | end; 17 | end; 18 | end; 19 | -------------------------------------------------------------------------------- /TOOLBOX_calib/fov.m: -------------------------------------------------------------------------------- 1 | % small program that computes the field of view of a camera (in degrees) 2 | 3 | if ~exist('fc')|~exist('cc')|~exist('nx')|~exist('ny'), 4 | error('Need calibration results to compute FOV (fc,cc,Wcal,Hcal)'); 5 | end; 6 | 7 | FOV_HOR = 180 * ( atan((nx - (cc(1)+.5))/fc(1)) + atan((cc(1)+.5)/fc(1)) )/pi; 8 | 9 | FOV_VER = 180 * ( atan((ny - (cc(2)+.5))/fc(2)) + atan((cc(2)+.5)/fc(2)) )/pi; 10 | 11 | fprintf(1,'Horizontal field of view = %.2f degrees\n',FOV_HOR); 12 | fprintf(1,'Vertical field of view = %.2f degrees\n',FOV_VER); -------------------------------------------------------------------------------- /TOOLBOX_calib/go_calib_optim.m: -------------------------------------------------------------------------------- 1 | %go_calib_optim 2 | % 3 | %Main calibration function. Computes the intrinsic andextrinsic parameters. 4 | %Runs as a script. 5 | % 6 | %INPUT: x_1,x_2,x_3,...: Feature locations on the images 7 | % X_1,X_2,X_3,...: Corresponding grid coordinates 8 | % 9 | %OUTPUT: fc: Camera focal length 10 | % cc: Principal point coordinates 11 | % alpha_c: Skew coefficient 12 | % kc: Distortion coefficients 13 | % KK: The camera matrix (containing fc and cc) 14 | % omc_1,omc_2,omc_3,...: 3D rotation vectors attached to the grid positions in space 15 | % Tc_1,Tc_2,Tc_3,...: 3D translation vectors attached to the grid positions in space 16 | % Rc_1,Rc_2,Rc_3,...: 3D rotation matrices corresponding to the omc vectors 17 | % 18 | %Method: Minimizes the pixel reprojection error in the least squares sense over the intrinsic 19 | % camera parameters, and the extrinsic parameters (3D locations of the grids in space) 20 | % 21 | %Note: If the intrinsic camera parameters (fc, cc, kc) do not exist before, they are initialized through 22 | % the function init_intrinsic_param.m. Otherwise, the variables in memory are used as initial guesses. 23 | % 24 | %Note: The row vector active_images consists of zeros and ones. To deactivate an image, set the 25 | % corresponding entry in the active_images vector to zero. 26 | % 27 | %VERY IMPORTANT: This function works for 2D and 3D calibration rigs, except for init_intrinsic_param.m 28 | %that is so far implemented to work only with 2D rigs. 29 | %In the future, a more general function will be there. 30 | %For now, if using a 3D calibration rig, set quick_init to 1 for an easy initialization of the focal length 31 | 32 | 33 | if ~exist('n_ima'), 34 | data_calib; % Load the images 35 | click_calib; % Extract the corners 36 | end; 37 | 38 | 39 | check_active_images; 40 | check_extracted_images; 41 | check_active_images; 42 | desactivated_images = []; 43 | 44 | recompute_extrinsic = (length(ind_active) < 100); % if there are too many images, do not spend time recomputing the extrinsic parameters twice.. 45 | 46 | %%% MAIN OPTIMIZATION CALL!!!!! (look into this function for the details of implementation) 47 | go_calib_optim_iter; 48 | 49 | if ~isempty(desactivated_images), 50 | param_list_save = param_list; 51 | fprintf(1,'\nNew optimization including the images that have been deactivated during the previous optimization.\n'); 52 | active_images(desactivated_images) = ones(1,length(desactivated_images)); 53 | desactivated_images = []; 54 | go_calib_optim_iter; 55 | if ~isempty(desactivated_images), 56 | fprintf(1,['List of images left desactivated: ' num2str(desactivated_images) '\n' ] ); 57 | end; 58 | param_list = [param_list_save(:,1:end-1) param_list]; 59 | end; 60 | -------------------------------------------------------------------------------- /TOOLBOX_calib/go_calib_optim_fisheye_no_read.m: -------------------------------------------------------------------------------- 1 | %go_calib_optim 2 | % 3 | %Main calibration function. Computes the intrinsic andextrinsic parameters. 4 | %Runs as a script. 5 | % 6 | %INPUT: x_1,x_2,x_3,...: Feature locations on the images 7 | % X_1,X_2,X_3,...: Corresponding grid coordinates 8 | % 9 | %OUTPUT: fc: Camera focal length 10 | % cc: Principal point coordinates 11 | % alpha_c: Skew coefficient 12 | % kc: Distortion coefficients 13 | % KK: The camera matrix (containing fc and cc) 14 | % omc_1,omc_2,omc_3,...: 3D rotation vectors attached to the grid positions in space 15 | % Tc_1,Tc_2,Tc_3,...: 3D translation vectors attached to the grid positions in space 16 | % Rc_1,Rc_2,Rc_3,...: 3D rotation matrices corresponding to the omc vectors 17 | % 18 | %Method: Minimizes the pixel reprojection error in the least squares sense over the intrinsic 19 | % camera parameters, and the extrinsic parameters (3D locations of the grids in space) 20 | % 21 | %Note: If the intrinsic camera parameters (fc, cc, kc) do not exist before, they are initialized through 22 | % the function init_intrinsic_param.m. Otherwise, the variables in memory are used as initial guesses. 23 | % 24 | %Note: The row vector active_images consists of zeros and ones. To deactivate an image, set the 25 | % corresponding entry in the active_images vector to zero. 26 | % 27 | %VERY IMPORTANT: This function works for 2D and 3D calibration rigs, except for init_intrinsic_param.m 28 | %that is so far implemented to work only with 2D rigs. 29 | %In the future, a more general function will be there. 30 | %For now, if using a 3D calibration rig, set quick_init to 1 for an easy initialization of the focal length 31 | 32 | 33 | if ~exist('n_ima'), 34 | data_calib_no_read; % Load the images 35 | click_calib_fisheye_no_read; % Extract the corners 36 | end; 37 | 38 | 39 | check_active_images; 40 | check_extracted_images; 41 | check_active_images; 42 | desactivated_images = []; 43 | 44 | recompute_extrinsic = (length(ind_active) < 100); % if there are too many images, do not spend time recomputing the extrinsic parameters twice.. 45 | 46 | %%% MAIN OPTIMIZATION CALL!!!!! (look into this function for the details of implementation) 47 | go_calib_optim_iter_fisheye; 48 | 49 | if ~isempty(desactivated_images), 50 | param_list_save = param_list; 51 | fprintf(1,'\nNew optimization including the images that have been deactivated during the previous optimization.\n'); 52 | active_images(desactivated_images) = ones(1,length(desactivated_images)); 53 | desactivated_images = []; 54 | go_calib_optim_iter_fisheye; 55 | if ~isempty(desactivated_images), 56 | fprintf(1,['List of images left desactivated: ' num2str(desactivated_images) '\n' ] ); 57 | end; 58 | param_list = [param_list_save(:,1:end-1) param_list]; 59 | end; 60 | -------------------------------------------------------------------------------- /TOOLBOX_calib/go_calib_optim_iter_fisheye.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RobVisLab/camera_calibration/f56c17c297653ba0a156d125c2b162b19432b48b/TOOLBOX_calib/go_calib_optim_iter_fisheye.m -------------------------------------------------------------------------------- /TOOLBOX_calib/go_calib_optim_iter_weak.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RobVisLab/camera_calibration/f56c17c297653ba0a156d125c2b162b19432b48b/TOOLBOX_calib/go_calib_optim_iter_weak.m -------------------------------------------------------------------------------- /TOOLBOX_calib/go_calib_optim_no_read.m: -------------------------------------------------------------------------------- 1 | %go_calib_optim 2 | % 3 | %Main calibration function. Computes the intrinsic andextrinsic parameters. 4 | %Runs as a script. 5 | % 6 | %INPUT: x_1,x_2,x_3,...: Feature locations on the images 7 | % X_1,X_2,X_3,...: Corresponding grid coordinates 8 | % 9 | %OUTPUT: fc: Camera focal length 10 | % cc: Principal point coordinates 11 | % alpha_c: Skew coefficient 12 | % kc: Distortion coefficients 13 | % KK: The camera matrix (containing fc and cc) 14 | % omc_1,omc_2,omc_3,...: 3D rotation vectors attached to the grid positions in space 15 | % Tc_1,Tc_2,Tc_3,...: 3D translation vectors attached to the grid positions in space 16 | % Rc_1,Rc_2,Rc_3,...: 3D rotation matrices corresponding to the omc vectors 17 | % 18 | %Method: Minimizes the pixel reprojection error in the least squares sense over the intrinsic 19 | % camera parameters, and the extrinsic parameters (3D locations of the grids in space) 20 | % 21 | %Note: If the intrinsic camera parameters (fc, cc, kc) do not exist before, they are initialized through 22 | % the function init_intrinsic_param.m. Otherwise, the variables in memory are used as initial guesses. 23 | % 24 | %Note: The row vector active_images consists of zeros and ones. To deactivate an image, set the 25 | % corresponding entry in the active_images vector to zero. 26 | % 27 | %VERY IMPORTANT: This function works for 2D and 3D calibration rigs, except for init_intrinsic_param.m 28 | %that is so far implemented to work only with 2D rigs. 29 | %In the future, a more general function will be there. 30 | %For now, if using a 3D calibration rig, set quick_init to 1 for an easy initialization of the focal length 31 | 32 | 33 | if ~exist('n_ima'), 34 | data_calib_no_read; % Load the images 35 | click_calib_no_read; % Extract the corners 36 | end; 37 | 38 | 39 | check_active_images; 40 | check_extracted_images; 41 | check_active_images; 42 | desactivated_images = []; 43 | 44 | recompute_extrinsic = (length(ind_active) < 100); % if there are too many images, do not spend time recomputing the extrinsic parameters twice.. 45 | 46 | %%% MAIN OPTIMIZATION CALL!!!!! (look into this function for the details of implementation) 47 | go_calib_optim_iter; 48 | 49 | if ~isempty(desactivated_images), 50 | param_list_save = param_list; 51 | fprintf(1,'\nNew optimization including the images that have been deactivated during the previous optimization.\n'); 52 | active_images(desactivated_images) = ones(1,length(desactivated_images)); 53 | desactivated_images = []; 54 | go_calib_optim_iter; 55 | if ~isempty(desactivated_images), 56 | fprintf(1,['List of images left desactivated: ' num2str(desactivated_images) '\n' ] ); 57 | end; 58 | param_list = [param_list_save(:,1:end-1) param_list]; 59 | end; 60 | 61 | -------------------------------------------------------------------------------- /TOOLBOX_calib/go_calib_stereo.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RobVisLab/camera_calibration/f56c17c297653ba0a156d125c2b162b19432b48b/TOOLBOX_calib/go_calib_stereo.m -------------------------------------------------------------------------------- /TOOLBOX_calib/init_intrinsic_param_fisheye.m: -------------------------------------------------------------------------------- 1 | %init_intrinsic_param_fisheye 2 | % 3 | %Initialization of the intrinsic parameters. 4 | %Runs as a script. 5 | % 6 | %INPUT: x_1,x_2,x_3,...: Feature locations on the images 7 | % X_1,X_2,X_3,...: Corresponding grid coordinates 8 | % 9 | %OUTPUT: fc: Camera focal length 10 | % cc: Principal point coordinates 11 | % kc: Fisheye distortion coefficients 12 | % alpha_c: skew coefficient 13 | % KK: The camera matrix (containing fc, cc and alpha_c) 14 | % 15 | %Method: Computes the planar homographies H_1, H_2, H_3, ... and computes 16 | % the focal length fc from orthogonal vanishing points constraint. 17 | % The principal point cc is assumed at the center of the image. 18 | % Assumes no image distortion (kc = [0;0;0;0]) 19 | % 20 | %Note: The row vector active_images consists of zeros and ones. To deactivate an image, set the 21 | % corresponding entry in the active_images vector to zero. 22 | % 23 | % 24 | %Important function called within that program: 25 | % 26 | %compute_homography.m: Computes the planar homography between points on the grid in 3D, and the image plane. 27 | % 28 | % 29 | %VERY IMPORTANT: This function works only with 2D rigs. 30 | %In the future, a more general function will be there (working with 3D rigs as well). 31 | 32 | 33 | if ~exist('two_focals_init'), 34 | two_focals_init = 0; 35 | end; 36 | 37 | if ~exist('est_aspect_ratio'), 38 | est_aspect_ratio = 1; 39 | end; 40 | 41 | check_active_images; 42 | 43 | if ~exist(['x_' num2str(ind_active(1)) ]), 44 | click_calib; 45 | end; 46 | 47 | 48 | fprintf(1,'\nInitialization of the intrinsic parameters - Number of images: %d\n',length(ind_active)); 49 | 50 | check_active_images; 51 | 52 | % initial guess for principal point and distortion: 53 | 54 | if ~exist('nx'), [ny,nx] = size(I); end; 55 | 56 | f_init = (max(nx,ny) / pi) * ones(2,1); 57 | c_init = [nx;ny]/2 - 0.5; % initialize at the center of the image 58 | k_init = [0;0;0;0]; % initialize to zero (no distortion) 59 | 60 | if ~est_aspect_ratio, 61 | f_init(1) = (f_init(1)+f_init(2))/2; 62 | f_init(2) = f_init(1); 63 | end; 64 | 65 | alpha_init = 0; 66 | 67 | % Global calibration matrix (initial guess): 68 | 69 | KK = [f_init(1) alpha_init*f_init(1) c_init(1);0 f_init(2) c_init(2); 0 0 1]; 70 | inv_KK = inv(KK); 71 | 72 | cc = c_init; 73 | fc = f_init; 74 | kc = k_init; 75 | alpha_c = alpha_init; 76 | 77 | 78 | fprintf(1,'\n\nCalibration parameters after initialization:\n\n'); 79 | fprintf(1,'Focal Length: fc = [ %3.5f %3.5f ]\n',fc); 80 | fprintf(1,'Principal point: cc = [ %3.5f %3.5f ]\n',cc); 81 | fprintf(1,'Skew: alpha_c = [ %3.5f ] => angle of pixel = %3.5f degrees\n',alpha_c,90 - atan(alpha_c)*180/pi); 82 | fprintf(1,'Fisheye Distortion: kc = [ %3.5f %3.5f %3.5f %3.5f ]\n',kc); 83 | -------------------------------------------------------------------------------- /TOOLBOX_calib/inverse_motion.m: -------------------------------------------------------------------------------- 1 | function [om2,T2,dom2dom,dom2dT,dT2dom,dT2dT] = inverse_motion(om,T); 2 | 3 | % This function computes the inverse motion corresponding to (om,T) 4 | 5 | 6 | om2 = -om; 7 | dom2dom = -eye(3); 8 | dom2dT = zeros(3,3); 9 | 10 | 11 | [R,dRdom] = rodrigues(om); 12 | Rinv = R'; 13 | dRinvdR = zeros(9,9); 14 | dRinvdR([1 4 7],[1 2 3]) = eye(3); 15 | dRinvdR([2 5 8],[4 5 6]) = eye(3); 16 | dRinvdR([3 6 9],[7 8 9]) = eye(3); 17 | dRinvdom = dRinvdR * dRdom; 18 | 19 | Tt = Rinv * T; 20 | [dTtdRinv,dTtdT] = dAB(Rinv,T); 21 | 22 | T2 = -Tt; 23 | 24 | dT2dom = - dTtdRinv * dRinvdom; 25 | dT2dT = - dTtdT; 26 | 27 | 28 | return; 29 | 30 | % Test of the Jacobians: 31 | 32 | om = randn(3,1); 33 | T = 10*randn(3,1); 34 | [om2,T2,dom2dom,dom2dT,dT2dom,dT2dT] = inverse_motion(om,T); 35 | 36 | dom = randn(3,1) / 100000; 37 | dT = randn(3,1) / 100000; 38 | 39 | [om3r,T3r] = inverse_motion(om+dom,T+dT); 40 | 41 | om3p = om2 + dom2dom*dom + dom2dT*dT; 42 | T3p = T2 + dT2dom*dom + dT2dT*dT; 43 | 44 | %norm(om3r - om2) / norm(om3r - om3p) %-> Leads to infinity, since the opreation is linear! 45 | norm(T3r - T2) / norm(T3r - T3p) 46 | -------------------------------------------------------------------------------- /TOOLBOX_calib/is3D.m: -------------------------------------------------------------------------------- 1 | function test = is3D(X), 2 | 3 | 4 | Np = size(X,2); 5 | 6 | %% Check for planarity of the structure: 7 | 8 | X_mean = mean(X')'; 9 | 10 | Y = X - (X_mean*ones(1,Np)); 11 | 12 | YY = Y*Y'; 13 | 14 | [U,S,V] = svd(YY); 15 | 16 | r = S(3,3)/S(2,2); 17 | 18 | test = (r > 1e-3); 19 | 20 | -------------------------------------------------------------------------------- /TOOLBOX_calib/load_image.m: -------------------------------------------------------------------------------- 1 | function I = load_image(kk , calib_name , format_image , type_numbering , image_numbers , N_slots), 2 | 3 | 4 | if ~type_numbering, 5 | number_ext = num2str(image_numbers(kk)); 6 | else 7 | number_ext = sprintf(['%.' num2str(N_slots) 'd'],image_numbers(kk)); 8 | end; 9 | 10 | 11 | ima_name = [calib_name number_ext '.' format_image]; 12 | 13 | if ~exist(ima_name), 14 | 15 | fprintf(1,'Image %s not found!!!\n',ima_name); 16 | I = NaN; 17 | 18 | else 19 | 20 | fprintf(1,'Loading image %s...\n',ima_name); 21 | 22 | if format_image(1) == 'p', 23 | if format_image(2) == 'p', 24 | I = double(loadppm(ima_name)); 25 | else 26 | I = double(loadpgm(ima_name)); 27 | end; 28 | else 29 | if format_image(1) == 'r', 30 | I = readras(ima_name); 31 | else 32 | I = double(imread(ima_name)); 33 | end; 34 | end; 35 | 36 | 37 | if size(I,3)>1, 38 | I = 0.299 * I(:,:,1) + 0.5870 * I(:,:,2) + 0.114 * I(:,:,3); 39 | end; 40 | 41 | end; 42 | -------------------------------------------------------------------------------- /TOOLBOX_calib/load_stereo_calib_files.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RobVisLab/camera_calibration/f56c17c297653ba0a156d125c2b162b19432b48b/TOOLBOX_calib/load_stereo_calib_files.m -------------------------------------------------------------------------------- /TOOLBOX_calib/loading_calib.m: -------------------------------------------------------------------------------- 1 | if ~exist('Calib_Results.mat'), 2 | fprintf(1,'\nCalibration file Calib_Results.mat not found!\n'); 3 | return; 4 | end; 5 | 6 | fprintf(1,'\nLoading calibration results from Calib_Results.mat\n'); 7 | 8 | load Calib_Results 9 | 10 | fprintf(1,'done\n'); 11 | -------------------------------------------------------------------------------- /TOOLBOX_calib/loading_stereo_calib.m: -------------------------------------------------------------------------------- 1 | if exist('Calib_Results_stereo.mat')~=2, 2 | fprintf(1,'\nStereo calibration file Calib_Results_stereo.mat not found!\n'); 3 | return; 4 | end; 5 | 6 | fprintf(1,'Loading stereo calibration results from Calib_Results_stereo.mat...\n'); 7 | load('Calib_Results_stereo.mat'); 8 | -------------------------------------------------------------------------------- /TOOLBOX_calib/loadinr.m: -------------------------------------------------------------------------------- 1 | %LOADINR Load an INRIMAGE format file 2 | % 3 | % LOADINR(filename, im) 4 | % 5 | % Load an INRIA image format file and return it as a matrix 6 | % 7 | % SEE ALSO: saveinr 8 | % 9 | % Copyright (c) Peter Corke, 1999 Machine Vision Toolbox for Matlab 10 | 11 | 12 | % Peter Corke 1996 13 | 14 | function im = loadinr(fname, im) 15 | 16 | fid = fopen(fname, 'r'); 17 | 18 | s = fgets(fid); 19 | if strcmp(s(1:12), '#INRIMAGE-4#') == 0, 20 | error('not INRIMAGE format'); 21 | end 22 | 23 | % not very complete, only looks for the X/YDIM keys 24 | while 1, 25 | s = fgets(fid); 26 | n = length(s) - 1; 27 | if s(1) == '#', 28 | break 29 | end 30 | if strcmp(s(1:5), 'XDIM='), 31 | cols = str2num(s(6:n)); 32 | end 33 | if strcmp(s(1:5), 'YDIM='), 34 | rows = str2num(s(6:n)); 35 | end 36 | if strcmp(s(1:4), 'CPU='), 37 | if strcmp(s(5:n), 'sun') == 0, 38 | error('not sun data ordering'); 39 | end 40 | end 41 | 42 | end 43 | disp(['INRIMAGE format file ' num2str(rows) ' x ' num2str(cols)]) 44 | 45 | % now the binary data 46 | fseek(fid, 256, 'bof'); 47 | [im count] = fread(fid, [cols rows], 'float32'); 48 | im = im'; 49 | if count ~= (rows*cols), 50 | error('file too short'); 51 | end 52 | fclose(fid); 53 | -------------------------------------------------------------------------------- /TOOLBOX_calib/loadpgm.m: -------------------------------------------------------------------------------- 1 | %LOADPGM Load a PGM image 2 | % 3 | % I = loadpgm(filename) 4 | % 5 | % Returns a matrix containing the image loaded from the PGM format 6 | % file filename. Handles ASCII (P2) and binary (P5) PGM file formats. 7 | % 8 | % If the filename has no extension, and open fails, a '.pgm' will 9 | % be appended. 10 | % 11 | % 12 | % Copyright (c) Peter Corke, 1999 Machine Vision Toolbox for Matlab 13 | 14 | 15 | % Peter Corke 1994 16 | 17 | function I = loadpgm(file) 18 | white = [' ' 9 10 13]; % space, tab, lf, cr 19 | white = setstr(white); 20 | 21 | fid = fopen(file, 'r'); 22 | if fid < 0, 23 | fid = fopen([file '.pgm'], 'r'); 24 | end 25 | if fid < 0, 26 | error('Couldn''t open file'); 27 | end 28 | 29 | magic = fread(fid, 2, 'char'); 30 | while 1 31 | c = fread(fid,1,'char'); 32 | if c == '#', 33 | fgetl(fid); 34 | elseif ~any(c == white) 35 | fseek(fid, -1, 'cof'); % unputc() 36 | break; 37 | end 38 | end 39 | cols = fscanf(fid, '%d', 1); 40 | while 1 41 | c = fread(fid,1,'char'); 42 | if c == '#', 43 | fgetl(fid); 44 | elseif ~any(c == white) 45 | fseek(fid, -1, 'cof'); % unputc() 46 | break; 47 | end 48 | end 49 | rows = fscanf(fid, '%d', 1); 50 | while 1 51 | c = fread(fid,1,'char'); 52 | if c == '#', 53 | fgetl(fid); 54 | elseif ~any(c == white) 55 | fseek(fid, -1, 'cof'); % unputc() 56 | break; 57 | end 58 | end 59 | maxval = fscanf(fid, '%d', 1); 60 | while 1 61 | c = fread(fid,1,'char'); 62 | if c == '#', 63 | fgetl(fid); 64 | elseif ~any(c == white) 65 | fseek(fid, -1, 'cof'); % unputc() 66 | break; 67 | end 68 | end 69 | if magic(1) == 'P', 70 | if magic(2) == '2', 71 | %disp(['ASCII PGM file ' num2str(rows) ' x ' num2str(cols)]) 72 | I = fscanf(fid, '%d', [cols rows])'; 73 | elseif magic(2) == '5', 74 | %disp(['Binary PGM file ' num2str(rows) ' x ' num2str(cols)]) 75 | if maxval == 1, 76 | fmt = 'unint1'; 77 | elseif maxval == 15, 78 | fmt = 'uint4'; 79 | elseif maxval == 255, 80 | fmt = 'uint8'; 81 | elseif maxval == 2^32-1, 82 | fmt = 'uint32'; 83 | end 84 | I = fread(fid, [cols rows], fmt)'; 85 | else 86 | disp('Not a PGM file'); 87 | end 88 | end 89 | fclose(fid); 90 | -------------------------------------------------------------------------------- /TOOLBOX_calib/loadppm.m: -------------------------------------------------------------------------------- 1 | %LOADPPM Load a PPM image 2 | % 3 | % I = loadppm(filename) 4 | % 5 | % Returns a matrix containing the image loaded from the PPM format 6 | % file filename. Handles ASCII (P3) and binary (P6) PPM file formats. 7 | % 8 | % If the filename has no extension, and open fails, a '.ppm' and 9 | % '.pnm' extension will be tried. 10 | % 11 | % SEE ALSO: saveppm loadpgm 12 | % 13 | % Copyright (c) Peter Corke, 1999 Machine Vision Toolbox for Matlab 14 | 15 | 16 | % Peter Corke 1994 17 | 18 | function I = loadppm(file) 19 | white = [' ' 9 10 13]; % space, tab, lf, cr 20 | white = setstr(white); 21 | 22 | fid = fopen(file, 'r'); 23 | if fid < 0, 24 | fid = fopen([file '.ppm'], 'r'); 25 | end 26 | if fid < 0, 27 | fid = fopen([file '.pnm'], 'r'); 28 | end 29 | if fid < 0, 30 | error('Couldn''t open file'); 31 | end 32 | 33 | magic = fread(fid, 2, 'char'); 34 | while 1 35 | c = fread(fid,1,'char'); 36 | if c == '#', 37 | fgetl(fid); 38 | elseif ~any(c == white) 39 | fseek(fid, -1, 'cof'); % unputc() 40 | break; 41 | end 42 | end 43 | cols = fscanf(fid, '%d', 1); 44 | while 1 45 | c = fread(fid,1,'char'); 46 | if c == '#', 47 | fgetl(fid); 48 | elseif ~any(c == white) 49 | fseek(fid, -1, 'cof'); % unputc() 50 | break; 51 | end 52 | end 53 | rows = fscanf(fid, '%d', 1); 54 | while 1 55 | c = fread(fid,1,'char'); 56 | if c == '#', 57 | fgetl(fid); 58 | elseif ~any(c == white) 59 | fseek(fid, -1, 'cof'); % unputc() 60 | break; 61 | end 62 | end 63 | maxval = fscanf(fid, '%d', 1); 64 | 65 | % assume a carriage return only: 66 | 67 | c = fread(fid,1,'char'); 68 | 69 | % bug: because the image might be starting with special characters! 70 | %while 1 71 | % c = fread(fid,1,'char'); 72 | % if c == '#', 73 | % fgetl(fid); 74 | % elseif ~any(c == white) 75 | % fseek(fid, -1, 'cof'); % unputc() 76 | % break; 77 | % end 78 | %end 79 | if magic(1) == 'P', 80 | if magic(2) == '3', 81 | %disp(['ASCII PPM file ' num2str(rows) ' x ' num2str(cols)]) 82 | I = fscanf(fid, '%d', [cols*3 rows]); 83 | elseif magic(2) == '6', 84 | %disp(['Binary PPM file ' num2str(rows) ' x ' num2str(cols)]) 85 | if maxval == 1, 86 | fmt = 'unint1'; 87 | elseif maxval == 15, 88 | fmt = 'uint4'; 89 | elseif maxval == 255, 90 | fmt = 'uint8'; 91 | elseif maxval == 2^32-1, 92 | fmt = 'uint32'; 93 | end 94 | I = fread(fid, [cols*3 rows], fmt); 95 | else 96 | disp('Not a PPM file'); 97 | end 98 | end 99 | % 100 | % now the matrix has interleaved columns of R, G, B 101 | % 102 | I = I'; 103 | size(I); 104 | R = I(:,1:3:(cols*3)); 105 | G = I(:,2:3:(cols*3)); 106 | B = I(:,3:3:(cols*3)); 107 | fclose(fid); 108 | 109 | 110 | I = zeros(rows,cols,3); 111 | I(:,:,1) = R; 112 | I(:,:,2) = G; 113 | I(:,:,3) = B; 114 | I = uint8(I); 115 | -------------------------------------------------------------------------------- /TOOLBOX_calib/mean_std_robust.m: -------------------------------------------------------------------------------- 1 | function [m,s] = mean_std_robust(x); 2 | 3 | x = x(:); 4 | 5 | m = median(x); 6 | 7 | s = median(abs(x - m))*1.4836; 8 | -------------------------------------------------------------------------------- /TOOLBOX_calib/merge_calibration_sets.m: -------------------------------------------------------------------------------- 1 | 2 | set1 = load(data_set1); % part1\Calib_Results; 3 | set2 = load(data_set2); % part2\Calib_Results; 4 | 5 | shift = set1.n_ima; 6 | 7 | for kk = 1:set1.n_ima 8 | 9 | eval(['X_' num2str(kk) ' = set1.X_' num2str(kk) ';']); 10 | 11 | eval(['dX_' num2str(kk) ' = set1.dX_' num2str(kk) ';']); 12 | eval(['dY_' num2str(kk) ' = set1.dY_' num2str(kk) ';']); 13 | 14 | eval(['wintx_' num2str(kk) ' = set1.wintx_' num2str(kk) ';']); 15 | eval(['winty_' num2str(kk) ' = set1.winty_' num2str(kk) ';']); 16 | 17 | eval(['x_' num2str(kk) ' = set1.x_' num2str(kk) ';']); 18 | 19 | if isfield(set1,'y') 20 | eval(['y_' num2str(kk) ' = set1.y_' num2str(kk) ';']); 21 | else 22 | eval(['y_' num2str(kk) ' = [NaN];']); 23 | end; 24 | 25 | eval(['n_sq_x_' num2str(kk) ' = set1.n_sq_x_' num2str(kk) ';']); 26 | eval(['n_sq_y_' num2str(kk) ' = set1.n_sq_y_' num2str(kk) ';']); 27 | 28 | 29 | if isfield(set1,['omc_' num2str(kk+shift)]) 30 | eval(['omc_' num2str(kk+shift) ' = set1.omc_' num2str(kk) ';']); 31 | eval(['Tc_' num2str(kk+shift) ' = set1.Tc_' num2str(kk) ';']); 32 | else 33 | eval(['omc_' num2str(kk+shift) ' = [NaN;NaN;NaN];']); 34 | eval(['Tc_' num2str(kk+shift) ' = [NaN;NaN;NaN];']); 35 | end; 36 | 37 | end; 38 | 39 | for kk = 1:set2.n_ima 40 | 41 | eval(['X_' num2str(kk+shift) ' = set2.X_' num2str(kk) ';']); 42 | 43 | 44 | eval(['dX_' num2str(kk+shift) ' = set2.dX_' num2str(kk) ';']); 45 | eval(['dY_' num2str(kk+shift) ' = set2.dY_' num2str(kk) ';']); 46 | 47 | eval(['wintx_' num2str(kk+shift) ' = set2.wintx_' num2str(kk) ';']); 48 | eval(['winty_' num2str(kk+shift) ' = set2.winty_' num2str(kk) ';']); 49 | 50 | eval(['x_' num2str(kk+shift) ' = set2.x_' num2str(kk) ';']); 51 | 52 | if isfield(set2,'y') 53 | eval(['y_' num2str(kk) ' = set2.y_' num2str(kk) ';']); 54 | else 55 | eval(['y_' num2str(kk) ' = [NaN];']); 56 | end; 57 | 58 | eval(['n_sq_x_' num2str(kk+shift) ' = set2.n_sq_x_' num2str(kk) ';']); 59 | eval(['n_sq_y_' num2str(kk+shift) ' = set2.n_sq_y_' num2str(kk) ';']); 60 | 61 | 62 | 63 | if isfield(set2,['omc_' num2str(kk+shift)]) 64 | eval(['omc_' num2str(kk+shift) ' = set2.omc_' num2str(kk) ';']); 65 | eval(['Tc_' num2str(kk+shift) ' = set2.Tc_' num2str(kk) ';']); 66 | else 67 | eval(['omc_' num2str(kk+shift) ' = [NaN;NaN;NaN];']); 68 | eval(['Tc_' num2str(kk+shift) ' = [NaN;NaN;NaN];']); 69 | end; 70 | 71 | end; 72 | 73 | 74 | fc = set2.fc; 75 | kc = set2.kc; 76 | cc = set2.cc; 77 | alpha_c = set2.alpha_c; 78 | KK = set2.KK; 79 | inv_KK = set2.inv_KK; 80 | 81 | 82 | n_ima = set1.n_ima + set2.n_ima; 83 | active_images = [set1.active_images set2.active_images]; 84 | 85 | no_image = 1; 86 | -------------------------------------------------------------------------------- /TOOLBOX_calib/merge_two_datasets.m: -------------------------------------------------------------------------------- 1 | fprintf(1,'Script that merges two "Cabib_Results.mat" data sets of the same camera into a single dataset\n') 2 | 3 | dir; 4 | 5 | 6 | cont = 1; 7 | while cont 8 | data_set1 = input('Filename of the first dataset (with complete path if necessary): ','s'); 9 | cont = ((exist(data_set1)~=2)); 10 | if cont, 11 | fprintf(1,'File not found. Try again.\n'); 12 | end; 13 | end; 14 | cont = 1; 15 | while cont 16 | data_set2 = input('Filename of the second dataset (with complete path if necessary): ','s'); 17 | cont = ((exist(data_set2)~=2)); 18 | if cont, 19 | fprintf(1,'File not found. Try again.\n'); 20 | end; 21 | end; 22 | 23 | 24 | load(data_set1); % part1\Calib_Results; 25 | 26 | shift = n_ima; 27 | 28 | load(data_set2); % part2\Calib_Results; 29 | 30 | active_images2 = active_images; 31 | n_ima2 = n_ima; 32 | 33 | 34 | for kk = 1:n_ima 35 | 36 | eval(['X_' num2str(kk+shift) ' = X_' num2str(kk) ';']); 37 | 38 | 39 | eval(['dX_' num2str(kk+shift) ' = dX_' num2str(kk) ';']); 40 | eval(['dY_' num2str(kk+shift) ' = dY_' num2str(kk) ';']); 41 | 42 | eval(['wintx_' num2str(kk+shift) ' = wintx_' num2str(kk) ';']); 43 | eval(['winty_' num2str(kk+shift) ' = winty_' num2str(kk) ';']); 44 | 45 | eval(['x_' num2str(kk+shift) ' = x_' num2str(kk) ';']); 46 | eval(['y_' num2str(kk+shift) ' = y_' num2str(kk) ';']); 47 | 48 | eval(['n_sq_x_' num2str(kk+shift) ' = n_sq_x_' num2str(kk) ';']); 49 | eval(['n_sq_y_' num2str(kk+shift) ' = n_sq_y_' num2str(kk) ';']); 50 | 51 | 52 | eval(['omc_' num2str(kk+shift) ' = omc_' num2str(kk) ';']); 53 | eval(['Tc_' num2str(kk+shift) ' = Tc_' num2str(kk) ';']); 54 | 55 | end; 56 | 57 | load(data_set1); % part1\Calib_Results; 58 | 59 | n_ima = n_ima + n_ima2; 60 | active_images = [active_images active_images2]; 61 | 62 | no_image = 1; 63 | 64 | % Recompute the error (in the vector ex): 65 | comp_error_calib; 66 | 67 | fprintf('The two calibration datasets are now merged. You are now ready to run calibration. \n'); 68 | 69 | -------------------------------------------------------------------------------- /TOOLBOX_calib/mosaic.m: -------------------------------------------------------------------------------- 1 | 2 | if ~exist('I_1'), 3 | active_images_save = active_images; 4 | ima_read_calib; 5 | active_images = active_images_save; 6 | check_active_images; 7 | end; 8 | 9 | check_active_images; 10 | 11 | if isempty(ind_read), 12 | return; 13 | end; 14 | 15 | 16 | n_col = floor(sqrt(n_ima*nx/ny)); 17 | 18 | n_row = ceil(n_ima / n_col); 19 | 20 | 21 | ker2 = 1; 22 | for ii = 1:n_col, 23 | ker2 = conv(ker2,[1/4 1/2 1/4]); 24 | end; 25 | 26 | 27 | II = I_1(1:n_col:end,1:n_col:end); 28 | 29 | [ny2,nx2] = size(II); 30 | 31 | 32 | 33 | kk_c = 1; 34 | 35 | II_mosaic = []; 36 | 37 | for jj = 1:n_row, 38 | 39 | 40 | II_row = []; 41 | 42 | for ii = 1:n_col, 43 | 44 | if (exist(['I_' num2str(kk_c)])) & (kk_c <= n_ima), 45 | 46 | if active_images(kk_c), 47 | eval(['I = I_' num2str(kk_c) ';']); 48 | %I = conv2(conv2(I,ker2,'same'),ker2','same'); % anti-aliasing 49 | I = I(1:n_col:end,1:n_col:end); 50 | else 51 | I = zeros(ny2,nx2); 52 | end; 53 | 54 | else 55 | 56 | I = zeros(ny2,nx2); 57 | 58 | end; 59 | 60 | 61 | 62 | II_row = [II_row I]; 63 | 64 | if ii ~= n_col, 65 | 66 | II_row = [II_row zeros(ny2,3)]; 67 | 68 | end; 69 | 70 | 71 | kk_c = kk_c + 1; 72 | 73 | end; 74 | 75 | nn2 = size(II_row,2); 76 | 77 | if jj ~= n_row, 78 | II_row = [II_row; zeros(3,nn2)]; 79 | end; 80 | 81 | II_mosaic = [II_mosaic ; II_row]; 82 | 83 | end; 84 | 85 | figure(2); 86 | image(II_mosaic); 87 | colormap(gray(256)); 88 | title('Calibration images'); 89 | set(gca,'Xtick',[]) 90 | set(gca,'Ytick',[]) 91 | axis('image'); 92 | 93 | -------------------------------------------------------------------------------- /TOOLBOX_calib/mosaic_no_read.m: -------------------------------------------------------------------------------- 1 | 2 | 3 | if ~exist('n_ima'), 4 | data_calib_no_read; 5 | end; 6 | 7 | 8 | check_active_images; 9 | 10 | n_col = floor(sqrt(n_ima*nx/ny)); 11 | 12 | n_row = ceil(n_ima / n_col); 13 | 14 | 15 | ker2 = 1; 16 | for ii = 1:n_col, 17 | ker2 = conv(ker2,[1/4 1/2 1/4]); 18 | end; 19 | 20 | ny2 = length(1:n_col:ny); 21 | nx2 = length(1:n_col:nx); 22 | 23 | %II = I_1(1:n_col:end,1:n_col:end); 24 | %[ny2,nx2] = size(II); 25 | 26 | 27 | 28 | kk_c = 1; 29 | 30 | II_mosaic = []; 31 | 32 | for jj = 1:n_row, 33 | 34 | 35 | II_row = []; 36 | 37 | for ii = 1:n_col, 38 | 39 | 40 | if (kk_c <= n_ima), 41 | 42 | if ~type_numbering, 43 | number_ext = num2str(image_numbers(kk_c)); 44 | else 45 | number_ext = sprintf(['%.' num2str(N_slots) 'd'],image_numbers(kk_c)); 46 | end; 47 | 48 | ima_name = [calib_name number_ext '.' format_image]; 49 | 50 | 51 | 52 | if (exist(ima_name)) & (kk_c <= n_ima), 53 | 54 | if active_images(kk_c), 55 | 56 | if format_image(1) == 'p', 57 | if format_image(2) == 'p', 58 | I = double(loadppm(ima_name)); 59 | else 60 | I = double(loadpgm(ima_name)); 61 | end; 62 | else 63 | if format_image(1) == 'r', 64 | I = readras(ima_name); 65 | else 66 | I = double(imread(ima_name)); 67 | end; 68 | end; 69 | 70 | %I = conv2(conv2(I,ker2,'same'),ker2','same'); % anti-aliasing 71 | I = I(1:n_col:end,1:n_col:end); 72 | else 73 | I = zeros(ny2,nx2); 74 | end; 75 | 76 | else 77 | 78 | I = zeros(ny2,nx2); 79 | 80 | end; 81 | 82 | else 83 | 84 | I = zeros(ny2,nx2); 85 | 86 | end; 87 | 88 | II_row = [II_row I]; 89 | 90 | if ii ~= n_col, 91 | 92 | II_row = [II_row zeros(ny2,3)]; 93 | 94 | end; 95 | 96 | 97 | kk_c = kk_c + 1; 98 | 99 | end; 100 | 101 | nn2 = size(II_row,2); 102 | 103 | if jj ~= n_row, 104 | II_row = [II_row; zeros(3,nn2)]; 105 | end; 106 | 107 | II_mosaic = [II_mosaic ; II_row]; 108 | 109 | end; 110 | 111 | figure(2); 112 | image(II_mosaic); 113 | colormap(gray(256)); 114 | title('Calibration images'); 115 | set(gca,'Xtick',[]) 116 | set(gca,'Ytick',[]) 117 | axis('image'); 118 | 119 | -------------------------------------------------------------------------------- /TOOLBOX_calib/normalize.m: -------------------------------------------------------------------------------- 1 | function [xn] = normalize(x_kk,fc,cc,kc,alpha_c) 2 | 3 | %normalize 4 | % 5 | %[xn] = normalize(x_kk,fc,cc,kc,alpha_c) 6 | % 7 | %Computes the normalized coordinates xn given the pixel coordinates x_kk 8 | %and the intrinsic camera parameters fc, cc and kc. 9 | % 10 | %INPUT: x_kk: Feature locations on the images 11 | % fc: Camera focal length 12 | % cc: Principal point coordinates 13 | % kc: Distortion coefficients 14 | % alpha_c: Skew coefficient 15 | % 16 | %OUTPUT: xn: Normalized feature locations on the image plane (a 2XN matrix) 17 | % 18 | %Important functions called within that program: 19 | % 20 | %comp_distortion_oulu: undistort pixel coordinates. 21 | 22 | if nargin < 5, 23 | alpha_c = 0; 24 | if nargin < 4; 25 | kc = [0;0;0;0;0]; 26 | if nargin < 3; 27 | cc = [0;0]; 28 | if nargin < 2, 29 | fc = [1;1]; 30 | end; 31 | end; 32 | end; 33 | end; 34 | 35 | 36 | % First: Subtract principal point, and divide by the focal length: 37 | x_distort = [(x_kk(1,:) - cc(1))/fc(1);(x_kk(2,:) - cc(2))/fc(2)]; 38 | 39 | % Second: undo skew 40 | x_distort(1,:) = x_distort(1,:) - alpha_c * x_distort(2,:); 41 | 42 | if norm(kc) ~= 0, 43 | % Third: Compensate for lens distortion: 44 | xn = comp_distortion_oulu(x_distort,kc); 45 | else 46 | xn = x_distort; 47 | end; 48 | -------------------------------------------------------------------------------- /TOOLBOX_calib/normalize2.m: -------------------------------------------------------------------------------- 1 | function [xn,dxdf,dxdc,dxdk,dxdalpha] = normalize2(x_kk,fc,cc,kc,alpha_c), 2 | 3 | %normalize 4 | % 5 | %[xn] = normalize(x_kk,fc,cc,kc,alpha_c) 6 | % 7 | %Computes the normalized coordinates xn given the pixel coordinates x_kk 8 | %and the intrinsic camera parameters fc, cc and kc. 9 | % 10 | %INPUT: x_kk: Feature locations on the images 11 | % fc: Camera focal length 12 | % cc: Principal point coordinates 13 | % kc: Distortion coefficients 14 | % alpha_c: Skew coefficient 15 | % 16 | %OUTPUT: xn: Normalized feature locations on the image plane (a 2XN matrix) 17 | % 18 | %Important functions called within that program: 19 | 20 | k1 = kc(1); 21 | k2 = kc(2); 22 | k3 = kc(5); 23 | p1 = kc(3); 24 | p2 = kc(4); 25 | 26 | N = size(x_kk,2); 27 | 28 | % First: Subtract principal point, and divide by the focal length: 29 | x_distort = [(x_kk(1,:) - cc(1))/fc(1);(x_kk(2,:) - cc(2))/fc(2)]; 30 | 31 | 32 | v1 = - x_distort(1,:) / fc(1); 33 | v2 = - x_distort(2,:) / fc(1); 34 | 35 | dx_distortdfc = zeros(2*N,2); 36 | dx_distortdfc(1:2:end,1) = v1'; 37 | dx_distortdfc(2:2:end,2) = v2'; 38 | 39 | v1 = - x_distort(1,:) / fc(1); 40 | v2 = - x_distort(2,:) / fc(1); 41 | 42 | dx_distortdcc = zeros(2*N,2); 43 | dx_distortdcc(1:2:end,1) = -(1/fc(1)) * ones(N,1); 44 | dx_distortdcc(2:2:end,2) = -(1/fc(2)) * ones(N,1); 45 | 46 | % Second: undo skew 47 | x_distort(1,:) = x_distort(1,:) - alpha_c * x_distort(2,:); 48 | 49 | dx_distort2dfc = [ dx_distortdfc(:,1)-alpha_c *dx_distortdfc(:,2) dx_distortdfc(:,2)]; 50 | dx_distort2dcc = [ dx_distortdcc(:,1)-alpha_c *dx_distortdcc(:,2) dx_distortdcc(:,2)]; 51 | 52 | dx_distort2dalpha_c = zeros(2*N,1); 53 | dx_distort2dalpha_c(1:2:end) = -x_distort(2,:)'; 54 | 55 | x = x_distort; % initial guess 56 | 57 | for kk=1:20, 58 | 59 | r_2 = sum(x.^2); 60 | k_radial = 1 + k1 * r_2 + k2 * r_2.^2 + k3 * r_2.^3; 61 | delta_x = [2*p1*x(1,:).*x(2,:) + p2*(r_2 + 2*x(1,:).^2); p1 * (r_2 + 2*x(2,:).^2)+2*p2*x(1,:).*x(2,:)]; 62 | x = (x_distort - delta_x)./(ones(2,1)*k_radial); 63 | 64 | end; 65 | 66 | 67 | xn = x; 68 | 69 | 70 | dxdk = zeros(2*N,5); % Approximation (no time) 71 | dxdf = dx_distort2dfc; 72 | dxdc = dx_distort2dcc; 73 | dxdalpha = dx_distort2dalpha_c; 74 | -------------------------------------------------------------------------------- /TOOLBOX_calib/normalize_pixel.m: -------------------------------------------------------------------------------- 1 | function [xn] = normalize_pixel(x_kk,fc,cc,kc,alpha_c) 2 | 3 | %normalize 4 | % 5 | %[xn] = normalize_pixel(x_kk,fc,cc,kc,alpha_c) 6 | % 7 | %Computes the normalized coordinates xn given the pixel coordinates x_kk 8 | %and the intrinsic camera parameters fc, cc and kc. 9 | % 10 | %INPUT: x_kk: Feature locations on the images 11 | % fc: Camera focal length 12 | % cc: Principal point coordinates 13 | % kc: Distortion coefficients 14 | % alpha_c: Skew coefficient 15 | % 16 | %OUTPUT: xn: Normalized feature locations on the image plane (a 2XN matrix) 17 | % 18 | %Important functions called within that program: 19 | % 20 | %comp_distortion_oulu: undistort pixel coordinates. 21 | 22 | if nargin < 5, 23 | alpha_c = 0; 24 | if nargin < 4; 25 | kc = [0;0;0;0;0]; 26 | if nargin < 3; 27 | cc = [0;0]; 28 | if nargin < 2, 29 | fc = [1;1]; 30 | end; 31 | end; 32 | end; 33 | end; 34 | 35 | 36 | % First: Subtract principal point, and divide by the focal length: 37 | x_distort = [(x_kk(1,:) - cc(1))/fc(1);(x_kk(2,:) - cc(2))/fc(2)]; 38 | 39 | % Second: undo skew 40 | x_distort(1,:) = x_distort(1,:) - alpha_c * x_distort(2,:); 41 | 42 | if norm(kc) ~= 0, 43 | % Third: Compensate for lens distortion: 44 | xn = comp_distortion_oulu(x_distort,kc); 45 | else 46 | xn = x_distort; 47 | end; 48 | -------------------------------------------------------------------------------- /TOOLBOX_calib/normalize_pixel_fisheye.m: -------------------------------------------------------------------------------- 1 | function [xn] = normalize_pixel_fisheye(x_kk,fc,cc,kc,alpha_c) 2 | 3 | %normalize 4 | % 5 | %[xn] = normalize_pixel(x_kk,fc,cc,kc,alpha_c) 6 | % 7 | %Computes the normalized coordinates xn given the pixel coordinates x_kk 8 | %and the intrinsic camera parameters fc, cc and kc. 9 | % 10 | %INPUT: x_kk: Feature locations on the images 11 | % fc: Camera focal length 12 | % cc: Principal point coordinates 13 | % kc: Fisheye distortion coefficients 14 | % alpha_c: Skew coefficient 15 | % 16 | %OUTPUT: xn: Normalized feature locations on the image plane (a 2XN matrix) 17 | 18 | if nargin < 5, 19 | alpha_c = 0; 20 | if nargin < 4; 21 | kc = [0;0;0;0;0]; 22 | if nargin < 3; 23 | cc = [0;0]; 24 | if nargin < 2, 25 | fc = [1;1]; 26 | end; 27 | end; 28 | end; 29 | end; 30 | 31 | 32 | % First: Subtract principal point, and divide by the focal length: 33 | x_distort = [(x_kk(1,:) - cc(1))/fc(1);(x_kk(2,:) - cc(2))/fc(2)]; 34 | 35 | % Second: undo skew 36 | x_distort(1,:) = x_distort(1,:) - alpha_c * x_distort(2,:); 37 | 38 | % Third: Compensate for lens distortion: 39 | xn = comp_fisheye_distortion(x_distort,kc); 40 | 41 | -------------------------------------------------------------------------------- /TOOLBOX_calib/pattern.eps: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RobVisLab/camera_calibration/f56c17c297653ba0a156d125c2b162b19432b48b/TOOLBOX_calib/pattern.eps -------------------------------------------------------------------------------- /TOOLBOX_calib/pgmread.m: -------------------------------------------------------------------------------- 1 | function img = pgmread(filename) 2 | % function img = pgmread(filename) 3 | % this is my version of pgmread for the pgm file created by XV. 4 | % 5 | % this program also corrects for the shifts in the image from pm file. 6 | 7 | fid = fopen(filename,'r'); 8 | fscanf(fid, 'P5\n'); 9 | cmt = '#'; 10 | while findstr(cmt, '#'), 11 | cmt = fgets(fid); 12 | if length(findstr(cmt, '#')) ~= 1, 13 | YX = sscanf(cmt, '%d %d'); 14 | y = YX(1); x = YX(2); 15 | end 16 | end 17 | 18 | %fgets(fid); 19 | 20 | %img = fscanf(fid,'%d',size); 21 | %img = img'; 22 | 23 | img = fread(fid,[y,x],'uint8'); 24 | img = img'; 25 | fclose(fid); 26 | 27 | -------------------------------------------------------------------------------- /TOOLBOX_calib/point_distribution.m: -------------------------------------------------------------------------------- 1 | % Point Distribution 2 | 3 | colors = 'brgkcm'; 4 | 5 | if ~exist('n_ima')|~exist('fc'), 6 | fprintf(1,'No calibration data available.\n'); 7 | return; 8 | end; 9 | 10 | check_active_images; 11 | 12 | if ~exist(['ex_' num2str(ind_active(1)) ]), 13 | fprintf(1,'Need to calibrate before analysing reprojection error. Maybe need to load Calib_Results.mat file.\n'); 14 | return; 15 | end; 16 | 17 | figure(6); 18 | 19 | for kk=1:n_ima 20 | 21 | if exist(['x_' num2str(kk)]), 22 | 23 | if active_images(kk) & eval(['~isnan(x_' num2str(kk) '(1,1))']), 24 | 25 | eval(['plot(x_' num2str(kk) '(1,:),x_' num2str(kk) '(2,:),''' colors(rem(kk-1,6)+1) '+'');']); 26 | 27 | hold on; 28 | end; 29 | 30 | end; 31 | 32 | end; 33 | 34 | axis('equal'); 35 | 36 | axis([0 nx 0 ny]); 37 | 38 | title1=pwd; 39 | title1=strrep(title1,'_','\_'); 40 | 41 | title({'Point Distribution in Images',title1}); 42 | 43 | xlabel('x'); 44 | 45 | ylabel('y'); 46 | -------------------------------------------------------------------------------- /TOOLBOX_calib/project2_oulu.m: -------------------------------------------------------------------------------- 1 | function [x] = project2_oulu(X,R,T,f,t,k) 2 | %PROJECT Subsidiary to calib 3 | 4 | % (c) Pietro Perona -- March 24, 1994 5 | % California Institute of Technology 6 | % Pasadena, CA 7 | % 8 | % Renamed because project exists in matlab 5.2!!! 9 | % Now uses the more elaborate intrinsic model from Oulu 10 | 11 | 12 | 13 | [m,n] = size(X); 14 | 15 | Y = R*X + T*ones(1,n); 16 | Z = Y(3,:); 17 | 18 | f = f(:); %% make a column vector 19 | if length(f)==1, 20 | f = [f f]'; 21 | end; 22 | 23 | x = (Y(1:2,:) ./ (ones(2,1) * Z)) ; 24 | 25 | 26 | radius_2 = x(1,:).^2 + x(2,:).^2; 27 | 28 | if length(k) > 1, 29 | 30 | radial_distortion = 1 + ones(2,1) * ((k(1) * radius_2) + (k(2) * radius_2.^2)); 31 | 32 | if length(k) < 4, 33 | 34 | delta_x = zeros(2,n); 35 | 36 | else 37 | 38 | delta_x = [2*k(3)*x(1,:).*x(2,:) + k(4)*(radius_2 + 2*x(1,:).^2) ; 39 | k(3) * (radius_2 + 2*x(2,:).^2)+2*k(4)*x(1,:).*x(2,:)]; 40 | 41 | end; 42 | 43 | 44 | else 45 | 46 | radial_distortion = 1 + ones(2,1) * ((k(1) * radius_2)); 47 | 48 | delta_x = zeros(2,n); 49 | 50 | end; 51 | 52 | 53 | x = (x .* radial_distortion + delta_x).* (f * ones(1,n)) + t*ones(1,n); 54 | -------------------------------------------------------------------------------- /TOOLBOX_calib/projectedGrid.m: -------------------------------------------------------------------------------- 1 | function [XX,H] = projectedGrid ( P1, P2, P3, P4 , nx, ny); 2 | 3 | % new formalism using homographies 4 | 5 | a00 = [P1;1]; 6 | a10 = [P2;1]; 7 | a11 = [P3;1]; 8 | a01 = [P4;1]; 9 | 10 | % Compute the planart collineation: 11 | 12 | [H] = compute_collineation (a00, a10, a11, a01); 13 | 14 | 15 | % Build the grid using the planar collineation: 16 | 17 | x_l = ((0:(nx-1))'*ones(1,ny))/(nx-1); 18 | y_l = (ones(nx,1)*(0:(ny-1)))/(ny-1); 19 | 20 | pts = [x_l(:) y_l(:) ones(nx*ny,1)]'; 21 | 22 | XX = H*pts; 23 | 24 | XX = XX(1:2,:) ./ (ones(2,1)*XX(3,:)); 25 | -------------------------------------------------------------------------------- /TOOLBOX_calib/projector_calib.m: -------------------------------------------------------------------------------- 1 | 2 | % load camera results (for setting active images, n_ima,...) 3 | load camera_results; 4 | 5 | 6 | % Projection settings: 7 | 8 | wintx = 12; %8; 9 | winty = 12; %8; 10 | nx = 1024; 11 | ny = 768; 12 | dX = 32; 13 | dY = 32; 14 | dXoff=511.5; 15 | dYoff=383.5; 16 | 17 | 18 | proj_name = 'proj'; %input('Basename projector calibration images (without number nor suffix): ','s'); 19 | camera_name = 'cam'; 20 | 21 | 22 | xr_list = NaN*ones(1,n_ima); 23 | yr_list = NaN*ones(1,n_ima); 24 | 25 | 26 | ind_proc = ind_active; 27 | 28 | 29 | 30 | DEBUG = 1; 31 | %ind_ima_proj = [18]; 32 | 33 | for i = ind_proc, 34 | 35 | projector_marker; 36 | 37 | end; 38 | 39 | 40 | fprintf(1,'\nExtraction of the grid corners on the image\n'); 41 | 42 | 43 | 44 | recompute_corner = 0; 45 | 46 | if recompute_corner & ~exist(['xproj_' num2str(ind_proc(1))]), 47 | if exist('projector_data'), 48 | load projector_data; 49 | else 50 | recompute_corner = 0; 51 | disp('WARNING: Cannot recompute corners. Data need to be extracted at least once'); 52 | end; 53 | end; 54 | 55 | if ~recompute_corner, 56 | disp('Manual extraction mode'); 57 | else 58 | disp('Automatic recomputation of the corners'); 59 | end; 60 | 61 | % extract the projector corners: 62 | 63 | 64 | for kk = ind_proc, 65 | 66 | projector_ima_corners 67 | 68 | end; 69 | 70 | 71 | string_save = 'save projector_data '; 72 | 73 | for kk = ind_active, 74 | string_save = [string_save ' xproj_' num2str(kk) ' x_proj_' num2str(kk)]; 75 | end; 76 | eval(string_save); 77 | 78 | 79 | 80 | 81 | return; 82 | 83 | 84 | i = 18; 85 | 86 | figure(4) 87 | image(I_29); 88 | colormap(gray(256)); 89 | hold on; 90 | plot(x_29(1,:)+1,x_29(2,:)+1,'r+','markersize',13,'linewidth',3) 91 | hold off; 92 | axis image 93 | axis off; 94 | -------------------------------------------------------------------------------- /TOOLBOX_calib/projector_ima_corners.m: -------------------------------------------------------------------------------- 1 | 2 | eval(['Ip = Ip_' num2str(kk) ';']); 3 | eval(['In = In_' num2str(kk) ';']); 4 | 5 | xr = xr_list(kk); 6 | yr = yr_list(kk); 7 | 8 | fprintf(1,'Processing image %d...',kk); 9 | 10 | if ~recompute_corner, 11 | [x,X,n_sq_x,n_sq_y,ind_orig,ind_x,ind_y] = extract_grid(Ip,wintx,winty,fc_save,cc_save,kc_save,dX,dY,xr,yr,1); 12 | xproj = x; 13 | else 14 | eval(['xproj = xproj_' num2str(kk) ';']); 15 | x = cornerfinder(xproj+1,Ip,winty,wintx); 16 | xproj = x - 1; 17 | end; 18 | 19 | Np_proj = size(x,2); 20 | 21 | figure(2); 22 | image(Ip); 23 | hold on; 24 | plot(xproj(1,:)+1,xproj(2,:)+1,'r+'); 25 | %title('Click on your reference point'); 26 | xlabel('Xc (in camera frame)'); 27 | ylabel('Yc (in camera frame)'); 28 | hold off; 29 | 30 | %disp('Click on your reference point...'); 31 | 32 | %[xr,yr] = ginput2(1); 33 | 34 | xr = xr_list(kk); 35 | yr = yr_list(kk); 36 | 37 | err = sqrt(sum((xproj - [xr;yr]*ones(1,Np_proj)).^2)); 38 | ind_ref = find(err == min(err)); 39 | 40 | ref_pt = xproj(:,ind_ref); 41 | 42 | figure(2); 43 | hold on; 44 | plot(ref_pt(1)+1,ref_pt(2)+1,'go'); hold off; 45 | title(['Image ' num2str(kk)]); 46 | drawnow; 47 | 48 | 49 | if ~recompute_corner, 50 | 51 | off_x = mod(ind_ref-1,n_sq_x+1); 52 | off_y = n_sq_y - floor((ind_ref-1)/(n_sq_x+1)); 53 | 54 | x_proj = X(1:2,:) + ([dXoff - dX * off_x ; dYoff - dY * off_y]*ones(1,Np_proj)); 55 | 56 | eval(['x_proj_' num2str(kk) ' = x_proj;']); % coordinates of the points in the projector image 57 | 58 | end; 59 | 60 | eval(['xproj_' num2str(kk) ' = xproj;']); % coordinates of the points in the camera image 61 | 62 | -------------------------------------------------------------------------------- /TOOLBOX_calib/readras.m: -------------------------------------------------------------------------------- 1 | function [X, map] = readras(filename, ys, ye, xs, xe); 2 | %READRAS Read an image file in sun raster format. 3 | % READRAS('imagefile.ras') reads a "sun.raster" image file. 4 | % [X, map] = READRAS('imagefile.ras') returns both the image and a 5 | % color map, so that 6 | % [X, map] = readras('imagefile.ras'); 7 | % image(X) 8 | % colormap(map) 9 | % axis('equal') 10 | % will display the result with the proper colors. 11 | % NOTE: readras cannot deal with complicated color maps. 12 | % In fact, Matlab doesn't quite allow to work with colormaps 13 | % with more than 64 entries. 14 | % 15 | 16 | %% 17 | %% (C) Thomas K. Leung 3/30/93. 18 | %% California Institute of Technology. 19 | %% Modified by Andrea Mennucci to deal with color images 20 | %% 21 | 22 | % PC and UNIX version of readras - Jean-Yves Bouguet - Dec. 1998 23 | 24 | dot = max(find(filename == '.')); 25 | suffix = filename(dot+1:dot+3); 26 | 27 | if(strcmp(lower(suffix), 'ras')) % raster file format % 28 | fp = fopen(filename, 'rb'); 29 | if(fp<0) error(['Cannot open ' filename '.']), end 30 | 31 | %Read and crack the 32-byte header 32 | fseek(fp, 4, -1); 33 | 34 | width = 2^24 * fread(fp, 1, 'uchar') + 2^16 * fread(fp, 1, 'uchar') + 2^8 * fread(fp, 1, 'uchar') + fread(fp, 1, 'uchar'); 35 | 36 | height = 2^24 * fread(fp, 1, 'uchar') + 2^16 * fread(fp, 1, 'uchar') + 2^8 * fread(fp, 1, 'uchar') + fread(fp, 1, 'uchar'); 37 | 38 | depth = 2^24 * fread(fp, 1, 'uchar') + 2^16 * fread(fp, 1, 'uchar') + 2^8 * fread(fp, 1, 'uchar') + fread(fp, 1, 'uchar'); 39 | 40 | length = 2^24 * fread(fp, 1, 'uchar') + 2^16 * fread(fp, 1, 'uchar') + 2^8 * fread(fp, 1, 'uchar') + fread(fp, 1, 'uchar'); 41 | 42 | type = 2^24 * fread(fp, 1, 'uchar') + 2^16 * fread(fp, 1, 'uchar') + 2^8 * fread(fp, 1, 'uchar') + fread(fp, 1, 'uchar'); 43 | 44 | maptype = 2^24 * fread(fp, 1, 'uchar') + 2^16 * fread(fp, 1, 'uchar') + 2^8 * fread(fp, 1, 'uchar') + fread(fp, 1, 'uchar'); 45 | 46 | maplen = 2^24 * fread(fp, 1, 'uchar') + 2^16 * fread(fp, 1, 'uchar') + 2^8 * fread(fp, 1, 'uchar') + fread(fp, 1, 'uchar'); 47 | 48 | maplen = maplen / 3; 49 | 50 | if maptype == 2 % RMT_RAW 51 | map = fread(fp, [maplen, 3], 'uchar')/255; 52 | % if maplen<64, map=[map',zeros(3,64-maplen)]';maplen=64; end; 53 | elseif maptype == 1 % RMT_EQUAL_RGB 54 | map(:,1) = fread(fp, [maplen], 'uchar'); 55 | map(:,2) = fread(fp, [maplen], 'uchar'); 56 | map(:,3) = fread(fp, [maplen], 'uchar'); 57 | %maxmap = max(max(map)); 58 | map = map/255; 59 | if maplen<64, map=[map',zeros(3,64-maplen)]'; maplen=64; end; 60 | else % RMT_NONE 61 | map = []; 62 | end 63 | % if maplen>64, 64 | % map=[map',zeros(3,256-maplen)]'; 65 | % end; 66 | 67 | % Read the image 68 | 69 | if rem(width,2) == 1 70 | Xt = fread(fp, [width+1, height], 'uchar'); 71 | X = Xt(1:width, :)'; 72 | else 73 | Xt = fread(fp, [width, height], 'uchar'); 74 | X = Xt'; 75 | end 76 | X = X + 1; 77 | fclose(fp); 78 | else 79 | error('Image file name must end in either ''ras'' or ''rast''.'); 80 | end 81 | 82 | 83 | if nargin == 5 84 | 85 | X = X(ys:ye, xs:xe); 86 | 87 | end -------------------------------------------------------------------------------- /TOOLBOX_calib/recomp_corner_calib_saddle_points.m: -------------------------------------------------------------------------------- 1 | fprintf(1,'Recomputation of the image corners using Lucchese''s saddle point detector\n'); 2 | 3 | q = input('This detector only works for X junctions (checker board corners). Is this the case here? ([]=yes, other=no)','s'); 4 | 5 | 6 | if isempty(q), 7 | 8 | ima_proc = 1:n_ima; 9 | 10 | fprintf(1,'Processing image '); 11 | 12 | for kk = ima_proc; 13 | 14 | if active_images(kk), 15 | 16 | fprintf(1,'%d...',kk); 17 | 18 | 19 | eval(['I = I_' num2str(kk) ';']); 20 | 21 | eval(['y = x_' num2str(kk) ';']); 22 | eval(['wintx = wintx_' num2str(kk) ';']); 23 | eval(['winty = winty_' num2str(kk) ';']); 24 | 25 | xc = cornerfinder_saddle_point(y+1,I,winty,wintx); % the four corners 26 | 27 | eval(['x_' num2str(kk) '= xc - 1;']); 28 | 29 | else 30 | 31 | if ~exist(['omc_' num2str(kk)]), 32 | 33 | eval(['dX_' num2str(kk) ' = NaN;']); 34 | eval(['dY_' num2str(kk) ' = NaN;']); 35 | 36 | eval(['wintx_' num2str(kk) ' = NaN;']); 37 | eval(['winty_' num2str(kk) ' = NaN;']); 38 | 39 | eval(['x_' num2str(kk) ' = NaN*ones(2,1);']); 40 | eval(['X_' num2str(kk) ' = NaN*ones(3,1);']); 41 | 42 | eval(['n_sq_x_' num2str(kk) ' = NaN;']); 43 | eval(['n_sq_y_' num2str(kk) ' = NaN;']); 44 | 45 | end; 46 | 47 | end; 48 | 49 | end; 50 | 51 | % Recompute the error: 52 | 53 | comp_error_calib; 54 | 55 | fprintf(1,'\ndone\n'); 56 | 57 | else 58 | 59 | fprintf(1,'No recomputation done (The points are not locally saddle points)\n'); 60 | 61 | end; 62 | -------------------------------------------------------------------------------- /TOOLBOX_calib/rect.m: -------------------------------------------------------------------------------- 1 | function [Irec] = rect(I,R,f,c,k,alpha,KK_new); 2 | 3 | 4 | if nargin < 5, 5 | k = [0;0;0;0;0]; 6 | if nargin < 4, 7 | c = [0;0]; 8 | if nargin < 3, 9 | f = [1;1]; 10 | if nargin < 2, 11 | R = eye(3); 12 | if nargin < 1, 13 | error('ERROR: Need an image to rectify'); 14 | end; 15 | end; 16 | end; 17 | end; 18 | end; 19 | 20 | 21 | if nargin < 7, 22 | if nargin < 6, 23 | KK_new = [f(1) 0 c(1);0 f(2) c(2);0 0 1]; 24 | else 25 | KK_new = alpha; % the 6th argument is actually KK_new 26 | end; 27 | alpha = 0; 28 | end; 29 | 30 | 31 | 32 | % Note: R is the motion of the points in space 33 | % So: X2 = R*X where X: coord in the old reference frame, X2: coord in the new ref frame. 34 | 35 | 36 | if ~exist('KK_new'), 37 | KK_new = [f(1) alpha*f(1) c(1);0 f(2) c(2);0 0 1]; 38 | end; 39 | 40 | 41 | [nr,nc] = size(I); 42 | 43 | Irec = 255*ones(nr,nc); 44 | 45 | [mx,my] = meshgrid(1:nc, 1:nr); 46 | px = reshape(mx',nc*nr,1); 47 | py = reshape(my',nc*nr,1); 48 | 49 | rays = inv(KK_new)*[(px - 1)';(py - 1)';ones(1,length(px))]; 50 | 51 | 52 | % Rotation: (or affine transformation): 53 | 54 | rays2 = R'*rays; 55 | 56 | x = [rays2(1,:)./rays2(3,:);rays2(2,:)./rays2(3,:)]; 57 | 58 | 59 | % Add distortion: 60 | xd = apply_distortion(x,k); 61 | 62 | 63 | % Reconvert in pixels: 64 | 65 | px2 = f(1)*(xd(1,:)+alpha*xd(2,:))+c(1); 66 | py2 = f(2)*xd(2,:)+c(2); 67 | 68 | 69 | % Interpolate between the closest pixels: 70 | 71 | px_0 = floor(px2); 72 | 73 | 74 | py_0 = floor(py2); 75 | py_1 = py_0 + 1; 76 | 77 | 78 | good_points = find((px_0 >= 0) & (px_0 <= (nc-2)) & (py_0 >= 0) & (py_0 <= (nr-2))); 79 | 80 | px2 = px2(good_points); 81 | py2 = py2(good_points); 82 | px_0 = px_0(good_points); 83 | py_0 = py_0(good_points); 84 | 85 | alpha_x = px2 - px_0; 86 | alpha_y = py2 - py_0; 87 | 88 | a1 = (1 - alpha_y).*(1 - alpha_x); 89 | a2 = (1 - alpha_y).*alpha_x; 90 | a3 = alpha_y .* (1 - alpha_x); 91 | a4 = alpha_y .* alpha_x; 92 | 93 | ind_lu = px_0 * nr + py_0 + 1; 94 | ind_ru = (px_0 + 1) * nr + py_0 + 1; 95 | ind_ld = px_0 * nr + (py_0 + 1) + 1; 96 | ind_rd = (px_0 + 1) * nr + (py_0 + 1) + 1; 97 | 98 | ind_new = (px(good_points)-1)*nr + py(good_points); 99 | 100 | 101 | 102 | Irec(ind_new) = a1 .* I(ind_lu) + a2 .* I(ind_ru) + a3 .* I(ind_ld) + a4 .* I(ind_rd); 103 | 104 | 105 | 106 | return; 107 | 108 | 109 | % Convert in indices: 110 | 111 | fact = 3; 112 | 113 | [XX,YY]= meshgrid(1:nc,1:nr); 114 | [XXi,YYi]= meshgrid(1:1/fact:nc,1:1/fact:nr); 115 | 116 | %tic; 117 | Iinterp = interp2(XX,YY,I,XXi,YYi); 118 | %toc 119 | 120 | [nri,nci] = size(Iinterp); 121 | 122 | 123 | ind_col = round(fact*(f(1)*xd(1,:)+c(1)))+1; 124 | ind_row = round(fact*(f(2)*xd(2,:)+c(2)))+1; 125 | 126 | good_points = find((ind_col >=1)&(ind_col<=nci)&(ind_row >=1)& (ind_row <=nri)); 127 | -------------------------------------------------------------------------------- /TOOLBOX_calib/rect_index.m: -------------------------------------------------------------------------------- 1 | function [Irec,ind_new,ind_1,ind_2,ind_3,ind_4,a1,a2,a3,a4] = rect_index(I,R,f,c,k,alpha,KK_new); 2 | 3 | 4 | if nargin < 5, 5 | k = [0;0;0;0;0]; 6 | if nargin < 4, 7 | c = [0;0]; 8 | if nargin < 3, 9 | f = [1;1]; 10 | if nargin < 2, 11 | R = eye(3); 12 | if nargin < 1, 13 | error('ERROR: Need an image to rectify'); 14 | %break; 15 | end; 16 | end; 17 | end; 18 | end; 19 | end; 20 | 21 | 22 | if nargin < 7, 23 | if nargin < 6, 24 | KK_new = [f(1) 0 c(1);0 f(2) c(2);0 0 1]; 25 | else 26 | KK_new = alpha; % the 6th argument is actually KK_new 27 | end; 28 | alpha = 0; 29 | end; 30 | 31 | 32 | 33 | % Note: R is the motion of the points in space 34 | % So: X2 = R*X where X: coord in the old reference frame, X2: coord in the new ref frame. 35 | 36 | 37 | if ~exist('KK_new'), 38 | KK_new = [f(1) alpha_c*fc(1) c(1);0 f(2) c(2);0 0 1]; 39 | end; 40 | 41 | 42 | [nr,nc] = size(I); 43 | 44 | Irec = 255*ones(nr,nc); 45 | 46 | [mx,my] = meshgrid(1:nc, 1:nr); 47 | px = reshape(mx',nc*nr,1); 48 | py = reshape(my',nc*nr,1); 49 | 50 | rays = inv(KK_new)*[(px - 1)';(py - 1)';ones(1,length(px))]; 51 | 52 | 53 | % Rotation: (or affine transformation): 54 | 55 | rays2 = R'*rays; 56 | 57 | x = [rays2(1,:)./rays2(3,:);rays2(2,:)./rays2(3,:)]; 58 | 59 | 60 | % Add distortion: 61 | xd = apply_distortion(x,k); 62 | 63 | 64 | % Reconvert in pixels: 65 | 66 | px2 = f(1)*(xd(1,:)+alpha*xd(2,:))+c(1); 67 | py2 = f(2)*xd(2,:)+c(2); 68 | 69 | 70 | % Interpolate between the closest pixels: 71 | 72 | px_0 = floor(px2); 73 | py_0 = floor(py2); 74 | 75 | 76 | good_points = find((px_0 >= 0) & (px_0 <= (nc-2)) & (py_0 >= 0) & (py_0 <= (nr-2))); 77 | 78 | px2 = px2(good_points); 79 | py2 = py2(good_points); 80 | px_0 = px_0(good_points); 81 | py_0 = py_0(good_points); 82 | 83 | alpha_x = px2 - px_0; 84 | alpha_y = py2 - py_0; 85 | 86 | a1 = (1 - alpha_y).*(1 - alpha_x); 87 | a2 = (1 - alpha_y).*alpha_x; 88 | a3 = alpha_y .* (1 - alpha_x); 89 | a4 = alpha_y .* alpha_x; 90 | 91 | ind_1 = px_0 * nr + py_0 + 1; 92 | ind_2 = (px_0 + 1) * nr + py_0 + 1; 93 | ind_3 = px_0 * nr + (py_0 + 1) + 1; 94 | ind_4 = (px_0 + 1) * nr + (py_0 + 1) + 1; 95 | 96 | ind_new = (px(good_points)-1)*nr + py(good_points); 97 | 98 | 99 | Irec(ind_new) = a1 .* I(ind_1) + a2 .* I(ind_2) + a3 .* I(ind_3) + a4 .* I(ind_4); 100 | 101 | 102 | 103 | return; 104 | 105 | 106 | % Convert in indices: 107 | 108 | fact = 3; 109 | 110 | [XX,YY]= meshgrid(1:nc,1:nr); 111 | [XXi,YYi]= meshgrid(1:1/fact:nc,1:1/fact:nr); 112 | 113 | %tic; 114 | Iinterp = interp2(XX,YY,I,XXi,YYi); 115 | %toc 116 | 117 | [nri,nci] = size(Iinterp); 118 | 119 | 120 | ind_col = round(fact*(f(1)*xd(1,:)+c(1)))+1; 121 | ind_row = round(fact*(f(2)*xd(2,:)+c(2)))+1; 122 | 123 | good_points = find((ind_col >=1)&(ind_col<=nci)&(ind_row >=1)& (ind_row <=nri)); 124 | -------------------------------------------------------------------------------- /TOOLBOX_calib/rigid_motion.m: -------------------------------------------------------------------------------- 1 | function [Y,dYdom,dYdT] = rigid_motion(X,om,T); 2 | 3 | %rigid_motion.m 4 | % 5 | %[Y,dYdom,dYdT] = rigid_motion(X,om,T) 6 | % 7 | %Computes the rigid motion transformation Y = R*X+T, where R = rodrigues(om). 8 | % 9 | %INPUT: X: 3D structure in the world coordinate frame (3xN matrix for N points) 10 | % (om,T): Rigid motion parameters between world coordinate frame and camera reference frame 11 | % om: rotation vector (3x1 vector); T: translation vector (3x1 vector) 12 | % 13 | %OUTPUT: Y: 3D coordinates of the structure points in the camera reference frame (3xN matrix for N points) 14 | % dYdom: Derivative of Y with respect to om ((3N)x3 matrix) 15 | % dYdT: Derivative of Y with respect to T ((3N)x3 matrix) 16 | % 17 | %Definitions: 18 | %Let P be a point in 3D of coordinates X in the world reference frame (stored in the matrix X) 19 | %The coordinate vector of P in the camera reference frame is: Y = R*X + T 20 | %where R is the rotation matrix corresponding to the rotation vector om: R = rodrigues(om); 21 | % 22 | %Important function called within that program: 23 | % 24 | %rodrigues.m: Computes the rotation matrix corresponding to a rotation vector 25 | 26 | 27 | 28 | if nargin < 3, 29 | T = zeros(3,1); 30 | if nargin < 2, 31 | om = zeros(3,1); 32 | if nargin < 1, 33 | error('Need at least a 3D structure as input (in rigid_motion.m)'); 34 | return; 35 | end; 36 | end; 37 | end; 38 | 39 | 40 | [R,dRdom] = rodrigues(om); 41 | 42 | [m,n] = size(X); 43 | 44 | Y = R*X + repmat(T,[1 n]); 45 | 46 | if nargout > 1, 47 | 48 | 49 | dYdR = zeros(3*n,9); 50 | dYdT = zeros(3*n,3); 51 | 52 | dYdR(1:3:end,1:3:end) = X'; 53 | dYdR(2:3:end,2:3:end) = X'; 54 | dYdR(3:3:end,3:3:end) = X'; 55 | 56 | dYdT(1:3:end,1) = ones(n,1); 57 | dYdT(2:3:end,2) = ones(n,1); 58 | dYdT(3:3:end,3) = ones(n,1); 59 | 60 | dYdom = dYdR * dRdom; 61 | 62 | end; 63 | 64 | 65 | 66 | 67 | -------------------------------------------------------------------------------- /TOOLBOX_calib/rotation.m: -------------------------------------------------------------------------------- 1 | function [] = rotation(st); 2 | 3 | if nargin < 1, 4 | st= 1; 5 | end; 6 | 7 | 8 | fig = gcf; 9 | 10 | ax = gca; 11 | 12 | vv = get(ax,'view'); 13 | 14 | az = vv(1); 15 | el = vv(2); 16 | 17 | for azi = az:-abs(st):az-360, 18 | 19 | set(ax,'view',[azi el]); 20 | figure(fig); 21 | drawnow; 22 | 23 | end; 24 | -------------------------------------------------------------------------------- /TOOLBOX_calib/run_error_analysis.m: -------------------------------------------------------------------------------- 1 | %%% Program that launchs the complete 2 | 3 | for N_ima_active = 1:30, 4 | 5 | error_analysis; 6 | 7 | end; 8 | 9 | 10 | 11 | return; 12 | 13 | 14 | f = []; 15 | f_std = []; 16 | 17 | c = []; 18 | c_std = []; 19 | 20 | k = []; 21 | k_std = []; 22 | 23 | NN = 30; 24 | 25 | for rr = 1:NN, 26 | 27 | load(['Calib_Accuracies_' num2str(rr)]); 28 | 29 | [m1,s1] = mean_std_robust(fc_list(1,:)); 30 | [m2,s2] = mean_std_robust(fc_list(2,:)); 31 | 32 | f = [f [m1;m2]]; 33 | f_std = [f_std [s1;s2]]; 34 | 35 | [m1,s1] = mean_std_robust(cc_list(1,:)); 36 | [m2,s2] = mean_std_robust(cc_list(2,:)); 37 | 38 | c = [c [m1;m2]]; 39 | c_std = [c_std [s1;s2]]; 40 | 41 | [m1,s1] = mean_std_robust(kc_list(1,:)); 42 | [m2,s2] = mean_std_robust(kc_list(2,:)); 43 | [m3,s3] = mean_std_robust(kc_list(3,:)); 44 | [m4,s4] = mean_std_robust(kc_list(4,:)); 45 | 46 | k = [k [m1;m2;m3;m4]]; 47 | k_std = [k_std [s1;s2;s3;s4]]; 48 | 49 | end; 50 | 51 | figure(1); 52 | errorbar([1:NN;1:NN]',f',f_std'); 53 | figure(2); 54 | errorbar([1:NN;1:NN]',c',c_std'); 55 | figure(3); 56 | errorbar([1:NN;1:NN;1:NN;1:NN]',k',k_std'); 57 | 58 | figure(4); 59 | semilogy(f_std'); 60 | 61 | figure(5); 62 | semilogy(c_std'); 63 | 64 | figure(6); 65 | semilogy(k_std'); 66 | -------------------------------------------------------------------------------- /TOOLBOX_calib/saveinr.m: -------------------------------------------------------------------------------- 1 | %SAVEINR Write an INRIMAGE format file 2 | % 3 | % SAVEINR(filename, im) 4 | % 5 | % Saves the specified image array in a INRIA image format file. 6 | % 7 | % SEE ALSO: loadinr 8 | % 9 | % Copyright (c) Peter Corke, 1999 Machine Vision Toolbox for Matlab 10 | 11 | % Peter Corke 1996 12 | 13 | function saveinr(fname, im) 14 | 15 | fid = fopen(fname, 'w'); 16 | [r,c] = size(im'); 17 | 18 | % build the header 19 | hdr = []; 20 | s = sprintf('#INRIMAGE-4#{\n'); 21 | hdr = [hdr s]; 22 | s = sprintf('XDIM=%d\n',c); 23 | hdr = [hdr s]; 24 | s = sprintf('YDIM=%d\n',r); 25 | hdr = [hdr s]; 26 | s = sprintf('ZDIM=1\n'); 27 | hdr = [hdr s]; 28 | s = sprintf('VDIM=1\n'); 29 | hdr = [hdr s]; 30 | s = sprintf('TYPE=float\n'); 31 | hdr = [hdr s]; 32 | s = sprintf('PIXSIZE=32\n'); 33 | hdr = [hdr s]; 34 | s = sprintf('SCALE=2**0\n'); 35 | hdr = [hdr s]; 36 | s = sprintf('CPU=sun\n#'); 37 | hdr = [hdr s]; 38 | 39 | % make it 256 bytes long and write it 40 | hdr256 = zeros(1,256); 41 | hdr256(1:length(hdr)) = hdr; 42 | fwrite(fid, hdr256, 'uchar'); 43 | 44 | % now the binary data 45 | fwrite(fid, im', 'float32'); 46 | fclose(fid) 47 | -------------------------------------------------------------------------------- /TOOLBOX_calib/savepgm.m: -------------------------------------------------------------------------------- 1 | %SAVEPGM Write a PGM format file 2 | % 3 | % SAVEPGM(filename, im) 4 | % 5 | % Saves the specified image array in a binary (P5) format PGM image file. 6 | % 7 | % SEE ALSO: loadpgm 8 | % 9 | % Copyright (c) Peter Corke, 1999 Machine Vision Toolbox for Matlab 10 | 11 | 12 | % Peter Corke 1994 13 | 14 | function savepgm(fname, im) 15 | 16 | fid = fopen(fname, 'w'); 17 | [r,c] = size(im'); 18 | fprintf(fid, 'P5\n'); 19 | fprintf(fid, '%d %d\n', r, c); 20 | fprintf(fid, '255\n'); 21 | fwrite(fid, im', 'uchar'); 22 | fclose(fid); 23 | -------------------------------------------------------------------------------- /TOOLBOX_calib/saveppm.m: -------------------------------------------------------------------------------- 1 | %SAVEPPM Write a PPM format file 2 | % 3 | % SAVEPPM(filename, I) 4 | % 5 | % Saves the specified red, green and blue planes in a binary (P6) 6 | % format PPM image file. 7 | % 8 | % SEE ALSO: loadppm 9 | % 10 | % Copyright (c) Peter Corke, 1999 Machine Vision Toolbox for Matlab 11 | 12 | 13 | % Peter Corke 1994 14 | 15 | function saveppm(fname, I) 16 | 17 | I = double(I); 18 | 19 | if size(I,3) == 1, 20 | R = I; 21 | G = I; 22 | B = I; 23 | else 24 | R = I(:,:,1); 25 | G = I(:,:,2); 26 | B = I(:,:,3); 27 | end; 28 | 29 | %keyboard; 30 | 31 | fid = fopen(fname, 'w'); 32 | [r,c] = size(R'); 33 | fprintf(fid, 'P6\n'); 34 | fprintf(fid, '%d %d\n', r, c); 35 | fprintf(fid, '255\n'); 36 | R = R'; 37 | G = G'; 38 | B = B'; 39 | im = [R(:) G(:) B(:)]; 40 | %im = reshape(im,r,c*3); 41 | im = im'; 42 | %im = im(:); 43 | fwrite(fid, im, 'uchar'); 44 | fclose(fid); 45 | 46 | -------------------------------------------------------------------------------- /TOOLBOX_calib/saving_stereo_calib.m: -------------------------------------------------------------------------------- 1 | fprintf(1,'Saving the stereo calibration results in Calib_Results_stereo.mat...\n'); 2 | 3 | string_save = 'save Calib_Results_stereo om R T recompute_intrinsic_right recompute_intrinsic_left calib_name_left format_image_left type_numbering_left image_numbers_left N_slots_left calib_name_right format_image_right type_numbering_right image_numbers_right N_slots_right fc_left cc_left kc_left alpha_c_left KK_left fc_right cc_right kc_right alpha_c_right KK_right active_images dX dY nx ny n_ima active_images_right active_images_left inconsistent_images center_optim_left est_alpha_left est_dist_left est_fc_left est_aspect_ratio_left center_optim_right est_alpha_right est_dist_right est_fc_right est_aspect_ratio_right history param param_error sigma_x om_error T_error fc_left_error cc_left_error kc_left_error alpha_c_left_error fc_right_error cc_right_error kc_right_error alpha_c_right_error'; 4 | 5 | for kk = 1:n_ima, 6 | if active_images(kk), 7 | string_save = [string_save ' x_right_' num2str(kk) ' X_right_' num2str(kk) ' x_left_' num2str(kk) ' X_left_' num2str(kk) ' omc_left_' num2str(kk) ' Tc_left_' num2str(kk) ' omc_left_error_' num2str(kk) ' Tc_left_error_' num2str(kk) ' n_sq_x_' num2str(kk) ' n_sq_y_' num2str(kk)]; 8 | end; 9 | end; 10 | eval(string_save); 11 | -------------------------------------------------------------------------------- /TOOLBOX_calib/scanning_script.m: -------------------------------------------------------------------------------- 1 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 2 | %-- Main 3D Scanning Script 3 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 4 | 5 | 6 | if exist('calib_cam_proj_optim.mat')~=2, 7 | error('The scanner calibration file does not exist. Make sure to go to the directory where the scanning images and the calibration file are located (folder scanning_example)'); 8 | end; 9 | 10 | % Loading the scanner calibration parameters: 11 | fprintf(1,'Loading the scanner calibration data...\n'); 12 | load calib_cam_proj_optim; 13 | 14 | 15 | % Choose a dataset (scan #20 for example) 16 | ind_view = 20; 17 | stripe_image = ['strip' sprintf('%.4d',ind_view) ]; 18 | 19 | if exist([stripe_image '_pat00p.bmp'])~=2, 20 | error('The scanning images cannot be found. Make sure to go to the directory where the scanning images are located (folder scanning_example)'); 21 | end; 22 | 23 | 24 | % Compute the projector coordinates at every pixel in the camera image: 25 | fprintf(1,'Computing the subpixel projector coordinates at every pixel in the camera image...\n'); 26 | [xc,xp,nx,ny] = ComputeStripes(stripe_image,1); 27 | 28 | 29 | % Triangulate the 3D geometry: 30 | fprintf(1,'Triangulating the 3D geometry...\n'); 31 | [Xc,Xp] = Compute3D(xc,xp,R,T,fc,fp,cc,cp,kc,kp,alpha_c,alpha_p); 32 | 33 | 34 | % Meshing the points: 35 | Thresh_connect = 15; 36 | N_smoothing = 1; 37 | fprintf(1,'Computing the 3D surface mesh...\n'); 38 | [Xc2,tri2,xc2,xp2,dc2,xc_texture,nc2,conf_nc2,Nn2] = Meshing(Xc,xc,xp,Thresh_connect,N_smoothing,om,T,nx,ny,fc,cc,kc,alpha_c,fp,cp,kp,alpha_p); 39 | 40 | 41 | % Display the 3D mesh: 42 | figure(7); 43 | h = trimesh(tri2,Xc2(1,:),Xc2(3,:),-Xc2(2,:)); 44 | set(h,'EdgeColor', 'b'); 45 | xlabel('X'); 46 | ylabel('Y'); 47 | zlabel('Z'); 48 | axis('equal'); 49 | rotate3d on; 50 | view(0.5,12); 51 | axis equal 52 | 53 | 54 | 55 | % Save the mesh in a VRML file: 56 | TT = [0;0;0]; 57 | wwn = [1;0;0]; 58 | nw = pi; 59 | fieldOfView = 3*atan((ny/2)/fc(2)); 60 | 61 | filename_VRML = ['mesh' sprintf('%.4d',ind_view) '.wrl']; 62 | 63 | fprintf(1,'Saving Geometry in the VRML file %s...(use Cosmo Player to visualize the mesh)\n',filename_VRML); 64 | 65 | file = fopen(filename_VRML,'wt'); 66 | fprintf(file ,'#VRML V2.0 utf8\n'); 67 | fprintf(file,'Transform { children [ Viewpoint {position %.4f %.4f %.4f orientation %.4f %.4f %.4f %.4f fieldOfView %.4f } \n',[TT ; wwn ; nw; fieldOfView]); 68 | %fprintf(file,'Transform { children [ Viewpoint {position %.4f %.4f %.4f orientation %.4f %.4f %.4f %.4f fieldOfView %.4f } \n',[TT ; wwn ; 0; atan((ny/2)/fc(2))]); 69 | fprintf(file ,'Transform { children [ Shape { appearance Appearance { material Material { }} geometry IndexedFaceSet { coord Coordinate { point [\n'); 70 | fprintf(file,'%.3f %.3f %.3f,\n',Xc2); 71 | fprintf(file ,']} coordIndex [\n'); 72 | fprintf(file,'%d,%d,%d,-1,\n',tri2'-1); 73 | fprintf(file ,']}}]}\n'); 74 | fprintf(file,']} '); 75 | fclose(file) ; 76 | -------------------------------------------------------------------------------- /TOOLBOX_calib/script_fit_distortion.m: -------------------------------------------------------------------------------- 1 | 2 | satis_distort = 0; 3 | 4 | disp(['Estimated focal: ' num2str(f_g) ' pixels']); 5 | 6 | while ~satis_distort, 7 | 8 | k_g = input('Guess for distortion factor kc ([]=0): '); 9 | 10 | if isempty(k_g), k_g = 0; end; 11 | 12 | xy_corners_undist = comp_distortion2([x' - c_g(1);y'-c_g(2)]/f_g,k_g); 13 | 14 | xu = xy_corners_undist(1,:)'; 15 | yu = xy_corners_undist(2,:)'; 16 | 17 | [XXu] = projectedGrid ( [xu(1);yu(1)], [xu(2);yu(2)],[xu(3);yu(3)], [xu(4);yu(4)],n_sq_x+1,n_sq_y+1); % The full grid 18 | 19 | XX = (ones(2,1)*(1 + k_g * sum(XXu.^2))) .* XXu; 20 | XX(1,:) = f_g*XX(1,:)+c_g(1); 21 | XX(2,:) = f_g*XX(2,:)+c_g(2); 22 | 23 | figure(2); 24 | image(I); 25 | colormap(map); 26 | zoom on; 27 | hold on; 28 | %plot(f_g*XXu(1,:)+c_g(1),f_g*XXu(2,:)+c_g(2),'ro'); 29 | plot(XX(1,:),XX(2,:),'r+'); 30 | title('The red crosses should be on the grid corners...'); 31 | hold off; 32 | 33 | satis_distort = input('Satisfied with distortion? ([]=no, other=yes) '); 34 | 35 | satis_distort = ~isempty(satis_distort); 36 | 37 | 38 | end; 39 | -------------------------------------------------------------------------------- /TOOLBOX_calib/script_fit_distortion_fisheye.m: -------------------------------------------------------------------------------- 1 | 2 | satis_distort = 0; 3 | 4 | while ~satis_distort, 5 | 6 | k_g_save = k_g; 7 | 8 | k_g = input(['Guess for distortion factor kc ([]=' num2str(k_g_save) '): ']); 9 | 10 | if isempty(k_g), k_g = k_g_save; end; 11 | 12 | 13 | x_n = (x - c_g(1))/f_g; 14 | y_n = (y - c_g(2))/f_g; 15 | 16 | [x_pn] = comp_fisheye_distortion([x_n' ; y_n'],[k_g;0;0;0]); 17 | 18 | 19 | % Compute the inside points through computation of the planar homography (collineation) 20 | 21 | a00 = [x_pn(1,1);x_pn(2,1);1]; 22 | a10 = [x_pn(1,2);x_pn(2,2);1]; 23 | a11 = [x_pn(1,3);x_pn(2,3);1]; 24 | a01 = [x_pn(1,4);x_pn(2,4);1]; 25 | 26 | % Compute the planar collineation: (return the normalization matrix as well) 27 | [Homo,Hnorm,inv_Hnorm] = compute_homography([a00 a10 a11 a01],[0 1 1 0;0 0 1 1;1 1 1 1]); 28 | 29 | 30 | % Build the grid using the planar collineation: 31 | 32 | x_l = ((0:n_sq_x)'*ones(1,n_sq_y+1))/n_sq_x; 33 | y_l = (ones(n_sq_x+1,1)*(0:n_sq_y))/n_sq_y; 34 | pts = [x_l(:) y_l(:) ones((n_sq_x+1)*(n_sq_y+1),1)]'; 35 | 36 | XXpn = Homo*pts; 37 | XXpn = XXpn(1:2,:) ./ (ones(2,1)*XXpn(3,:)); 38 | 39 | XX = apply_fisheye_distortion(XXpn,[k_g;0;0;0]); 40 | 41 | XX(1,:) = f_g*XX(1,:) + c_g(1); 42 | XX(2,:) = f_g*XX(2,:) + c_g(2); 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | figure(2); 51 | image(I); 52 | colormap(map); 53 | zoom on; 54 | hold on; 55 | plot(XX(1,:),XX(2,:),'r+'); 56 | title('The red crosses should be on the grid corners...'); 57 | hold off; 58 | 59 | satis_distort = input('Satisfied with distortion? ([]=no, other=yes) '); 60 | 61 | satis_distort = ~isempty(satis_distort); 62 | 63 | 64 | end; 65 | -------------------------------------------------------------------------------- /TOOLBOX_calib/show_calib_results.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RobVisLab/camera_calibration/f56c17c297653ba0a156d125c2b162b19432b48b/TOOLBOX_calib/show_calib_results.m -------------------------------------------------------------------------------- /TOOLBOX_calib/show_calib_results_fisheye.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RobVisLab/camera_calibration/f56c17c297653ba0a156d125c2b162b19432b48b/TOOLBOX_calib/show_calib_results_fisheye.m -------------------------------------------------------------------------------- /TOOLBOX_calib/show_stereo_calib_results.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RobVisLab/camera_calibration/f56c17c297653ba0a156d125c2b162b19432b48b/TOOLBOX_calib/show_stereo_calib_results.m -------------------------------------------------------------------------------- /TOOLBOX_calib/show_window.m: -------------------------------------------------------------------------------- 1 | function show_window(cell_list,fig_number,title_figure,x_size,y_size,gap_x,font_name,font_size) 2 | 3 | 4 | if ~exist('cell_list'), 5 | error('No description of the functions'); 6 | end; 7 | 8 | if ~exist('fig_number'), 9 | fig_number = 1; 10 | end; 11 | if ~exist('title_figure'), 12 | title_figure = ''; 13 | end; 14 | if ~exist('x_size'), 15 | x_size = 85; 16 | end; 17 | if ~exist('y_size'), 18 | y_size = 14; 19 | end; 20 | if ~exist('gap_x'), 21 | gap_x = 0; 22 | end; 23 | if ~exist('font_name'), 24 | font_name = 'clean'; 25 | end; 26 | if ~exist('font_size'), 27 | font_size = 8; 28 | end; 29 | 30 | figure(fig_number); clf; 31 | pos = get(fig_number,'Position'); 32 | 33 | [n_row,n_col] = size(cell_list); 34 | 35 | fig_size_x = x_size*n_col+(n_col+1)*gap_x; 36 | fig_size_y = y_size*n_row+(n_row+1)*gap_x; 37 | 38 | set(fig_number,'Units','points', ... 39 | 'BackingStore','off', ... 40 | 'Color',[0.8 0.8 0.8], ... 41 | 'MenuBar','none', ... 42 | 'Resize','off', ... 43 | 'Name',title_figure, ... 44 | 'Position',[pos(1) pos(2) fig_size_x fig_size_y], ... 45 | 'NumberTitle','off'); %,'WindowButtonMotionFcn',['figure(' num2str(fig_number) ');']); 46 | 47 | h_mat = zeros(n_row,n_col); 48 | 49 | posx = zeros(n_row,n_col); 50 | posy = zeros(n_row,n_col); 51 | 52 | for i=n_row:-1:1, 53 | for j = n_col:-1:1, 54 | posx(i,j) = gap_x+(j-1)*(x_size+gap_x); 55 | posy(i,j) = fig_size_y - i*(gap_x+y_size); 56 | end; 57 | end; 58 | 59 | for i=n_row:-1:1, 60 | for j = n_col:-1:1, 61 | if ~isempty(cell_list{i,j}), 62 | if ~isempty(cell_list{i,j}{1}) & ~isempty(cell_list{i,j}{2}), 63 | h_mat(i,j) = uicontrol('Parent',fig_number, ... 64 | 'Units','points', ... 65 | 'Callback',cell_list{i,j}{2}, ... 66 | 'ListboxTop',0, ... 67 | 'Position',[posx(i,j) posy(i,j) x_size y_size], ... 68 | 'String',cell_list{i,j}{1}, ... 69 | 'fontsize',font_size,... 70 | 'fontname',font_name,... 71 | 'Tag','Pushbutton1'); 72 | end; 73 | end; 74 | end; 75 | end; 76 | 77 | %------ END PROTECTED REGION ----------------% 78 | -------------------------------------------------------------------------------- /TOOLBOX_calib/skew3.m: -------------------------------------------------------------------------------- 1 | function [S,dSdT] = skew3(T); 2 | 3 | S = [ 0 -T(3) T(2) 4 | T(3) 0 -T(1) 5 | -T(2) T(1) 0 ]; 6 | 7 | dSdT = [0 0 0;0 0 1;0 -1 0 ;0 0 -1;0 0 0;1 0 0 ;0 1 0;-1 0 0; 0 0 0]; 8 | 9 | return; 10 | 11 | 12 | % Test of Jacobian: 13 | 14 | T1 = randn(3,1); 15 | 16 | dT = 0.001*randn(3,1); 17 | 18 | T2 = T1 + dT; 19 | 20 | [S1,dSdT] = skew3(T1); 21 | [S2] = skew3(T2); 22 | 23 | S2app = S1; 24 | S2app(:) = S2app(:) + dSdT*dT; 25 | 26 | 27 | norm(S1 - S2) / norm(S2app - S2) 28 | -------------------------------------------------------------------------------- /TOOLBOX_calib/small_test_script.m: -------------------------------------------------------------------------------- 1 | % This is small script that demonstrate the computation of extrinsic 2 | % parameters using 3D structures. 3 | % This test was build from data provided by Daniel Small (thank you Daniel!) 4 | 5 | 6 | %-- Image points (in pixels): 7 | 8 | x = [479.5200 236.0800 9 | 608.4100 415.3700 10 | 461.0000 40.0000 11 | 451.4800 308.7000 12 | 373.9900 314.8900 13 | 299.3200 319.1300 14 | 231.5500 321.3700 15 | 443.7300 282.9200 16 | 378.3600 288.3000 17 | 314.6900 292.7400 18 | 255.4700 296.2300]'; 19 | 20 | 21 | % 3D world coordinates: 22 | 23 | X = [ 0 0 0 24 | 54.0000 0 0 25 | 0 0 40.5000 26 | 27.0000 -8.4685 -2.3750 27 | 27.0000 -18.4685 -2.3750 28 | 27.0000 -28.4685 -2.3750 29 | 27.0000 -38.4685 -2.3750 30 | 17.0000 -8.4685 -2.3750 31 | 17.0000 -18.4685 -2.3750 32 | 17.0000 -28.4685 -2.3750 33 | 17.0000 -38.4685 -2.3750]'; 34 | 35 | 36 | %------------ Intrinsic parameters: 37 | %--- focal: 38 | fc = [ 395.0669 357.1178 ]'; 39 | %--- Principal point: 40 | cc = [ 380.5387 230.5278 ]'; 41 | %--- Distortion coefficients: 42 | kc = [-0.2601 0.0702 -0.0019 -0.0003 0]'; 43 | %--- Skew coefficient: 44 | alpha_c = 0; 45 | 46 | %----- Computation of the pose of the object in space 47 | %----- (or the rigid motion between world reference frame and camera ref. frame) 48 | [om,T,R] = compute_extrinsic(x,X,fc,cc,kc,alpha_c); 49 | 50 | %--- Try to reproject the structure to see if the computed pose makes sense: 51 | x2 = project_points2(X_1,omckk,Tckk,fc,cc,kc,alpha_c); 52 | 53 | 54 | % Graphical output: 55 | figure(2); 56 | plot(x(1,:),x(2,:),'r+'); 57 | hold on; 58 | plot(x2(1,:),x2(2,:),'go'); 59 | hold off; 60 | axis('equal'); 61 | axis('image'); 62 | title('red crosses: data, green circles: reprojected structure -- IT WORKS!!!'); 63 | xlabel('x'); 64 | ylabel('y'); 65 | 66 | -------------------------------------------------------------------------------- /TOOLBOX_calib/smooth_images.m: -------------------------------------------------------------------------------- 1 | % Anisotropically diffuse the calibration image 2 | % to enhance the corner detection 3 | 4 | 5 | fprintf(1,'Anisotropic diffusion of the images for corner enhancement (the images have to be loaded in memory)\n'); 6 | 7 | 8 | ker = [1/4 1/2 1/4]; 9 | ker2 = conv2(ker,ker); 10 | ker2 = conv2(ker2,ker); 11 | ker2 = conv2(ker2,ker); 12 | 13 | 14 | 15 | if ~exist(['I_' num2str(ind_active(1))]), 16 | ima_read_calib; 17 | end; 18 | 19 | check_active_images; 20 | 21 | format_image2 = format_image; 22 | if format_image2(1) == 'j', 23 | format_image2 = 'bmp'; 24 | end; 25 | 26 | for kk = 1:n_ima, 27 | 28 | if exist(['I_' num2str(kk)]), 29 | 30 | %fprintf(1,'%d...',kk); 31 | 32 | eval(['I = I_' num2str(kk) ';']); 33 | 34 | 35 | % Compute the sigI automatically: 36 | [nn,xx] = hist(I(:),50); 37 | nn = conv2(nn,ker2,'same'); 38 | 39 | max_nn = max(nn); 40 | 41 | 42 | localmax = [0 (nn(2:end-1)>=nn(3:end)) & (nn(2:end-1) > nn(1:end-2)) 0] .* (nn >= max_nn/5); 43 | 44 | %plot(xx,nn); 45 | %hold on; 46 | %plot(xx,nn .* localmax,'r' ); 47 | %hold off; 48 | 49 | localmax_ind = find(localmax); 50 | nn_local_max = nn(localmax_ind); 51 | 52 | % order the picks: 53 | [a,b] = sort(-nn_local_max); 54 | 55 | localmax_ind = localmax_ind(b); 56 | nn_local_max = nn_local_max(b); 57 | 58 | sig_I = abs(xx(localmax_ind(1)) - xx(localmax_ind(2)))/4.25; 59 | 60 | 61 | 62 | 63 | I2 = anisdiff(I,sig_I,30); 64 | 65 | 66 | if ~type_numbering, 67 | number_ext = num2str(image_numbers(kk)); 68 | else 69 | number_ext = sprintf(['%.' num2str(N_slots) 'd'],image_numbers(kk)); 70 | end; 71 | 72 | ima_name2 = [calib_name '_smth' number_ext '.' format_image2]; 73 | 74 | fprintf(1,['Saving smoothed image under ' ima_name2 '...\n']); 75 | 76 | if format_image2(1) == 'p', 77 | if format_images2(2) == 'p', 78 | saveppm(ima_name2,uint8(round(I2))); 79 | else 80 | savepgm(ima_name2,uint8(round(I2))); 81 | end; 82 | else 83 | if format_image2(1) == 'r', 84 | writeras(ima_name2,round(I2),gray(256)); 85 | else 86 | imwrite(uint8(round(I2)),gray(256),ima_name2,format_image2); 87 | end; 88 | end; 89 | 90 | end; 91 | 92 | end; 93 | 94 | fprintf(1,'\ndone\n'); -------------------------------------------------------------------------------- /TOOLBOX_calib/startup.m: -------------------------------------------------------------------------------- 1 | % Main camera calibration toolbox: 2 | format compact 3 | -------------------------------------------------------------------------------- /TOOLBOX_calib/stereo_triangulation.m: -------------------------------------------------------------------------------- 1 | function [XL,XR] = stereo_triangulation(xL,xR,om,T,fc_left,cc_left,kc_left,alpha_c_left,fc_right,cc_right,kc_right,alpha_c_right), 2 | 3 | % [XL,XR] = stereo_triangulation(xL,xR,om,T,fc_left,cc_left,kc_left,alpha_c_left,fc_right,cc_right,kc_right,alpha_c_right), 4 | % 5 | % Function that computes the position of a set on N points given the left and right image projections. 6 | % The cameras are assumed to be calibrated, intrinsically, and extrinsically. 7 | % 8 | % Input: 9 | % xL: 2xN matrix of pixel coordinates in the left image 10 | % xR: 2xN matrix of pixel coordinates in the right image 11 | % om,T: rotation vector and translation vector between right and left cameras (output of stereo calibration) 12 | % fc_left,cc_left,...: intrinsic parameters of the left camera (output of stereo calibration) 13 | % fc_right,cc_right,...: intrinsic parameters of the right camera (output of stereo calibration) 14 | % 15 | % Output: 16 | % 17 | % XL: 3xN matrix of coordinates of the points in the left camera reference frame 18 | % XR: 3xN matrix of coordinates of the points in the right camera reference frame 19 | % 20 | % Note: XR and XL are related to each other through the rigid motion equation: XR = R * XL + T, where R = rodrigues(om) 21 | % For more information, visit http://www.vision.caltech.edu/bouguetj/calib_doc/htmls/example5.html 22 | % 23 | % 24 | % (c) Jean-Yves Bouguet - Intel Corporation - April 9th, 2003 25 | 26 | 27 | 28 | %--- Normalize the image projection according to the intrinsic parameters of the left and right cameras 29 | xt = normalize_pixel(xL,fc_left,cc_left,kc_left,alpha_c_left); 30 | xtt = normalize_pixel(xR,fc_right,cc_right,kc_right,alpha_c_right); 31 | 32 | %--- Extend the normalized projections in homogeneous coordinates 33 | xt = [xt;ones(1,size(xt,2))]; 34 | xtt = [xtt;ones(1,size(xtt,2))]; 35 | 36 | %--- Number of points: 37 | N = size(xt,2); 38 | 39 | %--- Rotation matrix corresponding to the rigid motion between left and right cameras: 40 | R = rodrigues(om); 41 | 42 | 43 | %--- Triangulation of the rays in 3D space: 44 | 45 | u = R * xt; 46 | 47 | n_xt2 = dot(xt,xt); 48 | n_xtt2 = dot(xtt,xtt); 49 | 50 | T_vect = repmat(T, [1 N]); 51 | 52 | DD = n_xt2 .* n_xtt2 - dot(u,xtt).^2; 53 | 54 | dot_uT = dot(u,T_vect); 55 | dot_xttT = dot(xtt,T_vect); 56 | dot_xttu = dot(u,xtt); 57 | 58 | NN1 = dot_xttu.*dot_xttT - n_xtt2 .* dot_uT; 59 | NN2 = n_xt2.*dot_xttT - dot_uT.*dot_xttu; 60 | 61 | Zt = NN1./DD; 62 | Ztt = NN2./DD; 63 | 64 | X1 = xt .* repmat(Zt,[3 1]); 65 | X2 = R'*(xtt.*repmat(Ztt,[3,1]) - T_vect); 66 | 67 | 68 | %--- Left coordinates: 69 | XL = 1/2 * (X1 + X2); 70 | 71 | %--- Right coordinates: 72 | XR = R*XL + T_vect; 73 | 74 | -------------------------------------------------------------------------------- /TOOLBOX_calib/willson_convert.m: -------------------------------------------------------------------------------- 1 | function [fc,cc,kc,alpha_c,Rc,Tc,omc,nx,ny,x_dist,xd] = willson_convert(Ncx,Nfx,dx,dy,dpx,dpy,Cx,Cy,sx,f,kappa1,Tx,Ty,Tz,Rx,Ry,Rz,p1,p2); 2 | 3 | %Conversion from Reg Willson's calibration format to my format 4 | 5 | % Conversion: 6 | 7 | % Focal length: 8 | fc = [sx/dpx ; 1/dpy]*f; 9 | 10 | % Principal point; 11 | cc = [Cx;Cy]; 12 | 13 | % Skew: 14 | alpha_c = 0; 15 | 16 | % Extrinsic parameters: 17 | Rx = rodrigues([Rx;0;0]); 18 | Ry = rodrigues([0;Ry;0]); 19 | Rz = rodrigues([0;0;Rz]); 20 | 21 | Rc = Rz * Ry * Rx; 22 | 23 | omc = rodrigues(Rc); 24 | 25 | Tc = [Tx;Ty;Tz]; 26 | 27 | 28 | % More tricky: Take care of the distorsion: 29 | 30 | Nfy = round(Nfx * 3/4); 31 | 32 | nx = Nfx; 33 | ny = Nfy; 34 | 35 | % Select a set of DISTORTED coordinates uniformely distributed across the image: 36 | 37 | [xp_dist,yp_dist] = meshgrid(0:Nfx-1,0:Nfy); 38 | 39 | xp_dist = xp_dist(:)'; 40 | yp_dist = yp_dist(:)'; 41 | 42 | 43 | % Apply UNDISTORTION according to Willson: 44 | 45 | xp_sensor_dist = dpx*(xp_dist - Cx)/sx; 46 | yp_sensor_dist = dpy*(yp_dist - Cy); 47 | 48 | dist_fact = 1 + kappa1*(xp_sensor_dist.^2 + yp_sensor_dist.^2); 49 | 50 | xp_sensor = xp_sensor_dist .* dist_fact; 51 | yp_sensor = yp_sensor_dist .* dist_fact; 52 | 53 | xp = xp_sensor * sx / dpx + Cx; 54 | yp = yp_sensor / dpy + Cy; 55 | 56 | ind= find((xp > 0) & (xp < Nfx-1) & (yp > 0) & (yp < Nfy-1)); 57 | 58 | xp = xp(ind); 59 | yp = yp(ind); 60 | xp_dist = xp_dist(ind); 61 | yp_dist = yp_dist(ind); 62 | 63 | 64 | % Now, find my own set of parameters: 65 | 66 | x_dist = [(xp_dist - cc(1))/fc(1);(yp_dist - cc(2))/fc(2)]; 67 | x_dist(1,:) = x_dist(1,:) - alpha_c * x_dist(2,:); 68 | 69 | x = [(xp - cc(1))/fc(1);(yp - cc(2))/fc(2)]; 70 | x(1,:) = x(1,:) - alpha_c * x(2,:); 71 | 72 | k = [0;0;0;0;0]; 73 | 74 | for kk = 1:5, 75 | 76 | [xd,dxddk] = apply_distortion(x,k); 77 | 78 | err = x_dist - xd; 79 | 80 | %norm(err) 81 | 82 | k_step = inv(dxddk'*dxddk)*(dxddk')*err(:); 83 | 84 | k = k + k_step; %inv(dxddk'*dxddk)*(dxddk')*err(:); 85 | 86 | %norm(k_step)/norm(k) 87 | 88 | if norm(k_step)/norm(k) < 10e-10, 89 | break; 90 | end; 91 | 92 | end; 93 | 94 | 95 | kc = k; 96 | -------------------------------------------------------------------------------- /TOOLBOX_calib/willson_read.m: -------------------------------------------------------------------------------- 1 | % Read in Reg Willson's data file, and convert it into my data format: 2 | 3 | if exist('calib_file'), 4 | if exist(calib_file)~=2, 5 | ask = 1; 6 | else 7 | ask = 0; 8 | end; 9 | else 10 | ask = 1; 11 | end; 12 | 13 | 14 | while ask, 15 | 16 | calib_file = input('Reg Willson calibration file name to convert: ','s'); 17 | 18 | if ~isempty(calib_file), 19 | if exist(calib_file)~=2, 20 | ask = 1; 21 | else 22 | ask = 0; 23 | end; 24 | else 25 | ask = 1; 26 | end; 27 | 28 | if ask, 29 | fprintf(1,'File not found. Try again.\n'); 30 | end; 31 | 32 | end; 33 | 34 | if exist(calib_file), 35 | 36 | fprintf(1,['Loading Willson calibration parameters from file ' calib_file '...\n']); 37 | 38 | load(calib_file); 39 | 40 | inddot = find(calib_file == '.'); 41 | 42 | if isempty(inddot), 43 | varname = calib_file; 44 | else 45 | varname = calib_file(1:inddot(1)-1); 46 | end; 47 | 48 | eval(['calib_params = ' varname ';']) 49 | 50 | Ncx = calib_params(1); 51 | Nfx = calib_params(2); 52 | dx = calib_params(3); 53 | dy = calib_params(4); 54 | dpx = calib_params(5); 55 | dpy = calib_params(6); 56 | Cx = calib_params(7); 57 | Cy = calib_params(8); 58 | sx = calib_params(9); 59 | f = calib_params(10); 60 | kappa1 = calib_params(11); 61 | Tx = calib_params(12); 62 | Ty = calib_params(13); 63 | Tz = calib_params(14); 64 | Rx = calib_params(15); 65 | Ry = calib_params(16); 66 | Rz = calib_params(17); 67 | p1 = calib_params(18); 68 | p2 = calib_params(19); 69 | 70 | fprintf(1,'Converting the calibration parameters... (may take a while due to the convertion of the distortion model)...\n'); 71 | 72 | % Conversion: 73 | [fc,cc,kc,alpha_c,Rc_1,Tc_1,omc_1,nx,ny,x_dist,xd] = willson_convert(Ncx,Nfx,dx,dy,dpx,dpy,Cx,Cy,sx,f,kappa1,Tx,Ty,Tz,Rx,Ry,Rz,p1,p2); 74 | 75 | KK = [fc(1) alpha_c*fc(1) cc(1);0 fc(2) cc(2) ; 0 0 1]; 76 | 77 | n_ima = 1; 78 | 79 | X_1 = [NaN;NaN;NaN]; 80 | x_1 = [NaN;NaN]; 81 | 82 | map = gray(256); 83 | 84 | fprintf(1,'done\n'); 85 | fprintf(1,'You are now ready to save the calibration parameters by clicking on Save.\n'); 86 | 87 | else 88 | 89 | disp(['WARNING: Calibration file ' calib_file ' not found']); 90 | 91 | end; 92 | 93 | -------------------------------------------------------------------------------- /TOOLBOX_calib/write_image.m: -------------------------------------------------------------------------------- 1 | function [] = write_image(I, kk , calib_name , format_image , type_numbering , image_numbers , N_slots), 2 | 3 | if format_image(1) == 'j', 4 | format_image = 'bmp'; 5 | end; 6 | 7 | 8 | if ~type_numbering, 9 | number_ext = num2str(image_numbers(kk)); 10 | else 11 | number_ext = sprintf(['%.' num2str(N_slots) 'd'],image_numbers(kk)); 12 | end; 13 | 14 | ima_name2 = [calib_name number_ext '.' format_image]; 15 | 16 | fprintf(1,['Saving image under ' ima_name2 '...\n']); 17 | 18 | if format_image(1) == 'p', 19 | if format_image(2) == 'p', 20 | saveppm(ima_name2,uint8(round(I))); 21 | else 22 | savepgm(ima_name2,uint8(round(I))); 23 | end; 24 | else 25 | if format_image(1) == 'r', 26 | writeras(ima_name2,round(I),gray(256)); 27 | else 28 | imwrite(uint8(round(I)),gray(256),ima_name2,format_image); 29 | end; 30 | end; 31 | -------------------------------------------------------------------------------- /TOOLBOX_calib/writeras.m: -------------------------------------------------------------------------------- 1 | function writeras(filename, image, map); 2 | %WRITERAS Write an image file in sun raster format. 3 | % WRITERAS('imagefile.ras', image_matrix, map) writes a 4 | % "sun.raster" image file. 5 | 6 | % Written by Thomas K. Leung 3/30/93. 7 | % @ California Institute of Technology. 8 | 9 | 10 | % PC and UNIX version of writeras - Jean-Yves Bouguet - Dec. 1998 11 | 12 | dot = max(find(filename == '.')); 13 | suffix = filename(dot+1:dot+3); 14 | 15 | if nargin < 3, 16 | map = []; 17 | end; 18 | 19 | if(strcmp(suffix, 'ras')) 20 | %Write header 21 | 22 | fp = fopen(filename, 'wb'); 23 | if(fp < 0) error(['Cannot open ' filename '.']), end 24 | 25 | [height, width] = size(image); 26 | image = image - 1; 27 | mapsize = size(map, 1)*size(map,2); 28 | %fwrite(fp, ... 29 | % [1504078485, width, height, 8, height*width, 1, 1, mapsize], ... 30 | % 'long'); 31 | 32 | 33 | zero_str = '00000000'; 34 | 35 | % MAGIC NUMBER: 36 | 37 | 38 | fwrite(fp,89,'uchar'); 39 | fwrite(fp,166,'uchar'); 40 | fwrite(fp,106,'uchar'); 41 | fwrite(fp,149,'uchar'); 42 | 43 | width_str = dec2hex(width); 44 | width_str = [zero_str(1:8-length(width_str)) width_str]; 45 | 46 | for ii = 1:2:7, 47 | fwrite(fp,hex2dec(width_str(ii:ii+1)),'uchar'); 48 | end; 49 | 50 | 51 | height_str = dec2hex(height); 52 | height_str = [zero_str(1:8-length(height_str)) height_str]; 53 | 54 | for ii = 1:2:7, 55 | fwrite(fp,hex2dec(height_str(ii:ii+1)),'uchar'); 56 | end; 57 | 58 | fwrite(fp,0,'uchar'); 59 | fwrite(fp,0,'uchar'); 60 | fwrite(fp,0,'uchar'); 61 | fwrite(fp,8,'uchar'); 62 | 63 | ll = height*width; 64 | ll_str = dec2hex(ll); 65 | ll_str = [zero_str(1:8-length(ll_str)) ll_str]; 66 | 67 | for ii = 1:2:7, 68 | fwrite(fp,hex2dec(ll_str(ii:ii+1)),'uchar'); 69 | end; 70 | 71 | fwrite(fp,0,'uchar'); 72 | fwrite(fp,0,'uchar'); 73 | fwrite(fp,0,'uchar'); 74 | fwrite(fp,1,'uchar'); 75 | fwrite(fp,0,'uchar'); 76 | fwrite(fp,0,'uchar'); 77 | fwrite(fp,0,'uchar'); 78 | fwrite(fp,~~mapsize,'uchar'); 79 | 80 | mapsize_str = dec2hex(mapsize); 81 | mapsize_str = [zero_str(1:8-length(mapsize_str)) mapsize_str]; 82 | 83 | %keyboard; 84 | 85 | for ii = 1:2:7, 86 | fwrite(fp,hex2dec(mapsize_str(ii:ii+1)),'uchar'); 87 | end; 88 | 89 | 90 | if mapsize ~= 0 91 | map = min(255, fix(255*map)); 92 | fwrite(fp, map, 'uchar'); 93 | end 94 | if rem(width,2) == 1 95 | image = [image'; zeros(1, height)]'; 96 | top = 255 * ones(size(image)); 97 | fwrite(fp, min(top,image)', 'uchar'); 98 | else 99 | top = 255 * ones(size(image)); 100 | fwrite(fp, min(top,image)', 'uchar'); 101 | end 102 | fclose(fp); 103 | else 104 | error('Image file name must end in either ''ras'' or ''rast''.'); 105 | end 106 | -------------------------------------------------------------------------------- /auto_calib.m: -------------------------------------------------------------------------------- 1 | % Automatic feature extraction 2 | [folder, name, ext] = fileparts(mfilename('fullpath')); 3 | addpath(fullfile(folder,'ICG_functions/')); 4 | 5 | if(~exist('parameters','var')) 6 | % Set default parameters 7 | parameters.image_threshold = []; 8 | parameters.corner_extraction_method = 'projective'; 9 | parameters.target_type = 'circles'; 10 | parameters.template_type = 'circles'; 11 | parameters.pixel_size_wh_mm = [6, 6]*1e-3; 12 | parameters.feature_subsampling_rate = 2; 13 | parameters.USE_IPP=0; 14 | parameters.verbose=1; 15 | 16 | % Ask for mandatory parameters 17 | parameters.approx_marker_width_pixels = input('Enter approx. minimal marker width in pixels: '); 18 | parameters.grid_width_mm = input('Enter grid with in millimeters: '); 19 | parameters.checker_aspect_ratio = input('Enter grid aspect ratio (= height/width): '); 20 | parameters.grid_coordinates_h = input('Enter horizontal grid dimensions (i.e. -11:11): '); 21 | parameters.grid_coordinates_v = input('Enter vertical grid dimensions (i.e. -18:16): '); 22 | end 23 | dX = parameters.grid_width_mm; 24 | dY = parameters.grid_width_mm*parameters.checker_aspect_ratio; 25 | verbose_info.verbose = parameters.verbose; 26 | verbose_info.figure_handles = []; 27 | template = ICG_createCalibrationMarker(16*7, 7, parameters.template_type, 0); 28 | template_pattern = template.pattern(template.corner_points(2,1):template.corner_points(2,3), template.corner_points(1,1):template.corner_points(1,3)); 29 | template_pattern = imresize(template_pattern, [32 32], 'bilinear'); 30 | 31 | corner_points = cell(1, n_ima);%{}; 32 | grid_coords = cell(1, n_ima);%{}; 33 | image_stack = cell(1, n_ima); 34 | for i=1:n_ima, 35 | image_stack{i} = eval(['I_' num2str(i)]); 36 | end 37 | 38 | for i=1:n_ima, 39 | fprintf('Processing image %d/%d', i, n_ima); 40 | localverbose_info = verbose_info; 41 | [active_images(i), corner_points{i}, grid_coords{i}] = ICG_extractCornersLoop (image_stack{i}, template_pattern, parameters, localverbose_info,i); 42 | end 43 | 44 | for i=1:n_ima, 45 | if active_images(i) 46 | eval(['X_' num2str(i) '=grid_coords{i};']); 47 | eval(['x_' num2str(i) '=corner_points{i}(1:2,:);']); 48 | else 49 | eval(['X_' num2str(i) '=nan(3,1);']); 50 | eval(['x_' num2str(i) '=nan(2,1);']); 51 | end 52 | end 53 | -------------------------------------------------------------------------------- /autocalibration.m: -------------------------------------------------------------------------------- 1 | % Autocalibration addon for Bouguet Toolbox version July 2014 2 | 3 | toolbox_path = fullfile(pwd,'TOOLBOX_calib/'); 4 | 5 | if(~exist(toolbox_path, 'dir')) 6 | !wget http://www.vision.caltech.edu/bouguetj/calib_doc/download/toolbox_calib.zip 7 | !unzip toolbox_calib.zip 8 | !rm toolbox_calib.zip 9 | end 10 | 11 | addpath(toolbox_path); 12 | 13 | image_dir = './testdata'; 14 | %% Read in images 15 | path__ = pwd; 16 | cd(image_dir); 17 | data_calib; 18 | ima_read_calib; 19 | cd(path__);clear path__ 20 | %% Set parameters 21 | parameters.approx_marker_width_pixels = 60; 22 | parameters.grid_width_mm = 10.218; 23 | parameters.image_threshold = []; 24 | parameters.checker_aspect_ratio = 10.18/10.218; 25 | parameters.corner_extraction_method = 'projective'; 26 | parameters.target_type = 'circles'; 27 | parameters.template_type = 'circles'; 28 | parameters.grid_coordinates_h = -11:11; 29 | parameters.grid_coordinates_v = -18:16; 30 | parameters.pixel_size_wh_mm = [6, 6]*1e-3; 31 | parameters.feature_subsampling_rate = 2; 32 | parameters.USE_IPP=0; 33 | parameters.verbose=1; 34 | 35 | %% Run feature extraction 36 | auto_calib; 37 | 38 | %% Run calibration 39 | go_calib_optim; -------------------------------------------------------------------------------- /calib_gui_normal_auto.m: -------------------------------------------------------------------------------- 1 | %function calib_gui_normal, 2 | 3 | 4 | cell_list = {}; 5 | 6 | 7 | %-------- Begin editable region -------------% 8 | %-------- Begin editable region -------------% 9 | 10 | 11 | fig_number = 1; 12 | 13 | title_figure = 'Camera Calibration Toolbox with automatic feature extraction - Standard Version'; 14 | 15 | clear fc cc kc KK 16 | kc = zeros(5,1); 17 | clearwin; 18 | 19 | cell_list{1,1} = {'Image names','data_calib;'}; 20 | cell_list{1,2} = {'Read images','ima_read_calib;'}; 21 | cell_list{1,3} = {'Extract grid corners','auto_calib;'}; 22 | cell_list{1,4} = {'Calibration','go_calib_optim;'}; 23 | cell_list{2,1} = {'Show Extrinsic','ext_calib;'}; 24 | cell_list{2,2} = {'Reproject on images','reproject_calib;'}; 25 | cell_list{2,3} = {'Analyse error','analyse_error;'}; 26 | % cell_list{2,4} = {'Recomp. corners','recomp_corner_calib;'}; 27 | cell_list{3,1} = {'Add/Suppress images','add_suppress;'}; 28 | cell_list{3,2} = {'Save','saving_calib;'}; 29 | cell_list{3,3} = {'Load','loading_calib;'}; 30 | cell_list{3,4} = {'Exit',['disp(''Bye. To run again, type calib_gui_auto.''); close(' num2str(fig_number) ');']}; %{'Exit','calib_gui;'}; 31 | cell_list{4,1} = {'Comp. Extrinsic','extrinsic_computation;'}; 32 | cell_list{4,2} = {'Undistort image','undistort_image;'}; 33 | cell_list{4,3} = {'Export calib data','export_calib_data;'}; 34 | cell_list{4,4} = {'Show calib results','show_calib_results;'}; 35 | %cell_list{5,1} = {'Smooth images','smooth_images;'}; 36 | 37 | 38 | show_window(cell_list,fig_number,title_figure,130,18,0,'clean',12); 39 | 40 | 41 | %-------- End editable region -------------% 42 | %-------- End editable region -------------% 43 | 44 | -------------------------------------------------------------------------------- /download_scipt.sh: -------------------------------------------------------------------------------- 1 | wget http://www.vision.caltech.edu/bouguetj/calib_doc/download/toolbox_calib.zip 2 | unzip toolbox_calib.zip 3 | 4 | rm -R toolbox_calib.zip 5 | -------------------------------------------------------------------------------- /testdata/image_001.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RobVisLab/camera_calibration/f56c17c297653ba0a156d125c2b162b19432b48b/testdata/image_001.jpg -------------------------------------------------------------------------------- /testdata/image_002.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RobVisLab/camera_calibration/f56c17c297653ba0a156d125c2b162b19432b48b/testdata/image_002.jpg -------------------------------------------------------------------------------- /testdata/image_003.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RobVisLab/camera_calibration/f56c17c297653ba0a156d125c2b162b19432b48b/testdata/image_003.jpg -------------------------------------------------------------------------------- /testdata/image_004.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RobVisLab/camera_calibration/f56c17c297653ba0a156d125c2b162b19432b48b/testdata/image_004.jpg -------------------------------------------------------------------------------- /testdata/image_005.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RobVisLab/camera_calibration/f56c17c297653ba0a156d125c2b162b19432b48b/testdata/image_005.jpg -------------------------------------------------------------------------------- /testdata/image_006.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RobVisLab/camera_calibration/f56c17c297653ba0a156d125c2b162b19432b48b/testdata/image_006.jpg -------------------------------------------------------------------------------- /testdata/image_007.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RobVisLab/camera_calibration/f56c17c297653ba0a156d125c2b162b19432b48b/testdata/image_007.jpg -------------------------------------------------------------------------------- /testdata/image_008.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RobVisLab/camera_calibration/f56c17c297653ba0a156d125c2b162b19432b48b/testdata/image_008.jpg -------------------------------------------------------------------------------- /testdata/image_009.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RobVisLab/camera_calibration/f56c17c297653ba0a156d125c2b162b19432b48b/testdata/image_009.jpg -------------------------------------------------------------------------------- /testdata/image_010.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RobVisLab/camera_calibration/f56c17c297653ba0a156d125c2b162b19432b48b/testdata/image_010.jpg -------------------------------------------------------------------------------- /testdata/image_011.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RobVisLab/camera_calibration/f56c17c297653ba0a156d125c2b162b19432b48b/testdata/image_011.jpg -------------------------------------------------------------------------------- /testdata/image_012.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RobVisLab/camera_calibration/f56c17c297653ba0a156d125c2b162b19432b48b/testdata/image_012.jpg -------------------------------------------------------------------------------- /testdata/image_013.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RobVisLab/camera_calibration/f56c17c297653ba0a156d125c2b162b19432b48b/testdata/image_013.jpg -------------------------------------------------------------------------------- /testdata/image_014.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RobVisLab/camera_calibration/f56c17c297653ba0a156d125c2b162b19432b48b/testdata/image_014.jpg -------------------------------------------------------------------------------- /testdata/image_015.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RobVisLab/camera_calibration/f56c17c297653ba0a156d125c2b162b19432b48b/testdata/image_015.jpg -------------------------------------------------------------------------------- /testdata/image_016.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RobVisLab/camera_calibration/f56c17c297653ba0a156d125c2b162b19432b48b/testdata/image_016.jpg -------------------------------------------------------------------------------- /testdata/image_017.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RobVisLab/camera_calibration/f56c17c297653ba0a156d125c2b162b19432b48b/testdata/image_017.jpg -------------------------------------------------------------------------------- /testdata/image_018.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RobVisLab/camera_calibration/f56c17c297653ba0a156d125c2b162b19432b48b/testdata/image_018.jpg -------------------------------------------------------------------------------- /testdata/image_019.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RobVisLab/camera_calibration/f56c17c297653ba0a156d125c2b162b19432b48b/testdata/image_019.jpg -------------------------------------------------------------------------------- /testdata/image_020.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/RobVisLab/camera_calibration/f56c17c297653ba0a156d125c2b162b19432b48b/testdata/image_020.jpg --------------------------------------------------------------------------------